raspberrypi: audiopwmout: subtle for #5092

I noticed that the loop over 65535 possible denominators took a long time,
causing up to 100ms wait for a sound sample to start playing!

This algorithm, adapted from an algorithm shown in Python's fractions.py,
is guaranteed to find the best denominator in a small number of steps
(I think log2-many steps but I'm not sure). In practice, it means the time
between samples playing is just 10ms, and some of that is recreating the
sine wave sample in Python each time.

It often finds the same solution as the old code, but sometimes it finds
one a bit better since it compares the ratios using float point instead
of integer arithmetic.

Author: jepler

ports/raspberrypi/common-hal/audiopwmio/PWMAudioOut.c

@@ -26,6 +26,7 @@
 
 #include "common-hal/audiopwmio/PWMAudioOut.h"
 
+#include <math.h>
 #include <stdint.h>
 #include <string.h>
 
@@ -51,6 +52,58 @@
 #define SAMPLE_BITS_TO_DISCARD (16 - BITS_PER_SAMPLE)
 #define PWM_TOP ((1 << BITS_PER_SAMPLE) - 1)
 
+
+static uint32_t gcd(uint32_t a, uint32_t b) {
+    while (b) {
+        uint32_t tmp = a % b;
+        a = b;
+        b = tmp;
+    }
+    return a;
+}
+
+static uint32_t limit_denominator(uint32_t max_denominator, uint32_t num_in, uint32_t den_in, uint32_t *den_out) {
+// Algorithm based on Python's limit_denominator
+    uint32_t p0 = 0, q0 = 1, p1 = 1, q1 = 0;
+    uint32_t d = den_in, n = num_in;
+    uint32_t g = gcd(n, d);
+    d /= g;
+    n /= g;
+    if (d < max_denominator) {
+        *den_out = d;
+        return n;
+    }
+    while (1) {
+        uint32_t a = n / d;
+        uint32_t q2 = q0 + a * q1;
+        if (q2 > max_denominator) {
+            break;
+        }
+
+        uint32_t p_tmp = p0 + a * p1;
+        p0 = p1;
+        q0 = q1;
+        p1 = p_tmp;
+        q1 = q2;
+
+        uint32_t d_tmp = n - a * d;
+        n = d;
+        d = d_tmp;
+    }
+    uint32_t k = (max_denominator - q0) / q1;
+    uint32_t bound1_num = p0 + k * p1, bound1_den = q0 + k * q1;
+    uint32_t bound2_num = p1, bound2_den = q1;
+
+    if (fabsf((float)bound1_num / bound1_den - (float)num_in / den_in) <=
+        fabsf((float)bound2_num / bound2_den - (float)num_in / den_in)) {
+        *den_out = bound2_den;
+        return bound2_num;
+    }
+
+    *den_out = bound1_den;
+    return bound1_num;
+}
+
 void audiopwmout_reset() {
     for (size_t i = 0; i < NUM_DMA_TIMERS; i++) {
         dma_hw->timer[i] = 0;
@@ -170,30 +223,10 @@ void common_hal_audiopwmio_pwmaudioout_play(audiopwmio_pwmaudioout_obj_t *self,
     uint32_t sample_rate = audiosample_sample_rate(sample);
 
     uint32_t system_clock = common_hal_mcu_processor_get_frequency();
-    uint32_t best_numerator = 0;
-    uint32_t best_denominator = 0;
-    uint32_t best_error = system_clock;
-
-    for (uint32_t denominator = 0xffff; denominator > 0; denominator--) {
-        uint32_t numerator = ((uint64_t)denominator * sample_rate) / system_clock;
-        uint32_t remainder = ((uint64_t)denominator * sample_rate) % system_clock;
-        if (remainder > (system_clock / 2)) {
-            numerator += 1;
-            remainder = system_clock - remainder;
-        }
-        if (remainder < best_error) {
-            best_denominator = denominator;
-            best_numerator = numerator;
-            best_error = remainder;
-            // Stop early if we can't do better.
-            if (remainder == 0) {
-                break;
-            }
-        }
-    }
+    uint32_t best_denominator;
+    uint32_t best_numerator = limit_denominator(0xffff, sample_rate, system_clock, &best_denominator);
 
     dma_hw->timer[pacing_timer] = best_numerator << 16 | best_denominator;
-
     audio_dma_result result = audio_dma_setup_playback(
         &self->dma,
         sample,