From b0a50412614a92d60a490d33a07194b6b2638d74 Mon Sep 17 00:00:00 2001
From: sl-firmware <sl-firmware@saltylab.local>
Date: Fri, 6 Mar 2026 23:14:49 -0500
Subject: [PATCH] fix: MPU6000 IMU calibration SPI/DCache issue (Issue #520)
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

Three bugs prevented mpu6000_is_calibrated() from returning true,
blocking arming and balance mode:

1. WHO_AM_I single-attempt: one SPI glitch returning 0x00 caused
   icm42688_init() to return -128, skipping mpu6000_calibrate()
   entirely. Fix: retry WHO_AM_I up to 3 times with 10ms gaps.

2. icm42688_read() rx[15] uninitialized: if HAL_SPI_TransmitReceive()
   failed, garbage stack data was accumulated as gyro bias. Fix: zero-
   init rx[15] so failed transfers produce zero data.

3. mpu6000_calibrate() raw uninitialized: UB if icm42688_read() is
   a no-op (imu_type mismatch). Fix: zero-init raw each iteration.

Also add SCB_InvalidateDCache_by_Addr() on SPI rx buffers in rreg()
and icm42688_read() for DCache coherency. Currently a no-op (DCache
is not enabled), but required if SCB_EnableDCache() is added — stack
buffers in SRAM2 are in the cacheable memory region on STM32F7.

Fix misleading DCache comment in icm42688.c (claimed DCache was
disabled by main.c; actually SCB_EnableDCache() is never called).

Build: 59904 bytes Flash (+512), 17100 bytes RAM — SUCCESS

Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
---
 src/icm42688.c | 24 +++++++++++++++++++-----
 src/mpu6000.c  |  2 +-
 2 files changed, 20 insertions(+), 6 deletions(-)

diff --git a/src/icm42688.c b/src/icm42688.c
index 37b6644..9e83861 100644
--- a/src/icm42688.c
+++ b/src/icm42688.c
@@ -45,6 +45,9 @@ static uint8_t rreg(uint8_t reg) {
     cs_low();
     HAL_SPI_TransmitReceive(&hspi1, tx, rx, 2, 100);
     cs_high();
+    /* DCache coherency: invalidate rx so CPU reads SPI-written SRAM, not stale cache.
+     * No-op when DCache is disabled; required if DCache is on (e.g. SCB_EnableDCache). */
+    SCB_InvalidateDCache_by_Addr((uint32_t *)(uintptr_t)rx, (int32_t)sizeof(rx));
     return rx[1];
 }
 
@@ -104,8 +107,10 @@ int icm42688_init(void) {
     HAL_GPIO_Init(MPU_CS_PORT, &gpio);
     cs_high();
 
-    /* DCache is disabled at startup in main.c before USB/peripherals init.
-     * No SCB_DisableDCache() here — calling it after USB starts corrupts USB state. */
+    /* DCache: main.c does NOT call SCB_EnableDCache(), so DCache is currently OFF.
+     * If DCache is ever enabled, all SPI rx buffers need SCB_InvalidateDCache_by_Addr()
+     * (already added in rreg() and icm42688_read()) and USB buffers must remain mapped
+     * non-cacheable via MPU Region 0 in usbd_conf.c. */
 
     hspi1.Instance = SPI1;
     hspi1.Init.Mode = SPI_MODE_MASTER;
@@ -127,7 +132,13 @@ int icm42688_init(void) {
     wreg(0x6B, 0x01); /* Wake, PLL */
     HAL_Delay(50);
 
-    uint8_t who = rreg(MPU_REG_WHO_AM_I);
+    /* Retry WHO_AM_I up to 3 times: a single SPI glitch returning 0x00
+     * would otherwise abort init and prevent calibration from ever running. */
+    uint8_t who = 0;
+    for (int attempt = 0; attempt < 3 && who == 0; attempt++) {
+        if (attempt > 0) HAL_Delay(10);
+        who = rreg(MPU_REG_WHO_AM_I);
+    }
     tr(who);  /* trace[0] */
 
     int ret;
@@ -153,12 +164,15 @@ int icm42688_init(void) {
 void icm42688_read(icm42688_data_t *d) {
     if (imu_type == 1) {
         /* MPU6000: ACCEL_XOUT_H (0x3B) → 14 bytes: accel(6)+temp(2)+gyro(6) */
-        uint8_t tx[15], rx[15];
+        uint8_t tx[15] = {0};
+        uint8_t rx[15] = {0};   /* zero-init: failed SPI transfers return 0, not garbage */
         tx[0] = MPU_REG_ACCEL_XOUT_H | 0x80;
-        for (int i = 1; i < 15; i++) tx[i] = 0x00;
         cs_low();
         HAL_SPI_TransmitReceive(&hspi1, tx, rx, 15, 100);
         cs_high();
+        /* DCache coherency: force CPU to read SPI-written SRAM, not a stale cache line.
+         * No-op when DCache is disabled; critical if SCB_EnableDCache() is called. */
+        SCB_InvalidateDCache_by_Addr((uint32_t *)(uintptr_t)rx, (int32_t)sizeof(rx));
 
         d->ax = (int16_t)((rx[1] << 8) | rx[2]);
         d->ay = (int16_t)((rx[3] << 8) | rx[4]);
diff --git a/src/mpu6000.c b/src/mpu6000.c
index 55b4837..215c5b4 100644
--- a/src/mpu6000.c
+++ b/src/mpu6000.c
@@ -62,7 +62,7 @@ void mpu6000_calibrate(void) {
      */
     int32_t sum_gx = 0, sum_gy = 0, sum_gz = 0;
     for (int i = 0; i < GYRO_CAL_SAMPLES; i++) {
-        icm42688_data_t raw;
+        icm42688_data_t raw = {0};  /* zero-init: guard against icm42688_read no-op on imu_type mismatch */
         icm42688_read(&raw);
         sum_gx += raw.gx;
         sum_gy += raw.gy;