command-parser.c

Go to the documentation of this file.

#include "main-header.h"
#include "coms/coms.h"
#include "platform-header.h"
 
#define command_names_size 60
#ifndef CNWDEBUGCOM
#define CNWDEBUGCOM(a, ...) noop()
#endif
 
// #define CNWDEBUGCOM(a, ...) noop()
//#define CNWDEBUGCOM printf
 
// do not change the order!
// do remember to adjust command_names_size
// also see command-handler.c "process_packet()" (which is where we jump into once we received one and then decide what to do with it)
// command->com starts counting at 1 (not 0!)
const char *const command_names[] = { "noop",                   //  1
                                      "log",                    //  2
                                      "are-you-a-singingcat?",  //  3
                                      "set-wireless",           //  4
                                      "set-pin",                //  5
                                      "reset",                  //  6
                                      "wifireset",              //  7
                                      "DISradio-ping",          //  8
                                      "DISradiopingloop",       //  9
                                      "freeze",                 // 10
                                      "list-modules",           // 11
                                      "route-request",          // 12
                                      "strobe",                 // 13
                                      "getpubkey",              // 14
                                      "setcloudtoken",          // 15
                                      "set-logging",            // 16
                                      "set-dac",                // 17
                                      "set-serial-port",
                                      "flash-app",              // 19
                                      "micro-strobe",           // 20
                                      "set-server-name",        // 21
                                      "sensor-value",           // 22
                                      "announce",               // 23
                                      "factory-default",        // 24
                                      "software-info",          // 25
                                      "button-pressed",         // 26
                                      "streamsetup",            // 27
                                      "streamdata",             // 28
                                      "streamcontrol",          // 29
                                      "blinkled",               // 30
                                      "sensor-request",         // 31
                                      "user",                   // 32
                                      "radio-get-config",       // 33
                                      "radio-set-config",       // 34
                                      "startapp",               // 35
                                      "installed-software",     // 36
                                      "powerup",                // 37
                                      "getcloudtoken",          // 38
                                      "request-radio-forward",  // 39
                                      "radio-forward",          // 40
                                      "set-config-flag",        // 41
                                      "set-config-flags",       // 42
                                      "set-sensor-server",      // 43
                                      "wifi-info",              // 44
                                      "get-config-flags",       // 45
                                      "wifi-scan",              // 46
                                      "route",                  // 47
                                      "sensor-list",            // 48
                                      "sensor-config",          // 49
                                      "sensor-report",          // 50
                                      "route-update",           //51
                                      "setconfig",              //52
                                      "userapp-info",           //53
                                      "userapp-control",        //54
                                      "bt-ble-write-char",      //55
                                      "metric",                 // 56
                                      "bt-ble-read-char",       //57
                                      "bt-peers",               //58
                                      "clock-sync",             //59
                                      "powerinfo",              //60
                                      NULL };
 
static uint8_t command_match[command_names_size];
void command_start() {
  memset(&command_match, 0, sizeof(command_match));
}
const char *command_get_name(int num) {
  if ((num > command_names_size) || (num < 1)) {
    return "ERROR!";
  }
  return command_names[num - 1];
}
 
/*
 * \brief add an encoded arg, that is a buf starting with the 'fieldtype'. return 0 of ok
 */
int _command_add_encoded_arg(struct command *com, byte *buf) {
  return 0;
}
void command_add_arg(struct command *com, const char *format, ...) {
  va_list args;
 
  va_start(args, format);
  command_add_varg(com, format, args);
  va_end(args);
}
void command_add_varg(struct command *com, const char *format, va_list args) {
  char *buf;
  int r;
 
  if (com->argctr >= COMMAND_MAXARGS) {
    printf("ERROR - command has already maximum args\r\n");
    command_print(com);
    return;
  }
  if (com->argctr == 0) {
    buf = com->argbuf;
    r = sizeof(com->argbuf);
  } else {
    r = get_arg_size_inmem(com, com->argctr - 1);               // end of last arg;
    buf = (char *)get_arg_new(com, com->argctr - 1);            // to last arg
    buf = buf + r;
  }
  vsnprintf(buf + 3, sizeof(com->argbuf) - (buf - com->argbuf) - 3, format, args);
  buf[0] = com->argctr + 1;     // fieldnum
  buf[1] = 0x81;                // byte array field type
  buf[2] = strlen(buf + 3);
  com->argctr++;
}
int command_add_binary_arg(struct command *com, const int len, const byte *srcbuf) {
  char *buf;
  int r;
 
  if (com->argctr == 0) {
    buf = com->argbuf;
    r = sizeof(com->argbuf);
  } else {
    r = get_arg_size(com, com->argctr - 1);             // end of last arg;
    buf = (char *)get_arg(com, com->argctr - 1);        // to last arg
    buf = buf + r + 1;
    r = sizeof(com->argbuf) - (buf - com->argbuf);
  }
  int i;
  int t = 3;
 
  for (i = 0; i < len; i++) {
    byte b = srcbuf[i];
    //    if ( (b == ',') || (b == '}')){
    //  buf[t++] = '\\';
    //}
    buf[t++] = b;
    if (t >= r) {
      IPLOG("buf too long: %i, only got %i bytes left\r\n", len, r);
      return 1;
    }
  }
  buf[0] = com->argctr + 1;
  buf[1] = 129;
  buf[2] = len;
  com->argctr++;
  return 0;
}
 
int ascii_commandstring_to_num(const byte *buf, int size, int *const newpos) {
  int i = 0;
  int j = 0;
  byte found = 0;
 
  for (i = 0; i < command_names_size; i++) {
    const char *com_name = command_names[i];
    if (strlen(com_name) > size) {
      continue;
    }
    found = 1;
    for (j = 0; j < strlen(com_name); j++) {
      if (buf[j] != com_name[j]) {
        found = 0;
        break;
      }
    }
    if (!found) {
      continue;
    }
    if ((j < size) && (buf[j] != ',') && (buf[j] != '}')) {
      continue;
    }
    *newpos = strlen(com_name);
    return i + 1; // (command counting starts at 1 not 0)
  }
  *newpos = 0;
  return 0;
}
char *command_get_source_name(byte sourcedev) {
  if (sourcedev == 1) {
    return "wifi";
  } else if (sourcedev == 2) {
    return "radio";
  } else if (sourcedev == 3) {
    return "localtty";
  } else if (sourcedev == 4) {
    return "usb";
  } else if (sourcedev == 5) {
    return "lora";
  } else if (sourcedev == 6) {
    return "bluetooth";
  } else {
    return "unknown";
  }
}
char sbuf[10];
char rbuf[10];
char tbuf[10];
void flagstostr(char *buf, int flags) {
  char *deli;
 
  deli = "";
  buf[0] = 0;
  if (flags == 0) {
    snprintf(buf + strlen(buf), 20, "NONE");
  }
  if (flags & COMFLAGS_ACK) {
    snprintf(buf + strlen(buf), 20, "%sACK", deli);
    deli = "|";
  }
  if (flags & COMFLAGS_SUCCESS) {
    snprintf(buf + strlen(buf), 20, "%sSUCCESS", deli);
    deli = "|";
  } else if (flags != 0) {
    snprintf(buf + strlen(buf), 20, "%sFAILURE", deli);
    deli = "|";
  }
  if (flags & COMFLAGS_DATA) {
    snprintf(buf + strlen(buf), 20, "%sDATA", deli);
    deli = "|";
  }
  if (flags & COMFLAGS_FORWARDED) {
    snprintf(buf + strlen(buf), 20, "%sFWD", deli);
    deli = "|";
  }
  if (flags & COMFLAGS_POWERSAVE) {
    snprintf(buf + strlen(buf), 20, "%sPWR", deli);
    deli = "|";
  }
  snprintf(buf + strlen(buf), 20, "(%i)", flags);
}
void command_print(struct command *com) {
  int i;
  char flagbuf[80];
 
  if (com == NULL) {
    return;
  }
  if (!is_command_valid(com)) {
    error_com(com);
    return;
  }
  if ((com->com > command_names_size) || (com->com < 1)) {
    IPLOG("Command error: %i is out of range (not a valid command!)\r\n", com->com);
    return;
  }
  flagstostr((char *)&flagbuf, com->flags);
  pretty_node_to_str(com->sender, (char *)&sbuf);
  pretty_node_to_str(com->recipient, (char *)&rbuf);
  pretty_node_to_str(com->target, (char *)&tbuf);
  IPLOG(" [command %p]\r\n device=%s, index=%i, com=%s, flags=%s\r\n sender=%s recipient=%s target=%s\r\n",
        com,
        command_get_source_name(com->sourcedev), com->index,
        command_get_name(com->com), &flagbuf,
        (char *)&sbuf, (char *)&rbuf, (char *)&tbuf
        );
  if (com->com == 28) { // do not print args of data!
    IPLOG("   arg: [data]\r\n");
    return;
  }
  for (i = 0; i < com->argctr; i++) {
    IPLOG("   arg %i: %s\r\n", i, get_arg(com, i));
  }
}
 
/********************************************************
*********** parse a buffer to a command structure ********
********************************************************/
int command_decode_binary(struct command *com, const byte *buf, int size) {
  //  fatal_error("binary commands not yet supported!");
  return 5;
}
 
 
// *buf->first address,second adr, ...
// (version==1,2,3..) (from packet {1,[version],..}
// return 0 if ok, otherwise error
int command_decode_ascii(struct command *com, const byte *buf, int size, int version) {
  int curpos;
  int newpos;
  int num;
  long h;
  int i;
  byte chksum;
  byte nc;
 
  CNWDEBUGCOM("Parsing asciibuf \"%s\"\r\n", buf);
  command_start();
  if (size < 5) {
    CNWDEBUG("Buf too short: %i\r\n", size);
    return 1;
  }
  curpos = 0;
  newpos = 0;
  num = ascii_parse_int(buf + curpos, size - curpos, &newpos);
  curpos = curpos + newpos;
  if ((newpos <= 0) || (curpos >= size) || (buf[curpos] != ',')) {
    CNWDEBUG("started incorrectly: newpos=%i,curpos=%i,b[]=%i\r\n", newpos, curpos, buf[curpos]);
    return 2;
  }
  com->index = num;
  CNWDEBUGCOM("1. size=%i,newpos=%i,curpos=%i,buf->%s\r\n", size, newpos, curpos, buf + curpos);
 
  for (i = 0; i < 3; i++) {
    curpos++; // skip comma
    h = ascii_parse_hex(buf + curpos, size - curpos, &newpos);
    curpos = curpos + newpos;
    if ((newpos <= 0) || (curpos >= size) || (buf[curpos] != ',')) {
      CNWDEBUG("nodeids invalid: newpos=%i,curpos=%i,b[]=%i\r\n", newpos, curpos, buf[curpos]);
      return 3;
    }
    if (i == 0) {
      com->sender = h;
    } else if (i == 1) {
      com->recipient = h;
    } else if (i == 2) {
      com->target = h;
    }
    CNWDEBUGCOM("2. size=%i,newpos=%i,curpos=%i,buf->%s\r\n", size, newpos, curpos, buf + curpos);
  }
 
  curpos++;   // skip comma
 
  // parse checksum
  chksum = atoh((const char *)buf + curpos);
  curpos = curpos + 3;
 
  // parse index
  num = ascii_parse_int(buf + curpos, size - curpos, &newpos);
  curpos = curpos + newpos;
  if ((newpos <= 0) || (curpos >= size) || (buf[curpos] != ',')) {
    CNWDEBUG("index invalid: idx=%i, newpos=%i,curpos=%i,b[]=%i\r\n", num, newpos, curpos, buf[curpos]);
    return 4;
  }
  com->index = num;
 
  curpos++;   // skip comma, parse flags
  com->flags = (byte)ascii_parse_int(buf + curpos, size - curpos, &newpos);
  curpos = curpos + newpos;
  if ((newpos <= 0) || (curpos >= size) || (buf[curpos] != ',')) {
    return 5;
  }
 
  CNWDEBUGCOM("3. size=%i,newpos=%i,curpos=%i,buf->%s\r\n", size, newpos, curpos, buf + curpos);
 
  curpos++;   // skip comma, parse command number
  num = ascii_commandstring_to_num(buf + curpos, size - curpos, &newpos);
  curpos = curpos + newpos;
  CNWDEBUGCOM("4. size=%i,newpos=%i,curpos=%i,buf->%s\r\n", size, newpos, curpos, buf + curpos);
  if ((num < 1) || (newpos <= 0)) {
    CNWDEBUG("not a command! (result=%i, newpos=%i)\r\n", num, newpos);
    return 6;
  }
  com->com = num;
  if (size <= curpos) {
    CNWDEBUG("5. Minimum break, size=%i, curpos=%i\r\n", size, curpos);
    goto returncommand;
  }
 
  // check for args
  com->argctr = 0;
 
  if (buf[curpos] == '}') {
    // no args, com ended
    goto returncommand;
  }
  if (buf[curpos] != ',') {
    CNWDEBUG("6. break, illegal character at pos %i: \"%s\"\r\n", curpos, &buf[curpos]);
    return 7;
  }
 
  // parse args
  int argbufpos = 0;
  byte backslash_detected = 0;
 
  // we are only parsing byte arrays in ascii decoder!
  // insert type and length at beginning
 
  for (;;) {
    // any args?
    curpos++; // skip comma
 
    if (curpos >= size) {
      break;
    }
    int field_header_pos = argbufpos;
    argbufpos = argbufpos + 3;
    com->argbuf[field_header_pos] = com->argctr + 1;    //fieldnum
    com->argbuf[field_header_pos + 1] = 129;            //array
 
    backslash_detected = 0;
    com->argctr++;
    CNWDEBUGCOM("argctr=%i,buf->%s\n", com->argctr, buf + curpos);
    if (com->argctr > COMMAND_MAXARGS) {
      CNWDEBUG("Command has too many arguments: %i (buf->%s)\n", com->argctr, buf + curpos);
      return 8;
    }
    int argsize = 0;
 
    // repeat until either end of buf (by size) or an unescaped comma is found
    while ((backslash_detected || (buf[curpos] != ','))
           && (curpos < size)) {
      // single backslash?
      if (((!backslash_detected)) && (buf[curpos] == '\\')) {
        backslash_detected = 1;
        curpos++;
        continue;
      }
      if ((!backslash_detected) && (buf[curpos] == '}')) {
        com->argbuf[field_header_pos + 2] = argsize;
        CNWDEBUGCOM("   arg length: %i\n", argsize);
        CNWDEBUGCOM("end of command detected at pos %i (%s)\n", curpos, buf + curpos);
        goto returncommand;
      }
      backslash_detected = 0;
 
      com->argbuf[argbufpos] = buf[curpos];
      curpos++;
      argbufpos++;
      argsize++;
    }
    com->argbuf[field_header_pos + 2] = argsize;
    CNWDEBUGCOM("   arg length: %i\n", argsize);
    com->argbuf[argbufpos] = 0;
    argbufpos++;
  }
 
returncommand:
  nc = command_calc_checksum(com);
  if (nc > 0xFF) {
    fatal_error("checksum broken");
  }
  nc = nc & 0xFF;
  if (nc != chksum) {
    //    hexdump(&com->argbuf,60);
    free_command(com);
    printf("Command checksum invalid! (calc=%x, received=%x)\r\n", nc, chksum);
    //    printHex("", (uint8_t *)buf, size);
    //    hexdump((uint8_t *)buf, size);
    return 9;
  }
 
  return 0;
}
// returns a command or NULL if not a valid command
struct command *command_parse(const byte *buf, int size) {
  if (size < 3) {
    CNWDEBUG("size less than 2! (%i)\n", size);
    return NULL;
  }
  if (buf[0] != '{') {
    CNWDEBUG("Buf does not start with {\n");
    return NULL;
  }
  // we're trying to find end of buf here (removing superflous stuff)
  int x = size - 1;
 
  while ((x > 4) && (buf[x] != '}')) {
    x--;
  }
  if (buf[x] != '}') {
    CNWDEBUG("Buf does not end with } but with '%i', size=%i\r\n\r\n", buf[size - 1], size);
    return NULL;
  }
 
  struct command *com = alloc_command();
 
  if (com == NULL) {
    // cannot parse if we have no command to parse into
    return NULL;
  }
  int res = -10;
 
  if (buf[1] == 'a') {
    int version = buf[3] - '0'; // version
    res = command_decode_ascii(com, buf + 3, x, version);
  } else if (buf[1] == 'b') {
    res = command_decode_binary(com, buf + 1, x);
  } else {
    CNWDEBUG("buf is neither ascii nor binary (%s)\n", buf);
  }
  if (res == 0) {
    return com;
  }
  printf("not a valid command (parse error: %i)\r\n", res);
  //hexdump(com->argbuf,250);
  int i;
 
  for (i = 0; i < com->argctr; i++) {
    const char *arg = get_arg(com, i);
    printf("Arg %i: %i (%s)\n", i, (arg - com->argbuf), arg);
  }
  free_command(com);
  return NULL;
}
 
/*********************************************************
*********** turn a command structure to ascii ***********
*********************************************************/
int command_encode_ascii(struct command *com, int bufsize, char *buf) {
  int bctr = 0;
  int i, j;
  long id;
  const char *arg;
 
  if (!(is_command_valid(com))) {
    return -3;
  }
  memset(buf, 0, bufsize);
  if ((com->com == 0) || (com->com > command_names_size)) {
    return -2;
  }
  if (bufsize < 5) {
    printf("Cannot encode ascii to a buffer <5 bytes!\n");
    return -1;
  }
  buf[bctr++] = '{';    // command start
  buf[bctr++] = 'a';    // com->encoding;
  snprintf(buf + bctr, bufsize - bctr, ",%i,", PROTOCOL_VERSION_ASCII);
  bctr = strlen(buf);
  if (bufsize - bctr < 17) {
    return -1;
  }
  for (i = 0; i < 3; i++) {
    if (i == 0) {
      id = com->sender;
    } else if (i == 1) {
      id = com->recipient;
    } else if (i == 2) {
      id = com->target;
    }
    node_to_str(id, buf + bctr);
    bctr = strlen(buf);
    buf[bctr++] = ',';
    buf[bctr] = 0;
    if (bufsize - bctr < 17) {
      return -1;
    }
  }
  // add checksum
  bctr = strlen(buf);
  int crc = command_calc_checksum(com);
 
  crc = crc & 0xFF;
  snprintf(buf + bctr, bufsize - bctr, "%x,", crc);
 
  // add index, flags and commandname
  bctr = strlen(buf);
  snprintf(buf + bctr, bufsize - bctr, "%i,%i,%s", com->index,
           (int)com->flags,
           command_get_name(com->com)
           );
  // add parameters
  bctr = strlen(buf);
  for (i = 0; i < com->argctr; i++) {
    arg = get_arg(com, i);
    if (arg == NULL) {
      break;
    }
    buf[bctr] = ',';
    bctr++;
    j = 0;
    while (j < get_arg_size(com, i)) {
      // escape } and ,
      if ((arg[j] == ',')
          || (arg[j] == '\\')
          || (arg[j] == '{')
          || (arg[j] == '}')) {
        buf[bctr++] = '\\';
      }
      // copy arg to buf
      buf[bctr++] = arg[j];
      if (bctr >= bufsize) {
        return -1;
      }
      j++;
    }
    buf[bctr] = 0;
  }
  buf[bctr++] = '}'; // command end
  buf[bctr] = 0;
  return bctr;
}
void command_init(struct command *com) {
  memset(com, 0, sizeof(struct command));
  com->sender = config_get_ram_struct()->nodeid;
  com->index = -1;
  com->sourcedev = 0;
}
 
const byte *get_arg_new(const struct command *com, int index) {
  int i;
  const byte *buf = (const byte *)com->argbuf;
 
  if (index >= com->argctr) {
    return buf;
  }
 
  if (index == 0) {
    return buf;
  }
 
  int len;
 
  for (i = 0; i < index; i++) {
    len = fieldlen_inmem(buf[1], buf[2]);
    //    printf("arg %i -> %i bytes in memory\n",i,len);
    buf = buf + len;
  }
  //  printf("arg %i -> first byte: %i\n",index,buf[0]);
  return buf;
}
 
int get_arg_size_inmem(const struct command *com, int index) {
  const byte *buf = get_arg_new(com, index);
 
  if (buf == NULL) {
    return 0;
  }
  int len = fieldlen_inmem(buf[1], buf[2]);
 
  return len;
}
 
int get_arg_size(const struct command *com, int index) {
  if (index >= com->argctr) {
    return 0;
  }
  const byte *buf = get_arg_new(com, index);
 
  return fieldlen(buf[1], buf[2]);
}
 
const char *get_arg(const struct command *com, int index) {
  const byte *buf = get_arg_new(com, index);
 
  if (buf[1] == 129) {
    return (const char *)buf + 3;       // skip fieldnum+type+size
  }
  return (const char *)buf + 2;         // skip fieldnum+type
}
/*
 * \brief calc checksum of command
 *
 */
byte command_calc_checksum(struct command *com) {
  int i;
  int j;
  uint32_t chk = 0xC5;
  int r = 0;
 
  CNWDEBUGCOM("Checksum step #%i: %lx\r\n", r++, chk);
  chk ^= com->index;
  CNWDEBUGCOM("Checksum step #%i: %lx\r\n", r++, chk);
  chk ^= com->sender;
  CNWDEBUGCOM("Checksum step #%i: %lx\r\n", r++, chk);
  chk ^= com->recipient;
  CNWDEBUGCOM("Checksum step #%i: %lx\r\n", r++, chk);
  chk ^= com->target;
  CNWDEBUGCOM("Checksum step #%i: %lx\r\n", r++, chk);
  chk ^= com->com;
  CNWDEBUGCOM("Checksum step #%i: %lx\r\n", r++, chk);
  chk ^= com->argctr;
  CNWDEBUGCOM("Checksum step #%i: %lx\r\n", r++, chk);
  chk ^= com->flags;
  CNWDEBUGCOM("Checksum step #%i: %lx\r\n", r++, chk);
  for (i = 0; i < com->argctr; i++) {
    const char *x = get_arg(com, i);
    for (j = 0; j < get_arg_size(com, i); j++) {
      chk ^= x[j] & 0xFF;
    }
    CNWDEBUGCOM("checksum after arg #%i of size %i: %lx (arg=%s)\r\n", i, get_arg_size(com, i), chk, x);
  }
  CNWDEBUGCOM("After paras: Checksum step #%i: %lx\r\n", r++, chk);
  byte res = 0;
  byte z;
 
  for (i = 0; i < 4; i++) {
    CNWDEBUGCOM("RChecksum step #%i: %x\r\n", r++, res);
    z = chk >> (i * 8);
    res ^= z;
  }
  chk = res;
  CNWDEBUGCOM("Checksum step #%i: %lx\r\n", r++, chk);
  r++;  // keep gcc happy
  //res = res ;
  res = res & 0xFF;
  return res;
}
const char *namedarg(struct command *com, const char *name) {
  int i;
 
  for (i = 0; i < com->argctr; i++) {
    if (startsWith(get_arg(com, i), name) && (get_arg(com, i)[strlen(name)] == '=')) {
      return get_arg(com, i) + strlen(name) + 1;
    }
  }
  return NULL;
}
int namedarg_uint32(struct command *com, const char *name, uint32_t *value) {
  const char *v = namedarg(com, name);
 
  if (v == NULL) {
    return 1;
  }
  uint32_t res = atoi(v);
 
  *value = res;
  return 0;
}
int namedarg_uint16(struct command *com, const char *name, uint16_t *value) {
  const char *v = namedarg(com, name);
 
  if (v == NULL) {
    return 1;
  }
  uint16_t res = atoi(v);
 
  *value = res;
  return 0;
}
 
int get_arg_int_array(const struct command *com, const int index, int *result, const int arraysize) {
  if (com->argctr <= index) {
    printf("asked for more args than we have (req=%i, got=%i)\r\n", index, com->argctr);
    return 0;
  }
  int size = get_arg_size(com, index);
  int i = 0;
  int num = 0;
  const char *buf = get_arg(com, index);
 
  while (i < size) {
    if (num >= arraysize) {
      break;
    }
    int r;
    *(result + num) = ascii_parse_hex((const unsigned char *)&buf[i], size - i, &r);
    i = i + r + 1;
    num++;
  }
  return num;
}
 
// basic sanity check if it's within a minimum sane memory area
int is_command_valid(struct command *com) {
#ifdef DEBUG_ON_LINUX
  return 1;
#endif
  if ((void *)com > (void *)0x2000BFFF) {
    return 0;
  }
  return 1;
}
void error_com(void *com) {
  printf("Invalid command at %p\r\n", com);
}