diff --git a/quantum/config_common.h b/quantum/config_common.h
index 62f5ba222d..10e0b67d13 100644
--- a/quantum/config_common.h
+++ b/quantum/config_common.h
@@ -27,16 +27,17 @@
 #define CUSTOM_MATRIX 2 /* Disables built-in matrix scanning code */
 
 /* I/O pins */
-#define PINDEF(port, pin) (uint8_t)((((uint16_t)&PORT##port) << 4) + PIN##port##pin)
+// #define PINDEF(port, pin) (uint8_t)((((uint16_t)&PIN##port) << 4) + PIN##port##pin)
+#define PINDEF(port, pin) (uint8_t)(((((uint16_t)&PIN##port) - __SFR_OFFSET)<< 4) + PIN##port##pin)
 
-#define PIN(p) (*((volatile uint8_t*)(p >> 4) + 0))
+#define PIN(p) (*((volatile uint16_t*)(p >> 4) + 0 + __SFR_OFFSET))
 #define PIN_VALUE(p) (PIN(p) & _BV(p & 0xF))
 
-#define DDR(p) (*((volatile uint8_t*)(p >> 4) + 1))
+#define DDR(p) (*((volatile uint16_t*)(p >> 4) + 1 + __SFR_OFFSET))
 #define DDR_OUTPUT(p) (DDR(p) |= _BV(p & 0xF))
 #define DDR_INPUT(p) (DDR(p) &= ~_BV(p & 0xF))
 
-#define PORT(p) (*((volatile uint8_t*)(p >> 4) + 2))
+#define PORT(p) (*((volatile uint16_t*)(p >> 4) + 2 + __SFR_OFFSET))
 #define PORT_HIGH(p) (PORT(p) |= _BV(p & 0xF))
 #define PORT_LOW(p) (PORT(p) &= ~_BV(p & 0xF))
 
diff --git a/quantum/matrix.c b/quantum/matrix.c
index 7bbcd429a3..00110a3296 100644
--- a/quantum/matrix.c
+++ b/quantum/matrix.c
@@ -220,7 +220,10 @@ uint8_t matrix_scan(void)
         }
 #   endif
 
+    
+    dprintf("%x\n", PINDEF(B,0));
     matrix_scan_quantum();
+
     return 1;
 }