icmp.c

来自「嵌入式RMON,RMON为Remote monitor的缩写,基于SNMP为网络」· C语言 代码 · 共 211 行

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


typedef struct _HDR  HDR;

struct _HDR
{
    BYTE        type;
    BYTE        code;
    WORD        check;
};



static BOOLEAN  Rcve(CHAIN *chain, IP_HDR *ipHdr);
static CHAIN    *HdrDecode(CHAIN *chain, ICMP_HDR *icmpHdr);
static CHAIN    *HdrEncode(CHAIN *chain, ICMP_HDR *icmpHdr);
static ICMP_DESCR *DescrFind(ICMP_HDR *icmpHdr, IP_HDR *ipHdr);
static WORD Wildcards(ICMP_DESCR *icmp);


IP_PROT icmpIp =
{
    Rcve,
    IP_PROT_ICMP
};


ICMP_DESCR *icmpDescrList = 0;


BOOLEAN IcmpInit(void)
{
    static BOOLEAN init = FALSE;
    
    if (!init)
    {
        init = IpProtRegister(&icmpIp);
    }

    return init;
}




BOOLEAN IcmpRegister(ICMP_DESCR *icmp)
{
    ICMP_DESCR **p;


    for (p=&icmpDescrList; *p!=0; p=&(*p)->next)
    {
        if (Wildcards(*p) > Wildcards(icmp))
            break;
    }
    icmp->next = *p;
    *p = icmp;

    return TRUE;
}


BOOLEAN IcmpRemove(ICMP_DESCR *icmp)
{
    ICMP_DESCR **p;

    for (p=&icmpDescrList; *p!=0; p=&(*p)->next)
    {
        if (*p == icmp)
            *p = icmp->next;
        return TRUE;
    }

    return FALSE;
}

BOOLEAN IcmpSend(CHAIN *chain, ICMP_HDR *icmpHdr, IP_HDR *ipHdr)
{
    CHAIN       *new;
    BOOLEAN     success = FALSE;
    
    new = HdrEncode(chain, icmpHdr);
    if (new != 0)
    {
        ipHdr->prot     = IP_PROT_ICMP;
        ipHdr->offset   = 0;
        ipHdr->flags    = 0;
        ipHdr->tos      = 0;
        if(IpSend(new, ipHdr))
        {
            success = TRUE;
        }
        if (new != chain)
            ChainFree(new);
    }
    
    return success;
}




static BOOLEAN Rcve(CHAIN *chain, IP_HDR *ipHdr)
{
    CHAIN       *new;
    ICMP_HDR    icmpHdr;
    ICMP_DESCR  *descr;
    BOOLEAN     success = FALSE;
    
    new = HdrDecode(chain, &icmpHdr);
    if (new != 0)
    {
        descr = DescrFind(&icmpHdr, ipHdr);
        if(descr)
        {
            if(descr->Rcve(descr, chain, &icmpHdr, ipHdr))
            {
                success = TRUE;
            }
        }
        if (new != chain)
            ChainFree(new);
    }
    
    return success;
}


static CHAIN *HdrDecode(CHAIN *chain, ICMP_HDR *icmpHdr)
{
    HDR *h;
    

    if (IpHdrCheck(chain, ChainLength(chain)) != 0)
        return 0;
    
    h = (HDR *)ChainPop(&chain, sizeof(HDR));
    if (h==0)
        return 0;
    
    icmpHdr->type  = h->type;
    icmpHdr->code  = h->code;
    icmpHdr->check = IpN2HWord(h->check);

    return chain;
}


static CHAIN *HdrEncode(CHAIN *chain, ICMP_HDR *icmpHdr)
{
    HDR *h;
    
    h = (HDR *)ChainPush(&chain, sizeof(HDR));
    if (h==0)
        return 0;

    h->type     = icmpHdr->type;  
    h->code     = icmpHdr->code;  
    h->check    = 0;

    h->check    = IpHdrCheck(chain, ChainLength(chain));
    
    return chain;
}




static ICMP_DESCR *DescrFind(ICMP_HDR *icmpHdr, IP_HDR *ipHdr)
{
    ICMP_DESCR *p;


    for (p=icmpDescrList; p!=0; p=p->next)
    {
        if (
                (p->locAddr==ICMP_ADDR_ANY || p->locAddr==ipHdr->dst)    &&
                (p->remAddr==ICMP_ADDR_ANY || p->remAddr==ipHdr->src)    &&
                (p->type==ICMP_TYPE_ANY || p->type==icmpHdr->type)    &&
                (p->code==ICMP_CODE_ANY || p->type==icmpHdr->code) 
           ) 
        {
            return p;
        }
    }
   
    return 0;
}

                                 
static WORD Wildcards(ICMP_DESCR *icmp)
{
    WORD n=0;

    if (icmp->locAddr == ICMP_ADDR_ANY)
        n++;
    if (icmp->remAddr == ICMP_ADDR_ANY)
        n++;
    if (icmp->type == ICMP_TYPE_ANY)
        n++;
    if (icmp->code == ICMP_CODE_ANY)
        n++;

    return n;
}