protocal.c

Linux 上的socket嗅探器

/*
 *
 * Copyright (C) 2003 Xiangbin Lee <[e-mail]> <[e-mail]>
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software Foundation.
 */

#include <stdio.h>
#include <string.h>

#include "ache.h"
#include "datetime.h"
#include "protocal.h"
#include "packet.h"

int ReadBits(
        unsigned char *dest,
        unsigned char *src,
        unsigned short bitsfrom,
        unsigned int bitleng)
{
    unsigned int pos=0,len=0,iob,lob;

    if(src==0||dest==0||bitleng<=0)
        return 0;

    iob=IOB(bitsfrom);
    lob=LOB(bitsfrom);
    if(bitleng<=8)
    {
        dest[0]=src[iob]<<lob;
        len=1;
    }
    else if(bitsfrom>0)
    {
        bitsfrom%=8;
        while(pos<=bitleng)
        {
            if(len==0)
                pos+=(8-bitsfrom);
            else pos+=8;
            dest[len]=(src[len+iob]<<lob)|(src[len+iob+1]>>(8-lob));
            len++;
        }
    }
    else
    {
        len=IOB(bitleng);
        if(LOB(bitleng))
            len++;
        memcpy(dest,src,len);
    }

    if(len>0)
        dest[len-1]&=(0xFF<<(LOB(bitleng)==0?0:(8-LOB(bitleng))));

    return len;
}

int CmpBits(
    unsigned char *buf1, unsigned short bf1,
    unsigned char *buf2, unsigned short bf2,
    unsigned int bitleng)
{
    unsigned int blen=0;
    unsigned char res=0;
    unsigned short iob[2],lob[2],iol,lol;

    iob[0]=IOB(bf1);  lob[0]=LOB(bf1);
    iob[1]=IOB(bf2);  lob[1]=LOB(bf2);
    iol=IOB(bitleng); lol=LOB(bitleng);

    while(blen<bitleng)
    {
        res=(buf1[iob[0]]<<lob[0])|(buf1[iob[0]+1]>>(8-lob[0]));
        res^=(buf2[iob[1]]<<lob[1])|(buf2[iob[1]+1]>>(8-lob[1]));
        blen+=8;
        if(res)
            break;
    }

    if(blen>=bitleng)
        res&=(0xFF<<(LOB(bitleng)==0?0:(8-LOB(bitleng))));

    return (int)res;
}

// exchang data and mulite child PDSs
//    A. Deal with the special head data in PDS.type
//    B. Pretranslate the data in PDS.data for parse
//    C. Parse the data and change system param for this PDS
//    D. Search child PDS protocal and set the data of child PDSes
//      E. Exchange child PDS and mark this PSD finished !

unsigned int GetPDSChildCount(PDS * lpPds)
{
    if(lpPds==0)
        return 0;
    return GetCircleAcheCount(&(lpPds->cldPDS));
}

PDS *GetCurChildPDS(PDS * lpPds)
{
    if(lpPds==0)
        return 0;

    return (PDS *)GetCurCircleAche(&(lpPds->cldPDS));
}

void RewindPDSChild(PDS * lpPds)
{
    if(lpPds==0)
        return ;
    CircleAcheResetRead(&(lpPds->cldPDS));
}

unsigned int AddPDSChild(PDS * lpPds, PDS * lpCld)
{
    if(lpPds==0||lpCld==0)
        return 0;
    AddCircleAche(&(lpCld->parPDS),(void*)lpPds);
    return AddCircleAche(&(lpPds->cldPDS),(void*)lpCld);
}

unsigned int DelPDSChild(PDS * lpPds, PDS * lpCld)
{
    if(lpPds==0||lpCld==0)
        return 0;
    DelCircleAche(&(lpCld->parPDS),(void*)lpPds);
    return DelCircleAche(&(lpPds->cldPDS),(void*)lpCld);
}

void DelallPDSChild(PDS * lpPds)
{
    PDS *lcldPds=0;
    int cldn=0,tmpn=0;

    if(lpPds==0)
        return ;

    cldn=GetCircleAcheCount(&(lpPds->cldPDS));
    for(tmpn=0;tmpn<cldn;tmpn++)
    {
        if((lcldPds=(PDS *)GetCurCircleAche(&(lpPds->cldPDS)))!=0)
            DelCircleAche(&(lcldPds->parPDS),(void*)lpPds);
    }

    ResetCircleAche(&(lpPds->cldPDS));
}

void DisbandPDS(PDS * lpPds)
{
    PDS *lcldPds=0;
    int cldn=0,tmpn=0;

    if(lpPds==0)
        return ;

    cldn=GetCircleAcheCount(&(lpPds->cldPDS));
    for(tmpn=0;tmpn<cldn;tmpn++)
    {
        if((lcldPds=(PDS *)GetCurCircleAche(&(lpPds->cldPDS)))!=0)
        {
            DelCircleAche(&(lcldPds->parPDS),(void*)lpPds);
            DisbandPDS(lcldPds);
        }
    }

    ResetCircleAche(&(lpPds->cldPDS));
}


int PDSParse(PDS * lpPds)
{
    int freei, freej, allstatic=1;

    if(lpPds==0||lpPds->p_data.length<=0)
        return -1;

    // A. Deal with the special head data in PDS.type
    if(lpPds->PF_type!=0&&lpPds->type.length>0)
    {
        freei=lpPds->PF_type(&(lpPds->type));
        if(freei!=0)
        {
            lpPds->p_data.nhbits=0;

            lpPds->p_data.length=0;
            /* Release memory */
            if(lpPds->p_data.memtype==PDSMEMTYPE_AUTO && lpPds->p_data.lpbuf!=0)
                free(lpPds->p_data.lpbuf);
            if(lpPds->p_data.memtype!=PDSMEMTYPE_STATIC)
                lpPds->p_data.lpbuf=0;

            return freei;
        }
    }

    // B. Pretranslate the data

    if(lpPds->PF_trans!=0)
    {
        PDSDATA *lpres=0;
        lpres=lpPds->PF_trans(&(lpPds->p_data));
        if(lpres!=0)
        {
            if(lpPds->p_data.memtype==PDSMEMTYPE_AUTO && lpPds->p_data.lpbuf!=0)
                free(lpPds->p_data.lpbuf);
            if(lpPds->p_data.memtype!=PDSMEMTYPE_STATIC)
                lpPds->p_data.lpbuf=0;
            lpPds->p_data.nhbits=lpres->nhbits;
            lpPds->p_data.sibits=lpres->sibits;
            lpPds->p_data.length=lpres->length;

            if(lpPds->p_data.length>0)
            {
                if(lpPds->p_data.memtype!=PDSMEMTYPE_STATIC)
                {
                    lpPds->p_data.memtype=lpres->memtype;
                    lpPds->p_data.lpbuf=lpres->lpbuf;
                }
                else
                {
                    memcpy(lpPds->p_data.lpbuf,lpres->lpbuf,BOB(lpPds->p_data.length));
                    if(lpres->memtype==PDSMEMTYPE_AUTO)
                        free(lpres->lpbuf);
                    if(lpres->memtype!=PDSMEMTYPE_STATIC)
                        lpres->lpbuf=0;
                }
            }
        }
    }

    // C. Parse the data and change system param for this PDS

    if(lpPds->p_data.length<=0)
    {
        lpPds->p_data.nhbits=0;
        lpPds->p_data.length=0;
        if(lpPds->p_data.memtype==PDSMEMTYPE_AUTO && lpPds->p_data.lpbuf!=0)
            free(lpPds->p_data.lpbuf);
        if(lpPds->p_data.memtype!=PDSMEMTYPE_STATIC)
            lpPds->p_data.lpbuf=0;
        return 0;
    }

    if(lpPds->PF_data!=0)
    {
        freei=lpPds->PF_data(&(lpPds->p_data));
        if(freei!=0)
        {
            lpPds->p_data.nhbits=0;
            lpPds->p_data.length=0;
            if(lpPds->p_data.memtype==PDSMEMTYPE_AUTO && lpPds->p_data.lpbuf!=0)
                free(lpPds->p_data.lpbuf);
            if(lpPds->p_data.memtype!=PDSMEMTYPE_STATIC)
                lpPds->p_data.lpbuf=0;
            return freei;
        }
    }


    // D. Search child PDS protocal and set the data of child PDSes

    allstatic=1;
    freei=GetCircleAcheCount(&(lpPds->cldPDS));

    if(freei>0)
    {

        PDS *lpchild=0;
        int intbytes=0, leftbits=0,ifequ=0;

        intbytes=IOB(lpPds->p_data.nhbits+lpPds->p_data.sibits);
        leftbits=LOB(lpPds->p_data.nhbits+lpPds->p_data.sibits);

        CircleAcheResetRead(&(lpPds->cldPDS));
        for(freej=0;freej<freei;freej++)
        {
            lpchild=(PDS *)GetCurCircleAche(&(lpPds->cldPDS));
            if(lpchild!=0&&lpchild!=lpPds)
            {

                lpchild->p_data.length=0;
                lpchild->p_data.nhbits=0;
                if(lpchild->p_data.memtype==PDSMEMTYPE_AUTO && lpchild->p_data.lpbuf!=0)
                    free(lpchild->p_data.lpbuf);
                if(lpchild->p_data.memtype!=PDSMEMTYPE_STATIC)
                    lpchild->p_data.lpbuf=0;

                if(lpchild->type.length>0&&lpPds->p_data.length>lpchild->type.sibits)
                {
                    ifequ=0;

                    if(CmpBits(
                        lpPds->p_data.lpbuf+intbytes,leftbits,
                        lpchild->type.lpbuf,lpchild->type.nhbits,
                        lpchild->type.length)==0)
                        ifequ=1;

                    if(ifequ)
                    {
                        unsigned short allbits = lpPds->p_data.nhbits + lpchild->type.sibits;
                        unsigned short intb=0, nonbit =0;
                        intb=IOB(allbits);
                        nonbit=LOB(allbits);
                        lpchild->p_data.length=lpPds->p_data.length-lpchild->type.sibits;
                        lpchild->p_data.nhbits=nonbit;

                        if(lpchild->p_data.memtype== PDSMEMTYPE_STATIC)
                        {
                            memcpy(lpchild->p_data.lpbuf,lpPds->p_data.lpbuf+intb,BOB(lpchild->p_data.length+nonbit));
                        }
                        else
                        {
                            allstatic=0;
                            lpchild->p_data.memtype=PDSMEMTYPE_TRANS;
                            lpchild->p_data.lpbuf=lpPds->p_data.lpbuf+intb;
                        }
                    }
                }
                else if(lpchild->type.length==0)
                {
                    lpchild->p_data.length=lpPds->p_data.length;
                    lpchild->p_data.nhbits=lpPds->p_data.nhbits;
                    if(lpchild->p_data.memtype== PDSMEMTYPE_STATIC)
                    {
                        memcpy(lpchild->p_data.lpbuf,lpPds->p_data.lpbuf,BOB(lpchild->p_data.length));
                    }
                    else
                    {
                        allstatic=0;
                        lpchild->p_data.memtype=PDSMEMTYPE_TRANS;
                        lpchild->p_data.lpbuf=lpPds->p_data.lpbuf;
                    }
                }

            }//if(lpchild!=0)
        }//for(freej=0;freej<freei;freej++)
    }//GetCircleAcheCount>0

    if(allstatic)
    {
        // Set finished
        lpPds->p_data.nhbits=0;

        lpPds->p_data.length=0;
        if(lpPds->p_data.memtype==PDSMEMTYPE_AUTO && lpPds->p_data.lpbuf!=0)
            free(lpPds->p_data.lpbuf);
        if(lpPds->p_data.memtype!=PDSMEMTYPE_STATIC)
            lpPds->p_data.lpbuf=0;
    }
    // E. Exchange child PDS
    freei=GetCircleAcheCount(&(lpPds->cldPDS));
    if(freei>0)
    {
        PDS *lpchild=0;
        CircleAcheResetRead(&(lpPds->cldPDS));
        for(freej=0;freej<freei;freej++)
        {
            lpchild=(PDS *)GetCurCircleAche(&(lpPds->cldPDS));
            if(lpchild!=0&&lpchild->p_data.length>0)
            {
                PDSParse(lpchild);
            };
        }
    }

    if(!allstatic)
    {
        // Set finished
        lpPds->p_data.nhbits=0;
        lpPds->p_data.length=0;
        if(lpPds->p_data.memtype==PDSMEMTYPE_AUTO && lpPds->p_data.lpbuf!=0)
            free(lpPds->p_data.lpbuf);
        if(lpPds->p_data.memtype!=PDSMEMTYPE_STATIC)
            lpPds->p_data.lpbuf=0;
    }

    return 0;
}