180 lines
6.3 KiB
C
180 lines
6.3 KiB
C
#include "arp_packet.h"
|
|
|
|
translation_table_entry translation_table[TB_L];
|
|
int entry_length = 0;
|
|
int entry_count = 0;
|
|
|
|
int search_entry(translation_table_entry * entry, uint32_t ip, int * table_id){
|
|
for(size_t i = 0 ; i < entry_length; i++){
|
|
if(ip == translation_table[i].ar_spa){
|
|
entry->ar_pro = translation_table[i].ar_pro;
|
|
entry->ar_spa = translation_table[i].ar_spa;
|
|
for(size_t j = 0 ; j < MAC_A_S ; j++){
|
|
entry->ar_sha[j] = translation_table[i].ar_sha[j];
|
|
}
|
|
*table_id = i;
|
|
translation_table[i].count = entry_count;
|
|
entry_count++;
|
|
return SUCCESS;
|
|
}
|
|
}
|
|
return FAILED;
|
|
}
|
|
|
|
int update_entry(int table_id, uint8_t * mac_ares){
|
|
if(table_id >= TB_L) {
|
|
perror("table_id wrong");
|
|
return FAILED;
|
|
}
|
|
for(size_t j = 0 ; j < MAC_A_S ; j++){
|
|
translation_table[table_id].ar_sha[j] = mac_ares[j];
|
|
}
|
|
return SUCCESS;
|
|
}
|
|
|
|
int insert_entry(translation_table_entry * entry){
|
|
int final_id = entry_length;
|
|
if(entry_length == TB_L){
|
|
int minn = 1<<31;
|
|
int id = -1;
|
|
for(size_t i = 0 ; i < entry_length ; i++){
|
|
if(translation_table[i].count < minn){
|
|
minn = translation_table[i].count;
|
|
id = i;
|
|
}
|
|
}
|
|
final_id = id;
|
|
}
|
|
translation_table[final_id].ar_pro = entry->ar_pro;
|
|
translation_table[final_id].ar_spa = entry->ar_spa;
|
|
for(size_t j = 0 ; j < MAC_A_S ; j++){
|
|
translation_table[final_id].ar_sha[j] = entry->ar_sha[j];
|
|
}
|
|
translation_table[final_id].count = entry_count;
|
|
entry_count++;
|
|
return SUCCESS;
|
|
}
|
|
|
|
int parse_arp_packet(mac_hdr * hdr, pkt_data * pkt, uint8_t * buf, size_t size){
|
|
size_t offset = 0;
|
|
for(size_t i=offset;i<offset+MAC_A_S;i++) hdr->dest_ares[i-offset]=buf[i]; offset += MAC_A_S;
|
|
for(size_t i=offset;i<offset+MAC_A_S;i++) hdr->send_ares[i-offset]=buf[i]; offset += MAC_A_S;
|
|
hdr->pro_T =combine_uint8(buf[offset], buf[offset+1]); offset += 2;
|
|
pkt->ar_hrd=combine_uint8(buf[offset], buf[offset+1]); offset += 2;
|
|
pkt->ar_pro=combine_uint8(buf[offset], buf[offset+1]); offset += 2;
|
|
pkt->ar_hln=buf[offset]; offset += 1;
|
|
pkt->ar_pln=buf[offset]; offset += 1;
|
|
pkt->ar_op =combine_uint8(buf[offset], buf[offset+1]); offset += 2;
|
|
for(size_t i=offset;i<offset+MAC_A_S;i++) pkt->ar_sha[i-offset]=buf[i]; offset += MAC_A_S;
|
|
pkt->ar_spa = combine_uint8_32(buf+offset); offset+=IP_A_S;
|
|
for(size_t i=offset;i<offset+MAC_A_S;i++) pkt->ar_tha[i-offset]=buf[i]; offset += MAC_A_S;
|
|
pkt->ar_tpa = combine_uint8_32(buf+offset); offset+=IP_A_S;
|
|
}
|
|
|
|
int receive_arp_packet(mac_hdr * hdr, pkt_data * pkt, uint8_t * buf, size_t size, uint8_t * local_ip, uint8_t * local_mac){
|
|
uint32_t local_ip32 = combine_uint8_32(local_ip);
|
|
|
|
if(size < 42) {
|
|
printf("The packet is too short! This is not an ARP packet!\n");
|
|
return FAILED;
|
|
}
|
|
parse_arp_packet(hdr, pkt, buf, size);
|
|
if(hdr->pro_T != ETHER_T_ARP) {
|
|
printf("The packet is not ARP packet!\n");
|
|
return FAILED;
|
|
}
|
|
if(pkt->ar_hrd != ARES_HRD_ETHERNET){
|
|
printf("The hardware is not Ethernet!\n");
|
|
return FAILED;
|
|
}
|
|
if(pkt->ar_pro!= ARES_PRO_IP){
|
|
printf("The protocol is not IPv4!\n");
|
|
return FAILED;
|
|
}
|
|
|
|
int merge_flag = FALSE;
|
|
int table_id = -1;
|
|
|
|
translation_table_entry entry;
|
|
if(search_entry(&entry, pkt->ar_spa, &table_id) == SUCCESS){
|
|
update_entry(table_id, pkt->ar_sha);
|
|
merge_flag = TRUE;
|
|
}
|
|
if(merge_flag == FALSE){
|
|
entry.ar_pro = pkt->ar_pro;
|
|
entry.ar_spa = pkt->ar_spa;
|
|
for(size_t j = 0 ; j < MAC_A_S ; j++){
|
|
entry.ar_sha[j] = pkt->ar_sha[j];
|
|
}
|
|
}
|
|
|
|
if(pkt->ar_op==ARES_OP_REQ){
|
|
printf("This is a request packet!\n");
|
|
|
|
uint32_t tmp_ip;
|
|
uint8_t tmp_mac[MAC_A_S];
|
|
tmp_ip = pkt->ar_spa; pkt->ar_spa = pkt->ar_tpa; pkt->ar_tpa = tmp_ip;
|
|
memcpy(tmp_mac, pkt->ar_sha, MAC_A_S); memcpy(pkt->ar_sha, pkt->ar_tha, MAC_A_S); memcpy(pkt->ar_tha, tmp_mac, MAC_A_S);
|
|
|
|
memcpy(pkt->ar_sha, local_mac, MAC_A_S);
|
|
pkt->ar_spa = local_ip32;
|
|
|
|
pkt->ar_op = ARES_OP_REP;
|
|
memcpy(tmp_mac, hdr->dest_ares, MAC_A_S); memcpy(hdr->dest_ares, hdr->send_ares, MAC_A_S); memcpy(hdr->send_ares, tmp_mac, MAC_A_S);
|
|
memcpy(hdr->send_ares, local_mac, MAC_A_S);
|
|
return SUCCESS;
|
|
}
|
|
printf("This is a response packet!\n");
|
|
return SUCCESS;
|
|
}
|
|
|
|
int cv_uint16_to_uint8(uint8_t * tmp, uint16_t number){
|
|
tmp[1] = number & 0x00FF;
|
|
tmp[0] = (number >> 8 ) & 0x00FF;
|
|
return SUCCESS;
|
|
}
|
|
|
|
int cv_arp_packet_to_uint8(uint8_t * resp_buffer, const pkt_data* pkt, const mac_hdr *hdr, size_t length){
|
|
if( length < sizeof(pkt_data) + sizeof(mac_hdr) ) return FAILED;
|
|
memcpy(resp_buffer, hdr->dest_ares, MAC_A_S); print_hex(resp_buffer, MAC_A_S); resp_buffer += MAC_A_S;
|
|
memcpy(resp_buffer, hdr->send_ares, MAC_A_S); print_hex(resp_buffer, MAC_A_S); resp_buffer += MAC_A_S;
|
|
uint8_t tmp[2];
|
|
cv_uint16_to_uint8(tmp, hdr->pro_T);
|
|
memcpy(resp_buffer, tmp, sizeof(uint16_t)); print_hex(resp_buffer, sizeof(uint16_t)); resp_buffer += sizeof(uint16_t);
|
|
|
|
cv_uint16_to_uint8(tmp, pkt->ar_hrd);
|
|
memcpy(resp_buffer, tmp, sizeof(uint16_t)); print_hex(resp_buffer, sizeof(uint16_t)); resp_buffer += sizeof(uint16_t);
|
|
cv_uint16_to_uint8(tmp, pkt->ar_pro);
|
|
memcpy(resp_buffer, tmp, sizeof(uint16_t)); print_hex(resp_buffer, sizeof(uint16_t)); resp_buffer += sizeof(uint16_t);
|
|
|
|
uint16_t hln_pln = (pkt->ar_hln << 8) | (pkt->ar_pln);
|
|
cv_uint16_to_uint8(tmp, hln_pln);
|
|
memcpy(resp_buffer, tmp, sizeof(uint16_t)); print_hex(resp_buffer, sizeof(uint16_t)); resp_buffer += sizeof(uint16_t);
|
|
|
|
cv_uint16_to_uint8(tmp, pkt->ar_op);
|
|
memcpy(resp_buffer, tmp, sizeof(uint16_t)); print_hex(resp_buffer, sizeof(uint16_t)); resp_buffer += sizeof(uint16_t);
|
|
|
|
memcpy(resp_buffer, pkt->ar_sha, MAC_A_S); print_hex(resp_buffer, MAC_A_S); resp_buffer += MAC_A_S;
|
|
|
|
uint8_t tmp_ip[4];
|
|
for(size_t i = 0 ; i < 4 ; i++)
|
|
tmp_ip[3-i] = ((pkt->ar_spa >> i * 8) & 0xFF);
|
|
memcpy(resp_buffer, tmp_ip, IP_A_S); print_hex(resp_buffer, IP_A_S); resp_buffer += IP_A_S;
|
|
|
|
memcpy(resp_buffer, pkt->ar_tha, MAC_A_S); print_hex(resp_buffer, MAC_A_S); resp_buffer += MAC_A_S;
|
|
|
|
for(size_t i = 0 ; i < 4 ; i++)
|
|
tmp_ip[3-i] = ((pkt->ar_tpa >> i * 8) & 0xFF);
|
|
memcpy(resp_buffer, tmp_ip, IP_A_S); print_hex(resp_buffer, IP_A_S); resp_buffer += IP_A_S;
|
|
|
|
return SUCCESS;
|
|
}
|
|
|
|
int send_arp_response(int tun_fd, uint8_t * resp_buffer, size_t length, const pkt_data * pkt, const mac_hdr * hdr){
|
|
if(cv_arp_packet_to_uint8(resp_buffer, pkt, hdr, length) == FAILED) return FAILED;
|
|
ssize_t nwrite = write(tun_fd, resp_buffer, length);
|
|
if(nwrite < 0) return FAILED;
|
|
return SUCCESS;
|
|
}
|
|
|