math.c

TDK 6521 SOC 芯片 DEMO程序

/***************************************************************************
 * This code and information is provided "as is" without warranty of any   *
 * kind, either expressed or implied, including but not limited to the     *
 * implied warranties of merchantability and/or fitness for a particular   *
 * purpose.                                                                *
 *                                                                         *
 * Copyright (C) 2005 Teridian Semiconductor Corp. All Rights Reserved.    *
 ***************************************************************************/
//**************************************************************************
//  DESCRIPTION: 71M652x Meter MATH library. 
// 
//  AUTHOR:  MTF
//
//  HISTORY: See end of file.
//**************************************************************************
// File:  CLASSICS.C
//
#include "options.h"
#include "library.h"
#include "mmath.h"

//===========================================================================//
// Binary addition of 8n-bit 'x',
//                 to 8n-bit 'y'.
// *x += *y;       returns carry.
int8_t add (uint8x_t *x, uint8p_t *y, uint8_t n) 
{   // *x += y; where 'x' & 'y' are 'n' bytes wide.
    uint8_16_t t;                       // Carry is always 0 or 1.

    x += n - 1;                         // Point to LSB of 'x'.
    y += n - 1;                         // Point to LSB of 'y'.
    t.c[ HI ] = 0;                      // Initial 'carry' is zero (0).

    do
    {                                   // Single precision add plus carry.
       t.i = *x + *y + t.c[ HI ];       // Compute one-byte sum and carry.         
       *x = t.c[ LO ];                  // x[ n - 1].
       x--;  y--;
    } while (--n);

    return ((int8_t) t.c[ HI ]);        // Last 'carry' can be -1.
}

#if EXTRAS || !MATH_FAST || (AUTOCAL && \
    (EQUATION != _1ELEMENT_3WIRE \
     && EQUATION != _2ELEMENT_4WIRE_DELTA \
     && EQUATION != _2ELEMENT_4WIRE_WYE))
// Binary addition of 8-bit 'y',
//                 to 8n-bit 'x'.
// *x += y_;       returns carry.
int8_t add_1 (uint8x_t *x, uint8_t n, int8_t y_)
{   // *x += y; where 'x' is 'n' bytes wide, 'y_' is 8-bit integer.
    int8_16_t t;                        // Carry is 0, 1 or -1.
    
    x += --n;                           
    t.i = *x + y_;                      // (Carry,Sum) = *x + y_.
                                        // Sign extended.
    do
    {
       *x = t.c[ LO ];                  // LSB of sum.
       x--;                             // Next.
        
       t.i = *x + t.c[ HI ];            // Plus carry (sign extended).
    } while (--n);

    *x = t.c[ LO ];                     // LSB of sum.
    return (t.c[ HI ]);                 // Last 'carry'.
}
#endif

#if EXTRAS
#pragma save
#pragma NOAREGS
// Binary addition of 16-bit 'y_',
//                 to 8n-bit 'x'.
// *x += y_;       returns carry.
int8_t add_2 (uint8x_t *x, uint8_t n, int16_t y_) small reentrant
{            // *x += y; where 'x' is 'n' bytes wide, 'y_' is 16-bit int.
    int8_16_t t;                        // Carry is 0, 1 or -1.

    x += --n;
    t.i = *x + (uint8_t) (y_ & 0xFF);   // (Carry,Sum) = *x + y_.
    *x = t.c[ LO ];      x--;           // x[n - 1] of sum.
                                        // x[n - 2] of sum.         
    t.i = *x + (y_ >> 8) + t.c[ HI ];   // Sign extended.
    --n;

    do
    {
       *x = t.c[ LO ];                  // LSB of sum.
       x--;                             // Next.
       t.i = *x + t.c[ HI ];            // Plus carry (sign extended).
    } while (--n);

    *x = t.c[ LO ];                     // LSB of sum.
    return (t.c[ HI ]);                 // Last 'carry'.
}
#pragma restore
#endif

#if (EQUATION != _1ELEMENT_3WIRE) && (WATT_SUMS || VAR_SUMS || VA_SUMS || AUTOCAL)
// Binary addition of 64-bit 'y',
//                 to 64-bit 'x'.
// *x += *y;
#if MATH_FAST
void add8_8 (uint8x_t *x, uint8x_t *yy) 
{                                       // (uint64_t *) *x += (uint64_t *) *y;
    uint8_16_t t;                       // Carry is always 0 or 1.
    uint8_t pdata yi[8];
    uint8_t pdata *y;

    y = yi;
    memcpy_px (y, yy, 8);
    x += 8 - 1;                         // Point to LSB of 'x'.
    y += 8 - 1;                         // Point to LSB of 'y'.

    // Compute one-byte sums and associated carries;
    t.i = *x + *y;                *x = t.c[ LO ];      x--;  y--;  // x[7] of sum.         
    t.i = *x + *y + t.c[ HI ];    *x = t.c[ LO ];      x--;  y--;  // x[6] of sum.         
    t.i = *x + *y + t.c[ HI ];    *x = t.c[ LO ];      x--;  y--;  // x[5] of sum.         
    t.i = *x + *y + t.c[ HI ];    *x = t.c[ LO ];      x--;  y--;  // x[4] of sum.         
    t.i = *x + *y + t.c[ HI ];    *x = t.c[ LO ];      x--;  y--;  // x[3] of sum.         
    t.i = *x + *y + t.c[ HI ];    *x = t.c[ LO ];      x--;  y--;  // x[2] of sum.         
    t.i = *x + *y + t.c[ HI ];    *x = t.c[ LO ];      x--;  y--;  // x[1] of sum.         
    *x +=      *y + t.c[ HI ];    // x[0] of sum (assumes no more carries).
}
#else
void add8_8 (uint8x_t *x, uint8x_t *yy) 
{
    uint8_t pdata y[8];

    memcpy_px (y, yy, 8);
    add (x, y, 8);
}
#endif // MATH_FAST.
#endif // WATT_SUMS/VAR_SUMS/VA_SUMS.

// Binary addition of 32-bit 'y',
//                 to 64-bit 'x'.
// *x += *y;
#if MATH_FAST
void add8_4 (uint8x_t *x, uint8p_t *y) 
{                                       // (uint64_t *) *x += (U32 *) *y;
    int8_16_t t;                        // Carry is always 0, 1 or -1.

    x += 8 - 1;                         // Point to LSB of 'x'.
    y += 4 - 1;

    // Compute one-byte sums and associated carries;
    t.i = *x + *y;                *x = t.c[ LO ];      x--;  y--;  // x[7] of sum.         
    t.i = *x + *y + t.c[ HI ];    *x = t.c[ LO ];      x--;  y--;  // x[6] of sum.         
    t.i = *x + *y + t.c[ HI ];    *x = t.c[ LO ];      x--;  y--;  // x[5] of sum.         
    t.i = *x + (int8_t) *y + t.c[ HI ];                 // Forces sign extension.
                                  *x = t.c[ LO ];      x--;        // x[4] of sum.         
    t.i = *x      + t.c[ HI ];    *x = t.c[ LO ];      x--;        // x[3] of sum.
    t.i = *x      + t.c[ HI ];    *x = t.c[ LO ];      x--;        // x[2] of sum.
    t.i = *x      + t.c[ HI ];    *x = t.c[ LO ];      x--;        // x[1] of sum.
    *x +=           t.c[ HI ];    // x[0] of sum (assumes no more carries).
}
#else
void add8_4 (uint8x_t *x, uint8p_t *y) 
{
    add_1 (x, 4, add (x + 4, y, 4));
}
#endif // MATH_FAST.

//===========================================================================//
// Binary subtraction of 8n-bit 'y'
//                  from 8n-bit 'x'.
// *x -= *y;        returns borrow.
int8_t sub (uint8x_t *x, uint8p_t *y, uint8_t n) 
{                                       // *x -= *y; where 'x' & 'y' are 'n' bytes wide.
    int8_16_t t;                        // Borrow is always 0, 1 or -1.

    x += n - 1;                         // Point to LSB of 'x'.
    y += n - 1;                         // Point to LSB of 'y'.
    t.c[ HI ] = 0;                      // Initial 'borrow' is zero (0).

    do
    {                                   // Single precision subtract plus borrow.
       t.i  = *x - *y + t.c[ HI ];   
       *x = t.c[ LO ];                  // Compute one-byte difference and borrow.
       x--;  y--;                       // x[ n - 1].
    } while (--n);

    return (t.c[ HI ]);                 // Last 'borrow'.
}

#if EXTRAS
// Binary subtraction of 8-bit 'y',
//                 from 8n-bit 'x'.
// x -= y;         returns the borrow.
int8_t sub_1 (uint8x_t *x, uint8_t n, int8_t y_)
{                                       // x -= y; where 'x' is 'n' bytes wide, 'y' is single byte.                        
    int8_16_t t;                        // Borrow is always 0, 1 or -1.

    x += --n;                           // Point to LSB of 'x'.
    t.i = *x - y_;                      // (Borrow,Sum) = *x - y.

    do
    {
       *x = t.c[ LO ];                  // LSB of difference.
       x--;                             // Next.
       t.i = *x + t.c[ HI ];            // Plus borrow (sign extended).
    } while (--n);

    *x = t.c[ LO ];                     // LSB of sum.
    return (t.c[ HI ]);                 // Last 'borrow'.
}

// Binary subtraction of 16-bit 'y_',
//                  from 8n-bit 'x'.
// *x += y_;       returns borrow.
int8_t sub_2 (uint8x_t *x, uint8_t n, int16_t y_)
{   // *x += y; where 'x' is 'n' bytes wide, 'y_' is 16-bit integer.
    int8_16_t t;                        // Forces sign extensions.

    x += --n;
    t.i = *x - (uint8_t) (y_ & 0xFF);   // (Carry,Sum) = *x + y.
    *x = t.c[ LO ];      x--;           // x[n - 1] of sum.
                                        // x[n - 2] of sum.         
    t.i = *x - (y_ >> 8) + t.c[ HI ];   // Sign extended.
    --n;

    do
    {
       *x = t.c[ LO ];                  // LSB of sum.
       x--;                             // Next.
       t.i = *x + t.c[ HI ];            // Plus carry (sign extended).
    } while (--n);

    *x = t.c[ LO ];                     // LSB of sum.
    return (t.c[ HI ]);                 // Last 'carry'.
}

// Binary subtraction 64-bit 'y',
//               from 64-bit 'x'.
// *x -= *y;
void sub8_8 (uint8x_t *x, uint8p_t *y) 
{                                       // Subtract (in-place)  8-byte 'y' from 8-byte 'x'.
    int8_16_t t;                        // Borrow is always 0, 1 or -1.

    x += 8 - 1;                         // Point to LSB of 'x'.
    y += 8 - 1;                         // Point to LSB of 'y'.

    // Compute one-byte differences and borrows.
    t.i = *x - *y;                *x = t.c[ LO ];   x--;  y--; // x[7] of difference.
    t.i = *x - *y + t.c[ HI ];    *x = t.c[ LO ];   x--;  y--; // x[6] of difference.
    t.i = *x - *y + t.c[ HI ];    *x = t.c[ LO ];   x--;  y--; // x[5] of difference.
    t.i = *x - *y + t.c[ HI ];    *x = t.c[ LO ];   x--;  y--; // x[4] of difference.
    t.i = *x - *y + t.c[ HI ];    *x = t.c[ LO ];   x--;  y--; // x[3] of difference.
    t.i = *x - *y + t.c[ HI ];    *x = t.c[ LO ];   x--;  y--; // x[2] of difference.
    t.i = *x - *y + t.c[ HI ];    *x = t.c[ LO ];   x--;  y--; // x[1] of difference.
     *x = *x - *y + t.c[ HI ];    // x[0] of difference (assumes no more borrows).
}
#endif // EXTRAS.

#if ABS_VALUE
// Binary subtraction 32-bit 'y',
//               from 64-bit 'x'.
// x -= y;
void sub8_4 (uint8x_t *x, uint8p_t *y) 
{                                       // Subtract (in-place)  4-byte 'y' from 8-byte 'x'.
    int8_16_t t;                        // Borrow is always 0, 1 or -1.

    x += 8 - 1;                         // Point to LSB of 'x'.
    y += 4 - 1;                         // Point to LSB of 'y'.

    // Compute one-byte differences and borrows.
    t.i = *x - *y;                *x = t.c[ LO ];   x--;  y--; // x[7] of difference.
    t.i = *x - *y + t.c[ HI ];    *x = t.c[ LO ];   x--;  y--; // x[6] of difference.
    t.i = *x - *y + t.c[ HI ];    *x = t.c[ LO ];   x--;  y--; // x[5] of difference.
    t.i = *x - (int8_t) *y + t.c[ HI ];                     // Force sign extension.    
                                  *x = t.c[ LO ];   x--;  y--; // x[4] of difference.
    t.i = *x      + t.c[ HI ];    *x = t.c[ LO ];   x--;  y--; // x[3] of difference.
    t.i = *x      + t.c[ HI ];    *x = t.c[ LO ];   x--;  y--; // x[2] of difference.
    t.i = *x      + t.c[ HI ];    *x = t.c[ LO ];   x--;  y--; // x[1] of difference.
    *x +=           t.c[ HI ];    // x[0] of difference (assumes no more borrows).
}
#endif // EXTRAS.

#if !MATH_FAST
//===========================================================================//
// Binary multiplication of 8m-bit 'y',
//                      and 8n-bit 'x',
//    leaving result in 8(n+m)-bit 'w'; 't' is a scratchpad for partial products.
// *w = *x * *y; 
void multiply (uint8x_t *w, uint8x_t *x, uint8_t m, uint8p_t *y, uint8_t n, uint8p_t *t) 
{                                       // ((m+n)-byte) w = (m-byte) x * (n-byte) y;
    n--;
    w += n;                             // Point to low-order 'm+1' bytes of product 'w'.
    y += n;                             // Point to LSB of 'y'.
    memset_x (w, 0, m + 1);             // Initially (m+1)-byte partial product is zero (0).
                                  
    do                                  // Accumulate partial products.
    {                                   // ((m+1)-byte) w += (m-byte) x * y.
        *(w - 1) = macp (w, x, m, *y, t);  w--;  y--;  // Point to w[n] & y[n].      
    } while (--n);
                                        
    macp (w, x, m, *y, t);              // ((m-byte) w += (m-byte) x * y; No carry possible.
}

// Multiply and accumulate positive 8n-bit partial product into 'w'.
// Multiplication of 8n-bit 'x',
//               and  8-bit 'y_', accumulated positive into 'w'.
// *w += *x * y_;       't' is a scratchpad for partial products.
uint8_t macp (uint8x_t *w, uint8x_t *x, uint8_t n, uint8_t y_, uint8p_t *t) 
{                                       // ((n+1)-byte) w += (n-byte) x * (uint8_t) y;
    *t = multiply_1 (t + 1, x, n, y_);  // ((n+1)-byte) t  = (n-byte) x * (uint8_t) y;
    return ((uint8_t) add (w, t, n + 1));   // Return carry; ((n+1)-byte) w += ((n+1)-byte) t;
}
#endif

// w = x * y; n is the length