1 files changed, 95 insertions, 0 deletions
diff --git a/arch/arm/mach-rpc/irq.c b/arch/arm/mach-rpc/irq.c
index 803aeb126f0e..dc29384b6ef8 100644
--- a/arch/arm/mach-rpc/irq.c
+++ b/arch/arm/mach-rpc/irq.c
@@ -14,6 +14,99 @@
 #define CLR	0x04
 #define MASK	0x08
 
+static const u8 irq_prio_h[256] = {
+	 0, 8, 9, 8,10,10,10,10,11,11,11,11,10,10,10,10,
+	12, 8, 9, 8,10,10,10,10,11,11,11,11,10,10,10,10,
+	13,13,13,13,10,10,10,10,11,11,11,11,10,10,10,10,
+	13,13,13,13,10,10,10,10,11,11,11,11,10,10,10,10,
+	14,14,14,14,10,10,10,10,11,11,11,11,10,10,10,10,
+	14,14,14,14,10,10,10,10,11,11,11,11,10,10,10,10,
+	13,13,13,13,10,10,10,10,11,11,11,11,10,10,10,10,
+	13,13,13,13,10,10,10,10,11,11,11,11,10,10,10,10,
+	15,15,15,15,10,10,10,10,11,11,11,11,10,10,10,10,
+	15,15,15,15,10,10,10,10,11,11,11,11,10,10,10,10,
+	13,13,13,13,10,10,10,10,11,11,11,11,10,10,10,10,
+	13,13,13,13,10,10,10,10,11,11,11,11,10,10,10,10,
+	15,15,15,15,10,10,10,10,11,11,11,11,10,10,10,10,
+	15,15,15,15,10,10,10,10,11,11,11,11,10,10,10,10,
+	13,13,13,13,10,10,10,10,11,11,11,11,10,10,10,10,
+	13,13,13,13,10,10,10,10,11,11,11,11,10,10,10,10,
+};
+
+static const u8 irq_prio_d[256] = {
+	 0,16,17,16,18,16,17,16,19,16,17,16,18,16,17,16,
+	20,16,17,16,18,16,17,16,19,16,17,16,18,16,17,16,
+	21,16,17,16,18,16,17,16,19,16,17,16,18,16,17,16,
+	21,16,17,16,18,16,17,16,19,16,17,16,18,16,17,16,
+	22,16,17,16,18,16,17,16,19,16,17,16,18,16,17,16,
+	22,16,17,16,18,16,17,16,19,16,17,16,18,16,17,16,
+	21,16,17,16,18,16,17,16,19,16,17,16,18,16,17,16,
+	21,16,17,16,18,16,17,16,19,16,17,16,18,16,17,16,
+	23,16,17,16,18,16,17,16,19,16,17,16,18,16,17,16,
+	23,16,17,16,18,16,17,16,19,16,17,16,18,16,17,16,
+	21,16,17,16,18,16,17,16,19,16,17,16,18,16,17,16,
+	21,16,17,16,18,16,17,16,19,16,17,16,18,16,17,16,
+	22,16,17,16,18,16,17,16,19,16,17,16,18,16,17,16,
+	22,16,17,16,18,16,17,16,19,16,17,16,18,16,17,16,
+	21,16,17,16,18,16,17,16,19,16,17,16,18,16,17,16,
+	21,16,17,16,18,16,17,16,19,16,17,16,18,16,17,16,
+};
+
+static const u8 irq_prio_l[256] = {
+	 0, 0, 1, 0, 2, 2, 2, 2, 3, 3, 3, 3, 3, 3, 3, 3,
+	 4, 0, 1, 0, 2, 2, 2, 2, 3, 3, 3, 3, 3, 3, 3, 3,
+	 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5,
+	 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5,
+	 6, 6, 6, 6, 6, 6, 6, 6, 3, 3, 3, 3, 3, 3, 3, 3,
+	 6, 6, 6, 6, 6, 6, 6, 6, 3, 3, 3, 3, 3, 3, 3, 3,
+	 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5,
+	 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5,
+	 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7,
+	 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7,
+	 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7,
+	 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7,
+	 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7,
+	 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7,
+	 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7,
+	 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7,
+};
+
+static int iomd_get_irq_nr(void)
+{
+	int irq;
+	u8 reg;
+
+	/* get highest priority first */
+	reg = readb(IOC_BASE + IOMD_IRQREQB);
+	irq = irq_prio_h[reg];
+	if (irq)
+		return irq;
+
+	/* get DMA  */
+	reg = readb(IOC_BASE + IOMD_DMAREQ);
+	irq = irq_prio_d[reg];
+	if (irq)
+		return irq;
+
+	/* get low priority */
+	reg = readb(IOC_BASE + IOMD_IRQREQA);
+	irq = irq_prio_l[reg];
+	if (irq)
+		return irq;
+	return 0;
+}
+
+static void iomd_handle_irq(struct pt_regs *regs)
+{
+	int irq;
+
+	do {
+		irq = iomd_get_irq_nr();
+		if (irq)
+			generic_handle_irq(irq);
+	} while (irq);
+}
+
 static void __iomem *iomd_get_base(struct irq_data *d)
 {
 	void *cd = irq_data_get_irq_chip_data(d);
@@ -82,6 +175,8 @@ void __init rpc_init_irq(void)
 	set_fiq_handler(&rpc_default_fiq_start,
 		&rpc_default_fiq_end - &rpc_default_fiq_start);
 
+	set_handle_irq(iomd_handle_irq);
+
 	for (irq = 0; irq < NR_IRQS; irq++) {
 		clr = IRQ_NOREQUEST;
 		set = 0;