diff --git a/ports/rp2/modules/rp2.py b/ports/rp2/modules/rp2.py index c7e4d1fdd9..1e4bb26cf0 100644 --- a/ports/rp2/modules/rp2.py +++ b/ports/rp2/modules/rp2.py @@ -88,6 +88,10 @@ class PIOASMEmit: def side(self, value): self.num_sideset += 1 if self.pass_ > 0: + if self.sideset_count == 0: + raise PIOASMError("no sideset") + elif value >= (1 << self.sideset_count): + raise PIOASMError("sideset too large") set_bit = 13 - self.sideset_count self.prog[_PROG_DATA][-1] |= self.sideset_opt << 12 | value << set_bit return self @@ -269,17 +273,17 @@ def asm_pio(**kw): # sideset_count is inclusive of enable bit -def asm_pio_encode(instr, sideset_count): +def asm_pio_encode(instr, sideset_count, sideset_opt=False): emit = PIOASMEmit() - emit.delay_max = 31 emit.sideset_count = sideset_count - if emit.sideset_count: - emit.delay_max >>= emit.sideset_count + emit.sideset_opt = sideset_opt != 0 + emit.delay_max = 31 >> (emit.sideset_count + emit.sideset_opt) emit.pass_ = 1 emit.num_instr = 0 emit.num_sideset = 0 gl = _pio_funcs + gl["word"] = emit.word gl["nop"] = emit.nop # gl["jmp"] = emit.jmp currently not supported gl["wait"] = emit.wait diff --git a/ports/rp2/rp2_pio.c b/ports/rp2/rp2_pio.c index 414fa8bd70..fe09ebe249 100644 --- a/ports/rp2/rp2_pio.c +++ b/ports/rp2/rp2_pio.c @@ -613,7 +613,12 @@ STATIC mp_obj_t rp2_state_machine_exec(mp_obj_t self_in, mp_obj_t instr_in) { mp_obj_t rp2_module = mp_import_name(MP_QSTR_rp2, mp_const_none, MP_OBJ_NEW_SMALL_INT(0)); mp_obj_t asm_pio_encode = mp_load_attr(rp2_module, MP_QSTR_asm_pio_encode); uint32_t sideset_count = self->pio->sm[self->sm].pinctrl >> PIO_SM0_PINCTRL_SIDESET_COUNT_LSB; - mp_obj_t encoded_obj = mp_call_function_2(asm_pio_encode, instr_in, MP_OBJ_NEW_SMALL_INT(sideset_count)); + uint8_t sideset_opt = !!(self->pio->sm[self->sm].execctrl & (1 << PIO_SM0_EXECCTRL_SIDE_EN_LSB)); + mp_obj_t args[3]; + args[0] = instr_in; + args[1] = MP_OBJ_NEW_SMALL_INT(sideset_count); + args[2] = MP_OBJ_NEW_SMALL_INT(sideset_opt); + mp_obj_t encoded_obj = mp_call_function_n_kw(asm_pio_encode, 3, 0, args); mp_int_t encoded = mp_obj_get_int(encoded_obj); pio_sm_exec(self->pio, self->sm, encoded); return mp_const_none;