feat(core:): introduce BLE driver

core/SConscript.firmware

@@ -29,7 +29,7 @@ FEATURE_FLAGS = {
     "AES_GCM": BENCHMARK or THP,
 }
 
-FEATURES_WANTED = ["input", "sd_card", "rgb_led", "dma2d", "consumption_mask", "usb" ,"optiga", "haptic"]
+FEATURES_WANTED = ["input", "sd_card", "rgb_led", "dma2d", "consumption_mask", "usb" ,"optiga", "haptic", "ble"]
 if DISABLE_OPTIGA and PYOPT == '0':
     FEATURES_WANTED.remove("optiga")
 

core/SConscript.kernel

@@ -23,7 +23,7 @@ FEATURE_FLAGS = {
     "AES_GCM": False,
 }
 
-FEATURES_WANTED = ["input", "sd_card", "rgb_led", "dma2d", "consumption_mask", "usb" ,"optiga", "haptic"]
+FEATURES_WANTED = ["input", "sd_card", "rgb_led", "dma2d", "consumption_mask", "usb" ,"optiga", "haptic", "ble"]
 if DISABLE_OPTIGA and PYOPT == '0':
     FEATURES_WANTED.remove("optiga")
 

core/embed/io/ble/inc/io/ble.h

@@ -0,0 +1,125 @@
+/*
+ * This file is part of the Trezor project, https://trezor.io/
+ *
+ * Copyright (c) SatoshiLabs
+ *
+ * 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 <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef TREZORHAL_BLE_H
+#define TREZORHAL_BLE_H
+
+// This module provides interface to BLE (Bluetooth Low Energy) functionality.
+// It allows the device to advertise itself, connect to other devices, and
+// exchange data over BLE.
+
+#include <trezor_types.h>
+
+#define BLE_RX_PACKET_SIZE 244
+#define BLE_TX_PACKET_SIZE 64
+
+typedef enum {
+  BLE_SWITCH_OFF = 0,      // Turn off BLE advertising, disconnect
+  BLE_SWITCH_ON = 1,       // Turn on BLE advertising
+  BLE_PAIRING_MODE = 2,    // Enter pairing mode
+  BLE_DISCONNECT = 3,      // Disconnect from the connected device
+  BLE_ERASE_BONDS = 4,     // Erase all bonding information
+  BLE_ALLOW_PAIRING = 5,   // Accept pairing request
+  BLE_REJECT_PAIRING = 6,  // Reject pairing request
+} ble_command_t;
+
+typedef enum {
+  BLE_NONE = 0,               // No event
+  BLE_CONNECTED = 1,          // Connected to a device
+  BLE_DISCONNECTED = 2,       // Disconnected from a device
+  BLE_PAIRING_REQUEST = 3,    // Pairing request received
+  BLE_PAIRING_CANCELLED = 4,  // Pairing was cancelled by host
+} ble_event_type_t;
+
+typedef struct {
+  ble_event_type_t type;
+  int connection_id;
+  uint8_t data_len;
+  uint8_t data[6];
+} ble_event_t;
+
+typedef struct {
+  bool connected;
+  uint8_t peer_count;
+} ble_state_t;
+
+// Initializes the BLE module
+//
+// Sets up the BLE hardware and software resources,
+// preparing the module for operation.
+// The function has no effect if the module was already initialized.
+void ble_init(void);
+
+// Deinitializes the BLE module
+//
+// Releases resources allocated during initialization
+// and shuts down the BLE module.
+void ble_deinit(void);
+
+// Starts BLE operations
+//
+// Enables reception of messages over BLE
+void ble_start(void);
+
+// Stops BLE operations
+//
+// Disables reception of messages over BLE
+// Flushes any queued messages
+void ble_stop(void);
+
+// Issues a command to the BLE module
+//
+// Sends a specific command to the BLE module for execution.
+//
+// Returns `true` if the command was successfully issued.
+bool ble_issue_command(ble_command_t command);
+
+// Reads an event from the BLE module
+//
+// Retrieves the next event from the BLE module's event queue.
+//
+// Returns `true` if an event was successfully read, `false` if no event is
+// available.
+bool ble_get_event(ble_event_t *event);
+
+// Retrieves the current state of the BLE module
+//
+// Obtains the current operational state of the BLE module.
+void ble_get_state(ble_state_t *state);
+
+// Check if write is possible
+bool ble_can_write(void);
+
+// Writes data to a connected BLE device
+//
+// Sends data over an established BLE connection.
+bool ble_write(const uint8_t *data, uint16_t len);
+
+// Check if read is possible
+bool ble_can_read(void);
+
+// Reads data from a connected BLE device
+//
+// max_len indicates the maximum number of bytes to read. Rest of the data
+// will be discarded.
+//
+// Returns the number of bytes actually read.
+uint32_t ble_read(uint8_t *data, uint16_t max_len);
+
+#endif

core/embed/io/ble/stm32/ble.c

@@ -0,0 +1,505 @@
+/*
+ * This file is part of the Trezor project, https://trezor.io/
+ *
+ * Copyright (c) SatoshiLabs
+ *
+ * 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 <http://www.gnu.org/licenses/>.
+ */
+
+#ifdef KERNEL_MODE
+
+#include <trezor_rtl.h>
+
+#include <io/ble.h>
+#include <io/nrf.h>
+#include <sys/systimer.h>
+#include <util/tsqueue.h>
+
+#include "ble_comm_defs.h"
+
+typedef enum {
+  BLE_MODE_OFF,
+  BLE_MODE_CONNECTABLE,
+  BLE_MODE_PAIRING,
+  BLE_MODE_DFU,
+} ble_mode_t;
+
+#define EVENT_QUEUE_LEN 4
+#define DATA_QUEUE_LEN 16
+#define SEND_QUEUE_LEN 1
+#define LOOP_PERIOD_MS 20
+#define PING_PERIOD 100
+
+typedef struct {
+  ble_mode_t mode_requested;
+  ble_mode_t mode_current;
+  bool connected;
+  uint8_t peer_count;
+  bool initialized;
+  bool status_valid;
+  bool accept_msgs;
+  ble_event_t event_buffers[EVENT_QUEUE_LEN];
+  tsqueue_entry_t event_queue_entries[EVENT_QUEUE_LEN];
+  tsqueue_t event_queue;
+
+  uint8_t data_buffers[DATA_QUEUE_LEN][BLE_RX_PACKET_SIZE];
+  tsqueue_entry_t data_queue_entries[DATA_QUEUE_LEN];
+  tsqueue_t data_queue;
+
+  uint8_t send_buffer[NRF_MAX_TX_DATA_SIZE];
+  tsqueue_entry_t send_queue_entries[DATA_QUEUE_LEN];
+  tsqueue_t send_queue;
+
+  uint16_t ping_cntr;
+} ble_driver_t;
+
+static ble_driver_t g_ble_driver = {0};
+
+static void ble_send_state_request(void) {
+  uint8_t cmd = INTERNAL_CMD_PING;
+  nrf_send_msg(NRF_SERVICE_BLE_MANAGER, &cmd, sizeof(cmd), NULL, NULL);
+}
+
+static void ble_send_advertising_on(bool whitelist) {
+  uint8_t data[2];
+  data[0] = INTERNAL_CMD_ADVERTISING_ON;
+  data[1] = whitelist ? 1 : 0;
+  nrf_send_msg(NRF_SERVICE_BLE_MANAGER, data, sizeof(data), NULL, NULL);
+}
+
+static void ble_send_advertising_off(void) {
+  uint8_t cmd = INTERNAL_CMD_ADVERTISING_OFF;
+  nrf_send_msg(NRF_SERVICE_BLE_MANAGER, &cmd, sizeof(cmd), NULL, NULL);
+}
+
+static bool ble_send_erase_bonds(void) {
+  if (!nrf_is_running()) {
+    return false;
+  }
+  uint8_t cmd = INTERNAL_CMD_ERASE_BONDS;
+  nrf_send_msg(NRF_SERVICE_BLE_MANAGER, &cmd, sizeof(cmd), NULL, NULL);
+
+  return true;
+}
+
+static bool ble_send_disconnect(void) {
+  if (!nrf_is_running()) {
+    return false;
+  }
+  uint8_t cmd = INTERNAL_CMD_DISCONNECT;
+  nrf_send_msg(NRF_SERVICE_BLE_MANAGER, &cmd, sizeof(cmd), NULL, NULL);
+
+  return true;
+}
+
+static void ble_send_pairing_reject(void) {
+  uint8_t cmd = INTERNAL_CMD_REJECT_PAIRING;
+  nrf_send_msg(NRF_SERVICE_BLE_MANAGER, &cmd, sizeof(cmd), NULL, NULL);
+}
+
+static void ble_send_pairing_accept(void) {
+  uint8_t cmd = INTERNAL_CMD_ALLOW_PAIRING;
+  nrf_send_msg(NRF_SERVICE_BLE_MANAGER, &cmd, sizeof(cmd), NULL, NULL);
+}
+
+static void ble_process_rx_msg_status(const uint8_t *data, uint32_t len) {
+  ble_driver_t *drv = &g_ble_driver;
+
+  if (!drv->initialized) {
+    return;
+  }
+  if (len < sizeof(event_status_msg_t)) {
+    // insufficient data length
+    return;
+  }
+
+  event_status_msg_t msg;
+  memcpy(&msg, data, sizeof(event_status_msg_t));
+
+  if (!drv->status_valid) {
+    if (msg.peer_count > 0) {
+      drv->mode_requested = BLE_MODE_CONNECTABLE;
+    } else {
+      drv->mode_requested = BLE_MODE_OFF;
+    }
+  }
+
+  if (drv->connected != msg.connected) {
+    if (msg.connected) {
+      // new connection
+
+      ble_event_t event = {.type = BLE_CONNECTED};
+      tsqueue_enqueue(&drv->event_queue, (uint8_t *)&event, sizeof(event),
+                      NULL);
+
+    } else {
+      // connection lost
+      ble_event_t event = {.type = BLE_DISCONNECTED};
+      tsqueue_enqueue(&drv->event_queue, (uint8_t *)&event, sizeof(event),
+                      NULL);
+
+      if (drv->mode_current == BLE_MODE_PAIRING) {
+        drv->mode_requested = BLE_MODE_CONNECTABLE;
+      }
+    }
+
+    drv->connected = msg.connected;
+  }
+
+  if (msg.advertising && !msg.advertising_whitelist) {
+    drv->mode_current = BLE_MODE_PAIRING;
+  } else if (msg.advertising) {
+    drv->mode_current = BLE_MODE_CONNECTABLE;
+  } else {
+    drv->mode_current = BLE_MODE_OFF;
+  }
+
+  drv->peer_count = msg.peer_count;
+
+  drv->status_valid = true;
+}
+
+static void ble_process_rx_msg_pairing_request(const uint8_t *data,
+                                               uint32_t len) {
+  ble_driver_t *drv = &g_ble_driver;
+  if (!drv->initialized) {
+    return;
+  }
+
+  if (len < 7) {
+    // insufficient data length
+    return;
+  }
+
+  if (drv->mode_requested != BLE_MODE_PAIRING ||
+      drv->mode_current != BLE_MODE_PAIRING) {
+    ble_send_pairing_reject();
+    return;
+  }
+
+  ble_event_t event = {.type = BLE_PAIRING_REQUEST, .data_len = 6};
+  memcpy(event.data, &data[1], 6);
+  tsqueue_enqueue(&drv->event_queue, (uint8_t *)&event, sizeof(event), NULL);
+}
+
+static void ble_process_rx_msg_pairing_cancelled(const uint8_t *data,
+                                                 uint32_t len) {
+  ble_driver_t *drv = &g_ble_driver;
+  if (!drv->initialized) {
+    return;
+  }
+
+  ble_event_t event = {.type = BLE_PAIRING_CANCELLED, .data_len = 0};
+  tsqueue_enqueue(&drv->event_queue, (uint8_t *)&event, sizeof(event), NULL);
+}
+
+static void ble_process_rx_msg(const uint8_t *data, uint32_t len) {
+  switch (data[0]) {
+    case INTERNAL_EVENT_STATUS:
+      ble_process_rx_msg_status(data, len);
+      break;
+    case INTERNAL_EVENT_PAIRING_REQUEST:
+      ble_process_rx_msg_pairing_request(data, len);
+      break;
+    case INTERNAL_EVENT_PAIRING_CANCELLED:
+      ble_process_rx_msg_pairing_cancelled(data, len);
+      break;
+    default:
+      break;
+  }
+}
+
+static void ble_process_data(const uint8_t *data, uint32_t len) {
+  ble_driver_t *drv = &g_ble_driver;
+
+  if (!drv->initialized) {
+    return;
+  }
+
+  if (len != BLE_RX_PACKET_SIZE) {
+    return;
+  }
+
+  tsqueue_enqueue(&drv->data_queue, data, len, NULL);
+}
+
+// background loop, called from systimer every 10ms
+static void ble_loop(void *context) {
+  (void)context;
+  ble_driver_t *drv = &g_ble_driver;
+
+  if (!drv->initialized) {
+    return;
+  }
+
+  if (nrf_is_running()) {
+    if (!drv->status_valid) {
+      ble_send_state_request();
+    }
+
+    if (drv->ping_cntr++ > (PING_PERIOD / LOOP_PERIOD_MS)) {
+      ble_send_state_request();
+      drv->ping_cntr = 0;
+    }
+
+    uint8_t data[NRF_MAX_TX_DATA_SIZE] = {0};
+    if (tsqueue_dequeue(&drv->send_queue, data, NRF_MAX_TX_DATA_SIZE, NULL,
+                        NULL)) {
+      if (!nrf_send_msg(NRF_SERVICE_BLE, data, NRF_MAX_TX_DATA_SIZE, NULL,
+                        NULL)) {
+        tsqueue_enqueue(&drv->send_queue, data, NRF_MAX_TX_DATA_SIZE, NULL);
+      }
+    }
+
+    if (drv->mode_current != drv->mode_requested) {
+      if (drv->mode_requested == BLE_MODE_OFF) {
+        ble_send_advertising_off();
+        // if (drv->connected) {
+        //   nrf_send_disconnect();
+        // }
+      } else if (drv->mode_requested == BLE_MODE_CONNECTABLE) {
+        ble_send_advertising_on(true);
+      } else if (drv->mode_requested == BLE_MODE_PAIRING) {
+        ble_send_advertising_on(false);
+      }
+    }
+  } else {
+    drv->status_valid = false;
+  }
+}
+
+void ble_init(void) {
+  ble_driver_t *drv = &g_ble_driver;
+
+  if (drv->initialized) {
+    return;
+  }
+
+  memset(drv, 0, sizeof(ble_driver_t));
+
+  tsqueue_init(&drv->event_queue, drv->event_queue_entries,
+               (uint8_t *)drv->event_buffers, sizeof(ble_event_t),
+               EVENT_QUEUE_LEN);
+
+  tsqueue_init(&drv->data_queue, drv->data_queue_entries,
+               (uint8_t *)drv->data_buffers, BLE_RX_PACKET_SIZE,
+               DATA_QUEUE_LEN);
+
+  tsqueue_init(&drv->send_queue, drv->send_queue_entries,
+               (uint8_t *)drv->send_buffer, NRF_MAX_TX_DATA_SIZE,
+               SEND_QUEUE_LEN);
+
+  systimer_t *timer = systimer_create(ble_loop, NULL);
+
+  systimer_set_periodic(timer, LOOP_PERIOD_MS);
+
+  nrf_init();
+  nrf_register_listener(NRF_SERVICE_BLE_MANAGER, ble_process_rx_msg);
+  nrf_register_listener(NRF_SERVICE_BLE, ble_process_data);
+
+  drv->initialized = true;
+}
+
+void ble_deinit(void) {
+  ble_driver_t *drv = &g_ble_driver;
+
+  if (!drv->initialized) {
+    return;
+  }
+
+  tsqueue_reset(&drv->event_queue);
+  tsqueue_reset(&drv->data_queue);
+  tsqueue_reset(&drv->send_queue);
+
+  nrf_unregister_listener(NRF_SERVICE_BLE);
+  nrf_unregister_listener(NRF_SERVICE_BLE_MANAGER);
+
+  drv->initialized = false;
+}
+
+bool ble_connected(void) {
+  ble_driver_t *drv = &g_ble_driver;
+
+  if (!drv->initialized) {
+    return false;
+  }
+
+  return drv->connected && nrf_is_running();
+}
+
+void ble_start(void) {
+  ble_driver_t *drv = &g_ble_driver;
+
+  if (!drv->initialized) {
+    return;
+  }
+
+  drv->accept_msgs = true;
+}
+
+void ble_stop(void) {
+  ble_driver_t *drv = &g_ble_driver;
+
+  if (!drv->initialized) {
+    return;
+  }
+
+  drv->accept_msgs = false;
+  tsqueue_reset(&drv->data_queue);
+}
+
+bool ble_can_write(void) {
+  ble_driver_t *drv = &g_ble_driver;
+
+  if (!drv->initialized) {
+    return false;
+  }
+
+  if (!drv->connected || !drv->accept_msgs) {
+    return false;
+  }
+
+  return !tsqueue_full(&drv->send_queue);
+}
+
+bool ble_write(const uint8_t *data, uint16_t len) {
+  ble_driver_t *drv = &g_ble_driver;
+
+  if (!drv->initialized) {
+    return false;
+  }
+
+  if (!drv->connected || !drv->accept_msgs) {
+    return false;
+  }
+
+  bool sent = nrf_send_msg(NRF_SERVICE_BLE, data, len, NULL, NULL);
+
+  if (!sent) {
+    bool queued = tsqueue_enqueue(&drv->send_queue, data, len, NULL);
+    return queued;
+  }
+
+  return true;
+}
+
+bool ble_can_read(void) {
+  ble_driver_t *drv = &g_ble_driver;
+
+  if (!drv->initialized) {
+    return false;
+  }
+
+  return !tsqueue_empty(&drv->data_queue);
+}
+
+uint32_t ble_read(uint8_t *data, uint16_t max_len) {
+  ble_driver_t *drv = &g_ble_driver;
+
+  if (!drv->initialized) {
+    return 0;
+  }
+
+  tsqueue_t *queue = &drv->data_queue;
+
+  uint16_t read_len = 0;
+
+  bool received = tsqueue_dequeue(queue, data, max_len, &read_len, NULL);
+
+  if (!received) {
+    return 0;
+  }
+
+  return max_len;
+}
+
+bool ble_issue_command(ble_command_t command) {
+  ble_driver_t *drv = &g_ble_driver;
+
+  if (!drv->initialized) {
+    return false;
+  }
+
+  switch (command) {
+    case BLE_SWITCH_OFF:
+      drv->mode_requested = BLE_MODE_OFF;
+      break;
+    case BLE_SWITCH_ON:
+      drv->mode_requested = BLE_MODE_CONNECTABLE;
+      break;
+    case BLE_PAIRING_MODE:
+      drv->mode_requested = BLE_MODE_PAIRING;
+      break;
+    case BLE_DISCONNECT:
+      ble_send_disconnect();
+      break;
+    case BLE_ERASE_BONDS:
+      ble_send_erase_bonds();
+      break;
+    case BLE_ALLOW_PAIRING:
+      ble_send_pairing_accept();
+      break;
+    case BLE_REJECT_PAIRING:
+      ble_send_pairing_reject();
+      break;
+    default:
+      // unknown command
+      return false;
+  }
+
+  return true;
+}
+
+bool ble_get_event(ble_event_t *event) {
+  ble_driver_t *drv = &g_ble_driver;
+
+  if (!drv->initialized) {
+    return false;
+  }
+
+  ble_event_t tmp_event = {0};
+  uint16_t len = 0;
+  bool read = tsqueue_dequeue(&drv->event_queue, (uint8_t *)&tmp_event,
+                              sizeof(tmp_event), &len, NULL);
+
+  if (!read) {
+    return false;
+  }
+
+  if (len != sizeof(ble_event_t)) {
+    return false;
+  }
+
+  memcpy(event, &tmp_event, sizeof(ble_event_t));
+
+  return true;
+}
+
+void ble_get_state(ble_state_t *state) {
+  const ble_driver_t *drv = &g_ble_driver;
+
+  if (state == NULL) {
+    return;
+  }
+
+  if (!drv->initialized) {
+    memset(state, 0, sizeof(ble_state_t));
+    return;
+  }
+
+  state->connected = drv->connected;
+  state->peer_count = drv->peer_count;
+}
+
+#endif

core/embed/io/ble/stm32/ble_comm_defs.h

@@ -0,0 +1,59 @@
+/*
+ * This file is part of the Trezor project, https://trezor.io/
+ *
+ * Copyright (c) SatoshiLabs
+ *
+ * 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 <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef TREZORHAL_BLE_COMM_DEFS_H_
+#define TREZORHAL_BLE_COMM_DEFS_H_
+
+#include <trezor_types.h>
+
+typedef struct {
+  uint8_t msg_id;
+  uint8_t connected;
+  uint8_t advertising;
+  uint8_t advertising_whitelist;
+
+  uint8_t peer_count;
+  uint8_t reserved[2];
+  uint8_t sd_version_number;
+
+  uint16_t sd_company_id;
+  uint16_t sd_subversion_number;
+
+  uint32_t app_version;
+  uint32_t bld_version;
+
+} event_status_msg_t;
+
+typedef enum {
+  INTERNAL_EVENT_STATUS = 0x01,
+  INTERNAL_EVENT_PAIRING_REQUEST = 0x04,
+  INTERNAL_EVENT_PAIRING_CANCELLED = 0x05,
+} InternalEvent_t;
+
+typedef enum {
+  INTERNAL_CMD_PING = 0x00,
+  INTERNAL_CMD_ADVERTISING_ON = 0x01,
+  INTERNAL_CMD_ADVERTISING_OFF = 0x02,
+  INTERNAL_CMD_ERASE_BONDS = 0x03,
+  INTERNAL_CMD_DISCONNECT = 0x04,
+  INTERNAL_CMD_ACK = 0x05,
+  INTERNAL_CMD_ALLOW_PAIRING = 0x06,
+  INTERNAL_CMD_REJECT_PAIRING = 0x07,
+} InternalCmd_t;
+#endif

core/embed/io/nrf/crc8.c

@@ -0,0 +1,53 @@
+/*
+ * Copyright (c) 2020 Intel Corporation
+ * Copyright (c) 2017 Nordic Semiconductor ASA
+ * Copyright (c) 2015 Runtime Inc
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ */
+
+#include "crc8.h"
+
+static const uint8_t crc8_ccitt_small_table[16] = {
+    0x00, 0x07, 0x0e, 0x09, 0x1c, 0x1b, 0x12, 0x15,
+    0x38, 0x3f, 0x36, 0x31, 0x24, 0x23, 0x2a, 0x2d};
+
+uint8_t crc8_ccitt(uint8_t val, const void *buf, size_t cnt) {
+  size_t i;
+  const uint8_t *p = buf;
+
+  for (i = 0; i < cnt; i++) {
+    val ^= p[i];
+    val = (val << 4) ^ crc8_ccitt_small_table[val >> 4];
+    val = (val << 4) ^ crc8_ccitt_small_table[val >> 4];
+  }
+  return val;
+}
+
+uint8_t crc8(const uint8_t *src, size_t len, uint8_t polynomial,
+             uint8_t initial_value, bool reversed) {
+  uint8_t crc = initial_value;
+  size_t i, j;
+
+  for (i = 0; i < len; i++) {
+    crc ^= src[i];
+
+    for (j = 0; j < 8; j++) {
+      if (reversed) {
+        if (crc & 0x01) {
+          crc = (crc >> 1) ^ polynomial;
+        } else {
+          crc >>= 1;
+        }
+      } else {
+        if (crc & 0x80) {
+          crc = (crc << 1) ^ polynomial;
+        } else {
+          crc <<= 1;
+        }
+      }
+    }
+  }
+
+  return crc;
+}

core/embed/io/nrf/crc8.h

@@ -0,0 +1,28 @@
+/*
+ * This file is part of the Trezor project, https://trezor.io/
+ *
+ * Copyright (c) SatoshiLabs
+ *
+ * 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 <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef TREZORHAL_NRF_CRC8_H
+#define TREZORHAL_NRF_CRC8_H
+
+#include <trezor_types.h>
+
+uint8_t crc8(const uint8_t *src, size_t len, uint8_t polynomial,
+             uint8_t initial_value, bool reversed);
+
+#endif

core/embed/io/nrf/inc/io/nrf.h

@@ -0,0 +1,81 @@
+/*
+ * This file is part of the Trezor project, https://trezor.io/
+ *
+ * Copyright (c) SatoshiLabs
+ *
+ * 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 <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef TREZORHAL_NRF_H
+#define TREZORHAL_NRF_H
+
+#include <trezor_types.h>
+
+// maximum data size allowed to be sent
+#define NRF_MAX_TX_DATA_SIZE (64)
+
+typedef enum {
+  NRF_SERVICE_BLE = 0,
+  NRF_SERVICE_BLE_MANAGER = 1,
+
+  NRF_SERVICE_CNT  // Number of services
+} nrf_service_id_t;
+
+typedef enum {
+  NRF_STATUS_OK = 0,       // Packet completed successfully
+  NRF_STATUS_TIMEOUT = 1,  // Timeout occurred
+  NRF_STATUS_ERROR = 2,    // General error
+  NRF_STATUS_ABORTED = 3,  // Packet was aborted
+} nrf_status_t;
+
+typedef struct {
+  const uint8_t *data;
+  uint32_t len;
+
+} nrf_packet_t;
+
+// Initialize the NRF driver
+void nrf_init(void);
+
+// Deinitialize the NRF driver
+void nrf_deinit(void);
+
+// Check that NRF is running
+bool nrf_is_running(void);
+
+// Register listener for a service
+// The listener will be called when a message is received for the service
+// The listener will be called from an interrupt context
+void nrf_register_listener(nrf_service_id_t service,
+                           void (*listener)(const uint8_t *data, uint32_t len));
+
+// Unregister listener for a service
+void nrf_unregister_listener(nrf_service_id_t service);
+
+// Send a message to a service
+// The message will be queued and sent as soon as possible
+// If the queue is full, the message will be dropped
+// returns ID of the message if it was successfully queued, otherwise -1
+int32_t nrf_send_msg(nrf_service_id_t service, const uint8_t *data,
+                     uint32_t len,
+                     void (*callback)(nrf_status_t status, void *context),
+                     void *context);
+
+// Abort a message by ID
+// If the message is already sent or the id is not found, it does nothing and
+// returns false If the message is queued, it will be removed from the queue If
+// the message is being sent, it will be sent. The callback will not be called.
+bool nrf_abort_msg(int32_t id);
+
+#endif

core/embed/io/nrf/nrf_internal.h

@@ -0,0 +1,39 @@
+/*
+ * This file is part of the Trezor project, https://trezor.io/
+ *
+ * Copyright (c) SatoshiLabs
+ *
+ * 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 <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef TREZORHAL_NRF_INTERNAL_H
+#define TREZORHAL_NRF_INTERNAL_H
+
+#include <trezor_types.h>
+
+void nrf_dfu_comm_send(const uint8_t *data, uint32_t len);
+uint32_t nrf_dfu_comm_receive(uint8_t *data, uint32_t len);
+
+void nrf_int_send(const uint8_t *data, uint32_t len);
+uint32_t nrf_int_receive(uint8_t *data, uint32_t len);
+
+bool nrf_firmware_running(void);
+
+bool nrf_reboot(void);
+bool nrf_reboot_to_bootloader(void);
+
+void nrf_signal_running(void);
+void nrf_signal_off(void);
+
+#endif

core/embed/io/nrf/stm32u5/nrf.c

@@ -0,0 +1,767 @@
+/*
+ * This file is part of the Trezor project, https://trezor.io/
+ *
+ * Copyright (c) SatoshiLabs
+ *
+ * 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 <http://www.gnu.org/licenses/>.
+ */
+
+#ifdef KERNEL_MODE
+
+#include <trezor_bsp.h>
+#include <trezor_rtl.h>
+
+#include <io/nrf.h>
+#include <sys/irq.h>
+#include <sys/mpu.h>
+#include <util/tsqueue.h>
+
+#include "../crc8.h"
+#include "../nrf_internal.h"
+
+#define MAX_SPI_DATA_SIZE (244)
+
+typedef struct {
+  uint8_t service_id;
+} spi_header_t;
+
+typedef struct {
+  uint8_t crc;
+} spi_footer_t;
+
+#define SPI_HEADER_SIZE (sizeof(spi_header_t))
+#define SPI_FOOTER_SIZE (sizeof(spi_footer_t))
+#define SPI_OVERHEAD_SIZE (SPI_HEADER_SIZE + SPI_FOOTER_SIZE)
+#define SPI_PACKET_SIZE (MAX_SPI_DATA_SIZE + SPI_OVERHEAD_SIZE)
+
+typedef struct {
+  uint8_t service_id;
+  uint8_t msg_len;
+} uart_header_t;
+
+typedef struct {
+  uint8_t crc;
+} uart_footer_t;
+
+#define UART_HEADER_SIZE (sizeof(uart_header_t))
+#define UART_FOOTER_SIZE (sizeof(uart_footer_t))
+#define UART_OVERHEAD_SIZE (UART_HEADER_SIZE + UART_FOOTER_SIZE)
+#define UART_PACKET_SIZE (NRF_MAX_TX_DATA_SIZE + UART_OVERHEAD_SIZE)
+#define UART_QUEUE_SIZE (8)
+
+#define START_BYTE (0xA0)
+
+typedef struct {
+  uint8_t data[UART_PACKET_SIZE];
+  uint8_t len;
+  void (*callback)(nrf_status_t status, void *context);
+  void *context;
+} nrf_uart_tx_data_t;
+
+typedef struct {
+  UART_HandleTypeDef urt;
+  DMA_HandleTypeDef urt_tx_dma;
+
+  uint8_t urt_tx_buffers[UART_QUEUE_SIZE][sizeof(nrf_uart_tx_data_t)];
+  tsqueue_entry_t urt_tx_queue_entries[UART_QUEUE_SIZE];
+  tsqueue_t urt_tx_queue;
+  nrf_uart_tx_data_t urt_sending;
+
+  uint8_t urt_rx_buf[UART_PACKET_SIZE];
+  uint8_t urt_rx_len;
+  uint8_t urt_rx_byte;
+  uint16_t urt_rx_idx;
+
+  SPI_HandleTypeDef spi;
+  DMA_HandleTypeDef spi_dma;
+  uint8_t spi_buffer[SPI_PACKET_SIZE];
+
+  int32_t urt_tx_msg_id;
+  bool urt_tx_running;
+  bool spi_rx_running;
+  bool comm_running;
+
+  bool initialized;
+
+  void (*service_listeners[NRF_SERVICE_CNT])(const uint8_t *data, uint32_t len);
+
+} nrf_driver_t;
+
+static nrf_driver_t g_nrf_driver = {0};
+
+static void nrf_start(void) {
+  nrf_driver_t *drv = &g_nrf_driver;
+  if (!drv->initialized) {
+    return;
+  }
+
+  HAL_SPI_Receive_DMA(&drv->spi, drv->spi_buffer, SPI_PACKET_SIZE);
+
+  tsqueue_reset(&drv->urt_tx_queue);
+  HAL_UART_Receive_IT(&drv->urt, &drv->urt_rx_byte, 1);
+
+  drv->spi_rx_running = true;
+  drv->comm_running = true;
+
+  nrf_signal_running();
+}
+
+static void nrf_stop(void) {
+  nrf_driver_t *drv = &g_nrf_driver;
+  if (!drv->initialized) {
+    return;
+  }
+
+  nrf_signal_off();
+  irq_key_t key = irq_lock();
+  drv->comm_running = false;
+  HAL_SPI_DMAStop(&drv->spi);
+  // todo notify listeners
+  tsqueue_reset(&drv->urt_tx_queue);
+  irq_unlock(key);
+}
+
+void nrf_init(void) {
+  nrf_driver_t *drv = &g_nrf_driver;
+
+  if (drv->initialized) {
+    return;
+  }
+
+  __HAL_RCC_USART3_CLK_ENABLE();
+  __HAL_RCC_GPDMA1_CLK_ENABLE();
+  __HAL_RCC_SPI1_CLK_ENABLE();
+  __HAL_RCC_GPIOA_CLK_ENABLE();
+  __HAL_RCC_GPIOB_CLK_ENABLE();
+  __HAL_RCC_GPIOD_CLK_ENABLE();
+
+  memset(drv, 0, sizeof(*drv));
+  tsqueue_init(&drv->urt_tx_queue, drv->urt_tx_queue_entries,
+               (uint8_t *)drv->urt_tx_buffers, sizeof(nrf_uart_tx_data_t),
+               UART_QUEUE_SIZE);
+
+  GPIO_InitTypeDef GPIO_InitStructure = {0};
+
+  // synchronization signals
+  NRF_RESET_CLK_ENA();
+  HAL_GPIO_WritePin(NRF_RESET_PORT, NRF_RESET_PIN, GPIO_PIN_SET);
+  GPIO_InitStructure.Mode = GPIO_MODE_OUTPUT_PP;
+  GPIO_InitStructure.Pull = GPIO_PULLDOWN;
+  GPIO_InitStructure.Speed = GPIO_SPEED_FREQ_LOW;
+  GPIO_InitStructure.Pin = NRF_RESET_PIN;
+  HAL_GPIO_Init(NRF_RESET_PORT, &GPIO_InitStructure);
+
+  NRF_GPIO0_CLK_ENA();
+  GPIO_InitStructure.Mode = GPIO_MODE_INPUT;
+  GPIO_InitStructure.Pull = GPIO_PULLDOWN;
+  GPIO_InitStructure.Speed = GPIO_SPEED_FREQ_LOW;
+  GPIO_InitStructure.Pin = NRF_GPIO0_PIN;
+  HAL_GPIO_Init(NRF_GPIO0_PORT, &GPIO_InitStructure);
+
+  NRF_GPIO1_CLK_ENA();
+  GPIO_InitStructure.Mode = GPIO_MODE_INPUT;
+  GPIO_InitStructure.Pull = GPIO_PULLDOWN;
+  GPIO_InitStructure.Speed = GPIO_SPEED_FREQ_LOW;
+  GPIO_InitStructure.Pin = NRF_GPIO1_PIN;
+  HAL_GPIO_Init(NRF_GPIO1_PORT, &GPIO_InitStructure);
+
+  NRF_GPIO2_CLK_ENA();
+  GPIO_InitStructure.Mode = GPIO_MODE_OUTPUT_PP;
+  GPIO_InitStructure.Pull = GPIO_PULLDOWN;
+  GPIO_InitStructure.Speed = GPIO_SPEED_FREQ_LOW;
+  GPIO_InitStructure.Pin = NRF_GPIO2_PIN;
+  HAL_GPIO_Init(NRF_GPIO2_PORT, &GPIO_InitStructure);
+
+  NRF_GPIO3_CLK_ENA();
+  GPIO_InitStructure.Mode = GPIO_MODE_OUTPUT_PP;
+  GPIO_InitStructure.Pull = GPIO_PULLDOWN;
+  GPIO_InitStructure.Speed = GPIO_SPEED_FREQ_LOW;
+  GPIO_InitStructure.Pin = NRF_GPIO3_PIN;
+  HAL_GPIO_Init(NRF_GPIO3_PORT, &GPIO_InitStructure);
+
+  // UART PINS
+  GPIO_InitStructure.Mode = GPIO_MODE_AF_PP;
+  GPIO_InitStructure.Pull = GPIO_NOPULL;
+  GPIO_InitStructure.Alternate = GPIO_AF7_USART3;
+  GPIO_InitStructure.Speed = GPIO_SPEED_FREQ_LOW;
+
+  GPIO_InitStructure.Pin = GPIO_PIN_5;
+  HAL_GPIO_Init(GPIOA, &GPIO_InitStructure);
+  GPIO_InitStructure.Pin = GPIO_PIN_10 | GPIO_PIN_1;
+  HAL_GPIO_Init(GPIOB, &GPIO_InitStructure);
+  GPIO_InitStructure.Pin = GPIO_PIN_11;
+  HAL_GPIO_Init(GPIOD, &GPIO_InitStructure);
+
+  drv->urt.Init.Mode = UART_MODE_TX_RX;
+  drv->urt.Init.BaudRate = 1000000;
+  drv->urt.Init.HwFlowCtl = UART_HWCONTROL_RTS_CTS;
+  drv->urt.Init.OverSampling = UART_OVERSAMPLING_16;
+  drv->urt.Init.Parity = UART_PARITY_NONE;
+  drv->urt.Init.StopBits = UART_STOPBITS_1;
+  drv->urt.Init.WordLength = UART_WORDLENGTH_8B;
+  drv->urt.Instance = USART3;
+  drv->urt.hdmatx = &drv->urt_tx_dma;
+
+  drv->urt_tx_dma.Init.Direction = DMA_MEMORY_TO_PERIPH;
+  drv->urt_tx_dma.Init.Mode = DMA_NORMAL;
+  drv->urt_tx_dma.Instance = GPDMA1_Channel1;
+  drv->urt_tx_dma.Init.Request = GPDMA1_REQUEST_USART3_TX;
+  drv->urt_tx_dma.Init.BlkHWRequest = DMA_BREQ_SINGLE_BURST;
+  drv->urt_tx_dma.Init.SrcInc = DMA_SINC_INCREMENTED;
+  drv->urt_tx_dma.Init.DestInc = DMA_DINC_FIXED;
+  drv->urt_tx_dma.Init.SrcDataWidth = DMA_SRC_DATAWIDTH_BYTE;
+  drv->urt_tx_dma.Init.DestDataWidth = DMA_DEST_DATAWIDTH_BYTE;
+  drv->urt_tx_dma.Init.Priority = DMA_LOW_PRIORITY_HIGH_WEIGHT;
+  drv->urt_tx_dma.Init.SrcBurstLength = 1;
+  drv->urt_tx_dma.Init.DestBurstLength = 1;
+  drv->urt_tx_dma.Init.TransferAllocatedPort =
+      DMA_SRC_ALLOCATED_PORT1 | DMA_DEST_ALLOCATED_PORT0;
+  drv->urt_tx_dma.Init.TransferEventMode = DMA_TCEM_BLOCK_TRANSFER;
+
+  drv->urt_tx_dma.Parent = &drv->urt;
+  HAL_DMA_Init(&drv->urt_tx_dma);
+  HAL_DMA_ConfigChannelAttributes(
+      &drv->urt_tx_dma, DMA_CHANNEL_PRIV | DMA_CHANNEL_SEC |
+                            DMA_CHANNEL_SRC_SEC | DMA_CHANNEL_DEST_SEC);
+
+  HAL_UART_Init(&drv->urt);
+
+  NVIC_SetPriority(GPDMA1_Channel1_IRQn, IRQ_PRI_NORMAL);
+  NVIC_EnableIRQ(GPDMA1_Channel1_IRQn);
+  NVIC_SetPriority(USART3_IRQn, IRQ_PRI_NORMAL);
+  NVIC_EnableIRQ(USART3_IRQn);
+
+  // SPI pins
+  GPIO_InitStructure.Mode = GPIO_MODE_AF_PP;
+  GPIO_InitStructure.Pull = GPIO_NOPULL;
+  GPIO_InitStructure.Alternate = GPIO_AF5_SPI1;
+  GPIO_InitStructure.Speed = GPIO_SPEED_FREQ_MEDIUM;
+  GPIO_InitStructure.Pin = GPIO_PIN_1 | GPIO_PIN_4 | GPIO_PIN_6 | GPIO_PIN_7;
+  HAL_GPIO_Init(GPIOA, &GPIO_InitStructure);
+
+  drv->spi_dma.Instance = GPDMA1_Channel2;
+  drv->spi_dma.Init.Direction = DMA_PERIPH_TO_MEMORY;
+  drv->spi_dma.Init.Mode = DMA_NORMAL;
+  drv->spi_dma.Init.Request = GPDMA1_REQUEST_SPI1_RX;
+  drv->spi_dma.Init.BlkHWRequest = DMA_BREQ_SINGLE_BURST;
+  drv->spi_dma.Init.SrcInc = DMA_SINC_FIXED;
+  drv->spi_dma.Init.DestInc = DMA_DINC_INCREMENTED;
+  drv->spi_dma.Init.SrcDataWidth = DMA_SRC_DATAWIDTH_BYTE;
+  drv->spi_dma.Init.DestDataWidth = DMA_DEST_DATAWIDTH_BYTE;
+  drv->spi_dma.Init.Priority = DMA_LOW_PRIORITY_HIGH_WEIGHT;
+  drv->spi_dma.Init.SrcBurstLength = 1;
+  drv->spi_dma.Init.DestBurstLength = 1;
+  drv->spi_dma.Init.TransferAllocatedPort =
+      DMA_SRC_ALLOCATED_PORT1 | DMA_DEST_ALLOCATED_PORT0;
+  drv->spi_dma.Init.TransferEventMode = DMA_TCEM_BLOCK_TRANSFER;
+
+  HAL_DMA_Init(&drv->spi_dma);
+  HAL_DMA_ConfigChannelAttributes(
+      &drv->spi_dma, DMA_CHANNEL_PRIV | DMA_CHANNEL_SEC | DMA_CHANNEL_SRC_SEC |
+                         DMA_CHANNEL_DEST_SEC);
+
+  drv->spi.Instance = SPI1;
+  drv->spi.Init.Mode = SPI_MODE_SLAVE;
+  drv->spi.Init.Direction = SPI_DIRECTION_2LINES_RXONLY;
+  drv->spi.Init.DataSize = SPI_DATASIZE_8BIT;
+  drv->spi.Init.CLKPolarity = SPI_POLARITY_LOW;
+  drv->spi.Init.CLKPhase = SPI_PHASE_1EDGE;
+  drv->spi.Init.NSS = SPI_NSS_HARD_INPUT;
+  drv->spi.Init.BaudRatePrescaler = SPI_BAUDRATEPRESCALER_2;
+  drv->spi.Init.FirstBit = SPI_FIRSTBIT_MSB;
+  drv->spi.Init.TIMode = SPI_TIMODE_DISABLE;
+  drv->spi.Init.CRCCalculation = SPI_CRCCALCULATION_DISABLE;
+  drv->spi.Init.CRCPolynomial = 0;
+  drv->spi.hdmarx = &drv->spi_dma;
+
+  drv->spi_dma.Parent = &drv->spi;
+
+  HAL_SPI_Init(&drv->spi);
+
+  NVIC_SetPriority(GPDMA1_Channel2_IRQn, IRQ_PRI_NORMAL);
+  NVIC_EnableIRQ(GPDMA1_Channel2_IRQn);
+  NVIC_SetPriority(SPI1_IRQn, IRQ_PRI_NORMAL);
+  NVIC_EnableIRQ(SPI1_IRQn);
+
+  drv->initialized = true;
+
+  nrf_start();
+}
+
+void nrf_deinit(void) {
+  nrf_driver_t *drv = &g_nrf_driver;
+  if (!drv->initialized) {
+    return;
+  }
+
+  nrf_stop();
+
+  drv->initialized = false;
+}
+
+void nrf_register_listener(nrf_service_id_t service,
+                           void (*listener)(const uint8_t *data,
+                                            uint32_t len)) {
+  nrf_driver_t *drv = &g_nrf_driver;
+  if (!drv->initialized) {
+    return;
+  }
+
+  if (service < NRF_SERVICE_CNT) {
+    drv->service_listeners[service] = listener;
+  }
+}
+
+void nrf_unregister_listener(nrf_service_id_t service) {
+  nrf_driver_t *drv = &g_nrf_driver;
+  if (!drv->initialized) {
+    return;
+  }
+
+  if (service < NRF_SERVICE_CNT) {
+    drv->service_listeners[service] = NULL;
+  }
+}
+
+static void nrf_process_msg(nrf_driver_t *drv, const uint8_t *data,
+                            uint32_t len, nrf_service_id_t service,
+                            uint8_t header_size, uint8_t overhead_size) {
+  const uint8_t *service_data = data + header_size;
+  uint32_t service_data_len = len - overhead_size;
+  if (drv->service_listeners[service] != NULL) {
+    drv->service_listeners[service](service_data, service_data_len);
+  }
+}
+
+/// DFU communication
+/// ----------------------------------------------------------
+
+void nrf_dfu_comm_send(const uint8_t *data, uint32_t len) {
+  nrf_driver_t *drv = &g_nrf_driver;
+  if (!drv->initialized) {
+    return;
+  }
+
+  HAL_UART_Transmit(&drv->urt, (uint8_t *)data, len, 30);
+}
+
+uint32_t nrf_dfu_comm_receive(uint8_t *data, uint32_t len) {
+  nrf_driver_t *drv = &g_nrf_driver;
+  if (!drv->initialized) {
+    return 0;
+  }
+
+  if (__HAL_UART_GET_FLAG(&drv->urt, UART_FLAG_RXNE)) {
+    HAL_StatusTypeDef result = HAL_UART_Receive(&drv->urt, data, len, 30);
+
+    if (result == HAL_OK) {
+      return len;
+    }
+
+    if (drv->urt.RxXferCount == len) {
+      return 0;
+    }
+
+    return len - drv->urt.RxXferCount - 1;
+  }
+
+  return 0;
+}
+
+/// UART communication
+/// ---------------------------------------------------------
+
+int32_t nrf_send_msg(nrf_service_id_t service, const uint8_t *data,
+                     uint32_t len,
+                     void (*callback)(nrf_status_t status, void *context),
+                     void *context) {
+  nrf_driver_t *drv = &g_nrf_driver;
+  if (!drv->initialized) {
+    return -1;
+  }
+
+  if (len > NRF_MAX_TX_DATA_SIZE) {
+    return -1;
+  }
+
+  if (service > NRF_SERVICE_CNT) {
+    return -1;
+  }
+
+  int32_t id = 0;
+
+  nrf_uart_tx_data_t buffer;
+
+  buffer.callback = callback;
+  buffer.context = context;
+  buffer.len = len + UART_OVERHEAD_SIZE;
+
+  uart_header_t header = {
+      .service_id = 0xA0 | (uint8_t)service,
+      .msg_len = len + UART_OVERHEAD_SIZE,
+  };
+  memcpy(buffer.data, &header, UART_HEADER_SIZE);
+
+  memcpy(&buffer.data[UART_HEADER_SIZE], data, len);
+
+  uart_footer_t footer = {
+      .crc = crc8(buffer.data, len + UART_HEADER_SIZE, 0x07, 0x00, false),
+  };
+  memcpy(&buffer.data[UART_HEADER_SIZE + len], &footer, UART_FOOTER_SIZE);
+
+  if (!tsqueue_enqueue(&drv->urt_tx_queue, (uint8_t *)&buffer,
+                       sizeof(nrf_uart_tx_data_t), &id)) {
+    return -1;
+  }
+
+  irq_key_t key = irq_lock();
+  if (!drv->urt_tx_running) {
+    int32_t tx_id = 0;
+    if (tsqueue_dequeue(&drv->urt_tx_queue, (uint8_t *)&drv->urt_sending,
+                        sizeof(nrf_uart_tx_data_t), NULL, &tx_id)) {
+      HAL_UART_Transmit_DMA(&drv->urt, drv->urt_sending.data,
+                            drv->urt_sending.len);
+      drv->urt_tx_msg_id = tx_id;
+      drv->urt_tx_running = true;
+    }
+  }
+  irq_unlock(key);
+
+  return id;
+}
+
+bool nrf_abort_msg(int32_t id) {
+  nrf_driver_t *drv = &g_nrf_driver;
+  if (!drv->initialized) {
+    return false;
+  }
+
+  bool aborted = tsqueue_abort(&drv->urt_tx_queue, id, NULL, 0, NULL);
+
+  if (aborted) {
+    return true;
+  }
+
+  if (drv->urt_tx_msg_id == id) {
+    drv->urt_tx_msg_id = -1;
+    return true;
+  }
+
+  return false;
+}
+
+static bool nrf_is_valid_startbyte(uint8_t val) {
+  if ((val & 0xF0) != 0xA0) {
+    return false;
+  }
+
+  if ((val & 0x0F) >= NRF_SERVICE_CNT) {
+    return false;
+  }
+
+  return true;
+}
+
+void HAL_UART_RxCpltCallback(UART_HandleTypeDef *urt) {
+  nrf_driver_t *drv = &g_nrf_driver;
+  if (drv->initialized && urt == &drv->urt) {
+    if (drv->urt_rx_idx == 0) {
+      // received first byte: START BYTE
+      if (nrf_is_valid_startbyte(drv->urt_rx_byte)) {
+        drv->urt_rx_buf[0] = drv->urt_rx_byte;
+        drv->urt_rx_idx++;
+      } else {
+        // bad message, flush the line
+        drv->urt_rx_idx = 0;
+      }
+    } else if (drv->urt_rx_idx == 1) {
+      // received second byte: LEN
+
+      drv->urt_rx_buf[1] = drv->urt_rx_byte;
+      drv->urt_rx_len = drv->urt_rx_byte;
+
+      if (drv->urt_rx_len > UART_PACKET_SIZE) {
+        drv->urt_rx_len = 0;
+      } else {
+        drv->urt_rx_idx++;
+      }
+    } else if (drv->urt_rx_idx >= UART_HEADER_SIZE &&
+               drv->urt_rx_idx < (drv->urt_rx_len - 1)) {
+      // receive the rest of the message
+
+      drv->urt_rx_buf[drv->urt_rx_idx] = drv->urt_rx_byte;
+      drv->urt_rx_idx++;
+
+      if (drv->urt_rx_idx >= NRF_MAX_TX_DATA_SIZE) {
+        // message is too long, flush the line
+        drv->urt_rx_idx = 0;
+        drv->urt_rx_len = 0;
+      }
+
+    } else if (drv->urt_rx_idx == (drv->urt_rx_len - 1)) {
+      // received last byte: CRC
+
+      uint8_t crc =
+          crc8(drv->urt_rx_buf, drv->urt_rx_len - 1, 0x07, 0x00, false);
+
+      if (drv->urt_rx_byte == crc) {
+        uart_header_t *header = (uart_header_t *)drv->urt_rx_buf;
+        nrf_process_msg(drv, drv->urt_rx_buf, drv->urt_rx_len,
+                        header->service_id & 0x0F, UART_HEADER_SIZE,
+                        UART_OVERHEAD_SIZE);
+      }
+
+      drv->urt_rx_idx = 0;
+      drv->urt_rx_len = 0;
+
+    } else {
+      // bad message, flush the line
+      drv->urt_rx_idx = 0;
+      drv->urt_rx_len = 0;
+    }
+  }
+
+  // receive the rest of the message, or new message in any case.
+  HAL_UART_Receive_IT(&drv->urt, &drv->urt_rx_byte, 1);
+}
+
+void HAL_UART_ErrorCallback(UART_HandleTypeDef *urt) {
+  nrf_driver_t *drv = &g_nrf_driver;
+  if (drv->initialized && urt == &drv->urt) {
+    HAL_UART_AbortReceive(urt);
+    HAL_UART_AbortTransmit(urt);
+
+    // todo senders notify about error
+
+    tsqueue_reset(&drv->urt_tx_queue);
+
+    drv->urt_rx_idx = 0;
+    drv->urt_rx_len = 0;
+
+    HAL_UART_Receive_IT(&drv->urt, &drv->urt_rx_byte, 1);
+  }
+}
+
+void HAL_UART_TxCpltCallback(UART_HandleTypeDef *urt) {
+  nrf_driver_t *drv = &g_nrf_driver;
+  if (drv->initialized && urt == &drv->urt) {
+    if (drv->urt_tx_msg_id >= 0 && (drv->urt_sending.callback != NULL)) {
+      drv->urt_sending.callback(NRF_STATUS_OK, drv->urt_sending.context);
+      drv->urt_tx_msg_id = -1;
+    }
+
+    bool msg =
+        tsqueue_dequeue(&drv->urt_tx_queue, (uint8_t *)&drv->urt_sending,
+                        sizeof(nrf_uart_tx_data_t), NULL, &drv->urt_tx_msg_id);
+    if (msg) {
+      HAL_UART_Transmit_DMA(&drv->urt, drv->urt_sending.data,
+                            drv->urt_sending.len);
+      drv->urt_tx_running = true;
+    } else {
+      drv->urt_tx_running = false;
+    }
+  }
+}
+
+void USART3_IRQHandler(void) {
+  IRQ_LOG_ENTER();
+
+  mpu_mode_t mpu_mode = mpu_reconfig(MPU_MODE_DEFAULT);
+
+  nrf_driver_t *drv = &g_nrf_driver;
+  if (drv->initialized) {
+    HAL_UART_IRQHandler(&drv->urt);
+  }
+
+  mpu_restore(mpu_mode);
+
+  IRQ_LOG_EXIT();
+}
+
+void GPDMA1_Channel1_IRQHandler(void) {
+  IRQ_LOG_ENTER();
+
+  mpu_mode_t mpu_mode = mpu_reconfig(MPU_MODE_DEFAULT);
+
+  nrf_driver_t *drv = &g_nrf_driver;
+  if (drv->initialized) {
+    HAL_DMA_IRQHandler(&drv->urt_tx_dma);
+  }
+
+  mpu_restore(mpu_mode);
+
+  IRQ_LOG_EXIT();
+}
+
+/// SPI communication
+/// ----------------------------------------------------------
+
+static bool start_spi_dma(nrf_driver_t *drv) {
+  if (!drv->comm_running) {
+    return false;
+  }
+
+  HAL_SPI_Receive_DMA(&drv->spi, drv->spi_buffer, SPI_PACKET_SIZE);
+  drv->spi_rx_running = true;
+
+  return true;
+}
+
+void GPDMA1_Channel2_IRQHandler(void) {
+  IRQ_LOG_ENTER();
+
+  mpu_mode_t mpu_mode = mpu_reconfig(MPU_MODE_DEFAULT);
+
+  nrf_driver_t *drv = &g_nrf_driver;
+  if (drv->initialized) {
+    HAL_DMA_IRQHandler(&drv->spi_dma);
+  }
+
+  mpu_restore(mpu_mode);
+
+  IRQ_LOG_EXIT();
+}
+
+void SPI1_IRQHandler(void) {
+  IRQ_LOG_ENTER();
+
+  mpu_mode_t mpu_mode = mpu_reconfig(MPU_MODE_DEFAULT);
+
+  nrf_driver_t *drv = &g_nrf_driver;
+  if (drv->initialized) {
+    HAL_SPI_IRQHandler(&drv->spi);
+  }
+
+  mpu_restore(mpu_mode);
+
+  IRQ_LOG_EXIT();
+}
+
+void HAL_SPI_RxCpltCallback(SPI_HandleTypeDef *hspi) {
+  nrf_driver_t *drv = &g_nrf_driver;
+
+  if (!drv->initialized) {
+    return;
+  }
+
+  if (hspi != &drv->spi) {
+    return;
+  }
+
+  spi_header_t *header = (spi_header_t *)drv->spi_buffer;
+  spi_footer_t *footer =
+      (spi_footer_t *)(drv->spi_buffer + SPI_PACKET_SIZE - SPI_FOOTER_SIZE);
+
+  uint8_t crc = crc8(drv->spi_buffer, SPI_PACKET_SIZE - SPI_FOOTER_SIZE, 0x07,
+                     0x00, false);
+
+  if ((header->service_id & 0xF0) != START_BYTE || footer->crc != crc) {
+    HAL_SPI_Abort(&drv->spi);
+    HAL_SPI_Receive_DMA(&drv->spi, drv->spi_buffer, SPI_PACKET_SIZE);
+    return;
+  }
+
+  nrf_process_msg(drv, drv->spi_buffer, SPI_PACKET_SIZE,
+                  header->service_id & 0x0F, SPI_HEADER_SIZE,
+                  SPI_OVERHEAD_SIZE);
+  drv->spi_rx_running = false;
+  start_spi_dma(drv);
+}
+
+/// GPIO communication
+/// ---------------------------------------------------------
+
+bool nrf_reboot_to_bootloader(void) {
+  // uint32_t tick_start = 0;
+
+  HAL_GPIO_WritePin(NRF_RESET_PORT, NRF_RESET_PIN, GPIO_PIN_SET);
+  // HAL_GPIO_WritePin(GPIOA, GPIO_PIN_1, GPIO_PIN_RESET);
+  //
+  // HAL_Delay(10);
+  // HAL_GPIO_WritePin(GPIOA, GPIO_PIN_1, GPIO_PIN_SET);
+  //
+  // tick_start = HAL_GetTick();
+  //
+  // while (HAL_GPIO_ReadPin(GPIO_1_PORT, GPIO_1_PIN) == GPIO_PIN_RESET) {
+  //   if (HAL_GetTick() - tick_start > 4000) {
+  //     return false;
+  //   }
+  // }
+
+  HAL_GPIO_WritePin(NRF_RESET_PORT, NRF_RESET_PIN, GPIO_PIN_RESET);
+
+  HAL_Delay(1000);
+
+  return true;
+}
+
+bool nrf_reboot(void) {
+  HAL_GPIO_WritePin(NRF_RESET_PORT, NRF_RESET_PIN, GPIO_PIN_SET);
+  HAL_Delay(50);
+  HAL_GPIO_WritePin(NRF_RESET_PORT, NRF_RESET_PIN, GPIO_PIN_RESET);
+  return true;
+}
+
+void nrf_signal_running(void) {
+  HAL_GPIO_WritePin(NRF_GPIO3_PORT, NRF_GPIO3_PIN, GPIO_PIN_SET);
+}
+
+void nrf_signal_off(void) {
+  HAL_GPIO_WritePin(NRF_GPIO3_PORT, NRF_GPIO3_PIN, GPIO_PIN_RESET);
+}
+
+bool nrf_firmware_running(void) {
+  return HAL_GPIO_ReadPin(NRF_GPIO0_PORT, NRF_GPIO0_PIN) != 0;
+}
+
+bool nrf_is_running(void) {
+  nrf_driver_t *drv = &g_nrf_driver;
+  if (!drv->initialized) {
+    return false;
+  }
+
+  if (!nrf_firmware_running()) {
+    return false;
+  }
+
+  return drv->comm_running;
+}
+
+void nrf_set_dfu_mode(void) {
+  nrf_driver_t *drv = &g_nrf_driver;
+
+  if (!drv->initialized) {
+    return;
+  }
+
+  // TODO
+  //  if (nrf_reboot_to_bootloader()) {
+  //    drv->mode_current = BLE_MODE_DFU;
+  //  } else {
+  //    drv->status_valid = false;
+  //  }
+}
+
+bool nrf_is_dfu_mode(void) {
+  nrf_driver_t *drv = &g_nrf_driver;
+
+  if (!drv->initialized) {
+    return false;
+  }
+
+  return true;
+  // TODO
+}
+
+#endif

core/embed/projects/kernel/main.c

@@ -42,6 +42,10 @@
 #include <io/button.h>
 #endif
 
+#ifdef USE_BLE
+#include <io/ble.h>
+#endif
+
 #ifdef USE_CONSUMPTION_MASK
 #include <sec/consumption_mask.h>
 #endif
@@ -177,6 +181,10 @@ void drivers_init() {
   haptic_init();
 #endif
 
+#ifdef USE_BLE
+  ble_init();
+#endif
+
 #ifdef USE_OPTIGA
 
 #if !PYOPT

core/embed/sys/syscall/stm32/syscall_dispatch.c

@@ -39,6 +39,10 @@
 #include <util/translations.h>
 #include <util/unit_properties.h>
 
+#ifdef USE_BLE
+#include <io/ble.h>
+#endif
+
 #ifdef USE_BUTTON
 #include <io/button.h>
 #endif
@@ -673,6 +677,47 @@ __attribute((no_stack_protector)) void syscall_handler(uint32_t *args,
           firmware_hash_callback_wrapper, callback_context);
     } break;
 
+#ifdef USE_BLE
+    case SYSCALL_BLE_START: {
+      ble_start();
+    } break;
+
+    case SYSCALL_BLE_ISSUE_COMMAND: {
+      ble_command_t command = args[0];
+      ble_issue_command(command);
+    } break;
+
+    case SYSCALL_BLE_GET_STATE: {
+      ble_state_t *state = (ble_state_t *)args[0];
+      ble_get_state__verified(state);
+    } break;
+
+    case SYSCALL_BLE_GET_EVENT: {
+      ble_event_t *event = (ble_event_t *)args[0];
+      args[0] = ble_get_event__verified(event);
+    } break;
+
+    case SYSCALL_BLE_CAN_WRITE: {
+      args[0] = ble_can_write();
+    } break;
+
+    case SYSCALL_BLE_WRITE: {
+      uint8_t *data = (uint8_t *)args[0];
+      size_t len = args[1];
+      args[0] = ble_write__verified(data, len);
+    } break;
+
+    case SYSCALL_BLE_CAN_READ: {
+      args[0] = ble_can_read();
+    } break;
+
+    case SYSCALL_BLE_READ: {
+      uint8_t *data = (uint8_t *)args[0];
+      size_t len = args[1];
+      args[0] = ble_read__verified(data, len);
+    } break;
+#endif
+
     default:
       args[0] = 0xffffffff;
       break;

core/embed/sys/syscall/stm32/syscall_numbers.h

@@ -139,6 +139,14 @@ typedef enum {
   SYSCALL_FIRMWARE_GET_VENDOR,
   SYSCALL_FIRMWARE_CALC_HASH,
 
+  SYSCALL_BLE_START,
+  SYSCALL_BLE_ISSUE_COMMAND,
+  SYSCALL_BLE_GET_EVENT,
+  SYSCALL_BLE_GET_STATE,
+  SYSCALL_BLE_CAN_WRITE,
+  SYSCALL_BLE_WRITE,
+  SYSCALL_BLE_CAN_READ,
+  SYSCALL_BLE_READ,
 } syscall_number_t;
 
 #endif  // SYSCALL_NUMBERS_H

core/embed/sys/syscall/stm32/syscall_stubs.c

@@ -645,4 +645,40 @@ secbool firmware_calc_hash(const uint8_t *challenge, size_t challenge_len,
                          SYSCALL_FIRMWARE_CALC_HASH);
 }
 
+#ifdef USE_BLE
+
+// =============================================================================
+// ble.h
+// =============================================================================
+
+#include <io/ble.h>
+
+void ble_start(void) { syscall_invoke0(SYSCALL_BLE_START); }
+
+bool ble_issue_command(ble_command_t command) {
+  return (bool)syscall_invoke1((uint32_t)command, SYSCALL_BLE_ISSUE_COMMAND);
+}
+
+bool ble_get_event(ble_event_t *event) {
+  return (bool)syscall_invoke1((uint32_t)event, SYSCALL_BLE_GET_EVENT);
+}
+
+void ble_get_state(ble_state_t *state) {
+  syscall_invoke1((uint32_t)state, SYSCALL_BLE_GET_STATE);
+}
+
+bool ble_can_write(void) { return syscall_invoke0(SYSCALL_BLE_CAN_WRITE); }
+
+bool ble_write(const uint8_t *data, uint16_t len) {
+  return syscall_invoke2((uint32_t)data, len, SYSCALL_BLE_WRITE);
+}
+
+bool ble_can_read(void) { return syscall_invoke0(SYSCALL_BLE_CAN_READ); }
+
+uint32_t ble_read(uint8_t *data, uint16_t len) {
+  return (uint32_t)syscall_invoke2((uint32_t)data, len, SYSCALL_BLE_READ);
+}
+
+#endif
+
 #endif  // KERNEL_MODE

core/embed/sys/syscall/stm32/syscall_verifiers.c

@@ -715,4 +715,56 @@ access_violation:
   return secfalse;
 }
 
+// ---------------------------------------------------------------------
+
+#ifdef USE_BLE
+void ble_get_state__verified(ble_state_t *state) {
+  if (!probe_write_access(state, sizeof(*state))) {
+    goto access_violation;
+  }
+
+  ble_get_state(state);
+  return;
+
+access_violation:
+  apptask_access_violation();
+}
+
+bool ble_get_event__verified(ble_event_t *event) {
+  if (!probe_write_access(event, sizeof(*event))) {
+    goto access_violation;
+  }
+
+  return ble_get_event(event);
+
+access_violation:
+  apptask_access_violation();
+  return false;
+}
+
+bool ble_write__verified(const uint8_t *data, size_t len) {
+  if (!probe_read_access(data, len)) {
+    goto access_violation;
+  }
+
+  return ble_write(data, len);
+
+access_violation:
+  apptask_access_violation();
+  return false;
+}
+
+uint32_t ble_read__verified(uint8_t *data, size_t len) {
+  if (!probe_write_access(data, len)) {
+    goto access_violation;
+  }
+
+  return ble_read(data, len);
+
+access_violation:
+  apptask_access_violation();
+  return 0;
+}
+#endif
+
 #endif  // SYSCALL_DISPATCH

core/embed/sys/syscall/stm32/syscall_verifiers.h

@@ -186,6 +186,21 @@ secbool firmware_calc_hash__verified(const uint8_t *challenge,
 
 secbool firmware_get_vendor__verified(char *buff, size_t buff_size);
 
+// ---------------------------------------------------------------------
+#ifdef USE_BLE
+
+#include <io/ble.h>
+
+void ble_get_state__verified(ble_state_t *state);
+
+bool ble_get_event__verified(ble_event_t *event);
+
+bool ble_write__verified(const uint8_t *data, size_t len);
+
+secbool ble_read__verified(uint8_t *data, size_t len);
+
+#endif
+
 #endif  // SYSCALL_DISPATCH
 
 #endif  // TREZORHAL_SYSCALL_VERIFIERS_H

core/embed/util/tsqueue/inc/util/tsqueue.h

@@ -0,0 +1,67 @@
+/*
+ * This file is part of the Trezor project, https://trezor.io/
+ *
+ * Copyright (c) SatoshiLabs
+ *
+ * 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 <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef TREZORHAL_TSQUEUE_H
+#define TREZORHAL_TSQUEUE_H
+
+#include <trezor_types.h>
+
+typedef struct {
+  uint8_t *buffer;  // Pointer to the data buffer
+  uint16_t len;     // Length of data in the buffer
+  int32_t id;       // ID of the entry
+  bool used;        // Used flag
+  bool aborted;     // Aborted flag
+} tsqueue_entry_t;
+
+typedef struct {
+  tsqueue_entry_t *entries;  // Array of queue entries
+  int rix;                   // Read index
+  int wix;                   // Write index
+  int qlen;                  // Queue length
+  uint16_t size;             // Size of each buffer
+  int32_t next_id;           // ID of the next item
+} tsqueue_t;
+
+// Initialize the queue
+void tsqueue_init(tsqueue_t *queue, tsqueue_entry_t *entries,
+                  uint8_t *buffer_mem, uint16_t size, int qlen);
+
+void tsqueue_reset(tsqueue_t *queue);
+
+// Insert data into the queue
+bool tsqueue_enqueue(tsqueue_t *queue, const uint8_t *data, uint16_t len,
+                     int32_t *id);
+
+// Read data from the queue
+bool tsqueue_dequeue(tsqueue_t *queue, uint8_t *data, uint16_t max_len,
+                     uint16_t *len, int32_t *id);
+
+// Checks if the queue is full
+bool tsqueue_full(tsqueue_t *queue);
+
+// Checks if the queue is empty
+bool tsqueue_empty(tsqueue_t *queue);
+
+// Aborts item in the queue
+// The space in the queue is not freed until the item is attempted to be read
+bool tsqueue_abort(tsqueue_t *queue, int32_t id, uint8_t *data,
+                   uint16_t max_len, uint16_t *len);
+
+#endif

core/embed/util/tsqueue/tsqueue.c

@@ -0,0 +1,192 @@
+/*
+ * This file is part of the Trezor project, https://trezor.io/
+ *
+ * Copyright (c) SatoshiLabs
+ *
+ * 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 <http://www.gnu.org/licenses/>.
+ */
+
+#include <trezor_rtl.h>
+
+#include <sys/irq.h>
+#include <util/tsqueue.h>
+
+// Initialize the queue
+void tsqueue_init(tsqueue_t *queue, tsqueue_entry_t *entries,
+                  uint8_t *buffer_mem, uint16_t size, int qlen) {
+  irq_key_t key = irq_lock();
+  queue->entries = entries;
+  queue->rix = 0;
+  queue->wix = 0;
+  queue->qlen = qlen;
+  queue->size = size;
+  queue->next_id = 1;
+
+  for (int i = 0; i < qlen; i++) {
+    if (buffer_mem != NULL) {
+      queue->entries[i].buffer = buffer_mem + i * size;
+      memset(queue->entries[i].buffer, 0, size);
+    }
+    queue->entries[i].len = 0;
+  }
+
+  irq_unlock(key);
+}
+
+static void tsqueue_entry_reset(tsqueue_entry_t *entry, uint32_t data_size) {
+  entry->len = 0;
+  entry->used = 0;
+  entry->aborted = false;
+  entry->id = 0;
+  memset(entry->buffer, 0, data_size);
+}
+
+void tsqueue_reset(tsqueue_t *queue) {
+  irq_key_t key = irq_lock();
+  queue->rix = 0;
+  queue->wix = 0;
+  queue->next_id = 1;
+
+  for (int i = 0; i < queue->qlen; i++) {
+    tsqueue_entry_reset(&queue->entries[i], queue->size);
+  }
+
+  irq_unlock(key);
+}
+
+static int32_t get_next_id(tsqueue_t *queue) {
+  int val = 1;
+  if (queue->next_id < INT32_MAX) {
+    val = queue->next_id;
+    queue->next_id++;
+  } else {
+    queue->next_id = 1;
+  }
+  return val;
+}
+
+bool tsqueue_enqueue(tsqueue_t *queue, const uint8_t *data, uint16_t len,
+                     int32_t *id) {
+  irq_key_t key = irq_lock();
+
+  if (queue->entries[queue->wix].used) {
+    // Full queue
+    irq_unlock(key);
+    return false;
+  }
+
+  if (len > queue->size) {
+    irq_unlock(key);
+    return false;
+  }
+
+  memcpy(queue->entries[queue->wix].buffer, data, len);
+  queue->entries[queue->wix].id = get_next_id(queue);
+  queue->entries[queue->wix].len = len;
+  queue->entries[queue->wix].used = true;
+
+  if (id != NULL) {
+    *id = queue->entries[queue->wix].id;
+  }
+  queue->wix = (queue->wix + 1) % queue->qlen;
+
+  irq_unlock(key);
+  return true;
+}
+
+static void tsqueue_discard_aborted(tsqueue_t *queue) {
+  while (queue->entries[queue->rix].aborted) {
+    tsqueue_entry_reset(&queue->entries[queue->rix], queue->size);
+    queue->rix = (queue->rix + 1) % queue->qlen;
+  }
+}
+
+bool tsqueue_dequeue(tsqueue_t *queue, uint8_t *data, uint16_t max_len,
+                     uint16_t *len, int32_t *id) {
+  irq_key_t key = irq_lock();
+
+  tsqueue_discard_aborted(queue);
+
+  if (!queue->entries[queue->rix].used) {
+    irq_unlock(key);
+    return false;
+  }
+
+  if (len != NULL) {
+    *len = queue->entries[queue->rix].len;
+  }
+
+  if (id != NULL) {
+    *id = queue->entries[queue->rix].id;
+  }
+
+  memcpy(data, queue->entries[queue->rix].buffer,
+         MIN(queue->entries[queue->rix].len, max_len));
+
+  tsqueue_entry_reset(queue->entries + queue->rix, queue->size);
+  queue->rix = (queue->rix + 1) % queue->qlen;
+
+  tsqueue_discard_aborted(queue);
+
+  irq_unlock(key);
+  return true;
+}
+
+// Check if the queue is full
+bool tsqueue_full(tsqueue_t *queue) {
+  irq_key_t key = irq_lock();
+
+  tsqueue_discard_aborted(queue);
+
+  bool full = queue->entries[queue->wix].used;
+  irq_unlock(key);
+  return full;
+}
+
+bool tsqueue_empty(tsqueue_t *queue) {
+  irq_key_t key = irq_lock();
+
+  tsqueue_discard_aborted(queue);
+
+  bool empty = !queue->entries[queue->rix].used;
+
+  irq_unlock(key);
+
+  return empty;
+}
+
+bool tsqueue_abort(tsqueue_t *queue, int32_t id, uint8_t *data,
+                   uint16_t max_len, uint16_t *len) {
+  bool found = false;
+  irq_key_t key = irq_lock();
+
+  for (int i = 0; i < queue->qlen; i++) {
+    if (queue->entries[i].used && queue->entries[i].id == id) {
+      queue->entries[i].aborted = true;
+      if (len != NULL) {
+        *len = queue->entries[i].len;
+      }
+
+      if (data != NULL) {
+        memcpy(data, queue->entries[i].buffer,
+               MIN(queue->entries[i].len, max_len));
+      }
+
+      found = true;
+    }
+  }
+
+  irq_unlock(key);
+  return found;
+}

core/site_scons/models/stm32f4_common.py

@@ -29,6 +29,7 @@ def stm32f4_common_files(env, defines, sources, paths):
         "embed/util/flash/inc",
         "embed/util/fwutils/inc",
         "embed/util/option_bytes/inc",
+        "embed/util/tsqueue/inc",
         "embed/util/unit_properties/inc",
         "vendor/micropython/lib/cmsis/inc",
         "vendor/micropython/lib/stm32lib/STM32F4xx_HAL_Driver/Inc",
@@ -87,6 +88,7 @@ def stm32f4_common_files(env, defines, sources, paths):
         "embed/util/flash/stm32f4/flash_otp.c",
         "embed/util/fwutils/fwutils.c",
         "embed/util/option_bytes/stm32f4/option_bytes.c",
+        "embed/util/tsqueue/tsqueue.c",
         "embed/util/unit_properties/stm32/unit_properties.c",
     ]
 

core/site_scons/models/stm32u5_common.py

@@ -32,6 +32,7 @@ def stm32u5_common_files(env, defines, sources, paths):
         "embed/util/flash/inc",
         "embed/util/fwutils/inc",
         "embed/util/option_bytes/inc",
+        "embed/util/tsqueue/inc",
         "embed/util/unit_properties/inc",
         "vendor/stm32u5xx_hal_driver/Inc",
         "vendor/cmsis_device_u5/Include",
@@ -106,6 +107,7 @@ def stm32u5_common_files(env, defines, sources, paths):
         "embed/util/flash/stm32u5/flash_otp.c",
         "embed/util/fwutils/fwutils.c",
         "embed/util/option_bytes/stm32u5/option_bytes.c",
+        "embed/util/tsqueue/tsqueue.c",
         "embed/util/unit_properties/stm32/unit_properties.c",
     ]