tor_multipath_voip/src/algo_naive.c

#include "algo_skel.h"
#define NAIVE_BUFFER 128

struct naive_ctx {
  int ref_count;
  struct buffer_packet tcp_to_udp;
  struct buffer_packet udp_to_tcp;
  struct ring_buffer tcp_to_udp_rb;
  struct ring_buffer udp_to_tcp_rb;
  struct sockaddr_in udp_addr;
  socklen_t udp_addrlen;
};

void free_nothing(void* app_ctx) {}
void free_naive(void* app_ctx) {
  struct naive_ctx* ctx = (struct naive_ctx*) app_ctx;
  ctx->ref_count--;
  if (ctx->ref_count <= 0) free(ctx);
}

void on_tcp_co(struct evt_core_ctx* ctx, struct evt_core_cat* cat, int fd) {
  int conn_sock1, conn_sock2;
  struct sockaddr addr;
  socklen_t in_len;
  struct epoll_event current_event;

  in_len = sizeof(addr);

  conn_sock1 = accept(fd, &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);
  evt_core_add_fd (ctx, "tcp-read", conn_sock1);
  evt_core_add_fd (ctx, "tcp-write", conn_sock2);

  return;

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

enum FD_STATE read_packet_from_tcp(int fd, struct buffer_packet* bp) {
  int nread;
  size_t pkt_size_size = sizeof(bp->ip.ap.str.size);
  if (bp->mode == BP_WRITING) return FDS_ERR;

  while (bp->aread < pkt_size_size) {
    nread = read(fd, &(bp->ip.ap.raw) + bp->aread, pkt_size_size - bp->aread);
    if (nread == 0) return FDS_AGAIN;
    if (nread == -1 && errno == EAGAIN) return FDS_AGAIN;
    if (nread == -1) return FDS_ERR;
    bp->aread += nread;
  }

  while (bp->aread < bp->ip.ap.str.size) {
    nread = read(fd, &(bp->ip.ap.raw) + bp->aread, bp->ip.ap.str.size - bp->aread);
    if (nread == 0) return FDS_AGAIN;
    if (nread == -1 && errno == EAGAIN) return FDS_AGAIN;
    if (nread == -1) return FDS_ERR;
    bp->aread += nread;
  }
  bp->mode = BP_WRITING;

  return FDS_READY;
}

enum FD_STATE write_packet_to_udp(int fd, struct buffer_packet* bp, struct sockaddr* addr, socklen_t addrlen) {
  int nwrite, bytes_to_send;
  size_t pkt_size_size = sizeof(bp->ip.ap.str.size);

  if (bp->mode == BP_READING) return FDS_ERR;

  bytes_to_send = bp->ip.ap.str.size - pkt_size_size;
  nwrite = sendto(fd,
                  &(bp->ip.ap.str.payload),
                  bytes_to_send,
                  0,
                  addr,
                  addrlen);
  if (nwrite == -1 && errno == EAGAIN) return FDS_AGAIN;
  if (nwrite != bytes_to_send) return FDS_ERR;

  return FDS_READY;
}

void tcp_to_udp(struct evt_core_ctx* ctx, struct evt_core_cat* cat, int fd) {
  // Get target file descriptor
  struct evt_core_cat* udp = g_hash_table_lookup (ctx->catlist, "udp-write");
  if (udp == NULL || udp->socklist->len < 1) goto co_error;
  int udp_fd = g_array_index(udp->socklist, int, 0);

  // Init data structures for the transfer
  struct naive_ctx* app_ctx = cat->app_ctx;
  struct buffer_packet* bp = &(app_ctx->tcp_to_udp);
  int read_res = FDS_READY;
  int write_res = FDS_READY;

  // Either we can read something, either we can write something
  while ((read_res != FDS_AGAIN && bp->mode == BP_READING)
         || (write_res != FDS_AGAIN && bp->mode == BP_WRITING)) {

    // 1. Read packet from TCP socket
    if (bp->mode == BP_READING) {
      read_res = read_packet_from_tcp (fd, bp);
      if (read_res == FDS_ERR) goto co_error;
    }

    // 2. Write packet to UDP socket
    if (bp->mode == BP_WRITING) {
      write_res = write_packet_to_udp(udp_fd,
                                      bp,
                                      (struct sockaddr*) &(app_ctx->udp_addr),
                                      app_ctx->udp_addrlen);
      if (read_res == FDS_ERR) goto co_error;
    }
  }
  return;

co_error:
  perror("Failed to handle read write for tcp_to_udp");
  exit(EXIT_FAILURE);
}

void udp_to_tcp(struct evt_core_ctx* ctx, struct evt_core_cat* cat, int fd) {
  // Get target file descriptor
  struct evt_core_cat* tcp = g_hash_table_lookup (ctx->catlist, "tcp-write");
  if (tcp == NULL || tcp->socklist->len < 1) goto co_error;
  int tcp_fd = g_array_index(tcp->socklist, int, 0);

  // Init data structures for the transfer
  struct naive_ctx* app_ctx = cat->app_ctx;
  struct ring_buffer* rb = &(app_ctx->udp_to_tcp_rb);
  char buffer[NET_BUFFER_SIZE];
  int nread, nwrite, rb_free_space;

  while (1) {
    rb_free_space = ring_buffer_free_space (rb); // We can't afford to read more
    app_ctx->udp_addrlen = sizeof(struct sockaddr_in);
    nread = recvfrom(fd, buffer, rb_free_space, MSG_TRUNC, (struct sockaddr*)&(app_ctx->udp_addr), &(app_ctx->udp_addrlen));     // Effective read
    if (nread == 0) return;                      // End of file
    if (nread == -1 && errno == EAGAIN) return;  // No more data to read
    if (nread == -1) goto co_error;              // A bad error
    if (nread > rb_free_space) {
      fprintf(stderr, "[warn!] we partially read a UDP packet\n");
    } else {
      ring_buffer_write(rb, buffer, nread);        // Persist read data in our buffer
    }

    nread = ring_buffer_read(rb, buffer, RING_BUFFER_SIZE);
    nwrite = write(tcp_fd, buffer, nread);
    if (nwrite == -1 && errno == EAGAIN) return;
    if (nwrite == -1) goto co_error;
    printf("written to tcp_fd=%d\n", nwrite);
    ring_buffer_ack_read (rb, nwrite);
  }

  return;

co_error:
  perror("Failed to handle read write for udp_to_tcp");
  exit(EXIT_FAILURE);
}

void algo_naive(struct algo_skel* as) {
  struct naive_ctx* ctx = malloc(sizeof(struct naive_ctx));
  if (ctx == NULL) goto init_err;
  memset(ctx, 0, sizeof(struct naive_ctx));
  ctx->udp_addrlen = sizeof(struct sockaddr_in);

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

  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 = tcp_to_udp;
  as->on_tcp_read.socklist = NULL;
  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 = udp_to_tcp;
  as->on_udp_read.socklist = NULL;
  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 = udp_to_tcp;
  as->on_tcp_write.socklist = NULL;
  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 = tcp_to_udp;
  as->on_udp_write.socklist = NULL;
  ctx->ref_count++;

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