diff --git a/bricks/_common/micropython.c b/bricks/_common/micropython.c index 8135f8057..b5769a104 100644 --- a/bricks/_common/micropython.c +++ b/bricks/_common/micropython.c @@ -9,6 +9,7 @@ #include #include +#include #include #include #include @@ -39,14 +40,9 @@ // Implementation for MICROPY_EVENT_POLL_HOOK void pb_event_poll_hook(void) { - // Drive pbio event loop. - while (pbio_do_one_event()) { - } - mp_handle_pending(true); - // Platform-specific code to run on completing the poll hook. - pb_event_poll_hook_leave(); + pbio_os_run_while_idle(); } // callback for when stop button is pressed in IDE or on hub diff --git a/bricks/_common/sources.mk b/bricks/_common/sources.mk index bf91fe6ff..4717aa353 100644 --- a/bricks/_common/sources.mk +++ b/bricks/_common/sources.mk @@ -215,6 +215,7 @@ PBIO_SRC_C = $(addprefix lib/pbio/,\ src/motor_process.c \ src/motor/servo_settings.c \ src/observer.c \ + src/os.c \ src/parent.c \ src/port.c \ src/port_dcm_pup.c \ diff --git a/bricks/_common_stm32/mpconfigport.h b/bricks/_common_stm32/mpconfigport.h index 8a846fffb..6d965ed83 100644 --- a/bricks/_common_stm32/mpconfigport.h +++ b/bricks/_common_stm32/mpconfigport.h @@ -24,31 +24,15 @@ typedef unsigned mp_uint_t; // must be pointer size typedef long mp_off_t; -// We have inlined IRQ functions for efficiency (they are generally -// 1 machine instruction). -// -// Note on IRQ state: you should not need to know the specific -// value of the state variable, but rather just pass the return -// value from disable_irq back to enable_irq. If you really need -// to know the machine-specific values, see irq.h. +#include -static inline void enable_irq(mp_uint_t state) { - __set_PRIMASK(state); -} - -static inline mp_uint_t disable_irq(void) { - mp_uint_t state = __get_PRIMASK(); - __disable_irq(); - return state; -} - -#define MICROPY_BEGIN_ATOMIC_SECTION() disable_irq() -#define MICROPY_END_ATOMIC_SECTION(state) enable_irq(state) +#define MICROPY_BEGIN_ATOMIC_SECTION() pbio_os_hook_disable_irq() +#define MICROPY_END_ATOMIC_SECTION(state) pbio_os_hook_enable_irq(state) #define MICROPY_VM_HOOK_LOOP \ do { \ - extern int pbio_do_one_event(void); \ - pbio_do_one_event(); \ + extern bool pbio_os_run_processes_once(void); \ + pbio_os_run_processes_once(); \ } while (0); #define MICROPY_GC_HOOK_LOOP(i) do { \ diff --git a/bricks/_common_stm32/mphalport.c b/bricks/_common_stm32/mphalport.c index 19356583d..09708c554 100644 --- a/bricks/_common_stm32/mphalport.c +++ b/bricks/_common_stm32/mphalport.c @@ -24,17 +24,18 @@ void pb_stack_get_info(char **sstack, char **estack) { *estack = (char *)&_estack; } -void pb_event_poll_hook_leave(void) { - // There is a possible race condition where an interrupt occurs and sets the - // Contiki poll_requested flag after all events have been processed. So we - // have a critical section where we disable interrupts and check see if there - // are any last second events. If not, we can call __WFI(), which still wakes - // up the CPU on interrupt even though interrupts are otherwise disabled. - mp_uint_t state = disable_irq(); - if (!process_nevents()) { - __WFI(); - } - enable_irq(state); +uint32_t pbio_os_hook_disable_irq(void) { + mp_uint_t flags = __get_PRIMASK(); + __disable_irq(); + return flags; +} + +void pbio_os_hook_enable_irq(uint32_t flags) { + __set_PRIMASK(flags); +} + +void pbio_os_hook_wait_for_interrupt(void) { + __WFI(); } // using "internal" pbdrv variable diff --git a/bricks/ev3/mpconfigport.h b/bricks/ev3/mpconfigport.h index f191642e5..a62672070 100644 --- a/bricks/ev3/mpconfigport.h +++ b/bricks/ev3/mpconfigport.h @@ -79,18 +79,15 @@ typedef unsigned mp_uint_t; // must be pointer size typedef long mp_off_t; -#define CPSR_IRQ_MASK (1 << 7) // IRQ disable -#define CPSR_FIQ_MASK (1 << 6) // FIQ disable +#include -#include - -#define MICROPY_BEGIN_ATOMIC_SECTION() IntDisable() -#define MICROPY_END_ATOMIC_SECTION(state) IntEnable(state) +#define MICROPY_BEGIN_ATOMIC_SECTION() pbio_os_hook_disable_irq() +#define MICROPY_END_ATOMIC_SECTION(state) pbio_os_hook_enable_irq(state) #define MICROPY_VM_HOOK_LOOP \ do { \ - extern int pbio_do_one_event(void); \ - pbio_do_one_event(); \ + extern bool pbio_os_run_processes_once(void); \ + pbio_os_run_processes_once(); \ } while (0); #define MICROPY_GC_HOOK_LOOP(i) do { \ diff --git a/bricks/ev3/mphalport.c b/bricks/ev3/mphalport.c index 6345063a2..1dc426565 100644 --- a/bricks/ev3/mphalport.c +++ b/bricks/ev3/mphalport.c @@ -18,6 +18,8 @@ #include "py/mpconfig.h" #include "py/stream.h" +#include + void pb_stack_get_info(char **sstack, char **estack) { extern uint32_t _estack; extern uint32_t _sstack; @@ -25,26 +27,20 @@ void pb_stack_get_info(char **sstack, char **estack) { *estack = (char *)&_estack; } -static inline int arm_wfi(void) { +uint32_t pbio_os_hook_disable_irq(void) { + return IntDisable(); +} + +void pbio_os_hook_enable_irq(uint32_t flags) { + IntEnable(flags); +} + +void pbio_os_hook_wait_for_interrupt(void) { __asm volatile ( "mov r0, #0\n" "mcr p15, 0, r0, c7, c0, 4\n" /* wait for interrupt */ ::: "r0" ); - return 0; -} - -void pb_event_poll_hook_leave(void) { - // There is a possible race condition where an interrupt occurs and sets the - // Contiki poll_requested flag after all events have been processed. So we - // have a critical section where we disable interrupts and check see if there - // are any last second events. If not, we can call __WFI(), which still wakes - // up the CPU on interrupt even though interrupts are otherwise disabled. - mp_uint_t state = MICROPY_BEGIN_ATOMIC_SECTION(); - if (!process_nevents()) { - arm_wfi(); - } - MICROPY_END_ATOMIC_SECTION(state); } // Core delay function that does an efficient sleep and may switch thread context. diff --git a/bricks/nxt/mpconfigport.h b/bricks/nxt/mpconfigport.h index 3d142736c..8d1290308 100644 --- a/bricks/nxt/mpconfigport.h +++ b/bricks/nxt/mpconfigport.h @@ -79,18 +79,23 @@ typedef long mp_off_t; // We need to provide a declaration/definition of alloca() #include -uint32_t nx_interrupts_disable(void); -void nx_interrupts_enable(uint32_t); +#include -#define MICROPY_BEGIN_ATOMIC_SECTION() nx_interrupts_disable() -#define MICROPY_END_ATOMIC_SECTION(state) nx_interrupts_enable(state) +#define MICROPY_BEGIN_ATOMIC_SECTION() pbio_os_hook_disable_irq() +#define MICROPY_END_ATOMIC_SECTION(state) pbio_os_hook_enable_irq(state) #define MICROPY_VM_HOOK_LOOP \ do { \ - extern int pbio_do_one_event(void); \ - pbio_do_one_event(); \ + extern bool pbio_os_run_processes_once(void); \ + pbio_os_run_processes_once(); \ } while (0); +#define MICROPY_GC_HOOK_LOOP(i) do { \ + if (((i) & 0xf) == 0) { \ + MICROPY_VM_HOOK_LOOP \ + } \ +} while (0) + #define MICROPY_EVENT_POLL_HOOK \ do { \ extern void pb_event_poll_hook(void); \ diff --git a/bricks/nxt/mphalport.c b/bricks/nxt/mphalport.c index 87e398d6f..239b837d1 100644 --- a/bricks/nxt/mphalport.c +++ b/bricks/nxt/mphalport.c @@ -20,21 +20,17 @@ #include "py/mpconfig.h" #include "py/stream.h" -void pb_event_poll_hook_leave(void) { - // There is a possible race condition where an interrupt occurs and sets - // the Contiki poll_requested flag after all events have been processed. So - // we have a critical section where we disable interrupts and check see if - // there are any last second events. If not, we can enter Idle Mode, which - // still wakes up the CPU on interrupt even though interrupts are otherwise - // disabled. - uint32_t state = nx_interrupts_disable(); - - if (!process_nevents()) { - // disable the processor clock which puts it in Idle Mode. - AT91C_BASE_PMC->PMC_SCDR = AT91C_PMC_PCK; - } +uint32_t pbio_os_hook_disable_irq(void) { + return nx_interrupts_disable(); +} + +void pbio_os_hook_enable_irq(uint32_t flags) { + nx_interrupts_enable(flags); +} - nx_interrupts_enable(state); +void pbio_os_hook_wait_for_interrupt(void) { + // disable the processor clock which puts it in Idle Mode. + AT91C_BASE_PMC->PMC_SCDR = AT91C_PMC_PCK; } void pb_stack_get_info(char **sstack, char **estack) { diff --git a/bricks/virtualhub/mp_port.c b/bricks/virtualhub/mp_port.c index 664300470..4493c229f 100644 --- a/bricks/virtualhub/mp_port.c +++ b/bricks/virtualhub/mp_port.c @@ -15,6 +15,7 @@ #include #include +#include #include #include #include @@ -72,7 +73,8 @@ void pb_virtualhub_port_init(void) { pbsys_init(); pbsys_status_set(PBIO_PYBRICKS_STATUS_USER_PROGRAM_RUNNING); - while (pbio_do_one_event()) { + + while (pbio_os_run_processes_once()) { } pb_package_pybricks_init(true); @@ -84,62 +86,44 @@ void pb_virtualhub_port_deinit(void) { pb_package_pybricks_deinit(); } -// MICROPY_VM_HOOK_LOOP -void pb_virtualhub_poll(void) { - while (pbio_do_one_event()) { - } -} +// Implementation for MICROPY_EVENT_POLL_HOOK +void pb_event_poll_hook(void) { -// MICROPY_EVENT_POLL_HOOK -void pb_virtualhub_event_poll(void) { -start: mp_handle_pending(true); - int events_handled = 0; - - while (pbio_do_one_event()) { - events_handled++; - } + pbio_os_run_while_idle(); +} - // If there were any pbio events handled, don't sleep because there may - // be something waiting on one of the events that was just handled. - if (events_handled) { - return; - } +// This is used instead of the uint32_t flags used in embedded builds. +static sigset_t global_origmask; +uint32_t pbio_os_hook_disable_irq(void) { sigset_t sigmask; sigfillset(&sigmask); + pthread_sigmask(SIG_SETMASK, &sigmask, &global_origmask); + return 0; +} - // disable "interrupts" - sigset_t origmask; - pthread_sigmask(SIG_SETMASK, &sigmask, &origmask); - - if (process_nevents()) { - // something was scheduled since the event loop above - pthread_sigmask(SIG_SETMASK, &origmask, NULL); - goto start; - } +void pbio_os_hook_enable_irq(uint32_t flags) { + pthread_sigmask(SIG_SETMASK, &global_origmask, NULL); +} +void pbio_os_hook_wait_for_interrupt(void) { struct timespec timeout = { .tv_sec = 0, .tv_nsec = 100000, }; - // "sleep" with "interrupts" enabled MP_THREAD_GIL_EXIT(); - pselect(0, NULL, NULL, NULL, &timeout, &origmask); + pselect(0, NULL, NULL, NULL, &timeout, &global_origmask); MP_THREAD_GIL_ENTER(); - - // restore "interrupts" - pthread_sigmask(SIG_SETMASK, &origmask, NULL); } - void pb_virtualhub_delay_us(mp_uint_t us) { mp_uint_t start = mp_hal_ticks_us(); while (mp_hal_ticks_us() - start < us) { - pb_virtualhub_poll(); + MICROPY_VM_HOOK_LOOP; } } diff --git a/bricks/virtualhub/mpconfigvariant.h b/bricks/virtualhub/mpconfigvariant.h index 21e1fdb30..fbf37c9c2 100644 --- a/bricks/virtualhub/mpconfigvariant.h +++ b/bricks/virtualhub/mpconfigvariant.h @@ -97,8 +97,8 @@ } while (0) #define MICROPY_VM_HOOK_LOOP do { \ - extern void pb_virtualhub_poll(void); \ - pb_virtualhub_poll(); \ + extern bool pbio_os_run_processes_once(void); \ + pbio_os_run_processes_once(); \ } while (0); #define MICROPY_GC_HOOK_LOOP(i) do { \ @@ -108,6 +108,6 @@ } while (0) #define MICROPY_EVENT_POLL_HOOK do { \ - extern void pb_virtualhub_event_poll(void); \ - pb_virtualhub_event_poll(); \ + extern void pb_event_poll_hook(void); \ + pb_event_poll_hook(); \ } while (0); diff --git a/lib/pbio/drv/charger/charger_mp2639a.c b/lib/pbio/drv/charger/charger_mp2639a.c index 16637b450..0e5797245 100644 --- a/lib/pbio/drv/charger/charger_mp2639a.c +++ b/lib/pbio/drv/charger/charger_mp2639a.c @@ -27,14 +27,13 @@ #include #include #include +#include #include "../core.h" #include "charger_mp2639a.h" #define platform pbdrv_charger_mp2639a_platform_data -PROCESS(pbdrv_charger_mp2639a_process, "MP2639A"); - #if PBDRV_CONFIG_CHARGER_MP2639A_MODE_PWM static pbdrv_pwm_dev_t *mode_pwm; #endif @@ -45,11 +44,6 @@ static pbdrv_pwm_dev_t *iset_pwm; static pbdrv_charger_status_t pbdrv_charger_status; static bool mode_pin_is_low; -void pbdrv_charger_init(void) { - pbdrv_init_busy_up(); - process_start(&pbdrv_charger_mp2639a_process); -} - pbio_error_t pbdrv_charger_get_current_now(uint16_t *current) { pbio_error_t err = pbdrv_adc_get_ch(platform.ib_adc_ch, current); if (err != PBIO_SUCCESS) { @@ -182,18 +176,23 @@ static bool read_chg(void) { #define PBDRV_CHARGER_MP2639A_CHARGE_TIMEOUT_MS (60 * 60 * 1000) #define PBDRV_CHARGER_MP2639A_CHARGE_PAUSE_MS (30 * 1000) -PROCESS_THREAD(pbdrv_charger_mp2639a_process, ev, data) { - PROCESS_BEGIN(); +static pbio_os_process_t pbdrv_charger_mp2639a_process; + +pbio_error_t pbdrv_charger_mp2639a_process_thread(pbio_os_state_t *state, void *context) { + + static pbio_os_timer_t timer; + + ASYNC_BEGIN(state); #if PBDRV_CONFIG_CHARGER_MP2639A_MODE_PWM while (pbdrv_pwm_get_dev(platform.mode_pwm_id, &mode_pwm) != PBIO_SUCCESS) { - PROCESS_PAUSE(); + AWAIT_ONCE(state); } #endif #if PBDRV_CONFIG_CHARGER_MP2639A_ISET_PWM while (pbdrv_pwm_get_dev(platform.iset_pwm_id, &iset_pwm) != PBIO_SUCCESS) { - PROCESS_PAUSE(); + AWAIT_ONCE(state); } #endif @@ -212,12 +211,10 @@ PROCESS_THREAD(pbdrv_charger_mp2639a_process, ev, data) { static bool chg_samples[7]; static uint8_t chg_index = 0; - static struct etimer timer; static uint32_t charge_count = 0; for (;;) { - etimer_set(&timer, PBDRV_CHARGER_MP2639A_STATUS_SAMPLE_TIME); - PROCESS_WAIT_EVENT_UNTIL(ev == PROCESS_EVENT_TIMER && etimer_expired(&timer)); + AWAIT_MS(state, &timer, PBDRV_CHARGER_MP2639A_STATUS_SAMPLE_TIME); // Enable charger chip based on USB state. We don't need to disable it // on charger fault since the chip will automatically disable itself. @@ -275,13 +272,17 @@ PROCESS_THREAD(pbdrv_charger_mp2639a_process, ev, data) { if (charge_count > (PBDRV_CHARGER_MP2639A_CHARGE_TIMEOUT_MS / PBDRV_CHARGER_MP2639A_STATUS_SAMPLE_TIME)) { pbdrv_charger_status = PBDRV_CHARGER_STATUS_DISCHARGE; pbdrv_charger_enable(false, PBDRV_CHARGER_LIMIT_NONE); - etimer_set(&timer, PBDRV_CHARGER_MP2639A_CHARGE_PAUSE_MS); - PROCESS_WAIT_EVENT_UNTIL(etimer_expired(&timer)); + AWAIT_MS(state, &timer, PBDRV_CHARGER_MP2639A_CHARGE_PAUSE_MS); charge_count = 0; } } - PROCESS_END(); + ASYNC_END(PBIO_SUCCESS); +} + +void pbdrv_charger_init(void) { + pbdrv_init_busy_up(); + pbio_os_process_start(&pbdrv_charger_mp2639a_process, pbdrv_charger_mp2639a_process_thread, NULL); } #endif // PBDRV_CONFIG_CHARGER_MP2639A diff --git a/lib/pbio/drv/clock/clock_ev3.c b/lib/pbio/drv/clock/clock_ev3.c index 81d853f7b..193b24297 100644 --- a/lib/pbio/drv/clock/clock_ev3.c +++ b/lib/pbio/drv/clock/clock_ev3.c @@ -13,6 +13,8 @@ #include +#include + #include #include #include @@ -49,6 +51,7 @@ void systick_isr_C(void) { ++systick_ms; etimer_request_poll(); + pbio_os_request_poll(); /* Enable the timer interrupt */ TimerIntEnable(SOC_TMR_0_REGS, TMR_INT_TMR34_NON_CAPT_MODE); diff --git a/lib/pbio/drv/clock/clock_linux.c b/lib/pbio/drv/clock/clock_linux.c index f4f139183..a9b6760dd 100644 --- a/lib/pbio/drv/clock/clock_linux.c +++ b/lib/pbio/drv/clock/clock_linux.c @@ -9,6 +9,8 @@ #include #include +#include + // The SIGNAL option adds a timer that acts as the 1ms tick on embedded systems. #if PBDRV_CONFIG_CLOCK_LINUX_SIGNAL @@ -34,6 +36,7 @@ static void handle_signal(int sig) { // main thread is interrupted and the event poll hook runs. if (pthread_self() == main_thread) { etimer_request_poll(); + pbio_os_request_poll(); } else { pthread_kill(main_thread, TIMER_SIGNAL); } diff --git a/lib/pbio/drv/clock/clock_nxt.c b/lib/pbio/drv/clock/clock_nxt.c index 2549e4c09..891044480 100644 --- a/lib/pbio/drv/clock/clock_nxt.c +++ b/lib/pbio/drv/clock/clock_nxt.c @@ -9,6 +9,8 @@ #include +#include + #include #include #include @@ -41,6 +43,7 @@ static void systick_isr(void) { /* Do the system timekeeping. */ pbdrv_clock_ticks++; etimer_request_poll(); + pbio_os_request_poll(); /* Keeping up with the AVR link is a crucial task in the system, and * must absolutely be kept up with at all costs. diff --git a/lib/pbio/drv/clock/clock_stm32.c b/lib/pbio/drv/clock/clock_stm32.c index ffd8bbfac..75cce938a 100644 --- a/lib/pbio/drv/clock/clock_stm32.c +++ b/lib/pbio/drv/clock/clock_stm32.c @@ -8,6 +8,8 @@ #if PBDRV_CONFIG_CLOCK_STM32 +#include + #include STM32_H // NB: pbdrv_clock_ticks is intended to be private, but making it static @@ -80,6 +82,7 @@ void SysTick_Handler(void) { SysTick->CTRL; etimer_request_poll(); + pbio_os_request_poll(); } uint32_t HAL_GetTick(void) { diff --git a/lib/pbio/drv/clock/clock_test.c b/lib/pbio/drv/clock/clock_test.c index 7b6010b02..670064028 100644 --- a/lib/pbio/drv/clock/clock_test.c +++ b/lib/pbio/drv/clock/clock_test.c @@ -5,6 +5,8 @@ #if PBDRV_CONFIG_CLOCK_TEST +#include + // Clock implementation for tests. This allows tests to exactly control the // clock ticks to get repeatable tests rather than relying on a system clock. @@ -21,6 +23,7 @@ static uint32_t clock_ticks; void pbio_test_clock_tick(uint32_t ticks) { clock_ticks += ticks; etimer_request_poll(); + pbio_os_request_poll(); } void pbdrv_clock_init(void) { diff --git a/lib/pbio/drv/core.c b/lib/pbio/drv/core.c index f01f71431..c559d4fd7 100644 --- a/lib/pbio/drv/core.c +++ b/lib/pbio/drv/core.c @@ -5,6 +5,7 @@ #include #include +#include #include "core.h" #include "adc/adc.h" @@ -79,12 +80,12 @@ void pbdrv_init(void) { // Wait for all async pbdrv drivers to initialize before starting // higher level system processes. while (pbdrv_init_busy()) { - process_run(); + pbio_os_run_processes_once(); } #if PBDRV_CONFIG_IOPORT_PUP_QUIRK_POWER_CYCLE while (pbdrv_clock_get_ms() - ioport_reset_time < 500) { - process_run(); + pbio_os_run_processes_once(); } #endif pbdrv_ioport_enable_vcc(true); diff --git a/lib/pbio/drv/pwm/pwm_lp50xx_stm32.c b/lib/pbio/drv/pwm/pwm_lp50xx_stm32.c index 2e01a55a1..49b6990ec 100644 --- a/lib/pbio/drv/pwm/pwm_lp50xx_stm32.c +++ b/lib/pbio/drv/pwm/pwm_lp50xx_stm32.c @@ -12,11 +12,11 @@ #include #include -#include #include STM32_HAL_H #include #include +#include #include #include "../core.h" @@ -84,8 +84,8 @@ typedef struct { DMA_HandleTypeDef hdma_rx; /** HAL Tx DMA data */ DMA_HandleTypeDef hdma_tx; - /** Protothread */ - struct pt pt; + /** Process for this device */ + pbio_os_process_t process; /** Pointer to generic PWM device instance */ pbdrv_pwm_dev_t *pwm; /** PWM values */ @@ -94,7 +94,6 @@ typedef struct { bool changed; } pbdrv_pwm_lp50xx_stm32_priv_t; -PROCESS(pwm_lp50xx_stm32, "pwm_lp50xx_stm32"); static pbdrv_pwm_lp50xx_stm32_priv_t dev_priv[PBDRV_CONFIG_PWM_LP50XX_STM32_NUM_DEV]; static pbio_error_t pbdrv_pwm_lp50xx_stm32_set_duty(pbdrv_pwm_dev_t *dev, uint32_t ch, uint32_t value) { @@ -107,7 +106,7 @@ static pbio_error_t pbdrv_pwm_lp50xx_stm32_set_duty(pbdrv_pwm_dev_t *dev, uint32 // (the data sheet says 12-bit PWM but the I2C registers are only 8-bit). priv->values[ch] = value >> 8; priv->changed = true; - process_poll(&pwm_lp50xx_stm32); + pbio_os_request_poll(); return PBIO_SUCCESS; } @@ -116,6 +115,66 @@ static const pbdrv_pwm_driver_funcs_t pbdrv_pwm_lp50xx_stm32_funcs = { .set_duty = pbdrv_pwm_lp50xx_stm32_set_duty, }; +static pbio_error_t pbdrv_pwm_lp50xx_stm32_process_thread(pbio_os_state_t *state, void *context) { + + pbdrv_pwm_lp50xx_stm32_priv_t *priv = context; + + ASYNC_BEGIN(state); + + // Need to allow all drivers to init first. + AWAIT_ONCE(state); + + static const struct { + uint8_t reg; + uint8_t values[11]; + } __attribute__((packed)) init_data = { + .reg = DEVICE_CONFIG0, + .values = { + [DEVICE_CONFIG0] = DEVICE_CONFIG0_CHIP_EN, + [DEVICE_CONFIG1] = DEVICE_CONFIG1_POWER_SAVE_EN | DEVICE_CONFIG1_PWM_DITHERING_EN | DEVICE_CONFIG1_AUTO_INCR_EN, + [LED_CONFIG0] = 0, + [BANK_BRIGHTNESS] = 0, + [BANK_A_COLOR] = 0, + [BANK_B_COLOR] = 0, + [BANK_C_COLOR] = 0, + // Official LEGO firmware has LED0_BRIGHTNESS set to 255 and LED1_BRIGHTNESS + // set to 190 but then divides the incoming PWM duty cycle by 5. By doing + // the divide by 5 here, we end up with the same max brightness while + // allowing full dynamic range of the PWM duty cycle. + [LED0_BRIGHTNESS] = 51, + [LED1_BRIGHTNESS] = 38, + [LED2_BRIGHTNESS] = 0, + [LED3_BRIGHTNESS] = 0, + } + }; + + HAL_FMPI2C_Master_Transmit_DMA(&priv->hfmpi2c, I2C_ADDR, (void *)&init_data, sizeof(init_data)); + AWAIT_UNTIL(state, HAL_FMPI2C_GetState(&priv->hfmpi2c) == HAL_FMPI2C_STATE_READY); + + // initialization is finished so consumers can use this PWM device now. + priv->pwm->funcs = &pbdrv_pwm_lp50xx_stm32_funcs; + pbdrv_init_busy_down(); + + for (;;) { + AWAIT_UNTIL(state, priv->changed); + + static struct { + uint8_t reg; + uint8_t values[LP50XX_NUM_CH]; + } __attribute__((packed)) color_data = { + .reg = OUT0_COLOR, + }; + + memcpy(color_data.values, priv->values, LP50XX_NUM_CH); + HAL_FMPI2C_Master_Transmit_DMA(&priv->hfmpi2c, I2C_ADDR, (void *)&color_data, sizeof(color_data)); + priv->changed = false; + AWAIT_UNTIL(state, HAL_FMPI2C_GetState(&priv->hfmpi2c) == HAL_FMPI2C_STATE_READY); + } + + // Unreachable. + ASYNC_END(PBIO_ERROR_FAILED); +} + void pbdrv_pwm_lp50xx_stm32_init(pbdrv_pwm_dev_t *devs) { for (int i = 0; i < PBDRV_CONFIG_PWM_LP50XX_STM32_NUM_DEV; i++) { const pbdrv_pwm_lp50xx_stm32_platform_data_t *pdata = &pbdrv_pwm_lp50xx_stm32_platform_data[i]; @@ -186,68 +245,14 @@ void pbdrv_pwm_lp50xx_stm32_init(pbdrv_pwm_dev_t *devs) { HAL_NVIC_SetPriority(pdata->i2c_er_irq, 7, 3); HAL_NVIC_EnableIRQ(pdata->i2c_er_irq); - PT_INIT(&priv->pt); priv->pwm = pwm; pwm->pdata = pdata; pwm->priv = priv; + pbio_os_process_start(&priv->process, pbdrv_pwm_lp50xx_stm32_process_thread, priv); + // don't set funcs yet since we are not fully initialized pbdrv_init_busy_up(); } - - process_start(&pwm_lp50xx_stm32); -} - -static PT_THREAD(pbdrv_pwm_lp50xx_stm32_handle_event(pbdrv_pwm_lp50xx_stm32_priv_t * priv, process_event_t ev)) { - PT_BEGIN(&priv->pt); - - static const struct { - uint8_t reg; - uint8_t values[11]; - } __attribute__((packed)) init_data = { - .reg = DEVICE_CONFIG0, - .values = { - [DEVICE_CONFIG0] = DEVICE_CONFIG0_CHIP_EN, - [DEVICE_CONFIG1] = DEVICE_CONFIG1_POWER_SAVE_EN | DEVICE_CONFIG1_PWM_DITHERING_EN | DEVICE_CONFIG1_AUTO_INCR_EN, - [LED_CONFIG0] = 0, - [BANK_BRIGHTNESS] = 0, - [BANK_A_COLOR] = 0, - [BANK_B_COLOR] = 0, - [BANK_C_COLOR] = 0, - // Official LEGO firmware has LED0_BRIGHTNESS set to 255 and LED1_BRIGHTNESS - // set to 190 but then divides the incoming PWM duty cycle by 5. By doing - // the divide by 5 here, we end up with the same max brightness while - // allowing full dynamic range of the PWM duty cycle. - [LED0_BRIGHTNESS] = 51, - [LED1_BRIGHTNESS] = 38, - [LED2_BRIGHTNESS] = 0, - [LED3_BRIGHTNESS] = 0, - } - }; - - HAL_FMPI2C_Master_Transmit_DMA(&priv->hfmpi2c, I2C_ADDR, (void *)&init_data, sizeof(init_data)); - PT_WAIT_UNTIL(&priv->pt, HAL_FMPI2C_GetState(&priv->hfmpi2c) == HAL_FMPI2C_STATE_READY); - - // initialization is finished so consumers can use this PWM device now. - priv->pwm->funcs = &pbdrv_pwm_lp50xx_stm32_funcs; - pbdrv_init_busy_down(); - - for (;;) { - PT_WAIT_UNTIL(&priv->pt, priv->changed); - - static struct { - uint8_t reg; - uint8_t values[LP50XX_NUM_CH]; - } __attribute__((packed)) color_data = { - .reg = OUT0_COLOR, - }; - - memcpy(color_data.values, priv->values, LP50XX_NUM_CH); - HAL_FMPI2C_Master_Transmit_DMA(&priv->hfmpi2c, I2C_ADDR, (void *)&color_data, sizeof(color_data)); - priv->changed = false; - PT_WAIT_UNTIL(&priv->pt, HAL_FMPI2C_GetState(&priv->hfmpi2c) == HAL_FMPI2C_STATE_READY); - } - - PT_END(&priv->pt); } /** @@ -287,24 +292,7 @@ void pbdrv_pwm_lp50xx_stm32_i2c_er_irq(uint8_t index) { } void HAL_FMPI2C_MasterTxCpltCallback(FMPI2C_HandleTypeDef *hfmpi2c) { - process_poll(&pwm_lp50xx_stm32); -} - -PROCESS_THREAD(pwm_lp50xx_stm32, ev, data) { - PROCESS_BEGIN(); - - // need to allow all drivers to init first - PROCESS_PAUSE(); - - for (;;) { - for (int i = 0; i < PBDRV_CONFIG_PWM_LP50XX_STM32_NUM_DEV; i++) { - pbdrv_pwm_lp50xx_stm32_priv_t *priv = &dev_priv[i]; - pbdrv_pwm_lp50xx_stm32_handle_event(priv, ev); - } - PROCESS_WAIT_EVENT(); - } - - PROCESS_END(); + pbio_os_request_poll(); } #endif // PBDRV_CONFIG_PWM_LP50XX_STM32 diff --git a/lib/pbio/include/pbio/os.h b/lib/pbio/include/pbio/os.h new file mode 100644 index 000000000..e3db3bd5b --- /dev/null +++ b/lib/pbio/include/pbio/os.h @@ -0,0 +1,282 @@ +// SPDX-License-Identifier: BSD-3-Clause +// Copyright (c) 2004-2005, Swedish Institute of Computer Science +// Copyright (c) 2025 The Pybricks Authors + +/* + * Copyright (c) 2004-2005, Swedish Institute of Computer Science. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * 3. Neither the name of the Institute nor the names of its contributors + * may be used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE INSTITUTE AND CONTRIBUTORS ``AS IS'' AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE INSTITUTE OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS + * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUASYNCION) + * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY + * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF + * SUCH DAMAGE. + * + * Author: Adam Dunkels + * + * Implementation of local continuations based on switch() statement + * + * This implementation of local continuations uses the C switch() + * statement to resume execution of a function somewhere inside the + * function's body. The implementation is based on the fact that + * switch() statements are able to jump directly into the bodies of + * control structures such as if() or while() statements. + * + * This implementation borrows heavily from Simon Tatham's coroutines + * implementation in C: + * http://www.chiark.greenend.org.uk/~sgtatham/coroutines.html + * + * --------------------------------------------------------------------------- + * + * The code in this file stems from Contiki's implementation of protothreads as + * listed above. This adaptation changes the API to work better with Pybricks. + */ + +#ifndef _PBIO_OS_H_ +#define _PBIO_OS_H_ + +#include +#include +#include + +/** + * Millisecond timer. + */ +typedef struct pbio_os_timer_t { + uint32_t start; + uint32_t duration; +} pbio_os_timer_t; + +void pbio_os_timer_set(pbio_os_timer_t *timer, uint32_t duration); + +bool pbio_os_timer_is_expired(pbio_os_timer_t *timer); + + +/** + * WARNING! LC implementation using switch() does not work if an + * PB_LC_SET() is done within another switch() statement! + */ + +#define PB_LC_FALLTHROUGH __attribute__((fallthrough)); +#define PB_LC_INIT(state) *state = 0; +#define PB_LC_RESUME(state) switch (*state) { case 0: +#define PB_LC_SET(state) *state = __LINE__; PB_LC_FALLTHROUGH; case __LINE__: +#define PB_LC_END() } + +/** + * Protothread state. Effectively the line number in the current file where it + * yields so it can jump there to resume later. + */ +typedef uint32_t pbio_os_state_t; + +typedef pbio_error_t (*pbio_os_process_func_t)(pbio_os_state_t *state, void *context); + +typedef struct _pbio_os_process_t pbio_os_process_t; + +/** + * A process. + */ +struct _pbio_os_process_t { + /** + * Pointer to the next process in the list. + */ + pbio_os_process_t *next; + /** + * State of the protothread. + */ + pbio_os_state_t state; + /** + * The protothread. + */ + pbio_os_process_func_t func; + /** + * Context passed on each call to the protothread. + */ + void *context; + /** + * Most recent result of running one iteration of the protothread. + */ + pbio_error_t err; +}; + +/** + * Initialize a protothread. + * + * Initializes a protothread. Initialization must be done prior to + * starting to execute the protothread. + * + * @param [in] state Protothread state. + */ +#define ASYNC_INIT(state) PB_LC_INIT(state) + +/** + * Declare the start of a protothread inside the C function + * implementing the protothread. + * + * This macro is used to declare the starting point of a + * protothread. It should be placed at the start of the function in + * which the protothread runs. All C statements above the ASYNC_BEGIN() + * invocation will be executed each time the protothread is scheduled. + * + * @param [in] state Protothread state. + */ +#define ASYNC_BEGIN(state) { char do_yield_now = 0; if (do_yield_now) {;} PB_LC_RESUME(state) + +/** + * Declare the end of a protothread and returns with given code. + * + * This macro is used for declaring that a protothread ends. It must + * always be used together with a matching ASYNC_BEGIN() macro. + * + * NB: In contrast to Contiki, this does not call ASYNC_INIT() before exiting. + * + * @param [in] err Error code to return. + */ +#define ASYNC_END(err) PB_LC_END(); return err; } + +/** + * Yields the protothread while the specified condition is true. + * + * @param [in] state Protothread state. + * @param [in] condition The condition. + */ +#define AWAIT_WHILE(state, condition) \ + do { \ + PB_LC_SET(state); \ + if (condition) { \ + return PBIO_ERROR_AGAIN; \ + } \ + } while (0) + +/** + * Yields the protothread until the specified condition is true. + * + * @param [in] state Protothread state. + * @param [in] condition The condition. + */ +#define AWAIT_UNTIL(state, condition) \ + do { \ + PB_LC_SET(state); \ + if (!(condition)) { \ + return PBIO_ERROR_AGAIN; \ + } \ + } while (0) + +/** + * Awaits a protothread until it is done. + * + * @param [in] state Protothread state. + * @param [in] child Protothread state of the child. + * @param [in] statement The statement to await. + */ +#define AWAIT(state, child, statement) \ + do { \ + ASYNC_INIT((child)); \ + PB_LC_SET(state); \ + if ((statement) == PBIO_ERROR_AGAIN) { \ + return PBIO_ERROR_AGAIN; \ + } \ + } while (0) + +/** + * Awaits two protothreads until one of them is done. + * + * @param [in] state Protothread state. + * @param [in] child1 Protothread state of the first child. + * @param [in] child2 Protothread state of the second child. + * @param [in] statement1 The first statement to await. + * @param [in] statement2 The second statement to await. + */ +#define AWAIT_RACE(state, child1, child2, statement1, statement2) \ + do { \ + ASYNC_INIT((child1)); \ + ASYNC_INIT((child2)); \ + PB_LC_SET(state); \ + if ((statement1) == PBIO_ERROR_AGAIN && (statement2) == PBIO_ERROR_AGAIN) { \ + return PBIO_ERROR_AGAIN; \ + } \ + } while (0) + +/** + * Yields the protothread once and polls to request handling again immediately. + * + * Should be used sparingly as it can cause busy waiting. Processes will keep + * running, but there is always another event pending. + * + * @param [in] state Protothread state. + */ +#define AWAIT_ONCE(state) \ + do { \ + do_yield_now = 1; \ + PB_LC_SET(state); \ + if (do_yield_now) { \ + pbio_os_request_poll(); \ + return PBIO_ERROR_AGAIN; \ + } \ + } while (0) + +#define AWAIT_MS(state, timer, duration) \ + do { \ + pbio_os_timer_set(timer, duration); \ + PB_LC_SET(state); \ + if (!pbio_os_timer_is_expired(timer)) { \ + return PBIO_ERROR_AGAIN; \ + } \ + } while (0) \ + +bool pbio_os_run_processes_once(void); + +void pbio_os_run_while_idle(void); + +void pbio_os_request_poll(void); + +pbio_error_t pbio_port_process_none_thread(pbio_os_state_t *state, void *context); + +void pbio_os_process_start(pbio_os_process_t *process, pbio_os_process_func_t func, void *context); + +void pbio_os_process_reset(pbio_os_process_t *process, pbio_os_process_func_t func); + +/** + * Disables interrupts and returns the previous interrupt state. + * + * Must be implemented by the platform. + * + * @return The previous interrupt state. + */ +uint32_t pbio_os_hook_disable_irq(void); + +/** + * Enables interrupts. + * + * Must be implemented by the platform. + * + * @param [in] flags The previous interrupt state. + */ +void pbio_os_hook_enable_irq(uint32_t flags); + +/** + * Waits for an interrupt. + * + * Must be implemented by the platform. + */ +void pbio_os_hook_wait_for_interrupt(void); + +#endif // _PBIO_OS_H_ diff --git a/lib/pbio/platform/nxt/platform.c b/lib/pbio/platform/nxt/platform.c index 07ca67395..b0c0b71eb 100644 --- a/lib/pbio/platform/nxt/platform.c +++ b/lib/pbio/platform/nxt/platform.c @@ -9,6 +9,8 @@ #include #include #include +#include + #include #include #include @@ -75,13 +77,13 @@ static bool bluetooth_connect(void) { return false; } - pbio_do_one_event(); + pbio_os_run_processes_once(); } nx_bt_stream_open(connection_handle); } - pbio_do_one_event(); + pbio_os_run_processes_once(); } nx_display_clear(); @@ -131,7 +133,7 @@ void SystemInit(void) { goto out; } - pbio_do_one_event(); + pbio_os_run_processes_once(); } nx_display_string("Connected. REPL.\n"); diff --git a/lib/pbio/src/os.c b/lib/pbio/src/os.c new file mode 100644 index 000000000..c7ca39bae --- /dev/null +++ b/lib/pbio/src/os.c @@ -0,0 +1,177 @@ +// SPDX-License-Identifier: MIT +// Copyright (c) 2025 The Pybricks Authors + +#include + +#include +#include + +#include +#include + +/** + * Sets the timer to expire after the specified duration. + * + * The 1ms interrupt polls all processes, so no special events are needed. + * + * @param timer The timer to initialize. + * @param duration The duration in milliseconds. + */ +void pbio_os_timer_set(pbio_os_timer_t *timer, uint32_t duration) { + timer->start = pbdrv_clock_get_ms(); + timer->duration = duration; +} + +/** + * Whether the timer has expired. + * + * @param timer The timer to check. + * @return Whether the timer has expired. + */ +bool pbio_os_timer_is_expired(pbio_os_timer_t *timer) { + return pbdrv_clock_get_ms() - timer->start >= timer->duration; +} + +/** + * Whether a poll request is pending. + */ +static bool poll_request_is_pending = false; + +/** + * Request that the event loop polls all processes. + */ +void pbio_os_request_poll(void) { + poll_request_is_pending = true; +} + +static pbio_os_process_t *process_list = NULL; + +/** + * Placeholder thread that does nothing. + * + * @param [in] state The protothread state. + * @param [in] context The context. + */ +pbio_error_t pbio_port_process_none_thread(pbio_os_state_t *state, void *context) { + return PBIO_ERROR_AGAIN; +} + +/** + * Adds a process to the list of processes to run and starts it soon. + * + * @param process The process to start. + * @param func The process thread function. + * @param context The context to pass to the process. + */ +void pbio_os_process_start(pbio_os_process_t *process, pbio_os_process_func_t func, void *context) { + + // Add the new process to the end of the list. + pbio_os_process_t *last = process_list; + if (!last) { + process_list = process; + } else { + while (last->next) { + last = last->next; + } + last->next = process; + } + + // Initialize the process. + process->next = NULL; + process->context = context; + + pbio_os_process_reset(process, func); +} + +/** + * Resets an existing process to the initial state with a new function and context. + * + * @param process The process to start. + * @param func The process thread function. Choose NULL if it does not need changing. + */ +void pbio_os_process_reset(pbio_os_process_t *process, pbio_os_process_func_t func) { + process->err = PBIO_ERROR_AGAIN; + process->state = 0; + if (func) { + process->func = func; + } + + // Request a poll to start the process soon, running to its first yield. + pbio_os_request_poll(); +} + +/** + * Drives the event loop once: Runs one iteration of all processes. + * + * Can be used in hooks from blocking loops. + * + * @return Whether there are more pending poll requests. + */ +bool pbio_os_run_processes_once(void) { + + // DELETEME: Legacy hook to drive pbio event loop until all processes migrated. + extern int pbio_do_one_event(void); + bool pbio_event_pending = pbio_do_one_event(); + + if (!poll_request_is_pending) { + + // DELETEME: Legacy pbio event loop hook until all processes migrated. + if (pbio_event_pending) { + return true; + } + + return false; + } + + poll_request_is_pending = false; + + pbio_os_process_t *process = process_list; + while (process) { + // Run one iteration of the process if not yet completed or errored. + if (process->err == PBIO_ERROR_AGAIN) { + process->err = process->func(&process->state, process->context); + } + process = process->next; + } + + // Poll requests may have been set while running the processes. + return poll_request_is_pending || pbio_event_pending; +} + +/** + * Drives the event loop from code that is waiting or sleeping. + * + * Expected to be called in a loop. This will keep running the event loop but + * enter a low power mode when possible. It will sleep for at most one + * millisecond. + */ +void pbio_os_run_while_idle(void) { + + // Run the event loop until there is no more pending poll request. + while (pbio_os_run_processes_once()) { + ; + } + + // It is possible that an interrupt occurs *just now* and sets the + // poll_request_is_pending flag. If not, we can call wait_for_interrupt(), + // which still wakes up the CPU on interrupt even though interrupts are + // otherwise disabled. + uint32_t irq_flags = pbio_os_hook_disable_irq(); + + // DELETEME: Legacy hook for pbio event loop that plays the same role as + // the pending flag. Here it ensures we don't enter sleep if there are + // pending events. Can be removed when all processes are migrated. + extern int process_nevents(void); + if (process_nevents()) { + poll_request_is_pending = true; + } + + if (!poll_request_is_pending) { + pbio_os_hook_wait_for_interrupt(); + } + pbio_os_hook_enable_irq(irq_flags); + + // Since this function is expected to be called in a loop, pending events + // will be handled right away on the next entry. If not, then they will be + // handled "soon". +} diff --git a/lib/pbio/sys/core.c b/lib/pbio/sys/core.c index 9449b96fd..c77e454d4 100644 --- a/lib/pbio/sys/core.c +++ b/lib/pbio/sys/core.c @@ -3,9 +3,8 @@ #include -#include - #include +#include #include #include @@ -51,7 +50,7 @@ void pbsys_init(void) { process_start(&pbsys_system_process); while (pbsys_init_busy()) { - pbio_do_one_event(); + pbio_os_run_processes_once(); } } @@ -64,6 +63,6 @@ void pbsys_deinit(void) { // Wait for all relevant pbsys processes to end, but at least 500 ms so we // see a shutdown animation even if the button is released sooner. while (pbsys_init_busy() || pbdrv_clock_get_ms() - start < 500) { - pbio_do_one_event(); + pbio_os_run_processes_once(); } } diff --git a/lib/pbio/sys/main.c b/lib/pbio/sys/main.c index fd848019e..b64a7b736 100644 --- a/lib/pbio/sys/main.c +++ b/lib/pbio/sys/main.c @@ -10,6 +10,7 @@ #include #include #include +#include #include #include #include @@ -93,10 +94,8 @@ int main(int argc, char **argv) { pbsys_main_program_request_start(PBIO_PYBRICKS_USER_PROGRAM_ID_REPL, PBSYS_MAIN_PROGRAM_START_REQUEST_TYPE_BOOT); #endif - // REVISIT: this can be long waiting, so we could do a more efficient - // wait (i.e. __WFI() on embedded system) - while (pbio_do_one_event()) { - } + // Drives all processes while we wait for user input. + pbio_os_run_while_idle(); if (!pbsys_main_program_start_requested()) { continue; @@ -109,7 +108,7 @@ int main(int argc, char **argv) { // Handle pending events triggered by the status change, such as // starting status light animation. - while (pbio_do_one_event()) { + while (pbio_os_run_processes_once()) { } // Run the main application. diff --git a/lib/pbio/test/test-pbio.c b/lib/pbio/test/test-pbio.c index 80cf66bef..4356a1e72 100644 --- a/lib/pbio/test/test-pbio.c +++ b/lib/pbio/test/test-pbio.c @@ -91,6 +91,16 @@ struct testcase_setup_t pbio_test_setup = { .cleanup_fn = cleanup, }; +uint32_t pbio_os_hook_disable_irq(void) { + return 0; +} + +void pbio_os_hook_enable_irq(uint32_t flags) { +} + +void pbio_os_hook_wait_for_interrupt(void) { +} + extern struct testcase_t pbdrv_bluetooth_tests[]; extern struct testcase_t pbdrv_pwm_tests[]; extern struct testcase_t pbio_angle_tests[];