Inline synthio_sat16 and use ssat

shared-module/synthio/__init__.c

@@ -24,27 +24,6 @@ static const int16_t square_wave[] = {-32768, 32767};
 static const uint16_t notes[] = {8372, 8870, 9397, 9956, 10548, 11175, 11840,
                                  12544, 13290, 14080, 14917, 15804}; // 9th octave
 
-// cleaner sat16 by http://www.moseleyinstruments.com/
-int16_t synthio_sat16(int32_t n, int rshift) {
-    // we should always round towards 0
-    // to avoid recirculating round-off noise
-    //
-    // a 2s complement positive number is always
-    // rounded down, so we only need to take
-    // care of negative numbers
-    if (n < 0) {
-        n = n + (~(0xFFFFFFFFUL << rshift));
-    }
-    n = n >> rshift;
-    if (n > 32767) {
-        return 32767;
-    }
-    if (n < -32768) {
-        return -32768;
-    }
-    return n;
-}
-
 static int64_t round_float_to_int64(mp_float_t f) {
     return (int64_t)(f + MICROPY_FLOAT_CONST(0.5));
 }

shared-module/synthio/__init__.h

@@ -22,6 +22,10 @@
 #include "shared-bindings/synthio/__init__.h"
 #include "shared-bindings/synthio/Biquad.h"
 
+#if defined(__ARM_FEATURE_SAT) && __ARM_FEATURE_SAT
+#include <arm_acle.h>
+#endif
+
 typedef struct {
     uint16_t dur;
     mp_obj_t note_obj[CIRCUITPY_SYNTHIO_MAX_CHANNELS];
@@ -97,4 +101,20 @@ extern mp_float_t synthio_global_rate_scale, synthio_global_W_scale;
 extern uint8_t synthio_global_tick;
 void shared_bindings_synthio_lfo_tick(uint32_t sample_rate, uint16_t num_samples);
 
-int16_t synthio_sat16(int32_t n, int rshift);
+// cleaner sat16 by http://www.moseleyinstruments.com/
+static inline int16_t synthio_sat16(int32_t n, int rshift) {
+    // Round towards 0 to avoid recirculating round-off noise.
+    n += (n >> 31) & ((1 << rshift) - 1);
+    n >>= rshift;
+    #if defined(__ARM_FEATURE_SAT) && __ARM_FEATURE_SAT
+    return __ssat(n, 16);
+    #else
+    if (n > 32767) {
+        return 32767;
+    }
+    if (n < -32768) {
+        return -32768;
+    }
+    return n;
+    #endif
+}