From 9f7aa715b1b45f632e86d444ee0491ab9f0c25d4 Mon Sep 17 00:00:00 2001
From: Nathan Costa <nathan.costa@hiteccs.com>
Date: Mon, 4 Oct 2021 09:53:26 -0700
Subject: [PATCH] AP_HAL_ChibiOS: add support for CAN/I2C switch

---
 libraries/AP_HAL_ChibiOS/I2CDevice.cpp        | 39 +++++++++++++++++++
 libraries/AP_HAL_ChibiOS/I2CDevice.h          |  2 +
 .../hwdef/Hitec-Airspeed/hwdef.dat            |  6 +++
 3 files changed, 47 insertions(+)

diff --git a/libraries/AP_HAL_ChibiOS/I2CDevice.cpp b/libraries/AP_HAL_ChibiOS/I2CDevice.cpp
index c64e107705..18149b8fc1 100644
--- a/libraries/AP_HAL_ChibiOS/I2CDevice.cpp
+++ b/libraries/AP_HAL_ChibiOS/I2CDevice.cpp
@@ -105,6 +105,45 @@ void I2CBus::clear_all()
     }
 }
 
+/*
+  If bus exists, set its data and clock lines to floating
+ */
+void I2CBus::set_bus_to_floating(uint8_t busidx)
+{
+    if (busidx < ARRAY_SIZE(I2CD)) {
+        const struct I2CInfo &info = I2CD[busidx];
+        const ioline_t sda_line = GPIO::resolve_alt_config(info.sda_line, PERIPH_TYPE::I2C_SDA, info.instance);
+        const ioline_t scl_line = GPIO::resolve_alt_config(info.scl_line, PERIPH_TYPE::I2C_SCL, info.instance);
+        palSetLineMode(sda_line, PAL_MODE_INPUT);
+        palSetLineMode(scl_line, PAL_MODE_INPUT);
+    }
+}
+
+
+/*
+  Check enabled I2C/CAN select pins against check_pins bitmask
+ */
+bool I2CBus::check_select_pins(uint8_t check_pins)
+{
+    uint8_t enabled_pins = 0;
+
+#ifdef HAL_GPIO_PIN_GPIO_CAN_I2C1_SEL
+    enabled_pins |= palReadLine(HAL_GPIO_PIN_GPIO_CAN_I2C1_SEL) << 0;
+#endif
+#ifdef HAL_GPIO_PIN_GPIO_CAN_I2C2_SEL
+    enabled_pins |= palReadLine(HAL_GPIO_PIN_GPIO_CAN_I2C2_SEL) << 1;
+#endif
+#ifdef HAL_GPIO_PIN_GPIO_CAN_I2C3_SEL
+    enabled_pins |= palReadLine(HAL_GPIO_PIN_GPIO_CAN_I2C3_SEL) << 2;
+#endif
+#ifdef HAL_GPIO_PIN_GPIO_CAN_I2C4_SEL
+    enabled_pins |= palReadLine(HAL_GPIO_PIN_GPIO_CAN_I2C4_SEL) << 3;
+#endif
+
+    return (enabled_pins & check_pins) == check_pins;
+}
+
+
 /*
   clear a stuck bus (bus held by a device that is holding SDA low) by
   clocking out pulses on SCL to let the device complete its
diff --git a/libraries/AP_HAL_ChibiOS/I2CDevice.h b/libraries/AP_HAL_ChibiOS/I2CDevice.h
index 72580d7a88..143fe29f1d 100644
--- a/libraries/AP_HAL_ChibiOS/I2CDevice.h
+++ b/libraries/AP_HAL_ChibiOS/I2CDevice.h
@@ -52,6 +52,8 @@ public:
     static void clear_all(void);
     static void clear_bus(uint8_t busidx);
     static uint8_t read_sda(uint8_t busidx);
+    static bool check_select_pins(uint8_t check_pins);
+    static void set_bus_to_floating(uint8_t busidx);
 };
 
 class I2CDevice : public AP_HAL::I2CDevice {
diff --git a/libraries/AP_HAL_ChibiOS/hwdef/Hitec-Airspeed/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/Hitec-Airspeed/hwdef.dat
index 6414288e66..b5e5496614 100644
--- a/libraries/AP_HAL_ChibiOS/hwdef/Hitec-Airspeed/hwdef.dat
+++ b/libraries/AP_HAL_ChibiOS/hwdef/Hitec-Airspeed/hwdef.dat
@@ -39,6 +39,9 @@ PB7 USART1_RX USART1 SPEED_HIGH
 # LEDs
 PA10 LED OUTPUT LOW
 
+# a CAN/I2C selection LED
+PA5 LED_CAN_I2C OUTPUT LOW
+
 # a fault LED
 PA6 LED_FAULT OUTPUT LOW
 
@@ -70,6 +73,9 @@ define HAL_DISABLE_LOOP_DELAY
 PA11 CAN1_RX CAN1
 PA12 CAN1_TX CAN1
 
+# GPIO for CAN/I2C selection
+PB5 GPIO_CAN_I2C1_SEL INPUT PUSHPULL FLOATING
+
 # only one I2C bus in normal config
 PA15 I2C1_SCL I2C1
 PB9 I2C1_SDA I2C1