From 117343a4d4c85e2790f0d0e0bda2b8ba493fadcc Mon Sep 17 00:00:00 2001
From: pichenettes <ol.gillet@gmail.com>
Date: Tue, 26 Apr 2011 22:07:25 +0200
Subject: [PATCH] Added wavetable interpolation function

---
 op.h | 42 ++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 42 insertions(+)

diff --git a/op.h b/op.h
index af328cd..c51991d 100755
--- a/op.h
+++ b/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