sbc/sbc.c

   1 /*
   2  *
   3  *  Bluetooth low-complexity, subband codec (SBC) library
   4  *
   5  *  Copyright (C) 2004-2008  Marcel Holtmann <marcel@holtmann.org>
   6  *  Copyright (C) 2004-2005  Henryk Ploetz <henryk@ploetzli.ch>
   7  *  Copyright (C) 2005-2008  Brad Midgley <bmidgley@xmission.com>
   8  *
   9  *
  10  *  This library is free software; you can redistribute it and/or
  11  *  modify it under the terms of the GNU Lesser General Public
  12  *  License as published by the Free Software Foundation; either
  13  *  version 2.1 of the License, or (at your option) any later version.
  14  *
  15  *  This library is distributed in the hope that it will be useful,
  16  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
  17  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  18  *  Lesser General Public License for more details.
  19  *
  20  *  You should have received a copy of the GNU Lesser General Public
  21  *  License along with this library; if not, write to the Free Software
  22  *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
  23  *
  24  */
  25
  26 /* todo items:
  27
  28   use a log2 table for byte integer scale factors calculation (sum log2 results
  29   for high and low bytes) fill bitpool by 16 bits instead of one at a time in
  30   bits allocation/bitpool generation port to the dsp
  31
  32 */
  33
  34 #ifdef HAVE_CONFIG_H
  35 #include <config.h>
  36 #endif
  37
  38 #include <stdio.h>
  39 #include <errno.h>
  40 #include <string.h>
  41 #include <stdlib.h>
  42 #include <sys/types.h>
  43
  44 #include "sbc_math.h"
  45 #include "sbc_tables.h"
  46
  47 #include "sbc.h"
  48
  49 #define SBC_SYNCWORD    0x9C
  50
  51 /* This structure contains an unpacked SBC frame.
  52    Yes, there is probably quite some unused space herein */
  53 struct sbc_frame {
  54         uint8_t frequency;
  55         uint8_t block_mode;
  56         uint8_t blocks;
  57         enum {
  58                 MONO            = SBC_MODE_MONO,
  59                 DUAL_CHANNEL    = SBC_MODE_DUAL_CHANNEL,
  60                 STEREO          = SBC_MODE_STEREO,
  61                 JOINT_STEREO    = SBC_MODE_JOINT_STEREO
  62         } mode;
  63         uint8_t channels;
  64         enum {
  65                 LOUDNESS        = SBC_AM_LOUDNESS,
  66                 SNR             = SBC_AM_SNR
  67         } allocation;
  68         uint8_t subband_mode;
  69         uint8_t subbands;
  70         uint8_t bitpool;
  71         uint8_t codesize;
  72         uint8_t length;
  73
  74         /* bit number x set means joint stereo has been used in subband x */
  75         uint8_t joint;
  76
  77         /* only the lower 4 bits of every element are to be used */
  78         uint8_t scale_factor[2][8];
  79
  80         /* raw integer subband samples in the frame */
  81
  82         int32_t sb_sample_f[16][2][8];
  83         int32_t sb_sample[16][2][8];    /* modified subband samples */
  84         int16_t pcm_sample[2][16*8];    /* original pcm audio samples */
  85 };
  86
  87 struct sbc_decoder_state {
  88         int subbands;
  89         int32_t V[2][170];
  90         int offset[2][16];
  91 };
  92
  93 struct sbc_encoder_state {
  94         int subbands;
  95         int position[2];
  96         int32_t X[2][160];
  97 };
  98
  99 /*
 100  * Calculates the CRC-8 of the first len bits in data
 101  */
 102 static const uint8_t crc_table[256] = {
 103         0x00, 0x1D, 0x3A, 0x27, 0x74, 0x69, 0x4E, 0x53,
 104         0xE8, 0xF5, 0xD2, 0xCF, 0x9C, 0x81, 0xA6, 0xBB,
 105         0xCD, 0xD0, 0xF7, 0xEA, 0xB9, 0xA4, 0x83, 0x9E,
 106         0x25, 0x38, 0x1F, 0x02, 0x51, 0x4C, 0x6B, 0x76,
 107         0x87, 0x9A, 0xBD, 0xA0, 0xF3, 0xEE, 0xC9, 0xD4,
 108         0x6F, 0x72, 0x55, 0x48, 0x1B, 0x06, 0x21, 0x3C,
 109         0x4A, 0x57, 0x70, 0x6D, 0x3E, 0x23, 0x04, 0x19,
 110         0xA2, 0xBF, 0x98, 0x85, 0xD6, 0xCB, 0xEC, 0xF1,
 111         0x13, 0x0E, 0x29, 0x34, 0x67, 0x7A, 0x5D, 0x40,
 112         0xFB, 0xE6, 0xC1, 0xDC, 0x8F, 0x92, 0xB5, 0xA8,
 113         0xDE, 0xC3, 0xE4, 0xF9, 0xAA, 0xB7, 0x90, 0x8D,
 114         0x36, 0x2B, 0x0C, 0x11, 0x42, 0x5F, 0x78, 0x65,
 115         0x94, 0x89, 0xAE, 0xB3, 0xE0, 0xFD, 0xDA, 0xC7,
 116         0x7C, 0x61, 0x46, 0x5B, 0x08, 0x15, 0x32, 0x2F,
 117         0x59, 0x44, 0x63, 0x7E, 0x2D, 0x30, 0x17, 0x0A,
 118         0xB1, 0xAC, 0x8B, 0x96, 0xC5, 0xD8, 0xFF, 0xE2,
 119         0x26, 0x3B, 0x1C, 0x01, 0x52, 0x4F, 0x68, 0x75,
 120         0xCE, 0xD3, 0xF4, 0xE9, 0xBA, 0xA7, 0x80, 0x9D,
 121         0xEB, 0xF6, 0xD1, 0xCC, 0x9F, 0x82, 0xA5, 0xB8,
 122         0x03, 0x1E, 0x39, 0x24, 0x77, 0x6A, 0x4D, 0x50,
 123         0xA1, 0xBC, 0x9B, 0x86, 0xD5, 0xC8, 0xEF, 0xF2,
 124         0x49, 0x54, 0x73, 0x6E, 0x3D, 0x20, 0x07, 0x1A,
 125         0x6C, 0x71, 0x56, 0x4B, 0x18, 0x05, 0x22, 0x3F,
 126         0x84, 0x99, 0xBE, 0xA3, 0xF0, 0xED, 0xCA, 0xD7,
 127         0x35, 0x28, 0x0F, 0x12, 0x41, 0x5C, 0x7B, 0x66,
 128         0xDD, 0xC0, 0xE7, 0xFA, 0xA9, 0xB4, 0x93, 0x8E,
 129         0xF8, 0xE5, 0xC2, 0xDF, 0x8C, 0x91, 0xB6, 0xAB,
 130         0x10, 0x0D, 0x2A, 0x37, 0x64, 0x79, 0x5E, 0x43,
 131         0xB2, 0xAF, 0x88, 0x95, 0xC6, 0xDB, 0xFC, 0xE1,
 132         0x5A, 0x47, 0x60, 0x7D, 0x2E, 0x33, 0x14, 0x09,
 133         0x7F, 0x62, 0x45, 0x58, 0x0B, 0x16, 0x31, 0x2C,
 134         0x97, 0x8A, 0xAD, 0xB0, 0xE3, 0xFE, 0xD9, 0xC4
 135 };
 136
 137 static uint8_t sbc_crc8(const uint8_t *data, size_t len)
 138 {
 139         uint8_t crc = 0x0f;
 140         size_t i;
 141         uint8_t octet;
 142
 143         for (i = 0; i < len / 8; i++)
 144                 crc = crc_table[crc ^ data[i]];
 145
 146         octet = data[i];
 147         for (i = 0; i < len % 8; i++) {
 148                 char bit = ((octet ^ crc) & 0x80) >> 7;
 149
 150                 crc = ((crc & 0x7f) << 1) ^ (bit ? 0x1d : 0);
 151
 152                 octet = octet << 1;
 153         }
 154
 155         return crc;
 156 }
 157
 158 /*
 159  * Code straight from the spec to calculate the bits array
 160  * Takes a pointer to the frame in question, a pointer to the bits array and
 161  * the sampling frequency (as 2 bit integer)
 162  */
 163 static void sbc_calculate_bits(const struct sbc_frame *frame, int (*bits)[8])
 164 {
 165         uint8_t sf = frame->frequency;
 166
 167         if (frame->mode == MONO || frame->mode == DUAL_CHANNEL) {
 168                 int bitneed[2][8], loudness, max_bitneed, bitcount, slicecount, bitslice;
 169                 int ch, sb;
 170
 171                 for (ch = 0; ch < frame->channels; ch++) {
 172                         max_bitneed = 0;
 173                         if (frame->allocation == SNR) {
 174                                 for (sb = 0; sb < frame->subbands; sb++) {
 175                                         bitneed[ch][sb] = frame->scale_factor[ch][sb];
 176                                         if (bitneed[ch][sb] > max_bitneed)
 177                                                 max_bitneed = bitneed[ch][sb];
 178                                 }
 179                         } else {
 180                                 for (sb = 0; sb < frame->subbands; sb++) {
 181                                         if (frame->scale_factor[ch][sb] == 0)
 182                                                 bitneed[ch][sb] = -5;
 183                                         else {
 184                                                 if (frame->subbands == 4)
 185                                                         loudness = frame->scale_factor[ch][sb] - sbc_offset4[sf][sb];
 186                                                 else
 187                                                         loudness = frame->scale_factor[ch][sb] - sbc_offset8[sf][sb];
 188                                                 if (loudness > 0)
 189                                                         bitneed[ch][sb] = loudness / 2;
 190                                                 else
 191                                                         bitneed[ch][sb] = loudness;
 192                                         }
 193                                         if (bitneed[ch][sb] > max_bitneed)
 194                                                 max_bitneed = bitneed[ch][sb];
 195                                 }
 196                         }
 197
 198                         bitcount = 0;
 199                         slicecount = 0;
 200                         bitslice = max_bitneed + 1;
 201                         do {
 202                                 bitslice--;
 203                                 bitcount += slicecount;
 204                                 slicecount = 0;
 205                                 for (sb = 0; sb < frame->subbands; sb++) {
 206                                         if ((bitneed[ch][sb] > bitslice + 1) && (bitneed[ch][sb] < bitslice + 16))
 207                                                 slicecount++;
 208                                         else if (bitneed[ch][sb] == bitslice + 1)
 209                                                 slicecount += 2;
 210                                 }
 211                         } while (bitcount + slicecount < frame->bitpool);
 212
 213                         if (bitcount + slicecount == frame->bitpool) {
 214                                 bitcount += slicecount;
 215                                 bitslice--;
 216                         }
 217
 218                         for (sb = 0; sb < frame->subbands; sb++) {
 219                                 if (bitneed[ch][sb] < bitslice + 2)
 220                                         bits[ch][sb] = 0;
 221                                 else {
 222                                         bits[ch][sb] = bitneed[ch][sb] - bitslice;
 223                                         if (bits[ch][sb] > 16)
 224                                                 bits[ch][sb] = 16;
 225                                 }
 226                         }
 227
 228                         for (sb = 0; bitcount < frame->bitpool && sb < frame->subbands; sb++) {
 229                                 if ((bits[ch][sb] >= 2) && (bits[ch][sb] < 16)) {
 230                                         bits[ch][sb]++;
 231                                         bitcount++;
 232                                 } else if ((bitneed[ch][sb] == bitslice + 1) && (frame->bitpool > bitcount + 1)) {
 233                                         bits[ch][sb] = 2;
 234                                         bitcount += 2;
 235                                 }
 236                         }
 237
 238                         for (sb = 0; bitcount < frame->bitpool && sb < frame->subbands; sb++) {
 239                                 if (bits[ch][sb] < 16) {
 240                                         bits[ch][sb]++;
 241                                         bitcount++;
 242                                 }
 243                         }
 244
 245                 }
 246
 247         } else if (frame->mode == STEREO || frame->mode == JOINT_STEREO) {
 248                 int bitneed[2][8], loudness, max_bitneed, bitcount, slicecount, bitslice;
 249                 int ch, sb;
 250
 251                 max_bitneed = 0;
 252                 if (frame->allocation == SNR) {
 253                         for (ch = 0; ch < 2; ch++) {
 254                                 for (sb = 0; sb < frame->subbands; sb++) {
 255                                         bitneed[ch][sb] = frame->scale_factor[ch][sb];
 256                                         if (bitneed[ch][sb] > max_bitneed)
 257                                                 max_bitneed = bitneed[ch][sb];
 258                                 }
 259                         }
 260                 } else {
 261                         for (ch = 0; ch < 2; ch++) {
 262                                 for (sb = 0; sb < frame->subbands; sb++) {
 263                                         if (frame->scale_factor[ch][sb] == 0)
 264                                                 bitneed[ch][sb] = -5;
 265                                         else {
 266                                                 if (frame->subbands == 4)
 267                                                         loudness = frame->scale_factor[ch][sb] - sbc_offset4[sf][sb];
 268                                                 else
 269                                                         loudness = frame->scale_factor[ch][sb] - sbc_offset8[sf][sb];
 270                                                 if (loudness > 0)
 271                                                         bitneed[ch][sb] = loudness / 2;
 272                                                 else
 273                                                         bitneed[ch][sb] = loudness;
 274                                         }
 275                                         if (bitneed[ch][sb] > max_bitneed)
 276                                                 max_bitneed = bitneed[ch][sb];
 277                                 }
 278                         }
 279                 }
 280
 281                 bitcount = 0;
 282                 slicecount = 0;
 283                 bitslice = max_bitneed + 1;
 284                 do {
 285                         bitslice--;
 286                         bitcount += slicecount;
 287                         slicecount = 0;
 288                         for (ch = 0; ch < 2; ch++) {
 289                                 for (sb = 0; sb < frame->subbands; sb++) {
 290                                         if ((bitneed[ch][sb] > bitslice + 1) && (bitneed[ch][sb] < bitslice + 16))
 291                                                 slicecount++;
 292                                         else if (bitneed[ch][sb] == bitslice + 1)
 293                                                 slicecount += 2;
 294                                 }
 295                         }
 296                 } while (bitcount + slicecount < frame->bitpool);
 297
 298                 if (bitcount + slicecount == frame->bitpool) {
 299                         bitcount += slicecount;
 300                         bitslice--;
 301                 }
 302
 303                 for (ch = 0; ch < 2; ch++) {
 304                         for (sb = 0; sb < frame->subbands; sb++) {
 305                                 if (bitneed[ch][sb] < bitslice + 2) {
 306                                         bits[ch][sb] = 0;
 307                                 } else {
 308                                         bits[ch][sb] = bitneed[ch][sb] - bitslice;
 309                                         if (bits[ch][sb] > 16)
 310                                                 bits[ch][sb] = 16;
 311                                 }
 312                         }
 313                 }
 314
 315                 ch = 0;
 316                 sb = 0;
 317                 while (bitcount < frame->bitpool) {
 318                         if ((bits[ch][sb] >= 2) && (bits[ch][sb] < 16)) {
 319                                 bits[ch][sb]++;
 320                                 bitcount++;
 321                         } else if ((bitneed[ch][sb] == bitslice + 1) && (frame->bitpool > bitcount + 1)) {
 322                                 bits[ch][sb] = 2;
 323                                 bitcount += 2;
 324                         }
 325                         if (ch == 1) {
 326                                 ch = 0;
 327                                 sb++;
 328                                 if (sb >= frame->subbands) break;
 329                         } else
 330                                 ch = 1;
 331                 }
 332
 333                 ch = 0;
 334                 sb = 0;
 335                 while (bitcount < frame->bitpool) {
 336                         if (bits[ch][sb] < 16) {
 337                                 bits[ch][sb]++;
 338                                 bitcount++;
 339                         }
 340                         if (ch == 1) {
 341                                 ch = 0;
 342                                 sb++;
 343                                 if (sb >= frame->subbands) break;
 344                         } else
 345                                 ch = 1;
 346                 }
 347
 348         }
 349
 350 }
 351
 352 /*
 353  * Unpacks a SBC frame at the beginning of the stream in data,
 354  * which has at most len bytes into frame.
 355  * Returns the length in bytes of the packed frame, or a negative
 356  * value on error. The error codes are:
 357  *
 358  *  -1   Data stream too short
 359  *  -2   Sync byte incorrect
 360  *  -3   CRC8 incorrect
 361  *  -4   Bitpool value out of bounds
 362  */
 363 static int sbc_unpack_frame(const uint8_t *data, struct sbc_frame *frame,
 364                                 size_t len)
 365 {
 366         int consumed;
 367         /* Will copy the parts of the header that are relevant to crc
 368          * calculation here */
 369         uint8_t crc_header[11] = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 };
 370         int crc_pos = 0;
 371         int32_t temp;
 372
 373         int audio_sample;
 374         int ch, sb, blk, bit;   /* channel, subband, block and bit standard
 375                                    counters */
 376         int bits[2][8];         /* bits distribution */
 377         uint32_t levels[2][8];  /* levels derived from that */
 378
 379         if (len < 4)
 380                 return -1;
 381
 382         if (data[0] != SBC_SYNCWORD)
 383                 return -2;
 384
 385         frame->frequency = (data[1] >> 6) & 0x03;
 386
 387         frame->block_mode = (data[1] >> 4) & 0x03;
 388         switch (frame->block_mode) {
 389         case SBC_BLK_4:
 390                 frame->blocks = 4;
 391                 break;
 392         case SBC_BLK_8:
 393                 frame->blocks = 8;
 394                 break;
 395         case SBC_BLK_12:
 396                 frame->blocks = 12;
 397                 break;
 398         case SBC_BLK_16:
 399                 frame->blocks = 16;
 400                 break;
 401         }
 402
 403         frame->mode = (data[1] >> 2) & 0x03;
 404         switch (frame->mode) {
 405         case MONO:
 406                 frame->channels = 1;
 407                 break;
 408         case DUAL_CHANNEL:      /* fall-through */
 409         case STEREO:
 410         case JOINT_STEREO:
 411                 frame->channels = 2;
 412                 break;
 413         }
 414
 415         frame->allocation = (data[1] >> 1) & 0x01;
 416
 417         frame->subband_mode = (data[1] & 0x01);
 418         frame->subbands = frame->subband_mode ? 8 : 4;
 419
 420         frame->bitpool = data[2];
 421
 422         if ((frame->mode == MONO || frame->mode == DUAL_CHANNEL) &&
 423                         frame->bitpool > 16 * frame->subbands)
 424                 return -4;
 425
 426         if ((frame->mode == STEREO || frame->mode == JOINT_STEREO) &&
 427                         frame->bitpool > 32 * frame->subbands)
 428                 return -4;
 429
 430         /* data[3] is crc, we're checking it later */
 431
 432         consumed = 32;
 433
 434         crc_header[0] = data[1];
 435         crc_header[1] = data[2];
 436         crc_pos = 16;
 437
 438         if (frame->mode == JOINT_STEREO) {
 439                 if (len * 8 < consumed + frame->subbands)
 440                         return -1;
 441
 442                 frame->joint = 0x00;
 443                 for (sb = 0; sb < frame->subbands - 1; sb++)
 444                         frame->joint |= ((data[4] >> (7 - sb)) & 0x01) << sb;
 445                 if (frame->subbands == 4)
 446                         crc_header[crc_pos / 8] = data[4] & 0xf0;
 447                 else
 448                         crc_header[crc_pos / 8] = data[4];
 449
 450                 consumed += frame->subbands;
 451                 crc_pos += frame->subbands;
 452         }
 453
 454         if (len * 8 < consumed + (4 * frame->subbands * frame->channels))
 455                 return -1;
 456
 457         for (ch = 0; ch < frame->channels; ch++) {
 458                 for (sb = 0; sb < frame->subbands; sb++) {
 459                         /* FIXME assert(consumed % 4 == 0); */
 460                         frame->scale_factor[ch][sb] =
 461                                 (data[consumed >> 3] >> (4 - (consumed & 0x7))) & 0x0F;
 462                         crc_header[crc_pos >> 3] |=
 463                                 frame->scale_factor[ch][sb] << (4 - (crc_pos & 0x7));
 464
 465                         consumed += 4;
 466                         crc_pos += 4;
 467                 }
 468         }
 469
 470         if (data[3] != sbc_crc8(crc_header, crc_pos))
 471                 return -3;
 472
 473         sbc_calculate_bits(frame, bits);
 474
 475         for (ch = 0; ch < frame->channels; ch++) {
 476                 for (sb = 0; sb < frame->subbands; sb++)
 477                         levels[ch][sb] = (1 << bits[ch][sb]) - 1;
 478         }
 479
 480         for (blk = 0; blk < frame->blocks; blk++) {
 481                 for (ch = 0; ch < frame->channels; ch++) {
 482                         for (sb = 0; sb < frame->subbands; sb++) {
 483                                 if (levels[ch][sb] > 0) {
 484                                         audio_sample = 0;
 485                                         for (bit = 0; bit < bits[ch][sb]; bit++) {
 486                                                 if (consumed > len * 8)
 487                                                         return -1;
 488
 489                                                 if ((data[consumed >> 3] >> (7 - (consumed & 0x7))) & 0x01)
 490                                                         audio_sample |= 1 << (bits[ch][sb] - bit - 1);
 491
 492                                                 consumed++;
 493                                         }
 494
 495                                         frame->sb_sample[blk][ch][sb] =
 496                                                 (((audio_sample << 1) | 1) << frame->scale_factor[ch][sb]) /
 497                                                 levels[ch][sb] - (1 << frame->scale_factor[ch][sb]);
 498                                 } else
 499                                         frame->sb_sample[blk][ch][sb] = 0;
 500                         }
 501                 }
 502         }
 503
 504         if (frame->mode == JOINT_STEREO) {
 505                 for (blk = 0; blk < frame->blocks; blk++) {
 506                         for (sb = 0; sb < frame->subbands; sb++) {
 507                                 if (frame->joint & (0x01 << sb)) {
 508                                         temp = frame->sb_sample[blk][0][sb] +
 509                                                 frame->sb_sample[blk][1][sb];
 510                                         frame->sb_sample[blk][1][sb] =
 511                                                 frame->sb_sample[blk][0][sb] -
 512                                                 frame->sb_sample[blk][1][sb];
 513                                         frame->sb_sample[blk][0][sb] = temp;
 514                                 }
 515                         }
 516                 }
 517         }
 518
 519         if ((consumed & 0x7) != 0)
 520                 consumed += 8 - (consumed & 0x7);
 521
 522         return consumed >> 3;
 523 }
 524
 525 static void sbc_decoder_init(struct sbc_decoder_state *state,
 526                                 const struct sbc_frame *frame)
 527 {
 528         int i, ch;
 529
 530         memset(state->V, 0, sizeof(state->V));
 531         state->subbands = frame->subbands;
 532
 533         for (ch = 0; ch < 2; ch++)
 534                 for (i = 0; i < frame->subbands * 2; i++)
 535                         state->offset[ch][i] = (10 * i + 10);
 536 }
 537
 538 static inline void sbc_synthesize_four(struct sbc_decoder_state *state,
 539                                 struct sbc_frame *frame, int ch, int blk)
 540 {
 541         int i, k, idx;
 542         int32_t *v = state->V[ch];
 543         int *offset = state->offset[ch];
 544
 545         for (i = 0; i < 8; i++) {
 546                 /* Shifting */
 547                 offset[i]--;
 548                 if (offset[i] < 0) {
 549                         offset[i] = 79;
 550                         memcpy(v + 80, v, 9 * sizeof(*v));
 551                 }
 552
 553                 /* Distribute the new matrix value to the shifted position */
 554                 v[offset[i]] = SCALE4_STAGED1(
 555                         MULA(synmatrix4[i][0], frame->sb_sample[blk][ch][0],
 556                         MULA(synmatrix4[i][1], frame->sb_sample[blk][ch][1],
 557                         MULA(synmatrix4[i][2], frame->sb_sample[blk][ch][2],
 558                         MUL (synmatrix4[i][3], frame->sb_sample[blk][ch][3])))));
 559         }
 560
 561         /* Compute the samples */
 562         for (idx = 0, i = 0; i < 4; i++, idx += 5) {
 563                 k = (i + 4) & 0xf;
 564
 565                 /* Store in output, Q0 */
 566                 frame->pcm_sample[ch][blk * 4 + i] = SCALE4_STAGED1(
 567                         MULA(v[offset[i] + 0], sbc_proto_4_40m0[idx + 0],
 568                         MULA(v[offset[k] + 1], sbc_proto_4_40m1[idx + 0],
 569                         MULA(v[offset[i] + 2], sbc_proto_4_40m0[idx + 1],
 570                         MULA(v[offset[k] + 3], sbc_proto_4_40m1[idx + 1],
 571                         MULA(v[offset[i] + 4], sbc_proto_4_40m0[idx + 2],
 572                         MULA(v[offset[k] + 5], sbc_proto_4_40m1[idx + 2],
 573                         MULA(v[offset[i] + 6], sbc_proto_4_40m0[idx + 3],
 574                         MULA(v[offset[k] + 7], sbc_proto_4_40m1[idx + 3],
 575                         MULA(v[offset[i] + 8], sbc_proto_4_40m0[idx + 4],
 576                         MUL( v[offset[k] + 9], sbc_proto_4_40m1[idx + 4])))))))))));
 577         }
 578 }
 579
 580 static inline void sbc_synthesize_eight(struct sbc_decoder_state *state,
 581                                 struct sbc_frame *frame, int ch, int blk)
 582 {
 583         int i, j, k, idx;
 584         int *offset = state->offset[ch];
 585
 586         for (i = 0; i < 16; i++) {
 587                 /* Shifting */
 588                 offset[i]--;
 589                 if (offset[i] < 0) {
 590                         offset[i] = 159;
 591                         for (j = 0; j < 9; j++)
 592                                 state->V[ch][j + 160] = state->V[ch][j];
 593                 }
 594
 595                 /* Distribute the new matrix value to the shifted position */
 596                 state->V[ch][offset[i]] = SCALE8_STAGED1(
 597                         MULA(synmatrix8[i][0], frame->sb_sample[blk][ch][0],
 598                         MULA(synmatrix8[i][1], frame->sb_sample[blk][ch][1],
 599                         MULA(synmatrix8[i][2], frame->sb_sample[blk][ch][2],
 600                         MULA(synmatrix8[i][3], frame->sb_sample[blk][ch][3],
 601                         MULA(synmatrix8[i][4], frame->sb_sample[blk][ch][4],
 602                         MULA(synmatrix8[i][5], frame->sb_sample[blk][ch][5],
 603                         MULA(synmatrix8[i][6], frame->sb_sample[blk][ch][6],
 604                         MUL( synmatrix8[i][7], frame->sb_sample[blk][ch][7])))))))));
 605         }
 606
 607         /* Compute the samples */
 608         for (idx = 0, i = 0; i < 8; i++, idx += 5) {
 609                 k = (i + 8) & 0xf;
 610
 611                 /* Store in output */
 612                 frame->pcm_sample[ch][blk * 8 + i] = SCALE8_STAGED1( // Q0
 613                         MULA(state->V[ch][offset[i] + 0], sbc_proto_8_80m0[idx + 0],
 614                         MULA(state->V[ch][offset[k] + 1], sbc_proto_8_80m1[idx + 0],
 615                         MULA(state->V[ch][offset[i] + 2], sbc_proto_8_80m0[idx + 1],
 616                         MULA(state->V[ch][offset[k] + 3], sbc_proto_8_80m1[idx + 1],
 617                         MULA(state->V[ch][offset[i] + 4], sbc_proto_8_80m0[idx + 2],
 618                         MULA(state->V[ch][offset[k] + 5], sbc_proto_8_80m1[idx + 2],
 619                         MULA(state->V[ch][offset[i] + 6], sbc_proto_8_80m0[idx + 3],
 620                         MULA(state->V[ch][offset[k] + 7], sbc_proto_8_80m1[idx + 3],
 621                         MULA(state->V[ch][offset[i] + 8], sbc_proto_8_80m0[idx + 4],
 622                         MUL( state->V[ch][offset[k] + 9], sbc_proto_8_80m1[idx + 4])))))))))));
 623         }
 624 }
 625
 626 static int sbc_synthesize_audio(struct sbc_decoder_state *state,
 627                                 struct sbc_frame *frame)
 628 {
 629         int ch, blk;
 630
 631         switch (frame->subbands) {
 632         case 4:
 633                 for (ch = 0; ch < frame->channels; ch++) {
 634                         for (blk = 0; blk < frame->blocks; blk++)
 635                                 sbc_synthesize_four(state, frame, ch, blk);
 636                 }
 637                 return frame->blocks * 4;
 638
 639         case 8:
 640                 for (ch = 0; ch < frame->channels; ch++) {
 641                         for (blk = 0; blk < frame->blocks; blk++)
 642                                 sbc_synthesize_eight(state, frame, ch, blk);
 643                 }
 644                 return frame->blocks * 8;
 645
 646         default:
 647                 return -EIO;
 648         }
 649 }
 650
 651 static void sbc_encoder_init(struct sbc_encoder_state *state,
 652                                 const struct sbc_frame *frame)
 653 {
 654         memset(&state->X, 0, sizeof(state->X));
 655         state->subbands = frame->subbands;
 656         state->position[0] = state->position[1] = 9 * frame->subbands;
 657 }
 658
 659 static inline void _sbc_analyze_four(const int32_t *in, int32_t *out)
 660 {
 661         sbc_fixed_t t[8], s[5];
 662
 663         t[0] = SCALE4_STAGE1( /* Q8 */
 664                 MULA(_sbc_proto_4[0], in[8] - in[32], /* Q18 */
 665                 MUL( _sbc_proto_4[1], in[16] - in[24])));
 666
 667         t[1] = SCALE4_STAGE1(
 668                 MULA(_sbc_proto_4[2], in[1],
 669                 MULA(_sbc_proto_4[3], in[9],
 670                 MULA(_sbc_proto_4[4], in[17],
 671                 MULA(_sbc_proto_4[5], in[25],
 672                 MUL( _sbc_proto_4[6], in[33]))))));
 673
 674         t[2] = SCALE4_STAGE1(
 675                 MULA(_sbc_proto_4[7], in[2],
 676                 MULA(_sbc_proto_4[8], in[10],
 677                 MULA(_sbc_proto_4[9], in[18],
 678                 MULA(_sbc_proto_4[10], in[26],
 679                 MUL( _sbc_proto_4[11], in[34]))))));
 680
 681         t[3] = SCALE4_STAGE1(
 682                 MULA(_sbc_proto_4[12], in[3],
 683                 MULA(_sbc_proto_4[13], in[11],
 684                 MULA(_sbc_proto_4[14], in[19],
 685                 MULA(_sbc_proto_4[15], in[27],
 686                 MUL( _sbc_proto_4[16], in[35]))))));
 687
 688         t[4] = SCALE4_STAGE1(
 689                 MULA(_sbc_proto_4[17], in[4] + in[36],
 690                 MULA(_sbc_proto_4[18], in[12] + in[28],
 691                 MUL( _sbc_proto_4[19], in[20]))));
 692
 693         t[5] = SCALE4_STAGE1(
 694                 MULA(_sbc_proto_4[16], in[5],
 695                 MULA(_sbc_proto_4[15], in[13],
 696                 MULA(_sbc_proto_4[14], in[21],
 697                 MULA(_sbc_proto_4[13], in[29],
 698                 MUL( _sbc_proto_4[12], in[37]))))));
 699
 700         /* don't compute t[6]... this term always multiplies
 701          * with cos(pi/2) = 0 */
 702
 703         t[7] = SCALE4_STAGE1(
 704                 MULA(_sbc_proto_4[6], in[7],
 705                 MULA(_sbc_proto_4[5], in[15],
 706                 MULA(_sbc_proto_4[4], in[23],
 707                 MULA(_sbc_proto_4[3], in[31],
 708                 MUL( _sbc_proto_4[2], in[39]))))));
 709
 710         s[0] = MUL( _anamatrix4[0], t[0] + t[4]);
 711         s[1] = MUL( _anamatrix4[2], t[2]);
 712         s[2] = MULA(_anamatrix4[1], t[1] + t[3],
 713                 MUL(_anamatrix4[3], t[5]));
 714         s[3] = MULA(_anamatrix4[3], t[1] + t[3],
 715                 MUL(_anamatrix4[1], -t[5] + t[7]));
 716         s[4] = MUL( _anamatrix4[3], t[7]);
 717
 718         out[0] = SCALE4_STAGE2( s[0] + s[1] + s[2] + s[4]); /* Q0 */
 719         out[1] = SCALE4_STAGE2(-s[0] + s[1] + s[3]);
 720         out[2] = SCALE4_STAGE2(-s[0] + s[1] - s[3]);
 721         out[3] = SCALE4_STAGE2( s[0] + s[1] - s[2] - s[4]);
 722 }
 723
 724 static inline void sbc_analyze_four(struct sbc_encoder_state *state,
 725                                 struct sbc_frame *frame, int ch, int blk)
 726 {
 727         int32_t *x = &state->X[ch][state->position[ch]];
 728         int16_t *pcm = &frame->pcm_sample[ch][blk * 4];
 729
 730         /* Input 4 Audio Samples */
 731         x[40] = x[0] = pcm[3];
 732         x[41] = x[1] = pcm[2];
 733         x[42] = x[2] = pcm[1];
 734         x[43] = x[3] = pcm[0];
 735
 736         _sbc_analyze_four(x, frame->sb_sample_f[blk][ch]);
 737
 738         state->position[ch] -= 4;
 739         if (state->position[ch] < 0)
 740                 state->position[ch] = 36;
 741 }
 742
 743 static inline void _sbc_analyze_eight(const int32_t *in, int32_t *out)
 744 {
 745         sbc_fixed_t t[8], s[8];
 746
 747         t[0] = SCALE8_STAGE1( /* Q10 */
 748                 MULA(_sbc_proto_8[0], (in[16] - in[64]), /* Q18 = Q18 * Q0 */
 749                 MULA(_sbc_proto_8[1], (in[32] - in[48]),
 750                 MULA(_sbc_proto_8[2], in[4],
 751                 MULA(_sbc_proto_8[3], in[20],
 752                 MULA(_sbc_proto_8[4], in[36],
 753                 MUL( _sbc_proto_8[5], in[52])))))));
 754
 755         t[1] = SCALE8_STAGE1(
 756                 MULA(_sbc_proto_8[6], in[2],
 757                 MULA(_sbc_proto_8[7], in[18],
 758                 MULA(_sbc_proto_8[8], in[34],
 759                 MULA(_sbc_proto_8[9], in[50],
 760                 MUL(_sbc_proto_8[10], in[66]))))));
 761
 762         t[2] = SCALE8_STAGE1(
 763                 MULA(_sbc_proto_8[11], in[1],
 764                 MULA(_sbc_proto_8[12], in[17],
 765                 MULA(_sbc_proto_8[13], in[33],
 766                 MULA(_sbc_proto_8[14], in[49],
 767                 MULA(_sbc_proto_8[15], in[65],
 768                 MULA(_sbc_proto_8[16], in[3],
 769                 MULA(_sbc_proto_8[17], in[19],
 770                 MULA(_sbc_proto_8[18], in[35],
 771                 MULA(_sbc_proto_8[19], in[51],
 772                 MUL( _sbc_proto_8[20], in[67])))))))))));
 773
 774         t[3] = SCALE8_STAGE1(
 775                 MULA( _sbc_proto_8[21], in[5],
 776                 MULA( _sbc_proto_8[22], in[21],
 777                 MULA( _sbc_proto_8[23], in[37],
 778                 MULA( _sbc_proto_8[24], in[53],
 779                 MULA( _sbc_proto_8[25], in[69],
 780                 MULA(-_sbc_proto_8[15], in[15],
 781                 MULA(-_sbc_proto_8[14], in[31],
 782                 MULA(-_sbc_proto_8[13], in[47],
 783                 MULA(-_sbc_proto_8[12], in[63],
 784                 MUL( -_sbc_proto_8[11], in[79])))))))))));
 785
 786         t[4] = SCALE8_STAGE1(
 787                 MULA( _sbc_proto_8[26], in[6],
 788                 MULA( _sbc_proto_8[27], in[22],
 789                 MULA( _sbc_proto_8[28], in[38],
 790                 MULA( _sbc_proto_8[29], in[54],
 791                 MULA( _sbc_proto_8[30], in[70],
 792                 MULA(-_sbc_proto_8[10], in[14],
 793                 MULA(-_sbc_proto_8[9], in[30],
 794                 MULA(-_sbc_proto_8[8], in[46],
 795                 MULA(-_sbc_proto_8[7], in[62],
 796                 MUL( -_sbc_proto_8[6], in[78])))))))))));
 797
 798         t[5] = SCALE8_STAGE1(
 799                 MULA( _sbc_proto_8[31], in[7],
 800                 MULA( _sbc_proto_8[32], in[23],
 801                 MULA( _sbc_proto_8[33], in[39],
 802                 MULA( _sbc_proto_8[34], in[55],
 803                 MULA( _sbc_proto_8[35], in[71],
 804                 MULA(-_sbc_proto_8[20], in[13],
 805                 MULA(-_sbc_proto_8[19], in[29],
 806                 MULA(-_sbc_proto_8[18], in[45],
 807                 MULA(-_sbc_proto_8[17], in[61],
 808                 MUL( -_sbc_proto_8[16], in[77])))))))))));
 809
 810         t[6] = SCALE8_STAGE1(
 811                 MULA( _sbc_proto_8[36], (in[8] + in[72]),
 812                 MULA( _sbc_proto_8[37], (in[24] + in[56]),
 813                 MULA( _sbc_proto_8[38], in[40],
 814                 MULA(-_sbc_proto_8[39], in[12],
 815                 MULA(-_sbc_proto_8[5], in[28],
 816                 MULA(-_sbc_proto_8[4], in[44],
 817                 MULA(-_sbc_proto_8[3], in[60],
 818                 MUL( -_sbc_proto_8[2], in[76])))))))));
 819
 820         t[7] = SCALE8_STAGE1(
 821                 MULA( _sbc_proto_8[35], in[9],
 822                 MULA( _sbc_proto_8[34], in[25],
 823                 MULA( _sbc_proto_8[33], in[41],
 824                 MULA( _sbc_proto_8[32], in[57],
 825                 MULA( _sbc_proto_8[31], in[73],
 826                 MULA(-_sbc_proto_8[25], in[11],
 827                 MULA(-_sbc_proto_8[24], in[27],
 828                 MULA(-_sbc_proto_8[23], in[43],
 829                 MULA(-_sbc_proto_8[22], in[59],
 830                 MUL( -_sbc_proto_8[21], in[75])))))))))));
 831
 832         s[0] = MULA(  _anamatrix8[0], t[0],
 833                 MUL(  _anamatrix8[1], t[6]));
 834         s[1] = MUL(   _anamatrix8[7], t[1]);
 835         s[2] = MULA(  _anamatrix8[2], t[2],
 836                 MULA( _anamatrix8[3], t[3],
 837                 MULA( _anamatrix8[4], t[5],
 838                 MUL(  _anamatrix8[5], t[7]))));
 839         s[3] = MUL(   _anamatrix8[6], t[4]);
 840         s[4] = MULA(  _anamatrix8[3], t[2],
 841                 MULA(-_anamatrix8[5], t[3],
 842                 MULA(-_anamatrix8[2], t[5],
 843                 MUL( -_anamatrix8[4], t[7]))));
 844         s[5] = MULA(  _anamatrix8[4], t[2],
 845                 MULA(-_anamatrix8[2], t[3],
 846                 MULA( _anamatrix8[5], t[5],
 847                 MUL(  _anamatrix8[3], t[7]))));
 848         s[6] = MULA(  _anamatrix8[1], t[0],
 849                 MUL( -_anamatrix8[0], t[6]));
 850         s[7] = MULA(  _anamatrix8[5], t[2],
 851                 MULA(-_anamatrix8[4], t[3],
 852                 MULA( _anamatrix8[3], t[5],
 853                 MUL( -_anamatrix8[2], t[7]))));
 854
 855         out[0] = SCALE8_STAGE2( s[0] + s[1] + s[2] + s[3]);
 856         out[1] = SCALE8_STAGE2( s[1] - s[3] + s[4] + s[6]);
 857         out[2] = SCALE8_STAGE2( s[1] - s[3] + s[5] - s[6]);
 858         out[3] = SCALE8_STAGE2(-s[0] + s[1] + s[3] + s[7]);
 859         out[4] = SCALE8_STAGE2(-s[0] + s[1] + s[3] - s[7]);
 860         out[5] = SCALE8_STAGE2( s[1] - s[3] - s[5] - s[6]);
 861         out[6] = SCALE8_STAGE2( s[1] - s[3] - s[4] + s[6]);
 862         out[7] = SCALE8_STAGE2( s[0] + s[1] - s[2] + s[3]);
 863 }
 864
 865 static inline void sbc_analyze_eight(struct sbc_encoder_state *state,
 866                                         struct sbc_frame *frame, int ch,
 867                                         int blk)
 868 {
 869         int32_t *x = &state->X[ch][state->position[ch]];
 870         int16_t *pcm = &frame->pcm_sample[ch][blk * 8];
 871
 872         /* Input 8 Audio Samples */
 873         x[80] = x[0] = pcm[7];
 874         x[81] = x[1] = pcm[6];
 875         x[82] = x[2] = pcm[5];
 876         x[83] = x[3] = pcm[4];
 877         x[84] = x[4] = pcm[3];
 878         x[85] = x[5] = pcm[2];
 879         x[86] = x[6] = pcm[1];
 880         x[87] = x[7] = pcm[0];
 881
 882         _sbc_analyze_eight(x, frame->sb_sample_f[blk][ch]);
 883
 884         state->position[ch] -= 8;
 885         if (state->position[ch] < 0)
 886                 state->position[ch] = 72;
 887 }
 888
 889 static int sbc_analyze_audio(struct sbc_encoder_state *state,
 890                                 struct sbc_frame *frame)
 891 {
 892         int ch, blk;
 893
 894         switch (frame->subbands) {
 895         case 4:
 896                 for (ch = 0; ch < frame->channels; ch++)
 897                         for (blk = 0; blk < frame->blocks; blk++)
 898                                 sbc_analyze_four(state, frame, ch, blk);
 899                 return frame->blocks * 4;
 900
 901         case 8:
 902                 for (ch = 0; ch < frame->channels; ch++)
 903                         for (blk = 0; blk < frame->blocks; blk++)
 904                                 sbc_analyze_eight(state, frame, ch, blk);
 905                 return frame->blocks * 8;
 906
 907         default:
 908                 return -EIO;
 909         }
 910 }
 911
 912 /* Supplementary bitstream writing macros for 'sbc_pack_frame' */
 913
 914 #define PUT_BITS(v, n)\
 915         bits_cache = (v) | (bits_cache << (n));\
 916         bits_count += (n);\
 917         if (bits_count >= 16) {\
 918                 bits_count -= 8;\
 919                 *data_ptr++ = (uint8_t) (bits_cache >> bits_count);\
 920                 bits_count -= 8;\
 921                 *data_ptr++ = (uint8_t) (bits_cache >> bits_count);\
 922         }\
 923
 924 #define FLUSH_BITS()\
 925         while (bits_count >= 8) {\
 926                 bits_count -= 8;\
 927                 *data_ptr++ = (uint8_t) (bits_cache >> bits_count);\
 928         }\
 929         if (bits_count > 0)\
 930             *data_ptr++ = (uint8_t) (bits_cache << (8 - bits_count));\
 931
 932 /*
 933  * Packs the SBC frame from frame into the memory at data. At most len
 934  * bytes will be used, should more memory be needed an appropriate
 935  * error code will be returned. Returns the length of the packed frame
 936  * on success or a negative value on error.
 937  *
 938  * The error codes are:
 939  * -1 Not enough memory reserved
 940  * -2 Unsupported sampling rate
 941  * -3 Unsupported number of blocks
 942  * -4 Unsupported number of subbands
 943  * -5 Bitpool value out of bounds
 944  * -99 not implemented
 945  */
 946
 947 static int sbc_pack_frame(uint8_t *data, struct sbc_frame *frame, size_t len)
 948 {
 949         /* Bitstream writer starts from the fourth byte */
 950         uint8_t *data_ptr = data + 4;
 951         uint32_t bits_cache = 0;
 952         uint32_t bits_count = 0;
 953
 954         /* Will copy the header parts for CRC-8 calculation here */
 955         uint8_t crc_header[11] = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 };
 956         int crc_pos = 0;
 957
 958         uint16_t audio_sample;
 959
 960         int ch, sb, blk;        /* channel, subband, block and bit counters */
 961         int bits[2][8];         /* bits distribution */
 962         int levels[2][8];       /* levels are derived from that */
 963
 964         u_int32_t scalefactor[2][8];    /* derived from frame->scale_factor */
 965
 966         data[0] = SBC_SYNCWORD;
 967
 968         data[1] = (frame->frequency & 0x03) << 6;
 969
 970         data[1] |= (frame->block_mode & 0x03) << 4;
 971
 972         data[1] |= (frame->mode & 0x03) << 2;
 973
 974         data[1] |= (frame->allocation & 0x01) << 1;
 975
 976         switch (frame->subbands) {
 977         case 4:
 978                 /* Nothing to do */
 979                 break;
 980         case 8:
 981                 data[1] |= 0x01;
 982                 break;
 983         default:
 984                 return -4;
 985                 break;
 986         }
 987
 988         data[2] = frame->bitpool;
 989
 990         if ((frame->mode == MONO || frame->mode == DUAL_CHANNEL) &&
 991                         frame->bitpool > frame->subbands << 4)
 992                 return -5;
 993
 994         if ((frame->mode == STEREO || frame->mode == JOINT_STEREO) &&
 995                         frame->bitpool > frame->subbands << 5)
 996                 return -5;
 997
 998         /* Can't fill in crc yet */
 999
1000         crc_header[0] = data[1];
1001         crc_header[1] = data[2];
1002         crc_pos = 16;
1003
1004         for (ch = 0; ch < frame->channels; ch++) {
1005                 for (sb = 0; sb < frame->subbands; sb++) {
1006                         frame->scale_factor[ch][sb] = 0;
1007                         scalefactor[ch][sb] = 2;
1008                         for (blk = 0; blk < frame->blocks; blk++) {
1009                                 while (scalefactor[ch][sb] < fabs(frame->sb_sample_f[blk][ch][sb])) {
1010                                         frame->scale_factor[ch][sb]++;
1011                                         scalefactor[ch][sb] *= 2;
1012                                 }
1013                         }
1014                 }
1015         }
1016
1017         if (frame->mode == JOINT_STEREO) {
1018                 /* like frame->sb_sample but joint stereo */
1019                 int32_t sb_sample_j[16][2];
1020                 /* scalefactor and scale_factor in joint case */
1021                 u_int32_t scalefactor_j[2];
1022                 uint8_t scale_factor_j[2];
1023
1024                 uint8_t joint = 0;
1025                 frame->joint = 0;
1026
1027                 for (sb = 0; sb < frame->subbands - 1; sb++) {
1028                         scale_factor_j[0] = 0;
1029                         scalefactor_j[0] = 2;
1030                         scale_factor_j[1] = 0;
1031                         scalefactor_j[1] = 2;
1032
1033                         for (blk = 0; blk < frame->blocks; blk++) {
1034                                 /* Calculate joint stereo signal */
1035                                 sb_sample_j[blk][0] =
1036                                         (frame->sb_sample_f[blk][0][sb] +
1037                                                 frame->sb_sample_f[blk][1][sb]) >> 1;
1038                                 sb_sample_j[blk][1] =
1039                                         (frame->sb_sample_f[blk][0][sb] -
1040                                                 frame->sb_sample_f[blk][1][sb]) >> 1;
1041
1042                                 /* calculate scale_factor_j and scalefactor_j for joint case */
1043                                 while (scalefactor_j[0] < fabs(sb_sample_j[blk][0])) {
1044                                         scale_factor_j[0]++;
1045                                         scalefactor_j[0] *= 2;
1046                                 }
1047                                 while (scalefactor_j[1] < fabs(sb_sample_j[blk][1])) {
1048                                         scale_factor_j[1]++;
1049                                         scalefactor_j[1] *= 2;
1050                                 }
1051                         }
1052
1053                         /* decide whether to join this subband */
1054                         if ((scalefactor[0][sb] + scalefactor[1][sb]) >
1055                                         (scalefactor_j[0] + scalefactor_j[1]) ) {
1056                                 /* use joint stereo for this subband */
1057                                 joint |= 1 << (frame->subbands - 1 - sb);
1058                                 frame->joint |= 1 << sb;
1059                                 frame->scale_factor[0][sb] = scale_factor_j[0];
1060                                 frame->scale_factor[1][sb] = scale_factor_j[1];
1061                                 scalefactor[0][sb] = scalefactor_j[0];
1062                                 scalefactor[1][sb] = scalefactor_j[1];
1063                                 for (blk = 0; blk < frame->blocks; blk++) {
1064                                         frame->sb_sample_f[blk][0][sb] =
1065                                                         sb_sample_j[blk][0];
1066                                         frame->sb_sample_f[blk][1][sb] =
1067                                                         sb_sample_j[blk][1];
1068                                 }
1069                         }
1070                 }
1071
1072                 PUT_BITS(joint, frame->subbands);
1073                 crc_header[crc_pos >> 3] = joint;
1074                 crc_pos += frame->subbands;
1075         }
1076
1077         for (ch = 0; ch < frame->channels; ch++) {
1078                 for (sb = 0; sb < frame->subbands; sb++) {
1079                         PUT_BITS(frame->scale_factor[ch][sb] & 0x0F, 4);
1080                         crc_header[crc_pos >> 3] <<= 4;
1081                         crc_header[crc_pos >> 3] |= frame->scale_factor[ch][sb] & 0x0F;
1082                         crc_pos += 4;
1083                 }
1084         }
1085
1086         /* align the last crc byte */
1087         if (crc_pos % 8)
1088                 crc_header[crc_pos >> 3] <<= 8 - (crc_pos % 8);
1089
1090         data[3] = sbc_crc8(crc_header, crc_pos);
1091
1092         sbc_calculate_bits(frame, bits);
1093
1094         for (ch = 0; ch < frame->channels; ch++) {
1095                 for (sb = 0; sb < frame->subbands; sb++)
1096                         levels[ch][sb] = (1 << bits[ch][sb]) - 1;
1097         }
1098
1099         for (blk = 0; blk < frame->blocks; blk++) {
1100                 for (ch = 0; ch < frame->channels; ch++) {
1101                         for (sb = 0; sb < frame->subbands; sb++) {
1102                                 if (levels[ch][sb] > 0) {
1103                                         audio_sample =
1104                                                 (uint16_t) (((((int64_t)frame->sb_sample_f[blk][ch][sb]*levels[ch][sb]) >>
1105                                                                         (frame->scale_factor[ch][sb] + 1)) +
1106                                                                 levels[ch][sb]) >> 1);
1107                                         PUT_BITS(audio_sample & levels[ch][sb], bits[ch][sb]);
1108                                 }
1109                         }
1110                 }
1111         }
1112
1113         FLUSH_BITS();
1114
1115         return data_ptr - data;
1116 }
1117
1118 struct sbc_priv {
1119         int init;
1120         struct sbc_frame frame;
1121         struct sbc_decoder_state dec_state;
1122         struct sbc_encoder_state enc_state;
1123 };
1124
1125 static void sbc_set_defaults(sbc_t *sbc, unsigned long flags)
1126 {
1127         sbc->frequency = SBC_FREQ_44100;
1128         sbc->mode = SBC_MODE_STEREO;
1129         sbc->subbands = SBC_SB_8;
1130         sbc->blocks = SBC_BLK_16;
1131         sbc->bitpool = 32;
1132 #if __BYTE_ORDER == __LITTLE_ENDIAN
1133         sbc->endian = SBC_LE;
1134 #elif __BYTE_ORDER == __BIG_ENDIAN
1135         sbc->endian = SBC_BE;
1136 #else
1137 #error "Unknown byte order"
1138 #endif
1139 }
1140
1141 int sbc_init(sbc_t *sbc, unsigned long flags)
1142 {
1143         if (!sbc)
1144                 return -EIO;
1145
1146         memset(sbc, 0, sizeof(sbc_t));
1147
1148         sbc->priv = malloc(sizeof(struct sbc_priv));
1149         if (!sbc->priv)
1150                 return -ENOMEM;
1151
1152         memset(sbc->priv, 0, sizeof(struct sbc_priv));
1153
1154         sbc_set_defaults(sbc, flags);
1155
1156         return 0;
1157 }
1158
1159 int sbc_parse(sbc_t *sbc, void *input, int input_len)
1160 {
1161         return sbc_decode(sbc, input, input_len, NULL, 0, NULL);
1162 }
1163
1164 int sbc_decode(sbc_t *sbc, void *input, int input_len, void *output,
1165                 int output_len, int *written)
1166 {
1167         struct sbc_priv *priv;
1168         char *ptr;
1169         int i, ch, framelen, samples;
1170
1171         if (!sbc && !input)
1172                 return -EIO;
1173
1174         priv = sbc->priv;
1175
1176         framelen = sbc_unpack_frame(input, &priv->frame, input_len);
1177
1178         if (!priv->init) {
1179                 sbc_decoder_init(&priv->dec_state, &priv->frame);
1180                 priv->init = 1;
1181
1182                 sbc->frequency = priv->frame.frequency;
1183                 sbc->mode = priv->frame.mode;
1184                 sbc->subbands = priv->frame.subband_mode;
1185                 sbc->blocks = priv->frame.block_mode;
1186                 sbc->allocation = priv->frame.allocation;
1187                 sbc->bitpool = priv->frame.bitpool;
1188
1189                 priv->frame.codesize = sbc_get_codesize(sbc);
1190                 priv->frame.length = sbc_get_frame_length(sbc);
1191         }
1192
1193         if (!output)
1194                 return framelen;
1195
1196         if (written)
1197                 *written = 0;
1198
1199         samples = sbc_synthesize_audio(&priv->dec_state, &priv->frame);
1200
1201         ptr = output;
1202
1203         if (output_len < samples * priv->frame.channels * 2)
1204                 samples = output_len / (priv->frame.channels * 2);
1205
1206         for (i = 0; i < samples; i++) {
1207                 for (ch = 0; ch < priv->frame.channels; ch++) {
1208                         int16_t s;
1209                         s = priv->frame.pcm_sample[ch][i];
1210
1211 #if __BYTE_ORDER == __LITTLE_ENDIAN
1212                         if (sbc->endian == SBC_BE) {
1213 #elif __BYTE_ORDER == __BIG_ENDIAN
1214                         if (sbc->endian == SBC_LE) {
1215 #else
1216 #error "Unknown byte order"
1217 #endif
1218                                 *ptr++ = (s & 0xff00) >> 8;
1219                                 *ptr++ = (s & 0x00ff);
1220                         } else {
1221                                 *ptr++ = (s & 0x00ff);
1222                                 *ptr++ = (s & 0xff00) >> 8;
1223                         }
1224                 }
1225         }
1226
1227         if (written)
1228                 *written = samples * priv->frame.channels * 2;
1229
1230         return framelen;
1231 }
1232
1233 int sbc_encode(sbc_t *sbc, void *input, int input_len, void *output,
1234                 int output_len, int *written)
1235 {
1236         struct sbc_priv *priv;
1237         char *ptr;
1238         int i, ch, framelen, samples;
1239
1240         if (!sbc && !input)
1241                 return -EIO;
1242
1243         priv = sbc->priv;
1244
1245         if (written)
1246                 *written = 0;
1247
1248         if (!priv->init) {
1249                 priv->frame.frequency = sbc->frequency;
1250                 priv->frame.mode = sbc->mode;
1251                 priv->frame.channels = sbc->mode == SBC_MODE_MONO ? 1 : 2;
1252                 priv->frame.allocation = sbc->allocation;
1253                 priv->frame.subband_mode = sbc->subbands;
1254                 priv->frame.subbands = sbc->subbands ? 8 : 4;
1255                 priv->frame.block_mode = sbc->blocks;
1256                 priv->frame.blocks = 4 + (sbc->blocks * 4);
1257                 priv->frame.bitpool = sbc->bitpool;
1258                 priv->frame.codesize = sbc_get_codesize(sbc);
1259                 priv->frame.length = sbc_get_frame_length(sbc);
1260
1261                 sbc_encoder_init(&priv->enc_state, &priv->frame);
1262                 priv->init = 1;
1263         }
1264
1265         /* input must be large enough to encode a complete frame */
1266         if (input_len < priv->frame.codesize)
1267                 return 0;
1268
1269         /* output must be large enough to receive the encoded frame */
1270         if (!output || output_len < priv->frame.length)
1271                 return -ENOSPC;
1272
1273         ptr = input;
1274
1275         for (i = 0; i < priv->frame.subbands * priv->frame.blocks; i++) {
1276                 for (ch = 0; ch < priv->frame.channels; ch++) {
1277                         int16_t s;
1278 #if __BYTE_ORDER == __LITTLE_ENDIAN
1279                         if (sbc->endian == SBC_BE)
1280 #elif __BYTE_ORDER == __BIG_ENDIAN
1281                         if (sbc->endian == SBC_LE)
1282 #else
1283 #error "Unknown byte order"
1284 #endif
1285                                 s = (ptr[0] & 0xff) << 8 | (ptr[1] & 0xff);
1286                         else
1287                                 s = (ptr[0] & 0xff) | (ptr[1] & 0xff) << 8;
1288                         ptr += 2;
1289                         priv->frame.pcm_sample[ch][i] = s;
1290                 }
1291         }
1292
1293         samples = sbc_analyze_audio(&priv->enc_state, &priv->frame);
1294
1295         framelen = sbc_pack_frame(output, &priv->frame, output_len);
1296
1297         if (written)
1298                 *written = framelen;
1299
1300         return samples * priv->frame.channels * 2;
1301 }
1302
1303 void sbc_finish(sbc_t *sbc)
1304 {
1305         if (!sbc)
1306                 return;
1307
1308         if (sbc->priv)
1309                 free(sbc->priv);
1310
1311         memset(sbc, 0, sizeof(sbc_t));
1312 }
1313
1314 int sbc_get_frame_length(sbc_t *sbc)
1315 {
1316         int ret;
1317         uint8_t subbands, channels, blocks, joint;
1318         struct sbc_priv *priv;
1319
1320         priv = sbc->priv;
1321         if (!priv->init) {
1322                 subbands = sbc->subbands ? 8 : 4;
1323                 blocks = 4 + (sbc->blocks * 4);
1324                 channels = sbc->mode == SBC_MODE_MONO ? 1 : 2;
1325                 joint = sbc->mode == SBC_MODE_JOINT_STEREO ? 1 : 0;
1326         } else {
1327                 subbands = priv->frame.subbands;
1328                 blocks = priv->frame.blocks;
1329                 channels = priv->frame.channels;
1330                 joint = priv->frame.joint;
1331         }
1332
1333         ret = 4 + (4 * subbands * channels) / 8;
1334
1335         /* This term is not always evenly divide so we round it up */
1336         if (channels == 1)
1337                 ret += ((blocks * channels * sbc->bitpool) + 7) / 8;
1338         else
1339                 ret += (((joint ? subbands : 0) + blocks * sbc->bitpool) + 7)
1340                         / 8;
1341
1342         return ret;
1343 }
1344
1345 int sbc_get_frame_duration(sbc_t *sbc)
1346 {
1347         uint8_t subbands, blocks;
1348         uint16_t frequency;
1349         struct sbc_priv *priv;
1350
1351         priv = sbc->priv;
1352         if (!priv->init) {
1353                 subbands = sbc->subbands ? 8 : 4;
1354                 blocks = 4 + (sbc->blocks * 4);
1355         } else {
1356                 subbands = priv->frame.subbands;
1357                 blocks = priv->frame.blocks;
1358         }
1359
1360         switch (sbc->frequency) {
1361         case SBC_FREQ_16000:
1362                 frequency = 16000;
1363                 break;
1364
1365         case SBC_FREQ_32000:
1366                 frequency = 32000;
1367                 break;
1368
1369         case SBC_FREQ_44100:
1370                 frequency = 44100;
1371                 break;
1372
1373         case SBC_FREQ_48000:
1374                 frequency = 48000;
1375                 break;
1376         default:
1377                 return 0;
1378         }
1379
1380         return (1000000 * blocks * subbands) / frequency;
1381 }
1382
1383 int sbc_get_codesize(sbc_t *sbc)
1384 {
1385         uint8_t subbands, channels, blocks;
1386         struct sbc_priv *priv;
1387
1388         priv = sbc->priv;
1389         if (!priv->init) {
1390                 subbands = sbc->subbands ? 8 : 4;
1391                 blocks = 4 + (sbc->blocks * 4);
1392                 channels = sbc->mode == SBC_MODE_MONO ? 1 : 2;
1393         } else {
1394                 subbands = priv->frame.subbands;
1395                 blocks = priv->frame.blocks;
1396                 channels = priv->frame.channels;
1397         }
1398
1399         return subbands * blocks * channels * 2;
1400 }
1401
1402 int sbc_reinit(sbc_t *sbc, unsigned long flags)
1403 {
1404         struct sbc_priv *priv;
1405
1406         if (!sbc || !sbc->priv)
1407                 return -EIO;
1408
1409         priv = sbc->priv;
1410
1411         if (priv->init == 1)
1412                 memset(sbc->priv, 0, sizeof(struct sbc_priv));
1413
1414         sbc_set_defaults(sbc, flags);
1415
1416         return 0;
1417 }