Added wavetable interpolation function

op.h

@@ -21,6 +21,8 @@
 #ifndef AVRLIB_OP_H_
 #define AVRLIB_OP_H_
 
+#include <avr/pgmspace.h>
+
 #include "avrlib/base.h"
 
 namespace avrlib {
@@ -389,6 +391,37 @@ static inline uint8_t ShiftRight7(uint16_t value) {
   return result;
 }
 
+static inline uint8_t InterpolateSample(
+    const prog_uint8_t* table,
+    uint16_t phase) __attribute__((always_inline));
+
+static inline uint8_t InterpolateSample(
+    const prog_uint8_t* table,
+    uint16_t phase) {
+  uint8_t result;
+  uint8_t work;
+  asm(
+    "movw r30, %A2"           "\n\t"  // copy base address to r30:r31
+    "add r30, %B3"            "\n\t"  // increment table address by phaseH
+    "adc r31, r1"             "\n\t"  // just carry
+    "mov %1, %A3"             "\n\t"  // move phaseL to working register
+    "lpm %0, z+"              "\n\t"  // load sample[n]
+    "lpm r1, z+"              "\n\t"  // load sample[n+1]
+    "mul %1, r1"              "\n\t"  // multiply second sample by phaseL
+    "movw r30, r0"            "\n\t"  // result to accumulator
+    "com %1"                  "\n\t"  // 255 - phaseL -> phaseL
+    "mul %1, %0"              "\n\t"  // multiply first sample by phaseL
+    "add r30, r0"             "\n\t"  // accumulate L
+    "adc r31, r1"             "\n\t"  // accumulate H
+    "eor r1, r1"              "\n\t"  // reset r1 after multiplication
+    "mov %0, r31"             "\n\t"  // use sum H as output
+    : "=r" (result), "=r" (work)
+    : "r" (table), "r" (phase)
+    : "r30", "r31"
+  );
+  return result;
+}
+
 #else
 
 static inline uint8_t Clip8(int16_t value) {
@@ -463,6 +496,15 @@ static inline uint8_t ShiftRight7(uint16_t value) {
   return value >> 7;
 }
 
+static inline uint8_t InterpolateSample(
+    const prog_uint8_t* table,
+    uint16_t phase) {
+  return Mix(
+      pgm_read_byte(table + (phase >> 8)),
+      pgm_read_byte(1 + table + (phase >> 8)),
+      phase & 0xff);
+}
+
 #endif  // USE_OPTIMIZED_OP
 
 }  // namespace avrlib