ping.c

一个简单的ISOS模块代码。A simple ISOS module code

/* ###########################################################################
# (c) Copyright Virata Limited 2001
#
# Virata Limited Confidential and Proprietary
#
# The following software source code ("Software") is strictly confidential and
# is proprietary to Virata Limited ("Virata").  It may only be read, used,
# copied, adapted, modified or otherwise dealt with by you if you have
# entered into a confidentiality agreement with Virata and then subject to the
# terms of that confidentiality agreement and any other applicable agreement
# between you and Virata.  If you are in any doubt as to whether you are
# entitled to access, read, use, copy, adapt, modify or otherwise deal with
# the Software or whether you are entitled to disclose the Software to any
# other person you should contact Virata.  If you have not entered into a
# confidentiality agreement with Virata granting access to this Software you
# should forthwith return all media, copies and printed listings containing
# the Software to Virata.
#
# Virata reserves the right to take legal action against you should you breach
# the above provisions.
#
# If you are unsure, or to report violations, please contact
# [e-mail]
# ##########################################################################*/

/*
 * ISOS Ping - very simple ping support
 *
 * Ideas for extensions:
 *     Specify IP address in format of your choice (e.g. as wrapper for
 *         do_ping), not just as const char *
 *     Add an argument to send up to N pings, rather than default constant
 *     Add an argument to configure timeout (max length of time to
 *         wait for a reply), rather than default constant
 *     More advanced error reporting (i.e. report wider range of errors)
 *     Verbose output
 *     Support more ICMP options; flood pings
 *     Ping by hostname, not just by IP address
 *     ...and so on.
 */

#include <atypes.h>
#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <messages.h>
#include <errno.h>
#include <net/if.h>
#include <netinet/in.h>
#include <netinet/ip_icmp.h>
#include <netinet/icmp6.h>
#include <sys/socket.h>
#include <arpa/inet.h>
#include <netdb.h>
#include <ip.h>

#include "ping.h"


#if ATIC_SUPPORT_IPV6
#define PING_SUPPORT_IPV6
#else
#undef PING_SUPPORT_IPV6
#endif


/* Length of the message buffers */
#define PING_MSG_BUFFER_LEN     1500

/* Macro to output string messages to user */
#define PRINTF(x...) \
    do {                            \
        foreground_output_begin();  \
        fprintf(stdout, "%C: " x);  \
        foreground_output_end();    \
    } while (0)


static int ping_prepare_packet(int family, int fd, char *p_buf,
                               int *p_buf_len);


/*
 * ping_message_handler() is used by the socket library to manage
 * all non-socket-related messages received
 * As the only messages are ping requests or tell messages we just
 * reply to the message with an error. The sender can then retry 
 * if they want
 */
static void ping_message_handler(ATMOS_MESSAGE *msg)
{
    if (!(msg->code & MSG_REPLY_BIT))
    {
        msg->errno = EINTR;
        sendreply(msg);
    }
}

int
main(void)
{
    /* Register ourself with the console */
    console_register_process(atmos_pcb_current_get_name(), FALSE);

    /*
     * Since ping uses the socket library, it must also register
     * a handler function for non-socket messages such as new tell messages or
     * ping request messages
     */
    socket_register_message_handler(&ping_message_handler);

    while (TRUE) /* Forever loop */
    {
        ATMOS_MESSAGE *msg = awaitmessage();

        switch (msg->code)
        {
            case MSG_N_TELL:
            {
                MSG_D_TELL(data, msg);
                do_ping(data->cmd);
                sendreply(msg);
                break;
            }
            case MSG_N_PING:
            {
                MSG_D_PING(data, msg);
                msg->errno = do_ping_v4_v6(data->dest_addr, data->ifname);
                sendreply(msg);
                break;
            }
            default:
            {
                PRINTF("Unhandled message code %0x\n", msg->code);
                if (!(msg->code & MSG_REPLY_BIT))
                {
                    sendreply(msg);
                }
            }
        } /* end switch */
    
    } /* end forever loop */

} /* main */


/* do_ping_v4_v6 --
 *     simple 'ping' algorithm implmentation for IPv4 and IPv6
 *
 * PARAMETERS:
 *     dest   - destination IP (v4 or v6) address in string notation
 *     ifname - outgoing interface name
 *              (for IPv6 link-local destination)
 *
 * RETURNS:
 *     error code 
 */
int
do_ping_v4_v6(const char *dest, const char *ifname)
{
    /* Destination address storage */
    struct sockaddr_storage to;
    
    /* String with address */
    /* Assume INET6_ADDRSTRLEN > INET_ADDRSTRLEN */
    char    to_str[INET6_ADDRSTRLEN];

    int     err = ESUCCESS;              /* Error code */
    int     family = AF_INET;            /* Protocol/address family */
    int     protocol;                    /* Raw socket protocol */
    int     status;                      /* Socket API call return code */
    int     fd;                          /* Socket */
    int     total = PING_MSG_BUFFER_LEN; /* Transmit buffer length */
    char   *pkt;                         /* Buffer for transmit */
    
    struct timeval  timeout;    /* Ping roundtrip timeout */
    int             attempt;    /* Number of current attempt */

    /* Preset destination address storage by zeros */
    memset(&to, 0, sizeof(to));
    /* Try to convert destination address in IPv4 */
    if (inet_pton(AF_INET, dest,
                  &(((struct sockaddr_in *)&to)->sin_addr)) == 1)
    {
        ((struct sockaddr *)&to)->sa_family = AF_INET;
    }
#ifdef PING_SUPPORT_IPV6
    /* Try to convert destination address in IPv6 */
    else if (inet_pton(AF_INET6, dest,
                       &(((struct sockaddr_in6 *)&to)->sin6_addr)) == 1)
    {
        ((struct sockaddr *)&to)->sa_family = AF_INET6;
    }
#endif    
    else
    {
#ifdef DNS_CLIENT
        struct hostent *hptr;
        int             error_num;
        int             flags = 0;
#ifdef PING_SUPPORT_IPV6
        family = AF_INET6;
        flags = AI_DEFAULT;
#endif
        hptr = getipnodebyname(dest, family, flags, &error_num);
        if ((hptr != NULL) && (hptr->h_addr_list != NULL) &&
            (hptr->h_addr_list[0] != NULL))
        {
            if ((hptr->h_addrtype == AF_INET6) &&
                (IN6_IS_ADDR_V4MAPPED(hptr->h_addr_list[0])))
            {
                ((struct sockaddr *)&to)->sa_family = AF_INET;
                MAKE_INADDR_BY_IN6ADDR_V4(hptr->h_addr_list[0], 
                    &((struct sockaddr_in *)&to)->sin_addr);
            }
            else
            {
                /* Copy type of address */
                ((struct sockaddr *)&to)->sa_family = hptr->h_addrtype;
                /* Copy address */
                memcpy((hptr->h_addrtype == AF_INET) ?
                          (void *)&((struct sockaddr_in *)&to)->sin_addr :
                          (void *)&((struct sockaddr_in6 *)&to)->sin6_addr,
                       hptr->h_addr_list[0], hptr->h_length);
            }
            /* Free host entry structure */                
            freehostent(hptr);
        }
        else
#endif
        {
#ifdef DNS_CLIENT
            if (hptr != NULL)
            {
                /* Free host entry structure */
                freehostent(hptr);
            }
#endif
            PRINTF("Invalid IP address specified.\n");
            return EINVAL;
        }
    }
   
    /* Get address family and related ICMP protocol number */
    family = ((struct sockaddr *)&to)->sa_family;
    protocol = (family == AF_INET) ? IPPROTO_ICMP :
               (family == AF_INET6) ? IPPROTO_ICMPV6 :
               (err = EINVAL);
    if (err != 0)
    {
        PRINTF("Unknown address family %u.\n", family);
        return err;
    }

#ifdef PING_SUPPORT_IPV6
    /* 
     * For IPv6 link-local destination address additional 
     * parameter is necessary - outgoing interface name
     */
    if ((family == AF_INET6) &&
        IN6_IS_ADDR_LINKLOCAL(&(((struct sockaddr_in6 *)&to)->sin6_addr)))
    {
        if (ifname == NULL)
        {
            PRINTF("Outgoing interface must be specified for IPv6 "
                   "link-local destination.\n");
            return EINVAL;            
        }
        else if ((((struct sockaddr_in6 *)&to)->sin6_scope_id =
                     if_nametoindex(ifname)) == 0)
        {
            PRINTF("Unknown interface name '%s'.\n", ifname);
            return EINVAL;            
        }        
    }
#else
    UNUSED(ifname);
#endif    

    /* Convert address from binary to string format */
    if (inet_ntop(family,
                  (family == AF_INET)
                  ? (const void *)(&(((struct sockaddr_in *)&to)->sin_addr))
                  : (const void *)(&(((struct sockaddr_in6 *)&to)->sin6_addr)),
                  to_str, sizeof(to_str)) == NULL)
    {
        PRINTF("Could not convert source address.\n");
        return -1;
    }

    /* Get a Raw socket for the ICMP protocol */
    fd = socket(family, SOCK_RAW, protocol);
    if (fd < 0)
    {
        PRINTF("Unable to create ICMP socket.\n");
        return EINVAL;
    }

    /* It's better to allocate memory dynamic */
    pkt = malloc(PING_MSG_BUFFER_LEN);
    if (pkt == NULL)
    {
        PRINTF("Memory allocation failure\n");
        return ENOMEM;
    }
    
    /* Prepare ICMP packet */
    err = ping_prepare_packet(family, fd, pkt, &total);
    if (err != 0)
    {
        free(pkt);
        PRINTF("Unable to create packet.\n");
        return err;
    }

    PRINTF("PING %s: %d data bytes\n", to_str, PING_EXTRA_DATA_LEN);

    /* 
     * Change symbol DEFAULT_NUM_PING_ATTEMPTS or parameterise
     * the function to increase the number of ping attempts from 1
     */
    for (attempt = 0; attempt < DEFAULT_NUM_PING_ATTEMPTS; attempt++)
    {
        /* Send the ICMP packet... */
        status = sendto(fd, pkt, total, 0, (struct sockaddr *)&to, sizeof(to));
        if (status != total)
        {
            PRINTF("Ping to host %s failed: %s\n", to_str, strerror(errno));
            err = -1;
            break;
        }

        /* 
         * Now wait for something to come back or the nominated timeout
         * to expire (currently defaults to 4 seconds, but could easily
         * paramterise the function and dispense with the constant)
         */
        timeout.tv_sec = DEFAULT_PING_TIMEOUT;
        timeout.tv_usec = 0;

        while (TRUE) /* Forever loop */
        {
            /* Set of file descriptors to be passed in select to read */
            fd_set  read;

            /* Length of the ICMP6 message */
            int     len = 0;
            
            /* Source address of the received message */
            struct sockaddr_storage from;
            /* String with address */
            /* Assume INET6_ADDRSTRLEN > INET_ADDRSTRLEN */
            char    from_str[INET6_ADDRSTRLEN];

            /* Buffer for incoming message */            
            u_int8_t    p_buf[PING_MSG_BUFFER_LEN];

            /* Send request and receive reply time */
            struct timeval  time_send, time_recv;
            /* Request-reply round trip time */            
            unsigned long   roundtrip;
            
            
            /* Prepare set of file descriptors to read from */
            FD_ZERO(&read);
            FD_SET(fd, &read);

            /* Record when we called select */
            gettimeofday(&time_send, NULL);

            /* Wait for a packet or a timeout... */
            status = select(fd + 1, &read, NULL, NULL, &timeout);
            if (status == 0)
            {
                PRINTF("Request timed out.\n");
                err = ETIMEDOUT;
                break;
            }
            else if(status == -1)
            {
                PRINTF("Select call failed.\n");
                err = EINVAL;
                break;
            }

            /* 
             * We've received a packet. Calculate how long
             * we have been in the select, and adjust the timeout,
             * in case we have to enter the select again.
             */
            gettimeofday(&time_recv, NULL);
            /* Calculate round trip time - in the time_recv */
            tv_sub(&time_recv, &time_send);
            /* Calculate timeout value for the next select call */
            tv_sub(&timeout, &time_recv);

            /* Receive packet */
            len = sizeof(from);
            status = recvfrom(fd, (char *)p_buf, sizeof(p_buf), 0,
                              (struct sockaddr *)&from, &len);
            if (status < 0)
            {
                PRINTF("Receive failed: recvfrom\n");
                err = EINVAL;
                break;
            }
            
            /* Convert address from binary to string format */