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