diff --git a/keyboard/phantom/keymap.c b/keyboard/phantom/keymap.c
index 79b7d0c5..7433d53c 100644
--- a/keyboard/phantom/keymap.c
+++ b/keyboard/phantom/keymap.c
@@ -30,47 +30,89 @@ along with this program.  If not, see <http://www.gnu.org/licenses/>.
 #include "debug.h"
 #include "keymap.h"
 
-
 // Convert physical keyboard layout to matrix array.
 // This is a macro to define keymap easily in keyboard layout form.
+// Use this for 7bit layout.
 #define KEYMAP( \
-    K5A, K5C, K5D, K5E, K5F, K5G, K5H, K5I, K5J, K5K, K5L, K5M, K5N,           K5O, K5P, K5Q, \
-    K5B, K4A, K4B, K4C, K4D, K4E, K4F, K4G, K4H, K4I, K4J, K4K, K4L, K4N,      K4O, K4P, K4Q, \
-    K3A, K3B, K3C, K3D, K3E, K3F, K3G, K3H, K3I, K3J, K3K, K3L, K3M, K3N,      K3O, K3P, K3Q, \
-    K2A, K2B, K2C, K2D, K2E, K2F, K2G, K2H, K2I, K2J, K2K, K2L, K2N,           K2O, K2P, K2Q, \
-    K1A,      K1C, K1D, K1E, K1F, K1G, K1H, K1I, K1J, K1K, K1L, K1N,           K1O, K1P, K1Q, \
-    K0A, K0B, K0C,           K0H,                     K0K, K0L, K0M, K0N,      K0O, K0P, K0Q  \
+    K5A, K5B, K5C, K5D, K5E, K5F, K5G, K5H, K5I, K5J, K5K, K5L, K5M, K5N, K5O, K5P, K5Q, K5R, \
+    K4A, K4B, K4C, K4D, K4E, K4F, K4G, K4H, K4I, K4J, K4K, K4L, K4M, K4N, K4O, K4P, K4Q, K4R, \
+    K3A, K3B, K3C, K3D, K3E, K3F, K3G, K3H, K3I, K3J, K3K, K3L, K3M, K3N,      K3P, K3Q, K3R, \
+    K2A, K2B, K2C, K2D, K2E, K2F, K2G, K2H, K2I, K2J, K2K, K2L, K2M, K2N,      K2P, K2Q, K2R, \
+    K1A, K1B, K1C, K1D, K1E, K1F, K1G, K1H, K1I, K1J, K1K, K1L, K1M, K1N,      K1P, K1Q, K1R, \
+    K0A, K0B, K0C, K0D,      K0F, K0G, K0H,           K0K, K0L, K0M, K0N,      K0P, K0Q, K0R  \
 ) { \
-/*             A         B         C         D         E         F         G         H         I         J         K         L          M        N          O         P         Q    */  \
-/* 0 */   { KC_##K0A, KC_##K0B, KC_##K0C, KC_NO   , KC_NO   , KC_NO   , KC_NO   , KC_##K0H, KC_NO   , KC_NO   , KC_##K0K, KC_##K0L, KC_##K0M, KC_##K0N, KC_##K0O, KC_##K0P, KC_##K0Q}, \
-/* 1 */   { KC_##K1A, KC_NO   , KC_##K1C, KC_##K1D, KC_##K1E, KC_##K1F, KC_##K1G, KC_##K1H, KC_##K1I, KC_##K1J, KC_##K1K, KC_##K1L, KC_NO   , KC_##K1N, KC_##K1O, KC_##K1P, KC_##K1Q}, \
-/* 2 */   { KC_##K2A, KC_##K2B, KC_##K2C, KC_##K2D, KC_##K2E, KC_##K2F, KC_##K2G, KC_##K2H, KC_##K2I, KC_##K2J, KC_##K2K, KC_##K2L, KC_NO   , KC_##K2N, KC_##K2O, KC_##K2P, KC_##K2Q}, \
-/* 3 */   { KC_##K3A, KC_##K3B, KC_##K3C, KC_##K3D, KC_##K3E, KC_##K3F, KC_##K3G, KC_##K3H, KC_##K3I, KC_##K3J, KC_##K3K, KC_##K3L, KC_##K3M, KC_##K3N, KC_##K3O, KC_##K3P, KC_##K3Q}, \
-/* 4 */   { KC_##K4A, KC_##K4B, KC_##K4C, KC_##K4D, KC_##K4E, KC_##K4F, KC_##K4G, KC_##K4H, KC_##K4I, KC_##K4J, KC_##K4K, KC_##K4L, KC_NO   , KC_##K4N, KC_##K4O, KC_##K4P, KC_##K4Q}, \
-/* 5 */   { KC_##K5A, KC_##K5B, KC_##K5C, KC_##K5D, KC_##K5E, KC_##K5F, KC_##K5G, KC_##K5H, KC_##K5I, KC_##K5J, KC_##K5K, KC_##K5L, KC_##K5M, KC_##K5N, KC_##K5O, KC_##K5P, KC_##K5Q}, \
+/*             0         1         2         3         4         5         6         7         8         9         10        11        12        13        14        15        16   */  \
+/* 0 */   { KC_##K0A, KC_##K0B, KC_##K0C, KC_##K5B, KC_##K0D, KC_##K0F, KC_##K5G, KC_##K0G, KC_##K0H, KC_NO,    KC_##K0K, KC_##K0L, KC_##K0M, KC_##K0N, KC_##K0P, KC_##K0Q, KC_##K0R}, \
+/* 1 */   { KC_##K1A, KC_##K1B, KC_##K1C, KC_##K1D, KC_##K1E, KC_##K1F, KC_##K1G, KC_##K1H, KC_##K1I, KC_##K1J, KC_##K1K, KC_##K1L, KC_##K1M, KC_##K1N, KC_##K1P, KC_##K1Q, KC_##K1R}, \
+/* 2 */   { KC_##K2A, KC_##K2B, KC_##K2C, KC_##K2D, KC_##K2E, KC_##K2F, KC_##K2G, KC_##K2H, KC_##K2I, KC_##K2J, KC_##K2K, KC_##K2L, KC_##K2M, KC_##K2N, KC_##K2P, KC_##K2Q, KC_##K2R}, \
+/* 3 */   { KC_##K3A, KC_##K3B, KC_##K3C, KC_##K3D, KC_##K3E, KC_##K3F, KC_##K3G, KC_##K3H, KC_##K3I, KC_##K3J, KC_##K3K, KC_##K3L, KC_##K3M, KC_##K3N, KC_##K3P, KC_##K3Q, KC_##K3R}, \
+/* 4 */   { KC_##K4B, KC_##K4C, KC_##K4D, KC_##K4E, KC_##K4F, KC_##K4G, KC_##K4H, KC_##K4I, KC_##K4J, KC_##K4K, KC_##K4L, KC_##K4M, KC_##K4N, KC_##K4O, KC_##K4P, KC_##K4Q, KC_##K4R}, \
+/* 5 */   { KC_##K5A, KC_##K4A, KC_##K5C, KC_##K5D, KC_##K5E, KC_##K5F, KC_##K5H, KC_##K5I, KC_##K5J, KC_##K5K, KC_##K5L, KC_##K5M, KC_##K5N, KC_##K5O, KC_##K5P, KC_##K5Q, KC_##K5R}  \
 }
 
-/*
- * Phantom keyboard layout with winkeys and 7bit style editing block. I am
- * Not in the mood to implement full 7-bit keymap.
- * 
- *
- * ,---.   ,---------------. ,---------------. ,---------------. ,-----------.
- * |Esc|   |F1 |F2 |F3 |F4 | |F5 |F6 |F7 |F8 | |F9 |F10|F11|F12| |PrS|ScL|Pau|
- * `---'   `---------------' `---------------' `---------------' `-----------'
- * ,-----------------------------------------------------------. ,-----------.
- * |~  |  1|  2|  3|  4|  5|  6|  7|  8|  9|  0|  -|  =|Backsp | |Ins|Hom|PgU|
- * |-----------------------------------------------------------| |-----------|
- * |Tab  |  Q|  W|  E|  R|  T|  Y|  U|  I|  O|  P|  [|  ]|    \| |Del|End|PgD|
- * |-----------------------------------------------------------| |---|---|---|
- * |Caps  |  A|  S|  D|  F|  G|  H|  J|  K|  L|  ;|  '|Return  | |???|???|???|
- * |-----------------------------------------------------------| |---|---|---|
- * |Shift   |  Z|  X|  C|  V|  B|  N|  M|  ,|  .|  /|Shift     | |???|Up |???|
- * |-----------------------------------------------------------| |-----------|
- * |Ctl|Gui|Alt|          Space                |Alt|Gui|App|Ctl| |Lef|Dow|Rig|
- * `-----------------------------------------------------------' `-----------'
- */
+#define KEYMAP_ANSI( \
+    K5A,      K5C, K5D, K5E, K5F, K5H, K5I, K5J, K5K, K5L, K5M, K5N, K5O,      K5P, K5Q, K5R, \
+    K4A, K4B, K4C, K4D, K4E, K4F, K4G, K4H, K4I, K4J, K4K, K4L, K4M, K4O,      K4P, K4Q, K4R, \
+    K3A, K3B, K3C, K3D, K3E, K3F, K3G, K3H, K3I, K3J, K3K, K3L, K3M, K3N,      K3P, K3Q, K3R, \
+    K2A, K2B, K2C, K2D, K2E, K2F, K2G, K2H, K2I, K2J, K2K, K2L,      K2N,                     \
+    K1A,      K1C, K1D, K1E, K1F, K1G, K1H, K1I, K1J, K1K, K1L,      K1N,           K1Q,      \
+    K0A, K0B, K0C,                K0G,                K0K, K0L, K0M, K0N,      K0P, K0Q, K0R  \
+) KEYMAP( \
+    K5A, NO,  K5C, K5D, K5E, K5F, NO,  K5H, K5I, K5J, K5K, K5L, K5M, K5N, K5O, K5P, K5Q, K5R, \
+    K4A, K4B, K4C, K4D, K4E, K4F, K4G, K4H, K4I, K4J, K4K, K4L, K4M, NO,  K4O, K4P, K4Q, K4R, \
+    K3A, K3B, K3C, K3D, K3E, K3F, K3G, K3H, K3I, K3J, K3K, K3L, K3M, K3N,      K3P, K3Q, K3R, \
+    K2A, K2B, K2C, K2D, K2E, K2F, K2G, K2H, K2I, K2J, K2K, K2L, NO,  K2N,      NO,  NO,  NO,  \
+    K1A, NO,  K1C, K1D, K1E, K1F, K1G, K1H, K1I, K1J, K1K, K1L, NO,  K1N,      NO,  K1Q, NO,  \
+    K0A, K0B, K0C, NO,       NO,  K0G, NO,            K0K, K0L, K0M, K0N,      K0P, K0Q, K0R  \
+)
 
+#define KEYMAP_ANSI_150( \
+    K5A,      K5C, K5D, K5E, K5F, K5H, K5I, K5J, K5K, K5L, K5M, K5N, K5O,      K5P, K5Q, K5R, \
+    K4A, K4B, K4C, K4D, K4E, K4F, K4G, K4H, K4I, K4J, K4K, K4L, K4M, K4O,      K4P, K4Q, K4R, \
+    K3A, K3B, K3C, K3D, K3E, K3F, K3G, K3H, K3I, K3J, K3K, K3L, K3M, K3N,      K3P, K3Q, K3R, \
+    K2A, K2B, K2C, K2D, K2E, K2F, K2G, K2H, K2I, K2J, K2K, K2L,      K2N,                     \
+    K1A,      K1C, K1D, K1E, K1F, K1G, K1H, K1I, K1J, K1K, K1L,      K1N,           K1Q,      \
+    K0A, K0B, K0C,                K0G,                     K0L, K0M, K0N,      K0P, K0Q, K0R  \
+) KEYMAP( \
+    K5A, NO,  K5C, K5D, K5E, K5F, NO,  K5H, K5I, K5J, K5K, K5L, K5M, K5N, K5O, K5P, K5Q, K5R, \
+    K4A, K4B, K4C, K4D, K4E, K4F, K4G, K4H, K4I, K4J, K4K, K4L, K4M, NO,  K4O, K4P, K4Q, K4R, \
+    K3A, K3B, K3C, K3D, K3E, K3F, K3G, K3H, K3I, K3J, K3K, K3L, K3M, K3N,      K3P, K3Q, K3R, \
+    K2A, K2B, K2C, K2D, K2E, K2F, K2G, K2H, K2I, K2J, K2K, K2L, NO,  K2N,      NO,  NO,  NO,  \
+    K1A, NO,  K1C, K1D, K1E, K1F, K1G, K1H, K1I, K1J, K1K, K1L, NO,  K1N,      NO,  K1Q, NO,  \
+    K0A, K0B, K0C, NO,       NO,  K0G, NO,            NO,  K0L, K0M, K0N,      K0P, K0Q, K0R  \
+)
+
+#define KEYMAP_ISO( \
+    K5A,      K5C, K5D, K5E, K5F, K5H, K5I, K5J, K5K, K5L, K5M, K5N, K5O,      K5P, K5Q, K5R, \
+    K4A, K4B, K4C, K4D, K4E, K4F, K4G, K4H, K4I, K4J, K4K, K4L, K4M, K4O,      K4P, K4Q, K4R, \
+    K3A, K3B, K3C, K3D, K3E, K3F, K3G, K3H, K3I, K3J, K3K, K3L, K3M,           K3P, K3Q, K3R, \
+    K2A, K2B, K2C, K2D, K2E, K2F, K2G, K2H, K2I, K2J, K2K, K2L, K2M, K2N,                     \
+    K1A, K1B, K1C, K1D, K1E, K1F, K1G, K1H, K1I, K1J, K1K, K1L,      K1N,           K1Q,      \
+    K0A, K0B, K0C,                K0G,                K0K, K0L, K0M, K0N,      K0P, K0Q, K0R  \
+) KEYMAP( \
+    K5A, NO,  K5C, K5D, K5E, K5F, NO,  K5H, K5I, K5J, K5K, K5L, K5M, K5N, K5O, K5P, K5Q, K5R, \
+    K4A, K4B, K4C, K4D, K4E, K4F, K4G, K4H, K4I, K4J, K4K, K4L, K4M, NO,  K4O, K4P, K4Q, K4R, \
+    K3A, K3B, K3C, K3D, K3E, K3F, K3G, K3H, K3I, K3J, K3K, K3L, K3M, NO,       K3P, K3Q, K3R, \
+    K2A, K2B, K2C, K2D, K2E, K2F, K2G, K2H, K2I, K2J, K2K, K2L, K2M, K2N,      NO,  NO,  NO,  \
+    K1A, K1B, K1C, K1D, K1E, K1F, K1G, K1H, K1I, K1J, K1K, K1L, NO,  K1N,      NO,  K1Q, NO,  \
+    K0A, K0B, K0C, NO,       NO,  K0G, NO,            K0K, K0L, K0M, K0N,      K0P, K0Q, K0R  \
+)
+
+#define KEYMAP_ISO_150( \
+    K5A,      K5C, K5D, K5E, K5F, K5H, K5I, K5J, K5K, K5L, K5M, K5N, K5O,      K5P, K5Q, K5R, \
+    K4A, K4B, K4C, K4D, K4E, K4F, K4G, K4H, K4I, K4J, K4K, K4L, K4M, K4O,      K4P, K4Q, K4R, \
+    K3A, K3B, K3C, K3D, K3E, K3F, K3G, K3H, K3I, K3J, K3K, K3L, K3M,           K3P, K3Q, K3R, \
+    K2A, K2B, K2C, K2D, K2E, K2F, K2G, K2H, K2I, K2J, K2K, K2L, K2M, K2N,                     \
+    K1A, K1B, K1C, K1D, K1E, K1F, K1G, K1H, K1I, K1J, K1K, K1L,      K1N,           K1Q,      \
+    K0A, K0B, K0C,                K0G,                     K0L, K0M, K0N,      K0P, K0Q, K0R  \
+) KEYMAP( \
+    K5A, NO,  K5C, K5D, K5E, K5F, NO,  K5H, K5I, K5J, K5K, K5L, K5M, K5N, K5O, K5P, K5Q, K5R, \
+    K4A, K4B, K4C, K4D, K4E, K4F, K4G, K4H, K4I, K4J, K4K, K4L, K4M, NO,  K4O, K4P, K4Q, K4R, \
+    K3A, K3B, K3C, K3D, K3E, K3F, K3G, K3H, K3I, K3J, K3K, K3L, K3M, NO,       K3P, K3Q, K3R, \
+    K2A, K2B, K2C, K2D, K2E, K2F, K2G, K2H, K2I, K2J, K2K, K2L, K2M, K2N,      NO,  NO,  NO,  \
+    K1A, K1B, K1C, K1D, K1E, K1F, K1G, K1H, K1I, K1J, K1K, K1L, NO,  K1N,      NO,  K1Q, NO,  \
+    K0A, K0B, K0C, NO,       NO,  K0G, NO,            NO,  K0L, K0M, K0N,      K0P, K0Q, K0R  \
+)
 
 static const uint8_t PROGMEM keymaps[][MATRIX_ROWS][MATRIX_COLS] = {
     /* Layer 0: Default Layer
@@ -84,22 +126,22 @@ static const uint8_t PROGMEM keymaps[][MATRIX_ROWS][MATRIX_COLS] = {
      * |~  |  1|  2|  3|  4|  5|  6|  7|  8|  9|  0|  -|  =|Backsp | |Ins|Hom|PgU|
      * |-----------------------------------------------------------| |-----------|
      * |Tab  |  Q|  W|  E|  R|  T|  Y|  U|  I|  O|  P|  [|  ]|    \| |Del|End|PgD|
-     * |-----------------------------------------------------------| |---|---|---|
-     * |Caps  |  A|  S|  D|  F|  G|  H|  J|  K|  L|  ;|  '|Return  | |???|???|???|
-     * |-----------------------------------------------------------| |---|---|---|
-     * |Shift   |  Z|  X|  C|  V|  B|  N|  M|  ,|  .|  /|Shift     | |???|Up |???|
-     * |-----------------------------------------------------------| |-----------|
-     * |Ctl|Gui|Alt|          Space                |Alt|Gui|App|Ctl| |Lef|Dow|Rig|
+     * |-----------------------------------------------------------| '-----------'
+     * |Caps  |  A|  S|  D|  F|  G|  H|  J|  K|  L|  ;|  '|Return  |
+     * |-----------------------------------------------------------|     ,---.
+     * |Shift   |  Z|  X|  C|  V|  B|  N|  M|  ,|  .|  /|Shift     |     |Up |
+     * |-----------------------------------------------------------| ,-----------.
+     * |Ctl|Gui|Alt|             Space             |Alt|Gui|App|Ctl| |Lef|Dow|Rig|
      * `-----------------------------------------------------------' `-----------'
      */
 
-    KEYMAP(\
-        ESC, F1,  F2,  F3,  F4,  F5,  F6,  F7,  F8,  F9,  F10, F11, F12,            PSCR,SLCK,BRK, \
+    KEYMAP_ANSI(\
+        ESC,      F1,  F2,  F3,  F4,  F5,  F6,  F7,  F8,  F9,  F10, F11, F12,       PSCR,SLCK,BRK,  \
         GRV, 1,   2,   3,   4,   5,   6,   7,   8,   9,   0,   MINS,EQL, BSPC,      INS, HOME,PGUP, \
         TAB, Q,   W,   E,   R,   T,   Y,   U,   I,   O,   P,   LBRC,RBRC,BSLS,      DEL, END, PGDN, \
-        FN0, A,   S,   D,   F,   G,   H,   J,   K,   L,   SCLN,QUOT,     ENT,       0,   0,   0, \
-        LSFT,Z,   X,   C,   V,   B,   N,   M,   COMM,DOT, SLSH,          RSFT,      0,   UP,  0, \
-        LCTL,LGUI,LALT,          SPC,                     RALT,RGUI,APP, RCTL,      LEFT,DOWN,RGHT),
+        FN0, A,   S,   D,   F,   G,   H,   J,   K,   L,   SCLN,QUOT,     ENT,                       \
+        LSFT,     Z,   X,   C,   V,   B,   N,   M,   COMM,DOT, SLSH,     RSFT,           UP,        \
+        LCTL,LGUI,LALT,               SPC,                RALT,RGUI,APP, RCTL,      LEFT,DOWN,RGHT),
 
     /* Overlay 1
      *
@@ -110,22 +152,22 @@ static const uint8_t PROGMEM keymaps[][MATRIX_ROWS][MATRIX_COLS] = {
      * |   |   |   |   |   |   |   |   |   |   |Mut|V- |V+ |       | |   |   |   |
      * |-----------------------------------------------------------| |-----------|
      * |     |   |   |   |   |   |   |   |   |MSt|Ply|Prv|Nxt|Media| |   |   |   |
-     * |-----------------------------------------------------------| |-----------|
-     * |      |   |   |   |   |   |   |   |   |   |   |   |        | |???|???|???|
-     * |-----------------------------------------------------------| |-----------|
-     * |        |   |   |Clc|   |   |   |   |   |   |   |Caps      | |???|   |???|
-     * |-----------------------------------------------------------| |-----------|
+     * |-----------------------------------------------------------| '-----------'
+     * |      |   |   |   |   |   |   |   |   |   |   |   |        |
+     * |-----------------------------------------------------------|     ,---.
+     * |        |   |   |Clc|   |   |   |   |   |   |   |Caps      |     |   |
+     * |-----------------------------------------------------------| ,-----------.
      * |   |   |   |                               |   |   |   |   | |   |   |   |
      * `-----------------------------------------------------------' `-----------'
      */
 
-    KEYMAP(\
-        TRNS,TRNS,TRNS,TRNS,TRNS,TRNS,TRNS,TRNS,TRNS,TRNS,TRNS,TRNS,TRNS,           TRNS,TRNS,SLEP, \
+    KEYMAP_ANSI(\
+        TRNS,     TRNS,TRNS,TRNS,TRNS,TRNS,TRNS,TRNS,TRNS,TRNS,TRNS,TRNS,TRNS,      TRNS,TRNS,SLEP, \
         TRNS,TRNS,TRNS,TRNS,TRNS,TRNS,TRNS,TRNS,TRNS,TRNS,MUTE,VOLD,VOLU,TRNS,      TRNS,TRNS,TRNS, \
         TRNS,TRNS,TRNS,TRNS,TRNS,TRNS,TRNS,TRNS,TRNS,MSTP,MPLY,MPRV,MNXT,MSEL,      TRNS,TRNS,TRNS, \
-        TRNS,TRNS,TRNS,TRNS,TRNS,TRNS,TRNS,TRNS,TRNS,TRNS,TRNS,TRNS,     TRNS,      TRNS,TRNS,TRNS, \
-        TRNS,TRNS,TRNS,CALC,TRNS,TRNS,TRNS,TRNS,TRNS,TRNS,TRNS,          CAPS,      TRNS,TRNS,TRNS, \
-        TRNS,TRNS,TRNS,          TRNS,                    TRNS,TRNS,TRNS,TRNS,      TRNS,TRNS,TRNS),
+        TRNS,TRNS,TRNS,TRNS,TRNS,TRNS,TRNS,TRNS,TRNS,TRNS,TRNS,TRNS,     TRNS,                      \
+        TRNS,     TRNS,TRNS,CALC,TRNS,TRNS,TRNS,TRNS,TRNS,TRNS,TRNS,     CAPS,           TRNS,      \
+        TRNS,TRNS,TRNS,               TRNS,               TRNS,TRNS,TRNS,TRNS,      TRNS,TRNS,TRNS)
 };
 
 /*
diff --git a/keyboard/phantom/matrix.c b/keyboard/phantom/matrix.c
index 7b2461dc..386feea4 100644
--- a/keyboard/phantom/matrix.c
+++ b/keyboard/phantom/matrix.c
@@ -27,91 +27,10 @@ static uint8_t debouncing = DEBOUNCE;
 static matrix_row_t matrix[MATRIX_ROWS];
 static matrix_row_t matrix_debouncing[MATRIX_ROWS];
 
-
-#define _DDRA (uint8_t *const)&DDRA
-#define _DDRB (uint8_t *const)&DDRB
-#define _DDRC (uint8_t *const)&DDRC
-#define _DDRD (uint8_t *const)&DDRD
-#define _DDRE (uint8_t *const)&DDRE
-#define _DDRF (uint8_t *const)&DDRF
-
-#define _PINA (uint8_t *const)&PINA
-#define _PINB (uint8_t *const)&PINB
-#define _PINC (uint8_t *const)&PINC
-#define _PIND (uint8_t *const)&PIND
-#define _PINE (uint8_t *const)&PINE
-#define _PINF (uint8_t *const)&PINF
-
-#define _PORTA (uint8_t *const)&PORTA
-#define _PORTB (uint8_t *const)&PORTB
-#define _PORTC (uint8_t *const)&PORTC
-#define _PORTD (uint8_t *const)&PORTD
-#define _PORTE (uint8_t *const)&PORTE
-#define _PORTF (uint8_t *const)&PORTF
-
-#define _BIT0 0x01
-#define _BIT1 0x02
-#define _BIT2 0x04
-#define _BIT3 0x08
-#define _BIT4 0x10
-#define _BIT5 0x20
-#define _BIT6 0x40
-#define _BIT7 0x80
-
-/* Specifies the ports and pin numbers for the rows */
-static
-uint8_t *const row_ddr[MATRIX_ROWS] = {_DDRB,  _DDRB,  _DDRB,  _DDRB,  _DDRB,  _DDRB};
-
-static
-uint8_t *const row_port[MATRIX_ROWS] = {_PORTB, _PORTB, _PORTB, _PORTB, _PORTB, _PORTB};
-
-static
-uint8_t *const row_pin[MATRIX_ROWS] = {_PINB,  _PINB,  _PINB,  _PINB,  _PINB,  _PINB};
-
-static
-const uint8_t row_bit[MATRIX_ROWS] = { _BIT0,  _BIT1,  _BIT2,  _BIT3,  _BIT4,  _BIT5};
-
-/* Specifies the ports and pin numbers for the columns */
-static
-uint8_t *const  col_ddr[MATRIX_COLS] = { _DDRD,  _DDRC,  _DDRC,  _DDRD,  _DDRD,  _DDRE,
-				  _DDRF,  _DDRF,  _DDRF,  _DDRF,  _DDRF,  _DDRF,
-				  _DDRD,  _DDRD,  _DDRD,  _DDRD,  _DDRD};
-
-static
-uint8_t *const col_port[MATRIX_COLS] = {_PORTD, _PORTC, _PORTC, _PORTD, _PORTD, _PORTE,
-				 _PORTF, _PORTF, _PORTF, _PORTF, _PORTF, _PORTF,
-				 _PORTD, _PORTD, _PORTD, _PORTD, _PORTD};
-
-static
-const uint8_t   col_bit[MATRIX_COLS] = {  _BIT5,  _BIT7,  _BIT6,  _BIT4,  _BIT0,  _BIT6,
-				  _BIT0,  _BIT1,  _BIT4,  _BIT5,  _BIT6,  _BIT7,
-				  _BIT7,  _BIT6,  _BIT1,  _BIT2,  _BIT3};
-
-static
-inline void pull_column(int col) {
-  *col_port[col] &= ~col_bit[col];
-}
-
-static
-inline void release_column(int col) {
-  *col_port[col] |=  col_bit[col];
-}
-
-/* PORTB is set as input with pull-up resistors
-   PORTC,D,E,F are set to high output */
-
-static
-void setup_io_pins(void) {
-  uint8_t row, col;
-  for(row = 0; row < MATRIX_ROWS; row++) {
-    *row_ddr[row]  &= ~row_bit[row];
-    *row_port[row] |=  row_bit[row];
-  }
-  for(col = 0; col < MATRIX_COLS; col++) {
-    *col_ddr[col]  |= col_bit[col];
-    *col_port[col] |= col_bit[col];
-  }
-}
+static uint8_t read_rows(void);
+static void init_rows(void);
+static void unselect_cols(void);
+static void select_col(uint8_t col);
 
 /* LEDs are on output compare pins OC1B OC1C
    This activates fast PWM mode on them.
@@ -123,22 +42,22 @@ void setup_io_pins(void) {
    reaches the value in the output compare register,
    and are turned on when it reaches TOP (=256). */
 static
-void setup_leds(void) {
-  TCCR1A |=      // Timer control register 1A
-    (1<<WGM10) | // Fast PWM 8-bit
-    (1<<COM1B1)| // Clear OC1B on match, set at TOP
-    (1<<COM1C1); // Clear OC1C on match, set at TOP
-  TCCR1B |=      // Timer control register 1B
-    (1<<WGM12) | // Fast PWM 8-bit
-    (1<<CS12);   // Prescaler 256
-  OCR1B = 250;    // Output compare register 1B
-  OCR1C = 250;    // Output compare register 1C
-  // LEDs: LED_A -> PORTB6, LED_B -> PORTB7
-  DDRB  &= 0x3F;
-  PORTB &= 0x3F;
+void setup_leds(void)
+{
+    TCCR1A |=      // Timer control register 1A
+        (1<<WGM10) | // Fast PWM 8-bit
+        (1<<COM1B1)| // Clear OC1B on match, set at TOP
+        (1<<COM1C1); // Clear OC1C on match, set at TOP
+    TCCR1B |=      // Timer control register 1B
+        (1<<WGM12) | // Fast PWM 8-bit
+        (1<<CS12);   // Prescaler 256
+    OCR1B = 250;    // Output compare register 1B
+    OCR1C = 250;    // Output compare register 1C
+    // LEDs: LED_A -> PORTB6, LED_B -> PORTB7
+    DDRB  &= 0x3F;
+    PORTB &= 0x3F;
 }
 
-
 inline
 uint8_t matrix_rows(void)
 {
@@ -158,11 +77,12 @@ void matrix_init(void)
     MCUCR |= (1<<JTD);
 	
     // initialize row and col
-    setup_io_pins();
+    unselect_cols();
+    init_rows();
     setup_leds();
 
     // initialize matrix state: all keys off
-    for (uint8_t i=0; i < MATRIX_ROWS; i++)  {
+    for (uint8_t i = 0; i < MATRIX_ROWS; i++)  {
         matrix[i] = 0;
         matrix_debouncing[i] = 0;
     }
@@ -171,11 +91,12 @@ void matrix_init(void)
 uint8_t matrix_scan(void)
 {
     for (uint8_t col = 0; col < MATRIX_COLS; col++) {  // 0-16
-        pull_column(col);   // output hi on theline
+        select_col(col);
         _delay_us(3);       // without this wait it won't read stable value.
+        uint8_t rows = read_rows();
         for (uint8_t row = 0; row < MATRIX_ROWS; row++) {  // 0-5
             bool prev_bit = matrix_debouncing[row] & ((matrix_row_t)1<<col);
-            bool curr_bit = !(*row_pin[row] & row_bit[row]);
+            bool curr_bit = rows & (1<<row);
             if (prev_bit != curr_bit) {
                 matrix_debouncing[row] ^= ((matrix_row_t)1<<col);
                 if (debouncing) {
@@ -184,7 +105,7 @@ uint8_t matrix_scan(void)
                 debouncing = DEBOUNCE;
             }
         }
-        release_column(col);
+        unselect_cols();
     }
 
     if (debouncing) {
@@ -202,7 +123,7 @@ uint8_t matrix_scan(void)
 
 bool matrix_is_modified(void)
 {
-    // NOTE: no longer used
+    if (debouncing) return false;
     return true;
 }
 
@@ -220,10 +141,10 @@ matrix_row_t matrix_get_row(uint8_t row)
 
 void matrix_print(void)
 {
-    print("\nr/c 01234567\n");
+    print("\nr/c 0123456789ABCDEF\n");
     for (uint8_t row = 0; row < MATRIX_ROWS; row++) {
         phex(row); print(": ");
-        pbin_reverse(matrix_get_row(row));
+        print_bin_reverse32(matrix_get_row(row));
         print("\n");
     }
 }
@@ -232,10 +153,120 @@ uint8_t matrix_key_count(void)
 {
     uint8_t count = 0;
     for (uint8_t i = 0; i < MATRIX_ROWS; i++) {
-        for (uint8_t j = 0; j < MATRIX_COLS; j++) {
-            if (matrix_is_on(i, j))
-                count++;
-        }
+        count += bitpop32(matrix[i]);
     }
     return count;
 }
+
+/* Row pin configuration
+ * row: 0   1   2   3   4   5
+ * pin: B0  B1  B2  B3  B4  B5
+ */
+static void init_rows(void)
+{
+    // Input with pull-up(DDR:0, PORT:1)
+    DDRB  &= ~0b00111111;
+    PORTB |= 0b00111111;
+}
+
+static uint8_t read_rows(void)
+{
+    return (PINB&(1<<0) ? 0 : (1<<0)) |
+           (PINB&(1<<1) ? 0 : (1<<1)) |
+           (PINB&(1<<2) ? 0 : (1<<2)) |
+           (PINB&(1<<3) ? 0 : (1<<3)) |
+           (PINB&(1<<4) ? 0 : (1<<4)) |
+           (PINB&(1<<5) ? 0 : (1<<5));
+}
+
+/* Column pin configuration
+ * col: 0   1   2   3   4   5   6   7   8   9   10  11  12  13  14  15  16
+ * pin: D5  C7  C6  D4  D0  E6  F0  F1  F4  F5  F6  F7  D7  D6  D1  D2  D3
+ */
+static void unselect_cols(void)
+{
+    // Hi-Z(DDR:0, PORT:0) to unselect
+    DDRC  |= 0b11000000; // PC: 7 6
+    PORTC |= 0b11000000;
+    DDRD  |= 0b11111111; // PD: 7 6 5 4 3 2 1 0
+    PORTD |= 0b11111111;
+    DDRE  |= 0b01000000; // PE: 6
+    PORTE |= 0b01000000;
+    DDRF  |= 0b11110011; // PF: 7 6 5 4 1 0
+    PORTF |= 0b11110011;
+}
+
+static void select_col(uint8_t col)
+{
+    // Output low(DDR:1, PORT:0) to select
+    switch (col) {
+        case 0:
+            DDRD  |= (1<<5);
+            PORTD &= ~(1<<5);
+            break;
+        case 1:
+            DDRC  |= (1<<7);
+            PORTC &= ~(1<<7);
+            break;
+        case 2:
+            DDRC  |= (1<<6);
+            PORTC &= ~(1<<6);
+            break;
+        case 3:
+            DDRD  |= (1<<4);
+            PORTD &= ~(1<<4);
+            break;
+        case 4:
+            DDRD  |= (1<<0);
+            PORTD &= ~(1<<0);
+            break;
+        case 5:
+            DDRE  |= (1<<6);
+            PORTE &= ~(1<<6);
+            break;
+        case 6:
+            DDRF  |= (1<<0);
+            PORTF &= ~(1<<0);
+            break;
+        case 7:
+            DDRF  |= (1<<1);
+            PORTF &= ~(1<<1);
+            break;
+        case 8:
+            DDRF  |= (1<<4);
+            PORTF &= ~(1<<4);
+            break;
+        case 9:
+            DDRF  |= (1<<5);
+            PORTF &= ~(1<<5);
+            break;
+        case 10:
+            DDRF  |= (1<<6);
+            PORTF &= ~(1<<6);
+            break;
+        case 11:
+            DDRF  |= (1<<7);
+            PORTF &= ~(1<<7);
+            break;
+        case 12:
+            DDRD  |= (1<<7);
+            PORTD &= ~(1<<7);
+            break;
+        case 13:
+            DDRD  |= (1<<6);
+            PORTD &= ~(1<<6);
+            break;
+        case 14:
+            DDRD  |= (1<<1);
+            PORTD &= ~(1<<1);
+            break;
+        case 15:
+            DDRD  |= (1<<2);
+            PORTD &= ~(1<<2);
+            break;
+        case 16:
+            DDRD  |= (1<<3);
+            PORTD &= ~(1<<3);
+            break;
+    }
+}