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