From 44fdd0b4ab182525db35a28f20f67512bb80e535 Mon Sep 17 00:00:00 2001
From: Rhys Mainwaring <rhys.mainwaring@me.com>
Date: Sat, 4 Jan 2025 14:09:52 +0000
Subject: [PATCH] AP_HAL_ESP32: check UART thread ownership

Signed-off-by: Rhys Mainwaring <rhys.mainwaring@me.com>
---
 libraries/AP_HAL_ESP32/UARTDriver.cpp | 26 ++++++++++++++++++++++----
 libraries/AP_HAL_ESP32/UARTDriver.h   |  5 +++++
 2 files changed, 27 insertions(+), 4 deletions(-)

diff --git a/libraries/AP_HAL_ESP32/UARTDriver.cpp b/libraries/AP_HAL_ESP32/UARTDriver.cpp
index 90befdb6cc..7d59e3b2c1 100644
--- a/libraries/AP_HAL_ESP32/UARTDriver.cpp
+++ b/libraries/AP_HAL_ESP32/UARTDriver.cpp
@@ -38,6 +38,12 @@ void UARTDriver::vprintf(const char *fmt, va_list ap)
 
 void UARTDriver::_begin(uint32_t b, uint16_t rxS, uint16_t txS)
 {
+    if (b == 0 && txS == 0 && rxS == 0 && _initialized) {
+        // the thread owning this port has changed
+        _uart_owner_thd = xTaskGetCurrentTaskHandle();
+        return;
+    }
+
     if (uart_num < ARRAY_SIZE(uart_desc)) {
         uart_port_t p = uart_desc[uart_num].port;
         if (!_initialized) {
@@ -58,6 +64,7 @@ void UARTDriver::_begin(uint32_t b, uint16_t rxS, uint16_t txS)
             uart_driver_install(p, 2*UART_HW_FIFO_LEN(p), 0, 0, nullptr, 0);
             _readbuf.set_size(RX_BUF_SIZE);
             _writebuf.set_size(TX_BUF_SIZE);
+            _uart_owner_thd = xTaskGetCurrentTaskHandle();
 
             _initialized = true;
         } else {
@@ -98,7 +105,7 @@ bool UARTDriver::tx_pending()
 
 uint32_t UARTDriver::_available()
 {
-    if (!_initialized) {
+    if (!_initialized || _uart_owner_thd != xTaskGetCurrentTaskHandle()) {
         return 0;
     }
     return _readbuf.available();
@@ -117,6 +124,10 @@ uint32_t UARTDriver::txspace()
 
 ssize_t IRAM_ATTR UARTDriver::_read(uint8_t *buffer, uint16_t count)
 {
+    if (_uart_owner_thd != xTaskGetCurrentTaskHandle()) {
+        return -1;
+    }
+
     if (!_initialized) {
         return -1;
     }
@@ -184,9 +195,16 @@ size_t IRAM_ATTR UARTDriver::_write(const uint8_t *buffer, size_t size)
 
 bool UARTDriver::_discard_input()
 {
-    //uart_port_t p = uart_desc[uart_num].port;
-    //return uart_flush_input(p) == ESP_OK;
-    return false;
+    if (_uart_owner_thd != xTaskGetCurrentTaskHandle()) {
+        return false;
+    }
+    if (!_initialized) {
+        return false;
+    }
+
+    _readbuf.clear();
+
+    return true;
 }
 
 // record timestamp of new incoming data
diff --git a/libraries/AP_HAL_ESP32/UARTDriver.h b/libraries/AP_HAL_ESP32/UARTDriver.h
index 2ec490f0a0..c05d355752 100644
--- a/libraries/AP_HAL_ESP32/UARTDriver.h
+++ b/libraries/AP_HAL_ESP32/UARTDriver.h
@@ -23,6 +23,9 @@
 #include "driver/gpio.h"
 #include "driver/uart.h"
 
+#include "freertos/FreeRTOS.h"
+#include "freertos/task.h"
+
 namespace ESP32
 {
 
@@ -92,6 +95,8 @@ private:
     uint8_t _receive_timestamp_idx;
     uint32_t _baudrate;
 
+    const tskTaskControlBlock* _uart_owner_thd;
+
     void _receive_timestamp_update(void);
 
 protected: