68 lines
2.1 KiB
C
68 lines
2.1 KiB
C
#include <stdbool.h>
|
|
#include "udp_packet.h"
|
|
#include "../../utils.h"
|
|
#include "ipv4_packet.h"
|
|
|
|
#define ETH_UDP_HEADER_SIZE (8)
|
|
|
|
typedef struct {
|
|
uint32_t sourceIpAddr;
|
|
uint32_t destIpAddr;
|
|
uint8_t zero;
|
|
uint8_t protocol;
|
|
uint16_t udpLength;
|
|
} UdpPseudoHeader;
|
|
|
|
static uint16_t udp_checksum(const UdpPseudoHeader * pseudoHeader, const uint8_t * headerPayload, uint32_t headerPayloadSize) {
|
|
uint16_t chksum = 0;
|
|
uint16_t i;
|
|
|
|
// pseudoheader
|
|
for (i = 0; i < sizeof(UdpPseudoHeader) / 2; i++) {
|
|
chksum += ~((uint16_t *)pseudoHeader)[i];
|
|
}
|
|
|
|
// UDP header and payload
|
|
for (i = 0; i < headerPayloadSize / 2; i++) {
|
|
if (i != 3) { // skip Checksum field
|
|
chksum += ~((uint16_t *) headerPayload)[i];
|
|
}
|
|
}
|
|
|
|
// pad if packet size is odd
|
|
chksum += ~((uint16_t)headerPayload[headerPayloadSize - 1] << 8);
|
|
|
|
return chksum;
|
|
}
|
|
|
|
int parse_udp(const uint8_t *hdr, uint32_t size, PcktProps * props) {
|
|
UdpProps * udpProps = (UdpProps *)props;
|
|
FETCH_WORD_H2N_ADVANCE(&udpProps->SourcePort, hdr);
|
|
FETCH_WORD_H2N_ADVANCE(&udpProps->DestinationPort, hdr);
|
|
FETCH_WORD_H2N_ADVANCE(&udpProps->Length, hdr);
|
|
FETCH_WORD_H2N_ADVANCE(&udpProps->Checksum, hdr);
|
|
|
|
// common fields...
|
|
udpProps->headerSize = ETH_UDP_HEADER_SIZE;
|
|
udpProps->validityOK = size == udpProps->Length; // TODO UDP checksum validation!
|
|
udpProps->containedPacketClass = 0;
|
|
|
|
return udpProps->validityOK ? 0 : -1;
|
|
}
|
|
|
|
void insert_udp_header(uint8_t *hdr, const PcktHeaderElement *headers) {
|
|
UdpProps * udpProps = (UdpProps *) &headers->props;
|
|
FILL_WORD_H2N_ADVANCE(hdr, udpProps->SourcePort);
|
|
FILL_WORD_H2N_ADVANCE(hdr, udpProps->DestinationPort);
|
|
FILL_WORD_H2N_ADVANCE(hdr, udpProps->Length);
|
|
|
|
// calculate checksum
|
|
const IPv4Props * ipProps = (IPv4Props *)&headers->prev->props;
|
|
UdpPseudoHeader ph = { ipProps->SourceIPAddr, ipProps->DestIPAddr, 0, ETH_UDP_PACKET_CLASS, udpProps->Length };
|
|
udpProps->Checksum = udp_checksum(&ph, hdr, udpProps->Length);
|
|
FILL_WORD_H2N_ADVANCE(hdr, udpProps->Checksum);
|
|
}
|
|
|
|
|
|
|