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