pcycles.c

来自「在ARM7和UC/OSII的平台上实现了GPS自动报站的功能,涉及GPS模块LE」· C语言 代码 · 共 852 行 · 第 1/2 页

   pck_xchgpkt(pkt);

   /* Give server a chance to accept the connection */
   conn_tmo = cticks + TPS;
   while(conn_tmo > cticks)
   {
      if(esvr_sock != INVALID_SOCKET)
         break;
      tk_yield();    /* spin while TCP server socket is invalid */
   }

   return 0;
}


/* close tcp connection */

int
pck_tcpfin(void * pio)
{
   PACKET   pkt;
   u_long   tmpseq;

   /* make and send a FIN packet */
   pkt = pkc_bldtcp(40, TH_FIN|TH_ACK);
   if(!pkt)
   {
      ns_printf(pio, "pck_tcpfin: no pkt\n");
      return -1;
   }
   tmpseq = tcpseq;     /* save pre-fin sequence number */
   pck_xchgpkt(pkt);

   /* see if the stack responded with a FIN bit */
   if((tcp_sentflags & TH_FIN) == 0)
   {
      /* NicheStack will ACK our FIN before sending his own FIN.
       * If this happens then return without error. His FIN 
       * will be acked by the pkt_send() routine.
       */
      if(tcpseq == (tmpseq + 1))
         return 0;
      ns_printf(pio, "pck_tcpfin: no ACK of FIN\n");
      return -1;
   }

   /* ACK his FIN to complete the close */
   pkt = pkc_bldtcp(40, TH_FIN);
   if(!pkt)
   {
      ns_printf(pio, "pck_tcpfin: no pkt\n");
      return -1;
   }
   pck_xchgpkt(pkt);
   return 0;
}

/* tcp_hdrfixup() - prepare tcp header for next itteration */

static void
tcp_hdrfixup(char * pbuf)
{
   struct   tcphdr * ptcp;
   u_long   seq_change;
   u_long   ack_change;
   u_long   tcpsum;

   /* Change the sequence and ACK numbers to reflect the data sent and
    * received. We get this info from tcpseq and tcpack values set in
    * pck_pkt_send. Since we change the TCP header we also need to fix
    * the checksum.
    */
   ptcp = (struct tcphdr *)(pbuf + sizeof (struct ip));
   seq_change = tcpseq - htonl(ptcp->th_seq);
   seq_change = htonl(seq_change);
   ack_change = tcpack - htonl(ptcp->th_ack);
   ack_change = htonl(ack_change);

   tcpsum = ((~(u_long)ptcp->th_sum) & 0x0000ffff);
   tcpsum += (u_short)(seq_change >> 16);
   tcpsum += (u_short)(seq_change & 0x0000ffff);
   tcpsum += (u_short)(ack_change >> 16);
   tcpsum += (u_short)(ack_change & 0x0000ffff);

   while(tcpsum  & 0xffff0000)   /* if overflow, add high bits back in */
      tcpsum = (tcpsum & 0x0000ffff) + (tcpsum >> 16);

   ptcp->th_seq = htonl(tcpseq);    /* set the new seq number */
   ptcp->th_ack = htonl(tcpack);    /* set the new ACK number */
   ptcp->th_sum = ~(unshort)tcpsum; /* set new checksum */
}
#endif /* TCP_ECHOTEST */

void
pc_printstat(void * pio, u_long itterations, u_long pticks, char * typetext)
{
   u_long mult;      /* multiplier to avoid data over flow */
   u_long pt_pkts;
   u_long msecs;
   u_long decimal;

   ns_printf(pio, "Ran %lu reps in %lu ticks; ", itterations, pticks);
   if(calibration && pticks)
   {
      if(pticks/itterations < 1)
         mult = 10000000;
      else if (pticks/itterations < 1000)
         mult = 10000;
      else if (pticks/itterations < 1000000)
         mult = 10;
      else
         mult = 1;

      if(mult <= 10)
         pt_pkts = (pticks * mult) / itterations;  /* pticks/packet (scaled) */
      else
         pt_pkts = pticks * (mult / itterations);  /* pticks/packet (scaled) */

      /* figure microseconds per pkt. First check for 32 bit overflow... */
      if((calibration / (10000000 / mult)) > 0)
      {

         msecs = pt_pkts / (calibration / (10000000 / mult));
         decimal = msecs;
         msecs /= 10;
         decimal -= (msecs * 10);

         ns_printf(pio, "%lu.%lu u_secs per %s", 
            msecs, decimal, typetext );
      }
   }
   ns_printf(pio, "\n");
   return;
}

/* mock up an incoming ping packet and pass it up to IP layer. */

int
pkc_ping(void * pio)
{
   return(pkc_rep(pio, ICMP_PROT));
}
   

int
pkc_rep(void * pio, u_char prot)
{
   u_long   itterations;   /* number of pings (from cmd line) */
   struct   ping * png;    /* ping (ICMP) header to pass to stack */
   struct   udp * pup;     /* UCP header to pass to stack */
   u_long   pticks;        /* elpased profiler (fast) ticks */
   int      ip_len;        /* length of IP packet */
   int      err;           /* return error holder */
   PACKET   pkt;           /* packet to send */
   char     pbuf[40];      /* packet header buffer */
   int      headerslen;    /* total length of headers */
   char *   typetext;      /* printable pkt type */
   char *   arg   =  nextarg( ((GEN_IO)pio)->inbuf );
#ifdef DEBUG_PCKTCP
   struct tcphdr * ptcp;
#endif

   if(pc_if == NULL)
   {
      ns_printf(pio, "Open test device with pcmake first.\n");
      return -1;
   }

   if(*arg != 0)
      itterations = (u_long)atol(arg);
   else
      itterations = 1;     /* default to one */

   test_prot = prot;       /* save prot for pcn_pkt_send() call */
   pcn_active = TRUE;      /* now officially testing */

   switch (prot)           /* more per-protocol setup */
   {
   case ICMP_PROT:
      {
      int pingsize = sizeof(struct ping) + deflength;
      headerslen = sizeof(struct ip) + sizeof(struct ping);
      ip_len = deflength + headerslen;    /* allow for IP and ICMP headers */
      typetext = "ping";
      pkt = pkc_bldip(prot, ip_len);   /* get basic IP pkt */
      /* build icmp header after IP */
      png = (struct ping *)(pkt->nb_prot + sizeof(struct ip));
      png->ptype = ECHOREQ;
      png->pseq = 1;
      png->pchksum = 0;
      if (ip_len & 1)  /* pad odd length packets  for checksum routine */
         *(pkt->nb_prot + sizeof(struct ip) + pingsize) = 0;
      png->pchksum = ~cksum(png, (pingsize + 1) >> 1);
      break;
      }
   case UDP_PROT:
      {
      unshort udplen;
      headerslen = sizeof(struct ip) + sizeof(struct udp);
      ip_len = deflength + headerslen;    /* allow for IP and ICMP headers */
      udplen= ip_len - sizeof(struct ip);
      typetext = "UDP echo";
      pkt = pkc_bldip(prot, ip_len);   /* get basic IP pkt */
      /* build UDP header after IP */
      pup = (struct udp *)(pkt->nb_prot + sizeof(struct ip));
      pup->ud_dstp = htons(7);            /* dest port is echo */
      pup->ud_srcp = htons(udp_socket()); /* get a free port for source */
      pup->ud_len = htons(udplen);        /* IP length minus IP header */
      pup->ud_cksum = 0;                  /* no cksum, leave that for TCP */
      break;
      }
#ifdef TCP_ECHOTEST
   case IPPROTO_TCP:
      err = pck_tcpconn(pio);        /* make the tcp echo connection */
      if(err)
      {
         ns_printf(pio, "TCP setup error\n");
         return -1;
      }
      headerslen = sizeof(struct ip) + sizeof(struct tcphdr);
      ip_len = deflength + headerslen;    /* allow for IP and transport headers */
      typetext = "TCP echo";
      pkt = pkc_bldtcp(ip_len, TH_ACK);
      break;
#endif /* TCP_ECHOTEST */
   default:
      dtrap("pcycles 1\n");    /* bad prot arg */
      return -1;
   }

   MEMCPY(pbuf, pkt->nb_prot, headerslen);   /* save copy of headers */

   ns_printf(pio, "Sending %d %ss into stack...\n", itterations, typetext);
   
   pcn_starttime = get_ptick();
   pcn_sends = pcn_recvs = 0;       /* clear counters */
   err = 0;

   /* main loop for sending packets. pcn_sends is incremented by the 
    * pck_pkt_send routine, which should be called before pck_xchgpkt()
    * returns. Since pck_pkt_send() does not free the packet, just keep
    * sending the same one. This saves a lot of allocating packets and 
    * copying.
    */
   while(pcn_recvs++ < itterations)
   {
      pck_xchgpkt(pkt);
      if(pcn_sends < pcn_recvs)    /* make sure we got response */
      {
         err = -1;
         ns_printf(pio, "Lost pkt #%lu, aborting.\n", pcn_recvs);
         break;
      }
      if(!pcn_active)      /* see if we got reset or something */
      {
         err = -1;
         ns_printf(pio, "reset, aborting.\n");
         break;
      }
      pc_if->mib.ifInNUcastPkts++;
      pc_if->mib.ifInOctets += ip_len;

      /* modify the TCP header for the next pass */
      if((prot == IPPROTO_TCP) && 
         (pcn_recvs < itterations))    /* skip last pass */
      {
         tcp_hdrfixup(&pbuf[0]);
      }
      MEMCPY(pkt->nb_prot, pbuf, headerslen);   /* restore headers from copy */

#ifdef DEBUG_PCKTCP
      /* Double-check the checksum modification */
      if((prot == IPPROTO_TCP) &&
         (pcn_recvs < itterations))    /* skip last pass */
      {
         u_short  tcpsum;

         ptcp = (struct tcphdr *)(pkt->nb_prot + sizeof (struct ip));
         tcpsum = ptcp->th_sum;     /* save sum to check */
         ptcp->th_sum = 0;          /* zero pkt buffer sum for computation */
         ptcp->th_sum = tcp_cksum((struct ip *)(pkt->nb_prot));
         if(ptcp->th_sum != tcpsum)
         {
            dtrap("pcycles 2\n");
            break;
         }
      }
#endif   /* DEBUG_PCKTCP */
   }

   /* If there was no error in the main loop, print results */
   if(!err)
   {
      pticks = get_ptick() - pcn_starttime;

      if(prot == IPPROTO_TCP)
         pck_tcpfin(pio);    /* close tcp connection before we printf */

      pc_printstat(pio, itterations, pticks, typetext);
   }
   pcn_active = FALSE;               /* Officially done testing */

   /* If we got an error the packet is probably freed, else free it here: */
   if(err == 0)
      pk_free(pkt);
   return err;
}

int
pkc_tcpecho(void * pio)
{
#ifdef TCP_ECHOTEST
   return(pkc_rep(pio, IPPROTO_TCP));
#else
   ns_printf(pio, "No TCP_ECHOTEST in this build\n");
   return -1;
#endif
}

int
pkc_udpecho(void * pio)
{
   return(pkc_rep(pio, UDP_PROT));
}


int
pkc_tcpsess(void * pio)
{
#ifdef TCP_ECHOTEST
   u_long   itterations;   /* number of sessions (from cmd line) */
   u_long   sessions;      /* number already done */
   u_long   pticks;
   int      err;
   int      ip_len;
   char *   arg   =  nextarg( ((GEN_IO)pio)->inbuf );
   PACKET   pkt;

   if(pc_if == NULL)
   {
      ns_printf(pio, "Open test device with pcmake first.\n");
      return -1;
   }

   if(*arg != 0)
      itterations = (u_long)atol(arg);
   else
      itterations = 1;     /* default to one */

   test_prot = 6;          /* save prot for pcn_pkt_send() call */
   pcn_active = TRUE;      /* now officially testing */
   ns_printf(pio, "Timing %d TCP echo sessions...\n", itterations);

   sessions = err = 0;
   ip_len = deflength + sizeof(struct ip) + sizeof(struct tcphdr);

   pcn_starttime = get_ptick();

   while(sessions++ < itterations)
   {
      err = pck_tcpconn(pio);        /* make the tcp echo connection */
      if(err)
      {
         ns_printf(pio, "TCP setup error\n");
         break;
      }

      pcn_sends = pcn_recvs = 0;    /* clear counters */
      pkt = pkc_bldtcp(ip_len, TH_ACK);
      pck_xchgpkt(pkt);             /* send a single echo packet */
      if(pcn_sends < pcn_recvs)     /* make sure we got response */
      {
         err = -1;
         ns_printf(pio, "Lost data pkt in loop #%lu, aborting.\n", sessions);
         break;
      }
      if(!pcn_active)      /* see if we got reset or something */
      {
         err = -1;
         ns_printf(pio, "reset, aborting.\n");
         break;
      }
      pc_if->mib.ifInNUcastPkts++;
      pc_if->mib.ifInOctets += ip_len;

   }

   pticks = get_ptick() - pcn_starttime;

   /* If there was no error in the main loop, print results */
   if(!err)
   {
      pticks = get_ptick() - pcn_starttime;
      pck_tcpfin(pio);    /* close tcp connection before we printf */
      pc_printstat(pio, itterations, pticks, "TCP echo session");
   }
   pcn_active = FALSE;               /* Officially done testing */

   return err;

#else /* TCP_ECHOTEST */
   ns_printf(pio, "No TCP in this build\n");
   return -1;
#endif
}

int
pkc_data(void * pio)
{
   ns_printf(pio, "pkt Cycle measurement status:\n");
   ns_printf(pio, "  Device is %s\n", pc_if?"open":"closed");
   if(calibration == 0)
      ns_printf(pio, "  Profile ticks not calibrated\n");
   else
      ns_printf(pio, "  Profile ticks calibrated at %d ticks/sec\n", calibration);
   if(pcn_rx_mode)
      ns_printf(pio, "  receives use rcvdq and a task switch\n");
   else
      ns_printf(pio, "  receives passed direct to ip_rcv()\n");

   return 0;
}

#endif   /* PKT_CYCLES  */