324 lines · c
1// SPDX-License-Identifier: GPL-2.0-or-later2/*3 * Stereo and SAP detection for cx884 *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 tones27 * based on 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 need36 * further adjustments, because they are not tested at all. You may even need37 * to play a bit with the registers of the chip to select the proper signal38 * for the input of the audio rds fifo, and measure it's sampling rate to39 * 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 the97 * given frequency in the signal98 */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 else198 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 detection209 */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