text: /*
   * RaspberryPi based FM transmitter.  For the original idea, see:
   *
   * http://www.icrobotics.co.uk/wiki/index.php/Turning_the_Raspberry_Pi_Into_an_FM_Transmitter
   *
   * All credit to Oliver Mattos and Oskar Weigl for creating the original code.
   *
   * I have taken their idea and reworked it to use the Pi DMA engine, so
   * reducing the CPU overhead for playing a .wav file from 100% to about 1.6%.
   *
   * I have implemented this in user space, using an idea I picked up from Joan
   * on the Raspberry Pi forums - credit to Joan for the DMA from user space
   * idea.
   *
   * The idea of feeding the PWM FIFO in order to pace DMA control blocks comes
   * from ServoBlaster, and I take credit for that :-)
   *
   * This code uses DMA channel 0 and the PWM hardware, with no regard for
   * whether something else might be trying to use it at the same time (such as
   * the 3.5mm jack audio driver).
   *
   * I know nothing much about sound, subsampling, or FM broadcasting, so it is
   * quite likely the sound quality produced by this code can be improved by
   * someone who knows what they are doing.  There may be issues realting to
   * caching, as the user space process just writes to its virtual address space,
   * and expects the DMA controller to see the data; it seems to work for me
   * though.
   *
   * NOTE: THIS CODE MAY WELL CRASH YOUR PI, TRASH YOUR FILE SYSTEMS, AND
   * POTENTIALLY EVEN DAMAGE YOUR HARDWARE.  THIS IS BECAUSE IT STARTS UP THE DMA
   * CONTROLLER USING MEMORY OWNED BY A USER PROCESS.  IF THAT USER PROCESS EXITS
   * WITHOUT STOPPING THE DMA CONTROLLER, ALL HELL COULD BREAK LOOSE AS THE
   * MEMORY GETS REALLOCATED TO OTHER PROCESSES WHILE THE DMA CONTROLLER IS STILL
   * USING IT.  I HAVE ATTEMPTED TO MINIMISE ANY RISK BY CATCHING SIGNALS AND
   * RESETTING THE DMA CONTROLLER BEFORE EXITING, BUT YOU HAVE BEEN WARNED.  I
   * ACCEPT NO LIABILITY OR RESPONSIBILITY FOR ANYTHING THAT HAPPENS AS A RESULT
   * OF YOU RUNNING THIS CODE.  IF IT BREAKS, YOU GET TO KEEP ALL THE PIECES.
   *
   * NOTE ALSO:  THIS MAY BE ILLEGAL IN YOUR COUNTRY.  HERE ARE SOME COMMENTS
   * FROM MORE KNOWLEDGEABLE PEOPLE ON THE FORUM:
   *
   * "Just be aware that in some countries FM broadcast and especially long
   * distance FM broadcast could get yourself into trouble with the law, stray FM
   * broadcasts over Airband aviation is also strictly forbidden."
   *
   * "A low pass filter is really really required for this as it has strong
   * harmonics at the 3rd, 5th 7th and 9th which sit in licensed and rather
   * essential bands, ie GSM, HAM, emergency services and others. Polluting these
   * frequencies is immoral and dangerous, whereas "breaking in" on FM bands is
   * just plain illegal."
   *
   * "Don't get caught, this GPIO use has the potential to exceed the legal
   * limits by about 2000% with a proper aerial."
   *
   *
   * As for the original code, this code is released under the GPL.
   *
   * Richard Hirst <richardghirst@gmail.com>  December 2012
   */
  
  #include <arpa/inet.h>
  #include <assert.h>
  #include <errno.h>
  #include <fcntl.h>
  #include <getopt.h>
  #include <jansson.h>
  #include <math.h>
  #include <memory.h>
  #include <signal.h>
  #include <stdarg.h>
  #include <stdint.h>
  #include <stdio.h>
  #include <stdlib.h>
  #include <string.h>
  #include <sys/mman.h>
  #include <sys/socket.h>
  #include <sys/stat.h>
  #include <sys/time.h>
  #include <sys/types.h>
  #include <time.h>
  #include <unistd.h>
  #include "cfv_bellman.h"
  
  // #include "mailbox.h"
  // #define MBFILE            DEVICE_FILE_NAME    /* From mailbox.h */
  
  /**
   * RC commands are almost always sent at some multiple of 5 us.  The basic idea
   * is to maintain a buffer of 4000 values to write to the clock control
   * register and then arrange for the DMA controller to write the values
   * sequentially at 5us intervals.  The control code can then wake up every 10ms
   * or so and populate the buffer with new samples.  At 5us per sample, a 4000
   * sample buffer will last 20ms, so waking every 10ms should be sufficient.
   *
   * Total memory needed is:
   *
   * The frequencies      4000 * 4
   * CBs to set the frequency 4000 * 32
   * CBs to cause delays      4000 * 32
   *
   * Process can wake every 10ms and update all samples based on where the DMA
   * CB is pointed.
   */
  
  // #if (RASPI)==1
  // #define PERIPH_VIRT_BASE 0x20000000
  // #define PERIPH_PHYS_BASE 0x7e000000
  // #define DRAM_PHYS_BASE 0x40000000
  // #define MEM_FLAG 0x0c
  // #elif (RASPI)==2
  // #define PERIPH_VIRT_BASE 0x3f000000
  // #define PERIPH_PHYS_BASE 0x7e000000
  // #define DRAM_PHYS_BASE 0xc0000000
  // #define MEM_FLAG 0x04
  // #else
  // #error Unknown Raspberry Pi version (variable RASPI)
  // #endif
  
  // #define NUM_SAMPLES     4000
  // #define NUM_CBS         (NUM_SAMPLES * 2)
  
  // #define BCM2708_DMA_NO_WIDE_BURSTS  (1<<26)
  // #define BCM2708_DMA_WAIT_RESP       (1<<3)
  // #define BCM2708_DMA_D_DREQ      (1<<6)
  // #define BCM2708_DMA_PER_MAP(x)      ((x)<<16)
  // #define BCM2708_DMA_END         (1<<1)
  // #define BCM2708_DMA_RESET       (1<<31)
  // #define BCM2708_DMA_INT         (1<<2)
  // #define BCM2708_DMA_ABORT       (1<<30)  /* Stop current CB, go to next, WO */
  // #define BCM2708_DMA_ACTIVE      (1<<0)
  
  // #define DMA_CS          (0x00/4)
  // #define DMA_CONBLK_AD       (0x04/4)
  // #define DMA_DEBUG       (0x20/4)
  
  // #define DMA_BASE_OFFSET        0x00007000
  // #define PWM_BASE_OFFSET        0x0020C000
  // #define CLK_BASE_OFFSET            0x00101000
  // #define GPIO_BASE_OFFSET    0x00200000
  
  // #define DMA_VIRT_BASE        (PERIPH_VIRT_BASE + DMA_BASE_OFFSET)
  // #define PWM_VIRT_BASE        (PERIPH_VIRT_BASE + PWM_BASE_OFFSET)
  // #define CLK_VIRT_BASE        (PERIPH_VIRT_BASE + CLK_BASE_OFFSET)
  // #define GPIO_VIRT_BASE        (PERIPH_VIRT_BASE + GPIO_BASE_OFFSET)
  
  // #define PWM_PHYS_BASE        (PERIPH_PHYS_BASE + PWM_BASE_OFFSET)
  
  // #define DMA_LEN         0x24
  // #define PWM_LEN         0x28
  // #define CLK_LEN         0xA8
  // #define GPIO_LEN        0xB4
  
  // #define PWM_CTL         (0x00/4)
  // #define PWM_DMAC        (0x08/4)
  // #define PWM_RNG1        (0x10/4)
  // #define PWM_FIFO        (0x18/4)
  
  // #define PWMCLK_CNTL     40
  // #define PWMCLK_DIV      41
  
  // #define GPCLK_CNTL      (0x70/4)
  // #define GPCLK_DIV       (0x74/4)
  
  // #define CM_GP0DIV (0x7e101074)
  
  // #define PWMCTL_MODE1        (1<<1)
  // #define PWMCTL_PWEN1        (1<<0)
  // #define PWMCTL_CLRF     (1<<6)
  // #define PWMCTL_USEF1        (1<<5)
  
  // #define PWMDMAC_ENAB        (1<<31)
  // /**
  //  * I think this means it requests as soon as there is one free slot in the FIFO
  //  * which is what we want as burst DMA would mess up our timing..
  //  */
  // #define PWMDMAC_THRSHLD     ((15<<8)|(15<<0))
  
  // #define GPFSEL0         (0x00/4)
  
  #define BUFFER_SIZE 20000
  
  // struct dma_cb_t {
  //     uint32_t info, src, dst, length,
  //              stride, next, pad[2];
  // };
  
  // #define BUS_TO_PHYS(x) ((x)&~0xC0000000)
  
  // struct page_map_t {
  //     uint8_t* virtaddr;
  //     uint32_t physaddr;
  // };
  
  
  struct command_node_t {
      float burst_us;
      float spacing_us;
      int repeats;
      float frequency;
      float dead_frequency;
  
      struct command_node_t* next;
  };
  
  // struct page_map_t* page_map;
  
  // static struct {
  //     int handle;            /* From mbox_open() */
  //     unsigned mem_ref;     /* From mem_alloc() */
  //     unsigned bus_addr;    /* From mem_lock() */
  //     uint8_t *virt_addr;    /* From mapmem() */
  // } mbox;
  
  // static volatile uint32_t* pwm_reg;
  // static volatile uint32_t* clk_reg;
  // static volatile uint32_t* dma_reg;
  // static volatile uint32_t* gpio_reg;
  
  // struct control_data_s {
  //     struct dma_cb_t cb[NUM_CBS];
  //     uint32_t sample[NUM_SAMPLES];
  // };
  
  // #define PAGE_SIZE   4096
  // #define PAGE_SHIFT  12
  // #define NUM_PAGES   (int)((sizeof(struct control_data_s) + PAGE_SIZE - 1) >> PAGE_SHIFT)
  
  // static struct control_data_s* ctl;
  static int __attribute__((annotate("sensitive"))) socket_handle = 0;
  static int __attribute__((annotate("sensitive"))) tcp_socket_connection = 0;
  static int __attribute__((annotate("sensitive"))) websocket = 0;
  
  struct pi_options {
      int verbose;
      int udp;
      int tcp;
      int port;
  };
  static struct pi_options get_args(int argc, char* argv[]);
  static void print_usage(const char* program);
  // static void udelay(int us);
  static void terminate(int unused);
  static void fatal(char* fmt, ...);
  // static uint32_t mem_virt_to_phys(void* virt);
  // static uint32_t mem_phys_to_virt(uint32_t phys);
  // static void* map_peripheral(uint32_t base, uint32_t len);
  static int fill_buffer(
      const struct command_node_t* command,
      const struct command_node_t* next_command,
      int bytes_count
      // struct control_data_s* ctl_,
      // const volatile uint32_t* dma_reg_
  );
  // static int frequency_to_control(float frequency);
  static int parse_json(
      const char* json,
      struct command_node_t** new_command,
      int* request_response
  );
  static void free_command(struct command_node_t* command);
  #define BYTE unsigned char
  #define WORD unsigned int
  struct SHA1_CTX {
      BYTE data[64];
      WORD datalen;
      unsigned long long bitlen;
      WORD state[5];
      WORD k[4];
  };
  void sha1_init(struct SHA1_CTX *ctx);
  void sha1_update(struct SHA1_CTX *ctx, const BYTE data[], size_t len);
  void sha1_final(struct SHA1_CTX *ctx, BYTE hash[]);
  void sha1_transform(struct SHA1_CTX *ctx, const BYTE data[]);
  int base64_encode(unsigned char *in, int inlen, char *out);
  const char* websocket_handshake_response(const char* const handshake);
  void decode_websocket_message(char message[]);
  
  
  int main(int argc, char** argv) {
      struct pi_options options = get_args(argc, argv);
      int i;
  
      struct sigaction sa;
      /**
       * Catch all signals possible - it is vital we kill the DMA engine
       * on process exit!
       */
      for (i = 1; i < 64; i++) {
          /* These are uncatchable or harmless */
          if (
              i != SIGKILL
              && i != SIGSTOP
              && i != SIGVTALRM
              && i != SIGWINCH
              && i != SIGPROF
          ) {
              memset(&sa, 0, sizeof(sa));
              sa.sa_handler = terminate;
              sigaction(i, &sa, NULL);
          }
      }
  
      /* Calculate the frequency control word */
      /* The fractional part is stored in the lower 12 bits */
      float frequency = 49.830;
  
      // dma_reg = map_peripheral(DMA_VIRT_BASE, DMA_LEN);
      // pwm_reg = map_peripheral(PWM_VIRT_BASE, PWM_LEN);
      // clk_reg = map_peripheral(CLK_VIRT_BASE, CLK_LEN);
      // gpio_reg = map_peripheral(GPIO_VIRT_BASE, GPIO_LEN);
  
      // mbox.handle = mbox_open();
      // if (mbox.handle < 0)
      //     fatal("Failed to open mailbox. Check kernel support for vcio / BCM2708 mailbox.\n");
      // printf("Allocating physical memory: size = %d     ", NUM_PAGES * 4096);
      // if(! (mbox.mem_ref = mem_alloc(mbox.handle, NUM_PAGES * 4096, 4096, MEM_FLAG))) {
      //     fatal("Could not allocate memory.\n");
      // }
      // // TODO: How do we know that succeeded?
      // printf("mem_ref = %u     ", mbox.mem_ref);
      // if(! (mbox.bus_addr = mem_lock(mbox.handle, mbox.mem_ref))) {
      //     fatal("Could not lock memory.\n");
      // }
      // printf("bus_addr = %x     ", mbox.bus_addr);
      // if(! (mbox.virt_addr = mapmem(BUS_TO_PHYS(mbox.bus_addr), NUM_PAGES * 4096))) {
      //     fatal("Could not map memory.\n");
      // }
      // printf("virt_addr = %p\n", mbox.virt_addr);
  
      // /* GPIO4 needs to be ALT FUNC 0 to otuput the clock */
      // gpio_reg[GPFSEL0] = (gpio_reg[GPFSEL0] & ~(7 << 12)) | (4 << 12);
  
      /* Program GPCLK to use MASH setting 1, so fractional dividers work */
      // clk_reg[GPCLK_CNTL] = 0x5A << 24 | 6;
      // udelay(100);
      // clk_reg[GPCLK_CNTL] = 0x5A << 24 | 1 << 9 | 1 << 4 | 6;
  
      // ctl = (struct control_data_s*)mbox.virt_addr;
      // struct dma_cb_t* cbp = ctl->cb;
      // uint32_t phys_sample_dst = CM_GP0DIV;
      // uint32_t phys_pwm_fifo_addr = PWM_PHYS_BASE + 0x18;
  
      // const int frequency_control = frequency_to_control(frequency);
      // for (i = 0; i < NUM_SAMPLES; i++) {
      //     ctl->sample[i] = 0x5a << 24 | frequency_control; /* Silence */
      //     /* Write a frequency sample */
      //     cbp->info = BCM2708_DMA_NO_WIDE_BURSTS | BCM2708_DMA_WAIT_RESP;
      //     cbp->src = mem_virt_to_phys(ctl->sample + i);
      //     cbp->dst = phys_sample_dst;
      //     cbp->length = 4;
      //     cbp->stride = 0;
      //     cbp->next = mem_virt_to_phys(cbp + 1);
      //     cbp++;
      //     /* Delay */
      //     cbp->info = BCM2708_DMA_NO_WIDE_BURSTS | BCM2708_DMA_WAIT_RESP | BCM2708_DMA_D_DREQ | BCM2708_DMA_PER_MAP(5);
      //     cbp->src = mem_virt_to_phys(mbox.virt_addr);
      //     cbp->dst = phys_pwm_fifo_addr;
      //     cbp->length = 4;
      //     cbp->stride = 0;
      //     cbp->next = mem_virt_to_phys(cbp + 1);
      //     cbp++;
      // }
      // cbp--;
      // cbp->next = mem_virt_to_phys(mbox.virt_addr);
  
      /**
       * Initialise PWM to use a 100MHz clock too, and set the range to 500 bits,
       * which is 5us, the rate at which we want to update the GPCLK control
       * register.
       */
      // pwm_reg[PWM_CTL] = 0;
      // udelay(10);
      // clk_reg[PWMCLK_CNTL] = 0x5A000006;              /* Source=PLLD and disable */
      // udelay(100);
      // clk_reg[PWMCLK_DIV] = 0x5A000000 | (5 << 12);  /* set pwm div to 5, for 100MHz */
      // udelay(100);
      // clk_reg[PWMCLK_CNTL] = 0x5A000016;              /* Source=PLLD and enable */
      // udelay(100);
      // pwm_reg[PWM_RNG1] = 500;
      // udelay(10);
      // pwm_reg[PWM_DMAC] = PWMDMAC_ENAB | PWMDMAC_THRSHLD;
      // udelay(10);
      // pwm_reg[PWM_CTL] = PWMCTL_CLRF;
      // udelay(10);
      // pwm_reg[PWM_CTL] = PWMCTL_USEF1 | PWMCTL_PWEN1;
      // udelay(10);
  
      /* Initialise the DMA */
      // dma_reg[DMA_CS] = BCM2708_DMA_RESET;
      // udelay(10);
      // dma_reg[DMA_CS] = BCM2708_DMA_INT | BCM2708_DMA_END;
      // dma_reg[DMA_CONBLK_AD] = mem_virt_to_phys(ctl->cb);
      // if (options.verbose) {
      //     printf("virt %x\n", mem_virt_to_phys(ctl->cb));
      // }
      // dma_reg[DMA_DEBUG] = 7; /* clear debug error flags */
      // dma_reg[DMA_CS] = 0x10880001;   /* go, mid priority, wait for outstanding writes */
  
      printf(" debug line:%d\n", __LINE__);
  
  //    struct command_node_t* __attribute__((annotate("sensitive")))  command = malloc(sizeof(*command));
  //    struct command_node_t* __attribute__((annotate("sensitive")))  command = malloc(32);
      struct command_node_t* command = malloc(32);
  
      printf("command pointer addr: %p\n", command);
  
      command->burst_us = 100.0f;
      command->spacing_us = 100.0f;
      command->repeats = 1;
      command->frequency = frequency;
      command->dead_frequency = frequency;
      command->next = NULL;
  
  //    struct command_node_t* __attribute__((annotate("sensitive"))) new_command = NULL;
      struct command_node_t* new_command = NULL;
  
      if (options.udp) {
          socket_handle = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP);
      } else {
          socket_handle = socket(AF_INET, SOCK_STREAM, 0);
      }
      if (socket_handle < 0) {
          fatal("Unable to create socket\n");
      }
      struct sockaddr_in __attribute__((annotate("sensitive"))) server_address;
      struct sockaddr_in __attribute__((annotate("sensitive"))) client_address;
      socklen_t __attribute__((annotate("sensitive"))) client_length = sizeof(client_address);
  
      printf(" debug line:%d", __LINE__);
  
      bzero(&client_address, sizeof(client_address));
      server_address.sin_family = AF_INET;
      server_address.sin_addr.s_addr = htonl(INADDR_ANY);
      server_address.sin_port = htons(options.port);
      const int bind_status = bind(
          socket_handle,
          (struct sockaddr*)&server_address,
          sizeof(server_address));
      if (bind_status < 0) {
          fatal("Unable to bind socket\n");
      }
  
      if (options.tcp) {
          const int listen_status = listen(socket_handle, 1);
          if (listen_status < 0) {
              fatal("Unable to listen\n");
          }
          tcp_socket_connection = accept(socket_handle, NULL, NULL);
          if (tcp_socket_connection < 0) {
              fatal("Unable to accept TCP connection\n");
          }
      }
  
      printf(" debug line:%d", __LINE__);
  
      int fcntl_status = fcntl(socket_handle, F_SETFL, O_NONBLOCK);
      if (fcntl_status < 0) {
          fatal("Unable to set socket options\n");
      }
      if (options.tcp) {
          fcntl_status = fcntl(tcp_socket_connection, F_SETFL, O_NONBLOCK);
          if (fcntl_status < 0) {
              fatal("Unable to set socket options\n");
          }
      }
  
      char __attribute__((annotate("sensitive"))) json_buffer[BUFFER_SIZE];
  
      // for eval purpose
      int count = 0;
      unsigned long start, end;
  
      printf(" debug line:%d", __LINE__);
  
      start = usecs();
      cfv_init(1024);
  
      while (count++ < 100) {
          /* This is nonblocking because we set it as such as above */
          int bytes_count = 0;
          if (options.udp) {
              bytes_count = recvfrom(
                  socket_handle,
                  json_buffer,
                  sizeof(json_buffer) / sizeof(json_buffer[0]),
                  0,
                  (struct sockaddr*)&client_address,
                  &client_length);
          } else {
              if (tcp_socket_connection != 0) {
                  bytes_count = recv(
                      tcp_socket_connection,
                      json_buffer,
                      sizeof(json_buffer) / sizeof(json_buffer[0]),
                      0);
                  if (bytes_count == 0) {
                      /* Socket closed, wait for another connection */
                      tcp_socket_connection = 0;
                  }
              } else {
                  /* Look for another connection */
                  websocket = 0;
                  tcp_socket_connection = accept(socket_handle, NULL, NULL);
                  if (tcp_socket_connection < 0) {
                      if (errno == EAGAIN || errno == EWOULDBLOCK) {
                          /* No connection ready, just keep running */
                          tcp_socket_connection = 0;
                          continue;
                      } else {
                          fatal("Unable to accept TCP connection\n");
                      }
                  } else {
                      fcntl_status = fcntl(tcp_socket_connection, F_SETFL, O_NONBLOCK);
                      if (fcntl_status < 0) {
                          fatal("Unable to set socket options\n");
                      }
                      continue;
                  }
              }
          }
          /**
           * If the received message is too long, then it will be truncated.
           * All valid messages should always fit in the buffer, so just ignore it
           * if it's too long.
           */
          if (
              bytes_count > 0
              && (size_t)bytes_count < sizeof(json_buffer) / sizeof(json_buffer[0])
          ) {
              json_buffer[bytes_count] = '\0';
              struct command_node_t* parsed_command = NULL;
              int request_response = 0;
              /**
               * WebSocket handshake messages have a plain GET request, so we
               * respond with handshake message
               */
              if (strncmp("GET ", json_buffer, 4) == 0) {
                  const char* const response = websocket_handshake_response(json_buffer);
                  send(tcp_socket_connection, response, strlen(response), 0);
                  websocket = 1;
                  continue;
              }
              if (websocket) {
                  decode_websocket_message(json_buffer);
              }
  
              /**
               * I tried to do terminate json_buffer in decode_websocket_message,
               * but sometimes it just doesn't work and I don't know why.
               */
              char* close_bracket = strstr(json_buffer, "]");
              if (close_bracket) {
                  *(close_bracket + 1) = '\0';
              }
  
              if (options.verbose) {
                  printf("%s\n", json_buffer);
              }
              const int parse_status = parse_json(
                  json_buffer,
                  &parsed_command,
                  &request_response);
              if (request_response) {
                  const int length = snprintf(
                      json_buffer,
                      sizeof(json_buffer) / sizeof(json_buffer[0]),
                      "{\"time\": %d}",
                      (int)time(NULL));
                  /**
                   * The client should be listening for responses on the port one
                   * above the port that it sent to us
                   */
                  client_address.sin_port = htons(ntohs(server_address.sin_port) + 1);
                  /**
                   * TODO: This probably needs to be updated for TCP,
                   * although, TCP guarantees delivery anyway, so maybe it's
                   * not necessary.
                   */
                  sendto(
                      socket_handle,
                      json_buffer,
                      length,
                      0,
                      (struct sockaddr*)&client_address,
                      client_length);
              }
  
              if (parse_status == 0) {
                  /**
                   * Command bursts come after synchronization bursts, and
                   * they're the interesting part, so print those
                   */
                  const struct command_node_t* const print_command = (
                      parsed_command->next == NULL
                      ? parsed_command
                      : parsed_command->next);
                  if (options.verbose) {
                      printf(
                          "Sending command %d %d:%d bursts @ %4.3f (%4.3f)\n",
                          print_command->repeats,
                          (int)print_command->burst_us,
                          (int)print_command->spacing_us,
                          print_command->frequency,
                          print_command->dead_frequency);
                  }
  
                  if (new_command == NULL) {
                      new_command = parsed_command;
                  } else {
                      free_command(command);
                      command = new_command;
                      new_command = parsed_command;
                  }
              } else {
                  /* The error message will be printed in the JSON parser */
                  free_command(parsed_command);
              }
          }
  
          // const int used = fill_buffer(command, new_command, ctl, dma_reg);
          const int used = fill_buffer(command, new_command, bytes_count);
          if (used > 0) {
              if (new_command != NULL) {
                  free_command(command);
                  command = new_command;
                  new_command = NULL;
              }
          }
  
          usleep(100000);
      }
  
      cfv_quote();
      end = usecs();
      printf("round with attestation time usecs: %lu\n", end - start);
  
      terminate(0);
  
      return 0;
  }
  
  
  // static void udelay(int us) {
  //     struct timespec ts = { 0, us * 1000 };
  //     nanosleep(&ts, NULL);
  // }
  
  
  static void terminate(const int signal_) {
      printf("Terminating with signal %d\n", signal_);
      // if (dma_reg) {
      //     dma_reg[DMA_CS] = BCM2708_DMA_RESET;
      //     udelay(500);
          /* Abort the whole DMA */
      //     dma_reg[DMA_CS] = BCM2708_DMA_ABORT | BCM2708_DMA_ACTIVE;
      //     udelay(500);
      // }
      if (tcp_socket_connection != 0) {
          close(tcp_socket_connection);
      }
      if (socket_handle != 0) {
          close(socket_handle);
      }
      exit(1);
  }
  
  
  static void fatal(char* fmt, ...) {
      va_list ap;
      va_start(ap, fmt);
      vfprintf(stderr, fmt, ap);
      va_end(ap);
      terminate(0);
  }
  
  
  // static uint32_t mem_virt_to_phys(void* virt) {
  //     uint32_t offset = (uint8_t*)virt - mbox.virt_addr;
  //     return mbox.bus_addr + offset;
  // }
  
  
  // static uint32_t mem_phys_to_virt(uint32_t phys) {
  //   return phys - (uint32_t)mbox.bus_addr + (uint32_t)mbox.virt_addr;
  // }
  
  // static void* map_peripheral(uint32_t base, uint32_t len) {
  //     int fd = open("/dev/mem", O_RDWR);
  //     void* vaddr;
  
  //     if (fd < 0) {
  //         fatal("Failed to open /dev/mem: %m\n");
  //     }
  //     vaddr = mmap(NULL, len, PROT_READ | PROT_WRITE, MAP_SHARED, fd, base);
  //     if (vaddr == MAP_FAILED) {
  //         fatal("Failed to map peripheral at 0x%08x: %m\n", base);
  //     }
  //     close(fd);
  
  //     return vaddr;
  // }
  
  
  int fill_buffer(
      const struct command_node_t* const command,
      const struct command_node_t* const next_command,
      int bytes_count
      // struct control_data_s* const ctl_,
      // const volatile uint32_t* const dma_reg_
  ) {
      assert(command != NULL || next_command != NULL);
  
      int nodes_processed = 0;
  
      if (!bytes_count)
  	return 0;
  
      if (command) {
  	if (command->repeats == 1 || command->repeats == 20) {
  		//printf("idle\n");
  	} else
           	printf("Sending cmd: %f\t%f\t%d\t%f\t%f\n",command->burst_us,command->spacing_us,command->repeats,command->frequency,command->dead_frequency);
          nodes_processed ++; 
      }
      /*   
      if (next_command) {
          printf("Sending next_cmd: %f\t%f\t%d\t%f\t%f\n",command->burst_us,command->spacing_us,command->repeats,command->frequency,command->dead_frequency);
          nodes_processed ++;
      }
      */
  
      // static enum {
      //     BURST,
      //     SPACING,
      // } state = BURST;
  
      // static float time_us = 0.0f;
  
      // static int repeat_count = -1;
      // static const struct command_node_t* current_command;
      // if (repeat_count == -1) {
      //     repeat_count = command->repeats;
      //     current_command = command;
      // }
      // static int frequency_control;
  
      // static uint32_t last_cb = 0;
      // if (last_cb == 0) {
      //     last_cb = (uint32_t)ctl->cb;
      // }
      // const uint32_t cur_cb = mem_phys_to_virt(dma_reg_[DMA_CONBLK_AD]);
      // int last_sample = (last_cb - (uint32_t)mbox.virt_addr) / (sizeof(struct dma_cb_t) * 2);
      // const int this_sample = (cur_cb - (uint32_t)mbox.virt_addr) / (sizeof(struct dma_cb_t) * 2);
      // int free_slots = this_sample - last_sample;
  
      // if (free_slots < 0) {
      //     free_slots += NUM_SAMPLES;
      // }
  
      // for (; free_slots >= 0; --free_slots) {
      //     /* From DMA settings above, each sample is 5us */
      //     time_us -= 5.0f;
  
      //     if (time_us <= 0.0f) {
      //         switch (state) {
      //             case BURST:
      //                 time_us += current_command->spacing_us;
      //                 state = SPACING;
      //                 frequency_control = frequency_to_control(current_command->dead_frequency);
      //                 break;
      //             case SPACING:
      //                 --repeat_count;
      //                 if (repeat_count == 0) {
      //                     current_command = current_command->next;
      //                     if (current_command == NULL) {
      //                         if (next_command != NULL) {
      //                             current_command = next_command;
      //                         } else {
      //                             current_command = command;
      //                         }
      //                         ++nodes_processed;
      //                     }
      //                     time_us += current_command->burst_us;
      //                     state = BURST;
      //                     repeat_count = current_command->repeats;
      //                 } else {
      //                     time_us += current_command->burst_us;
      //                     state = BURST;
      //                 }
      //                 frequency_control = frequency_to_control(current_command->frequency);
      //                 break;
      //             default:
      //                 assert(0 && "Unknown state");
      //         }
      //     }
      //     assert(time_us > 0.0f && "Time should be positive");
  
      //     ctl_->sample[last_sample++] = (0x5A << 24 | frequency_control);
      //     if (last_sample == NUM_SAMPLES) {
      //         last_sample = 0;
      //     }
      // }
      // last_cb = (uint32_t)mbox.virt_addr + last_sample * sizeof(struct dma_cb_t) * 2;
      return nodes_processed;
  }
  
  
  // static int frequency_to_control(const float frequency) {
  //     const int PLLFREQ = 500000000;  /* PLLD is running at 500MHz */
  //     const float PLL_MHZ = (float)PLLFREQ / 1000000.0f;
  //     const float poll_per_carrier = PLL_MHZ / frequency;
  //     const int frequency_control = (int)round(poll_per_carrier * (1 << 12));
  //     return frequency_control;
  // }
  
  
  static int parse_json(
      const char* const json,
      struct command_node_t** command,
      int* const request_response
  ) {
      json_t* root = NULL;
      json_t* object = NULL;
      json_t* data = NULL;
      json_error_t error;
      int return_value = -1;
  
      assert(*command == NULL);
  
      root = json_loads(json, 0, &error);
      if (!root) {
          fprintf(stderr, "error: on line %d: %s\n", error.line, error.text);
          goto CLEANUP;
      }
      if (!json_is_array(root)) {
          fprintf(stderr, "not a JSON array\n");
          goto CLEANUP;
      }
  
      size_t array_index;
      const size_t array_size = json_array_size(root);
      for (array_index = 0; array_index < array_size; ++array_index) {
          object = json_array_get(root, array_index);
          if (!json_is_object(object)) {
              fprintf(
                  stderr,
                  "Item %zu in array is not an object\n",
                  array_index + 1);
              goto CLEANUP;
          }
  
          //*command = malloc(sizeof(**command));
          *command = malloc(32);
          (*command)->next = NULL;
  
          data = json_object_get(object, "burst_us");
          if (json_is_number(data)) {
              (*command)->burst_us = json_number_value(data);
          } else {
              fprintf(
                  stderr,
                  "In item %zu: missing or invalid field: burst_us\n",
                  array_index + 1);
              goto CLEANUP;
          }
  
          data = json_object_get(object, "spacing_us");
          if (json_is_number(data)) {
              (*command)->spacing_us = json_number_value(data);
          } else {
              fprintf(
                  stderr,
                  "In item %zu: missing or invalid field: spacing_us\n",
                  array_index + 1);
              goto CLEANUP;
          }
  
          data = json_object_get(object, "repeats");
          if (json_is_number(data)) {
              (*command)->repeats = json_number_value(data);
          } else {
              fprintf(
                  stderr,
                  "In item %zu: missing or invalid field: repeats\n",
                  array_index + 1);
              goto CLEANUP;
          }
  
          data = json_object_get(object, "frequency");
          if (json_is_number(data)) {
              (*command)->frequency = json_number_value(data);
          } else {
              fprintf(
                  stderr,
                  "In item %zu: missing or invalid field: frequency\n",
                  array_index + 1);
              goto CLEANUP;
          }
  
          data = json_object_get(object, "dead_frequency");
          if (json_is_number(data)) {
              (*command)->dead_frequency = json_number_value(data);
          } else {
              fprintf(
                  stderr,
                  "In item %zu: missing or invalid field: dead_frequency\n",
                  array_index + 1);
              goto CLEANUP;
          }
  
          data = json_object_get(object, "request_response");
          /**
           * request_response is optional so if it isn't a port number, ignore it
           */
          if (data != NULL && json_is_true(data)) {
              *request_response = 1;
          } else {
              *request_response = 0;
          }
  
          command = &((*command)->next);
      }
  
      return_value = 0;
  
  CLEANUP:
      if (root != NULL) {
          json_decref(root);
      }
      return return_value;
  }
  
  
  static void free_command(struct command_node_t* command) {
      while (command) {
          struct command_node_t* const previous = command;
          command = command->next;
          free(previous);
      }
  }
  
  
  static struct pi_options get_args(const int argc, char* argv[]) {
      const struct option long_options[] = {
          {"help", no_argument, NULL, 'h'},
          {"verbose", no_argument, NULL, 'v'},
          {"tcp", no_argument, NULL, 't'},
          {"udp", no_argument, NULL, 'u'},
          {"port", required_argument, NULL, 'p'},
          {0, 0, 0, 0}
      };
      struct pi_options options;
      options.port = 12345;
      options.verbose = 0;
      options.udp = 0;
      options.tcp = 0;
      int opt;
      int long_index = 0;
      while (1) {
          opt = getopt_long(argc, argv, "hvp:tu", long_options, &long_index);
          if (opt == -1) {
              break;
          }
          switch (opt) {
              case 'h':
                  print_usage(argv[0]);
                  exit(EXIT_SUCCESS);
                  break;
              case 'v':
                  options.verbose = 1;
                  break;
              case 'p':
                  options.port = atoi(optarg);
                  break;
              case 't':
                  options.tcp = 1;
                  break;
              case 'u':
                  options.udp = 1;
                  break;
              default:
                  fprintf(stderr, "Unknown option\n");
                  print_usage(argv[0]);
                  exit(EXIT_FAILURE);
          }
      }
      if (options.tcp && options.udp) {
          fprintf(stderr, "Only one of --tcp and --udp may be specified\n");
          exit(EXIT_FAILURE);
      }
      if (!options.tcp && !options.udp) {
          /* Default to UDP */
          options.udp = 1;
      }
      return options;
  }
  
  
  static void print_usage(const char* program) {
      printf("Usage: %s [OPTIONS]\n", program);
      printf("-p, --port     The port to listen for messags on.\n");
      printf("-h, --help     Print this help message.\n");
      printf("-v, --verbose  Print more debugging information.\n");
      printf("-t, --tcp      Listen for messages on a TCP connection.\n");
      printf("-u, --udp      Listen for messages on a UDP connection.\n");
  }
  
  
  const char* websocket_handshake_response(const char* const handshake) {
      const char* const key_header = "Sec-WebSocket-Key: ";
      const char* const start = strstr(handshake, key_header);
      static char buffer[512];
      char hash[20];
  
      const int key_length = strstr(start, "\n") - start - strlen(key_header) - 1;
      const char* const start_key = start + strlen(key_header);
      char key[64];
      /**
       * The response needs to include base64(SHA-1(key +
       * "258EAFA5-E914-47DA-95CA-C5AB0DC85B11))
       */
      strncpy(key, start_key, key_length);
      key[key_length] = '\0';
      strncpy(buffer, key, key_length);
      strcpy(buffer + key_length, "258EAFA5-E914-47DA-95CA-C5AB0DC85B11");
      struct SHA1_CTX sha1_context;
      sha1_init(&sha1_context);
      sha1_update(&sha1_context, (BYTE*)buffer, strlen(buffer));
      sha1_final(&sha1_context, (BYTE*)hash);
      const int length = base64_encode((BYTE*)hash, 20, key);
      key[length] = '\0';
  
      snprintf(
          buffer,
          sizeof(buffer) / sizeof(buffer[0]),
          "HTTP/1.1 101 Web Sockt Protocol Handshake\r\n"
              "Upgrade: WebSocket\r\n"
              "Connection: Upgrade\r\n"
              "Sec-WebSocket-Accept: %s\r\n"
              "Acces-Control-Allow-Headers: x-websocket-protocol\r\n\r\n",
          key);
      return buffer;
  }
  
  
  void decode_websocket_message(char message[]) {
      uint8_t masking_key[4];
      char buffer[BUFFER_SIZE];
      /* Skip the flags */
      const char* iter = &message[1];
      int payload_length = *iter;
      ++iter;
      if (payload_length == 0xfe) {
          payload_length = ntohs(*((uint16_t*)iter));
          iter += sizeof(uint16_t);
      }
      strncpy((char*)masking_key, iter, sizeof(masking_key) / sizeof(masking_key[0]));
      iter += sizeof(masking_key) / sizeof(masking_key[0]);
      strncpy(buffer, iter, payload_length);
      int i;
      for (i = 0; i < payload_length; ++i) {
          /**
           * This is one of the weirdest bugs I've ever seen, but occasionally,
           * when receiving messages from a browser, this code will decode about
           * 1/4 of the message, and then fill the rest with some random garbage
           * byte(s). I've printed the buffer before messing with it and it looks
           * right, and decoding with the key values in Python looks right. I
           * can't read ARM assembly enough to know if the assembly is right.
           * Valgrind doesn't notice anything wrong either.
           */
          message[i] = buffer[i] ^ masking_key[i % 4];
      }
      message[payload_length] = '\0';
  }
  
  
  /*********************************************************************
  * Filename:   sha1.c
  * Author:     Brad Conte (brad AT bradconte.com)
  * Copyright:
  * Disclaimer: This code is presented "as is" without any guarantees.
  * Details:    Implementation of the SHA1 hashing algorithm.
                Algorithm specification can be found here:
                 * http://csrc.nist.gov/publications/fips/fips180-2/fips180-2withchangenotice.pdf
                This implementation uses little endian byte order.
  *********************************************************************/
  
  
  /****************************** MACROS ******************************/
  #define ROTLEFT(a, b) ((a << b) | (a >> (32 - b)))
  
  /*********************** FUNCTION DEFINITIONS ***********************/
  void sha1_transform(struct SHA1_CTX *ctx, const BYTE data[])
  {
      WORD a, b, c, d, e, i, j, t, m[80];
  
      for (i = 0, j = 0; i < 16; ++i, j += 4)
          m[i] = (data[j] << 24) + (data[j + 1] << 16) + (data[j + 2] << 8) + (data[j + 3]);
      for ( ; i < 80; ++i) {
          m[i] = (m[i - 3] ^ m[i - 8] ^ m[i - 14] ^ m[i - 16]);
          m[i] = (m[i] << 1) | (m[i] >> 31);
      }
  
      a = ctx->state[0];
      b = ctx->state[1];
      c = ctx->state[2];
      d = ctx->state[3];
      e = ctx->state[4];
  
      for (i = 0; i < 20; ++i) {
          t = ROTLEFT(a, 5) + ((b & c) ^ (~b & d)) + e + ctx->k[0] + m[i];
          e = d;
          d = c;
          c = ROTLEFT(b, 30);
          b = a;
          a = t;
      }
      for ( ; i < 40; ++i) {
          t = ROTLEFT(a, 5) + (b ^ c ^ d) + e + ctx->k[1] + m[i];
          e = d;
          d = c;
          c = ROTLEFT(b, 30);
          b = a;
          a = t;
      }
      for ( ; i < 60; ++i) {
          t = ROTLEFT(a, 5) + ((b & c) ^ (b & d) ^ (c & d))  + e + ctx->k[2] + m[i];
          e = d;
          d = c;
          c = ROTLEFT(b, 30);
          b = a;
          a = t;
      }
      for ( ; i < 80; ++i) {
          t = ROTLEFT(a, 5) + (b ^ c ^ d) + e + ctx->k[3] + m[i];
          e = d;
          d = c;
          c = ROTLEFT(b, 30);
          b = a;
          a = t;
      }
  
      ctx->state[0] += a;
      ctx->state[1] += b;
      ctx->state[2] += c;
      ctx->state[3] += d;
      ctx->state[4] += e;
  }
  
  void sha1_init(struct SHA1_CTX *ctx)
  {
      ctx->datalen = 0;
      ctx->bitlen = 0;
      ctx->state[0] = 0x67452301;
      ctx->state[1] = 0xEFCDAB89;
      ctx->state[2] = 0x98BADCFE;
      ctx->state[3] = 0x10325476;
      ctx->state[4] = 0xc3d2e1f0;
      ctx->k[0] = 0x5a827999;
      ctx->k[1] = 0x6ed9eba1;
      ctx->k[2] = 0x8f1bbcdc;
      ctx->k[3] = 0xca62c1d6;
  }
  
  void sha1_update(struct SHA1_CTX *ctx, const BYTE data[], size_t len)
  {
      size_t i;
  
      for (i = 0; i < len; ++i) {
          ctx->data[ctx->datalen] = data[i];
          ctx->datalen++;
          if (ctx->datalen == 64) {
              sha1_transform(ctx, ctx->data);
              ctx->bitlen += 512;
              ctx->datalen = 0;
          }
      }
  }
  
  void sha1_final(struct SHA1_CTX *ctx, BYTE hash[])
  {
      WORD i;
  
      i = ctx->datalen;
  
      /* Pad whatever data is left in the buffer. */
      if (ctx->datalen < 56) {
          ctx->data[i++] = 0x80;
          while (i < 56)
              ctx->data[i++] = 0x00;
      }
      else {
          ctx->data[i++] = 0x80;
          while (i < 64)
              ctx->data[i++] = 0x00;
          sha1_transform(ctx, ctx->data);
          memset(ctx->data, 0, 56);
      }
  
      /**
       * Append to the padding the total message's length in bits and transform.
       */
      ctx->bitlen += ctx->datalen * 8;
      ctx->data[63] = ctx->bitlen;
      ctx->data[62] = ctx->bitlen >> 8;
      ctx->data[61] = ctx->bitlen >> 16;
      ctx->data[60] = ctx->bitlen >> 24;
      ctx->data[59] = ctx->bitlen >> 32;
      ctx->data[58] = ctx->bitlen >> 40;
      ctx->data[57] = ctx->bitlen >> 48;
      ctx->data[56] = ctx->bitlen >> 56;
      sha1_transform(ctx, ctx->data);
  
      /**
       * Since this implementation uses little endian byte ordering and MD uses big endian,
       * reverse all the bytes when copying the final state to the output hash.
       */
      for (i = 0; i < 4; ++i) {
          hash[i]      = (ctx->state[0] >> (24 - i * 8)) & 0x000000ff;
          hash[i + 4]  = (ctx->state[1] >> (24 - i * 8)) & 0x000000ff;
          hash[i + 8]  = (ctx->state[2] >> (24 - i * 8)) & 0x000000ff;
          hash[i + 12] = (ctx->state[3] >> (24 - i * 8)) & 0x000000ff;
          hash[i + 16] = (ctx->state[4] >> (24 - i * 8)) & 0x000000ff;
      }
  }
  
  
  /* This is a public domain base64 implementation written by WEI Zhicheng. */
  /* BASE 64 encode table */
  static char base64en[] = {
      'A', 'B', 'C', 'D', 'E', 'F', 'G', 'H',
      'I', 'J', 'K', 'L', 'M', 'N', 'O', 'P',
      'Q', 'R', 'S', 'T', 'U', 'V', 'W', 'X',
      'Y', 'Z', 'a', 'b', 'c', 'd', 'e', 'f',
      'g', 'h', 'i', 'j', 'k', 'l', 'm', 'n',
      'o', 'p', 'q', 'r', 's', 't', 'u', 'v',
      'w', 'x', 'y', 'z', '0', '1', '2', '3',
      '4', '5', '6', '7', '8', '9', '+', '/',
  };
  
  #define BASE64_PAD  '='
  
  
  #define BASE64DE_FIRST  '+'
  #define BASE64DE_LAST   'z'
  
  int
  base64_encode(unsigned char *in, int inlen, char *out)
  {
      int i, j;
  
      for (i = j = 0; i < inlen; i++) {
          int s = i % 3;          /* from 6/gcd(6, 8) */
  
          switch (s) {
          case 0:
              out[j++] = base64en[(in[i] >> 2) & 0x3F];
              continue;
          case 1:
              out[j++] = base64en[((in[i-1] & 0x3) << 4) + ((in[i] >> 4) & 0xF)];
              continue;
          case 2:
              out[j++] = base64en[((in[i-1] & 0xF) << 2) + ((in[i] >> 6) & 0x3)];
              out[j++] = base64en[in[i] & 0x3F];
          default:;
          }
      }
  
      /* move back */
      i -= 1;
  
      /* check the last and add padding */
      if ((i % 3) == 0) {
          out[j++] = base64en[(in[i] & 0x3) << 4];
          out[j++] = BASE64_PAD;
          out[j++] = BASE64_PAD;
      } else if ((i % 3) == 1) {
          out[j++] = base64en[(in[i] & 0xF) << 2];
          out[j++] = BASE64_PAD;
      }
  
      return j;
  }
  