POWERPC: overhaul with cpm2_map mechanism

Incorporating the new way of cpm2 immr access, introduced in the previous
patch, into CPM2 peripheral devices (fs_enet and cpm_uart). Both ppc and
powerpc approved working( real actions taken in powerpc only, ppc just
has a wrapper to keep init stuff consistent).

Signed-off-by: Vitaly Bordug <vbordug@ru.mvista.com>

3 files changed, 157 insertions, 49 deletions

diff --git a/arch/powerpc/platforms/85xx/mpc85xx_ads.c b/arch/powerpc/platforms/85xx/mpc85xx_ads.c
index 7ebfe74bde4..28070e7ae50 100644
--- a/arch/powerpc/platforms/85xx/mpc85xx_ads.c
+++ b/arch/powerpc/platforms/85xx/mpc85xx_ads.c
@@ -33,6 +33,7 @@
 #include "mpc85xx.h"
 
 #ifdef CONFIG_CPM2
+#include <linux/fs_enet_pd.h>
 #include <asm/cpm2.h>
 #include <sysdev/cpm2_pic.h>
 #include <asm/fs_pd.h>
@@ -146,70 +147,81 @@ void __init mpc85xx_ads_pic_init(void)
  * Setup the architecture
  */
 #ifdef CONFIG_CPM2
-static void init_fcc_ioports(void)
+void init_fcc_ioports(struct fs_platform_info *fpi)
 {
-	struct immap *immap;
-	struct io_port *io;
+	struct io_port *io = cpm2_map(im_ioport);
+	int fcc_no = fs_get_fcc_index(fpi->fs_no);
+	int target;
 	u32 tempval;
 
-	immap = cpm2_immr;
-
-	io = &immap->im_ioport;
-	/* FCC2/3 are on the ports B/C. */
-	tempval = in_be32(&io->iop_pdirb);
-	tempval &= ~PB2_DIRB0;
-	tempval |= PB2_DIRB1;
-	out_be32(&io->iop_pdirb, tempval);
-
-	tempval = in_be32(&io->iop_psorb);
-	tempval &= ~PB2_PSORB0;
-	tempval |= PB2_PSORB1;
-	out_be32(&io->iop_psorb, tempval);
-
-	tempval = in_be32(&io->iop_pparb);
-	tempval |= (PB2_DIRB0 | PB2_DIRB1);
-	out_be32(&io->iop_pparb, tempval);
-
-	tempval = in_be32(&io->iop_pdirb);
-	tempval &= ~PB3_DIRB0;
-	tempval |= PB3_DIRB1;
-	out_be32(&io->iop_pdirb, tempval);
-
-	tempval = in_be32(&io->iop_psorb);
-	tempval &= ~PB3_PSORB0;
-	tempval |= PB3_PSORB1;
-	out_be32(&io->iop_psorb, tempval);
-
-	tempval = in_be32(&io->iop_pparb);
-	tempval |= (PB3_DIRB0 | PB3_DIRB1);
-	out_be32(&io->iop_pparb, tempval);
-
-	tempval = in_be32(&io->iop_pdirc);
-	tempval |= PC3_DIRC1;
-	out_be32(&io->iop_pdirc, tempval);
-
-	tempval = in_be32(&io->iop_pparc);
-	tempval |= PC3_DIRC1;
-	out_be32(&io->iop_pparc, tempval);
+	switch(fcc_no) {
+	case 1:
+		tempval = in_be32(&io->iop_pdirb);
+		tempval &= ~PB2_DIRB0;
+		tempval |= PB2_DIRB1;
+		out_be32(&io->iop_pdirb, tempval);
+
+		tempval = in_be32(&io->iop_psorb);
+		tempval &= ~PB2_PSORB0;
+		tempval |= PB2_PSORB1;
+		out_be32(&io->iop_psorb, tempval);
+
+		tempval = in_be32(&io->iop_pparb);
+		tempval |= (PB2_DIRB0 | PB2_DIRB1);
+		out_be32(&io->iop_pparb, tempval);
+
+		target = CPM_CLK_FCC2;
+		break;
+	case 2:
+		tempval = in_be32(&io->iop_pdirb);
+		tempval &= ~PB3_DIRB0;
+		tempval |= PB3_DIRB1;
+		out_be32(&io->iop_pdirb, tempval);
+
+		tempval = in_be32(&io->iop_psorb);
+		tempval &= ~PB3_PSORB0;
+		tempval |= PB3_PSORB1;
+		out_be32(&io->iop_psorb, tempval);
+
+		tempval = in_be32(&io->iop_pparb);
+		tempval |= (PB3_DIRB0 | PB3_DIRB1);
+		out_be32(&io->iop_pparb, tempval);
+
+		tempval = in_be32(&io->iop_pdirc);
+		tempval |= PC3_DIRC1;
+		out_be32(&io->iop_pdirc, tempval);
+
+		tempval = in_be32(&io->iop_pparc);
+		tempval |= PC3_DIRC1;
+		out_be32(&io->iop_pparc, tempval);
+
+		target = CPM_CLK_FCC3;
+		break;
+	default:
+		printk(KERN_ERR "init_fcc_ioports: invalid FCC number\n");
+		return;
+	}
 
 	/* Port C has clocks......  */
 	tempval = in_be32(&io->iop_psorc);
-	tempval &= ~(CLK_TRX);
+	tempval &= ~(PC_CLK(fpi->clk_rx - 8) | PC_CLK(fpi->clk_tx - 8));
 	out_be32(&io->iop_psorc, tempval);
 
 	tempval = in_be32(&io->iop_pdirc);
-	tempval &= ~(CLK_TRX);
+	tempval &= ~(PC_CLK(fpi->clk_rx - 8) | PC_CLK(fpi->clk_tx - 8));
 	out_be32(&io->iop_pdirc, tempval);
 	tempval = in_be32(&io->iop_pparc);
-	tempval |= (CLK_TRX);
+	tempval |= (PC_CLK(fpi->clk_rx - 8) | PC_CLK(fpi->clk_tx - 8));
 	out_be32(&io->iop_pparc, tempval);
 
+	cpm2_unmap(io);
+
 	/* Configure Serial Interface clock routing.
-	 * First,  clear all FCC bits to zero,
+	 * First,  clear FCC bits to zero,
 	 * then set the ones we want.
 	 */
-	immap->im_cpmux.cmx_fcr &= ~(CPMUX_CLK_MASK);
-	immap->im_cpmux.cmx_fcr |= CPMUX_CLK_ROUTE;
+	cpm2_clk_setup(target, fpi->clk_rx, CPM_CLK_RX);
+	cpm2_clk_setup(target, fpi->clk_tx, CPM_CLK_TX);
 }
 #endif
 
@@ -237,7 +249,6 @@ static void __init mpc85xx_ads_setup_arch(void)
 
 #ifdef CONFIG_CPM2
 	cpm2_reset();
-	init_fcc_ioports();
 #endif
 
 #ifdef CONFIG_PCI
diff --git a/arch/powerpc/sysdev/cpm2_common.c b/arch/powerpc/sysdev/cpm2_common.c
index 73376f9c156..ec265995d5d 100644
--- a/arch/powerpc/sysdev/cpm2_common.c
+++ b/arch/powerpc/sysdev/cpm2_common.c
@@ -130,6 +130,96 @@ cpm2_fastbrg(uint brg, uint rate, int div16)
 	cpm2_unmap(bp);
 }
 
+int cpm2_clk_setup(enum cpm_clk_target target, int clock, int mode)
+{
+	int ret = 0;
+	int shift;
+	int i, bits = 0;
+	cpmux_t *im_cpmux;
+	u32 *reg;
+	u32 mask = 7;
+	u8 clk_map [24][3] = {
+		{CPM_CLK_FCC1, CPM_BRG5, 0},
+		{CPM_CLK_FCC1, CPM_BRG6, 1},
+		{CPM_CLK_FCC1, CPM_BRG7, 2},
+		{CPM_CLK_FCC1, CPM_BRG8, 3},
+		{CPM_CLK_FCC1, CPM_CLK9, 4},
+		{CPM_CLK_FCC1, CPM_CLK10, 5},
+		{CPM_CLK_FCC1, CPM_CLK11, 6},
+		{CPM_CLK_FCC1, CPM_CLK12, 7},
+		{CPM_CLK_FCC2, CPM_BRG5, 0},
+		{CPM_CLK_FCC2, CPM_BRG6, 1},
+		{CPM_CLK_FCC2, CPM_BRG7, 2},
+		{CPM_CLK_FCC2, CPM_BRG8, 3},
+		{CPM_CLK_FCC2, CPM_CLK13, 4},
+		{CPM_CLK_FCC2, CPM_CLK14, 5},
+		{CPM_CLK_FCC2, CPM_CLK15, 6},
+		{CPM_CLK_FCC2, CPM_CLK16, 7},
+		{CPM_CLK_FCC3, CPM_BRG5, 0},
+		{CPM_CLK_FCC3, CPM_BRG6, 1},
+		{CPM_CLK_FCC3, CPM_BRG7, 2},
+		{CPM_CLK_FCC3, CPM_BRG8, 3},
+		{CPM_CLK_FCC3, CPM_CLK13, 4},
+		{CPM_CLK_FCC3, CPM_CLK14, 5},
+		{CPM_CLK_FCC3, CPM_CLK15, 6},
+		{CPM_CLK_FCC3, CPM_CLK16, 7}
+		};
+
+	im_cpmux = cpm2_map(im_cpmux);
+
+	switch (target) {
+	case CPM_CLK_SCC1:
+		reg = &im_cpmux->cmx_scr;
+		shift = 24;
+	case CPM_CLK_SCC2:
+		reg = &im_cpmux->cmx_scr;
+		shift = 16;
+		break;
+	case CPM_CLK_SCC3:
+		reg = &im_cpmux->cmx_scr;
+		shift = 8;
+		break;
+	case CPM_CLK_SCC4:
+		reg = &im_cpmux->cmx_scr;
+		shift = 0;
+		break;
+	case CPM_CLK_FCC1:
+		reg = &im_cpmux->cmx_fcr;
+		shift = 24;
+		break;
+	case CPM_CLK_FCC2:
+		reg = &im_cpmux->cmx_fcr;
+		shift = 16;
+		break;
+	case CPM_CLK_FCC3:
+		reg = &im_cpmux->cmx_fcr;
+		shift = 8;
+		break;
+	default:
+		printk(KERN_ERR "cpm2_clock_setup: invalid clock target\n");
+		return -EINVAL;
+	}
+
+	if (mode == CPM_CLK_RX)
+		shift +=3;
+
+	for (i=0; i<24; i++) {
+		if (clk_map[i][0] == target && clk_map[i][1] == clock) {
+			bits = clk_map[i][2];
+			break;
+		}
+	}
+	if (i == sizeof(clk_map)/3)
+	    ret = -EINVAL;
+
+	bits <<= shift;
+	mask <<= shift;
+	out_be32(reg, (in_be32(reg) & ~mask) | bits);
+
+	cpm2_unmap(im_cpmux);
+	return ret;
+}
+
 /*
  * dpalloc / dpfree bits.
  */
diff --git a/arch/powerpc/sysdev/fsl_soc.c b/arch/powerpc/sysdev/fsl_soc.c
index 0b8a03cc304..4e72bb98363 100644
--- a/arch/powerpc/sysdev/fsl_soc.c
+++ b/arch/powerpc/sysdev/fsl_soc.c
@@ -36,6 +36,7 @@
 #include <mm/mmu_decl.h>
 #include <asm/cpm2.h>
 
+extern void init_fcc_ioports(struct fs_platform_info*);
 static phys_addr_t immrbase = -1;
 
 phys_addr_t get_immrbase(void)
@@ -630,6 +631,9 @@ static int __init fs_enet_of_init(void)
                         goto unreg;
                 }
 
+		fs_enet_data.clk_rx = *((u32 *) get_property(np, "rx-clock", NULL));
+		fs_enet_data.clk_tx = *((u32 *) get_property(np, "tx-clock", NULL));
+
 		if (strstr(model, "FCC")) {
 			int fcc_index = fs_get_fcc_index(*id);
 
@@ -646,6 +650,7 @@ static int __init fs_enet_of_init(void)
 			snprintf((char*)&bus_id[(*id)], BUS_ID_SIZE, "%x:%02x",
 							(u32)res.start, fs_enet_data.phy_addr);
 			fs_enet_data.bus_id = (char*)&bus_id[(*id)];
+			fs_enet_data.init_ioports = init_fcc_ioports;
 		}
 
 		of_node_put(phy);
@@ -717,6 +722,8 @@ static int __init cpm_uart_of_init(void)
 		cpm_uart_data.tx_buf_size = 32;
 		cpm_uart_data.rx_num_fifo = 4;
 		cpm_uart_data.rx_buf_size = 32;
+		cpm_uart_data.clk_rx = *((u32 *) get_property(np, "rx-clock", NULL));
+		cpm_uart_data.clk_tx = *((u32 *) get_property(np, "tx-clock", NULL));
 
 		ret =
 		    platform_device_add_data(cpm_uart_dev, &cpm_uart_data,