sys/dev/sound/pcm/feeder_matrix.c

   1 /*-
   2  * Copyright (c) 2008-2009 Ariff Abdullah <ariff@FreeBSD.org>
   3  * All rights reserved.
   4  *
   5  * Redistribution and use in source and binary forms, with or without
   6  * modification, are permitted provided that the following conditions
   7  * are met:
   8  * 1. Redistributions of source code must retain the above copyright
   9  *    notice, this list of conditions and the following disclaimer.
  10  * 2. Redistributions in binary form must reproduce the above copyright
  11  *    notice, this list of conditions and the following disclaimer in the
  12  *    documentation and/or other materials provided with the distribution.
  13  *
  14  * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
  15  * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
  16  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
  17  * ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
  18  * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
  19  * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
  20  * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
  21  * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
  22  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
  23  * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
  24  * SUCH DAMAGE.
  25  */
  26
  27 /*
  28  * feeder_matrix: Generic any-to-any channel matrixing. Probably not the
  29  *                accurate way of doing things, but it should be fast and
  30  *                transparent enough, not to mention capable of handling
  31  *                possible non-standard way of multichannel interleaving
  32  *                order. In other words, it is tough to break.
  33  *
  34  * The Good:
  35  * + very generic and compact, provided that the supplied matrix map is in a
  36  *   sane form.
  37  * + should be fast enough.
  38  *
  39  * The Bad:
  40  * + somebody might disagree with it.
  41  * + 'matrix' is kind of 0x7a69, due to prolong mental block.
  42  */
  43
  44 #ifdef _KERNEL
  45 #ifdef HAVE_KERNEL_OPTION_HEADERS
  46 #include "opt_snd.h"
  47 #endif
  48 #include <dev/sound/pcm/sound.h>
  49 #include <dev/sound/pcm/pcm.h>
  50 #include "feeder_if.h"
  51
  52 #define SND_USE_FXDIV
  53 #include "snd_fxdiv_gen.h"
  54
  55 SND_DECLARE_FILE("$FreeBSD$");
  56 #endif
  57
  58 #define FEEDMATRIX_RESERVOIR    (SND_CHN_MAX * PCM_32_BPS)
  59
  60 #define SND_CHN_T_EOF           0x00e0fe0f
  61 #define SND_CHN_T_NULL          0x0e0e0e0e
  62
  63 struct feed_matrix_info;
  64
  65 typedef void (*feed_matrix_t)(struct feed_matrix_info *, uint8_t *,
  66     uint8_t *, uint32_t);
  67
  68 struct feed_matrix_info {
  69         uint32_t bps;
  70         uint32_t ialign, oalign;
  71         uint32_t in, out;
  72         feed_matrix_t apply;
  73 #ifdef FEEDMATRIX_GENERIC
  74         intpcm_read_t *rd;
  75         intpcm_write_t *wr;
  76 #endif
  77         struct {
  78                 int chn[SND_CHN_T_MAX + 1];
  79                 int mul, shift;
  80         } matrix[SND_CHN_T_MAX + 1];
  81         uint8_t reservoir[FEEDMATRIX_RESERVOIR];
  82 };
  83
  84 static struct pcmchan_matrix feeder_matrix_maps[SND_CHN_MATRIX_MAX] = {
  85         [SND_CHN_MATRIX_1_0] = SND_CHN_MATRIX_MAP_1_0,
  86         [SND_CHN_MATRIX_2_0] = SND_CHN_MATRIX_MAP_2_0,
  87         [SND_CHN_MATRIX_2_1] = SND_CHN_MATRIX_MAP_2_1,
  88         [SND_CHN_MATRIX_3_0] = SND_CHN_MATRIX_MAP_3_0,
  89         [SND_CHN_MATRIX_3_1] = SND_CHN_MATRIX_MAP_3_1,
  90         [SND_CHN_MATRIX_4_0] = SND_CHN_MATRIX_MAP_4_0,
  91         [SND_CHN_MATRIX_4_1] = SND_CHN_MATRIX_MAP_4_1,
  92         [SND_CHN_MATRIX_5_0] = SND_CHN_MATRIX_MAP_5_0,
  93         [SND_CHN_MATRIX_5_1] = SND_CHN_MATRIX_MAP_5_1,
  94         [SND_CHN_MATRIX_6_0] = SND_CHN_MATRIX_MAP_6_0,
  95         [SND_CHN_MATRIX_6_1] = SND_CHN_MATRIX_MAP_6_1,
  96         [SND_CHN_MATRIX_7_0] = SND_CHN_MATRIX_MAP_7_0,
  97         [SND_CHN_MATRIX_7_1] = SND_CHN_MATRIX_MAP_7_1
  98 };
  99
 100 static int feeder_matrix_default_ids[9] = {
 101         [0] = SND_CHN_MATRIX_UNKNOWN,
 102         [1] = SND_CHN_MATRIX_1,
 103         [2] = SND_CHN_MATRIX_2,
 104         [3] = SND_CHN_MATRIX_3,
 105         [4] = SND_CHN_MATRIX_4,
 106         [5] = SND_CHN_MATRIX_5,
 107         [6] = SND_CHN_MATRIX_6,
 108         [7] = SND_CHN_MATRIX_7,
 109         [8] = SND_CHN_MATRIX_8
 110 };
 111
 112 #ifdef _KERNEL
 113 #define FEEDMATRIX_CLIP_CHECK(...)
 114 #else
 115 #define FEEDMATRIX_CLIP_CHECK(v, BIT)   do {                            \
 116         if ((v) < PCM_S##BIT##_MIN || (v) > PCM_S##BIT##_MAX)           \
 117             errx(1, "\n\n%s(): Sample clipping: %jd\n",                 \
 118                 __func__, (intmax_t)(v));                               \
 119 } while (0)
 120 #endif
 121
 122 #define FEEDMATRIX_DECLARE(SIGN, BIT, ENDIAN)                           \
 123 static void                                                             \
 124 feed_matrix_##SIGN##BIT##ENDIAN(struct feed_matrix_info *info,          \
 125     uint8_t *src, uint8_t *dst, uint32_t count)                         \
 126 {                                                                       \
 127         intpcm64_t accum;                                               \
 128         intpcm_t v;                                                     \
 129         int i, j;                                                       \
 130                                                                         \
 131         do {                                                            \
 132                 for (i = 0; info->matrix[i].chn[0] != SND_CHN_T_EOF;    \
 133                     i++) {                                              \
 134                         if (info->matrix[i].chn[0] == SND_CHN_T_NULL) { \
 135                                 _PCM_WRITE_##SIGN##BIT##_##ENDIAN(dst,  \
 136                                     0);                                 \
 137                                 dst += PCM_##BIT##_BPS;                 \
 138                                 continue;                               \
 139                         } else if (info->matrix[i].chn[1] ==            \
 140                             SND_CHN_T_EOF) {                            \
 141                                 v = _PCM_READ_##SIGN##BIT##_##ENDIAN(   \
 142                                     src + info->matrix[i].chn[0]);      \
 143                                 _PCM_WRITE_##SIGN##BIT##_##ENDIAN(dst,  \
 144                                     v);                                 \
 145                                 dst += PCM_##BIT##_BPS;                 \
 146                                 continue;                               \
 147                         }                                               \
 148                                                                         \
 149                         accum = 0;                                      \
 150                         for (j = 0;                                     \
 151                             info->matrix[i].chn[j] != SND_CHN_T_EOF;    \
 152                             j++) {                                      \
 153                                 v = _PCM_READ_##SIGN##BIT##_##ENDIAN(   \
 154                                     src + info->matrix[i].chn[j]);      \
 155                                 accum += v;                             \
 156                         }                                               \
 157                                                                         \
 158                         accum = (accum * info->matrix[i].mul) >>        \
 159                             info->matrix[i].shift;                      \
 160                                                                         \
 161                         FEEDMATRIX_CLIP_CHECK(accum, BIT);              \
 162                                                                         \
 163                         v = (accum > PCM_S##BIT##_MAX) ?                \
 164                             PCM_S##BIT##_MAX :                          \
 165                             ((accum < PCM_S##BIT##_MIN) ?               \
 166                             PCM_S##BIT##_MIN :                          \
 167                             accum);                                     \
 168                         _PCM_WRITE_##SIGN##BIT##_##ENDIAN(dst, v);      \
 169                         dst += PCM_##BIT##_BPS;                         \
 170                 }                                                       \
 171                 src += info->ialign;                                    \
 172         } while (--count != 0);                                         \
 173 }
 174
 175 #if BYTE_ORDER == LITTLE_ENDIAN || defined(SND_FEEDER_MULTIFORMAT)
 176 FEEDMATRIX_DECLARE(S, 16, LE)
 177 FEEDMATRIX_DECLARE(S, 32, LE)
 178 #endif
 179 #if BYTE_ORDER == BIG_ENDIAN || defined(SND_FEEDER_MULTIFORMAT)
 180 FEEDMATRIX_DECLARE(S, 16, BE)
 181 FEEDMATRIX_DECLARE(S, 32, BE)
 182 #endif
 183 #ifdef SND_FEEDER_MULTIFORMAT
 184 FEEDMATRIX_DECLARE(S,  8, NE)
 185 FEEDMATRIX_DECLARE(S, 24, LE)
 186 FEEDMATRIX_DECLARE(S, 24, BE)
 187 FEEDMATRIX_DECLARE(U,  8, NE)
 188 FEEDMATRIX_DECLARE(U, 16, LE)
 189 FEEDMATRIX_DECLARE(U, 24, LE)
 190 FEEDMATRIX_DECLARE(U, 32, LE)
 191 FEEDMATRIX_DECLARE(U, 16, BE)
 192 FEEDMATRIX_DECLARE(U, 24, BE)
 193 FEEDMATRIX_DECLARE(U, 32, BE)
 194 #endif
 195
 196 #define FEEDMATRIX_ENTRY(SIGN, BIT, ENDIAN)                             \
 197         {                                                               \
 198                 AFMT_##SIGN##BIT##_##ENDIAN,                            \
 199                 feed_matrix_##SIGN##BIT##ENDIAN                         \
 200         }
 201
 202 static const struct {
 203         uint32_t format;
 204         feed_matrix_t apply;
 205 } feed_matrix_tab[] = {
 206 #if BYTE_ORDER == LITTLE_ENDIAN || defined(SND_FEEDER_MULTIFORMAT)
 207         FEEDMATRIX_ENTRY(S, 16, LE),
 208         FEEDMATRIX_ENTRY(S, 32, LE),
 209 #endif
 210 #if BYTE_ORDER == BIG_ENDIAN || defined(SND_FEEDER_MULTIFORMAT)
 211         FEEDMATRIX_ENTRY(S, 16, BE),
 212         FEEDMATRIX_ENTRY(S, 32, BE),
 213 #endif
 214 #ifdef SND_FEEDER_MULTIFORMAT
 215         FEEDMATRIX_ENTRY(S,  8, NE),
 216         FEEDMATRIX_ENTRY(S, 24, LE),
 217         FEEDMATRIX_ENTRY(S, 24, BE),
 218         FEEDMATRIX_ENTRY(U,  8, NE),
 219         FEEDMATRIX_ENTRY(U, 16, LE),
 220         FEEDMATRIX_ENTRY(U, 24, LE),
 221         FEEDMATRIX_ENTRY(U, 32, LE),
 222         FEEDMATRIX_ENTRY(U, 16, BE),
 223         FEEDMATRIX_ENTRY(U, 24, BE),
 224         FEEDMATRIX_ENTRY(U, 32, BE)
 225 #endif
 226 };
 227
 228 static void
 229 feed_matrix_reset(struct feed_matrix_info *info)
 230 {
 231         uint32_t i, j;
 232
 233         for (i = 0; i < (sizeof(info->matrix) / sizeof(info->matrix[0])); i++) {
 234                 for (j = 0;
 235                     j < (sizeof(info->matrix[i].chn) /
 236                     sizeof(info->matrix[i].chn[0])); j++) {
 237                         info->matrix[i].chn[j] = SND_CHN_T_EOF;
 238                 }
 239                 info->matrix[i].mul   = 1;
 240                 info->matrix[i].shift = 0;
 241         }
 242 }
 243
 244 #ifdef FEEDMATRIX_GENERIC
 245 static void
 246 feed_matrix_apply_generic(struct feed_matrix_info *info,
 247     uint8_t *src, uint8_t *dst, uint32_t count)
 248 {
 249         intpcm64_t accum;
 250         intpcm_t v;
 251         int i, j;
 252
 253         do {
 254                 for (i = 0; info->matrix[i].chn[0] != SND_CHN_T_EOF;
 255                     i++) {
 256                         if (info->matrix[i].chn[0] == SND_CHN_T_NULL) {
 257                                 info->wr(dst, 0);
 258                                 dst += info->bps;
 259                                 continue;
 260                         } else if (info->matrix[i].chn[1] ==
 261                             SND_CHN_T_EOF) {
 262                                 v = info->rd(src + info->matrix[i].chn[0]);
 263                                 info->wr(dst, v);
 264                                 dst += info->bps;
 265                                 continue;
 266                         }
 267
 268                         accum = 0;
 269                         for (j = 0;
 270                             info->matrix[i].chn[j] != SND_CHN_T_EOF;
 271                             j++) {
 272                                 v = info->rd(src + info->matrix[i].chn[j]);
 273                                 accum += v;
 274                         }
 275
 276                         accum = (accum * info->matrix[i].mul) >>
 277                             info->matrix[i].shift;
 278
 279                         FEEDMATRIX_CLIP_CHECK(accum, 32);
 280
 281                         v = (accum > PCM_S32_MAX) ? PCM_S32_MAX :
 282                             ((accum < PCM_S32_MIN) ? PCM_S32_MIN : accum);
 283                         info->wr(dst, v);
 284                         dst += info->bps;
 285                 }
 286                 src += info->ialign;
 287         } while (--count != 0);
 288 }
 289 #endif
 290
 291 static int
 292 feed_matrix_setup(struct feed_matrix_info *info, struct pcmchan_matrix *m_in,
 293     struct pcmchan_matrix *m_out)
 294 {
 295         uint32_t i, j, ch, in_mask, merge_mask;
 296         int mul, shift;
 297
 298
 299         if (info == NULL || m_in == NULL || m_out == NULL ||
 300             AFMT_CHANNEL(info->in) != m_in->channels ||
 301             AFMT_CHANNEL(info->out) != m_out->channels ||
 302             m_in->channels < SND_CHN_MIN || m_in->channels > SND_CHN_MAX ||
 303             m_out->channels < SND_CHN_MIN || m_out->channels > SND_CHN_MAX)
 304                 return (EINVAL);
 305
 306         feed_matrix_reset(info);
 307
 308         /*
 309          * If both in and out are part of standard matrix and identical, skip
 310          * everything alltogether.
 311          */
 312         if (m_in->id == m_out->id && !(m_in->id < SND_CHN_MATRIX_BEGIN ||
 313             m_in->id > SND_CHN_MATRIX_END))
 314                 return (0);
 315
 316         /*
 317          * Special case for mono input matrix. If the output supports
 318          * possible 'center' channel, route it there. Otherwise, let it be
 319          * matrixed to left/right.
 320          */
 321         if (m_in->id == SND_CHN_MATRIX_1_0) {
 322                 if (m_out->id == SND_CHN_MATRIX_1_0)
 323                         in_mask = SND_CHN_T_MASK_FL;
 324                 else if (m_out->mask & SND_CHN_T_MASK_FC)
 325                         in_mask = SND_CHN_T_MASK_FC;
 326                 else
 327                         in_mask = SND_CHN_T_MASK_FL | SND_CHN_T_MASK_FR;
 328         } else
 329                 in_mask = m_in->mask;
 330
 331         /* Merge, reduce, expand all possibilites. */
 332         for (ch = SND_CHN_T_BEGIN; ch <= SND_CHN_T_END &&
 333             m_out->map[ch].type != SND_CHN_T_MAX; ch += SND_CHN_T_STEP) {
 334                 merge_mask = m_out->map[ch].members & in_mask;
 335                 if (merge_mask == 0) {
 336                         info->matrix[ch].chn[0] = SND_CHN_T_NULL;
 337                         continue;
 338                 }
 339
 340                 j = 0;
 341                 for (i = SND_CHN_T_BEGIN; i <= SND_CHN_T_END;
 342                     i += SND_CHN_T_STEP) {
 343                         if (merge_mask & (1 << i)) {
 344                                 if (m_in->offset[i] >= 0 &&
 345                                     m_in->offset[i] < (int)m_in->channels)
 346                                         info->matrix[ch].chn[j++] =
 347                                             m_in->offset[i] * info->bps;
 348                                 else {
 349                                         info->matrix[ch].chn[j++] =
 350                                             SND_CHN_T_EOF;
 351                                         break;
 352                                 }
 353                         }
 354                 }
 355
 356 #define FEEDMATRIX_ATTN_SHIFT   16
 357
 358                 if (j > 1) {
 359                         /*
 360                          * XXX For channel that require accumulation from
 361                          * multiple channels, apply a slight attenuation to
 362                          * avoid clipping.
 363                          */
 364                         mul   = (1 << (FEEDMATRIX_ATTN_SHIFT - 1)) + 143 - j;
 365                         shift = FEEDMATRIX_ATTN_SHIFT;
 366                         while ((mul & 1) == 0 && shift > 0) {
 367                                 mul >>= 1;
 368                                 shift--;
 369                         }
 370                         info->matrix[ch].mul   = mul;
 371                         info->matrix[ch].shift = shift;
 372                 }
 373         }
 374
 375 #ifndef _KERNEL
 376         fprintf(stderr, "Total: %d\n", ch);
 377
 378         for (i = 0; info->matrix[i].chn[0] != SND_CHN_T_EOF; i++) {
 379                 fprintf(stderr, "%d: [", i);
 380                 for (j = 0; info->matrix[i].chn[j] != SND_CHN_T_EOF; j++) {
 381                         if (j != 0)
 382                                 fprintf(stderr, ", ");
 383                         fprintf(stderr, "%d",
 384                             (info->matrix[i].chn[j] == SND_CHN_T_NULL) ?
 385                             0xffffffff : info->matrix[i].chn[j] / info->bps);
 386                 }
 387                 fprintf(stderr, "] attn: (x * %d) >> %d\n",
 388                     info->matrix[i].mul, info->matrix[i].shift);
 389         }
 390 #endif
 391
 392         return (0);
 393 }
 394
 395 static int
 396 feed_matrix_init(struct pcm_feeder *f)
 397 {
 398         struct feed_matrix_info *info;
 399         struct pcmchan_matrix *m_in, *m_out;
 400         uint32_t i;
 401         int ret;
 402
 403         if (AFMT_ENCODING(f->desc->in) != AFMT_ENCODING(f->desc->out))
 404                 return (EINVAL);
 405
 406         info = malloc(sizeof(*info), M_DEVBUF, M_NOWAIT | M_ZERO);
 407         if (info == NULL)
 408                 return (ENOMEM);
 409
 410         info->in = f->desc->in;
 411         info->out = f->desc->out;
 412         info->bps = AFMT_BPS(info->in);
 413         info->ialign = AFMT_ALIGN(info->in);
 414         info->oalign = AFMT_ALIGN(info->out);
 415         info->apply = NULL;
 416
 417         for (i = 0; info->apply == NULL &&
 418             i < (sizeof(feed_matrix_tab) / sizeof(feed_matrix_tab[0])); i++) {
 419                 if (AFMT_ENCODING(info->in) == feed_matrix_tab[i].format)
 420                         info->apply = feed_matrix_tab[i].apply;
 421         }
 422
 423         if (info->apply == NULL) {
 424 #ifdef FEEDMATRIX_GENERIC
 425                 info->rd = feeder_format_read_op(info->in);
 426                 info->wr = feeder_format_write_op(info->out);
 427                 if (info->rd == NULL || info->wr == NULL) {
 428                         free(info, M_DEVBUF);
 429                         return (EINVAL);
 430                 }
 431                 info->apply = feed_matrix_apply_generic;
 432 #else
 433                 free(info, M_DEVBUF);
 434                 return (EINVAL);
 435 #endif
 436         }
 437
 438         m_in  = feeder_matrix_format_map(info->in);
 439         m_out = feeder_matrix_format_map(info->out);
 440
 441         ret = feed_matrix_setup(info, m_in, m_out);
 442         if (ret != 0) {
 443                 free(info, M_DEVBUF);
 444                 return (ret);
 445         }
 446
 447         f->data = info;
 448
 449         return (0);
 450 }
 451
 452 static int
 453 feed_matrix_free(struct pcm_feeder *f)
 454 {
 455         struct feed_matrix_info *info;
 456
 457         info = f->data;
 458         if (info != NULL)
 459                 free(info, M_DEVBUF);
 460
 461         f->data = NULL;
 462
 463         return (0);
 464 }
 465
 466 static int
 467 feed_matrix_feed(struct pcm_feeder *f, struct pcm_channel *c, uint8_t *b,
 468     uint32_t count, void *source)
 469 {
 470         struct feed_matrix_info *info;
 471         uint32_t j, inmax;
 472         uint8_t *src, *dst;
 473
 474         info = f->data;
 475         if (info->matrix[0].chn[0] == SND_CHN_T_EOF)
 476                 return (FEEDER_FEED(f->source, c, b, count, source));
 477
 478         dst = b;
 479         count = SND_FXROUND(count, info->oalign);
 480         inmax = info->ialign + info->oalign;
 481
 482         /*
 483          * This loop might look simmilar to other feeder_* loops, but be
 484          * advised: matrixing might involve overlapping (think about
 485          * swapping end to front or something like that). In this regard it
 486          * might be simmilar to feeder_format, but feeder_format works on
 487          * 'sample' domain where it can be fitted into single 32bit integer
 488          * while matrixing works on 'sample frame' domain.
 489          */
 490         do {
 491                 if (count < info->oalign)
 492                         break;
 493
 494                 if (count < inmax) {
 495                         src = info->reservoir;
 496                         j = info->ialign;
 497                 } else {
 498                         if (info->ialign == info->oalign)
 499                                 j = count - info->oalign;
 500                         else if (info->ialign > info->oalign)
 501                                 j = SND_FXROUND(count - info->oalign,
 502                                     info->ialign);
 503                         else
 504                                 j = (SND_FXDIV(count, info->oalign) - 1) *
 505                                     info->ialign;
 506                         src = dst + count - j;
 507                 }
 508
 509                 j = SND_FXDIV(FEEDER_FEED(f->source, c, src, j, source),
 510                     info->ialign);
 511                 if (j == 0)
 512                         break;
 513
 514                 info->apply(info, src, dst, j);
 515
 516                 j *= info->oalign;
 517                 dst += j;
 518                 count -= j;
 519
 520         } while (count != 0);
 521
 522         return (dst - b);
 523 }
 524
 525 static struct pcm_feederdesc feeder_matrix_desc[] = {
 526         { FEEDER_MATRIX, 0, 0, 0, 0 },
 527         { 0, 0, 0, 0, 0 }
 528 };
 529
 530 static kobj_method_t feeder_matrix_methods[] = {
 531         KOBJMETHOD(feeder_init,         feed_matrix_init),
 532         KOBJMETHOD(feeder_free,         feed_matrix_free),
 533         KOBJMETHOD(feeder_feed,         feed_matrix_feed),
 534         KOBJMETHOD_END
 535 };
 536
 537 FEEDER_DECLARE(feeder_matrix, NULL);
 538
 539 /* External */
 540 int
 541 feeder_matrix_setup(struct pcm_feeder *f, struct pcmchan_matrix *m_in,
 542     struct pcmchan_matrix *m_out)
 543 {
 544
 545         if (f == NULL || f->desc == NULL || f->desc->type != FEEDER_MATRIX ||
 546             f->data == NULL)
 547                 return (EINVAL);
 548
 549         return (feed_matrix_setup(f->data, m_in, m_out));
 550 }
 551
 552 /*
 553  * feeder_matrix_default_id(): For a given number of channels, return
 554  *                             default prefered id (example: both 5.1 and
 555  *                             6.0 are simply 6 channels, but 5.1 is more
 556  *                             preferable).
 557  */
 558 int
 559 feeder_matrix_default_id(uint32_t ch)
 560 {
 561
 562         if (ch < feeder_matrix_maps[SND_CHN_MATRIX_BEGIN].channels ||
 563             ch > feeder_matrix_maps[SND_CHN_MATRIX_END].channels)
 564                 return (SND_CHN_MATRIX_UNKNOWN);
 565
 566         return (feeder_matrix_maps[feeder_matrix_default_ids[ch]].id);
 567 }
 568
 569 /*
 570  * feeder_matrix_default_channel_map(): Ditto, but return matrix map
 571  *                                      instead.
 572  */
 573 struct pcmchan_matrix *
 574 feeder_matrix_default_channel_map(uint32_t ch)
 575 {
 576
 577         if (ch < feeder_matrix_maps[SND_CHN_MATRIX_BEGIN].channels ||
 578             ch > feeder_matrix_maps[SND_CHN_MATRIX_END].channels)
 579                 return (NULL);
 580
 581         return (&feeder_matrix_maps[feeder_matrix_default_ids[ch]]);
 582 }
 583
 584 /*
 585  * feeder_matrix_default_format(): For a given audio format, return the
 586  *                                 proper audio format based on preferable
 587  *                                 matrix.
 588  */
 589 uint32_t
 590 feeder_matrix_default_format(uint32_t format)
 591 {
 592         struct pcmchan_matrix *m;
 593         uint32_t i, ch, ext;
 594
 595         ch = AFMT_CHANNEL(format);
 596         ext = AFMT_EXTCHANNEL(format);
 597
 598         if (ext != 0) {
 599                 for (i = SND_CHN_MATRIX_BEGIN; i <= SND_CHN_MATRIX_END; i++) {
 600                         if (feeder_matrix_maps[i].channels == ch &&
 601                             feeder_matrix_maps[i].ext == ext)
 602                         return (SND_FORMAT(format, ch, ext));
 603                 }
 604         }
 605
 606         m = feeder_matrix_default_channel_map(ch);
 607         if (m == NULL)
 608                 return (0x00000000);
 609
 610         return (SND_FORMAT(format, ch, m->ext));
 611 }
 612
 613 /*
 614  * feeder_matrix_format_id(): For a given audio format, return its matrix
 615  *                            id.
 616  */
 617 int
 618 feeder_matrix_format_id(uint32_t format)
 619 {
 620         uint32_t i, ch, ext;
 621
 622         ch = AFMT_CHANNEL(format);
 623         ext = AFMT_EXTCHANNEL(format);
 624
 625         for (i = SND_CHN_MATRIX_BEGIN; i <= SND_CHN_MATRIX_END; i++) {
 626                 if (feeder_matrix_maps[i].channels == ch &&
 627                     feeder_matrix_maps[i].ext == ext)
 628                         return (feeder_matrix_maps[i].id);
 629         }
 630
 631         return (SND_CHN_MATRIX_UNKNOWN);
 632 }
 633
 634 /*
 635  * feeder_matrix_format_map(): For a given audio format, return its matrix
 636  *                             map.
 637  */
 638 struct pcmchan_matrix *
 639 feeder_matrix_format_map(uint32_t format)
 640 {
 641         uint32_t i, ch, ext;
 642
 643         ch = AFMT_CHANNEL(format);
 644         ext = AFMT_EXTCHANNEL(format);
 645
 646         for (i = SND_CHN_MATRIX_BEGIN; i <= SND_CHN_MATRIX_END; i++) {
 647                 if (feeder_matrix_maps[i].channels == ch &&
 648                     feeder_matrix_maps[i].ext == ext)
 649                         return (&feeder_matrix_maps[i]);
 650         }
 651
 652         return (NULL);
 653 }
 654
 655 /*
 656  * feeder_matrix_id_map(): For a given matrix id, return its matrix map.
 657  */
 658 struct pcmchan_matrix *
 659 feeder_matrix_id_map(int id)
 660 {
 661
 662         if (id < SND_CHN_MATRIX_BEGIN || id > SND_CHN_MATRIX_END)
 663                 return (NULL);
 664
 665         return (&feeder_matrix_maps[id]);
 666 }
 667
 668 /*
 669  * feeder_matrix_compare(): Compare the simmilarities of matrices.
 670  */
 671 int
 672 feeder_matrix_compare(struct pcmchan_matrix *m_in, struct pcmchan_matrix *m_out)
 673 {
 674         uint32_t i;
 675
 676         if (m_in == m_out)
 677                 return (0);
 678
 679         if (m_in->channels != m_out->channels || m_in->ext != m_out->ext ||
 680             m_in->mask != m_out->mask)
 681                 return (1);
 682
 683         for (i = 0; i < (sizeof(m_in->map) / sizeof(m_in->map[0])); i++) {
 684                 if (m_in->map[i].type != m_out->map[i].type)
 685                         return (1);
 686                 if (m_in->map[i].type == SND_CHN_T_MAX)
 687                         break;
 688                 if (m_in->map[i].members != m_out->map[i].members)
 689                         return (1);
 690                 if (i <= SND_CHN_T_END) {
 691                         if (m_in->offset[m_in->map[i].type] !=
 692                             m_out->offset[m_out->map[i].type])
 693                                 return (1);
 694                 }
 695         }
 696
 697         return (0);
 698 }
 699
 700 /*
 701  * XXX 4front intepretation of "surround" is ambigous and sort of
 702  *     conflicting with "rear"/"back". Map it to "side". Well..
 703  *     who cares?
 704  */
 705 static int snd_chn_to_oss[SND_CHN_T_MAX] = {
 706         [SND_CHN_T_FL] = CHID_L,
 707         [SND_CHN_T_FR] = CHID_R,
 708         [SND_CHN_T_FC] = CHID_C,
 709         [SND_CHN_T_LF] = CHID_LFE,
 710         [SND_CHN_T_SL] = CHID_LS,
 711         [SND_CHN_T_SR] = CHID_RS,
 712         [SND_CHN_T_BL] = CHID_LR,
 713         [SND_CHN_T_BR] = CHID_RR
 714 };
 715
 716 #define SND_CHN_OSS_VALIDMASK                                           \
 717                         (SND_CHN_T_MASK_FL | SND_CHN_T_MASK_FR |        \
 718                          SND_CHN_T_MASK_FC | SND_CHN_T_MASK_LF |        \
 719                          SND_CHN_T_MASK_SL | SND_CHN_T_MASK_SR |        \
 720                          SND_CHN_T_MASK_BL | SND_CHN_T_MASK_BR)
 721
 722 #define SND_CHN_OSS_MAX         8
 723 #define SND_CHN_OSS_BEGIN       CHID_L
 724 #define SND_CHN_OSS_END         CHID_RR
 725
 726 static int oss_to_snd_chn[SND_CHN_OSS_END + 1] = {
 727         [CHID_L]   = SND_CHN_T_FL,
 728         [CHID_R]   = SND_CHN_T_FR,
 729         [CHID_C]   = SND_CHN_T_FC,
 730         [CHID_LFE] = SND_CHN_T_LF,
 731         [CHID_LS]  = SND_CHN_T_SL,
 732         [CHID_RS]  = SND_CHN_T_SR,
 733         [CHID_LR]  = SND_CHN_T_BL,
 734         [CHID_RR]  = SND_CHN_T_BR
 735 };
 736
 737 /*
 738  * Used by SNDCTL_DSP_GET_CHNORDER.
 739  */
 740 int
 741 feeder_matrix_oss_get_channel_order(struct pcmchan_matrix *m,
 742     unsigned long long *map)
 743 {
 744         unsigned long long tmpmap;
 745         uint32_t i;
 746
 747         if (m == NULL || map == NULL || (m->mask & ~SND_CHN_OSS_VALIDMASK) ||
 748             m->channels > SND_CHN_OSS_MAX)
 749                 return (EINVAL);
 750
 751         tmpmap = 0x0000000000000000ULL;
 752
 753         for (i = 0; m->map[i].type != SND_CHN_T_MAX &&
 754             i < SND_CHN_OSS_MAX; i++) {
 755                 if ((1 << m->map[i].type) & ~SND_CHN_OSS_VALIDMASK)
 756                         return (EINVAL);
 757                 tmpmap |=
 758                     (unsigned long long)snd_chn_to_oss[m->map[i].type] <<
 759                     (i * 4);
 760         }
 761
 762         *map = tmpmap;
 763
 764         return (0);
 765 }
 766
 767 /*
 768  * Used by SNDCTL_DSP_SET_CHNORDER.
 769  */
 770 int
 771 feeder_matrix_oss_set_channel_order(struct pcmchan_matrix *m,
 772     unsigned long long *map)
 773 {
 774         struct pcmchan_matrix tmp;
 775         uint32_t chmask, i;
 776         int ch, cheof;
 777
 778         if (m == NULL || map == NULL || (m->mask & ~SND_CHN_OSS_VALIDMASK) ||
 779             m->channels > SND_CHN_OSS_MAX || (*map & 0xffffffff00000000ULL))
 780                 return (EINVAL);
 781
 782         tmp = *m;
 783         tmp.channels = 0;
 784         tmp.ext = 0;
 785         tmp.mask = 0;
 786         memset(tmp.offset, -1, sizeof(tmp.offset));
 787         cheof = 0;
 788
 789         for (i = 0; i < SND_CHN_OSS_MAX; i++) {
 790                 ch = (*map >> (i * 4)) & 0xf;
 791                 if (ch < SND_CHN_OSS_BEGIN) {
 792                         if (cheof == 0 && m->map[i].type != SND_CHN_T_MAX)
 793                                 return (EINVAL);
 794                         cheof++;
 795                         tmp.map[i] = m->map[i];
 796                         continue;
 797                 } else if (ch > SND_CHN_OSS_END)
 798                         return (EINVAL);
 799                 else if (cheof != 0)
 800                         return (EINVAL);
 801                 ch = oss_to_snd_chn[ch];
 802                 chmask = 1 << ch;
 803                 /* channel not exist in matrix */
 804                 if (!(chmask & m->mask))
 805                         return (EINVAL);
 806                 /* duplicated channel */
 807                 if (chmask & tmp.mask)
 808                         return (EINVAL);
 809                 tmp.map[i] = m->map[m->offset[ch]];
 810                 if (tmp.map[i].type != ch)
 811                         return (EINVAL);
 812                 tmp.offset[ch] = i;
 813                 tmp.mask |= chmask;
 814                 tmp.channels++;
 815                 if (chmask & SND_CHN_T_MASK_LF)
 816                         tmp.ext++;
 817         }
 818
 819         if (tmp.channels != m->channels || tmp.ext != m->ext ||
 820             tmp.mask != m->mask ||
 821             tmp.map[m->channels].type != SND_CHN_T_MAX)
 822                 return (EINVAL);
 823
 824         *m = tmp;
 825
 826         return (0);
 827 }