sys/dev/sound/pcm/feeder_matrix.c

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