From a37a589f152223c77b46db834473cd817f498981 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Thu, 1 Jul 2021 10:59:51 -0400 Subject: [PATCH] WIP: afbrs50 try SPI_LOCK --- .../distance_sensor/broadcom/afbrs50/API/Src/irq.c | 10 +++++++--- .../distance_sensor/broadcom/afbrs50/API/Src/s2pi.c | 9 ++++++--- 2 files changed, 13 insertions(+), 6 deletions(-) diff --git a/src/drivers/distance_sensor/broadcom/afbrs50/API/Src/irq.c b/src/drivers/distance_sensor/broadcom/afbrs50/API/Src/irq.c index 833cf9d486..81eb62abb6 100644 --- a/src/drivers/distance_sensor/broadcom/afbrs50/API/Src/irq.c +++ b/src/drivers/distance_sensor/broadcom/afbrs50/API/Src/irq.c @@ -1,4 +1,4 @@ - +#include #include static volatile irqstate_t irqstate_flags; @@ -10,7 +10,9 @@ static volatile irqstate_t irqstate_flags; *****************************************************************************/ void IRQ_UNLOCK(void) { - leave_critical_section(irqstate_flags); + if (!up_interrupt_context()) { + leave_critical_section(irqstate_flags); + } } /*!*************************************************************************** @@ -20,5 +22,7 @@ void IRQ_UNLOCK(void) *****************************************************************************/ void IRQ_LOCK(void) { - irqstate_flags = enter_critical_section(); + if (!up_interrupt_context()) { + irqstate_flags = enter_critical_section(); + } } diff --git a/src/drivers/distance_sensor/broadcom/afbrs50/API/Src/s2pi.c b/src/drivers/distance_sensor/broadcom/afbrs50/API/Src/s2pi.c index 4442be32f0..c70a3d1859 100644 --- a/src/drivers/distance_sensor/broadcom/afbrs50/API/Src/s2pi.c +++ b/src/drivers/distance_sensor/broadcom/afbrs50/API/Src/s2pi.c @@ -308,12 +308,14 @@ status_t S2PI_CycleCsPin(s2pi_slave_t slave) static void broadcom_s2pi_transfer_callout(void *arg) { + SPI_LOCK(s2pi_.spidev, true); perf_begin(s2pi_transfer_perf); px4_arch_gpiowrite(s2pi_.GPIOs[S2PI_CS], 0); SPI_EXCHANGE(s2pi_.spidev, s2pi_.spi_tx_data, s2pi_.spi_rx_data, s2pi_.spi_frame_size); s2pi_.Status = STATUS_IDLE; px4_arch_gpiowrite(s2pi_.GPIOs[S2PI_CS], 1); perf_end(s2pi_transfer_perf); + SPI_LOCK(s2pi_.spidev, false); /* Invoke callback if there is one */ if (s2pi_.Callback != 0) { @@ -338,12 +340,13 @@ status_t S2PI_TransferFrame(s2pi_slave_t spi_slave, uint8_t const *txData, uint8 return ERROR_S2PI_INVALID_SLAVE; } + SPI_LOCK(s2pi_.spidev, true); + /* Check the driver status, lock if idle. */ - irqstate_t irqstate_flags = px4_enter_critical_section(); status_t status = s2pi_.Status; if (status != STATUS_IDLE) { - px4_leave_critical_section(irqstate_flags); + SPI_LOCK(s2pi_.spidev, false); return status; } @@ -358,7 +361,7 @@ status_t S2PI_TransferFrame(s2pi_slave_t spi_slave, uint8_t const *txData, uint8 s2pi_.spi_frame_size = frameSize; work_queue(HPWORK, &broadcom_s2pi_transfer_work, broadcom_s2pi_transfer_callout, NULL, 0); - px4_leave_critical_section(irqstate_flags); + SPI_LOCK(s2pi_.spidev, false); return STATUS_OK; }