VCVRack
/
Rack
mirror of https://github.com/VCVRack/Rack.git
1
0
Fork 0
Code Issues 0 Releases 44 Wiki Activity
You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
1235 Commits
4 Branches
616MB
Tree: 0f5576f92e
Branches Tags
${ item.name }
Create branch ${ searchTerm }
from '0f5576f92e'
${ noResults }
Rack/plugins/community/repos/AudibleInstruments/eurorack/avrlibx/utils/op.h

784 lines
20KB
Raw Blame History 

  1. // Copyright 2011 Olivier Gillet.

  2. //

  3. // Author: Olivier Gillet (ol.gillet@gmail.com)

  4. //

  5. // This program is free software: you can redistribute it and/or modify

  6. // it under the terms of the GNU General Public License as published by

  7. // the Free Software Foundation, either version 3 of the License, or

  8. // (at your option) any later version.

  9. // This program is distributed in the hope that it will be useful,

  10. // but WITHOUT ANY WARRANTY; without even the implied warranty of

  11. // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the

  12. // GNU General Public License for more details.

  13. // You should have received a copy of the GNU General Public License

  14. // along with this program.  If not, see <http://www.gnu.org/licenses/>.

  15. //

  16. // -----------------------------------------------------------------------------

  17. //

  18. // A set of basic operands, especially useful for fixed-point arithmetic, with

  19. // fast ASM implementations.

  20. 

  21. #ifndef AVRLIBX_UTILS_OP_H_

  22. #define AVRLIBX_UTILS_OP_H_

  23. 

  24. #define USE_OPTIMIZED_OP

  25. 

  26. #include <avr/pgmspace.h>

  27. 

  28. #include "avrlibx/avrlibx.h"

  29. 

  30. namespace avrlibx {

  31. 

  32. static inline int16_t Clip(int16_t value, int16_t min, int16_t max) {

  33.   return value < min ? min : (value > max ? max : value);

  34. }

  35. 

  36. static inline int16_t S16ClipU14(int16_t value) {

  37.   uint8_t msb = static_cast<uint16_t>(value) >> 8;

  38.   if (msb & 0x80) {

  39.     return 0;

  40.   } if (msb & 0x40) {

  41.     return 16383;

  42.   }

  43.   return value;

  44. }

  45. 

  46. static inline uint8_t U8AddClip(uint8_t value, uint8_t increment, uint8_t max) {

  47.   value += increment;

  48.   if (value > max) {

  49.     value = max;

  50.   }

  51.   return value;

  52. }

  53. 

  54. // Correct only if the input is positive.

  55. static inline uint8_t S16ShiftRight8(int16_t value) {

  56.   return static_cast<uint16_t>(value) >> 8;

  57. }

  58. 

  59. #ifdef USE_OPTIMIZED_OP

  60. 

  61. static inline uint24c_t U24AddC(uint24c_t a, uint24_t b) {

  62.   uint16_t a_int = a.integral;

  63.   uint16_t b_int = b.integral;

  64.   uint8_t a_frac = a.fractional;

  65.   uint8_t b_frac = b.fractional;

  66.   uint8_t a_carry = 0;

  67.   uint24c_t result;

  68.   asm(

  69.     "add %0, %6"      "\n\t"

  70.     "adc %A1, %A7"    "\n\t"

  71.     "adc %B1, %B7"    "\n\t"

  72.     "adc %2, r1"      "\n\t"

  73.     : "=r" (a_frac), "=r" (a_int), "=r" (a_carry)

  74.     : "0" (a_frac), "1" (a_int), "2" (a_carry), "a" (b_frac), "a" (b_int)

  75.   );

  76.   result.integral = a_int;

  77.   result.fractional = a_frac;

  78.   result.carry = a_carry;

  79.   return result;

  80. }

  81. 

  82. static inline uint24_t U24Add(uint24_t a, uint24_t b) {

  83.   uint16_t a_int = a.integral;

  84.   uint16_t b_int = b.integral;

  85.   uint8_t a_frac = a.fractional;

  86.   uint8_t b_frac = b.fractional;

  87.   uint24_t result;

  88.   asm(

  89.     "add %0, %4"      "\n\t"

  90.     "adc %A1, %A5"    "\n\t"

  91.     "adc %B1, %B5"    "\n\t"

  92.     : "=r" (a_frac), "=r" (a_int)

  93.     : "0" (a_frac), "1" (a_int), "a" (b_frac), "a" (b_int)

  94.   );

  95.   result.integral = a_int;

  96.   result.fractional = a_frac;

  97.   return result;

  98. }

  99. 

  100. static inline uint24_t U24Sub(uint24_t a, uint24_t b) {

  101.   uint16_t a_int = a.integral;

  102.   uint16_t b_int = b.integral;

  103.   uint8_t a_frac = a.fractional;

  104.   uint8_t b_frac = b.fractional;

  105.   uint24_t result;

  106.   asm(

  107.     "sub %0, %4"      "\n\t"

  108.     "sbc %A1, %A5"    "\n\t"

  109.     "sbc %B1, %B5"    "\n\t"

  110.     : "=r" (a_frac), "=r" (a_int)

  111.     : "0" (a_frac), "1" (a_int), "a" (b_frac), "a" (b_int)

  112.   );

  113.   result.integral = a_int;

  114.   result.fractional = a_frac;

  115.   return result;

  116. }

  117. 

  118. static inline uint24_t U24ShiftRight(uint24_t a) {

  119.   uint16_t a_int = a.integral;

  120.   uint8_t a_frac = a.fractional;

  121.   uint24_t result;

  122.   asm(

  123.     "lsr %B1"      "\n\t"

  124.     "ror %A1"    "\n\t"

  125.     "ror %0"    "\n\t"

  126.     : "=r" (a_frac), "=r" (a_int)

  127.     : "0" (a_frac), "1" (a_int)

  128.   );

  129.   result.integral = a_int;

  130.   result.fractional = a_frac;

  131.   return result;

  132. }

  133. 

  134. static inline uint24_t U24ShiftLeft(uint24_t a) {

  135.   uint16_t a_int = a.integral;

  136.   uint8_t a_frac = a.fractional;

  137.   uint24_t result;

  138.   asm(

  139.     "lsl %0"    "\n\t"

  140.     "rol %A1"    "\n\t"

  141.     "rol %B1"      "\n\t"

  142.     : "=r" (a_frac), "=r" (a_int)

  143.     : "0" (a_frac), "1" (a_int)

  144.   );

  145.   result.integral = a_int;

  146.   result.fractional = a_frac;

  147.   return result;

  148. }

  149. 

  150. static inline uint8_t S16ClipU8(int16_t value) {

  151.   uint8_t result;

  152.   asm(

  153.     "mov %0, %A1"   "\n\t"  // by default, copy the value.

  154.     "or %B1, %B1"   "\n\t"  // load H to set flags.

  155.     "brpl .+4"      "\n\t"  // if positive, skip

  156.     "ldi %0, 0"     "\n\t"  // set to 0.

  157.     "rjmp .+4"      "\n\t"  // and jump.

  158.     "breq .+2"      "\n\t"  // if null, skip

  159.     "ldi %0, 255"   "\n\t"  // set to 255

  160.     : "=r" (result)

  161.     : "a" (value)

  162.   );

  163.   return result;

  164. }

  165. 

  166. static inline int8_t S16ClipS8(int16_t value) {

  167.   return S16ClipU8(value + 128) + 128;

  168. }

  169. 

  170. static inline uint8_t U8Mix(uint8_t a, uint8_t b, uint8_t balance) {

  171.   Word sum;

  172.   asm(

  173.     "mul %3, %2"      "\n\t"  // b * balance

  174.     "movw %A0, r0"    "\n\t"  // to sum

  175.     "com %2"          "\n\t"  // 255 - balance

  176.     "mul %1, %2"      "\n\t"  // a * (255 - balance)

  177.     "com %2"          "\n\t"  // reset balance to its previous value

  178.     "add %A0, r0"     "\n\t"  // add to sum L

  179.     "adc %B0, r1"     "\n\t"  // add to sum H

  180.     "eor r1, r1"      "\n\t"  // reset r1 after multiplication

  181.     : "&=r" (sum)

  182.     : "a" (a), "a" (balance), "a" (b)

  183.     );

  184.   return sum.bytes[1];

  185. }

  186. 

  187. static inline uint8_t U8Mix(

  188.     uint8_t a, uint8_t b,

  189.     uint8_t gain_a, uint8_t gain_b) {

  190.   Word sum;

  191.   asm(

  192.     "mul %3, %4"      "\n\t"  // b * gain_b

  193.     "movw %A0, r0"    "\n\t"  // to sum

  194.     "mul %1, %2"      "\n\t"  // a * gain_a

  195.     "add %A0, r0"     "\n\t"  // add to sum L

  196.     "adc %B0, r1"     "\n\t"  // add to sum H

  197.     "eor r1, r1"      "\n\t"  // reset r1 after multiplication

  198.     : "&=r" (sum)

  199.     : "a" (a), "a" (gain_a), "a" (b), "a" (gain_b)

  200.     );

  201.   return sum.bytes[1];

  202. }

  203. 

  204. static inline int8_t S8Mix(

  205.     int8_t a, int8_t b,

  206.     uint8_t gain_a, uint8_t gain_b) {

  207.   Word sum;

  208.   asm(

  209.     "mulsu %3, %4"    "\n\t"  // b * gain_b

  210.     "movw %A0, r0"    "\n\t"  // to sum

  211.     "mulsu %1, %2"    "\n\t"  // a * gain_a

  212.     "add %A0, r0"     "\n\t"  // add to sum L

  213.     "adc %B0, r1"     "\n\t"  // add to sum H

  214.     "eor r1, r1"      "\n\t"  // reset r1 after multiplication

  215.     : "&=r" (sum)

  216.     : "a" (a), "a" (gain_a), "a" (b), "a" (gain_b)

  217.     );

  218.   return sum.bytes[1];

  219. }

  220. 

  221. static inline uint16_t U8MixU16(uint8_t a, uint8_t b, uint8_t balance) {

  222.   Word sum;

  223.   asm(

  224.     "mul %3, %2"      "\n\t"  // b * balance

  225.     "movw %A0, r0"    "\n\t"  // to sum

  226.     "com %2"          "\n\t"  // 255 - balance

  227.     "mul %1, %2"      "\n\t"  // a * (255 - balance)

  228.     "com %2"          "\n\t"  // reset balance to its previous value

  229.     "add %A0, r0"     "\n\t"  // add to sum L

  230.     "adc %B0, r1"     "\n\t"  // add to sum H

  231.     "eor r1, r1"      "\n\t"  // reset r1 after multiplication

  232.     : "&=r" (sum)

  233.     : "a" (a), "a" (balance), "a" (b)

  234.     );

  235.   return sum.value;

  236. }

  237. 

  238. static inline uint8_t U8U4MixU8(uint8_t a, uint8_t b, uint8_t balance) {

  239.   uint16_t sum;

  240.   asm(

  241.     "mul %2, %1"      "\n\t"  // b * balance

  242.     "movw %A3, r0"    "\n\t"  // to sum

  243.     "com %1"          "\n\t"  // 255 - balance

  244.     "subi %1, 240"    "\n\t"  // 15 - balance

  245.     "mul %0, %1"      "\n\t"  // a * (15 - balance)

  246.     "subi %1, 16"     "\n\t"

  247.     "com %1"          "\n\t"  // reset balance to its previous value

  248.     "add %A3, r0"     "\n\t"  // add to sum L

  249.     "adc %B3, r1"     "\n\t"  // add to sum H

  250.     "eor r1, r1"      "\n\t"  // reset r1 after multiplication

  251.     "andi %B3, 15"    "\n\t"  // keep 4 lowest bits of H

  252.     "andi %A3, 240"   "\n\t"  // keep 4 highest bits of L 

  253.     "or %B3, %A3"     "\n\t"  // copy 4 high bits of L to H -> LLLLHHHH

  254.     "swap %B3"        "\n\t"  // swap to get HHHHLLLL

  255.     "mov %0, %B3"     "\n\t"  // move to output

  256.     : "=r" (a)

  257.     : "a" (balance), "a" (b), "a" (sum)

  258.     );

  259.   return a;

  260. }

  261. 

  262. static inline uint16_t U8U4MixU12(uint8_t a, uint8_t b, uint8_t balance) {

  263.   uint16_t sum;

  264.   asm(

  265.     "mul %3, %2"      "\n\t"  // b * balance

  266.     "movw %A0, r0"    "\n\t"  // to sum

  267.     "com %2"          "\n\t"  // 255 - balance

  268.     "subi %2, 240"    "\n\t"  // 15 - balance

  269.     "mul %1, %2"      "\n\t"  // a * (15 - balance)

  270.     "subi %2, 16"     "\n\t"

  271.     "com %2"          "\n\t"  // reset balance to its previous value

  272.     "add %A0, r0"     "\n\t"  // add to sum L

  273.     "adc %B0, r1"     "\n\t"  // add to sum H

  274.     "eor r1, r1"      "\n\t"  // reset r1 after multiplication

  275.     : "&=r" (sum)

  276.     : "a" (a), "a" (balance), "a" (b)

  277.     );

  278.   return sum;

  279. }

  280. 

  281. static inline uint8_t U8ShiftLeft4(uint8_t a) {

  282.   uint8_t result;

  283.   asm(

  284.     "mov %0, %1"      "\n\t"

  285.     "swap %0"         "\n\t"

  286.     "andi %0, 240"     "\n\t"

  287.     : "=r" (result)

  288.     : "a" (a)

  289.     );

  290.   return result;

  291. }

  292. 

  293. static inline uint8_t U8Swap4(uint8_t a) {

  294.   uint8_t result;

  295.   asm(

  296.     "mov %0, %1"      "\n\t"

  297.     "swap %0"         "\n\t"

  298.     : "=r" (result)

  299.     : "a" (a)

  300.     );

  301.   return result;

  302. }

  303. 

  304. static inline uint8_t U8ShiftRight4(uint8_t a) {

  305.   uint8_t result;

  306.   asm(

  307.     "mov %0, %1"      "\n\t"

  308.     "swap %0"         "\n\t"

  309.     "andi %0, 15"     "\n\t"

  310.     : "=r" (result)

  311.     : "a" (a)

  312.     );

  313.   return result;

  314. }

  315. 

  316. static inline uint16_t U16ShiftRight4(uint16_t a) {

  317.   uint16_t result;

  318.   asm(

  319.     "movw %A0, %A1" "\n\t"

  320.     "lsr %B0"      "\n\t"

  321.     "ror %A0"      "\n\t"

  322.     "lsr %B0"      "\n\t"

  323.     "ror %A0"      "\n\t"

  324.     "lsr %B0"      "\n\t"

  325.     "ror %A0"      "\n\t"

  326.     "lsr %B0"      "\n\t"

  327.     "ror %A0"      "\n\t"

  328.     : "=r" (result)

  329.     : "a" (a)

  330.     );

  331.   return result;

  332. }

  333. 

  334. 

  335. static inline uint8_t U8U8MulShift8(uint8_t a, uint8_t b) {

  336.   uint8_t result;

  337.   asm(

  338.     "mul %1, %2"      "\n\t"

  339.     "mov %0, r1"      "\n\t"

  340.     "eor r1, r1"      "\n\t"

  341.     : "=r" (result)

  342.     : "a" (a), "a" (b)

  343.   );

  344.   return result;

  345. }

  346. 

  347. static inline int8_t S8U8MulShift8(int8_t a, uint8_t b) {

  348.   uint8_t result;

  349.   asm(

  350.     "mulsu %1, %2"    "\n\t"

  351.     "mov %0, r1"      "\n\t"

  352.     "eor r1, r1"      "\n\t"

  353.     : "=r" (result)

  354.     : "a" (a), "a" (b)

  355.   );

  356.   return result;

  357. }

  358. 

  359. static inline int16_t S8U8Mul(int8_t a, uint8_t b) {

  360.   int16_t result;

  361.   asm(

  362.     "mulsu %1, %2"    "\n\t"

  363.     "movw %0, r0"      "\n\t"

  364.     "eor r1, r1"      "\n\t"

  365.     : "=r" (result)

  366.     : "a" (a), "a" (b)

  367.   );

  368.   return result;

  369. }

  370. 

  371. static inline int16_t S8S8Mul(int8_t a, int8_t b) {

  372.   int16_t result;

  373.   asm(

  374.     "muls %1, %2"    "\n\t"

  375.     "movw %0, r0"      "\n\t"

  376.     "eor r1, r1"      "\n\t"

  377.     : "=r" (result)

  378.     : "a" (a), "a" (b)

  379.   );

  380.   return result;

  381. }

  382. 

  383. static inline uint16_t U8U8Mul(uint8_t a, uint8_t b) {

  384.   uint16_t result;

  385.   asm(

  386.     "mul %1, %2"    "\n\t"

  387.     "movw %0, r0"      "\n\t"

  388.     "eor r1, r1"      "\n\t"

  389.     : "=r" (result)

  390.     : "a" (a), "a" (b)

  391.   );

  392.   return result;

  393. }

  394. 

  395. static inline int8_t S8S8MulShift8(int8_t a, int8_t b) {

  396.   uint8_t result;

  397.   asm(

  398.     "muls %1, %2"    "\n\t"

  399.     "mov %0, r1"      "\n\t"

  400.     "eor r1, r1"      "\n\t"

  401.     : "=r" (result)

  402.     : "a" (a), "a" (b)

  403.   );

  404.   return result;

  405. }

  406. 

  407. // The code generated by gcc for >> 6 is short but uses a loop. This saves

  408. // a couple of cycles. Note that this solution only works for operands with

  409. // a 14-bits resolution.

  410. static inline uint8_t U14ShiftRight6(uint16_t value) {

  411.   uint8_t b = value >> 8;

  412.   uint8_t a = value & 0xff;

  413.   uint8_t result;

  414.   asm(

  415.     "add %1, %1"       "\n\t"

  416.     "adc %2, %2"       "\n\t"

  417.     "add %1, %1"       "\n\t"

  418.     "adc %2, %2"       "\n\t"

  419.     : "=r" (result)

  420.     : "a" (a), "0" (b)

  421.   );

  422.   return result;

  423. }

  424. 

  425. static inline uint8_t U15ShiftRight7(uint16_t value) {

  426.   uint8_t b = value >> 8;

  427.   uint8_t a = value & 0xff;

  428.   uint8_t result;

  429.   asm(

  430.     "add %1, %1"       "\n\t"

  431.     "adc %2, %2"       "\n\t"

  432.     : "=r" (result)

  433.     : "a" (a), "0" (b)

  434.   );

  435.   return result;

  436. }

  437. 

  438. static inline int16_t S16U16MulShift16(int16_t a, uint16_t b) {

  439.   int16_t result;

  440.   int16_t tmp;

  441.   asm(

  442.     "eor %A1, %A1"    "\n\t"

  443.     "mul %A2, %A3"    "\n\t"

  444.     "mov %B1, r1"    "\n\t"

  445.     "mulsu %B2, %B3"  "\n\t"

  446.     "movw %A0, r0"    "\n\t"

  447.     "mul %B3, %A2"    "\n\t"

  448.     "add %B1, r0"     "\n\t"

  449.     "adc %A0, r1"     "\n\t"

  450.     "adc %B0, %A1"    "\n\t"

  451.     "mulsu %B2, %A3"  "\n\t"

  452.     "sbc %B0, %A1"    "\n\t"

  453.     "add %B1, r0"     "\n\t"

  454.     "adc %A0, r1"     "\n\t"

  455.     "adc %B0, %A1"    "\n\t"

  456.     "eor r1, r1"      "\n\t"

  457.     : "=&r" (result), "=&r" (tmp)

  458.     : "a" (a), "a" (b)

  459.   );

  460.   return result;

  461. }

  462. 

  463. static inline int16_t S16U12MulShift12(int16_t a, uint16_t b) {

  464.   int16_t result;

  465.   int16_t tmp;

  466.   asm(

  467.     "add %A3, %A3"    "\n\t"

  468.     "adc %B3, %B3"    "\n\t"

  469.     "add %A3, %A3"    "\n\t"

  470.     "adc %B3, %B3"    "\n\t"

  471.     "add %A3, %A3"    "\n\t"

  472.     "adc %B3, %B3"    "\n\t"

  473.     "add %A3, %A3"    "\n\t"

  474.     "adc %B3, %B3"    "\n\t"

  475.     "eor %A1, %A1"    "\n\t"

  476.     "mul %A2, %A3"    "\n\t"

  477.     "mov %B1, r1"    "\n\t"

  478.     "mulsu %B2, %B3"  "\n\t"

  479.     "movw %A0, r0"    "\n\t"

  480.     "mul %B3, %A2"    "\n\t"

  481.     "add %B1, r0"     "\n\t"

  482.     "adc %A0, r1"     "\n\t"

  483.     "adc %B0, %A1"    "\n\t"

  484.     "mulsu %B2, %A3"  "\n\t"

  485.     "sbc %B0, %A1"    "\n\t"

  486.     "add %B1, r0"     "\n\t"

  487.     "adc %A0, r1"     "\n\t"

  488.     "adc %B0, %A1"    "\n\t"

  489.     "eor r1, r1"      "\n\t"

  490.     : "=&r" (result), "=&r" (tmp)

  491.     : "a" (a), "a" (b)

  492.   );

  493.   return result;

  494. }

  495. 

  496. static inline uint16_t U16U16MulShift16(uint16_t a, uint16_t b) {

  497.   uint16_t result;

  498.   uint16_t tmp;

  499.   asm(

  500.     "eor %A1, %A1"    "\n\t"

  501.     "mul %A2, %A3"    "\n\t"

  502.     "mov %B1, r1"     "\n\t"

  503.     "mul %B2, %B3"    "\n\t"

  504.     "movw %A0, r0"    "\n\t"

  505.     "mul %B3, %A2"    "\n\t"

  506.     "add %B1, r0"     "\n\t"

  507.     "adc %A0, r1"     "\n\t"

  508.     "adc %B0, %A1"    "\n\t"

  509.     "mul %B2, %A3"    "\n\t"

  510.     "add %B1, r0"     "\n\t"

  511.     "adc %A0, r1"     "\n\t"

  512.     "adc %B0, %A1"    "\n\t"

  513.     "eor r1, r1"      "\n\t"

  514.     : "=&r" (result), "=&r" (tmp)

  515.     : "a" (a), "a" (b)

  516.   );

  517.   return result;

  518. }

  519. 

  520. static inline int16_t S16U8MulShift8(int16_t a, uint8_t b) {

  521.   int16_t result;

  522.   asm(

  523.     "eor %B0, %B0"    "\n\t"

  524.     "mul %A1, %A2"    "\n\t"

  525.     "mov %A0, r1"     "\n\t"

  526.     "mulsu %B1, %A2"  "\n\t"

  527.     "add %A0, r0"     "\n\t"

  528.     "adc %B0, r1"     "\n\t"

  529.     "eor r1, r1"      "\n\t"

  530.     : "=&r" (result)

  531.     : "a" (a), "a" (b)

  532.   );

  533.   return result;

  534. }

  535. 

  536. static inline uint16_t U16U8MulShift8(uint16_t a, uint8_t b) {

  537.   uint16_t result;

  538.   asm(

  539.     "eor %B0, %B0"    "\n\t"

  540.     "mul %A1, %A2"    "\n\t"

  541.     "mov %A0, r1"     "\n\t"

  542.     "mul %B1, %A2"  "\n\t"

  543.     "add %A0, r0"     "\n\t"

  544.     "adc %B0, r1"     "\n\t"

  545.     "eor r1, r1"      "\n\t"

  546.     : "=&r" (result)

  547.     : "a" (a), "a" (b)

  548.   );

  549.   return result;

  550. }

  551. 

  552. static inline int16_t S16S8MulShift8(int16_t a, int8_t b) {

  553.   int16_t result;

  554.   asm(

  555.     "eor %B0, %B0"    "\n\t"

  556.     "muls %A2, %B1"   "\n\t"

  557.     "movw %A0, r0"    "\n\t"

  558.     "mulsu %A2, %A1"  "\n\t"

  559.     "eor r0, r0"      "\n\t"

  560.     "sbc %B0, r0"     "\n\t"

  561.     "add %A0, r1"     "\n\t"

  562.     "adc %B0, r0"     "\n\t"

  563.     "eor r1, r1"      "\n\t"

  564.     : "=&r" (result)

  565.     : "a" (a), "a" (b)

  566.   );

  567.   return result;

  568. }

  569. 

  570. static inline uint8_t InterpolateSample(

  571.     const prog_uint8_t* table,

  572.     uint16_t phase) __attribute__((always_inline));

  573. 

  574. static inline uint8_t InterpolateSample(

  575.     const prog_uint8_t* table,

  576.     uint16_t phase) {

  577.   uint8_t result;

  578.   uint8_t work;

  579.   asm(

  580.     "movw r30, %A2"           "\n\t"  // copy base address to r30:r31

  581.     "add r30, %B3"            "\n\t"  // increment table address by phaseH

  582.     "adc r31, r1"             "\n\t"  // just carry

  583.     "mov %1, %A3"             "\n\t"  // move phaseL to working register

  584.     "lpm %0, z+"              "\n\t"  // load sample[n]

  585.     "lpm r1, z+"              "\n\t"  // load sample[n+1]

  586.     "mul %1, r1"              "\n\t"  // multiply second sample by phaseL

  587.     "movw r30, r0"            "\n\t"  // result to accumulator

  588.     "com %1"                  "\n\t"  // 255 - phaseL -> phaseL

  589.     "mul %1, %0"              "\n\t"  // multiply first sample by phaseL

  590.     "add r30, r0"             "\n\t"  // accumulate L

  591.     "adc r31, r1"             "\n\t"  // accumulate H

  592.     "eor r1, r1"              "\n\t"  // reset r1 after multiplication

  593.     "mov %0, r31"             "\n\t"  // use sum H as output

  594.     : "=r" (result), "=r" (work)

  595.     : "r" (table), "r" (phase)

  596.     : "r30", "r31"

  597.   );

  598.   return result;

  599. }

  600. 

  601. #else

  602. 

  603. static inline uint24c_t U24AddC(uint24c_t a, uint24_t b) {

  604.   uint24c_t result;

  605. 

  606.   uint32_t av = static_cast<uint32_t>(a.integral) << 8;

  607.   av += a.fractional;

  608. 

  609.   uint32_t bv = static_cast<uint32_t>(b.integral) << 8;

  610.   bv += b.fractional;

  611. 

  612.   uint32_t sum = av + bv;

  613.   result.integral = sum >> 8;

  614.   result.fractional = sum & 0xff;

  615.   result.carry = (sum & 0xff000000) != 0;

  616.   return result;

  617. }

  618. 

  619. static inline uint24_t U24Add(uint24_t a, uint24_t b) {

  620.   uint24_t result;

  621. 

  622.   uint32_t av = static_cast<uint32_t>(a.integral) << 8;

  623.   av += a.fractional;

  624. 

  625.   uint32_t bv = static_cast<uint32_t>(b.integral) << 8;

  626.   bv += b.fractional;

  627. 

  628.   uint32_t sum = av + bv;

  629.   result.integral = sum >> 8;

  630.   result.fractional = sum & 0xff;

  631.   return result;

  632. }

  633. 

  634. static inline uint24_t U24Sub(uint24_t a, uint24_t b) {

  635.   uint24_t result;

  636. 

  637.   uint32_t av = static_cast<uint32_t>(a.integral) << 8;

  638.   av += a.fractional;

  639. 

  640.   uint32_t bv = static_cast<uint32_t>(b.integral) << 8;

  641.   bv += b.fractional;

  642. 

  643.   uint32_t difference = av - bv;

  644.   result.integral = sum >> 8;

  645.   result.fractional = sum & 0xff;

  646.   return result;

  647. }

  648. 

  649. static inline uint24_t U24ShiftRight(uint24_t a) {

  650.   uint24_t result;

  651.   uint32_t av = static_cast<uint32_t>(a.integral) << 8;

  652.   av += a.fractional;

  653.   av >>= 1;

  654.   result.integral = av >> 8;

  655.   result.fractional = av & 0xff;

  656.   return result;

  657. }

  658. 

  659. static inline uint24_t U24ShiftLeft(uint24_t a) {

  660.   uint24_t result;

  661.   uint32_t av = static_cast<uint32_t>(a.integral) << 8;

  662.   av += a.fractional;

  663.   av <<= 1;

  664.   result.integral = av >> 8;

  665.   result.fractional = av & 0xff;

  666.   return result;

  667. }

  668. 

  669. static inline uint8_t S16ClipU8(int16_t value) {

  670.   return value < 0 ? 0 : (value > 255 ? 255 : value);

  671. }

  672. 

  673. static inline int8_t S16ClipS8(int16_t value) {

  674.   return value < -128 ? -128 : (value > 127 ? 127 : value);

  675. }

  676. 

  677. static inline uint8_t U8Mix(uint8_t a, uint8_t b, uint8_t balance) {

  678.   return a * (255 - balance) + b * balance >> 8;

  679. }

  680. 

  681. static inline uint8_t U8Mix(uint8_t a, uint8_t b, uint8_t gain_a, uint8_t gain_b) {

  682.   return a * gain_a + b * gain_b >> 8;

  683. }

  684. 

  685. static inline int8_t S8Mix(

  686.     int8_t a, int8_t b,

  687.     uint8_t gain_a, uint8_t gain_b) {

  688.   return a * gain_a + b * gain_b >> 8;

  689. }

  690. 

  691. static inline uint16_t U8MixU16(uint8_t a, uint8_t b, uint8_t balance) {

  692.   return a * (255 - balance) + b * balance;

  693. }

  694. 

  695. static inline uint8_t U8U4MixU8(uint8_t a, uint8_t b, uint8_t balance) {

  696.   return (a * (15 - balance) + b * balance) >> 4;

  697. }

  698. 

  699. static inline uint16_t U8U4MixU12(uint8_t a, uint8_t b, uint8_t balance) {

  700.   return a * (15 - balance) + b * balance;

  701. }

  702. 

  703. static inline uint8_t U8ShiftRight4(uint8_t a) {

  704.   return a >> 4;

  705. }

  706. 

  707. static inline uint8_t U8ShiftLeft4(uint8_t a) {

  708.   return a << 4;

  709. }

  710. 

  711. static inline uint8_t U8Swap4(uint8_t a) {

  712.   return (a << 4) | (a >> 4);

  713. }

  714. 

  715. static inline uint8_t U8U8MulShift8(uint8_t a, uint8_t b) {

  716.   return a * b >> 8;

  717. }

  718. 

  719. static inline int8_t S8U8MulShift8(int8_t a, uint8_t b) {

  720.   return a * b >> 8;

  721. }

  722. 

  723. static inline int16_t S8U8Mul(int8_t a, uint8_t b) {

  724.   return a * b;

  725. }

  726. 

  727. static inline int16_t S8S8Mul(int8_t a, int8_t b) {

  728.   return a * b;

  729. }

  730. 

  731. static inline uint16_t U8U8Mul(uint8_t a, uint8_t b) {

  732.   return a * b;

  733. }

  734. 

  735. static inline int8_t S8S8MulShift8(int8_t a, int8_t b) {

  736.   return a * b >> 8;

  737. }

  738. 

  739. static inline uint16_t Mul16Scale8(uint16_t a, uint16_t b) {

  740.   return static_cast<uint32_t>(a) * b >> 8;

  741. }

  742. 

  743. static inline uint8_t U14ShiftRight6(uint16_t value) {

  744.   return value >> 6;

  745. }

  746. 

  747. static inline uint8_t U15ShiftRight7(uint16_t value) {

  748.   return value >> 7;

  749. }

  750. 

  751. static inline uint16_t U16ShiftRight4(uint16_t a) {

  752.   return a >> 4;

  753. }

  754. 

  755. static inline int16_t S16U16MulShift16(int16_t a, uint16_t b) {

  756.   return (static_cast<int32_t>(a) * static_cast<uint32_t>(b)) >> 16;

  757. }

  758. 

  759. static inline int12_t S16U12MulShift12(int16_t a, uint16_t b) {

  760.   return (static_cast<int32_t>(a) * static_cast<uint32_t>(b)) >> 12;

  761. }

  762. 

  763. static inline int16_t S16U8MulShift8(int16_t a, uint8_t b) {

  764.   return (static_cast<int32_t>(a) * static_cast<uint32_t>(b)) >> 8;

  765. }

  766. 

  767. static inline uint16_t U16U8MulShift8(uint16_t a, uint8_t b) {

  768.   return (static_cast<uint32_t>(a) * static_cast<uint32_t>(b)) >> 8;

  769. }

  770. 

  771. static inline uint8_t InterpolateSample(

  772.     const prog_uint8_t* table,

  773.     uint16_t phase) {

  774.   return U8Mix(

  775.       pgm_read_byte(table + (phase >> 8)),

  776.       pgm_read_byte(1 + table + (phase >> 8)),

  777.       phase & 0xff);

  778. }

  779. 

  780. #endif  // USE_OPTIMIZED_OP

  781. 

  782. }  // namespace avrlibx

  783. 

  784. #endif  // AVRLIBX_UTILS_OP_H_