pci/cx88/cx88-dsp.c

0001 // SPDX-License-Identifier: GPL-2.0-or-later
0002 /*
0003  *  Stereo and SAP detection for cx88
0004  *
0005  *  Copyright (c) 2009 Marton Balint <cus@fazekas.hu>
0006  */
0007
0008 #include "cx88.h"
0009 #include "cx88-reg.h"
0010
0011 #include <linux/slab.h>
0012 #include <linux/kernel.h>
0013 #include <linux/module.h>
0014 #include <linux/jiffies.h>
0015 #include <asm/div64.h>
0016
0017 #define INT_PI          ((s32)(3.141592653589 * 32768.0))
0018
0019 #define compat_remainder(a, b) \
0020      ((float)(((s32)((a) * 100)) % ((s32)((b) * 100))) / 100.0)
0021
0022 #define baseband_freq(carrier, srate, tone) ((s32)( \
0023      (compat_remainder(carrier + tone, srate)) / srate * 2 * INT_PI))
0024
0025 /*
0026  * We calculate the baseband frequencies of the carrier and the pilot tones
0027  * based on the the sampling rate of the audio rds fifo.
0028  */
0029
0030 #define FREQ_A2_CARRIER         baseband_freq(54687.5, 2689.36, 0.0)
0031 #define FREQ_A2_DUAL            baseband_freq(54687.5, 2689.36, 274.1)
0032 #define FREQ_A2_STEREO          baseband_freq(54687.5, 2689.36, 117.5)
0033
0034 /*
0035  * The frequencies below are from the reference driver. They probably need
0036  * further adjustments, because they are not tested at all. You may even need
0037  * to play a bit with the registers of the chip to select the proper signal
0038  * for the input of the audio rds fifo, and measure it's sampling rate to
0039  * calculate the proper baseband frequencies...
0040  */
0041
0042 #define FREQ_A2M_CARRIER    ((s32)(2.114516 * 32768.0))
0043 #define FREQ_A2M_DUAL       ((s32)(2.754916 * 32768.0))
0044 #define FREQ_A2M_STEREO     ((s32)(2.462326 * 32768.0))
0045
0046 #define FREQ_EIAJ_CARRIER   ((s32)(1.963495 * 32768.0)) /* 5pi/8  */
0047 #define FREQ_EIAJ_DUAL      ((s32)(2.562118 * 32768.0))
0048 #define FREQ_EIAJ_STEREO    ((s32)(2.601053 * 32768.0))
0049
0050 #define FREQ_BTSC_DUAL      ((s32)(1.963495 * 32768.0)) /* 5pi/8  */
0051 #define FREQ_BTSC_DUAL_REF  ((s32)(1.374446 * 32768.0)) /* 7pi/16 */
0052
0053 #define FREQ_BTSC_SAP       ((s32)(2.471532 * 32768.0))
0054 #define FREQ_BTSC_SAP_REF   ((s32)(1.730072 * 32768.0))
0055
0056 /* The spectrum of the signal should be empty between these frequencies. */
0057 #define FREQ_NOISE_START    ((s32)(0.100000 * 32768.0))
0058 #define FREQ_NOISE_END      ((s32)(1.200000 * 32768.0))
0059
0060 static unsigned int dsp_debug;
0061 module_param(dsp_debug, int, 0644);
0062 MODULE_PARM_DESC(dsp_debug, "enable audio dsp debug messages");
0063
0064 #define dprintk(level, fmt, arg...) do {                \
0065     if (dsp_debug >= level)                     \
0066         printk(KERN_DEBUG pr_fmt("%s: dsp:" fmt),       \
0067             __func__, ##arg);               \
0068 } while (0)
0069
0070 static s32 int_cos(u32 x)
0071 {
0072     u32 t2, t4, t6, t8;
0073     s32 ret;
0074     u16 period = x / INT_PI;
0075
0076     if (period % 2)
0077         return -int_cos(x - INT_PI);
0078     x = x % INT_PI;
0079     if (x > INT_PI / 2)
0080         return -int_cos(INT_PI / 2 - (x % (INT_PI / 2)));
0081     /*
0082      * Now x is between 0 and INT_PI/2.
0083      * To calculate cos(x) we use it's Taylor polinom.
0084      */
0085     t2 = x * x / 32768 / 2;
0086     t4 = t2 * x / 32768 * x / 32768 / 3 / 4;
0087     t6 = t4 * x / 32768 * x / 32768 / 5 / 6;
0088     t8 = t6 * x / 32768 * x / 32768 / 7 / 8;
0089     ret = 32768 - t2 + t4 - t6 + t8;
0090     return ret;
0091 }
0092
0093 static u32 int_goertzel(s16 x[], u32 N, u32 freq)
0094 {
0095     /*
0096      * We use the Goertzel algorithm to determine the power of the
0097      * given frequency in the signal
0098      */
0099     s32 s_prev = 0;
0100     s32 s_prev2 = 0;
0101     s32 coeff = 2 * int_cos(freq);
0102     u32 i;
0103
0104     u64 tmp;
0105     u32 divisor;
0106
0107     for (i = 0; i < N; i++) {
0108         s32 s = x[i] + ((s64)coeff * s_prev / 32768) - s_prev2;
0109
0110         s_prev2 = s_prev;
0111         s_prev = s;
0112     }
0113
0114     tmp = (s64)s_prev2 * s_prev2 + (s64)s_prev * s_prev -
0115               (s64)coeff * s_prev2 * s_prev / 32768;
0116
0117     /*
0118      * XXX: N must be low enough so that N*N fits in s32.
0119      * Else we need two divisions.
0120      */
0121     divisor = N * N;
0122     do_div(tmp, divisor);
0123
0124     return (u32)tmp;
0125 }
0126
0127 static u32 freq_magnitude(s16 x[], u32 N, u32 freq)
0128 {
0129     u32 sum = int_goertzel(x, N, freq);
0130
0131     return (u32)int_sqrt(sum);
0132 }
0133
0134 static u32 noise_magnitude(s16 x[], u32 N, u32 freq_start, u32 freq_end)
0135 {
0136     int i;
0137     u32 sum = 0;
0138     u32 freq_step;
0139     int samples = 5;
0140
0141     if (N > 192) {
0142         /* The last 192 samples are enough for noise detection */
0143         x += (N - 192);
0144         N = 192;
0145     }
0146
0147     freq_step = (freq_end - freq_start) / (samples - 1);
0148
0149     for (i = 0; i < samples; i++) {
0150         sum += int_goertzel(x, N, freq_start);
0151         freq_start += freq_step;
0152     }
0153
0154     return (u32)int_sqrt(sum / samples);
0155 }
0156
0157 static s32 detect_a2_a2m_eiaj(struct cx88_core *core, s16 x[], u32 N)
0158 {
0159     s32 carrier, stereo, dual, noise;
0160     s32 carrier_freq, stereo_freq, dual_freq;
0161     s32 ret;
0162
0163     switch (core->tvaudio) {
0164     case WW_BG:
0165     case WW_DK:
0166         carrier_freq = FREQ_A2_CARRIER;
0167         stereo_freq = FREQ_A2_STEREO;
0168         dual_freq = FREQ_A2_DUAL;
0169         break;
0170     case WW_M:
0171         carrier_freq = FREQ_A2M_CARRIER;
0172         stereo_freq = FREQ_A2M_STEREO;
0173         dual_freq = FREQ_A2M_DUAL;
0174         break;
0175     case WW_EIAJ:
0176         carrier_freq = FREQ_EIAJ_CARRIER;
0177         stereo_freq = FREQ_EIAJ_STEREO;
0178         dual_freq = FREQ_EIAJ_DUAL;
0179         break;
0180     default:
0181         pr_warn("unsupported audio mode %d for %s\n",
0182             core->tvaudio, __func__);
0183         return UNSET;
0184     }
0185
0186     carrier = freq_magnitude(x, N, carrier_freq);
0187     stereo  = freq_magnitude(x, N, stereo_freq);
0188     dual    = freq_magnitude(x, N, dual_freq);
0189     noise   = noise_magnitude(x, N, FREQ_NOISE_START, FREQ_NOISE_END);
0190
0191     dprintk(1,
0192         "detect a2/a2m/eiaj: carrier=%d, stereo=%d, dual=%d, noise=%d\n",
0193         carrier, stereo, dual, noise);
0194
0195     if (stereo > dual)
0196         ret = V4L2_TUNER_SUB_STEREO;
0197     else
0198         ret = V4L2_TUNER_SUB_LANG1 | V4L2_TUNER_SUB_LANG2;
0199
0200     if (core->tvaudio == WW_EIAJ) {
0201         /* EIAJ checks may need adjustments */
0202         if ((carrier > max(stereo, dual) * 2) &&
0203             (carrier < max(stereo, dual) * 6) &&
0204             (carrier > 20 && carrier < 200) &&
0205             (max(stereo, dual) > min(stereo, dual))) {
0206             /*
0207              * For EIAJ the carrier is always present,
0208              * so we probably don't need noise detection
0209              */
0210             return ret;
0211         }
0212     } else {
0213         if ((carrier > max(stereo, dual) * 2) &&
0214             (carrier < max(stereo, dual) * 8) &&
0215             (carrier > 20 && carrier < 200) &&
0216             (noise < 10) &&
0217             (max(stereo, dual) > min(stereo, dual) * 2)) {
0218             return ret;
0219         }
0220     }
0221     return V4L2_TUNER_SUB_MONO;
0222 }
0223
0224 static s32 detect_btsc(struct cx88_core *core, s16 x[], u32 N)
0225 {
0226     s32 sap_ref = freq_magnitude(x, N, FREQ_BTSC_SAP_REF);
0227     s32 sap = freq_magnitude(x, N, FREQ_BTSC_SAP);
0228     s32 dual_ref = freq_magnitude(x, N, FREQ_BTSC_DUAL_REF);
0229     s32 dual = freq_magnitude(x, N, FREQ_BTSC_DUAL);
0230
0231     dprintk(1, "detect btsc: dual_ref=%d, dual=%d, sap_ref=%d, sap=%d\n",
0232         dual_ref, dual, sap_ref, sap);
0233     /* FIXME: Currently not supported */
0234     return UNSET;
0235 }
0236
0237 static s16 *read_rds_samples(struct cx88_core *core, u32 *N)
0238 {
0239     const struct sram_channel *srch = &cx88_sram_channels[SRAM_CH27];
0240     s16 *samples;
0241
0242     unsigned int i;
0243     unsigned int bpl = srch->fifo_size / AUD_RDS_LINES;
0244     unsigned int spl = bpl / 4;
0245     unsigned int sample_count = spl * (AUD_RDS_LINES - 1);
0246
0247     u32 current_address = cx_read(srch->ptr1_reg);
0248     u32 offset = (current_address - srch->fifo_start + bpl);
0249
0250     dprintk(1,
0251         "read RDS samples: current_address=%08x (offset=%08x), sample_count=%d, aud_intstat=%08x\n",
0252         current_address,
0253         current_address - srch->fifo_start, sample_count,
0254         cx_read(MO_AUD_INTSTAT));
0255     samples = kmalloc_array(sample_count, sizeof(*samples), GFP_KERNEL);
0256     if (!samples)
0257         return NULL;
0258
0259     *N = sample_count;
0260
0261     for (i = 0; i < sample_count; i++)  {
0262         offset = offset % (AUD_RDS_LINES * bpl);
0263         samples[i] = cx_read(srch->fifo_start + offset);
0264         offset += 4;
0265     }
0266
0267     dprintk(2, "RDS samples dump: %*ph\n", sample_count, samples);
0268
0269     return samples;
0270 }
0271
0272 s32 cx88_dsp_detect_stereo_sap(struct cx88_core *core)
0273 {
0274     s16 *samples;
0275     u32 N = 0;
0276     s32 ret = UNSET;
0277
0278     /* If audio RDS fifo is disabled, we can't read the samples */
0279     if (!(cx_read(MO_AUD_DMACNTRL) & 0x04))
0280         return ret;
0281     if (!(cx_read(AUD_CTL) & EN_FMRADIO_EN_RDS))
0282         return ret;
0283
0284     /* Wait at least 500 ms after an audio standard change */
0285     if (time_before(jiffies, core->last_change + msecs_to_jiffies(500)))
0286         return ret;
0287
0288     samples = read_rds_samples(core, &N);
0289
0290     if (!samples)
0291         return ret;
0292
0293     switch (core->tvaudio) {
0294     case WW_BG:
0295     case WW_DK:
0296     case WW_EIAJ:
0297     case WW_M:
0298         ret = detect_a2_a2m_eiaj(core, samples, N);
0299         break;
0300     case WW_BTSC:
0301         ret = detect_btsc(core, samples, N);
0302         break;
0303     case WW_NONE:
0304     case WW_I:
0305     case WW_L:
0306     case WW_I2SPT:
0307     case WW_FM:
0308     case WW_I2SADC:
0309         break;
0310     }
0311
0312     kfree(samples);
0313
0314     if (ret != UNSET)
0315         dprintk(1, "stereo/sap detection result:%s%s%s\n",
0316             (ret & V4L2_TUNER_SUB_MONO) ? " mono" : "",
0317             (ret & V4L2_TUNER_SUB_STEREO) ? " stereo" : "",
0318             (ret & V4L2_TUNER_SUB_LANG2) ? " dual" : "");
0319
0320     return ret;
0321 }
0322 EXPORT_SYMBOL(cx88_dsp_detect_stereo_sap);
0323