summaryrefslogtreecommitdiffstats
diff options
context:
space:
mode:
authorPaul Kocialkowski <contact@paulk.fr>2012-08-13 13:41:21 (GMT)
committerPaul Kocialkowski <contact@paulk.fr>2012-08-13 13:41:21 (GMT)
commit273a537e06a1ea1351e76d5e49a12794732cf7bb (patch)
treec2ad538db3e555bd9aea7a590977cb067dfd1cfb
parent927c1c92dd092cec8c56351bf663101183f19076 (diff)
downloadbcm4751-273a537e06a1ea1351e76d5e49a12794732cf7bb.zip
bcm4751-273a537e06a1ea1351e76d5e49a12794732cf7bb.tar.gz
bcm4751-273a537e06a1ea1351e76d5e49a12794732cf7bb.tar.bz2
MEIF: Added utility functions
Signed-off-by: Paul Kocialkowski <contact@paulk.fr>
-rw-r--r--meif.c94
1 files changed, 89 insertions, 5 deletions
diff --git a/meif.c b/meif.c
index 7fe9cc7..0fdf813 100644
--- a/meif.c
+++ b/meif.c
@@ -30,6 +30,84 @@
#include "meif.h"
+struct meif_message *meif_message_from_data(void *data, int length)
+{
+ struct meif_message *meif_message = NULL;
+ struct meif_header *meif_header = NULL;
+ void *message_data = NULL;
+ int message_data_length = 0;
+ uint16_t marker = 0;
+
+ if(data == NULL || length < sizeof(struct meif_header))
+ return NULL;
+
+ meif_header = (struct meif_header *) data;
+
+ marker = meif_header->marker;
+ if(marker != MEIF_START)
+ return NULL;
+
+ marker = *((uint16_t *) ((uint8_t *) data + length - sizeof(marker)));
+ if(marker != MEIF_END)
+ return NULL;
+
+ meif_message = calloc(1, sizeof(struct meif_message));
+ message_data_length = length - sizeof(struct meif_header) - sizeof(marker);
+ if(message_data_length > 0) {
+ message_data = calloc(1, message_data_length);
+ memcpy(message_data, (void *) ((uint8_t *) data + sizeof(struct meif_header)), message_data_length);
+
+ meif_message->length = message_data_length;
+ meif_message->data = message_data;
+ }
+
+ meif_message->command = meif_header->command;
+
+ return meif_message;
+}
+
+void meif_message_free(struct meif_message *meif_message)
+{
+ if(meif_message == NULL)
+ return;
+
+ if(meif_message->data != NULL)
+ free(meif_message->data);
+
+ memset(meif_message, 0, sizeof(struct meif_message));
+ free(meif_message);
+}
+
+void meif_message_log(struct meif_message *meif_message)
+{
+ char *command = NULL;
+
+ if(meif_message == NULL)
+ return;
+
+ switch(meif_message->command) {
+ case MEIF_ACK_MSG:
+ command = "MEIF_ACK_MSG";
+ break;
+ case MEIF_NACK_MSG:
+ command = "MEIF_NACK_MSG";
+ break;
+ case MEIF_STATE_REPORT_MSG:
+ command = "MEIF_STATE_REPORT_MSG";
+ break;
+ case MEIF_CONFIG_VALUES_MSG:
+ command = "MEIF_CONFIG_VALUES_MSG";
+ break;
+ default:
+ command = "MEIF_UNKNOWN_MSG";
+ break;
+ }
+
+ printf("MEIF message: %s with %d bytes of data:\n", command, meif_message->length);
+ if(meif_message->data != NULL && meif_message->length > 0)
+ hex_dump(meif_message->data, meif_message->length);
+}
+
void meif_read_loop(int fd)
{
void *data = NULL;
@@ -51,6 +129,7 @@ void meif_read_loop(int fd)
data = malloc(length);
while(run) {
+ meif_message = NULL;
meif_header = NULL;
marker = 0;
@@ -72,6 +151,8 @@ void meif_read_loop(int fd)
meif_data_length = 0;
}
+ printf("Read %d bytes\n", rc);
+
if(meif_data_waiting == NULL) {
// Read the start marker
for(i=0 ; i < rc ; i++) {
@@ -119,10 +200,9 @@ void meif_read_loop(int fd)
// We go with a 6 left offset from the announced end of the message
marker = *((uint16_t *) ((void *) meif_data + i));
if(marker == MEIF_END) {
- // TODO: convert to meif_message
- printf("MEIF message (%d bytes):\n", meif_data_length);
- hex_dump(meif_data, i + sizeof(marker));
-
+ meif_message = meif_message_from_data((void *) meif_data, i + sizeof(marker));
+ if(meif_message == NULL)
+ printf("Failed to create a MEIF message!\n");
break;
}
}
@@ -152,6 +232,10 @@ void meif_read_loop(int fd)
meif_data_waiting_length = 0;
}
- // TODO: if there is a meif message, dispatch and free and set to NULL
+ if(meif_message != NULL) {
+ meif_message_log(meif_message);
+ // TODO: dispatch
+ meif_message_free(meif_message);
+ }
}
}