git.sesse.net Git - ffmpeg/blob - libavcodec/aptx.c

   1 /*
   2  * Audio Processing Technology codec for Bluetooth (aptX)
   3  *
   4  * Copyright (C) 2017  Aurelien Jacobs <aurel@gnuage.org>
   5  *
   6  * This file is part of FFmpeg.
   7  *
   8  * FFmpeg is free software; you can redistribute it and/or
   9  * modify it under the terms of the GNU Lesser General Public
  10  * License as published by the Free Software Foundation; either
  11  * version 2.1 of the License, or (at your option) any later version.
  12  *
  13  * FFmpeg is distributed in the hope that it will be useful,
  14  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  15  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  16  * Lesser General Public License for more details.
  17  *
  18  * You should have received a copy of the GNU Lesser General Public
  19  * License along with FFmpeg; if not, write to the Free Software
  20  * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
  21  */
  22
  23 #include "libavutil/intreadwrite.h"
  24 #include "avcodec.h"
  25 #include "internal.h"
  26 #include "mathops.h"
  27 #include "audio_frame_queue.h"
  28
  29
  30 enum channels {
  31     LEFT,
  32     RIGHT,
  33     NB_CHANNELS
  34 };
  35
  36 enum subbands {
  37     LF,  // Low Frequency (0-5.5 kHz)
  38     MLF, // Medium-Low Frequency (5.5-11kHz)
  39     MHF, // Medium-High Frequency (11-16.5kHz)
  40     HF,  // High Frequency (16.5-22kHz)
  41     NB_SUBBANDS
  42 };
  43
  44 #define NB_FILTERS 2
  45 #define FILTER_TAPS 16
  46
  47 typedef struct {
  48     int pos;
  49     int32_t buffer[2*FILTER_TAPS];
  50 } FilterSignal;
  51
  52 typedef struct {
  53     FilterSignal outer_filter_signal[NB_FILTERS];
  54     FilterSignal inner_filter_signal[NB_FILTERS][NB_FILTERS];
  55 } QMFAnalysis;
  56
  57 typedef struct {
  58     int32_t quantized_sample;
  59     int32_t quantized_sample_parity_change;
  60     int32_t error;
  61 } Quantize;
  62
  63 typedef struct {
  64     int32_t quantization_factor;
  65     int32_t factor_select;
  66     int32_t reconstructed_difference;
  67 } InvertQuantize;
  68
  69 typedef struct {
  70     int32_t prev_sign[2];
  71     int32_t s_weight[2];
  72     int32_t d_weight[24];
  73     int32_t pos;
  74     int32_t reconstructed_differences[48];
  75     int32_t previous_reconstructed_sample;
  76     int32_t predicted_difference;
  77     int32_t predicted_sample;
  78 } Prediction;
  79
  80 typedef struct {
  81     int32_t codeword_history;
  82     int32_t dither_parity;
  83     int32_t dither[NB_SUBBANDS];
  84
  85     QMFAnalysis qmf;
  86     Quantize quantize[NB_SUBBANDS];
  87     InvertQuantize invert_quantize[NB_SUBBANDS];
  88     Prediction prediction[NB_SUBBANDS];
  89 } Channel;
  90
  91 typedef struct {
  92     int32_t sync_idx;
  93     Channel channels[NB_CHANNELS];
  94     AudioFrameQueue afq;
  95 } AptXContext;
  96
  97
  98 static const int32_t quantize_intervals_LF[65] = {
  99       -9948,    9948,   29860,   49808,   69822,   89926,  110144,  130502,
 100      151026,  171738,  192666,  213832,  235264,  256982,  279014,  301384,
 101      324118,  347244,  370790,  394782,  419250,  444226,  469742,  495832,
 102      522536,  549890,  577936,  606720,  636290,  666700,  698006,  730270,
 103      763562,  797958,  833538,  870398,  908640,  948376,  989740, 1032874,
 104     1077948, 1125150, 1174700, 1226850, 1281900, 1340196, 1402156, 1468282,
 105     1539182, 1615610, 1698514, 1789098, 1888944, 2000168, 2125700, 2269750,
 106     2438670, 2642660, 2899462, 3243240, 3746078, 4535138, 5664098, 7102424,
 107     8897462,
 108 };
 109 static const int32_t invert_quantize_dither_factors_LF[65] = {
 110        9948,   9948,   9962,   9988,  10026,  10078,  10142,  10218,
 111       10306,  10408,  10520,  10646,  10784,  10934,  11098,  11274,
 112       11462,  11664,  11880,  12112,  12358,  12618,  12898,  13194,
 113       13510,  13844,  14202,  14582,  14988,  15422,  15884,  16380,
 114       16912,  17484,  18098,  18762,  19480,  20258,  21106,  22030,
 115       23044,  24158,  25390,  26760,  28290,  30008,  31954,  34172,
 116       36728,  39700,  43202,  47382,  52462,  58762,  66770,  77280,
 117       91642, 112348, 144452, 199326, 303512, 485546, 643414, 794914,
 118     1000124,
 119 };
 120 static const int32_t quantize_dither_factors_LF[65] = {
 121         0,     4,     7,    10,    13,    16,    19,    22,
 122        26,    28,    32,    35,    38,    41,    44,    47,
 123        51,    54,    58,    62,    65,    70,    74,    79,
 124        84,    90,    95,   102,   109,   116,   124,   133,
 125       143,   154,   166,   180,   195,   212,   231,   254,
 126       279,   308,   343,   383,   430,   487,   555,   639,
 127       743,   876,  1045,  1270,  1575,  2002,  2628,  3591,
 128      5177,  8026, 13719, 26047, 45509, 39467, 37875, 51303,
 129         0,
 130 };
 131 static const int16_t quantize_factor_select_offset_LF[65] = {
 132       0, -21, -19, -17, -15, -12, -10,  -8,
 133      -6,  -4,  -1,   1,   3,   6,   8,  10,
 134      13,  15,  18,  20,  23,  26,  29,  31,
 135      34,  37,  40,  43,  47,  50,  53,  57,
 136      60,  64,  68,  72,  76,  80,  85,  89,
 137      94,  99, 105, 110, 116, 123, 129, 136,
 138     144, 152, 161, 171, 182, 194, 207, 223,
 139     241, 263, 291, 328, 382, 467, 522, 522,
 140     522,
 141 };
 142
 143
 144 static const int32_t quantize_intervals_MLF[9] = {
 145     -89806, 89806, 278502, 494338, 759442, 1113112, 1652322, 2720256, 5190186,
 146 };
 147 static const int32_t invert_quantize_dither_factors_MLF[9] = {
 148     89806, 89806, 98890, 116946, 148158, 205512, 333698, 734236, 1735696,
 149 };
 150 static const int32_t quantize_dither_factors_MLF[9] = {
 151     0, 2271, 4514, 7803, 14339, 32047, 100135, 250365, 0,
 152 };
 153 static const int16_t quantize_factor_select_offset_MLF[9] = {
 154     0, -14, 6, 29, 58, 96, 154, 270, 521,
 155 };
 156
 157
 158 static const int32_t quantize_intervals_MHF[3] = {
 159     -194080, 194080, 890562,
 160 };
 161 static const int32_t invert_quantize_dither_factors_MHF[3] = {
 162     194080, 194080, 502402,
 163 };
 164 static const int32_t quantize_dither_factors_MHF[3] = {
 165     0, 77081, 0,
 166 };
 167 static const int16_t quantize_factor_select_offset_MHF[3] = {
 168     0, -33, 136,
 169 };
 170
 171
 172 static const int32_t quantize_intervals_HF[5] = {
 173     -163006, 163006, 542708, 1120554, 2669238,
 174 };
 175 static const int32_t invert_quantize_dither_factors_HF[5] = {
 176     163006, 163006, 216698, 361148, 1187538,
 177 };
 178 static const int32_t quantize_dither_factors_HF[5] = {
 179     0, 13423, 36113, 206598, 0,
 180 };
 181 static const int16_t quantize_factor_select_offset_HF[5] = {
 182     0, -8, 33, 95, 262,
 183 };
 184
 185 typedef const struct {
 186     const int32_t *quantize_intervals;
 187     const int32_t *invert_quantize_dither_factors;
 188     const int32_t *quantize_dither_factors;
 189     const int16_t *quantize_factor_select_offset;
 190     int tables_size;
 191     int32_t quantized_bits;
 192     int32_t prediction_order;
 193 } ConstTables;
 194
 195 static ConstTables tables[NB_SUBBANDS] = {
 196     [LF]  = { quantize_intervals_LF,
 197               invert_quantize_dither_factors_LF,
 198               quantize_dither_factors_LF,
 199               quantize_factor_select_offset_LF,
 200               FF_ARRAY_ELEMS(quantize_intervals_LF),
 201               7, 24 },
 202     [MLF] = { quantize_intervals_MLF,
 203               invert_quantize_dither_factors_MLF,
 204               quantize_dither_factors_MLF,
 205               quantize_factor_select_offset_MLF,
 206               FF_ARRAY_ELEMS(quantize_intervals_MLF),
 207               4, 12 },
 208     [MHF] = { quantize_intervals_MHF,
 209               invert_quantize_dither_factors_MHF,
 210               quantize_dither_factors_MHF,
 211               quantize_factor_select_offset_MHF,
 212               FF_ARRAY_ELEMS(quantize_intervals_MHF),
 213               2, 6 },
 214     [HF]  = { quantize_intervals_HF,
 215               invert_quantize_dither_factors_HF,
 216               quantize_dither_factors_HF,
 217               quantize_factor_select_offset_HF,
 218               FF_ARRAY_ELEMS(quantize_intervals_HF),
 219               3, 12 },
 220 };
 221
 222 static const int16_t quantization_factors[32] = {
 223     2048, 2093, 2139, 2186, 2233, 2282, 2332, 2383,
 224     2435, 2489, 2543, 2599, 2656, 2714, 2774, 2834,
 225     2896, 2960, 3025, 3091, 3158, 3228, 3298, 3371,
 226     3444, 3520, 3597, 3676, 3756, 3838, 3922, 4008,
 227 };
 228
 229
 230 /* Rounded right shift with optionnal clipping */
 231 #define RSHIFT_SIZE(size)                                                     \
 232 av_always_inline                                                              \
 233 static int##size##_t rshift##size(int##size##_t value, int shift)             \
 234 {                                                                             \
 235     int##size##_t rounding = (int##size##_t)1 << (shift - 1);                 \
 236     int##size##_t mask = ((int##size##_t)1 << (shift + 1)) - 1;               \
 237     return ((value + rounding) >> shift) - ((value & mask) == rounding);      \
 238 }                                                                             \
 239 av_always_inline                                                              \
 240 static int##size##_t rshift##size##_clip24(int##size##_t value, int shift)    \
 241 {                                                                             \
 242     return av_clip_intp2(rshift##size(value, shift), 23);                     \
 243 }
 244 RSHIFT_SIZE(32)
 245 RSHIFT_SIZE(64)
 246
 247
 248 av_always_inline
 249 static void aptx_update_codeword_history(Channel *channel)
 250 {
 251     int32_t cw = ((channel->quantize[0].quantized_sample & 3) << 0) +
 252                  ((channel->quantize[1].quantized_sample & 2) << 1) +
 253                  ((channel->quantize[2].quantized_sample & 1) << 3);
 254     channel->codeword_history = (cw << 8) + (channel->codeword_history << 4);
 255 }
 256
 257 static void aptx_generate_dither(Channel *channel)
 258 {
 259     int subband;
 260     int64_t m;
 261     int32_t d;
 262
 263     aptx_update_codeword_history(channel);
 264
 265     m = (int64_t)5184443 * (channel->codeword_history >> 7);
 266     d = (m << 2) + (m >> 22);
 267     for (subband = 0; subband < NB_SUBBANDS; subband++)
 268         channel->dither[subband] = d << (23 - 5*subband);
 269     channel->dither_parity = (d >> 25) & 1;
 270 }
 271
 272 /*
 273  * Convolution filter coefficients for the outer QMF of the QMF tree.
 274  * The 2 sets are a mirror of each other.
 275  */
 276 static const int32_t aptx_qmf_outer_coeffs[NB_FILTERS][FILTER_TAPS] = {
 277     {
 278         730, -413, -9611, 43626, -121026, 269973, -585547, 2801966,
 279         697128, -160481, 27611, 8478, -10043, 3511, 688, -897,
 280     },
 281     {
 282         -897, 688, 3511, -10043, 8478, 27611, -160481, 697128,
 283         2801966, -585547, 269973, -121026, 43626, -9611, -413, 730,
 284     },
 285 };
 286
 287 /*
 288  * Convolution filter coefficients for the inner QMF of the QMF tree.
 289  * The 2 sets are a mirror of each other.
 290  */
 291 static const int32_t aptx_qmf_inner_coeffs[NB_FILTERS][FILTER_TAPS] = {
 292     {
 293        1033, -584, -13592, 61697, -171156, 381799, -828088, 3962579,
 294        985888, -226954, 39048, 11990, -14203, 4966, 973, -1268,
 295     },
 296     {
 297       -1268, 973, 4966, -14203, 11990, 39048, -226954, 985888,
 298       3962579, -828088, 381799, -171156, 61697, -13592, -584, 1033,
 299     },
 300 };
 301
 302 /*
 303  * Push one sample into a circular signal buffer.
 304  */
 305 av_always_inline
 306 static void aptx_qmf_filter_signal_push(FilterSignal *signal, int32_t sample)
 307 {
 308     signal->buffer[signal->pos            ] = sample;
 309     signal->buffer[signal->pos+FILTER_TAPS] = sample;
 310     signal->pos = (signal->pos + 1) & (FILTER_TAPS - 1);
 311 }
 312
 313 /*
 314  * Compute the convolution of the signal with the coefficients, and reduce
 315  * to 24 bits by applying the specified right shifting.
 316  */
 317 av_always_inline
 318 static int32_t aptx_qmf_convolution(FilterSignal *signal,
 319                                     const int32_t coeffs[FILTER_TAPS],
 320                                     int shift)
 321 {
 322     int32_t *sig = &signal->buffer[signal->pos];
 323     int64_t e = 0;
 324     int i;
 325
 326     for (i = 0; i < FILTER_TAPS; i++)
 327         e += MUL64(sig[i], coeffs[i]);
 328
 329     return rshift64_clip24(e, shift);
 330 }
 331
 332 /*
 333  * Half-band QMF analysis filter realized with a polyphase FIR filter.
 334  * Split into 2 subbands and downsample by 2.
 335  * So for each pair of samples that goes in, one sample goes out,
 336  * split into 2 separate subbands.
 337  */
 338 av_always_inline
 339 static void aptx_qmf_polyphase_analysis(FilterSignal signal[NB_FILTERS],
 340                                         const int32_t coeffs[NB_FILTERS][FILTER_TAPS],
 341                                         int shift,
 342                                         int32_t samples[NB_FILTERS],
 343                                         int32_t *low_subband_output,
 344                                         int32_t *high_subband_output)
 345 {
 346     int32_t subbands[NB_FILTERS];
 347     int i;
 348
 349     for (i = 0; i < NB_FILTERS; i++) {
 350         aptx_qmf_filter_signal_push(&signal[i], samples[NB_FILTERS-1-i]);
 351         subbands[i] = aptx_qmf_convolution(&signal[i], coeffs[i], shift);
 352     }
 353
 354     *low_subband_output  = av_clip_intp2(subbands[0] + subbands[1], 23);
 355     *high_subband_output = av_clip_intp2(subbands[0] - subbands[1], 23);
 356 }
 357
 358 /*
 359  * Two stage QMF analysis tree.
 360  * Split 4 input samples into 4 subbands and downsample by 4.
 361  * So for each group of 4 samples that goes in, one sample goes out,
 362  * split into 4 separate subbands.
 363  */
 364 static void aptx_qmf_tree_analysis(QMFAnalysis *qmf,
 365                                    int32_t samples[4],
 366                                    int32_t subband_samples[4])
 367 {
 368     int32_t intermediate_samples[4];
 369     int i;
 370
 371     /* Split 4 input samples into 2 intermediate subbands downsampled to 2 samples */
 372     for (i = 0; i < 2; i++)
 373         aptx_qmf_polyphase_analysis(qmf->outer_filter_signal,
 374                                     aptx_qmf_outer_coeffs, 23,
 375                                     &samples[2*i],
 376                                     &intermediate_samples[0+i],
 377                                     &intermediate_samples[2+i]);
 378
 379     /* Split 2 intermediate subband samples into 4 final subbands downsampled to 1 sample */
 380     for (i = 0; i < 2; i++)
 381         aptx_qmf_polyphase_analysis(qmf->inner_filter_signal[i],
 382                                     aptx_qmf_inner_coeffs, 23,
 383                                     &intermediate_samples[2*i],
 384                                     &subband_samples[2*i+0],
 385                                     &subband_samples[2*i+1]);
 386 }
 387
 388 /*
 389  * Half-band QMF synthesis filter realized with a polyphase FIR filter.
 390  * Join 2 subbands and upsample by 2.
 391  * So for each 2 subbands sample that goes in, a pair of samples goes out.
 392  */
 393 av_always_inline
 394 static void aptx_qmf_polyphase_synthesis(FilterSignal signal[NB_FILTERS],
 395                                          const int32_t coeffs[NB_FILTERS][FILTER_TAPS],
 396                                          int shift,
 397                                          int32_t low_subband_input,
 398                                          int32_t high_subband_input,
 399                                          int32_t samples[NB_FILTERS])
 400 {
 401     int32_t subbands[NB_FILTERS];
 402     int i;
 403
 404     subbands[0] = low_subband_input + high_subband_input;
 405     subbands[1] = low_subband_input - high_subband_input;
 406
 407     for (i = 0; i < NB_FILTERS; i++) {
 408         aptx_qmf_filter_signal_push(&signal[i], subbands[1-i]);
 409         samples[i] = aptx_qmf_convolution(&signal[i], coeffs[i], shift);
 410     }
 411 }
 412
 413 /*
 414  * Two stage QMF synthesis tree.
 415  * Join 4 subbands and upsample by 4.
 416  * So for each 4 subbands sample that goes in, a group of 4 samples goes out.
 417  */
 418 static void aptx_qmf_tree_synthesis(QMFAnalysis *qmf,
 419                                     int32_t subband_samples[4],
 420                                     int32_t samples[4])
 421 {
 422     int32_t intermediate_samples[4];
 423     int i;
 424
 425     /* Join 4 subbands into 2 intermediate subbands upsampled to 2 samples. */
 426     for (i = 0; i < 2; i++)
 427         aptx_qmf_polyphase_synthesis(qmf->inner_filter_signal[i],
 428                                      aptx_qmf_inner_coeffs, 22,
 429                                      subband_samples[2*i+0],
 430                                      subband_samples[2*i+1],
 431                                      &intermediate_samples[2*i]);
 432
 433     /* Join 2 samples from intermediate subbands upsampled to 4 samples. */
 434     for (i = 0; i < 2; i++)
 435         aptx_qmf_polyphase_synthesis(qmf->outer_filter_signal,
 436                                      aptx_qmf_outer_coeffs, 21,
 437                                      intermediate_samples[0+i],
 438                                      intermediate_samples[2+i],
 439                                      &samples[2*i]);
 440 }
 441
 442
 443 av_always_inline
 444 static int32_t aptx_bin_search(int32_t value, int32_t factor,
 445                                const int32_t *intervals, int32_t nb_intervals)
 446 {
 447     int32_t idx = 0;
 448     int i;
 449
 450     for (i = nb_intervals >> 1; i > 0; i >>= 1)
 451         if (MUL64(factor, intervals[idx + i]) <= ((int64_t)value << 24))
 452             idx += i;
 453
 454     return idx;
 455 }
 456
 457 static void aptx_quantize_difference(Quantize *quantize,
 458                                      int32_t sample_difference,
 459                                      int32_t dither,
 460                                      int32_t quantization_factor,
 461                                      ConstTables *tables)
 462 {
 463     const int32_t *intervals = tables->quantize_intervals;
 464     int32_t quantized_sample, dithered_sample, parity_change;
 465     int32_t d, mean, interval, inv;
 466     int64_t error;
 467
 468     quantized_sample = aptx_bin_search(FFABS(sample_difference) >> 4,
 469                                        quantization_factor,
 470                                        intervals, tables->tables_size);
 471
 472     d = rshift32_clip24(MULH(dither, dither), 7) - (1 << 23);
 473     d = rshift64(MUL64(d, tables->quantize_dither_factors[quantized_sample]), 23);
 474
 475     intervals += quantized_sample;
 476     mean = (intervals[1] + intervals[0]) / 2;
 477     interval = (intervals[1] - intervals[0]) * (-(sample_difference < 0) | 1);
 478
 479     dithered_sample = rshift64_clip24(MUL64(dither, interval) + ((int64_t)(mean + d) << 32), 32);
 480     error = ((int64_t)FFABS(sample_difference) << 20) - MUL64(dithered_sample, quantization_factor);
 481     quantize->error = FFABS(rshift64(error, 23));
 482
 483     parity_change = quantized_sample;
 484     if (error < 0)
 485         quantized_sample--;
 486     else
 487         parity_change--;
 488
 489     inv = -(sample_difference < 0);
 490     quantize->quantized_sample               = quantized_sample ^ inv;
 491     quantize->quantized_sample_parity_change = parity_change    ^ inv;
 492 }
 493
 494 static void aptx_encode_channel(Channel *channel, int32_t samples[4])
 495 {
 496     int32_t subband_samples[4];
 497     int subband;
 498     aptx_qmf_tree_analysis(&channel->qmf, samples, subband_samples);
 499     aptx_generate_dither(channel);
 500     for (subband = 0; subband < NB_SUBBANDS; subband++) {
 501         int32_t diff = av_clip_intp2(subband_samples[subband] - channel->prediction[subband].predicted_sample, 23);
 502         aptx_quantize_difference(&channel->quantize[subband], diff,
 503                                  channel->dither[subband],
 504                                  channel->invert_quantize[subband].quantization_factor,
 505                                  &tables[subband]);
 506     }
 507 }
 508
 509 static void aptx_decode_channel(Channel *channel, int32_t samples[4])
 510 {
 511     int32_t subband_samples[4];
 512     int subband;
 513     for (subband = 0; subband < NB_SUBBANDS; subband++)
 514         subband_samples[subband] = channel->prediction[subband].previous_reconstructed_sample;
 515     aptx_qmf_tree_synthesis(&channel->qmf, subband_samples, samples);
 516 }
 517
 518
 519 static void aptx_invert_quantization(InvertQuantize *invert_quantize,
 520                                      int32_t quantized_sample, int32_t dither,
 521                                      ConstTables *tables)
 522 {
 523     int32_t qr, idx, shift, factor_select;
 524
 525     idx = (quantized_sample ^ -(quantized_sample < 0)) + 1;
 526     qr = tables->quantize_intervals[idx] / 2;
 527     if (quantized_sample < 0)
 528         qr = -qr;
 529
 530     qr = rshift64_clip24(((int64_t)qr<<32) + MUL64(dither, tables->invert_quantize_dither_factors[idx]), 32);
 531     invert_quantize->reconstructed_difference = MUL64(invert_quantize->quantization_factor, qr) >> 19;
 532
 533     shift = 24 - tables->quantized_bits;
 534
 535     /* update factor_select */
 536     factor_select = 32620 * invert_quantize->factor_select;
 537     factor_select = rshift32(factor_select + (tables->quantize_factor_select_offset[idx] << 15), 15);
 538     invert_quantize->factor_select = av_clip(factor_select, 0, (shift << 8) | 0xFF);
 539
 540     /* update quantization factor */
 541     idx = (invert_quantize->factor_select & 0xFF) >> 3;
 542     shift -= invert_quantize->factor_select >> 8;
 543     invert_quantize->quantization_factor = (quantization_factors[idx] << 11) >> shift;
 544 }
 545
 546 static int32_t *aptx_reconstructed_differences_update(Prediction *prediction,
 547                                                       int32_t reconstructed_difference,
 548                                                       int order)
 549 {
 550     int32_t *rd1 = prediction->reconstructed_differences, *rd2 = rd1 + order;
 551     int p = prediction->pos;
 552
 553     rd1[p] = rd2[p];
 554     prediction->pos = p = (p + 1) % order;
 555     rd2[p] = reconstructed_difference;
 556     return &rd2[p];
 557 }
 558
 559 static void aptx_prediction_filtering(Prediction *prediction,
 560                                       int32_t reconstructed_difference,
 561                                       int order)
 562 {
 563     int32_t reconstructed_sample, predictor, srd0;
 564     int32_t *reconstructed_differences;
 565     int64_t predicted_difference = 0;
 566     int i;
 567
 568     reconstructed_sample = av_clip_intp2(reconstructed_difference + prediction->predicted_sample, 23);
 569     predictor = av_clip_intp2((MUL64(prediction->s_weight[0], prediction->previous_reconstructed_sample)
 570                              + MUL64(prediction->s_weight[1], reconstructed_sample)) >> 22, 23);
 571     prediction->previous_reconstructed_sample = reconstructed_sample;
 572
 573     reconstructed_differences = aptx_reconstructed_differences_update(prediction, reconstructed_difference, order);
 574     srd0 = FFDIFFSIGN(reconstructed_difference, 0) << 23;
 575     for (i = 0; i < order; i++) {
 576         int32_t srd = FF_SIGNBIT(reconstructed_differences[-i-1]) | 1;
 577         prediction->d_weight[i] -= rshift32(prediction->d_weight[i] - srd*srd0, 8);
 578         predicted_difference += MUL64(reconstructed_differences[-i], prediction->d_weight[i]);
 579     }
 580
 581     prediction->predicted_difference = av_clip_intp2(predicted_difference >> 22, 23);
 582     prediction->predicted_sample = av_clip_intp2(predictor + prediction->predicted_difference, 23);
 583 }
 584
 585 static void aptx_process_subband(InvertQuantize *invert_quantize,
 586                                  Prediction *prediction,
 587                                  int32_t quantized_sample, int32_t dither,
 588                                  ConstTables *tables)
 589 {
 590     int32_t sign, same_sign[2], weight[2], sw1, range;
 591
 592     aptx_invert_quantization(invert_quantize, quantized_sample, dither, tables);
 593
 594     sign = FFDIFFSIGN(invert_quantize->reconstructed_difference,
 595                       -prediction->predicted_difference);
 596     same_sign[0] = sign * prediction->prev_sign[0];
 597     same_sign[1] = sign * prediction->prev_sign[1];
 598     prediction->prev_sign[0] = prediction->prev_sign[1];
 599     prediction->prev_sign[1] = sign | 1;
 600
 601     range = 0x100000;
 602     sw1 = rshift32(-same_sign[1] * prediction->s_weight[1], 1);
 603     sw1 = (av_clip(sw1, -range, range) & ~0xF) << 4;
 604
 605     range = 0x300000;
 606     weight[0] = 254 * prediction->s_weight[0] + 0x800000*same_sign[0] + sw1;
 607     prediction->s_weight[0] = av_clip(rshift32(weight[0], 8), -range, range);
 608
 609     range = 0x3C0000 - prediction->s_weight[0];
 610     weight[1] = 255 * prediction->s_weight[1] + 0xC00000*same_sign[1];
 611     prediction->s_weight[1] = av_clip(rshift32(weight[1], 8), -range, range);
 612
 613     aptx_prediction_filtering(prediction,
 614                               invert_quantize->reconstructed_difference,
 615                               tables->prediction_order);
 616 }
 617
 618 static void aptx_invert_quantize_and_prediction(Channel *channel)
 619 {
 620     int subband;
 621     for (subband = 0; subband < NB_SUBBANDS; subband++)
 622         aptx_process_subband(&channel->invert_quantize[subband],
 623                              &channel->prediction[subband],
 624                              channel->quantize[subband].quantized_sample,
 625                              channel->dither[subband],
 626                              &tables[subband]);
 627 }
 628
 629 static int32_t aptx_quantized_parity(Channel *channel)
 630 {
 631     int32_t parity = channel->dither_parity;
 632     int subband;
 633
 634     for (subband = 0; subband < NB_SUBBANDS; subband++)
 635         parity ^= channel->quantize[subband].quantized_sample;
 636
 637     return parity & 1;
 638 }
 639
 640 /* For each sample, ensure that the parity of all subbands of all channels
 641  * is 0 except once every 8 samples where the parity is forced to 1. */
 642 static int aptx_check_parity(Channel channels[NB_CHANNELS], int32_t *idx)
 643 {
 644     int32_t parity = aptx_quantized_parity(&channels[LEFT])
 645                    ^ aptx_quantized_parity(&channels[RIGHT]);
 646
 647     int eighth = *idx == 7;
 648     *idx = (*idx + 1) & 7;
 649
 650     return parity ^ eighth;
 651 }
 652
 653 static void aptx_insert_sync(Channel channels[NB_CHANNELS], int32_t *idx)
 654 {
 655     if (aptx_check_parity(channels, idx)) {
 656         int i;
 657         Channel *c;
 658         static const int map[] = { 1, 2, 0, 3 };
 659         Quantize *min = &channels[NB_CHANNELS-1].quantize[map[0]];
 660         for (c = &channels[NB_CHANNELS-1]; c >= channels; c--)
 661             for (i = 0; i < NB_SUBBANDS; i++)
 662                 if (c->quantize[map[i]].error < min->error)
 663                     min = &c->quantize[map[i]];
 664
 665         /* Forcing the desired parity is done by offsetting by 1 the quantized
 666          * sample from the subband featuring the smallest quantization error. */
 667         min->quantized_sample = min->quantized_sample_parity_change;
 668     }
 669 }
 670
 671 static uint16_t aptx_pack_codeword(Channel *channel)
 672 {
 673     int32_t parity = aptx_quantized_parity(channel);
 674     return (((channel->quantize[3].quantized_sample & 0x06) | parity) << 13)
 675          | (((channel->quantize[2].quantized_sample & 0x03)         ) << 11)
 676          | (((channel->quantize[1].quantized_sample & 0x0F)         ) <<  7)
 677          | (((channel->quantize[0].quantized_sample & 0x7F)         ) <<  0);
 678 }
 679
 680 static void aptx_unpack_codeword(Channel *channel, uint16_t codeword)
 681 {
 682     channel->quantize[0].quantized_sample = sign_extend(codeword >>  0, 7);
 683     channel->quantize[1].quantized_sample = sign_extend(codeword >>  7, 4);
 684     channel->quantize[2].quantized_sample = sign_extend(codeword >> 11, 2);
 685     channel->quantize[3].quantized_sample = sign_extend(codeword >> 13, 3);
 686     channel->quantize[3].quantized_sample = (channel->quantize[3].quantized_sample & ~1)
 687                                           | aptx_quantized_parity(channel);
 688 }
 689
 690 static void aptx_encode_samples(AptXContext *ctx,
 691                                 int32_t samples[NB_CHANNELS][4],
 692                                 uint8_t output[2*NB_CHANNELS])
 693 {
 694     int channel;
 695     for (channel = 0; channel < NB_CHANNELS; channel++)
 696         aptx_encode_channel(&ctx->channels[channel], samples[channel]);
 697
 698     aptx_insert_sync(ctx->channels, &ctx->sync_idx);
 699
 700     for (channel = 0; channel < NB_CHANNELS; channel++) {
 701         aptx_invert_quantize_and_prediction(&ctx->channels[channel]);
 702         AV_WB16(output + 2*channel, aptx_pack_codeword(&ctx->channels[channel]));
 703     }
 704 }
 705
 706 static int aptx_decode_samples(AptXContext *ctx,
 707                                 const uint8_t input[2*NB_CHANNELS],
 708                                 int32_t samples[NB_CHANNELS][4])
 709 {
 710     int channel, ret;
 711
 712     for (channel = 0; channel < NB_CHANNELS; channel++) {
 713         uint16_t codeword;
 714         aptx_generate_dither(&ctx->channels[channel]);
 715
 716         codeword = AV_RB16(input + 2*channel);
 717         aptx_unpack_codeword(&ctx->channels[channel], codeword);
 718         aptx_invert_quantize_and_prediction(&ctx->channels[channel]);
 719     }
 720
 721     ret = aptx_check_parity(ctx->channels, &ctx->sync_idx);
 722
 723     for (channel = 0; channel < NB_CHANNELS; channel++)
 724         aptx_decode_channel(&ctx->channels[channel], samples[channel]);
 725
 726     return ret;
 727 }
 728
 729
 730 static av_cold int aptx_init(AVCodecContext *avctx)
 731 {
 732     AptXContext *s = avctx->priv_data;
 733     int chan, subband;
 734
 735     if (avctx->frame_size == 0)
 736         avctx->frame_size = 1024;
 737
 738     if (avctx->frame_size & 3) {
 739         av_log(avctx, AV_LOG_ERROR, "Frame size must be a multiple of 4 samples\n");
 740         return AVERROR(EINVAL);
 741     }
 742
 743     for (chan = 0; chan < NB_CHANNELS; chan++) {
 744         Channel *channel = &s->channels[chan];
 745         for (subband = 0; subband < NB_SUBBANDS; subband++) {
 746             Prediction *prediction = &channel->prediction[subband];
 747             prediction->prev_sign[0] = 1;
 748             prediction->prev_sign[1] = 1;
 749         }
 750     }
 751
 752     ff_af_queue_init(avctx, &s->afq);
 753     return 0;
 754 }
 755
 756 static int aptx_decode_frame(AVCodecContext *avctx, void *data,
 757                              int *got_frame_ptr, AVPacket *avpkt)
 758 {
 759     AptXContext *s = avctx->priv_data;
 760     AVFrame *frame = data;
 761     int pos, channel, sample, ret;
 762
 763     if (avpkt->size < 4) {
 764         av_log(avctx, AV_LOG_ERROR, "Packet is too small\n");
 765         return AVERROR_INVALIDDATA;
 766     }
 767
 768     /* get output buffer */
 769     frame->channels = NB_CHANNELS;
 770     frame->format = AV_SAMPLE_FMT_S32P;
 771     frame->nb_samples = avpkt->size & ~3;
 772     if ((ret = ff_get_buffer(avctx, frame, 0)) < 0)
 773         return ret;
 774
 775     for (pos = 0; pos < frame->nb_samples; pos += 4) {
 776         int32_t samples[NB_CHANNELS][4];
 777
 778         if (aptx_decode_samples(s, &avpkt->data[pos], samples)) {
 779             av_log(avctx, AV_LOG_ERROR, "Synchronization error\n");
 780             return AVERROR_INVALIDDATA;
 781         }
 782
 783         for (channel = 0; channel < NB_CHANNELS; channel++)
 784             for (sample = 0; sample < 4; sample++)
 785                 AV_WN32A(&frame->data[channel][4*(sample+pos)],
 786                          samples[channel][sample] << 8);
 787     }
 788
 789     *got_frame_ptr = 1;
 790     return frame->nb_samples;
 791 }
 792
 793 static int aptx_encode_frame(AVCodecContext *avctx, AVPacket *avpkt,
 794                              const AVFrame *frame, int *got_packet_ptr)
 795 {
 796     AptXContext *s = avctx->priv_data;
 797     int pos, channel, sample, ret;
 798
 799     if ((ret = ff_af_queue_add(&s->afq, frame)) < 0)
 800         return ret;
 801
 802     if ((ret = ff_alloc_packet2(avctx, avpkt, frame->nb_samples, 0)) < 0)
 803         return ret;
 804
 805     for (pos = 0; pos < frame->nb_samples; pos += 4) {
 806         int32_t samples[NB_CHANNELS][4];
 807
 808         for (channel = 0; channel < NB_CHANNELS; channel++)
 809             for (sample = 0; sample < 4; sample++)
 810                 samples[channel][sample] = (int32_t)AV_RN32A(&frame->data[channel][4*(sample+pos)]) >> 8;
 811
 812         aptx_encode_samples(s, samples, avpkt->data + pos);
 813     }
 814
 815     ff_af_queue_remove(&s->afq, frame->nb_samples, &avpkt->pts, &avpkt->duration);
 816     *got_packet_ptr = 1;
 817     return 0;
 818 }
 819
 820 static av_cold int aptx_close(AVCodecContext *avctx)
 821 {
 822     AptXContext *s = avctx->priv_data;
 823     ff_af_queue_close(&s->afq);
 824     return 0;
 825 }
 826
 827
 828 #if CONFIG_APTX_DECODER
 829 AVCodec ff_aptx_decoder = {
 830     .name                  = "aptx",
 831     .long_name             = NULL_IF_CONFIG_SMALL("aptX (Audio Processing Technology for Bluetooth)"),
 832     .type                  = AVMEDIA_TYPE_AUDIO,
 833     .id                    = AV_CODEC_ID_APTX,
 834     .priv_data_size        = sizeof(AptXContext),
 835     .init                  = aptx_init,
 836     .decode                = aptx_decode_frame,
 837     .close                 = aptx_close,
 838     .capabilities          = AV_CODEC_CAP_DR1,
 839     .channel_layouts       = (const uint64_t[]) { AV_CH_LAYOUT_STEREO, 0},
 840     .sample_fmts           = (const enum AVSampleFormat[]) { AV_SAMPLE_FMT_S32P,
 841                                                              AV_SAMPLE_FMT_NONE },
 842 };
 843 #endif
 844
 845 #if CONFIG_APTX_ENCODER
 846 AVCodec ff_aptx_encoder = {
 847     .name                  = "aptx",
 848     .long_name             = NULL_IF_CONFIG_SMALL("aptX (Audio Processing Technology for Bluetooth)"),
 849     .type                  = AVMEDIA_TYPE_AUDIO,
 850     .id                    = AV_CODEC_ID_APTX,
 851     .priv_data_size        = sizeof(AptXContext),
 852     .init                  = aptx_init,
 853     .encode2               = aptx_encode_frame,
 854     .close                 = aptx_close,
 855     .channel_layouts       = (const uint64_t[]) { AV_CH_LAYOUT_STEREO, 0},
 856     .sample_fmts           = (const enum AVSampleFormat[]) { AV_SAMPLE_FMT_S32P,
 857                                                              AV_SAMPLE_FMT_NONE },
 858     .supported_samplerates = (const int[]) {8000, 16000, 24000, 32000, 44100, 48000, 0},
 859 };
 860 #endif