From: bsekisser <squirmyworms@embarqmail.com>
Date: Mon, 20 Oct 2014 07:48:55 +0000 (-0400)
Subject: watchdog: reworked to add support for software reset.
X-Git-Tag: v1.3~60^2
X-Git-Url: https://git.htl-mechatronik.at/public/?a=commitdiff_plain;h=a10d508fb1a7d9394fbab16082be56eead3ad81b;p=sx%2Fsimavr.git

watchdog: reworked to add support for software reset.

support for software reset due to watchdog timeout...
	based on watchdog documentation.

	modified:   sim/sim_avr.c
		modifications to avr_reset and avr_init to support software
			reset.

	modified:   sim/sim_avr.h
		added type avr_run_t...
		avr_t.run updated accordingly.

	modified:   sim/avr_watchdog.c
		largely rewritten to support software reset.

	modified:   sim/avr_watchdog.h
		data record 'reset_context' added to support software reset

	modified:   sim/sim_regbit.h
		added: avr_regbit_from_value
			for checking flags before commiting register value.
		added: avr_regbit_set_array_from_value
			reverse operation of avr_regbit_get_array
---

diff --git a/simavr/sim/avr_watchdog.c b/simavr/sim/avr_watchdog.c
index e70521d..b4628ca 100644
--- a/simavr/sim/avr_watchdog.c
+++ b/simavr/sim/avr_watchdog.c
@@ -24,16 +24,32 @@
 #include <stdlib.h>
 #include "avr_watchdog.h"
 
+static void avr_watchdog_run_callback_software_reset(avr_t * avr)
+{
+	avr_reset(avr);
+}
+
 static avr_cycle_count_t avr_watchdog_timer(struct avr_t * avr, avr_cycle_count_t when, void * param)
 {
 	avr_watchdog_t * p = (avr_watchdog_t *)param;
 
-	AVR_LOG(avr, LOG_TRACE, "WATCHDOG: timer fired.\n");
-	avr_raise_interrupt(avr, &p->watchdog);
-
-	if (!avr_regbit_get(avr, p->watchdog.enable)) {
-		AVR_LOG(avr, LOG_ERROR, "WATCHDOG: timer fired and interrupt is not enabled. Quitting\n");
-		avr_sadly_crashed(avr, 10);
+	if (avr_regbit_get(avr, p->watchdog.enable)) {
+		AVR_LOG(avr, LOG_TRACE, "WATCHDOG: timer fired.\n");
+		avr_raise_interrupt(avr, &p->watchdog);
+		return(when + p->cycle_count);
+	} else if (avr_regbit_get(avr, p->wde)) {
+		AVR_LOG(avr, LOG_TRACE, "WATCHDOG: timer fired without interrupt. Resetting\n");
+
+		p->reset_context.avr_run = avr->run;
+		p->reset_context.wdrf = 1;
+
+		/* Ideally we would perform a reset here via 'avr_reset'
+		 * However, returning after reset would result in an unconsistent state.
+		 * It seems our best (and cleanest) solution is to set a temporary call 
+		 * back which can safely perform the reset for us...  During reset,
+		 * the previous callback can be restored and safely resume.
+		 */
+		avr->run = avr_watchdog_run_callback_software_reset;
 	}
 
 	return 0;
@@ -46,46 +62,83 @@ static avr_cycle_count_t avr_wdce_clear(struct avr_t * avr, avr_cycle_count_t wh
 	return 0;
 }
 
+static void avr_watchdog_set_cycle_count_and_timer(
+	avr_t * avr, 
+	avr_watchdog_t * p, 
+	uint8_t was_enabled, 
+	int8_t old_wdp)
+{
+	// If nothing else, always ensure we have a valid cycle count...
+	uint8_t wdp = avr_regbit_get_array(avr, p->wdp, 4);
+
+	p->cycle_count = 2048 << wdp;
+	p->cycle_count = (p->cycle_count * avr->frequency) / 128000;
+
+	uint8_t wde = avr_regbit_get(avr, p->wde);
+	uint8_t wdie = avr_regbit_get(avr, p->watchdog.enable);
+
+	uint8_t enable_changed = (was_enabled != (wde || wdie));
+
+	uint8_t wdp_changed = ((old_wdp >= 0) ? (wdp != old_wdp) : 0);
+	
+	if (!enable_changed && !wdp_changed)
+		return;
+
+	static char *message[2][2] = { { 0, "reset" }, { "enabled", "enabled and set" } };
+
+	if (wde || wdie) {
+		AVR_LOG(avr, LOG_TRACE, "WATCHDOG: %s to %d cycles @ 128kz (* %d) = %d CPU cycles.\n",
+			message[enable_changed][wdp_changed], 2048 << wdp, 1 << wdp, (int)p->cycle_count);
+
+		avr_cycle_timer_register(avr, p->cycle_count, avr_watchdog_timer, p);
+	} else if (enable_changed) {
+		AVR_LOG(avr, LOG_TRACE, "WATCHDOG: disabled\n");
+		avr_cycle_timer_cancel(avr, avr_watchdog_timer, p);
+	}
+}
+
 static void avr_watchdog_write(avr_t * avr, avr_io_addr_t addr, uint8_t v, void * param)
 {
 	avr_watchdog_t * p = (avr_watchdog_t *)param;
-	// backup the registers
-	// uint8_t wd = avr->data[p->wdce.reg];
-	uint8_t wdce_o = avr_regbit_get(avr, p->wdce);	// old
-	uint8_t wde_o = avr_regbit_get(avr, p->wde);
-	uint8_t wdp_o[4];
-
-//	printf("avr_watchdog_write %02x\n", v);
-	for (int i = 0; i < 4; i++)
-		wdp_o[i] = avr_regbit_get(avr, p->wdp[i]);
-
-	avr->data[p->wdce.reg] = v;
-	uint8_t wdce_n = avr_regbit_get(avr, p->wdce);	// new
-
-	if (wdce_o /* || wdce_n */) {
-		// make sure bit gets reset eventually
-		if (wdce_n)
-			avr_cycle_timer_register(avr, 4, avr_wdce_clear, p);
 
-		uint8_t wdp = avr_regbit_get_array(avr, p->wdp, 4);
-		p->cycle_count = 2048 << wdp;
-		p->cycle_count = (p->cycle_count * avr->frequency) / 128000;
-		if (avr_regbit_get(avr, p->wde)) {
-			AVR_LOG(avr, LOG_TRACE, "WATCHDOG: reset to %d cycles @ 128kz (* %d) = %d CPU cycles)\n",
-					2048 << wdp, 1 << wdp, (int)p->cycle_count);
-			avr_cycle_timer_register(avr, p->cycle_count, avr_watchdog_timer, p);
+	uint8_t old_wde = avr_regbit_get(avr, p->wde);
+	uint8_t old_wdie = avr_regbit_get(avr, p->watchdog.enable);
+
+	uint8_t was_enabled = (old_wde || old_wdie);
+
+	uint8_t old_v = avr->data[addr]; // allow gdb to see write...
+	avr_core_watch_write(avr, addr, v);
+
+	if (avr_regbit_get(avr, p->wdce)) {
+		uint8_t old_wdp = avr_regbit_get_array(avr, p->wdp, 4);
+
+		// wdrf (watchdog reset flag) must be cleared before wde can be cleared.
+		if (avr_regbit_get(avr, p->wdrf))
+			avr_regbit_set(avr, p->wde);
+
+		avr_watchdog_set_cycle_count_and_timer(avr, p, was_enabled, old_wdp);
+	} else {
+		/* easier to change only what we need rather than check and reset 
+		 * locked/read-only bits.
+		 */
+		avr->data[addr] = old_v;
+		
+		uint8_t wdce_v = avr_regbit_from_value(avr, p->wdce, v);
+		uint8_t wde_v = avr_regbit_from_value(avr, p->wde, v);
+
+		if (wdce_v && wde_v) {
+			avr_regbit_set(avr, p->wdce);
+
+			avr_cycle_timer_register(avr, 4, avr_wdce_clear, p);
 		} else {
-			AVR_LOG(avr, LOG_TRACE, "WATCHDOG: disabled\n");
-			avr_cycle_timer_cancel(avr, avr_watchdog_timer, p);
+			if (wde_v) // wde can be set but not cleared
+				avr_regbit_set(avr, p->wde);
+
+			avr_regbit_setto_raw(avr, p->watchdog.enable, v);
+
+			avr_watchdog_set_cycle_count_and_timer(avr, p, was_enabled, -1);
 		}
-	} else {
-		// reset old values
-		avr_regbit_setto(avr, p->wde, wde_o);
-		for (int i = 0; i < 4; i++)
-			avr_regbit_setto(avr, p->wdp[i], wdp_o[i]);
-		v = avr->data[p->wdce.reg];
 	}
-	avr_core_watch_write(avr, addr, v);
 }
 
 /*
@@ -97,7 +150,7 @@ static int avr_watchdog_ioctl(struct avr_io_t * port, uint32_t ctl, void * io_pa
 	int res = -1;
 
 	if (ctl == AVR_IOCTL_WATCHDOG_RESET) {
-		if (avr_regbit_get(p->io.avr, p->wde))
+		if (avr_regbit_get(p->io.avr, p->wde) || avr_regbit_get(p->io.avr, p->watchdog.enable))
 			avr_cycle_timer_register(p->io.avr, p->cycle_count, avr_watchdog_timer, p);
 		res = 0;
 	}
@@ -105,10 +158,45 @@ static int avr_watchdog_ioctl(struct avr_io_t * port, uint32_t ctl, void * io_pa
 	return res;
 }
 
+static void avr_watchdog_irq_notify(
+		struct avr_irq_t * irq,
+		uint32_t value,
+		void * param)
+{
+	avr_watchdog_t * p = (avr_watchdog_t *)param;
+	avr_t * avr = p->io.avr;
+
+	/* interrupt handling calls this twice...
+	 * first when raised (during queuing), value = 1
+	 * again when cleared (after servicing), value = 0
+	 */
+
+	if (!value && avr_regbit_get(avr, p->watchdog.raised)) {
+		avr_regbit_clear(avr, p->watchdog.enable);
+	}
+}
+		
 static void avr_watchdog_reset(avr_io_t * port)
 {
-//	avr_watchdog_t * p = (avr_watchdog_t *)port;
+	avr_watchdog_t * p = (avr_watchdog_t *)port;
+	avr_t * avr = p->io.avr;
 
+	if (p->reset_context.wdrf) {
+		/*
+		 * if watchdog reset kicked, then watchdog gets restated at 
+		 * fastest interval
+		 */
+
+		avr->run = p->reset_context.avr_run;
+
+		avr_regbit_set(avr, p->wde);
+		avr_regbit_set(avr, p->wdrf);
+		avr_regbit_set_array_from_value(avr, p->wdp, 4, 0);
+		
+		avr_watchdog_set_cycle_count_and_timer(avr, p, 0, 0);
+	}
+
+	avr_irq_register_notify(&p->watchdog.irq, avr_watchdog_irq_notify, p);
 }
 
 static	avr_io_t	_io = {
@@ -125,5 +213,7 @@ void avr_watchdog_init(avr_t * avr, avr_watchdog_t * p)
 	avr_register_vector(avr, &p->watchdog);
 
 	avr_register_io_write(avr, p->wdce.reg, avr_watchdog_write, p);
+
+	p->reset_context.wdrf = 0;
 }
 
diff --git a/simavr/sim/avr_watchdog.h b/simavr/sim/avr_watchdog.h
index eb816a4..da86371 100644
--- a/simavr/sim/avr_watchdog.h
+++ b/simavr/sim/avr_watchdog.h
@@ -41,6 +41,11 @@ typedef struct avr_watchdog_t {
 	avr_int_vector_t watchdog;	// watchdog interrupt
 
 	avr_cycle_count_t	cycle_count;
+
+	struct {
+		uint8_t		wdrf;		// saved watchdog reset flag
+		avr_run_t	avr_run;	// restored during reset
+	} reset_context;
 } avr_watchdog_t;
 
 /* takes no parameter */
diff --git a/simavr/sim/sim_avr.c b/simavr/sim/sim_avr.c
index 5138cd5..6c985cc 100644
--- a/simavr/sim/sim_avr.c
+++ b/simavr/sim/sim_avr.c
@@ -90,7 +90,6 @@ int avr_init(avr_t * avr)
 	// set default (non gdb) fast callbacks
 	avr->run = avr_callback_run_raw;
 	avr->sleep = avr_callback_sleep_raw;
-	avr->state = cpu_Running;
 	// number of address bytes to push/pull on/off the stack
 	avr->address_size = avr->eind ? 3 : 2;
 	avr->log = 1;
@@ -121,7 +120,9 @@ void avr_reset(avr_t * avr)
 {
 	AVR_LOG(avr, LOG_TRACE, "%s reset\n", avr->mmcu);
 
-	memset(avr->data, 0x0, avr->ramend + 1);
+	avr->state = cpu_Running;
+	for(int i = 0x20; i <= MAX_IOs; i++)
+		avr->data[i] = 0;
 	_avr_sp_set(avr, avr->ramend);
 	avr->pc = 0;
 	for (int i = 0; i < 8; i++)
diff --git a/simavr/sim/sim_avr.h b/simavr/sim/sim_avr.h
index aa0226b..7a93b4d 100644
--- a/simavr/sim/sim_avr.h
+++ b/simavr/sim/sim_avr.h
@@ -132,6 +132,9 @@ struct avr_trace_data_t {
 	uint32_t	touched[256 / 32];	// debug
 };
 
+typedef void (*avr_run_t)(
+		struct avr_t * avr);
+
 /*
  * Main AVR instance. Some of these fields are set by the AVR "Core" definition files
  * the rest is runtime data (as little as possible)
@@ -191,7 +194,7 @@ typedef struct avr_t {
 	 * it can, and a "gdb" mode that also watchouts for gdb events
 	 * and is a little bit slower.
 	 */
-	void (*run)(struct avr_t * avr);
+	avr_run_t	run;
 
 	/*!
 	 * Sleep default behaviour.
diff --git a/simavr/sim/sim_regbit.h b/simavr/sim/sim_regbit.h
index 96941c2..82df38d 100644
--- a/simavr/sim/sim_regbit.h
+++ b/simavr/sim/sim_regbit.h
@@ -88,6 +88,19 @@ static inline uint8_t avr_regbit_get(avr_t * avr, avr_regbit_t rb)
 	return (avr->data[a] >> rb.bit) & rb.mask;
 }
 
+/*
+ * Using regbit from value eliminates some of the 
+ * set to test then clear register operations.
+ * makes cheking register bits before setting easier.
+ */
+static inline uint8_t avr_regbit_from_value(avr_t * avr, avr_regbit_t rb, uint8_t value)
+{
+	uint8_t a = rb.reg;
+	if (!a)
+		return 0;
+	return (value >> rb.bit) & rb.mask;
+}
+
 /*
  * Return the bit(s) 'in position' instead of zero based
  */
@@ -126,6 +139,22 @@ static inline uint8_t avr_regbit_get_array(avr_t * avr, avr_regbit_t *rb, int co
 	return res;
 }
 
+/*
+ * Does the reverse of avr_regbit_get_array
+ */
+static inline void avr_regbit_set_array_from_value(
+	avr_t * avr, 
+	avr_regbit_t * rb, 
+	uint8_t count, 
+	uint8_t value)
+{
+	int i;
+	for (i = 0; i < count; i++, rb++) if (rb->reg) {
+		uint8_t rbv = (value >> (count - i)) & 1;
+		avr_regbit_setto(avr, *rb, rbv);
+	}
+}
+
 #define AVR_IO_REGBIT(_io, _bit) { . reg = (_io), .bit = (_bit), .mask = 1 }
 #define AVR_IO_REGBITS(_io, _bit, _mask) { . reg = (_io), .bit = (_bit), .mask = (_mask) }