[Qemu-devel] SH: Improve the interrupt controller

[Vladimir Prus <vladimir@codesourcery.com>]

[-- Attachment #1: Type: text/plain, Size: 831 bytes --]


This patch improves the intc implementation in these ways:

   - On interrupt, the priority mask in SSR is updated,
     if OPM register tells it should be
   - We check interrupt priority and compare it with
     priority mask
   - Priorities for IRL interrupts (which are fixed), are
     assigned
   - The ICR register is supported, and LVLMODE bit, which
     controls if interrupt is automatically de-asserted,
     is implemented
   - A bug where handling of paired set mask / clear mask
     registers was done backward is fixed
   - A bug where enabling a group did not work was fixed.

I have tested this only with SH4A and it's desirable to test
with 7751/R2D. However, I no longer sure I know which kernel
to use for that. Can anybody either provide me with instructions,
or test this patch with R2D for me?

- Volodya

[-- Attachment #2: intc.diff --]
[-- Type: text/x-diff, Size: 14947 bytes --]

commit aac8877a31db83fe5cba30a742fc23b2dccee518
Author: Vladimir Prus <vladimir@codesourcery.com>
Date:   Sun Oct 12 20:02:08 2008 +0400

    SH: Improve the interrupt controller
    
    	* hw/sh7750.c (sh7750_init): Pass the base address to
    	sh_intc_init.
    	* hw/sh_intc.c (ICR0_LVLMODE): New.
    	(sh_intc_get_pending_vector): Remame parameter imask to priority,
    	and make it in/out.  Find source with the highest priority.
    	(sh_intc_toggle_mask): New parameter 'priority'. Set source
    	priority, if provided and pass priority to other members of a
    	group.
    	(sh_intc_read): Handle read of ICR.
    	(sh_intc_write): Handle write of ICR.  Fix masking in set/clear
    	register pair case.
    	(sh_intc_register_source, sh_intc_register_sources): If we have
    	priority register that controls a group, properly set enable_max
    	on the group members.
    	(sh_intc_init): Remember and register the base address.
    	(sh_intc_set_irl_priorities): New.
    	* hw/sh_intc.h (struct intc_source): New field priority.
    	(struct intc_desc): New fields base, icr0.
    	(sh_intc_get_pending_vector): Adjust prototype.
    	(sh_intc_init): New parameter 'base'.
    	(sh_intc_set_irl_priorities): New.
    	* target-sh4/cpu.h (struct CPUSH4State): New field opm.
    	* helper.c (do_interrupt): Update interrupt mask in SR if
    	necessary.

diff --git a/hw/sh7750.c b/hw/sh7750.c
index af86f0e..a43c2f1 100644
--- a/hw/sh7750.c
+++ b/hw/sh7750.c
@@ -701,6 +701,7 @@ SH7750State *sh7750_init(CPUSH4State * cpu)
 				 sh7750_mm_cache_and_tlb);
 
     sh_intc_init(&s->intc, NR_SOURCES,
+		 0x1fd00000,
 		 _INTC_ARRAY(mask_registers),
 		 _INTC_ARRAY(prio_registers));
 
diff --git a/hw/sh_intc.c b/hw/sh_intc.c
index 136e7dd..0f2ddf0 100644
--- a/hw/sh_intc.c
+++ b/hw/sh_intc.c
@@ -19,6 +19,8 @@
 #define INTC_A7(x) ((x) & 0x1fffffff)
 #define INTC_ARRAY(x) (sizeof(x) / sizeof(x[0]))
 
+#define ICR0_LVLMODE (1 << 21)
+
 void sh_intc_toggle_source(struct intc_source *source,
 			   int enable_adj, int assert_adj)
 {
@@ -46,8 +48,9 @@ void sh_intc_toggle_source(struct intc_source *source,
     if (pending_changed) {
         if (source->pending) {
             source->parent->pending++;
-	    if (source->parent->pending == 1)
+	    if (source->parent->pending == 1) {
                 cpu_interrupt(first_cpu, CPU_INTERRUPT_HARD);
+	    }
 	}
 	else {
             source->parent->pending--;
@@ -84,30 +87,46 @@ 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) {
+	if (source->pending && source->priority > highest_priority)
+	    {
+	        highest_priority = source->priority;
+		found = i;
+	    }
+    }
+
+    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",
+	printf("sh_intc: (%d) returning interrupt source 0x%x\n",
 		   desc->pending, source->vect);
 #endif
-            return source->vect;
-	}
-    }
 
-    assert(0);
+	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
@@ -187,7 +206,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;
 
@@ -202,7 +221,11 @@ static void sh_intc_toggle_mask(struct intc_desc *desc, intc_enum id,
     }
 
     if (source->vect)
+      {
         sh_intc_toggle_source(source, enable ? 1 : -1, 0);
+	if (priority != -1)
+	  source->priority = priority;
+      }
 
 #ifdef DEBUG_INTC
     else {
@@ -211,7 +234,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
@@ -233,6 +256,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 (INTC_A7(offset) == INTC_A7(desc->base))
+      return desc->icr0;
 
     sh_intc_locate(desc, (unsigned long)offset, &valuep, 
 		   &enum_ids, &first, &width, &mode);
@@ -255,18 +280,26 @@ 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 (INTC_A7(offset) == INTC_A7(desc->base))
+      {
+	desc->icr0 = value;
+	return;	
+      }
+
     sh_intc_locate(desc, (unsigned long)offset, &valuep, 
 		   &enum_ids, &first, &width, &mode);
 
     switch (mode) {
     case INTC_MODE_ENABLE_REG | INTC_MODE_IS_PRIO: break;
-    case INTC_MODE_DUAL_SET: value |= *valuep; break;
-    case INTC_MODE_DUAL_CLR: value = *valuep & ~value; break;
+    case INTC_MODE_DUAL_SET: value = *valuep & ~value; break;
+    case INTC_MODE_DUAL_CLR: value |= *valuep; break;
     default: assert(0);
     }
 
     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;
@@ -274,7 +307,18 @@ 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;
@@ -316,24 +360,24 @@ static void sh_intc_register(struct intc_desc *desc,
 }
 
 static void sh_intc_register_source(struct intc_desc *desc,
-				    intc_enum source,
+				    intc_enum source_id,
 				    struct intc_group *groups,
 				    int nr_groups)
 {
     unsigned int i, k;
-    struct intc_source *s;
+    struct intc_source *source = sh_intc_source(desc, source_id);
+    assert(source);
 
     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 < INTC_ARRAY(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_id)
+		{
+		  source->enable_max++;
+		  break;
+		}
 	    }
 	}
     }
@@ -343,31 +387,14 @@ static void sh_intc_register_source(struct intc_desc *desc,
 	    struct intc_prio_reg *pr = desc->prio_regs + i;
 
 	    for (k = 0; k < INTC_ARRAY(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 (pr->enum_ids[k] == source_id)
+		{
+		  source->enable_max++;
+		  break;
+		}
 	    }
 	}
     }
-
-    if (groups) {
-        for (i = 0; i < nr_groups; i++) {
-	    struct intc_group *gr = groups + i;
-
-	    for (k = 0; k < INTC_ARRAY(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++;
-	    }
-	}
-    }
-
 }
 
 void sh_intc_register_sources(struct intc_desc *desc,
@@ -377,7 +404,7 @@ void sh_intc_register_sources(struct intc_desc *desc,
 			      int nr_groups)
 {
     unsigned int i, k;
-    struct intc_source *s;
+    struct intc_source *s, *s2;
 
     for (i = 0; i < nr_vectors; i++) {
 	struct intc_vect *vect = vectors + i;
@@ -394,10 +421,34 @@ 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 < INTC_ARRAY(gr->enum_ids); k++) {
+                if (!gr->enum_ids[k])
+                    continue;
+
+		s2 = sh_intc_source(desc, gr->enum_ids[k]);
+		s2->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 < INTC_ARRAY(gr->enum_ids); k++) {
@@ -405,6 +456,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];
 	    }
 
@@ -417,6 +469,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,
@@ -431,6 +484,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 = base;
 
     i = sizeof(struct intc_source) * nr_sources;
     desc->sources = malloc(i);
@@ -448,6 +502,10 @@ int sh_intc_init(struct intc_desc *desc,
  
     desc->iomemtype = cpu_register_io_memory(0, sh_intc_readfn,
 					     sh_intc_writefn, desc);
+
+    cpu_register_physical_memory_offset(A7ADDR(base), 4, desc->iomemtype, INTC_A7(base));
+    cpu_register_physical_memory_offset(P4ADDR(base), 4, desc->iomemtype, INTC_A7(base));
+
     if (desc->mask_regs) {
         for (i = 0; i < desc->nr_mask_regs; i++) {
 	    struct intc_mask_reg *mr = desc->mask_regs + i;
@@ -483,3 +541,19 @@ void sh_intc_set_irl(void *opaque, int n, int level)
 	        sh_intc_toggle_source(s, 0, -1);
     }
 }
+
+void sh_intc_set_irl_priorities(struct intc_desc *desc,
+				int low_priority_source, 
+				int high_priority_source)
+{
+  int priority = 15;
+  int i;
+
+  assert (low_priority_source - high_priority_source + 1 == 15);
+  for (i = high_priority_source; i <= low_priority_source; ++i, --priority)
+    {
+      struct intc_source *sources = desc->sources + i;
+      sources->priority = priority;
+    }
+  assert(priority == 0);
+}
diff --git a/hw/sh_intc.h b/hw/sh_intc.h
index 4e36f00..d35f8dc 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,14 @@ int sh_intc_init(struct intc_desc *desc,
 		 int nr_prio_regs);
 
 void sh_intc_set_irl(void *opaque, int n, int level);
+		 
+/* Assign priorities for IRL interrupt sources. The
+   low_priority_source gets priority of 1, and
+   high_priority_source gets priority of 15.  Sources in
+   between get priorities in between. The number of
+   sources in the range should be 15.  */
+void sh_intc_set_irl_priorities(struct intc_desc *desc,
+				int low_priority_source, 
+				int high_priority_source);
 
 #endif /* __SH_INTC_H__ */
diff --git a/target-sh4/cpu.h b/target-sh4/cpu.h
index eed3b1b..fe02a24 100644
--- a/target-sh4/cpu.h
+++ b/target-sh4/cpu.h
@@ -140,6 +140,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 */
 
     uint32_t ldst;
 
diff --git a/target-sh4/helper.c b/target-sh4/helper.c
index 61b668b..10bcf84 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,14 @@ 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;