commit aac8877a31db83fe5cba30a742fc23b2dccee518 Author: Vladimir Prus 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;