sbc: Add low-complexity, subband codec support

8 files changed, 2970 insertions, 0 deletions

diff --git a/sbc/sbc.c b/sbc/sbc.c
new file mode 100644
index 0000000..6303421
--- /dev/null
+++ b/sbc/sbc.c
@@ -0,0 +1,1411 @@
+/*
+ *
+ *  Bluetooth low-complexity, subband codec (SBC) library
+ *
+ *  Copyright (C) 2004-2008  Marcel Holtmann <marcel@holtmann.org>
+ *  Copyright (C) 2004-2005  Henryk Ploetz <henryk@ploetzli.ch>
+ *  Copyright (C) 2005-2008  Brad Midgley <bmidgley@xmission.com>
+ *
+ *
+ *  This library is free software; you can redistribute it and/or
+ *  modify it under the terms of the GNU Lesser General Public
+ *  License as published by the Free Software Foundation; either
+ *  version 2.1 of the License, or (at your option) any later version.
+ *
+ *  This library is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ *  Lesser General Public License for more details.
+ *
+ *  You should have received a copy of the GNU Lesser General Public
+ *  License along with this library; if not, write to the Free Software
+ *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+/* todo items:
+
+  use a log2 table for byte integer scale factors calculation (sum log2 results
+  for high and low bytes) fill bitpool by 16 bits instead of one at a time in
+  bits allocation/bitpool generation port to the dsp
+
+*/
+
+#ifdef HAVE_CONFIG_H
+#include <config.h>
+#endif
+
+#include <stdio.h>
+#include <errno.h>
+#include <string.h>
+#include <stdlib.h>
+#include <sys/types.h>
+
+#include "sbc_math.h"
+#include "sbc_tables.h"
+
+#include "sbc.h"
+
+#define SBC_SYNCWORD	0x9C
+
+/* This structure contains an unpacked SBC frame.
+   Yes, there is probably quite some unused space herein */
+struct sbc_frame {
+	uint8_t frequency;
+	uint8_t block_mode;
+	uint8_t blocks;
+	enum {
+		MONO		= SBC_MODE_MONO,
+		DUAL_CHANNEL	= SBC_MODE_DUAL_CHANNEL,
+		STEREO		= SBC_MODE_STEREO,
+		JOINT_STEREO	= SBC_MODE_JOINT_STEREO
+	} mode;
+	uint8_t channels;
+	enum {
+		LOUDNESS	= SBC_AM_LOUDNESS,
+		SNR		= SBC_AM_SNR
+	} allocation;
+	uint8_t subband_mode;
+	uint8_t subbands;
+	uint8_t bitpool;
+	uint8_t codesize;
+	uint8_t length;
+
+	/* bit number x set means joint stereo has been used in subband x */
+	uint8_t joint;
+
+	/* only the lower 4 bits of every element are to be used */
+	uint8_t scale_factor[2][8];
+
+	/* raw integer subband samples in the frame */
+
+	int32_t sb_sample_f[16][2][8];
+	int32_t sb_sample[16][2][8];	/* modified subband samples */
+	int16_t pcm_sample[2][16*8];	/* original pcm audio samples */
+};
+
+struct sbc_decoder_state {
+	int subbands;
+	int32_t V[2][170];
+	int offset[2][16];
+};
+
+struct sbc_encoder_state {
+	int subbands;
+	int position[2];
+	int32_t X[2][160];
+};
+
+/*
+ * Calculates the CRC-8 of the first len bits in data
+ */
+static const uint8_t crc_table[256] = {
+	0x00, 0x1D, 0x3A, 0x27, 0x74, 0x69, 0x4E, 0x53,
+	0xE8, 0xF5, 0xD2, 0xCF, 0x9C, 0x81, 0xA6, 0xBB,
+	0xCD, 0xD0, 0xF7, 0xEA, 0xB9, 0xA4, 0x83, 0x9E,
+	0x25, 0x38, 0x1F, 0x02, 0x51, 0x4C, 0x6B, 0x76,
+	0x87, 0x9A, 0xBD, 0xA0, 0xF3, 0xEE, 0xC9, 0xD4,
+	0x6F, 0x72, 0x55, 0x48, 0x1B, 0x06, 0x21, 0x3C,
+	0x4A, 0x57, 0x70, 0x6D, 0x3E, 0x23, 0x04, 0x19,
+	0xA2, 0xBF, 0x98, 0x85, 0xD6, 0xCB, 0xEC, 0xF1,
+	0x13, 0x0E, 0x29, 0x34, 0x67, 0x7A, 0x5D, 0x40,
+	0xFB, 0xE6, 0xC1, 0xDC, 0x8F, 0x92, 0xB5, 0xA8,
+	0xDE, 0xC3, 0xE4, 0xF9, 0xAA, 0xB7, 0x90, 0x8D,
+	0x36, 0x2B, 0x0C, 0x11, 0x42, 0x5F, 0x78, 0x65,
+	0x94, 0x89, 0xAE, 0xB3, 0xE0, 0xFD, 0xDA, 0xC7,
+	0x7C, 0x61, 0x46, 0x5B, 0x08, 0x15, 0x32, 0x2F,
+	0x59, 0x44, 0x63, 0x7E, 0x2D, 0x30, 0x17, 0x0A,
+	0xB1, 0xAC, 0x8B, 0x96, 0xC5, 0xD8, 0xFF, 0xE2,
+	0x26, 0x3B, 0x1C, 0x01, 0x52, 0x4F, 0x68, 0x75,
+	0xCE, 0xD3, 0xF4, 0xE9, 0xBA, 0xA7, 0x80, 0x9D,
+	0xEB, 0xF6, 0xD1, 0xCC, 0x9F, 0x82, 0xA5, 0xB8,
+	0x03, 0x1E, 0x39, 0x24, 0x77, 0x6A, 0x4D, 0x50,
+	0xA1, 0xBC, 0x9B, 0x86, 0xD5, 0xC8, 0xEF, 0xF2,
+	0x49, 0x54, 0x73, 0x6E, 0x3D, 0x20, 0x07, 0x1A,
+	0x6C, 0x71, 0x56, 0x4B, 0x18, 0x05, 0x22, 0x3F,
+	0x84, 0x99, 0xBE, 0xA3, 0xF0, 0xED, 0xCA, 0xD7,
+	0x35, 0x28, 0x0F, 0x12, 0x41, 0x5C, 0x7B, 0x66,
+	0xDD, 0xC0, 0xE7, 0xFA, 0xA9, 0xB4, 0x93, 0x8E,
+	0xF8, 0xE5, 0xC2, 0xDF, 0x8C, 0x91, 0xB6, 0xAB,
+	0x10, 0x0D, 0x2A, 0x37, 0x64, 0x79, 0x5E, 0x43,
+	0xB2, 0xAF, 0x88, 0x95, 0xC6, 0xDB, 0xFC, 0xE1,
+	0x5A, 0x47, 0x60, 0x7D, 0x2E, 0x33, 0x14, 0x09,
+	0x7F, 0x62, 0x45, 0x58, 0x0B, 0x16, 0x31, 0x2C,
+	0x97, 0x8A, 0xAD, 0xB0, 0xE3, 0xFE, 0xD9, 0xC4
+};
+
+static uint8_t sbc_crc8(const uint8_t *data, size_t len)
+{
+	uint8_t crc = 0x0f;
+	size_t i;
+	uint8_t octet;
+
+	for (i = 0; i < len / 8; i++)
+		crc = crc_table[crc ^ data[i]];
+
+	octet = data[i];
+	for (i = 0; i < len % 8; i++) {
+		char bit = ((octet ^ crc) & 0x80) >> 7;
+
+		crc = ((crc & 0x7f) << 1) ^ (bit ? 0x1d : 0);
+
+		octet = octet << 1;
+	}
+
+	return crc;
+}
+
+/*
+ * Code straight from the spec to calculate the bits array
+ * Takes a pointer to the frame in question, a pointer to the bits array and
+ * the sampling frequency (as 2 bit integer)
+ */
+static void sbc_calculate_bits(const struct sbc_frame *frame, int (*bits)[8])
+{
+	uint8_t sf = frame->frequency;
+
+	if (frame->mode == MONO || frame->mode == DUAL_CHANNEL) {
+		int bitneed[2][8], loudness, max_bitneed, bitcount, slicecount, bitslice;
+		int ch, sb;
+
+		for (ch = 0; ch < frame->channels; ch++) {
+			max_bitneed = 0;
+			if (frame->allocation == SNR) {
+				for (sb = 0; sb < frame->subbands; sb++) {
+					bitneed[ch][sb] = frame->scale_factor[ch][sb];
+					if (bitneed[ch][sb] > max_bitneed)
+						max_bitneed = bitneed[ch][sb];
+				}
+			} else {
+				for (sb = 0; sb < frame->subbands; sb++) {
+					if (frame->scale_factor[ch][sb] == 0)
+						bitneed[ch][sb] = -5;
+					else {
+						if (frame->subbands == 4)
+							loudness = frame->scale_factor[ch][sb] - sbc_offset4[sf][sb];
+						else
+							loudness = frame->scale_factor[ch][sb] - sbc_offset8[sf][sb];
+						if (loudness > 0)
+							bitneed[ch][sb] = loudness / 2;
+						else
+							bitneed[ch][sb] = loudness;
+					}
+					if (bitneed[ch][sb] > max_bitneed)
+						max_bitneed = bitneed[ch][sb];
+				}
+			}
+
+			bitcount = 0;
+			slicecount = 0;
+			bitslice = max_bitneed + 1;
+			do {
+				bitslice--;
+				bitcount += slicecount;
+				slicecount = 0;
+				for (sb = 0; sb < frame->subbands; sb++) {
+					if ((bitneed[ch][sb] > bitslice + 1) && (bitneed[ch][sb] < bitslice + 16))
+						slicecount++;
+					else if (bitneed[ch][sb] == bitslice + 1)
+						slicecount += 2;
+				}
+			} while (bitcount + slicecount < frame->bitpool);
+
+			if (bitcount + slicecount == frame->bitpool) {
+				bitcount += slicecount;
+				bitslice--;
+			}
+
+			for (sb = 0; sb < frame->subbands; sb++) {
+				if (bitneed[ch][sb] < bitslice + 2)
+					bits[ch][sb] = 0;
+				else {
+					bits[ch][sb] = bitneed[ch][sb] - bitslice;
+					if (bits[ch][sb] > 16)
+						bits[ch][sb] = 16;
+				}
+			}
+
+			for (sb = 0; bitcount < frame->bitpool && sb < frame->subbands; sb++) {
+				if ((bits[ch][sb] >= 2) && (bits[ch][sb] < 16)) {
+					bits[ch][sb]++;
+					bitcount++;
+				} else if ((bitneed[ch][sb] == bitslice + 1) && (frame->bitpool > bitcount + 1)) {
+					bits[ch][sb] = 2;
+					bitcount += 2;
+				}
+			}
+
+			for (sb = 0; bitcount < frame->bitpool && sb < frame->subbands; sb++) {
+				if (bits[ch][sb] < 16) {
+					bits[ch][sb]++;
+					bitcount++;
+				}
+			}
+
+		}
+
+	} else if (frame->mode == STEREO || frame->mode == JOINT_STEREO) {
+		int bitneed[2][8], loudness, max_bitneed, bitcount, slicecount, bitslice;
+		int ch, sb;
+
+		max_bitneed = 0;
+		if (frame->allocation == SNR) {
+			for (ch = 0; ch < 2; ch++) {
+				for (sb = 0; sb < frame->subbands; sb++) {
+					bitneed[ch][sb] = frame->scale_factor[ch][sb];
+					if (bitneed[ch][sb] > max_bitneed)
+						max_bitneed = bitneed[ch][sb];
+				}
+			}
+		} else {
+			for (ch = 0; ch < 2; ch++) {
+				for (sb = 0; sb < frame->subbands; sb++) {
+					if (frame->scale_factor[ch][sb] == 0)
+						bitneed[ch][sb] = -5;
+					else {
+						if (frame->subbands == 4)
+							loudness = frame->scale_factor[ch][sb] - sbc_offset4[sf][sb];
+						else
+							loudness = frame->scale_factor[ch][sb] - sbc_offset8[sf][sb];
+						if (loudness > 0)
+							bitneed[ch][sb] = loudness / 2;
+						else
+							bitneed[ch][sb] = loudness;
+					}
+					if (bitneed[ch][sb] > max_bitneed)
+						max_bitneed = bitneed[ch][sb];
+				}
+			}
+		}
+
+		bitcount = 0;
+		slicecount = 0;
+		bitslice = max_bitneed + 1;
+		do {
+			bitslice--;
+			bitcount += slicecount;
+			slicecount = 0;
+			for (ch = 0; ch < 2; ch++) {
+				for (sb = 0; sb < frame->subbands; sb++) {
+					if ((bitneed[ch][sb] > bitslice + 1) && (bitneed[ch][sb] < bitslice + 16))
+						slicecount++;
+					else if (bitneed[ch][sb] == bitslice + 1)
+						slicecount += 2;
+				}
+			}
+		} while (bitcount + slicecount < frame->bitpool);
+
+		if (bitcount + slicecount == frame->bitpool) {
+			bitcount += slicecount;
+			bitslice--;
+		}
+
+		for (ch = 0; ch < 2; ch++) {
+			for (sb = 0; sb < frame->subbands; sb++) {
+				if (bitneed[ch][sb] < bitslice + 2) {
+					bits[ch][sb] = 0;
+				} else {
+					bits[ch][sb] = bitneed[ch][sb] - bitslice;
+					if (bits[ch][sb] > 16)
+						bits[ch][sb] = 16;
+				}
+			}
+		}
+
+		ch = 0;
+		sb = 0;
+		while (bitcount < frame->bitpool) {
+			if ((bits[ch][sb] >= 2) && (bits[ch][sb] < 16)) {
+				bits[ch][sb]++;
+				bitcount++;
+			} else if ((bitneed[ch][sb] == bitslice + 1) && (frame->bitpool > bitcount + 1)) {
+				bits[ch][sb] = 2;
+				bitcount += 2;
+			}
+			if (ch == 1) {
+				ch = 0;
+				sb++;
+				if (sb >= frame->subbands) break;
+			} else
+				ch = 1;
+		}
+
+		ch = 0;
+		sb = 0;
+		while (bitcount < frame->bitpool) {
+			if (bits[ch][sb] < 16) {
+				bits[ch][sb]++;
+				bitcount++;
+			}
+			if (ch == 1) {
+				ch = 0;
+				sb++;
+				if (sb >= frame->subbands) break;
+			} else
+				ch = 1;
+		}
+
+	}
+
+}
+
+/*
+ * Unpacks a SBC frame at the beginning of the stream in data,
+ * which has at most len bytes into frame.
+ * Returns the length in bytes of the packed frame, or a negative
+ * value on error. The error codes are:
+ *
+ *  -1   Data stream too short
+ *  -2   Sync byte incorrect
+ *  -3   CRC8 incorrect
+ *  -4   Bitpool value out of bounds
+ */
+static int sbc_unpack_frame(const uint8_t *data, struct sbc_frame *frame,
+				size_t len)
+{
+	int consumed;
+	/* Will copy the parts of the header that are relevant to crc
+	 * calculation here */
+	uint8_t crc_header[11] = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 };
+	int crc_pos = 0;
+	int32_t temp;
+
+	int audio_sample;
+	int ch, sb, blk, bit;	/* channel, subband, block and bit standard
+				   counters */
+	int bits[2][8];		/* bits distribution */
+	uint32_t levels[2][8];	/* levels derived from that */
+
+	if (len < 4)
+		return -1;
+
+	if (data[0] != SBC_SYNCWORD)
+		return -2;
+
+	frame->frequency = (data[1] >> 6) & 0x03;
+
+	frame->block_mode = (data[1] >> 4) & 0x03;
+	switch (frame->block_mode) {
+	case SBC_BLK_4:
+		frame->blocks = 4;
+		break;
+	case SBC_BLK_8:
+		frame->blocks = 8;
+		break;
+	case SBC_BLK_12:
+		frame->blocks = 12;
+		break;
+	case SBC_BLK_16:
+		frame->blocks = 16;
+		break;
+	}
+
+	frame->mode = (data[1] >> 2) & 0x03;
+	switch (frame->mode) {
+	case MONO:
+		frame->channels = 1;
+		break;
+	case DUAL_CHANNEL:	/* fall-through */
+	case STEREO:
+	case JOINT_STEREO:
+		frame->channels = 2;
+		break;
+	}
+
+	frame->allocation = (data[1] >> 1) & 0x01;
+
+	frame->subband_mode = (data[1] & 0x01);
+	frame->subbands = frame->subband_mode ? 8 : 4;
+
+	frame->bitpool = data[2];
+
+	if ((frame->mode == MONO || frame->mode == DUAL_CHANNEL) &&
+			frame->bitpool > 16 * frame->subbands)
+		return -4;
+
+	if ((frame->mode == STEREO || frame->mode == JOINT_STEREO) &&
+			frame->bitpool > 32 * frame->subbands)
+		return -4;
+
+	/* data[3] is crc, we're checking it later */
+
+	consumed = 32;
+
+	crc_header[0] = data[1];
+	crc_header[1] = data[2];
+	crc_pos = 16;
+
+	if (frame->mode == JOINT_STEREO) {
+		if (len * 8 < consumed + frame->subbands)
+			return -1;
+
+		frame->joint = 0x00;
+		for (sb = 0; sb < frame->subbands - 1; sb++)
+			frame->joint |= ((data[4] >> (7 - sb)) & 0x01) << sb;
+		if (frame->subbands == 4)
+			crc_header[crc_pos / 8] = data[4] & 0xf0;
+		else
+			crc_header[crc_pos / 8] = data[4];
+
+		consumed += frame->subbands;
+		crc_pos += frame->subbands;
+	}
+
+	if (len * 8 < consumed + (4 * frame->subbands * frame->channels))
+		return -1;
+
+	for (ch = 0; ch < frame->channels; ch++) {
+		for (sb = 0; sb < frame->subbands; sb++) {
+			/* FIXME assert(consumed % 4 == 0); */
+			frame->scale_factor[ch][sb] =
+				(data[consumed >> 3] >> (4 - (consumed & 0x7))) & 0x0F;
+			crc_header[crc_pos >> 3] |=
+				frame->scale_factor[ch][sb] << (4 - (crc_pos & 0x7));
+
+			consumed += 4;
+			crc_pos += 4;
+		}
+	}
+
+	if (data[3] != sbc_crc8(crc_header, crc_pos))
+		return -3;
+
+	sbc_calculate_bits(frame, bits);
+
+	for (ch = 0; ch < frame->channels; ch++) {
+		for (sb = 0; sb < frame->subbands; sb++)
+			levels[ch][sb] = (1 << bits[ch][sb]) - 1;
+	}
+
+	for (blk = 0; blk < frame->blocks; blk++) {
+		for (ch = 0; ch < frame->channels; ch++) {
+			for (sb = 0; sb < frame->subbands; sb++) {
+				if (levels[ch][sb] > 0) {
+					audio_sample = 0;
+					for (bit = 0; bit < bits[ch][sb]; bit++) {
+						if (consumed > len * 8)
+							return -1;
+
+						if ((data[consumed >> 3] >> (7 - (consumed & 0x7))) & 0x01)
+							audio_sample |= 1 << (bits[ch][sb] - bit - 1);
+
+						consumed++;
+					}
+
+					frame->sb_sample[blk][ch][sb] =
+						(((audio_sample << 1) | 1) << frame->scale_factor[ch][sb]) /
+						levels[ch][sb] - (1 << frame->scale_factor[ch][sb]);
+				} else
+					frame->sb_sample[blk][ch][sb] = 0;
+			}
+		}
+	}
+
+	if (frame->mode == JOINT_STEREO) {
+		for (blk = 0; blk < frame->blocks; blk++) {
+			for (sb = 0; sb < frame->subbands; sb++) {
+				if (frame->joint & (0x01 << sb)) {
+					temp = frame->sb_sample[blk][0][sb] +
+						frame->sb_sample[blk][1][sb];
+					frame->sb_sample[blk][1][sb] =
+						frame->sb_sample[blk][0][sb] -
+						frame->sb_sample[blk][1][sb];
+					frame->sb_sample[blk][0][sb] = temp;
+				}
+			}
+		}
+	}
+
+	if ((consumed & 0x7) != 0)
+		consumed += 8 - (consumed & 0x7);
+
+	return consumed >> 3;
+}
+
+static void sbc_decoder_init(struct sbc_decoder_state *state,
+				const struct sbc_frame *frame)
+{
+	int i, ch;
+
+	memset(state->V, 0, sizeof(state->V));
+	state->subbands = frame->subbands;
+
+	for (ch = 0; ch < 2; ch++)
+		for (i = 0; i < frame->subbands * 2; i++)
+			state->offset[ch][i] = (10 * i + 10);
+}
+
+static inline void sbc_synthesize_four(struct sbc_decoder_state *state,
+				struct sbc_frame *frame, int ch, int blk)
+{
+	int i, k, idx;
+	int32_t *v = state->V[ch];
+	int *offset = state->offset[ch];
+
+	for (i = 0; i < 8; i++) {
+		/* Shifting */
+		offset[i]--;
+		if (offset[i] < 0) {
+			offset[i] = 79;
+			memcpy(v + 80, v, 9 * sizeof(*v));
+		}
+
+		/* Distribute the new matrix value to the shifted position */
+		v[offset[i]] = SCALE4_STAGED1(
+			MULA(synmatrix4[i][0], frame->sb_sample[blk][ch][0],
+			MULA(synmatrix4[i][1], frame->sb_sample[blk][ch][1],
+			MULA(synmatrix4[i][2], frame->sb_sample[blk][ch][2],
+			MUL (synmatrix4[i][3], frame->sb_sample[blk][ch][3])))));
+	}
+
+	/* Compute the samples */
+	for (idx = 0, i = 0; i < 4; i++, idx += 5) {
+		k = (i + 4) & 0xf;
+
+		/* Store in output, Q0 */
+		frame->pcm_sample[ch][blk * 4 + i] = SCALE4_STAGED2(
+			MULA(v[offset[i] + 0], sbc_proto_4_40m0[idx + 0],
+			MULA(v[offset[k] + 1], sbc_proto_4_40m1[idx + 0],
+			MULA(v[offset[i] + 2], sbc_proto_4_40m0[idx + 1],
+			MULA(v[offset[k] + 3], sbc_proto_4_40m1[idx + 1],
+			MULA(v[offset[i] + 4], sbc_proto_4_40m0[idx + 2],
+			MULA(v[offset[k] + 5], sbc_proto_4_40m1[idx + 2],
+			MULA(v[offset[i] + 6], sbc_proto_4_40m0[idx + 3],
+			MULA(v[offset[k] + 7], sbc_proto_4_40m1[idx + 3],
+			MULA(v[offset[i] + 8], sbc_proto_4_40m0[idx + 4],
+			MUL( v[offset[k] + 9], sbc_proto_4_40m1[idx + 4])))))))))));
+	}
+}
+
+static inline void sbc_synthesize_eight(struct sbc_decoder_state *state,
+				struct sbc_frame *frame, int ch, int blk)
+{
+	int i, j, k, idx;
+	int *offset = state->offset[ch];
+
+	for (i = 0; i < 16; i++) {
+		/* Shifting */
+		offset[i]--;
+		if (offset[i] < 0) {
+			offset[i] = 159;
+			for (j = 0; j < 9; j++)
+				state->V[ch][j + 160] = state->V[ch][j];
+		}
+
+		/* Distribute the new matrix value to the shifted position */
+		state->V[ch][offset[i]] = SCALE8_STAGED1(
+			MULA(synmatrix8[i][0], frame->sb_sample[blk][ch][0],
+			MULA(synmatrix8[i][1], frame->sb_sample[blk][ch][1],
+			MULA(synmatrix8[i][2], frame->sb_sample[blk][ch][2],
+			MULA(synmatrix8[i][3], frame->sb_sample[blk][ch][3],
+			MULA(synmatrix8[i][4], frame->sb_sample[blk][ch][4],
+			MULA(synmatrix8[i][5], frame->sb_sample[blk][ch][5],
+			MULA(synmatrix8[i][6], frame->sb_sample[blk][ch][6],
+			MUL( synmatrix8[i][7], frame->sb_sample[blk][ch][7])))))))));
+	}
+
+	/* Compute the samples */
+	for (idx = 0, i = 0; i < 8; i++, idx += 5) {
+		k = (i + 8) & 0xf;
+
+		/* Store in output */
+		frame->pcm_sample[ch][blk * 8 + i] = SCALE8_STAGED2( // Q0
+			MULA(state->V[ch][offset[i] + 0], sbc_proto_8_80m0[idx + 0],
+			MULA(state->V[ch][offset[k] + 1], sbc_proto_8_80m1[idx + 0],
+			MULA(state->V[ch][offset[i] + 2], sbc_proto_8_80m0[idx + 1],
+			MULA(state->V[ch][offset[k] + 3], sbc_proto_8_80m1[idx + 1],
+			MULA(state->V[ch][offset[i] + 4], sbc_proto_8_80m0[idx + 2],
+			MULA(state->V[ch][offset[k] + 5], sbc_proto_8_80m1[idx + 2],
+			MULA(state->V[ch][offset[i] + 6], sbc_proto_8_80m0[idx + 3],
+			MULA(state->V[ch][offset[k] + 7], sbc_proto_8_80m1[idx + 3],
+			MULA(state->V[ch][offset[i] + 8], sbc_proto_8_80m0[idx + 4],
+			MUL( state->V[ch][offset[k] + 9], sbc_proto_8_80m1[idx + 4])))))))))));
+	}
+}
+
+static int sbc_synthesize_audio(struct sbc_decoder_state *state,
+				struct sbc_frame *frame)
+{
+	int ch, blk;
+
+	switch (frame->subbands) {
+	case 4:
+		for (ch = 0; ch < frame->channels; ch++) {
+			for (blk = 0; blk < frame->blocks; blk++)
+				sbc_synthesize_four(state, frame, ch, blk);
+		}
+		return frame->blocks * 4;
+
+	case 8:
+		for (ch = 0; ch < frame->channels; ch++) {
+			for (blk = 0; blk < frame->blocks; blk++)
+				sbc_synthesize_eight(state, frame, ch, blk);
+		}
+		return frame->blocks * 8;
+
+	default:
+		return -EIO;
+	}
+}
+
+static void sbc_encoder_init(struct sbc_encoder_state *state,
+				const struct sbc_frame *frame)
+{
+	memset(&state->X, 0, sizeof(state->X));
+	state->subbands = frame->subbands;
+	state->position[0] = state->position[1] = 9 * frame->subbands;
+}
+
+static inline void _sbc_analyze_four(const int32_t *in, int32_t *out)
+{
+	sbc_fixed_t t[8], s[5];
+
+	t[0] = SCALE4_STAGE1( /* Q8 */
+		MULA(_sbc_proto_4[0], in[8] - in[32], /* Q18 */
+		MUL( _sbc_proto_4[1], in[16] - in[24])));
+
+	t[1] = SCALE4_STAGE1(
+		MULA(_sbc_proto_4[2], in[1],
+		MULA(_sbc_proto_4[3], in[9],
+		MULA(_sbc_proto_4[4], in[17],
+		MULA(_sbc_proto_4[5], in[25],
+		MUL( _sbc_proto_4[6], in[33]))))));
+
+	t[2] = SCALE4_STAGE1(
+		MULA(_sbc_proto_4[7], in[2],
+		MULA(_sbc_proto_4[8], in[10],
+		MULA(_sbc_proto_4[9], in[18],
+		MULA(_sbc_proto_4[10], in[26],
+		MUL( _sbc_proto_4[11], in[34]))))));
+
+	t[3] = SCALE4_STAGE1(
+		MULA(_sbc_proto_4[12], in[3],
+		MULA(_sbc_proto_4[13], in[11],
+		MULA(_sbc_proto_4[14], in[19],
+		MULA(_sbc_proto_4[15], in[27],
+		MUL( _sbc_proto_4[16], in[35]))))));
+
+	t[4] = SCALE4_STAGE1(
+		MULA(_sbc_proto_4[17], in[4] + in[36],
+		MULA(_sbc_proto_4[18], in[12] + in[28],
+		MUL( _sbc_proto_4[19], in[20]))));
+
+	t[5] = SCALE4_STAGE1(
+		MULA(_sbc_proto_4[16], in[5],
+		MULA(_sbc_proto_4[15], in[13],
+		MULA(_sbc_proto_4[14], in[21],
+		MULA(_sbc_proto_4[13], in[29],
+		MUL( _sbc_proto_4[12], in[37]))))));
+
+	/* don't compute t[6]... this term always multiplies
+	 * with cos(pi/2) = 0 */
+
+	t[7] = SCALE4_STAGE1(
+		MULA(_sbc_proto_4[6], in[7],
+		MULA(_sbc_proto_4[5], in[15],
+		MULA(_sbc_proto_4[4], in[23],
+		MULA(_sbc_proto_4[3], in[31],
+		MUL( _sbc_proto_4[2], in[39]))))));
+
+	s[0] = MUL( _anamatrix4[0], t[0] + t[4]);
+	s[1] = MUL( _anamatrix4[2], t[2]);
+	s[2] = MULA(_anamatrix4[1], t[1] + t[3],
+		MUL(_anamatrix4[3], t[5]));
+	s[3] = MULA(_anamatrix4[3], t[1] + t[3],
+		MUL(_anamatrix4[1], -t[5] + t[7]));
+	s[4] = MUL( _anamatrix4[3], t[7]);
+
+	out[0] = SCALE4_STAGE2( s[0] + s[1] + s[2] + s[4]); /* Q0 */
+	out[1] = SCALE4_STAGE2(-s[0] + s[1] + s[3]);
+	out[2] = SCALE4_STAGE2(-s[0] + s[1] - s[3]);
+	out[3] = SCALE4_STAGE2( s[0] + s[1] - s[2] - s[4]);
+}
+
+static inline void sbc_analyze_four(struct sbc_encoder_state *state,
+				struct sbc_frame *frame, int ch, int blk)
+{
+	int32_t *x = &state->X[ch][state->position[ch]];
+	int16_t *pcm = &frame->pcm_sample[ch][blk * 4];
+
+	/* Input 4 Audio Samples */
+	x[40] = x[0] = pcm[3];
+	x[41] = x[1] = pcm[2];
+	x[42] = x[2] = pcm[1];
+	x[43] = x[3] = pcm[0];
+
+	_sbc_analyze_four(x, frame->sb_sample_f[blk][ch]);
+
+	state->position[ch] -= 4;
+	if (state->position[ch] < 0)
+		state->position[ch] = 36;
+}
+
+static inline void _sbc_analyze_eight(const int32_t *in, int32_t *out)
+{
+	sbc_fixed_t t[8], s[8];
+
+	t[0] = SCALE8_STAGE1( /* Q10 */
+		MULA(_sbc_proto_8[0], (in[16] - in[64]), /* Q18 = Q18 * Q0 */
+		MULA(_sbc_proto_8[1], (in[32] - in[48]),
+		MULA(_sbc_proto_8[2], in[4],
+		MULA(_sbc_proto_8[3], in[20],
+		MULA(_sbc_proto_8[4], in[36],
+		MUL( _sbc_proto_8[5], in[52])))))));
+
+	t[1] = SCALE8_STAGE1(
+		MULA(_sbc_proto_8[6], in[2],
+		MULA(_sbc_proto_8[7], in[18],
+		MULA(_sbc_proto_8[8], in[34],
+		MULA(_sbc_proto_8[9], in[50],
+		MUL(_sbc_proto_8[10], in[66]))))));
+
+	t[2] = SCALE8_STAGE1(
+		MULA(_sbc_proto_8[11], in[1],
+		MULA(_sbc_proto_8[12], in[17],
+		MULA(_sbc_proto_8[13], in[33],
+		MULA(_sbc_proto_8[14], in[49],
+		MULA(_sbc_proto_8[15], in[65],
+		MULA(_sbc_proto_8[16], in[3],
+		MULA(_sbc_proto_8[17], in[19],
+		MULA(_sbc_proto_8[18], in[35],
+		MULA(_sbc_proto_8[19], in[51],
+		MUL( _sbc_proto_8[20], in[67])))))))))));
+
+	t[3] = SCALE8_STAGE1(
+		MULA( _sbc_proto_8[21], in[5],
+		MULA( _sbc_proto_8[22], in[21],
+		MULA( _sbc_proto_8[23], in[37],
+		MULA( _sbc_proto_8[24], in[53],
+		MULA( _sbc_proto_8[25], in[69],
+		MULA(-_sbc_proto_8[15], in[15],
+		MULA(-_sbc_proto_8[14], in[31],
+		MULA(-_sbc_proto_8[13], in[47],
+		MULA(-_sbc_proto_8[12], in[63],
+		MUL( -_sbc_proto_8[11], in[79])))))))))));
+
+	t[4] = SCALE8_STAGE1(
+		MULA( _sbc_proto_8[26], in[6],
+		MULA( _sbc_proto_8[27], in[22],
+		MULA( _sbc_proto_8[28], in[38],
+		MULA( _sbc_proto_8[29], in[54],
+		MULA( _sbc_proto_8[30], in[70],
+		MULA(-_sbc_proto_8[10], in[14],
+		MULA(-_sbc_proto_8[9], in[30],
+		MULA(-_sbc_proto_8[8], in[46],
+		MULA(-_sbc_proto_8[7], in[62],
+		MUL( -_sbc_proto_8[6], in[78])))))))))));
+
+	t[5] = SCALE8_STAGE1(
+		MULA( _sbc_proto_8[31], in[7],
+		MULA( _sbc_proto_8[32], in[23],
+		MULA( _sbc_proto_8[33], in[39],
+		MULA( _sbc_proto_8[34], in[55],
+		MULA( _sbc_proto_8[35], in[71],
+		MULA(-_sbc_proto_8[20], in[13],
+		MULA(-_sbc_proto_8[19], in[29],
+		MULA(-_sbc_proto_8[18], in[45],
+		MULA(-_sbc_proto_8[17], in[61],
+		MUL( -_sbc_proto_8[16], in[77])))))))))));
+
+	t[6] = SCALE8_STAGE1(
+		MULA( _sbc_proto_8[36], (in[8] + in[72]),
+		MULA( _sbc_proto_8[37], (in[24] + in[56]),
+		MULA( _sbc_proto_8[38], in[40],
+		MULA(-_sbc_proto_8[39], in[12],
+		MULA(-_sbc_proto_8[5], in[28],
+		MULA(-_sbc_proto_8[4], in[44],
+		MULA(-_sbc_proto_8[3], in[60],
+		MUL( -_sbc_proto_8[2], in[76])))))))));
+
+	t[7] = SCALE8_STAGE1(
+		MULA( _sbc_proto_8[35], in[9],
+		MULA( _sbc_proto_8[34], in[25],
+		MULA( _sbc_proto_8[33], in[41],
+		MULA( _sbc_proto_8[32], in[57],
+		MULA( _sbc_proto_8[31], in[73],
+		MULA(-_sbc_proto_8[25], in[11],
+		MULA(-_sbc_proto_8[24], in[27],
+		MULA(-_sbc_proto_8[23], in[43],
+		MULA(-_sbc_proto_8[22], in[59],
+		MUL( -_sbc_proto_8[21], in[75])))))))))));
+
+	s[0] = MULA(  _anamatrix8[0], t[0],
+		MUL(  _anamatrix8[1], t[6]));
+	s[1] = MUL(   _anamatrix8[7], t[1]);
+	s[2] = MULA(  _anamatrix8[2], t[2],
+		MULA( _anamatrix8[3], t[3],
+		MULA( _anamatrix8[4], t[5],
+		MUL(  _anamatrix8[5], t[7]))));
+	s[3] = MUL(   _anamatrix8[6], t[4]);
+	s[4] = MULA(  _anamatrix8[3], t[2],
+		MULA(-_anamatrix8[5], t[3],
+		MULA(-_anamatrix8[2], t[5],
+		MUL( -_anamatrix8[4], t[7]))));
+	s[5] = MULA(  _anamatrix8[4], t[2],
+		MULA(-_anamatrix8[2], t[3],
+		MULA( _anamatrix8[5], t[5],
+		MUL(  _anamatrix8[3], t[7]))));
+	s[6] = MULA(  _anamatrix8[1], t[0],
+		MUL( -_anamatrix8[0], t[6]));
+	s[7] = MULA(  _anamatrix8[5], t[2],
+		MULA(-_anamatrix8[4], t[3],
+		MULA( _anamatrix8[3], t[5],
+		MUL( -_anamatrix8[2], t[7]))));
+
+	out[0] = SCALE8_STAGE2( s[0] + s[1] + s[2] + s[3]);
+	out[1] = SCALE8_STAGE2( s[1] - s[3] + s[4] + s[6]);
+	out[2] = SCALE8_STAGE2( s[1] - s[3] + s[5] - s[6]);
+	out[3] = SCALE8_STAGE2(-s[0] + s[1] + s[3] + s[7]);
+	out[4] = SCALE8_STAGE2(-s[0] + s[1] + s[3] - s[7]);
+	out[5] = SCALE8_STAGE2( s[1] - s[3] - s[5] - s[6]);
+	out[6] = SCALE8_STAGE2( s[1] - s[3] - s[4] + s[6]);
+	out[7] = SCALE8_STAGE2( s[0] + s[1] - s[2] + s[3]);
+}
+
+static inline void sbc_analyze_eight(struct sbc_encoder_state *state,
+					struct sbc_frame *frame, int ch,
+					int blk)
+{
+	int32_t *x = &state->X[ch][state->position[ch]];
+	int16_t *pcm = &frame->pcm_sample[ch][blk * 8];
+
+	/* Input 8 Audio Samples */
+	x[80] = x[0] = pcm[7];
+	x[81] = x[1] = pcm[6];
+	x[82] = x[2] = pcm[5];
+	x[83] = x[3] = pcm[4];
+	x[84] = x[4] = pcm[3];
+	x[85] = x[5] = pcm[2];
+	x[86] = x[6] = pcm[1];
+	x[87] = x[7] = pcm[0];
+
+	_sbc_analyze_eight(x, frame->sb_sample_f[blk][ch]);
+
+	state->position[ch] -= 8;
+	if (state->position[ch] < 0)
+		state->position[ch] = 72;
+}
+
+static int sbc_analyze_audio(struct sbc_encoder_state *state,
+				struct sbc_frame *frame)
+{
+	int ch, blk;
+
+	switch (frame->subbands) {
+	case 4:
+		for (ch = 0; ch < frame->channels; ch++)
+			for (blk = 0; blk < frame->blocks; blk++)
+				sbc_analyze_four(state, frame, ch, blk);
+		return frame->blocks * 4;
+
+	case 8:
+		for (ch = 0; ch < frame->channels; ch++)
+			for (blk = 0; blk < frame->blocks; blk++)
+				sbc_analyze_eight(state, frame, ch, blk);
+		return frame->blocks * 8;
+
+	default:
+		return -EIO;
+	}
+}
+
+/*
+ * Packs the SBC frame from frame into the memory at data. At most len
+ * bytes will be used, should more memory be needed an appropriate
+ * error code will be returned. Returns the length of the packed frame
+ * on success or a negative value on error.
+ *
+ * The error codes are:
+ * -1 Not enough memory reserved
+ * -2 Unsupported sampling rate
+ * -3 Unsupported number of blocks
+ * -4 Unsupported number of subbands
+ * -5 Bitpool value out of bounds
+ * -99 not implemented
+ */
+
+static int sbc_pack_frame(uint8_t *data, struct sbc_frame *frame, size_t len)
+{
+	int produced;
+	/* Will copy the header parts for CRC-8 calculation here */
+	uint8_t crc_header[11] = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 };
+	int crc_pos = 0;
+
+	uint16_t audio_sample;
+
+	int ch, sb, blk, bit;	/* channel, subband, block and bit counters */
+	int bits[2][8];		/* bits distribution */
+	int levels[2][8];	/* levels are derived from that */
+
+	u_int32_t scalefactor[2][8];	/* derived from frame->scale_factor */
+
+	data[0] = SBC_SYNCWORD;
+
+	data[1] = (frame->frequency & 0x03) << 6;
+
+	data[1] |= (frame->block_mode & 0x03) << 4;
+
+	data[1] |= (frame->mode & 0x03) << 2;
+
+	data[1] |= (frame->allocation & 0x01) << 1;
+
+	switch (frame->subbands) {
+	case 4:
+		/* Nothing to do */
+		break;
+	case 8:
+		data[1] |= 0x01;
+		break;
+	default:
+		return -4;
+		break;
+	}
+
+	data[2] = frame->bitpool;
+
+	if ((frame->mode == MONO || frame->mode == DUAL_CHANNEL) &&
+			frame->bitpool > frame->subbands << 4)
+		return -5;
+
+	if ((frame->mode == STEREO || frame->mode == JOINT_STEREO) &&
+			frame->bitpool > frame->subbands << 5)
+		return -5;
+
+	/* Can't fill in crc yet */
+
+	produced = 32;
+
+	crc_header[0] = data[1];
+	crc_header[1] = data[2];
+	crc_pos = 16;
+
+	for (ch = 0; ch < frame->channels; ch++) {
+		for (sb = 0; sb < frame->subbands; sb++) {
+			frame->scale_factor[ch][sb] = 0;
+			scalefactor[ch][sb] = 2;
+			for (blk = 0; blk < frame->blocks; blk++) {
+				while (scalefactor[ch][sb] < fabs(frame->sb_sample_f[blk][ch][sb])) {
+					frame->scale_factor[ch][sb]++;
+					scalefactor[ch][sb] *= 2;
+				}
+			}
+		}
+	}
+
+	if (frame->mode == JOINT_STEREO) {
+		/* like frame->sb_sample but joint stereo */
+		int32_t sb_sample_j[16][2];
+		/* scalefactor and scale_factor in joint case */
+		u_int32_t scalefactor_j[2];
+		uint8_t scale_factor_j[2];
+
+		frame->joint = 0;
+
+		for (sb = 0; sb < frame->subbands - 1; sb++) {
+			scale_factor_j[0] = 0;
+			scalefactor_j[0] = 2;
+			scale_factor_j[1] = 0;
+			scalefactor_j[1] = 2;
+
+			for (blk = 0; blk < frame->blocks; blk++) {
+				/* Calculate joint stereo signal */
+				sb_sample_j[blk][0] =
+					(frame->sb_sample_f[blk][0][sb] +
+						frame->sb_sample_f[blk][1][sb]) >> 1;
+				sb_sample_j[blk][1] =
+					(frame->sb_sample_f[blk][0][sb] -
+						frame->sb_sample_f[blk][1][sb]) >> 1;
+
+				/* calculate scale_factor_j and scalefactor_j for joint case */
+				while (scalefactor_j[0] < fabs(sb_sample_j[blk][0])) {
+					scale_factor_j[0]++;
+					scalefactor_j[0] *= 2;
+				}
+				while (scalefactor_j[1] < fabs(sb_sample_j[blk][1])) {
+					scale_factor_j[1]++;
+					scalefactor_j[1] *= 2;
+				}
+			}
+
+			/* decide whether to join this subband */
+			if ((scalefactor[0][sb] + scalefactor[1][sb]) >
+					(scalefactor_j[0] + scalefactor_j[1]) ) {
+				/* use joint stereo for this subband */
+				frame->joint |= 1 << sb;
+				frame->scale_factor[0][sb] = scale_factor_j[0];
+				frame->scale_factor[1][sb] = scale_factor_j[1];
+				scalefactor[0][sb] = scalefactor_j[0];
+				scalefactor[1][sb] = scalefactor_j[1];
+				for (blk = 0; blk < frame->blocks; blk++) {
+					frame->sb_sample_f[blk][0][sb] =
+							sb_sample_j[blk][0];
+					frame->sb_sample_f[blk][1][sb] =
+							sb_sample_j[blk][1];
+				}
+			}
+		}
+
+		data[4] = 0;
+		for (sb = 0; sb < frame->subbands - 1; sb++)
+			data[4] |= ((frame->joint >> sb) & 0x01) << (frame->subbands - 1 - sb);
+
+		crc_header[crc_pos >> 3] = data[4];
+
+		produced += frame->subbands;
+		crc_pos += frame->subbands;
+	}
+
+	for (ch = 0; ch < frame->channels; ch++) {
+		for (sb = 0; sb < frame->subbands; sb++) {
+			data[produced >> 3] <<= 4;
+			crc_header[crc_pos >> 3] <<= 4;
+			data[produced >> 3] |= frame->scale_factor[ch][sb] & 0x0F;
+			crc_header[crc_pos >> 3] |= frame->scale_factor[ch][sb] & 0x0F;
+
+			produced += 4;
+			crc_pos += 4;
+		}
+	}
+
+	/* align the last crc byte */
+	if (crc_pos % 8)
+		crc_header[crc_pos >> 3] <<= 8 - (crc_pos % 8);
+
+	data[3] = sbc_crc8(crc_header, crc_pos);
+
+	sbc_calculate_bits(frame, bits);
+
+	for (ch = 0; ch < frame->channels; ch++) {
+		for (sb = 0; sb < frame->subbands; sb++)
+			levels[ch][sb] = (1 << bits[ch][sb]) - 1;
+	}
+
+	for (blk = 0; blk < frame->blocks; blk++) {
+		for (ch = 0; ch < frame->channels; ch++) {
+			for (sb = 0; sb < frame->subbands; sb++) {
+				if (levels[ch][sb] > 0) {
+					audio_sample =
+						(uint16_t) ((((frame->sb_sample_f[blk][ch][sb]*levels[ch][sb]) >>
+									(frame->scale_factor[ch][sb] + 1)) +
+								levels[ch][sb]) >> 1);
+					audio_sample <<= 16 - bits[ch][sb];
+					for (bit = 0; bit < bits[ch][sb]; bit++) {
+						data[produced >> 3] <<= 1;
+						if (audio_sample & 0x8000)
+							data[produced >> 3] |= 0x1;
+						audio_sample <<= 1;
+						produced++;
+					}
+				}
+			}
+		}
+	}
+
+	/* align the last byte */
+	if (produced % 8) {
+		data[produced >> 3] <<= 8 - (produced % 8);
+	}
+
+	return (produced + 7) >> 3;
+}
+
+struct sbc_priv {
+	int init;
+	struct sbc_frame frame;
+	struct sbc_decoder_state dec_state;
+	struct sbc_encoder_state enc_state;
+};
+
+static void sbc_set_defaults(sbc_t *sbc, unsigned long flags)
+{
+	sbc->frequency = SBC_FREQ_44100;
+	sbc->mode = SBC_MODE_STEREO;
+	sbc->subbands = SBC_SB_8;
+	sbc->blocks = SBC_BLK_16;
+	sbc->bitpool = 32;
+#if __BYTE_ORDER == __LITTLE_ENDIAN
+	sbc->endian = SBC_LE;
+#elif __BYTE_ORDER == __BIG_ENDIAN
+	sbc->endian = SBC_BE;
+#else
+#error "Unknown byte order"
+#endif
+}
+
+int sbc_init(sbc_t *sbc, unsigned long flags)
+{
+	if (!sbc)
+		return -EIO;
+
+	memset(sbc, 0, sizeof(sbc_t));
+
+	sbc->priv = malloc(sizeof(struct sbc_priv));
+	if (!sbc->priv)
+		return -ENOMEM;
+
+	memset(sbc->priv, 0, sizeof(struct sbc_priv));
+
+	sbc_set_defaults(sbc, flags);
+
+	return 0;
+}
+
+int sbc_parse(sbc_t *sbc, void *input, int input_len)
+{
+	return sbc_decode(sbc, input, input_len, NULL, 0, NULL);
+}
+
+int sbc_decode(sbc_t *sbc, void *input, int input_len, void *output,
+		int output_len, int *written)
+{
+	struct sbc_priv *priv;
+	char *ptr;
+	int i, ch, framelen, samples;
+
+	if (!sbc && !input)
+		return -EIO;
+
+	priv = sbc->priv;
+
+	framelen = sbc_unpack_frame(input, &priv->frame, input_len);
+
+	if (!priv->init) {
+		sbc_decoder_init(&priv->dec_state, &priv->frame);
+		priv->init = 1;
+
+		sbc->frequency = priv->frame.frequency;
+		sbc->mode = priv->frame.mode;
+		sbc->subbands = priv->frame.subband_mode;
+		sbc->blocks = priv->frame.block_mode;
+		sbc->allocation = priv->frame.allocation;
+		sbc->bitpool = priv->frame.bitpool;
+
+		priv->frame.codesize = sbc_get_codesize(sbc);
+		priv->frame.length = sbc_get_frame_length(sbc);
+	}
+
+	if (!output)
+		return framelen;
+
+	if (written)
+		*written = 0;
+
+	samples = sbc_synthesize_audio(&priv->dec_state, &priv->frame);
+
+	ptr = output;
+
+	if (output_len < samples * priv->frame.channels * 2)
+		samples = output_len / (priv->frame.channels * 2);
+
+	for (i = 0; i < samples; i++) {
+		for (ch = 0; ch < priv->frame.channels; ch++) {
+			int16_t s;
+			s = priv->frame.pcm_sample[ch][i];
+
+#if __BYTE_ORDER == __LITTLE_ENDIAN
+			if (sbc->endian == SBC_BE) {
+#elif __BYTE_ORDER == __BIG_ENDIAN
+			if (sbc->endian == SBC_LE) {
+#else
+#error "Unknown byte order"
+#endif
+				*ptr++ = (s & 0xff00) >> 8;
+				*ptr++ = (s & 0x00ff);
+			} else {
+				*ptr++ = (s & 0x00ff);
+				*ptr++ = (s & 0xff00) >> 8;
+			}
+		}
+	}
+
+	if (written)
+		*written = samples * priv->frame.channels * 2;
+
+	return framelen;
+}
+
+int sbc_encode(sbc_t *sbc, void *input, int input_len, void *output,
+		int output_len, int *written)
+{
+	struct sbc_priv *priv;
+	char *ptr;
+	int i, ch, framelen, samples;
+
+	if (!sbc && !input)
+		return -EIO;
+
+	priv = sbc->priv;
+
+	if (written)
+		*written = 0;
+
+	if (!priv->init) {
+		priv->frame.frequency = sbc->frequency;
+		priv->frame.mode = sbc->mode;
+		priv->frame.channels = sbc->mode == SBC_MODE_MONO ? 1 : 2;
+		priv->frame.allocation = sbc->allocation;
+		priv->frame.subband_mode = sbc->subbands;
+		priv->frame.subbands = sbc->subbands ? 8 : 4;
+		priv->frame.block_mode = sbc->blocks;
+		priv->frame.blocks = 4 + (sbc->blocks * 4);
+		priv->frame.bitpool = sbc->bitpool;
+		priv->frame.codesize = sbc_get_codesize(sbc);
+		priv->frame.length = sbc_get_frame_length(sbc);
+
+		sbc_encoder_init(&priv->enc_state, &priv->frame);
+		priv->init = 1;
+	}
+
+	/* input must be large enough to encode a complete frame */
+	if (input_len < priv->frame.codesize)
+		return 0;
+
+	/* output must be large enough to receive the encoded frame */
+	if (!output || output_len < priv->frame.length)
+		return -ENOSPC;
+
+	ptr = input;
+
+	for (i = 0; i < priv->frame.subbands * priv->frame.blocks; i++) {
+		for (ch = 0; ch < priv->frame.channels; ch++) {
+			int16_t s;
+#if __BYTE_ORDER == __LITTLE_ENDIAN
+			if (sbc->endian == SBC_BE)
+#elif __BYTE_ORDER == __BIG_ENDIAN
+			if (sbc->endian == SBC_LE)
+#else
+#error "Unknown byte order"
+#endif
+				s = (ptr[0] & 0xff) << 8 | (ptr[1] & 0xff);
+			else
+				s = (ptr[0] & 0xff) | (ptr[1] & 0xff) << 8;
+			ptr += 2;
+			priv->frame.pcm_sample[ch][i] = s;
+		}
+	}
+
+	samples = sbc_analyze_audio(&priv->enc_state, &priv->frame);
+
+	framelen = sbc_pack_frame(output, &priv->frame, output_len);
+
+	if (written)
+		*written = framelen;
+
+	return samples * priv->frame.channels * 2;
+}
+
+void sbc_finish(sbc_t *sbc)
+{
+	if (!sbc)
+		return;
+
+	if (sbc->priv)
+		free(sbc->priv);
+
+	memset(sbc, 0, sizeof(sbc_t));
+}
+
+int sbc_get_frame_length(sbc_t *sbc)
+{
+	int ret;
+	uint8_t subbands, channels, blocks, joint;
+	struct sbc_priv *priv;
+
+	priv = sbc->priv;
+	if (!priv->init) {
+		subbands = sbc->subbands ? 8 : 4;
+		blocks = 4 + (sbc->blocks * 4);
+		channels = sbc->mode == SBC_MODE_MONO ? 1 : 2;
+		joint = sbc->mode == SBC_MODE_JOINT_STEREO ? 1 : 0;
+	} else {
+		subbands = priv->frame.subbands;
+		blocks = priv->frame.blocks;
+		channels = priv->frame.channels;
+		joint = priv->frame.joint;
+	}
+
+	ret = 4 + (4 * subbands * channels) / 8;
+
+	/* This term is not always evenly divide so we round it up */
+	if (channels == 1)
+		ret += ((blocks * channels * sbc->bitpool) + 7) / 8;
+	else
+		ret += (((joint ? subbands : 0) + blocks * sbc->bitpool) + 7)
+			/ 8;
+
+	return ret;
+}
+
+int sbc_get_frame_duration(sbc_t *sbc)
+{
+	uint8_t subbands, blocks;
+	uint16_t frequency;
+	struct sbc_priv *priv;
+
+	priv = sbc->priv;
+	if (!priv->init) {
+		subbands = sbc->subbands ? 8 : 4;
+		blocks = 4 + (sbc->blocks * 4);
+	} else {
+		subbands = priv->frame.subbands;
+		blocks = priv->frame.blocks;
+	}
+
+	switch (sbc->frequency) {
+	case SBC_FREQ_16000:
+		frequency = 16000;
+		break;
+
+	case SBC_FREQ_32000:
+		frequency = 32000;
+		break;
+
+	case SBC_FREQ_44100:
+		frequency = 44100;
+		break;
+
+	case SBC_FREQ_48000:
+		frequency = 48000;
+		break;
+	default:
+		return 0;
+	}
+
+	return (1000000 * blocks * subbands) / frequency;
+}
+
+int sbc_get_codesize(sbc_t *sbc)
+{
+	uint8_t subbands, channels, blocks;
+	struct sbc_priv *priv;
+
+	priv = sbc->priv;
+	if (!priv->init) {
+		subbands = sbc->subbands ? 8 : 4;
+		blocks = 4 + (sbc->blocks * 4);
+		channels = sbc->mode == SBC_MODE_MONO ? 1 : 2;
+	} else {
+		subbands = priv->frame.subbands;
+		blocks = priv->frame.blocks;
+		channels = priv->frame.channels;
+	}
+
+	return subbands * blocks * channels * 2;
+}
+
+int sbc_reinit(sbc_t *sbc, unsigned long flags)
+{
+	struct sbc_priv *priv;
+
+	if (!sbc || !sbc->priv)
+		return -EIO;
+
+	priv = sbc->priv;
+
+	if (priv->init == 1)
+		memset(sbc->priv, 0, sizeof(struct sbc_priv));
+
+	sbc_set_defaults(sbc, flags);
+
+	return 0;
+}
diff --git a/sbc/sbc.h b/sbc/sbc.h
new file mode 100644
index 0000000..ab47e32
--- /dev/null
+++ b/sbc/sbc.h
@@ -0,0 +1,97 @@
+/*
+ *
+ *  Bluetooth low-complexity, subband codec (SBC) library
+ *
+ *  Copyright (C) 2004-2008  Marcel Holtmann <marcel@holtmann.org>
+ *  Copyright (C) 2004-2005  Henryk Ploetz <henryk@ploetzli.ch>
+ *  Copyright (C) 2005-2006  Brad Midgley <bmidgley@xmission.com>
+ *
+ *
+ *  This library is free software; you can redistribute it and/or
+ *  modify it under the terms of the GNU Lesser General Public
+ *  License as published by the Free Software Foundation; either
+ *  version 2.1 of the License, or (at your option) any later version.
+ *
+ *  This library is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ *  Lesser General Public License for more details.
+ *
+ *  You should have received a copy of the GNU Lesser General Public
+ *  License along with this library; if not, write to the Free Software
+ *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+#ifndef __SBC_H
+#define __SBC_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdint.h>
+
+/* sampling frequency */
+#define SBC_FREQ_16000		0x00
+#define SBC_FREQ_32000		0x01
+#define SBC_FREQ_44100		0x02
+#define SBC_FREQ_48000		0x03
+
+/* blocks */
+#define SBC_BLK_4		0x00
+#define SBC_BLK_8		0x01
+#define SBC_BLK_12		0x02
+#define SBC_BLK_16		0x03
+
+/* channel mode */
+#define SBC_MODE_MONO		0x00
+#define SBC_MODE_DUAL_CHANNEL	0x01
+#define SBC_MODE_STEREO		0x02
+#define SBC_MODE_JOINT_STEREO	0x03
+
+/* allocation method */
+#define SBC_AM_LOUDNESS		0x00
+#define SBC_AM_SNR		0x01
+
+/* subbands */
+#define SBC_SB_4		0x00
+#define SBC_SB_8		0x01
+
+/* Data endianess */
+#define SBC_LE			0x00
+#define SBC_BE			0x01
+
+struct sbc_struct {
+	unsigned long flags;
+
+	uint8_t frequency;
+	uint8_t blocks;
+	uint8_t subbands;
+	uint8_t mode;
+	uint8_t allocation;
+	uint8_t bitpool;
+	uint8_t endian;
+
+	void *priv;
+};
+
+typedef struct sbc_struct sbc_t;
+
+int sbc_init(sbc_t *sbc, unsigned long flags);
+int sbc_reinit(sbc_t *sbc, unsigned long flags);
+int sbc_parse(sbc_t *sbc, void *input, int input_len);
+int sbc_decode(sbc_t *sbc, void *input, int input_len, void *output,
+		int output_len, int *len);
+int sbc_encode(sbc_t *sbc, void *input, int input_len, void *output,
+		int output_len, int *written);
+int sbc_get_frame_length(sbc_t *sbc);
+int sbc_get_frame_duration(sbc_t *sbc);
+int sbc_get_codesize(sbc_t *sbc);
+void sbc_finish(sbc_t *sbc);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* __SBC_H */
diff --git a/sbc/sbc_math.h b/sbc/sbc_math.h
new file mode 100644
index 0000000..b3d87a6
--- /dev/null
+++ b/sbc/sbc_math.h
@@ -0,0 +1,72 @@
+/*
+ *
+ *  Bluetooth low-complexity, subband codec (SBC) library
+ *
+ *  Copyright (C) 2004-2008  Marcel Holtmann <marcel@holtmann.org>
+ *  Copyright (C) 2004-2005  Henryk Ploetz <henryk@ploetzli.ch>
+ *  Copyright (C) 2005-2008  Brad Midgley <bmidgley@xmission.com>
+ *
+ *
+ *  This library is free software; you can redistribute it and/or
+ *  modify it under the terms of the GNU Lesser General Public
+ *  License as published by the Free Software Foundation; either
+ *  version 2.1 of the License, or (at your option) any later version.
+ *
+ *  This library is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ *  Lesser General Public License for more details.
+ *
+ *  You should have received a copy of the GNU Lesser General Public
+ *  License along with this library; if not, write to the Free Software
+ *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+#define fabs(x) ((x) < 0 ? -(x) : (x))
+/* C does not provide an explicit arithmetic shift right but this will
+   always be correct and every compiler *should* generate optimal code */
+#define ASR(val, bits) ((-2 >> 1 == -1) ? \
+		 ((int32_t)(val)) >> (bits) : ((int32_t) (val)) / (1 << (bits)))
+
+#define SCALE_PROTO4_TBL	15
+#define SCALE_ANA4_TBL		17
+#define SCALE_PROTO8_TBL	16
+#define SCALE_ANA8_TBL		17
+#define SCALE_SPROTO4_TBL	12
+#define SCALE_SPROTO8_TBL	14
+#define SCALE_NPROTO4_TBL	11
+#define SCALE_NPROTO8_TBL	11
+#define SCALE4_STAGE1_BITS	15
+#define SCALE4_STAGE2_BITS	16
+#define SCALE4_STAGED1_BITS	15
+#define SCALE4_STAGED2_BITS	16
+#define SCALE8_STAGE1_BITS	15
+#define SCALE8_STAGE2_BITS	15
+#define SCALE8_STAGED1_BITS	15
+#define SCALE8_STAGED2_BITS	16
+
+typedef int32_t sbc_fixed_t;
+
+#define SCALE4_STAGE1(src)  ASR(src, SCALE4_STAGE1_BITS)
+#define SCALE4_STAGE2(src)  ASR(src, SCALE4_STAGE2_BITS)
+#define SCALE4_STAGED1(src) ASR(src, SCALE4_STAGED1_BITS)
+#define SCALE4_STAGED2(src) ASR(src, SCALE4_STAGED2_BITS)
+#define SCALE8_STAGE1(src)  ASR(src, SCALE8_STAGE1_BITS)
+#define SCALE8_STAGE2(src)  ASR(src, SCALE8_STAGE2_BITS)
+#define SCALE8_STAGED1(src) ASR(src, SCALE8_STAGED1_BITS)
+#define SCALE8_STAGED2(src) ASR(src, SCALE8_STAGED2_BITS)
+
+#define SBC_FIXED_0(val) { val = 0; }
+#define MUL(a, b)        ((a) * (b))
+#ifdef __arm__
+#define MULA(a, b, res) ({				\
+		int tmp = res;			\
+		__asm__(				\
+			"mla %0, %2, %3, %0"		\
+			: "=&r" (tmp)			\
+			: "0" (tmp), "r" (a), "r" (b));	\
+		tmp; })
+#else
+#define MULA(a, b, res)  ((a) * (b) + (res))
+#endif
diff --git a/sbc/sbc_tables.h b/sbc/sbc_tables.h
new file mode 100644
index 0000000..7ac4e68
--- /dev/null
+++ b/sbc/sbc_tables.h
@@ -0,0 +1,167 @@
+/*
+ *
+ *  Bluetooth low-complexity, subband codec (SBC) library
+ *
+ *  Copyright (C) 2004-2008  Marcel Holtmann <marcel@holtmann.org>
+ *  Copyright (C) 2004-2005  Henryk Ploetz <henryk@ploetzli.ch>
+ *  Copyright (C) 2005-2006  Brad Midgley <bmidgley@xmission.com>
+ *
+ *
+ *  This library is free software; you can redistribute it and/or
+ *  modify it under the terms of the GNU Lesser General Public
+ *  License as published by the Free Software Foundation; either
+ *  version 2.1 of the License, or (at your option) any later version.
+ *
+ *  This library is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ *  Lesser General Public License for more details.
+ *
+ *  You should have received a copy of the GNU Lesser General Public
+ *  License along with this library; if not, write to the Free Software
+ *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+/* A2DP specification: Appendix B, page 69 */
+static const int sbc_offset4[4][4] = {
+	{ -1, 0, 0, 0 },
+	{ -2, 0, 0, 1 },
+	{ -2, 0, 0, 1 },
+	{ -2, 0, 0, 1 }
+};
+
+/* A2DP specification: Appendix B, page 69 */
+static const int sbc_offset8[4][8] = {
+	{ -2, 0, 0, 0, 0, 0, 0, 1 },
+	{ -3, 0, 0, 0, 0, 0, 1, 2 },
+	{ -4, 0, 0, 0, 0, 0, 1, 2 },
+	{ -4, 0, 0, 0, 0, 0, 1, 2 }
+};
+
+#define SP4(val) ASR(val, SCALE_PROTO4_TBL)
+#define SA4(val) ASR(val, SCALE_ANA4_TBL)
+#define SP8(val) ASR(val, SCALE_PROTO8_TBL)
+#define SA8(val) ASR(val, SCALE_ANA8_TBL)
+#define SS4(val) ASR(val, SCALE_SPROTO4_TBL)
+#define SS8(val) ASR(val, SCALE_SPROTO8_TBL)
+#define SN4(val) ASR(val, SCALE_NPROTO4_TBL)
+#define SN8(val) ASR(val, SCALE_NPROTO8_TBL)
+
+static const int32_t _sbc_proto_4[20] = {
+	SP4(0x02cb3e8c), SP4(0x22b63dc0), SP4(0x002329cc), SP4(0x053b7548),
+	SP4(0x31eab940), SP4(0xec1f5e60), SP4(0xff3773a8), SP4(0x0061c5a7),
+	SP4(0x07646680), SP4(0x3f239480), SP4(0xf89f23a8), SP4(0x007a4737),
+	SP4(0x00b32807), SP4(0x083ddc80), SP4(0x4825e480), SP4(0x0191e578),
+	SP4(0x00ff11ca), SP4(0x00fb7991), SP4(0x069fdc58), SP4(0x4b584000)
+};
+
+static const int32_t _anamatrix4[4] = {
+	SA4(0x2d413cc0), SA4(0x3b20d780), SA4(0x40000000), SA4(0x187de2a0)
+};
+
+static const int32_t _sbc_proto_8[40] = {
+	SP8(0x02e5cd20), SP8(0x22d0c200), SP8(0x006bfe27), SP8(0x07808930),
+	SP8(0x3f1c8800), SP8(0xf8810d70), SP8(0x002cfdc6), SP8(0x055acf28),
+	SP8(0x31f566c0), SP8(0xebfe57e0), SP8(0xff27c437), SP8(0x001485cc),
+	SP8(0x041c6e58), SP8(0x2a7cfa80), SP8(0xe4c4a240), SP8(0xfe359e4c),
+	SP8(0x0048b1f8), SP8(0x0686ce30), SP8(0x38eec5c0), SP8(0xf2a1b9f0),
+	SP8(0xffe8904a), SP8(0x0095698a), SP8(0x0824a480), SP8(0x443b3c00),
+	SP8(0xfd7badc8), SP8(0x00d3e2d9), SP8(0x00c183d2), SP8(0x084e1950),
+	SP8(0x4810d800), SP8(0x017f43fe), SP8(0x01056dd8), SP8(0x00e9cb9f),
+	SP8(0x07d7d090), SP8(0x4a708980), SP8(0x0488fae8), SP8(0x0113bd20),
+	SP8(0x0107b1a8), SP8(0x069fb3c0), SP8(0x4b3db200), SP8(0x00763f48)
+};
+
+static const int32_t sbc_proto_4_40m0[] = {
+	SS4(0x00000000), SS4(0xffa6982f), SS4(0xfba93848), SS4(0x0456c7b8),
+	SS4(0x005967d1), SS4(0xfffb9ac7), SS4(0xff589157), SS4(0xf9c2a8d8),
+	SS4(0x027c1434), SS4(0x0019118b), SS4(0xfff3c74c), SS4(0xff137330),
+	SS4(0xf81b8d70), SS4(0x00ec1b8b), SS4(0xfff0b71a), SS4(0xffe99b00),
+	SS4(0xfef84470), SS4(0xf6fb4370), SS4(0xffcdc351), SS4(0xffe01dc7)
+};
+
+static const int32_t sbc_proto_4_40m1[] = {
+	SS4(0xffe090ce), SS4(0xff2c0475), SS4(0xf694f800), SS4(0xff2c0475),
+	SS4(0xffe090ce), SS4(0xffe01dc7), SS4(0xffcdc351), SS4(0xf6fb4370),
+	SS4(0xfef84470), SS4(0xffe99b00), SS4(0xfff0b71a), SS4(0x00ec1b8b),
+	SS4(0xf81b8d70), SS4(0xff137330), SS4(0xfff3c74c), SS4(0x0019118b),
+	SS4(0x027c1434), SS4(0xf9c2a8d8), SS4(0xff589157), SS4(0xfffb9ac7)
+};
+
+static const int32_t sbc_proto_8_80m0[] = {
+	SS8(0x00000000), SS8(0xfe8d1970), SS8(0xee979f00), SS8(0x11686100),
+	SS8(0x0172e690), SS8(0xfff5bd1a), SS8(0xfdf1c8d4), SS8(0xeac182c0),
+	SS8(0x0d9daee0), SS8(0x00e530da), SS8(0xffe9811d), SS8(0xfd52986c),
+	SS8(0xe7054ca0), SS8(0x0a00d410), SS8(0x006c1de4), SS8(0xffdba705),
+	SS8(0xfcbc98e8), SS8(0xe3889d20), SS8(0x06af2308), SS8(0x000bb7db),
+	SS8(0xffca00ed), SS8(0xfc3fbb68), SS8(0xe071bc00), SS8(0x03bf7948),
+	SS8(0xffc4e05c), SS8(0xffb54b3b), SS8(0xfbedadc0), SS8(0xdde26200),
+	SS8(0x0142291c), SS8(0xff960e94), SS8(0xff9f3e17), SS8(0xfbd8f358),
+	SS8(0xdbf79400), SS8(0xff405e01), SS8(0xff7d4914), SS8(0xff8b1a31),
+	SS8(0xfc1417b8), SS8(0xdac7bb40), SS8(0xfdbb828c), SS8(0xff762170)
+};
+
+static const int32_t sbc_proto_8_80m1[] = {
+	SS8(0xff7c272c), SS8(0xfcb02620), SS8(0xda612700), SS8(0xfcb02620),
+	SS8(0xff7c272c), SS8(0xff762170), SS8(0xfdbb828c), SS8(0xdac7bb40),
+	SS8(0xfc1417b8), SS8(0xff8b1a31), SS8(0xff7d4914), SS8(0xff405e01),
+	SS8(0xdbf79400), SS8(0xfbd8f358), SS8(0xff9f3e17), SS8(0xff960e94),
+	SS8(0x0142291c), SS8(0xdde26200), SS8(0xfbedadc0), SS8(0xffb54b3b),
+	SS8(0xffc4e05c), SS8(0x03bf7948), SS8(0xe071bc00), SS8(0xfc3fbb68),
+	SS8(0xffca00ed), SS8(0x000bb7db), SS8(0x06af2308), SS8(0xe3889d20),
+	SS8(0xfcbc98e8), SS8(0xffdba705), SS8(0x006c1de4), SS8(0x0a00d410),
+	SS8(0xe7054ca0), SS8(0xfd52986c), SS8(0xffe9811d), SS8(0x00e530da),
+	SS8(0x0d9daee0), SS8(0xeac182c0), SS8(0xfdf1c8d4), SS8(0xfff5bd1a)
+};
+
+static const int32_t _anamatrix8[8] = {
+	SA8(0x3b20d780), SA8(0x187de2a0), SA8(0x3ec52f80), SA8(0x3536cc40),
+	SA8(0x238e7680), SA8(0x0c7c5c20), SA8(0x2d413cc0), SA8(0x40000000)
+};
+
+static const int32_t synmatrix4[8][4] = {
+	{ SN4(0x05a82798), SN4(0xfa57d868), SN4(0xfa57d868), SN4(0x05a82798) },
+	{ SN4(0x030fbc54), SN4(0xf89be510), SN4(0x07641af0), SN4(0xfcf043ac) },
+	{ SN4(0x00000000), SN4(0x00000000), SN4(0x00000000), SN4(0x00000000) },
+	{ SN4(0xfcf043ac), SN4(0x07641af0), SN4(0xf89be510), SN4(0x030fbc54) },
+	{ SN4(0xfa57d868), SN4(0x05a82798), SN4(0x05a82798), SN4(0xfa57d868) },
+	{ SN4(0xf89be510), SN4(0xfcf043ac), SN4(0x030fbc54), SN4(0x07641af0) },
+	{ SN4(0xf8000000), SN4(0xf8000000), SN4(0xf8000000), SN4(0xf8000000) },
+	{ SN4(0xf89be510), SN4(0xfcf043ac), SN4(0x030fbc54), SN4(0x07641af0) }
+};
+
+static const int32_t synmatrix8[16][8] = {
+	{ SN8(0x05a82798), SN8(0xfa57d868), SN8(0xfa57d868), SN8(0x05a82798),
+	  SN8(0x05a82798), SN8(0xfa57d868), SN8(0xfa57d868), SN8(0x05a82798) },
+	{ SN8(0x0471ced0), SN8(0xf8275a10), SN8(0x018f8b84), SN8(0x06a6d988),
+	  SN8(0xf9592678), SN8(0xfe70747c), SN8(0x07d8a5f0), SN8(0xfb8e3130) },
+	{ SN8(0x030fbc54), SN8(0xf89be510), SN8(0x07641af0), SN8(0xfcf043ac),
+	  SN8(0xfcf043ac), SN8(0x07641af0), SN8(0xf89be510), SN8(0x030fbc54) },
+	{ SN8(0x018f8b84), SN8(0xfb8e3130), SN8(0x06a6d988), SN8(0xf8275a10),
+	  SN8(0x07d8a5f0), SN8(0xf9592678), SN8(0x0471ced0), SN8(0xfe70747c) },
+	{ SN8(0x00000000), SN8(0x00000000), SN8(0x00000000), SN8(0x00000000),
+	  SN8(0x00000000), SN8(0x00000000), SN8(0x00000000), SN8(0x00000000) },
+	{ SN8(0xfe70747c), SN8(0x0471ced0), SN8(0xf9592678), SN8(0x07d8a5f0),
+	  SN8(0xf8275a10), SN8(0x06a6d988), SN8(0xfb8e3130), SN8(0x018f8b84) },
+	{ SN8(0xfcf043ac), SN8(0x07641af0), SN8(0xf89be510), SN8(0x030fbc54),
+	  SN8(0x030fbc54), SN8(0xf89be510), SN8(0x07641af0), SN8(0xfcf043ac) },
+	{ SN8(0xfb8e3130), SN8(0x07d8a5f0), SN8(0xfe70747c), SN8(0xf9592678),
+	  SN8(0x06a6d988), SN8(0x018f8b84), SN8(0xf8275a10), SN8(0x0471ced0) },
+	{ SN8(0xfa57d868), SN8(0x05a82798), SN8(0x05a82798), SN8(0xfa57d868),
+	  SN8(0xfa57d868), SN8(0x05a82798), SN8(0x05a82798), SN8(0xfa57d868) },
+	{ SN8(0xf9592678), SN8(0x018f8b84), SN8(0x07d8a5f0), SN8(0x0471ced0),
+	  SN8(0xfb8e3130), SN8(0xf8275a10), SN8(0xfe70747c), SN8(0x06a6d988) },
+	{ SN8(0xf89be510), SN8(0xfcf043ac), SN8(0x030fbc54), SN8(0x07641af0),
+	  SN8(0x07641af0), SN8(0x030fbc54), SN8(0xfcf043ac), SN8(0xf89be510) },
+	{ SN8(0xf8275a10), SN8(0xf9592678), SN8(0xfb8e3130), SN8(0xfe70747c),
+	  SN8(0x018f8b84), SN8(0x0471ced0), SN8(0x06a6d988), SN8(0x07d8a5f0) },
+	{ SN8(0xf8000000), SN8(0xf8000000), SN8(0xf8000000), SN8(0xf8000000),
+	  SN8(0xf8000000), SN8(0xf8000000), SN8(0xf8000000), SN8(0xf8000000) },
+	{ SN8(0xf8275a10), SN8(0xf9592678), SN8(0xfb8e3130), SN8(0xfe70747c),
+	  SN8(0x018f8b84), SN8(0x0471ced0), SN8(0x06a6d988), SN8(0x07d8a5f0) },
+	{ SN8(0xf89be510), SN8(0xfcf043ac), SN8(0x030fbc54), SN8(0x07641af0),
+	  SN8(0x07641af0), SN8(0x030fbc54), SN8(0xfcf043ac), SN8(0xf89be510) },
+	{ SN8(0xf9592678), SN8(0x018f8b84), SN8(0x07d8a5f0), SN8(0x0471ced0),
+	  SN8(0xfb8e3130), SN8(0xf8275a10), SN8(0xfe70747c), SN8(0x06a6d988) }
+};
diff --git a/sbc/sbcdec.c b/sbc/sbcdec.c
new file mode 100644
index 0000000..5ac02b4
--- /dev/null
+++ b/sbc/sbcdec.c
@@ -0,0 +1,267 @@
+/*
+ *
+ *  Bluetooth low-complexity, subband codec (SBC) decoder
+ *
+ *  Copyright (C) 2004-2008  Marcel Holtmann <marcel@holtmann.org>
+ *
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program; if not, write to the Free Software
+ *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+#ifdef HAVE_CONFIG_H
+#include <config.h>
+#endif
+
+#include <stdio.h>
+#include <errno.h>
+#include <fcntl.h>
+#include <unistd.h>
+#include <stdlib.h>
+#include <string.h>
+#include <getopt.h>
+#include <sys/stat.h>
+#include <sys/ioctl.h>
+#include <sys/soundcard.h>
+
+#include "sbc.h"
+
+#define BUF_SIZE 8192
+
+static void decode(char *filename, char *output, int tofile)
+{
+	unsigned char buf[BUF_SIZE], *stream;
+	struct stat st;
+	off_t filesize;
+	sbc_t sbc;
+	int fd, ad, pos, streamlen, framelen, count, written, len;
+	int format = AFMT_S16_BE, frequency, channels;
+
+	if (stat(filename, &st) < 0) {
+		fprintf(stderr, "Can't get size of file %s: %s\n",
+						filename, strerror(errno));
+		return;
+	}
+
+	filesize = st.st_size;
+	stream = malloc(st.st_size);
+
+	if (!stream) {
+		fprintf(stderr, "Can't allocate memory for %s: %s\n",
+						filename, strerror(errno));
+		return;
+	}
+
+	fd = open(filename, O_RDONLY);
+	if (fd < 0) {
+		fprintf(stderr, "Can't open file %s: %s\n",
+						filename, strerror(errno));
+		goto free;
+	}
+
+	if (read(fd, stream, st.st_size) != st.st_size) {
+		fprintf(stderr, "Can't read content of %s: %s\n",
+						filename, strerror(errno));
+		close(fd);
+		goto free;
+	}
+
+	close(fd);
+
+	pos = 0;
+	streamlen = st.st_size;
+
+	if (tofile)
+		ad = open(output, O_WRONLY | O_CREAT | O_TRUNC, 0644);
+	else
+		ad = open(output, O_WRONLY, 0);
+
+	if (ad < 0) {
+		fprintf(stderr, "Can't open output %s: %s\n",
+						output, strerror(errno));
+		goto free;
+	}
+
+	sbc_init(&sbc, 0L);
+	sbc.endian = SBC_BE;
+
+	framelen = sbc_decode(&sbc, stream, streamlen, buf, sizeof(buf), &len);
+	channels = sbc.mode == SBC_MODE_MONO ? 1 : 2;
+	switch (sbc.frequency) {
+	case SBC_FREQ_16000:
+		frequency = 16000;
+		break;
+
+	case SBC_FREQ_32000:
+		frequency = 32000;
+		break;
+
+	case SBC_FREQ_44100:
+		frequency = 44100;
+		break;
+
+	case SBC_FREQ_48000:
+		frequency = 48000;
+		break;
+	default:
+		frequency = 0;
+	}
+
+	printf("%d Hz, %d channels\n", frequency, channels);
+	if (!tofile) {
+		if (ioctl(ad, SNDCTL_DSP_SETFMT, &format) < 0) {
+			fprintf(stderr, "Can't set audio format on %s: %s\n",
+					output, strerror(errno));
+			goto close;
+		}
+		if (ioctl(ad, SNDCTL_DSP_CHANNELS, &channels) < 0) {
+			fprintf(stderr,
+				"Can't set number of channels on %s: %s\n",
+				output, strerror(errno));
+			goto close;
+		}
+
+		if (ioctl(ad, SNDCTL_DSP_SPEED, &frequency) < 0) {
+			fprintf(stderr, "Can't set audio rate on %s: %s\n",
+					output, strerror(errno));
+			goto close;
+		}
+	}
+
+	count = 0;
+	while (framelen > 0) {
+		/* we have completed an sbc_decode at this point sbc.len is the
+		 * length of the frame we just decoded count is the number of
+		 * decoded bytes yet to be written */
+
+		if (count + len >= BUF_SIZE) {
+			/* buffer is too full to stuff decoded audio in so it
+			 * must be written to the device */
+			written = write(ad, buf, count);
+			if (written > 0)
+				count -= written;
+		}
+
+		/* sanity check */
+		if (count + len >= BUF_SIZE) {
+			fprintf(stderr,
+				"buffer size of %d is too small for decoded"
+				" data (%d)\n", BUF_SIZE, len + count);
+			exit(1);
+		}
+
+		/* increase the count */
+		count += len;
+
+		/* push the pointer in the file forward to the next bit to be
+		 * decoded tell the decoder to decode up to the remaining
+		 * length of the file (!) */
+		pos += framelen;
+		framelen = sbc_decode(&sbc, stream + pos, streamlen - pos,
+					buf + count, sizeof(buf) - count,
+					&len);
+	}
+
+	if (count > 0) {
+		written = write(ad, buf, count);
+		if (written > 0)
+			count -= written;
+	}
+
+close:
+	sbc_finish(&sbc);
+
+	close(ad);
+
+free:
+	free(stream);
+}
+
+static void usage(void)
+{
+	printf("SBC decoder utility ver %s\n", VERSION);
+	printf("Copyright (c) 2004-2008  Marcel Holtmann\n\n");
+
+	printf("Usage:\n"
+		"\tsbcdec [options] file(s)\n"
+		"\n");
+
+	printf("Options:\n"
+		"\t-h, --help           Display help\n"
+		"\t-v, --verbose        Verbose mode\n"
+		"\t-d, --device <dsp>   Sound device\n"
+		"\t-f, --file <file>    Decode to a file\n"
+		"\n");
+}
+
+static struct option main_options[] = {
+	{ "help",	0, 0, 'h' },
+	{ "device",	1, 0, 'd' },
+	{ "verbose",	0, 0, 'v' },
+	{ "file",	1, 0, 'f' },
+	{ 0, 0, 0, 0 }
+};
+
+int main(int argc, char *argv[])
+{
+	char *output = NULL;
+	int i, opt, verbose = 0, tofile = 0;
+
+	while ((opt = getopt_long(argc, argv, "+hvd:f:", main_options, NULL)) != -1) {
+		switch(opt) {
+		case 'h':
+			usage();
+			exit(0);
+
+		case 'v':
+			verbose = 1;
+			break;
+
+		case 'd':
+			if (output)
+				free(output);
+			output = strdup(optarg);
+			tofile = 0;
+			break;
+
+		case 'f' :
+			if (output)
+				free(output);
+			output = strdup(optarg);
+			tofile = 1;
+			break;
+
+		default:
+			exit(1);
+		}
+	}
+
+	argc -= optind;
+	argv += optind;
+	optind = 0;
+
+	if (argc < 1) {
+		usage();
+		exit(1);
+	}
+
+	for (i = 0; i < argc; i++)
+		decode(argv[i], output ? output : "/dev/dsp", tofile);
+
+	if (output)
+		free(output);
+
+	return 0;
+}
diff --git a/sbc/sbcenc.c b/sbc/sbcenc.c
new file mode 100644
index 0000000..e17f8fd
--- /dev/null
+++ b/sbc/sbcenc.c
@@ -0,0 +1,280 @@
+/*
+ *
+ *  Bluetooth low-complexity, subband codec (SBC) encoder
+ *
+ *  Copyright (C) 2004-2008  Marcel Holtmann <marcel@holtmann.org>
+ *
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program; if not, write to the Free Software
+ *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+#ifdef HAVE_CONFIG_H
+#include <config.h>
+#endif
+
+#include <stdio.h>
+#include <errno.h>
+#include <fcntl.h>
+#include <unistd.h>
+#include <stdlib.h>
+#include <stdint.h>
+#include <string.h>
+#include <getopt.h>
+#include <byteswap.h>
+#include <sys/stat.h>
+
+#include "sbc.h"
+
+#if __BYTE_ORDER == __LITTLE_ENDIAN
+#define COMPOSE_ID(a,b,c,d)	((a) | ((b)<<8) | ((c)<<16) | ((d)<<24))
+#define LE_SHORT(v)		(v)
+#define LE_INT(v)		(v)
+#define BE_SHORT(v)		bswap_16(v)
+#define BE_INT(v)		bswap_32(v)
+#elif __BYTE_ORDER == __BIG_ENDIAN
+#define COMPOSE_ID(a,b,c,d)	((d) | ((c)<<8) | ((b)<<16) | ((a)<<24))
+#define LE_SHORT(v)		bswap_16(v)
+#define LE_INT(v)		bswap_32(v)
+#define BE_SHORT(v)		(v)
+#define BE_INT(v)		(v)
+#else
+#error "Wrong endian"
+#endif
+
+#define AU_MAGIC		COMPOSE_ID('.','s','n','d')
+
+#define AU_FMT_ULAW		1
+#define AU_FMT_LIN8		2
+#define AU_FMT_LIN16		3
+
+struct au_header {
+	uint32_t magic;		/* '.snd' */
+	uint32_t hdr_size;	/* size of header (min 24) */
+	uint32_t data_size;	/* size of data */
+	uint32_t encoding;	/* see to AU_FMT_XXXX */
+	uint32_t sample_rate;	/* sample rate */
+	uint32_t channels;	/* number of channels (voices) */
+};
+
+static ssize_t __read(int fd, void *buf, size_t count)
+{
+	ssize_t len, pos = 0;
+
+	while (count > 0) {
+		len = read(fd, buf + pos, count);
+		if (len <= 0)
+			return len;
+
+		count -= len;
+		pos   += len;
+	}
+
+	return pos;
+}
+
+static ssize_t __write(int fd, const void *buf, size_t count)
+{
+	ssize_t len, pos = 0;
+
+	while (count > 0) {
+		len = write(fd, buf + pos, count);
+		if (len <= 0)
+			return len;
+
+		count -= len;
+		pos   += len;
+	}
+
+	return pos;
+}
+
+static void encode(char *filename, int subbands, int joint)
+{
+	struct au_header *au_hdr;
+	unsigned char input[2048], output[2048];
+	sbc_t sbc;
+	int fd, len, size, count, encoded;
+
+	if (strcmp(filename, "-")) {
+		fd = open(filename, O_RDONLY);
+		if (fd < 0) {
+			fprintf(stderr, "Can't open file %s: %s\n",
+						filename, strerror(errno));
+			return;
+		}
+	} else
+		fd = fileno(stdin);
+
+	len = __read(fd, input, sizeof(input));
+	if (len < sizeof(*au_hdr)) {
+		if (fd > fileno(stderr))
+			fprintf(stderr, "Can't read header from file %s: %s\n",
+						filename, strerror(errno));
+		else
+			perror("Can't read audio header");
+		goto done;
+	}
+
+	au_hdr = (struct au_header *) input;
+
+	if (au_hdr->magic != AU_MAGIC ||
+			BE_INT(au_hdr->hdr_size) > 128 ||
+			BE_INT(au_hdr->hdr_size) < 24 ||
+			BE_INT(au_hdr->encoding) != AU_FMT_LIN16) {
+		fprintf(stderr, "Data is not in Sun/NeXT audio S16_BE format\n");
+		goto done;
+	}
+
+	sbc_init(&sbc, 0L);
+
+	switch (BE_INT(au_hdr->sample_rate)) {
+	case 16000:
+		sbc.frequency = SBC_FREQ_16000;
+		break;
+	case 32000:
+		sbc.frequency = SBC_FREQ_32000;
+		break;
+	case 44100:
+		sbc.frequency = SBC_FREQ_44100;
+		break;
+	case 48000:
+		sbc.frequency = SBC_FREQ_48000;
+		break;
+	}
+
+	sbc.subbands = subbands == 4 ? SBC_SB_4 : SBC_SB_8;
+
+	if (BE_INT(au_hdr->channels) == 1)
+		sbc.mode = SBC_MODE_MONO;
+	else if (joint)
+		sbc.mode = SBC_MODE_JOINT_STEREO;
+	else
+		sbc.mode = SBC_MODE_STEREO;
+
+	sbc.endian = SBC_BE;
+	count = BE_INT(au_hdr->data_size);
+	size = len - BE_INT(au_hdr->hdr_size);
+	memmove(input, input + BE_INT(au_hdr->hdr_size), size);
+
+	while (1) {
+		if (size < sizeof(input)) {
+			len = __read(fd, input + size, sizeof(input) - size);
+			if (len == 0)
+				break;
+
+			if (len < 0) {
+				perror("Can't read audio data");
+				break;
+			}
+
+			size += len;
+		}
+
+		len = sbc_encode(&sbc, input, size, output, sizeof(output),
+					&encoded);
+		if (len < size)
+			memmove(input, input + len, size - len);
+
+		size -= len;
+
+		len = __write(fileno(stdout), output, encoded);
+		if (len == 0)
+			break;
+
+		if (len < 0 || len != encoded) {
+			perror("Can't write SBC output");
+			break;
+		}
+	}
+
+	sbc_finish(&sbc);
+
+done:
+	if (fd > fileno(stderr))
+		close(fd);
+}
+
+static void usage(void)
+{
+	printf("SBC encoder utility ver %s\n", VERSION);
+	printf("Copyright (c) 2004-2008  Marcel Holtmann\n\n");
+
+	printf("Usage:\n"
+		"\tsbcenc [options] file(s)\n"
+		"\n");
+
+	printf("Options:\n"
+		"\t-h, --help           Display help\n"
+		"\t-v, --verbose        Verbose mode\n"
+		"\t-s, --subbands       Number of subbands to use (4 or 8)\n"
+		"\t-j, --joint          Joint stereo\n"
+		"\n");
+}
+
+static struct option main_options[] = {
+	{ "help",	0, 0, 'h' },
+	{ "verbose",	0, 0, 'v' },
+	{ "subbands",	1, 0, 's' },
+	{ "joint",	0, 0, 'j' },
+	{ 0, 0, 0, 0 }
+};
+
+int main(int argc, char *argv[])
+{
+	int i, opt, verbose = 0, subbands = 8, joint = 0;
+
+	while ((opt = getopt_long(argc, argv, "+hvs:j", main_options, NULL)) != -1) {
+		switch(opt) {
+		case 'h':
+			usage();
+			exit(0);
+
+		case 'v':
+			verbose = 1;
+			break;
+
+		case 's':
+			subbands = atoi(strdup(optarg));
+			if (subbands != 8 && subbands != 4) {
+				fprintf(stderr, "Invalid subbands %d!\n",
+						subbands);
+				exit(1);
+			}
+			break;
+
+		case 'j':
+			joint = 1;
+			break;
+
+		default:
+			exit(1);
+		}
+	}
+
+	argc -= optind;
+	argv += optind;
+	optind = 0;
+
+	if (argc < 1) {
+		usage();
+		exit(1);
+	}
+
+	for (i = 0; i < argc; i++)
+		encode(argv[i], subbands, joint);
+
+	return 0;
+}
diff --git a/sbc/sbcinfo.c b/sbc/sbcinfo.c
new file mode 100644
index 0000000..c1f0631
--- /dev/null
+++ b/sbc/sbcinfo.c
@@ -0,0 +1,319 @@
+/*
+ *
+ *  Bluetooth low-complexity, subband codec (SBC) library
+ *
+ *  Copyright (C) 2004-2008  Marcel Holtmann <marcel@holtmann.org>
+ *
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program; if not, write to the Free Software
+ *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+#ifdef HAVE_CONFIG_H
+#include <config.h>
+#endif
+
+#include <stdio.h>
+#include <errno.h>
+#include <fcntl.h>
+#include <unistd.h>
+#include <stdlib.h>
+#include <stdint.h>
+#include <string.h>
+#include <libgen.h>
+
+#if __BYTE_ORDER == __LITTLE_ENDIAN
+struct sbc_frame_hdr {
+	uint8_t syncword:8;		/* Sync word */
+	uint8_t subbands:1;		/* Subbands */
+	uint8_t allocation_method:1;	/* Allocation method */
+	uint8_t channel_mode:2;		/* Channel mode */
+	uint8_t blocks:2;		/* Blocks */
+	uint8_t sampling_frequency:2;	/* Sampling frequency */
+	uint8_t bitpool:8;		/* Bitpool */
+	uint8_t crc_check:8;		/* CRC check */
+} __attribute__ ((packed));
+#elif __BYTE_ORDER == __BIG_ENDIAN
+struct sbc_frame_hdr {
+	uint8_t syncword:8;		/* Sync word */
+	uint8_t sampling_frequency:2;	/* Sampling frequency */
+	uint8_t blocks:2;		/* Blocks */
+	uint8_t channel_mode:2;		/* Channel mode */
+	uint8_t allocation_method:1;	/* Allocation method */
+	uint8_t subbands:1;		/* Subbands */
+	uint8_t bitpool:8;		/* Bitpool */
+	uint8_t crc_check:8;		/* CRC check */
+} __attribute__ ((packed));
+#else
+#error "Unknown byte order"
+#endif
+
+static int calc_frame_len(struct sbc_frame_hdr *hdr)
+{
+	int tmp, nrof_subbands, nrof_blocks;
+
+	nrof_subbands = (hdr->subbands + 1) * 4;
+	nrof_blocks = (hdr->blocks + 1) * 4;
+
+	switch (hdr->channel_mode) {
+	case 0x00:
+		nrof_subbands /= 2;
+		tmp = nrof_blocks * hdr->bitpool;
+		break;
+	case 0x01:
+		tmp = nrof_blocks * hdr->bitpool * 2;
+		break;
+	case 0x02:
+		tmp = nrof_blocks * hdr->bitpool;
+		break;
+	case 0x03:
+		tmp = nrof_blocks * hdr->bitpool + nrof_subbands;
+		break;
+	default:
+		return 0;
+	}
+
+	return (nrof_subbands + ((tmp + 7) / 8));
+}
+
+static double calc_bit_rate(struct sbc_frame_hdr *hdr)
+{
+	int nrof_subbands, nrof_blocks;
+	double f;
+
+	nrof_subbands = (hdr->subbands + 1) * 4;
+	nrof_blocks = (hdr->blocks + 1) * 4;
+
+	switch (hdr->sampling_frequency) {
+	case 0:
+		f = 16;
+		break;
+	case 1:
+		f = 32;
+		break;
+	case 2:
+		f = 44.1;
+		break;
+	case 3:
+		f = 48;
+		break;
+	default:
+		return 0;
+	}
+
+	return ((8 * (calc_frame_len(hdr) + 4) * f) /
+			(nrof_subbands * nrof_blocks));
+}
+
+static char *freq2str(uint8_t freq)
+{
+	switch (freq) {
+	case 0:
+		return "16 kHz";
+	case 1:
+		return "32 kHz";
+	case 2:
+		return "44.1 kHz";
+	case 3:
+		return "48 kHz";
+	default:
+		return "Unknown";
+	}
+}
+
+static char *mode2str(uint8_t mode)
+{
+	switch (mode) {
+	case 0:
+		return "Mono";
+	case 1:
+		return "Dual Channel";
+	case 2:
+		return "Stereo";
+	case 3:
+		return "Joint Stereo";
+	default:
+		return "Unknown";
+	}
+}
+
+static ssize_t __read(int fd, void *buf, size_t count)
+{
+	ssize_t len, pos = 0;
+
+	while (count > 0) {
+		len = read(fd, buf + pos, count);
+		if (len <= 0)
+			return len;
+
+		count -= len;
+		pos   += len;
+	}
+
+	return pos;
+}
+
+#define SIZE 32
+
+static int analyze_file(char *filename)
+{
+	struct sbc_frame_hdr hdr;
+	unsigned char buf[64];
+	double rate;
+	int bitpool[SIZE], frame_len[SIZE];
+	int subbands, blocks, freq, mode, method;
+	int n, p1, p2, fd, len, size, count, num;
+
+	if (strcmp(filename, "-")) {
+		printf("Filename\t\t%s\n", basename(filename));
+
+		fd = open(filename, O_RDONLY);
+		if (fd < 0) {
+			perror("Can't open file");
+			return -1;
+		}
+	} else
+		fd = fileno(stdin);
+
+	len = __read(fd, &hdr, sizeof(hdr));
+	if (len != sizeof(hdr) || hdr.syncword != 0x9c) {
+		fprintf(stderr, "Not a SBC audio file\n");
+		return -1;
+	}
+
+	subbands = (hdr.subbands + 1) * 4;
+	blocks = (hdr.blocks + 1) * 4;
+	freq = hdr.sampling_frequency;
+	mode = hdr.channel_mode;
+	method = hdr.allocation_method;
+
+	count = calc_frame_len(&hdr);
+
+	bitpool[0] = hdr.bitpool;
+	frame_len[0] = count + 4;
+
+	for (n = 1; n < SIZE; n++) {
+		bitpool[n] = 0;
+		frame_len[n] = 0;
+	}
+
+	if (lseek(fd, 0, SEEK_SET) < 0) {
+		num = 1;
+		rate = calc_bit_rate(&hdr);
+		while (count) {
+			size = count > sizeof(buf) ? sizeof(buf) : count;
+			len = __read(fd, buf, size);
+			if (len < 0)
+				break;
+			count -= len;
+		}
+	} else {
+		num = 0;
+		rate = 0;
+	}
+
+	while (1) {
+		len = __read(fd, &hdr, sizeof(hdr));
+		if (len < 0) {
+			fprintf(stderr, "Unable to read frame header"
+					" (error %d)\n", errno);
+			break;
+		}
+
+		if (len == 0)
+			break;
+
+		if (len < sizeof(hdr) || hdr.syncword != 0x9c) {
+			fprintf(stderr, "Corrupted SBC stream "
+					"(len %d syncword 0x%02x)\n",
+					len, hdr.syncword);
+			break;
+		}
+
+		count = calc_frame_len(&hdr);
+		len = count + 4;
+
+		p1 = -1;
+		p2 = -1;
+		for (n = 0; n < SIZE; n++) {
+			if (p1 < 0 && (bitpool[n] == 0 || bitpool[n] == hdr.bitpool))
+				p1 = n;
+			if (p2 < 0 && (frame_len[n] == 0 || frame_len[n] == len))
+				p2 = n;
+		}
+		if (p1 >= 0)
+			bitpool[p1] = hdr.bitpool;
+		if (p2 >= 0)
+			frame_len[p2] = len;
+
+		while (count) {
+			size = count > sizeof(buf) ? sizeof(buf) : count;
+
+			len = __read(fd, buf, size);
+			if (len != size) {
+				fprintf(stderr, "Unable to read frame data "
+						"(error %d)\n", errno);
+				break;
+			}
+
+			count -= len;
+		}
+
+		rate += calc_bit_rate(&hdr);
+		num++;
+	}
+
+	printf("Subbands\t\t%d\n", subbands);
+	printf("Block length\t\t%d\n", blocks);
+	printf("Sampling frequency\t%s\n", freq2str(freq));
+	printf("Channel mode\t\t%s\n", mode2str(hdr.channel_mode));
+	printf("Allocation method\t%s\n", method ? "SNR" : "Loudness");
+	printf("Bitpool\t\t\t%d", bitpool[0]);
+	for (n = 1; n < SIZE; n++)
+		if (bitpool[n] > 0)
+			printf(", %d", bitpool[n]);
+	printf("\n");
+	printf("Number of frames\t%d\n", num);
+	printf("Frame length\t\t%d", frame_len[0]);
+	for (n = 1; n < SIZE; n++)
+		if (frame_len[n] > 0)
+			printf(", %d", frame_len[n]);
+	printf(" Bytes\n");
+	if (num > 0)
+		printf("Bit rate\t\t%.3f kbps\n", rate / num);
+
+	if (fd > fileno(stderr))
+		close(fd);
+
+	printf("\n");
+
+	return 0;
+}
+
+int main(int argc, char *argv[])
+{
+	int i;
+
+	if (argc < 2) {
+		fprintf(stderr, "Usage: sbcinfo <file>\n");
+		exit(1);
+	}
+
+	for (i = 0; i < argc - 1; i++)
+		if (analyze_file(argv[i + 1]) < 0)
+			exit(1);
+
+	return 0;
+}
diff --git a/sbc/sbctester.c b/sbc/sbctester.c
new file mode 100644
index 0000000..260f34a
--- /dev/null
+++ b/sbc/sbctester.c
@@ -0,0 +1,357 @@
+/*
+ *
+ *  Bluetooth low-complexity, subband codec (SBC) library
+ *
+ *  Copyright (C) 2007-2008  Marcel Holtmann <marcel@holtmann.org>
+ *  Copyright (C) 2007-2008  Frederic Dalleau <fdalleau@free.fr>
+ *
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program; if not, write to the Free Software
+ *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+#ifdef HAVE_CONFIG_H
+#include <config.h>
+#endif
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <sndfile.h>
+#include <math.h>
+#include <string.h>
+
+#define MAXCHANNELS 2
+#define DEFACCURACY 7
+
+static double sampletobits(short sample16, int verbose)
+{
+	double bits = 0;
+	unsigned short bit;
+	int i;
+
+	if (verbose)
+		printf("===> sampletobits(%hd, %04hX)\n", sample16, sample16);
+
+	/* Bit 0 is MSB */
+	if (sample16 < 0)
+		bits = -1;
+
+	if (verbose)
+		printf("%d", (sample16 < 0) ? 1 : 0);
+
+	/* Bit 15 is LSB */
+	for (i = 1; i < 16; i++) {
+		bit = (unsigned short) sample16;
+		bit >>= 15 - i;
+		bit %= 2;
+
+		if (verbose)
+			printf("%d", bit);
+
+		if (bit)
+			bits += (1.0 / pow(2.0, i));
+	}
+
+	if (verbose)
+		printf("\n");
+
+	return bits;
+}
+
+static int calculate_rms_level(SNDFILE * sndref, SF_INFO * infosref,
+				SNDFILE * sndtst, SF_INFO * infostst,
+						int accuracy, char *csvname)
+{
+	short refsample[MAXCHANNELS], tstsample[MAXCHANNELS];
+	double refbits, tstbits;
+	double rms_accu[MAXCHANNELS];
+	double rms_level[MAXCHANNELS];
+	double rms_limit = 1.0 / (pow(2.0, accuracy - 1) * pow(12.0, 0.5));
+	FILE *csv = NULL;
+	int i, j, r1, r2, verdict;
+
+	if (csvname)
+		csv = fopen(csvname, "wt");
+
+	if (csv) {
+		fprintf(csv, "num;");
+		for (j = 0; j < infostst->channels; j++)
+			fprintf(csv, "ref channel %d;tst channel %d;", j, j);
+		fprintf(csv, "\r\n");
+	}
+
+	sf_seek(sndref, 0, SEEK_SET);
+	sf_seek(sndtst, 0, SEEK_SET);
+
+	memset(rms_accu, 0, sizeof(rms_accu));
+	memset(rms_level, 0, sizeof(rms_level));
+
+	for (i = 0; i < infostst->frames; i++) {
+		if (csv)
+			fprintf(csv, "%d;", i);
+
+		r1 = sf_read_short(sndref, refsample, infostst->channels);
+		if (r1 != infostst->channels) {
+			printf("Failed to read reference data: %s "
+					"(r1=%d, channels=%d)",
+					sf_strerror(sndref), r1,
+					infostst->channels);
+			if (csv)
+				fclose(csv);
+			return -1;
+		}
+
+		r2 = sf_read_short(sndtst, tstsample, infostst->channels);
+		if (r2 != infostst->channels) {
+			printf("Failed to read test data: %s "
+					"(r2=%d, channels=%d)\n",
+					sf_strerror(sndtst), r2,
+					infostst->channels);
+			if (csv)
+				fclose(csv);
+			return -1;
+		}
+
+		for (j = 0; j < infostst->channels; j++) {
+			if (csv)
+				fprintf(csv, "%d;%d;", refsample[j],
+						tstsample[j]);
+
+			refbits = sampletobits(refsample[j], 0);
+			tstbits = sampletobits(tstsample[j], 0);
+
+			rms_accu[j] += pow(tstbits - refbits, 2.0);
+		}
+
+		if (csv)
+			fprintf(csv, "\r\n");
+	}
+
+	printf("Limit: %f\n", rms_limit);
+
+	for (j = 0; j < infostst->channels; j++) {
+		printf("Channel %d\n", j);
+		printf("Accumulated %f\n", rms_accu[j]);
+		rms_accu[j] /= (double) infostst->frames;
+		printf("Accumulated / %f = %f\n", (double) infostst->frames,
+				rms_accu[j]);
+		rms_level[j] = sqrt(rms_accu[j]);
+		printf("Level = %f (%f x %f = %f)\n",
+				rms_level[j], rms_level[j], rms_level[j],
+						rms_level[j] * rms_level[j]);
+	}
+
+	verdict = 1;
+
+	for (j = 0; j < infostst->channels; j++) {
+		printf("Channel %d: %f\n", j, rms_level[j]);
+
+		if (rms_level[j] > rms_limit)
+			verdict = 0;
+	}
+
+	printf("%s return %d\n", __FUNCTION__, verdict);
+
+	return verdict;
+}
+
+static int check_absolute_diff(SNDFILE * sndref, SF_INFO * infosref,
+				SNDFILE * sndtst, SF_INFO * infostst,
+				int accuracy)
+{
+	short refsample[MAXCHANNELS], tstsample[MAXCHANNELS];
+	short refmax[MAXCHANNELS], tstmax[MAXCHANNELS];
+	double refbits, tstbits;
+	double rms_absolute = 1.0 / (pow(2, accuracy - 2));
+	double calc_max[MAXCHANNELS];
+	int calc_count = 0;
+	short r1, r2;
+	double cur_diff;
+	int i, j, verdict;
+
+	memset(&refmax, 0, sizeof(refmax));
+	memset(&tstmax, 0, sizeof(tstmax));
+	memset(&calc_max, 0, sizeof(calc_max));
+	memset(&refsample, 0, sizeof(refsample));
+	memset(&tstsample, 0, sizeof(tstsample));
+
+	sf_seek(sndref, 0, SEEK_SET);
+	sf_seek(sndtst, 0, SEEK_SET);
+
+	verdict = 1;
+
+	printf("Absolute max: %f\n", rms_absolute);
+	for (i = 0; i < infostst->frames; i++) {
+		r1 = sf_read_short(sndref, refsample, infostst->channels);
+
+		if (r1 != infostst->channels) {
+			printf("Failed to read reference data: %s "
+					"(r1=%d, channels=%d)",
+					sf_strerror(sndref), r1,
+					infostst->channels);
+			return -1;
+		}
+
+		r2 = sf_read_short(sndtst, tstsample, infostst->channels);
+		if (r2 != infostst->channels) {
+			printf("Failed to read test data: %s "
+					"(r2=%d, channels=%d)\n",
+					sf_strerror(sndtst), r2,
+					infostst->channels);
+			return -1;
+		}
+
+		for (j = 0; j < infostst->channels; j++) {
+			refbits = sampletobits(refsample[j], 0);
+			tstbits = sampletobits(tstsample[j], 0);
+
+			cur_diff = fabs(tstbits - refbits);
+
+			if (cur_diff > rms_absolute) {
+				calc_count++;
+				/* printf("Channel %d exceeded : fabs(%f - %f) = %f > %f\n", j, tstbits, refbits, cur_diff, rms_absolute); */
+				verdict = 0;
+			}
+
+			if (cur_diff > calc_max[j]) {
+				calc_max[j] = cur_diff;
+				refmax[j] = refsample[j];
+				tstmax[j] = tstsample[j];
+			}
+		}
+	}
+
+	for (j = 0; j < infostst->channels; j++) {
+		printf("Calculated max: %f (%hd-%hd=%hd)\n",
+			calc_max[j], tstmax[j], refmax[j],
+			tstmax[j] - refmax[j]);
+	}
+
+	printf("%s return %d\n", __FUNCTION__, verdict);
+
+	return verdict;
+}
+
+static void usage()
+{
+	printf("SBC conformance test ver %s\n", VERSION);
+	printf("Copyright (c) 2007-2008 Marcel Holtmann\n");
+	printf("Copyright (c) 2007-2008 Frederic Dalleau\n\n");
+
+	printf("Usage:\n"
+		"\tsbctester reference.wav checkfile.wav\n"
+		"\tsbctester integer\n"
+		"\n");
+
+	printf("To test the encoder:\n");
+	printf("\tUse a reference codec to encode original.wav to reference.sbc\n");
+	printf("\tUse sbcenc to encode original.wav to checkfile.sbc\n");
+	printf("\tDecode both file using the reference decoder\n");
+	printf("\tRun sbctester with these two wav files to get the result\n\n");
+
+	printf("\tA file called out.csv is generated to use the data in a\n");
+	printf("\tspreadsheet application or database.\n\n");
+}
+
+int main(int argc, char *argv[])
+{
+	SNDFILE *sndref = NULL;
+	SNDFILE *sndtst = NULL;
+	SF_INFO infosref;
+	SF_INFO infostst;
+	char *ref;
+	char *tst;
+	int pass_rms, pass_absolute, pass, accuracy;
+
+	if (argc == 2) {
+		double db;
+
+		printf("Test sampletobits\n");
+		db = sampletobits((short) atoi(argv[1]), 1);
+		printf("db = %f\n", db);
+		exit(0);
+	}
+
+	if (argc < 3) {
+		usage();
+		exit(1);
+	}
+
+	ref = argv[1];
+	tst = argv[2];
+
+	printf("opening reference %s\n", ref);
+
+	sndref = sf_open(ref, SFM_READ, &infosref);
+	if (!sndref) {
+		printf("Failed to open reference file\n");
+		exit(1);
+	}
+
+	printf("opening testfile %s\n", tst);
+	sndtst = sf_open(tst, SFM_READ, &infostst);
+	if (!sndtst) {
+		printf("Failed to open test file\n");
+		sf_close(sndref);
+		exit(1);
+	}
+
+	printf("reference:\n\t%d frames,\n\t%d hz,\n\t%d channels\n",
+		(int) infosref.frames, (int) infosref.samplerate,
+		(int) infosref.channels);
+	printf("testfile:\n\t%d frames,\n\t%d hz,\n\t%d channels\n",
+		(int) infostst.frames, (int) infostst.samplerate,
+		(int) infostst.channels);
+
+	/* check number of channels */
+	if (infosref.channels > 2 || infostst.channels > 2) {
+		printf("Too many channels\n");
+		goto error;
+	}
+
+	/* compare number of samples */
+	if (infosref.samplerate != infostst.samplerate ||
+				infosref.channels != infostst.channels) {
+		printf("Cannot compare files with different charasteristics\n");
+		goto error;
+	}
+
+	accuracy = DEFACCURACY;
+	printf("Accuracy: %d\n", accuracy);
+
+	/* Condition 1 rms level */
+	pass_rms = calculate_rms_level(sndref, &infosref, sndtst, &infostst,
+					accuracy, "out.csv");
+	if (pass_rms < 0)
+		goto error;
+
+	/* Condition 2 absolute difference */
+	pass_absolute = check_absolute_diff(sndref, &infosref, sndtst,
+						&infostst, accuracy);
+	if (pass_absolute < 0)
+		goto error;
+
+	/* Verdict */
+	pass = pass_rms && pass_absolute;
+	printf("Verdict: %s\n", pass ? "pass" : "fail");
+
+	return 0;
+
+error:
+	sf_close(sndref);
+	sf_close(sndtst);
+
+	exit(1);
+}