rp2: StateMachine: Allow loading programs at fixed offsets

Author: jepler

ports/raspberrypi/bindings/rp2pio/StateMachine.c

@@ -93,6 +93,7 @@
 //|         user_interruptible: bool = True,
 //|         wrap_target: int = 0,
 //|         wrap: int = -1,
+//|         offset: int = -1,
 //|     ) -> None:
 //|         """Construct a StateMachine object on the given pins with the given program.
 //|
@@ -150,6 +151,9 @@
 //|         :param int wrap: The instruction after which to wrap to the ``wrap``
 //|             instruction. As a special case, -1 (the default) indicates the
 //|             last instruction of the program.
+//|         :param int offset: A specific offset in the state machine's program memory where the program must be loaded.
+//|             The default value, -1, allows the program to be loaded at any offset.
+//|             This is appropriate for most programs.
 //|         """
 //|         ...
 
@@ -170,7 +174,8 @@ STATIC mp_obj_t rp2pio_statemachine_make_new(const mp_obj_type_t *type, size_t n
            ARG_auto_push, ARG_push_threshold, ARG_in_shift_right,
            ARG_user_interruptible,
            ARG_wrap_target,
-           ARG_wrap,};
+           ARG_wrap,
+           ARG_offset,};
     static const mp_arg_t allowed_args[] = {
         { MP_QSTR_program, MP_ARG_REQUIRED | MP_ARG_OBJ },
         { MP_QSTR_frequency, MP_ARG_REQUIRED | MP_ARG_INT },
@@ -214,6 +219,7 @@ STATIC mp_obj_t rp2pio_statemachine_make_new(const mp_obj_type_t *type, size_t n
 
         { MP_QSTR_wrap_target, MP_ARG_KW_ONLY | MP_ARG_INT, {.u_int = 0} },
         { MP_QSTR_wrap, MP_ARG_KW_ONLY | MP_ARG_INT, {.u_int = -1} },
+        { MP_QSTR_offset, MP_ARG_KW_ONLY | MP_ARG_INT, {.u_int = -1} },
     };
     mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)];
     mp_arg_parse_all_kw_array(n_args, n_kw, all_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args);
@@ -286,7 +292,7 @@ STATIC mp_obj_t rp2pio_statemachine_make_new(const mp_obj_type_t *type, size_t n
         args[ARG_wait_for_txstall].u_bool,
         args[ARG_auto_push].u_bool, push_threshold, args[ARG_in_shift_right].u_bool,
         args[ARG_user_interruptible].u_bool,
-        wrap_target, wrap);
+        wrap_target, wrap, args[ARG_offset].u_int);
     return MP_OBJ_FROM_PTR(self);
 }
 

ports/raspberrypi/bindings/rp2pio/StateMachine.h

@@ -54,7 +54,8 @@ void common_hal_rp2pio_statemachine_construct(rp2pio_statemachine_obj_t *self,
     bool wait_for_txstall,
     bool auto_push, uint8_t push_threshold, bool in_shift_right,
     bool user_interruptible,
-    int wrap_taget, int wrap);
+    int wrap_taget, int wrap,
+    int offset);
 
 void common_hal_rp2pio_statemachine_deinit(rp2pio_statemachine_obj_t *self);
 bool common_hal_rp2pio_statemachine_deinited(rp2pio_statemachine_obj_t *self);

ports/raspberrypi/common-hal/audiobusio/I2SOut.c

@@ -136,7 +136,8 @@ void common_hal_audiobusio_i2sout_construct(audiobusio_i2sout_obj_t *self,
         false, // Wait for txstall
         false, 32, false, // in settings
         false, // Not user-interruptible.
-        0, -1); // wrap settings
+        0, -1, // wrap settings
+        PIO_ANY_OFFSET);
 
     self->playing = false;
     audio_dma_init(&self->dma);

ports/raspberrypi/common-hal/audiobusio/PDMIn.c

@@ -80,8 +80,8 @@ void common_hal_audiobusio_pdmin_construct(audiobusio_pdmin_obj_t *self,
         false, // Wait for txstall
         false, 32, true, // in settings
         false, // Not user-interruptible.
-        0, -1); // wrap settings
-
+        0, -1, // wrap settings
+        PIO_ANY_OFFSET);
     uint32_t actual_frequency = common_hal_rp2pio_statemachine_get_frequency(&self->state_machine);
     if (actual_frequency < MIN_MIC_CLOCK) {
         mp_raise_ValueError(translate("sampling rate out of range"));

ports/raspberrypi/common-hal/imagecapture/ParallelImageCapture.c

@@ -120,7 +120,8 @@ void common_hal_imagecapture_parallelimagecapture_construct(imagecapture_paralle
         false, // wait for txstall
         true, 32, true,  // in settings
         false, // Not user-interruptible.
-        2, 5); // wrap settings
+        2, 5, // wrap settings
+        PIO_ANY_OFFSET);
 }
 
 void common_hal_imagecapture_parallelimagecapture_deinit(imagecapture_parallelimagecapture_obj_t *self) {

ports/raspberrypi/common-hal/neopixel_write/__init__.c

@@ -79,7 +79,9 @@ void common_hal_neopixel_write(const digitalio_digitalinout_obj_t *digitalinout,
         false, // claim pins
         false, // Not user-interruptible.
         false, // No sideset enable
-        0, -1); // wrap
+        0, -1, // wrap
+        PIO_ANY_OFFSET  // offset
+        );
     if (!ok) {
         // Do nothing. Maybe bitbang?
         return;

ports/raspberrypi/common-hal/paralleldisplay/ParallelBus.c

@@ -111,7 +111,8 @@ void common_hal_paralleldisplay_parallelbus_construct(paralleldisplay_parallelbu
         false, // wait for TX stall
         false, 32, true, // RX setting we don't use
         false, // Not user-interruptible.
-        0, -1); // wrap settings
+        0, -1, // wrap settings
+        PIO_ANY_OFFSET);
 
     common_hal_rp2pio_statemachine_never_reset(&self->state_machine);
 }

ports/raspberrypi/common-hal/pulseio/PulseIn.c

@@ -74,7 +74,8 @@ void common_hal_pulseio_pulsein_construct(pulseio_pulsein_obj_t *self,
         false, // wait for TX stall
         true, 32, true, // RX auto pull every 32 bits. shift left to output msb first
         false, // Not user-interruptible.
-        0, -1); // wrap settings
+        0, -1, // wrap settings
+        PIO_ANY_OFFSET);
 
     common_hal_pulseio_pulsein_pause(self);
 

ports/raspberrypi/common-hal/rotaryio/IncrementalEncoder.c

@@ -95,7 +95,8 @@ void common_hal_rotaryio_incrementalencoder_construct(rotaryio_incrementalencode
         false, // Wait for txstall
         false, 32, false, // in settings
         false, // Not user-interruptible.
-        0, MP_ARRAY_SIZE(encoder) - 1 // wrap settings
+        0, MP_ARRAY_SIZE(encoder) - 1, // wrap settings
+        PIO_ANY_OFFSET
         );
 
     // We're guaranteed by the init code that some output will be available promptly

ports/raspberrypi/common-hal/rp2pio/StateMachine.c

@@ -178,6 +178,21 @@ STATIC uint32_t _check_pins_free(const mcu_pin_obj_t *first_pin, uint8_t pin_cou
     return pins_we_use;
 }
 
+static bool can_add_program(PIO pio, const pio_program_t *program, int offset) {
+    if (offset == -1) {
+        return pio_can_add_program(pio, program);
+    }
+    return pio_can_add_program_at_offset(pio, program, offset);
+}
+
+static uint add_program(PIO pio, const pio_program_t *program, int offset) {
+    if (offset == -1) {
+        return pio_add_program(pio, program);
+    } else {
+        pio_add_program_at_offset(pio, program, offset);
+        return offset;
+    }
+}
 
 bool rp2pio_statemachine_construct(rp2pio_statemachine_obj_t *self,
     const uint16_t *program, size_t program_len,
@@ -197,7 +212,8 @@ bool rp2pio_statemachine_construct(rp2pio_statemachine_obj_t *self,
     bool claim_pins,
     bool user_interruptible,
     bool sideset_enable,
-    int wrap_target, int wrap
+    int wrap_target, int wrap,
+    int offset
     ) {
     // Create a program id that isn't the pointer so we can store it without storing the original object.
     uint32_t program_id = ~((uint32_t)program);
@@ -215,14 +231,15 @@ bool rp2pio_statemachine_construct(rp2pio_statemachine_obj_t *self,
         uint8_t free_count = 0;
         for (size_t j = 0; j < NUM_PIO_STATE_MACHINES; j++) {
             if (_current_program_id[i][j] == program_id &&
-                _current_program_len[i][j] == program_len) {
+                _current_program_len[i][j] == program_len &&
+                (offset == -1 || offset == _current_program_offset[i][j])) {
                 program_offset = _current_program_offset[i][j];
             }
             if (!pio_sm_is_claimed(pio, j)) {
                 free_count++;
             }
         }
-        if (free_count > 0 && (program_offset < 32 || pio_can_add_program(pio, &program_struct))) {
+        if (free_count > 0 && (program_offset < 32 || can_add_program(pio, &program_struct, offset))) {
             pio_index = i;
             if (program_offset < 32) {
                 break;
@@ -254,7 +271,7 @@ bool rp2pio_statemachine_construct(rp2pio_statemachine_obj_t *self,
     self->pio = pio_instances[pio_index];
     self->state_machine = state_machine;
     if (program_offset == 32) {
-        program_offset = pio_add_program(self->pio, &program_struct);
+        program_offset = add_program(self->pio, &program_struct, offset);
     }
     self->offset = program_offset;
     _current_program_id[pio_index][state_machine] = program_id;
@@ -509,7 +526,8 @@ void common_hal_rp2pio_statemachine_construct(rp2pio_statemachine_obj_t *self,
     bool wait_for_txstall,
     bool auto_push, uint8_t push_threshold, bool in_shift_right,
     bool user_interruptible,
-    int wrap_target, int wrap) {
+    int wrap_target, int wrap,
+    int offset) {
 
     // First, check that all pins are free OR already in use by any PIO if exclusive_pin_use is false.
     uint32_t pins_we_use = wait_gpio_mask;
@@ -598,7 +616,7 @@ void common_hal_rp2pio_statemachine_construct(rp2pio_statemachine_obj_t *self,
         true /* claim pins */,
         user_interruptible,
         sideset_enable,
-        wrap_target, wrap);
+        wrap_target, wrap, offset);
     if (!ok) {
         mp_raise_RuntimeError(translate("All state machines in use"));
     }