Increase matrix scanning speed on xd84 (#7517)

* Increase matrix scanning speed

* Fix func name in failure output

2 files changed, 19 insertions, 0 deletions

diff --git a/drivers/gpio/pca9555.c b/drivers/gpio/pca9555.c
index 496bbca04e..02b5abbdde 100644
--- a/drivers/gpio/pca9555.c
+++ b/drivers/gpio/pca9555.c
@@ -76,3 +76,20 @@ uint8_t pca9555_readPins(uint8_t slave_addr, uint8_t port) {
     }
     return data;
 }
+
+uint16_t pca9555_readAllPins(uint8_t slave_addr) {
+    uint8_t addr = SLAVE_TO_ADDR(slave_addr);
+
+    typedef union {
+        uint8_t  u8[2];
+        uint16_t u16;
+    } data16;
+
+    data16 data;
+
+    i2c_status_t ret = i2c_readReg(addr, CMD_INPUT_0, &data.u8[0], sizeof(data), TIMEOUT);
+    if (ret != I2C_STATUS_SUCCESS) {
+        print("pca9555_readAllPins::FAILED\n");
+    }
+    return data.u16;
+}
diff --git a/drivers/gpio/pca9555.h b/drivers/gpio/pca9555.h
index ebb97e2f30..3341ec3eb5 100644
--- a/drivers/gpio/pca9555.h
+++ b/drivers/gpio/pca9555.h
@@ -53,3 +53,5 @@ void pca9555_set_config(uint8_t slave_addr, uint8_t port, uint8_t conf);
 void pca9555_set_output(uint8_t slave_addr, uint8_t port, uint8_t conf);
 
 uint8_t pca9555_readPins(uint8_t slave_addr, uint8_t port);
+
+uint16_t pca9555_readAllPins(uint8_t slave_addr);