Merge branch 'mpi-master' into wip-k-fmlpwip-k-fmlp

Conflicts:
	litmus/sched_cedf.c

1 files changed, 225 insertions, 155 deletions

diff --git a/arch/powerpc/sysdev/fsl_rio.c b/arch/powerpc/sysdev/fsl_rio.c
index 3017532319c8..b3fd081d56f5 100644
--- a/arch/powerpc/sysdev/fsl_rio.c
+++ b/arch/powerpc/sysdev/fsl_rio.c
@@ -10,7 +10,7 @@
  * - Added Port-Write message handling
  * - Added Machine Check exception handling
  *
- * Copyright (C) 2007, 2008 Freescale Semiconductor, Inc.
+ * Copyright (C) 2007, 2008, 2010 Freescale Semiconductor, Inc.
  * Zhang Wei <wei.zhang@freescale.com>
  *
  * Copyright 2005 MontaVista Software, Inc.
@@ -47,14 +47,33 @@
 #define IRQ_RIO_RX(m)           (((struct rio_priv *)(m->priv))->rxirq)
 #define IRQ_RIO_PW(m)           (((struct rio_priv *)(m->priv))->pwirq)
 
+#define IPWSR_CLEAR             0x98
+#define OMSR_CLEAR              0x1cb3
+#define IMSR_CLEAR              0x491
+#define IDSR_CLEAR              0x91
+#define ODSR_CLEAR              0x1c00
+#define LTLEECSR_ENABLE_ALL     0xFFC000FC
+#define ESCSR_CLEAR             0x07120204
+
+#define RIO_PORT1_EDCSR         0x0640
+#define RIO_PORT2_EDCSR         0x0680
+#define RIO_PORT1_IECSR         0x10130
+#define RIO_PORT2_IECSR         0x101B0
+#define RIO_IM0SR               0x13064
+#define RIO_IM1SR               0x13164
+#define RIO_OM0SR               0x13004
+#define RIO_OM1SR               0x13104
+
 #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_PORT2_ESCSR         0x178
 #define RIO_CCSR                0x15c
 #define RIO_LTLEDCSR            0x0608
-#define  RIO_LTLEDCSR_IER       0x80000000
-#define  RIO_LTLEDCSR_PRT       0x01000000
+#define RIO_LTLEDCSR_IER        0x80000000
+#define RIO_LTLEDCSR_PRT        0x01000000
 #define RIO_LTLEECSR            0x060c
 #define RIO_EPWISR              0x10010
 #define RIO_ISR_AACR            0x10120
@@ -87,6 +106,12 @@
 #define RIO_IPWSR_PWD           0x00000008
 #define RIO_IPWSR_PWB           0x00000004
 
+/* EPWISR Error match value */
+#define RIO_EPWISR_PINT1        0x80000000
+#define RIO_EPWISR_PINT2        0x40000000
+#define RIO_EPWISR_MU           0x00000002
+#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 +142,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 {
@@ -241,35 +281,32 @@ 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)
+int fsl_rio_mcheck_exception(struct pt_regs *regs)
 {
-        const struct exception_table_entry *entry = NULL;
-        unsigned long reason = mfspr(SPRN_MCSR);
-
-        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;
-                        }
+        const struct exception_table_entry *entry;
+        unsigned long reason;
+
+        if (!rio_regs_win)
+                return 0;
+
+        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);
+        return 0;
 }
+EXPORT_SYMBOL_GPL(fsl_rio_mcheck_exception);
 #endif
 
 /**
@@ -463,7 +500,7 @@ fsl_rio_config_write(struct rio_mport *mport, int index, u16 destid,
 }
 
 /**
- * rio_hw_add_outb_message - Add message to the MPC85xx outbound message queue
+ * fsl_add_outb_message - Add message to the MPC85xx outbound message queue
  * @mport: Master port with outbound message queue
  * @rdev: Target of outbound message
  * @mbox: Outbound mailbox
@@ -473,8 +510,8 @@ fsl_rio_config_write(struct rio_mport *mport, int index, u16 destid,
  * Adds the @buffer message to the MPC85xx outbound message queue. Returns
  * %0 on success or %-EINVAL on failure.
  */
-int
-rio_hw_add_outb_message(struct rio_mport *mport, struct rio_dev *rdev, int mbox,
+static int
+fsl_add_outb_message(struct rio_mport *mport, struct rio_dev *rdev, int mbox,
                         void *buffer, size_t len)
 {
         struct rio_priv *priv = mport->priv;
@@ -483,9 +520,8 @@ rio_hw_add_outb_message(struct rio_mport *mport, struct rio_dev *rdev, int mbox,
                                         + priv->msg_tx_ring.tx_slot;
         int ret = 0;
 
-        pr_debug
-            ("RIO: rio_hw_add_outb_message(): destid %4.4x mbox %d buffer %8.8x len %8.8x\n",
-             rdev->destid, mbox, (int)buffer, len);
+        pr_debug("RIO: fsl_add_outb_message(): destid %4.4x mbox %d buffer " \
+                 "%8.8x len %8.8x\n", rdev->destid, mbox, (int)buffer, len);
 
         if ((len < 8) || (len > RIO_MAX_MSG_SIZE)) {
                 ret = -EINVAL;
@@ -535,8 +571,6 @@ rio_hw_add_outb_message(struct rio_mport *mport, struct rio_dev *rdev, int mbox,
         return ret;
 }
 
-EXPORT_SYMBOL_GPL(rio_hw_add_outb_message);
-
 /**
  * fsl_rio_tx_handler - MPC85xx outbound message interrupt handler
  * @irq: Linux interrupt number
@@ -581,7 +615,7 @@ fsl_rio_tx_handler(int irq, void *dev_instance)
 }
 
 /**
- * rio_open_outb_mbox - Initialize MPC85xx outbound mailbox
+ * fsl_open_outb_mbox - Initialize MPC85xx outbound mailbox
  * @mport: Master port implementing the outbound message unit
  * @dev_id: Device specific pointer to pass on event
  * @mbox: Mailbox to open
@@ -591,7 +625,8 @@ fsl_rio_tx_handler(int irq, void *dev_instance)
  * and enables the outbound message unit. Returns %0 on success and
  * %-EINVAL or %-ENOMEM on failure.
  */
-int rio_open_outb_mbox(struct rio_mport *mport, void *dev_id, int mbox, int entries)
+static int
+fsl_open_outb_mbox(struct rio_mport *mport, void *dev_id, int mbox, int entries)
 {
         int i, j, rc = 0;
         struct rio_priv *priv = mport->priv;
@@ -687,14 +722,14 @@ int rio_open_outb_mbox(struct rio_mport *mport, void *dev_id, int mbox, int entr
 }
 
 /**
- * rio_close_outb_mbox - Shut down MPC85xx outbound mailbox
+ * fsl_close_outb_mbox - Shut down MPC85xx outbound mailbox
  * @mport: Master port implementing the outbound message unit
  * @mbox: Mailbox to close
  *
  * Disables the outbound message unit, free all buffers, and
  * frees the outbound message interrupt.
  */
-void rio_close_outb_mbox(struct rio_mport *mport, int mbox)
+static void fsl_close_outb_mbox(struct rio_mport *mport, int mbox)
 {
         struct rio_priv *priv = mport->priv;
         /* Disable inbound message unit */
@@ -751,7 +786,7 @@ fsl_rio_rx_handler(int irq, void *dev_instance)
 }
 
 /**
- * rio_open_inb_mbox - Initialize MPC85xx inbound mailbox
+ * fsl_open_inb_mbox - Initialize MPC85xx inbound mailbox
  * @mport: Master port implementing the inbound message unit
  * @dev_id: Device specific pointer to pass on event
  * @mbox: Mailbox to open
@@ -761,7 +796,8 @@ fsl_rio_rx_handler(int irq, void *dev_instance)
  * and enables the inbound message unit. Returns %0 on success
  * and %-EINVAL or %-ENOMEM on failure.
  */
-int rio_open_inb_mbox(struct rio_mport *mport, void *dev_id, int mbox, int entries)
+static int
+fsl_open_inb_mbox(struct rio_mport *mport, void *dev_id, int mbox, int entries)
 {
         int i, rc = 0;
         struct rio_priv *priv = mport->priv;
@@ -825,14 +861,14 @@ int rio_open_inb_mbox(struct rio_mport *mport, void *dev_id, int mbox, int entri
 }
 
 /**
- * rio_close_inb_mbox - Shut down MPC85xx inbound mailbox
+ * fsl_close_inb_mbox - Shut down MPC85xx inbound mailbox
  * @mport: Master port implementing the inbound message unit
  * @mbox: Mailbox to close
  *
  * Disables the inbound message unit, free all buffers, and
  * frees the inbound message interrupt.
  */
-void rio_close_inb_mbox(struct rio_mport *mport, int mbox)
+static void fsl_close_inb_mbox(struct rio_mport *mport, int mbox)
 {
         struct rio_priv *priv = mport->priv;
         /* Disable inbound message unit */
@@ -847,7 +883,7 @@ void rio_close_inb_mbox(struct rio_mport *mport, int mbox)
 }
 
 /**
- * rio_hw_add_inb_buffer - Add buffer to the MPC85xx inbound message queue
+ * fsl_add_inb_buffer - Add buffer to the MPC85xx inbound message queue
  * @mport: Master port implementing the inbound message unit
  * @mbox: Inbound mailbox number
  * @buf: Buffer to add to inbound queue
@@ -855,12 +891,12 @@ void rio_close_inb_mbox(struct rio_mport *mport, int mbox)
  * Adds the @buf buffer to the MPC85xx inbound message queue. Returns
  * %0 on success or %-EINVAL on failure.
  */
-int rio_hw_add_inb_buffer(struct rio_mport *mport, int mbox, void *buf)
+static int fsl_add_inb_buffer(struct rio_mport *mport, int mbox, void *buf)
 {
         int rc = 0;
         struct rio_priv *priv = mport->priv;
 
-        pr_debug("RIO: rio_hw_add_inb_buffer(), msg_rx_ring.rx_slot %d\n",
+        pr_debug("RIO: fsl_add_inb_buffer(), msg_rx_ring.rx_slot %d\n",
                  priv->msg_rx_ring.rx_slot);
 
         if (priv->msg_rx_ring.virt_buffer[priv->msg_rx_ring.rx_slot]) {
@@ -879,17 +915,15 @@ int rio_hw_add_inb_buffer(struct rio_mport *mport, int mbox, void *buf)
         return rc;
 }
 
-EXPORT_SYMBOL_GPL(rio_hw_add_inb_buffer);
-
 /**
- * rio_hw_get_inb_message - Fetch inbound message from the MPC85xx message unit
+ * fsl_get_inb_message - Fetch inbound message from the MPC85xx message unit
  * @mport: Master port implementing the inbound message unit
  * @mbox: Inbound mailbox number
  *
  * Gets the next available inbound message from the inbound message queue.
  * A pointer to the message is returned on success or NULL on failure.
  */
-void *rio_hw_get_inb_message(struct rio_mport *mport, int mbox)
+static void *fsl_get_inb_message(struct rio_mport *mport, int mbox)
 {
         struct rio_priv *priv = mport->priv;
         u32 phys_buf, virt_buf;
@@ -926,8 +960,6 @@ void *rio_hw_get_inb_message(struct rio_mport *mport, int mbox)
         return buf;
 }
 
-EXPORT_SYMBOL_GPL(rio_hw_get_inb_message);
-
 /**
  * fsl_rio_dbell_handler - MPC85xx doorbell interrupt handler
  * @irq: Linux interrupt number
@@ -954,7 +986,6 @@ fsl_rio_dbell_handler(int irq, void *dev_instance)
         if (dsr & DOORBELL_DSR_QFI) {
                 pr_info("RIO: doorbell queue full\n");
                 out_be32(&priv->msg_regs->dsr, DOORBELL_DSR_QFI);
-                goto out;
         }
 
         /* XXX Need to check/dispatch until queue empty */
@@ -1051,6 +1082,40 @@ static int fsl_rio_doorbell_init(struct rio_mport *mport)
         return rc;
 }
 
+static void port_error_handler(struct rio_mport *port, int offset)
+{
+        /*XXX: Error recovery is not implemented, we just clear errors */
+        out_be32((u32 *)(rio_regs_win + RIO_LTLEDCSR), 0);
+
+        if (offset == 0) {
+                out_be32((u32 *)(rio_regs_win + RIO_PORT1_EDCSR), 0);
+                out_be32((u32 *)(rio_regs_win + RIO_PORT1_IECSR), 0);
+                out_be32((u32 *)(rio_regs_win + RIO_ESCSR), ESCSR_CLEAR);
+        } else {
+                out_be32((u32 *)(rio_regs_win + RIO_PORT2_EDCSR), 0);
+                out_be32((u32 *)(rio_regs_win + RIO_PORT2_IECSR), 0);
+                out_be32((u32 *)(rio_regs_win + RIO_PORT2_ESCSR), ESCSR_CLEAR);
+        }
+}
+
+static void msg_unit_error_handler(struct rio_mport *port)
+{
+        struct rio_priv *priv = port->priv;
+
+        /*XXX: Error recovery is not implemented, we just clear errors */
+        out_be32((u32 *)(rio_regs_win + RIO_LTLEDCSR), 0);
+
+        out_be32((u32 *)(rio_regs_win + RIO_IM0SR), IMSR_CLEAR);
+        out_be32((u32 *)(rio_regs_win + RIO_IM1SR), IMSR_CLEAR);
+        out_be32((u32 *)(rio_regs_win + RIO_OM0SR), OMSR_CLEAR);
+        out_be32((u32 *)(rio_regs_win + RIO_OM1SR), OMSR_CLEAR);
+
+        out_be32(&priv->msg_regs->odsr, ODSR_CLEAR);
+        out_be32(&priv->msg_regs->dsr, IDSR_CLEAR);
+
+        out_be32(&priv->msg_regs->pwsr, IPWSR_CLEAR);
+}
+
 /**
  * fsl_rio_port_write_handler - MPC85xx port write interrupt handler
  * @irq: Linux interrupt number
@@ -1067,18 +1132,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);
@@ -1094,20 +1153,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),
@@ -1119,16 +1164,55 @@ 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_PINT1) {
+                tmp = in_be32(priv->regs_win + RIO_LTLEDCSR);
+                pr_debug("RIO_LTLEDCSR = 0x%x\n", tmp);
+                port_error_handler(port, 0);
+        }
+
+        if (epwisr & RIO_EPWISR_PINT2) {
+                tmp = in_be32(priv->regs_win + RIO_LTLEDCSR);
+                pr_debug("RIO_LTLEDCSR = 0x%x\n", tmp);
+                port_error_handler(port, 1);
+        }
+
+        if (epwisr & RIO_EPWISR_MU) {
+                tmp = in_be32(priv->regs_win + RIO_LTLEDCSR);
+                pr_debug("RIO_LTLEDCSR = 0x%x\n", tmp);
+                msg_unit_error_handler(port);
+        }
 
         return IRQ_HANDLED;
 }
@@ -1238,12 +1322,14 @@ static int fsl_rio_port_write_init(struct rio_mport *mport)
 
 
         /* Hook up port-write handler */
-        rc = request_irq(IRQ_RIO_PW(mport), fsl_rio_port_write_handler, 0,
-                         "port-write", (void *)mport);
+        rc = request_irq(IRQ_RIO_PW(mport), fsl_rio_port_write_handler,
+                        IRQF_SHARED, "port-write", (void *)mport);
         if (rc < 0) {
                 pr_err("MPC85xx RIO: unable to request inbound doorbell irq");
                 goto err_out;
         }
+        /* Enable Error Interrupt */
+        out_be32((u32 *)(rio_regs_win + RIO_LTLEECSR), LTLEECSR_ENABLE_ALL);
 
         INIT_WORK(&priv->pw_work, fsl_pw_dpc);
         spin_lock_init(&priv->pw_fifo_lock);
@@ -1268,28 +1354,6 @@ err_out:
         return rc;
 }
 
-static char *cmdline = NULL;
-
-static int fsl_rio_get_hdid(int index)
-{
-        /* XXX Need to parse multiple entries in some format */
-        if (!cmdline)
-                return -1;
-
-        return simple_strtol(cmdline, NULL, 0);
-}
-
-static int fsl_rio_get_cmdline(char *s)
-{
-        if (!s)
-                return 0;
-
-        cmdline = s;
-        return 1;
-}
-
-__setup("riohdid=", fsl_rio_get_cmdline);
-
 static inline void fsl_rio_info(struct device *dev, u32 ccsr)
 {
         const char *str;
@@ -1406,13 +1470,19 @@ int fsl_rio_setup(struct platform_device *dev)
         ops->cwrite = fsl_rio_config_write;
         ops->dsend = fsl_rio_doorbell_send;
         ops->pwenable = fsl_rio_pw_enable;
+        ops->open_outb_mbox = fsl_open_outb_mbox;
+        ops->open_inb_mbox = fsl_open_inb_mbox;
+        ops->close_outb_mbox = fsl_close_outb_mbox;
+        ops->close_inb_mbox = fsl_close_inb_mbox;
+        ops->add_outb_message = fsl_add_outb_message;
+        ops->add_inb_buffer = fsl_add_inb_buffer;
+        ops->get_inb_message = fsl_get_inb_message;
 
         port = kzalloc(sizeof(struct rio_mport), GFP_KERNEL);
         if (!port) {
                 rc = -ENOMEM;
                 goto err_port;
         }
-        port->id = 0;
         port->index = 0;
 
         priv = kzalloc(sizeof(struct rio_priv), GFP_KERNEL);
@@ -1428,6 +1498,14 @@ int fsl_rio_setup(struct platform_device *dev)
         port->iores.flags = IORESOURCE_MEM;
         port->iores.name = "rio_io_win";
 
+        if (request_resource(&iomem_resource, &port->iores) < 0) {
+                dev_err(&dev->dev, "RIO: Error requesting master port region"
+                        " 0x%016llx-0x%016llx\n",
+                        (u64)port->iores.start, (u64)port->iores.end);
+                        rc = -ENOMEM;
+                        goto err_res;
+        }
+
         priv->pwirq   = irq_of_parse_and_map(dev->dev.of_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);
@@ -1443,10 +1521,8 @@ int fsl_rio_setup(struct platform_device *dev)
         priv->dev = &dev->dev;
 
         port->ops = ops;
-        port->host_deviceid = fsl_rio_get_hdid(port->id);
-
         port->priv = priv;
-        rio_register_mport(port);
+        port->phys_efptr = 0x100;
 
         priv->regs_win = ioremap(regs.start, regs.end - regs.start + 1);
         rio_regs_win = priv->regs_win;
@@ -1493,6 +1569,15 @@ 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 (rio_register_mport(port))
+                goto err;
+
+        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;
@@ -1519,16 +1604,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));
-
         return 0;
 err:
         iounmap(priv->regs_win);
+err_res:
         kfree(priv);
 err_priv:
         kfree(port);
@@ -1540,21 +1619,12 @@ err_ops:
 
 /* The probe function for RapidIO peer-to-peer network.
  */
-static int __devinit fsl_of_rio_rpn_probe(struct platform_device *dev,
-                                     const struct of_device_id *match)
+static int __devinit fsl_of_rio_rpn_probe(struct platform_device *dev)
 {
-        int rc;
         printk(KERN_INFO "Setting up RapidIO peer-to-peer network %s\n",
                         dev->dev.of_node->full_name);
 
-        rc = fsl_rio_setup(dev);
-        if (rc)
-                goto out;
-
-        /* Enumerate all registered ports */
-        rc = rio_init_mports();
-out:
-        return rc;
+        return fsl_rio_setup(dev);
 };
 
 static const struct of_device_id fsl_of_rio_rpn_ids[] = {
@@ -1564,7 +1634,7 @@ static const struct of_device_id fsl_of_rio_rpn_ids[] = {
         {},
 };
 
-static struct of_platform_driver fsl_of_rio_rpn_driver = {
+static struct platform_driver fsl_of_rio_rpn_driver = {
         .driver = {
                 .name = "fsl-of-rio",
                 .owner = THIS_MODULE,
@@ -1575,7 +1645,7 @@ static struct of_platform_driver fsl_of_rio_rpn_driver = {
 
 static __init int fsl_of_rio_rpn_init(void)
 {
-        return of_register_platform_driver(&fsl_of_rio_rpn_driver);
+        return platform_driver_register(&fsl_of_rio_rpn_driver);
 }
 
 subsys_initcall(fsl_of_rio_rpn_init);