rawiptst.c

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

/*
 * FILENAME: rawiptst.c
 *
 * Copyright 1995-2002 InterNiche Technologies Inc. All rights reserved.
 *
 * Test program for raw IP sockets API.  Implements raw IP 
 * testers which send and receive datagrams from raw sockets.
 *
 * MODULE: MISCLIB
 *
 * ROUTINES: raw_testerq_lock(), raw_testerq_unlock(),
 * ROUTINES: raw_tester_new(), raw_tester_del(), raw_tester_delid(),
 * ROUTINES: raw_tester_findid(), 
 * ROUTINES: raw_testerq_poll(),
 * ROUTINES: raw_ristart(), raw_ridelete(), raw_rilist(), raw_risend(),
 * ROUTINES: raw_rihalt(), raw_ribind(), raw_riconnect(), raw_richeck(),
 * ROUTINES: raw_rihdrincl(), raw_test_init()
 *
 * PORTABLE: yes
 */

#include "ipport.h"     /* NetPort embedded system includes */

#ifdef RAWIPTEST        /* whole file can be ifdef'd out */

#include "tcpport.h"    /* NetPort embedded system includes */
#include "in_utils.h"
#include "dns.h"
#include "menu.h"

/* RAWIPTEST_BUFLEN - length of buffer used for receives and sends
 *
 * Ideally, this should be large enough to hold the largest datagram
 * you expect to send or receive, including its IP header.
 *
 * You can override this definition from ipport.h if desired.
 */
#ifndef RAWIPTEST_BUFLEN
#define RAWIPTEST_BUFLEN      1536
#endif  /* RAWIPTEST_BUFLEN */

/* build up a sockets-API abstraction
 */
#ifndef SOCKTYPE
#define SOCKTYPE        long
#endif /* SOCKTYPE */

#define  socket(x,y,z)           t_socket(x,y,z)
#define  bind(s,a,al)            t_bind(s,a)
#define  connect(s,a,al)         t_connect(s,a)
#define  send(s,b,l,f)           t_send(s,b,l,f)
#define  sendto(s,b,l,f,a,al)    t_sendto(s,b,l,f,a)
#define  recvfrom(s,b,l,f,a,al)  t_recvfrom(s,b,l,f,a)
#define  socketclose(s)          t_socketclose(s)
#define  getsockopt(s,l,o,d,dl)  t_getsockopt(s,o,d)
#define  setsockopt(s,l,o,d,dl)  t_setsockopt(s,o,d)

/* raw_tester_buf[] - buffer used for receives and sends
 */
static char raw_tester_buf[RAWIPTEST_BUFLEN];

extern   ip_addr  activehost;    /* default host to send to */
extern   u_long   pingdelay;     /* time (ticks) to delay between sends */
extern   int      deflength;     /* default send data length */

static   char *   echodata =    "UDP echo number:            ";

/* List of error codes used by this file */
#define  RITE_BASE                  (-9100)
#define  RITE_ALLOC_ERROR           (RITE_BASE+1)
#define  RITE_SOCKET_FAILED         (RITE_BASE+2)
#define  RITE_SETOPT_FAILED         (RITE_BASE+3)
#define  RITE_TESTER_NOTFOUND       (RITE_BASE+4)

#define  SUCCESS                    0

/* struct raw_tester - raw IP socket tester
 */
struct raw_tester
{
   struct raw_tester * next;     /* next raw_tester in queue */
   void * pio;                   /* where to write console output */
   unsigned id;                  /* tester identification */
   SOCKTYPE sock;                /* socket for this tester */
   unsigned conn : 1;            /* flag: socket is connected */
   unsigned sending : 1;         /* flag: tester is sending */
   unsigned rxcheck : 1;         /* flag: check received data */
   unsigned long sendtoaddr;     /* IP address (network order) to send to */
   long sendcount;               /* how many sends to do */
   long sendsdone;               /* how many sends done */
   int sendlen;                  /* # bytes to send each time */
   unsigned long senddelay;      /* ticks to delay after each send */
   unsigned long lastsendtime;   /* tick count at last send */
   int senderr;                  /* socket error that stopped sending */
};

/* raw_testerq - queue of testers
 */
static struct raw_tester * raw_testerq = NULL;

/* raw_testerq_locked - flag indicating that raw_testerq is locked
 *                      for modification
 */
static volatile int raw_testerq_locked = 0;

/* raw_tester_idseq - identification sequence for raw tester
 */
static unsigned raw_tester_idseq = 0;

/* FUNCTION: raw_testerq_lock()
 *
 * Unconditionally locks the raw_testerq, blocking until locked.
 *
 * RETURNS: void
 */
void
raw_testerq_lock(void)
{
   do {
      while (raw_testerq_locked)
         tk_yield();
      raw_testerq_locked++;
      if (raw_testerq_locked != 1)
         raw_testerq_locked--;
   } 
   while (raw_testerq_locked != 1);
}

/* FUNCTION: raw_testerq_unlock()
 *
 * Unlocks the raw_testerq.
 *
 * RETURNS: void
 */
void
raw_testerq_unlock(void)
{
   raw_testerq_locked--;
}

/* FUNCTION: raw_tester_new()
 *
 * Creates a new raw IP tester.  Allocates memory for a new 
 * raw_tester structure, fills it in, links it to the queue.
 */
int
raw_tester_new(void * pio, 
               int protocol)
{
   struct raw_tester * newtester;
   int e;

   /* allocate storage for the tester state */
   newtester = (struct raw_tester *) npalloc(sizeof(struct raw_tester));
   if (newtester == NULL)
   {
      ns_printf(pio, "raw_tester_new: npalloc() failed\n");
      return RITE_ALLOC_ERROR;
   }

   /* give it a new ID */
   newtester->id = raw_tester_idseq++;

   /* keep track of where to write console output */
   newtester->pio = pio;

   /* make a socket of given protocol */
   newtester->sock = socket(PF_INET, SOCK_RAW, protocol);
   if (newtester->sock == -1)
   {
      ns_printf(pio, "raw_tester_new: [%u] socket() failed\n",
                newtester->id);
      npfree(newtester);
      return RITE_SOCKET_FAILED;
   }

   /* put the socket in non-blocking mode 
    * NOTE this needs changing for non-InterNiche stacks
    */
   e = t_setsockopt(newtester->sock, SO_NBIO, NULL);
   if (e == -1)
   {
      ns_printf(pio, 
                "raw_tester_new: [%u] t_setsockopt() failed, err %d\n",
                newtester->id, t_errno(newtester->sock));
      socketclose(newtester->sock);
      npfree(newtester);
      return RITE_SETOPT_FAILED;
   }

   /* lock the tester queue */
   raw_testerq_lock();

   /* link the tester state into the queue */
   newtester->next = raw_testerq;
   raw_testerq = newtester;

   /* unlock the tester queue */
   raw_testerq_unlock();

   /* and return the ID of the new tester */
   return newtester->id;
}

/* FUNCTION: raw_tester_del()
 *
 * Deletes a raw IP tester.  Removes it from the tester queue,
 * closes its socket (if open), frees its storage.
 * Assumes tester queue is locked.
 *
 * PARAM1: struct raw_tester * tester; IN- ptr to tester to delete
 * 
 * RETURNS:
 *
 */
int
raw_tester_del(struct raw_tester * tester)
{
   struct raw_tester * qtester;     /* current entry in queue */
   struct raw_tester * pqtester;    /* previous entry in queue */

   /* locate the tester in the queue */
   for (pqtester = NULL, qtester = raw_testerq; 
        qtester != NULL;
        pqtester = qtester, qtester = qtester->next)
      if (qtester == tester)
         break;

   /* if we didn't find it, bail out */
   if (qtester == NULL)
      return RITE_TESTER_NOTFOUND;

   /* unlink it from the queue */
   if (pqtester == NULL)
      raw_testerq = qtester->next;
   else
      pqtester->next = qtester->next;

   /* if it's got a socket, close it */
   if (qtester->sock != INVALID_SOCKET)
   {
      socketclose(qtester->sock);
   }

   /* free its storage */
   npfree(qtester);

   /* and return success */
   return SUCCESS;
}

/* FUNCTION: raw_tester_delid()
 *
 * Deletes a raw IP tester.  Removes it from the tester queue,
 * closes its socket (if open), frees its storage.
 * Assumes tester queue is locked.
 *
 * PARAM1: unsigned id; IN- ID of tester to delete
 * 
 * RETURNS: integer indicating success or failure.
 */
int
raw_tester_delid(unsigned id)
{
   struct raw_tester * qtester;     /* current entry in queue */
   struct raw_tester * pqtester;    /* previous entry in queue */

   /* locate the tester in the queue */
   for (pqtester = NULL, qtester = raw_testerq; 
        qtester != NULL;
        pqtester = qtester, qtester = qtester->next)
      if (qtester->id == id)
         break;

   /* if we didn't find it, bail out */
   if (qtester == NULL)
      return RITE_TESTER_NOTFOUND;

   /* unlink it from the queue */
   if (pqtester == NULL)
      raw_testerq = qtester->next;
   else
      pqtester->next = qtester->next;

   /* if it's got a socket, close it */
   if (qtester->sock != INVALID_SOCKET)
   {
      socketclose(qtester->sock);
   }

   /* free its storage */
   npfree(qtester);

   /* and return success */
   return SUCCESS;
}

/* FUNCTION: raw_tester_findid()
 *
 * Deletes a raw IP tester.  Removes it from the tester queue,
 * closes its socket (if open), frees its storage.
 * Assumes tester queue is locked.
 *
 * PARAM1: unsigned id; IN- ID of tester to delete
 * 
 * RETURNS: integer indicating success or failure.
 */
struct raw_tester *
raw_tester_findid(unsigned id)
{
   struct raw_tester * qtester;     /* current entry in queue */

   /* locate the tester in the queue */
   for (qtester = raw_testerq; 
        qtester != NULL;
        qtester = qtester->next)
      if (qtester->id == id)
         break;

   /* return pointer to it (or NULL if not found) */
   return qtester;
}

/* FUNCTION: raw_testerq_poll()
 *
 * Polls all the raw testers for work
 *
 * RETURNS: void
 */
void
raw_testerq_poll(void)
{
   struct raw_tester * tester;
   struct raw_tester * nexttester;
   int len;
   struct sockaddr_in addr;
   struct ip * pip;
   unsigned addrlen;
   char * dptr;
   unsigned donedata;
   int hdrincl;
   int match;
   int e;

   /* only proceed if we can lock the tester queue immediately */
   if (raw_testerq_locked++)
   {
      raw_testerq_locked--;
      return;
   }

   /* iterate through the testers, checking for work to do */
   for (tester = raw_testerq; tester != NULL; tester = nexttester)
   {

      /* keep track of which tester is next,
       * so we know what to do next 
       * even if we have to delete the current one due to error
       */
      nexttester = tester->next;

      if (tester->sock != INVALID_SOCKET)
      {
         do 
         {
            /* find out whether the socket has the IP_HDRINCL option set
             */
            e = getsockopt(tester->sock, IPPROTO_IP, IP_HDRINCL,
                           &hdrincl, sizeof(hdrincl));
            if (e < 0)
            {
               ns_printf(tester->pio,
                         "raw_testerq_poll: [%d] getsockopt error %d\n",
                         tester->id, t_errno(tester->sock));
               tester->sending = 0;
               break;
            }

            /* check the tester for received data */
            addrlen = sizeof(addr);
            len = recvfrom(tester->sock, 
                           raw_tester_buf, sizeof(raw_tester_buf), 0,
                           (struct sockaddr *)&addr, &addrlen);
            if (len < 0)
            {
               e = t_errno(tester->sock);
               if (e != EWOULDBLOCK)
               {
                  ns_printf(tester->pio, 
                            "raw_testerq_poll: [%d] recvfrom() error %d\n",
                            tester->id, e);
                  e = raw_tester_del(tester);
               }
               break;
            }

            /* if requested, verify received data
             * this leaves match set as follows:
             *   0 if the received data is not verified
             *   1 if the received data matches what we send
             *   -1 if the received data does not match what we send
             */
            match = 0;
            if (tester->rxcheck)
            {
               dptr = raw_tester_buf;
               if (hdrincl)
               {
                  pip = (struct ip *)dptr;
                  dptr += (pip->ip_ver_ihl & 0xf) << 2;
               }
               donedata = 0;
               while (dptr < raw_tester_buf + len)
               {
                  if (*dptr != (char)(donedata & 0xff))
                  {
                     match = -1;
                     break;
                  }
                  dptr++;
                  donedata++;
               }
               if (match == 0)
                  match = 1;
            }

            ns_printf(tester->pio,
                      "raw_testerq_poll: [%d] rcvd %d bytes %sfrom %u.%u.%u.%u\n",
                      tester->id, len, 
                      (match > 0) ? "(OK) " : (match < 0) ? "(not OK) " : "",
                      PUSH_IPADDR(addr.sin_addr.s_addr));

         }