1193640Sariff/*- 2193640Sariff * Copyright (c) 2008-2009 Ariff Abdullah <ariff@FreeBSD.org> 3193640Sariff * All rights reserved. 4193640Sariff * 5193640Sariff * Redistribution and use in source and binary forms, with or without 6193640Sariff * modification, are permitted provided that the following conditions 7193640Sariff * are met: 8193640Sariff * 1. Redistributions of source code must retain the above copyright 9193640Sariff * notice, this list of conditions and the following disclaimer. 10193640Sariff * 2. Redistributions in binary form must reproduce the above copyright 11193640Sariff * notice, this list of conditions and the following disclaimer in the 12193640Sariff * documentation and/or other materials provided with the distribution. 13193640Sariff * 14193640Sariff * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND 15193640Sariff * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 16193640Sariff * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 17193640Sariff * ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE 18193640Sariff * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 19193640Sariff * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS 20193640Sariff * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 21193640Sariff * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 22193640Sariff * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY 23193640Sariff * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF 24193640Sariff * SUCH DAMAGE. 25193640Sariff */ 26193640Sariff 27193640Sariff/* 28193640Sariff * feeder_matrix: Generic any-to-any channel matrixing. Probably not the 29193640Sariff * accurate way of doing things, but it should be fast and 30193640Sariff * transparent enough, not to mention capable of handling 31193640Sariff * possible non-standard way of multichannel interleaving 32193640Sariff * order. In other words, it is tough to break. 33193640Sariff * 34193640Sariff * The Good: 35193640Sariff * + very generic and compact, provided that the supplied matrix map is in a 36193640Sariff * sane form. 37193640Sariff * + should be fast enough. 38193640Sariff * 39193640Sariff * The Bad: 40193640Sariff * + somebody might disagree with it. 41193640Sariff * + 'matrix' is kind of 0x7a69, due to prolong mental block. 42193640Sariff */ 43193640Sariff 44193640Sariff#ifdef _KERNEL 45193640Sariff#ifdef HAVE_KERNEL_OPTION_HEADERS 46193640Sariff#include "opt_snd.h" 47193640Sariff#endif 48193640Sariff#include <dev/sound/pcm/sound.h> 49193640Sariff#include <dev/sound/pcm/pcm.h> 50193640Sariff#include "feeder_if.h" 51193640Sariff 52193640Sariff#define SND_USE_FXDIV 53193640Sariff#include "snd_fxdiv_gen.h" 54193640Sariff 55193640SariffSND_DECLARE_FILE("$FreeBSD$"); 56193640Sariff#endif 57193640Sariff 58193640Sariff#define FEEDMATRIX_RESERVOIR (SND_CHN_MAX * PCM_32_BPS) 59193640Sariff 60193640Sariff#define SND_CHN_T_EOF 0x00e0fe0f 61193640Sariff#define SND_CHN_T_NULL 0x0e0e0e0e 62193640Sariff 63193640Sariffstruct feed_matrix_info; 64193640Sariff 65193640Sarifftypedef void (*feed_matrix_t)(struct feed_matrix_info *, uint8_t *, 66193640Sariff uint8_t *, uint32_t); 67193640Sariff 68193640Sariffstruct feed_matrix_info { 69193640Sariff uint32_t bps; 70193640Sariff uint32_t ialign, oalign; 71193640Sariff uint32_t in, out; 72193640Sariff feed_matrix_t apply; 73193640Sariff#ifdef FEEDMATRIX_GENERIC 74193640Sariff intpcm_read_t *rd; 75193640Sariff intpcm_write_t *wr; 76193640Sariff#endif 77193640Sariff struct { 78193640Sariff int chn[SND_CHN_T_MAX + 1]; 79193640Sariff int mul, shift; 80193640Sariff } matrix[SND_CHN_T_MAX + 1]; 81193640Sariff uint8_t reservoir[FEEDMATRIX_RESERVOIR]; 82193640Sariff}; 83193640Sariff 84193640Sariffstatic struct pcmchan_matrix feeder_matrix_maps[SND_CHN_MATRIX_MAX] = { 85193640Sariff [SND_CHN_MATRIX_1_0] = SND_CHN_MATRIX_MAP_1_0, 86193640Sariff [SND_CHN_MATRIX_2_0] = SND_CHN_MATRIX_MAP_2_0, 87193640Sariff [SND_CHN_MATRIX_2_1] = SND_CHN_MATRIX_MAP_2_1, 88193640Sariff [SND_CHN_MATRIX_3_0] = SND_CHN_MATRIX_MAP_3_0, 89193640Sariff [SND_CHN_MATRIX_4_0] = SND_CHN_MATRIX_MAP_4_0, 90193640Sariff [SND_CHN_MATRIX_4_1] = SND_CHN_MATRIX_MAP_4_1, 91193640Sariff [SND_CHN_MATRIX_5_0] = SND_CHN_MATRIX_MAP_5_0, 92193640Sariff [SND_CHN_MATRIX_5_1] = SND_CHN_MATRIX_MAP_5_1, 93193640Sariff [SND_CHN_MATRIX_6_0] = SND_CHN_MATRIX_MAP_6_0, 94193640Sariff [SND_CHN_MATRIX_6_1] = SND_CHN_MATRIX_MAP_6_1, 95193640Sariff [SND_CHN_MATRIX_7_1] = SND_CHN_MATRIX_MAP_7_1 96193640Sariff}; 97193640Sariff 98193640Sariffstatic int feeder_matrix_default_ids[9] = { 99193640Sariff [0] = SND_CHN_MATRIX_UNKNOWN, 100193640Sariff [1] = SND_CHN_MATRIX_1, 101193640Sariff [2] = SND_CHN_MATRIX_2, 102193640Sariff [3] = SND_CHN_MATRIX_3, 103193640Sariff [4] = SND_CHN_MATRIX_4, 104193640Sariff [5] = SND_CHN_MATRIX_5, 105193640Sariff [6] = SND_CHN_MATRIX_6, 106193640Sariff [7] = SND_CHN_MATRIX_7, 107193640Sariff [8] = SND_CHN_MATRIX_8 108193640Sariff}; 109193640Sariff 110193640Sariff#ifdef _KERNEL 111193640Sariff#define FEEDMATRIX_CLIP_CHECK(...) 112193640Sariff#else 113193640Sariff#define FEEDMATRIX_CLIP_CHECK(v, BIT) do { \ 114193640Sariff if ((v) < PCM_S##BIT##_MIN || (v) > PCM_S##BIT##_MAX) \ 115193640Sariff errx(1, "\n\n%s(): Sample clipping: %jd\n", \ 116193640Sariff __func__, (intmax_t)(v)); \ 117193640Sariff} while (0) 118193640Sariff#endif 119193640Sariff 120193640Sariff#define FEEDMATRIX_DECLARE(SIGN, BIT, ENDIAN) \ 121193640Sariffstatic void \ 122193640Sarifffeed_matrix_##SIGN##BIT##ENDIAN(struct feed_matrix_info *info, \ 123193640Sariff uint8_t *src, uint8_t *dst, uint32_t count) \ 124193640Sariff{ \ 125193640Sariff intpcm64_t accum; \ 126193640Sariff intpcm_t v; \ 127193640Sariff int i, j; \ 128193640Sariff \ 129193640Sariff do { \ 130193640Sariff for (i = 0; info->matrix[i].chn[0] != SND_CHN_T_EOF; \ 131193640Sariff i++) { \ 132193640Sariff if (info->matrix[i].chn[0] == SND_CHN_T_NULL) { \ 133193640Sariff _PCM_WRITE_##SIGN##BIT##_##ENDIAN(dst, \ 134193640Sariff 0); \ 135193640Sariff dst += PCM_##BIT##_BPS; \ 136193640Sariff continue; \ 137193640Sariff } else if (info->matrix[i].chn[1] == \ 138193640Sariff SND_CHN_T_EOF) { \ 139193640Sariff v = _PCM_READ_##SIGN##BIT##_##ENDIAN( \ 140193640Sariff src + info->matrix[i].chn[0]); \ 141193640Sariff _PCM_WRITE_##SIGN##BIT##_##ENDIAN(dst, \ 142193640Sariff v); \ 143193640Sariff dst += PCM_##BIT##_BPS; \ 144193640Sariff continue; \ 145193640Sariff } \ 146193640Sariff \ 147193640Sariff accum = 0; \ 148193640Sariff for (j = 0; \ 149193640Sariff info->matrix[i].chn[j] != SND_CHN_T_EOF; \ 150193640Sariff j++) { \ 151193640Sariff v = _PCM_READ_##SIGN##BIT##_##ENDIAN( \ 152193640Sariff src + info->matrix[i].chn[j]); \ 153193640Sariff accum += v; \ 154193640Sariff } \ 155193640Sariff \ 156193640Sariff accum = (accum * info->matrix[i].mul) >> \ 157193640Sariff info->matrix[i].shift; \ 158193640Sariff \ 159193640Sariff FEEDMATRIX_CLIP_CHECK(accum, BIT); \ 160193640Sariff \ 161193640Sariff v = (accum > PCM_S##BIT##_MAX) ? \ 162193640Sariff PCM_S##BIT##_MAX : \ 163193640Sariff ((accum < PCM_S##BIT##_MIN) ? \ 164193640Sariff PCM_S##BIT##_MIN : \ 165193640Sariff accum); \ 166193640Sariff _PCM_WRITE_##SIGN##BIT##_##ENDIAN(dst, v); \ 167193640Sariff dst += PCM_##BIT##_BPS; \ 168193640Sariff } \ 169193640Sariff src += info->ialign; \ 170193640Sariff } while (--count != 0); \ 171193640Sariff} 172193640Sariff 173193640Sariff#if BYTE_ORDER == LITTLE_ENDIAN || defined(SND_FEEDER_MULTIFORMAT) 174193640SariffFEEDMATRIX_DECLARE(S, 16, LE) 175193640SariffFEEDMATRIX_DECLARE(S, 32, LE) 176193640Sariff#endif 177193640Sariff#if BYTE_ORDER == BIG_ENDIAN || defined(SND_FEEDER_MULTIFORMAT) 178193640SariffFEEDMATRIX_DECLARE(S, 16, BE) 179193640SariffFEEDMATRIX_DECLARE(S, 32, BE) 180193640Sariff#endif 181193640Sariff#ifdef SND_FEEDER_MULTIFORMAT 182193640SariffFEEDMATRIX_DECLARE(S, 8, NE) 183193640SariffFEEDMATRIX_DECLARE(S, 24, LE) 184193640SariffFEEDMATRIX_DECLARE(S, 24, BE) 185193640SariffFEEDMATRIX_DECLARE(U, 8, NE) 186193640SariffFEEDMATRIX_DECLARE(U, 16, LE) 187193640SariffFEEDMATRIX_DECLARE(U, 24, LE) 188193640SariffFEEDMATRIX_DECLARE(U, 32, LE) 189193640SariffFEEDMATRIX_DECLARE(U, 16, BE) 190193640SariffFEEDMATRIX_DECLARE(U, 24, BE) 191193640SariffFEEDMATRIX_DECLARE(U, 32, BE) 192193640Sariff#endif 193193640Sariff 194193640Sariff#define FEEDMATRIX_ENTRY(SIGN, BIT, ENDIAN) \ 195193640Sariff { \ 196193640Sariff AFMT_##SIGN##BIT##_##ENDIAN, \ 197193640Sariff feed_matrix_##SIGN##BIT##ENDIAN \ 198193640Sariff } 199193640Sariff 200193640Sariffstatic const struct { 201193640Sariff uint32_t format; 202193640Sariff feed_matrix_t apply; 203193640Sariff} feed_matrix_tab[] = { 204193640Sariff#if BYTE_ORDER == LITTLE_ENDIAN || defined(SND_FEEDER_MULTIFORMAT) 205193640Sariff FEEDMATRIX_ENTRY(S, 16, LE), 206193640Sariff FEEDMATRIX_ENTRY(S, 32, LE), 207193640Sariff#endif 208193640Sariff#if BYTE_ORDER == BIG_ENDIAN || defined(SND_FEEDER_MULTIFORMAT) 209193640Sariff FEEDMATRIX_ENTRY(S, 16, BE), 210193640Sariff FEEDMATRIX_ENTRY(S, 32, BE), 211193640Sariff#endif 212193640Sariff#ifdef SND_FEEDER_MULTIFORMAT 213193640Sariff FEEDMATRIX_ENTRY(S, 8, NE), 214193640Sariff FEEDMATRIX_ENTRY(S, 24, LE), 215193640Sariff FEEDMATRIX_ENTRY(S, 24, BE), 216193640Sariff FEEDMATRIX_ENTRY(U, 8, NE), 217193640Sariff FEEDMATRIX_ENTRY(U, 16, LE), 218193640Sariff FEEDMATRIX_ENTRY(U, 24, LE), 219193640Sariff FEEDMATRIX_ENTRY(U, 32, LE), 220193640Sariff FEEDMATRIX_ENTRY(U, 16, BE), 221193640Sariff FEEDMATRIX_ENTRY(U, 24, BE), 222193640Sariff FEEDMATRIX_ENTRY(U, 32, BE) 223193640Sariff#endif 224193640Sariff}; 225193640Sariff 226193640Sariffstatic void 227193640Sarifffeed_matrix_reset(struct feed_matrix_info *info) 228193640Sariff{ 229193640Sariff uint32_t i, j; 230193640Sariff 231193640Sariff for (i = 0; i < (sizeof(info->matrix) / sizeof(info->matrix[0])); i++) { 232193640Sariff for (j = 0; 233193640Sariff j < (sizeof(info->matrix[i].chn) / 234193640Sariff sizeof(info->matrix[i].chn[0])); j++) { 235193640Sariff info->matrix[i].chn[j] = SND_CHN_T_EOF; 236193640Sariff } 237193640Sariff info->matrix[i].mul = 1; 238193640Sariff info->matrix[i].shift = 0; 239193640Sariff } 240193640Sariff} 241193640Sariff 242193640Sariff#ifdef FEEDMATRIX_GENERIC 243193640Sariffstatic void 244193640Sarifffeed_matrix_apply_generic(struct feed_matrix_info *info, 245193640Sariff uint8_t *src, uint8_t *dst, uint32_t count) 246193640Sariff{ 247193640Sariff intpcm64_t accum; 248193640Sariff intpcm_t v; 249193640Sariff int i, j; 250193640Sariff 251193640Sariff do { 252193640Sariff for (i = 0; info->matrix[i].chn[0] != SND_CHN_T_EOF; 253193640Sariff i++) { 254193640Sariff if (info->matrix[i].chn[0] == SND_CHN_T_NULL) { 255193640Sariff info->wr(dst, 0); 256193640Sariff dst += info->bps; 257193640Sariff continue; 258193640Sariff } else if (info->matrix[i].chn[1] == 259193640Sariff SND_CHN_T_EOF) { 260193640Sariff v = info->rd(src + info->matrix[i].chn[0]); 261193640Sariff info->wr(dst, v); 262193640Sariff dst += info->bps; 263193640Sariff continue; 264193640Sariff } 265193640Sariff 266193640Sariff accum = 0; 267193640Sariff for (j = 0; 268193640Sariff info->matrix[i].chn[j] != SND_CHN_T_EOF; 269193640Sariff j++) { 270193640Sariff v = info->rd(src + info->matrix[i].chn[j]); 271193640Sariff accum += v; 272193640Sariff } 273193640Sariff 274193640Sariff accum = (accum * info->matrix[i].mul) >> 275193640Sariff info->matrix[i].shift; 276193640Sariff 277193640Sariff FEEDMATRIX_CLIP_CHECK(accum, 32); 278193640Sariff 279193640Sariff v = (accum > PCM_S32_MAX) ? PCM_S32_MAX : 280193640Sariff ((accum < PCM_S32_MIN) ? PCM_S32_MIN : accum); 281193640Sariff info->wr(dst, v); 282193640Sariff dst += info->bps; 283193640Sariff } 284193640Sariff src += info->ialign; 285193640Sariff } while (--count != 0); 286193640Sariff} 287193640Sariff#endif 288193640Sariff 289193640Sariffstatic int 290193640Sarifffeed_matrix_setup(struct feed_matrix_info *info, struct pcmchan_matrix *m_in, 291193640Sariff struct pcmchan_matrix *m_out) 292193640Sariff{ 293193640Sariff uint32_t i, j, ch, in_mask, merge_mask; 294193640Sariff int mul, shift; 295193640Sariff 296193640Sariff 297193640Sariff if (info == NULL || m_in == NULL || m_out == NULL || 298193640Sariff AFMT_CHANNEL(info->in) != m_in->channels || 299193640Sariff AFMT_CHANNEL(info->out) != m_out->channels || 300193640Sariff m_in->channels < SND_CHN_MIN || m_in->channels > SND_CHN_MAX || 301193640Sariff m_out->channels < SND_CHN_MIN || m_out->channels > SND_CHN_MAX) 302193640Sariff return (EINVAL); 303193640Sariff 304193640Sariff feed_matrix_reset(info); 305193640Sariff 306193640Sariff /* 307193640Sariff * If both in and out are part of standard matrix and identical, skip 308193640Sariff * everything alltogether. 309193640Sariff */ 310193640Sariff if (m_in->id == m_out->id && !(m_in->id < SND_CHN_MATRIX_BEGIN || 311193640Sariff m_in->id > SND_CHN_MATRIX_END)) 312193640Sariff return (0); 313193640Sariff 314193640Sariff /* 315193640Sariff * Special case for mono input matrix. If the output supports 316193640Sariff * possible 'center' channel, route it there. Otherwise, let it be 317193640Sariff * matrixed to left/right. 318193640Sariff */ 319193640Sariff if (m_in->id == SND_CHN_MATRIX_1_0) { 320193640Sariff if (m_out->id == SND_CHN_MATRIX_1_0) 321193640Sariff in_mask = SND_CHN_T_MASK_FL; 322193640Sariff else if (m_out->mask & SND_CHN_T_MASK_FC) 323193640Sariff in_mask = SND_CHN_T_MASK_FC; 324193640Sariff else 325193640Sariff in_mask = SND_CHN_T_MASK_FL | SND_CHN_T_MASK_FR; 326193640Sariff } else 327193640Sariff in_mask = m_in->mask; 328193640Sariff 329193640Sariff /* Merge, reduce, expand all possibilites. */ 330193640Sariff for (ch = SND_CHN_T_BEGIN; ch <= SND_CHN_T_END && 331193640Sariff m_out->map[ch].type != SND_CHN_T_MAX; ch += SND_CHN_T_STEP) { 332193640Sariff merge_mask = m_out->map[ch].members & in_mask; 333193640Sariff if (merge_mask == 0) { 334193640Sariff info->matrix[ch].chn[0] = SND_CHN_T_NULL; 335193640Sariff continue; 336193640Sariff } 337193640Sariff 338193640Sariff j = 0; 339193640Sariff for (i = SND_CHN_T_BEGIN; i <= SND_CHN_T_END; 340193640Sariff i += SND_CHN_T_STEP) { 341193640Sariff if (merge_mask & (1 << i)) { 342193640Sariff if (m_in->offset[i] >= 0 && 343193640Sariff m_in->offset[i] < (int)m_in->channels) 344193640Sariff info->matrix[ch].chn[j++] = 345193640Sariff m_in->offset[i] * info->bps; 346193640Sariff else { 347193640Sariff info->matrix[ch].chn[j++] = 348193640Sariff SND_CHN_T_EOF; 349193640Sariff break; 350193640Sariff } 351193640Sariff } 352193640Sariff } 353193640Sariff 354193640Sariff#define FEEDMATRIX_ATTN_SHIFT 16 355193640Sariff 356193640Sariff if (j > 1) { 357193640Sariff /* 358193640Sariff * XXX For channel that require accumulation from 359193640Sariff * multiple channels, apply a slight attenuation to 360193640Sariff * avoid clipping. 361193640Sariff */ 362193640Sariff mul = (1 << (FEEDMATRIX_ATTN_SHIFT - 1)) + 143 - j; 363193640Sariff shift = FEEDMATRIX_ATTN_SHIFT; 364193640Sariff while ((mul & 1) == 0 && shift > 0) { 365193640Sariff mul >>= 1; 366193640Sariff shift--; 367193640Sariff } 368193640Sariff info->matrix[ch].mul = mul; 369193640Sariff info->matrix[ch].shift = shift; 370193640Sariff } 371193640Sariff } 372193640Sariff 373193640Sariff#ifndef _KERNEL 374193640Sariff fprintf(stderr, "Total: %d\n", ch); 375193640Sariff 376193640Sariff for (i = 0; info->matrix[i].chn[0] != SND_CHN_T_EOF; i++) { 377193640Sariff fprintf(stderr, "%d: [", i); 378193640Sariff for (j = 0; info->matrix[i].chn[j] != SND_CHN_T_EOF; j++) { 379193640Sariff if (j != 0) 380193640Sariff fprintf(stderr, ", "); 381193640Sariff fprintf(stderr, "%d", 382193640Sariff (info->matrix[i].chn[j] == SND_CHN_T_NULL) ? 383193640Sariff 0xffffffff : info->matrix[i].chn[j] / info->bps); 384193640Sariff } 385193640Sariff fprintf(stderr, "] attn: (x * %d) >> %d\n", 386193640Sariff info->matrix[i].mul, info->matrix[i].shift); 387193640Sariff } 388193640Sariff#endif 389193640Sariff 390193640Sariff return (0); 391193640Sariff} 392193640Sariff 393193640Sariffstatic int 394193640Sarifffeed_matrix_init(struct pcm_feeder *f) 395193640Sariff{ 396193640Sariff struct feed_matrix_info *info; 397193640Sariff struct pcmchan_matrix *m_in, *m_out; 398193640Sariff uint32_t i; 399193640Sariff int ret; 400193640Sariff 401193640Sariff if (AFMT_ENCODING(f->desc->in) != AFMT_ENCODING(f->desc->out)) 402193640Sariff return (EINVAL); 403193640Sariff 404193640Sariff info = malloc(sizeof(*info), M_DEVBUF, M_NOWAIT | M_ZERO); 405193640Sariff if (info == NULL) 406193640Sariff return (ENOMEM); 407193640Sariff 408193640Sariff info->in = f->desc->in; 409193640Sariff info->out = f->desc->out; 410193640Sariff info->bps = AFMT_BPS(info->in); 411193640Sariff info->ialign = AFMT_ALIGN(info->in); 412193640Sariff info->oalign = AFMT_ALIGN(info->out); 413193640Sariff info->apply = NULL; 414193640Sariff 415193640Sariff for (i = 0; info->apply == NULL && 416193640Sariff i < (sizeof(feed_matrix_tab) / sizeof(feed_matrix_tab[0])); i++) { 417193640Sariff if (AFMT_ENCODING(info->in) == feed_matrix_tab[i].format) 418193640Sariff info->apply = feed_matrix_tab[i].apply; 419193640Sariff } 420193640Sariff 421193640Sariff if (info->apply == NULL) { 422193640Sariff#ifdef FEEDMATRIX_GENERIC 423193640Sariff info->rd = feeder_format_read_op(info->in); 424193640Sariff info->wr = feeder_format_write_op(info->out); 425193640Sariff if (info->rd == NULL || info->wr == NULL) { 426193640Sariff free(info, M_DEVBUF); 427193640Sariff return (EINVAL); 428193640Sariff } 429193640Sariff info->apply = feed_matrix_apply_generic; 430193640Sariff#else 431193640Sariff free(info, M_DEVBUF); 432193640Sariff return (EINVAL); 433193640Sariff#endif 434193640Sariff } 435193640Sariff 436193640Sariff m_in = feeder_matrix_format_map(info->in); 437193640Sariff m_out = feeder_matrix_format_map(info->out); 438193640Sariff 439193640Sariff ret = feed_matrix_setup(info, m_in, m_out); 440193640Sariff if (ret != 0) { 441193640Sariff free(info, M_DEVBUF); 442193640Sariff return (ret); 443193640Sariff } 444193640Sariff 445193640Sariff f->data = info; 446193640Sariff 447193640Sariff return (0); 448193640Sariff} 449193640Sariff 450193640Sariffstatic int 451193640Sarifffeed_matrix_free(struct pcm_feeder *f) 452193640Sariff{ 453193640Sariff struct feed_matrix_info *info; 454193640Sariff 455193640Sariff info = f->data; 456193640Sariff if (info != NULL) 457193640Sariff free(info, M_DEVBUF); 458193640Sariff 459193640Sariff f->data = NULL; 460193640Sariff 461193640Sariff return (0); 462193640Sariff} 463193640Sariff 464193640Sariffstatic int 465193640Sarifffeed_matrix_feed(struct pcm_feeder *f, struct pcm_channel *c, uint8_t *b, 466193640Sariff uint32_t count, void *source) 467193640Sariff{ 468193640Sariff struct feed_matrix_info *info; 469193640Sariff uint32_t j, inmax; 470193640Sariff uint8_t *src, *dst; 471193640Sariff 472193640Sariff info = f->data; 473193640Sariff if (info->matrix[0].chn[0] == SND_CHN_T_EOF) 474193640Sariff return (FEEDER_FEED(f->source, c, b, count, source)); 475193640Sariff 476193640Sariff dst = b; 477193640Sariff count = SND_FXROUND(count, info->oalign); 478193640Sariff inmax = info->ialign + info->oalign; 479193640Sariff 480193640Sariff /* 481193640Sariff * This loop might look simmilar to other feeder_* loops, but be 482193640Sariff * advised: matrixing might involve overlapping (think about 483193640Sariff * swapping end to front or something like that). In this regard it 484193640Sariff * might be simmilar to feeder_format, but feeder_format works on 485193640Sariff * 'sample' domain where it can be fitted into single 32bit integer 486193640Sariff * while matrixing works on 'sample frame' domain. 487193640Sariff */ 488193640Sariff do { 489193640Sariff if (count < info->oalign) 490193640Sariff break; 491193640Sariff 492193640Sariff if (count < inmax) { 493193640Sariff src = info->reservoir; 494193640Sariff j = info->ialign; 495193640Sariff } else { 496193640Sariff if (info->ialign == info->oalign) 497193640Sariff j = count - info->oalign; 498193640Sariff else if (info->ialign > info->oalign) 499193640Sariff j = SND_FXROUND(count - info->oalign, 500193640Sariff info->ialign); 501193640Sariff else 502193640Sariff j = (SND_FXDIV(count, info->oalign) - 1) * 503193640Sariff info->ialign; 504193640Sariff src = dst + count - j; 505193640Sariff } 506193640Sariff 507193640Sariff j = SND_FXDIV(FEEDER_FEED(f->source, c, src, j, source), 508193640Sariff info->ialign); 509193640Sariff if (j == 0) 510193640Sariff break; 511193640Sariff 512193640Sariff info->apply(info, src, dst, j); 513193640Sariff 514193640Sariff j *= info->oalign; 515193640Sariff dst += j; 516193640Sariff count -= j; 517193640Sariff 518193640Sariff } while (count != 0); 519193640Sariff 520193640Sariff return (dst - b); 521193640Sariff} 522193640Sariff 523193640Sariffstatic struct pcm_feederdesc feeder_matrix_desc[] = { 524193640Sariff { FEEDER_MATRIX, 0, 0, 0, 0 }, 525193640Sariff { 0, 0, 0, 0, 0 } 526193640Sariff}; 527193640Sariff 528193640Sariffstatic kobj_method_t feeder_matrix_methods[] = { 529193640Sariff KOBJMETHOD(feeder_init, feed_matrix_init), 530193640Sariff KOBJMETHOD(feeder_free, feed_matrix_free), 531193640Sariff KOBJMETHOD(feeder_feed, feed_matrix_feed), 532193640Sariff KOBJMETHOD_END 533193640Sariff}; 534193640Sariff 535193640SariffFEEDER_DECLARE(feeder_matrix, NULL); 536193640Sariff 537193640Sariff/* External */ 538193640Sariffint 539193640Sarifffeeder_matrix_setup(struct pcm_feeder *f, struct pcmchan_matrix *m_in, 540193640Sariff struct pcmchan_matrix *m_out) 541193640Sariff{ 542193640Sariff 543193640Sariff if (f == NULL || f->desc == NULL || f->desc->type != FEEDER_MATRIX || 544193640Sariff f->data == NULL) 545193640Sariff return (EINVAL); 546193640Sariff 547193640Sariff return (feed_matrix_setup(f->data, m_in, m_out)); 548193640Sariff} 549193640Sariff 550193640Sariff/* 551193640Sariff * feeder_matrix_default_id(): For a given number of channels, return 552193640Sariff * default prefered id (example: both 5.1 and 553193640Sariff * 6.0 are simply 6 channels, but 5.1 is more 554193640Sariff * preferable). 555193640Sariff */ 556193640Sariffint 557193640Sarifffeeder_matrix_default_id(uint32_t ch) 558193640Sariff{ 559193640Sariff 560193640Sariff if (ch < feeder_matrix_maps[SND_CHN_MATRIX_BEGIN].channels || 561193640Sariff ch > feeder_matrix_maps[SND_CHN_MATRIX_END].channels) 562193640Sariff return (SND_CHN_MATRIX_UNKNOWN); 563193640Sariff 564193640Sariff return (feeder_matrix_maps[feeder_matrix_default_ids[ch]].id); 565193640Sariff} 566193640Sariff 567193640Sariff/* 568193640Sariff * feeder_matrix_default_channel_map(): Ditto, but return matrix map 569193640Sariff * instead. 570193640Sariff */ 571193640Sariffstruct pcmchan_matrix * 572193640Sarifffeeder_matrix_default_channel_map(uint32_t ch) 573193640Sariff{ 574193640Sariff 575193640Sariff if (ch < feeder_matrix_maps[SND_CHN_MATRIX_BEGIN].channels || 576193640Sariff ch > feeder_matrix_maps[SND_CHN_MATRIX_END].channels) 577193640Sariff return (NULL); 578193640Sariff 579193640Sariff return (&feeder_matrix_maps[feeder_matrix_default_ids[ch]]); 580193640Sariff} 581193640Sariff 582193640Sariff/* 583193640Sariff * feeder_matrix_default_format(): For a given audio format, return the 584193640Sariff * proper audio format based on preferable 585193640Sariff * matrix. 586193640Sariff */ 587193640Sariffuint32_t 588193640Sarifffeeder_matrix_default_format(uint32_t format) 589193640Sariff{ 590193640Sariff struct pcmchan_matrix *m; 591193640Sariff uint32_t i, ch, ext; 592193640Sariff 593193640Sariff ch = AFMT_CHANNEL(format); 594193640Sariff ext = AFMT_EXTCHANNEL(format); 595193640Sariff 596193640Sariff if (ext != 0) { 597193640Sariff for (i = SND_CHN_MATRIX_BEGIN; i <= SND_CHN_MATRIX_END; i++) { 598193640Sariff if (feeder_matrix_maps[i].channels == ch && 599193640Sariff feeder_matrix_maps[i].ext == ext) 600193640Sariff return (SND_FORMAT(format, ch, ext)); 601193640Sariff } 602193640Sariff } 603193640Sariff 604193640Sariff m = feeder_matrix_default_channel_map(ch); 605193640Sariff if (m == NULL) 606193640Sariff return (0x00000000); 607193640Sariff 608193640Sariff return (SND_FORMAT(format, ch, m->ext)); 609193640Sariff} 610193640Sariff 611193640Sariff/* 612193640Sariff * feeder_matrix_format_id(): For a given audio format, return its matrix 613193640Sariff * id. 614193640Sariff */ 615193640Sariffint 616193640Sarifffeeder_matrix_format_id(uint32_t format) 617193640Sariff{ 618193640Sariff uint32_t i, ch, ext; 619193640Sariff 620193640Sariff ch = AFMT_CHANNEL(format); 621193640Sariff ext = AFMT_EXTCHANNEL(format); 622193640Sariff 623193640Sariff for (i = SND_CHN_MATRIX_BEGIN; i <= SND_CHN_MATRIX_END; i++) { 624193640Sariff if (feeder_matrix_maps[i].channels == ch && 625193640Sariff feeder_matrix_maps[i].ext == ext) 626193640Sariff return (feeder_matrix_maps[i].id); 627193640Sariff } 628193640Sariff 629193640Sariff return (SND_CHN_MATRIX_UNKNOWN); 630193640Sariff} 631193640Sariff 632193640Sariff/* 633193640Sariff * feeder_matrix_format_map(): For a given audio format, return its matrix 634193640Sariff * map. 635193640Sariff */ 636193640Sariffstruct pcmchan_matrix * 637193640Sarifffeeder_matrix_format_map(uint32_t format) 638193640Sariff{ 639193640Sariff uint32_t i, ch, ext; 640193640Sariff 641193640Sariff ch = AFMT_CHANNEL(format); 642193640Sariff ext = AFMT_EXTCHANNEL(format); 643193640Sariff 644193640Sariff for (i = SND_CHN_MATRIX_BEGIN; i <= SND_CHN_MATRIX_END; i++) { 645193640Sariff if (feeder_matrix_maps[i].channels == ch && 646193640Sariff feeder_matrix_maps[i].ext == ext) 647193640Sariff return (&feeder_matrix_maps[i]); 648193640Sariff } 649193640Sariff 650193640Sariff return (NULL); 651193640Sariff} 652193640Sariff 653193640Sariff/* 654193640Sariff * feeder_matrix_id_map(): For a given matrix id, return its matrix map. 655193640Sariff */ 656193640Sariffstruct pcmchan_matrix * 657193640Sarifffeeder_matrix_id_map(int id) 658193640Sariff{ 659193640Sariff 660193640Sariff if (id < SND_CHN_MATRIX_BEGIN || id > SND_CHN_MATRIX_END) 661193640Sariff return (NULL); 662193640Sariff 663193640Sariff return (&feeder_matrix_maps[id]); 664193640Sariff} 665193640Sariff 666193640Sariff/* 667193640Sariff * feeder_matrix_compare(): Compare the simmilarities of matrices. 668193640Sariff */ 669193640Sariffint 670193640Sarifffeeder_matrix_compare(struct pcmchan_matrix *m_in, struct pcmchan_matrix *m_out) 671193640Sariff{ 672193640Sariff uint32_t i; 673193640Sariff 674193640Sariff if (m_in == m_out) 675193640Sariff return (0); 676193640Sariff 677193640Sariff if (m_in->channels != m_out->channels || m_in->ext != m_out->ext || 678193640Sariff m_in->mask != m_out->mask) 679193640Sariff return (1); 680193640Sariff 681193640Sariff for (i = 0; i < (sizeof(m_in->map) / sizeof(m_in->map[0])); i++) { 682193640Sariff if (m_in->map[i].type != m_out->map[i].type) 683193640Sariff return (1); 684193640Sariff if (m_in->map[i].type == SND_CHN_T_MAX) 685193640Sariff break; 686193640Sariff if (m_in->map[i].members != m_out->map[i].members) 687193640Sariff return (1); 688193640Sariff if (i <= SND_CHN_T_END) { 689193640Sariff if (m_in->offset[m_in->map[i].type] != 690193640Sariff m_out->offset[m_out->map[i].type]) 691193640Sariff return (1); 692193640Sariff } 693193640Sariff } 694193640Sariff 695193640Sariff return (0); 696193640Sariff} 697193640Sariff 698193640Sariff/* 699193640Sariff * XXX 4front intepretation of "surround" is ambigous and sort of 700193640Sariff * conflicting with "rear"/"back". Map it to "side". Well.. 701193640Sariff * who cares? 702193640Sariff */ 703193640Sariffstatic int snd_chn_to_oss[SND_CHN_T_MAX] = { 704193640Sariff [SND_CHN_T_FL] = CHID_L, 705193640Sariff [SND_CHN_T_FR] = CHID_R, 706193640Sariff [SND_CHN_T_FC] = CHID_C, 707193640Sariff [SND_CHN_T_LF] = CHID_LFE, 708193640Sariff [SND_CHN_T_SL] = CHID_LS, 709193640Sariff [SND_CHN_T_SR] = CHID_RS, 710193640Sariff [SND_CHN_T_BL] = CHID_LR, 711193640Sariff [SND_CHN_T_BR] = CHID_RR 712193640Sariff}; 713193640Sariff 714193640Sariff#define SND_CHN_OSS_VALIDMASK \ 715193640Sariff (SND_CHN_T_MASK_FL | SND_CHN_T_MASK_FR | \ 716193640Sariff SND_CHN_T_MASK_FC | SND_CHN_T_MASK_LF | \ 717193640Sariff SND_CHN_T_MASK_SL | SND_CHN_T_MASK_SR | \ 718193640Sariff SND_CHN_T_MASK_BL | SND_CHN_T_MASK_BR) 719193640Sariff 720193640Sariff#define SND_CHN_OSS_MAX 8 721193640Sariff#define SND_CHN_OSS_BEGIN CHID_L 722193640Sariff#define SND_CHN_OSS_END CHID_RR 723193640Sariff 724193640Sariffstatic int oss_to_snd_chn[SND_CHN_OSS_END + 1] = { 725193640Sariff [CHID_L] = SND_CHN_T_FL, 726193640Sariff [CHID_R] = SND_CHN_T_FR, 727193640Sariff [CHID_C] = SND_CHN_T_FC, 728193640Sariff [CHID_LFE] = SND_CHN_T_LF, 729193640Sariff [CHID_LS] = SND_CHN_T_SL, 730193640Sariff [CHID_RS] = SND_CHN_T_SR, 731193640Sariff [CHID_LR] = SND_CHN_T_BL, 732193640Sariff [CHID_RR] = SND_CHN_T_BR 733193640Sariff}; 734193640Sariff 735193640Sariff/* 736193640Sariff * Used by SNDCTL_DSP_GET_CHNORDER. 737193640Sariff */ 738193640Sariffint 739193640Sarifffeeder_matrix_oss_get_channel_order(struct pcmchan_matrix *m, 740193640Sariff unsigned long long *map) 741193640Sariff{ 742193640Sariff unsigned long long tmpmap; 743193640Sariff uint32_t i; 744193640Sariff 745193640Sariff if (m == NULL || map == NULL || (m->mask & ~SND_CHN_OSS_VALIDMASK) || 746193640Sariff m->channels > SND_CHN_OSS_MAX) 747193640Sariff return (EINVAL); 748193640Sariff 749193640Sariff tmpmap = 0x0000000000000000ULL; 750193640Sariff 751193640Sariff for (i = 0; m->map[i].type != SND_CHN_T_MAX && 752193640Sariff i < SND_CHN_OSS_MAX; i++) { 753193640Sariff if ((1 << m->map[i].type) & ~SND_CHN_OSS_VALIDMASK) 754193640Sariff return (EINVAL); 755193640Sariff tmpmap |= 756193640Sariff (unsigned long long)snd_chn_to_oss[m->map[i].type] << 757193640Sariff (i * 4); 758193640Sariff } 759193640Sariff 760193640Sariff *map = tmpmap; 761193640Sariff 762193640Sariff return (0); 763193640Sariff} 764193640Sariff 765193640Sariff/* 766193640Sariff * Used by SNDCTL_DSP_SET_CHNORDER. 767193640Sariff */ 768193640Sariffint 769193640Sarifffeeder_matrix_oss_set_channel_order(struct pcmchan_matrix *m, 770193640Sariff unsigned long long *map) 771193640Sariff{ 772193640Sariff struct pcmchan_matrix tmp; 773193640Sariff uint32_t chmask, i; 774193640Sariff int ch, cheof; 775193640Sariff 776193640Sariff if (m == NULL || map == NULL || (m->mask & ~SND_CHN_OSS_VALIDMASK) || 777193640Sariff m->channels > SND_CHN_OSS_MAX || (*map & 0xffffffff00000000ULL)) 778193640Sariff return (EINVAL); 779193640Sariff 780193640Sariff tmp = *m; 781193640Sariff tmp.channels = 0; 782193640Sariff tmp.ext = 0; 783193640Sariff tmp.mask = 0; 784193640Sariff memset(tmp.offset, -1, sizeof(tmp.offset)); 785193640Sariff cheof = 0; 786193640Sariff 787193640Sariff for (i = 0; i < SND_CHN_OSS_MAX; i++) { 788193640Sariff ch = (*map >> (i * 4)) & 0xf; 789193640Sariff if (ch < SND_CHN_OSS_BEGIN) { 790193640Sariff if (cheof == 0 && m->map[i].type != SND_CHN_T_MAX) 791193640Sariff return (EINVAL); 792193640Sariff cheof++; 793193640Sariff tmp.map[i] = m->map[i]; 794193640Sariff continue; 795193640Sariff } else if (ch > SND_CHN_OSS_END) 796193640Sariff return (EINVAL); 797193640Sariff else if (cheof != 0) 798193640Sariff return (EINVAL); 799193640Sariff ch = oss_to_snd_chn[ch]; 800193640Sariff chmask = 1 << ch; 801193640Sariff /* channel not exist in matrix */ 802193640Sariff if (!(chmask & m->mask)) 803193640Sariff return (EINVAL); 804193640Sariff /* duplicated channel */ 805193640Sariff if (chmask & tmp.mask) 806193640Sariff return (EINVAL); 807193640Sariff tmp.map[i] = m->map[m->offset[ch]]; 808193640Sariff if (tmp.map[i].type != ch) 809193640Sariff return (EINVAL); 810193640Sariff tmp.offset[ch] = i; 811193640Sariff tmp.mask |= chmask; 812193640Sariff tmp.channels++; 813193640Sariff if (chmask & SND_CHN_T_MASK_LF) 814193640Sariff tmp.ext++; 815193640Sariff } 816193640Sariff 817193640Sariff if (tmp.channels != m->channels || tmp.ext != m->ext || 818193640Sariff tmp.mask != m->mask || 819193640Sariff tmp.map[m->channels].type != SND_CHN_T_MAX) 820193640Sariff return (EINVAL); 821193640Sariff 822193640Sariff *m = tmp; 823193640Sariff 824193640Sariff return (0); 825193640Sariff} 826