iphdr.c

嵌入式RMON,RMON为Remote monitor的缩写,基于SNMP为网络提供主动监控及错误告警,智能交换路由必备协议

/* Beholder RMON ethernet network monitor,Copyright (C) 1993 DNPAP group */
/* See file COPYING 'GNU General Public Licence' for copyright details   */
#include <stdio.h>
#include <memory.h>

#ifdef UNIX
#include <sys/types.h>
#include <netinet/in.h>
#endif

#include <dnpap.h>

#include "ip.h"


#define IP_VERSION 4

#define SWAP(a) ((WORD)((((DWORD)(((WORD)a))) << 16 | ((DWORD)(((WORD)a)))) >> 8))




typedef struct _HDR HDR;

struct _HDR
{
    BYTE        ver_ihl;
    BYTE	    tos;
    WORD        length;
	WORD	    id;
    WORD        flags_offset;
    BYTE        ttl;
    BYTE        prot;
    WORD        check;
    DWORD       src;
    DWORD       dst;
};


static DWORD SumChain(CHAIN *chain, WORD length);
static DWORD SumBuffer(WORD *buffer, WORD count);


#ifdef UNIX


WORD IpH2NWord(WORD w)
{
    return (WORD)htons(w);
}


WORD IpN2HWord(WORD w)
{
    return (WORD)ntohs(w);
}

DWORD IpH2NDWord(DWORD d)
{
    return (DWORD)htonl(d);
}

DWORD IpN2HDWord(DWORD d)
{
    return (DWORD)ntohl(d);
}

#else

WORD IpH2NWord(WORD w)
{
    return SWAP(w);
}


WORD IpN2HWord(WORD w)
{
    return SWAP(w);
}

DWORD IpH2NDWord(DWORD d)
{
    return ((DWORD)SWAP((WORD)(d >> 16)))
            | ((DWORD)SWAP((WORD)d) << 16);
}

DWORD IpN2HDWord(DWORD d)
{
    return ((DWORD)SWAP((WORD)(d >> 16)))
            | ((DWORD)SWAP((WORD)d) << 16);

}
 

#endif


CHAIN *IpHdrDecode(CHAIN *chain, IP_HDR *ipHdr)
{

    HDR     *h;
    WORD    n;

    h = (HDR *)ChainPop(&chain, sizeof(HDR));
    if (h==0)
        return 0;

    ipHdr->ver      = (BYTE)(h->ver_ihl >> 4);
    ipHdr->ihl      = (h->ver_ihl & 0x0f) * 4;
    ipHdr->tos      = h->tos;
    ipHdr->length   = IpN2HWord(h->length);
    ipHdr->id       = IpN2HWord(h->id);
    ipHdr->flags    = (BYTE)(IpN2HWord(h->flags_offset) >> 13);
    ipHdr->offset   = IpN2HWord(h->flags_offset) & 0x1fff;
    ipHdr->ttl      = h->ttl;
    ipHdr->prot     = h->prot;
    ipHdr->check    = IpN2HWord(h->check);
    ipHdr->src      = IpN2HDWord(h->src);
    ipHdr->dst      = IpN2HDWord(h->dst);
    ipHdr->iol      = ipHdr->ihl - sizeof(HDR);
    
    
    ChainPush(&chain, sizeof(HDR));

    if (IpHdrCheck(chain, ipHdr->ihl) != 0)
        return 0;

    ChainPop(&chain, sizeof(HDR));
    
    if (ipHdr->iol != 0)
    {
        ipHdr->options = (BYTE *)ChainPop(&chain, ipHdr->iol);
        if (ipHdr->options==0)
            return 0;
    }

    if (ipHdr->ver != IP_VERSION)
        return 0;

    n = ipHdr->length - ipHdr->ihl;
    if (chain->length > n)
        chain->length = n;

    return chain;
}



CHAIN *IpHdrEncode(CHAIN *chain, IP_HDR *ipHdr)
{
    HDR     *h;
    BYTE    *p;

    
    ipHdr->ver      = IP_VERSION;
    ipHdr->ihl      = sizeof(HDR) + ipHdr->iol;
    ipHdr->length   = ipHdr->ihl + ChainLength(chain);
    ipHdr->check    = 0;

    if (ipHdr->iol != 0)
    {
        p = (BYTE *)ChainPush(&chain, ipHdr->iol);
        if (p==0)
            return 0;
        memcpy(p, ipHdr->options, ipHdr->iol);
    }

    h = (HDR *)ChainPush(&chain, 20);
    if (h==0)
        return 0;

    h->ver_ihl      = (BYTE)((ipHdr->ver << 4) | (ipHdr->ihl/4 & 0x0f));
    h->tos          = ipHdr->tos;
    h->length       = IpH2NWord(ipHdr->length);
    h->id           = IpH2NWord(ipHdr->id);
    h->flags_offset = IpH2NWord((ipHdr->flags << 13) | (ipHdr->offset & 0x1fff)); 
    h->ttl          = ipHdr->ttl; 
    h->prot         = ipHdr->prot; 
    h->check        = IpH2NWord(ipHdr->check);
    h->src          = IpH2NDWord(ipHdr->src); 
    h->dst          = IpH2NDWord(ipHdr->dst);

    h->check        = IpHdrCheck(chain, ipHdr->ihl);

    return chain;
}




WORD IpHdrCheck(CHAIN *chain, WORD length)
{
    DWORD  sum = 0L;

    sum = SumChain(chain, length);
    sum = (sum >> 16) + (sum & 0xffffL);
    sum = (sum >> 16) + (sum & 0xffffL);
    
    return (WORD)~sum;
}








static DWORD SumChain(CHAIN *chain, WORD length)
{
    DWORD   sum = 0L;
    BOOLEAN odd = FALSE;
    USHORT  n;
    BYTE    *p;
    
    while (chain != 0 && length > 0)
    {
        n = chain->length;
        if (n > length)
        {
            n = length;
            length = 0;
        }
        else
        {
            length -= n;
        }

        if (n > 0)
        {
            p = chain->buffer + chain->offset;
            if (odd)
            {
                sum += (WORD)*p << 8;
                p += 1;
                n -= 1;
                odd = FALSE;
            } 
            sum += SumBuffer((WORD *)p, n >> 1);
            if (n & 1)
            {
                sum += (WORD)*(p + n - 1);
                odd = TRUE;
            }
        }
        chain = chain->next;
    }
    return sum;
}


static DWORD SumBuffer(WORD *buffer, WORD count)
{
    DWORD sum = 0L;

    while (count-- > 0)
        sum += *buffer++;
    return sum;
}