[ALSA] Use standard bitmap functions
[linux-2.6.git] / sound / core / oss / route.c
1 /*
2  *  Attenuated route Plug-In
3  *  Copyright (c) 2000 by Abramo Bagnara <abramo@alsa-project.org>
4  *
5  *
6  *   This library is free software; you can redistribute it and/or modify
7  *   it under the terms of the GNU Library General Public License as
8  *   published by the Free Software Foundation; either version 2 of
9  *   the License, or (at your option) any later version.
10  *
11  *   This program is distributed in the hope that it will be useful,
12  *   but WITHOUT ANY WARRANTY; without even the implied warranty of
13  *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
14  *   GNU Library General Public License for more details.
15  *
16  *   You should have received a copy of the GNU Library General Public
17  *   License along with this library; if not, write to the Free Software
18  *   Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307 USA
19  *
20  */
21
22 #include <sound/driver.h>
23 #include <linux/slab.h>
24 #include <linux/time.h>
25 #include <sound/core.h>
26 #include <sound/pcm.h>
27 #include "pcm_plugin.h"
28
29 /* The best possible hack to support missing optimization in gcc 2.7.2.3 */
30 #if ROUTE_PLUGIN_RESOLUTION & (ROUTE_PLUGIN_RESOLUTION - 1) != 0
31 #define div(a) a /= ROUTE_PLUGIN_RESOLUTION
32 #elif ROUTE_PLUGIN_RESOLUTION == 16
33 #define div(a) a >>= 4
34 #else
35 #error "Add some code here"
36 #endif
37
38 typedef struct ttable_dst ttable_dst_t;
39 typedef struct route_private_data route_t;
40
41 typedef void (*route_channel_f)(snd_pcm_plugin_t *plugin,
42                               const snd_pcm_plugin_channel_t *src_channels,
43                               snd_pcm_plugin_channel_t *dst_channel,
44                               ttable_dst_t* ttable, snd_pcm_uframes_t frames);
45
46 typedef struct {
47         int channel;
48         int as_int;
49 } ttable_src_t;
50
51 struct ttable_dst {
52         int att;        /* Attenuated */
53         unsigned int nsrcs;
54         ttable_src_t* srcs;
55         route_channel_f func;
56 };
57
58 struct route_private_data {
59         enum {R_UINT32=0, R_UINT64=1} sum_type;
60         int get, put;
61         int conv;
62         int src_sample_size;
63         ttable_dst_t ttable[0];
64 };
65
66 typedef union {
67         u_int32_t as_uint32;
68         u_int64_t as_uint64;
69 } sum_t;
70
71
72 static void route_to_channel_from_zero(snd_pcm_plugin_t *plugin,
73                                      const snd_pcm_plugin_channel_t *src_channels,
74                                      snd_pcm_plugin_channel_t *dst_channel,
75                                      ttable_dst_t* ttable, snd_pcm_uframes_t frames)
76 {
77         if (dst_channel->wanted)
78                 snd_pcm_area_silence(&dst_channel->area, 0, frames, plugin->dst_format.format);
79         dst_channel->enabled = 0;
80 }
81
82 static void route_to_channel_from_one(snd_pcm_plugin_t *plugin,
83                                     const snd_pcm_plugin_channel_t *src_channels,
84                                     snd_pcm_plugin_channel_t *dst_channel,
85                                     ttable_dst_t* ttable, snd_pcm_uframes_t frames)
86 {
87 #define CONV_LABELS
88 #include "plugin_ops.h"
89 #undef CONV_LABELS
90         route_t *data = (route_t *)plugin->extra_data;
91         void *conv;
92         const snd_pcm_plugin_channel_t *src_channel = NULL;
93         unsigned int srcidx;
94         char *src, *dst;
95         int src_step, dst_step;
96         for (srcidx = 0; srcidx < ttable->nsrcs; ++srcidx) {
97                 src_channel = &src_channels[ttable->srcs[srcidx].channel];
98                 if (src_channel->area.addr != NULL)
99                         break;
100         }
101         if (srcidx == ttable->nsrcs) {
102                 route_to_channel_from_zero(plugin, src_channels, dst_channel, ttable, frames);
103                 return;
104         }
105
106         dst_channel->enabled = 1;
107         conv = conv_labels[data->conv];
108         src = src_channel->area.addr + src_channel->area.first / 8;
109         src_step = src_channel->area.step / 8;
110         dst = dst_channel->area.addr + dst_channel->area.first / 8;
111         dst_step = dst_channel->area.step / 8;
112         while (frames-- > 0) {
113                 goto *conv;
114 #define CONV_END after
115 #include "plugin_ops.h"
116 #undef CONV_END
117         after:
118                 src += src_step;
119                 dst += dst_step;
120         }
121 }
122
123 static void route_to_channel(snd_pcm_plugin_t *plugin,
124                            const snd_pcm_plugin_channel_t *src_channels,
125                            snd_pcm_plugin_channel_t *dst_channel,
126                            ttable_dst_t* ttable, snd_pcm_uframes_t frames)
127 {
128 #define GET_U_LABELS
129 #define PUT_U32_LABELS
130 #include "plugin_ops.h"
131 #undef GET_U_LABELS
132 #undef PUT_U32_LABELS
133         static void *zero_labels[2] = { &&zero_int32, &&zero_int64 };
134         /* sum_type att */
135         static void *add_labels[2 * 2] = { &&add_int32_noatt, &&add_int32_att,
136                                     &&add_int64_noatt, &&add_int64_att,
137         };
138         /* sum_type att shift */
139         static void *norm_labels[2 * 2 * 4] = { NULL,
140                                          &&norm_int32_8_noatt,
141                                          &&norm_int32_16_noatt,
142                                          &&norm_int32_24_noatt,
143                                          NULL,
144                                          &&norm_int32_8_att,
145                                          &&norm_int32_16_att,
146                                          &&norm_int32_24_att,
147                                          &&norm_int64_0_noatt,
148                                          &&norm_int64_8_noatt,
149                                          &&norm_int64_16_noatt,
150                                          &&norm_int64_24_noatt,
151                                          &&norm_int64_0_att,
152                                          &&norm_int64_8_att,
153                                          &&norm_int64_16_att,
154                                          &&norm_int64_24_att,
155         };
156         route_t *data = (route_t *)plugin->extra_data;
157         void *zero, *get, *add, *norm, *put_u32;
158         int nsrcs = ttable->nsrcs;
159         char *dst;
160         int dst_step;
161         char *srcs[nsrcs];
162         int src_steps[nsrcs];
163         ttable_src_t src_tt[nsrcs];
164         u_int32_t sample = 0;
165         int srcidx, srcidx1 = 0;
166         for (srcidx = 0; srcidx < nsrcs; ++srcidx) {
167                 const snd_pcm_plugin_channel_t *src_channel = &src_channels[ttable->srcs[srcidx].channel];
168                 if (!src_channel->enabled)
169                         continue;
170                 srcs[srcidx1] = src_channel->area.addr + src_channel->area.first / 8;
171                 src_steps[srcidx1] = src_channel->area.step / 8;
172                 src_tt[srcidx1] = ttable->srcs[srcidx];
173                 srcidx1++;
174         }
175         nsrcs = srcidx1;
176         if (nsrcs == 0) {
177                 route_to_channel_from_zero(plugin, src_channels, dst_channel, ttable, frames);
178                 return;
179         } else if (nsrcs == 1 && src_tt[0].as_int == ROUTE_PLUGIN_RESOLUTION) {
180                 route_to_channel_from_one(plugin, src_channels, dst_channel, ttable, frames);
181                 return;
182         }
183
184         dst_channel->enabled = 1;
185         zero = zero_labels[data->sum_type];
186         get = get_u_labels[data->get];
187         add = add_labels[data->sum_type * 2 + ttable->att];
188         norm = norm_labels[data->sum_type * 8 + ttable->att * 4 + 4 - data->src_sample_size];
189         put_u32 = put_u32_labels[data->put];
190         dst = dst_channel->area.addr + dst_channel->area.first / 8;
191         dst_step = dst_channel->area.step / 8;
192
193         while (frames-- > 0) {
194                 ttable_src_t *ttp = src_tt;
195                 sum_t sum;
196
197                 /* Zero sum */
198                 goto *zero;
199         zero_int32:
200                 sum.as_uint32 = 0;
201                 goto zero_end;
202         zero_int64: 
203                 sum.as_uint64 = 0;
204                 goto zero_end;
205         zero_end:
206                 for (srcidx = 0; srcidx < nsrcs; ++srcidx) {
207                         char *src = srcs[srcidx];
208                         
209                         /* Get sample */
210                         goto *get;
211 #define GET_U_END after_get
212 #include "plugin_ops.h"
213 #undef GET_U_END
214                 after_get:
215
216                         /* Sum */
217                         goto *add;
218                 add_int32_att:
219                         sum.as_uint32 += sample * ttp->as_int;
220                         goto after_sum;
221                 add_int32_noatt:
222                         if (ttp->as_int)
223                                 sum.as_uint32 += sample;
224                         goto after_sum;
225                 add_int64_att:
226                         sum.as_uint64 += (u_int64_t) sample * ttp->as_int;
227                         goto after_sum;
228                 add_int64_noatt:
229                         if (ttp->as_int)
230                                 sum.as_uint64 += sample;
231                         goto after_sum;
232                 after_sum:
233                         srcs[srcidx] += src_steps[srcidx];
234                         ttp++;
235                 }
236                 
237                 /* Normalization */
238                 goto *norm;
239         norm_int32_8_att:
240                 sum.as_uint64 = sum.as_uint32;
241         norm_int64_8_att:
242                 sum.as_uint64 <<= 8;
243         norm_int64_0_att:
244                 div(sum.as_uint64);
245                 goto norm_int;
246
247         norm_int32_16_att:
248                 sum.as_uint64 = sum.as_uint32;
249         norm_int64_16_att:
250                 sum.as_uint64 <<= 16;
251                 div(sum.as_uint64);
252                 goto norm_int;
253
254         norm_int32_24_att:
255                 sum.as_uint64 = sum.as_uint32;
256         norm_int64_24_att:
257                 sum.as_uint64 <<= 24;
258                 div(sum.as_uint64);
259                 goto norm_int;
260
261         norm_int32_8_noatt:
262                 sum.as_uint64 = sum.as_uint32;
263         norm_int64_8_noatt:
264                 sum.as_uint64 <<= 8;
265                 goto norm_int;
266
267         norm_int32_16_noatt:
268                 sum.as_uint64 = sum.as_uint32;
269         norm_int64_16_noatt:
270                 sum.as_uint64 <<= 16;
271                 goto norm_int;
272
273         norm_int32_24_noatt:
274                 sum.as_uint64 = sum.as_uint32;
275         norm_int64_24_noatt:
276                 sum.as_uint64 <<= 24;
277                 goto norm_int;
278
279         norm_int64_0_noatt:
280         norm_int:
281                 if (sum.as_uint64 > (u_int32_t)0xffffffff)
282                         sample = (u_int32_t)0xffffffff;
283                 else
284                         sample = sum.as_uint64;
285                 goto after_norm;
286
287         after_norm:
288                 
289                 /* Put sample */
290                 goto *put_u32;
291 #define PUT_U32_END after_put_u32
292 #include "plugin_ops.h"
293 #undef PUT_U32_END
294         after_put_u32:
295                 
296                 dst += dst_step;
297         }
298 }
299
300 static int route_src_channels_mask(snd_pcm_plugin_t *plugin,
301                                    unsigned long *dst_vmask,
302                                    unsigned long **src_vmask)
303 {
304         route_t *data = (route_t *)plugin->extra_data;
305         int schannels = plugin->src_format.channels;
306         int dchannels = plugin->dst_format.channels;
307         unsigned long *vmask = plugin->src_vmask;
308         int channel;
309         ttable_dst_t *dp = data->ttable;
310         bitmap_zero(vmask, schannels);
311         for (channel = 0; channel < dchannels; channel++, dp++) {
312                 unsigned int src;
313                 ttable_src_t *sp;
314                 if (!test_bit(channel, dst_vmask))
315                         continue;
316                 sp = dp->srcs;
317                 for (src = 0; src < dp->nsrcs; src++, sp++)
318                         set_bit(sp->channel, vmask);
319         }
320         *src_vmask = vmask;
321         return 0;
322 }
323
324 static int route_dst_channels_mask(snd_pcm_plugin_t *plugin,
325                                    unsigned long *src_vmask,
326                                    unsigned long **dst_vmask)
327 {
328         route_t *data = (route_t *)plugin->extra_data;
329         int dchannels = plugin->dst_format.channels;
330         unsigned long *vmask = plugin->dst_vmask;
331         int channel;
332         ttable_dst_t *dp = data->ttable;
333         bitmap_zero(vmask, dchannels);
334         for (channel = 0; channel < dchannels; channel++, dp++) {
335                 unsigned int src;
336                 ttable_src_t *sp;
337                 sp = dp->srcs;
338                 for (src = 0; src < dp->nsrcs; src++, sp++) {
339                         if (test_bit(sp->channel, src_vmask)) {
340                                 set_bit(channel, vmask);
341                                 break;
342                         }
343                 }
344         }
345         *dst_vmask = vmask;
346         return 0;
347 }
348
349 static void route_free(snd_pcm_plugin_t *plugin)
350 {
351         route_t *data = (route_t *)plugin->extra_data;
352         unsigned int dst_channel;
353         for (dst_channel = 0; dst_channel < plugin->dst_format.channels; ++dst_channel) {
354                 kfree(data->ttable[dst_channel].srcs);
355         }
356 }
357
358 static int route_load_ttable(snd_pcm_plugin_t *plugin, 
359                              const route_ttable_entry_t* src_ttable)
360 {
361         route_t *data;
362         unsigned int src_channel, dst_channel;
363         const route_ttable_entry_t *sptr;
364         ttable_dst_t *dptr;
365         if (src_ttable == NULL)
366                 return 0;
367         data = (route_t *)plugin->extra_data;
368         dptr = data->ttable;
369         sptr = src_ttable;
370         plugin->private_free = route_free;
371         for (dst_channel = 0; dst_channel < plugin->dst_format.channels; ++dst_channel) {
372                 route_ttable_entry_t t = 0;
373                 int att = 0;
374                 int nsrcs = 0;
375                 ttable_src_t srcs[plugin->src_format.channels];
376                 for (src_channel = 0; src_channel < plugin->src_format.channels; ++src_channel) {
377                         snd_assert(*sptr >= 0 || *sptr <= FULL, return -ENXIO);
378                         if (*sptr != 0) {
379                                 srcs[nsrcs].channel = src_channel;
380                                 srcs[nsrcs].as_int = *sptr;
381                                 if (*sptr != FULL)
382                                         att = 1;
383                                 t += *sptr;
384                                 nsrcs++;
385                         }
386                         sptr++;
387                 }
388                 dptr->att = att;
389                 dptr->nsrcs = nsrcs;
390                 if (nsrcs == 0)
391                         dptr->func = route_to_channel_from_zero;
392                 else if (nsrcs == 1 && !att)
393                         dptr->func = route_to_channel_from_one;
394                 else
395                         dptr->func = route_to_channel;
396                 if (nsrcs > 0) {
397                         int srcidx;
398                         dptr->srcs = kcalloc(nsrcs, sizeof(*srcs), GFP_KERNEL);
399                         for(srcidx = 0; srcidx < nsrcs; srcidx++)
400                                 dptr->srcs[srcidx] = srcs[srcidx];
401                 } else
402                         dptr->srcs = NULL;
403                 dptr++;
404         }
405         return 0;
406 }
407
408 static snd_pcm_sframes_t route_transfer(snd_pcm_plugin_t *plugin,
409                               const snd_pcm_plugin_channel_t *src_channels,
410                               snd_pcm_plugin_channel_t *dst_channels,
411                               snd_pcm_uframes_t frames)
412 {
413         route_t *data;
414         int src_nchannels, dst_nchannels;
415         int dst_channel;
416         ttable_dst_t *ttp;
417         snd_pcm_plugin_channel_t *dvp;
418
419         snd_assert(plugin != NULL && src_channels != NULL && dst_channels != NULL, return -ENXIO);
420         if (frames == 0)
421                 return 0;
422         data = (route_t *)plugin->extra_data;
423
424         src_nchannels = plugin->src_format.channels;
425         dst_nchannels = plugin->dst_format.channels;
426
427 #ifdef CONFIG_SND_DEBUG
428         {
429                 int src_channel;
430                 for (src_channel = 0; src_channel < src_nchannels; ++src_channel) {
431                         snd_assert(src_channels[src_channel].area.first % 8 == 0 ||
432                                    src_channels[src_channel].area.step % 8 == 0,
433                                    return -ENXIO);
434                 }
435                 for (dst_channel = 0; dst_channel < dst_nchannels; ++dst_channel) {
436                         snd_assert(dst_channels[dst_channel].area.first % 8 == 0 ||
437                                    dst_channels[dst_channel].area.step % 8 == 0,
438                                    return -ENXIO);
439                 }
440         }
441 #endif
442
443         ttp = data->ttable;
444         dvp = dst_channels;
445         for (dst_channel = 0; dst_channel < dst_nchannels; ++dst_channel) {
446                 ttp->func(plugin, src_channels, dvp, ttp, frames);
447                 dvp++;
448                 ttp++;
449         }
450         return frames;
451 }
452
453 int getput_index(int format)
454 {
455         int sign, width, endian;
456         sign = !snd_pcm_format_signed(format);
457         width = snd_pcm_format_width(format) / 8 - 1;
458         if (width < 0 || width > 3) {
459                 snd_printk(KERN_ERR "snd-pcm-oss: invalid format %d\n", format);
460                 width = 0;
461         }
462 #ifdef SNDRV_LITTLE_ENDIAN
463         endian = snd_pcm_format_big_endian(format);
464 #else
465         endian = snd_pcm_format_little_endian(format);
466 #endif
467         if (endian < 0)
468                 endian = 0;
469         return width * 4 + endian * 2 + sign;
470 }
471
472 int snd_pcm_plugin_build_route(snd_pcm_plug_t *plug,
473                                snd_pcm_plugin_format_t *src_format,
474                                snd_pcm_plugin_format_t *dst_format,
475                                route_ttable_entry_t *ttable,
476                                snd_pcm_plugin_t **r_plugin)
477 {
478         route_t *data;
479         snd_pcm_plugin_t *plugin;
480         int err;
481
482         snd_assert(r_plugin != NULL, return -ENXIO);
483         *r_plugin = NULL;
484         snd_assert(src_format->rate == dst_format->rate, return -ENXIO);
485         snd_assert(snd_pcm_format_linear(src_format->format) != 0 &&
486                    snd_pcm_format_linear(dst_format->format) != 0,
487                    return -ENXIO);
488
489         err = snd_pcm_plugin_build(plug, "attenuated route conversion",
490                                    src_format, dst_format,
491                                    sizeof(route_t) + sizeof(data->ttable[0]) * dst_format->channels,
492                                    &plugin);
493         if (err < 0)
494                 return err;
495
496         data = (route_t *) plugin->extra_data;
497
498         data->get = getput_index(src_format->format);
499         snd_assert(data->get >= 0 && data->get < 4*2*2, return -EINVAL);
500         data->put = getput_index(dst_format->format);
501         snd_assert(data->get >= 0 && data->get < 4*2*2, return -EINVAL);
502         data->conv = conv_index(src_format->format, dst_format->format);
503
504         if (snd_pcm_format_width(src_format->format) == 32)
505                 data->sum_type = R_UINT64;
506         else
507                 data->sum_type = R_UINT32;
508         data->src_sample_size = snd_pcm_format_width(src_format->format) / 8;
509
510         if ((err = route_load_ttable(plugin, ttable)) < 0) {
511                 snd_pcm_plugin_free(plugin);
512                 return err;
513         }
514         plugin->transfer = route_transfer;
515         plugin->src_channels_mask = route_src_channels_mask;
516         plugin->dst_channels_mask = route_dst_channels_mask;
517         *r_plugin = plugin;
518         return 0;
519 }