diff --git a/Bluetooth.h b/Bluetooth.h
index 6329964..4761e24 100644
--- a/Bluetooth.h
+++ b/Bluetooth.h
@@ -307,6 +307,11 @@ bool bt_passkey_callback(uint16_t conn_handle, uint8_t const passkey[6], bool ma
 void bt_connect_callback(uint16_t conn_handle) {
     bt_state = BT_STATE_CONNECTED;
     cable_state = CABLE_STATE_DISCONNECTED;
+
+    BLEConnection* conn = Bluefruit.Connection(conn_handle);
+    conn->requestPHY(BLE_GAP_PHY_2MBPS);
+    conn->requestMtuExchange(512+3);
+    conn->requestDataLengthUpdate();
 }
 
 void bt_disconnect_callback(uint16_t conn_handle, uint8_t reason) {
@@ -342,6 +347,8 @@ bool bt_setup_hw() {
       Bluefruit.Security.setSecuredCallback(bt_connect_callback);
       Bluefruit.Periph.setDisconnectCallback(bt_disconnect_callback);
       Bluefruit.Security.setPairCompleteCallback(bt_pairing_complete);
+      Bluefruit.Periph.setConnInterval(6, 12); // 7.5 - 15 ms
+
       const ble_gap_addr_t gap_addr = Bluefruit.getAddr();
       char *data = (char*)malloc(BT_DEV_ADDR_LEN+1);
       for (int i = 0; i < BT_DEV_ADDR_LEN; i++) {
@@ -372,6 +379,9 @@ void bt_start() {
     // start device information service
     bledis.begin();
 
+    SerialBT.bufferTXD(true); // enable buffering
+
+    SerialBT.setPermission(SECMODE_ENC_WITH_MITM, SECMODE_ENC_WITH_MITM); // enable encryption for BLE serial
     SerialBT.begin();
 
     blebas.begin();
diff --git a/Config.h b/Config.h
index cef991f..6f1c2a8 100644
--- a/Config.h
+++ b/Config.h
@@ -28,6 +28,8 @@
 	#define CABLE_STATE_DISCONNECTED 0x00
 	#define CABLE_STATE_CONNECTED    0x01
 	uint8_t cable_state = CABLE_STATE_DISCONNECTED;
+
+    #define MAX_INTERFACES 12
 	
 	#define BT_STATE_NA        0xff
 	#define BT_STATE_OFF       0x00
@@ -88,6 +90,12 @@
 	uint8_t seq				= 0xFF;
 	uint16_t read_len		= 0;
 
+    bool serial_in_frame = false;
+
+    FIFOBuffer packet_rdy_interfaces;
+
+    uint8_t packet_rdy_interfaces_buf[MAX_INTERFACES];
+
 	// Incoming packet buffer
 	uint8_t pbuf[MTU];
 
diff --git a/Documentation/BUILDING.md b/Documentation/BUILDING.md
index 6e90f8e..d995d31 100644
--- a/Documentation/BUILDING.md
+++ b/Documentation/BUILDING.md
@@ -54,6 +54,60 @@ Ensure you replace [target] with the target you selected. For example:
 
 `make upload-rak4631`
 
+If you are flashing a custom board, you will need to generate a signing key in rnodeconf prior to flashing if you do not already have one by running:
+
+`rnodeconf -k`
+
+After flashing a custom board, you will also need to provision the EEPROM before use:
+
+`rnodeconf /dev/ttyACM0 -r --platform ESP32 --model a9 --product f0 --hwrev 3`
+
+- platform must either be AVR, ESP32 or NRF52
+- hwrev is required (any integer between 1 and 255)
+- model should be something from the list below without the leading `0x` and in lowercase (example `e8`):
+```
+0x11: [430000000, 510000000, 22, "430 - 510 MHz", "rnode_firmware_rak4631.zip", "SX1262"],
+0x12: [779000000, 928000000, 22, "779 - 928 MHz", "rnode_firmware_rak4631.zip", "SX1262"],
+0xA4: [410000000, 525000000, 14, "410 - 525 MHz", "rnode_firmware.hex", "SX1278"],
+0xA9: [820000000, 1020000000, 17, "820 - 1020 MHz", "rnode_firmware.hex", "SX1276"],
+0xA1: [410000000, 525000000, 22, "410 - 525 MHz", "rnode_firmware_t3s3.zip", "SX1268"],
+0xA6: [820000000, 1020000000, 22, "820 - 960 MHz", "rnode_firmware_t3s3.zip", "SX1262"],
+0xA2: [410000000, 525000000, 17, "410 - 525 MHz", "rnode_firmware_ng21.zip", "SX1278"],
+0xA7: [820000000, 1020000000, 17, "820 - 1020 MHz", "rnode_firmware_ng21.zip", "SX1276"],
+0xA3: [410000000, 525000000, 17, "410 - 525 MHz", "rnode_firmware_ng20.zip", "SX1278"],
+0xA8: [820000000, 1020000000, 17, "820 - 1020 MHz", "rnode_firmware_ng20.zip", "SX1276"],
+0xB3: [420000000, 520000000, 17, "420 - 520 MHz", "rnode_firmware_lora32v20.zip", "SX1278"],
+0xB8: [850000000, 950000000, 17, "850 - 950 MHz", "rnode_firmware_lora32v20.zip", "SX1276"],
+0xB4: [420000000, 520000000, 17, "420 - 520 MHz", "rnode_firmware_lora32v21.zip", "SX1278"],
+0xB9: [850000000, 950000000, 17, "850 - 950 MHz", "rnode_firmware_lora32v21.zip", "SX1276"],
+0x04: [420000000, 520000000, 17, "420 - 520 MHz", "rnode_firmware_lora32v21_tcxo.zip", "SX1278"],
+0x09: [850000000, 950000000, 17, "850 - 950 MHz", "rnode_firmware_lora32v21_tcxo.zip", "SX1276"],
+0xBA: [420000000, 520000000, 17, "420 - 520 MHz", "rnode_firmware_lora32v10.zip", "SX1278"],
+0xBB: [850000000, 950000000, 17, "850 - 950 MHz", "rnode_firmware_lora32v10.zip", "SX1276"],
+0xC4: [420000000, 520000000, 17, "420 - 520 MHz", "rnode_firmware_heltec32v2.zip", "SX1278"],
+0xC9: [850000000, 950000000, 17, "850 - 950 MHz", "rnode_firmware_heltec32v2.zip", "SX1276"],
+0xC5: [470000000, 510000000, 21, "470 - 510 MHz", "rnode_firmware_heltec32v3.zip", "SX1262"],
+0xCA: [863000000, 928000000, 21, "863 - 928 MHz", "rnode_firmware_heltec32v3.zip", "SX1262"],
+0xE4: [420000000, 520000000, 17, "420 - 520 MHz", "rnode_firmware_tbeam.zip", "SX1278"],
+0xE9: [850000000, 950000000, 17, "850 - 950 MHz", "rnode_firmware_tbeam.zip", "SX1276"],
+0xE3: [420000000, 520000000, 22, "420 - 520 MHz", "rnode_firmware_tbeam_sx1262.zip", "SX1268"],
+0xE8: [850000000, 950000000, 22, "850 - 950 MHz", "rnode_firmware_tbeam_sx1262.zip", "SX1262"],
+0xFE: [100000000, 1100000000, 17, "(Band capabilities unknown)", None, "Unknown"],
+0xFF: [100000000, 1100000000, 14, "(Band capabilities unknown)", None, "Unknown"],
+```
+- product should be a code from the following list below without the leading `0x` and in lowercase (example `f0`):
+```
+PRODUCT_RAK4631 = 0x10
+PRODUCT_RNODE  = 0x03
+PRODUCT_T32_10 = 0xB2
+PRODUCT_T32_20 = 0xB0
+PRODUCT_T32_21 = 0xB1
+PRODUCT_H32_V2 = 0xC0
+PRODUCT_H32_V3 = 0xC1
+PRODUCT_TBEAM  = 0xE0
+PRODUCT_HMBRW  = 0xF0
+```
+
 **Please note**, you must re-compile the firmware each time you make changes **before** you flash it, else you will just be flashing the previous version of the firmware without the new changes!
 
 These commands can also be run as a one liner. For example:
diff --git a/Makefile b/Makefile
index 56fb9ba..d5c1bfe 100644
--- a/Makefile
+++ b/Makefile
@@ -54,7 +54,7 @@ spiffs-image:
 
 upload-spiffs:
 	@echo Deploying SPIFFS image...
-	python ./Release/esptool/esptool.py --chip esp32 --port /dev/ttyACM0 --baud 921600 --before default_reset --after hard_reset write_flash -z --flash_mode dio --flash_freq 80m --flash_size 4MB 0x210000 ./Release/console_image.bin
+	python ./Release/esptool/esptool.py --chip esp32 --port $(or $(port), /dev/ttyACM0) --baud 921600 --before default_reset --after hard_reset write_flash -z --flash_mode dio --flash_freq 80m --flash_size 4MB 0x210000 ./Release/console_image.bin
 
 firmware-tbeam:
 	arduino-cli compile --fqbn esp32:esp32:t-beam -e --build-property "build.partitions=no_ota" --build-property "upload.maximum_size=2097152" --build-property "compiler.cpp.extra_flags=\"-DBOARD_MODEL=0x33\""
@@ -117,85 +117,85 @@ firmware-freenode:
 	arduino-cli compile --fqbn rakwireless:nrf52:WisCoreRAK4631Board -e --build-property "build.partitions=no_ota" --build-property "upload.maximum_size=2097152" --build-property "compiler.cpp.extra_flags=\"-DBOARD_MODEL=0x52\" \"-DBOARD_VARIANT=0x21\""
 
 upload-tbeam:
-	arduino-cli upload -p /dev/ttyACM0 --fqbn esp32:esp32:t-beam
+	arduino-cli upload -p $(or $(port), /dev/ttyACM0) --fqbn esp32:esp32:t-beam
 	@sleep 1
-	rnodeconf /dev/ttyACM0 --firmware-hash $$(./partition_hashes ./build/esp32.esp32.t-beam/opencom_xl_firmware.ino.bin)
+	rnodeconf $(or $(port), /dev/ttyACM0) --firmware-hash $$(./partition_hashes ./build/esp32.esp32.t-beam/opencom_xl_firmware.ino.bin)
 	@sleep 3
-	python ./Release/esptool/esptool.py --chip esp32 --port /dev/ttyACM0 --baud 921600 --before default_reset --after hard_reset write_flash -z --flash_mode dio --flash_freq 80m --flash_size 4MB 0x210000 ./Release/console_image.bin
+	python ./Release/esptool/esptool.py --chip esp32 --port $(or $(port), /dev/ttyACM0) --baud 921600 --before default_reset --after hard_reset write_flash -z --flash_mode dio --flash_freq 80m --flash_size 4MB 0x210000 ./Release/console_image.bin
 
 upload-lora32_v10:
-	arduino-cli upload -p /dev/ttyUSB0 --fqbn esp32:esp32:ttgo-lora32
+	arduino-cli upload -p $(or $(port), /dev/ttyUSB0) --fqbn esp32:esp32:ttgo-lora32
 	@sleep 1
-	rnodeconf /dev/ttyUSB0 --firmware-hash $$(./partition_hashes ./build/esp32.esp32.ttgo-lora32/opencom_xl_firmware.ino.bin)
+	rnodeconf $(or $(port), /dev/ttyUSB0) --firmware-hash $$(./partition_hashes ./build/esp32.esp32.ttgo-lora32/opencom_xl_firmware.ino.bin)
 	@sleep 3
-	python ./Release/esptool/esptool.py --chip esp32 --port /dev/ttyUSB0 --baud 921600 --before default_reset --after hard_reset write_flash -z --flash_mode dio --flash_freq 80m --flash_size 4MB 0x210000 ./Release/console_image.bin
+	python ./Release/esptool/esptool.py --chip esp32 --port $(or $(port), /dev/ttyUSB0) --baud 921600 --before default_reset --after hard_reset write_flash -z --flash_mode dio --flash_freq 80m --flash_size 4MB 0x210000 ./Release/console_image.bin
 
 upload-lora32_v20:
-	arduino-cli upload -p /dev/ttyUSB0 --fqbn esp32:esp32:ttgo-lora32
+	arduino-cli upload -p $(or $(port), /dev/ttyUSB0) --fqbn esp32:esp32:ttgo-lora32
 	@sleep 1
-	rnodeconf /dev/ttyUSB0 --firmware-hash $$(./partition_hashes ./build/esp32.esp32.ttgo-lora32/opencom_xl_firmware.ino.bin)
+	rnodeconf $(or $(port), /dev/ttyUSB0) --firmware-hash $$(./partition_hashes ./build/esp32.esp32.ttgo-lora32/opencom_xl_firmware.ino.bin)
 	@sleep 3
-	python ./Release/esptool/esptool.py --chip esp32 --port /dev/ttyUSB0 --baud 921600 --before default_reset --after hard_reset write_flash -z --flash_mode dio --flash_freq 80m --flash_size 4MB 0x210000 ./Release/console_image.bin
+	python ./Release/esptool/esptool.py --chip esp32 --port $(or $(port), /dev/ttyUSB0) --baud 921600 --before default_reset --after hard_reset write_flash -z --flash_mode dio --flash_freq 80m --flash_size 4MB 0x210000 ./Release/console_image.bin
 
 upload-lora32_v21:
-	arduino-cli upload -p /dev/ttyACM0 --fqbn esp32:esp32:ttgo-lora32
+	arduino-cli upload -p $(or $(port), /dev/ttyACM0) --fqbn esp32:esp32:ttgo-lora32
 	@sleep 1
-	rnodeconf /dev/ttyACM0 --firmware-hash $$(./partition_hashes ./build/esp32.esp32.ttgo-lora32/opencom_xl_firmware.ino.bin)
+	rnodeconf $(or $(port), /dev/ttyACM0) --firmware-hash $$(./partition_hashes ./build/esp32.esp32.ttgo-lora32/opencom_xl_firmware.ino.bin)
 	@sleep 3
-	python ./Release/esptool/esptool.py --chip esp32 --port /dev/ttyACM0 --baud 921600 --before default_reset --after hard_reset write_flash -z --flash_mode dio --flash_freq 80m --flash_size 4MB 0x210000 ./Release/console_image.bin
+	python ./Release/esptool/esptool.py --chip esp32 --port $(or $(port), /dev/ttyACM0) --baud 921600 --before default_reset --after hard_reset write_flash -z --flash_mode dio --flash_freq 80m --flash_size 4MB 0x210000 ./Release/console_image.bin
 
 upload-heltec32_v2:
-	arduino-cli upload -p /dev/ttyUSB1 --fqbn esp32:esp32:heltec_wifi_lora_32_V2
+	arduino-cli upload -p $(or $(port), /dev/ttyUSB0) --fqbn esp32:esp32:heltec_wifi_lora_32_V2
 	@sleep 1
-	rnodeconf /dev/ttyUSB1 --firmware-hash $$(./partition_hashes ./build/esp32.esp32.heltec_wifi_lora_32_V2/opencom_xl_firmware.ino.bin)
+	rnodeconf $(or $(port), /dev/ttyUSB0) --firmware-hash $$(./partition_hashes ./build/esp32.esp32.heltec_wifi_lora_32_V2/opencom_xl_firmware.ino.bin)
 	@sleep 3
-	python ./Release/esptool/esptool.py --chip esp32 --port /dev/ttyUSB1 --baud 921600 --before default_reset --after hard_reset write_flash -z --flash_mode dio --flash_freq 80m --flash_size 4MB 0x210000 ./Release/console_image.bin
+	python ./Release/esptool/esptool.py --chip esp32 --port $(or $(port), /dev/ttyUSB0) --baud 921600 --before default_reset --after hard_reset write_flash -z --flash_mode dio --flash_freq 80m --flash_size 4MB 0x210000 ./Release/console_image.bin
 
 upload-heltec32_v3:
-	arduino-cli upload -p /dev/ttyUSB1 --fqbn esp32:esp32:heltec_wifi_lora_32_V3
+	arduino-cli upload -p $(or $(port), /dev/ttyUSB0) --fqbn esp32:esp32:heltec_wifi_lora_32_V3
 	@sleep 1
-	rnodeconf /dev/ttyUSB1 --firmware-hash $$(./partition_hashes ./build/esp32.esp32.heltec_wifi_lora_32_V3/opencom_xl_firmware.ino.bin)
+	rnodeconf $(or $(port), /dev/ttyUSB0) --firmware-hash $$(./partition_hashes ./build/esp32.esp32.heltec_wifi_lora_32_V3/opencom_xl_firmware.ino.bin)
 	@sleep 3
-	python ./Release/esptool/esptool.py --chip esp32-s3 --port /dev/ttyUSB1 --baud 921600 --before default_reset --after hard_reset write_flash -z --flash_mode dio --flash_freq 80m --flash_size 4MB 0x210000 ./Release/console_image.bin
+	python ./Release/esptool/esptool.py --chip esp32-s3 --port $(or $(port), /dev/ttyUSB0) --baud 921600 --before default_reset --after hard_reset write_flash -z --flash_mode dio --flash_freq 80m --flash_size 4MB 0x210000 ./Release/console_image.bin
 
 upload-rnode_ng_20:
-	arduino-cli upload -p /dev/ttyUSB0 --fqbn esp32:esp32:ttgo-lora32
+	arduino-cli upload -p $(or $(port), /dev/ttyUSB0) --fqbn esp32:esp32:ttgo-lora32
 	@sleep 1
-	rnodeconf /dev/ttyUSB0 --firmware-hash $$(./partition_hashes ./build/esp32.esp32.ttgo-lora32/opencom_xl_firmware.ino.bin)
+	rnodeconf $(or $(port), /dev/ttyUSB0) --firmware-hash $$(./partition_hashes ./build/esp32.esp32.ttgo-lora32/opencom_xl_firmware.ino.bin)
 	@sleep 3
-	python ./Release/esptool/esptool.py --chip esp32 --port /dev/ttyUSB0 --baud 921600 --before default_reset --after hard_reset write_flash -z --flash_mode dio --flash_freq 80m --flash_size 4MB 0x210000 ./Release/console_image.bin
+	python ./Release/esptool/esptool.py --chip esp32 --port $(or $(port), /dev/ttyUSB0) --baud 921600 --before default_reset --after hard_reset write_flash -z --flash_mode dio --flash_freq 80m --flash_size 4MB 0x210000 ./Release/console_image.bin
 
 upload-rnode_ng_21:
-	arduino-cli upload -p /dev/ttyACM0 --fqbn esp32:esp32:ttgo-lora32
+	arduino-cli upload -p $(or $(port), /dev/ttyACM0) --fqbn esp32:esp32:ttgo-lora32
 	@sleep 1
-	rnodeconf /dev/ttyACM0 --firmware-hash $$(./partition_hashes ./build/esp32.esp32.ttgo-lora32/opencom_xl_firmware.ino.bin)
+	rnodeconf $(or $(port), /dev/ttyACM0) --firmware-hash $$(./partition_hashes ./build/esp32.esp32.ttgo-lora32/opencom_xl_firmware.ino.bin)
 	@sleep 3
-	python ./Release/esptool/esptool.py --chip esp32 --port /dev/ttyACM0 --baud 921600 --before default_reset --after hard_reset write_flash -z --flash_mode dio --flash_freq 80m --flash_size 4MB 0x210000 ./Release/console_image.bin
+	python ./Release/esptool/esptool.py --chip esp32 --port $(or $(port), /dev/ttyACM0) --baud 921600 --before default_reset --after hard_reset write_flash -z --flash_mode dio --flash_freq 80m --flash_size 4MB 0x210000 ./Release/console_image.bin
 
 upload-t3s3:
 	@echo
 	@echo Put board into flashing mode by holding BOOT button while momentarily pressing the RESET button. Hit enter when done.
 	@read
-	arduino-cli upload -p /dev/ttyACM0 --fqbn esp32:esp32:esp32s3
+	arduino-cli upload -p $(or $(port), /dev/ttyACM0) --fqbn esp32:esp32:esp32s3
 	@sleep 2
-	python ./Release/esptool/esptool.py --chip esp32s3 --port /dev/ttyACM0 --baud 921600 --before default_reset --after hard_reset write_flash -z --flash_mode dio --flash_freq 80m --flash_size 4MB 0x210000 ./Release/console_image.bin
+	python ./Release/esptool/esptool.py --chip esp32s3 --port $(or $(port), /dev/ttyACM0) --baud 921600 --before default_reset --after hard_reset write_flash -z --flash_mode dio --flash_freq 80m --flash_size 4MB 0x210000 ./Release/console_image.bin
 	@echo
 	@echo Press the RESET button on the board now, and hit enter
 	@read
 	@sleep 1
-	rnodeconf /dev/ttyACM0 --firmware-hash $$(./partition_hashes ./build/esp32.esp32.esp32s3/opencom_xl_firmware.ino.bin)
+	rnodeconf $(or $(port), /dev/ttyACM0) --firmware-hash $$(./partition_hashes ./build/esp32.esp32.esp32s3/opencom_xl_firmware.ino.bin)
 
 upload-featheresp32:
-	arduino-cli upload -p /dev/ttyUSB0 --fqbn esp32:esp32:featheresp32
+	arduino-cli upload -p $(or $(port), /dev/ttyUSB0) --fqbn esp32:esp32:featheresp32
 	@sleep 1
-	rnodeconf /dev/ttyUSB0 --firmware-hash $$(./partition_hashes ./build/esp32.esp32.featheresp32/opencom_xl_firmware.ino.bin)
+	rnodeconf $(or $(port), /dev/ttyUSB0) --firmware-hash $$(./partition_hashes ./build/esp32.esp32.featheresp32/opencom_xl_firmware.ino.bin)
 	@sleep 3
-	python ./Release/esptool/esptool.py --chip esp32 --port /dev/ttyUSB0 --baud 921600 --before default_reset --after hard_reset write_flash -z --flash_mode dio --flash_freq 80m --flash_size 4MB 0x210000 ./Release/console_image.bin
+	python ./Release/esptool/esptool.py --chip esp32 --port $(or $(port), /dev/ttyUSB0) --baud 921600 --before default_reset --after hard_reset write_flash -z --flash_mode dio --flash_freq 80m --flash_size 4MB 0x210000 ./Release/console_image.bin
 
 upload-freenode:
 	arduino-cli upload -p /dev/ttyACM0 --fqbn rakwireless:nrf52:WisCoreRAK4631Board
 	unzip -o build/rakwireless.nrf52.WisCoreRAK4631Board/opencom_xl_firmware.ino.zip -d build/rakwireless.nrf52.WisCoreRAK4631Board
-	rnodeconf /dev/ttyACM0 --firmware-hash $$(sha256sum ./build/rakwireless.nrf52.WisCoreRAK4631Board/opencom_xl_firmware.ino.bin | grep -o '^\S*')
+	rnodeconf $(or $(port), /dev/ttyACM0) --firmware-hash $$(sha256sum ./build/rakwireless.nrf52.WisCoreRAK4631Board/opencom_xl_firmware.ino.bin | grep -o '^\S*')
 	@echo
 	@echo This target currently uses a custom version of rnodeconf to set the firmware length on the device.
 	@echo This will be removed once the feature has been included upstream, or another solution has been found.
@@ -370,11 +370,11 @@ release-freenode:
 
 release-rak4631:
 	arduino-cli compile --fqbn rakwireless:nrf52:WisCoreRAK4631Board -e --build-property "build.partitions=no_ota" --build-property "upload.maximum_size=2097152" --build-property "compiler.cpp.extra_flags=\"-DBOARD_MODEL=0x51\" \"-DBOARD_VARIANT=0x12\""
-	cp build/rakwireless.nrf52.WisCoreRAK4631Board/RNode_Firmware_CE.ino.hex build/rnode_firmware_rak4631.hex
+	cp build/rakwireless.nrf52.WisCoreRAK4631Board/opencom_xl_firmware.ino.hex build/rnode_firmware_rak4631.hex
 	adafruit-nrfutil dfu genpkg --dev-type 0x0052 --application build/rnode_firmware_rak4631.hex Release/rnode_firmware_rak4631.zip
 
 release-rak4631_sx1280:
 	arduino-cli compile --fqbn rakwireless:nrf52:WisCoreRAK4631Board -e --build-property "build.partitions=no_ota" --build-property "upload.maximum_size=2097152" --build-property "compiler.cpp.extra_flags=\"-DBOARD_MODEL=0x51\" \"-DBOARD_VARIANT=0x14\""
-	cp build/rakwireless.nrf52.WisCoreRAK4631Board/RNode_Firmware_CE.ino.hex build/rnode_firmware_rak4631_sx1280.hex
+	cp build/rakwireless.nrf52.WisCoreRAK4631Board/opencom_xl_firmware.ino.hex build/rnode_firmware_rak4631_sx1280.hex
 	adafruit-nrfutil dfu genpkg --dev-type 0x0052 --application build/rnode_firmware_rak4631_sx1280.hex Release/rnode_firmware_rak4631_sx1280.zip
 
diff --git a/Power.h b/Power.h
index 02aa41d..2fa7ac7 100644
--- a/Power.h
+++ b/Power.h
@@ -273,6 +273,16 @@ void measure_battery() {
     if (battery_percent >= 98) {
         battery_state = BATTERY_STATE_CHARGED;
     }
+
+    #if HAS_BLE
+    if ((bt_state == BT_STATE_ON) || bt_state == BT_STATE_CONNECTED) {
+        if (battery_state != BATTERY_STATE_CHARGING) {
+            blebas.write(battery_percent);
+        } else {
+            blebas.write(100);
+        }
+    }
+    #endif
   #endif
 
   if (battery_ready) {
diff --git a/Radio.cpp b/Radio.cpp
index 36cece6..cfd8d59 100644
--- a/Radio.cpp
+++ b/Radio.cpp
@@ -88,8 +88,7 @@
 #define FREQ_DIV_6X (double)pow(2.0, 25.0)
 #define FREQ_STEP_6X (double)(XTAL_FREQ_6X / FREQ_DIV_6X)
 
-extern bool process_packet;
-extern uint8_t packet_interface;
+extern FIFOBuffer packet_rdy_interfaces;
 extern RadioInterface* interface_obj[];
 
 // ISRs cannot provide parameters to the functions they call. Since we have
@@ -99,9 +98,17 @@ extern RadioInterface* interface_obj[];
 void ISR_VECT onDio0Rise() {
     for (int i = 0; i < INTERFACE_COUNT; i++) {
         if (digitalRead(interface_pins[i][5]) == HIGH) {
-            process_packet = true;
-            packet_interface = i;
-            break;
+            if (interface_obj[i]->getPacketValidity()) {
+                fifo_push(&packet_rdy_interfaces, i);
+            }
+            if (interfaces[i] == SX128X) {
+                // On the SX1280, there is a bug which can cause the busy line
+                // to remain high if a high amount of packets are received when
+                // in continuous RX mode. This is documented as Errata 16.1 in
+                // the SX1280 datasheet v3.2 (page 149)
+                // Therefore, the modem is set into receive mode each time a packet is received.
+                interface_obj[i]->receive();
+            }
         }
     }
 }
@@ -965,33 +972,17 @@ void sx126x::implicitHeaderMode()
 
 void sx126x::handleDio0Rise()
 {
-    uint8_t buf[2];
+    // received a packet
+    _packetIndex = 0;
 
-    buf[0] = 0x00;
-    buf[1] = 0x00;
+    // read packet length
+    uint8_t rxbuf[2] = {0};
+    executeOpcodeRead(OP_RX_BUFFER_STATUS_6X, rxbuf, 2);
+    int packetLength = rxbuf[0];
 
-    executeOpcodeRead(OP_GET_IRQ_STATUS_6X, buf, 2);
-
-    executeOpcode(OP_CLEAR_IRQ_STATUS_6X, buf, 2);
-
-    if ((buf[1] & IRQ_PAYLOAD_CRC_ERROR_MASK_6X) == 0) {
-        // received a packet
-        _packetIndex = 0;
-
-        // read packet length
-        uint8_t rxbuf[2] = {0};
-        executeOpcodeRead(OP_RX_BUFFER_STATUS_6X, rxbuf, 2);
-        int packetLength = rxbuf[0];
-
-        if (_onReceive) {
-            _onReceive(_index, packetLength);
-        }
+    if (_onReceive) {
+        _onReceive(_index, packetLength);
     }
-    // else {
-    //   Serial.println("CRCE");
-    //   Serial.println(buf[0]);
-    //   Serial.println(buf[1]);
-    // }
 }
 
 void sx126x::updateBitrate() {
@@ -1014,7 +1005,7 @@ void sx126x::updateBitrate() {
     }
 }
 
-void sx126x::clearIRQStatus() {
+bool ISR_VECT sx126x::getPacketValidity() {
     uint8_t buf[2];
 
     buf[0] = 0x00;
@@ -1023,6 +1014,12 @@ void sx126x::clearIRQStatus() {
     executeOpcodeRead(OP_GET_IRQ_STATUS_6X, buf, 2);
 
     executeOpcode(OP_CLEAR_IRQ_STATUS_6X, buf, 2);
+
+    if ((buf[1] & IRQ_PAYLOAD_CRC_ERROR_MASK_6X) == 0) {
+        return true;
+    } else {
+        return false;
+    }
 }
 // SX127x registers
 #define REG_FIFO_7X                   0x00
@@ -1515,11 +1512,6 @@ void sx127x::optimizeModemSensitivity() {
 }
 
 void sx127x::handleDio0Rise() {
-  int irqFlags = readRegister(REG_IRQ_FLAGS_7X);
-
-  // Clear IRQs
-  writeRegister(REG_IRQ_FLAGS_7X, irqFlags);
-  if ((irqFlags & IRQ_PAYLOAD_CRC_ERROR_MASK_7X) == 0) {
     _packetIndex = 0;
     int packetLength = _implicitHeaderMode ? readRegister(REG_PAYLOAD_LENGTH_7X) : readRegister(REG_RX_NB_BYTES_7X);
     writeRegister(REG_FIFO_ADDR_PTR_7X, readRegister(REG_FIFO_RX_CURRENT_ADDR_7X));
@@ -1527,7 +1519,6 @@ void sx127x::handleDio0Rise() {
         _onReceive(_index, packetLength); 
     }
     writeRegister(REG_FIFO_ADDR_PTR_7X, 0);
-  }
 }
 
 void sx127x::updateBitrate() {
@@ -1549,11 +1540,17 @@ void sx127x::updateBitrate() {
     }
 }
 
-void sx127x::clearIRQStatus() {
+bool ISR_VECT sx127x::getPacketValidity() {
   int irqFlags = readRegister(REG_IRQ_FLAGS_7X);
 
   // Clear IRQs
   writeRegister(REG_IRQ_FLAGS_7X, irqFlags);
+
+  if ((irqFlags & IRQ_PAYLOAD_CRC_ERROR_MASK_7X) == 0) {
+      return true;
+  } else {
+      return false;
+  }
 }
 
 // SX128x registers
@@ -2043,7 +2040,7 @@ uint8_t ISR_VECT sx128x::packetSnrRaw() {
 
 float ISR_VECT sx128x::packetSnr() {
     uint8_t buf[5] = {0};
-    executeOpcodeRead(OP_PACKET_STATUS_8X, buf, 3);
+    executeOpcodeRead(OP_PACKET_STATUS_8X, buf, 5);
     return float(buf[1]) * 0.25;
 }
 
@@ -2079,13 +2076,34 @@ int ISR_VECT sx128x::available()
 
 int ISR_VECT sx128x::read()
 {
-  if (!available()) {
-    return -1;
-  }
+    if (!available()) {
+        return -1;
+    }
 
-  uint8_t byte = _packet[_packetIndex];
-  _packetIndex++;
-  return byte;
+    // if received new packet
+    if (_packetIndex == 0) {
+        uint8_t rxbuf[2] = {0};
+        executeOpcodeRead(OP_RX_BUFFER_STATUS_8X, rxbuf, 2);
+        int size;
+        // If implicit header mode is enabled, read packet length as payload length instead.
+        // See SX1280 datasheet v3.2, page 92
+        if (_implicitHeaderMode == 0x80) {
+            size = _payloadLength;
+        } else {
+            size = rxbuf[0];
+        }
+        _fifo_rx_addr_ptr = rxbuf[1];
+
+        if (size > 255) {
+            size = 255;
+        }
+
+        readBuffer(_packet, size);
+    }
+
+    uint8_t byte = _packet[_packetIndex];
+    _packetIndex++;
+    return byte;
 }
 
 int sx128x::peek()
@@ -2116,9 +2134,16 @@ void sx128x::onReceive(void(*callback)(uint8_t, int))
       buf[0] = 0xFF; 
       buf[1] = 0xFF;
 
+      // On the SX1280, no RxDone IRQ is generated if a packet is received with
+      // an invalid header, but the modem will be taken out of single RX mode.
+      // This can cause the modem to not receive packets until it is reset
+      // again. This is documented as Errata 16.2 in the SX1280 datasheet v3.2
+      // (page 150) Below, the header error IRQ is mapped to dio0 so that the
+      // modem can be set into RX mode again on reception of a corrupted
+      // header.
       // set dio0 masks
       buf[2] = 0x00;
-      buf[3] = IRQ_RX_DONE_MASK_8X; 
+      buf[3] = IRQ_RX_DONE_MASK_8X | IRQ_HEADER_ERROR_MASK_8X; 
 
       // set dio1 masks
       buf[4] = 0x00; 
@@ -2129,9 +2154,9 @@ void sx128x::onReceive(void(*callback)(uint8_t, int))
       buf[7] = 0x00;
 
       executeOpcode(OP_SET_IRQ_FLAGS_8X, buf, 8);
-//#ifdef SPI_HAS_NOTUSINGINTERRUPT
-//    _spiModem->usingInterrupt(digitalPinToInterrupt(_dio0));
-//#endif
+#ifdef SPI_HAS_NOTUSINGINTERRUPT
+    _spiModem->usingInterrupt(digitalPinToInterrupt(_dio0));
+#endif
 
     // make function available
     extern void onDio0Rise();
@@ -2139,9 +2164,9 @@ void sx128x::onReceive(void(*callback)(uint8_t, int))
     attachInterrupt(digitalPinToInterrupt(_dio0), onDio0Rise, RISING);
   } else {
     detachInterrupt(digitalPinToInterrupt(_dio0));
-//#ifdef SPI_HAS_NOTUSINGINTERRUPT
-//    _spiModem->notUsingInterrupt(digitalPinToInterrupt(_dio0));
-//#endif
+#ifdef SPI_HAS_NOTUSINGINTERRUPT
+    _spiModem->notUsingInterrupt(digitalPinToInterrupt(_dio0));
+#endif
   }
 }
 
@@ -2160,7 +2185,12 @@ void sx128x::receive(int size)
 
   rxAntEnable();
 
-    uint8_t mode[3] = {0xFF, 0xFF, 0xFF}; // continuous mode
+    // On the SX1280, there is a bug which can cause the busy line
+    // to remain high if a high amount of packets are received when
+    // in continuous RX mode. This is documented as Errata 16.1 in
+    // the SX1280 datasheet v3.2 (page 149)
+    // Therefore, the modem is set to single RX mode below instead.
+    uint8_t mode[3] = {0}; // single RX mode
     executeOpcode(OP_RX_8X, mode, 3);
 }
 
@@ -2563,29 +2593,22 @@ void sx128x::implicitHeaderMode()
 
 void sx128x::handleDio0Rise()
 {
-    uint8_t buf[2];
+    // received a packet
+    _packetIndex = 0;
 
-    buf[0] = 0x00;
-    buf[1] = 0x00;
+    uint8_t rxbuf[2] = {0};
+    executeOpcodeRead(OP_RX_BUFFER_STATUS_8X, rxbuf, 2);
 
-    executeOpcodeRead(OP_GET_IRQ_STATUS_8X, buf, 2);
-
-    executeOpcode(OP_CLEAR_IRQ_STATUS_8X, buf, 2);
-
-    if ((buf[1] & IRQ_PAYLOAD_CRC_ERROR_MASK_8X) == 0) {
-        // received a packet
-        _packetIndex = 0;
-
-        uint8_t rxbuf[2] = {0};
-        executeOpcodeRead(OP_RX_BUFFER_STATUS_8X, rxbuf, 2);
+    // If implicit header mode is enabled, read packet length as payload length instead.
+    // See SX1280 datasheet v3.2, page 92
+    if (_implicitHeaderMode == 0x80) {
+        _rxPacketLength = _payloadLength;
+    } else {
         _rxPacketLength = rxbuf[0];
-        _fifo_rx_addr_ptr = rxbuf[1];
-        readBuffer(_packet, _rxPacketLength);
-
-        if (_onReceive) {
-            _onReceive(_index, _rxPacketLength);
-        }
+    }
 
+    if (_onReceive) {
+        _onReceive(_index, _rxPacketLength);
     }
 }
 
@@ -2615,7 +2638,7 @@ void sx128x::updateBitrate() {
     }
 }
 
-void sx128x::clearIRQStatus() {
+bool ISR_VECT sx128x::getPacketValidity() {
     uint8_t buf[2];
 
     buf[0] = 0x00;
@@ -2624,4 +2647,10 @@ void sx128x::clearIRQStatus() {
     executeOpcodeRead(OP_GET_IRQ_STATUS_8X, buf, 2);
 
     executeOpcode(OP_CLEAR_IRQ_STATUS_8X, buf, 2);
+
+    if ((buf[1] & IRQ_PAYLOAD_CRC_ERROR_MASK_8X) == 0) {
+        return true;
+    } else {
+        return false;
+    }
 }
diff --git a/Radio.hpp b/Radio.hpp
index 3ac2f19..8360a04 100644
--- a/Radio.hpp
+++ b/Radio.hpp
@@ -11,6 +11,7 @@
 #include <SPI.h>
 #include "Interfaces.h"
 #include "Boards.h"
+#include "src/misc/FIFOBuffer.h"
 
 #define MAX_PKT_LENGTH                255
 
@@ -132,7 +133,7 @@ public:
 
     virtual void updateBitrate() = 0;
     virtual void handleDio0Rise() = 0;
-    virtual void clearIRQStatus() = 0;
+    virtual bool getPacketValidity() = 0;
     uint32_t getBitrate() { return _bitrate; };
     uint8_t getIndex() { return _index; };
     void setRadioLock(bool lock) { _radio_locked = lock; };
@@ -428,7 +429,7 @@ private:
   void reset(void);
   void calibrate(void);
   void calibrate_image(uint32_t frequency);
-  void clearIRQStatus();
+  bool getPacketValidity();
 
 private:
   SPISettings _spiSettings;
@@ -518,7 +519,7 @@ public:
   void updateBitrate();
 
   void handleDio0Rise();
-  void clearIRQStatus();
+  bool getPacketValidity();
 private:
   void setSyncWord(uint8_t sw);
   void explicitHeaderMode();
@@ -615,7 +616,8 @@ public:
 
   void handleDio0Rise();
 
-  void clearIRQStatus();
+  bool getPacketValidity();
+
 private:
   void writeBuffer(const uint8_t* buffer, size_t size);
   void readBuffer(uint8_t* buffer, size_t size);
diff --git a/Utilities.h b/Utilities.h
index a21ffad..b1b923c 100644
--- a/Utilities.h
+++ b/Utilities.h
@@ -584,6 +584,16 @@ void serial_write(uint8_t byte) {
 			Serial.write(byte);
 		} else {
 			SerialBT.write(byte);
+
+            // This ensures that the TX buffer is flushed after a frame is queued in serial.
+            // serial_in_frame is used to ensure that the flush only happens at the end of the frame
+            if (serial_in_frame && byte == FEND) {
+                SerialBT.flushTXD();
+                serial_in_frame = false;
+            }
+            else if (!serial_in_frame && byte == FEND) {
+                serial_in_frame = true;
+            }
 		}
 	#else
 		Serial.write(byte);
@@ -1522,168 +1532,4 @@ void unlock_rom() {
 	eeprom_erase();
 }
 
-typedef struct FIFOBuffer
-{
-  unsigned char *begin;
-  unsigned char *end;
-  unsigned char * volatile head;
-  unsigned char * volatile tail;
-} FIFOBuffer;
-
-inline bool fifo_isempty(const FIFOBuffer *f) {
-  return f->head == f->tail;
-}
-
-inline bool fifo_isfull(const FIFOBuffer *f) {
-  return ((f->head == f->begin) && (f->tail == f->end)) || (f->tail == f->head - 1);
-}
-
-inline void fifo_push(FIFOBuffer *f, unsigned char c) {
-  *(f->tail) = c;
-  
-  if (f->tail == f->end) {
-    f->tail = f->begin;
-  } else {
-    f->tail++;
-  }
-}
-
-inline unsigned char fifo_pop(FIFOBuffer *f) {
-  if(f->head == f->end) {
-    f->head = f->begin;
-    return *(f->end);
-  } else {
-    return *(f->head++);
-  }
-}
-
-inline void fifo_flush(FIFOBuffer *f) {
-  f->head = f->tail;
-}
-
-#if MCU_VARIANT != MCU_ESP32 && MCU_VARIANT != MCU_NRF52
-	static inline bool fifo_isempty_locked(const FIFOBuffer *f) {
-	  bool result;
-	  ATOMIC_BLOCK(ATOMIC_RESTORESTATE) {
-	    result = fifo_isempty(f);
-	  }
-	  return result;
-	}
-
-	static inline bool fifo_isfull_locked(const FIFOBuffer *f) {
-	  bool result;
-	  ATOMIC_BLOCK(ATOMIC_RESTORESTATE) {
-	    result = fifo_isfull(f);
-	  }
-	  return result;
-	}
-
-	static inline void fifo_push_locked(FIFOBuffer *f, unsigned char c) {
-	  ATOMIC_BLOCK(ATOMIC_RESTORESTATE) {
-	    fifo_push(f, c);
-	  }
-	}
-#endif
-
-/*
-static inline unsigned char fifo_pop_locked(FIFOBuffer *f) {
-  unsigned char c;
-  ATOMIC_BLOCK(ATOMIC_RESTORESTATE) {
-    c = fifo_pop(f);
-  }
-  return c;
-}
-*/
-
-inline void fifo_init(FIFOBuffer *f, unsigned char *buffer, size_t size) {
-  f->head = f->tail = f->begin = buffer;
-  f->end = buffer + size;
-}
-
-inline size_t fifo_len(FIFOBuffer *f) {
-  return f->end - f->begin;
-}
-
-typedef struct FIFOBuffer16
-{
-  uint16_t *begin;
-  uint16_t *end;
-  uint16_t * volatile head;
-  uint16_t * volatile tail;
-} FIFOBuffer16;
-
-inline bool fifo16_isempty(const FIFOBuffer16 *f) {
-  return f->head == f->tail;
-}
-
-inline bool fifo16_isfull(const FIFOBuffer16 *f) {
-  return ((f->head == f->begin) && (f->tail == f->end)) || (f->tail == f->head - 1);
-}
-
-inline void fifo16_push(FIFOBuffer16 *f, uint16_t c) {
-  *(f->tail) = c;
-
-  if (f->tail == f->end) {
-    f->tail = f->begin;
-  } else {
-    f->tail++;
-  }
-}
-
-inline uint16_t fifo16_pop(FIFOBuffer16 *f) {
-  if(f->head == f->end) {
-    f->head = f->begin;
-    return *(f->end);
-  } else {
-    return *(f->head++);
-  }
-}
-
-inline void fifo16_flush(FIFOBuffer16 *f) {
-  f->head = f->tail;
-}
-
-#if MCU_VARIANT != MCU_ESP32 && MCU_VARIANT != MCU_NRF52
-	static inline bool fifo16_isempty_locked(const FIFOBuffer16 *f) {
-	  bool result;
-	  ATOMIC_BLOCK(ATOMIC_RESTORESTATE) {
-	    result = fifo16_isempty(f);
-	  }
-
-	  return result;
-	}
-#endif
-
-/*
-static inline bool fifo16_isfull_locked(const FIFOBuffer16 *f) {
-  bool result;
-  ATOMIC_BLOCK(ATOMIC_RESTORESTATE) {
-    result = fifo16_isfull(f);
-  }
-  return result;
-}
-
-
-static inline void fifo16_push_locked(FIFOBuffer16 *f, uint16_t c) {
-  ATOMIC_BLOCK(ATOMIC_RESTORESTATE) {
-    fifo16_push(f, c);
-  }
-}
-
-static inline size_t fifo16_pop_locked(FIFOBuffer16 *f) {
-  size_t c;
-  ATOMIC_BLOCK(ATOMIC_RESTORESTATE) {
-    c = fifo16_pop(f);
-  }
-  return c;
-}
-*/
-
-inline void fifo16_init(FIFOBuffer16 *f, uint16_t *buffer, uint16_t size) {
-  f->head = f->tail = f->begin = buffer;
-  f->end = buffer + size;
-}
-
-inline uint16_t fifo16_len(FIFOBuffer16 *f) {
-  return (f->end - f->begin);
-}
+#include "src/misc/FIFOBuffer.h"
diff --git a/opencom_xl_firmware.ino b/opencom_xl_firmware.ino
index e1df8b5..98365ed 100644
--- a/opencom_xl_firmware.ino
+++ b/opencom_xl_firmware.ino
@@ -64,11 +64,6 @@ volatile bool serial_buffering = false;
 
 char sbuf[128];
 
-bool packet_ready = false;
-
-volatile bool process_packet = false;
-volatile uint8_t packet_interface = 0;
-
 uint8_t *packet_queue[INTERFACE_COUNT];
 
 void setup() {
@@ -139,11 +134,15 @@ void setup() {
   memset(packet_lengths_buf, 0, sizeof(packet_starts_buf));
 
   for (int i = 0; i < INTERFACE_COUNT; i++) {
-      fifo16_init(&packet_starts[i], packet_starts_buf, CONFIG_QUEUE_MAX_LENGTH);
-      fifo16_init(&packet_lengths[i], packet_lengths_buf, CONFIG_QUEUE_MAX_LENGTH);
+      fifo16_init(&packet_starts[i], packet_starts_buf, CONFIG_QUEUE_MAX_LENGTH+1);
+      fifo16_init(&packet_lengths[i], packet_lengths_buf, CONFIG_QUEUE_MAX_LENGTH+1);
       packet_queue[i] = (uint8_t*)malloc(getQueueSize(i)+1);
   }
 
+  memset(packet_rdy_interfaces_buf, 0, sizeof(packet_rdy_interfaces_buf));
+
+  fifo_init(&packet_rdy_interfaces, packet_rdy_interfaces_buf, MAX_INTERFACES);
+
   // Create and configure interface objects
   for (uint8_t i = 0; i < INTERFACE_COUNT; i++) {
       switch (interfaces[i]) {
@@ -325,7 +324,6 @@ inline void kiss_write_packet(int index) {
   }
   serial_write(FEND);
   read_len = 0;
-  packet_ready = false;
 }
 
 inline void getPacketData(RadioInterface* radio, uint16_t len) {
@@ -334,18 +332,18 @@ inline void getPacketData(RadioInterface* radio, uint16_t len) {
   }
 }
 
-void ISR_VECT receive_callback(uint8_t index, int packet_size) {
+void receive_callback(uint8_t index, int packet_size) {
+        selected_radio = interface_obj[index];
+    bool    ready    = false;
   if (!promisc) {
-    selected_radio = interface_obj[index];
-
     // The standard operating mode allows large
     // packets with a payload up to 500 bytes,
     // by combining two raw LoRa packets.
     // We read the 1-byte header and extract
     // packet sequence number and split flags
+    
     uint8_t header   = selected_radio->read(); packet_size--;
     uint8_t sequence = packetSequence(header);
-    bool    ready    = false;
 
     if (isSplitPacket(header) && seq == SEQ_UNSET) {
       // This is the first part of a split
@@ -391,17 +389,31 @@ void ISR_VECT receive_callback(uint8_t index, int packet_size) {
       getPacketData(selected_radio, packet_size);
       ready = true;
     }
-
-    if (ready) {
-        packet_ready = true;
-    }  
   } else {
     // In promiscuous mode, raw packets are
     // output directly to the host
     read_len = 0;
 
     getPacketData(selected_radio, packet_size);
-    packet_ready = true;
+    ready = true;
+  }
+
+  if (ready) {
+        #if MCU_VARIANT == MCU_ESP32
+        portENTER_CRITICAL(&update_lock);
+        #elif MCU_VARIANT == MCU_NRF52
+        portENTER_CRITICAL();
+        #endif
+        last_rssi = selected_radio->packetRssi();
+        last_snr_raw = selected_radio->packetSnrRaw();
+        #if MCU_VARIANT == MCU_ESP32
+        portEXIT_CRITICAL(&update_lock);
+        #elif MCU_VARIANT == MCU_NRF52
+        portEXIT_CRITICAL();
+        #endif
+        kiss_indicate_stat_rssi();
+        kiss_indicate_stat_snr();
+        kiss_write_packet(index);
   }
   last_rx = millis();
 }
@@ -595,7 +607,7 @@ void serialCallback(uint8_t sbyte) {
             uint8_t index = getInterfaceIndex(command);
         if (!fifo16_isfull(&packet_starts[index]) && (queued_bytes[index] < (getQueueSize(index)))) {
             uint16_t s = current_packet_start[index];
-            uint16_t e = queue_cursor[index]-1; if (e == -1) e = (getQueueSize(index))-1;
+            int32_t e = queue_cursor[index]-1; if (e == -1) e = (getQueueSize(index))-1;
             uint16_t l;
 
             if (s != e) {
@@ -1191,25 +1203,6 @@ void loop() {
 
   // If at least one radio is online then we can continue
   if (ready) {
-      if (packet_ready) {
-        selected_radio = interface_obj[packet_interface];
-        #if MCU_VARIANT == MCU_ESP32
-        portENTER_CRITICAL(&update_lock);
-        #elif MCU_VARIANT == MCU_NRF52
-        portENTER_CRITICAL();
-        #endif
-        last_rssi = selected_radio->packetRssi();
-        last_snr_raw = selected_radio->packetSnrRaw();
-        #if MCU_VARIANT == MCU_ESP32
-        portEXIT_CRITICAL(&update_lock);
-        #elif MCU_VARIANT == MCU_NRF52
-        portEXIT_CRITICAL();
-        #endif
-        kiss_indicate_stat_rssi();
-        kiss_indicate_stat_snr();
-        kiss_write_packet(packet_interface);
-      }
-        
     for (int i = 0; i < INTERFACE_COUNT; i++) {
         selected_radio = interface_obj_sorted[i];
 
@@ -1222,7 +1215,7 @@ void loop() {
         // loop, it still needs to be the first to transmit, so check if this
         // is the case.
         for (int j = 0; j < INTERFACE_COUNT; j++) {
-            if (!interface_obj_sorted[j]->calculateALock() || interface_obj_sorted[j]->getRadioOnline()) {
+            if (!interface_obj_sorted[j]->calculateALock() && interface_obj_sorted[j]->getRadioOnline()) {
                 if (interface_obj_sorted[j]->getBitrate() > selected_radio->getBitrate()) {
                     if (queue_height[interface_obj_sorted[j]->getIndex()] > 0) {
                         selected_radio = interface_obj_sorted[j];
@@ -1232,7 +1225,7 @@ void loop() {
         }
 
         if (queue_height[selected_radio->getIndex()] > 0) {
-            long check_time = millis();
+            uint32_t check_time = millis();
             if (check_time > selected_radio->getPostTxYieldTimeout()) {
                 if (selected_radio->getDCDWaiting() && (check_time >= selected_radio->getDCDWaitUntil())) { selected_radio->setDCDWaiting(false); }
                 if (!selected_radio->getDCDWaiting()) {
@@ -1335,23 +1328,22 @@ void poll_buffers() {
 }
 
 void packet_poll() {
-    #if MCU_VARIANT == MCU_ESP32
-    portENTER_CRITICAL(&update_lock);
-    #elif MCU_VARIANT == MCU_NRF52
-    portENTER_CRITICAL();
-    #endif
     // If we have received a packet on an interface which needs to be processed
-    if (process_packet) {
-        selected_radio = interface_obj[packet_interface];
-        selected_radio->clearIRQStatus();
+    while (!fifo_isempty(&packet_rdy_interfaces)) {
+        #if MCU_VARIANT == MCU_ESP32
+        portENTER_CRITICAL(&update_lock);
+        #elif MCU_VARIANT == MCU_NRF52
+        portENTER_CRITICAL();
+        #endif
+        uint8_t packet_int = fifo_pop(&packet_rdy_interfaces);
+        #if MCU_VARIANT == MCU_ESP32
+        portEXIT_CRITICAL(&update_lock);
+        #elif MCU_VARIANT == MCU_NRF52
+        portEXIT_CRITICAL();
+        #endif
+        selected_radio = interface_obj[packet_int];
         selected_radio->handleDio0Rise();
-        process_packet = false;
     }
-    #if MCU_VARIANT == MCU_ESP32
-    portEXIT_CRITICAL(&update_lock);
-    #elif MCU_VARIANT == MCU_NRF52
-    portEXIT_CRITICAL();
-    #endif
 }
 
 volatile bool serial_polling = false;
diff --git a/src/misc/FIFOBuffer.c b/src/misc/FIFOBuffer.c
new file mode 100644
index 0000000..86fa0ca
--- /dev/null
+++ b/src/misc/FIFOBuffer.c
@@ -0,0 +1,95 @@
+#include <stdint.h>
+#include <stdbool.h>
+#include <stddef.h>
+#include "FIFOBuffer.h"
+
+#ifdef __cplusplus
+ extern "C" {
+#endif
+
+ bool fifo_isempty(const FIFOBuffer *f) {
+  return f->head == f->tail;
+}
+
+ bool fifo_isfull(const FIFOBuffer *f) {
+  return ((f->head == f->begin) && (f->tail == f->end)) || (f->tail == f->head - 1);
+}
+
+ void fifo_push(FIFOBuffer *f, unsigned char c) {
+  *(f->tail) = c;
+  
+  if (f->tail == f->end) {
+    f->tail = f->begin;
+  } else {
+    f->tail++;
+  }
+}
+
+ unsigned char fifo_pop(FIFOBuffer *f) {
+  if(f->head == f->end) {
+    f->head = f->begin;
+    return *(f->end);
+  } else {
+    return *(f->head++);
+  }
+}
+
+ void fifo_flush(FIFOBuffer *f) {
+  f->head = f->tail;
+}
+
+
+ void fifo_init(FIFOBuffer *f, unsigned char *buffer, size_t size) {
+  f->head = f->tail = f->begin = buffer;
+  f->end = buffer + size;
+}
+
+// todo, fix this so it actually displays the amount of data in the fifo
+// buffer, not just the size allocated for the buffer
+ size_t fifo_len(FIFOBuffer *f) {
+  return f->end - f->begin;
+}
+
+ bool fifo16_isempty(const FIFOBuffer16 *f) {
+  return f->head == f->tail;
+}
+
+ bool fifo16_isfull(const FIFOBuffer16 *f) {
+  return ((f->head == f->begin) && (f->tail == f->end)) || (f->tail == f->head - 1);
+}
+
+ void fifo16_push(FIFOBuffer16 *f, uint16_t c) {
+  *(f->tail) = c;
+
+  if (f->tail == f->end) {
+    f->tail = f->begin;
+  } else {
+    f->tail++;
+  }
+}
+
+ uint16_t fifo16_pop(FIFOBuffer16 *f) {
+  if(f->head == f->end) {
+    f->head = f->begin;
+    return *(f->end);
+  } else {
+    return *(f->head++);
+  }
+}
+
+ void fifo16_flush(FIFOBuffer16 *f) {
+  f->head = f->tail;
+}
+
+ void fifo16_init(FIFOBuffer16 *f, uint16_t *buffer, uint16_t size) {
+  f->head = f->tail = f->begin = buffer;
+  f->end = buffer + size;
+}
+
+ uint16_t fifo16_len(FIFOBuffer16 *f) {
+  return (f->end - f->begin);
+}
+
+#ifdef __cplusplus
+}
+#endif
diff --git a/src/misc/FIFOBuffer.h b/src/misc/FIFOBuffer.h
new file mode 100644
index 0000000..bdc6d00
--- /dev/null
+++ b/src/misc/FIFOBuffer.h
@@ -0,0 +1,59 @@
+#ifndef FIFOBUFFER_H
+
+#define FIFOBUFFER_H
+
+#ifdef __cplusplus
+ extern "C" {
+#endif
+
+/* An 8 bit FIFO buffer implementation */
+typedef struct FIFOBuffer
+{
+  unsigned char *begin;
+  unsigned char *end;
+  unsigned char * volatile head;
+  unsigned char * volatile tail;
+} FIFOBuffer;
+
+bool fifo_isempty(const FIFOBuffer *f);
+
+bool fifo_isfull(const FIFOBuffer *f);
+
+void fifo_push(FIFOBuffer *f, unsigned char c);
+
+unsigned char fifo_pop(FIFOBuffer *f);
+
+void fifo_flush(FIFOBuffer *f);
+
+void fifo_init(FIFOBuffer *f, unsigned char *buffer, size_t size);
+
+size_t fifo_len(FIFOBuffer *f);
+
+/* A 16-bit implementation of the same FIFO buffer. */
+typedef struct FIFOBuffer16
+{
+  uint16_t *begin;
+  uint16_t *end;
+  uint16_t * volatile head;
+  uint16_t * volatile tail;
+} FIFOBuffer16;
+
+bool fifo16_isempty(const FIFOBuffer16 *f);
+
+bool fifo16_isfull(const FIFOBuffer16 *f);
+
+void fifo16_push(FIFOBuffer16 *f, uint16_t c);
+
+uint16_t fifo16_pop(FIFOBuffer16 *f);
+
+void fifo16_flush(FIFOBuffer16 *f);
+
+void fifo16_init(FIFOBuffer16 *f, uint16_t *buffer, uint16_t size);
+
+uint16_t fifo16_len(FIFOBuffer16 *f);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif