212 lines
6.2 KiB
C
212 lines
6.2 KiB
C
#include "inttypes.h"
|
|
#include "pvec.h"
|
|
#include "arp.h"
|
|
#include "ether.h"
|
|
#include "memory.h"
|
|
#include "string.h"
|
|
#include "console.h"
|
|
|
|
void init_arp(struct ether_t *ether) {
|
|
init_ptr_vector(ðer->arp_table);
|
|
init_ptr_vector(ðer->arp_wait);
|
|
}
|
|
|
|
static uint8_t bcast_mac[6] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
|
|
|
|
void arp_process_packet(struct ether_t *ether, struct arp_packet_t *packet) {
|
|
if (htons(packet->oper) == 1) {
|
|
// request
|
|
if (htonl(packet->tpa) == ether->ipv4) {
|
|
arp_update_ipv4(ether, packet->sha, htonl(packet->spa));
|
|
|
|
struct arp_packet_t arp_packet;
|
|
|
|
arp_packet.htype = htons(1); // ethernet
|
|
arp_packet.ptype = htons(0x0800); // ipv4
|
|
arp_packet.hlen = 6;
|
|
arp_packet.plen = 4;
|
|
arp_packet.oper = htons(2); // reply
|
|
arp_packet.sha[0] = ether->mac[0];
|
|
arp_packet.sha[1] = ether->mac[1];
|
|
arp_packet.sha[2] = ether->mac[2];
|
|
arp_packet.sha[3] = ether->mac[3];
|
|
arp_packet.sha[4] = ether->mac[4];
|
|
arp_packet.sha[5] = ether->mac[5];
|
|
arp_packet.spa = htonl(ether->ipv4);
|
|
arp_packet.tha[0] = packet->sha[0];
|
|
arp_packet.tha[1] = packet->sha[1];
|
|
arp_packet.tha[2] = packet->sha[2];
|
|
arp_packet.tha[3] = packet->sha[3];
|
|
arp_packet.tha[4] = packet->sha[4];
|
|
arp_packet.tha[5] = packet->sha[5];
|
|
arp_packet.tpa = packet->spa;
|
|
struct ether_packet_t *ether_packet =
|
|
(struct ether_packet_t *)dbmalloc(sizeof(struct arp_packet_t) + sizeof(struct ether_packet_t), "arp_process_packet 1");
|
|
|
|
ether_packet->destmac[0] = packet->sha[0];
|
|
ether_packet->destmac[1] = packet->sha[1];
|
|
ether_packet->destmac[2] = packet->sha[2];
|
|
ether_packet->destmac[3] = packet->sha[3];
|
|
ether_packet->destmac[4] = packet->sha[4];
|
|
ether_packet->destmac[5] = packet->sha[5];
|
|
|
|
ether_packet->sourcemac[0] = ether->mac[0];
|
|
ether_packet->sourcemac[1] = ether->mac[1];
|
|
ether_packet->sourcemac[2] = ether->mac[2];
|
|
ether_packet->sourcemac[3] = ether->mac[3];
|
|
ether_packet->sourcemac[4] = ether->mac[4];
|
|
ether_packet->sourcemac[5] = ether->mac[5];
|
|
|
|
ether_packet->ethertype = htons(0x806);
|
|
|
|
memcpy(ether_packet->payload, &arp_packet, sizeof(struct arp_packet_t));
|
|
|
|
if (ether_send(ether, (char *)ether_packet, sizeof(struct arp_packet_t) + sizeof(struct ether_packet_t)) == -1) {
|
|
kprintf("Error sending reply.\n");
|
|
}
|
|
|
|
dbfree(ether_packet, "arp_process_packet 1");
|
|
}
|
|
|
|
} else if (htons(packet->oper) == 2) {
|
|
// reply
|
|
arp_update_ipv4(ether, packet->sha, htonl(packet->spa));
|
|
}
|
|
}
|
|
|
|
void arp_update_ipv4(struct ether_t *ether, uint8_t *mac, uint32_t ipv4) {
|
|
size_t i;
|
|
|
|
if (mac == NULL) {
|
|
kprintf("ARP: attempting to replace mac with NULL\n");
|
|
return;
|
|
}
|
|
|
|
for (i = 0; i < ptr_vector_len(ðer->arp_wait); i++) {
|
|
struct arp_wait_t *w = (struct arp_wait_t *)ptr_vector_get(ðer->arp_wait, i);
|
|
if (w->ipv4 == ipv4) {
|
|
ptr_vector_del(ðer->arp_wait, i);
|
|
dbfree(w, "arp_update_ipv4 1");
|
|
}
|
|
}
|
|
|
|
for (i = 0; i < ptr_vector_len(ðer->arp_table); i++) {
|
|
struct arp_table_t *t = (struct arp_table_t *)ptr_vector_get(ðer->arp_table, i);
|
|
if (t->mac[0] == mac[0] && t->mac[1] == mac[1] && t->mac[2] == mac[2] && t->mac[3] == mac[3] && t->mac[4] == mac[4] && t->mac[5] == mac[5]) {
|
|
t->ipv4 = ipv4;
|
|
return;
|
|
}
|
|
}
|
|
|
|
struct arp_table_t *nt = (struct arp_table_t *)dbmalloc(sizeof(struct arp_table_t), "arp_update_ipv4 2");
|
|
if (!nt) {
|
|
// die?
|
|
return;
|
|
}
|
|
|
|
nt->mac[0] = mac[0];
|
|
nt->mac[1] = mac[1];
|
|
nt->mac[2] = mac[2];
|
|
nt->mac[3] = mac[3];
|
|
nt->mac[4] = mac[4];
|
|
nt->mac[5] = mac[5];
|
|
nt->ipv4 = ipv4;
|
|
|
|
ptr_vector_append(ðer->arp_table, nt);
|
|
}
|
|
|
|
char *arp_req_ipv4(struct ether_t *ether, uint32_t ipv4) {
|
|
size_t i;
|
|
|
|
if (ipv4 == 0xffffffff) {
|
|
return (char *)bcast_mac;
|
|
}
|
|
|
|
for (i = 0; i < ptr_vector_len(ðer->arp_table); i++) {
|
|
struct arp_table_t *t = (struct arp_table_t *)ptr_vector_get(ðer->arp_table, i);
|
|
if (t->ipv4 == ipv4) {
|
|
return (char *)t->mac;
|
|
}
|
|
}
|
|
int found = 0;
|
|
|
|
for (i = 0; i < ptr_vector_len(ðer->arp_wait); i++) {
|
|
struct arp_wait_t *w = (struct arp_wait_t *)ptr_vector_get(ðer->arp_wait, i);
|
|
if (w->ipv4 == ipv4) {
|
|
|
|
if (w->wait > 0) {
|
|
w->wait--;
|
|
return NULL;
|
|
}
|
|
w->wait = 0x1000;
|
|
found = 1;
|
|
break;
|
|
}
|
|
}
|
|
|
|
if (!found) {
|
|
struct arp_wait_t *w = (struct arp_wait_t *)dbmalloc(sizeof(struct arp_wait_t), "arp_req_ipv4 1");
|
|
if (!w) {
|
|
// die?
|
|
return NULL;
|
|
}
|
|
w->ipv4 = ipv4;
|
|
w->wait = 0x1000;
|
|
ptr_vector_append(ðer->arp_wait, w);
|
|
}
|
|
|
|
// address not in table...
|
|
|
|
struct arp_packet_t arp_packet;
|
|
|
|
arp_packet.htype = htons(1); // ethernet
|
|
arp_packet.ptype = htons(0x0800); // ipv4
|
|
arp_packet.hlen = 6;
|
|
arp_packet.plen = 4;
|
|
arp_packet.oper = htons(1); // request
|
|
arp_packet.sha[0] = ether->mac[0];
|
|
arp_packet.sha[1] = ether->mac[1];
|
|
arp_packet.sha[2] = ether->mac[2];
|
|
arp_packet.sha[3] = ether->mac[3];
|
|
arp_packet.sha[4] = ether->mac[4];
|
|
arp_packet.sha[5] = ether->mac[5];
|
|
arp_packet.spa = htonl(ether->ipv4);
|
|
arp_packet.tha[0] = 0xff;
|
|
arp_packet.tha[1] = 0xff;
|
|
arp_packet.tha[2] = 0xff;
|
|
arp_packet.tha[3] = 0xff;
|
|
arp_packet.tha[4] = 0xff;
|
|
arp_packet.tha[5] = 0xff;
|
|
arp_packet.tpa = htonl(ipv4);
|
|
|
|
struct ether_packet_t *ether_packet = (struct ether_packet_t *)dbmalloc(sizeof(struct arp_packet_t) + sizeof(struct ether_packet_t), "arp_req_ipv4 2");
|
|
if (!ether_packet) {
|
|
// die?
|
|
return NULL;
|
|
}
|
|
|
|
ether_packet->destmac[0] = 0xff;
|
|
ether_packet->destmac[1] = 0xff;
|
|
ether_packet->destmac[2] = 0xff;
|
|
ether_packet->destmac[3] = 0xff;
|
|
ether_packet->destmac[4] = 0xff;
|
|
ether_packet->destmac[5] = 0xff;
|
|
|
|
ether_packet->sourcemac[0] = ether->mac[0];
|
|
ether_packet->sourcemac[1] = ether->mac[1];
|
|
ether_packet->sourcemac[2] = ether->mac[2];
|
|
ether_packet->sourcemac[3] = ether->mac[3];
|
|
ether_packet->sourcemac[4] = ether->mac[4];
|
|
ether_packet->sourcemac[5] = ether->mac[5];
|
|
|
|
ether_packet->ethertype = htons(0x806);
|
|
|
|
memcpy(ether_packet->payload, &arp_packet, sizeof(struct arp_packet_t));
|
|
|
|
ether_send(ether, (char *)ether_packet, sizeof(struct arp_packet_t) + sizeof(struct ether_packet_t));
|
|
|
|
dbfree(ether_packet, "arp_req_ipv4 3");
|
|
|
|
return NULL;
|
|
}
|