sbp.c
Go to the documentation of this file.
00001 /*
00002  * Copyright (C) 2011-2014 Swift Navigation Inc.
00003  * Contact: Fergus Noble <fergus@swift-nav.com>
00004  *
00005  * This source is subject to the license found in the file 'LICENSE' which must
00006  * be be distributed together with this source. All other rights reserved.
00007  *
00008  * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND,
00009  * EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE IMPLIED
00010  * WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR PURPOSE.
00011  */
00012 
00013 #include "edc.h"
00014 
00015 #include "sbp.h"
00016 
00017 #define SBP_PREAMBLE 0x55
00018 
00162 s8 sbp_register_callback(sbp_state_t *s, u16 msg_type, sbp_msg_callback_t cb, void *context,
00163                          sbp_msg_callbacks_node_t *node)
00164 {
00165   /* Check our callback function pointer isn't NULL. */
00166   if (cb == 0)
00167     return SBP_NULL_ERROR;
00168 
00169   /* Check our callback node pointer isn't NULL. */
00170   if (node == 0)
00171     return SBP_NULL_ERROR;
00172 
00173   /* Check if callback was already registered for this type. */
00174   if (sbp_find_callback(s, msg_type) != 0)
00175     return SBP_CALLBACK_ERROR;
00176 
00177   /* Fill in our new sbp_msg_callback_node_t. */
00178   node->msg_type = msg_type;
00179   node->cb = cb;
00180   node->context = context;
00181   /* The next pointer is set to NULL, i.e. this
00182    * will be the new end of the linked list.
00183    */
00184   node->next = 0;
00185 
00186   /* If our linked list is empty then just
00187    * add the new node to the start.
00188    */
00189   if (s->sbp_msg_callbacks_head == 0) {
00190     s->sbp_msg_callbacks_head = node;
00191     return SBP_OK;
00192   }
00193 
00194   /* Find the tail of our linked list and
00195    * add our new node to the end.
00196    */
00197   sbp_msg_callbacks_node_t *p = s->sbp_msg_callbacks_head;
00198   while (p->next)
00199     p = p->next;
00200 
00201   p->next = node;
00202 
00203   return SBP_OK;
00204 }
00205 
00209 void sbp_clear_callbacks(sbp_state_t *s)
00210 {
00211   /* Reset the head of the callbacks list to NULL. */
00212   s->sbp_msg_callbacks_head = 0;
00213 }
00214 
00223 sbp_msg_callbacks_node_t* sbp_find_callback(sbp_state_t *s, u16 msg_type)
00224 {
00225   /* If our list is empty, return NULL. */
00226   if (!s->sbp_msg_callbacks_head)
00227     return 0;
00228 
00229   /* Traverse the linked list and return the callback
00230    * function pointer if we find a node with a matching
00231    * message id.
00232    */
00233   sbp_msg_callbacks_node_t *p = s->sbp_msg_callbacks_head;
00234   do
00235     if (p->msg_type == msg_type)
00236       return p;
00237 
00238   while ((p = p->next));
00239 
00240   /* Didn't find a matching callback, return NULL. */
00241   return 0;
00242 }
00243 
00251 void sbp_state_init(sbp_state_t *s)
00252 {
00253   s->state = WAITING;
00254 
00255   /* Set the IO context pointer, passed to read and write functions, to NULL. */
00256   s->io_context = 0;
00257 
00258   /* Clear the callbacks, if any, currently in s */
00259   sbp_clear_callbacks(s);
00260 }
00261 
00262 
00269 void sbp_state_set_io_context(sbp_state_t *s, void *context)
00270 {
00271   s->io_context = context;
00272 }
00273 
00313 s8 sbp_process(sbp_state_t *s, u32 (*read)(u8 *buff, u32 n, void *context))
00314 {
00315   u8 temp;
00316   u16 crc;
00317 
00318   switch (s->state) {
00319   case WAITING:
00320     if ((*read)(&temp, 1, s->io_context) == 1)
00321       if (temp == SBP_PREAMBLE) {
00322         s->n_read = 0;
00323         s->state = GET_TYPE;
00324       }
00325     break;
00326 
00327   case GET_TYPE:
00328     s->n_read += (*read)((u8*)&(s->msg_type) + s->n_read,
00329                          2-s->n_read, s->io_context);
00330     if (s->n_read >= 2) {
00331       /* Swap bytes to little endian. */
00332       s->n_read = 0;
00333       s->state = GET_SENDER;
00334     }
00335     break;
00336 
00337   case GET_SENDER:
00338     s->n_read += (*read)((u8*)&(s->sender_id) + s->n_read,
00339                          2-s->n_read, s->io_context);
00340     if (s->n_read >= 2) {
00341       /* Swap bytes to little endian. */
00342       s->state = GET_LEN;
00343     }
00344     break;
00345 
00346   case GET_LEN:
00347     if ((*read)(&(s->msg_len), 1, s->io_context) == 1) {
00348       s->n_read = 0;
00349       s->state = GET_MSG;
00350     }
00351     break;
00352 
00353   case GET_MSG:
00354     /* Not received whole message yet, try and read some more. */
00355     s->n_read += (*read)(
00356       &(s->msg_buff[s->n_read]),
00357       s->msg_len - s->n_read,
00358       s->io_context
00359     );
00360     if (s->msg_len - s->n_read <= 0) {
00361       s->n_read = 0;
00362       s->state = GET_CRC;
00363     }
00364     break;
00365 
00366   case GET_CRC:
00367     s->n_read += (*read)((u8*)&(s->crc) + s->n_read,
00368                          2-s->n_read, s->io_context);
00369     if (s->n_read >= 2) {
00370       s->state = WAITING;
00371 
00372       /* Swap bytes to little endian. */
00373       crc = crc16_ccitt((u8*)&(s->msg_type), 2, 0);
00374       crc = crc16_ccitt((u8*)&(s->sender_id), 2, crc);
00375       crc = crc16_ccitt(&(s->msg_len), 1, crc);
00376       crc = crc16_ccitt(s->msg_buff, s->msg_len, crc);
00377       if (s->crc == crc) {
00378 
00379         /* Message complete, process it. */
00380         sbp_msg_callbacks_node_t* node = sbp_find_callback(s, s->msg_type);
00381         if (node) {
00382           (*node->cb)(s->sender_id, s->msg_len, s->msg_buff, node->context);
00383           return SBP_OK_CALLBACK_EXECUTED;
00384         } else {
00385           return SBP_OK_CALLBACK_UNDEFINED;
00386         }
00387       } else
00388           return SBP_CRC_ERROR;
00389     }
00390     break;
00391 
00392   default:
00393     s->state = WAITING;
00394     break;
00395   }
00396 
00397   return SBP_OK;
00398 }
00399 
00429 s8 sbp_send_message(sbp_state_t *s, u16 msg_type, u16 sender_id, u8 len, u8 *payload,
00430                     u32 (*write)(u8 *buff, u32 n, void *context))
00431 {
00432   /* Check our payload data pointer isn't NULL unless len = 0. */
00433   if (len != 0 && payload == 0)
00434     return SBP_NULL_ERROR;
00435 
00436   /* Check our write function pointer isn't NULL. */
00437   if (write == 0)
00438     return SBP_NULL_ERROR;
00439 
00440   u16 crc;
00441 
00442   u8 preamble = SBP_PREAMBLE;
00443   if ((*write)(&preamble, 1, s->io_context) != 1)
00444     return SBP_SEND_ERROR;
00445 
00446   if ((*write)((u8*)&msg_type, 2, s->io_context) != 2)
00447     return SBP_SEND_ERROR;
00448 
00449   if ((*write)((u8*)&sender_id, 2, s->io_context) != 2)
00450     return SBP_SEND_ERROR;
00451 
00452   if ((*write)(&len, 1, s->io_context) != 1)
00453     return SBP_SEND_ERROR;
00454 
00455   if (len > 0) {
00456     if ((*write)(payload, len, s->io_context) != len)
00457       return SBP_SEND_ERROR;
00458   }
00459 
00460   crc = crc16_ccitt((u8*)&(msg_type), 2, 0);
00461   crc = crc16_ccitt((u8*)&(sender_id), 2, crc);
00462   crc = crc16_ccitt(&(len), 1, crc);
00463   crc = crc16_ccitt(payload, len, crc);
00464 
00465   if ((*write)((u8*)&crc, 2, s->io_context) != 2)
00466     return SBP_SEND_ERROR;
00467 
00468   return SBP_OK;
00469 }
00470 


swiftnav
Author(s):
autogenerated on Sat Jun 8 2019 18:55:58