dcpgpkt.c

大量的汇编程序源代码

#ifdef   LINKTEST
   /***** Link Testing Mods *****/
   unsigned char dpkerr[10];
   /***** End Link Testing Mods *****/
#endif   /* LINKTEST */

#ifdef   LINKTEST
   /***** Link Testing Mods - create artificial errors *****/
   printf("**n:normal,e:error,l:lost,p:partial,h:bad header,s:new seq--> ");
   gets(dpkerr);
   if (dpkerr[0] == 's')
      sscanf(&dpkerr[1], "%d", &xxx);
   /***** End Link Testing Mods *****/
#endif   /* LINKTEST */

   if ( debuglevel > 4 )
      printmsg(5, "send packet type %d, yyy=%d, xxx=%d, len=%d, buf = %d",
               type, yyy, xxx, len, xmit);

   header[0] = '\020';
   header[4] = (unsigned char) (type << 3);

   switch (type) {

      case CLOSE:
         break;   /* stop protocol */

      case NAK:
         header[4] += yyy;
         break;   /* reject */

      case SRJ:
         break;

      case ACK:
         header[4] += yyy;
         break;   /* ack */

      case INITA:
      case INITC:
         header[4] += xmit;
         break;

      case INITB:
         i = MINPKT;
         while( i < xmit )
         {
            header[4] ++;
            i *= 2;
         }
         break;

      case DATA:
         header[4] = (unsigned char) (0x80 + (xxx << 3) + yyy);
         if (len < (short) xmit)      /* Short packet?              */
            header[4] |= 0x40;/* Count byte handled at higher level */

#ifdef UDEBUG
            printmsg(7, "data=|%.*s|", len, data);
#endif
         break;

      default:
         printmsg(0,"gspack: Invalid packet type %d",type);
         panic();
   } /* switch */

/*--------------------------------------------------------------------*/
/*    Now we finish up the header.  For data packets, determine       */
/*    the K number in header[1], which specifies the number of        */
/*    actual data bytes transmitted as a power of 2; we also          */
/*    compute a checksum on the data.                                 */
/*--------------------------------------------------------------------*/

   if (type == DATA)
   {
      header[1] = 1;
      i = MINPKT;
      while( i < xmit )
      {
         header[1] ++;
         i *= 2;
      }

      if ( i != xmit )        /* Did it come out exact power of 2?   */
      {
         printmsg(0,"Packet length error ... %d != %d for K = %d",
               i, xmit, (int) header[1]);
         panic();             /* No --> Well, we blew THAT math      */
      } /* if ( i != xmit ) */

/*--------------------------------------------------------------------*/
/*                        Compute the checksum                        */
/*--------------------------------------------------------------------*/

      check = checksum(data, xmit);
      i = header[4]; /* got to do this on PC for ex-or high bits */
      i &= 0xff;
      check = (check ^ i) & 0xffff;
      check = (0xaaaa - check) & 0xffff;
   }
   else {
      header[1] = 9;          /* Control packet size K number (9)    */
      check = (0xaaaa - header[4]) & 0xffff;
                              /* Simple checksum for control         */
   } /* else */

   header[2] = (unsigned char) (check & 0xff);
   header[3] = (unsigned char) ((check >> 8) & 0xff);
   header[5] = (unsigned char)
            ((header[1] ^ header[2] ^ header[3] ^ header[4]) & 0xff) ;

#ifdef   LINKTEST
   /***** More Link Testing Mods *****/
   switch(dpkerr[0]) {
   case 'e':
      data[10] = - data[10];
      break;
   case 'h':
      header[5] = - header[5];
      break;
   case 'l':
      return;
   case 'p':
      swrite((char *) header, HDRSIZE);
      if (header[1] != 9)
         swrite(data, xmit - 3);
      return;
   default:
      break;
   }
   /***** End Link Testing Mods *****/
#endif   /* LINKTEST */

   swrite((char *) header, HDRSIZE);      /* header is 6-bytes long */
   if (header[1] != 9)
      swrite(data, xmit);

} /*gspack*/


/*
   g r p a c k

   Read packet

   on return: yyy=pkrec xxx=pksent len=length<=PKTSIZE  data=*data

   ret(type)       ok
   ret(DCP_EMPTY)  input buf empty
   ret(DCP_ERROR)  bad header

   ret(DCP_EMPTY)  lost packet timeout
   ret(DCP_ERROR)  checksum error

   NOTE (specifications for sread()):

   sread(buf, n, timeout)
      while(TRUE) {
         if (# of chars available >= n) (without dec internal counter)
            read n chars into buf (decrement internal char counter)
            break
    else
       if (time > timeout)
          break;
      }
      return(# of chars available)

*/

static short grpack(short *yyy,
                  short *xxx,
                  short *len,
                  char UUFAR *data,
                  const short timeout)
{
   static short got_hdr  = FALSE;
   static short received = 0;     /* Bytes already read into buffer */
   short needed;

   unsigned short type, check, checkchk, i, total = 0;
   unsigned char c, c2;

   time_t start;

   if (got_hdr)
      goto get_data;

/*--------------------------------------------------------------------*/
/*   Spin up to timeout waiting for a Control-P, our sync character   */
/*--------------------------------------------------------------------*/

   start = 0;
   while (!got_hdr)
   {
      unsigned char *psync;

      needed = HDRSIZE - received;
      if ( needed > 0 )       /* Have enough bytes for header?       */
      {                       /* No --> Read as much as we need      */
         short wait;

         if ( start == 0 )    /* First pass through data?            */
         {                    /* Yes --> Set timers up               */
            start = time(nil(time_t));
            wait = timeout;
         } /* if ( start == 0 ) */
         else {
            wait = (short) (time(NULL) - start) - timeout;
            if (wait < 0)     /* Negative timeout?                   */
               wait = 0;      /* Make it no time out                 */
         } /* else */

         if (sread((char *) &grpkt[received], needed, wait ) <
             (unsigned short) needed )
                              /* Did we get the needed data?         */
            return DCP_EMPTY; /* No --> Return to caller             */

         received += needed;
      } /* if ( needed < received ) */

/*--------------------------------------------------------------------*/
/*            Search for sync character in newly read data            */
/*--------------------------------------------------------------------*/

#ifdef UDEBUG
      printmsg(10,"grpack: Have %d characters after reading %d",
               received, needed);
#endif

      psync = memchr( grpkt, '\020', received );
      if ( psync == NULL )    /* Did we find the sync character?     */
         received = 0;        /* No --> Reset to empty buffer        */
      else if ( psync != grpkt ) /* First character in buffer?       */
      {                       /* No --> Make it first character      */
         received -= psync - grpkt;
         shifts++;
         memmove( grpkt, psync, received );
                              /* Shift buffer over                   */
      } /* else */

/*--------------------------------------------------------------------*/
/*    If we have read an entire packet header, then perform a         */
/*    simple XOR checksum to determine if it is valid.  If we have    */
/*    a valid checksum, drop out of this search, else drop the        */
/*    sync character and restart the scan.                            */
/*--------------------------------------------------------------------*/

      if ( received >= HDRSIZE )
      {
         i = (unsigned short) (grpkt[1] ^ grpkt[2] ^ grpkt[3] ^
                        grpkt[4] ^ grpkt[5]);
         i &= 0xff;
         printmsg(i ? 2 : 10, "prpkt %02x %02x %02x %02x %02x .. %02x ",
            grpkt[1], grpkt[2], grpkt[3], grpkt[4], grpkt[5], i);

         if (i == 0)          /* Good header?                        */
            got_hdr = TRUE;   /* Yes --> Drop out of loop            */
         else {               /* No  --> Flag it, continue loop      */
            badhdr++;
            printmsg(GDEBUG, "*** bad pkt header ***");
            memmove( grpkt, &grpkt[ 1 ], --received );
                              /* Begin scanning for sync character
                                 with next byte                      */
         } /* else */
      } /* if ( received > HDRSIZE ) */
   } /* while */

/*--------------------------------------------------------------------*/
/*                       Handle control packets                       */
/*--------------------------------------------------------------------*/

   if (grpkt[1] == 9)
   {
      if ( data != NULL )
         *data = '\0';
      *len = 0;
      c = grpkt[4];
      type = c >> 3;
      *yyy = c & 0x07;
      *xxx = 0;
      check = 0;
      checkchk = 0;
      got_hdr = FALSE;
   }

/*--------------------------------------------------------------------*/
/*                        Handle data packets                         */
/*--------------------------------------------------------------------*/
   else {
get_data:
      if ( data == NULL )
      {
         printmsg(0,"grpack: Unexpected data packet!");
         received = 0;
         return(DCP_ERROR);
      }

/*--------------------------------------------------------------------*/
/*             Compute the size of the data block desired             */
/*--------------------------------------------------------------------*/

      total = 8 * (2 << grpkt[1]);
      if (total > r_pktsize)  /* Within the defined limits?          */
      {                       /* No --> Other system has bad header,
                                 or the header got corrupted         */
         printmsg(0,"grpack: Invalid packet size %d (%d)",
            total, (int) grpkt[1]);
         received = 0;
         got_hdr = FALSE;
         return(DCP_ERROR);
      }

      needed = total + HDRSIZE - received;
                                 /* Compute byte required to fill
                                    data buffer                      */

/*--------------------------------------------------------------------*/
/*     If we don't have enough data in the buffer, read some more     */
/*--------------------------------------------------------------------*/

      if ((needed > 0) &&
          (sread((char *) &grpkt[HDRSIZE+total-needed], needed, timeout) <
           (unsigned short)needed))
         return(DCP_EMPTY);

      got_hdr = FALSE;           /* Must re-process header next pass */

/*--------------------------------------------------------------------*/
/*              Break packet header into various values               */
/*--------------------------------------------------------------------*/

      type = 0;
      c2 = grpkt[4];
      c = (unsigned char) (c2 & 0x3f);
      *xxx = c >> 3;
      *yyy = c & 0x07;
      i = grpkt[3];
      i = (i << 8) & 0xff00;
      check = grpkt[2];
      check = i | (check & 0xff);
      checkchk = checksum( (char *) grpkt + HDRSIZE , total);
      i = grpkt[4] | 0x80;
      i &= 0xff;
      checkchk = 0xaaaa - (checkchk ^ i);
      checkchk &= 0xffff;
      if (checkchk != check)
      {
         printmsg(4, "*** checksum error ***");
         memmove( grpkt, grpkt + HDRSIZE, total );
                              /* Save data so we can scan for sync   */
         received = total;    /* Note the amount of the data in buf  */
         return(DCP_ERROR);   /* Return to caller with error         */
      }

/*--------------------------------------------------------------------*/
/*    The checksum is correct, now determine the length of the        */
/*    data to return.                                                 */
/*--------------------------------------------------------------------*/

      *len = total;

      if (c2 & 0x40)
      {
         short ii;
         if ( grpkt[HDRSIZE] & 0x80 )
         {
            ii = (grpkt[HDRSIZE] & 0x7f) + ((grpkt[HDRSIZE+1] & 0xff) << 7);
            *len -= ii;
            MEMCPY(data, grpkt + HDRSIZE + 2, *len);
         }
         else {
            ii = (grpkt[HDRSIZE] & 0xff);
            *len -= ii;
            MEMCPY(data, grpkt + HDRSIZE + 1, *len);
         } /* else */
      }
      else
         MEMCPY( data, grpkt + HDRSIZE, *len);
   } /* else */

/*--------------------------------------------------------------------*/
/*           Announce what we got and return to the caller            */
/*--------------------------------------------------------------------*/

   received = 0;              /* Returning entire buffer, reset count */
   printmsg(5, "receive packet type %d, yyy=%d, xxx=%d, len=%d",
      type, *yyy, *xxx, *len);

#ifdef UDEBUG
   printmsg(13, " checksum rec=%04x comp=%04x\ndata=|%.*s|",
      check, checkchk, total, grpkt + HDRSIZE);
#endif

   return(type);

} /*grpack*/


/*
   c h e c k s u m
*/

static unsigned short checksum(char *data, short len)
{
   short i, j;
   unsigned short tmp, chk1, chk2;
   chk1 = 0xffff;
   chk2 = 0;
   j = len;
   for (i = 0; i < len; i++) {
      if (chk1 & 0x8000) {
         chk1 <<= 1;
         chk1++;
      } else {
         chk1 <<= 1;
      }
      tmp = chk1;
      chk1 += (data[i] & 0xff);
      chk2 += chk1 ^ j;
      if ((chk1 & 0xffff) <= (tmp & 0xffff))
         chk1 ^= chk2;
      j--;
   }
   return(chk1 & 0xffff);

} /*checksum*/