Ejemplo n.º 1
0
/** Register a callback for a message type.
 * Register a callback that is called when a message
 * with type msg_type is received.
 *
 * \param msg_type Message type associated with callback
 * \param cb       Pointer to message callback function
 * \param context  Pointer to context for callback function
 * \param node     Statically allocated #sbp_msg_callbacks_node_t struct
 * \return `SBP_OK` (0) if successful, `SBP_CALLBACK_ERROR` if callback was
 *         already registered for that message type.
 */
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;
}
Ejemplo n.º 2
0
END_TEST

START_TEST(test_callbacks)
{

  sbp_state_t s;
  sbp_state_init(&s);

  /* Start with no callbacks registered.  */
  sbp_clear_callbacks(&s);

  fail_unless(sbp_find_callback(&s, 0x1234) == 0,
      "sbp_find_callback should return NULL if no callbacks registered");

  fail_unless(sbp_register_callback(&s, 0x2233, &test_callback, 0, 0) == SBP_NULL_ERROR,
      "sbp_register_callback should return an error if node is NULL");

  /* Add a first callback. */

  static sbp_msg_callbacks_node_t n;

  int NUMBER = 42;

  fail_unless(sbp_register_callback(&s, 0x2233, 0, 0, &n) == SBP_NULL_ERROR,
      "sbp_register_callback should return an error if cb is NULL");

  fail_unless(sbp_register_callback(&s, 0x2233, &test_callback, &NUMBER, &n) == SBP_OK,
      "sbp_register_callback should return success if everything is groovy");

  fail_unless(sbp_register_callback(&s, 0x2233, &test_callback, 0, &n)
        == SBP_CALLBACK_ERROR,
      "sbp_register_callback should return SBP_CALLBACK_ERROR if a callback "
      "of the same type is already registered");

  fail_unless(sbp_find_callback(&s, 0x1234) == 0,
      "sbp_find_callback should return NULL if callback not registered");

  fail_unless(sbp_find_callback(&s, 0x2233) == &n,
      "sbp_find_callback didn't return the correct callback node pointer");

  fail_unless(sbp_find_callback(&s, 0x2233)->context == &NUMBER,
      "sbp_find_callback didn't return the correct context pointer");

  /* Add a second callback. */

  static sbp_msg_callbacks_node_t m;

  int NUMBER2 = 84;

  fail_unless(sbp_register_callback(&s, 0x1234, &test_callback2, &NUMBER2, &m) == SBP_OK,
      "sbp_register_callback should return success if everything is groovy (2)");

  fail_unless(sbp_find_callback(&s, 0x2233) == &n,
      "sbp_find_callback didn't return the correct callback function pointer (2)");

  fail_unless(sbp_find_callback(&s, 0x2233)->context == &NUMBER,
      "sbp_find_callback didn't return the correct context pointer");

  fail_unless(sbp_find_callback(&s, 0x1234) == &m,
      "sbp_find_callback didn't return the correct callback function pointer (3)");

  fail_unless(sbp_find_callback(&s, 0x1234)->context == &NUMBER2,
      "sbp_find_callback didn't return the correct context pointer");

  fail_unless(sbp_register_callback(&s, 0x1234, &test_callback, 0, &n)
        == SBP_CALLBACK_ERROR,
      "sbp_register_callback should return SBP_CALLBACK_ERROR if a callback "
      "of the same type is already registered (2)");

  fail_unless(sbp_find_callback(&s, 0x7788) == 0,
      "sbp_find_callback should return NULL if callback not registered (2)");

  /* Clear all the registered callbacks and check they can no longer be found. */
  sbp_clear_callbacks(&s);

  fail_unless(sbp_find_callback(&s, 0x1234) == 0,
      "sbp_find_callback should return NULL if no callbacks registered (2)");

  fail_unless(sbp_find_callback(&s, 0x2233) == 0,
      "sbp_find_callback should return NULL if no callbacks registered (3)");

}
Ejemplo n.º 3
0
/** Read and process SBP messages.
 * Reads bytes from an input source using the provided `read` function, decodes
 * the SBP framing and performs a CRC check on the message.
 *
 * When an SBP message is successfully received then the list of callbacks is
 * searched for a callback corresponding to the received message type. If a
 * callback is found then it is called with the ID of the sender, the message
 * length and the message payload data buffer as arguments.
 *
 * \note sbp_process will always call `read` with n > 0
 *       (aka it will attempt to always read something)
 *
 * The supplied `read` function must have the prototype:
 *
 * ~~~
 * u32 read(u8 *buff, u32 n, void* context)
 * ~~~
 *
 * where `n` is the number of bytes requested and `buff` is the buffer into
 * which to write the received data, and `context` is the arbitrary pointer
 * set by `sbp_state_set_io_context`.
 * The function should return the number of
 * bytes successfully written into `buff` which may be between 0 and `n`
 * inclusive, but must never be greater than `n`.
 *
 * Note that `sbp_process` may not read all available bytes from the `read`
 * function so the caller should loop until all bytes available from the input
 * source have been consumed.
 *
 * \param s State structure
 * \param read Function pointer to a function that reads `n` bytes from the
 *             input source into `buff` and returns the number of bytes
 *             successfully read.
 * \return `SBP_OK` (0) if successful but no complete message yet,
 *         `SBP_OK_CALLBACK_EXECUTED` (1) if message decoded and callback executed,
 *         `SBP_OK_CALLBACK_UNDEFINED` (2) if message decoded with no associated
 *         callback, and `SBP_CRC_ERROR` (-2) if a CRC error
 *         has occurred. Thus can check for >0 to ensure good processing.
 */
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;
}