zc030x_fp.c

Linux下USB接口的摄像头源码,很详细!对写USB接口驱动有很大帮助的

/* Include declaration */
#include "zc030x_fp.h"

/* Include kernel string */
#include <linux/string.h>

/* Declare division macro */
#define div64(a,b,c) Divide(a,b,c)

int64_t Divide(int64_t a, int64_t b, int64_t *remainder)
{
    /* Temporary variables */
    int64_t c;
    int32_t i, shiftcnt=0;
    /* Check for attempt to divide by zero */
    if (b == (int64_t)0)
    shiftcnt = 1 / shiftcnt;  // Force a divide by zero exception. (shiftcnt=0)
    
    c=(int64_t)0;
    
    /* Left shift B until it is greater than or equal to A */
    while (b<a)
    {
        b <<= 1;
        shiftcnt++;
    }
    
    if (b>a)      /* If B is greater than A, right shift B */
    {
        b >>= 1;
        shiftcnt--;
    }
    
    for(i=0; i<=shiftcnt; i++)
    {
        if (b<=a)    /* If B is greater than A, then the bit is a 1 */
        {
            a -= b;     /* Subtract B from A */
            b >>= 1;  /* Right shift B     */
            c <<= 1;  /* Left shift quotient */
            c++;       /* Increment quotient  */
        }
        else
        {
            b >>= 1;  /* Bit is 0, right shift B, left shift quotient */
            c <<= 1;
        }
    }
    
    if (remainder != NULL)
        *remainder = a;
    
    return c;
}

/* Normalize a FPNum */
void Normalize(FPNum * this)
{
    uint64_t  radix;
    uint64_t  max, min, tmp, zero, mod;
    int32_t  digits=0;
    
    uint32_t d;
    
    zero = 0;
    
    radix = (uint64_t) 10;
    tmp = this->Mant;
    
    if (tmp == zero)  // Special Case
    {
        this->Exp = 0;
        return;
    }
    
    do
    {
        max = radix;
        min = (uint64_t)1;
        d = 0;
        
        if (tmp == zero)  // Special Case
        {
            this->Exp += digits;
            return;
        }
        
        div64(tmp, max, &mod);
        while (mod == 0)
        {
            if (d == 0) 
                d = 1;
            else
                d <<= 1;
            
            min = max;
            if (max >> 32) // If a multiply overflow would occur
                break;
            max = max * max;

            div64(tmp, max, &mod);
        } 
        
        digits += d;
        
        tmp = div64(tmp, min, NULL);  
    } while (d);
    
    this->Mant = tmp;
    this->Exp += digits;
}

int32_t SignificantDigits(FPNum * this)
{
    uint64_t radix;
    uint64_t max, min, tmp;
    int32_t digits=0;
    uint32_t d;
    
    radix = (uint64_t) 10;
    tmp = this->Mant;
    
    do
    {
        max = radix;
        min = (uint64_t) 1;
        d = 0;
        
        if (tmp == (uint64_t) 0)  // Special Case
            return digits;
        
        while (max <= tmp)
        {
            if (d == 0) 
                d = 1;
            else
                d <<= 1;
            
            min = max;
            if (max >> 32) // If a multiply overflow would occur
                break;
            max = max * max;
        } 
        
        digits += d;
        
        tmp = div64(tmp, min, NULL);  // Get the uncounted digits in tmp
    } while (tmp >= radix);
    
    digits++;
    
    return digits;
}





uint64_t PowerR(int32_t n)
{
    uint64_t   radix;
    uint64_t   result, tmpmult;
    int32_t   pwr = 64;
    int32_t   i;
    
    
    if (n == 0)  /* Special case */
    {
        result = (uint64_t) 0;
        return result;
    }
    
    radix = (uint64_t)10;
    result = (uint64_t) 1;
    
    do
    {
        while ((pwr & n) == 0)
        {
            pwr >>= 1;
            if (pwr == 0)
                return result;
        }
    
        i = pwr;
        pwr >>= 1;
        tmpmult = radix;
        while (i > 1)
        {
            i >>= 1;
            tmpmult = tmpmult * tmpmult;
        }
        
        result = result * tmpmult;
    } while (pwr);
    
    return result;
}


void HalfPrecision(FPNum * this)
{
    uint64_t divisor;
    int32_t  divdigits, digits;
    
    Normalize(this);
    digits = SignificantDigits(this);
    if (digits <= (HalfDigits - 1))
    return;
    
    divdigits = digits - HalfDigits + 1;
    
    divisor = PowerR(divdigits);
    this->Mant = div64(this->Mant, divisor, NULL);
    this->Exp = this->Exp + divdigits;
}


void InitFPNum(FPNum * this, const char * Number)
{
    int32_t leading_zeros;
    int32_t trailing_zeros;
    int32_t significant_digits;
    char *p, *q;
    char Striped[256];
    uint64_t multiplier;
    int32_t  DecimalFound = -1;
    int32_t  i;
    
    this->Mant = (uint64_t) 0;
    this->Exp = 0;
    this->Sign = 1;
    
    /* If there is a minus sign in the number then it is negative */
    if (strstr(Number,"-") != NULL)
        this->Sign = -1;
    
    /* First off, find all of the leading zeros, trailing zeros, and
     siginificant digits */
    p = (char*)Number;
    
    /* Move p to first significant digit */
    for(;;)
    {
        if (*p >= '1' && *p <= '9') break;
        if (*p == '.')              break;
        if (*p == 0)                break;
        p++;
    }
    
    /* Copy the string onto Stripped */
    q = Striped;
    significant_digits=0;
    for(;;)
    {
        if (*p == 0)        break;
        
        if (*p == '.')
        {
          DecimalFound = significant_digits;
          p++;
          continue;
        }
        if (*p < '0' || *p > '9')
        {
          p++;
          continue;
        }
        
        *q = *p;
        q++;
        p++;
        significant_digits++;
    }
    *q = 0;
    
    p = strpbrk(Striped, "123456789");
    if (p == NULL)          p = Striped;
    
    if (strlen(p) > 18) // Limit sig. digits.
    {
        for (i=strlen(p) -1 ; i>=18; i--)       p[i] = '0';
    }
    
    // If the decimal point has been found then get rid of trailing zeros.
    if (DecimalFound != -1)   
    {     
        for (;;)
        {
            q--;
            if (q == Striped)       break;
            if (*q == '0')
            {
                *q = 0;
                significant_digits--;
            }
            else
            {
                break;
            }
        }
    }
    
    
    if (DecimalFound == -1) 
        DecimalFound = strlen(Striped);
    
    /* Find the number of significant leading zeros */
    q = Striped;
    leading_zeros = 0;
    for(;;)
    {
        if (*q != '0')      break;
        leading_zeros++;
        q++;
    }
    
    /* Find the number of significant trailing zeros */
    p = &Striped[strlen(Striped) - 1];
    
    trailing_zeros = 0;
    while (p>q)
    {
        if (*p != '0')      break;
        trailing_zeros++;
        p--;
    }
    
    /* Ok, now we know how many leading and trailing zeros there are, and
     where the least significant digit is. */
    multiplier = (uint64_t) 1;
    for (i=0; i<(significant_digits - leading_zeros - trailing_zeros); i++)
    {
        this->Mant = this->Mant + multiplier * ((uint64_t) (uint32_t) (*p - '0'));
        multiplier = multiplier * (uint64_t) 10;
        p--;
    }
    
    DecimalFound = DecimalFound - strlen(Striped);
    this->Exp = DecimalFound	+ trailing_zeros;
    HalfPrecision(this);

}



void GetFPNumAsString(FPNum * this, char * output)
{
    char     decimal[1024];
    uint32_t mantdigits;
    int32_t  i;
    char *p, *q;
    int64_t digit;
    int32_t  zeros;
    uint64_t x;
    FPNum    y;
    
    
    
    y = *this;
    HalfPrecision(&y);
    x = y.Mant;
    p = &decimal[MaxDigits-1];
    p[1] = 0;
    
    while (x != (uint64_t) 0)
    {
        // this <=> digit = x%10 and x/=10
        x = div64(x, (uint64_t)10, &digit);
        *p = ((unsigned char) digit) + '0';
        p--;
    }
    
    /* Now lets figure out where to put the decimal point */
    p++;
    mantdigits = strlen(p);
    
    /* Put in leading zeros if necessary */
    zeros = -(mantdigits + y.Exp);
    q = output;
    
    if (y.Sign < 0)
        *q++ = '-';
    
    if (zeros >= 0)
    {
        *q = '0';
        q++;
        *q = '.';
        q++;
        for (i=0; i<zeros; i++)
        {
            *q = '0';
            q++;
        }
    }
    
    /* Put in the matissa digits and decimal point */
    zeros = -zeros;
    for (i=0; i<mantdigits; i++)
    {
        *q = p[i];
        q++;
        zeros--;
        if (zeros == 0)
        {
            *q='.';
            q++;
        }
    }
    
    /* Put in the trailing zeros */
    for (i=0; i<zeros; i++)
    {
        *q = '0';
        q++;
    }
    
    *q = 0;
    
}

int64_t GetFPNumAsInt(FPNum * this)
{
    FPNum     a = *this;
    FPNum     Round;
    uint64_t  x =0;
    int64_t   ret;
    
    /* Create the Round number to be added */
    Round.Sign = a.Sign;
    Round.Exp  = -1;
    Round.Mant = 5;
    
    /* Round this number to the nearest integer */
    a = Add(a, Round);
    
    /* Make it half precision */
    HalfPrecision(&a);
    
    /* Get its mantissa */
    x = a.Mant;
    
    /* Now the integer is Mant / 10^-Exp */
    ret = 0;
    x = 1;
    while (ret < -a.Exp)
    {
        x *= 10;
        ret ++;
    }
    
    /* Return */
    a.Mant = div64(a.Mant, x, NULL);
    return (int64_t)a.Sign * (int64_t) (a.Mant);
    
}


void InitFPU(FPNum * this)
{
/* The next lines are removed because of MaxDigits being fixed */
/*    this->Mant = ~(uint64_t) 0;                              */
/*    MaxDigits = SignificantDigits(this) - 1;                 */
/*    this->Mant = this->Mant >> 32;                           */
/*    HalfDigits = SignificantDigits(this) - 1;                */
    this->Mant = 0;
    this->Sign = 1;
    this->Exp = 0;
}



void DeNormalize(FPNum * this)
{
    uint64_t multiplier;
    int32_t digits, multexp;
    
    
    digits  = SignificantDigits(this);
    multexp = MaxDigits - digits - 1;
    
    if (multexp <= 0) return;
    multiplier = PowerR(multexp);
    
    this->Mant = this->Mant * multiplier;
    this->Exp = this->Exp - multexp;
}

void Align(FPNum * p, FPNum * q)
{
    uint64_t   multiplier;
    FPNum     *a, *b;
    int32_t   expdiff;
    int32_t   bdigits, multexp;
    
    Normalize(p);
    Normalize(q);
    
    if (p->Exp == q->Exp)
        return;
    
    if (p->Exp > q->Exp)
    {
        a = q;
        b = p;
    }
    else
    {
        a = p;
        b = q;
    }
    
    /* a now points to the arg with the smaller exponent */
    expdiff = b->Exp - a->Exp;
    bdigits = SignificantDigits(b);
    
    /* First, make b's exponent smaller --
     This does not cause a loss of precision. */
    
    multexp = MaxDigits - bdigits - 1;
    if (multexp > expdiff)
        multexp = expdiff;
    
    
    if (bdigits == 0)   // Special case when b=0
    {
        b->Exp = a->Exp;
        return;
    }
    
    if (multexp > 0)
    {
        multiplier = PowerR(multexp);
        b->Mant = b->Mant * multiplier;
        b->Exp = b->Exp - multexp;
        
        expdiff -= multexp;
    }
    
    /* Next, make a's exponent larger -- 
      Note, this causes loss of precision, but its OK since we couldn't
      represent this loss in the sum of the two numbers anyways. */
    
    if (expdiff)
    {
        multiplier = PowerR(expdiff);
        a->Mant = div64(a->Mant, multiplier, NULL);
        a->Exp = a->Exp + expdiff;
    }
}


FPNum Add(const FPNum a, const FPNum b)
{
    FPNum     result, p, q;
    int8_t    invertthis=0, invertq=0;
    
    q = a;
    p = b;
    Align(&p, &q);
    
    if (p.Sign < 0)
    {
        invertthis = 1;
        p.Mant = -p.Mant;
    }
    
    if (q.Sign < 0)
    {
        invertq = 1;
        q.Mant = -q.Mant;
    }
    
    result.Mant = p.Mant + q.Mant;
    result.Exp = p.Exp;
    
    if (invertthis)
        p.Mant = -p.Mant;
    
    if (invertq)
        q.Mant = -q.Mant;
    
    if (result.Mant >> 63)
    {
        result.Mant = -result.Mant;
        result.Sign = -1;
    }
    else
    {
        result.Sign = 1;
    }
    
    Normalize(&result);
    return result;
}

void MultAdjust(FPNum *p, FPNum *q)
{
    uint64_t divisor;
    FPNum *a, *b;
    int32_t adigits, bdigits, totaldigits, divdigits;
    
    Normalize(p);
    Normalize(q);
    adigits = SignificantDigits(p);
    bdigits = SignificantDigits(q);
    
    if (adigits > bdigits)
    {
        a = p;
        b = q;
    }
    else
    {
        a = q;
        b = p;
        totaldigits = adigits;
        adigits = bdigits;
        bdigits = totaldigits;
    }
    
    /* A is now the number with more digits */ 
    totaldigits = adigits + bdigits;
    if (totaldigits <= (MaxDigits-1))    return;
    
    
    /* Reduce the number of digits in a so it has as many as b OR
       until totaldigits is acceptable */
    divdigits = adigits - bdigits;
    
    /* If we can make the number of total digits acceptable without
       making a and b have the same number of digits, do so! */
    if (divdigits > (totaldigits - MaxDigits + 1))
    divdigits = totaldigits - MaxDigits + 1;
    
    if (divdigits)
    {
        divisor = PowerR(divdigits);
        a->Mant = div64(a->Mant, divisor, NULL); // exactly like a->Mant /= divisor
        a->Exp = a->Exp + divdigits;
        adigits = adigits - divdigits;
    }
    totaldigits = adigits + bdigits;
    
    if (totaldigits <= (MaxDigits-1))   return;
    
    /* Reduce a and b so that totaldigits <=(MaxDigits-1) */
    divdigits = totaldigits - MaxDigits + 1;
    divdigits = divdigits / 2;
    
   
    /* SPR2 divdigits = 0 */
    if (divdigits == 0)     divdigits++;       
    
    divisor = PowerR(divdigits);
    
    a->Mant = div64(a->Mant, divisor, NULL);
    a->Exp = a->Exp + divdigits;
    b->Mant = div64(b->Mant, divisor, NULL);
    b->Exp = b->Exp + divdigits;
}


FPNum Mul(const FPNum a, const FPNum b)
{
    FPNum result, p, q;
    
    q = b;
    p = a;
    
    MultAdjust(&p, &q);
    
    result.Mant = q.Mant * p.Mant;
    result.Exp = q.Exp + p.Exp;
    result.Sign = q.Sign * p.Sign;
    
    Normalize(&result);
    return result;
}

const FPNum CreateFPNumFromInt(int32_t Number)
{
    /* The return */
    FPNum a;
    
    /* Simple creation */
    a.Sign = Number < 0 ? -1 : 1;
    a.Exp = 0;
    a.Mant = a.Sign * Number;
    /* Return */
    return a;
}