/** 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;
}
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)");
}
/** 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;
}
