From 62649d833bdb35eb86cff0c9c16e96e45042df91 Mon Sep 17 00:00:00 2001
From: Chris Waddell <quic_cwaddell@quicinc.com>
Date: Wed, 19 Oct 2022 17:18:43 +0100
Subject: [PATCH] Add encoder for aptX and encoder for aptX HD source code.
Bug: 226572369
Test: In follow-up CL
Change-Id: I3cc67c9f1466fc8518c81160fbf3034b7ae6807b
---
.../encoder_for_aptx/include/aptXbtenc.h | 52 +
.../embdrv/encoder_for_aptx/src/AptxEncoder.h | 80 +
.../encoder_for_aptx/src/AptxParameters.h | 247 +++
.../embdrv/encoder_for_aptx/src/AptxTables.h | 424 +++++
system/embdrv/encoder_for_aptx/src/CBStruct.h | 28 +
.../encoder_for_aptx/src/CodewordPacker.h | 59 +
.../encoder_for_aptx/src/DitherGenerator.h | 102 ++
.../encoder_for_aptx/src/ProcessSubband.c | 43 +
system/embdrv/encoder_for_aptx/src/Qmf.h | 142 ++
system/embdrv/encoder_for_aptx/src/QmfConv.c | 327 ++++
.../encoder_for_aptx/src/QuantiseDifference.c | 711 +++++++++
.../embdrv/encoder_for_aptx/src/Quantiser.h | 27 +
.../encoder_for_aptx/src/SubbandFunctions.h | 184 +++
.../src/SubbandFunctionsCommon.h | 515 +++++++
.../encoder_for_aptx/src/SyncInserter.h | 217 +++
.../embdrv/encoder_for_aptx/src/aptXbtenc.c | 218 +++
.../embdrv/encoder_for_aptx/src/swversion.h | 8 +
.../encoder_for_aptxhd/include/aptXHDbtenc.h | 49 +
.../encoder_for_aptxhd/src/AptxEncoder.h | 87 ++
.../encoder_for_aptxhd/src/AptxParameters.h | 248 +++
.../encoder_for_aptxhd/src/AptxTables.h | 1365 +++++++++++++++++
.../embdrv/encoder_for_aptxhd/src/CBStruct.h | 28 +
.../encoder_for_aptxhd/src/CodewordPacker.h | 50 +
.../encoder_for_aptxhd/src/DitherGenerator.h | 101 ++
.../encoder_for_aptxhd/src/ProcessSubband.c | 43 +
system/embdrv/encoder_for_aptxhd/src/Qmf.h | 150 ++
.../embdrv/encoder_for_aptxhd/src/QmfConv.c | 337 ++++
.../src/QuantiseDifference.c | 763 +++++++++
.../embdrv/encoder_for_aptxhd/src/Quantiser.h | 27 +
.../encoder_for_aptxhd/src/SubbandFunctions.h | 183 +++
.../src/SubbandFunctionsCommon.h | 520 +++++++
.../encoder_for_aptxhd/src/SyncInserter.h | 114 ++
.../encoder_for_aptxhd/src/aptXHDbtenc.c | 179 +++
.../embdrv/encoder_for_aptxhd/src/swversion.h | 8 +
34 files changed, 7636 insertions(+)
create mode 100644 system/embdrv/encoder_for_aptx/include/aptXbtenc.h
create mode 100644 system/embdrv/encoder_for_aptx/src/AptxEncoder.h
create mode 100644 system/embdrv/encoder_for_aptx/src/AptxParameters.h
create mode 100644 system/embdrv/encoder_for_aptx/src/AptxTables.h
create mode 100644 system/embdrv/encoder_for_aptx/src/CBStruct.h
create mode 100644 system/embdrv/encoder_for_aptx/src/CodewordPacker.h
create mode 100644 system/embdrv/encoder_for_aptx/src/DitherGenerator.h
create mode 100644 system/embdrv/encoder_for_aptx/src/ProcessSubband.c
create mode 100644 system/embdrv/encoder_for_aptx/src/Qmf.h
create mode 100644 system/embdrv/encoder_for_aptx/src/QmfConv.c
create mode 100644 system/embdrv/encoder_for_aptx/src/QuantiseDifference.c
create mode 100644 system/embdrv/encoder_for_aptx/src/Quantiser.h
create mode 100644 system/embdrv/encoder_for_aptx/src/SubbandFunctions.h
create mode 100644 system/embdrv/encoder_for_aptx/src/SubbandFunctionsCommon.h
create mode 100644 system/embdrv/encoder_for_aptx/src/SyncInserter.h
create mode 100644 system/embdrv/encoder_for_aptx/src/aptXbtenc.c
create mode 100644 system/embdrv/encoder_for_aptx/src/swversion.h
create mode 100644 system/embdrv/encoder_for_aptxhd/include/aptXHDbtenc.h
create mode 100644 system/embdrv/encoder_for_aptxhd/src/AptxEncoder.h
create mode 100644 system/embdrv/encoder_for_aptxhd/src/AptxParameters.h
create mode 100644 system/embdrv/encoder_for_aptxhd/src/AptxTables.h
create mode 100644 system/embdrv/encoder_for_aptxhd/src/CBStruct.h
create mode 100644 system/embdrv/encoder_for_aptxhd/src/CodewordPacker.h
create mode 100644 system/embdrv/encoder_for_aptxhd/src/DitherGenerator.h
create mode 100644 system/embdrv/encoder_for_aptxhd/src/ProcessSubband.c
create mode 100644 system/embdrv/encoder_for_aptxhd/src/Qmf.h
create mode 100644 system/embdrv/encoder_for_aptxhd/src/QmfConv.c
create mode 100644 system/embdrv/encoder_for_aptxhd/src/QuantiseDifference.c
create mode 100644 system/embdrv/encoder_for_aptxhd/src/Quantiser.h
create mode 100644 system/embdrv/encoder_for_aptxhd/src/SubbandFunctions.h
create mode 100644 system/embdrv/encoder_for_aptxhd/src/SubbandFunctionsCommon.h
create mode 100644 system/embdrv/encoder_for_aptxhd/src/SyncInserter.h
create mode 100644 system/embdrv/encoder_for_aptxhd/src/aptXHDbtenc.c
create mode 100644 system/embdrv/encoder_for_aptxhd/src/swversion.h
diff --git a/system/embdrv/encoder_for_aptx/include/aptXbtenc.h b/system/embdrv/encoder_for_aptx/include/aptXbtenc.h
new file mode 100644
index 00000000000..47aa29309c6
--- /dev/null
+++ b/system/embdrv/encoder_for_aptx/include/aptXbtenc.h
@@ -0,0 +1,52 @@
+/*------------------------------------------------------------------------------
+ *
+ * This file exposes a public interface to allow clients to invoke aptX
+ * encoding on 4 new PCM samples, generating 2 new codeword (one for the
+ * left channel and one for the right channel).
+ *
+ *----------------------------------------------------------------------------*/
+
+#ifndef APTXBTENC_H
+#define APTXBTENC_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#ifdef _DLLEXPORT
+#define APTXBTENCEXPORT __declspec(dllexport)
+#else
+#define APTXBTENCEXPORT
+#endif
+
+/* SizeofAptxbtenc returns the size (in byte) of the memory
+ * allocation required to store the state of the encoder */
+APTXBTENCEXPORT int SizeofAptxbtenc(void);
+
+/* aptxbtenc_version can be used to extract the version number
+ * of the aptX encoder */
+APTXBTENCEXPORT const char* aptxbtenc_version(void);
+
+/* aptxbtenc_init is used to initialise the encoder structure.
+ * _state should be a pointer to the encoder structure (stereo).
+ * endian represent the endianness of the output data
+ * (0=little endian. Big endian otherwise)
+ * The function returns 1 if an error occurred during the initialisation.
+ * The function returns 0 if no error occurred during the initialisation. */
+APTXBTENCEXPORT int aptxbtenc_init(void* _state, short endian);
+
+/* aptxbtenc_setsync_mode is used to initialise the sync mode in the encoder state structure.
+ * _state should be a pointer to the encoder structure (stereo, though strictly-speaking it is dual channel).
+ * 'sync_mode' is an enumerated type {stereo=0, dualmono=1, no_sync=2}
+ * The function returns 0 if no error occurred during the initialisation. */
+APTXBTENCEXPORT int aptxbtenc_setsync_mode(void* _state, int32_t sync_mode);
+
+/* StereoEncode will take 8 audio samples (16-bit per sample)
+ * and generate one 32-bit codeword with autosync inserted. */
+APTXBTENCEXPORT int aptxbtenc_encodestereo(void* _state, void* _pcmL, void* _pcmR, void* _buffer);
+
+#ifdef __cplusplus
+} // /extern "C"
+#endif
+
+#endif //APTXBTENC_H
diff --git a/system/embdrv/encoder_for_aptx/src/AptxEncoder.h b/system/embdrv/encoder_for_aptx/src/AptxEncoder.h
new file mode 100644
index 00000000000..81b75d04a5e
--- /dev/null
+++ b/system/embdrv/encoder_for_aptx/src/AptxEncoder.h
@@ -0,0 +1,80 @@
+/*------------------------------------------------------------------------------
+ *
+ * All declarations relevant for aptxEncode. This function allows clients
+ * to invoke bt-aptX encoding on 4 new PCM samples,
+ * generating 4 new quantised codes. A separate function allows the
+ * packing of the 4 codes into a 16-bit word.
+ *
+ *----------------------------------------------------------------------------*/
+
+#ifndef APTXENCODER_H
+#define APTXENCODER_H
+#ifdef _GCC
+ #pragma GCC visibility push(hidden)
+#endif
+
+#include "AptxParameters.h"
+#include "DitherGenerator.h"
+#include "Quantiser.h"
+#include "SubbandFunctionsCommon.h"
+#include "Qmf.h"
+
+
+ /* Function to carry out a single-channel aptX encode on 4 new PCM samples */
+XBT_INLINE_ void aptxEncode(int32_t pcm[4], Qmf_storage* Qmf_St, Encoder_data* EncoderDataPt)
+{
+ int32_t predVals[4];
+ int32_t qCodes[4];
+ int32_t aqmfOutputs[4];
+
+ /* Extract the previous predicted values and quantised codes into arrays */
+ for (int i = 0; i < 4; i++)
+ {
+ predVals[i] = EncoderDataPt->m_SubbandData[i].m_predData.m_predVal;
+ qCodes[i] = EncoderDataPt->m_qdata[i].qCode;
+ }
+
+ /* Update codeword history, then generate new dither values. */
+ EncoderDataPt->m_codewordHistory = xbtEncupdateCodewordHistory(qCodes, EncoderDataPt->m_codewordHistory);
+ EncoderDataPt->m_dithSyncRandBit = xbtEncgenerateDither(EncoderDataPt->m_codewordHistory, EncoderDataPt->m_ditherOutputs);
+
+ /* Run the analysis QMF */
+ QmfAnalysisFilter(pcm, Qmf_St, predVals, aqmfOutputs);
+
+ /* Run the quantiser for each subband */
+ quantiseDifferenceLL(aqmfOutputs[0], EncoderDataPt->m_ditherOutputs[0], EncoderDataPt->m_SubbandData[0].m_iqdata.delta, &EncoderDataPt->m_qdata[0]);
+ quantiseDifferenceLH(aqmfOutputs[1], EncoderDataPt->m_ditherOutputs[1], EncoderDataPt->m_SubbandData[1].m_iqdata.delta, &EncoderDataPt->m_qdata[1]);
+ quantiseDifferenceHL(aqmfOutputs[2], EncoderDataPt->m_ditherOutputs[2], EncoderDataPt->m_SubbandData[2].m_iqdata.delta, &EncoderDataPt->m_qdata[2]);
+ quantiseDifferenceHH(aqmfOutputs[3], EncoderDataPt->m_ditherOutputs[3], EncoderDataPt->m_SubbandData[3].m_iqdata.delta, &EncoderDataPt->m_qdata[3]);
+}
+
+XBT_INLINE_ void aptxPostEncode(Encoder_data* EncoderDataPt)
+{
+ /* Run the remaining subband processing for each subband */
+ /* Manual inlining on the 4 subband */
+ processSubbandLL(EncoderDataPt->m_qdata[0].qCode,
+ EncoderDataPt->m_ditherOutputs[0],
+ &EncoderDataPt->m_SubbandData[0],
+ &EncoderDataPt->m_SubbandData[0].m_iqdata);
+
+ processSubband(EncoderDataPt->m_qdata[1].qCode,
+ EncoderDataPt->m_ditherOutputs[1],
+ &EncoderDataPt->m_SubbandData[1],
+ &EncoderDataPt->m_SubbandData[1].m_iqdata);
+
+ processSubbandHL(EncoderDataPt->m_qdata[2].qCode,
+ EncoderDataPt->m_ditherOutputs[2],
+ &EncoderDataPt->m_SubbandData[2],
+ &EncoderDataPt->m_SubbandData[2].m_iqdata);
+
+ processSubband(EncoderDataPt->m_qdata[3].qCode,
+ EncoderDataPt->m_ditherOutputs[3],
+ &EncoderDataPt->m_SubbandData[3],
+ &EncoderDataPt->m_SubbandData[3].m_iqdata);
+}
+
+
+#ifdef _GCC
+ #pragma GCC visibility pop
+#endif
+#endif //APTXENCODER_H
diff --git a/system/embdrv/encoder_for_aptx/src/AptxParameters.h b/system/embdrv/encoder_for_aptx/src/AptxParameters.h
new file mode 100644
index 00000000000..f8a7d04532a
--- /dev/null
+++ b/system/embdrv/encoder_for_aptx/src/AptxParameters.h
@@ -0,0 +1,247 @@
+/*------------------------------------------------------------------------------
+ *
+ * General shared aptX parameters.
+ *
+ *----------------------------------------------------------------------------*/
+
+#ifndef APTXPARAMETERS_H
+#define APTXPARAMETERS_H
+#ifdef _GCC
+ #pragma GCC visibility push(hidden)
+#endif
+
+
+#include <stdint.h>
+#include "CBStruct.h"
+
+#if defined _MSC_VER
+ #define XBT_INLINE_ inline
+ #define _STDQMFOUTERCOEFF 1
+#elif defined __clang__
+ #define XBT_INLINE_ static inline
+ #define _STDQMFOUTERCOEFF 1
+#elif defined __GNUC__
+ #define XBT_INLINE_ inline
+ #define _STDQMFOUTERCOEFF 1
+#else
+ #define XBT_INLINE_ static
+ #define _STDQMFOUTERCOEFF 1
+#endif
+
+
+/* Signed saturate to a 24bit value */
+XBT_INLINE_ int32_t ssat24(int32_t val)
+{
+ if (val > 8388607) val = 8388607;
+ if (val < -8388608) val = -8388608;
+ return val;
+}
+
+typedef union u_reg64
+{
+ uint64_t u64;
+ int64_t s64;
+ struct s_u32
+ {
+#ifdef __BIGENDIAN
+ uint32_t h;
+ uint32_t l;
+#else
+ uint32_t l;
+ uint32_t h;
+#endif
+ } u32;
+
+ struct s_s32
+ {
+#ifdef __BIGENDIAN
+ int32_t h;
+ int32_t l;
+#else
+ int32_t l;
+ int32_t h;
+#endif
+ } s32;
+} reg64_t;
+
+typedef union u_reg32
+{
+ uint32_t u32;
+ int32_t s32;
+
+ struct s_u16
+ {
+#ifdef __BIGENDIAN
+ uint16_t h;
+ uint16_t l;
+#else
+ uint16_t l;
+ uint16_t h;
+#endif
+ } u16;
+ struct s_s16
+ {
+#ifdef __BIGENDIAN
+ int16_t h;
+ int16_t l;
+#else
+ int16_t l;
+ int16_t h;
+#endif
+ } s16;
+} reg32_t;
+
+/* Each aptX enc/dec round consumes/produces 4 PCM samples */
+static const uint32_t numPcmSamples = 4;
+
+/* Symbolic constants for PCM data indicies. */
+enum {FirstPcm=0, SecondPcm=1, ThirdPcm=2, FourthPcm=3};
+
+/* Symbolic constants for sync modes. */
+enum {stereo=0, dualmono=1, no_sync=2};
+
+/* Number of subbands is fixed at 4 */
+#define NUMSUBBANDS 4
+
+/* Symbolic constants for subband identification. */
+typedef enum {LL=0, LH=1, HL=2, HH=3} bands;
+
+/* Structure declaration to bind a set of subband parameters */
+typedef struct
+{
+ const int32_t* threshTable;
+ const int32_t* threshTable_sl1;
+ const int32_t* dithTable;
+ const int32_t* dithTable_sh1;
+ const int32_t* minusLambdaDTable;
+ const int32_t* incrTable;
+ int32_t numBits;
+ int32_t maxLogDelta;
+ int32_t minLogDelta;
+ int32_t numZeros;
+} SubbandParameters;
+
+/* Struct required for the polecoeffcalculator function of bt-aptX encoder and decoder*/
+/* Size of structure: 16 Bytes */
+typedef struct
+{
+ /* 2-tap delay line for previous sgn values */
+ reg32_t m_poleAdaptDelayLine;
+ /* 2 pole filter coeffs */
+ int32_t m_poleCoeff[2];
+} PoleCoeff_data;
+
+/* Struct required for the zerocoeffcalculator function of bt-aptX encoder and decoder*/
+/* Size of structure: 100 Bytes */
+typedef struct
+{
+ /* The zero filter length for this subband */
+ int32_t m_numZeros;
+ /* Maximum number of zeros for any subband is 24. */
+ /* 24 zero filter coeffs */
+ int32_t m_zeroCoeff[24];
+} ZeroCoeff_data;
+
+/* Struct required for the prediction filtering function of bt-aptX encoder and decoder*/
+/* Size of structure: 200+20=220 Bytes */
+typedef struct
+{
+ /* Number of zeros associated with this subband */
+ int32_t m_numZeros;
+ /* Zero data delay line (circular) */
+ circularBuffer m_zeroDelayLine;
+ /* 2-tap pole data delay line */
+ int32_t m_poleDelayLine[2];
+ /* Output from zero filter */
+ int32_t m_zeroVal;
+ /* Output from overall ARMA filter */
+ int32_t m_predVal;
+} Predictor_data;
+
+/* Struct required for the Quantisation function of bt-aptX encoder and decoder*/
+/* Size of structure: 24 Bytes */
+typedef struct
+{
+ /* Number of bits in the quantised code for this subband */
+ int32_t codeBits;
+ /* Pointer to threshold table */
+ const int32_t* thresholdTablePtr;
+ const int32_t* thresholdTablePtr_sl1;
+ /* Pointer to dither table */
+ const int32_t* ditherTablePtr;
+ /* Pointer to minus Lambda table */
+ const int32_t* minusLambdaDTable;
+ /* Output quantised code */
+ int32_t qCode;
+ /* Alternative quantised code for sync purposes */
+ int32_t altQcode;
+ /* Penalty associated with choosing alternative code */
+ int32_t distPenalty;
+} Quantiser_data;
+
+/* Struct required for the inverse Quantisation function of bt-aptX encoder and decoder*/
+/* Size of structure: 32 Bytes */
+typedef struct
+{
+ /* Pointer to threshold table */
+ const int32_t* thresholdTablePtr;
+ const int32_t* thresholdTablePtr_sl1;
+ /* Pointer to dither table */
+ const int32_t* ditherTablePtr_sf1;
+ /* Pointer to increment table */
+ const int32_t* incrTablePtr;
+ /* Upper and lower bounds for logDelta */
+ int32_t maxLogDelta;
+ int32_t minLogDelta;
+ /* Delta (quantisation step size */
+ int32_t delta;
+ /* Delta, expressed as a log base 2 */
+ uint16_t logDelta;
+ /* Output dequantised signal */
+ int32_t invQ;
+ /* pointer to IQuant_tableLogT */
+ const int32_t* iquantTableLogPtr;
+} IQuantiser_data;
+
+/* Subband data structure bt-aptX encoder*/
+/* Size of structure: 116+220+32= 368 Bytes */
+typedef struct
+{
+ /* Subband processing consists of inverse quantisation, predictor
+ * coefficient update, and predictor filtering. */
+ ZeroCoeff_data m_ZeroCoeffData;
+ PoleCoeff_data m_PoleCoeffData;
+ /* structure holding the data associated with the predictor */
+ Predictor_data m_predData;
+ /* iqdata holds the data associated with the instance of inverse quantiser */
+ IQuantiser_data m_iqdata;
+} Subband_data;
+
+/* Encoder data structure bt-aptX encoder*/
+/* Size of structure: 368*4+24+4*24 = 1592 Bytes */
+typedef struct
+{
+ /* Subband processing consists of inverse quantisation, predictor
+ * coefficient update, and predictor filtering. */
+ Subband_data m_SubbandData[4];
+ int32_t m_codewordHistory;
+ int32_t m_dithSyncRandBit;
+ int32_t m_ditherOutputs[4];
+ /* structure holding data values for this quantiser */
+ Quantiser_data m_qdata[4];
+} Encoder_data;
+
+/* Number of predictor pole filter coefficients is fixed at 2 for all subbands */
+static const uint32_t numPoleFilterCoeffs = 2;
+
+/* Subband-specific number of predictor zero filter coefficients. */
+static const uint32_t numZeroFilterCoeffs[4] = {24, 12, 6, 12};
+
+/* Delta is scaled by 4 positions within the quantiser and inverse quantiser. */
+static const uint32_t deltaScale = 4;
+
+
+#ifdef _GCC
+ #pragma GCC visibility pop
+#endif
+#endif //APTXPARAMETERS_H
diff --git a/system/embdrv/encoder_for_aptx/src/AptxTables.h b/system/embdrv/encoder_for_aptx/src/AptxTables.h
new file mode 100644
index 00000000000..e357b50e664
--- /dev/null
+++ b/system/embdrv/encoder_for_aptx/src/AptxTables.h
@@ -0,0 +1,424 @@
+/*------------------------------------------------------------------------------
+ *
+ * All table definitions used for the quantizer.
+ *
+ *----------------------------------------------------------------------------*/
+
+#ifndef APTXTABLES_H
+#define APTXTABLES_H
+
+
+#include "AptxParameters.h"
+
+
+/* Quantisation threshold, logDelta increment and dither tables for 2-bit codes */
+static const int32_t dq2bit16_sl1[3] =
+{
+ -194080,
+ 194080,
+ 890562,
+};
+
+static const int32_t q2incr16[3] =
+{
+ 0,
+ -33,
+ 136,
+};
+
+static const int32_t dq2dith16_sf1[3] =
+{
+ 194080,
+ 194080,
+ 502402,
+};
+
+static const int32_t dq2mLamb16[2] =
+{
+ 0,
+ -77081,
+};
+
+/* Quantisation threshold, logDelta increment and dither tables for 3-bit codes */
+static const int32_t dq3bit16_sl1[5] =
+{
+ -163006,
+ 163006,
+ 542708,
+ 1120554,
+ 2669238,
+};
+
+static const int32_t q3incr16[5] =
+{
+ 0,
+ -8,
+ 33,
+ 95,
+ 262,
+};
+
+static const int32_t dq3dith16_sf1[5] =
+{
+ 163006,
+ 163006,
+ 216698,
+ 361148,
+ 1187538,
+};
+
+static const int32_t dq3mLamb16[4] =
+{
+ 0,
+ -13423,
+ -36113,
+ -206598,
+};
+
+/* Quantisation threshold, logDelta increment and dither tables for 4-bit codes */
+static const int32_t dq4bit16_sl1[9] =
+{
+ -89806,
+ 89806,
+ 278502,
+ 494338,
+ 759442,
+ 1113112,
+ 1652322,
+ 2720256,
+ 5190186,
+};
+
+static const int32_t q4incr16[9] =
+{
+ 0,
+ -14,
+ 6,
+ 29,
+ 58,
+ 96,
+ 154,
+ 270,
+ 521,
+};
+
+static const int32_t dq4dith16_sf1[9] =
+{
+ 89806,
+ 89806,
+ 98890,
+ 116946,
+ 148158,
+ 205512,
+ 333698,
+ 734236,
+ 1735696,
+};
+
+static const int32_t dq4mLamb16[8] =
+{
+ 0,
+ -2271,
+ -4514,
+ -7803,
+ -14339,
+ -32047,
+ -100135,
+ -250365,
+};
+
+/* Quantisation threshold, logDelta increment and dither tables for 7-bit codes */
+static const int32_t dq7bit16_sl1[65] =
+{
+ -9948,
+ 9948,
+ 29860,
+ 49808,
+ 69822,
+ 89926,
+ 110144,
+ 130502,
+ 151026,
+ 171738,
+ 192666,
+ 213832,
+ 235264,
+ 256982,
+ 279014,
+ 301384,
+ 324118,
+ 347244,
+ 370790,
+ 394782,
+ 419250,
+ 444226,
+ 469742,
+ 495832,
+ 522536,
+ 549890,
+ 577936,
+ 606720,
+ 636290,
+ 666700,
+ 698006,
+ 730270,
+ 763562,
+ 797958,
+ 833538,
+ 870398,
+ 908640,
+ 948376,
+ 989740,
+ 1032874,
+ 1077948,
+ 1125150,
+ 1174700,
+ 1226850,
+ 1281900,
+ 1340196,
+ 1402156,
+ 1468282,
+ 1539182,
+ 1615610,
+ 1698514,
+ 1789098,
+ 1888944,
+ 2000168,
+ 2125700,
+ 2269750,
+ 2438670,
+ 2642660,
+ 2899462,
+ 3243240,
+ 3746078,
+ 4535138,
+ 5664098,
+ 7102424,
+ 8897462,
+};
+
+static const int32_t q7incr16[65] =
+{
+ 0,
+ -21,
+ -19,
+ -17,
+ -15,
+ -12,
+ -10,
+ -8,
+ -6,
+ -4,
+ -1,
+ 1,
+ 3,
+ 6,
+ 8,
+ 10,
+ 13,
+ 15,
+ 18,
+ 20,
+ 23,
+ 26,
+ 29,
+ 31,
+ 34,
+ 37,
+ 40,
+ 43,
+ 47,
+ 50,
+ 53,
+ 57,
+ 60,
+ 64,
+ 68,
+ 72,
+ 76,
+ 80,
+ 85,
+ 89,
+ 94,
+ 99,
+ 105,
+ 110,
+ 116,
+ 123,
+ 129,
+ 136,
+ 144,
+ 152,
+ 161,
+ 171,
+ 182,
+ 194,
+ 207,
+ 223,
+ 241,
+ 263,
+ 291,
+ 328,
+ 382,
+ 467,
+ 522,
+ 522,
+ 522,
+};
+
+static const int32_t dq7dith16_sf1[65] =
+{
+ 9948,
+ 9948,
+ 9962,
+ 9988,
+ 10026,
+ 10078,
+ 10142,
+ 10218,
+ 10306,
+ 10408,
+ 10520,
+ 10646,
+ 10784,
+ 10934,
+ 11098,
+ 11274,
+ 11462,
+ 11664,
+ 11880,
+ 12112,
+ 12358,
+ 12618,
+ 12898,
+ 13194,
+ 13510,
+ 13844,
+ 14202,
+ 14582,
+ 14988,
+ 15422,
+ 15884,
+ 16380,
+ 16912,
+ 17484,
+ 18098,
+ 18762,
+ 19480,
+ 20258,
+ 21106,
+ 22030,
+ 23044,
+ 24158,
+ 25390,
+ 26760,
+ 28290,
+ 30008,
+ 31954,
+ 34172,
+ 36728,
+ 39700,
+ 43202,
+ 47382,
+ 52462,
+ 58762,
+ 66770,
+ 77280,
+ 91642,
+ 112348,
+ 144452,
+ 199326,
+ 303512,
+ 485546,
+ 643414,
+ 794914,
+ 1000124,
+};
+
+static const int32_t dq7mLamb16[65] =
+{
+ 0,
+ -4,
+ -7,
+ -10,
+ -13,
+ -16,
+ -19,
+ -22,
+ -26,
+ -28,
+ -32,
+ -35,
+ -38,
+ -41,
+ -44,
+ -47,
+ -51,
+ -54,
+ -58,
+ -62,
+ -65,
+ -70,
+ -74,
+ -79,
+ -84,
+ -90,
+ -95,
+ -102,
+ -109,
+ -116,
+ -124,
+ -133,
+ -143,
+ -154,
+ -166,
+ -180,
+ -195,
+ -212,
+ -231,
+ -254,
+ -279,
+ -308,
+ -343,
+ -383,
+ -430,
+ -487,
+ -555,
+ -639,
+ -743,
+ -876,
+ -1045,
+ -1270,
+ -1575,
+ -2002,
+ -2628,
+ -3591,
+ -5177,
+ -8026,
+ -13719,
+ -26047,
+ -45509,
+ -39467,
+ -37875,
+ -51303,
+ 0,
+};
+
+/* Array of structures containing subband parameters. */
+static const SubbandParameters subbandParameters[NUMSUBBANDS] =
+{
+ /* LL band */
+ { 0, dq7bit16_sl1, 0, dq7dith16_sf1, dq7mLamb16, q7incr16, 7, (18*256)-1, -20, 24 },
+
+ /* LH band */
+ { 0, dq4bit16_sl1, 0, dq4dith16_sf1, dq4mLamb16, q4incr16, 4, (21*256)-1, -23, 12 },
+
+ /* HL band */
+ { 0, dq2bit16_sl1, 0, dq2dith16_sf1, dq2mLamb16, q2incr16, 2, (23*256)-1, -25, 6 },
+
+ /* HH band */
+ { 0, dq3bit16_sl1, 0, dq3dith16_sf1, dq3mLamb16, q3incr16, 3, (22*256)-1, -24, 12 }
+};
+
+
+#endif //APTXTABLES_H
diff --git a/system/embdrv/encoder_for_aptx/src/CBStruct.h b/system/embdrv/encoder_for_aptx/src/CBStruct.h
new file mode 100644
index 00000000000..b3d77cfebb5
--- /dev/null
+++ b/system/embdrv/encoder_for_aptx/src/CBStruct.h
@@ -0,0 +1,28 @@
+/*------------------------------------------------------------------------------
+ *
+ * Structure required to implement a circular buffer.
+ *
+ *----------------------------------------------------------------------------*/
+
+#ifndef CBSTRUCT_H
+#define CBSTRUCT_H
+#ifdef _GCC
+ #pragma GCC visibility push(hidden)
+#endif
+
+
+typedef struct circularBuffer_t
+{
+ /* Buffer storage */
+ int32_t buffer[48];
+ /* Pointer to current buffer location */
+ uint32_t pointer;
+ /* Modulo length of circular buffer */
+ uint32_t modulo;
+}circularBuffer;
+
+
+#ifdef _GCC
+ #pragma GCC visibility pop
+#endif
+#endif //CBSTRUCT_H
\ No newline at end of file
diff --git a/system/embdrv/encoder_for_aptx/src/CodewordPacker.h b/system/embdrv/encoder_for_aptx/src/CodewordPacker.h
new file mode 100644
index 00000000000..688245b3213
--- /dev/null
+++ b/system/embdrv/encoder_for_aptx/src/CodewordPacker.h
@@ -0,0 +1,59 @@
+/*------------------------------------------------------------------------------
+ *
+ * Prototype declaration of the CodewordPacker Function
+ *
+ * This functions allows a client to supply an array of 4 quantised codes
+ * (1 per subband) and obtain a packed version as a 16-bit aptX codeword.
+ *
+ *----------------------------------------------------------------------------*/
+
+#ifndef CODEWORDPACKER_H
+#define CODEWORDPACKER_H
+
+
+#include "AptxParameters.h"
+
+
+XBT_INLINE_ int32_t packCodeword(Encoder_data* EncoderDataPt, uint32_t aligned)
+{
+ int32_t syncContribution;
+ int32_t hhCode;
+ int32_t codeword;
+
+ /* The per-channel contribution to derive the current sync bit is the XOR of
+ * the 4 code lsbs and the random dither bit. The SyncInserter engineers it
+ * such that the XOR of the sync contributions from the left and right
+ * channel give the actual sync bit value. The per-channel sync bit
+ * contribution overwrites the HH code lsb in the packed codeword. */
+ if (aligned != no_sync)
+ {
+ syncContribution =
+ (EncoderDataPt->m_qdata[0].qCode ^
+ EncoderDataPt->m_qdata[1].qCode ^
+ EncoderDataPt->m_qdata[2].qCode ^
+ EncoderDataPt->m_qdata[3].qCode ^
+ EncoderDataPt->m_dithSyncRandBit) & 0x1;
+ hhCode = (EncoderDataPt->m_qdata[HH].qCode & 0x6) | syncContribution;
+
+ /* Pack the 16-bit codeword with the appropriate number of lsbs from each
+ * quantised code (LL=7, LH=4, HL=2, HH=3). */
+ codeword =
+ (EncoderDataPt->m_qdata[LL].qCode & 0x7fL) |
+ ((EncoderDataPt->m_qdata[LH].qCode & 0xfL) << 7) |
+ ((EncoderDataPt->m_qdata[HL].qCode & 0x3L) << 11) |
+ (hhCode << 13);
+ }
+ else
+ { // don't add sync contribution for non-autosync mode
+ codeword =
+ (EncoderDataPt->m_qdata[LL].qCode & 0x7fL) |
+ ((EncoderDataPt->m_qdata[LH].qCode & 0xfL) << 7) |
+ ((EncoderDataPt->m_qdata[HL].qCode & 0x3L) << 11) |
+ ((EncoderDataPt->m_qdata[HH].qCode & 0x7L) << 13);
+
+ }
+ return codeword;
+}
+
+
+#endif //CODEWORDPACKER_H
diff --git a/system/embdrv/encoder_for_aptx/src/DitherGenerator.h b/system/embdrv/encoder_for_aptx/src/DitherGenerator.h
new file mode 100644
index 00000000000..9b4c686f8a5
--- /dev/null
+++ b/system/embdrv/encoder_for_aptx/src/DitherGenerator.h
@@ -0,0 +1,102 @@
+/*------------------------------------------------------------------------------
+ *
+ * These functions allow clients to update an internal codeword history
+ * attribute from previously-generated quantised codes, and to generate a new
+ * pseudo-random dither value per subband from this internal attribute.
+ *
+ *----------------------------------------------------------------------------*/
+
+#ifndef DITHERGENERATOR_H
+#define DITHERGENERATOR_H
+
+
+#include "AptxParameters.h"
+
+
+/* This function updates an internal bit-pool (private
+ * variable in DitherGenerator) based on bits obtained from
+ * previously encoded or received aptX codewords. */
+XBT_INLINE_ int32_t xbtEncupdateCodewordHistory(int32_t quantisedCodes[4], int32_t m_codewordHistory)
+{
+ int32_t newBits;
+ int32_t updatedCodewordHistory;
+
+ const int32_t llMask = 0x3L;
+ const int32_t lhMask = 0x2L;
+ const int32_t hlMask = 0x1L;
+ const uint32_t lhShift = 1;
+ const uint32_t hlShift = 3;
+ /* Shift value to left-justify a 24-bit value in a 32-bit signed variable*/
+ const uint32_t leftJustifyShift = 8;
+ const uint32_t numNewBits = 4;
+
+ /* Make a 4-bit vector from particular bits from 3 quantised codes */
+ newBits = (quantisedCodes[LL] & llMask) +
+ ((quantisedCodes[LH] & lhMask) << lhShift) +
+ ((quantisedCodes[HL] & hlMask) << hlShift);
+
+ /* Add the 4 new bits to the codeword history. Note that this is a 24-bit
+ * value LEFT-JUSTIFIED in a 32-bit signed variable. Maintaining the history
+ * as signed is useful in the dither generation process below. */
+ updatedCodewordHistory =
+ (m_codewordHistory << numNewBits) + (newBits << leftJustifyShift);
+
+ return updatedCodewordHistory;
+}
+
+/* Function to generate a dither value for each subband based
+ * on the current contents of the codewordHistory bit-pool. */
+XBT_INLINE_ int32_t xbtEncgenerateDither(int32_t m_codewordHistory, int32_t* m_ditherOutputs)
+{
+ int32_t history24b;
+ int32_t upperAcc, lowerAcc;
+ int32_t accSum;
+ int64_t tmp_acc;
+ int32_t ditherSample;
+ int32_t m_dithSyncRandBit;
+
+ /* Fixed value to multiply codeword history variable by */
+ const uint32_t dithConstMultiplier = 0x4f1bbbL;
+ /* Shift value to left-justify a 24-bit value in a 32-bit signed variable*/
+ const uint32_t leftJustifyShift = 8;
+
+ /* AND mask to retain only the lower 24 bits of a variable */
+ const int32_t keepLower24bitsMask = 0xffffffL;
+
+ /* Convert the codeword history to a 24-bit signed value. This can be done
+ * cheaply with a 8-position right-shift since it is maintained as 24-bits
+ * value left-justified in a signed 32-bit variable. */
+ history24b = m_codewordHistory >> (leftJustifyShift - 1);
+
+ /* Multiply the history by a fixed constant. The constant has already been
+ * shifted right by 1 position to compensate for the left-shift introduced
+ * on the product by the fractional multiplier. */
+ tmp_acc = ((int64_t)history24b * (int64_t)dithConstMultiplier);
+
+ /* Get the upper and lower 24-bit values from the accumulator, and form
+ * their sum. */
+ upperAcc = ((int32_t)(tmp_acc >> 24)) & 0x00FFFFFFL;
+ lowerAcc = ((int32_t)tmp_acc) & 0x00FFFFFFL;
+ accSum = upperAcc + lowerAcc;
+
+ /* The dither sample is the 2 msbs of lowerAcc and the 22 lsbs of accSum */
+ ditherSample = ((lowerAcc >> 22) + (accSum << 2)) & keepLower24bitsMask;
+
+ /* The sign bit of 24-bit accSum is saved as a random bit to
+ * assist in the aptX sync insertion process. */
+ m_dithSyncRandBit = (accSum >> 23) & 0x1;
+
+ /* Successive dither outputs for the 4 subbands are versions of ditherSample
+ * offset by a further 5-position left shift for each subband. Also apply a
+ * constant left-shift of 8 to turn the values into signed 24-bit values
+ * left-justified in the 32-bit ditherOutput variable. */
+ m_ditherOutputs[HH] = ditherSample << leftJustifyShift;
+ m_ditherOutputs[HL] = ditherSample << (5 + leftJustifyShift);
+ m_ditherOutputs[LH] = ditherSample << (10 + leftJustifyShift);
+ m_ditherOutputs[LL] = ditherSample << (15 + leftJustifyShift);
+
+ return m_dithSyncRandBit;
+};
+
+
+#endif //DITHERGENERATOR_H
diff --git a/system/embdrv/encoder_for_aptx/src/ProcessSubband.c b/system/embdrv/encoder_for_aptx/src/ProcessSubband.c
new file mode 100644
index 00000000000..0dfbb4fa8cb
--- /dev/null
+++ b/system/embdrv/encoder_for_aptx/src/ProcessSubband.c
@@ -0,0 +1,43 @@
+#include "AptxParameters.h"
+#include "SubbandFunctionsCommon.h"
+#include "SubbandFunctions.h"
+
+
+/* This function carries out all subband processing (common to both encode and decode). */
+void processSubband(const int32_t qCode, const int32_t ditherVal, Subband_data* SubbandDataPt, IQuantiser_data* iqDataPt)
+{
+ /* Inverse quantisation */
+ invertQuantisation(qCode, ditherVal, iqDataPt);
+
+ /* Predictor pole coefficient update */
+ updatePredictorPoleCoefficients(iqDataPt->invQ, SubbandDataPt->m_predData.m_zeroVal, &SubbandDataPt->m_PoleCoeffData);
+
+ /* Predictor filtering */
+ performPredictionFiltering(iqDataPt->invQ, SubbandDataPt);
+}
+
+/* processSubbandLL is used for the LL subband only. */
+void processSubbandLL(const int32_t qCode, const int32_t ditherVal, Subband_data* SubbandDataPt, IQuantiser_data* iqDataPt)
+{
+ /* Inverse quantisation */
+ invertQuantisation(qCode, ditherVal, iqDataPt);
+
+ /* Predictor pole coefficient update */
+ updatePredictorPoleCoefficients(iqDataPt->invQ, SubbandDataPt->m_predData.m_zeroVal, &SubbandDataPt->m_PoleCoeffData);
+
+ /* Predictor filtering */
+ performPredictionFilteringLL(iqDataPt->invQ, SubbandDataPt);
+}
+
+/* processSubbandHL is used for the HL subband only. */
+void processSubbandHL(const int32_t qCode, const int32_t ditherVal, Subband_data* SubbandDataPt, IQuantiser_data* iqDataPt)
+{
+ /* Inverse quantisation */
+ invertQuantisationHL(qCode, ditherVal, iqDataPt);
+
+ /* Predictor pole coefficient update */
+ updatePredictorPoleCoefficients(iqDataPt->invQ, SubbandDataPt->m_predData.m_zeroVal, &SubbandDataPt->m_PoleCoeffData);
+
+ /* Predictor filtering */
+ performPredictionFilteringHL(iqDataPt->invQ, SubbandDataPt);
+}
diff --git a/system/embdrv/encoder_for_aptx/src/Qmf.h b/system/embdrv/encoder_for_aptx/src/Qmf.h
new file mode 100644
index 00000000000..acc6696b747
--- /dev/null
+++ b/system/embdrv/encoder_for_aptx/src/Qmf.h
@@ -0,0 +1,142 @@
+/*------------------------------------------------------------------------------
+ *
+ * This file includes the coefficient tables or the two convolution function
+ * It also includes the definition of Qmf_storage and the prototype of all
+ * necessary functions required to implement the QMF filtering.
+ *
+ *----------------------------------------------------------------------------*/
+
+#ifndef QMF_H
+#define QMF_H
+
+
+#include "AptxParameters.h"
+
+
+typedef struct
+{
+ int16_t QmfL_buf[32];
+ int16_t QmfH_buf[32];
+ int32_t QmfLH_buf[32];
+ int32_t QmfHL_buf[32];
+ int32_t QmfLL_buf[32];
+ int32_t QmfHH_buf[32];
+ int32_t QmfI_pt;
+ int32_t QmfO_pt;
+}Qmf_storage;
+
+/* Outer QMF filter for Enhanced aptX is a symmetrical 32-tap filter (16
+ * different coefficients). The table in defined in QmfConv.c */
+#ifndef _STDQMFOUTERCOEFF
+const int32_t Qmf_outerCoeffs[12] =
+{
+ /* (C(1/30)C(3/28)), C(5/26), C(7/24) */
+ 0xFE6302DA, 0xFFFFDA75, 0x0000AA6A,
+ /* C(9/22), C(11/20), C(13/18), C(15/16) */
+ 0xFFFE273E, 0x00041E95, 0xFFF710B5, 0x002AC12E,
+ /* C(17/14), C(19/12), (C(21/10)C(23/8)) */
+ 0x000AA328, 0xFFFD8D1F, 0x211E6BDB,
+ /* (C(25/6)C(27/4)), (C(29/2)C(31/0)) */
+ 0x0DB7D8C5, 0xFC7F02B0
+};
+#else
+const int32_t Qmf_outerCoeffs[16] =
+{
+ 730, -413, -9611, 43626,
+ -121026, 269973, -585547, 2801966,
+ 697128, -160481, 27611, 8478,
+ -10043, 3511, 688, -897
+};
+#endif
+
+/* Each inner QMF filter for Enhanced aptX is a symmetrical 32-tap filter (16
+ * different coefficients) */
+const int32_t Qmf_innerCoeffs[16] =
+{
+ 1033, -584, -13592, 61697,
+ -171156, 381799, -828088, 3962579,
+ 985888, -226954, 39048, 11990,
+ -14203, 4966, 973, -1268
+};
+
+void AsmQmfConvI (const int32_t *p1dl_buffPtr, const int32_t *p2dl_buffPtr, const int32_t *coeffPtr, int32_t* filterOutputs);
+void AsmQmfConvO (const int16_t *p1dl_buffPtr, const int16_t *p2dl_buffPtr, const int32_t *coeffPtr, int32_t* convSumDiff);
+
+XBT_INLINE_ void QmfAnalysisFilter(int32_t pcm[4], Qmf_storage* Qmf_St, int32_t* predVals, int32_t* aqmfOutputs)
+{
+ int32_t convSumDiff[4];
+ int32_t filterOutputs[4];
+
+ int32_t lc_QmfO_pt = (Qmf_St->QmfO_pt);
+ int32_t lc_QmfI_pt = (Qmf_St->QmfI_pt);
+
+ /* Symbolic constants to represent the first and second set out outer filter
+ * outputs. */
+ enum { FirstOuterOutputs = 0, SecondOuterOutputs = 1 };
+
+ /* Load outer filter phase1 and phase2 delay lines with the first 2 PCM
+ * samples. Convolve the filter and get the 2 convolution results. */
+ Qmf_St->QmfL_buf[lc_QmfO_pt + 16] = (int16_t)pcm[FirstPcm];
+ Qmf_St->QmfL_buf[lc_QmfO_pt] = (int16_t)pcm[FirstPcm];
+ Qmf_St->QmfH_buf[lc_QmfO_pt + 16] = (int16_t)pcm[SecondPcm];
+ Qmf_St->QmfH_buf[lc_QmfO_pt++] = (int16_t)pcm[SecondPcm];
+ lc_QmfO_pt &= 0xF;
+
+ AsmQmfConvO(&Qmf_St->QmfL_buf[lc_QmfO_pt + 15], &Qmf_St->QmfH_buf[lc_QmfO_pt], Qmf_outerCoeffs, &convSumDiff[0]);
+
+
+ /* Load outer filter phase1 and phase2 delay lines with the second 2 PCM
+ * samples. Convolve the filter and get the 2 convolution results. */
+ Qmf_St->QmfL_buf[lc_QmfO_pt + 16] = (int16_t)pcm[ThirdPcm];
+ Qmf_St->QmfL_buf[lc_QmfO_pt] = (int16_t)pcm[ThirdPcm];
+ Qmf_St->QmfH_buf[lc_QmfO_pt + 16] = (int16_t)pcm[FourthPcm];
+ Qmf_St->QmfH_buf[lc_QmfO_pt++] = (int16_t)pcm[FourthPcm];
+ lc_QmfO_pt &= 0xF;
+
+ AsmQmfConvO(&Qmf_St->QmfL_buf[lc_QmfO_pt + 15], &Qmf_St->QmfH_buf[lc_QmfO_pt], Qmf_outerCoeffs, &convSumDiff[1]);
+
+ /* Load the first inner filter phase1 and phase2 delay lines with the 2
+ * convolution sum (low-pass) outer filter outputs. Convolve the filter and
+ * get the 2 convolution results. The first 2 analysis filter outputs are
+ * the sum and difference values for the first inner filter convolutions. */
+ Qmf_St->QmfLL_buf[lc_QmfI_pt + 16] = convSumDiff[0];
+ Qmf_St->QmfLL_buf[lc_QmfI_pt] = convSumDiff[0];
+ Qmf_St->QmfLH_buf[lc_QmfI_pt + 16] = convSumDiff[1];
+ Qmf_St->QmfLH_buf[lc_QmfI_pt] = convSumDiff[1];
+
+ AsmQmfConvI(&Qmf_St->QmfLL_buf[lc_QmfI_pt + 16], &Qmf_St->QmfLH_buf[lc_QmfI_pt + 1],&Qmf_innerCoeffs[0], &filterOutputs[LL]);
+
+ /* Load the second inner filter phase1 and phase2 delay lines with the 2
+ * convolution difference (high-pass) outer filter outputs. Convolve the
+ * filter and get the 2 convolution results. The second 2 analysis filter
+ * outputs are the sum and difference values for the second inner filter
+ * convolutions. */
+ Qmf_St->QmfHL_buf[lc_QmfI_pt + 16] = convSumDiff[2];
+ Qmf_St->QmfHL_buf[lc_QmfI_pt] = convSumDiff[2];
+ Qmf_St->QmfHH_buf[lc_QmfI_pt + 16] = convSumDiff[3];
+ Qmf_St->QmfHH_buf[lc_QmfI_pt++] = convSumDiff[3];
+ lc_QmfI_pt &= 0xF;
+
+ AsmQmfConvI(&Qmf_St->QmfHL_buf[lc_QmfI_pt + 15], &Qmf_St->QmfHH_buf[lc_QmfI_pt],&Qmf_innerCoeffs[0], &filterOutputs[HL]);
+
+ /* Subtracted the previous predicted value from the filter output on a
+ * per-subband basis. Ensure these values are saturated, if necessary.
+ * Manual unrolling */
+ aqmfOutputs[LL] = filterOutputs[LL] - predVals[LL];
+ aqmfOutputs[LL] = ssat24(aqmfOutputs[LL]);
+
+ aqmfOutputs[LH] = filterOutputs[LH] - predVals[LH];
+ aqmfOutputs[LH] = ssat24(aqmfOutputs[LH]);
+
+ aqmfOutputs[HL] = filterOutputs[HL] - predVals[HL];
+ aqmfOutputs[HL] = ssat24(aqmfOutputs[HL]);
+
+ aqmfOutputs[HH] = filterOutputs[HH] - predVals[HH];
+ aqmfOutputs[HH] = ssat24(aqmfOutputs[HH]);
+
+ (Qmf_St->QmfO_pt) = lc_QmfO_pt;
+ (Qmf_St->QmfI_pt) = lc_QmfI_pt;
+}
+
+
+#endif //QMF_H
diff --git a/system/embdrv/encoder_for_aptx/src/QmfConv.c b/system/embdrv/encoder_for_aptx/src/QmfConv.c
new file mode 100644
index 00000000000..14e342ef5ae
--- /dev/null
+++ b/system/embdrv/encoder_for_aptx/src/QmfConv.c
@@ -0,0 +1,327 @@
+/*------------------------------------------------------------------------------
+ *
+ * This file includes convolution functions required for the Qmf.
+ *
+ *----------------------------------------------------------------------------*/
+
+
+#include "AptxParameters.h"
+
+
+void AsmQmfConvO(const int16_t* p1dl_buffPtr, const int16_t* p2dl_buffPtr, const int32_t* coeffPtr, int32_t* convSumDiff)
+{
+ /* Since all manipulated data are "int16_t" it is possible to
+ * reduce the number of loads by using int32_t type and manipulating
+ * pairs of data
+ */
+ int32_t acc;
+ // Manual inlining as IAR compiler does not seem to do it itself...
+ // WARNING: This inlining assumes that m_qmfDelayLineLength == 16
+ int32_t tmp_round0;
+ int64_t local_acc0;
+ int64_t local_acc1;
+ int32_t coeffVal0;
+ int32_t coeffVal1;
+ int16_t data0;
+ int16_t data1;
+ int16_t data2;
+ int16_t data3;
+ int32_t phaseConv[2];
+ int32_t convSum;
+ int32_t convDiff;
+
+ coeffVal0 = (*(coeffPtr));
+ coeffVal1 = (*(coeffPtr + 1));
+ data0 = (*(p1dl_buffPtr));
+ data1 = (*(p2dl_buffPtr));
+ data2 = (*(p1dl_buffPtr - 1));
+ data3 = (*(p2dl_buffPtr + 1));
+
+ local_acc0 = ((int64_t)(coeffVal0) * (int64_t)data0);
+ local_acc1 = ((int64_t)(coeffVal0) * (int64_t)data1);
+ local_acc0 += ((int64_t)(coeffVal1) * (int64_t)data2);
+ local_acc1 += ((int64_t)(coeffVal1) * (int64_t)data3);
+
+ coeffVal0 = (*(coeffPtr + 2));
+ coeffVal1 = (*(coeffPtr + 3));
+ data0 = (*(p1dl_buffPtr - 2));
+ data1 = (*(p2dl_buffPtr + 2));
+ data2 = (*(p1dl_buffPtr - 3));
+ data3 = (*(p2dl_buffPtr + 3));
+
+ local_acc0 += ((int64_t)(coeffVal0) * (int64_t)data0);
+ local_acc1 += ((int64_t)(coeffVal0) * (int64_t)data1);
+ local_acc0 += ((int64_t)(coeffVal1) * (int64_t)data2);
+ local_acc1 += ((int64_t)(coeffVal1) * (int64_t)data3);
+
+ coeffVal0 = (*(coeffPtr + 4));
+ coeffVal1 = (*(coeffPtr + 5));
+ data0 = (*(p1dl_buffPtr - 4));
+ data1 = (*(p2dl_buffPtr + 4));
+ data2 = (*(p1dl_buffPtr - 5));
+ data3 = (*(p2dl_buffPtr + 5));
+
+ local_acc0 += ((int64_t)(coeffVal0) * (int64_t)data0);
+ local_acc1 += ((int64_t)(coeffVal0) * (int64_t)data1);
+ local_acc0 += ((int64_t)(coeffVal1) * (int64_t)data2);
+ local_acc1 += ((int64_t)(coeffVal1) * (int64_t)data3);
+
+ coeffVal0 = (*(coeffPtr + 6));
+ coeffVal1 = (*(coeffPtr + 7));
+ data0 = (*(p1dl_buffPtr - 6));
+ data1 = (*(p2dl_buffPtr + 6));
+ data2 = (*(p1dl_buffPtr - 7));
+ data3 = (*(p2dl_buffPtr + 7));
+
+ local_acc0 += ((int64_t)(coeffVal0) * (int64_t)data0);
+ local_acc1 += ((int64_t)(coeffVal0) * (int64_t)data1);
+ local_acc0 += ((int64_t)(coeffVal1) * (int64_t)data2);
+ local_acc1 += ((int64_t)(coeffVal1) * (int64_t)data3);
+
+ coeffVal0 = (*(coeffPtr + 8));
+ coeffVal1 = (*(coeffPtr + 9));
+ data0 = (*(p1dl_buffPtr - 8));
+ data1 = (*(p2dl_buffPtr + 8));
+ data2 = (*(p1dl_buffPtr - 9));
+ data3 = (*(p2dl_buffPtr + 9));
+
+ local_acc0 += ((int64_t)(coeffVal0) * (int64_t)data0);
+ local_acc1 += ((int64_t)(coeffVal0) * (int64_t)data1);
+ local_acc0 += ((int64_t)(coeffVal1) * (int64_t)data2);
+ local_acc1 += ((int64_t)(coeffVal1) * (int64_t)data3);
+
+ coeffVal0 = (*(coeffPtr + 10));
+ coeffVal1 = (*(coeffPtr + 11));
+ data0 = (*(p1dl_buffPtr - 10));
+ data1 = (*(p2dl_buffPtr + 10));
+ data2 = (*(p1dl_buffPtr - 11));
+ data3 = (*(p2dl_buffPtr + 11));
+
+ local_acc0 += ((int64_t)(coeffVal0) * (int64_t)data0);
+ local_acc1 += ((int64_t)(coeffVal0) * (int64_t)data1);
+ local_acc0 += ((int64_t)(coeffVal1) * (int64_t)data2);
+ local_acc1 += ((int64_t)(coeffVal1) * (int64_t)data3);
+
+ coeffVal0 = (*(coeffPtr + 12));
+ coeffVal1 = (*(coeffPtr + 13));
+ data0 = (*(p1dl_buffPtr - 12));
+ data1 = (*(p2dl_buffPtr + 12));
+ data2 = (*(p1dl_buffPtr - 13));
+ data3 = (*(p2dl_buffPtr + 13));
+
+ local_acc0 += ((int64_t)(coeffVal0) * (int64_t)data0);
+ local_acc1 += ((int64_t)(coeffVal0) * (int64_t)data1);
+ local_acc0 += ((int64_t)(coeffVal1) * (int64_t)data2);
+ local_acc1 += ((int64_t)(coeffVal1) * (int64_t)data3);
+
+ coeffVal0 = (*(coeffPtr + 14));
+ coeffVal1 = (*(coeffPtr + 15));
+ data0 = (*(p1dl_buffPtr - 14));
+ data1 = (*(p2dl_buffPtr + 14));
+ data2 = (*(p1dl_buffPtr - 15));
+ data3 = (*(p2dl_buffPtr + 15));
+
+ local_acc0 += ((int64_t)(coeffVal0) * (int64_t)data0);
+ local_acc1 += ((int64_t)(coeffVal0) * (int64_t)data1);
+ local_acc0 += ((int64_t)(coeffVal1) * (int64_t)data2);
+ local_acc1 += ((int64_t)(coeffVal1) * (int64_t)data3);
+
+ tmp_round0 = (int32_t)local_acc0 & 0x00FFFFL;
+
+ local_acc0 += 0x004000L;
+ acc = (int32_t)(local_acc0 >> 15);
+ if (tmp_round0 == 0x004000L)
+ acc--;
+ if (acc > 8388607)
+ acc = 8388607;
+ if (acc < -8388608)
+ acc = -8388608;
+
+ phaseConv[0] = (int32_t)acc;
+
+ tmp_round0 = (int32_t)local_acc1 & 0x00FFFFL;
+
+ local_acc1 += 0x004000L;
+ acc = (int32_t)(local_acc1 >> 15);
+ if (tmp_round0 == 0x004000L)
+ acc--;
+ if (acc > 8388607)
+ acc = 8388607;
+ if (acc < -8388608)
+ acc = -8388608;
+
+ phaseConv[1] = (int32_t)acc;
+
+ convSum = phaseConv[1] + phaseConv[0];
+ if (convSum > 8388607)
+ convSum = 8388607;
+ if (convSum < -8388608)
+ convSum = -8388608;
+
+ convDiff = phaseConv[1] - phaseConv[0];
+ if (convDiff > 8388607)
+ convDiff = 8388607;
+ if (convDiff < -8388608)
+ convDiff = -8388608;
+
+ *(convSumDiff) = convSum;
+ *(convSumDiff + 2) = convDiff;
+}
+
+void AsmQmfConvI(const int32_t* p1dl_buffPtr, const int32_t* p2dl_buffPtr, const int32_t* coeffPtr, int32_t* filterOutputs)
+{
+ int32_t acc;
+ // WARNING: This inlining assumes that m_qmfDelayLineLength == 16
+ int32_t tmp_round0;
+ int64_t local_acc0;
+ int64_t local_acc1;
+ int32_t coeffVal0;
+ int32_t coeffVal1;
+ int32_t data0;
+ int32_t data1;
+ int32_t data2;
+ int32_t data3;
+ int32_t phaseConv[2];
+ int32_t convSum;
+ int32_t convDiff;
+
+ coeffVal0 = (*(coeffPtr));
+ coeffVal1 = (*(coeffPtr + 1));
+ data0 = (*(p1dl_buffPtr));
+ data1 = (*(p2dl_buffPtr));
+ data2 = (*(p1dl_buffPtr - 1));
+ data3 = (*(p2dl_buffPtr + 1));
+
+ local_acc0 = ((int64_t)(coeffVal0)*data0);
+ local_acc1 = ((int64_t)(coeffVal0)*data1);
+ local_acc0 += ((int64_t)(coeffVal1)*data2);
+ local_acc1 += ((int64_t)(coeffVal1)*data3);
+
+ coeffVal0 = (*(coeffPtr + 2));
+ coeffVal1 = (*(coeffPtr + 3));
+ data0 = (*(p1dl_buffPtr - 2));
+ data1 = (*(p2dl_buffPtr + 2));
+ data2 = (*(p1dl_buffPtr - 3));
+ data3 = (*(p2dl_buffPtr + 3));
+
+ local_acc0 += ((int64_t)(coeffVal0)*data0);
+ local_acc1 += ((int64_t)(coeffVal0)*data1);
+ local_acc0 += ((int64_t)(coeffVal1)*data2);
+ local_acc1 += ((int64_t)(coeffVal1)*data3);
+
+ coeffVal0 = (*(coeffPtr + 4));
+ coeffVal1 = (*(coeffPtr + 5));
+ data0 = (*(p1dl_buffPtr - 4));
+ data1 = (*(p2dl_buffPtr + 4));
+ data2 = (*(p1dl_buffPtr - 5));
+ data3 = (*(p2dl_buffPtr + 5));
+
+ local_acc0 += ((int64_t)(coeffVal0)*data0);
+ local_acc1 += ((int64_t)(coeffVal0)*data1);
+ local_acc0 += ((int64_t)(coeffVal1)*data2);
+ local_acc1 += ((int64_t)(coeffVal1)*data3);
+
+ coeffVal0 = (*(coeffPtr + 6));
+ coeffVal1 = (*(coeffPtr + 7));
+ data0 = (*(p1dl_buffPtr - 6));
+ data1 = (*(p2dl_buffPtr + 6));
+ data2 = (*(p1dl_buffPtr - 7));
+ data3 = (*(p2dl_buffPtr + 7));
+
+ local_acc0 += ((int64_t)(coeffVal0)*data0);
+ local_acc1 += ((int64_t)(coeffVal0)*data1);
+ local_acc0 += ((int64_t)(coeffVal1)*data2);
+ local_acc1 += ((int64_t)(coeffVal1)*data3);
+
+ coeffVal0 = (*(coeffPtr + 8));
+ coeffVal1 = (*(coeffPtr + 9));
+ data0 = (*(p1dl_buffPtr - 8));
+ data1 = (*(p2dl_buffPtr + 8));
+ data2 = (*(p1dl_buffPtr - 9));
+ data3 = (*(p2dl_buffPtr + 9));
+
+ local_acc0 += ((int64_t)(coeffVal0)*data0);
+ local_acc1 += ((int64_t)(coeffVal0)*data1);
+ local_acc0 += ((int64_t)(coeffVal1)*data2);
+ local_acc1 += ((int64_t)(coeffVal1)*data3);
+
+ coeffVal0 = (*(coeffPtr + 10));
+ coeffVal1 = (*(coeffPtr + 11));
+ data0 = (*(p1dl_buffPtr - 10));
+ data1 = (*(p2dl_buffPtr + 10));
+ data2 = (*(p1dl_buffPtr - 11));
+ data3 = (*(p2dl_buffPtr + 11));
+
+ local_acc0 += ((int64_t)(coeffVal0)*data0);
+ local_acc1 += ((int64_t)(coeffVal0)*data1);
+ local_acc0 += ((int64_t)(coeffVal1)*data2);
+ local_acc1 += ((int64_t)(coeffVal1)*data3);
+
+ coeffVal0 = (*(coeffPtr + 12));
+ coeffVal1 = (*(coeffPtr + 13));
+ data0 = (*(p1dl_buffPtr - 12));
+ data1 = (*(p2dl_buffPtr + 12));
+ data2 = (*(p1dl_buffPtr - 13));
+ data3 = (*(p2dl_buffPtr + 13));
+
+ local_acc0 += ((int64_t)(coeffVal0)*data0);
+ local_acc1 += ((int64_t)(coeffVal0)*data1);
+ local_acc0 += ((int64_t)(coeffVal1)*data2);
+ local_acc1 += ((int64_t)(coeffVal1)*data3);
+
+ coeffVal0 = (*(coeffPtr + 14));
+ coeffVal1 = (*(coeffPtr + 15));
+ data0 = (*(p1dl_buffPtr - 14));
+ data1 = (*(p2dl_buffPtr + 14));
+ data2 = (*(p1dl_buffPtr - 15));
+ data3 = (*(p2dl_buffPtr + 15));
+
+ local_acc0 += ((int64_t)(coeffVal0)*data0);
+ local_acc1 += ((int64_t)(coeffVal0)*data1);
+ local_acc0 += ((int64_t)(coeffVal1)*data2);
+ local_acc1 += ((int64_t)(coeffVal1)*data3);
+
+ tmp_round0 = (int32_t)local_acc0;
+
+ local_acc0 += 0x00400000L;
+ acc = (int32_t)(local_acc0 >> 23);
+
+ if ((((tmp_round0 << 8) ^ 0x40000000) == 0))
+ {
+ acc--;
+ }
+
+ if (acc > 8388607)
+ acc = 8388607;
+ if (acc < -8388608)
+ acc = -8388608;
+
+ phaseConv[0] = (int32_t)acc;
+ tmp_round0 = (int32_t)local_acc1;
+
+ local_acc1 += 0x00400000L;
+ acc = (int32_t)(local_acc1 >> 23);
+ if ((((tmp_round0 << 8) ^ 0x40000000) == 0))
+ {
+ acc--;
+ }
+
+ if (acc > 8388607)
+ acc = 8388607;
+ if (acc < -8388608)
+ acc = -8388608;
+
+ phaseConv[1] = (int32_t)acc;
+
+ convSum = phaseConv[1] + phaseConv[0];
+ if (convSum > 8388607) convSum = 8388607;
+ if (convSum < -8388608) convSum = -8388608;
+
+ *(filterOutputs) = convSum;
+
+ convDiff = phaseConv[1] - phaseConv[0];
+ if (convDiff > 8388607) convDiff = 8388607;
+ if (convDiff < -8388608) convDiff = -8388608;
+
+ *(filterOutputs + 1) = convDiff;
+}
diff --git a/system/embdrv/encoder_for_aptx/src/QuantiseDifference.c b/system/embdrv/encoder_for_aptx/src/QuantiseDifference.c
new file mode 100644
index 00000000000..17e412d230f
--- /dev/null
+++ b/system/embdrv/encoder_for_aptx/src/QuantiseDifference.c
@@ -0,0 +1,711 @@
+#include "AptxParameters.h"
+#include "Quantiser.h"
+#include "AptxTables.h"
+
+
+XBT_INLINE_ int32_t BsearchLL(const int32_t absDiffSignalShifted, const int32_t delta, const int32_t* dqbitTablePrt)
+{
+ int32_t qCode;
+ reg64_t tmp_acc;
+ int32_t tmp;
+ int32_t lc_delta = delta << 8;
+
+ qCode = 0;
+
+ tmp_acc.s64 = (int64_t)lc_delta * (int64_t)dqbitTablePrt[32];
+ tmp_acc.s32.h -= absDiffSignalShifted;
+ tmp = tmp_acc.s32.h | (tmp_acc.u32.l >> 1);
+ if (tmp <= 0) qCode = 32;
+
+ tmp_acc.s64 = (int64_t)lc_delta * (int64_t)dqbitTablePrt[qCode + 16];
+ tmp_acc.s32.h -= absDiffSignalShifted;
+ tmp = tmp_acc.s32.h | (tmp_acc.u32.l >> 1);
+ if (tmp <= 0) qCode += 16;
+ tmp_acc.s64 = (int64_t)lc_delta * (int64_t)dqbitTablePrt[qCode + 8];
+ tmp_acc.s32.h -= absDiffSignalShifted;
+ tmp = tmp_acc.s32.h | (tmp_acc.u32.l >> 1);
+ if (tmp <= 0) qCode += 8;
+
+ tmp_acc.s64 = (int64_t)lc_delta * (int64_t)dqbitTablePrt[qCode + 4];
+ tmp_acc.s32.h -= absDiffSignalShifted;
+ tmp = tmp_acc.s32.h | (tmp_acc.u32.l >> 1);
+ if (tmp <= 0) qCode += 4;
+
+ tmp_acc.s64 = (int64_t)lc_delta * (int64_t)dqbitTablePrt[qCode + 2];
+ tmp_acc.s32.h -= absDiffSignalShifted;
+ tmp = tmp_acc.s32.h | (tmp_acc.u32.l >> 1);
+ if (tmp <= 0) qCode += 2;
+
+ tmp_acc.s64 = (int64_t)lc_delta * (int64_t)dqbitTablePrt[qCode + 1];
+ tmp_acc.s32.h -= absDiffSignalShifted;
+ tmp = tmp_acc.s32.h | (tmp_acc.u32.l >> 1);
+ if (tmp <= 0) qCode++;
+
+ return (qCode);
+}
+
+XBT_INLINE_ int32_t BsearchHL(const int32_t absDiffSignalShifted, const int32_t delta)
+{
+ reg64_t tmp_acc;
+ int32_t lc_delta = delta << 8;
+
+ /* first iteration */
+ tmp_acc.s64 = (int64_t)lc_delta * (int64_t)(97040 << 1);
+ tmp_acc.s32.h -= absDiffSignalShifted;
+ if (tmp_acc.s64 <= 0)
+ return (1);
+ else
+ return (0);
+}
+
+XBT_INLINE_ int32_t BsearchHH(const int32_t absDiffSignalShifted, const int32_t delta, const int32_t* dqbitTablePrt)
+{
+ int32_t qCode;
+ reg64_t tmp_acc;
+ int32_t tmp;
+ int32_t lc_delta = delta << 8;
+ qCode = 0;
+
+ /* first iteration */
+ tmp_acc.s64 = (int64_t)lc_delta * (int64_t)dqbitTablePrt[2];
+ tmp_acc.s32.h -= absDiffSignalShifted;
+ tmp = tmp_acc.s32.h | (tmp_acc.u32.l >> 1);
+ if (tmp <= 0) qCode += 2;
+ tmp_acc.s64 = (int64_t)lc_delta * (int64_t)dqbitTablePrt[qCode + 1];
+ tmp_acc.s32.h -= absDiffSignalShifted;
+ tmp = tmp_acc.s32.h | (tmp_acc.u32.l >> 1);
+ if (tmp <= 0) qCode++;
+
+ return (qCode);
+}
+
+XBT_INLINE_ int32_t BsearchLH(const int32_t absDiffSignalShifted, const int32_t delta, const int32_t* dqbitTablePrt)
+{
+ int32_t qCode;
+ reg64_t tmp_acc;
+ int32_t tmp;
+ int32_t lc_delta = delta << 8;
+
+ /* first iteration */
+ qCode = 0;
+
+ tmp_acc.s64 = (int64_t)lc_delta * (int64_t)dqbitTablePrt[4];
+ tmp_acc.s32.h -= absDiffSignalShifted;
+ tmp = tmp_acc.s32.h | (tmp_acc.u32.l >> 1);
+ if (tmp <= 0) qCode = 4;
+
+ tmp_acc.s64 = (int64_t)lc_delta * (int64_t)dqbitTablePrt[qCode + 2];
+ tmp_acc.s32.h -= absDiffSignalShifted;
+ tmp = tmp_acc.s32.h | (tmp_acc.u32.l >> 1);
+ if (tmp <= 0) qCode += 2;
+ tmp_acc.s64 = (int64_t)lc_delta * (int64_t)dqbitTablePrt[qCode + 1];
+ tmp_acc.s32.h -= absDiffSignalShifted;
+ tmp = tmp_acc.s32.h | (tmp_acc.u32.l >> 1);
+ if (tmp <= 0) qCode++;
+
+ return (qCode);
+}
+
+void quantiseDifferenceHL(const int32_t diffSignal, const int32_t ditherVal, const int32_t delta, Quantiser_data* qdata_pt)
+{
+ int32_t absDiffSignal;
+ int32_t absDiffSignalShifted;
+ int32_t index;
+ int32_t dithSquared;
+ int32_t minusLambdaD;
+ int32_t acc;
+ int32_t threshDiff;
+ reg64_t tmp_acc;
+ reg64_t tmp_reg64;
+ int32_t tmp_accL;
+ int32_t tmp_qCode;
+ int32_t tmp_altQcode;
+ uint32_t tmp_round0;
+ int32_t _delta;
+
+ /* Form the absolute value of the difference signal and maintain a version
+ * that is right-shifted 4 places for delta scaling. */
+ absDiffSignal = -diffSignal;
+ if (diffSignal >= 0) absDiffSignal = diffSignal;
+ absDiffSignal = ssat24(absDiffSignal);
+ absDiffSignalShifted = absDiffSignal >> deltaScale;
+ absDiffSignalShifted = ssat24(absDiffSignalShifted);
+
+ /* Binary search for the quantised code. This search terminates with the
+ * table index of the LARGEST threshold table value for which
+ * absDiffSignalShifted >= (delta * threshold)
+ */
+ index = BsearchHL(absDiffSignalShifted, delta);
+
+ /* We actually wanted the SMALLEST magnitude quantised code for which
+ * absDiffSignalShifted < (delta * threshold)
+ * i.e. the code with the next highest magnitude than the one we actually
+ * found. We could add +1 to the code magnitude to do this, but we need to
+ * subtract 1 from the code magnitude to compensate for the "phantom
+ * element" at the base of the quantisation table. These two effects cancel
+ * out, so we leave the value of code alone. However, we need to form code+1
+ * to get the proper index into the both the threshold and dither tables,
+ * since we must skip over the phantom element at the base. */
+ qdata_pt->qCode = index;
+
+ /* Square the dither and get the value back from the ALU
+ * (saturated/rounded). */
+ tmp_acc.s64 = ((int64_t)ditherVal * (int64_t)ditherVal);
+
+ acc = tmp_acc.s32.h;
+
+ tmp_round0 = (uint32_t)acc << 8;
+
+ acc = (acc >> 6) + 1;
+ acc >>= 1;
+ if (tmp_round0 == 0x40000000L)
+ acc--;
+
+ acc = ssat24(acc);
+
+ dithSquared = acc;
+
+ /* Form the negative difference of the dither values at index and index-1.
+ * Load the accumulator with this value divided by 2. Ensure saturation is
+ * applied to the difference calculation. */
+ minusLambdaD = qdata_pt->minusLambdaDTable[index];
+
+ tmp_accL = (1 << 23) - (int32_t)dithSquared;
+ tmp_acc.s64 = (int64_t)tmp_accL * minusLambdaD;
+
+ tmp_round0 = (int32_t)tmp_acc.s32.l << 8;
+
+ acc = (tmp_acc.u32.l >> 22) | (tmp_acc.s32.h << 10);
+ acc++;
+ acc >>= 1;
+ if (tmp_round0 == 0x40000000L)
+ acc--;
+
+ // worst case value for acc = 0x000d3e08
+ // no saturation required
+
+ /* Add the threshold table values at index and index-1 to the accumulated
+ * value. */
+ acc += qdata_pt->thresholdTablePtr_sl1[index + 1] >> 1;
+ //// worst case value for acc = 0x000d3e08 + 0x43E1DB = 511FE3
+ acc += qdata_pt->thresholdTablePtr_sl1[index] >> 1;
+ //// worst case value for acc = 0x511FE3 + 0x362FEC = 874FCF
+
+ /* Form the threshold table difference at index and index-1. Ensure
+ * saturation is applied to the difference calculation. */
+ threshDiff = qdata_pt->thresholdTablePtr_sl1[index + 1] - qdata_pt->thresholdTablePtr_sl1[index];
+
+ /* Based on the sign of the difference signal, either add or subtract the
+ * threshold table difference from the accumulated value. Recover the final
+ * accumulated value (saturated/rounded) */
+ if (diffSignal < 0)
+ threshDiff = -threshDiff;
+ tmp_reg64.s64 = ((int64_t)ditherVal * (int64_t)threshDiff);
+
+ tmp_reg64.s32.h += acc;
+ acc = tmp_reg64.s32.h;
+
+ if (tmp_reg64.u32.l >= 0x80000000)
+ acc++;
+ tmp_round0 = (tmp_reg64.u32.l >> 1) | (tmp_reg64.s32.h << 31);
+
+ acc = ssat24(acc);
+
+ if (tmp_round0 == 0x40000000L)
+ acc--;
+ _delta = -delta << 8;
+
+ acc = acc << 4;
+
+ /* Form (absDiffSignal * 0.125) - (acc * delta), which is the final distance
+ * signal used to determine if dithering alters the quantised code value or
+ * not. */
+ // worst case value for delta is 0x7d400
+ tmp_reg64.s64 = ((int64_t)acc * (int64_t)_delta);
+ tmp_reg64.s32.h += absDiffSignal;
+ tmp_round0 = (tmp_reg64.u32.l >> 4) | (tmp_reg64.s32.h << 28);
+ acc = tmp_reg64.s32.h + (1 << 2);
+ acc >>= 3;
+ if (tmp_round0 == 0x40000000L)
+ {
+ acc--;
+ }
+
+ tmp_qCode = qdata_pt->qCode;
+ tmp_altQcode = tmp_qCode - 1;
+ /* Check the sign of the distance penalty. Get the sign from the
+ * full-precision accumulator, as done in the Kalimba code. */
+ if (tmp_reg64.s32.h < 0)
+ {
+ /* The distance is -ve. The optimum code needs decremented by 1 and the
+ * alternative code is 1 greater than this. Get the rounded version of the
+ * -ve distance penalty and negate this (form distance magnitude) before
+ * writing the value out */
+ tmp_qCode = tmp_altQcode;
+ tmp_altQcode++;
+ acc = -acc;
+ }
+
+ qdata_pt->distPenalty = acc;
+ /* If the difference signal is negative, bitwise invert the code (restores
+ * sign to the magnitude). */
+ if (diffSignal < 0)
+ {
+ tmp_qCode = ~tmp_qCode;
+ tmp_altQcode = ~tmp_altQcode;
+ }
+ qdata_pt->altQcode = tmp_altQcode;
+ qdata_pt->qCode = tmp_qCode;
+}
+
+void quantiseDifferenceHH(const int32_t diffSignal, const int32_t ditherVal, const int32_t delta, Quantiser_data* qdata_pt)
+{
+ int32_t absDiffSignal;
+ int32_t absDiffSignalShifted;
+ int32_t index;
+ int32_t dithSquared;
+ int32_t minusLambdaD;
+ int32_t acc;
+ int32_t threshDiff;
+ reg64_t tmp_acc;
+ reg64_t tmp_reg64;
+ int32_t tmp_accL;
+ int32_t tmp_qCode;
+ int32_t tmp_altQcode;
+ uint32_t tmp_round0;
+ int32_t _delta;
+
+ /* Form the absolute value of the difference signal and maintain a version
+ * that is right-shifted 4 places for delta scaling. */
+ absDiffSignal = -diffSignal;
+ if (diffSignal >= 0) absDiffSignal = diffSignal;
+ absDiffSignal = ssat24(absDiffSignal);
+ absDiffSignalShifted = absDiffSignal >> deltaScale;
+ absDiffSignalShifted = ssat24(absDiffSignalShifted);
+
+ /* Binary search for the quantised code. This search terminates with the
+ * table index of the LARGEST threshold table value for which
+ * absDiffSignalShifted >= (delta * threshold)
+ */
+ index = BsearchHH(absDiffSignalShifted, delta, qdata_pt->thresholdTablePtr_sl1);
+
+ /* We actually wanted the SMALLEST magnitude quantised code for which
+ * absDiffSignalShifted < (delta * threshold)
+ * i.e. the code with the next highest magnitude than the one we actually
+ * found. We could add +1 to the code magnitude to do this, but we need to
+ * subtract 1 from the code magnitude to compensate for the "phantom
+ * element" at the base of the quantisation table. These two effects cancel
+ * out, so we leave the value of code alone. However, we need to form code+1
+ * to get the proper index into the both the threshold and dither tables,
+ * since we must skip over the phantom element at the base. */
+ qdata_pt->qCode = index;
+
+ /* Square the dither and get the value back from the ALU
+ * (saturated/rounded). */
+ tmp_acc.s64 = ((int64_t)ditherVal * (int64_t)ditherVal);
+
+ acc = tmp_acc.s32.h;
+
+ tmp_round0 = (uint32_t)acc << 8;
+
+ acc = (acc >> 6) + 1;
+ acc >>= 1;
+ if (tmp_round0 == 0x40000000L)
+ acc--;
+
+ acc = ssat24(acc);
+
+ dithSquared = acc;
+
+ /* Form the negative difference of the dither values at index and index-1.
+ * Load the accumulator with this value divided by 2. Ensure saturation is
+ * applied to the difference calculation. */
+ minusLambdaD = qdata_pt->minusLambdaDTable[index];
+
+ tmp_accL = (1 << 23) - (int32_t)dithSquared;
+ tmp_acc.s64 = (int64_t)tmp_accL * minusLambdaD;
+
+ tmp_round0 = (int32_t)tmp_acc.s32.l << 8;
+
+ acc = (tmp_acc.u32.l >> 22) | (tmp_acc.s32.h << 10);
+ acc++;
+ acc >>= 1;
+ if (tmp_round0 == 0x40000000L)
+ acc--;
+
+ // worst case value for acc = 0x000d3e08
+ // no saturation required
+
+ /* Add the threshold table values at index and index-1 to the accumulated
+ * value. */
+ acc += qdata_pt->thresholdTablePtr_sl1[index + 1] >> 1;
+ //// worst case value for acc = 0x000d3e08 + 0x43E1DB = 511FE3
+ acc += qdata_pt->thresholdTablePtr_sl1[index] >> 1;
+ //// worst case value for acc = 0x511FE3 + 0x362FEC = 874FCF
+
+ /* Form the threshold table difference at index and index-1. Ensure
+ * saturation is applied to the difference calculation. */
+ threshDiff = qdata_pt->thresholdTablePtr_sl1[index + 1] - qdata_pt->thresholdTablePtr_sl1[index];
+
+ /* Based on the sign of the difference signal, either add or subtract the
+ * threshold table difference from the accumulated value. Recover the final
+ * accumulated value (saturated/rounded) */
+ if (diffSignal < 0)
+ threshDiff = -threshDiff;
+ tmp_reg64.s64 = ((int64_t)ditherVal * (int64_t)threshDiff);
+ tmp_reg64.s32.h += acc;
+ acc = tmp_reg64.s32.h;
+
+ if (tmp_reg64.u32.l >= 0x80000000)
+ acc++;
+ tmp_round0 = (tmp_reg64.u32.l >> 1) | (tmp_reg64.s32.h << 31);
+
+ acc = ssat24(acc);
+
+ if (tmp_round0 == 0x40000000L)
+ acc--;
+ _delta = -delta << 8;
+
+ acc = acc << 4;
+
+ /* Form (absDiffSignal * 0.125) - (acc * delta), which is the final distance
+ * signal used to determine if dithering alters the quantised code value or
+ * not. */
+ // worst case value for delta is 0x7d400
+ tmp_reg64.s64 = ((int64_t)acc * (int64_t)_delta);
+ tmp_reg64.s32.h += absDiffSignal;
+ tmp_round0 = (tmp_reg64.u32.l >> 4) | (tmp_reg64.s32.h << 28);
+ acc = tmp_reg64.s32.h + (1 << 2);
+ acc >>= 3;
+ if (tmp_round0 == 0x40000000L)
+ {
+ acc--;
+ }
+
+ tmp_qCode = qdata_pt->qCode;
+ tmp_altQcode = tmp_qCode - 1;
+ /* Check the sign of the distance penalty. Get the sign from the
+ * full-precision accumulator, as done in the Kalimba code. */
+ if (tmp_reg64.s32.h < 0)
+ {
+ /* The distance is -ve. The optimum code needs decremented by 1 and the
+ * alternative code is 1 greater than this. Get the rounded version of the
+ * -ve distance penalty and negate this (form distance magnitude) before
+ * writing the value out */
+ tmp_qCode = tmp_altQcode;
+ tmp_altQcode++;
+ acc = -acc;
+ }
+
+ qdata_pt->distPenalty = acc;
+ /* If the difference signal is negative, bitwise invert the code (restores
+ * sign to the magnitude). */
+ if (diffSignal < 0)
+ {
+ tmp_qCode = ~tmp_qCode;
+ tmp_altQcode = ~tmp_altQcode;
+ }
+ qdata_pt->altQcode = tmp_altQcode;
+ qdata_pt->qCode = tmp_qCode;
+}
+
+void quantiseDifferenceLL(const int32_t diffSignal, const int32_t ditherVal, const int32_t delta, Quantiser_data* qdata_pt)
+{
+ int32_t absDiffSignal;
+ int32_t absDiffSignalShifted;
+ int32_t index;
+ int32_t dithSquared;
+ int32_t minusLambdaD;
+ int32_t acc;
+ int32_t threshDiff;
+ reg64_t tmp_acc;
+ reg64_t tmp_reg64;
+ int32_t tmp_accL;
+ int32_t tmp_qCode;
+ int32_t tmp_altQcode;
+ uint32_t tmp_round0;
+ int32_t _delta;
+
+ /* Form the absolute value of the difference signal and maintain a version
+ * that is right-shifted 4 places for delta scaling. */
+ absDiffSignal = -diffSignal;
+ if (diffSignal >= 0) absDiffSignal = diffSignal;
+ absDiffSignal = ssat24(absDiffSignal);
+ absDiffSignalShifted = absDiffSignal >> deltaScale;
+
+ /* Binary search for the quantised code. This search terminates with the
+ * table index of the LARGEST threshold table value for which
+ * absDiffSignalShifted >= (delta * threshold)
+ */
+ index = BsearchLL(absDiffSignalShifted, delta, qdata_pt->thresholdTablePtr_sl1);
+
+ /* We actually wanted the SMALLEST magnitude quantised code for which
+ * absDiffSignalShifted < (delta * threshold)
+ * i.e. the code with the next highest magnitude than the one we actually
+ * found. We could add +1 to the code magnitude to do this, but we need to
+ * subtract 1 from the code magnitude to compensate for the "phantom
+ * element" at the base of the quantisation table. These two effects cancel
+ * out, so we leave the value of code alone. However, we need to form code+1
+ * to get the proper index into the both the threshold and dither tables,
+ * since we must skip over the phantom element at the base. */
+ qdata_pt->qCode = index;
+
+ /* Square the dither and get the value back from the ALU
+ * (saturated/rounded). */
+ tmp_acc.s64 = ((int64_t)ditherVal * (int64_t)ditherVal);
+
+ acc = tmp_acc.s32.h;
+
+ tmp_round0 = (uint32_t)acc << 8;
+
+ acc = (acc >> 6) + 1;
+ acc >>= 1;
+ if (tmp_round0 == 0x40000000L)
+ acc--;
+
+ acc = ssat24(acc);
+
+ dithSquared = acc;
+
+ /* Form the negative difference of the dither values at index and index-1.
+ * Load the accumulator with this value divided by 2. Ensure saturation is
+ * applied to the difference calculation. */
+ minusLambdaD = qdata_pt->minusLambdaDTable[index];
+
+ tmp_accL = (1 << 23) - (int32_t)dithSquared;
+ tmp_acc.s64 = (int64_t)tmp_accL * minusLambdaD;
+
+ tmp_round0 = (int32_t)tmp_acc.s32.l << 8;
+
+ tmp_acc.s64 >>= 22;
+ acc = tmp_acc.s32.l;
+ acc++;
+ acc >>= 1;
+ if (tmp_round0 == 0x40000000L)
+ acc--;
+
+ // worst case value for acc = 0x000d3e08
+ // no saturation required
+
+ /* Add the threshold table values at index and index-1 to the accumulated
+ * value. */
+ acc += qdata_pt->thresholdTablePtr_sl1[index + 1] >> 1;
+ //// worst case value for acc = 0x000d3e08 + 0x43E1DB = 511FE3
+ acc += qdata_pt->thresholdTablePtr_sl1[index] >> 1;
+ //// worst case value for acc = 0x511FE3 + 0x362FEC = 874FCF
+
+ /* Form the threshold table difference at index and index-1. Ensure
+ * saturation is applied to the difference calculation. */
+ threshDiff = qdata_pt->thresholdTablePtr_sl1[index + 1] - qdata_pt->thresholdTablePtr_sl1[index];
+
+ /* Based on the sign of the difference signal, either add or subtract the
+ * threshold table difference from the accumulated value. Recover the final
+ * accumulated value (saturated/rounded) */
+ if (diffSignal < 0)
+ threshDiff = -threshDiff;
+ tmp_reg64.s64 = ((int64_t)ditherVal * (int64_t)threshDiff);
+ tmp_reg64.s32.h += acc;
+ acc = tmp_reg64.s32.h;
+
+ if (tmp_reg64.u32.l >= 0x80000000)
+ acc++;
+ tmp_round0 = (tmp_reg64.u32.l >> 1) | (tmp_reg64.s32.h << 31);
+
+ acc = ssat24(acc);
+
+ if (tmp_round0 == 0x40000000L)
+ acc--;
+ _delta = -delta << 8;
+
+ acc = acc << 4;
+ /* Form (absDiffSignal * 0.125) - (acc * delta), which is the final distance
+ * signal used to determine if dithering alters the quantised code value or
+ * not. */
+ // worst case value for delta is 0x7d400
+
+ tmp_reg64.s64 = ((int64_t)acc * (int64_t)_delta);
+ tmp_reg64.s32.h += absDiffSignal;
+ tmp_round0 = (tmp_reg64.u32.l >> 4) | (tmp_reg64.s32.h << 28);
+ acc = tmp_reg64.s32.h + (1 << 2);
+ acc >>= 3;
+ if (tmp_round0 == 0x40000000L)
+ {
+ acc--;
+ }
+
+ tmp_qCode = qdata_pt->qCode;
+ tmp_altQcode = tmp_qCode - 1;
+ /* Check the sign of the distance penalty. Get the sign from the
+ * full-precision accumulator, as done in the Kalimba code. */
+ if (tmp_reg64.s32.h < 0)
+ {
+ /* The distance is -ve. The optimum code needs decremented by 1 and the
+ * alternative code is 1 greater than this. Get the rounded version of the
+ * -ve distance penalty and negate this (form distance magnitude) before
+ * writing the value out */
+ tmp_qCode = tmp_altQcode;
+ tmp_altQcode++;
+ acc = -acc;
+ }
+
+ qdata_pt->distPenalty = acc;
+ /* If the difference signal is negative, bitwise invert the code (restores
+ * sign to the magnitude). */
+ if (diffSignal < 0)
+ {
+ tmp_qCode = ~tmp_qCode;
+ tmp_altQcode = ~tmp_altQcode;
+ }
+ qdata_pt->altQcode = tmp_altQcode;
+ qdata_pt->qCode = tmp_qCode;
+}
+
+void quantiseDifferenceLH(const int32_t diffSignal, const int32_t ditherVal, const int32_t delta, Quantiser_data* qdata_pt)
+{
+ int32_t absDiffSignal;
+ int32_t absDiffSignalShifted;
+ int32_t index;
+ int32_t dithSquared;
+ int32_t minusLambdaD;
+ int32_t acc;
+ int32_t threshDiff;
+ reg64_t tmp_acc;
+ reg64_t tmp_reg64;
+ int32_t tmp_accL;
+ int32_t tmp_qCode;
+ int32_t tmp_altQcode;
+ uint32_t tmp_round0;
+ int32_t _delta;
+
+ /* Form the absolute value of the difference signal and maintain a version
+ * that is right-shifted 4 places for delta scaling. */
+ absDiffSignal = -diffSignal;
+ if (diffSignal >= 0) absDiffSignal = diffSignal;
+ absDiffSignal = ssat24(absDiffSignal);
+ absDiffSignalShifted = absDiffSignal >> deltaScale;
+
+ /* Binary search for the quantised code. This search terminates with the
+ * table index of the LARGEST threshold table value for which
+ * absDiffSignalShifted >= (delta * threshold)
+ */
+ index = BsearchLH(absDiffSignalShifted, delta, qdata_pt->thresholdTablePtr_sl1);
+
+ /* We actually wanted the SMALLEST magnitude quantised code for which
+ * absDiffSignalShifted < (delta * threshold)
+ * i.e. the code with the next highest magnitude than the one we actually
+ * found. We could add +1 to the code magnitude to do this, but we need to
+ * subtract 1 from the code magnitude to compensate for the "phantom
+ * element" at the base of the quantisation table. These two effects cancel
+ * out, so we leave the value of code alone. However, we need to form code+1
+ * to get the proper index into the both the threshold and dither tables,
+ * since we must skip over the phantom element at the base. */
+ qdata_pt->qCode = index;
+
+ /* Square the dither and get the value back from the ALU
+ * (saturated/rounded). */
+ tmp_reg64.s64 = ((int64_t)ditherVal * (int64_t)ditherVal);
+
+ acc = tmp_reg64.s32.h;
+
+ tmp_round0 = (uint32_t)acc << 8;
+
+ acc = (acc >> 6) + 1;
+ acc >>= 1;
+ if (tmp_round0 == 0x40000000L)
+ acc--;
+
+ acc = ssat24(acc);
+
+ dithSquared = acc;
+
+ /* Form the negative difference of the dither values at index and index-1.
+ * Load the accumulator with this value divided by 2. Ensure saturation is
+ * applied to the difference calculation. */
+ minusLambdaD = qdata_pt->minusLambdaDTable[index];
+
+ tmp_accL = (1 << 23) - (int32_t)dithSquared;
+ tmp_acc.s64 = (int64_t)tmp_accL * minusLambdaD;
+
+ tmp_round0 = (int32_t)tmp_acc.s32.l << 8;
+
+ acc = (tmp_acc.u32.l >> 22) | (tmp_acc.s32.h << 10);
+ if (tmp_round0 == 0x40000000L)
+ acc -= 2;
+ acc++;
+
+ // worst case value for acc = 0x000d3e08
+ // no saturation required
+ /* Add the threshold table values at index and index-1 to the accumulated
+ * value. */
+ acc += qdata_pt->thresholdTablePtr_sl1[index + 1];
+ //// worst case value for acc = 0x000d3e08 + 0x43E1DB = 511FE3
+ acc += qdata_pt->thresholdTablePtr_sl1[index];
+ acc >>= 1;
+
+ /* Form the threshold table difference at index and index-1. Ensure
+ * saturation is applied to the difference calculation. */
+ threshDiff = qdata_pt->thresholdTablePtr_sl1[index + 1] - qdata_pt->thresholdTablePtr_sl1[index];
+
+ /* Based on the sign of the difference signal, either add or subtract the
+ * threshold table difference from the accumulated value. Recover the final
+ * accumulated value (saturated/rounded) */
+ if (diffSignal < 0)
+ threshDiff = -threshDiff;
+ tmp_reg64.s64 = ((int64_t)ditherVal * (int64_t)threshDiff);
+
+ tmp_reg64.s32.h += acc;
+ acc = tmp_reg64.s32.h;
+
+ if (tmp_reg64.u32.l >= 0x80000000)
+ acc++;
+ tmp_round0 = (tmp_reg64.u32.l >> 1) | (tmp_reg64.s32.h << 31);
+
+ acc = ssat24(acc);
+
+ if (tmp_round0 == 0x40000000L)
+ acc--;
+ _delta = -delta << 8;
+
+ acc = acc << 4;
+
+ /* Form (absDiffSignal * 0.125) - (acc * delta), which is the final distance
+ * signal used to determine if dithering alters the quantised code value or
+ * not. */
+ // worst case value for delta is 0x7d400
+ tmp_reg64.s64 = ((int64_t)acc * (int64_t)_delta);
+ tmp_reg64.s32.h += absDiffSignal;
+ tmp_round0 = (tmp_reg64.u32.l >> 4) | (tmp_reg64.s32.h << 28);
+ acc = tmp_reg64.s32.h + (1 << 2);
+ acc >>= 3;
+ if (tmp_round0 == 0x40000000L)
+ {
+ acc--;
+ }
+
+ tmp_qCode = qdata_pt->qCode;
+ tmp_altQcode = tmp_qCode - 1;
+ /* Check the sign of the distance penalty. Get the sign from the
+ * full-precision accumulator, as done in the Kalimba code. */
+
+ if (tmp_reg64.s32.h < 0)
+ {
+ /* The distance is -ve. The optimum code needs decremented by 1 and the
+ * alternative code is 1 greater than this. Get the rounded version of the
+ * -ve distance penalty and negate this (form distance magnitude) before
+ * writing the value out */
+ tmp_qCode = tmp_altQcode;
+ tmp_altQcode++;
+ acc = -acc;
+ }
+
+ qdata_pt->distPenalty = acc;
+ /* If the difference signal is negative, bitwise invert the code (restores
+ * sign to the magnitude). */
+ if (diffSignal < 0)
+ {
+ tmp_qCode = ~tmp_qCode;
+ tmp_altQcode = ~tmp_altQcode;
+ }
+ qdata_pt->altQcode = tmp_altQcode;
+ qdata_pt->qCode = tmp_qCode;
+}
diff --git a/system/embdrv/encoder_for_aptx/src/Quantiser.h b/system/embdrv/encoder_for_aptx/src/Quantiser.h
new file mode 100644
index 00000000000..63a32c65905
--- /dev/null
+++ b/system/embdrv/encoder_for_aptx/src/Quantiser.h
@@ -0,0 +1,27 @@
+/*------------------------------------------------------------------------------
+ *
+ * Function to calculate a quantised representation of an input
+ * difference signal, based on additional dither values and step-size inputs.
+ *
+ *-----------------------------------------------------------------------------*/
+
+#ifndef QUANTISER_H
+#define QUANTISER_H
+#ifdef _GCC
+ #pragma GCC visibility push(hidden)
+#endif
+
+
+#include "AptxParameters.h"
+
+
+void quantiseDifferenceLL(const int32_t diffSignal, const int32_t ditherVal, const int32_t delta, Quantiser_data* qdata_pt);
+void quantiseDifferenceHL(const int32_t diffSignal, const int32_t ditherVal, const int32_t delta, Quantiser_data* qdata_pt);
+void quantiseDifferenceLH(const int32_t diffSignal, const int32_t ditherVal, const int32_t delta, Quantiser_data* qdata_pt);
+void quantiseDifferenceHH(const int32_t diffSignal, const int32_t ditherVal, const int32_t delta, Quantiser_data* qdata_pt);
+
+
+#ifdef _GCC
+ #pragma GCC visibility pop
+#endif
+#endif //QUANTISER_H
diff --git a/system/embdrv/encoder_for_aptx/src/SubbandFunctions.h b/system/embdrv/encoder_for_aptx/src/SubbandFunctions.h
new file mode 100644
index 00000000000..b8e743d480c
--- /dev/null
+++ b/system/embdrv/encoder_for_aptx/src/SubbandFunctions.h
@@ -0,0 +1,184 @@
+/*------------------------------------------------------------------------------
+ *
+ * Subband processing consists of:
+ * inverse quantisation (defined in a separate file),
+ * predictor coefficient update (Pole and Zero Coeff update),
+ * predictor filtering.
+ *
+ *----------------------------------------------------------------------------*/
+
+#ifndef SUBBANDFUNCTIONS_H
+#define SUBBANDFUNCTIONS_H
+#ifdef _GCC
+ #pragma GCC visibility push(hidden)
+#endif
+
+
+#include "AptxParameters.h"
+
+
+XBT_INLINE_ void updatePredictorPoleCoefficients(const int32_t invQ, const int32_t prevZfiltOutput, PoleCoeff_data* PoleCoeffDataPt)
+{
+ int32_t adaptSum;
+ int32_t sgnP[3];
+ int32_t newCoeffs[2];
+ int32_t Bacc;
+ int32_t acc;
+ int32_t acc2;
+ int32_t tmp3_round0;
+ int16_t tmp2_round0;
+ int16_t tmp_round0;
+ /* Various constants in various Q formats */
+ const int32_t oneQ22 = 4194304L;
+ const int32_t minusOneQ22 = -4194304L;
+ const int32_t pointFiveQ21 = 1048576L;
+ const int32_t minusPointFiveQ21 = -1048576L;
+ const int32_t pointSevenFiveQ22 = 3145728L;
+ const int32_t minusPointSevenFiveQ22 = -3145728L;
+ const int32_t oneMinusTwoPowerMinusFourQ22 = 3932160L;
+
+ /* Symbolic indicies for the pole coefficient arrays. Here we are using a1
+ * to represent the first pole filter coefficient and a2 the second. This
+ * seems to be common ADPCM terminology. */
+ enum { a1 = 0, a2 = 1 };
+
+ /* Symbolic indicies for the sgn array (k, k-1 and k-2 respectively) */
+ enum { k = 0, k_1 = 1, k_2 = 2 };
+
+ /* Form the sum of the inverse quantiser and previous zero filter values */
+ adaptSum = invQ + prevZfiltOutput;
+ adaptSum = ssat24(adaptSum);
+
+ /* Form the sgn of the sum just formed (note +1 and -1 are Q22) */
+ if (adaptSum < 0L)
+ {
+ sgnP[k] = minusOneQ22;
+ sgnP[k_1] = -(((int32_t)PoleCoeffDataPt->m_poleAdaptDelayLine.s16.l) << 22);
+ sgnP[k_2] = -(((int32_t)PoleCoeffDataPt->m_poleAdaptDelayLine.s16.h) << 22);
+ PoleCoeffDataPt->m_poleAdaptDelayLine.s16.h = PoleCoeffDataPt->m_poleAdaptDelayLine.s16.l;
+ PoleCoeffDataPt->m_poleAdaptDelayLine.s16.l = -1;
+ }
+ if (adaptSum == 0L)
+ {
+ sgnP[k] = 0L;
+ sgnP[k_1] = 0L;
+ sgnP[k_2] = 0L;
+ PoleCoeffDataPt->m_poleAdaptDelayLine.s16.h = PoleCoeffDataPt->m_poleAdaptDelayLine.s16.l;
+ PoleCoeffDataPt->m_poleAdaptDelayLine.s16.l = 1;
+ }
+ if (adaptSum > 0L)
+ {
+ sgnP[k] = oneQ22;
+ sgnP[k_1] = ((int32_t)PoleCoeffDataPt->m_poleAdaptDelayLine.s16.l) << 22;
+ sgnP[k_2] = ((int32_t)PoleCoeffDataPt->m_poleAdaptDelayLine.s16.h) << 22;
+ PoleCoeffDataPt->m_poleAdaptDelayLine.s16.h = PoleCoeffDataPt->m_poleAdaptDelayLine.s16.l;
+ PoleCoeffDataPt->m_poleAdaptDelayLine.s16.l = 1;
+ }
+
+ /* Clear the accumulator and form -a1(k) * sgn(p(k))sgn(p(k-1)) in Q21. Clip
+ * it to +/- 0.5 (Q21) so that we can take f(a1) = 4 * a1. This is a partial
+ * result for the new a2 */
+ acc = 0;
+ acc -= PoleCoeffDataPt->m_poleCoeff[a1] * (sgnP[k_1] >> 22);
+
+ tmp3_round0 = acc & 0x3L;
+
+ acc += 0x001;
+ acc >>= 1;
+ if (tmp3_round0 == 0x001L)
+ {
+ acc--;
+ }
+
+ newCoeffs[a2] = acc;
+
+ if (newCoeffs[a2] < minusPointFiveQ21)
+ {
+ newCoeffs[a2] = minusPointFiveQ21;
+ }
+ if (newCoeffs[a2] > pointFiveQ21)
+ {
+ newCoeffs[a2] = pointFiveQ21;
+ }
+
+ /* Load the accumulator with sgn(p(k))sgn(p(k-2)) right-shifted by 3. The
+ * 3-position shift is to multiply it by 0.25 and convert from Q22 to Q21.
+ * */
+ Bacc = (sgnP[k_2] >> 3);
+ /* Add the current a2 update value to the accumulator (Q21) */
+ Bacc += newCoeffs[a2];
+ /* Shift the accumulator right by 4 positions.
+ * Right 7 places to multiply by 2^(-7)
+ * Left 2 places to scale by 4 (0.25A + B -> A + 4B)
+ * Left 1 place to convert from Q21 to Q22
+ */
+ Bacc >>= 4;
+ /* Add a2(k-1) * (1 - 2^(-7)) to the accumulator. Note that the constant is
+ * expressed as Q23, hence the product is Q22. Get the accumulator value
+ * back out. */
+ acc2 = PoleCoeffDataPt->m_poleCoeff[a2] << 8;
+ acc2 -= PoleCoeffDataPt->m_poleCoeff[a2] << 1;
+ Bacc <<= 8;
+ Bacc += acc2;
+
+ tmp2_round0 = (int16_t)Bacc & 0x01FFL;
+
+ Bacc += 0x0080L;
+ Bacc >>= 8;
+
+ if (tmp2_round0 == 0x0080L)
+ {
+ Bacc--;
+ }
+
+ newCoeffs[a2] = Bacc;
+
+ /* Clip the new a2(k) value to +/- 0.75 (Q22) */
+ if (newCoeffs[a2] < minusPointSevenFiveQ22)
+ {
+ newCoeffs[a2] = minusPointSevenFiveQ22;
+ }
+ if (newCoeffs[a2] > pointSevenFiveQ22)
+ {
+ newCoeffs[a2] = pointSevenFiveQ22;
+ }
+ PoleCoeffDataPt->m_poleCoeff[a2] = newCoeffs[a2];
+
+ /* Form sgn(p(k))sgn(p(k-1)) * (3 * 2^(-8)). The constant is Q23, hence the
+ * product is Q22. */
+ /* Add a1(k-1) * (1 - 2^(-8)) to the accumulator. The constant is Q23, hence
+ * the product is Q22. Get the value from the accumulator. */
+ acc2 = PoleCoeffDataPt->m_poleCoeff[a1] << 8;
+ acc2 -= PoleCoeffDataPt->m_poleCoeff[a1];
+ acc2 += (sgnP[k_1] << 2);
+ acc2 -= (sgnP[k_1]);
+
+ tmp_round0 = (int16_t)acc2 & 0x01FF;
+
+ acc2 += 0x0080;
+ acc = (acc2 >> 8);
+ if (tmp_round0 == 0x0080)
+ {
+ acc--;
+ }
+
+ newCoeffs[a1] = (int32_t)acc;
+
+ /* Clip the new value of a1(k) to +/- (1 - 2^4 - a2(k)). The constant 1 -
+ * 2^4 is expressed in Q22 format (as is a1 and a2) */
+ if (newCoeffs[a1] < (newCoeffs[a2] - oneMinusTwoPowerMinusFourQ22))
+ {
+ newCoeffs[a1] = newCoeffs[a2] - oneMinusTwoPowerMinusFourQ22;
+ }
+ if (newCoeffs[a1] > (oneMinusTwoPowerMinusFourQ22 - newCoeffs[a2]))
+ {
+ newCoeffs[a1] = oneMinusTwoPowerMinusFourQ22 - newCoeffs[a2];
+ }
+ PoleCoeffDataPt->m_poleCoeff[a1] = newCoeffs[a1];
+}
+
+
+#ifdef _GCC
+ #pragma GCC visibility pop
+#endif
+#endif //SUBBANDFUNCTIONS_H
diff --git a/system/embdrv/encoder_for_aptx/src/SubbandFunctionsCommon.h b/system/embdrv/encoder_for_aptx/src/SubbandFunctionsCommon.h
new file mode 100644
index 00000000000..3f8f05e240d
--- /dev/null
+++ b/system/embdrv/encoder_for_aptx/src/SubbandFunctionsCommon.h
@@ -0,0 +1,515 @@
+/*------------------------------------------------------------------------------
+ *
+ * Subband processing consists of:
+ * inverse quantisation (defined in a separate file),
+ * predictor coefficient update (Pole and Zero Coeff update),
+ * predictor filtering.
+ *
+ *----------------------------------------------------------------------------*/
+
+#ifndef SUBBANDFUNCTIONSCOMMON_H
+#define SUBBANDFUNCTIONSCOMMON_H
+
+
+enum reg64_reg{reg64_H=1,reg64_L=0};
+
+void processSubband(const int32_t qCode, const int32_t ditherVal, Subband_data* SubbandDataPt, IQuantiser_data* iqDataPt);
+void processSubbandLL(const int32_t qCode, const int32_t ditherVal, Subband_data* SubbandDataPt, IQuantiser_data* iqDataPt);
+void processSubbandHL(const int32_t qCode, const int32_t ditherVal, Subband_data* SubbandDataPt, IQuantiser_data* iqDataPt);
+
+
+/* Function to carry out inverse quantisation for LL, LH and HH subband types */
+XBT_INLINE_ void invertQuantisation(const int32_t qCode, const int32_t ditherVal, IQuantiser_data* iqdata_pt)
+{
+ int32_t invQ;
+ int32_t index;
+ int32_t acc;
+ reg64_t tmp_r64;
+ int64_t tmp_acc;
+ int32_t tmp_accL;
+ int32_t tmp_accH;
+ uint32_t tmp_round0;
+ uint32_t tmp_round1;
+
+ unsigned u16t;
+ /* log delta leak value (Q23) */
+ const uint32_t logDeltaLeakVal = 0x7F6CL;
+
+ /* Turn the quantised code back into an index into the threshold table. This
+ * involves bitwise inversion of the code (if -ve) and adding 1 (phantom
+ * element at table base). Then set invQ to be +/- the threshold value,
+ * depending on the code sign. */
+ index = qCode;
+ if (qCode < 0)
+ index = (~index);
+ index = index + 1;
+ invQ = iqdata_pt->thresholdTablePtr_sl1[index];
+ if (qCode < 0)
+ invQ = -invQ;
+
+ /* Load invQ into the accumulator. Add the product of the dither value times
+ * the indexed dither table value. Then get the result back from the
+ * accumulator as an updated invQ. */
+ tmp_r64.s64 = ((int64_t)ditherVal * iqdata_pt->ditherTablePtr_sf1[index]);
+ tmp_r64.s32.h += invQ >> 1;
+
+ acc = tmp_r64.s32.h;
+
+ tmp_round1 = tmp_r64.s32.h & 0x00000001L;
+ if (tmp_r64.u32.l >= 0x80000000)
+ acc++;
+ if (tmp_round1 == 0 && tmp_r64.s32.l == (int32_t)0x80000000L)
+ acc--;
+ acc = ssat24(acc);
+
+ invQ = (int32_t)acc;
+
+ /* Scale invQ by the current delta value. Left-shift the result (in the
+ * accumulator) by 4 positions for the delta scaling. Get the updated invQ
+ * back from the accumulator. */
+
+ u16t = iqdata_pt->logDelta;
+ tmp_acc = ((int64_t)invQ * iqdata_pt->delta);
+ tmp_accL = u16t * logDeltaLeakVal;
+ tmp_accH = iqdata_pt->incrTablePtr[index];
+ acc = (int32_t)(tmp_acc >> (23 - deltaScale));
+ invQ = ssat24(acc);
+
+ /* Now update the value of logDelta. Load the accumulator with the index
+ * value of the logDelta increment table. Add the product of the current
+ * logDelta scaled by a leaky coefficient (16310 in Q14). Get the value back
+ * from the accumulator. */
+ tmp_accH += tmp_accL >> (32 - 17);
+
+ acc = tmp_accH;
+
+ tmp_r64.u32.l = ((uint32_t)tmp_accL << 17);
+ tmp_r64.s32.h = tmp_accH;
+
+ tmp_round0 = tmp_r64.u32.l;
+ tmp_round1 = (int32_t)(tmp_r64.u64 >> 1);
+ if (tmp_round0 >= 0x80000000L)
+ acc++;
+ if (tmp_round1 == 0x40000000L)
+ acc--;
+
+ /* Limit the updated logDelta between 0 and its subband-specific maximum. */
+ if (acc < 0)
+ acc = 0;
+ if (acc > iqdata_pt->maxLogDelta)
+ acc = iqdata_pt->maxLogDelta;
+
+ iqdata_pt->logDelta = (uint16_t)acc;
+
+ /* The updated value of delta is the logTable output (indexed by 5 bits from
+ * the updated logDelta) shifted by a value involving the logDelta minimum
+ * and the updated logDelta itself. */
+ iqdata_pt->delta = iqdata_pt->iquantTableLogPtr[(acc >> 3) & 0x1f] >>
+ (22 - 25 - iqdata_pt->minLogDelta - (acc >> 8));
+
+ iqdata_pt->invQ = invQ;
+}
+
+/* Function to carry out inverse quantisation for a HL subband type */
+XBT_INLINE_ void invertQuantisationHL(const int32_t qCode, const int32_t ditherVal, IQuantiser_data* iqdata_pt)
+{
+ int32_t invQ;
+ int32_t index;
+ int32_t acc;
+ reg64_t tmp_r64;
+ int64_t tmp_acc;
+ int32_t tmp_accL;
+ int32_t tmp_accH;
+ uint32_t tmp_round0;
+ uint32_t tmp_round1;
+
+ unsigned u16t;
+ /* log delta leak value (Q23) */
+ const uint32_t logDeltaLeakVal = 0x7F6CL;
+
+ /* Turn the quantised code back into an index into the threshold table. This
+ * involves bitwise inversion of the code (if -ve) and adding 1 (phantom
+ * element at table base). Then set invQ to be +/- the threshold value,
+ * depending on the code sign. */
+ index = qCode;
+ if (qCode < 0)
+ index = (~index);
+ index = index + 1;
+ invQ = iqdata_pt->thresholdTablePtr_sl1[index];
+ if (qCode < 0)
+ invQ = -invQ;
+
+ /* Load invQ into the accumulator. Add the product of the dither value times
+ * the indexed dither table value. Then get the result back from the
+ * accumulator as an updated invQ. */
+ tmp_r64.s64 = ((int64_t)ditherVal * iqdata_pt->ditherTablePtr_sf1[index]);
+ tmp_r64.s32.h += invQ >> 1;
+
+ acc = tmp_r64.s32.h;
+
+ tmp_round1 = tmp_r64.s32.h & 0x00000001L;
+ if (tmp_r64.u32.l >= 0x80000000)
+ acc++;
+ if (tmp_round1 == 0 && tmp_r64.u32.l == 0x80000000L)
+ acc--;
+ acc = ssat24(acc);
+
+ invQ = (int32_t)acc;
+
+ /* Scale invQ by the current delta value. Left-shift the result (in the
+ * accumulator) by 4 positions for the delta scaling. Get the updated invQ
+ * back from the accumulator. */
+ u16t = iqdata_pt->logDelta;
+ tmp_acc = ((int64_t)invQ * iqdata_pt->delta);
+ tmp_accL = u16t * logDeltaLeakVal;
+ tmp_accH = iqdata_pt->incrTablePtr[index];
+ acc = (int32_t)(tmp_acc >> (23 - deltaScale));
+ invQ = acc;
+
+ /* Now update the value of logDelta. Load the accumulator with the index
+ * value of the logDelta increment table. Add the product of the current
+ * logDelta scaled by a leaky coefficient (16310 in Q14). Get the value back
+ * from the accumulator. */
+ tmp_accH += tmp_accL >> (32 - 17);
+
+ acc = tmp_accH;
+
+ tmp_r64.u32.l = ((uint32_t)tmp_accL << 17);
+ tmp_r64.s32.h = tmp_accH;
+
+ tmp_round0 = tmp_r64.u32.l;
+ tmp_round1 = (int32_t)(tmp_r64.u64 >> 1);
+ if (tmp_round0 >= 0x80000000L)
+ acc++;
+ if (tmp_round1 == 0x40000000L)
+ acc--;
+
+ /* Limit the updated logDelta between 0 and its subband-specific maximum. */
+ if (acc < 0)
+ acc = 0;
+ if (acc > iqdata_pt->maxLogDelta)
+ acc = iqdata_pt->maxLogDelta;
+
+ iqdata_pt->logDelta = (uint16_t)acc;
+
+ /* The updated value of delta is the logTable output (indexed by 5 bits from
+ * the updated logDelta) shifted by a value involving the logDelta minimum
+ * and the updated logDelta itself. */
+ iqdata_pt->delta = iqdata_pt->iquantTableLogPtr[(acc >> 3) & 0x1f] >>
+ (22 - 25 - iqdata_pt->minLogDelta - (acc >> 8));
+
+ iqdata_pt->invQ = invQ;
+}
+
+/* Function to carry out prediction ARMA filtering for the current subband
+ * performPredictionFiltering should only be used for HH and LH subband! */
+XBT_INLINE_ void performPredictionFiltering(const int32_t invQ, Subband_data* SubbandDataPt)
+{
+ int32_t poleVal;
+ int32_t acc;
+ int64_t accL;
+ uint32_t pointer;
+ int32_t poleDelayLine;
+ int32_t predVal;
+ int32_t* zeroCoeffPt = SubbandDataPt->m_ZeroCoeffData.m_zeroCoeff;
+ int32_t* poleCoeff = SubbandDataPt->m_PoleCoeffData.m_poleCoeff;
+ int32_t zData0;
+ int32_t* cbuf_pt;
+ int32_t invQincr_pos;
+ int32_t invQincr_neg;
+ int32_t k;
+ int32_t oldZData;
+ /* Pole coefficient and data indicies */
+ enum { a1 = 0, a2 = 1 };
+
+ /* Write the newest pole input sample to the pole delay line.
+ * Ensure the sum of the current dequantised error and the previous
+ * predictor output is saturated if necessary. */
+ poleDelayLine = invQ + SubbandDataPt->m_predData.m_predVal;
+
+ poleDelayLine = ssat24(poleDelayLine);
+
+ /* Pole filter convolution. Shift convolution result 1 place to the left
+ * before retrieving it, since the pole coefficients are Q22 (data is Q23)
+ * and we want a Q23 result */
+ accL = ((int64_t)poleCoeff[a2] * (int64_t)SubbandDataPt->m_predData.m_poleDelayLine[a2]);
+ /* Update the pole delay line for the next pass by writing the new input
+ * sample into the 2nd element */
+ SubbandDataPt->m_predData.m_poleDelayLine[a2] = poleDelayLine;
+ accL += ((int64_t)poleCoeff[a1] * (int64_t)poleDelayLine);
+ poleVal = (int32_t)(accL >> 22);
+ poleVal = ssat24(poleVal);
+
+ /* Create (2^(-7)) * sgn(invQ) in Q22 format. */
+ if (invQ == 0)
+ {
+ invQincr_pos = 0L;
+ }
+ else
+ {
+ invQincr_pos = 0x800000;
+ }
+ if (invQ < 0L)
+ {
+ invQincr_pos = -invQincr_pos;
+ }
+
+ invQincr_neg = 0x0080 - invQincr_pos;
+ invQincr_pos += 0x0080;
+
+ pointer = (SubbandDataPt->m_predData.m_zeroDelayLine.pointer++) + 12;
+ cbuf_pt = &SubbandDataPt->m_predData.m_zeroDelayLine.buffer[pointer];
+ /* partial manual unrolling to improve performance */
+ if (SubbandDataPt->m_predData.m_zeroDelayLine.pointer >= 12)
+ SubbandDataPt->m_predData.m_zeroDelayLine.pointer = 0;
+
+ SubbandDataPt->m_predData.m_zeroDelayLine.modulo = invQ;
+
+ /* Iterate over the number of coefficients for this subband */
+ oldZData = invQ;
+ accL = 0;
+ for (k = 0; k < 12; k++)
+ {
+ uint32_t tmp_round0;
+ int32_t coeffValue;
+
+ zData0 = (*(cbuf_pt--));
+ coeffValue = *(zeroCoeffPt + k);
+ if (zData0 < 0L)
+ {
+ acc = invQincr_neg - coeffValue;
+ }
+ else
+ {
+ acc = invQincr_pos - coeffValue;
+ }
+ tmp_round0 = acc;
+ acc = (acc >> 8) + coeffValue;
+ if (((tmp_round0 << 23) ^ 0x80000000) == 0) acc--;
+ accL += (int64_t)acc * (int64_t)(oldZData);
+ oldZData = zData0;
+ *(zeroCoeffPt + k) = acc;
+ }
+
+ acc = (int32_t)(accL >> 22);
+ acc = ssat24(acc);
+ /* Predictor output is the sum of the pole and zero filter outputs. Ensure
+ * this is saturated, if necessary. */
+ predVal = acc + poleVal;
+ predVal = ssat24(predVal);
+ SubbandDataPt->m_predData.m_zeroVal = acc;
+ SubbandDataPt->m_predData.m_predVal = predVal;
+
+ /* Update the zero filter delay line by writing the new input sample to the
+ * circular buffer. */
+ SubbandDataPt->m_predData.m_zeroDelayLine.buffer[SubbandDataPt->m_predData.m_zeroDelayLine.pointer] = SubbandDataPt->m_predData.m_zeroDelayLine.modulo;
+ SubbandDataPt->m_predData.m_zeroDelayLine.buffer[SubbandDataPt->m_predData.m_zeroDelayLine.pointer + 12] = SubbandDataPt->m_predData.m_zeroDelayLine.modulo;
+}
+
+XBT_INLINE_ void performPredictionFilteringLL(const int32_t invQ, Subband_data* SubbandDataPt)
+{
+ int32_t poleVal;
+ int32_t acc;
+ int64_t accL;
+ uint32_t pointer;
+ int32_t poleDelayLine;
+ int32_t predVal;
+ int32_t* zeroCoeffPt = SubbandDataPt->m_ZeroCoeffData.m_zeroCoeff;
+ int32_t* poleCoeff = SubbandDataPt->m_PoleCoeffData.m_poleCoeff;
+ int32_t* cbuf_pt;
+ int32_t invQincr_pos;
+ int32_t invQincr_neg;
+ int32_t k;
+ int32_t oldZData;
+ /* Pole coefficient and data indicies */
+ enum { a1 = 0, a2 = 1 };
+
+ /* Write the newest pole input sample to the pole delay line.
+ * Ensure the sum of the current dequantised error and the previous
+ * predictor output is saturated if necessary. */
+ poleDelayLine = invQ + SubbandDataPt->m_predData.m_predVal;
+
+ poleDelayLine = ssat24(poleDelayLine);
+
+ /* Pole filter convolution. Shift convolution result 1 place to the left
+ * before retrieving it, since the pole coefficients are Q22 (data is Q23)
+ * and we want a Q23 result */
+ accL = ((int64_t)poleCoeff[a2] * (int64_t)SubbandDataPt->m_predData.m_poleDelayLine[a2]);
+ /* Update the pole delay line for the next pass by writing the new input
+ * sample into the 2nd element */
+ SubbandDataPt->m_predData.m_poleDelayLine[a2] = poleDelayLine;
+ accL += ((int64_t)poleCoeff[a1] * (int64_t)poleDelayLine);
+ poleVal = (int32_t)(accL >> 22);
+ poleVal = ssat24(poleVal);
+ // store poleVal to free one register.
+ SubbandDataPt->m_predData.m_predVal = poleVal;
+
+ /* Create (2^(-7)) * sgn(invQ) in Q22 format. */
+ if (invQ == 0)
+ {
+ invQincr_pos = 0L;
+ }
+ else
+ {
+ invQincr_pos = 0x800000;
+ }
+ if (invQ < 0L)
+ {
+ invQincr_pos = -invQincr_pos;
+ }
+
+ invQincr_neg = 0x0080 - invQincr_pos;
+ invQincr_pos += 0x0080;
+
+ pointer = (SubbandDataPt->m_predData.m_zeroDelayLine.pointer++) + 24;
+ cbuf_pt = &SubbandDataPt->m_predData.m_zeroDelayLine.buffer[pointer];
+ /* partial manual unrolling to improve performance */
+ if (SubbandDataPt->m_predData.m_zeroDelayLine.pointer >= 24)
+ SubbandDataPt->m_predData.m_zeroDelayLine.pointer = 0;
+
+ SubbandDataPt->m_predData.m_zeroDelayLine.modulo = invQ;
+
+ /* Iterate over the number of coefficients for this subband */
+
+ oldZData = invQ;
+ accL = 0;
+ for (k = 0; k < 24; k++)
+ {
+ int32_t zData0;
+ int32_t coeffValue;
+
+ zData0 = (*(cbuf_pt--));
+ coeffValue = *(zeroCoeffPt + k);
+ if (zData0 < 0L) {
+ acc = invQincr_neg - coeffValue;
+ }
+ else {
+ acc = invQincr_pos - coeffValue;
+ }
+ if (((acc << 23) ^ 0x80000000) == 0) coeffValue--;
+ acc = (acc >> 8) + coeffValue;
+ accL += (int64_t)acc * (int64_t)(oldZData);
+ oldZData = zData0;
+ *(zeroCoeffPt + k) = acc;
+ }
+
+ acc = (int32_t)(accL >> 22);
+ acc = ssat24(acc);
+ /* Predictor output is the sum of the pole and zero filter outputs. Ensure
+ * this is saturated, if necessary. */
+ // recover value of PoleVal stored at beginning of routine...
+ predVal = acc + SubbandDataPt->m_predData.m_predVal;
+ predVal = ssat24(predVal);
+ SubbandDataPt->m_predData.m_zeroVal = acc;
+ SubbandDataPt->m_predData.m_predVal = predVal;
+
+ /* Update the zero filter delay line by writing the new input sample to the
+ * circular buffer. */
+ SubbandDataPt->m_predData.m_zeroDelayLine.buffer[SubbandDataPt->m_predData.m_zeroDelayLine.pointer] = SubbandDataPt->m_predData.m_zeroDelayLine.modulo;
+ SubbandDataPt->m_predData.m_zeroDelayLine.buffer[SubbandDataPt->m_predData.m_zeroDelayLine.pointer + 24] = SubbandDataPt->m_predData.m_zeroDelayLine.modulo;
+}
+
+XBT_INLINE_ void performPredictionFilteringHL(const int32_t invQ, Subband_data* SubbandDataPt)
+{
+ int32_t poleVal;
+ int32_t acc;
+ int64_t accL;
+ uint32_t pointer;
+ int32_t poleDelayLine;
+ int32_t predVal;
+ int32_t* zeroCoeffPt = SubbandDataPt->m_ZeroCoeffData.m_zeroCoeff;
+ int32_t* poleCoeff = SubbandDataPt->m_PoleCoeffData.m_poleCoeff;
+ int32_t zData0;
+ int32_t* cbuf_pt;
+ int32_t invQincr_pos;
+ int32_t invQincr_neg;
+ int32_t k;
+ int32_t oldZData;
+ const int32_t roundCte = 0x80000000;
+ /* Pole coefficient and data indicies */
+ enum { a1 = 0, a2 = 1 };
+
+ /* Write the newest pole input sample to the pole delay line.
+ * Ensure the sum of the current dequantised error and the previous
+ * predictor output is saturated if necessary. */
+ poleDelayLine = invQ + SubbandDataPt->m_predData.m_predVal;
+
+ poleDelayLine = ssat24(poleDelayLine);
+
+ /* Pole filter convolution. Shift convolution result 1 place to the left
+ * before retrieving it, since the pole coefficients are Q22 (data is Q23)
+ * and we want a Q23 result */
+ accL = ((int64_t)poleCoeff[a2] * (int64_t)SubbandDataPt->m_predData.m_poleDelayLine[a2]);
+ /* Update the pole delay line for the next pass by writing the new input
+ * sample into the 2nd element */
+ SubbandDataPt->m_predData.m_poleDelayLine[a2] = poleDelayLine;
+ accL += ((int64_t)poleCoeff[a1] * (int64_t)poleDelayLine);
+ poleVal = (int32_t)(accL >> 22);
+ poleVal = ssat24(poleVal);
+
+ /* Create (2^(-7)) * sgn(invQ) in Q22 format. */
+ invQincr_pos = 0L;
+ if (invQ != 0)
+ {
+ invQincr_pos = 0x800000;
+ }
+ if (invQ < 0L)
+ {
+ invQincr_pos = -invQincr_pos;
+ }
+
+ invQincr_neg = 0x0080 - invQincr_pos;
+ invQincr_pos += 0x0080;
+
+ pointer = (SubbandDataPt->m_predData.m_zeroDelayLine.pointer++) + 6;
+ cbuf_pt = &SubbandDataPt->m_predData.m_zeroDelayLine.buffer[pointer];
+ /* partial manual unrolling to improve performance */
+ if (SubbandDataPt->m_predData.m_zeroDelayLine.pointer >= 6)
+ SubbandDataPt->m_predData.m_zeroDelayLine.pointer = 0;
+
+ SubbandDataPt->m_predData.m_zeroDelayLine.modulo = invQ;
+
+ /* Iterate over the number of coefficients for this subband */
+ oldZData = invQ;
+ accL = 0;
+
+ for (k = 0; k < 6; k++)
+ {
+ uint32_t tmp_round0;
+ int32_t coeffValue;
+
+ zData0 = (*(cbuf_pt--));
+ coeffValue = *(zeroCoeffPt + k);
+ if (zData0 < 0L)
+ {
+ acc = invQincr_neg - coeffValue;
+ }
+ else
+ {
+ acc = invQincr_pos - coeffValue;
+ }
+ tmp_round0 = acc;
+ acc = (acc >> 8) + coeffValue;
+ if (((tmp_round0 << 23) ^ roundCte) == 0) acc--;
+ accL += (int64_t)acc * (int64_t)(oldZData);
+ oldZData = zData0;
+ *(zeroCoeffPt + k) = acc;
+ }
+
+ acc = (int32_t)(accL >> 22);
+ acc = ssat24(acc);
+ /* Predictor output is the sum of the pole and zero filter outputs. Ensure
+ * this is saturated, if necessary. */
+ predVal = acc + poleVal;
+ predVal = ssat24(predVal);
+ SubbandDataPt->m_predData.m_zeroVal = acc;
+ SubbandDataPt->m_predData.m_predVal = predVal;
+
+ /* Update the zero filter delay line by writing the new input sample to the
+ * circular buffer. */
+ SubbandDataPt->m_predData.m_zeroDelayLine.buffer[SubbandDataPt->m_predData.m_zeroDelayLine.pointer] = SubbandDataPt->m_predData.m_zeroDelayLine.modulo;
+ SubbandDataPt->m_predData.m_zeroDelayLine.buffer[SubbandDataPt->m_predData.m_zeroDelayLine.pointer + 6] = SubbandDataPt->m_predData.m_zeroDelayLine.modulo;
+}
+
+
+#endif //SUBBANDFUNCTIONSCOMMON_H
diff --git a/system/embdrv/encoder_for_aptx/src/SyncInserter.h b/system/embdrv/encoder_for_aptx/src/SyncInserter.h
new file mode 100644
index 00000000000..347fd50b49e
--- /dev/null
+++ b/system/embdrv/encoder_for_aptx/src/SyncInserter.h
@@ -0,0 +1,217 @@
+/*------------------------------------------------------------------------------
+ *
+ * All declarations relevant for the SyncInserter class. This class exposes a
+ * public interface that lets a client supply two aptX encoder objects (left
+ * and right stereo channel) and have the current quantised codes adjusted to
+ * bury an autosync bit.
+ *
+ *----------------------------------------------------------------------------*/
+
+#ifndef SYNCINSERTER_H
+#define SYNCINSERTER_H
+#ifdef _GCC
+ #pragma GCC visibility push(hidden)
+#endif
+
+
+#include "AptxParameters.h"
+
+
+
+/* Function to insert sync information into one of the 8 quantised codes
+ * spread across 2 aptX codewords (1 codeword per channel) */
+XBT_INLINE_ void xbtEncinsertSync(Encoder_data* leftChannelEncoder, Encoder_data* rightChannelEncoder, uint32_t* syncWordPhase)
+{
+ /* Currently using 0x1 as the 8-bit sync pattern */
+ static const uint32_t syncWord = 0x1;
+ uint32_t tmp_var;
+
+ uint32_t i;
+
+ /* Variable to hold the XOR of all the quantised code lsbs */
+ uint32_t xorCodeLsbs;
+
+ /* Variable to point to the quantiser with the minimum calculated distance
+ * penalty. */
+ Quantiser_data* minPenaltyQuantiser;
+
+ /* Get the vector of quantiser pointers from the left and right encoders */
+ Quantiser_data* leftQuant[4];
+ Quantiser_data* rightQuant[4];
+ leftQuant[0] = &leftChannelEncoder->m_qdata[0];
+ leftQuant[1] = &leftChannelEncoder->m_qdata[1];
+ leftQuant[2] = &leftChannelEncoder->m_qdata[2];
+ leftQuant[3] = &leftChannelEncoder->m_qdata[3];
+ rightQuant[0] = &rightChannelEncoder->m_qdata[0];
+ rightQuant[1] = &rightChannelEncoder->m_qdata[1];
+ rightQuant[2] = &rightChannelEncoder->m_qdata[2];
+ rightQuant[3] = &rightChannelEncoder->m_qdata[3];
+
+ /* Starting quantiser traversal with the LL quantiser from the left channel.
+ * Initialise the pointer to the minimum penalty quantiser with the details
+ * of the left LL quantiser. Initialise the code lsbs XOR variable with the
+ * left LL quantised code lsbs and also XOR in the left and right random
+ * dither bit generated by the 2 encoders. */
+ xorCodeLsbs =
+ ((rightQuant[LL]->qCode) & 0x1) ^
+ leftChannelEncoder->m_dithSyncRandBit ^
+ rightChannelEncoder->m_dithSyncRandBit;
+ minPenaltyQuantiser = rightQuant[LH];
+
+ /* Traverse across the LH, HL and HH quantisers from the right channel */
+ for (i = LH; i <= HH; i++)
+ {
+ /* XOR in the lsb of the quantised code currently examined */
+ xorCodeLsbs ^= (rightQuant[i]->qCode) & 0x1;
+ }
+
+ /* If the distance penalty associated with a quantiser is less than the
+ * current minimum, then make that quantiser the minimum penalty
+ * quantiser. */
+ if (rightQuant[HL]->distPenalty < minPenaltyQuantiser->distPenalty)
+ minPenaltyQuantiser = rightQuant[HL];
+ if (rightQuant[LL]->distPenalty < minPenaltyQuantiser->distPenalty)
+ minPenaltyQuantiser = rightQuant[LL];
+ if (rightQuant[HH]->distPenalty < minPenaltyQuantiser->distPenalty)
+ minPenaltyQuantiser = rightQuant[HH];
+
+ /* Traverse across all quantisers from the left channel */
+ for (i = LL; i <= HH; i++)
+ {
+ /* XOR in the lsb of the quantised code currently examined */
+ xorCodeLsbs ^= (leftQuant[i]->qCode) & 0x1;
+ }
+
+ /* If the distance penalty associated with a quantiser is less than the
+ * current minimum, then make that quantiser the minimum penalty
+ * quantiser. */
+ if (leftQuant[LH]->distPenalty < minPenaltyQuantiser->distPenalty)
+ minPenaltyQuantiser = leftQuant[LH];
+ if (leftQuant[HL]->distPenalty < minPenaltyQuantiser->distPenalty)
+ minPenaltyQuantiser = leftQuant[HL];
+ if (leftQuant[LL]->distPenalty < minPenaltyQuantiser->distPenalty)
+ minPenaltyQuantiser = leftQuant[LL];
+ if (leftQuant[HH]->distPenalty < minPenaltyQuantiser->distPenalty)
+ minPenaltyQuantiser = leftQuant[HH];
+
+ /* If the lsbs of all 8 quantised codes don't happen to equal the desired
+ * sync bit to embed, then force them to be by replacing the optimum code
+ * with the alternate code in the minimum penalty quantiser (changes the lsb
+ * of the code in this quantiser) */
+ if (xorCodeLsbs != ((syncWord >> (*syncWordPhase)) & 0x1))
+ {
+ minPenaltyQuantiser->qCode = minPenaltyQuantiser->altQcode;
+ }
+
+ /* Decrement the selected sync word bit modulo 8 for the next pass. */
+ tmp_var = --(*syncWordPhase);
+ (*syncWordPhase) = tmp_var & 0x7;
+}
+
+XBT_INLINE_ void xbtEncinsertSyncDualMono(Encoder_data* leftChannelEncoder, Encoder_data* rightChannelEncoder, uint32_t* syncWordPhase)
+{
+ /* Currently using 0x1 as the 8-bit sync pattern */
+ static const uint32_t syncWord = 0x1;
+ uint32_t tmp_var;
+
+ uint32_t i;
+
+ /* Variable to hold the XOR of all the quantised code lsbs */
+ uint32_t xorCodeLsbs;
+
+ /* Variable to point to the quantiser with the minimum calculated distance
+ * penalty. */
+ Quantiser_data* minPenaltyQuantiser;
+
+ /* Get the vector of quantiser pointers from the left and right encoders */
+ Quantiser_data* leftQuant[4];
+ Quantiser_data* rightQuant[4];
+ leftQuant[0] = &leftChannelEncoder->m_qdata[0];
+ leftQuant[1] = &leftChannelEncoder->m_qdata[1];
+ leftQuant[2] = &leftChannelEncoder->m_qdata[2];
+ leftQuant[3] = &leftChannelEncoder->m_qdata[3];
+ rightQuant[0] = &rightChannelEncoder->m_qdata[0];
+ rightQuant[1] = &rightChannelEncoder->m_qdata[1];
+ rightQuant[2] = &rightChannelEncoder->m_qdata[2];
+ rightQuant[3] = &rightChannelEncoder->m_qdata[3];
+
+ /* Starting quantiser traversal with the LL quantiser from the left channel.
+ * Initialise the pointer to the minimum penalty quantiser with the details
+ * of the left LL quantiser. Initialise the code lsbs XOR variable with the
+ * left LL quantised code lsbs */
+ xorCodeLsbs = leftChannelEncoder->m_dithSyncRandBit;
+
+ minPenaltyQuantiser = leftQuant[LH];
+
+ /* Traverse across all the quantisers from the left channel */
+ for (i = LL; i <= HH; i++)
+ {
+ /* XOR in the lsb of the quantised code currently examined */
+ xorCodeLsbs ^= (leftQuant[i]->qCode) & 0x1;
+ }
+
+ /* If the distance penalty associated with a quantiser is less than the
+ * current minimum, then make that quantiser the minimum penalty
+ * quantiser. */
+ if (leftQuant[LH]->distPenalty < minPenaltyQuantiser->distPenalty)
+ minPenaltyQuantiser = leftQuant[LH];
+ if (leftQuant[HL]->distPenalty < minPenaltyQuantiser->distPenalty)
+ minPenaltyQuantiser = leftQuant[HL];
+ if (leftQuant[LL]->distPenalty < minPenaltyQuantiser->distPenalty)
+ minPenaltyQuantiser = leftQuant[LL];
+ if (leftQuant[HH]->distPenalty < minPenaltyQuantiser->distPenalty)
+ minPenaltyQuantiser = leftQuant[HH];
+
+ /* If the lsbs of all 4 quantised codes don't happen to equal the desired
+ * sync bit to embed, then force them to be by replacing the optimum code
+ * with the alternate code in the minimum penalty quantiser (changes the lsb
+ * of the code in this quantiser) */
+ if (xorCodeLsbs != ((syncWord >> (*syncWordPhase)) & 0x1))
+ {
+ minPenaltyQuantiser->qCode = minPenaltyQuantiser->altQcode;
+ }
+
+ /**** Insert sync on the Right channel ****/
+ xorCodeLsbs = rightChannelEncoder->m_dithSyncRandBit;
+
+ minPenaltyQuantiser = rightQuant[LH];
+
+ /* Traverse across all quantisers from the right channel */
+ for (i = LL; i <= HH; i++)
+ {
+ /* XOR in the lsb of the quantised code currently examined */
+ xorCodeLsbs ^= (rightQuant[i]->qCode) & 0x1;
+ }
+
+ /* If the distance penalty associated with a quantiser is less than the
+ * current minimum, then make that quantiser the minimum penalty
+ * quantiser. */
+ if (rightQuant[LH]->distPenalty < minPenaltyQuantiser->distPenalty)
+ minPenaltyQuantiser = rightQuant[LH];
+ if (rightQuant[HL]->distPenalty < minPenaltyQuantiser->distPenalty)
+ minPenaltyQuantiser = rightQuant[HL];
+ if (rightQuant[LL]->distPenalty < minPenaltyQuantiser->distPenalty)
+ minPenaltyQuantiser = rightQuant[LL];
+ if (rightQuant[HH]->distPenalty < minPenaltyQuantiser->distPenalty)
+ minPenaltyQuantiser = rightQuant[HH];
+
+ /* If the lsbs of all 4 quantised codes don't happen to equal the desired
+ * sync bit to embed, then force them to be by replacing the optimum code
+ * with the alternate code in the minimum penalty quantiser (changes the lsb
+ * of the code in this quantiser) */
+ if (xorCodeLsbs != ((syncWord >> (*syncWordPhase)) & 0x1))
+ {
+ minPenaltyQuantiser->qCode = minPenaltyQuantiser->altQcode;
+ }
+
+ /* End of Right channel autosync insert*/
+ /* Decrement the selected sync word bit modulo 8 for the next pass. */
+ tmp_var = --(*syncWordPhase);
+ (*syncWordPhase) = tmp_var & 0x7;
+}
+
+
+#ifdef _GCC
+ #pragma GCC visibility pop
+#endif
+#endif //SYNCINSERTER_H
diff --git a/system/embdrv/encoder_for_aptx/src/aptXbtenc.c b/system/embdrv/encoder_for_aptx/src/aptXbtenc.c
new file mode 100644
index 00000000000..4fde99262fb
--- /dev/null
+++ b/system/embdrv/encoder_for_aptx/src/aptXbtenc.c
@@ -0,0 +1,218 @@
+#include "AptxParameters.h"
+#include "AptxTables.h"
+#include "aptXbtenc.h"
+#include "AptxEncoder.h"
+#include "SyncInserter.h"
+#include "CodewordPacker.h"
+#include "swversion.h"
+
+
+typedef struct aptxbtenc_t
+{
+ /* m_endian should either be 0 (little endian) or 8 (big endian). */
+ int32_t m_endian;
+
+ /* m_sync_mode is an enumerated type and will be
+ 0 (stereo sync),
+ 1 (for dual mono sync), or
+ 2 (for dual channel with no autosync).
+ */
+ int32_t m_sync_mode;
+
+ /* Autosync inserter & Checker for use with the stereo aptX codec. */
+ /* The current phase of the sync word insertion (7 down to 0) */
+ uint32_t m_syncWordPhase;
+
+ /* Stereo channel aptX encoder (annotated to produce Kalimba test vectors
+ * for it's I/O. This will process valid PCM from a WAV file). */
+ /* Each Encoder_data structure requires 1592 bytes */
+ Encoder_data m_encoderData[2];
+ Qmf_storage m_qmf_l;
+ Qmf_storage m_qmf_r;
+}aptxbtenc;
+
+/* Log to linear lookup table used in inverse quantiser*/
+/* Size of Table: 32*4 = 128 bytes */
+static const int32_t IQuant_tableLogT[32] =
+{
+ 16384 * 256, 16744 * 256, 17112 * 256, 17488 * 256,
+ 17864 * 256, 18256 * 256, 18656 * 256, 19064 * 256,
+ 19480 * 256, 19912 * 256, 20344 * 256, 20792 * 256,
+ 21248 * 256, 21712 * 256, 22192 * 256, 22672 * 256,
+ 23168 * 256, 23680 * 256, 24200 * 256, 24728 * 256,
+ 25264 * 256, 25824 * 256, 26384 * 256, 26968 * 256,
+ 27552 * 256, 28160 * 256, 28776 * 256, 29408 * 256,
+ 30048 * 256, 30704 * 256, 31376 * 256, 32064 * 256
+};
+
+void clearmem(void* mem, int32_t sz)
+{
+ int8_t* m = (int8_t*)mem;
+ int32_t i = 0;
+ for (; i < sz; i++)
+ {
+ *m = 0;
+ m++;
+ }
+}
+
+APTXBTENCEXPORT int SizeofAptxbtenc(void)
+{
+ return (sizeof(aptxbtenc));
+}
+
+APTXBTENCEXPORT const char* aptxbtenc_version()
+{
+ return (swversion);
+}
+
+APTXBTENCEXPORT int aptxbtenc_init(void* _state, short endian)
+{
+ aptxbtenc* state = (aptxbtenc*)_state;
+ int32_t j = 0;
+ int32_t k;
+ int32_t t;
+
+ clearmem(_state, sizeof(aptxbtenc));
+
+ if (state == 0)
+ {
+ return 1;
+ }
+ state->m_syncWordPhase = 7L;
+
+ if (endian == 0)
+ {
+ state->m_endian = 0;
+ }
+ else
+ {
+ state->m_endian = 8;
+ }
+
+ /* default setting should be stereo autosync,
+ for backwards-compatability with legacy applications that use this library */
+ state->m_sync_mode = stereo;
+
+ for (j = 0; j < 2; j++)
+ {
+ Encoder_data* encode_dat = &state->m_encoderData[j];
+ uint32_t i;
+
+ /* Create a quantiser and subband processor for each subband */
+ for (i = LL; i <= HH; i++)
+ {
+ encode_dat->m_codewordHistory = 0L;
+
+ encode_dat->m_qdata[i].thresholdTablePtr = subbandParameters[i].threshTable;
+ encode_dat->m_qdata[i].thresholdTablePtr_sl1 = subbandParameters[i].threshTable_sl1;
+ encode_dat->m_qdata[i].ditherTablePtr = subbandParameters[i].dithTable;
+ encode_dat->m_qdata[i].minusLambdaDTable = subbandParameters[i].minusLambdaDTable;
+ encode_dat->m_qdata[i].codeBits = subbandParameters[i].numBits;
+ encode_dat->m_qdata[i].qCode = 0L;
+ encode_dat->m_qdata[i].altQcode = 0L;
+ encode_dat->m_qdata[i].distPenalty = 0L;
+
+ /* initialisation of inverseQuantiser data */
+ encode_dat->m_SubbandData[i].m_iqdata.thresholdTablePtr = subbandParameters[i].threshTable;
+ encode_dat->m_SubbandData[i].m_iqdata.thresholdTablePtr_sl1 = subbandParameters[i].threshTable_sl1;
+ encode_dat->m_SubbandData[i].m_iqdata.ditherTablePtr_sf1 = subbandParameters[i].dithTable_sh1;
+ encode_dat->m_SubbandData[i].m_iqdata.incrTablePtr = subbandParameters[i].incrTable;
+ encode_dat->m_SubbandData[i].m_iqdata.maxLogDelta = subbandParameters[i].maxLogDelta;
+ encode_dat->m_SubbandData[i].m_iqdata.minLogDelta = subbandParameters[i].minLogDelta;
+ encode_dat->m_SubbandData[i].m_iqdata.delta = 0;
+ encode_dat->m_SubbandData[i].m_iqdata.logDelta = 0;
+ encode_dat->m_SubbandData[i].m_iqdata.invQ = 0;
+ encode_dat->m_SubbandData[i].m_iqdata.iquantTableLogPtr = &IQuant_tableLogT[0];
+
+ // Initializing data for predictor filter
+ encode_dat->m_SubbandData[i].m_predData.m_zeroDelayLine.modulo = subbandParameters[i].numZeros;
+
+ for (t = 0; t < 48; t++)
+ {
+ encode_dat->m_SubbandData[i].m_predData.m_zeroDelayLine.buffer[t] = 0;
+ }
+
+ encode_dat->m_SubbandData[i].m_predData.m_zeroDelayLine.pointer = 0;
+ /* Initialise the previous zero filter output and predictor output to zero */
+ encode_dat->m_SubbandData[i].m_predData.m_zeroVal = 0L;
+ encode_dat->m_SubbandData[i].m_predData.m_predVal = 0L;
+ encode_dat->m_SubbandData[i].m_predData.m_numZeros = subbandParameters[i].numZeros;
+ /* Initialise the contents of the pole data delay line to zero */
+ encode_dat->m_SubbandData[i].m_predData.m_poleDelayLine[0] = 0L;
+ encode_dat->m_SubbandData[i].m_predData.m_poleDelayLine[1] = 0L;
+
+ for (k = 0; k < 24; k++)
+ {
+ encode_dat->m_SubbandData[i].m_ZeroCoeffData.m_zeroCoeff[k] = 0;
+ }
+ // Initializing data for zerocoeff update function.
+ encode_dat->m_SubbandData[i].m_ZeroCoeffData.m_numZeros = subbandParameters[i].numZeros;
+
+ /* Initializing data for PoleCoeff update function.
+ * Fill the adaptation delay line with +1 initially */
+ encode_dat->m_SubbandData[i].m_PoleCoeffData.m_poleAdaptDelayLine.s32 = 0x00010001;
+
+ /* Zero the pole coefficients */
+ encode_dat->m_SubbandData[i].m_PoleCoeffData.m_poleCoeff[0] = 0L;
+ encode_dat->m_SubbandData[i].m_PoleCoeffData.m_poleCoeff[1] = 0L;
+ }
+ }
+ return 0;
+}
+
+APTXBTENCEXPORT int aptxbtenc_setsync_mode(void* _state, int32_t sync_mode)
+{
+ aptxbtenc* state = (aptxbtenc*)_state;
+ state->m_sync_mode = sync_mode;
+
+ return 0;
+}
+
+APTXBTENCEXPORT int aptxbtenc_encodestereo(void* _state, void* _pcmL, void* _pcmR, void* _buffer)
+{
+ aptxbtenc* state = (aptxbtenc*)_state;
+ int32_t* pcmL = (int32_t*)_pcmL;
+ int32_t* pcmR = (int32_t*)_pcmR;
+ int16_t* buffer = (int16_t*)_buffer;
+ int32_t tmp_reg;
+ int16_t tmp_out;
+ //Feed the PCM to the dual aptX encoders
+ aptxEncode(pcmL, &state->m_qmf_l, &state->m_encoderData[0]);
+ aptxEncode(pcmR, &state->m_qmf_r, &state->m_encoderData[1]);
+
+ // only insert sync information if we are not in non-autosync mode.
+ // The Non-autosync mode changes only take effect in the packCodeword() function.
+ if (state->m_sync_mode != no_sync)
+ {
+ if (state->m_sync_mode == stereo)
+ {
+ //Insert the autosync information into the stereo quantised codes
+ xbtEncinsertSync(&state->m_encoderData[0], &state->m_encoderData[1], &state->m_syncWordPhase);
+ }
+ else
+ {
+ //Insert the autosync information into the two individual mono quantised codes
+ xbtEncinsertSyncDualMono(&state->m_encoderData[0], &state->m_encoderData[1], &state->m_syncWordPhase);
+ }
+ }
+
+ aptxPostEncode(&state->m_encoderData[0]);
+ aptxPostEncode(&state->m_encoderData[1]);
+
+ //Pack the (possibly adjusted) codes into a 16-bit codeword per channel
+ tmp_reg = packCodeword(&state->m_encoderData[0], state->m_sync_mode);
+ // Swap bytes to output data in big-endian as expected by bc5 code...
+ tmp_out = tmp_reg >> state->m_endian;
+ tmp_out |= tmp_reg << state->m_endian;
+
+ buffer[0] = (int16_t)tmp_out;
+ tmp_reg = packCodeword(&state->m_encoderData[1], state->m_sync_mode);
+ // Swap bytes to output data in big-endian as expected by bc5 code...
+ tmp_out = tmp_reg >> state->m_endian;
+ tmp_out |= tmp_reg << state->m_endian;
+
+ buffer[1] = (int16_t)tmp_out;
+
+ return 0;
+}
diff --git a/system/embdrv/encoder_for_aptx/src/swversion.h b/system/embdrv/encoder_for_aptx/src/swversion.h
new file mode 100644
index 00000000000..792b576a8f5
--- /dev/null
+++ b/system/embdrv/encoder_for_aptx/src/swversion.h
@@ -0,0 +1,8 @@
+#ifndef SWVERSION_H
+#define SWVERSION_H
+
+
+const char *swversion = "1.0.0";
+
+
+#endif //SWVERSION_H
diff --git a/system/embdrv/encoder_for_aptxhd/include/aptXHDbtenc.h b/system/embdrv/encoder_for_aptxhd/include/aptXHDbtenc.h
new file mode 100644
index 00000000000..bd2bc801c3e
--- /dev/null
+++ b/system/embdrv/encoder_for_aptxhd/include/aptXHDbtenc.h
@@ -0,0 +1,49 @@
+/*-----------------------------------------------------------------------------
+ *
+ * This file exposes a public interface to allow clients to invoke aptX HD
+ * encoding on 4 new PCM samples, generating 2 new codeword (one for the
+ * left channel and one for the right channel).
+ *
+ *----------------------------------------------------------------------------*/
+
+#ifndef APTXHDBTENC_H
+#define APTXHDBTENC_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#ifdef _DLLEXPORT
+#define APTXHDBTENCEXPORT __declspec(dllexport)
+#else
+#define APTXHDBTENCEXPORT
+#endif
+
+
+/* SizeofAptxhdbtenc returns the size (in byte) of the memory
+ * allocation required to store the state of the encoder */
+APTXHDBTENCEXPORT int SizeofAptxhdbtenc (void);
+
+/* aptxhdbtenc_version can be used to extract the version number
+ * of the aptX HD encoder */
+APTXHDBTENCEXPORT const char* aptxhdbtenc_version(void);
+
+/* aptxhdbtenc_init is used to initialise the encoder structure.
+ * _state should be a pointer to the encoder structure (stereo).
+ * endian represent the endianness of the output data
+ * (0=little endian. Big endian otherwise)
+ * The function returns 1 if an error occurred during the initialisation.
+ * The function returns 0 if no error occurred during the initialisation. */
+APTXHDBTENCEXPORT int aptxhdbtenc_init(void* _state, short endian);
+
+/* StereoEncode will take 8 audio samples (24-bit per sample)
+ * and generate one 24-bit codeword with autosync inserted.
+ * The bitstream is compatible with be BC05 implementation. */
+APTXHDBTENCEXPORT int aptxhdbtenc_encodestereo(void* _state, void* _pcmL, void* _pcmR, void* _buffer);
+
+
+#ifdef __cplusplus
+} // /extern "C"
+#endif
+
+#endif //APTXHDBTENC_H
diff --git a/system/embdrv/encoder_for_aptxhd/src/AptxEncoder.h b/system/embdrv/encoder_for_aptxhd/src/AptxEncoder.h
new file mode 100644
index 00000000000..bfd5fb4f86a
--- /dev/null
+++ b/system/embdrv/encoder_for_aptxhd/src/AptxEncoder.h
@@ -0,0 +1,87 @@
+/*------------------------------------------------------------------------------
+ *
+ * All declarations relevant for aptxhdEncode. This function allows clients
+ * to invoke aptX HD encoding on 4 new PCM samples,
+ * generating 4 new quantised codes. A separate function allows the
+ * packing of the 4 codes into a 24-bit word.
+ *
+ *----------------------------------------------------------------------------*/
+
+#ifndef APTXENCODER_H
+#define APTXENCODER_H
+#ifdef _GCC
+ #pragma GCC visibility push(hidden)
+#endif
+
+
+#include "AptxParameters.h"
+#include "DitherGenerator.h"
+#include "Quantiser.h"
+#include "SubbandFunctionsCommon.h"
+#include "Qmf.h"
+
+
+/* Function to carry out a single-channel aptX HD encode on 4 new PCM samples */
+XBT_INLINE_ void aptxhdEncode(int32_t pcm[4], Qmf_storage* Qmf_St, Encoder_data* EncoderDataPt)
+{
+ int32_t predVals[4];
+ int32_t qCodes[4];
+ int32_t aqmfOutputs[4];
+
+ /* Extract the previous predicted values and quantised codes into arrays */
+ predVals[0] = EncoderDataPt->m_SubbandData[0].m_predData.m_predVal;
+ qCodes[0] = EncoderDataPt->m_qdata[0].qCode;
+
+ predVals[1] = EncoderDataPt->m_SubbandData[1].m_predData.m_predVal;
+ qCodes[1] = EncoderDataPt->m_qdata[1].qCode;
+
+ predVals[2] = EncoderDataPt->m_SubbandData[2].m_predData.m_predVal;
+ qCodes[2] = EncoderDataPt->m_qdata[2].qCode;
+
+ predVals[3] = EncoderDataPt->m_SubbandData[3].m_predData.m_predVal;
+ qCodes[3] = EncoderDataPt->m_qdata[3].qCode;
+
+ /* Update codeword history, then generate new dither values. */
+ EncoderDataPt->m_codewordHistory = xbtEncupdateCodewordHistory(qCodes, EncoderDataPt->m_codewordHistory);
+ EncoderDataPt->m_dithSyncRandBit = xbtEncgenerateDither(EncoderDataPt->m_codewordHistory, EncoderDataPt->m_ditherOutputs);
+
+ /* Run the analysis QMF */
+ QmfAnalysisFilter(pcm, Qmf_St, predVals, aqmfOutputs);
+
+ /* Run the quantiser for each subband */
+ quantiseDifference_HDLL(aqmfOutputs[0], EncoderDataPt->m_ditherOutputs[0], EncoderDataPt->m_SubbandData[0].m_iqdata.delta, &EncoderDataPt->m_qdata[0]);
+ quantiseDifference_HDLH(aqmfOutputs[1], EncoderDataPt->m_ditherOutputs[1], EncoderDataPt->m_SubbandData[1].m_iqdata.delta, &EncoderDataPt->m_qdata[1]);
+ quantiseDifference_HDHL(aqmfOutputs[2], EncoderDataPt->m_ditherOutputs[2], EncoderDataPt->m_SubbandData[2].m_iqdata.delta, &EncoderDataPt->m_qdata[2]);
+ quantiseDifference_HDHH(aqmfOutputs[3], EncoderDataPt->m_ditherOutputs[3], EncoderDataPt->m_SubbandData[3].m_iqdata.delta, &EncoderDataPt->m_qdata[3]);
+}
+
+XBT_INLINE_ void aptxhdPostEncode(Encoder_data* EncoderDataPt)
+{
+ /* Run the remaining subband processing for each subband */
+ /* Manual inlining on the 4 subband */
+ processSubband_HDLL(EncoderDataPt->m_qdata[0].qCode,
+ EncoderDataPt->m_ditherOutputs[0],
+ &EncoderDataPt->m_SubbandData[0],
+ &EncoderDataPt->m_SubbandData[0].m_iqdata);
+
+ processSubband_HD(EncoderDataPt->m_qdata[1].qCode,
+ EncoderDataPt->m_ditherOutputs[1],
+ &EncoderDataPt->m_SubbandData[1],
+ &EncoderDataPt->m_SubbandData[1].m_iqdata);
+
+ processSubband_HDHL(EncoderDataPt->m_qdata[2].qCode,
+ EncoderDataPt->m_ditherOutputs[2],
+ &EncoderDataPt->m_SubbandData[2],
+ &EncoderDataPt->m_SubbandData[2].m_iqdata);
+
+ processSubband_HD(EncoderDataPt->m_qdata[3].qCode,
+ EncoderDataPt->m_ditherOutputs[3],
+ &EncoderDataPt->m_SubbandData[3],
+ &EncoderDataPt->m_SubbandData[3].m_iqdata);
+}
+
+
+#ifdef _GCC
+ #pragma GCC visibility pop
+#endif
+#endif //APTXENCODER_H
diff --git a/system/embdrv/encoder_for_aptxhd/src/AptxParameters.h b/system/embdrv/encoder_for_aptxhd/src/AptxParameters.h
new file mode 100644
index 00000000000..b3e9dd3225d
--- /dev/null
+++ b/system/embdrv/encoder_for_aptxhd/src/AptxParameters.h
@@ -0,0 +1,248 @@
+/*------------------------------------------------------------------------------
+ *
+ * General shared aptX HD parameters.
+ *
+ *-----------------------------------------------------------------------------*/
+
+#ifndef APTXPARAMETERS_H
+#define APTXPARAMETERS_H
+#ifdef _GCC
+ #pragma GCC visibility push(hidden)
+#endif
+
+#include <stdint.h>
+#include "CBStruct.h"
+
+#if defined _MSC_VER
+ #define XBT_INLINE_ inline
+ #define _STDQMFOUTERCOEFF 1
+#elif defined __clang__
+ #define XBT_INLINE_ static inline
+ #define _STDQMFOUTERCOEFF 1
+#elif defined __GNUC__
+ #define XBT_INLINE_ inline
+ #define _STDQMFOUTERCOEFF 1
+#else
+ #define XBT_INLINE_ static
+ #define _STDQMFOUTERCOEFF 1
+#endif
+
+
+/* Signed saturate to a 24bit value */
+XBT_INLINE_ int32_t ssat24(int32_t val)
+{
+ if (val > 0x7FFFFF)
+ val = 0x7FFFFF;
+ if (val < -0x800000)
+ val = -0x800000;
+ return val;
+}
+
+typedef union u_reg64
+{
+ uint64_t u64;
+ int64_t s64;
+ struct s_u32
+ {
+#ifdef __BIGENDIAN
+ uint32_t h;
+ uint32_t l;
+#else
+ uint32_t l;
+ uint32_t h;
+#endif
+ } u32;
+ struct s_s32
+ {
+#ifdef __BIGENDIAN
+ int32_t h;
+ int32_t l;
+#else
+ int32_t l;
+ int32_t h;
+#endif
+ } s32;
+} reg64_t;
+
+typedef union u_reg32
+{
+ uint32_t u32;
+ int32_t s32;
+ struct s_u16
+ {
+#ifdef __BIGENDIAN
+ uint16_t h;
+ uint16_t l;
+#else
+ uint16_t l;
+ uint16_t h;
+#endif
+ } u16;
+ struct s_s16
+ {
+#ifdef __BIGENDIAN
+ int16_t h;
+ int16_t l;
+#else
+ int16_t l;
+ int16_t h;
+#endif
+ } s16;
+} reg32_t;
+
+/* Each aptX HD enc/dec round consumes/produces 4 PCM samples */
+static const uint32_t numPcmSamples = 4;
+
+/* Symbolic constants for PCM data indicies. */
+enum
+{
+ FirstPcm = 0,
+ SecondPcm = 1,
+ ThirdPcm = 2,
+ FourthPcm = 3
+};
+
+/* Number of subbands is fixed at 4 */
+#define NUMSUBBANDS 4
+
+/* Symbolic constants for subband identification. */
+typedef enum {LL=0, LH=1, HL=2, HH=3} bands;
+
+/* Structure declaration to bind a set of subband parameters */
+typedef struct
+{
+ const int32_t* threshTable;
+ const int32_t* threshTable_sl1;
+ const int32_t* dithTable;
+ const int32_t* dithTable_sh1;
+ const int32_t* minusLambdaDTable;
+ const int32_t* incrTable;
+ int32_t numBits;
+ int32_t maxLogDelta;
+ int32_t minLogDelta;
+ int32_t numZeros;
+} SubbandParameters;
+
+/* Struct required for the polecoeffcalculator function of btaptXHD encoder and decoder*/
+/* Size of structure: 16 Bytes */
+typedef struct
+{
+ /* delay line for previous sgn values */
+ reg32_t m_poleAdaptDelayLine;
+ /* 2 pole filter coeffs */
+ int32_t m_poleCoeff[2];
+} PoleCoeff_data;
+
+/* Struct required for the zerocoeffcalculator function of btaptXHD encoder and decoder*/
+/* Size of structure: 100 Bytes */
+typedef struct
+{
+ /* The zero filter length for this subband */
+ int32_t m_numZeros;
+ /* Maximum number of zeros for any subband is 24. */
+ /* 24 zero filter coeffs */
+ int32_t m_zeroCoeff[24];
+} ZeroCoeff_data;
+
+/* Struct required for the prediction filtering function of btaptXHD encoder and decoder*/
+/* Size of structure: 200+20=220 Bytes */
+typedef struct
+{
+ /* Number of zeros associated with this subband */
+ int32_t m_numZeros;
+ /* Zero data delay line (circular) */
+ circularBuffer m_zeroDelayLine;
+ /* 2-tap pole data delay line */
+ int32_t m_poleDelayLine[2];
+ /* Output from zero filter */
+ int32_t m_zeroVal;
+ /* Output from overall ARMA filter */
+ int32_t m_predVal;
+} Predictor_data;
+
+/* Struct required for the Quantisation function of btaptXHD encoder and decoder*/
+/* Size of structure: 24 Bytes */
+typedef struct
+{
+ /* Number of bits in the quantised code for this subband */
+ int32_t codeBits;
+ /* Pointer to threshold table */
+ const int32_t* thresholdTablePtr;
+ const int32_t* thresholdTablePtr_sl1;
+
+ /* Pointer to dither table */
+ const int32_t* ditherTablePtr;
+ /* Pointer to minus Lambda table */
+ const int32_t* minusLambdaDTable;
+ /* Output quantised code */
+ int32_t qCode;
+ /* Alternative quantised code for sync purposes */
+ int32_t altQcode;
+ /* Penalty associated with choosing alternative code */
+ int32_t distPenalty;
+} Quantiser_data;
+
+/* Struct required for the inverse Quantisation function of btaptXHD encoder and decoder*/
+/* Size of structure: 32 Bytes */
+typedef struct
+{
+ /* Pointer to threshold table */
+ const int32_t* thresholdTablePtr;
+ const int32_t* thresholdTablePtr_sl1;
+ /* Pointer to dither table */
+ const int32_t* ditherTablePtr_sf1;
+
+ /* Pointer to increment table */
+ const int32_t* incrTablePtr;
+ /* Upper and lower bounds for logDelta */
+ int32_t maxLogDelta;
+ int32_t minLogDelta;
+ /* Delta (quantisation step size */
+ int32_t delta;
+ /* Delta, expressed as a log base 2 */
+ uint16_t logDelta;
+ /* Output dequantised signal */
+ int32_t invQ;
+ /* pointer to IQuant_tableLogT */
+ const int32_t* iquantTableLogPtr;
+} IQuantiser_data;
+
+/* Subband data structure btaptXHD encoder*/
+/* Size of structure: 116+220+32= 368 Bytes */
+typedef struct
+{
+ /* Subband processing consists of inverse quantisation, predictor
+ * coefficient update, and predictor filtering. */
+ ZeroCoeff_data m_ZeroCoeffData;
+ PoleCoeff_data m_PoleCoeffData;
+ /* structure holding the data associated with the predictor */
+ Predictor_data m_predData;
+ /* iqdata holds the data associated with the instance of inverse quantiser */
+ IQuantiser_data m_iqdata;
+} Subband_data;
+
+/* Encoder data structure btaptXHD encoder*/
+/* Size of structure: 368*4+24+4*24 = 1592 Bytes */
+typedef struct
+{
+ /* Subband processing consists of inverse quantisation, predictor
+ * coefficient update, and predictor filtering. */
+ Subband_data m_SubbandData[4];
+ int32_t m_codewordHistory;
+ int32_t m_dithSyncRandBit;
+ int32_t m_ditherOutputs[4];
+ /* structure holding data values for this quantiser */
+ Quantiser_data m_qdata[4];
+} Encoder_data;
+
+/* Subband-specific number of predcitor zero filter coefficients. */
+static const uint32_t numZeroFilterCoeffs[4] = {24, 12, 6, 12};
+
+/* Delta is scaled by 4 positions within the quantiser and inverse quantiser. */
+static const uint32_t deltaScale = 4;
+
+
+#ifdef _GCC
+ #pragma GCC visibility pop
+#endif
+#endif //APTXPARAMETERS_H
diff --git a/system/embdrv/encoder_for_aptxhd/src/AptxTables.h b/system/embdrv/encoder_for_aptxhd/src/AptxTables.h
new file mode 100644
index 00000000000..5db25e9b36d
--- /dev/null
+++ b/system/embdrv/encoder_for_aptxhd/src/AptxTables.h
@@ -0,0 +1,1365 @@
+/*------------------------------------------------------------------------------
+ *
+ * All table definitions used for the quantizer.
+ *
+ *----------------------------------------------------------------------------*/
+
+#ifndef APTXTABLES_H
+#define APTXTABLES_H
+
+#include "AptxParameters.h"
+
+/* Quantisation threshold, logDelta increment and dither tables for 4-bit codes */
+static const int32_t dq4bit24_sl1[9] =
+{
+ -95044,
+ 95044,
+ 295844,
+ 528780,
+ 821332,
+ 1226438,
+ 1890540,
+ 3344850,
+ 6450664,
+};
+
+static const int32_t q4incr24[9] =
+{
+ 0,
+ -17,
+ 5,
+ 30,
+ 62,
+ 105,
+ 177,
+ 334,
+ 518,
+};
+
+static const int32_t dq4dith24_sf1[9] =
+{
+ 95044,
+ 95044,
+ 105754,
+ 127180,
+ 165372,
+ 39736,
+ 424366,
+ 1029946,
+ 2075866,
+};
+
+static const int32_t dq4mLamb24[8] =
+{
+ 0,
+ -2678,
+ -5357,
+ -9548,
+ 31409,
+ -96158,
+ -151395,
+ -261480,
+};
+
+/* Quantisation threshold, logDelta increment and dither tables for 5-bit codes */
+static const int32_t dq5bit24_sl1[17] =
+{
+ -45754,
+ 45754,
+ 138496,
+ 234896,
+ 337336,
+ 448310,
+ 570738,
+ 708380,
+ 866534,
+ 1053262,
+ 1281958,
+ 1577438,
+ 1993050,
+ 2665984,
+ 3900982,
+ 5902844,
+ 8897462,
+};
+
+static const int32_t q5incr24[17] =
+{
+ 0,
+ -18,
+ -8,
+ 2,
+ 13,
+ 25,
+ 38,
+ 53,
+ 70,
+ 90,
+ 115,
+ 147,
+ 192,
+ 264,
+ 398,
+ 521,
+ 521,
+};
+
+static const int32_t dq5dith24_sf1[17] =
+{
+ 45754,
+ 45754,
+ 46988,
+ 49412,
+ 53026,
+ 57950,
+ 64478,
+ 73164,
+ 84988,
+ 101740,
+ 126958,
+ 168522,
+ 247092,
+ 425842,
+ 809154,
+ 1192708,
+ 1801910,
+};
+
+static const int32_t dq5mLamb24[16] =
+{
+ 0,
+ -309,
+ -606,
+ -904,
+ -1231,
+ -1632,
+ -2172,
+ -2956,
+ -4188,
+ -6305,
+ -10391,
+ -19643,
+ -44688,
+ -95828,
+ -95889,
+ -152301,
+};
+
+/* Quantisation threshold, logDelta increment and dither tables for 6-bit codes */
+static const int32_t dq6bit24_sl1[33] =
+{
+ -21236,
+ 21236,
+ 63830,
+ 106798,
+ 150386,
+ 194832,
+ 240376,
+ 287258,
+ 335726,
+ 386034,
+ 438460,
+ 493308,
+ 550924,
+ 611696,
+ 676082,
+ 744626,
+ 817986,
+ 896968,
+ 982580,
+ 1076118,
+ 1179278,
+ 1294344,
+ 1424504,
+ 1574386,
+ 1751090,
+ 1966260,
+ 2240868,
+ 2617662,
+ 3196432,
+ 4176450,
+ 5658260,
+ 7671068,
+ 10380372,
+};
+
+static const int32_t q6incr24[33] =
+{
+ 0,
+ -21,
+ -16,
+ -12,
+ -7,
+ -2,
+ 3,
+ 8,
+ 13,
+ 19,
+ 24,
+ 30,
+ 36,
+ 43,
+ 50,
+ 57,
+ 65,
+ 74,
+ 83,
+ 93,
+ 104,
+ 117,
+ 131,
+ 147,
+ 166,
+ 189,
+ 219,
+ 259,
+ 322,
+ 427,
+ 521,
+ 521,
+ 521,
+};
+
+static const int32_t dq6dith24_sf1[33] =
+{
+ 21236,
+ 21236,
+ 21360,
+ 21608,
+ 21978,
+ 22468,
+ 23076,
+ 23806,
+ 24660,
+ 25648,
+ 26778,
+ 28070,
+ 29544,
+ 31228,
+ 33158,
+ 35386,
+ 37974,
+ 41008,
+ 44606,
+ 48934,
+ 54226,
+ 60840,
+ 69320,
+ 80564,
+ 96140,
+ 119032,
+ 155576,
+ 221218,
+ 357552,
+ 622468,
+ 859344,
+ 1153464,
+ 1555840,
+};
+
+static const int32_t dq6mLamb24[32] =
+{
+ 0,
+ -31,
+ -62,
+ -93,
+ -123,
+ -152,
+ -183,
+ -214,
+ -247,
+ -283,
+ -323,
+ -369,
+ -421,
+ -483,
+ -557,
+ -647,
+ -759,
+ -900,
+ -1082,
+ -1323,
+ -1654,
+ -2120,
+ -2811,
+ -3894,
+ -5723,
+ -9136,
+ -16411,
+ -34084,
+ -66229,
+ -59219,
+ -73530,
+ -100594,
+};
+
+/* Quantisation threshold, logDelta increment and dither tables for 9-bit codes */
+static const int32_t dq9bit24_sl1[257] =
+{
+ -2436,
+ 2436,
+ 7308,
+ 12180,
+ 17054,
+ 21930,
+ 26806,
+ 31686,
+ 36566,
+ 41450,
+ 46338,
+ 51230,
+ 56124,
+ 61024,
+ 65928,
+ 70836,
+ 75750,
+ 80670,
+ 85598,
+ 90530,
+ 95470,
+ 100418,
+ 105372,
+ 110336,
+ 115308,
+ 120288,
+ 125278,
+ 130276,
+ 135286,
+ 140304,
+ 145334,
+ 150374,
+ 155426,
+ 160490,
+ 165566,
+ 170654,
+ 175756,
+ 180870,
+ 185998,
+ 191138,
+ 196294,
+ 201466,
+ 206650,
+ 211850,
+ 217068,
+ 222300,
+ 227548,
+ 232814,
+ 238096,
+ 243396,
+ 248714,
+ 254050,
+ 259406,
+ 264778,
+ 270172,
+ 275584,
+ 281018,
+ 286470,
+ 291944,
+ 297440,
+ 302956,
+ 308496,
+ 314056,
+ 319640,
+ 325248,
+ 330878,
+ 336532,
+ 342212,
+ 347916,
+ 353644,
+ 359398,
+ 365178,
+ 370986,
+ 376820,
+ 382680,
+ 388568,
+ 394486,
+ 400430,
+ 406404,
+ 412408,
+ 418442,
+ 424506,
+ 430600,
+ 436726,
+ 442884,
+ 449074,
+ 455298,
+ 461554,
+ 467844,
+ 474168,
+ 480528,
+ 486922,
+ 493354,
+ 499820,
+ 506324,
+ 512866,
+ 519446,
+ 526064,
+ 532722,
+ 539420,
+ 546160,
+ 552940,
+ 559760,
+ 566624,
+ 573532,
+ 580482,
+ 587478,
+ 594520,
+ 601606,
+ 608740,
+ 615920,
+ 623148,
+ 630426,
+ 637754,
+ 645132,
+ 652560,
+ 660042,
+ 667576,
+ 675164,
+ 682808,
+ 690506,
+ 698262,
+ 706074,
+ 713946,
+ 721876,
+ 729868,
+ 737920,
+ 746036,
+ 754216,
+ 762460,
+ 770770,
+ 779148,
+ 787594,
+ 796108,
+ 804694,
+ 813354,
+ 822086,
+ 830892,
+ 839774,
+ 848736,
+ 857776,
+ 866896,
+ 876100,
+ 885386,
+ 894758,
+ 904218,
+ 913766,
+ 923406,
+ 933138,
+ 942964,
+ 952886,
+ 962908,
+ 973030,
+ 983254,
+ 993582,
+ 1004020,
+ 1014566,
+ 1025224,
+ 1035996,
+ 1046886,
+ 1057894,
+ 1069026,
+ 1080284,
+ 1091670,
+ 1103186,
+ 1114838,
+ 1126628,
+ 1138558,
+ 1150634,
+ 1162858,
+ 1175236,
+ 1187768,
+ 1200462,
+ 1213320,
+ 1226346,
+ 1239548,
+ 1252928,
+ 1266490,
+ 1280242,
+ 1294188,
+ 1308334,
+ 1322688,
+ 1337252,
+ 1352034,
+ 1367044,
+ 1382284,
+ 1397766,
+ 1413494,
+ 1429478,
+ 1445728,
+ 1462252,
+ 1479058,
+ 1496158,
+ 1513562,
+ 1531280,
+ 1549326,
+ 1567710,
+ 1586446,
+ 1605550,
+ 1625034,
+ 1644914,
+ 1665208,
+ 1685932,
+ 1707108,
+ 1728754,
+ 1750890,
+ 1773542,
+ 1796732,
+ 1820488,
+ 1844840,
+ 1869816,
+ 1895452,
+ 1921780,
+ 1948842,
+ 1976680,
+ 2005338,
+ 2034868,
+ 2065322,
+ 2096766,
+ 2129260,
+ 2162880,
+ 2197708,
+ 2233832,
+ 2271352,
+ 2310384,
+ 2351050,
+ 2393498,
+ 2437886,
+ 2484404,
+ 2533262,
+ 2584710,
+ 2639036,
+ 2696578,
+ 2757738,
+ 2822998,
+ 2892940,
+ 2968278,
+ 3049896,
+ 3138912,
+ 3236760,
+ 3345312,
+ 3467068,
+ 3605434,
+ 3765154,
+ 3952904,
+ 4177962,
+ 4452178,
+ 4787134,
+ 5187290,
+ 5647128,
+ 6159120,
+ 6720518,
+ 7332904,
+ 8000032,
+ 8726664,
+ 9518152,
+ 10380372,
+};
+
+static const int32_t q9incr24[257] =
+{
+ 0,
+ -22,
+ -21,
+ -21,
+ -20,
+ -20,
+ -19,
+ -19,
+ -18,
+ -18,
+ -17,
+ -17,
+ -16,
+ -16,
+ -15,
+ -14,
+ -14,
+ -13,
+ -13,
+ -12,
+ -12,
+ -11,
+ -11,
+ -10,
+ -10,
+ -9,
+ -9,
+ -8,
+ -7,
+ -7,
+ -6,
+ -6,
+ -5,
+ -5,
+ -4,
+ -4,
+ -3,
+ -3,
+ -2,
+ -1,
+ -1,
+ 0,
+ 0,
+ 1,
+ 1,
+ 2,
+ 2,
+ 3,
+ 4,
+ 4,
+ 5,
+ 5,
+ 6,
+ 6,
+ 7,
+ 8,
+ 8,
+ 9,
+ 9,
+ 10,
+ 11,
+ 11,
+ 12,
+ 12,
+ 13,
+ 14,
+ 14,
+ 15,
+ 15,
+ 16,
+ 17,
+ 17,
+ 18,
+ 19,
+ 19,
+ 20,
+ 20,
+ 21,
+ 22,
+ 22,
+ 23,
+ 24,
+ 24,
+ 25,
+ 26,
+ 26,
+ 27,
+ 28,
+ 28,
+ 29,
+ 30,
+ 30,
+ 31,
+ 32,
+ 33,
+ 33,
+ 34,
+ 35,
+ 35,
+ 36,
+ 37,
+ 38,
+ 38,
+ 39,
+ 40,
+ 41,
+ 41,
+ 42,
+ 43,
+ 44,
+ 44,
+ 45,
+ 46,
+ 47,
+ 48,
+ 48,
+ 49,
+ 50,
+ 51,
+ 52,
+ 52,
+ 53,
+ 54,
+ 55,
+ 56,
+ 57,
+ 58,
+ 58,
+ 59,
+ 60,
+ 61,
+ 62,
+ 63,
+ 64,
+ 65,
+ 66,
+ 67,
+ 68,
+ 69,
+ 69,
+ 70,
+ 71,
+ 72,
+ 73,
+ 74,
+ 75,
+ 77,
+ 78,
+ 79,
+ 80,
+ 81,
+ 82,
+ 83,
+ 84,
+ 85,
+ 86,
+ 87,
+ 89,
+ 90,
+ 91,
+ 92,
+ 93,
+ 94,
+ 96,
+ 97,
+ 98,
+ 99,
+ 101,
+ 102,
+ 103,
+ 105,
+ 106,
+ 107,
+ 109,
+ 110,
+ 112,
+ 113,
+ 115,
+ 116,
+ 118,
+ 119,
+ 121,
+ 122,
+ 124,
+ 125,
+ 127,
+ 129,
+ 130,
+ 132,
+ 134,
+ 136,
+ 137,
+ 139,
+ 141,
+ 143,
+ 145,
+ 147,
+ 149,
+ 151,
+ 153,
+ 155,
+ 158,
+ 160,
+ 162,
+ 164,
+ 167,
+ 169,
+ 172,
+ 174,
+ 177,
+ 180,
+ 182,
+ 185,
+ 188,
+ 191,
+ 194,
+ 197,
+ 201,
+ 204,
+ 208,
+ 211,
+ 215,
+ 219,
+ 223,
+ 227,
+ 232,
+ 236,
+ 241,
+ 246,
+ 251,
+ 257,
+ 263,
+ 269,
+ 275,
+ 283,
+ 290,
+ 298,
+ 307,
+ 317,
+ 327,
+ 339,
+ 352,
+ 367,
+ 384,
+ 404,
+ 429,
+ 458,
+ 494,
+ 522,
+ 522,
+ 522,
+ 522,
+ 522,
+ 522,
+ 522,
+ 522,
+ 522,
+};
+
+static const int32_t dq9dith24_sf1[257] =
+{
+ 2436,
+ 2436,
+ 2436,
+ 2436,
+ 2438,
+ 2438,
+ 2438,
+ 2440,
+ 2442,
+ 2442,
+ 2444,
+ 2446,
+ 2448,
+ 2450,
+ 2454,
+ 2456,
+ 2458,
+ 2462,
+ 2464,
+ 2468,
+ 2472,
+ 2476,
+ 2480,
+ 2484,
+ 2488,
+ 2492,
+ 2498,
+ 2502,
+ 2506,
+ 2512,
+ 2518,
+ 2524,
+ 2528,
+ 2534,
+ 2540,
+ 2548,
+ 2554,
+ 2560,
+ 2568,
+ 2574,
+ 2582,
+ 2588,
+ 2596,
+ 2604,
+ 2612,
+ 2620,
+ 2628,
+ 2636,
+ 2646,
+ 2654,
+ 2664,
+ 2672,
+ 2682,
+ 2692,
+ 2702,
+ 2712,
+ 2722,
+ 2732,
+ 2742,
+ 2752,
+ 2764,
+ 2774,
+ 2786,
+ 2798,
+ 2810,
+ 2822,
+ 2834,
+ 2846,
+ 2858,
+ 2870,
+ 2884,
+ 2896,
+ 2910,
+ 2924,
+ 2938,
+ 2952,
+ 2966,
+ 2980,
+ 2994,
+ 3010,
+ 3024,
+ 3040,
+ 3056,
+ 3070,
+ 3086,
+ 3104,
+ 3120,
+ 3136,
+ 3154,
+ 3170,
+ 3188,
+ 3206,
+ 3224,
+ 3242,
+ 3262,
+ 3280,
+ 3300,
+ 3320,
+ 3338,
+ 3360,
+ 3380,
+ 3400,
+ 3422,
+ 3442,
+ 3464,
+ 3486,
+ 3508,
+ 3532,
+ 3554,
+ 3578,
+ 3602,
+ 3626,
+ 3652,
+ 3676,
+ 3702,
+ 3728,
+ 3754,
+ 3780,
+ 3808,
+ 3836,
+ 3864,
+ 3892,
+ 3920,
+ 3950,
+ 3980,
+ 4010,
+ 4042,
+ 4074,
+ 4106,
+ 4138,
+ 4172,
+ 4206,
+ 4240,
+ 4276,
+ 4312,
+ 4348,
+ 4384,
+ 4422,
+ 4460,
+ 4500,
+ 4540,
+ 4580,
+ 4622,
+ 4664,
+ 4708,
+ 4752,
+ 4796,
+ 4842,
+ 4890,
+ 4938,
+ 4986,
+ 5036,
+ 5086,
+ 5138,
+ 5192,
+ 5246,
+ 5300,
+ 5358,
+ 5416,
+ 5474,
+ 5534,
+ 5596,
+ 5660,
+ 5726,
+ 5792,
+ 5860,
+ 5930,
+ 6002,
+ 6074,
+ 6150,
+ 6226,
+ 6306,
+ 6388,
+ 6470,
+ 6556,
+ 6644,
+ 6736,
+ 6828,
+ 6924,
+ 7022,
+ 7124,
+ 7228,
+ 7336,
+ 7448,
+ 7562,
+ 7680,
+ 7802,
+ 7928,
+ 8058,
+ 8192,
+ 8332,
+ 8476,
+ 8624,
+ 8780,
+ 8940,
+ 9106,
+ 9278,
+ 9458,
+ 9644,
+ 9840,
+ 10042,
+ 10252,
+ 10472,
+ 10702,
+ 10942,
+ 11194,
+ 11458,
+ 11734,
+ 12024,
+ 12328,
+ 12648,
+ 12986,
+ 13342,
+ 13720,
+ 14118,
+ 14540,
+ 14990,
+ 15466,
+ 15976,
+ 16520,
+ 17102,
+ 17726,
+ 18398,
+ 19124,
+ 19908,
+ 20760,
+ 21688,
+ 22702,
+ 23816,
+ 25044,
+ 26404,
+ 27922,
+ 29622,
+ 31540,
+ 33720,
+ 36222,
+ 39116,
+ 42502,
+ 46514,
+ 51334,
+ 57218,
+ 64536,
+ 73830,
+ 85890,
+ 101860,
+ 123198,
+ 151020,
+ 183936,
+ 216220,
+ 243618,
+ 268374,
+ 293022,
+ 319362,
+ 347768,
+ 378864,
+ 412626,
+ 449596,
+};
+
+static const int32_t dq9mLamb24[256] =
+{
+ 0,
+ 0,
+ 0,
+ -1,
+ 0,
+ 0,
+ -1,
+ -1,
+ 0,
+ -1,
+ -1,
+ -1,
+ -1,
+ -1,
+ -1,
+ -1,
+ -1,
+ -1,
+ -1,
+ -1,
+ -1,
+ -1,
+ -1,
+ -1,
+ -1,
+ -2,
+ -1,
+ -1,
+ -2,
+ -2,
+ -2,
+ -1,
+ -2,
+ -2,
+ -2,
+ -2,
+ -2,
+ -2,
+ -2,
+ -2,
+ -2,
+ -2,
+ -2,
+ -2,
+ -2,
+ -2,
+ -2,
+ -3,
+ -2,
+ -3,
+ -2,
+ -3,
+ -3,
+ -3,
+ -3,
+ -3,
+ -3,
+ -3,
+ -3,
+ -3,
+ -3,
+ -3,
+ -3,
+ -3,
+ -3,
+ -3,
+ -3,
+ -3,
+ -3,
+ -4,
+ -3,
+ -4,
+ -4,
+ -4,
+ -4,
+ -4,
+ -4,
+ -4,
+ -4,
+ -4,
+ -4,
+ -4,
+ -4,
+ -4,
+ -5,
+ -4,
+ -4,
+ -5,
+ -4,
+ -5,
+ -5,
+ -5,
+ -5,
+ -5,
+ -5,
+ -5,
+ -5,
+ -5,
+ -6,
+ -5,
+ -5,
+ -6,
+ -5,
+ -6,
+ -6,
+ -6,
+ -6,
+ -6,
+ -6,
+ -6,
+ -6,
+ -7,
+ -6,
+ -7,
+ -7,
+ -7,
+ -7,
+ -7,
+ -7,
+ -7,
+ -7,
+ -7,
+ -8,
+ -8,
+ -8,
+ -8,
+ -8,
+ -8,
+ -8,
+ -9,
+ -9,
+ -9,
+ -9,
+ -9,
+ -9,
+ -9,
+ -10,
+ -10,
+ -10,
+ -10,
+ -10,
+ -11,
+ -11,
+ -11,
+ -11,
+ -11,
+ -12,
+ -12,
+ -12,
+ -12,
+ -13,
+ -13,
+ -13,
+ -14,
+ -14,
+ -14,
+ -15,
+ -15,
+ -15,
+ -15,
+ -16,
+ -16,
+ -17,
+ -17,
+ -17,
+ -18,
+ -18,
+ -18,
+ -19,
+ -19,
+ -20,
+ -21,
+ -21,
+ -22,
+ -22,
+ -23,
+ -23,
+ -24,
+ -25,
+ -26,
+ -26,
+ -27,
+ -28,
+ -29,
+ -30,
+ -31,
+ -32,
+ -33,
+ -34,
+ -35,
+ -36,
+ -37,
+ -39,
+ -40,
+ -42,
+ -43,
+ -45,
+ -47,
+ -49,
+ -51,
+ -53,
+ -55,
+ -58,
+ -60,
+ -63,
+ -66,
+ -69,
+ -73,
+ -76,
+ -80,
+ -85,
+ -89,
+ -95,
+ -100,
+ -106,
+ -113,
+ -119,
+ -128,
+ -136,
+ -146,
+ -156,
+ -168,
+ -182,
+ -196,
+ -213,
+ -232,
+ -254,
+ -279,
+ -307,
+ -340,
+ -380,
+ -425,
+ -480,
+ -545,
+ -626,
+ -724,
+ -847,
+ -1003,
+ -1205,
+ -1471,
+ -1830,
+ -2324,
+ -3015,
+ -3993,
+ -5335,
+ -6956,
+ -8229,
+ -8071,
+ -6850,
+ -6189,
+ -6162,
+ -6585,
+ -7102,
+ -7774,
+ -8441,
+ -9243,
+};
+
+/* Array of structures containing subband parameters. */
+static const SubbandParameters subbandParameters[NUMSUBBANDS] =
+{
+ /* LL band */
+ {
+ 0, dq9bit24_sl1, 0, dq9dith24_sf1, dq9mLamb24, q9incr24, 9, (18 * 256) - 1, -20, 24
+ },
+
+ /* LH band */
+ {
+ 0, dq6bit24_sl1, 0, dq6dith24_sf1, dq6mLamb24, q6incr24, 6, (21 * 256) - 1, -23, 12
+ },
+
+ /* HL band */
+ {
+ 0, dq4bit24_sl1, 0, dq4dith24_sf1, dq4mLamb24, q4incr24, 4, (23 * 256) - 1, -25, 6
+ },
+
+ /* HH band */
+ {
+ 0, dq5bit24_sl1, 0, dq5dith24_sf1, dq5mLamb24, q5incr24, 5, (22 * 256) - 1, -24, 12
+ }
+};
+
+
+#endif //APTXTABLES_H
diff --git a/system/embdrv/encoder_for_aptxhd/src/CBStruct.h b/system/embdrv/encoder_for_aptxhd/src/CBStruct.h
new file mode 100644
index 00000000000..95e4d59e488
--- /dev/null
+++ b/system/embdrv/encoder_for_aptxhd/src/CBStruct.h
@@ -0,0 +1,28 @@
+/*------------------------------------------------------------------------------
+ *
+ * Structure required to implement a circular buffer.
+ *
+ *-----------------------------------------------------------------------------*/
+
+#ifndef CBSTRUCT_H
+#define CBSTRUCT_H
+#ifdef _GCC
+ #pragma GCC visibility push(hidden)
+#endif
+
+
+typedef struct circularBuffer_t
+{
+ /* Buffer storage */
+ int32_t buffer[48];
+ /* Pointer to current buffer location */
+ uint32_t pointer;
+ /* Modulo length of circular buffer */
+ uint32_t modulo;
+} circularBuffer;
+
+
+#ifdef _GCC
+ #pragma GCC visibility pop
+#endif
+#endif //CBSTRUCT_H
diff --git a/system/embdrv/encoder_for_aptxhd/src/CodewordPacker.h b/system/embdrv/encoder_for_aptxhd/src/CodewordPacker.h
new file mode 100644
index 00000000000..67a9efbbbfb
--- /dev/null
+++ b/system/embdrv/encoder_for_aptxhd/src/CodewordPacker.h
@@ -0,0 +1,50 @@
+/*------------------------------------------------------------------------------
+ *
+ * Prototype declaration of the CodewordPacker Function
+ *
+ * This functions allows a client to supply an array of 4 quantised codes
+ * (1 per subband) and obtain a packed version as a 24-bit aptX HD codeword.
+ *
+ *----------------------------------------------------------------------------*/
+
+#ifndef CODEWORDPACKER_H
+#define CODEWORDPACKER_H
+
+
+#include "AptxParameters.h"
+
+
+/* This functions allows a client to supply an array of 4 quantised codes
+ * (1 per subband) and obtain a packed version as a 24-bit aptX HD codeword. */
+XBT_INLINE_ int32_t packCodeword(Encoder_data* EncoderDataPt)
+{
+ int32_t syncContribution;
+ int32_t hhCode;
+ int32_t codeword;
+
+ /* The per-channel contribution to derive the current sync bit is the XOR of
+ * the 4 code lsbs and the random dither bit. The SyncInserter engineers it
+ * such that the XOR of the sync contributions from the left and right
+ * channel give the actual sync bit value. The per-channel sync bit
+ * contribution overwrites the HH code lsb in the packed codeword. */
+ syncContribution =
+ (EncoderDataPt->m_qdata[0].qCode ^
+ EncoderDataPt->m_qdata[1].qCode ^
+ EncoderDataPt->m_qdata[2].qCode ^
+ EncoderDataPt->m_qdata[3].qCode ^
+ EncoderDataPt->m_dithSyncRandBit) & 0x1;
+ hhCode = (EncoderDataPt->m_qdata[HH].qCode & 0x1eL) | syncContribution;
+
+ /* Pack the 24-bit codeword with the appropriate number of lsbs from each
+ * quantised code (LL=9, LH=6, HL=4, HH=5). */
+ codeword =
+ (EncoderDataPt->m_qdata[LL].qCode & 0x1ff) |
+ ((EncoderDataPt->m_qdata[LH].qCode & 0x3f) << 9) |
+ ((EncoderDataPt->m_qdata[HL].qCode & 0xf) << 15) |
+ (hhCode << 19);
+
+ return codeword;
+}
+
+
+#endif //CODEWORDPACKER_H
diff --git a/system/embdrv/encoder_for_aptxhd/src/DitherGenerator.h b/system/embdrv/encoder_for_aptxhd/src/DitherGenerator.h
new file mode 100644
index 00000000000..60fb15d7f80
--- /dev/null
+++ b/system/embdrv/encoder_for_aptxhd/src/DitherGenerator.h
@@ -0,0 +1,101 @@
+/*------------------------------------------------------------------------------
+ *
+ * These functions allow clients to update an internal codeword history
+ * attribute from previously-generated quantised codes, and to generate a new
+ * pseudo-random dither value per subband from this internal attribute.
+ *
+ *----------------------------------------------------------------------------*/
+
+#ifndef DITHERGENERATOR_H
+#define DITHERGENERATOR_H
+
+
+#include "AptxParameters.h"
+
+
+/* This function updates an internal bit-pool (private variable in DitherGenerator)
+ * based on bits obtained from previously-encoded or received aptX HD codewords. */
+XBT_INLINE_ int32_t xbtEncupdateCodewordHistory(const int32_t quantisedCodes[4], int32_t m_codewordHistory)
+{
+ int32_t newBits;
+ int32_t updatedCodewordHistory;
+
+ const int32_t llMask = 0x3L;
+ const int32_t lhMask = 0x2L;
+ const int32_t hlMask = 0x1L;
+ const uint32_t lhShift = 1;
+ const uint32_t hlShift = 3;
+ /* Shift value to left-justify a 24-bit value in a 32-bit signed variable*/
+ const uint32_t leftJustifyShift = 8;
+ const uint32_t numNewBits = 4;
+
+ /* Make a 4-bit vector from particular bits from 3 quantised codes */
+ newBits = (quantisedCodes[LL] & llMask) +
+ ((quantisedCodes[LH] & lhMask) << lhShift) +
+ ((quantisedCodes[HL] & hlMask) << hlShift);
+
+ /* Add the 4 new bits to the codeword history. Note that this is a 24-bit
+ * value LEFT-JUSTIFIED in a 32-bit signed variable. Maintaining the history
+ * as signed is useful in the dither generation process below. */
+ updatedCodewordHistory =
+ (m_codewordHistory << numNewBits) + (newBits << leftJustifyShift);
+
+ return updatedCodewordHistory;
+}
+
+/* Function to generate a dither value for each subband based
+ * on the current contents of the codewordHistory bit-pool. */
+XBT_INLINE_ int32_t xbtEncgenerateDither(int32_t m_codewordHistory, int32_t* m_ditherOutputs)
+{
+ int32_t history24b;
+ int32_t upperAcc, lowerAcc;
+ int32_t accSum;
+ int64_t tmp_acc;
+ int32_t ditherSample;
+ int32_t m_dithSyncRandBit;
+
+ /* Fixed value to multiply codeword history variable by */
+ const uint32_t dithConstMultiplier = 0x4f1bbbL;
+ /* Shift value to left-justify a 24-bit value in a 32-bit signed variable*/
+ const uint32_t leftJustifyShift = 8;
+
+ /* AND mask to retain only the lower 24 bits of a variable */
+ const int32_t keepLower24bitsMask = 0xffffffL;
+
+ /* Convert the codeword history to a 24-bit signed value. This can be done
+ * cheaply with a 8-position right-shift since it is maintained as 24-bits
+ * value left-justified in a signed 32-bit variable. */
+ history24b = m_codewordHistory >> (leftJustifyShift - 1);
+
+ /* Multiply the history by a fixed constant. The constant has already been
+ * shifted right by 1 position to compensate for the left-shift introduced
+ * on the product by the fractional multiplier. */
+ tmp_acc = ((int64_t)history24b * (int64_t)dithConstMultiplier);
+
+ /* Get the upper and lower 24-bit values from the accumulator, and form
+ * their sum. */
+ upperAcc = ((int32_t)(tmp_acc >> 24)) & 0x00FFFFFFL;
+ lowerAcc = ((int32_t)tmp_acc) & 0x00FFFFFFL;
+ accSum = upperAcc + lowerAcc;
+
+ /* The dither sample is the 2 msbs of lowerAcc and the 22 lsbs of accSum */
+ ditherSample = ((lowerAcc >> 22) + (accSum << 2)) & keepLower24bitsMask;
+
+ /* The sign bit of 24-bit accSum is saved as a random bit to
+ * assist in the apt-X sync insertion process. */
+ m_dithSyncRandBit = (accSum >> 23) & 0x1;
+
+ /* Successive dither outputs for the 4 subbands are versions of ditherSample
+ * offset by a further 5-position left shift for each subband. Also apply a
+ * constant left-shift of 8 to turn the values into signed 24-bit values
+ * left-justified in the 32-bit ditherOutput variable. */
+ m_ditherOutputs[HH] = ditherSample << leftJustifyShift;
+ m_ditherOutputs[HL] = ditherSample << (5 + leftJustifyShift);
+ m_ditherOutputs[LH] = ditherSample << (10 + leftJustifyShift);
+ m_ditherOutputs[LL] = ditherSample << (15 + leftJustifyShift);
+
+ return m_dithSyncRandBit;
+}
+
+
+#endif //DITHERGENERATOR_H
diff --git a/system/embdrv/encoder_for_aptxhd/src/ProcessSubband.c b/system/embdrv/encoder_for_aptxhd/src/ProcessSubband.c
new file mode 100644
index 00000000000..e4de607c382
--- /dev/null
+++ b/system/embdrv/encoder_for_aptxhd/src/ProcessSubband.c
@@ -0,0 +1,43 @@
+#include "AptxParameters.h"
+#include "SubbandFunctionsCommon.h"
+#include "SubbandFunctions.h"
+
+
+/* This function carries out all subband processing (common to both encode and decode). */
+void processSubband_HD(const int32_t qCode, const int32_t ditherVal, Subband_data* SubbandDataPt, IQuantiser_data* iqDataPt)
+{
+ /* Inverse quantisation */
+ invertQuantisation(qCode, ditherVal, iqDataPt);
+
+ /* Predictor pole coefficient update */
+ updatePredictorPoleCoefficients(iqDataPt->invQ, SubbandDataPt->m_predData.m_zeroVal, &SubbandDataPt->m_PoleCoeffData);
+
+ /* Predictor filtering */
+ performPredictionFiltering(iqDataPt->invQ, SubbandDataPt);
+}
+
+/* processSubband_HDLL is used for the LL subband only. */
+void processSubband_HDLL(const int32_t qCode, const int32_t ditherVal, Subband_data* SubbandDataPt, IQuantiser_data* iqDataPt)
+{
+ /* Inverse quantisation */
+ invertQuantisation(qCode, ditherVal, iqDataPt);
+
+ /* Predictor pole coefficient update */
+ updatePredictorPoleCoefficients(iqDataPt->invQ, SubbandDataPt->m_predData.m_zeroVal, &SubbandDataPt->m_PoleCoeffData);
+
+ /* Predictor filtering */
+ performPredictionFilteringLL(iqDataPt->invQ, SubbandDataPt);
+}
+
+/* processSubband_HDLL is used for the HL subband only. */
+void processSubband_HDHL(const int32_t qCode, const int32_t ditherVal, Subband_data* SubbandDataPt, IQuantiser_data* iqDataPt)
+{
+ /* Inverse quantisation */
+ invertQuantisationHL(qCode, ditherVal, iqDataPt);
+
+ /* Predictor pole coefficient update */
+ updatePredictorPoleCoefficients(iqDataPt->invQ, SubbandDataPt->m_predData.m_zeroVal, &SubbandDataPt->m_PoleCoeffData);
+
+ /* Predictor filtering */
+ performPredictionFilteringHL(iqDataPt->invQ, SubbandDataPt);
+}
diff --git a/system/embdrv/encoder_for_aptxhd/src/Qmf.h b/system/embdrv/encoder_for_aptxhd/src/Qmf.h
new file mode 100644
index 00000000000..91d91b8d94f
--- /dev/null
+++ b/system/embdrv/encoder_for_aptxhd/src/Qmf.h
@@ -0,0 +1,150 @@
+/*------------------------------------------------------------------------------
+ *
+ * This file includes the coefficient tables or the two convolution function
+ * It also includes the definition of Qmf_storage and the prototype of all
+ * necessary functions required to implement the QMF filtering.
+ *
+ *----------------------------------------------------------------------------*/
+
+#ifndef QMF_H
+#define QMF_H
+
+
+#include "AptxParameters.h"
+
+
+typedef struct
+{
+ int32_t QmfL_buf[32];
+ int32_t QmfH_buf[32];
+ int32_t QmfLH_buf[32];
+ int32_t QmfHL_buf[32];
+ int32_t QmfLL_buf[32];
+ int32_t QmfHH_buf[32];
+ int32_t QmfI_pt;
+ int32_t QmfO_pt;
+} Qmf_storage;
+
+/* Outer QMF filter for aptX HD is a symmetrical 32-tap filter (16
+ * different coefficients). The table defined in QmfConv.c */
+#ifndef _STDQMFOUTERCOEFF
+const int32_t Qmf_outerCoeffs[12] =
+{
+ /* (C(1/30)C(3/28)), C(5/26), C(7/24) */
+ 0xFE6302DA, 0xFFFFDA75, 0x0000AA6A,
+ /* C(9/22), C(11/20), C(13/18), C(15/16) */
+ 0xFFFE273E, 0x00041E95, 0xFFF710B5, 0x002AC12E,
+ /* C(17/14), C(19/12), (C(21/10)C(23/8)) */
+ 0x000AA328, 0xFFFD8D1F, 0x211E6BDB,
+ /* (C(25/6)C(27/4)), (C(29/2)C(31/0)) */
+ 0x0DB7D8C5, 0xFC7F02B0
+};
+#else
+const int32_t Qmf_outerCoeffs[16] =
+{
+ 730, -413, -9611, 43626,
+ -121026, 269973, -585547, 2801966,
+ 697128, -160481, 27611, 8478,
+ -10043, 3511, 688, -897
+};
+#endif
+
+/* Each inner QMF filter for aptX HD is a symmetrical 32-tap filter (16
+ * different coefficients) */
+const int32_t Qmf_innerCoeffs[16] =
+{
+ 1033, -584, -13592, 61697,
+ -171156, 381799, -828088, 3962579,
+ 985888, -226954, 39048, 11990,
+ -14203, 4966, 973, -1268
+};
+
+void AsmQmfConvI_HD (const int32_t *p1dl_buffPtr, const int32_t *p2dl_buffPtr, const int32_t *coeffPtr, int32_t* filterOutputs);
+void AsmQmfConvO_HD(const int32_t *p1dl_buffPtr, const int32_t *p2dl_buffPtr, const int32_t *coeffPtr, int32_t* convSumDiff);
+
+XBT_INLINE_ void QmfAnalysisFilter(const int32_t pcm[4], Qmf_storage* Qmf_St, const int32_t* predVals, int32_t* aqmfOutputs)
+{
+ int32_t convSumDiff[4];
+ int32_t filterOutputs[4];
+
+ int32_t lc_QmfO_pt = (Qmf_St->QmfO_pt);
+ int32_t lc_QmfI_pt = (Qmf_St->QmfI_pt);
+
+ /* Run the analysis QMF */
+ /* Symbolic constants to represent the first and second set out outer filter
+ * outputs. */
+ enum { FirstOuterOutputs = 0, SecondOuterOutputs = 1 };
+
+ /* Load outer filter phase1 and phase2 delay lines with the first 2 PCM
+ * samples. Convolve the filter and get the 2 convolution results. */
+ Qmf_St->QmfL_buf[lc_QmfO_pt + 16] = pcm[FirstPcm];
+ Qmf_St->QmfL_buf[lc_QmfO_pt] = pcm[FirstPcm];
+ Qmf_St->QmfH_buf[lc_QmfO_pt + 16] = pcm[SecondPcm];
+ Qmf_St->QmfH_buf[lc_QmfO_pt++] = pcm[SecondPcm];
+ lc_QmfO_pt &= 0xF;
+
+ AsmQmfConvO_HD(&Qmf_St->QmfL_buf[lc_QmfO_pt + 15], &Qmf_St->QmfH_buf[lc_QmfO_pt],
+ Qmf_outerCoeffs, &convSumDiff[0]);
+
+ /* Load outer filter phase1 and phase2 delay lines with the second 2 PCM
+ * samples. Convolve the filter and get the 2 convolution results. */
+ Qmf_St->QmfL_buf[lc_QmfO_pt + 16] = pcm[ThirdPcm];
+ Qmf_St->QmfL_buf[lc_QmfO_pt] = pcm[ThirdPcm];
+ Qmf_St->QmfH_buf[lc_QmfO_pt + 16] = pcm[FourthPcm];
+ Qmf_St->QmfH_buf[lc_QmfO_pt++] = pcm[FourthPcm];
+ lc_QmfO_pt &= 0xF;
+
+ AsmQmfConvO_HD(&Qmf_St->QmfL_buf[lc_QmfO_pt + 15], &Qmf_St->QmfH_buf[lc_QmfO_pt],
+ Qmf_outerCoeffs, &convSumDiff[1]);
+
+ /* Load the first inner filter phase1 and phase2 delay lines with the 2
+ * convolution sum (low-pass) outer filter outputs. Convolve the filter and
+ * get the 2 convolution results. The first 2 analysis filter outputs are
+ * the sum and difference values for the first inner filter convolutions. */
+ Qmf_St->QmfLL_buf[lc_QmfI_pt + 16] = convSumDiff[0];
+ Qmf_St->QmfLL_buf[lc_QmfI_pt] = convSumDiff[0];
+ Qmf_St->QmfLH_buf[lc_QmfI_pt + 16] = convSumDiff[1];
+ Qmf_St->QmfLH_buf[lc_QmfI_pt] = convSumDiff[1];
+
+ AsmQmfConvI_HD(&Qmf_St->QmfLL_buf[lc_QmfI_pt + 16], &Qmf_St->QmfLH_buf[lc_QmfI_pt + 1],
+ &Qmf_innerCoeffs[0], &filterOutputs[LL]);
+
+ /* Load the second inner filter phase1 and phase2 delay lines with the 2
+ * convolution difference (high-pass) outer filter outputs. Convolve the
+ * filter and get the 2 convolution results. The second 2 analysis filter
+ * outputs are the sum and difference values for the second inner filter
+ * convolutions. */
+ Qmf_St->QmfHL_buf[lc_QmfI_pt + 16] = convSumDiff[2];
+ Qmf_St->QmfHL_buf[lc_QmfI_pt] = convSumDiff[2];
+ Qmf_St->QmfHH_buf[lc_QmfI_pt + 16] = convSumDiff[3];
+ Qmf_St->QmfHH_buf[lc_QmfI_pt++] = convSumDiff[3];
+ lc_QmfI_pt &= 0xF;
+
+ AsmQmfConvI_HD(&Qmf_St->QmfHL_buf[lc_QmfI_pt + 15], &Qmf_St->QmfHH_buf[lc_QmfI_pt],
+ &Qmf_innerCoeffs[0], &filterOutputs[HL]);
+
+ /* Subtracted the previous predicted value from the filter output on a
+ * per-subband basis. Ensure these values are saturated, if necessary.
+ * Manual unrolling */
+ aqmfOutputs[LL] = filterOutputs[LL] - predVals[LL];
+ if (aqmfOutputs[LL] > 8388607) aqmfOutputs[LL] = 8388607;
+ if (aqmfOutputs[LL] < -8388608) aqmfOutputs[LL] = -8388608;
+
+ aqmfOutputs[LH] = filterOutputs[LH] - predVals[LH];
+ if (aqmfOutputs[LH] > 8388607) aqmfOutputs[LH] = 8388607;
+ if (aqmfOutputs[LH] < -8388608) aqmfOutputs[LH] = -8388608;
+
+ aqmfOutputs[HL] = filterOutputs[HL] - predVals[HL];
+ if (aqmfOutputs[HL] > 8388607) aqmfOutputs[HL] = 8388607;
+ if (aqmfOutputs[HL] < -8388608) aqmfOutputs[HL] = -8388608;
+
+ aqmfOutputs[HH] = filterOutputs[HH] - predVals[HH];
+ if (aqmfOutputs[HH] > 8388607) aqmfOutputs[HH] = 8388607;
+ if (aqmfOutputs[HH] < -8388608) aqmfOutputs[HH] = -8388608;
+
+ (Qmf_St->QmfO_pt) = lc_QmfO_pt;
+ (Qmf_St->QmfI_pt) = lc_QmfI_pt;
+}
+
+
+#endif //QMF_H
diff --git a/system/embdrv/encoder_for_aptxhd/src/QmfConv.c b/system/embdrv/encoder_for_aptxhd/src/QmfConv.c
new file mode 100644
index 00000000000..16ab0c8d76c
--- /dev/null
+++ b/system/embdrv/encoder_for_aptxhd/src/QmfConv.c
@@ -0,0 +1,337 @@
+/*------------------------------------------------------------------------------
+ *
+ * This file includes convolution functions required for the Qmf.
+ *
+ *----------------------------------------------------------------------------*/
+
+#include "AptxParameters.h"
+
+
+void AsmQmfConvO_HD(const int32_t* p1dl_buffPtr, const int32_t* p2dl_buffPtr, const int32_t* coeffPtr, int32_t* convSumDiff)
+{
+ /* Since all manipulated data are "int16_t" it is possible to
+ * reduce the number of loads by using int32_t type and manipulating
+ * pairs of data
+ */
+
+ int32_t acc;
+ // Manual inlining as IAR compiler does not seem to do it itself...
+ // WARNING: This inlining assumes that m_qmfDelayLineLength == 16
+ int32_t tmp_round0;
+ int64_t local_acc0;
+ int64_t local_acc1;
+
+ int32_t coeffVal0;
+ int32_t coeffVal1;
+ int32_t data0;
+ int32_t data1;
+ int32_t data2;
+ int32_t data3;
+ int32_t phaseConv[2];
+ int32_t convSum;
+ int32_t convDiff;
+
+ coeffVal0 = (*(coeffPtr));
+ coeffVal1 = (*(coeffPtr + 1));
+ data0 = (*(p1dl_buffPtr));
+ data1 = (*(p2dl_buffPtr));
+ data2 = (*(p1dl_buffPtr - 1));
+ data3 = (*(p2dl_buffPtr + 1));
+
+ local_acc0 = ((int64_t)(coeffVal0) * (int64_t)data0);
+ local_acc1 = ((int64_t)(coeffVal0) * (int64_t)data1);
+ local_acc0 += ((int64_t)(coeffVal1) * (int64_t)data2);
+ local_acc1 += ((int64_t)(coeffVal1) * (int64_t)data3);
+
+ coeffVal0 = (*(coeffPtr + 2));
+ coeffVal1 = (*(coeffPtr + 3));
+ data0 = (*(p1dl_buffPtr - 2));
+ data1 = (*(p2dl_buffPtr + 2));
+ data2 = (*(p1dl_buffPtr - 3));
+ data3 = (*(p2dl_buffPtr + 3));
+
+ local_acc0 += ((int64_t)(coeffVal0) * (int64_t)data0);
+ local_acc1 += ((int64_t)(coeffVal0) * (int64_t)data1);
+ local_acc0 += ((int64_t)(coeffVal1) * (int64_t)data2);
+ local_acc1 += ((int64_t)(coeffVal1) * (int64_t)data3);
+
+ coeffVal0 = (*(coeffPtr + 4));
+ coeffVal1 = (*(coeffPtr + 5));
+ data0 = (*(p1dl_buffPtr - 4));
+ data1 = (*(p2dl_buffPtr + 4));
+ data2 = (*(p1dl_buffPtr - 5));
+ data3 = (*(p2dl_buffPtr + 5));
+
+ local_acc0 += ((int64_t)(coeffVal0) * (int64_t)data0);
+ local_acc1 += ((int64_t)(coeffVal0) * (int64_t)data1);
+ local_acc0 += ((int64_t)(coeffVal1) * (int64_t)data2);
+ local_acc1 += ((int64_t)(coeffVal1) * (int64_t)data3);
+
+ coeffVal0 = (*(coeffPtr + 6));
+ coeffVal1 = (*(coeffPtr + 7));
+ data0 = (*(p1dl_buffPtr - 6));
+ data1 = (*(p2dl_buffPtr + 6));
+ data2 = (*(p1dl_buffPtr - 7));
+ data3 = (*(p2dl_buffPtr + 7));
+
+ local_acc0 += ((int64_t)(coeffVal0) * (int64_t)data0);
+ local_acc1 += ((int64_t)(coeffVal0) * (int64_t)data1);
+ local_acc0 += ((int64_t)(coeffVal1) * (int64_t)data2);
+ local_acc1 += ((int64_t)(coeffVal1) * (int64_t)data3);
+
+ coeffVal0 = (*(coeffPtr + 8));
+ coeffVal1 = (*(coeffPtr + 9));
+ data0 = (*(p1dl_buffPtr - 8));
+ data1 = (*(p2dl_buffPtr + 8));
+ data2 = (*(p1dl_buffPtr - 9));
+ data3 = (*(p2dl_buffPtr + 9));
+
+ local_acc0 += ((int64_t)(coeffVal0) * (int64_t)data0);
+ local_acc1 += ((int64_t)(coeffVal0) * (int64_t)data1);
+ local_acc0 += ((int64_t)(coeffVal1) * (int64_t)data2);
+ local_acc1 += ((int64_t)(coeffVal1) * (int64_t)data3);
+
+ coeffVal0 = (*(coeffPtr + 10));
+ coeffVal1 = (*(coeffPtr + 11));
+ data0 = (*(p1dl_buffPtr - 10));
+ data1 = (*(p2dl_buffPtr + 10));
+ data2 = (*(p1dl_buffPtr - 11));
+ data3 = (*(p2dl_buffPtr + 11));
+
+ local_acc0 += ((int64_t)(coeffVal0) * (int64_t)data0);
+ local_acc1 += ((int64_t)(coeffVal0) * (int64_t)data1);
+ local_acc0 += ((int64_t)(coeffVal1) * (int64_t)data2);
+ local_acc1 += ((int64_t)(coeffVal1) * (int64_t)data3);
+
+ coeffVal0 = (*(coeffPtr + 12));
+ coeffVal1 = (*(coeffPtr + 13));
+ data0 = (*(p1dl_buffPtr - 12));
+ data1 = (*(p2dl_buffPtr + 12));
+ data2 = (*(p1dl_buffPtr - 13));
+ data3 = (*(p2dl_buffPtr + 13));
+
+ local_acc0 += ((int64_t)(coeffVal0) * (int64_t)data0);
+ local_acc1 += ((int64_t)(coeffVal0) * (int64_t)data1);
+ local_acc0 += ((int64_t)(coeffVal1) * (int64_t)data2);
+ local_acc1 += ((int64_t)(coeffVal1) * (int64_t)data3);
+
+ coeffVal0 = (*(coeffPtr + 14));
+ coeffVal1 = (*(coeffPtr + 15));
+ data0 = (*(p1dl_buffPtr - 14));
+ data1 = (*(p2dl_buffPtr + 14));
+ data2 = (*(p1dl_buffPtr - 15));
+ data3 = (*(p2dl_buffPtr + 15));
+
+ local_acc0 += ((int64_t)(coeffVal0) * (int64_t)data0);
+ local_acc1 += ((int64_t)(coeffVal0) * (int64_t)data1);
+ local_acc0 += ((int64_t)(coeffVal1) * (int64_t)data2);
+ local_acc1 += ((int64_t)(coeffVal1) * (int64_t)data3);
+
+ tmp_round0 = (int32_t)local_acc0;
+
+ local_acc0 += 0x00400000L;
+ acc = (int32_t)(local_acc0 >> 23);
+
+ if ((((tmp_round0 << 8) ^ 0x40000000) == 0))
+ {
+ acc--;
+ }
+
+ if (acc > 8388607)
+ acc = 8388607;
+ if (acc < -8388608)
+ acc = -8388608;
+
+ phaseConv[0] = (int32_t)acc;
+
+ tmp_round0 = (int32_t)local_acc1;
+
+ local_acc1 += 0x00400000L;
+ acc = (int32_t)(local_acc1 >> 23);
+ if ((((tmp_round0 << 8) ^ 0x40000000) == 0))
+ {
+ acc--;
+ }
+
+ if (acc > 8388607)
+ acc = 8388607;
+ if (acc < -8388608)
+ acc = -8388608;
+
+ phaseConv[1] = (int32_t)acc;
+
+ convSum = phaseConv[1] + phaseConv[0];
+ if (convSum > 8388607)
+ convSum = 8388607;
+ if (convSum < -8388608)
+ convSum = -8388608;
+
+ convDiff = phaseConv[1] - phaseConv[0];
+ if (convDiff > 8388607)
+ convDiff = 8388607;
+ if (convDiff < -8388608)
+ convDiff = -8388608;
+
+ *(convSumDiff) = convSum;
+ *(convSumDiff + 2) = convDiff;
+}
+
+void AsmQmfConvI_HD(const int32_t* p1dl_buffPtr, const int32_t* p2dl_buffPtr, const int32_t* coeffPtr, int32_t* filterOutputs)
+{
+ int32_t acc;
+ // WARNING: This inlining assumes that m_qmfDelayLineLength == 16
+ int32_t tmp_round0;
+ int64_t local_acc0;
+ int64_t local_acc1;
+
+ int32_t coeffVal0;
+ int32_t coeffVal1;
+ int32_t data0;
+ int32_t data1;
+ int32_t data2;
+ int32_t data3;
+ int32_t phaseConv[2];
+ int32_t convSum;
+ int32_t convDiff;
+
+ coeffVal0 = (*(coeffPtr));
+ coeffVal1 = (*(coeffPtr + 1));
+ data0 = (*(p1dl_buffPtr));
+ data1 = (*(p2dl_buffPtr));
+ data2 = (*(p1dl_buffPtr - 1));
+ data3 = (*(p2dl_buffPtr + 1));
+
+ local_acc0 = ((int64_t)(coeffVal0)*data0);
+ local_acc1 = ((int64_t)(coeffVal0)*data1);
+ local_acc0 += ((int64_t)(coeffVal1)*data2);
+ local_acc1 += ((int64_t)(coeffVal1)*data3);
+
+ coeffVal0 = (*(coeffPtr + 2));
+ coeffVal1 = (*(coeffPtr + 3));
+ data0 = (*(p1dl_buffPtr - 2));
+ data1 = (*(p2dl_buffPtr + 2));
+ data2 = (*(p1dl_buffPtr - 3));
+ data3 = (*(p2dl_buffPtr + 3));
+
+ local_acc0 += ((int64_t)(coeffVal0)*data0);
+ local_acc1 += ((int64_t)(coeffVal0)*data1);
+ local_acc0 += ((int64_t)(coeffVal1)*data2);
+ local_acc1 += ((int64_t)(coeffVal1)*data3);
+
+ coeffVal0 = (*(coeffPtr + 4));
+ coeffVal1 = (*(coeffPtr + 5));
+ data0 = (*(p1dl_buffPtr - 4));
+ data1 = (*(p2dl_buffPtr + 4));
+ data2 = (*(p1dl_buffPtr - 5));
+ data3 = (*(p2dl_buffPtr + 5));
+
+ local_acc0 += ((int64_t)(coeffVal0)*data0);
+ local_acc1 += ((int64_t)(coeffVal0)*data1);
+ local_acc0 += ((int64_t)(coeffVal1)*data2);
+ local_acc1 += ((int64_t)(coeffVal1)*data3);
+
+ coeffVal0 = (*(coeffPtr + 6));
+ coeffVal1 = (*(coeffPtr + 7));
+ data0 = (*(p1dl_buffPtr - 6));
+ data1 = (*(p2dl_buffPtr + 6));
+ data2 = (*(p1dl_buffPtr - 7));
+ data3 = (*(p2dl_buffPtr + 7));
+
+ local_acc0 += ((int64_t)(coeffVal0)*data0);
+ local_acc1 += ((int64_t)(coeffVal0)*data1);
+ local_acc0 += ((int64_t)(coeffVal1)*data2);
+ local_acc1 += ((int64_t)(coeffVal1)*data3);
+
+ coeffVal0 = (*(coeffPtr + 8));
+ coeffVal1 = (*(coeffPtr + 9));
+ data0 = (*(p1dl_buffPtr - 8));
+ data1 = (*(p2dl_buffPtr + 8));
+ data2 = (*(p1dl_buffPtr - 9));
+ data3 = (*(p2dl_buffPtr + 9));
+
+ local_acc0 += ((int64_t)(coeffVal0)*data0);
+ local_acc1 += ((int64_t)(coeffVal0)*data1);
+ local_acc0 += ((int64_t)(coeffVal1)*data2);
+ local_acc1 += ((int64_t)(coeffVal1)*data3);
+
+ coeffVal0 = (*(coeffPtr + 10));
+ coeffVal1 = (*(coeffPtr + 11));
+ data0 = (*(p1dl_buffPtr - 10));
+ data1 = (*(p2dl_buffPtr + 10));
+ data2 = (*(p1dl_buffPtr - 11));
+ data3 = (*(p2dl_buffPtr + 11));
+
+ local_acc0 += ((int64_t)(coeffVal0)*data0);
+ local_acc1 += ((int64_t)(coeffVal0)*data1);
+ local_acc0 += ((int64_t)(coeffVal1)*data2);
+ local_acc1 += ((int64_t)(coeffVal1)*data3);
+
+ coeffVal0 = (*(coeffPtr + 12));
+ coeffVal1 = (*(coeffPtr + 13));
+ data0 = (*(p1dl_buffPtr - 12));
+ data1 = (*(p2dl_buffPtr + 12));
+ data2 = (*(p1dl_buffPtr - 13));
+ data3 = (*(p2dl_buffPtr + 13));
+
+ local_acc0 += ((int64_t)(coeffVal0)*data0);
+ local_acc1 += ((int64_t)(coeffVal0)*data1);
+ local_acc0 += ((int64_t)(coeffVal1)*data2);
+ local_acc1 += ((int64_t)(coeffVal1)*data3);
+
+ coeffVal0 = (*(coeffPtr + 14));
+ coeffVal1 = (*(coeffPtr + 15));
+ data0 = (*(p1dl_buffPtr - 14));
+ data1 = (*(p2dl_buffPtr + 14));
+ data2 = (*(p1dl_buffPtr - 15));
+ data3 = (*(p2dl_buffPtr + 15));
+
+ local_acc0 += ((int64_t)(coeffVal0)*data0);
+ local_acc1 += ((int64_t)(coeffVal0)*data1);
+ local_acc0 += ((int64_t)(coeffVal1)*data2);
+ local_acc1 += ((int64_t)(coeffVal1)*data3);
+
+ tmp_round0 = (int32_t)local_acc0;
+
+ local_acc0 += 0x00400000L;
+ acc = (int32_t)(local_acc0 >> 23);
+
+ if ((((tmp_round0 << 8) ^ 0x40000000) == 0))
+ {
+ acc--;
+ }
+
+ if (acc > 8388607)
+ acc = 8388607;
+ if (acc < -8388608)
+ acc = -8388608;
+
+ phaseConv[0] = (int32_t)acc;
+
+ tmp_round0 = (int32_t)local_acc1;
+
+ local_acc1 += 0x00400000L;
+ acc = (int32_t)(local_acc1 >> 23);
+ if ((((tmp_round0 << 8) ^ 0x40000000) == 0))
+ {
+ acc--;
+ }
+
+ if (acc > 8388607)
+ acc = 8388607;
+ if (acc < -8388608)
+ acc = -8388608;
+
+ phaseConv[1] = (int32_t)acc;
+
+ convSum = phaseConv[1] + phaseConv[0];
+ if (convSum > 8388607) convSum = 8388607;
+ if (convSum < -8388608) convSum = -8388608;
+
+ *(filterOutputs) = convSum;
+
+ convDiff = phaseConv[1] - phaseConv[0];
+ if (convDiff > 8388607) convDiff = 8388607;
+ if (convDiff < -8388608) convDiff = -8388608;
+
+ *(filterOutputs + 1) = convDiff;
+}
diff --git a/system/embdrv/encoder_for_aptxhd/src/QuantiseDifference.c b/system/embdrv/encoder_for_aptxhd/src/QuantiseDifference.c
new file mode 100644
index 00000000000..8b069a52ff3
--- /dev/null
+++ b/system/embdrv/encoder_for_aptxhd/src/QuantiseDifference.c
@@ -0,0 +1,763 @@
+#include "AptxParameters.h"
+
+
+XBT_INLINE_ int32_t BsearchLL(const int32_t absDiffSignalShifted, const int32_t delta, const int32_t* dqbitTablePrt)
+{
+ int32_t qCode;
+ reg64_t tmp_acc;
+ int32_t tmp;
+ int32_t lc_delta = delta << 8;
+
+ qCode = 0;
+
+ tmp_acc.s64 = (int64_t)lc_delta * (int64_t)dqbitTablePrt[128];
+ tmp_acc.s32.h -= absDiffSignalShifted;
+ tmp = tmp_acc.s32.h | (tmp_acc.u32.l >> 1);
+ if (tmp <= 0) qCode = 128;
+
+ tmp_acc.s64 = (int64_t)lc_delta * (int64_t)dqbitTablePrt[qCode + 64];
+ tmp_acc.s32.h -= absDiffSignalShifted;
+ tmp = tmp_acc.s32.h | (tmp_acc.u32.l >> 1);
+ if (tmp <= 0) qCode += 64;
+
+ tmp_acc.s64 = (int64_t)lc_delta * (int64_t)dqbitTablePrt[qCode + 32];
+ tmp_acc.s32.h -= absDiffSignalShifted;
+ tmp = tmp_acc.s32.h | (tmp_acc.u32.l >> 1);
+ if (tmp <= 0) qCode += 32;
+
+ tmp_acc.s64 = (int64_t)lc_delta * (int64_t)dqbitTablePrt[qCode + 16];
+ tmp_acc.s32.h -= absDiffSignalShifted;
+ tmp = tmp_acc.s32.h | (tmp_acc.u32.l >> 1);
+ if (tmp <= 0) qCode += 16;
+ tmp_acc.s64 = (int64_t)lc_delta * (int64_t)dqbitTablePrt[qCode + 8];
+ tmp_acc.s32.h -= absDiffSignalShifted;
+ tmp = tmp_acc.s32.h | (tmp_acc.u32.l >> 1);
+ if (tmp <= 0) qCode += 8;
+
+ tmp_acc.s64 = (int64_t)lc_delta * (int64_t)dqbitTablePrt[qCode + 4];
+ tmp_acc.s32.h -= absDiffSignalShifted;
+ tmp = tmp_acc.s32.h | (tmp_acc.u32.l >> 1);
+ if (tmp <= 0) qCode += 4;
+
+ tmp_acc.s64 = (int64_t)lc_delta * (int64_t)dqbitTablePrt[qCode + 2];
+ tmp_acc.s32.h -= absDiffSignalShifted;
+ tmp = tmp_acc.s32.h | (tmp_acc.u32.l >> 1);
+ if (tmp <= 0) qCode += 2;
+
+ tmp_acc.s64 = (int64_t)lc_delta * (int64_t)dqbitTablePrt[qCode + 1];
+ tmp_acc.s32.h -= absDiffSignalShifted;
+ tmp = tmp_acc.s32.h | (tmp_acc.u32.l >> 1);
+ if (tmp <= 0) qCode++;
+
+ return (qCode);
+}
+
+XBT_INLINE_ int32_t BsearchHL(const int32_t absDiffSignalShifted, const int32_t delta, const int32_t* dqbitTablePrt)
+{
+ int32_t qCode;
+ reg64_t tmp_acc;
+ int32_t tmp;
+ int32_t lc_delta = delta << 8;
+
+ /* first iteration */
+ qCode = 0;
+
+ tmp_acc.s64 = (int64_t)lc_delta * (int64_t)dqbitTablePrt[4];
+ tmp_acc.s32.h -= absDiffSignalShifted;
+ tmp = tmp_acc.s32.h | (tmp_acc.u32.l >> 1);
+ if (tmp <= 0) qCode = 4;
+
+ tmp_acc.s64 = (int64_t)lc_delta * (int64_t)dqbitTablePrt[qCode + 2];
+ tmp_acc.s32.h -= absDiffSignalShifted;
+ tmp = tmp_acc.s32.h | (tmp_acc.u32.l >> 1);
+ if (tmp <= 0) qCode += 2;
+
+ tmp_acc.s64 = (int64_t)lc_delta * (int64_t)dqbitTablePrt[qCode + 1];
+ tmp_acc.s32.h -= absDiffSignalShifted;
+ tmp = tmp_acc.s32.h | (tmp_acc.u32.l >> 1);
+ if (tmp <= 0) qCode++;
+
+ return (qCode);
+}
+
+XBT_INLINE_ int32_t BsearchHH(const int32_t absDiffSignalShifted, const int32_t delta, const int32_t* dqbitTablePrt)
+{
+ int32_t qCode;
+ reg64_t tmp_acc;
+ int32_t tmp;
+ int32_t lc_delta = delta << 8;
+
+ qCode = 0;
+
+ tmp_acc.s64 = (int64_t)lc_delta * (int64_t)dqbitTablePrt[8];
+ tmp_acc.s32.h -= absDiffSignalShifted;
+ tmp = tmp_acc.s32.h | (tmp_acc.u32.l >> 1);
+ if (tmp <= 0) qCode = 8;
+
+ tmp_acc.s64 = (int64_t)lc_delta * (int64_t)dqbitTablePrt[qCode + 4];
+ tmp_acc.s32.h -= absDiffSignalShifted;
+ tmp = tmp_acc.s32.h | (tmp_acc.u32.l >> 1);
+ if (tmp <= 0) qCode += 4;
+
+ tmp_acc.s64 = (int64_t)lc_delta * (int64_t)dqbitTablePrt[qCode + 2];
+ tmp_acc.s32.h -= absDiffSignalShifted;
+ tmp = tmp_acc.s32.h | (tmp_acc.u32.l >> 1);
+ if (tmp <= 0) qCode += 2;
+
+ tmp_acc.s64 = (int64_t)lc_delta * (int64_t)dqbitTablePrt[qCode + 1];
+ tmp_acc.s32.h -= absDiffSignalShifted;
+ tmp = tmp_acc.s32.h | (tmp_acc.u32.l >> 1);
+ if (tmp <= 0) qCode++;
+
+ return (qCode);
+}
+
+void quantiseDifference_HDHL(const int32_t diffSignal, const int32_t ditherVal, const int32_t delta, Quantiser_data* qdata_pt)
+{
+ int32_t absDiffSignal;
+ int32_t absDiffSignalShifted;
+ int32_t index;
+ int32_t dithSquared;
+ int32_t minusLambdaD;
+ int32_t acc;
+ int32_t threshDiff;
+ reg64_t tmp_acc;
+ reg64_t tmp_reg64;
+ int32_t tmp_accL;
+ int32_t tmp_qCode;
+ int32_t tmp_altQcode;
+ uint32_t tmp_round0;
+ int32_t _delta;
+
+ /* Form the absolute value of the difference signal and maintain a version
+ * that is right-shifted 4 places for delta scaling. */
+ absDiffSignal = -diffSignal;
+ if (diffSignal >= 0) absDiffSignal = diffSignal;
+ absDiffSignal = ssat24(absDiffSignal);
+ absDiffSignalShifted = absDiffSignal >> deltaScale;
+
+ /* Binary search for the quantised code. This search terminates with the
+ * table index of the LARGEST threshold table value for which
+ * absDiffSignalShifted >= (delta * threshold)
+ */
+ index = BsearchHL(absDiffSignalShifted, delta, qdata_pt->thresholdTablePtr_sl1);
+
+ /* We actually wanted the SMALLEST magnitude quantised code for which
+ * absDiffSignalShifted < (delta * threshold)
+ * i.e. the code with the next highest magnitude than the one we actually
+ * found. We could add +1 to the code magnitude to do this, but we need to
+ * subtract 1 from the code magnitude to compensate for the "phantom
+ * element" at the base of the quantisation table. These two effects cancel
+ * out, so we leave the value of code alone. However, we need to form code+1
+ * to get the proper index into the both the threshold and dither tables,
+ * since we must skip over the phantom element at the base. */
+ qdata_pt->qCode = index;
+
+ /* Square the dither and get the value back from the ALU
+ * (saturated/rounded). */
+ tmp_acc.s64 = ((int64_t)ditherVal * (int64_t)ditherVal);
+
+ acc = tmp_acc.s32.h;
+ tmp_round0 = (uint32_t)acc << 8;
+
+ acc = (acc >> 6) + 1;
+ acc >>= 1;
+ if (tmp_round0 == 0x40000000L)
+ acc--;
+ acc = ssat24(acc);
+
+ dithSquared = acc;
+
+ /* Form the negative difference of the dither values at index and index-1.
+ * Load the accumulator with this value divided by 2. Ensure saturation is
+ * applied to the difference calculation. */
+ minusLambdaD = qdata_pt->minusLambdaDTable[index];
+
+ tmp_accL = (1 << 23) - (int32_t)dithSquared;
+ tmp_acc.s64 = (int64_t)tmp_accL * minusLambdaD;
+
+ tmp_round0 = (int32_t)tmp_acc.s32.l << 8;
+
+ acc = (tmp_acc.u32.l >> 22) | (tmp_acc.s32.h << 10);
+ acc++;
+ acc >>= 1;
+ if (tmp_round0 == 0x40000000L)
+ acc--;
+
+ /* Add the threshold table values at index and index-1 to the accumulated
+ * value. */
+ acc += qdata_pt->thresholdTablePtr_sl1[index + 1] >> 1;
+ //// worst case value for acc = 0x000d3e08 + 0x43E1DB = 511FE3
+ acc += qdata_pt->thresholdTablePtr_sl1[index] >> 1;
+ //// worst case value for acc = 0x511FE3 + 0x362FEC = 874FCF
+
+ // saturation required
+ acc = ssat24(acc);
+
+ /* Form the threshold table difference at index and index-1. Ensure
+ * saturation is applied to the difference calculation. */
+ threshDiff = qdata_pt->thresholdTablePtr_sl1[index + 1] - qdata_pt->thresholdTablePtr_sl1[index];
+
+ /* Based on the sign of the difference signal, either add or subtract the
+ * threshold table difference from the accumulated value. Recover the final
+ * accumulated value (saturated/rounded) */
+ if (diffSignal < 0)
+ threshDiff = -threshDiff;
+ tmp_reg64.s64 = ((int64_t)ditherVal * (int64_t)threshDiff);
+
+ tmp_reg64.s32.h += acc;
+ acc = tmp_reg64.s32.h;
+
+ if (tmp_reg64.u32.l >= 0x80000000)
+ acc++;
+ tmp_round0 = (tmp_reg64.u32.l >> 1) | (tmp_reg64.s32.h << 31);
+
+ acc = ssat24(acc);
+
+ if (tmp_round0 == 0x40000000L)
+ acc--;
+ _delta = -delta << 8;
+
+ acc = acc << 4;
+
+ /* Form (absDiffSignal * 0.125) - (acc * delta), which is the final distance
+ * signal used to determine if dithering alters the quantised code value or
+ * not. */
+ // worst case value for delta is 0x7d400
+ tmp_reg64.s64 = ((int64_t)acc * (int64_t)_delta);
+ tmp_reg64.s32.h += absDiffSignal;
+ tmp_round0 = (tmp_reg64.u32.l >> 4) | (tmp_reg64.s32.h << 28);
+ acc = tmp_reg64.s32.h + (1 << 2);
+ acc >>= 3;
+ if (tmp_round0 == 0x40000000L)
+ {
+ acc--;
+ }
+
+ tmp_qCode = qdata_pt->qCode;
+ tmp_altQcode = tmp_qCode - 1;
+ /* Check the sign of the distance penalty. Get the sign from the
+ * full-precision accumulator, as done in the Kalimba code. */
+ if (tmp_reg64.s32.h < 0)
+ {
+ /* The distance is -ve. The optimum code needs decremented by 1 and the
+ * alternative code is 1 greater than this. Get the rounded version of the
+ * -ve distance penalty and negate this (form distance magnitude) before
+ * writing the value out */
+ tmp_qCode = tmp_altQcode;
+ tmp_altQcode++;
+ acc = -acc;
+ }
+
+ qdata_pt->distPenalty = acc;
+ /* If the difference signal is negative, bitwise invert the code (restores
+ * sign to the magnitude). */
+ if (diffSignal < 0)
+ {
+ tmp_qCode = ~tmp_qCode;
+ tmp_altQcode = ~tmp_altQcode;
+ }
+ qdata_pt->altQcode = tmp_altQcode;
+ qdata_pt->qCode = tmp_qCode;
+}
+
+void quantiseDifference_HDHH(const int32_t diffSignal, const int32_t ditherVal, const int32_t delta, Quantiser_data* qdata_pt)
+{
+ int32_t absDiffSignal;
+ int32_t absDiffSignalShifted;
+ int32_t index;
+ int32_t dithSquared;
+ int32_t minusLambdaD;
+ int32_t acc;
+ int32_t threshDiff;
+ reg64_t tmp_acc;
+ reg64_t tmp_reg64;
+ int32_t tmp_accL;
+ int32_t tmp_qCode;
+ int32_t tmp_altQcode;
+ uint32_t tmp_round0;
+ int32_t _delta;
+
+ /* Form the absolute value of the difference signal and maintain a version
+ * that is right-shifted 4 places for delta scaling. */
+ absDiffSignal = -diffSignal;
+ if (diffSignal >= 0) absDiffSignal = diffSignal;
+ absDiffSignal = ssat24(absDiffSignal);
+ absDiffSignalShifted = absDiffSignal >> deltaScale;
+
+ /* Binary search for the quantised code. This search terminates with the
+ * table index of the LARGEST threshold table value for which
+ * absDiffSignalShifted >= (delta * threshold)
+ */
+ index = BsearchHH(absDiffSignalShifted, delta, qdata_pt->thresholdTablePtr_sl1);
+
+ /* We actually wanted the SMALLEST magnitude quantised code for which
+ * absDiffSignalShifted < (delta * threshold)
+ * i.e. the code with the next highest magnitude than the one we actually
+ * found. We could add +1 to the code magnitude to do this, but we need to
+ * subtract 1 from the code magnitude to compensate for the "phantom
+ * element" at the base of the quantisation table. These two effects cancel
+ * out, so we leave the value of code alone. However, we need to form code+1
+ * to get the proper index into the both the threshold and dither tables,
+ * since we must skip over the phantom element at the base. */
+ qdata_pt->qCode = index;
+
+ /* Square the dither and get the value back from the ALU
+ * (saturated/rounded). */
+ tmp_acc.s64 = ((int64_t)ditherVal * (int64_t)ditherVal);
+
+ acc = tmp_acc.s32.h;
+ tmp_round0 = (uint32_t)acc << 8;
+
+ acc = (acc >> 6) + 1;
+ acc >>= 1;
+ if (tmp_round0 == 0x40000000L)
+ acc--;
+
+ acc = ssat24(acc);
+
+ dithSquared = acc;
+
+ /* Form the negative difference of the dither values at index and index-1.
+ * Load the accumulator with this value divided by 2. Ensure saturation is
+ * applied to the difference calculation. */
+ minusLambdaD = qdata_pt->minusLambdaDTable[index];
+
+ tmp_accL = (1 << 23) - (int32_t)dithSquared;
+ tmp_acc.s64 = (int64_t)tmp_accL * minusLambdaD;
+
+ tmp_round0 = (int32_t)tmp_acc.s32.l << 8;
+
+ acc = (tmp_acc.u32.l >> 22) | (tmp_acc.s32.h << 10);
+ acc++;
+ acc >>= 1;
+ if (tmp_round0 == 0x40000000L)
+ acc--;
+
+ /* Add the threshold table values at index and index-1 to the accumulated
+ * value. */
+ acc += qdata_pt->thresholdTablePtr_sl1[index + 1] >> 1;
+ //// worst case value for acc = 0x000d3e08 + 0x43E1DB = 511FE3
+ acc += qdata_pt->thresholdTablePtr_sl1[index] >> 1;
+ //// worst case value for acc = 0x511FE3 + 0x362FEC = 874FCF
+
+ // saturation required
+ acc = ssat24(acc);
+
+ /* Form the threshold table difference at index and index-1. Ensure
+ * saturation is applied to the difference calculation. */
+ threshDiff = qdata_pt->thresholdTablePtr_sl1[index + 1] - qdata_pt->thresholdTablePtr_sl1[index];
+
+ /* Based on the sign of the difference signal, either add or subtract the
+ * threshold table difference from the accumulated value. Recover the final
+ * accumulated value (saturated/rounded) */
+ if (diffSignal < 0)
+ threshDiff = -threshDiff;
+ tmp_reg64.s64 = ((int64_t)ditherVal * (int64_t)threshDiff);
+ tmp_reg64.s32.h += acc;
+ acc = tmp_reg64.s32.h;
+
+ if (tmp_reg64.u32.l >= 0x80000000)
+ acc++;
+ tmp_round0 = (tmp_reg64.u32.l >> 1) | (tmp_reg64.s32.h << 31);
+
+ acc = ssat24(acc);
+
+ if (tmp_round0 == 0x40000000L)
+ acc--;
+ _delta = -delta << 8;
+
+ acc = acc << 4;
+
+ /* Form (absDiffSignal * 0.125) - (acc * delta), which is the final distance
+ * signal used to determine if dithering alters the quantised code value or
+ * not. */
+ // worst case value for delta is 0x7d400
+ tmp_reg64.s64 = ((int64_t)acc * (int64_t)_delta);
+ tmp_reg64.s32.h += absDiffSignal;
+ tmp_round0 = (tmp_reg64.u32.l >> 4) | (tmp_reg64.s32.h << 28);
+ acc = tmp_reg64.s32.h + (1 << 2);
+ acc >>= 3;
+ if (tmp_round0 == 0x40000000L)
+ {
+ acc--;
+ }
+
+ tmp_qCode = qdata_pt->qCode;
+ tmp_altQcode = tmp_qCode - 1;
+ /* Check the sign of the distance penalty. Get the sign from the
+ * full-precision accumulator, as done in the Kalimba code. */
+ if (tmp_reg64.s32.h < 0)
+ {
+ /* The distance is -ve. The optimum code needs decremented by 1 and the
+ * alternative code is 1 greater than this. Get the rounded version of the
+ * -ve distance penalty and negate this (form distance magnitude) before
+ * writing the value out */
+ tmp_qCode = tmp_altQcode;
+ tmp_altQcode++;
+ acc = -acc;
+ }
+
+ qdata_pt->distPenalty = acc;
+ /* If the difference signal is negative, bitwise invert the code (restores
+ * sign to the magnitude). */
+ if (diffSignal < 0)
+ {
+ tmp_qCode = ~tmp_qCode;
+ tmp_altQcode = ~tmp_altQcode;
+ }
+ qdata_pt->altQcode = tmp_altQcode;
+ qdata_pt->qCode = tmp_qCode;
+}
+
+void quantiseDifference_HDLL(const int32_t diffSignal, const int32_t ditherVal, const int32_t delta, Quantiser_data* qdata_pt)
+{
+ int32_t absDiffSignal;
+ int32_t absDiffSignalShifted;
+ int32_t index;
+ int32_t dithSquared;
+ int32_t minusLambdaD;
+ int32_t acc;
+ int32_t threshDiff;
+ reg64_t tmp_acc;
+ reg64_t tmp_reg64;
+ int32_t tmp_accL;
+ int32_t tmp_qCode;
+ int32_t tmp_altQcode;
+ uint32_t tmp_round0;
+ int32_t _delta;
+
+ /* Form the absolute value of the difference signal and maintain a version
+ * that is right-shifted 4 places for delta scaling. */
+ absDiffSignal = -diffSignal;
+ if (diffSignal >= 0) absDiffSignal = diffSignal;
+ absDiffSignal = ssat24(absDiffSignal);
+ absDiffSignalShifted = absDiffSignal >> deltaScale;
+
+ /* Binary search for the quantised code. This search terminates with the
+ * table index of the LARGEST threshold table value for which
+ * absDiffSignalShifted >= (delta * threshold)
+ */
+ index = 0;
+
+ index = BsearchLL(absDiffSignalShifted, delta, qdata_pt->thresholdTablePtr_sl1);
+
+ /* We actually wanted the SMALLEST magnitude quantised code for which
+ * absDiffSignalShifted < (delta * threshold)
+ * i.e. the code with the next highest magnitude than the one we actually
+ * found. We could add +1 to the code magnitude to do this, but we need to
+ * subtract 1 from the code magnitude to compensate for the "phantom
+ * element" at the base of the quantisation table. These two effects cancel
+ * out, so we leave the value of code alone. However, we need to form code+1
+ * to get the proper index into the both the threshold and dither tables,
+ * since we must skip over the phantom element at the base. */
+ qdata_pt->qCode = index;
+
+ /* Square the dither and get the value back from the ALU
+ * (saturated/rounded). */
+
+ tmp_acc.s64 = ((int64_t)ditherVal * (int64_t)ditherVal);
+
+ acc = tmp_acc.s32.h;
+ tmp_round0 = (uint32_t)acc << 8;
+
+ acc = (acc >> 6) + 1;
+ acc >>= 1;
+ if (tmp_round0 == 0x40000000L)
+ acc--;
+
+ acc = ssat24(acc);
+
+ dithSquared = acc;
+
+ /* Form the negative difference of the dither values at index and index-1.
+ * Load the accumulator with this value divided by 2. Ensure saturation is
+ * applied to the difference calculation. */
+ minusLambdaD = qdata_pt->minusLambdaDTable[index];
+
+ tmp_accL = (1 << 23) - (int32_t)dithSquared;
+ tmp_acc.s64 = (int64_t)tmp_accL * minusLambdaD;
+
+ tmp_round0 = (int32_t)tmp_acc.s32.l << 8;
+
+ acc = (tmp_acc.u32.l >> 22) | (tmp_acc.s32.h << 10);
+ acc++;
+ acc >>= 1;
+ if (tmp_round0 == 0x40000000L)
+ acc--;
+
+ /* Add the threshold table values at index and index-1 to the accumulated
+ * value. */
+
+ acc += qdata_pt->thresholdTablePtr_sl1[index + 1] >> 1;
+ //// worst case value for acc = 0x000d3e08 + 0x43E1DB = 511FE3
+ acc += qdata_pt->thresholdTablePtr_sl1[index] >> 1;
+ //// worst case value for acc = 0x511FE3 + 0x362FEC = 874FCF
+ // saturation required
+ acc = ssat24(acc);
+
+ /* Form the threshold table difference at index and index-1. Ensure
+ * saturation is applied to the difference calculation. */
+ threshDiff = qdata_pt->thresholdTablePtr_sl1[index + 1] - qdata_pt->thresholdTablePtr_sl1[index];
+
+ /* Based on the sign of the difference signal, either add or subtract the
+ * threshold table difference from the accumulated value. Recover the final
+ * accumulated value (saturated/rounded) */
+
+ if (diffSignal < 0)
+ threshDiff = -threshDiff;
+ tmp_reg64.s64 = ((int64_t)ditherVal * (int64_t)threshDiff);
+ tmp_reg64.s32.h += acc;
+ acc = tmp_reg64.s32.h;
+
+ if (tmp_reg64.u32.l >= 0x80000000)
+ acc++;
+ tmp_round0 = (tmp_reg64.u32.l >> 1) | (tmp_reg64.s32.h << 31);
+
+ acc = ssat24(acc);
+
+ if (tmp_round0 == 0x40000000L)
+ acc--;
+ _delta = -delta << 8;
+
+ acc = acc << 4;
+
+ /* Form (absDiffSignal * 0.125) - (acc * delta), which is the final distance
+ * signal used to determine if dithering alters the quantised code value or
+ * not. */
+ // worst case value for delta is 0x7d400
+
+ tmp_reg64.s64 = ((int64_t)acc * (int64_t)_delta);
+ tmp_reg64.s32.h += absDiffSignal;
+ tmp_round0 = (tmp_reg64.u32.l >> 4) | (tmp_reg64.s32.h << 28);
+ acc = tmp_reg64.s32.h + (1 << 2);
+ acc >>= 3;
+ if (tmp_round0 == 0x40000000L)
+ {
+ acc--;
+ }
+
+ tmp_qCode = qdata_pt->qCode;
+ tmp_altQcode = tmp_qCode - 1;
+ /* Check the sign of the distance penalty. Get the sign from the
+ * full-precision accumulator, as done in the Kalimba code. */
+
+ if (tmp_reg64.s32.h < 0)
+ {
+ /* The distance is -ve. The optimum code needs decremented by 1 and the
+ * alternative code is 1 greater than this. Get the rounded version of the
+ * -ve distance penalty and negate this (form distance magnitude) before
+ * writing the value out */
+ tmp_qCode = tmp_altQcode;
+ tmp_altQcode++;
+ acc = -acc;
+ }
+
+ qdata_pt->distPenalty = acc;
+ /* If the difference signal is negative, bitwise invert the code (restores
+ * sign to the magnitude). */
+ if (diffSignal < 0)
+ {
+ tmp_qCode = ~tmp_qCode;
+ tmp_altQcode = ~tmp_altQcode;
+ }
+ qdata_pt->altQcode = tmp_altQcode;
+ qdata_pt->qCode = tmp_qCode;
+}
+
+
+int32_t BsearchLH(const int32_t absDiffSignalShifted, const int32_t delta, const int32_t* dqbitTablePrt)
+{
+ int32_t qCode;
+ reg64_t tmp_acc;
+ int32_t tmp;
+ int32_t lc_delta = delta << 8;
+
+ qCode = 0;
+
+ tmp_acc.s64 = (int64_t)lc_delta * (int64_t)dqbitTablePrt[16];
+ tmp_acc.s32.h -= absDiffSignalShifted;
+ tmp = tmp_acc.s32.h | (tmp_acc.u32.l >> 1);
+ if (tmp <= 0) qCode = 16;
+
+ tmp_acc.s64 = (int64_t)lc_delta * (int64_t)dqbitTablePrt[qCode + 8];
+ tmp_acc.s32.h -= absDiffSignalShifted;
+ tmp = tmp_acc.s32.h | (tmp_acc.u32.l >> 1);
+ if (tmp <= 0) qCode += 8;
+
+ tmp_acc.s64 = (int64_t)lc_delta * (int64_t)dqbitTablePrt[qCode + 4];
+ tmp_acc.s32.h -= absDiffSignalShifted;
+ tmp = tmp_acc.s32.h | (tmp_acc.u32.l >> 1);
+ if (tmp <= 0) qCode += 4;
+
+ tmp_acc.s64 = (int64_t)lc_delta * (int64_t)dqbitTablePrt[qCode + 2];
+ tmp_acc.s32.h -= absDiffSignalShifted;
+ tmp = tmp_acc.s32.h | (tmp_acc.u32.l >> 1);
+ if (tmp <= 0) qCode += 2;
+
+ tmp_acc.s64 = (int64_t)lc_delta * (int64_t)dqbitTablePrt[qCode + 1];
+ tmp_acc.s32.h -= absDiffSignalShifted;
+ tmp = tmp_acc.s32.h | (tmp_acc.u32.l >> 1);
+ if (tmp <= 0) qCode++;
+
+ return (qCode);
+}
+
+
+void quantiseDifference_HDLH(const int32_t diffSignal, const int32_t ditherVal, const int32_t delta, Quantiser_data* qdata_pt)
+{
+ int32_t absDiffSignal;
+ int32_t absDiffSignalShifted;
+ int32_t index;
+ int32_t dithSquared;
+ int32_t minusLambdaD;
+ int32_t acc;
+ int32_t threshDiff;
+ reg64_t tmp_acc;
+ reg64_t tmp_reg64;
+ int32_t tmp_accL;
+ int32_t tmp_qCode;
+ int32_t tmp_altQcode;
+
+ uint32_t tmp_round0;
+ int32_t _delta;
+
+ /* Form the absolute value of the difference signal and maintain a version
+ * that is right-shifted 4 places for delta scaling. */
+ absDiffSignal = -diffSignal;
+ if (diffSignal >= 0) absDiffSignal = diffSignal;
+ absDiffSignal = ssat24(absDiffSignal);
+ absDiffSignalShifted = absDiffSignal >> deltaScale;
+
+ /* Binary search for the quantised code. This search terminates with the
+ * table index of the LARGEST threshold table value for which
+ * absDiffSignalShifted >= (delta * threshold)
+ */
+
+ /* first iteration */
+ index = BsearchLH(absDiffSignalShifted, delta, qdata_pt->thresholdTablePtr_sl1);
+
+ /* We actually wanted the SMALLEST magnitude quantised code for which
+ * absDiffSignalShifted < (delta * threshold)
+ * i.e. the code with the next highest magnitude than the one we actually
+ * found. We could add +1 to the code magnitude to do this, but we need to
+ * subtract 1 from the code magnitude to compensate for the "phantom
+ * element" at the base of the quantisation table. These two effects cancel
+ * out, so we leave the value of code alone. However, we need to form code+1
+ * to get the proper index into the both the threshold and dither tables,
+ * since we must skip over the phantom element at the base. */
+ qdata_pt->qCode = index;
+
+ /* Square the dither and get the value back from the ALU
+ * (saturated/rounded). */
+
+ tmp_reg64.s64 = ((int64_t)ditherVal * (int64_t)ditherVal);
+
+ acc = tmp_reg64.s32.h;
+
+ tmp_round0 = (uint32_t)acc << 8;
+
+ acc = (acc >> 6) + 1;
+ acc >>= 1;
+ if (tmp_round0 == 0x40000000L)
+ acc--;
+ acc = ssat24(acc);
+
+ dithSquared = acc;
+
+ /* Form the negative difference of the dither values at index and index-1.
+ * Load the accumulator with this value divided by 2. Ensure saturation is
+ * applied to the difference calculation. */
+
+ minusLambdaD = qdata_pt->minusLambdaDTable[index];
+
+ tmp_accL = (1 << 23) - (int32_t)dithSquared;
+ tmp_acc.s64 = (int64_t)tmp_accL * minusLambdaD;
+
+ tmp_round0 = (int32_t)tmp_acc.s32.l << 8;
+
+ acc = (tmp_acc.u32.l >> 22) | (tmp_acc.s32.h << 10);
+ if (tmp_round0 == 0x40000000L)
+ acc -= 2;
+ acc++;
+
+ /* Add the threshold table values at index and index-1 to the accumulated
+ * value. */
+
+ acc += qdata_pt->thresholdTablePtr_sl1[index + 1];
+ //// worst case value for acc = 0x000d3e08 + 0x43E1DB = 511FE3
+ acc += qdata_pt->thresholdTablePtr_sl1[index];
+ acc >>= 1;
+
+ // saturation required
+ acc = ssat24(acc);
+
+ /* Form the threshold table difference at index and index-1. Ensure
+ * saturation is applied to the difference calculation. */
+ threshDiff = qdata_pt->thresholdTablePtr_sl1[index + 1] - qdata_pt->thresholdTablePtr_sl1[index];
+
+ /* Based on the sign of the difference signal, either add or subtract the
+ * threshold table difference from the accumulated value. Recover the final
+ * accumulated value (saturated/rounded) */
+
+ if (diffSignal < 0)
+ threshDiff = -threshDiff;
+ tmp_reg64.s64 = ((int64_t)ditherVal * (int64_t)threshDiff);
+
+ tmp_reg64.s32.h += acc;
+ acc = tmp_reg64.s32.h;
+
+ if (tmp_reg64.u32.l >= 0x80000000)
+ acc++;
+ tmp_round0 = (tmp_reg64.u32.l >> 1) | (tmp_reg64.s32.h << 31);
+
+ acc = ssat24(acc);
+
+ if (tmp_round0 == 0x40000000L)
+ acc--;
+ _delta = -delta << 8;
+
+ acc = acc << 4;
+
+ /* Form (absDiffSignal * 0.125) - (acc * delta), which is the final distance
+ * signal used to determine if dithering alters the quantised code value or
+ * not. */
+ // worst case value for delta is 0x7d400
+
+ tmp_reg64.s64 = ((int64_t)acc * (int64_t)_delta);
+ tmp_reg64.s32.h += absDiffSignal;
+ tmp_round0 = (tmp_reg64.u32.l >> 4) | (tmp_reg64.s32.h << 28);
+ acc = tmp_reg64.s32.h + (1 << 2);
+ acc >>= 3;
+ if (tmp_round0 == 0x40000000L)
+ {
+ acc--;
+ }
+
+ tmp_qCode = qdata_pt->qCode;
+ tmp_altQcode = tmp_qCode - 1;
+ /* Check the sign of the distance penalty. Get the sign from the
+ * full-precision accumulator, as done in the Kalimba code. */
+
+ if (tmp_reg64.s32.h < 0)
+ {
+ /* The distance is -ve. The optimum code needs decremented by 1 and the
+ * alternative code is 1 greater than this. Get the rounded version of the
+ * -ve distance penalty and negate this (form distance magnitude) before
+ * writing the value out */
+ tmp_qCode = tmp_altQcode;
+ tmp_altQcode++;
+ acc = -acc;
+ }
+
+ qdata_pt->distPenalty = acc;
+ /* If the difference signal is negative, bitwise invert the code (restores
+ * sign to the magnitude). */
+ if (diffSignal < 0)
+ {
+ tmp_qCode = ~tmp_qCode;
+ tmp_altQcode = ~tmp_altQcode;
+ }
+ qdata_pt->altQcode = tmp_altQcode;
+ qdata_pt->qCode = tmp_qCode;
+}
diff --git a/system/embdrv/encoder_for_aptxhd/src/Quantiser.h b/system/embdrv/encoder_for_aptxhd/src/Quantiser.h
new file mode 100644
index 00000000000..e08fec47683
--- /dev/null
+++ b/system/embdrv/encoder_for_aptxhd/src/Quantiser.h
@@ -0,0 +1,27 @@
+/*------------------------------------------------------------------------------
+ *
+ * Function to calculate a quantised representation of an input
+ * difference signal, based on additional dither values and step-size inputs.
+ *
+ *-----------------------------------------------------------------------------*/
+
+#ifndef QUANTISER_H
+#define QUANTISER_H
+#ifdef _GCC
+ #pragma GCC visibility push(hidden)
+#endif
+
+
+#include "AptxParameters.h"
+
+
+void quantiseDifference_HDLL(const int32_t diffSignal, const int32_t ditherVal, const int32_t delta, Quantiser_data* qdata_pt);
+void quantiseDifference_HDHL(const int32_t diffSignal, const int32_t ditherVal, const int32_t delta, Quantiser_data* qdata_pt);
+void quantiseDifference_HDLH(const int32_t diffSignal, const int32_t ditherVal, const int32_t delta, Quantiser_data* qdata_pt);
+void quantiseDifference_HDHH(const int32_t diffSignal, const int32_t ditherVal, const int32_t delta, Quantiser_data* qdata_p);
+
+
+#ifdef _GCC
+ #pragma GCC visibility pop
+#endif
+#endif //QUANTISER_H
diff --git a/system/embdrv/encoder_for_aptxhd/src/SubbandFunctions.h b/system/embdrv/encoder_for_aptxhd/src/SubbandFunctions.h
new file mode 100644
index 00000000000..84cd2dd42dd
--- /dev/null
+++ b/system/embdrv/encoder_for_aptxhd/src/SubbandFunctions.h
@@ -0,0 +1,183 @@
+/*------------------------------------------------------------------------------
+ *
+ * Subband processing consists of:
+ * inverse quantisation (defined in a separate file),
+ * predictor coefficient update (Pole and Zero Coeff update),
+ * predictor filtering.
+ *
+ *----------------------------------------------------------------------------*/
+
+#ifndef SUBBANDFUNCTIONS_H
+#define SUBBANDFUNCTIONS_H
+#ifdef _GCC
+ #pragma GCC visibility push(hidden)
+#endif
+
+
+#include "AptxParameters.h"
+
+
+XBT_INLINE_ void updatePredictorPoleCoefficients(const int32_t invQ, const int32_t prevZfiltOutput, PoleCoeff_data* PoleCoeffDataPt)
+{
+ int32_t adaptSum;
+ int32_t sgnP[3];
+ int32_t newCoeffs[2];
+ int32_t Bacc;
+ int32_t acc;
+ int32_t acc2;
+ int32_t tmp3_round0;
+ int16_t tmp2_round0;
+ int16_t tmp_round0;
+ /* Various constants in various Q formats */
+ const int32_t oneQ22 = 4194304L;
+ const int32_t minusOneQ22 = -4194304L;
+ const int32_t pointFiveQ21 = 1048576L;
+ const int32_t minusPointFiveQ21 = -1048576L;
+ const int32_t pointSevenFiveQ22 = 3145728L;
+ const int32_t minusPointSevenFiveQ22 = -3145728L;
+ const int32_t oneMinusTwoPowerMinusFourQ22 = 3932160L;
+
+ /* Symbolic indicies for the pole coefficient arrays. Here we are using a1
+ * to represent the first pole filter coefficient and a2 the second. This
+ * seems to be common ADPCM terminology. */
+ enum { a1 = 0, a2 = 1 };
+
+ /* Symbolic indicies for the sgn array (k, k-1 and k-2 respectively) */
+ enum { k = 0, k_1 = 1, k_2 = 2 };
+
+ /* Form the sum of the inverse quantiser and previous zero filter values */
+ adaptSum = invQ + prevZfiltOutput;
+ adaptSum = ssat24(adaptSum);
+
+ /* Form the sgn of the sum just formed (note +1 and -1 are Q22) */
+ if (adaptSum < 0L)
+ {
+ sgnP[k] = minusOneQ22;
+ sgnP[k_1] = -(((int32_t)PoleCoeffDataPt->m_poleAdaptDelayLine.s16.l) << 22);
+ sgnP[k_2] = -(((int32_t)PoleCoeffDataPt->m_poleAdaptDelayLine.s16.h) << 22);
+ PoleCoeffDataPt->m_poleAdaptDelayLine.s16.h = PoleCoeffDataPt->m_poleAdaptDelayLine.s16.l;
+ PoleCoeffDataPt->m_poleAdaptDelayLine.s16.l = -1;
+ }
+ if (adaptSum == 0L)
+ {
+ sgnP[k] = 0L;
+ sgnP[k_1] = 0L;
+ sgnP[k_2] = 0L;
+ PoleCoeffDataPt->m_poleAdaptDelayLine.s16.h = PoleCoeffDataPt->m_poleAdaptDelayLine.s16.l;
+ PoleCoeffDataPt->m_poleAdaptDelayLine.s16.l = 1;
+ }
+ if (adaptSum > 0L)
+ {
+ sgnP[k] = oneQ22;
+ sgnP[k_1] = ((int32_t)PoleCoeffDataPt->m_poleAdaptDelayLine.s16.l) << 22;
+ sgnP[k_2] = ((int32_t)PoleCoeffDataPt->m_poleAdaptDelayLine.s16.h) << 22;
+ PoleCoeffDataPt->m_poleAdaptDelayLine.s16.h = PoleCoeffDataPt->m_poleAdaptDelayLine.s16.l;
+ PoleCoeffDataPt->m_poleAdaptDelayLine.s16.l = 1;
+ }
+
+ /* Clear the accumulator and form -a1(k) * sgn(p(k))sgn(p(k-1)) in Q21. Clip
+ * it to +/- 0.5 (Q21) so that we can take f(a1) = 4 * a1. This is a partial
+ * result for the new a2 */
+ acc = 0;
+ acc -= PoleCoeffDataPt->m_poleCoeff[a1] * (sgnP[k_1] >> 22);
+
+ tmp3_round0 = acc & 0x3L;
+
+ acc += 0x001;
+ acc >>= 1;
+ if (tmp3_round0 == 0x001L)
+ {
+ acc--;
+ }
+
+ newCoeffs[a2] = acc;
+
+ if (newCoeffs[a2] < minusPointFiveQ21)
+ {
+ newCoeffs[a2] = minusPointFiveQ21;
+ }
+ if (newCoeffs[a2] > pointFiveQ21)
+ {
+ newCoeffs[a2] = pointFiveQ21;
+ }
+
+ /* Load the accumulator with sgn(p(k))sgn(p(k-2)) right-shifted by 3. The
+ * 3-position shift is to multiply it by 0.25 and convert from Q22 to Q21. */
+ Bacc = (sgnP[k_2] >> 3);
+ /* Add the current a2 update value to the accumulator (Q21) */
+ Bacc += newCoeffs[a2];
+ /* Shift the accumulator right by 4 positions.
+ * Right 7 places to multiply by 2^(-7)
+ * Left 2 places to scale by 4 (0.25A + B -> A + 4B)
+ * Left 1 place to convert from Q21 to Q22 */
+ Bacc >>= 4;
+ /* Add a2(k-1) * (1 - 2^(-7)) to the accumulator. Note that the constant is
+ * expressed as Q23, hence the product is Q22. Get the accumulator value
+ * back out. */
+ acc2 = PoleCoeffDataPt->m_poleCoeff[a2] << 8;
+ acc2 -= PoleCoeffDataPt->m_poleCoeff[a2] << 1;
+ Bacc <<= 8;
+ Bacc += acc2;
+
+ tmp2_round0 = (int16_t)Bacc & 0x01FFL;
+
+ Bacc += 0x0080L;
+ Bacc >>= 8;
+
+ if (tmp2_round0 == 0x0080L)
+ {
+ Bacc--;
+ }
+
+ newCoeffs[a2] = Bacc;
+
+ /* Clip the new a2(k) value to +/- 0.75 (Q22) */
+ if (newCoeffs[a2] < minusPointSevenFiveQ22)
+ {
+ newCoeffs[a2] = minusPointSevenFiveQ22;
+ }
+ if (newCoeffs[a2] > pointSevenFiveQ22)
+ {
+ newCoeffs[a2] = pointSevenFiveQ22;
+ }
+ PoleCoeffDataPt->m_poleCoeff[a2] = newCoeffs[a2];
+
+ /* Form sgn(p(k))sgn(p(k-1)) * (3 * 2^(-8)). The constant is Q23, hence the
+ * product is Q22. */
+ /* Add a1(k-1) * (1 - 2^(-8)) to the accumulator. The constant is Q23, hence
+ * the product is Q22. Get the value from the accumulator. */
+ acc2 = PoleCoeffDataPt->m_poleCoeff[a1] << 8;
+ acc2 -= PoleCoeffDataPt->m_poleCoeff[a1];
+ acc2 += (sgnP[k_1] << 2);
+ acc2 -= (sgnP[k_1]);
+
+ tmp_round0 = (int16_t)acc2 & 0x01FF;
+
+ acc2 += 0x0080;
+ acc = (acc2 >> 8);
+ if (tmp_round0 == 0x0080)
+ {
+ acc--;
+ }
+
+ newCoeffs[a1] = (int32_t)acc;
+
+ /* Clip the new value of a1(k) to +/- (1 - 2^4 - a2(k)). The constant 1 -
+ * 2^4 is expressed in Q22 format (as is a1 and a2) */
+ if (newCoeffs[a1] < (newCoeffs[a2] - oneMinusTwoPowerMinusFourQ22))
+ {
+ newCoeffs[a1] = newCoeffs[a2] - oneMinusTwoPowerMinusFourQ22;
+ }
+ if (newCoeffs[a1] > (oneMinusTwoPowerMinusFourQ22 - newCoeffs[a2]))
+ {
+ newCoeffs[a1] = oneMinusTwoPowerMinusFourQ22 - newCoeffs[a2];
+ }
+
+ PoleCoeffDataPt->m_poleCoeff[a1] = newCoeffs[a1];
+}
+
+
+#ifdef _GCC
+ #pragma GCC visibility pop
+#endif
+#endif //SUBBANDFUNCTIONS_H
diff --git a/system/embdrv/encoder_for_aptxhd/src/SubbandFunctionsCommon.h b/system/embdrv/encoder_for_aptxhd/src/SubbandFunctionsCommon.h
new file mode 100644
index 00000000000..618d4399197
--- /dev/null
+++ b/system/embdrv/encoder_for_aptxhd/src/SubbandFunctionsCommon.h
@@ -0,0 +1,520 @@
+/*------------------------------------------------------------------------------
+ *
+ * Subband processing consists of:
+ * inverse quantisation (defined in a separate file),
+ * predictor coefficient update (Pole and Zero Coeff update),
+ * predictor filtering.
+ *
+ *----------------------------------------------------------------------------*/
+
+#ifndef SUBBANDFUNCTIONSCOMMON_H
+#define SUBBANDFUNCTIONSCOMMON_H
+
+
+enum reg64_reg
+{
+ reg64_H = 1,
+ reg64_L = 0
+};
+
+void processSubband_HD(const int32_t qCode, const int32_t ditherVal, Subband_data* SubbandDataPt, IQuantiser_data* iqDataPt);
+void processSubband_HDLL(const int32_t qCode, const int32_t ditherVal, Subband_data* SubbandDataPt, IQuantiser_data* iqDataPt);
+void processSubband_HDHL(const int32_t qCode, const int32_t ditherVal, Subband_data* SubbandDataPt, IQuantiser_data* iqDataPt);
+
+/* Function to carry out inverse quantisation for a subband */
+XBT_INLINE_ void invertQuantisation(const int32_t qCode, const int32_t ditherVal, IQuantiser_data* iqdata_pt)
+{
+ int32_t invQ;
+ int32_t index;
+ int32_t acc;
+ reg64_t tmp_r64;
+ int64_t tmp_acc;
+ int32_t tmp_accL;
+ int32_t tmp_accH;
+ uint32_t tmp_round0;
+ uint32_t tmp_round1;
+ unsigned u16t;
+ /* log delta leak value (Q23) */
+ const uint32_t logDeltaLeakVal = 0x7F6CL;
+
+ /* Turn the quantised code back into an index into the threshold table. This
+ * involves bitwise inversion of the code (if -ve) and adding 1 (phantom
+ * element at table base). Then set invQ to be +/- the threshold value,
+ * depending on the code sign. */
+ index = qCode;
+ if (qCode < 0)
+ index = (~index);
+ index = index + 1;
+ invQ = iqdata_pt->thresholdTablePtr_sl1[index];
+ if (qCode < 0)
+ invQ = -invQ;
+
+ /* Load invQ into the accumulator. Add the product of the dither value times
+ * the indexed dither table value. Then get the result back from the
+ * accumulator as an updated invQ. */
+ tmp_r64.s64 = ((int64_t)ditherVal * iqdata_pt->ditherTablePtr_sf1[index]);
+ tmp_r64.s32.h += invQ >> 1;
+
+ acc = tmp_r64.s32.h;
+
+ tmp_round1 = tmp_r64.s32.h & 0x00000001L;
+ if (tmp_r64.u32.l >= 0x80000000)
+ acc++;
+ if (tmp_round1 == 0 && tmp_r64.s32.l == (int32_t)0x80000000L)
+ acc--;
+ acc = ssat24(acc);
+
+ invQ = (int32_t)acc;
+
+ /* Scale invQ by the current delta value. Left-shift the result (in the
+ * accumulator) by 4 positions for the delta scaling. Get the updated invQ
+ * back from the accumulator. */
+ u16t = iqdata_pt->logDelta;
+ tmp_acc = ((int64_t)invQ * iqdata_pt->delta);
+ tmp_accL = u16t * logDeltaLeakVal;
+ tmp_accH = iqdata_pt->incrTablePtr[index];
+ acc = (int32_t)(tmp_acc >> (23 - deltaScale));
+ invQ = ssat24(acc);
+
+ /* Now update the value of logDelta. Load the accumulator with the index
+ * value of the logDelta increment table. Add the product of the current
+ * logDelta scaled by a leaky coefficient (16310 in Q14). Get the value back
+ * from the accumulator. */
+ tmp_accH += tmp_accL >> (32 - 17);
+
+ acc = tmp_accH;
+
+ tmp_r64.u32.l = ((uint32_t)tmp_accL << 17);
+ tmp_r64.s32.h = tmp_accH;
+
+ tmp_round0 = tmp_r64.u32.l;
+ tmp_round1 = (int32_t)(tmp_r64.u64 >> 1);
+ if (tmp_round0 >= 0x80000000L)
+ acc++;
+ if (tmp_round1 == 0x40000000L)
+ acc--;
+
+ /* Limit the updated logDelta between 0 and its subband-specific maximum. */
+ if (acc < 0)
+ acc = 0;
+ if (acc > iqdata_pt->maxLogDelta)
+ acc = iqdata_pt->maxLogDelta;
+
+ iqdata_pt->logDelta = (uint16_t)acc;
+
+ /* The updated value of delta is the logTable output (indexed by 5 bits from
+ * the updated logDelta) shifted by a value involving the logDelta minimum
+ * and the updated logDelta itself. */
+ iqdata_pt->delta = iqdata_pt->iquantTableLogPtr[(acc >> 3) & 0x1f] >>
+ (22 - 25 - iqdata_pt->minLogDelta - (acc >> 8));
+
+ iqdata_pt->invQ = invQ;
+}
+
+XBT_INLINE_ void invertQuantisationHL(const int32_t qCode, const int32_t ditherVal, IQuantiser_data* iqdata_pt)
+{
+ int32_t invQ;
+ int32_t index;
+ int32_t acc;
+ reg64_t tmp_r64;
+ int64_t tmp_acc;
+ int32_t tmp_accL;
+ int32_t tmp_accH;
+ uint32_t tmp_round0;
+ uint32_t tmp_round1;
+ unsigned u16t;
+ /* log delta leak value (Q23) */
+ const uint32_t logDeltaLeakVal = 0x7F6CL;
+
+ /* Turn the quantised code back into an index into the threshold table. This
+ * involves bitwise inversion of the code (if -ve) and adding 1 (phantom
+ * element at table base). Then set invQ to be +/- the threshold value,
+ * depending on the code sign. */
+ index = qCode;
+ if (qCode < 0)
+ index = (~index);
+ index = index + 1;
+ invQ = iqdata_pt->thresholdTablePtr_sl1[index];
+ if (qCode < 0)
+ invQ = -invQ;
+
+ /* Load invQ into the accumulator. Add the product of the dither value times
+ * the indexed dither table value. Then get the result back from the
+ * accumulator as an updated invQ. */
+ tmp_r64.s64 = ((int64_t)ditherVal * iqdata_pt->ditherTablePtr_sf1[index]);
+ tmp_r64.s32.h += invQ >> 1;
+
+ acc = tmp_r64.s32.h;
+
+ tmp_round1 = tmp_r64.s32.h & 0x00000001L;
+ if (tmp_r64.u32.l >= 0x80000000)
+ acc++;
+ if (tmp_round1 == 0 && tmp_r64.u32.l == 0x80000000L)
+ acc--;
+ acc = ssat24(acc);
+
+ invQ = (int32_t)acc;
+
+ /* Scale invQ by the current delta value. Left-shift the result (in the
+ * accumulator) by 4 positions for the delta scaling. Get the updated invQ
+ * back from the accumulator. */
+ u16t = iqdata_pt->logDelta;
+ tmp_acc = ((int64_t)invQ * iqdata_pt->delta);
+ tmp_accL = u16t * logDeltaLeakVal;
+ tmp_accH = iqdata_pt->incrTablePtr[index];
+ acc = (int32_t)(tmp_acc >> (23 - deltaScale));
+ invQ = ssat24(acc);
+
+ /* Now update the value of logDelta. Load the accumulator with the index
+ * value of the logDelta increment table. Add the product of the current
+ * logDelta scaled by a leaky coefficient (16310 in Q14). Get the value back
+ * from the accumulator. */
+ tmp_accH += tmp_accL >> (32 - 17);
+
+ acc = tmp_accH;
+
+ tmp_r64.u32.l = ((uint32_t)tmp_accL << 17);
+ tmp_r64.s32.h = tmp_accH;
+
+ tmp_round0 = tmp_r64.u32.l;
+ tmp_round1 = (int32_t)(tmp_r64.u64 >> 1);
+ if (tmp_round0 >= 0x80000000L)
+ acc++;
+ if (tmp_round1 == 0x40000000L)
+ acc--;
+
+ /* Limit the updated logDelta between 0 and its subband-specific maximum. */
+ if (acc < 0)
+ acc = 0;
+ if (acc > iqdata_pt->maxLogDelta)
+ acc = iqdata_pt->maxLogDelta;
+
+ iqdata_pt->logDelta = (uint16_t)acc;
+
+ /* The updated value of delta is the logTable output (indexed by 5 bits from
+ * the updated logDelta) shifted by a value involving the logDelta minimum
+ * and the updated logDelta itself. */
+ iqdata_pt->delta = iqdata_pt->iquantTableLogPtr[(acc >> 3) & 0x1f] >>
+ (22 - 25 - iqdata_pt->minLogDelta - (acc >> 8));
+
+ iqdata_pt->invQ = invQ;
+}
+
+/* Function to carry out prediction ARMA filtering for the current subband
+ * performPredictionFiltering should only be used for HH and LH subband! */
+XBT_INLINE_ void performPredictionFiltering(const int32_t invQ, Subband_data* SubbandDataPt)
+{
+ int32_t poleVal;
+ int32_t acc;
+ int64_t accL;
+ uint32_t pointer;
+ int32_t poleDelayLine;
+ int32_t predVal;
+ int32_t* zeroCoeffPt = SubbandDataPt->m_ZeroCoeffData.m_zeroCoeff;
+ int32_t* poleCoeff = SubbandDataPt->m_PoleCoeffData.m_poleCoeff;
+ int32_t* cbuf_pt;
+ int32_t invQincr_pos;
+ int32_t invQincr_neg;
+ int32_t k;
+ int32_t oldZData;
+ /* Pole coefficient and data indicies */
+ enum { a1 = 0, a2 = 1 };
+
+ /* Write the newest pole input sample to the pole delay line.
+ * Ensure the sum of the current dequantised error and the previous
+ * predictor output is saturated if necessary. */
+ poleDelayLine = invQ + SubbandDataPt->m_predData.m_predVal;
+
+ poleDelayLine = ssat24(poleDelayLine);
+
+ /* Pole filter convolution. Shift convolution result 1 place to the left
+ * before retrieving it, since the pole coefficients are Q22 (data is Q23)
+ * and we want a Q23 result */
+ accL = ((int64_t)poleCoeff[a2] * (int64_t)SubbandDataPt->m_predData.m_poleDelayLine[a2]);
+ /* Update the pole delay line for the next pass by writing the new input
+ * sample into the 2nd element */
+ SubbandDataPt->m_predData.m_poleDelayLine[a2] = poleDelayLine;
+ accL += ((int64_t)poleCoeff[a1] * (int64_t)poleDelayLine);
+ poleVal = (int32_t)(accL >> 22);
+ poleVal = ssat24(poleVal);
+
+ /* Create (2^(-7)) * sgn(invQ) in Q22 format. */
+ if (invQ == 0)
+ {
+ invQincr_pos = 0L;
+ }
+ else
+ {
+ invQincr_pos = 0x800000;
+ }
+ if (invQ < 0L)
+ {
+ invQincr_pos = -invQincr_pos;
+ }
+
+ invQincr_neg = 0x0080 - invQincr_pos;
+ invQincr_pos += 0x0080;
+
+ pointer = (SubbandDataPt->m_predData.m_zeroDelayLine.pointer++) + 12;
+ cbuf_pt = &SubbandDataPt->m_predData.m_zeroDelayLine.buffer[pointer];
+ /* partial manual unrolling to improve performance */
+ if (SubbandDataPt->m_predData.m_zeroDelayLine.pointer >= 12)
+ SubbandDataPt->m_predData.m_zeroDelayLine.pointer = 0;
+
+ SubbandDataPt->m_predData.m_zeroDelayLine.modulo = invQ;
+
+ /* Iterate over the number of coefficients for this subband */
+ oldZData = invQ;
+ accL = 0;
+ for (k = 0; k < 12; k++)
+ {
+ uint32_t tmp_round0;
+ int32_t coeffValue;
+ int32_t zData0;
+
+ /* ------------------------------------------------------------------*/
+ zData0 = (*(cbuf_pt--));
+ coeffValue = *(zeroCoeffPt + k);
+ if (zData0 < 0L)
+ {
+ acc = invQincr_neg - coeffValue;
+ }
+ else
+ {
+ acc = invQincr_pos - coeffValue;
+ }
+ tmp_round0 = acc;
+ acc = (acc >> 8) + coeffValue;
+ if (((tmp_round0 << 23) ^ 0x80000000) == 0) acc--;
+ accL += (int64_t)acc * (int64_t)(oldZData);
+ oldZData = zData0;
+ *(zeroCoeffPt + k) = acc;
+ }
+
+ acc = (int32_t)(accL >> 22);
+ acc = ssat24(acc);
+
+ /* Predictor output is the sum of the pole and zero filter outputs. Ensure
+ * this is saturated, if necessary. */
+ predVal = acc + poleVal;
+ predVal = ssat24(predVal);
+ SubbandDataPt->m_predData.m_zeroVal = acc;
+ SubbandDataPt->m_predData.m_predVal = predVal;
+
+ /* Update the zero filter delay line by writing the new input sample to the
+ * circular buffer. */
+ SubbandDataPt->m_predData.m_zeroDelayLine.buffer[SubbandDataPt->m_predData.m_zeroDelayLine.pointer] = SubbandDataPt->m_predData.m_zeroDelayLine.modulo;
+ SubbandDataPt->m_predData.m_zeroDelayLine.buffer[SubbandDataPt->m_predData.m_zeroDelayLine.pointer + 12] = SubbandDataPt->m_predData.m_zeroDelayLine.modulo;
+}
+
+XBT_INLINE_ void performPredictionFilteringLL(const int32_t invQ, Subband_data* SubbandDataPt)
+{
+ int32_t poleVal;
+ int32_t acc;
+ int64_t accL;
+ uint32_t pointer;
+ int32_t poleDelayLine;
+ int32_t predVal;
+ int32_t* zeroCoeffPt = SubbandDataPt->m_ZeroCoeffData.m_zeroCoeff;
+ int32_t* poleCoeff = SubbandDataPt->m_PoleCoeffData.m_poleCoeff;
+ int32_t* cbuf_pt;
+ int32_t invQincr_pos;
+ int32_t invQincr_neg;
+ int32_t k;
+ int32_t oldZData;
+ /* Pole coefficient and data indicies */
+ enum { a1 = 0, a2 = 1 };
+
+ /* Write the newest pole input sample to the pole delay line.
+ * Ensure the sum of the current dequantised error and the previous
+ * predictor output is saturated if necessary. */
+ poleDelayLine = invQ + SubbandDataPt->m_predData.m_predVal;
+
+ poleDelayLine = ssat24(poleDelayLine);
+
+ /* Pole filter convolution. Shift convolution result 1 place to the left
+ * before retrieving it, since the pole coefficients are Q22 (data is Q23)
+ * and we want a Q23 result */
+ accL = ((int64_t)poleCoeff[a2] * (int64_t)SubbandDataPt->m_predData.m_poleDelayLine[a2]);
+ /* Update the pole delay line for the next pass by writing the new input
+ * sample into the 2nd element */
+ SubbandDataPt->m_predData.m_poleDelayLine[a2] = poleDelayLine;
+ accL += ((int64_t)poleCoeff[a1] * (int64_t)poleDelayLine);
+ poleVal = (int32_t)(accL >> 22);
+ poleVal = ssat24(poleVal);
+ /* store poleVal to free one register. */
+ SubbandDataPt->m_predData.m_predVal = poleVal;
+
+ /* Create (2^(-7)) * sgn(invQ) in Q22 format. */
+ if (invQ == 0)
+ {
+ invQincr_pos = 0L;
+ }
+ else
+ {
+ invQincr_pos = 0x800000;
+ }
+ if (invQ < 0L)
+ {
+ invQincr_pos = -invQincr_pos;
+ }
+
+ invQincr_neg = 0x0080 - invQincr_pos;
+ invQincr_pos += 0x0080;
+
+ pointer = (SubbandDataPt->m_predData.m_zeroDelayLine.pointer++) + 24;
+ cbuf_pt = &SubbandDataPt->m_predData.m_zeroDelayLine.buffer[pointer];
+ /* partial manual unrolling to improve performance */
+ if (SubbandDataPt->m_predData.m_zeroDelayLine.pointer >= 24)
+ SubbandDataPt->m_predData.m_zeroDelayLine.pointer = 0;
+
+ SubbandDataPt->m_predData.m_zeroDelayLine.modulo = invQ;
+
+ /* Iterate over the number of coefficients for this subband */
+ oldZData = invQ;
+ accL = 0;
+ for (k = 0; k < 24; k++)
+ {
+ int32_t zData0;
+ int32_t coeffValue;
+
+ zData0 = (*(cbuf_pt--));
+ coeffValue = *(zeroCoeffPt + k);
+ if (zData0 < 0L)
+ {
+ acc = invQincr_neg - coeffValue;
+ }
+ else
+ {
+ acc = invQincr_pos - coeffValue;
+ }
+ if (((acc << 23) ^ 0x80000000) == 0) coeffValue--;
+ acc = (acc >> 8) + coeffValue;
+ accL += (int64_t)acc * (int64_t)(oldZData);
+ oldZData = zData0;
+ *(zeroCoeffPt + k) = acc;
+ }
+
+ acc = (int32_t)(accL >> 22);
+ acc = ssat24(acc);
+
+ /* Predictor output is the sum of the pole and zero filter outputs. Ensure
+ * this is saturated, if necessary.
+ * recover value of PoleVal stored at beginning of routine... */
+ predVal = acc + SubbandDataPt->m_predData.m_predVal;
+ predVal = ssat24(predVal);
+ SubbandDataPt->m_predData.m_zeroVal = acc;
+ SubbandDataPt->m_predData.m_predVal = predVal;
+
+ /* Update the zero filter delay line by writing the new input sample to the
+ * circular buffer. */
+ SubbandDataPt->m_predData.m_zeroDelayLine.buffer[SubbandDataPt->m_predData.m_zeroDelayLine.pointer] = SubbandDataPt->m_predData.m_zeroDelayLine.modulo;
+ SubbandDataPt->m_predData.m_zeroDelayLine.buffer[SubbandDataPt->m_predData.m_zeroDelayLine.pointer + 24] = SubbandDataPt->m_predData.m_zeroDelayLine.modulo;
+}
+
+XBT_INLINE_ void performPredictionFilteringHL(const int32_t invQ, Subband_data* SubbandDataPt)
+{
+ int32_t poleVal;
+ int32_t acc;
+ int64_t accL;
+ uint32_t pointer;
+ int32_t poleDelayLine;
+ int32_t predVal;
+ int32_t* zeroCoeffPt = SubbandDataPt->m_ZeroCoeffData.m_zeroCoeff;
+ int32_t* poleCoeff = SubbandDataPt->m_PoleCoeffData.m_poleCoeff;
+ int32_t* cbuf_pt;
+ int32_t invQincr_pos;
+ int32_t invQincr_neg;
+ int32_t k;
+ int32_t oldZData;
+ const int32_t roundCte = 0x80000000;
+ /* Pole coefficient and data indicies */
+ enum { a1 = 0, a2 = 1 };
+
+ /* Write the newest pole input sample to the pole delay line.
+ * Ensure the sum of the current dequantised error and the previous
+ * predictor output is saturated if necessary. */
+ poleDelayLine = invQ + SubbandDataPt->m_predData.m_predVal;
+
+ poleDelayLine = ssat24(poleDelayLine);
+
+ /* Pole filter convolution. Shift convolution result 1 place to the left
+ * before retrieving it, since the pole coefficients are Q22 (data is Q23)
+ * and we want a Q23 result */
+ accL = ((int64_t)poleCoeff[a2] * (int64_t)SubbandDataPt->m_predData.m_poleDelayLine[a2]);
+ /* Update the pole delay line for the next pass by writing the new input
+ * sample into the 2nd element */
+ SubbandDataPt->m_predData.m_poleDelayLine[a2] = poleDelayLine;
+ accL += ((int64_t)poleCoeff[a1] * (int64_t)poleDelayLine);
+ poleVal = (int32_t)(accL >> 22);
+ poleVal = ssat24(poleVal);
+
+ /* Create (2^(-7)) * sgn(invQ) in Q22 format. */
+ invQincr_pos = 0L;
+ if (invQ != 0)
+ {
+ invQincr_pos = 0x800000;
+ }
+ if (invQ < 0L)
+ {
+ invQincr_pos = -invQincr_pos;
+ }
+
+ invQincr_neg = 0x0080 - invQincr_pos;
+ invQincr_pos += 0x0080;
+
+ pointer = (SubbandDataPt->m_predData.m_zeroDelayLine.pointer++) + 6;
+ cbuf_pt = &SubbandDataPt->m_predData.m_zeroDelayLine.buffer[pointer];
+ /* partial manual unrolling to improve performance */
+ if (SubbandDataPt->m_predData.m_zeroDelayLine.pointer >= 6)
+ SubbandDataPt->m_predData.m_zeroDelayLine.pointer = 0;
+
+ SubbandDataPt->m_predData.m_zeroDelayLine.modulo = invQ;
+
+ /* Iterate over the number of coefficients for this subband */
+ oldZData = invQ;
+ accL = 0;
+
+ for (k = 0; k < 6; k++)
+ {
+ uint32_t tmp_round0;
+ int32_t coeffValue;
+ int32_t zData0;
+
+ /* ------------------------------------------------------------------*/
+ zData0 = (*(cbuf_pt--));
+ coeffValue = *(zeroCoeffPt + k);
+ if (zData0 < 0L)
+ {
+ acc = invQincr_neg - coeffValue;
+ }
+ else
+ {
+ acc = invQincr_pos - coeffValue;
+ }
+ tmp_round0 = acc;
+ acc = (acc >> 8) + coeffValue;
+ if (((tmp_round0 << 23) ^ roundCte) == 0) acc--;
+ accL += (int64_t)acc * (int64_t)(oldZData);
+ oldZData = zData0;
+ *(zeroCoeffPt + k) = acc;
+ }
+
+ acc = (int32_t)(accL >> 22);
+ acc = ssat24(acc);
+
+ /* Predictor output is the sum of the pole and zero filter outputs. Ensure
+ * this is saturated, if necessary. */
+ predVal = acc + poleVal;
+ predVal = ssat24(predVal);
+ SubbandDataPt->m_predData.m_zeroVal = acc;
+ SubbandDataPt->m_predData.m_predVal = predVal;
+
+ /* Update the zero filter delay line by writing the new input sample to the
+ * circular buffer. */
+ SubbandDataPt->m_predData.m_zeroDelayLine.buffer[SubbandDataPt->m_predData.m_zeroDelayLine.pointer] = SubbandDataPt->m_predData.m_zeroDelayLine.modulo;
+ SubbandDataPt->m_predData.m_zeroDelayLine.buffer[SubbandDataPt->m_predData.m_zeroDelayLine.pointer + 6] = SubbandDataPt->m_predData.m_zeroDelayLine.modulo;
+}
+
+
+#endif //SUBBANDFUNCTIONSCOMMON_H
diff --git a/system/embdrv/encoder_for_aptxhd/src/SyncInserter.h b/system/embdrv/encoder_for_aptxhd/src/SyncInserter.h
new file mode 100644
index 00000000000..f393a0d223e
--- /dev/null
+++ b/system/embdrv/encoder_for_aptxhd/src/SyncInserter.h
@@ -0,0 +1,114 @@
+/*------------------------------------------------------------------------------
+ *
+ * All declarations relevant for the SyncInserter class. This class exposes a
+ * public interface that lets a client supply two aptX HD encoder objects (left
+ * and right stereo channel) and have the current quantised codes adjusted to
+ * bury an autosync bit.
+ *
+ *----------------------------------------------------------------------------*/
+#ifndef SYNCINSERTER_H
+#define SYNCINSERTER_H
+#ifdef _GCC
+ #pragma GCC visibility push(hidden)
+#endif
+
+
+#include "AptxParameters.h"
+
+
+/* Function to insert sync information into one of the 8
+ * quantised codes spread across 2 aptX HD codewords (1 codeword
+ * per channel) */
+XBT_INLINE_ void xbtEncinsertSync(Encoder_data* leftChannelEncoder, Encoder_data* rightChannelEncoder, uint32_t* syncWordPhase)
+{
+ /* Currently using 0x1 as the 8-bit sync pattern */
+ static const uint32_t syncWord = 0x1;
+ uint32_t tmp_var;
+
+ uint32_t i;
+
+ /* Variable to hold the XOR of all the quantised code lsbs */
+ uint32_t xorCodeLsbs;
+
+ /* Variable to point to the quantiser with the minimum calculated distance
+ * penalty. */
+ Quantiser_data* minPenaltyQuantiser;
+
+ /* Get the vector of quantiser pointers from the left and right encoders */
+ Quantiser_data* leftQuant[4];
+ Quantiser_data* rightQuant[4];
+ leftQuant[0] = &leftChannelEncoder->m_qdata[0];
+ leftQuant[1] = &leftChannelEncoder->m_qdata[1];
+ leftQuant[2] = &leftChannelEncoder->m_qdata[2];
+ leftQuant[3] = &leftChannelEncoder->m_qdata[3];
+ rightQuant[0] = &rightChannelEncoder->m_qdata[0];
+ rightQuant[1] = &rightChannelEncoder->m_qdata[1];
+ rightQuant[2] = &rightChannelEncoder->m_qdata[2];
+ rightQuant[3] = &rightChannelEncoder->m_qdata[3];
+
+ /* Starting quantiser traversal with the LL quantiser from the left channel.
+ * Initialise the pointer to the minimum penalty quantiser with the details
+ * of the left LL quantiser. Initialise the code lsbs XOR variable with the
+ * left LL quantised code lsbs and also XOR in the left and right random
+ * dither bit generated by the 2 encoders. */
+ xorCodeLsbs =
+ ((rightQuant[LL]->qCode) & 0x1) ^
+ leftChannelEncoder->m_dithSyncRandBit ^
+ rightChannelEncoder->m_dithSyncRandBit;
+ minPenaltyQuantiser = rightQuant[LH];
+
+ /* Traverse across the LH, HL and HH quantisers from the right channel */
+ for (i = LH; i <= HH; i++)
+ {
+ /* XOR in the lsb of the quantised code currently examined */
+ xorCodeLsbs ^= (rightQuant[i]->qCode) & 0x1;
+ }
+
+ /* If the distance penalty associated with a quantiser is less than the
+ * current minimum, then make that quantiser the minimum penalty
+ * quantiser. */
+ if (rightQuant[HL]->distPenalty < minPenaltyQuantiser->distPenalty)
+ minPenaltyQuantiser = rightQuant[HL];
+ if (rightQuant[LL]->distPenalty < minPenaltyQuantiser->distPenalty)
+ minPenaltyQuantiser = rightQuant[LL];
+ if (rightQuant[HH]->distPenalty < minPenaltyQuantiser->distPenalty)
+ minPenaltyQuantiser = rightQuant[HH];
+
+ /* Traverse across all quantisers from the left channel */
+ for (i = LL; i <= HH; i++)
+ {
+ /* XOR in the lsb of the quantised code currently examined */
+ xorCodeLsbs ^= (leftQuant[i]->qCode) & 0x1;
+ }
+
+ /* If the distance penalty associated with a quantiser is less than the
+ * current minimum, then make that quantiser the minimum penalty
+ * quantiser. */
+ if (leftQuant[LH]->distPenalty < minPenaltyQuantiser->distPenalty)
+ minPenaltyQuantiser = leftQuant[LH];
+ if (leftQuant[HL]->distPenalty < minPenaltyQuantiser->distPenalty)
+ minPenaltyQuantiser = leftQuant[HL];
+ if (leftQuant[LL]->distPenalty < minPenaltyQuantiser->distPenalty)
+ minPenaltyQuantiser = leftQuant[LL];
+ if (leftQuant[HH]->distPenalty < minPenaltyQuantiser->distPenalty)
+ minPenaltyQuantiser = leftQuant[HH];
+
+ /* If the lsbs of all 8 quantised codes don't happen to equal the desired
+ * sync bit to embed, then force them to be by replacing the optimum code
+ * with the alternate code in the minimum penalty quantiser (changes the lsb
+ * of the code in this quantiser) */
+ if (xorCodeLsbs != ((syncWord >> (*syncWordPhase)) & 0x1))
+ {
+ minPenaltyQuantiser->qCode = minPenaltyQuantiser->altQcode;
+ }
+
+ /* Decrement the selected sync word bit modulo 8 for the next pass. */
+ tmp_var = --(*syncWordPhase);
+ (*syncWordPhase) = tmp_var & 0x7;
+}
+
+
+#ifdef _GCC
+ #pragma GCC visibility pop
+#endif
+#endif //SYNCINSERTER_H
diff --git a/system/embdrv/encoder_for_aptxhd/src/aptXHDbtenc.c b/system/embdrv/encoder_for_aptxhd/src/aptXHDbtenc.c
new file mode 100644
index 00000000000..9eac1a378e3
--- /dev/null
+++ b/system/embdrv/encoder_for_aptxhd/src/aptXHDbtenc.c
@@ -0,0 +1,179 @@
+#include "AptxParameters.h"
+#include "AptxTables.h"
+#include "aptXHDbtenc.h"
+#include "AptxEncoder.h"
+#include "SyncInserter.h"
+#include "CodewordPacker.h"
+#include "swversion.h"
+
+
+typedef struct aptxhdbtenc_t
+{
+ /* m_endian should either be 0 (little endian) or 8 (big endian). */
+ int32_t m_endian;
+
+ /* Autosync inserter & Checker for use with the stereo aptX HD codec. */
+ /* The current phase of the sync word insertion (7 down to 0) */
+ uint32_t m_syncWordPhase;
+
+ /* Stereo channel aptX HD encoder (annotated to produce Kalimba test vectors
+ * for it's I/O. This will process valid PCM from a WAV file). */
+ /* Each Encoder_data structure requires 1592 bytes */
+ Encoder_data m_encoderData[2];
+ Qmf_storage m_qmf_l;
+ Qmf_storage m_qmf_r;
+} aptxhdbtenc;
+
+/* Constants */
+/* Log to linear lookup table used in inverse quantiser*/
+/* Size of Table: 32*4 = 128 bytes */
+static const int32_t IQuant_tableLogT[32] =
+{ 16384 * 256, 16744 * 256, 17112 * 256, 17488 * 256,
+ 17864 * 256, 18256 * 256, 18656 * 256, 19064 * 256,
+ 19480 * 256, 19912 * 256, 20344 * 256, 20792 * 256,
+ 21248 * 256, 21712 * 256, 22192 * 256, 22672 * 256,
+ 23168 * 256, 23680 * 256, 24200 * 256, 24728 * 256,
+ 25264 * 256, 25824 * 256, 26384 * 256, 26968 * 256,
+ 27552 * 256, 28160 * 256, 28776 * 256, 29408 * 256,
+ 30048 * 256, 30704 * 256, 31376 * 256, 32064 * 256
+};
+
+
+void clearmem_HD(void* mem, int32_t sz)
+{
+ int8_t* m = (int8_t*)mem;
+ int32_t i = 0;
+ for (; i < sz; i++)
+ {
+ *m = 0;
+ m++;
+ }
+}
+
+APTXHDBTENCEXPORT int SizeofAptxhdbtenc()
+{
+ return (sizeof(aptxhdbtenc));
+}
+
+APTXHDBTENCEXPORT const char* aptxhdbtenc_version()
+{
+ return (swversion);
+}
+
+APTXHDBTENCEXPORT int aptxhdbtenc_init(void* _state, short endian)
+{
+ aptxhdbtenc* state = (aptxhdbtenc*)_state;
+ int32_t j = 0;
+ int32_t k;
+ int32_t t;
+
+ clearmem_HD(_state, sizeof(aptxhdbtenc));
+
+ if (state == 0)
+ return 1;
+ state->m_syncWordPhase = 7L;
+
+ if (endian == 0)
+ {
+ state->m_endian = 0;
+ }
+ else
+ {
+ state->m_endian = 8;
+ }
+
+ for (j = 0; j < 2; j++)
+ {
+ Encoder_data* encode_dat = &state->m_encoderData[j];
+ uint32_t i;
+
+ /* Create a quantiser and subband processor for each suband */
+ for (i = LL; i <= HH; i++)
+ {
+ encode_dat->m_codewordHistory = 0L;
+
+ encode_dat->m_qdata[i].thresholdTablePtr = subbandParameters[i].threshTable;
+ encode_dat->m_qdata[i].thresholdTablePtr_sl1 = subbandParameters[i].threshTable_sl1;
+ encode_dat->m_qdata[i].ditherTablePtr = subbandParameters[i].dithTable;
+ encode_dat->m_qdata[i].minusLambdaDTable = subbandParameters[i].minusLambdaDTable;
+ encode_dat->m_qdata[i].codeBits = subbandParameters[i].numBits;
+ encode_dat->m_qdata[i].qCode = 0L;
+ encode_dat->m_qdata[i].altQcode = 0L;
+ encode_dat->m_qdata[i].distPenalty = 0L;
+
+ /* initialisation of inverseQuantiser data */
+ encode_dat->m_SubbandData[i].m_iqdata.thresholdTablePtr = subbandParameters[i].threshTable;
+ encode_dat->m_SubbandData[i].m_iqdata.thresholdTablePtr_sl1 = subbandParameters[i].threshTable_sl1;
+ encode_dat->m_SubbandData[i].m_iqdata.ditherTablePtr_sf1 = subbandParameters[i].dithTable_sh1;
+ encode_dat->m_SubbandData[i].m_iqdata.incrTablePtr = subbandParameters[i].incrTable;
+ encode_dat->m_SubbandData[i].m_iqdata.maxLogDelta = subbandParameters[i].maxLogDelta;
+ encode_dat->m_SubbandData[i].m_iqdata.minLogDelta = subbandParameters[i].minLogDelta;
+ encode_dat->m_SubbandData[i].m_iqdata.delta = 0;
+ encode_dat->m_SubbandData[i].m_iqdata.logDelta = 0;
+ encode_dat->m_SubbandData[i].m_iqdata.invQ = 0;
+ encode_dat->m_SubbandData[i].m_iqdata.iquantTableLogPtr = &IQuant_tableLogT[0];
+
+ // Initializing data for predictor filter
+ encode_dat->m_SubbandData[i].m_predData.m_zeroDelayLine.modulo = subbandParameters[i].numZeros;
+
+ for (t = 0; t < 48; t++)
+ {
+ encode_dat->m_SubbandData[i].m_predData.m_zeroDelayLine.buffer[t] = 0;
+ }
+
+ encode_dat->m_SubbandData[i].m_predData.m_zeroDelayLine.pointer = 0;
+ /* Initialise the previous zero filter output and predictor output to zero */
+ encode_dat->m_SubbandData[i].m_predData.m_zeroVal = 0L;
+ encode_dat->m_SubbandData[i].m_predData.m_predVal = 0L;
+ encode_dat->m_SubbandData[i].m_predData.m_numZeros = subbandParameters[i].numZeros;
+ /* Initialise the contents of the pole data delay line to zero */
+ encode_dat->m_SubbandData[i].m_predData.m_poleDelayLine[0] = 0L;
+ encode_dat->m_SubbandData[i].m_predData.m_poleDelayLine[1] = 0L;
+
+ for (k = 0; k < 24; k++)
+ {
+ encode_dat->m_SubbandData[i].m_ZeroCoeffData.m_zeroCoeff[k] = 0;
+ }
+
+ // Initializing data for zerocoeff update function.
+ encode_dat->m_SubbandData[i].m_ZeroCoeffData.m_numZeros = subbandParameters[i].numZeros;
+
+ /* Initializing data for PoleCoeff update function.
+ * Fill the adaptation delay line with +1 initially */
+ encode_dat->m_SubbandData[i].m_PoleCoeffData.m_poleAdaptDelayLine.s32 = 0x00010001;
+
+ /* Zero the pole coefficients */
+ encode_dat->m_SubbandData[i].m_PoleCoeffData.m_poleCoeff[0] = 0L;
+ encode_dat->m_SubbandData[i].m_PoleCoeffData.m_poleCoeff[1] = 0L;
+ }
+ }
+ return 0;
+}
+
+APTXHDBTENCEXPORT int aptxhdbtenc_encodestereo(void* _state, void* _pcmL, void* _pcmR, void* _buffer)
+{
+ aptxhdbtenc* state = (aptxhdbtenc*)_state;
+ int32_t* pcmL = (int32_t*)_pcmL;
+ int32_t* pcmR = (int32_t*)_pcmR;
+ int32_t* buffer = (int32_t*)_buffer;
+ int32_t tmp_reg;
+
+ //Feed the PCM to the dual aptX HD encoders
+ aptxhdEncode(pcmL, &state->m_qmf_l, &state->m_encoderData[0]);
+ aptxhdEncode(pcmR, &state->m_qmf_r, &state->m_encoderData[1]);
+
+ //Insert the autosync information into the stereo quantised codes
+ xbtEncinsertSync(&state->m_encoderData[0], &state->m_encoderData[1], &state->m_syncWordPhase);
+
+ aptxhdPostEncode(&state->m_encoderData[0]);
+ aptxhdPostEncode(&state->m_encoderData[1]);
+
+ //Pack the (possibly adjusted) codes into a 24-bit codeword per channel
+ tmp_reg = packCodeword(&state->m_encoderData[0]);
+ buffer[0] = (int32_t)tmp_reg;
+
+ tmp_reg = packCodeword(&state->m_encoderData[1]);
+ buffer[1] = (int32_t)tmp_reg;
+
+ return 0;
+}
diff --git a/system/embdrv/encoder_for_aptxhd/src/swversion.h b/system/embdrv/encoder_for_aptxhd/src/swversion.h
new file mode 100644
index 00000000000..7ca62d48845
--- /dev/null
+++ b/system/embdrv/encoder_for_aptxhd/src/swversion.h
@@ -0,0 +1,8 @@
+#ifndef SWVERSION_H
+#define SWVERSION_H
+
+
+const char* swversion = "1.0.0";
+
+
+#endif //SWVERSION_H
--
GitLab