Merge branch 'mbed' into dev

8 files changed, 1080 insertions, 0 deletions

diff --git a/common/avr/bootloader.c b/common/avr/bootloader.c
new file mode 100644
index 0000000000..cda295b181
--- /dev/null
+++ b/common/avr/bootloader.c
@@ -0,0 +1,148 @@
+#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
+
+
+/* Boot Section Size in *BYTEs*
+ *   Teensy   halfKay    512
+ *   Teensy++ halfKay    1024
+ *   Atmel DFU loader    4096
+ *   LUFA bootloader     4096
+ *   USBaspLoader        2048
+ */
+#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/common/avr/eeconfig.c b/common/avr/eeconfig.c
new file mode 100644
index 0000000000..5bd47dc6ad
--- /dev/null
+++ b/common/avr/eeconfig.c
@@ -0,0 +1,45 @@
+#include <stdint.h>
+#include <stdbool.h>
+#include <avr/eeprom.h>
+#include "eeconfig.h"
+
+void eeconfig_init(void)
+{
+    eeprom_write_word(EECONFIG_MAGIC,          EECONFIG_MAGIC_NUMBER);
+    eeprom_write_byte(EECONFIG_DEBUG,          0);
+    eeprom_write_byte(EECONFIG_DEFAULT_LAYER,  0);
+    eeprom_write_byte(EECONFIG_KEYMAP,         0);
+    eeprom_write_byte(EECONFIG_MOUSEKEY_ACCEL, 0);
+#ifdef BACKLIGHT_ENABLE
+    eeprom_write_byte(EECONFIG_BACKLIGHT,      0);
+#endif
+}
+
+void eeconfig_enable(void)
+{
+    eeprom_write_word(EECONFIG_MAGIC, EECONFIG_MAGIC_NUMBER);
+}
+
+void eeconfig_disable(void)
+{
+    eeprom_write_word(EECONFIG_MAGIC, 0xFFFF);
+}
+
+bool eeconfig_is_enabled(void)
+{
+    return (eeprom_read_word(EECONFIG_MAGIC) == EECONFIG_MAGIC_NUMBER);
+}
+
+uint8_t eeconfig_read_debug(void)      { return eeprom_read_byte(EECONFIG_DEBUG); }
+void eeconfig_write_debug(uint8_t val) { eeprom_write_byte(EECONFIG_DEBUG, val); }
+
+uint8_t eeconfig_read_default_layer(void)      { return eeprom_read_byte(EECONFIG_DEFAULT_LAYER); }
+void eeconfig_write_default_layer(uint8_t val) { eeprom_write_byte(EECONFIG_DEFAULT_LAYER, val); }
+
+uint8_t eeconfig_read_keymap(void)      { return eeprom_read_byte(EECONFIG_KEYMAP); }
+void eeconfig_write_keymap(uint8_t val) { eeprom_write_byte(EECONFIG_KEYMAP, val); }
+
+#ifdef BACKLIGHT_ENABLE
+uint8_t eeconfig_read_backlight(void)      { return eeprom_read_byte(EECONFIG_BACKLIGHT); }
+void eeconfig_write_backlight(uint8_t val) { eeprom_write_byte(EECONFIG_BACKLIGHT, val); }
+#endif
diff --git a/common/avr/suspend.c b/common/avr/suspend.c
new file mode 100644
index 0000000000..f44a036beb
--- /dev/null
+++ b/common/avr/suspend.c
@@ -0,0 +1,98 @@
+#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"
+
+
+#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_power_down(void)
+{
+#ifdef BACKLIGHT_ENABLE
+    backlight_set(0);
+#endif
+#ifndef NO_SUSPEND_POWER_DOWN
+    // Enable watchdog to wake from MCU sleep
+    cli();
+    wdt_reset();
+
+    // Watchdog Interrupt and System Reset Mode
+    //wdt_enable(WDTO_1S);
+    //WDTCSR |= _BV(WDIE);
+    
+    // Watchdog Interrupt Mode
+    wdt_intr_enable(WDTO_120MS);
+    
+    // TODO: more power saving
+    // See PicoPower application note
+    // - I/O port input with pullup
+    // - prescale clock
+    // - BOD disable
+    // - Power Reduction Register PRR
+    // sleep in power down mode
+    set_sleep_mode(SLEEP_MODE_PWR_DOWN);
+    sleep_enable();
+    sei();
+    sleep_cpu();
+    sleep_disable();
+
+    // Disable watchdog after sleep
+    wdt_disable();
+#endif
+}
+
+bool suspend_wakeup_condition(void)
+{
+    matrix_scan();
+    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_init();
+#endif
+}
+
+#ifndef NO_SUSPEND_POWER_DOWN
+/* watchdog timeout */
+ISR(WDT_vect)
+{
+    /* wakeup from MCU sleep mode */
+/*
+    // blink LED
+    static uint8_t led_state = 0;
+    static uint8_t led_count = 0;
+    led_count++;
+    if ((led_count & 0x07) == 0) {
+        led_set((led_state ^= (1<<USB_LED_CAPS_LOCK)));
+    }
+*/
+}
+#endif
diff --git a/common/avr/suspend_avr.h b/common/avr/suspend_avr.h
new file mode 100644
index 0000000000..357102da44
--- /dev/null
+++ b/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/common/avr/timer.c b/common/avr/timer.c
new file mode 100644
index 0000000000..292b41c3a6
--- /dev/null
+++ b/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/common/avr/timer_avr.h b/common/avr/timer_avr.h
new file mode 100644
index 0000000000..0e85eb1017
--- /dev/null
+++ b/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/common/avr/xprintf.S b/common/avr/xprintf.S
new file mode 100644
index 0000000000..0cec70ce22
--- /dev/null
+++ b/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/common/avr/xprintf.h b/common/avr/xprintf.h
new file mode 100644
index 0000000000..f58bca817b
--- /dev/null
+++ b/common/avr/xprintf.h
@@ -0,0 +1,103 @@
+/*---------------------------------------------------------------------------

+   Extended itoa, puts and printf                    (C)ChaN, 2011

+-----------------------------------------------------------------------------*/

+

+#ifndef XPRINTF_H

+#define XPRINTF_H

+

+#include <inttypes.h>

+#include <avr/pgmspace.h>

+

+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);

+

+/* Extended itoa().

+

+      value  radix  width   output

+        100     10      6   "   100"

+        100     10     -6   "000100"

+        100     10      0   "100"

+ 4294967295     10      0   "4294967295"

+ 4294967295    -10      0   "-1"

+     655360     16     -8   "000A0000"

+       1024     16      0   "400"

+       0x55      2     -8   "01010101"

+*/

+

+

+/*-----------------------------------------------------------------------------*/

+#define xprintf(format, ...)            __xprintf(PSTR(format), ##__VA_ARGS__)

+#define xsprintf(str, format, ...)      __xsprintf(str, PSTR(format), ##__VA_ARGS__)

+#define xfprintf(func, format, ...)     __xfprintf(func, PSTR(format), ##__VA_ARGS__)

+

+void __xprintf(const char *format_p, ...);	/* Send formatted string to the registered device */

+void __xsprintf(char*, const char *format_p, ...);	/* Put formatted string to the memory */

+void __xfprintf(void(*func)(uint8_t), const char *format_p, ...); /* Send formatted string to the specified device */

+

+/* Format string is placed in the ROM. The format flags is similar to printf().

+

+   %[flag][width][size]type

+

+   flag

+     A '0' means filled with '0' when output is shorter than width.

+     ' ' is used in default. This is effective only numeral type.

+   width

+     Minimum width in decimal number. This is effective only numeral type.

+     Default width is zero.

+   size

+     A 'l' means the argument is long(32bit). Default is short(16bit).

+     This is effective only numeral type.

+   type

+     'c' : Character, argument is the value

+     's' : String placed on the RAM, argument is the pointer

+     'S' : String placed on the ROM, argument is the pointer

+     'd' : Signed decimal, argument is the value

+     'u' : Unsigned decimal, argument is the value

+     'X' : Hexdecimal, argument is the value

+     'b' : Binary, argument is the value

+     '%' : '%'

+

+*/

+

+

+/*-----------------------------------------------------------------------------*/

+char xatoi(char **str, long *ret);

+

+/* Get value of the numeral string. 

+

+  str

+    Pointer to pointer to source string

+

+    "0b11001010" binary

+    "0377" octal

+    "0xff800" hexdecimal

+    "1250000" decimal

+    "-25000" decimal

+

+  ret

+    Pointer to return value

+*/

+

+#endif

+