protocol.cpp

#include <cinttypes>
#include <cstdlib>
#include <cstring>
#include <cstdio>
#include <functional>

const uint8_t READ_FAILED = 0;
const uint8_t READ_SUCCESS = 1;
const uint8_t MAX_FIELDS = 8;
const uint32_t FIELDS_BUFFER_SIZE = 512;
const uint8_t BUFFER_SIZE = 128;
const uint8_t MAX_OUTSTANDING_REQUESTS = 8;
const char * FIELD_SEPERATOR = "";
const char * EOT = "";
const char * SEPERATOR = "\0";

uint8_t crc_table[256] = {
                          0x00, 0x07, 0x0e, 0x09, 0x1c, 0x1b, 0x12, 0x15, 0x38, 0x3f, 0x36, 0x31, 0x24, 0x23, 0x2a, 0x2d, 0x70, 0x77, 0x7e, 0x79, 0x6c, 0x6b, 0x62, 0x65, 0x48, 0x4f, 0x46, 0x41, 0x54, 0x53, 0x5a, 0x5d, 0xe0, 0xe7, 0xee, 0xe9, 0xfc, 0xfb, 0xf2, 0xf5, 0xd8, 0xdf, 0xd6, 0xd1, 0xc4, 0xc3, 0xca, 0xcd, 0x90, 0x97, 0x9e, 0x99, 0x8c, 0x8b, 0x82, 0x85, 0xa8, 0xaf, 0xa6, 0xa1, 0xb4, 0xb3, 0xba, 0xbd, 0xc7, 0xc0, 0xc9, 0xce, 0xdb, 0xdc, 0xd5, 0xd2, 0xff, 0xf8, 0xf1, 0xf6, 0xe3, 0xe4, 0xed, 0xea, 0xb7, 0xb0, 0xb9, 0xbe, 0xab, 0xac, 0xa5, 0xa2, 0x8f, 0x88, 0x81, 0x86, 0x93, 0x94, 0x9d, 0x9a, 0x27, 0x20, 0x29, 0x2e, 0x3b, 0x3c, 0x35, 0x32, 0x1f, 0x18, 0x11, 0x16, 0x03, 0x04, 0x0d, 0x0a, 0x57, 0x50, 0x59, 0x5e, 0x4b, 0x4c, 0x45, 0x42, 0x6f, 0x68, 0x61, 0x66, 0x73, 0x74, 0x7d, 0x7a, 0x89, 0x8e, 0x87, 0x80, 0x95, 0x92, 0x9b, 0x9c, 0xb1, 0xb6, 0xbf, 0xb8, 0xad, 0xaa, 0xa3, 0xa4, 0xf9, 0xfe, 0xf7, 0xf0, 0xe5, 0xe2, 0xeb, 0xec, 0xc1, 0xc6, 0xcf, 0xc8, 0xdd, 0xda, 0xd3, 0xd4, 0x69, 0x6e, 0x67, 0x60, 0x75, 0x72, 0x7b, 0x7c, 0x51, 0x56, 0x5f, 0x58, 0x4d, 0x4a, 0x43, 0x44, 0x19, 0x1e, 0x17, 0x10, 0x05, 0x02, 0x0b, 0x0c, 0x21, 0x26, 0x2f, 0x28, 0x3d, 0x3a, 0x33, 0x34, 0x4e, 0x49, 0x40, 0x47, 0x52, 0x55, 0x5c, 0x5b, 0x76, 0x71, 0x78, 0x7f, 0x6a, 0x6d, 0x64, 0x63, 0x3e, 0x39, 0x30, 0x37, 0x22, 0x25, 0x2c, 0x2b, 0x06, 0x01, 0x08, 0x0f, 0x1a, 0x1d, 0x14, 0x13, 0xae, 0xa9, 0xa0, 0xa7, 0xb2, 0xb5, 0xbc, 0xbb, 0x96, 0x91, 0x98, 0x9f, 0x8a, 0x8d, 0x84, 0x83, 0xde, 0xd9, 0xd0, 0xd7, 0xc2, 0xc5, 0xcc, 0xcb, 0xe6, 0xe1, 0xe8, 0xef, 0xfa, 0xfd, 0xf4, 0xf3
};

uint8_t crc8(uint8_t old, uint8_t input) {
  return crc_table[input ^ old];
}

template<typename T>
auto parse_string(char * string) -> T;

template<>
auto parse_string(char * string) -> float {
  return strtof(string, NULL);
}

template<>
auto parse_string(char * string) -> double {
  return strtod(string, NULL);
}

template<>
auto parse_string(char * string) -> uint32_t {
  return strtol(string, NULL, 10);
}

template<typename W, typename R>
class ControlDaemonInterface {
private:
  uint16_t request_id_counter = 0;

  class BufferedWriter {
  private:
    char buffer[BUFFER_SIZE] = { 0 };
    W writer;
    uint32_t data_amount = 0;
  public:
    uint8_t crc = 0;

    // BufferedWriter(W writer) : writer(writer) {};

    uint8_t write(const char * data, uint32_t len) {
      do {
        for(; (data_amount < BUFFER_SIZE) && (len > 0); data_amount++, len--) {
          crc = crc8(crc, *data);
          buffer[data_amount] = *data++;
        }

        if ((data_amount == BUFFER_SIZE) && (len > 0)) {
          flush(/* reset_crc = */ false);
        }
      } while (len > 0);

      return crc;
    }

    void flush(bool reset_crc = true) {
      uint32_t written_total = 0;

      while (written_total < data_amount) {
        written_total += writer.write(&buffer[written_total], BUFFER_SIZE - written_total);
      }

      if (reset_crc) {
        crc = 0;
      }

      data_amount = 0;
    }
  };

  BufferedWriter writer;

  template<typename C>
  class BufferedReader {
  private:
    char buffer[BUFFER_SIZE] = { 0 };
    R reader;
    uint32_t pos = 0;
    uint32_t data_amount = 0;

  public:
    // BufferedReader(R reader) : reader(reader) {};

    auto read_until(char stop_char, C callback, bool nonblocking = true) -> void {
      auto found = false;

      while (!found) {
        auto start = pos;
        auto max = pos + data_amount;
        for (; pos < max; pos++) {
          if (buffer[pos] == stop_char) {
            found = true;
            pos++;
            break;
          }
        }

        callback(false, &buffer[start], pos - start);

        if ((!found) && (pos < BUFFER_SIZE)) {
          data_amount = reader.read(&buffer[pos], BUFFER_SIZE - pos);

          if ((data_amount == 0) && nonblocking) {
            return;
          }
        } else { // buffer full, or we found something
          pos = 0;
          data_amount = 0;
          callback(true, nullptr, 0);
          return;
        }
      }
    }
  };

  BufferedReader<std::function<void(bool, char*, uint32_t)>> reader;

  // template<typename T>
  // using RegReadCallback = std::function<void(uint8_t, T)>;
  // using WriteCallback = std::function<void(const char[BUFFER_SIZE])>;
  // using ReadCallback = std::function<void(const char[BUFFER_SIZE])>;
  using Callback = std::function<void(char**)>;

  class RequestBuffer {
  private:
    class Request {
    public:
      char id[4];
      Callback callback;

      Request() {}
      Request(char req_id[4], Callback callback) : callback(callback) {
        id[0] = req_id[0];
        id[1] = req_id[1];
        id[2] = req_id[2];
        id[3] = req_id[3];
      }
    };

    Request outstanding_requests[MAX_OUTSTANDING_REQUESTS];
    bool free_outstanding_requests[MAX_OUTSTANDING_REQUESTS];

  public:
    RequestBuffer() {
        for (uint32_t i = 0; i < MAX_OUTSTANDING_REQUESTS; i++) {
          free_outstanding_requests[i] = true;
        }
    }

    auto push(char id[4], Callback callback) {
      for (uint32_t i = 0; i < MAX_OUTSTANDING_REQUESTS; i++) {
        if (free_outstanding_requests[i]) {
          free_outstanding_requests[i] = false;
          outstanding_requests[i] =  Request(id, callback);
        }
      }
    }

    auto notify_callback(char id[4], char ** data) {
      for (uint32_t i = 0; i < MAX_OUTSTANDING_REQUESTS; i++) {
        if (!free_outstanding_requests[i]) {
          if (!strncmp(outstanding_requests[i].id, id, 4)) {
            free_outstanding_requests[i] = true;
            outstanding_requests[i].callback(data);
            break;
          }
        }
      }
    }
  };

  RequestBuffer request_buffer;

  void send_request(const char type, const char ** fields, Callback callback) {
    char id_buf[5];
    snprintf(id_buf, 5, "%04hx", request_id_counter++);

    request_buffer.push(id_buf, callback);

    writer.write(&type, 1);

    writer.write(id_buf, 4);

    do {
      writer.write(FIELD_SEPERATOR, 1);
      writer.write(*fields, strlen(*fields));
    } while (*(++fields));

    writer.write(EOT, 1);
    snprintf(id_buf, 3, "%02hhx", writer.crc);
    writer.write(id_buf, 2);
    writer.write(SEPERATOR, 1);

    writer.flush();
  }

  struct ParserState {
    enum State {
                READING_TYPE,
                READING_ID0,
                READING_ID1,
                READING_ID2,
                READING_ID3,
                READING_FIELDS,
                READING_CRC0,
                READING_CRC1,
                READING_SEPERATOR,
    };

    State state;

    char type;
    char id[4];
    char transmitted_crc[3];
    uint8_t crc;
    uint8_t calculated_crc;
    uint32_t field_num;
    char* fields[MAX_FIELDS];
  };

  ParserState parser_state;

  auto process_incoming_data(bool buffer_overrun, char * data, uint32_t bytes) {
    using State = typename ParserState::State;

    if (buffer_overrun) {
      parser_state.crc = 0;
      parser_state.state = State::READING_TYPE;
      return;
    }

    for (uint32_t i = 0; i < bytes; i++) {
      auto byte = data[i];

      parser_state.crc = crc8(parser_state.crc, byte);

      switch (parser_state.state) {
      case State::READING_TYPE: {
        parser_state.field_num = 0;

        for (uint32_t i = 0; i < MAX_FIELDS; i++) {
          parser_state.fields[i] = nullptr;
        }

        parser_state.type = byte;
        parser_state.state = State::READING_ID0;
        break;
      }
      case State::READING_ID0: {
        parser_state.id[0] = byte;
        parser_state.state = State::READING_ID1;
        break;
      }
      case State::READING_ID1: {
        parser_state.id[1] = byte;
        parser_state.state = State::READING_ID2;
        break;
      }
      case State::READING_ID2: {
        parser_state.id[2] = byte;
        parser_state.state = State::READING_ID3;
        break;
      }
      case State::READING_ID3: {
        parser_state.id[3] = byte;
        parser_state.state = State::READING_FIELDS;
        break;
      }
      case State::READING_FIELDS: {
        if (byte == *FIELD_SEPERATOR) {
          data[i] = '\0';

          if (parser_state.field_num < MAX_FIELDS) {
            parser_state.fields[parser_state.field_num++] = &data[i + 1];
          } else {
            // TODO(robin): what to do about fields overrun?
          }
        } else if (byte == *EOT) {
          data[i] = '\0';

          parser_state.calculated_crc = parser_state.crc;
          parser_state.state = State::READING_CRC0;
        }
        break;
      }
      case State::READING_CRC0: {
        parser_state.transmitted_crc[0] = byte;
        parser_state.state = State::READING_CRC1;
        break;
      }
      case State::READING_CRC1: {
        parser_state.transmitted_crc[1] = byte;

        uint8_t crc = strtol(parser_state.transmitted_crc, nullptr, 16);

        if (crc != parser_state.calculated_crc) {
          // TODO(robin): what to do about that?
        }

        parser_state.state = State::READING_SEPERATOR;
        break;
      }
      case State::READING_SEPERATOR: {
        if (byte == *SEPERATOR) {
          // TODO(robin): handle other types
          if (parser_state.type == 'R') {
            request_buffer.notify_callback(parser_state.id, parser_state.fields);
          }

          parser_state.calculated_crc = 0;
          parser_state.state = State::READING_TYPE;
        } else {
          // TODO(robin): error handling?
          parser_state.crc = 0;
          parser_state.state = State::READING_TYPE;
        }
        break;
      }
      }
    }
  }

public:
  void handle_incoming() {
    reader.read_until(SEPERATOR[0], [&](bool overrun, char * data, uint32_t bytes) {
                                    process_incoming_data(overrun, data, bytes);
                                     });
  }

  template<typename T, typename C>
  void read(const char * name, C callback) {
    const char * fields[2] = { name, nullptr };
    send_request('G', fields,
                 [&](char ** fields) {
                   if (!strcmp(fields[0], "OK")) {
                     callback(READ_SUCCESS, parse_string<T>(fields[1]));
                   } else {
                     callback(READ_FAILED, 0);
                   }
                 });
  }
  // X|xxxx|FIELD_SEPERATOR|FIELD_SEPERATOR.join(["analog_gain", "5"])|EOT|CRC|\0
};

struct DummyWriter {
  uint32_t write(const char * data, uint32_t size) {
    for (uint32_t i = 0; i < size; i++) {
      printf("%c", data[i]);
    }

    return size;
  }
};

struct DummyReader {
  const char values[16] = {
                        'R',
                        '0',
                        '0',
                        '0',
                        '0',
                        0x1e,
                        'O',
                        'K',
                        0x1e,
                        '1',
                        '.',
                        '2',
                        0x04,
                        '9',
                        '2',
                        0
  };

  const uint8_t len = 16;
  uint8_t pos = 0;

  uint32_t read(char * data, uint32_t amount) {
    if (pos < len) {
      data[0] = values[pos++];
      return 1;
    } else {
      return 0;
    }
  }
};

int main() {
  auto control_daemon_interface = ControlDaemonInterface<DummyWriter, DummyReader>();

  control_daemon_interface.read<float>("analog_gain",
                                       [](uint8_t status, float value) {
                                         printf("read analog_gain, status: %d, value: %f\n", status, value);
                                       });

  printf("\n");

  control_daemon_interface.handle_incoming();

  // char *x = (char*)malloc(10 * sizeof(char*));
  // free(x);
  // return x[5];

  /* uint8_t * input = " analog_gain\n"; */
  // uint8_t * input = "analog_gain";
  /* uint8_t * input = "analog_gain"; */

  // uint8_t rem = crc8(input);

  // printf("0x%x\n", rem);
}