forked from haraldwolff/avr-fw-modules
143 lines
1.7 KiB
ArmAsm
143 lines
1.7 KiB
ArmAsm
|
|
.global float_fix16
|
|
|
|
float_fix16:
|
|
; param: in float r22:r25
|
|
; ret int64(48.16)r18:r25
|
|
; var: exp r13
|
|
; sign r12.1
|
|
|
|
push r17
|
|
push r16
|
|
push r15
|
|
push r14
|
|
push r13
|
|
push r12
|
|
|
|
cp r22, r1
|
|
cpc r23, r1
|
|
cpc r24, r1
|
|
cpc r25, r1
|
|
breq _zero
|
|
|
|
; Mantisse verschieben => r18:r20.(0-6)
|
|
mov r18, r22
|
|
mov r19, r23
|
|
mov r20, r24
|
|
ori r20, 0x80
|
|
|
|
; Exponent sichern => r13 (korrektur: -7, wg. Position mantisse B.24 auf E+8)
|
|
mov r13, r25
|
|
mov r12, r24
|
|
lsl r12
|
|
rol r13
|
|
ldi r16, 134
|
|
sub r13, r16
|
|
|
|
; Sign => r12.0
|
|
eor r12, r12
|
|
ldi r16, 0x01
|
|
sbrc r25, 7
|
|
or r12, r16
|
|
|
|
eor r21, r21
|
|
eor r22, r22
|
|
eor r23, r23
|
|
eor r24, r24
|
|
eor r25, r25
|
|
|
|
cp r13, r1
|
|
breq _l1
|
|
brmi _shr
|
|
|
|
; Shift left...
|
|
|
|
_shl:
|
|
lsl r18
|
|
rol r19
|
|
rol r20
|
|
rol r21
|
|
rol r22
|
|
rol r23
|
|
rol r24
|
|
rol r25
|
|
dec r13
|
|
tst r13
|
|
brne _shl
|
|
|
|
rjmp _l1
|
|
|
|
; Shift right...
|
|
_shr:
|
|
ldi r16, -23
|
|
cp r13, r16
|
|
brlo _zero
|
|
|
|
_shr2:
|
|
lsr r20
|
|
ror r19
|
|
ror r18
|
|
inc r13
|
|
tst r13
|
|
brne _shr2
|
|
|
|
|
|
_l1:
|
|
sbrs r12, 0
|
|
rjmp _exit
|
|
|
|
com r18
|
|
com r19
|
|
com r20
|
|
com r21
|
|
com r22
|
|
com r23
|
|
com r24
|
|
com r25
|
|
ldi r16, 0x01
|
|
add r18, r16
|
|
adc r19, r1
|
|
adc r20, r1
|
|
adc r21, r1
|
|
adc r22, r1
|
|
adc r23, r1
|
|
adc r24, r1
|
|
adc r25, r1
|
|
|
|
rjmp _exit
|
|
|
|
_zero:
|
|
eor r18, r18
|
|
eor r19, r19
|
|
eor r20, r20
|
|
eor r21, r21
|
|
eor r22, r22
|
|
eor r23, r23
|
|
eor r24, r24
|
|
eor r25, r25
|
|
|
|
_exit:
|
|
pop r12
|
|
pop r13
|
|
pop r14
|
|
pop r15
|
|
pop r16
|
|
pop r17
|
|
ret
|
|
|
|
|
|
#if 0
|
|
fp4816_t fp4816_from_float(float value)
|
|
{
|
|
IEEEFLOAT ieee = { value };
|
|
int16_t exp = ieee.exponent - 127;
|
|
int32_t mantisse = ieee.mantisse | 0x00800000;
|
|
fp4816_t fp = (fp4816_t)(mantisse >> (15 - exp));
|
|
|
|
if (ieee.sign)
|
|
fp *= -1;
|
|
|
|
return fp;
|
|
};
|
|
#endif
|