1 | // SPDX-License-Identifier: GPL-2.0-or-later |
2 | /* |
3 | * Stereo and SAP detection for cx88 |
4 | * |
5 | * Copyright (c) 2009 Marton Balint <cus@fazekas.hu> |
6 | */ |
7 | |
8 | #include "cx88.h" |
9 | #include "cx88-reg.h" |
10 | |
11 | #include <linux/slab.h> |
12 | #include <linux/kernel.h> |
13 | #include <linux/module.h> |
14 | #include <linux/jiffies.h> |
15 | #include <asm/div64.h> |
16 | |
17 | #define INT_PI ((s32)(3.141592653589 * 32768.0)) |
18 | |
19 | #define compat_remainder(a, b) \ |
20 | ((float)(((s32)((a) * 100)) % ((s32)((b) * 100))) / 100.0) |
21 | |
22 | #define baseband_freq(carrier, srate, tone) ((s32)( \ |
23 | (compat_remainder(carrier + tone, srate)) / srate * 2 * INT_PI)) |
24 | |
25 | /* |
26 | * We calculate the baseband frequencies of the carrier and the pilot tones |
27 | * based on the sampling rate of the audio rds fifo. |
28 | */ |
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 | |
34 | /* |
35 | * The frequencies below are from the reference driver. They probably need |
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 |
39 | * calculate the proper baseband frequencies... |
40 | */ |
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 | |
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) |
69 | |
70 | static s32 int_cos(u32 x) |
71 | { |
72 | u32 t2, t4, t6, t8; |
73 | s32 ret; |
74 | u16 period = x / INT_PI; |
75 | |
76 | if (period % 2) |
77 | return -int_cos(x: x - INT_PI); |
78 | x = x % INT_PI; |
79 | if (x > INT_PI / 2) |
80 | return -int_cos(INT_PI / 2 - (x % (INT_PI / 2))); |
81 | /* |
82 | * Now x is between 0 and INT_PI/2. |
83 | * To calculate cos(x) we use it's Taylor polinom. |
84 | */ |
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; |
90 | return ret; |
91 | } |
92 | |
93 | static u32 int_goertzel(s16 x[], u32 N, u32 freq) |
94 | { |
95 | /* |
96 | * We use the Goertzel algorithm to determine the power of the |
97 | * given frequency in the signal |
98 | */ |
99 | s32 s_prev = 0; |
100 | s32 s_prev2 = 0; |
101 | s32 coeff = 2 * int_cos(x: freq); |
102 | u32 i; |
103 | |
104 | u64 tmp; |
105 | u32 divisor; |
106 | |
107 | for (i = 0; i < N; i++) { |
108 | s32 s = x[i] + ((s64)coeff * s_prev / 32768) - s_prev2; |
109 | |
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 | /* |
118 | * XXX: N must be low enough so that N*N fits in s32. |
119 | * Else we need two divisions. |
120 | */ |
121 | divisor = N * N; |
122 | do_div(tmp, divisor); |
123 | |
124 | return (u32)tmp; |
125 | } |
126 | |
127 | static u32 freq_magnitude(s16 x[], u32 N, u32 freq) |
128 | { |
129 | u32 sum = int_goertzel(x, N, freq); |
130 | |
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 */ |
143 | x += (N - 192); |
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: 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; |
180 | default: |
181 | pr_warn("unsupported audio mode %d for %s\n" , |
182 | core->tvaudio, __func__); |
183 | return UNSET; |
184 | } |
185 | |
186 | carrier = freq_magnitude(x, N, freq: carrier_freq); |
187 | stereo = freq_magnitude(x, N, freq: stereo_freq); |
188 | dual = freq_magnitude(x, N, freq: dual_freq); |
189 | noise = noise_magnitude(x, N, FREQ_NOISE_START, FREQ_NOISE_END); |
190 | |
191 | dprintk(1, |
192 | "detect a2/a2m/eiaj: carrier=%d, stereo=%d, dual=%d, noise=%d\n" , |
193 | carrier, stereo, dual, noise); |
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 */ |
202 | if ((carrier > max(stereo, dual) * 2) && |
203 | (carrier < max(stereo, dual) * 6) && |
204 | (carrier > 20 && carrier < 200) && |
205 | (max(stereo, dual) > min(stereo, dual))) { |
206 | /* |
207 | * For EIAJ the carrier is always present, |
208 | * so we probably don't need noise detection |
209 | */ |
210 | return ret; |
211 | } |
212 | } else { |
213 | if ((carrier > max(stereo, dual) * 2) && |
214 | (carrier < max(stereo, dual) * 8) && |
215 | (carrier > 20 && carrier < 200) && |
216 | (noise < 10) && |
217 | (max(stereo, dual) > min(stereo, dual) * 2)) { |
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); |
230 | |
231 | dprintk(1, "detect btsc: dual_ref=%d, dual=%d, sap_ref=%d, sap=%d\n" , |
232 | dual_ref, dual, sap_ref, sap); |
233 | /* FIXME: Currently not supported */ |
234 | return UNSET; |
235 | } |
236 | |
237 | static s16 *read_rds_samples(struct cx88_core *core, u32 *N) |
238 | { |
239 | const struct sram_channel *srch = &cx88_sram_channels[SRAM_CH27]; |
240 | s16 *samples; |
241 | |
242 | unsigned int i; |
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); |
246 | |
247 | u32 current_address = cx_read(srch->ptr1_reg); |
248 | u32 offset = (current_address - srch->fifo_start + bpl); |
249 | |
250 | dprintk(1, |
251 | "read RDS samples: current_address=%08x (offset=%08x), sample_count=%d, aud_intstat=%08x\n" , |
252 | current_address, |
253 | current_address - srch->fifo_start, sample_count, |
254 | cx_read(MO_AUD_INTSTAT)); |
255 | samples = kmalloc_array(n: sample_count, size: sizeof(*samples), GFP_KERNEL); |
256 | if (!samples) |
257 | return NULL; |
258 | |
259 | *N = sample_count; |
260 | |
261 | for (i = 0; i < sample_count; i++) { |
262 | offset = offset % (AUD_RDS_LINES * bpl); |
263 | samples[i] = cx_read(srch->fifo_start + offset); |
264 | offset += 4; |
265 | } |
266 | |
267 | dprintk(2, "RDS samples dump: %*ph\n" , sample_count, samples); |
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: &N); |
289 | |
290 | if (!samples) |
291 | return ret; |
292 | |
293 | switch (core->tvaudio) { |
294 | case WW_BG: |
295 | case WW_DK: |
296 | case WW_EIAJ: |
297 | case WW_M: |
298 | ret = detect_a2_a2m_eiaj(core, x: samples, N); |
299 | break; |
300 | case WW_BTSC: |
301 | ret = detect_btsc(core, x: samples, N); |
302 | break; |
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; |
310 | } |
311 | |
312 | kfree(objp: samples); |
313 | |
314 | if (ret != UNSET) |
315 | dprintk(1, "stereo/sap detection result:%s%s%s\n" , |
316 | (ret & V4L2_TUNER_SUB_MONO) ? " mono" : "" , |
317 | (ret & V4L2_TUNER_SUB_STEREO) ? " stereo" : "" , |
318 | (ret & V4L2_TUNER_SUB_LANG2) ? " dual" : "" ); |
319 | |
320 | return ret; |
321 | } |
322 | EXPORT_SYMBOL(cx88_dsp_detect_stereo_sap); |
323 | |
324 | |