Mercurial > ublox > ublox8

changeset 87:0e4ab6c2a99c

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
ubx: actually wait for acks/naks Signed-off-by: Josef 'Jeff' Sipek <jeffpc@josefsipek.net>
author Josef 'Jeff' Sipek <jeffpc@josefsipek.net>
date Mon, 22 Feb 2021 08:46:16 -0500
parents 310a6abb7454
children 2875fe2d8fd5
files capture.c iothread.c ubx.c ubx.h
diffstat 4 files changed, 78 insertions(+), 10 deletions(-) [+]
line wrap: on
line diff
--- a/capture.c	Mon Feb 22 07:56:56 2021 -0500
+++ b/capture.c	Mon Feb 22 08:46:16 2021 -0500
@@ -359,6 +359,9 @@
 	if (setvbuf(ifile, file_buffer, _IOFBF, sizeof(file_buffer)) == EOF)
 		fprintf(stderr, "Warn: Failed to mark stream as buffered\n");
 
+	/* make sure ack queuing is set up */
+	ubx_init_queue();
+
 	/* start the iothread before we make any writes to the device */
 	ret = iothread_start(ifile, rfile, verbose, !input_readonly);
 	if (ret) {
--- a/iothread.c	Mon Feb 22 07:56:56 2021 -0500
+++ b/iothread.c	Mon Feb 22 08:46:16 2021 -0500
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2019-2020 Josef 'Jeff' Sipek <jeffpc@josefsipek.net>
+ * Copyright (c) 2019-2021 Josef 'Jeff' Sipek <jeffpc@josefsipek.net>
  *
  * Permission is hereby granted, free of charge, to any person obtaining a copy
  * of this software and associated documentation files (the "Software"), to deal
@@ -168,6 +168,10 @@
 			raw[hdr.len], raw[hdr.len + 1]);
 	}
 
+	if ((mkmsgid(hdr.class, hdr.id) == UBX_ACK_ACK) ||
+	    (mkmsgid(hdr.class, hdr.id) == UBX_ACK_NAK))
+		notify_ubx_ack(raw, hdr.len, hdr.id);
+
 	ret = log_raw_ubx(&hdr, raw, start_time, start_tick);
 	if (ret)
 		fprintf(stderr, "Failed to log raw UBX message: %s\n",
--- a/ubx.c	Mon Feb 22 07:56:56 2021 -0500
+++ b/ubx.c	Mon Feb 22 08:46:16 2021 -0500
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2019-2020 Josef 'Jeff' Sipek <jeffpc@josefsipek.net>
+ * Copyright (c) 2019-2021 Josef 'Jeff' Sipek <jeffpc@josefsipek.net>
  *
  * Permission is hereby granted, free of charge, to any person obtaining a copy
  * of this software and associated documentation files (the "Software"), to deal
@@ -22,11 +22,33 @@
 
 #include <jeffpc/io.h>
 #include <jeffpc/hexdump.h>
+#include <jeffpc/synch.h>
+#include <jeffpc/list.h>
 
 #include "ubx.h"
 #include "xstdio.h"
 #include "rfc1145.h"
 
+static LOCK_CLASS(ack_queue_lc);
+static struct lock ack_lock;
+static struct list ack_waiters;
+
+struct ack_waiter {
+	struct list_node node;
+	struct cond cond;
+	enum ubx_msg_id id;
+	bool finished;
+	bool success;
+};
+
+void ubx_init_queue(void)
+{
+	MXINIT(&ack_lock, &ack_queue_lc);
+
+	list_create(&ack_waiters, sizeof(struct ack_waiter),
+		    offsetof(struct ack_waiter, node));
+}
+
 const char *ubx_msg_name(enum ubx_msg_id id)
 {
 	const char *names[0xffff] = {
@@ -96,12 +118,51 @@
 	return 0;
 }
 
-static int __read_ubx_ack(FILE *file, bool *ack_r)
+static bool __read_ubx_ack(FILE *file, enum ubx_msg_id id)
+{
+	struct ack_waiter waiter;
+
+	CONDINIT(&waiter.cond);
+	waiter.finished = false;
+	waiter.id = id;
+
+	MXLOCK(&ack_lock);
+	list_insert_tail(&ack_waiters, &waiter);
+
+	while (!waiter.finished)
+		CONDWAIT(&waiter.cond, &ack_lock);
+	MXUNLOCK(&ack_lock);
+
+	CONDDESTROY(&waiter.cond);
+
+	return waiter.success;
+}
+
+void notify_ubx_ack(const uint8_t *data, size_t len, bool success)
 {
-	FIXME("get ack");
+	struct ack_waiter *cur, *tmp;
+	enum ubx_msg_id id;
+
+	ASSERT3U(len, ==, 2);
+
+	id = mkmsgid(data[0], data[1]);
+
+	fprintf(stderr, "%s for %02x %02x (%s)\n", success ? "ACK" : "NAK",
+		data[0], data[1], ubx_msg_name(id));
 
-	*ack_r = true;
-	return 0;
+	MXLOCK(&ack_lock);
+	list_for_each_safe(cur, tmp, &ack_waiters) {
+		if (cur->id != id)
+			continue;
+
+		cur->finished = true;
+		cur->success = success;
+		CONDSIG(&cur->cond);
+
+		/* remove from queue */
+		list_remove(&ack_waiters, cur);
+	}
+	MXUNLOCK(&ack_lock);
 }
 
 int send_ubx(FILE *file, enum ubx_msg_id id, const void *data, size_t len)
@@ -119,9 +180,7 @@
 	if (ret)
 		return ret;
 
-	ret = __read_ubx_ack(file, &ack);
-	if (ret)
-		return ret;
+	ack = __read_ubx_ack(file, id);
 
 	return ack ? 0 : -ENOTSUP;
 }
--- a/ubx.h	Mon Feb 22 07:56:56 2021 -0500
+++ b/ubx.h	Mon Feb 22 08:46:16 2021 -0500
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2019-2020 Josef 'Jeff' Sipek <jeffpc@josefsipek.net>
+ * Copyright (c) 2019-2021 Josef 'Jeff' Sipek <jeffpc@josefsipek.net>
  *
  * Permission is hereby granted, free of charge, to any person obtaining a copy
  * of this software and associated documentation files (the "Software"), to deal
@@ -280,12 +280,14 @@
 } __attribute__((packed,aligned(4)));
 STATIC_ASSERT(sizeof(struct ubx_rxm_sfrbx) == 8);
 
+extern void ubx_init_queue(void);
 extern const char *ubx_msg_name(enum ubx_msg_id id);
 extern int send_ubx(FILE *file, enum ubx_msg_id id, const void *data,
 		    size_t len);
 extern int send_ubx_with_ack(FILE *file, enum ubx_msg_id id, const void *data,
 			     size_t len);
 extern int enable_ubx_msg(FILE *file, enum ubx_msg_id id, int port, int rate);
+extern void notify_ubx_ack(const uint8_t *data, size_t len, bool success);
 
 extern bool parse_and_process_ubx_message(const uint8_t *buf, size_t len,
 					  const uint64_t tick,