A2DPVolumeControl.h

#pragma once
 
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
 
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
//
// Copyright 2020 Phil Schatzmann
// Copyright 2015-2016 Espressif Systems (Shanghai) PTE LTD
 
#include "esp_log.h"
 
    struct __attribute__((packed)) Frame {
  int16_t channel1;  
  int16_t channel2;  
 
  Frame(int v = 0) { channel1 = channel2 = v; }
 
  Frame(int ch1, int ch2) {
    channel1 = ch1;
    channel2 = ch2;
  }
};
 
class A2DPVolumeControl {
 public:
  A2DPVolumeControl() = default;
 
  virtual void update_audio_data(uint8_t* data, uint16_t byteCount) {
    update_audio_data((Frame*)data, byteCount / 4);
  }
 
  virtual void update_audio_data(Frame* data, uint16_t frameCount) {
    if (data != nullptr && frameCount > 0 && (mono_downmix || is_volume_used)) {
      ESP_LOGD("VolumeControl", "update_audio_data");
      for (int i = 0; i < frameCount; i++) {
        int32_t pcmLeft = data[i].channel1;
        int32_t pcmRight = data[i].channel2;
        // if mono -> we provide the same output on both channels
        if (mono_downmix) {
          pcmRight = pcmLeft = (pcmLeft + pcmRight) / 2;
        }
        // adjust the volume
        if (is_volume_used) {
          pcmLeft = clip(pcmLeft * volumeFactor / volumeFactorMax);
          pcmRight = clip(pcmRight * volumeFactor / volumeFactorMax);
        }
        data[i].channel1 = pcmLeft;
        data[i].channel2 = pcmRight;
      }
    }
  }
 
  int32_t get_volume_factor() { return volumeFactor; }
 
  int32_t get_volume_factor_max() { return volumeFactorMax; }
 
  void set_enabled(bool enabled) { is_volume_used = enabled; }
 
  void set_mono_downmix(bool enabled) { mono_downmix = enabled; }
 
  virtual void set_volume(uint8_t volume) = 0;
 
 protected:
  bool is_volume_used = false;  
  bool mono_downmix = false;    
  int32_t volumeFactor = 1;     
  int32_t volumeFactorMax = 0x1000;     
  int32_t volumeFactorClippingLimit = 0xfff;  
 
  int32_t clip(int32_t value) {
    int32_t result = value;
    if (value < -32768) result = -32768;
    if (value > 32767) result = 32767;
    return result;
  }
};
 
class A2DPDefaultVolumeControl : public A2DPVolumeControl {
 public:
  A2DPDefaultVolumeControl() = default;
 
  A2DPDefaultVolumeControl(int32_t limit) {
    assert(limit < volumeFactorMax);
    volumeFactorClippingLimit = limit;
  };
 
 protected:
  void set_volume(uint8_t volume) override {
    constexpr float base = 1.4f;
    constexpr float bits = 12.0f;
    constexpr float zero_ofs = pow(base, -bits);
    constexpr float scale = pow(2.0f, bits);
    float volumeFactorFloat =
        (pow(base, volume * bits / 127.0f - bits) - zero_ofs) * scale /
        (1.0f - zero_ofs);
    volumeFactor = volumeFactorFloat;
    if (volumeFactor > volumeFactorClippingLimit) {
      volumeFactor = volumeFactorClippingLimit;
    }
  }
};
 
class A2DPSimpleExponentialVolumeControl : public A2DPVolumeControl {
 public:
  A2DPSimpleExponentialVolumeControl() = default;
 
  A2DPSimpleExponentialVolumeControl(int32_t limit) {
    assert(limit < volumeFactorMax);
    volumeFactorClippingLimit = limit;
  };
 
 protected:
  void set_volume(uint8_t volume) override {
    float volumeFactorFloat = volume;
    volumeFactorFloat = pow(2.0f, volumeFactorFloat * 12.0f / 127.0f);
    volumeFactor = volumeFactorFloat - 1.0f;
    if (volumeFactor > volumeFactorClippingLimit) {
      volumeFactor = volumeFactorClippingLimit;
    }
  }
};
 
class A2DPLinearVolumeControl : public A2DPVolumeControl {
 public:
  A2DPLinearVolumeControl() { volumeFactorMax = 128; }
 
 protected:
  void set_volume(uint8_t volume) override { volumeFactor = volume; }
};
 
class A2DPNoVolumeControl : public A2DPVolumeControl {
 public:
  A2DPNoVolumeControl(int32_t fixedVolume = 0x1000) {
    is_volume_used = fixedVolume != 0x1000;
    mono_downmix = false;
    volumeFactor = fixedVolume;  // fixed volume
    volumeFactorMax = 0x1000;  // no change
  }
  
  void update_audio_data(Frame* data, uint16_t frameCount) override {}
 
  void set_volume(uint8_t volume) override {}
};