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