/* __fp_rempio2 (float x); The __fp_rempio2() function computes the remainder of dividing absolute value of x by Pi/2. The return value is x - n*Pi/2, where n is the quotient of abs(x)/(Pi/2), rounded towards zero to an integer. Output: rA3.rA2.rA1.rA0.rAE - flt40_t remainder ZL - low byte of n */ #include "fp32def.h" #include "asmdef.h" #define MNT32_PIO2 0xC90FDAA2 /* mantissa of Pi/2, 32 bits */ FUNCTION __fp_rempio2 0: rjmp _U(__fp_nan) ENTRY __fp_rempio2 rcall _U(__fp_splitA) brcs 0b ; C set, caller takes care of INF, NAN clt ; Result always positive clr ZL clr rAE subi rA3,0x7f ; We know the exp of pi/2 is 0 brcs 6f ; jump if exp less than exp of pi/2 ldi rB0, hi8(MNT32_PIO2) ldi rB1,hlo8(MNT32_PIO2) ldi rB2,hhi8(MNT32_PIO2) rjmp 1f ;This jump speeds up the loop .Loop: lsl ZL lsl rAE rol rA0 rol rA1 rol rA2 brcs 2f 1: cpi rAE,lo8(MNT32_PIO2) cpc rA0,rB0 cpc rA1,rB1 cpc rA2,rB2 brcs 3f 2: subi rAE,lo8(MNT32_PIO2) sbc rA0,rB0 sbc rA1,rB1 sbc rA2,rB2 inc ZL ; One more times subtracted 3: dec rA3 brpl .Loop ; normalize, we know that rA2.1.0.E >= 0x0E cpi rA2,0x80 brcc 6f 4: dec rA3 lsl rAE rol rA0 rol rA1 rol rA2 brpl 4b 6: sbci rA3,0x80 ; undo the subi 0x7f rjmp _U(__fp_mpack) ENDFUNC