From 19fb0d31faf0c6052ccd4bbfbb3f14508e89141c Mon Sep 17 00:00:00 2001
From: Werner Almesberger <werner@almesberger.net>
Date: Sun, 1 Jul 2012 10:43:37 -0300
Subject: [PATCH] fw/: add DIAG protocol (supply voltage measurement, for
 diagnostics)

---
 doc/PROTOCOL  |  8 +++++
 fw/Makefile   |  2 +-
 fw/antorcha.c |  1 +
 fw/diag.c     | 91 +++++++++++++++++++++++++++++++++++++++++++++++++++
 fw/dispatch.h |  1 +
 fw/proto.h    |  3 ++
 6 files changed, 105 insertions(+), 1 deletion(-)
 create mode 100644 fw/diag.c

diff --git a/doc/PROTOCOL b/doc/PROTOCOL
index 0eed887..60d2a3e 100644
--- a/doc/PROTOCOL
+++ b/doc/PROTOCOL
@@ -37,6 +37,14 @@ Protocol
 
    13	n	0	Samples
 
+   14	0	4	Diagnostic request (64 bytes payload)
+   14	1	4	Salt A
+   14	2	4	Salt B
+   14	3	4	Hash A
+   14	4	4	Hash B
+
+   15	n	0	Diagnostic response (with payload)
+
 Unlock, firmware, image, and parameter packets all have a fixed-size
 64 bytes payload. Pings and acknowledgements have no payload. Pongs
 may have a variable-size payload.
diff --git a/fw/Makefile b/fw/Makefile
index a78dc4e..642af17 100644
--- a/fw/Makefile
+++ b/fw/Makefile
@@ -30,7 +30,7 @@ OBJCOPY = $(AVR_PREFIX)objcopy
 #OBJDUMP = $(AVR_PREFIX)objdump
 SIZE = $(AVR_PREFIX)size
 
-OBJS = $(NAME).o accel.o dispatch.o hash.o image.o param.o \
+OBJS = $(NAME).o accel.o diag.o dispatch.o hash.o image.o param.o \
        reset.o sample.o secret.o sweep.o \
        $(COMMON_OBJS)
 BOOT_OBJS = boot.o flash.o fw.o $(COMMON_OBJS)
diff --git a/fw/antorcha.c b/fw/antorcha.c
index 02b98da..38e5cb7 100644
--- a/fw/antorcha.c
+++ b/fw/antorcha.c
@@ -113,6 +113,7 @@ static const struct handler *protos[] = {
 	&reset_handler,
 	&sample_handler,
 	&param_handler,
+	&diag_handler,
 	NULL
 };
 
diff --git a/fw/diag.c b/fw/diag.c
new file mode 100644
index 0000000..ff86d8a
--- /dev/null
+++ b/fw/diag.c
@@ -0,0 +1,91 @@
+/*
+ * fw/diag.c - Diagnostic request
+ *
+ * Written 2012 by Werner Almesberger
+ * Copyright 2012 Werner Almesberger
+ *
+ * This program 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 2 of the License, or
+ * (at your option) any later version.
+ */
+
+
+#include <stdbool.h>
+#include <stdint.h>
+#include <string.h>
+
+#include <avr/interrupt.h>
+
+#include "hash.h"
+#include "proto.h"
+#include "rf.h"
+#include "dispatch.h"
+#include "secret.h"
+#include "image.h"
+#include "sweep.h"
+#include "accel.h"
+
+
+static uint8_t tmp[2];
+static bool failed;
+
+
+static void do_diag(void)
+{
+	uint8_t pkg[7] = { DIAG_ACK, 0, 0, };
+	uint16_t v;
+
+	cli();
+	set_line(localize_line(tmp[0], tmp[1]));
+	v = measure_ref(1);
+	pkg[3] = v;
+	pkg[4] = v >> 8;
+	v = measure_ref(0);
+	pkg[5] = v;
+	pkg[6] = v >> 8;
+	set_line(localize_line(0, 0));
+	sei();
+	rf_send(pkg, sizeof(pkg));
+}
+
+
+static bool diag_more(uint8_t seq, uint8_t limit, const uint8_t *payload)
+{
+	switch (limit-seq) {
+	default:
+		hash_merge(payload, PAYLOAD);
+		break;
+	case 1:
+		hash_end();
+		failed = !hash_eq(payload, PAYLOAD, 0);
+		break;
+	case 0:
+		if (!hash_eq(payload, PAYLOAD, PAYLOAD))
+			failed = 1;
+		if (failed)
+			return 0;
+		do_diag();
+		break;
+	}
+	/* do_diag sends the ACK, not the dispatcher */
+	return 0;
+}
+
+
+static bool diag_first(uint8_t limit, const uint8_t *payload)
+{
+	hash_init();
+	hash_merge_progmem(image_secret, sizeof(image_secret));
+	hash_merge(payload, PAYLOAD);
+	memcpy(&tmp, payload, sizeof(tmp));
+	failed = 0;
+	return 1;
+}
+
+
+struct handler diag_handler = {
+        .type   = DIAG,
+        .first  = diag_first,
+        .more	= diag_more,
+};
diff --git a/fw/dispatch.h b/fw/dispatch.h
index b8612f7..c8803c0 100644
--- a/fw/dispatch.h
+++ b/fw/dispatch.h
@@ -30,6 +30,7 @@ extern struct handler image_handler;
 extern struct handler reset_handler;
 extern struct handler sample_handler;
 extern struct handler param_handler;
+extern struct handler diag_handler;
 
 
 bool dispatch(const uint8_t *buf, uint8_t len, const struct handler **protos);
diff --git a/fw/proto.h b/fw/proto.h
index 5f119e5..0c045cc 100644
--- a/fw/proto.h
+++ b/fw/proto.h
@@ -31,6 +31,9 @@ enum pck_type {
 	SAMPLE		= 10,	/* start/stop sampling */
 	SAMPLE_ACK	= 11,	/* start/stop sampling acknowledgement */
 	SAMPLES		= 13,	/* samples */
+	DIAG		= 14,	/* diagnostic request */
+	DIAG_ACK	= 15,	/* diagnostic response */
+	
 };