Return to fpu_cordic.c CVS log

Up to [Isaki's NoNo m68k/m88k emulator] / nono / fpe

Annotation of nono/fpe/fpu_cordic.c, revision 1.1

1.1     ! root        1: /*     $NetBSD: fpu_cordic.c,v 1.4 2016/12/06 05:58:19 isaki Exp $     */
        !             2: 
        !             3: /*
        !             4:  * Copyright (c) 2013 Tetsuya Isaki. All rights reserved.
        !             5:  *
        !             6:  * Redistribution and use in source and binary forms, with or without
        !             7:  * modification, are permitted provided that the following conditions
        !             8:  * are met:
        !             9:  * 1. Redistributions of source code must retain the above copyright
        !            10:  *    notice, this list of conditions and the following disclaimer.
        !            11:  * 2. Redistributions in binary form must reproduce the above copyright
        !            12:  *    notice, this list of conditions and the following disclaimer in the
        !            13:  *    documentation and/or other materials provided with the distribution.
        !            14:  *
        !            15:  * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
        !            16:  * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
        !            17:  * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
        !            18:  * IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT,
        !            19:  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
        !            20:  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
        !            21:  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
        !            22:  * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
        !            23:  * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
        !            24:  * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
        !            25:  * SUCH DAMAGE.
        !            26:  */
        !            27: 
        !            28: #include "fpu_emulate.h"
        !            29: 
        !            30: /*
        !            31:  * sfpn = shoftened fp number; the idea is from fpu_log.c but not the same.
        !            32:  * The most significant byte of sp_m0 is EXP (signed byte) and the rest
        !            33:  * of sp_m0 is fp_mant[0].
        !            34:  */
        !            35: struct sfpn {
        !            36:        uint32_t sp_m0;
        !            37:        uint32_t sp_m1;
        !            38:        uint32_t sp_m2;
        !            39: };
        !            40: 
        !            41: #if defined(CORDIC_BOOTSTRAP)
        !            42: /*
        !            43:  * 事前計算しておくテーブルを出力する。
        !            44:  * 出力されたテーブルを #else 側に手動で貼り付ける。
        !            45:  *
        !            46:  * % cc -c fpu_add.c
        !            47:  * % cc -c fpu_div.c
        !            48:  * % cc -c fpu_exp.c
        !            49:  * % cc -c fpu_explode.c
        !            50:  * % cc -c fpu_fmovecr.c
        !            51:  * % cc -c fpu_implode.c
        !            52:  * % cc -c fpu_log.c
        !            53:  * % cc -c fpu_mul.c
        !            54:  * % cc -c fpu_subr.c
        !            55:  * % cc -c fpu_cordic.c -DCORDIC_BOOTSTRAP
        !            56:  * % cc -o bootstrap_cordic *.o
        !            57:  * % ./bootstrap_cordic
        !            58:  */
        !            59: /*
        !            60:  * This is a bootstrap code to generate a pre-calculated tables such as
        !            61:  * atan_table[].  However, it's just for reference.
        !            62:  * If you want to run the bootstrap, you will define CORDIC_BOOTSTRAP
        !            63:  * and modify these files as a userland application.
        !            64:  */
        !            65: 
        !            66: #include <stdio.h>
        !            67: #include <stdlib.h>
        !            68: #include <string.h>
        !            69: #include <float.h>
        !            70: 
        !            71: static void prepare_cordic_const(struct fpemu *);
        !            72: static struct fpn *fpu_gain1_cordic(struct fpemu *);
        !            73: static struct fpn *fpu_atan_taylor(struct fpemu *);
        !            74: static void printf_fpn(const struct fpn *);
        !            75: static void printf_sfpn(const struct sfpn *);
        !            76: static void fpn_to_sfpn(struct sfpn *, const struct fpn *);
        !            77: 
        !            78: static struct sfpn atan_table[EXT_FRACBITS];
        !            79: static struct fpn inv_gain1;
        !            80: 
        !            81: int
        !            82: main(int argc, char *argv[])
        !            83: {
        !            84:        struct fpemu dummyfe;
        !            85:        int i;
        !            86:        struct fpn fp;
        !            87: 
        !            88:        memset(&dummyfe, 0, sizeof(dummyfe));
        !            89:        prepare_cordic_const(&dummyfe);
        !            90: 
        !            91:        /* output as source code */
        !            92:        printf("static const struct sfpn atan_table[] = {\n");
        !            93:        for (i = 0; i < EXT_FRACBITS; i++) {
        !            94:                printf("\t");
        !            95:                printf_sfpn(&atan_table[i]);
        !            96:                printf(",\n");
        !            97:        }
        !            98:        printf("};\n\n");
        !            99: 
        !           100:        printf("const struct fpn fpu_cordic_inv_gain1 =\n\t");
        !           101:        printf_fpn(&inv_gain1);
        !           102:        printf(";\n\n");
        !           103: }
        !           104: 
        !           105: /*
        !           106:  * This routine uses fpu_const(), fpu_add(), fpu_div(), fpu_logn()
        !           107:  * and fpu_atan_taylor() as bootstrap.
        !           108:  */
        !           109: static void
        !           110: prepare_cordic_const(struct fpemu *fe)
        !           111: {
        !           112:        struct fpn t;
        !           113:        struct fpn x;
        !           114:        struct fpn *r;
        !           115:        int i;
        !           116: 
        !           117:        /* atan_table */
        !           118:        fpu_const(&t, FPU_CONST_1);
        !           119:        for (i = 0; i < EXT_FRACBITS; i++) {
        !           120:                /* atan(t) */
        !           121:                CPYFPN(&fe->fe_f2, &t);
        !           122:                r = fpu_atan_taylor(fe);
        !           123:                fpn_to_sfpn(&atan_table[i], r);
        !           124: 
        !           125:                /* t /= 2 */
        !           126:                t.fp_exp--;
        !           127:        }
        !           128: 
        !           129:        /* inv_gain1 = 1 / gain1cordic() */
        !           130:        r = fpu_gain1_cordic(fe);
        !           131:        CPYFPN(&fe->fe_f2, r);
        !           132:        fpu_const(&fe->fe_f1, FPU_CONST_1);
        !           133:        r = fpu_div(fe);
        !           134:        CPYFPN(&inv_gain1, r);
        !           135: }
        !           136: 
        !           137: static struct fpn *
        !           138: fpu_gain1_cordic(struct fpemu *fe)
        !           139: {
        !           140:        struct fpn x;
        !           141:        struct fpn y;
        !           142:        struct fpn z;
        !           143:        struct fpn v;
        !           144: 
        !           145:        fpu_const(&x, FPU_CONST_1);
        !           146:        fpu_const(&y, FPU_CONST_0);
        !           147:        fpu_const(&z, FPU_CONST_0);
        !           148:        CPYFPN(&v, &x);
        !           149:        v.fp_sign = !v.fp_sign;
        !           150: 
        !           151:        fpu_cordit1(fe, &x, &y, &z, &v);
        !           152:        CPYFPN(&fe->fe_f2, &x);
        !           153:        return &fe->fe_f2;
        !           154: }
        !           155: 
        !           156: /*
        !           157:  * arctan(x) = pi/4 (for |x| = 1)
        !           158:  *
        !           159:  *                 x^3   x^5   x^7
        !           160:  * arctan(x) = x - --- + --- - --- + ...   (for |x| < 1)
        !           161:  *                  3     5     7
        !           162:  */
        !           163: static struct fpn *
        !           164: fpu_atan_taylor(struct fpemu *fe)
        !           165: {
        !           166:        struct fpn res;
        !           167:        struct fpn x2;
        !           168:        struct fpn s0;
        !           169:        struct fpn *s1;
        !           170:        struct fpn *r;
        !           171:        uint32_t k;
        !           172: 
        !           173:        /* arctan(1) is pi/4 */
        !           174:        if (fe->fe_f2.fp_exp == 0) {
        !           175:                fpu_const(&fe->fe_f2, FPU_CONST_PI);
        !           176:                fe->fe_f2.fp_exp -= 2;
        !           177:                return &fe->fe_f2;
        !           178:        }
        !           179: 
        !           180:        /* s0 := x */
        !           181:        CPYFPN(&s0, &fe->fe_f2);
        !           182: 
        !           183:        /* res := x */
        !           184:        CPYFPN(&res, &fe->fe_f2);
        !           185: 
        !           186:        /* x2 := x * x */
        !           187:        CPYFPN(&fe->fe_f1, &fe->fe_f2);
        !           188:        r = fpu_mul(fe);
        !           189:        CPYFPN(&x2, r);
        !           190: 
        !           191:        k = 3;
        !           192:        for (;;) {
        !           193:                /* s1 := -s0 * x2 */
        !           194:                CPYFPN(&fe->fe_f1, &s0);
        !           195:                CPYFPN(&fe->fe_f2, &x2);
        !           196:                s1 = fpu_mul(fe);
        !           197:                s1->fp_sign ^= 1;
        !           198:                CPYFPN(&fe->fe_f1, s1);
        !           199: 
        !           200:                /* s0 := s1 for next loop */
        !           201:                CPYFPN(&s0, s1);
        !           202: 
        !           203:                /* s1 := s1 / k */
        !           204:                fpu_explode(fe, &fe->fe_f2, FTYPE_LNG, &k);
        !           205:                s1 = fpu_div(fe);
        !           206: 
        !           207:                /* break if s1 is enough small */
        !           208:                if (ISZERO(s1))
        !           209:                        break;
        !           210:                if (res.fp_exp - s1->fp_exp >= FP_NMANT)
        !           211:                        break;
        !           212: 
        !           213:                /* res += s1 */
        !           214:                CPYFPN(&fe->fe_f2, s1);
        !           215:                CPYFPN(&fe->fe_f1, &res);
        !           216:                r = fpu_add(fe);
        !           217:                CPYFPN(&res, r);
        !           218: 
        !           219:                k += 2;
        !           220:        }
        !           221: 
        !           222:        CPYFPN(&fe->fe_f2, &res);
        !           223:        return &fe->fe_f2;
        !           224: }
        !           225: 
        !           226: static void
        !           227: printf_fpn(const struct fpn *fp)
        !           228: {
        !           229:        printf("{ %d, %d, %3d, %d, { 0x%08x, 0x%08x, 0x%08x, }, }",
        !           230:                fp->fp_class, fp->fp_sign, fp->fp_exp, fp->fp_sticky ? 1 : 0,
        !           231:                fp->fp_mant[0], fp->fp_mant[1], fp->fp_mant[2]);
        !           232: }
        !           233: 
        !           234: static void
        !           235: printf_sfpn(const struct sfpn *sp)
        !           236: {
        !           237:        printf("{ 0x%08x, 0x%08x, 0x%08x, }",
        !           238:                sp->sp_m0, sp->sp_m1, sp->sp_m2);
        !           239: }
        !           240: 
        !           241: static void
        !           242: fpn_to_sfpn(struct sfpn *sp, const struct fpn *fp)
        !           243: {
        !           244:        sp->sp_m0 = (fp->fp_exp << 24) | fp->fp_mant[0];
        !           245:        sp->sp_m1 = fp->fp_mant[1];
        !           246:        sp->sp_m2 = fp->fp_mant[2];
        !           247: }
        !           248: 
        !           249: #else /* CORDIC_BOOTSTRAP */
        !           250: 
        !           251: static const struct sfpn atan_table[] = {
        !           252:        { 0xff06487e, 0xd5110b46, 0x11a80000, },
        !           253:        { 0xfe076b19, 0xc1586ed3, 0xda2b7f0d, },
        !           254:        { 0xfd07d6dd, 0x7e4b2037, 0x58ab6e33, },
        !           255:        { 0xfc07f56e, 0xa6ab0bdb, 0x719644b5, },
        !           256:        { 0xfb07fd56, 0xedcb3f7a, 0x71b65937, },
        !           257:        { 0xfa07ff55, 0x6eea5d89, 0x2a13bce7, },
        !           258:        { 0xf907ffd5, 0x56eedca6, 0xaddf3c5f, },
        !           259:        { 0xf807fff5, 0x556eeea5, 0xcb403117, },
        !           260:        { 0xf707fffd, 0x5556eeed, 0xca5d8956, },
        !           261:        { 0xf607ffff, 0x55556eee, 0xea5ca6ab, },
        !           262:        { 0xf507ffff, 0xd55556ee, 0xeedca5c8, },
        !           263:        { 0xf407ffff, 0xf555556e, 0xeeeea5c8, },
        !           264:        { 0xf307ffff, 0xfd555556, 0xeeeeedc8, },
        !           265:        { 0xf207ffff, 0xff555555, 0x6eeeeee8, },
        !           266:        { 0xf107ffff, 0xffd55555, 0x56eeeeed, },
        !           267:        { 0xf007ffff, 0xfff55555, 0x556eeeed, },
        !           268:        { 0xef07ffff, 0xfffd5555, 0x5556eeed, },
        !           269:        { 0xee07ffff, 0xffff5555, 0x55556eed, },
        !           270:        { 0xed07ffff, 0xffffd555, 0x555556ed, },
        !           271:        { 0xec07ffff, 0xfffff555, 0x5555556d, },
        !           272:        { 0xeb07ffff, 0xfffffd55, 0x55555555, },
        !           273:        { 0xea07ffff, 0xffffff55, 0x55555554, },
        !           274:        { 0xe907ffff, 0xffffffd5, 0x55555554, },
        !           275:        { 0xe807ffff, 0xfffffff5, 0x55555554, },
        !           276:        { 0xe707ffff, 0xfffffffd, 0x55555554, },
        !           277:        { 0xe607ffff, 0xffffffff, 0x55555554, },
        !           278:        { 0xe507ffff, 0xffffffff, 0xd5555554, },
        !           279:        { 0xe407ffff, 0xffffffff, 0xf5555554, },
        !           280:        { 0xe307ffff, 0xffffffff, 0xfd555554, },
        !           281:        { 0xe207ffff, 0xffffffff, 0xff555554, },
        !           282:        { 0xe107ffff, 0xffffffff, 0xffd55554, },
        !           283:        { 0xe007ffff, 0xffffffff, 0xfff55554, },
        !           284:        { 0xdf07ffff, 0xffffffff, 0xfffd5554, },
        !           285:        { 0xde07ffff, 0xffffffff, 0xffff5554, },
        !           286:        { 0xdd07ffff, 0xffffffff, 0xffffd554, },
        !           287:        { 0xdc07ffff, 0xffffffff, 0xfffff554, },
        !           288:        { 0xdb07ffff, 0xffffffff, 0xfffffd54, },
        !           289:        { 0xda07ffff, 0xffffffff, 0xffffff54, },
        !           290:        { 0xd907ffff, 0xffffffff, 0xffffffd4, },
        !           291:        { 0xd807ffff, 0xffffffff, 0xfffffff4, },
        !           292:        { 0xd707ffff, 0xffffffff, 0xfffffffc, },
        !           293:        { 0xd7040000, 0x00000000, 0x00000000, },
        !           294:        { 0xd6040000, 0x00000000, 0x00000000, },
        !           295:        { 0xd5040000, 0x00000000, 0x00000000, },
        !           296:        { 0xd4040000, 0x00000000, 0x00000000, },
        !           297:        { 0xd3040000, 0x00000000, 0x00000000, },
        !           298:        { 0xd2040000, 0x00000000, 0x00000000, },
        !           299:        { 0xd1040000, 0x00000000, 0x00000000, },
        !           300:        { 0xd0040000, 0x00000000, 0x00000000, },
        !           301:        { 0xcf040000, 0x00000000, 0x00000000, },
        !           302:        { 0xce040000, 0x00000000, 0x00000000, },
        !           303:        { 0xcd040000, 0x00000000, 0x00000000, },
        !           304:        { 0xcc040000, 0x00000000, 0x00000000, },
        !           305:        { 0xcb040000, 0x00000000, 0x00000000, },
        !           306:        { 0xca040000, 0x00000000, 0x00000000, },
        !           307:        { 0xc9040000, 0x00000000, 0x00000000, },
        !           308:        { 0xc8040000, 0x00000000, 0x00000000, },
        !           309:        { 0xc7040000, 0x00000000, 0x00000000, },
        !           310:        { 0xc6040000, 0x00000000, 0x00000000, },
        !           311:        { 0xc5040000, 0x00000000, 0x00000000, },
        !           312:        { 0xc4040000, 0x00000000, 0x00000000, },
        !           313:        { 0xc3040000, 0x00000000, 0x00000000, },
        !           314:        { 0xc2040000, 0x00000000, 0x00000000, },
        !           315:        { 0xc1040000, 0x00000000, 0x00000000, },
        !           316: };
        !           317: 
        !           318: const struct fpn fpu_cordic_inv_gain1 =
        !           319:        { 1, 0,  -1, 1, { 0x0004dba7, 0x6d421af2, 0xd33fafd1, }, };
        !           320: 
        !           321: #endif /* CORDIC_BOOTSTRAP */
        !           322: 
        !           323: static inline void
        !           324: sfpn_to_fpn(struct fpn *fp, const struct sfpn *s)
        !           325: {
        !           326:        fp->fp_class = FPC_NUM;
        !           327:        fp->fp_sign = 0;
        !           328:        fp->fp_sticky = 0;
        !           329:        fp->fp_exp = s->sp_m0 >> 24;
        !           330:        if (fp->fp_exp & 0x80) {
        !           331:                fp->fp_exp |= 0xffffff00;
        !           332:        }
        !           333:        fp->fp_mant[0] = s->sp_m0 & 0x000fffff;
        !           334:        fp->fp_mant[1] = s->sp_m1;
        !           335:        fp->fp_mant[2] = s->sp_m2;
        !           336: }
        !           337: 
        !           338: void
        !           339: fpu_cordit1(struct fpemu *fe, struct fpn *x0, struct fpn *y0, struct fpn *z0,
        !           340:        const struct fpn *vecmode)
        !           341: {
        !           342:        struct fpn t;
        !           343:        struct fpn x;
        !           344:        struct fpn y;
        !           345:        struct fpn z;
        !           346:        struct fpn *r;
        !           347:        int i;
        !           348:        int sign;
        !           349: 
        !           350:        fpu_const(&t, FPU_CONST_1);
        !           351:        CPYFPN(&x, x0);
        !           352:        CPYFPN(&y, y0);
        !           353:        CPYFPN(&z, z0);
        !           354: 
        !           355:        for (i = 0; i < EXT_FRACBITS; i++) {
        !           356:                struct fpn x1;
        !           357: 
        !           358:                /* y < vecmode */
        !           359:                CPYFPN(&fe->fe_f1, &y);
        !           360:                CPYFPN(&fe->fe_f2, vecmode);
        !           361:                fe->fe_f2.fp_sign = !fe->fe_f2.fp_sign;
        !           362:                r = fpu_add(fe);
        !           363: 
        !           364:                if ((vecmode->fp_sign == 0 && r->fp_sign) ||
        !           365:                    (vecmode->fp_sign && z.fp_sign == 0)) {
        !           366:                        sign = 1;
        !           367:                } else {
        !           368:                        sign = 0;
        !           369:                }
        !           370: 
        !           371:                /* y * t */
        !           372:                CPYFPN(&fe->fe_f1, &y);
        !           373:                CPYFPN(&fe->fe_f2, &t);
        !           374:                r = fpu_mul(fe);
        !           375: 
        !           376:                /*
        !           377:                 * x1 = x - y*t (if sign)
        !           378:                 * x1 = x + y*t
        !           379:                 */
        !           380:                CPYFPN(&fe->fe_f2, r);
        !           381:                if (sign)
        !           382:                        fe->fe_f2.fp_sign = !fe->fe_f2.fp_sign;
        !           383:                CPYFPN(&fe->fe_f1, &x);
        !           384:                r = fpu_add(fe);
        !           385:                CPYFPN(&x1, r);
        !           386: 
        !           387:                /* x * t */
        !           388:                CPYFPN(&fe->fe_f1, &x);
        !           389:                CPYFPN(&fe->fe_f2, &t);
        !           390:                r = fpu_mul(fe);
        !           391: 
        !           392:                /*
        !           393:                 * y = y + x*t (if sign)
        !           394:                 * y = y - x*t
        !           395:                 */
        !           396:                CPYFPN(&fe->fe_f2, r);
        !           397:                if (!sign)
        !           398:                        fe->fe_f2.fp_sign = !fe->fe_f2.fp_sign;
        !           399:                CPYFPN(&fe->fe_f1, &y);
        !           400:                r = fpu_add(fe);
        !           401:                CPYFPN(&y, r);
        !           402: 
        !           403:                /*
        !           404:                 * z = z - atan_table[i] (if sign)
        !           405:                 * z = z + atan_table[i]
        !           406:                 */
        !           407:                CPYFPN(&fe->fe_f1, &z);
        !           408:                sfpn_to_fpn(&fe->fe_f2, &atan_table[i]);
        !           409:                if (sign)
        !           410:                        fe->fe_f2.fp_sign = !fe->fe_f2.fp_sign;
        !           411:                r = fpu_add(fe);
        !           412:                CPYFPN(&z, r);
        !           413: 
        !           414:                /* x = x1 */
        !           415:                CPYFPN(&x, &x1);
        !           416: 
        !           417:                /* t /= 2 */
        !           418:                t.fp_exp--;
        !           419:        }
        !           420: 
        !           421:        CPYFPN(x0, &x);
        !           422:        CPYFPN(y0, &y);
        !           423:        CPYFPN(z0, &z);
        !           424: }

This archive runs on limited infrastructure. Preserving old code on modern bandwidth. Automated agents are requested to crawl responsibly.