ports/nrf: factor out routines for allocating, freeing pwm channels

2 files changed, 67 insertions, 49 deletions

diff --git a/ports/nrf/common-hal/pulseio/PWMOut.c b/ports/nrf/common-hal/pulseio/PWMOut.c
index 5233c9adc..b827470d3 100644
--- a/ports/nrf/common-hal/pulseio/PWMOut.c
+++ b/ports/nrf/common-hal/pulseio/PWMOut.c
@@ -57,6 +57,12 @@ STATIC uint16_t pwm_seq[MP_ARRAY_SIZE(pwms)][CHANNELS_PER_PWM];
 
 static uint8_t never_reset_pwm[MP_ARRAY_SIZE(pwms)];
 
+STATIC int pwm_idx(NRF_PWM_Type *pwm) {
+    for(size_t i=0; i < MP_ARRAY_SIZE(pwms); i++)
+        if(pwms[i] == pwm) return i;
+    return -1;
+}
+
 void common_hal_pulseio_pwmout_never_reset(pulseio_pwmout_obj_t *self) {
     for(size_t i=0; i < MP_ARRAY_SIZE(pwms); i++) {
         NRF_PWM_Type* pwm = pwms[i];
@@ -133,29 +139,11 @@ bool convert_frequency(uint32_t frequency, uint16_t *countertop, nrf_pwm_clk_t *
     return false;
 }
 
-pwmout_result_t common_hal_pulseio_pwmout_construct(pulseio_pwmout_obj_t* self,
-                                                    const mcu_pin_obj_t* pin,
-                                                    uint16_t duty,
-                                                    uint32_t frequency,
-                                                    bool variable_frequency) {
-
-    // We don't use the nrfx driver here because we want to dynamically allocate channels
-    // as needed in an already-enabled PWM.
-
-    uint16_t countertop;
-    nrf_pwm_clk_t base_clock;
-    if (frequency == 0 || !convert_frequency(frequency, &countertop, &base_clock)) {
-        return PWMOUT_INVALID_FREQUENCY;
-    }
-
-    self->pwm = NULL;
-    self->channel = CHANNELS_PER_PWM;    // out-of-range value.
-    bool pwm_already_in_use;
-    NRF_PWM_Type* pwm;
-    size_t pwm_index = 0;
-    for (; pwm_index < MP_ARRAY_SIZE(pwms); pwm_index++) {
-        pwm = pwms[pwm_index];
-        pwm_already_in_use = pwm->ENABLE & SPIM_ENABLE_ENABLE_Msk;
+NRF_PWM_Type *pwmout_allocate(uint16_t countertop, nrf_pwm_clk_t base_clock,
+        bool variable_frequency, int8_t *channel_out, bool *pwm_already_in_use_out) {
+    for (size_t pwm_index = 0; pwm_index < MP_ARRAY_SIZE(pwms); pwm_index++) {
+        NRF_PWM_Type *pwm = pwms[pwm_index];
+        bool pwm_already_in_use = pwm->ENABLE & SPIM_ENABLE_ENABLE_Msk;
         if (pwm_already_in_use) {
             if (variable_frequency) {
                 // Variable frequency requires exclusive use of a PWM, so try the next one.
@@ -168,28 +156,65 @@ pwmout_result_t common_hal_pulseio_pwmout_construct(pulseio_pwmout_obj_t* self,
                 for (size_t chan = 0; chan < CHANNELS_PER_PWM; chan++) {
                     if (pwm->PSEL.OUT[chan] == 0xFFFFFFFF) {
                         // Channel is free.
-                        self->pwm = pwm;
-                        self->channel = chan;
-                        break;
+                        if(channel_out)
+                            *channel_out = chan;
+                        if(pwm_already_in_use_out)
+                            *pwm_already_in_use_out = pwm_already_in_use;
+                        return pwm;
                     }
                 }
-                // Did we find a channel? If not, loop and check the next pwm.
-                if (self->pwm != NULL) {
-                    break;
-                }
             }
         } else {
             // PWM not yet in use, so we can start to use it. Use channel 0.
-            self->pwm = pwm;
-            self->channel = 0;
-            break;
+            if(channel_out)
+                *channel_out = 0;
+            if(pwm_already_in_use_out)
+                *pwm_already_in_use_out = pwm_already_in_use;
+            return pwm;
         }
     }
+    return NULL;
+}
+
+void pwmout_free_channel(NRF_PWM_Type *pwm, int8_t channel) {
+    // Disconnect pin from channel.
+    pwm->PSEL.OUT[channel] = 0xFFFFFFFF;
+
+    for(int i=0; i < CHANNELS_PER_PWM; i++) {
+        if (pwm->PSEL.OUT[i] != 0xFFFFFFFF) {
+            // Some channel is still being used, so don't disable.
+            return;
+        }
+    }
+
+    nrf_pwm_disable(pwm);
+}
+
+pwmout_result_t common_hal_pulseio_pwmout_construct(pulseio_pwmout_obj_t* self,
+                                                    const mcu_pin_obj_t* pin,
+                                                    uint16_t duty,
+                                                    uint32_t frequency,
+                                                    bool variable_frequency) {
+
+    // We don't use the nrfx driver here because we want to dynamically allocate channels
+    // as needed in an already-enabled PWM.
+
+    uint16_t countertop;
+    nrf_pwm_clk_t base_clock;
+    if (frequency == 0 || !convert_frequency(frequency, &countertop, &base_clock)) {
+        return PWMOUT_INVALID_FREQUENCY;
+    }
+
+    int8_t channel;
+    bool pwm_already_in_use;
+    self->pwm = pwmout_allocate(countertop, base_clock, variable_frequency,
+        &channel, &pwm_already_in_use);
 
     if (self->pwm == NULL) {
         return PWMOUT_ALL_TIMERS_IN_USE;
     }
 
+    self->channel = channel;
     self->pin_number = pin->number;
     claim_pin(pin);
 
@@ -200,17 +225,17 @@ pwmout_result_t common_hal_pulseio_pwmout_construct(pulseio_pwmout_obj_t* self,
     nrf_gpio_cfg_output(self->pin_number);
 
     // disable before mapping pin channel
-    nrf_pwm_disable(pwm);
+    nrf_pwm_disable(self->pwm);
 
     if (!pwm_already_in_use) {
-        reset_single_pwmout(pwm_index);
-        nrf_pwm_configure(pwm, base_clock, NRF_PWM_MODE_UP, countertop);
+        reset_single_pwmout(pwm_idx(self->pwm));
+        nrf_pwm_configure(self->pwm, base_clock, NRF_PWM_MODE_UP, countertop);
     }
 
     // Connect channel to pin, without disturbing other channels.
-    pwm->PSEL.OUT[self->channel] = pin->number;
+    self->pwm->PSEL.OUT[self->channel] = pin->number;
 
-    nrf_pwm_enable(pwm);
+    nrf_pwm_enable(self->pwm);
 
     common_hal_pulseio_pwmout_set_duty_cycle(self, duty);
     return PWMOUT_OK;
@@ -230,17 +255,7 @@ void common_hal_pulseio_pwmout_deinit(pulseio_pwmout_obj_t* self) {
     NRF_PWM_Type* pwm = self->pwm;
     self->pwm = NULL;
 
-    // Disconnect pin from channel.
-    pwm->PSEL.OUT[self->channel] = 0xFFFFFFFF;
-
-    for(int i=0; i < CHANNELS_PER_PWM; i++) {
-        if (self->pwm->PSEL.OUT[i] != 0xFFFFFFFF) {
-            // Some channel is still being used, so don't disable.
-            return;
-        }
-    }
-
-    nrf_pwm_disable(pwm);
+    pwmout_free_channel(pwm, self->channel);
 }
 
 void common_hal_pulseio_pwmout_set_duty_cycle(pulseio_pwmout_obj_t* self, uint16_t duty_cycle) {
diff --git a/ports/nrf/common-hal/pulseio/PWMOut.h b/ports/nrf/common-hal/pulseio/PWMOut.h
index a4e58dc1a..b6798cb68 100644
--- a/ports/nrf/common-hal/pulseio/PWMOut.h
+++ b/ports/nrf/common-hal/pulseio/PWMOut.h
@@ -41,5 +41,8 @@ typedef struct {
 } pulseio_pwmout_obj_t;
 
 void pwmout_reset(void);
+NRF_PWM_Type *pwmout_allocate(uint16_t countertop, nrf_pwm_clk_t base_clock,
+    bool variable_frequency, int8_t *channel_out, bool *pwm_already_in_use_out);
+void pwmout_free_channel(NRF_PWM_Type *pwm, int8_t channel);
 
 #endif // MICROPY_INCLUDED_NRF_COMMON_HAL_PULSEIO_PWMOUT_H