103 lines
3.8 KiB
C
103 lines
3.8 KiB
C
//
|
|
// Created by epagris on 2022.11.03..
|
|
//
|
|
|
|
#include <stdbool.h>
|
|
#include "ethernet_frame.h"
|
|
#include "ipv4_packet.h"
|
|
#include "../../utils.h"
|
|
|
|
#include "../../eth_interface.h"
|
|
|
|
static uint16_t ip_checksum(const uint8_t * hdr) {
|
|
// sum fields
|
|
uint32_t sum = 0;
|
|
const uint16_t *pField = (const uint16_t *) hdr;
|
|
for (uint8_t i = 0; i < (ETH_IPv4_HEADER_SIZE / sizeof(uint16_t)); i++) {
|
|
uint16_t field = pField[i];
|
|
if (i != 5) { // 6. 16-bit long field is the checksum itself, do not count in
|
|
sum += field;
|
|
}
|
|
}
|
|
|
|
while (sum >> 16) {
|
|
sum = (sum & 0xFFFF) + (sum >> 16);
|
|
}
|
|
|
|
// invert result
|
|
uint16_t sum16 = sum ^ 0xFFFF;
|
|
return sum16;
|
|
}
|
|
|
|
#define ETH_IP_HEADER_LENGTH (20)
|
|
|
|
static bool check_ipv4_validity(const uint8_t * hdr, const IPv4Props *ipProps) {
|
|
bool valid =
|
|
(ipProps->Version == 4) &&
|
|
(ipProps->IHL == (ETH_IP_HEADER_LENGTH / 4)) &&
|
|
(ntohs(ipProps->HeaderChecksum) == ip_checksum(hdr));
|
|
return valid;
|
|
}
|
|
|
|
int parse_ipv4(const uint8_t *hdr, uint32_t size, PcktHeaderElement *pcktHdrLe, struct EthInterface_ *intf) {
|
|
const uint8_t * hdrBegin = hdr;
|
|
IPv4Props *ipProps = HEADER_FETCH_PROPS(IPv4Props, pcktHdrLe);
|
|
uint8_t version_length;
|
|
FETCH_ADVANCE(&version_length, hdr, 1);
|
|
ipProps->Version = (version_length) >> 4;
|
|
ipProps->IHL = (version_length) & 0x0F;
|
|
FETCH_ADVANCE(&ipProps->DSF, hdr, 1);
|
|
FETCH_WORD_H2N_ADVANCE(&ipProps->TotalLength, hdr);
|
|
FETCH_WORD_H2N_ADVANCE(&ipProps->Identification, hdr);
|
|
uint16_t flags_fragOffset;
|
|
FETCH_WORD_H2N_ADVANCE(&flags_fragOffset, hdr);
|
|
ipProps->Flags = (flags_fragOffset >> 13) & 0x07;
|
|
ipProps->FragmentOffset = (flags_fragOffset & ~(0x07 << 13));
|
|
FETCH_ADVANCE(&ipProps->TTL, hdr, 1);
|
|
FETCH_ADVANCE(&ipProps->Protocol, hdr, 1);
|
|
FETCH_WORD_H2N_ADVANCE(&ipProps->HeaderChecksum, hdr);
|
|
FETCH_ADVANCE(&ipProps->SourceIPAddr, hdr, 4); // NO network -> host byte order conversion!
|
|
FETCH_ADVANCE(&ipProps->DestIPAddr, hdr, 4);
|
|
|
|
// fill-in common packet header fields
|
|
ipProps->validityOK = check_ipv4_validity(hdrBegin, ipProps);
|
|
ipProps->containedPacketClass = ipProps->Protocol;
|
|
ipProps->headerSize = ETH_IP_HEADER_LENGTH;
|
|
|
|
// if packet is valid, learn MAC-IP mapping
|
|
if (ipProps->validityOK) {
|
|
EthernetProps * ethProps = HEADER_FETCH_PROPS(EthernetProps, pcktHdrLe->prev); // fetch Ethernet header
|
|
ArpEntry entry; // fill entry
|
|
memcpy(&entry.eth, ethProps->sourceAddr, ETH_HW_ADDR_LEN);
|
|
entry.ip = ipProps->SourceIPAddr;
|
|
arpc_learn(intf->arpc, &entry); // learn new assignment
|
|
}
|
|
|
|
return ipProps->validityOK ? ipProps->Protocol : -1;
|
|
}
|
|
|
|
static uint16_t nextIpIdentification = 0;
|
|
|
|
void insert_ipv4_header(uint8_t *hdr, const PcktHeaderElement *headers) {
|
|
uint8_t * hdrOrig = hdr;
|
|
IPv4Props *ipProps = (IPv4Props *) &headers->props;
|
|
ipProps->Identification = nextIpIdentification++; // auto-insert identification
|
|
uint8_t version_length = (ipProps->Version << 4) | (ipProps->IHL & 0x0F);
|
|
FILL_ADVANCE(hdr, &version_length, 1);
|
|
FILL_ADVANCE(hdr, &ipProps->DSF, 1);
|
|
FILL_WORD_H2N_ADVANCE(hdr, ipProps->TotalLength);
|
|
FILL_WORD_H2N_ADVANCE(hdr, ipProps->Identification);
|
|
uint16_t flags_fragOffset = ((ipProps->Flags & 0x07) << 13) | (ipProps->FragmentOffset & ~(0x07 << 13));
|
|
FILL_WORD_H2N_ADVANCE(hdr, flags_fragOffset); // TODO...
|
|
FILL_ADVANCE(hdr, &ipProps->TTL, 1);
|
|
FILL_ADVANCE(hdr, &ipProps->Protocol, 1);
|
|
uint8_t * ChkSumPtr = hdr;
|
|
FILL_WORD_H2N_ADVANCE(hdr, 0x0000);
|
|
FILL_ADVANCE(hdr, &ipProps->SourceIPAddr, 4);
|
|
FILL_ADVANCE(hdr, &ipProps->DestIPAddr, 4);
|
|
|
|
// calculate checksum after filling header
|
|
uint16_t checksum = ip_checksum(hdrOrig);
|
|
memcpy(ChkSumPtr, &checksum, 2);
|
|
}
|