diff options
Diffstat (limited to 'drivers/media/video/cx88/cx88-dsp.c')
-rw-r--r-- | drivers/media/video/cx88/cx88-dsp.c | 322 |
1 files changed, 322 insertions, 0 deletions
diff --git a/drivers/media/video/cx88/cx88-dsp.c b/drivers/media/video/cx88/cx88-dsp.c new file mode 100644 index 00000000000..a9907265ff6 --- /dev/null +++ b/drivers/media/video/cx88/cx88-dsp.c | |||
@@ -0,0 +1,322 @@ | |||
1 | /* | ||
2 | * | ||
3 | * Stereo and SAP detection for cx88 | ||
4 | * | ||
5 | * Copyright (c) 2009 Marton Balint <cus@fazekas.hu> | ||
6 | * | ||
7 | * This program is free software; you can redistribute it and/or modify | ||
8 | * it under the terms of the GNU General Public License as published by | ||
9 | * the Free Software Foundation; either version 2 of the License, or | ||
10 | * (at your option) any later version. | ||
11 | * | ||
12 | * This program is distributed in the hope that it will be useful, | ||
13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of | ||
14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the | ||
15 | * GNU General Public License for more details. | ||
16 | * | ||
17 | * You should have received a copy of the GNU General Public License | ||
18 | * along with this program; if not, write to the Free Software | ||
19 | * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. | ||
20 | */ | ||
21 | |||
22 | #include <linux/slab.h> | ||
23 | #include <linux/kernel.h> | ||
24 | #include <linux/module.h> | ||
25 | #include <linux/jiffies.h> | ||
26 | #include <asm/div64.h> | ||
27 | |||
28 | #include "cx88.h" | ||
29 | #include "cx88-reg.h" | ||
30 | |||
31 | #define INT_PI ((s32)(3.141592653589 * 32768.0)) | ||
32 | |||
33 | #define compat_remainder(a, b) \ | ||
34 | ((float)(((s32)((a)*100))%((s32)((b)*100)))/100.0) | ||
35 | |||
36 | #define baseband_freq(carrier, srate, tone) ((s32)( \ | ||
37 | (compat_remainder(carrier + tone, srate)) / srate * 2 * INT_PI)) | ||
38 | |||
39 | /* We calculate the baseband frequencies of the carrier and the pilot tones | ||
40 | * based on the the sampling rate of the audio rds fifo. */ | ||
41 | |||
42 | #define FREQ_A2_CARRIER baseband_freq(54687.5, 2689.36, 0.0) | ||
43 | #define FREQ_A2_DUAL baseband_freq(54687.5, 2689.36, 274.1) | ||
44 | #define FREQ_A2_STEREO baseband_freq(54687.5, 2689.36, 117.5) | ||
45 | |||
46 | /* The frequencies below are from the reference driver. They probably need | ||
47 | * further adjustments, because they are not tested at all. You may even need | ||
48 | * to play a bit with the registers of the chip to select the proper signal | ||
49 | * for the input of the audio rds fifo, and measure it's sampling rate to | ||
50 | * calculate the proper baseband frequencies... */ | ||
51 | |||
52 | #define FREQ_A2M_CARRIER ((s32)(2.114516 * 32768.0)) | ||
53 | #define FREQ_A2M_DUAL ((s32)(2.754916 * 32768.0)) | ||
54 | #define FREQ_A2M_STEREO ((s32)(2.462326 * 32768.0)) | ||
55 | |||
56 | #define FREQ_EIAJ_CARRIER ((s32)(1.963495 * 32768.0)) /* 5pi/8 */ | ||
57 | #define FREQ_EIAJ_DUAL ((s32)(2.562118 * 32768.0)) | ||
58 | #define FREQ_EIAJ_STEREO ((s32)(2.601053 * 32768.0)) | ||
59 | |||
60 | #define FREQ_BTSC_DUAL ((s32)(1.963495 * 32768.0)) /* 5pi/8 */ | ||
61 | #define FREQ_BTSC_DUAL_REF ((s32)(1.374446 * 32768.0)) /* 7pi/16 */ | ||
62 | |||
63 | #define FREQ_BTSC_SAP ((s32)(2.471532 * 32768.0)) | ||
64 | #define FREQ_BTSC_SAP_REF ((s32)(1.730072 * 32768.0)) | ||
65 | |||
66 | /* The spectrum of the signal should be empty between these frequencies. */ | ||
67 | #define FREQ_NOISE_START ((s32)(0.100000 * 32768.0)) | ||
68 | #define FREQ_NOISE_END ((s32)(1.200000 * 32768.0)) | ||
69 | |||
70 | static unsigned int dsp_debug; | ||
71 | module_param(dsp_debug, int, 0644); | ||
72 | MODULE_PARM_DESC(dsp_debug, "enable audio dsp debug messages"); | ||
73 | |||
74 | #define dprintk(level, fmt, arg...) if (dsp_debug >= level) \ | ||
75 | printk(KERN_DEBUG "%s/0: " fmt, core->name , ## arg) | ||
76 | |||
77 | static s32 int_cos(u32 x) | ||
78 | { | ||
79 | u32 t2, t4, t6, t8; | ||
80 | s32 ret; | ||
81 | u16 period = x / INT_PI; | ||
82 | if (period % 2) | ||
83 | return -int_cos(x - INT_PI); | ||
84 | x = x % INT_PI; | ||
85 | if (x > INT_PI/2) | ||
86 | return -int_cos(INT_PI/2 - (x % (INT_PI/2))); | ||
87 | /* Now x is between 0 and INT_PI/2. | ||
88 | * To calculate cos(x) we use it's Taylor polinom. */ | ||
89 | t2 = x*x/32768/2; | ||
90 | t4 = t2*x/32768*x/32768/3/4; | ||
91 | t6 = t4*x/32768*x/32768/5/6; | ||
92 | t8 = t6*x/32768*x/32768/7/8; | ||
93 | ret = 32768-t2+t4-t6+t8; | ||
94 | return ret; | ||
95 | } | ||
96 | |||
97 | static u32 int_goertzel(s16 x[], u32 N, u32 freq) | ||
98 | { | ||
99 | /* We use the Goertzel algorithm to determine the power of the | ||
100 | * given frequency in the signal */ | ||
101 | s32 s_prev = 0; | ||
102 | s32 s_prev2 = 0; | ||
103 | s32 coeff = 2*int_cos(freq); | ||
104 | u32 i; | ||
105 | |||
106 | u64 tmp; | ||
107 | u32 divisor; | ||
108 | |||
109 | for (i = 0; i < N; i++) { | ||
110 | s32 s = x[i] + ((s64)coeff*s_prev/32768) - s_prev2; | ||
111 | s_prev2 = s_prev; | ||
112 | s_prev = s; | ||
113 | } | ||
114 | |||
115 | tmp = (s64)s_prev2 * s_prev2 + (s64)s_prev * s_prev - | ||
116 | (s64)coeff * s_prev2 * s_prev / 32768; | ||
117 | |||
118 | /* XXX: N must be low enough so that N*N fits in s32. | ||
119 | * Else we need two divisions. */ | ||
120 | divisor = N * N; | ||
121 | do_div(tmp, divisor); | ||
122 | |||
123 | return (u32) tmp; | ||
124 | } | ||
125 | |||
126 | static u32 freq_magnitude(s16 x[], u32 N, u32 freq) | ||
127 | { | ||
128 | u32 sum = int_goertzel(x, N, freq); | ||
129 | return (u32)int_sqrt(sum); | ||
130 | } | ||
131 | |||
132 | static u32 noise_magnitude(s16 x[], u32 N, u32 freq_start, u32 freq_end) | ||
133 | { | ||
134 | int i; | ||
135 | u32 sum = 0; | ||
136 | u32 freq_step; | ||
137 | int samples = 5; | ||
138 | |||
139 | if (N > 192) { | ||
140 | /* The last 192 samples are enough for noise detection */ | ||
141 | x += (N-192); | ||
142 | N = 192; | ||
143 | } | ||
144 | |||
145 | freq_step = (freq_end - freq_start) / (samples - 1); | ||
146 | |||
147 | for (i = 0; i < samples; i++) { | ||
148 | sum += int_goertzel(x, N, freq_start); | ||
149 | freq_start += freq_step; | ||
150 | } | ||
151 | |||
152 | return (u32)int_sqrt(sum / samples); | ||
153 | } | ||
154 | |||
155 | static s32 detect_a2_a2m_eiaj(struct cx88_core *core, s16 x[], u32 N) | ||
156 | { | ||
157 | s32 carrier, stereo, dual, noise; | ||
158 | s32 carrier_freq, stereo_freq, dual_freq; | ||
159 | s32 ret; | ||
160 | |||
161 | switch (core->tvaudio) { | ||
162 | case WW_BG: | ||
163 | case WW_DK: | ||
164 | carrier_freq = FREQ_A2_CARRIER; | ||
165 | stereo_freq = FREQ_A2_STEREO; | ||
166 | dual_freq = FREQ_A2_DUAL; | ||
167 | break; | ||
168 | case WW_M: | ||
169 | carrier_freq = FREQ_A2M_CARRIER; | ||
170 | stereo_freq = FREQ_A2M_STEREO; | ||
171 | dual_freq = FREQ_A2M_DUAL; | ||
172 | break; | ||
173 | case WW_EIAJ: | ||
174 | carrier_freq = FREQ_EIAJ_CARRIER; | ||
175 | stereo_freq = FREQ_EIAJ_STEREO; | ||
176 | dual_freq = FREQ_EIAJ_DUAL; | ||
177 | break; | ||
178 | default: | ||
179 | printk(KERN_WARNING "%s/0: unsupported audio mode %d for %s\n", | ||
180 | core->name, core->tvaudio, __func__); | ||
181 | return UNSET; | ||
182 | } | ||
183 | |||
184 | carrier = freq_magnitude(x, N, carrier_freq); | ||
185 | stereo = freq_magnitude(x, N, stereo_freq); | ||
186 | dual = freq_magnitude(x, N, dual_freq); | ||
187 | noise = noise_magnitude(x, N, FREQ_NOISE_START, FREQ_NOISE_END); | ||
188 | |||
189 | dprintk(1, "detect a2/a2m/eiaj: carrier=%d, stereo=%d, dual=%d, " | ||
190 | "noise=%d\n", carrier, stereo, dual, noise); | ||
191 | |||
192 | if (stereo > dual) | ||
193 | ret = V4L2_TUNER_SUB_STEREO; | ||
194 | else | ||
195 | ret = V4L2_TUNER_SUB_LANG1 | V4L2_TUNER_SUB_LANG2; | ||
196 | |||
197 | if (core->tvaudio == WW_EIAJ) { | ||
198 | /* EIAJ checks may need adjustments */ | ||
199 | if ((carrier > max(stereo, dual)*2) && | ||
200 | (carrier < max(stereo, dual)*6) && | ||
201 | (carrier > 20 && carrier < 200) && | ||
202 | (max(stereo, dual) > min(stereo, dual))) { | ||
203 | /* For EIAJ the carrier is always present, | ||
204 | so we probably don't need noise detection */ | ||
205 | return ret; | ||
206 | } | ||
207 | } else { | ||
208 | if ((carrier > max(stereo, dual)*2) && | ||
209 | (carrier < max(stereo, dual)*8) && | ||
210 | (carrier > 20 && carrier < 200) && | ||
211 | (noise < 10) && | ||
212 | (max(stereo, dual) > min(stereo, dual)*2)) { | ||
213 | return ret; | ||
214 | } | ||
215 | } | ||
216 | return V4L2_TUNER_SUB_MONO; | ||
217 | } | ||
218 | |||
219 | static s32 detect_btsc(struct cx88_core *core, s16 x[], u32 N) | ||
220 | { | ||
221 | s32 sap_ref = freq_magnitude(x, N, FREQ_BTSC_SAP_REF); | ||
222 | s32 sap = freq_magnitude(x, N, FREQ_BTSC_SAP); | ||
223 | s32 dual_ref = freq_magnitude(x, N, FREQ_BTSC_DUAL_REF); | ||
224 | s32 dual = freq_magnitude(x, N, FREQ_BTSC_DUAL); | ||
225 | dprintk(1, "detect btsc: dual_ref=%d, dual=%d, sap_ref=%d, sap=%d" | ||
226 | "\n", dual_ref, dual, sap_ref, sap); | ||
227 | /* FIXME: Currently not supported */ | ||
228 | return UNSET; | ||
229 | } | ||
230 | |||
231 | static s16 *read_rds_samples(struct cx88_core *core, u32 *N) | ||
232 | { | ||
233 | const struct sram_channel *srch = &cx88_sram_channels[SRAM_CH27]; | ||
234 | s16 *samples; | ||
235 | |||
236 | unsigned int i; | ||
237 | unsigned int bpl = srch->fifo_size/AUD_RDS_LINES; | ||
238 | unsigned int spl = bpl/4; | ||
239 | unsigned int sample_count = spl*(AUD_RDS_LINES-1); | ||
240 | |||
241 | u32 current_address = cx_read(srch->ptr1_reg); | ||
242 | u32 offset = (current_address - srch->fifo_start + bpl); | ||
243 | |||
244 | dprintk(1, "read RDS samples: current_address=%08x (offset=%08x), " | ||
245 | "sample_count=%d, aud_intstat=%08x\n", current_address, | ||
246 | current_address - srch->fifo_start, sample_count, | ||
247 | cx_read(MO_AUD_INTSTAT)); | ||
248 | |||
249 | samples = kmalloc(sizeof(s16)*sample_count, GFP_KERNEL); | ||
250 | if (!samples) | ||
251 | return NULL; | ||
252 | |||
253 | *N = sample_count; | ||
254 | |||
255 | for (i = 0; i < sample_count; i++) { | ||
256 | offset = offset % (AUD_RDS_LINES*bpl); | ||
257 | samples[i] = cx_read(srch->fifo_start + offset); | ||
258 | offset += 4; | ||
259 | } | ||
260 | |||
261 | if (dsp_debug >= 2) { | ||
262 | dprintk(2, "RDS samples dump: "); | ||
263 | for (i = 0; i < sample_count; i++) | ||
264 | printk("%hd ", samples[i]); | ||
265 | printk(".\n"); | ||
266 | } | ||
267 | |||
268 | return samples; | ||
269 | } | ||
270 | |||
271 | s32 cx88_dsp_detect_stereo_sap(struct cx88_core *core) | ||
272 | { | ||
273 | s16 *samples; | ||
274 | u32 N = 0; | ||
275 | s32 ret = UNSET; | ||
276 | |||
277 | /* If audio RDS fifo is disabled, we can't read the samples */ | ||
278 | if (!(cx_read(MO_AUD_DMACNTRL) & 0x04)) | ||
279 | return ret; | ||
280 | if (!(cx_read(AUD_CTL) & EN_FMRADIO_EN_RDS)) | ||
281 | return ret; | ||
282 | |||
283 | /* Wait at least 500 ms after an audio standard change */ | ||
284 | if (time_before(jiffies, core->last_change + msecs_to_jiffies(500))) | ||
285 | return ret; | ||
286 | |||
287 | samples = read_rds_samples(core, &N); | ||
288 | |||
289 | if (!samples) | ||
290 | return ret; | ||
291 | |||
292 | switch (core->tvaudio) { | ||
293 | case WW_BG: | ||
294 | case WW_DK: | ||
295 | case WW_EIAJ: | ||
296 | case WW_M: | ||
297 | ret = detect_a2_a2m_eiaj(core, samples, N); | ||
298 | break; | ||
299 | case WW_BTSC: | ||
300 | ret = detect_btsc(core, samples, N); | ||
301 | break; | ||
302 | case WW_NONE: | ||
303 | case WW_I: | ||
304 | case WW_L: | ||
305 | case WW_I2SPT: | ||
306 | case WW_FM: | ||
307 | case WW_I2SADC: | ||
308 | break; | ||
309 | } | ||
310 | |||
311 | kfree(samples); | ||
312 | |||
313 | if (UNSET != ret) | ||
314 | dprintk(1, "stereo/sap detection result:%s%s%s\n", | ||
315 | (ret & V4L2_TUNER_SUB_MONO) ? " mono" : "", | ||
316 | (ret & V4L2_TUNER_SUB_STEREO) ? " stereo" : "", | ||
317 | (ret & V4L2_TUNER_SUB_LANG2) ? " dual" : ""); | ||
318 | |||
319 | return ret; | ||
320 | } | ||
321 | EXPORT_SYMBOL(cx88_dsp_detect_stereo_sap); | ||
322 | |||