decode.c

#include <coms/coms.h>
#include "main-header.h"
#include <platform-header.h>
#ifndef CNWDEBUGCOM
#define CNWDEBUGCOM(a, ...) noop()
#endif
//#define CNWDEBUGCOM printf
 
 
/*
 * \brief add an encoded arg to com
 * buf is a pointer to the on-the-wire formatted command (buf[0] ==  'b')
 * bufsize is the total size in bytes of buf
 * pos is the position at which the argument begins (buf[*pos] == fieldnum).
 * if result != 0 -> error
 * pos will contain the position of the next byte after the argument
 */
int command_add_encoded_arg_with_pos(struct command *com, byte *buf, int bufsize, int *pos) {
  if (buf[0] != 'b') {
    return 13;
  }
  if ((*pos) > (bufsize - 3)) {
    CNWDEBUGCOM("cannot decode out an argument at position %i with bufsize %i\n", *pos, bufsize);
    return 11;
  }
  byte fieldnum = buf[(*pos)];
  byte fieldtype = buf[(*pos) + 1];
  byte arraysize = buf[(*pos) + 2];
  int len = fieldlen(fieldtype, arraysize);
 
 
  byte *nbuf;
 
  // find position of new arg:
  if (com->argctr == 0) {
    nbuf = (byte *)com->argbuf;
  } else {
    int r = get_arg_size_inmem(com, com->argctr - 1);           // end of last arg;
    nbuf = (byte *)get_arg_new(com, com->argctr - 1);           // to last arg
    nbuf = nbuf + r;
  }
 
  CNWDEBUGCOM("  Copying bytes from pos %i - %i bytes of argument fieldnum=%i,type=%i, writing to combuf %i\n", *pos, len, fieldnum, fieldtype, (nbuf - (uint8_t *)&com->argbuf));
  int npos = 0;
 
  nbuf[npos++] = fieldnum;
  nbuf[npos++] = fieldtype;
  int header = 0;
 
  if (fieldtype & 0x80) {
    nbuf[npos++] = arraysize;
    header = 1;
  }
  if ((len + (*pos) + 2) > bufsize) {
    CNWDEBUGCOM("Len: %i, pos: %i, (len+pos+2: %i), bufsize: %i, writing to %i\n", len, *pos, (*pos) + len + 2, bufsize, (nbuf - (uint8_t *)&com->argbuf));
    return 12;
  }
 
  int j;
 
  for (j = 0; j < len; j++) {
    nbuf[npos++] = buf[j + (*pos) + 2 + header];
  }
  // if array -> add nul terminator
  if (fieldtype & 0x80) {
    nbuf[npos++] = 0;
    *pos = (*pos) + 1;
  }
  *pos = (*pos) + len + 2;
  com->argctr++;
  return 0;
}
 
/************************************** start decoding ******************************/
 
// 0 == ok
int get_uint64(byte *buf, int bufsize, int *pos, uint64_t *res) {
  if (((*pos) + 8) > bufsize) {
    return 3;
  }
  int i;
  uint64_t r = 0;
 
  for (i = 0; i < 8; i++) {
    uint64_t v = ((uint64_t)buf[i + (*pos)]);
    v = v << ((i) * 8);
    r = r | v;
  }
  *pos = *pos + 8;
  *res = r;
  return 0;
}
// 0 == ok
int get_uint32(byte *buf, int bufsize, int *pos, uint32_t *res) {
  if (((*pos) + 4) > bufsize) {
    return 4;
  }
  int i;
  uint32_t r = 0;
 
  for (i = 0; i < 4; i++) {
    uint32_t v = ((uint32_t)buf[i + (*pos)]);
    v = v << ((i) * 8);
    r = r | v;
  }
  *pos = *pos + 4;
  *res = r;
  return 0;
}
// 0 == ok
int get_uint16(byte *buf, int bufsize, int *pos, uint16_t *res) {
  if (((*pos) + 2) > bufsize) {
    return 2;
  }
  int i;
  uint32_t r = 0;
 
  for (i = 0; i < 2; i++) {
    uint32_t v = ((uint32_t)buf[i + (*pos)]);
    v = v << ((i) * 8);
    r = r | v;
  }
  *pos = *pos + 2;
  *res = r;
  return 0;
}
 
 
// 0 == ok
int get_uint8(byte *buf, int bufsize, int *pos, uint8_t *res) {
  if (((*pos)) >= bufsize) {
    CNWDEBUGCOM("Pos: %i, bufsize: %i\n", *pos, bufsize);
    return 5;
  }
  *res = buf[*pos];
  *pos = *pos + 1;
  return 0;
}
 
/*
 * \brief  decode buf into com. return 0 - ok, otherwise error. buf must start with '{' and end with '}'
 */
int decode(byte *buf, int bufsize, struct command *com) {
  if (bufsize < 36) {
    return 1;
  }
  if (buf[0] != 'b') {
    return 2;
  }
  CNWDEBUGCOM("-------- Decoding --------\n");
  uint64_t res;
  uint32_t res32;
  uint8_t res8;
  int pos = 0;
  int e;
 
  if ((e = get_uint8(buf, bufsize, &pos, &res8)) != 0) {
    return e;
  }
  if (res8 != 'b') {
    CNWDEBUGCOM("not a binary encoding 0x%x\n", res8);
    return 6;
  }
  if ((e = get_uint8(buf, bufsize, &pos, &res8)) != 0) {
    return e;
  }
  if (res8 != 1) {
    CNWDEBUGCOM("Incompatible version 0x%x\n", res8);
    return 7;
  }
 
  pos = pos + 4; // length,checksum
 
  if ((e = get_uint64(buf, bufsize, &pos, &res)) != 0) {
    return e;
  }
  com->sender = res;
 
  if ((e = get_uint64(buf, bufsize, &pos, &res)) != 0) {
    return e;
  }
  com->recipient = res;
 
  if ((e = get_uint64(buf, bufsize, &pos, &res)) != 0) {
    return e;
  }
  com->target = res;
 
  if ((e = get_uint32(buf, bufsize, &pos, &res32)) != 0) {
    return e;
  }
  com->index = (int)res32;
  //  printf("Res: 0x%x %i, index: 0x%x %i\n",res32,res32,com->index,com->index);
 
  if ((e = get_uint8(buf, bufsize, &pos, &res8)) != 0) {
    return e;
  }
  com->com = res8;
 
  if ((e = get_uint8(buf, bufsize, &pos, &res8)) != 0) {
    return e;
  }
  com->flags = res8;
 
  if ((e = get_uint8(buf, bufsize, &pos, &res8)) != 0) {
    return e;
  }
  com->argctr = 0; // we set com->argctr by adding arguments one by one
  int args = res8;
 
  CNWDEBUGCOM("Command type: %i\n", com->com);
  CNWDEBUGCOM("Sender: %p\n", com->sender);
  CNWDEBUGCOM("Index: %p\n", com->index);
  CNWDEBUGCOM("Args: %i\n", args);
  if (com->sender == 0) {
    return 8;
  }
  if (com->sender > 0xFFFFFFFF) {
    return 9;
  }
 
  //decode args
  int i;
 
  for (i = 0; i < args; i++) {
    /*
     * uint8_t fieldnum;
     * uint8_t fieldtype;
     * uint8_t asize=0;
     *
     * if ((e = get_uint8(buf, bufsize, &pos, &fieldnum)) != 0) { // field num
     * return e;
     * }
     * if ((e = get_uint8(buf, bufsize, &pos, &fieldtype)) != 0) { // field type
     * return e;
     * }
     * if (fieldtype & (1<<7)) {
     * if ((e = get_uint8(buf, bufsize, &pos, &asize)) != 0) { // field type
     *  return e;
     * }
     * }
     * CNWDEBUGCOM("Position %i, arg %i, fieldnum: %i, fieldtype: %i, arraysize: %i\n", pos, i, fieldnum,fieldtype,asize);
     */
    e = command_add_encoded_arg_with_pos(com, buf, bufsize, &pos);
    if (e != 0) {
      return e;
    }
  }
  // args decoded
//printf("Decoding: %s\n",buf);
  return 0;
}