diff --git a/board/freescale/p1022ds/diu.c b/board/freescale/p1022ds/diu.c
index d5428ea16de4fd0b2c893cbb768fc06ed66b158a..898f4c7b194d040bc1434ab7e48bf1c032f83bb3 100644
--- a/board/freescale/p1022ds/diu.c
+++ b/board/freescale/p1022ds/diu.c
@@ -63,6 +63,8 @@ static u8 px_brdcfg0;
static u32 pmuxcr;
static void *lbc_lcs0_ba;
static void *lbc_lcs1_ba;
+static u32 old_br0, old_or0, old_br1, old_or1;
+static u32 new_br0, new_or0, new_br1, new_or1;
void diu_set_pixel_clock(unsigned int pixclock)
{
@@ -88,10 +90,63 @@ int platform_diu_init(unsigned int xres, unsigned int yres, const char *port)
const char *name;
u32 pixel_format;
u8 temp;
+ phys_addr_t phys0, phys1; /* BR0/BR1 physical addresses */
- /* Save the LBC LCS0 and LCS1 addresses for the DIU mux functions */
- lbc_lcs0_ba = (void *)(get_lbc_br(0) & get_lbc_or(0) & 0xFFFF8000);
- lbc_lcs1_ba = (void *)(get_lbc_br(1) & get_lbc_or(1) & 0xFFFF8000);
+ /*
+ * Indirect mode requires both BR0 and BR1 to be set to "GPCM",
+ * otherwise writes to these addresses won't actually appear on the
+ * local bus, and so the PIXIS won't see them.
+ *
+ * In FCM mode, writes go to the NAND controller, which does not pass
+ * them to the localbus directly. So we force BR0 and BR1 into GPCM
+ * mode, since we don't care about what's behind the localbus any
+ * more. However, we save those registers first, so that we can
+ * restore them when necessary.
+ */
+ new_br0 = old_br0 = get_lbc_br(0);
+ new_br1 = old_br1 = get_lbc_br(1);
+ new_or0 = old_or0 = get_lbc_or(0);
+ new_or1 = old_or1 = get_lbc_or(1);
+
+ /*
+ * Use the existing BRx/ORx values if it's already GPCM. Otherwise,
+ * force the values to simple 32KB GPCM windows with the most
+ * conservative timing.
+ */
+ if ((old_br0 & BR_MSEL) != BR_MS_GPCM) {
+ new_br0 = (get_lbc_br(0) & BR_BA) | BR_V;
+ new_or0 = OR_AM_32KB | 0xFF7;
+ set_lbc_br(0, new_br0);
+ set_lbc_or(0, new_or0);
+ }
+ if ((old_br1 & BR_MSEL) != BR_MS_GPCM) {
+ new_br1 = (get_lbc_br(1) & BR_BA) | BR_V;
+ new_or1 = OR_AM_32KB | 0xFF7;
+ set_lbc_br(1, new_br1);
+ set_lbc_or(1, new_or1);
+ }
+
+ /*
+ * Determine the physical addresses for Chip Selects 0 and 1. The
+ * BR0/BR1 registers contain the truncated physical addresses for the
+ * chip selects, mapped via the localbus LAW. Since the BRx registers
+ * only contain the lower 32 bits of the address, we have to determine
+ * the upper 4 bits some other way. The proper way is to scan the LAW
+ * table looking for a matching localbus address. Instead, we cheat.
+ * We know that the upper bits are 0 for 32-bit addressing, or 0xF for
+ * 36-bit addressing.
+ */
+#ifdef CONFIG_PHYS_64BIT
+ phys0 = 0xf00000000ULL | (old_br0 & old_or0 & BR_BA);
+ phys1 = 0xf00000000ULL | (old_br1 & old_or1 & BR_BA);
+#else
+ phys0 = old_br0 & old_or0 & BR_BA;
+ phys1 = old_br1 & old_or1 & BR_BA;
+#endif
+
+ /* Save the LBC LCS0 and LCS1 addresses for the DIU mux functions */
+ lbc_lcs0_ba = map_physmem(phys0, 1, 0);
+ lbc_lcs1_ba = map_physmem(phys1, 1, 0);
pixel_format = cpu_to_le32(AD_BYTE_F | (3 << AD_ALPHA_C_SHIFT) |
(0 << AD_BLUE_C_SHIFT) | (1 << AD_GREEN_C_SHIFT) |
@@ -134,6 +189,7 @@ int platform_diu_init(unsigned int xres, unsigned int yres, const char *port)
out_8(lbc_lcs0_ba, offsetof(ngpixis_t, brdcfg0));
px_brdcfg0 = in_8(lbc_lcs1_ba);
out_8(lbc_lcs1_ba, px_brdcfg0 | PX_BRDCFG0_ELBC_DIU);
+ in_8(lbc_lcs1_ba);
/* Set PMUXCR to switch the muxed pins from the LBC to the DIU */
clrsetbits_be32(&gur->pmuxcr, PMUXCR_ELBCDIU_MASK, PMUXCR_ELBCDIU_DIU);
@@ -168,12 +224,10 @@ static int set_mux_to_lbc(void)
* In DIU mode, the PIXIS can only be accessed indirectly
* since we can't read/write the LBC directly.
*/
-
/* Set the board mux to LBC. This will disable the display. */
out_8(lbc_lcs0_ba, offsetof(ngpixis_t, brdcfg0));
- px_brdcfg0 = in_8(lbc_lcs1_ba);
- out_8(lbc_lcs1_ba, (px_brdcfg0 & ~(PX_BRDCFG0_ELBC_SPI_MASK
- | PX_BRDCFG0_ELBC_DIU)) | PX_BRDCFG0_ELBC_SPI_ELBC);
+ out_8(lbc_lcs1_ba, px_brdcfg0);
+ in_8(lbc_lcs1_ba);
/* Disable indirect PIXIS mode */
out_8(lbc_lcs0_ba, offsetof(ngpixis_t, csr));
@@ -184,6 +238,12 @@ static int set_mux_to_lbc(void)
PMUXCR_ELBCDIU_NOR16);
in_be32(&gur->pmuxcr);
+ /* Restore the BR0 and BR1 settings */
+ set_lbc_br(0, old_br0);
+ set_lbc_or(0, old_or0);
+ set_lbc_br(1, old_br1);
+ set_lbc_or(1, old_or1);
+
return 1;
}
@@ -199,12 +259,18 @@ static void set_mux_to_diu(void)
{
ccsr_gur_t *gur = (void *)CONFIG_SYS_MPC85xx_GUTS_ADDR;
+ /* Set BR0 and BR1 to GPCM mode */
+ set_lbc_br(0, new_br0);
+ set_lbc_or(0, new_or0);
+ set_lbc_br(1, new_br1);
+ set_lbc_or(1, new_or1);
+
/* Enable indirect PIXIS mode */
setbits_8(&pixis->csr, PX_CTL_ALTACC);
/* Set the board mux to DIU. This will enable the display. */
out_8(lbc_lcs0_ba, offsetof(ngpixis_t, brdcfg0));
- out_8(lbc_lcs1_ba, px_brdcfg0);
+ out_8(lbc_lcs1_ba, px_brdcfg0 | PX_BRDCFG0_ELBC_DIU);
in_8(lbc_lcs1_ba);
/* Set the chip mux to DIU mode. */