tor_multipath_voip/src/algo_thunder.c

#include <sys/timerfd.h>
#include "algo_utils.h"
#include "utils.h"
#include "url.h"
#include "proxy.h"
#include "timer.h"

// A Tor cell size is 512 bytes but handle only 498 bytes of data
#define TOR_CELL_SIZE 498
#define MAX_LINKS 64

struct thunder_ctx {
  uint16_t recv_id;
  uint16_t emit_id;
  uint8_t selected_link;
  uint8_t total_links;
  uint64_t delta_t_per_link[MAX_LINKS];
  uint64_t rcv_delta_t_per_link[MAX_LINKS];
  uint64_t received_pkts_on_link[MAX_LINKS];
  uint64_t blacklisted[MAX_LINKS];
  size_t monit_pkt_size;
  int64_t allowed_jitter_ms;
  struct timespec prev_link_time, prev_rcv_link_time;
};

uint64_t compute_delta(struct timespec* prev_time, uint64_t max) {
  struct timespec curr;
  int secs, nsecs;
  uint64_t mili_sec;

  // 1. We compute the time difference
  if (clock_gettime(CLOCK_MONOTONIC, &curr) == -1){
    perror("clock_gettime error");
    exit(EXIT_FAILURE);
  }
  secs = curr.tv_sec - prev_time->tv_sec;
  nsecs = curr.tv_nsec - prev_time->tv_nsec;
  *prev_time = curr;
  mili_sec = secs * 1000 + nsecs / 1000000;
  if (mili_sec > max) mili_sec = max;

  return mili_sec;
}

int is_blacklisted(struct thunder_ctx* thunderc, int link_id) {
  return thunderc->blacklisted[link_id] >= thunderc->received_pkts_on_link[link_id];
}

void prepare(struct evt_core_ctx* ctx, struct evt_core_fdinfo* fdinfo, struct buffer_packet* bp) {
  struct algo_ctx* app_ctx = fdinfo->cat->app_ctx;
  struct thunder_ctx* thunderc = app_ctx->misc;

  thunderc->emit_id++;
  union abstract_packet metadata = {
    .fmt.headers.cmd = CMD_UDP_METADATA_THUNDER,
    .fmt.headers.size = sizeof(metadata.fmt.headers) + sizeof(metadata.fmt.content.udp_metadata_thunder),
    .fmt.headers.flags = 0,
    .fmt.content.udp_metadata_thunder.id = thunderc->emit_id,
  };
  buffer_append_ap (bp, &metadata);
  if (ctx->verbose > 1) fprintf(stderr, "    [algo_thunder] UDP metadata added\n");
}

void pad(struct evt_core_ctx* ctx, struct evt_core_fdinfo* fdinfo, struct buffer_packet* bp) {
  struct algo_ctx* app_ctx = fdinfo->cat->app_ctx;
  struct thunder_ctx* thunderc = app_ctx->misc;
  uint64_t ref = 0l + thunderc->emit_id;

  dup_buffer_toa (&app_ctx->br, bp, (void *)ref);

  // 1. Clean old buffers (we keep only thunderc->total_links buffer, keeping more would be useless)
  if (ref > thunderc->total_links && get_app_buffer (&app_ctx->br, (void *)(ref - thunderc->total_links))) {
    mv_buffer_atof (&app_ctx->br, (void *)(ref - thunderc->total_links));
  }

  // 2. Append abstract packets stored in our buffers
  uint64_t add_ref = ref;
  while(1) {
    if (add_ref < 1) break;
    add_ref--;
    struct buffer_packet *bp_iter = get_app_buffer (&app_ctx->br, (void *)add_ref);
    if (bp_iter == NULL) break;
    union abstract_packet *ap = buffer_first_ap (bp_iter);
    if (ap->fmt.headers.cmd != CMD_UDP_ENCAPSULATED) {
      fprintf(stderr, "Invalid buffer payload!\n");
      exit(EXIT_FAILURE);
    }
    union abstract_packet *ap_meta = ap_next (ap);
    if (ap_meta->fmt.headers.cmd != CMD_UDP_METADATA_THUNDER) {
      fprintf(stderr, "Invalid buffer metadata!\n");
      exit(EXIT_FAILURE);
    }

    if (buffer_full_size (bp) + ap->fmt.headers.size + ap_meta->fmt.headers.size > TOR_CELL_SIZE - thunderc->monit_pkt_size) break;

    buffer_append_ap (bp, ap);
    buffer_append_ap (bp, ap_meta);
    if (ctx->verbose > 1) fprintf(stderr, "    [algo_thunder] Pad packet (now %ld bytes)\n", buffer_full_size (bp));
  }
}

int schedule(struct evt_core_ctx* ctx, struct evt_core_fdinfo* fdinfo, struct buffer_packet* bp) {
  char url[256];
  struct algo_ctx* app_ctx = fdinfo->cat->app_ctx;
  struct thunder_ctx* thunderc = app_ctx->misc;
  struct evt_core_fdinfo *to_fdinfo = NULL;
  struct evt_core_cat* cat = evt_core_get_from_cat (ctx, "tcp-write");

  uint8_t protect = thunderc->total_links;
  do {
    // 1. We choose the link
    if (cat->socklist->len == 0) {
      if (ctx->verbose > 1) fprintf(stderr, "    [algo_thunder] No link available, packet will be dropped\n");
      break;
    }

    to_fdinfo = NULL;
    do {
      thunderc->selected_link = (thunderc->selected_link + 1) % thunderc->total_links;
      sprintf(url, "tcp:write:127.0.0.1:%d", 7500 + thunderc->selected_link);
      to_fdinfo = evt_core_get_from_url (ctx, url);
    } while (to_fdinfo == NULL);
    //printf("URL %s has been retained\n", url);

    // 2. We create the packet template
    union abstract_packet links = {
      .fmt.headers.cmd = CMD_LINK_MONITORING_THUNDER,
      .fmt.headers.size = thunderc->monit_pkt_size,
      .fmt.headers.flags = 0,
      .fmt.content.link_monitoring_thunder.links_status = {}
    };

    // 3. We append the template to the buffer
    struct buffer_packet* bp_dup = dup_buffer_tow (&app_ctx->br, bp, to_fdinfo);
    union abstract_packet *new_ap = buffer_append_ap (bp_dup, &links);

    // 4. We compute the time difference
    uint64_t mili_sec = compute_delta (&thunderc->prev_link_time, UINT16_MAX);

    // 5. We create the array
    struct link_info *li = &new_ap->fmt.content.link_monitoring_thunder.links_status;
    for (int i = 0; i < thunderc->total_links; i++) {
      thunderc->delta_t_per_link[i] += mili_sec;
      li[i].delta_t = thunderc->delta_t_per_link[i] > UINT16_MAX ? UINT16_MAX : thunderc->delta_t_per_link[i];
    }
    thunderc->delta_t_per_link[thunderc->selected_link] = 0;
    li[thunderc->selected_link].delta_t = 0;

    if (ctx->verbose > 1) {
      dump_buffer_packet(bp_dup);
      fprintf(stderr, "    [algo_thunder] Will send this info\n");
    }
    main_on_tcp_write(ctx, to_fdinfo);

  } while (is_blacklisted (thunderc, thunderc->selected_link) && protect-- > 0);
  
  if (protect == 0) {
    fprintf(stderr, "all links were blacklisted, resetting\n");
    for (int i = 0; i < thunderc->total_links; i++) {
      fprintf(stderr, "  link=%d, blacklisted=%d, rcved=%d\n", i, thunderc->blacklisted[i], thunderc->received_pkts_on_link[i]);
      thunderc->received_pkts_on_link[i] = thunderc->blacklisted[i] + 1;
    }
  }

  if (ctx->verbose > 1) fprintf(stderr, "    [algo_thunder] Packets sent\n");

  // Release the buffer
  mv_buffer_rtof (&app_ctx->br, fdinfo);

  return 0;
}

struct block_info { uint8_t i; struct algo_ctx* app_ctx; uint64_t missing;};

void on_block (struct evt_core_ctx* ctx, void* raw) {
  struct block_info* bi = raw;
  struct thunder_ctx* thunderc = bi->app_ctx->misc;

  if (thunderc->received_pkts_on_link[bi->i] >= bi->missing) goto release;
  if (thunderc->blacklisted[bi->i] >= bi->missing) goto release;

  //printf("[algo_thunder] Blacklisting link %d\n", bi->i);
  thunderc->blacklisted[bi->i] = bi->missing;

release:
  free(bi);
}

int is_in_order(struct thunder_ctx* thunderc, uint8_t link_id) {
  uint64_t ref = thunderc->received_pkts_on_link[link_id];
  for (int i = 0; i < thunderc->total_links; i++) {
    uint64_t expected = link_id >= i ? ref : ref - 1;
    if (thunderc->received_pkts_on_link[i] > expected) {
      //printf("link_id=%d, i=%d, pkt_i=%ld, pkt_i_expected=%ld, pkt_link_id=%ld\n", link_id, i, thunderc->received_pkts_on_link[i], expected, ref);
      return 0;
    }
  }

  return 1;
}

void classify(struct evt_core_ctx* ctx, struct evt_core_fdinfo* fdinfo, struct buffer_packet* bp) {
  struct algo_ctx* app_ctx = fdinfo->cat->app_ctx;
  struct thunder_ctx* thunderc = app_ctx->misc;

  union abstract_packet* ap = buffer_first_ap (bp);
  while (ap != NULL && ap->fmt.headers.cmd != CMD_LINK_MONITORING_THUNDER) ap = ap_next(ap);
  if (ap == NULL) {
    fprintf(stderr, "Unable to find our packet\n");
    exit(EXIT_FAILURE);
  }

  /*
  if (ap->fmt.headers.flags & FLAG_RESET) {
    for (int i = 0; i < MAX_LINKS; i++) thunderc->received_pkts_on_link[i] = 1;
  }
  */

  // 1. Update link info
  int link_id = url_get_port_int(fdinfo->url) - 7500;
  thunderc->received_pkts_on_link[link_id]++;
  //printf("Received %ld packets on link %d\n", thunderc->received_pkts_on_link[link_id], link_id);
  struct link_info *li = &ap->fmt.content.link_monitoring_thunder.links_status;

  uint64_t mili_sec = compute_delta (&thunderc->prev_rcv_link_time, UINT16_MAX);
  for (int i = 0; i < thunderc->total_links; i++) {
    thunderc->rcv_delta_t_per_link[i] += mili_sec;
  }
  thunderc->rcv_delta_t_per_link[link_id] = 0;

  // 2. Disable links that have received packets too late
  if (is_in_order (thunderc, link_id)) {
    /*printf("Local: ");
    for (int i = 0; i < thunderc->total_links; i++) {
      printf("%ld ", thunderc->rcv_delta_t_per_link[i]);
    }
    printf("\n");
    printf("Packet: ");
    for (int i = 0; i < thunderc->total_links; i++) {
      printf("%d ", li[i].delta_t);
    }
    printf("\n");*/

    for (int i = 0; i < thunderc->total_links; i++) {
      int64_t remote_delta = 0, local_delta = 0, owd_difference = 0;
      remote_delta = li[i].delta_t;
      local_delta = thunderc->rcv_delta_t_per_link[i];
      if (remote_delta > 1000) continue; // Too many time elapsed for useful comparison

      owd_difference = local_delta - remote_delta;
      if (owd_difference <= thunderc->allowed_jitter_ms && owd_difference >= -thunderc->allowed_jitter_ms) continue;

      struct block_info *bi = malloc(sizeof(struct block_info));

      if (owd_difference < -thunderc->allowed_jitter_ms) {
        bi->i = i;
        bi->app_ctx = app_ctx;
        bi->missing = thunderc->received_pkts_on_link[i];
      } else if (owd_difference > thunderc->allowed_jitter_ms) {
        bi->i = link_id;
        bi->app_ctx = app_ctx;
        bi->missing = thunderc->received_pkts_on_link[link_id];
      } else {
        fprintf(stderr, "Algorithm is wrong\n");
        exit(EXIT_FAILURE);
      }

      printf("  Packet Too Late - Blocked link %d owd_diff=%ld, local_delta=%ld, remote_delta=%ld\n", i, owd_difference, local_delta, remote_delta);
      on_block(ctx, bi);
    }
  }

  // 3. Disable links that miss packets
  for (uint8_t i = 0; i < thunderc->total_links; i++) {
    uint64_t expected = i <= link_id ? thunderc->received_pkts_on_link[link_id] : thunderc->received_pkts_on_link[link_id] - 1;
    if (thunderc->received_pkts_on_link[i] >= expected) continue; // Nothing to do, all packets have been received

    int64_t timeout = thunderc->allowed_jitter_ms - li[i].delta_t;

    struct block_info *bi = malloc(sizeof(struct block_info));
    bi->i = i; bi->app_ctx = app_ctx; bi->missing = expected;

    if (timeout <= 0) {
      on_block(ctx, bi);
      //printf("  Missing Packet - Blocked link %d (expected: %ld, seen: %ld)\n", i, expected, thunderc->received_pkts_on_link[i]);
      continue;
    }

    set_timeout (ctx, timeout, bi, on_block);
    //printf("  Missing Packet - Triggered timeout for link %d in %ldms (expected: %ld, seen: %ld)\n", i, timeout, expected, thunderc->received_pkts_on_link[i]);
    if (ctx->verbose > 1) {
      fprintf(stderr, "    [algo_thunder] Set timeout on link %d of %ld ms (packets expected: %ld, seen: %ld)\n",
              i, timeout, expected, thunderc->received_pkts_on_link[i]);
    }
  }
  if (ctx->verbose > 1) fprintf(stderr, "    [algo_thunder] Classify done\n");

  printf("Blacklisted links: ");
  for (int i = 0; i < thunderc->total_links; i++) {
    if (is_blacklisted (thunderc, i)) printf("_");
    else printf("U");
  }
  printf("\n");
}

struct unpad_info {
  union abstract_packet *ap_arr_pl[MAX_LINKS], *ap_arr_meta[MAX_LINKS];
  uint8_t ap_arr_vals;
};

void unpad(struct evt_core_ctx* ctx, struct evt_core_fdinfo* fdinfo, struct buffer_packet* bp, struct unpad_info *ui) {
  struct algo_ctx* app_ctx = fdinfo->cat->app_ctx;
  struct thunder_ctx* thunderc = app_ctx->misc;

  for (union abstract_packet* ap = buffer_first_ap (bp); ap != NULL; ap = ap_next(ap)) {
    if (ap->fmt.headers.cmd != CMD_UDP_ENCAPSULATED) continue;

    union abstract_packet* ap_meta = ap_next(ap);
    if (ap_meta == NULL || ap_meta->fmt.headers.cmd != CMD_UDP_METADATA_THUNDER) {
      fprintf(stderr, "Unexpected packet, expecting udp metadata\n");
    }

    if (ap_meta->fmt.content.udp_metadata_thunder.id > thunderc->recv_id) {
      ui->ap_arr_pl[ui->ap_arr_vals] = ap;
      ui->ap_arr_meta[ui->ap_arr_vals] = ap_meta;
      ui->ap_arr_vals++;
    }
  }
  if (ctx->verbose > 1) fprintf(stderr, "    [algo_thunder] Unpad done\n");
}

void adapt(struct evt_core_ctx* ctx, struct evt_core_fdinfo* fdinfo, struct buffer_packet* bp, struct unpad_info *ui) {
  struct algo_ctx* app_ctx = fdinfo->cat->app_ctx;
  struct thunder_ctx* thunderc = app_ctx->misc;
  char url[256];
  struct evt_core_fdinfo *to_fdinfo = NULL;
  uint64_t delivered = 0;

  for (int i = ui->ap_arr_vals-1; i >= 0; i--) {
    //fprintf(stderr, "i=%d, ui->ap_arr_vals=%d\n", i, ui->ap_arr_vals);
    if (ui->ap_arr_meta[i]->fmt.content.udp_metadata_thunder.id <= thunderc->recv_id) continue;
    thunderc->recv_id = ui->ap_arr_meta[i]->fmt.content.udp_metadata_thunder.id;

    // Find destination
    sprintf(url, "udp:write:127.0.0.1:%d", ui->ap_arr_pl[i]->fmt.content.udp_encapsulated.port);
    to_fdinfo = evt_core_get_from_url (ctx, url);
    if (to_fdinfo == NULL) {
      fprintf(stderr, "No fd for URL %s in tcp-read. Dropping packet :( \n", url);
    }

    struct buffer_packet *bp_dest = inject_buffer_tow (&app_ctx->br, to_fdinfo);
    bp_dest->mode = BP_WRITING;
    //dump_buffer_packet (bp_dest);
    buffer_append_ap (bp_dest, ui->ap_arr_pl[i]);
    main_on_udp_write(ctx, to_fdinfo);
    delivered++;
  }

  if (delivered != 1) {
    //printf("[algo_thunder] Delivered %ld packets (now id=%d)\n", delivered, thunderc->recv_id);
  }

  mv_buffer_rtof (&app_ctx->br, fdinfo);
  if (ctx->verbose > 1) fprintf(stderr, "    [algo_thunder] Adapt done\n");
}

int algo_thunder_on_stream(struct evt_core_ctx* ctx, struct evt_core_fdinfo* fdinfo, struct buffer_packet* bp) {
  struct unpad_info ui = {0};

  classify(ctx, fdinfo, bp);
  unpad(ctx, fdinfo, bp, &ui);
  adapt(ctx, fdinfo, bp, &ui);
  return 0;
}

int algo_thunder_on_datagram(struct evt_core_ctx* ctx, struct evt_core_fdinfo* fdinfo, struct buffer_packet* bp) {
  prepare(ctx, fdinfo, bp);
  pad(ctx, fdinfo, bp);
  schedule(ctx, fdinfo, bp);
  return 0;
}

void algo_thunder_free(void* v) {
  struct rr_ctx* rr = v;
  free(rr);
}

void algo_thunder_init(struct evt_core_ctx* ctx, struct algo_ctx* app_ctx, struct algo_params* ap) {
  app_ctx->misc = malloc(sizeof(struct thunder_ctx));
  app_ctx->free_misc = algo_thunder_free;
  if (app_ctx->misc == NULL) {
    perror("malloc failed in algo thunder init");
    exit(EXIT_FAILURE);
  }
  memset(app_ctx->misc, 0, sizeof(struct thunder_ctx));
  struct thunder_ctx* thunderc = app_ctx->misc;
  thunderc->recv_id = 1;
  thunderc->emit_id = 1;
  thunderc->total_links = app_ctx->ap.links;
  thunderc->selected_link = thunderc->total_links - 1;
  thunderc->allowed_jitter_ms = 200;
  for (int i = 0; i < thunderc->total_links; i++) {
    thunderc->received_pkts_on_link[i] = 1;
    thunderc->rcv_delta_t_per_link[i] = 0;
  }

  union abstract_packet links = {};
  //fprintf(stderr, "Total links %d\n", thunderc->total_links);
  thunderc->monit_pkt_size = sizeof(links.fmt.headers) + sizeof(links.fmt.content.link_monitoring_thunder) + sizeof(struct link_info) * (thunderc->total_links - 1);

  if (ap->algo_specific_params != NULL) {
    sscanf(ap->algo_specific_params, "%ld", &thunderc->allowed_jitter_ms);
  }
  printf("Allowed jitter set to %ld ms\n", thunderc->allowed_jitter_ms);

  init_timer(ctx);
}

int algo_thunder_on_err(struct evt_core_ctx *ctx, struct evt_core_fdinfo *fdinfo) {
  if (strstr(fdinfo->cat->name, "udp") != NULL) return 1;
  return 0;
}