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