Support trying all possible vendors when reading OTP

Add support for trying all possible board vendors (untrusted, RAD,
testing) when reading OTP via --otp-read=auto option.

Signed-off-by: Marek Behún <kabel@kernel.org>

README.md

@@ -9,6 +9,7 @@ Features over Marvell's original `WtpDownloader`:
 Other features:
 * create ECDSA signed images (Turris MOX boards are locked to only boot signed firmware)
 * burning board information to OTP (MAC address, serial number, signing key) - currently implemented for Turris MOX
+* read eFuse OTP memory
 
 ## Usage
 
@@ -64,6 +65,12 @@ mox-imager -D /dev/ttyUSB0 -b 6000000 -t .../flash-image.bin
 mox-imager -D /dev/ttyUSB0 -t
 ```
 
+### Read device eFuse OTP memory
+
+```
+mox-imager -D /dev/ttyUSB0 -E -Rauto
+```
+
 ### Print image info
 
 ```

images.c

@@ -87,6 +87,21 @@ void image_hash(u32 alg, void *buf, size_t size, void *out, u32 hashaddr)
 	die("Failed hashing (%s)", hash2name(alg));
 }
 
+void image_delete_all(void)
+{
+	for (int i = 0; i < 32; ++i) {
+		if (!images[i].id)
+			continue;
+
+		if (images[i].id == TIMH_ID || images[i].id == TIMN_ID)
+			free(images[i].data);
+
+		images[i].id = 0;
+		images[i].data = NULL;
+		images[i].size = 0;
+	}
+}
+
 image_t *image_new(void *data, u32 size, u32 id)
 {
 	int i;

images.h

@@ -17,6 +17,7 @@ typedef struct {
 extern image_t *image_find(u32 id);
 extern _Bool image_exists(u32 id);
 extern void image_hash(u32 alg, void *buf, size_t size, void *out, u32 hashaddr);
+extern void image_delete_all(void);
 extern image_t *image_new(void *data, u32 size, u32 id);
 extern void image_load(const char *path);
 extern void image_load_bundled(void *data, size_t size);

mox-imager.c

@@ -587,7 +587,7 @@ static void load_bundled_otp_read_image(const char *otp_read)
 #include "bundled-read-otp-rad.c"
 		image_load_bundled(bundled_read_otp_data, bundled_read_otp_data_size);
 	} else {
-		die("Invalid value for option --otp-read. Supported values: \"testing\", \"RAD\"");
+		die("Invalid value for option --otp-read. Supported values: \"testing\", \"RAD\" and \"auto\"");
 	}
 }
 
@@ -598,8 +598,10 @@ static void create_otp_read_image(const args_t *args)
 	create_image_from_bundled_wtmi(args);
 }
 
-static void do_otp_read(const args_t *args)
+static void _do_otp_read(void *arg)
 {
+	const args_t *args = arg;
+
 	if (!strcmp(args->otp_read, ""))
 		create_otp_read_image(args);
 	else
@@ -608,6 +610,44 @@ static void do_otp_read(const args_t *args)
 	do_uart(args);
 }
 
+static __attribute__((__noreturn__)) void do_otp_read(const args_t *args)
+{
+	static const char * const vendors[3] = { "", "RAD", "testing" };
+	args_t new_args;
+
+	/* if VENDIR in --otp-read=VENDOR is not "auto", just call it */
+	if (strcmp(args->otp_read, "auto")) {
+		_do_otp_read((void *)args);
+		exit(EXIT_SUCCESS);
+	}
+
+	/* otherwise try all vendors */
+	new_args = *args;
+
+	for_each_const(vendor, vendors) {
+		char fw[64];
+
+		if (!strcmp(*vendor, ""))
+			strcpy(fw, "untrusted firmware");
+		else
+			snprintf(fw, sizeof(fw), "firmware signed by %s vendor key", *vendor);
+
+		info("Trying to read OTP by sending %s\n", fw);
+
+		new_args.otp_read = *vendor;
+
+		if (try_catch(_do_otp_read, &new_args)) {
+			info("Failed reading OTP with %s\n\n", fw);
+			image_delete_all();
+			continue;
+		}
+
+		exit(EXIT_SUCCESS);
+	}
+
+	die("Could not read OTP, tried firmware for all possible board vendors");
+}
+
 static void set_bootfs_if_possible(u32 bootfs)
 {
 	image_t *timh = image_find(TIMH_ID);
@@ -670,8 +710,8 @@ static void help(void)
 		"  -o, --output=IMAGE                          output SPI NOR flash image to IMAGE\n"
 		"  -k, --key=KEY                               read ECDSA-521 private key from file KEY\n"
 		"  -r, --random-seed=FILE                      read random seed from file (for deterministic private key generation)\n\n"
-		"  -R, --otp-read[=VENDOR]                     read OTP memory (use the optional option VENDOR to read OTP on trusted\n"
-		"                                              boards signed with VENDOR's key)\n\n"
+		"  -R, --otp-read[=VENDOR|auto]                read OTP memory (use the optional option VENDOR to read OTP on trusted\n"
+		"                                              boards signed with VENDOR's key, or \"auto\" to try all possibilities)\n\n"
 		"  -d, --deploy[=no-board-info]                deploy device (write OTP memory).\n"
 		"                                              Serial number, MAC address, board type and board version\n"
 		"                                              must not be given if the 'no-board-info' parameter is given.\n"
@@ -995,7 +1035,6 @@ int main(int argc, char **argv)
 
 	if (args.otp_read) {
 		do_otp_read(&args);
-		exit(EXIT_SUCCESS);
 	} else if (args.deploy) {
 		create_deploy_image(&args);
 	} else if (images_given) {

utils.c

@@ -11,6 +11,7 @@
 #include <string.h>
 #include <sys/random.h>
 #include <time.h>
+#include <setjmp.h>
 
 #include "mox-imager.h"
 #include "wtptp.h"
@@ -55,19 +56,15 @@ __attribute__((__format__(printf, 1, 2))) void notice(const char * restrict fmt,
 	va_end(ap);
 }
 
-__attribute__((__noreturn__, __format__(printf, 1, 2))) void die(const char *fmt, ...)
+static __attribute__((__noreturn__)) void vdie(const char *fmt, va_list ap)
 {
-	va_list ap;
-
 #ifndef GPP_COMPILER
 	int saved_errno = errno;
 	closewtp();
 	errno = saved_errno;
 #endif
 
-	va_start(ap, fmt);
 	vffprintf(8 | 1, stderr, fmt, ap);
-	va_end(ap);
 
 	fprintf(stderr, "\n\n");
 
@@ -81,6 +78,47 @@ __attribute__((__noreturn__, __format__(printf, 1, 2))) void die(const char *fmt
 	exit(EXIT_FAILURE);
 }
 
+__attribute__((__noreturn__, __format__(printf, 1, 2))) void die(const char *fmt, ...)
+{
+	va_list ap;
+
+	va_start(ap, fmt);
+	vdie(fmt, ap);
+	va_end(ap);
+}
+
+static jmp_buf *exception_buf;
+
+__attribute__((__noreturn__, __format__(printf, 2, 3))) void throw_or_die(_Bool do_throw, const char *fmt, ...)
+{
+	va_list ap;
+
+	if (do_throw && exception_buf)
+		longjmp(*exception_buf, 1);
+
+	va_start(ap, fmt);
+	vdie(fmt, ap);
+	va_end(ap);
+}
+
+_Bool try_catch(void (*cb)(void *), void *arg)
+{
+	jmp_buf buf;
+	int res;
+
+	exception_buf = &buf;
+	res = setjmp(buf);
+	if (res) {
+		exception_buf = NULL;
+		return 1;
+	}
+
+	cb(arg);
+	exception_buf = NULL;
+
+	return 0;
+}
+
 double now(void)
 {
 	struct timespec ts;

utils.h

@@ -10,6 +10,12 @@
 #include <stdio.h>
 #include <endian.h>
 
+#define ARRAY_SIZE(__x) (sizeof((__x)) / sizeof((__x)[0]))
+#define for_each_const(__m, __a)			\
+	for (const typeof((__a)[0]) *(__m) = &(__a)[0];	\
+	     (__m) < &(__a)[ARRAY_SIZE((__a))];		\
+	     ++(__m))
+
 typedef unsigned char u8;
 typedef unsigned short u16;
 typedef unsigned int u32;
@@ -18,6 +24,10 @@ typedef unsigned long long u64;
 extern __attribute__((__format__(printf, 1, 2))) void info(const char * restrict fmt, ...);
 extern __attribute__((__format__(printf, 1, 2))) void notice(const char * restrict fmt, ...);
 extern __attribute__((__noreturn__, __format__(printf, 1, 2))) void die(const char *fmt, ...);
+
+extern _Bool try_catch(void (*cb)(void *), void *arg);
+extern __attribute__((__noreturn__, __format__(printf, 2, 3))) void throw_or_die(_Bool do_throw, const char *fmt, ...);
+
 extern double now(void);
 extern void *xmalloc(size_t sz);
 extern void *xrealloc(void *ptr, size_t sz);

wtptp.c

@@ -86,8 +86,9 @@ static void xread(void *buf, size_t size)
 		pfd.revents = 0;
 		res = poll(&pfd, 1, 2 * 1000);
 		if (res == 0)
-			die("Timed out while waiting for response!%s",
-			    xread_timed_out_msg ?: "");
+			throw_or_die(xread_timed_out_msg,
+				     "Timed out while waiting for response!%s",
+				     xread_timed_out_msg ?: "");
 		else if (res < 0)
 			die("Cannot poll: %m");
 
@@ -713,8 +714,8 @@ static void readresp(u8 cmd, u8 seq, u8 cid, resp_t *resp)
 	xread(((void *) resp) + 3, 2);
 
 	if (resp->status > 0x2)
-		die("Unknown response status code 0x%x%s", resp->status,
-		    unk_resp_sts_msg ?: "");
+		throw_or_die(unk_resp_sts_msg, "Unknown response status code 0x%x%s",
+			     resp->status, unk_resp_sts_msg ?: "");
 
 	xread(((void *) resp) + 5, 1);
 	if (resp->len > 0)
@@ -765,7 +766,7 @@ static void checkresp(resp_t *resp)
 	if (resp->status == 0x2)
 		die("Sequence error on command %02x", resp->cmd);
 	else if (resp->status == 0x1)
-		die("NACK on command %02x%s", resp->cmd, nack_msg ?: "");
+		throw_or_die(nack_msg, "NACK on command %02x%s", resp->cmd, nack_msg ?: "");
 }
 
 static void sendcmd(u8 cmd, u8 seq, u8 cid, u8 flags, u32 len, const void *data,