diff --git a/src/rp2_common/pico_status_led/status_led.c b/src/rp2_common/pico_status_led/status_led.c index 309b21e73..c7d6c1537 100644 --- a/src/rp2_common/pico_status_led/status_led.c +++ b/src/rp2_common/pico_status_led/status_led.c @@ -6,6 +6,8 @@ #include "pico/status_led.h" +#include "hardware/sync/spin_lock.h" + #if PICO_STATUS_LED_AVAILABLE && defined(CYW43_WL_GPIO_LED_PIN) && !defined(PICO_DEFAULT_LED_PIN) #define STATUS_LED_USING_WL_GPIO 1 #else @@ -33,7 +35,8 @@ static uint32_t colored_status_led_on_color = PICO_DEFAULT_COLORED_STATUS_LED_ON static bool colored_status_led_on; #if COLORED_STATUS_LED_USING_WS2812_PIO -#include +#include "hardware/pio.h" +#include "pico/time.h" #include "ws2812.pio.h" // PICO_CONFIG: PICO_COLORED_STATUS_LED_WS2812_FREQ, Frequency per bit for the WS2812 colored status LED, type=int, default=800000, group=pico_status_led @@ -41,9 +44,28 @@ static bool colored_status_led_on; #define PICO_COLORED_STATUS_LED_WS2812_FREQ 800000 #endif +// PICO_CONFIG: PICO_COLORED_STATUS_LED_RESET_DELAY_US, Required reset delay in microseconds for the WS2812 colored status LED, type=int, default=100, group=pico_status_led +#ifndef PICO_COLORED_STATUS_LED_RESET_DELAY_US +#define PICO_COLORED_STATUS_LED_RESET_DELAY_US 100 +#endif + +#ifndef PICO_COLORED_STATUS_LED_USE_DEFAULT_ALARM_POOL +#define PICO_COLORED_STATUS_LED_USE_DEFAULT_ALARM_POOL !PICO_TIME_DEFAULT_ALARM_POOL_DISABLED +#endif + static PIO pio; static uint sm; static uint offset; +static uint32_t next_value; +static uint64_t next_safe_set_time; +#if PICO_COLORED_STATUS_LED_USE_DEFAULT_ALARM_POOL +static alarm_id_t alarm_id; +static int8_t alarm_pending; +#else +#define alarm_pending false +#endif + +#define COLOR_STATUS_LED_UPDATE_TIME_US (1 + (1000000 * (PICO_COLORED_STATUS_LED_USES_WRGB ? 32 : 24)) / PICO_COLORED_STATUS_LED_WS2812_FREQ) // Extract from 0xWWRRGGBB #define RED(c) (((c) >> 16) & 0xff) @@ -51,8 +73,9 @@ static uint offset; #define BLUE(c) (((c) >> 0) & 0xff) #define WHITE(c) (((c) >> 24) && 0xff) -bool set_ws2812(uint32_t value) { +static void unsafe_set_ws2812(uint32_t value, uint64_t now) { if (pio) { + pio_sm_drain_tx_fifo(pio, sm); // want to jump past any previous queued values #if PICO_COLORED_STATUS_LED_USES_WRGB // Convert to 0xWWGGRRBB pio_sm_put_blocking(pio, sm, WHITE(value) << 24 | GREEN(value) << 16 | RED(value) << 8 | BLUE(value)); @@ -60,9 +83,66 @@ bool set_ws2812(uint32_t value) { // Convert to 0xGGRRBB00 pio_sm_put_blocking(pio, sm, GREEN(value) << 24 | RED(value) << 16 | BLUE(value) << 8); #endif - return true; + next_safe_set_time = now + COLOR_STATUS_LED_UPDATE_TIME_US + PICO_COLORED_STATUS_LED_RESET_DELAY_US; + } +} + +#if PICO_COLORED_STATUS_LED_USE_DEFAULT_ALARM_POOL +static int64_t deferred_set_ws2812(__unused alarm_id_t id, __unused void *user_data) { + spin_lock_t *spin_lock = spin_lock_instance(PICO_SPINLOCK_ID_ATOMIC); + uint32_t save = spin_lock_blocking(spin_lock); + unsafe_set_ws2812(next_value, time_us_64()); + alarm_id = 0; + alarm_pending--; + spin_unlock(spin_lock, save); + return 0; +} +#endif + +static void set_ws2812(uint32_t value) { + spin_lock_t *spin_lock = spin_lock_instance(PICO_SPINLOCK_ID_ATOMIC); + uint32_t save = spin_lock_blocking(spin_lock); + next_value = value; + while (true) { +#if !PICO_COLORED_STATUS_LED_USE_DEFAULT_ALARM_POOL + uint64_t now = time_us_64(); + if (now >= next_safe_set_time) { + unsafe_set_ws2812(value, now); + spin_unlock(spin_lock, save); + break; + } else { + spin_unlock(spin_lock, save); + busy_wait_until(next_safe_set_time); + save = spin_lock_blocking(spin_lock); + } +#else + if (alarm_pending) { + // we defer the set to the already waiting alarm + break; + } else { + uint64_t now = time_us_64(); + if (now >= next_safe_set_time) { + unsafe_set_ws2812(value, now); + spin_unlock(spin_lock, save); + break; + } else { + // we want to defer the set until it is safe to do so + // + // note we use alarm_pending separate from alarm_id, as alarm_id may be returned even if the + // alarm fires during the add_alarm_at. and don't use a boolean because if we fail + // to add the alarm, we don't know what has happened in between since we unlock the spin lock + // before adding the alarm since that is a slowish call + alarm_pending++; + spin_unlock(spin_lock, save); + alarm_id = add_alarm_at(next_safe_set_time, deferred_set_ws2812, NULL, true); + if (alarm_id > 0) break; + busy_wait_until(next_safe_set_time); + save = spin_lock_blocking(spin_lock); + alarm_pending--; + } + } +#endif } - return false; } #endif @@ -76,20 +156,20 @@ uint32_t colored_status_led_get_on_color(void) { } bool colored_status_led_set_state(bool led_on) { - bool success = false; if (colored_status_led_supported()) { #if COLORED_STATUS_LED_USING_WS2812_PIO - success = true; if (led_on) { // Turn the LED "on" even if it was already on, as the color might have changed - success = set_ws2812(colored_status_led_on_color); - } else if (!led_on && colored_status_led_on) { - success = set_ws2812(0); + set_ws2812(colored_status_led_on_color); + } else if (colored_status_led_on) { + set_ws2812(0); } + colored_status_led_on = led_on; + return true; #endif } - if (success) colored_status_led_on = led_on; - return success; + ((void)led_on); + return false; } bool colored_status_led_get_state(void) { @@ -165,6 +245,13 @@ void status_led_deinit(void) { status_led_context = NULL; #endif #if COLORED_STATUS_LED_USING_WS2812_PIO +#if PICO_COLORED_STATUS_LED_USE_DEFAULT_ALARM_POOL + if (alarm_id > 0) { + cancel_alarm(alarm_id); + alarm_id = 0; + } + alarm_pending = false; +#endif if (pio) { pio_remove_program_and_unclaim_sm(&ws2812_program, pio, sm, offset); pio = NULL;