tor_multipath_voip/src/algo_rr.c

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

#define WAITING 10

struct deferred_pkt {
  int link_fd;
  struct buffer_packet* bp;
};

struct rr_ctx {
  uint8_t my_links;
  uint16_t my_links_ver;
  uint8_t remote_links;
  uint16_t mjit;
  uint16_t recv_id;
  uint16_t sent_id;
  uint8_t current_link;
  struct deferred_pkt real[WAITING];
  struct buffer_packet stub_bp;
};

int rr_on_tcp_read(struct evt_core_ctx* ctx, struct evt_core_fdinfo* fdinfo);
int rr_on_tcp_write(struct evt_core_ctx* ctx, struct evt_core_fdinfo* fdinfo);
int rr_on_udp_read(struct evt_core_ctx* ctx, struct evt_core_fdinfo* fdinfo);
int rr_on_udp_write(struct evt_core_ctx* ctx, struct evt_core_fdinfo* fdinfo);

int rr_on_tcp_co(struct evt_core_ctx* ctx, struct evt_core_fdinfo* fdinfo) {
  int conn_sock1, conn_sock2;
  struct sockaddr_in addr;
  socklen_t in_len;
  char url[1024], port[6];
  struct evt_core_cat local_cat = {0};
  struct evt_core_fdinfo to_fdinfo = {0};
  to_fdinfo.cat = &local_cat;
  to_fdinfo.url = url;

  in_len = sizeof(addr);
  conn_sock1 = accept(fdinfo->fd, (struct sockaddr*)&addr, &in_len);

  if (conn_sock1 == -1) goto co_error;
  conn_sock2 = dup(conn_sock1);
  if (conn_sock2 == -1) goto co_error;
  //printf("fd=%d accepts, creating fds=%d,%d\n", fd, conn_sock1, conn_sock2);

  url_get_port(port, fdinfo->url);

  to_fdinfo.fd = conn_sock1;
  to_fdinfo.cat->name = "tcp-read";
  sprintf(to_fdinfo.url, "tcp:read:127.0.0.1:%s", port);
  evt_core_add_fd (ctx, &to_fdinfo);

  to_fdinfo.fd = conn_sock2;
  to_fdinfo.cat->name = "tcp-write";
  sprintf(to_fdinfo.url, "tcp:write:127.0.0.1:%s", port);
  evt_core_add_fd (ctx, &to_fdinfo);

  return 1;

co_error:
  perror("Failed to handle new connection");
  exit(EXIT_FAILURE);
}

void set_timeout(struct evt_core_ctx* evts, uint64_t micro_sec, int64_t link) {
  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;

  if (clock_gettime(CLOCK_REALTIME, &now) == -1) {
    perror("clock_gettime");
    exit(EXIT_FAILURE);
  }

  timer_config.it_value.tv_sec = now.tv_sec + micro_sec / 1000;
  timer_config.it_value.tv_nsec = now.tv_nsec + micro_sec % 1000 * 1000000;
  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_time");
    exit(EXIT_FAILURE);
  }
  fdinfo.cat->name = "timeout";
  fdinfo.other = (void*)link; // Should put the link number and the id
  fdinfo.free_other = NULL;
  sprintf(fdinfo.url, "timer:%ld:1", micro_sec);
  evt_core_add_fd (evts, &fdinfo);
}

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

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

  // 2. If packet arrived too late
  if (ring_gt(rr->recv_id, bp->ip.ap.str.id - 1)) {
    if (bp->ip.ap.str.flags & PKT_TIMEOUT) return 0; // We don't use real buffer pkt for PKT_TIMEOUT
    // Packet has already been delivered or dropped, we free the buffer
    g_hash_table_remove (app_ctx->used_buffer, &(fdinfo->fd));
    memset(bp, 0, sizeof(struct buffer_packet));
    g_queue_push_tail(app_ctx->free_buffer, bp);
    return 0;
  }

  // 3. If packet arrived too early
  if (ring_lt(rr->recv_id, bp->ip.ap.str.id - 1)) {
    int64_t timeout = rr->mjit - (int64_t) bp->ip.ap.str.deltat;
    if (timeout <= 0) timeout = 0;
    set_timeout(ctx, timeout, bp->ip.ap.str.prevlink);
    // Add a buffer to stub too
    // Bitfield can be anything as a greater packet has triggered the bitfield update before
    int idx = bp->ip.ap.str.id % PACKET_BUFFER_SIZE;
    rr->real[idx].bp = bp;
    rr->real[idx].link_fd = fdinfo->fd;
    return 0;
  }

  // 4. If we were waiting this packet
  rr->recv_id = bp->ip.ap.str.id;

  char buffer[16];
  url_get_port (buffer, fdinfo->url);
  int link_num = atoi(buffer) - 7500; // @FIXME Hardcoded

  // 4.1 This is a timeout packet, we set the link as dead
  if (bp->ip.ap.str.flags & PKT_TIMEOUT) {
    rr->remote_links &= (1 << link_num) ^ UINT16_MAX;
    return 0;
  }

  // 4.2 This is a control packet, we set the link as alive
  if (bp->ip.ap.str.flags & PKT_CONTROL) {
    rr->remote_links |= 1 << link_num;
    return 0;
  }

  return 1;
}

void rr_deliver(struct evt_core_ctx* ctx, struct buffer_packet* bp, struct evt_core_fdinfo* fdinfo) {
  struct evt_core_fdinfo *to_fdinfo = NULL;
  struct algo_ctx* app_ctx = fdinfo->cat->app_ctx;
  char url[255];

  // 1. A whole packet has been read, we will 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);
  }

  // 2. We move the buffer and notify the target
  mv_buffer_rtow (app_ctx, fdinfo, to_fdinfo, bp);
  rr_on_udp_write(ctx, to_fdinfo);
}

void rr_pkt_recv(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;

  do {
    if(rr_update_states(ctx, bp, fdinfo)) rr_deliver(ctx, bp, fdinfo);
    do {
      struct deferred_pkt* def = &rr->real[(rr->recv_id+1) % PACKET_BUFFER_SIZE];
      if (def->bp == NULL) break;
      def->bp = NULL;
      struct evt_core_fdinfo* 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++;
      }
    } while (fdinfo == NULL);
  } while(1);
}

int rr_on_tcp_read(struct evt_core_ctx* ctx, struct evt_core_fdinfo* fdinfo) {
  struct buffer_packet* bp;
  struct algo_ctx* app_ctx = fdinfo->cat->app_ctx;
  int read_res = FDS_READY;

  // 1. Get current read buffer OR a new read buffer OR subscribe to be notified later
  if ((bp = get_read_buffer(app_ctx, fdinfo)) == NULL) return 1;

  // 2. Try to read a whole packet in the buffer
  while (bp->mode == BP_READING) {
    read_res = read_packet_from_tcp (fdinfo->fd, bp);
    if (read_res == FDS_ERR) goto co_error;
    if (read_res == FDS_AGAIN) return 1;
  }

  // 3. Logic on packet
  rr_pkt_recv (ctx, fdinfo, bp);

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

int rr_on_udp_write (struct evt_core_ctx* ctx, struct evt_core_fdinfo* fdinfo) {
  struct buffer_packet* bp;
  struct algo_ctx* app_ctx = fdinfo->cat->app_ctx;
  int write_res = FDS_READY;

  // 1. Get current write buffer OR a buffer from the waiting queue OR leave
  if ((bp = get_write_buffer(app_ctx, fdinfo)) == NULL) return 1;

  // 2. Write buffer
  write_res = write_packet_to_udp(fdinfo->fd, bp, fdinfo->other);
  if (write_res == FDS_ERR) goto co_error;
  if (write_res == FDS_AGAIN) return 1;

  // 3. A whole packet has been written
  // Release the buffer and notify
  mv_buffer_wtor(app_ctx, fdinfo, bp);
  notify_read(ctx, app_ctx);

  return 0;
co_error:
  perror("Failed to UDP write");
  exit(EXIT_FAILURE);
}

int 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;
  uint16_t id;

  evt_core_rm_fd (ctx, fdinfo->fd); // We use the timeout only once
  int fd = (int)fdinfo->other;
  struct evt_core_fdinfo* tfdinfo = evt_core_get_from_fd (ctx, fd);
  if (tfdinfo == NULL) {
    fprintf(stderr, "An error occured as the link seems to be closed for the requested fd\n");
    rr->recv_id++;
    return 1;
  }

  rr->stub_bp.ip.ap.str.id = 0; //?
  rr->stub_bp.ip.ap.str.flags = PKT_TIMEOUT;
  rr->stub_bp.ip.ap.str.deltat = rr->mjit;
  rr->stub_bp.ip.ap.str.prevlink = UINT8_MAX;
  rr_pkt_recv (ctx, tfdinfo, &rr->stub_bp);
  return 1;
}

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

  // 1. If has a "used" buffer, remove it
  bp = g_hash_table_lookup (app_ctx->used_buffer, &(fdinfo->fd));
  if (bp != NULL) {
    g_hash_table_remove (app_ctx->used_buffer, &(fdinfo->fd));
    memset(bp, 0, sizeof(struct buffer_packet));
    g_queue_push_tail(app_ctx->free_buffer, bp);
  }

  // 2. If appears in the write waiting queue, remove it
  GQueue* writew = g_hash_table_lookup (app_ctx->write_waiting, &(fdinfo->fd));
  while (writew != NULL && (bp = g_queue_pop_head (writew)) != NULL) {
    memset(bp, 0, sizeof(struct buffer_packet));
    g_queue_push_tail(app_ctx->free_buffer, bp);
  }
  g_hash_table_remove (app_ctx->write_waiting, &(fdinfo->fd));

  // 3. If appears in the read waiting queue, remove it
  g_queue_remove_all (app_ctx->read_waiting, &(fdinfo->fd));

  return 0;
}

void algo_rr(struct evt_core_ctx* evt, struct algo_skel* as) {
  struct algo_ctx* ctx = malloc(sizeof(struct algo_ctx));
  if (ctx == NULL) goto init_err;
  memset(ctx, 0, sizeof(struct algo_ctx));
  ctx->free_buffer = g_queue_new ();
  ctx->read_waiting = g_queue_new ();
  ctx->used_buffer = g_hash_table_new(g_int_hash, g_int_equal);
  ctx->write_waiting = g_hash_table_new_full (g_int_hash, g_int_equal, NULL, naive_free_simple);
  struct rr_ctx* rr = malloc(sizeof(struct rr_ctx));
  if (rr == NULL) goto init_err;
  memset(rr, 0, sizeof(struct rr_ctx));
  rr->mjit = 200;
  rr->my_links = 0xff;
  rr->remote_links = 0xff;
  ctx->misc = rr;
  for (int i = 0; i < sizeof(ctx->bps) / sizeof(ctx->bps[0]); i++) {
    g_queue_push_tail(ctx->free_buffer, &(ctx->bps[i]));
  }


  as->on_tcp_co.name = "tcp-listen";
  as->on_tcp_co.flags = EPOLLIN;
  as->on_tcp_co.free_app_ctx = free_nothing;
  as->on_tcp_co.cb = rr_on_tcp_co;

  as->on_tcp_read.name = "tcp-read";
  as->on_tcp_read.flags = EPOLLIN | EPOLLET | EPOLLRDHUP;
  as->on_tcp_read.app_ctx = ctx;
  as->on_tcp_read.free_app_ctx = free_naive;
  as->on_tcp_read.cb = rr_on_tcp_read;
  as->on_tcp_read.err_cb = rr_on_err;
  ctx->ref_count++;

/*
  as->on_udp_read.name = "udp-read";
  as->on_udp_read.flags = EPOLLIN | EPOLLET;
  as->on_udp_read.app_ctx = ctx;
  as->on_udp_read.free_app_ctx = free_naive;
  as->on_udp_read.cb = on_udp_read;
  as->on_udp_read.err_cb = on_err;
  ctx->ref_count++;

  as->on_tcp_write.name = "tcp-write";
  as->on_tcp_write.flags = EPOLLOUT | EPOLLET | EPOLLRDHUP;
  as->on_tcp_write.app_ctx = ctx;
  as->on_tcp_write.free_app_ctx = free_naive;
  as->on_tcp_write.cb = on_tcp_write;
  as->on_tcp_write.err_cb = on_err;
  ctx->ref_count++;
*/

  as->on_udp_write.name = "udp-write";
  as->on_udp_write.flags = EPOLLOUT | EPOLLET;
  as->on_udp_write.app_ctx = ctx;
  as->on_udp_write.free_app_ctx = free_naive;
  as->on_udp_write.cb = rr_on_udp_write;
  as->on_udp_write.err_cb = rr_on_err;
  ctx->ref_count++;

  struct evt_core_cat tcat = {
    .name = "timeout",
    .flags = EPOLLIN | EPOLLET,
    .app_ctx = ctx,
    .free_app_ctx = free_naive,
    .cb = rr_on_timer,
    .err_cb = NULL
  };
  ctx->ref_count++;
  evt_core_add_cat(evt, &tcat);

  return;
init_err:
  fprintf(stderr, "Failed to init algo naive\n");
  exit(EXIT_FAILURE);
}