Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Bring in floating point support for BetterStream::printf, courtesy of the avr-libc implementation. 

Strip the stdio integration from FastSerial as we aren't using it and it just wastes space.

Note that this does not attempt to fix the bogus floating point handling in ::print(ln).  That's an issue for another day.

BetterStream::printf(_P) aka FastSerial::printf(_P) support is now as documented for avr-libc printf with floating point support enabled.


git-svn-id: https://arducopter.googlecode.com/svn/trunk@895 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
DrZiplok@gmail.com 2010-11-23 00:38:16 +00:00
parent 04364e90c0
commit 43262a573f
12 changed files with 1791 additions and 62 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

40
libraries/FastSerial/BetterStream.cpp
View File

@ -32,55 +32,23 @@ BetterStream::println_P(const char *s)
println(); println();
} }
// STDIO emulation ///////////////////////////////////////////////////////////// void
int
BetterStream::_putchar(char c, FILE *stream)
{
BetterStream *bs;
bs = (BetterStream *)fdev_get_udata(stream);
if ('\n' == c)
bs->write('\r'); // ASCII translation on the cheap
bs->write(c);
return(0);
}
int
BetterStream::_getchar(FILE *stream)
{
BetterStream *bs;
bs = (BetterStream *)fdev_get_udata(stream);
// We return -1 if there is nothing to read, which the library interprets
// as an error, which our clients will need to deal with.
return(bs->read());
}
int
BetterStream::printf(const char *fmt, ...) BetterStream::printf(const char *fmt, ...)
{ {
va_list ap; va_list ap;
int i;
va_start(ap, fmt); va_start(ap, fmt);
i = vfprintf(&fd, fmt, ap); _vprintf(0, fmt, ap);
va_end(ap); va_end(ap);
return(i);
} }
int void
BetterStream::printf_P(const char *fmt, ...) BetterStream::printf_P(const char *fmt, ...)
{ {
va_list ap; va_list ap;
int i;
va_start(ap, fmt); va_start(ap, fmt);
i = vfprintf_P(&fd, fmt, ap); _vprintf(1, fmt, ap);
va_end(ap); va_end(ap);
return(i);
} }

21
libraries/FastSerial/BetterStream.h
View File

@ -17,27 +17,16 @@
class BetterStream : public Stream { class BetterStream : public Stream {
public: public:
BetterStream(void) { BetterStream(void) {
// init stdio
fdev_setup_stream(&fd, &BetterStream::_putchar, &BetterStream::_getchar, _FDEV_SETUP_RW);
fdev_set_udata(&fd, this);
} }
// Stream extensions // Stream extensions
void print_P(const char *s); void print_P(const char *);
void println_P(const char *s); void println_P(const char *);
void printf(const char *, ...);
// stdio extensions void printf_P(const char *, ...);
int printf(const char *fmt, ...);
int printf_P(const char *fmt, ...);
protected:
// subclasses can use this to e.g. set up stdin/stdout/stderr.
FILE fd;
private: private:
// stdio emulation void _vprintf(unsigned char, const char *, va_list);
static int _putchar(char c, FILE *stream);
static int _getchar(FILE *stream);
}; };
#endif // __BETTERSTREAM_H #endif // __BETTERSTREAM_H

6
libraries/FastSerial/FastSerial.cpp
View File

@ -101,12 +101,6 @@ FastSerial::FastSerial(const uint8_t portNumber,
_txBuffer->head = _txBuffer->tail = 0; _txBuffer->head = _txBuffer->tail = 0;
_txBuffer = &__FastSerial__txBuffer[portNumber]; _txBuffer = &__FastSerial__txBuffer[portNumber];
_rxBuffer->head = _rxBuffer->tail = 0; _rxBuffer->head = _rxBuffer->tail = 0;
if (0 == portNumber) {
stdout = &fd; // serial port 0 is always the default console
stdin = &fd;
stderr = &fd;
}
} }
// Public Methods ////////////////////////////////////////////////////////////// // Public Methods //////////////////////////////////////////////////////////////

1
libraries/FastSerial/FastSerial.h
View File

@ -48,7 +48,6 @@
#define HardwareSerial_h #define HardwareSerial_h
#include <inttypes.h> #include <inttypes.h>
#include <stdio.h>
#include <stdlib.h> #include <stdlib.h>
#include <avr/interrupt.h> #include <avr/interrupt.h>

11
libraries/FastSerial/examples/FastSerial/FastSerial.pde
View File

@ -35,11 +35,16 @@ void setup(void)
Serial.begin(38400); Serial.begin(38400);
// //
// And send a message. // Test printing things
// //
Serial.println("begin"); Serial.print("test");
Serial.printf("printf\n"); Serial.println(" begin");
Serial.println(1000);
Serial.println(1000, 8);
Serial.println(1000, 10);
Serial.println(1000, 16);
Serial.println_P(PSTR("progmem")); Serial.println_P(PSTR("progmem"));
Serial.printf("printf %d %u %#x %p %f %S\n", -1000, 1000, 1000, 1000, 1.2345, PSTR("progmem"));
} }
void void

532
libraries/FastSerial/ftoa_engine.S Normal file
View File

@ -0,0 +1,532 @@
/* Copyright (c) 2005, Dmitry Xmelkov
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in
the documentation and/or other materials provided with the
distribution.
* Neither the name of the copyright holders nor the names of
contributors may be used to endorse or promote products derived
from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
POSSIBILITY OF SUCH DAMAGE. */
/* $Id: ftoa_engine.S,v 1.3 2009/04/01 23:11:00 arcanum Exp $ */
#ifndef __DOXYGEN__
#include "macros.inc"
#include "ftoa_engine.h"
#if defined(__AVR_HAVE_LPMX__) && __AVR_HAVE_LPMX__
# define AVR_ENH_LPM 1
#else
# define AVR_ENH_LPM 0
#endif
/*
int __ftoa_engine (double val, char *buf,
unsigned char prec, unsigned char maxdgs)
Input:
val - value to convert
buf - output buffer address
prec - precision: number of decimal digits is 'prec + 1'
maxdgs - (0 if unused) precision restriction for "%f" specification
Output:
return - decimal exponent of first digit
buf[0] - flags (FTOA_***)
buf[1],... - decimal digits
Number of digits:
maxdgs == 0 ? prec+1 :
(buf[0] & FTOA_CARRY) == 0 || buf[1] != '1' ?
aver(1, maxdgs+exp, prec+1) :
aver(1, masdgs+exp-1, prec+1)
Notes:
* Output string is not 0-terminated. For possibility of user's buffer
usage in any case.
* If used, 'maxdgs' is a number of digits for value with zero exponent.
*/
/* Input */
#define maxdgs r16
#define prec r18
#define buf_lo r20
#define buf_hi r21
#define val_lo r22
#define val_hi r23
#define val_hlo r24
#define val_hhi r25
/* Float value parse */
#define flag r19
/* Multiplication of mantisses */
#define exp_sv r17
#define mlt_1 r19 /* lowest result byte */
#define mlt_2 r14
#define mlt_3 r15
#define mlt_4 r20
#define mlt_5 r21
#define mlt_6 r28
#define mlt_7 r29
/* Conversion to string */
#define pwr_2 r1 /* lowest byte of 'powr10' element */
#define pwr_3 r17
#define pwr_4 r19
#define pwr_5 r22
#define pwr_6 r25
#define pwr_7 r0
#define digit r23
#define exp10 r24
/* Fixed */
#define zero r1
/* ASSEMBLY_CLIB_SECTION */
.global __ftoa_engine
.type __ftoa_engine, "function"
__ftoa_engine:
/* --------------------------------------------------------------------
Float value parse.
*/
; limit 'prec'
cpi prec, 8
brlo 1f
ldi prec, 7
1:
; init.
clr flag
X_movw XL, buf_lo
; val_hhi := exponent, sign test and remove
#if FTOA_MINUS != 1
# error FTOA_MINUS must be 1: add with carry used
#endif
lsl val_hhi
adc flag, zero ; FTOA_MINUS
sbrc val_hlo, 7
ori val_hhi, 1
; zero test
adiw val_hlo, 0
cpc val_lo, zero
cpc val_hi, zero
brne 3f
; return 0
ori flag, FTOA_ZERO
subi prec, -2
2: st X+, flag
ldi flag, '0'
dec prec
brne 2b
ret ; r24,r25 == 0
3:
; infinity, NaN ?
#if FTOA_NAN != 2 * FTOA_INF
# error Must: FTOA_NAN == 2*FTOA_INF: 'rjmp' is absent
#endif
cpi val_hhi, 0xff
brlo 6f
cpi val_hlo, 0x80
cpc val_hi, zero
cpc val_lo, zero
breq 5f
subi flag, -FTOA_INF ; FTOA_NAN
5: subi flag, -FTOA_INF
6:
; write flags byte
st X+, flag
; hidden bit
cpi val_hhi, 1
brlo 7f ; if subnormal value
ori val_hlo, 0x80
7: adc val_hhi, zero
; pushes
push r29
push r28
push r17
push r16
push r15
push r14
/* --------------------------------------------------------------------
Multiplication of mantisses (val and table).
At the begin:
val_hlo .. val_lo - input value mantisse
val_hhi - input value exponent
X - second byte address (string begin)
At the end:
mlt_7 .. mlt_2 - multiplication result
exp10 - decimal exponent
*/
; save
mov exp_sv, val_hhi
; Z := & base10[exp / 8] (sizeof(base10[0]) == 5)
andi val_hhi, ~7
lsr val_hhi ; (exp/8) * 4
mov ZL, val_hhi
lsr val_hhi
lsr val_hhi ; exp/8
add ZL, val_hhi ; (exp/8) * 5
clr ZH
subi ZL, lo8(-(.L_base10))
sbci ZH, hi8(-(.L_base10))
; highest mantissa byte (mult. shifting prepare)
clr val_hhi
; result initializ.
clr mlt_1
clr mlt_2
clr mlt_3
X_movw mlt_4, mlt_2
X_movw mlt_6, mlt_2
; multiply to 1-st table byte
#if AVR_ENH_LPM
lpm r0, Z+
#else
lpm
adiw ZL, 1
#endif
sec ; for loop end control
ror r0
; addition
10: brcc 11f
add mlt_1, val_lo
adc mlt_2, val_hi
adc mlt_3, val_hlo
adc mlt_4, val_hhi
adc mlt_5, zero
; arg shift
11: lsl val_lo
rol val_hi
rol val_hlo
rol val_hhi
; next bit
lsr r0
brne 10b
; second table byte
#if AVR_ENH_LPM
lpm r0, Z+ ; C flag is stay 1
#else
lpm
adiw ZL, 1
sec
#endif
ror r0
; addition
12: brcc 13f
add mlt_2, val_hi ; val_hi is the least byte now
adc mlt_3, val_hlo
adc mlt_4, val_hhi
adc mlt_5, val_lo
adc mlt_6, zero
; arg shift
13: lsl val_hi
rol val_hlo
rol val_hhi
rol val_lo
; next bit
lsr r0
brne 12b
; 3-t table byte
#if AVR_ENH_LPM
lpm r0, Z+ ; C flag is stay 1
#else
lpm
adiw ZL, 1
sec
#endif
ror r0
; addition
14: brcc 15f
add mlt_3, val_hlo ; val_hlo is the least byte now
adc mlt_4, val_hhi
adc mlt_5, val_lo
adc mlt_6, val_hi
adc mlt_7, zero
; arg shift
15: lsl val_hlo
rol val_hhi
rol val_lo
rol val_hi
; next bit
lsr r0
brne 14b
; 4-t table byte
#if AVR_ENH_LPM
lpm r0, Z+ ; C flag is stay 1
#else
lpm
#endif
ror r0
; addition
16: brcc 17f
add mlt_4, val_hhi ; val_hhi is the least byte now
adc mlt_5, val_lo
adc mlt_6, val_hi
adc mlt_7, val_hlo
; arg shift
17: lsl val_hhi
rol val_lo
rol val_hi
rol val_hlo
; next bit
lsr r0
brne 16b
; decimal exponent
#if AVR_ENH_LPM
lpm exp10, Z
#else
adiw ZL, 1
lpm
mov exp10, r0
#endif
; result shift: mlt_7..2 >>= (~exp & 7)
com exp_sv
andi exp_sv, 7
breq 19f
18: lsr mlt_7
ror mlt_6
ror mlt_5
ror mlt_4
ror mlt_3
ror mlt_2
dec exp_sv
brne 18b
19:
/* --------------------------------------------------------------------
Conversion to string.
Registers usage:
mlt_7 .. mlt_2 - new mantissa (multiplication result)
pwr_7 .. pwr_2 - 'powr10' table element
Z - 'powr10' table pointer
X - output string pointer
maxdgs - number of digits
prec - number of digits stays to output
exp10 - decimal exponent
digit - conversion process
At the end:
X - end of buffer (nonfilled byte)
exp10 - corrected dec. exponent
mlt_7 .. mlt_2 - remainder
pwr_7 .. pwr_2 - last powr10[] element
Notes:
* It is possible to leave out powr10'x table with subnormal value.
Result: accuracy degrease on the rounding phase. No matter: high
precision with subnormals is not needed. (Now 0x00000001 is converted
exactly on prec = 5, i.e. 6 digits.)
*/
; to find first digit
ldi ZL, lo8(.L_powr10)
ldi ZH, hi8(.L_powr10)
set
; 'pwr10' element reading
.L_digit:
X_lpm pwr_2, Z+
X_lpm pwr_3, Z+
X_lpm pwr_4, Z+
X_lpm pwr_5, Z+
X_lpm pwr_6, Z+
X_lpm pwr_7, Z+
; 'digit' init.
ldi digit, '0' - 1
; subtraction loop
20: inc digit
sub mlt_2, pwr_2
sbc mlt_3, pwr_3
sbc mlt_4, pwr_4
sbc mlt_5, pwr_5
sbc mlt_6, pwr_6
sbc mlt_7, pwr_7
brsh 20b
; restore mult
add mlt_2, pwr_2
adc mlt_3, pwr_3
adc mlt_4, pwr_4
adc mlt_5, pwr_5
adc mlt_6, pwr_6
adc mlt_7, pwr_7
; analisys
brtc 25f
cpi digit, '0'
brne 21f ; this is the first digit finded
dec exp10
rjmp .L_digit
; now is the first digit
21: clt
; number of digits
subi maxdgs, 1
brlo 23f ; maxdgs was 0
add maxdgs, exp10
brpl 22f
clr maxdgs
22: cp maxdgs, prec
brsh 23f
mov prec, maxdgs
23: inc prec
mov maxdgs, prec
; operate digit
25: cpi digit, '0' + 10
brlo 27f
; overflow, digit > '9'
ldi digit, '9'
26: st X+, digit
dec prec
brne 26b
rjmp .L_up
; write digit
27: st X+, digit
dec prec
brne .L_digit
/* --------------------------------------------------------------------
Rounding.
*/
.L_round:
; pwr10 /= 2
lsr pwr_7
ror pwr_6
ror pwr_5
ror pwr_4
ror pwr_3
ror pwr_2
; mult -= pwr10 (half of last 'pwr10' value)
sub mlt_2, pwr_2
sbc mlt_3, pwr_3
sbc mlt_4, pwr_4
sbc mlt_5, pwr_5
sbc mlt_6, pwr_6
sbc mlt_7, pwr_7
; rounding direction?
brlo .L_rest
; round to up
.L_up:
inc prec
ld digit, -X
inc digit
cpi digit, '9' + 1
brlo 31f
ldi digit, '0'
31: st X, digit
cpse prec, maxdgs
brsh .L_up
; it was a carry to master digit
ld digit, -X ; flags
ori digit, FTOA_CARRY ; 'C' is not changed
st X+, digit
brlo .L_rest ; above comparison
; overflow
inc exp10
ldi digit, '1'
32: st X+, digit
ldi digit, '0'
dec prec
brne 32b
; restore
.L_rest:
clr zero
pop r14
pop r15
pop r16
pop r17
pop r28
pop r29
; return
clr r25
sbrc exp10, 7 ; high byte
com r25
ret
.size __ftoa_engine, . - __ftoa_engine
/* --------------------------------------------------------------------
Tables. '.L_powr10' is placed first -- for subnormals stability.
*/
.section .progmem.data,"a",@progbits
.type .L_powr10, "object"
.L_powr10:
.byte 0, 64, 122, 16, 243, 90 ; 100000000000000
.byte 0, 160, 114, 78, 24, 9 ; 10000000000000
.byte 0, 16, 165, 212, 232, 0 ; 1000000000000
.byte 0, 232, 118, 72, 23, 0 ; 100000000000
.byte 0, 228, 11, 84, 2, 0 ; 10000000000
.byte 0, 202, 154, 59, 0, 0 ; 1000000000
.byte 0, 225, 245, 5, 0, 0 ; 100000000
.byte 128, 150, 152, 0, 0, 0 ; 10000000
.byte 64, 66, 15, 0, 0, 0 ; 1000000
.byte 160, 134, 1, 0, 0, 0 ; 100000
.byte 16, 39, 0, 0, 0, 0 ; 10000
.byte 232, 3, 0, 0, 0, 0 ; 1000
.byte 100, 0, 0, 0, 0, 0 ; 100
.byte 10, 0, 0, 0, 0, 0 ; 10
.byte 1, 0, 0, 0, 0, 0 ; 1
.size .L_powr10, . - .L_powr10
.type .L_base10, "object"
.L_base10:
.byte 44, 118, 216, 136, -36 ; 2295887404
.byte 103, 79, 8, 35, -33 ; 587747175
.byte 193, 223, 174, 89, -31 ; 1504632769
.byte 177, 183, 150, 229, -29 ; 3851859889
.byte 228, 83, 198, 58, -26 ; 986076132
.byte 81, 153, 118, 150, -24 ; 2524354897
.byte 230, 194, 132, 38, -21 ; 646234854
.byte 137, 140, 155, 98, -19 ; 1654361225
.byte 64, 124, 111, 252, -17 ; 4235164736
.byte 188, 156, 159, 64, -14 ; 1084202172
.byte 186, 165, 111, 165, -12 ; 2775557562
.byte 144, 5, 90, 42, -9 ; 710542736
.byte 92, 147, 107, 108, -7 ; 1818989404
.byte 103, 109, 193, 27, -4 ; 465661287
.byte 224, 228, 13, 71, -2 ; 1192092896
.byte 245, 32, 230, 181, 0 ; 3051757813
.byte 208, 237, 144, 46, 3 ; 781250000
.byte 0, 148, 53, 119, 5 ; 2000000000
.byte 0, 128, 132, 30, 8 ; 512000000
.byte 0, 0, 32, 78, 10 ; 1310720000
.byte 0, 0, 0, 200, 12 ; 3355443200
.byte 51, 51, 51, 51, 15 ; 858993459
.byte 152, 110, 18, 131, 17 ; 2199023256
.byte 65, 239, 141, 33, 20 ; 562949953
.byte 137, 59, 230, 85, 22 ; 1441151881
.byte 207, 254, 230, 219, 24 ; 3689348815
.byte 209, 132, 75, 56, 27 ; 944473297
.byte 247, 124, 29, 144, 29 ; 2417851639
.byte 164, 187, 228, 36, 32 ; 618970020
.byte 50, 132, 114, 94, 34 ; 1584563250
.byte 129, 0, 201, 241, 36 ; 4056481921
.byte 236, 161, 229, 61, 39 ; 1038459372
.size .L_base10, . - .L_base10
.end
#endif /* !__DOXYGEN__ */

48
libraries/FastSerial/ftoa_engine.h Normal file
View File

@ -0,0 +1,48 @@
/* Copyright (c) 2005, Dmitry Xmelkov
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in
the documentation and/or other materials provided with the
distribution.
* Neither the name of the copyright holders nor the names of
contributors may be used to endorse or promote products derived
from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
POSSIBILITY OF SUCH DAMAGE. */
/* $Id: ftoa_engine.h 1218 2007-02-18 13:18:41Z dmix $ */
#ifndef _FTOA_ENGINE_H
#define _FTOA_ENGINE_H
#ifndef __ASSEMBLER__
int __ftoa_engine (double val, char *buf,
unsigned char prec, unsigned char maxdgs);
#endif
/* '__ftoa_engine' return next flags (in buf[0]): */
#define FTOA_MINUS 1
#define FTOA_ZERO 2
#define FTOA_INF 4
#define FTOA_NAN 8
#define FTOA_CARRY 16 /* Carry was to master position. */
#endif /* !_FTOA_ENGINE_H */

365
libraries/FastSerial/macros.inc Normal file
View File

@ -0,0 +1,365 @@
/* Copyright (c) 2002, 2005, 2006, 2007 Marek Michalkiewicz
Copyright (c) 2006 Dmitry Xmelkov
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in
the documentation and/or other materials provided with the
distribution.
* Neither the name of the copyright holders nor the names of
contributors may be used to endorse or promote products derived
from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
POSSIBILITY OF SUCH DAMAGE. */
/*
macros.inc - macros for use in assembler sources
Contributors:
Created by Marek Michalkiewicz <marekm@linux.org.pl>
*/
#include <avr/io.h>
//#include "sectionname.h"
/* if not defined, assume old version with underscores */
#ifndef __USER_LABEL_PREFIX__
#define __USER_LABEL_PREFIX__ _
#endif
#ifndef __REGISTER_PREFIX__
#define __REGISTER_PREFIX__
#endif
/* the assembler line separator (just in case it ever changes) */
#define _L $
#define CONCAT1(a, b) CONCAT2(a, b)
#define CONCAT2(a, b) a ## b
#define _U(x) CONCAT1(__USER_LABEL_PREFIX__, x)
#define _R(x) CONCAT1(__REGISTER_PREFIX__, x)
/* these should help to fix the "can't have function named r1()" bug
which may require adding '%' in front of register names. */
#define r0 _R(r0)
#define r1 _R(r1)
#define r2 _R(r2)
#define r3 _R(r3)
#define r4 _R(r4)
#define r5 _R(r5)
#define r6 _R(r6)
#define r7 _R(r7)
#define r8 _R(r8)
#define r9 _R(r9)
#define r10 _R(r10)
#define r11 _R(r11)
#define r12 _R(r12)
#define r13 _R(r13)
#define r14 _R(r14)
#define r15 _R(r15)
#define r16 _R(r16)
#define r17 _R(r17)
#define r18 _R(r18)
#define r19 _R(r19)
#define r20 _R(r20)
#define r21 _R(r21)
#define r22 _R(r22)
#define r23 _R(r23)
#define r24 _R(r24)
#define r25 _R(r25)
#define r26 _R(r26)
#define r27 _R(r27)
#define r28 _R(r28)
#define r29 _R(r29)
#define r30 _R(r30)
#define r31 _R(r31)
#ifndef __tmp_reg__
#define __tmp_reg__ r0
#endif
#ifndef __zero_reg__
#define __zero_reg__ r1
#endif
#if __AVR_MEGA__
#define XJMP jmp
#define XCALL call
#else
#define XJMP rjmp
#define XCALL rcall
#endif
/* used only by fplib/strtod.S - libgcc internal function calls */
#define PROLOGUE_SAVES(offset) XJMP (__prologue_saves__ + 2 * (offset))
#define EPILOGUE_RESTORES(offset) XJMP (__epilogue_restores__ + 2 * (offset))
#if FLASHEND > 0x10000 /* ATmega103 */
#define BIG_CODE 1
#else
#define BIG_CODE 0
#endif
#ifndef __AVR_HAVE_MOVW__
# if defined(__AVR_ENHANCED__) && __AVR_ENHANCED__
# define __AVR_HAVE_MOVW__ 1
# endif
#endif
#ifndef __AVR_HAVE_LPMX__
# if defined(__AVR_ENHANCED__) && __AVR_ENHANCED__
# define __AVR_HAVE_LPMX__ 1
# endif
#endif
#ifndef __AVR_HAVE_MUL__
# if defined(__AVR_ENHANCED__) && __AVR_ENHANCED__
# define __AVR_HAVE_MUL__ 1
# endif
#endif
/*
Smart version of movw:
- uses "movw" if possible (supported by MCU, and both registers even)
- handles overlapping register pairs correctly
- no instruction generated if source and destination are the same
(may expand to 0, 1 or 2 instructions).
*/
.macro X_movw dst src
.L_movw_dst = -1
.L_movw_src = -1
.L_movw_n = 0
.irp reg, r0, r1, r2, r3, r4, r5, r6, r7, r8, r9, \
r10,r11,r12,r13,r14,r15,r16,r17,r18,r19, \
r20,r21,r22,r23,r24,r25,r26,r27,r28,r29, \
r30,r31
.ifc \reg,\dst
.L_movw_dst = .L_movw_n
.endif
.ifc \reg,\src
.L_movw_src = .L_movw_n
.endif
.L_movw_n = .L_movw_n + 1
.endr
.L_movw_n = 0
.irp reg, R0, R1, R2, R3, R4, R5, R6, R7, R8, R9, \
R10,R11,R12,R13,R14,R15,R16,R17,R18,R19, \
R20,R21,R22,R23,R24,R25,R26,R27,R28,R29, \
R30,R31
.ifc \reg,\dst
.L_movw_dst = .L_movw_n
.endif
.ifc \reg,\src
.L_movw_src = .L_movw_n
.endif
.L_movw_n = .L_movw_n + 1
.endr
.if .L_movw_dst < 0
.L_movw_n = 0
.rept 32
.if \dst == .L_movw_n
.L_movw_dst = .L_movw_n
.endif
.L_movw_n = .L_movw_n + 1
.endr
.endif
.if .L_movw_src < 0
.L_movw_n = 0
.rept 32
.if \src == .L_movw_n
.L_movw_src = .L_movw_n
.endif
.L_movw_n = .L_movw_n + 1
.endr
.endif
.if (.L_movw_dst < 0) || (.L_movw_src < 0)
.err ; Invalid 'X_movw' arg.
.endif
.if ((.L_movw_src) - (.L_movw_dst)) /* different registers */
.if (((.L_movw_src) | (.L_movw_dst)) & 0x01)
.if (((.L_movw_src)-(.L_movw_dst)) & 0x80) /* src < dest */
mov (.L_movw_dst)+1, (.L_movw_src)+1
mov (.L_movw_dst), (.L_movw_src)
.else /* src > dest */
mov (.L_movw_dst), (.L_movw_src)
mov (.L_movw_dst)+1, (.L_movw_src)+1
.endif
.else /* both even -> overlap not possible */
#if defined(__AVR_HAVE_MOVW__) && __AVR_HAVE_MOVW__
movw \dst, \src
#else
mov (.L_movw_dst), (.L_movw_src)
mov (.L_movw_dst)+1, (.L_movw_src)+1
#endif
.endif
.endif
.endm
/* Macro 'X_lpm' extends enhanced lpm instruction for classic chips.
Usage:
X_lpm reg, dst
where
reg is 0..31, r0..r31 or R0..R31
dst is z, Z, z+ or Z+
It is possible to omit both arguments.
Possible results for classic chips:
lpm
lpm / mov Rd,r0
lpm / adiw ZL,1
lpm / mov Rd,r0 / adiw ZL,1
For enhanced chips it is one instruction always.
ATTENTION: unlike enhanced chips SREG (S,V,N,Z,C) flags are
changed in case of 'Z+' dst. R0 is scratch.
*/
.macro X_lpm dst=r0, src=Z
/* dst evaluation */
.L_lpm_dst = -1
.L_lpm_n = 0
.irp reg, r0, r1, r2, r3, r4, r5, r6, r7, r8, r9, \
r10,r11,r12,r13,r14,r15,r16,r17,r18,r19, \
r20,r21,r22,r23,r24,r25,r26,r27,r28,r29, \
r30,r31
.ifc \reg,\dst
.L_lpm_dst = .L_lpm_n
.endif
.L_lpm_n = .L_lpm_n + 1
.endr
.L_lpm_n = 0
.irp reg, R0, R1, R2, R3, R4, R5, R6, R7, R8, R9, \
R10,R11,R12,R13,R14,R15,R16,R17,R18,R19, \
R20,R21,R22,R23,R24,R25,R26,R27,R28,R29, \
R30,R31
.ifc \reg,\dst
.L_lpm_dst = .L_lpm_n
.endif
.L_lpm_n = .L_lpm_n + 1
.endr
.if .L_lpm_dst < 0
.L_lpm_n = 0
.rept 32
.if \dst == .L_lpm_n
.L_lpm_dst = .L_lpm_n
.endif
.L_lpm_n = .L_lpm_n + 1
.endr
.endif
.if (.L_lpm_dst < 0)
.err ; Invalid dst arg of 'X_lpm' macro.
.endif
/* src evaluation */
.L_lpm_src = -1
.L_lpm_n = 0
.irp reg, z,Z,z+,Z+
.ifc \reg,\src
.L_lpm_src = .L_lpm_n
.endif
.L_lpm_n = .L_lpm_n + 1
.endr
.if (.L_lpm_src < 0)
.err ; Invalid src arg of 'X_lpm' macro.
.endif
/* instruction(s) */
.if .L_lpm_src < 2
.if .L_lpm_dst == 0
lpm
.else
#if defined(__AVR_HAVE_LPMX__) && __AVR_HAVE_LPMX__
lpm .L_lpm_dst, Z
#else
lpm
mov .L_lpm_dst, r0
#endif
.endif
.else
.if (.L_lpm_dst >= 30)
.err ; Registers 30 and 31 are inhibited as 'X_lpm *,Z+' dst.
.endif
#if defined(__AVR_HAVE_LPMX__) && __AVR_HAVE_LPMX__
lpm .L_lpm_dst, Z+
#else
lpm
.if .L_lpm_dst
mov .L_lpm_dst, r0
.endif
adiw r30, 1
#endif
.endif
.endm
/*
LPM_R0_ZPLUS_INIT is used before the loop to initialize RAMPZ
for future devices with RAMPZ:Z auto-increment - [e]lpm r0, Z+.
LPM_R0_ZPLUS_NEXT is used inside the loop to load a byte from
the program memory at [RAMPZ:]Z to R0, and increment [RAMPZ:]Z.
The argument in both macros is a register that contains the
high byte (bits 23-16) of the address, bits 15-0 should be in
the Z (r31:r30) register. It can be any register except for:
r0, r1 (__zero_reg__ - assumed to always contain 0), r30, r31.
*/
.macro LPM_R0_ZPLUS_INIT hhi
#if __AVR_ENHANCED__
#if BIG_CODE
out AVR_RAMPZ_ADDR, \hhi
#endif
#endif
.endm
.macro LPM_R0_ZPLUS_NEXT hhi
#if __AVR_ENHANCED__
#if BIG_CODE
/* ELPM with RAMPZ:Z post-increment, load RAMPZ only once */
elpm r0, Z+
#else
/* LPM with Z post-increment, max 64K, no RAMPZ (ATmega83/161/163/32) */
lpm r0, Z+
#endif
#else
#if BIG_CODE
/* ELPM without post-increment, load RAMPZ each time (ATmega103) */
out AVR_RAMPZ_ADDR, \hhi
elpm
adiw r30,1
adc \hhi, __zero_reg__
#else
/* LPM without post-increment, max 64K, no RAMPZ (AT90S*) */
lpm
adiw r30,1
#endif
#endif
.endm

54
libraries/FastSerial/ntz.h Normal file
View File

@ -0,0 +1,54 @@
/* Copyright (c) 2007, Dmitry Xmelkov
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in
the documentation and/or other materials provided with the
distribution.
* Neither the name of the copyright holders nor the names of
contributors may be used to endorse or promote products derived
from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
POSSIBILITY OF SUCH DAMAGE. */
/* $Id: ntz.h 1217 2007-02-18 13:18:05Z dmix $ */
#ifndef _NTZ_H
#define _NTZ_H
/* Number of Tail Zeros: ntz(x)= (ffs(x) ? ffs(x)-1 : 16)
It works with all: cpp, gcc and gas expressions. */
#define ntz(x) \
( (1 & (((x) & 1) == 0)) \
+ (1 & (((x) & 3) == 0)) \
+ (1 & (((x) & 7) == 0)) \
+ (1 & (((x) & 017) == 0)) \
+ (1 & (((x) & 037) == 0)) \
+ (1 & (((x) & 077) == 0)) \
+ (1 & (((x) & 0177) == 0)) \
+ (1 & (((x) & 0377) == 0)) \
+ (1 & (((x) & 0777) == 0)) \
+ (1 & (((x) & 01777) == 0)) \
+ (1 & (((x) & 03777) == 0)) \
+ (1 & (((x) & 07777) == 0)) \
+ (1 & (((x) & 017777) == 0)) \
+ (1 & (((x) & 037777) == 0)) \
+ (1 & (((x) & 077777) == 0)) \
+ (1 & (((x) & 0177777) == 0)) )
#endif /* !_NTZ_H */

216
libraries/FastSerial/ultoa_invert.S Normal file
View File

@ -0,0 +1,216 @@
/* Copyright (c) 2005,2007 Dmitry Xmelkov
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in
the documentation and/or other materials provided with the
distribution.
* Neither the name of the copyright holders nor the names of
contributors may be used to endorse or promote products derived
from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
POSSIBILITY OF SUCH DAMAGE. */
/* $Id: ultoa_invert.S 1944 2009-04-01 23:12:20Z arcanum $ */
#ifndef __DOXYGEN__
#include "macros.inc"
#include "ntz.h"
#include "xtoa_fast.h"
/* --------------------------------------------------------------------
char * __ultoa_invert (unsigned long val, char * str, int base)
This function is intended for usage as internal printf's one.
It differs from others of `xtoa_fast' family:
* srt[] will NOT 0 terminated.
* Sequence of digits is inverted.
* It returns pointer to first byte after a string.
* Only `XTOA_UPPER' flag is operated.
Notes:
* base: check only 8 and 16, all others are treated as 10.
(internal printf's function).
*/
/* Input */
#define v_lo r22
#define v_hi r23
#define v_hlo r24
#define v_hhi r25
#define str_lo r20
#define str_hi r21
#define base r18
#define flags r19
/* Used */
#define v_fifth r26 /* val: bits 39..32 */
#define t_lo r18 /* temporary for shifted `val' */
#define t_hi r19
#define t_hlo r20
#define t_hhi r21
#define symb r20 /* write to string */
#define cnt r27 /* shift loop counter, local arg */
/* Fixed */
#define rzero r1
/* ASSEMBLY_CLIB_SECTION */
.global __ultoa_invert
.type __ultoa_invert, "function"
__ultoa_invert:
X_movw ZL, str_lo
clr v_fifth ; needed for all (ultoa_lsr)
cpi base, 8
breq .L_oct
cpi base, 16
breq .L_hex
; decimal format
clt ; flag of val == 0
.L_dec_loop:
push v_lo ; to calculate remander
; val &= ~1
andi v_lo, ~1
; val += 2
subi v_lo, lo8(-2)
sbci v_hi, hi8(-2)
sbci v_hlo, hlo8(-2)
sbci v_hhi, hhi8(-2)
sbci v_fifth, hhi8(-2)
; val += val/2
ldi cnt, 1
rcall .L_div_add
; val += val/16
ldi cnt, 4
rcall .L_div_add
; val += val/256
add v_lo, v_hi
adc v_hi, v_hlo
adc v_hlo, v_hhi
adc v_hhi, v_fifth
adc v_fifth, rzero
; val += val/65536
add v_lo, v_hlo
adc v_hi, v_hhi
adc v_hlo, v_fifth
adc v_hhi, rzero
adc v_fifth, rzero
; val += val >> 32
add v_lo, v_fifth
adc v_hi, rzero
adc v_hlo, rzero
adc v_hhi, rzero
adc v_fifth, rzero
; division result: val /= 16
rcall .L_lsr_4 ; v_fitth := 0
brne 1f
set ; T := Z flag
1:
; rem: val_original - 10*val
pop t_hi
#if defined(__AVR_ENHANCED__) && __AVR_ENHANCED__
ldi t_lo, 10
mul t_lo, v_lo
clr r1
#else
mov r0, v_lo
lsl r0
sub t_hi, r0
lsl r0
lsl r0
#endif
sub t_hi, r0
; output digit
subi t_hi, lo8(-'0')
st Z+, t_hi
; quotient == 0 ?
brtc .L_dec_loop
; end of string
.L_eos:
X_movw r24, ZL
ret
; octal format
.L_oct:
mov symb, v_lo
andi symb, 7
subi symb, lo8(-'0')
st Z+, symb
ldi cnt, 3
rcall .L_lsr
brne .L_oct
rjmp .L_eos
; hex format
.L_hex:
mov symb, v_lo
andi symb, 0x0f
subi symb, lo8(-'0')
cpi symb, '9' + 1
brlo 3f
subi symb, lo8('9' + 1 - 'a')
sbrc flags, ntz(XTOA_UPPER) - 8
subi symb, lo8('a' - 'A')
3: st Z+, symb
rcall .L_lsr_4
brne .L_hex
rjmp .L_eos
.L_lsr_4:
ldi cnt, 4
.L_lsr:
lsr v_fifth
ror v_hhi
ror v_hlo
ror v_hi
ror v_lo
dec cnt
brne .L_lsr
; tst
sbiw v_hlo, 0 ; only Z flag is needed
cpc v_lo, rzero
cpc v_hi, rzero
ret
.L_div_add:
; copy to temporary
X_movw t_lo, v_lo
X_movw t_hlo, v_hlo
mov r0, v_fifth
; lsr temporary
7: lsr r0
ror t_hhi
ror t_hlo
ror t_hi
ror t_lo
dec cnt
brne 7b
; add
add v_lo, t_lo
adc v_hi, t_hi
adc v_hlo, t_hlo
adc v_hhi, t_hhi
adc v_fifth, r0 ; here r0 == 0
ret
.size __ultoa_invert, . - __ultoa_invert
.end
#endif /* !__DOXYGEN__ */

511
libraries/FastSerial/vprintf.cpp Normal file
View File

@ -0,0 +1,511 @@
// -*- Mode: C++; c-basic-offset: 8; indent-tabs-mode: nil -*-
/*
Adapted from the avr-libc vfprintf:
Copyright (c) 2002, Alexander Popov (sasho@vip.bg)
Copyright (c) 2002,2004,2005 Joerg Wunsch
Copyright (c) 2005, Helmut Wallner
Copyright (c) 2007, Dmitry Xmelkov
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in
the documentation and/or other materials provided with the
distribution.
* Neither the name of the copyright holders nor the names of
contributors may be used to endorse or promote products derived
from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
POSSIBILITY OF SUCH DAMAGE.
*/
/* From: Id: printf_p_new.c,v 1.1.1.9 2002/10/15 20:10:28 joerg_wunsch Exp */
/* $Id: vfprintf.c,v 1.18.2.1 2009/04/01 23:12:06 arcanum Exp $ */
#include "BetterStream.h"
#include <avr/pgmspace.h>
#include <stdarg.h>
#include <string.h>
extern "C" {
#include "ftoa_engine.h"
#include "ntz.h"
#include "xtoa_fast.h"
}
#define GETBYTE(flag, mask, pnt) ({ \
unsigned char __c; \
asm ( \
"sbrc %2,%3 \n\t" \
"lpm %0,Z+ \n\t" \
"sbrs %2,%3 \n\t" \
"ld %0,Z+ " \
: "=r" (__c), \
"+z" (pnt) \
: "r" (flag), \
"I" (ntz(mask)) \
); \
__c; \
})
/*
#define GETBYTE(flag, mask, pnt) ({ \
unsigned char __c; \
__c = ((flag) & (mask)) \
? pgm_read_byte(pnt) : *pnt; \
pnt++; \
__c; \
})
*/
#define FL_ZFILL 0x01
#define FL_PLUS 0x02
#define FL_SPACE 0x04
#define FL_LPAD 0x08
#define FL_ALT 0x10
#define FL_WIDTH 0x20
#define FL_PREC 0x40
#define FL_LONG 0x80
#define FL_PGMSTRING FL_LONG
#define FL_NEGATIVE FL_LONG
#define FL_ALTUPP FL_PLUS
#define FL_ALTHEX FL_SPACE
#define FL_FLTUPP FL_ALT
#define FL_FLTEXP FL_PREC
#define FL_FLTFIX FL_LONG
void
BetterStream::_vprintf (unsigned char in_progmem, const char *fmt, va_list ap)
{
unsigned char c; /* holds a char from the format string */
unsigned char flags;
unsigned char width;
unsigned char prec;
unsigned char buf[11]; /* size for -1 in octal, without '\0' */
for (;;) {
/*
* Process non-format characters
*/
for (;;) {
c = GETBYTE (in_progmem, 1, fmt);
if (!c) return;
if (c == '%') {
c = GETBYTE (in_progmem, 1, fmt);
if (c != '%') break;
}
write(c);
}
flags = 0;
width = 0;
prec = 0;
/*
* Process format adjustment characters, precision, width.
*/
do {
if (flags < FL_WIDTH) {
switch (c) {
case '0':
flags |= FL_ZFILL;
continue;
case '+':
flags |= FL_PLUS;
/* FALLTHROUGH */
case ' ':
flags |= FL_SPACE;
continue;
case '-':
flags |= FL_LPAD;
continue;
case '#':
flags |= FL_ALT;
continue;
}
}
if (flags < FL_LONG) {
if (c >= '0' && c <= '9') {
c -= '0';
if (flags & FL_PREC) {
prec = 10*prec + c;
continue;
}
width = 10*width + c;
flags |= FL_WIDTH;
continue;
}
if (c == '.') {
if (flags & FL_PREC)
return;
flags |= FL_PREC;
continue;
}
if (c == 'l') {
flags |= FL_LONG;
continue;
}
if (c == 'h')
continue;
}
break;
} while ( (c = GETBYTE (in_progmem, 1, fmt)) != 0);
/*
* Handle floating-point formats E, F, G, e, f, g.
*/
if (c >= 'E' && c <= 'G') {
flags |= FL_FLTUPP;
c += 'e' - 'E';
goto flt_oper;
} else if (c >= 'e' && c <= 'g') {
int exp; /* exponent of master decimal digit */
int n;
unsigned char vtype; /* result of float value parse */
unsigned char sign; /* sign character (or 0) */
unsigned char ndigs;
flags &= ~FL_FLTUPP;
flt_oper:
if (!(flags & FL_PREC))
prec = 6;
flags &= ~(FL_FLTEXP | FL_FLTFIX);
if (c == 'e')
flags |= FL_FLTEXP;
else if (c == 'f')
flags |= FL_FLTFIX;
else if (prec > 0)
prec -= 1;
if (flags & FL_FLTFIX) {
vtype = 7; /* 'prec' arg for 'ftoa_engine' */
ndigs = prec < 60 ? prec + 1 : 60;
} else {
if (prec > 7) prec = 7;
vtype = prec;
ndigs = 0;
}
exp = __ftoa_engine (va_arg(ap,double), (char *)buf, vtype, ndigs);
vtype = buf[0];
sign = 0;
if ((vtype & FTOA_MINUS) && !(vtype & FTOA_NAN))
sign = '-';
else if (flags & FL_PLUS)
sign = '+';
else if (flags & FL_SPACE)
sign = ' ';
if (vtype & (FTOA_NAN | FTOA_INF)) {
const char *p;
ndigs = sign ? 4 : 3;
if (width > ndigs) {
width -= ndigs;
if (!(flags & FL_LPAD)) {
do {
write(' ');
} while (--width);
}
} else {
width = 0;
}
if (sign)
write(sign);
p = PSTR("inf");
if (vtype & FTOA_NAN)
p = PSTR("nan");
while ( (ndigs = pgm_read_byte(p)) != 0) {
if (flags & FL_FLTUPP)
ndigs += 'I' - 'i';
write(ndigs);
p++;
}
goto tail;
}
/* Output format adjustment, number of decimal digits in buf[] */
if (flags & FL_FLTFIX) {
ndigs += exp;
if ((vtype & FTOA_CARRY) && buf[1] == '1')
ndigs -= 1;
if ((signed char)ndigs < 1)
ndigs = 1;
else if (ndigs > 8)
ndigs = 8;
} else if (!(flags & FL_FLTEXP)) { /* 'g(G)' format */
if (exp <= prec && exp >= -4)
flags |= FL_FLTFIX;
while (prec && buf[1+prec] == '0')
prec--;
if (flags & FL_FLTFIX) {
ndigs = prec + 1; /* number of digits in buf */
prec = prec > exp
? prec - exp : 0; /* fractional part length */
}
}
/* Conversion result length, width := free space length */
if (flags & FL_FLTFIX)
n = (exp>0 ? exp+1 : 1);
else
n = 5; /* 1e+00 */
if (sign) n += 1;
if (prec) n += prec + 1;
width = width > n ? width - n : 0;
/* Output before first digit */
if (!(flags & (FL_LPAD | FL_ZFILL))) {
while (width) {
write(' ');
width--;
}
}
if (sign) write(sign);
if (!(flags & FL_LPAD)) {
while (width) {
write('0');
width--;
}
}
if (flags & FL_FLTFIX) { /* 'f' format */
n = exp > 0 ? exp : 0; /* exponent of left digit */
do {
if (n == -1)
write('.');
flags = (n <= exp && n > exp - ndigs)
? buf[exp - n + 1] : '0';
if (--n < -prec)
break;
write(flags);
} while (1);
if (n == exp
&& (buf[1] > '5'
|| (buf[1] == '5' && !(vtype & FTOA_CARRY))) )
{
flags = '1';
}
write(flags);
} else { /* 'e(E)' format */
/* mantissa */
if (buf[1] != '1')
vtype &= ~FTOA_CARRY;
write(buf[1]);
if (prec) {
write('.');
sign = 2;
do {
write(buf[sign++]);
} while (--prec);
}
/* exponent */
write(flags & FL_FLTUPP ? 'E' : 'e');
ndigs = '+';
if (exp < 0 || (exp == 0 && (vtype & FTOA_CARRY) != 0)) {
exp = -exp;
ndigs = '-';
}
write(ndigs);
for (ndigs = '0'; exp >= 10; exp -= 10)
ndigs += 1;
write(ndigs);
write('0' + exp);
}
goto tail;
}
/*
* Handle string formats c, s, S.
*/
{
const char * pnt;
size_t size;
switch (c) {
case 'c':
buf[0] = va_arg (ap, int);
pnt = (char *)buf;
size = 1;
goto no_pgmstring;
case 's':
pnt = va_arg (ap, char *);
size = strnlen (pnt, (flags & FL_PREC) ? prec : ~0);
no_pgmstring:
flags &= ~FL_PGMSTRING;
goto str_lpad;
case 'S':
pnt = va_arg (ap, char *);
size = strnlen_P (pnt, (flags & FL_PREC) ? prec : ~0);
flags |= FL_PGMSTRING;
str_lpad:
if (!(flags & FL_LPAD)) {
while (size < width) {
write(' ');
width--;
}
}
while (size) {
write(GETBYTE (flags, FL_PGMSTRING, pnt));
if (width) width -= 1;
size -= 1;
}
goto tail;
}
}
/*
* Handle integer formats variations for d/i, u, o, p, x, X.
*/
if (c == 'd' || c == 'i') {
long x = (flags & FL_LONG) ? va_arg(ap,long) : va_arg(ap,int);
flags &= ~(FL_NEGATIVE | FL_ALT);
if (x < 0) {
x = -x;
flags |= FL_NEGATIVE;
}
c = __ultoa_invert (x, (char *)buf, 10) - (char *)buf;
} else {
int base;
if (c == 'u') {
flags &= ~FL_ALT;
base = 10;
goto ultoa;
}
flags &= ~(FL_PLUS | FL_SPACE);
switch (c) {
case 'o':
base = 8;
goto ultoa;
case 'p':
flags |= FL_ALT;
/* no break */
case 'x':
if (flags & FL_ALT)
flags |= FL_ALTHEX;
base = 16;
goto ultoa;
case 'X':
if (flags & FL_ALT)
flags |= (FL_ALTHEX | FL_ALTUPP);
base = 16 | XTOA_UPPER;
ultoa:
c = __ultoa_invert ((flags & FL_LONG)
? va_arg(ap, unsigned long)
: va_arg(ap, unsigned int),
(char *)buf, base) - (char *)buf;
flags &= ~FL_NEGATIVE;
break;
default:
return;
}
}
/*
* Format integers.
*/
{
unsigned char len;
len = c;
if (flags & FL_PREC) {
flags &= ~FL_ZFILL;
if (len < prec) {
len = prec;
if ((flags & FL_ALT) && !(flags & FL_ALTHEX))
flags &= ~FL_ALT;
}
}
if (flags & FL_ALT) {
if (buf[c-1] == '0') {
flags &= ~(FL_ALT | FL_ALTHEX | FL_ALTUPP);
} else {
len += 1;
if (flags & FL_ALTHEX)
len += 1;
}
} else if (flags & (FL_NEGATIVE | FL_PLUS | FL_SPACE)) {
len += 1;
}
if (!(flags & FL_LPAD)) {
if (flags & FL_ZFILL) {
prec = c;
if (len < width) {
prec += width - len;
len = width;
}
}
while (len < width) {
write(' ');
len++;
}
}
width = (len < width) ? width - len : 0;
if (flags & FL_ALT) {
write('0');
if (flags & FL_ALTHEX)
write(flags & FL_ALTUPP ? 'X' : 'x');
} else if (flags & (FL_NEGATIVE | FL_PLUS | FL_SPACE)) {
unsigned char z = ' ';
if (flags & FL_PLUS) z = '+';
if (flags & FL_NEGATIVE) z = '-';
write(z);
}
while (prec > c) {
write('0');
prec--;
}
do {
write(buf[--c]);
} while (c);
}
tail:
/* Tail is possible. */
while (width) {
write(' ');
width--;
}
} /* for (;;) */
}

48
libraries/FastSerial/xtoa_fast.h Normal file
View File

@ -0,0 +1,48 @@
/*
Adapted from avr-libc:
Copyright (c) 2005, Dmitry Xmelkov
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in
the documentation and/or other materials provided with the
distribution.
* Neither the name of the copyright holders nor the names of
contributors may be used to endorse or promote products derived
from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
POSSIBILITY OF SUCH DAMAGE. */
/* $Id: xtoa_fast.h 1223 2007-02-18 13:33:09Z dmix $ */
#ifndef _XTOA_FAST_H_
#define _XTOA_FAST_H_
#ifndef __ASSEMBLER__
/* Internal function for use from `printf'. */
char * __ultoa_invert (unsigned long val, char *s, int base);
#endif /* ifndef __ASSEMBLER__ */
/* Next flags are to use with `base'. Unused fields are reserved. */
#define XTOA_PREFIX 0x0100 /* put prefix for octal or hex */
#define XTOA_UPPER 0x0200 /* use upper case letters */
#endif /* _XTOA_FAST_H_ */