Subversion Repositories pentevo

Rev

Blame | Last modification | View Log | Download | RSS feed | ?url?

  1. /* bpemu.c */
  2. /*****************************************************************************/
  3. /* SPDX-License-Identifier: GPL-2.0-only OR GPL-3.0-only                     */
  4. /*                                                                           */
  5. /* AS-Portierung                                                             */
  6. /*                                                                           */
  7. /* 64 bit arithmetic for platforms not having a 64 bit integer               */
  8. /*                                                                           */
  9. /*****************************************************************************/
  10.  
  11. #include <assert.h>
  12.  
  13. #include "stdinc.h"
  14. #include "math64.h"
  15.  
  16. /*!------------------------------------------------------------------------
  17.  * \fn     nlz16(Word n)
  18.  * \brief  find # of tleading zeros
  19.  * \param  n 16-bit word to examine
  20.  * \return # of leading zeros
  21.  * ------------------------------------------------------------------------ */
  22.  
  23. static int nlz16(Word n)
  24. {
  25.   unsigned z;
  26.   for (z = 0; z < 16; z++)
  27.   {
  28.     if (n & 0x8000u)
  29.       break;
  30.     n = n << 1;
  31.   }
  32.   return z;
  33. }
  34.  
  35. /*!------------------------------------------------------------------------
  36.  * \fn     to16(Word *pOut, const t64 *pIn)
  37.  * \brief  Convert t64 to array of uint16 needed by mul/div
  38.  * \param  pOut destination array
  39.  * \param  pIn source value
  40.  * \return # of words written (1..4)
  41.  * ------------------------------------------------------------------------ */
  42.  
  43. static int to16(Word *pOut, const t64 *pIn)
  44. {
  45.   int res = 0;
  46.  
  47.   pOut[res++] = pIn->low & 0xffff;
  48.   pOut[res++] = (pIn->low >> 16) & 0xffff;
  49.   pOut[res++] = pIn->high & 0xffff;
  50.   pOut[res++] = (pIn->high >> 16) & 0xffff;
  51.   while ((res > 1) && !pOut[res - 1])
  52.     res--;
  53.   return res;
  54. }
  55.  
  56. /*!------------------------------------------------------------------------
  57.  * \fn     from16(t64 *pOut, const Word *pIn)
  58.  * \brief  convert uint16 array back to t64
  59.  * \param  pOut value to reconstruct
  60.  * \param  pIn uint16 array
  61.  * ------------------------------------------------------------------------ */
  62.  
  63. static void from16(t64 *pOut, const Word *pIn)
  64. {
  65.   pOut->low = pIn[1];
  66.   pOut->low = (pOut->low << 16) | pIn[0];
  67.   pOut->high = pIn[3];
  68.   pOut->high = (pOut->high << 16) | pIn[2];
  69. }
  70.  
  71. /*!------------------------------------------------------------------------
  72.  * \fn     mulmnu(Word w[], Word u[], Word v[], int m, int n)
  73.  * \brief  unsigned multi-precision multiply (taken from Hacker's Delight)
  74.  * \param  w result buffer
  75.  * \param  u, v numbers to multiply
  76.  * \param  m length of u in words
  77.  * \param  n length of v in words
  78.  * ------------------------------------------------------------------------ */
  79.  
  80. void mulmnu(Word w[], Word u[], Word v[], int m, int n)
  81. {
  82.   unsigned int k, t;
  83.   int i, j;
  84.  
  85.   for (i = 0; i < m; i++)
  86.     w[i] = 0;
  87.  
  88.   for (j = 0; j < n; j++)
  89.   {
  90.     k = 0;
  91.     for (i = 0; i < m; i++)
  92.     {
  93.       t = u[i]*v[j] + w[i + j] + k;
  94.       w[i + j] = t;          /* (I.e., t & 0xFFFF). */
  95.       k = t >> 16;
  96.     }
  97.     w[j + m] = k;
  98.   }
  99. }
  100.  
  101. /*!------------------------------------------------------------------------
  102.  * \fn     mulmnu(Word q[], Word r[], Word u[], Word v[], int m, int n)
  103.  * \brief  unsigned multi-precision divide (taken from Hacker's Delight)
  104.  * \param  q quotient buffer
  105.  * \param  r remainder buffer (may be NULL)r
  106.  * \param  u divident
  107.  * \param  v divisor
  108.  * \param  m length of u in words
  109.  * \param  n length of v in words
  110.  * \return 0 on success, 1 on error
  111.  * ------------------------------------------------------------------------ */
  112.  
  113. static int divmnu(Word q[], Word r[], const Word u[], const Word v[], int m, int n)
  114. {
  115.   const unsigned b = 65536; /* Number base (16 bits). */
  116.   Word un[16], vn[16]; /* Normalized form of u, v. */
  117.   unsigned qhat;   /* Estimated quotient digit. */
  118.   unsigned rhat;   /* A remainder. */
  119.   unsigned p;      /* Product of two digits. */
  120.   int s, i, j, t, k;
  121.  
  122.   /* Return if invalid param. */
  123.  
  124.   if (m < n || n <= 0 || v[n - 1] == 0)
  125.     return 1;
  126.  
  127.   /* Take care of the case of a single-digit divisor here. */
  128.  
  129.   if (n == 1)
  130.   {
  131.     k = 0;
  132.     for (j = m - 1; j >= 0; j--)
  133.     {
  134.       q[j] = (k*b + u[j]) / v[0];
  135.       k = (k * b + u[j]) - q[j] * v[0];
  136.     }
  137.     if (r != NULL) r[0] = k;
  138.     return 0;
  139.   }
  140.  
  141.   /* Normalize by shifting v left just enough so that
  142.      its high-order bit is on, and shift u left the
  143.      same amount. We may have to append a high-order
  144.      digit on the dividend; we do that unconditionally. */
  145.  
  146.   s = nlz16(v[n - 1]); /* 0 <= s <= 16. */
  147.   assert(n < 16); /* vn = (Word *)alloca(2 * n); */
  148.   for (i = n - 1; i > 0; i--)
  149.     vn[i] = (v[i] << s) | (v[i - 1] >> (16 - s));
  150.   vn[0] = v[0] << s;
  151.   assert(m + 1 < 16); /* un = (Word *)alloca(2 * (m + 1)); */
  152.   un[m] = u[m-1] >> (16 - s);
  153.   for (i = m - 1; i > 0; i--)
  154.     un[i] = (u[i] << s) | (u[i - 1] >> (16 - s));
  155.   un[0] = u[0] << s;
  156.  
  157.   /* Main loop. */
  158.  
  159.   for (j = m - n; j >= 0; j--)
  160.   {
  161.     /* Compute estimate qhat of q[j]. */
  162.  
  163.     qhat = (un[j+n]*b + un[j+n-1])/vn[n-1];
  164.     rhat = (un[j+n]*b + un[j+n-1]) - qhat*vn[n-1];
  165.   again:
  166.     if (qhat >= b || qhat*vn[n-2] > b*rhat + un[j+n-2])
  167.     {
  168.       qhat = qhat - 1;
  169.       rhat = rhat + vn[n-1];
  170.       if (rhat < b) goto again;
  171.     }
  172.  
  173.     /* Multiply and subtract. */
  174.  
  175.     k = 0;
  176.     for (i = 0; i < n; i++)
  177.     {
  178.       p = qhat*vn[i];
  179.       t = un[i + j] - k - (p & 0xFFFF);
  180.       un[i+j] = t;
  181.       k = (p >> 16) - (t >> 16);
  182.     }
  183.     t = un[j+n] - k;
  184.     un[j + n] = t;
  185.  
  186.     /* Store quotient digit. */
  187.  
  188.     q[j] = qhat;
  189.  
  190.     /* If we subtracted too much, add back. */
  191.  
  192.     if (t < 0)
  193.     {
  194.       q[j] = q[j] - 1;
  195.       k = 0;
  196.       for (i = 0; i < n; i++)
  197.       {
  198.         t = un[i + j] + vn[i] + k;
  199.         un[i + j] = t;
  200.         k = t >> 16;
  201.       }
  202.       un[j+n] = un[j+n] + k;
  203.     }
  204.   } /* End j. */
  205.  
  206.   /* If the caller wants the remainder, unnormalize
  207.      it and pass it back. */
  208.  
  209.   if (r != NULL)
  210.   {
  211.     for (i = 0; i < n; i++)
  212.       r[i] = (un[i] >> s) | (un[i + 1] << (16 - s));
  213.   }
  214.   return 0;
  215. }
  216.  
  217. /*!------------------------------------------------------------------------
  218.  * \fn     add64(t64 *pRes, const t64 *pA, const t64 *pB)
  219.  * \brief  add two values
  220.  * \param  pRes sum buffer
  221.  * \param  pA, pB values to add
  222.  * ------------------------------------------------------------------------ */
  223.  
  224. void add64(t64 *pRes, const t64 *pA, const t64 *pB)
  225. {
  226.   pRes->low = pA->low + pB->low;
  227.   pRes->high = pA->high + pB->high + !!(pRes->low < pA->low);
  228. }
  229.  
  230. /*!------------------------------------------------------------------------
  231.  * \fn     sub64(t64 *pRes, const t64 *pA, const t64 *pB)
  232.  * \brief  subtract two values
  233.  * \param  pRes difference buffer
  234.  * \param  pA, pB values to subtract
  235.  * ------------------------------------------------------------------------ */
  236.  
  237. void sub64(t64 *pRes, const t64 *pA, const t64 *pB)
  238. {
  239.   pRes->low = pA->low - pB->low;
  240.   pRes->high = pA->high - pB->high - !!(pRes->low > pA->low);
  241. }
  242.  
  243. /*!------------------------------------------------------------------------
  244.  * \fn     mul64(t64 *pRes, const t64 *pA, const t64 *pB)
  245.  * \brief  multiply two values
  246.  * \param  pRes product buffer
  247.  * \param  pA, pB values to multiply
  248.  * ------------------------------------------------------------------------ */
  249.  
  250. void mul64(t64 *pRes, const t64 *pA, const t64 *pB)
  251. {
  252.   Word u[4], v[4], w[8];
  253.   int m = to16(u, pA),
  254.       n = to16(v, pB);
  255.  
  256.   mulmnu(w, u, v, m, n);
  257.   from16(pRes, w);
  258. }
  259.  
  260. /*!------------------------------------------------------------------------
  261.  * \fn     div64(t64 *pRes, const t64 *pA, const t64 *pB)
  262.  * \brief  divide two values
  263.  * \param  pRes quotient buffer
  264.  * \param  pA, pB values to divide
  265.  * ------------------------------------------------------------------------ */
  266.  
  267. void div64(t64 *pRes, const t64 *pA, const t64 *pB)
  268. {
  269.   Word u[4], v[4], q[4];
  270.   int m = to16(u, pA),
  271.       n = to16(v, pB);
  272.  
  273.   q[0] = q[1] = q[2] = q[3] = 0;
  274.   divmnu(q, NULL, u, v, m, n);
  275.   from16(pRes, q);
  276. }
  277.  
  278. /*!------------------------------------------------------------------------
  279.  * \fn     mod64(t64 *pRes, const t64 *pA, const t64 *pB)
  280.  * \brief  modulo-divide two values
  281.  * \param  pRes remainder buffer
  282.  * \param  pA, pB values to divide
  283.  * ------------------------------------------------------------------------ */
  284.  
  285. void mod64(t64 *pRes, const t64 *pA, const t64 *pB)
  286. {
  287.   Word u[4], v[4], q[4], r[4];
  288.   int m = to16(u, pA),
  289.       n = to16(v, pB);
  290.  
  291.   q[0] = q[1] = q[2] = q[3] = 0;
  292.   divmnu(q, r, u, v, m, n);
  293.   from16(pRes, r);
  294. }
  295.  
  296. /*!------------------------------------------------------------------------
  297.  * \fn     mod_div64(t64 *pQuot, t64 *pRem, const t64 *pA, const t64 *pB)
  298.  * \brief  divide two values, and return also remainder
  299.  * \param  pQuot quotient buffer
  300.  * \param  pRem remainder buffer
  301.  * \param  pA, pB values to divide
  302.  * ------------------------------------------------------------------------ */
  303.  
  304. void mod_div64(t64 *pQuot, t64 *pRem, const t64 *pA, const t64 *pB)
  305. {
  306.   Word u[4], v[4], q[4], r[4];
  307.   int m = to16(u, pA),
  308.       n = to16(v, pB);
  309.  
  310.   q[0] = q[1] = q[2] = q[3] = 0;
  311.   divmnu(q, r, u, v, m, n);
  312.   from16(pQuot, q);
  313.   from16(pRem, r);
  314. }
  315.