summaryrefslogtreecommitdiff
path: root/big-little/common/vgiclib.c
diff options
context:
space:
mode:
Diffstat (limited to 'big-little/common/vgiclib.c')
-rw-r--r--big-little/common/vgiclib.c687
1 files changed, 349 insertions, 338 deletions
diff --git a/big-little/common/vgiclib.c b/big-little/common/vgiclib.c
index ad2a55d..2a27088 100644
--- a/big-little/common/vgiclib.c
+++ b/big-little/common/vgiclib.c
@@ -18,7 +18,7 @@
* contributors may be used to endorse or promote products
* derived from this software without specific prior written
* permission.
- */
+ */
#include "vgiclib.h"
#include "misc.h"
@@ -34,64 +34,65 @@
static struct overflowint *freeoverflows[NUM_CPUS];
static struct overflowint theoverflowints[NUM_CPUS][MAXOVERFLOWINTS];
static struct gic_cpuif cpuifs[NUM_CPUS];
-static unsigned hv_lr_count[NUM_CPUS] = {0};
+static unsigned hv_lr_count[NUM_CPUS] = { 0 };
void dump_vgic_state()
{
- unsigned int i;
-
- printf("VGIC state:\n");
- printf(" Control : 0x%x \n", read32(VGIC_HV_PHY_BASE + GICH_CTL));
- printf(" ActivePri: 0x%x \n", read32(VGIC_HV_PHY_BASE + GICH_APR0));
- for (i = 0; i < 4; i++) {
- printf(" List : 0x%x \n", read32(VGIC_HV_PHY_BASE + GICH_LR_BASE + (i * 4)));
- }
+ unsigned int i;
+
+ printf("VGIC state:\n");
+ printf(" Control : 0x%x \n", read32(VGIC_HV_PHY_BASE + GICH_CTL));
+ printf(" ActivePri: 0x%x \n", read32(VGIC_HV_PHY_BASE + GICH_APR0));
+ for (i = 0; i < 4; i++) {
+ printf(" List : 0x%x \n",
+ read32(VGIC_HV_PHY_BASE + GICH_LR_BASE + (i * 4)));
+ }
}
static struct overflowint *get_overflowint(unsigned cpuid)
{
- struct overflowint *p = freeoverflows[cpuid];
+ struct overflowint *p = freeoverflows[cpuid];
- if (!p) {
- printf("Panic: Out of overflow interrupt slots.\n");
- printf("Recompile with larger MAXOVERFLOWINTS.\n");
- panic();
- }
+ if (!p) {
+ printf("Panic: Out of overflow interrupt slots.\n");
+ printf("Recompile with larger MAXOVERFLOWINTS.\n");
+ panic();
+ }
- freeoverflows[cpuid] = p->next;
+ freeoverflows[cpuid] = p->next;
- return p;
+ return p;
}
static void free_overflowint(struct overflowint *p, unsigned cpuid)
{
- p->next = freeoverflows[cpuid];
- freeoverflows[cpuid] = p;
+ p->next = freeoverflows[cpuid];
+ freeoverflows[cpuid] = p;
}
void vgic_init(void)
{
- unsigned int i;
- unsigned cpuid = read_cpuid();
+ unsigned int i;
+ unsigned cpuid = read_cpuid();
- freeoverflows[cpuid] = 0x0;
+ freeoverflows[cpuid] = 0x0;
- for (i = 0; i < MAXOVERFLOWINTS; i++) {
- free_overflowint(&(theoverflowints[cpuid][i]), cpuid);
- }
+ for (i = 0; i < MAXOVERFLOWINTS; i++) {
+ free_overflowint(&(theoverflowints[cpuid][i]), cpuid);
+ }
- /*
- * Find the number of List registers
- * TODO: Will not work if individual cpus can have different number
- * of list registers across clusters. Needs to be detected for each
- * access then.
- */
- hv_lr_count[cpuid] = (read32(VGIC_HV_PHY_BASE + GICH_VTR) & 0x3f) + 1;
+ /*
+ * Find the number of List registers
+ * TODO: Will not work if individual cpus can have different number
+ * of list registers across clusters. Needs to be detected for each
+ * access then.
+ */
+ hv_lr_count[cpuid] = (read32(VGIC_HV_PHY_BASE + GICH_VTR) & 0x3f) + 1;
- /* Enable virtual interrupts & if required, maintenance interrupts */
- write32(VGIC_HV_PHY_BASE + GICH_CTL, VGICH_HCR_EN);
+ /* Enable virtual interrupts & if required, maintenance interrupts */
+ write32(VGIC_HV_PHY_BASE + GICH_CTL, VGICH_HCR_EN);
- return;
+ return;
}
/*
@@ -99,12 +100,12 @@ void vgic_init(void)
*/
static void set_vgic_entry(unsigned int descr, unsigned int slot)
{
- write32(VGIC_HV_PHY_BASE + GICH_LR_BASE + (slot * 4), descr);
+ write32(VGIC_HV_PHY_BASE + GICH_LR_BASE + (slot * 4), descr);
}
static unsigned int get_vgic_entry(unsigned int slot)
{
- return read32(VGIC_HV_PHY_BASE + GICH_LR_BASE + (slot * 4));
+ return read32(VGIC_HV_PHY_BASE + GICH_LR_BASE + (slot * 4));
}
/*
@@ -112,12 +113,12 @@ static unsigned int get_vgic_entry(unsigned int slot)
*/
static void set_vgic_status(unsigned int status)
{
- write32(VGIC_HV_PHY_BASE + GICH_CTL, status);
+ write32(VGIC_HV_PHY_BASE + GICH_CTL, status);
}
static unsigned int get_vgic_status(void)
{
- return read32(VGIC_HV_PHY_BASE + GICH_CTL);
+ return read32(VGIC_HV_PHY_BASE + GICH_CTL);
}
/*
@@ -128,28 +129,28 @@ static unsigned int get_vgic_status(void)
*/
static void set_vgic_queue_entry(struct gic_cpuif *cpuif, unsigned int descr)
{
- unsigned int pri = (descr >> 20) & 0xFF;
- struct overflowint **oflowh, *oflowp;
- unsigned cpuid = read_cpuid();
-
- /*
- * If we are queuing something and there is currently no queue, set the interrupt bit
- */
- if (!(cpuif->overflow))
- set_vgic_status(get_vgic_status() | 0x2);
-
- /*
- * Determine insertion point, might be the end of the list
- */
- for (oflowh = &(cpuif->overflow); *oflowh; oflowh = &((*oflowh)->next))
- if ((*oflowh)->priority > pri)
- break;
-
- oflowp = get_overflowint(cpuid);
- oflowp->priority = pri;
- oflowp->value = descr;
- oflowp->next = *oflowh;
- *oflowh = oflowp;
+ unsigned int pri = (descr >> 20) & 0xFF;
+ struct overflowint **oflowh, *oflowp;
+ unsigned cpuid = read_cpuid();
+
+ /*
+ * If we are queuing something and there is currently no queue, set the interrupt bit
+ */
+ if (!(cpuif->overflow))
+ set_vgic_status(get_vgic_status() | 0x2);
+
+ /*
+ * Determine insertion point, might be the end of the list
+ */
+ for (oflowh = &(cpuif->overflow); *oflowh; oflowh = &((*oflowh)->next))
+ if ((*oflowh)->priority > pri)
+ break;
+
+ oflowp = get_overflowint(cpuid);
+ oflowp->priority = pri;
+ oflowp->value = descr;
+ oflowp->next = *oflowh;
+ *oflowh = oflowp;
}
/*
@@ -159,7 +160,7 @@ static void set_vgic_queue_entry(struct gic_cpuif *cpuif, unsigned int descr)
*/
static inline unsigned elrsr_max_index(unsigned cpuid)
{
- return (hv_lr_count[cpuid] - 1) >> 5;
+ return (hv_lr_count[cpuid] - 1) >> 5;
}
/*
@@ -169,107 +170,117 @@ static inline unsigned elrsr_max_index(unsigned cpuid)
* inverse of what the elrsr returns while taking into account unimplemented
* interrupts.
*/
-static unsigned get_elrsr_active_bits(unsigned index, unsigned cpuid, unsigned max_index)
+static unsigned get_elrsr_active_bits(unsigned index, unsigned cpuid,
+ unsigned max_index)
{
- unsigned elrsr = ~(read32(VGIC_HV_PHY_BASE + GICH_ELRSR0 + (index << 2)));
-
- if (index == max_index) {
- /*
- * Get the remainder, shift 1 times remainder and subtract 1
- * from it to form the mask.
- */
- elrsr &= (1 << (hv_lr_count[cpuid] - (32 * max_index))) - 1;
- } else if (index > max_index) {
- /*
- * There can never be active virqs when the list registers
- * do not exist.
- */
- elrsr = 0;
- }
-
- return elrsr;
+ unsigned elrsr =
+ ~(read32(VGIC_HV_PHY_BASE + GICH_ELRSR0 + (index << 2)));
+
+ if (index == max_index) {
+ /*
+ * Get the remainder, shift 1 times remainder and subtract 1
+ * from it to form the mask.
+ */
+ elrsr &= (1 << (hv_lr_count[cpuid] - (32 * max_index))) - 1;
+ } else if (index > max_index) {
+ /*
+ * There can never be active virqs when the list registers
+ * do not exist.
+ */
+ elrsr = 0;
+ }
+
+ return elrsr;
}
void vgic_savestate(unsigned int cpu)
{
- struct gic_cpuif *cpuif = &(cpuifs[cpu]);
- unsigned int i, ctr = 0, cur_elrsr = 0;
- unsigned max_index = elrsr_max_index(cpu);
-
- for(ctr = 0; ctr <= max_index; ctr++) {
- /* Negate read value so that set bit corresponds to a !inactive register */
- cur_elrsr = get_elrsr_active_bits(ctr, cpu, max_index);
- cpuif->elrsr[ctr] = cur_elrsr;
-
- for(i = bitindex(cur_elrsr); ((int) i) >= 0; i = bitindex(cur_elrsr)) {
- unsigned list_reg =
- read32(VGIC_HV_PHY_BASE + GICH_LR_BASE + ((1 << 7) * ctr) + (i << 2));
- unsigned int_id = (list_reg >> 10) & 0x3ff;
-
- /* Clear the saved bit index */
- cur_elrsr &= ~(1 << i);
-
- /*
- * Invalidate the pending/active virtual interrupt. Since its a shared vGIC
- * this irq will persist till the next switch and hence create a duplicate.
- */
- write32(VGIC_HV_PHY_BASE + GICH_LR_BASE + ((1 << 7) * ctr) + (i << 2), list_reg & ~(0x3 << 28));
-
- /*
- * While saving queued IPI context, ensure that the requesting cpu
- * interface is mapped to it counterpart on the inbound cluster
- */
- if (int_id < 16) {
- unsigned ob_cpuid = int_id & 0x7;
- unsigned ob_clusterid = read_clusterid();
- unsigned ib_cpuif = 0;
-
- ib_cpuif = get_cpuif(!ob_clusterid, ob_cpuid);
- /* Clear the cpu interface bits and place inbound cpu interface instead */
- list_reg = (list_reg & ~(0x7 << 10)) | (ib_cpuif << 10);
- } else if (int_id < 32) {
- /*
- * Pending Private peripheral interrupts will be recreated from scratch
- * so no need to save them.
- */
- cpuif->elrsr[ctr] &= ~(1 << i);
- continue;
- }
-
- cpuif->ints[i] = list_reg;
-
- }
- }
-
- cpuif->status = read32(VGIC_HV_PHY_BASE + GICH_CTL);
- cpuif->activepris = read32(VGIC_HV_PHY_BASE + GICH_APR0);
-
- write32(VGIC_HV_PHY_BASE + GICH_CTL, 0); /* SMP */
-
- return;
+ struct gic_cpuif *cpuif = &(cpuifs[cpu]);
+ unsigned int i, ctr = 0, cur_elrsr = 0;
+ unsigned max_index = elrsr_max_index(cpu);
+
+ for (ctr = 0; ctr <= max_index; ctr++) {
+ /* Negate read value so that set bit corresponds to a !inactive register */
+ cur_elrsr = get_elrsr_active_bits(ctr, cpu, max_index);
+ cpuif->elrsr[ctr] = cur_elrsr;
+
+ for (i = bitindex(cur_elrsr); ((int)i) >= 0;
+ i = bitindex(cur_elrsr)) {
+ unsigned list_reg =
+ read32(VGIC_HV_PHY_BASE + GICH_LR_BASE +
+ ((1 << 7) * ctr) + (i << 2));
+ unsigned int_id = (list_reg >> 10) & 0x3ff;
+
+ /* Clear the saved bit index */
+ cur_elrsr &= ~(1 << i);
+
+ /*
+ * Invalidate the pending/active virtual interrupt. Since its a shared vGIC
+ * this irq will persist till the next switch and hence create a duplicate.
+ */
+ write32(VGIC_HV_PHY_BASE + GICH_LR_BASE +
+ ((1 << 7) * ctr) + (i << 2),
+ list_reg & ~(0x3 << 28));
+
+ /*
+ * While saving queued IPI context, ensure that the requesting cpu
+ * interface is mapped to it counterpart on the inbound cluster
+ */
+ if (int_id < 16) {
+ unsigned ob_cpuid = int_id & 0x7;
+ unsigned ob_clusterid = read_clusterid();
+ unsigned ib_cpuif = 0;
+
+ ib_cpuif = get_cpuif(!ob_clusterid, ob_cpuid);
+ /* Clear the cpu interface bits and place inbound cpu interface instead */
+ list_reg =
+ (list_reg & ~(0x7 << 10)) | (ib_cpuif <<
+ 10);
+ } else if (int_id < 32) {
+ /*
+ * Pending Private peripheral interrupts will be recreated from scratch
+ * so no need to save them.
+ */
+ cpuif->elrsr[ctr] &= ~(1 << i);
+ continue;
+ }
+
+ cpuif->ints[i] = list_reg;
+
+ }
+ }
+
+ cpuif->status = read32(VGIC_HV_PHY_BASE + GICH_CTL);
+ cpuif->activepris = read32(VGIC_HV_PHY_BASE + GICH_APR0);
+
+ write32(VGIC_HV_PHY_BASE + GICH_CTL, 0); /* SMP */
+
+ return;
}
void vgic_loadstate(unsigned int cpu)
{
- struct gic_cpuif *cpuif = &(cpuifs[cpu]);
- unsigned int i, ctr = 0, cur_elrsr = 0;
- unsigned max_index = elrsr_max_index(cpu);
+ struct gic_cpuif *cpuif = &(cpuifs[cpu]);
+ unsigned int i, ctr = 0, cur_elrsr = 0;
+ unsigned max_index = elrsr_max_index(cpu);
+
+ for (ctr = 0; ctr <= max_index; ctr++) {
+ cur_elrsr = cpuif->elrsr[ctr];
- for(ctr = 0; ctr <= max_index; ctr++) {
- cur_elrsr = cpuif->elrsr[ctr];
+ for (i = bitindex(cur_elrsr); ((int)i) >= 0;
+ i = bitindex(cur_elrsr)) {
+ write32(VGIC_HV_PHY_BASE + GICH_LR_BASE +
+ ((1 << 7) * ctr) + (i << 2), cpuif->ints[i]);
- for(i = bitindex(cur_elrsr); ((int) i) >= 0; i = bitindex(cur_elrsr)) {
- write32(VGIC_HV_PHY_BASE + GICH_LR_BASE + ((1 << 7) * ctr) + (i << 2), cpuif->ints[i]);
-
- /* Clear the restored bit index */
- cur_elrsr &= ~(1 << i);
- }
- }
+ /* Clear the restored bit index */
+ cur_elrsr &= ~(1 << i);
+ }
+ }
- write32(VGIC_HV_PHY_BASE + GICH_CTL, cpuif->status);
- write32(VGIC_HV_PHY_BASE + GICH_APR0, cpuif->activepris);
+ write32(VGIC_HV_PHY_BASE + GICH_CTL, cpuif->status);
+ write32(VGIC_HV_PHY_BASE + GICH_APR0, cpuif->activepris);
- return;
+ return;
}
/*
@@ -296,130 +307,130 @@ void vgic_loadstate(unsigned int cpu)
*/
void vgic_refresh(unsigned int cpu)
{
- struct gic_cpuif *cpuif = &(cpuifs[cpu]);
- unsigned int i, value, status, newstatus;
- struct overflowint **oflowh, *oflowp;
-
- /*
- * Grab a copy of the status.
- */
- status = get_vgic_status();
-
- /*
- * "newstatus" is the value to be written back if needed. Whatever
- * * happens, we will clear the slipped EOI count by the time we are done
- */
- newstatus = status & 0x07FFFFFF;
-
- /*
- * See if there are any "slipped" EOIs
- */
- i = (status >> 27) & 0x1F;
-
- if (i) {
- /*
- * If there are, let's deal with them.
- * *
- * * We will walk through the list of queued interrupts, deactivating the
- * * ACTIVE ones as needed until we either have no more slipped EOI's to
- * * do or run out of queued interrupts. If we run out of queued
- * * interrupts first, that's UNPREDICTABLE behaviour (and the fault of
- * * the VM). In this case we will just ignore the surplus EOIs.
- * *
- * * After EOI'ing, we delete the entry if it was just ACTIVE or set it
- * * to PENDING if it was PENDING+ACTIVE.
- * *
- * * Use a handle to point to the list entries to avoid the need for
- * * special cases in the loop.
- */
- oflowh = &(cpuif->overflow);
-
- while (i && *oflowh) {
- value = (*oflowh)->value;
- if (value & VGIC_ENTRY_ACTIVE) {
- /*
- * It's ACTIVE (or PENDING+ACTIVE)
- */
- i--;
-
- if (value & VGIC_ENTRY_HW) {
- /*
- * HW bit set, so we need to pass on an EOI. This doesn't ever happen
- * * for IPIs, so just pass on the 10-bit "Hardware ID"
- */
- gic_deactivate_int((value >> 10) &
- 0x3FF);
- }
-
- if (value & VGIC_ENTRY_PENDING) {
- /*
- * It was PENDING+ACTIVE, clear the ACTIVE bit and move on
- */
- (*oflowh)->value &= ~VGIC_ENTRY_ACTIVE;
- } else {
- /*
- * It was only ACTIVE, so we need to delete it..
- */
- oflowp = *oflowh;
- oflowh = &(oflowp->next);
- free_overflowint(oflowp, cpu);
- }
- } else {
- /*
- * It wasn't ACTIVE :( Try the next one.
- */
- oflowh = &((*oflowh)->next);
- }
- }
- }
-
- /*
- * Now populate any spare slots with entries from the list (if any). Also fix up the free slot bitmap
- */
- for (i = 0; i < hv_lr_count[cpu]; i++) {
- value = get_vgic_entry(i);
-
- if (value & 0x30000000) {
- /*
- * This entry already contains a valid interrupt, skip
- */
- continue;
- }
-
- /*
- * Not a valid interrupt
- */
- oflowp = cpuif->overflow;
- if (oflowp) {
- /*
- * If there's a queue, move the top entry out of the queue and into
- * * this slot..
- */
- cpuif->overflow = oflowp->next;
-
- set_vgic_entry(oflowp->value, i);
- free_overflowint(oflowp, cpu);
- } else {
- /*
- * .. otherwise mark it as available.
- */
- cpuif->freelist |= (1 << i);
- }
- }
-
- /*
- * If we now don't have any overflow, clear the status bit
- */
- if (!(cpuif->overflow)) {
- newstatus &= ~0x2;
- }
-
- /*
- * Refresh status if needed
- */
- if (newstatus != status) {
- set_vgic_status(newstatus);
- }
+ struct gic_cpuif *cpuif = &(cpuifs[cpu]);
+ unsigned int i, value, status, newstatus;
+ struct overflowint **oflowh, *oflowp;
+
+ /*
+ * Grab a copy of the status.
+ */
+ status = get_vgic_status();
+
+ /*
+ * "newstatus" is the value to be written back if needed. Whatever
+ * * happens, we will clear the slipped EOI count by the time we are done
+ */
+ newstatus = status & 0x07FFFFFF;
+
+ /*
+ * See if there are any "slipped" EOIs
+ */
+ i = (status >> 27) & 0x1F;
+
+ if (i) {
+ /*
+ * If there are, let's deal with them.
+ * *
+ * * We will walk through the list of queued interrupts, deactivating the
+ * * ACTIVE ones as needed until we either have no more slipped EOI's to
+ * * do or run out of queued interrupts. If we run out of queued
+ * * interrupts first, that's UNPREDICTABLE behaviour (and the fault of
+ * * the VM). In this case we will just ignore the surplus EOIs.
+ * *
+ * * After EOI'ing, we delete the entry if it was just ACTIVE or set it
+ * * to PENDING if it was PENDING+ACTIVE.
+ * *
+ * * Use a handle to point to the list entries to avoid the need for
+ * * special cases in the loop.
+ */
+ oflowh = &(cpuif->overflow);
+
+ while (i && *oflowh) {
+ value = (*oflowh)->value;
+ if (value & VGIC_ENTRY_ACTIVE) {
+ /*
+ * It's ACTIVE (or PENDING+ACTIVE)
+ */
+ i--;
+
+ if (value & VGIC_ENTRY_HW) {
+ /*
+ * HW bit set, so we need to pass on an EOI. This doesn't ever happen
+ * * for IPIs, so just pass on the 10-bit "Hardware ID"
+ */
+ gic_deactivate_int((value >> 10) &
+ 0x3FF);
+ }
+
+ if (value & VGIC_ENTRY_PENDING) {
+ /*
+ * It was PENDING+ACTIVE, clear the ACTIVE bit and move on
+ */
+ (*oflowh)->value &= ~VGIC_ENTRY_ACTIVE;
+ } else {
+ /*
+ * It was only ACTIVE, so we need to delete it..
+ */
+ oflowp = *oflowh;
+ oflowh = &(oflowp->next);
+ free_overflowint(oflowp, cpu);
+ }
+ } else {
+ /*
+ * It wasn't ACTIVE :( Try the next one.
+ */
+ oflowh = &((*oflowh)->next);
+ }
+ }
+ }
+
+ /*
+ * Now populate any spare slots with entries from the list (if any). Also fix up the free slot bitmap
+ */
+ for (i = 0; i < hv_lr_count[cpu]; i++) {
+ value = get_vgic_entry(i);
+
+ if (value & 0x30000000) {
+ /*
+ * This entry already contains a valid interrupt, skip
+ */
+ continue;
+ }
+
+ /*
+ * Not a valid interrupt
+ */
+ oflowp = cpuif->overflow;
+ if (oflowp) {
+ /*
+ * If there's a queue, move the top entry out of the queue and into
+ * * this slot..
+ */
+ cpuif->overflow = oflowp->next;
+
+ set_vgic_entry(oflowp->value, i);
+ free_overflowint(oflowp, cpu);
+ } else {
+ /*
+ * .. otherwise mark it as available.
+ */
+ cpuif->freelist |= (1 << i);
+ }
+ }
+
+ /*
+ * If we now don't have any overflow, clear the status bit
+ */
+ if (!(cpuif->overflow)) {
+ newstatus &= ~0x2;
+ }
+
+ /*
+ * Refresh status if needed
+ */
+ if (newstatus != status) {
+ set_vgic_status(newstatus);
+ }
}
/*
@@ -432,67 +443,67 @@ void vgic_refresh(unsigned int cpu)
*/
void enqueue_interrupt(unsigned int descr, unsigned int cpu)
{
- unsigned int slot;
- struct gic_cpuif *cpuif;
-
- cpuif = &(cpuifs[cpu]);
-
- /*
- * If there are no free slots, trigger a maintenance
- */
- if (!(cpuif->freelist)) {
- vgic_refresh(cpu);
- }
-
- if (cpuif->freelist) {
- /*
- * There is a free slot, use it.
- */
- slot = cpuif->freelist; /* Take the free list.. */
- slot &= (-slot); /* .. extract one set bit .. */
- cpuif->freelist &= (~slot); /* .. clear that bit from free list .. */
- slot = bitindex(slot); /* .. and convert to number. */
-
- set_vgic_entry(descr, slot);
- } else {
- /*
- * There are no free slots, we are either queuing this one or swapping another out
- */
- unsigned int pri = (descr >> 20) & 0xFF;
- unsigned int minpri = 0;
- unsigned int minslot = 0;
- unsigned int i, j;
-
- if (cpuif->overflow && cpuif->overflow->priority <= pri) {
- /*
- * There are already queued interrupts with the same or higher priority, just queue this one
- */
- set_vgic_queue_entry(cpuif, descr);
- return;
- }
-
- /*
- * Otherwise find the lowest priority entry..
- */
- for (i = 0; i < hv_lr_count[cpu]; i++) {
- j = (get_vgic_entry(i) >> 20) & 0xFF; /* Get the priority for the current thing in this slot */
- if (i == 0 || (j > minpri)) {
- minpri = j;
- minslot = i;
- }
- }
-
- if (minpri > pri) {
- /*
- * If it's lower priority than this new one we kick it out
- */
- set_vgic_queue_entry(cpuif, get_vgic_entry(minslot));
- set_vgic_entry(descr, minslot);
- } else {
- /*
- * Otherwise just queue the new one
- */
- set_vgic_queue_entry(cpuif, descr);
- }
- }
+ unsigned int slot;
+ struct gic_cpuif *cpuif;
+
+ cpuif = &(cpuifs[cpu]);
+
+ /*
+ * If there are no free slots, trigger a maintenance
+ */
+ if (!(cpuif->freelist)) {
+ vgic_refresh(cpu);
+ }
+
+ if (cpuif->freelist) {
+ /*
+ * There is a free slot, use it.
+ */
+ slot = cpuif->freelist; /* Take the free list.. */
+ slot &= (-slot); /* .. extract one set bit .. */
+ cpuif->freelist &= (~slot); /* .. clear that bit from free list .. */
+ slot = bitindex(slot); /* .. and convert to number. */
+
+ set_vgic_entry(descr, slot);
+ } else {
+ /*
+ * There are no free slots, we are either queuing this one or swapping another out
+ */
+ unsigned int pri = (descr >> 20) & 0xFF;
+ unsigned int minpri = 0;
+ unsigned int minslot = 0;
+ unsigned int i, j;
+
+ if (cpuif->overflow && cpuif->overflow->priority <= pri) {
+ /*
+ * There are already queued interrupts with the same or higher priority, just queue this one
+ */
+ set_vgic_queue_entry(cpuif, descr);
+ return;
+ }
+
+ /*
+ * Otherwise find the lowest priority entry..
+ */
+ for (i = 0; i < hv_lr_count[cpu]; i++) {
+ j = (get_vgic_entry(i) >> 20) & 0xFF; /* Get the priority for the current thing in this slot */
+ if (i == 0 || (j > minpri)) {
+ minpri = j;
+ minslot = i;
+ }
+ }
+
+ if (minpri > pri) {
+ /*
+ * If it's lower priority than this new one we kick it out
+ */
+ set_vgic_queue_entry(cpuif, get_vgic_entry(minslot));
+ set_vgic_entry(descr, minslot);
+ } else {
+ /*
+ * Otherwise just queue the new one
+ */
+ set_vgic_queue_entry(cpuif, descr);
+ }
+ }
}