pcycles.c

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

/*
 * FILENAME: pcycles.c
 *
 * Copyright  2001 By InterNiche Technologies Inc. All rights reserved
 *
 * Routines to measure processing time for various types of packets.
 *
 * MODULE: MISCLIB
 *
 *
 * PORTABLE: yes
 */


#include "ipport.h"

#ifdef   PKT_CYCLES     /* whole file can be ifdeffed */
    
#ifndef USE_PROFILER
#error need USE_PROFILER to use PKT_CYCLES
#endif

#ifndef DYNAMIC_IFACES
#error need DYNAMIC_IFACES to use PKT_CYCLES
#endif

#include "in_utils.h"
#include "menu.h"
#include "profiler.h"
#include "netbuf.h"
#include "net.h"
#include "ip.h"
#include "icmp.h"
#include "udp.h"
#include "tcpport.h"
#include "tcp.h"

#ifdef NOTDEF
#undef INCLUDE_TCP
#define DEBUG_PCKTCP
#endif

extern   void     ip_bldhead(PACKET p, 
   unsigned pid, u_char prot, unshort fragword);
extern   int   deflength;

/* pseudo iface to generate traffic and measure respose times */
NET      pc_if;
static int     pcn_rx_mode = 0;     /* 0 == direct to ip_rcv(); 1 == via pktdemux */
static u_long  pcn_starttime;       /* pticks at start of current transaction */
static int     pcn_active;          /* TRUE if we're activly testing */

/* Per-session send and receive counters. They are named from 
 * the stack's perspective, i.e. a packet passed to this code
 * from the stack is a "send"
 */
static   u_long   pcn_sends;           /* echo replies in current session */
static   u_long   pcn_recvs;           /* echo replies in current session */

static   char *   cyname = "cy0";      /* iface name */
static   int      pcn_ipid = 0;        /* ID for IP packets */
static   u_char   test_prot;           /* protocol of current test */
static   u_long   calibration = 0;     /* pticks/sec */

/* TCP port numbers for connection */
static   unshort  client_port = 1024;
static   unshort  echo_port = 7;

/* sequence and ack numbers, from client's perspective */
static   u_long   tcpseq;
static   u_long   tcpack;

static   u_char   tcp_sentflags;    /* last flags passed to pkt_send() */
static   PACKET   pkc_bldtcp(int ip_length, u_char flags);


/* internal to this file */
int      pkc_rep(void * pio, u_char prot);
PACKET   pkc_bldip(u_char prot, int length);



/* MAC routines to implement to pseudo net */

/* Standard MAC net initialization routine */

int
pcn_n_init(int iface)
{
   dtrap("pcycles 0\n");   /* never called??? */
   USE_ARG(iface);
   return 0;
}

/* Standard MAC PACKET send routine */

int
pcn_pkt_send(struct netbuf * pkt)
{
   pcn_sends++;         /* echo replies in current session */

   if(test_prot == IPPROTO_TCP)
   {
      struct ip * pip;
      struct tcphdr * ptcp;
      int datalen;

      /* extract and save TCP fields needed to interact with stack */
      pip = (struct ip *)(pkt->nb_prot);
      ptcp = (struct tcphdr *)ip_data(pip);
      datalen = (int)htons(pip->ip_len);
      datalen -= (sizeof(struct ip) + ((ptcp->th_doff & 0xf0) >> 2));
      tcp_sentflags = ptcp->th_flags;

      tcpseq = htonl(ptcp->th_ack);    /* next SEQ is after his ACK */
      tcpack = htonl(ptcp->th_seq)  + datalen;  /* ack data */

#ifdef DEBUG_PCKTCP
      dprintf("send (tcp) pkt:%p flags: %4x, seq:%lx ack %lx pkt-len %d\n", 
        pkt, tcp_sentflags, tcpseq, tcpack, pkt->nb_plen);
#endif /* DEBUG_PCKTCP */

      /* if he sent RESET bit then break the test loop */
      if(tcp_sentflags & TH_RST)
      {
         pcn_active = FALSE;
         return 0;
      }

      /* if he sent a SYN or FIN we need to add it to data sequence */
      if(tcp_sentflags & (TH_SYN|TH_FIN))
      {
         tcpack++;
      }

      /* if this is a packet with no data then the stack has not saved 
       * a pointer to it for a later retry. This means we must free
       * it now:
       */
      if(htons(pip->ip_len) ==
         (((pip->ip_ver_ihl & 0x0F) + GET_TH_OFF((*ptcp))) << 2))
      {
         pk_free(pkt);
      }

      /* If the test loop is done then this is probably a straggling
       * FIN pkt or a retry of some sort. Ack it now. We deliver the
       * packet via the rcvdq to avoid potential intinite recursion
       * issues.
       */
      if(!pcn_active)
      {
         pkt = pkc_bldtcp(40, TH_ACK);   /* build ack of FIN */
         if(pkt)
         {
            putq(&rcvdq, (qp)pkt);     /* queue ACK of FIN for stack.  */
            SignalPktDemux();          /* make sure it spins */
         }
      }
   }

   return 0;
}

/* Standard MAC close routine */

int
pcn_n_close(int iface)
{
   NET ifp = if_getbynum(iface);
   if(ifp == NULL)
      return ENP_PARAM;

   ifp->mib.ifAdminStatus = ifp->mib.ifOperStatus = NI_DOWN;
   return 0;
}



/* callback routine for cycle net creation */
int
pcn_init(NET ifp, void * bindinfo)
{
   int   iface;
   
   ifp->n_init = pcn_n_init;           /* set normal callback routines */
   ifp->pkt_send = pcn_pkt_send;
   ifp->n_close = pcn_n_close;
   ifp->n_lnh = 0;                     /* No mac header */
   ifp->mib.ifPhysAddress = (u_char*)"";  /* null MAC address */
   ifp->mib.ifType = SLIP;             /* fake SLIP - simple and headerless */
   ifp->n_lnh = 0;                     /* net's local net header  size */
   ifp->n_mtu = 1500;                  /* largest legal buffer size */
   ifp->mib.ifType = LOOPIFTYPE;
   ifp->mib.ifDescr = (u_char*)"Packet timing test device";
   ifp->n_flags = NF_NBPROT;

   /* set a default local network address */
   ifp->n_ipaddr = htonl(0xC0800101);
   ifp->snmask = htonl(0xFFFFFF00);

   /* add a locally configured route to this iface */
   iface = if_netnumber(ifp);
   if(add_route(ifp->n_ipaddr, ifp->snmask, ifp->n_ipaddr, 
            iface, IPRP_LOCAL) == NULL)
   {
      return ENP_RESOURCE;
   }

   USE_VOID(bindinfo);
   return 0;
}


int
pkc_start(void * pio)
{
   int      err;        /* error holder */

   err = ni_create(&pc_if, pcn_init, cyname, NULL);
   if(err)
   {
      ns_printf(pio, "error %d creating cycle measure device\n", err);
      return (err);
   }

   ns_printf(pio, "Created cycle measure device %s\n", cyname);
   return 0;
}

   /* calibrate pticks to real time */

int
pkc_calibrate(void * pio)
{
   u_long   ptime;      /* scratch ptick holder */
   u_long   ctime;      /* end ctick for ptick calibration */
   int      seconds;    /* number of seconds to run */
   char *   arg   =  nextarg( ((GEN_IO)pio)->inbuf );

   seconds = atoi(arg);
   if(seconds <= 0)
   {
      ns_printf(pio, "usage: calibrate X (X is number of seconds)\n");
      return -1;
   }
   ns_printf(pio, "calibrating pticks...");

   ctime = cticks;
   while(cticks == ctime)     /* wait for next ctick to start */
      tk_yield();

   ptime = get_ptick();       /* get current value of fast hardware timer */
   ctime = cticks + (TPS * (u_long)seconds); 
                              /* get slow real time stop value */

   while(ctime != cticks)     /* wait desired number of seconds */
      tk_yield();

   ptime = get_ptick() - ptime;    /* get elapsed pticks */

   calibration = ptime/seconds;
   ns_printf(pio, "done, %lu pticks/sec\n", calibration);
   
   return 0;
}


int
pkc_del(void * pio)
{
   int err;
   
   /* Since this is not real hardware we don't really need a gracefull
    * shutdown. Just delete the dynamic iface
    */
   if(pc_if == NULL)
      err = -1;
   else
   {
      err = ni_delete(pc_if);
      pc_if = NULL;
   }

   ns_printf(pio, "Deleting cycle iface, code\n");
   return(err);
}

/* toggle pcn_rx_mode. */

int
pkc_mode(void * pio)
{
   pcn_rx_mode = !pcn_rx_mode;

   ns_printf(pio, "Pkt Cycle tests set to use %s.\n",
      pcn_rx_mode?"rcvdq and pktdemux":"direct ip_rcv call");

   return 0;
}



/* pkc_bldip() - build the fake received packet IP layer. Building the
 * transport header is handled in the various routine below. IP header
 * is built at pkt->nb_prot and nb_plen is adjusted.
 */

PACKET
pkc_bldip(u_char prot, int length)
{
   PACKET      pkt;
   struct ip * pip;

   pkt = pk_alloc(length + MaxLnh);
   if(!pkt)
      return NULL;
   pkt->net = pc_if;
   pkt->nb_plen = length;
   pkt->nb_prot = pkt->nb_buff + MaxLnh + sizeof(struct ip);
   pkt->type = IP_TYPE;

   pip = (struct ip *)pkt->nb_prot;

   ip_bldhead(pkt, pcn_ipid++, prot, 0);
   pip = (struct ip *)pkt->nb_prot;
   pip->ip_len = htons((unshort)length);

   pip->ip_src = pip->ip_dest = pc_if->n_ipaddr;

   pip->ip_chksum = 0;
   pip->ip_chksum = ~cksum(pip, 10);

   return pkt;
}


void
pck_xchgpkt(PACKET pkt)
{
   if(pcn_rx_mode == 0)    /* direct feed to IP */
      ip_rcv(pkt);
   else     /* send it up through the rcvdq */
   {
      putq(&rcvdq, (qp)pkt);  /* enqueue pkt */
      SignalPktDemux();       /* wake demuxer task */
      tk_yield();             /* let demux task spin */
   }
}

#ifdef TCP_ECHOTEST  /* next few routines needed only for TCP */

extern SOCKTYPE esvr_sock;    /* Echo server socket in tcp_echo.c */

/* pkc_bldtcp() - build a tcp packet based on passed parameters. This
 * allocates it's own packet buffer and returns it.
 *
 * Returns NULL if error
 */

static PACKET
pkc_bldtcp(int ip_length, u_char flags)
{
   PACKET pkt;
   struct tcphdr * ptcp;

   pkt = pkc_bldip(IPPROTO_TCP, ip_length);
   if(!pkt)
      return NULL;

   ptcp = (struct tcphdr *)(pkt->nb_prot + 20);

   ptcp->th_sport = htons(client_port);
   ptcp->th_dport = htons(echo_port);
   ptcp->th_seq = htonl(tcpseq);
   ptcp->th_ack = htonl(tcpack);
   ptcp->th_doff = 0x50;
   ptcp->th_win = htons(0x2000);
   ptcp->th_urp = 0;
   ptcp->th_sum = 0;
   ptcp->th_flags = flags;
   ptcp->th_sum = tcp_cksum((struct ip *)(pkt->nb_prot));

#ifdef DEBUG_PCKTCP
   dprintf("bldpkt (tcp) flags: %4x, seq:%lx ack %lx, ip-len:%d\n", 
        flags, tcpseq, tcpack, ip_length);
#endif   /* DEBUG_PCKTCP */

   return pkt;
}

/* pck_tcpconn() - make the tcp echo connection. 
 */

int
pck_tcpconn(void * pio)
{
   PACKET pkt;
   u_long conn_tmo;

   client_port++;          /* use a different port each time */
   tcpseq = 0x00100000;    /* reset sequence number */

    /* make and send a syn packet */
   pkt = pkc_bldtcp(40, TH_SYN);
   if(!pkt)
   {
      ns_printf(pio, "pck_tcpconn: no pkt\n");
      return -1;
   }
   pck_xchgpkt(pkt);

   /* Make sure that stack responded properly */
   if(tcp_sentflags != (TH_SYN|TH_ACK))
   {
      ns_printf(pio, "pck_tcpconn: no syn-ack\n");
      return -1;
   }
   
   /* make and send the ACK packet */
   pkt = pkc_bldtcp(40, TH_ACK);
   if(!pkt)
   {
      ns_printf(pio, "pck_tcpconn: no pkt\n");
      return -1;
   }