From 316bdbb07fd72cff987604ee6b68999d7f001ad9 Mon Sep 17 00:00:00 2001
From: richardclli <richard.li@ces.hk>
Date: Thu, 18 Jun 2026 10:40:17 +0800
Subject: [PATCH] fix(pa01): optimize key matrix polling with split-cycle
 scanning (#7405)

---
 radio/src/targets/pa01/bsp_io.cpp     |  46 +++++--
 radio/src/targets/pa01/key_driver.cpp | 173 +++++++++++++++-----------
 2 files changed, 138 insertions(+), 81 deletions(-)

diff --git a/radio/src/targets/pa01/bsp_io.cpp b/radio/src/targets/pa01/bsp_io.cpp
index 6e58c3230..54778450f 100644
--- a/radio/src/targets/pa01/bsp_io.cpp
+++ b/radio/src/targets/pa01/bsp_io.cpp
@@ -25,6 +25,8 @@
 #include "drivers/aw9523b.h"
 #include "stm32_i2c_driver.h"
 #include "stm32_switch_driver.h"
+#include "delays_driver.h"
+#include "debug.h"
 
 #define BSP_I2C_BUS I2C_Bus_1
 #define BSP_I2C_ADDR 0x5b
@@ -56,15 +58,38 @@ bool bsp_get_shouldReadKeys()
   return tmp;
 }
 
-static volatile bool errorOccurs = false;
+#define I2C_ERROR_RECOVER_THRESHOLD 3
+
+static uint8_t i2c_consecutive_errors = 0;
+
+static void _recover_i2c()
+{
+  TRACE("I2C ERROR: resetting I2C bus");
+  i2c_deinit(BSP_I2C_BUS);
+  delay_ms(1);
+  i2c_init(BSP_I2C_BUS);
+  TRACE("I2C recovery complete");
+}
+
+static void _track_i2c_error(int result)
+{
+  if (result < 0) {
+    i2c_consecutive_errors++;
+    if (i2c_consecutive_errors >= I2C_ERROR_RECOVER_THRESHOLD) {
+      _recover_i2c();
+      i2c_consecutive_errors = 0;
+    }
+  } else {
+    i2c_consecutive_errors = 0;
+  }
+}
+
 static void bsp_input_read()
 {
   uint16_t value;
-  if (aw9523b_read(&i2c_exp, BSP_IN_MASK, &value) < 0) {
-    errorOccurs = true;
-    return;
-  }
-  inputState = value;
+  int ret = aw9523b_read(&i2c_exp, BSP_IN_MASK, &value);
+  _track_i2c_error(ret);
+  if (ret >= 0) inputState = value;
 }
 
 int bsp_io_init()
@@ -81,11 +106,14 @@ int bsp_io_init()
   return 0;
 }
 
-void bsp_output_set(uint16_t pin) { aw9523b_write(&i2c_exp, pin, pin); }
+void bsp_output_set(uint16_t pin)
+  { _track_i2c_error(aw9523b_write(&i2c_exp, pin, pin)); }
 
-void bsp_output_set(uint16_t mask, uint16_t pin) { aw9523b_write(&i2c_exp, mask, pin); }
+void bsp_output_set(uint16_t mask, uint16_t pin)
+  { _track_i2c_error(aw9523b_write(&i2c_exp, mask, pin)); }
 
-void bsp_output_clear(uint16_t pin) { aw9523b_write(&i2c_exp, pin, 0); }
+void bsp_output_clear(uint16_t pin)
+  { _track_i2c_error(aw9523b_write(&i2c_exp, pin, 0)); }
 
 uint16_t bsp_input_get()
 {
diff --git a/radio/src/targets/pa01/key_driver.cpp b/radio/src/targets/pa01/key_driver.cpp
index 522a2f8bb..ba1771625 100644
--- a/radio/src/targets/pa01/key_driver.cpp
+++ b/radio/src/targets/pa01/key_driver.cpp
@@ -30,7 +30,6 @@
 #include "delays_driver.h"
 #include "keys.h"
 
-#define BSP_READ_AFTER_WRITE_DELAY   10 // us
 #define BSP_KEY_OUT_MASK                                      \
   (BSP_KEY_OUT1 | BSP_KEY_OUT2 | BSP_KEY_OUT3 | BSP_KEY_OUT4)
 
@@ -74,29 +73,88 @@ extern bool suspendI2CTasks;
 
 static bool fct_state[4] = {false, false, false, false};
 static uint32_t keyState = 0;
+
+#define SCAN_COLS 4
+#define IDLE_FORCE_SCAN 25 // cycles (250ms)
+
 #if !defined(BOOT)
-static uint32_t nonReadCount = 0;
+static uint8_t scan_col = 0;
+static uint8_t read_col = 0xFF;
+static uint8_t scan_pending = SCAN_COLS;
+static uint8_t idle_cycles = 0;
+static uint32_t col_cache[SCAN_COLS] = {0, 0, 0, 0};
 #endif
 
+static const uint16_t col_drive[SCAN_COLS] = {
+  (uint16_t)~BSP_KEY_OUT1, (uint16_t)~BSP_KEY_OUT2,
+  (uint16_t)~BSP_KEY_OUT3, (uint16_t)~BSP_KEY_OUT4
+};
+
+static uint32_t read_col_to_keys(uint8_t col, uint16_t inputs)
+{
+  uint32_t result = 0;
+  if (col == 0) {
+    if ((inputs & BSP_KEY_IN1) == 0) result |= 1 << TR1U;
+    if ((inputs & BSP_KEY_IN2) == 0) result |= 1 << TR1D;
+    if ((inputs & BSP_KEY_IN3) == 0) result |= 1 << TR2U;
+    if ((inputs & BSP_KEY_IN4) == 0) result |= 1 << TR2D;
+  } else if (col == 1) {
+    if ((inputs & BSP_KEY_IN1) == 0) result |= 1 << TR3L;
+    if ((inputs & BSP_KEY_IN2) == 0) result |= 1 << TR3R;
+    if ((inputs & BSP_KEY_IN3) == 0) result |= 1 << TR4L;
+    if ((inputs & BSP_KEY_IN4) == 0) result |= 1 << TR4R;
+  } else if (col == 2) {
+    if ((inputs & BSP_KEY_IN1) == 0) result |= 1 << PGDN;
+    if ((inputs & BSP_KEY_IN2) == 0) result |= 1 << PGUP;
+    if ((inputs & BSP_KEY_IN3) == 0) result |= 1 << RTN;
+    if ((inputs & BSP_KEY_IN4) == 0) result |= 1 << MODEL;
+  } else {
+    if ((inputs & BSP_KEY_IN1) == 0) result |= 1 << KEY1;
+    if ((inputs & BSP_KEY_IN2) == 0) result |= 1 << KEY2;
+    if ((inputs & BSP_KEY_IN3) == 0) result |= 1 << KEY3;
+    if ((inputs & BSP_KEY_IN4) == 0) result |= 1 << KEY4;
+  }
+  return result;
+}
+
 void pollKeys()
 {
-#if !defined(BOOT)
-  if(!bsp_get_shouldReadKeys() && nonReadCount < 10)
-  {
-    if (gpio_read(KEYS_GPIO_ENTER) == 0)
-      keyState |= 1<<ENT;
+#if defined(BOOT)
+  uint32_t ent_mask = 0;
+  if (gpio_read(KEYS_GPIO_ENTER) == 0)
+    ent_mask = (1 << ENT);
 
-    nonReadCount++;
+  if (suspendI2CTasks) {
+    keyState = ent_mask;
+    return;
   }
-  nonReadCount = 0;
-#endif
-
-  if (suspendI2CTasks) return;
-  
-  // This function avoids concurrent matrix agitation
 
   uint32_t result = 0;
-  uint16_t bsp_input = 0;
+  for (uint8_t col = 0; col < SCAN_COLS; col++) {
+    bsp_output_set(BSP_KEY_OUT_MASK, col_drive[col]);
+    delay_us(10);
+    result |= read_col_to_keys(col, bsp_input_get());
+  }
+
+  result |= ent_mask;
+  bsp_output_set(BSP_KEY_OUT_MASK, 0);
+  bsp_get_shouldReadKeys();
+
+  fct_state[0] = (result & (1 << KEY1)) ? true : false;
+  fct_state[1] = (result & (1 << KEY2)) ? true : false;
+  fct_state[2] = (result & (1 << KEY3)) ? true : false;
+  fct_state[3] = (result & (1 << KEY4)) ? true : false;
+
+  keyState = result;
+#else
+  uint32_t ent_mask = 0;
+  if (gpio_read(KEYS_GPIO_ENTER) == 0)
+    ent_mask = (1 << ENT);
+
+  if (suspendI2CTasks) {
+    keyState = col_cache[0] | col_cache[1] | col_cache[2] | col_cache[3] | ent_mask;
+    return;
+  }
 
   volatile static struct
   {
@@ -106,80 +164,51 @@ void pollKeys()
 
   if (syncelem.ui8ReadInProgress != 0) {
     keyState = syncelem.oldResult;
+    return;
   }
 
-  // ui8ReadInProgress was 0, increment it
   syncelem.ui8ReadInProgress++;
-  // Double check before continuing, as non-atomic, non-blocking so far
-  // If ui8ReadInProgress is above 1, then there was concurrent task calling it, exit
   if (syncelem.ui8ReadInProgress > 1) {
     keyState = syncelem.oldResult;
+    syncelem.ui8ReadInProgress--;
+    return;
   }
 
-  // If we land here, we have exclusive access to Matrix
-  bsp_output_set(BSP_KEY_OUT_MASK, ~BSP_KEY_OUT1);
-  delay_us(BSP_READ_AFTER_WRITE_DELAY);
-  bsp_input = bsp_input_get();
-  if ((bsp_input & BSP_KEY_IN1) == 0)
-    result |= 1<<TR1U;
-  if ((bsp_input & BSP_KEY_IN2) == 0)
-    result |= 1<<TR1D;
-  if ((bsp_input & BSP_KEY_IN3) == 0)
-    result |= 1<<TR2U;
-  if ((bsp_input & BSP_KEY_IN4) == 0)
-    result |= 1<<TR2D;
+  if (bsp_get_shouldReadKeys()) {
+    scan_pending = SCAN_COLS;
+    idle_cycles = 0;
+  }
 
-  bsp_output_set(BSP_KEY_OUT_MASK, ~BSP_KEY_OUT2);
-  delay_us(BSP_READ_AFTER_WRITE_DELAY);
-  bsp_input = bsp_input_get();
-  if ((bsp_input & BSP_KEY_IN1) == 0)
-    result |= 1<<TR3L;
-  if ((bsp_input & BSP_KEY_IN2) == 0)
-    result |= 1<<TR3R;
-  if ((bsp_input & BSP_KEY_IN3) == 0)
-    result |= 1<<TR4L;
-  if ((bsp_input & BSP_KEY_IN4) == 0)
-    result |= 1<<TR4R;
+  if (scan_pending == 0) {
+    idle_cycles++;
+    if (idle_cycles >= IDLE_FORCE_SCAN) {
+      scan_pending = SCAN_COLS;
+      idle_cycles = 0;
+    }
+  }
 
-  bsp_output_set(BSP_KEY_OUT_MASK, ~BSP_KEY_OUT3);
-  delay_us(BSP_READ_AFTER_WRITE_DELAY);
-  bsp_input = bsp_input_get();
-  if ((bsp_input & BSP_KEY_IN1) == 0)
-    result |= 1<<PGDN;
-  if ((bsp_input & BSP_KEY_IN2) == 0)
-    result |= 1<<PGUP;
-  if ((bsp_input & BSP_KEY_IN3) == 0)
-    result |= 1<<RTN;
-  if ((bsp_input & BSP_KEY_IN4) == 0)
-    result |= 1<<MODEL;
+  if (read_col < SCAN_COLS && scan_pending > 0) {
+    uint16_t inputs = bsp_input_get();
+    col_cache[read_col] = read_col_to_keys(read_col, inputs);
+    scan_pending--;
+  }
 
-  bsp_output_set(BSP_KEY_OUT_MASK, ~BSP_KEY_OUT4);
-  delay_us(BSP_READ_AFTER_WRITE_DELAY);
-  bsp_input = bsp_input_get();
-  if ((bsp_input & BSP_KEY_IN1) == 0)
-    result |= 1<<KEY1;
-  if ((bsp_input & BSP_KEY_IN2) == 0)
-    result |= 1<<KEY2;
-  if ((bsp_input & BSP_KEY_IN3) == 0)
-    result |= 1<<KEY3;
-  if ((bsp_input & BSP_KEY_IN4) == 0)
-    result |= 1<<KEY4;
+  read_col = scan_col;
+  bsp_output_set(BSP_KEY_OUT_MASK, col_drive[scan_col]);
+  scan_col = (scan_col + 1) % SCAN_COLS;
 
-  if (gpio_read(KEYS_GPIO_ENTER) == 0)
-    result |= 1<<ENT;
+  uint32_t result = col_cache[0] | col_cache[1] | col_cache[2] | col_cache[3] | ent_mask;
 
   syncelem.oldResult = result;
   syncelem.ui8ReadInProgress = 0;
 
-  bsp_output_set(BSP_KEY_OUT_MASK, 0);
-  bsp_get_shouldReadKeys();
-
-  fct_state[0] = (result & 1<<KEY1)?true:false;
-  fct_state[1] = (result & 1<<KEY2)?true:false;
-  fct_state[2] = (result & 1<<KEY3)?true:false;
-  fct_state[3] = (result & 1<<KEY4)?true:false;
+  fct_state[0] = (result & (1 << KEY1)) ? true : false;
+  fct_state[1] = (result & (1 << KEY2)) ? true : false;
+  fct_state[2] = (result & (1 << KEY3)) ? true : false;
+  fct_state[3] = (result & (1 << KEY4)) ? true : false;
 
   keyState = result;
+#endif
 }
 
 void keysInit()