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 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
int_cos(u32 x)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 - 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
int_goertzel(s16 x[],u32 N,u32 freq)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(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
freq_magnitude(s16 x[],u32 N,u32 freq)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
noise_magnitude(s16 x[],u32 N,u32 freq_start,u32 freq_end)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_start);
151 freq_start += freq_step;
152 }
153
154 return (u32)int_sqrt(sum / samples);
155 }
156
detect_a2_a2m_eiaj(struct cx88_core * core,s16 x[],u32 N)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, 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
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
detect_btsc(struct cx88_core * core,s16 x[],u32 N)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
read_rds_samples(struct cx88_core * core,u32 * N)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(sample_count, 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
cx88_dsp_detect_stereo_sap(struct cx88_core * core)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:
296 case WW_EIAJ:
297 case WW_M:
298 ret = detect_a2_a2m_eiaj(core, samples, N);
299 break;
300 case WW_BTSC:
301 ret = detect_btsc(core, 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(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