From 0c9f05dc8038f22f868df79f7446797e1644b8a9 Mon Sep 17 00:00:00 2001
From: Sebastien Bourdeauducq <sb@m-labs.hk>
Date: Sun, 21 Sep 2014 23:32:14 +0800
Subject: [PATCH] soc/runtime: add exception management functions

---
 soc/runtime/Makefile     |  2 +-
 soc/runtime/exceptions.c | 32 ++++++++++++++++++++++++++++++++
 soc/runtime/exceptions.h | 15 +++++++++++++++
 3 files changed, 48 insertions(+), 1 deletion(-)
 create mode 100644 soc/runtime/exceptions.c
 create mode 100644 soc/runtime/exceptions.h

diff --git a/soc/runtime/Makefile b/soc/runtime/Makefile
index 7529b68c4..b8257ad8f 100644
--- a/soc/runtime/Makefile
+++ b/soc/runtime/Makefile
@@ -3,7 +3,7 @@ include $(MSCDIR)/software/common.mak
 BOARD=papilio_pro
 SERIAL=/dev/ttyUSB1
 
-OBJECTS=isr.o elf_loader.o services.o corecom_serial.o gpio.o rtio.o dds.o main.o
+OBJECTS=isr.o elf_loader.o exceptions.o services.o corecom_serial.o gpio.o rtio.o dds.o main.o
 
 all: runtime.bin
 
diff --git a/soc/runtime/exceptions.c b/soc/runtime/exceptions.c
new file mode 100644
index 000000000..63e6ef6dd
--- /dev/null
+++ b/soc/runtime/exceptions.c
@@ -0,0 +1,32 @@
+#include <setjmp.h>
+
+#include "exceptions.h"
+
+static struct exception_env *env_top;
+static int stored_id;
+
+int exception_catch(struct exception_env *ee, int *id)
+{
+	ee->prev = env_top;
+	env_top = ee;
+	if(setjmp(env_top->jb)) {
+		*id = stored_id;
+		return 1;
+	} else
+		return 0;
+}
+
+void exception_pop(void)
+{
+	env_top = env_top->prev;
+}
+
+void exception_raise(int id)
+{
+	struct exception_env *ee;
+
+	ee = env_top;
+	env_top = env_top->prev;
+	stored_id = id; /* __builtin_longjmp needs its second argument set to 1 */
+	longjmp(ee->jb, 1);
+}
diff --git a/soc/runtime/exceptions.h b/soc/runtime/exceptions.h
new file mode 100644
index 000000000..4c935ca63
--- /dev/null
+++ b/soc/runtime/exceptions.h
@@ -0,0 +1,15 @@
+#ifndef __EXCEPTIONS_H
+#define __EXCEPTIONS_H
+
+#include <setjmp.h>
+
+struct exception_env {
+	jmp_buf jb;
+	struct exception_env *prev;
+};
+
+int exception_catch(struct exception_env *ee, int *id);
+void exception_pop(void);
+void exception_raise(int id) __attribute__((noreturn));
+
+#endif /* __EXCEPTIONS_H */