X-Git-Url: http://git.rot13.org/?a=blobdiff_plain;f=simavr%2Fsim%2Fsim_interrupts.c;h=a2f3758709dad1f15c3844a1f5e5591942c13a23;hb=54c68474e17e329deb2d729cc14ecd27e1773d1b;hp=cc4736f8f4d98c5453eee5cd61c62bd3c51f3156;hpb=1b82822bf8b61bd4aa84882112f9f4690e865e15;p=simavr

diff --git a/simavr/sim/sim_interrupts.c b/simavr/sim/sim_interrupts.c
index cc4736f..a2f3758 100644
--- a/simavr/sim/sim_interrupts.c
+++ b/simavr/sim/sim_interrupts.c
@@ -1,7 +1,7 @@
 /*
 	sim_interrupts.c
 
-	Copyright 2008, 2009 Michel Pollet <buserror@gmail.com>
+	Copyright 2008-2012 Michel Pollet <buserror@gmail.com>
 
  	This file is part of simavr.
 
@@ -23,58 +23,101 @@
 #include <stdlib.h>
 #include <stdio.h>
 #include <string.h>
+#include <strings.h>
 #include "sim_interrupts.h"
+#include "sim_avr.h"
 #include "sim_core.h"
 
-void avr_register_vector(avr_t *avr, avr_int_vector_t * vector)
+// modulo a cursor value on the pending interrupt fifo
+#define INT_FIFO_SIZE (sizeof(table->pending) / sizeof(avr_int_vector_t *))
+#define INT_FIFO_MOD(_v) ((_v) &  (INT_FIFO_SIZE - 1))
+
+void
+avr_interrupt_reset(
+		avr_t * avr )
 {
-	if (vector->vector) {
-		vector->irq.irq = vector->vector;
-		avr->vector[vector->vector] = vector;
-		if (vector->trace)
-			printf("%s register vector %d (enabled %04x:%d)\n", __FUNCTION__, vector->vector, vector->enable.reg, vector->enable.bit);
+	avr_int_table_p table = &avr->interrupts;
+	memset(table, 0, sizeof(*table));
+}
 
-		if (!vector->enable.reg)
-			printf("avr_register_vector: No 'enable' bit on vector %d !\n", vector->vector);
-	}
+void
+avr_register_vector(
+		avr_t *avr,
+		avr_int_vector_t * vector)
+{
+	if (!vector->vector)
+		return;
+
+	avr_int_table_p table = &avr->interrupts;
+
+	vector->irq.irq = vector->vector;
+	table->vector[table->vector_count++] = vector;
+	if (vector->trace)
+		printf("%s register vector %d (enabled %04x:%d)\n", __FUNCTION__, vector->vector, vector->enable.reg, vector->enable.bit);
+
+	if (!vector->enable.reg)
+		AVR_LOG(avr, LOG_WARNING, "INT: avr_register_vector: No 'enable' bit on vector %d !\n", vector->vector);
 }
 
-int avr_has_pending_interrupts(avr_t * avr)
+int
+avr_has_pending_interrupts(
+		avr_t * avr)
 {
-	return avr->pending[0] || avr->pending[1];
+	avr_int_table_p table = &avr->interrupts;
+	return table->pending_r != table->pending_w;
 }
 
-int avr_is_interrupt_pending(avr_t * avr, avr_int_vector_t * vector)
+int
+avr_is_interrupt_pending(
+		avr_t * avr,
+		avr_int_vector_t * vector)
 {
-	return avr->pending[vector->vector >> 5] & (1 << (vector->vector & 0x1f));
+	return vector->pending;
 }
 
-int avr_is_interrupt_enabled(avr_t * avr, avr_int_vector_t * vector)
+int
+avr_is_interrupt_enabled(
+		avr_t * avr,
+		avr_int_vector_t * vector)
 {
 	return avr_regbit_get(avr, vector->enable);
 }
 
-int avr_raise_interrupt(avr_t * avr, avr_int_vector_t * vector)
+int
+avr_raise_interrupt(
+		avr_t * avr,
+		avr_int_vector_t * vector)
 {
 	if (!vector || !vector->vector)
 		return 0;
 	if (vector->trace)
 		printf("%s raising %d (enabled %d)\n", __FUNCTION__, vector->vector, avr_regbit_get(avr, vector->enable));
+	if (vector->pending) {
+		if (vector->trace)
+			printf("%s trying to double raise %d (enabled %d)\n", __FUNCTION__, vector->vector, avr_regbit_get(avr, vector->enable));
+		return 0;
+	}
 	// always mark the 'raised' flag to one, even if the interrupt is disabled
-	// this allow "pooling" for the "raised" flag, like for non-interrupt
+	// this allow "polling" for the "raised" flag, like for non-interrupt
 	// driven UART and so so. These flags are often "write one to clear"
 	if (vector->raised.reg)
 		avr_regbit_set(avr, vector->raised);
 
-	// Mark the interrupt as pending
-	avr->pending[vector->vector >> 5] |= (1 << (vector->vector & 0x1f));
 	avr_raise_irq(&vector->irq, 1);
 
 	// If the interrupt is enabled, attempt to wake the core
 	if (avr_regbit_get(avr, vector->enable)) {
-		if (!avr->pending_wait)
-			avr->pending_wait = 1;		// latency on interrupts ??
-		if (avr->state != cpu_Running) {
+		// Mark the interrupt as pending
+		vector->pending = 1;
+
+		avr_int_table_p table = &avr->interrupts;
+
+		table->pending[table->pending_w++] = vector;
+		table->pending_w = INT_FIFO_MOD(table->pending_w);
+
+		if (!table->pending_wait)
+			table->pending_wait = 1;		// latency on interrupts ??
+		if (avr->state == cpu_Sleeping) {
 			if (vector->trace)
 				printf("Waking CPU due to interrupt\n");
 			avr->state = cpu_Running;	// in case we were sleeping
@@ -84,74 +127,105 @@ int avr_raise_interrupt(avr_t * avr, avr_int_vector_t * vector)
 	return 1;
 }
 
-void avr_clear_interrupt(avr_t * avr, int v)
+void
+avr_clear_interrupt(
+		avr_t * avr,
+		avr_int_vector_t * vector)
 {
-	avr_int_vector_t * vector = avr->vector[v];
-	avr->pending[v >> 5] &= ~(1 << (v & 0x1f));
 	if (!vector)
 		return;
 	if (vector->trace)
 		printf("%s cleared %d\n", __FUNCTION__, vector->vector);
+	vector->pending = 0;
 	avr_raise_irq(&vector->irq, 0);
-	if (vector->raised.reg)
+	if (vector->raised.reg && !vector->raise_sticky)
 		avr_regbit_clear(avr, vector->raised);
 }
 
-int avr_clear_interupt_if(avr_t * avr, avr_int_vector_t * vector, uint8_t old)
+int
+avr_clear_interrupt_if(
+		avr_t * avr,
+		avr_int_vector_t * vector,
+		uint8_t old)
 {
 	if (avr_regbit_get(avr, vector->raised)) {
-		avr_clear_interrupt(avr, vector->vector);
-		avr_regbit_clear(avr, vector->raised);
+		avr_clear_interrupt(avr, vector);
 		return 1;
 	}
 	avr_regbit_setto(avr, vector->raised, old);
 	return 0;
 }
 
-avr_irq_t * avr_get_interupt_irq(avr_t * avr, uint8_t v)
+avr_irq_t *
+avr_get_interrupt_irq(
+		avr_t * avr,
+		uint8_t v)
 {
-	avr_int_vector_t * vector = avr->vector[v];
-	return vector ? &vector->irq : NULL;
+	avr_int_table_p table = &avr->interrupts;
+	for (int i = 0; i < table->vector_count; i++)
+		if (table->vector[i]->vector == v)
+			return &table->vector[i]->irq;
+	return NULL;
 }
 
 /*
- * check wether interrupts are pending. I so, check if the interrupt "latency" is reached,
+ * check whether interrupts are pending. If so, check if the interrupt "latency" is reached,
  * and if so triggers the handlers and jump to the vector.
  */
-void avr_service_interrupts(avr_t * avr)
+void
+avr_service_interrupts(
+		avr_t * avr)
 {
 	if (!avr->sreg[S_I])
 		return;
 
-	if (avr_has_pending_interrupts(avr)) {
-		if (avr->pending_wait) {
-			avr->pending_wait--;
-			if (avr->pending_wait == 0) {
-				int done = 0;
-				for (int bi = 0; bi < 2 && !done; bi++) if (avr->pending[bi]) {
-					for (int ii = 0; ii < 32 && !done; ii++)
-						if (avr->pending[bi] & (1 << ii)) {
-
-							int v = (bi * 32) + ii;	// vector
-							avr_int_vector_t * vector = avr->vector[v];
-							// if that single interupt is masked, ignore it and continue
-							if (vector && !avr_regbit_get(avr, vector->enable))
-								continue;
-							if (vector && vector->trace)
-								printf("%s calling %d\n", __FUNCTION__, v);
-							_avr_push16(avr, avr->pc >> 1);
-							avr->sreg[S_I] = 0;
-							avr->pc = v * avr->vector_size;
-
-							avr_clear_interrupt(avr, v);
-							done++;
-							break;
-						}
-					break;
-				}
-			}
-		} else
-			avr->pending_wait = 2;	// for next one...
+	if (!avr_has_pending_interrupts(avr))
+		return;
+
+	avr_int_table_p table = &avr->interrupts;
+
+	if (!table->pending_wait) {
+		table->pending_wait = 2;	// for next one...
+		return;
+	}
+	table->pending_wait--;
+	if (table->pending_wait)
+		return;
+
+	// how many are pending...
+	int cnt = table->pending_w > table->pending_r ?
+			table->pending_w - table->pending_r :
+			(table->pending_w + INT_FIFO_SIZE) - table->pending_r;
+	// locate the highest priority one
+	int min = 0xff;
+	int mini = 0;
+	for (int ii = 0; ii < cnt; ii++) {
+		int vi = INT_FIFO_MOD(table->pending_r + ii);
+		avr_int_vector_t * v = table->pending[vi];
+		if (v->vector < min) {
+			min = v->vector;
+			mini = vi;
+		}
+	}
+	avr_int_vector_t * vector = table->pending[mini];
+
+	// now move the one at the front of the fifo in the slot of
+	// the one we service
+	table->pending[mini] = table->pending[table->pending_r++];
+	table->pending_r = INT_FIFO_MOD(table->pending_r);
+
+	// if that single interrupt is masked, ignore it and continue
+	// could also have been disabled, or cleared
+	if (!avr_regbit_get(avr, vector->enable) || !vector->pending) {
+		vector->pending = 0;
+	} else {
+		if (vector && vector->trace)
+			printf("%s calling %d\n", __FUNCTION__, (int)vector->vector);
+		_avr_push16(avr, avr->pc >> 1);
+		avr->sreg[S_I] = 0;
+		avr->pc = vector->vector * avr->vector_size;
+
+		avr_clear_interrupt(avr, vector);
 	}
 }