sys/dev/aurateconv.c

   1 /*      $NetBSD: aurateconv.c,v 1.17 2006/11/16 01:32:44 christos Exp $ */
   2
   3 /*-
   4  * Copyright (c) 2002 The NetBSD Foundation, Inc.
   5  * All rights reserved.
   6  *
   7  * This code is derived from software contributed to The NetBSD Foundation
   8  * by TAMURA Kent
   9  *
  10  * Redistribution and use in source and binary forms, with or without
  11  * modification, are permitted provided that the following conditions
  12  * are met:
  13  * 1. Redistributions of source code must retain the above copyright
  14  *    notice, this list of conditions and the following disclaimer.
  15  * 2. Redistributions in binary form must reproduce the above copyright
  16  *    notice, this list of conditions and the following disclaimer in the
  17  *    documentation and/or other materials provided with the distribution.
  18  *
  19  * THIS SOFTWARE IS PROVIDED BY THE NETBSD FOUNDATION, INC. AND CONTRIBUTORS
  20  * ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
  21  * TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
  22  * PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE FOUNDATION OR CONTRIBUTORS
  23  * BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
  24  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
  25  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
  26  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
  27  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
  28  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  29  * POSSIBILITY OF SUCH DAMAGE.
  30  */
  31
  32 #include <sys/cdefs.h>
  33 __KERNEL_RCSID(0, "$NetBSD: aurateconv.c,v 1.17 2006/11/16 01:32:44 christos Exp $");
  34
  35 #include <sys/systm.h>
  36 #include <sys/types.h>
  37 #include <sys/device.h>
  38 #include <sys/errno.h>
  39 #include <sys/malloc.h>
  40 #include <sys/select.h>
  41 #include <sys/audioio.h>
  42
  43 #include <dev/audio_if.h>
  44 #include <dev/audiovar.h>
  45 #include <dev/auconv.h>
  46
  47 #ifndef _KERNEL
  48 #include <stdio.h>
  49 #include <string.h>
  50 #endif
  51
  52 /* #define AURATECONV_DEBUG */
  53 #ifdef AURATECONV_DEBUG
  54 #define DPRINTF(x)      printf x
  55 #else
  56 #define DPRINTF(x)
  57 #endif
  58
  59 typedef struct aurateconv {
  60         stream_filter_t base;
  61         audio_params_t from;
  62         audio_params_t to;
  63         long    count;
  64         int32_t prev[AUDIO_MAX_CHANNELS];
  65         int32_t next[AUDIO_MAX_CHANNELS];
  66 } aurateconv_t;
  67
  68 static int aurateconv_fetch_to(stream_fetcher_t *, audio_stream_t *, int);
  69 static void aurateconv_dtor(stream_filter_t *);
  70 static int aurateconv_slinear16_LE(aurateconv_t *, audio_stream_t *,
  71                                    int, int, int);
  72 static int aurateconv_slinear24_LE(aurateconv_t *, audio_stream_t *,
  73                                    int, int, int);
  74 static int aurateconv_slinear32_LE(aurateconv_t *, audio_stream_t *,
  75                                    int, int, int);
  76 static int aurateconv_slinear16_BE(aurateconv_t *, audio_stream_t *,
  77                                    int, int, int);
  78 static int aurateconv_slinear24_BE(aurateconv_t *, audio_stream_t *,
  79                                    int, int, int);
  80 static int aurateconv_slinear32_BE(aurateconv_t *, audio_stream_t *,
  81                                    int, int, int);
  82
  83 static int32_t int32_mask[33] = {
  84         0x0, 0x80000000, 0xc0000000, 0xe0000000,
  85         0xf0000000, 0xf8000000, 0xfc000000, 0xfe000000,
  86         0xff000000, 0xff800000, 0xffc00000, 0xffe00000,
  87         0xfff00000, 0xfff80000, 0xfffc0000, 0xfffe0000,
  88         0xffff0000, 0xffff8000, 0xffffc000, 0xffffe000,
  89         0xfffff000, 0xfffff800, 0xfffffc00, 0xfffffe00,
  90         0xffffff00, 0xffffff80, 0xffffffc0, 0xffffffe0,
  91         0xfffffff0, 0xfffffff8, 0xfffffffc, 0xfffffffe,
  92         0xffffffff
  93 };
  94
  95 stream_filter_t *
  96 aurateconv(struct audio_softc *sc, const audio_params_t *from,
  97            const audio_params_t *to)
  98 {
  99         aurateconv_t *this;
 100
 101         DPRINTF(("Construct '%s' filter: rate=%u:%u chan=%u:%u prec=%u/%u:%u/"
 102                  "%u enc=%u:%u\n", __func__, from->sample_rate,
 103                  to->sample_rate, from->channels, to->channels,
 104                  from->validbits, from->precision, to->validbits,
 105                  to->precision, from->encoding, to->encoding));
 106 #ifdef DIAGNOSTIC
 107         /* check from/to */
 108         if (from->channels == to->channels
 109             && from->sample_rate == to->sample_rate)
 110                 printf("%s: no conversion\n", __func__); /* No conversion */
 111
 112         if (from->encoding != to->encoding
 113             || from->precision != to->precision
 114             || from->validbits != to->validbits) {
 115                 printf("%s: encoding/precision must not be changed\n", __func__);
 116                 return NULL;
 117         }
 118         if ((from->encoding != AUDIO_ENCODING_SLINEAR_LE
 119              && from->encoding != AUDIO_ENCODING_SLINEAR_BE)
 120             || (from->precision != 16 && from->precision != 24 && from->precision != 32)) {
 121                 printf("%s: encoding/precision must be SLINEAR_LE 16/24/32bit, "
 122                        "or SLINEAR_BE 16/24/32bit", __func__);
 123                 return NULL;
 124         }
 125
 126         if (from->channels > AUDIO_MAX_CHANNELS || from->channels <= 0
 127             || to->channels > AUDIO_MAX_CHANNELS || to->channels <= 0) {
 128                 printf("%s: invalid channels: from=%u to=%u\n",
 129                        __func__, from->channels, to->channels);
 130                 return NULL;
 131         }
 132
 133         if (from->sample_rate <= 0 || to->sample_rate <= 0) {
 134                 printf("%s: invalid sampling rate: from=%u to=%u\n",
 135                        __func__, from->sample_rate, to->sample_rate);
 136                 return NULL;
 137         }
 138 #endif
 139
 140         /* initialize context */
 141         this = malloc(sizeof(aurateconv_t), M_DEVBUF, M_WAITOK | M_ZERO);
 142         this->count = from->sample_rate < to->sample_rate
 143                 ? to->sample_rate + from->sample_rate : 0;
 144         this->from = *from;
 145         this->to = *to;
 146
 147         /* initialize vtbl */
 148         this->base.base.fetch_to = aurateconv_fetch_to;
 149         this->base.dtor = aurateconv_dtor;
 150         this->base.set_fetcher = stream_filter_set_fetcher;
 151         this->base.set_inputbuffer = stream_filter_set_inputbuffer;
 152         return &this->base;
 153 }
 154
 155 static void
 156 aurateconv_dtor(struct stream_filter *this)
 157 {
 158         if (this != NULL)
 159                 free(this, M_DEVBUF);
 160 }
 161
 162 static int
 163 aurateconv_fetch_to(stream_fetcher_t *self, audio_stream_t *dst, int max_used)
 164 {
 165         aurateconv_t *this;
 166         int m, err, frame_dst, frame_src;
 167
 168         this = (aurateconv_t *)self;
 169         frame_dst = (this->to.precision / 8) * this->to.channels;
 170         frame_src = (this->from.precision / 8) * this->from.channels;
 171         max_used = max_used / frame_dst * frame_dst;
 172         if (max_used <= 0)
 173                 max_used = frame_dst;
 174         /* calculate required input size for output max_used bytes */
 175         m = max_used / frame_dst;
 176         m *= this->from.sample_rate;
 177         m /= this->to.sample_rate;
 178         m *= frame_src;
 179         if (m <= 0)
 180                 m = frame_src;
 181
 182         if ((err = this->base.prev->fetch_to(this->base.prev, this->base.src, m)))
 183             return err;
 184         m = (dst->end - dst->start) / frame_dst * frame_dst;
 185         m = min(m, max_used);
 186
 187         switch (this->from.encoding) {
 188         case AUDIO_ENCODING_SLINEAR_LE:
 189                 switch (this->from.precision) {
 190                 case 16:
 191                         return aurateconv_slinear16_LE(this, dst, m,
 192                                                        frame_src, frame_dst);
 193                 case 24:
 194                         return aurateconv_slinear24_LE(this, dst, m,
 195                                                        frame_src, frame_dst);
 196                 case 32:
 197                         return aurateconv_slinear32_LE(this, dst, m,
 198                                                        frame_src, frame_dst);
 199                 }
 200                 break;
 201         case AUDIO_ENCODING_SLINEAR_BE:
 202                 switch (this->from.precision) {
 203                 case 16:
 204                         return aurateconv_slinear16_BE(this, dst, m,
 205                                                        frame_src, frame_dst);
 206                 case 24:
 207                         return aurateconv_slinear24_BE(this, dst, m,
 208                                                        frame_src, frame_dst);
 209                 case 32:
 210                         return aurateconv_slinear32_BE(this, dst, m,
 211                                                        frame_src, frame_dst);
 212                 }
 213                 break;
 214         }
 215         printf("%s: internal error: unsupported encoding: enc=%u prec=%u\n",
 216                __func__, this->from.encoding, this->from.precision);
 217         return 0;
 218 }
 219
 220
 221 #define READ_S8LE(P)            *(const int8_t*)(P)
 222 #define WRITE_S8LE(P, V)        *(int8_t*)(P) = V
 223 #define READ_S8BE(P)            *(const int8_t*)(P)
 224 #define WRITE_S8BE(P, V)        *(int8_t*)(P) = V
 225 #if BYTE_ORDER == LITTLE_ENDIAN
 226 # define READ_S16LE(P)          *(const int16_t*)(P)
 227 # define WRITE_S16LE(P, V)      *(int16_t*)(P) = V
 228 # define READ_S16BE(P)          (int16_t)((P)[0] | ((P)[1]<<8))
 229 # define WRITE_S16BE(P, V)      \
 230         do { \
 231                 int vv = V; \
 232                 (P)[0] = vv; \
 233                 (P)[1] = vv >> 8; \
 234         } while (/*CONSTCOND*/ 0)
 235 # define READ_S32LE(P)          *(const int32_t*)(P)
 236 # define WRITE_S32LE(P, V)      *(int32_t*)(P) = V
 237 # define READ_S32BE(P)          (int32_t)((P)[3] | ((P)[2]<<8) | ((P)[1]<<16) | (((int8_t)((P)[0]))<<24))
 238 # define WRITE_S32BE(P, V)      \
 239         do { \
 240                 int vvv = V; \
 241                 (P)[0] = vvv >> 24; \
 242                 (P)[1] = vvv >> 16; \
 243                 (P)[2] = vvv >> 8; \
 244                 (P)[3] = vvv; \
 245         } while (/*CONSTCOND*/ 0)
 246 #else  /* !LITTLE_ENDIAN */
 247 # define READ_S16LE(P)          (int16_t)((P)[0] | ((P)[1]<<8))
 248 # define WRITE_S16LE(P, V)      \
 249         do { \
 250                 int vv = V; \
 251                 (P)[0] = vv; \
 252                 (P)[1] = vv >> 8; \
 253         } while (/*CONSTCOND*/ 0)
 254 # define READ_S16BE(P)          *(const int16_t*)(P)
 255 # define WRITE_S16BE(P, V)      *(int16_t*)(P) = V
 256 # define READ_S32LE(P)          (int32_t)((P)[0] | ((P)[1]<<8) | ((P)[2]<<16) | (((int8_t)((P)[3]))<<24))
 257 # define WRITE_S32LE(P, V)      \
 258         do { \
 259                 int vvv = V; \
 260                 (P)[0] = vvv; \
 261                 (P)[1] = vvv >> 8; \
 262                 (P)[2] = vvv >> 16; \
 263                 (P)[3] = vvv >> 24; \
 264         } while (/*CONSTCOND*/ 0)
 265 # define READ_S32BE(P)          *(const int32_t*)(P)
 266 # define WRITE_S32BE(P, V)      *(int32_t*)(P) = V
 267 #endif /* !LITTLE_ENDIAN */
 268 #define READ_S24LE(P)           (int32_t)((P)[0] | ((P)[1]<<8) | (((int8_t)((P)[2]))<<16))
 269 #define WRITE_S24LE(P, V)       \
 270         do { \
 271                 int vvv = V; \
 272                 (P)[0] = vvv; \
 273                 (P)[1] = vvv >> 8; \
 274                 (P)[2] = vvv >> 16; \
 275         } while (/*CONSTCOND*/ 0)
 276 #define READ_S24BE(P)           (int32_t)((P)[2] | ((P)[1]<<8) | (((int8_t)((P)[0]))<<16))
 277 #define WRITE_S24BE(P, V)       \
 278         do { \
 279                 int vvv = V; \
 280                 (P)[0] = vvv >> 16; \
 281                 (P)[1] = vvv >> 8; \
 282                 (P)[2] = vvv; \
 283         } while (/*CONSTCOND*/ 0)
 284
 285 #define READ_Sn(BITS, EN, V, STREAM, RP, PAR)   \
 286         do { \
 287                 int j; \
 288                 for (j = 0; j < (PAR)->channels; j++) { \
 289                         (V)[j] = READ_S##BITS##EN(RP); \
 290                         RP = audio_stream_add_outp(STREAM, RP, (BITS) / NBBY); \
 291                 } \
 292         } while (/*CONSTCOND*/ 0)
 293 #define WRITE_Sn(BITS, EN, V, STREAM, WP, FROM, TO)     \
 294         do { \
 295                 if ((FROM)->channels == 2 && (TO)->channels == 1) { \
 296                         WRITE_S##BITS##EN(WP, ((V)[0] + (V)[1]) / 2); \
 297                         WP = audio_stream_add_inp(STREAM, WP, (BITS) / NBBY); \
 298                 } else if (from->channels <= to->channels) { \
 299                         int j; \
 300                         for (j = 0; j < (FROM)->channels; j++) { \
 301                                 WRITE_S##BITS##EN(WP, (V)[j]); \
 302                                 WP = audio_stream_add_inp(STREAM, WP, (BITS) / NBBY); \
 303                         } \
 304                         if (j == 1 && 1 < (TO)->channels) { \
 305                                 WRITE_S##BITS##EN(WP, (V)[0]); \
 306                                 WP = audio_stream_add_inp(STREAM, WP, (BITS) / NBBY); \
 307                                 j++; \
 308                         } \
 309                         for (; j < (TO)->channels; j++) { \
 310                                 WRITE_S##BITS##EN(WP, 0); \
 311                                 WP = audio_stream_add_inp(STREAM, WP, (BITS) / NBBY); \
 312                         } \
 313                 } else {        /* from->channels < to->channels */ \
 314                         int j; \
 315                         for (j = 0; j < (TO)->channels; j++) { \
 316                                 WRITE_S##BITS##EN(WP, (V)[j]); \
 317                                 WP = audio_stream_add_inp(STREAM, WP, (BITS) / NBBY); \
 318                         } \
 319                 } \
 320         } while (/*CONSTCOND*/ 0)
 321
 322 /*
 323  * Function template
 324  *
 325  *   Don't use this for 32bit data because this linear interpolation overflows
 326  *   for 32bit data.
 327  */
 328 #define AURATECONV_SLINEAR(BITS, EN)    \
 329 static int \
 330 aurateconv_slinear##BITS##_##EN (aurateconv_t *this, audio_stream_t *dst, \
 331                                  int m, int frame_src, int frame_dst) \
 332 { \
 333         uint8_t *w; \
 334         const uint8_t *r; \
 335         const audio_params_t *from, *to; \
 336         audio_stream_t *src; \
 337         int32_t v[AUDIO_MAX_CHANNELS]; \
 338         int32_t *prev, *next, c256; \
 339         int i, values_size; \
 340  \
 341         src = this->base.src; \
 342         w = dst->inp; \
 343         r = src->outp; \
 344         DPRINTF(("%s: ENTER w=%p r=%p dst->used=%d src->used=%d\n", \
 345                 __func__, w, r, dst->used, src->used)); \
 346         from = &this->from; \
 347         to = &this->to; \
 348         if (this->from.sample_rate == this->to.sample_rate) { \
 349                 while (dst->used < m && src->used >= frame_src) { \
 350                         READ_Sn(BITS, EN, v, src, r, from); \
 351                         WRITE_Sn(BITS, EN, v, dst, w, from, to); \
 352                 } \
 353         } else if (to->sample_rate < from->sample_rate) { \
 354                 while (dst->used < m && src->used >= frame_src) { \
 355                         READ_Sn(BITS, EN, v, src, r, from); \
 356                         this->count += to->sample_rate; \
 357                         if (this->count >= from->sample_rate) { \
 358                                 this->count -= from->sample_rate; \
 359                                 WRITE_Sn(BITS, EN, v, dst, w, from, to); \
 360                         } \
 361                 } \
 362         } else { \
 363                 /* Initial value of this->count >= to->sample_rate */ \
 364                 values_size = sizeof(int32_t) * from->channels; \
 365                 prev = this->prev; \
 366                 next = this->next; \
 367                 while (dst->used < m \
 368                        && ((this->count >= to->sample_rate && src->used >= frame_src) \
 369                            || this->count < to->sample_rate)) { \
 370                         if (this->count >= to->sample_rate) { \
 371                                 this->count -= to->sample_rate; \
 372                                 memcpy(prev, next, values_size); \
 373                                 READ_Sn(BITS, EN, next, src, r, from); \
 374                         } \
 375                         c256 = this->count * 256 / to->sample_rate; \
 376                         for (i = 0; i < from->channels; i++) \
 377                                 v[i] = (c256 * next[i] + (256 - c256) * prev[i]) >> 8; \
 378                         WRITE_Sn(BITS, EN, v, dst, w, from, to); \
 379                         this->count += from->sample_rate; \
 380                 } \
 381         } \
 382         DPRINTF(("%s: LEAVE w=%p r=%p dst->used=%d src->used=%d\n", \
 383                 __func__, w, r, dst->used, src->used)); \
 384         dst->inp = w; \
 385         src->outp = r; \
 386         return 0; \
 387 }
 388
 389 /*
 390  * Function template for 32bit container
 391  */
 392 #define AURATECONV_SLINEAR32(EN)        \
 393 static int \
 394 aurateconv_slinear32_##EN (aurateconv_t *this, audio_stream_t *dst, \
 395                            int m, int frame_src, int frame_dst) \
 396 { \
 397         uint8_t *w; \
 398         const uint8_t *r; \
 399         const audio_params_t *from, *to; \
 400         audio_stream_t *src; \
 401         int32_t v[AUDIO_MAX_CHANNELS]; \
 402         int32_t *prev, *next; \
 403         int64_t c256, mask; \
 404         int i, values_size, used_src, used_dst; \
 405  \
 406         src = this->base.src; \
 407         w = dst->inp; \
 408         r = src->outp; \
 409         used_dst = audio_stream_get_used(dst); \
 410         used_src = audio_stream_get_used(src); \
 411         from = &this->from; \
 412         to = &this->to; \
 413         if (this->from.sample_rate == this->to.sample_rate) { \
 414                 while (used_dst < m && used_src >= frame_src) { \
 415                         READ_Sn(32, EN, v, src, r, from); \
 416                         used_src -= frame_src; \
 417                         WRITE_Sn(32, EN, v, dst, w, from, to); \
 418                         used_dst += frame_dst; \
 419                 } \
 420         } else if (to->sample_rate < from->sample_rate) { \
 421                 while (used_dst < m && used_src >= frame_src) { \
 422                         READ_Sn(32, EN, v, src, r, from); \
 423                         used_src -= frame_src; \
 424                         this->count += to->sample_rate; \
 425                         if (this->count >= from->sample_rate) { \
 426                                 this->count -= from->sample_rate; \
 427                                 WRITE_Sn(32, EN, v, dst, w, from, to); \
 428                                 used_dst += frame_dst; \
 429                         } \
 430                 } \
 431         } else { \
 432                 /* Initial value of this->count >= to->sample_rate */ \
 433                 values_size = sizeof(int32_t) * from->channels; \
 434                 mask = int32_mask[to->validbits]; \
 435                 prev = this->prev; \
 436                 next = this->next; \
 437                 while (used_dst < m \
 438                        && ((this->count >= to->sample_rate && used_src >= frame_src) \
 439                            || this->count < to->sample_rate)) { \
 440                         if (this->count >= to->sample_rate) { \
 441                                 this->count -= to->sample_rate; \
 442                                 memcpy(prev, next, values_size); \
 443                                 READ_Sn(32, EN, next, src, r, from); \
 444                                 used_src -= frame_src; \
 445                         } \
 446                         c256 = this->count * 256 / to->sample_rate; \
 447                         for (i = 0; i < from->channels; i++) \
 448                                 v[i] = (int32_t)((c256 * next[i] + (INT64_C(256) - c256) * prev[i]) >> 8) & mask; \
 449                         WRITE_Sn(32, EN, v, dst, w, from, to); \
 450                         used_dst += frame_dst; \
 451                         this->count += from->sample_rate; \
 452                 } \
 453         } \
 454         dst->inp = w; \
 455         src->outp = r; \
 456         return 0; \
 457 }
 458
 459 AURATECONV_SLINEAR(16, LE)
 460 AURATECONV_SLINEAR(24, LE)
 461 AURATECONV_SLINEAR32(LE)
 462 AURATECONV_SLINEAR(16, BE)
 463 AURATECONV_SLINEAR(24, BE)
 464 AURATECONV_SLINEAR32(BE)