diff --git a/firmware/.cproject b/firmware/.cproject
index d883781b08..532d89df05 100644
--- a/firmware/.cproject
+++ b/firmware/.cproject
@@ -165,7 +165,7 @@
-
+
@@ -330,7 +330,7 @@
-
+
diff --git a/firmware/chibios/os/hal/platforms/STM32/SPIv2/spi_lld.c b/firmware/chibios/os/hal/platforms/STM32/SPIv2/spi_lld.c
new file mode 100644
index 0000000000..300ea68ce6
--- /dev/null
+++ b/firmware/chibios/os/hal/platforms/STM32/SPIv2/spi_lld.c
@@ -0,0 +1,502 @@
+/*
+ ChibiOS/RT - Copyright (C) 2006-2013 Giovanni Di Sirio
+
+ 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.
+*/
+
+/**
+ * @file STM32/SPIv2/spi_lld.c
+ * @brief STM32 SPI subsystem low level driver source.
+ *
+ * @addtogroup SPI
+ * @{
+ */
+
+#include "ch.h"
+#include "hal.h"
+
+#if HAL_USE_SPI || defined(__DOXYGEN__)
+
+/*===========================================================================*/
+/* Driver local definitions. */
+/*===========================================================================*/
+
+#define SPI1_RX_DMA_CHANNEL \
+ STM32_DMA_GETCHANNEL(STM32_SPI_SPI1_RX_DMA_STREAM, \
+ STM32_SPI1_RX_DMA_CHN)
+
+#define SPI1_TX_DMA_CHANNEL \
+ STM32_DMA_GETCHANNEL(STM32_SPI_SPI1_TX_DMA_STREAM, \
+ STM32_SPI1_TX_DMA_CHN)
+
+#define SPI2_RX_DMA_CHANNEL \
+ STM32_DMA_GETCHANNEL(STM32_SPI_SPI2_RX_DMA_STREAM, \
+ STM32_SPI2_RX_DMA_CHN)
+
+#define SPI2_TX_DMA_CHANNEL \
+ STM32_DMA_GETCHANNEL(STM32_SPI_SPI2_TX_DMA_STREAM, \
+ STM32_SPI2_TX_DMA_CHN)
+
+#define SPI3_RX_DMA_CHANNEL \
+ STM32_DMA_GETCHANNEL(STM32_SPI_SPI3_RX_DMA_STREAM, \
+ STM32_SPI3_RX_DMA_CHN)
+
+#define SPI3_TX_DMA_CHANNEL \
+ STM32_DMA_GETCHANNEL(STM32_SPI_SPI3_TX_DMA_STREAM, \
+ STM32_SPI3_TX_DMA_CHN)
+
+/*===========================================================================*/
+/* Driver exported variables. */
+/*===========================================================================*/
+
+/** @brief SPI1 driver identifier.*/
+#if STM32_SPI_USE_SPI1 || defined(__DOXYGEN__)
+SPIDriver SPID1;
+#endif
+
+/** @brief SPI2 driver identifier.*/
+#if STM32_SPI_USE_SPI2 || defined(__DOXYGEN__)
+SPIDriver SPID2;
+#endif
+
+/** @brief SPI3 driver identifier.*/
+#if STM32_SPI_USE_SPI3 || defined(__DOXYGEN__)
+SPIDriver SPID3;
+#endif
+
+/*===========================================================================*/
+/* Driver local variables and types. */
+/*===========================================================================*/
+
+static uint16_t dummytx;
+static uint16_t dummyrx;
+
+/*===========================================================================*/
+/* Driver local functions. */
+/*===========================================================================*/
+
+/**
+ * @brief Shared end-of-rx service routine.
+ *
+ * @param[in] spip pointer to the @p SPIDriver object
+ * @param[in] flags pre-shifted content of the ISR register
+ */
+static void spi_lld_serve_rx_interrupt(SPIDriver *spip, uint32_t flags) {
+
+ /* DMA errors handling.*/
+#if defined(STM32_SPI_DMA_ERROR_HOOK)
+ if ((flags & (STM32_DMA_ISR_TEIF | STM32_DMA_ISR_DMEIF)) != 0) {
+ STM32_SPI_DMA_ERROR_HOOK(spip);
+ }
+#else
+ (void)flags;
+#endif
+
+ /* Stop everything.*/
+ dmaStreamDisable(spip->dmatx);
+ dmaStreamDisable(spip->dmarx);
+
+ /* Portable SPI ISR code defined in the high level driver, note, it is
+ a macro.*/
+ _spi_isr_code(spip);
+}
+
+/**
+ * @brief Shared end-of-tx service routine.
+ *
+ * @param[in] spip pointer to the @p SPIDriver object
+ * @param[in] flags pre-shifted content of the ISR register
+ */
+static void spi_lld_serve_tx_interrupt(SPIDriver *spip, uint32_t flags) {
+
+ /* DMA errors handling.*/
+#if defined(STM32_SPI_DMA_ERROR_HOOK)
+ (void)spip;
+ if ((flags & (STM32_DMA_ISR_TEIF | STM32_DMA_ISR_DMEIF)) != 0) {
+ STM32_SPI_DMA_ERROR_HOOK(spip);
+ }
+#else
+ (void)spip;
+ (void)flags;
+#endif
+}
+
+/*===========================================================================*/
+/* Driver interrupt handlers. */
+/*===========================================================================*/
+
+/*===========================================================================*/
+/* Driver exported functions. */
+/*===========================================================================*/
+
+/**
+ * @brief Low level SPI driver initialization.
+ *
+ * @notapi
+ */
+void spi_lld_init(void) {
+
+ dummytx = 0xFFFF;
+
+#if STM32_SPI_USE_SPI1
+ spiObjectInit(&SPID1);
+ SPID1.spi = SPI1;
+ SPID1.dmarx = STM32_DMA_STREAM(STM32_SPI_SPI1_RX_DMA_STREAM);
+ SPID1.dmatx = STM32_DMA_STREAM(STM32_SPI_SPI1_TX_DMA_STREAM);
+ SPID1.rxdmamode = STM32_DMA_CR_CHSEL(SPI1_RX_DMA_CHANNEL) |
+ STM32_DMA_CR_PL(STM32_SPI_SPI1_DMA_PRIORITY) |
+ STM32_DMA_CR_DIR_P2M |
+ STM32_DMA_CR_TCIE |
+ STM32_DMA_CR_DMEIE |
+ STM32_DMA_CR_TEIE;
+ SPID1.txdmamode = STM32_DMA_CR_CHSEL(SPI1_TX_DMA_CHANNEL) |
+ STM32_DMA_CR_PL(STM32_SPI_SPI1_DMA_PRIORITY) |
+ STM32_DMA_CR_DIR_M2P |
+ STM32_DMA_CR_DMEIE |
+ STM32_DMA_CR_TEIE;
+#endif
+
+#if STM32_SPI_USE_SPI2
+ spiObjectInit(&SPID2);
+ SPID2.spi = SPI2;
+ SPID2.dmarx = STM32_DMA_STREAM(STM32_SPI_SPI2_RX_DMA_STREAM);
+ SPID2.dmatx = STM32_DMA_STREAM(STM32_SPI_SPI2_TX_DMA_STREAM);
+ SPID2.rxdmamode = STM32_DMA_CR_CHSEL(SPI2_RX_DMA_CHANNEL) |
+ STM32_DMA_CR_PL(STM32_SPI_SPI2_DMA_PRIORITY) |
+ STM32_DMA_CR_DIR_P2M |
+ STM32_DMA_CR_TCIE |
+ STM32_DMA_CR_DMEIE |
+ STM32_DMA_CR_TEIE;
+ SPID2.txdmamode = STM32_DMA_CR_CHSEL(SPI2_TX_DMA_CHANNEL) |
+ STM32_DMA_CR_PL(STM32_SPI_SPI2_DMA_PRIORITY) |
+ STM32_DMA_CR_DIR_M2P |
+ STM32_DMA_CR_DMEIE |
+ STM32_DMA_CR_TEIE;
+#endif
+
+#if STM32_SPI_USE_SPI3
+ spiObjectInit(&SPID3);
+ SPID3.spi = SPI3;
+ SPID3.dmarx = STM32_DMA_STREAM(STM32_SPI_SPI3_RX_DMA_STREAM);
+ SPID3.dmatx = STM32_DMA_STREAM(STM32_SPI_SPI3_TX_DMA_STREAM);
+ SPID3.rxdmamode = STM32_DMA_CR_CHSEL(SPI3_RX_DMA_CHANNEL) |
+ STM32_DMA_CR_PL(STM32_SPI_SPI3_DMA_PRIORITY) |
+ STM32_DMA_CR_DIR_P2M |
+ STM32_DMA_CR_TCIE |
+ STM32_DMA_CR_DMEIE |
+ STM32_DMA_CR_TEIE;
+ SPID3.txdmamode = STM32_DMA_CR_CHSEL(SPI3_TX_DMA_CHANNEL) |
+ STM32_DMA_CR_PL(STM32_SPI_SPI3_DMA_PRIORITY) |
+ STM32_DMA_CR_DIR_M2P |
+ STM32_DMA_CR_DMEIE |
+ STM32_DMA_CR_TEIE;
+#endif
+}
+
+/**
+ * @brief Configures and activates the SPI peripheral.
+ *
+ * @param[in] spip pointer to the @p SPIDriver object
+ *
+ * @notapi
+ */
+void spi_lld_start(SPIDriver *spip) {
+ uint32_t ds;
+
+ /* If in stopped state then enables the SPI and DMA clocks.*/
+ if (spip->state == SPI_STOP) {
+#if STM32_SPI_USE_SPI1
+ if (&SPID1 == spip) {
+ bool_t b;
+ b = dmaStreamAllocate(spip->dmarx,
+ STM32_SPI_SPI1_IRQ_PRIORITY,
+ (stm32_dmaisr_t)spi_lld_serve_rx_interrupt,
+ (void *)spip);
+ chDbgAssert(!b, "spi_lld_start(), #1", "stream already allocated");
+ b = dmaStreamAllocate(spip->dmatx,
+ STM32_SPI_SPI1_IRQ_PRIORITY,
+ (stm32_dmaisr_t)spi_lld_serve_tx_interrupt,
+ (void *)spip);
+ chDbgAssert(!b, "spi_lld_start(), #2", "stream already allocated");
+ rccEnableSPI1(FALSE);
+ }
+#endif
+#if STM32_SPI_USE_SPI2
+ if (&SPID2 == spip) {
+ bool_t b;
+ b = dmaStreamAllocate(spip->dmarx,
+ STM32_SPI_SPI2_IRQ_PRIORITY,
+ (stm32_dmaisr_t)spi_lld_serve_rx_interrupt,
+ (void *)spip);
+ chDbgAssert(!b, "spi_lld_start(), #3", "stream already allocated");
+ b = dmaStreamAllocate(spip->dmatx,
+ STM32_SPI_SPI2_IRQ_PRIORITY,
+ (stm32_dmaisr_t)spi_lld_serve_tx_interrupt,
+ (void *)spip);
+ chDbgAssert(!b, "spi_lld_start(), #4", "stream already allocated");
+ rccEnableSPI2(FALSE);
+ }
+#endif
+#if STM32_SPI_USE_SPI3
+ if (&SPID3 == spip) {
+ bool_t b;
+ b = dmaStreamAllocate(spip->dmarx,
+ STM32_SPI_SPI3_IRQ_PRIORITY,
+ (stm32_dmaisr_t)spi_lld_serve_rx_interrupt,
+ (void *)spip);
+ chDbgAssert(!b, "spi_lld_start(), #5", "stream already allocated");
+ b = dmaStreamAllocate(spip->dmatx,
+ STM32_SPI_SPI3_IRQ_PRIORITY,
+ (stm32_dmaisr_t)spi_lld_serve_tx_interrupt,
+ (void *)spip);
+ chDbgAssert(!b, "spi_lld_start(), #6", "stream already allocated");
+ rccEnableSPI3(FALSE);
+ }
+#endif
+
+ /* DMA setup.*/
+ dmaStreamSetPeripheral(spip->dmarx, &spip->spi->DR);
+ dmaStreamSetPeripheral(spip->dmatx, &spip->spi->DR);
+ }
+
+ /* Configuration-specific DMA setup.*/
+ ds = spip->config->cr2 & SPI_CR2_DS;
+ if (!ds || (ds <= (SPI_CR2_DS_2 | SPI_CR2_DS_1 | SPI_CR2_DS_0))) {
+ /* Frame width is 8 bits or smaller.*/
+ spip->rxdmamode = (spip->rxdmamode & ~STM32_DMA_CR_SIZE_MASK) |
+ STM32_DMA_CR_PSIZE_BYTE | STM32_DMA_CR_MSIZE_BYTE;
+ spip->txdmamode = (spip->txdmamode & ~STM32_DMA_CR_SIZE_MASK) |
+ STM32_DMA_CR_PSIZE_BYTE | STM32_DMA_CR_MSIZE_BYTE;
+ }
+ else {
+ /* Frame width is larger than 8 bits.*/
+ spip->rxdmamode = (spip->rxdmamode & ~STM32_DMA_CR_SIZE_MASK) |
+ STM32_DMA_CR_PSIZE_HWORD | STM32_DMA_CR_MSIZE_HWORD;
+ spip->txdmamode = (spip->txdmamode & ~STM32_DMA_CR_SIZE_MASK) |
+ STM32_DMA_CR_PSIZE_HWORD | STM32_DMA_CR_MSIZE_HWORD;
+ }
+ /* SPI setup and enable.*/
+ spip->spi->CR1 = 0;
+ spip->spi->CR1 = spip->config->cr1 | SPI_CR1_MSTR | SPI_CR1_SSM |
+ SPI_CR1_SSI;
+ spip->spi->CR2 = spip->config->cr2 | SPI_CR2_FRXTH | SPI_CR2_SSOE |
+ SPI_CR2_RXDMAEN | SPI_CR2_TXDMAEN;
+ spip->spi->CR1 |= SPI_CR1_SPE;
+}
+
+/**
+ * @brief Deactivates the SPI peripheral.
+ *
+ * @param[in] spip pointer to the @p SPIDriver object
+ *
+ * @notapi
+ */
+void spi_lld_stop(SPIDriver *spip) {
+
+ /* If in ready state then disables the SPI clock.*/
+ if (spip->state == SPI_READY) {
+
+ /* SPI disable.*/
+ spip->spi->CR1 = 0;
+ spip->spi->CR2 = 0;
+ dmaStreamRelease(spip->dmarx);
+ dmaStreamRelease(spip->dmatx);
+
+#if STM32_SPI_USE_SPI1
+ if (&SPID1 == spip)
+ rccDisableSPI1(FALSE);
+#endif
+#if STM32_SPI_USE_SPI2
+ if (&SPID2 == spip)
+ rccDisableSPI2(FALSE);
+#endif
+#if STM32_SPI_USE_SPI3
+ if (&SPID3 == spip)
+ rccDisableSPI3(FALSE);
+#endif
+ }
+}
+
+/**
+ * @brief Asserts the slave select signal and prepares for transfers.
+ *
+ * @param[in] spip pointer to the @p SPIDriver object
+ *
+ * @notapi
+ */
+void spi_lld_select(SPIDriver *spip) {
+
+ palClearPad(spip->config->ssport, spip->config->sspad);
+}
+
+/**
+ * @brief Deasserts the slave select signal.
+ * @details The previously selected peripheral is unselected.
+ *
+ * @param[in] spip pointer to the @p SPIDriver object
+ *
+ * @notapi
+ */
+void spi_lld_unselect(SPIDriver *spip) {
+
+ palSetPad(spip->config->ssport, spip->config->sspad);
+}
+
+/**
+ * @brief Ignores data on the SPI bus.
+ * @details This asynchronous function starts the transmission of a series of
+ * idle words on the SPI bus and ignores the received data.
+ * @post At the end of the operation the configured callback is invoked.
+ *
+ * @param[in] spip pointer to the @p SPIDriver object
+ * @param[in] n number of words to be ignored
+ *
+ * @notapi
+ */
+void spi_lld_ignore(SPIDriver *spip, size_t n) {
+
+ dmaStreamSetMemory0(spip->dmarx, &dummyrx);
+ dmaStreamSetTransactionSize(spip->dmarx, n);
+ dmaStreamSetMode(spip->dmarx, spip->rxdmamode);
+
+ dmaStreamSetMemory0(spip->dmatx, &dummytx);
+ dmaStreamSetTransactionSize(spip->dmatx, n);
+ dmaStreamSetMode(spip->dmatx, spip->txdmamode);
+
+ dmaStreamEnable(spip->dmarx);
+ dmaStreamEnable(spip->dmatx);
+}
+
+/**
+ * @brief Exchanges data on the SPI bus.
+ * @details This asynchronous function starts a simultaneous transmit/receive
+ * operation.
+ * @post At the end of the operation the configured callback is invoked.
+ * @note The buffers are organized as uint8_t arrays for data sizes below or
+ * equal to 8 bits else it is organized as uint16_t arrays.
+ *
+ * @param[in] spip pointer to the @p SPIDriver object
+ * @param[in] n number of words to be exchanged
+ * @param[in] txbuf the pointer to the transmit buffer
+ * @param[out] rxbuf the pointer to the receive buffer
+ *
+ * @notapi
+ */
+void spi_lld_exchange(SPIDriver *spip, size_t n,
+ const void *txbuf, void *rxbuf) {
+
+ dmaStreamSetMemory0(spip->dmarx, rxbuf);
+ dmaStreamSetTransactionSize(spip->dmarx, n);
+ dmaStreamSetMode(spip->dmarx, spip->rxdmamode| STM32_DMA_CR_MINC);
+
+ dmaStreamSetMemory0(spip->dmatx, txbuf);
+ dmaStreamSetTransactionSize(spip->dmatx, n);
+ dmaStreamSetMode(spip->dmatx, spip->txdmamode | STM32_DMA_CR_MINC);
+
+ dmaStreamEnable(spip->dmarx);
+ dmaStreamEnable(spip->dmatx);
+}
+
+/**
+ * @brief Sends data over the SPI bus.
+ * @details This asynchronous function starts a transmit operation.
+ * @post At the end of the operation the configured callback is invoked.
+ * @note The buffers are organized as uint8_t arrays for data sizes below or
+ * equal to 8 bits else it is organized as uint16_t arrays.
+ *
+ * @param[in] spip pointer to the @p SPIDriver object
+ * @param[in] n number of words to send
+ * @param[in] txbuf the pointer to the transmit buffer
+ *
+ * @notapi
+ */
+void spi_lld_send(SPIDriver *spip, size_t n, const void *txbuf) {
+
+ dmaStreamSetMemory0(spip->dmarx, &dummyrx);
+ dmaStreamSetTransactionSize(spip->dmarx, n);
+ dmaStreamSetMode(spip->dmarx, spip->rxdmamode);
+
+ dmaStreamSetMemory0(spip->dmatx, txbuf);
+ dmaStreamSetTransactionSize(spip->dmatx, n);
+ dmaStreamSetMode(spip->dmatx, spip->txdmamode | STM32_DMA_CR_MINC);
+
+ dmaStreamEnable(spip->dmarx);
+ dmaStreamEnable(spip->dmatx);
+}
+
+/**
+ * @brief Receives data from the SPI bus.
+ * @details This asynchronous function starts a receive operation.
+ * @post At the end of the operation the configured callback is invoked.
+ * @note The buffers are organized as uint8_t arrays for data sizes below or
+ * equal to 8 bits else it is organized as uint16_t arrays.
+ *
+ * @param[in] spip pointer to the @p SPIDriver object
+ * @param[in] n number of words to receive
+ * @param[out] rxbuf the pointer to the receive buffer
+ *
+ * @notapi
+ */
+void spi_lld_receive(SPIDriver *spip, size_t n, void *rxbuf) {
+
+ dmaStreamSetMemory0(spip->dmarx, rxbuf);
+ dmaStreamSetTransactionSize(spip->dmarx, n);
+ dmaStreamSetMode(spip->dmarx, spip->rxdmamode | STM32_DMA_CR_MINC);
+
+ dmaStreamSetMemory0(spip->dmatx, &dummytx);
+ dmaStreamSetTransactionSize(spip->dmatx, n);
+ dmaStreamSetMode(spip->dmatx, spip->txdmamode);
+
+ dmaStreamEnable(spip->dmarx);
+ dmaStreamEnable(spip->dmatx);
+}
+
+/**
+ * @brief Exchanges one frame using a polled wait.
+ * @details This synchronous function exchanges one frame using a polled
+ * synchronization method. This function is useful when exchanging
+ * small amount of data on high speed channels, usually in this
+ * situation is much more efficient just wait for completion using
+ * polling than suspending the thread waiting for an interrupt.
+ *
+ * @param[in] spip pointer to the @p SPIDriver object
+ * @param[in] frame the data frame to send over the SPI bus
+ * @return The received data frame from the SPI bus.
+ */
+uint16_t spi_lld_polled_exchange(SPIDriver *spip, uint16_t frame) {
+
+ /*
+ * Data register must be accessed with the appropriate data size.
+ * Byte size access (uint8_t *) for transactions that are <= 8-bit.
+ * Halfword size access (uint16_t) for transactions that are <= 8-bit.
+ */
+ if ((spip->config->cr2 & SPI_CR2_DS) <= (SPI_CR2_DS_2 |
+ SPI_CR2_DS_1 |
+ SPI_CR2_DS_0)) {
+ volatile uint8_t *spidr = (volatile uint8_t *)&spip->spi->DR;
+ *spidr = (uint8_t)frame;
+ while ((spip->spi->SR & SPI_SR_RXNE) == 0)
+ ;
+ return (uint16_t)*spidr;
+ }
+ else {
+ spip->spi->DR = frame;
+ while ((spip->spi->SR & SPI_SR_RXNE) == 0)
+ ;
+ return spip->spi->DR;
+ }
+}
+
+#endif /* HAL_USE_SPI */
+
+/** @} */
diff --git a/firmware/chibios/os/hal/platforms/STM32/SPIv2/spi_lld.h b/firmware/chibios/os/hal/platforms/STM32/SPIv2/spi_lld.h
new file mode 100644
index 0000000000..9dfe2f2424
--- /dev/null
+++ b/firmware/chibios/os/hal/platforms/STM32/SPIv2/spi_lld.h
@@ -0,0 +1,424 @@
+/*
+ ChibiOS/RT - Copyright (C) 2006-2013 Giovanni Di Sirio
+
+ 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.
+*/
+
+/**
+ * @file STM32/SPIv2/spi_lld.h
+ * @brief STM32 SPI subsystem low level driver header.
+ *
+ * @addtogroup SPI
+ * @{
+ */
+
+#ifndef _SPI_LLD_H_
+#define _SPI_LLD_H_
+
+#if HAL_USE_SPI || defined(__DOXYGEN__)
+
+/*===========================================================================*/
+/* Driver constants. */
+/*===========================================================================*/
+
+/*===========================================================================*/
+/* Driver pre-compile time settings. */
+/*===========================================================================*/
+
+/**
+ * @name Configuration options
+ * @{
+ */
+/**
+ * @brief SPI1 driver enable switch.
+ * @details If set to @p TRUE the support for SPI1 is included.
+ * @note The default is @p FALSE.
+ */
+#if !defined(STM32_SPI_USE_SPI1) || defined(__DOXYGEN__)
+#define STM32_SPI_USE_SPI1 FALSE
+#endif
+
+/**
+ * @brief SPI2 driver enable switch.
+ * @details If set to @p TRUE the support for SPI2 is included.
+ * @note The default is @p FALSE.
+ */
+#if !defined(STM32_SPI_USE_SPI2) || defined(__DOXYGEN__)
+#define STM32_SPI_USE_SPI2 FALSE
+#endif
+
+/**
+ * @brief SPI3 driver enable switch.
+ * @details If set to @p TRUE the support for SPI3 is included.
+ * @note The default is @p FALSE.
+ */
+#if !defined(STM32_SPI_USE_SPI3) || defined(__DOXYGEN__)
+#define STM32_SPI_USE_SPI3 FALSE
+#endif
+
+/**
+ * @brief SPI1 interrupt priority level setting.
+ */
+#if !defined(STM32_SPI_SPI1_IRQ_PRIORITY) || defined(__DOXYGEN__)
+#define STM32_SPI_SPI1_IRQ_PRIORITY 10
+#endif
+
+/**
+ * @brief SPI2 interrupt priority level setting.
+ */
+#if !defined(STM32_SPI_SPI2_IRQ_PRIORITY) || defined(__DOXYGEN__)
+#define STM32_SPI_SPI2_IRQ_PRIORITY 10
+#endif
+
+/**
+ * @brief SPI3 interrupt priority level setting.
+ */
+#if !defined(STM32_SPI_SPI3_IRQ_PRIORITY) || defined(__DOXYGEN__)
+#define STM32_SPI_SPI3_IRQ_PRIORITY 10
+#endif
+
+/**
+ * @brief SPI1 DMA priority (0..3|lowest..highest).
+ * @note The priority level is used for both the TX and RX DMA streams but
+ * because of the streams ordering the RX stream has always priority
+ * over the TX stream.
+ */
+#if !defined(STM32_SPI_SPI1_DMA_PRIORITY) || defined(__DOXYGEN__)
+#define STM32_SPI_SPI1_DMA_PRIORITY 1
+#endif
+
+/**
+ * @brief SPI2 DMA priority (0..3|lowest..highest).
+ * @note The priority level is used for both the TX and RX DMA streams but
+ * because of the streams ordering the RX stream has always priority
+ * over the TX stream.
+ */
+#if !defined(STM32_SPI_SPI2_DMA_PRIORITY) || defined(__DOXYGEN__)
+#define STM32_SPI_SPI2_DMA_PRIORITY 1
+#endif
+
+/**
+ * @brief SPI3 DMA priority (0..3|lowest..highest).
+ * @note The priority level is used for both the TX and RX DMA streams but
+ * because of the streams ordering the RX stream has always priority
+ * over the TX stream.
+ */
+#if !defined(STM32_SPI_SPI3_DMA_PRIORITY) || defined(__DOXYGEN__)
+#define STM32_SPI_SPI3_DMA_PRIORITY 1
+#endif
+
+/**
+ * @brief SPI DMA error hook.
+ */
+#if !defined(STM32_SPI_DMA_ERROR_HOOK) || defined(__DOXYGEN__)
+#define STM32_SPI_DMA_ERROR_HOOK(spip) chSysHalt()
+#endif
+
+#if STM32_ADVANCED_DMA || defined(__DOXYGEN__)
+
+/**
+ * @brief DMA stream used for SPI1 RX operations.
+ * @note This option is only available on platforms with enhanced DMA.
+ */
+#if !defined(STM32_SPI_SPI1_RX_DMA_STREAM) || defined(__DOXYGEN__)
+#define STM32_SPI_SPI1_RX_DMA_STREAM STM32_DMA_STREAM_ID(2, 0)
+#endif
+
+/**
+ * @brief DMA stream used for SPI1 TX operations.
+ * @note This option is only available on platforms with enhanced DMA.
+ */
+#if !defined(STM32_SPI_SPI1_TX_DMA_STREAM) || defined(__DOXYGEN__)
+#define STM32_SPI_SPI1_TX_DMA_STREAM STM32_DMA_STREAM_ID(2, 3)
+#endif
+
+/**
+ * @brief DMA stream used for SPI2 RX operations.
+ * @note This option is only available on platforms with enhanced DMA.
+ */
+#if !defined(STM32_SPI_SPI2_RX_DMA_STREAM) || defined(__DOXYGEN__)
+#define STM32_SPI_SPI2_RX_DMA_STREAM STM32_DMA_STREAM_ID(1, 3)
+#endif
+
+/**
+ * @brief DMA stream used for SPI2 TX operations.
+ * @note This option is only available on platforms with enhanced DMA.
+ */
+#if !defined(STM32_SPI_SPI2_TX_DMA_STREAM) || defined(__DOXYGEN__)
+#define STM32_SPI_SPI2_TX_DMA_STREAM STM32_DMA_STREAM_ID(1, 4)
+#endif
+
+/**
+ * @brief DMA stream used for SPI3 RX operations.
+ * @note This option is only available on platforms with enhanced DMA.
+ */
+#if !defined(STM32_SPI_SPI3_RX_DMA_STREAM) || defined(__DOXYGEN__)
+#define STM32_SPI_SPI3_RX_DMA_STREAM STM32_DMA_STREAM_ID(1, 0)
+#endif
+
+/**
+ * @brief DMA stream used for SPI3 TX operations.
+ * @note This option is only available on platforms with enhanced DMA.
+ */
+#if !defined(STM32_SPI_SPI3_TX_DMA_STREAM) || defined(__DOXYGEN__)
+#define STM32_SPI_SPI3_TX_DMA_STREAM STM32_DMA_STREAM_ID(1, 7)
+#endif
+
+#else /* !STM32_ADVANCED_DMA */
+
+#if defined(STM32F0XX)
+/* Fixed values for STM32F0xx devices.*/
+#define STM32_SPI_SPI1_RX_DMA_STREAM STM32_DMA_STREAM_ID(1, 2)
+#define STM32_SPI_SPI1_TX_DMA_STREAM STM32_DMA_STREAM_ID(1, 3)
+#define STM32_SPI_SPI2_RX_DMA_STREAM STM32_DMA_STREAM_ID(1, 4)
+#define STM32_SPI_SPI2_TX_DMA_STREAM STM32_DMA_STREAM_ID(1, 5)
+#endif /* defined(STM32F0XX) */
+
+#if defined(STM32F30X) || defined(STM32F37X)
+/* Fixed values for STM32F3xx devices.*/
+#define STM32_SPI_SPI1_RX_DMA_STREAM STM32_DMA_STREAM_ID(1, 2)
+#define STM32_SPI_SPI1_TX_DMA_STREAM STM32_DMA_STREAM_ID(1, 3)
+#define STM32_SPI_SPI2_RX_DMA_STREAM STM32_DMA_STREAM_ID(1, 4)
+#define STM32_SPI_SPI2_TX_DMA_STREAM STM32_DMA_STREAM_ID(1, 5)
+#define STM32_SPI_SPI3_RX_DMA_STREAM STM32_DMA_STREAM_ID(2, 1)
+#define STM32_SPI_SPI3_TX_DMA_STREAM STM32_DMA_STREAM_ID(2, 2)
+#endif /* defined(STM32F30X) */
+
+#endif /* !STM32_ADVANCED_DMA*/
+/** @} */
+
+/*===========================================================================*/
+/* Derived constants and error checks. */
+/*===========================================================================*/
+
+#if STM32_SPI_USE_SPI1 && !STM32_HAS_SPI1
+#error "SPI1 not present in the selected device"
+#endif
+
+#if STM32_SPI_USE_SPI2 && !STM32_HAS_SPI2
+#error "SPI2 not present in the selected device"
+#endif
+
+#if STM32_SPI_USE_SPI3 && !STM32_HAS_SPI3
+#error "SPI3 not present in the selected device"
+#endif
+
+#if !STM32_SPI_USE_SPI1 && !STM32_SPI_USE_SPI2 && !STM32_SPI_USE_SPI3
+#error "SPI driver activated but no SPI peripheral assigned"
+#endif
+
+#if STM32_SPI_USE_SPI1 && \
+ !CORTEX_IS_VALID_KERNEL_PRIORITY(STM32_SPI_SPI1_IRQ_PRIORITY)
+#error "Invalid IRQ priority assigned to SPI1"
+#endif
+
+#if STM32_SPI_USE_SPI2 && \
+ !CORTEX_IS_VALID_KERNEL_PRIORITY(STM32_SPI_SPI2_IRQ_PRIORITY)
+#error "Invalid IRQ priority assigned to SPI2"
+#endif
+
+#if STM32_SPI_USE_SPI3 && \
+ !CORTEX_IS_VALID_KERNEL_PRIORITY(STM32_SPI_SPI3_IRQ_PRIORITY)
+#error "Invalid IRQ priority assigned to SPI3"
+#endif
+
+#if STM32_SPI_USE_SPI1 && \
+ !STM32_DMA_IS_VALID_PRIORITY(STM32_SPI_SPI1_DMA_PRIORITY)
+#error "Invalid DMA priority assigned to SPI1"
+#endif
+
+#if STM32_SPI_USE_SPI2 && \
+ !STM32_DMA_IS_VALID_PRIORITY(STM32_SPI_SPI2_DMA_PRIORITY)
+#error "Invalid DMA priority assigned to SPI2"
+#endif
+
+#if STM32_SPI_USE_SPI3 && \
+ !STM32_DMA_IS_VALID_PRIORITY(STM32_SPI_SPI3_DMA_PRIORITY)
+#error "Invalid DMA priority assigned to SPI3"
+#endif
+
+#if STM32_SPI_USE_SPI1 && \
+ !STM32_DMA_IS_VALID_ID(STM32_SPI_SPI1_RX_DMA_STREAM, STM32_SPI1_RX_DMA_MSK)
+#error "invalid DMA stream associated to SPI1 RX"
+#endif
+
+#if STM32_SPI_USE_SPI1 && \
+ !STM32_DMA_IS_VALID_ID(STM32_SPI_SPI1_TX_DMA_STREAM, STM32_SPI1_TX_DMA_MSK)
+#error "invalid DMA stream associated to SPI1 TX"
+#endif
+
+#if STM32_SPI_USE_SPI2 && \
+ !STM32_DMA_IS_VALID_ID(STM32_SPI_SPI2_RX_DMA_STREAM, STM32_SPI2_RX_DMA_MSK)
+#error "invalid DMA stream associated to SPI2 RX"
+#endif
+
+#if STM32_SPI_USE_SPI2 && \
+ !STM32_DMA_IS_VALID_ID(STM32_SPI_SPI2_TX_DMA_STREAM, STM32_SPI2_TX_DMA_MSK)
+#error "invalid DMA stream associated to SPI2 TX"
+#endif
+
+#if STM32_SPI_USE_SPI3 && \
+ !STM32_DMA_IS_VALID_ID(STM32_SPI_SPI3_RX_DMA_STREAM, STM32_SPI3_RX_DMA_MSK)
+#error "invalid DMA stream associated to SPI3 RX"
+#endif
+
+#if STM32_SPI_USE_SPI3 && \
+ !STM32_DMA_IS_VALID_ID(STM32_SPI_SPI3_TX_DMA_STREAM, STM32_SPI3_TX_DMA_MSK)
+#error "invalid DMA stream associated to SPI3 TX"
+#endif
+
+#if !defined(STM32_DMA_REQUIRED)
+#define STM32_DMA_REQUIRED
+#endif
+
+/*===========================================================================*/
+/* Driver data structures and types. */
+/*===========================================================================*/
+
+/**
+ * @brief Type of a structure representing an SPI driver.
+ */
+typedef struct SPIDriver SPIDriver;
+
+/**
+ * @brief SPI notification callback type.
+ *
+ * @param[in] spip pointer to the @p SPIDriver object triggering the
+ * callback
+ */
+typedef void (*spicallback_t)(SPIDriver *spip);
+
+/**
+ * @brief Driver configuration structure.
+ */
+typedef struct {
+ /**
+ * @brief Operation complete callback or @p NULL.
+ */
+ spicallback_t end_cb;
+ /* End of the mandatory fields.*/
+ /**
+ * @brief The chip select line port.
+ */
+ ioportid_t ssport;
+ /**
+ * @brief The chip select line pad number.
+ */
+ uint16_t sspad;
+ /**
+ * @brief SPI CR1 register initialization data.
+ */
+ uint16_t cr1;
+ /**
+ * @brief SPI CR2 register initialization data.
+ */
+ uint16_t cr2;
+} SPIConfig;
+
+/**
+ * @brief Structure representing a SPI driver.
+ */
+struct SPIDriver {
+ /**
+ * @brief Driver state.
+ */
+ spistate_t state;
+ /**
+ * @brief Current configuration data.
+ */
+ const SPIConfig *config;
+#if SPI_USE_WAIT || defined(__DOXYGEN__)
+ /**
+ * @brief Waiting thread.
+ */
+ Thread *thread;
+#endif /* SPI_USE_WAIT */
+#if SPI_USE_MUTUAL_EXCLUSION || defined(__DOXYGEN__)
+#if CH_USE_MUTEXES || defined(__DOXYGEN__)
+ /**
+ * @brief Mutex protecting the bus.
+ */
+ Mutex mutex;
+#elif CH_USE_SEMAPHORES
+ Semaphore semaphore;
+#endif
+#endif /* SPI_USE_MUTUAL_EXCLUSION */
+#if defined(SPI_DRIVER_EXT_FIELDS)
+ SPI_DRIVER_EXT_FIELDS
+#endif
+ /* End of the mandatory fields.*/
+ /**
+ * @brief Pointer to the SPIx registers block.
+ */
+ SPI_TypeDef *spi;
+ /**
+ * @brief Receive DMA stream.
+ */
+ const stm32_dma_stream_t *dmarx;
+ /**
+ * @brief Transmit DMA stream.
+ */
+ const stm32_dma_stream_t *dmatx;
+ /**
+ * @brief RX DMA mode bit mask.
+ */
+ uint32_t rxdmamode;
+ /**
+ * @brief TX DMA mode bit mask.
+ */
+ uint32_t txdmamode;
+};
+
+/*===========================================================================*/
+/* Driver macros. */
+/*===========================================================================*/
+
+/*===========================================================================*/
+/* External declarations. */
+/*===========================================================================*/
+
+#if STM32_SPI_USE_SPI1 && !defined(__DOXYGEN__)
+extern SPIDriver SPID1;
+#endif
+
+#if STM32_SPI_USE_SPI2 && !defined(__DOXYGEN__)
+extern SPIDriver SPID2;
+#endif
+
+#if STM32_SPI_USE_SPI3 && !defined(__DOXYGEN__)
+extern SPIDriver SPID3;
+#endif
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+ void spi_lld_init(void);
+ void spi_lld_start(SPIDriver *spip);
+ void spi_lld_stop(SPIDriver *spip);
+ void spi_lld_select(SPIDriver *spip);
+ void spi_lld_unselect(SPIDriver *spip);
+ void spi_lld_ignore(SPIDriver *spip, size_t n);
+ void spi_lld_exchange(SPIDriver *spip, size_t n,
+ const void *txbuf, void *rxbuf);
+ void spi_lld_send(SPIDriver *spip, size_t n, const void *txbuf);
+ void spi_lld_receive(SPIDriver *spip, size_t n, void *rxbuf);
+ uint16_t spi_lld_polled_exchange(SPIDriver *spip, uint16_t frame);
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* HAL_USE_SPI */
+
+#endif /* _SPI_LLD_H_ */
+
+/** @} */
diff --git a/firmware/chibios/os/hal/platforms/STM32/USARTv2/serial_lld.c b/firmware/chibios/os/hal/platforms/STM32/USARTv2/serial_lld.c
new file mode 100644
index 0000000000..c19ac260e1
--- /dev/null
+++ b/firmware/chibios/os/hal/platforms/STM32/USARTv2/serial_lld.c
@@ -0,0 +1,534 @@
+/*
+ ChibiOS/RT - Copyright (C) 2006-2013 Giovanni Di Sirio
+
+ 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.
+*/
+
+/**
+ * @file STM32/USARTv2/serial_lld.c
+ * @brief STM32 low level serial driver code.
+ *
+ * @addtogroup SERIAL
+ * @{
+ */
+
+#include "ch.h"
+#include "hal.h"
+
+#if HAL_USE_SERIAL || defined(__DOXYGEN__)
+
+/*===========================================================================*/
+/* Driver local definitions. */
+/*===========================================================================*/
+
+/*===========================================================================*/
+/* Driver exported variables. */
+/*===========================================================================*/
+
+/** @brief USART1 serial driver identifier.*/
+#if STM32_SERIAL_USE_USART1 || defined(__DOXYGEN__)
+SerialDriver SD1;
+#endif
+
+/** @brief USART2 serial driver identifier.*/
+#if STM32_SERIAL_USE_USART2 || defined(__DOXYGEN__)
+SerialDriver SD2;
+#endif
+
+/** @brief USART3 serial driver identifier.*/
+#if STM32_SERIAL_USE_USART3 || defined(__DOXYGEN__)
+SerialDriver SD3;
+#endif
+
+/** @brief UART4 serial driver identifier.*/
+#if STM32_SERIAL_USE_UART4 || defined(__DOXYGEN__)
+SerialDriver SD4;
+#endif
+
+/** @brief UART5 serial driver identifier.*/
+#if STM32_SERIAL_USE_UART5 || defined(__DOXYGEN__)
+SerialDriver SD5;
+#endif
+
+/** @brief USART6 serial driver identifier.*/
+#if STM32_SERIAL_USE_USART6 || defined(__DOXYGEN__)
+SerialDriver SD6;
+#endif
+
+/*===========================================================================*/
+/* Driver local variables and types. */
+/*===========================================================================*/
+
+/** @brief Driver default configuration.*/
+static const SerialConfig default_config =
+{
+ SERIAL_DEFAULT_BITRATE,
+ 0,
+ USART_CR2_STOP1_BITS | USART_CR2_LINEN,
+ 0
+};
+
+/*===========================================================================*/
+/* Driver local functions. */
+/*===========================================================================*/
+
+/**
+ * @brief USART initialization.
+ * @details This function must be invoked with interrupts disabled.
+ *
+ * @param[in] sdp pointer to a @p SerialDriver object
+ * @param[in] config the architecture-dependent serial driver configuration
+ */
+static void usart_init(SerialDriver *sdp, const SerialConfig *config) {
+ USART_TypeDef *u = sdp->usart;
+
+ /* Baud rate setting.*/
+ u->BRR = (uint16_t)(sdp->clock / config->speed);
+
+ /* Note that some bits are enforced.*/
+ u->CR2 = config->cr2 | USART_CR2_LBDIE;
+ u->CR3 = config->cr3 | USART_CR3_EIE;
+ u->CR1 = config->cr1 | USART_CR1_UE | USART_CR1_PEIE |
+ USART_CR1_RXNEIE | USART_CR1_TE |
+ USART_CR1_RE;
+ u->ICR = 0xFFFFFFFF;
+}
+
+/**
+ * @brief USART de-initialization.
+ * @details This function must be invoked with interrupts disabled.
+ *
+ * @param[in] u pointer to an USART I/O block
+ */
+static void usart_deinit(USART_TypeDef *u) {
+
+ u->CR1 = 0;
+ u->CR2 = 0;
+ u->CR3 = 0;
+}
+
+/**
+ * @brief Error handling routine.
+ *
+ * @param[in] sdp pointer to a @p SerialDriver object
+ * @param[in] isr USART ISR register value
+ */
+static void set_error(SerialDriver *sdp, uint32_t isr) {
+ flagsmask_t sts = 0;
+
+ if (isr & USART_ISR_ORE)
+ sts |= SD_OVERRUN_ERROR;
+ if (isr & USART_ISR_PE)
+ sts |= SD_PARITY_ERROR;
+ if (isr & USART_ISR_FE)
+ sts |= SD_FRAMING_ERROR;
+ if (isr & USART_ISR_NE)
+ sts |= SD_NOISE_ERROR;
+ chSysLockFromIsr();
+ chnAddFlagsI(sdp, sts);
+ chSysUnlockFromIsr();
+}
+
+/**
+ * @brief Common IRQ handler.
+ *
+ * @param[in] sdp communication channel associated to the USART
+ */
+static void serve_interrupt(SerialDriver *sdp) {
+ USART_TypeDef *u = sdp->usart;
+ uint32_t cr1 = u->CR1;
+ uint32_t isr;
+
+ /* Reading and clearing status.*/
+ isr = u->ISR;
+ u->ICR = isr;
+
+ /* Error condition detection.*/
+ if (isr & (USART_ISR_ORE | USART_ISR_NE | USART_ISR_FE | USART_ISR_PE))
+ set_error(sdp, isr);
+
+ /* Special case, LIN break detection.*/
+ if (isr & USART_ISR_LBD) {
+ chSysLockFromIsr();
+ chnAddFlagsI(sdp, SD_BREAK_DETECTED);
+ chSysUnlockFromIsr();
+ }
+
+ /* Data available.*/
+ if (isr & USART_ISR_RXNE) {
+ chSysLockFromIsr();
+ sdIncomingDataI(sdp, (uint8_t)u->RDR);
+ chSysUnlockFromIsr();
+ }
+
+ /* Transmission buffer empty.*/
+ if ((cr1 & USART_CR1_TXEIE) && (isr & USART_ISR_TXE)) {
+ msg_t b;
+ chSysLockFromIsr();
+ b = chOQGetI(&sdp->oqueue);
+ if (b < Q_OK) {
+ chnAddFlagsI(sdp, CHN_OUTPUT_EMPTY);
+ u->CR1 = (cr1 & ~USART_CR1_TXEIE) | USART_CR1_TCIE;
+ }
+ else
+ u->TDR = b;
+ chSysUnlockFromIsr();
+ }
+
+ /* Physical transmission end.*/
+ if (isr & USART_ISR_TC) {
+ chSysLockFromIsr();
+ if (chOQIsEmptyI(&sdp->oqueue))
+ chnAddFlagsI(sdp, CHN_TRANSMISSION_END);
+ u->CR1 = cr1 & ~USART_CR1_TCIE;
+ chSysUnlockFromIsr();
+ }
+}
+
+#if STM32_SERIAL_USE_USART1 || defined(__DOXYGEN__)
+static void notify1(GenericQueue *qp) {
+
+ (void)qp;
+ USART1->CR1 |= USART_CR1_TXEIE;
+}
+#endif
+
+#if STM32_SERIAL_USE_USART2 || defined(__DOXYGEN__)
+static void notify2(GenericQueue *qp) {
+
+ (void)qp;
+ USART2->CR1 |= USART_CR1_TXEIE;
+}
+#endif
+
+#if STM32_SERIAL_USE_USART3 || defined(__DOXYGEN__)
+static void notify3(GenericQueue *qp) {
+
+ (void)qp;
+ USART3->CR1 |= USART_CR1_TXEIE;
+}
+#endif
+
+#if STM32_SERIAL_USE_UART4 || defined(__DOXYGEN__)
+static void notify4(GenericQueue *qp) {
+
+ (void)qp;
+ UART4->CR1 |= USART_CR1_TXEIE;
+}
+#endif
+
+#if STM32_SERIAL_USE_UART5 || defined(__DOXYGEN__)
+static void notify5(GenericQueue *qp) {
+
+ (void)qp;
+ UART5->CR1 |= USART_CR1_TXEIE;
+}
+#endif
+
+#if STM32_SERIAL_USE_USART6 || defined(__DOXYGEN__)
+static void notify6(GenericQueue *qp) {
+
+ (void)qp;
+ USART6->CR1 |= USART_CR1_TXEIE;
+}
+#endif
+
+/*===========================================================================*/
+/* Driver interrupt handlers. */
+/*===========================================================================*/
+
+#if STM32_SERIAL_USE_USART1 || defined(__DOXYGEN__)
+#if !defined(STM32_USART1_HANDLER)
+#error "STM32_USART1_HANDLER not defined"
+#endif
+/**
+ * @brief USART1 interrupt handler.
+ *
+ * @isr
+ */
+CH_IRQ_HANDLER(STM32_USART1_HANDLER) {
+
+ CH_IRQ_PROLOGUE();
+
+ serve_interrupt(&SD1);
+
+ CH_IRQ_EPILOGUE();
+}
+#endif
+
+#if STM32_SERIAL_USE_USART2 || defined(__DOXYGEN__)
+#if !defined(STM32_USART2_HANDLER)
+#error "STM32_USART2_HANDLER not defined"
+#endif
+/**
+ * @brief USART2 interrupt handler.
+ *
+ * @isr
+ */
+CH_IRQ_HANDLER(STM32_USART2_HANDLER) {
+
+ CH_IRQ_PROLOGUE();
+
+ serve_interrupt(&SD2);
+
+ CH_IRQ_EPILOGUE();
+}
+#endif
+
+#if STM32_SERIAL_USE_USART3 || defined(__DOXYGEN__)
+#if !defined(STM32_USART3_HANDLER)
+#error "STM32_USART3_HANDLER not defined"
+#endif
+/**
+ * @brief USART3 interrupt handler.
+ *
+ * @isr
+ */
+CH_IRQ_HANDLER(STM32_USART3_HANDLER) {
+
+ CH_IRQ_PROLOGUE();
+
+ serve_interrupt(&SD3);
+
+ CH_IRQ_EPILOGUE();
+}
+#endif
+
+#if STM32_SERIAL_USE_UART4 || defined(__DOXYGEN__)
+#if !defined(STM32_UART4_HANDLER)
+#error "STM32_UART4_HANDLER not defined"
+#endif
+/**
+ * @brief UART4 interrupt handler.
+ *
+ * @isr
+ */
+CH_IRQ_HANDLER(STM32_UART4_HANDLER) {
+
+ CH_IRQ_PROLOGUE();
+
+ serve_interrupt(&SD4);
+
+ CH_IRQ_EPILOGUE();
+}
+#endif
+
+#if STM32_SERIAL_USE_UART5 || defined(__DOXYGEN__)
+#if !defined(STM32_UART5_HANDLER)
+#error "STM32_UART5_HANDLER not defined"
+#endif
+/**
+ * @brief UART5 interrupt handler.
+ *
+ * @isr
+ */
+CH_IRQ_HANDLER(STM32_UART5_HANDLER) {
+
+ CH_IRQ_PROLOGUE();
+
+ serve_interrupt(&SD5);
+
+ CH_IRQ_EPILOGUE();
+}
+#endif
+
+#if STM32_SERIAL_USE_USART6 || defined(__DOXYGEN__)
+#if !defined(STM32_USART6_HANDLER)
+#error "STM32_USART6_HANDLER not defined"
+#endif
+/**
+ * @brief USART1 interrupt handler.
+ *
+ * @isr
+ */
+CH_IRQ_HANDLER(STM32_USART6_HANDLER) {
+
+ CH_IRQ_PROLOGUE();
+
+ serve_interrupt(&SD6);
+
+ CH_IRQ_EPILOGUE();
+}
+#endif
+
+/*===========================================================================*/
+/* Driver exported functions. */
+/*===========================================================================*/
+
+/**
+ * @brief Low level serial driver initialization.
+ *
+ * @notapi
+ */
+void sd_lld_init(void) {
+
+#if STM32_SERIAL_USE_USART1
+ sdObjectInit(&SD1, NULL, notify1);
+ SD1.usart = USART1;
+ SD1.clock = STM32_USART1CLK;
+#endif
+
+#if STM32_SERIAL_USE_USART2
+ sdObjectInit(&SD2, NULL, notify2);
+ SD2.usart = USART2;
+ SD2.clock = STM32_USART2CLK;
+#endif
+
+#if STM32_SERIAL_USE_USART3
+ sdObjectInit(&SD3, NULL, notify3);
+ SD3.usart = USART3;
+ SD3.clock = STM32_USART3CLK;
+#endif
+
+#if STM32_SERIAL_USE_UART4
+ sdObjectInit(&SD4, NULL, notify4);
+ SD4.usart = UART4;
+ SD4.clock = STM32_UART4CLK;
+#endif
+
+#if STM32_SERIAL_USE_UART5
+ sdObjectInit(&SD5, NULL, notify5);
+ SD5.usart = UART5;
+ SD5.clock = STM32_UART5CLK;
+#endif
+
+#if STM32_SERIAL_USE_USART6
+ sdObjectInit(&SD6, NULL, notify6);
+ SD6.usart = USART6;
+ SD6.clock = STM32_USART6CLK;
+#endif
+}
+
+/**
+ * @brief Low level serial driver configuration and (re)start.
+ *
+ * @param[in] sdp pointer to a @p SerialDriver object
+ * @param[in] config the architecture-dependent serial driver configuration.
+ * If this parameter is set to @p NULL then a default
+ * configuration is used.
+ *
+ * @notapi
+ */
+void sd_lld_start(SerialDriver *sdp, const SerialConfig *config) {
+
+ if (config == NULL)
+ config = &default_config;
+
+ if (sdp->state == SD_STOP) {
+#if STM32_SERIAL_USE_USART1
+ if (&SD1 == sdp) {
+ rccEnableUSART1(FALSE);
+ nvicEnableVector(STM32_USART1_NUMBER,
+ CORTEX_PRIORITY_MASK(STM32_SERIAL_USART1_PRIORITY));
+ }
+#endif
+#if STM32_SERIAL_USE_USART2
+ if (&SD2 == sdp) {
+ rccEnableUSART2(FALSE);
+ nvicEnableVector(STM32_USART2_NUMBER,
+ CORTEX_PRIORITY_MASK(STM32_SERIAL_USART2_PRIORITY));
+ }
+#endif
+#if STM32_SERIAL_USE_USART3
+ if (&SD3 == sdp) {
+ rccEnableUSART3(FALSE);
+ nvicEnableVector(STM32_USART3_NUMBER,
+ CORTEX_PRIORITY_MASK(STM32_SERIAL_USART3_PRIORITY));
+ }
+#endif
+#if STM32_SERIAL_USE_UART4
+ if (&SD4 == sdp) {
+ rccEnableUART4(FALSE);
+ nvicEnableVector(STM32_UART4_NUMBER,
+ CORTEX_PRIORITY_MASK(STM32_SERIAL_UART4_PRIORITY));
+ }
+#endif
+#if STM32_SERIAL_USE_UART5
+ if (&SD5 == sdp) {
+ rccEnableUART5(FALSE);
+ nvicEnableVector(STM32_UART5_NUMBER,
+ CORTEX_PRIORITY_MASK(STM32_SERIAL_UART5_PRIORITY));
+ }
+#endif
+#if STM32_SERIAL_USE_USART6
+ if (&SD6 == sdp) {
+ rccEnableUSART6(FALSE);
+ nvicEnableVector(STM32_USART6_NUMBER,
+ CORTEX_PRIORITY_MASK(STM32_SERIAL_USART6_PRIORITY));
+ }
+#endif
+ }
+ usart_init(sdp, config);
+}
+
+/**
+ * @brief Low level serial driver stop.
+ * @details De-initializes the USART, stops the associated clock, resets the
+ * interrupt vector.
+ *
+ * @param[in] sdp pointer to a @p SerialDriver object
+ *
+ * @notapi
+ */
+void sd_lld_stop(SerialDriver *sdp) {
+
+ if (sdp->state == SD_READY) {
+ usart_deinit(sdp->usart);
+#if STM32_SERIAL_USE_USART1
+ if (&SD1 == sdp) {
+ rccDisableUSART1(FALSE);
+ nvicDisableVector(STM32_USART1_NUMBER);
+ return;
+ }
+#endif
+#if STM32_SERIAL_USE_USART2
+ if (&SD2 == sdp) {
+ rccDisableUSART2(FALSE);
+ nvicDisableVector(STM32_USART2_NUMBER);
+ return;
+ }
+#endif
+#if STM32_SERIAL_USE_USART3
+ if (&SD3 == sdp) {
+ rccDisableUSART3(FALSE);
+ nvicDisableVector(STM32_USART3_NUMBER);
+ return;
+ }
+#endif
+#if STM32_SERIAL_USE_UART4
+ if (&SD4 == sdp) {
+ rccDisableUART4(FALSE);
+ nvicDisableVector(STM32_UART4_NUMBER);
+ return;
+ }
+#endif
+#if STM32_SERIAL_USE_UART5
+ if (&SD5 == sdp) {
+ rccDisableUART5(FALSE);
+ nvicDisableVector(STM32_UART5_NUMBER);
+ return;
+ }
+#endif
+#if STM32_SERIAL_USE_USART6
+ if (&SD6 == sdp) {
+ rccDisableUSART6(FALSE);
+ nvicDisableVector(STM32_USART6_NUMBER);
+ return;
+ }
+#endif
+ }
+}
+
+#endif /* HAL_USE_SERIAL */
+
+/** @} */
diff --git a/firmware/chibios/os/hal/platforms/STM32/USARTv2/serial_lld.h b/firmware/chibios/os/hal/platforms/STM32/USARTv2/serial_lld.h
new file mode 100644
index 0000000000..003e200b61
--- /dev/null
+++ b/firmware/chibios/os/hal/platforms/STM32/USARTv2/serial_lld.h
@@ -0,0 +1,305 @@
+/*
+ ChibiOS/RT - Copyright (C) 2006-2013 Giovanni Di Sirio
+
+ 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.
+*/
+
+/**
+ * @file STM32/USARTv2/serial_lld.h
+ * @brief STM32 low level serial driver header.
+ *
+ * @addtogroup SERIAL
+ * @{
+ */
+
+#ifndef _SERIAL_LLD_H_
+#define _SERIAL_LLD_H_
+
+#if HAL_USE_SERIAL || defined(__DOXYGEN__)
+
+/*===========================================================================*/
+/* Driver constants. */
+/*===========================================================================*/
+
+/*===========================================================================*/
+/* Driver pre-compile time settings. */
+/*===========================================================================*/
+
+/**
+ * @name Configuration options
+ * @{
+ */
+/**
+ * @brief USART1 driver enable switch.
+ * @details If set to @p TRUE the support for USART1 is included.
+ * @note The default is @p TRUE.
+ */
+#if !defined(STM32_SERIAL_USE_USART1) || defined(__DOXYGEN__)
+#define STM32_SERIAL_USE_USART1 FALSE
+#endif
+
+/**
+ * @brief USART2 driver enable switch.
+ * @details If set to @p TRUE the support for USART2 is included.
+ * @note The default is @p TRUE.
+ */
+#if !defined(STM32_SERIAL_USE_USART2) || defined(__DOXYGEN__)
+#define STM32_SERIAL_USE_USART2 FALSE
+#endif
+
+/**
+ * @brief USART3 driver enable switch.
+ * @details If set to @p TRUE the support for USART3 is included.
+ * @note The default is @p TRUE.
+ */
+#if !defined(STM32_SERIAL_USE_USART3) || defined(__DOXYGEN__)
+#define STM32_SERIAL_USE_USART3 FALSE
+#endif
+
+/**
+ * @brief UART4 driver enable switch.
+ * @details If set to @p TRUE the support for UART4 is included.
+ * @note The default is @p TRUE.
+ */
+#if !defined(STM32_SERIAL_USE_UART4) || defined(__DOXYGEN__)
+#define STM32_SERIAL_USE_UART4 FALSE
+#endif
+
+/**
+ * @brief UART5 driver enable switch.
+ * @details If set to @p TRUE the support for UART5 is included.
+ * @note The default is @p TRUE.
+ */
+#if !defined(STM32_SERIAL_USE_UART5) || defined(__DOXYGEN__)
+#define STM32_SERIAL_USE_UART5 FALSE
+#endif
+
+/**
+ * @brief USART6 driver enable switch.
+ * @details If set to @p TRUE the support for USART6 is included.
+ * @note The default is @p TRUE.
+ */
+#if !defined(STM32_SERIAL_USE_USART6) || defined(__DOXYGEN__)
+#define STM32_SERIAL_USE_USART6 FALSE
+#endif
+
+/**
+ * @brief USART1 interrupt priority level setting.
+ */
+#if !defined(STM32_SERIAL_USART1_PRIORITY) || defined(__DOXYGEN__)
+#define STM32_SERIAL_USART1_PRIORITY 12
+#endif
+
+/**
+ * @brief USART2 interrupt priority level setting.
+ */
+#if !defined(STM32_SERIAL_USART2_PRIORITY) || defined(__DOXYGEN__)
+#define STM32_SERIAL_USART2_PRIORITY 12
+#endif
+
+/**
+ * @brief USART3 interrupt priority level setting.
+ */
+#if !defined(STM32_SERIAL_USART3_PRIORITY) || defined(__DOXYGEN__)
+#define STM32_SERIAL_USART3_PRIORITY 12
+#endif
+
+/**
+ * @brief UART4 interrupt priority level setting.
+ */
+#if !defined(STM32_SERIAL_UART4_PRIORITY) || defined(__DOXYGEN__)
+#define STM32_SERIAL_UART4_PRIORITY 12
+#endif
+
+/**
+ * @brief UART5 interrupt priority level setting.
+ */
+#if !defined(STM32_SERIAL_UART5_PRIORITY) || defined(__DOXYGEN__)
+#define STM32_SERIAL_UART5_PRIORITY 12
+#endif
+
+/**
+ * @brief USART6 interrupt priority level setting.
+ */
+#if !defined(STM32_SERIAL_USART6_PRIORITY) || defined(__DOXYGEN__)
+#define STM32_SERIAL_USART6_PRIORITY 12
+#endif
+/** @} */
+
+/*===========================================================================*/
+/* Derived constants and error checks. */
+/*===========================================================================*/
+
+#if STM32_SERIAL_USE_USART1 && !STM32_HAS_USART1
+#error "USART1 not present in the selected device"
+#endif
+
+#if STM32_SERIAL_USE_USART2 && !STM32_HAS_USART2
+#error "USART2 not present in the selected device"
+#endif
+
+#if STM32_SERIAL_USE_USART3 && !STM32_HAS_USART3
+#error "USART3 not present in the selected device"
+#endif
+
+#if STM32_SERIAL_USE_UART4 && !STM32_HAS_UART4
+#error "UART4 not present in the selected device"
+#endif
+
+#if STM32_SERIAL_USE_UART5 && !STM32_HAS_UART5
+#error "UART5 not present in the selected device"
+#endif
+
+#if STM32_SERIAL_USE_USART6 && !STM32_HAS_USART6
+#error "USART6 not present in the selected device"
+#endif
+
+#if !STM32_SERIAL_USE_USART1 && !STM32_SERIAL_USE_USART2 && \
+ !STM32_SERIAL_USE_USART3 && !STM32_SERIAL_USE_UART4 && \
+ !STM32_SERIAL_USE_UART5 && !STM32_SERIAL_USE_USART6
+#error "SERIAL driver activated but no USART/UART peripheral assigned"
+#endif
+
+#if STM32_SERIAL_USE_USART1 && \
+ !CORTEX_IS_VALID_KERNEL_PRIORITY(STM32_SERIAL_USART1_PRIORITY)
+#error "Invalid IRQ priority assigned to USART1"
+#endif
+
+#if STM32_SERIAL_USE_USART2 && \
+ !CORTEX_IS_VALID_KERNEL_PRIORITY(STM32_SERIAL_USART2_PRIORITY)
+#error "Invalid IRQ priority assigned to USART2"
+#endif
+
+#if STM32_SERIAL_USE_USART3 && \
+ !CORTEX_IS_VALID_KERNEL_PRIORITY(STM32_SERIAL_USART3_PRIORITY)
+#error "Invalid IRQ priority assigned to USART3"
+#endif
+
+#if STM32_SERIAL_USE_UART4 && \
+ !CORTEX_IS_VALID_KERNEL_PRIORITY(STM32_SERIAL_UART4_PRIORITY)
+#error "Invalid IRQ priority assigned to UART4"
+#endif
+
+#if STM32_SERIAL_USE_UART5 && \
+ !CORTEX_IS_VALID_KERNEL_PRIORITY(STM32_SERIAL_UART5_PRIORITY)
+#error "Invalid IRQ priority assigned to UART5"
+#endif
+
+#if STM32_SERIAL_USE_USART6 && \
+ !CORTEX_IS_VALID_KERNEL_PRIORITY(STM32_SERIAL_USART6_PRIORITY)
+#error "Invalid IRQ priority assigned to USART6"
+#endif
+
+/*===========================================================================*/
+/* Driver data structures and types. */
+/*===========================================================================*/
+
+/**
+ * @brief STM32 Serial Driver configuration structure.
+ * @details An instance of this structure must be passed to @p sdStart()
+ * in order to configure and start a serial driver operations.
+ * @note This structure content is architecture dependent, each driver
+ * implementation defines its own version and the custom static
+ * initializers.
+ */
+typedef struct {
+ /**
+ * @brief Bit rate.
+ */
+ uint32_t speed;
+ /* End of the mandatory fields.*/
+ /**
+ * @brief Initialization value for the CR1 register.
+ */
+ uint32_t cr1;
+ /**
+ * @brief Initialization value for the CR2 register.
+ */
+ uint32_t cr2;
+ /**
+ * @brief Initialization value for the CR3 register.
+ */
+ uint32_t cr3;
+} SerialConfig;
+
+/**
+ * @brief @p SerialDriver specific data.
+ */
+#define _serial_driver_data \
+ _base_asynchronous_channel_data \
+ /* Driver state.*/ \
+ sdstate_t state; \
+ /* Input queue.*/ \
+ InputQueue iqueue; \
+ /* Output queue.*/ \
+ OutputQueue oqueue; \
+ /* Input circular buffer.*/ \
+ uint8_t ib[SERIAL_BUFFERS_SIZE]; \
+ /* Output circular buffer.*/ \
+ uint8_t ob[SERIAL_BUFFERS_SIZE]; \
+ /* End of the mandatory fields.*/ \
+ /* Pointer to the USART registers block.*/ \
+ USART_TypeDef *usart; \
+ /* Clock frequency for the associated USART/UART.*/ \
+ uint32_t clock;
+
+/*===========================================================================*/
+/* Driver macros. */
+/*===========================================================================*/
+
+/*
+ * Extra USARTs definitions here (missing from the ST header file).
+ */
+#define USART_CR2_STOP1_BITS (0 << 12) /**< @brief CR2 1 stop bit value.*/
+#define USART_CR2_STOP0P5_BITS (1 << 12) /**< @brief CR2 0.5 stop bit value.*/
+#define USART_CR2_STOP2_BITS (2 << 12) /**< @brief CR2 2 stop bit value.*/
+#define USART_CR2_STOP1P5_BITS (3 << 12) /**< @brief CR2 1.5 stop bit value.*/
+
+/*===========================================================================*/
+/* External declarations. */
+/*===========================================================================*/
+
+#if STM32_SERIAL_USE_USART1 && !defined(__DOXYGEN__)
+extern SerialDriver SD1;
+#endif
+#if STM32_SERIAL_USE_USART2 && !defined(__DOXYGEN__)
+extern SerialDriver SD2;
+#endif
+#if STM32_SERIAL_USE_USART3 && !defined(__DOXYGEN__)
+extern SerialDriver SD3;
+#endif
+#if STM32_SERIAL_USE_UART4 && !defined(__DOXYGEN__)
+extern SerialDriver SD4;
+#endif
+#if STM32_SERIAL_USE_UART5 && !defined(__DOXYGEN__)
+extern SerialDriver SD5;
+#endif
+#if STM32_SERIAL_USE_USART6 && !defined(__DOXYGEN__)
+extern SerialDriver SD6;
+#endif
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+ void sd_lld_init(void);
+ void sd_lld_start(SerialDriver *sdp, const SerialConfig *config);
+ void sd_lld_stop(SerialDriver *sdp);
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* HAL_USE_SERIAL */
+
+#endif /* _SERIAL_LLD_H_ */
+
+/** @} */
diff --git a/firmware/chibios/os/hal/platforms/STM32/USARTv2/uart_lld.c b/firmware/chibios/os/hal/platforms/STM32/USARTv2/uart_lld.c
new file mode 100644
index 0000000000..37305bbee0
--- /dev/null
+++ b/firmware/chibios/os/hal/platforms/STM32/USARTv2/uart_lld.c
@@ -0,0 +1,594 @@
+/*
+ ChibiOS/RT - Copyright (C) 2006-2013 Giovanni Di Sirio
+
+ 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.
+*/
+
+/**
+ * @file STM32/USARTv2/uart_lld.c
+ * @brief STM32 low level UART driver code.
+ *
+ * @addtogroup UART
+ * @{
+ */
+
+#include "ch.h"
+#include "hal.h"
+
+#if HAL_USE_UART || defined(__DOXYGEN__)
+
+/*===========================================================================*/
+/* Driver local definitions. */
+/*===========================================================================*/
+
+#define USART1_RX_DMA_CHANNEL \
+ STM32_DMA_GETCHANNEL(STM32_UART_USART1_RX_DMA_STREAM, \
+ STM32_USART1_RX_DMA_CHN)
+
+#define USART1_TX_DMA_CHANNEL \
+ STM32_DMA_GETCHANNEL(STM32_UART_USART1_TX_DMA_STREAM, \
+ STM32_USART1_TX_DMA_CHN)
+
+#define USART2_RX_DMA_CHANNEL \
+ STM32_DMA_GETCHANNEL(STM32_UART_USART2_RX_DMA_STREAM, \
+ STM32_USART2_RX_DMA_CHN)
+
+#define USART2_TX_DMA_CHANNEL \
+ STM32_DMA_GETCHANNEL(STM32_UART_USART2_TX_DMA_STREAM, \
+ STM32_USART2_TX_DMA_CHN)
+
+#define USART3_RX_DMA_CHANNEL \
+ STM32_DMA_GETCHANNEL(STM32_UART_USART3_RX_DMA_STREAM, \
+ STM32_USART3_RX_DMA_CHN)
+
+#define USART3_TX_DMA_CHANNEL \
+ STM32_DMA_GETCHANNEL(STM32_UART_USART3_TX_DMA_STREAM, \
+ STM32_USART3_TX_DMA_CHN)
+
+/*===========================================================================*/
+/* Driver exported variables. */
+/*===========================================================================*/
+
+/** @brief USART1 UART driver identifier.*/
+#if STM32_UART_USE_USART1 || defined(__DOXYGEN__)
+UARTDriver UARTD1;
+#endif
+
+/** @brief USART2 UART driver identifier.*/
+#if STM32_UART_USE_USART2 || defined(__DOXYGEN__)
+UARTDriver UARTD2;
+#endif
+
+/** @brief USART3 UART driver identifier.*/
+#if STM32_UART_USE_USART3 || defined(__DOXYGEN__)
+UARTDriver UARTD3;
+#endif
+
+/*===========================================================================*/
+/* Driver local variables and types. */
+/*===========================================================================*/
+
+/*===========================================================================*/
+/* Driver local functions. */
+/*===========================================================================*/
+
+/**
+ * @brief Status bits translation.
+ *
+ * @param[in] sr USART SR register value
+ *
+ * @return The error flags.
+ */
+static uartflags_t translate_errors(uint32_t isr) {
+ uartflags_t sts = 0;
+
+ if (isr & USART_ISR_ORE)
+ sts |= UART_OVERRUN_ERROR;
+ if (isr & USART_ISR_PE)
+ sts |= UART_PARITY_ERROR;
+ if (isr & USART_ISR_FE)
+ sts |= UART_FRAMING_ERROR;
+ if (isr & USART_ISR_NE)
+ sts |= UART_NOISE_ERROR;
+ if (isr & USART_ISR_LBD)
+ sts |= UART_BREAK_DETECTED;
+ return sts;
+}
+
+/**
+ * @brief Puts the receiver in the UART_RX_IDLE state.
+ *
+ * @param[in] uartp pointer to the @p UARTDriver object
+ */
+static void set_rx_idle_loop(UARTDriver *uartp) {
+ uint32_t mode;
+
+ /* RX DMA channel preparation, if the char callback is defined then the
+ TCIE interrupt is enabled too.*/
+ if (uartp->config->rxchar_cb == NULL)
+ mode = STM32_DMA_CR_DIR_P2M | STM32_DMA_CR_CIRC;
+ else
+ mode = STM32_DMA_CR_DIR_P2M | STM32_DMA_CR_CIRC | STM32_DMA_CR_TCIE;
+ dmaStreamSetMemory0(uartp->dmarx, &uartp->rxbuf);
+ dmaStreamSetTransactionSize(uartp->dmarx, 1);
+ dmaStreamSetMode(uartp->dmarx, uartp->dmamode | mode);
+ dmaStreamEnable(uartp->dmarx);
+}
+
+/**
+ * @brief USART de-initialization.
+ * @details This function must be invoked with interrupts disabled.
+ *
+ * @param[in] uartp pointer to the @p UARTDriver object
+ */
+static void usart_stop(UARTDriver *uartp) {
+
+ /* Stops RX and TX DMA channels.*/
+ dmaStreamDisable(uartp->dmarx);
+ dmaStreamDisable(uartp->dmatx);
+
+ /* Stops USART operations.*/
+ uartp->usart->CR1 = 0;
+ uartp->usart->CR2 = 0;
+ uartp->usart->CR3 = 0;
+}
+
+/**
+ * @brief USART initialization.
+ * @details This function must be invoked with interrupts disabled.
+ *
+ * @param[in] uartp pointer to the @p UARTDriver object
+ */
+static void usart_start(UARTDriver *uartp) {
+ uint32_t cr1;
+ USART_TypeDef *u = uartp->usart;
+
+ /* Defensive programming, starting from a clean state.*/
+ usart_stop(uartp);
+
+ /* Baud rate setting.*/
+#if defined(STM32F0XX)
+ if (uartp->usart == USART1)
+ u->BRR = STM32_USART1CLK / uartp->config->speed;
+ else
+ u->BRR = STM32_PCLK / uartp->config->speed;
+#else /* !defined(STM32F0XX) */
+ if (uartp->usart == USART1)
+ u->BRR = STM32_PCLK2 / uartp->config->speed;
+ else
+ u->BRR = STM32_PCLK1 / uartp->config->speed;
+#endif /* !defined(STM32F0XX) */
+
+ /* Resetting eventual pending status flags.*/
+ u->ICR = 0xFFFFFFFF;
+
+ /* Note that some bits are enforced because required for correct driver
+ operations.*/
+ u->CR2 = uartp->config->cr2 | USART_CR2_LBDIE;
+ u->CR3 = uartp->config->cr3 | USART_CR3_DMAT | USART_CR3_DMAR |
+ USART_CR3_EIE;
+ if (uartp->config->txend2_cb == NULL)
+ cr1 = USART_CR1_UE | USART_CR1_PEIE | USART_CR1_TE | USART_CR1_RE;
+ else
+ cr1 = USART_CR1_UE | USART_CR1_PEIE | USART_CR1_TE | USART_CR1_RE |
+ USART_CR1_TCIE;
+ u->CR1 = uartp->config->cr1 | cr1;
+
+ /* Starting the receiver idle loop.*/
+ set_rx_idle_loop(uartp);
+}
+
+/**
+ * @brief RX DMA common service routine.
+ *
+ * @param[in] uartp pointer to the @p UARTDriver object
+ * @param[in] flags pre-shifted content of the ISR register
+ */
+static void uart_lld_serve_rx_end_irq(UARTDriver *uartp, uint32_t flags) {
+
+ /* DMA errors handling.*/
+#if defined(STM32_UART_DMA_ERROR_HOOK)
+ if ((flags & (STM32_DMA_ISR_TEIF | STM32_DMA_ISR_DMEIF)) != 0) {
+ STM32_UART_DMA_ERROR_HOOK(uartp);
+ }
+#else
+ (void)flags;
+#endif
+
+ if (uartp->rxstate == UART_RX_IDLE) {
+ /* Receiver in idle state, a callback is generated, if enabled, for each
+ received character and then the driver stays in the same state.*/
+ if (uartp->config->rxchar_cb != NULL)
+ uartp->config->rxchar_cb(uartp, uartp->rxbuf);
+ }
+ else {
+ /* Receiver in active state, a callback is generated, if enabled, after
+ a completed transfer.*/
+ dmaStreamDisable(uartp->dmarx);
+ uartp->rxstate = UART_RX_COMPLETE;
+ if (uartp->config->rxend_cb != NULL)
+ uartp->config->rxend_cb(uartp);
+
+ /* If the callback didn't explicitly change state then the receiver
+ automatically returns to the idle state.*/
+ if (uartp->rxstate == UART_RX_COMPLETE) {
+ uartp->rxstate = UART_RX_IDLE;
+ set_rx_idle_loop(uartp);
+ }
+ }
+}
+
+/**
+ * @brief TX DMA common service routine.
+ *
+ * @param[in] uartp pointer to the @p UARTDriver object
+ * @param[in] flags pre-shifted content of the ISR register
+ */
+static void uart_lld_serve_tx_end_irq(UARTDriver *uartp, uint32_t flags) {
+
+ /* DMA errors handling.*/
+#if defined(STM32_UART_DMA_ERROR_HOOK)
+ if ((flags & (STM32_DMA_ISR_TEIF | STM32_DMA_ISR_DMEIF)) != 0) {
+ STM32_UART_DMA_ERROR_HOOK(uartp);
+ }
+#else
+ (void)flags;
+#endif
+
+ dmaStreamDisable(uartp->dmatx);
+
+ /* A callback is generated, if enabled, after a completed transfer.*/
+ uartp->txstate = UART_TX_COMPLETE;
+ if (uartp->config->txend1_cb != NULL)
+ uartp->config->txend1_cb(uartp);
+
+ /* If the callback didn't explicitly change state then the transmitter
+ automatically returns to the idle state.*/
+ if (uartp->txstate == UART_TX_COMPLETE)
+ uartp->txstate = UART_TX_IDLE;
+}
+
+/**
+ * @brief USART common service routine.
+ *
+ * @param[in] uartp pointer to the @p UARTDriver object
+ */
+static void serve_usart_irq(UARTDriver *uartp) {
+ uint32_t isr;
+ USART_TypeDef *u = uartp->usart;
+
+ /* Reading and clearing status.*/
+ isr = u->ISR;
+ u->ICR = isr;
+
+ if (isr & (USART_ISR_LBD | USART_ISR_ORE | USART_ISR_NE |
+ USART_ISR_FE | USART_ISR_PE)) {
+ if (uartp->config->rxerr_cb != NULL)
+ uartp->config->rxerr_cb(uartp, translate_errors(isr));
+ }
+ if (isr & USART_ISR_TC) {
+ /* End of transmission, a callback is generated.*/
+ if (uartp->config->txend2_cb != NULL)
+ uartp->config->txend2_cb(uartp);
+ }
+}
+
+/*===========================================================================*/
+/* Driver interrupt handlers. */
+/*===========================================================================*/
+
+#if STM32_UART_USE_USART1 || defined(__DOXYGEN__)
+#if !defined(STM32_USART1_HANDLER)
+#error "STM32_USART1_HANDLER not defined"
+#endif
+/**
+ * @brief USART1 IRQ handler.
+ *
+ * @isr
+ */
+CH_IRQ_HANDLER(STM32_USART1_HANDLER) {
+
+ CH_IRQ_PROLOGUE();
+
+ serve_usart_irq(&UARTD1);
+
+ CH_IRQ_EPILOGUE();
+}
+#endif /* STM32_UART_USE_USART1 */
+
+#if STM32_UART_USE_USART2 || defined(__DOXYGEN__)
+#if !defined(STM32_USART2_HANDLER)
+#error "STM32_USART2_HANDLER not defined"
+#endif
+/**
+ * @brief USART2 IRQ handler.
+ *
+ * @isr
+ */
+CH_IRQ_HANDLER(STM32_USART2_HANDLER) {
+
+ CH_IRQ_PROLOGUE();
+
+ serve_usart_irq(&UARTD2);
+
+ CH_IRQ_EPILOGUE();
+}
+#endif /* STM32_UART_USE_USART2 */
+
+#if STM32_UART_USE_USART3 || defined(__DOXYGEN__)
+#if !defined(STM32_USART3_HANDLER)
+#error "STM32_USART3_HANDLER not defined"
+#endif
+/**
+ * @brief USART3 IRQ handler.
+ *
+ * @isr
+ */
+CH_IRQ_HANDLER(STM32_USART3_HANDLER) {
+
+ CH_IRQ_PROLOGUE();
+
+ serve_usart_irq(&UARTD3);
+
+ CH_IRQ_EPILOGUE();
+}
+#endif /* STM32_UART_USE_USART3 */
+
+/*===========================================================================*/
+/* Driver exported functions. */
+/*===========================================================================*/
+
+/**
+ * @brief Low level UART driver initialization.
+ *
+ * @notapi
+ */
+void uart_lld_init(void) {
+
+#if STM32_UART_USE_USART1
+ uartObjectInit(&UARTD1);
+ UARTD1.usart = USART1;
+ UARTD1.dmamode = STM32_DMA_CR_DMEIE | STM32_DMA_CR_TEIE;
+ UARTD1.dmarx = STM32_DMA_STREAM(STM32_UART_USART1_RX_DMA_STREAM);
+ UARTD1.dmatx = STM32_DMA_STREAM(STM32_UART_USART1_TX_DMA_STREAM);
+#endif
+
+#if STM32_UART_USE_USART2
+ uartObjectInit(&UARTD2);
+ UARTD2.usart = USART2;
+ UARTD2.dmamode = STM32_DMA_CR_DMEIE | STM32_DMA_CR_TEIE;
+ UARTD2.dmarx = STM32_DMA_STREAM(STM32_UART_USART2_RX_DMA_STREAM);
+ UARTD2.dmatx = STM32_DMA_STREAM(STM32_UART_USART2_TX_DMA_STREAM);
+#endif
+
+#if STM32_UART_USE_USART3
+ uartObjectInit(&UARTD3);
+ UARTD3.usart = USART3;
+ UARTD3.dmamode = STM32_DMA_CR_DMEIE | STM32_DMA_CR_TEIE;
+ UARTD3.dmarx = STM32_DMA_STREAM(STM32_UART_USART3_RX_DMA_STREAM);
+ UARTD3.dmatx = STM32_DMA_STREAM(STM32_UART_USART3_TX_DMA_STREAM);
+#endif
+}
+
+/**
+ * @brief Configures and activates the UART peripheral.
+ *
+ * @param[in] uartp pointer to the @p UARTDriver object
+ *
+ * @notapi
+ */
+void uart_lld_start(UARTDriver *uartp) {
+
+ if (uartp->state == UART_STOP) {
+#if STM32_UART_USE_USART1
+ if (&UARTD1 == uartp) {
+ bool_t b;
+ b = dmaStreamAllocate(uartp->dmarx,
+ STM32_UART_USART1_IRQ_PRIORITY,
+ (stm32_dmaisr_t)uart_lld_serve_rx_end_irq,
+ (void *)uartp);
+ chDbgAssert(!b, "uart_lld_start(), #1", "stream already allocated");
+ b = dmaStreamAllocate(uartp->dmatx,
+ STM32_UART_USART1_IRQ_PRIORITY,
+ (stm32_dmaisr_t)uart_lld_serve_tx_end_irq,
+ (void *)uartp);
+ chDbgAssert(!b, "uart_lld_start(), #2", "stream already allocated");
+ rccEnableUSART1(FALSE);
+ nvicEnableVector(STM32_USART1_NUMBER,
+ CORTEX_PRIORITY_MASK(STM32_UART_USART1_IRQ_PRIORITY));
+ uartp->dmamode |= STM32_DMA_CR_CHSEL(USART1_RX_DMA_CHANNEL) |
+ STM32_DMA_CR_PL(STM32_UART_USART1_DMA_PRIORITY);
+ }
+#endif
+
+#if STM32_UART_USE_USART2
+ if (&UARTD2 == uartp) {
+ bool_t b;
+ b = dmaStreamAllocate(uartp->dmarx,
+ STM32_UART_USART2_IRQ_PRIORITY,
+ (stm32_dmaisr_t)uart_lld_serve_rx_end_irq,
+ (void *)uartp);
+ chDbgAssert(!b, "uart_lld_start(), #3", "stream already allocated");
+ b = dmaStreamAllocate(uartp->dmatx,
+ STM32_UART_USART2_IRQ_PRIORITY,
+ (stm32_dmaisr_t)uart_lld_serve_tx_end_irq,
+ (void *)uartp);
+ chDbgAssert(!b, "uart_lld_start(), #4", "stream already allocated");
+ rccEnableUSART2(FALSE);
+ nvicEnableVector(STM32_USART2_NUMBER,
+ CORTEX_PRIORITY_MASK(STM32_UART_USART2_IRQ_PRIORITY));
+ uartp->dmamode |= STM32_DMA_CR_CHSEL(USART2_RX_DMA_CHANNEL) |
+ STM32_DMA_CR_PL(STM32_UART_USART2_DMA_PRIORITY);
+ }
+#endif
+
+#if STM32_UART_USE_USART3
+ if (&UARTD3 == uartp) {
+ bool_t b;
+ b = dmaStreamAllocate(uartp->dmarx,
+ STM32_UART_USART3_IRQ_PRIORITY,
+ (stm32_dmaisr_t)uart_lld_serve_rx_end_irq,
+ (void *)uartp);
+ chDbgAssert(!b, "uart_lld_start(), #5", "stream already allocated");
+ b = dmaStreamAllocate(uartp->dmatx,
+ STM32_UART_USART3_IRQ_PRIORITY,
+ (stm32_dmaisr_t)uart_lld_serve_tx_end_irq,
+ (void *)uartp);
+ chDbgAssert(!b, "uart_lld_start(), #6", "stream already allocated");
+ rccEnableUSART3(FALSE);
+ nvicEnableVector(STM32_USART3_NUMBER,
+ CORTEX_PRIORITY_MASK(STM32_UART_USART3_IRQ_PRIORITY));
+ uartp->dmamode |= STM32_DMA_CR_CHSEL(USART3_RX_DMA_CHANNEL) |
+ STM32_DMA_CR_PL(STM32_UART_USART3_DMA_PRIORITY);
+ }
+#endif
+
+ /* Static DMA setup, the transfer size depends on the USART settings,
+ it is 16 bits if M=1 and PCE=0 else it is 8 bits.*/
+ if ((uartp->config->cr1 & (USART_CR1_M | USART_CR1_PCE)) == USART_CR1_M)
+ uartp->dmamode |= STM32_DMA_CR_PSIZE_HWORD | STM32_DMA_CR_MSIZE_HWORD;
+ dmaStreamSetPeripheral(uartp->dmarx, &uartp->usart->RDR);
+ dmaStreamSetPeripheral(uartp->dmatx, &uartp->usart->TDR);
+ uartp->rxbuf = 0;
+ }
+
+ uartp->rxstate = UART_RX_IDLE;
+ uartp->txstate = UART_TX_IDLE;
+ usart_start(uartp);
+}
+
+/**
+ * @brief Deactivates the UART peripheral.
+ *
+ * @param[in] uartp pointer to the @p UARTDriver object
+ *
+ * @notapi
+ */
+void uart_lld_stop(UARTDriver *uartp) {
+
+ if (uartp->state == UART_READY) {
+ usart_stop(uartp);
+ dmaStreamRelease(uartp->dmarx);
+ dmaStreamRelease(uartp->dmatx);
+
+#if STM32_UART_USE_USART1
+ if (&UARTD1 == uartp) {
+ nvicDisableVector(STM32_USART1_NUMBER);
+ rccDisableUSART1(FALSE);
+ return;
+ }
+#endif
+
+#if STM32_UART_USE_USART2
+ if (&UARTD2 == uartp) {
+ nvicDisableVector(STM32_USART2_NUMBER);
+ rccDisableUSART2(FALSE);
+ return;
+ }
+#endif
+
+#if STM32_UART_USE_USART3
+ if (&UARTD3 == uartp) {
+ nvicDisableVector(STM32_USART3_NUMBER);
+ rccDisableUSART3(FALSE);
+ return;
+ }
+#endif
+ }
+}
+
+/**
+ * @brief Starts a transmission on the UART peripheral.
+ * @note The buffers are organized as uint8_t arrays for data sizes below
+ * or equal to 8 bits else it is organized as uint16_t arrays.
+ *
+ * @param[in] uartp pointer to the @p UARTDriver object
+ * @param[in] n number of data frames to send
+ * @param[in] txbuf the pointer to the transmit buffer
+ *
+ * @notapi
+ */
+void uart_lld_start_send(UARTDriver *uartp, size_t n, const void *txbuf) {
+
+ /* TX DMA channel preparation and start.*/
+ dmaStreamSetMemory0(uartp->dmatx, txbuf);
+ dmaStreamSetTransactionSize(uartp->dmatx, n);
+ dmaStreamSetMode(uartp->dmatx, uartp->dmamode | STM32_DMA_CR_DIR_M2P |
+ STM32_DMA_CR_MINC | STM32_DMA_CR_TCIE);
+ dmaStreamEnable(uartp->dmatx);
+}
+
+/**
+ * @brief Stops any ongoing transmission.
+ * @note Stopping a transmission also suppresses the transmission callbacks.
+ *
+ * @param[in] uartp pointer to the @p UARTDriver object
+ *
+ * @return The number of data frames not transmitted by the
+ * stopped transmit operation.
+ *
+ * @notapi
+ */
+size_t uart_lld_stop_send(UARTDriver *uartp) {
+
+ dmaStreamDisable(uartp->dmatx);
+ return dmaStreamGetTransactionSize(uartp->dmatx);
+}
+
+/**
+ * @brief Starts a receive operation on the UART peripheral.
+ * @note The buffers are organized as uint8_t arrays for data sizes below
+ * or equal to 8 bits else it is organized as uint16_t arrays.
+ *
+ * @param[in] uartp pointer to the @p UARTDriver object
+ * @param[in] n number of data frames to send
+ * @param[out] rxbuf the pointer to the receive buffer
+ *
+ * @notapi
+ */
+void uart_lld_start_receive(UARTDriver *uartp, size_t n, void *rxbuf) {
+
+ /* Stopping previous activity (idle state).*/
+ dmaStreamDisable(uartp->dmarx);
+
+ /* RX DMA channel preparation and start.*/
+ dmaStreamSetMemory0(uartp->dmarx, rxbuf);
+ dmaStreamSetTransactionSize(uartp->dmarx, n);
+ dmaStreamSetMode(uartp->dmarx, uartp->dmamode | STM32_DMA_CR_DIR_P2M |
+ STM32_DMA_CR_MINC | STM32_DMA_CR_TCIE);
+ dmaStreamEnable(uartp->dmarx);
+}
+
+/**
+ * @brief Stops any ongoing receive operation.
+ * @note Stopping a receive operation also suppresses the receive callbacks.
+ *
+ * @param[in] uartp pointer to the @p UARTDriver object
+ *
+ * @return The number of data frames not received by the
+ * stopped receive operation.
+ *
+ * @notapi
+ */
+size_t uart_lld_stop_receive(UARTDriver *uartp) {
+ size_t n;
+
+ dmaStreamDisable(uartp->dmarx);
+ n = dmaStreamGetTransactionSize(uartp->dmarx);
+ set_rx_idle_loop(uartp);
+ return n;
+}
+
+#endif /* HAL_USE_UART */
+
+/** @} */
diff --git a/firmware/chibios/os/hal/platforms/STM32/USARTv2/uart_lld.h b/firmware/chibios/os/hal/platforms/STM32/USARTv2/uart_lld.h
new file mode 100644
index 0000000000..8d8392f2a6
--- /dev/null
+++ b/firmware/chibios/os/hal/platforms/STM32/USARTv2/uart_lld.h
@@ -0,0 +1,458 @@
+/*
+ ChibiOS/RT - Copyright (C) 2006-2013 Giovanni Di Sirio
+
+ 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.
+*/
+
+/**
+ * @file STM32/USARTv2/uart_lld.h
+ * @brief STM32 low level UART driver header.
+ *
+ * @addtogroup UART
+ * @{
+ */
+
+#ifndef _UART_LLD_H_
+#define _UART_LLD_H_
+
+#if HAL_USE_UART || defined(__DOXYGEN__)
+
+/*===========================================================================*/
+/* Driver constants. */
+/*===========================================================================*/
+
+/*===========================================================================*/
+/* Driver pre-compile time settings. */
+/*===========================================================================*/
+
+/**
+ * @name Configuration options
+ * @{
+ */
+/**
+ * @brief UART driver on USART1 enable switch.
+ * @details If set to @p TRUE the support for USART1 is included.
+ * @note The default is @p FALSE.
+ */
+#if !defined(STM32_UART_USE_USART1) || defined(__DOXYGEN__)
+#define STM32_UART_USE_USART1 FALSE
+#endif
+
+/**
+ * @brief UART driver on USART2 enable switch.
+ * @details If set to @p TRUE the support for USART2 is included.
+ * @note The default is @p FALSE.
+ */
+#if !defined(STM32_UART_USE_USART2) || defined(__DOXYGEN__)
+#define STM32_UART_USE_USART2 FALSE
+#endif
+
+/**
+ * @brief UART driver on USART3 enable switch.
+ * @details If set to @p TRUE the support for USART3 is included.
+ * @note The default is @p FALSE.
+ */
+#if !defined(STM32_UART_USE_USART3) || defined(__DOXYGEN__)
+#define STM32_UART_USE_USART3 FALSE
+#endif
+
+/**
+ * @brief USART1 interrupt priority level setting.
+ */
+#if !defined(STM32_UART_USART1_IRQ_PRIORITY) || defined(__DOXYGEN__)
+#define STM32_UART_USART1_IRQ_PRIORITY 12
+#endif
+
+/**
+ * @brief USART2 interrupt priority level setting.
+ */
+#if !defined(STM32_UART_USART2_IRQ_PRIORITY) || defined(__DOXYGEN__)
+#define STM32_UART_USART2_IRQ_PRIORITY 12
+#endif
+
+/**
+ * @brief USART3 interrupt priority level setting.
+ */
+#if !defined(STM32_UART_USART3_IRQ_PRIORITY) || defined(__DOXYGEN__)
+#define STM32_UART_USART3_IRQ_PRIORITY 12
+#endif
+
+/**
+ * @brief USART1 DMA priority (0..3|lowest..highest).
+ * @note The priority level is used for both the TX and RX DMA channels but
+ * because of the channels ordering the RX channel has always priority
+ * over the TX channel.
+ */
+#if !defined(STM32_UART_USART1_DMA_PRIORITY) || defined(__DOXYGEN__)
+#define STM32_UART_USART1_DMA_PRIORITY 0
+#endif
+
+/**
+ * @brief USART2 DMA priority (0..3|lowest..highest).
+ * @note The priority level is used for both the TX and RX DMA channels but
+ * because of the channels ordering the RX channel has always priority
+ * over the TX channel.
+ */
+#if !defined(STM32_UART_USART2_DMA_PRIORITY) || defined(__DOXYGEN__)
+#define STM32_UART_USART2_DMA_PRIORITY 0
+#endif
+
+/**
+ * @brief USART3 DMA priority (0..3|lowest..highest).
+ * @note The priority level is used for both the TX and RX DMA channels but
+ * because of the channels ordering the RX channel has always priority
+ * over the TX channel.
+ */
+#if !defined(STM32_UART_USART3_DMA_PRIORITY) || defined(__DOXYGEN__)
+#define STM32_UART_USART3_DMA_PRIORITY 0
+#endif
+
+/**
+ * @brief USART1 DMA error hook.
+ * @note The default action for DMA errors is a system halt because DMA
+ * error can only happen because programming errors.
+ */
+#if !defined(STM32_UART_DMA_ERROR_HOOK) || defined(__DOXYGEN__)
+#define STM32_UART_DMA_ERROR_HOOK(uartp) chSysHalt()
+#endif
+
+#if STM32_ADVANCED_DMA || defined(__DOXYGEN__)
+
+/**
+ * @brief DMA stream used for USART1 RX operations.
+ * @note This option is only available on platforms with enhanced DMA.
+ */
+#if !defined(STM32_UART_USART1_RX_DMA_STREAM) || defined(__DOXYGEN__)
+#define STM32_UART_USART1_RX_DMA_STREAM STM32_DMA_STREAM_ID(2, 5)
+#endif
+
+/**
+ * @brief DMA stream used for USART1 TX operations.
+ * @note This option is only available on platforms with enhanced DMA.
+ */
+#if !defined(STM32_UART_USART1_TX_DMA_STREAM) || defined(__DOXYGEN__)
+#define STM32_UART_USART1_TX_DMA_STREAM STM32_DMA_STREAM_ID(2, 7)
+#endif
+
+/**
+ * @brief DMA stream used for USART2 RX operations.
+ * @note This option is only available on platforms with enhanced DMA.
+ */
+#if !defined(STM32_UART_USART2_RX_DMA_STREAM) || defined(__DOXYGEN__)
+#define STM32_UART_USART2_RX_DMA_STREAM STM32_DMA_STREAM_ID(1, 5)
+#endif
+
+/**
+ * @brief DMA stream used for USART2 TX operations.
+ * @note This option is only available on platforms with enhanced DMA.
+ */
+#if !defined(STM32_UART_USART2_TX_DMA_STREAM) || defined(__DOXYGEN__)
+#define STM32_UART_USART2_TX_DMA_STREAM STM32_DMA_STREAM_ID(1, 6)
+#endif
+
+/**
+ * @brief DMA stream used for USART3 RX operations.
+ * @note This option is only available on platforms with enhanced DMA.
+ */
+#if !defined(STM32_UART_USART3_RX_DMA_STREAM) || defined(__DOXYGEN__)
+#define STM32_UART_USART3_RX_DMA_STREAM STM32_DMA_STREAM_ID(1, 1)
+#endif
+
+/**
+ * @brief DMA stream used for USART3 TX operations.
+ * @note This option is only available on platforms with enhanced DMA.
+ */
+#if !defined(STM32_UART_USART3_TX_DMA_STREAM) || defined(__DOXYGEN__)
+#define STM32_UART_USART3_TX_DMA_STREAM STM32_DMA_STREAM_ID(1, 3)
+#endif
+
+#else /* !STM32_ADVANCED_DMA*/
+
+#if defined(STM32F0XX)
+/* Fixed values for STM32F0xx devices.*/
+#define STM32_UART_USART1_RX_DMA_STREAM STM32_DMA_STREAM_ID(1, 3)
+#define STM32_UART_USART1_TX_DMA_STREAM STM32_DMA_STREAM_ID(1, 2)
+#define STM32_UART_USART2_RX_DMA_STREAM STM32_DMA_STREAM_ID(1, 5)
+#define STM32_UART_USART2_TX_DMA_STREAM STM32_DMA_STREAM_ID(1, 4)
+#endif /* defined(STM32F0XX) */
+
+#if defined(STM32F30X)|| defined(STM32F37X)
+/* Fixed values for STM32F3xx devices.*/
+#define STM32_UART_USART1_RX_DMA_STREAM STM32_DMA_STREAM_ID(1, 5)
+#define STM32_UART_USART1_TX_DMA_STREAM STM32_DMA_STREAM_ID(1, 4)
+#define STM32_UART_USART2_RX_DMA_STREAM STM32_DMA_STREAM_ID(1, 6)
+#define STM32_UART_USART2_TX_DMA_STREAM STM32_DMA_STREAM_ID(1, 7)
+#define STM32_UART_USART3_RX_DMA_STREAM STM32_DMA_STREAM_ID(1, 3)
+#define STM32_UART_USART3_TX_DMA_STREAM STM32_DMA_STREAM_ID(1, 2)
+#endif /* defined(STM32F30X) */
+
+#endif /* !STM32_ADVANCED_DMA*/
+/** @} */
+
+/*===========================================================================*/
+/* Derived constants and error checks. */
+/*===========================================================================*/
+
+#if STM32_UART_USE_USART1 && !STM32_HAS_USART1
+#error "USART1 not present in the selected device"
+#endif
+
+#if STM32_UART_USE_USART2 && !STM32_HAS_USART2
+#error "USART2 not present in the selected device"
+#endif
+
+#if STM32_UART_USE_USART3 && !STM32_HAS_USART3
+#error "USART3 not present in the selected device"
+#endif
+
+#if !STM32_UART_USE_USART1 && !STM32_UART_USE_USART2 && \
+ !STM32_UART_USE_USART3
+#error "UART driver activated but no USART/UART peripheral assigned"
+#endif
+
+#if STM32_UART_USE_USART1 && \
+ !CORTEX_IS_VALID_KERNEL_PRIORITY(STM32_UART_USART1_IRQ_PRIORITY)
+#error "Invalid IRQ priority assigned to USART1"
+#endif
+
+#if STM32_UART_USE_USART2 && \
+ !CORTEX_IS_VALID_KERNEL_PRIORITY(STM32_UART_USART2_IRQ_PRIORITY)
+#error "Invalid IRQ priority assigned to USART2"
+#endif
+
+#if STM32_UART_USE_USART3 && \
+ !CORTEX_IS_VALID_KERNEL_PRIORITY(STM32_UART_USART3_IRQ_PRIORITY)
+#error "Invalid IRQ priority assigned to USART3"
+#endif
+
+#if STM32_UART_USE_USART1 && \
+ !STM32_DMA_IS_VALID_PRIORITY(STM32_UART_USART1_DMA_PRIORITY)
+#error "Invalid DMA priority assigned to USART1"
+#endif
+
+#if STM32_UART_USE_USART2 && \
+ !STM32_DMA_IS_VALID_PRIORITY(STM32_UART_USART2_DMA_PRIORITY)
+#error "Invalid DMA priority assigned to USART2"
+#endif
+
+#if STM32_UART_USE_USART3 && \
+ !STM32_DMA_IS_VALID_PRIORITY(STM32_UART_USART3_DMA_PRIORITY)
+#error "Invalid DMA priority assigned to USART3"
+#endif
+
+#if STM32_UART_USE_USART1 && \
+ !STM32_DMA_IS_VALID_ID(STM32_UART_USART1_RX_DMA_STREAM, \
+ STM32_USART1_RX_DMA_MSK)
+#error "invalid DMA stream associated to USART1 RX"
+#endif
+
+#if STM32_UART_USE_USART1 && \
+ !STM32_DMA_IS_VALID_ID(STM32_UART_USART1_TX_DMA_STREAM, \
+ STM32_USART1_TX_DMA_MSK)
+#error "invalid DMA stream associated to USART1 TX"
+#endif
+
+#if STM32_UART_USE_USART2 && \
+ !STM32_DMA_IS_VALID_ID(STM32_UART_USART2_RX_DMA_STREAM, \
+ STM32_USART2_RX_DMA_MSK)
+#error "invalid DMA stream associated to USART2 RX"
+#endif
+
+#if STM32_UART_USE_USART2 && \
+ !STM32_DMA_IS_VALID_ID(STM32_UART_USART2_TX_DMA_STREAM, \
+ STM32_USART2_TX_DMA_MSK)
+#error "invalid DMA stream associated to USART2 TX"
+#endif
+
+#if STM32_UART_USE_USART3 && \
+ !STM32_DMA_IS_VALID_ID(STM32_UART_USART3_RX_DMA_STREAM, \
+ STM32_USART3_RX_DMA_MSK)
+#error "invalid DMA stream associated to USART3 RX"
+#endif
+
+#if STM32_UART_USE_USART3 && \
+ !STM32_DMA_IS_VALID_ID(STM32_UART_USART3_TX_DMA_STREAM, \
+ STM32_USART3_TX_DMA_MSK)
+#error "invalid DMA stream associated to USART3 TX"
+#endif
+
+#if !defined(STM32_DMA_REQUIRED)
+#define STM32_DMA_REQUIRED
+#endif
+
+/*===========================================================================*/
+/* Driver data structures and types. */
+/*===========================================================================*/
+
+/**
+ * @brief UART driver condition flags type.
+ */
+typedef uint32_t uartflags_t;
+
+/**
+ * @brief Structure representing an UART driver.
+ */
+typedef struct UARTDriver UARTDriver;
+
+/**
+ * @brief Generic UART notification callback type.
+ *
+ * @param[in] uartp pointer to the @p UARTDriver object
+ */
+typedef void (*uartcb_t)(UARTDriver *uartp);
+
+/**
+ * @brief Character received UART notification callback type.
+ *
+ * @param[in] uartp pointer to the @p UARTDriver object
+ * @param[in] c received character
+ */
+typedef void (*uartccb_t)(UARTDriver *uartp, uint16_t c);
+
+/**
+ * @brief Receive error UART notification callback type.
+ *
+ * @param[in] uartp pointer to the @p UARTDriver object
+ * @param[in] e receive error mask
+ */
+typedef void (*uartecb_t)(UARTDriver *uartp, uartflags_t e);
+
+/**
+ * @brief Driver configuration structure.
+ * @note It could be empty on some architectures.
+ */
+typedef struct {
+ /**
+ * @brief End of transmission buffer callback.
+ */
+ uartcb_t txend1_cb;
+ /**
+ * @brief Physical end of transmission callback.
+ */
+ uartcb_t txend2_cb;
+ /**
+ * @brief Receive buffer filled callback.
+ */
+ uartcb_t rxend_cb;
+ /**
+ * @brief Character received while out if the @p UART_RECEIVE state.
+ */
+ uartccb_t rxchar_cb;
+ /**
+ * @brief Receive error callback.
+ */
+ uartecb_t rxerr_cb;
+ /* End of the mandatory fields.*/
+ /**
+ * @brief Bit rate.
+ */
+ uint32_t speed;
+ /**
+ * @brief Initialization value for the CR1 register.
+ */
+ uint32_t cr1;
+ /**
+ * @brief Initialization value for the CR2 register.
+ */
+ uint32_t cr2;
+ /**
+ * @brief Initialization value for the CR3 register.
+ */
+ uint32_t cr3;
+} UARTConfig;
+
+/**
+ * @brief Structure representing an UART driver.
+ */
+struct UARTDriver {
+ /**
+ * @brief Driver state.
+ */
+ uartstate_t state;
+ /**
+ * @brief Transmitter state.
+ */
+ uarttxstate_t txstate;
+ /**
+ * @brief Receiver state.
+ */
+ uartrxstate_t rxstate;
+ /**
+ * @brief Current configuration data.
+ */
+ const UARTConfig *config;
+#if defined(UART_DRIVER_EXT_FIELDS)
+ UART_DRIVER_EXT_FIELDS
+#endif
+ /* End of the mandatory fields.*/
+ /**
+ * @brief Pointer to the USART registers block.
+ */
+ USART_TypeDef *usart;
+ /**
+ * @brief DMA mode bit mask.
+ */
+ uint32_t dmamode;
+ /**
+ * @brief Receive DMA channel.
+ */
+ const stm32_dma_stream_t *dmarx;
+ /**
+ * @brief Transmit DMA channel.
+ */
+ const stm32_dma_stream_t *dmatx;
+ /**
+ * @brief Default receive buffer while into @p UART_RX_IDLE state.
+ */
+ volatile uint16_t rxbuf;
+};
+
+/*===========================================================================*/
+/* Driver macros. */
+/*===========================================================================*/
+
+/*===========================================================================*/
+/* External declarations. */
+/*===========================================================================*/
+
+#if STM32_UART_USE_USART1 && !defined(__DOXYGEN__)
+extern UARTDriver UARTD1;
+#endif
+
+#if STM32_UART_USE_USART2 && !defined(__DOXYGEN__)
+extern UARTDriver UARTD2;
+#endif
+
+#if STM32_UART_USE_USART3 && !defined(__DOXYGEN__)
+extern UARTDriver UARTD3;
+#endif
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+ void uart_lld_init(void);
+ void uart_lld_start(UARTDriver *uartp);
+ void uart_lld_stop(UARTDriver *uartp);
+ void uart_lld_start_send(UARTDriver *uartp, size_t n, const void *txbuf);
+ size_t uart_lld_stop_send(UARTDriver *uartp);
+ void uart_lld_start_receive(UARTDriver *uartp, size_t n, void *rxbuf);
+ size_t uart_lld_stop_receive(UARTDriver *uartp);
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* HAL_USE_UART */
+
+#endif /* _UART_LLD_H_ */
+
+/** @} */
diff --git a/firmware/chibios/os/ports/GCC/ARMCMx/chcore_v6m.c b/firmware/chibios/os/ports/GCC/ARMCMx/chcore_v6m.c
new file mode 100644
index 0000000000..33bb8ab453
--- /dev/null
+++ b/firmware/chibios/os/ports/GCC/ARMCMx/chcore_v6m.c
@@ -0,0 +1,206 @@
+/*
+ ChibiOS/RT - Copyright (C) 2006,2007,2008,2009,2010,
+ 2011,2012,2013 Giovanni Di Sirio.
+
+ This file is part of ChibiOS/RT.
+
+ ChibiOS/RT is free software; you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation; either version 3 of the License, or
+ (at your option) any later version.
+
+ ChibiOS/RT is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with this program. If not, see .
+
+ ---
+
+ A special exception to the GPL can be applied should you wish to distribute
+ a combined work that includes ChibiOS/RT, without being obliged to provide
+ the source code for any proprietary components. See the file exception.txt
+ for full details of how and when the exception can be applied.
+*/
+
+/**
+ * @file GCC/ARMCMx/chcore_v6m.c
+ * @brief ARMv6-M architecture port code.
+ *
+ * @addtogroup ARMCMx_V6M_CORE
+ * @{
+ */
+
+#include "ch.h"
+
+/*===========================================================================*/
+/* Port interrupt handlers. */
+/*===========================================================================*/
+
+/**
+ * @brief System Timer vector.
+ * @details This interrupt is used as system tick.
+ * @note The timer must be initialized in the startup code.
+ */
+CH_IRQ_HANDLER(SysTickVector) {
+
+ CH_IRQ_PROLOGUE();
+
+ chSysLockFromIsr();
+ chSysTimerHandlerI();
+ chSysUnlockFromIsr();
+
+ CH_IRQ_EPILOGUE();
+}
+
+#if !CORTEX_ALTERNATE_SWITCH || defined(__DOXYGEN__)
+/**
+ * @brief NMI vector.
+ * @details The NMI vector is used for exception mode re-entering after a
+ * context switch.
+ */
+void NMIVector(void) {
+ register struct extctx *ctxp;
+
+ /* Discarding the current exception context and positioning the stack to
+ point to the real one.*/
+ asm volatile ("mrs %0, PSP" : "=r" (ctxp) : : "memory");
+ ctxp++;
+ asm volatile ("msr PSP, %0" : : "r" (ctxp) : "memory");
+ port_unlock_from_isr();
+}
+#endif /* !CORTEX_ALTERNATE_SWITCH */
+
+#if CORTEX_ALTERNATE_SWITCH || defined(__DOXYGEN__)
+/**
+ * @brief PendSV vector.
+ * @details The PendSV vector is used for exception mode re-entering after a
+ * context switch.
+ */
+void PendSVVector(void) {
+ register struct extctx *ctxp;
+
+ /* Discarding the current exception context and positioning the stack to
+ point to the real one.*/
+ asm volatile ("mrs %0, PSP" : "=r" (ctxp) : : "memory");
+ ctxp++;
+ asm volatile ("msr PSP, %0" : : "r" (ctxp) : "memory");
+}
+#endif /* CORTEX_ALTERNATE_SWITCH */
+
+/*===========================================================================*/
+/* Port exported functions. */
+/*===========================================================================*/
+
+/**
+ * @brief IRQ epilogue code.
+ *
+ * @param[in] lr value of the @p LR register on ISR entry
+ */
+void _port_irq_epilogue(regarm_t lr) {
+
+ if (lr != (regarm_t)0xFFFFFFF1) {
+ register struct extctx *ctxp;
+
+ port_lock_from_isr();
+ /* Adding an artificial exception return context, there is no need to
+ populate it fully.*/
+ asm volatile ("mrs %0, PSP" : "=r" (ctxp) : : "memory");
+ ctxp--;
+ asm volatile ("msr PSP, %0" : : "r" (ctxp) : "memory");
+ ctxp->xpsr = (regarm_t)0x01000000;
+
+ /* The exit sequence is different depending on if a preemption is
+ required or not.*/
+ if (chSchIsPreemptionRequired()) {
+ /* Preemption is required we need to enforce a context switch.*/
+ ctxp->pc = (void *)_port_switch_from_isr;
+ }
+ else {
+ /* Preemption not required, we just need to exit the exception
+ atomically.*/
+ ctxp->pc = (void *)_port_exit_from_isr;
+ }
+
+ /* Note, returning without unlocking is intentional, this is done in
+ order to keep the rest of the context switch atomic.*/
+ }
+}
+
+/**
+ * @brief Post-IRQ switch code.
+ * @details The switch is performed in thread context then an NMI exception
+ * is enforced in order to return to the exact point before the
+ * preemption.
+ */
+#if !defined(__DOXYGEN__)
+__attribute__((naked))
+#endif
+void _port_switch_from_isr(void) {
+
+ dbg_check_lock();
+ chSchDoReschedule();
+ dbg_check_unlock();
+ asm volatile ("_port_exit_from_isr:" : : : "memory");
+#if CORTEX_ALTERNATE_SWITCH
+ SCB_ICSR = ICSR_PENDSVSET;
+ port_unlock();
+#else
+ SCB_ICSR = ICSR_NMIPENDSET;
+#endif
+ /* The following loop should never be executed, the exception will kick in
+ immediately.*/
+ while (TRUE)
+ ;
+}
+
+/**
+ * @brief Performs a context switch between two threads.
+ * @details This is the most critical code in any port, this function
+ * is responsible for the context switch between 2 threads.
+ * @note The implementation of this code affects directly the context
+ * switch performance so optimize here as much as you can.
+ *
+ * @param[in] ntp the thread to be switched in
+ * @param[in] otp the thread to be switched out
+ */
+#if !defined(__DOXYGEN__)
+__attribute__((naked))
+#endif
+void _port_switch(Thread *ntp, Thread *otp) {
+ register struct intctx *r13 asm ("r13");
+
+ asm volatile ("push {r4, r5, r6, r7, lr} \n\t"
+ "mov r4, r8 \n\t"
+ "mov r5, r9 \n\t"
+ "mov r6, r10 \n\t"
+ "mov r7, r11 \n\t"
+ "push {r4, r5, r6, r7}" : : : "memory");
+
+ otp->p_ctx.r13 = r13;
+ r13 = ntp->p_ctx.r13;
+
+ asm volatile ("pop {r4, r5, r6, r7} \n\t"
+ "mov r8, r4 \n\t"
+ "mov r9, r5 \n\t"
+ "mov r10, r6 \n\t"
+ "mov r11, r7 \n\t"
+ "pop {r4, r5, r6, r7, pc}" : : "r" (r13) : "memory");
+}
+
+/**
+ * @brief Start a thread by invoking its work function.
+ * @details If the work function returns @p chThdExit() is automatically
+ * invoked.
+ */
+void _port_thread_start(void) {
+
+ chSysUnlock();
+ asm volatile ("mov r0, r5 \n\t"
+ "blx r4 \n\t"
+ "bl chThdExit");
+}
+
+/** @} */
diff --git a/firmware/chibios/os/ports/GCC/ARMCMx/chcore_v6m.h b/firmware/chibios/os/ports/GCC/ARMCMx/chcore_v6m.h
new file mode 100644
index 0000000000..3047c45c90
--- /dev/null
+++ b/firmware/chibios/os/ports/GCC/ARMCMx/chcore_v6m.h
@@ -0,0 +1,383 @@
+/*
+ ChibiOS/RT - Copyright (C) 2006,2007,2008,2009,2010,
+ 2011,2012,2013 Giovanni Di Sirio.
+
+ This file is part of ChibiOS/RT.
+
+ ChibiOS/RT is free software; you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation; either version 3 of the License, or
+ (at your option) any later version.
+
+ ChibiOS/RT is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with this program. If not, see .
+
+ ---
+
+ A special exception to the GPL can be applied should you wish to distribute
+ a combined work that includes ChibiOS/RT, without being obliged to provide
+ the source code for any proprietary components. See the file exception.txt
+ for full details of how and when the exception can be applied.
+*/
+
+/**
+ * @file GCC/ARMCMx/chcore_v6m.h
+ * @brief ARMv6-M architecture port macros and structures.
+ *
+ * @addtogroup ARMCMx_V6M_CORE
+ * @{
+ */
+
+#ifndef _CHCORE_V6M_H_
+#define _CHCORE_V6M_H_
+
+/*===========================================================================*/
+/* Port constants. */
+/*===========================================================================*/
+
+/**
+ * @brief PendSV priority level.
+ * @note This priority is enforced to be equal to @p 0,
+ * this handler always has the highest priority that cannot preempt
+ * the kernel.
+ */
+#define CORTEX_PRIORITY_PENDSV 0
+
+/*===========================================================================*/
+/* Port macros. */
+/*===========================================================================*/
+
+/*===========================================================================*/
+/* Port configurable parameters. */
+/*===========================================================================*/
+
+/**
+ * @brief Stack size for the system idle thread.
+ * @details This size depends on the idle thread implementation, usually
+ * the idle thread should take no more space than those reserved
+ * by @p PORT_INT_REQUIRED_STACK.
+ * @note In this port it is set to 16 because the idle thread does have
+ * a stack frame when compiling without optimizations. You may
+ * reduce this value to zero when compiling with optimizations.
+ */
+#if !defined(PORT_IDLE_THREAD_STACK_SIZE)
+#define PORT_IDLE_THREAD_STACK_SIZE 16
+#endif
+
+/**
+ * @brief Per-thread stack overhead for interrupts servicing.
+ * @details This constant is used in the calculation of the correct working
+ * area size.
+ * @note In this port this value is conservatively set to 64 because the
+ * function @p chSchDoReschedule() can have a stack frame, especially
+ * with compiler optimizations disabled. The value can be reduced
+ * when compiler optimizations are enabled.
+ */
+#if !defined(PORT_INT_REQUIRED_STACK)
+#define PORT_INT_REQUIRED_STACK 64
+#endif
+
+/**
+ * @brief Enables the use of the WFI instruction in the idle thread loop.
+ */
+#if !defined(CORTEX_ENABLE_WFI_IDLE)
+#define CORTEX_ENABLE_WFI_IDLE FALSE
+#endif
+
+/**
+ * @brief SYSTICK handler priority.
+ * @note The default SYSTICK handler priority is calculated as the priority
+ * level in the middle of the numeric priorities range.
+ */
+#if !defined(CORTEX_PRIORITY_SYSTICK)
+#define CORTEX_PRIORITY_SYSTICK (CORTEX_PRIORITY_LEVELS >> 1)
+#elif !CORTEX_IS_VALID_PRIORITY(CORTEX_PRIORITY_SYSTICK)
+/* If it is externally redefined then better perform a validity check on it.*/
+#error "invalid priority level specified for CORTEX_PRIORITY_SYSTICK"
+#endif
+
+/**
+ * @brief Alternate preemption method.
+ * @details Activating this option will make the Kernel use the PendSV
+ * handler for preemption instead of the NMI handler.
+ */
+#ifndef CORTEX_ALTERNATE_SWITCH
+#define CORTEX_ALTERNATE_SWITCH FALSE
+#endif
+
+/*===========================================================================*/
+/* Port derived parameters. */
+/*===========================================================================*/
+
+/**
+ * @brief Maximum usable priority for normal ISRs.
+ */
+#if CORTEX_ALTERNATE_SWITCH || defined(__DOXYGEN__)
+#define CORTEX_MAX_KERNEL_PRIORITY 1
+#else
+#define CORTEX_MAX_KERNEL_PRIORITY 0
+#endif
+
+/*===========================================================================*/
+/* Port exported info. */
+/*===========================================================================*/
+
+/**
+ * @brief Macro defining the specific ARM architecture.
+ */
+#define CH_ARCHITECTURE_ARM_v6M
+
+/**
+ * @brief Name of the implemented architecture.
+ */
+#define CH_ARCHITECTURE_NAME "ARMv6-M"
+
+/**
+ * @brief Name of the architecture variant.
+ */
+#if (CORTEX_MODEL == CORTEX_M0) || defined(__DOXYGEN__)
+#define CH_CORE_VARIANT_NAME "Cortex-M0"
+#elif (CORTEX_MODEL == CORTEX_M1)
+#define CH_CORE_VARIANT_NAME "Cortex-M1"
+#endif
+
+/**
+ * @brief Port-specific information string.
+ */
+#if !CORTEX_ALTERNATE_SWITCH || defined(__DOXYGEN__)
+#define CH_PORT_INFO "Preemption through NMI"
+#else
+#define CH_PORT_INFO "Preemption through PendSV"
+#endif
+
+/*===========================================================================*/
+/* Port implementation part. */
+/*===========================================================================*/
+
+#if !defined(_FROM_ASM_)
+
+/**
+ * @brief Generic ARM register.
+ */
+typedef void *regarm_t;
+
+ /* The documentation of the following declarations is in chconf.h in order
+ to not have duplicated structure names into the documentation.*/
+#if !defined(__DOXYGEN__)
+
+typedef uint64_t stkalign_t __attribute__ ((aligned (8)));
+
+struct extctx {
+ regarm_t r0;
+ regarm_t r1;
+ regarm_t r2;
+ regarm_t r3;
+ regarm_t r12;
+ regarm_t lr_thd;
+ regarm_t pc;
+ regarm_t xpsr;
+};
+
+struct intctx {
+ regarm_t r8;
+ regarm_t r9;
+ regarm_t r10;
+ regarm_t r11;
+ regarm_t r4;
+ regarm_t r5;
+ regarm_t r6;
+ regarm_t r7;
+ regarm_t lr;
+};
+
+#endif /* !defined(__DOXYGEN__) */
+
+/**
+ * @brief Platform dependent part of the @p Thread structure.
+ * @details In this port the structure just holds a pointer to the @p intctx
+ * structure representing the stack pointer at context switch time.
+ */
+struct context {
+ struct intctx *r13;
+};
+
+/**
+ * @brief Platform dependent part of the @p chThdCreateI() API.
+ * @details This code usually setup the context switching frame represented
+ * by an @p intctx structure.
+ */
+#define SETUP_CONTEXT(workspace, wsize, pf, arg) { \
+ tp->p_ctx.r13 = (struct intctx *)((uint8_t *)workspace + \
+ wsize - \
+ sizeof(struct intctx)); \
+ tp->p_ctx.r13->r4 = (void *)(pf); \
+ tp->p_ctx.r13->r5 = (void *)(arg); \
+ tp->p_ctx.r13->lr = (void *)(_port_thread_start); \
+}
+
+/**
+ * @brief Enforces a correct alignment for a stack area size value.
+ */
+#define STACK_ALIGN(n) ((((n) - 1) | (sizeof(stkalign_t) - 1)) + 1)
+
+/**
+ * @brief Computes the thread working area global size.
+ */
+#define THD_WA_SIZE(n) STACK_ALIGN(sizeof(Thread) + \
+ sizeof(struct intctx) + \
+ sizeof(struct extctx) + \
+ (n) + (PORT_INT_REQUIRED_STACK))
+
+/**
+ * @brief Static working area allocation.
+ * @details This macro is used to allocate a static thread working area
+ * aligned as both position and size.
+ */
+#define WORKING_AREA(s, n) stkalign_t s[THD_WA_SIZE(n) / sizeof(stkalign_t)]
+
+/**
+ * @brief IRQ prologue code.
+ * @details This macro must be inserted at the start of all IRQ handlers
+ * enabled to invoke system APIs.
+ */
+#define PORT_IRQ_PROLOGUE() \
+ regarm_t _saved_lr; \
+ asm volatile ("mov %0, lr" : "=r" (_saved_lr) : : "memory")
+
+/**
+ * @brief IRQ epilogue code.
+ * @details This macro must be inserted at the end of all IRQ handlers
+ * enabled to invoke system APIs.
+ */
+#define PORT_IRQ_EPILOGUE() _port_irq_epilogue(_saved_lr)
+
+/**
+ * @brief IRQ handler function declaration.
+ * @note @p id can be a function name or a vector number depending on the
+ * port implementation.
+ */
+#define PORT_IRQ_HANDLER(id) void id(void)
+
+/**
+ * @brief Fast IRQ handler function declaration.
+ * @note @p id can be a function name or a vector number depending on the
+ * port implementation.
+ */
+#define PORT_FAST_IRQ_HANDLER(id) void id(void)
+
+/**
+ * @brief Port-related initialization code.
+ */
+#define port_init() { \
+ SCB_AIRCR = AIRCR_VECTKEY | AIRCR_PRIGROUP(0); \
+ nvicSetSystemHandlerPriority(HANDLER_PENDSV, \
+ CORTEX_PRIORITY_MASK(CORTEX_PRIORITY_PENDSV)); \
+ nvicSetSystemHandlerPriority(HANDLER_SYSTICK, \
+ CORTEX_PRIORITY_MASK(CORTEX_PRIORITY_SYSTICK)); \
+}
+
+/**
+ * @brief Kernel-lock action.
+ * @details Usually this function just disables interrupts but may perform
+ * more actions.
+ */
+#define port_lock() asm volatile ("cpsid i" : : : "memory")
+
+/**
+ * @brief Kernel-unlock action.
+ * @details Usually this function just enables interrupts but may perform
+ * more actions.
+ */
+#define port_unlock() asm volatile ("cpsie i" : : : "memory")
+
+/**
+ * @brief Kernel-lock action from an interrupt handler.
+ * @details This function is invoked before invoking I-class APIs from
+ * interrupt handlers. The implementation is architecture dependent,
+ * in its simplest form it is void.
+ * @note Same as @p port_lock() in this port.
+ */
+#define port_lock_from_isr() port_lock()
+
+/**
+ * @brief Kernel-unlock action from an interrupt handler.
+ * @details This function is invoked after invoking I-class APIs from interrupt
+ * handlers. The implementation is architecture dependent, in its
+ * simplest form it is void.
+ * @note Same as @p port_lock() in this port.
+ */
+#define port_unlock_from_isr() port_unlock()
+
+/**
+ * @brief Disables all the interrupt sources.
+ */
+#define port_disable() asm volatile ("cpsid i" : : : "memory")
+
+/**
+ * @brief Disables the interrupt sources below kernel-level priority.
+ */
+#define port_suspend() asm volatile ("cpsid i" : : : "memory")
+
+/**
+ * @brief Enables all the interrupt sources.
+ */
+#define port_enable() asm volatile ("cpsie i" : : : "memory")
+
+/**
+ * @brief Enters an architecture-dependent IRQ-waiting mode.
+ * @details The function is meant to return when an interrupt becomes pending.
+ * The simplest implementation is an empty function or macro but this
+ * would not take advantage of architecture-specific power saving
+ * modes.
+ * @note Implemented as an inlined @p WFI instruction.
+ */
+#if CORTEX_ENABLE_WFI_IDLE || defined(__DOXYGEN__)
+#define port_wait_for_interrupt() asm volatile ("wfi" : : : "memory")
+#else
+#define port_wait_for_interrupt()
+#endif
+
+/**
+ * @brief Performs a context switch between two threads.
+ * @details This is the most critical code in any port, this function
+ * is responsible for the context switch between 2 threads.
+ * @note The implementation of this code affects directly the context
+ * switch performance so optimize here as much as you can.
+ *
+ * @param[in] ntp the thread to be switched in
+ * @param[in] otp the thread to be switched out
+ */
+#if !CH_DBG_ENABLE_STACK_CHECK || defined(__DOXYGEN__)
+#define port_switch(ntp, otp) _port_switch(ntp, otp)
+#else
+#define port_switch(ntp, otp) { \
+ register struct intctx *r13 asm ("r13"); \
+ if ((stkalign_t *)(r13 - 1) < otp->p_stklimit) \
+ chDbgPanic("stack overflow"); \
+ _port_switch(ntp, otp); \
+}
+#endif
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+ void port_halt(void);
+ void _port_irq_epilogue(regarm_t lr);
+ void _port_switch_from_isr(void);
+ void _port_exit_from_isr(void);
+ void _port_switch(Thread *ntp, Thread *otp);
+ void _port_thread_start(void);
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* _FROM_ASM_ */
+
+#endif /* _CHCORE_V6M_H_ */
+
+/** @} */