1.1 --- /dev/null	Thu Jan 01 00:00:00 1970 +0000
     1.2 +++ b/src/fixed.c	Sat Jun 14 03:04:56 2014 +0300
     1.3 @@ -0,0 +1,101 @@
     1.4 +#include "fixed.h"
     1.5 +
     1.6 +#if 0
     1.7 +/* fast 16.16 division ripped from http://me.henri.net/fp-div.html */
     1.8 +int32_t x16div(register int32_t numerator, register int32_t denominator)
     1.9 +{
    1.10 +    register int32_t quotient;
    1.11 +    asm("num     .req %[numerator]      @ Map Register Equates\n\t"
    1.12 +        "den     .req %[denominator]\n\t"
    1.13 +        "mod     .req r2\n\t"
    1.14 +        "cnt     .req r3\n\t"
    1.15 +        "quo     .req r4\n\t"
    1.16 +        "sign    .req r12\n\t"
    1.17 +        /* set sign and ensure numerator and denominator are positive */
    1.18 +        "cmp den, #0                    @ exceptioin if den == zero\n\t"
    1.19 +        "beq .div0\n\t"
    1.20 +        "eor sign, num, den             @ sign = num ^ den\n\t"
    1.21 +        "rsbmi den, den, #0             @ den = -den if den < 0\n\t"
    1.22 +        "subs mod, den, #1              @ mod = den - 1\n\t"
    1.23 +        "beq .div1                      @ return if den == 1\n\t"
    1.24 +        "movs cnt, num                  @ num = -num if num < 0\n\t"
    1.25 +        "rsbmi num, num, #0\n\t"
    1.26 +        /* skip if deniminator >= numerator */
    1.27 +        "movs cnt, num, lsr #16         @ return if den >= num << 16\n\t"
    1.28 +        "bne .cont\n\t"
    1.29 +        "cmp den, num, lsl #16\n\t"
    1.30 +        "bhs .numLeDen\n\t"
    1.31 +    "\n.cont:\n\t"
    1.32 +        /* test if denominator is a power of two */
    1.33 +        "tst den, mod                   @ if(den & (den - 1) == 0)\n\t"
    1.34 +        "beq .powerOf2                  @ den is power of 2\n\t"
    1.35 +        /* count leading zeros */
    1.36 +        "stmfd sp!, {r4}                @ push r4 (quo) onto the stack\n\t"
    1.37 +        "mov cnt, #28                   @ count difference in leading zeros\n\t"
    1.38 +        "mov mod, num, lsr #4           @ between num and den\n\t"
    1.39 +        "cmp den, mod, lsr #12; subls cnt, cnt, #16; movls mod, mod, lsr #16\n\t"
    1.40 +        "cmp den, mod, lsr #4 ; subls cnt, cnt, #8 ; movls mod, mod, lsr #8\n\t"
    1.41 +        "cmp den, mod         ; subls cnt, cnt, #4 ; movls mod, mod, lsr #4\n\t"
    1.42 +        /* shift numerator left by cnt bits */
    1.43 +        "mov num, num, lsl cnt          @ mod:num = num << cnt\n\t"
    1.44 +        "mov quo, #0\n\t"
    1.45 +        "rsb den, den, #0               @ negate den for divide loop\n\t"
    1.46 +        /* skip cnt iterations in the divide loop */
    1.47 +        "adds num, num, num             @ start: num = mod:num / den\n\t"
    1.48 +        "add pc, pc, cnt, lsl #4        @ skip cnt x 4 x 4 iterations\n\t"
    1.49 +        "nop                            @ nop instruction takes care of pipeline\n\t"
    1.50 +        /* inner loop unrolled x 48 */
    1.51 +        ".rept 47                       @ inner loop x 48\n\t"
    1.52 +        "    adcs mod, den, mod, lsl #1\n\t"
    1.53 +        "    subcc mod, mod, den\n\t"
    1.54 +        "    adc quo, quo, quo\n\t"
    1.55 +        "    adds num, num, num\n\t"
    1.56 +        ".endr\n\t"
    1.57 +        "adcs mod, den, mod, lsl #1\n\t"
    1.58 +        "subcc mod, mod, den\n\t"
    1.59 +        "adc quo, quo, quo\n\t"
    1.60 +        /* negate quotient if signed */
    1.61 +        "cmp sign, #0                   @ negate quotient if sign < 0\n\t"
    1.62 +        "mov num, quo\n\t"
    1.63 +        "rsbmi num, num, #0\n\t"
    1.64 +        "ldmfd sp!, {r4}                @ pop r4 (quo) off the stack\n\t"
    1.65 +        "mov pc, lr                     @return\n\t"
    1.66 +        /* divide by zero handler */
    1.67 +    "\n.div0:\n\t"
    1.68 +        "mov num, #0\n\t"
    1.69 +        "mov pc, lr                     @return\n\t"
    1.70 +        /* divide by one handler */
    1.71 +    "\n.div1:\n\t"
    1.72 +        "cmp sign, #0\n\t"
    1.73 +        "mov num, num, asl #16\n\t"
    1.74 +        "rsbmi num, num, #0\n\t"
    1.75 +        "mov pc, lr                     @return\n\t"
    1.76 +        /* numerator less than or equal to denominator handler */
    1.77 +    "\n.numLeDen:\n\t"
    1.78 +        "mov num, #0                    @ quotient = 0 if num < den\n\t"
    1.79 +        "moveq num, sign, asr #31       @ negate quotient if sign < 0\n\t"
    1.80 +        "orreq num, num, #1             @ quotient = 1 if num == den\n\t"
    1.81 +        "mov pc, lr                     @return\n\t"
    1.82 +        /* power of two handler */
    1.83 +    "\n.powerOf2:\n\t"
    1.84 +        "mov cnt, #0\n\t"
    1.85 +        "cmp den, #(1 << 16); movhs cnt, #16    ; movhs den, den, lsr #16\n\t"
    1.86 +        "cmp den, #(1 << 8) ; addhs cnt, cnt, #8; movhs den, den, lsr #8\n\t"
    1.87 +        "cmp den, #(1 << 4) ; addhs cnt, cnt, #4; movhs den, den, lsr #4\n\t"
    1.88 +        "cmp den, #(1 << 2) ; addhi cnt, cnt, #3; addls cnt, cnt, den, lsr #1\n\t"
    1.89 +        "rsb mod, cnt, #32\n\t"
    1.90 +        "mov den, num, lsr #16          @ den:num = num << 16\n\t"
    1.91 +        "mov num, num, lsl #16\n\t"
    1.92 +        "mov num, num, lsr cnt          @ num = num >> cnt | den << mod\n\t"
    1.93 +        "orr num, num, den, lsl mod\n\t"
    1.94 +        "cmp sign, #0\n\t"
    1.95 +        "rsbmi num, num, #0             @ negate quotient if sign < 0"
    1.96 +        /* output registers */
    1.97 +        : [quotient] "=r" (quotient)
    1.98 +        /* input registers */
    1.99 +        : [numerator] "0" (numerator), [denominator] "r" (denominator)
   1.100 +        /* clobbered registers */
   1.101 +        : "r2" /* mod */, "r3" /* cnt */, "r12" /* sign */);
   1.102 +    return quotient;
   1.103 +}
   1.104 +#endif