// TODO use instanceof callback, see protocols/*.c
static const protocol_t * get_first_protocol(const packet_t * packet) {
const protocol_t * protocol = NULL;
switch (packet_guess_address_family(packet)) {
case AF_INET:
protocol = protocol_search("ipv4");
break;
case AF_INET6:
protocol = protocol_search("ipv6");
break;
default:
fprintf(stderr, "Cannot guess Internet address family\n");
break;
}
return protocol;
}
const protocol_t * icmpv4_get_next_protocol(const layer_t * icmpv4_layer) {
const protocol_t * next_protocol = NULL;
uint8_t icmpv4_type;
if (layer_extract(icmpv4_layer, "type", &icmpv4_type)) {
switch (icmpv4_type) {
case ICMP_DEST_UNREACH:
case ICMP_TIME_EXCEEDED:
next_protocol = protocol_search("ipv4");
break;
default:
break;
}
}
return next_protocol;
}
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;
}
