infinity: Add initial files for keyboard support

13 files changed, 1007 insertions, 406 deletions

diff --git a/keyboard/infinity/MEMO.txt b/keyboard/infinity/MEMO.txt
deleted file mode 100644
index ca0da937d7..0000000000
--- a/keyboard/infinity/MEMO.txt
+++ /dev/null
@@ -1,398 +0,0 @@
-mbed patch for Infinity
------------------------
-Without ld script patch vector table it doesn't place vector table in binary file.
-And clock setting is changed as Infinity uses internal oscillator instead of exteranl crystal.
-
-diff --git a/libraries/mbed/targets/cmsis/TARGET_Freescale/TARGET_K20D50M/TOOLCHAIN_GCC_ARM/MK20D5.ld b/libraries/mbed/targets/cmsis/TARGET_Freescale/TARGET_K20D50M/TOOLCHAIN_GCC_ARM/MK20D5.ld
-index 600751c..55c3393 100644
---- a/libraries/mbed/targets/cmsis/TARGET_Freescale/TARGET_K20D50M/TOOLCHAIN_GCC_ARM/MK20D5.ld
-+++ b/libraries/mbed/targets/cmsis/TARGET_Freescale/TARGET_K20D50M/TOOLCHAIN_GCC_ARM/MK20D5.ld
-@@ -43,7 +43,7 @@ SECTIONS
-     .isr_vector :
-     {
-         __vector_table = .;
--        KEEP(*(.vector_table))
-+        KEEP(*(.isr_vector))
-         *(.text.Reset_Handler)
-         *(.text.System_Init)
-          . = ALIGN(4);
-diff --git a/libraries/mbed/targets/cmsis/TARGET_Freescale/TARGET_K20D50M/system_MK20D5.c b/libraries/mbed/targets/cmsis/TARGET_Freescale/TARGET_K20D50M/system_MK20D5.c
-index 393d1f0..b78b71a 100644
---- a/libraries/mbed/targets/cmsis/TARGET_Freescale/TARGET_K20D50M/system_MK20D5.c
-+++ b/libraries/mbed/targets/cmsis/TARGET_Freescale/TARGET_K20D50M/system_MK20D5.c
-@@ -44,7 +44,7 @@
-
- #define DISABLE_WDOG    1
-
--#define CLOCK_SETUP     1
-+#define CLOCK_SETUP     0
- /* Predefined clock setups
-    0 ... Multipurpose Clock Generator (MCG) in FLL Engaged Internal (FEI) mode
-          Reference clock source for MCG module is the slow internal clock source 32.768kHz
-
-
-
-CMSIS/mbed HAL
-==============
-http://developer.mbed.org/users/MACRUM/notebook/mbed-library-internals/
-
-api/        mbed API
-├── AnalogIn.h
-├── AnalogOut.h
-├── BusIn.h
-├── BusInOut.h
-├── BusOut.h
-├── CallChain.h
-├── CAN.h
-├── can_helper.h
-├── DigitalIn.h
-├── DigitalInOut.h
-├── DigitalOut.h
-├── DirHandle.h
-├── Ethernet.h
-├── FileBase.h
-├── FileHandle.h
-├── FileLike.h
-├── FilePath.h
-├── FileSystemLike.h
-├── FunctionPointer.h
-├── I2C.h
-├── I2CSlave.h
-├── InterruptIn.h
-├── InterruptManager.h
-├── LocalFileSystem.h
-├── mbed_assert.h
-├── mbed_debug.h
-├── mbed_error.h
-├── mbed.h
-├── mbed_interface.h
-├── platform.h
-├── PortIn.h
-├── PortInOut.h
-├── PortOut.h
-├── PwmOut.h
-├── RawSerial.h
-├── rtc_time.h
-├── semihost_api.h
-├── SerialBase.h
-├── Serial.h
-├── SPI.h
-├── SPISlave.h
-├── Stream.h
-├── Ticker.h
-├── Timeout.h
-├── TimerEvent.h
-├── Timer.h
-├── toolchain.h
-└── wait_api.h
-
-common/     mbed API implementation
-
-hal/        mbed HAL implementation interface
-├── analogin_api.h
-├── analogout_api.h
-├── can_api.h
-├── ethernet_api.h
-├── gpio_api.h
-├── gpio_irq_api.h
-├── i2c_api.h
-├── pinmap.h
-├── port_api.h
-├── pwmout_api.h
-├── rtc_api.h
-├── serial_api.h
-├── sleep_api.h
-├── spi_api.h
-└── us_ticker_api.h
-
-targets/hal/TARGET_<vendor>/TARGET_<board>/
-    mbed HAL implementation
-
-targets/cmsis/      CMSIS interface
-    core_caInstr.h
-    core_ca9.h
-    core_cmFunc.h
-    core_caFunc.h
-    core_cmInstr.h
-    core_cm4_simd.h
-    core_ca_mmu.h
-    core_cm0.h
-    core_cm0plus.h
-    core_cm3.h
-    core_cm4.h
-
-targets/cmsis/TARGET_<vendor>/TARGET_<chip>/
-    <chip>.h
-    system_<chip>.[ch]
-    cmsis.h　
-    cmsis_nvic.[ch]
-
-targets/cmsis/TARGET_<vendor>/TARGET_<chip>/TOOLSCHAIN_<tool>/
-    startup_<chip>.s
-    linker-script
-
-
-
-Infinity
-========
-Massdrop Infinity Keyboard:
-https://www.massdrop.com/buy/infinity-keyboard-kit
-
-Freescale MK20DX128VLF5 48-QFP:
-http://cache.freescale.com/files/32bit/doc/data_sheet/K20P48M50SF0.pdf
-
-kiibohd controller(MD1):
-https://github.com/kiibohd/controller
-
-DFU bootloader:
-https://github.com/kiibohd/controller/tree/master/Bootloader
-
-Program with bootloader:
-    $ dfu-util -D kiibohd.dfu.bin
-
-Pinout:
-https://github.com/kiibohd/controller/blob/master/Scan/MD1/pinout
-
-
-
-Pin Usage
-=========
-
-mk20dx128vlf5
-
- ----
-|Keys|
- ----
-
-* Strobe (Columns)
-
-PTB0
-PTB1
-PTB2
-PTB3
-PTB16
-PTB17
-PTC4
-PTC5
-PTD0
-
-
-* Sense (Rows)
-
-PTD1
-PTD2
-PTD3
-PTD4
-PTD5
-PTD6
-PTD7
-
-
- -----
-|Debug|
- -----
-
-* SWD
-
-PTA0 (Pull-down)
-PTA3 (Pull-up)
-
-* LEDs
-
-PTA19 (LED only for PCB, not McHCK) (XTAL)
-
-* UARTs
-
-PTA1 - RX0
-PTA2 - TX0
-
-
- ------
-|Unused|
- ------
-
-* GPIO
-
-PTA1 (Not broken out on PCB, available on McHCK) (Pull-up)
-PTA2 (")
-PTA4 (Pull-up)
-PTA18 (EXTAL)
-
-PTC0
-PTC1
-PTC2
-PTC3
-PTC6
-PTC7
-
-* Analog
-
-ADC0_DP0
-ADC0_DM0
-
-
-
-Freescale kinetis MK20DX128
-===========================
-If FSEC of flash config at 0x400-40F is changed accidentally SWD/JTAG debug access will be lost and very difficult to get back.
-For example, high level adapter like stlink cannot work to get access back after FSEC is changed. To regain the chip to be programmable low level DAP inteface like JTAG, CMSIS-DAP or OpenSAD.
-
-
-Memory map
-==========
-kiibohd bootloader: Lib/mk20dx128vlf5.bootloader.ld
-0x0000_0000 +-------------------+ -----------------+---------------+
-            | .vectors          | ---------.       | StackPointer0 |
-            | .startup          |           \      | ResetHandler1 |
-            | .rodata           |            \     | ...           |
-0x0000_0400 | .flashconfig      | 0x10        \    | ...           |
-            | .text             |              \   | ...        61 | 0xF7
-            | .init             |               `--+---------------+ 0xF8
-0x0000_1000 +-------------------+ 4KB
-            | _app_rom          |
-            ~                   ~
-            ~                   ~
-            |                   |
-0x07FF_FFFF +-------------------+ 128KB
-
-
-0x1FFF_E000 +-------------------+
-            |                   |
-            | RAM               |
-            |                8KB|
-0x2000_0000 +-------------------+
-            |                   |
-            | RAM               |
-            |                8KB|
-0x2000_2000 +-------------------+ _estack
-
-
-
-
-
-OpenOCD
-========
-Synopsis of SWD, JTAG and SWJ-DP transport:
-https://fedcsis.org/proceedings/2012/pliks/279.pdf
-
-OpenSDA Freescale: Mass storage bootloader & serial port; part of CMSIS-DAP?
-http://cache.freescale.com/files/32bit/doc/user_guide/OPENSDAUG.pdf
-
-
-interface_list:
-    1: ftdi
-    2: usb_blaster
-    3: usbprog
-    4: jlink
-    5: vsllink
-    6: rlink
-    7: ulink
-    8: arm-jtag-ew
-    9: hla
-    10: osbdm
-    11: opendous
-    12: aice
-    13: cmsis-dap
-
-transport list:
-    stlink_swim
-    hla_jtag
-    hla_swd
-    aice_jtag
-    swd
-    cmsis-dap
-    jtag
-
-target types:
-    arm7tdmi
-    arm9tdmi
-    arm920t
-    arm720t
-    arm966e
-    arm946e
-    arm926ejs
-    fa526
-    feroceon
-    dragonite
-    xscale
-    cortex_m
-    cortex_a
-    cortex_r4
-    arm11
-    mips_m4k
-    avr
-    dsp563xx
-    dsp5680xx
-    testee
-    avr32_ap7k
-    hla_target
-    nds32_v2
-    nds32_v3
-    nds32_v3m
-    or1k
-    quark_x10xx
-
-
-TAP
----
-http://openocd.sourceforge.net/doc/html/TAP-Declaration.html#TAP-Declaration
-    jtag newtap chipname tapname configparams...
-    hla newtap chipname tapname configparams...
-    swd newtap chipname tapname configparams...
-    cmsis-dap newtap chipname tapname configparams...
-
-        The tapname reflects the role of that TAP, and should follow this convention:
-        bs – For boundary scan if this is a separate TAP;
-        cpu – The main CPU of the chip, alternatively arm and dsp on chips with both ARM and DSP CPUs, arm1 and arm2 on chips with two ARMs, and so forth;
-        etb – For an embedded trace buffer (example: an ARM ETB11);
-        flash – If the chip has a flash TAP, like the str912;
-        jrc – For JTAG route controller (example: the ICEPick modules on many Texas Instruments chips, like the OMAP3530 on Beagleboards);
-        tap – Should be used only for FPGA- or CPLD-like devices with a single TAP;
-        unknownN – If you have no idea what the TAP is for (N is a number);
-        when in doubt – Use the chip maker's name in their data sheet. For example, the Freescale i.MX31 has a SDMA (Smart DMA) with a JTAG TAP; that TAP should be named sdma.
-
--irlen NUMBER 
-The length in bits of the instruction register, such as 4 or 5 bits.
-
-Target
-------
-http://openocd.sourceforge.net/doc/html/CPU-Configuration.html#CPU-Configuration
-
-    target create target_name type configparams...
-    $target_name configure configparams...
-
-        -chain-position dotted.name – names the TAP used to access this target.
-        -endian (big|little) – specifies whether the CPU uses big or little endian conventions
-        -event event_name event_body – See Target Events. Note that this updates a list of named event handlers. Calling this twice with two different event names assigns two different handlers, but calling it twice with the same event name assigns only one handler.
-        -work-area-backup (0|1) – says whether the work area gets backed up; by default, it is not backed up. When possible, use a working_area that doesn't need to be backed up, since performing a backup slows down operations. For example, the beginning of an SRAM block is likely to be used by most build systems, but the end is often unused.
-        -work-area-size size – specify work are size, in bytes. The same size applies regardless of whether its physical or virtual address is being used.
-        -work-area-phys address – set the work area base address to be used when no MMU is active.
-        -work-area-virt address – set the work area base address to be used when an MMU is active. Do not specify a value for this except on targets with an MMU. The value should normally correspond to a static mapping for the -work-area-phys address, set up by the current operating system.
-        -rtos rtos_type – enable rtos support for target, rtos_type can be one of auto|eCos|ThreadX| FreeRTOS|linux|ChibiOS|embKernel See RTOS Support.
-
-Flash
------
-http://openocd.sourceforge.net/doc/html/Flash-Commands.html#norconfiguration
-
-    flash bank name driver base size chip_width bus_width target [driver_options]
-
-        name ... may be used to reference the flash bank in other flash commands. A number is also available.
-        driver ... identifies the controller driver associated with the flash bank being declared. This is usually cfi for external flash, or else the name of a microcontroller with embedded flash memory. See Flash Driver List.
-        base ... Base address of the flash chip.
-        size ... Size of the chip, in bytes. For some drivers, this value is detected from the hardware.
-        chip_width ... Width of the flash chip, in bytes; ignored for most microcontroller drivers.
-        bus_width ... Width of the data bus used to access the chip, in bytes; ignored for most microcontroller drivers.
-        target ... Names the target used to issue commands to the flash controller.
-        driver_options ... drivers may support, or require, additional parameters. See the driver-specific documentation for more information.
-
-    flash write_image [erase] [unlock] filename [offset] [type]
-
-
-Memory Access
--------------
-http://openocd.sourceforge.net/doc/html/General-Commands.html#imageaccess
-    {mdw,mdh,mdb} addr [count]
-        dump {word, half-word, byte} data
-    {mww,mwh,mwb} addr data
-        write data
-
diff --git a/keyboard/infinity/Makefile b/keyboard/infinity/Makefile
index e97f759dfa..758296f335 100644
--- a/keyboard/infinity/Makefile
+++ b/keyboard/infinity/Makefile
@@ -11,6 +11,10 @@ vpath %.cpp .:$(MBED_DIR):$(TMK_DIR)
 OBJDIR = ./build
 
 OBJECTS = \
+	$(OBJDIR)/matrix.o \
+	$(OBJDIR)/keymap.o \
+	$(OBJDIR)/keymap_common.o \
+	$(OBJDIR)/led.o \
 	$(OBJDIR)/main.o
 
 CONFIG_H = config.h
@@ -29,5 +33,5 @@ include $(TMK_DIR)/tool/mbed/mbed.mk
 include $(TMK_DIR)/tool/mbed/common.mk
 include $(TMK_DIR)/tool/mbed/gcc.mk
 
-program:
+program: $(OBJDIR)/$(PROJECT).bin
 	dfu-util -D $(OBJDIR)/$(PROJECT).bin
diff --git a/keyboard/infinity/README b/keyboard/infinity/README
new file mode 100644
index 0000000000..53d1c91737
--- /dev/null
+++ b/keyboard/infinity/README
@@ -0,0 +1,81 @@
+Infinity
+========
+Massdrop Infinity Keyboard:
+https://www.massdrop.com/buy/infinity-keyboard-kit
+
+kiibohd controller(MD1):
+https://github.com/kiibohd/controller
+
+DFU bootloader:
+https://github.com/kiibohd/controller/tree/master/Bootloader
+
+Program with bootloader:
+    $ dfu-util -D kiibohd.dfu.bin
+
+Pinout:
+https://github.com/kiibohd/controller/blob/master/Scan/MD1/pinout
+
+MCHCK compatible:
+https://mchck.org/about/
+
+MCU Freescale MK20DX128VLF5 48-QFP:
+http://cache.freescale.com/files/32bit/doc/data_sheet/K20P48M50SF0.pdf
+
+
+
+Pin Usage
+=========
+Key Matrix:
+    Strobe(output high):            PTB0 PTB1 PTB2 PTB3 PTB16 PTB17 PTC4 PTC5 PTD0
+    Sense(input with pull-down):    PTD1 PTD2 PTD3 PTD4 PTD5 PTD6 PTD7
+
+
+SWD pinout:
+    SWD_CLK(PTA0) SWD_DIO(PTA3)
+    SWD pins are placed next to reset button; SWD_CLK, SWD_DIO, GND, VCC from top.
+    Note that RESET is also needed to get full control with OpenOCD.
+
+LED:
+    PTA19(turns on with output high)
+
+
+
+Memory map
+==========
+kiibohd bootloader: Lib/mk20dx128vlf5.bootloader.ld
+0x0000_0000 +-------------------+ -----------------+---------------+ Vector table
+            | .vectors          | ------------.    | StackPointer0 |     of Bootloader
+            | .startup          |              \   | ResetHandler1 |
+            | .rodata           |               `--+---------------+ 0xF8
+0x0000_0400 | .flashconfig(0x10)| 
+      _0410 | .text             |
+            | .init             |
+0x0000_1000 +-------------------+ -----------------+---------------+ Vector table
+            | _app_rom          | ------------.    |               |     of App
+            |                   |              \   |               |
+            |                   |               `--+---------------+
+            |                   |
+            ~                   ~
+            |                   |
+0x07FF_FFFF +-------------------+ 128KB
+
+
+0x1FFF_E000 +-------------------+ -----------------+---------------+ Vector table
+      _E0F8 |                   | ------------.    |               |     of App(copied)
+            |                   |              \   |               |  
+            |                   |               `--+---------------+ mbed NVIC
+            |                   |
+            | RAM               |
+            |                8KB|
+0x2000_0000 +-------------------+
+            |                   |
+            |                   |
+            |                   |
+            |                   |
+            | RAM               |
+            |                8KB|
+0x2000_2000 +-------------------+ _estack
+
+
+
+
diff --git a/keyboard/infinity/keymap.c b/keyboard/infinity/keymap.c
new file mode 100644
index 0000000000..100142f41c
--- /dev/null
+++ b/keyboard/infinity/keymap.c
@@ -0,0 +1,47 @@
+#include "keymap_common.h"
+
+const uint8_t PROGMEM keymaps[][MATRIX_ROWS][MATRIX_COLS] = {
+    /* Layer 0: Default Layer
+     * ,-----------------------------------------------------------.
+     * |Esc|  1|  2|  3|  4|  5|  6|  7|  8|  9|  0|  -|  =|  `|BSp|
+     * |-----------------------------------------------------------|
+     * |Tab  |  Q|  W|  E|  R|  T|  Y|  U|  I|  O|  P|  [|  ]|    \|
+     * |-----------------------------------------------------------|
+     * |Contro|  A|  S|  D|  F|  G|  H|  J|  K|  L|  ;|  '|Enter   |
+     * |-----------------------------------------------------------|
+     * |Shift   |  Z|  X|  C|  V|  B|  N|  M|  ,|  .|  /|Shift |Fn0|
+     * |-----------------------------------------------------------'
+     * |     |Gui|Alt  |         Space         |Alt  |Gui|   |     |
+     * `-----------------------------------------------------------'
+     */
+    [0] =
+    KEYMAP(ESC, 1,   2,   3,   4,   5,   6,   7,   8,   9,   0,   MINS,EQL, BSLS, GRV, \
+           TAB, Q,   W,   E,   R,   T,   Y,   U,   I,   O,   P,   LBRC,RBRC,BSPC, \
+           LCTL,A,   S,   D,   F,   G,   H,   J,   K,   L,   SCLN,QUOT,ENT,  \
+           LSFT,Z,   X,   C,   V,   B,   N,   M,   COMM,DOT, SLSH,RSFT,FN0, \
+           NO,  LGUI,LALT,          SPC,                RALT,RGUI,NO, NO),
+    /* Layer 1: HHKB mode (HHKB Fn)
+     * ,-----------------------------------------------------------.
+     * |Pwr| F1| F2| F3| F4| F5| F6| F7| F8| F9|F10|F11|F12|Ins|Del|
+     * |-----------------------------------------------------------|
+     * |Caps |   |   |   |   |   |   |   |Psc|Slk|Pus|Up |   |Backs|
+     * |-----------------------------------------------------------|
+     * |      |VoD|VoU|Mut|   |   |  *|  /|Hom|PgU|Lef|Rig|Enter   |
+     * |-----------------------------------------------------------|
+     * |        |   |   |   |   |   |  +|  -|End|PgD|Dow|      |   |
+     * `-----------------------------------------------------------'
+     * |     |Gui|Alt  |         Space         |Alt  |Gui|   |     |
+     * `-----------------------------------------------------------'
+     */ 
+    [1]= 
+    KEYMAP(PWR, F1,  F2,  F3,  F4,  F5,  F6,  F7,  F8,  F9,  F10, F11, F12, INS, DEL,   \
+           CAPS,TRNS,TRNS,TRNS,TRNS,TRNS,TRNS,TRNS,PSCR,SLCK,PAUS, UP, TRNS, BSPC,      \
+           TRNS,VOLD,VOLU,MUTE,TRNS,TRNS,PAST,PSLS,HOME,PGUP,LEFT,RGHT,PENT,            \
+           TRNS,TRNS,TRNS,TRNS,TRNS,TRNS,PPLS,PMNS,END, PGDN,DOWN,TRNS,TRNS,            \
+           TRNS,TRNS,TRNS,          TRNS,               TRNS,TRNS,TRNS,TRNS),
+};
+
+const uint16_t PROGMEM fn_actions[] = {
+    [0] = ACTION_LAYER_MOMENTARY(1),
+};
+
diff --git a/keyboard/infinity/keymap_common.c b/keyboard/infinity/keymap_common.c
new file mode 100644
index 0000000000..fdb1769e1c
--- /dev/null
+++ b/keyboard/infinity/keymap_common.c
@@ -0,0 +1,30 @@
+/*
+Copyright 2012,2013 Jun Wako <wakojun@gmail.com>
+
+This program is free software: you can redistribute it and/or modify
+it under the terms of the GNU General Public License as published by
+the Free Software Foundation, either version 2 of the License, or
+(at your option) any later version.
+
+This program is distributed in the hope that it will be useful,
+but WITHOUT ANY WARRANTY; without even the implied warranty of
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+GNU General Public License for more details.
+
+You should have received a copy of the GNU General Public License
+along with this program.  If not, see <http://www.gnu.org/licenses/>.
+*/
+#include "keymap_common.h"
+
+
+/* translates key to keycode */
+uint8_t keymap_key_to_keycode(uint8_t layer, keypos_t key)
+{
+    return pgm_read_byte(&keymaps[(layer)][(key.row)][(key.col)]);
+}
+
+/* translates Fn keycode to action */
+action_t keymap_fn_to_action(uint8_t keycode)
+{
+    return (action_t){ .code = pgm_read_word(&fn_actions[FN_INDEX(keycode)]) };
+}
diff --git a/keyboard/infinity/keymap_common.h b/keyboard/infinity/keymap_common.h
new file mode 100644
index 0000000000..7a5f778f2d
--- /dev/null
+++ b/keyboard/infinity/keymap_common.h
@@ -0,0 +1,58 @@
+/*
+Copyright 2014 Jun Wako <wakojun@gmail.com>
+
+This program is free software: you can redistribute it and/or modify
+it under the terms of the GNU General Public License as published by
+the Free Software Foundation, either version 2 of the License, or
+(at your option) any later version.
+
+This program is distributed in the hope that it will be useful,
+but WITHOUT ANY WARRANTY; without even the implied warranty of
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+GNU General Public License for more details.
+
+You should have received a copy of the GNU General Public License
+along with this program.  If not, see <http://www.gnu.org/licenses/>.
+*/
+#ifndef KEYMAP_COMMON_H
+#define KEYMAP_COMMON_H
+
+#include <stdint.h>
+#include <stdbool.h>
+#include "keycode.h"
+#include "action.h"
+#include "action_macro.h"
+#include "report.h"
+#include "host.h"
+#include "print.h"
+#include "debug.h"
+#include "keymap.h"
+
+
+extern const uint8_t keymaps[][MATRIX_ROWS][MATRIX_COLS];
+extern const uint16_t fn_actions[];
+
+
+/* GH60 keymap definition macro
+ * K2C, K31 and  K3C are extra keys for ISO
+ */
+#define KEYMAP( \
+    K00, K10, K20, K30, K40, K50, K60, K70, K80, K01, K11, K21, K31, K41, K86, \
+    K51, K61, K71, K81, K02, K12, K22, K32, K42, K52, K62, K72, K82, K03, \
+    K13, K23, K33, K43, K53, K63, K73, K83, K04, K14, K24, K34, K44, \
+    K54, K64, K74, K84, K05, K15, K25, K35, K45, K55, K65, K75, K85, \
+    K06, K16, K26,           K36,                K46, K56, K66, K76 \
+) { \
+    { KC_##K00, KC_##K01, KC_##K02, KC_##K03, KC_##K04, KC_##K05, KC_##K06 }, \
+    { KC_##K10, KC_##K11, KC_##K12, KC_##K13, KC_##K14, KC_##K15, KC_##K16 }, \
+    { KC_##K20, KC_##K21, KC_##K22, KC_##K23, KC_##K24, KC_##K25, KC_##K26 }, \
+    { KC_##K30, KC_##K31, KC_##K32, KC_##K33, KC_##K34, KC_##K35, KC_##K36 }, \
+    { KC_##K40, KC_##K41, KC_##K42, KC_##K43, KC_##K44, KC_##K45, KC_##K46 }, \
+    { KC_##K50, KC_##K51, KC_##K52, KC_##K53, KC_##K54, KC_##K55, KC_##K56 }, \
+    { KC_##K60, KC_##K61, KC_##K62, KC_##K63, KC_##K64, KC_##K65, KC_##K66 }, \
+    { KC_##K70, KC_##K71, KC_##K72, KC_##K73, KC_##K74, KC_##K75, KC_##K76 }, \
+    { KC_##K80, KC_##K81, KC_##K82, KC_##K83, KC_##K84, KC_##K85, KC_##K86 }  \
+}
+
+#endif
+
diff --git a/keyboard/infinity/led.c b/keyboard/infinity/led.c
new file mode 100644
index 0000000000..b7e638b254
--- /dev/null
+++ b/keyboard/infinity/led.c
@@ -0,0 +1,25 @@
+/*
+Copyright 2011 Jun Wako <wakojun@gmail.com>
+
+This program is free software: you can redistribute it and/or modify
+it under the terms of the GNU General Public License as published by
+the Free Software Foundation, either version 2 of the License, or
+(at your option) any later version.
+
+This program is distributed in the hope that it will be useful,
+but WITHOUT ANY WARRANTY; without even the implied warranty of
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+GNU General Public License for more details.
+
+You should have received a copy of the GNU General Public License
+along with this program.  If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#include "stdint.h"
+#include "led.h"
+
+
+/* HHKB has no LEDs */
+void led_set(uint8_t usb_led)
+{
+}
diff --git a/keyboard/infinity/main.cpp b/keyboard/infinity/main.cpp
index f28840d3ef..847668a1de 100644
--- a/keyboard/infinity/main.cpp
+++ b/keyboard/infinity/main.cpp
@@ -1,20 +1,44 @@
-//#include "mbed.h"
 #include "MK20D5.h"
 #include "wait.h"
 #include "gpio_api.h"
 #include "PinNames.h"
+#include "matrix.h"
+#include "timer.h"
+
+#include "action.h"
+#include "keycode.h"
+#include "host.h"
+#include "host_driver.h"
+#include "mbed_driver.h"
 
 
 int main() {
     gpio_t led;
     gpio_init_out(&led, PTA19);
+
+    uint16_t t = 0;
+
+    host_set_driver(&mbed_driver);
+    keyboard_init();
+
     while(1) {
-        wait_ms(500);
-        uint32_t delay = 0xf;
-        while (delay--) {
-            uint32_t delay2 = 0xffff;
-            while (delay2--) ;
+        keyboard_task();
+
+        bool matrix_on = false;
+        matrix_scan();
+        for (int i = 0; i < MATRIX_ROWS; i++) {
+            if (matrix_get_row(i)) {
+                matrix_on = true;
+                break;
+            }
+        }
+        if (matrix_on)
+            gpio_write(&led, 1);
+        else {
+            if (timer_elapsed(t) > 500) {
+                gpio_write(&led, !gpio_read(&led));
+                t = timer_read();
+            }
         }
-        gpio_write(&led, !gpio_read(&led));
     }
 }
diff --git a/keyboard/infinity/matrix.c b/keyboard/infinity/matrix.c
new file mode 100644
index 0000000000..478a40fd9e
--- /dev/null
+++ b/keyboard/infinity/matrix.c
@@ -0,0 +1,107 @@
+#include <stdint.h>
+#include <stdbool.h>
+#include "gpio_api.h"
+#include "timer.h"
+#include "wait.h"
+#include "matrix.h"
+
+
+#ifndef DEBOUNCE
+#define DEBOUNCE    5
+#endif
+
+/*
+ * Infinity Pinusage:
+ * Column pins are input with internal pull-down. Row pins are output and strobe with high.
+ * Key is high or 1 when it turns on.
+ *
+ *     col: { PTD1, PTD2, PTD3, PTD4, PTD5, PTD6, PTD7 }
+ *     row: { PTB0, PTB1, PTB2, PTB3, PTB16, PTB17, PTC4, PTC5, PTD0 }
+ */
+static gpio_t col[MATRIX_COLS];
+static gpio_t row[MATRIX_ROWS];
+
+/* matrix state(1:on, 0:off) */
+static matrix_row_t matrix[MATRIX_ROWS];
+static matrix_row_t matrix_debouncing[MATRIX_ROWS];
+static bool debouncing = false;
+static uint16_t debouncing_time = 0;
+
+
+void matrix_init(void)
+{
+    /* Column(sense) */
+    gpio_init_in_ex(&col[0], PTD1, PullDown);
+    gpio_init_in_ex(&col[1], PTD2, PullDown);
+    gpio_init_in_ex(&col[2], PTD3, PullDown);
+    gpio_init_in_ex(&col[3], PTD4, PullDown);
+    gpio_init_in_ex(&col[4], PTD5, PullDown);
+    gpio_init_in_ex(&col[5], PTD6, PullDown);
+    gpio_init_in_ex(&col[6], PTD7, PullDown);
+
+    /* Row(strobe) */
+    gpio_init_out_ex(&row[0], PTB0, 0);
+    gpio_init_out_ex(&row[1], PTB1, 0);
+    gpio_init_out_ex(&row[2], PTB2, 0);
+    gpio_init_out_ex(&row[3], PTB3, 0);
+    gpio_init_out_ex(&row[4], PTB16, 0);
+    gpio_init_out_ex(&row[5], PTB17, 0);
+    gpio_init_out_ex(&row[6], PTC4, 0);
+    gpio_init_out_ex(&row[7], PTC5, 0);
+    gpio_init_out_ex(&row[8], PTD0, 0);
+}
+
+uint8_t matrix_scan(void)
+{
+    for (int i = 0; i < MATRIX_ROWS; i++) {
+        matrix_row_t r = 0;
+
+        gpio_write(&row[i], 1);
+        wait_us(1); // need wait to settle pin state
+        for (int j = 0; j < MATRIX_COLS; j++) {
+            if (gpio_read(&col[j])) {
+                r |= (1<<j);
+            }
+        }
+        gpio_write(&row[i], 0);
+
+        if (matrix_debouncing[i] != r) {
+            matrix_debouncing[i] = r;
+            debouncing = true;
+            debouncing_time = timer_read();
+        }
+    }
+
+    if (debouncing && timer_elapsed(debouncing_time) > DEBOUNCE) {
+        for (int i = 0; i < MATRIX_ROWS; i++) {
+            matrix[i] = matrix_debouncing[i];
+        }
+        debouncing = false;
+    }
+/*
+    if (debouncing) {
+        if (--debouncing) {
+            return 0;
+        } else {
+            for (int i = 0; i < MATRIX_ROWS; i++) {
+                matrix[i] = matrix_debouncing[i];
+            }
+        }
+    }
+*/
+    return 1;
+}
+
+bool matrix_is_on(uint8_t row, uint8_t col)
+{
+    return (matrix[row] & (1<<col));
+}
+
+matrix_row_t matrix_get_row(uint8_t row)
+{
+    return matrix[row];
+}
+
+void matrix_print(void)
+{
+}
diff --git a/keyboard/infinity/mbed-infinity.mk b/keyboard/infinity/mbed-infinity.mk
index cd4434a230..f5fd2c9975 100644
--- a/keyboard/infinity/mbed-infinity.mk
+++ b/keyboard/infinity/mbed-infinity.mk
@@ -4,6 +4,7 @@
 CPU = -mcpu=cortex-m4 -mthumb
 
 CC_SYMBOLS += \
+	-DTARGET_INFINITY \
 	-DTARGET_K20D50M \
 	-DTARGET_M4 \
 	-DTARGET_CORTEX_M \
@@ -17,6 +18,7 @@ CC_SYMBOLS += \
 OBJECTS += \
 	$(OBJDIR)/mbed-infinity/cmsis_nvic.o \
 	$(OBJDIR)/mbed-infinity/system_MK20D5.o \
+	$(OBJDIR)/mbed-infinity/USBHAL_KL25Z.o \
 	$(OBJDIR)/libraries/mbed/targets/cmsis/TARGET_Freescale/TARGET_K20D50M/TOOLCHAIN_GCC_ARM/startup_MK20D5.o \
 	$(OBJDIR)/libraries/mbed/targets/hal/TARGET_Freescale/TARGET_K20D50M/analogin_api.o \
 	$(OBJDIR)/libraries/mbed/targets/hal/TARGET_Freescale/TARGET_K20D50M/gpio_api.o \
diff --git a/keyboard/infinity/mbed-infinity/README b/keyboard/infinity/mbed-infinity/README
new file mode 100644
index 0000000000..b0d226f793
--- /dev/null
+++ b/keyboard/infinity/mbed-infinity/README
@@ -0,0 +1,39 @@
+mbed fix for Infinity
+=====================
+Without linker script patch it doesn't place vector table in final binary.
+And clock is configured to 48MHz using internal clock reference and FLL multiplication.
+
+
+mbed/targets/cmsis/TARGET_Freescale/TARGET_K20D50M/system_MK20D5.c
+    Fix SystemInit: clock setup for internal clock. Inifinity has no external Xtal.
+
+mbed/targets/cmsis/TARGET_Freescale/TARGET_K20D50M/cmsis_nvic.c
+    Fix NVIC vector address of firmware 0x1000 instead of 0x0
+
+mbed/targets/cmsis/TARGET_Freescale/TARGET_K20D50M/TOOLCHAIN_GCC_ARM/MK20D5.ld
+    Fix memory map for Infinity bootloader
+        Flash starts at 0x1000
+        No flash config bytes sector
+
+USBDevice/USBDevice/USBHAL_KL25Z.cpp
+    Fix USB clock setup, see below.
+
+
+2015/01/04  Based on mbed-sdk @2f63fa7d78a26.
+
+
+
+Kinetis USB config
+==================
+Clock source: Internal reference clock wth FLL
+    SIM_SOPT[USBSRC] = 1(MCGPLLCLK/MCGFLLCLK)
+    SIM_SOPT[PLLSEL] = 0(MCGFLLCLK)
+
+Clock dividor:
+    SIM_CLKDIV2[USBDIV] = 0
+    SIM_CLKDIV2[USBFAC] = 0
+
+Clock enable:
+    SIM_SCGC4[USBOTG] = 1
+
+
diff --git a/keyboard/infinity/mbed-infinity/USBHAL_KL25Z.cpp b/keyboard/infinity/mbed-infinity/USBHAL_KL25Z.cpp
new file mode 100644
index 0000000000..90f02fa322
--- /dev/null
+++ b/keyboard/infinity/mbed-infinity/USBHAL_KL25Z.cpp
@@ -0,0 +1,557 @@
+/* Copyright (c) 2010-2011 mbed.org, MIT License
+*
+* Permission is hereby granted, free of charge, to any person obtaining a copy of this software
+* and associated documentation files (the "Software"), to deal in the Software without
+* restriction, including without limitation the rights to use, copy, modify, merge, publish,
+* distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the
+* Software is furnished to do so, subject to the following conditions:
+*
+* The above copyright notice and this permission notice shall be included in all copies or
+* substantial portions of the Software.
+*
+* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING
+* BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+* NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM,
+* DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+*/
+
+#if defined(TARGET_KL25Z) | defined(TARGET_KL43Z) | defined(TARGET_KL46Z) | defined(TARGET_K20D50M) | defined(TARGET_K64F) | defined(TARGET_K22F)
+
+#include "USBHAL.h"
+
+USBHAL * USBHAL::instance;
+
+static volatile int epComplete = 0;
+
+// Convert physical endpoint number to register bit
+#define EP(endpoint) (1<<(endpoint))
+
+// Convert physical to logical
+#define PHY_TO_LOG(endpoint)    ((endpoint)>>1)
+
+// Get endpoint direction
+#define IN_EP(endpoint)     ((endpoint) & 1U ? true : false)
+#define OUT_EP(endpoint)    ((endpoint) & 1U ? false : true)
+
+#define BD_OWN_MASK        (1<<7)
+#define BD_DATA01_MASK     (1<<6)
+#define BD_KEEP_MASK       (1<<5)
+#define BD_NINC_MASK       (1<<4)
+#define BD_DTS_MASK        (1<<3)
+#define BD_STALL_MASK      (1<<2)
+
+#define TX    1
+#define RX    0
+#define ODD   0
+#define EVEN  1
+// this macro waits a physical endpoint number
+#define EP_BDT_IDX(ep, dir, odd) (((ep * 4) + (2 * dir) + (1 *  odd)))
+
+#define SETUP_TOKEN    0x0D
+#define IN_TOKEN       0x09
+#define OUT_TOKEN      0x01
+#define TOK_PID(idx)   ((bdt[idx].info >> 2) & 0x0F)
+
+// for each endpt: 8 bytes
+typedef struct BDT {
+    uint8_t   info;       // BD[0:7]
+    uint8_t   dummy;      // RSVD: BD[8:15]
+    uint16_t  byte_count; // BD[16:32]
+    uint32_t  address;    // Addr
+} BDT;
+
+
+// there are:
+//    * 16 bidirectionnal endpt -> 32 physical endpt
+//    * as there are ODD and EVEN buffer -> 32*2 bdt
+__attribute__((__aligned__(512))) BDT bdt[NUMBER_OF_PHYSICAL_ENDPOINTS * 2];
+uint8_t * endpoint_buffer[(NUMBER_OF_PHYSICAL_ENDPOINTS - 2) * 2];
+uint8_t * endpoint_buffer_iso[2*2];
+
+static uint8_t set_addr = 0;
+static uint8_t addr = 0;
+
+static uint32_t Data1  = 0x55555555;
+
+static uint32_t frameNumber() {
+    return((USB0->FRMNUML | (USB0->FRMNUMH << 8)) & 0x07FF);
+}
+
+uint32_t USBHAL::endpointReadcore(uint8_t endpoint, uint8_t *buffer) {
+    return 0;
+}
+
+USBHAL::USBHAL(void) {
+    // Disable IRQ
+    NVIC_DisableIRQ(USB0_IRQn);
+
+#if defined(TARGET_K64F)
+    MPU->CESR=0;
+#endif
+    // fill in callback array
+    epCallback[0] = &USBHAL::EP1_OUT_callback;
+    epCallback[1] = &USBHAL::EP1_IN_callback;
+    epCallback[2] = &USBHAL::EP2_OUT_callback;
+    epCallback[3] = &USBHAL::EP2_IN_callback;
+    epCallback[4] = &USBHAL::EP3_OUT_callback;
+    epCallback[5] = &USBHAL::EP3_IN_callback;
+    epCallback[6] = &USBHAL::EP4_OUT_callback;
+    epCallback[7] = &USBHAL::EP4_IN_callback;
+    epCallback[8] = &USBHAL::EP5_OUT_callback;
+    epCallback[9] = &USBHAL::EP5_IN_callback;
+    epCallback[10] = &USBHAL::EP6_OUT_callback;
+    epCallback[11] = &USBHAL::EP6_IN_callback;
+    epCallback[12] = &USBHAL::EP7_OUT_callback;
+    epCallback[13] = &USBHAL::EP7_IN_callback;
+    epCallback[14] = &USBHAL::EP8_OUT_callback;
+    epCallback[15] = &USBHAL::EP8_IN_callback;
+    epCallback[16] = &USBHAL::EP9_OUT_callback;
+    epCallback[17] = &USBHAL::EP9_IN_callback;
+    epCallback[18] = &USBHAL::EP10_OUT_callback;
+    epCallback[19] = &USBHAL::EP10_IN_callback;
+    epCallback[20] = &USBHAL::EP11_OUT_callback;
+    epCallback[21] = &USBHAL::EP11_IN_callback;
+    epCallback[22] = &USBHAL::EP12_OUT_callback;
+    epCallback[23] = &USBHAL::EP12_IN_callback;
+    epCallback[24] = &USBHAL::EP13_OUT_callback;
+    epCallback[25] = &USBHAL::EP13_IN_callback;
+    epCallback[26] = &USBHAL::EP14_OUT_callback;
+    epCallback[27] = &USBHAL::EP14_IN_callback;
+    epCallback[28] = &USBHAL::EP15_OUT_callback;
+    epCallback[29] = &USBHAL::EP15_IN_callback;
+
+#if defined(TARGET_KL43Z)
+    // enable USBFS clock
+    SIM->SCGC4 |= SIM_SCGC4_USBFS_MASK;
+
+    // enable the IRC48M clock
+    USB0->CLK_RECOVER_IRC_EN |= USB_CLK_RECOVER_IRC_EN_IRC_EN_MASK;
+
+    // enable the USB clock recovery tuning
+    USB0->CLK_RECOVER_CTRL |= USB_CLK_RECOVER_CTRL_CLOCK_RECOVER_EN_MASK;
+
+    // choose usb src clock
+    SIM->SOPT2 |= SIM_SOPT2_USBSRC_MASK;
+#elif defined(TARGET_INFINITY)
+    // USB clock source: FLL
+    SIM->SOPT2 |= SIM_SOPT2_USBSRC_MASK;
+
+    // enable OTG clock
+    SIM->SCGC4 |= SIM_SCGC4_USBOTG_MASK;
+#else
+    // choose usb src as PLL
+    SIM->SOPT2 &= ~SIM_SOPT2_PLLFLLSEL_MASK;
+    SIM->SOPT2 |= (SIM_SOPT2_USBSRC_MASK | (1 << SIM_SOPT2_PLLFLLSEL_SHIFT));
+
+    // enable OTG clock
+    SIM->SCGC4 |= SIM_SCGC4_USBOTG_MASK;
+#endif
+
+    // Attach IRQ
+    instance = this;
+    NVIC_SetVector(USB0_IRQn, (uint32_t)&_usbisr);
+    NVIC_EnableIRQ(USB0_IRQn);
+
+    // USB Module Configuration
+    // Reset USB Module
+    USB0->USBTRC0 |= USB_USBTRC0_USBRESET_MASK;
+    while(USB0->USBTRC0 & USB_USBTRC0_USBRESET_MASK);
+
+    // Set BDT Base Register
+    USB0->BDTPAGE1 = (uint8_t)((uint32_t)bdt>>8);
+    USB0->BDTPAGE2 = (uint8_t)((uint32_t)bdt>>16);
+    USB0->BDTPAGE3 = (uint8_t)((uint32_t)bdt>>24);
+
+    // Clear interrupt flag
+    USB0->ISTAT = 0xff;
+
+    // USB Interrupt Enablers
+    USB0->INTEN |= USB_INTEN_TOKDNEEN_MASK |
+                   USB_INTEN_SOFTOKEN_MASK |
+                   USB_INTEN_ERROREN_MASK  |
+                   USB_INTEN_USBRSTEN_MASK;
+
+    // Disable weak pull downs
+    USB0->USBCTRL &= ~(USB_USBCTRL_PDE_MASK | USB_USBCTRL_SUSP_MASK);
+
+    USB0->USBTRC0 |= 0x40;
+}
+
+USBHAL::~USBHAL(void) { }
+
+void USBHAL::connect(void) {
+    // enable USB
+    USB0->CTL |= USB_CTL_USBENSOFEN_MASK;
+    // Pull up enable
+    USB0->CONTROL |= USB_CONTROL_DPPULLUPNONOTG_MASK;
+}
+
+void USBHAL::disconnect(void) {
+    // disable USB
+    USB0->CTL &= ~USB_CTL_USBENSOFEN_MASK;
+    // Pull up disable
+    USB0->CONTROL &= ~USB_CONTROL_DPPULLUPNONOTG_MASK;
+
+    //Free buffers if required:
+    for (int i = 0; i<(NUMBER_OF_PHYSICAL_ENDPOINTS - 2) * 2; i++) {
+        free(endpoint_buffer[i]);
+        endpoint_buffer[i] = NULL;
+    }
+    free(endpoint_buffer_iso[2]);
+    endpoint_buffer_iso[2] = NULL;
+    free(endpoint_buffer_iso[0]);
+    endpoint_buffer_iso[0] = NULL;
+}
+
+void USBHAL::configureDevice(void) {
+    // not needed
+}
+
+void USBHAL::unconfigureDevice(void) {
+    // not needed
+}
+
+void USBHAL::setAddress(uint8_t address) {
+    // we don't set the address now otherwise the usb controller does not ack
+    // we set a flag instead
+    // see usbisr when an IN token is received
+    set_addr = 1;
+    addr = address;
+}
+
+bool USBHAL::realiseEndpoint(uint8_t endpoint, uint32_t maxPacket, uint32_t flags) {
+    uint32_t handshake_flag = 0;
+    uint8_t * buf;
+
+    if (endpoint > NUMBER_OF_PHYSICAL_ENDPOINTS - 1) {
+        return false;
+    }
+
+    uint32_t log_endpoint = PHY_TO_LOG(endpoint);
+
+    if ((flags & ISOCHRONOUS) == 0) {
+        handshake_flag = USB_ENDPT_EPHSHK_MASK;
+        if (IN_EP(endpoint)) {
+            if (endpoint_buffer[EP_BDT_IDX(log_endpoint, TX, ODD)] == NULL)
+                endpoint_buffer[EP_BDT_IDX(log_endpoint, TX, ODD)] = (uint8_t *) malloc (64*2);
+            buf = &endpoint_buffer[EP_BDT_IDX(log_endpoint, TX, ODD)][0];
+        } else {
+            if (endpoint_buffer[EP_BDT_IDX(log_endpoint, RX, ODD)] == NULL)
+                endpoint_buffer[EP_BDT_IDX(log_endpoint, RX, ODD)] = (uint8_t *) malloc (64*2);
+            buf = &endpoint_buffer[EP_BDT_IDX(log_endpoint, RX, ODD)][0];
+        }
+    } else {
+        if (IN_EP(endpoint)) {
+            if (endpoint_buffer_iso[2] == NULL)
+                endpoint_buffer_iso[2] = (uint8_t *) malloc (1023*2);
+            buf = &endpoint_buffer_iso[2][0];
+        } else {
+            if (endpoint_buffer_iso[0] == NULL)
+                endpoint_buffer_iso[0] = (uint8_t *) malloc (1023*2);
+            buf = &endpoint_buffer_iso[0][0];
+        }
+    }
+
+    // IN endpt -> device to host (TX)
+    if (IN_EP(endpoint)) {
+        USB0->ENDPOINT[log_endpoint].ENDPT |= handshake_flag |        // ep handshaking (not if iso endpoint)
+                                              USB_ENDPT_EPTXEN_MASK;  // en TX (IN) tran
+        bdt[EP_BDT_IDX(log_endpoint, TX, ODD )].address = (uint32_t) buf;
+        bdt[EP_BDT_IDX(log_endpoint, TX, EVEN)].address = 0;
+    }
+    // OUT endpt -> host to device (RX)
+    else {
+        USB0->ENDPOINT[log_endpoint].ENDPT |= handshake_flag |        // ep handshaking (not if iso endpoint)
+                                              USB_ENDPT_EPRXEN_MASK;  // en RX (OUT) tran.
+        bdt[EP_BDT_IDX(log_endpoint, RX, ODD )].byte_count = maxPacket;
+        bdt[EP_BDT_IDX(log_endpoint, RX, ODD )].address    = (uint32_t) buf;
+        bdt[EP_BDT_IDX(log_endpoint, RX, ODD )].info       = BD_OWN_MASK | BD_DTS_MASK;
+        bdt[EP_BDT_IDX(log_endpoint, RX, EVEN)].info       = 0;
+    }
+
+    Data1 |= (1 << endpoint);
+
+    return true;
+}
+
+// read setup packet
+void USBHAL::EP0setup(uint8_t *buffer) {
+    uint32_t sz;
+    endpointReadResult(EP0OUT, buffer, &sz);
+}
+
+void USBHAL::EP0readStage(void) {
+    Data1 &= ~1UL;  // set DATA0
+    bdt[0].info = (BD_DTS_MASK | BD_OWN_MASK);
+}
+
+void USBHAL::EP0read(void) {
+    uint32_t idx = EP_BDT_IDX(PHY_TO_LOG(EP0OUT), RX, 0);
+    bdt[idx].byte_count = MAX_PACKET_SIZE_EP0;
+}
+
+uint32_t USBHAL::EP0getReadResult(uint8_t *buffer) {
+    uint32_t sz;
+    endpointReadResult(EP0OUT, buffer, &sz);
+    return sz;
+}
+
+void USBHAL::EP0write(uint8_t *buffer, uint32_t size) {
+    endpointWrite(EP0IN, buffer, size);
+}
+
+void USBHAL::EP0getWriteResult(void) {
+}
+
+void USBHAL::EP0stall(void) {
+    stallEndpoint(EP0OUT);
+}
+
+EP_STATUS USBHAL::endpointRead(uint8_t endpoint, uint32_t maximumSize) {
+    endpoint = PHY_TO_LOG(endpoint);
+    uint32_t idx = EP_BDT_IDX(endpoint, RX, 0);
+    bdt[idx].byte_count = maximumSize;
+    return EP_PENDING;
+}
+
+EP_STATUS USBHAL::endpointReadResult(uint8_t endpoint, uint8_t * buffer, uint32_t *bytesRead) {
+    uint32_t n, sz, idx, setup = 0;
+    uint8_t not_iso;
+    uint8_t * ep_buf;
+
+    uint32_t log_endpoint = PHY_TO_LOG(endpoint);
+
+    if (endpoint > NUMBER_OF_PHYSICAL_ENDPOINTS - 1) {
+        return EP_INVALID;
+    }
+
+    // if read on a IN endpoint -> error
+    if (IN_EP(endpoint)) {
+        return EP_INVALID;
+    }
+
+    idx = EP_BDT_IDX(log_endpoint, RX, 0);
+    sz  = bdt[idx].byte_count;
+    not_iso = USB0->ENDPOINT[log_endpoint].ENDPT & USB_ENDPT_EPHSHK_MASK;
+
+    //for isochronous endpoint, we don't wait an interrupt
+    if ((log_endpoint != 0) && not_iso && !(epComplete & EP(endpoint))) {
+        return EP_PENDING;
+    }
+
+    if ((log_endpoint == 0) && (TOK_PID(idx) == SETUP_TOKEN)) {
+        setup = 1;
+    }
+
+    // non iso endpoint
+    if (not_iso) {
+        ep_buf = endpoint_buffer[idx];
+    } else {
+        ep_buf = endpoint_buffer_iso[0];
+    }
+
+    for (n = 0; n < sz; n++) {
+        buffer[n] = ep_buf[n];
+    }
+
+    if (((Data1 >> endpoint) & 1) == ((bdt[idx].info >> 6) & 1)) {
+        if (setup && (buffer[6] == 0))  // if no setup data stage,
+            Data1 &= ~1UL;              // set DATA0
+        else
+            Data1 ^= (1 << endpoint);
+    }
+
+    if (((Data1 >> endpoint) & 1)) {
+        bdt[idx].info = BD_DTS_MASK | BD_DATA01_MASK | BD_OWN_MASK;
+    }
+    else {
+        bdt[idx].info = BD_DTS_MASK | BD_OWN_MASK;
+    }
+
+    USB0->CTL &= ~USB_CTL_TXSUSPENDTOKENBUSY_MASK;
+    *bytesRead = sz;
+
+    epComplete &= ~EP(endpoint);
+    return EP_COMPLETED;
+}
+
+EP_STATUS USBHAL::endpointWrite(uint8_t endpoint, uint8_t *data, uint32_t size) {
+    uint32_t idx, n;
+    uint8_t * ep_buf;
+
+    if (endpoint > NUMBER_OF_PHYSICAL_ENDPOINTS - 1) {
+        return EP_INVALID;
+    }
+
+    // if write on a OUT endpoint -> error
+    if (OUT_EP(endpoint)) {
+        return EP_INVALID;
+    }
+
+    idx = EP_BDT_IDX(PHY_TO_LOG(endpoint), TX, 0);
+    bdt[idx].byte_count = size;
+
+
+    // non iso endpoint
+    if (USB0->ENDPOINT[PHY_TO_LOG(endpoint)].ENDPT & USB_ENDPT_EPHSHK_MASK) {
+        ep_buf = endpoint_buffer[idx];
+    } else {
+        ep_buf = endpoint_buffer_iso[2];
+    }
+
+    for (n = 0; n < size; n++) {
+        ep_buf[n] = data[n];
+    }
+
+    if ((Data1 >> endpoint) & 1) {
+        bdt[idx].info = BD_OWN_MASK | BD_DTS_MASK;
+    } else {
+        bdt[idx].info = BD_OWN_MASK | BD_DTS_MASK | BD_DATA01_MASK;
+    }
+
+    Data1 ^= (1 << endpoint);
+
+    return EP_PENDING;
+}
+
+EP_STATUS USBHAL::endpointWriteResult(uint8_t endpoint) {
+    if (epComplete & EP(endpoint)) {
+        epComplete &= ~EP(endpoint);
+        return EP_COMPLETED;
+    }
+
+    return EP_PENDING;
+}
+
+void USBHAL::stallEndpoint(uint8_t endpoint) {
+    USB0->ENDPOINT[PHY_TO_LOG(endpoint)].ENDPT |= USB_ENDPT_EPSTALL_MASK;
+}
+
+void USBHAL::unstallEndpoint(uint8_t endpoint) {
+    USB0->ENDPOINT[PHY_TO_LOG(endpoint)].ENDPT &= ~USB_ENDPT_EPSTALL_MASK;
+}
+
+bool USBHAL::getEndpointStallState(uint8_t endpoint) {
+    uint8_t stall = (USB0->ENDPOINT[PHY_TO_LOG(endpoint)].ENDPT & USB_ENDPT_EPSTALL_MASK);
+    return (stall) ? true : false;
+}
+
+void USBHAL::remoteWakeup(void) {
+    // [TODO]
+}
+
+
+void USBHAL::_usbisr(void) {
+    instance->usbisr();
+}
+
+
+void USBHAL::usbisr(void) {
+    uint8_t i;
+    uint8_t istat = USB0->ISTAT;
+
+    // reset interrupt
+    if (istat & USB_ISTAT_USBRST_MASK) {
+        // disable all endpt
+        for(i = 0; i < 16; i++) {
+            USB0->ENDPOINT[i].ENDPT = 0x00;
+        }
+
+        // enable control endpoint
+        realiseEndpoint(EP0OUT, MAX_PACKET_SIZE_EP0, 0);
+        realiseEndpoint(EP0IN, MAX_PACKET_SIZE_EP0, 0);
+
+        Data1 = 0x55555555;
+        USB0->CTL |=  USB_CTL_ODDRST_MASK;
+
+        USB0->ISTAT   =  0xFF;  // clear all interrupt status flags
+        USB0->ERRSTAT =  0xFF;  // clear all error flags
+        USB0->ERREN   =  0xFF;  // enable error interrupt sources
+        USB0->ADDR    =  0x00;  // set default address
+
+        return;
+    }
+
+    // resume interrupt
+    if (istat & USB_ISTAT_RESUME_MASK) {
+        USB0->ISTAT = USB_ISTAT_RESUME_MASK;
+    }
+
+    // SOF interrupt
+    if (istat & USB_ISTAT_SOFTOK_MASK) {
+        USB0->ISTAT = USB_ISTAT_SOFTOK_MASK;
+        // SOF event, read frame number
+        SOF(frameNumber());
+    }
+
+    // stall interrupt
+    if (istat & 1<<7) {
+        if (USB0->ENDPOINT[0].ENDPT & USB_ENDPT_EPSTALL_MASK)
+            USB0->ENDPOINT[0].ENDPT &= ~USB_ENDPT_EPSTALL_MASK;
+        USB0->ISTAT |= USB_ISTAT_STALL_MASK;
+    }
+
+    // token interrupt
+    if (istat & 1<<3) {
+        uint32_t num  = (USB0->STAT >> 4) & 0x0F;
+        uint32_t dir  = (USB0->STAT >> 3) & 0x01;
+        uint32_t ev_odd = (USB0->STAT >> 2) & 0x01;
+
+        // setup packet
+        if ((num == 0) && (TOK_PID((EP_BDT_IDX(num, dir, ev_odd))) == SETUP_TOKEN)) {
+            Data1 &= ~0x02;
+            bdt[EP_BDT_IDX(0, TX, EVEN)].info &= ~BD_OWN_MASK;
+            bdt[EP_BDT_IDX(0, TX, ODD)].info  &= ~BD_OWN_MASK;
+
+            // EP0 SETUP event (SETUP data received)
+            EP0setupCallback();
+
+        } else {
+            // OUT packet
+            if (TOK_PID((EP_BDT_IDX(num, dir, ev_odd))) == OUT_TOKEN) {
+                if (num == 0)
+                    EP0out();
+                else {
+                    epComplete |= (1 << EP(num));
+                    if ((instance->*(epCallback[EP(num) - 2]))()) {
+                        epComplete &= ~(1 << EP(num));
+                    }
+                }
+            }
+
+            // IN packet
+            if (TOK_PID((EP_BDT_IDX(num, dir, ev_odd))) == IN_TOKEN) {
+                if (num == 0) {
+                    EP0in();
+                    if (set_addr == 1) {
+                        USB0->ADDR = addr & 0x7F;
+                        set_addr = 0;
+                    }
+                }
+                else {
+                    epComplete |= (1 << (EP(num) + 1));
+                    if ((instance->*(epCallback[EP(num) + 1 - 2]))()) {
+                        epComplete &= ~(1 << (EP(num) + 1));
+                    }
+                }
+            }
+        }
+
+        USB0->ISTAT = USB_ISTAT_TOKDNE_MASK;
+    }
+
+    // sleep interrupt
+    if (istat & 1<<4) {
+        USB0->ISTAT |= USB_ISTAT_SLEEP_MASK;
+    }
+
+    // error interrupt
+    if (istat & USB_ISTAT_ERROR_MASK) {
+        USB0->ERRSTAT = 0xFF;
+        USB0->ISTAT |= USB_ISTAT_ERROR_MASK;
+    }
+}
+
+
+#endif
diff --git a/keyboard/infinity/tool/README b/keyboard/infinity/tool/README
new file mode 100644
index 0000000000..3dd3316568
--- /dev/null
+++ b/keyboard/infinity/tool/README
@@ -0,0 +1,25 @@
+OpenOCD config files for Kinetis 
+================================
+http://nemuisan.blog.bai.ne.jp/?eid=192848#OPENOCD
+
+These are needed for SWD debug and programing bootloader. To flash keyboard firmware use 'dfu-util'.
+
+Flash security of Freescale kinetis
+-----------------------------------
+If FSEC of flash config is changed accidentally SWD/JTAG you can't get debug access until doing 'mdm mass_erase' with JTAG, CMSIS-DAP or OpenSAD adapter. HLA(high level adapter) like stlink doesn't work for this.
+
+
+Example
+-------
+Debug:
+    $ openocd -s tool -f tool/openocd.cfg
+    $ arm-none-eabi-gdb build/infinity.elf -ex "target remote localhost:3333"
+
+Flash bootloader:
+    $ openocd -s tool -f tool/openocd.cfg -c "mt_flash kiibohd_bootloader.bin"
+
+
+Infinity SWD pinout
+-------------------
+SWD pins are placed next to reset button; SWD_CLK, SWD_DIO, GND, VCC from top.
+Note that RESET is also needed to get full control with OpenOCD.