From 3e238ca49bbd49a827c0a68c580a1f19c7319708 Mon Sep 17 00:00:00 2001 From: dzollo Date: Mon, 16 Sep 2019 09:53:20 -0700 Subject: [PATCH 1/5] c_frame_callback: macros for frame callback (sbp.h) --- c/include/libsbp/sbp.h | 30 +++++++++++++++++++++++++++++- 1 file changed, 29 insertions(+), 1 deletion(-) diff --git a/c/include/libsbp/sbp.h b/c/include/libsbp/sbp.h index 75db2327b8..4d04d662b0 100644 --- a/c/include/libsbp/sbp.h +++ b/c/include/libsbp/sbp.h @@ -43,8 +43,36 @@ extern "C" { /** Default sender ID. Intended for messages sent from the host to the device. */ #define SBP_SENDER_ID 0x42 +/** Header length in bytes. */ +#define SBP_HEADER_LEN 6 +/** CRC length in bytes. */ +#define SBP_CRC_LEN 2 +/** Max payload length in bytes. */ +#define SBP_MAX_PAYLOAD_LEN (UINT8_MAX) +/** Max frame length in bytes. */ +#define SBP_MAX_FRAME_LEN (SBP_HEADER_LEN + SBP_MAX_PAYLOAD_LEN + SBP_CRC_LEN) -/** SBP callback function prototype definition. */ +/** Frame offset for the preamble. */ +#define SBP_FRAME_OFFSET_PREAMBLE (0u) +/** Frame offset for the preamble. */ +#define SBP_FRAME_OFFSET_MSGTYPE (SBP_FRAME_OFFSET_PREAMBLE + sizeof(u8)) +/** Frame offset for the preamble. */ +#define SBP_FRAME_OFFSET_SENDERID (SBP_FRAME_OFFSET_MSGTYPE + sizeof(u16)) +/** Frame offset for the preamble. */ +#define SBP_FRAME_OFFSET_MSGLEN (SBP_FRAME_OFFSET_SENDERID + sizeof(u16)) +/** Frame offset for the preamble. */ +#define SBP_FRAME_OFFSET_MSG (SBP_FRAME_OFFSET_MSGLEN + sizeof(u8)) +/** Frame offset for the CRC is a function of msg length. */ +#define SBP_FRAME_OFFSET_CRC(msg_len) (SBP_FRAME_OFFSET_MSG + msg_len) +#define SBP_FRAME_CALC_LEN(msg_len) (SBP_HEADER_LEN + msg_len + SBP_CRC_LEN) + +/** Get message payload pointer from frame */ +#define SBP_FRAME_MSG_PAYLOAD(frame_ptr) (&(frame_ptr[SBP_FRAME_OFFSET_MSG])) + +/** SBP_MSG_ID to use to register frame callback for ALL messages. */ +#define SBP_MSG_ALL 0 + +/** SBP callback function prototype definitions. */ typedef void (*sbp_msg_callback_t)(u16 sender_id, u8 len, u8 msg[], void *context); /** SBP callback node. From f1e0f282c226f839bc1f5be818e68969aa539290 Mon Sep 17 00:00:00 2001 From: dzollo Date: Mon, 16 Sep 2019 09:53:46 -0700 Subject: [PATCH 2/5] c_frame_callback: whitespace changes to sbp.h --- c/include/libsbp/sbp.h | 19 ++++++++++--------- 1 file changed, 10 insertions(+), 9 deletions(-) diff --git a/c/include/libsbp/sbp.h b/c/include/libsbp/sbp.h index 4d04d662b0..2f28f637c3 100644 --- a/c/include/libsbp/sbp.h +++ b/c/include/libsbp/sbp.h @@ -23,23 +23,23 @@ extern "C" { * \{ */ /** Return value indicating success. */ -#define SBP_OK 0 +#define SBP_OK 0 /** Return value indicating message decoded and callback executed by sbp_process. */ -#define SBP_OK_CALLBACK_EXECUTED 1 +#define SBP_OK_CALLBACK_EXECUTED 1 /** Return value indicating message decoded with no associated callback in sbp_process. */ #define SBP_OK_CALLBACK_UNDEFINED 2 /** Return value indicating an error with the callback (function defined). */ -#define SBP_CALLBACK_ERROR -1 +#define SBP_CALLBACK_ERROR -1 /** Return value indicating a CRC error. */ -#define SBP_CRC_ERROR -2 +#define SBP_CRC_ERROR -2 /** Return value indicating an error occured whilst sending an SBP message. */ -#define SBP_SEND_ERROR -3 +#define SBP_SEND_ERROR -3 /** Return value indicating an error occured because an argument was NULL. */ -#define SBP_NULL_ERROR -4 +#define SBP_NULL_ERROR -4 /** Return value indicating an error occured in the write() operation. */ -#define SBP_WRITE_ERROR -5 +#define SBP_WRITE_ERROR -5 /** Return value indicating an error occured in the read() operation. */ -#define SBP_READ_ERROR -6 +#define SBP_READ_ERROR -6 /** Default sender ID. Intended for messages sent from the host to the device. */ #define SBP_SENDER_ID 0x42 @@ -77,7 +77,8 @@ typedef void (*sbp_msg_callback_t)(u16 sender_id, u8 len, u8 msg[], void *contex /** SBP callback node. * Forms a linked list of callbacks. - * \note Must be statically allocated for use with sbp_register_callback(). + * \note Must be statically allocated for use with sbp_register_callback() + * and sbp_register_frame_callback(). */ typedef struct sbp_msg_callbacks_node { u16 msg_type; /**< Message ID associated with callback. */ From 48ce9b3be2518d8be4b3773383f30d617179c841 Mon Sep 17 00:00:00 2001 From: dzollo Date: Mon, 16 Sep 2019 09:52:36 -0700 Subject: [PATCH 3/5] c_frame_callback: add frame callback api to c lib * classify callbacks into SBP_FRAME_CALLBACK(new) and SBP_PAYLOAD_CALLBACK (original) * add API for an ALL message cb to c lib * add macro for frame length calculation * c_frame_callback: add sbp_u8_array_to_u16 for sbp_process. --- c/include/libsbp/sbp.h | 33 +++++- c/src/sbp.c | 230 ++++++++++++++++++++++++++++++++++------- 2 files changed, 222 insertions(+), 41 deletions(-) diff --git a/c/include/libsbp/sbp.h b/c/include/libsbp/sbp.h index 2f28f637c3..e7eef94d28 100644 --- a/c/include/libsbp/sbp.h +++ b/c/include/libsbp/sbp.h @@ -74,7 +74,27 @@ extern "C" { /** SBP callback function prototype definitions. */ typedef void (*sbp_msg_callback_t)(u16 sender_id, u8 len, u8 msg[], void *context); +typedef void (*sbp_frame_callback_t)(u16 sender_id, u16 msg_type, + u8 payload_len, u8 payload[], + u16 frame_len, u8 frame[], void *context); + +/** SBP callback type enum: + * SBP_PAYLOAD_CALLBACK are the original callbacks in libsbp without framing args + * SBP_FRAME_CALLBACK are raw frame callbacks that include framing data as args. + * This enum is stored on each sbp_msg_callback_node struct to identify how + * to cast the callback function pointer stored within. + */ +enum sbp_cb_type { + SBP_PAYLOAD_CALLBACK = 0, + SBP_FRAME_CALLBACK = 1, + SBP_CALLBACK_TYPE_COUNT = 2, +}; + +#define SBP_CALLBACK_FLAG(cb_type) (1u << (cb_type)) +#define SBP_CALLBACK_ALL_MASK \ + ((SBP_CALLBACK_FLAG(SBP_CALLBACK_TYPE_COUNT)) - 1) +typedef enum sbp_cb_type sbp_cb_type; /** SBP callback node. * Forms a linked list of callbacks. * \note Must be statically allocated for use with sbp_register_callback() @@ -82,9 +102,10 @@ typedef void (*sbp_msg_callback_t)(u16 sender_id, u8 len, u8 msg[], void *contex */ typedef struct sbp_msg_callbacks_node { u16 msg_type; /**< Message ID associated with callback. */ - sbp_msg_callback_t cb; /**< Pointer to callback function. */ + void* cb; /**< Pointer to callback function. */ void *context; /**< Pointer to a context */ struct sbp_msg_callbacks_node *next; /**< Pointer to next node in list. */ + sbp_cb_type cb_type; /**< Enum that holds the type of callback. */ } sbp_msg_callbacks_node_t; /** State structure for processing SBP messages. */ @@ -101,8 +122,9 @@ typedef struct { u16 sender_id; u16 crc; u8 msg_len; + u16 frame_len; u8 n_read; - u8 msg_buff[256]; + u8 frame_buff[SBP_MAX_FRAME_LEN]; void* io_context; sbp_msg_callbacks_node_t* sbp_msg_callbacks_head; } sbp_state_t; @@ -111,6 +133,11 @@ typedef struct { s8 sbp_register_callback(sbp_state_t* s, u16 msg_type, sbp_msg_callback_t cb, void* context, sbp_msg_callbacks_node_t *node); +s8 sbp_register_frame_callback(sbp_state_t* s, u16 msg_type, + sbp_frame_callback_t cb, void* context, + sbp_msg_callbacks_node_t *node); +s8 sbp_register_all_msg_callback(sbp_state_t *s, sbp_frame_callback_t cb, + void *context, sbp_msg_callbacks_node_t *node); s8 sbp_remove_callback(sbp_state_t *s, sbp_msg_callbacks_node_t *node); void sbp_clear_callbacks(sbp_state_t* s); void sbp_state_init(sbp_state_t *s); @@ -118,6 +145,8 @@ void sbp_state_set_io_context(sbp_state_t *s, void* context); s8 sbp_process(sbp_state_t *s, s32 (*read)(u8 *buff, u32 n, void* context)); s8 sbp_process_payload(sbp_state_t *s, u16 sender_id, u16 msg_type, u8 msg_len, u8 payload[]); +s8 sbp_process_frame(sbp_state_t *s, u16 sender_id, u16 msg_type, + u8 payload_len, u8 payload[], u16 frame_len, u8 frame[], u8 cb_mask); 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)); diff --git a/c/src/sbp.c b/c/src/sbp.c index 3e2efb068b..36a0630717 100644 --- a/c/src/sbp.c +++ b/c/src/sbp.c @@ -158,9 +158,10 @@ * \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) -{ +static s8 sbp_register_callback_generic(sbp_state_t *s, u16 msg_type, + void* cb, sbp_cb_type cb_type, + void *context, + sbp_msg_callbacks_node_t *node) { /* Check our callback function pointer isn't NULL. */ if (cb == 0) return SBP_NULL_ERROR; @@ -171,13 +172,15 @@ s8 sbp_register_callback(sbp_state_t *s, u16 msg_type, sbp_msg_callback_t cb, vo 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))) + ((n->cb == cb) && (n->msg_type == msg_type) && + (n->context == context) && n->cb_type == cb_type)) return SBP_CALLBACK_ERROR; /* Fill in our new sbp_msg_callback_node_t. */ node->msg_type = msg_type; node->cb = cb; node->context = context; + node->cb_type = cb_type; /* The next pointer is set to NULL, i.e. this * will be the new end of the linked list. */ @@ -227,6 +230,61 @@ s8 sbp_remove_callback(sbp_state_t *s, sbp_msg_callbacks_node_t *node) return SBP_CALLBACK_ERROR; } + /** Register a frame callback for a msg_type. + * + * \param s Pointer to sbp_state + * \param cb Pointer to message callback function + * \param msg_type Message type on which to fire frame callback, + * SBP_MSG_ALL will fire for every message + * \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_NULL_ERROR` on usage errors, + * `SBP_CALLBACK_ERROR` if the if callback was already + * registered for that message type. + */ +s8 sbp_register_frame_callback(sbp_state_t *s, u16 msg_type, + sbp_frame_callback_t cb, void *context, + sbp_msg_callbacks_node_t *node) { + return sbp_register_callback_generic(s, msg_type, cb, SBP_FRAME_CALLBACK, context, node); +} + +/** Register a frame callback for ANY message. + * + * \param s Pointer to sbp_state + * \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_NULL_ERROR` if a usage error, + * `SBP_CALLBACK_ERROR` if the node already exists + */ + +s8 sbp_register_all_msg_callback(sbp_state_t *s, sbp_frame_callback_t cb, + void *context, + sbp_msg_callbacks_node_t *node) { + return sbp_register_frame_callback(s, SBP_MSG_ALL, cb, context, node); +} + +/** Register a payload callback for a message type. + * + * Register a payload callback that is called when a message + * with type msg_type is received. Note, this might better + * be called sbp_register_payload_callback, but is left + * as sbp_register_callback for backwards compatibility. + * + * \param s Pointer to sbp_state + * \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_NULL_ERROR` on usage errors, + * `SBP_CALLBACK_ERROR` if the 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) { + return sbp_register_callback_generic(s, msg_type, cb, SBP_PAYLOAD_CALLBACK, context, node); +} + /** Clear all registered callbacks. * This is probably only useful for testing but who knows! */ @@ -266,6 +324,52 @@ void sbp_state_set_io_context(sbp_state_t *s, void *context) s->io_context = context; } +/** Helper function to fill frame buffer. Uses the `read` function and its + * context to read into the `frame_buff` stored on the sbp_state. + * Increments the n_read counter (number of bytes read for current read + * operation) and the `frame_len` counter (total number of bytes read into + * frame_buff), both stored on the sbp_state. + * \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. + * \param to_read number of bytes to read. + * \return `SBP_OK` (0) if successful, + * `SBP_READ_ERROR` (-6) if no bytes could be read or read function + * returned error code. + */ +static s8 sbp_state_read_to_frame_buffer(sbp_state_t *s, + s32 (*read)(u8 *buff, u32 n, void *context), + u8 to_read) +{ + u8 rd = (*read)(s->frame_buff + s->frame_len, to_read, s->io_context); + if (0 > rd) return SBP_READ_ERROR; + s->frame_len += rd; + s->n_read += rd; + return SBP_OK; +} + +/** Reset frame buffer on the sbp_state to zeros and reset frame length counter. + */ +static void sbp_state_frame_buffer_clear(sbp_state_t *s) +{ + /* Note, library functions are not used to avoid more dependencies. */ + for (int i = 0; i < sizeof(s->frame_buff); i++) { + s->frame_buff[i] = 0; + } + s->frame_len = 0; +} + + +/** Helper to convert a array of 2 bytes in network byte order + * to the platform's representation of a u16 without + * needing to use the word endian + */ +static u16 sbp_u8_array_to_u16(u8 *array_start) +{ + return (u16) array_start[0] + ((u16) array_start[1] << 8); +} + /** 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. @@ -315,43 +419,47 @@ s8 sbp_process(sbp_state_t *s, s32 (*read)(u8 *buff, u32 n, void *context)) u8 temp; u16 crc; s32 rd = 0; + s8 ret = SBP_OK; switch (s->state) { case WAITING: - rd = (*read)(&temp, 1, s->io_context); + rd = (*read)(&temp, sizeof(temp), s->io_context); if (0 > rd) return SBP_READ_ERROR; - if (1 == rd) + if (sizeof(temp) == rd) if (temp == SBP_PREAMBLE) { + /* set frame_buff and n_read to 0 after each preamble. */ + sbp_state_frame_buffer_clear(s); + s->frame_buff[s->frame_len++] = temp; 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. */ + ret = sbp_state_read_to_frame_buffer(s, read, sizeof(s->msg_type)-s->n_read); + if (ret != SBP_OK) return ret; + if (s->n_read >= sizeof(s->msg_type)) { + s->msg_type = sbp_u8_array_to_u16(&(s->frame_buff[SBP_FRAME_OFFSET_MSGTYPE])); 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. */ + ret = sbp_state_read_to_frame_buffer(s, read, sizeof(s->sender_id)-s->n_read); + if (ret != SBP_OK) return ret; + if (s->n_read >= sizeof(s->sender_id)) { + s->sender_id = sbp_u8_array_to_u16(&(s->frame_buff[SBP_FRAME_OFFSET_SENDERID])); + s->n_read = 0; 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) { + ret = sbp_state_read_to_frame_buffer(s, read, sizeof(s->msg_len)-s->n_read); + if (ret != SBP_OK) return ret; + if (s->n_read == sizeof(s->msg_len)) { + s->msg_len = s->frame_buff[SBP_FRAME_OFFSET_MSGLEN]; s->n_read = 0; s->state = GET_MSG; } @@ -359,9 +467,8 @@ s8 sbp_process(sbp_state_t *s, s32 (*read)(u8 *buff, u32 n, void *context)) 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; + ret = sbp_state_read_to_frame_buffer(s, read, s->msg_len - s->n_read); + if (ret != SBP_OK) return ret; if (s->msg_len - s->n_read <= 0) { s->n_read = 0; s->state = GET_CRC; @@ -369,22 +476,20 @@ s8 sbp_process(sbp_state_t *s, s32 (*read)(u8 *buff, u32 n, void *context)) 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) { + ret = sbp_state_read_to_frame_buffer(s, read, SBP_CRC_LEN - s->n_read); + if (ret != SBP_OK) return ret; + if (s->n_read >= SBP_CRC_LEN) { 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); + s->crc = sbp_u8_array_to_u16(&(s->frame_buff[SBP_FRAME_OFFSET_CRC(s->msg_len)])); + crc = crc16_ccitt((u8*)&(s->msg_type), sizeof(s->msg_type), 0); + crc = crc16_ccitt((u8*)&(s->sender_id), sizeof(s->sender_id), crc); + crc = crc16_ccitt(&(s->msg_len), sizeof(s->msg_len), crc); + crc = crc16_ccitt(SBP_FRAME_MSG_PAYLOAD(s->frame_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); + /* Message complete, process frame callbacks and payload callbacks. */ + ret = sbp_process_frame(s, s->sender_id, s->msg_type, + s->msg_len, SBP_FRAME_MSG_PAYLOAD(s->frame_buff), + s->frame_len, s->frame_buff, SBP_CALLBACK_ALL_MASK); return ret; } else { return SBP_CRC_ERROR; @@ -415,12 +520,59 @@ s8 sbp_process(sbp_state_t *s, s32 (*read)(u8 *buff, u32 n, void *context)) */ s8 sbp_process_payload(sbp_state_t *s, u16 sender_id, u16 msg_type, u8 msg_len, u8 payload[]) { + return sbp_process_frame(s, sender_id, msg_type, msg_len, payload, + 0, 0, SBP_CALLBACK_FLAG(SBP_PAYLOAD_CALLBACK)); +} + + +/** Directly process an SBP frame. + * Use this function to directly process the entire SBP frame after + * it is succesfully deframed and the CRC has passed. It will fire any + * callbacks registered for the message which match the cb_mask. + * + * \param s State structure + * \param sender_id SBP message sender id + * \param msg_type SBP message type. Type SBP_MSG_ALL will fire for all messages. + * \param payload_len SBP message length + * \param payload SBP Message payload + * \param frame_len Length of ENTIRE frame (from header to CRC). Max of 263. + * \param frame Pointer to the entire SBP frame (stored on state struct) + * \param cb_mask Bitmask defining which callbacks to include/exclude from + * processing. Use SBP_CALLBACK_ALL_MASK for all callback + * types or construct custom mask using + * SBP_CALLBACK_FLAG(cb_type). + * \return `SBP_OK_CALLBACK_EXECUTED` (1) if message decoded and callback executed + * SBP_OK_CALLBACK_UNDEFINED` (2) if message decoded with no + * associated callback. + */ +s8 sbp_process_frame(sbp_state_t *s, u16 sender_id, u16 msg_type, + u8 payload_len, u8 payload[], + u16 frame_len, u8 frame[], + u8 cb_mask) { 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; + if ((SBP_CALLBACK_FLAG(node->cb_type) & cb_mask) && + ((node->msg_type == msg_type) || (node->msg_type == SBP_MSG_ALL))) { + switch (node->cb_type) { + case SBP_FRAME_CALLBACK: + { + ((sbp_frame_callback_t)(node->cb))(sender_id, msg_type, payload_len, + payload, frame_len, frame, + node->context); + ret = SBP_OK_CALLBACK_EXECUTED; + } break; + case SBP_PAYLOAD_CALLBACK: + { + ((sbp_msg_callback_t)(node->cb))(sender_id, payload_len, payload, + node->context); + ret = SBP_OK_CALLBACK_EXECUTED; + } break; + default: + { + // NOP + }; + } } } return ret; From 27ec7b817c491a9d12f6d4aed70d2c1402927d4d Mon Sep 17 00:00:00 2001 From: dzollo Date: Mon, 16 Sep 2019 09:54:31 -0700 Subject: [PATCH 4/5] c_frame_callback: update existing docs (sbp.c) --- c/src/sbp.c | 48 +++++++++++++++++++++++++++++++++--------------- 1 file changed, 33 insertions(+), 15 deletions(-) diff --git a/c/src/sbp.c b/c/src/sbp.c index 36a0630717..9e28b4c47c 100644 --- a/c/src/sbp.c +++ b/c/src/sbp.c @@ -28,7 +28,8 @@ * --------- * * First setup a callback for the message you will be receiving. Our callback - * function must have type #sbp_msg_callback_t, i.e. it must be of the form: + * function must have type #sbp_msg_callback_t or #sbp_msg_frame_callback_t, + * i.e. it must be of the form: * * ~~~ * void my_callback(u16 sender_id, u8 len, u8 msg[], void *context) @@ -36,6 +37,14 @@ * // Process msg. * } * ~~~ + * or + * ~~~ + * void my_frame_callback(u16 sender_id, u16 msg_type, u8 payload_len, + * u8 payload[], u16 frame_len, u8 frame[], void *context) + * { + * // Process msg. + * } + * ~~~ * * You must also statically allocate a #sbp_msg_callbacks_node_t that will be * used to keep track of the callback function. You do not need to initialize @@ -50,6 +59,11 @@ * ~~~ * sbp_register_callback(&sbp_state, SBP_MY_MSG_TYPE, &my_callback, &context, &my_callback_node); * ~~~ + * or + * ~~~ + * sbp_register_frame_callback(&sbp_state, SBP_MY_MSG_TYPE, &my_frame_callback, + * &context, &my_callback_node); + * ~~~ * * where `SBP_MY_MSG_TYPE` is the numerical identifier of your message type. * @@ -147,16 +161,20 @@ * * \{ */ -/** Register a callback for a message type. - * Register a callback that is called when a message - * with type msg_type is received. +/** Register a callback for a particular msg_type, specifying the cb_type. * - * \param msg_type Message type associated with callback + * \param s Pointer to sbp_state + * \param msg_type Message type on which to fire callback. + * SBP_MSG_ALL will fire for every message, but only + * for callbacks of type SBP_FRAME_CALLBACK. * \param cb Pointer to message callback function + * \param cb_type sbp_cb_type indicating what kind of cb is in use. + * (e.g SBP_PAYLOAD_CALLBACK or SBP_FRAME_CALLBACK) * \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. + * \return `SBP_OK` (0) if successful, `SBP_NULL_ERROR` on usage errors, + * `SBP_CALLBACK_ERROR` if the if callback was already + * registered for that message type. */ static s8 sbp_register_callback_generic(sbp_state_t *s, u16 msg_type, void* cb, sbp_cb_type cb_type, @@ -376,8 +394,8 @@ static u16 sbp_u8_array_to_u16(u8 *array_start) * * 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. + * callback is found then it is called with its respective arguments depending + * on its cb_type. * * \note sbp_process will always call `read` with n > 0 * (aka it will attempt to always read something) @@ -399,7 +417,7 @@ static u16 sbp_u8_array_to_u16(u8 *array_start) * function so the caller should loop until all bytes available from the input * source have been consumed. * - * \param s State structure + * \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. @@ -505,15 +523,15 @@ s8 sbp_process(sbp_state_t *s, s32 (*read)(u8 *buff, u32 n, void *context)) return SBP_OK; } -/** Directly process a SBP message. +/** Directly process an SBP message. * If a SBP message has already been decoded (for example, from a binary * stream or from a JSON log file) use this function to directly process it. * - * \param s State structure + * \param s State structure * \param sender_id SBP message sender id - * \param msg_type SBP message type - * \param msg_len SBP message length - * \param payload SBP message payload + * \param msg_type SBP message type + * \param msg_len SBP message length + * \param payload SBP message payload * \return `SBP_OK_CALLBACK_EXECUTED` (1) if message decoded and callback executed, * `SBP_OK_CALLBACK_UNDEFINED` (2) if message decoded with no associated * callback. From 7b9ac446077c232800b183b8906bd2f210a1756a Mon Sep 17 00:00:00 2001 From: dzollo Date: Fri, 6 Sep 2019 15:02:39 -0700 Subject: [PATCH 5/5] c_frame_callback: add frame_callback unit tests * c_frame_callback: Add unit tests for all_msg callback api * c_frame_callback: Add unit test for max length (255) to c lib --- c/test/check_sbp.c | 367 +++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 367 insertions(+) diff --git a/c/test/check_sbp.c b/c/test/check_sbp.c index ae504cb25c..de40e75edf 100644 --- a/c/test/check_sbp.c +++ b/c/test/check_sbp.c @@ -83,11 +83,28 @@ u8 last_msg[256]; void* last_context; +u32 n_frame_callbacks_logged; +u16 last_frame_sender_id; +u16 last_frame_msg_type; +u8 last_frame_payload_len; +u16 last_frame_len; +u8 last_frame[256 + 8]; +void* last_frame_context; + void logging_reset(void) { n_callbacks_logged = 0; + n_frame_callbacks_logged = 0; last_context = 0; + last_sender_id = 0; + last_len = 0; + last_frame_context = 0; + last_frame_sender_id = 0; + last_frame_len = 0; + last_frame_sender_id = 0; + last_frame_msg_type = 0; memset(last_msg, 0, sizeof(last_msg)); + memset(last_frame, 0, sizeof(last_frame)); } void logging_callback(u16 sender_id, u8 len, u8 msg[], void* context) @@ -101,6 +118,20 @@ void logging_callback(u16 sender_id, u8 len, u8 msg[], void* context) /*printy_callback(sender_id, len, msg);*/ } +void frame_logging_callback(u16 sender_id, u16 msg_type, u8 payload_len, + u8 payload[], u16 frame_len, u8 frame[], + void *context) { + n_frame_callbacks_logged++; + last_frame_sender_id = sender_id; + last_frame_msg_type = msg_type; + last_frame_payload_len = payload_len; + last_frame_context = context; + last_frame_len = frame_len; + memcpy(last_frame, frame, frame_len); + + /*printy_callback(sender_id, len, msg);*/ +} + void test_callback(u16 sender_id, u8 len, u8 msg[], void* context) { /* Do nothing. */ @@ -118,6 +149,34 @@ void test_callback2(u16 sender_id, u8 len, u8 msg[], void* context) (void)context; } +void test_frame_callback(u16 sender_id, u16 msg_type, u8 payload_len, + u8 payload[], u16 frame_len, u8 frame[], + void *context) +{ + /* Do nothing. */ + (void)sender_id; + (void)msg_type; + (void)payload_len; + (void)payload; + (void)frame_len; + (void)frame; + (void)context; +} + +void test_frame_callback2(u16 sender_id, u16 msg_type, u8 payload_len, + u8 payload[], u16 frame_len, u8 frame[], + void *context) +{ + /* Do nothing. */ + (void)sender_id; + (void)msg_type; + (void)payload_len; + (void)payload; + (void)frame_len; + (void)frame; + (void)context; +} + sbp_msg_callbacks_node_t* sbp_find_callback(sbp_state_t *s, u16 msg_type) { /* If our list is empty, return NULL. */ @@ -328,6 +387,227 @@ START_TEST(test_sbp_process) } END_TEST +START_TEST(test_sbp_frame) +{ + /* TODO: Tests with different read function behaviour. */ + + sbp_state_t s; + sbp_state_init(&s); + sbp_state_set_io_context(&s, &DUMMY_MEMORY_FOR_IO); + + static sbp_msg_callbacks_node_t n; + static sbp_msg_callbacks_node_t n2; + + sbp_register_frame_callback(&s, 0x2269, &frame_logging_callback, &DUMMY_MEMORY_FOR_CALLBACKS, &n); + + u8 test_data[] = { 0x01, 0x02, 0x03, 0x04 }; + u8 test_frame[] = { 0x55, 0x69, 0x22, 0x42, 0x00, 0x04, 0x01, 0x02, 0x03, 0x04, 0x3D, 0xF7 }; + + dummy_reset(); + sbp_send_message(&s, 0x2269, 0x42, sizeof(test_data), test_data, &dummy_write); + + while (dummy_rd < dummy_wr) { + fail_unless(sbp_process(&s, &dummy_read) >= SBP_OK, + "sbp_process threw an error!"); + } + + fail_unless(n_frame_callbacks_logged == 1, + "one frame callback should have been logged"); + fail_unless(last_frame_sender_id == 0x42, + "sender_id decoded incorrectly"); + fail_unless(last_frame_payload_len == sizeof(test_data), + "len decoded incorrectly"); + fail_unless(last_frame_len == sizeof(test_data) + 8, + "frame len decoded incorrectly"); + fail_unless(last_frame_msg_type == 0x2269, + "msg_type decoded incorrectly"); + char test[1024]; + char* ptr = test; + for (int i = 0; i < last_frame_len; i++) { + ptr += sprintf(ptr, "%02X", last_frame[i]); + } + fail_unless(memcmp(last_frame, test_frame, sizeof(test_frame)-1) + == 0, + "decoded incorrectly %s", test); + fail_unless(last_frame_context == &DUMMY_MEMORY_FOR_CALLBACKS, + "context pointer incorrectly passed"); + + sbp_register_frame_callback(&s, 0x2270, &frame_logging_callback, &DUMMY_MEMORY_FOR_CALLBACKS, &n2); + fail_unless(sbp_find_callback(&s, 0x2270) != 0, + "second callback not found"); + + sbp_remove_callback(&s, &n2); + fail_unless(sbp_find_callback(&s, 0x2270) == 0, + "callback not removed"); + + /* Test sbp_process with both a frame callback and a not frame callback */ + + static sbp_msg_callbacks_node_t n3; + static sbp_msg_callbacks_node_t n4; + + logging_reset(); + dummy_reset(); + sbp_clear_callbacks(&s); + + static sbp_msg_callbacks_node_t q; + sbp_register_callback(&s, 0x2269, &logging_callback, 0, &n3); + sbp_register_frame_callback(&s, 0x2269, &frame_logging_callback, &DUMMY_MEMORY_FOR_CALLBACKS, &n4); + sbp_send_message(&s, 0x2269, 0x42, sizeof(test_data), test_data, &dummy_write); + + while (dummy_rd < dummy_wr) { + fail_unless(sbp_process(&s, &dummy_read_single_byte) >= SBP_OK, + "sbp_process threw an error! (3)"); + } + + fail_unless(n_callbacks_logged == 1, + "one regular callback should have been logged (3)"); + fail_unless(n_frame_callbacks_logged == 1, + "one frame callback should have been logged (3)"); + + /* now remove frame callback and make sure that the regular callback is still there */ + sbp_remove_callback(&s, &n4); + dummy_reset(); + sbp_send_message(&s, 0x2269, 0x42, sizeof(test_data), test_data, &dummy_write); + while (dummy_rd < dummy_wr) { + fail_unless(sbp_process(&s, &dummy_read_single_byte) >= SBP_OK, + "sbp_process threw an error! (3)"); + } + + fail_unless(n_callbacks_logged == 2, + "two regular callback should have been logged (3)"); + + /* now test that no frame callback with bad msg and direct writing to buffer */ + u8 awesome_message[] = {0x55, 0x33, 0x22, 0x77, 0x66, + 0x02, 0x22, 0x33, 0x8A, 0x33}; + logging_reset(); + dummy_reset(); + dummy_rd = 0; + dummy_wr = sizeof(awesome_message); + memcpy(dummy_buff, awesome_message, sizeof(awesome_message)); + + static sbp_msg_callbacks_node_t m; + sbp_register_frame_callback(&s, 0x2233, &frame_logging_callback, 0, &m); + + while (dummy_rd < dummy_wr) { + fail_unless(sbp_process(&s, &dummy_read) >= SBP_OK, + "sbp_process threw an error! (3)"); + } + + fail_unless(n_frame_callbacks_logged == 1, + "one callback should have been logged (3)"); + fail_unless(last_frame_sender_id == 0x6677, + "sender_id decoded incorrectly (3)"); + fail_unless(last_frame_payload_len == 2, + "len decoded incorrectly (3)"); + fail_unless(memcmp(last_frame, awesome_message, sizeof(awesome_message)) + == 0, + "test data decoded incorrectly (3) %x", last_frame[sizeof(awesome_message)-1]); + + awesome_message[4] = 0xAA; + logging_reset(); + dummy_reset(); + dummy_rd = 0; + dummy_wr = sizeof(awesome_message); + memcpy(dummy_buff, awesome_message, sizeof(awesome_message)); + + s8 ret = 0; + while (dummy_rd < dummy_wr) { + ret |= sbp_process(&s, &dummy_read); + } + + fail_unless(ret == SBP_CRC_ERROR, + "sbp_process should have returned SBP_CRC_ERROR " + "for malformed message"); + + fail_unless(n_frame_callbacks_logged == 0, + "no frame callbacks should have been logged (2)"); + +} +END_TEST + +START_TEST(test_sbp_all_msg) +{ + /* Tests registering for all messages */ + + sbp_state_t s; + sbp_state_init(&s); + sbp_state_set_io_context(&s, &DUMMY_MEMORY_FOR_IO); + + static sbp_msg_callbacks_node_t n; + + sbp_register_all_msg_callback(&s, &frame_logging_callback, &DUMMY_MEMORY_FOR_CALLBACKS, &n); + + u8 msg_1[] = { 0x01, 0x02, 0x03, 0x04 }; + u8 msg_2[] = { 0x05, 0x06, 0x07 }; + + dummy_reset(); + logging_reset(); + sbp_send_message(&s, 0x2269, 0x42, sizeof(msg_1), msg_1, &dummy_write); + sbp_send_message(&s, 0x2270, 0x43, sizeof(msg_2), msg_2, &dummy_write); + + while (dummy_rd < dummy_wr) { + fail_unless(sbp_process(&s, &dummy_read) >= SBP_OK, + "sbp_process threw an error!"); + } + + fail_unless(n_frame_callbacks_logged == 2, + "two frame callback should have been logged, %u were", n_frame_callbacks_logged); + fail_unless(last_frame_sender_id == 0x43, + "sender_id decoded incorrectly"); + fail_unless(last_frame_payload_len == sizeof(msg_2), + "len decoded incorrectly"); + fail_unless(last_frame_len == sizeof(msg_2) + 8, + "frame len decoded incorrectly"); +} +END_TEST + +START_TEST(test_sbp_big_msg) +{ + /* Tests registering for max size message SBP_MAX_PAYLOAD_LEN (255) */ + + sbp_state_t s; + sbp_state_init(&s); + sbp_state_set_io_context(&s, &DUMMY_MEMORY_FOR_IO); + + static sbp_msg_callbacks_node_t n; + static sbp_msg_callbacks_node_t n2; + + sbp_register_frame_callback(&s, 0x2269, &frame_logging_callback, &DUMMY_MEMORY_FOR_CALLBACKS, &n); + sbp_register_callback(&s, 0x2269, &logging_callback, &DUMMY_MEMORY_FOR_CALLBACKS, &n2); + + u8 big_msg[SBP_MAX_PAYLOAD_LEN]; + for(int i = 0; i < sizeof(big_msg); i++) { big_msg[i]=i;} + + dummy_reset(); + logging_reset(); + sbp_send_message(&s, 0x2269, 0x42, sizeof(big_msg), big_msg, &dummy_write); + + s8 ret = SBP_OK; + while (dummy_rd < dummy_wr) { + ret = sbp_process(&s, &dummy_read); + fail_unless(ret >= SBP_OK, + "sbp_process threw an error! error_code: %d", ret); + } + + fail_unless(n_frame_callbacks_logged == 1, + "one frame callback should have been logged, %u were", n_frame_callbacks_logged); + fail_unless(n_callbacks_logged == 1, + "one callbackx should have been logged, %u were", n_frame_callbacks_logged); + fail_unless(last_frame_sender_id == 0x42, + "sender_id decoded incorrectly"); + fail_unless(last_frame_payload_len == sizeof(big_msg), + "len decoded incorrectly"); + fail_unless(last_frame_len == SBP_MAX_FRAME_LEN, + "frame len decoded incorrectly"); + fail_unless(memcmp(SBP_FRAME_MSG_PAYLOAD(last_frame), big_msg, sizeof(big_msg)) + == 0, + "frame data decoded incorrectly (3) %x"); + /* check that CRC wasn't chopped off */ + fail_unless((last_frame[262] == 0x35 && last_frame[261] == 0xA6), + "CRC was incorrect. Should be %x and was %x", 0x35A6, *((u16*) &(last_frame[261]))); +} +END_TEST + START_TEST(test_sbp_send_message) { /* TODO: Tests with different write function behaviour. */ @@ -451,6 +731,89 @@ START_TEST(test_callbacks) } END_TEST +START_TEST(test_frame_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_frame_callback(&s, 0x2233, &test_frame_callback, 0, 0) == SBP_NULL_ERROR, + "sbp_register_frame_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_frame_callback(&s, 0x2233, 0, 0, &n) == SBP_NULL_ERROR, + "sbp_register_callback should return an error if cb is NULL"); + + fail_unless(sbp_register_frame_callback(&s, 0x2233, &test_frame_callback, &NUMBER, &n) == SBP_OK, + "sbp_register_callback should return success if everything is groovy"); + + fail_unless(sbp_register_frame_callback(&s, 0x2233, &test_frame_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_frame_callback(&s, 0x1234, &test_frame_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_frame_callback(&s, 0x1234, &test_frame_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)"); + +} +END_TEST + Suite* sbp_suite(void) { Suite *s = suite_create("SBP"); @@ -461,6 +824,10 @@ Suite* sbp_suite(void) tcase_add_test(tc_core, test_callbacks); tcase_add_test(tc_core, test_sbp_send_message); tcase_add_test(tc_core, test_sbp_process); + tcase_add_test(tc_core, test_sbp_frame); + tcase_add_test(tc_core, test_frame_callbacks); + tcase_add_test(tc_core, test_sbp_all_msg); + tcase_add_test(tc_core, test_sbp_big_msg); suite_add_tcase(s, tc_core);