[ARM] 3401/1: lpd7a40x: platform update

Patch from Marc Singer

Updates to the lpd7a40x_platform files.  Includes support for new
architecture, lpd7a400.
Signed-off-by: Marc Singer <elf@buici.com>
Signed-off-by: Russell King <rmk+kernel@arm.linux.org.uk>

arch/arm/mach-lh7a40x/Makefile

@@ -4,11 +4,14 @@
 
 # Object file lists.
 
-obj-y			:= time.o
+obj-y				:= time.o clocks.o
+obj-m				:=
+obj-n				:=
+obj-				:=
+
 obj-$(CONFIG_MACH_KEV7A400)	+= arch-kev7a400.o irq-lh7a400.o
 obj-$(CONFIG_MACH_LPD7A400)	+= arch-lpd7a40x.o irq-lh7a400.o
 obj-$(CONFIG_MACH_LPD7A404)	+= arch-lpd7a40x.o irq-lh7a404.o
+obj-$(CONFIG_LPD7A40X_CPLD_SSP)	+= ssp-cpld.o
+obj-$(CONFIG_FB_ARMCLCD)	+= clcd.o
 
-obj-m			:=
-obj-n			:=
-obj-			:=

arch/arm/mach-lh7a40x/arch-lpd7a40x.c

@@ -23,6 +23,28 @@
 
 #include "common.h"
 
+#define CPLD_INT_NETHERNET	(1<<0)
+#define CPLD_INTMASK_ETHERNET	(1<<2)
+#if defined (CONFIG_MACH_LPD7A400)
+# define CPLD_INT_NTOUCH		(1<<1)
+# define CPLD_INTMASK_TOUCH	(1<<3)
+# define CPLD_INT_PEN		(1<<4)
+# define CPLD_INTMASK_PEN	(1<<4)
+# define CPLD_INT_PIRQ		(1<<4)
+#endif
+#define CPLD_INTMASK_CPLD	(1<<7)
+#define CPLD_INT_CPLD		(1<<6)
+
+#define CPLD_CONTROL_SWINT		(1<<7) /* Disable all CPLD IRQs */
+#define CPLD_CONTROL_OCMSK		(1<<6) /* Mask USB1 connect IRQ */
+#define CPLD_CONTROL_PDRV		(1<<5) /* PCC_nDRV high */
+#define CPLD_CONTROL_USB1C		(1<<4) /* USB1 connect IRQ active */
+#define CPLD_CONTROL_USB1P		(1<<3) /* USB1 power disable */
+#define CPLD_CONTROL_AWKP		(1<<2) /* Auto-wakeup disabled  */
+#define CPLD_CONTROL_LCD_ENABLE		(1<<1) /* LCD Vee enable */
+#define CPLD_CONTROL_WRLAN_NENABLE	(1<<0) /* SMC91x power disable */
+
+
 static struct resource smc91x_resources[] = {
 	[0] = {
 		.start	= CPLD00_PHYS,
@@ -48,12 +70,12 @@ static struct platform_device smc91x_device = {
 static struct resource lh7a40x_usbclient_resources[] = {
 	[0] = {
 		.start	= USB_PHYS,
-		.end	= (USB_PHYS + 0xFF),
+		.end	= (USB_PHYS + PAGE_SIZE),
 		.flags	= IORESOURCE_MEM,
 	},
 	[1] = {
-		.start	= IRQ_USBINTR,
-		.end	= IRQ_USBINTR,
+		.start	= IRQ_USB,
+		.end	= IRQ_USB,
 		.flags	= IORESOURCE_IRQ,
 	},
 };
@@ -61,7 +83,8 @@ static struct resource lh7a40x_usbclient_resources[] = {
 static u64 lh7a40x_usbclient_dma_mask = 0xffffffffUL;
 
 static struct platform_device lh7a40x_usbclient_device = {
-	.name		= "lh7a40x_udc",
+//	.name		= "lh7a40x_udc",
+	.name		= "lh7-udc",
 	.id		= 0,
 	.dev		= {
 		.dma_mask = &lh7a40x_usbclient_dma_mask,
@@ -101,7 +124,7 @@ static struct platform_device lh7a404_usbhost_device = {
 
 #endif
 
-static struct platform_device *lpd7a40x_devs[] __initdata = {
+static struct platform_device* lpd7a40x_devs[] __initdata = {
 	&smc91x_device,
 	&lh7a40x_usbclient_device,
 #if defined (CONFIG_ARCH_LH7A404)
@@ -113,29 +136,52 @@ extern void lpd7a400_map_io (void);
 
 static void __init lpd7a40x_init (void)
 {
-	CPLD_CONTROL |=     (1<<6); /* Mask USB1 connection IRQ */
+#if defined (CONFIG_MACH_LPD7A400)
+	CPLD_CONTROL |= 0
+		| CPLD_CONTROL_SWINT /* Disable software interrupt */
+		| CPLD_CONTROL_OCMSK; /* Mask USB1 connection IRQ */
 	CPLD_CONTROL &= ~(0
-			  | (1<<1) /* Disable LCD */
-			  | (1<<0) /* Enable WLAN */
+			  | CPLD_CONTROL_LCD_ENABLE	/* Disable LCD */
+			  | CPLD_CONTROL_WRLAN_NENABLE	/* Enable SMC91x */
 		);
+#endif
+
+#if defined (CONFIG_MACH_LPD7A404)
+	CPLD_CONTROL &= ~(0
+			  | CPLD_CONTROL_WRLAN_NENABLE	/* Enable SMC91x */
+		);
+#endif
 
 	platform_add_devices (lpd7a40x_devs, ARRAY_SIZE (lpd7a40x_devs));
+#if defined (CONFIG_FB_ARMCLCD)
+        lh7a40x_clcd_init ();
+#endif
 }
 
 static void lh7a40x_ack_cpld_irq (u32 irq)
 {
-	/* CPLD doesn't have ack capability */
+	/* CPLD doesn't have ack capability, but some devices may */
+
+#if defined (CPLD_INTMASK_TOUCH)
+	/* The touch control *must* mask the the interrupt because the
+	 * interrupt bit is read by the driver to determine if the pen
+	 * is still down. */
+	if (irq == IRQ_TOUCH)
+		CPLD_INTERRUPTS |= CPLD_INTMASK_TOUCH;
+#endif
 }
 
 static void lh7a40x_mask_cpld_irq (u32 irq)
 {
 	switch (irq) {
 	case IRQ_LPD7A40X_ETH_INT:
-		CPLD_INTERRUPTS = CPLD_INTERRUPTS | 0x4;
+		CPLD_INTERRUPTS |= CPLD_INTMASK_ETHERNET;
 		break;
-	case IRQ_LPD7A400_TS:
-		CPLD_INTERRUPTS = CPLD_INTERRUPTS | 0x8;
+#if defined (IRQ_TOUCH)
+	case IRQ_TOUCH:
+		CPLD_INTERRUPTS |= CPLD_INTMASK_TOUCH;
 		break;
+#endif
 	}
 }
 
@@ -143,11 +189,13 @@ static void lh7a40x_unmask_cpld_irq (u32 irq)
 {
 	switch (irq) {
 	case IRQ_LPD7A40X_ETH_INT:
-		CPLD_INTERRUPTS = CPLD_INTERRUPTS & ~ 0x4;
+		CPLD_INTERRUPTS &= ~CPLD_INTMASK_ETHERNET;
 		break;
-	case IRQ_LPD7A400_TS:
-		CPLD_INTERRUPTS = CPLD_INTERRUPTS & ~ 0x8;
+#if defined (IRQ_TOUCH)
+	case IRQ_TOUCH:
+		CPLD_INTERRUPTS &= ~CPLD_INTMASK_TOUCH;
 		break;
+#endif
 	}
 }
 
@@ -164,11 +212,13 @@ static void lpd7a40x_cpld_handler (unsigned int irq, struct irqdesc *desc,
 
 	desc->chip->ack (irq);
 
-	if ((mask & 0x1) == 0)	/* WLAN */
+	if ((mask & (1<<0)) == 0)	/* WLAN */
 		IRQ_DISPATCH (IRQ_LPD7A40X_ETH_INT);
 
-	if ((mask & 0x2) == 0)	/* Touch */
-		IRQ_DISPATCH (IRQ_LPD7A400_TS);
+#if defined (IRQ_TOUCH)
+	if ((mask & (1<<1)) == 0)	/* Touch */
+		IRQ_DISPATCH (IRQ_TOUCH);
+#endif
 
 	desc->chip->unmask (irq); /* Level-triggered need this */
 }
@@ -204,9 +254,21 @@ void __init lh7a40x_init_board_irq (void)
 
 		/* Then, configure CPLD interrupt */
 
-	CPLD_INTERRUPTS	=   0x9c; /* Disable all CPLD interrupts */
+			/* Disable all CPLD interrupts */
+#if defined (CONFIG_MACH_LPD7A400)
+	CPLD_INTERRUPTS	= CPLD_INTMASK_TOUCH | CPLD_INTMASK_PEN
+		| CPLD_INTMASK_ETHERNET;
+	/* *** FIXME: don't know why we need 7 and 4. 7 is way wrong
+               and 4 is uncefined. */
+	// (1<<7)|(1<<4)|(1<<3)|(1<<2);
+#endif
+#if defined (CONFIG_MACH_LPD7A404)
+	CPLD_INTERRUPTS	= CPLD_INTMASK_ETHERNET;
+	/* *** FIXME: don't know why we need 6 and 5, neither is defined. */
+	// (1<<6)|(1<<5)|(1<<3);
+#endif
 	GPIO_PFDD	&= ~(1 << pinCPLD); /* Make input */
-	GPIO_INTTYPE1	|=  (1 << pinCPLD); /* Edge triggered */
+	GPIO_INTTYPE1	&= ~(1 << pinCPLD); /* Level triggered */
 	GPIO_INTTYPE2	&= ~(1 << pinCPLD); /* Active low */
 	barrier ();
 	GPIO_GPIOFINTEN |=  (1 << pinCPLD); /* Enable */
@@ -216,7 +278,7 @@ void __init lh7a40x_init_board_irq (void)
 	for (irq = IRQ_BOARD_START;
 	     irq < IRQ_BOARD_START + NR_IRQ_BOARD; ++irq) {
 		set_irq_chip (irq, &lpd7a40x_cpld_chip);
-		set_irq_handler (irq, do_edge_IRQ);
+		set_irq_handler (irq, do_level_IRQ);
 		set_irq_flags (irq, IRQF_VALID);
 	}
 
@@ -226,91 +288,109 @@ void __init lh7a40x_init_board_irq (void)
 				 lpd7a40x_cpld_handler);
 }
 
-static struct map_desc lpd7a400_io_desc[] __initdata = {
+static struct map_desc lpd7a40x_io_desc[] __initdata = {
 	{
 		.virtual	= IO_VIRT,
 		.pfn		= __phys_to_pfn(IO_PHYS),
 		.length		= IO_SIZE,
 		.type		= MT_DEVICE
-	}, {	/* Mapping added to work around chip select problems */
+	},
+	{	/* Mapping added to work around chip select problems */
 		.virtual	= IOBARRIER_VIRT,
 		.pfn		= __phys_to_pfn(IOBARRIER_PHYS),
 		.length		= IOBARRIER_SIZE,
 		.type		= MT_DEVICE
-	}, {
+	},
+	{
 		.virtual	= CF_VIRT,
 		.pfn		= __phys_to_pfn(CF_PHYS),
 		.length		= CF_SIZE,
 		.type		= MT_DEVICE
-	}, {
+	},
+	{
 		.virtual	= CPLD02_VIRT,
 		.pfn		= __phys_to_pfn(CPLD02_PHYS),
 		.length		= CPLD02_SIZE,
 		.type		= MT_DEVICE
-	}, {
+	},
+	{
 		.virtual	= CPLD06_VIRT,
 		.pfn		= __phys_to_pfn(CPLD06_PHYS),
 		.length		= CPLD06_SIZE,
 		.type		= MT_DEVICE
-	}, {
+	},
+	{
 		.virtual	= CPLD08_VIRT,
 		.pfn		= __phys_to_pfn(CPLD08_PHYS),
 		.length		= CPLD08_SIZE,
 		.type		= MT_DEVICE
-	}, {
+	},
+	{
+		.virtual	= CPLD08_VIRT,
+		.pfn		= __phys_to_pfn(CPLD08_PHYS),
+		.length		= CPLD08_SIZE,
+		.type		= MT_DEVICE
+	},
+	{
+		.virtual	= CPLD0A_VIRT,
+		.pfn		= __phys_to_pfn(CPLD0A_PHYS),
+		.length		= CPLD0A_SIZE,
+		.type		= MT_DEVICE
+	},
+	{
 		.virtual	= CPLD0C_VIRT,
 		.pfn		= __phys_to_pfn(CPLD0C_PHYS),
 		.length		= CPLD0C_SIZE,
 		.type		= MT_DEVICE
-	}, {
+	},
+	{
 		.virtual	= CPLD0E_VIRT,
 		.pfn		= __phys_to_pfn(CPLD0E_PHYS),
 		.length		= CPLD0E_SIZE,
 		.type		= MT_DEVICE
-	}, {
+	},
+	{
 		.virtual	= CPLD10_VIRT,
 		.pfn		= __phys_to_pfn(CPLD10_PHYS),
 		.length		= CPLD10_SIZE,
 		.type		= MT_DEVICE
-	}, {
+	},
+	{
 		.virtual	= CPLD12_VIRT,
 		.pfn		= __phys_to_pfn(CPLD12_PHYS),
 		.length		= CPLD12_SIZE,
 		.type		= MT_DEVICE
-	}, {
+	},
+	{
 		.virtual	= CPLD14_VIRT,
 		.pfn		= __phys_to_pfn(CPLD14_PHYS),
 		.length		= CPLD14_SIZE,
 		.type		= MT_DEVICE
-	}, {
+	},
+	{
 		.virtual	= CPLD16_VIRT,
 		.pfn		= __phys_to_pfn(CPLD16_PHYS),
 		.length		= CPLD16_SIZE,
 		.type		= MT_DEVICE
-	}, {
+	},
+	{
 		.virtual	= CPLD18_VIRT,
 		.pfn		= __phys_to_pfn(CPLD18_PHYS),
 		.length		= CPLD18_SIZE,
 		.type		= MT_DEVICE
-	}, {
+	},
+	{
 		.virtual	= CPLD1A_VIRT,
 		.pfn		= __phys_to_pfn(CPLD1A_PHYS),
 		.length		= CPLD1A_SIZE,
 		.type		= MT_DEVICE
 	},
-	/* This mapping is redundant since the smc driver performs another. */
-/*	{ CPLD00_VIRT,  CPLD00_PHYS,	CPLD00_SIZE,	MT_DEVICE }, */
 };
 
 void __init
-lpd7a400_map_io(void)
+lpd7a40x_map_io(void)
 {
-	iotable_init (lpd7a400_io_desc, ARRAY_SIZE (lpd7a400_io_desc));
-
-		/* Fixup (improve) Static Memory Controller settings */
-	SMC_BCR0 = 0x200039af;	/* Boot Flash */
-	SMC_BCR6 = 0x1000fbe0;	/* CPLD */
-	SMC_BCR7 = 0x1000b2c2;	/* Compact Flash */
+	iotable_init (lpd7a40x_io_desc, ARRAY_SIZE (lpd7a40x_io_desc));
 }
 
 #ifdef CONFIG_MACH_LPD7A400
@@ -320,7 +400,7 @@ MACHINE_START (LPD7A400, "Logic Product Development LPD7A400-10")
 	.phys_io	= 0x80000000,
 	.io_pg_offst	= ((io_p2v (0x80000000))>>18) & 0xfffc,
 	.boot_params	= 0xc0000100,
-	.map_io		= lpd7a400_map_io,
+	.map_io		= lpd7a40x_map_io,
 	.init_irq	= lh7a400_init_irq,
 	.timer		= &lh7a40x_timer,
 	.init_machine	= lpd7a40x_init,
@@ -335,7 +415,7 @@ MACHINE_START (LPD7A404, "Logic Product Development LPD7A404-10")
 	.phys_io	= 0x80000000,
 	.io_pg_offst	= ((io_p2v (0x80000000))>>18) & 0xfffc,
 	.boot_params	= 0xc0000100,
-	.map_io		= lpd7a400_map_io,
+	.map_io		= lpd7a40x_map_io,
 	.init_irq	= lh7a404_init_irq,
 	.timer		= &lh7a40x_timer,
 	.init_machine	= lpd7a40x_init,

arch/arm/mach-lh7a40x/clocks.c

+/* arch/arm/mach-lh7a40x/clocks.c
+ *
+ *  Copyright (C) 2004 Marc Singer
+ *
+ *  This program is free software; you can redistribute it and/or
+ *  modify it under the terms of the GNU General Public License
+ *  version 2 as published by the Free Software Foundation.
+ *
+ */
+
+#include <linux/config.h>
+#include <linux/cpufreq.h>
+#include <asm/hardware.h>
+#include <asm/arch/clocks.h>
+#include <linux/err.h>
+
+struct module;
+struct icst525_params;
+
+struct clk {
+	struct list_head node;
+	unsigned long rate;
+	struct module *owner;
+	const char *name;
+//	void *data;
+//	const struct icst525_params *params;
+//	void (*setvco)(struct clk *, struct icst525_vco vco);
+};
+
+int clk_register(struct clk *clk);
+void clk_unregister(struct clk *clk);
+
+/* ----- */
+
+#define MAINDIV1(c)	(((c) >>  7) & 0x0f)
+#define MAINDIV2(c)	(((c) >> 11) & 0x1f)
+#define PS(c)		(((c) >> 18) & 0x03)
+#define PREDIV(c)	(((c) >>  2) & 0x1f)
+#define HCLKDIV(c)	(((c) >>  0) & 0x02)
+#define PCLKDIV(c)	(((c) >> 16) & 0x03)
+
+unsigned int cpufreq_get (unsigned int cpu) /* in kHz */
+{
+	return fclkfreq_get ()/1000;
+}
+EXPORT_SYMBOL(cpufreq_get);
+
+unsigned int fclkfreq_get (void)
+{
+	unsigned int clkset = CSC_CLKSET;
+	unsigned int gclk
+		= XTAL_IN
+		/ (1 << PS(clkset))
+		* (MAINDIV1(clkset) + 2)
+		/ (PREDIV(clkset)   + 2)
+		* (MAINDIV2(clkset) + 2)
+		;
+	return gclk;
+}
+
+unsigned int hclkfreq_get (void)
+{
+	unsigned int clkset = CSC_CLKSET;
+	unsigned int hclk = fclkfreq_get () / (HCLKDIV(clkset) + 1);
+
+	return hclk;
+}
+
+unsigned int pclkfreq_get (void)
+{
+	unsigned int clkset = CSC_CLKSET;
+	int pclkdiv = PCLKDIV(clkset);
+	unsigned int pclk;
+	if (pclkdiv == 0x3)
+		pclkdiv = 0x2;
+	pclk = hclkfreq_get () / (1 << pclkdiv);
+
+	return pclk;
+}
+
+/* ----- */
+
+static LIST_HEAD(clocks);
+static DECLARE_MUTEX(clocks_sem);
+
+struct clk *clk_get (struct device *dev, const char *id)
+{
+	struct clk *p;
+	struct clk *clk = ERR_PTR(-ENOENT);
+
+	down (&clocks_sem);
+	list_for_each_entry (p, &clocks, node) {
+		if (strcmp (id, p->name) == 0
+		    && try_module_get(p->owner)) {
+			clk = p;
+			break;
+		}
+	}
+	up (&clocks_sem);
+
+	return clk;
+}
+EXPORT_SYMBOL(clk_get);
+
+void clk_put (struct clk *clk)
+{
+	module_put(clk->owner);
+}
+EXPORT_SYMBOL(clk_put);
+
+int clk_enable (struct clk *clk)
+{
+	return 0;
+}
+EXPORT_SYMBOL(clk_enable);
+
+void clk_disable (struct clk *clk)
+{
+}
+EXPORT_SYMBOL(clk_disable);
+
+int clk_use (struct clk *clk)
+{
+	return 0;
+}
+EXPORT_SYMBOL(clk_use);
+
+void clk_unuse (struct clk *clk)
+{
+}
+EXPORT_SYMBOL(clk_unuse);
+
+unsigned long clk_get_rate (struct clk *clk)
+{
+	return clk->rate;
+}
+EXPORT_SYMBOL(clk_get_rate);
+
+long clk_round_rate (struct clk *clk, unsigned long rate)
+{
+	return rate;
+}
+EXPORT_SYMBOL(clk_round_rate);
+
+int clk_set_rate (struct clk *clk, unsigned long rate)
+{
+	int ret = -EIO;
+	return ret;
+}
+EXPORT_SYMBOL(clk_set_rate);
+
+#if 0
+/*
+ * These are fixed clocks.
+ */
+static struct clk kmi_clk = {
+	.name	= "KMIREFCLK",
+	.rate	= 24000000,
+};
+
+static struct clk uart_clk = {
+	.name	= "UARTCLK",
+	.rate	= 24000000,
+};
+
+static struct clk mmci_clk = {
+	.name	= "MCLK",
+	.rate	= 33000000,
+};
+#endif
+
+static struct clk clcd_clk = {
+	.name	= "CLCDCLK",
+	.rate	= 0,
+};
+
+int clk_register (struct clk *clk)
+{
+	down (&clocks_sem);
+	list_add (&clk->node, &clocks);
+	up (&clocks_sem);
+	return 0;
+}
+EXPORT_SYMBOL(clk_register);
+
+void clk_unregister (struct clk *clk)
+{
+	down (&clocks_sem);
+	list_del (&clk->node);
+	up (&clocks_sem);
+}
+EXPORT_SYMBOL(clk_unregister);
+
+static int __init clk_init (void)
+{
+	clk_register(&clcd_clk);
+	return 0;
+}
+arch_initcall(clk_init);

arch/arm/mach-lh7a40x/irq-lh7a404.c

@@ -28,13 +28,17 @@
 
 static unsigned char irq_pri_vic1[] = {
 #if defined (USE_PRIORITIES)
-IRQ_GPIO3INTR,
+	IRQ_GPIO3INTR,			/* CPLD */
+	IRQ_DMAM2P4, IRQ_DMAM2P5,	/* AC97 */
 #endif
 };
 static unsigned char irq_pri_vic2[] = {
 #if defined (USE_PRIORITIES)
-	IRQ_T3UI, IRQ_GPIO7INTR,
+	IRQ_T3UI,			/* Timer */
+	IRQ_GPIO7INTR,			/* CPLD */
 	IRQ_UART1INTR, IRQ_UART2INTR, IRQ_UART3INTR,
+	IRQ_LCDINTR,			/* LCD */
+	IRQ_TSCINTR,			/* ADC/Touchscreen */
 #endif
 };
 
@@ -98,10 +102,19 @@ static struct irqchip lh7a404_gpio_vic2_chip = {
 
   /* IRQ initialization */
 
+#if defined (CONFIG_ARCH_LH7A400) && defined (CONFIG_ARCH_LH7A404)
+extern void* branch_irq_lh7a400;
+#endif
+
 void __init lh7a404_init_irq (void)
 {
 	int irq;
 
+#if defined (CONFIG_ARCH_LH7A400) && defined (CONFIG_ARCH_LH7A404)
+#define NOP 0xe1a00000			/* mov r0, r0 */
+	branch_irq_lh7a400 = NOP;
+#endif
+
 	VIC1_INTENCLR = 0xffffffff;
 	VIC2_INTENCLR = 0xffffffff;
 	VIC1_INTSEL = 0;		/* All IRQs */

arch/arm/mach-lh7a40x/time.c

@@ -57,7 +57,7 @@ static struct irqaction lh7a40x_timer_irq = {
 	.handler	= lh7a40x_timer_interrupt,
 };
 
-static void __init lh7a40x_timer_init(void)
+static void __init lh7a40x_timer_init (void)
 {
 				/* Stop/disable all timers */
 	TIMER_CONTROL1 = 0;

include/asm-arm/arch-lh7a40x/entry-macro.S

@@ -10,11 +10,73 @@
 #include <asm/hardware.h>
 #include <asm/arch/irqs.h>
 
-# if defined (CONFIG_ARCH_LH7A400) && defined (CONFIG_ARCH_LH7A404)
-#  error "LH7A400 and LH7A404 are mutually exclusive"
-# endif
+/* In order to allow there to be support for both of the processor
+   classes at the same time, we make a hack here that isn't very
+   pretty.  At startup, the link pointed to with the
+   branch_irq_lh7a400 symbol is replaced with a NOP when the CPU is
+   detected as a lh7a404.
 
-# if defined (CONFIG_ARCH_LH7A400)
+   *** FIXME: we should clean this up so that there is only one
+	      implementation for each CPU's design.
+
+*/
+
+#if defined (CONFIG_ARCH_LH7A400) && defined (CONFIG_ARCH_LH7A404)
+
+		.macro	disable_fiq
+		.endm
+
+		.macro	get_irqnr_and_base, irqnr, irqstat, base, tmp
+
+branch_irq_lh7a400: b 1000f
+
+@ Implementation of the LH7A404 get_irqnr_and_base.
+
+		mov	\irqnr, #0			@ VIC1 irq base
+		mov	\base, #io_p2v(0x80000000)	@ APB registers
+		add	\base, \base, #0x8000
+		ldr	\tmp, [\base, #0x0030]		@ VIC1_VECTADDR
+		tst	\tmp, #VA_VECTORED		@ Direct vectored
+		bne	1002f
+		tst	\tmp, #VA_VIC1DEFAULT		@ Default vectored VIC1
+		ldrne	\irqstat, [\base, #0]		@ VIC1_IRQSTATUS
+		bne	1001f
+		add	\base, \base, #(0xa000 - 0x8000)
+		ldr	\tmp, [\base, #0x0030]		@ VIC2_VECTADDR
+		tst	\tmp, #VA_VECTORED		@ Direct vectored
+		bne	1002f
+		ldr	\irqstat, [\base, #0]		@ VIC2_IRQSTATUS
+		mov	\irqnr, #32			@ VIC2 irq base
+
+1001:		movs	\irqstat, \irqstat, lsr #1	@ Shift into carry
+		bcs	1008f				@ Bit set; irq found
+		add	\irqnr, \irqnr, #1
+		bne	1001b				@ Until no bits
+		b	1009f				@ Nothing?  Hmm.
+1002:		and	\irqnr, \tmp, #0x3f		@ Mask for valid bits
+1008:		movs	\irqstat, #1			@ Force !Z
+		str	\tmp, [\base, #0x0030]		@ Clear vector
+		b	1009f
+
+@ Implementation of the LH7A400 get_irqnr_and_base.
+
+1000:		mov	\irqnr, #0
+		mov	\base, #io_p2v(0x80000000)	@ APB registers
+		ldr	\irqstat, [\base, #0x500]	@ PIC INTSR
+
+1001:		movs	\irqstat, \irqstat, lsr #1	@ Shift into carry
+		bcs	1008f				@ Bit set; irq found
+		add	\irqnr, \irqnr, #1
+		bne	1001b				@ Until no bits
+		b	1009f				@ Nothing?  Hmm.
+1008:		movs	\irqstat, #1			@ Force !Z
+
+1009:
+               .endm
+
+
+
+#elif defined (CONFIG_ARCH_LH7A400)
 		.macro	disable_fiq
 		.endm