diff options
Diffstat (limited to 'drivers/media/pci/cx88/cx88-dsp.c')
| -rw-r--r-- | drivers/media/pci/cx88/cx88-dsp.c | 323 |
1 files changed, 323 insertions, 0 deletions
diff --git a/drivers/media/pci/cx88/cx88-dsp.c b/drivers/media/pci/cx88/cx88-dsp.c new file mode 100644 index 000000000..e378f3b21 --- /dev/null +++ b/drivers/media/pci/cx88/cx88-dsp.c @@ -0,0 +1,323 @@ +// SPDX-License-Identifier: GPL-2.0-or-later +/* + * Stereo and SAP detection for cx88 + * + * Copyright (c) 2009 Marton Balint <cus@fazekas.hu> + */ + +#include "cx88.h" +#include "cx88-reg.h" + +#include <linux/slab.h> +#include <linux/kernel.h> +#include <linux/module.h> +#include <linux/jiffies.h> +#include <asm/div64.h> + +#define INT_PI ((s32)(3.141592653589 * 32768.0)) + +#define compat_remainder(a, b) \ + ((float)(((s32)((a) * 100)) % ((s32)((b) * 100))) / 100.0) + +#define baseband_freq(carrier, srate, tone) ((s32)( \ + (compat_remainder(carrier + tone, srate)) / srate * 2 * INT_PI)) + +/* + * We calculate the baseband frequencies of the carrier and the pilot tones + * based on the sampling rate of the audio rds fifo. + */ + +#define FREQ_A2_CARRIER baseband_freq(54687.5, 2689.36, 0.0) +#define FREQ_A2_DUAL baseband_freq(54687.5, 2689.36, 274.1) +#define FREQ_A2_STEREO baseband_freq(54687.5, 2689.36, 117.5) + +/* + * The frequencies below are from the reference driver. They probably need + * further adjustments, because they are not tested at all. You may even need + * to play a bit with the registers of the chip to select the proper signal + * for the input of the audio rds fifo, and measure it's sampling rate to + * calculate the proper baseband frequencies... + */ + +#define FREQ_A2M_CARRIER ((s32)(2.114516 * 32768.0)) +#define FREQ_A2M_DUAL ((s32)(2.754916 * 32768.0)) +#define FREQ_A2M_STEREO ((s32)(2.462326 * 32768.0)) + +#define FREQ_EIAJ_CARRIER ((s32)(1.963495 * 32768.0)) /* 5pi/8 */ +#define FREQ_EIAJ_DUAL ((s32)(2.562118 * 32768.0)) +#define FREQ_EIAJ_STEREO ((s32)(2.601053 * 32768.0)) + +#define FREQ_BTSC_DUAL ((s32)(1.963495 * 32768.0)) /* 5pi/8 */ +#define FREQ_BTSC_DUAL_REF ((s32)(1.374446 * 32768.0)) /* 7pi/16 */ + +#define FREQ_BTSC_SAP ((s32)(2.471532 * 32768.0)) +#define FREQ_BTSC_SAP_REF ((s32)(1.730072 * 32768.0)) + +/* The spectrum of the signal should be empty between these frequencies. */ +#define FREQ_NOISE_START ((s32)(0.100000 * 32768.0)) +#define FREQ_NOISE_END ((s32)(1.200000 * 32768.0)) + +static unsigned int dsp_debug; +module_param(dsp_debug, int, 0644); +MODULE_PARM_DESC(dsp_debug, "enable audio dsp debug messages"); + +#define dprintk(level, fmt, arg...) do { \ + if (dsp_debug >= level) \ + printk(KERN_DEBUG pr_fmt("%s: dsp:" fmt), \ + __func__, ##arg); \ +} while (0) + +static s32 int_cos(u32 x) +{ + u32 t2, t4, t6, t8; + s32 ret; + u16 period = x / INT_PI; + + if (period % 2) + return -int_cos(x - INT_PI); + x = x % INT_PI; + if (x > INT_PI / 2) + return -int_cos(INT_PI / 2 - (x % (INT_PI / 2))); + /* + * Now x is between 0 and INT_PI/2. + * To calculate cos(x) we use it's Taylor polinom. + */ + t2 = x * x / 32768 / 2; + t4 = t2 * x / 32768 * x / 32768 / 3 / 4; + t6 = t4 * x / 32768 * x / 32768 / 5 / 6; + t8 = t6 * x / 32768 * x / 32768 / 7 / 8; + ret = 32768 - t2 + t4 - t6 + t8; + return ret; +} + +static u32 int_goertzel(s16 x[], u32 N, u32 freq) +{ + /* + * We use the Goertzel algorithm to determine the power of the + * given frequency in the signal + */ + s32 s_prev = 0; + s32 s_prev2 = 0; + s32 coeff = 2 * int_cos(freq); + u32 i; + + u64 tmp; + u32 divisor; + + for (i = 0; i < N; i++) { + s32 s = x[i] + ((s64)coeff * s_prev / 32768) - s_prev2; + + s_prev2 = s_prev; + s_prev = s; + } + + tmp = (s64)s_prev2 * s_prev2 + (s64)s_prev * s_prev - + (s64)coeff * s_prev2 * s_prev / 32768; + + /* + * XXX: N must be low enough so that N*N fits in s32. + * Else we need two divisions. + */ + divisor = N * N; + do_div(tmp, divisor); + + return (u32)tmp; +} + +static u32 freq_magnitude(s16 x[], u32 N, u32 freq) +{ + u32 sum = int_goertzel(x, N, freq); + + return (u32)int_sqrt(sum); +} + +static u32 noise_magnitude(s16 x[], u32 N, u32 freq_start, u32 freq_end) +{ + int i; + u32 sum = 0; + u32 freq_step; + int samples = 5; + + if (N > 192) { + /* The last 192 samples are enough for noise detection */ + x += (N - 192); + N = 192; + } + + freq_step = (freq_end - freq_start) / (samples - 1); + + for (i = 0; i < samples; i++) { + sum += int_goertzel(x, N, freq_start); + freq_start += freq_step; + } + + return (u32)int_sqrt(sum / samples); +} + +static s32 detect_a2_a2m_eiaj(struct cx88_core *core, s16 x[], u32 N) +{ + s32 carrier, stereo, dual, noise; + s32 carrier_freq, stereo_freq, dual_freq; + s32 ret; + + switch (core->tvaudio) { + case WW_BG: + case WW_DK: + carrier_freq = FREQ_A2_CARRIER; + stereo_freq = FREQ_A2_STEREO; + dual_freq = FREQ_A2_DUAL; + break; + case WW_M: + carrier_freq = FREQ_A2M_CARRIER; + stereo_freq = FREQ_A2M_STEREO; + dual_freq = FREQ_A2M_DUAL; + break; + case WW_EIAJ: + carrier_freq = FREQ_EIAJ_CARRIER; + stereo_freq = FREQ_EIAJ_STEREO; + dual_freq = FREQ_EIAJ_DUAL; + break; + default: + pr_warn("unsupported audio mode %d for %s\n", + core->tvaudio, __func__); + return UNSET; + } + + carrier = freq_magnitude(x, N, carrier_freq); + stereo = freq_magnitude(x, N, stereo_freq); + dual = freq_magnitude(x, N, dual_freq); + noise = noise_magnitude(x, N, FREQ_NOISE_START, FREQ_NOISE_END); + + dprintk(1, + "detect a2/a2m/eiaj: carrier=%d, stereo=%d, dual=%d, noise=%d\n", + carrier, stereo, dual, noise); + + if (stereo > dual) + ret = V4L2_TUNER_SUB_STEREO; + else + ret = V4L2_TUNER_SUB_LANG1 | V4L2_TUNER_SUB_LANG2; + + if (core->tvaudio == WW_EIAJ) { + /* EIAJ checks may need adjustments */ + if ((carrier > max(stereo, dual) * 2) && + (carrier < max(stereo, dual) * 6) && + (carrier > 20 && carrier < 200) && + (max(stereo, dual) > min(stereo, dual))) { + /* + * For EIAJ the carrier is always present, + * so we probably don't need noise detection + */ + return ret; + } + } else { + if ((carrier > max(stereo, dual) * 2) && + (carrier < max(stereo, dual) * 8) && + (carrier > 20 && carrier < 200) && + (noise < 10) && + (max(stereo, dual) > min(stereo, dual) * 2)) { + return ret; + } + } + return V4L2_TUNER_SUB_MONO; +} + +static s32 detect_btsc(struct cx88_core *core, s16 x[], u32 N) +{ + s32 sap_ref = freq_magnitude(x, N, FREQ_BTSC_SAP_REF); + s32 sap = freq_magnitude(x, N, FREQ_BTSC_SAP); + s32 dual_ref = freq_magnitude(x, N, FREQ_BTSC_DUAL_REF); + s32 dual = freq_magnitude(x, N, FREQ_BTSC_DUAL); + + dprintk(1, "detect btsc: dual_ref=%d, dual=%d, sap_ref=%d, sap=%d\n", + dual_ref, dual, sap_ref, sap); + /* FIXME: Currently not supported */ + return UNSET; +} + +static s16 *read_rds_samples(struct cx88_core *core, u32 *N) +{ + const struct sram_channel *srch = &cx88_sram_channels[SRAM_CH27]; + s16 *samples; + + unsigned int i; + unsigned int bpl = srch->fifo_size / AUD_RDS_LINES; + unsigned int spl = bpl / 4; + unsigned int sample_count = spl * (AUD_RDS_LINES - 1); + + u32 current_address = cx_read(srch->ptr1_reg); + u32 offset = (current_address - srch->fifo_start + bpl); + + dprintk(1, + "read RDS samples: current_address=%08x (offset=%08x), sample_count=%d, aud_intstat=%08x\n", + current_address, + current_address - srch->fifo_start, sample_count, + cx_read(MO_AUD_INTSTAT)); + samples = kmalloc_array(sample_count, sizeof(*samples), GFP_KERNEL); + if (!samples) + return NULL; + + *N = sample_count; + + for (i = 0; i < sample_count; i++) { + offset = offset % (AUD_RDS_LINES * bpl); + samples[i] = cx_read(srch->fifo_start + offset); + offset += 4; + } + + dprintk(2, "RDS samples dump: %*ph\n", sample_count, samples); + + return samples; +} + +s32 cx88_dsp_detect_stereo_sap(struct cx88_core *core) +{ + s16 *samples; + u32 N = 0; + s32 ret = UNSET; + + /* If audio RDS fifo is disabled, we can't read the samples */ + if (!(cx_read(MO_AUD_DMACNTRL) & 0x04)) + return ret; + if (!(cx_read(AUD_CTL) & EN_FMRADIO_EN_RDS)) + return ret; + + /* Wait at least 500 ms after an audio standard change */ + if (time_before(jiffies, core->last_change + msecs_to_jiffies(500))) + return ret; + + samples = read_rds_samples(core, &N); + + if (!samples) + return ret; + + switch (core->tvaudio) { + case WW_BG: + case WW_DK: + case WW_EIAJ: + case WW_M: + ret = detect_a2_a2m_eiaj(core, samples, N); + break; + case WW_BTSC: + ret = detect_btsc(core, samples, N); + break; + case WW_NONE: + case WW_I: + case WW_L: + case WW_I2SPT: + case WW_FM: + case WW_I2SADC: + break; + } + + kfree(samples); + + if (ret != UNSET) + dprintk(1, "stereo/sap detection result:%s%s%s\n", + (ret & V4L2_TUNER_SUB_MONO) ? " mono" : "", + (ret & V4L2_TUNER_SUB_STEREO) ? " stereo" : "", + (ret & V4L2_TUNER_SUB_LANG2) ? " dual" : ""); + + return ret; +} +EXPORT_SYMBOL(cx88_dsp_detect_stereo_sap); + |