diff --git a/include/adc.h b/include/adc.h
index 9add128..0f047d8 100644
--- a/include/adc.h
+++ b/include/adc.h
@@ -45,6 +45,5 @@ enum AnalogPins {
  **************************************************************************************/
 
 void adc_init();
-void get_adc_value(enum AnalogPins name);
 
 #endif  //ADC_H
\ No newline at end of file
diff --git a/include/can.h b/include/can.h
index 6d3355f..9ff078b 100644
--- a/include/can.h
+++ b/include/can.h
@@ -76,12 +76,12 @@ typedef enum {
  **************************************************************************************/
 
 void          can_init();
-void          can_handle_data();
+int           can_handle_data();
 
 void          can_init_device(FDCAN_HandleTypeDef * handle, CANName peripheral, CanNominalBitTimingResult const can_bit_timing);
 int           can_frequency(FDCAN_HandleTypeDef * handle, uint32_t const can_bitrate);
 
-void          can_write(FDCAN_HandleTypeDef * handle, union x8h7_can_frame_message const * msg);
+int           can_write(FDCAN_HandleTypeDef * handle, union x8h7_can_frame_message const * msg);
 int           can_read(FDCAN_HandleTypeDef * handle, union x8h7_can_frame_message *msg);
 int           can_filter(FDCAN_HandleTypeDef * handle, uint32_t const filter_index, uint32_t const id, uint32_t const mask, bool const is_extended_id);
 unsigned char can_rderror(FDCAN_HandleTypeDef * handle);
diff --git a/include/gpio.h b/include/gpio.h
index fabb713..fff2e1e 100644
--- a/include/gpio.h
+++ b/include/gpio.h
@@ -33,6 +33,6 @@ void gpio_init();
 
 void gpio_set_initial_config();
 
-void gpio_handle_data();
+int  gpio_handle_data();
 
 #endif  //GPIO_H
\ No newline at end of file
diff --git a/include/rtc.h b/include/rtc.h
index 1f31579..a1e56c7 100644
--- a/include/rtc.h
+++ b/include/rtc.h
@@ -44,7 +44,5 @@ struct rtc_time {
  **************************************************************************************/
 
 void rtc_init();
-void rtc_set_date(uint8_t *data);
-void rtc_get_date(uint8_t *data);
 
 #endif //RTC_H
\ No newline at end of file
diff --git a/include/spi.h b/include/spi.h
index 5084901..7c863d1 100644
--- a/include/spi.h
+++ b/include/spi.h
@@ -33,6 +33,6 @@ void spi_init();
 
 void spi_end();
 
-void spi_transmit_receive(uint8_t peripheral, uint8_t *tx_buf, uint8_t *rx_buf, uint16_t size);
+void spi_transmit_receive(uint8_t * tx_buf, uint8_t * rx_buf, uint16_t size);
 
 #endif  //SPI_H
diff --git a/include/system.h b/include/system.h
index 3d32334..1c3fe6a 100644
--- a/include/system.h
+++ b/include/system.h
@@ -65,17 +65,16 @@ void system_init();
 
 void dma_init();
 
-void enqueue_packet(uint8_t peripheral, uint8_t opcode, uint16_t size, void* data);
+int enqueue_packet(uint8_t const peripheral, uint8_t const opcode, uint16_t const size, void * data);
+void set_nirq_low();
+void set_nirq_high();
+uint16_t get_tx_packet_size();
+bool is_dma_transfer_complete();
 
-void dispatchPacket(uint8_t peripheral, uint8_t opcode, uint16_t size, uint8_t* data);
-
-struct complete_packet* get_dma_packet();
-
-int get_dma_packet_size();
 
 void dma_handle_data();
 
-typedef void (*PeriphCallbackFunc) (uint8_t opcode, uint8_t *data, uint16_t size);
+typedef int (*PeriphCallbackFunc) (uint8_t opcode, uint8_t *data, uint16_t size);
 void register_peripheral_callback(uint8_t peripheral, PeriphCallbackFunc func);
 
 #endif //SYSTEM_H
\ No newline at end of file
diff --git a/include/uart.h b/include/uart.h
index adf6362..1692754 100644
--- a/include/uart.h
+++ b/include/uart.h
@@ -51,11 +51,11 @@ int uart_write_with_timeout(uint8_t *data, uint16_t size, uint32_t timeout);
 
 int uart_data_available();
 
-void uart_handle_data();
+int uart_handle_data();
 
 int virtual_uart_data_available();
 
-void virtual_uart_handle_data();
+int virtual_uart_handle_data();
 
 void UART2_enable_rx_irq();
 
diff --git a/src/adc.c b/src/adc.c
index 72fe965..a55461e 100644
--- a/src/adc.c
+++ b/src/adc.c
@@ -63,20 +63,30 @@ static void MX_ADC1_Init(void);
 static void MX_ADC2_Init(void);
 static void MX_ADC3_Init(void);
 
+static int get_adc_value(enum AnalogPins name);
+
 /**************************************************************************************
  * FUNCTION DEFINITION
  **************************************************************************************/
 
-void adc_handler(uint8_t opcode, uint8_t *data, uint16_t size) {
-  if (opcode == CONFIGURE) {
+int adc_handler(uint8_t opcode, uint8_t *data, uint16_t size)
+{
+  if (opcode == CONFIGURE)
+  {
     /* Note: ADC currently only supports polling mode.
      * uint16_t adc_sample_rate = *((uint16_t*)data);
      * dbg_printf("Setting ADC samplerate to %d milliseconds\n", adc_sample_rate);
      */
-  } else if ((opcode >= A0) && (opcode <= A7)) {
-    get_adc_value(opcode);
-  } else {
-    dbg_printf("Invalid ADC opcode %02x\n", opcode);
+    return 0;
+  }
+  else if ((opcode >= A0) && (opcode <= A7))
+  {
+    return get_adc_value(opcode);
+  }
+  else
+  {
+    dbg_printf("adc_handler: invalid ADC opcode %02x\n", opcode);
+    return 0;
   }
 }
 
@@ -89,7 +99,7 @@ void adc_init() {
   register_peripheral_callback(PERIPH_ADC, &adc_handler);
 }
 
-void get_adc_value(enum AnalogPins name) {
+int get_adc_value(enum AnalogPins name) {
   ADC_ChannelConfTypeDef conf = {0};
   ADC_HandleTypeDef* peripheral;
 
@@ -109,7 +119,7 @@ void get_adc_value(enum AnalogPins name) {
 
   dbg_printf("ADC%d: %d\n", name-1, value);
 
-  enqueue_packet(PERIPH_ADC, name, sizeof(value), &value);
+  return enqueue_packet(PERIPH_ADC, name, sizeof(value), &value);
 }
 
 static void MX_ADC1_Init(void) {
diff --git a/src/can.c b/src/can.c
index 03ec6c2..bc4a474 100644
--- a/src/can.c
+++ b/src/can.c
@@ -155,12 +155,13 @@ static void error(char* string) {
     while (1);
 }
 
-void fdcan1_handler(uint8_t opcode, uint8_t *data, uint16_t size) {
+int fdcan1_handler(uint8_t opcode, uint8_t *data, uint16_t size) {
     if (opcode == CONFIGURE)
     {
       uint32_t const can_bitrate = *((uint32_t *)data);
       can_frequency(&fdcan_1, can_bitrate);
       dbg_printf("fdcan1_handler: configuring fdcan1 with frequency %ld\n", can_bitrate);
+      return 0;
     }
     else if (opcode == CAN_FILTER)
     {
@@ -175,27 +176,31 @@ void fdcan1_handler(uint8_t opcode, uint8_t *data, uint16_t size) {
       {
         dbg_printf("fdcan1_handler: can_filter failed for idx: %ld, id: %lX, mask: %lX\n", x8h7_msg.field.idx, x8h7_msg.field.id, x8h7_msg.field.mask);
       }
+      return 0;
     }
     else if (opcode == CAN_TX_FRAME)
     {
       union x8h7_can_frame_message msg;
       memcpy(&msg, data, size);
 
-      dbg_printf("fdcan1_handler: sending CAN message to %x, size %d, content[0]=0x%02X\n", msg.id, msg.len, msg.data[0]);
-
-      can_write(&fdcan_1, &msg);
+      dbg_printf("fdcan1_handler: sending CAN message to %lx, size %d, content[0]=0x%02X\n", msg.field.id, msg.field.len, msg.field.data[0]);
+      return can_write(&fdcan_1, &msg);
     }
-    else {
+    else
+    {
       dbg_printf("fdcan1_handler: error invalid opcode (:%d)\n", opcode);
+      return 0;
     }
 }
 
-void fdcan2_handler(uint8_t opcode, uint8_t *data, uint16_t size) {
+int fdcan2_handler(uint8_t opcode, uint8_t *data, uint16_t size)
+{
     if (opcode == CONFIGURE)
     {
       uint32_t const can_bitrate = *((uint32_t *)data);
       can_frequency(&fdcan_2, can_bitrate);
       dbg_printf("fdcan2_handler: configuring fdcan2 with frequency %ld\n", can_bitrate);
+      return 0;
     }
     else if (opcode == CAN_FILTER)
     {
@@ -210,18 +215,20 @@ void fdcan2_handler(uint8_t opcode, uint8_t *data, uint16_t size) {
       {
         dbg_printf("fdcan2_handler: can_filter failed for idx: %ld, id: %lX, mask: %lX\n", x8h7_msg.field.idx, x8h7_msg.field.id, x8h7_msg.field.mask);
       }
+      return 0;
     }
     else if (opcode == CAN_TX_FRAME)
     {
       union x8h7_can_frame_message msg;
       memcpy(&msg, data, size);
 
-      dbg_printf("fdcan2_handler: sending CAN message to %x, size %d, content[0]=0x%02X\n", msg.id, msg.len, msg.data[0]);
-
-      can_write(&fdcan_2, &msg);
+      dbg_printf("fdcan2_handler: sending CAN message to %lx, size %d, content[0]=0x%02X\n", msg.field.id, msg.field.len, msg.field.data[0]);
+      return can_write(&fdcan_2, &msg);
     }
-    else {
+    else
+    {
       dbg_printf("fdcan2_handler: error invalid opcode (:%d)\n", opcode);
+      return 0;
     }
 }
 
@@ -251,25 +258,28 @@ void can_init()
   register_peripheral_callback(PERIPH_FDCAN2, &fdcan2_handler);
 }
 
-void can_handle_data()
+int can_handle_data()
 {
+  int bytes_enqueued = 0;
   union x8h7_can_frame_message msg;
 
-  if (can_read(&fdcan_1, &msg))
+  /* Note: the last read package is lost in this implementation. We need to fix this by
+   * implementing some peek method or by buffering messages in a ringbuffer.
+   */
+
+  for (int rc_enq = 0; can_read(&fdcan_1, &msg); bytes_enqueued += rc_enq)
   {
-    enqueue_packet(PERIPH_FDCAN1,
-                   CAN_RX_FRAME,
-                   X8H7_CAN_HEADER_SIZE + msg.field.len,
-                   msg.buf);
+    rc_enq = enqueue_packet(PERIPH_FDCAN1, CAN_RX_FRAME, X8H7_CAN_HEADER_SIZE + msg.field.len, msg.buf);
+    if (!rc_enq) return bytes_enqueued;
   }
 
-  if (can_read(&fdcan_2, &msg))
+  for (int rc_enq = 0; can_read(&fdcan_2, &msg); bytes_enqueued += rc_enq)
   {
-    enqueue_packet(PERIPH_FDCAN2,
-                   CAN_RX_FRAME,
-                   X8H7_CAN_HEADER_SIZE + msg.field.len,
-                   msg.buf);
+    rc_enq = enqueue_packet(PERIPH_FDCAN2, CAN_RX_FRAME, X8H7_CAN_HEADER_SIZE + msg.field.len, msg.buf);
+    if (!rc_enq) return bytes_enqueued;
   }
+
+  return bytes_enqueued;
 }
 
 /** Call all the init functions
@@ -405,7 +415,7 @@ int can_filter(FDCAN_HandleTypeDef * handle, uint32_t const filter_index, uint32
 }
 
 
-void can_write(FDCAN_HandleTypeDef * handle, union x8h7_can_frame_message const * msg)
+int can_write(FDCAN_HandleTypeDef * handle, union x8h7_can_frame_message const * msg)
 {
     FDCAN_TxHeaderTypeDef TxHeader = {0};
 
@@ -455,12 +465,12 @@ void can_write(FDCAN_HandleTypeDef * handle, union x8h7_can_frame_message const
       uint8_t msg[2] = {X8H7_CAN_STS_INT_ERR, 0};
       if (err_code == HAL_FDCAN_ERROR_FIFO_FULL) msg[1] = X8H7_CAN_STS_FLG_TX_OVR;
 
-      enqueue_packet(handle == &fdcan_1 ? PERIPH_FDCAN1 : PERIPH_FDCAN2, CAN_STATUS, sizeof(msg), msg);
+      return enqueue_packet(handle == &fdcan_1 ? PERIPH_FDCAN1 : PERIPH_FDCAN2, CAN_STATUS, sizeof(msg), msg);
     }
     else
     {
       uint8_t msg[2] = {X8H7_CAN_STS_INT_TX, 0};
-      enqueue_packet(handle == &fdcan_1 ? PERIPH_FDCAN1 : PERIPH_FDCAN2, CAN_STATUS, sizeof(msg), msg);
+      return enqueue_packet(handle == &fdcan_1 ? PERIPH_FDCAN1 : PERIPH_FDCAN2, CAN_STATUS, sizeof(msg), msg);
     }
 }
 
@@ -468,9 +478,8 @@ int can_read(FDCAN_HandleTypeDef * handle, union x8h7_can_frame_message *msg)
 {
   static const uint8_t DLCtoBytes[] = {0, 1, 2, 3, 4, 5, 6, 7, 8, 12, 16, 20, 24, 32, 48, 64};
 
-    if (HAL_FDCAN_GetRxFifoFillLevel(handle, FDCAN_RX_FIFO0) == 0) {
-        return 0; // No message arrived
-    }
+  if (HAL_FDCAN_GetRxFifoFillLevel(handle, FDCAN_RX_FIFO0) == 0)
+    return 0; // No message arrived
 
   FDCAN_RxHeaderTypeDef RxHeader = {0};
   uint8_t RxData[64] = {0};
diff --git a/src/crc32.c b/src/crc32.c
deleted file mode 100644
index 3b202b4..0000000
--- a/src/crc32.c
+++ /dev/null
@@ -1,67 +0,0 @@
-/*
- * Firmware for the Portenta X8 STM32H747AIIX/Cortex-M7 core.
- * Copyright (C) 2022 Arduino (http://www.arduino.cc/)
- *
- * This program is free software: you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation, either version 3 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- *  GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program.  If not, see <https://www.gnu.org/licenses/>.
- */
-
-/**************************************************************************************
- * INCLUDE
- **************************************************************************************/
-
-#include "crc32.h"
-
-#include "stm32h7xx_hal.h"
-
-/**************************************************************************************
- * GLOBAL VARIABLES
- **************************************************************************************/
-
-CRC_HandleTypeDef crc_hdl;
-
-/**************************************************************************************
- * FUNCTION DEFINITION
- **************************************************************************************/
-
-bool crc32_init()
-{
-  crc_hdl.Instance                     = CRC;
-
-  crc_hdl.Init.DefaultPolynomialUse    = DEFAULT_POLYNOMIAL_ENABLE;
-  crc_hdl.Init.DefaultInitValueUse     = DEFAULT_INIT_VALUE_ENABLE;
-  crc_hdl.Init.CRCLength               = CRC_POLYLENGTH_32B;
-  crc_hdl.Init.InputDataInversionMode  = CRC_INPUTDATA_INVERSION_NONE;
-  crc_hdl.Init.OutputDataInversionMode = CRC_OUTPUTDATA_INVERSION_DISABLE;
-
-  crc_hdl.InputDataFormat              = CRC_INPUTDATA_FORMAT_BYTES;
-
-  return (HAL_OK == HAL_CRC_Init(&crc_hdl));
-}
-
-uint32_t crc32_compute(uint8_t const * buf, uint32_t const buf_size)
-{
-  return HAL_CRC_Calculate(&crc_hdl, (uint32_t *)buf, buf_size);
-}
-
-void HAL_CRC_MspInit(CRC_HandleTypeDef * hcrc)
-{
-  if(hcrc->Instance == CRC)
-    __HAL_RCC_CRC_CLK_ENABLE();
-}
-
-void HAL_CRC_MspDeInit(CRC_HandleTypeDef * hcrc)
-{
-  if(hcrc->Instance == CRC)
-    __HAL_RCC_CRC_CLK_DISABLE();
-}
diff --git a/src/gpio.c b/src/gpio.c
index b25741d..9dc636e 100644
--- a/src/gpio.c
+++ b/src/gpio.c
@@ -202,7 +202,7 @@ static uint8_t GPIO_PIN_to_index(uint32_t pin) {
   return index;
 }
 
-void gpio_handler(uint8_t opcode, uint8_t *pdata, uint16_t size) {
+int gpio_handler(uint8_t opcode, uint8_t *pdata, uint16_t size) {
   uint16_t data = *((uint16_t*)pdata);
   enum Opcodes_GPIO action = opcode;
 
@@ -226,7 +226,7 @@ void gpio_handler(uint8_t opcode, uint8_t *pdata, uint16_t size) {
 
       if      (value == GPIO_MODE_IN_RE) GPIO_InitStruct.Mode = GPIO_MODE_IT_RISING;
       else if (value == GPIO_MODE_IN_FE) GPIO_InitStruct.Mode = GPIO_MODE_IT_FALLING;
-      else                               return;
+      else                               return 0;
 
       GPIO_InitStruct.Pull = GPIO_PULLUP;
       GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH;
@@ -250,8 +250,8 @@ void gpio_handler(uint8_t opcode, uint8_t *pdata, uint16_t size) {
     case READ:
       response[0] = index;
       response[1] = HAL_GPIO_ReadPin(GPIO_pinmap[index].port, GPIO_pinmap[index].pin);
-      enqueue_packet(PERIPH_GPIO, opcode, sizeof(response), &response);
       dbg_printf("GPIO%d: READ %d\n", index, response[1]);
+      return enqueue_packet(PERIPH_GPIO, opcode, sizeof(response), &response);
       break;
     case IRQ_SIGNAL:
       // do nothing;
@@ -266,6 +266,7 @@ void gpio_handler(uint8_t opcode, uint8_t *pdata, uint16_t size) {
       gpio_enable_irq(GPIO_pinmap[index].pin);
       break;
   }
+  return 0;
 }
 
 void gpio_init() {
@@ -301,8 +302,10 @@ void gpio_set_initial_config() {
   HAL_NVIC_EnableIRQ(EXTI15_10_IRQn);
 }
 
-void gpio_handle_data()
+int gpio_handle_data()
 {
+  int bytes_enqueued = 0;
+
   /* Take a threadsafe copy of the interrupt flags. */
   __disable_irq();
   uint16_t const copy_int_event_flags = int_event_flags;
@@ -315,10 +318,12 @@ void gpio_handle_data()
     if (copy_int_event_flags & (1 << index))
     {
       uint8_t irq_pin = IRQ_pinmap[index].pin;
-      enqueue_packet(PERIPH_GPIO, IRQ_SIGNAL, sizeof(irq_pin), &irq_pin);
+      bytes_enqueued += enqueue_packet(PERIPH_GPIO, IRQ_SIGNAL, sizeof(irq_pin), &irq_pin);
       __disable_irq();
       int_event_flags &= ~(1 << index); /*Clear this flag */
       __enable_irq();
     }
   }
+
+  return bytes_enqueued;
 }
diff --git a/src/main.c b/src/main.c
index 44d7073..2617bd0 100644
--- a/src/main.c
+++ b/src/main.c
@@ -39,12 +39,6 @@
 #include "watchdog.h"
 #include "m4_utilities.h"
 
-/**************************************************************************************
- * GLOBAL VARIABLE
- **************************************************************************************/
-
-volatile bool trigger_irq = false;
-
 /**************************************************************************************
  * FUNCTION DEFINITION
  **************************************************************************************/
@@ -70,34 +64,24 @@ void peripheral_init() {
   can_init();
 }
 
-void handle_data() {
-
+void handle_data()
+{
   __WFI();
 
   watchdog_refresh();
 
-  if (uart_data_available()) {
+  if (uart_data_available())
     uart_handle_data();
-  }
 
-  if (virtual_uart_data_available()) {
+  if (virtual_uart_data_available())
     virtual_uart_handle_data();
-  }
 
   can_handle_data();
-
   gpio_handle_data();
-
   dma_handle_data();
 
-/*
-  if (trigger_irq) {
-    HAL_GPIO_WritePin(GPIOA, GPIO_PIN_9, 0);
-    HAL_GPIO_WritePin(GPIOA, GPIO_PIN_9, 1);
-    trigger_irq = false;
-  }
-*/
-
+  if (is_dma_transfer_complete() && (get_tx_packet_size() > 0))
+    set_nirq_low();
 }
 
 /**************************************************************************************
diff --git a/src/pwm.c b/src/pwm.c
index 4e12d95..67b1113 100644
--- a/src/pwm.c
+++ b/src/pwm.c
@@ -132,7 +132,7 @@ static bool isValidPwmChannelNumber(unsigned int const channel_number);
  * FUNCTION DEFINITION
  **************************************************************************************/
 
-void pwm_handler(uint8_t opcode, uint8_t *data, uint16_t size) {
+int pwm_handler(uint8_t opcode, uint8_t *data, uint16_t size) {
   if (opcode & CAPTURE) {
     uint8_t const channel = opcode & 0x0F;
     if (isValidPwmChannelNumber(channel))
@@ -147,6 +147,7 @@ void pwm_handler(uint8_t opcode, uint8_t *data, uint16_t size) {
     else
       dbg_printf("Invalid PWM channel number provided for mode PWM: %d\n", channel);
   }
+  return 0;
 }
 
 void pwm_init() {
diff --git a/src/rtc.c b/src/rtc.c
index 0f9fd8f..0b8c36c 100644
--- a/src/rtc.c
+++ b/src/rtc.c
@@ -36,6 +36,9 @@ RTC_HandleTypeDef hrtc;
  * FUNCTION DEFINITION
  **************************************************************************************/
 
+static int rtc_set_date(uint8_t *data);
+static int rtc_get_date(uint8_t *data);
+
 static void MX_RTC_Init(void)
 {
   hrtc.Instance = RTC;
@@ -76,12 +79,16 @@ void HAL_RTC_MspDeInit(RTC_HandleTypeDef *hrtc)
   }
 }
 
-void rtc_handler(uint8_t opcode, uint8_t *data, uint16_t size)
+int rtc_handler(uint8_t opcode, uint8_t *data, uint16_t size)
 {
   if (opcode == SET_DATE)
-    rtc_set_date(data);
-  if (opcode == GET_DATE)
-    rtc_get_date(data);
+    return rtc_set_date(data);
+  else if (opcode == GET_DATE)
+    return rtc_get_date(data);
+  else
+    dbg_printf("rtc_handler: error invalid opcode (:%d)\n", opcode);
+
+  return 0;
 }
 
 void rtc_init()
@@ -90,7 +97,7 @@ void rtc_init()
   register_peripheral_callback(PERIPH_RTC, &rtc_handler);
 }
 
-void rtc_set_date(uint8_t *data)
+int rtc_set_date(uint8_t *data)
 {
   struct rtc_time *tm = (struct rtc_time*)data;
   RTC_TimeTypeDef sTime = {0};
@@ -110,9 +117,11 @@ void rtc_set_date(uint8_t *data)
   sDate.Year = tm->tm_year;
   if (HAL_RTC_SetDate(&hrtc, &sDate, RTC_FORMAT_BIN) != HAL_OK)
     Error_Handler();
+
+  return 0; /* no bytes enqueued */
 }
 
-void rtc_get_date(uint8_t *data)
+int rtc_get_date(uint8_t *data)
 {
   RTC_TimeTypeDef sTime = {0};
   RTC_DateTypeDef sDate = {0};
@@ -133,5 +142,5 @@ void rtc_get_date(uint8_t *data)
   now.tm_mday = sDate.Date;
   now.tm_year = sDate.Year;
 
-  enqueue_packet(PERIPH_RTC, GET_DATE, sizeof(now), &now);
+  return enqueue_packet(PERIPH_RTC, GET_DATE, sizeof(now), &now);
 }
diff --git a/src/spi.c b/src/spi.c
index eb7a1a6..bca4662 100644
--- a/src/spi.c
+++ b/src/spi.c
@@ -37,7 +37,8 @@ DMA_HandleTypeDef hdma_spi3_rx;
  * FUNCTION DEFINIION
  **************************************************************************************/
 
-static void MX_SPI3_Init(void) {
+static void MX_SPI3_Init(void)
+{
 
   hspi3.Instance = SPI3;
   hspi3.Init.Mode = SPI_MODE_SLAVE;
@@ -73,7 +74,8 @@ static void MX_SPI3_Init(void) {
  * @param hspi: SPI handle pointer
  * @retval None
  */
-void HAL_SPI_MspInit(SPI_HandleTypeDef *hspi) {
+void HAL_SPI_MspInit(SPI_HandleTypeDef *hspi)
+{
   GPIO_InitTypeDef GPIO_InitStruct = {0};
   RCC_PeriphCLKInitTypeDef PeriphClkInitStruct = {0};
 
@@ -155,7 +157,8 @@ void HAL_SPI_MspInit(SPI_HandleTypeDef *hspi) {
  * @param hspi: SPI handle pointer
  * @retval None
  */
-void HAL_SPI_MspDeInit(SPI_HandleTypeDef *hspi) {
+void HAL_SPI_MspDeInit(SPI_HandleTypeDef *hspi)
+{
   __HAL_RCC_SPI3_CLK_DISABLE();
 
   HAL_GPIO_DeInit(GPIOA, GPIO_PIN_15);
@@ -168,18 +171,17 @@ void HAL_SPI_MspDeInit(SPI_HandleTypeDef *hspi) {
   HAL_NVIC_DisableIRQ(SPI3_IRQn);
 }
 
-void spi_init() {
-
+void spi_init()
+{
   MX_SPI3_Init();
-
 }
 
-void spi_end() {
-
+void spi_end()
+{
   HAL_SPI_Abort(&hspi3);
 }
 
-
-inline void spi_transmit_receive(uint8_t peripheral, uint8_t *tx_buf, uint8_t *rx_buf, uint16_t size) {
+void spi_transmit_receive(uint8_t *tx_buf, uint8_t *rx_buf, uint16_t size)
+{
   HAL_SPI_TransmitReceive_DMA(&hspi3, tx_buf, rx_buf, size);
-}
\ No newline at end of file
+}
diff --git a/src/system.c b/src/system.c
index 3c33a75..9350e2c 100644
--- a/src/system.c
+++ b/src/system.c
@@ -35,34 +35,32 @@
 #include "spi.h"
 
 /**************************************************************************************
- * TYPEDEF
+ * DEFINE
  **************************************************************************************/
 
-/*
-DMA_HandleTypeDef hdma_spi3_tx;
-DMA_HandleTypeDef hdma_spi3_rx;
-
-DMA_HandleTypeDef hdma_spi2_tx;
-DMA_HandleTypeDef hdma_spi2_rx;
-*/
+#define NUM_PERIPHERAL_CALLBACKS (20)
 
-PeriphCallbackFunc PeriphCallbacks[20];
+/**************************************************************************************
+ * TYPEDEF
+ **************************************************************************************/
 
-enum { TRANSFER_WAIT, TRANSFER_COMPLETE, TRANSFER_ERROR };
+PeriphCallbackFunc PeriphCallbacks[NUM_PERIPHERAL_CALLBACKS];
 
 /**************************************************************************************
  * GLOBAL VARIABLES
  **************************************************************************************/
 
-__IO uint32_t transferState = TRANSFER_WAIT;
-
 __attribute__((section("dma"), aligned(2048))) volatile uint8_t TX_Buffer[SPI_DMA_BUFFER_SIZE];
 __attribute__((section("dma"), aligned(2048))) volatile uint8_t RX_Buffer[SPI_DMA_BUFFER_SIZE];
-
 __attribute__((section("dma"), aligned(2048))) volatile uint8_t RX_Buffer_userspace[SPI_DMA_BUFFER_SIZE];
 
-volatile bool get_data_amount = true;
-volatile uint16_t data_amount = 0;
+typedef enum
+{
+  Idle, Header, Data, Complete, Error
+} eTransferState;
+volatile eTransferState transaction_state = Idle;
+
+volatile bool is_rx_buf_userspace_processed = false;
 
 /**************************************************************************************
  * FUNCTION DEFINITION
@@ -186,8 +184,8 @@ static void MPU_Config(void)
   HAL_MPU_Enable(MPU_PRIVILEGED_DEFAULT);
 }
 
-static void MX_DMA_Init(void) {
-
+static void MX_DMA_Init(void)
+{
   /* DMA controller clock enable */
   __HAL_RCC_DMA1_CLK_ENABLE();
 
@@ -204,25 +202,17 @@ static void MX_DMA_Init(void) {
   HAL_NVIC_EnableIRQ(DMA1_Stream3_IRQn);
 }
 
-void clean_dma_buffer() {
+void clean_dma_buffer()
+{
   memset((uint8_t*)TX_Buffer, 0, sizeof(TX_Buffer));
   memset((uint8_t*)RX_Buffer, 0, sizeof(RX_Buffer));
+  memset((uint8_t*)RX_Buffer_userspace, 0, sizeof(RX_Buffer_userspace));
 }
 
-void enqueue_packet(uint8_t peripheral, uint8_t opcode, uint16_t size, void* data) {
-
-/*
-  int timeout = 100000;
-  // don't feed data in the middle of a transmission
-  while (get_data_amount == false && timeout > 0) {
-    // wait for the DMA interrupt to be over
-    timeout--;
-  }
-*/
-
-  while (get_data_amount == false) {
-    // wait for the DMA interrupt to be over
-  }
+int enqueue_packet(uint8_t const peripheral, uint8_t const opcode, uint16_t const size, void * data)
+{
+  /* Wait for transfer to be complete. */
+  while (!is_dma_transfer_complete()) { }
 
   /* Enter critical section: Since this function is called both from inside
    * interrupt context (handle_irq/gpio.c) as well as from normal execution
@@ -230,7 +220,7 @@ void enqueue_packet(uint8_t peripheral, uint8_t opcode, uint16_t size, void* dat
    * to store the current interrupt situation and restore it at the end of
    * the critical section.
    */
-  uint32_t const primask_bit = __get_PRIMASK();
+  volatile uint32_t primask_bit = __get_PRIMASK();
   /*
    * PRIMASK - Typically configured in code using the CMSIS __disable_irq() and
    * __enable_irq() routines or the cpsid i and cpsie i assembly instructions
@@ -241,49 +231,79 @@ void enqueue_packet(uint8_t peripheral, uint8_t opcode, uint16_t size, void* dat
    */
   __set_PRIMASK(1) ;
 
+  int bytes_enqueued = 0;
+
   /* complete_packet:
    * - uint16_t size;      |
    * - uint16_t checksum;  | sizeof(complete_packet.header) = 4 Bytes
    */
-  struct complete_packet *tx_pkt = get_dma_packet();
-  if ((tx_pkt->header.size + size) > get_dma_packet_size()) {
+  struct complete_packet * pkt = (struct complete_packet *)TX_Buffer;
+  if ((pkt->header.size + size) > sizeof(TX_Buffer))
     goto cleanup;
-  }
+
   /* subpacket:
    * - uint8_t peripheral; |
    * - uint8_t opcode;     |
    * - uint16_t size;      | sizeof(subpacket.header) = 4 Bytes
    * - uint8_t raw_data;
    */
-  struct subpacket pkt;
-  pkt.header.peripheral = peripheral;
-  pkt.header.opcode = opcode;
-  pkt.header.size = size;
+  struct subpacket subpkt;
+  subpkt.header.peripheral = peripheral;
+  subpkt.header.opcode = opcode;
+  subpkt.header.size = size;
   /* Copy subpacket.header at the end of the current complete_packet superframe. */
-  memcpy((uint8_t*)&(tx_pkt->data) + tx_pkt->header.size, &pkt, sizeof(pkt.header));
-  tx_pkt->header.size += sizeof(pkt.header);
+  memcpy((uint8_t*)&(pkt->data) + pkt->header.size, &subpkt, sizeof(subpkt.header));
+  pkt->header.size += sizeof(subpkt.header);
   /* Copy subpacket.raw_data at after subpacket.header. */
-  memcpy((uint8_t*)&(tx_pkt->data) + tx_pkt->header.size, data, size);
-  tx_pkt->header.size += size;
+  memcpy((uint8_t*)&(pkt->data) + pkt->header.size, data, size);
+  pkt->header.size += size;
   /* Calculate a simple checksum to ensure bit flips in the length field can be recognized. */
-  tx_pkt->header.checksum = tx_pkt->header.size ^ 0x5555;
+  pkt->header.checksum = pkt->header.size ^ 0x5555;
+  /* Update internal status variable of how many bytes have been enqueued. */
+  bytes_enqueued += sizeof(subpkt.header) + size;
 
+#ifdef DEBUG
   dbg_printf("Enqueued packet for peripheral: %s Opcode: %X Size: %X\n  data: ",
       to_peripheral_string(peripheral), opcode, size);
 
-  for (int i = 0; i < size; i++) {
+  for (int i = 0; i < size; i++)
     dbg_printf("0x%02X ", *(((uint8_t*)data) + i));
-  }
+
   dbg_printf("\n");
+#endif
 
 cleanup:
   /* Exit critical section: restore previous priority mask */
   __set_PRIMASK(primask_bit);
 
+  return bytes_enqueued;
+}
+
+void set_nirq_low()
+{
+  /* Trigger transfer. */
   HAL_GPIO_WritePin(GPIOC, GPIO_PIN_1, 0);
+}
+
+void set_nirq_high()
+{
+  /* Signal completion of transfer. */
   HAL_GPIO_WritePin(GPIOC, GPIO_PIN_1, 1);
+}
+
+uint16_t get_tx_packet_size()
+{
+  /* Enter critical section. */
+  uint32_t primask_bit = __get_PRIMASK();
+  __set_PRIMASK(1) ;
+
+  struct complete_packet * tx_pkt = (struct complete_packet *)TX_Buffer;
+  uint16_t const tx_packet_size = tx_pkt->header.size;
+
+  /* Exit critical section: restore previous priority mask */
+  __set_PRIMASK(primask_bit);
 
-  //trigger_irq = true;
+  return tx_packet_size;
 }
 
 #ifndef REALVERSION
@@ -292,19 +312,20 @@ void enqueue_packet(uint8_t peripheral, uint8_t opcode, uint16_t size, void* dat
 
 char const __attribute__((section (".fw_version_section"))) REAL_VERSION_FLASH[] = REALVERSION;
 
-void writeVersion() {
-  const char* version = REAL_VERSION_FLASH;
-  enqueue_packet(PERIPH_H7, FW_VERSION, strlen(version), (void*)version);
-}
-
 extern int m4_booted_correctly;
 
-void h7_handler(uint8_t opcode, uint8_t *data, uint16_t size) {
+int h7_handler(uint8_t opcode, uint8_t *data, uint16_t size)
+{
   if (opcode == FW_VERSION) {
-    writeVersion();
+    const char* version = REAL_VERSION_FLASH;
+    return enqueue_packet(PERIPH_H7, FW_VERSION, strlen(version), (void*)version);
   }
-  if (opcode == BOOT_M4) {
-    enqueue_packet(PERIPH_H7, BOOT_M4, sizeof(m4_booted_correctly), &m4_booted_correctly);
+  else if (opcode == BOOT_M4) {
+    return enqueue_packet(PERIPH_H7, BOOT_M4, sizeof(m4_booted_correctly), &m4_booted_correctly);
+  }
+  else {
+    dbg_printf("h7_handler: error invalid opcode (:%d)\n", opcode);
+    return 0;
   }
 }
 
@@ -330,25 +351,23 @@ void dma_init() {
   clean_dma_buffer();
 }
 
-void dispatchPacket(uint8_t peripheral, uint8_t opcode, uint16_t size, uint8_t* data) {
-  //Get function callback from LUT (peripherals vs opcodes)
-  (*PeriphCallbacks[peripheral])(opcode, data, size);
-}
-
-struct complete_packet* get_dma_packet() {
-  return (struct complete_packet *)TX_Buffer;
-}
-
-int get_dma_packet_size() {
-  return sizeof(TX_Buffer);
-}
-
 void EXTI15_10_IRQHandler(void)
 {
-  if (get_data_amount) {
-    spi_transmit_receive(PERIPH_SPI3, (uint8_t *)TX_Buffer,
-                         (uint8_t *)RX_Buffer, sizeof(uint16_t) * 2);
+  /* Step #1:
+   * This function is called when IMX8 is pulling CS -> LOW.
+   */
+  if (transaction_state == Idle || transaction_state == Complete)
+  {
+    struct complete_packet * tx_pkt = (struct complete_packet *)TX_Buffer;
+    struct complete_packet * rx_pkt = (struct complete_packet *)RX_Buffer;
+
+    spi_transmit_receive((uint8_t *)&(tx_pkt->header),
+                         (uint8_t *)&(rx_pkt->header),
+                         sizeof(tx_pkt->header));
+
+    transaction_state = Header;
   }
+
   HAL_GPIO_EXTI_IRQHandler(GPIO_PIN_15);
 }
 
@@ -357,89 +376,118 @@ void HAL_SPI_TxRxCpltCallback(SPI_HandleTypeDef *hspi) {
   struct complete_packet *rx_pkt = (struct complete_packet *)RX_Buffer;
   struct complete_packet *tx_pkt = (struct complete_packet *)TX_Buffer;
 
-  if (get_data_amount) {
-
-/*
-    if (rx_pkt->checksum != rx_pkt->size ^ 0x5555) {
-      return;
-    }
-*/
-
+  if (transaction_state == Header)
+  {
+    /* Step #2:
+     * Task the system with the transport of the actual data.
+     */
 #pragma GCC diagnostic push
 #pragma GCC diagnostic ignored "-Wpedantic"
-    data_amount = max(tx_pkt->header.size, rx_pkt->header.size);
+    uint16_t const bytes_to_transfer = max(tx_pkt->header.size, rx_pkt->header.size);
 #pragma GCC diagnostic pop
 
-    if (data_amount == 0) {
+    /* Nothing to transfer. */
+    if (bytes_to_transfer == 0)
+    {
+      /* Cleanup. */
+      tx_pkt->header.size = 0;
+      tx_pkt->header.checksum = 0;
+      /* Transition to Idle state. */
+      transaction_state = Idle;
       return;
     }
 
     // reconfigure the DMA to actually receive the data
-    spi_transmit_receive(PERIPH_SPI3, (uint8_t*)&(tx_pkt->data), (uint8_t*)&(rx_pkt->data), data_amount);
-    get_data_amount = false;
-
-  } else {
-    // real end of operation, pause DMA, memcpy stuff around and re-enable DMA
-    // HAL_SPI_DMAPause(&hspi1);
-
-    transferState = TRANSFER_COMPLETE;
-
+    spi_transmit_receive((uint8_t*)&(tx_pkt->data), (uint8_t*)&(rx_pkt->data), bytes_to_transfer);
+    transaction_state = Data;
+  }
+  else if (transaction_state == Data)
+  {
+    /* Step #3:
+     * The SPI transfer is now complete, copy to userspace memory.
+     */
     memcpy((void *)RX_Buffer_userspace, &(rx_pkt->data), rx_pkt->header.size);
 
-    // mark the next packet as invalid
+    /* Mark the next packet as invalid. */
     *((uint32_t*)((uint8_t *)RX_Buffer_userspace + rx_pkt->header.size)) = 0xFFFFFFFF; // INVALID;
 
-    // clean the transfer buffer size to restart
+    /* Clean the transfer buffer size to restart. */
     tx_pkt->header.size = 0;
+    tx_pkt->header.checksum = 0;
 
-    get_data_amount = true;
+    transaction_state = Complete;
+    is_rx_buf_userspace_processed = false;
 
-    // HAL_SPI_DMAResume(&hspi1);
+    set_nirq_high();
   }
 }
 
-void HAL_SPI_ErrorCallback(SPI_HandleTypeDef *hspi) {
-  transferState = TRANSFER_ERROR;
+void HAL_SPI_ErrorCallback(SPI_HandleTypeDef *hspi)
+{
+  transaction_state = Error;
   spi_end();
-
-/*
-  // Restart DMA
-  HAL_SPI_TransmitReceive_DMA(&hspi3, (uint8_t *)TX_Buffer,
-                                  (uint8_t *)RX_Buffer, sizeof(uint16_t) * 2);
-*/
 }
 
-void dma_handle_data() {
-  if (transferState == TRANSFER_COMPLETE) {
+void dma_handle_data()
+{
+  if (transaction_state == Error)
+  {
+    dbg_printf("dma_handle_data: got transfer error, recovering\n");
+    HAL_GPIO_WritePin(GPIOA, GPIO_PIN_9, 0);
+    HAL_GPIO_WritePin(GPIOA, GPIO_PIN_9, 1);
+    transaction_state = Idle;
+    return;
+  }
+
+  /* Enter critical section. */
+  volatile uint32_t primask_bit = __get_PRIMASK();
+  __set_PRIMASK(1) ;
 
-    struct subpacket *rx_pkt_userspace =
-        (struct subpacket *)RX_Buffer_userspace;
+  if (transaction_state == Complete && !is_rx_buf_userspace_processed)
+  {
+    struct subpacket *rx_pkt_userspace = (struct subpacket *)RX_Buffer_userspace;
 
     while (rx_pkt_userspace->header.peripheral != 0xFF &&
-            rx_pkt_userspace->header.peripheral != 0x00) {
+           rx_pkt_userspace->header.peripheral != 0x00)
+    {
+#ifdef DEBUG
       dbg_printf("Peripheral: %s Opcode: %X Size: %X\n  data: ",
-          to_peripheral_string(rx_pkt_userspace->header.peripheral), rx_pkt_userspace->header.opcode,
-              rx_pkt_userspace->header.size);
-      for (int i = 0; i < rx_pkt_userspace->header.size; i++) {
+                 to_peripheral_string(rx_pkt_userspace->header.peripheral),
+                 rx_pkt_userspace->header.opcode,
+                 rx_pkt_userspace->header.size);
+
+      for (int i = 0; i < rx_pkt_userspace->header.size; i++)
         dbg_printf("0x%02X ", *((&rx_pkt_userspace->raw_data) + i));
-      }
+
       dbg_printf("\n");
+#endif
+
+      if (rx_pkt_userspace->header.peripheral >= NUM_PERIPHERAL_CALLBACKS) {
+        printf("error, invalid peripheral id received: %d\n", rx_pkt_userspace->header.peripheral);
+        break; /* Leave this loop. */
+      }
 
-      // do something useful with this packet
-      dispatchPacket(rx_pkt_userspace->header.peripheral, rx_pkt_userspace->header.opcode,
-          rx_pkt_userspace->header.size, &(rx_pkt_userspace->raw_data));
+      /* Obtain the registered callback for the selected peripheral. */
+      PeriphCallbackFunc const peripheral_callback = PeriphCallbacks[rx_pkt_userspace->header.peripheral];
+
+      /* Invoke the registered callback for the selected peripheral. */
+      int const rc = peripheral_callback(rx_pkt_userspace->header.opcode,
+                                         (uint8_t *)(&(rx_pkt_userspace->raw_data)),
+                                         rx_pkt_userspace->header.size);
+      if (rc < 0) {
+        dbg_printf("dma_handle_data: %s callback error: %d",
+                   to_peripheral_string(rx_pkt_userspace->header.peripheral), rc);
+      }
 
+      /* Advance to the next package. */
       rx_pkt_userspace = (struct subpacket *)((uint8_t *)rx_pkt_userspace + 4 + rx_pkt_userspace->header.size);
     }
-    transferState = TRANSFER_WAIT;
-  }
 
-  if (transferState == TRANSFER_ERROR) {
-      dbg_printf("got transfer error, recovering\n");
-      HAL_GPIO_WritePin(GPIOA, GPIO_PIN_9, 0);
-      HAL_GPIO_WritePin(GPIOA, GPIO_PIN_9, 1);
-      transferState = TRANSFER_WAIT;
+    is_rx_buf_userspace_processed = true;
   }
+
+  /* Leave critical section. */
+  __set_PRIMASK(primask_bit);
 }
 
 void register_peripheral_callback(uint8_t peripheral, PeriphCallbackFunc func)
@@ -453,3 +501,18 @@ void Error_Handler_Name(const char* name) {
   while (1) {
   }
 }
+
+bool is_dma_transfer_complete()
+{
+  bool is_dma_transfer_complete_flag_temp = false;
+  /* Enter critical section. */
+  volatile uint32_t primask_bit = __get_PRIMASK();
+  __set_PRIMASK(1) ;
+
+  is_dma_transfer_complete_flag_temp = (transaction_state == Idle) || (transaction_state == Complete);
+
+  /* Leave critical section. */
+  __set_PRIMASK(primask_bit);
+
+  return is_dma_transfer_complete_flag_temp;
+}
diff --git a/src/uart.c b/src/uart.c
index cc8f81c..ec7371f 100644
--- a/src/uart.c
+++ b/src/uart.c
@@ -214,17 +214,19 @@ void HAL_UART_MspDeInit(UART_HandleTypeDef *huart) {
   }
 }
 
-void uart_handler(uint8_t opcode, uint8_t *data, uint16_t size) {
+int uart_handler(uint8_t opcode, uint8_t *data, uint16_t size) {
   if (opcode == CONFIGURE) {
     uart_configure(data);
   }
   if (opcode == DATA) {
     uart_write(data, size);
   }
+  return 0;
 }
 
-void virtual_uart_handler(uint8_t opcode, uint8_t *data, uint16_t size) {
+int virtual_uart_handler(uint8_t opcode, uint8_t *data, uint16_t size) {
   serial_rpc_write(data, size);
+  return 0;
 }
 
 
@@ -247,24 +249,24 @@ int uart_data_available() {
   return !ring_buffer_is_empty(&uart_ring_buffer);
 }
 
-void uart_handle_data() {
+int uart_handle_data() {
   uint8_t temp_buf[RING_BUFFER_SIZE];
   __disable_irq();
-  int cnt = ring_buffer_dequeue_arr(&uart_ring_buffer, (char *)temp_buf, ring_buffer_num_items(&uart_ring_buffer));
+  int const cnt = ring_buffer_dequeue_arr(&uart_ring_buffer, (char *)temp_buf, ring_buffer_num_items(&uart_ring_buffer));
   __enable_irq();
-  enqueue_packet(PERIPH_UART, DATA, cnt, temp_buf);
+  return enqueue_packet(PERIPH_UART, DATA, cnt, temp_buf);
 }
 
 int virtual_uart_data_available() {
   return !ring_buffer_is_empty(&virtual_uart_ring_buffer);
 }
 
-void virtual_uart_handle_data() {
+int virtual_uart_handle_data() {
   uint8_t temp_buf[RING_BUFFER_SIZE];
   __disable_irq();
-  int cnt = ring_buffer_dequeue_arr(&virtual_uart_ring_buffer, (char *)temp_buf, ring_buffer_num_items(&virtual_uart_ring_buffer));
+  int const cnt = ring_buffer_dequeue_arr(&virtual_uart_ring_buffer, (char *)temp_buf, ring_buffer_num_items(&virtual_uart_ring_buffer));
   __enable_irq();
-  enqueue_packet(PERIPH_VIRTUAL_UART, DATA, cnt, temp_buf);
+  return enqueue_packet(PERIPH_VIRTUAL_UART, DATA, cnt, temp_buf);
 }
 
 void UART2_enable_rx_irq() {