summaryrefslogtreecommitdiff
path: root/common/avr
diff options
context:
space:
mode:
Diffstat (limited to 'common/avr')
-rw-r--r--common/avr/bootloader.c148
-rw-r--r--common/avr/eeconfig.c45
-rw-r--r--common/avr/suspend.c122
-rw-r--r--common/avr/suspend_avr.h27
-rw-r--r--common/avr/timer.c117
-rw-r--r--common/avr/timer_avr.h42
-rw-r--r--common/avr/xprintf.S500
-rw-r--r--common/avr/xprintf.h111
8 files changed, 1112 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..80243f02bc
--- /dev/null
+++ b/common/avr/suspend.c
@@ -0,0 +1,122 @@
+#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"
+#ifdef PROTOCOL_LUFA
+#include "lufa.h"
+#endif
+
+
+#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);
+
+ // 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);
+}
+
+bool suspend_wakeup_condition(void)
+{
+ 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_init();
+#endif
+}
+
+#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/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..59c6f25312
--- /dev/null
+++ b/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);
+
+/* 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
+*/
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
+