diff --git a/ports/raspberrypi/common-hal/rp2pio/StateMachine.c b/ports/raspberrypi/common-hal/rp2pio/StateMachine.c
index 5f35445cff527..b3d0e39e22184 100644
--- a/ports/raspberrypi/common-hal/rp2pio/StateMachine.c
+++ b/ports/raspberrypi/common-hal/rp2pio/StateMachine.c
@@ -64,19 +64,6 @@ static void *_interrupt_arg[NUM_PIOS][NUM_PIO_STATE_MACHINES];
 
 static void rp2pio_statemachine_interrupt_handler(void);
 
-// Workaround for sdk bug: https://github.com/raspberrypi/pico-sdk/issues/1878
-// This workaround can be removed when we upgrade to sdk 2.0.1
-static inline void sm_config_set_in_pin_count_issue1878(pio_sm_config *c, uint in_count) {
-    #if PICO_PIO_VERSION == 0
-    // can't be changed from 32 on PIO v0
-    ((void)c);
-    valid_params_if(HARDWARE_PIO, in_count == 32);
-    #else
-    valid_params_if(HARDWARE_PIO, in_count && in_count <= 32);
-    c->shiftctrl = (c->shiftctrl & ~PIO_SM0_SHIFTCTRL_IN_COUNT_BITS) |
-        ((in_count & 0x1fu) << PIO_SM0_SHIFTCTRL_IN_COUNT_LSB);
-    #endif
-}
 static void rp2pio_statemachine_set_pull(pio_pinmask_t pull_pin_up, pio_pinmask_t pull_pin_down, pio_pinmask_t pins_we_use) {
     for (size_t i = 0; i < NUM_BANK0_GPIOS; i++) {
         bool used = PIO_PINMASK_IS_SET(pins_we_use, i);
@@ -260,10 +247,12 @@ static bool use_existing_program(PIO *pio_out, uint *sm_out, int *offset_inout,
             if (_current_program_id[i][j] == program_id &&
                 _current_program_len[i][j] == program_len &&
                 (*offset_inout == -1 || *offset_inout == _current_program_offset[i][j])) {
-                *pio_out = pio;
-                *sm_out = j;
-                *offset_inout = _current_program_offset[i][j];
-                return true;
+                *sm_out = pio_claim_unused_sm(pio, false);
+                if (*sm_out >= 0) {
+                    *pio_out = pio;
+                    *offset_inout = _current_program_offset[i][j];
+                    return true;
+                }
             }
         }
     }
@@ -442,7 +431,7 @@ bool rp2pio_statemachine_construct(rp2pio_statemachine_obj_t *self,
     sm_config_set_wrap(&c, wrap_target, wrap);
     sm_config_set_in_shift(&c, in_shift_right, auto_push, push_threshold);
     #if PICO_PIO_VERSION > 0
-    sm_config_set_in_pin_count_issue1878(&c, in_pin_count);
+    sm_config_set_in_pin_count(&c, in_pin_count);
     #endif
 
     sm_config_set_out_shift(&c, out_shift_right, auto_pull, pull_threshold);