8 files changed, 1225 insertions, 0 deletions
diff --git a/tmk_core/common/avr/bootloader.c b/tmk_core/common/avr/bootloader.c
new file mode 100644
index 0000000000..7c744e8c79
--- /dev/null
+++ b/tmk_core/common/avr/bootloader.c
@@ -0,0 +1,185 @@
+#include <stdint.h>
+#include <stdbool.h>
+#include <avr/io.h>
+#include <avr/interrupt.h>
+#include <avr/wdt.h>
+#include <util/delay.h>
+#include "bootloader.h"
+
+#ifdef PROTOCOL_LUFA
+#include <LUFA/Drivers/USB/USB.h>
+#endif
+
+
+/* Bootloader Size in *bytes*
+ *
+ * AVR Boot section size are defined by setting BOOTSZ fuse in fact. Consult with your MCU datasheet.
+ * Note that 'Word'(2 bytes) size and address are used in datasheet while TMK uses 'Byte'.
+ *
+ *
+ * Size of Bootloaders in bytes:
+ *   Atmel DFU loader(ATmega32U4)   4096
+ *   Atmel DFU loader(AT90USB128)   8192
+ *   LUFA bootloader(ATmega32U4)    4096
+ *   Arduino Caterina(ATmega32U4)   4096
+ *   USBaspLoader(ATmega***)        2048
+ *   Teensy   halfKay(ATmega32U4)   512
+ *   Teensy++ halfKay(AT90USB128)   1024
+ *
+ *
+ * AVR Boot section is located at the end of Flash memory like the followings.
+ *
+ *
+ * byte     Atmel/LUFA(ATMega32u4)          byte     Atmel(AT90SUB128)
+ * 0x0000   +---------------+               0x00000  +---------------+
+ *          |               |                        |               |
+ *          |               |                        |               |
+ *          |  Application  |                        |  Application  |
+ *          |               |                        |               |
+ *          =               =                        =               =
+ *          |               | 32KB-4KB               |               | 128KB-8KB
+ * 0x6000   +---------------+               0x1FC00  +---------------+
+ *          |  Bootloader   | 4KB                    |  Bootloader   | 8KB
+ * 0x7FFF   +---------------+               0x1FFFF  +---------------+
+ *
+ *
+ * byte     Teensy(ATMega32u4)              byte     Teensy++(AT90SUB128)
+ * 0x0000   +---------------+               0x00000  +---------------+
+ *          |               |                        |               |
+ *          |               |                        |               |
+ *          |  Application  |                        |  Application  |
+ *          |               |                        |               |
+ *          =               =                        =               =
+ *          |               | 32KB-512B              |               | 128KB-1KB
+ * 0x7E00   +---------------+               0x1FC00  +---------------+
+ *          |  Bootloader   | 512B                   |  Bootloader   | 1KB
+ * 0x7FFF   +---------------+               0x1FFFF  +---------------+
+ */
+#ifndef BOOTLOADER_SIZE
+#warning To use bootloader_jump() you need to define BOOTLOADER_SIZE in config.h.
+#define BOOTLOADER_SIZE     4096
+#endif
+
+#define FLASH_SIZE          (FLASHEND + 1L)
+#define BOOTLOADER_START    (FLASH_SIZE - BOOTLOADER_SIZE)
+
+
+/* 
+ * Entering the Bootloader via Software 
+ * http://www.fourwalledcubicle.com/files/LUFA/Doc/120730/html/_page__software_bootloader_start.html
+ */
+#define BOOTLOADER_RESET_KEY 0xB007B007
+uint32_t reset_key  __attribute__ ((section (".noinit")));
+
+/* initialize MCU status by watchdog reset */
+void bootloader_jump(void) {
+#ifdef PROTOCOL_LUFA
+    USB_Disable();
+    cli();
+    _delay_ms(2000);
+#endif
+
+#ifdef PROTOCOL_PJRC
+    cli();
+    UDCON = 1;
+    USBCON = (1<<FRZCLK);
+    UCSR1B = 0;
+    _delay_ms(5);
+#endif
+
+    // watchdog reset
+    reset_key = BOOTLOADER_RESET_KEY;
+    wdt_enable(WDTO_250MS);
+    for (;;);
+}
+
+
+/* this runs before main() */
+void bootloader_jump_after_watchdog_reset(void) __attribute__ ((used, naked, section (".init3")));
+void bootloader_jump_after_watchdog_reset(void)
+{
+    if ((MCUSR & (1<<WDRF)) && reset_key == BOOTLOADER_RESET_KEY) {
+        reset_key = 0;
+
+        // My custom USBasploader requires this to come up.
+        MCUSR = 0;
+
+        // Seems like Teensy halfkay loader requires clearing WDRF and disabling watchdog.
+        MCUSR &= ~(1<<WDRF);
+        wdt_disable();
+
+        // This is compled into 'icall', address should be in word unit, not byte.
+        ((void (*)(void))(BOOTLOADER_START/2))();
+    }
+}
+
+
+#if 0
+/* Jumping To The Bootloader
+ * http://www.pjrc.com/teensy/jump_to_bootloader.html
+ * 
+ * This method doen't work when using LUFA. idk why.
+ * - needs to initialize more regisers or interrupt setting?
+ */
+void bootloader_jump(void) {
+#ifdef PROTOCOL_LUFA
+    USB_Disable();
+    cli();
+    _delay_ms(2000);
+#endif
+
+#ifdef PROTOCOL_PJRC
+    cli();
+    UDCON = 1;
+    USBCON = (1<<FRZCLK);
+    UCSR1B = 0;
+    _delay_ms(5);
+#endif
+
+    /*
+     * Initialize
+     */
+#if defined(__AVR_AT90USB162__)
+    EIMSK = 0; PCICR = 0; SPCR = 0; ACSR = 0; EECR = 0;
+    TIMSK0 = 0; TIMSK1 = 0; UCSR1B = 0;
+    DDRB = 0; DDRC = 0; DDRD = 0;
+    PORTB = 0; PORTC = 0; PORTD = 0;
+#elif defined(__AVR_ATmega32U4__)
+    EIMSK = 0; PCICR = 0; SPCR = 0; ACSR = 0; EECR = 0; ADCSRA = 0;
+    TIMSK0 = 0; TIMSK1 = 0; TIMSK3 = 0; TIMSK4 = 0; UCSR1B = 0; TWCR = 0;
+    DDRB = 0; DDRC = 0; DDRD = 0; DDRE = 0; DDRF = 0; TWCR = 0;
+    PORTB = 0; PORTC = 0; PORTD = 0; PORTE = 0; PORTF = 0;
+#elif defined(__AVR_AT90USB646__)
+    EIMSK = 0; PCICR = 0; SPCR = 0; ACSR = 0; EECR = 0; ADCSRA = 0;
+    TIMSK0 = 0; TIMSK1 = 0; TIMSK2 = 0; TIMSK3 = 0; UCSR1B = 0; TWCR = 0;
+    DDRA = 0; DDRB = 0; DDRC = 0; DDRD = 0; DDRE = 0; DDRF = 0;
+    PORTA = 0; PORTB = 0; PORTC = 0; PORTD = 0; PORTE = 0; PORTF = 0;
+#elif defined(__AVR_AT90USB1286__)
+    EIMSK = 0; PCICR = 0; SPCR = 0; ACSR = 0; EECR = 0; ADCSRA = 0;
+    TIMSK0 = 0; TIMSK1 = 0; TIMSK2 = 0; TIMSK3 = 0; UCSR1B = 0; TWCR = 0;
+    DDRA = 0; DDRB = 0; DDRC = 0; DDRD = 0; DDRE = 0; DDRF = 0;
+    PORTA = 0; PORTB = 0; PORTC = 0; PORTD = 0; PORTE = 0; PORTF = 0;
+#endif
+
+    /*
+     * USBaspLoader
+     */
+#if defined(__AVR_ATmega168__) || defined(__AVR_ATmega168P__) || defined(__AVR_ATmega328P__)
+    // This makes custom USBasploader come up.
+    MCUSR = 0;
+
+    // initialize ports
+    PORTB = 0; PORTC= 0; PORTD = 0;
+    DDRB = 0; DDRC= 0; DDRD = 0;
+
+    // disable interrupts
+    EIMSK = 0; EECR = 0; SPCR = 0;
+    ACSR = 0; SPMCSR = 0; WDTCSR = 0; PCICR = 0;
+    TIMSK0 = 0; TIMSK1 = 0; TIMSK2 = 0;
+    ADCSRA = 0; TWCR = 0; UCSR0B = 0;
+#endif
+
+    // This is compled into 'icall', address should be in word unit, not byte.
+    ((void (*)(void))(BOOTLOADER_START/2))();
+}
+#endif
diff --git a/tmk_core/common/avr/sleep_led.c b/tmk_core/common/avr/sleep_led.c
new file mode 100644
index 0000000000..dab3eb0f3c
--- /dev/null
+++ b/tmk_core/common/avr/sleep_led.c
@@ -0,0 +1,95 @@
+#include <stdint.h>
+#include <avr/io.h>
+#include <avr/interrupt.h>
+#include <avr/pgmspace.h>
+#include "led.h"
+#include "sleep_led.h"
+
+/* Software PWM
+ *  ______           ______           __
+ * |  ON  |___OFF___|  ON  |___OFF___|   ....
+ * |<-------------->|<-------------->|<- ....
+ *     PWM period       PWM period
+ *
+ * 256              interrupts/period[resolution]
+ * 64               periods/second[frequency]
+ * 256*64           interrupts/second
+ * F_CPU/(256*64)   clocks/interrupt
+ */
+#define SLEEP_LED_TIMER_TOP F_CPU/(256*64)
+
+void sleep_led_init(void)
+{
+    /* Timer1 setup */
+    /* CTC mode */
+    TCCR1B |= _BV(WGM12);
+    /* Clock selelct: clk/1 */
+    TCCR1B |= _BV(CS10);
+    /* Set TOP value */
+    uint8_t sreg = SREG;
+    cli();
+    OCR1AH = (SLEEP_LED_TIMER_TOP>>8)&0xff;
+    OCR1AL = SLEEP_LED_TIMER_TOP&0xff;
+    SREG = sreg;
+}
+
+void sleep_led_enable(void)
+{
+    /* Enable Compare Match Interrupt */
+    TIMSK1 |= _BV(OCIE1A);
+}
+
+void sleep_led_disable(void)
+{
+    /* Disable Compare Match Interrupt */
+    TIMSK1 &= ~_BV(OCIE1A);
+}
+
+void sleep_led_toggle(void)
+{
+    /* Disable Compare Match Interrupt */
+    TIMSK1 ^= _BV(OCIE1A);
+}
+
+
+/* Breathing Sleep LED brighness(PWM On period) table
+ * (64[steps] * 4[duration]) / 64[PWM periods/s] = 4 second breath cycle
+ *
+ * http://www.wolframalpha.com/input/?i=%28sin%28+x%2F64*pi%29**8+*+255%2C+x%3D0+to+63
+ * (0..63).each {|x| p ((sin(x/64.0*PI)**8)*255).to_i }
+ */
+static const uint8_t breathing_table[64] PROGMEM = {
+0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 2, 4, 6, 10,
+15, 23, 32, 44, 58, 74, 93, 113, 135, 157, 179, 199, 218, 233, 245, 252,
+255, 252, 245, 233, 218, 199, 179, 157, 135, 113, 93, 74, 58, 44, 32, 23,
+15, 10, 6, 4, 2, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0
+};
+
+ISR(TIMER1_COMPA_vect)
+{
+    /* Software PWM
+     * timer:1111 1111 1111 1111
+     *       \_____/\/ \_______/____  count(0-255)
+     *          \    \______________  duration of step(4)
+     *           \__________________  index of step table(0-63)
+     */
+    static union {
+        uint16_t row;
+        struct {
+            uint8_t count:8;
+            uint8_t duration:2;
+            uint8_t index:6;
+        } pwm;
+    } timer = { .row = 0 };
+
+    timer.row++;
+    
+    // LED on
+    if (timer.pwm.count == 0) {
+        led_set(1<<USB_LED_CAPS_LOCK);
+    }
+    // LED off
+    if (timer.pwm.count == pgm_read_byte(&breathing_table[timer.pwm.index])) {
+        led_set(0);
+    }
+}
diff --git a/tmk_core/common/avr/suspend.c b/tmk_core/common/avr/suspend.c
new file mode 100644
index 0000000000..8a7272bbc5
--- /dev/null
+++ b/tmk_core/common/avr/suspend.c
@@ -0,0 +1,148 @@
+#include <stdbool.h>
+#include <avr/sleep.h>
+#include <avr/wdt.h>
+#include <avr/interrupt.h>
+#include "matrix.h"
+#include "action.h"
+#include "backlight.h"
+#include "suspend_avr.h"
+#include "suspend.h"
+#include "timer.h"
+#include "led.h"
+
+#ifdef PROTOCOL_LUFA
+	#include "lufa.h"
+#endif
+
+#ifdef AUDIO_ENABLE
+    #include "audio.h"
+#endif /* AUDIO_ENABLE */
+
+
+
+#define wdt_intr_enable(value)   \
+__asm__ __volatile__ (  \
+    "in __tmp_reg__,__SREG__" "\n\t"    \
+    "cli" "\n\t"    \
+    "wdr" "\n\t"    \
+    "sts %0,%1" "\n\t"  \
+    "out __SREG__,__tmp_reg__" "\n\t"   \
+    "sts %0,%2" "\n\t" \
+    : /* no outputs */  \
+    : "M" (_SFR_MEM_ADDR(_WD_CONTROL_REG)), \
+    "r" (_BV(_WD_CHANGE_BIT) | _BV(WDE)), \
+    "r" ((uint8_t) ((value & 0x08 ? _WD_PS3_MASK : 0x00) | \
+        _BV(WDIE) | (value & 0x07)) ) \
+    : "r0"  \
+)
+
+
+void suspend_idle(uint8_t time)
+{
+    cli();
+    set_sleep_mode(SLEEP_MODE_IDLE);
+    sleep_enable();
+    sei();
+    sleep_cpu();
+    sleep_disable();
+}
+
+/* Power down MCU with watchdog timer
+ * wdto: watchdog timer timeout defined in <avr/wdt.h>
+ *          WDTO_15MS
+ *          WDTO_30MS
+ *          WDTO_60MS
+ *          WDTO_120MS
+ *          WDTO_250MS
+ *          WDTO_500MS
+ *          WDTO_1S
+ *          WDTO_2S
+ *          WDTO_4S
+ *          WDTO_8S
+ */
+static uint8_t wdt_timeout = 0;
+static void power_down(uint8_t wdto)
+{
+#ifdef PROTOCOL_LUFA
+    if (USB_DeviceState == DEVICE_STATE_Configured) return;
+#endif
+    wdt_timeout = wdto;
+
+    // Watchdog Interrupt Mode
+    wdt_intr_enable(wdto);
+
+#ifdef BACKLIGHT_ENABLE
+	backlight_set(0);
+#endif
+
+	// Turn off LED indicators
+	led_set(0);
+
+	#ifdef AUDIO_ENABLE
+        // This sometimes disables the start-up noise, so it's been disabled
+		// stop_all_notes();
+	#endif /* AUDIO_ENABLE */
+
+    // TODO: more power saving
+    // See PicoPower application note
+    // - I/O port input with pullup
+    // - prescale clock
+    // - BOD disable
+    // - Power Reduction Register PRR
+    set_sleep_mode(SLEEP_MODE_PWR_DOWN);
+    sleep_enable();
+    sei();
+    sleep_cpu();
+    sleep_disable();
+
+    // Disable watchdog after sleep
+    wdt_disable();
+}
+
+void suspend_power_down(void)
+{
+    power_down(WDTO_15MS);
+}
+
+__attribute__ ((weak)) void matrix_power_up(void) {}
+__attribute__ ((weak)) void matrix_power_down(void) {}
+bool suspend_wakeup_condition(void)
+{
+#ifdef BACKLIGHT_ENABLE
+    backlight_set(0);
+#endif
+    matrix_power_up();
+    matrix_scan();
+    matrix_power_down();
+    for (uint8_t r = 0; r < MATRIX_ROWS; r++) {
+        if (matrix_get_row(r)) return true;
+    }
+     return false;
+}
+
+// run immediately after wakeup
+void suspend_wakeup_init(void)
+{
+    // clear keyboard state
+    clear_keyboard();
+#ifdef BACKLIGHT_ENABLE
+    backlight_set(0);
+    backlight_init();
+#endif
+led_set(host_keyboard_leds());
+}
+
+#ifndef NO_SUSPEND_POWER_DOWN
+/* watchdog timeout */
+ISR(WDT_vect)
+{
+    // compensate timer for sleep
+    switch (wdt_timeout) {
+        case WDTO_15MS:
+            timer_count += 15 + 2;  // WDTO_15MS + 2(from observation)
+            break;
+        default:
+            ;
+    }
+}
+#endif
diff --git a/tmk_core/common/avr/suspend_avr.h b/tmk_core/common/avr/suspend_avr.h
new file mode 100644
index 0000000000..357102da44
--- /dev/null
+++ b/tmk_core/common/avr/suspend_avr.h
@@ -0,0 +1,27 @@
+#ifndef SUSPEND_AVR_H
+#define SUSPEND_AVR_H
+
+#include <stdint.h>
+#include <stdbool.h>
+#include <avr/sleep.h>
+#include <avr/wdt.h>
+#include <avr/interrupt.h>
+
+
+#define wdt_intr_enable(value)   \
+__asm__ __volatile__ (  \
+    "in __tmp_reg__,__SREG__" "\n\t"    \
+    "cli" "\n\t"    \
+    "wdr" "\n\t"    \
+    "sts %0,%1" "\n\t"  \
+    "out __SREG__,__tmp_reg__" "\n\t"   \
+    "sts %0,%2" "\n\t" \
+    : /* no outputs */  \
+    : "M" (_SFR_MEM_ADDR(_WD_CONTROL_REG)), \
+    "r" (_BV(_WD_CHANGE_BIT) | _BV(WDE)), \
+    "r" ((uint8_t) ((value & 0x08 ? _WD_PS3_MASK : 0x00) | \
+        _BV(WDIE) | (value & 0x07)) ) \
+    : "r0"  \
+)
+
+#endif
diff --git a/tmk_core/common/avr/timer.c b/tmk_core/common/avr/timer.c
new file mode 100644
index 0000000000..292b41c3a6
--- /dev/null
+++ b/tmk_core/common/avr/timer.c
@@ -0,0 +1,117 @@
+/*
+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 <avr/io.h>
+#include <avr/interrupt.h>
+#include <stdint.h>
+#include "timer_avr.h"
+#include "timer.h"
+
+
+// counter resolution 1ms
+// NOTE: union { uint32_t timer32; struct { uint16_t dummy; uint16_t timer16; }}
+volatile uint32_t timer_count = 0;
+
+void timer_init(void)
+{
+    // Timer0 CTC mode
+    TCCR0A = 0x02;
+
+#if TIMER_PRESCALER == 1
+    TCCR0B = 0x01;
+#elif TIMER_PRESCALER == 8
+    TCCR0B = 0x02;
+#elif TIMER_PRESCALER == 64
+    TCCR0B = 0x03;
+#elif TIMER_PRESCALER == 256
+    TCCR0B = 0x04;
+#elif TIMER_PRESCALER == 1024
+    TCCR0B = 0x05;
+#else
+#   error "Timer prescaler value is NOT vaild."
+#endif
+
+    OCR0A = TIMER_RAW_TOP;
+    TIMSK0 = (1<<OCIE0A);
+}
+
+inline
+void timer_clear(void)
+{
+    uint8_t sreg = SREG;
+    cli();
+    timer_count = 0;
+    SREG = sreg;
+}
+
+inline
+uint16_t timer_read(void)
+{
+    uint32_t t;
+
+    uint8_t sreg = SREG;
+    cli();
+    t = timer_count;
+    SREG = sreg;
+
+    return (t & 0xFFFF);
+}
+
+inline
+uint32_t timer_read32(void)
+{
+    uint32_t t;
+
+    uint8_t sreg = SREG;
+    cli();
+    t = timer_count;
+    SREG = sreg;
+
+    return t;
+}
+
+inline
+uint16_t timer_elapsed(uint16_t last)
+{
+    uint32_t t;
+
+    uint8_t sreg = SREG;
+    cli();
+    t = timer_count;
+    SREG = sreg;
+
+    return TIMER_DIFF_16((t & 0xFFFF), last);
+}
+
+inline
+uint32_t timer_elapsed32(uint32_t last)
+{
+    uint32_t t;
+
+    uint8_t sreg = SREG;
+    cli();
+    t = timer_count;
+    SREG = sreg;
+
+    return TIMER_DIFF_32(t, last);
+}
+
+// excecuted once per 1ms.(excess for just timer count?)
+ISR(TIMER0_COMPA_vect)
+{
+    timer_count++;
+}
diff --git a/tmk_core/common/avr/timer_avr.h b/tmk_core/common/avr/timer_avr.h
new file mode 100644
index 0000000000..0e85eb1017
--- /dev/null
+++ b/tmk_core/common/avr/timer_avr.h
@@ -0,0 +1,42 @@
+/*
+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/>.
+*/
+
+#ifndef TIMER_AVR_H
+#define TIMER_AVR_H 1
+
+#include <stdint.h>
+
+#ifndef TIMER_PRESCALER
+#   if F_CPU > 16000000
+#       define TIMER_PRESCALER      256
+#   elif F_CPU > 2000000
+#       define TIMER_PRESCALER      64
+#   elif F_CPU > 250000
+#       define TIMER_PRESCALER      8
+#   else
+#       define TIMER_PRESCALER      1
+#   endif
+#endif
+#define TIMER_RAW_FREQ      (F_CPU/TIMER_PRESCALER)
+#define TIMER_RAW           TCNT0
+#define TIMER_RAW_TOP       (TIMER_RAW_FREQ/1000)
+
+#if (TIMER_RAW_TOP > 255)
+#   error "Timer0 can't count 1ms at this clock freq. Use larger prescaler."
+#endif
+
+#endif
diff --git a/tmk_core/common/avr/xprintf.S b/tmk_core/common/avr/xprintf.S
new file mode 100644
index 0000000000..0cec70ce22
--- /dev/null
+++ b/tmk_core/common/avr/xprintf.S
@@ -0,0 +1,500 @@
+;---------------------------------------------------------------------------;
+; Extended itoa, puts, printf and atoi                     (C)ChaN, 2011
+;---------------------------------------------------------------------------;
+
+				// Base size is 152 bytes
+#define	CR_CRLF		0	// Convert \n to \r\n (+10 bytes)
+#define USE_XPRINTF	1	// Enable xprintf function (+194 bytes)
+#define USE_XSPRINTF	0	// Add xsprintf function (+78 bytes)
+#define USE_XFPRINTF	0	// Add xfprintf function (+54 bytes)
+#define USE_XATOI	0	// Enable xatoi function (+182 bytes)
+
+
+#if FLASHEND > 0x1FFFF
+#error xitoa module does not support 256K devices
+#endif
+
+.nolist
+#include <avr/io.h>	// Include device specific definitions.
+.list
+
+#ifdef SPM_PAGESIZE	// Recent devices have "lpm Rd,Z+" and "movw".
+.macro	_LPMI	reg
+	lpm	\reg, Z+
+.endm
+.macro	_MOVW	dh,dl, sh,sl
+	movw	\dl, \sl
+.endm
+#else			// Earlier devices do not have "lpm Rd,Z+" nor "movw".
+.macro	_LPMI	reg
+	lpm
+	mov	\reg, r0
+	adiw	ZL, 1
+.endm
+.macro	_MOVW	dh,dl, sh,sl
+	mov	\dl, \sl
+	mov	\dh, \sh
+.endm
+#endif
+
+
+
+;---------------------------------------------------------------------------
+; Stub function to forward to user output function
+;
+;Prototype: void xputc (char chr	// a character to be output
+;			);
+;Size: 12/12 words
+
+.section .bss
+.global xfunc_out	; xfunc_out must be initialized before using this module.
+xfunc_out:	.ds.w	1
+.section .text
+
+
+.func xputc
+.global xputc
+xputc:
+#if CR_CRLF
+	cpi	r24, 10		;LF --> CRLF
+	brne	1f		;
+	ldi	r24, 13		;
+	rcall	1f		;
+	ldi	r24, 10		;/
+1:
+#endif
+	push	ZH
+	push	ZL
+	lds	ZL, xfunc_out+0	;Pointer to the registered output function.
+	lds	ZH, xfunc_out+1	;/
+	sbiw	ZL, 0		;Skip if null
+	breq	2f		;/
+	icall
+2:	pop	ZL
+	pop	ZH
+	ret
+.endfunc
+
+
+
+;---------------------------------------------------------------------------
+; Direct ROM string output
+;
+;Prototype: void xputs (const char *str_p // rom string to be output
+;			);
+
+.func xputs
+.global xputs
+xputs:
+	_MOVW	ZH,ZL, r25,r24	; Z = pointer to rom string
+1:	_LPMI	r24
+	cpi	r24, 0
+	breq	2f
+	rcall	xputc
+	rjmp	1b
+2:	ret
+.endfunc
+
+
+;---------------------------------------------------------------------------
+; Extended direct numeral string output (32bit version)
+;
+;Prototype: void xitoa (long value,	// value to be output
+;                       char radix,	// radix
+;                       char width);	// minimum width
+;
+
+.func xitoa
+.global xitoa
+xitoa:
+				;r25:r22 = value, r20 = base, r18 = digits
+	clr	r31		;r31 = stack level
+	ldi	r30, ' '	;r30 = sign
+	ldi	r19, ' '	;r19 = filler
+	sbrs	r20, 7		;When base indicates signd format and the value
+	rjmp	0f		;is minus, add a '-'.
+	neg	r20		;
+	sbrs	r25, 7		;
+	rjmp	0f		;
+	ldi	r30, '-'	;
+	com	r22		;
+	com	r23		;
+	com	r24		;
+	com	r25		;
+	adc	r22, r1		;
+	adc	r23, r1		;
+	adc	r24, r1		;
+	adc	r25, r1		;/
+0:	sbrs	r18, 7		;When digits indicates zero filled,
+	rjmp	1f		;filler is '0'.
+	neg	r18		;
+	ldi	r19, '0'	;/
+				;----- string conversion loop
+1:	ldi	r21, 32		;r26 = r25:r22 % r20
+	clr	r26		;r25:r22 /= r20
+2:	lsl	r22		;
+	rol	r23		;
+	rol	r24		;
+	rol	r25		;
+	rol	r26		;
+	cp	r26, r20	;
+	brcs	3f		;
+	sub	r26, r20	;
+	inc	r22		;
+3:	dec	r21		;
+	brne	2b		;/
+	cpi	r26, 10		;r26 is a numeral digit '0'-'F'
+	brcs	4f		;
+	subi	r26, -7		;
+4:	subi	r26, -'0'	;/
+	push	r26		;Stack it
+	inc	r31		;/
+	cp	r22, r1		;Repeat until r25:r22 gets zero
+	cpc	r23, r1		;
+	cpc	r24, r1		;
+	cpc	r25, r1		;
+	brne	1b		;/
+
+	cpi	r30, '-'	;Minus sign if needed
+	brne	5f		;
+	push	r30		;
+	inc	r31		;/
+5:	cp	r31, r18	;Filler
+	brcc	6f		;
+	push	r19		;
+	inc	r31		;
+	rjmp	5b		;/
+
+6:	pop	r24		;Flush stacked digits and exit
+	rcall	xputc		;
+	dec	r31		;
+	brne	6b		;/
+
+	ret
+.endfunc
+
+
+
+;---------------------------------------------------------------------------;
+; Formatted string output (16/32bit version)
+;
+;Prototype:
+; void __xprintf (const char *format_p, ...);
+; void __xsprintf(char*, const char *format_p, ...);
+; void __xfprintf(void(*func)(char), const char *format_p, ...);
+;
+
+#if USE_XPRINTF
+
+.func xvprintf
+xvprintf:
+	ld	ZL, Y+		;Z = pointer to format string
+	ld	ZH, Y+		;/
+
+0:	_LPMI	r24		;Get a format char
+	cpi	r24, 0		;End of format string?
+	breq	90f		;/
+	cpi	r24, '%'	;Is format?
+	breq	20f		;/
+1:	rcall	xputc		;Put a normal character
+	rjmp	0b		;/
+90:	ret
+
+20:	ldi	r18, 0		;r18: digits
+	clt			;T: filler
+	_LPMI	r21		;Get flags
+	cpi	r21, '%'	;Is a %?
+	breq	1b		;/
+	cpi	r21, '0'	;Zero filled?
+	brne	23f		;
+	set			;/
+22:	_LPMI	r21		;Get width
+23:	cpi	r21, '9'+1	;
+	brcc	24f		;
+	subi	r21, '0'	;
+	brcs	90b		;
+	lsl	r18		;
+	mov	r0, r18		;
+	lsl	r18		;
+	lsl	r18		;
+	add	r18, r0		;
+	add	r18, r21	;
+	rjmp	22b		;/
+
+24:	brtc	25f		;get value (low word)
+	neg	r18		;
+25:	ld	r24, Y+		;
+	ld	r25, Y+		;/
+	cpi	r21, 'c'	;Is type character?
+	breq	1b		;/
+	cpi	r21, 's'	;Is type RAM string?
+	breq	50f		;/
+	cpi	r21, 'S'	;Is type ROM string?
+	breq	60f		;/
+	_MOVW	r23,r22,r25,r24	;r25:r22 = value
+	clr	r24		;
+	clr	r25		;
+	clt			;/
+	cpi	r21, 'l'	;Is long int?
+	brne	26f		;
+	ld	r24, Y+		;get value (high word)
+	ld	r25, Y+		;
+	set			;
+	_LPMI	r21		;/
+26:	cpi	r21, 'd'	;Is type signed decimal?
+	brne	27f		;/
+	ldi	r20, -10	;
+	brts	40f		;
+	sbrs	r23, 7		;
+	rjmp	40f		;
+	ldi	r24, -1		;
+	ldi	r25, -1		;
+	rjmp	40f		;/
+27:	cpi	r21, 'u'	;Is type unsigned decimal?
+	ldi	r20, 10		;
+	breq	40f		;/
+	cpi	r21, 'X'	;Is type hexdecimal?
+	ldi	r20, 16		;
+	breq	40f		;/
+	cpi	r21, 'b'	;Is type binary?
+	ldi	r20, 2		;
+	breq	40f		;/
+	ret			;abort
+40:	push	ZH		;Output the value
+	push	ZL		;
+	rcall	xitoa		;
+42:	pop	ZL		;
+	pop	ZH		;
+	rjmp	0b		;/
+
+50:	push	ZH		;Put a string on the RAM
+	push	ZL
+	_MOVW	ZH,ZL, r25,r24
+51:	ld	r24, Z+
+	cpi	r24, 0
+	breq	42b
+	rcall	xputc
+	rjmp	51b
+
+60:	push	ZH		;Put a string on the ROM
+	push	ZL
+	rcall	xputs
+	rjmp	42b
+.endfunc
+
+
+.func __xprintf
+.global __xprintf
+__xprintf:
+	push	YH
+	push	YL
+	in	YL, _SFR_IO_ADDR(SPL)
+#ifdef SPH
+	in	YH, _SFR_IO_ADDR(SPH)
+#else
+	clr	YH
+#endif
+	adiw	YL, 5		;Y = pointer to arguments
+	rcall	xvprintf
+	pop	YL
+	pop	YH
+	ret
+.endfunc
+
+
+#if USE_XSPRINTF
+
+.func __xsprintf
+putram:
+	_MOVW	ZH,ZL, r15,r14
+	st	Z+, r24
+	_MOVW	r15,r14, ZH,ZL
+	ret
+.global __xsprintf
+__xsprintf:
+	push	YH
+	push	YL
+	in	YL, _SFR_IO_ADDR(SPL)
+#ifdef SPH
+	in	YH, _SFR_IO_ADDR(SPH)
+#else
+	clr	YH
+#endif
+	adiw	YL, 5		;Y = pointer to arguments
+	lds	ZL, xfunc_out+0	;Save registered output function
+	lds	ZH, xfunc_out+1	;
+	push	ZL		;
+	push	ZH		;/
+	ldi	ZL, lo8(pm(putram));Set local output function
+	ldi	ZH, hi8(pm(putram));
+	sts	xfunc_out+0, ZL	;
+	sts	xfunc_out+1, ZH	;/
+	push	r15		;Initialize pointer to string buffer
+	push	r14		;
+	ld	r14, Y+		;
+	ld	r15, Y+		;/
+	rcall	xvprintf
+	_MOVW	ZH,ZL, r15,r14	;Terminate string
+	st	Z, r1		;
+	pop	r14		;
+	pop	r15		;/
+	pop	ZH		;Restore registered output function
+	pop	ZL		;
+	sts	xfunc_out+0, ZL	;
+	sts	xfunc_out+1, ZH	;/
+	pop	YL
+	pop	YH
+	ret
+.endfunc
+#endif
+
+
+#if USE_XFPRINTF
+.func __xfprintf
+.global __xfprintf
+__xfprintf:
+	push	YH
+	push	YL
+	in	YL, _SFR_IO_ADDR(SPL)
+#ifdef SPH
+	in	YH, _SFR_IO_ADDR(SPH)
+#else
+	clr	YH
+#endif
+	adiw	YL, 5		;Y = pointer to arguments
+	lds	ZL, xfunc_out+0	;Save registered output function
+	lds	ZH, xfunc_out+1	;
+	push	ZL		;
+	push	ZH		;/
+	ld	ZL, Y+		;Set output function
+	ld	ZH, Y+		;
+	sts	xfunc_out+0, ZL	;
+	sts	xfunc_out+1, ZH	;/
+	rcall	xvprintf
+	pop	ZH		;Restore registered output function
+	pop	ZL		;
+	sts	xfunc_out+0, ZL	;
+	sts	xfunc_out+1, ZH	;/
+	pop	YL
+	pop	YH
+	ret
+.endfunc
+#endif
+
+#endif
+
+
+
+;---------------------------------------------------------------------------
+; Extended numeral string input
+;
+;Prototype:
+; char xatoi (           /* 1: Successful, 0: Failed */
+;      const char **str, /* pointer to pointer to source string */
+;      long *res         /* result */
+; );
+;
+
+
+#if USE_XATOI
+.func xatoi
+.global xatoi
+xatoi:
+	_MOVW	r1, r0, r23, r22
+	_MOVW	XH, XL, r25, r24
+	ld	ZL, X+
+	ld	ZH, X+
+	clr	r18		;r21:r18 = 0;
+	clr	r19		;
+	clr	r20		;
+	clr	r21		;/
+	clt			;T = 0;
+
+	ldi	r25, 10		;r25 = 10;
+	rjmp	41f		;/
+40:	adiw	ZL, 1		;Z++;
+41:	ld	r22, Z		;r22 = *Z;
+	cpi	r22, ' '	;if(r22 == ' ') continue
+	breq	40b		;/
+	brcs	70f		;if(r22 < ' ') error;
+	cpi	r22, '-'	;if(r22 == '-') {
+	brne	42f		; T = 1;
+	set			; continue;
+	rjmp	40b		;}
+42:	cpi	r22, '9'+1	;if(r22 > '9') error;
+	brcc	70f		;/
+	cpi	r22, '0'	;if(r22 < '0') error;
+	brcs	70f		;/
+	brne	51f		;if(r22 > '0') cv_start;
+	ldi	r25, 8		;r25 = 8;
+	adiw	ZL, 1		;r22 = *(++Z);
+	ld	r22, Z		;/
+	cpi	r22, ' '+1	;if(r22 <= ' ') exit;
+	brcs	80f		;/
+	cpi	r22, 'b'	;if(r22 == 'b') {
+	brne	43f		; r25 = 2;
+	ldi	r25, 2		; cv_start;
+	rjmp	50f		;}
+43:	cpi	r22, 'x'	;if(r22 != 'x') error;
+	brne	51f		;/
+	ldi	r25, 16		;r25 = 16;
+
+50:	adiw	ZL, 1		;Z++;
+	ld	r22, Z		;r22 = *Z;
+51:	cpi	r22, ' '+1	;if(r22 <= ' ') break;
+	brcs	80f		;/
+	cpi	r22, 'a'	;if(r22 >= 'a') r22 =- 0x20;
+	brcs	52f		;
+	subi	r22, 0x20	;/
+52:	subi	r22, '0'	;if((r22 -= '0') < 0) error;
+	brcs	70f		;/
+	cpi	r22, 10		;if(r22 >= 10) {
+	brcs	53f		; r22 -= 7;
+	subi	r22, 7		; if(r22 < 10) 
+	cpi	r22, 10		;
+	brcs	70f		;}
+53:	cp	r22, r25	;if(r22 >= r25) error;
+	brcc	70f		;/
+60:	ldi	r24, 33		;r21:r18 *= r25;
+	sub	r23, r23	;
+61:	brcc	62f		;
+	add	r23, r25	;
+62:	lsr	r23		;
+	ror	r21		;
+	ror	r20		;
+	ror	r19		;
+	ror	r18		;
+	dec	r24		;
+	brne	61b		;/
+	add	r18, r22	;r21:r18 += r22;
+	adc	r19, r24	;
+	adc	r20, r24	;
+	adc	r21, r24	;/
+	rjmp	50b		;repeat
+
+70:	ldi	r24, 0
+	rjmp	81f
+80:	ldi	r24, 1
+81:	brtc	82f
+	clr	r22
+	com	r18
+	com	r19
+	com	r20
+	com	r21
+	adc	r18, r22
+	adc	r19, r22
+	adc	r20, r22
+	adc	r21, r22
+82:	st	-X, ZH
+	st	-X, ZL
+	_MOVW	XH, XL, r1, r0
+	st	X+, r18
+	st	X+, r19
+	st	X+, r20
+	st	X+, r21
+	clr	r1
+	ret
+.endfunc
+#endif
+
+
diff --git a/tmk_core/common/avr/xprintf.h b/tmk_core/common/avr/xprintf.h
new file mode 100644
index 0000000000..59c6f25312
--- /dev/null
+++ b/tmk_core/common/avr/xprintf.h
@@ -0,0 +1,111 @@
+/*---------------------------------------------------------------------------
+   Extended itoa, puts and printf                    (C)ChaN, 2011
+-----------------------------------------------------------------------------*/
+
+#ifndef XPRINTF_H
+#define XPRINTF_H
+
+#include <inttypes.h>
+#include <avr/pgmspace.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+extern void (*xfunc_out)(uint8_t);
+#define xdev_out(func) xfunc_out = (void(*)(uint8_t))(func)
+
+/* This is a pointer to user defined output function. It must be initialized
+   before using this modle.
+*/
+
+void xputc(char chr);
+
+/* This is a stub function to forward outputs to user defined output function.
+   All outputs from this module are output via this function.
+*/
+
+
+/*-----------------------------------------------------------------------------*/
+void xputs(const char *string_p);
+
+/*  The string placed in the ROM is forwarded to xputc() directly.
+*/
+
+
+/*-----------------------------------------------------------------------------*/
+void xitoa(long value, char radix, char width);
+