static bool probe_packet_resize(probe_t * probe, size_t size)
{
size_t offset = 0, // offset between the begining of the packet and the current position
i, num_layers = probe_get_num_layers(probe);
layer_t * layer;
uint8_t * segment;
if (!packet_resize(probe->packet, size)) {
return false;
}
// TODO update bitfield
// Update each layer's segment
for (i = 0; i < num_layers; i++) {
layer = probe_get_layer(probe, i);
segment = packet_get_bytes(probe->packet) + offset;
layer_set_segment(layer, segment);
if (layer->protocol) {
// We're in a layer related to a protocol. Update "length" field (if any).
// It concerns: ipv4, ipv6, udp but not tcp, icmpv4, icmpv6
layer_set_field_and_free(layer, I16("length", size - offset));
offset += layer->segment_size;
}
}
return true;
}
static bool probe_push_payload(probe_t * probe, size_t payload_size) {
layer_t * payload_layer,
* first_layer;
uint8_t * payload_bytes;
size_t packet_size;
// Check whether a payload is already set
if ((payload_layer = probe_get_layer_payload(probe))) {
fprintf(stderr, "Payload already set\n");
goto ERR_PAYLOAD_ALREADY_SET;
}
// Get the first protocol layer
if (!(first_layer = probe_get_layer(probe, 0))) {
fprintf(stderr, "No protocol layer defined in this probe\n");
goto ERR_NO_FIRST_LAYER;
}
// The first segment stores the packet size, thus:
// @payload = @first_segment + packet_size - payload_size
packet_size = probe_get_size(probe) ;
payload_bytes = packet_get_bytes(probe->packet) + packet_size - payload_size;
if (!(payload_layer = layer_create_from_segment(NULL, payload_bytes, payload_size))) {
goto ERR_LAYER_CREATE;
}
// Add the payload layer in the probe
if (!(probe_push_layer(probe, payload_layer))) {
fprintf(stderr, "Can't push payload layer\n");
goto ERR_PUSH_LAYER;
}
// Resize the payload if required
if (payload_size > 0) {
if (!probe_payload_resize(probe, payload_size)) {
fprintf(stderr, "Can't resize payload\n");
goto ERR_PAYLOAD_RESIZE;
}
}
return true;
ERR_PAYLOAD_RESIZE:
dynarray_del_ith_element(probe->layers, probe_get_num_layers(probe) - 1, NULL);
ERR_PUSH_LAYER:
layer_free(payload_layer);
ERR_LAYER_CREATE:
ERR_NO_FIRST_LAYER:
ERR_PAYLOAD_ALREADY_SET:
return false;
}
bool socketpool_send_packet(const socketpool_t * socketpool, const packet_t * packet)
{
sockaddr_u sock;
int sockfd;
socklen_t socklen;
const struct sockaddr * dst_addr;
memset(&sock, 0, sizeof(sockaddr_u));
// Prepare socket
// We don't care about the dst_port set in the packet
switch (packet->dst_ip->family) {
#ifdef USE_IPV4
case AF_INET:
sock.sin.sin_family = AF_INET;
sock.sin.sin_addr = packet->dst_ip->ip.ipv4;
sockfd = socketpool->ipv4_sockfd;
socklen = sizeof(struct sockaddr_in);
dst_addr = (struct sockaddr *) &sock.sin;
break;
#endif
#ifdef USE_IPV6
case AF_INET6:
sock.sin6.sin6_family = AF_INET6;
memcpy(&sock.sin6.sin6_addr, &packet->dst_ip->ip.ipv6, sizeof(ipv6_t));
sockfd = socketpool->ipv6_sockfd;
socklen = sizeof(struct sockaddr_in6);
dst_addr = (struct sockaddr *) &sock.sin6;
break;
#endif
default:
fprintf(stderr, "socketpool_send_packet: Address family not supported");
goto ERR_INVALID_FAMILY;
}
// Send the packet
if (sendto(sockfd, packet_get_bytes(packet), packet_get_size(packet), 0, dst_addr, socklen) == -1) {
perror("send_data: Sending error in queue");
goto ERR_SEND_TO;
}
return true;
ERR_SEND_TO:
ERR_INVALID_FAMILY:
return false;
}
bool probe_set_protocols(probe_t * probe, const char * name1, ...)
{
// TODO A similar function should allow hooking into the layer structure
// and not at the top layer
va_list args, args2;
size_t packet_size, offset, segment_size;
const char * name;
layer_t * layer = NULL,
* prev_layer;
const protocol_t * protocol;
// Remove the former layer structure
probe_layers_clear(probe);
// Set up the new layer structure
va_start(args, name1);
// Allocate the buffer according to the layer structure
packet_size = 0;
va_copy(args2, args);
for (name = name1; name; name = va_arg(args2, char *)) {
if (!(protocol = protocol_search(name))) {
fprintf(stderr, "Cannot find %s protocol, known protocols are:", name);
protocols_dump();
goto ERR_PROTOCOL_SEARCH;
}
packet_size += protocol->write_default_header(NULL);
}
va_end(args2);
if (!(packet_resize(probe->packet, packet_size))) goto ERR_PACKET_RESIZE;
// Create each layer
offset = 0;
prev_layer = NULL;
for (name = name1; name; name = va_arg(args, char *)) {
// Associate protocol to the layer
if (!(protocol = protocol_search(name))) goto ERR_PROTOCOL_SEARCH2;
segment_size = protocol->write_default_header(packet_get_bytes(probe->packet) + offset);
if (!(layer = layer_create_from_segment(protocol, packet_get_bytes(probe->packet) + offset, segment_size))) {
fprintf(stderr, "Can't create segment for %s header\n", layer->protocol->name);
goto ERR_LAYER_CREATE;
}
// TODO layer_set_mask(layer, bitfield_get_mask(probe->bitfield) + offset);
// Update 'length' field (if any). It concerns IPv* and UDP, but not TCP or ICMPv*
layer_set_field_and_free(layer, I16("length", packet_size - offset));
// Update 'protocol' field of the previous inserted layer (if any)
if (prev_layer) {
if (!layer_set_field_and_free(prev_layer, I8("protocol", layer->protocol->protocol))) {
fprintf(stderr, "Can't set 'protocol' in %s header\n", layer->protocol->name);
goto ERR_SET_PROTOCOL;
}
}
offset += layer->segment_size;
if (!probe_push_layer(probe, layer)) {
fprintf(stderr, "Can't add protocol layer\n");
goto ERR_PUSH_LAYER;
}
prev_layer = layer;
}
va_end(args);
layer = NULL;
// Payload : initially empty
if (!probe_push_payload(probe, 0)) {
goto ERR_PUSH_PAYLOAD;
}
// Size and checksum are pending, they depend on payload
return true;
ERR_PUSH_PAYLOAD:
ERR_PUSH_LAYER:
ERR_SET_PROTOCOL:
if (layer) layer_free(layer);
ERR_LAYER_CREATE:
ERR_PROTOCOL_SEARCH2:
probe_layers_clear(probe);
ERR_PACKET_RESIZE:
ERR_PROTOCOL_SEARCH:
return false;
}
probe_t * probe_wrap_packet(packet_t * packet)
{
probe_t * probe;
size_t segment_size, remaining_size;
layer_t * layer;
uint8_t * segment;
const protocol_t * protocol;
if (!(probe = probe_create())) {
goto ERR_PROBE_CREATE;
}
// Clear the probe
packet_free(probe->packet);
probe->packet = packet;
probe_layers_clear(probe);
// Prepare iteration
segment = packet_get_bytes(probe->packet);
remaining_size = packet_get_size(probe->packet);
// Push layers
for (protocol = get_first_protocol(packet); protocol; protocol = protocol->get_next_protocol(layer)) {
if (remaining_size < protocol->write_default_header(NULL)) {
// Not enough bytes left for the header, packet is truncated
segment_size = remaining_size;
} else {
segment_size = protocol->get_header_size(segment);
}
if (!(layer = layer_create_from_segment(protocol, segment, segment_size))) {
goto ERR_CREATE_LAYER;
}
if (!probe_push_layer(probe, layer)) {
goto ERR_PUSH_LAYER;
}
segment += segment_size;
remaining_size -= segment_size;
if (remaining_size < 0) {
fprintf(stderr, "probe_wrap_packet: Truncated packet\n");
goto ERR_TRUNCATED_PACKET;
}
if (!protocol->get_next_protocol) {
break;
}
continue;
ERR_TRUNCATED_PACKET:
ERR_PUSH_LAYER:
layer_free(layer);
ERR_CREATE_LAYER:
goto ERR_LAYER_DISCOVER_LAYER;
}
// Rq: Some packets (e.g ICMP type 3) do not have payload.
// In this case we push an empty payload
probe_push_payload(probe, remaining_size);
return probe;
ERR_LAYER_DISCOVER_LAYER:
probe_free(probe);
ERR_PROBE_CREATE:
return NULL;
}