rewrite arm_get_next_irq to always make forward progress (should be optimized)

1 files changed, 20 insertions, 8 deletions

diff --git a/sys/arm/xscale/ixp425/ixp425.c b/sys/arm/xscale/ixp425/ixp425.c
index f19f973..ffe48ae 100644
--- a/sys/arm/xscale/ixp425/ixp425.c
+++ b/sys/arm/xscale/ixp425/ixp425.c
@@ -214,15 +214,27 @@ ixp435_irq_read(void)
 }
 
 int
-arm_get_next_irq(int last __unused)
+arm_get_next_irq(int last)
 {
-	uint32_t irq;
-
-	if ((irq = ixp425_irq_read()))
-		return (ffs(irq) - 1);
-	if (cpu_is_ixp43x() && (irq = ixp435_irq_read()))
-		return (32 + ffs(irq) - 1);
-	return (-1);
+	uint32_t mask;
+
+	last += 1;		/* always advance fwd, NB: handles -1 */
+	if (last < 32) {
+		mask = ixp425_irq_read() >> last;
+		for (; mask != 0; mask >>= 1, last += 1) {
+			if (mask & 1)
+				return last;
+		}
+		last = 32;
+	}
+	if (cpu_is_ixp43x()) {
+		mask = ixp435_irq_read() >> (32-last);
+		for (; mask != 0; mask >>= 1, last++) {
+			if (mask & 1)
+				return last;
+		}
+	}
+	return -1;
 }
 
 void