Re: [Qemu-devel] SH: Improve the interrupt controller

[takasi-y]

Hi, Vladimir.

I managed to make the code working.
r2d boot, SCI and CF working, and /proc/interrupts increases.

Essential points modified are below.
1. sh_intc_init() caller/callee mismatch.
2. nobody calls sh_intc_set_irl_priorities()
3. INTC_MODE_DUAL_SET/CLR swapped
I'm not sure 3 in your patch is on purpose or not.

Attached patch is a diff against rev#6563.
This is yours + my small fixes, which are..
- Above three
- sh_intc_set_irl_priorities() has switched to sh_intc_init_irl_priorities().
- sh_intc_set_irl()'s enable hack removed.
- indent,tab/space,brace changed (to what looks like code around)
- reduce INTC_A7() usage
- some others.

/yoshii
---
 hw/sh7750.c         |    9 ++-
 hw/sh_intc.c        |  156 ++++++++++++++++++++++++++++++++++-----------------
 hw/sh_intc.h        |    8 ++-
 target-sh4/cpu.h    |    1 +
 target-sh4/helper.c |   10 +++-
 5 files changed, 127 insertions(+), 57 deletions(-)

diff --git a/hw/sh7750.c b/hw/sh7750.c
index 423c43f..57a5d7d 100644
--- a/hw/sh7750.c
+++ b/hw/sh7750.c
@@ -732,7 +732,7 @@ SH7750State *sh7750_init(CPUSH4State * cpu)
     cpu_register_physical_memory(0xf0000000, 0x08000000,
                                 sh7750_mm_cache_and_tlb);
 
-    sh_intc_init(&s->intc, NR_SOURCES,
+    sh_intc_init(&s->intc, 0x1fd00000, NR_SOURCES,
                 _INTC_ARRAY(mask_registers),
                 _INTC_ARRAY(prio_registers));
 
@@ -806,7 +806,8 @@ SH7750State *sh7750_init(CPUSH4State * cpu)
 
 qemu_irq sh7750_irl(SH7750State *s)
 {
-    sh_intc_toggle_source(sh_intc_source(&s->intc, IRL), 1, 0); /* enable */
-    return qemu_allocate_irqs(sh_intc_set_irl, sh_intc_source(&s->intc, IRL),
-                               1)[0];
+    struct intc_source *irl = sh_intc_source(&s->intc, IRL);
+
+    sh_intc_init_irl_priorities(irl);
+    return qemu_allocate_irqs(sh_intc_set_irl, irl, 1)[0];
 }
diff --git a/hw/sh_intc.c b/hw/sh_intc.c
index f4138fd..7641b11 100644
--- a/hw/sh_intc.c
+++ b/hw/sh_intc.c
@@ -18,6 +18,8 @@
 
 #define INTC_A7(x) ((x) & 0x1fffffff)
 
+#define ICR0_LVLMODE (1 << 21)
+
 void sh_intc_toggle_source(struct intc_source *source,
                           int enable_adj, int assert_adj)
 {
@@ -83,30 +85,42 @@ static void sh_intc_set_irq (void *opaque, int n, int level)
     sh_intc_toggle_source(source, 0, -1);
 }
 
-int sh_intc_get_pending_vector(struct intc_desc *desc, int imask)
+int sh_intc_get_pending_vector(struct intc_desc *desc, int *priority)
 {
     unsigned int i;
+    unsigned highest_priority = 0;
+    int found = -1;
 
-    /* slow: use a linked lists of pending sources instead */
-    /* wrong: take interrupt priority into account (one list per priority) */
+    /* slow: use a linked list of pending sources instead */
 
-    if (imask == 0x0f) {
-        return -1; /* FIXME, update code to include priority per source */
-    }
+    if (*priority == 0x0f)
+        return -1;
 
     for (i = 0; i < desc->nr_sources; i++) {
         struct intc_source *source = desc->sources + i;
 
-       if (source->pending) {
-#ifdef DEBUG_INTC_SOURCES
-            printf("sh_intc: (%d) returning interrupt source 0x%x\n",
-                  desc->pending, source->vect);
-#endif
-            return source->vect;
+       if (source->pending && source->priority > highest_priority) {
+           highest_priority = source->priority;
+           found = i;
        }
     }
 
-    assert(0);
+    if (found != -1  && highest_priority > *priority) {
+       struct intc_source *source = desc->sources + found;
+       
+#ifdef DEBUG_INTC_SOURCES
+       printf("sh_intc: (%d) returning interrupt source 0x%x\n",
+              desc->pending, source->vect);
+#endif
+       if (desc->icr0 & ICR0_LVLMODE)
+           sh_intc_toggle_source(source, 0, -1);
+
+       /* Priority in intc is 5 bits, whereas processor knows only 4 bits. */
+       *priority = highest_priority >> 1;
+       
+       return source->vect;
+    }
+    return -1;
 }
 
 #define INTC_MODE_NONE       0
@@ -186,7 +200,7 @@ static void sh_intc_locate(struct intc_desc *desc,
 }
 
 static void sh_intc_toggle_mask(struct intc_desc *desc, intc_enum id,
-                               int enable, int is_group)
+                               int enable, int priority, int is_group)
 {
     struct intc_source *source = desc->sources + id;
 
@@ -200,8 +214,11 @@ static void sh_intc_toggle_mask(struct intc_desc *desc, 
intc_enum id,
        return;
     }
 
-    if (source->vect)
+    if (source->vect) {
         sh_intc_toggle_source(source, enable ? 1 : -1, 0);
+        if (priority != -1)
+            source->priority = priority;
+    }
 
 #ifdef DEBUG_INTC
     else {
@@ -210,7 +227,7 @@ static void sh_intc_toggle_mask(struct intc_desc *desc, 
intc_enum id,
 #endif
 
     if ((is_group || !source->vect) && source->next_enum_id) {
-        sh_intc_toggle_mask(desc, source->next_enum_id, enable, 1);
+        sh_intc_toggle_mask(desc, source->next_enum_id, enable, priority, 1);
     }
 
 #ifdef DEBUG_INTC
@@ -232,6 +249,8 @@ static uint32_t sh_intc_read(void *opaque, 
target_phys_addr_t offset)
 #ifdef DEBUG_INTC
     printf("sh_intc_read 0x%lx\n", (unsigned long) offset);
 #endif
+    if (offset == desc->base)
+       return desc->icr0;
 
     sh_intc_locate(desc, (unsigned long)offset, &valuep, 
                   &enum_ids, &first, &width, &mode);
@@ -254,6 +273,11 @@ static void sh_intc_write(void *opaque, target_phys_addr_t 
offset,
     printf("sh_intc_write 0x%lx 0x%08x\n", (unsigned long) offset, value);
 #endif
 
+    if (offset == desc->base) {
+       desc->icr0 = value;
+       return; 
+    }
+
     sh_intc_locate(desc, (unsigned long)offset, &valuep, 
                   &enum_ids, &first, &width, &mode);
 
@@ -265,7 +289,9 @@ static void sh_intc_write(void *opaque, target_phys_addr_t 
offset,
     }
 
     for (k = 0; k <= first; k++) {
-        mask = ((1 << width) - 1) << ((first - k) * width);
+        int priority = -1;      
+        unsigned shift = ((first - k) * width);
+        mask = ((1 << width) - 1) << shift;
 
        if ((*valuep & mask) == (value & mask))
             continue;
@@ -273,7 +299,15 @@ static void sh_intc_write(void *opaque, target_phys_addr_t 
offset,
        printf("k = %d, first = %d, enum = %d, mask = 0x%08x\n", 
               k, first, enum_ids[k], (unsigned int)mask);
 #endif
-        sh_intc_toggle_mask(desc, enum_ids[k], value & mask, 0);
+       if (mode & INTC_MODE_IS_PRIO) {
+           assert (width == 4 || width == 8);
+           priority = (value & mask) >> shift;
+           if (width == 8)
+               priority &= 0x1f;
+           else if (width == 4)
+               priority <<= 1;
+       }
+       sh_intc_toggle_mask(desc, enum_ids[k], value & mask, priority, 0);
     }
 
     *valuep = value;
@@ -320,19 +354,18 @@ static void sh_intc_register_source(struct intc_desc 
*desc,
                                    int nr_groups)
 {
     unsigned int i, k;
-    struct intc_source *s;
+    struct intc_source *s = sh_intc_source(desc, source);
+    assert(s);
 
     if (desc->mask_regs) {
         for (i = 0; i < desc->nr_mask_regs; i++) {
            struct intc_mask_reg *mr = desc->mask_regs + i;
 
            for (k = 0; k < ARRAY_SIZE(mr->enum_ids); k++) {
-                if (mr->enum_ids[k] != source)
-                    continue;
-
-               s = sh_intc_source(desc, mr->enum_ids[k]);
-               if (s)
-                    s->enable_max++;
+               if (mr->enum_ids[k] == source) {
+                   s->enable_max++;
+                   break;
+               }
            }
        }
     }
@@ -342,31 +375,13 @@ static void sh_intc_register_source(struct intc_desc 
*desc,
            struct intc_prio_reg *pr = desc->prio_regs + i;
 
            for (k = 0; k < ARRAY_SIZE(pr->enum_ids); k++) {
-                if (pr->enum_ids[k] != source)
-                    continue;
-
-               s = sh_intc_source(desc, pr->enum_ids[k]);
-               if (s)
-                    s->enable_max++;
-           }
-       }
-    }
-
-    if (groups) {
-        for (i = 0; i < nr_groups; i++) {
-           struct intc_group *gr = groups + i;
-
-           for (k = 0; k < ARRAY_SIZE(gr->enum_ids); k++) {
-                if (gr->enum_ids[k] != source)
-                    continue;
-
-               s = sh_intc_source(desc, gr->enum_ids[k]);
-               if (s)
-                    s->enable_max++;
+               if (pr->enum_ids[k] == source) {
+                   s->enable_max++;
+                   break;
+               }
            }
        }
     }
-
 }
 
 void sh_intc_register_sources(struct intc_desc *desc,
@@ -393,10 +408,33 @@ void sh_intc_register_sources(struct intc_desc *desc,
     }
 
     if (groups) {
+       /* First of all, register group's sources, so that enable_max is
+          property set.  */
+       for (i = 0; i < nr_groups; i++) {
+           struct intc_group *gr = groups + i;
+           sh_intc_register_source(desc, gr->enum_id, groups, nr_groups);
+       }
+
         for (i = 0; i < nr_groups; i++) {
            struct intc_group *gr = groups + i;
 
            s = sh_intc_source(desc, gr->enum_id);
+
+           /* Propagate group's enable_max to children.  */
+           for (k = 0; k < ARRAY_SIZE(gr->enum_ids); k++) {
+               struct intc_source *child;
+                if (!gr->enum_ids[k])
+                    continue;
+
+               child = sh_intc_source(desc, gr->enum_ids[k]);
+               child->enable_max += s->enable_max;
+           }
+           
+           /* Chain sources within each group via source->next_enum_id,
+              so that we can easily enable/disable all sources in 
+              a group later.  */
+           
+           assert(s->next_enum_id == 0);
            s->next_enum_id = gr->enum_ids[0];
 
            for (k = 1; k < ARRAY_SIZE(gr->enum_ids); k++) {
@@ -404,6 +442,7 @@ void sh_intc_register_sources(struct intc_desc *desc,
                     continue;
 
                s = sh_intc_source(desc, gr->enum_ids[k - 1]);
+               assert(s->next_enum_id == 0);
                s->next_enum_id = gr->enum_ids[k];
            }
 
@@ -416,6 +455,7 @@ void sh_intc_register_sources(struct intc_desc *desc,
 }
 
 int sh_intc_init(struct intc_desc *desc,
+                target_phys_addr_t base,
                 int nr_sources,
                 struct intc_mask_reg *mask_regs,
                 int nr_mask_regs,
@@ -430,6 +470,7 @@ int sh_intc_init(struct intc_desc *desc,
     desc->nr_mask_regs = nr_mask_regs;
     desc->prio_regs = prio_regs;
     desc->nr_prio_regs = nr_prio_regs;
+    desc->base = INTC_A7(base);
 
     i = sizeof(struct intc_source) * nr_sources;
     desc->sources = qemu_malloc(i);
@@ -445,6 +486,9 @@ int sh_intc_init(struct intc_desc *desc,
  
     desc->iomemtype = cpu_register_io_memory(0, sh_intc_readfn,
                                             sh_intc_writefn, desc);
+
+    sh_intc_register(desc, base); /* icr0 */
+
     if (desc->mask_regs) {
         for (i = 0; i < desc->nr_mask_regs; i++) {
            struct intc_mask_reg *mr = desc->mask_regs + i;
@@ -471,12 +515,22 @@ int sh_intc_init(struct intc_desc *desc,
 void sh_intc_set_irl(void *opaque, int n, int level)
 {
     struct intc_source *s = opaque;
-    int i, irl = level ^ 15;
-    for (i = 0; (s = sh_intc_source(s->parent, s->next_enum_id)); i++) {
-       if (i == irl)
-           sh_intc_toggle_source(s, s->enable_count?0:1, s->asserted?0:1);
+    while ((s = sh_intc_source(s->parent, s->next_enum_id))) {
+       if (s->priority == level)
+           sh_intc_toggle_source(s, 0, s->asserted?0:1);
        else
            if (s->asserted)
                sh_intc_toggle_source(s, 0, -1);
     }
 }
+
+/* Initialize priorities for IRL interrupt sources.
+   Member #1 of the IRL group is set to 15, ... #15 is set to 1.
+   The number of members must be 15 */
+void sh_intc_init_irl_priorities(struct intc_source *s)
+{
+    int priority = 15;
+    while ((s = sh_intc_source(s->parent, s->next_enum_id)))
+       s->priority = priority--;
+    assert(priority == 0);
+}
diff --git a/hw/sh_intc.h b/hw/sh_intc.h
index a9750ae..5666497 100644
--- a/hw/sh_intc.h
+++ b/hw/sh_intc.h
@@ -42,6 +42,7 @@ struct intc_source {
     int enable_count;
     int enable_max;
     int pending; /* emulates the result of signal and masking */
+    int priority;
     struct intc_desc *parent;
 };
 
@@ -55,9 +56,11 @@ struct intc_desc {
     int nr_prio_regs;
     int iomemtype;
     int pending; /* number of interrupt sources that has pending set */
+    target_phys_addr_t base;
+    unsigned icr0;    
 };
 
-int sh_intc_get_pending_vector(struct intc_desc *desc, int imask);
+int sh_intc_get_pending_vector(struct intc_desc *desc, int *priority);
 struct intc_source *sh_intc_source(struct intc_desc *desc, intc_enum id);
 void sh_intc_toggle_source(struct intc_source *source,
                           int enable_adj, int assert_adj);
@@ -69,6 +72,7 @@ void sh_intc_register_sources(struct intc_desc *desc,
                              int nr_groups);
 
 int sh_intc_init(struct intc_desc *desc,
+                target_phys_addr_t base,
                 int nr_sources,
                 struct intc_mask_reg *mask_regs,
                 int nr_mask_regs,
@@ -76,5 +80,7 @@ int sh_intc_init(struct intc_desc *desc,
                 int nr_prio_regs);
 
 void sh_intc_set_irl(void *opaque, int n, int level);
+                
+void sh_intc_init_irl_priorities(struct intc_source *s);
 
 #endif /* __SH_INTC_H__ */
diff --git a/target-sh4/cpu.h b/target-sh4/cpu.h
index 86a4a6b..0051bcd 100644
--- a/target-sh4/cpu.h
+++ b/target-sh4/cpu.h
@@ -139,6 +139,7 @@ typedef struct CPUSH4State {
     uint32_t pvr;              /* Processor Version Register */
     uint32_t prr;              /* Processor Revision Register */
     uint32_t cvr;              /* Cache Version Register */
+    uint32_t opm;              /* CPU Operation Mode Register */
 
      CPU_COMMON tlb_t utlb[UTLB_SIZE]; /* unified translation table */
     tlb_t itlb[ITLB_SIZE];     /* instruction translation table */
diff --git a/target-sh4/helper.c b/target-sh4/helper.c
index 7f5430a..cf52cbf 100644
--- a/target-sh4/helper.c
+++ b/target-sh4/helper.c
@@ -81,6 +81,7 @@ void do_interrupt(CPUState * env)
 {
     int do_irq = env->interrupt_request & CPU_INTERRUPT_HARD;
     int do_exp, irq_vector = env->exception_index;
+    int priority = (env->sr >> 4) & 0xf;
 
     /* prioritize exceptions over interrupts */
 
@@ -99,7 +100,7 @@ void do_interrupt(CPUState * env)
 
     if (do_irq) {
         irq_vector = sh_intc_get_pending_vector(env->intc_handle,
-                                               (env->sr >> 4) & 0xf);
+                                               &priority);
         if (irq_vector == -1) {
             return; /* masked */
        }
@@ -157,6 +158,13 @@ void do_interrupt(CPUState * env)
     }
 
     env->ssr = env->sr;
+
+    if (env->opm & (1 << 3)) {
+       unsigned mask = 0xf << 4;
+       env->ssr &= ~mask;
+       env->ssr |= (priority << 4) & mask;
+    }
+
     env->spc = env->pc;
     env->sgr = env->gregs[15];
     env->sr |= SR_BL | SR_MD | SR_RB;
-- 
1.5.6.3