Merge branch 'master' of https://github.com/ChibiOS/ChibiOS-Contrib

8 files changed, 1635 insertions, 60 deletions

diff --git a/os/various/devices_lib/rf/nrf52_radio.c b/os/various/devices_lib/rf/nrf52_radio.c
new file mode 100644
index 0000000..e55870f
--- /dev/null
+++ b/os/various/devices_lib/rf/nrf52_radio.c
@@ -0,0 +1,1111 @@
+/* Copyright (c) 2014 Nordic Semiconductor. All Rights Reserved.
+ *
+ * The information contained herein is property of Nordic Semiconductor ASA.
+ * Terms and conditions of usage are described in detail in NORDIC
+ * SEMICONDUCTOR STANDARD SOFTWARE LICENSE AGREEMENT.
+ *
+ * Licensees are granted free, non-transferable use of the information. NO
+ * WARRANTY of ANY KIND is provided. This heading must NOT be removed from
+ * the file.
+ *
+ * Enhanced ShockBurst proprietary protocol to ChibiOS port
+ *
+ *  ported on: 25/10/2018, by andru
+ *
+ */
+
+#include <stdint.h>
+#include <string.h>
+
+#include "ch.h"
+#include "hal.h"
+
+#include "nrf52_radio.h"
+
+
+#define BIT_MASK_UINT_8(x) 					  (0xFF >> (8 - (x)))
+#define NRF52_PIPE_COUNT 					  9
+
+#define RADIO_SHORTS_COMMON ( RADIO_SHORTS_READY_START_Msk | RADIO_SHORTS_END_DISABLE_Msk | \
+            RADIO_SHORTS_ADDRESS_RSSISTART_Msk | RADIO_SHORTS_DISABLED_RSSISTOP_Msk )
+
+// Constant parameters
+#define RX_WAIT_FOR_ACK_TIMEOUT_US_2MBPS      (48)        /**< 2MBit RX wait for ack timeout value. Smallest reliable value - 43 */
+#define RX_WAIT_FOR_ACK_TIMEOUT_US_1MBPS      (64)        /**< 1MBit RX wait for ack timeout value. Smallest reliable value - 59 */
+
+#define NRF52_ADDR_UPDATE_MASK_BASE0          (1 << 0)    /*< Mask value to signal updating BASE0 radio address. */
+#define NRF52_ADDR_UPDATE_MASK_BASE1          (1 << 1)    /*< Mask value to signal updating BASE1 radio address. */
+#define NRF52_ADDR_UPDATE_MASK_PREFIX         (1 << 2)    /*< Mask value to signal updating radio prefixes */
+
+#define NRF52_PID_RESET_VALUE                 0xFF        /**< Invalid PID value which is guaranteed to not collide with any valid PID value. */
+#define NRF52_PID_MAX                         3           /**< Maximum value for PID. */
+#define NRF52_CRC_RESET_VALUE                 0xFFFF      /**< CRC reset value*/
+
+#ifndef NRF52_RADIO_USE_TIMER0
+#define NRF52_RADIO_USE_TIMER0                FALSE
+#endif
+
+#ifndef NRF52_RADIO_USE_TIMER1
+#define NRF52_RADIO_USE_TIMER1                FALSE
+#endif
+
+#ifndef NRF52_RADIO_USE_TIMER2
+#define NRF52_RADIO_USE_TIMER2                FALSE
+#endif
+
+#ifndef NRF52_RADIO_USE_TIMER3
+#define NRF52_RADIO_USE_TIMER3                FALSE
+#endif
+
+#ifndef NRF52_RADIO_USE_TIMER4
+#define NRF52_RADIO_USE_TIMER4                FALSE
+#endif
+
+#ifndef NRF52_RADIO_IRQ_PRIORITY
+#define NRF52_RADIO_IRQ_PRIORITY			  3                   /**< RADIO interrupt priority. */
+#endif
+
+#ifndef NRF52_RADIO_PPI_TIMER_START
+#error "PPI channel NRF52_RADIO_PPI_TIMER_START need to be defined"
+#endif
+
+#ifndef NRF52_RADIO_PPI_TIMER_STOP
+#error "PPI channel NRF52_RADIO_PPI_TIMER_STOP need to be defined"
+#endif
+
+#ifndef NRF52_RADIO_PPI_RX_TIMEOUT
+#error "PPI channel NRF52_RADIO_PPI_RX_TIMEOUT need to be defined"
+#endif
+
+#ifndef NRF52_RADIO_PPI_TX_START
+#error "PPI channel NRF52_RADIO_PPI_TX_START need to be defined"
+#endif
+
+#if (NRF52_RADIO_USE_TIMER0 == FALSE) && (NRF52_RADIO_USE_TIMER1 == FALSE) && \
+	(NRF52_RADIO_USE_TIMER2 == FALSE) && (NRF52_RADIO_USE_TIMER3 == FALSE) && \
+	(NRF52_RADIO_USE_TIMER4 == FALSE)
+#error "At least one hardware TIMER must be defined"
+#endif
+
+#ifndef NRF52_RADIO_INTTHD_PRIORITY
+#error "Interrupt handle thread priority need to be defined"
+#endif
+
+#ifndef NRF52_RADIO_EVTTHD_PRIORITY
+#error "Event thread priority need to be defined"
+#endif
+
+#define VERIFY_PAYLOAD_LENGTH(p)                            \
+do                                                          \
+{                                                           \
+    if(p->length == 0 ||                                    \
+       p->length > NRF52_MAX_PAYLOAD_LENGTH ||              \
+       (RFD1.config.protocol == NRF52_PROTOCOL_ESB &&       \
+        p->length > RFD1.config.payload_length))            \
+    {                                                       \
+        return NRF52_ERROR_INVALID_LENGTH;                  \
+    }                                                       \
+}while(0)
+
+//Structure holding pipe info PID and CRC and ack payload.
+typedef struct
+{
+    uint16_t            m_crc;
+    uint8_t             m_pid;
+    uint8_t             m_ack_payload;
+} pipe_info_t;
+
+// First in first out queue of payloads to be transmitted.
+typedef struct
+{
+    nrf52_payload_t *   p_payload[NRF52_TX_FIFO_SIZE];      /**< Pointer to the actual queue. */
+    uint32_t            entry_point;                        /**< Current start of queue. */
+    uint32_t            exit_point;                         /**< Current end of queue. */
+    uint32_t            count;                              /**< Current number of elements in the queue. */
+} nrf52_payload_tx_fifo_t;
+
+// First in first out queue of received payloads.
+typedef struct
+{
+    nrf52_payload_t *   p_payload[NRF52_RX_FIFO_SIZE];      /**< Pointer to the actual queue. */
+    uint32_t            entry_point;                        /**< Current start of queue. */
+    uint32_t            exit_point;                         /**< Current end of queue. */
+    uint32_t            count;                              /**< Current number of elements in the queue. */
+} nrf52_payload_rx_fifo_t;
+
+// These function pointers are changed dynamically, depending on protocol configuration and state.
+//static void (*on_radio_end)(RFDriver *rfp) = NULL;
+static void (*set_rf_payload_format)(RFDriver *rfp, uint32_t payload_length) = NULL;
+
+// The following functions are assigned to the function pointers above.
+static void on_radio_disabled_tx_noack(RFDriver *rfp);
+static void on_radio_disabled_tx(RFDriver *rfp);
+static void on_radio_disabled_tx_wait_for_ack(RFDriver *rfp);
+static void on_radio_disabled_rx(RFDriver *rfp);
+static void on_radio_disabled_rx_ack(RFDriver *rfp);
+
+static volatile uint16_t wait_for_ack_timeout_us;
+static nrf52_payload_t * p_current_payload;
+
+// TX FIFO
+static nrf52_payload_t            tx_fifo_payload[NRF52_TX_FIFO_SIZE];
+static nrf52_payload_tx_fifo_t    tx_fifo;
+
+// RX FIFO
+static nrf52_payload_t            rx_fifo_payload[NRF52_RX_FIFO_SIZE];
+static nrf52_payload_rx_fifo_t    rx_fifo;
+
+// Payload buffers
+static uint8_t                    tx_payload_buffer[NRF52_MAX_PAYLOAD_LENGTH + 2];
+static uint8_t                    rx_payload_buffer[NRF52_MAX_PAYLOAD_LENGTH + 2];
+
+static uint8_t                    pids[NRF52_PIPE_COUNT];
+static pipe_info_t                rx_pipe_info[NRF52_PIPE_COUNT];
+
+ // disable and events semaphores.
+static binary_semaphore_t disable_sem;
+static binary_semaphore_t events_sem;
+
+RFDriver RFD1;
+
+// Function to do bytewise bit-swap on a unsigned 32 bit value
+static uint32_t bytewise_bit_swap(uint8_t const * p_inp) {
+    uint32_t inp = (*(uint32_t*)p_inp);
+
+    return __REV((uint32_t)__RBIT(inp)); //lint -esym(628, __rev) -esym(526, __rev) -esym(628, __rbit) -esym(526, __rbit) */
+}
+
+// Internal function to convert base addresses from nRF24L type addressing to nRF52 type addressing
+static uint32_t addr_conv(uint8_t const* p_addr) {
+    return __REV(bytewise_bit_swap(p_addr)); //lint -esym(628, __rev) -esym(526, __rev) */
+}
+
+static thread_t *rfEvtThread_p;
+static THD_WORKING_AREA(waRFEvtThread, 64);
+static THD_FUNCTION(rfEvtThread, arg) {
+    (void)arg;
+
+    chRegSetThreadName("rfevent");
+
+    while (!chThdShouldTerminateX()) {
+    	chBSemWait(&events_sem);
+
+    	nrf52_int_flags_t interrupts = RFD1.flags;
+        RFD1.flags = 0;
+
+        if (interrupts & NRF52_INT_TX_SUCCESS_MSK) {
+            chEvtBroadcastFlags(&RFD1.eventsrc, (eventflags_t) NRF52_EVENT_TX_SUCCESS);
+        }
+        if (interrupts & NRF52_INT_TX_FAILED_MSK) {
+        	chEvtBroadcastFlags(&RFD1.eventsrc, (eventflags_t) NRF52_EVENT_TX_FAILED);
+        }
+        if (interrupts & NRF52_INT_RX_DR_MSK) {
+        	chEvtBroadcastFlags(&RFD1.eventsrc, (eventflags_t) NRF52_EVENT_RX_RECEIVED);
+        }
+    }
+    chThdExit((msg_t) 0);
+}
+
+static thread_t *rfIntThread_p;
+static THD_WORKING_AREA(waRFIntThread, 64);
+static THD_FUNCTION(rfIntThread, arg) {
+    (void)arg;
+
+    chRegSetThreadName("rfint");
+
+    while (!chThdShouldTerminateX()) {
+    	chBSemWait(&disable_sem);
+    	switch (RFD1.state) {
+    	  case NRF52_STATE_PTX_TX:
+    		  on_radio_disabled_tx_noack(&RFD1);
+    		  break;
+    	  case NRF52_STATE_PTX_TX_ACK:
+    		  on_radio_disabled_tx(&RFD1);
+    		  break;
+    	  case NRF52_STATE_PTX_RX_ACK:
+    		  on_radio_disabled_tx_wait_for_ack(&RFD1);
+    		  break;
+    	  case NRF52_STATE_PRX:
+    		  on_radio_disabled_rx(&RFD1);
+    		  break;
+    	  case NRF52_STATE_PRX_SEND_ACK:
+    		  on_radio_disabled_rx_ack(&RFD1);
+    		  break;
+    	  default:
+    		  break;
+    	}
+    }
+	chThdExit((msg_t) 0);
+}
+
+static void serve_radio_interrupt(RFDriver *rfp) {
+	(void) rfp;
+    if ((NRF_RADIO->INTENSET & RADIO_INTENSET_READY_Msk) && NRF_RADIO->EVENTS_READY) {
+        NRF_RADIO->EVENTS_READY = 0;
+        (void) NRF_RADIO->EVENTS_READY;
+    }
+    if ((NRF_RADIO->INTENSET & RADIO_INTENSET_DISABLED_Msk) && NRF_RADIO->EVENTS_DISABLED) {
+        NRF_RADIO->EVENTS_DISABLED = 0;
+        (void) NRF_RADIO->EVENTS_DISABLED;
+        chSysLockFromISR();
+       	chBSemSignalI(&disable_sem);
+       	chSysUnlockFromISR();
+    }
+}
+
+/**
+ * @brief   RADIO events interrupt handler.
+ *
+ * @isr
+ */
+OSAL_IRQ_HANDLER(Vector44) {
+
+  OSAL_IRQ_PROLOGUE();
+
+  serve_radio_interrupt(&RFD1);
+
+  OSAL_IRQ_EPILOGUE();
+}
+
+static void set_rf_payload_format_esb_dpl(RFDriver *rfp, uint32_t payload_length) {
+	(void)payload_length;
+#if (NRF52_MAX_PAYLOAD_LENGTH <= 32)
+    // Using 6 bits for length
+    NRF_RADIO->PCNF0 = (0 << RADIO_PCNF0_S0LEN_Pos) |
+                       (6 << RADIO_PCNF0_LFLEN_Pos) |
+                       (3 << RADIO_PCNF0_S1LEN_Pos) ;
+#else
+    // Using 8 bits for length
+    NRF_RADIO->PCNF0 = (0 << RADIO_PCNF0_S0LEN_Pos) |
+                       (8 << RADIO_PCNF0_LFLEN_Pos) |
+                       (3 << RADIO_PCNF0_S1LEN_Pos) ;
+#endif
+    NRF_RADIO->PCNF1 = (RADIO_PCNF1_WHITEEN_Disabled    << RADIO_PCNF1_WHITEEN_Pos) |
+                       (RADIO_PCNF1_ENDIAN_Big          << RADIO_PCNF1_ENDIAN_Pos)  |
+                       ((rfp->config.address.addr_length - 1)  << RADIO_PCNF1_BALEN_Pos)   |
+                       (0                               << RADIO_PCNF1_STATLEN_Pos) |
+                       (NRF52_MAX_PAYLOAD_LENGTH        << RADIO_PCNF1_MAXLEN_Pos);
+}
+
+static void set_rf_payload_format_esb(RFDriver *rfp, uint32_t payload_length) {
+    NRF_RADIO->PCNF0 = (1 << RADIO_PCNF0_S0LEN_Pos) |
+                       (0 << RADIO_PCNF0_LFLEN_Pos) |
+                       (1 << RADIO_PCNF0_S1LEN_Pos);
+
+    NRF_RADIO->PCNF1 = (RADIO_PCNF1_WHITEEN_Disabled    << RADIO_PCNF1_WHITEEN_Pos) |
+                       (RADIO_PCNF1_ENDIAN_Big          << RADIO_PCNF1_ENDIAN_Pos)  |
+                       ((rfp->config.address.addr_length - 1)  << RADIO_PCNF1_BALEN_Pos)   |
+                       (payload_length                  << RADIO_PCNF1_STATLEN_Pos) |
+                       (payload_length                  << RADIO_PCNF1_MAXLEN_Pos);
+}
+
+/* Set BASE0 and BASE1 addresses & prefixes registers
+ *   NRF52 { prefixes[0], base0_addr[0], base0_addr[1], base0_addr[2], base0_addr[3] } ==
+ *   NRF24 { addr[0], addr[1], addr[2], addr[3], addr[4] }
+ */
+static void set_addresses(RFDriver *rfp, uint8_t update_mask) {
+    if (update_mask & NRF52_ADDR_UPDATE_MASK_BASE0) {
+        NRF_RADIO->BASE0 = addr_conv(rfp->config.address.base_addr_p0);
+        NRF_RADIO->DAB[0] = addr_conv(rfp->config.address.base_addr_p0);
+    }
+
+    if (update_mask & NRF52_ADDR_UPDATE_MASK_BASE1) {
+        NRF_RADIO->BASE1 = addr_conv(rfp->config.address.base_addr_p1);
+        NRF_RADIO->DAB[1] = addr_conv(rfp->config.address.base_addr_p1);
+    }
+
+    if (update_mask & NRF52_ADDR_UPDATE_MASK_PREFIX) {
+        NRF_RADIO->PREFIX0 = bytewise_bit_swap(&rfp->config.address.pipe_prefixes[0]);
+        NRF_RADIO->DAP[0] = bytewise_bit_swap(&rfp->config.address.pipe_prefixes[0]);
+        NRF_RADIO->PREFIX1 = bytewise_bit_swap(&rfp->config.address.pipe_prefixes[4]);
+        NRF_RADIO->DAP[1] = bytewise_bit_swap(&rfp->config.address.pipe_prefixes[4]);
+    }
+}
+
+static void set_tx_power(RFDriver *rfp) {
+    NRF_RADIO->TXPOWER = rfp->config.tx_power << RADIO_TXPOWER_TXPOWER_Pos;
+}
+
+static void set_bitrate(RFDriver *rfp) {
+    NRF_RADIO->MODE = rfp->config.bitrate << RADIO_MODE_MODE_Pos;
+
+    switch (rfp->config.bitrate) {
+        case NRF52_BITRATE_2MBPS:
+            wait_for_ack_timeout_us = RX_WAIT_FOR_ACK_TIMEOUT_US_2MBPS;
+            break;
+        case NRF52_BITRATE_1MBPS:
+            wait_for_ack_timeout_us = RX_WAIT_FOR_ACK_TIMEOUT_US_1MBPS;
+            break;
+    }
+}
+
+static void set_protocol(RFDriver *rfp) {
+    switch (rfp->config.protocol) {
+        case NRF52_PROTOCOL_ESB_DPL:
+            set_rf_payload_format = set_rf_payload_format_esb_dpl;
+            break;
+        case NRF52_PROTOCOL_ESB:
+            set_rf_payload_format = set_rf_payload_format_esb;
+            break;
+    }
+}
+
+static void set_crc(RFDriver *rfp) {
+    NRF_RADIO->CRCCNF = rfp->config.crc << RADIO_CRCCNF_LEN_Pos;
+
+    if (rfp->config.crc == RADIO_CRCCNF_LEN_Two)
+    {
+        NRF_RADIO->CRCINIT = 0xFFFFUL;      // Initial value
+        NRF_RADIO->CRCPOLY = 0x11021UL;     // CRC poly: x^16+x^12^x^5+1
+    }
+    else if (rfp->config.crc == RADIO_CRCCNF_LEN_One)
+    {
+        NRF_RADIO->CRCINIT = 0xFFUL;        // Initial value
+        NRF_RADIO->CRCPOLY = 0x107UL;       // CRC poly: x^8+x^2^x^1+1
+    }
+}
+
+static void ppi_init(RFDriver *rfp) {
+    NRF_PPI->CH[NRF52_RADIO_PPI_TIMER_START].EEP = (uint32_t)&NRF_RADIO->EVENTS_READY;
+    NRF_PPI->CH[NRF52_RADIO_PPI_TIMER_START].TEP = (uint32_t)&rfp->timer->TASKS_START;
+
+    NRF_PPI->CH[NRF52_RADIO_PPI_TIMER_STOP].EEP  = (uint32_t)&NRF_RADIO->EVENTS_ADDRESS;
+    NRF_PPI->CH[NRF52_RADIO_PPI_TIMER_STOP].TEP  = (uint32_t)&rfp->timer->TASKS_STOP;
+
+    NRF_PPI->CH[NRF52_RADIO_PPI_RX_TIMEOUT].EEP  = (uint32_t)&rfp->timer->EVENTS_COMPARE[0];
+    NRF_PPI->CH[NRF52_RADIO_PPI_RX_TIMEOUT].TEP  = (uint32_t)&NRF_RADIO->TASKS_DISABLE;
+
+    NRF_PPI->CH[NRF52_RADIO_PPI_TX_START].EEP    = (uint32_t)&rfp->timer->EVENTS_COMPARE[1];
+    NRF_PPI->CH[NRF52_RADIO_PPI_TX_START].TEP    = (uint32_t)&NRF_RADIO->TASKS_TXEN;
+}
+
+static void set_parameters(RFDriver *rfp) {
+    set_tx_power(rfp);
+    set_bitrate(rfp);
+    set_protocol(rfp);
+    set_crc(rfp);
+    set_rf_payload_format(rfp, rfp->config.payload_length);
+}
+
+static void reset_fifo(void) {
+    tx_fifo.entry_point = 0;
+    tx_fifo.exit_point  = 0;
+    tx_fifo.count       = 0;
+
+    rx_fifo.entry_point = 0;
+    rx_fifo.exit_point  = 0;
+    rx_fifo.count       = 0;
+}
+
+static void init_fifo(void) {
+	uint8_t i;
+    reset_fifo();
+
+    for (i = 0; i < NRF52_TX_FIFO_SIZE; i++) {
+        tx_fifo.p_payload[i] = &tx_fifo_payload[i];
+    }
+
+    for (i = 0; i < NRF52_RX_FIFO_SIZE; i++) {
+        rx_fifo.p_payload[i] = &rx_fifo_payload[i];
+    }
+}
+
+static void tx_fifo_remove_last(void) {
+    if (tx_fifo.count > 0) {
+        nvicDisableVector(RADIO_IRQn);
+
+        tx_fifo.count--;
+        if (++tx_fifo.exit_point >= NRF52_TX_FIFO_SIZE) {
+            tx_fifo.exit_point = 0;
+        }
+
+        nvicEnableVector(RADIO_IRQn, NRF52_RADIO_IRQ_PRIORITY);
+    }
+}
+
+/** @brief  Function to push the content of the rx_buffer to the RX FIFO.
+ *
+ *  The module will point the register NRF_RADIO->PACKETPTR to a buffer for receiving packets.
+ *  After receiving a packet the module will call this function to copy the received data to
+ *  the RX FIFO.
+ *
+ *  @param  pipe Pipe number to set for the packet.
+ *  @param  pid  Packet ID.
+ *
+ *  @retval true   Operation successful.
+ *  @retval false  Operation failed.
+ */
+static bool rx_fifo_push_rfbuf(RFDriver *rfp, uint8_t pipe, uint8_t pid) {
+    if (rx_fifo.count < NRF52_RX_FIFO_SIZE) {
+        if (rfp->config.protocol == NRF52_PROTOCOL_ESB_DPL) {
+            if (rx_payload_buffer[0] > NRF52_MAX_PAYLOAD_LENGTH) {
+                return false;
+            }
+
+            rx_fifo.p_payload[rx_fifo.entry_point]->length = rx_payload_buffer[0];
+        }
+        else if (rfp->state == NRF52_STATE_PTX_RX_ACK) {
+            // Received packet is an acknowledgment
+            rx_fifo.p_payload[rx_fifo.entry_point]->length = 0;
+        }
+        else {
+            rx_fifo.p_payload[rx_fifo.entry_point]->length = rfp->config.payload_length;
+        }
+
+        memcpy(rx_fifo.p_payload[rx_fifo.entry_point]->data, &rx_payload_buffer[2],
+               rx_fifo.p_payload[rx_fifo.entry_point]->length);
+
+        rx_fifo.p_payload[rx_fifo.entry_point]->pipe = pipe;
+        rx_fifo.p_payload[rx_fifo.entry_point]->rssi = NRF_RADIO->RSSISAMPLE;
+        rx_fifo.p_payload[rx_fifo.entry_point]->pid = pid;
+        if (++rx_fifo.entry_point >= NRF52_RX_FIFO_SIZE) {
+            rx_fifo.entry_point = 0;
+        }
+        rx_fifo.count++;
+
+        return true;
+    }
+
+    return false;
+}
+
+static void timer_init(RFDriver *rfp) {
+    // Configure the system timer with a 1 MHz base frequency
+    rfp->timer->PRESCALER = 4;
+    rfp->timer->BITMODE   = TIMER_BITMODE_BITMODE_16Bit;
+    rfp->timer->SHORTS    = TIMER_SHORTS_COMPARE1_CLEAR_Msk | TIMER_SHORTS_COMPARE1_STOP_Msk;
+}
+
+static void start_tx_transaction(RFDriver *rfp) {
+    bool ack;
+
+    rfp->tx_attempt = 1;
+    rfp->tx_remaining = rfp->config.retransmit.count;
+
+    // Prepare the payload
+    p_current_payload = tx_fifo.p_payload[tx_fifo.exit_point];
+
+    // Handling ack if noack is set to false or if selctive auto ack is turned turned off
+    ack = !p_current_payload->noack || !rfp->config.selective_auto_ack;
+
+    switch (rfp->config.protocol) {
+        case NRF52_PROTOCOL_ESB:
+            set_rf_payload_format(rfp, p_current_payload->length);
+            tx_payload_buffer[0] = p_current_payload->pid;
+            tx_payload_buffer[1] = 0;
+            memcpy(&tx_payload_buffer[2], p_current_payload->data, p_current_payload->length);
+
+            NRF_RADIO->SHORTS   = RADIO_SHORTS_COMMON | RADIO_SHORTS_DISABLED_RXEN_Msk;
+            NRF_RADIO->INTENSET = RADIO_INTENSET_DISABLED_Msk | RADIO_INTENSET_READY_Msk;
+
+            // Configure the retransmit counter
+            rfp->tx_remaining = rfp->config.retransmit.count;
+            rfp->state = NRF52_STATE_PTX_TX_ACK;
+            break;
+
+        case NRF52_PROTOCOL_ESB_DPL:
+            tx_payload_buffer[0] = p_current_payload->length;
+            tx_payload_buffer[1] = p_current_payload->pid << 1;
+            tx_payload_buffer[1] |= ack ? 0x00 : 0x01;
+            memcpy(&tx_payload_buffer[2], p_current_payload->data, p_current_payload->length);
+
+            if (ack) {
+                NRF_RADIO->SHORTS   = RADIO_SHORTS_COMMON | RADIO_SHORTS_DISABLED_RXEN_Msk;
+                NRF_RADIO->INTENSET = RADIO_INTENSET_DISABLED_Msk | RADIO_INTENSET_READY_Msk;
+
+                // Configure the retransmit counter
+                rfp->tx_remaining = rfp->config.retransmit.count;
+                rfp->state = NRF52_STATE_PTX_TX_ACK;
+            }
+            else {
+                NRF_RADIO->SHORTS   = RADIO_SHORTS_COMMON;
+                NRF_RADIO->INTENSET = RADIO_INTENSET_DISABLED_Msk;
+                rfp->state = NRF52_STATE_PTX_TX;
+            }
+            break;
+    }
+
+    NRF_RADIO->TXADDRESS    = p_current_payload->pipe;
+    NRF_RADIO->RXADDRESSES  = 1 << p_current_payload->pipe;
+
+    NRF_RADIO->FREQUENCY    = rfp->config.address.rf_channel;
+    NRF_RADIO->PACKETPTR    = (uint32_t)tx_payload_buffer;
+
+    NRF_RADIO->EVENTS_READY = 0;
+    NRF_RADIO->EVENTS_DISABLED = 0;
+    (void)NRF_RADIO->EVENTS_READY;
+    (void)NRF_RADIO->EVENTS_DISABLED;
+
+    nvicClearPending(RADIO_IRQn);
+    nvicEnableVector(RADIO_IRQn, NRF52_RADIO_IRQ_PRIORITY);
+
+    NRF_RADIO->TASKS_TXEN  = 1;
+}
+
+static void on_radio_disabled_tx_noack(RFDriver *rfp) {
+    rfp->flags |= NRF52_INT_TX_SUCCESS_MSK;
+    tx_fifo_remove_last();
+
+	chBSemSignal(&events_sem);
+
+	if (tx_fifo.count == 0) {
+        rfp->state = NRF52_STATE_IDLE;
+    }
+    else {
+        start_tx_transaction(rfp);
+    }
+}
+
+static void on_radio_disabled_tx(RFDriver *rfp) {
+    // Remove the DISABLED -> RXEN shortcut, to make sure the radio stays
+    // disabled after the RX window
+    NRF_RADIO->SHORTS = RADIO_SHORTS_COMMON;
+
+    // Make sure the timer is started the next time the radio is ready,
+    // and that it will disable the radio automatically if no packet is
+    // received by the time defined in m_wait_for_ack_timeout_us
+    rfp->timer->CC[0]    = wait_for_ack_timeout_us + 130;
+    rfp->timer->CC[1]    = rfp->config.retransmit.delay - 130;
+    rfp->timer->TASKS_CLEAR = 1;
+    rfp->timer->EVENTS_COMPARE[0] = 0;
+    rfp->timer->EVENTS_COMPARE[1] = 0;
+    (void)rfp->timer->EVENTS_COMPARE[0];
+    (void)rfp->timer->EVENTS_COMPARE[1];
+
+    NRF_PPI->CHENSET = (1 << NRF52_RADIO_PPI_TIMER_START) |
+                       (1 << NRF52_RADIO_PPI_RX_TIMEOUT) |
+                       (1 << NRF52_RADIO_PPI_TIMER_STOP);
+    NRF_PPI->CHENCLR = (1 << NRF52_RADIO_PPI_TX_START);
+
+    NRF_RADIO->EVENTS_END = 0;
+    (void)NRF_RADIO->EVENTS_END;
+
+    if (rfp->config.protocol == NRF52_PROTOCOL_ESB) {
+        set_rf_payload_format(rfp, 0);
+    }
+
+    NRF_RADIO->PACKETPTR = (uint32_t)rx_payload_buffer;
+    rfp->state = NRF52_STATE_PTX_RX_ACK;
+}
+
+static void on_radio_disabled_tx_wait_for_ack(RFDriver *rfp) {
+    // This marks the completion of a TX_RX sequence (TX with ACK)
+
+    // Make sure the timer will not deactivate the radio if a packet is received
+    NRF_PPI->CHENCLR = (1 << NRF52_RADIO_PPI_TIMER_START) |
+                       (1 << NRF52_RADIO_PPI_RX_TIMEOUT)  |
+                       (1 << NRF52_RADIO_PPI_TIMER_STOP);
+
+    // If the radio has received a packet and the CRC status is OK
+    if (NRF_RADIO->EVENTS_END && NRF_RADIO->CRCSTATUS != 0) {
+        rfp->timer->TASKS_STOP = 1;
+        NRF_PPI->CHENCLR = (1 << NRF52_RADIO_PPI_TX_START);
+        rfp->flags |= NRF52_INT_TX_SUCCESS_MSK;
+        rfp->tx_attempt++;// = rfp->config.retransmit.count - rfp->tx_remaining + 1;
+
+        tx_fifo_remove_last();
+
+        if (rfp->config.protocol != NRF52_PROTOCOL_ESB && rx_payload_buffer[0] > 0) {
+            if (rx_fifo_push_rfbuf(rfp, (uint8_t)NRF_RADIO->TXADDRESS, 0)) {
+                rfp->flags |= NRF52_INT_RX_DR_MSK;
+            }
+        }
+
+    	chBSemSignal(&events_sem);
+
+        if ((tx_fifo.count == 0) || (rfp->config.tx_mode == NRF52_TXMODE_MANUAL)) {
+            rfp->state = NRF52_STATE_IDLE;
+        }
+        else {
+            start_tx_transaction(rfp);
+        }
+    }
+    else {
+        if (rfp->tx_remaining-- == 0) {
+            rfp->timer->TASKS_STOP = 1;
+            NRF_PPI->CHENCLR = (1 << NRF52_RADIO_PPI_TX_START);
+            // All retransmits are expended, and the TX operation is suspended
+            rfp->tx_attempt = rfp->config.retransmit.count + 1;
+            rfp->flags |= NRF52_INT_TX_FAILED_MSK;
+
+            chBSemSignal(&events_sem);
+
+            rfp->state = NRF52_STATE_IDLE;
+        }
+        else {
+            // There are still have more retransmits left, TX mode should be
+            // entered again as soon as the system timer reaches CC[1].
+            NRF_RADIO->SHORTS = RADIO_SHORTS_COMMON | RADIO_SHORTS_DISABLED_RXEN_Msk;
+            set_rf_payload_format(rfp, p_current_payload->length);
+            NRF_RADIO->PACKETPTR = (uint32_t)tx_payload_buffer;
+            rfp->state = NRF52_STATE_PTX_TX_ACK;
+            rfp->timer->TASKS_START = 1;
+            NRF_PPI->CHENSET = (1 << NRF52_RADIO_PPI_TX_START);
+            if (rfp->timer->EVENTS_COMPARE[1])
+                NRF_RADIO->TASKS_TXEN = 1;
+        }
+    }
+}
+
+static void clear_events_restart_rx(RFDriver *rfp) {
+    NRF_RADIO->SHORTS = RADIO_SHORTS_COMMON;
+    set_rf_payload_format(rfp, rfp->config.payload_length);
+    NRF_RADIO->PACKETPTR = (uint32_t)rx_payload_buffer;
+
+    NRF_RADIO->INTENCLR = RADIO_INTENCLR_DISABLED_Msk;
+    NRF_RADIO->EVENTS_DISABLED = 0;
+    (void) NRF_RADIO->EVENTS_DISABLED;
+
+    NRF_RADIO->TASKS_DISABLE = 1;
+    while (NRF_RADIO->EVENTS_DISABLED == 0);
+
+    NRF_RADIO->EVENTS_DISABLED = 0;
+    (void) NRF_RADIO->EVENTS_DISABLED;
+    NRF_RADIO->INTENSET = RADIO_INTENSET_DISABLED_Msk;
+
+    NRF_RADIO->SHORTS = RADIO_SHORTS_COMMON | RADIO_SHORTS_DISABLED_TXEN_Msk;
+    NRF_RADIO->TASKS_RXEN = 1;
+}
+
+static void on_radio_disabled_rx(RFDriver *rfp) {
+    bool            ack                = false;
+    bool            retransmit_payload = false;
+    bool            send_rx_event      = true;
+    pipe_info_t *   p_pipe_info;
+
+    if (NRF_RADIO->CRCSTATUS == 0) {
+        clear_events_restart_rx(rfp);
+        return;
+    }
+
+    if(rx_fifo.count >= NRF52_RX_FIFO_SIZE) {
+        clear_events_restart_rx(rfp);
+        return;
+    }
+
+    p_pipe_info = &rx_pipe_info[NRF_RADIO->RXMATCH];
+    if (NRF_RADIO->RXCRC           == p_pipe_info->m_crc &&
+       (rx_payload_buffer[1] >> 1) == p_pipe_info->m_pid  ) {
+        retransmit_payload = true;
+        send_rx_event = false;
+    }
+
+    p_pipe_info->m_pid = rx_payload_buffer[1] >> 1;
+    p_pipe_info->m_crc = NRF_RADIO->RXCRC;
+
+    if(rfp->config.selective_auto_ack == false || ((rx_payload_buffer[1] & 0x01) == 0))
+        ack = true;
+
+    if(ack) {
+        NRF_RADIO->SHORTS = RADIO_SHORTS_COMMON | RADIO_SHORTS_DISABLED_RXEN_Msk;
+
+        switch(rfp->config.protocol) {
+            case NRF52_PROTOCOL_ESB_DPL:
+                {
+                    if (tx_fifo.count > 0 &&
+                        (tx_fifo.p_payload[tx_fifo.exit_point]->pipe == NRF_RADIO->RXMATCH))
+                    {
+                        // Pipe stays in ACK with payload until TX fifo is empty
+                        // Do not report TX success on first ack payload or retransmit
+                        if (p_pipe_info->m_ack_payload != 0 && !retransmit_payload) {
+                            if(++tx_fifo.exit_point >= NRF52_TX_FIFO_SIZE) {
+                                tx_fifo.exit_point = 0;
+                            }
+
+                            tx_fifo.count--;
+
+                            // ACK payloads also require TX_DS
+                            // (page 40 of the 'nRF24LE1_Product_Specification_rev1_6.pdf').
+                            rfp->flags |= NRF52_INT_TX_SUCCESS_MSK;
+                        }
+
+                        p_pipe_info->m_ack_payload = 1;
+
+                        p_current_payload = tx_fifo.p_payload[tx_fifo.exit_point];
+
+                        set_rf_payload_format(rfp, p_current_payload->length);
+                        tx_payload_buffer[0] = p_current_payload->length;
+                        memcpy(&tx_payload_buffer[2],
+                               p_current_payload->data,
+                               p_current_payload->length);
+                    }
+                    else {
+                        p_pipe_info->m_ack_payload = 0;
+                        set_rf_payload_format(rfp, 0);
+                        tx_payload_buffer[0] = 0;
+                    }
+
+                    tx_payload_buffer[1] = rx_payload_buffer[1];
+                }
+                break;
+
+            case NRF52_PROTOCOL_ESB:
+                {
+                    set_rf_payload_format(rfp, 0);
+                    tx_payload_buffer[0] = rx_payload_buffer[0];
+                    tx_payload_buffer[1] = 0;
+                }
+                break;
+        }
+
+        rfp->state = NRF52_STATE_PRX_SEND_ACK;
+        NRF_RADIO->TXADDRESS = NRF_RADIO->RXMATCH;
+        NRF_RADIO->PACKETPTR = (uint32_t)tx_payload_buffer;
+    }
+    else {
+        clear_events_restart_rx(rfp);
+    }
+
+    if (send_rx_event) {
+        // Push the new packet to the RX buffer and trigger a received event if the operation was
+        // successful.
+        if (rx_fifo_push_rfbuf(rfp, NRF_RADIO->RXMATCH, p_pipe_info->m_pid)) {
+            rfp->flags |= NRF52_INT_RX_DR_MSK;
+            chBSemSignal(&events_sem);
+        }
+    }
+}
+
+static void on_radio_disabled_rx_ack(RFDriver *rfp) {
+    NRF_RADIO->SHORTS = RADIO_SHORTS_COMMON | RADIO_SHORTS_DISABLED_TXEN_Msk;
+    set_rf_payload_format(rfp, rfp->config.payload_length);
+
+    NRF_RADIO->PACKETPTR = (uint32_t)rx_payload_buffer;
+
+    rfp->state = NRF52_STATE_PRX;
+}
+
+nrf52_error_t radio_disable(void) {
+    RFD1.state = NRF52_STATE_IDLE;
+
+    // Clear PPI
+    NRF_PPI->CHENCLR = (1 << NRF52_RADIO_PPI_TIMER_START) |
+                       (1 << NRF52_RADIO_PPI_TIMER_STOP)  |
+                       (1 << NRF52_RADIO_PPI_RX_TIMEOUT);
+
+    reset_fifo();
+
+    memset(rx_pipe_info, 0, sizeof(rx_pipe_info));
+    memset(pids, 0, sizeof(pids));
+
+    // Disable the radio
+    NRF_RADIO->SHORTS = RADIO_SHORTS_READY_START_Enabled << RADIO_SHORTS_READY_START_Pos |
+                        RADIO_SHORTS_END_DISABLE_Enabled << RADIO_SHORTS_END_DISABLE_Pos;
+
+    nvicDisableVector(RADIO_IRQn);
+
+    // Terminate interrupts handle thread
+    chThdTerminate(rfIntThread_p);
+    chBSemSignal(&disable_sem);
+    chThdWait(rfIntThread_p);
+
+    // Terminate events handle thread
+    chThdTerminate(rfEvtThread_p);
+    RFD1.flags = 0;
+    chBSemSignal(&events_sem);
+    chThdWait(rfEvtThread_p);
+
+    RFD1.state = NRF52_STATE_UNINIT;
+
+    return NRF52_SUCCESS;
+}
+
+//
+nrf52_error_t radio_init(nrf52_config_t const *config) {
+	osalDbgAssert(config != NULL,
+		"config must be defined");
+	osalDbgAssert(&config->address != NULL,
+		"address must be defined");
+	osalDbgAssert(NRF52_RADIO_IRQ_PRIORITY <= 7,
+		"wrong radio irq priority");
+
+    if (RFD1.state != NRF52_STATE_UNINIT) {
+    	nrf52_error_t err = radio_disable();
+        if (err != NRF52_SUCCESS)
+            return err;
+    }
+
+    RFD1.radio = NRF_RADIO;
+	RFD1.config = *config;
+    RFD1.flags    = 0;
+
+    init_fifo();
+
+#if NRF52_RADIO_USE_TIMER0
+    RFD1.timer = NRF_TIMER0;
+#endif
+#if NRF52_RADIO_USE_TIMER1
+    RFD1.timer = NRF_TIMER1;
+#endif
+#if NRF52_RADIO_USE_TIMER2
+    RFD1.timer = NRF_TIMER2;
+#endif
+#if NRF52_RADIO_USE_TIMER3
+    RFD1.timer = NRF_TIMER3;
+#endif
+#if NRF52_RADIO_USE_TIMER4
+    RFD1.timer = NRF_TIMER4;
+#endif
+
+    set_parameters(&RFD1);
+
+    set_addresses(&RFD1, NRF52_ADDR_UPDATE_MASK_BASE0);
+    set_addresses(&RFD1, NRF52_ADDR_UPDATE_MASK_BASE1);
+    set_addresses(&RFD1, NRF52_ADDR_UPDATE_MASK_PREFIX);
+
+    ppi_init(&RFD1);
+    timer_init(&RFD1);
+
+    chBSemObjectInit(&disable_sem, TRUE);
+    chBSemObjectInit(&events_sem, TRUE);
+
+    chEvtObjectInit(&RFD1.eventsrc);
+
+    // interrupt handle thread
+    rfIntThread_p = chThdCreateStatic(waRFIntThread, sizeof(waRFIntThread),
+    		NRF52_RADIO_INTTHD_PRIORITY, rfIntThread, NULL);
+
+    // events handle thread
+    rfEvtThread_p = chThdCreateStatic(waRFEvtThread, sizeof(waRFEvtThread),
+    		NRF52_RADIO_EVTTHD_PRIORITY, rfEvtThread, NULL);
+
+    nvicEnableVector(RADIO_IRQn, NRF52_RADIO_IRQ_PRIORITY);
+
+    RFD1.state = NRF52_STATE_IDLE;
+
+    return NRF52_SUCCESS;
+}
+
+nrf52_error_t radio_write_payload(nrf52_payload_t const * p_payload) {
+    if (RFD1.state == NRF52_STATE_UNINIT)
+    	return NRF52_INVALID_STATE;
+    if(p_payload == NULL)
+    	return NRF52_ERROR_NULL;
+    VERIFY_PAYLOAD_LENGTH(p_payload);
+    if (tx_fifo.count >= NRF52_TX_FIFO_SIZE)
+    	return NRF52_ERROR_INVALID_LENGTH;
+
+    if (RFD1.config.mode == NRF52_MODE_PTX &&
+        p_payload->noack && !RFD1.config.selective_auto_ack )
+    {
+        return NRF52_ERROR_NOT_SUPPORTED;
+    }
+
+    nvicDisableVector(RADIO_IRQn);
+
+    memcpy(tx_fifo.p_payload[tx_fifo.entry_point], p_payload, sizeof(nrf52_payload_t));
+
+    pids[p_payload->pipe] = (pids[p_payload->pipe] + 1) % (NRF52_PID_MAX + 1);
+    tx_fifo.p_payload[tx_fifo.entry_point]->pid = pids[p_payload->pipe];
+
+    if (++tx_fifo.entry_point >= NRF52_TX_FIFO_SIZE) {
+        tx_fifo.entry_point = 0;
+    }
+
+    tx_fifo.count++;
+
+    nvicEnableVector(RADIO_IRQn, NRF52_RADIO_IRQ_PRIORITY);
+
+    if (RFD1.config.mode == NRF52_MODE_PTX &&
+        RFD1.config.tx_mode == NRF52_TXMODE_AUTO &&
+        RFD1.state == NRF52_STATE_IDLE)
+    {
+        start_tx_transaction(&RFD1);
+    }
+
+    return NRF52_SUCCESS;
+}
+
+nrf52_error_t radio_read_rx_payload(nrf52_payload_t * p_payload) {
+    if (RFD1.state == NRF52_STATE_UNINIT)
+    	return NRF52_INVALID_STATE;
+    if (p_payload == NULL)
+    	return NRF52_ERROR_NULL;
+
+    if (rx_fifo.count == 0) {
+        return NRF52_ERROR_INVALID_LENGTH;
+    }
+
+    nvicDisableVector(RADIO_IRQn);
+
+    p_payload->length = rx_fifo.p_payload[rx_fifo.exit_point]->length;
+    p_payload->pipe   = rx_fifo.p_payload[rx_fifo.exit_point]->pipe;
+    p_payload->rssi   = rx_fifo.p_payload[rx_fifo.exit_point]->rssi;
+    p_payload->pid    = rx_fifo.p_payload[rx_fifo.exit_point]->pid;
+    memcpy(p_payload->data, rx_fifo.p_payload[rx_fifo.exit_point]->data, p_payload->length);
+
+    if (++rx_fifo.exit_point >= NRF52_RX_FIFO_SIZE) {
+        rx_fifo.exit_point = 0;
+    }
+
+    rx_fifo.count--;
+
+    nvicEnableVector(RADIO_IRQn, NRF52_RADIO_IRQ_PRIORITY);
+
+    return NRF52_SUCCESS;
+}
+
+nrf52_error_t radio_start_tx(void) {
+    if (RFD1.state != NRF52_STATE_IDLE)
+    	return NRF52_ERROR_BUSY;
+
+    if (tx_fifo.count == 0) {
+        return NRF52_ERROR_INVALID_LENGTH;
+    }
+
+    start_tx_transaction(&RFD1);
+
+    return NRF52_SUCCESS;
+}
+
+nrf52_error_t radio_start_rx(void) {
+    if (RFD1.state != NRF52_STATE_IDLE)
+    	return NRF52_ERROR_BUSY;
+
+    NRF_RADIO->INTENCLR = 0xFFFFFFFF;
+    NRF_RADIO->EVENTS_DISABLED = 0;
+    (void) NRF_RADIO->EVENTS_DISABLED;
+
+    NRF_RADIO->SHORTS      = RADIO_SHORTS_COMMON | RADIO_SHORTS_DISABLED_TXEN_Msk;
+    NRF_RADIO->INTENSET    = RADIO_INTENSET_DISABLED_Msk;
+    RFD1.state             = NRF52_STATE_PRX;
+
+    NRF_RADIO->RXADDRESSES  = RFD1.config.address.rx_pipes;
+    NRF_RADIO->FREQUENCY    = RFD1.config.address.rf_channel;
+    NRF_RADIO->PACKETPTR    = (uint32_t)rx_payload_buffer;
+
+    nvicClearPending(RADIO_IRQn);
+    nvicEnableVector(RADIO_IRQn, NRF52_RADIO_IRQ_PRIORITY);
+
+    NRF_RADIO->EVENTS_ADDRESS = 0;
+    NRF_RADIO->EVENTS_PAYLOAD = 0;
+    NRF_RADIO->EVENTS_DISABLED = 0;
+    (void) NRF_RADIO->EVENTS_ADDRESS;
+    (void) NRF_RADIO->EVENTS_PAYLOAD;
+    (void) NRF_RADIO->EVENTS_DISABLED;
+
+    NRF_RADIO->TASKS_RXEN  = 1;
+
+    return NRF52_SUCCESS;
+}
+
+nrf52_error_t radio_stop_rx(void) {
+    if (RFD1.state != NRF52_STATE_PRX) {
+        return NRF52_INVALID_STATE;
+    }
+
+    NRF_RADIO->SHORTS = 0;
+    NRF_RADIO->INTENCLR = 0xFFFFFFFF;
+    NRF_RADIO->EVENTS_DISABLED = 0;
+    (void) NRF_RADIO->EVENTS_DISABLED;
+    NRF_RADIO->TASKS_DISABLE = 1;
+    while (NRF_RADIO->EVENTS_DISABLED == 0);
+    RFD1.state = NRF52_STATE_IDLE;
+
+    return NRF52_SUCCESS;
+}
+
+nrf52_error_t radio_flush_tx(void) {
+    if (RFD1.state == NRF52_STATE_UNINIT)
+    	return NRF52_INVALID_STATE;
+
+    nvicDisableVector(RADIO_IRQn);
+
+    tx_fifo.count = 0;
+    tx_fifo.entry_point = 0;
+    tx_fifo.exit_point = 0;
+
+    nvicEnableVector(RADIO_IRQn, NRF52_RADIO_IRQ_PRIORITY);
+
+    return NRF52_SUCCESS;
+}
+
+nrf52_error_t radio_pop_tx(void) {
+    if (RFD1.state == NRF52_STATE_UNINIT)
+    	return NRF52_INVALID_STATE;
+    if (tx_fifo.count == 0)
+    	return NRF52_ERROR_INVALID_LENGTH;
+
+    nvicDisableVector(RADIO_IRQn);
+
+    if (++tx_fifo.entry_point >= NRF52_TX_FIFO_SIZE) {
+        tx_fifo.entry_point = 0;
+    }
+    tx_fifo.count--;
+
+    nvicEnableVector(RADIO_IRQn, NRF52_RADIO_IRQ_PRIORITY);
+
+    return NRF52_SUCCESS;
+}
+
+nrf52_error_t radio_flush_rx(void) {
+    if (RFD1.state == NRF52_STATE_UNINIT)
+    	return NRF52_INVALID_STATE;
+
+    nvicDisableVector(RADIO_IRQn);
+
+    rx_fifo.count = 0;
+    rx_fifo.entry_point = 0;
+    rx_fifo.exit_point = 0;
+
+    memset(rx_pipe_info, 0, sizeof(rx_pipe_info));
+
+    nvicEnableVector(RADIO_IRQn, NRF52_RADIO_IRQ_PRIORITY);
+
+    return NRF52_SUCCESS;
+}
+
+nrf52_error_t radio_set_base_address_0(uint8_t const * p_addr) {
+    if (RFD1.state != NRF52_STATE_IDLE)
+    	return NRF52_ERROR_BUSY;
+    if (p_addr == NULL)
+        return NRF52_ERROR_NULL;
+
+    memcpy(RFD1.config.address.base_addr_p0, p_addr, 4);
+    set_addresses(&RFD1, NRF52_ADDR_UPDATE_MASK_BASE0);
+
+    return NRF52_SUCCESS;
+}
+
+nrf52_error_t radio_set_base_address_1(uint8_t const * p_addr) {
+    if (RFD1.state != NRF52_STATE_IDLE)
+    	return NRF52_ERROR_BUSY;
+    if (p_addr == NULL)
+        return NRF52_ERROR_NULL;
+
+    memcpy(RFD1.config.address.base_addr_p1, p_addr, 4);
+    set_addresses(&RFD1, NRF52_ADDR_UPDATE_MASK_BASE1);
+
+    return NRF52_SUCCESS;
+}
+
+nrf52_error_t radio_set_prefixes(uint8_t const * p_prefixes, uint8_t num_pipes) {
+    if (RFD1.state != NRF52_STATE_IDLE)
+    	return NRF52_ERROR_BUSY;
+    if (p_prefixes == NULL)
+        return NRF52_ERROR_NULL;
+    if (num_pipes > 8)
+    	return NRF52_ERROR_INVALID_PARAM;
+
+    memcpy(RFD1.config.address.pipe_prefixes, p_prefixes, num_pipes);
+    RFD1.config.address.num_pipes = num_pipes;
+    RFD1.config.address.rx_pipes = BIT_MASK_UINT_8(num_pipes);
+
+    set_addresses(&RFD1, NRF52_ADDR_UPDATE_MASK_PREFIX);
+
+    return NRF52_SUCCESS;
+}
+
+nrf52_error_t radio_set_prefix(uint8_t pipe, uint8_t prefix) {
+    if (RFD1.state != NRF52_STATE_IDLE)
+    	return NRF52_ERROR_BUSY;
+    if (pipe > 8)
+    	return NRF52_ERROR_INVALID_PARAM;
+
+    RFD1.config.address.pipe_prefixes[pipe] = prefix;
+
+    NRF_RADIO->PREFIX0 = bytewise_bit_swap(&RFD1.config.address.pipe_prefixes[0]);
+    NRF_RADIO->PREFIX1 = bytewise_bit_swap(&RFD1.config.address.pipe_prefixes[4]);
+
+    return NRF52_SUCCESS;
+}
diff --git a/os/various/devices_lib/rf/nrf52_radio.h b/os/various/devices_lib/rf/nrf52_radio.h
new file mode 100644
index 0000000..2f94465
--- /dev/null
+++ b/os/various/devices_lib/rf/nrf52_radio.h
@@ -0,0 +1,256 @@
+/* Copyright (c) 2014 Nordic Semiconductor. All Rights Reserved.
+ *
+ * The information contained herein is property of Nordic Semiconductor ASA.
+ * Terms and conditions of usage are described in detail in NORDIC
+ * SEMICONDUCTOR STANDARD SOFTWARE LICENSE AGREEMENT.
+ *
+ * Licensees are granted free, non-transferable use of the information. NO
+ * WARRANTY of ANY KIND is provided. This heading must NOT be removed from
+ * the file.
+ *
+ * @brief Enhanced ShockBurst (ESB) is a basic protocol supporting two-way data
+ *        packet communication including packet buffering, packet acknowledgment
+ *        and automatic retransmission of lost packets.
+ *
+ * ported on: 25/10/2018, by andru
+ *
+ */
+
+#ifndef NRF52_RADIO_H_
+#define NRF52_RADIO_H_
+
+// Hard coded parameters - change if necessary
+#ifndef NRF52_MAX_PAYLOAD_LENGTH
+#define NRF52_MAX_PAYLOAD_LENGTH            32                  /**< The max size of the payload. Valid values are 1 to 252 */
+#endif
+
+#define NRF52_CRC_RESET_VALUE             	0xFFFF              /**< CRC reset value*/
+
+#define NRF52_TX_FIFO_SIZE                  8                   /**< The size of the transmission first in first out buffer. */
+#define NRF52_RX_FIFO_SIZE                  8                   /**< The size of the reception first in first out buffer. */
+
+#define NRF52_RADIO_USE_TIMER0            	FALSE               /**< TIMER0 will be used by the module. */
+#define NRF52_RADIO_USE_TIMER1            	TRUE                /**< TIMER1 will be used by the module. */
+#define NRF52_RADIO_USE_TIMER2            	FALSE               /**< TIMER2 will be used by the module. */
+#define NRF52_RADIO_USE_TIMER3            	FALSE               /**< TIMER3 will be used by the module. */
+#define NRF52_RADIO_USE_TIMER4            	FALSE               /**< TIMER4 will be used by the module. */
+
+#define NRF52_RADIO_IRQ_PRIORITY			3                   /**< RADIO interrupt priority. */
+#define NRF52_RADIO_INTTHD_PRIORITY         (NORMALPRIO+2)      /**< Interrupts handle thread priority. */
+#define NRF52_RADIO_EVTTHD_PRIORITY         (NORMALPRIO+1)      /**< Events handle thread priority */
+
+#define NRF52_RADIO_PPI_TIMER_START         10                  /**< The PPI channel used for timer start. */
+#define NRF52_RADIO_PPI_TIMER_STOP          11                  /**< The PPI channel used for timer stop. */
+#define NRF52_RADIO_PPI_RX_TIMEOUT          12                  /**< The PPI channel used for RX timeout. */
+#define NRF52_RADIO_PPI_TX_START            13                  /**< The PPI channel used for starting TX. */
+
+
+typedef enum {
+	NRF52_SUCCESS,                                        /* Call was successful.                  */
+	NRF52_INVALID_STATE,                                  /* Module is not initialized.            */
+	NRF52_ERROR_BUSY,                                     /* Module was not in idle state.         */
+	NRF52_ERROR_NULL,                                     /* Required parameter was NULL.          */
+	NRF52_ERROR_INVALID_PARAM,                            /* Required parameter is invalid         */
+	NRF52_ERROR_NOT_SUPPORTED,                            /* p_payload->noack was false while selective ack was not enabled. */
+	NRF52_ERROR_INVALID_LENGTH,                           /* Payload length was invalid (zero or larger than max allowed).   */
+} nrf52_error_t;
+
+// Internal radio module state.
+typedef enum {
+    NRF52_STATE_UNINIT,                                   /**< Module not initialized. */
+    NRF52_STATE_IDLE,                                     /**< Module idle. */
+    NRF52_STATE_PTX_TX,                                   /**< Module transmitting without ack. */
+    NRF52_STATE_PTX_TX_ACK,                               /**< Module transmitting with ack. */
+    NRF52_STATE_PTX_RX_ACK,                               /**< Module transmitting with ack and reception of payload with the ack response. */
+    NRF52_STATE_PRX,                                      /**< Module receiving packets without ack. */
+    NRF52_STATE_PRX_SEND_ACK,                             /**< Module transmitting ack in RX mode. */
+} nrf52_state_t;
+
+/**@brief Events to indicate the last transmission/receiving status. */
+typedef enum {
+    NRF52_EVENT_TX_SUCCESS  = 0x01,   /**< Event triggered on TX success.     */
+    NRF52_EVENT_TX_FAILED   = 0x02,   /**< Event triggered on TX failed.      */
+    NRF52_EVENT_RX_RECEIVED = 0x04,   /**< Event triggered on RX Received.    */
+} nrf52_event_t;
+
+// Interrupt flags
+typedef enum {
+	NRF52_INT_TX_SUCCESS_MSK = 0x01,  /**< The flag used to indicate a success since last event. */
+	NRF52_INT_TX_FAILED_MSK  = 0x02,  /**< The flag used to indicate a failiure since last event. */
+	NRF52_INT_RX_DR_MSK 	 = 0x04,  /**< The flag used to indicate a received packet since last event. */
+} nrf52_int_flags_t;
+
+/**Macro to create initializer for a TX data packet.
+ *
+ * @details This macro generates an initializer. It is more efficient
+ *          than setting the individual parameters dynamically.
+ *
+ * @param[in]   _pipe   The pipe to use for the data packet.
+ * @param[in]   ...     Comma separated list of character data to put in the TX buffer.
+ *                      Supported values are from 1 to 63 characters.
+ *
+ * @return  Initializer that sets up pipe, length and the byte array for content of the TX data.
+ */
+#define NRF52_CREATE_PAYLOAD(_pipe, ...)                                                  \
+        {.pipe = _pipe, .length = NUM_VA_ARGS(__VA_ARGS__), .data = {__VA_ARGS__}};         \
+        STATIC_ASSERT(NUM_VA_ARGS(__VA_ARGS__) > 0 && NUM_VA_ARGS(__VA_ARGS__) <= 63)
+
+/**@brief Enhanced ShockBurst protocol. */
+typedef enum {
+    NRF52_PROTOCOL_ESB,      /*< Enhanced ShockBurst with fixed payload length.                                            */
+    NRF52_PROTOCOL_ESB_DPL   /*< Enhanced ShockBurst with dynamic payload length.                                          */
+} nrf52_protocol_t;
+
+/**@brief Enhanced ShockBurst mode. */
+typedef enum {
+    NRF52_MODE_PTX,          /*< Primary transmitter mode. */
+    NRF52_MODE_PRX           /*< Primary receiver mode.    */
+} nrf52_mode_t;
+
+/**@brief Enhanced ShockBurst bitrate mode. */
+typedef enum {
+    NRF52_BITRATE_2MBPS     = RADIO_MODE_MODE_Nrf_2Mbit,      /**< 2Mbit radio mode.                                             */
+    NRF52_BITRATE_1MBPS     = RADIO_MODE_MODE_Nrf_1Mbit,      /**< 1Mbit radio mode.                                             */
+} nrf52_bitrate_t;
+
+/**@brief Enhanced ShockBurst CRC modes. */
+typedef enum {
+    NRF52_CRC_16BIT         = RADIO_CRCCNF_LEN_Two,                   /**< Use two byte CRC. */
+    NRF52_CRC_8BIT          = RADIO_CRCCNF_LEN_One,                   /**< Use one byte CRC. */
+    NRF52_CRC_OFF           = RADIO_CRCCNF_LEN_Disabled               /**< Disable CRC.      */
+} nrf52_crc_t;
+
+/**@brief Enhanced ShockBurst radio transmission power modes. */
+typedef enum {
+    NRF52_TX_POWER_4DBM     = RADIO_TXPOWER_TXPOWER_Pos4dBm,  /**< 4 dBm radio transmit power.   */
+    NRF52_TX_POWER_0DBM     = RADIO_TXPOWER_TXPOWER_0dBm,     /**< 0 dBm radio transmit power.   */
+    NRF52_TX_POWER_NEG4DBM  = RADIO_TXPOWER_TXPOWER_Neg4dBm,  /**< -4 dBm radio transmit power.  */
+    NRF52_TX_POWER_NEG8DBM  = RADIO_TXPOWER_TXPOWER_Neg8dBm,  /**< -8 dBm radio transmit power.  */
+    NRF52_TX_POWER_NEG12DBM = RADIO_TXPOWER_TXPOWER_Neg12dBm, /**< -12 dBm radio transmit power. */
+    NRF52_TX_POWER_NEG16DBM = RADIO_TXPOWER_TXPOWER_Neg16dBm, /**< -16 dBm radio transmit power. */
+    NRF52_TX_POWER_NEG20DBM = RADIO_TXPOWER_TXPOWER_Neg20dBm, /**< -20 dBm radio transmit power. */
+    NRF52_TX_POWER_NEG30DBM = RADIO_TXPOWER_TXPOWER_Neg30dBm  /**< -30 dBm radio transmit power. */
+} nrf52_tx_power_t;
+
+/**@brief Enhanced ShockBurst transmission modes. */
+typedef enum {
+    NRF52_TXMODE_AUTO,        /*< Automatic TX mode - When the TX fifo is non-empty and the radio is idle packets will be sent automatically. */
+    NRF52_TXMODE_MANUAL,      /*< Manual TX mode - Packets will not be sent until radio_start_tx() is called. Can be used to ensure consistent packet timing. */
+    NRF52_TXMODE_MANUAL_START /*< Manual start TX mode - Packets will not be sent until radio_start_tx() is called, but transmission will continue automatically until the TX fifo is empty. */
+} nrf52_tx_mode_t;
+
+/**@brief Enhanced ShockBurst addresses.
+ *
+ * @details The module is able to transmit packets with the TX address stored in tx_address.
+            The module can also receive packets from peers with up to eight different tx_addresses
+            stored in esb_addr_p0 - esb_addr_p7. esb_addr_p0 can have 5 arbitrary bytes
+            independent of the other addresses. esb_addr_p1 - esb_addr_p7 will share the
+            same four byte base address found in the last four bytes of esb_addr_p1.
+            They have an independent prefix byte found in esb_addr_p1[0] and esb_addr_p2 -
+            esb_addr_p7.
+*/
+typedef struct {
+    uint8_t base_addr_p0[4];        /**< Base address for pipe 0 encoded in big endian. */
+    uint8_t base_addr_p1[4];        /**< Base address for pipe 1-7 encoded in big endian. */
+    uint8_t pipe_prefixes[8];       /**< Address prefix for pipe P0 to P7. */
+    uint8_t num_pipes;              /**< Number of pipes available. */
+    uint8_t addr_length;            /**< Length of address including prefix */
+    uint8_t rx_pipes;		        /**< Bitfield for enabled RX pipes. */
+    uint8_t rf_channel;             /**< Which channel is to be used. Must be in range 0 and 125 to be valid. */
+} nrf52_address_t;
+
+/**@brief Enhanced ShockBurst payload.
+ *
+ * @note The payload is used both for transmission and receive with ack and payload.
+*/
+typedef struct
+{
+    uint8_t length;                              /**< Length of the packet. Should be equal or less than NRF_ESB_MAX_PAYLOAD_LENGTH. */
+    uint8_t pipe;                                /**< Pipe used for this payload. */
+    int8_t  rssi;                                /**< RSSI for received packet. */
+    uint8_t noack;                               /**< Flag indicating that this packet will not be acknowledged. */
+    uint8_t pid;                                 /**< PID assigned during communication. */
+    uint8_t data[NRF52_MAX_PAYLOAD_LENGTH];      /**< The payload data. */
+} nrf52_payload_t;
+
+/**@brief Retransmit attempts delay and counter. */
+typedef struct {
+    uint16_t              delay;                  /**< The delay between each retransmission of unacked packets. */
+    uint16_t              count;                  /**< The number of retransmissions attempts before transmission fail. */
+} nrf52_retransmit_t;
+
+/**@brief Main nrf_esb configuration struct. */
+typedef struct {
+    nrf52_protocol_t      protocol;               /**< Enhanced ShockBurst protocol. */
+    nrf52_mode_t          mode;                   /**< Enhanced ShockBurst default RX or TX mode. */
+
+    // General RF parameters
+    nrf52_bitrate_t       bitrate;                /**< Enhanced ShockBurst bitrate mode. */
+    nrf52_crc_t           crc;                    /**< Enhanced ShockBurst CRC mode. */
+    nrf52_tx_power_t      tx_power;   		      /**< Enhanced ShockBurst radio transmission power mode.*/
+
+    // Control settings
+    nrf52_tx_mode_t       tx_mode;                /**< Enhanced ShockBurst transmit mode. */
+
+    bool                  selective_auto_ack;     /**< Enable or disable selective auto acknowledgement. */
+
+    nrf52_retransmit_t    retransmit;             /**< Packet retransmit parameters */
+
+    uint8_t               payload_length;         /**< Enhanced ShockBurst static payload length */
+
+    nrf52_address_t    	  address;                /**< Address parameters structure */
+} nrf52_config_t;
+
+typedef struct {
+  /**
+   * @brief NRF52 radio peripheral.
+   */
+  NRF_RADIO_Type          *radio;
+  /**
+   * @brief NRF52 timer peripheral.
+   */
+  NRF_TIMER_Type          *timer;
+  /**
+   * @brief Driver state.
+   */
+  nrf52_state_t           state;
+  /**
+   * @brief RF parameters.
+   */
+  nrf52_config_t          config;
+  /**
+   * @brief Interrupts flag.
+   */
+  nrf52_int_flags_t	      flags;
+  /**
+   * @brief TX attempt number.
+   */
+  uint16_t                tx_attempt;
+  /**
+   * @brief TX retransmits remaining.
+   */
+  uint16_t                tx_remaining;
+  /**
+   * @brief Radio events source.
+   */
+  event_source_t eventsrc;
+} RFDriver;
+
+extern RFDriver RFD1;
+
+nrf52_error_t radio_init(nrf52_config_t const *config);
+nrf52_error_t radio_disable(void);
+nrf52_error_t radio_write_payload(nrf52_payload_t const * p_payload);
+nrf52_error_t radio_read_rx_payload(nrf52_payload_t * p_payload);
+nrf52_error_t radio_start_tx(void);
+nrf52_error_t radio_start_rx(void);
+nrf52_error_t radio_stop_rx(void);
+nrf52_error_t radio_flush_tx(void);
+nrf52_error_t radio_flush_rx(void);
+nrf52_error_t radio_pop_tx(void);
+nrf52_error_t radio_set_base_address_0(uint8_t const * p_addr);
+nrf52_error_t radio_set_base_address_1(uint8_t const * p_addr);
+nrf52_error_t radio_set_prefixes(uint8_t const * p_prefixes, uint8_t num_pipes);
+nrf52_error_t radio_set_prefix(uint8_t pipe, uint8_t prefix);
+
+#endif /* NRF52_RADIO_H_ */
diff --git a/os/various/fatfs_bindings/fatfs.mk b/os/various/fatfs_bindings/fatfs.mk
index 02ebed5..7548acf 100644
--- a/os/various/fatfs_bindings/fatfs.mk
+++ b/os/various/fatfs_bindings/fatfs.mk
@@ -5,3 +5,7 @@ FATFSSRC = ${CHIBIOS_CONTRIB}/os/various/fatfs_bindings/fatfs_diskio.c \
            $(CHIBIOS)/ext/fatfs/src/ffunicode.c
 
 FATFSINC = ${CHIBIOS}/ext/fatfs/src ${CHIBIOS_CONTRIB}/os/various/fatfs_bindings
+
+# Shared variables
+ALLCSRC += $(FATFSSRC)
+ALLINC  += $(FATFSINC)
diff --git a/os/various/fatfs_bindings/fatfs_devices.h b/os/various/fatfs_bindings/fatfs_devices.h
index 5ec05ca..fe28383 100644
--- a/os/various/fatfs_bindings/fatfs_devices.h
+++ b/os/various/fatfs_bindings/fatfs_devices.h
@@ -20,39 +20,21 @@
 
 #include "hal.h"
 
-#if HAL_USE_MMC_SPI && HAL_USE_SDC
-#error "cannot specify both MMC_SPI and SDC drivers"
-#endif
-
-#if HAL_USE_MMC_SPI
-extern MMCDriver MMCD1;
-#elif HAL_USE_SDC
-extern SDCDriver SDCD1;
-#elif HAL_USBH_USE_MSD
-
-#else
-#error "MMC_SPI, SDC or USBH_MSD driver must be specified"
-#endif
-
 /*-----------------------------------------------------------------------*/
 /* Correspondence between physical drive number and physical drive.      */
-#if HAL_USE_MMC_SPI
+#if HAL_USE_MMC_SPI || HAL_USE_SDC
 #define FATFSDEV_MMC         0
 #define FATFSDEV_MMC_DRIVE   "0:"
 #endif
 
-#if HAL_USE_SDC
-#define FATFSDEV_SDC         0
-#define FATFSDEV_SDC_DRIVE   "0:"
-#endif
 
 #if HAL_USBH_USE_MSD
-#if defined(FATFSDEV_MMC) || defined(FATFSDEV_SDC)
-#define FATFSDEV_MSDLUN0         1
-#define FATFSDEV_MSDLUN0_DRIVE   "1:"
+#if defined(FATFSDEV_MMC)
+#define FATFSDEV_MSD         1
+#define FATFSDEV_MSD_DRIVE   "1:"
 #else
-#define FATFSDEV_MSDLUN0         0
-#define FATFSDEV_MSDLUN0_DRIVE   "0:"
+#define FATFSDEV_MSD         0
+#define FATFSDEV_MSD_DRIVE   "0:"
 #endif
 #endif
 
diff --git a/os/various/fatfs_bindings/fatfs_diskio.c b/os/various/fatfs_bindings/fatfs_diskio.c
index 0104a11..e3cfd53 100644
--- a/os/various/fatfs_bindings/fatfs_diskio.c
+++ b/os/various/fatfs_bindings/fatfs_diskio.c
@@ -7,15 +7,48 @@
 
 #include "hal.h"
 #include "ffconf.h"
+#include "ff.h"
 #include "diskio.h"
 #include "usbh/dev/msd.h"
 #include "fatfs_devices.h"
 
+#if HAL_USE_MMC_SPI && HAL_USE_SDC
+#error "cannot specify both MMC_SPI and FATFSDEV_MMC drivers"
+#endif
+
+// sanity check for no FAT option selected
+// why is the FAT sources being pulled into the build?
+#if !HAL_USE_MMC_SPI && !HAL_USE_SDC & !HAL_USBH_USE_MSD
+#error "MMC_SPI, FATFSDEV_MMC or USBH_MSD driver must be specified. None was."
+#endif
+
+#if !defined(FATFS_HAL_DEVICE)
+#if HAL_USE_MMC_SPI
+#define FATFS_HAL_DEVICE MMCD1
+#else
+#define FATFS_HAL_DEVICE SDCD1
+#endif
+#endif
+
+#if HAL_USE_MMC_SPI
+extern MMCDriver FATFS_HAL_DEVICE;
+#endif
+#if HAL_USE_SDC
+extern SDCDriver FATFS_HAL_DEVICE;
+#endif
+#if HAL_USBH_USE_MSD
+#endif
+
+#if HAL_USE_RTC
+extern RTCDriver RTCD1;
+#endif
+
+
 /*-----------------------------------------------------------------------*/
 /* Inidialize a Drive                                                    */
 
 DSTATUS disk_initialize (
-    BYTE pdrv                /* Physical drive nmuber (0..) */
+    BYTE pdrv         /* Physical drive number (0..) */
 )
 {
   DSTATUS stat;
@@ -25,23 +58,23 @@ DSTATUS disk_initialize (
   case FATFSDEV_MMC:
     stat = 0;
     /* It is initialized externally, just reads the status.*/
-    if (blkGetDriverState(&MMCD1) != BLK_READY)
+    if (blkGetDriverState(&FATFS_HAL_DEVICE) != BLK_READY)
       stat |= STA_NOINIT;
-    if (mmcIsWriteProtected(&MMCD1))
+    if (mmcIsWriteProtected(&FATFS_HAL_DEVICE))
       stat |=  STA_PROTECT;
     return stat;
 #elif HAL_USE_SDC
-  case FATFSDEV_SDC:
+  case FATFSDEV_MMC:
     stat = 0;
     /* It is initialized externally, just reads the status.*/
-    if (blkGetDriverState(&SDCD1) != BLK_READY)
+    if (blkGetDriverState(&FATFS_HAL_DEVICE) != BLK_READY)
       stat |= STA_NOINIT;
-    if (sdcIsWriteProtected(&SDCD1))
+    if (sdcIsWriteProtected(&FATFS_HAL_DEVICE))
       stat |=  STA_PROTECT;
     return stat;
 #endif
 #if HAL_USBH_USE_MSD
-  case FATFSDEV_MSDLUN0:
+  case FATFSDEV_MSD:
 	stat = 0;
 	/* It is initialized externally, just reads the status.*/
 	if (blkGetDriverState(&MSBLKD[0]) != BLK_READY)
@@ -68,23 +101,23 @@ DSTATUS disk_status (
   case FATFSDEV_MMC:
     stat = 0;
     /* It is initialized externally, just reads the status.*/
-    if (blkGetDriverState(&MMCD1) != BLK_READY)
+    if (blkGetDriverState(&FATFS_HAL_DEVICE) != BLK_READY)
       stat |= STA_NOINIT;
-    if (mmcIsWriteProtected(&MMCD1))
+    if (mmcIsWriteProtected(&FATFS_HAL_DEVICE))
       stat |= STA_PROTECT;
     return stat;
 #elif HAL_USE_SDC
-  case FATFSDEV_SDC:
+  case FATFSDEV_MMC:
     stat = 0;
     /* It is initialized externally, just reads the status.*/
-    if (blkGetDriverState(&SDCD1) != BLK_READY)
+    if (blkGetDriverState(&FATFS_HAL_DEVICE) != BLK_READY)
       stat |= STA_NOINIT;
-    if (sdcIsWriteProtected(&SDCD1))
+    if (sdcIsWriteProtected(&FATFS_HAL_DEVICE))
       stat |= STA_PROTECT;
     return stat;
 #endif
 #if HAL_USBH_USE_MSD
-  case FATFSDEV_MSDLUN0:
+  case FATFSDEV_MSD:
     stat = 0;
     /* It is initialized externally, just reads the status.*/
     if (blkGetDriverState(&MSBLKD[0]) != BLK_READY)
@@ -110,29 +143,29 @@ DRESULT disk_read (
   switch (pdrv) {
 #if HAL_USE_MMC_SPI
   case FATFSDEV_MMC:
-    if (blkGetDriverState(&MMCD1) != BLK_READY)
+    if (blkGetDriverState(&FATFS_HAL_DEVICE) != BLK_READY)
       return RES_NOTRDY;
-    if (mmcStartSequentialRead(&MMCD1, sector))
+    if (mmcStartSequentialRead(&FATFS_HAL_DEVICE, sector))
       return RES_ERROR;
     while (count > 0) {
-      if (mmcSequentialRead(&MMCD1, buff))
+      if (mmcSequentialRead(&FATFS_HAL_DEVICE, buff))
         return RES_ERROR;
       buff += MMCSD_BLOCK_SIZE;
       count--;
     }
-    if (mmcStopSequentialRead(&MMCD1))
+    if (mmcStopSequentialRead(&FATFS_HAL_DEVICE))
         return RES_ERROR;
     return RES_OK;
 #elif HAL_USE_SDC
-  case FATFSDEV_SDC:
-    if (blkGetDriverState(&SDCD1) != BLK_READY)
+  case FATFSDEV_MMC:
+    if (blkGetDriverState(&FATFS_HAL_DEVICE) != BLK_READY)
       return RES_NOTRDY;
-    if (sdcRead(&SDCD1, sector, buff, count))
+    if (sdcRead(&FATFS_HAL_DEVICE, sector, buff, count))
       return RES_ERROR;
     return RES_OK;
 #endif
 #if HAL_USBH_USE_MSD
-  case FATFSDEV_MSDLUN0:
+  case FATFSDEV_MSD:
 	/* It is initialized externally, just reads the status.*/
 	if (blkGetDriverState(&MSBLKD[0]) != BLK_READY)
 		return RES_NOTRDY;
@@ -149,6 +182,7 @@ DRESULT disk_read (
 /*-----------------------------------------------------------------------*/
 /* Write Sector(s)                                                       */
 
+#if !FF_FS_READONLY
 DRESULT disk_write (
     BYTE pdrv,        /* Physical drive number (0..) */
     const BYTE *buff, /* Data to be written */
@@ -156,37 +190,51 @@ DRESULT disk_write (
     UINT count        /* Number of sectors to write (1..255) */
 )
 {
+
   switch (pdrv) {
 #if HAL_USE_MMC_SPI
   case FATFSDEV_MMC:
-    if (blkGetDriverState(&MMCD1) != BLK_READY)
+    if (blkGetDriverState(&FATFS_HAL_DEVICE) != BLK_READY)
         return RES_NOTRDY;
-    if (mmcIsWriteProtected(&MMCD1))
+    if (mmcIsWriteProtected(&FATFS_HAL_DEVICE))
         return RES_WRPRT;
-    if (mmcStartSequentialWrite(&MMCD1, sector))
+    if (mmcStartSequentialWrite(&FATFS_HAL_DEVICE, sector))
         return RES_ERROR;
+
     while (count > 0) {
-        if (mmcSequentialWrite(&MMCD1, buff))
+        // invalidate cache on buffer
+        cacheBufferFlush(buff, MMCSD_BLOCK_SIZE);
+
+        if (mmcSequentialWrite(&FATFS_HAL_DEVICE, buff))
             return RES_ERROR;
         buff += MMCSD_BLOCK_SIZE;
         count--;
     }
-    if (mmcStopSequentialWrite(&MMCD1))
+    if (mmcStopSequentialWrite(&FATFS_HAL_DEVICE))
         return RES_ERROR;
     return RES_OK;
 #elif HAL_USE_SDC
-  case FATFSDEV_SDC:
-    if (blkGetDriverState(&SDCD1) != BLK_READY)
+  case FATFSDEV_MMC:
+    if (blkGetDriverState(&FATFS_HAL_DEVICE) != BLK_READY)
       return RES_NOTRDY;
-    if (sdcWrite(&SDCD1, sector, buff, count))
-      return RES_ERROR;
+
+    // invalidate cache on buffer
+    cacheBufferFlush(buff, count * MMCSD_BLOCK_SIZE);
+
+    if (sdcWrite(&FATFS_HAL_DEVICE, sector, buff, count))
+        return RES_ERROR;
+
     return RES_OK;
 #endif
 #if HAL_USBH_USE_MSD
-  case FATFSDEV_MSDLUN0:
+  case FATFSDEV_MSD:
 	/* It is initialized externally, just reads the status.*/
 	if (blkGetDriverState(&MSBLKD[0]) != BLK_READY)
 		return RES_NOTRDY;
+
+    // invalidate cache on buffer
+    cacheBufferFlush(buff, count * MSBLKD[0].info.blk_size);
+
 	if (usbhmsdLUNWrite(&MSBLKD[0], sector, buff, count))
 		return RES_ERROR;
 	return RES_OK;
@@ -194,6 +242,7 @@ DRESULT disk_write (
   }
   return RES_PARERR;
 }
+#endif /* _FS_READONLY */
 
 
 
@@ -207,6 +256,7 @@ DRESULT disk_ioctl (
 )
 {
   (void)buff;
+
   switch (pdrv) {
 #if HAL_USE_MMC_SPI
   case FATFSDEV_MMC:
@@ -220,19 +270,19 @@ DRESULT disk_ioctl (
 #endif
 #if FF_USE_TRIM
     case CTRL_TRIM:
-        mmcErase(&MMCD1, *((DWORD *)buff), *((DWORD *)buff + 1));
+        mmcErase(&FATFS_HAL_DEVICE, *((DWORD *)buff), *((DWORD *)buff + 1));
         return RES_OK;
 #endif
     default:
         return RES_PARERR;
     }
 #elif HAL_USE_SDC
-  case FATFSDEV_SDC:
+  case FATFSDEV_MMC:
     switch (cmd) {
     case CTRL_SYNC:
         return RES_OK;
     case GET_SECTOR_COUNT:
-        *((DWORD *)buff) = mmcsdGetCardCapacity(&SDCD1);
+        *((DWORD *)buff) = mmcsdGetCardCapacity(&FATFS_HAL_DEVICE);
         return RES_OK;
 #if FF_MAX_SS > FF_MIN_SS
     case GET_SECTOR_SIZE:
@@ -244,7 +294,7 @@ DRESULT disk_ioctl (
         return RES_OK;
 #if FF_USE_TRIM
     case CTRL_TRIM:
-        sdcErase(&SDCD1, *((DWORD *)buff), *((DWORD *)buff + 1));
+        sdcErase(&FATFS_HAL_DEVICE, *((DWORD *)buff), *((DWORD *)buff + 1));
         return RES_OK;
 #endif
     default:
@@ -252,7 +302,7 @@ DRESULT disk_ioctl (
     }
 #endif
 #if HAL_USBH_USE_MSD
-    case FATFSDEV_MSDLUN0:
+    case FATFSDEV_MSD:
       switch (cmd) {
       case CTRL_SYNC:
           return RES_OK;
diff --git a/os/various/median.c b/os/various/median.c
new file mode 100644
index 0000000..6e2f99f
--- /dev/null
+++ b/os/various/median.c
@@ -0,0 +1,129 @@
+/*
+    ChibiOS-Contrib - Copyright (C) 2014...2019 Fabien Poussin
+
+    Licensed under the Apache License, Version 2.0 (the "License");
+    you may not use this file except in compliance with the License.
+    You may obtain a copy of the License at
+
+        http://www.apache.org/licenses/LICENSE-2.0
+
+    Unless required by applicable law or agreed to in writing, software
+    distributed under the License is distributed on an "AS IS" BASIS,
+    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+    See the License for the specific language governing permissions and
+    limitations under the License.
+*/
+
+#include "median.h"
+
+void median_init(median_t* conf, uint16_t stopper, pair_t* buffer, uint16_t size)
+{
+  conf->stopper = stopper;
+  conf->buffer = buffer;
+  conf->size = size;
+  pair_t small_tmp = {NULL, conf->stopper};
+  pair_t big_tmp = {&conf->small, 0};
+  conf->datpoint = conf->buffer;                  /* Pointer into circular buffer of data */
+  conf->small = small_tmp;                        /* Chain stopper */
+  conf->big = big_tmp;                            /* Pointer to head (largest) of linked list.*/
+}
+
+uint16_t median_filter(median_t* conf, uint16_t datum)
+{
+  pair_t *successor;                              /* Pointer to successor of replaced data item */
+  pair_t *scan;                                   /* Pointer used to scan down the sorted list */
+  pair_t *scanold;                                /* Previous value of scan */
+  pair_t *median;                                 /* Pointer to median */
+  uint16_t i;
+
+  if (datum == conf->stopper)
+  {
+    datum = conf->stopper + 1;                    /* No stoppers allowed. */
+  }
+
+  if ( (++conf->datpoint - conf->buffer) >= conf->size)
+  {
+    conf->datpoint = conf->buffer;               /* Increment and wrap data in pointer.*/
+  }
+
+  conf->datpoint->value = datum;                 /* Copy in new datum */
+  successor = conf->datpoint->point;             /* Save pointer to old value's successor */
+  median = &conf->big;                           /* Median initially to first in chain */
+  scanold = NULL;                                /* Scanold initially null. */
+  scan = &conf->big;                             /* Points to pointer to first (largest) datum in chain */
+
+  /* Handle chain-out of first item in chain as special case */
+  if (scan->point == conf->datpoint)
+  {
+    scan->point = successor;
+  }
+  scanold = scan;                                     /* Save this pointer and   */
+  scan = scan->point ;                                /* step down chain */
+
+  /* Loop through the chain, normal loop exit via break. */
+  for (i = 0 ; i < conf->size; ++i)
+  {
+    /* Handle odd-numbered item in chain  */
+    if (scan->point == conf->datpoint)
+    {
+      scan->point = successor;                      /* Chain out the old datum.*/
+    }
+
+    if (scan->value < datum)                        /* If datum is larger than scanned value,*/
+    {
+      conf->datpoint->point = scanold->point;             /* Chain it in here.  */
+      scanold->point = conf->datpoint;                    /* Mark it chained in. */
+      datum = conf->stopper;
+    };
+
+    /* Step median pointer down chain after doing odd-numbered element */
+    median = median->point;                       /* Step median pointer.  */
+    if (scan == &conf->small)
+    {
+        break;                                      /* Break at end of chain  */
+    }
+    scanold = scan;                               /* Save this pointer and   */
+    scan = scan->point;                           /* step down chain */
+
+    /* Handle even-numbered item in chain.  */
+    if (scan->point == conf->datpoint)
+    {
+      scan->point = successor;
+    }
+
+    if (scan->value < datum)
+    {
+      conf->datpoint->point = scanold->point;
+      scanold->point = conf->datpoint;
+      datum = conf->stopper;
+    }
+
+    if (scan == &conf->small)
+    {
+      break;
+    }
+
+    scanold = scan;
+    scan = scan->point;
+  }
+  return median->value;
+}
+
+uint16_t middle_of_3(uint16_t a, uint16_t b, uint16_t c)
+{
+  uint16_t middle;
+
+  if ((a <= b) && (a <= c))
+  {
+    middle = (b <= c) ? b : c;
+  }
+  else if ((b <= a) && (b <= c))
+  {
+    middle = (a <= c) ? a : c;
+  }
+  else
+  {
+    middle = (a <= b) ? a : b;
+  }
+  return middle;
+}
diff --git a/os/various/median.h b/os/various/median.h
new file mode 100644
index 0000000..c74cb66
--- /dev/null
+++ b/os/various/median.h
@@ -0,0 +1,42 @@
+/*
+    ChibiOS-Contrib - Copyright (C) 2014...2019 Fabien Poussin
+
+    Licensed under the Apache License, Version 2.0 (the "License");
+    you may not use this file except in compliance with the License.
+    You may obtain a copy of the License at
+
+        http://www.apache.org/licenses/LICENSE-2.0
+
+    Unless required by applicable law or agreed to in writing, software
+    distributed under the License is distributed on an "AS IS" BASIS,
+    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+    See the License for the specific language governing permissions and
+    limitations under the License.
+*/
+
+#ifndef MEDIAN_H_
+#define MEDIAN_H_
+
+#include "ch.h"
+
+typedef struct pair_
+{
+  struct pair_* point; /* Pointers forming list linked in sorted order */
+  uint16_t value;      /* Values to sort */
+} pair_t;
+
+typedef struct
+{
+  uint16_t stopper;    /* Smaller than any datum */
+  uint16_t size;       /* 3 or more */
+  pair_t* buffer;      /* Buffer of nwidth pairs */
+  pair_t* datpoint;    /* Pointer into circular buffer of data */
+  pair_t small;        /* Chain stopper */
+  pair_t big;          /* Pointer to head (largest) of linked list.*/
+} median_t;
+
+void median_init(median_t* conf, uint16_t stopper, pair_t* buffer, uint16_t size);
+uint16_t median_filter(median_t* conf, uint16_t datum);
+uint16_t middle_of_3(uint16_t a, uint16_t b, uint16_t c);
+
+#endif /* MEDIAN_H_ */
diff --git a/os/various/ramdisk.c b/os/various/ramdisk.c
index 23bf658..d0d366a 100644
--- a/os/various/ramdisk.c
+++ b/os/various/ramdisk.c
@@ -139,6 +139,7 @@ static bool get_info(void *instance, BlockDeviceInfo *bdip) {
  *
  */
 static const struct BaseBlockDeviceVMT vmt = {
+    (size_t)0,
     is_inserted,
     is_protected,
     connect,