queue.c

Go to the documentation of this file.

#include <main-header.h>
#include <queue.h>
#include <command-parser.h>
#include <command-handler.h>
#include <multiram.h>
#include <routing.h>
#include <config.h>
#include "espressif/esp8266_cloud.h"
#include <ti1101.h>
#include <route_command.h>
#include "metrics/metrics.h"
 
#define MAX_COM_RETRIES 30
 
/****************************
 * command send failure codes
 **************************/
// permanent failures
#define SEND_BUF_RESERVED1 1
#define SEND_BUF_RESERVED2 2
#define SEND_NO_CLOUD 3
#define SEND_NO_TARGET 5
#define SEND_NO_RECIPIENT 6
#define SEND_COM_FAILED_TO_ENCODE 7
#define SEND_COM_NOT_ENCODED 12
#define SEND_COM_INVALID 13
#define SEND_COM_STILL_INVALID 14
#define SEND_FAILURE_NODEV 16
// temporary failures
#define SEND_DEVICE_BUSY -113
 
 
 
//#include <bare-metal.h>
static int command_index = 0;
static struct command *foocom;
static char queuebuf[256]; // limits the max size of an outbound packet
// must be larger than max commands that can be allocated
#define COM_QUEUE_SIZE 80
typedef struct queue_entry {
  struct command *      command;        
  int                   attemptctr;     
  pkt_callback          callback;       
  long                  last_attempt;   
} _queue_entry;
 
static void callback(struct queue_entry *q, struct command *com);
static struct queue_entry out_queue[COM_QUEUE_SIZE];
static int send_command_internal(struct command *com, int print);
 
static int broadcast_device();
 
#define COMMAND_AGE_BEFORE_GC 120
 
void queue_init() {
  memset(&out_queue, 0, sizeof(out_queue));
}
void clear_outbound_queue() {
  printf("Outbound command queue cleared.\r\n");
  queue_init();
}
void print_outbound_queue() {
  int i;
  int elements = get_mpm()->max_commands;
  struct comnode *p;
 
  for (i = 0; i < elements; i++) {
    p = &get_mpm()->commem[i];
    if (p->allocated == 0) {
      continue;
    }
    printf("------- Queue position %i: ------\r\n", i);
    command_print(&p->command);
  }
}
 
struct command *alloc_command_with_minfree(int minfree) {
  int i;
  struct comnode *p;
  struct comnode *res;
  int elements = get_mpm()->max_commands;
 
  int free = 0;
 
  res = NULL;
  for (i = 0; i < elements; i++) {
    p = &get_mpm()->commem[i];
    if (p->allocated == 0) {
      free++;
      if ((free >= minfree) && (res != NULL)) {
        break;
      }
 
      if (res == NULL) {
        res = p;
      }
    }
  }
  if (res != NULL) {
    command_init(&res->command);
    res->command.index = -1;
    res->allocated = mculib_get_seconds_since_boot();
    return &res->command;
  }
 
  // we have now command buffers left to allocate
  res = NULL;
  printf("command buffers are all allocated (reserved %i):\n", minfree);
  for (i = 0; i < elements; i++) {
    p = &get_mpm()->commem[i];
    long now = mculib_get_seconds_since_boot();
    long age = (now - p->allocated);
    if (p->allocated != 0) {
      printf("%i. (age seconds: %i) (%s) \r\n", i, (int)age, command_get_name(p->command.com));
      //command_print(&p->command);
    }
    if ((age > COMMAND_AGE_BEFORE_GC) || (age < -10)) {
      //IPLOG("Garbage collecting above command due to its age and using it!\r\n");
      if (res == NULL) {
        res = p; // update result (reuse last command gc'ed)
      } else {
        p->allocated = 0;
      }
      break;
    }
  }
  if (res == NULL) {
    IPLOG("BIG FAT WARNING - No command left to allocate (%i) (reserved %i).\r\n", elements, minfree);
    return NULL;
  }
  printf("Reallocated garbage collected command: %p\r\n", &res->command);
  command_init(&res->command);
  return &res->command;
}
 
 
struct command *alloc_command() {
  return alloc_command_with_minfree(0);
}
 
 
void free_commands(int index) {
  struct comnode *p;
  int i;
  int elements = sizeof(get_mpm()->commem) / sizeof(get_mpm()->commem[0]);
 
  if (index == -1) {
    for (i = 0; i < elements; i++) {
      p = &get_mpm()->commem[i];
      if (p->allocated != 0) {
        free_command(&p->command);
      }
    }
  }
}
void free_command(struct command *com) {
  struct comnode *p;
 
  p = (struct comnode *)com;
  if (p->allocated == 0) {
    //    printf("BIG WARNING: DUPLICATE FREE OF COMMAND! \r\n");
    return;
  }
  p->allocated = 0;
}
 
/*
 * free any commands that are too old
 */
static void garbage_collect_commands() {
  int i;
  struct comnode *p;
  int elements = get_mpm()->max_commands;
 
  long now = mculib_get_seconds_since_boot();
 
  for (i = 0; i < elements; i++) {
    p = &get_mpm()->commem[i];
    if (p->allocated == 0) {
      continue;
    }
    long age = (now - p->allocated);
    if ((age > COMMAND_AGE_BEFORE_GC) || (age < -10)) {
      free_command((struct command *)p);
    }
  }
}
/*
 * free queue entries which point to commands that are no longer valid
 */
static void garbage_collect_queue_entries() {
  int elements = sizeof(out_queue) / sizeof(out_queue[0]);
  int i;
  struct comnode *p;
 
  for (i = 0; i < elements; i++) {
    if (out_queue[i].command == NULL) {
      continue;
    }
    p = (struct comnode *)out_queue[i].command;
    if (p->allocated == 0) {
      out_queue[i].command = NULL;
    }
  }
}
 
/*
 * set or clear the POWERSAVE flag, depending on module config
 */
static void set_power_mode_on_com(struct command *com) {
  if (config_get_flag(CONFIG_FLAGS_POWER_SAVE)) {
    com->flags |= COMFLAGS_POWERSAVE;
  } else {
    com->flags &= ~COMFLAGS_POWERSAVE;
  }
}
 
 
 
 
 
struct command *get_data_reply(struct command *com) {
  struct command *dcom;
 
  dcom = alloc_command();
  if (dcom == NULL) {
    return NULL;
  }
  command_init(dcom);
 
  dcom->target = com->sender;
  dcom->encoding = 'a';
  dcom->index = com->index;
  dcom->sourcedev = com->sourcedev;
  dcom->connid = com->connid;
  dcom->com = com->com;
  dcom->flags = COMFLAGS_DATA | COMFLAGS_SUCCESS;
  return dcom;
}
int send_data(struct command *com, const char *format, ...) {
  struct command *dcom;
  va_list args;
 
 
  dcom = get_data_reply(com);
  if (dcom == NULL) {
    return 1;
  }
  va_start(args, format);
  command_add_varg(dcom, format, args);
  send_command(dcom);
  free_command(dcom);
  va_end(args);
  return 0;
}
 
int process_queue_reply(struct command *ack) {
  int elements = sizeof(out_queue) / sizeof(out_queue[0]);
  struct queue_entry *q;
  int i;
 
  for (i = 0; i < elements; i++) {
    q = &out_queue[i];
    if (q->command == NULL) {
      continue;
    }
    // check if this is a reply to one of our commands
    if ((q->command->index == ack->index)
        && (q->command->target == ack->sender)
        ) {
      printf("That is my command at index %i\r\n", i);
      callback(q, ack);
      struct command *c = q->command;
      if (c->target != 0xFFFFFFFF) {
        q->command = NULL;
        free_command(c);
      }
      return 1;
    }
  }
  return 0;
}
 
static int get_next_command_index() {
  if ((command_index > 32000) || (command_index < 0)) {
    command_index = 0;
  }
  return ++command_index;
}
int send_command(struct command *com) {
  printf("Sending command:\r\n");
  int a = send_command_internal(com, 1);
 
  if (a != 0) {
    printf("Sending failed (code=%i)\r\n", a);
  }
  return a;
}
 
int send_command_quietly(struct command *com) {
  return send_command_internal(com, 0);
}
 
/*
 * \brief same as send_command plus flag wether to print command or not
 * return: 0 ok, >0 on permanent errors, <0 on temporary errors
 */
static int send_command_internal(struct command *com, int p) {
  struct hostroute *host;
 
  // I cannot do this, because we might be FORWARDING a packet.
  //com->sender = get_my_node_id();
 
  //printf("Sending command to device %i: %s\r\n", com->sourcedev, queuebuf);
  if (com->index == -1) {
    com->index = get_next_command_index();
  }
  if (com->target == 0) {
    return SEND_NO_TARGET;
  }
 
  // special case: send to server
  if (com->target == CLOUD_SERVER) {
    if ((esp_cloud_is_connected()) || ((com->local_flags & (1 << COM_LOCAL_FLAG_FORCE_DEVICE)) && (com->sourcedev == SOURCE_WIFI))) {
      //  send to server
      com->sourcedev = 1;
      com->recipient = CLOUD_SERVER;
    } else {
      // no direct route to target and no server connection:
      // try to use a configured route
      struct route *r = get_configured_route(com);
      if (r != NULL) {
        com->recipient = r->sendto;
        com->sourcedev = r->out_device;
      }
    }
  }
 
  if (com->recipient == 0) {
    if (com->target == BROADCAST) {
      com->sourcedev = broadcast_device();
      com->recipient = BROADCAST;
      goto gotdev_no_recipt;
    }
    //  forward to "best" device
    host = routing_find_host(com->target);
    if (host == NULL) {
      // use server as default gateway
      request_route(com->target);
      if (esp_cloud_is_connected()) {
        com->sourcedev = SOURCE_WIFI;
        com->recipient = CLOUD_SERVER;
      } else {
        return SEND_NO_CLOUD; // cloud not connected
      }
    } else {
      com->sourcedev = host->device;
      com->recipient = host->nexthop;
    }
  }
gotdev_no_recipt:
 
  if (com->recipient == 0) {
    return SEND_NO_RECIPIENT;
  }
 
  if ((com->sourcedev == 0) && (com->recipient = BROADCAST)) {
    struct network_context *nc = nic_by_index(broadcast_device());
    if (nc != NULL) {
      com->sourcedev = nc->source;
    }
  }
 
  if (com->sourcedev == 0) {
    return SEND_FAILURE_NODEV;
  }
  // TODO: fix wifi netdev
  if ((com->sourcedev != SOURCE_WIFI) && (nic_is_busy(com->sourcedev))) {
    // device busy? return negative (temporary response)
    return SEND_DEVICE_BUSY;
  }
  if (p) {
    if (!is_command_valid(com)) {
      printf("Cannot print at %p\r\n", com);
      return SEND_COM_INVALID;
    }
    foocom = com;
    command_print(com);
    if (!is_command_valid(com)) {
      printf("print broke stuff at %p (was: %p)\r\n", com, foocom);
      return SEND_COM_STILL_INVALID;
    }
  }
  if (!is_command_valid(com)) {
    printf("Cannot encode at %p\r\n", com);
    return SEND_COM_NOT_ENCODED;
  }
  set_power_mode_on_com(com);
  int len = command_encode_ascii(com, 256, (char *)&queuebuf);
 
  if (len < 0) {
    printf("Failed to encode packet (%i) at %p.\r\n", len, com);
    return SEND_COM_FAILED_TO_ENCODE;
  }
  printf("Sending via %s: %i bytes (%s)\r\n", command_get_source_name(com->sourcedev), len, &queuebuf);
 
  int i = send_buf_via_device(com->sourcedev, (const byte *)&queuebuf, len);
 
  if (i == 0) {
    return 0;
  }
  printf("Failed to sent packet:\r\n");
  command_print(com);
  return i;
  /*
   * if (com->sourcedev == 1) {
   * wifi_send_data(PKT_COMMAND, (const byte *)&queuebuf, len);       // wifi
   * return 0;
   * } else if (com->sourcedev == 3) {
   * printf("REPLY: %s\r\n", (char *)&queuebuf);                                  // local tty
   * return 2;
   * } else if (com->sourcedev == 2) {                                             // radio
   * if (!ti1101_send(len, (const byte *)&queuebuf)) {
   * printf("Not sent\r\n");
   * return -1;
   * }
   * printf("Sent\r\n");
   * return 0;
   * } else {
   * printf("******************\r\n");
   * printf("Unable to send packet! Unknown source: %i\r\n", com->sourcedev);
   * if (p) {
   * command_print(com);
   * }
   * printf("******************\r\n");
   * return 1;
   * }
   */
}
 
int send_command_fw_info(struct command *com, int err) {
  struct command *reply;
 
  reply = alloc_command();
  if (reply == NULL) {
    return 2;
  }
  command_init(reply);
 
  reply->target = com->sender;
  reply->encoding = 'a';
  reply->index = com->index;
  reply->sourcedev = com->sourcedev;
  reply->connid = com->connid;
  reply->com = com->com;
  reply->flags = COMFLAGS_FORWARDED;
  if (err == 0) {
    reply->flags = reply->flags | COMFLAGS_ACK;
  }
  return deliver_command(reply, NULL);
}
int send_command_reply(struct command *com, byte flags) {
  struct command *reply;
 
  reply = alloc_command();
  if (reply == NULL) {
    return 2;
  }
  command_init(reply);
 
  reply->target = com->sender;
  reply->encoding = 'a';
  reply->index = com->index;
  reply->sourcedev = com->sourcedev;
  reply->connid = com->connid;
  reply->com = com->com;
  reply->flags = flags & ~(COMFLAGS_FORWARDED); // clear forwarded flag. it is _not_ yet forwarded
  return deliver_command(reply, NULL);
}
int send_command_reply_with_args(struct command *com, byte flags, const char *format, ...) {
  va_list args;
 
  struct command *reply;
 
  reply = alloc_command();
  if (reply == NULL) {
    return 2;
  }
  command_init(reply);
 
  reply->target = com->sender;
  reply->encoding = 'a';
  reply->index = com->index;
  reply->sourcedev = com->sourcedev;
  reply->connid = com->connid;
  reply->com = com->com;
  reply->flags = flags & ~(COMFLAGS_FORWARDED); // clear forwarded flag. it is _not_ yet forwarded
  va_start(args, format);
  command_add_varg(reply, format, args);
  va_end(args);
 
  return deliver_command(reply, NULL);
}
static struct queue_entry *alloc_free_queue_entry(struct command *com) {
  int elements = sizeof(out_queue) / sizeof(out_queue[0]);
  int i;
 
  for (i = 0; i < elements; i++) {
    if (out_queue[i].command == NULL) {
      memset(&out_queue[i], 0, sizeof(out_queue[0]));
      out_queue[i].command = com;
      return &out_queue[i];
    }
  }
  printf("BIG FAT WARNING - no free queue slot found!\r\n");
  return NULL;
}
 
int deliver_command(struct command *com, pkt_callback cb) {
  struct queue_entry *q;
 
  if (!is_command_valid(com)) {
    error_com(com);
    return 100;
  }
  q = alloc_free_queue_entry(com);
  if (q == NULL) {
    printf("No available queue slot\r\n");
    return 1;
  }
  q->callback = cb;
  IncMetric_COM_QUEUE_ADDED;
  return 0;
}
 
static void callback(struct queue_entry *q, struct command *com) {
  if ((q == NULL)) {
    return;
  }
  if (q->callback == NULL) {
    return;
  }
  q->callback(q->command, com);
}
 
void process_command_queue() {
  int res;
  int x;
  int elements = sizeof(out_queue) / sizeof(out_queue[0]);
  struct queue_entry *q;
  struct command *c;
  int i;
  long now = mculib_get_seconds_since_boot();
  char t[16];
 
  garbage_collect_commands();
  garbage_collect_queue_entries();
  res = 0;
  x = 0;
  for (i = 0; i < elements; i++) {
    q = &out_queue[i];
    if (q->command == NULL) {
      continue;
    }
    if (!is_command_valid(q->command)) {
      printf("Command %i in queue is invalid\r\n", i);
      continue;
    }
    res++;
    if ((q->last_attempt != 0) && (now - q->last_attempt) <= 1) {
      continue;
    }
    x++;
  }
  if (x == 0) {
    return; // no elements are due
  }
  IPLOG("Processing queue with %i elements, due elements: %i\r\n", res, x);
  for (i = 0; i < elements; i++) {
    q = &out_queue[i];
    if (q->command == NULL) {
      continue;
    }
    if (q->last_attempt > now) {
      q->last_attempt = now; // overflow fix?
      continue;
    }
    if ((q->last_attempt != 0) && (now - q->last_attempt) <= 1) {
      continue;
    }
    q->last_attempt = now;
    c = q->command;
    pretty_node_to_str(c->target, (char *)&t);
    // try at most 5 times //
    if (q->attemptctr > MAX_COM_RETRIES) {
      callback(q, NULL);
      q->command = NULL;
      free_command(c);
      printf("Discarded command, No response to command %i (target: %s)\r\n", i, t);
      break;
    }
    printf("sending com %i (attempt #%i) to %s\r\n", i, q->attemptctr, t);
    res = send_command(q->command);
    if (res < 0) {
      // temporarily failed to send
      // such as, wifi AP is offline
      // or radio is busy
      printf("Queue: temporarily unable to send command %i\r\n", i);
    } else if (res > 0) {
      // permanently failed to send
      // such as Invalid Address or so
      printf("Queue: command #%i (type %i) unsendable (given up) reason: %i\r\n", i, q->command->com, res);
      callback(q, NULL);
      q->command = NULL;
      free_command(c);
      continue;
    } else {
      // we did _send_ it, so increase attemptctr
      q->attemptctr++;
    }
 
    // any combination of these flags will only be retried if the send error was temporary, otherwise freed
    if (res >= 0) {
      if (c->flags & (COMFLAGS_ACK | COMFLAGS_FORWARDED | COMFLAGS_DATA)) {
        callback(q, NULL);
        q->command = NULL;
        free_command(c);
      }
    }
    // break;   // we only process one command at a time to give time
    // to process responses, if any
    // cnw: I think this is non-sense, this method is being executed immediately
    // after it exits anyways, so we might as well process them all ;-)
  }
}
 
void request_route(long nodeid) {
  struct command *com;
 
  printf("Requesting route for %N\r\n", nodeid);
  // wifi
  com = alloc_command();
  if (com == NULL) {
    return;
  }
  com->sourcedev = SOURCE_WIFI;
  com->com = 12;         // announce
  com->target = 0xFFFFFFFF;
  com->recipient = 0xFFFFFFFF;
  com->flags = 0;
  command_add_arg(com, "%N", nodeid);
 
  if (!config_get_wifidisable()) {
    com->sourcedev = SOURCE_WIFI;
    send_command(com);
  }
 
  com->flags = COMFLAGS_ACK | COMFLAGS_SUCCESS;
  // TODO: make this uniform for all nics
  if (isready_by_type(SOURCE_RADIO)) {
    com->sourcedev = SOURCE_RADIO;
    send_command(com);
  }
  if (isready_by_type(SOURCE_LORA)) {
    com->sourcedev = SOURCE_LORA;
    send_command(com);
  }
  free_command(com);
}
 
int get_outbound_command_count_important() {
  int i;
  int coms = 0;
  int elements = sizeof(out_queue) / sizeof(out_queue[0]);
  struct queue_entry *q;
 
  for (i = 0; i < elements; i++) {
    q = &out_queue[i];
    if (q->command == NULL) {
      continue;
    }
    if ((q->command->com == 1) || (q->command->com == 23)) {
      continue;
    }
    coms++;
  }
  return coms;
}
 
static int broadcast_device() {
  int i = 0;
  int somewhat_good = 0;
  int verygood = 0;
 
  for (;;) {
    struct network_context *nc = nic_by_index(i);
    i++;
    if (nc == NULL) {
      break;
    }
    uint8_t type = nc->source;
    if (!isonline_by_type(type)) {
      continue;
    }
    somewhat_good = type;
    if ((isready_by_type(type))) {
      verygood = type;
    }
  }
  if (verygood != 0) {
    return verygood;
  }
  return somewhat_good;
}