src/media/codec/codecs/sw/cvsd/codec_adapter_cvsd.cc

// Copyright 2022 The Fuchsia Authors. All rights reserved.
// Use of this source code is governed by a BSD-style license that can be
// found in the LICENSE file.

#include "codec_adapter_cvsd.h"

#include <lib/async/cpp/task.h>

bool AreJbitsEqual(const CvsdParams& params) {
  uint32_t lastKBits = params.historic_bits;
  for (uint32_t i = 0; i < params.k - params.j + 1; i++) {
    auto tmp = lastKBits & params.equal_bit_mask;
    if (tmp == 0 || tmp == params.equal_bit_mask) {
      return true;
    }
    lastKBits = lastKBits >> 1;
  }
  return false;
}

// Calculates the new step size based on Bluetooth Core v5.3 section
// 9.2 CVSD CODEC, equations 16 and 17.
double GetNewStepSize(const CvsdParams& params) {
  // EQ 16 and EQ 17.
  // If J bits in the last K output bits are equal, step size is incremented
  // otherwise, it's decremented.
  double new_delta;
  if (AreJbitsEqual(params)) {
    new_delta =
        std::min(params.step_size + kStepDecaySizeMin, static_cast<double>(kStepDecaySizeMax));
  } else {
    new_delta = static_cast<int16_t>(
        std::max(params.step_size * kStepDecayNumerator / kStepDecayDenominator,
                 static_cast<double>(kStepDecaySizeMin)));
  }

  ZX_DEBUG_ASSERT(new_delta <= std::numeric_limits<int16_t>::max());
  ZX_DEBUG_ASSERT(new_delta >= std::numeric_limits<int16_t>::min());

  return new_delta;
}

// Calculates the new accumulator value based on Bluetooth Core v5.3 section
// 9.2 CVSD CODEC equations.
double GetNewAccumulator(const CvsdParams& params, const uint8_t& bit) {
  double new_accum = params.accumulator;
  new_accum += (bit) ? -params.step_size : params.step_size;

  // EQ 18. Limit the internal reference.
  if (new_accum >= 0) {
    new_accum = std::min(new_accum, static_cast<double>(kAccumulatorPosSaturation));
  } else {
    new_accum = std::max(new_accum, static_cast<double>(kAccumulatorNegSaturation));
  }
  // EQ 14. Multiply by the decay factor.
  new_accum = new_accum * kAccumDecayNumerator / kAccumDecayDenominator;

  ZX_DEBUG_ASSERT(new_accum <= std::numeric_limits<int16_t>::max());
  ZX_DEBUG_ASSERT(new_accum >= std::numeric_limits<int16_t>::min());

  return new_accum;
}

void UpdateCvsdParams(CvsdParams* params, uint8_t bit) {
  // Shift last value into historic bits buffer.
  params->historic_bits = ((params->historic_bits << 1) | bit) & params->historic_bit_mask;

  // Update the step size.
  params->step_size = GetNewStepSize(*params);

  // Update the internal accumulator.
  params->accumulator = GetNewAccumulator(*params, bit);
}

void InitCvsdParams(std::optional<CvsdParams>& codec_params) {
  // Number of equal bits (run of 1's or 0's), J, should be less than or equal to
  // the number of historic output bits we keep track of.
  codec_params.emplace(CvsdParams{
      .k = kK,
      .j = kJ,
      .equal_bit_mask = static_cast<uint32_t>(pow(2, kJ) - 1),
      .historic_bit_mask = static_cast<uint32_t>(pow(2, kK) - 1),
      .historic_bits = 0,
      .accumulator = 0,
      .step_size = 0,
  });
}