GPSD: Switch protocol after patch upload and send the first unknown bytes

Signed-off-by: Paul Kocialkowski <contact@paulk.fr>

2 files changed, 25 insertions, 1 deletions

diff --git a/bcm4751_gpsd.c b/bcm4751_gpsd.c
index d7cdc9c..690433a 100644
--- a/bcm4751_gpsd.c
+++ b/bcm4751_gpsd.c
@@ -253,6 +253,22 @@ void bcm4751_read_dump(int fd)
 	}
 }
 
+void bcm4751_switch_protocol(int fd)
+{
+	uint8_t data1[] = { 0xff, 0x00, 0xfd, 0x40, 0x00, 0x00, 0xe6, 0xfc };
+	uint8_t data2[] = { 0xfe, 0x00, 0xfd, 0x6f, 0xfe, 0x00, 0x00, 0x00, 0x00, 0x00, 0xb2, 0xfc };
+	uint8_t data3[] = { 0xfe, 0x00, 0xfd, 0x6f, 0x3a, 0x01, 0x00, 0x00, 0x00, 0x00, 0x34, 0xfc };
+
+	sleep(1);
+
+	printf("Sending unknown bytes!\n");
+	bcm4751_serial_write(fd, data1, sizeof(data1));
+	bcm4751_serial_write(fd, data2, sizeof(data2));
+	bcm4751_serial_write(fd, data3, sizeof(data3));
+
+	bcm4751_read_dump(fd);
+}
+
 int main(void)
 {
 	int fd = -1;
diff --git a/meif.c b/meif.c
index 39e9f62..b0a5830 100644
--- a/meif.c
+++ b/meif.c
@@ -30,6 +30,8 @@
 
 #include "meif.h"
 
+#define RC_SWITCH_PROTOCOL	-4
+
 /*
  * Utils
  */
@@ -445,7 +447,7 @@ int meif_dispatch(struct meif_message *meif_message)
 				patch_send_stage = 3;
 				printf("Ready to switch protocol!\n");
 
-				return -1;
+				return RC_SWITCH_PROTOCOL;
 			}
 			break;
 		case MEIF_NACK_MSG:
@@ -634,4 +636,10 @@ void meif_read_loop(int fd)
 			meif_message_free(meif_message);
 		}
 	}
+
+	switch(rc) {
+		case RC_SWITCH_PROTOCOL:
+			bcm4751_switch_protocol(fd);
+			break;
+	}
 }