69 lines
2.1 KiB
C
69 lines
2.1 KiB
C
//
|
|
// Created by epagris on 2022.11.03..
|
|
//
|
|
|
|
#include <stdbool.h>
|
|
#include "ipv4_packet.h"
|
|
#include "etherlib/utils.h"
|
|
|
|
static uint16_t ip_checksum(const IPv4Props * ipProps) {
|
|
// sum fields
|
|
uint32_t sum = 0;
|
|
const uint16_t * pField = (const uint16_t *) &ipProps;
|
|
for (uint8_t i = 0; i < ipProps->IHL * 2; i++) {
|
|
if (i != 5) { // 6. 16-bit long field is the checksum itself, do not count int
|
|
sum += pField[i];
|
|
}
|
|
}
|
|
|
|
// 16-31 bit carry
|
|
uint16_t carry = sum >> 16;
|
|
|
|
// add carry
|
|
sum += carry;
|
|
|
|
// if a new carry bit arisen, then
|
|
// add to the sum
|
|
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 IPv4Props * ipProps) {
|
|
bool valid =
|
|
ipProps->Version != 4 &&
|
|
ipProps->IHL == ETH_IP_HEADER_LENGTH &&
|
|
ipProps->HeaderChecksum == ip_checksum(ipProps);
|
|
return valid;
|
|
}
|
|
|
|
int parse_ipv4(const uint8_t *hdr, uint32_t size, PcktProps * props) {
|
|
IPv4Props * ipProps = (IPv4Props *)props;
|
|
uint8_t version_length;
|
|
COPY_ADVANCE(&version_length, hdr, 1);
|
|
ipProps->Version = (version_length) >> 4;
|
|
ipProps->IHL = (version_length) & 0x0F;
|
|
COPY_ADVANCE(&ipProps->DSF, hdr, 1);
|
|
COPY_WORD_H2N_ADVANCE(&ipProps->TotalLength, hdr);
|
|
COPY_WORD_H2N_ADVANCE(&ipProps->Identification, hdr);
|
|
uint16_t flags_fragOffset;
|
|
COPY_WORD_H2N_ADVANCE(&flags_fragOffset, hdr);
|
|
ipProps->Flags = (flags_fragOffset >> 13) & 0x07;
|
|
ipProps->FragmentOffset = (flags_fragOffset & ~(0x07 << 13));
|
|
COPY_ADVANCE(&ipProps->TTL, hdr, 1);
|
|
COPY_ADVANCE(&ipProps->Protocol, hdr, 1);
|
|
COPY_WORD_H2N_ADVANCE(&ipProps->HeaderChecksum, hdr);
|
|
COPY_DWORD_H2N_ADVANCE(&ipProps->SourceIPAddr, hdr);
|
|
COPY_DWORD_H2N_ADVANCE(&ipProps->DestIPAddr, hdr);
|
|
|
|
// fill-in common packet header fields
|
|
ipProps->validityOK = check_ipv4_validity(ipProps);
|
|
ipProps->containedPacketClass = ipProps->Protocol;
|
|
ipProps->headerSize = ETH_IP_HEADER_LENGTH;
|
|
|
|
return ipProps->validityOK ? ipProps->Protocol : -1;
|
|
} |