SAFE public projects git trees. - safe/jmp/linux-2.6/blob - drivers/media/video/cx88/cx88-dsp.c

   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/kernel.h>
  23 #include <linux/module.h>
  24 #include <linux/jiffies.h>
  25
  26 #include "cx88.h"
  27 #include "cx88-reg.h"
  28
  29 #define INT_PI                  ((s32)(3.141592653589 * 32768.0))
  30
  31 #define compat_remainder(a, b) \
  32          ((float)(((s32)((a)*100))%((s32)((b)*100)))/100.0)
  33
  34 #define baseband_freq(carrier, srate, tone) ((s32)( \
  35          (compat_remainder(carrier + tone, srate)) / srate * 2 * INT_PI))
  36
  37 /* We calculate the baseband frequencies of the carrier and the pilot tones
  38  * based on the the sampling rate of the audio rds fifo. */
  39
  40 #define FREQ_A2_CARRIER         baseband_freq(54687.5, 2689.36, 0.0)
  41 #define FREQ_A2_DUAL            baseband_freq(54687.5, 2689.36, 274.1)
  42 #define FREQ_A2_STEREO          baseband_freq(54687.5, 2689.36, 117.5)
  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 #define FREQ_A2M_CARRIER        ((s32)(2.114516 * 32768.0))
  51 #define FREQ_A2M_DUAL           ((s32)(2.754916 * 32768.0))
  52 #define FREQ_A2M_STEREO         ((s32)(2.462326 * 32768.0))
  53
  54 #define FREQ_EIAJ_CARRIER       ((s32)(1.963495 * 32768.0)) /* 5pi/8  */
  55 #define FREQ_EIAJ_DUAL          ((s32)(2.562118 * 32768.0))
  56 #define FREQ_EIAJ_STEREO        ((s32)(2.601053 * 32768.0))
  57
  58 #define FREQ_BTSC_DUAL          ((s32)(1.963495 * 32768.0)) /* 5pi/8  */
  59 #define FREQ_BTSC_DUAL_REF      ((s32)(1.374446 * 32768.0)) /* 7pi/16 */
  60
  61 #define FREQ_BTSC_SAP           ((s32)(2.471532 * 32768.0))
  62 #define FREQ_BTSC_SAP_REF       ((s32)(1.730072 * 32768.0))
  63
  64 /* The spectrum of the signal should be empty between these frequencies. */
  65 #define FREQ_NOISE_START        ((s32)(0.100000 * 32768.0))
  66 #define FREQ_NOISE_END          ((s32)(1.200000 * 32768.0))
  67
  68 static unsigned int dsp_debug;
  69 module_param(dsp_debug, int, 0644);
  70 MODULE_PARM_DESC(dsp_debug, "enable audio dsp debug messages");
  71
  72 #define dprintk(level, fmt, arg...)     if (dsp_debug >= level) \
  73         printk(KERN_DEBUG "%s/0: " fmt, core->name , ## arg)
  74
  75 static s32 int_cos(u32 x)
  76 {
  77         u32 t2, t4, t6, t8;
  78         s32 ret;
  79         u16 period = x / INT_PI;
  80         if (period % 2)
  81                 return -int_cos(x - INT_PI);
  82         x = x % INT_PI;
  83         if (x > INT_PI/2)
  84                 return -int_cos(INT_PI/2 - (x % (INT_PI/2)));
  85         /* Now x is between 0 and INT_PI/2.
  86          * To calculate cos(x) we use it's Taylor polinom. */
  87         t2 = x*x/32768/2;
  88         t4 = t2*x/32768*x/32768/3/4;
  89         t6 = t4*x/32768*x/32768/5/6;
  90         t8 = t6*x/32768*x/32768/7/8;
  91         ret = 32768-t2+t4-t6+t8;
  92         return ret;
  93 }
  94
  95 static u32 int_goertzel(s16 x[], u32 N, u32 freq)
  96 {
  97         /* We use the Goertzel algorithm to determine the power of the
  98          * given frequency in the signal */
  99         s32 s_prev = 0;
 100         s32 s_prev2 = 0;
 101         s32 coeff = 2*int_cos(freq);
 102         u32 i;
 103         for (i = 0; i < N; i++) {
 104                 s32 s = x[i] + ((s64)coeff*s_prev/32768) - s_prev2;
 105                 s_prev2 = s_prev;
 106                 s_prev = s;
 107         }
 108         return (u32)(((s64)s_prev2*s_prev2 + (s64)s_prev*s_prev -
 109                       (s64)coeff*s_prev2*s_prev/32768)/N/N);
 110 }
 111
 112 static u32 freq_magnitude(s16 x[], u32 N, u32 freq)
 113 {
 114         u32 sum = int_goertzel(x, N, freq);
 115         return (u32)int_sqrt(sum);
 116 }
 117
 118 static u32 noise_magnitude(s16 x[], u32 N, u32 freq_start, u32 freq_end)
 119 {
 120         int i;
 121         u32 sum = 0;
 122         u32 freq_step;
 123         int samples = 5;
 124
 125         if (N > 192) {
 126                 /* The last 192 samples are enough for noise detection */
 127                 x += (N-192);
 128                 N = 192;
 129         }
 130
 131         freq_step = (freq_end - freq_start) / (samples - 1);
 132
 133         for (i = 0; i < samples; i++) {
 134                 sum += int_goertzel(x, N, freq_start);
 135                 freq_start += freq_step;
 136         }
 137
 138         return (u32)int_sqrt(sum / samples);
 139 }
 140
 141 static s32 detect_a2_a2m_eiaj(struct cx88_core *core, s16 x[], u32 N)
 142 {
 143         s32 carrier, stereo, dual, noise;
 144         s32 carrier_freq, stereo_freq, dual_freq;
 145         s32 ret;
 146
 147         switch (core->tvaudio) {
 148         case WW_BG:
 149         case WW_DK:
 150                 carrier_freq = FREQ_A2_CARRIER;
 151                 stereo_freq = FREQ_A2_STEREO;
 152                 dual_freq = FREQ_A2_DUAL;
 153                 break;
 154         case WW_M:
 155                 carrier_freq = FREQ_A2M_CARRIER;
 156                 stereo_freq = FREQ_A2M_STEREO;
 157                 dual_freq = FREQ_A2M_DUAL;
 158                 break;
 159         case WW_EIAJ:
 160                 carrier_freq = FREQ_EIAJ_CARRIER;
 161                 stereo_freq = FREQ_EIAJ_STEREO;
 162                 dual_freq = FREQ_EIAJ_DUAL;
 163                 break;
 164         default:
 165                 printk(KERN_WARNING "%s/0: unsupported audio mode %d for %s\n",
 166                        core->name, core->tvaudio, __func__);
 167                 return UNSET;
 168         }
 169
 170         carrier = freq_magnitude(x, N, carrier_freq);
 171         stereo  = freq_magnitude(x, N, stereo_freq);
 172         dual    = freq_magnitude(x, N, dual_freq);
 173         noise   = noise_magnitude(x, N, FREQ_NOISE_START, FREQ_NOISE_END);
 174
 175         dprintk(1, "detect a2/a2m/eiaj: carrier=%d, stereo=%d, dual=%d, "
 176                    "noise=%d\n", carrier, stereo, dual, noise);
 177
 178         if (stereo > dual)
 179                 ret = V4L2_TUNER_SUB_STEREO;
 180         else
 181                 ret = V4L2_TUNER_SUB_LANG1 | V4L2_TUNER_SUB_LANG2;
 182
 183         if (core->tvaudio == WW_EIAJ) {
 184                 /* EIAJ checks may need adjustments */
 185                 if ((carrier > max(stereo, dual)*2) &&
 186                     (carrier < max(stereo, dual)*6) &&
 187                     (carrier > 20 && carrier < 200) &&
 188                     (max(stereo, dual) > min(stereo, dual))) {
 189                         /* For EIAJ the carrier is always present,
 190                            so we probably don't need noise detection */
 191                         return ret;
 192                 }
 193         } else {
 194                 if ((carrier > max(stereo, dual)*2) &&
 195                     (carrier < max(stereo, dual)*8) &&
 196                     (carrier > 20 && carrier < 200) &&
 197                     (noise < 10) &&
 198                     (max(stereo, dual) > min(stereo, dual)*2)) {
 199                         return ret;
 200                 }
 201         }
 202         return V4L2_TUNER_SUB_MONO;
 203 }
 204
 205 static s32 detect_btsc(struct cx88_core *core, s16 x[], u32 N)
 206 {
 207         s32 sap_ref = freq_magnitude(x, N, FREQ_BTSC_SAP_REF);
 208         s32 sap = freq_magnitude(x, N, FREQ_BTSC_SAP);
 209         s32 dual_ref = freq_magnitude(x, N, FREQ_BTSC_DUAL_REF);
 210         s32 dual = freq_magnitude(x, N, FREQ_BTSC_DUAL);
 211         dprintk(1, "detect btsc: dual_ref=%d, dual=%d, sap_ref=%d, sap=%d"
 212                    "\n", dual_ref, dual, sap_ref, sap);
 213         /* FIXME: Currently not supported */
 214         return UNSET;
 215 }
 216
 217 static s16 *read_rds_samples(struct cx88_core *core, u32 *N)
 218 {
 219         struct sram_channel *srch = &cx88_sram_channels[SRAM_CH27];
 220         s16 *samples;
 221
 222         unsigned int i;
 223         unsigned int bpl = srch->fifo_size/AUD_RDS_LINES;
 224         unsigned int spl = bpl/4;
 225         unsigned int sample_count = spl*(AUD_RDS_LINES-1);
 226
 227         u32 current_address = cx_read(srch->ptr1_reg);
 228         u32 offset = (current_address - srch->fifo_start + bpl);
 229
 230         dprintk(1, "read RDS samples: current_address=%08x (offset=%08x), "
 231                 "sample_count=%d, aud_intstat=%08x\n", current_address,
 232                 current_address - srch->fifo_start, sample_count,
 233                 cx_read(MO_AUD_INTSTAT));
 234
 235         samples = kmalloc(sizeof(s16)*sample_count, GFP_KERNEL);
 236         if (!samples)
 237                 return NULL;
 238
 239         *N = sample_count;
 240
 241         for (i = 0; i < sample_count; i++)  {
 242                 offset = offset % (AUD_RDS_LINES*bpl);
 243                 samples[i] = cx_read(srch->fifo_start + offset);
 244                 offset += 4;
 245         }
 246
 247         if (dsp_debug >= 2) {
 248                 dprintk(2, "RDS samples dump: ");
 249                 for (i = 0; i < sample_count; i++)
 250                         printk("%hd ", samples[i]);
 251                 printk(".\n");
 252         }
 253
 254         return samples;
 255 }
 256
 257 s32 cx88_dsp_detect_stereo_sap(struct cx88_core *core)
 258 {
 259         s16 *samples;
 260         u32 N = 0;
 261         s32 ret = UNSET;
 262
 263         /* If audio RDS fifo is disabled, we can't read the samples */
 264         if (!(cx_read(MO_AUD_DMACNTRL) & 0x04))
 265                 return ret;
 266         if (!(cx_read(AUD_CTL) & EN_FMRADIO_EN_RDS))
 267                 return ret;
 268
 269         /* Wait at least 500 ms after an audio standard change */
 270         if (time_before(jiffies, core->last_change + msecs_to_jiffies(500)))
 271                 return ret;
 272
 273         samples = read_rds_samples(core, &N);
 274
 275         if (!samples)
 276                 return ret;
 277
 278         switch (core->tvaudio) {
 279         case WW_BG:
 280         case WW_DK:
 281                 ret = detect_a2_a2m_eiaj(core, samples, N);
 282                 break;
 283         case WW_BTSC:
 284                 ret = detect_btsc(core, samples, N);
 285                 break;
 286         }
 287
 288         kfree(samples);
 289
 290         if (UNSET != ret)
 291                 dprintk(1, "stereo/sap detection result:%s%s%s\n",
 292                            (ret & V4L2_TUNER_SUB_MONO) ? " mono" : "",
 293                            (ret & V4L2_TUNER_SUB_STEREO) ? " stereo" : "",
 294                            (ret & V4L2_TUNER_SUB_LANG2) ? " dual" : "");
 295
 296         return ret;
 297 }
 298 EXPORT_SYMBOL(cx88_dsp_detect_stereo_sap);
 299