From 59f9b179f9d85f811a4456a45da3686dc62f030d Mon Sep 17 00:00:00 2001 From: Mitch Bradley Date: Fri, 25 Oct 2024 18:08:07 -1000 Subject: [PATCH] Fixed bug in i2s engine that broke homing The problem was that pulse_func() can do nothing if it is not awake, and in that case, the _pulse_data variable was holding old data. --- FluidNC/esp32/i2s_engine.c | 69 ++++++++++-------------------- FluidNC/include/Driver/i2s_out.h | 16 +------ FluidNC/src/Pins/I2SOPinDetail.cpp | 1 - FluidNC/src/Stepper.cpp | 4 +- 4 files changed, 25 insertions(+), 65 deletions(-) diff --git a/FluidNC/esp32/i2s_engine.c b/FluidNC/esp32/i2s_engine.c index 4dae502f9..4be514616 100644 --- a/FluidNC/esp32/i2s_engine.c +++ b/FluidNC/esp32/i2s_engine.c @@ -31,33 +31,10 @@ #include "esp_intr_alloc.h" -/* 16-bit mode: 1000000 usec / ((160000000 Hz) / 10 / 2) x 16 bit/pulse x 2(stereo) = 4 usec/pulse */ /* 32-bit mode: 1000000 usec / ((160000000 Hz) / 5 / 2) x 32 bit/pulse x 2(stereo) = 4 usec/pulse */ const uint32_t I2S_OUT_USEC_PER_PULSE = 2; -// The library routines are not in IRAM so they can crash when called from FLASH -// The GCC intrinsic versions which are prefixed with __ are compiled inline -#define USE_INLINE_ATOMIC - -#ifdef USE_INLINE_ATOMIC -# define MEMORY_MODEL_FETCH __ATOMIC_RELAXED -# define MEMORY_MODEL_STORE __ATOMIC_RELAXED -# define ATOMIC_LOAD(var) __atomic_load_n(var, MEMORY_MODEL_FETCH) -# define ATOMIC_STORE(var, val) __atomic_store_n(var, val, MEMORY_MODEL_STORE) -# define ATOMIC_FETCH_AND(var, val) __atomic_fetch_and(var, val, MEMORY_MODEL_FETCH) -# define ATOMIC_FETCH_OR(var, val) __atomic_fetch_or(var, val, MEMORY_MODEL_FETCH) static uint32_t i2s_out_port_data = 0; -#else -# include -# define ATOMIC_LOAD(var) atomic_load(var) -# define ATOMIC_STORE(var, val) atomic_store(var, val) -# define ATOMIC_FETCH_AND(var, val) atomic_fetch_and(var, val) -# define ATOMIC_FETCH_OR(var, val) atomic_fetch_or(var, val) -static std::atomic i2s_out_port_data = ATOMIC_VAR_INIT(0); - -#endif - -const int I2S_SAMPLE_SIZE = 4; /* 4 bytes, 32 bits per sample */ static int i2s_out_initialized = 0; @@ -65,21 +42,9 @@ static pinnum_t i2s_out_ws_pin = 255; static pinnum_t i2s_out_bck_pin = 255; static pinnum_t i2s_out_data_pin = 255; -#if I2S_OUT_NUM_BITS == 16 -# define DATA_SHIFT 16 -#else -# define DATA_SHIFT 0 -#endif - // // Internal functions // -void IRAM_ATTR i2s_out_push_fifo(int count) { - uint32_t portData = i2s_out_port_data << DATA_SHIFT; - for (int i = 0; i < count; i++) { - I2S0.fifo_wr = portData; - } -} static inline void i2s_out_reset_tx_rx() { i2s_ll_tx_reset(&I2S0); @@ -137,7 +102,7 @@ static int i2s_out_stop() { gpio_write(i2s_out_bck_pin, 0); // Transmit recovery data to 74HC595 - uint32_t port_data = ATOMIC_LOAD(&i2s_out_port_data); // current expanded port value + uint32_t port_data = i2s_out_port_data; // current expanded port value i2s_out_gpio_shiftout(port_data); return 0; @@ -149,7 +114,7 @@ static int i2s_out_start() { } // Transmit recovery data to 74HC595 - uint32_t port_data = ATOMIC_LOAD(&i2s_out_port_data); // current expanded port value + uint32_t port_data = i2s_out_port_data; // current expanded port value i2s_out_gpio_shiftout(port_data); // Attach I2S to specified GPIO pin @@ -183,14 +148,14 @@ void i2s_out_delay() { void IRAM_ATTR i2s_out_write(pinnum_t pin, uint8_t val) { uint32_t bit = 1 << pin; if (val) { - ATOMIC_FETCH_OR(&i2s_out_port_data, bit); + i2s_out_port_data |= bit; } else { - ATOMIC_FETCH_AND(&i2s_out_port_data, ~bit); + i2s_out_port_data &= ~bit; } } uint8_t i2s_out_read(pinnum_t pin) { - uint32_t port_data = ATOMIC_LOAD(&i2s_out_port_data); + uint32_t port_data = i2s_out_port_data; return !!(port_data & (1 << pin)); } @@ -199,7 +164,7 @@ int i2s_out_init(i2s_out_init_t* init_param) { return -1; } - ATOMIC_STORE(&i2s_out_port_data, init_param->init_val); + i2s_out_port_data = init_param->init_val; // To make sure hardware is enabled before any hardware register operations. periph_module_reset(PERIPH_I2S0_MODULE); @@ -367,12 +332,18 @@ static uint32_t _delay_counts = 40; static uint32_t _tick_divisor; static void IRAM_ATTR set_timer_ticks(uint32_t ticks) { - _delay_counts = ticks / _tick_divisor; + if (ticks) { + _delay_counts = ticks / _tick_divisor; + } } static void IRAM_ATTR start_timer() { - i2s_ll_enable_intr(&I2S0, I2S_TX_PUT_DATA_INT_ENA, 1); - i2s_ll_clear_intr_status(&I2S0, I2S_PUT_DATA_INT_CLR); + static bool once = true; + if (once) { + i2s_ll_enable_intr(&I2S0, I2S_TX_PUT_DATA_INT_ENA, 1); + i2s_ll_clear_intr_status(&I2S0, I2S_PUT_DATA_INT_CLR); + once = false; + } } static void IRAM_ATTR stop_timer() { i2s_ll_enable_intr(&I2S0, I2S_TX_PUT_DATA_INT_ENA, 0); @@ -382,8 +353,8 @@ static void IRAM_ATTR i2s_isr() { // gpio_write(12, 1); // For debugging // Keeping local copies of this information speeds up the ISR - uint32_t pulse_data = _pulse_data; uint32_t delay_data = i2s_out_port_data; + uint32_t pulse_data = _pulse_data; uint32_t remaining_pulse_counts = _remaining_pulse_counts; uint32_t remaining_delay_counts = _remaining_delay_counts; @@ -398,6 +369,10 @@ static void IRAM_ATTR i2s_isr() { --i; --remaining_delay_counts; } else { + // Set _pulse_data to a safe value in case pulse_func() does nothing, + // which can happen if it is not awake + _pulse_data = i2s_out_port_data; + _pulse_func(); // Reload from variables that could have been modified by pulse_func @@ -405,7 +380,7 @@ static void IRAM_ATTR i2s_isr() { delay_data = i2s_out_port_data; remaining_pulse_counts = pulse_data == delay_data ? 0 : _pulse_counts; - remaining_delay_counts = _delay_counts - _pulse_counts; + remaining_delay_counts = _delay_counts - remaining_pulse_counts; } } while (i); @@ -471,7 +446,7 @@ static IRAM_ATTR void set_dir_pin(int pin, int level) { // that we use for step pulses, but the optimizaton might not // be worthwhile since direction changes are infrequent. static IRAM_ATTR void finish_dir() { - i2s_out_push_fifo(1); + I2S0.fifo_wr = i2s_out_port_data; delay_us(_dir_delay_us); } diff --git a/FluidNC/include/Driver/i2s_out.h b/FluidNC/include/Driver/i2s_out.h index d757b0121..98415428a 100644 --- a/FluidNC/include/Driver/i2s_out.h +++ b/FluidNC/include/Driver/i2s_out.h @@ -11,14 +11,7 @@ extern "C" { #include "Driver/fluidnc_gpio.h" -/* Assert */ -#if defined(I2S_OUT_NUM_BITS) -# if (I2S_OUT_NUM_BITS != 16) && (I2S_OUT_NUM_BITS != 32) -# error "I2S_OUT_NUM_BITS should be 16 or 32" -# endif -#else -# define I2S_OUT_NUM_BITS 32 -#endif +#define I2S_OUT_NUM_BITS 32 // # define I2SO(n) (I2S_OUT_PIN_BASE + n) @@ -67,13 +60,6 @@ uint8_t i2s_out_read(pinnum_t pin); */ void i2s_out_write(pinnum_t pin, uint8_t val); -/* - Push the I2S output value that was constructed by a series of - i2s_out_write() calls to the I2S FIFO so it will be shifted out - count: Number of repetitions -*/ -void i2s_out_push_fifo(int count); - /* Dynamically delay until the Shift Register Pin changes according to the current I2S processing state and mode. diff --git a/FluidNC/src/Pins/I2SOPinDetail.cpp b/FluidNC/src/Pins/I2SOPinDetail.cpp index 3ab23bf8f..40e18042d 100644 --- a/FluidNC/src/Pins/I2SOPinDetail.cpp +++ b/FluidNC/src/Pins/I2SOPinDetail.cpp @@ -51,7 +51,6 @@ namespace Pins { _lastWrittenValue = high; i2s_out_write(_index, _inverted ^ (bool)high); - i2s_out_push_fifo(1); i2s_out_delay(); } } diff --git a/FluidNC/src/Stepper.cpp b/FluidNC/src/Stepper.cpp index 563844052..ecbd78a93 100644 --- a/FluidNC/src/Stepper.cpp +++ b/FluidNC/src/Stepper.cpp @@ -204,6 +204,7 @@ bool IRAM_ATTR Stepper::pulse_func() { auto n_axis = Axes::_numberAxis; Stepping::step(st.step_outbits, st.dir_outbits); + st.step_outbits = 0; // If there is no step segment, attempt to pop one from the stepper buffer if (st.exec_segment == NULL) { @@ -244,11 +245,10 @@ bool IRAM_ATTR Stepper::pulse_func() { protocol_send_event_from_ISR(&cycleStopEvent); awake = false; + Stepping::unstep(); return false; // Nothing to do but exit. } } - // Reset step out bits. - st.step_outbits = 0; for (int axis = 0; axis < n_axis; axis++) { // Execute step displacement profile by Bresenham line algorithm