Pileus Git - ~andy/csm213a-hw/blob - hw2/messages.c

   1 #include "messages.h"
   2
   3 void msg_register(parser_t *parser, int msgid, handler_t handler)
   4 {
   5         if (msgid < MSG_MAX_ID)
   6                 parser->handler[msgid] = handler;
   7 }
   8
   9 void msg_receive(parser_t *parser, int byte)
  10 {
  11         //sirq_printf("msg_receive - %02x\r\n", byte);
  12
  13         // Lookup pointers
  14         header_t *head = (header_t*)parser->buffer;
  15         void     *body = (void*)(head+1);
  16         const int max_length = sizeof(parser->buffer)-sizeof(header_t);
  17
  18         // Process uart messages
  19         parser->buffer[parser->index++] = byte;
  20         switch (parser->state) {
  21                 case 0: // Search
  22                         if (parser->index == sizeof(uint16_t)) {
  23                                 if (head->header == MSG_HEADER) {
  24                                         parser->state = 1;
  25                                 } else {
  26                                         parser->buffer[0] = parser->buffer[1];
  27                                         parser->index = 1;
  28                                 }
  29                         }
  30                         break;
  31                 case 1: // Header
  32                         if (parser->index == sizeof(header_t)) {
  33                                 if (head->length <= max_length &&
  34                                     head->msgid  <= MSG_MAX_ID) {
  35                                         parser->state = 2;
  36                                 } else {
  37                                         parser->index = 0;
  38                                         parser->state = 0;
  39                                 }
  40                         }
  41                         break;
  42                 case 2: // Data
  43                         if (parser->index == (int)sizeof(header_t)+head->length) {
  44                                 handler_t handler = parser->handler[head->msgid];
  45                                 if (handler)
  46                                         handler(head, body);
  47
  48                                 parser->index = 0;
  49                                 parser->state = 0;
  50                         }
  51                         break;
  52         }
  53 }