122 lines
4.7 KiB
C++
122 lines
4.7 KiB
C++
#include <stdio.h>
|
|
#include <string.h>
|
|
|
|
#include <arpa/inet.h>
|
|
|
|
#include "packet.h"
|
|
|
|
namespace packet {
|
|
|
|
void parse(uint8_t const * buf, int length, packet * p)
|
|
{
|
|
//////////////////////////////////////////////////////////////////////
|
|
// eth
|
|
//////////////////////////////////////////////////////////////////////
|
|
|
|
eth::header const * eth_header = reinterpret_cast<eth::header const *>(&buf[eth_offset]);
|
|
|
|
//printf("eth source address ");
|
|
//print_eth_address(eth_header->source_address);
|
|
//printf("eth destination address ");
|
|
//print_eth_address(eth_header->destination_address);
|
|
|
|
//////////////////////////////////////////////////////////////////////
|
|
// ip4
|
|
//////////////////////////////////////////////////////////////////////
|
|
|
|
ip4::header const * ip4_header = reinterpret_cast<ip4::header const *>(&buf[ip4_offset]);
|
|
//printf("ip4 total length: %d\n", ntohs(ip4_header->total_length));
|
|
assert(ip4_offset + ntohs(ip4_header->total_length) == length);
|
|
|
|
printf("ip4 source address: %s\n", ip4::tostr(ip4_header->source_address));
|
|
printf("ip4 destination address: %s\n", ip4::tostr(ip4_header->destination_address));
|
|
|
|
//////////////////////////////////////////////////////////////////////
|
|
// udp4
|
|
//////////////////////////////////////////////////////////////////////
|
|
|
|
udp4::header const * udp4_header = reinterpret_cast<udp4::header const *>(&buf[udp4_offset]);
|
|
//printf("udp4 length: %d\n", ntohs(udp4_header->length));
|
|
assert(udp4_offset + ntohs(udp4_header->length) == length);
|
|
|
|
//////////////////////////////////////////////////////////////////////
|
|
// dhcp4
|
|
//////////////////////////////////////////////////////////////////////
|
|
|
|
dhcp4::message const * dhcp4_message = reinterpret_cast<dhcp4::message const *>(&buf[dhcp4_offset]);
|
|
uint8_t const * dhcp4_options = dhcp4_message->options;
|
|
//printf("dhcp4 cookie %02x %02x %02x %02x\n", dhcp4_options[0], dhcp4_options[1], dhcp4_options[2], dhcp4_options[3]);
|
|
assert(dhcp4_options[0] == 0x63 &&
|
|
dhcp4_options[1] == 0x82 &&
|
|
dhcp4_options[2] == 0x53 &&
|
|
dhcp4_options[3] == 0x63);
|
|
|
|
//////////////////////////////////////////////////////////////////////
|
|
// packet
|
|
//////////////////////////////////////////////////////////////////////
|
|
|
|
p->eth = eth_header;
|
|
p->ip4 = ip4_header;
|
|
p->udp4 = udp4_header;
|
|
p->dhcp4 = dhcp4_message;
|
|
}
|
|
|
|
int make(uint8_t * buf,
|
|
uint8_t const * eth_source_address,
|
|
uint8_t const * eth_destination_address,
|
|
uint32_t const ip4_source_address,
|
|
uint32_t const ip4_destination_address,
|
|
uint32_t const dhcp_transaction_id,
|
|
uint32_t const dhcp_message_type,
|
|
dhcp4::message_parameters const * params)
|
|
{
|
|
//////////////////////////////////////////////////////////////////////
|
|
// dhcp4
|
|
//////////////////////////////////////////////////////////////////////
|
|
|
|
|
|
|
|
int dhcp4_length = dhcp4::make_message(reinterpret_cast<void *>(&buf[dhcp4_offset]),
|
|
dhcp4::op::bootrequest,
|
|
dhcp_transaction_id,
|
|
eth_source_address,
|
|
dhcp_message_type,
|
|
params);
|
|
|
|
//////////////////////////////////////////////////////////////////////
|
|
// eth
|
|
//////////////////////////////////////////////////////////////////////
|
|
|
|
eth::header * eth_header = reinterpret_cast<eth::header *>(&buf[eth_offset]);
|
|
memcpy(eth_header->destination_address, eth_destination_address, 6);
|
|
memcpy(eth_header->source_address, eth_source_address, 6);
|
|
eth_header->ether_type = htons(0x0800);
|
|
|
|
//////////////////////////////////////////////////////////////////////
|
|
// ip4
|
|
//////////////////////////////////////////////////////////////////////
|
|
|
|
int ip4_payload_length = dhcp4_length + (sizeof (udp4::header));
|
|
int ip4_identification = 55179;
|
|
ip4::make_header(reinterpret_cast<void *>(&buf[ip4_offset]),
|
|
ip4_payload_length,
|
|
ip4_identification,
|
|
ip4_source_address,
|
|
ip4_destination_address);
|
|
|
|
//////////////////////////////////////////////////////////////////////
|
|
// udp4
|
|
//////////////////////////////////////////////////////////////////////
|
|
|
|
udp4::make_header(reinterpret_cast<void *>(&buf[udp4_offset]),
|
|
htons(68),
|
|
htons(67),
|
|
ip4_source_address,
|
|
ip4_destination_address,
|
|
&buf[dhcp4_offset],
|
|
dhcp4_length);
|
|
|
|
return dhcp4_offset + dhcp4_length;
|
|
}
|
|
}
|