Merge v2.6.37-rc8 into powerpc/next

16 files changed, 963 insertions, 303 deletions

diff --git a/arch/powerpc/sysdev/Makefile b/arch/powerpc/sysdev/Makefile
index 5642924fb9f..0bef9dacb64 100644
--- a/arch/powerpc/sysdev/Makefile
+++ b/arch/powerpc/sysdev/Makefile
@@ -1,8 +1,6 @@
 subdir-ccflags-$(CONFIG_PPC_WERROR) := -Werror
 
-ifeq ($(CONFIG_PPC64),y)
-EXTRA_CFLAGS			+= -mno-minimal-toc
-endif
+ccflags-$(CONFIG_PPC64)		:= -mno-minimal-toc
 
 mpic-msi-obj-$(CONFIG_PCI_MSI)	+= mpic_msi.o mpic_u3msi.o mpic_pasemi_msi.o
 obj-$(CONFIG_MPIC)		+= mpic.o $(mpic-msi-obj-y)
@@ -20,6 +18,7 @@ obj-$(CONFIG_FSL_PMC)		+= fsl_pmc.o
 obj-$(CONFIG_FSL_LBC)		+= fsl_lbc.o
 obj-$(CONFIG_FSL_GTM)		+= fsl_gtm.o
 obj-$(CONFIG_MPC8xxx_GPIO)	+= mpc8xxx_gpio.o
+obj-$(CONFIG_FSL_85XX_CACHE_SRAM)	+= fsl_85xx_l2ctlr.o fsl_85xx_cache_sram.o
 obj-$(CONFIG_SIMPLE_GPIO)	+= simple_gpio.o
 obj-$(CONFIG_RAPIDIO)		+= fsl_rio.o
 obj-$(CONFIG_TSI108_BRIDGE)	+= tsi108_pci.o tsi108_dev.o
diff --git a/arch/powerpc/sysdev/dart_iommu.c b/arch/powerpc/sysdev/dart_iommu.c
index 559db2b846a..17cf15ec38b 100644
--- a/arch/powerpc/sysdev/dart_iommu.c
+++ b/arch/powerpc/sysdev/dart_iommu.c
@@ -70,6 +70,8 @@ static int iommu_table_dart_inited;
 static int dart_dirty;
 static int dart_is_u4;
 
+#define DART_U4_BYPASS_BASE	0x8000000000ull
+
 #define DBG(...)
 
 static inline void dart_tlb_invalidate_all(void)
@@ -292,12 +294,20 @@ static void iommu_table_dart_setup(void)
 	set_bit(iommu_table_dart.it_size - 1, iommu_table_dart.it_map);
 }
 
-static void pci_dma_dev_setup_dart(struct pci_dev *dev)
+static void dma_dev_setup_dart(struct device *dev)
 {
 	/* We only have one iommu table on the mac for now, which makes
 	 * things simple. Setup all PCI devices to point to this table
 	 */
-	set_iommu_table_base(&dev->dev, &iommu_table_dart);
+	if (get_dma_ops(dev) == &dma_direct_ops)
+		set_dma_offset(dev, DART_U4_BYPASS_BASE);
+	else
+		set_iommu_table_base(dev, &iommu_table_dart);
+}
+
+static void pci_dma_dev_setup_dart(struct pci_dev *dev)
+{
+	dma_dev_setup_dart(&dev->dev);
 }
 
 static void pci_dma_bus_setup_dart(struct pci_bus *bus)
@@ -315,6 +325,45 @@ static void pci_dma_bus_setup_dart(struct pci_bus *bus)
 		PCI_DN(dn)->iommu_table = &iommu_table_dart;
 }
 
+static bool dart_device_on_pcie(struct device *dev)
+{
+	struct device_node *np = of_node_get(dev->of_node);
+
+	while(np) {
+		if (of_device_is_compatible(np, "U4-pcie") ||
+		    of_device_is_compatible(np, "u4-pcie")) {
+			of_node_put(np);
+			return true;
+		}
+		np = of_get_next_parent(np);
+	}
+	return false;
+}
+
+static int dart_dma_set_mask(struct device *dev, u64 dma_mask)
+{
+	if (!dev->dma_mask || !dma_supported(dev, dma_mask))
+		return -EIO;
+
+	/* U4 supports a DART bypass, we use it for 64-bit capable
+	 * devices to improve performances. However, that only works
+	 * for devices connected to U4 own PCIe interface, not bridged
+	 * through hypertransport. We need the device to support at
+	 * least 40 bits of addresses.
+	 */
+	if (dart_device_on_pcie(dev) && dma_mask >= DMA_BIT_MASK(40)) {
+		dev_info(dev, "Using 64-bit DMA iommu bypass\n");
+		set_dma_ops(dev, &dma_direct_ops);
+	} else {
+		dev_info(dev, "Using 32-bit DMA via iommu\n");
+		set_dma_ops(dev, &dma_iommu_ops);
+	}
+	dma_dev_setup_dart(dev);
+
+	*dev->dma_mask = dma_mask;
+	return 0;
+}
+
 void __init iommu_init_early_dart(void)
 {
 	struct device_node *dn;
@@ -328,20 +377,25 @@ void __init iommu_init_early_dart(void)
 		dart_is_u4 = 1;
 	}
 
+	/* Initialize the DART HW */
+	if (dart_init(dn) != 0)
+		goto bail;
+
 	/* Setup low level TCE operations for the core IOMMU code */
 	ppc_md.tce_build = dart_build;
 	ppc_md.tce_free  = dart_free;
 	ppc_md.tce_flush = dart_flush;
 
-	/* Initialize the DART HW */
-	if (dart_init(dn) == 0) {
-		ppc_md.pci_dma_dev_setup = pci_dma_dev_setup_dart;
-		ppc_md.pci_dma_bus_setup = pci_dma_bus_setup_dart;
+	/* Setup bypass if supported */
+	if (dart_is_u4)
+		ppc_md.dma_set_mask = dart_dma_set_mask;
 
-		/* Setup pci_dma ops */
-		set_pci_dma_ops(&dma_iommu_ops);
-		return;
-	}
+	ppc_md.pci_dma_dev_setup = pci_dma_dev_setup_dart;
+	ppc_md.pci_dma_bus_setup = pci_dma_bus_setup_dart;
+
+	/* Setup pci_dma ops */
+	set_pci_dma_ops(&dma_iommu_ops);
+	return;
 
  bail:
 	/* If init failed, use direct iommu and null setup functions */
diff --git a/arch/powerpc/sysdev/fsl_85xx_cache_ctlr.h b/arch/powerpc/sysdev/fsl_85xx_cache_ctlr.h
new file mode 100644
index 00000000000..60c9c0bd5ba
--- /dev/null
+++ b/arch/powerpc/sysdev/fsl_85xx_cache_ctlr.h
@@ -0,0 +1,101 @@
+/*
+ * Copyright 2009-2010 Freescale Semiconductor, Inc
+ *
+ * QorIQ based Cache Controller Memory Mapped Registers
+ *
+ * Author: Vivek Mahajan <vivek.mahajan@freescale.com>
+ *
+ * This program is free software; you can redistribute  it and/or modify it
+ * under  the terms of  the GNU General  Public License as published by the
+ * Free Software Foundation;  either version 2 of the  License, or (at your
+ * option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef __FSL_85XX_CACHE_CTLR_H__
+#define __FSL_85XX_CACHE_CTLR_H__
+
+#define L2CR_L2FI		0x40000000	/* L2 flash invalidate */
+#define L2CR_L2IO		0x00200000	/* L2 instruction only */
+#define L2CR_SRAM_ZERO		0x00000000	/* L2SRAM zero size */
+#define L2CR_SRAM_FULL		0x00010000	/* L2SRAM full size */
+#define L2CR_SRAM_HALF		0x00020000	/* L2SRAM half size */
+#define L2CR_SRAM_TWO_HALFS	0x00030000	/* L2SRAM two half sizes */
+#define L2CR_SRAM_QUART		0x00040000	/* L2SRAM one quarter size */
+#define L2CR_SRAM_TWO_QUARTS	0x00050000	/* L2SRAM two quarter size */
+#define L2CR_SRAM_EIGHTH	0x00060000	/* L2SRAM one eighth size */
+#define L2CR_SRAM_TWO_EIGHTH	0x00070000	/* L2SRAM two eighth size */
+
+#define L2SRAM_OPTIMAL_SZ_SHIFT	0x00000003	/* Optimum size for L2SRAM */
+
+#define L2SRAM_BAR_MSK_LO18	0xFFFFC000	/* Lower 18 bits */
+#define L2SRAM_BARE_MSK_HI4	0x0000000F	/* Upper 4 bits */
+
+enum cache_sram_lock_ways {
+	LOCK_WAYS_ZERO,
+	LOCK_WAYS_EIGHTH,
+	LOCK_WAYS_TWO_EIGHTH,
+	LOCK_WAYS_HALF = 4,
+	LOCK_WAYS_FULL = 8,
+};
+
+struct mpc85xx_l2ctlr {
+	u32	ctl;		/* 0x000 - L2 control */
+	u8	res1[0xC];
+	u32	ewar0;		/* 0x010 - External write address 0 */
+	u32	ewarea0;	/* 0x014 - External write address extended 0 */
+	u32	ewcr0;		/* 0x018 - External write ctrl */
+	u8	res2[4];
+	u32	ewar1;		/* 0x020 - External write address 1 */
+	u32	ewarea1;	/* 0x024 - External write address extended 1 */
+	u32	ewcr1;		/* 0x028 - External write ctrl 1 */
+	u8	res3[4];
+	u32	ewar2;		/* 0x030 - External write address 2 */
+	u32	ewarea2;	/* 0x034 - External write address extended 2 */
+	u32	ewcr2;		/* 0x038 - External write ctrl 2 */
+	u8	res4[4];
+	u32	ewar3;		/* 0x040 - External write address 3 */
+	u32	ewarea3;	/* 0x044 - External write address extended 3 */
+	u32	ewcr3;		/* 0x048 - External write ctrl 3 */
+	u8	res5[0xB4];
+	u32	srbar0;		/* 0x100 - SRAM base address 0 */
+	u32	srbarea0;	/* 0x104 - SRAM base addr reg ext address 0 */
+	u32	srbar1;		/* 0x108 - SRAM base address 1 */
+	u32	srbarea1;	/* 0x10C - SRAM base addr reg ext address 1 */
+	u8	res6[0xCF0];
+	u32	errinjhi;	/* 0xE00 - Error injection mask high */
+	u32	errinjlo;	/* 0xE04 - Error injection mask low */
+	u32	errinjctl;	/* 0xE08 - Error injection tag/ecc control */
+	u8	res7[0x14];
+	u32	captdatahi;	/* 0xE20 - Error data high capture */
+	u32	captdatalo;	/* 0xE24 - Error data low capture */
+	u32	captecc;	/* 0xE28 - Error syndrome */
+	u8	res8[0x14];
+	u32	errdet;		/* 0xE40 - Error detect */
+	u32	errdis;		/* 0xE44 - Error disable */
+	u32	errinten;	/* 0xE48 - Error interrupt enable */
+	u32	errattr;	/* 0xE4c - Error attribute capture */
+	u32	erradrrl;	/* 0xE50 - Error address capture low */
+	u32	erradrrh;	/* 0xE54 - Error address capture high */
+	u32	errctl;		/* 0xE58 - Error control */
+	u8	res9[0x1A4];
+};
+
+struct sram_parameters {
+	unsigned int sram_size;
+	uint64_t sram_offset;
+};
+
+extern int instantiate_cache_sram(struct platform_device *dev,
+		struct sram_parameters sram_params);
+extern void remove_cache_sram(struct platform_device *dev);
+
+#endif /* __FSL_85XX_CACHE_CTLR_H__ */
diff --git a/arch/powerpc/sysdev/fsl_85xx_cache_sram.c b/arch/powerpc/sysdev/fsl_85xx_cache_sram.c
new file mode 100644
index 00000000000..54fb1922fe3
--- /dev/null
+++ b/arch/powerpc/sysdev/fsl_85xx_cache_sram.c
@@ -0,0 +1,159 @@
+/*
+ * Copyright 2009-2010 Freescale Semiconductor, Inc.
+ *
+ * Simple memory allocator abstraction for QorIQ (P1/P2) based Cache-SRAM
+ *
+ * Author: Vivek Mahajan <vivek.mahajan@freescale.com>
+ *
+ * This file is derived from the original work done
+ * by Sylvain Munaut for the Bestcomm SRAM allocator.
+ *
+ * This program is free software; you can redistribute  it and/or modify it
+ * under  the terms of  the GNU General  Public License as published by the
+ * Free Software Foundation;  either version 2 of the  License, or (at your
+ * option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#include <linux/kernel.h>
+#include <linux/slab.h>
+#include <linux/err.h>
+#include <linux/of_platform.h>
+#include <asm/pgtable.h>
+#include <asm/fsl_85xx_cache_sram.h>
+
+#include "fsl_85xx_cache_ctlr.h"
+
+struct mpc85xx_cache_sram *cache_sram;
+
+void *mpc85xx_cache_sram_alloc(unsigned int size,
+				phys_addr_t *phys, unsigned int align)
+{
+	unsigned long offset;
+	unsigned long flags;
+
+	if (unlikely(cache_sram == NULL))
+		return NULL;
+
+	if (!size || (size > cache_sram->size) || (align > cache_sram->size)) {
+		pr_err("%s(): size(=%x) or align(=%x) zero or too big\n",
+			__func__, size, align);
+		return NULL;
+	}
+
+	if ((align & (align - 1)) || align <= 1) {
+		pr_err("%s(): align(=%x) must be power of two and >1\n",
+			__func__, align);
+		return NULL;
+	}
+
+	spin_lock_irqsave(&cache_sram->lock, flags);
+	offset = rh_alloc_align(cache_sram->rh, size, align, NULL);
+	spin_unlock_irqrestore(&cache_sram->lock, flags);
+
+	if (IS_ERR_VALUE(offset))
+		return NULL;
+
+	*phys = cache_sram->base_phys + offset;
+
+	return (unsigned char *)cache_sram->base_virt + offset;
+}
+EXPORT_SYMBOL(mpc85xx_cache_sram_alloc);
+
+void mpc85xx_cache_sram_free(void *ptr)
+{
+	unsigned long flags;
+	BUG_ON(!ptr);
+
+	spin_lock_irqsave(&cache_sram->lock, flags);
+	rh_free(cache_sram->rh, ptr - cache_sram->base_virt);
+	spin_unlock_irqrestore(&cache_sram->lock, flags);
+}
+EXPORT_SYMBOL(mpc85xx_cache_sram_free);
+
+int __init instantiate_cache_sram(struct platform_device *dev,
+		struct sram_parameters sram_params)
+{
+	int ret = 0;
+
+	if (cache_sram) {
+		dev_err(&dev->dev, "Already initialized cache-sram\n");
+		return -EBUSY;
+	}
+
+	cache_sram = kzalloc(sizeof(struct mpc85xx_cache_sram), GFP_KERNEL);
+	if (!cache_sram) {
+		dev_err(&dev->dev, "Out of memory for cache_sram structure\n");
+		return -ENOMEM;
+	}
+
+	cache_sram->base_phys = sram_params.sram_offset;
+	cache_sram->size = sram_params.sram_size;
+
+	if (!request_mem_region(cache_sram->base_phys, cache_sram->size,
+						"fsl_85xx_cache_sram")) {
+		dev_err(&dev->dev, "%s: request memory failed\n",
+				dev->dev.of_node->full_name);
+		ret = -ENXIO;
+		goto out_free;
+	}
+
+	cache_sram->base_virt = ioremap_flags(cache_sram->base_phys,
+				cache_sram->size, _PAGE_COHERENT | PAGE_KERNEL);
+	if (!cache_sram->base_virt) {
+		dev_err(&dev->dev, "%s: ioremap_flags failed\n",
+				dev->dev.of_node->full_name);
+		ret = -ENOMEM;
+		goto out_release;
+	}
+
+	cache_sram->rh = rh_create(sizeof(unsigned int));
+	if (IS_ERR(cache_sram->rh)) {
+		dev_err(&dev->dev, "%s: Unable to create remote heap\n",
+				dev->dev.of_node->full_name);
+		ret = PTR_ERR(cache_sram->rh);
+		goto out_unmap;
+	}
+
+	rh_attach_region(cache_sram->rh, 0, cache_sram->size);
+	spin_lock_init(&cache_sram->lock);
+
+	dev_info(&dev->dev, "[base:0x%llx, size:0x%x] configured and loaded\n",
+		(unsigned long long)cache_sram->base_phys, cache_sram->size);
+
+	return 0;
+
+out_unmap:
+	iounmap(cache_sram->base_virt);
+
+out_release:
+	release_mem_region(cache_sram->base_phys, cache_sram->size);
+
+out_free:
+	kfree(cache_sram);
+	return ret;
+}
+
+void remove_cache_sram(struct platform_device *dev)
+{
+	BUG_ON(!cache_sram);
+
+	rh_detach_region(cache_sram->rh, 0, cache_sram->size);
+	rh_destroy(cache_sram->rh);
+
+	iounmap(cache_sram->base_virt);
+	release_mem_region(cache_sram->base_phys, cache_sram->size);
+
+	kfree(cache_sram);
+	cache_sram = NULL;
+
+	dev_info(&dev->dev, "MPC85xx Cache-SRAM driver unloaded\n");
+}
diff --git a/arch/powerpc/sysdev/fsl_85xx_l2ctlr.c b/arch/powerpc/sysdev/fsl_85xx_l2ctlr.c
new file mode 100644
index 00000000000..cc8d6556d79
--- /dev/null
+++ b/arch/powerpc/sysdev/fsl_85xx_l2ctlr.c
@@ -0,0 +1,231 @@
+/*
+ * Copyright 2009-2010 Freescale Semiconductor, Inc.
+ *
+ * QorIQ (P1/P2) L2 controller init for Cache-SRAM instantiation
+ *
+ * Author: Vivek Mahajan <vivek.mahajan@freescale.com>
+ *
+ * This program is free software; you can redistribute  it and/or modify it
+ * under  the terms of  the GNU General  Public License as published by the
+ * Free Software Foundation;  either version 2 of the  License, or (at your
+ * option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#include <linux/kernel.h>
+#include <linux/of_platform.h>
+#include <asm/io.h>
+
+#include "fsl_85xx_cache_ctlr.h"
+
+static char *sram_size;
+static char *sram_offset;
+struct mpc85xx_l2ctlr __iomem *l2ctlr;
+
+static long get_cache_sram_size(void)
+{
+	unsigned long val;
+
+	if (!sram_size || (strict_strtoul(sram_size, 0, &val) < 0))
+		return -EINVAL;
+
+	return val;
+}
+
+static long get_cache_sram_offset(void)
+{
+	unsigned long val;
+
+	if (!sram_offset || (strict_strtoul(sram_offset, 0, &val) < 0))
+		return -EINVAL;
+
+	return val;
+}
+
+static int __init get_size_from_cmdline(char *str)
+{
+	if (!str)
+		return 0;
+
+	sram_size = str;
+	return 1;
+}
+
+static int __init get_offset_from_cmdline(char *str)
+{
+	if (!str)
+		return 0;
+
+	sram_offset = str;
+	return 1;
+}
+
+__setup("cache-sram-size=", get_size_from_cmdline);
+__setup("cache-sram-offset=", get_offset_from_cmdline);
+
+static int __devinit mpc85xx_l2ctlr_of_probe(struct platform_device *dev,
+					  const struct of_device_id *match)
+{
+	long rval;
+	unsigned int rem;
+	unsigned char ways;
+	const unsigned int *prop;
+	unsigned int l2cache_size;
+	struct sram_parameters sram_params;
+
+	if (!dev->dev.of_node) {
+		dev_err(&dev->dev, "Device's OF-node is NULL\n");
+		return -EINVAL;
+	}
+
+	prop = of_get_property(dev->dev.of_node, "cache-size", NULL);
+	if (!prop) {
+		dev_err(&dev->dev, "Missing L2 cache-size\n");
+		return -EINVAL;
+	}
+	l2cache_size = *prop;
+
+	sram_params.sram_size  = get_cache_sram_size();
+	if (sram_params.sram_size <= 0) {
+		dev_err(&dev->dev,
+			"Entire L2 as cache, Aborting Cache-SRAM stuff\n");
+		return -EINVAL;
+	}
+
+	sram_params.sram_offset  = get_cache_sram_offset();
+	if (sram_params.sram_offset <= 0) {
+		dev_err(&dev->dev,
+			"Entire L2 as cache, provide a valid sram offset\n");
+		return -EINVAL;
+	}
+
+
+	rem = l2cache_size % sram_params.sram_size;
+	ways = LOCK_WAYS_FULL * sram_params.sram_size / l2cache_size;
+	if (rem || (ways & (ways - 1))) {
+		dev_err(&dev->dev, "Illegal cache-sram-size in command line\n");
+		return -EINVAL;
+	}
+
+	l2ctlr = of_iomap(dev->dev.of_node, 0);
+	if (!l2ctlr) {
+		dev_err(&dev->dev, "Can't map L2 controller\n");
+		return -EINVAL;
+	}
+
+	/*
+	 * Write bits[0-17] to srbar0
+	 */
+	out_be32(&l2ctlr->srbar0,
+		sram_params.sram_offset & L2SRAM_BAR_MSK_LO18);
+
+	/*
+	 * Write bits[18-21] to srbare0
+	 */
+#ifdef CONFIG_PHYS_64BIT
+	out_be32(&l2ctlr->srbarea0,
+		(sram_params.sram_offset >> 32) & L2SRAM_BARE_MSK_HI4);
+#endif
+
+	clrsetbits_be32(&l2ctlr->ctl, L2CR_L2E, L2CR_L2FI);
+
+	switch (ways) {
+	case LOCK_WAYS_EIGHTH:
+		setbits32(&l2ctlr->ctl,
+			L2CR_L2E | L2CR_L2FI | L2CR_SRAM_EIGHTH);
+		break;
+
+	case LOCK_WAYS_TWO_EIGHTH:
+		setbits32(&l2ctlr->ctl,
+			L2CR_L2E | L2CR_L2FI | L2CR_SRAM_QUART);
+		break;
+
+	case LOCK_WAYS_HALF:
+		setbits32(&l2ctlr->ctl,
+			L2CR_L2E | L2CR_L2FI | L2CR_SRAM_HALF);
+		break;
+
+	case LOCK_WAYS_FULL:
+	default:
+		setbits32(&l2ctlr->ctl,
+			L2CR_L2E | L2CR_L2FI | L2CR_SRAM_FULL);
+		break;
+	}
+	eieio();
+
+	rval = instantiate_cache_sram(dev, sram_params);
+	if (rval < 0) {
+		dev_err(&dev->dev, "Can't instantiate Cache-SRAM\n");
+		iounmap(l2ctlr);
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int __devexit mpc85xx_l2ctlr_of_remove(struct platform_device *dev)
+{
+	BUG_ON(!l2ctlr);
+
+	iounmap(l2ctlr);
+	remove_cache_sram(dev);
+	dev_info(&dev->dev, "MPC85xx L2 controller unloaded\n");
+
+	return 0;
+}
+
+static struct of_device_id mpc85xx_l2ctlr_of_match[] = {
+	{
+		.compatible = "fsl,p2020-l2-cache-controller",
+	},
+	{
+		.compatible = "fsl,p2010-l2-cache-controller",
+	},
+	{
+		.compatible = "fsl,p1020-l2-cache-controller",
+	},
+	{
+		.compatible = "fsl,p1011-l2-cache-controller",
+	},
+	{
+		.compatible = "fsl,p1013-l2-cache-controller",
+	},
+	{
+		.compatible = "fsl,p1022-l2-cache-controller",
+	},
+	{},
+};
+
+static struct of_platform_driver mpc85xx_l2ctlr_of_platform_driver = {
+	.driver	= {
+		.name		= "fsl-l2ctlr",
+		.owner		= THIS_MODULE,
+		.of_match_table	= mpc85xx_l2ctlr_of_match,
+	},
+	.probe		= mpc85xx_l2ctlr_of_probe,
+	.remove		= __devexit_p(mpc85xx_l2ctlr_of_remove),
+};
+
+static __init int mpc85xx_l2ctlr_of_init(void)
+{
+	return of_register_platform_driver(&mpc85xx_l2ctlr_of_platform_driver);
+}
+
+static void __exit mpc85xx_l2ctlr_of_exit(void)
+{
+	of_unregister_platform_driver(&mpc85xx_l2ctlr_of_platform_driver);
+}
+
+subsys_initcall(mpc85xx_l2ctlr_of_init);
+module_exit(mpc85xx_l2ctlr_of_exit);
+
+MODULE_DESCRIPTION("Freescale MPC85xx L2 controller init");
+MODULE_LICENSE("GPL v2");
diff --git a/arch/powerpc/sysdev/fsl_lbc.c b/arch/powerpc/sysdev/fsl_lbc.c
index dceb8d1a843..4fcb5a4e60d 100644
--- a/arch/powerpc/sysdev/fsl_lbc.c
+++ b/arch/powerpc/sysdev/fsl_lbc.c
@@ -1,9 +1,12 @@
 /*
  * Freescale LBC and UPM routines.
  *
- * Copyright (c) 2007-2008  MontaVista Software, Inc.
+ * Copyright © 2007-2008  MontaVista Software, Inc.
+ * Copyright © 2010 Freescale Semiconductor
  *
  * Author: Anton Vorontsov <avorontsov@ru.mvista.com>
+ * Author: Jack Lan <Jack.Lan@freescale.com>
+ * Author: Roy Zang <tie-fei.zang@freescale.com>
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License as published by
@@ -19,39 +22,37 @@
 #include <linux/types.h>
 #include <linux/io.h>
 #include <linux/of.h>
+#include <linux/slab.h>
+#include <linux/platform_device.h>
+#include <linux/interrupt.h>
+#include <linux/mod_devicetable.h>
 #include <asm/prom.h>
 #include <asm/fsl_lbc.h>
 
 static spinlock_t fsl_lbc_lock = __SPIN_LOCK_UNLOCKED(fsl_lbc_lock);
-static struct fsl_lbc_regs __iomem *fsl_lbc_regs;
+struct fsl_lbc_ctrl *fsl_lbc_ctrl_dev;
+EXPORT_SYMBOL(fsl_lbc_ctrl_dev);
 
-static char __initdata *compat_lbc[] = {
-	"fsl,pq2-localbus",
-	"fsl,pq2pro-localbus",
-	"fsl,pq3-localbus",
-	"fsl,elbc",
-};
-
-static int __init fsl_lbc_init(void)
+/**
+ * fsl_lbc_addr - convert the base address
+ * @addr_base:	base address of the memory bank
+ *
+ * This function converts a base address of lbc into the right format for the
+ * BR register. If the SOC has eLBC then it returns 32bit physical address
+ * else it convers a 34bit local bus physical address to correct format of
+ * 32bit address for BR register (Example: MPC8641).
+ */
+u32 fsl_lbc_addr(phys_addr_t addr_base)
 {
-	struct device_node *lbus;
-	int i;
+	struct device_node *np = fsl_lbc_ctrl_dev->dev->of_node;
+	u32 addr = addr_base & 0xffff8000;
 
-	for (i = 0; i < ARRAY_SIZE(compat_lbc); i++) {
-		lbus = of_find_compatible_node(NULL, NULL, compat_lbc[i]);
-		if (lbus)
-			goto found;
-	}
-	return -ENODEV;
+	if (of_device_is_compatible(np, "fsl,elbc"))
+		return addr;
 
-found:
-	fsl_lbc_regs = of_iomap(lbus, 0);
-	of_node_put(lbus);
-	if (!fsl_lbc_regs)
-		return -ENOMEM;
-	return 0;
+	return addr | ((addr_base & 0x300000000ull) >> 19);
 }
-arch_initcall(fsl_lbc_init);
+EXPORT_SYMBOL(fsl_lbc_addr);
 
 /**
  * fsl_lbc_find - find Localbus bank
@@ -65,15 +66,17 @@ arch_initcall(fsl_lbc_init);
 int fsl_lbc_find(phys_addr_t addr_base)
 {
 	int i;
+	struct fsl_lbc_regs __iomem *lbc;
 
-	if (!fsl_lbc_regs)
+	if (!fsl_lbc_ctrl_dev || !fsl_lbc_ctrl_dev->regs)
 		return -ENODEV;
 
-	for (i = 0; i < ARRAY_SIZE(fsl_lbc_regs->bank); i++) {
-		__be32 br = in_be32(&fsl_lbc_regs->bank[i].br);
-		__be32 or = in_be32(&fsl_lbc_regs->bank[i].or);
+	lbc = fsl_lbc_ctrl_dev->regs;
+	for (i = 0; i < ARRAY_SIZE(lbc->bank); i++) {
+		__be32 br = in_be32(&lbc->bank[i].br);
+		__be32 or = in_be32(&lbc->bank[i].or);
 
-		if (br & BR_V && (br & or & BR_BA) == addr_base)
+		if (br & BR_V && (br & or & BR_BA) == fsl_lbc_addr(addr_base))
 			return i;
 	}
 
@@ -94,22 +97,27 @@ int fsl_upm_find(phys_addr_t addr_base, struct fsl_upm *upm)
 {
 	int bank;
 	__be32 br;
+	struct fsl_lbc_regs __iomem *lbc;
 
 	bank = fsl_lbc_find(addr_base);
 	if (bank < 0)
 		return bank;
 
-	br = in_be32(&fsl_lbc_regs->bank[bank].br);
+	if (!fsl_lbc_ctrl_dev || !fsl_lbc_ctrl_dev->regs)
+		return -ENODEV;
+
+	lbc = fsl_lbc_ctrl_dev->regs;
+	br = in_be32(&lbc->bank[bank].br);
 
 	switch (br & BR_MSEL) {
 	case BR_MS_UPMA:
-		upm->mxmr = &fsl_lbc_regs->mamr;
+		upm->mxmr = &lbc->mamr;
 		break;
 	case BR_MS_UPMB:
-		upm->mxmr = &fsl_lbc_regs->mbmr;
+		upm->mxmr = &lbc->mbmr;
 		break;
 	case BR_MS_UPMC:
-		upm->mxmr = &fsl_lbc_regs->mcmr;
+		upm->mxmr = &lbc->mcmr;
 		break;
 	default:
 		return -EINVAL;
@@ -148,9 +156,12 @@ int fsl_upm_run_pattern(struct fsl_upm *upm, void __iomem *io_base, u32 mar)
 	int ret = 0;
 	unsigned long flags;
 
+	if (!fsl_lbc_ctrl_dev || !fsl_lbc_ctrl_dev->regs)
+		return -ENODEV;
+
 	spin_lock_irqsave(&fsl_lbc_lock, flags);
 
-	out_be32(&fsl_lbc_regs->mar, mar);
+	out_be32(&fsl_lbc_ctrl_dev->regs->mar, mar);
 
 	switch (upm->width) {
 	case 8:
@@ -172,3 +183,166 @@ int fsl_upm_run_pattern(struct fsl_upm *upm, void __iomem *io_base, u32 mar)
 	return ret;
 }
 EXPORT_SYMBOL(fsl_upm_run_pattern);
+
+static int __devinit fsl_lbc_ctrl_init(struct fsl_lbc_ctrl *ctrl)
+{
+	struct fsl_lbc_regs __iomem *lbc = ctrl->regs;
+
+	/* clear event registers */
+	setbits32(&lbc->ltesr, LTESR_CLEAR);
+	out_be32(&lbc->lteatr, 0);
+	out_be32(&lbc->ltear, 0);
+	out_be32(&lbc->lteccr, LTECCR_CLEAR);
+	out_be32(&lbc->ltedr, LTEDR_ENABLE);
+
+	/* Enable interrupts for any detected events */
+	out_be32(&lbc->lteir, LTEIR_ENABLE);
+
+	return 0;
+}
+
+/*
+ * NOTE: This interrupt is used to report localbus events of various kinds,
+ * such as transaction errors on the chipselects.
+ */
+
+static irqreturn_t fsl_lbc_ctrl_irq(int irqno, void *data)
+{
+	struct fsl_lbc_ctrl *ctrl = data;
+	struct fsl_lbc_regs __iomem *lbc = ctrl->regs;
+	u32 status;
+
+	status = in_be32(&lbc->ltesr);
+	if (!status)
+		return IRQ_NONE;
+
+	out_be32(&lbc->ltesr, LTESR_CLEAR);
+	out_be32(&lbc->lteatr, 0);
+	out_be32(&lbc->ltear, 0);
+	ctrl->irq_status = status;
+
+	if (status & LTESR_BM)
+		dev_err(ctrl->dev, "Local bus monitor time-out: "
+			"LTESR 0x%08X\n", status);
+	if (status & LTESR_WP)
+		dev_err(ctrl->dev, "Write protect error: "
+			"LTESR 0x%08X\n", status);
+	if (status & LTESR_ATMW)
+		dev_err(ctrl->dev, "Atomic write error: "
+			"LTESR 0x%08X\n", status);
+	if (status & LTESR_ATMR)
+		dev_err(ctrl->dev, "Atomic read error: "
+			"LTESR 0x%08X\n", status);
+	if (status & LTESR_CS)
+		dev_err(ctrl->dev, "Chip select error: "
+			"LTESR 0x%08X\n", status);
+	if (status & LTESR_UPM)
+		;
+	if (status & LTESR_FCT) {
+		dev_err(ctrl->dev, "FCM command time-out: "
+			"LTESR 0x%08X\n", status);
+		smp_wmb();
+		wake_up(&ctrl->irq_wait);
+	}
+	if (status & LTESR_PAR) {
+		dev_err(ctrl->dev, "Parity or Uncorrectable ECC error: "
+			"LTESR 0x%08X\n", status);
+		smp_wmb();
+		wake_up(&ctrl->irq_wait);
+	}
+	if (status & LTESR_CC) {
+		smp_wmb();
+		wake_up(&ctrl->irq_wait);
+	}
+	if (status & ~LTESR_MASK)
+		dev_err(ctrl->dev, "Unknown error: "
+			"LTESR 0x%08X\n", status);
+	return IRQ_HANDLED;
+}
+
+/*
+ * fsl_lbc_ctrl_probe
+ *
+ * called by device layer when it finds a device matching
+ * one our driver can handled. This code allocates all of
+ * the resources needed for the controller only.  The
+ * resources for the NAND banks themselves are allocated
+ * in the chip probe function.
+*/
+
+static int __devinit fsl_lbc_ctrl_probe(struct platform_device *dev)
+{
+	int ret;
+
+	if (!dev->dev.of_node) {
+		dev_err(&dev->dev, "Device OF-Node is NULL");
+		return -EFAULT;
+	}
+
+	fsl_lbc_ctrl_dev = kzalloc(sizeof(*fsl_lbc_ctrl_dev), GFP_KERNEL);
+	if (!fsl_lbc_ctrl_dev)
+		return -ENOMEM;
+
+	dev_set_drvdata(&dev->dev, fsl_lbc_ctrl_dev);
+
+	spin_lock_init(&fsl_lbc_ctrl_dev->lock);
+	init_waitqueue_head(&fsl_lbc_ctrl_dev->irq_wait);
+
+	fsl_lbc_ctrl_dev->regs = of_iomap(dev->dev.of_node, 0);
+	if (!fsl_lbc_ctrl_dev->regs) {
+		dev_err(&dev->dev, "failed to get memory region\n");
+		ret = -ENODEV;
+		goto err;
+	}
+
+	fsl_lbc_ctrl_dev->irq = irq_of_parse_and_map(dev->dev.of_node, 0);
+	if (fsl_lbc_ctrl_dev->irq == NO_IRQ) {
+		dev_err(&dev->dev, "failed to get irq resource\n");
+		ret = -ENODEV;
+		goto err;
+	}
+
+	fsl_lbc_ctrl_dev->dev = &dev->dev;
+
+	ret = fsl_lbc_ctrl_init(fsl_lbc_ctrl_dev);
+	if (ret < 0)
+		goto err;
+
+	ret = request_irq(fsl_lbc_ctrl_dev->irq, fsl_lbc_ctrl_irq, 0,
+				"fsl-lbc", fsl_lbc_ctrl_dev);
+	if (ret != 0) {
+		dev_err(&dev->dev, "failed to install irq (%d)\n",
+			fsl_lbc_ctrl_dev->irq);
+		ret = fsl_lbc_ctrl_dev->irq;
+		goto err;
+	}
+
+	return 0;
+
+err:
+	iounmap(fsl_lbc_ctrl_dev->regs);
+	kfree(fsl_lbc_ctrl_dev);
+	return ret;
+}
+
+static const struct of_device_id fsl_lbc_match[] = {
+	{ .compatible = "fsl,elbc", },
+	{ .compatible = "fsl,pq3-localbus", },
+	{ .compatible = "fsl,pq2-localbus", },
+	{ .compatible = "fsl,pq2pro-localbus", },
+	{},
+};
+
+static struct platform_driver fsl_lbc_ctrl_driver = {
+	.driver = {
+		.name = "fsl-lbc",
+		.of_match_table = fsl_lbc_match,
+	},
+	.probe = fsl_lbc_ctrl_probe,
+};
+
+static int __init fsl_lbc_init(void)
+{
+	return platform_driver_register(&fsl_lbc_ctrl_driver);
+}
+module_init(fsl_lbc_init);
diff --git a/arch/powerpc/sysdev/fsl_msi.c b/arch/powerpc/sysdev/fsl_msi.c
index 87991d3abba..108d76fa8f1 100644
--- a/arch/powerpc/sysdev/fsl_msi.c
+++ b/arch/powerpc/sysdev/fsl_msi.c
@@ -24,6 +24,7 @@
 #include <asm/ppc-pci.h>
 #include <asm/mpic.h>
 #include "fsl_msi.h"
+#include "fsl_pci.h"
 
 LIST_HEAD(msi_head);
 
@@ -51,8 +52,8 @@ static void fsl_msi_end_irq(unsigned int virq)
 }
 
 static struct irq_chip fsl_msi_chip = {
-	.mask		= mask_msi_irq,
-	.unmask		= unmask_msi_irq,
+	.irq_mask	= mask_msi_irq,
+	.irq_unmask	= unmask_msi_irq,
 	.ack		= fsl_msi_end_irq,
 	.name		= "FSL-MSI",
 };
@@ -125,13 +126,11 @@ static void fsl_compose_msi_msg(struct pci_dev *pdev, int hwirq,
 {
 	struct fsl_msi *msi_data = fsl_msi_data;
 	struct pci_controller *hose = pci_bus_to_host(pdev->bus);
-	u32 base = 0;
+	u64 base = fsl_pci_immrbar_base(hose);
 
-	pci_bus_read_config_dword(hose->bus,
-		PCI_DEVFN(0, 0), PCI_BASE_ADDRESS_0, &base);
+	msg->address_lo = msi_data->msi_addr_lo + lower_32_bits(base);
+	msg->address_hi = msi_data->msi_addr_hi + upper_32_bits(base);
 
-	msg->address_lo = msi_data->msi_addr_lo + base;
-	msg->address_hi = msi_data->msi_addr_hi;
 	msg->data = hwirq;
 
 	pr_debug("%s: allocated srs: %d, ibs: %d\n",
diff --git a/arch/powerpc/sysdev/fsl_pci.c b/arch/powerpc/sysdev/fsl_pci.c
index 209384b6e03..818f7c6c8fa 100644
--- a/arch/powerpc/sysdev/fsl_pci.c
+++ b/arch/powerpc/sysdev/fsl_pci.c
@@ -1,7 +1,7 @@
 /*
  * MPC83xx/85xx/86xx PCI/PCIE support routing.
  *
- * Copyright 2007-2009 Freescale Semiconductor, Inc.
+ * Copyright 2007-2010 Freescale Semiconductor, Inc.
  * Copyright 2008-2009 MontaVista Software, Inc.
  *
  * Initial author: Xianghua Xiao <x.xiao@freescale.com>
@@ -34,7 +34,7 @@
 #include <sysdev/fsl_soc.h>
 #include <sysdev/fsl_pci.h>
 
-static int fsl_pcie_bus_fixup;
+static int fsl_pcie_bus_fixup, is_mpc83xx_pci;
 
 static void __init quirk_fsl_pcie_header(struct pci_dev *dev)
 {
@@ -399,16 +399,26 @@ DECLARE_PCI_FIXUP_HEADER(0x1957, PCI_DEVICE_ID_P1013E, quirk_fsl_pcie_header);
 DECLARE_PCI_FIXUP_HEADER(0x1957, PCI_DEVICE_ID_P1013, quirk_fsl_pcie_header);
 DECLARE_PCI_FIXUP_HEADER(0x1957, PCI_DEVICE_ID_P1020E, quirk_fsl_pcie_header);
 DECLARE_PCI_FIXUP_HEADER(0x1957, PCI_DEVICE_ID_P1020, quirk_fsl_pcie_header);
+DECLARE_PCI_FIXUP_HEADER(0x1957, PCI_DEVICE_ID_P1021E, quirk_fsl_pcie_header);
+DECLARE_PCI_FIXUP_HEADER(0x1957, PCI_DEVICE_ID_P1021, quirk_fsl_pcie_header);
 DECLARE_PCI_FIXUP_HEADER(0x1957, PCI_DEVICE_ID_P1022E, quirk_fsl_pcie_header);
 DECLARE_PCI_FIXUP_HEADER(0x1957, PCI_DEVICE_ID_P1022, quirk_fsl_pcie_header);
 DECLARE_PCI_FIXUP_HEADER(0x1957, PCI_DEVICE_ID_P2010E, quirk_fsl_pcie_header);
 DECLARE_PCI_FIXUP_HEADER(0x1957, PCI_DEVICE_ID_P2010, quirk_fsl_pcie_header);
 DECLARE_PCI_FIXUP_HEADER(0x1957, PCI_DEVICE_ID_P2020E, quirk_fsl_pcie_header);
 DECLARE_PCI_FIXUP_HEADER(0x1957, PCI_DEVICE_ID_P2020, quirk_fsl_pcie_header);
+DECLARE_PCI_FIXUP_HEADER(0x1957, PCI_DEVICE_ID_P2040E, quirk_fsl_pcie_header);
+DECLARE_PCI_FIXUP_HEADER(0x1957, PCI_DEVICE_ID_P2040, quirk_fsl_pcie_header);
+DECLARE_PCI_FIXUP_HEADER(0x1957, PCI_DEVICE_ID_P3041E, quirk_fsl_pcie_header);
+DECLARE_PCI_FIXUP_HEADER(0x1957, PCI_DEVICE_ID_P3041, quirk_fsl_pcie_header);
 DECLARE_PCI_FIXUP_HEADER(0x1957, PCI_DEVICE_ID_P4040E, quirk_fsl_pcie_header);
 DECLARE_PCI_FIXUP_HEADER(0x1957, PCI_DEVICE_ID_P4040, quirk_fsl_pcie_header);
 DECLARE_PCI_FIXUP_HEADER(0x1957, PCI_DEVICE_ID_P4080E, quirk_fsl_pcie_header);
 DECLARE_PCI_FIXUP_HEADER(0x1957, PCI_DEVICE_ID_P4080, quirk_fsl_pcie_header);
+DECLARE_PCI_FIXUP_HEADER(0x1957, PCI_DEVICE_ID_P5010E, quirk_fsl_pcie_header);
+DECLARE_PCI_FIXUP_HEADER(0x1957, PCI_DEVICE_ID_P5010, quirk_fsl_pcie_header);
+DECLARE_PCI_FIXUP_HEADER(0x1957, PCI_DEVICE_ID_P5020E, quirk_fsl_pcie_header);
+DECLARE_PCI_FIXUP_HEADER(0x1957, PCI_DEVICE_ID_P5020, quirk_fsl_pcie_header);
 #endif /* CONFIG_FSL_SOC_BOOKE || CONFIG_PPC_86xx */
 
 #if defined(CONFIG_PPC_83xx) || defined(CONFIG_PPC_MPC512x)
@@ -428,6 +438,13 @@ struct mpc83xx_pcie_priv {
 	u32 dev_base;
 };
 
+struct pex_inbound_window {
+	u32 ar;
+	u32 tar;
+	u32 barl;
+	u32 barh;
+};
+
 /*
  * With the convention of u-boot, the PCIE outbound window 0 serves
  * as configuration transactions outbound.
@@ -435,6 +452,8 @@ struct mpc83xx_pcie_priv {
 #define PEX_OUTWIN0_BAR		0xCA4
 #define PEX_OUTWIN0_TAL		0xCA8
 #define PEX_OUTWIN0_TAH		0xCAC
+#define PEX_RC_INWIN_BASE	0xE60
+#define PEX_RCIWARn_EN		0x1
 
 static int mpc83xx_pcie_exclude_device(struct pci_bus *bus, unsigned int devfn)
 {
@@ -602,6 +621,8 @@ int __init mpc83xx_add_bridge(struct device_node *dev)
 	const int *bus_range;
 	int primary;
 
+	is_mpc83xx_pci = 1;
+
 	if (!of_device_is_available(dev)) {
 		pr_warning("%s: disabled by the firmware.\n",
 			   dev->full_name);
@@ -681,3 +702,40 @@ err0:
 	return ret;
 }
 #endif /* CONFIG_PPC_83xx */
+
+u64 fsl_pci_immrbar_base(struct pci_controller *hose)
+{
+#ifdef CONFIG_PPC_83xx
+	if (is_mpc83xx_pci) {
+		struct mpc83xx_pcie_priv *pcie = hose->dn->data;
+		struct pex_inbound_window *in;
+		int i;
+
+		/* Walk the Root Complex Inbound windows to match IMMR base */
+		in = pcie->cfg_type0 + PEX_RC_INWIN_BASE;
+		for (i = 0; i < 4; i++) {
+			/* not enabled, skip */
+			if (!in_le32(&in[i].ar) & PEX_RCIWARn_EN)
+				 continue;
+
+			if (get_immrbase() == in_le32(&in[i].tar))
+				return (u64)in_le32(&in[i].barh) << 32 |
+					    in_le32(&in[i].barl);
+		}
+
+		printk(KERN_WARNING "could not find PCI BAR matching IMMR\n");
+	}
+#endif
+
+#if defined(CONFIG_FSL_SOC_BOOKE) || defined(CONFIG_PPC_86xx)
+	if (!is_mpc83xx_pci) {
+		u32 base;
+
+		pci_bus_read_config_dword(hose->bus,
+			PCI_DEVFN(0, 0), PCI_BASE_ADDRESS_0, &base);
+		return base;
+	}
+#endif
+
+	return 0;
+}
diff --git a/arch/powerpc/sysdev/fsl_pci.h b/arch/powerpc/sysdev/fsl_pci.h
index a9d8bbebed8..8ad72a11f77 100644
--- a/arch/powerpc/sysdev/fsl_pci.h
+++ b/arch/powerpc/sysdev/fsl_pci.h
@@ -88,6 +88,7 @@ struct ccsr_pci {
 extern int fsl_add_bridge(struct device_node *dev, int is_primary);
 extern void fsl_pcibios_fixup_bus(struct pci_bus *bus);
 extern int mpc83xx_add_bridge(struct device_node *dev);
+u64 fsl_pci_immrbar_base(struct pci_controller *hose);
 
 #endif /* __POWERPC_FSL_PCI_H */
 #endif /* __KERNEL__ */
diff --git a/arch/powerpc/sysdev/fsl_rio.c b/arch/powerpc/sysdev/fsl_rio.c
index 8bd86530ee2..9725369d432 100644
--- a/arch/powerpc/sysdev/fsl_rio.c
+++ b/arch/powerpc/sysdev/fsl_rio.c
@@ -50,6 +50,7 @@
 #define RIO_ATMU_REGS_OFFSET	0x10c00
 #define RIO_P_MSG_REGS_OFFSET	0x11000
 #define RIO_S_MSG_REGS_OFFSET	0x13000
+#define RIO_GCCSR		0x13c
 #define RIO_ESCSR		0x158
 #define RIO_CCSR		0x15c
 #define RIO_LTLEDCSR		0x0608
@@ -87,6 +88,9 @@
 #define RIO_IPWSR_PWD		0x00000008
 #define RIO_IPWSR_PWB		0x00000004
 
+#define RIO_EPWISR_PINT		0x80000000
+#define RIO_EPWISR_PW		0x00000001
+
 #define RIO_MSG_DESC_SIZE	32
 #define RIO_MSG_BUFFER_SIZE	4096
 #define RIO_MIN_TX_RING_SIZE	2
@@ -117,44 +121,59 @@ struct rio_atmu_regs {
 };
 
 struct rio_msg_regs {
-	u32 omr;
-	u32 osr;
+	u32 omr;	/* 0xD_3000 - Outbound message 0 mode register */
+	u32 osr;	/* 0xD_3004 - Outbound message 0 status register */
 	u32 pad1;
-	u32 odqdpar;
+	u32 odqdpar;	/* 0xD_300C - Outbound message 0 descriptor queue
+			   dequeue pointer address register */
 	u32 pad2;
-	u32 osar;
-	u32 odpr;
-	u32 odatr;
-	u32 odcr;
+	u32 osar;	/* 0xD_3014 - Outbound message 0 source address
+			   register */
+	u32 odpr;	/* 0xD_3018 - Outbound message 0 destination port
+			   register */
+	u32 odatr;	/* 0xD_301C - Outbound message 0 destination attributes
+			   Register*/
+	u32 odcr;	/* 0xD_3020 - Outbound message 0 double-word count
+			   register */
 	u32 pad3;
-	u32 odqepar;
+	u32 odqepar;	/* 0xD_3028 - Outbound message 0 descriptor queue
+			   enqueue pointer address register */
 	u32 pad4[13];
-	u32 imr;
-	u32 isr;
+	u32 imr;	/* 0xD_3060 - Inbound message 0 mode register */
+	u32 isr;	/* 0xD_3064 - Inbound message 0 status register */
 	u32 pad5;
-	u32 ifqdpar;
+	u32 ifqdpar;	/* 0xD_306C - Inbound message 0 frame queue dequeue
+			   pointer address register*/
 	u32 pad6;
-	u32 ifqepar;
+	u32 ifqepar;	/* 0xD_3074 - Inbound message 0 frame queue enqueue
+			   pointer address register */
 	u32 pad7[226];
-	u32 odmr;
-	u32 odsr;
+	u32 odmr;	/* 0xD_3400 - Outbound doorbell mode register */
+	u32 odsr;	/* 0xD_3404 - Outbound doorbell status register */
 	u32 res0[4];
-	u32 oddpr;
-	u32 oddatr;
+	u32 oddpr;	/* 0xD_3418 - Outbound doorbell destination port
+			   register */
+	u32 oddatr;	/* 0xD_341c - Outbound doorbell destination attributes
+			   register */
 	u32 res1[3];
-	u32 odretcr;
+	u32 odretcr;	/* 0xD_342C - Outbound doorbell retry error threshold
+			   configuration register */
 	u32 res2[12];
-	u32 dmr;
-	u32 dsr;
+	u32 dmr;	/* 0xD_3460 - Inbound doorbell mode register */
+	u32 dsr;	/* 0xD_3464 - Inbound doorbell status register */
 	u32 pad8;
-	u32 dqdpar;
+	u32 dqdpar;	/* 0xD_346C - Inbound doorbell queue dequeue Pointer
+			   address register */
 	u32 pad9;
-	u32 dqepar;
+	u32 dqepar;	/* 0xD_3474 - Inbound doorbell Queue enqueue pointer
+			   address register */
 	u32 pad10[26];
-	u32 pwmr;
-	u32 pwsr;
-	u32 epwqbar;
-	u32 pwqbar;
+	u32 pwmr;	/* 0xD_34E0 - Inbound port-write mode register */
+	u32 pwsr;	/* 0xD_34E4 - Inbound port-write status register */
+	u32 epwqbar;	/* 0xD_34E8 - Extended Port-Write Queue Base Address
+			   register */
+	u32 pwqbar;	/* 0xD_34EC - Inbound port-write queue base address
+			   register */
 };
 
 struct rio_tx_desc {
@@ -240,12 +259,13 @@ struct rio_priv {
 
 static void __iomem *rio_regs_win;
 
+#ifdef CONFIG_E500
 static int (*saved_mcheck_exception)(struct pt_regs *regs);
 
 static int fsl_rio_mcheck_exception(struct pt_regs *regs)
 {
 	const struct exception_table_entry *entry = NULL;
-	unsigned long reason = (mfspr(SPRN_MCSR) & MCSR_MASK);
+	unsigned long reason = mfspr(SPRN_MCSR);
 
 	if (reason & MCSR_BUS_RBERR) {
 		reason = in_be32((u32 *)(rio_regs_win + RIO_LTLEDCSR));
@@ -269,6 +289,7 @@ static int fsl_rio_mcheck_exception(struct pt_regs *regs)
 	else
 		return cur_cpu_spec->machine_check(regs);
 }
+#endif
 
 /**
  * fsl_rio_doorbell_send - Send a MPC85xx doorbell message
@@ -1065,18 +1086,12 @@ fsl_rio_port_write_handler(int irq, void *dev_instance)
 	struct rio_priv *priv = port->priv;
 	u32 epwisr, tmp;
 
-	ipwmr = in_be32(&priv->msg_regs->pwmr);
-	ipwsr = in_be32(&priv->msg_regs->pwsr);
-
 	epwisr = in_be32(priv->regs_win + RIO_EPWISR);
-	if (epwisr & 0x80000000) {
-		tmp = in_be32(priv->regs_win + RIO_LTLEDCSR);
-		pr_info("RIO_LTLEDCSR = 0x%x\n", tmp);
-		out_be32(priv->regs_win + RIO_LTLEDCSR, 0);
-	}
+	if (!(epwisr & RIO_EPWISR_PW))
+		goto pw_done;
 
-	if (!(epwisr & 0x00000001))
-		return IRQ_HANDLED;
+	ipwmr = in_be32(&priv->msg_regs->pwmr);
+	ipwsr = in_be32(&priv->msg_regs->pwsr);
 
 #ifdef DEBUG_PW
 	pr_debug("PW Int->IPWMR: 0x%08x IPWSR: 0x%08x (", ipwmr, ipwsr);
@@ -1092,20 +1107,6 @@ fsl_rio_port_write_handler(int irq, void *dev_instance)
 		pr_debug(" PWB");
 	pr_debug(" )\n");
 #endif
-	out_be32(&priv->msg_regs->pwsr,
-		 ipwsr & (RIO_IPWSR_TE | RIO_IPWSR_QFI | RIO_IPWSR_PWD));
-
-	if ((ipwmr & RIO_IPWMR_EIE) && (ipwsr & RIO_IPWSR_TE)) {
-		priv->port_write_msg.err_count++;
-		pr_info("RIO: Port-Write Transaction Err (%d)\n",
-			 priv->port_write_msg.err_count);
-	}
-	if (ipwsr & RIO_IPWSR_PWD) {
-		priv->port_write_msg.discard_count++;
-		pr_info("RIO: Port Discarded Port-Write Msg(s) (%d)\n",
-			 priv->port_write_msg.discard_count);
-	}
-
 	/* Schedule deferred processing if PW was received */
 	if (ipwsr & RIO_IPWSR_QFI) {
 		/* Save PW message (if there is room in FIFO),
@@ -1117,16 +1118,43 @@ fsl_rio_port_write_handler(int irq, void *dev_instance)
 				 RIO_PW_MSG_SIZE);
 		} else {
 			priv->port_write_msg.discard_count++;
-			pr_info("RIO: ISR Discarded Port-Write Msg(s) (%d)\n",
+			pr_debug("RIO: ISR Discarded Port-Write Msg(s) (%d)\n",
 				 priv->port_write_msg.discard_count);
 		}
+		/* Clear interrupt and issue Clear Queue command. This allows
+		 * another port-write to be received.
+		 */
+		out_be32(&priv->msg_regs->pwsr,	RIO_IPWSR_QFI);
+		out_be32(&priv->msg_regs->pwmr, ipwmr | RIO_IPWMR_CQ);
+
 		schedule_work(&priv->pw_work);
 	}
 
-	/* Issue Clear Queue command. This allows another
-	 * port-write to be received.
-	 */
-	out_be32(&priv->msg_regs->pwmr, ipwmr | RIO_IPWMR_CQ);
+	if ((ipwmr & RIO_IPWMR_EIE) && (ipwsr & RIO_IPWSR_TE)) {
+		priv->port_write_msg.err_count++;
+		pr_debug("RIO: Port-Write Transaction Err (%d)\n",
+			 priv->port_write_msg.err_count);
+		/* Clear Transaction Error: port-write controller should be
+		 * disabled when clearing this error
+		 */
+		out_be32(&priv->msg_regs->pwmr, ipwmr & ~RIO_IPWMR_PWE);
+		out_be32(&priv->msg_regs->pwsr,	RIO_IPWSR_TE);
+		out_be32(&priv->msg_regs->pwmr, ipwmr);
+	}
+
+	if (ipwsr & RIO_IPWSR_PWD) {
+		priv->port_write_msg.discard_count++;
+		pr_debug("RIO: Port Discarded Port-Write Msg(s) (%d)\n",
+			 priv->port_write_msg.discard_count);
+		out_be32(&priv->msg_regs->pwsr, RIO_IPWSR_PWD);
+	}
+
+pw_done:
+	if (epwisr & RIO_EPWISR_PINT) {
+		tmp = in_be32(priv->regs_win + RIO_LTLEDCSR);
+		pr_debug("RIO_LTLEDCSR = 0x%x\n", tmp);
+		out_be32(priv->regs_win + RIO_LTLEDCSR, 0);
+	}
 
 	return IRQ_HANDLED;
 }
@@ -1332,7 +1360,7 @@ static inline void fsl_rio_info(struct device *dev, u32 ccsr)
 
 /**
  * fsl_rio_setup - Setup Freescale PowerPC RapidIO interface
- * @dev: of_device pointer
+ * @dev: platform_device pointer
  *
  * Initializes MPC85xx RapidIO hardware interface, configures
  * master port with system-specific info, and registers the
@@ -1444,6 +1472,7 @@ int fsl_rio_setup(struct platform_device *dev)
 	port->host_deviceid = fsl_rio_get_hdid(port->id);
 
 	port->priv = priv;
+	port->phys_efptr = 0x100;
 	rio_register_mport(port);
 
 	priv->regs_win = ioremap(regs.start, regs.end - regs.start + 1);
@@ -1491,6 +1520,12 @@ int fsl_rio_setup(struct platform_device *dev)
 	dev_info(&dev->dev, "RapidIO Common Transport System size: %d\n",
 			port->sys_size ? 65536 : 256);
 
+	if (port->host_deviceid >= 0)
+		out_be32(priv->regs_win + RIO_GCCSR, RIO_PORT_GEN_HOST |
+			RIO_PORT_GEN_MASTER | RIO_PORT_GEN_DISCOVERED);
+	else
+		out_be32(priv->regs_win + RIO_GCCSR, 0x00000000);
+
 	priv->atmu_regs = (struct rio_atmu_regs *)(priv->regs_win
 					+ RIO_ATMU_REGS_OFFSET);
 	priv->maint_atmu_regs = priv->atmu_regs + 1;
@@ -1517,8 +1552,10 @@ int fsl_rio_setup(struct platform_device *dev)
 	fsl_rio_doorbell_init(port);
 	fsl_rio_port_write_init(port);
 
+#ifdef CONFIG_E500
 	saved_mcheck_exception = ppc_md.machine_check_exception;
 	ppc_md.machine_check_exception = fsl_rio_mcheck_exception;
+#endif
 	/* Ensure that RFXE is set */
 	mtspr(SPRN_HID1, (mfspr(SPRN_HID1) | 0x20000));
 
diff --git a/arch/powerpc/sysdev/fsl_soc.c b/arch/powerpc/sysdev/fsl_soc.c
index b91f7acdda6..19e5015e039 100644
--- a/arch/powerpc/sysdev/fsl_soc.c
+++ b/arch/powerpc/sysdev/fsl_soc.c
@@ -209,186 +209,29 @@ static int __init of_add_fixed_phys(void)
 arch_initcall(of_add_fixed_phys);
 #endif /* CONFIG_FIXED_PHY */
 
-static enum fsl_usb2_phy_modes determine_usb_phy(const char *phy_type)
-{
-	if (!phy_type)
-		return FSL_USB2_PHY_NONE;
-	if (!strcasecmp(phy_type, "ulpi"))
-		return FSL_USB2_PHY_ULPI;
-	if (!strcasecmp(phy_type, "utmi"))
-		return FSL_USB2_PHY_UTMI;
-	if (!strcasecmp(phy_type, "utmi_wide"))
-		return FSL_USB2_PHY_UTMI_WIDE;
-	if (!strcasecmp(phy_type, "serial"))
-		return FSL_USB2_PHY_SERIAL;
-
-	return FSL_USB2_PHY_NONE;
-}
+#if defined(CONFIG_FSL_SOC_BOOKE) || defined(CONFIG_PPC_86xx)
+static __be32 __iomem *rstcr;
 
-static int __init fsl_usb_of_init(void)
+static int __init setup_rstcr(void)
 {
 	struct device_node *np;
-	unsigned int i = 0;
-	struct platform_device *usb_dev_mph = NULL, *usb_dev_dr_host = NULL,
-		*usb_dev_dr_client = NULL;
-	int ret;
-
-	for_each_compatible_node(np, NULL, "fsl-usb2-mph") {
-		struct resource r[2];
-		struct fsl_usb2_platform_data usb_data;
-		const unsigned char *prop = NULL;
-
-		memset(&r, 0, sizeof(r));
-		memset(&usb_data, 0, sizeof(usb_data));
-
-		ret = of_address_to_resource(np, 0, &r[0]);
-		if (ret)
-			goto err;
-
-		of_irq_to_resource(np, 0, &r[1]);
-
-		usb_dev_mph =
-		    platform_device_register_simple("fsl-ehci", i, r, 2);
-		if (IS_ERR(usb_dev_mph)) {
-			ret = PTR_ERR(usb_dev_mph);
-			goto err;
-		}
-
-		usb_dev_mph->dev.coherent_dma_mask = 0xffffffffUL;
-		usb_dev_mph->dev.dma_mask = &usb_dev_mph->dev.coherent_dma_mask;
-
-		usb_data.operating_mode = FSL_USB2_MPH_HOST;
-
-		prop = of_get_property(np, "port0", NULL);
-		if (prop)
-			usb_data.port_enables |= FSL_USB2_PORT0_ENABLED;
-
-		prop = of_get_property(np, "port1", NULL);
-		if (prop)
-			usb_data.port_enables |= FSL_USB2_PORT1_ENABLED;
-
-		prop = of_get_property(np, "phy_type", NULL);
-		usb_data.phy_mode = determine_usb_phy(prop);
-
-		ret =
-		    platform_device_add_data(usb_dev_mph, &usb_data,
-					     sizeof(struct
-						    fsl_usb2_platform_data));
-		if (ret)
-			goto unreg_mph;
-		i++;
-	}
-
-	for_each_compatible_node(np, NULL, "fsl-usb2-dr") {
-		struct resource r[2];
-		struct fsl_usb2_platform_data usb_data;
-		const unsigned char *prop = NULL;
 
-		if (!of_device_is_available(np))
-			continue;
-
-		memset(&r, 0, sizeof(r));
-		memset(&usb_data, 0, sizeof(usb_data));
-
-		ret = of_address_to_resource(np, 0, &r[0]);
-		if (ret)
-			goto unreg_mph;
-
-		of_irq_to_resource(np, 0, &r[1]);
-
-		prop = of_get_property(np, "dr_mode", NULL);
-
-		if (!prop || !strcmp(prop, "host")) {
-			usb_data.operating_mode = FSL_USB2_DR_HOST;
-			usb_dev_dr_host = platform_device_register_simple(
-					"fsl-ehci", i, r, 2);
-			if (IS_ERR(usb_dev_dr_host)) {
-				ret = PTR_ERR(usb_dev_dr_host);
-				goto err;
-			}
-		} else if (prop && !strcmp(prop, "peripheral")) {
-			usb_data.operating_mode = FSL_USB2_DR_DEVICE;
-			usb_dev_dr_client = platform_device_register_simple(
-					"fsl-usb2-udc", i, r, 2);
-			if (IS_ERR(usb_dev_dr_client)) {
-				ret = PTR_ERR(usb_dev_dr_client);
-				goto err;
-			}
-		} else if (prop && !strcmp(prop, "otg")) {
-			usb_data.operating_mode = FSL_USB2_DR_OTG;
-			usb_dev_dr_host = platform_device_register_simple(
-					"fsl-ehci", i, r, 2);
-			if (IS_ERR(usb_dev_dr_host)) {
-				ret = PTR_ERR(usb_dev_dr_host);
-				goto err;
-			}
-			usb_dev_dr_client = platform_device_register_simple(
-					"fsl-usb2-udc", i, r, 2);
-			if (IS_ERR(usb_dev_dr_client)) {
-				ret = PTR_ERR(usb_dev_dr_client);
-				goto err;
-			}
-		} else {
-			ret = -EINVAL;
-			goto err;
+	for_each_node_by_name(np, "global-utilities") {
+		if ((of_get_property(np, "fsl,has-rstcr", NULL))) {
+			rstcr = of_iomap(np, 0) + 0xb0;
+			if (!rstcr)
+				printk (KERN_ERR "Error: reset control "
+						"register not mapped!\n");
+			break;
 		}
-
-		prop = of_get_property(np, "phy_type", NULL);
-		usb_data.phy_mode = determine_usb_phy(prop);
-
-		if (usb_dev_dr_host) {
-			usb_dev_dr_host->dev.coherent_dma_mask = 0xffffffffUL;
-			usb_dev_dr_host->dev.dma_mask = &usb_dev_dr_host->
-				dev.coherent_dma_mask;
-			if ((ret = platform_device_add_data(usb_dev_dr_host,
-						&usb_data, sizeof(struct
-						fsl_usb2_platform_data))))
-				goto unreg_dr;
-		}
-		if (usb_dev_dr_client) {
-			usb_dev_dr_client->dev.coherent_dma_mask = 0xffffffffUL;
-			usb_dev_dr_client->dev.dma_mask = &usb_dev_dr_client->
-				dev.coherent_dma_mask;
-			if ((ret = platform_device_add_data(usb_dev_dr_client,
-						&usb_data, sizeof(struct
-						fsl_usb2_platform_data))))
-				goto unreg_dr;
-		}
-		i++;
 	}
-	return 0;
-
-unreg_dr:
-	if (usb_dev_dr_host)
-		platform_device_unregister(usb_dev_dr_host);
-	if (usb_dev_dr_client)
-		platform_device_unregister(usb_dev_dr_client);
-unreg_mph:
-	if (usb_dev_mph)
-		platform_device_unregister(usb_dev_mph);
-err:
-	return ret;
-}
-
-arch_initcall(fsl_usb_of_init);
-
-#if defined(CONFIG_FSL_SOC_BOOKE) || defined(CONFIG_PPC_86xx)
-static __be32 __iomem *rstcr;
 
-static int __init setup_rstcr(void)
-{
-	struct device_node *np;
-	np = of_find_node_by_name(NULL, "global-utilities");
-	if ((np && of_get_property(np, "fsl,has-rstcr", NULL))) {
-		rstcr = of_iomap(np, 0) + 0xb0;
-		if (!rstcr)
-			printk (KERN_EMERG "Error: reset control register "
-					"not mapped!\n");
-	} else if (ppc_md.restart == fsl_rstcr_restart)
+	if (!rstcr && ppc_md.restart == fsl_rstcr_restart)
 		printk(KERN_ERR "No RSTCR register, warm reboot won't work\n");
 
 	if (np)
 		of_node_put(np);
+
 	return 0;
 }
 
diff --git a/arch/powerpc/sysdev/mpc8xxx_gpio.c b/arch/powerpc/sysdev/mpc8xxx_gpio.c
index 36499394a16..c48cd817807 100644
--- a/arch/powerpc/sysdev/mpc8xxx_gpio.c
+++ b/arch/powerpc/sysdev/mpc8xxx_gpio.c
@@ -389,6 +389,9 @@ static int __init mpc8xxx_add_gpiochips(void)
 	for_each_matching_node(np, mpc8xxx_gpio_ids)
 		mpc8xxx_add_controller(np);
 
+	for_each_compatible_node(np, NULL, "fsl,qoriq-gpio")
+		mpc8xxx_add_controller(np);
+
 	return 0;
 }
 arch_initcall(mpc8xxx_add_gpiochips);
diff --git a/arch/powerpc/sysdev/mpic_pasemi_msi.c b/arch/powerpc/sysdev/mpic_pasemi_msi.c
index 3b6a9a43718..320ad5a9a25 100644
--- a/arch/powerpc/sysdev/mpic_pasemi_msi.c
+++ b/arch/powerpc/sysdev/mpic_pasemi_msi.c
@@ -39,24 +39,24 @@
 static struct mpic *msi_mpic;
 
 
-static void mpic_pasemi_msi_mask_irq(unsigned int irq)
+static void mpic_pasemi_msi_mask_irq(struct irq_data *data)
 {
-	pr_debug("mpic_pasemi_msi_mask_irq %d\n", irq);
-	mask_msi_irq(irq);
-	mpic_mask_irq(irq);
+	pr_debug("mpic_pasemi_msi_mask_irq %d\n", data->irq);
+	mask_msi_irq(data);
+	mpic_mask_irq(data->irq);
 }
 
-static void mpic_pasemi_msi_unmask_irq(unsigned int irq)
+static void mpic_pasemi_msi_unmask_irq(struct irq_data *data)
 {
-	pr_debug("mpic_pasemi_msi_unmask_irq %d\n", irq);
-	mpic_unmask_irq(irq);
-	unmask_msi_irq(irq);
+	pr_debug("mpic_pasemi_msi_unmask_irq %d\n", data->irq);
+	mpic_unmask_irq(data->irq);
+	unmask_msi_irq(data);
 }
 
 static struct irq_chip mpic_pasemi_msi_chip = {
-	.shutdown	= mpic_pasemi_msi_mask_irq,
-	.mask		= mpic_pasemi_msi_mask_irq,
-	.unmask		= mpic_pasemi_msi_unmask_irq,
+	.irq_shutdown	= mpic_pasemi_msi_mask_irq,
+	.irq_mask	= mpic_pasemi_msi_mask_irq,
+	.irq_unmask	= mpic_pasemi_msi_unmask_irq,
 	.eoi		= mpic_end_irq,
 	.set_type	= mpic_set_irq_type,
 	.set_affinity	= mpic_set_affinity,
diff --git a/arch/powerpc/sysdev/mpic_u3msi.c b/arch/powerpc/sysdev/mpic_u3msi.c
index bcbfe79c704..a2b028b4a20 100644
--- a/arch/powerpc/sysdev/mpic_u3msi.c
+++ b/arch/powerpc/sysdev/mpic_u3msi.c
@@ -23,22 +23,22 @@
 /* A bit ugly, can we get this from the pci_dev somehow? */
 static struct mpic *msi_mpic;
 
-static void mpic_u3msi_mask_irq(unsigned int irq)
+static void mpic_u3msi_mask_irq(struct irq_data *data)
 {
-	mask_msi_irq(irq);
-	mpic_mask_irq(irq);
+	mask_msi_irq(data);
+	mpic_mask_irq(data->irq);
 }
 
-static void mpic_u3msi_unmask_irq(unsigned int irq)
+static void mpic_u3msi_unmask_irq(struct irq_data *data)
 {
-	mpic_unmask_irq(irq);
-	unmask_msi_irq(irq);
+	mpic_unmask_irq(data->irq);
+	unmask_msi_irq(data);
 }
 
 static struct irq_chip mpic_u3msi_chip = {
-	.shutdown	= mpic_u3msi_mask_irq,
-	.mask		= mpic_u3msi_mask_irq,
-	.unmask		= mpic_u3msi_unmask_irq,
+	.irq_shutdown	= mpic_u3msi_mask_irq,
+	.irq_mask	= mpic_u3msi_mask_irq,
+	.irq_unmask	= mpic_u3msi_unmask_irq,
 	.eoi		= mpic_end_irq,
 	.set_type	= mpic_set_irq_type,
 	.set_affinity	= mpic_set_affinity,
diff --git a/arch/powerpc/sysdev/pmi.c b/arch/powerpc/sysdev/pmi.c
index 24a0bb955b1..4260f368db5 100644
--- a/arch/powerpc/sysdev/pmi.c
+++ b/arch/powerpc/sysdev/pmi.c
@@ -114,7 +114,7 @@ static void pmi_notify_handlers(struct work_struct *work)
 
 	spin_lock(&data->handler_spinlock);
 	list_for_each_entry(handler, &data->handler, node) {
-		pr_debug(KERN_INFO "pmi: notifying handler %p\n", handler);
+		pr_debug("pmi: notifying handler %p\n", handler);
 		if (handler->type == data->msg.type)
 			handler->handle_pmi_message(data->msg);
 	}
diff --git a/arch/powerpc/sysdev/qe_lib/qe.c b/arch/powerpc/sysdev/qe_lib/qe.c
index 3da8014931c..90020de4dcf 100644
--- a/arch/powerpc/sysdev/qe_lib/qe.c
+++ b/arch/powerpc/sysdev/qe_lib/qe.c
@@ -640,6 +640,7 @@ unsigned int qe_get_num_of_snums(void)
 		if ((num_of_snums < 28) || (num_of_snums > QE_NUM_OF_SNUM)) {
 			/* No QE ever has fewer than 28 SNUMs */
 			pr_err("QE: number of snum is invalid\n");
+			of_node_put(qe);
 			return -EINVAL;
 		}
 	}