diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json
index 1f5743b9055e32f846f135ae502b67341914895c..ebe753e1b3a61bdf8b93e10c64f13e6199a5edea 100644
--- a/.vscode/c_cpp_properties.json
+++ b/.vscode/c_cpp_properties.json
@@ -318,6 +318,61 @@
                 "limitSymbolsToIncludedHeaders": true
             }
         },
+        {
+            "name": "stm32f429zi_skyward_groundstation_v2",
+            "cStandard": "c11",
+            "cppStandard": "c++14",
+            "compilerPath": "/opt/arm-miosix-eabi/bin/arm-miosix-eabi-g++",
+            "defines": [
+                "DEBUG",
+                "_ARCH_CORTEXM4_STM32F4",
+                "_BOARD_STM32F429ZI_SKYWARD_GS_V2",
+                "_MIOSIX_BOARDNAME=stm32f429zi_skyward_groundstation_v2",
+                "HSE_VALUE=8000000",
+                "SYSCLK_FREQ_168MHz=168000000",
+                "_MIOSIX",
+                "__cplusplus=201103L"
+            ],
+            "includePath": [
+                "${workspaceFolder}/libs/miosix-kernel/miosix/config/arch/cortexM4_stm32f4/stm32f429zi_skyward_groundstation_v2",
+                "${workspaceFolder}/libs/miosix-kernel/miosix/arch/cortexM4_stm32f4/stm32f429zi_skyward_groundstation_v2",
+                "${workspaceFolder}/libs/miosix-kernel/miosix/arch/cortexM4_stm32f4/common",
+                "${workspaceFolder}/libs/miosix-kernel/miosix/arch/common",
+                "${workspaceFolder}/libs/miosix-kernel/miosix",
+                "${workspaceFolder}/libs/mxgui/mxgui",
+                "${workspaceFolder}/libs/mxgui",
+                "${workspaceFolder}/libs/mavlink-skyward-lib",
+                "${workspaceFolder}/libs/fmt/include",
+                "${workspaceFolder}/libs/eigen",
+                "${workspaceFolder}/libs/tscpp",
+                "${workspaceFolder}/libs",
+                "${workspaceFolder}/src/shared",
+                "${workspaceFolder}/src/tests"
+            ],
+            "browse": {
+                "path": [
+                    "${workspaceFolder}/libs/miosix-kernel/miosix/config/arch/cortexM4_stm32f4/stm32f429zi_skyward_groundstation_v2",
+                    "${workspaceFolder}/libs/miosix-kernel/miosix/arch/cortexM4_stm32f4/stm32f429zi_skyward_groundstation_v2",
+                    "${workspaceFolder}/libs/miosix-kernel/miosix/arch/cortexM4_stm32f4/common",
+                    "${workspaceFolder}/libs/miosix-kernel/miosix/stdlib_integration",
+                    "${workspaceFolder}/libs/miosix-kernel/miosix/arch/common",
+                    "${workspaceFolder}/libs/miosix-kernel/miosix/interfaces",
+                    "${workspaceFolder}/libs/miosix-kernel/miosix/filesystem",
+                    "${workspaceFolder}/libs/miosix-kernel/miosix/kernel",
+                    "${workspaceFolder}/libs/miosix-kernel/miosix/util",
+                    "${workspaceFolder}/libs/miosix-kernel/miosix/e20",
+                    "${workspaceFolder}/libs/miosix-kernel/miosix/*",
+                    "${workspaceFolder}/libs/mavlink-skyward-lib",
+                    "${workspaceFolder}/libs/eigen",
+                    "${workspaceFolder}/libs/tscpp",
+                    "${workspaceFolder}/libs/mxgui",
+                    "${workspaceFolder}/libs/fmt",
+                    "${workspaceFolder}/src/shared",
+                    "${workspaceFolder}/src/tests"
+                ],
+                "limitSymbolsToIncludedHeaders": true
+            }
+        },
         {
             "name": "stm32f205rc_skyward_ciuti",
             "cStandard": "c11",
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 0082bcd90b01663b5fc29e92dedfa58b36b6aa2c..5a511e5de6b6133cf8c7601398c36f2cf61eef3b 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -57,6 +57,14 @@ sbs_target(runcam-settings stm32f407vg_stm32f4discovery)
 add_executable(sdcard-benchmark src/entrypoints/sdcard-benchmark.cpp)
 sbs_target(sdcard-benchmark stm32f429zi_skyward_death_stack_x)
 
+add_executable(sx1278fsk-serial src/entrypoints/sx1278-serial.cpp)
+target_compile_definitions(sx1278fsk-serial PRIVATE SX1278_IS_FSK)
+sbs_target(sx1278fsk-serial stm32f429zi_skyward_groundstation_v2)
+
+add_executable(sx1278lora-serial src/entrypoints/sx1278-serial.cpp)
+target_compile_definitions(sx1278lora-serial PRIVATE SX1278_IS_LORA)
+sbs_target(sx1278lora-serial stm32f429zi_skyward_groundstation_v2)
+
 #-----------------------------------------------------------------------------#
 #                                    Tests                                    #
 #-----------------------------------------------------------------------------#
@@ -262,42 +270,47 @@ sbs_target(test-fsm stm32f429zi_stm32f4discovery)
 #                               Tests - Radio                                 #
 #-----------------------------------------------------------------------------#
 
-# add_executable(test-sx1278-bidir src/tests/drivers/sx1278/test-sx1278-bidir.cpp)
-# sbs_target(test-sx1278-bidir stm32f407vg_stm32f4discovery)
+add_executable(test-sx1278fsk-bidir src/tests/radio/sx1278/fsk/test-sx1278-bidir.cpp)
+sbs_target(test-sx1278fsk-bidir stm32f429zi_skyward_groundstation_v2)
+
+add_executable(test-sx1278fsk-tx src/tests/radio/sx1278/fsk/test-sx1278-bench-serial.cpp)
+target_compile_definitions(test-sx1278fsk-tx PRIVATE DISABLE_RX)
+sbs_target(test-sx1278fsk-tx stm32f429zi_skyward_groundstation_v2)
 
-add_executable(test-sx1278-bidir-gs src/tests/drivers/sx1278/test-sx1278-bidir.cpp)
-sbs_target(test-sx1278-bidir-gs stm32f429zi_skyward_groundstation)
+add_executable(test-sx1278fsk-rx src/tests/radio/sx1278/fsk/test-sx1278-bench-serial.cpp)
+target_compile_definitions(test-sx1278fsk-rx PRIVATE DISABLE_TX)
+sbs_target(test-sx1278fsk-rx stm32f429zi_skyward_groundstation_v2)
 
-add_executable(test-sx1278-bidir-v3 src/tests/drivers/sx1278/test-sx1278-bidir.cpp)
-sbs_target(test-sx1278-bidir-v3 stm32f429zi_skyward_death_stack_v3)
+add_executable(test-sx1278fsk-gui src/tests/radio/sx1278/fsk/test-sx1278-bench-gui.cpp)
+sbs_target(test-sx1278fsk-gui stm32f429zi_skyward_groundstation_v2)
 
-add_executable(test-sx1278-tx src/tests/drivers/sx1278/test-sx1278-bench-serial.cpp)
-target_compile_definitions(test-sx1278-tx PRIVATE DISABLE_RX)
-sbs_target(test-sx1278-tx stm32f429zi_skyward_groundstation)
+add_executable(test-sx1278fsk-gui-rx src/tests/radio/sx1278/fsk/test-sx1278-bench-gui.cpp)
+target_compile_definitions(test-sx1278fsk-gui-rx PRIVATE DISABLE_TX)
+sbs_target(test-sx1278fsk-gui-rx stm32f429zi_skyward_groundstation_v2)
 
-add_executable(test-sx1278-rx src/tests/drivers/sx1278/test-sx1278-bench-serial.cpp)
-target_compile_definitions(test-sx1278-rx PRIVATE DISABLE_TX)
-sbs_target(test-sx1278-rx stm32f429zi_skyward_groundstation)
+add_executable(test-sx1278fsk-gui-tx src/tests/radio/sx1278/fsk/test-sx1278-bench-gui.cpp)
+target_compile_definitions(test-sx1278fsk-gui-tx PRIVATE DISABLE_RX)
+sbs_target(test-sx1278fsk-gui-tx stm32f429zi_skyward_groundstation_v2)
 
-add_executable(test-sx1278-gui src/tests/drivers/sx1278/test-sx1278-bench-gui.cpp)
-sbs_target(test-sx1278-gui stm32f429zi_skyward_groundstation)
+add_executable(test-sx1278fsk-mavlink src/tests/radio/sx1278/fsk/test-sx1278-mavlink.cpp)
+sbs_target(test-sx1278fsk-mavlink stm32f429zi_skyward_groundstation_v2)
 
-add_executable(test-sx1278-gui-rx src/tests/drivers/sx1278/test-sx1278-bench-gui.cpp)
-target_compile_definitions(test-sx1278-gui-rx PRIVATE DISABLE_TX)
-sbs_target(test-sx1278-gui-rx stm32f429zi_skyward_groundstation)
+# add_executable(test-mavlinkdriver src/tests/radio/test-mavlinkdriver.cpp)
+# sbs_target(test-mavlinkdriver stm32f407vg_stm32f4discovery)
 
-add_executable(test-sx1278-gui-tx src/tests/drivers/sx1278/test-sx1278-bench-gui.cpp)
-target_compile_definitions(test-sx1278-gui-tx PRIVATE DISABLE_RX)
-sbs_target(test-sx1278-gui-tx stm32f429zi_skyward_groundstation)
+add_executable(test-sx1278lora-bidir src/tests/radio/sx1278/lora/test-sx1278-bidir.cpp)
+sbs_target(test-sx1278lora-bidir stm32f429zi_skyward_groundstation_v2)
 
-add_executable(test-sx1278-mavlink src/tests/drivers/sx1278/test-sx1278-mavlink.cpp)
-sbs_target(test-sx1278-mavlink stm32f429zi_skyward_groundstation_v2)
+add_executable(test-sx1278lora-mavlink src/tests/radio/sx1278/lora/test-sx1278-mavlink.cpp)
+sbs_target(test-sx1278lora-mavlink stm32f429zi_skyward_groundstation_v2)
 
-add_executable(test-sx1278-serial src/tests/drivers/sx1278/test-sx1278-serial.cpp)
-sbs_target(test-sx1278-serial stm32f429zi_stm32f4discovery)
+add_executable(test-sx1278lora-rx src/tests/radio/sx1278/lora/test-sx1278-simple.cpp)
+target_compile_definitions(test-sx1278lora-rx PRIVATE ENABLE_RX)
+sbs_target(test-sx1278lora-rx stm32f429zi_skyward_groundstation_v2)
 
-add_executable(test-mavlinkdriver src/tests/radio/test-mavlinkdriver.cpp)
-sbs_target(test-mavlinkdriver stm32f407vg_stm32f4discovery)
+add_executable(test-sx1278lora-tx src/tests/radio/sx1278/lora/test-sx1278-simple.cpp)
+target_compile_definitions(test-sx1278lora-tx PRIVATE ENABLE_TX)
+sbs_target(test-sx1278lora-tx stm32f429zi_skyward_groundstation_v2)
 
 #-----------------------------------------------------------------------------#
 #                               Tests - Sensors                               #
diff --git a/cmake/boardcore.cmake b/cmake/boardcore.cmake
index 5632efa3cea92276884ecff0f5f77a7a3cb428ae..5a91d68a09de6e972b58f779eac3ab722f596cd7 100644
--- a/cmake/boardcore.cmake
+++ b/cmake/boardcore.cmake
@@ -70,7 +70,10 @@ foreach(OPT_BOARD ${BOARDS})
         ${SBS_BASE}/src/shared/radio/gamma868/Gamma868.cpp
         ${SBS_BASE}/src/shared/radio/Xbee/APIFrameParser.cpp
         ${SBS_BASE}/src/shared/radio/Xbee/Xbee.cpp
-        ${SBS_BASE}/src/shared/radio/SX1278/SX1278.cpp
+        ${SBS_BASE}/src/shared/radio/SX1278/SX1278Fsk.cpp
+        ${SBS_BASE}/src/shared/radio/SX1278/SX1278Lora.cpp
+        ${SBS_BASE}/src/shared/radio/SX1278/SX1278Common.cpp
+        ${SBS_BASE}/src/shared/radio/SX1278/Ebyte.cpp
 
         # Scheduler
         ${SBS_BASE}/src/shared/scheduler/TaskScheduler.cpp
diff --git a/src/entrypoints/sx1278-serial.cpp b/src/entrypoints/sx1278-serial.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..dc437055a385791de7aed7a047ee6e60e17c13bd
--- /dev/null
+++ b/src/entrypoints/sx1278-serial.cpp
@@ -0,0 +1,219 @@
+/* Copyright (c) 2022 Skyward Experimental Rocketry
+ * Author: Davide Mor
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#include <drivers/interrupt/external_interrupts.h>
+#include <filesystem/console/console_device.h>
+
+// SX1278 includes
+#include <radio/SX1278/Ebyte.h>
+#include <radio/SX1278/SX1278Fsk.h>
+#include <radio/SX1278/SX1278Lora.h>
+
+#include <thread>
+
+using namespace Boardcore;
+using namespace miosix;
+
+// Uncomment the following line to enable Lora mode
+// Or use SBS to define it for you
+// #define SX1278_IS_LORA
+
+#if defined _BOARD_STM32F429ZI_SKYWARD_GS_V2
+#include "interfaces-impl/hwmapping.h"
+
+// Uncomment the following line to enable Ebyte module
+// #define SX1278_IS_EBYTE
+
+using cs   = peripherals::ra01::pc13::cs;
+using dio0 = peripherals::ra01::pc13::dio0;
+using dio1 = peripherals::ra01::pc13::dio1;
+using dio3 = peripherals::ra01::pc13::dio3;
+
+using sck  = interfaces::spi4::sck;
+using miso = interfaces::spi4::miso;
+using mosi = interfaces::spi4::mosi;
+
+#ifdef SX1278_IS_EBYTE
+using txen = Gpio<GPIOE_BASE, 4>;
+using rxen = Gpio<GPIOD_BASE, 4>;
+#endif
+
+#define SX1278_SPI SPI4
+
+#define SX1278_IRQ_DIO0 EXTI6_IRQHandlerImpl
+#define SX1278_IRQ_DIO1 EXTI2_IRQHandlerImpl
+#define SX1278_IRQ_DIO3 EXTI11_IRQHandlerImpl
+
+#else
+#error "Target not supported"
+#endif
+
+#ifdef SX1278_IS_LORA
+static constexpr size_t SX1278_MTU = SX1278Lora::MTU;
+SX1278Lora *sx1278                 = nullptr;
+#else
+static constexpr size_t SX1278_MTU = SX1278Fsk::MTU;
+SX1278Fsk *sx1278                  = nullptr;
+#endif
+
+#ifdef SX1278_IRQ_DIO0
+void __attribute__((used)) SX1278_IRQ_DIO0()
+{
+    if (sx1278)
+        sx1278->handleDioIRQ(SX1278::Dio::DIO0);
+}
+#endif
+
+#ifdef SX1278_IRQ_DIO1
+void __attribute__((used)) SX1278_IRQ_DIO1()
+{
+    if (sx1278)
+        sx1278->handleDioIRQ(SX1278::Dio::DIO1);
+}
+#endif
+
+#ifdef SX1278_IRQ_DIO3
+void __attribute__((used)) SX1278_IRQ_DIO3()
+{
+    if (sx1278)
+        sx1278->handleDioIRQ(SX1278::Dio::DIO3);
+}
+#endif
+
+void initBoard()
+{
+#ifdef SX1278_IS_EBYTE
+    rxen::mode(Mode::OUTPUT);
+    txen::mode(Mode::OUTPUT);
+    rxen::low();
+    txen::low();
+#endif
+
+#ifdef SX1278_IRQ_DIO0
+    GpioPin dio0_pin = dio0::getPin();
+    enableExternalInterrupt(dio0_pin.getPort(), dio0_pin.getNumber(),
+                            InterruptTrigger::RISING_EDGE);
+#endif
+
+#ifdef SX1278_IRQ_DIO1
+    GpioPin dio1_pin = dio1::getPin();
+    enableExternalInterrupt(dio1_pin.getPort(), dio1_pin.getNumber(),
+                            InterruptTrigger::RISING_EDGE);
+#endif
+
+#ifdef SX1278_IRQ_DIO3
+    GpioPin dio3_pin = dio3::getPin();
+    enableExternalInterrupt(dio3_pin.getPort(), dio3_pin.getNumber(),
+                            InterruptTrigger::RISING_EDGE);
+#endif
+}
+
+void recvLoop()
+{
+    uint8_t msg[SX1278_MTU];
+    while (1)
+    {
+        int len = sx1278->receive(msg, sizeof(msg));
+        if (len > 0)
+        {
+            auto serial = miosix::DefaultConsole::instance().get();
+            serial->writeBlock(msg, len, 0);
+        }
+    }
+}
+
+void sendLoop()
+{
+    uint8_t msg[SX1278_MTU];
+    while (1)
+    {
+        auto serial = miosix::DefaultConsole::instance().get();
+        int len     = serial->readBlock(msg, sizeof(msg), 0);
+        if (len > 0)
+        {
+            sx1278->send(msg, len);
+        }
+    }
+}
+
+int main()
+{
+    initBoard();
+
+    // Generic SPI configuration
+    SPIBusConfig spi_config;
+    spi_config.clockDivider = SPI::ClockDivider::DIV_64;
+    spi_config.mode         = SPI::Mode::MODE_0;
+    spi_config.bitOrder     = SPI::BitOrder::MSB_FIRST;
+
+    SPIBus bus(SX1278_SPI);
+    GpioPin cs = cs::getPin();
+
+    SPISlave spi(bus, cs, spi_config);
+
+#ifdef SX1278_IS_LORA
+    // Run default configuration
+    SX1278Lora::Config config;
+    SX1278Lora::Error err;
+
+#ifdef SX1278_IS_EBYTE
+    sx1278 = new EbyteLora(spi, txen::getPin(), rxen::getPin());
+#else
+    sx1278 = new SX1278Lora(spi);
+#endif
+
+    printf("\n[sx1278] Configuring sx1278...\n");
+    if ((err = sx1278->init(config)) != SX1278Lora::Error::NONE)
+    {
+        printf("[sx1278] sx1278->init error\n");
+        return -1;
+    }
+
+    printf("\n[sx1278] Initialization complete!\n");
+#else
+    // Run default configuration
+    SX1278Fsk::Config config;
+    SX1278Fsk::Error err;
+
+#ifdef SX1278_IS_EBYTE
+    sx1278 = new EbyteFsk(spi, txen::getPin(), rxen::getPin());
+#else
+    sx1278 = new SX1278Fsk(spi);
+#endif
+
+    printf("\n[sx1278] Configuring sx1278...\n");
+    if ((err = sx1278->init(config)) != SX1278Fsk::Error::NONE)
+    {
+                         // FIXME: Why does clang-format put this line up here?
+        printf("[sx1278] sx1278->init error\n");
+        return -1;
+    }
+
+    printf("\n[sx1278] Initialization complete!\n");
+#endif
+
+    // Actually spawn threads
+    std::thread send([]() { sendLoop(); });
+    recvLoop();
+
+    return 0;
+}
diff --git a/src/shared/radio/SX1278/Ebyte.cpp b/src/shared/radio/SX1278/Ebyte.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..8ac662783c0eb1d26869326d19119b43cf81b819
--- /dev/null
+++ b/src/shared/radio/SX1278/Ebyte.cpp
@@ -0,0 +1,50 @@
+/* Copyright (c) 2022 Skyward Experimental Rocketry
+ * Author: Davide Mor
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#include "Ebyte.h"
+
+namespace Boardcore
+{
+
+EbyteFsk::Error EbyteFsk::configure(const Config& config)
+{
+    // TODO(davide.mor): Validate input parameters
+    return SX1278Fsk::configure(config);
+}
+
+void EbyteFsk::enableRxFrontend() { rx_enable.high(); }
+void EbyteFsk::disableRxFrontend() { rx_enable.low(); }
+void EbyteFsk::enableTxFrontend() { tx_enable.high(); }
+void EbyteFsk::disableTxFrontend() { tx_enable.low(); }
+
+EbyteLora::Error EbyteLora::configure(const Config& config)
+{
+    // TODO(davide.mor): Validate input parameters
+    return SX1278Lora::configure(config);
+}
+
+void EbyteLora::enableRxFrontend() { rx_enable.high(); }
+void EbyteLora::disableRxFrontend() { rx_enable.low(); }
+void EbyteLora::enableTxFrontend() { tx_enable.high(); }
+void EbyteLora::disableTxFrontend() { tx_enable.low(); }
+
+}  // namespace Boardcore
diff --git a/src/shared/radio/SX1278/Ebyte.h b/src/shared/radio/SX1278/Ebyte.h
new file mode 100644
index 0000000000000000000000000000000000000000..c7e22a5d7330a6545c8ca8992b5d3cb8b3163b02
--- /dev/null
+++ b/src/shared/radio/SX1278/Ebyte.h
@@ -0,0 +1,87 @@
+/* Copyright (c) 2022 Skyward Experimental Rocketry
+ * Author: Davide Mor
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#pragma once
+
+#include <miosix.h>
+
+#include "SX1278Fsk.h"
+#include "SX1278Lora.h"
+
+namespace Boardcore
+{
+
+/**
+ * @brief Specialized SX1278 driver for the E32 400M30S module
+ */
+class EbyteFsk : public SX1278Fsk
+{
+public:
+    EbyteFsk(SPISlave slave, miosix::GpioPin tx_enable,
+             miosix::GpioPin rx_enable)
+        : SX1278Fsk(slave), tx_enable(tx_enable), rx_enable(rx_enable)
+    {
+        // Make sure both the frontends are disabled!
+        tx_enable.low();
+        rx_enable.low();
+    }
+
+    [[nodiscard]] Error configure(const Config &config) override;
+
+private:
+    void enableRxFrontend() override;
+    void disableRxFrontend() override;
+    void enableTxFrontend() override;
+    void disableTxFrontend() override;
+
+    miosix::GpioPin tx_enable;
+    miosix::GpioPin rx_enable;
+};
+
+/**
+ * @brief Specialized SX1278 driver for the E32 400M30S module
+ */
+class EbyteLora : public SX1278Lora
+{
+public:
+    EbyteLora(SPISlave slave, miosix::GpioPin tx_enable,
+              miosix::GpioPin rx_enable)
+        : SX1278Lora(slave), tx_enable(tx_enable), rx_enable(rx_enable)
+    {
+        // Make sure both the frontends are disabled!
+        tx_enable.low();
+        rx_enable.low();
+    }
+
+    [[nodiscard]] Error configure(const Config &config) override;
+
+private:
+    void enableRxFrontend() override;
+    void disableRxFrontend() override;
+    void enableTxFrontend() override;
+    void disableTxFrontend() override;
+
+    miosix::GpioPin tx_enable;
+    miosix::GpioPin rx_enable;
+};
+
+}  // namespace Boardcore
diff --git a/src/shared/radio/SX1278/SX1278Common.cpp b/src/shared/radio/SX1278/SX1278Common.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..08b03a30126972efbd0e50c0ec75541a6fe6500d
--- /dev/null
+++ b/src/shared/radio/SX1278/SX1278Common.cpp
@@ -0,0 +1,227 @@
+/* Copyright (c) 2022 Skyward Experimental Rocketry
+ * Author: Davide Mor
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#include "SX1278Common.h"
+
+#include <kernel/scheduler/scheduler.h>
+
+namespace Boardcore
+{
+
+namespace SX1278
+{
+
+void SX1278Common::handleDioIRQ(Dio dio)
+{
+    if (state.waiting_dio_mask.test(dio) && state.irq_wait_thread)
+    {
+        state.irq_wait_thread->IRQwakeup();
+        if (state.irq_wait_thread->IRQgetPriority() >
+            miosix::Thread::IRQgetCurrentThread()->IRQgetPriority())
+        {
+            miosix::Scheduler::IRQfindNextThread();
+        }
+
+        state.irq_wait_thread = nullptr;
+    }
+}
+
+void SX1278Common::setDefaultMode(Mode mode, DioMapping mapping,
+                                  bool tx_frontend, bool rx_frontend)
+{
+    mutex.lock();
+    enterMode(mode, mapping, tx_frontend, rx_frontend);
+    mutex.unlock();
+}
+
+void SX1278Common::waitForIrq(LockMode &_guard, IrqFlags irq, bool unlock)
+{
+    // Take a reference to a _guard to MAKE SURE that the mutex is locked, but
+    // otherwise don't do anything with it
+    (void)_guard;
+
+    // Convert the IRQ mask into a DIO mask
+    DioMask waiting_dio_mask =
+        getDioMaskFromIrqFlags(irq, state.mode, state.mapping);
+
+    do
+    {
+        // An interrupt could occur and read from this variables
+        {
+            miosix::FastInterruptDisableLock dLock;
+            state.waiting_dio_mask = waiting_dio_mask;
+            state.irq_wait_thread  = miosix::Thread::IRQgetCurrentThread();
+        }
+
+        // Check that this hasn't already happened
+        if (checkForIrqAndReset(irq))
+        {
+            return;
+        }
+
+        if (unlock)
+        {
+            mutex.unlock();
+        }
+
+        {
+            miosix::FastInterruptDisableLock dLock;
+            while (state.irq_wait_thread)
+            {
+                miosix::Thread::IRQwait();
+                {
+                    miosix::FastInterruptEnableLock eLock(dLock);
+                    miosix::Thread::yield();
+                }
+            }
+        }
+
+        // Regain ownership of the lock
+        if (unlock)
+        {
+            mutex.lock();
+        }
+
+        // Protect against sporadic IRQs
+    } while (!checkForIrqAndReset(irq));
+}
+
+bool SX1278Common::waitForIrqBusy(LockMode &_guard, IrqFlags irq, int timeout)
+{
+    // Take a reference to a _guard to MAKE SURE that the mutex is locked, but
+    // otherwise don't do anything with it
+    (void)_guard;
+
+    long long start = miosix::getTick();
+
+    while ((miosix::getTick() - start) < timeout)
+    {
+        // Delay between polls
+        const unsigned int DELAY = 100;
+
+        // Tight loop on IRQ register
+        for (unsigned int i = 0; i < 1000 / DELAY; i++)
+        {
+            if (checkForIrqAndReset(irq))
+            {
+                return true;
+            }
+
+            miosix::delayUs(DELAY);
+        }
+    }
+
+    return false;
+}
+
+bool SX1278Common::checkForIrqAndReset(IrqFlags irq)
+{
+    IrqFlags cur_irq = getIrqFlags();
+    if (cur_irq & irq)
+    {
+        // Reset all of the interrupts we have detected
+        resetIrqFlags(cur_irq & irq);
+
+        return true;
+    }
+    else
+    {
+        return false;
+    }
+}
+
+SX1278Common::DeviceState SX1278Common::lockMode(Mode mode, DioMapping mapping,
+                                                 bool tx_frontend,
+                                                 bool rx_frontend)
+{
+    // Store previous state
+    DeviceState old_state = state;
+
+    enterMode(mode, mapping, tx_frontend, rx_frontend);
+    state.irq_wait_thread  = nullptr;
+    state.waiting_dio_mask = DioMask();
+
+    return old_state;
+}
+
+void SX1278Common::unlockMode(DeviceState old_state)
+{
+    // Do this copy manually, we want stuff to be copied in a specific order
+    state.irq_wait_thread  = old_state.irq_wait_thread;
+    state.waiting_dio_mask = old_state.waiting_dio_mask;
+    enterMode(old_state.mode, old_state.mapping, old_state.is_tx_frontend_on,
+              old_state.is_rx_frontend_on);
+}
+
+void SX1278Common::lock() { mutex.lock(); }
+
+void SX1278Common::unlock() { mutex.unlock(); }
+
+void SX1278Common::enterMode(Mode mode, DioMapping mapping,
+                             bool set_tx_frontend_on, bool set_rx_frontend_on)
+{
+    // disable - enable in order to avoid having both RX/TX frontends active at
+    // the same time
+
+    // First disable all of the frontend if necessary
+    if (set_tx_frontend_on != state.is_tx_frontend_on && !set_tx_frontend_on)
+    {
+        disableTxFrontend();
+    }
+
+    if (set_rx_frontend_on != state.is_rx_frontend_on && !set_rx_frontend_on)
+    {
+        disableRxFrontend();
+    }
+
+    // Then enable the newly requested ones
+    if (set_tx_frontend_on != state.is_tx_frontend_on && set_tx_frontend_on)
+    {
+        enableTxFrontend();
+    }
+
+    if (set_rx_frontend_on != state.is_rx_frontend_on && set_rx_frontend_on)
+    {
+        enableRxFrontend();
+    }
+
+    state.is_tx_frontend_on = set_tx_frontend_on;
+    state.is_rx_frontend_on = set_rx_frontend_on;
+
+    // Check if necessary
+    if (mode != state.mode)
+    {
+        setMode(mode);
+        state.mode = mode;
+    }
+
+    // Finally setup DIO mapping
+    if (mapping != state.mapping)
+    {
+        setMapping(mapping);
+        state.mapping = mapping;
+    }
+}
+
+}  // namespace SX1278
+
+}  // namespace Boardcore
diff --git a/src/shared/radio/SX1278/SX1278Common.h b/src/shared/radio/SX1278/SX1278Common.h
new file mode 100644
index 0000000000000000000000000000000000000000..0fee45b8cec93663327b98368a6d2a4581a9485c
--- /dev/null
+++ b/src/shared/radio/SX1278/SX1278Common.h
@@ -0,0 +1,201 @@
+/* Copyright (c) 2022 Skyward Experimental Rocketry
+ * Author: Davide Mor
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#pragma once
+
+#include <drivers/spi/SPIDriver.h>
+#include <miosix.h>
+#include <radio/Transceiver.h>
+
+#include "SX1278Defs.h"
+
+namespace Boardcore
+{
+
+namespace SX1278
+{
+
+/**
+ * @brief Represents a set of Dio
+ */
+class DioMask
+{
+public:
+    DioMask() : mask(0) {}
+
+    bool test(Dio dio) const
+    {
+        return (mask & (1 << static_cast<int>(dio))) != 0;
+    }
+    void set(Dio dio) { mask |= (1 << static_cast<int>(dio)); }
+    void reset(Dio dio) { mask &= ~(1 << static_cast<int>(dio)); }
+
+private:
+    uint8_t mask;
+};
+
+using DioMapping = RegDioMapping::Mapping;
+
+/**
+ * @brief Shared interface between all SX1278 drivers
+ */
+class ISX1278 : public Transceiver
+{
+protected:
+    /*
+     * Stuff used internally by SX1278Common
+     */
+
+    using IrqFlags = int;
+    using Mode     = int;
+
+    virtual void setMode(Mode mode)             = 0;
+    virtual void setMapping(DioMapping mapping) = 0;
+
+    virtual IrqFlags getIrqFlags()             = 0;
+    virtual void resetIrqFlags(IrqFlags flags) = 0;
+
+    virtual DioMask getDioMaskFromIrqFlags(IrqFlags flags, Mode mode,
+                                           DioMapping mapping) = 0;
+
+    virtual void enableRxFrontend()  = 0;
+    virtual void disableRxFrontend() = 0;
+    virtual void enableTxFrontend()  = 0;
+    virtual void disableTxFrontend() = 0;
+};
+
+class SX1278Common : public ISX1278
+{
+private:
+    struct DeviceState
+    {
+        // Current device mode (dummy number to signal no mode)
+        Mode mode = -1;
+        // Current Dio mapping
+        DioMapping mapping = DioMapping();
+        // Thread waiting listening for interrupts
+        miosix::Thread *irq_wait_thread = nullptr;
+        // What DIOs are we waiting on
+        DioMask waiting_dio_mask = DioMask();
+        // True if the RX frontend is enabled
+        bool is_rx_frontend_on = false;
+        // True if the TX frontend is enabled
+        bool is_tx_frontend_on = false;
+    };
+
+public:
+    using Dio = SX1278::Dio;
+
+    /**
+     * @brief Handle generic DIO irq.
+     */
+    void handleDioIRQ(Dio dio);
+
+protected:
+    explicit SX1278Common(SPISlave slave) : slave(slave) {}
+
+    /**
+     * @brief RAII scoped bus lock guard.
+     */
+    class Lock
+    {
+    public:
+        explicit Lock(SX1278Common &driver) : driver(driver) { driver.lock(); }
+
+        ~Lock() { driver.unlock(); }
+
+    private:
+        SX1278Common &driver;
+    };
+
+    /**
+     * @brief RAII scoped mode lock, requires a previous lock.
+     */
+    class LockMode
+    {
+    public:
+        LockMode(SX1278Common &driver, Lock &lock, Mode mode,
+                 DioMapping mapping, bool set_tx_frontend_on = false,
+                 bool set_rx_frontend_on = false)
+            : driver(driver), lock(lock)
+        {
+            // cppcheck-suppress useInitializationList
+            old_state = driver.lockMode(mode, mapping, set_tx_frontend_on,
+                                        set_rx_frontend_on);
+        }
+
+        ~LockMode() { driver.unlockMode(old_state); }
+
+    private:
+        SX1278Common &driver;
+        Lock &lock;
+        DeviceState old_state;
+    };
+
+    /**
+     * @brief Set default device mode.
+     *
+     * WARNING: This will lock the mutex.
+     */
+    void setDefaultMode(Mode mode, DioMapping mapping, bool set_tx_frontend_on,
+                        bool set_rx_frontend_on);
+
+    /**
+     * @brief Wait for generic irq.
+     */
+    void waitForIrq(LockMode &guard, IrqFlags irq, bool unlock = false);
+
+    /**
+     * @brief Busy waits for an interrupt by polling the irq register.
+     *
+     * USE ONLY DURING INITIALIZATION! BAD THINGS *HAVE* HAPPENED DUE TO THIS!
+     */
+    bool waitForIrqBusy(LockMode &guard, IrqFlags irq, int timeout);
+
+    /**
+     * @brief Returns if an interrupt happened, and clears it if it did.
+     */
+    bool checkForIrqAndReset(IrqFlags irq);
+
+    /**
+     * @brief The actual SPISlave, used by child classes.
+     */
+    SPISlave slave;
+
+private:
+    DeviceState lockMode(Mode mode, DioMapping mapping, bool set_tx_frontend_on,
+                         bool set_rx_frontend_on);
+    void unlockMode(DeviceState old_state);
+
+    void lock();
+    void unlock();
+
+    void enterMode(Mode mode, DioMapping mapping, bool set_tx_frontend_on,
+                   bool set_rx_frontend_on);
+
+    miosix::FastMutex mutex;
+    DeviceState state;
+};
+
+}  // namespace SX1278
+
+}  // namespace Boardcore
diff --git a/src/shared/radio/SX1278/SX1278Defs.h b/src/shared/radio/SX1278/SX1278Defs.h
index f91fc4dba33817b6ab75ad3a26259e41348365e1..e524279a806bd3685b2825f2393401a1ab2f4681 100644
--- a/src/shared/radio/SX1278/SX1278Defs.h
+++ b/src/shared/radio/SX1278/SX1278Defs.h
@@ -1,4 +1,4 @@
-/* Copyright (c) 2021 Skyward Experimental Rocketry
+/* Copyright (c) 2022 Skyward Experimental Rocketry
  * Author: Davide Mor
  *
  * Permission is hereby granted, free of charge, to any person obtaining a copy
@@ -22,22 +22,14 @@
 
 #pragma once
 
-#include <drivers/spi/SPIDriver.h>
+#include <cstdint>
 
 namespace Boardcore
 {
 
-/**
- * @brief Various SX1278 register/enums definitions.
- */
-namespace SX1278Defs
+namespace SX1278
 {
 
-/**
- * @brief Length of the internal FIFO
- */
-constexpr int FIFO_LEN = 64;
-
 /**
  * @brief Main oscillator frequency (Hz)
  */
@@ -48,26 +40,84 @@ constexpr int FXOSC = 32000000;
  */
 constexpr float FSTEP = 61.03515625;
 
-constexpr int TS_OSC = 250;
-constexpr int TS_FS  = 60;
-
 /**
- * @brief Get required spi config
+ * @brief Represents a DIO..
  */
-inline SPIBusConfig spiConfig()
-{
-    SPIBusConfig config = {};
+enum class Dio
+{
+    DIO0 = 0,
+    DIO1 = 1,
+    DIO2 = 2,
+    DIO3 = 3,
+    DIO4 = 4,
+    DIO5 = 5
+};
 
-    // FIXME(davide.mor): This depends on the device
-    config.clockDivider = SPI::ClockDivider::DIV_64;
-    config.mode         = SPI::Mode::MODE_0;
-    config.bitOrder     = SPI::BitOrder::MSB_FIRST;
-    // config.cs_setup_time_us = 30;
-    // config.cs_hold_time_us  = 100;
+namespace RegDioMapping
+{
 
-    return config;
+inline constexpr uint16_t make(int dio0, int dio1, int dio2, int dio3, int dio4,
+                               int dio5, bool map_preamble_detect)
+{
+    return (((dio0 & 0b11) << 14) | ((dio1 & 0b11) << 12) |
+            ((dio2 & 0b11) << 10) | ((dio3 & 0b11) << 8) |
+            ((dio4 & 0b11) << 6) | ((dio5 & 0b11) << 4) |
+            (map_preamble_detect ? 1 : 0));
 }
 
+/**
+ * @brief Represents an actual Dio mapping..
+ */
+struct Mapping
+{
+    constexpr Mapping() : raw(0) {}
+    constexpr Mapping(int dio0, int dio1, int dio2, int dio3, int dio4,
+                      int dio5, bool map_preamble_detect = false)
+        : raw(make(dio0, dio1, dio2, dio3, dio4, dio5, map_preamble_detect))
+    {
+    }
+
+    int getMapping(Dio dio) const
+    {
+        switch (dio)
+        {
+            case Dio::DIO0:
+                return (raw >> 14) & 0b11;
+            case Dio::DIO1:
+                return (raw >> 12) & 0b11;
+            case Dio::DIO2:
+                return (raw >> 10) & 0b11;
+            case Dio::DIO3:
+                return (raw >> 8) & 0b11;
+            case Dio::DIO4:
+                return (raw >> 6) & 0b11;
+            case Dio::DIO5:
+                return (raw >> 4) & 0b11;
+            default:
+                return 0;
+        }
+    }
+
+    bool operator==(const Mapping &other) const { return raw == other.raw; }
+
+    bool operator!=(const Mapping &other) const { return raw != other.raw; }
+
+    uint16_t raw;
+};
+
+}  // namespace RegDioMapping
+
+/**
+ * @brief Definitions only valid for Fsk
+ */
+namespace Fsk
+{
+
+/**
+ * @brief Length of the internal FIFO
+ */
+constexpr int FIFO_LEN = 64;
+
 namespace RegOpMode
 {
 
@@ -94,6 +144,7 @@ enum Mode
     MODE_FSRX  = 0b100,
     MODE_RX    = 0b101,
 };
+
 }  // namespace RegOpMode
 
 namespace RegPaConfig
@@ -214,22 +265,26 @@ enum TxStartCondition
 
 namespace RegIrqFlags
 {
-constexpr uint16_t MODE_READY         = 1 << 7;
-constexpr uint16_t RX_READY           = 1 << 6;
-constexpr uint16_t TX_READY           = 1 << 5;
-constexpr uint16_t PILL_LOCK          = 1 << 4;
-constexpr uint16_t RSSI               = 1 << 3;
-constexpr uint16_t TIMEOUT            = 1 << 2;
-constexpr uint16_t PREAMBLE_DETECT    = 1 << 1;
-constexpr uint16_t SYNC_ADDRESS_MATCH = 1 << 0;
-constexpr uint16_t FIFO_FULL          = 1 << 15;
-constexpr uint16_t FIFO_EMPTY         = 1 << 14;
-constexpr uint16_t FIFO_LEVEL         = 1 << 13;
-constexpr uint16_t FIFO_OVERRUN       = 1 << 12;
-constexpr uint16_t PACKET_SENT        = 1 << 11;
-constexpr uint16_t PAYLOAD_READY      = 1 << 10;
-constexpr uint16_t CRC_OK             = 1 << 9;
-constexpr uint16_t LOW_BAT            = 1 << 8;
+
+enum IrqFlags
+{
+    MODE_READY         = 1 << 15,
+    RX_READY           = 1 << 14,
+    TX_READY           = 1 << 13,
+    PILL_LOCK          = 1 << 12,
+    RSSI               = 1 << 11,
+    TIMEOUT            = 1 << 10,
+    PREAMBLE_DETECT    = 1 << 9,
+    SYNC_ADDRESS_MATCH = 1 << 8,
+    FIFO_FULL          = 1 << 7,
+    FIFO_EMPTY         = 1 << 6,
+    FIFO_LEVEL         = 1 << 5,
+    FIFO_OVERRUN       = 1 << 4,
+    PACKET_SENT        = 1 << 3,
+    PAYLOAD_READY      = 1 << 2,
+    CRC_OK             = 1 << 1,
+    LOW_BAT            = 1 << 0,
+};
 
 }  // namespace RegIrqFlags
 
@@ -355,22 +410,413 @@ enum Registers
     REG_AGC_THRESH_1 = 0x62,
     REG_AGC_THRESH_2 = 0x63,
     REG_AGC_THRESH_3 = 0x64,
+    REG_AGC_PILL     = 0x70,
+};
+
+static constexpr int DIO_MAPPINGS[6][8][4] =
+    {[0] =
+         {
+             [RegOpMode::MODE_SLEEP] = {0, 0, 0, 0},
+             [RegOpMode::MODE_STDBY] = {0, 0, 0, RegIrqFlags::LOW_BAT},
+             [RegOpMode::MODE_FSTX]  = {0, 0, 0, RegIrqFlags::LOW_BAT},
+             [RegOpMode::MODE_TX]    = {RegIrqFlags::PACKET_SENT, 0, 0,
+                                        RegIrqFlags::LOW_BAT},
+             [RegOpMode::MODE_FSRX]  = {0, 0, 0, RegIrqFlags::LOW_BAT},
+             [RegOpMode::MODE_RX] =
+                 {RegIrqFlags::PAYLOAD_READY, RegIrqFlags::CRC_OK, 0,
+                  RegIrqFlags::LOW_BAT},
+         },
+     [1] =
+         {
+             [RegOpMode::MODE_SLEEP] = {RegIrqFlags::FIFO_LEVEL,
+                                        RegIrqFlags::FIFO_EMPTY,
+                                        RegIrqFlags::FIFO_FULL, 0},
+             [RegOpMode::MODE_STDBY] = {RegIrqFlags::FIFO_LEVEL,
+                                        RegIrqFlags::FIFO_EMPTY,
+                                        RegIrqFlags::FIFO_FULL, 0},
+             [RegOpMode::MODE_FSTX] =
+                 {RegIrqFlags::FIFO_LEVEL, RegIrqFlags::FIFO_EMPTY,
+                  RegIrqFlags::FIFO_FULL, 0},
+             [RegOpMode::MODE_TX] = {RegIrqFlags::FIFO_LEVEL,
+                                     RegIrqFlags::FIFO_EMPTY,
+                                     RegIrqFlags::FIFO_FULL, 0},
+             [RegOpMode::MODE_FSRX] =
+                 {RegIrqFlags::FIFO_LEVEL, RegIrqFlags::FIFO_EMPTY,
+                  RegIrqFlags::FIFO_FULL, 0},
+             [RegOpMode::MODE_RX] = {RegIrqFlags::FIFO_LEVEL,
+                                     RegIrqFlags::FIFO_EMPTY,
+                                     RegIrqFlags::FIFO_FULL, 0},
+         },
+     [2] =
+         {
+             [RegOpMode::MODE_SLEEP] =
+                 {RegIrqFlags::FIFO_FULL, 0, RegIrqFlags::FIFO_FULL,
+                  RegIrqFlags::FIFO_FULL},
+             [RegOpMode::MODE_STDBY] =
+                 {RegIrqFlags::FIFO_FULL, 0, RegIrqFlags::FIFO_FULL,
+                  RegIrqFlags::FIFO_FULL},
+             [RegOpMode::MODE_FSTX] = {RegIrqFlags::FIFO_FULL, 0,
+                                       RegIrqFlags::FIFO_FULL,
+                                       RegIrqFlags::FIFO_FULL},
+             [RegOpMode::MODE_TX] =
+                 {RegIrqFlags::FIFO_FULL, 0, RegIrqFlags::FIFO_FULL,
+                  RegIrqFlags::FIFO_FULL},
+             [RegOpMode::MODE_FSRX] = {RegIrqFlags::FIFO_FULL, 0,
+                                       RegIrqFlags::FIFO_FULL,
+                                       RegIrqFlags::FIFO_FULL},
+             [RegOpMode::MODE_RX] =
+                 {RegIrqFlags::FIFO_FULL, RegIrqFlags::RX_READY,
+                  RegIrqFlags::TIMEOUT, RegIrqFlags::SYNC_ADDRESS_MATCH},
+         },
+     [3] =
+         {
+             [RegOpMode::MODE_SLEEP] =
+                 {
+                     RegIrqFlags::FIFO_EMPTY,
+                     0,
+                     RegIrqFlags::FIFO_EMPTY,
+                     RegIrqFlags::FIFO_EMPTY,
+                 },
+             [RegOpMode::MODE_STDBY] =
+                 {
+                     RegIrqFlags::FIFO_EMPTY,
+                     0,
+                     RegIrqFlags::FIFO_EMPTY,
+                     RegIrqFlags::FIFO_EMPTY,
+                 },
+             [RegOpMode::MODE_FSTX] =
+                 {
+                     RegIrqFlags::FIFO_EMPTY,
+                     0,
+                     RegIrqFlags::FIFO_EMPTY,
+                     RegIrqFlags::FIFO_EMPTY,
+                 },
+             [RegOpMode::MODE_TX] =
+                 {
+                     RegIrqFlags::FIFO_EMPTY,
+                     RegIrqFlags::TX_READY,
+                     RegIrqFlags::FIFO_EMPTY,
+                     RegIrqFlags::FIFO_EMPTY,
+                 },
+             [RegOpMode::MODE_FSRX] =
+                 {
+                     RegIrqFlags::FIFO_EMPTY,
+                     0,
+                     RegIrqFlags::FIFO_EMPTY,
+                     RegIrqFlags::FIFO_EMPTY,
+                 },
+             [RegOpMode::MODE_RX] =
+                 {
+                     RegIrqFlags::FIFO_EMPTY,
+                     0,
+                     RegIrqFlags::FIFO_EMPTY,
+                     RegIrqFlags::FIFO_EMPTY,
+                 },
+         },
+     [4] =
+         {
+             [RegOpMode::MODE_SLEEP] = {0, 0, 0, 0},
+             [RegOpMode::MODE_STDBY] = {RegIrqFlags::LOW_BAT, 0, 0, 0},
+             [RegOpMode::MODE_FSTX]  = {RegIrqFlags::LOW_BAT,
+                                        RegIrqFlags::PILL_LOCK, 0, 0},
+             [RegOpMode::MODE_TX]    = {RegIrqFlags::LOW_BAT,
+                                        RegIrqFlags::PILL_LOCK, 0, 0},
+             [RegOpMode::MODE_FSRX]  = {RegIrqFlags::LOW_BAT,
+                                        RegIrqFlags::PILL_LOCK, 0, 0},
+             [RegOpMode::MODE_RX] =
+                 {RegIrqFlags::LOW_BAT, RegIrqFlags::PILL_LOCK,
+                  RegIrqFlags::TIMEOUT,
+                  RegIrqFlags::RSSI | RegIrqFlags::PREAMBLE_DETECT},
+         },
+     [5] = {
+         [RegOpMode::MODE_SLEEP] = {0, 0, 0, 0},
+         [RegOpMode::MODE_STDBY] = {0, 0, 0, RegIrqFlags::MODE_READY},
+         [RegOpMode::MODE_FSTX]  = {0, RegIrqFlags::PILL_LOCK, 0,
+                                    RegIrqFlags::MODE_READY},
+
+         [RegOpMode::MODE_TX]   = {0, RegIrqFlags::PILL_LOCK, 0,
+                                   RegIrqFlags::MODE_READY},
+         [RegOpMode::MODE_FSRX] = {0, RegIrqFlags::PILL_LOCK, 0,
+                                   RegIrqFlags::MODE_READY},
+         [RegOpMode::MODE_RX]   = {0, RegIrqFlags::PILL_LOCK, 0,
+                                   RegIrqFlags::MODE_READY},
+     }};
+}  // namespace Fsk
+
+/**
+ * @brief Definitions only valid for LoRa
+ */
+namespace Lora
+{
+
+namespace RegOpMode
+{
+
+enum Mode
+{
+    MODE_SLEEP        = 0b000,
+    MODE_STDBY        = 0b001,
+    MODE_FSTX         = 0b010,
+    MODE_TX           = 0b011,
+    MODE_FSRX         = 0b100,
+    MODE_RXCONTINUOUS = 0b101,
+    MODE_RXSINGLE     = 0b110,
+    MODE_CAD          = 0b111
+};
+
+inline constexpr uint8_t make(Mode mode, bool low_frequency_mode_on,
+                              bool access_shared_reg)
+{
+    return mode | (low_frequency_mode_on ? (1 << 3) : 0) |
+           (access_shared_reg ? (1 << 6) : 0) | (1 << 7);
+}
+
+}  // namespace RegOpMode
+
+namespace RegPaConfig
+{
+
+inline constexpr uint8_t make(uint8_t output_power, uint8_t max_power,
+                              bool pa_select)
+{
+    return (output_power & 0b1111) | ((max_power & 0b111) << 4) |
+           (pa_select ? 1 << 7 : 0);
+}
+
+}  // namespace RegPaConfig
+
+namespace RegOcp
+{
+
+inline constexpr uint8_t make(uint8_t ocp_trim, bool ocp_on)
+{
+    return (ocp_trim & 0b11111) | (ocp_on ? 1 << 5 : 0);
+}
+
+}  // namespace RegOcp
+
+namespace RegIrqFlags
+{
+
+enum IrqFlags
+{
+    RX_TIMEOUT          = 1 << 7,
+    RX_DONE             = 1 << 6,
+    PAYLOAD_CRC_ERROR   = 1 << 5,
+    VALID_HEADER        = 1 << 4,
+    TX_DONE             = 1 << 3,
+    CAD_DONE            = 1 << 2,
+    FHSS_CHANGE_CHANNEL = 1 << 1,
+    CAD_DETECTED        = 1 << 0,
+};
+
+}
+
+namespace RegModemConfig1
+{
+
+enum Bw
+{
+    BW_HZ_7800   = 0b0000,
+    BW_HZ_10400  = 0b0001,
+    BW_HZ_15600  = 0b0010,
+    BW_HZ_20800  = 0b0011,
+    BW_HZ_31250  = 0b0100,
+    BW_HZ_41700  = 0b0101,
+    BW_HZ_62500  = 0b0110,
+    BW_HZ_125000 = 0b0111,
+    BW_HZ_250000 = 0b1000,
+    BW_HZ_500000 = 0b1001,
+};
+
+inline constexpr uint32_t bandwidthToInt(Bw bw)
+{
+    switch (bw)
+    {
+        case RegModemConfig1::BW_HZ_7800:
+            return 7800;
+        case RegModemConfig1::BW_HZ_10400:
+            return 10400;
+        case RegModemConfig1::BW_HZ_15600:
+            return 15600;
+        case RegModemConfig1::BW_HZ_20800:
+            return 20800;
+        case RegModemConfig1::BW_HZ_31250:
+            return 31250;
+        case RegModemConfig1::BW_HZ_41700:
+            return 41700;
+        case RegModemConfig1::BW_HZ_62500:
+            return 62500;
+        case RegModemConfig1::BW_HZ_125000:
+            return 125000;
+        case RegModemConfig1::BW_HZ_250000:
+            return 250000;
+        case RegModemConfig1::BW_HZ_500000:
+            return 500000;
+    }
+
+    // Gcc complains...
+    return 0;
+}
+
+enum Cr
+{
+    CR_1 = 0b001,
+    CR_2 = 0b010,
+    CR_3 = 0b011,
+    CR_4 = 0b100
+};
+
+inline constexpr uint8_t make(bool implicit_mode_on, Cr coding_rate, Bw bw)
+{
+    return (implicit_mode_on ? 1 : 0) | (coding_rate << 1) | (bw << 4);
+}
+
+}  // namespace RegModemConfig1
+
+namespace RegModemConfig2
+{
+
+enum Sf
+{
+    SF_6  = 6,
+    SF_7  = 7,
+    SF_8  = 8,
+    SF_9  = 9,
+    SF_10 = 10,
+    SF_11 = 11,
+    SF_12 = 12,
+};
+
+inline constexpr uint8_t make(bool rx_payload_crc_on, bool tx_continuous_mode,
+                              Sf spreading_factor)
+{
+    return (rx_payload_crc_on ? 1 << 2 : 0) |
+           (tx_continuous_mode ? 1 << 3 : 0) | (spreading_factor << 4);
+}
+
+}  // namespace RegModemConfig2
+
+namespace RegModemConfig3
+{
+
+inline constexpr uint8_t make(bool agc_auto_on, bool low_data_rate_optimize)
+{
+    return (agc_auto_on ? 1 << 2 : 0) | (low_data_rate_optimize ? 1 << 3 : 0);
+}
+
+}  // namespace RegModemConfig3
+
+namespace RegDetectOptimize
+{
+
+inline constexpr uint8_t make(uint8_t detection_optimize, bool automatic_if_on)
+{
+    return (detection_optimize & 0b11) | (automatic_if_on ? 1 << 7 : 0);
+}
+
+}  // namespace RegDetectOptimize
+
+namespace RegPaDac
+{
+
+inline constexpr uint8_t make(bool pa_boost)
+{
+    return (pa_boost ? 0x07 : 0x04) | (0x10 << 3);
+}
+
+}  // namespace RegPaDac
+
+enum Registers
+{
+    REG_FIFO = 0x00,
+
+    // Registers for common settings
+    REG_OP_MODE = 0x01,
+    REG_FRF_MSB = 0x06,
+    REG_FRF_MID = 0x07,
+    REG_FRF_LSB = 0x08,
+
+    // Registers for RF blocks
+    REG_PA_CONFIG = 0x09,
+    REG_PA_RAMP   = 0x0a,
+    REG_OCP       = 0x0b,
+    REG_LNA       = 0x0c,
+
+    // Lora page registers
+    REG_FIFO_ADDR_PTR           = 0x0d,
+    REG_FIFO_TX_BASE_ADDR       = 0x0e,
+    REG_FIFO_RX_BASE_ADDR       = 0x0f,
+    REG_FIFO_RX_CURRENT_ADDR    = 0x10,
+    REG_IRQ_FLAGS_MASK          = 0x11,
+    REG_IRQ_FLAGS               = 0x12,
+    REG_RX_NB_BYTES             = 0x13,
+    REG_RX_HEADER_CNT_VALUE_MSB = 0x14,
+    REG_RX_HEADER_CNT_VALUE_LSB = 0x15,
+    REG_RX_PACKET_CNT_VALUE_MSB = 0x16,
+    REG_RX_PACKET_CNT_VALUE_LSB = 0x17,
+    REG_MODEM_STAT              = 0x18,
+    REG_PKT_SNR_VALUE           = 0x19,
+    REG_PKT_RSSI_VALUE          = 0x1a,
+    REG_RSSI_VALUE              = 0x1b,
+    REG_HOP_CHANNEL             = 0x1c,
+    REG_MODEM_CONFIG_1          = 0x1d,
+    REG_MODEM_CONFIG_2          = 0x1e,
+    REG_SYMB_TIMEOUT_LSB        = 0x1f,
+    REG_PREAMBLE_MSB            = 0x20,
+    REG_PREAMBLE_LSB            = 0x21,
+    REG_PAYLOAD_LENGTH          = 0x22,
+    REG_MAX_PAYLOAD_LENGTH      = 0x23,
+    REG_HOP_PERIOD              = 0x24,
+    REG_FIFO_RX_BYTE_ADDR       = 0x25,
+    REG_MODEM_CONFIG_3          = 0x26,
+    REG_PPM_CORRECTION          = 0x27,
+    REG_FEI_MSB                 = 0x28,
+    REG_FEI_MID                 = 0x29,
+    REG_FEI_LSB                 = 0x2a,
+    REG_RSSI_WIDEBAND           = 0x2c,
+    REG_IF_FREQ_2               = 0x2f,
+    REG_IF_FREQ_1               = 0x30,
+    REG_DETECT_OPTIMIZE         = 0x31,
+    REG_INVERT_IQ               = 0x33,
+    REG_HIGH_BW_OPTIMIZE_1      = 0x36,
+    REG_DETECTION_THRESHOLD     = 0x37,
+    REG_SYNC_WORD               = 0x39,
+    REG_HIGH_BW_OPTIMIZE_2      = 0x3a,
+    REG_INVERT_IQ_2             = 0x3b,
+
+    // IO Control registers
+    REG_DIO_MAPPING_1 = 0x40,
+    REG_DIO_MAPPING_2 = 0x41,
+
+    // Version register
+    REG_VERSION = 0x42,
 
-    // Low frequency additional registers
-    REG_AGC_REF_LF      = 0x61,
-    REG_AGC_THRESH_1_LF = 0x62,
-    REG_AGC_THRESH_2_LF = 0x63,
-    REG_AGC_THRESH_3_LF = 0x61,
-    REG_AGC_PILL_LF     = 0x70,
-
-    // High frequency additional registers
-    REG_AGC_REF_HF      = 0x61,
-    REG_AGC_THRESH_1_HF = 0x62,
-    REG_AGC_THRESH_2_HF = 0x63,
-    REG_AGC_THRESH_3_HF = 0x61,
-    REG_AGC_PILL_HF     = 0x70,
+    // Additional registers
+    REG_TCXO         = 0x4b,
+    REG_PA_DAC       = 0x4d,
+    REG_FORMER_TEMP  = 0x5b,
+    REG_AGC_REF      = 0x61,
+    REG_AGC_THRESH_1 = 0x62,
+    REG_AGC_THRESH_2 = 0x63,
+    REG_AGC_THRESH_3 = 0x64,
+    REG_AGC_PILL     = 0x70,
 };
 
-}  // namespace SX1278Defs
+static constexpr int DIO_MAPPINGS[6][4] = {
+    [0] = {RegIrqFlags::RX_DONE, RegIrqFlags::TX_DONE, RegIrqFlags::CAD_DONE,
+           0},
+    [1] = {RegIrqFlags::RX_TIMEOUT, RegIrqFlags::FHSS_CHANGE_CHANNEL,
+           RegIrqFlags::CAD_DETECTED, 0},
+    [2] = {RegIrqFlags::FHSS_CHANGE_CHANNEL, RegIrqFlags::FHSS_CHANGE_CHANNEL,
+           RegIrqFlags::FHSS_CHANGE_CHANNEL, 0},
+    [3] = {RegIrqFlags::CAD_DONE, RegIrqFlags::VALID_HEADER,
+           RegIrqFlags::PAYLOAD_CRC_ERROR, 0},
+    [4] = {RegIrqFlags::CAD_DETECTED, 0, 0, 0},
+    [5] = {0, 0, 0, 0}};
+
+}  // namespace Lora
+
+}  // namespace SX1278
 
 }  // namespace Boardcore
diff --git a/src/shared/radio/SX1278/SX1278.cpp b/src/shared/radio/SX1278/SX1278Fsk.cpp
similarity index 63%
rename from src/shared/radio/SX1278/SX1278.cpp
rename to src/shared/radio/SX1278/SX1278Fsk.cpp
index dce36e6648a5acc20e682c5f18e579088fe334f7..d671006998140b115fff7302775ffbd4ae6cfe27 100644
--- a/src/shared/radio/SX1278/SX1278.cpp
+++ b/src/shared/radio/SX1278/SX1278Fsk.cpp
@@ -20,7 +20,7 @@
  * THE SOFTWARE.
  */
 
-#include "SX1278.h"
+#include "SX1278Fsk.h"
 
 #include <kernel/scheduler/scheduler.h>
 #include <utils/Debug.h>
@@ -30,7 +30,8 @@
 namespace Boardcore
 {
 
-using namespace SX1278Defs;
+using namespace SX1278;
+using namespace SX1278::Fsk;
 
 long long now() { return miosix::getTick() * 1000 / miosix::TICK_FREQ; }
 
@@ -45,157 +46,50 @@ constexpr uint8_t REG_SYNC_CONFIG_DEFAULT =
 
 constexpr uint8_t REG_IMAGE_CAL_DEFAULT = RegImageCal::TEMP_TRESHOLD_10DEG;
 
-SX1278BusManager::SX1278BusManager(SPIBusInterface &bus, miosix::GpioPin cs)
-    : slave(bus, cs, spiConfig()), mode(SX1278BusManager::Mode::MODE_SLEEP)
-{
-}
-
-void SX1278BusManager::lock() { mutex.lock(); }
-
-void SX1278BusManager::unlock() { mutex.unlock(); }
-
-void SX1278BusManager::lockMode(SX1278BusManager::Mode mode)
-{
-    mutex.lock();
-
-    enterMode(mode);
-    irq_wait_thread = nullptr;
-}
-
-void SX1278BusManager::unlockMode()
-{
-    if (rx_wait_thread != nullptr)
-    {
-        // Restore rx state
-        irq_wait_thread = rx_wait_thread;
-        enterMode(SX1278BusManager::Mode::MODE_RX);
-    }
-
-    mutex.unlock();
-}
-
-SPISlave &SX1278BusManager::getBus() { return slave; }
-
-void SX1278BusManager::handleDioIRQ()
-{
-    if (irq_wait_thread)
-    {
-        irq_wait_thread->IRQwakeup();
-        if (irq_wait_thread->IRQgetPriority() >
-            miosix::Thread::IRQgetCurrentThread()->IRQgetPriority())
-        {
-            miosix::Scheduler::IRQfindNextThread();
-        }
-
-        // Check if we woke the rx thread
-        if (irq_wait_thread == rx_wait_thread)
-            rx_wait_thread = nullptr;
-
-        irq_wait_thread = nullptr;
-    }
-}
+// Enable:
+// - PayloadReady, PacketSent on DIO0 (mode 00)
+// - FifoLevel on DIO1 (mode 00)
+// - TxReady on DIO3 (mode 01)
+constexpr DioMapping DEFAULT_MAPPING = DioMapping(0, 0, 0, 1, 0, 0, false);
 
-bool SX1278BusManager::waitForIrq(uint16_t mask, int timeout)
+SX1278Fsk::Error SX1278Fsk::init(const Config &config)
 {
-    long long start = miosix::getTick();
-    while ((miosix::getTick() - start) < timeout)
-    {
-        // Tight loop on IRQ register
-        for (int i = 0; i < 100; i++)
-        {
-            if (getIrqFlags() & mask)
-                return true;
-
-            miosix::delayUs(10);
-        }
-    }
-
-    return false;
-}
-
-void SX1278BusManager::waitForRxIrq()
-{
-    // NOTE: waitForRxIrq is only available for RX
-
-    // Check before entering irq mode
-    if (getIrqFlags() & RegIrqFlags::PAYLOAD_READY)
-        return;
-
-    miosix::FastInterruptDisableLock dLock;
-    irq_wait_thread = rx_wait_thread = miosix::Thread::getCurrentThread();
-    // Release lock to allow for writers
-    mutex.unlock();
-
-    // Avoid spurious wakeups
-    while (rx_wait_thread != 0)
+    // First probe for the device
+    if (!checkVersion())
     {
-        rx_wait_thread->IRQwait();
-        {
-            miosix::FastInterruptEnableLock eLock(dLock);
-            miosix::Thread::yield();
-        }
+        TRACE("[sx1278] Wrong chipid\n");
+        return Error::BAD_VALUE;
     }
 
-    // Regain ownership of the lock
-    mutex.lock();
-}
+    Error err;
+    if ((err = configure(config)) != Error::NONE)
+        return err;
 
-void SX1278BusManager::enterMode(Mode mode)
-{
-    // Check if necessary
-    if (mode == this->mode)
-        return;
-
-    setMode(mode);
-
-    // BUG: Removing the next line makes it, better?!?
-    // waitForIrq(RegIrqFlags::MODE_READY);
-
-    this->mode = mode;
+    return Error::NONE;
 }
 
-uint16_t SX1278BusManager::getIrqFlags()
+bool SX1278Fsk::checkVersion()
 {
-    SPITransaction spi(getBus(), SPITransaction::WriteBit::INVERTED);
-
-    uint8_t flags[2] = {0, 0};
-    spi.readRegisters(REG_IRQ_FLAGS_1, flags, 2);
+    Lock guard(*this);
+    SPITransaction spi(slave, SPITransaction::WriteBit::INVERTED);
 
-    return (flags[0] | flags[1] << 8);
+    return spi.readRegister(REG_VERSION) == 0x12;
 }
 
-void SX1278BusManager::setMode(Mode mode)
+SX1278Fsk::Error SX1278Fsk::configure(const Config &config)
 {
-    SPITransaction spi(getBus(), SPITransaction::WriteBit::INVERTED);
-    spi.writeRegister(REG_OP_MODE, REG_OP_MODE_DEFAULT | mode);
-}
-
-SX1278::SX1278(SPIBusInterface &bus, miosix::GpioPin cs) : bus_mgr(bus, cs) {}
-
-SX1278::Error SX1278::init(const Config &config)
-{
-    // First probe for the device
-    uint8_t version = getVersion();
-    if (version != 0x12)
-    {
-        TRACE("[sx1278] Wrong chipid: %d\n", version);
-        return Error::BAD_VALUE;
-    }
-
-    if (!configure(config))
-        return Error::CONFIGURE_FAILED;
-
-    return Error::NONE;
-}
-
-bool SX1278::probe() { return getVersion() == 0x12; }
+    // First cycle the device to bring it into Fsk mode (can only be changed in
+    // sleep)
+    setDefaultMode(RegOpMode::MODE_SLEEP, DEFAULT_MAPPING, false, false);
+    // Make sure the device remains in standby and not in sleep
+    setDefaultMode(RegOpMode::MODE_STDBY, DEFAULT_MAPPING, false, false);
 
-bool SX1278::configure(const Config &config)
-{
     // Lock the bus
-    SX1278BusManager::LockMode guard(bus_mgr, Mode::MODE_STDBY);
-    if (!bus_mgr.waitForIrq(RegIrqFlags::MODE_READY))
-        return false;
+    Lock guard(*this);
+    LockMode guard_mode(*this, guard, RegOpMode::MODE_STDBY, DEFAULT_MAPPING);
+
+    if (!waitForIrqBusy(guard_mode, RegIrqFlags::MODE_READY, 1000))
+        return Error::IRQ_TIMEOUT;
 
     setBitrate(config.bitrate);
     setFreqDev(config.freq_dev);
@@ -208,14 +102,13 @@ bool SX1278::configure(const Config &config)
 
     uint8_t sync_word[2] = {0x12, 0xad};
     setSyncWord(sync_word, 2);
-    setPreableLen(2);
-    setPa(config.power, true);
+    setPreambleLen(2);
+    setPa(config.power, config.pa_boost);
     setShaping(config.shaping);
 
     // Setup generic parameters
     {
-        SPITransaction spi(bus_mgr.getBus(),
-                           SPITransaction::WriteBit::INVERTED);
+        SPITransaction spi(slave, SPITransaction::WriteBit::INVERTED);
 
         spi.writeRegister(REG_RX_CONFIG,
                           RegRxConfig::AFC_AUTO_ON | RegRxConfig::AGC_AUTO_ON |
@@ -242,29 +135,27 @@ bool SX1278::configure(const Config &config)
             RegFifoThresh::TX_START_CONDITION_FIFO_NOT_EMPTY | 0x0f);
         spi.writeRegister(REG_NODE_ADRS, 0x00);
         spi.writeRegister(REG_BROADCAST_ADRS, 0x00);
-
-        // Enable PayloadReady, PacketSent on DIO0
-        spi.writeRegister(REG_DIO_MAPPING_1, 0x00);
     }
 
     // imageCalibrate();
-    return true;
+    return Error::NONE;
 }
 
-ssize_t SX1278::receive(uint8_t *pkt, size_t max_len)
+ssize_t SX1278Fsk::receive(uint8_t *pkt, size_t max_len)
 {
-    SX1278BusManager::LockMode guard(bus_mgr, Mode::MODE_RX);
+    Lock guard(*this);
+    LockMode guard_mode(*this, guard, RegOpMode::MODE_RX, DEFAULT_MAPPING,
+                        false, true);
 
     uint8_t len = 0;
     do
     {
         // Special wait for payload ready
-        bus_mgr.waitForRxIrq();
+        waitForIrq(guard_mode, RegIrqFlags::PAYLOAD_READY, true);
         last_rx_rssi = getRssi();
 
         {
-            SPITransaction spi(bus_mgr.getBus(),
-                               SPITransaction::WriteBit::INVERTED);
+            SPITransaction spi(slave, SPITransaction::WriteBit::INVERTED);
             len = spi.readRegister(REG_FIFO);
             if (len > max_len)
                 return -1;
@@ -278,55 +169,51 @@ ssize_t SX1278::receive(uint8_t *pkt, size_t max_len)
     return len;
 }
 
-bool SX1278::send(uint8_t *pkt, size_t len)
+bool SX1278Fsk::send(uint8_t *pkt, size_t len)
 {
     // Packets longer than FIFO_LEN (-1 for the len byte) are not supported
-    if (len > SX1278Defs::FIFO_LEN - 1)
+    if (len > MTU)
         return false;
 
     // This shouldn't be needed, but for some reason the device "lies" about
     // being ready, so lock up if we are going too fast
     rateLimitTx();
 
-    SX1278BusManager::LockMode guard(bus_mgr, Mode::MODE_TX);
+    Lock guard(*this);
+    LockMode guard_mode(*this, guard, RegOpMode::MODE_TX, DEFAULT_MAPPING, true,
+                        false);
 
-    // Wait for TX ready
-    if (!bus_mgr.waitForIrq(RegIrqFlags::TX_READY))
-        return false;
+    waitForIrq(guard_mode, RegIrqFlags::TX_READY);
 
     {
-        SPITransaction spi(bus_mgr.getBus(),
-                           SPITransaction::WriteBit::INVERTED);
+        SPITransaction spi(slave, SPITransaction::WriteBit::INVERTED);
 
         spi.writeRegister(REG_FIFO, static_cast<uint8_t>(len));
         spi.writeRegisters(REG_FIFO, pkt, len);
     }
 
     // Wait for packet sent
-    if (!bus_mgr.waitForIrq(RegIrqFlags::PACKET_SENT, len * 3))
-        return false;
+    waitForIrq(guard_mode, RegIrqFlags::PACKET_SENT);
 
     last_tx = now();
     return true;
 }
 
-void SX1278::handleDioIRQ() { bus_mgr.handleDioIRQ(); }
-
-float SX1278::getLastRxFei()
+float SX1278Fsk::getLastRxFei()
 {
-    SX1278BusManager::Lock guard(bus_mgr);
+    Lock guard(*this);
     return getFei();
 }
 
-float SX1278::getLastRxRssi() { return last_rx_rssi; }
+float SX1278Fsk::getLastRxRssi() { return last_rx_rssi; }
 
-float SX1278::getCurRssi()
+float SX1278Fsk::getCurRssi()
 {
-    SX1278BusManager::Lock guard(bus_mgr);
+    Lock guard(*this);
     return getRssi();
 }
 
-void SX1278::rateLimitTx()
+void SX1278Fsk::rateLimitTx()
 {
     const long long RATE_LIMIT = 2;
 
@@ -337,9 +224,9 @@ void SX1278::rateLimitTx()
     }
 }
 
-void SX1278::imageCalibrate()
+void SX1278Fsk::imageCalibrate()
 {
-    SPITransaction spi(bus_mgr.getBus(), SPITransaction::WriteBit::INVERTED);
+    SPITransaction spi(slave, SPITransaction::WriteBit::INVERTED);
     spi.writeRegister(REG_IMAGE_CAL, REG_IMAGE_CAL_DEFAULT | (1 << 6));
 
     // Wait for calibration complete by polling on running register
@@ -347,24 +234,67 @@ void SX1278::imageCalibrate()
         miosix::delayUs(10);
 }
 
-uint8_t SX1278::getVersion()
+DioMask SX1278Fsk::getDioMaskFromIrqFlags(IrqFlags flags, Mode mode,
+                                          DioMapping mapping)
 {
-    SX1278BusManager::Lock guard(bus_mgr);
-    SPITransaction spi(bus_mgr.getBus(), SPITransaction::WriteBit::INVERTED);
+    DioMask dio_mask;
+
+    if (DIO_MAPPINGS[0][mode][mapping.getMapping(Dio::DIO0)] & flags)
+        dio_mask.set(Dio::DIO0);
 
-    return spi.readRegister(REG_VERSION);
+    if (DIO_MAPPINGS[1][mode][mapping.getMapping(Dio::DIO1)] & flags)
+        dio_mask.set(Dio::DIO1);
+
+    if (DIO_MAPPINGS[2][mode][mapping.getMapping(Dio::DIO2)] & flags)
+        dio_mask.set(Dio::DIO2);
+
+    if (DIO_MAPPINGS[3][mode][mapping.getMapping(Dio::DIO3)] & flags)
+        dio_mask.set(Dio::DIO3);
+
+    if (DIO_MAPPINGS[4][mode][mapping.getMapping(Dio::DIO4)] & flags)
+        dio_mask.set(Dio::DIO4);
+
+    if (DIO_MAPPINGS[5][mode][mapping.getMapping(Dio::DIO5)] & flags)
+        dio_mask.set(Dio::DIO5);
+
+    return dio_mask;
+}
+
+ISX1278::IrqFlags SX1278Fsk::getIrqFlags()
+{
+    SPITransaction spi(slave, SPITransaction::WriteBit::INVERTED);
+
+    uint8_t flags_msb = spi.readRegister(REG_IRQ_FLAGS_1);
+    uint8_t flags_lsb = spi.readRegister(REG_IRQ_FLAGS_2);
+
+    return (flags_msb << 8 | flags_lsb);
 }
 
-float SX1278::getRssi()
+void SX1278Fsk::resetIrqFlags(IrqFlags flags)
 {
-    SPITransaction spi(bus_mgr.getBus(), SPITransaction::WriteBit::INVERTED);
+    // Mask only resettable flags
+    flags &= RegIrqFlags::RSSI | RegIrqFlags::PREAMBLE_DETECT |
+             RegIrqFlags::SYNC_ADDRESS_MATCH | RegIrqFlags::FIFO_OVERRUN |
+             RegIrqFlags::LOW_BAT;
+
+    if (flags != 0)
+    {
+        SPITransaction spi(slave, SPITransaction::WriteBit::INVERTED);
+        spi.writeRegister(REG_IRQ_FLAGS_1, flags >> 8);
+        spi.writeRegister(REG_IRQ_FLAGS_2, flags);
+    }
+}
+
+float SX1278Fsk::getRssi()
+{
+    SPITransaction spi(slave, SPITransaction::WriteBit::INVERTED);
 
     return static_cast<float>(spi.readRegister(REG_RSSI_VALUE)) * -0.5f;
 }
 
-float SX1278::getFei()
+float SX1278Fsk::getFei()
 {
-    SPITransaction spi(bus_mgr.getBus(), SPITransaction::WriteBit::INVERTED);
+    SPITransaction spi(slave, SPITransaction::WriteBit::INVERTED);
 
     // Order of read is important!!
     uint8_t fei_msb = spi.readRegister(REG_FEI_MSB);
@@ -376,38 +306,51 @@ float SX1278::getFei()
     return static_cast<float>(fei) * FSTEP;
 }
 
-void SX1278::setBitrate(int bitrate)
+void SX1278Fsk::setMode(Mode mode)
+{
+    SPITransaction spi(slave, SPITransaction::WriteBit::INVERTED);
+    spi.writeRegister(REG_OP_MODE, REG_OP_MODE_DEFAULT | mode);
+}
+
+void SX1278Fsk::setMapping(SX1278::DioMapping mapping)
+{
+    SPITransaction spi(slave, SPITransaction::WriteBit::INVERTED);
+    spi.writeRegister(REG_DIO_MAPPING_1, mapping.raw >> 8);
+    spi.writeRegister(REG_DIO_MAPPING_2, mapping.raw);
+}
+
+void SX1278Fsk::setBitrate(int bitrate)
 {
-    uint16_t val = SX1278Defs::FXOSC / bitrate;
+    uint16_t val = FXOSC / bitrate;
 
     // Update values
-    SPITransaction spi(bus_mgr.getBus(), SPITransaction::WriteBit::INVERTED);
+    SPITransaction spi(slave, SPITransaction::WriteBit::INVERTED);
     spi.writeRegister(REG_BITRATE_MSB, val >> 8);
     spi.writeRegister(REG_BITRATE_LSB, val);
 }
 
-void SX1278::setFreqDev(int freq_dev)
+void SX1278Fsk::setFreqDev(int freq_dev)
 {
     uint16_t val = freq_dev / FSTEP;
-    SPITransaction spi(bus_mgr.getBus(), SPITransaction::WriteBit::INVERTED);
+    SPITransaction spi(slave, SPITransaction::WriteBit::INVERTED);
     spi.writeRegister(REG_FDEV_MSB, (val >> 8) & 0x3f);
     spi.writeRegister(REG_FDEV_LSB, val);
 }
 
-void SX1278::setFreqRF(int freq_rf)
+void SX1278Fsk::setFreqRF(int freq_rf)
 {
     uint32_t val = freq_rf / FSTEP;
 
     // Update values
-    SPITransaction spi(bus_mgr.getBus(), SPITransaction::WriteBit::INVERTED);
+    SPITransaction spi(slave, SPITransaction::WriteBit::INVERTED);
     spi.writeRegister(REG_FRF_MSB, val >> 16);
     spi.writeRegister(REG_FRF_MID, val >> 8);
     spi.writeRegister(REG_FRF_LSB, val);
 }
 
-void SX1278::setOcp(int ocp)
+void SX1278Fsk::setOcp(int ocp)
 {
-    SPITransaction spi(bus_mgr.getBus(), SPITransaction::WriteBit::INVERTED);
+    SPITransaction spi(slave, SPITransaction::WriteBit::INVERTED);
     if (ocp == 0)
     {
         spi.writeRegister(REG_OCP, 0);
@@ -424,9 +367,9 @@ void SX1278::setOcp(int ocp)
     }
 }
 
-void SX1278::setSyncWord(uint8_t value[], int size)
+void SX1278Fsk::setSyncWord(uint8_t value[], int size)
 {
-    SPITransaction spi(bus_mgr.getBus(), SPITransaction::WriteBit::INVERTED);
+    SPITransaction spi(slave, SPITransaction::WriteBit::INVERTED);
     spi.writeRegister(REG_SYNC_CONFIG, REG_SYNC_CONFIG_DEFAULT | size);
 
     for (int i = 0; i < size; i++)
@@ -435,33 +378,33 @@ void SX1278::setSyncWord(uint8_t value[], int size)
     }
 }
 
-void SX1278::setRxBw(RxBw rx_bw)
+void SX1278Fsk::setRxBw(RxBw rx_bw)
 {
-    SPITransaction spi(bus_mgr.getBus(), SPITransaction::WriteBit::INVERTED);
+    SPITransaction spi(slave, SPITransaction::WriteBit::INVERTED);
     spi.writeRegister(REG_RX_BW, static_cast<uint8_t>(rx_bw));
 }
 
-void SX1278::setAfcBw(RxBw afc_bw)
+void SX1278Fsk::setAfcBw(RxBw afc_bw)
 {
-    SPITransaction spi(bus_mgr.getBus(), SPITransaction::WriteBit::INVERTED);
+    SPITransaction spi(slave, SPITransaction::WriteBit::INVERTED);
     spi.writeRegister(REG_AFC_BW, static_cast<uint8_t>(afc_bw));
 }
 
-void SX1278::setPreableLen(int len)
+void SX1278Fsk::setPreambleLen(int len)
 {
-    SPITransaction spi(bus_mgr.getBus(), SPITransaction::WriteBit::INVERTED);
+    SPITransaction spi(slave, SPITransaction::WriteBit::INVERTED);
     spi.writeRegister(REG_PREAMBLE_MSB, len >> 8);
     spi.writeRegister(REG_PREAMBLE_LSB, len);
 }
 
-void SX1278::setPa(int power, bool pa_boost)
+void SX1278Fsk::setPa(int power, bool pa_boost)
 {
     // [2, 17] or 20 if PA_BOOST
     // [0, 15] if !PA_BOOST
 
     const uint8_t MAX_POWER = 0b111;
 
-    SPITransaction spi(bus_mgr.getBus(), SPITransaction::WriteBit::INVERTED);
+    SPITransaction spi(slave, SPITransaction::WriteBit::INVERTED);
 
     if (!pa_boost)
     {
@@ -490,14 +433,15 @@ void SX1278::setPa(int power, bool pa_boost)
     // }
 }
 
-void SX1278::setShaping(Shaping shaping)
+void SX1278Fsk::setShaping(Shaping shaping)
 {
-    SPITransaction spi(bus_mgr.getBus(), SPITransaction::WriteBit::INVERTED);
+    SPITransaction spi(slave, SPITransaction::WriteBit::INVERTED);
     spi.writeRegister(REG_PA_RAMP, static_cast<uint8_t>(shaping) | 0x09);
 }
 
-void SX1278::debugDumpRegisters()
+void SX1278Fsk::debugDumpRegisters()
 {
+    Lock guard(*this);
     struct RegDef
     {
         const char *name;
@@ -561,7 +505,7 @@ void SX1278::debugDumpRegisters()
         RegDef{"REG_DIO_MAPPING_2", REG_DIO_MAPPING_2},
         RegDef{"REG_VERSION", REG_VERSION}, RegDef{NULL, 0}};
 
-    SPITransaction spi(bus_mgr.getBus(), SPITransaction::WriteBit::INVERTED);
+    SPITransaction spi(slave, SPITransaction::WriteBit::INVERTED);
 
     int i = 0;
     while (defs[i].name)
diff --git a/src/shared/radio/SX1278/SX1278.h b/src/shared/radio/SX1278/SX1278Fsk.h
similarity index 52%
rename from src/shared/radio/SX1278/SX1278.h
rename to src/shared/radio/SX1278/SX1278Fsk.h
index f891ce564d6b858d94374450f92a3113e0c442ba..300780978ceec32e8b776012ffdaaf798d63b613 100644
--- a/src/shared/radio/SX1278/SX1278.h
+++ b/src/shared/radio/SX1278/SX1278Fsk.h
@@ -24,146 +24,25 @@
 
 #include <diagnostic/PrintLogger.h>
 #include <drivers/spi/SPIDriver.h>
-#include <kernel/kernel.h>
-#include <radio/Transceiver.h>
 
 #include <cstdint>
 
-#include "SX1278Defs.h"
-
-/*
-# Description
-## Concurrent access
-The device is stateful, in order to send/receive data you need to set in a
-particular state. This is handled by SX1278BusManager::setMode. This causes a
-lot of issues with multithreaded code, and receiving/transmitting concurrently.
-
-So the driver wraps the bus in a BusManager for this reason, in order to perform
-anything you need to lock the bus. The driver then locks the internal mutex and
-sets the device in the correct mode for that operation.
-
-But this causes problems when receiving, as receive can block indefinetly, and
-would prevent any thread from actually sending stuff. The solution is to release
-the lock only when waiting for a packet. This allows other threads to lock it
-and change mode, and unlocking the bus resets its state back to RX so it can
-continue to listen for incoming packets.
-
-The BusManager also multiplexes who is currently waiting for an interrupt. This
-is needed because due to the limited interrupts pins, the device signals
-multiples of them on one pin. And for some random reasons, the designers decided
-to put "packet received" and "packed sent" on same pin (DIO0)...
-
-Again, the solution is to track who is actually using the bus, so when sending a
-packet that triggers the "packet sent" interrupt, the manager knows to dispatch
-it to the currect thread, and not wake up the RX thread.
-*/
+#include "SX1278Common.h"
 
 namespace Boardcore
 {
 
 /**
- * @brief SX1278 bus access manager.
+ * @brief Various SX1278 register/enums definitions.
  */
-class SX1278BusManager
+class SX1278Fsk : public SX1278::SX1278Common
 {
 public:
-    using Mode = SX1278Defs::RegOpMode::Mode;
-
-    /**
-     * @brief RAII scoped bus lock guard.
-     */
-    class Lock
-    {
-    public:
-        explicit Lock(SX1278BusManager &bus) : bus(bus) { bus.lock(); }
-
-        ~Lock() { bus.unlock(); }
-
-    private:
-        SX1278BusManager &bus;
-    };
-
-    /**
-     * @brief RAII scoped bus lock guard.
-     */
-    class LockMode
-    {
-    public:
-        LockMode(SX1278BusManager &bus, Mode mode) : bus(bus)
-        {
-            bus.lockMode(mode);
-        }
-
-        ~LockMode() { bus.unlockMode(); }
-
-    private:
-        SX1278BusManager &bus;
-    };
-
-    SX1278BusManager(SPIBusInterface &bus, miosix::GpioPin cs);
-
-    /**
-     * @brief Lock bus for exclusive access (does not change mode).
-     */
-    void lock();
-
-    /**
-     * @brief Release bus for exclusive access.
-     */
-    void unlock();
-
-    /**
-     * @brief Lock bus for exclusive access.
-     *
-     * @param mode Device mode requested.
-     */
-    void lockMode(Mode mode);
-
-    /**
-     * @brief Release bus for exclusive access.
-     */
-    void unlockMode();
-
     /**
-     * @brief Get underlying bus.
+     * @brief Maximum transmittable unit of this driver.
      */
-    SPISlave &getBus();
+    static constexpr size_t MTU = SX1278::Fsk::FIFO_LEN - 1;
 
-    /**
-     * @brief Handle DIO0 irq.
-     */
-    void handleDioIRQ();
-
-    /**
-     * @brief Wait for generic irq (DOES NOT RELEASE LOCK!).
-     */
-    bool waitForIrq(uint16_t mask, int timeout = 10);
-
-    /**
-     * @brief Wait for RX irq (releases lock safely).
-     */
-    void waitForRxIrq();
-
-private:
-    void enterMode(Mode mode);
-
-    uint16_t getIrqFlags();
-    void setMode(Mode mode);
-
-    miosix::Thread *irq_wait_thread = nullptr;
-    miosix::Thread *rx_wait_thread  = nullptr;
-    miosix::FastMutex mutex;
-
-    SPISlave slave;
-    Mode mode;
-};
-
-/**
- * @brief Various SX1278 register/enums definitions.
- */
-class SX1278 : public Transceiver
-{
-public:
     /**
      * @brief Channel filter bandwidth.
      */
@@ -194,20 +73,20 @@ public:
 
     enum class Shaping
     {
-        NONE = SX1278Defs::RegPaRamp::MODULATION_SHAPING_NONE,
+        NONE = SX1278::Fsk::RegPaRamp::MODULATION_SHAPING_NONE,
         GAUSSIAN_BT_1_0 =
-            SX1278Defs::RegPaRamp::MODULATION_SHAPING_GAUSSIAN_BT_1_0,
+            SX1278::Fsk::RegPaRamp::MODULATION_SHAPING_GAUSSIAN_BT_1_0,
         GAUSSIAN_BT_0_5 =
-            SX1278Defs::RegPaRamp::MODULATION_SHAPING_GAUSSIAN_BT_0_5,
+            SX1278::Fsk::RegPaRamp::MODULATION_SHAPING_GAUSSIAN_BT_0_5,
         GAUSSIAN_BT_0_3 =
-            SX1278Defs::RegPaRamp::MODULATION_SHAPING_GAUSSIAN_BT_0_3,
+            SX1278::Fsk::RegPaRamp::MODULATION_SHAPING_GAUSSIAN_BT_0_3,
     };
 
     enum class DcFree
     {
-        NONE       = SX1278Defs::RegPacketConfig1::DC_FREE_NONE,
-        MANCHESTER = SX1278Defs::RegPacketConfig1::DC_FREE_MANCHESTER,
-        WHITENING  = SX1278Defs::RegPacketConfig1::DC_FREE_WHITENING
+        NONE       = SX1278::Fsk::RegPacketConfig1::DC_FREE_NONE,
+        MANCHESTER = SX1278::Fsk::RegPacketConfig1::DC_FREE_MANCHESTER,
+        WHITENING  = SX1278::Fsk::RegPacketConfig1::DC_FREE_WHITENING
     };
 
     /**
@@ -224,6 +103,7 @@ public:
             120;  //< Over current protection limit in mA (0 for no limit).
         int power = 10;  //< Output power in dB (Between +2 and +17, or +20 for
                          // full power).
+        bool pa_boost   = true;  //< Enable output on PA_BOOST.
         Shaping shaping = Shaping::GAUSSIAN_BT_1_0;  //< Shaping applied to
                                                      // the modulation stream.
         DcFree dc_free = DcFree::WHITENING;  //< Dc free encoding scheme.
@@ -234,10 +114,10 @@ public:
      */
     enum class Error
     {
-        NONE,              //< No error encountered.
-        BAD_VALUE,         //< A requested value was outside the valid range.
-        BAD_VERSION,       //< Chip isn't connected.
-        CONFIGURE_FAILED,  //< Timeout on IRQ register.
+        NONE,         //< No error encountered.
+        BAD_VALUE,    //< A requested value was outside the valid range.
+        BAD_VERSION,  //< Chip isn't connected.
+        IRQ_TIMEOUT,  //< Timeout on IRQ register.
     };
 
     /**
@@ -246,22 +126,22 @@ public:
      * @param bus SPI bus used.
      * @param cs Chip select pin.
      */
-    SX1278(SPIBusInterface &bus, miosix::GpioPin cs);
+    explicit SX1278Fsk(SPISlave slave) : SX1278Common(slave) {}
 
     /**
      * @brief Setup the device.
      */
-    Error init(const Config &config);
+    [[nodiscard]] virtual Error init(const Config &config);
 
     /*
      * @brief Check if this device is connected.
      */
-    bool probe();
+    bool checkVersion();
 
     /**
      * @brief Configure this device on the fly.
      */
-    bool configure(const Config &config);
+    [[nodiscard]] virtual Error configure(const Config &config);
 
     /**
      * @brief Wait until a new packet is received.
@@ -270,7 +150,7 @@ public:
      * @param pkt_len   Maximum length of the received data.
      * @return          Size of the data received or -1 if failure
      */
-    ssize_t receive(uint8_t *pkt, size_t max_len);
+    ssize_t receive(uint8_t *pkt, size_t max_len) override;
 
     /**
      * @brief Send a packet.
@@ -278,15 +158,10 @@ public:
      *
      * @param pkt       Pointer to the packet (needs to be at least pkt_len
      * bytes).
-     * @param pkt_len   Lenght of the packet to be sent.
+     * @param pkt_len   Length of the packet to be sent.
      * @return          True if the message was sent correctly.
      */
-    bool send(uint8_t *pkt, size_t len);
-
-    /**
-     * @brief Return device version.
-     */
-    uint8_t getVersion();
+    bool send(uint8_t *pkt, size_t len) override;
 
     /**
      * @brief Get the current perceived RSSI in dBm.
@@ -303,25 +178,35 @@ public:
      */
     float getLastRxFei();
 
-    /**
-     * @brief Handle an incoming interrupt.
-     */
-    void handleDioIRQ();
-
     /**
      * @brief Dump all registers via TRACE.
      */
     void debugDumpRegisters();
 
-public:
-    using Mode = SX1278BusManager::Mode;
+protected:
+    // Stuff to work with various front-ends
+    virtual void enableRxFrontend() override {}
+    virtual void disableRxFrontend() override {}
+    virtual void enableTxFrontend() override {}
+    virtual void disableTxFrontend() override {}
 
+private:
     void rateLimitTx();
+
     void imageCalibrate();
 
+    SX1278::DioMask getDioMaskFromIrqFlags(IrqFlags flags, Mode mode,
+                                           SX1278::DioMapping mapping) override;
+
+    IrqFlags getIrqFlags() override;
+    void resetIrqFlags(IrqFlags flags) override;
+
     float getRssi();
     float getFei();
 
+    void setMode(Mode mode) override;
+    void setMapping(SX1278::DioMapping mapping) override;
+
     void setBitrate(int bitrate);
     void setFreqDev(int freq_dev);
     void setFreqRF(int freq_rf);
@@ -329,15 +214,12 @@ public:
     void setSyncWord(uint8_t value[], int size);
     void setRxBw(RxBw rx_bw);
     void setAfcBw(RxBw afc_bw);
-    void setPreableLen(int len);
+    void setPreambleLen(int len);
     void setPa(int power, bool pa_boost);
     void setShaping(Shaping shaping);
 
-    void waitForIrq(uint16_t mask);
-
     long long last_tx  = 0;
     float last_rx_rssi = 0.0f;
-    SX1278BusManager bus_mgr;
     PrintLogger logger = Logging::getLogger("sx1278");
 };
 
diff --git a/src/shared/radio/SX1278/SX1278Lora.cpp b/src/shared/radio/SX1278/SX1278Lora.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..de35bc5a11a065d764ca961bc4db2d4ebcaf21bf
--- /dev/null
+++ b/src/shared/radio/SX1278/SX1278Lora.cpp
@@ -0,0 +1,423 @@
+/* Copyright (c) 2022 Skyward Experimental Rocketry
+ * Author: Davide Mor
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#include "SX1278Lora.h"
+
+#include <utils/Debug.h>
+
+#include "SX1278LoraTimings.h"
+
+namespace Boardcore
+{
+
+using namespace SX1278;
+using namespace SX1278::Lora;
+
+static constexpr uint8_t MAX_PAYLOAD_LENGTH = 0xff;
+static constexpr uint8_t FIFO_TX_BASE_ADDR  = 0x00;
+static constexpr uint8_t FIFO_RX_BASE_ADDR  = 0x00;
+
+// This doesn't really matter, as we are going to change it anyway
+static constexpr DioMapping DEFAULT_MAPPING =
+    DioMapping(0, 0, 0, 0, 0, 0, false);
+
+// This registers allow for optmized rx spurious response
+struct ErrataRegistersValues
+{
+    int freq_rf                = 0;
+    int reg_if_freq_1          = 0;
+    int reg_if_freq_2          = 0;
+    bool automatic_if_on       = false;
+    int reg_high_bw_optimize_1 = 0;
+    int reg_high_bw_optimize_2 = 0;
+
+    static ErrataRegistersValues calculate(RegModemConfig1::Bw bw, int freq_rf)
+    {
+        ErrataRegistersValues values = {};
+
+        switch (bw)
+        {
+            case RegModemConfig1::BW_HZ_7800:
+                freq_rf += 7810;
+                break;
+            case RegModemConfig1::BW_HZ_10400:
+                freq_rf += 10420;
+                break;
+            case RegModemConfig1::BW_HZ_15600:
+                freq_rf += 15620;
+                break;
+            case RegModemConfig1::BW_HZ_20800:
+                freq_rf += 20830;
+                break;
+            case RegModemConfig1::BW_HZ_41700:
+                freq_rf += 41670;
+                break;
+            default:
+                break;
+        }
+
+        switch (bw)
+        {
+            case RegModemConfig1::BW_HZ_7800:
+                values.reg_if_freq_2 = 0x48;
+                values.reg_if_freq_1 = 0x00;
+                break;
+            case RegModemConfig1::BW_HZ_10400:
+            case RegModemConfig1::BW_HZ_15600:
+            case RegModemConfig1::BW_HZ_20800:
+            case RegModemConfig1::BW_HZ_31250:
+            case RegModemConfig1::BW_HZ_41700:
+                values.reg_if_freq_2 = 0x44;
+                values.reg_if_freq_1 = 0x00;
+                break;
+            case RegModemConfig1::BW_HZ_62500:
+            case RegModemConfig1::BW_HZ_125000:
+            case RegModemConfig1::BW_HZ_250000:
+                values.reg_if_freq_2 = 0x40;
+                values.reg_if_freq_1 = 0x00;
+                break;
+            case RegModemConfig1::BW_HZ_500000:
+                values.reg_if_freq_2 = -1;
+                values.reg_if_freq_1 = -1;
+                break;
+            default:
+                break;
+        }
+
+        values.automatic_if_on = bw == RegModemConfig1::BW_HZ_500000;
+
+        bool freq_rf_low_range  = 410000000 <= freq_rf && freq_rf <= 525000000;
+        bool freq_rf_high_range = 862000000 <= freq_rf && freq_rf <= 1020000000;
+
+        if (bw == RegModemConfig1::BW_HZ_500000 && freq_rf_low_range)
+        {
+            values.reg_high_bw_optimize_1 = 0x02;
+            values.reg_high_bw_optimize_2 = 0x64;
+        }
+        else if (bw == RegModemConfig1::BW_HZ_500000 && freq_rf_high_range)
+        {
+            values.reg_high_bw_optimize_1 = 0x02;
+            values.reg_high_bw_optimize_2 = 0x7f;
+        }
+        else
+        {
+            values.reg_high_bw_optimize_1 = 0x03;
+            values.reg_high_bw_optimize_2 = -1;
+        }
+
+        values.freq_rf = freq_rf;
+
+        return values;
+    }
+};
+
+SX1278Lora::Error SX1278Lora::init(const Config &config)
+{
+    // First probe for the device
+    if (!checkVersion())
+    {
+        TRACE("[sx1278] Wrong chipid\n");
+        return Error::BAD_VALUE;
+    }
+
+    Error err;
+    if ((err = configure(config)) != Error::NONE)
+        return err;
+
+    return Error::NONE;
+}
+
+bool SX1278Lora::checkVersion()
+{
+    Lock guard(*this);
+    SPITransaction spi(slave, SPITransaction::WriteBit::INVERTED);
+
+    return spi.readRegister(REG_VERSION) == 0x12;
+}
+
+SX1278Lora::Error SX1278Lora::configure(const Config &config)
+{
+    // First cycle the device to bring it into LoRa mode (can only be changed in
+    // sleep)
+    setDefaultMode(RegOpMode::MODE_SLEEP, DEFAULT_MAPPING, false, false);
+    // Make sure the device remains in standby and not in sleep
+    setDefaultMode(RegOpMode::MODE_STDBY, DEFAULT_MAPPING, false, false);
+
+    // Lock the bus
+    Lock guard(*this);
+    LockMode mode_guard(*this, guard, RegOpMode::MODE_STDBY, DEFAULT_MAPPING);
+
+    RegModemConfig1::Bw bw = static_cast<RegModemConfig1::Bw>(config.bandwidth);
+    RegModemConfig1::Cr cr =
+        static_cast<RegModemConfig1::Cr>(config.coding_rate);
+    RegModemConfig2::Sf sf =
+        static_cast<RegModemConfig2::Sf>(config.spreading_factor);
+
+    int freq_rf   = config.freq_rf;
+    int ocp       = config.ocp;
+    int power     = config.power;
+    bool pa_boost = config.pa_boost;
+
+    bool low_data_rate_optimize = config.low_data_rate_optimize;
+
+    // Mandated for when the symbol length exceeds 16ms
+    if (Lora::symbolDuration(sf, RegModemConfig1::bandwidthToInt(bw)) > 16000)
+        low_data_rate_optimize = true;
+
+    // Setup high BW errata values
+    ErrataRegistersValues errata_values =
+        ErrataRegistersValues::calculate(bw, freq_rf);
+
+    {
+        SPITransaction spi(slave, SPITransaction::WriteBit::INVERTED);
+
+        // Setup FIFO sections
+        spi.writeRegister(REG_FIFO_TX_BASE_ADDR, FIFO_TX_BASE_ADDR);
+        spi.writeRegister(REG_FIFO_RX_BASE_ADDR, FIFO_RX_BASE_ADDR);
+        spi.writeRegister(REG_MAX_PAYLOAD_LENGTH, MAX_PAYLOAD_LENGTH);
+
+        // Setup frequency
+        uint32_t freq_rf_raw = errata_values.freq_rf / FSTEP;
+
+        spi.writeRegister(REG_FRF_MSB, freq_rf_raw >> 16);
+        spi.writeRegister(REG_FRF_MID, freq_rf_raw >> 8);
+        spi.writeRegister(REG_FRF_LSB, freq_rf_raw);
+
+        // Setup reg power amplifier
+        const int MAX_POWER = 0b111;
+        if (!pa_boost)
+        {
+            // No power amplifier boost
+            spi.writeRegister(REG_PA_CONFIG,
+                              RegPaConfig::make(power, MAX_POWER, false));
+            spi.writeRegister(REG_PA_DAC, RegPaDac::make(false));
+        }
+        else if (power != 20)
+        {
+            // Run power amplifier boost but not at full power
+            spi.writeRegister(REG_PA_CONFIG,
+                              RegPaConfig::make(power - 2, MAX_POWER, true));
+            spi.writeRegister(REG_PA_DAC, RegPaDac::make(false));
+        }
+        else
+        {
+            // Run power amplifier at full power
+            spi.writeRegister(REG_PA_CONFIG,
+                              RegPaConfig::make(0b1111, MAX_POWER, true));
+            spi.writeRegister(REG_PA_DAC, RegPaDac::make(true));
+        }
+
+        // Setup reg over-current protection
+        if (config.ocp == 0)
+        {
+            spi.writeRegister(REG_OCP, RegOcp::make(0, false));
+        }
+        else if (ocp <= 120)
+        {
+            spi.writeRegister(REG_OCP, RegOcp::make((ocp - 45) / 5, true));
+        }
+        else
+        {
+            spi.writeRegister(REG_OCP, RegOcp::make((ocp + 30) / 10, true));
+        }
+
+        // Setup generic configuration registers
+        spi.writeRegister(REG_MODEM_CONFIG_1,
+                          RegModemConfig1::make(false, cr, bw));
+        spi.writeRegister(REG_MODEM_CONFIG_2,
+                          RegModemConfig2::make(false, false, sf));
+        spi.writeRegister(REG_MODEM_CONFIG_3,
+                          RegModemConfig3::make(true, low_data_rate_optimize));
+
+        // Setup detect optimize (lots of magic values here)
+        spi.writeRegister(
+            REG_DETECT_OPTIMIZE,
+            RegDetectOptimize::make(0x03, errata_values.automatic_if_on));
+        spi.writeRegister(REG_DETECTION_THRESHOLD, 0x0a);
+
+        // (just change it to something that isn't the default)
+        spi.writeRegister(REG_SYNC_WORD, 0x69);
+
+        // Setup weird errata registers (see errata note)
+        if (errata_values.reg_high_bw_optimize_1 != -1)
+            spi.writeRegister(REG_HIGH_BW_OPTIMIZE_1,
+                              errata_values.reg_high_bw_optimize_1);
+        if (errata_values.reg_high_bw_optimize_2 != -1)
+            spi.writeRegister(REG_HIGH_BW_OPTIMIZE_2,
+                              errata_values.reg_high_bw_optimize_2);
+        if (errata_values.reg_if_freq_1 != -1)
+            spi.writeRegister(REG_IF_FREQ_1, errata_values.reg_if_freq_1);
+        if (errata_values.reg_if_freq_2 != -1)
+            spi.writeRegister(REG_IF_FREQ_2, errata_values.reg_if_freq_2);
+    }
+
+    return Error::NONE;
+}
+
+ssize_t SX1278Lora::receive(uint8_t *pkt, size_t max_len)
+{
+    Lock guard(*this);
+
+    SPITransaction spi(slave, SPITransaction::WriteBit::INVERTED);
+
+    do
+    {
+        // Use continuous because it doesn't go into timeout (and you cannot
+        // disable it...)
+        LockMode mode_guard(*this, guard, RegOpMode::MODE_RXCONTINUOUS,
+                            DioMapping(0, 0, 0, 0, 0, 0), false, true);
+
+        waitForIrq(mode_guard, RegIrqFlags::RX_DONE, true);
+    } while (checkForIrqAndReset(RegIrqFlags::PAYLOAD_CRC_ERROR));
+
+    uint8_t len = spi.readRegister(REG_RX_NB_BYTES);
+    if (len > max_len)
+        return -1;
+
+    readFifo(FIFO_RX_BASE_ADDR, pkt, len);
+
+    return len;
+}
+
+bool SX1278Lora::send(uint8_t *pkt, size_t len)
+{
+    if (len > MTU)
+        return false;
+
+    Lock guard(*this);
+
+    SPITransaction spi(slave, SPITransaction::WriteBit::INVERTED);
+
+    spi.writeRegister(REG_PAYLOAD_LENGTH, len);
+    writeFifo(FIFO_TX_BASE_ADDR, pkt, len);
+
+    {
+        // Now enter in mode TX to send the packet
+        LockMode mode_guard(*this, guard, RegOpMode::MODE_TX,
+                            DioMapping(1, 0, 0, 0, 0, 0), true, false);
+
+        // Wait for the transmission to end
+        waitForIrq(mode_guard, RegIrqFlags::TX_DONE);
+    }
+
+    return true;
+}
+
+float SX1278Lora::getLastRxRssi()
+{
+    float rssi;
+    {
+        Lock guard(*this);
+        SPITransaction spi(slave, SPITransaction::WriteBit::INVERTED);
+        rssi =
+            static_cast<float>(spi.readRegister(REG_PKT_RSSI_VALUE)) - 164.0f;
+    }
+
+    float snr = getLastRxSnr();
+    // Handle packets below the noise floor
+    if (snr < 0.0f)
+        rssi += snr * 0.25f;
+
+    return rssi;
+}
+
+float SX1278Lora::getLastRxSnr()
+{
+    Lock guard(*this);
+    SPITransaction spi(slave, SPITransaction::WriteBit::INVERTED);
+    return static_cast<float>(
+               static_cast<int8_t>(spi.readRegister(REG_PKT_SNR_VALUE))) /
+           4.0f;
+}
+
+void SX1278Lora::readFifo(uint8_t addr, uint8_t *dst, uint8_t size)
+{
+    SPITransaction spi(slave, SPITransaction::WriteBit::INVERTED);
+    spi.writeRegister(REG_FIFO_ADDR_PTR, addr);
+    spi.readRegisters(REG_FIFO, dst, size);
+}
+
+void SX1278Lora::writeFifo(uint8_t addr, uint8_t *src, uint8_t size)
+{
+    SPITransaction spi(slave, SPITransaction::WriteBit::INVERTED);
+    spi.writeRegister(REG_FIFO_ADDR_PTR, addr);
+    spi.writeRegisters(REG_FIFO, src, size);
+}
+
+SX1278::DioMask SX1278Lora::getDioMaskFromIrqFlags(IrqFlags flags, Mode _mode,
+                                                   SX1278::DioMapping mapping)
+{
+    // In LoRa the mode doesn't matter for IRQs
+    (void)_mode;
+    DioMask dio_mask;
+
+    if (DIO_MAPPINGS[0][mapping.getMapping(Dio::DIO0)] & flags)
+        dio_mask.set(Dio::DIO0);
+
+    if (DIO_MAPPINGS[1][mapping.getMapping(Dio::DIO1)] & flags)
+        dio_mask.set(Dio::DIO1);
+
+    if (DIO_MAPPINGS[2][mapping.getMapping(Dio::DIO2)] & flags)
+        dio_mask.set(Dio::DIO2);
+
+    if (DIO_MAPPINGS[3][mapping.getMapping(Dio::DIO3)] & flags)
+        dio_mask.set(Dio::DIO3);
+
+    if (DIO_MAPPINGS[4][mapping.getMapping(Dio::DIO4)] & flags)
+        dio_mask.set(Dio::DIO4);
+
+    if (DIO_MAPPINGS[5][mapping.getMapping(Dio::DIO5)] & flags)
+        dio_mask.set(Dio::DIO5);
+
+    return dio_mask;
+}
+
+ISX1278::IrqFlags SX1278Lora::getIrqFlags()
+{
+    SPITransaction spi(slave, SPITransaction::WriteBit::INVERTED);
+    return spi.readRegister(REG_IRQ_FLAGS);
+}
+
+void SX1278Lora::resetIrqFlags(IrqFlags flags)
+{
+    SPITransaction spi(slave, SPITransaction::WriteBit::INVERTED);
+    // Register is write clear
+    spi.writeRegister(REG_IRQ_FLAGS, flags);
+}
+
+void SX1278Lora::setMode(ISX1278::Mode mode)
+{
+    SPITransaction spi(slave, SPITransaction::WriteBit::INVERTED);
+
+    spi.writeRegister(
+        REG_OP_MODE,
+        RegOpMode::make(static_cast<RegOpMode::Mode>(mode), true, false));
+}
+
+void SX1278Lora::setMapping(SX1278::DioMapping mapping)
+{
+    SPITransaction spi(slave, SPITransaction::WriteBit::INVERTED);
+    spi.writeRegister(REG_DIO_MAPPING_1, mapping.raw >> 8);
+    spi.writeRegister(REG_DIO_MAPPING_2, mapping.raw);
+}
+
+}  // namespace Boardcore
diff --git a/src/shared/radio/SX1278/SX1278Lora.h b/src/shared/radio/SX1278/SX1278Lora.h
new file mode 100644
index 0000000000000000000000000000000000000000..3d129fac0406f4ec2218ab1006140e5e4171a2ee
--- /dev/null
+++ b/src/shared/radio/SX1278/SX1278Lora.h
@@ -0,0 +1,206 @@
+/* Copyright (c) 2022 Skyward Experimental Rocketry
+ * Author: Davide Mor
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#pragma once
+
+#include "SX1278Common.h"
+#include "SX1278LoraTimings.h"
+
+namespace Boardcore
+{
+
+class SX1278Lora : public SX1278::SX1278Common
+{
+public:
+    static constexpr size_t MTU = 255;
+
+    /**
+     * @brief Requested SX1278 configuration.
+     */
+    struct Config
+    {
+        /**
+         * @brief Bandwidth of the device
+         */
+        enum class Bw
+        {
+            HZ_7800   = SX1278::Lora::RegModemConfig1::BW_HZ_7800,
+            HZ_10400  = SX1278::Lora::RegModemConfig1::BW_HZ_10400,
+            HZ_15600  = SX1278::Lora::RegModemConfig1::BW_HZ_15600,
+            HZ_20800  = SX1278::Lora::RegModemConfig1::BW_HZ_20800,
+            HZ_31250  = SX1278::Lora::RegModemConfig1::BW_HZ_31250,
+            HZ_41700  = SX1278::Lora::RegModemConfig1::BW_HZ_41700,
+            HZ_62500  = SX1278::Lora::RegModemConfig1::BW_HZ_62500,
+            HZ_125000 = SX1278::Lora::RegModemConfig1::BW_HZ_125000,
+            HZ_250000 = SX1278::Lora::RegModemConfig1::BW_HZ_250000,
+            HZ_500000 = SX1278::Lora::RegModemConfig1::BW_HZ_500000,
+        };
+
+        /**
+         * @brief Coding rate of the device.
+         *
+         * This defines how many bits in the datastream are error correction
+         * bits.
+         */
+        enum class Cr
+        {
+            CR_1 = SX1278::Lora::RegModemConfig1::CR_1,  //< +25% error
+                                                         // correction overhead.
+            CR_2 = SX1278::Lora::RegModemConfig1::CR_2,  //< +50% error
+                                                         // correction overhead.
+            CR_3 = SX1278::Lora::RegModemConfig1::CR_3,  //< +75% error
+                                                         // correction overhead.
+            CR_4 = SX1278::Lora::RegModemConfig1::CR_4,  //< +100% error
+                                                         // correction overhead.
+        };
+
+        /**
+         * @brief Spreading factor of the devide.
+         *
+         * This defines how long a chirp really is, every increment doubles the
+         * duration of the chirp, while adding one extra bit.
+         */
+        enum class Sf
+        {
+            // Spreading factor 6 is special, and not supported by this driver
+            // SF_6  = SX1278::Lora::RegModemConfig2::SF_6,
+            SF_7  = SX1278::Lora::RegModemConfig2::SF_7,
+            SF_8  = SX1278::Lora::RegModemConfig2::SF_8,
+            SF_9  = SX1278::Lora::RegModemConfig2::SF_9,
+            SF_10 = SX1278::Lora::RegModemConfig2::SF_10,
+            SF_11 = SX1278::Lora::RegModemConfig2::SF_11,
+            SF_12 = SX1278::Lora::RegModemConfig2::SF_12,
+        };
+
+        Bw bandwidth        = Bw::HZ_125000;
+        Cr coding_rate      = Cr::CR_1;
+        Sf spreading_factor = Sf::SF_7;
+
+        bool low_data_rate_optimize =
+            false;  //< Optimize the transmission at high spreading factors.
+
+        int freq_rf = 434000000;  //< RF frequency in Hz.
+        int ocp =
+            120;  //< Over current protection limit in mA (0 for no limit).
+        int power =
+            15;  //< Output power in dB (between +2 and +17 with pa_boost = on,
+                 // and between +0 and +14 with pa_boost = off, +20 for +20dBm
+                 // max power ignoring pa_boost).
+        bool pa_boost = true;  //< Enable output on PA_BOOST.
+
+        /**
+         * @brief Calculates effective and usable bitrate.
+         */
+        uint32_t effectiveBitrate() const
+        {
+            using namespace SX1278::Lora;
+
+            return SX1278::Lora::effectiveBitrate(
+                static_cast<RegModemConfig2::Sf>(spreading_factor),
+                RegModemConfig1::bandwidthToInt(
+                    static_cast<RegModemConfig1::Bw>(bandwidth)),
+                static_cast<RegModemConfig1::Cr>(coding_rate));
+        }
+    };
+
+    /**
+     * @brief Error enum.
+     */
+    enum class Error
+    {
+        NONE,         //< No error encountered.
+        BAD_VALUE,    //< A requested value was outside the valid range.
+        BAD_VERSION,  //< Chip isn't connected.
+        IRQ_TIMEOUT,  //< Timeout on IRQ register.
+    };
+
+    explicit SX1278Lora(SPISlave slave) : SX1278Common(slave) {}
+
+    /**
+     * @brief Setup the device.
+     */
+    [[nodiscard]] virtual Error init(const Config &config);
+
+    /*
+     * @brief Check if this device is connected.
+     */
+    bool checkVersion();
+
+    /**
+     * @brief Configure this device on the fly.
+     */
+    [[nodiscard]] virtual Error configure(const Config &config);
+
+    /**
+     * @brief Wait until a new packet is received.
+     *
+     * @param pkt       Buffer to store the received packet into.
+     * @param pkt_len   Maximum length of the received data.
+     * @return          Size of the data received or -1 if failure
+     */
+    ssize_t receive(uint8_t *pkt, size_t max_len) override;
+
+    /**
+     * @brief Send a packet.
+     * The function must block until the packet is sent (successfully or not)
+     *
+     * @param pkt       Pointer to the packet (needs to be at least pkt_len
+     * bytes).
+     * @param pkt_len   Length of the packet to be sent.
+     * @return          True if the message was sent correctly.
+     */
+    bool send(uint8_t *pkt, size_t len) override;
+
+    /**
+     * @brief Get the RSSI in dBm, during last packet receive.
+     */
+    float getLastRxRssi();
+
+    /**
+     * @brief Get the RSSI in dBm, during last packet receive.
+     */
+    float getLastRxSnr();
+
+protected:
+    // Stuff to work with various front-ends
+    virtual void enableRxFrontend() override {}
+    virtual void disableRxFrontend() override {}
+    virtual void enableTxFrontend() override {}
+    virtual void disableTxFrontend() override {}
+
+private:
+    void readFifo(uint8_t addr, uint8_t *dst, uint8_t size);
+    void writeFifo(uint8_t addr, uint8_t *src, uint8_t size);
+
+    SX1278::DioMask getDioMaskFromIrqFlags(IrqFlags flags, Mode mode,
+                                           SX1278::DioMapping mapping) override;
+
+    IrqFlags getIrqFlags() override;
+    void resetIrqFlags(IrqFlags flags) override;
+
+    void setMode(Mode mode) override;
+    void setMapping(SX1278::DioMapping mapping) override;
+
+    void setFreqRF(int freq_rf);
+};
+
+}  // namespace Boardcore
diff --git a/src/shared/radio/SX1278/SX1278LoraTimings.h b/src/shared/radio/SX1278/SX1278LoraTimings.h
new file mode 100644
index 0000000000000000000000000000000000000000..458d1f5e1128a1b202f4125e760009200db0d74e
--- /dev/null
+++ b/src/shared/radio/SX1278/SX1278LoraTimings.h
@@ -0,0 +1,88 @@
+/* Copyright (c) 2022 Skyward Experimental Rocketry
+ * Author: Davide Mor
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#pragma once
+
+#include <cstdint>
+
+namespace Boardcore
+{
+
+namespace SX1278
+{
+
+namespace Lora
+{
+
+/**
+ * @brief Computes the symbol duration in microseconds
+ */
+inline constexpr uint32_t symbolDuration(uint32_t spreading_factor,
+                                         uint32_t bandwidth)
+{
+    // The number of chips contained in each symbol is 2^spreadingFactor
+    uint32_t chip_count = (1 << spreading_factor);
+
+    // The number of chips per seconds is equal to the bandwidth
+    uint32_t chips_per_second = bandwidth;
+
+    // With an SR of 12 (4096) this _barely_ fits in an uint32_t
+    return (chip_count * 1000000) / chips_per_second;
+}
+
+/**
+ * @brief Computes the nominal (without error correction) bitrate in b/s
+ */
+inline constexpr uint32_t nominalBitrate(uint32_t spreading_factor,
+                                         uint32_t bandwidth)
+{
+    // The number of bits per symbol is equal to the spreading factor
+    uint32_t bits_per_symbol = spreading_factor;
+
+    // Calculate the number of symbols per seconds based on the symbol duration
+    uint32_t symbols_per_second =
+        (1000000 / symbolDuration(spreading_factor, bandwidth));
+
+    return symbols_per_second * bits_per_symbol;
+}
+
+/**
+ * @brief Computes the actual usable bitrate in b/s
+ */
+inline constexpr uint32_t effectiveBitrate(uint32_t spreading_factor,
+                                           uint32_t bandwidth,
+                                           uint32_t coding_rate)
+{
+    // Input user bits
+    uint32_t input_bits = 4;
+    // Output transmitted bits
+    uint32_t output_bits = 4 + coding_rate;
+
+    return (nominalBitrate(spreading_factor, bandwidth) / output_bits) *
+           input_bits;
+}
+
+}  // namespace Lora
+
+}  // namespace SX1278
+
+}  // namespace Boardcore
diff --git a/src/tests/drivers/sx1278/test-sx1278-serial.cpp b/src/tests/drivers/sx1278/test-sx1278-serial.cpp
deleted file mode 100644
index de609a9aac18dd6fcd3e9605fec86011b3eee7f3..0000000000000000000000000000000000000000
--- a/src/tests/drivers/sx1278/test-sx1278-serial.cpp
+++ /dev/null
@@ -1,148 +0,0 @@
-/* Copyright (c) 2021 Skyward Experimental Rocketry
- * Author: Davide Mor
- *
- * Permission is hereby granted, free of charge, to any person obtaining a copy
- * of this software and associated documentation files (the "Software"), to deal
- * in the Software without restriction, including without limitation the rights
- * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
- * copies of the Software, and to permit persons to whom the Software is
- * furnished to do so, subject to the following conditions:
- *
- * The above copyright notice and this permission notice shall be included in
- * all copies or substantial portions of the Software.
- *
- * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
- * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
- * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
- * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
- * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
- * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
- * THE SOFTWARE.
- */
-
-#include <drivers/interrupt/external_interrupts.h>
-#include <filesystem/console/console_device.h>
-#include <radio/SX1278/SX1278.h>
-
-#include <thread>
-
-#include "common.h"
-
-using namespace Boardcore;
-using namespace miosix;
-
-SPIBus bus(SPI4);
-
-GpioPin sck(GPIOE_BASE, 2);
-GpioPin miso(GPIOE_BASE, 5);
-GpioPin mosi(GPIOE_BASE, 6);
-GpioPin cs(GPIOC_BASE, 1);
-GpioPin dio(GPIOF_BASE, 10);
-
-SX1278* sx1278 = nullptr;
-
-void __attribute__((used)) EXTI10_IRQHandlerImpl()
-{
-    if (sx1278)
-        sx1278->handleDioIRQ();
-}
-
-/// Initialize stm32f407g board.
-void initBoard()
-{
-    {
-        miosix::FastInterruptDisableLock dLock;
-
-        // Enable SPI3
-        RCC->APB2ENR |= RCC_APB2ENR_SPI4EN;  // Enable SPI4 bus
-        RCC_SYNC();
-
-        // Setup SPI pins
-        sck.mode(miosix::Mode::ALTERNATE);
-        sck.alternateFunction(5);
-        miso.mode(miosix::Mode::ALTERNATE);
-        miso.alternateFunction(5);
-        mosi.mode(miosix::Mode::ALTERNATE);
-        mosi.alternateFunction(5);
-
-        cs.mode(miosix::Mode::OUTPUT);
-        dio.mode(miosix::Mode::INPUT);
-    }
-
-    cs.high();
-    enableExternalInterrupt(dio.getPort(), dio.getNumber(),
-                            InterruptTrigger::RISING_EDGE);
-}
-
-void recvLoop()
-{
-    uint8_t msg[256];
-    while (1)
-    {
-        int len = sx1278->receive(msg, sizeof(msg));
-        if (len > 0)
-        {
-            auto serial = miosix::DefaultConsole::instance().get();
-            serial->writeBlock(msg, len, 0);
-        }
-    }
-}
-
-void sendLoop()
-{
-    // I create a GPIO with the onboard led to tell the user that
-    // a package is being sent
-    miosix::GpioPin led(GPIOG_BASE, 13);
-    led.mode(miosix::Mode::OUTPUT);
-    led.low();
-
-    uint8_t msg[63];
-    while (1)
-    {
-        auto serial = miosix::DefaultConsole::instance().get();
-        int len     = serial->readBlock(msg, sizeof(msg), 0);
-        if (len > 0)
-        {
-            led.high();
-            sx1278->send(msg, len);
-            led.low();
-        }
-    }
-}
-
-int main()
-{
-    initBoard();
-
-    SX1278::Config config;
-    SX1278::Error err;
-
-    sx1278 = new SX1278(bus, cs);
-
-    printf("\n[sx1278] Configuring sx1278...\n");
-    printConfig(config);
-    if ((err = sx1278->init(config)) != SX1278::Error::NONE)
-    {
-        printf("[sx1278] sx1278->init error: %s\n", stringFromErr(err));
-        return -1;
-    }
-
-    printConfig(config);
-
-    printf("\n[sx1278] Initialization complete!\n");
-
-    std::thread recv([]() { recvLoop(); });
-    std::thread send([]() { sendLoop(); });
-
-    for (;;)
-    {
-        miosix::Thread::sleep(100);
-    }
-
-    // God please forgive me
-    // FIXME(davide.mor): ABSOLUTELY fix this
-    // miosix::Thread::sleep(20000);
-    // miosix::reboot();
-
-    return 0;
-}
diff --git a/src/tests/drivers/sx1278/common.h b/src/tests/radio/sx1278/fsk/common.h
similarity index 64%
rename from src/tests/drivers/sx1278/common.h
rename to src/tests/radio/sx1278/fsk/common.h
index c019e517f39b760b923c502be3e5a891b841f3ff..d95cacd0aa3f0b79eaa3463cf821c2572b8b9146 100644
--- a/src/tests/drivers/sx1278/common.h
+++ b/src/tests/radio/sx1278/fsk/common.h
@@ -22,73 +22,73 @@
 
 #pragma once
 
-#include <radio/SX1278/SX1278.h>
+#include <radio/SX1278/SX1278Fsk.h>
 
-static const char *stringFromErr(Boardcore::SX1278::Error err)
+static const char *stringFromErr(Boardcore::SX1278Fsk::Error err)
 {
     switch (err)
     {
-        case Boardcore::SX1278::Error::BAD_VALUE:
+        case Boardcore::SX1278Fsk::Error::BAD_VALUE:
             return "Error::BAD_VALUE";
-        case Boardcore::SX1278::Error::BAD_VERSION:
+        case Boardcore::SX1278Fsk::Error::BAD_VERSION:
             return "Error::BAD_VERSION";
         default:
             return "<unknown>";
     }
 }
 
-static const char *stringFromRxBw(Boardcore::SX1278::RxBw rx_bw)
+static const char *stringFromRxBw(Boardcore::SX1278Fsk::RxBw rx_bw)
 {
     switch (rx_bw)
     {
-        case Boardcore::SX1278::RxBw::HZ_2600:
+        case Boardcore::SX1278Fsk::RxBw::HZ_2600:
             return "RxBw::HZ_2600";
-        case Boardcore::SX1278::RxBw::HZ_3100:
+        case Boardcore::SX1278Fsk::RxBw::HZ_3100:
             return "RxBw::HZ_3100";
-        case Boardcore::SX1278::RxBw::HZ_3900:
+        case Boardcore::SX1278Fsk::RxBw::HZ_3900:
             return "RxBw::HZ_3900";
-        case Boardcore::SX1278::RxBw::HZ_5200:
+        case Boardcore::SX1278Fsk::RxBw::HZ_5200:
             return "RxBw::HZ_5200";
-        case Boardcore::SX1278::RxBw::HZ_6300:
+        case Boardcore::SX1278Fsk::RxBw::HZ_6300:
             return "RxBw::HZ_6300";
-        case Boardcore::SX1278::RxBw::HZ_7800:
+        case Boardcore::SX1278Fsk::RxBw::HZ_7800:
             return "RxBw::HZ_7800";
-        case Boardcore::SX1278::RxBw::HZ_10400:
+        case Boardcore::SX1278Fsk::RxBw::HZ_10400:
             return "RxBw::HZ_10400";
-        case Boardcore::SX1278::RxBw::HZ_12500:
+        case Boardcore::SX1278Fsk::RxBw::HZ_12500:
             return "RxBw::HZ_12500";
-        case Boardcore::SX1278::RxBw::HZ_15600:
+        case Boardcore::SX1278Fsk::RxBw::HZ_15600:
             return "RxBw::HZ_15600";
-        case Boardcore::SX1278::RxBw::HZ_20800:
+        case Boardcore::SX1278Fsk::RxBw::HZ_20800:
             return "RxBw::HZ_20800";
-        case Boardcore::SX1278::RxBw::HZ_25000:
+        case Boardcore::SX1278Fsk::RxBw::HZ_25000:
             return "RxBw::HZ_25000";
-        case Boardcore::SX1278::RxBw::HZ_31300:
+        case Boardcore::SX1278Fsk::RxBw::HZ_31300:
             return "RxBw::HZ_31300";
-        case Boardcore::SX1278::RxBw::HZ_41700:
+        case Boardcore::SX1278Fsk::RxBw::HZ_41700:
             return "RxBw::HZ_41700";
-        case Boardcore::SX1278::RxBw::HZ_50000:
+        case Boardcore::SX1278Fsk::RxBw::HZ_50000:
             return "RxBw::HZ_50000";
-        case Boardcore::SX1278::RxBw::HZ_62500:
+        case Boardcore::SX1278Fsk::RxBw::HZ_62500:
             return "RxBw::HZ_62500";
-        case Boardcore::SX1278::RxBw::HZ_83300:
+        case Boardcore::SX1278Fsk::RxBw::HZ_83300:
             return "RxBw::HZ_83300";
-        case Boardcore::SX1278::RxBw::HZ_100000:
+        case Boardcore::SX1278Fsk::RxBw::HZ_100000:
             return "RxBw::HZ_100000";
-        case Boardcore::SX1278::RxBw::HZ_125000:
+        case Boardcore::SX1278Fsk::RxBw::HZ_125000:
             return "RxBw::HZ_125000";
-        case Boardcore::SX1278::RxBw::HZ_166700:
+        case Boardcore::SX1278Fsk::RxBw::HZ_166700:
             return "RxBw::HZ_166700";
-        case Boardcore::SX1278::RxBw::HZ_200000:
+        case Boardcore::SX1278Fsk::RxBw::HZ_200000:
             return "RxBw::HZ_200000";
-        case Boardcore::SX1278::RxBw::HZ_250000:
+        case Boardcore::SX1278Fsk::RxBw::HZ_250000:
             return "RxBw::HZ_250000";
         default:
             return "<unknown>";
     }
 }
 
-static void printConfig(const Boardcore::SX1278::Config &config)
+static void printConfig(const Boardcore::SX1278Fsk::Config &config)
 {
     printf("config.freq_rf = %d\n", config.freq_rf);
     printf("config.freq_dev = %d\n", config.freq_dev);
diff --git a/src/tests/drivers/sx1278/gui/GUI.h b/src/tests/radio/sx1278/fsk/gui/GUI.h
similarity index 96%
rename from src/tests/drivers/sx1278/gui/GUI.h
rename to src/tests/radio/sx1278/fsk/gui/GUI.h
index 2286919f801221bbe14fd944cf3851dabfea9dcf..bd4544a4e595f265e2553cfe75127064f51416d2 100644
--- a/src/tests/drivers/sx1278/gui/GUI.h
+++ b/src/tests/radio/sx1278/fsk/gui/GUI.h
@@ -23,7 +23,7 @@
 #pragma once
 
 #include <mxgui/display.h>
-#include <radio/SX1278/SX1278.h>
+#include <radio/SX1278/SX1278Fsk.h>
 #include <utils/gui/GridLayout.h>
 #include <utils/gui/OptionView.h>
 #include <utils/gui/ScreenManager.h>
@@ -92,15 +92,15 @@ public:
         root.addView(&misc_data, 0.2);
     }
 
-    void updateError(Boardcore::SX1278::Error value)
+    void updateError(Boardcore::SX1278Fsk::Error value)
     {
         switch (value)
         {
-            case Boardcore::SX1278::Error::BAD_VALUE:
+            case Boardcore::SX1278Fsk::Error::BAD_VALUE:
                 status.setBackgroundColor(mxgui::red);
                 status.setText("BAD VALUE");
                 break;
-            case Boardcore::SX1278::Error::BAD_VERSION:
+            case Boardcore::SX1278Fsk::Error::BAD_VERSION:
                 status.setBackgroundColor(mxgui::red);
                 status.setText("BAD VERSION");
                 break;
diff --git a/src/tests/drivers/sx1278/test-sx1278-bench-gui.cpp b/src/tests/radio/sx1278/fsk/test-sx1278-bench-gui.cpp
similarity index 58%
rename from src/tests/drivers/sx1278/test-sx1278-bench-gui.cpp
rename to src/tests/radio/sx1278/fsk/test-sx1278-bench-gui.cpp
index 853c9002a5fccf6af160c41ee7408f335a85564e..2ae8e62082cb662296b9426ecc5312944fa06dc1 100644
--- a/src/tests/drivers/sx1278/test-sx1278-bench-gui.cpp
+++ b/src/tests/radio/sx1278/fsk/test-sx1278-bench-gui.cpp
@@ -21,7 +21,7 @@
  */
 
 #include <drivers/interrupt/external_interrupts.h>
-#include <radio/SX1278/SX1278.h>
+#include <radio/SX1278/SX1278Fsk.h>
 
 #include <thread>
 
@@ -35,16 +35,13 @@ using namespace Boardcore;
 using namespace miosix;
 using namespace mxgui;
 
-#if defined _BOARD_STM32F429ZI_SKYWARD_GS
+#if defined _BOARD_STM32F429ZI_SKYWARD_GS_V2
 #include "interfaces-impl/hwmapping.h"
 
-#if 1  // use ra01
-using cs   = peripherals::ra01::cs;
-using dio0 = peripherals::ra01::dio0;
-#else
-using cs   = peripherals::sx127x::cs;
-using dio0 = peripherals::sx127x::dio0;
-#endif
+using cs   = peripherals::ra01::pc13::cs;
+using dio0 = peripherals::ra01::pc13::dio0;
+using dio1 = peripherals::ra01::pc13::dio1;
+using dio3 = peripherals::ra01::pc13::dio3;
 
 using sck  = interfaces::spi4::sck;
 using miso = interfaces::spi4::miso;
@@ -52,31 +49,48 @@ using mosi = interfaces::spi4::mosi;
 
 #define SX1278_SPI SPI4
 
+#define SX1278_IRQ_DIO0 EXTI6_IRQHandlerImpl
+#define SX1278_IRQ_DIO1 EXTI2_IRQHandlerImpl
+#define SX1278_IRQ_DIO3 EXTI11_IRQHandlerImpl
+
 #else
 #error "Target not supported"
 #endif
 
-GUI *gui = nullptr;
+void __attribute__((used)) SX1278_IRQ_DIO0()
+{
+    if (sx1278)
+        sx1278->handleDioIRQ(SX1278Fsk::Dio::DIO0);
+}
 
-#if defined _BOARD_STM32F429ZI_SKYWARD_GS
-void __attribute__((used)) EXTI6_IRQHandlerImpl()
-#else
-#error "Target not supported"
-#endif
+void __attribute__((used)) SX1278_IRQ_DIO1()
 {
     if (sx1278)
-        sx1278->handleDioIRQ();
+        sx1278->handleDioIRQ(SX1278Fsk::Dio::DIO1);
+}
+
+void __attribute__((used)) SX1278_IRQ_DIO3()
+{
+    if (sx1278)
+        sx1278->handleDioIRQ(SX1278Fsk::Dio::DIO3);
 }
 
 void initBoard()
 {
-#if defined _BOARD_STM32F429ZI_SKYWARD_GS
-    enableExternalInterrupt(GPIOF_BASE, 6, InterruptTrigger::RISING_EDGE);
-#else
-#error "Target not supported"
-#endif
+    GpioPin dio0_pin = dio0::getPin();
+    GpioPin dio1_pin = dio1::getPin();
+    GpioPin dio3_pin = dio3::getPin();
+
+    enableExternalInterrupt(dio0_pin.getPort(), dio0_pin.getNumber(),
+                            InterruptTrigger::RISING_EDGE);
+    enableExternalInterrupt(dio1_pin.getPort(), dio1_pin.getNumber(),
+                            InterruptTrigger::RISING_EDGE);
+    enableExternalInterrupt(dio3_pin.getPort(), dio3_pin.getNumber(),
+                            InterruptTrigger::RISING_EDGE);
 }
 
+GUI *gui = nullptr;
+
 void initGUI()
 {
     // TODO: This should be in bsp
@@ -95,17 +109,30 @@ int main()
     initBoard();
     initGUI();
 
-    // Run default configuration
-    SX1278::Config config;
-    SX1278::Error err;
+    SX1278Fsk::Config config = {
+        .freq_rf  = 422075000,
+        .freq_dev = 25000,
+        .bitrate  = 19200,
+        .rx_bw    = Boardcore::SX1278Fsk::RxBw::HZ_83300,
+        .afc_bw   = Boardcore::SX1278Fsk::RxBw::HZ_125000,
+        .ocp      = 120,
+        .power    = 17,
+        .shaping  = Boardcore::SX1278Fsk::Shaping::GAUSSIAN_BT_1_0,
+        .dc_free  = Boardcore::SX1278Fsk::DcFree::WHITENING};
+    SX1278Fsk::Error err;
 
     SPIBus bus(SX1278_SPI);
     GpioPin cs = cs::getPin();
 
-    sx1278 = new SX1278(bus, cs);
+    SPIBusConfig spi_config = {};
+    spi_config.clockDivider = SPI::ClockDivider::DIV_64;
+    spi_config.mode         = SPI::Mode::MODE_0;
+    spi_config.bitOrder     = SPI::BitOrder::MSB_FIRST;
+
+    sx1278 = new SX1278Fsk(SPISlave(bus, cs, spi_config));
 
     printf("\n[sx1278] Configuring sx1278...\n");
-    if ((err = sx1278->init(config)) != SX1278::Error::NONE)
+    if ((err = sx1278->init(config)) != SX1278Fsk::Error::NONE)
     {
         gui->stats_screen.updateError(err);
         printf("[sx1278] sx1278->init error: %s\n", stringFromErr(err));
diff --git a/src/tests/drivers/sx1278/test-sx1278-bench-serial.cpp b/src/tests/radio/sx1278/fsk/test-sx1278-bench-serial.cpp
similarity index 53%
rename from src/tests/drivers/sx1278/test-sx1278-bench-serial.cpp
rename to src/tests/radio/sx1278/fsk/test-sx1278-bench-serial.cpp
index 897707629ed3c00b4ff000d217b1acc48e8b1532..3156a377ad7b42c6b7322725d4387859914a65b6 100644
--- a/src/tests/drivers/sx1278/test-sx1278-bench-serial.cpp
+++ b/src/tests/radio/sx1278/fsk/test-sx1278-bench-serial.cpp
@@ -21,7 +21,7 @@
  */
 
 #include <drivers/interrupt/external_interrupts.h>
-#include <radio/SX1278/SX1278.h>
+#include <radio/SX1278/SX1278Fsk.h>
 
 #include <thread>
 
@@ -33,16 +33,13 @@
 using namespace Boardcore;
 using namespace miosix;
 
-#if defined _BOARD_STM32F429ZI_SKYWARD_GS
+#if defined _BOARD_STM32F429ZI_SKYWARD_GS_V2
 #include "interfaces-impl/hwmapping.h"
 
-#if 1  // use ra01
-using cs   = peripherals::ra01::cs;
-using dio0 = peripherals::ra01::dio0;
-#else
-using cs   = peripherals::sx127x::cs;
-using dio0 = peripherals::sx127x::dio0;
-#endif
+using cs   = peripherals::ra01::pc13::cs;
+using dio0 = peripherals::ra01::pc13::dio0;
+using dio1 = peripherals::ra01::pc13::dio1;
+using dio3 = peripherals::ra01::pc13::dio3;
 
 using sck  = interfaces::spi4::sck;
 using miso = interfaces::spi4::miso;
@@ -50,60 +47,84 @@ using mosi = interfaces::spi4::mosi;
 
 #define SX1278_SPI SPI4
 
-#elif defined _BOARD_STM32F429ZI_SKYWARD_DEATHST_V3
-#include "interfaces-impl/hwmapping.h"
-
-using cs   = sensors::sx127x::cs;
-using dio0 = sensors::sx127x::dio0;
-
-using sck  = interfaces::spi5::sck;
-using miso = interfaces::spi5::miso;
-using mosi = interfaces::spi5::mosi;
-
-#define SX1278_SPI SPI5
+#define SX1278_IRQ_DIO0 EXTI6_IRQHandlerImpl
+#define SX1278_IRQ_DIO1 EXTI2_IRQHandlerImpl
+#define SX1278_IRQ_DIO3 EXTI11_IRQHandlerImpl
 
 #else
 #error "Target not supported"
 #endif
 
-#if defined _BOARD_STM32F429ZI_SKYWARD_GS
-void __attribute__((used)) EXTI6_IRQHandlerImpl()
-#elif defined _BOARD_STM32F429ZI_SKYWARD_DEATHST_V3
-void __attribute__((used)) EXTI10_IRQHandlerImpl()
-#else
-#error "Target not supported"
-#endif
+volatile int dio0_cnt = 0;
+volatile int dio1_cnt = 0;
+volatile int dio3_cnt = 0;
+
+void __attribute__((used)) SX1278_IRQ_DIO0()
 {
     if (sx1278)
-        sx1278->handleDioIRQ();
+        sx1278->handleDioIRQ(SX1278Fsk::Dio::DIO0);
+
+    dio0_cnt++;
+}
+
+void __attribute__((used)) SX1278_IRQ_DIO1()
+{
+    if (sx1278)
+        sx1278->handleDioIRQ(SX1278Fsk::Dio::DIO1);
+
+    dio1_cnt++;
+}
+
+void __attribute__((used)) SX1278_IRQ_DIO3()
+{
+    if (sx1278)
+        sx1278->handleDioIRQ(SX1278Fsk::Dio::DIO3);
+
+    dio3_cnt++;
 }
 
 void initBoard()
 {
-#if defined _BOARD_STM32F429ZI_SKYWARD_GS
-    enableExternalInterrupt(GPIOF_BASE, 6, InterruptTrigger::RISING_EDGE);
-#elif defined _BOARD_STM32F429ZI_SKYWARD_DEATHST_V3
-    enableExternalInterrupt(GPIOF_BASE, 10, InterruptTrigger::RISING_EDGE);
-#else
-#error "Target not supported"
-#endif
+    GpioPin dio0_pin = dio0::getPin();
+    GpioPin dio1_pin = dio1::getPin();
+    GpioPin dio3_pin = dio3::getPin();
+
+    enableExternalInterrupt(dio0_pin.getPort(), dio0_pin.getNumber(),
+                            InterruptTrigger::RISING_EDGE);
+    enableExternalInterrupt(dio1_pin.getPort(), dio1_pin.getNumber(),
+                            InterruptTrigger::RISING_EDGE);
+    enableExternalInterrupt(dio3_pin.getPort(), dio3_pin.getNumber(),
+                            InterruptTrigger::RISING_EDGE);
 }
 
 int main()
 {
     initBoard();
 
-    // Run default configuration
-    SX1278::Config config;
-    SX1278::Error err;
+    SX1278Fsk::Config config = {
+        .freq_rf  = 422075000,
+        .freq_dev = 25000,
+        .bitrate  = 19200,
+        .rx_bw    = Boardcore::SX1278Fsk::RxBw::HZ_83300,
+        .afc_bw   = Boardcore::SX1278Fsk::RxBw::HZ_125000,
+        .ocp      = 120,
+        .power    = 17,
+        .shaping  = Boardcore::SX1278Fsk::Shaping::GAUSSIAN_BT_1_0,
+        .dc_free  = Boardcore::SX1278Fsk::DcFree::WHITENING};
+    SX1278Fsk::Error err;
 
     SPIBus bus(SX1278_SPI);
     GpioPin cs = cs::getPin();
 
-    sx1278 = new SX1278(bus, cs);
+    SPIBusConfig spi_config = {};
+    spi_config.clockDivider = SPI::ClockDivider::DIV_64;
+    spi_config.mode         = SPI::Mode::MODE_0;
+    spi_config.bitOrder     = SPI::BitOrder::MSB_FIRST;
+
+    sx1278 = new SX1278Fsk(SPISlave(bus, cs, spi_config));
 
     printf("\n[sx1278] Configuring sx1278...\n");
-    if ((err = sx1278->init(config)) != SX1278::Error::NONE)
+    if ((err = sx1278->init(config)) != SX1278Fsk::Error::NONE)
     {
         printf("[sx1278] sx1278->init error: %s\n", stringFromErr(err));
         return -1;
@@ -126,11 +147,14 @@ int main()
             "Corrupted packets: %d\n"
             "Packet loss:       %.2f %%\n"
             "RSSI:              %.2f dBm\n"
-            "FEI:               %.2f dBm\n",
+            "FEI:               %.2f dBm\n"
+            "dio0:              %d\n"
+            "dio1:              %d\n"
+            "dio3:              %d\n",
             static_cast<float>(stats.txBitrate()) / 1000.0f, stats.sent_count,
             static_cast<float>(stats.rxBitrate()) / 1000.0f, stats.recv_count,
             stats.corrupted_count, 0.0f /* TODO: Packet loss */, stats.rssi,
-            stats.fei);
+            stats.fei, dio0_cnt, dio1_cnt, dio3_cnt);
 
         miosix::Thread::sleep(2000);
     }
diff --git a/src/tests/drivers/sx1278/test-sx1278-bench.cpp b/src/tests/radio/sx1278/fsk/test-sx1278-bench.cpp
similarity index 98%
rename from src/tests/drivers/sx1278/test-sx1278-bench.cpp
rename to src/tests/radio/sx1278/fsk/test-sx1278-bench.cpp
index 205b2bba61f59c146110e0c2df43ba2209c7bace..66e153a862d7c9c74f44fdd0ab51767c6795ff46 100644
--- a/src/tests/drivers/sx1278/test-sx1278-bench.cpp
+++ b/src/tests/radio/sx1278/fsk/test-sx1278-bench.cpp
@@ -22,7 +22,7 @@
 
 #include <drivers/timer/TimestampTimer.h>
 #include <miosix.h>
-#include <radio/SX1278/SX1278.h>
+#include <radio/SX1278/SX1278Fsk.h>
 #include <utils/MovingAverage.h>
 
 #include <thread>
@@ -30,7 +30,7 @@
 using namespace Boardcore;
 using namespace miosix;
 
-SX1278 *sx1278 = nullptr;
+SX1278Fsk *sx1278 = nullptr;
 
 // Simple xorshift RNG
 uint32_t xorshift32()
diff --git a/src/tests/drivers/sx1278/test-sx1278-bidir.cpp b/src/tests/radio/sx1278/fsk/test-sx1278-bidir.cpp
similarity index 60%
rename from src/tests/drivers/sx1278/test-sx1278-bidir.cpp
rename to src/tests/radio/sx1278/fsk/test-sx1278-bidir.cpp
index d6a04455448cd975d3a2435d428616777e33315f..828b9733e3e441f2233ba1e425a057a88f2346b9 100644
--- a/src/tests/drivers/sx1278/test-sx1278-bidir.cpp
+++ b/src/tests/radio/sx1278/fsk/test-sx1278-bidir.cpp
@@ -22,7 +22,8 @@
 
 #include <drivers/interrupt/external_interrupts.h>
 #include <miosix.h>
-#include <radio/SX1278/SX1278.h>
+#include <radio/SX1278/Ebyte.h>
+#include <radio/SX1278/SX1278Fsk.h>
 
 #include <cstring>
 #include <thread>
@@ -32,62 +33,68 @@
 using namespace Boardcore;
 using namespace miosix;
 
-#if defined _BOARD_STM32F429ZI_SKYWARD_GS
+#if defined _BOARD_STM32F429ZI_SKYWARD_GS_V2
 #include "interfaces-impl/hwmapping.h"
 
-#if 1  // use ra01
-using cs   = peripherals::ra01::cs;
-using dio0 = peripherals::ra01::dio0;
-#else
-using cs   = peripherals::sx127x::cs;
-using dio0 = peripherals::sx127x::dio0;
-#endif
+using cs   = peripherals::ra01::pc13::cs;
+using dio0 = peripherals::ra01::pc13::dio0;
+using dio1 = peripherals::ra01::pc13::dio1;
+using dio3 = peripherals::ra01::pc13::dio3;
 
 using sck  = interfaces::spi4::sck;
 using miso = interfaces::spi4::miso;
 using mosi = interfaces::spi4::mosi;
 
-#define SX1278_SPI SPI4
-
-#elif defined _BOARD_STM32F429ZI_SKYWARD_DEATHST_V3
-#include "interfaces-impl/hwmapping.h"
-
-using cs   = sensors::sx127x::cs;
-using dio0 = sensors::sx127x::dio0;
+using txen = Gpio<GPIOE_BASE, 4>;
+using rxen = Gpio<GPIOD_BASE, 4>;
 
-using sck  = interfaces::spi5::sck;
-using miso = interfaces::spi5::miso;
-using mosi = interfaces::spi5::mosi;
+#define SX1278_SPI SPI4
 
-#define SX1278_SPI SPI5
+#define SX1278_IRQ_DIO0 EXTI6_IRQHandlerImpl
+#define SX1278_IRQ_DIO1 EXTI2_IRQHandlerImpl
+#define SX1278_IRQ_DIO3 EXTI11_IRQHandlerImpl
 
 #else
 #error "Target not supported"
 #endif
 
-SX1278 *sx1278 = nullptr;
+SX1278Fsk *sx1278 = nullptr;
 
-#if defined _BOARD_STM32F429ZI_SKYWARD_GS
-void __attribute__((used)) EXTI6_IRQHandlerImpl()
-#elif defined _BOARD_STM32F429ZI_SKYWARD_DEATHST_V3
-void __attribute__((used)) EXTI10_IRQHandlerImpl()
-#else
-#error "Target not supported"
-#endif
+void __attribute__((used)) SX1278_IRQ_DIO0()
 {
     if (sx1278)
-        sx1278->handleDioIRQ();
+        sx1278->handleDioIRQ(SX1278Fsk::Dio::DIO0);
+}
+
+void __attribute__((used)) SX1278_IRQ_DIO1()
+{
+    if (sx1278)
+        sx1278->handleDioIRQ(SX1278Fsk::Dio::DIO1);
+}
+
+void __attribute__((used)) SX1278_IRQ_DIO3()
+{
+    if (sx1278)
+        sx1278->handleDioIRQ(SX1278Fsk::Dio::DIO3);
 }
 
 void initBoard()
 {
-#if defined _BOARD_STM32F429ZI_SKYWARD_GS
-    enableExternalInterrupt(GPIOF_BASE, 6, InterruptTrigger::RISING_EDGE);
-#elif defined _BOARD_STM32F429ZI_SKYWARD_DEATHST_V3
-    enableExternalInterrupt(GPIOF_BASE, 10, InterruptTrigger::RISING_EDGE);
-#else
-#error "Target not supported"
-#endif
+    rxen::mode(Mode::OUTPUT);
+    txen::mode(Mode::OUTPUT);
+    rxen::low();
+    txen::low();
+
+    GpioPin dio0_pin = dio0::getPin();
+    GpioPin dio1_pin = dio1::getPin();
+    GpioPin dio3_pin = dio3::getPin();
+
+    enableExternalInterrupt(dio0_pin.getPort(), dio0_pin.getNumber(),
+                            InterruptTrigger::RISING_EDGE);
+    enableExternalInterrupt(dio1_pin.getPort(), dio1_pin.getNumber(),
+                            InterruptTrigger::RISING_EDGE);
+    enableExternalInterrupt(dio3_pin.getPort(), dio3_pin.getNumber(),
+                            InterruptTrigger::RISING_EDGE);
 }
 
 void recvLoop()
@@ -126,16 +133,24 @@ int main()
     initBoard();
 
     // Run default configuration
-    SX1278::Config config;
-    SX1278::Error err;
+    SX1278Fsk::Config config;
+    config.power = 2;
+
+    SX1278Fsk::Error err;
 
     SPIBus bus(SX1278_SPI);
     GpioPin cs = cs::getPin();
 
-    sx1278 = new SX1278(bus, cs);
+    SPIBusConfig spi_config = {};
+    spi_config.clockDivider = SPI::ClockDivider::DIV_64;
+    spi_config.mode         = SPI::Mode::MODE_0;
+    spi_config.bitOrder     = SPI::BitOrder::MSB_FIRST;
+
+    sx1278 = new EbyteFsk(SPISlave(bus, cs, spi_config), txen::getPin(),
+                          rxen::getPin());
 
     printf("\n[sx1278] Configuring sx1278...\n");
-    if ((err = sx1278->init(config)) != SX1278::Error::NONE)
+    if ((err = sx1278->init(config)) != SX1278Fsk::Error::NONE)
     {
         printf("[sx1278] sx1278->init error: %s\n", stringFromErr(err));
         return -1;
diff --git a/src/tests/drivers/sx1278/test-sx1278-mavlink.cpp b/src/tests/radio/sx1278/fsk/test-sx1278-mavlink.cpp
similarity index 71%
rename from src/tests/drivers/sx1278/test-sx1278-mavlink.cpp
rename to src/tests/radio/sx1278/fsk/test-sx1278-mavlink.cpp
index ea0897055d64f61f620f2fbdcaba6501c71121ca..fba3c8bd5589affc6508e0ca4df4b8e48d5877d7 100644
--- a/src/tests/drivers/sx1278/test-sx1278-mavlink.cpp
+++ b/src/tests/radio/sx1278/fsk/test-sx1278-mavlink.cpp
@@ -21,7 +21,7 @@
  */
 
 #include <drivers/interrupt/external_interrupts.h>
-#include <radio/SX1278/SX1278.h>
+#include <radio/SX1278/SX1278Fsk.h>
 
 #include <thread>
 
@@ -51,53 +51,64 @@ constexpr uint32_t STATS_TM_PERIOD      = 2000;
 using Mav =
     MavlinkDriver<RADIO_PKT_LENGTH, RADIO_OUT_QUEUE_SIZE, RADIO_MAV_MSG_LENGTH>;
 
-Mav* channel;
-SX1278* sx1278 = nullptr;
-
 #if defined _BOARD_STM32F429ZI_SKYWARD_GS_V2
 #include "interfaces-impl/hwmapping.h"
 
-#define USE_RA01_PC13
-
-#ifdef USE_RA01_PC13  // use ra01
 using cs   = peripherals::ra01::pc13::cs;
 using dio0 = peripherals::ra01::pc13::dio0;
-#else
-using cs   = peripherals::ra01::pe4::cs;
-using dio0 = peripherals::ra01::pe4::dio0;
-#endif
+using dio1 = peripherals::ra01::pc13::dio1;
+using dio3 = peripherals::ra01::pc13::dio3;
+
+using sck  = interfaces::spi4::sck;
+using miso = interfaces::spi4::miso;
+using mosi = interfaces::spi4::mosi;
 
 #define SX1278_SPI SPI4
 
-#else
-#error "Target not supported"
-#endif
+#define SX1278_IRQ_DIO0 EXTI6_IRQHandlerImpl
+#define SX1278_IRQ_DIO1 EXTI2_IRQHandlerImpl
+#define SX1278_IRQ_DIO3 EXTI11_IRQHandlerImpl
 
-#if defined _BOARD_STM32F429ZI_SKYWARD_GS_V2
-#ifdef USE_RA01_PC13
-void __attribute__((used)) EXTI6_IRQHandlerImpl()
-#else
-void __attribute__((used)) EXTI3_IRQHandlerImpl()
-#endif
 #else
 #error "Target not supported"
 #endif
+
+SX1278Fsk* sx1278 = nullptr;
+
+void __attribute__((used)) SX1278_IRQ_DIO0()
 {
     if (sx1278)
-        sx1278->handleDioIRQ();
+        sx1278->handleDioIRQ(SX1278Fsk::Dio::DIO0);
+}
+
+void __attribute__((used)) SX1278_IRQ_DIO1()
+{
+    if (sx1278)
+        sx1278->handleDioIRQ(SX1278Fsk::Dio::DIO1);
+}
+
+void __attribute__((used)) SX1278_IRQ_DIO3()
+{
+    if (sx1278)
+        sx1278->handleDioIRQ(SX1278Fsk::Dio::DIO3);
 }
 
 void initBoard()
 {
-#if defined _BOARD_STM32F429ZI_SKYWARD_GS_V2
-    GpioPin dio0 = dio0::getPin();
-    enableExternalInterrupt(dio0.getPort(), dio0.getNumber(),
+    GpioPin dio0_pin = dio0::getPin();
+    GpioPin dio1_pin = dio1::getPin();
+    GpioPin dio3_pin = dio3::getPin();
+
+    enableExternalInterrupt(dio0_pin.getPort(), dio0_pin.getNumber(),
+                            InterruptTrigger::RISING_EDGE);
+    enableExternalInterrupt(dio1_pin.getPort(), dio1_pin.getNumber(),
+                            InterruptTrigger::RISING_EDGE);
+    enableExternalInterrupt(dio3_pin.getPort(), dio3_pin.getNumber(),
                             InterruptTrigger::RISING_EDGE);
-#else
-#error "Target not supported"
-#endif
 }
 
+Mav* channel;
+
 void onReceive(Mav* channel, const mavlink_message_t& msg)
 {
     // Prepare ack messages
@@ -144,26 +155,21 @@ int main()
 {
     initBoard();
 
-    // Run default configuration
-    SX1278::Config config = {.freq_rf  = 412000000,
-                             .freq_dev = 25000,
-                             .bitrate  = 19200,
-                             .rx_bw    = SX1278::RxBw::HZ_83300,
-                             .afc_bw   = SX1278::RxBw::HZ_125000,
-                             .ocp      = 120,
-                             .power    = 17,
-                             .shaping  = SX1278::Shaping::GAUSSIAN_BT_1_0,
-                             .dc_free  = SX1278::DcFree::WHITENING};
-
-    SX1278::Error err;
+    SX1278Fsk::Config config;
+    SX1278Fsk::Error err;
 
     SPIBus bus(SX1278_SPI);
     GpioPin cs = cs::getPin();
 
-    sx1278 = new SX1278(bus, cs);
+    SPIBusConfig spi_config;
+    spi_config.clockDivider = SPI::ClockDivider::DIV_64;
+    spi_config.mode         = SPI::Mode::MODE_0;
+    spi_config.bitOrder     = SPI::BitOrder::MSB_FIRST;
+
+    sx1278 = new SX1278Fsk(SPISlave(bus, cs, spi_config));
 
     printf("\n[sx1278] Configuring sx1278...\n");
-    if ((err = sx1278->init(config)) != SX1278::Error::NONE)
+    if ((err = sx1278->init(config)) != SX1278Fsk::Error::NONE)
     {
         printf("[sx1278] sx1278->init error: %s\n", stringFromErr(err));
 
diff --git a/src/tests/radio/sx1278/lora/test-sx1278-bidir.cpp b/src/tests/radio/sx1278/lora/test-sx1278-bidir.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..08b0c7e717606e54956fc2b456f2a90c74fe0fff
--- /dev/null
+++ b/src/tests/radio/sx1278/lora/test-sx1278-bidir.cpp
@@ -0,0 +1,219 @@
+/* Copyright (c) 2021 Skyward Experimental Rocketry
+ * Author: Davide Mor
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#include <drivers/interrupt/external_interrupts.h>
+#include <miosix.h>
+#include <radio/SX1278/Ebyte.h>
+#include <radio/SX1278/SX1278Lora.h>
+
+#include <cstring>
+#include <thread>
+
+using namespace Boardcore;
+using namespace miosix;
+
+#if defined _BOARD_STM32F429ZI_SKYWARD_GS_V2
+#include "interfaces-impl/hwmapping.h"
+
+// Uncomment the following line to enable Ebyte module
+// #define IS_EBYTE
+
+using cs   = peripherals::ra01::pc13::cs;
+using dio0 = peripherals::ra01::pc13::dio0;
+using dio1 = peripherals::ra01::pc13::dio1;
+using dio3 = peripherals::ra01::pc13::dio3;
+
+using sck  = interfaces::spi4::sck;
+using miso = interfaces::spi4::miso;
+using mosi = interfaces::spi4::mosi;
+
+#ifdef IS_EBYTE
+using txen = Gpio<GPIOE_BASE, 4>;
+using rxen = Gpio<GPIOD_BASE, 4>;
+#endif
+
+#define SX1278_SPI SPI4
+
+#define SX1278_IRQ_DIO0 EXTI6_IRQHandlerImpl
+#define SX1278_IRQ_DIO1 EXTI2_IRQHandlerImpl
+#define SX1278_IRQ_DIO3 EXTI11_IRQHandlerImpl
+
+#else
+#error "Target not supported"
+#endif
+
+SX1278Lora *sx1278 = nullptr;
+
+void __attribute__((used)) SX1278_IRQ_DIO0()
+{
+    if (sx1278)
+        sx1278->handleDioIRQ(SX1278Lora::Dio::DIO0);
+}
+
+void __attribute__((used)) SX1278_IRQ_DIO1()
+{
+    if (sx1278)
+        sx1278->handleDioIRQ(SX1278Lora::Dio::DIO1);
+}
+
+void __attribute__((used)) SX1278_IRQ_DIO3()
+{
+    if (sx1278)
+        sx1278->handleDioIRQ(SX1278Lora::Dio::DIO3);
+}
+
+void initBoard()
+{
+#ifdef IS_EBYTE
+    rxen::mode(Mode::OUTPUT);
+    txen::mode(Mode::OUTPUT);
+    rxen::low();
+    txen::low();
+#endif
+
+    GpioPin dio0_pin = dio0::getPin();
+    GpioPin dio1_pin = dio1::getPin();
+    GpioPin dio3_pin = dio3::getPin();
+
+    enableExternalInterrupt(dio0_pin.getPort(), dio0_pin.getNumber(),
+                            InterruptTrigger::RISING_EDGE);
+    enableExternalInterrupt(dio1_pin.getPort(), dio1_pin.getNumber(),
+                            InterruptTrigger::RISING_EDGE);
+    enableExternalInterrupt(dio3_pin.getPort(), dio3_pin.getNumber(),
+                            InterruptTrigger::RISING_EDGE);
+}
+
+bool isByteBuf(uint8_t *buf, ssize_t len)
+{
+    for (ssize_t i = 0; i < len; i++)
+    {
+        if (!isprint(buf[i]))
+            return true;
+    }
+
+    return false;
+}
+
+void formatByteBuf(uint8_t *buf, ssize_t len, char *out)
+{
+    for (ssize_t i = 0; i < len; i++)
+    {
+        if (i == 0)
+            out += sprintf(out, "%02X", buf[i]);
+        else
+            out += sprintf(out, ":%02X", buf[i]);
+    }
+
+    // Put terminator at the end
+    *out = '\0';
+}
+
+void recvLoop()
+{
+    uint8_t buf[SX1278Lora::MTU];
+    while (1)
+    {
+        ssize_t res = sx1278->receive(buf, sizeof(buf));
+        if (res != -1)
+        {
+            char msg[512] = {0};
+            if (isByteBuf(buf, res))
+            {
+                // This is a byte buffer, print it as an hexadecimal string
+                formatByteBuf(buf, res, msg);
+            }
+            else
+            {
+                memcpy(msg, buf, res);
+                msg[res + 1] = '\0';
+            }
+
+            // Make sure there is a terminator somewhere
+            buf[res] = 0;
+            printf("[sx1278] Received '%s', power: %.2f, snr: %.2f\n", msg,
+                   sx1278->getLastRxRssi(), sx1278->getLastRxSnr());
+        }
+    }
+}
+
+void sendLoop(int interval, const char *data)
+{
+    char buf[64];
+    strncpy(buf, data, sizeof(buf) - 1);
+
+    while (1)
+    {
+        miosix::Thread::sleep(interval);
+
+        sx1278->send(reinterpret_cast<uint8_t *>(buf), strlen(buf));
+        printf("[sx1278] Sent '%s'\n", buf);
+    }
+}
+
+int main()
+{
+    initBoard();
+
+    SX1278Lora::Config config = {};
+    config.pa_boost           = true;
+    config.power              = 10;
+    config.ocp                = 0;
+    config.coding_rate        = SX1278Lora::Config::Cr::CR_4;
+    config.spreading_factor   = SX1278Lora::Config::Sf::SF_12;
+
+    SX1278Lora::Error err;
+
+    SPIBus bus(SX1278_SPI);
+    GpioPin cs = cs::getPin();
+
+    SPIBusConfig spi_config = {};
+    spi_config.clockDivider = SPI::ClockDivider::DIV_64;
+    spi_config.mode         = SPI::Mode::MODE_0;
+    spi_config.bitOrder     = SPI::BitOrder::MSB_FIRST;
+
+#ifdef IS_EBYTE
+    sx1278 = new EbyteLora(SPISlave(bus, cs, spi_config), txen::getPin(),
+                           rxen::getPin());
+#else
+    sx1278 = new SX1278Lora(SPISlave(bus, cs, spi_config));
+#endif
+
+    printf("\n[sx1278] Configuring sx1278...\n");
+    if ((err = sx1278->init(config)) != SX1278Lora::Error::NONE)
+    {
+        printf("[sx1278] sx1278->init error: %s\n", "TODO");
+        return -1;
+    }
+
+    printf("\n[sx1278] Initialization complete!\n");
+
+    // Spawn all threads
+    std::thread send([]() { sendLoop(3333, "Sample radio message"); });
+    std::thread recv([]() { recvLoop(); });
+
+    // sx1278->debugDumpRegisters();
+
+    while (1)
+        miosix::Thread::wait();
+
+    return 0;
+}
diff --git a/src/tests/radio/sx1278/lora/test-sx1278-mavlink.cpp b/src/tests/radio/sx1278/lora/test-sx1278-mavlink.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..88422766f6af9b76835c6f634007f378e0704af0
--- /dev/null
+++ b/src/tests/radio/sx1278/lora/test-sx1278-mavlink.cpp
@@ -0,0 +1,230 @@
+/* Copyright (c) 2018 Skyward Experimental Rocketry
+ * Authors: Alvise de'Faveri Tron, Nuno Barcellos, Davide Mor
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#include <drivers/interrupt/external_interrupts.h>
+#include <radio/SX1278/SX1278Lora.h>
+
+#include <thread>
+
+// Ignore warnings, as we don't want to change third party generated files to
+// fix them
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wcast-align"
+#pragma GCC diagnostic ignored "-Waddress-of-packed-member"
+#include <mavlink_lib/pyxis/mavlink.h>
+#pragma GCC diagnostic pop
+
+#include <radio/MavlinkDriver/MavlinkDriver.h>
+
+using namespace Boardcore;
+using namespace miosix;
+
+constexpr uint32_t RADIO_PKT_LENGTH     = 255;
+constexpr uint32_t RADIO_OUT_QUEUE_SIZE = 10;
+constexpr uint32_t RADIO_MAV_MSG_LENGTH = MAVLINK_MAX_DIALECT_PAYLOAD_SIZE;
+constexpr size_t MAV_OUT_BUFFER_MAX_AGE = 0;
+constexpr uint16_t SLEEP_AFTER_SEND     = 0;
+constexpr uint32_t FLIGHT_TM_PERIOD     = 8000;
+constexpr uint32_t STATS_TM_PERIOD      = 1000;
+
+// Mavlink out buffer with 10 packets, 256 bytes each.
+using Mav =
+    MavlinkDriver<RADIO_PKT_LENGTH, RADIO_OUT_QUEUE_SIZE, RADIO_MAV_MSG_LENGTH>;
+
+#if defined _BOARD_STM32F429ZI_SKYWARD_GS_V2
+#include "interfaces-impl/hwmapping.h"
+
+// Uncomment the following line to enable Ebyte module
+// #define IS_EBYTE
+
+using cs   = peripherals::ra01::pc13::cs;
+using dio0 = peripherals::ra01::pc13::dio0;
+using dio1 = peripherals::ra01::pc13::dio1;
+using dio3 = peripherals::ra01::pc13::dio3;
+
+using sck  = interfaces::spi4::sck;
+using miso = interfaces::spi4::miso;
+using mosi = interfaces::spi4::mosi;
+
+#ifdef IS_EBYTE
+using txen = Gpio<GPIOE_BASE, 4>;
+using rxen = Gpio<GPIOD_BASE, 4>;
+#endif
+
+#define SX1278_SPI SPI4
+
+#define SX1278_IRQ_DIO0 EXTI6_IRQHandlerImpl
+#define SX1278_IRQ_DIO1 EXTI2_IRQHandlerImpl
+#define SX1278_IRQ_DIO3 EXTI11_IRQHandlerImpl
+
+#else
+#error "Target not supported"
+#endif
+
+SX1278Lora* sx1278 = nullptr;
+
+void __attribute__((used)) SX1278_IRQ_DIO0()
+{
+    if (sx1278)
+        sx1278->handleDioIRQ(SX1278Lora::Dio::DIO0);
+}
+
+void __attribute__((used)) SX1278_IRQ_DIO1()
+{
+    if (sx1278)
+        sx1278->handleDioIRQ(SX1278Lora::Dio::DIO1);
+}
+
+void __attribute__((used)) SX1278_IRQ_DIO3()
+{
+    if (sx1278)
+        sx1278->handleDioIRQ(SX1278Lora::Dio::DIO3);
+}
+
+void initBoard()
+{
+#ifdef IS_EBYTE
+    rxen::mode(Mode::OUTPUT);
+    txen::mode(Mode::OUTPUT);
+    rxen::low();
+    txen::low();
+#endif
+
+    GpioPin dio0_pin = dio0::getPin();
+    GpioPin dio1_pin = dio1::getPin();
+    GpioPin dio3_pin = dio3::getPin();
+
+    enableExternalInterrupt(dio0_pin.getPort(), dio0_pin.getNumber(),
+                            InterruptTrigger::RISING_EDGE);
+    enableExternalInterrupt(dio1_pin.getPort(), dio1_pin.getNumber(),
+                            InterruptTrigger::RISING_EDGE);
+    enableExternalInterrupt(dio3_pin.getPort(), dio3_pin.getNumber(),
+                            InterruptTrigger::RISING_EDGE);
+}
+
+Mav* channel;
+
+void onReceive(Mav* channel, const mavlink_message_t& msg)
+{
+    printf("Received something...\n");
+
+    if (msg.msgid != MAVLINK_MSG_ID_ACK_TM)
+    {
+        // Prepare ack messages
+        mavlink_message_t ackMsg;
+        mavlink_msg_ack_tm_pack(1, 1, &ackMsg, msg.msgid, msg.seq);
+
+        // Send the ack back to the sender
+        channel->enqueueMsg(ackMsg);
+    }
+    else
+    {
+        printf("Received ACK!\n");
+    }
+}
+
+void flightTmLoop()
+{
+    while (1)
+    {
+        long long start = miosix::getTick();
+
+        mavlink_message_t msg;
+        mavlink_rocket_flight_tm_t tm = {0};
+        mavlink_msg_rocket_flight_tm_encode(171, 96, &msg, &tm);
+
+        channel->enqueueMsg(msg);
+        printf("Enqueued flight_tm_tm!\n");
+
+        Thread::sleepUntil(start + FLIGHT_TM_PERIOD);
+    }
+}
+
+void statsTmLoop()
+{
+    while (1)
+    {
+        long long start = miosix::getTick();
+
+        mavlink_message_t msg;
+        mavlink_rocket_stats_tm_t tm = {0};
+        mavlink_msg_rocket_stats_tm_encode(171, 96, &msg, &tm);
+
+        channel->enqueueMsg(msg);
+        printf("Enqueued stats_tm!\n");
+
+        Thread::sleepUntil(start + STATS_TM_PERIOD);
+    }
+}
+
+int main()
+{
+    initBoard();
+
+    SX1278Lora::Config config = {};
+    config.pa_boost           = true;
+    config.power              = 15;
+    config.ocp                = 0;
+    config.coding_rate        = SX1278Lora::Config::Cr::CR_1;
+    config.spreading_factor   = SX1278Lora::Config::Sf::SF_7;
+    config.bandwidth          = SX1278Lora::Config::Bw::HZ_250000;
+
+    printf("Effective bitrate: %lukb/s\n", config.effectiveBitrate());
+
+    SX1278Lora::Error err;
+
+    SPIBus bus(SX1278_SPI);
+    GpioPin cs = cs::getPin();
+
+    SPIBusConfig spi_config = {};
+    spi_config.clockDivider = SPI::ClockDivider::DIV_64;
+    spi_config.mode         = SPI::Mode::MODE_0;
+    spi_config.bitOrder     = SPI::BitOrder::MSB_FIRST;
+
+#ifdef IS_EBYTE
+    sx1278 = new EbyteLora(SPISlave(bus, cs, spi_config), txen::getPin(),
+                           rxen::getPin());
+#else
+    sx1278 = new SX1278Lora(SPISlave(bus, cs, spi_config));
+#endif
+
+    printf("\n[sx1278] Configuring sx1278...\n");
+    if ((err = sx1278->init(config)) != SX1278Lora::Error::NONE)
+    {
+        printf("[sx1278] sx1278->init error: %s\n", "TODO");
+        return -1;
+    }
+
+    printf("\n[sx1278] Initialization complete!\n");
+
+    channel =
+        new Mav(sx1278, &onReceive, SLEEP_AFTER_SEND, MAV_OUT_BUFFER_MAX_AGE);
+    channel->start();
+
+    std::thread flight_tm_loop([]() { flightTmLoop(); });
+    std::thread stats_tm_loop([]() { statsTmLoop(); });
+
+    while (1)
+        Thread::wait();
+
+    return 0;
+}
diff --git a/src/tests/radio/sx1278/lora/test-sx1278-simple.cpp b/src/tests/radio/sx1278/lora/test-sx1278-simple.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..4653b87f0b0a9f2b11e31fd565e5876280186f63
--- /dev/null
+++ b/src/tests/radio/sx1278/lora/test-sx1278-simple.cpp
@@ -0,0 +1,171 @@
+/* Copyright (c) 2022 Skyward Experimental Rocketry
+ * Author: Davide Mor
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#include <drivers/interrupt/external_interrupts.h>
+#include <radio/SX1278/Ebyte.h>
+#include <radio/SX1278/SX1278Lora.h>
+
+using namespace Boardcore;
+using namespace miosix;
+
+#if defined _BOARD_STM32F429ZI_SKYWARD_GS_V2
+#include "interfaces-impl/hwmapping.h"
+
+// Uncomment the following line to enable Ebyte module
+// #define IS_EBYTE
+
+using cs   = peripherals::ra01::pc13::cs;
+using dio0 = peripherals::ra01::pc13::dio0;
+using dio1 = peripherals::ra01::pc13::dio1;
+using dio3 = peripherals::ra01::pc13::dio3;
+
+using sck  = interfaces::spi4::sck;
+using miso = interfaces::spi4::miso;
+using mosi = interfaces::spi4::mosi;
+
+#ifdef IS_EBYTE
+using txen = Gpio<GPIOE_BASE, 4>;
+using rxen = Gpio<GPIOD_BASE, 4>;
+#endif
+
+#define SX1278_SPI SPI4
+
+#define SX1278_IRQ_DIO0 EXTI6_IRQHandlerImpl
+#define SX1278_IRQ_DIO1 EXTI2_IRQHandlerImpl
+#define SX1278_IRQ_DIO3 EXTI11_IRQHandlerImpl
+
+#else
+#error "Target not supported"
+#endif
+
+SX1278Lora *sx1278 = nullptr;
+
+void __attribute__((used)) SX1278_IRQ_DIO0()
+{
+    if (sx1278)
+        sx1278->handleDioIRQ(SX1278Lora::Dio::DIO0);
+}
+
+void __attribute__((used)) SX1278_IRQ_DIO1()
+{
+    if (sx1278)
+        sx1278->handleDioIRQ(SX1278Lora::Dio::DIO1);
+}
+
+void __attribute__((used)) SX1278_IRQ_DIO3()
+{
+    if (sx1278)
+        sx1278->handleDioIRQ(SX1278Lora::Dio::DIO3);
+}
+
+void initBoard()
+{
+#ifdef IS_EBYTE
+    rxen::mode(Mode::OUTPUT);
+    txen::mode(Mode::OUTPUT);
+    rxen::low();
+    txen::low();
+#endif
+
+    GpioPin dio0_pin = dio0::getPin();
+    GpioPin dio1_pin = dio1::getPin();
+    GpioPin dio3_pin = dio3::getPin();
+
+    enableExternalInterrupt(dio0_pin.getPort(), dio0_pin.getNumber(),
+                            InterruptTrigger::RISING_EDGE);
+    enableExternalInterrupt(dio1_pin.getPort(), dio1_pin.getNumber(),
+                            InterruptTrigger::RISING_EDGE);
+    enableExternalInterrupt(dio3_pin.getPort(), dio3_pin.getNumber(),
+                            InterruptTrigger::RISING_EDGE);
+}
+
+int main()
+{
+
+    initBoard();
+
+    SX1278Lora::Config config = {};
+    config.pa_boost           = true;
+    config.power              = 10;
+    config.ocp                = 0;
+    config.coding_rate        = SX1278Lora::Config::Cr::CR_1;
+    config.spreading_factor   = SX1278Lora::Config::Sf::SF_7;
+
+    SX1278Lora::Error err;
+
+    SPIBus bus(SX1278_SPI);
+    GpioPin cs = cs::getPin();
+
+    SPIBusConfig spi_config = {};
+    spi_config.clockDivider = SPI::ClockDivider::DIV_64;
+    spi_config.mode         = SPI::Mode::MODE_0;
+    spi_config.bitOrder     = SPI::BitOrder::MSB_FIRST;
+
+#ifdef IS_EBYTE
+    sx1278 = new EbyteLora(SPISlave(bus, cs, spi_config), txen::getPin(),
+                           rxen::getPin());
+#else
+    sx1278 = new SX1278Lora(SPISlave(bus, cs, spi_config));
+#endif
+
+    printf("\n[sx1278] Configuring sx1278...\n");
+    if ((err = sx1278->init(config)) != SX1278Lora::Error::NONE)
+    {
+        printf("[sx1278] sx1278->init error: %s\n", "TODO");
+        return -1;
+    }
+
+    printf("\n[sx1278] Initialization complete!\n");
+
+#ifdef ENABLE_TX
+    int i = 0;
+
+    while (1)
+    {
+        uint8_t msg[24] = {(uint8_t)i, 0xfe, 0xba, 0xbe};
+        sx1278->send(msg, 24);
+
+        for (int j = 0; j < 4; j++)
+            printf("%x ", msg[j]);
+        printf("\n");
+
+        printf("[sx1278] sx1278->send successful!\n");
+        miosix::Thread::sleep(1000);
+
+        i += 1;
+    }
+#else
+    while (1)
+    {
+        uint8_t msg[24] = {0};
+        sx1278->receive(msg, 24);
+
+        for (int j = 0; j < 4; j++)
+            printf("%x ", msg[j]);
+
+        printf("- RSSI: %f, SNR: %f\n", sx1278->getLastRxRssi(),
+               sx1278->getLastRxSnr());
+    }
+#endif
+
+    return 0;
+}