sbp.c

Go to the documentation of this file.

/*
 * Copyright (C) 2011-2014 Swift Navigation Inc.
 * Contact: Fergus Noble <fergus@swift-nav.com>
 *
 * This source is subject to the license found in the file 'LICENSE' which must
 * be be distributed together with this source. All other rights reserved.
 *
 * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND,
 * EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR PURPOSE.
 */

#include "libsbp/edc.h"
#include "libsbp/sbp.h"

#define SBP_PREAMBLE 0x55

s8 sbp_register_callback(sbp_state_t *s, u16 msg_type, sbp_msg_callback_t cb, void *context,
                         sbp_msg_callbacks_node_t *node)
{
  /* Check our callback function pointer isn't NULL. */
  if (cb == 0)
    return SBP_NULL_ERROR;

  /* Check our callback node pointer isn't NULL. */
  if (node == 0)
    return SBP_NULL_ERROR;

  for (sbp_msg_callbacks_node_t *n = s->sbp_msg_callbacks_head; n; n = n->next)
    if ((n == node) ||
        ((n->cb == cb) && (n->msg_type == msg_type) && (n->context == context)))
      return SBP_CALLBACK_ERROR;

  /* Fill in our new sbp_msg_callback_node_t. */
  node->msg_type = msg_type;
  node->cb = cb;
  node->context = context;
  /* The next pointer is set to NULL, i.e. this
   * will be the new end of the linked list.
   */
  node->next = 0;

  /* If our linked list is empty then just
   * add the new node to the start.
   */
  if (s->sbp_msg_callbacks_head == 0) {
    s->sbp_msg_callbacks_head = node;
    return SBP_OK;
  }

  /* Find the tail of our linked list and
   * add our new node to the end.
   */
  sbp_msg_callbacks_node_t *p = s->sbp_msg_callbacks_head;
  while (p->next)
    p = p->next;

  p->next = node;

  return SBP_OK;
}

s8 sbp_remove_callback(sbp_state_t *s, sbp_msg_callbacks_node_t *node)
{
  sbp_msg_callbacks_node_t *n;

  if (s->sbp_msg_callbacks_head == node) {
    s->sbp_msg_callbacks_head = node->next;
    return SBP_OK;
  } else {
    for (n = s->sbp_msg_callbacks_head; n; n = n->next) {
      if (n->next == node) {
        n->next = node->next;
        return SBP_OK;
      }
    }
  }
  return SBP_CALLBACK_ERROR;
}

void sbp_clear_callbacks(sbp_state_t *s)
{
  /* Reset the head of the callbacks list to NULL. */
  s->sbp_msg_callbacks_head = 0;
}

void sbp_state_init(sbp_state_t *s)
{
  s->state = WAITING;

  /* Set the IO context pointer, passed to read and write functions, to NULL. */
  s->io_context = 0;

  /* Clear the callbacks, if any, currently in s */
  sbp_clear_callbacks(s);
}


void sbp_state_set_io_context(sbp_state_t *s, void *context)
{
  s->io_context = context;
}

s8 sbp_process(sbp_state_t *s, s32 (*read)(u8 *buff, u32 n, void *context))
{
  /* Sanity checks */
  if ((0 == s) || (0 == read)) {
    return SBP_NULL_ERROR;
  }

  u8 temp;
  u16 crc;
  s32 rd = 0;

  switch (s->state) {
  case WAITING:
    rd = (*read)(&temp, 1, s->io_context);
    if (0 > rd) return SBP_READ_ERROR;
    if (1 == rd)
      if (temp == SBP_PREAMBLE) {
        s->n_read = 0;
        s->state = GET_TYPE;
      }
    break;

  case GET_TYPE:
    rd = (*read)((u8*)&(s->msg_type) + s->n_read, 2-s->n_read, s->io_context);
    if (0 > rd) return SBP_READ_ERROR;
    s->n_read += rd;
    if (s->n_read >= 2) {
      /* Swap bytes to little endian. */
      s->n_read = 0;
      s->state = GET_SENDER;
    }
    break;

  case GET_SENDER:
    rd = (*read)((u8*)&(s->sender_id) + s->n_read, 2-s->n_read, s->io_context);
    if (0 > rd) return SBP_READ_ERROR;
    s->n_read += rd;
    if (s->n_read >= 2) {
      /* Swap bytes to little endian. */
      s->state = GET_LEN;
    }
    break;

  case GET_LEN:
    rd = (*read)(&(s->msg_len), 1, s->io_context);
    if (0 > rd) return SBP_READ_ERROR;
    if (1 == rd) {
      s->n_read = 0;
      s->state = GET_MSG;
    }
    break;

  case GET_MSG:
    /* Not received whole message yet, try and read some more. */
    rd = (*read)(&(s->msg_buff[s->n_read]), s->msg_len - s->n_read, s->io_context);
    if (0 > rd) return SBP_READ_ERROR;
    s->n_read += rd;
    if (s->msg_len - s->n_read <= 0) {
      s->n_read = 0;
      s->state = GET_CRC;
    }
    break;

  case GET_CRC:
    rd = (*read)((u8*)&(s->crc) + s->n_read, 2-s->n_read, s->io_context);
    if (0 > rd) return SBP_READ_ERROR;
    s->n_read += rd;
    if (s->n_read >= 2) {
      s->state = WAITING;

      /* Swap bytes to little endian. */
      crc = crc16_ccitt((u8*)&(s->msg_type), 2, 0);
      crc = crc16_ccitt((u8*)&(s->sender_id), 2, crc);
      crc = crc16_ccitt(&(s->msg_len), 1, crc);
      crc = crc16_ccitt(s->msg_buff, s->msg_len, crc);
      if (s->crc == crc) {

        /* Message complete, process it. */
        s8 ret = sbp_process_payload(s, s->sender_id, s->msg_type, s->msg_len,
                                     s->msg_buff);
        return ret;
      } else {
        return SBP_CRC_ERROR;
      }
    }
    break;

  default:
    s->state = WAITING;
    break;
  }

  return SBP_OK;
}

s8 sbp_process_payload(sbp_state_t *s, u16 sender_id, u16 msg_type, u8 msg_len,
                       u8 payload[]) {
  s8 ret = SBP_OK_CALLBACK_UNDEFINED;
  sbp_msg_callbacks_node_t *node;
  for (node = s->sbp_msg_callbacks_head; node; node = node->next) {
    if (node->msg_type == msg_type) {
      (*node->cb)(sender_id, msg_len, payload, node->context);
      ret = SBP_OK_CALLBACK_EXECUTED;
    }
  }
  return ret;
}


s8 sbp_send_message(sbp_state_t *s, u16 msg_type, u16 sender_id, u8 len, u8 *payload,
                    s32 (*write)(u8 *buff, u32 n, void *context))
{
  /* Check our payload data pointer isn't NULL unless len = 0. */
  if (len != 0 && payload == 0)
    return SBP_NULL_ERROR;

  /* Check our write function pointer isn't NULL. */
  if ((0 == s) || (0 == write))
    return SBP_NULL_ERROR;

  u16 crc;
  s32 wr = 0;

  u8 preamble = SBP_PREAMBLE;
  wr = (*write)(&preamble, 1, s->io_context);
  if (0 > wr) return SBP_WRITE_ERROR;
  if (wr != 1) return SBP_SEND_ERROR;

  wr = (*write)((u8*)&msg_type, 2, s->io_context);
  if (0 > wr) return SBP_WRITE_ERROR;
  if (wr != 2) return SBP_SEND_ERROR;

  wr = (*write)((u8*)&sender_id, 2, s->io_context);
  if (0 > wr) return SBP_WRITE_ERROR;
  if (wr != 2) return SBP_SEND_ERROR;

  wr = (*write)(&len, 1, s->io_context);
  if (0 > wr) return SBP_WRITE_ERROR;
  if (wr != 1) return SBP_SEND_ERROR;

  if (len > 0) {
    wr = (*write)(payload, len, s->io_context);
    if (0 > wr) return SBP_WRITE_ERROR;
    if (wr != len) return SBP_SEND_ERROR;
  }

  crc = crc16_ccitt((u8*)&(msg_type), 2, 0);
  crc = crc16_ccitt((u8*)&(sender_id), 2, crc);
  crc = crc16_ccitt(&(len), 1, crc);
  crc = crc16_ccitt(payload, len, crc);

  wr = (*write)((u8*)&crc, 2, s->io_context);
  if (0 > wr) return SBP_WRITE_ERROR;
  if (wr != 2) return SBP_SEND_ERROR;

  return SBP_OK;
}