From c2eee2db23c2b8defe5bffc4ebe2126b895aea63 Mon Sep 17 00:00:00 2001
From: mirkix <mirkix@gmail.com>
Date: Thu, 26 Apr 2018 20:56:40 +0200
Subject: [PATCH] AP_HAL_ChibiOS: add STM32F7 I2C support

---
 libraries/AP_HAL_ChibiOS/I2CDevice.cpp | 39 +++++++++++++++++++++++---
 libraries/AP_HAL_ChibiOS/I2CDevice.h   |  1 +
 2 files changed, 36 insertions(+), 4 deletions(-)

diff --git a/libraries/AP_HAL_ChibiOS/I2CDevice.cpp b/libraries/AP_HAL_ChibiOS/I2CDevice.cpp
index 01fec554c1..f12ca01346 100644
--- a/libraries/AP_HAL_ChibiOS/I2CDevice.cpp
+++ b/libraries/AP_HAL_ChibiOS/I2CDevice.cpp
@@ -45,6 +45,10 @@ I2CBus I2CDeviceManager::businfo[ARRAY_SIZE_SIMPLE(I2CD)];
 #define HAL_I2C_MAX_CLOCK 100000
 #endif
 
+// values calculated with STM32CubeMX tool, PCLK=54MHz
+#define HAL_I2C_F7_100_TIMINGR 0x20404768
+#define HAL_I2C_F7_400_TIMINGR 0x6000030D
+
 // get a handle for DMA sharing DMA channels with other subsystems
 void I2CBus::dma_init(void)
 {
@@ -95,13 +99,24 @@ I2CDeviceManager::I2CDeviceManager(void)
           setup default I2C config. As each device is opened we will
           drop the speed to be the minimum speed requested
          */
+        businfo[i].busclock = HAL_I2C_MAX_CLOCK;
+#if defined(STM32F7)
+        if (businfo[i].busclock <= 100000) {
+            businfo[i].i2ccfg.timingr = HAL_I2C_F7_100_TIMINGR;
+            businfo[i].busclock = 100000;
+        } else {
+            businfo[i].i2ccfg.timingr = HAL_I2C_F7_400_TIMINGR;
+            businfo[i].busclock = 400000;
+        }
+#else
         businfo[i].i2ccfg.op_mode = OPMODE_I2C;
-        businfo[i].i2ccfg.clock_speed = HAL_I2C_MAX_CLOCK;
+        businfo[i].i2ccfg.clock_speed = businfo[i].busclock;
         if (businfo[i].i2ccfg.clock_speed <= 100000) {
             businfo[i].i2ccfg.duty_cycle = STD_DUTY_CYCLE;
         } else {
             businfo[i].i2ccfg.duty_cycle = FAST_DUTY_CYCLE_2;
         }
+#endif
     }
 }
 
@@ -116,12 +131,20 @@ I2CDevice::I2CDevice(uint8_t busnum, uint8_t address, uint32_t bus_clock, bool u
     set_device_address(address);
     asprintf(&pname, "I2C:%u:%02x",
              (unsigned)busnum, (unsigned)address);
-    if (bus_clock < bus.i2ccfg.clock_speed) {
+    if (bus_clock < bus.busclock) {
+#if defined(STM32F7)
+        if (bus_clock <= 100000) {
+            bus.i2ccfg.timingr = HAL_I2C_F7_100_TIMINGR;
+            bus.busclock = 100000;
+        }
+#else
         bus.i2ccfg.clock_speed = bus_clock;
-        hal.console->printf("I2C%u clock %ukHz\n", busnum, unsigned(bus_clock/1000));
+        bus.busclock = bus_clock;
         if (bus_clock <= 100000) {
             bus.i2ccfg.duty_cycle = STD_DUTY_CYCLE;
         }
+#endif
+        hal.console->printf("I2C%u clock %ukHz\n", busnum, unsigned(bus.busclock/1000));
     }
 }
 
@@ -168,11 +191,19 @@ bool I2CDevice::transfer(const uint8_t *send, uint32_t send_len,
     
     bus.dma_handle->lock();
 
+#if defined(STM32F7)
+    if (_use_smbus) {
+        bus.i2ccfg.cr1 |= I2C_CR1_SMBHEN;
+    } else {
+        bus.i2ccfg.cr1 &= ~I2C_CR1_SMBHEN;
+    }
+#else
     if (_use_smbus) {
         bus.i2ccfg.op_mode = OPMODE_SMBUS_HOST;
     } else {
         bus.i2ccfg.op_mode = OPMODE_I2C;
     }
+#endif
     
     if (_split_transfers) {
         /*
@@ -217,7 +248,7 @@ bool I2CDevice::_transfer(const uint8_t *send, uint32_t send_len,
     for(uint8_t i=0 ; i <= _retries; i++) {
         int ret;
         // calculate a timeout as twice the expected transfer time, and set as min of 4ms
-        uint32_t timeout_ms = 1+2*(((8*1000000UL/bus.i2ccfg.clock_speed)*MAX(send_len, recv_len))/1000);
+        uint32_t timeout_ms = 1+2*(((8*1000000UL/bus.busclock)*MAX(send_len, recv_len))/1000);
         timeout_ms = MAX(timeout_ms, _timeout_ms);
         bus.i2c_active = true;
         osalDbgAssert(I2CD[bus.busnum].i2c->state == I2C_READY, "i2cStart state");
diff --git a/libraries/AP_HAL_ChibiOS/I2CDevice.h b/libraries/AP_HAL_ChibiOS/I2CDevice.h
index 8f39efda54..39c642d2bf 100644
--- a/libraries/AP_HAL_ChibiOS/I2CDevice.h
+++ b/libraries/AP_HAL_ChibiOS/I2CDevice.h
@@ -36,6 +36,7 @@ class I2CBus : public DeviceBus {
 public:
     I2CConfig i2ccfg;
     uint8_t busnum;
+    uint32_t busclock;
     bool i2c_started;
     bool i2c_active;