]> bbs.cooldavid.org Git - net-next-2.6.git/blame - drivers/media/video/cx88/cx88-dsp.c
Merge git://git.kernel.org/pub/scm/linux/kernel/git/gregkh/staging-2.6
[net-next-2.6.git] / drivers / media / video / cx88 / cx88-dsp.c
CommitLineData
e878cf3a
MB
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
5a0e3ad6 22#include <linux/slab.h>
e878cf3a
MB
23#include <linux/kernel.h>
24#include <linux/module.h>
25#include <linux/jiffies.h>
7561300a 26#include <asm/div64.h>
e878cf3a
MB
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;
7561300a
MS
105
106 u64 tmp;
107 u32 divisor;
108
e878cf3a
MB
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 }
7561300a
MS
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;
e878cf3a
MB
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;
b6b85ff8 178 default:
e878cf3a
MB
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{
2e4e98e7 233 const struct sram_channel *srch = &cx88_sram_channels[SRAM_CH27];
e878cf3a
MB
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:
d06b49ed 295 case WW_EIAJ:
296 case WW_M:
e878cf3a
MB
297 ret = detect_a2_a2m_eiaj(core, samples, N);
298 break;
299 case WW_BTSC:
300 ret = detect_btsc(core, samples, N);
301 break;
d06b49ed 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;
e878cf3a
MB
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