mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-02 14:13:42 -04:00
43262a573f
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
217 lines
4.9 KiB
ArmAsm
217 lines
4.9 KiB
ArmAsm
/* 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__ */
|