1 files changed, 31 insertions, 1 deletions

diff --git a/arch/powerpc/platforms/512x/mpc512x_shared.c b/arch/powerpc/platforms/512x/mpc512x_shared.c
index c7f47cfa9c29..d30235b7e3f7 100644
--- a/arch/powerpc/platforms/512x/mpc512x_shared.c
+++ b/arch/powerpc/platforms/512x/mpc512x_shared.c
@@ -426,8 +426,38 @@ void __init mpc512x_psc_fifo_init(void)
 
 void __init mpc512x_init(void)
 {
-        mpc512x_declare_of_platform_devices();
         mpc5121_clk_init();
+        mpc512x_declare_of_platform_devices();
         mpc512x_restart_init();
         mpc512x_psc_fifo_init();
 }
+
+/**
+ * mpc512x_cs_config - Setup chip select configuration
+ * @cs: chip select number
+ * @val: chip select configuration value
+ *
+ * Perform chip select configuration for devices on LocalPlus Bus.
+ * Intended to dynamically reconfigure the chip select parameters
+ * for configurable devices on the bus.
+ */
+int mpc512x_cs_config(unsigned int cs, u32 val)
+{
+        static struct mpc512x_lpc __iomem *lpc;
+        struct device_node *np;
+
+        if (cs > 7)
+                return -EINVAL;
+
+        if (!lpc) {
+                np = of_find_compatible_node(NULL, NULL, "fsl,mpc5121-lpc");
+                lpc = of_iomap(np, 0);
+                of_node_put(np);
+                if (!lpc)
+                        return -ENOMEM;
+        }
+
+        out_be32(&lpc->cs_cfg[cs], val);
+        return 0;
+}
+EXPORT_SYMBOL(mpc512x_cs_config);