From a0ed658fcc96ec82f151dda1571ca0aaa90ac354 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Thu, 15 Nov 2018 21:15:57 +1100
Subject: [PATCH] HAL_ChibiOS: added palReadLineMode() function

used to change and restore line modes
---
 .../AP_HAL_ChibiOS/hwdef/common/stm32_util.c  | 23 +++++++++++++++++++
 .../AP_HAL_ChibiOS/hwdef/common/stm32_util.h  | 10 +++++++-
 2 files changed, 32 insertions(+), 1 deletion(-)

diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/stm32_util.c b/libraries/AP_HAL_ChibiOS/hwdef/common/stm32_util.c
index 9b29ecb0cf..15bf86891e 100644
--- a/libraries/AP_HAL_ChibiOS/hwdef/common/stm32_util.c
+++ b/libraries/AP_HAL_ChibiOS/hwdef/common/stm32_util.c
@@ -279,3 +279,26 @@ void peripheral_power_enable(void)
 #endif
 #endif
 }
+
+#if defined(STM32F7) || defined(STM32F4)
+/*
+  read mode of a pin. This allows a pin config to be read, changed and
+  then written back
+ */
+iomode_t palReadLineMode(ioline_t line)
+{
+    ioportid_t port = PAL_PORT(line);
+    uint8_t pad = PAL_PAD(line);
+    iomode_t ret = 0;
+    ret |= (port->MODER >> (pad*2)) & 0x3;
+    ret |= ((port->OTYPER >> pad)&1) << 2;
+    ret |= ((port->OSPEEDR >> (pad*2))&3) << 3;
+    ret |= ((port->PUPDR >> (pad*2))&3) << 5;
+    if (pad < 8) {
+        ret |= ((port->AFRL >> (pad*4))&0xF) << 7;
+    } else {
+        ret |= ((port->AFRH >> ((pad-8)*4))&0xF) << 7;
+    }
+    return ret;
+}
+#endif
diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/stm32_util.h b/libraries/AP_HAL_ChibiOS/hwdef/common/stm32_util.h
index ac2f1092e8..14754f773f 100644
--- a/libraries/AP_HAL_ChibiOS/hwdef/common/stm32_util.h
+++ b/libraries/AP_HAL_ChibiOS/hwdef/common/stm32_util.h
@@ -69,7 +69,15 @@ void peripheral_power_enable(void);
 
 // initialise allocation subsystem
 void malloc_init(void);
-    
+
+/*
+  read mode of a pin. This allows a pin config to be read, changed and
+  then written back
+ */
+#if defined(STM32F7) || defined(STM32F4)
+iomode_t palReadLineMode(ioline_t line);
+#endif
+
 #ifdef __cplusplus
 }
 #endif