diff --git a/src/main.c b/src/main.c
index 13ad28e..31e7aba 100644
--- a/src/main.c
+++ b/src/main.c
@@ -350,21 +350,8 @@ keyfunc kb_keyfunc	(layer l, u8 row, u8 col)	{ return (keyfunc) pgm_read_word(&(
 void _kbfun_normal_press_release(keycode key, bool is_pressed) {
   if (key == 0) { return; } // noop
 
-  if (is_pressed) {
-    for (u8 i=0; i < sizeof(keyboard_keys); i++) {
-      if (keyboard_keys[i] == 0) {
-        keyboard_keys[i] = key;
-        return;
-      }
-    }
-  } else {
-    for (u8 i=0; i < sizeof(keyboard_keys); i++) {
-      if (keyboard_keys[i] == key) {
-        keyboard_keys[i] = 0;
-        return;
-      }
-    }
-  }
+  if (is_pressed)	{ _kbfun_normal_swap(0, key); }
+  else           	{ _kbfun_normal_swap(key, 0); }
 }
 
 void _kbfun_mediakey_press_release(keycode key, bool is_pressed) {
@@ -387,6 +374,15 @@ void _kbfun_modifier_press_release(keycode key, bool is_pressed) {
   }
 }
 
+void _kbfun_normal_swap(keycode from, keycode to) {
+  for (u8 i=0; i < sizeof(keyboard_keys); i++) {
+    if (keyboard_keys[i] == from) {
+      keyboard_keys[i] = to;
+      return;
+    }
+  }
+}
+
 bool _kbfun_normal_is_pressed(keycode key) {
   for (u8 i=0; i < sizeof(keyboard_keys); i++) {
     if (keyboard_keys[i] == key) {
diff --git a/src/main.h b/src/main.h
index 347f739..4cc4dd9 100644
--- a/src/main.h
+++ b/src/main.h
@@ -21,6 +21,7 @@ bool _kbfun_mediakey_is_pressed(keycode key);
 bool _kbfun_normal_is_pressed(keycode key);
 void _kbfun_modifier_press_release(keycode key,bool is_pressed);
 void _kbfun_mediakey_press_release(keycode key,bool is_pressed);
+void _kbfun_normal_swap(keycode from,keycode to);
 void _kbfun_normal_press_release(keycode key,bool is_pressed);
 keycode kb_keycode(layer l,u8 row,u8 col);
 bool is_layer_keyfunc(keyfunc f);