tor_multipath_voip/src/algo_rr.c

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

struct timer_info {
  uint16_t health_id;
  uint8_t prevlink;
  uint16_t min_blocked_pkt;
  struct algo_ctx* algo;
};

struct queued_pkt {
  uint8_t on;
  int link_fd;
  int idx;
  uint16_t id;
  struct algo_ctx* algo;
};

struct rr_ctx {
  uint8_t my_links;
  uint8_t remote_links;
  uint8_t current_link;
  int64_t mjit;
  uint16_t health_id;
  uint16_t health_id_late;
  uint16_t content_id;
  uint16_t sent_health_id;
  uint16_t sent_content_id;
  struct internet_packet prev_packet;
  struct timespec emit_time;
  struct queued_pkt real[PACKET_BUFFER_SIZE];
  struct timer_info wait[PACKET_BUFFER_SIZE];
};

void show_link_availability(struct rr_ctx* rr) {
  printf("Links availability: my_links[");
  for (int i = 0; i < 8; i++) {
    if (rr->my_links & 1 << i) printf("U");
    else printf("-");
  }
  printf("], rem_links[");
  for (int i = 0; i < 8; i++) {
    if (rr->remote_links & 1 << i) printf("U");
    else printf("-");
  }
  printf("]\n");
}

void on_timeout_health (struct evt_core_ctx* ctx, void* user);

void rr_pkt_register(struct evt_core_ctx* ctx,  struct evt_core_fdinfo* fdinfo, struct buffer_packet* bp) {
  struct algo_ctx* app_ctx = fdinfo->cat->app_ctx;
  struct rr_ctx* rr = app_ctx->misc;
  uint16_t real_idx = bp->ip.ap.fmt.content.clear.id % PACKET_BUFFER_SIZE;

  assert(bp->ip.ap.fmt.headers.cmd == CMD_CLEAR);

  // 1. We queue the packet to keep it
  if (rr->real[real_idx].on && ring_lt(rr->real[real_idx].id, bp->ip.ap.fmt.content.clear.id)) {
    fprintf(stderr, "Real array is full for id=%d, idx=%d, BUG: [\n", bp->ip.ap.fmt.content.clear.id, real_idx);
    for (int i = 0; i < PACKET_BUFFER_SIZE; i++) {
      fprintf(stderr, "\t%d => %d\n", rr->real[i].id, rr->real[i].on);
    }
    fprintf(stderr, "] - could be replaced by drop\n");
    exit(EXIT_FAILURE);
  } else if (!rr->real[real_idx].on && ring_gt(bp->ip.ap.fmt.content.clear.id, rr->content_id)) {
    rr->real[real_idx].on = 1;
    rr->real[real_idx].id = bp->ip.ap.fmt.content.clear.id;
    rr->real[real_idx].idx = real_idx;
    rr->real[real_idx].link_fd = fdinfo->fd;
    rr->real[real_idx].algo = app_ctx;
    mv_buffer_rtoa(app_ctx, fdinfo, &rr->real[real_idx].idx);
  } else {
    if (ctx->verbose) fprintf(stdout, "Packet %d already received (current: %d)\n", bp->ip.ap.fmt.content.clear.id, rr->content_id);
    mv_buffer_rtof (app_ctx, fdinfo);
  }
}

void rr_deliver(struct evt_core_ctx* ctx, struct algo_ctx* app_ctx, struct queued_pkt* dp) {
  struct evt_core_fdinfo *to_fdinfo = NULL;
  struct rr_ctx* rr = app_ctx->misc;
  char url[255];

  // 1. Marked the packet as handled
  dp->on = 0;

  // 2. Get the buffer
  struct buffer_packet* bp = get_app_buffer (app_ctx, &dp->idx);
  assert(bp->ip.ap.fmt.headers.cmd == CMD_CLEAR);

  // 3. We update our cursor
  rr->content_id = bp->ip.ap.fmt.content.clear.id;

  // 4. Find its target
  sprintf(url, "udp:write:127.0.0.1:%d", bp->ip.ap.fmt.content.clear.port);
  to_fdinfo = evt_core_get_from_url (ctx, url);
  if (to_fdinfo == NULL) {
    fprintf(stderr, "No fd for URL %s in udp:write for tcp-read. Dropping packet %d :( \n", url, dp->idx);
    //mv_buffer_wtor (app_ctx, fdinfo, bp);
    mv_buffer_atof (app_ctx, &dp->idx);
  }

  // 5. We move the buffer and notify the target
  //mv_buffer_rtow (app_ctx, fdinfo, to_fdinfo, bp);
  mv_buffer_atow (app_ctx, &dp->idx, to_fdinfo);
  main_on_udp_write(ctx, to_fdinfo);
}

void rr_pkt_manage_links(struct evt_core_ctx* ctx, struct evt_core_fdinfo* fdinfo, struct buffer_packet* bp) {
  struct algo_ctx* app_ctx = fdinfo->cat->app_ctx;
  struct rr_ctx* rr = app_ctx->misc;

  assert(bp->ip.ap.fmt.headers.cmd == CMD_HEALTH);

  // 1. Health packet was received too late, dropping it
  if (ring_le(bp->ip.ap.fmt.content.health.id, rr->health_id_late)) goto release;

  // 2. Reactivate link if deactivated
  char buffer[16];
  url_get_port (buffer, fdinfo->url);
  int link_num = atoi(buffer) - 7500; // @FIXME Hardcoded
  if (!(rr->remote_links & (1 << link_num))) {
    printf("Activate link=%d | ", link_num);
    rr->remote_links |= 1 << link_num; // Make sure that the link is marked as working
    show_link_availability (rr);
  }

  // 3. Update RR structure if its the greatest health_id we received
  if (ring_gt(bp->ip.ap.fmt.content.health.id, rr->health_id)) {
    // 3.1. Update current health id
    rr->health_id = bp->ip.ap.fmt.content.health.id;

    // 3.2. Update my links I can use thanks to target feedback
    if (bp->ip.ap.fmt.content.health.bitfield != rr->my_links) {
      rr->my_links = bp->ip.ap.fmt.content.health.bitfield;
      printf("Update my links | ");
      show_link_availability (rr);
    }
  }

  // 4. Set callback to close this health packet window
  int64_t timeout = rr->mjit - (int64_t) bp->ip.ap.fmt.content.health.deltat;
  if (timeout <= 0) timeout = 0;
  uint64_t idx = bp->ip.ap.fmt.content.health.id % PACKET_BUFFER_SIZE;

  rr->wait[idx].health_id = bp->ip.ap.fmt.content.health.id;
  rr->wait[idx].prevlink = bp->ip.ap.fmt.content.health.prevlink;
  rr->wait[idx].min_blocked_pkt = bp->ip.ap.fmt.content.health.min_blocked_pkt;
  rr->wait[idx].algo = app_ctx;

  set_timeout (ctx, timeout, &rr->wait[idx], on_timeout_health);

release:
  mv_buffer_rtof(app_ctx, fdinfo);
}

uint64_t rr_pkt_unroll(struct evt_core_ctx* ctx, struct algo_ctx* app_ctx) {
  struct rr_ctx* rr = app_ctx->misc;
  struct evt_core_fdinfo* fdinfo = NULL;
  struct buffer_packet* bp = NULL;
  uint64_t delivered = 0;

  while(1) {
    //printf("Trying to deliver %d\n", rr->recv_id+1);
    struct queued_pkt* def = &rr->real[(rr->content_id+1) % PACKET_BUFFER_SIZE];
    if (!def->on) break;
    rr_deliver(ctx, app_ctx, def);
    delivered++;
    //printf("Delivered %d\n", rr->recv_id);
  }
  return delivered;
}

//------

int algo_rr_on_stream(struct evt_core_ctx* ctx, struct evt_core_fdinfo* fdinfo, struct buffer_packet* bp) {
  struct algo_ctx* app_ctx = fdinfo->cat->app_ctx;
  struct rr_ctx* rr = app_ctx->misc;

  if (bp->ip.ap.fmt.headers.cmd == CMD_CLEAR) {
    if (ctx->verbose > 1) fprintf(stderr, "    [algo/rr] Received a CLEAR packet of size %d on URL %s\n", bp->ip.ap.fmt.headers.size, fdinfo->url);
    // 1. Register packet in our queue
    rr_pkt_register(ctx, fdinfo, bp);

    // 2. Process queue
    rr_pkt_unroll (ctx, app_ctx);
  } else if (bp->ip.ap.fmt.headers.cmd == CMD_HEALTH) {
    if (ctx->verbose > 1) fprintf(stderr, "    [algo/rr] Received a HEALTH packet of size %d on URL %s\n", bp->ip.ap.fmt.headers.size, fdinfo->url);
    rr_pkt_manage_links(ctx, fdinfo, bp);
  } else {
    fprintf(stderr, "    [algo/rr] Packet CMD unrecognized (%d)\n", bp->ip.ap.fmt.headers.cmd);
    mv_buffer_rtof(app_ctx, fdinfo);
  }

  return 0;
}

int algo_rr_on_datagram(struct evt_core_ctx* ctx, struct evt_core_fdinfo* fdinfo, struct buffer_packet* bp) {
  struct algo_ctx* app_ctx = fdinfo->cat->app_ctx;
  struct rr_ctx* rr = app_ctx->misc;
  struct evt_core_fdinfo *to_fdinfo = NULL;
  uint16_t min_pkt;
  char url[255];
  size_t health_packet_size = sizeof(bp->ip.ap.fmt.headers) + sizeof(bp->ip.ap.fmt.content.health);
  size_t max_size = sizeof(struct internet_packet) - health_packet_size;

  if (ctx->verbose > 1) fprintf(stderr, "    [algo/rr] Read a UDP packet on URL %s\n", fdinfo->url);

  // 1. Prepare RR state and packet values
  struct timespec curr;
  int secs, nsecs;
  uint64_t mili_sec;

  if (clock_gettime(CLOCK_MONOTONIC, &curr) == -1){
    perror("clock_gettime error");
    exit(EXIT_FAILURE);
  }

  // 2. Compute delta t
  secs = curr.tv_sec - rr->emit_time.tv_sec;
  nsecs = curr.tv_nsec - rr->emit_time.tv_nsec;
  rr->emit_time = curr;
  mili_sec = secs * 1000 + nsecs / 1000000;
  if (mili_sec > rr->mjit) mili_sec = rr->mjit;

  // 3. Prepare fresh packet
  assert(bp->ip.ap.fmt.headers.cmd == CMD_CLEAR);
  bp->ip.ap.fmt.content.clear.id = rr->sent_content_id;
  min_pkt = rr->sent_content_id;
  rr->sent_content_id++;
  if (bp->ip.ap.fmt.headers.size > max_size) {
    fprintf(stderr, "Packet is too big to be relayed. Oops...\n");
    exit(EXIT_FAILURE);
  }

  // 4. Append redundancy if needed
  if (app_ctx->ap.redundant_data == 1) {
    size_t current_size = get_full_size (bp);
    size_t final_size = current_size + rr->prev_packet.ap.fmt.headers.size;
    if (final_size <= max_size) {
      min_pkt = rr->prev_packet.ap.fmt.content.clear.id;
      append_buffer(&bp->ip.ap, bp->ap_count, &rr->prev_packet.ap); // We append previous packet
      bp->ap_count++;
    } else if (ctx->verbose) {
      fprintf(stderr, "    [algo/rr] Can't append redundancy (current=%ld, after=%ld, max=%ld)\n", current_size, final_size, max_size);
    }

    append_buffer(&rr->prev_packet.ap, 0, &bp->ip.ap); // We store current packet for next time
  }

  // 5. Append health packet
  struct buffer_packet hp;
  hp.ip.ap.fmt.headers.cmd = CMD_HEALTH;
  hp.ip.ap.fmt.headers.size = sizeof(bp->ip.ap.fmt.headers) + sizeof(bp->ip.ap.fmt.content.health);
  hp.ip.ap.fmt.content.health.id = rr->sent_health_id;
  hp.ip.ap.fmt.content.health.deltat = mili_sec;
  hp.ip.ap.fmt.content.health.prevlink = rr->current_link;
  hp.ip.ap.fmt.content.health.bitfield = rr->remote_links;
  hp.ip.ap.fmt.content.health.min_blocked_pkt = min_pkt;
  rr->sent_health_id++;
  append_buffer(&bp->ip.ap, bp->ap_count, &hp.ip.ap);
  bp->ap_count++;

  // 6. Try to find someone to send it
  int max = 16;
  uint8_t sel_link = rr->current_link;
  while(max-- >= 0) {
    if (app_ctx->ap.is_waiting_bootstrap && !app_ctx->is_rdy) goto not_ready; // Still bootstrapping
    sel_link = (sel_link + 1) % app_ctx->ap.links;
    sprintf(url, "tcp:write:127.0.0.1:%d", 7500 + sel_link); //@FIXME Hardcoded
    to_fdinfo = evt_core_get_from_url (ctx, url);
    if (to_fdinfo == NULL) continue; // Missing link
    if (!app_ctx->ap.is_healing /* if healing deactivated */|| rr->my_links & (1 << sel_link) /* if link not down */ ) {
      rr->current_link = sel_link;
      mv_buffer_rtow (app_ctx, fdinfo, to_fdinfo);
      main_on_tcp_write(ctx, to_fdinfo);
      return 0;
    } else {
      struct buffer_packet* dup_bp = dup_buffer_tow(app_ctx, bp, to_fdinfo);
      /*
       * for later
      dup_bp->ip.ap.fmt.content.health.min_blocked_pkt = 0;
      dup_bp->ap_count = 1; // We want to send only health packet to help link recover... Bwarf same traffic on Tor...
       */
      main_on_tcp_write(ctx, to_fdinfo);
    }
  }

not_ready:
  // 3. We find no up target
  fprintf(stderr, "Still bootstrapping or no link to forward data from %s in udp-read. Dropping packet :( \n", fdinfo->url);
  mv_buffer_wtof (app_ctx, fdinfo);
  return 0;
}

void on_timeout_health (struct evt_core_ctx* ctx, void* raw) {
  struct timer_info* t = raw;
  struct algo_ctx* app_ctx = t->algo;
  struct rr_ctx* rr = app_ctx->misc;

  // 1. Update link recovery window if needed
  if (ring_gt(t->health_id, rr->health_id_late)) rr->health_id_late = t->health_id;

  // 2. Blacklist previous link if needed
  uint16_t prev_health_id = (t->health_id - 1);
  uint16_t prev_health_idx = prev_health_id % PACKET_BUFFER_SIZE;
  struct timer_info* t_old = &rr->wait[prev_health_idx];
  if (t_old->health_id != prev_health_id) {
    printf("Blacklist link=%d | ", t->prevlink);
    rr->remote_links &= 0xff ^ 1 << t->prevlink;
    show_link_availability (rr);
  }

  // 3. Deliver blocked packets
  // @FIXME CRAPPY CODE / CRAPPY LOGIC
  //printf("t->min_blocked_pkt=%d, rr->content_id=%d\n", t->min_blocked_pkt, rr->content_id);
  if (ring_gt(t->min_blocked_pkt, rr->content_id) && !rr->real[t->min_blocked_pkt % PACKET_BUFFER_SIZE].on) {
    fprintf(stderr, "min_blocked_packet has not been received, t->min_blocked_pkt=%d, rr->content_id=%d\n", t->min_blocked_pkt, rr->content_id);
    exit(EXIT_FAILURE);
  }
  while (ring_gt(t->min_blocked_pkt, rr->content_id - 1)) {
    rr->content_id++;
    rr_pkt_unroll (ctx, app_ctx);
  }
  rr_pkt_unroll (ctx, app_ctx);
}

int algo_rr_on_err(struct evt_core_ctx* ctx, struct evt_core_fdinfo* fdinfo) {
  // We do nothing
  return 0;
}

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

void algo_rr_init(struct evt_core_ctx* ctx, struct algo_ctx* app_ctx, struct algo_params* ap) {
  struct rr_ctx* rr = malloc(sizeof(struct rr_ctx));
  if (rr == NULL) {
    perror("malloc failed for rr_init.");
    exit(EXIT_FAILURE);
  }
  memset(rr, 0, sizeof(struct rr_ctx));
  rr->mjit = 200;
  rr->my_links = 0xff;
  rr->remote_links = 0xff;
  rr->sent_health_id = 1;
  rr->sent_content_id = 1;
  rr->health_id = 0;
  rr->health_id_late = 0;
  rr->content_id = 0;
  rr->current_link = 0;
  app_ctx->misc = rr;
  app_ctx->free_misc = algo_rr_free;

  init_timer(ctx);
}