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