drivers/media/pci/cx88/cx88-dsp.c - linux-mtk - Git at Google

 /*
  *  Stereo and SAP detection for cx88
  *
  *  Copyright (c) 2009 Marton Balint <cus@fazekas.hu>
  *
  *  This program is free software; you can redistribute it and/or modify
  *  it under the terms of the GNU General Public License as published by
  *  the Free Software Foundation; either version 2 of the License, or
  *  (at your option) any later version.
  *
  *  This program is distributed in the hope that it will be useful,
  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  *  GNU General Public License for more details.
  */

 #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 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);
	/*
	* Stereo and SAP detection for cx88
	*
	* Copyright (c) 2009 Marton Balint <cus@fazekas.hu>
	*
	* This program is free software; you can redistribute it and/or modify
	* it under the terms of the GNU General Public License as published by
	* the Free Software Foundation; either version 2 of the License, or
	* (at your option) any later version.
	*
	* This program is distributed in the hope that it will be useful,
	* but WITHOUT ANY WARRANTY; without even the implied warranty of
	* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
	* GNU General Public License for more details.
	*/

	#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 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);