utils.c

altera epxa1的例子程序

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

#include "stripe.h"
#include "serial.h"
#include "adam.h"

#include "utils.h"




USHORT bitrev16(USHORT din)
{
    return ((din & 0x00ff) << 8) | ((din & 0xff00) >> 8);
}

ULONG bitrev32(ULONG din)
{
    return ((din & 0x000000ff) << 24) |
           ((din & 0x0000ff00) << 8)  |
           ((din & 0x00ff0000) >> 8)  |
           ((din & 0xff000000) >> 24);
}

USHORT checksum(UCHAR * buf, USHORT len)
{
    USHORT  i;
    ULONG   sum = 0x0;
    USHORT  din;

    for (i = 0; i < len; i += 2)
    {
        din = bitrev16(*(USHORT *)(buf + i));
        sum = sum + din;
    }
    sum = sum + ((sum >> 16) & 0xffff);
    sum &= 0xffff;
    sum ^= 0xffff;

    return (USHORT)sum;
}

// convert MAC address from string "12:34:56:78:9a:bc" to memory UCHAR mac[6]
int str2hex(UCHAR * instr, UCHAR * hex)
{
    int i,j,len;
    UCHAR   temphex[6];

    len = strlen((char *)instr);
    for(i=0,j=0; j<5; j++)
    {
        temphex[j] = atoi((char *)&instr[i]);
        if (instr[i] == '*') temphex[j] = '*';
        while ((instr[i++] != ':') && (i<len));
    }
    temphex[j] = atoi((char *)&instr[i]);
    if (i>=len)
    {
        return(1);
    }
    strncpy((char *)hex,(char *)temphex,6);
    return(0);
}

// convert IP address from string "198.9.200.21" to memory UCHAR ip[4]
int str2dec(UCHAR * instr, UCHAR * dec)
{
    int i,j,len;
    UCHAR   tempdec[4];

    len = strlen((char *)instr);
    for(i=0,j=0; j<3; j++)
    {
        tempdec[j] = atoi((char *)&instr[i]);
        while ((instr[i++] != '.') && (i<len));
    }
    tempdec[j] = atoi((char *)&instr[i]);
    if (i>=len)
    {
        return(1);
    }
    strncpy((char *)dec,(char *)tempdec,4);
    return(0);
}

void anykey()
{
    char key;
    flushall();
    printf("\rPress any key\n");
    key = receivechar();
}

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

void dump_buf(UCHAR *buf, int len)
{
    int     i,j;
    UCHAR   asc;
    USHORT hex;

    for(i = 0x00; i < len; i+= 0x10)
    {
        printf("%08lx  : ",(ULONG)buf+i);
        for(j=i; j<i+0x10; j++)
        {
            hex = buf[j];
            if (j < len)
                printf("%02x ",hex);
            else
                printf("   ");
        }
        printf("  ");
        for(j=i; j<i+0x10; j++)
        {
            asc = buf[j];
            if ((asc<0x20) || (asc>0x7f) || (j>=len)) asc = 0x20;
            printf("%c",asc);
        }
        printf("\n");
    }
}

void dump_buf32(ULONG buf, int len)
{
    int     i,j;
    ULONG hex;

    for(i = 0x00; i < len; i+= 0x10)
    {
        printf("%08lx  : ",buf+i);
        for(j=i; j<i+0x10; j+=4)
        {
            hex = *(ULONG *)(buf+j);
            if (j < len)
                printf("%08lx ",hex);
            else
                printf("   ");
        }
        printf("\n");
    }
}

void dump_pkt(pkt_type * pktp, int len)
{
    int     i,j;
    UCHAR   asc;
    USHORT hex;

    // dst addr
    for(i=0x00; i<0x05; i++)
        printf("%02x:",pktp->mac.dst_addr[i]);
    printf("%02x  ",pktp->mac.dst_addr[i]);
    // src addr
    for(i=0x00; i<0x05; i++)
        printf("%02x:",pktp->mac.src_addr[i]);
    printf("%02x  ",pktp->mac.src_addr[i]);

    // eth type
    printf("%02x%02x\n",pktp->mac.type[0],pktp->mac.type[1]);


    for(i = 0x0e; i < len; i+= 0x10)
    {
        printf("%04x  : ",i-0x0e);
        for(j=i; j<i+0x10; j++)
        {
            hex = pktp->buffer[j];
            if (j < len)
                printf("%02x ",hex);
            else
                printf("   ");
        }
        printf("  ");
        for(j=i; j<i+0x10; j++)
        {
            asc = pktp->buffer[j];
            if ((asc<0x20) || (asc>0x7f) || (j>=len)) asc = 0x20;
            printf("%c",asc);
        }
        printf("\n");
    }
}

void decode_pkt(pkt_type * pktp, int len)
{
    int     i;
//    int     j;
//    UCHAR   asc;
//    USHORT hex;
    USHORT  pkt_type;

    printf("\nMAC HEADER\n");
    // dst addr
    printf("dst mac: ");
    for(i=0x00; i<0x05; i++)
        printf("%02x:",pktp->mac.dst_addr[i]);
    printf("%02x",pktp->mac.dst_addr[i]);

    // src addr
    printf("   src mac: ");
    for(i=0x00; i<0x05; i++)
        printf("%02x:",pktp->mac.src_addr[i]);
    printf("%02x",pktp->mac.src_addr[i]);

    // eth type
    pkt_type = bitrev16(((USHORT *)pktp->mac.type)[0]);
    if (pkt_type >= 0x800)
        printf("   type  : %04x - ",pkt_type);
    else
        printf("   length: %04x   ",pkt_type);

    switch (pkt_type)
    {
        case ARP_TYPE :
            printf("ARP\n");
            decode_arp((arp_type *)pktp->arp.hardware_type);
            break;
        case IP_TYPE :
            printf("IP\n");
            decode_ip((ip_type *)&pktp->ip.vers_hlen);
            if (pktp->ip.protocol == TCP_PROTOCOL)
            {
                decode_tcp((tcp_type *)pktp->tcp.src_port);
                decode_data((raw_type *)pktp->tcp.data,bitrev16(((USHORT *)pktp->ip.len)[0])-20-20);
            }
            else if (pktp->ip.protocol == UDP_PROTOCOL)
            {
                decode_udp((udp_type *)pktp->udp.src_port);
                decode_data((raw_type *)pktp->udp.data,bitrev16(((USHORT *)pktp->udp.length)[0])-8);
            }
            else if (pktp->ip.protocol == ICMP_PROTOCOL)
            {
                decode_icmp((icmp_type *)&pktp->icmp.type);
                decode_data((raw_type *)&pktp->icmp.data[0],bitrev16(((USHORT *)pktp->ip.len)[0])-20-8);
            }
            else
            {
                decode_data((raw_type *)&pktp->raw.data,bitrev16(((USHORT *)pktp->ip.len)[0])-20);
            }
            break;
        default :
            printf("\n");
            decode_data((raw_type *)&pktp->buffer[0xe],len);
/*
            for(i = 0x0e; i < len; i+= 0x10)
            {
                printf("%04x  : ",i-0x0e);
                for(j=i; j<i+0x10; j++)
                {
                    hex = pktp->buffer[j];
                    if (j < len)
                        printf("%02x ",hex);
                    else
                        printf("   ");
                }
                printf("  ");
                for(j=i; j<i+0x10; j++)
                {
                    asc = pktp->buffer[j];
                    if ((asc<0x20) || (asc>0x7f) || (j>=len)) asc = 0x20;
                    printf("%c",asc);
                }
                printf("\n");
            }