tor_multipath_voip/src/algo_rr.c

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

struct waited_pkt {
  uint16_t id;
  int link_num;
  uint8_t on;
  int timer_fd;
};

struct deferred_pkt {
  int link_fd;
  int idx;
  uint16_t id;
  uint8_t on;
};

struct rr_ctx {
  uint8_t my_links;
  uint16_t my_links_ver;
  uint8_t remote_links;
  int64_t mjit;
  uint16_t recv_id;
  uint16_t recv_id_late;
  uint16_t sent_id;
  uint8_t current_link;
  struct timespec emit_time;
  struct deferred_pkt real[PACKET_BUFFER_SIZE];
  struct waited_pkt 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");
}

// @TODO Might be extracted from RR
int set_timeout(struct evt_core_ctx* evts, uint64_t milli_sec, struct waited_pkt* wpkt) {
  struct timespec now;
  struct itimerspec timer_config;
  char url[1024];
  struct evt_core_cat cat = {0};
  struct evt_core_fdinfo fdinfo = {0};
  fdinfo.cat = &cat;
  fdinfo.url = url;

  //printf("Will add a timeout of %ld ms\n", milli_sec);
  if (clock_gettime(CLOCK_REALTIME, &now) == -1) {
    perror("clock_gettime");
    exit(EXIT_FAILURE);
  }

  uint64_t ns = now.tv_nsec + (milli_sec % 1000) * 1000000;
  timer_config.it_value.tv_sec = now.tv_sec + milli_sec / 1000 + ns / 1000000000;
  timer_config.it_value.tv_nsec = ns % 1000000000;
  timer_config.it_interval.tv_sec = 60;
  timer_config.it_interval.tv_nsec = 0;

  fdinfo.fd = timerfd_create(CLOCK_REALTIME, 0);
  if (fdinfo.fd == -1) {
    perror("Unable to timerfd_create");
    exit(EXIT_FAILURE);
  }
  if (timerfd_settime (fdinfo.fd, TFD_TIMER_ABSTIME, &timer_config, NULL) == -1) {
    perror("Unable to timerfd_settime");
    exit(EXIT_FAILURE);
  }
  fdinfo.cat->name = "timeout";
  fdinfo.other = wpkt; // Should put the link number and the id
  fdinfo.free_other = NULL;
  sprintf(fdinfo.url, "timer:%ld:1", milli_sec);
  evt_core_add_fd (evts, &fdinfo);

  return fdinfo.fd;
}

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;
  char buffer[16];
  url_get_port (buffer, fdinfo->url);
  int link_num = atoi(buffer) - 7500; // @FIXME Hardcoded

  //printf("Selected url %s for pkt %d to be queued for delivery\n", fdinfo->url, bp->ip.ap.str.id);

  // 0. Update remote links
  if (ring_lt(rr->recv_id_late, bp->ip.ap.str.id) && !(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);
  }

  // 1. Update my links I can use thanks to target feedback
  if (bp->ip.ap.str.id > rr->my_links_ver && bp->ip.ap.str.bitfield != rr->my_links) {
    rr->my_links = bp->ip.ap.str.bitfield;
    rr->my_links_ver = bp->ip.ap.str.id;
    printf("Update my links | ");
    show_link_availability (rr);
  }

  // 2. If packet arrived too late, we discard it
  if (ring_gt(rr->recv_id, bp->ip.ap.str.id - 1)) {
    // Packet has already been delivered or dropped, we free the buffer
    fprintf(stderr, "Packet %d arrived too late (current: %d)\n", bp->ip.ap.str.id, rr->recv_id);
    mv_buffer_wtof (app_ctx, fdinfo);
    return;
  }

  // 3. If packet arrived too early, we register a timer
  //printf("%d < %d = %d\n", rr->recv_id, bp->ip.ap.str.id - 1, ring_lt(rr->recv_id, bp->ip.ap.str.id - 1));
  if (ring_lt(rr->recv_id, bp->ip.ap.str.id - 1)) {
    int64_t timeout = rr->mjit - (int64_t) bp->ip.ap.str.deltat;
    //printf("%ld - %ld = %ld\n", rr->mjit, (int64_t) bp->ip.ap.str.deltat, timeout);
    if (timeout <= 0) timeout = 0;
    int idx_waited = (bp->ip.ap.str.id - 1) % PACKET_BUFFER_SIZE;
    rr->wait[idx_waited].on = 1;
    rr->wait[idx_waited].id = bp->ip.ap.str.id - 1;
    rr->wait[idx_waited].link_num = bp->ip.ap.str.prevlink;
    rr->wait[idx_waited].timer_fd = set_timeout(ctx, timeout, &rr->wait[idx_waited]);
  }

  // 4. If packet has not already a timer or has a wrong timer
  int idx_real = bp->ip.ap.str.id % PACKET_BUFFER_SIZE;
  if (rr->wait[idx_real].on && rr->wait[idx_real].id != bp->ip.ap.str.id) {
    fprintf(stderr, "Waiting array is full, BUG\n");
    exit(EXIT_FAILURE);
  } else if (!rr->wait[idx_real].on) {
    rr->wait[idx_real].on = 1;
    rr->wait[idx_real].id = bp->ip.ap.str.id;
    rr->wait[idx_real].link_num = link_num;
    rr->wait[idx_real].timer_fd = set_timeout(ctx, rr->mjit + 1, &rr->wait[idx_real]);
  }

  // 5. We queue the packet
  if (rr->real[idx_real].on && rr->real[idx_real].id != bp->ip.ap.str.id) {
    fprintf(stderr, "Real array is full for id=%d, idx=%d, BUG: [\n", bp->ip.ap.str.id, idx_real);
    for (int i = 0; i < PACKET_BUFFER_SIZE; i++) {
      printf("\t%d => %d\n", i, rr->real[i].on);
    }
    printf("]\n");
    exit(EXIT_FAILURE);
  } else if (!rr->real[idx_real].on) {
    rr->real[idx_real].on = 1;
    rr->real[idx_real].id = bp->ip.ap.str.id;
    rr->real[idx_real].idx = idx_real;
    rr->real[idx_real].link_fd = fdinfo->fd;
    mv_buffer_rtoa(app_ctx, fdinfo, &rr->real[idx_real].idx);
    //printf("%d is added to real as %d\n", bp->ip.ap.str.id, idx_real);
  } else {
    fprintf(stdout, "Packet %d already received (current: %d)\n", bp->ip.ap.str.id, rr->recv_id);
    mv_buffer_wtof (app_ctx, fdinfo);
  }
}

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

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

  // 2. Get the buffer and update rr state
  struct buffer_packet* bp = get_app_buffer (app_ctx, &dp->idx);
  int idx_real = bp->ip.ap.str.id % PACKET_BUFFER_SIZE;
  rr->real[idx_real].on = 0;
  //printf("%d is removed from real as %d\n", bp->ip.ap.str.id, idx_real);
  //printf("Selected url %s for pkt %d to be delivered\n", fdinfo->url, bp->ip.ap.str.id);

  // 3. We update our cursor
  rr->recv_id = bp->ip.ap.str.id;

  // 4. We free the buffer if it's a control packet and quit
  /*if (bp->ip.ap.str.flags & PKT_CONTROL) {
    mv_buffer_atof (app_ctx, &dp->idx);
    return;
  }*/

  // 5. Find its target
  sprintf(url, "udp:write:127.0.0.1:%d", bp->ip.ap.str.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 :( \n", url);
    //mv_buffer_wtor (app_ctx, fdinfo, bp);
    mv_buffer_atof (app_ctx, &dp->idx);
  }

  // 6. 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_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;

  while(1) {
    //printf("Trying to deliver %d\n", rr->recv_id+1);
    struct deferred_pkt* def = &rr->real[(rr->recv_id+1) % PACKET_BUFFER_SIZE];
    if (!def->on) break;
    fdinfo = evt_core_get_from_fd (ctx, def->link_fd);
    if (fdinfo == NULL) {
      fprintf(stderr, "An error occured as the link seems to be closed for the requested fd\n");
      rr->recv_id++;
      continue;
    }

    rr_deliver(ctx, fdinfo, def);
    //printf("Delivered %d\n", rr->recv_id);
  }
}

//------

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;

  // 1. Register packet in our queue
  rr_pkt_register(ctx, fdinfo, bp);

  // 2. Process queue
  rr_pkt_unroll (ctx, app_ctx);

  return 0;
co_error:
  perror("Failed to TCP read");
  exit(EXIT_FAILURE);
}

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;
   char url[255];

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

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

  bp->ip.ap.str.id = rr->sent_id;
  bp->ip.ap.str.flags = 0;
  bp->ip.ap.str.deltat = mili_sec;
  bp->ip.ap.str.bitfield = rr->remote_links;
  bp->ip.ap.str.prevlink = rr->current_link;
  //printf("Will send packet id=%d\n", bp->ip.ap.str.id);

  rr->emit_time = curr;
  rr->sent_id++;

  // 2. Try to find someone to send it
  int max = 10;
  uint8_t sel_link = rr->current_link;
  while(max-- >= 0) {
    sel_link = (sel_link + 1) % 8;
    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_waiting_bootstrap && !app_ctx->is_rdy) goto not_ready; // Some links are down
    if (!app_ctx->ap.is_healing || rr->my_links & (1 << sel_link)) {
      rr->current_link = sel_link;
      mv_buffer_rtow (app_ctx, fdinfo, to_fdinfo);
      main_on_tcp_write(ctx, to_fdinfo);
      return 0;
    } else {
      dup_buffer_tow(app_ctx, bp, to_fdinfo);
      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;

co_error:
  perror("Failed to UDP read");
  exit(EXIT_FAILURE);
}

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

  struct waited_pkt* pkt = fdinfo->other;
  pkt->on = 0;
  if (ring_gt (pkt->id, rr->recv_id_late)) rr->recv_id_late = pkt->id;
  if (ring_le (pkt->id, rr->recv_id)) goto end;

  printf("Timer reached for packet %d\n", pkt->id);

  // !BLACKLIST LINK
  printf("Blacklist link=%d | ", pkt->link_num);
  rr->remote_links &= 0xff ^ 1 << pkt->link_num;
  show_link_availability (rr);

  while (ring_lt(rr->recv_id, pkt->id)) {
    rr->recv_id++;
    rr_pkt_unroll (ctx, app_ctx);
  }

end:
  evt_core_rm_fd(ctx, fdinfo->fd);
  return 1;
}

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


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_id = 1;
  rr->recv_id = 0;
  rr->recv_id_late = 0;
  app_ctx->misc = rr;
}