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 - 4 *
 692                                         state->increment + frame->blocks * 4];
 693                         for (blk = 0; blk < frame->blocks;
 694                                                 blk += state->increment) {
 695                                 state->sbc_analyze_4b_4s(
 696                                         state, x,
 697                                         frame->sb_sample_f[blk][ch],
 698                                         frame->sb_sample_f[blk + 1][ch] -
 699                                         frame->sb_sample_f[blk][ch]);
 700                                 x -= 4 * state->increment;
 701                         }
 702                 }
 703                 return frame->blocks * 4;
 704
 705         case 8:
 706                 for (ch = 0; ch < frame->channels; ch++) {
 707                         x = &state->X[ch][state->position - 8 *
 708                                         state->increment + frame->blocks * 8];
 709                         for (blk = 0; blk < frame->blocks;
 710                                                 blk += state->increment) {
 711                                 state->sbc_analyze_4b_8s(
 712                                         state, x,
 713                                         frame->sb_sample_f[blk][ch],
 714                                         frame->sb_sample_f[blk + 1][ch] -
 715                                         frame->sb_sample_f[blk][ch]);
 716                                 x -= 8 * state->increment;
 717                         }
 718                 }
 719                 return frame->blocks * 8;
 720
 721         default:
 722                 return -EIO;
 723         }
 724 }
 725
 726 /* Supplementary bitstream writing macros for 'sbc_pack_frame' */
 727
 728 #define PUT_BITS(data_ptr, bits_cache, bits_count, v, n)                \
 729         do {                                                            \
 730                 bits_cache = (v) | (bits_cache << (n));                 \
 731                 bits_count += (n);                                      \
 732                 if (bits_count >= 16) {                                 \
 733                         bits_count -= 8;                                \
 734                         *data_ptr++ = (uint8_t)                         \
 735                                 (bits_cache >> bits_count);             \
 736                         bits_count -= 8;                                \
 737                         *data_ptr++ = (uint8_t)                         \
 738                                 (bits_cache >> bits_count);             \
 739                 }                                                       \
 740         } while (0)
 741
 742 #define FLUSH_BITS(data_ptr, bits_cache, bits_count)                    \
 743         do {                                                            \
 744                 while (bits_count >= 8) {                               \
 745                         bits_count -= 8;                                \
 746                         *data_ptr++ = (uint8_t)                         \
 747                                 (bits_cache >> bits_count);             \
 748                 }                                                       \
 749                 if (bits_count > 0)                                     \
 750                         *data_ptr++ = (uint8_t)                         \
 751                                 (bits_cache << (8 - bits_count));       \
 752         } while (0)
 753
 754 /*
 755  * Packs the SBC frame from frame into the memory at data. At most len
 756  * bytes will be used, should more memory be needed an appropriate
 757  * error code will be returned. Returns the length of the packed frame
 758  * on success or a negative value on error.
 759  *
 760  * The error codes are:
 761  * -1 Not enough memory reserved
 762  * -2 Unsupported sampling rate
 763  * -3 Unsupported number of blocks
 764  * -4 Unsupported number of subbands
 765  * -5 Bitpool value out of bounds
 766  * -99 not implemented
 767  */
 768
 769 static SBC_ALWAYS_INLINE ssize_t sbc_pack_frame_internal(uint8_t *data,
 770                                         struct sbc_frame *frame, size_t len,
 771                                         int frame_subbands, int frame_channels,
 772                                         int joint)
 773 {
 774         /* Bitstream writer starts from the fourth byte */
 775         uint8_t *data_ptr = data + 4;
 776         uint32_t bits_cache = 0;
 777         uint32_t bits_count = 0;
 778
 779         /* Will copy the header parts for CRC-8 calculation here */
 780         uint8_t crc_header[11] = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 };
 781         int crc_pos = 0;
 782
 783         uint32_t audio_sample;
 784
 785         int ch, sb, blk;        /* channel, subband, block and bit counters */
 786         int bits[2][8];         /* bits distribution */
 787         uint32_t levels[2][8];  /* levels are derived from that */
 788         uint32_t sb_sample_delta[2][8];
 789
 790         data[0] = SBC_SYNCWORD;
 791
 792         data[1] = (frame->frequency & 0x03) << 6;
 793
 794         data[1] |= (frame->block_mode & 0x03) << 4;
 795
 796         data[1] |= (frame->mode & 0x03) << 2;
 797
 798         data[1] |= (frame->allocation & 0x01) << 1;
 799
 800         switch (frame_subbands) {
 801         case 4:
 802                 /* Nothing to do */
 803                 break;
 804         case 8:
 805                 data[1] |= 0x01;
 806                 break;
 807         default:
 808                 return -4;
 809                 break;
 810         }
 811
 812         data[2] = frame->bitpool;
 813
 814         if ((frame->mode == MONO || frame->mode == DUAL_CHANNEL) &&
 815                         frame->bitpool > frame_subbands << 4)
 816                 return -5;
 817
 818         if ((frame->mode == STEREO || frame->mode == JOINT_STEREO) &&
 819                         frame->bitpool > frame_subbands << 5)
 820                 return -5;
 821
 822         /* Can't fill in crc yet */
 823
 824         crc_header[0] = data[1];
 825         crc_header[1] = data[2];
 826         crc_pos = 16;
 827
 828         if (frame->mode == JOINT_STEREO) {
 829                 PUT_BITS(data_ptr, bits_cache, bits_count,
 830                         joint, frame_subbands);
 831                 crc_header[crc_pos >> 3] = joint;
 832                 crc_pos += frame_subbands;
 833         }
 834
 835         for (ch = 0; ch < frame_channels; ch++) {
 836                 for (sb = 0; sb < frame_subbands; sb++) {
 837                         PUT_BITS(data_ptr, bits_cache, bits_count,
 838                                 frame->scale_factor[ch][sb] & 0x0F, 4);
 839                         crc_header[crc_pos >> 3] <<= 4;
 840                         crc_header[crc_pos >> 3] |= frame->scale_factor[ch][sb] & 0x0F;
 841                         crc_pos += 4;
 842                 }
 843         }
 844
 845         /* align the last crc byte */
 846         if (crc_pos % 8)
 847                 crc_header[crc_pos >> 3] <<= 8 - (crc_pos % 8);
 848
 849         data[3] = sbc_crc8(crc_header, crc_pos);
 850
 851         sbc_calculate_bits(frame, bits);
 852
 853         for (ch = 0; ch < frame_channels; ch++) {
 854                 for (sb = 0; sb < frame_subbands; sb++) {
 855                         levels[ch][sb] = ((1 << bits[ch][sb]) - 1) <<
 856                                 (32 - (frame->scale_factor[ch][sb] +
 857                                         SCALE_OUT_BITS + 2));
 858                         sb_sample_delta[ch][sb] = (uint32_t) 1 <<
 859                                 (frame->scale_factor[ch][sb] +
 860                                         SCALE_OUT_BITS + 1);
 861                 }
 862         }
 863
 864         for (blk = 0; blk < frame->blocks; blk++) {
 865                 for (ch = 0; ch < frame_channels; ch++) {
 866                         for (sb = 0; sb < frame_subbands; sb++) {
 867
 868                                 if (bits[ch][sb] == 0)
 869                                         continue;
 870
 871                                 audio_sample = ((uint64_t) levels[ch][sb] *
 872                                         (sb_sample_delta[ch][sb] +
 873                                         frame->sb_sample_f[blk][ch][sb])) >> 32;
 874
 875                                 PUT_BITS(data_ptr, bits_cache, bits_count,
 876                                         audio_sample, bits[ch][sb]);
 877                         }
 878                 }
 879         }
 880
 881         FLUSH_BITS(data_ptr, bits_cache, bits_count);
 882
 883         return data_ptr - data;
 884 }
 885
 886 static ssize_t sbc_pack_frame(uint8_t *data, struct sbc_frame *frame, size_t len,
 887                                                                 int joint)
 888 {
 889         if (frame->subbands == 4) {
 890                 if (frame->channels == 1)
 891                         return sbc_pack_frame_internal(
 892                                 data, frame, len, 4, 1, joint);
 893                 else
 894                         return sbc_pack_frame_internal(
 895                                 data, frame, len, 4, 2, joint);
 896         } else {
 897                 if (frame->channels == 1)
 898                         return sbc_pack_frame_internal(
 899                                 data, frame, len, 8, 1, joint);
 900                 else
 901                         return sbc_pack_frame_internal(
 902                                 data, frame, len, 8, 2, joint);
 903         }
 904 }
 905
 906 static void sbc_encoder_init(struct sbc_encoder_state *state,
 907                                         const struct sbc_frame *frame)
 908 {
 909         memset(&state->X, 0, sizeof(state->X));
 910         state->position = (SBC_X_BUFFER_SIZE - frame->subbands * 9) & ~7;
 911         state->increment = 4;
 912
 913         sbc_init_primitives(state);
 914 }
 915
 916 struct sbc_priv {
 917         int init;
 918         struct SBC_ALIGNED sbc_frame frame;
 919         struct SBC_ALIGNED sbc_decoder_state dec_state;
 920         struct SBC_ALIGNED sbc_encoder_state enc_state;
 921 };
 922
 923 static void sbc_set_defaults(sbc_t *sbc, unsigned long flags)
 924 {
 925         sbc->frequency = SBC_FREQ_44100;
 926         sbc->mode = SBC_MODE_STEREO;
 927         sbc->subbands = SBC_SB_8;
 928         sbc->blocks = SBC_BLK_16;
 929         sbc->bitpool = 32;
 930 #if __BYTE_ORDER == __LITTLE_ENDIAN
 931         sbc->endian = SBC_LE;
 932 #elif __BYTE_ORDER == __BIG_ENDIAN
 933         sbc->endian = SBC_BE;
 934 #else
 935 #error "Unknown byte order"
 936 #endif
 937 }
 938
 939 SBC_EXPORT int sbc_init(sbc_t *sbc, unsigned long flags)
 940 {
 941         if (!sbc)
 942                 return -EIO;
 943
 944         memset(sbc, 0, sizeof(sbc_t));
 945
 946         sbc->priv_alloc_base = malloc(sizeof(struct sbc_priv) + SBC_ALIGN_MASK);
 947         if (!sbc->priv_alloc_base)
 948                 return -ENOMEM;
 949
 950         sbc->priv = (void *) (((uintptr_t) sbc->priv_alloc_base +
 951                         SBC_ALIGN_MASK) & ~((uintptr_t) SBC_ALIGN_MASK));
 952
 953         memset(sbc->priv, 0, sizeof(struct sbc_priv));
 954
 955         sbc_set_defaults(sbc, flags);
 956
 957         return 0;
 958 }
 959
 960 SBC_EXPORT ssize_t sbc_parse(sbc_t *sbc, const void *input, size_t input_len)
 961 {
 962         return sbc_decode(sbc, input, input_len, NULL, 0, NULL);
 963 }
 964
 965 SBC_EXPORT ssize_t sbc_decode(sbc_t *sbc, const void *input, size_t input_len,
 966                         void *output, size_t output_len, size_t *written)
 967 {
 968         struct sbc_priv *priv;
 969         char *ptr;
 970         int i, ch, framelen, samples;
 971
 972         if (!sbc || !input)
 973                 return -EIO;
 974
 975         priv = sbc->priv;
 976
 977         framelen = sbc_unpack_frame(input, &priv->frame, input_len);
 978
 979         if (!priv->init) {
 980                 sbc_decoder_init(&priv->dec_state, &priv->frame);
 981                 priv->init = 1;
 982
 983                 sbc->frequency = priv->frame.frequency;
 984                 sbc->mode = priv->frame.mode;
 985                 sbc->subbands = priv->frame.subband_mode;
 986                 sbc->blocks = priv->frame.block_mode;
 987                 sbc->allocation = priv->frame.allocation;
 988                 sbc->bitpool = priv->frame.bitpool;
 989
 990                 priv->frame.codesize = sbc_get_codesize(sbc);
 991                 priv->frame.length = framelen;
 992         } else if (priv->frame.bitpool != sbc->bitpool) {
 993                 priv->frame.length = framelen;
 994                 sbc->bitpool = priv->frame.bitpool;
 995         }
 996
 997         if (!output)
 998                 return framelen;
 999
1000         if (written)
1001                 *written = 0;
1002
1003         if (framelen <= 0)
1004                 return framelen;
1005
1006         samples = sbc_synthesize_audio(&priv->dec_state, &priv->frame);
1007
1008         ptr = output;
1009
1010         if (output_len < (size_t) (samples * priv->frame.channels * 2))
1011                 samples = output_len / (priv->frame.channels * 2);
1012
1013         for (i = 0; i < samples; i++) {
1014                 for (ch = 0; ch < priv->frame.channels; ch++) {
1015                         int16_t s;
1016                         s = priv->frame.pcm_sample[ch][i];
1017
1018                         if (sbc->endian == SBC_BE) {
1019                                 *ptr++ = (s & 0xff00) >> 8;
1020                                 *ptr++ = (s & 0x00ff);
1021                         } else {
1022                                 *ptr++ = (s & 0x00ff);
1023                                 *ptr++ = (s & 0xff00) >> 8;
1024                         }
1025                 }
1026         }
1027
1028         if (written)
1029                 *written = samples * priv->frame.channels * 2;
1030
1031         return framelen;
1032 }
1033
1034 SBC_EXPORT ssize_t sbc_encode(sbc_t *sbc, const void *input, size_t input_len,
1035                         void *output, size_t output_len, ssize_t *written)
1036 {
1037         struct sbc_priv *priv;
1038         int samples;
1039         ssize_t framelen;
1040         int (*sbc_enc_process_input)(int position,
1041                         const uint8_t *pcm, int16_t X[2][SBC_X_BUFFER_SIZE],
1042                         int nsamples, int nchannels);
1043
1044         if (!sbc || !input)
1045                 return -EIO;
1046
1047         priv = sbc->priv;
1048
1049         if (written)
1050                 *written = 0;
1051
1052         if (!priv->init) {
1053                 priv->frame.frequency = sbc->frequency;
1054                 priv->frame.mode = sbc->mode;
1055                 priv->frame.channels = sbc->mode == SBC_MODE_MONO ? 1 : 2;
1056                 priv->frame.allocation = sbc->allocation;
1057                 priv->frame.subband_mode = sbc->subbands;
1058                 priv->frame.subbands = sbc->subbands ? 8 : 4;
1059                 priv->frame.block_mode = sbc->blocks;
1060                 priv->frame.blocks = 4 + (sbc->blocks * 4);
1061                 priv->frame.bitpool = sbc->bitpool;
1062                 priv->frame.codesize = sbc_get_codesize(sbc);
1063                 priv->frame.length = sbc_get_frame_length(sbc);
1064
1065                 sbc_encoder_init(&priv->enc_state, &priv->frame);
1066                 priv->init = 1;
1067         } else if (priv->frame.bitpool != sbc->bitpool) {
1068                 priv->frame.length = sbc_get_frame_length(sbc);
1069                 priv->frame.bitpool = sbc->bitpool;
1070         }
1071
1072         /* input must be large enough to encode a complete frame */
1073         if (input_len < priv->frame.codesize)
1074                 return 0;
1075
1076         /* output must be large enough to receive the encoded frame */
1077         if (!output || output_len < priv->frame.length)
1078                 return -ENOSPC;
1079
1080         /* Select the needed input data processing function and call it */
1081         if (priv->frame.subbands == 8) {
1082                 if (sbc->endian == SBC_BE)
1083                         sbc_enc_process_input =
1084                                 priv->enc_state.sbc_enc_process_input_8s_be;
1085                 else
1086                         sbc_enc_process_input =
1087                                 priv->enc_state.sbc_enc_process_input_8s_le;
1088         } else {
1089                 if (sbc->endian == SBC_BE)
1090                         sbc_enc_process_input =
1091                                 priv->enc_state.sbc_enc_process_input_4s_be;
1092                 else
1093                         sbc_enc_process_input =
1094                                 priv->enc_state.sbc_enc_process_input_4s_le;
1095         }
1096
1097         priv->enc_state.position = sbc_enc_process_input(
1098                 priv->enc_state.position, (const uint8_t *) input,
1099                 priv->enc_state.X, priv->frame.subbands * priv->frame.blocks,
1100                 priv->frame.channels);
1101
1102         samples = sbc_analyze_audio(&priv->enc_state, &priv->frame);
1103
1104         if (priv->frame.mode == JOINT_STEREO) {
1105                 int j = priv->enc_state.sbc_calc_scalefactors_j(
1106                         priv->frame.sb_sample_f, priv->frame.scale_factor,
1107                         priv->frame.blocks, priv->frame.subbands);
1108                 framelen = sbc_pack_frame(output, &priv->frame, output_len, j);
1109         } else {
1110                 priv->enc_state.sbc_calc_scalefactors(
1111                         priv->frame.sb_sample_f, priv->frame.scale_factor,
1112                         priv->frame.blocks, priv->frame.channels,
1113                         priv->frame.subbands);
1114                 framelen = sbc_pack_frame(output, &priv->frame, output_len, 0);
1115         }
1116
1117         if (written)
1118                 *written = framelen;
1119
1120         return samples * priv->frame.channels * 2;
1121 }
1122
1123 SBC_EXPORT void sbc_finish(sbc_t *sbc)
1124 {
1125         if (!sbc)
1126                 return;
1127
1128         free(sbc->priv_alloc_base);
1129
1130         memset(sbc, 0, sizeof(sbc_t));
1131 }
1132
1133 SBC_EXPORT size_t sbc_get_frame_length(sbc_t *sbc)
1134 {
1135         int ret;
1136         uint8_t subbands, channels, blocks, joint, bitpool;
1137         struct sbc_priv *priv;
1138
1139         priv = sbc->priv;
1140         if (priv->init && priv->frame.bitpool == sbc->bitpool)
1141                 return priv->frame.length;
1142
1143         subbands = sbc->subbands ? 8 : 4;
1144         blocks = 4 + (sbc->blocks * 4);
1145         channels = sbc->mode == SBC_MODE_MONO ? 1 : 2;
1146         joint = sbc->mode == SBC_MODE_JOINT_STEREO ? 1 : 0;
1147         bitpool = sbc->bitpool;
1148
1149         ret = 4 + (4 * subbands * channels) / 8;
1150         /* This term is not always evenly divide so we round it up */
1151         if (channels == 1)
1152                 ret += ((blocks * channels * bitpool) + 7) / 8;
1153         else
1154                 ret += (((joint ? subbands : 0) + blocks * bitpool) + 7) / 8;
1155
1156         return ret;
1157 }
1158
1159 SBC_EXPORT unsigned sbc_get_frame_duration(sbc_t *sbc)
1160 {
1161         uint8_t subbands, blocks;
1162         uint16_t frequency;
1163         struct sbc_priv *priv;
1164
1165         priv = sbc->priv;
1166         if (!priv->init) {
1167                 subbands = sbc->subbands ? 8 : 4;
1168                 blocks = 4 + (sbc->blocks * 4);
1169         } else {
1170                 subbands = priv->frame.subbands;
1171                 blocks = priv->frame.blocks;
1172         }
1173
1174         switch (sbc->frequency) {
1175         case SBC_FREQ_16000:
1176                 frequency = 16000;
1177                 break;
1178
1179         case SBC_FREQ_32000:
1180                 frequency = 32000;
1181                 break;
1182
1183         case SBC_FREQ_44100:
1184                 frequency = 44100;
1185                 break;
1186
1187         case SBC_FREQ_48000:
1188                 frequency = 48000;
1189                 break;
1190         default:
1191                 return 0;
1192         }
1193
1194         return (1000000 * blocks * subbands) / frequency;
1195 }
1196
1197 SBC_EXPORT size_t sbc_get_codesize(sbc_t *sbc)
1198 {
1199         uint16_t subbands, channels, blocks;
1200         struct sbc_priv *priv;
1201
1202         priv = sbc->priv;
1203         if (!priv->init) {
1204                 subbands = sbc->subbands ? 8 : 4;
1205                 blocks = 4 + (sbc->blocks * 4);
1206                 channels = sbc->mode == SBC_MODE_MONO ? 1 : 2;
1207         } else {
1208                 subbands = priv->frame.subbands;
1209                 blocks = priv->frame.blocks;
1210                 channels = priv->frame.channels;
1211         }
1212
1213         return subbands * blocks * channels * 2;
1214 }
1215
1216 SBC_EXPORT const char *sbc_get_implementation_info(sbc_t *sbc)
1217 {
1218         struct sbc_priv *priv;
1219
1220         if (!sbc)
1221                 return NULL;
1222
1223         priv = sbc->priv;
1224         if (!priv)
1225                 return NULL;
1226
1227         return priv->enc_state.implementation_info;
1228 }
1229
1230 SBC_EXPORT int sbc_reinit(sbc_t *sbc, unsigned long flags)
1231 {
1232         struct sbc_priv *priv;
1233
1234         if (!sbc || !sbc->priv)
1235                 return -EIO;
1236
1237         priv = sbc->priv;
1238
1239         if (priv->init == 1)
1240                 memset(sbc->priv, 0, sizeof(struct sbc_priv));
1241
1242         sbc_set_defaults(sbc, flags);
1243
1244         return 0;
1245 }