diff --git a/make/mcu/STM32F7.mk b/make/mcu/STM32F7.mk
index 30a55bc30..29fcfda10 100644
--- a/make/mcu/STM32F7.mk
+++ b/make/mcu/STM32F7.mk
@@ -171,6 +171,7 @@ MCU_COMMON_SRC = \
drivers/adc_stm32f7xx.c \
drivers/audio_stm32f7xx.c \
drivers/bus_i2c_hal.c \
+ drivers/bus_i2c_hal_init.c \
drivers/dma_stm32f7xx.c \
drivers/light_ws2811strip_hal.c \
drivers/transponder_ir_io_hal.c \
diff --git a/make/mcu/STM32H7.mk b/make/mcu/STM32H7.mk
index 48b56a66a..0c766e58b 100644
--- a/make/mcu/STM32H7.mk
+++ b/make/mcu/STM32H7.mk
@@ -242,6 +242,7 @@ MCU_COMMON_SRC = \
drivers/light_ws2811strip_hal.c \
drivers/adc_stm32h7xx.c \
drivers/bus_i2c_hal.c \
+ drivers/bus_i2c_hal_init.c \
drivers/pwm_output_dshot_hal.c \
drivers/pwm_output_dshot_shared.c \
drivers/persistent.c \
diff --git a/make/source.mk b/make/source.mk
index aeb306868..abc8b833d 100644
--- a/make/source.mk
+++ b/make/source.mk
@@ -359,6 +359,9 @@ SPEED_OPTIMISED_SRC := $(SPEED_OPTIMISED_SRC) \
drivers/pwm_output_dshot.c \
drivers/pwm_output_dshot_shared.c \
drivers/pwm_output_dshot_hal.c
+
+SIZE_OPTIMISED_SRC := $(SIZE_OPTIMISED_SRC) \
+ drivers/bus_i2c_hal_init.c
endif #!F3
endif #!F1
diff --git a/src/main/drivers/bus_i2c_hal.c b/src/main/drivers/bus_i2c_hal.c
index dbfdc41e2..4bb46c6a4 100644
--- a/src/main/drivers/bus_i2c_hal.c
+++ b/src/main/drivers/bus_i2c_hal.c
@@ -35,122 +35,6 @@
#include "drivers/bus_i2c.h"
#include "drivers/bus_i2c_impl.h"
-#define CLOCKSPEED 800000 // i2c clockspeed 400kHz default (conform specs), 800kHz and 1200kHz (Betaflight default)
-
-// Number of bits in I2C protocol phase
-#define LEN_ADDR 7
-#define LEN_RW 1
-#define LEN_ACK 1
-
-// Clock period in us during unstick transfer
-#define UNSTICK_CLK_US 10
-
-// Allow 500us for clock strech to complete during unstick
-#define UNSTICK_CLK_STRETCH (500/UNSTICK_CLK_US)
-
-static void i2cUnstick(IO_t scl, IO_t sda);
-
-#define IOCFG_I2C_PU IO_CONFIG(GPIO_MODE_AF_OD, GPIO_SPEED_FREQ_VERY_HIGH, GPIO_PULLUP)
-#define IOCFG_I2C IO_CONFIG(GPIO_MODE_AF_OD, GPIO_SPEED_FREQ_VERY_HIGH, GPIO_NOPULL)
-
-#define GPIO_AF4_I2C GPIO_AF4_I2C1
-
-const i2cHardware_t i2cHardware[I2CDEV_COUNT] = {
-#if defined(STM32F7)
-#ifdef USE_I2C_DEVICE_1
- {
- .device = I2CDEV_1,
- .reg = I2C1,
- .sclPins = { I2CPINDEF(PB6), I2CPINDEF(PB8) },
- .sdaPins = { I2CPINDEF(PB7), I2CPINDEF(PB9) },
- .rcc = RCC_APB1(I2C1),
- .ev_irq = I2C1_EV_IRQn,
- .er_irq = I2C1_ER_IRQn,
- },
-#endif
-#ifdef USE_I2C_DEVICE_2
- {
- .device = I2CDEV_2,
- .reg = I2C2,
- .sclPins = { I2CPINDEF(PB10), I2CPINDEF(PF1) },
- .sdaPins = { I2CPINDEF(PB11), I2CPINDEF(PF0) },
- .rcc = RCC_APB1(I2C2),
- .ev_irq = I2C2_EV_IRQn,
- .er_irq = I2C2_ER_IRQn,
- },
-#endif
-#ifdef USE_I2C_DEVICE_3
- {
- .device = I2CDEV_3,
- .reg = I2C3,
- .sclPins = { I2CPINDEF(PA8) },
- .sdaPins = { I2CPINDEF(PC9) },
- .rcc = RCC_APB1(I2C3),
- .ev_irq = I2C3_EV_IRQn,
- .er_irq = I2C3_ER_IRQn,
- },
-#endif
-#ifdef USE_I2C_DEVICE_4
- {
- .device = I2CDEV_4,
- .reg = I2C4,
- .sclPins = { I2CPINDEF(PD12), I2CPINDEF(PF14) },
- .sdaPins = { I2CPINDEF(PD13), I2CPINDEF(PF15) },
- .rcc = RCC_APB1(I2C4),
- .ev_irq = I2C4_EV_IRQn,
- .er_irq = I2C4_ER_IRQn,
- },
-#endif
-#elif defined(STM32H7)
-#ifdef USE_I2C_DEVICE_1
- {
- .device = I2CDEV_1,
- .reg = I2C1,
- .sclPins = { I2CPINDEF(PB6, GPIO_AF4_I2C1), I2CPINDEF(PB8, GPIO_AF4_I2C1) },
- .sdaPins = { I2CPINDEF(PB7, GPIO_AF4_I2C1), I2CPINDEF(PB9, GPIO_AF4_I2C1) },
- .rcc = RCC_APB1L(I2C1),
- .ev_irq = I2C1_EV_IRQn,
- .er_irq = I2C1_ER_IRQn,
- },
-#endif
-#ifdef USE_I2C_DEVICE_2
- {
- .device = I2CDEV_2,
- .reg = I2C2,
- .sclPins = { I2CPINDEF(PB10, GPIO_AF4_I2C2), I2CPINDEF(PF1, GPIO_AF4_I2C2) },
- .sdaPins = { I2CPINDEF(PB11, GPIO_AF4_I2C2), I2CPINDEF(PF0, GPIO_AF4_I2C2) },
- .rcc = RCC_APB1L(I2C2),
- .ev_irq = I2C2_EV_IRQn,
- .er_irq = I2C2_ER_IRQn,
- },
-#endif
-#ifdef USE_I2C_DEVICE_3
- {
- .device = I2CDEV_3,
- .reg = I2C3,
- .sclPins = { I2CPINDEF(PA8, GPIO_AF4_I2C3) },
- .sdaPins = { I2CPINDEF(PC9, GPIO_AF4_I2C3) },
- .rcc = RCC_APB1L(I2C3),
- .ev_irq = I2C3_EV_IRQn,
- .er_irq = I2C3_ER_IRQn,
- },
-#endif
-#ifdef USE_I2C_DEVICE_4
- {
- .device = I2CDEV_4,
- .reg = I2C4,
- .sclPins = { I2CPINDEF(PD12, GPIO_AF4_I2C4), I2CPINDEF(PF14, GPIO_AF4_I2C4), I2CPINDEF(PB6, GPIO_AF6_I2C4), I2CPINDEF(PB8, GPIO_AF6_I2C4) },
- .sdaPins = { I2CPINDEF(PD13, GPIO_AF4_I2C4), I2CPINDEF(PF15, GPIO_AF4_I2C4), I2CPINDEF(PB7, GPIO_AF6_I2C4), I2CPINDEF(PB9, GPIO_AF6_I2C4) },
- .rcc = RCC_APB4(I2C4),
- .ev_irq = I2C4_EV_IRQn,
- .er_irq = I2C4_ER_IRQn,
- },
-#endif
-#endif
-};
-
-i2cDevice_t i2cDevice[I2CDEV_COUNT];
-
#ifdef USE_I2C_DEVICE_1
void I2C1_ER_IRQHandler(void)
{
@@ -210,6 +94,11 @@ static bool i2cHandleHardwareFailure(I2CDevice device)
return false;
}
+uint16_t i2cGetErrorCounter(void)
+{
+ return i2cErrorCount;
+}
+
// Blocking write
bool i2cWrite(I2CDevice device, uint8_t addr_, uint8_t reg_, uint8_t data)
{
@@ -341,124 +230,4 @@ bool i2cBusy(I2CDevice device, bool *error)
return true;
}
-void i2cInit(I2CDevice device)
-{
- if (device == I2CINVALID) {
- return;
- }
-
- i2cDevice_t *pDev = &i2cDevice[device];
-
- const i2cHardware_t *hardware = pDev->hardware;
-
- if (!hardware) {
- return;
- }
-
- IO_t scl = pDev->scl;
- IO_t sda = pDev->sda;
-
- RCC_ClockCmd(hardware->rcc, ENABLE);
-
- IOInit(scl, OWNER_I2C_SCL, RESOURCE_INDEX(device));
- IOInit(sda, OWNER_I2C_SDA, RESOURCE_INDEX(device));
-
- // Enable RCC
- RCC_ClockCmd(hardware->rcc, ENABLE);
-
- i2cUnstick(scl, sda);
-
- // Init pins
-#if defined(STM32F7)
- IOConfigGPIOAF(scl, pDev->pullUp ? IOCFG_I2C_PU : IOCFG_I2C, GPIO_AF4_I2C);
- IOConfigGPIOAF(sda, pDev->pullUp ? IOCFG_I2C_PU : IOCFG_I2C, GPIO_AF4_I2C);
-#elif defined(STM32H7)
- IOConfigGPIOAF(scl, pDev->pullUp ? IOCFG_I2C_PU : IOCFG_I2C, pDev->sclAF);
- IOConfigGPIOAF(sda, pDev->pullUp ? IOCFG_I2C_PU : IOCFG_I2C, pDev->sdaAF);
-#else
- IOConfigGPIO(scl, IOCFG_AF_OD);
- IOConfigGPIO(sda, IOCFG_AF_OD);
-#endif
-
- // Init I2C peripheral
-
- I2C_HandleTypeDef *pHandle = &pDev->handle;
-
- memset(pHandle, 0, sizeof(*pHandle));
-
- pHandle->Instance = pDev->hardware->reg;
-
- /// TODO: HAL check if I2C timing is correct
-
- if (pDev->overClock) {
- // 800khz Maximum speed tested on various boards without issues
- pHandle->Init.Timing = 0x00500D1D;
- } else {
- pHandle->Init.Timing = 0x00500C6F;
- }
-
- pHandle->Init.OwnAddress1 = 0x0;
- pHandle->Init.AddressingMode = I2C_ADDRESSINGMODE_7BIT;
- pHandle->Init.DualAddressMode = I2C_DUALADDRESS_DISABLE;
- pHandle->Init.OwnAddress2 = 0x0;
- pHandle->Init.GeneralCallMode = I2C_GENERALCALL_DISABLE;
- pHandle->Init.NoStretchMode = I2C_NOSTRETCH_DISABLE;
-
- HAL_I2C_Init(pHandle);
-
- // Enable the Analog I2C Filter
- HAL_I2CEx_ConfigAnalogFilter(pHandle, I2C_ANALOGFILTER_ENABLE);
-
- // Setup interrupt handlers
- HAL_NVIC_SetPriority(hardware->er_irq, NVIC_PRIORITY_BASE(NVIC_PRIO_I2C_ER), NVIC_PRIORITY_SUB(NVIC_PRIO_I2C_ER));
- HAL_NVIC_EnableIRQ(hardware->er_irq);
- HAL_NVIC_SetPriority(hardware->ev_irq, NVIC_PRIORITY_BASE(NVIC_PRIO_I2C_EV), NVIC_PRIORITY_SUB(NVIC_PRIO_I2C_EV));
- HAL_NVIC_EnableIRQ(hardware->ev_irq);
-}
-
-uint16_t i2cGetErrorCounter(void)
-{
- return i2cErrorCount;
-}
-
-static void i2cUnstick(IO_t scl, IO_t sda)
-{
- int i;
-
- IOHi(scl);
- IOHi(sda);
-
- IOConfigGPIO(scl, IOCFG_OUT_OD);
- IOConfigGPIO(sda, IOCFG_OUT_OD);
-
- // Clock out, with SDA high:
- // 7 data bits
- // 1 READ bit
- // 1 cycle for the ACK
- for (i = 0; i < (LEN_ADDR + LEN_RW + LEN_ACK); i++) {
- // Wait for any clock stretching to finish
- int timeout = UNSTICK_CLK_STRETCH;
- while (!IORead(scl) && timeout) {
- delayMicroseconds(UNSTICK_CLK_US);
- timeout--;
- }
-
- // Pull low
- IOLo(scl); // Set bus low
- delayMicroseconds(UNSTICK_CLK_US/2);
- IOHi(scl); // Set bus high
- delayMicroseconds(UNSTICK_CLK_US/2);
- }
-
- // Generate a stop condition in case there was none
- IOLo(scl);
- delayMicroseconds(UNSTICK_CLK_US/2);
- IOLo(sda);
- delayMicroseconds(UNSTICK_CLK_US/2);
-
- IOHi(scl); // Set bus scl high
- delayMicroseconds(UNSTICK_CLK_US/2);
- IOHi(sda); // Set bus sda high
-}
-
#endif
diff --git a/src/main/drivers/bus_i2c_hal_init.c b/src/main/drivers/bus_i2c_hal_init.c
new file mode 100644
index 000000000..5a39e4f87
--- /dev/null
+++ b/src/main/drivers/bus_i2c_hal_init.c
@@ -0,0 +1,361 @@
+/*
+ * This file is part of Cleanflight and Betaflight.
+ *
+ * Cleanflight and Betaflight are free software. You can redistribute
+ * this software and/or modify this software under the terms of the
+ * GNU General Public License as published by the Free Software
+ * Foundation, either version 3 of the License, or (at your option)
+ * any later version.
+ *
+ * Cleanflight and Betaflight are distributed in the hope that they
+ * will be useful, but WITHOUT ANY WARRANTY; without even the implied
+ * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ * See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this software.
+ *
+ * If not, see .
+ */
+
+#include
+#include
+#include
+
+#include "platform.h"
+
+#if defined(USE_I2C) && !defined(SOFT_I2C)
+
+#include "drivers/io.h"
+#include "drivers/io_impl.h"
+#include "drivers/nvic.h"
+#include "drivers/time.h"
+#include "drivers/rcc.h"
+
+#include "drivers/bus_i2c.h"
+#include "drivers/bus_i2c_impl.h"
+
+// Number of bits in I2C protocol phase
+#define LEN_ADDR 7
+#define LEN_RW 1
+#define LEN_ACK 1
+
+// Clock period in us during unstick transfer
+#define UNSTICK_CLK_US 10
+
+// Allow 500us for clock strech to complete during unstick
+#define UNSTICK_CLK_STRETCH (500/UNSTICK_CLK_US)
+
+static void i2cUnstick(IO_t scl, IO_t sda);
+
+#define IOCFG_I2C_PU IO_CONFIG(GPIO_MODE_AF_OD, GPIO_SPEED_FREQ_VERY_HIGH, GPIO_PULLUP)
+#define IOCFG_I2C IO_CONFIG(GPIO_MODE_AF_OD, GPIO_SPEED_FREQ_VERY_HIGH, GPIO_NOPULL)
+
+#define GPIO_AF4_I2C GPIO_AF4_I2C1
+
+const i2cHardware_t i2cHardware[I2CDEV_COUNT] = {
+#if defined(STM32F7)
+#ifdef USE_I2C_DEVICE_1
+ {
+ .device = I2CDEV_1,
+ .reg = I2C1,
+ .sclPins = { I2CPINDEF(PB6), I2CPINDEF(PB8) },
+ .sdaPins = { I2CPINDEF(PB7), I2CPINDEF(PB9) },
+ .rcc = RCC_APB1(I2C1),
+ .ev_irq = I2C1_EV_IRQn,
+ .er_irq = I2C1_ER_IRQn,
+ },
+#endif
+#ifdef USE_I2C_DEVICE_2
+ {
+ .device = I2CDEV_2,
+ .reg = I2C2,
+ .sclPins = { I2CPINDEF(PB10), I2CPINDEF(PF1) },
+ .sdaPins = { I2CPINDEF(PB11), I2CPINDEF(PF0) },
+ .rcc = RCC_APB1(I2C2),
+ .ev_irq = I2C2_EV_IRQn,
+ .er_irq = I2C2_ER_IRQn,
+ },
+#endif
+#ifdef USE_I2C_DEVICE_3
+ {
+ .device = I2CDEV_3,
+ .reg = I2C3,
+ .sclPins = { I2CPINDEF(PA8) },
+ .sdaPins = { I2CPINDEF(PC9) },
+ .rcc = RCC_APB1(I2C3),
+ .ev_irq = I2C3_EV_IRQn,
+ .er_irq = I2C3_ER_IRQn,
+ },
+#endif
+#ifdef USE_I2C_DEVICE_4
+ {
+ .device = I2CDEV_4,
+ .reg = I2C4,
+ .sclPins = { I2CPINDEF(PD12), I2CPINDEF(PF14) },
+ .sdaPins = { I2CPINDEF(PD13), I2CPINDEF(PF15) },
+ .rcc = RCC_APB1(I2C4),
+ .ev_irq = I2C4_EV_IRQn,
+ .er_irq = I2C4_ER_IRQn,
+ },
+#endif
+#elif defined(STM32H7)
+#ifdef USE_I2C_DEVICE_1
+ {
+ .device = I2CDEV_1,
+ .reg = I2C1,
+ .sclPins = { I2CPINDEF(PB6, GPIO_AF4_I2C1), I2CPINDEF(PB8, GPIO_AF4_I2C1) },
+ .sdaPins = { I2CPINDEF(PB7, GPIO_AF4_I2C1), I2CPINDEF(PB9, GPIO_AF4_I2C1) },
+ .rcc = RCC_APB1L(I2C1),
+ .ev_irq = I2C1_EV_IRQn,
+ .er_irq = I2C1_ER_IRQn,
+ },
+#endif
+#ifdef USE_I2C_DEVICE_2
+ {
+ .device = I2CDEV_2,
+ .reg = I2C2,
+ .sclPins = { I2CPINDEF(PB10, GPIO_AF4_I2C2), I2CPINDEF(PF1, GPIO_AF4_I2C2) },
+ .sdaPins = { I2CPINDEF(PB11, GPIO_AF4_I2C2), I2CPINDEF(PF0, GPIO_AF4_I2C2) },
+ .rcc = RCC_APB1L(I2C2),
+ .ev_irq = I2C2_EV_IRQn,
+ .er_irq = I2C2_ER_IRQn,
+ },
+#endif
+#ifdef USE_I2C_DEVICE_3
+ {
+ .device = I2CDEV_3,
+ .reg = I2C3,
+ .sclPins = { I2CPINDEF(PA8, GPIO_AF4_I2C3) },
+ .sdaPins = { I2CPINDEF(PC9, GPIO_AF4_I2C3) },
+ .rcc = RCC_APB1L(I2C3),
+ .ev_irq = I2C3_EV_IRQn,
+ .er_irq = I2C3_ER_IRQn,
+ },
+#endif
+#ifdef USE_I2C_DEVICE_4
+ {
+ .device = I2CDEV_4,
+ .reg = I2C4,
+ .sclPins = { I2CPINDEF(PD12, GPIO_AF4_I2C4), I2CPINDEF(PF14, GPIO_AF4_I2C4), I2CPINDEF(PB6, GPIO_AF6_I2C4), I2CPINDEF(PB8, GPIO_AF6_I2C4) },
+ .sdaPins = { I2CPINDEF(PD13, GPIO_AF4_I2C4), I2CPINDEF(PF15, GPIO_AF4_I2C4), I2CPINDEF(PB7, GPIO_AF6_I2C4), I2CPINDEF(PB9, GPIO_AF6_I2C4) },
+ .rcc = RCC_APB4(I2C4),
+ .ev_irq = I2C4_EV_IRQn,
+ .er_irq = I2C4_ER_IRQn,
+ },
+#endif
+#endif
+};
+
+i2cDevice_t i2cDevice[I2CDEV_COUNT];
+
+// Values from I2C-SMBus specification
+static uint16_t trmax; // Raise time (max)
+static uint16_t tfmax; // Fall time (max)
+static uint8_t tsuDATmin; // SDA setup time (min)
+static uint8_t thdDATmin; // SDA hold time (min)
+
+// Silicon specific values, from datasheet
+static uint8_t tAFmin; // Analog filter delay (min)
+static uint8_t tAFmax; // Analog filter delay (max)
+
+// Actual (estimated) values
+static uint16_t tr = 100; // Raise time
+static uint16_t tf = 100; // Fall time
+static uint8_t tAF = 70; // Analog filter delay
+
+/*
+ * Compute SCLDEL, SDADEL, SCLH and SCLL for TIMINGR register according to reference manuals.
+ */
+static void i2cClockComputeRaw(uint32_t pclkFreq, int i2cFreqKhz, int presc, int dfcoeff,
+ uint8_t *scldel, uint8_t *sdadel, uint16_t *sclh, uint16_t *scll)
+{
+ if (i2cFreqKhz > 400) {
+ // Fm+ (Fast mode plus)
+ trmax = 120;
+ tfmax = 120;
+ tsuDATmin = 50;
+ thdDATmin = 0;
+ } else {
+ // Fm (Fast mode)
+ trmax = 300;
+ tfmax = 300;
+ tsuDATmin = 100;
+ thdDATmin = 0;
+ }
+ tAFmin = 50;
+ tAFmax = 90;
+
+ // Convert pclkFreq into nsec
+ float tI2cclk = 1000000000.0f / pclkFreq;
+
+ // Convert target i2cFreq into cycle time (nsec)
+ float tSCL = 1000000.0f / i2cFreqKhz;
+
+ uint32_t SCLDELmin = (trmax + tsuDATmin)/((presc + 1) * tI2cclk) - 1;
+
+ uint32_t SDADELmin = (tfmax + thdDATmin - tAFmin - ((dfcoeff + 3) * tI2cclk)) / ((presc + 1) * tI2cclk);
+
+ float tsync1 = tf + tAF + dfcoeff * tI2cclk + 3 * tI2cclk;
+ float tsync2 = tr + tAF + dfcoeff * tI2cclk + 3 * tI2cclk;
+
+ float tSCLHL = tSCL - tsync1 - tsync2;
+ float SCLHL = tSCLHL / ((presc + 1) * tI2cclk) - 1;
+
+ uint32_t SCLH = SCLHL / 4.75; // STM32CubeMX seems to use a value like this
+ uint32_t SCLL = (uint32_t)(SCLHL + 0.5f) - SCLH;
+
+ *scldel = SCLDELmin;
+ *sdadel = SDADELmin;
+ *sclh = SCLH - 1;
+ *scll = SCLL - 1;
+}
+
+static uint32_t i2cClockTIMINGR(uint32_t pclkFreq, int i2cFreqKhz, int dfcoeff)
+{
+#define TIMINGR(presc, scldel, sdadel, sclh, scll) \
+ ((presc << 28)|(scldel << 20)|(sdadel << 16)|(sclh << 8)|(scll << 0))
+
+ uint8_t scldel;
+ uint8_t sdadel;
+ uint16_t sclh;
+ uint16_t scll;
+
+ for (int presc = 1; presc < 15; presc++) {
+ i2cClockComputeRaw(pclkFreq, i2cFreqKhz, presc, dfcoeff, &scldel, &sdadel, &sclh, &scll);
+
+ // If all fields are not overflowing, return TIMINGR.
+ // Otherwise, increase prescaler and try again.
+ if ((scldel < 16) && (sdadel < 16) && (sclh < 256) && (scll < 256)) {
+ return TIMINGR(presc, scldel, sdadel, sclh, scll);
+ }
+ }
+ return 0; // Shouldn't reach here
+}
+
+void i2cInit(I2CDevice device)
+{
+ if (device == I2CINVALID) {
+ return;
+ }
+
+ i2cDevice_t *pDev = &i2cDevice[device];
+
+ const i2cHardware_t *hardware = pDev->hardware;
+
+ if (!hardware) {
+ return;
+ }
+
+ IO_t scl = pDev->scl;
+ IO_t sda = pDev->sda;
+
+ RCC_ClockCmd(hardware->rcc, ENABLE);
+
+ IOInit(scl, OWNER_I2C_SCL, RESOURCE_INDEX(device));
+ IOInit(sda, OWNER_I2C_SDA, RESOURCE_INDEX(device));
+
+ // Enable RCC
+ RCC_ClockCmd(hardware->rcc, ENABLE);
+
+ i2cUnstick(scl, sda);
+
+ // Init pins
+#if defined(STM32F7)
+ IOConfigGPIOAF(scl, pDev->pullUp ? IOCFG_I2C_PU : IOCFG_I2C, GPIO_AF4_I2C);
+ IOConfigGPIOAF(sda, pDev->pullUp ? IOCFG_I2C_PU : IOCFG_I2C, GPIO_AF4_I2C);
+#elif defined(STM32H7)
+ IOConfigGPIOAF(scl, pDev->pullUp ? IOCFG_I2C_PU : IOCFG_I2C, pDev->sclAF);
+ IOConfigGPIOAF(sda, pDev->pullUp ? IOCFG_I2C_PU : IOCFG_I2C, pDev->sdaAF);
+#else
+ IOConfigGPIO(scl, IOCFG_AF_OD);
+ IOConfigGPIO(sda, IOCFG_AF_OD);
+#endif
+
+ // Init I2C peripheral
+
+ I2C_HandleTypeDef *pHandle = &pDev->handle;
+
+ memset(pHandle, 0, sizeof(*pHandle));
+
+ pHandle->Instance = pDev->hardware->reg;
+
+ // Compute TIMINGR value based on peripheral clock for this device instance
+
+ uint32_t i2cPclk;
+
+#if defined(STM32F7)
+ // Clock source configured in startup/system_stm32f7xx.c as:
+ // I2C1234 : PCLK1
+ i2cPclk = HAL_RCC_GetPCLK1Freq();
+#elif defined(STM32H7)
+ // Clock sources configured in startup/system_stm32h7xx.c as:
+ // I2C123 : D2PCLK1 (rcc_pclk1 for APB1)
+ // I2C4 : D3PCLK1 (rcc_pclk4 for APB4)
+ i2cPclk = (pHandle->Instance == I2C4) ? HAL_RCCEx_GetD3PCLK1Freq() : HAL_RCC_GetPCLK1Freq();
+#else
+#error Unknown MCU type
+#endif
+
+ pHandle->Init.Timing = i2cClockTIMINGR(i2cPclk, pDev->overClock ? 800 : 400, 0);
+
+ pHandle->Init.OwnAddress1 = 0x0;
+ pHandle->Init.AddressingMode = I2C_ADDRESSINGMODE_7BIT;
+ pHandle->Init.DualAddressMode = I2C_DUALADDRESS_DISABLE;
+ pHandle->Init.OwnAddress2 = 0x0;
+ pHandle->Init.GeneralCallMode = I2C_GENERALCALL_DISABLE;
+ pHandle->Init.NoStretchMode = I2C_NOSTRETCH_DISABLE;
+
+ HAL_I2C_Init(pHandle);
+
+ // Enable the Analog I2C Filter
+ HAL_I2CEx_ConfigAnalogFilter(pHandle, I2C_ANALOGFILTER_ENABLE);
+
+ // Setup interrupt handlers
+ HAL_NVIC_SetPriority(hardware->er_irq, NVIC_PRIORITY_BASE(NVIC_PRIO_I2C_ER), NVIC_PRIORITY_SUB(NVIC_PRIO_I2C_ER));
+ HAL_NVIC_EnableIRQ(hardware->er_irq);
+ HAL_NVIC_SetPriority(hardware->ev_irq, NVIC_PRIORITY_BASE(NVIC_PRIO_I2C_EV), NVIC_PRIORITY_SUB(NVIC_PRIO_I2C_EV));
+ HAL_NVIC_EnableIRQ(hardware->ev_irq);
+}
+
+static void i2cUnstick(IO_t scl, IO_t sda)
+{
+ int i;
+
+ IOHi(scl);
+ IOHi(sda);
+
+ IOConfigGPIO(scl, IOCFG_OUT_OD);
+ IOConfigGPIO(sda, IOCFG_OUT_OD);
+
+ // Clock out, with SDA high:
+ // 7 data bits
+ // 1 READ bit
+ // 1 cycle for the ACK
+ for (i = 0; i < (LEN_ADDR + LEN_RW + LEN_ACK); i++) {
+ // Wait for any clock stretching to finish
+ int timeout = UNSTICK_CLK_STRETCH;
+ while (!IORead(scl) && timeout) {
+ delayMicroseconds(UNSTICK_CLK_US);
+ timeout--;
+ }
+
+ // Pull low
+ IOLo(scl); // Set bus low
+ delayMicroseconds(UNSTICK_CLK_US/2);
+ IOHi(scl); // Set bus high
+ delayMicroseconds(UNSTICK_CLK_US/2);
+ }
+
+ // Generate a stop condition in case there was none
+ IOLo(scl);
+ delayMicroseconds(UNSTICK_CLK_US/2);
+ IOLo(sda);
+ delayMicroseconds(UNSTICK_CLK_US/2);
+
+ IOHi(scl); // Set bus scl high
+ delayMicroseconds(UNSTICK_CLK_US/2);
+ IOHi(sda); // Set bus sda high
+}
+
+#endif
diff --git a/src/main/startup/system_stm32f7xx.c b/src/main/startup/system_stm32f7xx.c
index c0b0960ae..7fa0c1a0a 100644
--- a/src/main/startup/system_stm32f7xx.c
+++ b/src/main/startup/system_stm32f7xx.c
@@ -222,10 +222,14 @@
PeriphClkInitStruct.Usart6ClockSelection = RCC_USART6CLKSOURCE_PCLK2;
PeriphClkInitStruct.Uart7ClockSelection = RCC_UART7CLKSOURCE_PCLK1;
PeriphClkInitStruct.Uart8ClockSelection = RCC_UART8CLKSOURCE_PCLK1;
+
+ // I2C clock sources: Note that peripheral clock determination in bus_i2c_hal_init.c must be modified when the sources are modified.
+
PeriphClkInitStruct.I2c1ClockSelection = RCC_I2C1CLKSOURCE_PCLK1;
PeriphClkInitStruct.I2c2ClockSelection = RCC_I2C2CLKSOURCE_PCLK1;
PeriphClkInitStruct.I2c3ClockSelection = RCC_I2C3CLKSOURCE_PCLK1;
PeriphClkInitStruct.I2c4ClockSelection = RCC_I2C4CLKSOURCE_PCLK1;
+
ret = HAL_RCCEx_PeriphCLKConfig(&PeriphClkInitStruct);
if (ret != HAL_OK) {
while (1);
diff --git a/src/main/startup/system_stm32h7xx.c b/src/main/startup/system_stm32h7xx.c
index 026cff221..035c40e9e 100644
--- a/src/main/startup/system_stm32h7xx.c
+++ b/src/main/startup/system_stm32h7xx.c
@@ -486,6 +486,7 @@ void SystemClock_Config(void)
// Current source for I2C4:
// D3PCLK1 (rcc_pclk4 = APB4 peripheral clock)
//
+ // Note that peripheral clock determination in bus_i2c_hal_init.c must be modified when the sources are modified.
RCC_PeriphClkInit.PeriphClockSelection = RCC_PERIPHCLK_I2C123|RCC_PERIPHCLK_I2C4;
RCC_PeriphClkInit.I2c123ClockSelection = RCC_I2C123CLKSOURCE_D2PCLK1;
diff --git a/src/main/target/NUCLEOH743/target.h b/src/main/target/NUCLEOH743/target.h
index 97d2d836c..a8eecc6d3 100644
--- a/src/main/target/NUCLEOH743/target.h
+++ b/src/main/target/NUCLEOH743/target.h
@@ -172,6 +172,12 @@
#define I2C1_SDA PB9
#define I2C_DEVICE (I2CDEV_1)
+// For testing I2C4on APB4
+//#define USE_I2C_DEVICE_4
+//#define I2C4_SCL PF14
+//#define I2C4_SDA PF15
+//#define I2C_DEVICE (I2CDEV_4)
+
#define USE_MAG
#define USE_MAG_HMC5883
#define USE_MAG_SPI_HMC5883
diff --git a/src/main/target/common_pre.h b/src/main/target/common_pre.h
index 08548f33e..c3ecd0f79 100644
--- a/src/main/target/common_pre.h
+++ b/src/main/target/common_pre.h
@@ -114,6 +114,8 @@
#define USE_DSHOT_TELEMETRY_STATS
#define USE_RPM_FILTER
#define USE_DYN_IDLE
+#define I2C3_OVERCLOCK true
+#define I2C4_OVERCLOCK true
#define USE_GYRO_DATA_ANALYSE
#define USE_ADC_INTERNAL
#define USE_USB_CDC_HID