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 "edc.h"

#include "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;

  /* Check if callback was already registered for this type. */
  if (sbp_find_callback(s, msg_type) != 0)
    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;
}

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

sbp_msg_callbacks_node_t* sbp_find_callback(sbp_state_t *s, u16 msg_type)
{
  /* If our list is empty, return NULL. */
  if (!s->sbp_msg_callbacks_head)
    return 0;

  /* Traverse the linked list and return the callback
   * function pointer if we find a node with a matching
   * message id.
   */
  sbp_msg_callbacks_node_t *p = s->sbp_msg_callbacks_head;
  do
    if (p->msg_type == msg_type)
      return p;

  while ((p = p->next));

  /* Didn't find a matching callback, return NULL. */
  return 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, u32 (*read)(u8 *buff, u32 n, void *context))
{
  u8 temp;
  u16 crc;

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

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

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

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

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

  case GET_CRC:
    s->n_read += (*read)((u8*)&(s->crc) + s->n_read,
                         2-s->n_read, s->io_context);
    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. */
        sbp_msg_callbacks_node_t* node = sbp_find_callback(s, s->msg_type);
        if (node) {
          (*node->cb)(s->sender_id, s->msg_len, s->msg_buff, node->context);
          return SBP_OK_CALLBACK_EXECUTED;
        } else {
          return SBP_OK_CALLBACK_UNDEFINED;
        }
      } else
          return SBP_CRC_ERROR;
    }
    break;

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

  return SBP_OK;
}

s8 sbp_send_message(sbp_state_t *s, u16 msg_type, u16 sender_id, u8 len, u8 *payload,
                    u32 (*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 (write == 0)
    return SBP_NULL_ERROR;

  u16 crc;

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

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

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

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

  if (len > 0) {
    if ((*write)(payload, len, s->io_context) != 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);

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

  return SBP_OK;
}