drivers/media/pci/cx88/cx88-dsp.c

   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