From bf2cf7834d0ca91aab0e08d69989c64ec3379697 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Tue, 27 Aug 2019 17:42:51 +1000
Subject: [PATCH] HAL_ChibiOS: fixed a bug in parity/stop-bit setting on
 STM32F7 and H7

and add get_options() method
---
 libraries/AP_HAL_ChibiOS/UARTDriver.cpp | 46 ++++++++++++++++++++++---
 libraries/AP_HAL_ChibiOS/UARTDriver.h   | 10 +++++-
 2 files changed, 51 insertions(+), 5 deletions(-)

diff --git a/libraries/AP_HAL_ChibiOS/UARTDriver.cpp b/libraries/AP_HAL_ChibiOS/UARTDriver.cpp
index e181098ba8..a35172d3a5 100644
--- a/libraries/AP_HAL_ChibiOS/UARTDriver.cpp
+++ b/libraries/AP_HAL_ChibiOS/UARTDriver.cpp
@@ -50,6 +50,9 @@ UARTDriver *UARTDriver::uart_drivers[UART_MAX_DRIVERS];
 // caller threads
 #define EVT_DATA EVENT_MASK(0)
 
+// event for parity error
+#define EVT_PARITY EVENT_MASK(1)
+
 #ifndef HAL_UART_MIN_TX_SIZE
 #define HAL_UART_MIN_TX_SIZE 1024
 #endif
@@ -973,6 +976,12 @@ void UARTDriver::_timer_tick(void)
             if (ret < 0) {
                 break;
             }
+#if CH_CFG_USE_EVENTS == TRUE
+            if (parity_enabled && ((chEvtGetAndClearFlags(&ev_listener) & SD_PARITY_ERROR))) {
+                // discard bytes with parity error
+                ret = -1;
+            }
+#endif
             if (half_duplex) {
                 uint32_t now = AP_HAL::micros();
                 if (now - hd_write_us < hd_read_delay_us) {
@@ -1116,27 +1125,48 @@ void UARTDriver::configure_parity(uint8_t v)
 #if HAL_USE_SERIAL == TRUE
     // stop and start to take effect
     sdStop((SerialDriver*)sdef.serial);
-    
+
+#ifdef USART_CR1_M0
+    // cope with F7 where there are 2 bits in CR1_M
+    const uint32_t cr1_m0 = USART_CR1_M0;
+#else
+    const uint32_t cr1_m0 = USART_CR1_M;
+#endif
+
     switch (v) {
     case 0:
         // no parity
-        sercfg.cr1 &= ~(USART_CR1_PCE | USART_CR1_PS);
+        sercfg.cr1 &= ~(USART_CR1_PCE | USART_CR1_PS | USART_CR1_M);
         break;
     case 1:
         // odd parity
         // setting USART_CR1_M ensures extra bit is used as parity
         // not last bit of data
-        sercfg.cr1 |= USART_CR1_M | USART_CR1_PCE;
+        sercfg.cr1 |= cr1_m0 | USART_CR1_PCE;
         sercfg.cr1 |= USART_CR1_PS;
         break;
     case 2:
         // even parity
-        sercfg.cr1 |= USART_CR1_M | USART_CR1_PCE;
+        sercfg.cr1 |= cr1_m0 | USART_CR1_PCE;
         sercfg.cr1 &= ~USART_CR1_PS;
         break;
     }
 
     sdStart((SerialDriver*)sdef.serial, &sercfg);
+
+#if CH_CFG_USE_EVENTS == TRUE
+    if (parity_enabled) {
+        chEvtUnregister(chnGetEventSource((SerialDriver*)sdef.serial), &ev_listener);
+    }
+    parity_enabled = (v != 0);
+    if (parity_enabled) {
+        chEvtRegisterMaskWithFlags(chnGetEventSource((SerialDriver*)sdef.serial),
+                                   &ev_listener,
+                                   EVT_PARITY,
+                                   SD_PARITY_ERROR);
+    }
+#endif
+    
 #ifndef HAL_UART_NODMA
     if(sdef.dma_rx) {
         //Configure serial driver to skip handling RX packets
@@ -1221,6 +1251,8 @@ bool UARTDriver::set_options(uint8_t options)
     }
     bool ret = true;
 
+    _last_options = options;
+
 #if HAL_USE_SERIAL == TRUE
     SerialDriver *sd = (SerialDriver*)(sdef.serial);
     uint32_t cr2 = sd->usart->CR2;
@@ -1299,4 +1331,10 @@ bool UARTDriver::set_options(uint8_t options)
     return ret;
 }
 
+// get optional features
+uint8_t UARTDriver::get_options(void) const
+{
+    return _last_options;
+}
+
 #endif //CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
diff --git a/libraries/AP_HAL_ChibiOS/UARTDriver.h b/libraries/AP_HAL_ChibiOS/UARTDriver.h
index fe75b78636..0e921b066f 100644
--- a/libraries/AP_HAL_ChibiOS/UARTDriver.h
+++ b/libraries/AP_HAL_ChibiOS/UARTDriver.h
@@ -55,7 +55,8 @@ public:
 
     // control optional features
     bool set_options(uint8_t options) override;
-
+    uint8_t get_options(void) const override;
+    
     // write to a locked port. If port is locked and key is not correct then 0 is returned
     // and write is discarded
     size_t write_locked(const uint8_t *buffer, size_t size, uint32_t key) override;
@@ -183,6 +184,7 @@ private:
     // we remember cr2 and cr2 options from set_options to apply on sdStart()
     uint32_t _cr3_options;
     uint32_t _cr2_options;
+    uint8_t _last_options;
 
     // half duplex control. After writing we throw away bytes for 4 byte widths to
     // prevent reading our own bytes back
@@ -193,6 +195,12 @@ private:
 
     // set to true for unbuffered writes (low latency writes)
     bool unbuffered_writes;
+
+#if CH_CFG_USE_EVENTS == TRUE
+    // listener for parity error events
+    event_listener_t ev_listener;
+    bool parity_enabled;
+#endif
     
 #ifndef HAL_UART_NODMA
     static void rx_irq_cb(void* sd);