Improved CAN, locking and general IRQ performance

Fixes issues with CAN stopping when overwhelmed by data RX\TX.
Keeps can in freezemode until explicitly activated.
CANINTF_TX is now a CAN TX in progress flag.
Runtime asserts are now disabled.

Signed-off-by: Dominik Sliwa <dominik.sliwa@toradex.com>

4 files changed, 159 insertions, 80 deletions

diff --git a/source/adc_task.c b/source/adc_task.c
index 2615943..daf4716 100644
--- a/source/adc_task.c
+++ b/source/adc_task.c
@@ -270,7 +270,7 @@ int tsc_registers(dspi_transfer_t *spi_transfer)
 	if (rx_buf[0] == APALIS_TK1_K20_WRITE_INST) {
 		switch (rx_buf[1]) {
 		case APALIS_TK1_K20_TSCREG:
-			return -ENOENT;
+			return 0;
 		default:
 			return -ENOENT;
 		}
@@ -320,7 +320,7 @@ int adc_registers(dspi_transfer_t *spi_transfer)
 	if (rx_buf[0] == APALIS_TK1_K20_WRITE_INST) {
 		switch (rx_buf[1]) {
 		case APALIS_TK1_K20_ADCREG:
-			return -ENOENT;
+			return 0;
 		default:
 			return -ENOENT;
 		}
diff --git a/source/can_task.c b/source/can_task.c
index bab19ac..e34d35d 100644
--- a/source/can_task.c
+++ b/source/can_task.c
@@ -14,19 +14,13 @@
 #define CAN_HEADER_MAX_LEN	5
 #define CAN_TRANSFER_BUF_LEN	(CAN_HEADER_MAX_LEN + CAN_FRAME_MAX_LEN)
 
-#define CAN_CTRLMODE_NORMAL		0x00	/* normal mode */
-#define CAN_CTRLMODE_LOOPBACK		0x01	/* Loopback mode */
-#define CAN_CTRLMODE_LISTENONLY		0x02 	/* Listen-only mode */
-#define CAN_CTRLMODE_3_SAMPLES		0x04	/* Triple sampling mode */
-#define CAN_CTRLMODE_ONE_SHOT		0x08	/* One-Shot mode */
-#define CAN_CTRLMODE_BERR_REPORTING	0x10	/* Bus-error reporting */
-
-#define CANCTRL_MODMASK	0x03
-#define CANCTRL_INTMASK	0x38
-#define CANCTRL_INTEN	BIT(2)
-#define CANINTF_RX	BIT(3)
-#define CANINTF_TX	BIT(4)
-#define CANINTF_ERR	BIT(5)
+#define CANCTRL_MODMASK		(BIT(1) | BIT(0))
+#define CANCTRL_INTEN		BIT(2)
+#define CANINTF_RX		BIT(3)
+#define CANINTF_TX		BIT(4)
+#define CANINTF_ERR		BIT(5)
+#define CANCTRL_ENABLE		BIT(6)
+#define CANCTRL_INTMASK	(CANINTF_RX | CANINTF_TX | CANINTF_ERR)
 
 #define EFLG_EWARN	0x01
 #define EFLG_RXWAR	0x02
@@ -39,24 +33,26 @@
 struct can_registers {
 	CAN_Type *base;
 	flexcan_handle_t handle;
-	uint8_t can_status_reg;
+	uint8_t can_enable;
 	uint8_t can_err_reg;
 	uint8_t can_mode;
 	uint8_t frames_in_buf;
 	uint8_t rx_buf_top;
 	uint8_t rx_buf_bottom;
+	uint8_t tx_counter;
 };
 
 static uint8_t data_buffer[2][CAN_RX_BUF_SIZE][CAN_TRANSFER_BUF_LEN];
 
 static struct can_registers can_regs[2];
 
-static void generate_can_irq(uint8_t id)
+static inline void generate_can_irq(uint8_t id)
 {
-	if (id == 0)
+	if (id == 0) {
 		GPIO_TogglePinsOutput(GPIOB, 1u << 8u);
-	else
+	} else {
 		GPIO_TogglePinsOutput(GPIOE, 1u << 26u);
+	}
 }
 
 void can_tx_notify_task(void *pvParameters)
@@ -66,17 +62,25 @@ void can_tx_notify_task(void *pvParameters)
 	while(1){
 		xTaskNotifyWait( 0x00, 0xFFFFFFFF, &ulInterruptStatus, portMAX_DELAY);
 		if (ulInterruptStatus & 0x01) {
-			registers[APALIS_TK1_K20_CANREG + APALIS_TK1_K20_CAN_DEV_OFFSET(0)] |= CANINTF_TX;
+			registers[APALIS_TK1_K20_CANREG + APALIS_TK1_K20_CAN_DEV_OFFSET(0)] &= ~CANINTF_TX;
 			generate_can_irq(0);
 		}
 		if (ulInterruptStatus & 0x02) {
-			registers[APALIS_TK1_K20_CANREG + APALIS_TK1_K20_CAN_DEV_OFFSET(1)] |= CANINTF_TX;
+			registers[APALIS_TK1_K20_CANREG + APALIS_TK1_K20_CAN_DEV_OFFSET(1)] &= ~CANINTF_TX;
+			generate_can_irq(1);
+		}
+		if (ulInterruptStatus & 0x04) {
+			registers[APALIS_TK1_K20_CANREG + APALIS_TK1_K20_CAN_DEV_OFFSET(0)] |= CANINTF_ERR;
+			generate_can_irq(0);
+		}
+		if (ulInterruptStatus & 0x08) {
+			registers[APALIS_TK1_K20_CANREG + APALIS_TK1_K20_CAN_DEV_OFFSET(1)] |= CANINTF_ERR;
 			generate_can_irq(1);
 		}
 	}
 }
 
-static void flexcan_callback(CAN_Type *base, flexcan_handle_t *handle, status_t status, uint32_t result, void *userData)
+static BaseType_t flexcan_callback(CAN_Type *base, flexcan_handle_t *handle, status_t status, uint32_t result, void *userData)
 {
 	callback_message_t * cb = (callback_message_t*) userData;
 	BaseType_t reschedule = pdFALSE;
@@ -94,6 +98,7 @@ static void flexcan_callback(CAN_Type *base, flexcan_handle_t *handle, status_t
 			switch ((int)base)
 			{
 			case (int)CAN0:
+
 				xTaskNotifyFromISR(can_tx_notify_task_handle, 0x01, eSetBits, &reschedule);
 				break;
 			case (int)CAN1:
@@ -104,10 +109,40 @@ static void flexcan_callback(CAN_Type *base, flexcan_handle_t *handle, status_t
 		}
 		break;
 
+	case kStatus_FLEXCAN_ErrorStatus:
+		if ((result & (kFLEXCAN_TxWarningIntFlag)) != 0) {
+			switch ((int)base)
+			{
+			case (int)CAN0:
+				registers[APALIS_TK1_K20_CANERR + APALIS_TK1_K20_CAN_DEV_OFFSET(0)] |= EFLG_TXWAR;
+				xTaskNotifyFromISR(can_tx_notify_task_handle, 0x04, eSetBits, &reschedule);
+				break;
+			case (int)CAN1:
+				registers[APALIS_TK1_K20_CANERR + APALIS_TK1_K20_CAN_DEV_OFFSET(1)] |= EFLG_TXWAR;
+				xTaskNotifyFromISR(can_tx_notify_task_handle, 0x08, eSetBits, &reschedule);
+				break;
+			}
+		}
+
+		if ((result & (kFLEXCAN_BusOffIntFlag)) != 0) {
+			switch ((int)base)
+			{
+			case (int)CAN0:
+				registers[APALIS_TK1_K20_CANERR + APALIS_TK1_K20_CAN_DEV_OFFSET(0)] |= EFLG_TXBO;
+				xTaskNotifyFromISR(can_tx_notify_task_handle, 0x04, eSetBits, &reschedule);
+				break;
+			case (int)CAN1:
+				registers[APALIS_TK1_K20_CANERR + APALIS_TK1_K20_CAN_DEV_OFFSET(1)] |= EFLG_TXBO;
+				xTaskNotifyFromISR(can_tx_notify_task_handle, 0x08, eSetBits, &reschedule);
+				break;
+			}
+		}
+		break;
 	default:
 		break;
 	}
-	portYIELD_FROM_ISR(reschedule);
+
+	return reschedule;
 }
 
 static void CAN_Init(uint8_t id)
@@ -139,7 +174,11 @@ static void CAN_Init(uint8_t id)
 	FLEXCAN_SetRxFifoConfig(can_regs[id].base, &fifoConfig, true);
 
 	/* errata #5641 */
-	FLEXCAN_SetTxMbConfig(can_regs[id].base, TX_MESSAGE_BUFFER_NUM0 - 1, true);
+	can_regs[id].base->MB[TX_MESSAGE_BUFFER_NUM0 - 1].ID = 0x0;
+	can_regs[id].base->MB[TX_MESSAGE_BUFFER_NUM0 - 1].WORD0 = 0x0;
+	can_regs[id].base->MB[TX_MESSAGE_BUFFER_NUM0 - 1].WORD1 = 0x0;
+	can_regs[id].base->MB[TX_MESSAGE_BUFFER_NUM0 - 1].CS = CAN_CS_CODE(0x8);
+	can_regs[id].base->MB[TX_MESSAGE_BUFFER_NUM0 - 1].CS = CAN_CS_CODE(0x8);
 
 	/* Setup Tx Message Buffer. */
 	FLEXCAN_SetTxMbConfig(can_regs[id].base, TX_MESSAGE_BUFFER_NUM0, true);
@@ -148,6 +187,7 @@ static void CAN_Init(uint8_t id)
 uint8_t available_data[2];
 
 static void can_calculate_available_data(uint8_t id) {
+	taskENTER_CRITICAL();
 	if ( can_regs[id].rx_buf_bottom <= can_regs[id].rx_buf_top)
 		available_data[id] = can_regs[id].rx_buf_top - can_regs[id].rx_buf_bottom;
 	else
@@ -155,13 +195,17 @@ static void can_calculate_available_data(uint8_t id) {
 
 	available_data[id] = (available_data[id] > APALIS_TK1_MAX_CAN_DMA_XREF) ? APALIS_TK1_MAX_CAN_DMA_XREF:available_data[id];
 	registers[APALIS_TK1_K20_CAN_IN_BUF_CNT + APALIS_TK1_K20_CAN_DEV_OFFSET(id)] = available_data[id];
+
 	if (can_regs[id].rx_buf_bottom == can_regs[id].rx_buf_top)
 		registers[APALIS_TK1_K20_CANREG + APALIS_TK1_K20_CAN_DEV_OFFSET(id)] &= ~CANINTF_RX;
+	else
+		registers[APALIS_TK1_K20_CANREG + APALIS_TK1_K20_CAN_DEV_OFFSET(id)] |= CANINTF_RX;
+	taskEXIT_CRITICAL();
 }
 
 void can_spi_read_complete(uint8_t id)
 {
-
+	can_calculate_available_data(id);
 }
 
 static void frame_to_buffer(flexcan_frame_t *frame, uint8_t id) {
@@ -195,24 +239,26 @@ static void frame_to_buffer(flexcan_frame_t *frame, uint8_t id) {
 	case 2:data_buffer[id][top_frame][5 + 1] = frame->dataByte1;
 	case 1:data_buffer[id][top_frame][5] = frame->dataByte0;
 	}
+
 	can_regs[id].rx_buf_top++;
 	can_regs[id].rx_buf_top %= CAN_RX_BUF_SIZE;
+
 	can_calculate_available_data(id);
 }
 
-static void can_fifo_rx(uint8_t id, flexcan_fifo_transfer_t * rxXfer)
+static inline void can_fifo_rx(uint8_t id, flexcan_fifo_transfer_t * rxXfer)
 {
 	callback_message_t *can_msg = (callback_message_t *) can_regs[id].handle.userData;
-	taskENTER_CRITICAL();
+
 	FLEXCAN_TransferReceiveFifoNonBlocking(can_regs[id].base, &can_regs[id].handle, rxXfer);
-	taskEXIT_CRITICAL();
 	xSemaphoreTake(can_msg->sem, portMAX_DELAY);
-	frame_to_buffer(rxXfer->frame, id);
-	can_regs[id].frames_in_buf++;
-	registers[APALIS_TK1_K20_CANREG + APALIS_TK1_K20_CAN_DEV_OFFSET(id)] |= CANINTF_RX;
-	generate_can_irq(id);
-	if (can_regs[id].frames_in_buf >= CAN_RX_BUF_SIZE)
-		vTaskSuspend(NULL);
+	if (can_msg->async_status == pdTRUE) {
+		frame_to_buffer(rxXfer->frame, id);
+		can_regs[id].frames_in_buf++;
+		generate_can_irq(id);
+		if (can_regs[id].frames_in_buf >= CAN_RX_BUF_SIZE)
+			vTaskSuspend(NULL);
+	}
 }
 
 void can0_task(void *pvParameters) {
@@ -263,16 +309,25 @@ void can1_task(void *pvParameters) {
 
 static void can_change_mode(int id, uint8_t new_mode)
 {
-	CAN_Type *base = id ? CAN1:CAN0;
-
 	can_regs[id].can_mode = new_mode;
 
-	switch (new_mode){
-	case CAN_CTRLMODE_LOOPBACK:
-		base->CTRL1 = base->CTRL1 | CAN_CTRL1_LPB_MASK;
-		break;
-	case CAN_CTRLMODE_NORMAL:
-		base->CTRL1 = base->CTRL1 & ~CAN_CTRL1_LPB_MASK;
+	FLEXCAN_SetMode(can_regs[id].base, new_mode);
+
+}
+
+static void can_enable(int id, uint8_t enable)
+{
+	can_regs[id].can_mode = enable;
+
+	if (enable) {
+		callback_message_t *can_msg = (callback_message_t *) can_regs[id].handle.userData;
+		FLEXCAN_ExitFreezeMode(can_regs[id].base);
+		can_msg->async_status = pdFALSE;
+		xSemaphoreGive(can_msg->sem);
+		generate_can_irq(id);
+	} else {
+		FLEXCAN_TransferAbortReceiveFifo(can_regs[id].base, &can_regs[id].handle);
+		FLEXCAN_EnterFreezeMode(can_regs[id].base);
 	}
 
 }
@@ -282,18 +337,21 @@ static uint8_t set_canreg (int id, uint8_t value)
 	registers[APALIS_TK1_K20_CANREG + APALIS_TK1_K20_CAN_DEV_OFFSET(id)] = value;
 	if ( can_regs[id].can_mode != (value & CANCTRL_MODMASK) )
 		can_change_mode(id, (value & CANCTRL_MODMASK));
+	if (can_regs[id].can_enable != (value & CANCTRL_ENABLE))
+		can_enable(id, (value & CANCTRL_ENABLE));
 	return 0;
 }
 
 static uint8_t clr_canreg (int id, uint8_t mask)
 {
-	mask &= (CANINTF_TX | CANINTF_ERR);
+	mask &= (CANINTF_RX | CANINTF_TX | CANINTF_ERR);
 	registers[APALIS_TK1_K20_CANREG + APALIS_TK1_K20_CAN_DEV_OFFSET(id)] &= ~mask;
 	return 0;
 }
 
 static uint8_t set_canerr (int id, uint8_t value)
 {
+	registers[APALIS_TK1_K20_CANERR + APALIS_TK1_K20_CAN_DEV_OFFSET(id)] = value;
 	return 0;
 }
 
@@ -304,9 +362,9 @@ static uint8_t set_canbadreg (int id, uint8_t value)
 	return 0;
 }
 
-static uint8_t set_canbittimig (int id, uint16_t value, int16_t mask)
+static uint8_t set_canbittiming (int id, uint16_t value, int16_t mask)
 {
-	/* According to NXP we should use default setting */
+	/* According to NXP we should use default settings */
 	return 0;
 }
 
@@ -339,12 +397,16 @@ uint8_t can_sendframe(uint8_t id, uint8_t *data, uint8_t len)
 
 	txXfer.frame = &tx_frame;
 	txXfer.mbIdx = TX_MESSAGE_BUFFER_NUM0;
-
-	taskENTER_CRITICAL();
-	FLEXCAN_TransferSendNonBlocking(can_regs[id].base , &can_regs[id].handle, &txXfer);
-	taskEXIT_CRITICAL();
-
-	return 0;
+	if( tx_frame.length <= 8 ) {
+		FLEXCAN_TransferSendNonBlocking(can_regs[id].base , &can_regs[id].handle, &txXfer);
+		registers[APALIS_TK1_K20_CANREG + APALIS_TK1_K20_CAN_DEV_OFFSET(id)] |= CANINTF_TX;
+		return 0;
+	}
+	else {
+		registers[APALIS_TK1_K20_CANREG + APALIS_TK1_K20_CAN_DEV_OFFSET(id)] &= ~CANINTF_TX;
+		generate_can_irq(id);
+		return -EIO;
+	}
 }
 
 uint16_t can_readframe(uint8_t id, dspi_transfer_t *spi_transfer)
@@ -354,11 +416,11 @@ uint16_t can_readframe(uint8_t id, dspi_transfer_t *spi_transfer)
 	rx_size = spi_transfer->rxData[2] / CAN_TRANSFER_BUF_LEN; /* size in frames, not bytes */
 	if (rx_size > available_data[id])
 		rx_size = available_data[id];
+
 	can_regs[id].rx_buf_bottom = can_regs[id].rx_buf_bottom + rx_size;
 	can_regs[id].rx_buf_bottom %= CAN_RX_BUF_SIZE;
 
 	can_regs[id].frames_in_buf -= rx_size;
-	can_calculate_available_data(id);
 
 	return rx_size * CAN_TRANSFER_BUF_LEN;
 }
@@ -378,9 +440,9 @@ int canx_registers(dspi_transfer_t *spi_transfer, int id)
 		case APALIS_TK1_K20_CAN_BAUD_REG:
 			return set_canbadreg(id, rx_buf[2]);
 		case APALIS_TK1_K20_CAN_BIT_1:
-			return set_canbittimig(id, rx_buf[2], 0x00FF);
+			return set_canbittiming(id, rx_buf[2], 0x00FF);
 		case APALIS_TK1_K20_CAN_BIT_2:
-			return set_canbittimig(id, (rx_buf[2] << 8), 0xFF00);
+			return set_canbittiming(id, (rx_buf[2] << 8), 0xFF00);
 		default:
 			return -EIO;
 		}
@@ -398,7 +460,7 @@ int canx_registers(dspi_transfer_t *spi_transfer, int id)
 		switch (rx_buf[1]) {
 		case APALIS_TK1_K20_CAN_OUT_BUF:
 			can_sendframe(id, &rx_buf[3], rx_buf[2]);
-			break;
+			return 0;
 		default:
 			return -EIO;
 		}
diff --git a/source/com_task.c b/source/com_task.c
index 56a4ce2..d6bc97d 100644
--- a/source/com_task.c
+++ b/source/com_task.c
@@ -7,7 +7,7 @@
 volatile uint8_t registers[APALIS_TK1_K20_LAST_REG];
 volatile uint8_t regRxHandled;
 
-/* Put FW version at known address in a binary. Make it 32-bit to have room for the future*/
+/* Put FW version at known address in a binary. Make it 32-bit to have room for the future */
 #ifndef TESTER_BUILD
 const uint32_t __attribute__((section(".FwVersion"), used)) fw_version = APALIS_TK1_K20_FW_VER;
 #else
@@ -18,17 +18,20 @@ static dspi_slave_handle_t spi_handle;
 static uint8_t slaveRxData[APALIS_TK1_K20_MAX_BULK + APALIS_TK1_K20_HEADER];
 static uint8_t slaveTxData[APALIS_TK1_K20_MAX_BULK + APALIS_TK1_K20_HEADER];
 
-#define SPI_DMA
-
 #ifdef SPI_DMA
 dspi_slave_edma_handle_t g_dspi_edma_s_handle;
 edma_handle_t dspiEdmaSlaveRxHandle;
 edma_handle_t dspiEdmaSlaveTxHandle;
 #endif
+
 void generate_irq(uint8_t irq) {
-	registers[APALIS_TK1_K20_IRQREG] |= BIT(irq);
-	/* Toggle INT1 pin */
-	GPIO_TogglePinsOutput(GPIOA, 1u << 16u);
+	if (!(registers[APALIS_TK1_K20_MSQREG] & BIT(irq))) {
+		taskENTER_CRITICAL();
+		registers[APALIS_TK1_K20_IRQREG] |= BIT(irq);
+		/* Toggle INT1 pin */
+		GPIO_TogglePinsOutput(GPIOA, 1u << 16u);
+		taskEXIT_CRITICAL();
+	}
 }
 
 void clear_irq_flag(uint8_t irq) {
@@ -110,7 +113,8 @@ inline int general_registers(dspi_transfer_t * spi_transfer)
 	return -ENXIO;
 }
 
-static void SPI_callback(SPI_Type *base, dspi_slave_handle_t *handle, status_t status, void *userData)
+
+inline void SPI_main_callback(status_t status, void *userData)
 {
 	callback_message_t * cb = (callback_message_t*) userData;
 	BaseType_t reschedule = pdFALSE;
@@ -119,14 +123,25 @@ static void SPI_callback(SPI_Type *base, dspi_slave_handle_t *handle, status_t s
 	{
 		xSemaphoreGiveFromISR(cb->sem, &reschedule);
 	}
-
+#if 0
 	if (status == kStatus_DSPI_Error)
 	{
 		__NOP();
 	}
+#endif
 	portYIELD_FROM_ISR(reschedule);
 }
 
+static void SPI_callback(SPI_Type *base, dspi_slave_handle_t *handle, status_t status, void *userData)
+{
+	SPI_main_callback(status, userData);
+}
+
+static void SPI_EDMA_callback(SPI_Type *base, dspi_slave_edma_handle_t *handle, status_t status, void *userData)
+{
+	SPI_main_callback(status, userData);
+}
+
 static dspi_slave_config_t spi2_slaveConfig;
 
 static void SPI_init() {
@@ -184,9 +199,10 @@ void spi_task(void *pvParameters) {
 	EDMA_CreateHandle(&dspiEdmaSlaveTxHandle, DMA0, slaveTxChannel);
 
 	g_dspi_edma_s_handle.userData = &spi_msg;
-	DSPI_SlaveTransferCreateHandleEDMA(SPI2, &g_dspi_edma_s_handle, SPI_callback, &spi_msg, &dspiEdmaSlaveRxHandle, &dspiEdmaSlaveTxHandle);
+	DSPI_SlaveTransferCreateHandleEDMA(SPI2, &g_dspi_edma_s_handle, SPI_EDMA_callback,
+			&spi_msg, &dspiEdmaSlaveRxHandle, &dspiEdmaSlaveTxHandle);
 #endif
-	memset(registers, 0x00, APALIS_TK1_K20_LAST_REG);
+	memset((uint8_t *)registers, 0x00, APALIS_TK1_K20_LAST_REG);
 	registers[APALIS_TK1_K20_REVREG] = APALIS_TK1_K20_FW_VER;
 	GPIO_SetPinsOutput(GPIOA, 1u << 29u); /* INT2 idle */
 	slaveXfer.configFlags = kDSPI_SlaveCtar0;
@@ -198,12 +214,12 @@ void spi_task(void *pvParameters) {
 		/* Wait for instructions from SoC */
 		ret = DSPI_SlaveTransferNonBlocking(SPI2, &spi_handle, &slaveXfer);
 		if ( ret == kStatus_Success) {
-
 			xSemaphoreTake(spi_msg.sem, portMAX_DELAY);
 
-			slaveXfer.txData = slaveTxData;
-			slaveXfer.rxData = slaveRxData;
 			if (slaveRxData[0] != APALIS_TK1_K20_READ_INST) {
+				taskENTER_CRITICAL();
+				slaveXfer.txData = slaveTxData;
+				slaveXfer.rxData = slaveRxData;
 				if (slaveRxData[1] <= 0x05) {
 					ret = general_registers(&slaveXfer);
 #ifndef TESTER_BUILD
@@ -231,34 +247,33 @@ void spi_task(void *pvParameters) {
 					ret = -EINVAL;
 				}
 
-
 				if (ret < 0) {
-					slaveTxData[0] = TK1_K20_INVAL;
-					slaveTxData[1] = ret;
-					slaveTxData[2] = slaveRxData[0];
-					slaveTxData[3] = slaveRxData[1];
-					slaveTxData[4] = slaveRxData[2];
-					slaveXfer.txData = slaveTxData;
-					ret = 5;
+					PRINTF("Invalid read/write ret = %d rx[0] = 0x%x, rx[1] = 0x%x, rx[2] = 0x%x\r\n",
+							ret, slaveRxData[0], slaveRxData[1], slaveRxData[2]);
 				}
 
-				if (slaveRxData[0] == APALIS_TK1_K20_BULK_READ_INST)
-				{
+				if (slaveRxData[0] == APALIS_TK1_K20_BULK_READ_INST) {
 					slaveXfer.dataSize = ret;
-					slaveXfer.rxData = NULL; /* We're not expecting any MOSI traffic, but NULL messes up stuff */
+					slaveXfer.rxData = NULL;
 #ifdef SPI_DMA
 					DSPI_SlaveTransferEDMA(SPI2, &g_dspi_edma_s_handle, &slaveXfer);
 #else
 					DSPI_SlaveTransferNonBlocking(SPI2, &spi_handle, &slaveXfer);
 #endif
+					taskEXIT_CRITICAL();
 					xSemaphoreTake(spi_msg.sem, portMAX_DELAY);
+
 					if (can_read >= 0) {
 						can_spi_read_complete(can_read);
 						can_read = -1;
 					}
+				} else {
+					taskEXIT_CRITICAL();
 				}
 			}
-
+		} else {
+			/* Something went wrong, retry */
+			DSPI_SlaveTransferAbort(SPI2, &spi_handle);
 		}
 	}
 }
diff --git a/source/com_task.h b/source/com_task.h
index 4a9557d..a541f88 100644
--- a/source/com_task.h
+++ b/source/com_task.h
@@ -22,6 +22,8 @@
 #include "semphr.h"
 #include "errno.h"
 
+#define SPI_DMA
+
 typedef struct _callback_message_t
 {
 	status_t async_status;