Factor out message parser

[~andy/csm213a-hw] / hw2 / messages.c

diff --git a/hw2/messages.c b/hw2/messages.c
new file mode 100644 (file)
index 0000000..eeeff80
--- /dev/null
+++ b/hw2/messages.c
@@ -0,0 +1,53 @@
+#include "messages.h"
+
+void msg_register(parser_t *parser, int msgid, handler_t handler)
+{
+       if (msgid < MSG_MAX_ID)
+               parser->handler[msgid] = handler;
+}
+
+void msg_receive(parser_t *parser, int byte)
+{
+       //sirq_printf("msg_receive - %02x\r\n", byte);
+
+       // Lookup pointers
+       header_t *head = (header_t*)parser->buffer;
+       void     *body = (void*)(head+1);
+       const int max_length = sizeof(parser->buffer)-sizeof(header_t);
+
+       // Process uart messages
+       parser->buffer[parser->index++] = byte;
+       switch (parser->state) {
+               case 0: // Search
+                       if (parser->index == sizeof(uint16_t)) {
+                               if (head->header == MSG_HEADER) {
+                                       parser->state = 1;
+                               } else {
+                                       parser->buffer[0] = parser->buffer[1];
+                                       parser->index = 1;
+                               }
+                       }
+                       break;
+               case 1: // Header
+                       if (parser->index == sizeof(header_t)) {
+                               if (head->length <= max_length &&
+                                   head->msgid  <= MSG_MAX_ID) {
+                                       parser->state = 2;
+                               } else {
+                                       parser->index = 0;
+                                       parser->state = 0;
+                               }
+                       }
+                       break;
+               case 2: // Data
+                       if (parser->index == (int)sizeof(header_t)+head->length) {
+                               handler_t handler = parser->handler[head->msgid];
+                               if (handler)
+                                       handler(head->msgid, body);
+
+                               parser->index = 0;
+                               parser->state = 0;
+                       }
+                       break;
+       }
+}