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 - 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(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_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, 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 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(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 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