7 files changed, 687 insertions, 47 deletions

diff --git a/arch/arm/mach-ixp4xx/Kconfig b/arch/arm/mach-ixp4xx/Kconfig
index 2c5a02b8520..264f4d59f89 100644
--- a/arch/arm/mach-ixp4xx/Kconfig
+++ b/arch/arm/mach-ixp4xx/Kconfig
@@ -78,6 +78,12 @@ config MACH_IXDP465
           IXDP465 Development Platform (Also known as BMP).
           For more information on this platform, see <file:Documentation/arm/IXP4xx>.
 
+config MACH_GORAMO_MLR
+        bool "GORAMO Multi Link Router"
+        help
+          Say 'Y' here if you want your kernel to support GORAMO
+          MultiLink router.
+
 config MACH_KIXRP435
         bool "KIXRP435"
         help
diff --git a/arch/arm/mach-ixp4xx/Makefile b/arch/arm/mach-ixp4xx/Makefile
index 2e6bbf927a7..47d1f60d23f 100644
--- a/arch/arm/mach-ixp4xx/Makefile
+++ b/arch/arm/mach-ixp4xx/Makefile
@@ -30,6 +30,7 @@ obj-$(CONFIG_MACH_DSMG600)      += dsmg600-setup.o
 obj-$(CONFIG_MACH_GATEWAY7001)  += gateway7001-setup.o
 obj-$(CONFIG_MACH_WG302V2)      += wg302v2-setup.o
 obj-$(CONFIG_MACH_FSG)          += fsg-setup.o
+obj-$(CONFIG_MACH_GORAMO_MLR)   += goramo_mlr.o
 
 obj-$(CONFIG_PCI)               += $(obj-pci-$(CONFIG_PCI)) common-pci.o
 obj-$(CONFIG_IXP4XX_QMGR)       += ixp4xx_qmgr.o
diff --git a/arch/arm/mach-ixp4xx/goramo_mlr.c b/arch/arm/mach-ixp4xx/goramo_mlr.c
new file mode 100644
index 00000000000..a733b8ff3ce
--- /dev/null
+++ b/arch/arm/mach-ixp4xx/goramo_mlr.c
@@ -0,0 +1,507 @@
+/*
+ * Goramo MultiLink router platform code
+ * Copyright (C) 2006-2009 Krzysztof Halasa <khc@pm.waw.pl>
+ */
+
+#include <linux/delay.h>
+#include <linux/hdlc.h>
+#include <linux/i2c-gpio.h>
+#include <linux/io.h>
+#include <linux/irq.h>
+#include <linux/kernel.h>
+#include <linux/pci.h>
+#include <linux/serial_8250.h>
+#include <asm/mach-types.h>
+#include <asm/system.h>
+#include <asm/mach/arch.h>
+#include <asm/mach/flash.h>
+#include <asm/mach/pci.h>
+
+#define xgpio_irq(n)            (IRQ_IXP4XX_GPIO ## n)
+#define gpio_irq(n)             xgpio_irq(n)
+
+#define SLOT_ETHA               0x0B    /* IDSEL = AD21 */
+#define SLOT_ETHB               0x0C    /* IDSEL = AD20 */
+#define SLOT_MPCI               0x0D    /* IDSEL = AD19 */
+#define SLOT_NEC                0x0E    /* IDSEL = AD18 */
+
+#define IRQ_ETHA                IRQ_IXP4XX_GPIO4
+#define IRQ_ETHB                IRQ_IXP4XX_GPIO5
+#define IRQ_NEC                 IRQ_IXP4XX_GPIO3
+#define IRQ_MPCI                IRQ_IXP4XX_GPIO12
+
+/* GPIO lines */
+#define GPIO_SCL                0
+#define GPIO_SDA                1
+#define GPIO_STR                2
+#define GPIO_HSS0_DCD_N         6
+#define GPIO_HSS1_DCD_N         7
+#define GPIO_HSS0_CTS_N         10
+#define GPIO_HSS1_CTS_N         11
+#define GPIO_HSS1_RTS_N         13
+#define GPIO_HSS0_RTS_N         14
+
+/* Control outputs from 74HC4094 */
+#define CONTROL_HSS0_CLK_INT    0
+#define CONTROL_HSS1_CLK_INT    1
+#define CONTROL_HSS0_DTR_N      2
+#define CONTROL_HSS1_DTR_N      3
+#define CONTROL_EXT             4
+#define CONTROL_AUTO_RESET      5
+#define CONTROL_PCI_RESET_N     6
+#define CONTROL_EEPROM_WC_N     7
+
+/* offsets from start of flash ROM = 0x50000000 */
+#define CFG_ETH0_ADDRESS        0x40 /* 6 bytes */
+#define CFG_ETH1_ADDRESS        0x46 /* 6 bytes */
+#define CFG_REV                 0x4C /* u32 */
+#define CFG_SDRAM_SIZE          0x50 /* u32 */
+#define CFG_SDRAM_CONF          0x54 /* u32 */
+#define CFG_SDRAM_MODE          0x58 /* u32 */
+#define CFG_SDRAM_REFRESH       0x5C /* u32 */
+
+#define CFG_HW_BITS             0x60 /* u32 */
+#define  CFG_HW_USB_PORTS       0x00000007 /* 0 = no NEC chip, 1-5 = ports # */
+#define  CFG_HW_HAS_PCI_SLOT    0x00000008
+#define  CFG_HW_HAS_ETH0        0x00000010
+#define  CFG_HW_HAS_ETH1        0x00000020
+#define  CFG_HW_HAS_HSS0        0x00000040
+#define  CFG_HW_HAS_HSS1        0x00000080
+#define  CFG_HW_HAS_UART0       0x00000100
+#define  CFG_HW_HAS_UART1       0x00000200
+#define  CFG_HW_HAS_EEPROM      0x00000400
+
+#define FLASH_CMD_READ_ARRAY    0xFF
+#define FLASH_CMD_READ_ID       0x90
+#define FLASH_SER_OFF           0x102 /* 0x81 in 16-bit mode */
+
+static u32 hw_bits = 0xFFFFFFFD;    /* assume all hardware present */;
+static u8 control_value;
+
+static void set_scl(u8 value)
+{
+        gpio_line_set(GPIO_SCL, !!value);
+        udelay(3);
+}
+
+static void set_sda(u8 value)
+{
+        gpio_line_set(GPIO_SDA, !!value);
+        udelay(3);
+}
+
+static void set_str(u8 value)
+{
+        gpio_line_set(GPIO_STR, !!value);
+        udelay(3);
+}
+
+static inline void set_control(int line, int value)
+{
+        if (value)
+                control_value |=  (1 << line);
+        else
+                control_value &= ~(1 << line);
+}
+
+
+static void output_control(void)
+{
+        int i;
+
+        gpio_line_config(GPIO_SCL, IXP4XX_GPIO_OUT);
+        gpio_line_config(GPIO_SDA, IXP4XX_GPIO_OUT);
+
+        for (i = 0; i < 8; i++) {
+                set_scl(0);
+                set_sda(control_value & (0x80 >> i)); /* MSB first */
+                set_scl(1);     /* active edge */
+        }
+
+        set_str(1);
+        set_str(0);
+
+        set_scl(0);
+        set_sda(1);             /* Be ready for START */
+        set_scl(1);
+}
+
+
+static void (*set_carrier_cb_tab[2])(void *pdev, int carrier);
+
+static int hss_set_clock(int port, unsigned int clock_type)
+{
+        int ctrl_int = port ? CONTROL_HSS1_CLK_INT : CONTROL_HSS0_CLK_INT;
+
+        switch (clock_type) {
+        case CLOCK_DEFAULT:
+        case CLOCK_EXT:
+                set_control(ctrl_int, 0);
+                output_control();
+                return CLOCK_EXT;
+
+        case CLOCK_INT:
+                set_control(ctrl_int, 1);
+                output_control();
+                return CLOCK_INT;
+
+        default:
+                return -EINVAL;
+        }
+}
+
+static irqreturn_t hss_dcd_irq(int irq, void *pdev)
+{
+        int i, port = (irq == gpio_irq(GPIO_HSS1_DCD_N));
+        gpio_line_get(port ? GPIO_HSS1_DCD_N : GPIO_HSS0_DCD_N, &i);
+        set_carrier_cb_tab[port](pdev, !i);
+        return IRQ_HANDLED;
+}
+
+
+static int hss_open(int port, void *pdev,
+                    void (*set_carrier_cb)(void *pdev, int carrier))
+{
+        int i, irq;
+
+        if (!port)
+                irq = gpio_irq(GPIO_HSS0_DCD_N);
+        else
+                irq = gpio_irq(GPIO_HSS1_DCD_N);
+
+        gpio_line_get(port ? GPIO_HSS1_DCD_N : GPIO_HSS0_DCD_N, &i);
+        set_carrier_cb(pdev, !i);
+
+        set_carrier_cb_tab[!!port] = set_carrier_cb;
+
+        if ((i = request_irq(irq, hss_dcd_irq, 0, "IXP4xx HSS", pdev)) != 0) {
+                printk(KERN_ERR "ixp4xx_hss: failed to request IRQ%i (%i)\n",
+                       irq, i);
+                return i;
+        }
+
+        set_control(port ? CONTROL_HSS1_DTR_N : CONTROL_HSS0_DTR_N, 0);
+        output_control();
+        gpio_line_set(port ? GPIO_HSS1_RTS_N : GPIO_HSS0_RTS_N, 0);
+        return 0;
+}
+
+static void hss_close(int port, void *pdev)
+{
+        free_irq(port ? gpio_irq(GPIO_HSS1_DCD_N) : gpio_irq(GPIO_HSS0_DCD_N),
+                 pdev);
+        set_carrier_cb_tab[!!port] = NULL; /* catch bugs */
+
+        set_control(port ? CONTROL_HSS1_DTR_N : CONTROL_HSS0_DTR_N, 1);
+        output_control();
+        gpio_line_set(port ? GPIO_HSS1_RTS_N : GPIO_HSS0_RTS_N, 1);
+}
+
+
+/* Flash memory */
+static struct flash_platform_data flash_data = {
+        .map_name       = "cfi_probe",
+        .width          = 2,
+};
+
+static struct resource flash_resource = {
+        .flags          = IORESOURCE_MEM,
+};
+
+static struct platform_device device_flash = {
+        .name           = "IXP4XX-Flash",
+        .id             = 0,
+        .dev            = { .platform_data = &flash_data },
+        .num_resources  = 1,
+        .resource       = &flash_resource,
+};
+
+
+/* I^2C interface */
+static struct i2c_gpio_platform_data i2c_data = {
+        .sda_pin        = GPIO_SDA,
+        .scl_pin        = GPIO_SCL,
+};
+
+static struct platform_device device_i2c = {
+        .name           = "i2c-gpio",
+        .id             = 0,
+        .dev            = { .platform_data = &i2c_data },
+};
+
+
+/* IXP425 2 UART ports */
+static struct resource uart_resources[] = {
+        {
+                .start          = IXP4XX_UART1_BASE_PHYS,
+                .end            = IXP4XX_UART1_BASE_PHYS + 0x0fff,
+                .flags          = IORESOURCE_MEM,
+        },
+        {
+                .start          = IXP4XX_UART2_BASE_PHYS,
+                .end            = IXP4XX_UART2_BASE_PHYS + 0x0fff,
+                .flags          = IORESOURCE_MEM,
+        }
+};
+
+static struct plat_serial8250_port uart_data[] = {
+        {
+                .mapbase        = IXP4XX_UART1_BASE_PHYS,
+                .membase        = (char __iomem *)IXP4XX_UART1_BASE_VIRT +
+                        REG_OFFSET,
+                .irq            = IRQ_IXP4XX_UART1,
+                .flags          = UPF_BOOT_AUTOCONF | UPF_SKIP_TEST,
+                .iotype         = UPIO_MEM,
+                .regshift       = 2,
+                .uartclk        = IXP4XX_UART_XTAL,
+        },
+        {
+                .mapbase        = IXP4XX_UART2_BASE_PHYS,
+                .membase        = (char __iomem *)IXP4XX_UART2_BASE_VIRT +
+                        REG_OFFSET,
+                .irq            = IRQ_IXP4XX_UART2,
+                .flags          = UPF_BOOT_AUTOCONF | UPF_SKIP_TEST,
+                .iotype         = UPIO_MEM,
+                .regshift       = 2,
+                .uartclk        = IXP4XX_UART_XTAL,
+        },
+        { },
+};
+
+static struct platform_device device_uarts = {
+        .name                   = "serial8250",
+        .id                     = PLAT8250_DEV_PLATFORM,
+        .dev.platform_data      = uart_data,
+        .num_resources          = 2,
+        .resource               = uart_resources,
+};
+
+
+/* Built-in 10/100 Ethernet MAC interfaces */
+static struct eth_plat_info eth_plat[] = {
+        {
+                .phy            = 0,
+                .rxq            = 3,
+                .txreadyq       = 32,
+        }, {
+                .phy            = 1,
+                .rxq            = 4,
+                .txreadyq       = 33,
+        }
+};
+
+static struct platform_device device_eth_tab[] = {
+        {
+                .name                   = "ixp4xx_eth",
+                .id                     = IXP4XX_ETH_NPEB,
+                .dev.platform_data      = eth_plat,
+        }, {
+                .name                   = "ixp4xx_eth",
+                .id                     = IXP4XX_ETH_NPEC,
+                .dev.platform_data      = eth_plat + 1,
+        }
+};
+
+
+/* IXP425 2 synchronous serial ports */
+static struct hss_plat_info hss_plat[] = {
+        {
+                .set_clock      = hss_set_clock,
+                .open           = hss_open,
+                .close          = hss_close,
+                .txreadyq       = 34,
+        }, {
+                .set_clock      = hss_set_clock,
+                .open           = hss_open,
+                .close          = hss_close,
+                .txreadyq       = 35,
+        }
+};
+
+static struct platform_device device_hss_tab[] = {
+        {
+                .name                   = "ixp4xx_hss",
+                .id                     = 0,
+                .dev.platform_data      = hss_plat,
+        }, {
+                .name                   = "ixp4xx_hss",
+                .id                     = 1,
+                .dev.platform_data      = hss_plat + 1,
+        }
+};
+
+
+static struct platform_device *device_tab[6] __initdata = {
+        &device_flash,          /* index 0 */
+};
+
+static inline u8 __init flash_readb(u8 __iomem *flash, u32 addr)
+{
+#ifdef __ARMEB__
+        return __raw_readb(flash + addr);
+#else
+        return __raw_readb(flash + (addr ^ 3));
+#endif
+}
+
+static inline u16 __init flash_readw(u8 __iomem *flash, u32 addr)
+{
+#ifdef __ARMEB__
+        return __raw_readw(flash + addr);
+#else
+        return __raw_readw(flash + (addr ^ 2));
+#endif
+}
+
+static void __init gmlr_init(void)
+{
+        u8 __iomem *flash;
+        int i, devices = 1; /* flash */
+
+        ixp4xx_sys_init();
+
+        if ((flash = ioremap(IXP4XX_EXP_BUS_BASE_PHYS, 0x80)) == NULL)
+                printk(KERN_ERR "goramo-mlr: unable to access system"
+                       " configuration data\n");
+        else {
+                system_rev = __raw_readl(flash + CFG_REV);
+                hw_bits = __raw_readl(flash + CFG_HW_BITS);
+
+                for (i = 0; i < ETH_ALEN; i++) {
+                        eth_plat[0].hwaddr[i] =
+                                flash_readb(flash, CFG_ETH0_ADDRESS + i);
+                        eth_plat[1].hwaddr[i] =
+                                flash_readb(flash, CFG_ETH1_ADDRESS + i);
+                }
+
+                __raw_writew(FLASH_CMD_READ_ID, flash);
+                system_serial_high = flash_readw(flash, FLASH_SER_OFF);
+                system_serial_high <<= 16;
+                system_serial_high |= flash_readw(flash, FLASH_SER_OFF + 2);
+                system_serial_low = flash_readw(flash, FLASH_SER_OFF + 4);
+                system_serial_low <<= 16;
+                system_serial_low |= flash_readw(flash, FLASH_SER_OFF + 6);
+                __raw_writew(FLASH_CMD_READ_ARRAY, flash);
+
+                iounmap(flash);
+        }
+
+        switch (hw_bits & (CFG_HW_HAS_UART0 | CFG_HW_HAS_UART1)) {
+        case CFG_HW_HAS_UART0:
+                memset(&uart_data[1], 0, sizeof(uart_data[1]));
+                device_uarts.num_resources = 1;
+                break;
+
+        case CFG_HW_HAS_UART1:
+                device_uarts.dev.platform_data = &uart_data[1];
+                device_uarts.resource = &uart_resources[1];
+                device_uarts.num_resources = 1;
+                break;
+        }
+        if (hw_bits & (CFG_HW_HAS_UART0 | CFG_HW_HAS_UART1))
+                device_tab[devices++] = &device_uarts; /* max index 1 */
+
+        if (hw_bits & CFG_HW_HAS_ETH0)
+                device_tab[devices++] = &device_eth_tab[0]; /* max index 2 */
+        if (hw_bits & CFG_HW_HAS_ETH1)
+                device_tab[devices++] = &device_eth_tab[1]; /* max index 3 */
+
+        if (hw_bits & CFG_HW_HAS_HSS0)
+                device_tab[devices++] = &device_hss_tab[0]; /* max index 4 */
+        if (hw_bits & CFG_HW_HAS_HSS1)
+                device_tab[devices++] = &device_hss_tab[1]; /* max index 5 */
+
+        if (hw_bits & CFG_HW_HAS_EEPROM)
+                device_tab[devices++] = &device_i2c; /* max index 6 */
+
+        gpio_line_config(GPIO_SCL, IXP4XX_GPIO_OUT);
+        gpio_line_config(GPIO_SDA, IXP4XX_GPIO_OUT);
+        gpio_line_config(GPIO_STR, IXP4XX_GPIO_OUT);
+        gpio_line_config(GPIO_HSS0_RTS_N, IXP4XX_GPIO_OUT);
+        gpio_line_config(GPIO_HSS1_RTS_N, IXP4XX_GPIO_OUT);
+        gpio_line_config(GPIO_HSS0_DCD_N, IXP4XX_GPIO_IN);
+        gpio_line_config(GPIO_HSS1_DCD_N, IXP4XX_GPIO_IN);
+        set_irq_type(gpio_irq(GPIO_HSS0_DCD_N), IRQ_TYPE_EDGE_BOTH);
+        set_irq_type(gpio_irq(GPIO_HSS1_DCD_N), IRQ_TYPE_EDGE_BOTH);
+
+        set_control(CONTROL_HSS0_DTR_N, 1);
+        set_control(CONTROL_HSS1_DTR_N, 1);
+        set_control(CONTROL_EEPROM_WC_N, 1);
+        set_control(CONTROL_PCI_RESET_N, 1);
+        output_control();
+
+        msleep(1);            /* Wait for PCI devices to initialize */
+
+        flash_resource.start = IXP4XX_EXP_BUS_BASE(0);
+        flash_resource.end = IXP4XX_EXP_BUS_BASE(0) + ixp4xx_exp_bus_size - 1;
+
+        platform_add_devices(device_tab, devices);
+}
+
+
+#ifdef CONFIG_PCI
+static void __init gmlr_pci_preinit(void)
+{
+        set_irq_type(IRQ_ETHA, IRQ_TYPE_LEVEL_LOW);
+        set_irq_type(IRQ_ETHB, IRQ_TYPE_LEVEL_LOW);
+        set_irq_type(IRQ_NEC, IRQ_TYPE_LEVEL_LOW);
+        set_irq_type(IRQ_MPCI, IRQ_TYPE_LEVEL_LOW);
+        ixp4xx_pci_preinit();
+}
+
+static void __init gmlr_pci_postinit(void)
+{
+        if ((hw_bits & CFG_HW_USB_PORTS) >= 2 &&
+            (hw_bits & CFG_HW_USB_PORTS) < 5) {
+                /* need to adjust number of USB ports on NEC chip */
+                u32 value, addr = BIT(32 - SLOT_NEC) | 0xE0;
+                if (!ixp4xx_pci_read(addr, NP_CMD_CONFIGREAD, &value)) {
+                        value &= ~7;
+                        value |= (hw_bits & CFG_HW_USB_PORTS);
+                        ixp4xx_pci_write(addr, NP_CMD_CONFIGWRITE, value);
+                }
+        }
+}
+
+static int __init gmlr_map_irq(struct pci_dev *dev, u8 slot, u8 pin)
+{
+        switch(slot) {
+        case SLOT_ETHA: return IRQ_ETHA;
+        case SLOT_ETHB: return IRQ_ETHB;
+        case SLOT_NEC:  return IRQ_NEC;
+        default:        return IRQ_MPCI;
+        }
+}
+
+static struct hw_pci gmlr_hw_pci __initdata = {
+        .nr_controllers = 1,
+        .preinit        = gmlr_pci_preinit,
+        .postinit       = gmlr_pci_postinit,
+        .swizzle        = pci_std_swizzle,
+        .setup          = ixp4xx_setup,
+        .scan           = ixp4xx_scan_bus,
+        .map_irq        = gmlr_map_irq,
+};
+
+static int __init gmlr_pci_init(void)
+{
+        if (machine_is_goramo_mlr() &&
+            (hw_bits & (CFG_HW_USB_PORTS | CFG_HW_HAS_PCI_SLOT)))
+                pci_common_init(&gmlr_hw_pci);
+        return 0;
+}
+
+subsys_initcall(gmlr_pci_init);
+#endif /* CONFIG_PCI */
+
+
+MACHINE_START(GORAMO_MLR, "MultiLink")
+        /* Maintainer: Krzysztof Halasa */
+        .phys_io        = IXP4XX_PERIPHERAL_BASE_PHYS,
+        .io_pg_offst    = ((IXP4XX_PERIPHERAL_BASE_VIRT) >> 18) & 0xFFFC,
+        .map_io         = ixp4xx_map_io,
+        .init_irq       = ixp4xx_init_irq,
+        .timer          = &ixp4xx_timer,
+        .boot_params    = 0x0100,
+        .init_machine   = gmlr_init,
+MACHINE_END
diff --git a/arch/arm/mach-ixp4xx/include/mach/cpu.h b/arch/arm/mach-ixp4xx/include/mach/cpu.h
index def7773be67..b2ef65db0e9 100644
--- a/arch/arm/mach-ixp4xx/include/mach/cpu.h
+++ b/arch/arm/mach-ixp4xx/include/mach/cpu.h
@@ -26,6 +26,8 @@
 #define IXP46X_PROCESSOR_ID_VALUE       0x69054200 /* including IXP455 */
 #define IXP46X_PROCESSOR_ID_MASK        0xfffffff0
 
+#define cpu_is_ixp42x_rev_a0() ((read_cpuid_id() & (IXP42X_PROCESSOR_ID_MASK | 0xF)) == \
+                                IXP42X_PROCESSOR_ID_VALUE)
 #define cpu_is_ixp42x() ((read_cpuid_id() & IXP42X_PROCESSOR_ID_MASK) == \
                          IXP42X_PROCESSOR_ID_VALUE)
 #define cpu_is_ixp43x() ((read_cpuid_id() & IXP43X_PROCESSOR_ID_MASK) == \
@@ -35,8 +37,11 @@
 
 static inline u32 ixp4xx_read_feature_bits(void)
 {
-        unsigned int val = ~*IXP4XX_EXP_CFG2;
+        u32 val = ~*IXP4XX_EXP_CFG2;
 
+        if (cpu_is_ixp42x_rev_a0())
+                return IXP42X_FEATURE_MASK & ~(IXP4XX_FEATURE_RCOMP |
+                                               IXP4XX_FEATURE_AES);
         if (cpu_is_ixp42x())
                 return val & IXP42X_FEATURE_MASK;
         if (cpu_is_ixp43x())
diff --git a/arch/arm/mach-ixp4xx/include/mach/qmgr.h b/arch/arm/mach-ixp4xx/include/mach/qmgr.h
index 0cbe6ceb67c..9e7cad2d54c 100644
--- a/arch/arm/mach-ixp4xx/include/mach/qmgr.h
+++ b/arch/arm/mach-ixp4xx/include/mach/qmgr.h
@@ -15,7 +15,7 @@
 #define DEBUG_QMGR      0
 
 #define HALF_QUEUES     32
-#define QUEUES          64      /* only 32 lower queues currently supported */
+#define QUEUES          64
 #define MAX_QUEUE_LENGTH 4      /* in dwords */
 
 #define QUEUE_STAT1_EMPTY               1 /* queue status bits */
@@ -110,48 +110,95 @@ static inline u32 qmgr_get_entry(unsigned int queue)
         return val;
 }
 
-static inline int qmgr_get_stat1(unsigned int queue)
+static inline int __qmgr_get_stat1(unsigned int queue)
 {
         extern struct qmgr_regs __iomem *qmgr_regs;
         return (__raw_readl(&qmgr_regs->stat1[queue >> 3])
                 >> ((queue & 7) << 2)) & 0xF;
 }
 
-static inline int qmgr_get_stat2(unsigned int queue)
+static inline int __qmgr_get_stat2(unsigned int queue)
 {
         extern struct qmgr_regs __iomem *qmgr_regs;
+        BUG_ON(queue >= HALF_QUEUES);
         return (__raw_readl(&qmgr_regs->stat2[queue >> 4])
                 >> ((queue & 0xF) << 1)) & 0x3;
 }
 
+/**
+ * qmgr_stat_empty() - checks if a hardware queue is empty
+ * @queue:      queue number
+ *
+ * Returns non-zero value if the queue is empty.
+ */
 static inline int qmgr_stat_empty(unsigned int queue)
 {
-        return !!(qmgr_get_stat1(queue) & QUEUE_STAT1_EMPTY);
+        BUG_ON(queue >= HALF_QUEUES);
+        return __qmgr_get_stat1(queue) & QUEUE_STAT1_EMPTY;
 }
 
-static inline int qmgr_stat_nearly_empty(unsigned int queue)
+/**
+ * qmgr_stat_below_low_watermark() - checks if a queue is below low watermark
+ * @queue:      queue number
+ *
+ * Returns non-zero value if the queue is below low watermark.
+ */
+static inline int qmgr_stat_below_low_watermark(unsigned int queue)
 {
-        return !!(qmgr_get_stat1(queue) & QUEUE_STAT1_NEARLY_EMPTY);
+        extern struct qmgr_regs __iomem *qmgr_regs;
+        if (queue >= HALF_QUEUES)
+                return (__raw_readl(&qmgr_regs->statne_h) >>
+                        (queue - HALF_QUEUES)) & 0x01;
+        return __qmgr_get_stat1(queue) & QUEUE_STAT1_NEARLY_EMPTY;
 }
 
-static inline int qmgr_stat_nearly_full(unsigned int queue)
+/**
+ * qmgr_stat_above_high_watermark() - checks if a queue is above high watermark
+ * @queue:      queue number
+ *
+ * Returns non-zero value if the queue is above high watermark
+ */
+static inline int qmgr_stat_above_high_watermark(unsigned int queue)
 {
-        return !!(qmgr_get_stat1(queue) & QUEUE_STAT1_NEARLY_FULL);
+        BUG_ON(queue >= HALF_QUEUES);
+        return __qmgr_get_stat1(queue) & QUEUE_STAT1_NEARLY_FULL;
 }
 
+/**
+ * qmgr_stat_full() - checks if a hardware queue is full
+ * @queue:      queue number
+ *
+ * Returns non-zero value if the queue is full.
+ */
 static inline int qmgr_stat_full(unsigned int queue)
 {
-        return !!(qmgr_get_stat1(queue) & QUEUE_STAT1_FULL);
+        extern struct qmgr_regs __iomem *qmgr_regs;
+        if (queue >= HALF_QUEUES)
+                return (__raw_readl(&qmgr_regs->statf_h) >>
+                        (queue - HALF_QUEUES)) & 0x01;
+        return __qmgr_get_stat1(queue) & QUEUE_STAT1_FULL;
 }
 
+/**
+ * qmgr_stat_underflow() - checks if a hardware queue experienced underflow
+ * @queue:      queue number
+ *
+ * Returns non-zero value if the queue experienced underflow.
+ */
 static inline int qmgr_stat_underflow(unsigned int queue)
 {
-        return !!(qmgr_get_stat2(queue) & QUEUE_STAT2_UNDERFLOW);
+        return __qmgr_get_stat2(queue) & QUEUE_STAT2_UNDERFLOW;
 }
 
+/**
+ * qmgr_stat_overflow() - checks if a hardware queue experienced overflow
+ * @queue:      queue number
+ *
+ * Returns non-zero value if the queue experienced overflow.
+ */
 static inline int qmgr_stat_overflow(unsigned int queue)
 {
-        return !!(qmgr_get_stat2(queue) & QUEUE_STAT2_OVERFLOW);
+        return __qmgr_get_stat2(queue) & QUEUE_STAT2_OVERFLOW;
 }
 
 #endif
diff --git a/arch/arm/mach-ixp4xx/ixp4xx_npe.c b/arch/arm/mach-ixp4xx/ixp4xx_npe.c
index 7bb8e778e4b..47ac69c7ec7 100644
--- a/arch/arm/mach-ixp4xx/ixp4xx_npe.c
+++ b/arch/arm/mach-ixp4xx/ixp4xx_npe.c
@@ -386,15 +386,6 @@ static int npe_reset(struct npe *npe)
         /* reset the NPE */
         ixp4xx_write_feature_bits(val &
                                   ~(IXP4XX_FEATURE_RESET_NPEA << npe->id));
-        for (i = 0; i < MAX_RETRIES; i++) {
-                if (!(ixp4xx_read_feature_bits() &
-                      (IXP4XX_FEATURE_RESET_NPEA << npe->id)))
-                        break;  /* reset completed */
-                udelay(1);
-        }
-        if (i == MAX_RETRIES)
-                return -ETIMEDOUT;
-
         /* deassert reset */
         ixp4xx_write_feature_bits(val |
                                   (IXP4XX_FEATURE_RESET_NPEA << npe->id));
diff --git a/arch/arm/mach-ixp4xx/ixp4xx_qmgr.c b/arch/arm/mach-ixp4xx/ixp4xx_qmgr.c
index bfddc73d0a2..bfdbe4b5a3c 100644
--- a/arch/arm/mach-ixp4xx/ixp4xx_qmgr.c
+++ b/arch/arm/mach-ixp4xx/ixp4xx_qmgr.c
@@ -18,8 +18,8 @@ struct qmgr_regs __iomem *qmgr_regs;
 static struct resource *mem_res;
 static spinlock_t qmgr_lock;
 static u32 used_sram_bitmap[4]; /* 128 16-dword pages */
-static void (*irq_handlers[HALF_QUEUES])(void *pdev);
-static void *irq_pdevs[HALF_QUEUES];
+static void (*irq_handlers[QUEUES])(void *pdev);
+static void *irq_pdevs[QUEUES];
 
 #if DEBUG_QMGR
 char qmgr_queue_descs[QUEUES][32];
@@ -28,51 +28,112 @@ char qmgr_queue_descs[QUEUES][32];
 void qmgr_set_irq(unsigned int queue, int src,
                   void (*handler)(void *pdev), void *pdev)
 {
-        u32 __iomem *reg = &qmgr_regs->irqsrc[queue / 8]; /* 8 queues / u32 */
-        int bit = (queue % 8) * 4; /* 3 bits + 1 reserved bit per queue */
         unsigned long flags;
 
-        src &= 7;
         spin_lock_irqsave(&qmgr_lock, flags);
-        __raw_writel((__raw_readl(reg) & ~(7 << bit)) | (src << bit), reg);
+        if (queue < HALF_QUEUES) {
+                u32 __iomem *reg;
+                int bit;
+                BUG_ON(src > QUEUE_IRQ_SRC_NOT_FULL);
+                reg = &qmgr_regs->irqsrc[queue >> 3]; /* 8 queues per u32 */
+                bit = (queue % 8) * 4; /* 3 bits + 1 reserved bit per queue */
+                __raw_writel((__raw_readl(reg) & ~(7 << bit)) | (src << bit),
+                             reg);
+        } else
+                /* IRQ source for queues 32-63 is fixed */
+                BUG_ON(src != QUEUE_IRQ_SRC_NOT_NEARLY_EMPTY);
+
         irq_handlers[queue] = handler;
         irq_pdevs[queue] = pdev;
         spin_unlock_irqrestore(&qmgr_lock, flags);
 }
 
 
-static irqreturn_t qmgr_irq1(int irq, void *pdev)
+static irqreturn_t qmgr_irq1_a0(int irq, void *pdev)
 {
-        int i;
-        u32 val = __raw_readl(&qmgr_regs->irqstat[0]);
-        __raw_writel(val, &qmgr_regs->irqstat[0]); /* ACK */
-
-        for (i = 0; i < HALF_QUEUES; i++)
-                if (val & (1 << i))
+        int i, ret = 0;
+        u32 en_bitmap, src, stat;
+
+        /* ACK - it may clear any bits so don't rely on it */
+        __raw_writel(0xFFFFFFFF, &qmgr_regs->irqstat[0]);
+
+        en_bitmap = qmgr_regs->irqen[0];
+        while (en_bitmap) {
+                i = __fls(en_bitmap); /* number of the last "low" queue */
+                en_bitmap &= ~BIT(i);
+                src = qmgr_regs->irqsrc[i >> 3];
+                stat = qmgr_regs->stat1[i >> 3];
+                if (src & 4) /* the IRQ condition is inverted */
+                        stat = ~stat;
+                if (stat & BIT(src & 3)) {
                         irq_handlers[i](irq_pdevs[i]);
+                        ret = IRQ_HANDLED;
+                }
+        }
+        return ret;
+}
+
+
+static irqreturn_t qmgr_irq2_a0(int irq, void *pdev)
+{
+        int i, ret = 0;
+        u32 req_bitmap;
+
+        /* ACK - it may clear any bits so don't rely on it */
+        __raw_writel(0xFFFFFFFF, &qmgr_regs->irqstat[1]);
+
+        req_bitmap = qmgr_regs->irqen[1] & qmgr_regs->statne_h;
+        while (req_bitmap) {
+                i = __fls(req_bitmap); /* number of the last "high" queue */
+                req_bitmap &= ~BIT(i);
+                irq_handlers[HALF_QUEUES + i](irq_pdevs[HALF_QUEUES + i]);
+                ret = IRQ_HANDLED;
+        }
+        return ret;
+}
 
-        return val ? IRQ_HANDLED : 0;
+
+static irqreturn_t qmgr_irq(int irq, void *pdev)
+{
+        int i, half = (irq == IRQ_IXP4XX_QM1 ? 0 : 1);
+        u32 req_bitmap = __raw_readl(&qmgr_regs->irqstat[half]);
+
+        if (!req_bitmap)
+                return 0;
+        __raw_writel(req_bitmap, &qmgr_regs->irqstat[half]); /* ACK */
+
+        while (req_bitmap) {
+                i = __fls(req_bitmap); /* number of the last queue */
+                req_bitmap &= ~BIT(i);
+                i += half * HALF_QUEUES;
+                irq_handlers[i](irq_pdevs[i]);
+        }
+        return IRQ_HANDLED;
 }
 
 
 void qmgr_enable_irq(unsigned int queue)
 {
         unsigned long flags;
+        int half = queue / 32;
+        u32 mask = 1 << (queue & (HALF_QUEUES - 1));
 
         spin_lock_irqsave(&qmgr_lock, flags);
-        __raw_writel(__raw_readl(&qmgr_regs->irqen[0]) | (1 << queue),
-                     &qmgr_regs->irqen[0]);
+        __raw_writel(__raw_readl(&qmgr_regs->irqen[half]) | mask,
+                     &qmgr_regs->irqen[half]);
         spin_unlock_irqrestore(&qmgr_lock, flags);
 }
 
 void qmgr_disable_irq(unsigned int queue)
 {
         unsigned long flags;
+        int half = queue / 32;
+        u32 mask = 1 << (queue & (HALF_QUEUES - 1));
 
         spin_lock_irqsave(&qmgr_lock, flags);
-        __raw_writel(__raw_readl(&qmgr_regs->irqen[0]) & ~(1 << queue),
-                     &qmgr_regs->irqen[0]);
-        __raw_writel(1 << queue, &qmgr_regs->irqstat[0]); /* clear */
+        __raw_writel(__raw_readl(&qmgr_regs->irqen[half]) & ~mask,
+                     &qmgr_regs->irqen[half]);
+        __raw_writel(mask, &qmgr_regs->irqstat[half]); /* clear */
         spin_unlock_irqrestore(&qmgr_lock, flags);
 }
 
@@ -98,8 +159,7 @@ int __qmgr_request_queue(unsigned int queue, unsigned int len /* dwords */,
         u32 cfg, addr = 0, mask[4]; /* in 16-dwords */
         int err;
 
-        if (queue >= HALF_QUEUES)
-                return -ERANGE;
+        BUG_ON(queue >= QUEUES);
 
         if ((nearly_empty_watermark | nearly_full_watermark) & ~7)
                 return -EINVAL;
@@ -180,7 +240,7 @@ void qmgr_release_queue(unsigned int queue)
 {
         u32 cfg, addr, mask[4];
 
-        BUG_ON(queue >= HALF_QUEUES); /* not in valid range */
+        BUG_ON(queue >= QUEUES); /* not in valid range */
 
         spin_lock_irq(&qmgr_lock);
         cfg = __raw_readl(&qmgr_regs->sram[queue]);
@@ -224,6 +284,8 @@ void qmgr_release_queue(unsigned int queue)
 static int qmgr_init(void)
 {
         int i, err;
+        irq_handler_t handler1, handler2;
+
         mem_res = request_mem_region(IXP4XX_QMGR_BASE_PHYS,
                                      IXP4XX_QMGR_REGION_SIZE,
                                      "IXP4xx Queue Manager");
@@ -247,23 +309,42 @@ static int qmgr_init(void)
                 __raw_writel(0, &qmgr_regs->irqen[i]);
         }
 
+        __raw_writel(0xFFFFFFFF, &qmgr_regs->statne_h);
+        __raw_writel(0, &qmgr_regs->statf_h);
+
         for (i = 0; i < QUEUES; i++)
                 __raw_writel(0, &qmgr_regs->sram[i]);
 
-        err = request_irq(IRQ_IXP4XX_QM1, qmgr_irq1, 0,
-                          "IXP4xx Queue Manager", NULL);
+        if (cpu_is_ixp42x_rev_a0()) {
+                handler1 = qmgr_irq1_a0;
+                handler2 = qmgr_irq2_a0;
+        } else
+                handler1 = handler2 = qmgr_irq;
+
+        err = request_irq(IRQ_IXP4XX_QM1, handler1, 0, "IXP4xx Queue Manager",
+                          NULL);
         if (err) {
-                printk(KERN_ERR "qmgr: failed to request IRQ%i\n",
-                       IRQ_IXP4XX_QM1);
+                printk(KERN_ERR "qmgr: failed to request IRQ%i (%i)\n",
+                       IRQ_IXP4XX_QM1, err);
                 goto error_irq;
         }
 
+        err = request_irq(IRQ_IXP4XX_QM2, handler2, 0, "IXP4xx Queue Manager",
+                          NULL);
+        if (err) {
+                printk(KERN_ERR "qmgr: failed to request IRQ%i (%i)\n",
+                       IRQ_IXP4XX_QM2, err);
+                goto error_irq2;
+        }
+
         used_sram_bitmap[0] = 0xF; /* 4 first pages reserved for config */
         spin_lock_init(&qmgr_lock);
 
         printk(KERN_INFO "IXP4xx Queue Manager initialized.\n");
         return 0;
 
+error_irq2:
+        free_irq(IRQ_IXP4XX_QM1, NULL);
 error_irq:
         iounmap(qmgr_regs);
 error_map:
@@ -274,7 +355,9 @@ error_map:
 static void qmgr_remove(void)
 {
         free_irq(IRQ_IXP4XX_QM1, NULL);
+        free_irq(IRQ_IXP4XX_QM2, NULL);
         synchronize_irq(IRQ_IXP4XX_QM1);
+        synchronize_irq(IRQ_IXP4XX_QM2);
         iounmap(qmgr_regs);
         release_mem_region(IXP4XX_QMGR_BASE_PHYS, IXP4XX_QMGR_REGION_SIZE);
 }