1 files changed, 25 insertions, 10 deletions

diff --git a/quantum/matrix.c b/quantum/matrix.c
index c68c56cac..cab0d2ddc 100644
--- a/quantum/matrix.c
+++ b/quantum/matrix.c
@@ -32,6 +32,19 @@ static const pin_t col_pins[MATRIX_COLS] = MATRIX_COL_PINS;
 extern matrix_row_t raw_matrix[MATRIX_ROWS];  // raw values
 extern matrix_row_t matrix[MATRIX_ROWS];      // debounced values
 
+static inline void setPinOutput_writeLow(pin_t pin) {
+    ATOMIC_BLOCK_FORCEON {
+        setPinOutput(pin);
+        writePinLow(pin);
+    }
+}
+
+static inline void setPinInputHigh_atomic(pin_t pin) {
+    ATOMIC_BLOCK_FORCEON {
+        setPinInputHigh(pin);
+    }
+}
+
 // matrix code
 
 #ifdef DIRECT_PINS
@@ -70,22 +83,23 @@ static bool read_cols_on_row(matrix_row_t current_matrix[], uint8_t current_row)
 #    if (DIODE_DIRECTION == COL2ROW)
 
 static void select_row(uint8_t row) {
-    setPinOutput(row_pins[row]);
-    writePinLow(row_pins[row]);
+    setPinOutput_writeLow(row_pins[row]);
 }
 
-static void unselect_row(uint8_t row) { setPinInputHigh(row_pins[row]); }
+static void unselect_row(uint8_t row) {
+    setPinInputHigh_atomic(row_pins[row]);
+}
 
 static void unselect_rows(void) {
     for (uint8_t x = 0; x < MATRIX_ROWS; x++) {
-        setPinInputHigh(row_pins[x]);
+        setPinInputHigh_atomic(row_pins[x]);
     }
 }
 
 static void init_pins(void) {
     unselect_rows();
     for (uint8_t x = 0; x < MATRIX_COLS; x++) {
-        setPinInputHigh(col_pins[x]);
+        setPinInputHigh_atomic(col_pins[x]);
     }
 }
 
@@ -120,22 +134,23 @@ static bool read_cols_on_row(matrix_row_t current_matrix[], uint8_t current_row)
 #    elif (DIODE_DIRECTION == ROW2COL)
 
 static void select_col(uint8_t col) {
-    setPinOutput(col_pins[col]);
-    writePinLow(col_pins[col]);
+    setPinOutput_writeLow(col_pins[col]);
 }
 
-static void unselect_col(uint8_t col) { setPinInputHigh(col_pins[col]); }
+static void unselect_col(uint8_t col) {
+    setPinInputHigh_atomic(col_pins[col]);
+}
 
 static void unselect_cols(void) {
     for (uint8_t x = 0; x < MATRIX_COLS; x++) {
-        setPinInputHigh(col_pins[x]);
+        setPinInputHigh_atomic(col_pins[x]);
     }
 }
 
 static void init_pins(void) {
     unselect_cols();
     for (uint8_t x = 0; x < MATRIX_ROWS; x++) {
-        setPinInputHigh(row_pins[x]);
+        setPinInputHigh_atomic(row_pins[x]);
     }
 }