1/*
2 *
3 *  Stereo and SAP detection for cx88
4 *
5 *  Copyright (c) 2009 Marton Balint <cus@fazekas.hu>
6 *
7 *  This program is free software; you can redistribute it and/or modify
8 *  it under the terms of the GNU General Public License as published by
9 *  the Free Software Foundation; either version 2 of the License, or
10 *  (at your option) any later version.
11 *
12 *  This program is distributed in the hope that it will be useful,
13 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
14 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
15 *  GNU General Public License for more details.
16 *
17 *  You should have received a copy of the GNU General Public License
18 *  along with this program; if not, write to the Free Software
19 *  Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
20 */
21
22#include <linux/slab.h>
23#include <linux/kernel.h>
24#include <linux/module.h>
25#include <linux/jiffies.h>
26#include <asm/div64.h>
27
28#include "cx88.h"
29#include "cx88-reg.h"
30
31#define INT_PI			((s32)(3.141592653589 * 32768.0))
32
33#define compat_remainder(a, b) \
34	 ((float)(((s32)((a)*100))%((s32)((b)*100)))/100.0)
35
36#define baseband_freq(carrier, srate, tone) ((s32)( \
37	 (compat_remainder(carrier + tone, srate)) / srate * 2 * INT_PI))
38
39/* We calculate the baseband frequencies of the carrier and the pilot tones
40 * based on the the sampling rate of the audio rds fifo. */
41
42#define FREQ_A2_CARRIER         baseband_freq(54687.5, 2689.36, 0.0)
43#define FREQ_A2_DUAL            baseband_freq(54687.5, 2689.36, 274.1)
44#define FREQ_A2_STEREO          baseband_freq(54687.5, 2689.36, 117.5)
45
46/* The frequencies below are from the reference driver. They probably need
47 * further adjustments, because they are not tested at all. You may even need
48 * to play a bit with the registers of the chip to select the proper signal
49 * for the input of the audio rds fifo, and measure it's sampling rate to
50 * calculate the proper baseband frequencies... */
51
52#define FREQ_A2M_CARRIER	((s32)(2.114516 * 32768.0))
53#define FREQ_A2M_DUAL		((s32)(2.754916 * 32768.0))
54#define FREQ_A2M_STEREO		((s32)(2.462326 * 32768.0))
55
56#define FREQ_EIAJ_CARRIER	((s32)(1.963495 * 32768.0)) /* 5pi/8  */
57#define FREQ_EIAJ_DUAL		((s32)(2.562118 * 32768.0))
58#define FREQ_EIAJ_STEREO	((s32)(2.601053 * 32768.0))
59
60#define FREQ_BTSC_DUAL		((s32)(1.963495 * 32768.0)) /* 5pi/8  */
61#define FREQ_BTSC_DUAL_REF	((s32)(1.374446 * 32768.0)) /* 7pi/16 */
62
63#define FREQ_BTSC_SAP		((s32)(2.471532 * 32768.0))
64#define FREQ_BTSC_SAP_REF	((s32)(1.730072 * 32768.0))
65
66/* The spectrum of the signal should be empty between these frequencies. */
67#define FREQ_NOISE_START	((s32)(0.100000 * 32768.0))
68#define FREQ_NOISE_END		((s32)(1.200000 * 32768.0))
69
70static unsigned int dsp_debug;
71module_param(dsp_debug, int, 0644);
72MODULE_PARM_DESC(dsp_debug, "enable audio dsp debug messages");
73
74#define dprintk(level, fmt, arg...)	if (dsp_debug >= level) \
75	printk(KERN_DEBUG "%s/0: " fmt, core->name , ## arg)
76
77static s32 int_cos(u32 x)
78{
79	u32 t2, t4, t6, t8;
80	s32 ret;
81	u16 period = x / INT_PI;
82	if (period % 2)
83		return -int_cos(x - INT_PI);
84	x = x % INT_PI;
85	if (x > INT_PI/2)
86		return -int_cos(INT_PI/2 - (x % (INT_PI/2)));
87	/* Now x is between 0 and INT_PI/2.
88	 * To calculate cos(x) we use it's Taylor polinom. */
89	t2 = x*x/32768/2;
90	t4 = t2*x/32768*x/32768/3/4;
91	t6 = t4*x/32768*x/32768/5/6;
92	t8 = t6*x/32768*x/32768/7/8;
93	ret = 32768-t2+t4-t6+t8;
94	return ret;
95}
96
97static u32 int_goertzel(s16 x[], u32 N, u32 freq)
98{
99	/* We use the Goertzel algorithm to determine the power of the
100	 * given frequency in the signal */
101	s32 s_prev = 0;
102	s32 s_prev2 = 0;
103	s32 coeff = 2*int_cos(freq);
104	u32 i;
105
106	u64 tmp;
107	u32 divisor;
108
109	for (i = 0; i < N; i++) {
110		s32 s = x[i] + ((s64)coeff*s_prev/32768) - s_prev2;
111		s_prev2 = s_prev;
112		s_prev = s;
113	}
114
115	tmp = (s64)s_prev2 * s_prev2 + (s64)s_prev * s_prev -
116		      (s64)coeff * s_prev2 * s_prev / 32768;
117
118	/* XXX: N must be low enough so that N*N fits in s32.
119	 * Else we need two divisions. */
120	divisor = N * N;
121	do_div(tmp, divisor);
122
123	return (u32) tmp;
124}
125
126static u32 freq_magnitude(s16 x[], u32 N, u32 freq)
127{
128	u32 sum = int_goertzel(x, N, freq);
129	return (u32)int_sqrt(sum);
130}
131
132static u32 noise_magnitude(s16 x[], u32 N, u32 freq_start, u32 freq_end)
133{
134	int i;
135	u32 sum = 0;
136	u32 freq_step;
137	int samples = 5;
138
139	if (N > 192) {
140		/* The last 192 samples are enough for noise detection */
141		x += (N-192);
142		N = 192;
143	}
144
145	freq_step = (freq_end - freq_start) / (samples - 1);
146
147	for (i = 0; i < samples; i++) {
148		sum += int_goertzel(x, N, freq_start);
149		freq_start += freq_step;
150	}
151
152	return (u32)int_sqrt(sum / samples);
153}
154
155static s32 detect_a2_a2m_eiaj(struct cx88_core *core, s16 x[], u32 N)
156{
157	s32 carrier, stereo, dual, noise;
158	s32 carrier_freq, stereo_freq, dual_freq;
159	s32 ret;
160
161	switch (core->tvaudio) {
162	case WW_BG:
163	case WW_DK:
164		carrier_freq = FREQ_A2_CARRIER;
165		stereo_freq = FREQ_A2_STEREO;
166		dual_freq = FREQ_A2_DUAL;
167		break;
168	case WW_M:
169		carrier_freq = FREQ_A2M_CARRIER;
170		stereo_freq = FREQ_A2M_STEREO;
171		dual_freq = FREQ_A2M_DUAL;
172		break;
173	case WW_EIAJ:
174		carrier_freq = FREQ_EIAJ_CARRIER;
175		stereo_freq = FREQ_EIAJ_STEREO;
176		dual_freq = FREQ_EIAJ_DUAL;
177		break;
178	default:
179		printk(KERN_WARNING "%s/0: unsupported audio mode %d for %s\n",
180		       core->name, core->tvaudio, __func__);
181		return UNSET;
182	}
183
184	carrier = freq_magnitude(x, N, carrier_freq);
185	stereo  = freq_magnitude(x, N, stereo_freq);
186	dual    = freq_magnitude(x, N, dual_freq);
187	noise   = noise_magnitude(x, N, FREQ_NOISE_START, FREQ_NOISE_END);
188
189	dprintk(1, "detect a2/a2m/eiaj: carrier=%d, stereo=%d, dual=%d, "
190		   "noise=%d\n", carrier, stereo, dual, noise);
191
192	if (stereo > dual)
193		ret = V4L2_TUNER_SUB_STEREO;
194	else
195		ret = V4L2_TUNER_SUB_LANG1 | V4L2_TUNER_SUB_LANG2;
196
197	if (core->tvaudio == WW_EIAJ) {
198		/* EIAJ checks may need adjustments */
199		if ((carrier > max(stereo, dual)*2) &&
200		    (carrier < max(stereo, dual)*6) &&
201		    (carrier > 20 && carrier < 200) &&
202		    (max(stereo, dual) > min(stereo, dual))) {
203			/* For EIAJ the carrier is always present,
204			   so we probably don't need noise detection */
205			return ret;
206		}
207	} else {
208		if ((carrier > max(stereo, dual)*2) &&
209		    (carrier < max(stereo, dual)*8) &&
210		    (carrier > 20 && carrier < 200) &&
211		    (noise < 10) &&
212		    (max(stereo, dual) > min(stereo, dual)*2)) {
213			return ret;
214		}
215	}
216	return V4L2_TUNER_SUB_MONO;
217}
218
219static s32 detect_btsc(struct cx88_core *core, s16 x[], u32 N)
220{
221	s32 sap_ref = freq_magnitude(x, N, FREQ_BTSC_SAP_REF);
222	s32 sap = freq_magnitude(x, N, FREQ_BTSC_SAP);
223	s32 dual_ref = freq_magnitude(x, N, FREQ_BTSC_DUAL_REF);
224	s32 dual = freq_magnitude(x, N, FREQ_BTSC_DUAL);
225	dprintk(1, "detect btsc: dual_ref=%d, dual=%d, sap_ref=%d, sap=%d"
226		   "\n", dual_ref, dual, sap_ref, sap);
227	/* FIXME: Currently not supported */
228	return UNSET;
229}
230
231static s16 *read_rds_samples(struct cx88_core *core, u32 *N)
232{
233	const struct sram_channel *srch = &cx88_sram_channels[SRAM_CH27];
234	s16 *samples;
235
236	unsigned int i;
237	unsigned int bpl = srch->fifo_size/AUD_RDS_LINES;
238	unsigned int spl = bpl/4;
239	unsigned int sample_count = spl*(AUD_RDS_LINES-1);
240
241	u32 current_address = cx_read(srch->ptr1_reg);
242	u32 offset = (current_address - srch->fifo_start + bpl);
243
244	dprintk(1, "read RDS samples: current_address=%08x (offset=%08x), "
245		"sample_count=%d, aud_intstat=%08x\n", current_address,
246		current_address - srch->fifo_start, sample_count,
247		cx_read(MO_AUD_INTSTAT));
248
249	samples = kmalloc(sizeof(s16)*sample_count, GFP_KERNEL);
250	if (!samples)
251		return NULL;
252
253	*N = sample_count;
254
255	for (i = 0; i < sample_count; i++)  {
256		offset = offset % (AUD_RDS_LINES*bpl);
257		samples[i] = cx_read(srch->fifo_start + offset);
258		offset += 4;
259	}
260
261	if (dsp_debug >= 2) {
262		dprintk(2, "RDS samples dump: ");
263		for (i = 0; i < sample_count; i++)
264			printk("%hd ", samples[i]);
265		printk(".\n");
266	}
267
268	return samples;
269}
270
271s32 cx88_dsp_detect_stereo_sap(struct cx88_core *core)
272{
273	s16 *samples;
274	u32 N = 0;
275	s32 ret = UNSET;
276
277	/* If audio RDS fifo is disabled, we can't read the samples */
278	if (!(cx_read(MO_AUD_DMACNTRL) & 0x04))
279		return ret;
280	if (!(cx_read(AUD_CTL) & EN_FMRADIO_EN_RDS))
281		return ret;
282
283	/* Wait at least 500 ms after an audio standard change */
284	if (time_before(jiffies, core->last_change + msecs_to_jiffies(500)))
285		return ret;
286
287	samples = read_rds_samples(core, &N);
288
289	if (!samples)
290		return ret;
291
292	switch (core->tvaudio) {
293	case WW_BG:
294	case WW_DK:
295	case WW_EIAJ:
296	case WW_M:
297		ret = detect_a2_a2m_eiaj(core, samples, N);
298		break;
299	case WW_BTSC:
300		ret = detect_btsc(core, samples, N);
301		break;
302	case WW_NONE:
303	case WW_I:
304	case WW_L:
305	case WW_I2SPT:
306	case WW_FM:
307	case WW_I2SADC:
308		break;
309	}
310
311	kfree(samples);
312
313	if (UNSET != ret)
314		dprintk(1, "stereo/sap detection result:%s%s%s\n",
315			   (ret & V4L2_TUNER_SUB_MONO) ? " mono" : "",
316			   (ret & V4L2_TUNER_SUB_STEREO) ? " stereo" : "",
317			   (ret & V4L2_TUNER_SUB_LANG2) ? " dual" : "");
318
319	return ret;
320}
321EXPORT_SYMBOL(cx88_dsp_detect_stereo_sap);
322
323