sbc/sbc.c

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