160 lines
4.5KB

  1. /* Audio Library for Teensy 3.X
  2. * Copyright (c) 2014, Paul Stoffregen, paul@pjrc.com
  3. *
  4. * Development of this audio library was funded by PJRC.COM, LLC by sales of
  5. * Teensy and Audio Adaptor boards. Please support PJRC's efforts to develop
  6. * open source software by purchasing Teensy or other PJRC products.
  7. *
  8. * Permission is hereby granted, free of charge, to any person obtaining a copy
  9. * of this software and associated documentation files (the "Software"), to deal
  10. * in the Software without restriction, including without limitation the rights
  11. * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
  12. * copies of the Software, and to permit persons to whom the Software is
  13. * furnished to do so, subject to the following conditions:
  14. *
  15. * The above copyright notice, development funding notice, and this permission
  16. * notice shall be included in all copies or substantial portions of the Software.
  17. *
  18. * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
  19. * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
  20. * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
  21. * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
  22. * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
  23. * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
  24. * THE SOFTWARE.
  25. */
  26. #include <Arduino.h>
  27. #include "mixer.h"
  28. #include "utility/dspinst.h"
  29. #if defined(__ARM_ARCH_7EM__)
  30. #define MULTI_UNITYGAIN 65536
  31. static void applyGain(int16_t *data, int32_t mult)
  32. {
  33. uint32_t *p = (uint32_t *)data;
  34. const uint32_t *end = (uint32_t *)(data + AUDIO_BLOCK_SAMPLES);
  35. do {
  36. uint32_t tmp32 = *p; // read 2 samples from *data
  37. int32_t val1 = signed_multiply_32x16b(mult, tmp32);
  38. int32_t val2 = signed_multiply_32x16t(mult, tmp32);
  39. val1 = signed_saturate_rshift(val1, 16, 0);
  40. val2 = signed_saturate_rshift(val2, 16, 0);
  41. *p++ = pack_16b_16b(val2, val1);
  42. } while (p < end);
  43. }
  44. static void applyGainThenAdd(int16_t *data, const int16_t *in, int32_t mult)
  45. {
  46. uint32_t *dst = (uint32_t *)data;
  47. const uint32_t *src = (uint32_t *)in;
  48. const uint32_t *end = (uint32_t *)(data + AUDIO_BLOCK_SAMPLES);
  49. if (mult == MULTI_UNITYGAIN) {
  50. do {
  51. uint32_t tmp32 = *dst;
  52. *dst++ = signed_add_16_and_16(tmp32, *src++);
  53. tmp32 = *dst;
  54. *dst++ = signed_add_16_and_16(tmp32, *src++);
  55. } while (dst < end);
  56. } else {
  57. do {
  58. uint32_t tmp32 = *src++; // read 2 samples from *data
  59. int32_t val1 = signed_multiply_32x16b(mult, tmp32);
  60. int32_t val2 = signed_multiply_32x16t(mult, tmp32);
  61. val1 = signed_saturate_rshift(val1, 16, 0);
  62. val2 = signed_saturate_rshift(val2, 16, 0);
  63. tmp32 = pack_16b_16b(val2, val1);
  64. uint32_t tmp32b = *dst;
  65. *dst++ = signed_add_16_and_16(tmp32, tmp32b);
  66. } while (dst < end);
  67. }
  68. }
  69. #elif defined(KINETISL)
  70. #define MULTI_UNITYGAIN 256
  71. static void applyGain(int16_t *data, int32_t mult)
  72. {
  73. const int16_t *end = data + AUDIO_BLOCK_SAMPLES;
  74. do {
  75. int32_t val = *data * mult;
  76. *data++ = signed_saturate_rshift(val, 16, 0);
  77. } while (data < end);
  78. }
  79. static void applyGainThenAdd(int16_t *dst, const int16_t *src, int32_t mult)
  80. {
  81. const int16_t *end = dst + AUDIO_BLOCK_SAMPLES;
  82. if (mult == MULTI_UNITYGAIN) {
  83. do {
  84. int32_t val = *dst + *src++;
  85. *dst++ = signed_saturate_rshift(val, 16, 0);
  86. } while (dst < end);
  87. } else {
  88. do {
  89. int32_t val = *dst + ((*src++ * mult) >> 8); // overflow possible??
  90. *dst++ = signed_saturate_rshift(val, 16, 0);
  91. } while (dst < end);
  92. }
  93. }
  94. #endif
  95. void AudioMixer4::update(void)
  96. {
  97. audio_block_t *in, *out=NULL;
  98. unsigned int channel;
  99. for (channel=0; channel < 4; channel++) {
  100. if (!out) {
  101. out = receiveWritable(channel);
  102. if (out) {
  103. int32_t mult = multiplier[channel];
  104. if (mult != MULTI_UNITYGAIN) applyGain(out->data, mult);
  105. }
  106. } else {
  107. in = receiveReadOnly(channel);
  108. if (in) {
  109. applyGainThenAdd(out->data, in->data, multiplier[channel]);
  110. release(in);
  111. }
  112. }
  113. }
  114. if (out) {
  115. transmit(out);
  116. release(out);
  117. }
  118. }
  119. void AudioAmplifier::update(void)
  120. {
  121. audio_block_t *block;
  122. int32_t mult = multiplier;
  123. if (mult == 0) {
  124. // zero gain, discard any input and transmit nothing
  125. block = receiveReadOnly(0);
  126. if (block) release(block);
  127. } else if (mult == MULTI_UNITYGAIN) {
  128. // unity gain, pass input to output without any change
  129. block = receiveReadOnly(0);
  130. if (block) {
  131. transmit(block);
  132. release(block);
  133. }
  134. } else {
  135. // apply gain to signal
  136. block = receiveWritable(0);
  137. if (block) {
  138. applyGain(block->data, mult);
  139. transmit(block);
  140. release(block);
  141. }
  142. }
  143. }