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

   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 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
  60static unsigned int dsp_debug;
  61module_param(dsp_debug, int, 0644);
  62MODULE_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
  70static 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
  93static 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
 127static 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
 134static 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
 157static 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
 224static 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
 237static 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
 272s32 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}
 322EXPORT_SYMBOL(cx88_dsp_detect_stereo_sap);
 323
 324