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