32 lines
900 B
C++
32 lines
900 B
C++
|
#include "udp.h"
|
||
|
|
#include "eth.h"
|
||
|
|
#include "ipv4.h"
|
||
|
|
#include "net.h"
|
||
|
|
#include "parse_buffer.h"
|
||
|
|
|
||
|
|
namespace udp {
|
||
|
|
|
||
|
|
void handle(std::span<const uint8_t> frame, span_writer& tx) {
|
||
|
|
parse_buffer pb(frame);
|
||
|
|
auto* eth_hdr = pb.consume<eth::header>();
|
||
|
|
auto* ip = pb.consume<ipv4::header>();
|
||
|
|
if (!ip) return;
|
||
|
|
|
||
|
|
size_t options_len = ip->header_len() - sizeof(ipv4::header);
|
||
|
|
if (options_len > 0 && !pb.skip(options_len)) return;
|
||
|
|
|
||
|
|
auto* uhdr = pb.consume<header>();
|
||
|
|
if (!uhdr) return;
|
||
|
|
if (uhdr->dst_port != PICOMAP_PORT_BE) return;
|
||
|
|
|
||
|
|
size_t udp_len = __builtin_bswap16(uhdr->length);
|
||
|
|
if (udp_len < sizeof(header)) return;
|
||
|
|
size_t payload_len = udp_len - sizeof(header);
|
||
|
|
if (pb.remaining_size() < payload_len) return;
|
||
|
|
|
||
|
|
address from{eth_hdr->src, ip->src, uhdr->src_port};
|
||
|
|
client::handler(pb.remaining().subspan(0, payload_len), from);
|
||
|
|
}
|
||
|
|
|
||
|
|
} // namespace udp
|