-#if DIODE_DIRECTION == COL2ROW
- int pin = row_pins[row];
-#else
- int pin = col_pins[row];
-#endif
- _SFR_IO8((pin >> 4) + 1) |= _BV(pin & 0xF);
- _SFR_IO8((pin >> 4) + 2) &= ~_BV(pin & 0xF);
+static bool read_rows_on_col(matrix_row_t current_matrix[], uint8_t current_col)
+{
+ bool matrix_changed = false;
+
+ // Select col and wait for col selecton to stabilize
+ select_col(current_col);
+ wait_us(30);
+
+ // For each row...
+ for(uint8_t row_index = 0; row_index < MATRIX_ROWS; row_index++)
+ {
+
+ // Store last value of row prior to reading
+ matrix_row_t last_row_value = current_matrix[row_index];
+
+ // Check row pin state
+ if (readPin(row_pins[row_index]) == 0)
+ {
+ // Pin LO, set col bit
+ current_matrix[row_index] |= (ROW_SHIFTER << current_col);
+ }
+ else
+ {
+ // Pin HI, clear col bit
+ current_matrix[row_index] &= ~(ROW_SHIFTER << current_col);
+ }
+
+ // Determine if the matrix changed state
+ if ((last_row_value != current_matrix[row_index]) && !(matrix_changed))
+ {
+ matrix_changed = true;
+ }
+ }
+
+ // Unselect col
+ unselect_col(current_col);
+
+ return matrix_changed;
+}
+
+static void select_col(uint8_t col)
+{
+ setPinOutput(col_pins[col]);
+ writePinLow(col_pins[col]);
+}
+
+static void unselect_col(uint8_t col)
+{
+ setPinInputHigh(col_pins[col]);
+}
+
+static void unselect_cols(void)
+{
+ for(uint8_t x = 0; x < MATRIX_COLS; x++) {
+ setPinInputHigh(col_pins[x]);
+ }