sbc/sbc.c

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