summaryrefslogtreecommitdiffstats
diff options
context:
space:
mode:
authorPaul Kocialkowski <contact@paulk.fr>2012-08-13 13:03:54 (GMT)
committerPaul Kocialkowski <contact@paulk.fr>2012-08-13 13:03:54 (GMT)
commit927c1c92dd092cec8c56351bf663101183f19076 (patch)
tree6e395a97e5cd50fb0ce6f2786cc95aaba4fbb601
parentb153cbd84327b97c6d1f7eacad52dab79e0c2d7e (diff)
downloadbcm4751-927c1c92dd092cec8c56351bf663101183f19076.zip
bcm4751-927c1c92dd092cec8c56351bf663101183f19076.tar.gz
bcm4751-927c1c92dd092cec8c56351bf663101183f19076.tar.bz2
MEIF: Added multi-line parser
Signed-off-by: Paul Kocialkowski <contact@paulk.fr>
-rw-r--r--Android.mk2
-rw-r--r--bcm4751_gpsd.c2
-rw-r--r--meif.c157
-rw-r--r--meif.h10
4 files changed, 167 insertions, 4 deletions
diff --git a/Android.mk b/Android.mk
index eecee6d..f5ed3f5 100644
--- a/Android.mk
+++ b/Android.mk
@@ -58,7 +58,7 @@ include $(BUILD_EXECUTABLE)
include $(CLEAR_VARS)
-LOCAL_SRC_FILES := bcm4751_gpsd.c
+LOCAL_SRC_FILES := bcm4751_gpsd.c meif.c
LOCAL_C_INCLUDES += $(LOCAL_PATH)
diff --git a/bcm4751_gpsd.c b/bcm4751_gpsd.c
index 5072751..6462525 100644
--- a/bcm4751_gpsd.c
+++ b/bcm4751_gpsd.c
@@ -288,7 +288,7 @@ int main(void)
return 1;
}
- bcm4751_read_dump(fd);
+ meif_read_loop(fd);
return 0;
}
diff --git a/meif.c b/meif.c
new file mode 100644
index 0000000..7fe9cc7
--- /dev/null
+++ b/meif.c
@@ -0,0 +1,157 @@
+/**
+ * This file is part of BCM4751 gpsd
+ *
+ * Copyright (C) 2011 Paul Kocialkowski <contact@oaulk.fr>
+ *
+ * BCM4751 gpsd is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * BCM4751 gpsd is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with BCM4751 gpsd. If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <unistd.h>
+#include <termios.h>
+#include <errno.h>
+
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <fcntl.h>
+
+#include "meif.h"
+
+void meif_read_loop(int fd)
+{
+ void *data = NULL;
+ int length = 0x100;
+
+ uint16_t *meif_data_waiting = NULL;
+ int meif_data_waiting_length = 0;
+
+ struct meif_message *meif_message = NULL;
+ struct meif_header *meif_header = NULL;
+ uint16_t *meif_data = NULL;
+ int meif_data_length = 0;
+ uint16_t marker = 0;
+
+ int run = 1;
+ int rc = -1;
+ int i;
+
+ data = malloc(length);
+
+ while(run) {
+ meif_header = NULL;
+ marker = 0;
+
+ if(meif_data == NULL) {
+ rc = bcm4751_serial_read(fd, data, length, NULL);
+ if(rc <= 0) {
+ free(data);
+ return;
+ }
+ } else if(meif_data_length > 0) {
+ if(meif_data_length > length)
+ meif_data_length = length;
+
+ memcpy(data, (void *) meif_data, meif_data_length);
+ rc = meif_data_length;
+
+ free(meif_data);
+ meif_data = NULL;
+ meif_data_length = 0;
+ }
+
+ if(meif_data_waiting == NULL) {
+ // Read the start marker
+ for(i=0 ; i < rc ; i++) {
+ marker = *((uint16_t *) (data + i));
+ if(marker == MEIF_START) {
+ // Raw MEIF data and header
+ meif_data_length = rc - i;
+ meif_data = (uint16_t *) malloc(meif_data_length);
+ memcpy((void *) meif_data, (void *) (data + i), meif_data_length);
+ meif_header = (struct meif_header *) meif_data;
+ break;
+ }
+ }
+ } else if(meif_data_waiting_length > 0) {
+ // There is an uncomplete message waiting
+
+ // Raw data and header from the waiting and the new data
+ meif_data_length = meif_data_waiting_length + rc;
+ meif_data = malloc(meif_data_length);
+ memcpy((void *) meif_data, (void *) meif_data_waiting, meif_data_waiting_length);
+ memcpy((void *) meif_data + meif_data_waiting_length, (void *) data, rc);
+ meif_header = (struct meif_header *) meif_data;
+
+ free(meif_data_waiting);
+ meif_data_waiting = NULL;
+ meif_data_waiting_length = 0;
+ }
+
+ if(meif_data == NULL || meif_header == NULL) {
+ printf("No usable data found, skipping!\n");
+ continue;
+ }
+
+ // The message wasn't fully retrieved
+ if(meif_data_length < sizeof(struct meif_header) || (meif_data_length - i - sizeof(struct meif_header)) < meif_header->length) {
+ meif_data_waiting = meif_data;
+ meif_data_waiting_length = meif_data_length;
+ meif_data = NULL;
+ meif_data_length = 0;
+
+ continue;
+ } else {
+ // Read the end marker
+ for(i=meif_header->length ; i < meif_data_length ; i++) {
+ // 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));
+
+ break;
+ }
+ }
+
+ // No end marker was found
+ if(i == meif_data_length) {
+ // meif_data and meif_data_length are still set, so they will be used
+ continue;
+ }
+
+ // There is still data after the end of the message
+ if(meif_data_length - i - 1 >= sizeof(marker)) {
+ meif_data_waiting_length = meif_data_length - i - 2;
+ meif_data_waiting = (uint16_t *) malloc(meif_data_waiting_length);
+ memcpy((void *) meif_data_waiting, (void *) meif_data + i + 2, meif_data_waiting_length);
+
+ free(meif_data);
+ meif_data = meif_data_waiting;
+ meif_data_length = meif_data_waiting_length;
+ } else {
+ free(meif_data);
+ meif_data = NULL;
+ meif_data_length = 0;
+ }
+
+ meif_data_waiting = NULL;
+ meif_data_waiting_length = 0;
+ }
+
+ // TODO: if there is a meif message, dispatch and free and set to NULL
+ }
+}
diff --git a/meif.h b/meif.h
index f79b9b8..2652529 100644
--- a/meif.h
+++ b/meif.h
@@ -24,8 +24,8 @@
#ifndef _MEIF_H_
#define _MEIF_H_
-#define MEIF_START 0xA593
-#define MEIF_END 0xA568
+#define MEIF_START 0xA5 + (0x93 << 8)
+#define MEIF_END 0xA5 + (0x68 << 8)
/*
* Commands
@@ -45,6 +45,12 @@
* Structures
*/
+struct meif_header {
+ uint16_t marker;
+ uint16_t command;
+ uint16_t length;
+} __attribute__((__packed__));
+
struct meif_message {
uint16_t command;
uint16_t length;