bitbang: Convert logging to AVR_LOG()

[simavr] / simavr / sim / sim_irq.c

diff --git a/simavr/sim/sim_irq.c b/simavr/sim/sim_irq.c
index 92c2c17..2b346c7 100644 (file)
--- a/simavr/sim/sim_irq.c
+++ b/simavr/sim/sim_irq.c
@@ -34,24 +34,68 @@ typedef struct avr_irq_hook_t {
        void * param;                           // "notify" parameter
 } avr_irq_hook_t;
 
-void avr_init_irq(avr_irq_t * irq, uint32_t base, uint32_t count)
+static void
+_avr_irq_pool_add(
+               avr_irq_pool_t * pool,
+               avr_irq_t * irq)
+{
+       if ((pool->count & 0xf) == 0) {
+               pool->irq = (avr_irq_t**)realloc(pool->irq,
+                               (pool->count + 16) * sizeof(avr_irq_t *));
+       }
+       pool->irq[pool->count++] = irq;
+       irq->pool = pool;
+}
+
+static void
+_avr_irq_pool_remove(
+               avr_irq_pool_t * pool,
+               avr_irq_t * irq)
+{
+       for (int i = 0; i < pool->count; i++)
+               if (pool->irq[i] == irq) {
+                       pool->irq[i] = 0;
+                       return;
+               }
+}
+
+void
+avr_init_irq(
+               avr_irq_pool_t * pool,
+               avr_irq_t * irq,
+               uint32_t base,
+               uint32_t count,
+               const char ** names /* optional */)
 {
        memset(irq, 0, sizeof(avr_irq_t) * count);
 
-       for (int i = 0; i < count; i++)
+       for (int i = 0; i < count; i++) {
                irq[i].irq = base + i;
+               irq[i].flags = IRQ_FLAG_INIT;
+               if (pool)
+                       _avr_irq_pool_add(pool, &irq[i]);
+               if (names && names[i])
+                       irq[i].name = strdup(names[i]);
+       }
 }
 
-avr_irq_t * avr_alloc_irq(uint32_t base, uint32_t count)
+avr_irq_t *
+avr_alloc_irq(
+               avr_irq_pool_t * pool,
+               uint32_t base,
+               uint32_t count,
+               const char ** names /* optional */)
 {
        avr_irq_t * irq = (avr_irq_t*)malloc(sizeof(avr_irq_t) * count);
-       avr_init_irq(irq, base, count);
+       avr_init_irq(pool, irq, base, count, names);
        for (int i = 0; i < count; i++)
                irq[i].flags |= IRQ_FLAG_ALLOC; 
        return irq;
 }
 
-static avr_irq_hook_t * _avr_alloc_irq_hook(avr_irq_t * irq)
+static avr_irq_hook_t *
+_avr_alloc_irq_hook(
+               avr_irq_t * irq)
 {
        avr_irq_hook_t *hook = malloc(sizeof(avr_irq_hook_t));
        memset(hook, 0, sizeof(avr_irq_hook_t));
@@ -60,26 +104,39 @@ static avr_irq_hook_t * _avr_alloc_irq_hook(avr_irq_t * irq)
        return hook;
 }
 
-void avr_free_irq(avr_irq_t * irq, uint32_t count)
+void
+avr_free_irq(
+               avr_irq_t * irq,
+               uint32_t count)
 {
        if (!irq || !count)
                return;
        for (int i = 0; i < count; i++) {
+               avr_irq_t * iq = irq + i;
+               if (iq->pool)
+                       _avr_irq_pool_remove(iq->pool, iq);
+               if (iq->name)
+                       free((char*)iq->name);
+               iq->name = NULL;
                // purge hooks
-               avr_irq_hook_t *hook = irq->hook;
+               avr_irq_hook_t *hook = iq->hook;
                while (hook) {
                        avr_irq_hook_t * next = hook->next;
                        free(hook);
                        hook = next;
                }
-               irq->hook = NULL;
+               iq->hook = NULL;
        }
        // if that irq list was allocated by us, free it
        if (irq->flags & IRQ_FLAG_ALLOC)
                free(irq);
 }
 
-void avr_irq_register_notify(avr_irq_t * irq, avr_irq_notify_t notify, void * param)
+void
+avr_irq_register_notify(
+               avr_irq_t * irq,
+               avr_irq_notify_t notify,
+               void * param)
 {
        if (!irq || !notify)
                return;
@@ -95,13 +152,45 @@ void avr_irq_register_notify(avr_irq_t * irq, avr_irq_notify_t notify, void * pa
        hook->param = param;
 }
 
-void avr_raise_irq(avr_irq_t * irq, uint32_t value)
+void
+avr_irq_unregister_notify(
+               avr_irq_t * irq,
+               avr_irq_notify_t notify,
+               void * param)
+{
+       avr_irq_hook_t *hook, *prev;
+       if (!irq || !notify)
+               return;
+
+       hook = irq->hook;
+       prev = NULL;
+       while (hook) {
+               if (hook->notify == notify && hook->param == param) {
+                       if ( prev )
+                               prev->next = hook->next;
+                       else
+                               irq->hook = hook->next;
+                       free(hook);
+                       return;
+               }
+               prev = hook;
+               hook = hook->next;
+       }
+}
+
+void
+avr_raise_irq(
+               avr_irq_t * irq,
+               uint32_t value)
 {
        if (!irq)
                return ;
        uint32_t output = (irq->flags & IRQ_FLAG_NOT) ? !value : value;
-       if (irq->value == output && (irq->flags & IRQ_FLAG_FILTERED))
+       // if value is the same but it's the first time, raise it anyway
+       if (irq->value == output &&
+                       (irq->flags & IRQ_FLAG_FILTERED) && !(irq->flags & IRQ_FLAG_INIT))
                return;
+       irq->flags &= ~IRQ_FLAG_INIT;
        avr_irq_hook_t *hook = irq->hook;
        while (hook) {
                avr_irq_hook_t * next = hook->next;
@@ -122,10 +211,13 @@ void avr_raise_irq(avr_irq_t * irq, uint32_t value)
        irq->value = output;
 }
 
-void avr_connect_irq(avr_irq_t * src, avr_irq_t * dst)
+void
+avr_connect_irq(
+               avr_irq_t * src,
+               avr_irq_t * dst)
 {
        if (!src || !dst || src == dst) {
-               printf("avr_connect_irq invalid irq %p/%p", src, dst);
+               fprintf(stderr, "error: %s invalid irq %p/%p", __FUNCTION__, src, dst);
                return;
        }
        avr_irq_hook_t *hook = src->hook;
@@ -137,3 +229,30 @@ void avr_connect_irq(avr_irq_t * src, avr_irq_t * dst)
        hook = _avr_alloc_irq_hook(src);
        hook->chain = dst;
 }
+
+void
+avr_unconnect_irq(
+               avr_irq_t * src,
+               avr_irq_t * dst)
+{
+       avr_irq_hook_t *hook, *prev;
+
+       if (!src || !dst || src == dst) {
+               fprintf(stderr, "error: %s invalid irq %p/%p", __FUNCTION__, src, dst);
+               return;
+       }
+       hook = src->hook;
+       prev = NULL;
+       while (hook) {
+               if (hook->chain == dst) {
+                       if ( prev )
+                               prev->next = hook->next;
+                       else
+                               src->hook = hook->next;
+                       free(hook);
+                       return;
+               }
+               prev = hook;
+               hook = hook->next;
+       }
+}