nuclear@4: #include "fixed.h"
nuclear@4: 
nuclear@4: #if 0
nuclear@4: /* fast 16.16 division ripped from http://me.henri.net/fp-div.html */
nuclear@4: int32_t x16div(register int32_t numerator, register int32_t denominator)
nuclear@4: {
nuclear@4:     register int32_t quotient;
nuclear@4:     asm("num     .req %[numerator]      @ Map Register Equates\n\t"
nuclear@4:         "den     .req %[denominator]\n\t"
nuclear@4:         "mod     .req r2\n\t"
nuclear@4:         "cnt     .req r3\n\t"
nuclear@4:         "quo     .req r4\n\t"
nuclear@4:         "sign    .req r12\n\t"
nuclear@4:         /* set sign and ensure numerator and denominator are positive */
nuclear@4:         "cmp den, #0                    @ exceptioin if den == zero\n\t"
nuclear@4:         "beq .div0\n\t"
nuclear@4:         "eor sign, num, den             @ sign = num ^ den\n\t"
nuclear@4:         "rsbmi den, den, #0             @ den = -den if den < 0\n\t"
nuclear@4:         "subs mod, den, #1              @ mod = den - 1\n\t"
nuclear@4:         "beq .div1                      @ return if den == 1\n\t"
nuclear@4:         "movs cnt, num                  @ num = -num if num < 0\n\t"
nuclear@4:         "rsbmi num, num, #0\n\t"
nuclear@4:         /* skip if deniminator >= numerator */
nuclear@4:         "movs cnt, num, lsr #16         @ return if den >= num << 16\n\t"
nuclear@4:         "bne .cont\n\t"
nuclear@4:         "cmp den, num, lsl #16\n\t"
nuclear@4:         "bhs .numLeDen\n\t"
nuclear@4:     "\n.cont:\n\t"
nuclear@4:         /* test if denominator is a power of two */
nuclear@4:         "tst den, mod                   @ if(den & (den - 1) == 0)\n\t"
nuclear@4:         "beq .powerOf2                  @ den is power of 2\n\t"
nuclear@4:         /* count leading zeros */
nuclear@4:         "stmfd sp!, {r4}                @ push r4 (quo) onto the stack\n\t"
nuclear@4:         "mov cnt, #28                   @ count difference in leading zeros\n\t"
nuclear@4:         "mov mod, num, lsr #4           @ between num and den\n\t"
nuclear@4:         "cmp den, mod, lsr #12; subls cnt, cnt, #16; movls mod, mod, lsr #16\n\t"
nuclear@4:         "cmp den, mod, lsr #4 ; subls cnt, cnt, #8 ; movls mod, mod, lsr #8\n\t"
nuclear@4:         "cmp den, mod         ; subls cnt, cnt, #4 ; movls mod, mod, lsr #4\n\t"
nuclear@4:         /* shift numerator left by cnt bits */
nuclear@4:         "mov num, num, lsl cnt          @ mod:num = num << cnt\n\t"
nuclear@4:         "mov quo, #0\n\t"
nuclear@4:         "rsb den, den, #0               @ negate den for divide loop\n\t"
nuclear@4:         /* skip cnt iterations in the divide loop */
nuclear@4:         "adds num, num, num             @ start: num = mod:num / den\n\t"
nuclear@4:         "add pc, pc, cnt, lsl #4        @ skip cnt x 4 x 4 iterations\n\t"
nuclear@4:         "nop                            @ nop instruction takes care of pipeline\n\t"
nuclear@4:         /* inner loop unrolled x 48 */
nuclear@4:         ".rept 47                       @ inner loop x 48\n\t"
nuclear@4:         "    adcs mod, den, mod, lsl #1\n\t"
nuclear@4:         "    subcc mod, mod, den\n\t"
nuclear@4:         "    adc quo, quo, quo\n\t"
nuclear@4:         "    adds num, num, num\n\t"
nuclear@4:         ".endr\n\t"
nuclear@4:         "adcs mod, den, mod, lsl #1\n\t"
nuclear@4:         "subcc mod, mod, den\n\t"
nuclear@4:         "adc quo, quo, quo\n\t"
nuclear@4:         /* negate quotient if signed */
nuclear@4:         "cmp sign, #0                   @ negate quotient if sign < 0\n\t"
nuclear@4:         "mov num, quo\n\t"
nuclear@4:         "rsbmi num, num, #0\n\t"
nuclear@4:         "ldmfd sp!, {r4}                @ pop r4 (quo) off the stack\n\t"
nuclear@4:         "mov pc, lr                     @return\n\t"
nuclear@4:         /* divide by zero handler */
nuclear@4:     "\n.div0:\n\t"
nuclear@4:         "mov num, #0\n\t"
nuclear@4:         "mov pc, lr                     @return\n\t"
nuclear@4:         /* divide by one handler */
nuclear@4:     "\n.div1:\n\t"
nuclear@4:         "cmp sign, #0\n\t"
nuclear@4:         "mov num, num, asl #16\n\t"
nuclear@4:         "rsbmi num, num, #0\n\t"
nuclear@4:         "mov pc, lr                     @return\n\t"
nuclear@4:         /* numerator less than or equal to denominator handler */
nuclear@4:     "\n.numLeDen:\n\t"
nuclear@4:         "mov num, #0                    @ quotient = 0 if num < den\n\t"
nuclear@4:         "moveq num, sign, asr #31       @ negate quotient if sign < 0\n\t"
nuclear@4:         "orreq num, num, #1             @ quotient = 1 if num == den\n\t"
nuclear@4:         "mov pc, lr                     @return\n\t"
nuclear@4:         /* power of two handler */
nuclear@4:     "\n.powerOf2:\n\t"
nuclear@4:         "mov cnt, #0\n\t"
nuclear@4:         "cmp den, #(1 << 16); movhs cnt, #16    ; movhs den, den, lsr #16\n\t"
nuclear@4:         "cmp den, #(1 << 8) ; addhs cnt, cnt, #8; movhs den, den, lsr #8\n\t"
nuclear@4:         "cmp den, #(1 << 4) ; addhs cnt, cnt, #4; movhs den, den, lsr #4\n\t"
nuclear@4:         "cmp den, #(1 << 2) ; addhi cnt, cnt, #3; addls cnt, cnt, den, lsr #1\n\t"
nuclear@4:         "rsb mod, cnt, #32\n\t"
nuclear@4:         "mov den, num, lsr #16          @ den:num = num << 16\n\t"
nuclear@4:         "mov num, num, lsl #16\n\t"
nuclear@4:         "mov num, num, lsr cnt          @ num = num >> cnt | den << mod\n\t"
nuclear@4:         "orr num, num, den, lsl mod\n\t"
nuclear@4:         "cmp sign, #0\n\t"
nuclear@4:         "rsbmi num, num, #0             @ negate quotient if sign < 0"
nuclear@4:         /* output registers */
nuclear@4:         : [quotient] "=r" (quotient)
nuclear@4:         /* input registers */
nuclear@4:         : [numerator] "0" (numerator), [denominator] "r" (denominator)
nuclear@4:         /* clobbered registers */
nuclear@4:         : "r2" /* mod */, "r3" /* cnt */, "r12" /* sign */);
nuclear@4:     return quotient;
nuclear@4: }
nuclear@4: #endif