00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
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
00166 if (cb == 0)
00167 return SBP_NULL_ERROR;
00168
00169
00170 if (node == 0)
00171 return SBP_NULL_ERROR;
00172
00173
00174 if (sbp_find_callback(s, msg_type) != 0)
00175 return SBP_CALLBACK_ERROR;
00176
00177
00178 node->msg_type = msg_type;
00179 node->cb = cb;
00180 node->context = context;
00181
00182
00183
00184 node->next = 0;
00185
00186
00187
00188
00189 if (s->sbp_msg_callbacks_head == 0) {
00190 s->sbp_msg_callbacks_head = node;
00191 return SBP_OK;
00192 }
00193
00194
00195
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
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
00226 if (!s->sbp_msg_callbacks_head)
00227 return 0;
00228
00229
00230
00231
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
00241 return 0;
00242 }
00243
00251 void sbp_state_init(sbp_state_t *s)
00252 {
00253 s->state = WAITING;
00254
00255
00256 s->io_context = 0;
00257
00258
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
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
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
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
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
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
00433 if (len != 0 && payload == 0)
00434 return SBP_NULL_ERROR;
00435
00436
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