71 lines
1.7 KiB
C++
71 lines
1.7 KiB
C++
#include <stdint.h>
|
|
#include <stddef.h>
|
|
#include <string.h>
|
|
|
|
#include <arpa/inet.h>
|
|
|
|
#include "ipv4.h"
|
|
|
|
namespace ipv4 {
|
|
|
|
uint32_t checksum_partial(uint32_t sum, void const * src, int length)
|
|
{
|
|
uint8_t const * addr = reinterpret_cast<uint8_t const *>(src);
|
|
|
|
ptrdiff_t i = 0;
|
|
while (length >= 2) {
|
|
sum += (addr[i+0] << 8) | (addr[i+1] << 0);
|
|
i += 2;
|
|
length -= 2;
|
|
}
|
|
|
|
if (length > 0) {
|
|
sum += (addr[i+0] << 8);
|
|
}
|
|
|
|
return sum;
|
|
}
|
|
|
|
uint32_t checksum_finish(uint32_t sum)
|
|
{
|
|
while (sum >> 16) {
|
|
sum = (sum & 0xffff) + (sum >> 16);
|
|
}
|
|
|
|
return static_cast<uint16_t>(~sum);
|
|
}
|
|
|
|
uint32_t checksum(void const * src, int length)
|
|
{
|
|
uint32_t sum = checksum_partial(0, src, length);
|
|
return checksum_finish(sum);
|
|
}
|
|
|
|
header * make_header(void * buf,
|
|
uint16_t payload_length,
|
|
uint16_t identification,
|
|
uint32_t source_address, // big endian
|
|
uint32_t destination_address) // big endian
|
|
{
|
|
memset(buf, 0, (sizeof (header)));
|
|
header * hdr = reinterpret_cast<header *>(buf);
|
|
|
|
const int version = 4;
|
|
const int ihl = (sizeof (header)) / 4;
|
|
const int total_length = payload_length + (sizeof (header));
|
|
|
|
hdr->version_ihl = (version << 4) | (ihl << 0);
|
|
//hdr->type_of_service = 0xb8;
|
|
hdr->total_length = htons(total_length);
|
|
hdr->identification = htons(identification);
|
|
hdr->time_to_live = 64;
|
|
hdr->protocol = protocol::udp;
|
|
hdr->source_address = source_address;
|
|
hdr->destination_address = destination_address;
|
|
|
|
hdr->header_checksum = htons(checksum(buf, (sizeof (header))));
|
|
|
|
return hdr;
|
|
}
|
|
}
|