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; }