]>
Commit | Line | Data |
---|---|---|
e878cf3a MB |
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 | ||
5a0e3ad6 | 22 | #include <linux/slab.h> |
e878cf3a MB |
23 | #include <linux/kernel.h> |
24 | #include <linux/module.h> | |
25 | #include <linux/jiffies.h> | |
7561300a | 26 | #include <asm/div64.h> |
e878cf3a MB |
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; | |
7561300a MS |
105 | |
106 | u64 tmp; | |
107 | u32 divisor; | |
108 | ||
e878cf3a MB |
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 | } | |
7561300a MS |
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; | |
e878cf3a MB |
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 | 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 | ret = detect_a2_a2m_eiaj(core, samples, N); | |
296 | break; | |
297 | case WW_BTSC: | |
298 | ret = detect_btsc(core, samples, N); | |
299 | break; | |
300 | } | |
301 | ||
302 | kfree(samples); | |
303 | ||
304 | if (UNSET != ret) | |
305 | dprintk(1, "stereo/sap detection result:%s%s%s\n", | |
306 | (ret & V4L2_TUNER_SUB_MONO) ? " mono" : "", | |
307 | (ret & V4L2_TUNER_SUB_STEREO) ? " stereo" : "", | |
308 | (ret & V4L2_TUNER_SUB_LANG2) ? " dual" : ""); | |
309 | ||
310 | return ret; | |
311 | } | |
312 | EXPORT_SYMBOL(cx88_dsp_detect_stereo_sap); | |
313 |