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
 301         if (info == NULL || m_in == NULL || m_out == NULL ||
 302             AFMT_CHANNEL(info->in) != m_in->channels ||
 303             AFMT_CHANNEL(info->out) != m_out->channels ||
 304             m_in->channels < SND_CHN_MIN || m_in->channels > SND_CHN_MAX ||
 305             m_out->channels < SND_CHN_MIN || m_out->channels > SND_CHN_MAX)
 306                 return (EINVAL);
 307
 308         feed_matrix_reset(info);
 309
 310         /*
 311          * If both in and out are part of standard matrix and identical, skip
 312          * everything alltogether.
 313          */
 314         if (m_in->id == m_out->id && !(m_in->id < SND_CHN_MATRIX_BEGIN ||
 315             m_in->id > SND_CHN_MATRIX_END))
 316                 return (0);
 317
 318         /*
 319          * Special case for mono input matrix. If the output supports
 320          * possible 'center' channel, route it there. Otherwise, let it be
 321          * matrixed to left/right.
 322          */
 323         if (m_in->id == SND_CHN_MATRIX_1_0) {
 324                 if (m_out->id == SND_CHN_MATRIX_1_0)
 325                         in_mask = SND_CHN_T_MASK_FL;
 326                 else if (m_out->mask & SND_CHN_T_MASK_FC)
 327                         in_mask = SND_CHN_T_MASK_FC;
 328                 else
 329                         in_mask = SND_CHN_T_MASK_FL | SND_CHN_T_MASK_FR;
 330         } else
 331                 in_mask = m_in->mask;
 332
 333         /* Merge, reduce, expand all possibilites. */
 334         for (ch = SND_CHN_T_BEGIN; ch <= SND_CHN_T_END &&
 335             m_out->map[ch].type != SND_CHN_T_MAX; ch += SND_CHN_T_STEP) {
 336                 merge_mask = m_out->map[ch].members & in_mask;
 337                 if (merge_mask == 0) {
 338                         info->matrix[ch].chn[0] = SND_CHN_T_NULL;
 339                         continue;
 340                 }
 341
 342                 j = 0;
 343                 for (i = SND_CHN_T_BEGIN; i <= SND_CHN_T_END;
 344                     i += SND_CHN_T_STEP) {
 345                         if (merge_mask & (1 << i)) {
 346                                 if (m_in->offset[i] >= 0 &&
 347                                     m_in->offset[i] < (int)m_in->channels)
 348                                         info->matrix[ch].chn[j++] =
 349                                             m_in->offset[i] * info->bps;
 350                                 else {
 351                                         info->matrix[ch].chn[j++] =
 352                                             SND_CHN_T_EOF;
 353                                         break;
 354                                 }
 355                         }
 356                 }
 357
 358 #define FEEDMATRIX_ATTN_SHIFT   16
 359
 360                 if (j > 1) {
 361                         /*
 362                          * XXX For channel that require accumulation from
 363                          * multiple channels, apply a slight attenuation to
 364                          * avoid clipping.
 365                          */
 366                         mul   = (1 << (FEEDMATRIX_ATTN_SHIFT - 1)) + 143 - j;
 367                         shift = FEEDMATRIX_ATTN_SHIFT;
 368                         while ((mul & 1) == 0 && shift > 0) {
 369                                 mul >>= 1;
 370                                 shift--;
 371                         }
 372                         info->matrix[ch].mul   = mul;
 373                         info->matrix[ch].shift = shift;
 374                 }
 375         }
 376
 377 #ifndef _KERNEL
 378         fprintf(stderr, "Total: %d\n", ch);
 379
 380         for (i = 0; info->matrix[i].chn[0] != SND_CHN_T_EOF; i++) {
 381                 fprintf(stderr, "%d: [", i);
 382                 for (j = 0; info->matrix[i].chn[j] != SND_CHN_T_EOF; j++) {
 383                         if (j != 0)
 384                                 fprintf(stderr, ", ");
 385                         fprintf(stderr, "%d",
 386                             (info->matrix[i].chn[j] == SND_CHN_T_NULL) ?
 387                             0xffffffff : info->matrix[i].chn[j] / info->bps);
 388                 }
 389                 fprintf(stderr, "] attn: (x * %d) >> %d\n",
 390                     info->matrix[i].mul, info->matrix[i].shift);
 391         }
 392 #endif
 393
 394         return (0);
 395 }
 396
 397 static int
 398 feed_matrix_init(struct pcm_feeder *f)
 399 {
 400         struct feed_matrix_info *info;
 401         struct pcmchan_matrix *m_in, *m_out;
 402         uint32_t i;
 403         int ret;
 404
 405         if (AFMT_ENCODING(f->desc->in) != AFMT_ENCODING(f->desc->out))
 406                 return (EINVAL);
 407
 408         info = malloc(sizeof(*info), M_DEVBUF, M_NOWAIT | M_ZERO);
 409         if (info == NULL)
 410                 return (ENOMEM);
 411
 412         info->in = f->desc->in;
 413         info->out = f->desc->out;
 414         info->bps = AFMT_BPS(info->in);
 415         info->ialign = AFMT_ALIGN(info->in);
 416         info->oalign = AFMT_ALIGN(info->out);
 417         info->apply = NULL;
 418
 419         for (i = 0; info->apply == NULL &&
 420             i < (sizeof(feed_matrix_tab) / sizeof(feed_matrix_tab[0])); i++) {
 421                 if (AFMT_ENCODING(info->in) == feed_matrix_tab[i].format)
 422                         info->apply = feed_matrix_tab[i].apply;
 423         }
 424
 425         if (info->apply == NULL) {
 426 #ifdef FEEDMATRIX_GENERIC
 427                 info->rd = feeder_format_read_op(info->in);
 428                 info->wr = feeder_format_write_op(info->out);
 429                 if (info->rd == NULL || info->wr == NULL) {
 430                         free(info, M_DEVBUF);
 431                         return (EINVAL);
 432                 }
 433                 info->apply = feed_matrix_apply_generic;
 434 #else
 435                 free(info, M_DEVBUF);
 436                 return (EINVAL);
 437 #endif
 438         }
 439
 440         m_in  = feeder_matrix_format_map(info->in);
 441         m_out = feeder_matrix_format_map(info->out);
 442
 443         ret = feed_matrix_setup(info, m_in, m_out);
 444         if (ret != 0) {
 445                 free(info, M_DEVBUF);
 446                 return (ret);
 447         }
 448
 449         f->data = info;
 450
 451         return (0);
 452 }
 453
 454 static int
 455 feed_matrix_free(struct pcm_feeder *f)
 456 {
 457         struct feed_matrix_info *info;
 458
 459         info = f->data;
 460         if (info != NULL)
 461                 free(info, M_DEVBUF);
 462
 463         f->data = NULL;
 464
 465         return (0);
 466 }
 467
 468 static int
 469 feed_matrix_feed(struct pcm_feeder *f, struct pcm_channel *c, uint8_t *b,
 470     uint32_t count, void *source)
 471 {
 472         struct feed_matrix_info *info;
 473         uint32_t j, inmax;
 474         uint8_t *src, *dst;
 475
 476         info = f->data;
 477         if (info->matrix[0].chn[0] == SND_CHN_T_EOF)
 478                 return (FEEDER_FEED(f->source, c, b, count, source));
 479
 480         dst = b;
 481         count = SND_FXROUND(count, info->oalign);
 482         inmax = info->ialign + info->oalign;
 483
 484         /*
 485          * This loop might look simmilar to other feeder_* loops, but be
 486          * advised: matrixing might involve overlapping (think about
 487          * swapping end to front or something like that). In this regard it
 488          * might be simmilar to feeder_format, but feeder_format works on
 489          * 'sample' domain where it can be fitted into single 32bit integer
 490          * while matrixing works on 'sample frame' domain.
 491          */
 492         do {
 493                 if (count < info->oalign)
 494                         break;
 495
 496                 if (count < inmax) {
 497                         src = info->reservoir;
 498                         j = info->ialign;
 499                 } else {
 500                         if (info->ialign == info->oalign)
 501                                 j = count - info->oalign;
 502                         else if (info->ialign > info->oalign)
 503                                 j = SND_FXROUND(count - info->oalign,
 504                                     info->ialign);
 505                         else
 506                                 j = (SND_FXDIV(count, info->oalign) - 1) *
 507                                     info->ialign;
 508                         src = dst + count - j;
 509                 }
 510
 511                 j = SND_FXDIV(FEEDER_FEED(f->source, c, src, j, source),
 512                     info->ialign);
 513                 if (j == 0)
 514                         break;
 515
 516                 info->apply(info, src, dst, j);
 517
 518                 j *= info->oalign;
 519                 dst += j;
 520                 count -= j;
 521
 522         } while (count != 0);
 523
 524         return (dst - b);
 525 }
 526
 527 static struct pcm_feederdesc feeder_matrix_desc[] = {
 528         { FEEDER_MATRIX, 0, 0, 0, 0 },
 529         { 0, 0, 0, 0, 0 }
 530 };
 531
 532 static kobj_method_t feeder_matrix_methods[] = {
 533         KOBJMETHOD(feeder_init,         feed_matrix_init),
 534         KOBJMETHOD(feeder_free,         feed_matrix_free),
 535         KOBJMETHOD(feeder_feed,         feed_matrix_feed),
 536         KOBJMETHOD_END
 537 };
 538
 539 FEEDER_DECLARE(feeder_matrix, NULL);
 540
 541 /* External */
 542 int
 543 feeder_matrix_setup(struct pcm_feeder *f, struct pcmchan_matrix *m_in,
 544     struct pcmchan_matrix *m_out)
 545 {
 546
 547         if (f == NULL || f->desc == NULL || f->desc->type != FEEDER_MATRIX ||
 548             f->data == NULL)
 549                 return (EINVAL);
 550
 551         return (feed_matrix_setup(f->data, m_in, m_out));
 552 }
 553
 554 /*
 555  * feeder_matrix_default_id(): For a given number of channels, return
 556  *                             default prefered id (example: both 5.1 and
 557  *                             6.0 are simply 6 channels, but 5.1 is more
 558  *                             preferable).
 559  */
 560 int
 561 feeder_matrix_default_id(uint32_t ch)
 562 {
 563
 564         if (ch < feeder_matrix_maps[SND_CHN_MATRIX_BEGIN].channels ||
 565             ch > feeder_matrix_maps[SND_CHN_MATRIX_END].channels)
 566                 return (SND_CHN_MATRIX_UNKNOWN);
 567
 568         return (feeder_matrix_maps[feeder_matrix_default_ids[ch]].id);
 569 }
 570
 571 /*
 572  * feeder_matrix_default_channel_map(): Ditto, but return matrix map
 573  *                                      instead.
 574  */
 575 struct pcmchan_matrix *
 576 feeder_matrix_default_channel_map(uint32_t ch)
 577 {
 578
 579         if (ch < feeder_matrix_maps[SND_CHN_MATRIX_BEGIN].channels ||
 580             ch > feeder_matrix_maps[SND_CHN_MATRIX_END].channels)
 581                 return (NULL);
 582
 583         return (&feeder_matrix_maps[feeder_matrix_default_ids[ch]]);
 584 }
 585
 586 /*
 587  * feeder_matrix_default_format(): For a given audio format, return the
 588  *                                 proper audio format based on preferable
 589  *                                 matrix.
 590  */
 591 uint32_t
 592 feeder_matrix_default_format(uint32_t format)
 593 {
 594         struct pcmchan_matrix *m;
 595         uint32_t i, ch, ext;
 596
 597         ch = AFMT_CHANNEL(format);
 598         ext = AFMT_EXTCHANNEL(format);
 599
 600         if (ext != 0) {
 601                 for (i = SND_CHN_MATRIX_BEGIN; i <= SND_CHN_MATRIX_END; i++) {
 602                         if (feeder_matrix_maps[i].channels == ch &&
 603                             feeder_matrix_maps[i].ext == ext)
 604                         return (SND_FORMAT(format, ch, ext));
 605                 }
 606         }
 607
 608         m = feeder_matrix_default_channel_map(ch);
 609         if (m == NULL)
 610                 return (0x00000000);
 611
 612         return (SND_FORMAT(format, ch, m->ext));
 613 }
 614
 615 /*
 616  * feeder_matrix_format_id(): For a given audio format, return its matrix
 617  *                            id.
 618  */
 619 int
 620 feeder_matrix_format_id(uint32_t format)
 621 {
 622         uint32_t i, ch, ext;
 623
 624         ch = AFMT_CHANNEL(format);
 625         ext = AFMT_EXTCHANNEL(format);
 626
 627         for (i = SND_CHN_MATRIX_BEGIN; i <= SND_CHN_MATRIX_END; i++) {
 628                 if (feeder_matrix_maps[i].channels == ch &&
 629                     feeder_matrix_maps[i].ext == ext)
 630                         return (feeder_matrix_maps[i].id);
 631         }
 632
 633         return (SND_CHN_MATRIX_UNKNOWN);
 634 }
 635
 636 /*
 637  * feeder_matrix_format_map(): For a given audio format, return its matrix
 638  *                             map.
 639  */
 640 struct pcmchan_matrix *
 641 feeder_matrix_format_map(uint32_t format)
 642 {
 643         uint32_t i, ch, ext;
 644
 645         ch = AFMT_CHANNEL(format);
 646         ext = AFMT_EXTCHANNEL(format);
 647
 648         for (i = SND_CHN_MATRIX_BEGIN; i <= SND_CHN_MATRIX_END; i++) {
 649                 if (feeder_matrix_maps[i].channels == ch &&
 650                     feeder_matrix_maps[i].ext == ext)
 651                         return (&feeder_matrix_maps[i]);
 652         }
 653
 654         return (NULL);
 655 }
 656
 657 /*
 658  * feeder_matrix_id_map(): For a given matrix id, return its matrix map.
 659  */
 660 struct pcmchan_matrix *
 661 feeder_matrix_id_map(int id)
 662 {
 663
 664         if (id < SND_CHN_MATRIX_BEGIN || id > SND_CHN_MATRIX_END)
 665                 return (NULL);
 666
 667         return (&feeder_matrix_maps[id]);
 668 }
 669
 670 /*
 671  * feeder_matrix_compare(): Compare the simmilarities of matrices.
 672  */
 673 int
 674 feeder_matrix_compare(struct pcmchan_matrix *m_in, struct pcmchan_matrix *m_out)
 675 {
 676         uint32_t i;
 677
 678         if (m_in == m_out)
 679                 return (0);
 680
 681         if (m_in->channels != m_out->channels || m_in->ext != m_out->ext ||
 682             m_in->mask != m_out->mask)
 683                 return (1);
 684
 685         for (i = 0; i < (sizeof(m_in->map) / sizeof(m_in->map[0])); i++) {
 686                 if (m_in->map[i].type != m_out->map[i].type)
 687                         return (1);
 688                 if (m_in->map[i].type == SND_CHN_T_MAX)
 689                         break;
 690                 if (m_in->map[i].members != m_out->map[i].members)
 691                         return (1);
 692                 if (i <= SND_CHN_T_END) {
 693                         if (m_in->offset[m_in->map[i].type] !=
 694                             m_out->offset[m_out->map[i].type])
 695                                 return (1);
 696                 }
 697         }
 698
 699         return (0);
 700 }
 701
 702 /*
 703  * XXX 4front intepretation of "surround" is ambigous and sort of
 704  *     conflicting with "rear"/"back". Map it to "side". Well..
 705  *     who cares?
 706  */
 707 static int snd_chn_to_oss[SND_CHN_T_MAX] = {
 708         [SND_CHN_T_FL] = CHID_L,
 709         [SND_CHN_T_FR] = CHID_R,
 710         [SND_CHN_T_FC] = CHID_C,
 711         [SND_CHN_T_LF] = CHID_LFE,
 712         [SND_CHN_T_SL] = CHID_LS,
 713         [SND_CHN_T_SR] = CHID_RS,
 714         [SND_CHN_T_BL] = CHID_LR,
 715         [SND_CHN_T_BR] = CHID_RR
 716 };
 717
 718 #define SND_CHN_OSS_VALIDMASK                                           \
 719                         (SND_CHN_T_MASK_FL | SND_CHN_T_MASK_FR |        \
 720                          SND_CHN_T_MASK_FC | SND_CHN_T_MASK_LF |        \
 721                          SND_CHN_T_MASK_SL | SND_CHN_T_MASK_SR |        \
 722                          SND_CHN_T_MASK_BL | SND_CHN_T_MASK_BR)
 723
 724 #define SND_CHN_OSS_MAX         8
 725 #define SND_CHN_OSS_BEGIN       CHID_L
 726 #define SND_CHN_OSS_END         CHID_RR
 727
 728 static int oss_to_snd_chn[SND_CHN_OSS_END + 1] = {
 729         [CHID_L]   = SND_CHN_T_FL,
 730         [CHID_R]   = SND_CHN_T_FR,
 731         [CHID_C]   = SND_CHN_T_FC,
 732         [CHID_LFE] = SND_CHN_T_LF,
 733         [CHID_LS]  = SND_CHN_T_SL,
 734         [CHID_RS]  = SND_CHN_T_SR,
 735         [CHID_LR]  = SND_CHN_T_BL,
 736         [CHID_RR]  = SND_CHN_T_BR
 737 };
 738
 739 /*
 740  * Used by SNDCTL_DSP_GET_CHNORDER.
 741  */
 742 int
 743 feeder_matrix_oss_get_channel_order(struct pcmchan_matrix *m,
 744     unsigned long long *map)
 745 {
 746         unsigned long long tmpmap;
 747         uint32_t i;
 748
 749         if (m == NULL || map == NULL || (m->mask & ~SND_CHN_OSS_VALIDMASK) ||
 750             m->channels > SND_CHN_OSS_MAX)
 751                 return (EINVAL);
 752
 753         tmpmap = 0x0000000000000000ULL;
 754
 755         for (i = 0; i < SND_CHN_OSS_MAX && m->map[i].type != SND_CHN_T_MAX;
 756             i++) {
 757                 if ((1 << m->map[i].type) & ~SND_CHN_OSS_VALIDMASK)
 758                         return (EINVAL);
 759                 tmpmap |=
 760                     (unsigned long long)snd_chn_to_oss[m->map[i].type] <<
 761                     (i * 4);
 762         }
 763
 764         *map = tmpmap;
 765
 766         return (0);
 767 }
 768
 769 /*
 770  * Used by SNDCTL_DSP_SET_CHNORDER.
 771  */
 772 int
 773 feeder_matrix_oss_set_channel_order(struct pcmchan_matrix *m,
 774     unsigned long long *map)
 775 {
 776         struct pcmchan_matrix tmp;
 777         uint32_t chmask, i;
 778         int ch, cheof;
 779
 780         if (m == NULL || map == NULL || (m->mask & ~SND_CHN_OSS_VALIDMASK) ||
 781             m->channels > SND_CHN_OSS_MAX || (*map & 0xffffffff00000000ULL))
 782                 return (EINVAL);
 783
 784         tmp = *m;
 785         tmp.channels = 0;
 786         tmp.ext = 0;
 787         tmp.mask = 0;
 788         memset(tmp.offset, -1, sizeof(tmp.offset));
 789         cheof = 0;
 790
 791         for (i = 0; i < SND_CHN_OSS_MAX; i++) {
 792                 ch = (*map >> (i * 4)) & 0xf;
 793                 if (ch < SND_CHN_OSS_BEGIN) {
 794                         if (cheof == 0 && m->map[i].type != SND_CHN_T_MAX)
 795                                 return (EINVAL);
 796                         cheof++;
 797                         tmp.map[i] = m->map[i];
 798                         continue;
 799                 } else if (ch > SND_CHN_OSS_END)
 800                         return (EINVAL);
 801                 else if (cheof != 0)
 802                         return (EINVAL);
 803                 ch = oss_to_snd_chn[ch];
 804                 chmask = 1 << ch;
 805                 /* channel not exist in matrix */
 806                 if (!(chmask & m->mask))
 807                         return (EINVAL);
 808                 /* duplicated channel */
 809                 if (chmask & tmp.mask)
 810                         return (EINVAL);
 811                 tmp.map[i] = m->map[m->offset[ch]];
 812                 if (tmp.map[i].type != ch)
 813                         return (EINVAL);
 814                 tmp.offset[ch] = i;
 815                 tmp.mask |= chmask;
 816                 tmp.channels++;
 817                 if (chmask & SND_CHN_T_MASK_LF)
 818                         tmp.ext++;
 819         }
 820
 821         if (tmp.channels != m->channels || tmp.ext != m->ext ||
 822             tmp.mask != m->mask ||
 823             tmp.map[m->channels].type != SND_CHN_T_MAX)
 824                 return (EINVAL);
 825
 826         *m = tmp;
 827
 828         return (0);
 829 }