Code:
#include <sys/socket.h>
#include <sys/ioctl.h>
#include <sys/time.h>
#include <asm/types.h>
#include "server.h"
#include "util.h"
#include <math.h>
#include <string.h>
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <signal.h>
#include <pthread.h>
#include "machdeps.h"
#include "fcframe.h"
#include <linux/if_packet.h>
#include <linux/if_ether.h>
#include <linux/if_arp.h>
#define BUF_SIZE ETH_FRAME_TOTALLEN
void * thread2(void*); /*Functions for the Threads*/
void * thread1(void*);
int FCoepipe[2]; /*The two pipes for the threads*/
int FIPpipe[2];
#define NUM_THREADS 2
pthread_t tid[NUM_THREADS]; /* array of thread IDs */
int s = 0; /*Socketdescriptor*/
long total_packets = 0;
long answered_packets = 0;
int typerror;
#define CFI 0x10
unsigned char test[4] = {0x00, 0x19, 0xB9, 0x3D};
unsigned char VLAN[2] = {0x81,0x00};
unsigned char FCoe[2] = {0x89,0x06};
unsigned char FIP[2] = {0x89,0x14};
void* buffer;
int readint;
/* The Ethernet Frame structure*/
struct Ethfrm_t
{
struct FCfrm_t* ptr;
unsigned char DestEthAdr[6];
unsigned char SrcEthAdr[6];
UINT32 VLAN_HdrSave;
UINT32 reserved;
UINT32 pad;
unsigned char Ethbuffer[BUF_SIZE];
} __attribute__ ((packed));
Ethfrm_t* FCoethread; /* pointers to the Ethernetframe for the pipes read() call */
Ethfrm_t* FIPthread;
void printEthfrm(Ethfrm_t * Ethfrm); /* prints out the error Frame*/
void deleteEthfrm(Ethfrm_t * Ethfrm); /* Freeing the Struct*/
Ethfrm_t * CopyEthfrm(Ethfrm_t * Ethfrm);
unsigned char bugger[BUF_SIZE+200];
int main(void) {
unsigned char* etherhead;
struct ethhdr *eh;
unsigned char* Etho;
unsigned char src_mac[6]; /*our MAC address*/
FCoethread = malloc(sizeof(*FCoethread));
FIPthread = malloc(sizeof(*FIPthread));
//Ethfrm_t Etherframe = malloc(sizeof(Etherframe));
Ethfrm_t* Ethfrm;// = Etherframe;
Ethfrm = malloc(sizeof(*Ethfrm));
etherhead = Ethfrm->Ethbuffer; /*Pointer to Ethenet Header*/
eh = (struct ethhdr *)etherhead; /*Another pointer to ethernet header*/
buffer = Ethfrm->Ethbuffer; /*Buffer for Ethernet Frame*/
struct ifreq ifr;
struct sockaddr_ll socket_address;
int ifindex = 0; /*Ethernet Interface index*/
int i;
int length; /*length of received packet*/
int sent;
/* Creating the two Ethernet thread Pipes*/
if (pipe( FCoepipe ) ) {
fprintf(stderr,"Pipe error!\n");
exit(1);
}
if (pipe( FIPpipe ) ) {
fprintf(stderr,"Pipe error!\n");
exit(1);
}
printf("Server started, entering initialiation phase...\n");
/*open socket*/
s = socket(AF_PACKET, SOCK_RAW, htons(ETH_P_ALL));
if (s == -1) {
perror("socket():");
exit(1);
}
printf("Successfully opened socket: %i\n", s);
/*retrieve ethernet interface index*/
strncpy(ifr.ifr_name, DEVICE, IFNAMSIZ);
if (ioctl(s, SIOCGIFINDEX, &ifr) == -1) {
perror("SIOCGIFINDEX");
exit(1);
}
ifindex = ifr.ifr_ifindex;
printf("Successfully got interface index: %i\n", ifindex);
/*retrieve corresponding MAC*/
if (ioctl(s, SIOCGIFHWADDR, &ifr) == -1) {
perror("SIOCGIFINDEX");
exit(1);
}
for (i = 0; i < 6; i++) {
src_mac[i] = ifr.ifr_hwaddr.sa_data[i];
}
printf("Successfully got our MAC address: %02X:%02X:%02X:%02X:%02X:%02X\n",
src_mac[0],src_mac[1],src_mac[2],src_mac[3],src_mac[4],src_mac[5]);
/*prepare sockaddr_ll*/
socket_address.sll_family = PF_PACKET;
socket_address.sll_protocol = htons(ETH_P_IP);
socket_address.sll_ifindex = ifindex;
socket_address.sll_hatype = ARPHRD_ETHER;
socket_address.sll_pkttype = PACKET_OTHERHOST;
socket_address.sll_halen = ETH_ALEN;
socket_address.sll_addr[6] = 0x00;
socket_address.sll_addr[7] = 0x00;
/*establish signal handler*/
signal(SIGINT, sigint);
printf("Successfully established signal handler for SIGINT\n");
printf("We are in production state, waiting for incoming packets....\n");
pthread_create(&tid[1], NULL, thread1, NULL);
pthread_create(&tid[0], NULL, thread2, NULL);
while (1) {
/*Wait for incoming packet...*/
length = recvfrom(s, buffer, BUF_SIZE, 0, NULL, NULL);
if (length == -1) {
perror("recvfrom():");
exit(1);
}
/*See if we should answer (Ethertype == 0x0 && destination address == our MAC)*/
if (eh->h_proto == ETH_P_NULL && memcmp( (const void*)eh->h_dest, (const void*)src_mac, ETH_MAC_LEN) == 0 ) {
/* Comparing the Ethertype*/
if(memcmp((void*)(etherhead+14), FCoe,2) == 0 || memcmp((void*)(etherhead+14),FIP,2) == 0){
Ethfrm->ptr = (struct FCfrm_t *)(&Ethfrm->pad); // Ethfrm pointer
} else if(memcmp((void*)(etherhead+14), VLAN,2) == 0){
Ethfrm->ptr = (struct FCfrm_t *)(&Ethfrm->reserved);
printf("VLAN HEADER\n");
if((etherhead[16]& CFI) == CFI){
printf("CFI is set:\n");
printEthfrm(Ethfrm);
continue;
}
} else {
printf("Not FCoe or FIP EtherType:\n");
printEthfrm(Ethfrm);
continue;
}
/* Setting the FCframe_t StructureStart Pointer to the EthernetFrame*/
Ethfrm->ptr->StructureStart = Ethfrm;
//printf("%02lX VLAN\n", Ethfrm->ptr->StructureStart);
/* Storing the Ethernet Destination/Source address*/
memcpy(Ethfrm->DestEthAdr, (void*)etherhead, ETH_MAC_LEN);
memcpy(Ethfrm->SrcEthAdr, (void*)(etherhead+ETH_MAC_LEN), ETH_MAC_LEN);
memcpy((unsigned char *)&Ethfrm->VLAN_HdrSave, (void*)(etherhead+(ETH_MAC_LEN*2)), 4);
/* Sending the Frames to either FCoe or FIP Threads*/
if(memcmp((void*)(etherhead+14), FCoe,2) == 0 || memcmp((void*)(etherhead+18), FCoe,2) == 0){
//Ethfrm_t* FCoeframe = CopyEthfrm(Ethfrm);
Ethfrm_t* FCoeframe = malloc(sizeof(*FCoeframe));
*FCoeframe = *Ethfrm;
write(FCoepipe[1],FCoeframe->ptr->StructureStart,sizeof(Ethfrm->ptr->StructureStart));
}
if(memcmp((void*)(etherhead+14),FIP,2) == 0 || memcmp((void*)(etherhead+18),FIP,2) == 0){
//Ethfrm_t* FIPframe = CopyEthfrm(Ethfrm);
Ethfrm_t* FIPframe = malloc(sizeof(*FIPframe));
*FIPframe = *Ethfrm;
FIPframe->ptr = (struct FCfrm_t *)(&FIPframe->pad);
FIPframe->reserved = 0x2020;
printf("Ethfrm %02X \n", Ethfrm->reserved);
printf("FIP %02X \n", FIPframe->reserved);
write(FIPpipe[1],FIPframe,sizeof(FIPframe));
}
/*exchange addresses in buffer*/
memcpy( (void*)etherhead, (const void*)(etherhead+ETH_MAC_LEN), ETH_MAC_LEN);
memcpy( (void*)(etherhead+ETH_MAC_LEN), (const void*)src_mac, ETH_MAC_LEN);
/*prepare sockaddr_ll*/
socket_address.sll_addr[0] = eh->h_dest[0];
socket_address.sll_addr[1] = eh->h_dest[1];
socket_address.sll_addr[2] = eh->h_dest[2];
socket_address.sll_addr[3] = eh->h_dest[3];
socket_address.sll_addr[4] = eh->h_dest[4];
socket_address.sll_addr[5] = eh->h_dest[5];
/*send answer*/
sent = sendto(s, buffer, length-4, 0, (struct sockaddr*)&socket_address, sizeof(socket_address));
if (sent == -1) {
perror("sendto():");
exit(1);
}
answered_packets++;
}
total_packets++;
}
}
void * thread1(void* parm){
int i;
while(1){
readint = read(FIPpipe[0], FIPthread, sizeof(FIPthread));
//for(i = 0; i <500000000; i++);
printf("FIP thread1 %02X\n", FIPthread->reserved);
//printf("read int %d\n", readint);
//deleteEthfrm(FIPthread);
}
}
void* thread2(void* parm){
while(1){
read(FCoepipe[0], FCoethread, sizeof(FCoethread));
printf("FCoe thread2 %02X\n", FCoethread->ptr->fr_ref);
//deleteEthfrm(FCoethread);
}
}
void printEthfrm(Ethfrm_t * Ethfrm){
unsigned char* etherhead = Ethfrm->Ethbuffer;
printf("EtherType error ");
for(typerror = 0; typerror<1; typerror++){
printf("%02X:",etherhead[typerror]);
}
printf("\n");
}
Ethfrm_t * CopyEthfrm(Ethfrm_t * Ethfrm){
int i;
Ethfrm_t * EthfrmTmp = malloc(sizeof(*EthfrmTmp));
EthfrmTmp->ptr= Ethfrm->ptr;
for(i = 0; i < 6; i++){
EthfrmTmp->DestEthAdr[i] = Ethfrm->DestEthAdr[i];
EthfrmTmp->SrcEthAdr[i] = Ethfrm->SrcEthAdr[i];
}
EthfrmTmp->VLAN_HdrSave = Ethfrm->VLAN_HdrSave;
EthfrmTmp->reserved = Ethfrm->reserved;
EthfrmTmp->pad = Ethfrm->pad;
for(i = 0; i < BUF_SIZE; i++){
EthfrmTmp->Ethbuffer[i] = Ethfrm->Ethbuffer[i];
}
return EthfrmTmp;
}
void deleteEthfrm(Ethfrm_t * Ethfrm){
/*
free(Ethfrm->ptr);
free(Ethfrm->DestEthAdr);
free(Ethfrm->SrcEthAdr);
free(Ethfrm->VLAN_HdrSave);
free(Ethfrm->reserved);
free(Ethfrm->pad);
free(Ethfrm->Ethbuffer);
free(Ethfrm);
*/
free(Ethfrm);
}
void sigint(int signum) {
/*Clean up.......*/
struct ifreq ifr;
if (s == -1)
return;
strncpy(ifr.ifr_name, DEVICE, IFNAMSIZ);
ioctl(s, SIOCGIFFLAGS, &ifr);
ifr.ifr_flags &= ~IFF_PROMISC;
ioctl(s, SIOCSIFFLAGS, &ifr);
close(s);
free(FCoethread->Ethbuffer);
printf("Server terminating....\n");
printf("Totally received: %d packets\n", total_packets);
printf("Answered %d packets\n", answered_packets);
exit(0);
}
Originally Posted by kiros88
