bool probe_extract_ext(const probe_t * probe, const char * name, size_t depth, void * value) {
size_t i, num_layers = probe_get_num_layers(probe);
const layer_t * layer;
const protocol_field_t * protocol_field;
// We go through the layers until we get the required field
for(i = depth; i < num_layers; i++) {
layer = probe_get_layer(probe, i);
if (!(protocol_field = layer_get_protocol_field(layer, name))) continue;
// Hack to convert ipv*_t extracted into address_t value.
switch (protocol_field->type) {
case TYPE_IPV4:
memset(value, 0, sizeof(address_t));
((address_t *) value)->family = AF_INET;
value = &((address_t *) value)->ip.ipv4;
break;
case TYPE_IPV6:
memset(value, 0, sizeof(address_t));
((address_t *) value)->family = AF_INET6;
value = &((address_t *) value)->ip.ipv6;
break;
default: break;
}
if ((layer = probe_get_layer(probe, i))
&& layer_extract(layer, name, value)) {
return true;
}
}
return false;
}
bool tcp_matches(const struct probe_s * _probe, const struct probe_s * _reply)
{
const probe_t * probe = (const probe_t *) _probe,
* reply = (const probe_t *) _reply;
uint16_t probe_src_port = 0,
probe_dst_port = 0,
reply_src_port = 0,
reply_dst_port = 0;
layer_t * tcp_layer = NULL;
if (probe_extract(probe, "src_port", &probe_src_port)
&& probe_extract(probe, "dst_port", &probe_dst_port)
&& probe_extract(reply, "src_port", &reply_src_port)
&& probe_extract(reply, "dst_port", &reply_dst_port)) {
if (probe_src_port == reply_dst_port && reply_src_port == probe_dst_port) {
return true;
} else { // it is not a TCP probe; is it an ICMP probe?
if (!strcmp((probe_get_layer(reply, 1))->protocol->name, "icmpv4")
|| !strcmp((probe_get_layer(reply, 1))->protocol->name, "icmpv6")) {
if (!(tcp_layer = probe_get_layer(reply, 3)) || strcmp(tcp_layer->protocol->name, "tcp")) {
return false;
}
return (probe_src_port == reply_src_port) && (probe_dst_port == reply_dst_port);
}
}
}
return false;
}
bool probe_update_checksum(probe_t * probe)
{
size_t i, j, num_layers = probe_get_num_layers(probe);
layer_t * layer,
* layer_prev;
buffer_t * pseudo_header;
// Update each layers from the (last - 1) one to the first one.
for (j = 0; j < num_layers; j++) {
i = num_layers - j - 1;
layer = probe_get_layer(probe, i);
// Does the protocol require a pseudoheader to compute its checksum?
if (layer->protocol && layer->protocol->write_checksum) {
// Compute the checksum according to the layer's buffer and
// the pseudo header (if any).
if (layer->protocol->create_pseudo_header) {
if (i == 0) {
// This layer has no previous layer which is required to compute its checksum.
fprintf(stderr, "No previous layer which is required to compute '%s' checksum\n", layer->protocol->name);
errno = EINVAL;
return false;
} else {
layer_prev = probe_get_layer(probe, i - 1);
if (strncmp(layer_prev->protocol->name, "ipv", 3) != 0) {
fprintf(stderr,
"Trying to calculate %s checksum but the previous layer is not an IP layer (%s)\n",
layer->protocol->name,
layer_prev->protocol->name
);
return false;
}
if (!(pseudo_header = layer->protocol->create_pseudo_header(layer_prev->segment))) {
return false;
}
}
} else pseudo_header = NULL;
// Update the checksum of this layer
if (!layer->protocol->write_checksum(layer->segment, pseudo_header)) {
fprintf(stderr, "Error while updating checksum (layer %s)\n", layer->protocol->name);
return false;
}
// Release the pseudo header (if any) from the memory
if (pseudo_header) buffer_free(pseudo_header);
}
}
return true;
}
bool icmpv4_matches(const struct probe_s * _probe, const struct probe_s * _reply)
{
uint8_t reply_type = 0,
reply_code = 0,
probe_type = 0,
probe_code = 0;
layer_t * icmp_layer = NULL;
const probe_t * probe = (const probe_t *) _probe;
const probe_t * reply = (const probe_t *) _reply;
if (probe_extract(reply, "type", &reply_type)
&& probe_extract(probe, "type", &probe_type)
&& probe_extract(probe, "code", &probe_code)) {
if (reply_type == ICMP_ECHOREPLY) {
return true;
}
if (!(icmp_layer = probe_get_layer(reply, 3)) || strcmp(icmp_layer->protocol->name, "icmpv4")) {
return false;
}
if (probe_extract_ext(reply, "type", 3, &reply_type) && probe_extract_ext(reply, "code", 3, &reply_code)) {
return (probe_type == reply_type) && (probe_code == reply_code);
}
}
return false;
}
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;
}
layer_t * probe_get_layer_payload(const probe_t * probe) {
size_t num_layers = probe_get_num_layers(probe);
layer_t * last_layer;
if (num_layers == 0) {
fprintf(stderr, "probe_get_layer_payload: No layer in this probe!\n");
return NULL;
}
last_layer = probe_get_layer(probe, num_layers - 1);
return last_layer && last_layer->protocol ? NULL : last_layer;
}
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;
}
void probe_debug(const probe_t * probe) {
size_t i, num_layers = probe_get_num_layers(probe);
layer_t * layer1,
* layer2;
probe_t * probe_should_be;
if ((probe_should_be = probe_dup(probe))) {
// Compute expected values
probe_update_fields(probe_should_be);
// Dump fields
printf("** PROBE **\n\n");
for (i = 0; i < num_layers; i++) {
layer1 = probe_get_layer(probe, i);
layer2 = probe_get_layer(probe_should_be, i);
layer_debug(layer1, layer2, i);
printf("\n");
}
printf("\n");
probe_free(probe_should_be);
}
}
bool probe_write_field_ext(probe_t * probe, size_t depth, const char * name, void * bytes, size_t num_bytes) {
bool ret = false;
size_t i, num_layers = probe_get_num_layers(probe);
layer_t * layer;
for (i = depth; i < num_layers; i++) {
layer = probe_get_layer(probe, i);
if (layer_write_field(layer, name, bytes, num_bytes)) {
ret = true;
break;
}
}
return ret;
}
static bool probe_update_protocol(probe_t * probe)
{
size_t i, num_layers = probe_get_num_layers(probe);
layer_t * layer,
* prev_layer;
for (i = 0, prev_layer = NULL; i < num_layers; i++, prev_layer = layer) {
layer = probe_get_layer(probe, i);
if (layer->protocol && prev_layer) {
// Update 'protocol' field (if any)
layer_set_field_and_free(layer, I8("protocol", prev_layer->protocol->protocol));
}
}
return true;
}
bool probe_set_field_ext(probe_t * probe, size_t depth, const field_t * field)
{
bool ret = false;
size_t i, num_layers = probe_get_num_layers(probe);
layer_t * layer;
for (i = depth; i < num_layers; i++) {
layer = probe_get_layer(probe, i);
if (layer_set_field(layer, field)) {
ret = true;
break;
}
}
return ret;
}
field_t * probe_create_field_ext(const probe_t * probe, const char * name, size_t depth)
{
size_t i, num_layers = probe_get_num_layers(probe);
layer_t * layer;
field_t * field;
// We go through the layers until we get the required field
for(i = depth; i < num_layers; i++) {
layer = probe_get_layer(probe, i);
if ((field = layer_create_field(layer, name))) {
return field;
}
}
// No matching field found, this is maybe a metafield
return probe_create_metafield_ext(probe, name, depth);
}
static bool probe_finalize(probe_t * probe)
{
bool ret = true;
size_t i, num_layers = probe_get_num_layers(probe);
layer_t * layer;
// Allow the protocol to do some processing before computing checksums.
for (i = 0; i < num_layers; i++) {
layer = probe_get_layer(probe, i);
if (layer->protocol && layer->protocol->finalize) {
if (!(ret &= layer->protocol->finalize(layer->segment))) {
fprintf(stderr, "W: Can't finalize layer %s\n", layer->protocol->name);
}
}
}
return ret;
}
bool probe_match(const struct probe_s * probe, const struct probe_s * reply)
{
size_t num_layers = probe_get_num_layers((const probe_t *)probe);
size_t i = 0;
layer_t * layer_to_analyse = NULL;
for (i = 0; i < num_layers - 1; i++)
{
layer_to_analyse = probe_get_layer((const probe_t *)probe, i);
if (layer_to_analyse->protocol->matches != NULL) {
if (!layer_to_analyse->protocol->matches(probe, reply)) {
return false;
}
} else {
return false;
}
}
return true;
}
void probe_dump(const probe_t * probe)
{
size_t i, num_layers = probe_get_num_layers(probe);
layer_t * layer;
printf("** PROBE **\n\n");
if (probe->delay) {
printf("probe delay \n\n");
field_dump(probe->delay);
printf("number of probes left to send: (%d) \n\n", (int)probe->left_to_send);
printf("probe structure\n\n");
}
for (i = 0; i < num_layers; i++) {
layer = probe_get_layer(probe, i);
layer_dump(layer, i);
printf("\n");
}
printf("\n");
}
static bool probe_update_length(probe_t * probe)
{
bool ret = true;
size_t i, offset,
num_layers = probe_get_num_layers(probe),
packet_size = probe_get_size(probe);
layer_t * layer;
for (i = 0, offset = 0; i < num_layers; i++) {
layer = probe_get_layer(probe, i);
if (layer->protocol) {
// Update 'length' field (if any)
// This protocol field must always corresponds to the size of the
// header + its contents.
layer_set_field_and_free(layer, I16("length", packet_size - offset));
offset += layer->protocol->get_header_size(layer->segment);
} else {
// Update payload size
layer_set_segment_size(layer, packet_size - offset);
}
}
return ret;
}
bool network_tag_probe(network_t * network, probe_t * probe)
{
uint16_t tag, // Network-side endianness
checksum; // Host-side endianness
size_t payload_size = probe_get_payload_size(probe);
size_t tag_size = sizeof(uint16_t);
size_t num_layers = probe_get_num_layers(probe);
// For probes having a payload of size 0 and a "body" field (like icmp)
layer_t * last_layer;
bool tag_in_body = false;
/* The probe gets assigned a unique tag. Currently we encode it in the UDP
* checksum, but I guess the tag will be protocol dependent. Also, since the
* tag changes the probe, the user has no direct control of what is sent on
* the wire, since typically a packet is tagged just before sending, after
* scheduling, to maximize the number of probes in flight. Available space
* in the headers is used for tagging, + encoding some information... */
/* XXX hardcoded XXX */
/* 1) Set payload = tag : this is only possible if both the payload and the
* checksum have not been set by the user.
* We need a list of used tags = in flight... + an efficient way to get a
* free one... Also, we need to share tags between several instances ?
* randomized tags ? Also, we need to determine how much size we have to
* encode information. */
if (num_layers < 2 || !(last_layer = probe_get_layer(probe, num_layers - 2))) {
fprintf(stderr, "network_tag_probe: not enough layer (num_layers = %d)\n", (unsigned int)num_layers);
goto ERR_GET_LAYER;
}
// The last layer is the payload, the previous one is the last protocol layer.
// If this layer has a "body" field like icmp, we have no payload and
// we use the body field.
if (last_layer->protocol && protocol_get_field(last_layer->protocol, "body")) {
tag_in_body = true;
}
tag = htons(network_get_available_tag(network));
// Write the tag at offset zero of the payload
if (tag_in_body) {
probe_write_field(probe, "body", &tag, tag_size);
} else {
if (payload_size < tag_size) {
fprintf(stderr, "Payload too short (payload_size = %u tag_size = %u)\n", (unsigned int)payload_size, (unsigned int)tag_size);
goto ERR_INVALID_PAYLOAD;
}
if (!(probe_write_payload(probe, &tag, tag_size))) {
goto ERR_PROBE_WRITE_PAYLOAD;
}
}
// Fix checksum to get a well-formed packet
if (!(probe_update_checksum(probe))) {
fprintf(stderr, "Can't update fields\n");
goto ERR_PROBE_UPDATE_FIELDS;
}
// Retrieve the checksum of UDP/TCP/ICMP checksum (host-side endianness)
if (!(probe_extract_tag(probe, &checksum))) {
fprintf(stderr, "Can't extract tag\n");
goto ERR_PROBE_EXTRACT_CHECKSUM;
}
// Write the probe ID in the UDP/TCP/ICMP checksum
if (!(probe_set_tag(probe, ntohs(tag)))) {
fprintf(stderr, "Can't set tag\n");
goto ERR_PROBE_SET_TAG;
}
// Write the tag using the network-side endianness in the payload
checksum = htons(checksum);
if (tag_in_body) {
if (!(probe_write_field(probe, "body", &checksum, tag_size))) {
fprintf(stderr, "Can't set body\n");
goto ERR_PROBE_SET_FIELD;
}
} else {
if (!probe_write_payload(probe, &checksum, tag_size)) {
fprintf(stderr, "Can't write payload (2)\n");
goto ERR_BUFFER_WRITE_BYTES2;
}
}
return true;
ERR_PROBE_SET_FIELD:
ERR_BUFFER_WRITE_BYTES2:
ERR_PROBE_SET_TAG:
ERR_PROBE_EXTRACT_CHECKSUM:
ERR_PROBE_UPDATE_FIELDS:
ERR_PROBE_WRITE_PAYLOAD:
ERR_INVALID_PAYLOAD:
ERR_GET_LAYER:
return false;
}
const char * probe_get_protocol_name(const probe_t * probe, size_t i) {
if (i + 1 == probe_get_num_layers(probe)) return "payload";
const layer_t * layer = probe_get_layer(probe, i);
return layer ? layer->protocol->name : NULL;
}