7 files changed, 338 insertions, 2 deletions

diff --git a/arch/powerpc/include/asm/mpc5121.h b/arch/powerpc/include/asm/mpc5121.h
index e6a30bb1d16a..8c0ab2ca689c 100644
--- a/arch/powerpc/include/asm/mpc5121.h
+++ b/arch/powerpc/include/asm/mpc5121.h
@@ -21,4 +21,36 @@ struct mpc512x_reset_module {
         u32     rcer;   /* Reset Control Enable Register */
 };
 
+/*
+ * Clock Control Module
+ */
+struct mpc512x_ccm {
+        u32     spmr;   /* System PLL Mode Register */
+        u32     sccr1;  /* System Clock Control Register 1 */
+        u32     sccr2;  /* System Clock Control Register 2 */
+        u32     scfr1;  /* System Clock Frequency Register 1 */
+        u32     scfr2;  /* System Clock Frequency Register 2 */
+        u32     scfr2s; /* System Clock Frequency Shadow Register 2 */
+        u32     bcr;    /* Bread Crumb Register */
+        u32     p0ccr;  /* PSC0 Clock Control Register */
+        u32     p1ccr;  /* PSC1 CCR */
+        u32     p2ccr;  /* PSC2 CCR */
+        u32     p3ccr;  /* PSC3 CCR */
+        u32     p4ccr;  /* PSC4 CCR */
+        u32     p5ccr;  /* PSC5 CCR */
+        u32     p6ccr;  /* PSC6 CCR */
+        u32     p7ccr;  /* PSC7 CCR */
+        u32     p8ccr;  /* PSC8 CCR */
+        u32     p9ccr;  /* PSC9 CCR */
+        u32     p10ccr; /* PSC10 CCR */
+        u32     p11ccr; /* PSC11 CCR */
+        u32     spccr;  /* SPDIF Clock Control Register */
+        u32     cccr;   /* CFM Clock Control Register */
+        u32     dccr;   /* DIU Clock Control Register */
+        u32     m1ccr;  /* MSCAN1 CCR */
+        u32     m2ccr;  /* MSCAN2 CCR */
+        u32     m3ccr;  /* MSCAN3 CCR */
+        u32     m4ccr;  /* MSCAN4 CCR */
+        u8      res[0x98]; /* Reserved */
+};
 #endif /* __ASM_POWERPC_MPC5121_H__ */
diff --git a/arch/powerpc/platforms/512x/mpc5121_ads.c b/arch/powerpc/platforms/512x/mpc5121_ads.c
index ee6ae129c25c..dcef6ade48e1 100644
--- a/arch/powerpc/platforms/512x/mpc5121_ads.c
+++ b/arch/powerpc/platforms/512x/mpc5121_ads.c
@@ -42,6 +42,7 @@ static void __init mpc5121_ads_setup_arch(void)
         for_each_compatible_node(np, "pci", "fsl,mpc5121-pci")
                 mpc83xx_add_bridge(np);
 #endif
+        mpc512x_setup_diu();
 }
 
 static void __init mpc5121_ads_init_IRQ(void)
@@ -65,6 +66,7 @@ define_machine(mpc5121_ads) {
         .probe                  = mpc5121_ads_probe,
         .setup_arch             = mpc5121_ads_setup_arch,
         .init                   = mpc512x_init,
+        .init_early             = mpc512x_init_diu,
         .init_IRQ               = mpc5121_ads_init_IRQ,
         .get_irq                = ipic_get_irq,
         .calibrate_decr         = generic_calibrate_decr,
diff --git a/arch/powerpc/platforms/512x/mpc5121_generic.c b/arch/powerpc/platforms/512x/mpc5121_generic.c
index a6c0e3a2615d..e487eb06ec6b 100644
--- a/arch/powerpc/platforms/512x/mpc5121_generic.c
+++ b/arch/powerpc/platforms/512x/mpc5121_generic.c
@@ -52,6 +52,8 @@ define_machine(mpc5121_generic) {
         .name                   = "MPC5121 generic",
         .probe                  = mpc5121_generic_probe,
         .init                   = mpc512x_init,
+        .init_early             = mpc512x_init_diu,
+        .setup_arch             = mpc512x_setup_diu,
         .init_IRQ               = mpc512x_init_IRQ,
         .get_irq                = ipic_get_irq,
         .calibrate_decr         = generic_calibrate_decr,
diff --git a/arch/powerpc/platforms/512x/mpc512x.h b/arch/powerpc/platforms/512x/mpc512x.h
index b2daca0d1488..1ab6d11d0b19 100644
--- a/arch/powerpc/platforms/512x/mpc512x.h
+++ b/arch/powerpc/platforms/512x/mpc512x.h
@@ -16,4 +16,6 @@ extern void __init mpc512x_init(void);
 extern int __init mpc5121_clk_init(void);
 void __init mpc512x_declare_of_platform_devices(void);
 extern void mpc512x_restart(char *cmd);
+extern void mpc512x_init_diu(void);
+extern void mpc512x_setup_diu(void);
 #endif                          /* __MPC512X_H__ */
diff --git a/arch/powerpc/platforms/512x/mpc512x_shared.c b/arch/powerpc/platforms/512x/mpc512x_shared.c
index 707e572b7c40..e41ebbdb3e12 100644
--- a/arch/powerpc/platforms/512x/mpc512x_shared.c
+++ b/arch/powerpc/platforms/512x/mpc512x_shared.c
@@ -16,7 +16,11 @@
 #include <linux/io.h>
 #include <linux/irq.h>
 #include <linux/of_platform.h>
+#include <linux/fsl-diu-fb.h>
+#include <linux/bootmem.h>
+#include <sysdev/fsl_soc.h>
 
+#include <asm/cacheflush.h>
 #include <asm/machdep.h>
 #include <asm/ipic.h>
 #include <asm/prom.h>
@@ -54,6 +58,286 @@ void mpc512x_restart(char *cmd)
                 ;
 }
 
+struct fsl_diu_shared_fb {
+        u8              gamma[0x300];   /* 32-bit aligned! */
+        struct diu_ad   ad0;            /* 32-bit aligned! */
+        phys_addr_t     fb_phys;
+        size_t          fb_len;
+        bool            in_use;
+};
+
+unsigned int mpc512x_get_pixel_format(unsigned int bits_per_pixel,
+                                      int monitor_port)
+{
+        switch (bits_per_pixel) {
+        case 32:
+                return 0x88883316;
+        case 24:
+                return 0x88082219;
+        case 16:
+                return 0x65053118;
+        }
+        return 0x00000400;
+}
+
+void mpc512x_set_gamma_table(int monitor_port, char *gamma_table_base)
+{
+}
+
+void mpc512x_set_monitor_port(int monitor_port)
+{
+}
+
+#define DIU_DIV_MASK    0x000000ff
+void mpc512x_set_pixel_clock(unsigned int pixclock)
+{
+        unsigned long bestval, bestfreq, speed, busfreq;
+        unsigned long minpixclock, maxpixclock, pixval;
+        struct mpc512x_ccm __iomem *ccm;
+        struct device_node *np;
+        u32 temp;
+        long err;
+        int i;
+
+        np = of_find_compatible_node(NULL, NULL, "fsl,mpc5121-clock");
+        if (!np) {
+                pr_err("Can't find clock control module.\n");
+                return;
+        }
+
+        ccm = of_iomap(np, 0);
+        of_node_put(np);
+        if (!ccm) {
+                pr_err("Can't map clock control module reg.\n");
+                return;
+        }
+
+        np = of_find_node_by_type(NULL, "cpu");
+        if (np) {
+                const unsigned int *prop =
+                        of_get_property(np, "bus-frequency", NULL);
+
+                of_node_put(np);
+                if (prop) {
+                        busfreq = *prop;
+                } else {
+                        pr_err("Can't get bus-frequency property\n");
+                        return;
+                }
+        } else {
+                pr_err("Can't find 'cpu' node.\n");
+                return;
+        }
+
+        /* Pixel Clock configuration */
+        pr_debug("DIU: Bus Frequency = %lu\n", busfreq);
+        speed = busfreq * 4; /* DIU_DIV ratio is 4 * CSB_CLK / DIU_CLK */
+
+        /* Calculate the pixel clock with the smallest error */
+        /* calculate the following in steps to avoid overflow */
+        pr_debug("DIU pixclock in ps - %d\n", pixclock);
+        temp = (1000000000 / pixclock) * 1000;
+        pixclock = temp;
+        pr_debug("DIU pixclock freq - %u\n", pixclock);
+
+        temp = temp / 20; /* pixclock * 0.05 */
+        pr_debug("deviation = %d\n", temp);
+        minpixclock = pixclock - temp;
+        maxpixclock = pixclock + temp;