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