int PrintEtherHeader( struct ether_header *eh, FILE *fp)
{
char buf[80];
fprintf(fp, "ether_header------------------------------\n");
fprintf(fp, "ehter_dhost = %s\n", my_ether_ntoa_r(eh->ether_dhost, buf, sizeof(buf)));
fprintf(fp, "ether_shost = %s\n", my_ether_ntoa_r(eh->ether_shost, buf, sizeof(buf)));
fprintf(fp, "ether_type = %02X", ntohs(eh->ether_type));
switch (ntohs(eh->ether_type)) {
case ETH_P_IP:
fprintf(fp, "(IP)\n");
break;
case ETH_P_IPV6:
fprintf(fp, "(IPv6)\n");
break;
case ETH_P_ARP:
fprintf(fp, "(ARP)\n");
break;
default:
fprintf(fp, "(unkown)\n");
break;
}
return (0);
}
int chkMyProtocol(u_char *data, char *smac, char *dmac, char *sip, char *dip, u_short type, int size)
{
u_char *ptr;
int lest;
struct ether_header *eh;
ptr=data;
lest=size;
eh=(struct ether_header *)ptr;
ptr+=sizeof(struct ether_header);
lest-=sizeof(struct ether_header);
char sMACaddr[18];
char dMACaddr[18];
my_ether_ntoa_r(eh->ether_shost, sMACaddr, sizeof(sMACaddr));
my_ether_ntoa_r(eh->ether_dhost, dMACaddr, sizeof(dMACaddr));
// Check Ethernet header
if((strncmp(dMACaddr, dmac, SIZE_MAC)==0) &&
(ntohs(eh->ether_type)==MYPROTOCOL)){
MYPROTO *myproto;
myproto=(MYPROTO *) ptr;
ptr+=sizeof(MYPROTO);
lest-=sizeof(MYPROTO);
// Check Myprotocol
if(ntohs(myproto->type)==type &&
(myproto->ip_src==inet_addr("00H.00H.00H.00H")) &&
(myproto->ip_dst==inet_addr("FF.FF.FF.FFH"))){
printf("Recieve Offer Packet\n");
memcpy(smac, sMACaddr, sizeof(sMACaddr));
return(-1);
} else if(ntohs(myproto->type)==type &&
(strncmp(sMACaddr, smac, SIZE_MAC)==0) &&
(myproto->ip_src==inet_addr(sip)) &&
(myproto->ip_dst==inet_addr(dip))){
printf("Receive Approval Packet\n");
return(-1);
}
}
return(0);
}
int chkMyProtocol(u_char *data, char *smac, char *dmac, char *sip, char *dip, u_short type, int size)
{
u_char *ptr;
int lest;
struct ether_header *eh;
ptr=data;
lest=size;
eh=(struct ether_header *)ptr;
ptr+=sizeof(struct ether_header);
lest-=sizeof(struct ether_header);
char sMACaddr[18];
char dMACaddr[18];
my_ether_ntoa_r(eh->ether_shost, sMACaddr, sizeof(sMACaddr));
my_ether_ntoa_r(eh->ether_dhost, dMACaddr, sizeof(dMACaddr));
// Check Ethernet header
if((strncmp(dMACaddr, dmac, SIZE_MAC)==0) &&
(ntohs(eh->ether_type)==MYPROTOCOL)){
MYPROTO *myproto;
myproto=(MYPROTO *) ptr;
ptr+=sizeof(MYPROTO);
lest-=sizeof(MYPROTO);
// Check Myprotocol
if(ntohs(myproto->type)==type){
printf("Recieve Offer Packet\n");
memcpy(smac, sMACaddr, sizeof(sMACaddr));
memcpy(sip, inet_ntoa(*(struct in_addr *)&myproto->ip_src), SIZE_IP);
memcpy(dip, inet_ntoa(*(struct in_addr *)&myproto->ip_dst), SIZE_IP);
if(chgIfIp(NameDev1, myproto->ip_dst)==0){
//StatusFlag=2;
return(-1);
}
}
}
return(0);
}
char *getIfMac(const char *device,char *macAddr)
{
struct ifreq ifreq;
u_char tmpAddr[6];
getIfInfo(device,&ifreq,SIOCGIFHWADDR);
int i;
for(i=0;i<6;i++) tmpAddr[i]=(char)ifreq.ifr_hwaddr.sa_data[i];
my_ether_ntoa_r(tmpAddr,macAddr,SIZE_MAC);
return(macAddr);
}
int AnalyzePacket(int deviceNo, u_char *data, int size)
{
u_char *ptr;
int lest;
struct ether_header *eh;
ptr=data;
lest=size;
if(lest<sizeof(struct ether_header)) {
DebugPrintf("[%d]:lest(%d)<sizeof(struct ether_header)\n",deviceNo,lest);
return(-1);
}
eh=(struct ether_header *)ptr;
ptr+=sizeof(struct ether_header);
lest-=sizeof(struct ether_header);
DebugPrintf("[%d]",deviceNo);
if(DebugOut) {
PrintEtherHeader(eh,stderr);
}
// Get Client Mac Address
if(ClientMacFlag==OFF) {
if(ntohs(eh->ether_type)==ETHERTYPE_IP) {
struct iphdr *iphdr;
iphdr=(struct iphdr *)ptr;
ptr+=sizeof(struct iphdr);
lest-=sizeof(struct iphdr);
if(strncmp(cliIpAddr, inet_ntoa(*(struct in_addr *)&iphdr->saddr), SIZE_IP)==0) {
my_ether_ntoa_r(eh->ether_shost, cliMacAddr, sizeof(cliMacAddr));
ClientMacFlag=ON;
printf("cliMacAddr: %s\n", cliMacAddr);
}
}
}
// Check My Protocol
if(ntohs(eh->ether_type)==MYPROTOCOL) {
MYPROTO *myproto;
myproto=(MYPROTO *)ptr;
ptr+=sizeof(MYPROTO);
lest-=sizeof(MYPROTO);
switch(ntohs(myproto->type)) {
case OFFER:
;
char offr_dMacAddr[18];
char offr_sMacAddr[18];
my_ether_ntoa_r(eh->ether_dhost, offr_dMacAddr, sizeof(offr_dMacAddr));
my_ether_ntoa_r(eh->ether_shost, offr_sMacAddr, sizeof(offr_sMacAddr));
if(strncmp(offr_dMacAddr, dev1MacAddr, sizeof(offr_dMacAddr))==0) {
strncpy(apMacAddr, offr_sMacAddr, SIZE_MAC);
memcpy(dev1IpAddr, inet_ntoa(*(struct in_addr *)&myproto->ip_dst), SIZE_IP);
memcpy(apIpAddr, inet_ntoa(*(struct in_addr *)&myproto->ip_src), SIZE_IP);
if(chgIfIp(NameDev1, inet_addr(dev1IpAddr))==0) {
DebugPrintf("Change IP Address\n%s IP: %s\n", NameDev1, dev1IpAddr);
printf("Send Approval Packet\n");
create_myprotocol(Device[MainDev].soc, dev1MacAddr, apMacAddr, dev1IpAddr, apIpAddr, APPROVAL);
StatusFlag=STA_WAIT;
return(-1);
}
}
break;
default:
break;
}
}
return(0);
}
int RewritePacket (int deviceNo, u_char *data, int size)
{
u_char *ptr;
struct ether_header *eh;
int lest, len;
ptr=data;
lest=size;
if(lest<sizeof(struct ether_header)) {
DebugPrintf("[%d]:lest(%d)<sizeof(struct ether_header)\n",deviceNo,lest);
return(-1);
}
eh=(struct ether_header *)ptr;
ptr+=sizeof(struct ether_header);
lest-=sizeof(struct ether_header);
char dMacAddr[18];
char sMacAddr[18];
// Get dMAC, sMAC
my_ether_ntoa_r(eh->ether_dhost, dMacAddr, sizeof(dMacAddr));
my_ether_ntoa_r(eh->ether_shost, sMacAddr, sizeof(sMacAddr));
// physicalNIC -> wirelessNIC
if((strncmp(sMacAddr, cliMacAddr, SIZE_MAC)==0) &&
(strncmp(dMacAddr, dev3MacAddr, SIZE_MAC)==0)) {
my_ether_aton_r(apMacAddr, eh->ether_dhost);
my_ether_aton_r(dev1MacAddr, eh->ether_shost);
// Case: IP
if (ntohs(eh->ether_type)==ETHERTYPE_IP) {
struct iphdr *iphdr;
u_char option[1500];
int optLen;
iphdr=(struct iphdr *)ptr;
ptr+=sizeof(struct iphdr);
lest-=sizeof(struct iphdr);
optLen=iphdr->ihl*4-sizeof(struct iphdr);
if(optLen>0) {
memcpy(option, ptr, optLen);
ptr+=optLen;
lest-=optLen;
}
// Rewrite IP Address
if(iphdr->saddr==inet_addr(cliIpAddr)) {
iphdr->saddr=inet_addr(dev1IpAddr);
}
iphdr->check=0;
iphdr->check=calcChecksum2((u_char *)iphdr, sizeof(struct iphdr), option, optLen);
// Case : TCP
if(iphdr->protocol==IPPROTO_TCP) {
struct tcphdr *tcphdr;
len=ntohs(iphdr->tot_len)-iphdr->ihl*4;
tcphdr=(struct tcphdr *)ptr;
tcphdr->check=0;
tcphdr->check=checkIPDATAchecksum(iphdr, ptr, len);
}
// Case : UDP
if(iphdr->protocol==IPPROTO_UDP) {
struct udphdr* udphdr;
len=ntohs(iphdr->tot_len)-iphdr->ihl*4;
udphdr=(struct udphdr *)ptr;
udphdr->check=0;
}
}
}
// wirelessNIC -> physicalNIC
else if (strncmp(dMacAddr, dev1MacAddr, SIZE_MAC)==0) {
my_ether_aton_r(cliMacAddr, eh->ether_dhost);
my_ether_aton_r(dev3MacAddr, eh->ether_dhost);
// Case: IP
if (ntohs(eh->ether_type)==ETHERTYPE_IP) {
struct iphdr *iphdr;
u_char option[1500];
int optLen;
iphdr=(struct iphdr *)ptr;
ptr+=sizeof(struct iphdr);
lest-=sizeof(struct iphdr);
optLen=iphdr->ihl*4-sizeof(struct iphdr);
if(optLen>0) {
memcpy(option, ptr, optLen);
ptr+=optLen;
lest-=optLen;
}
// Rewrite IP Address
if(iphdr->daddr==inet_addr(dev1IpAddr)) {
iphdr->daddr=inet_addr(cliIpAddr);
}
iphdr->check=0;
iphdr->check=calcChecksum2((u_char *)iphdr, sizeof(struct iphdr), option, optLen);
// Case : TCP
if(iphdr->protocol==IPPROTO_TCP) {
struct tcphdr *tcphdr;
len=ntohs(iphdr->tot_len)-iphdr->ihl*4;
tcphdr=(struct tcphdr *)ptr;
tcphdr->check=0;
tcphdr->check=checkIPDATAchecksum(iphdr, ptr, len);
}
// Case : UDP
if(iphdr->protocol==IPPROTO_UDP) {
struct udphdr* udphdr;
len=ntohs(iphdr->tot_len)-iphdr->ihl*4;
udphdr=(struct udphdr *)ptr;
udphdr->check=0;
}
}
}
return(0);
}
int PrintArp(struct ether_arp *arp, FILE *fp)
{
static char *hrd[] = {
"From KA9Q: NET/ROM pseudo.",
"Ethernet 10/100Mbps.",
"Experimental Ethernet.",
"AX.25 Level 2.",
"PROnet token ring.",
"Chaosnet.",
"IEEE 802.2 Ethernet/TR/TB.",
"ARCnet.",
"APPLEtalk.",
"undefine",
"undefine",
"undefine",
"undefine",
"undefine",
"undefine",
"Frame Relay DLCI.",
"undefine",
"undefine",
"undefine",
"ATM.",
"undefine",
"undefine",
"undefine",
"Metricom STRIP (new IANA id)."
};
static char *op[] = {
"undefine",
"ARP request.",
"ARP reply.",
"RARP request.",
"RARP reply",
"undefine",
"undefine",
"undefine",
"InARP request.",
"InARP reply.",
"(ATM)ARP NAK."
};
char buf[80];
fprintf(fp, "arp----------------------------\n");
fprintf(fp, "arp_hrd = %u", ntohs(arp->arp_hrd));
if (ntohs(arp->arp_hrd) <= 23) {
fprintf(fp, "(%s),", hrd[ntohs(arp->arp_hrd)]);
} else {
fprintf(fp, "(undefined),");
}
fprintf(fp, "arp_pro = %u", ntohs(arp->arp_pro));
switch (ntohs(arp->arp_pro)) {
case ETHERTYPE_IP:
fprintf(fp, "(IP)\n");
break;
case ETHERTYPE_ARP:
fprintf(fp, "(Address resolution)\n");
break;
case ETHERTYPE_REVARP:
fprintf(fp, "(Reverse ARP)\n");
break;
case ETHERTYPE_IPV6:
fprintf(fp, "(IPv6)\n");
break;
default:
fprintf(fp, "(unknown)\n");
break;
}
fprintf(fp, "arp_hln=%u,", arp->arp_hln);
fprintf(fp, "arp_pln=%u,", arp->arp_pln);
fprintf(fp, "arp_op =%u,", ntohs(arp->arp_op));
if (ntohs(arp->arp_op) <= 10) {
fprintf(fp, "'%s)\n", op[ntohs(arp->arp_op)]);
} else {
fprintf(fp, "(undefine)\n");
}
fprintf(fp, "arp_sha=%s\n", my_ether_ntoa_r(arp->arp_sha, buf, sizeof(buf)));
fprintf(fp, "arp_spa=%s\n", arp_ip2str(arp->arp_spa, buf, sizeof(buf)));
fprintf(fp, "arp_tha=%s\n", my_ether_ntoa_r(arp->arp_tha, buf, sizeof(buf)));
fprintf(fp, "arp_tpa=%s\n", arp_ip2str(arp->arp_spa, buf, sizeof(buf)));
return (0);
}