Add exception handling mechanism and raise exception on low-level comms failure.

src/Makefile

@@ -24,6 +24,7 @@ SRC =			\
 	command.c	\
 	cortexm.c	\
 	crc32.c		\
+	exception.c	\
 	gdb_if.c	\
 	gdb_main.c	\
 	gdb_packet.c	\

src/adiv5_jtagdp.c

@@ -23,6 +23,7 @@
  */
 
 #include "general.h"
+#include "exception.h"
 #include "adiv5.h"
 #include "jtag_scan.h"
 #include "jtagtap.h"
@@ -93,10 +94,8 @@ static uint32_t adiv5_jtagdp_low_access(ADIv5_DP_t *dp, uint8_t RnW,
 	if (dp->allow_timeout && (ack == JTAGDP_ACK_WAIT))
 		return 0;
 
-	if((ack != JTAGDP_ACK_OK)) {
+	if((ack != JTAGDP_ACK_OK))
-		/* Fatal error if invalid ACK response */
+		raise_exception(EXCEPTION_ERROR, "JTAG-DP invalid ACK");
-		PLATFORM_FATAL_ERROR(1);
-	}
 
 	return (uint32_t)(response >> 3);
 }

src/adiv5_swdp.c

@@ -23,6 +23,7 @@
  */
 
 #include "general.h"
+#include "exception.h"
 #include "adiv5.h"
 #include "swdptap.h"
 #include "jtagtap.h"
@@ -143,14 +144,12 @@ static uint32_t adiv5_swdp_low_access(ADIv5_DP_t *dp, uint8_t RnW,
 		return 0;
 	}
 
-	if(ack != SWDP_ACK_OK) {
+	if(ack != SWDP_ACK_OK)
-		/* Fatal error if invalid ACK response */
+		raise_exception(EXCEPTION_ERROR, "SWDP invalid ACK");
-		PLATFORM_FATAL_ERROR(1);
-	}
 
 	if(RnW) {
 		if(swdptap_seq_in_parity(&response, 32))  /* Give up on parity error */
-			PLATFORM_FATAL_ERROR(1);
+			raise_exception(EXCEPTION_ERROR, "SWDP Parity error");
 	} else {
 		swdptap_seq_out_parity(value, 32);
 	}

src/exception.c

@@ -0,0 +1,39 @@
+/*
+ * This file is part of the Black Magic Debug project.
+ *
+ * Copyright (C) 2015  Black Sphere Technologies Ltd.
+ * Written by Gareth McMullin <gareth@blacksphere.co.nz>
+ *
+ * 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 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program 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 this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "general.h"
+#include "exception.h"
+
+struct exception *innermost_exception;
+
+void raise_exception(uint32_t type, const char *msg)
+{
+	struct exception *e;
+	for (e = innermost_exception; e; e = e->outer) {
+		if (e->mask & type) {
+			e->type = type;
+			e->msg = msg;
+			innermost_exception = e->outer;
+			longjmp(e->jmpbuf, type);
+		}
+	}
+	PLATFORM_FATAL_ERROR(type);
+}
+

src/include/exception.h

@@ -0,0 +1,74 @@
+/*
+ * This file is part of the Black Magic Debug project.
+ *
+ * Copyright (C) 2015  Black Sphere Technologies Ltd.
+ * Written by Gareth McMullin <gareth@blacksphere.co.nz>
+ *
+ * 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 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program 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 this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+/* Exception handling to escape deep nesting.
+ * Used for the case of communicaiton failure and timeouts.
+ */
+
+/* Example usage:
+ *
+ * volatile struct exception e;
+ * TRY_CATCH (e, EXCEPTION_TIMEOUT) {
+ *    ...
+ *    raise_exception(EXCEPTION_TIMEOUT, "Timeout occurred");
+ *    ...
+ * }
+ * if (e.type == EXCEPTION_TIMEOUT) {
+ *    printf("timeout: %s\n", e.msg);
+ * }
+ */
+
+/* Limitations:
+ * Can't use break, return, goto, etc from inside the TRY_CATCH block.
+ */
+
+#ifndef __EXCEPTION_H
+#define __EXCEPTION_H
+
+#include <setjmp.h>
+#include <stdint.h>
+
+#define EXCEPTION_ERROR   0x01
+#define EXCEPTION_TIMEOUT 0x02
+#define EXCEPTION_ALL     -1
+
+struct exception {
+	uint32_t type;
+	const char *msg;
+	/* private */
+	uint32_t mask;
+	jmp_buf jmpbuf;
+	struct exception *outer;
+};
+
+extern struct exception *innermost_exception;
+
+#define TRY_CATCH(e, type_mask) \
+	(e).type = 0; \
+	(e).mask = (type_mask); \
+	(e).outer = innermost_exception; \
+	innermost_exception = (void*)&(e); \
+	if (setjmp(innermost_exception->jmpbuf) == 0) \
+		for (;innermost_exception == &(e); innermost_exception = (e).outer)
+
+void raise_exception(uint32_t type, const char *msg);
+
+#endif
+

src/platforms/native/platform.h

@@ -153,6 +153,7 @@ extern jmp_buf fatal_error_jmpbuf;
 #define SET_IDLE_STATE(state)	{gpio_set_val(LED_PORT, LED_IDLE_RUN, state);}
 #define SET_ERROR_STATE(state)	{gpio_set_val(LED_PORT, LED_ERROR, state);}
 
+#include "target.h"
 #define PLATFORM_SET_FATAL_ERROR_RECOVERY()	{setjmp(fatal_error_jmpbuf);}
 #define PLATFORM_FATAL_ERROR(error)	do { 		\
 	if(running_status) gdb_putpacketz("X1D");	\