From 391ed3478324a066dc6af4692da18ea00da0b3b5 Mon Sep 17 00:00:00 2001
From: Ian Scott <ian@polpo.org>
Date: Mon, 25 Mar 2024 10:47:00 -0600
Subject: [PATCH] Add clamp.h

---
 sw/clamp.h | 30 ++++++++++++++++++++++++++++++
 1 file changed, 30 insertions(+)
 create mode 100644 sw/clamp.h

diff --git a/sw/clamp.h b/sw/clamp.h
new file mode 100644
index 0000000..662f6e8
--- /dev/null
+++ b/sw/clamp.h
@@ -0,0 +1,30 @@
+#ifdef INTERP_CLAMP
+#include "hardware/interp.h"
+#endif
+
+static void clamp_setup(void) {
+#ifdef INTERP_CLAMP
+    interp_config cfg;
+    // Clamp setup
+    cfg = interp_default_config();
+    interp_config_set_clamp(&cfg, true);
+    interp_config_set_shift(&cfg, 14);
+    // set mask according to new position of sign bit..
+    interp_config_set_mask(&cfg, 0, 17);
+    // ...so that the shifted value is correctly sign extended
+    interp_config_set_signed(&cfg, true);
+    interp_set_config(interp1, 0, &cfg);
+    interp1->base[0] = -32768;
+    interp1->base[1] = 32767;
+#endif
+}
+
+
+static int16_t __force_inline clamp16(int32_t d) {
+#ifdef INTERP_CLAMP
+    interp1->accum[0] = d;
+    return interp1->peek[0];
+#else
+    return d < -32768 ? -32768 : (d > 32767 ? 32767 : d);
+#endif // INTERP_CLAMP
+}