Subversion Repositories 1g_ethernet_dpi

//////////////////////////////////////////////////////////////////////////////////

#include <stdlib.h>

#include <stdio.h>

#include "ether.h" // ETH_PKT_MAC_ADDR_LEN, ..

#include "eth_host_bfm.h" // eth_frm_write_wait, ..

//////////////////////////////////////////////////////////////////////////////////

// IPv4: name+id LIST

typedef struct {

    char id;

    char name[20];

} ipv4_prot_map_t;

const ipv4_prot_map_t ipv4_prot_map[] = {

    {IPv4_ICMP,         "IPv4_ICMP"},

    {IPv4_IGMP,         "IPv4_IGMP"},

    {IPv4_TCP,          "IPv4_TCP"},

    {IPv4_UDP,          "IPv4_UDP"},

    {IPv4_RDP,          "IPv4_RDP"},

    {IPv4_IRTP,         "IPv4_IRTP"},

    {IPv4_IPv6Encaps,   "IPv4_IPv6Encaps"},

    {IPv4_IPv6Route,    "IPv4_IPv6Route"},

    {IPv4_IPv6Frag,     "IPv4_IPv6Frag"},

    {IPv4_QNX,          "IPv4_QNX"},

    {IPv4_SMP,          "IPv4_SMP"},

    {IPv4_SCTP,         "IPv4_SCTP"},

    {IPv4_UDPLite,      "IPv4_UDPLite"}

};

//////////////////////////////////////////////////////////////////////////////////

// 

extern unsigned short crc_ip(void * ptr, unsigned count);

extern long crc_ether(char *buf, int len);

// 

void ether_tx_raw(char *ibuff, int ilen);

void ether_tx_ipv4(char *ibuff, int ilen);

//////////////////////////////////////////////////////////////////////////////////

//

// Ethernet TX routine

//

void ether_tx(char *ibuff, int ilen)

{

    // 

    unsigned short pkt_type = *((unsigned short *)((ibuff)+ETH_PKT_MAC_ADDR_LEN*2));

    switch(pkt_type) {

        case ETH_PKT_ARP : {

            printf("ether_tx: ETH_PKT_ARP, len=%04x\n", ilen);

            ether_tx_raw(ibuff, ilen);

            break;

        }

        case ETH_PKT_IPv4 : {

            ip_hdr_t *ptr_ipv4_hdr = (ip_hdr_t *)(ibuff+ETH_HDR_LEN);

            int i, prot=0;

            for (i = 0; i < (sizeof(ipv4_prot_map))/(sizeof(ipv4_prot_map_t)); i++) {

                if (ipv4_prot_map[i].id == ptr_ipv4_hdr->ip_prot) {

                    printf("ether_tx: %s, len=%04x\n", ipv4_prot_map[i].name, ilen);

                    prot++;

                }

            }

            if (prot == 0) { // not found in list

                printf("ether_tx: ETH_PKT_IPv4: IP protocol number = %x, len=%04x\n", ptr_ipv4_hdr->ip_prot, ilen);

            }

            /*

            switch(ptr_ipv4_hdr->ip_prot) {

                case IPv4_ICMP : {

                    printf("ether_tx: ETH_PKT_IPv4-ICMP, len=%04x\n", ilen);

                    break;

                }

                case IPv4_UDP : {

                    printf("ether_tx: ETH_PKT_IPv4-UDP, len=%04x\n", ilen);

                    break;

                }

                default : {

                    printf("ether_tx: ETH_PKT_IPv4: IP protocol number = %x, len=%04x\n", ptr_ipv4_hdr->ip_prot, ilen);

                    break;

                }

            }

            */

            ether_tx_ipv4(ibuff, ilen);

            break;

        }/*

        case ETH_PKT_IPv6 : {

            printf("ether_tx: ETH_PKT_IPv6, len=%04x\n", ilen);

            ether_tx_raw(ibuff, ilen);

        }*/

        default : {

            /*printf("ether_tx: pkt-type=%04x, len=%04x\n", ntohs(pkt_type), ilen);

            ether_tx_raw(ibuff, ilen);*/

            break;

        }

    }

}

//////////////////////////////////////////////////////////////////////////////////

//

// ARP+others

//

void ether_tx_raw(char *ibuff, int ilen)

{

    int i;

    // apr-len == 42 in case if {apr-len < ETH_MIN_TU} -> we will have rejection inside MAC

    int len_raw = (ilen < ETH_MIN_TU)? ETH_MIN_TU : ilen;

    char *ptr_raw = (char *)calloc(len_raw, sizeof(char)); // <- MEM-alloc

    if (ptr_raw == NULL) { return; }

    for (i = 0; i < ETH_MIN_TU; i++) { *(ptr_raw+i) = 0; }

    // data-copy

    for (i = 0; i < ilen; i++) {

        *(ptr_raw+i) = *(ibuff+i);

    }

    // upld

    for (i = 0; i < len_raw; i++) {

        eth_frm_write(*(ptr_raw+i), i);

    }

    // fcs

    int fcs = crc_ether(ptr_raw, len_raw);

    eth_frm_write(((fcs >>  0) & 0xFF), len_raw+0);

    eth_frm_write(((fcs >>  8) & 0xFF), len_raw+1);

    eth_frm_write(((fcs >> 16) & 0xFF), len_raw+2);

    eth_frm_write(((fcs >> 24) & 0xFF), len_raw+3);

    // len + STA

    eth_frm_write_len(len_raw+4); //+ FCS

    // 

    free(ptr_raw); // -> MEM-free

}

//////////////////////////////////////////////////////////////////////////////////

//

// IPv4-UDP

//

void ether_tx_ipv4(char *ibuff, int ilen)

{

    // check len

    char *ptr_ipv4;

    int i, tx_len;

    if (ilen < ETH_MIN_TU){

        ptr_ipv4 = (char *)calloc(ETH_MIN_TU, sizeof(char)); // <- MEM-alloc

        if (ptr_ipv4 == NULL) { return; }

        for (i = 0; i < ETH_MIN_TU; i++) { *(ptr_ipv4+i) = 0; }

        for (i = 0; i < ilen; i++) { *(ptr_ipv4+i) = *(ibuff+i); }

        tx_len = ETH_MIN_TU;

    } else {

        ptr_ipv4 = ibuff;

        tx_len = ilen;

    }

    // ipv4-csum

    ip_hdr_t *ptr_ipv4_hdr = (ip_hdr_t *)(ibuff+ETH_HDR_LEN);

    ptr_ipv4_hdr->ip_chksum = 0;

    unsigned short ipv4_xsum = ~crc_ip((char *)ptr_ipv4_hdr, ETH_IPv4_HDR_LEN >> 1);

    ptr_ipv4_hdr->ip_chksum = ipv4_xsum;

    // udp-csum

    udp_hdr_t *ptr_udp_hdr = (udp_hdr_t *)(ibuff+ETH_HDR_LEN+ETH_IPv4_HDR_LEN);

    ptr_udp_hdr->ud_cksum = 0;

    // upld

    for (i = 0; i < tx_len; i++) {

        eth_frm_write(*(ptr_ipv4+i), i);

    }

    // fcs

    int fcs = crc_ether(ptr_ipv4, tx_len);

    eth_frm_write(((fcs >>  0) & 0xFF), tx_len+0);

    eth_frm_write(((fcs >>  8) & 0xFF), tx_len+1);

    eth_frm_write(((fcs >> 16) & 0xFF), tx_len+2);

    eth_frm_write(((fcs >> 24) & 0xFF), tx_len+3);

    // len + STA

    eth_frm_write_len(tx_len+4); //+ FCS

    // 

    if (ilen < ETH_MIN_TU){

        free(ptr_ipv4); // -> MEM-free

    }

}

//////////////////////////////////////////////////////////////////////////////////