arith.c

IEEE 1588-2002编程源码

/* arith.c */
 
#include "ptpd.h"

/* from annex C of the spec */
/* compute the checksum of the subdomain_name
   inputs: pInput a pointer to the domain name character
   returns the CRC checksum as an unsigned long */
UInteger32 crc_algorithm(const Octet *pInput, Integer16 length)
{
  /* pInput pointer to character string of interest,
    length is strlen of string
    CRC-32 802.3 polynomial is 0x04c11db7
   bit-reverse 0x04c11db7: */
  UInteger32 polynomial = 0xedb88320;
  /* crc initialized to all 1s */
  UInteger32 crc = 0xffffffff;
  UInteger8 *blkPtr = (UInteger8*)pInput;
  /* sequence thru each byte of the input sequence */
  while(length-- > 0)
  {
    Integer16 i;
    UInteger8 data = *(blkPtr++);
    /* include each bit of the data, starting with the lsb */
    for (i=0; i<8; i++)
    {
      if((crc^data)&1)
      {
        crc = (crc>>1);
        crc ^= polynomial;
      }
      else
      {
        crc = (crc>>1);
      }
      data >>= 1;
    }
  }
  
  return crc^0xffffffff;
}

Integer16 log2_256(UInteger32 x)
{
  int i;
  
  x = x >> 1;
  for(i = 0; x > 0; ++i)
    x = x >> 1;
  return 256*i;
  /* add a lookup table for better resolution */
}

void sub_pos_times(TimeRepresentation *r, TimeRepresentation *x, TimeRepresentation *y)
{
  Integer32 nsec;
  
  /* nanoseconds normalization (makes reult nanoseconds < 1000000000) */
  nsec = (x->nanoseconds - y->nanoseconds) / 1000000000;
  
  /* sub nanosec */
  r->nanoseconds = x->nanoseconds - (y->nanoseconds + 1000000000 * nsec);
  
  /* do subtraction accounting for rollover */
  if(x->seconds == y->seconds)
  {
    r->seconds = nsec < 0 ? -nsec : nsec;
    
    if(nsec < 0)
    {
      r->seconds =  -nsec;
      if(!r->nanoseconds)
        r->nanoseconds = -1;
      return;
    }
    else if(nsec > 0)
    {
      r->seconds =  nsec;
      return;
    }
    else
      return;
  }
  else if(x->seconds > y->seconds)
  {
    r->seconds = x->seconds - y->seconds;
    
    if(nsec > 0 || r->seconds >= -nsec)
    {
      r->seconds = r->seconds + nsec;
      goto pos_carry;
    }
    else
    {
      r->seconds = (-nsec) - r->seconds;
      goto neg_carry;
    }
  }
  else /* x-seconds < y->seconds */
  {
    r->seconds = y->seconds - x->seconds;
    
    if(nsec < 0 || r->seconds >= nsec)
    {
      r->seconds = r->seconds - nsec;
      goto neg_carry;
    }
    else
    {
      r->seconds = nsec - r->seconds;
      goto pos_carry;
    }
  }
  
  pos_carry:
  if(r->nanoseconds < 0)
  {
    --r->seconds;
    r->nanoseconds += 1000000000;
  }
  return;
  
  neg_carry:
  if(r->nanoseconds > 0)
  {
    --r->seconds;
    r->nanoseconds -= 1000000000;
  }
  else if(r->seconds && !r->nanoseconds)
    r->nanoseconds = -1;
  
  return;
}

void add_pos_times(TimeRepresentation *r, TimeRepresentation *x, TimeRepresentation *y)
{
  UInteger32 nsec;
  
  /* nanoseconds normalization */
  nsec = ((UInteger32)x->nanoseconds + (UInteger32)y->nanoseconds) / 1000000000;
  
  r->seconds = x->seconds + (y->seconds + nsec);
  r->nanoseconds = x->nanoseconds + (y->nanoseconds - 1000000000 * nsec);
}

void subTime(TimeRepresentation *r, TimeRepresentation *x, TimeRepresentation *y)
{
  TimeRepresentation t1, t2;
  
  if(x->nanoseconds >= 0 && y->nanoseconds >= 0)
  {
    sub_pos_times(r, x, y);
  }
  else if (x->nanoseconds < 0 && y->nanoseconds < 0)
  {
    t1.seconds = x->seconds;
    t1.nanoseconds = -x->nanoseconds;
    
    t2.seconds = y->seconds;
    t2.nanoseconds = -y->nanoseconds;
    
    sub_pos_times(r, &t2, &t1);
  }
  else if(x->nanoseconds >= 0)  /* && y->nanoseconds < 0 */
  {
    t1.seconds = y->seconds;
    t1.nanoseconds = -y->nanoseconds;
    
    add_pos_times(r, x, &t1);
  }
  else  /* x->nanoseconds < 0  && y->nanoseconds >= 0 */
  {
    t1.seconds = x->seconds;
    t1.nanoseconds = -x->nanoseconds;
    
    add_pos_times(r, &t1, y);
    r->nanoseconds = -r->nanoseconds;
  }
}

void addTime(TimeRepresentation *r, TimeRepresentation *x, TimeRepresentation *y)
{
  TimeRepresentation t1, t2;
  
  if(x->nanoseconds >= 0 && y->nanoseconds >= 0)
  {
    add_pos_times(r, x, y);
  }
  else if(x->nanoseconds < 0 && y->nanoseconds < 0)
  {
    t1.seconds = x->seconds;
    t1.nanoseconds = -x->nanoseconds;
    
    t2.seconds = y->seconds;
    t2.nanoseconds = -y->nanoseconds;
    
    add_pos_times(r, &t1, &t2);
    r->nanoseconds = -r->nanoseconds;
  }
  else if(x->nanoseconds >= 0)  /* && y->nanoseconds < 0 */
  {
    t1.seconds = y->seconds;
    t1.nanoseconds = -y->nanoseconds;
    
    sub_pos_times(r, x, &t1);
  }
  else  /* x->nanoseconds < 0  && y->nanoseconds >= 0 */
  {
    t1.seconds = x->seconds;
    t1.nanoseconds = -x->nanoseconds;
    
    sub_pos_times(r, y, &t1);
  }
}

void divTime(TimeRepresentation *r, TimeRepresentation *x, Integer32 y)
{
  Boolean pos = TRUE;
  
  if(!y)
    return;
  
  if((x->nanoseconds < 0) != (y < 0))
    pos = FALSE;
  
  r->nanoseconds = (1000000000/y)*(x->seconds%y) +
    (x->nanoseconds < 0 ? -x->nanoseconds : x->nanoseconds)/y;
  r->seconds = x->seconds / y;
  
  if(!pos)
    r->nanoseconds = -r->nanoseconds;
}