fix typos in comments.

[simavr] / simavr / sim / sim_interrupts.c

diff --git a/simavr/sim/sim_interrupts.c b/simavr/sim/sim_interrupts.c
index bf620bb..e4a2683 100644 (file)
--- a/simavr/sim/sim_interrupts.c
+++ b/simavr/sim/sim_interrupts.c
@@ -23,13 +23,21 @@
 #include <stdlib.h>
 #include <stdio.h>
 #include <string.h>
+#include <strings.h>
 #include "sim_interrupts.h"
 #include "sim_core.h"
 
 void avr_register_vector(avr_t *avr, avr_int_vector_t * vector)
 {
-       if (vector->vector)
+       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);
+
+               if (!vector->enable.reg)
+                       printf("avr_register_vector: No 'enable' bit on vector %d !\n", vector->vector);
+       }
 }
 
 int avr_has_pending_interrupts(avr_t * avr)
@@ -42,27 +50,34 @@ int avr_is_interrupt_pending(avr_t * avr, avr_int_vector_t * vector)
        return avr->pending[vector->vector >> 5] & (1 << (vector->vector & 0x1f));
 }
 
+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)
 {
        if (!vector || !vector->vector)
                return 0;
-//     printf("%s raising %d\n", __FUNCTION__, vector->vector);
-       // always mark the 'raised' flag to one, even if the interuot is disabled
+       if (vector->trace)
+               printf("%s raising %d (enabled %d)\n", __FUNCTION__, vector->vector, avr_regbit_get(avr, vector->enable));
+       // always mark the 'raised' flag to one, even if the interrupt is disabled
        // this allow "pooling" 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);
-       if (vector->enable.reg) {
-               if (!avr_regbit_get(avr, vector->enable))
-                       return 0;
-       }
-       if (!avr_is_interrupt_pending(avr, vector)) {
-               if (!avr->pending_wait)
-                       avr->pending_wait = 2;          // latency on interrupts ??
-               avr->pending[vector->vector >> 5] |= (1 << (vector->vector & 0x1f));
 
+       // 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) {
-               //      printf("Waking CPU due to interrupt\n");
+                       if (vector->trace)
+                               printf("Waking CPU due to interrupt\n");
                        avr->state = cpu_Running;       // in case we were sleeping
                }
        }
@@ -76,13 +91,32 @@ void avr_clear_interrupt(avr_t * avr, int v)
        avr->pending[v >> 5] &= ~(1 << (v & 0x1f));
        if (!vector)
                return;
-//     printf("%s cleared %d\n", __FUNCTION__, vector->vector);
+       if (vector->trace)
+               printf("%s cleared %d\n", __FUNCTION__, vector->vector);
+       avr_raise_irq(&vector->irq, 0);
        if (vector->raised.reg)
                avr_regbit_clear(avr, vector->raised);
 }
 
+int avr_clear_interupt_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);
+               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_int_vector_t * vector = avr->vector[v];
+       return vector ? &vector->irq : 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)
@@ -95,23 +129,30 @@ void avr_service_interrupts(avr_t * avr)
                        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
-
-                                               //      printf("%s calling %d\n", __FUNCTION__, v);
+                               for (int bi = 0; bi < 2 && !done; bi++)
+                                       if (avr->pending[bi]) {
+                                               uint32_t map = avr->pending[bi];
+                                               while (map) {
+                                                       int bit = ffs(map)-1;
+                                                       int v = (bi * 32) + bit;        // 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)) {
+                                                               map &= ~(1 << bit);
+                                                               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++;
+                                                       done = 1;
                                                        break;
                                                }
-                                       break;
-                               }
+                                               break;
+                                       }
                        }
                } else
                        avr->pending_wait = 2;  // for next one...