10 files changed, 459 insertions, 58 deletions

diff --git a/arch/powerpc/Kconfig b/arch/powerpc/Kconfig
index c4c4549c22bb..66a315e06dce 100644
--- a/arch/powerpc/Kconfig
+++ b/arch/powerpc/Kconfig
@@ -663,6 +663,9 @@ config ZONE_DMA
 config NEED_DMA_MAP_STATE
         def_bool (PPC64 || NOT_COHERENT_CACHE)
 
+config NEED_SG_DMA_LENGTH
+        def_bool y
+
 config GENERIC_ISA_DMA
         bool
         depends on PPC64 || POWER4 || 6xx && !CPM2
diff --git a/arch/powerpc/include/asm/mpc52xx_psc.h b/arch/powerpc/include/asm/mpc52xx_psc.h
index 42561f4f032d..ecc4fc69ac13 100644
--- a/arch/powerpc/include/asm/mpc52xx_psc.h
+++ b/arch/powerpc/include/asm/mpc52xx_psc.h
@@ -248,6 +248,7 @@ struct mpc52xx_psc_fifo {
         u16             tflwfptr;       /* PSC + 0x9e */
 };
 
+#define MPC512x_PSC_FIFO_EOF            0x100
 #define MPC512x_PSC_FIFO_RESET_SLICE    0x80
 #define MPC512x_PSC_FIFO_ENABLE_SLICE   0x01
 #define MPC512x_PSC_FIFO_ENABLE_DMA     0x04
diff --git a/arch/powerpc/include/asm/scatterlist.h b/arch/powerpc/include/asm/scatterlist.h
index 912bf597870f..34cc78fd0ef4 100644
--- a/arch/powerpc/include/asm/scatterlist.h
+++ b/arch/powerpc/include/asm/scatterlist.h
@@ -9,38 +9,12 @@
  * 2 of the License, or (at your option) any later version.
  */
 
-#ifdef __KERNEL__
-#include <linux/types.h>
 #include <asm/dma.h>
-
-struct scatterlist {
-#ifdef CONFIG_DEBUG_SG
-        unsigned long sg_magic;
-#endif
-        unsigned long page_link;
-        unsigned int offset;
-        unsigned int length;
-
-        /* For TCE or SWIOTLB support */
-        dma_addr_t dma_address;
-        u32 dma_length;
-};
-
-/*
- * These macros should be used after a dma_map_sg call has been done
- * to get bus addresses of each of the SG entries and their lengths.
- * You should only work with the number of sg entries pci_map_sg
- * returns, or alternatively stop on the first sg_dma_len(sg) which
- * is 0.
- */
-#define sg_dma_address(sg)      ((sg)->dma_address)
-#define sg_dma_len(sg)          ((sg)->dma_length)
+#include <asm-generic/scatterlist.h>
 
 #ifdef __powerpc64__
 #define ISA_DMA_THRESHOLD       (~0UL)
 #endif
-
 #define ARCH_HAS_SG_CHAIN
 
-#endif /* __KERNEL__ */
 #endif /* _ASM_POWERPC_SCATTERLIST_H */
diff --git a/arch/powerpc/kernel/dma-swiotlb.c b/arch/powerpc/kernel/dma-swiotlb.c
index 4ff4da2c238b..e7fe218b8697 100644
--- a/arch/powerpc/kernel/dma-swiotlb.c
+++ b/arch/powerpc/kernel/dma-swiotlb.c
@@ -39,8 +39,8 @@ struct dma_map_ops swiotlb_dma_ops = {
         .dma_supported = swiotlb_dma_supported,
         .map_page = swiotlb_map_page,
         .unmap_page = swiotlb_unmap_page,
-        .sync_single_range_for_cpu = swiotlb_sync_single_range_for_cpu,
-        .sync_single_range_for_device = swiotlb_sync_single_range_for_device,
+        .sync_single_for_cpu = swiotlb_sync_single_for_cpu,
+        .sync_single_for_device = swiotlb_sync_single_for_device,
         .sync_sg_for_cpu = swiotlb_sync_sg_for_cpu,
         .sync_sg_for_device = swiotlb_sync_sg_for_device,
         .mapping_error = swiotlb_dma_mapping_error,
diff --git a/arch/powerpc/kernel/dma.c b/arch/powerpc/kernel/dma.c
index 6c1df5757cd6..8d1de6f31d5a 100644
--- a/arch/powerpc/kernel/dma.c
+++ b/arch/powerpc/kernel/dma.c
@@ -127,11 +127,11 @@ static inline void dma_direct_sync_sg(struct device *dev,
                 __dma_sync_page(sg_page(sg), sg->offset, sg->length, direction);
 }
 
-static inline void dma_direct_sync_single_range(struct device *dev,
-                dma_addr_t dma_handle, unsigned long offset, size_t size,
-                enum dma_data_direction direction)
+static inline void dma_direct_sync_single(struct device *dev,
+                                          dma_addr_t dma_handle, size_t size,
+                                          enum dma_data_direction direction)
 {
-        __dma_sync(bus_to_virt(dma_handle+offset), size, direction);
+        __dma_sync(bus_to_virt(dma_handle), size, direction);
 }
 #endif
 
@@ -144,8 +144,8 @@ struct dma_map_ops dma_direct_ops = {
         .map_page       = dma_direct_map_page,
         .unmap_page     = dma_direct_unmap_page,
 #ifdef CONFIG_NOT_COHERENT_CACHE
-        .sync_single_range_for_cpu      = dma_direct_sync_single_range,
-        .sync_single_range_for_device   = dma_direct_sync_single_range,
+        .sync_single_for_cpu            = dma_direct_sync_single,
+        .sync_single_for_device         = dma_direct_sync_single,
         .sync_sg_for_cpu                = dma_direct_sync_sg,
         .sync_sg_for_device             = dma_direct_sync_sg,
 #endif
diff --git a/arch/powerpc/platforms/512x/mpc512x_shared.c b/arch/powerpc/platforms/512x/mpc512x_shared.c
index b7f518a60f03..707e572b7c40 100644
--- a/arch/powerpc/platforms/512x/mpc512x_shared.c
+++ b/arch/powerpc/platforms/512x/mpc512x_shared.c
@@ -22,6 +22,7 @@
 #include <asm/prom.h>
 #include <asm/time.h>
 #include <asm/mpc5121.h>
+#include <asm/mpc52xx_psc.h>
 
 #include "mpc512x.h"
 
@@ -95,9 +96,86 @@ void __init mpc512x_declare_of_platform_devices(void)
         }
 }
 
+#define DEFAULT_FIFO_SIZE 16
+
+static unsigned int __init get_fifo_size(struct device_node *np,
+                                         char *prop_name)
+{
+        const unsigned int *fp;
+
+        fp = of_get_property(np, prop_name, NULL);
+        if (fp)
+                return *fp;
+
+        pr_warning("no %s property in %s node, defaulting to %d\n",
+                   prop_name, np->full_name, DEFAULT_FIFO_SIZE);
+
+        return DEFAULT_FIFO_SIZE;
+}
+
+#define FIFOC(_base) ((struct mpc512x_psc_fifo __iomem *) \
+                    ((u32)(_base) + sizeof(struct mpc52xx_psc)))
+
+/* Init PSC FIFO space for TX and RX slices */
+void __init mpc512x_psc_fifo_init(void)
+{
+        struct device_node *np;
+        void __iomem *psc;
+        unsigned int tx_fifo_size;
+        unsigned int rx_fifo_size;
+        int fifobase = 0; /* current fifo address in 32 bit words */
+
+        for_each_compatible_node(np, NULL, "fsl,mpc5121-psc") {
+                tx_fifo_size = get_fifo_size(np, "fsl,tx-fifo-size");
+                rx_fifo_size = get_fifo_size(np, "fsl,rx-fifo-size");
+
+                /* size in register is in 4 byte units */
+                tx_fifo_size /= 4;
+                rx_fifo_size /= 4;
+                if (!tx_fifo_size)
+                        tx_fifo_size = 1;
+                if (!rx_fifo_size)
+                        rx_fifo_size = 1;
+
+                psc = of_iomap(np, 0);
+                if (!psc) {
+                        pr_err("%s: Can't map %s device\n",
+                                __func__, np->full_name);
+                        continue;
+                }
+
+                /* FIFO space is 4KiB, check if requested size is available */
+                if ((fifobase + tx_fifo_size + rx_fifo_size) > 0x1000) {
+                        pr_err("%s: no fifo space available for %s\n",
+                                __func__, np->full_name);
+                        iounmap(psc);
+                        /*
+                         * chances are that another device requests less
+                         * fifo space, so we continue.
+                         */
+                        continue;
+                }
+
+                /* set tx and rx fifo size registers */
+                out_be32(&FIFOC(psc)->txsz, (fifobase << 16) | tx_fifo_size);
+                fifobase += tx_fifo_size;
+                out_be32(&FIFOC(psc)->rxsz, (fifobase << 16) | rx_fifo_size);
+                fifobase += rx_fifo_size;
+
+                /* reset and enable the slices */
+                out_be32(&FIFOC(psc)->txcmd, 0x80);
+                out_be32(&FIFOC(psc)->txcmd, 0x01);
+                out_be32(&FIFOC(psc)->rxcmd, 0x80);
+                out_be32(&FIFOC(psc)->rxcmd, 0x01);
+
+                iounmap(psc);
+        }
+}
+
 void __init mpc512x_init(void)
 {
         mpc512x_declare_of_platform_devices();
         mpc5121_clk_init();
         mpc512x_restart_init();
+        mpc512x_psc_fifo_init();
 }
diff --git a/arch/powerpc/platforms/cell/spufs/file.c b/arch/powerpc/platforms/cell/spufs/file.c
index 5c2808252516..1a40da92154c 100644
--- a/arch/powerpc/platforms/cell/spufs/file.c
+++ b/arch/powerpc/platforms/cell/spufs/file.c
@@ -1849,8 +1849,7 @@ out:
         return ret;
 }
 
-static int spufs_mfc_fsync(struct file *file, struct dentry *dentry,
-                           int datasync)
+static int spufs_mfc_fsync(struct file *file, int datasync)
 {
         return spufs_mfc_flush(file, NULL);
 }
diff --git a/arch/powerpc/platforms/cell/spufs/inode.c b/arch/powerpc/platforms/cell/spufs/inode.c
index fc1b1c42b1dc..e5e5f823d687 100644
--- a/arch/powerpc/platforms/cell/spufs/inode.c
+++ b/arch/powerpc/platforms/cell/spufs/inode.c
@@ -251,7 +251,7 @@ const struct file_operations spufs_context_fops = {
         .llseek         = dcache_dir_lseek,
         .read           = generic_read_dir,
         .readdir        = dcache_readdir,
-        .fsync          = simple_sync_file,
+        .fsync          = noop_fsync,
 };
 EXPORT_SYMBOL_GPL(spufs_context_fops);
 
diff --git a/arch/powerpc/platforms/pseries/hvCall_inst.c b/arch/powerpc/platforms/pseries/hvCall_inst.c
index 1fefae76e295..e19ff021e711 100644
--- a/arch/powerpc/platforms/pseries/hvCall_inst.c
+++ b/arch/powerpc/platforms/pseries/hvCall_inst.c
@@ -102,7 +102,7 @@ static const struct file_operations hcall_inst_seq_fops = {
 #define CPU_NAME_BUF_SIZE       32
 
 
-static void probe_hcall_entry(unsigned long opcode, unsigned long *args)
+static void probe_hcall_entry(void *ignored, unsigned long opcode, unsigned long *args)
 {
         struct hcall_stats *h;
 
@@ -114,7 +114,7 @@ static void probe_hcall_entry(unsigned long opcode, unsigned long *args)
         h->purr_start = mfspr(SPRN_PURR);
 }
 
-static void probe_hcall_exit(unsigned long opcode, unsigned long retval,
+static void probe_hcall_exit(void *ignored, unsigned long opcode, unsigned long retval,
                              unsigned long *retbuf)
 {
         struct hcall_stats *h;
@@ -140,11 +140,11 @@ static int __init hcall_inst_init(void)
         if (!firmware_has_feature(FW_FEATURE_LPAR))
                 return 0;
 
-        if (register_trace_hcall_entry(probe_hcall_entry))
+        if (register_trace_hcall_entry(probe_hcall_entry, NULL))
                 return -EINVAL;
 
-        if (register_trace_hcall_exit(probe_hcall_exit)) {
-                unregister_trace_hcall_entry(probe_hcall_entry);
+        if (register_trace_hcall_exit(probe_hcall_exit, NULL)) {
+                unregister_trace_hcall_entry(probe_hcall_entry, NULL);
                 return -EINVAL;
         }
 
diff --git a/arch/powerpc/sysdev/fsl_rio.c b/arch/powerpc/sysdev/fsl_rio.c
index 6a1fde0d22b0..cd37e49e7034 100644
--- a/arch/powerpc/sysdev/fsl_rio.c
+++ b/arch/powerpc/sysdev/fsl_rio.c
@@ -1,6 +1,15 @@
 /*
  * Freescale MPC85xx/MPC86xx RapidIO support
  *
+ * Copyright 2009 Sysgo AG
+ * Thomas Moll <thomas.moll@sysgo.com>
+ * - fixed maintenance access routines, check for aligned access
+ *
+ * Copyright 2009 Integrated Device Technology, Inc.
+ * Alex Bounine <alexandre.bounine@idt.com>
+ * - Added Port-Write message handling
+ * - Added Machine Check exception handling
+ *
  * Copyright (C) 2007, 2008 Freescale Semiconductor, Inc.
  * Zhang Wei <wei.zhang@freescale.com>
  *
@@ -24,19 +33,30 @@
 #include <linux/of_platform.h>
 #include <linux/delay.h>
 #include <linux/slab.h>
+#include <linux/kfifo.h>
 
 #include <asm/io.h>
+#include <asm/machdep.h>
+#include <asm/uaccess.h>
+
+#undef DEBUG_PW /* Port-Write debugging */
 
 /* RapidIO definition irq, which read from OF-tree */
 #define IRQ_RIO_BELL(m)         (((struct rio_priv *)(m->priv))->bellirq)
 #define IRQ_RIO_TX(m)           (((struct rio_priv *)(m->priv))->txirq)
 #define IRQ_RIO_RX(m)           (((struct rio_priv *)(m->priv))->rxirq)
+#define IRQ_RIO_PW(m)           (((struct rio_priv *)(m->priv))->pwirq)
 
 #define RIO_ATMU_REGS_OFFSET    0x10c00
 #define RIO_P_MSG_REGS_OFFSET   0x11000
 #define RIO_S_MSG_REGS_OFFSET   0x13000
 #define RIO_ESCSR               0x158
 #define RIO_CCSR                0x15c
+#define RIO_LTLEDCSR            0x0608
+#define  RIO_LTLEDCSR_IER       0x80000000
+#define  RIO_LTLEDCSR_PRT       0x01000000
+#define RIO_LTLEECSR            0x060c
+#define RIO_EPWISR              0x10010
 #define RIO_ISR_AACR            0x10120
 #define RIO_ISR_AACR_AA         0x1     /* Accept All ID */
 #define RIO_MAINT_WIN_SIZE      0x400000
@@ -55,6 +75,18 @@
 #define RIO_MSG_ISR_QFI         0x00000010
 #define RIO_MSG_ISR_DIQI        0x00000001
 
+#define RIO_IPWMR_SEN           0x00100000
+#define RIO_IPWMR_QFIE          0x00000100
+#define RIO_IPWMR_EIE           0x00000020
+#define RIO_IPWMR_CQ            0x00000002
+#define RIO_IPWMR_PWE           0x00000001
+
+#define RIO_IPWSR_QF            0x00100000
+#define RIO_IPWSR_TE            0x00000080
+#define RIO_IPWSR_QFI           0x00000010
+#define RIO_IPWSR_PWD           0x00000008
+#define RIO_IPWSR_PWB           0x00000004
+
 #define RIO_MSG_DESC_SIZE       32
 #define RIO_MSG_BUFFER_SIZE     4096
 #define RIO_MIN_TX_RING_SIZE    2
@@ -121,7 +153,7 @@ struct rio_msg_regs {
         u32 pad10[26];
         u32 pwmr;
         u32 pwsr;
-        u32 pad11;
+        u32 epwqbar;
         u32 pwqbar;
 };
 
@@ -160,6 +192,14 @@ struct rio_msg_rx_ring {
         void *dev_id;
 };
 
+struct rio_port_write_msg {
+        void *virt;
+        dma_addr_t phys;
+        u32 msg_count;
+        u32 err_count;
+        u32 discard_count;
+};
+
 struct rio_priv {
         struct device *dev;
         void __iomem *regs_win;
@@ -172,11 +212,64 @@ struct rio_priv {
         struct rio_dbell_ring dbell_ring;
         struct rio_msg_tx_ring msg_tx_ring;
         struct rio_msg_rx_ring msg_rx_ring;
+        struct rio_port_write_msg port_write_msg;
         int bellirq;
         int txirq;
         int rxirq;
+        int pwirq;
+        struct work_struct pw_work;
+        struct kfifo pw_fifo;
+        spinlock_t pw_fifo_lock;
 };
 
+#define __fsl_read_rio_config(x, addr, err, op)         \
+        __asm__ __volatile__(                           \
+                "1:     "op" %1,0(%2)\n"                \
+                "       eieio\n"                        \
+                "2:\n"                                  \
+                ".section .fixup,\"ax\"\n"              \
+                "3:     li %1,-1\n"                     \
+                "       li %0,%3\n"                     \
+                "       b 2b\n"                         \
+                ".section __ex_table,\"a\"\n"           \
+                "       .align 2\n"                     \
+                "       .long 1b,3b\n"                  \
+                ".text"                                 \
+                : "=r" (err), "=r" (x)                  \
+                : "b" (addr), "i" (-EFAULT), "0" (err))
+
+static void __iomem *rio_regs_win;
+
+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);
+
+        if (reason & MCSR_BUS_RBERR) {
+                reason = in_be32((u32 *)(rio_regs_win + RIO_LTLEDCSR));
+                if (reason & (RIO_LTLEDCSR_IER | RIO_LTLEDCSR_PRT)) {
+                        /* Check if we are prepared to handle this fault */
+                        entry = search_exception_tables(regs->nip);
+                        if (entry) {
+                                pr_debug("RIO: %s - MC Exception handled\n",
+                                         __func__);
+                                out_be32((u32 *)(rio_regs_win + RIO_LTLEDCSR),
+                                         0);
+                                regs->msr |= MSR_RI;
+                                regs->nip = entry->fixup;
+                                return 1;
+                        }
+                }
+        }
+
+        if (saved_mcheck_exception)
+                return saved_mcheck_exception(regs);
+        else
+                return cur_cpu_spec->machine_check(regs);
+}
+
 /**
  * fsl_rio_doorbell_send - Send a MPC85xx doorbell message
  * @mport: RapidIO master port info
@@ -277,27 +370,44 @@ fsl_rio_config_read(struct rio_mport *mport, int index, u16 destid,
 {
         struct rio_priv *priv = mport->priv;
         u8 *data;
+        u32 rval, err = 0;
 
         pr_debug
             ("fsl_rio_config_read: index %d destid %d hopcount %d offset %8.8x len %d\n",
              index, destid, hopcount, offset, len);
+
+        /* 16MB maintenance window possible */
+        /* allow only aligned access to maintenance registers */
+        if (offset > (0x1000000 - len) || !IS_ALIGNED(offset, len))
+                return -EINVAL;
+
         out_be32(&priv->maint_atmu_regs->rowtar,
-                 (destid << 22) | (hopcount << 12) | ((offset & ~0x3) >> 9));
+                 (destid << 22) | (hopcount << 12) | (offset >> 12));
+        out_be32(&priv->maint_atmu_regs->rowtear,  (destid >> 10));
 
-        data = (u8 *) priv->maint_win + offset;
+        data = (u8 *) priv->maint_win + (offset & (RIO_MAINT_WIN_SIZE - 1));
         switch (len) {
         case 1:
-                *val = in_8((u8 *) data);
+                __fsl_read_rio_config(rval, data, err, "lbz");
                 break;
         case 2:
-                *val = in_be16((u16 *) data);
+                __fsl_read_rio_config(rval, data, err, "lhz");
                 break;
-        default:
-                *val = in_be32((u32 *) data);
+        case 4:
+                __fsl_read_rio_config(rval, data, err, "lwz");
                 break;
+        default:
+                return -EINVAL;
         }
 
-        return 0;
+        if (err) {
+                pr_debug("RIO: cfg_read error %d for %x:%x:%x\n",
+                         err, destid, hopcount, offset);
+        }
+
+        *val = rval;
+
+        return err;
 }
 
 /**
@@ -322,10 +432,17 @@ fsl_rio_config_write(struct rio_mport *mport, int index, u16 destid,
         pr_debug
             ("fsl_rio_config_write: index %d destid %d hopcount %d offset %8.8x len %d val %8.8x\n",
              index, destid, hopcount, offset, len, val);
+
+        /* 16MB maintenance windows possible */
+        /* allow only aligned access to maintenance registers */
+        if (offset > (0x1000000 - len) || !IS_ALIGNED(offset, len))
+                return -EINVAL;
+
         out_be32(&priv->maint_atmu_regs->rowtar,
-                 (destid << 22) | (hopcount << 12) | ((offset & ~0x3) >> 9));
+                 (destid << 22) | (hopcount << 12) | (offset >> 12));
+        out_be32(&priv->maint_atmu_regs->rowtear,  (destid >> 10));
 
-        data = (u8 *) priv->maint_win + offset;
+        data = (u8 *) priv->maint_win + (offset & (RIO_MAINT_WIN_SIZE - 1));
         switch (len) {
         case 1:
                 out_8((u8 *) data, val);
@@ -333,9 +450,11 @@ fsl_rio_config_write(struct rio_mport *mport, int index, u16 destid,
         case 2:
                 out_be16((u16 *) data, val);
                 break;
-        default:
+        case 4:
                 out_be32((u32 *) data, val);
                 break;
+        default:
+                return -EINVAL;
         }
 
         return 0;
@@ -930,6 +1049,223 @@ static int fsl_rio_doorbell_init(struct rio_mport *mport)
         return rc;
 }
 
+/**
+ * fsl_rio_port_write_handler - MPC85xx port write interrupt handler
+ * @irq: Linux interrupt number
+ * @dev_instance: Pointer to interrupt-specific data
+ *
+ * Handles port write interrupts. Parses a list of registered
+ * port write event handlers and executes a matching event handler.
+ */
+static irqreturn_t
+fsl_rio_port_write_handler(int irq, void *dev_instance)
+{
+        u32 ipwmr, ipwsr;
+        struct rio_mport *port = (struct rio_mport *)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 & 0x00000001))
+                return IRQ_HANDLED;
+
+#ifdef DEBUG_PW
+        pr_debug("PW Int->IPWMR: 0x%08x IPWSR: 0x%08x (", ipwmr, ipwsr);
+        if (ipwsr & RIO_IPWSR_QF)
+                pr_debug(" QF");
+        if (ipwsr & RIO_IPWSR_TE)
+                pr_debug(" TE");
+        if (ipwsr & RIO_IPWSR_QFI)
+                pr_debug(" QFI");
+        if (ipwsr & RIO_IPWSR_PWD)
+                pr_debug(" PWD");
+        if (ipwsr & RIO_IPWSR_PWB)
+                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),
+                 * otherwise discard it.
+                 */
+                if (kfifo_avail(&priv->pw_fifo) >= RIO_PW_MSG_SIZE) {
+                        priv->port_write_msg.msg_count++;
+                        kfifo_in(&priv->pw_fifo, priv->port_write_msg.virt,
+                                 RIO_PW_MSG_SIZE);
+                } else {
+                        priv->port_write_msg.discard_count++;
+                        pr_info("RIO: ISR Discarded Port-Write Msg(s) (%d)\n",
+                                 priv->port_write_msg.discard_count);
+                }
+                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);
+
+        return IRQ_HANDLED;
+}
+
+static void fsl_pw_dpc(struct work_struct *work)
+{
+        struct rio_priv *priv = container_of(work, struct rio_priv, pw_work);
+        unsigned long flags;
+        u32 msg_buffer[RIO_PW_MSG_SIZE/sizeof(u32)];
+
+        /*
+         * Process port-write messages
+         */
+        spin_lock_irqsave(&priv->pw_fifo_lock, flags);
+        while (kfifo_out(&priv->pw_fifo, (unsigned char *)msg_buffer,
+                         RIO_PW_MSG_SIZE)) {
+                /* Process one message */
+                spin_unlock_irqrestore(&priv->pw_fifo_lock, flags);
+#ifdef DEBUG_PW
+                {
+                u32 i;
+                pr_debug("%s : Port-Write Message:", __func__);
+                for (i = 0; i < RIO_PW_MSG_SIZE/sizeof(u32); i++) {
+                        if ((i%4) == 0)
+                                pr_debug("\n0x%02x: 0x%08x", i*4,
+                                         msg_buffer[i]);
+                        else
+                                pr_debug(" 0x%08x", msg_buffer[i]);
+                }
+                pr_debug("\n");
+                }
+#endif
+                /* Pass the port-write message to RIO core for processing */
+                rio_inb_pwrite_handler((union rio_pw_msg *)msg_buffer);
+                spin_lock_irqsave(&priv->pw_fifo_lock, flags);
+        }
+        spin_unlock_irqrestore(&priv->pw_fifo_lock, flags);
+}
+
+/**
+ * fsl_rio_pw_enable - enable/disable port-write interface init
+ * @mport: Master port implementing the port write unit
+ * @enable:    1=enable; 0=disable port-write message handling
+ */
+static int fsl_rio_pw_enable(struct rio_mport *mport, int enable)
+{
+        struct rio_priv *priv = mport->priv;
+        u32 rval;
+
+        rval = in_be32(&priv->msg_regs->pwmr);
+
+        if (enable)
+                rval |= RIO_IPWMR_PWE;
+        else
+                rval &= ~RIO_IPWMR_PWE;
+
+        out_be32(&priv->msg_regs->pwmr, rval);
+
+        return 0;
+}
+
+/**
+ * fsl_rio_port_write_init - MPC85xx port write interface init
+ * @mport: Master port implementing the port write unit
+ *
+ * Initializes port write unit hardware and DMA buffer
+ * ring. Called from fsl_rio_setup(). Returns %0 on success
+ * or %-ENOMEM on failure.
+ */
+static int fsl_rio_port_write_init(struct rio_mport *mport)
+{
+        struct rio_priv *priv = mport->priv;
+        int rc = 0;
+
+        /* Following configurations require a disabled port write controller */
+        out_be32(&priv->msg_regs->pwmr,
+                 in_be32(&priv->msg_regs->pwmr) & ~RIO_IPWMR_PWE);
+
+        /* Initialize port write */
+        priv->port_write_msg.virt = dma_alloc_coherent(priv->dev,
+                                        RIO_PW_MSG_SIZE,
+                                        &priv->port_write_msg.phys, GFP_KERNEL);
+        if (!priv->port_write_msg.virt) {
+                pr_err("RIO: unable allocate port write queue\n");
+                return -ENOMEM;
+        }
+
+        priv->port_write_msg.err_count = 0;
+        priv->port_write_msg.discard_count = 0;
+
+        /* Point dequeue/enqueue pointers at first entry */
+        out_be32(&priv->msg_regs->epwqbar, 0);
+        out_be32(&priv->msg_regs->pwqbar, (u32) priv->port_write_msg.phys);
+
+        pr_debug("EIPWQBAR: 0x%08x IPWQBAR: 0x%08x\n",
+                 in_be32(&priv->msg_regs->epwqbar),
+                 in_be32(&priv->msg_regs->pwqbar));
+
+        /* Clear interrupt status IPWSR */
+        out_be32(&priv->msg_regs->pwsr,
+                 (RIO_IPWSR_TE | RIO_IPWSR_QFI | RIO_IPWSR_PWD));
+
+        /* Configure port write contoller for snooping enable all reporting,
+           clear queue full */
+        out_be32(&priv->msg_regs->pwmr,
+                 RIO_IPWMR_SEN | RIO_IPWMR_QFIE | RIO_IPWMR_EIE | RIO_IPWMR_CQ);
+
+
+        /* Hook up port-write handler */
+        rc = request_irq(IRQ_RIO_PW(mport), fsl_rio_port_write_handler, 0,
+                         "port-write", (void *)mport);
+        if (rc < 0) {
+                pr_err("MPC85xx RIO: unable to request inbound doorbell irq");
+                goto err_out;
+        }
+
+        INIT_WORK(&priv->pw_work, fsl_pw_dpc);
+        spin_lock_init(&priv->pw_fifo_lock);
+        if (kfifo_alloc(&priv->pw_fifo, RIO_PW_MSG_SIZE * 32, GFP_KERNEL)) {
+                pr_err("FIFO allocation failed\n");
+                rc = -ENOMEM;
+                goto err_out_irq;
+        }
+
+        pr_debug("IPWMR: 0x%08x IPWSR: 0x%08x\n",
+                 in_be32(&priv->msg_regs->pwmr),
+                 in_be32(&priv->msg_regs->pwsr));
+
+        return rc;
+
+err_out_irq:
+        free_irq(IRQ_RIO_PW(mport), (void *)mport);
+err_out:
+        dma_free_coherent(priv->dev, RIO_PW_MSG_SIZE,
+                          priv->port_write_msg.virt,
+                          priv->port_write_msg.phys);
+        return rc;
+}
+
 static char *cmdline = NULL;
 
 static int fsl_rio_get_hdid(int index)
@@ -1057,7 +1393,7 @@ int fsl_rio_setup(struct of_device *dev)
         dev_info(&dev->dev, "LAW start 0x%016llx, size 0x%016llx.\n",
                         law_start, law_size);
 
-        ops = kmalloc(sizeof(struct rio_ops), GFP_KERNEL);
+        ops = kzalloc(sizeof(struct rio_ops), GFP_KERNEL);
         if (!ops) {
                 rc = -ENOMEM;
                 goto err_ops;
@@ -1067,6 +1403,7 @@ int fsl_rio_setup(struct of_device *dev)
         ops->cread = fsl_rio_config_read;
         ops->cwrite = fsl_rio_config_write;
         ops->dsend = fsl_rio_doorbell_send;
+        ops->pwenable = fsl_rio_pw_enable;
 
         port = kzalloc(sizeof(struct rio_mport), GFP_KERNEL);
         if (!port) {
@@ -1089,11 +1426,12 @@ int fsl_rio_setup(struct of_device *dev)
         port->iores.flags = IORESOURCE_MEM;
         port->iores.name = "rio_io_win";
 
+        priv->pwirq   = irq_of_parse_and_map(dev->node, 0);
         priv->bellirq = irq_of_parse_and_map(dev->dev.of_node, 2);
         priv->txirq = irq_of_parse_and_map(dev->dev.of_node, 3);
         priv->rxirq = irq_of_parse_and_map(dev->dev.of_node, 4);
-        dev_info(&dev->dev, "bellirq: %d, txirq: %d, rxirq %d\n", priv->bellirq,
-                                priv->txirq, priv->rxirq);
+        dev_info(&dev->dev, "pwirq: %d, bellirq: %d, txirq: %d, rxirq %d\n",
+                 priv->pwirq, priv->bellirq, priv->txirq, priv->rxirq);
 
         rio_init_dbell_res(&port->riores[RIO_DOORBELL_RESOURCE], 0, 0xffff);
         rio_init_mbox_res(&port->riores[RIO_INB_MBOX_RESOURCE], 0, 0);
@@ -1109,6 +1447,7 @@ int fsl_rio_setup(struct of_device *dev)
         rio_register_mport(port);
 
         priv->regs_win = ioremap(regs.start, regs.end - regs.start + 1);
+        rio_regs_win = priv->regs_win;
 
         /* Probe the master port phy type */
         ccsr = in_be32(priv->regs_win + RIO_CCSR);
@@ -1166,7 +1505,8 @@ int fsl_rio_setup(struct of_device *dev)
 
         /* Configure maintenance transaction window */
         out_be32(&priv->maint_atmu_regs->rowbar, law_start >> 12);
-        out_be32(&priv->maint_atmu_regs->rowar, 0x80077015);    /* 4M */
+        out_be32(&priv->maint_atmu_regs->rowar,
+                 0x80077000 | (ilog2(RIO_MAINT_WIN_SIZE) - 1));
 
         priv->maint_win = ioremap(law_start, RIO_MAINT_WIN_SIZE);
 
@@ -1175,6 +1515,12 @@ int fsl_rio_setup(struct of_device *dev)
                         (law_start + RIO_MAINT_WIN_SIZE) >> 12);
         out_be32(&priv->dbell_atmu_regs->rowar, 0x8004200b);    /* 4k */
         fsl_rio_doorbell_init(port);
+        fsl_rio_port_write_init(port);
+
+        saved_mcheck_exception = ppc_md.machine_check_exception;
+        ppc_md.machine_check_exception = fsl_rio_mcheck_exception;
+        /* Ensure that RFXE is set */
+        mtspr(SPRN_HID1, (mfspr(SPRN_HID1) | 0x20000));
 
         return 0;
 err: