utils.c

一种AODV实现方法

/*
               Kernel AODV  v2.0
National Institute of Standards and Technology
               Luke Klein-Berndt
-----------------------------------------------------
  Version 2.0 new features:
     * Updated to AODV draft version 11
     * Managed internet gatewaying
     * Monitor wireles signal strength
     * Many bug fixes!
-----------------------------------------------------

Originally based upon MadHoc code. I am not
sure how much of it is left anymore, but MadHoc
proved to be a great starting point.

MadHoc was written by - Fredrik Lilieblad,
Oskar Mattsson, Petra Nylund, Dan Ouchterlony
and Anders Roxenhag Mail: [e-mail]

This software is Open Source under the GNU General Public Licence.

*/

#include "utils.h"

/****************************************************

   utils.h
----------------------------------------------------
Contains many misc funcations that provide
basic functionality.
****************************************************/

extern u_int32_t g_broadcast_ip;
extern u_int32_t g_my_ip;

static struct socket *iw_sock;
static struct sockaddr_in sin; //the port we are sending from

/****************************************************

   init_sock
----------------------------------------------------
Creates a socket for sending out data
****************************************************/

int init_sock(struct socket *sock, u_int32_t ip, char *dev_name)
{

    int error;
    struct ifreq interface;

    //set the address we are sending from
    memset(&sin,0,sizeof(sin));
    sin.sin_family = AF_INET;
    sin.sin_addr.s_addr=ip;
    sin.sin_port         = htons(AODVPORT);

    sock->sk->reuse =1;
    sock->sk->allocation = GFP_ATOMIC;
    sock->sk->priority = GFP_ATOMIC;

    error = sock->ops->bind(sock,(struct sockaddr*)&sin,sizeof(struct sockaddr_in));
    strncpy(interface.ifr_ifrn.ifrn_name,dev_name,IFNAMSIZ);

    if (sock_setsockopt(sock,SOL_SOCKET,SO_BINDTODEVICE, (char *) &interface, sizeof(interface))<0)
    {

    }
    if (error<0)
    {
        printk(KERN_ERR "Kernel AODV: Error, %d  binding socket. This means that some other \n",error);
        printk(KERN_ERR "        daemon is (or was a short time axgo) using port %i.\n",AODVPORT);
        return 0;
    }

    return 0;
}

/****************************************************

   close_sock
----------------------------------------------------
Closes the socket
****************************************************/
void close_sock(void)
{
    struct interface_list_entry *tmp_interface,*dead_interface;


    tmp_interface=find_first_interface_entry();
    while(tmp_interface!=NULL)
    {
        sock_release(tmp_interface->sock);
        dead_interface=tmp_interface;
        tmp_interface=tmp_interface->next;
        kfree(dead_interface);
    }
}


void init_iw_sock(void)
{
    int error;

    error = sock_create(AF_INET,SOCK_DGRAM,0,&iw_sock);
    if (error<0)
    {
        printk(KERN_ERR "Error during creation of socket; terminating, %d\n",error);

    }
}


void close_iw_sock(void)
{
    sock_release(iw_sock);
}


int set_spy()
{
    int errno;
    int			i;
    int			nbr;		/* Number of valid addresses */
    mm_segment_t oldfs;
    struct neighbor_list_entry *tmp_neigh;
    struct interface_list_entry *tmp_interface;
    struct sockaddr iw_sa[IW_MAX_SPY];
    struct iwreq		wrq;

    tmp_interface=find_first_interface_entry();

    while (tmp_interface!=NULL)
    {

        if ((tmp_interface->dev->get_wireless_stats!=NULL) && (tmp_interface->dev->do_ioctl!=NULL))
        {
            i=0;

            tmp_neigh=find_first_neighbor_list_entry();

            while ((tmp_neigh!=NULL) && ( i<IW_MAX_SPY))
            {



                if (tmp_interface->dev==tmp_neigh->dev)
                {
                    memcpy((char *) &(iw_sa[i].sa_data), (char *) &(tmp_neigh->hw_addr),sizeof(struct sockaddr));
                    i++;
                    tmp_neigh->link=101;
                }

                tmp_neigh=tmp_neigh->next;
            }

            strncpy(wrq.ifr_name, tmp_interface->dev->name, IFNAMSIZ);
            wrq.u.data.pointer = (caddr_t) &(iw_sa);
            wrq.u.data.length = i;
            wrq.u.data.flags = 0;



            oldfs = get_fs();
            set_fs(KERNEL_DS);
            errno=tmp_interface->dev->do_ioctl(tmp_interface->dev, (struct ifreq * ) &wrq,SIOCSIWSPY);
            set_fs(oldfs);

#ifndef NO_ERRORS
            if (errno<0)
                printk( "Error with SIOCSIWSPY: %d\n", errno);
#endif

        }
        tmp_interface=tmp_interface->next;

    }
}


int get_range_info(struct net_device *dev,char *		ifname, struct iw_range *	range)
{
    struct iwreq		wrq;
    char			buffer[sizeof(struct iw_range) * 2];	/* Large enough */

    /* Cleanup */
    memset(buffer, 0, sizeof(range));
    strcpy(wrq.ifr_name, ifname);
    wrq.u.data.pointer = (caddr_t) buffer;
    wrq.u.data.length = 0;
    wrq.u.data.flags = 0;

    if(dev->do_ioctl(dev, (struct ifreq * ) &wrq,SIOCGIWRANGE) < 0)
        return(-1);

    /* Copy stuff at the right place, ignore extra */
    memcpy((char *) range, buffer, sizeof(struct iw_range));

    return(0);
}


void get_wireless_stats()
{
    int n,i,has_range=0;
    char		buffer[(sizeof(struct iw_quality) +	sizeof(struct sockaddr)) * IW_MAX_SPY];
    u_int8_t temp;

    struct iwreq		wrq;
    struct neighbor_list_entry *tmp_neigh;
    struct interface_list_entry *tmp_interface;
    struct sockaddr 	hwa[IW_MAX_SPY];
    struct iw_quality 	qual[IW_MAX_SPY];
    struct iw_range	range;


    tmp_interface=find_first_interface_entry();

    while (tmp_interface!=NULL)
    {

        if ((tmp_interface->dev->get_wireless_stats!=NULL) && (tmp_interface->dev->do_ioctl!=NULL))
        {
            strncpy(wrq.ifr_name,tmp_interface->dev->name , IFNAMSIZ);
            wrq.u.data.pointer = (caddr_t) buffer;
            wrq.u.data.length = 0;
            wrq.u.data.flags = 0;

            tmp_interface->dev->do_ioctl(tmp_interface->dev,(struct ifreq * ) &wrq,SIOCGIWSPY );

            if(get_range_info(tmp_interface->dev, tmp_interface->dev->name , &(range)) >= 0)
                has_range = 1;

            n = wrq.u.data.length;

            memcpy(hwa, buffer, n * sizeof(struct sockaddr));
            memcpy(qual, buffer + n*sizeof(struct sockaddr), n*sizeof(struct iw_quality));

            for(i = 0; i < n; i++)
            {
                if(has_range && (qual[i].level != 0))
                {
                    if (range.max_qual.qual!=0)
                    {
                        temp= (u_int8_t)   ((float) (qual[i].qual) / (float) (range.max_qual.qual) *100);
                        update_link_by_hw(hwa[i].sa_data,temp);
                    }
                }
                else
                {
                    update_link_by_hw(hwa[i].sa_data,qual[i].qual);
                }
            }
        }
        tmp_interface=tmp_interface->next;
    }
}


/****************************************************

   send_datagram
----------------------------------------------------
Used to send out a UDP packet through a
socket
****************************************************/

int local_broadcast(u_int8_t ttl, void *data, int datalen)
{
    struct interface_list_entry *tmp_interface;
    struct msghdr msg;
    struct iovec iov;
    u_int64_t curr_time;
    mm_segment_t oldfs;
    int len=0;

    curr_time=getcurrtime();

    if (ttl == 0 )
        return 0;

    memset(&sin,0,sizeof(sin));
    sin.sin_family = AF_INET;
    sin.sin_addr.s_addr= g_broadcast_ip;
    sin.sin_port         = htons((unsigned short)AODVPORT);

    //define the message we are going to be sending out
    msg.msg_name     = (void *) &(sin);
    msg.msg_namelen  = sizeof(sin);
    msg.msg_iov  = &iov;
    msg.msg_iovlen   = 1;
    msg.msg_control  = NULL;
    msg.msg_controllen = 0;
    msg.msg_flags    = 0;
    msg.msg_iov->iov_len = (__kernel_size_t) datalen;
    msg.msg_iov->iov_base = (char*) data;

    tmp_interface=find_first_interface_entry();
    while(tmp_interface!=NULL)
    {
        tmp_interface->sock->sk->broadcast=1;
        tmp_interface->sock->sk->protinfo.af_inet.ttl=ttl;
        tmp_interface->last_broadcast=curr_time;

        oldfs = get_fs();
        set_fs(KERNEL_DS);

        len = sock_sendmsg(tmp_interface->sock,&msg,datalen);
	   #ifndef NO_ERROR
        if (len<0)
            printk("LOCAL_BROADCAST: Error sending! err no: %d,on interface: %s\n",len,tmp_interface->dev->name);
	    #endif
        set_fs(oldfs);
        tmp_interface=tmp_interface->next;
    }

    return len;
}


int send_message(u_int32_t dst_ip,u_int8_t ttl, void *data, int datalen)
{
    mm_segment_t oldfs;
    struct msghdr msg;
    struct interface_list_entry *tmp_interface;
    struct route_table_entry *tmp_route;
    struct iovec iov;
    u_int64_t curr_time;
    int   len;

#ifdef TRACE
    printk("SEND_DATAGRAM: Sending datagram out! \n");
#endif

    memset(&sin,0,sizeof(sin));
    sin.sin_family = AF_INET;
    sin.sin_addr.s_addr= dst_ip;
    sin.sin_port         = htons((unsigned short)AODVPORT);

    //define the message we are going to be sending out
    msg.msg_name     = (void *) &(sin);
    msg.msg_namelen  = sizeof(sin);
    msg.msg_iov  = &iov;
    msg.msg_iovlen   = 1;
    msg.msg_control  = NULL;
    msg.msg_controllen = 0;
    msg.msg_flags    = 0;
    msg.msg_iov->iov_len =  datalen;
    msg.msg_iov->iov_base = (char*) data;

#ifdef TRACE
    printk("SEND_DATAGRAM: constructed \n");
#endif



    if (ttl == 0 )
        return 0;

    curr_time=getcurrtime();
    tmp_route=find_route_table_entry(dst_ip);
    tmp_interface=find_interface_by_dev(tmp_route->dev);
    if (tmp_interface==NULL)
    {
	#ifndef NO_ERRORS
        printk("SEND_DATAGRAM: Error sending! Unable to find interface!\n");
	 #endif
        return 1;
    }
    tmp_interface->sock->sk->broadcast=0;
    tmp_interface->sock->sk->protinfo.af_inet.ttl=ttl;
    tmp_interface->last_broadcast=curr_time;
    oldfs = get_fs();
    set_fs(KERNEL_DS);

    len = sock_sendmsg(tmp_interface->sock,&msg,datalen);
    if (len<0)
    {
        printk("SEND_MESSAGE: Error sending! err no: %d, Dst: %s\n",len,inet_ntoa(dst_ip));
    }
    set_fs(oldfs);

    return 0;
}

/****************************************************

   getcurrtime
----------------------------------------------------
Returns the current time
****************************************************/
u_int64_t getcurrtime()
{
    struct timeval tv;
        u_int64_t      result;

    do_gettimeofday(&tv);

	//This is a fix for an error that occurs on ARM Linux Kernels because they do 64bits differently
	//Thanks to S. Peter Li for coming up with this fix!

    result = (u_int64_t)tv.tv_usec;
    do_div(result, 1000);
    return ((u_int64_t)tv.tv_sec) * 1000 + result;
}

/****************************************************

   inet_ntoa
----------------------------------------------------
Converts a IP address repersented in a 32 bit
unsigned int into a string
****************************************************/
char *inet_ntoa(__u32 ina)
{
    static char buf[4*sizeof "123"];
    unsigned char *ucp = (unsigned char *)&ina;
    sprintf(buf, "%d.%d.%d.%d",
            ucp[0] & 0xff,
            ucp[1] & 0xff,
            ucp[2] & 0xff,
            ucp[3] & 0xff);
    return buf;
}


/* tests to see if an address falls in the range of the Ad Hoc Subnet */
int adhoc_subnet_test( u_int32_t ina)
{

    unsigned char *ucp = (unsigned char *)&ina;
    unsigned char *uco = (unsigned char *)&g_aodv_subnet;


    if (g_aodv_subnet==0)
        return 1;

    if ((!(uco[0] & 0xff) || ((uco[0] & 0xff) == (ucp[0] & 0xff))) &&
            (!(uco[1] & 0xff) || ((uco[1] & 0xff) == (ucp[1] & 0xff))) &&
            (!(uco[2] & 0xff) || ((uco[2] & 0xff) == (ucp[2] & 0xff))) &&
            (!(uco[3] & 0xff) || ((uco[3] & 0xff) == (ucp[3] & 0xff))))
        return 1;
    else
        return 0;
}


int seq_less_or_equal(u_int32_t seq_one,u_int32_t seq_two)
{
    int *comp_seq_one = &seq_one;
    int *comp_seq_two = &seq_two;

    if (  ( *comp_seq_one - *comp_seq_two ) > 0 )
    {
        return 0;
    }
    else
        return 1;
}


int seq_greater(u_int32_t seq_one,u_int32_t seq_two)
{
    int *comp_seq_one = &seq_one;
    int *comp_seq_two = &seq_two;

    if (  ( *comp_seq_one - *comp_seq_two ) < 0 )
        return 0;
    else
        return 1;
}



/****************************************************

   inet_aton
----------------------------------------------------
Converts a string into a 32-bit unsigned int
****************************************************/
int inet_aton(const char *cp, __u32 *addr)
{
    unsigned int val;
    int                     base,
    n;
    char            c;
    u_int           parts[4];
    u_int      *pp = parts;

    for (;;)
    {

        //Collect number up to ``.''. Values are specified as for C:
        // 0x=hex, 0=octal, other=decimal.

        val = 0;
        base = 10;
        if (*cp == '0')
        {
            if (*++cp == 'x' || *cp == 'X')
                base = 16, cp++;
            else
                base = 8;
        }
        while ((c = *cp) != '\0')
        {
            if (isascii(c) && isdigit(c))
            {
                val = (val * base) + (c - '0');
                cp++;
                continue;

            }
            if (base == 16 && isascii(c) && isxdigit(c))
            {
                val = (val << 4) +
                      (c + 10 - (islower(c) ? 'a' : 'A'));
                cp++;
                continue;
            }
            break;
        }
        if (*cp == '.')
        {
            // Internet format: a.b.c.d a.b.c       (with c treated as
            // 16-bits) a.b         (with b treated as 24 bits)

            if (pp >= parts + 3 || val > 0xff)
                return (0);
            *pp++ = val, cp++;
        }
        else
            break;
    }

    // Check for trailing characters.

    if (*cp && (!isascii(*cp) || !isspace(*cp)))
        return (0);


    // Concoct the address according to the number of parts specified.

    n = pp - parts + 1;
    switch (n)
    {

    case 1:                 // a -- 32 bits
        break;

    case 2:                 //a.b -- 8.24 bits
        if (val > 0xffffff)
            return (0);
        val |= parts[0] << 24;
        break;

    case 3:                 //a.b.c -- 8.8.16 bits
        if (val > 0xffff)
            return (0);
        val |= (parts[0] << 24) | (parts[1] << 16);
        break;

    case 4:                 // a.b.c.d -- 8.8.8.8 bits
        if (val > 0xff)
            return (0);
        val |= (parts[0] << 24) | (parts[1] << 16) | (parts[2] << 8);
        break;
    }
    if (addr)
        *addr= htonl(val);
    return (1);
}