开通博客赚积分
发布资源赚积分
分类

源码开发语言/平台
当前位置：首页 > 源码/资料 > 嵌入式/单片机编程 > VxWorks > 查看源码
tx79Lib.c
资源名称：sourceVxworks_551.rar　[点击查看]
上传用户：smqsbj
上传日期：2019-04-24
资源大小：9972k
文件大小：14k
源码类别：

VxWorks

开发平台：

C/C++

tx79Lib.c：源码内容
  1. /* tx79Lib.c - Toshiba Tx79 support library */
  3. /* Copyright 1984-2002 Wind River Systems, Inc. */
  4. #include "copyright_wrs.h"
  6. /*
  7.  * This file has been developed or significantly modified by the
  8.  * MIPS Center of Excellence Dedicated Engineering Staff.
  9.  * This notice is as per the MIPS Center of Excellence Master Partner
  10.  * Agreement, do not remove this notice without checking first with
  11.  * WR/Platforms MIPS Center of Excellence engineering management.
  12.  */
  14. /*
  15. modification history
  16. --------------------
  17. 01c,15nov02,jmt  Add Interrupt Exception Handler
  18. 01b,03oct02,jmt  Modify to resolve code review issues
  19. 01a,04sep02,jmt  written.
  20. */
  22. /*
  23. DESCRIPTION
  24. This library provides a low-level support for the Toshiba Tx79 processor.
  26. INITIALIZATION
  28. SEE ALSO:
  30. */
  32. #include "vxWorks.h"
  33. #include "iv.h"
  34. #include "intLib.h"
  35. #include "esf.h"
  36. #include "arch/mips/dsmMipsLib.h"
  38. /* imports */
  39. extern void excNormVecInit(void *vecAdrs);
  41. #if (CPU == MIPS64)
  42. extern void excExcHandle(int vecNum, ESFMIPS * pEsf, REG_SET * pRegs);
  43. extern STATUS fppEmulateBranch (ESFMIPS *pEsf, uint32_t brInstr,
  44. uint32_t fpcsr);
  45. extern void tx79AMult64(uint64_t j, uint64_t k, uint64_t* pResLow,
  46. uint64_t* pResHigh);
  47. #endif /* (CPU == MIPS64) */
  49. /* globals */
  51. /* The define below this comment will enable the use of the C language
  52.  * version of the 64 bit mult (tx79Mult64) instead of the Asm version.
  53.  * They both do the same job, the assembly is faster.
  54.  */
  55. /* #define USE_C_MULT64_ROUTINE */
  57. /* Instruction Encodings for Unsupported Instructions
  58.  *
  59.  * Note: Mask only allows for two operand instructions (per MIPS spec)
  60.  */
  62. #define INSTR_MASK         (0xFC00FFFF)
  63. #define DDIV_INSTR         (0x0000001E)
  64. #define DDIVU_INSTR        (0x0000001F)
  65. #define DMULT_INSTR        (0x0000001C)
  66. #define DMULTU_INSTR       (0x0000001D)
  67. #if 0
  68. /* for testing on tx49 */
  69. #define DDIV_INSTR         (0x00000001)
  70. #define DDIVU_INSTR        (0x00000005)
  71. #define DMULT_INSTR        (0x0000000a)
  72. #define DMULTU_INSTR       (0x0000000b)
  73. #endif
  75. /* Interrupt Exception Definition */
  77. #define TX79_INT_VEC       (K0BASE+0x200)          /* interrupt vector */
  79. #if (CPU == MIPS64)
  80. /* Protect from initializing more than once */
  82. uint8_t tx79LibInitialized = FALSE;
  84. /* storage for old Reserved Inst Handler */
  86. typedef void (*resIntHandlerType)(int vecNum, ESFMIPS * pEsf, REG_SET * pRegs);
  88. resIntHandlerType oldResInstHandler;
  89. #endif  /* (CPU == MIPS64) */
  91. /* forward declarations */
  93. #if (CPU == MIPS64)
  94. void tx79ResvdInstHandler (int vectorNum, ESFMIPS *pEsf, REG_SET *pRegs);
  95. void tx79ddiv(uint64_t j, uint64_t k, uint64_t* pRes, uint64_t* pRem);
  96. void tx79ddivu(uint64_t j, uint64_t k, uint64_t* pRes, uint64_t* pRem);
  97. void tx79dmult(uint64_t j, uint64_t k, uint64_t* pResLow, uint64_t* pResHigh);
  98. void tx79dmultu(uint64_t j, uint64_t k, uint64_t* pResLow, uint64_t* pResHigh);
  99. #endif /* (CPU == MIPS64) */
  101. /******************************************************************************
  102. *
  103. * tx79ArchInit - initialize tx79 support.
  104. *
  105. * This routine must be called before using unsupported 64 bit mult/div
  106. * instructions.
  107. *
  108. * NOMANUAL
  109. */
  111. void tx79ArchInit (void)
  112.     {
  113.     /* initialize Interrupt Exception Handler */
  115.     excNormVecInit((void *) TX79_INT_VEC);
  117. #if (CPU == MIPS64)
  119.     /* only initialize once */
  120.     
  121.     if (tx79LibInitialized == FALSE)
  122. {
  123. tx79LibInitialized = TRUE;
  125. /* save old reserved instruction handler */
  126.     
  127. oldResInstHandler = (resIntHandlerType)
  128.   intVecGet((FUNCPTR *) INUM_TO_IVEC(IV_RESVDINST_VEC));
  129.     
  130. /* Register Reserved Instruction Handler */
  131.     
  132. intVecSet((FUNCPTR *) INUM_TO_IVEC(IV_RESVDINST_VEC),
  133.   (FUNCPTR) tx79ResvdInstHandler);
  134. }
  135. #endif /* (CPU == MIPS64) */
  136.     }
  138. #if (CPU == MIPS64)
  140. /* Tx79 does not support 64-bit mult/div insns */
  142. /******************************************************************************
  143. *
  144. * tx79ResvdInstHandler - Reserved Instruction Handler for the TX79
  145. *
  146. * This routine is called when a Reserved Instruction exception is raised
  147. * in the TX79.  If the instruction is an unsupported 64 bit mult/div
  148. * instruction, we will simulate the instruction and return.  Otherwise,
  149. * we will raise an exception by calling excExcHandle.
  150. *
  151. * NOMANUAL
  152. */
  154. void tx79ResvdInstHandler
  155.     (
  156.     int vectorNum,
  157.     ESFMIPS *pEsf,
  158.     REG_SET *pRegs
  159.     )
  160.     {
  161.     INSTR instruction;
  162.     INSTR *pNextInstr;
  163.     STATUS status;
  164.     REGISTERTYPE *pInstrDecode;
  165.     uint64_t rs;
  166.     uint64_t rt;
  167.     uint64_t resultHigh;
  168.     uint64_t resultLow;
  169.     
  170.     /* Determine the PC for the instruction that caused the exception */
  172.     instruction = *pRegs->pc;
  174.     /* calculate return address */
  175.     /* Check to see if the instruction was in a Branch Delay slot */
  177.     if ((pEsf->cause & CAUSE_BD) != 0)
  178. {
  179. /* Instruction is in the branch delay slot, simulate the branch */
  181. pNextInstr =
  182.   (INSTR *) fppEmulateBranch(pEsf, instruction, pEsf->fpcsr);
  183. if ((int) pNextInstr == 1)
  184.     {
  185.     /* branch emulation failed, raise exception */
  187.     if (oldResInstHandler != NULL)
  188. {
  189. oldResInstHandler(vectorNum, pEsf, pRegs);
  190. }
  191.     else
  192. {
  193. excExcHandle(vectorNum, pEsf, pRegs);
  194. }
  195.     }
  196. }
  197.     else
  198. {
  199. pNextInstr = pRegs->pc + 1;
  200. }
  202.     /* decode instruction */
  203.     status = OK;
  204.     switch (instruction & INSTR_MASK)
  205. {
  206. case DMULT_INSTR:
  207. case DMULTU_INSTR:
  208. case DDIV_INSTR:
  209. case DDIVU_INSTR:
  210.     /* decode the instruction */
  212.     pInstrDecode = (REGISTERTYPE *) &instruction;
  213.     rs = pRegs->gpreg[pInstrDecode->rs];
  214.     rt = pRegs->gpreg[pInstrDecode->rt];
  216.     /* emulate instruction */
  217.     
  218.     switch (instruction & INSTR_MASK)
  219. {
  220. case DMULT_INSTR:
  221.     tx79dmult(rs, rt, &resultLow, &resultHigh);
  222.     break;
  223. case DMULTU_INSTR:
  224.     tx79dmultu(rs, rt, &resultLow, &resultHigh);
  225.     break;
  226. case DDIV_INSTR:
  227.     tx79ddiv(rs, rt, &resultLow, &resultHigh);
  228.     break;
  229. case DDIVU_INSTR:
  230.     tx79ddivu(rs, rt, &resultLow, &resultHigh);
  231.     break;
  232. }
  234.     /* save results in hi and lo */
  235.     pRegs->hi = resultHigh;
  236.     pRegs->lo = resultLow;
  237.     
  238.     break;
  239.     
  240. default:
  241.     status = ERROR;
  242.     break;
  243. }
  245.     /* if the instruction was successfully emulated, return to the
  246.      * instruction after the emulated instruction
  247.      */
  249.     if (status == OK)
  250. {
  251. /* update the epc to point to the next instruction */
  253. pRegs->pc = pNextInstr;
  254. }
  255.     else
  256. {
  257. /* raise exception */
  259. excExcHandle(vectorNum, pEsf, pRegs);
  260. }
  261.     }
  264. /******************************************************************************
  265. *
  266. * tx79Div64 - perform a 64 bit unsigned divide
  267. *
  268. * This routine does a 64 bit unsigned divide and returns the result and
  269. * remainder.  Do not call this routine directly, call either tx79ddiv or
  270. * tx79ddivu.
  271. *
  272. * NOMANUAL
  273. */
  275. static void tx79Div64(uint64_t j,
  276.       uint64_t k,
  277.       uint64_t *pRes,
  278.       uint64_t *pRem)
  279.     {
  280.     int64_t i, m;
  281.     uint64_t n;
  282.     uint32_t c;
  284.     if (((k-1) & k) == 0)
  285. {
  286. /* Divide by power of 2. */
  288. *pRem = j & (k-1);
  290. /* shift value until k = 1 */
  292. n = j;
  293. if((k & 0x00000000ffffffffll) == 0)
  294.     {
  295.     k >>= 32;
  296.     n >>= 32;
  297.     }
  298. c = k;
  299. if((c & 0xffff) == 0)
  300.     {
  301.     c >>= 16;
  302.     n >>= 16;
  303.     }
  304. if((c & 0xff) == 0)
  305.     {
  306.     c >>= 8;
  307.     n >>= 8;
  308.     }
  309. while (c >>= 1)
  310.     {
  311.     n >>= 1;
  312.     }
  313. }
  314.     else if(((m = j | k) & 0xffffffff00000000ll) == 0)
  315. {
  316. /* each value fits in 32 bits, use long word arithmetic */
  318. n = (uint32_t)j/(uint32_t)k;
  319. *pRem = (uint32_t)j - ((uint32_t)n * (uint32_t)k);
  320. }
  321.     else
  322. {
  323. /* not a divide by power of 2 and more than 32 bits */
  325. /* set mask to uppermost byte that is non-zero */
  327. i = 0xff00000000000000ll;
  328. if ((m & i) == 0)
  329.     {
  330.     i >>= 8;
  331.     if ((m & i) == 0)
  332. {
  333. i >>= 8;
  334. if ((m & i) == 0)
  335.     {
  336.     i >>= 8;
  337.     }
  338. }
  339.     }
  341. /* shift the denominator to left until it is greater than
  342.  * the numerator.
  343.  */
  345. c = 0;            /* number of bits the denominator is shifted */
  346. while ((k & i) == 0)
  347.     {
  348.     k <<= 8;
  349.     c += 8;
  350.     }
  352. /* shift single bits until j < k */
  354. if(j & 0x8000000000000000ll)
  355.     {
  356.     while ((k & 0x8000000000000000ll) == 0)
  357. {
  358. k <<= 1;
  359. c++;
  360. }
  361.     n = 0;
  362.     if (j >= k)
  363. {
  364. j -= k;
  365. n |= 1;
  366. }
  367.     }
  368. else
  369.     {
  370.     while (k <= j)
  371. {
  372. k <<= 1;
  373. c++;
  374. }
  375.     n = 0;
  376.     }
  378. /* subtract the shifted denominator from numerator until the result
  379.  * bits are full
  380.  */
  382. while (c-- > 0)
  383.     {
  384.     k >>= 1;
  385.     n <<= 1;
  386.     if (j >= k)
  387. {
  388. j -= k;
  389. n |= 1;
  390. }
  391.     }
  393. /* save the remainder */
  395. *pRem = j;
  396. }
  398.     /* result is stored in n */
  399.     
  400.     *pRes = n;
  401.     }
  403. #ifdef USE_C_MULT64_ROUTINE
  404. /******************************************************************************
  405. *
  406. * tx79Mult64 - perform a 64 bit unsigned multiply
  407. *
  408. * This routine does a 64 bit unsigned multiply and returns the result as
  409. * two 64 bit unsigned integers.  Do not call this routine directly, call
  410. * either tx79dmult or tx79dmultu.
  411. *
  412. * The C compiler will truncate all results to 32 bits.  For this reason,
  413. * 16 bit multiplies must be used.  The 16 bit values will be stored in
  414. * 32 bit integers so that the 32 bit result will not be truncated.
  415. *
  416. * NOMANUAL
  417. */
  419. static void tx79Mult64(uint64_t j,
  420.        uint64_t k,
  421.        uint64_t *pResLow,
  422.        uint64_t *pResHigh)
  423.     {
  424.     uint32_t j0, j1, j2, j3;
  425.     uint32_t k0, k1, k2, k3;
  426.     uint64_t res0, res1, res2;
  428.     /* split up the incoming values */
  430.     j0 = (uint32_t) (j & 0xffff);
  431.     j1 = (uint32_t) ((j >> 16) & 0xffff);
  432.     j2 = (uint32_t) ((j >> 32) & 0xffff);
  433.     j3 = (uint32_t) ((j >> 48) & 0xffff);
  434.     k0 = (uint32_t) (k & 0xffff);
  435.     k1 = (uint32_t) ((k >> 16) & 0xffff);
  436.     k2 = (uint32_t) ((k >> 32) & 0xffff);
  437.     k3 = (uint32_t) ((k >> 48) & 0xffff);
  439.     /* multiply the pieces
  440.      * To keep the resN variables from overflowing while summing,
  441.      * the values added to resN will never be greater than 48 bits.
  442.      * Except for the j3 * k3 product.  If this overflows, the
  443.      * result will overflow.
  444.      */
  446.     /* res0 stores the lower 32 bits of the result */
  447.     
  448.     res0 = (uint64_t) (j0 * k0);
  449.     res0 += (uint64_t) (j0 * k1) << 16;
  450.     res0 += (uint64_t) (j1 * k0) << 16;
  452.     /* res1 stores the second 32 bits of the result
  453.      * it starts out with the overflow from res0
  454.      */
  456.     res1 = (res0 >> 32);
  457.     res1 += (uint64_t) (j1 * k1);
  458.     res1 += (uint64_t) (j0 * k2);
  459.     res1 += (uint64_t) (j2 * k0);
  460.     res1 += (uint64_t) (j1 * k2) << 16;
  461.     res1 += (uint64_t) (j2 * k1) << 16;
  462.     res1 += (uint64_t) (j0 * k3) << 16;
  463.     res1 += (uint64_t) (j3 * k0) << 16;
  465.     /* res2 stores the upper 64 bits of the result
  466.      * it starts out with the overflow from res1
  467.      */
  469.     res2 = (res1 >> 32);
  470.     res2 += (uint64_t) (j2 * k2);
  471.     res2 += (uint64_t) (j1 * k3);
  472.     res2 += (uint64_t) (j3 * k1);
  473.     res2 += (uint64_t) (j2 * k3) << 16;
  474.     res2 += (uint64_t) (j3 * k2) << 16;
  475.     res2 += (uint64_t) (j3 * k3) << 32;
  477.     /* combine and save the results */
  478.     
  479.     *pResLow = (res0 & 0xffffffff) + ((res1 & 0xffffffff) << 32);
  480.     *pResHigh = res2;
  481.     
  482.     }
  483. #endif /* ifdef USE_C_MULT64_ROUTINE */
  485. /******************************************************************************
  486. *
  487. * tx79ddiv - perform a 64 bit signed division
  488. *
  489. * This routine does a 64 bit signed division and returns the result as
  490. * a 64 bit signed result and a 64 bit signed remainder.
  491. *
  492. * NOMANUAL
  493. */
  495. void tx79ddiv(uint64_t j,
  496.       uint64_t k,
  497.       uint64_t* pRes,
  498.       uint64_t* pRem)
  499.     {
  500.     int s;  /* sign of result */
  501.     int ns; /* sign of remainder */
  502.     
  503.     /* make values positive and determine sign of result */
  504.     
  505.     s = 0;
  506.     ns = 0;
  507.     if ((int64_t)j < 0)
  508. {
  509. j = -j;
  510. s ^= 1;
  511. ns = 1;
  512. }
  513.     if ((int64_t)k <= 0)
  514. {
  515. /* handle divide by zero case */
  517. if ((int64_t)k == 0)
  518.     {
  519.     *pRes = s ? 0x8000000000000000ull : 0x7fffffffffffffffull;
  520.     *pRem = 0;
  521.     return;
  522.     }
  523. k = -k;
  524. s ^= 1;
  525. }
  527.     /* use unsigned div routine */
  528.     
  529.     tx79Div64(j, k, pRes, pRem);
  531.     /* modify result back to correct sign */
  533.     *pRes = s ? -(*pRes) : *pRes;
  534.     *pRem = ns ? -(*pRem) : *pRem;
  535. }
  537. /******************************************************************************
  538. *
  539. * tx79ddivu - perform a 64 bit unsigned division
  540. *
  541. * This routine does a 64 bit unsigned division and returns the result as
  542. * a 64 bit unsigned result and a 64 bit unsigned remainder.
  543. *
  544. * NOMANUAL
  545. */
  547. void tx79ddivu(uint64_t j,
  548.        uint64_t k,
  549.        uint64_t* pRes,
  550.        uint64_t* pRem)
  551.     {
  553.     /* handle divide by zero case */
  554.     
  555.     if (k == 0)
  556. {
  557. *pRes = 0;
  558. *pRem = 0;
  559. }
  560.     else
  561. tx79Div64(j, k, pRes, pRem);
  562.     }
  564. /******************************************************************************
  565. *
  566. * tx79dmult - perform a 64 bit signed multiplication
  567. *
  568. * This routine does a 64 bit signed multiplication and returns the result as
  569. * a 128 bit signed result.
  570. *
  571. * NOMANUAL
  572. */
  574. void tx79dmult(uint64_t j,
  575.        uint64_t k,
  576.        uint64_t* pResLow,
  577.        uint64_t* pResHigh)
  578.     {
  579.     int s;
  581.     /* take shortcut if either value is 0 */
  582.     
  583.     if ((j == 0) || (k == 0))
  584. {
  585. *pResLow = 0;
  586. *pResHigh = 0;
  587. }
  588.     else
  589. {
  590. /* make values positive and determine sign of result */
  591.     
  592. s = 0;
  593. if ((int64_t)j < 0)
  594.     {
  595.     j = -j;
  596.     s ^= 1;
  597.     }
  598. if ((int64_t)k < 0)
  599.     {
  600.     k = -k;
  601.     s ^= 1;
  602.     }
  604. /* use unsigned multiply */
  606. #ifdef USE_C_MULT64_ROUTINE
  607. tx79Mult64(j, k, pResLow, pResHigh);
  608. #else  /* ifdef USE_C_MULT64_ROUTINE */
  609. tx79AMult64(j, k, pResLow, pResHigh);
  610. #endif /* ifdef USE_C_MULT64_ROUTINE */
  612. /* modify result back to correct sign */
  614. if (s == 1)
  615.     {
  616.     if (*pResLow != 0)
  617. {
  618. *pResLow = -(*pResLow);
  619. *pResHigh =  ~(*pResHigh);
  620. }
  621.     else
  622. {
  623. *pResHigh =  -(*pResHigh);
  624. }
  625.     }
  626. }
  627.     }
  629. /******************************************************************************
  630. *
  631. * tx79dmultu - perform a 64 bit unsigned multiplication
  632. *
  633. * This routine does a 64 bit unsigned multiplication and returns the result as
  634. * a 128 bit unsigned result.
  635. *
  636. * NOMANUAL
  637. */
  639. void tx79dmultu(uint64_t j,
  640.        uint64_t k,
  641.        uint64_t* pResLow,
  642.        uint64_t* pResHigh)
  643.     {
  644.     if ((k == 0) || (j == 0))
  645. {
  646. *pResLow = 0;
  647. *pResHigh = 0;
  648. }
  649.     else
  650. #ifdef USE_C_MULT64_ROUTINE
  651. tx79Mult64(j, k, pResLow, pResHigh);
  652. #else  /* ifdef USE_C_MULT64_ROUTINE */
  653. tx79AMult64(j, k, pResLow, pResHigh);
  654. #endif /* ifdef USE_C_MULT64_ROUTINE */
  655.     }
  657. #endif /* (CPU == MIPS64) */