powerpc/p2041rdb: Add p2041rdb board support

P2041RDB Specification:
-----------------------
Memory subsystem:
 * 4Gbyte unbuffered DDR3 SDRAM SO-DIMM(64bit bus)
 * 128 Mbyte NOR flash single-chip memory
 * 256 Kbit M24256 I2C EEPROM
 * 16 Mbyte SPI memory
 * SD connector to interface with the SD memory card

Ethernet:
 * dTSEC1: connected to the Vitesse SGMII PHY (VSC8221)
 * dTSEC2: connected to the Vitesse SGMII PHY (VSC8221)
 * dTSEC3: connected to the Vitesse SGMII PHY (VSC8221)
 * dTSEC4: connected to the Vitesse RGMII PHY (VSC8641)
 * dTSEC5: connected to the Vitesse RGMII PHY (VSC8641)

PCIe:
 * Lanes E, F, G and H of Bank1 are connected to one x4 PCIe SLOT1
 * Lanes C and Land D of Bank2 are connected to one x4 PCIe SLOT2

SATA: Lanes C and Land D of Bank2 are connected to two SATA connectors

USB 2.0: connected via a internal UTMI PHY to two TYPE-A interfaces

I2C:
 * I2C1: Real time clock, Temperature sensor, Memory module
 * I2C2: Vcore Regulator, 256Kbit I2C Bus EEPROM, PCIe slot1/2

UART: supports two UARTs up to 115200 bps for console

Signed-off-by: Mingkai Hu <Mingkai.hu@freescale.com>
Signed-off-by: Kumar Gala <galak@kernel.crashing.org>

8 files changed, 793 insertions, 0 deletions

diff --git a/board/freescale/p2041rdb/Makefile b/board/freescale/p2041rdb/Makefile
new file mode 100644
index 0000000000..65f348fe31
--- /dev/null
+++ b/board/freescale/p2041rdb/Makefile
@@ -0,0 +1,56 @@
+#
+# Copyright 2011 Freescale Semiconductor, Inc.
+# (C) Copyright 2001-2006
+# Wolfgang Denk, DENX Software Engineering, wd@denx.de.
+#
+# See file CREDITS for list of people who contributed to this
+# project.
+#
+# This program is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License as
+# published by the Free Software Foundation; either version 2 of
+# the License, or (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+# GNU General Public License for more details.
+#
+# You should have received a copy of the GNU General Public License
+# along with this program; if not, write to the Free Software
+# Foundation, Inc., 59 Temple Place, Suite 330, Boston,
+# MA 02111-1307 USA
+#
+
+include $(TOPDIR)/config.mk
+
+LIB	= $(obj)lib$(BOARD).o
+
+COBJS-y	+= $(BOARD).o
+COBJS-y += cpld.o
+COBJS-y	+= ddr.o
+COBJS-y	+= law.o
+COBJS-y	+= tlb.o
+COBJS-$(CONFIG_PCI) += pci.o
+
+SRCS	:= $(SOBJS:.o=.S) $(COBJS-y:.o=.c)
+OBJS	:= $(addprefix $(obj),$(COBJS-y))
+SOBJS	:= $(addprefix $(obj),$(SOBJS))
+
+$(LIB):	$(obj).depend $(OBJS) $(SOBJS)
+	$(call cmd_link_o_target, $(OBJS))
+
+clean:
+	rm -f $(OBJS) $(SOBJS)
+
+distclean:	clean
+	rm -f $(LIB) core *.bak .depend
+
+#########################################################################
+
+# defines $(obj).depend target
+include $(SRCTREE)/rules.mk
+
+sinclude $(obj).depend
+
+#########################################################################
diff --git a/board/freescale/p2041rdb/cpld.c b/board/freescale/p2041rdb/cpld.c
new file mode 100644
index 0000000000..8e1f46e790
--- /dev/null
+++ b/board/freescale/p2041rdb/cpld.c
@@ -0,0 +1,171 @@
+/**
+ * Copyright 2011 Freescale Semiconductor
+ * Author: Mingkai Hu <Mingkai.hu@freescale.com>
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License as published by the Free
+ * Software Foundation; either version 2 of the License, or (at your option)
+ * any later version.
+ *
+ * This file provides support for the board-specific CPLD used on some Freescale
+ * reference boards.
+ *
+ * The following macros need to be defined:
+ *
+ * CPLD_BASE - The virtual address of the base of the CPLD register map
+ *
+ */
+
+#include <common.h>
+#include <command.h>
+#include <asm/io.h>
+
+#include "cpld.h"
+
+static u8 __cpld_read(unsigned int reg)
+{
+	void *p = (void *)CPLD_BASE;
+
+	return in_8(p + reg);
+}
+u8 cpld_read(unsigned int reg) __attribute__((weak, alias("__cpld_read")));
+
+static void __cpld_write(unsigned int reg, u8 value)
+{
+	void *p = (void *)CPLD_BASE;
+
+	out_8(p + reg, value);
+}
+void cpld_write(unsigned int reg, u8 value)
+	__attribute__((weak, alias("__cpld_write")));
+
+/*
+ * Reset the board. This honors the por_cfg registers.
+ */
+void __cpld_reset(void)
+{
+	CPLD_WRITE(system_rst, 1);
+}
+void cpld_reset(void) __attribute__((weak, alias("__cpld_reset")));
+
+/**
+ * Set the boot bank to the alternate bank
+ */
+void __cpld_set_altbank(void)
+{
+	CPLD_WRITE(fbank_sel, 1);
+}
+void cpld_set_altbank(void)
+	__attribute__((weak, alias("__cpld_set_altbank")));
+
+/**
+ * Set the boot bank to the default bank
+ */
+void __cpld_clear_altbank(void)
+{
+	CPLD_WRITE(fbank_sel, 0);
+}
+void cpld_clear_altbank(void)
+	__attribute__((weak, alias("__cpld_clear_altbank")));
+
+#ifdef DEBUG
+static void cpld_dump_regs(void)
+{
+	printf("cpld_ver	= 0x%02x\n", CPLD_READ(cpld_ver));
+	printf("cpld_ver_sub	= 0x%02x\n", CPLD_READ(cpld_ver_sub));
+	printf("pcba_ver	= 0x%02x\n", CPLD_READ(pcba_ver));
+	printf("system_rst	= 0x%02x\n", CPLD_READ(system_rst));
+	printf("wd_cfg		= 0x%02x\n", CPLD_READ(wd_cfg));
+	printf("sw_ctl_on	= 0x%02x\n", CPLD_READ(sw_ctl_on));
+	printf("por_cfg		= 0x%02x\n", CPLD_READ(por_cfg));
+	printf("switch_strobe	= 0x%02x\n", CPLD_READ(switch_strobe));
+	printf("jtag_sel	= 0x%02x\n", CPLD_READ(jtag_sel));
+	printf("sdbank1_clk	= 0x%02x\n", CPLD_READ(sdbank1_clk));
+	printf("sdbank2_clk	= 0x%02x\n", CPLD_READ(sdbank2_clk));
+	printf("fbank_sel	= 0x%02x\n", CPLD_READ(fbank_sel));
+	printf("serdes_mux	= 0x%02x\n", CPLD_READ(serdes_mux));
+	printf("SW[2]		= 0x%02x\n", in_8(&CPLD_SW(2)));
+	putc('\n');
+}
+#endif
+
+int cpld_cmd(cmd_tbl_t *cmdtp, int flag, int argc, char * const argv[])
+{
+	int rc = 0;
+	unsigned int i;
+
+	if (argc <= 1)
+		return cmd_usage(cmdtp);
+
+	if (strcmp(argv[1], "reset") == 0) {
+		if (strcmp(argv[2], "altbank") == 0)
+			cpld_set_altbank();
+		else
+			cpld_clear_altbank();
+
+		cpld_reset();
+	} else if (strcmp(argv[1], "watchdog") == 0) {
+		static char *period[8] = {"1ms", "10ms", "30ms", "disable",
+			"100ms", "1s", "10s", "60s"};
+		for (i = 0; i < ARRAY_SIZE(period); i++) {
+			if (strcmp(argv[2], period[i]) == 0)
+				CPLD_WRITE(wd_cfg, i);
+		}
+	} else if (strcmp(argv[1], "lane_mux") == 0) {
+		u32 lane = simple_strtoul(argv[2], NULL, 16);
+		u8 val = (u8)simple_strtoul(argv[3], NULL, 16);
+		u8 reg = CPLD_READ(serdes_mux);
+
+		switch (lane) {
+		case 0x6:
+			reg &= ~SERDES_MUX_LANE_6_MASK;
+			reg |= val << SERDES_MUX_LANE_6_SHIFT;
+			break;
+		case 0xa:
+			reg &= ~SERDES_MUX_LANE_A_MASK;
+			reg |= val << SERDES_MUX_LANE_A_SHIFT;
+			break;
+		case 0xc:
+			reg &= ~SERDES_MUX_LANE_C_MASK;
+			reg |= val << SERDES_MUX_LANE_C_SHIFT;
+			break;
+		case 0xd:
+			reg &= ~SERDES_MUX_LANE_D_MASK;
+			reg |= val << SERDES_MUX_LANE_D_SHIFT;
+			break;
+		default:
+			printf("Invalid value\n");
+			break;
+		}
+
+		CPLD_WRITE(serdes_mux, reg);
+#ifdef DEBUG
+	} else if (strcmp(argv[1], "dump") == 0) {
+		cpld_dump_regs();
+#endif
+	} else
+		rc = cmd_usage(cmdtp);
+
+	return rc;
+}
+
+U_BOOT_CMD(
+	cpld_cmd, CONFIG_SYS_MAXARGS, 1, cpld_cmd,
+	"Reset the board or pin mulexing selection using the CPLD sequencer",
+	"reset - hard reset to default bank\n"
+	"cpld_cmd reset altbank - reset to alternate bank\n"
+	"cpld_cmd watchdog <watchdog_period> - set the watchdog period\n"
+	"	period: 1ms 10ms 30ms 100ms 1s 10s 60s disable\n"
+	"cpld_cmd lane_mux <lane> <mux_value> - set multiplexed lane pin\n"
+	"	lane 6: 0 -> slot1 (Default)\n"
+	"		1 -> SGMII\n"
+	"	lane a: 0 -> slot2 (Default)\n"
+	"		1 -> AURORA\n"
+	"	lane c: 0 -> slot2 (Default)\n"
+	"		1 -> SATA0\n"
+	"	lane d: 0 -> slot2 (Default)\n"
+	"		1 -> SATA1\n"
+#ifdef DEBUG
+	"cpld_cmd dump - display the CPLD registers\n"
+#endif
+	);
diff --git a/board/freescale/p2041rdb/cpld.h b/board/freescale/p2041rdb/cpld.h
new file mode 100644
index 0000000000..3b24cb0480
--- /dev/null
+++ b/board/freescale/p2041rdb/cpld.h
@@ -0,0 +1,53 @@
+/**
+ * Copyright 2011 Freescale Semiconductor
+ * Author: Mingkai Hu <Mingkai.hu@freescale.com>
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License as published by the Free
+ * Software Foundation; either version 2 of the License, or (at your option)
+ * any later version.
+ *
+ * This file provides support for the ngPIXIS, a board-specific FPGA used on
+ * some Freescale reference boards.
+ */
+
+/*
+ * CPLD register set. Feel free to add board-specific #ifdefs where necessary.
+ */
+typedef struct cpld_data {
+	u8 cpld_ver;		/* 0x0 - CPLD Major Revision Register */
+	u8 cpld_ver_sub;	/* 0x1 - CPLD Minor Revision Register */
+	u8 pcba_ver;		/* 0x2 - PCBA Revision Register */
+	u8 system_rst;		/* 0x3 - system reset register */
+	u8 wd_cfg;		/* 0x4 - Watchdog Period Setting Register */
+	u8 sw_ctl_on;		/* 0x5 - Switch Control Enable Register */
+	u8 por_cfg;		/* 0x6 - POR Control Register */
+	u8 switch_strobe;	/* 0x7 - Multiplexed pin Select Register */
+	u8 jtag_sel;		/* 0x8 - JTAG or AURORA Selection */
+	u8 sdbank1_clk;		/* 0x9 - SerDes Bank1 Reference clock */
+	u8 sdbank2_clk;		/* 0xa - SerDes Bank2 Reference clock */
+	u8 fbank_sel;		/* 0xb - Flash bank selection */
+	u8 serdes_mux;		/* 0xc - Multiplexed pin Select Register */
+	u8 sw[1];		/* 0xd - SW2 Status */
+} __attribute__ ((packed)) cpld_data_t;
+
+#define SERDES_MUX_LANE_6_MASK	0x2
+#define SERDES_MUX_LANE_6_SHIFT	1
+#define SERDES_MUX_LANE_A_MASK	0x1
+#define SERDES_MUX_LANE_A_SHIFT	0
+#define SERDES_MUX_LANE_C_MASK	0x4
+#define SERDES_MUX_LANE_C_SHIFT	2
+#define SERDES_MUX_LANE_D_MASK	0x8
+#define SERDES_MUX_LANE_D_SHIFT	3
+
+/* Pointer to the CPLD register set */
+#define cpld ((cpld_data_t *)CPLD_BASE)
+
+/* The CPLD SW register that corresponds to board switch X, where x >= 1 */
+#define CPLD_SW(x)		(cpld->sw[(x) - 2])
+
+u8 cpld_read(unsigned int reg);
+void cpld_write(unsigned int reg, u8 value);
+
+#define CPLD_READ(reg) cpld_read(offsetof(cpld_data_t, reg))
+#define CPLD_WRITE(reg, value) cpld_write(offsetof(cpld_data_t, reg), value)
diff --git a/board/freescale/p2041rdb/ddr.c b/board/freescale/p2041rdb/ddr.c
new file mode 100644
index 0000000000..46de9109da
--- /dev/null
+++ b/board/freescale/p2041rdb/ddr.c
@@ -0,0 +1,115 @@
+/*
+ * Copyright 2011 Freescale Semiconductor, Inc.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * Version 2 as published by the Free Software Foundation.
+ */
+
+#include <common.h>
+#include <i2c.h>
+#include <hwconfig.h>
+#include <asm/mmu.h>
+#include <asm/fsl_ddr_sdram.h>
+#include <asm/fsl_ddr_dimm_params.h>
+#include <asm/fsl_law.h>
+
+typedef struct {
+	u32 datarate_mhz_low;
+	u32 datarate_mhz_high;
+	u32 n_ranks;
+	u32 clk_adjust;
+	u32 wrlvl_start;
+	u32 cpo;
+	u32 write_data_delay;
+	u32 force_2T;
+} board_specific_parameters_t;
+
+/*
+ * ranges for parameters:
+ *  wr_data_delay = 0-6
+ *  clk adjust = 0-8
+ *  cpo 2-0x1E (30)
+ */
+const board_specific_parameters_t board_specific_parameters[] = {
+	/*
+	 * memory controller 0
+	 *  lo|  hi|  num|  clk| wrlvl | cpo  |wrdata|2T
+	 * mhz| mhz|ranks|adjst| start | delay|
+	 */
+	{  1017, 1116,    2,    4,     6,   0xff,    2,  0},
+};
+
+void fsl_ddr_board_options(memctl_options_t *popts,
+				dimm_params_t *pdimm,
+				unsigned int ctrl_num)
+{
+	const board_specific_parameters_t *pbsp =
+				&board_specific_parameters[0];
+	u32 num_params = ARRAY_SIZE(board_specific_parameters);
+	u32 i;
+	ulong ddr_freq;
+
+	/*
+	 * Get clk_adjust, cpo, write_data_delay,2T, according to the board ddr
+	 * freqency and n_banks specified in board_specific_parameters table.
+	 */
+	ddr_freq = get_ddr_freq(0) / 1000000;
+	for (i = 0; i < num_params; i++) {
+		if (ddr_freq >= pbsp->datarate_mhz_low &&
+			ddr_freq <= pbsp->datarate_mhz_high &&
+			pdimm[0].n_ranks == pbsp->n_ranks) {
+			popts->cpo_override = pbsp->cpo;
+			popts->write_data_delay = pbsp->write_data_delay;
+			popts->clk_adjust = pbsp->clk_adjust;
+			popts->wrlvl_start = pbsp->wrlvl_start;
+			popts->twoT_en = pbsp->force_2T;
+			break;
+		}
+		pbsp++;
+	}
+
+	if (i == num_params) {
+		printf("Warning: board specific timing not found "
+			"for data rate %lu MT/s!\n", ddr_freq);
+	}
+
+	/*
+	 * Factors to consider for half-strength driver enable:
+	 *	- number of DIMMs installed
+	 */
+	popts->half_strength_driver_enable = 0;
+	/* Write leveling override */
+	popts->wrlvl_override = 1;
+	popts->wrlvl_sample = 0xf;
+
+	/* Rtt and Rtt_WR override */
+	popts->rtt_override = 0;
+
+	/* Enable ZQ calibration */
+	popts->zq_en = 1;
+
+	/* DHC_EN =1, ODT = 60 Ohm */
+	popts->ddr_cdr1 = DDR_CDR1_DHC_EN;
+}
+
+phys_size_t initdram(int board_type)
+{
+	phys_size_t dram_size = 0;
+
+	puts("Initializing....");
+
+	if (fsl_use_spd()) {
+		puts("using SPD\n");
+		dram_size = fsl_ddr_sdram();
+	} else {
+		puts("no SPD and fixed parameters\n");
+		return dram_size;
+	}
+
+	dram_size = setup_ddr_tlbs(dram_size / 0x100000);
+	dram_size *= 0x100000;
+
+	puts("    DDR: ");
+	return dram_size;
+}
diff --git a/board/freescale/p2041rdb/law.c b/board/freescale/p2041rdb/law.c
new file mode 100644
index 0000000000..127a478e46
--- /dev/null
+++ b/board/freescale/p2041rdb/law.c
@@ -0,0 +1,37 @@
+/*
+ * Copyright 2011 Freescale Semiconductor, Inc.
+ *
+ * See file CREDITS for list of people who contributed to this
+ * project.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation; either version 2 of
+ * the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston,
+ * MA 02111-1307 USA
+ */
+
+#include <common.h>
+#include <asm/fsl_law.h>
+#include <asm/mmu.h>
+
+struct law_entry law_table[] = {
+	SET_LAW(CONFIG_SYS_FLASH_BASE_PHYS, LAW_SIZE_256M, LAW_TRGT_IF_LBC),
+	SET_LAW(CONFIG_SYS_BMAN_MEM_PHYS, LAW_SIZE_2M, LAW_TRGT_IF_BMAN),
+	SET_LAW(CONFIG_SYS_QMAN_MEM_PHYS, LAW_SIZE_2M, LAW_TRGT_IF_QMAN),
+	SET_LAW(CPLD_BASE_PHYS, LAW_SIZE_4K, LAW_TRGT_IF_LBC),
+#ifdef CONFIG_SYS_DCSRBAR_PHYS
+	SET_LAW(CONFIG_SYS_DCSRBAR_PHYS, LAW_SIZE_32M, LAW_TRGT_IF_DCSR),
+#endif
+};
+
+int num_law_entries = ARRAY_SIZE(law_table);
diff --git a/board/freescale/p2041rdb/p2041rdb.c b/board/freescale/p2041rdb/p2041rdb.c
new file mode 100644
index 0000000000..52269d3ce4
--- /dev/null
+++ b/board/freescale/p2041rdb/p2041rdb.c
@@ -0,0 +1,203 @@
+/*
+ * Copyright 2011 Freescale Semiconductor, Inc.
+ *
+ * See file CREDITS for list of people who contributed to this
+ * project.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation; either version 2 of
+ * the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston,
+ * MA 02111-1307 USA
+ */
+
+#include <common.h>
+#include <command.h>
+#include <netdev.h>
+#include <linux/compiler.h>
+#include <asm/mmu.h>
+#include <asm/processor.h>
+#include <asm/cache.h>
+#include <asm/immap_85xx.h>
+#include <asm/fsl_law.h>
+#include <asm/fsl_serdes.h>
+#include <asm/fsl_portals.h>
+#include <asm/fsl_liodn.h>
+
+extern void pci_of_setup(void *blob, bd_t *bd);
+
+#include "cpld.h"
+
+DECLARE_GLOBAL_DATA_PTR;
+
+int checkboard(void)
+{
+	u8 sw;
+	struct cpu_type *cpu = gd->cpu;
+	ccsr_gur_t *gur = (void *)CONFIG_SYS_MPC85xx_GUTS_ADDR;
+	unsigned int i;
+
+	printf("Board: %sRDB, ", cpu->name);
+	printf("CPLD version: %d.%d ", CPLD_READ(cpld_ver),
+			CPLD_READ(cpld_ver_sub));
+
+	sw = CPLD_READ(fbank_sel);
+	printf("vBank: %d\n", sw & 0x1);
+
+#ifdef CONFIG_PHYS_64BIT
+	puts("36-bit Addressing\n");
+#endif
+
+	/*
+	 * Display the RCW, so that no one gets confused as to what RCW
+	 * we're actually using for this boot.
+	 */
+	puts("Reset Configuration Word (RCW):");
+	for (i = 0; i < ARRAY_SIZE(gur->rcwsr); i++) {
+		u32 rcw = in_be32(&gur->rcwsr[i]);
+
+		if ((i % 4) == 0)
+			printf("\n       %08x:", i * 4);
+		printf(" %08x", rcw);
+	}
+	puts("\n");
+
+	/*
+	 * Display the actual SERDES reference clocks as configured by the
+	 * dip switches on the board.  Note that the SWx registers could
+	 * technically be set to force the reference clocks to match the
+	 * values that the SERDES expects (or vice versa).  For now, however,
+	 * we just display both values and hope the user notices when they
+	 * don't match.
+	 */
+	puts("SERDES Reference Clocks: ");
+	sw = in_8(&CPLD_SW(2)) >> 2;
+	for (i = 0; i < 2; i++) {
+		static const char * const freq[] = {"0", "100", "125"};
+		unsigned int clock = (sw >> (2 * i)) & 3;
+
+		printf("Bank%u=%sMhz ", i+1, freq[clock]);
+	}
+	puts("\n");
+
+	return 0;
+}
+
+int board_early_init_f(void)
+{
+	ccsr_gur_t *gur = (void *)(CONFIG_SYS_MPC85xx_GUTS_ADDR);
+
+	/* board only uses the DDR_MCK0/1, so disable the DDR_MCK2/3 */
+	setbits_be32(&gur->ddrclkdr, 0x000f000f);
+
+	return 0;
+}
+
+int board_early_init_r(void)
+{
+	const unsigned int flashbase = CONFIG_SYS_FLASH_BASE;
+	const u8 flash_esel = find_tlb_idx((void *)flashbase, 1);
+
+	/*
+	 * Remap Boot flash + PROMJET region to caching-inhibited
+	 * so that flash can be erased properly.
+	 */
+
+	/* Flush d-cache and invalidate i-cache of any FLASH data */
+	flush_dcache();
+	invalidate_icache();
+
+	/* invalidate existing TLB entry for flash + promjet */
+	disable_tlb(flash_esel);
+
+	set_tlb(1, flashbase, CONFIG_SYS_FLASH_BASE_PHYS,
+			MAS3_SX|MAS3_SW|MAS3_SR, MAS2_I|MAS2_G,
+			0, flash_esel, BOOKE_PAGESZ_256M, 1);
+
+	set_liodns();
+	setup_portals();
+
+	return 0;
+}
+
+static const char *serdes_clock_to_string(u32 clock)
+{
+	switch (clock) {
+	case SRDS_PLLCR0_RFCK_SEL_100:
+		return "100";
+	case SRDS_PLLCR0_RFCK_SEL_125:
+		return "125";
+	case SRDS_PLLCR0_RFCK_SEL_156_25:
+		return "156.25";
+	default:
+		return "150";
+	}
+}
+
+#define NUM_SRDS_BANKS	2
+
+int misc_init_r(void)
+{
+	serdes_corenet_t *regs = (void *)CONFIG_SYS_FSL_CORENET_SERDES_ADDR;
+	u32 actual[NUM_SRDS_BANKS];
+	unsigned int i;
+	u8 sw;
+
+	sw = in_8(&CPLD_SW(2)) >> 2;
+	for (i = 0; i < NUM_SRDS_BANKS; i++) {
+		unsigned int clock = (sw >> (2 * i)) & 3;
+		switch (clock) {
+		case 1:
+			actual[i] = SRDS_PLLCR0_RFCK_SEL_100;
+			break;
+		case 2:
+			actual[i] = SRDS_PLLCR0_RFCK_SEL_125;
+			break;
+		default:
+			printf("Warning: SDREFCLK%u switch setting of '11' is "
+			       "unsupported\n", i + 1);
+			break;
+		}
+	}
+
+	for (i = 0; i < NUM_SRDS_BANKS; i++) {
+		u32 expected = in_be32(&regs->bank[i].pllcr0);
+		expected &= SRDS_PLLCR0_RFCK_SEL_MASK;
+		if (expected != actual[i]) {
+			printf("Warning: SERDES bank %u expects reference clock"
+			       " %sMHz, but actual is %sMHz\n", i + 1,
+			       serdes_clock_to_string(expected),
+			       serdes_clock_to_string(actual[i]));
+		}
+	}
+
+	return 0;
+}
+
+void ft_board_setup(void *blob, bd_t *bd)
+{
+	phys_addr_t base;
+	phys_size_t size;
+
+	ft_cpu_setup(blob, bd);
+
+	base = getenv_bootm_low();
+	size = getenv_bootm_size();
+
+	fdt_fixup_memory(blob, (u64)base, (u64)size);
+
+#ifdef CONFIG_PCI
+	pci_of_setup(blob, bd);
+#endif
+
+	fdt_fixup_liodn(blob);
+}
diff --git a/board/freescale/p2041rdb/pci.c b/board/freescale/p2041rdb/pci.c
new file mode 100644
index 0000000000..1ab4cdf5f8
--- /dev/null
+++ b/board/freescale/p2041rdb/pci.c
@@ -0,0 +1,39 @@
+/*
+ * Copyright 2011 Freescale Semiconductor, Inc.
+ *
+ * See file CREDITS for list of people who contributed to this
+ * project.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation; either version 2 of
+ * the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston,
+ * MA 02111-1307 USA
+ */
+
+#include <common.h>
+#include <command.h>
+#include <pci.h>
+#include <asm/fsl_pci.h>
+#include <libfdt.h>
+#include <fdt_support.h>
+#include <asm/fsl_serdes.h>
+
+void pci_init_board(void)
+{
+	fsl_pcie_init_board(0);
+}
+
+void pci_of_setup(void *blob, bd_t *bd)
+{
+	FT_FSL_PCI_SETUP;
+}
diff --git a/board/freescale/p2041rdb/tlb.c b/board/freescale/p2041rdb/tlb.c
new file mode 100644
index 0000000000..43f28ed14c
--- /dev/null
+++ b/board/freescale/p2041rdb/tlb.c
@@ -0,0 +1,119 @@
+/*
+ * Copyright 2011 Freescale Semiconductor, Inc.
+ *
+ * See file CREDITS for list of people who contributed to this
+ * project.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation; either version 2 of
+ * the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston,
+ * MA 02111-1307 USA
+ */
+
+#include <common.h>
+#include <asm/mmu.h>
+
+struct fsl_e_tlb_entry tlb_table[] = {
+	/* TLB 0 - for temp stack in cache */
+	SET_TLB_ENTRY(0, CONFIG_SYS_INIT_RAM_ADDR,
+		      CONFIG_SYS_INIT_RAM_ADDR_PHYS,
+		      MAS3_SX|MAS3_SW|MAS3_SR, 0,
+		      0, 0, BOOKE_PAGESZ_4K, 0),
+	SET_TLB_ENTRY(0, CONFIG_SYS_INIT_RAM_ADDR + 4 * 1024,
+		      CONFIG_SYS_INIT_RAM_ADDR_PHYS + 4 * 1024,
+		      MAS3_SX|MAS3_SW|MAS3_SR, 0,
+		      0, 0, BOOKE_PAGESZ_4K, 0),
+	SET_TLB_ENTRY(0, CONFIG_SYS_INIT_RAM_ADDR + 8 * 1024,
+		      CONFIG_SYS_INIT_RAM_ADDR_PHYS + 8 * 1024,
+		      MAS3_SX|MAS3_SW|MAS3_SR, 0,
+		      0, 0, BOOKE_PAGESZ_4K, 0),
+	SET_TLB_ENTRY(0, CONFIG_SYS_INIT_RAM_ADDR + 12 * 1024,
+		      CONFIG_SYS_INIT_RAM_ADDR_PHYS + 12 * 1024,
+		      MAS3_SX|MAS3_SW|MAS3_SR, 0,
+		      0, 0, BOOKE_PAGESZ_4K, 0),
+
+	SET_TLB_ENTRY(0, CPLD_BASE, CPLD_BASE_PHYS,
+		      MAS3_SX|MAS3_SW|MAS3_SR, MAS2_I|MAS2_G,
+		      0, 0, BOOKE_PAGESZ_4K, 0),
+
+	/* TLB 1 */
+	/* *I*** - Covers boot page */
+#if defined(CONFIG_SYS_RAMBOOT) && defined(CONFIG_SYS_INIT_L3_ADDR)
+	/*
+	 * *I*G - L3SRAM. When L3 is used as 1M SRAM, the address of the
+	 * SRAM is at 0xfff00000, it covered the 0xfffff000.
+	 */
+	SET_TLB_ENTRY(1, CONFIG_SYS_INIT_L3_ADDR, CONFIG_SYS_INIT_L3_ADDR,
+			MAS3_SX|MAS3_SW|MAS3_SR, MAS2_I|MAS2_G,
+			0, 0, BOOKE_PAGESZ_1M, 1),
+#else
+	SET_TLB_ENTRY(1, 0xfffff000, 0xfffff000,
+		      MAS3_SX|MAS3_SW|MAS3_SR, MAS2_I|MAS2_G,
+		      0, 0, BOOKE_PAGESZ_4K, 1),
+#endif
+
+	/* *I*G* - CCSRBAR */
+	SET_TLB_ENTRY(1, CONFIG_SYS_CCSRBAR, CONFIG_SYS_CCSRBAR_PHYS,
+		      MAS3_SX|MAS3_SW|MAS3_SR, MAS2_I|MAS2_G,
+		      0, 1, BOOKE_PAGESZ_16M, 1),
+
+	/* *I*G* - Flash, localbus */
+	/* This will be changed to *I*G* after relocation to RAM. */
+	SET_TLB_ENTRY(1, CONFIG_SYS_FLASH_BASE, CONFIG_SYS_FLASH_BASE_PHYS,
+		      MAS3_SX|MAS3_SR, MAS2_W|MAS2_G,
+		      0, 2, BOOKE_PAGESZ_256M, 1),
+
+	/* *I*G* - PCI */
+	SET_TLB_ENTRY(1, CONFIG_SYS_PCIE1_MEM_VIRT, CONFIG_SYS_PCIE1_MEM_PHYS,
+		      MAS3_SX|MAS3_SW|MAS3_SR, MAS2_I|MAS2_G,
+		      0, 3, BOOKE_PAGESZ_1G, 1),
+
+	/* *I*G* - PCI */
+	SET_TLB_ENTRY(1, CONFIG_SYS_PCIE1_MEM_VIRT + 0x40000000,
+		      CONFIG_SYS_PCIE1_MEM_PHYS + 0x40000000,
+		      MAS3_SX|MAS3_SW|MAS3_SR, MAS2_I|MAS2_G,
+		      0, 4, BOOKE_PAGESZ_256M, 1),
+
+	SET_TLB_ENTRY(1, CONFIG_SYS_PCIE1_MEM_VIRT + 0x50000000,
+		      CONFIG_SYS_PCIE1_MEM_PHYS + 0x50000000,
+		      MAS3_SX|MAS3_SW|MAS3_SR, MAS2_I|MAS2_G,
+		      0, 5, BOOKE_PAGESZ_256M, 1),
+
+	/* *I*G* - PCI I/O */
+	SET_TLB_ENTRY(1, CONFIG_SYS_PCIE1_IO_VIRT, CONFIG_SYS_PCIE1_IO_PHYS,
+		      MAS3_SX|MAS3_SW|MAS3_SR, MAS2_I|MAS2_G,
+		      0, 6, BOOKE_PAGESZ_256K, 1),
+
+	/* Bman/Qman */
+	SET_TLB_ENTRY(1, CONFIG_SYS_BMAN_MEM_BASE, CONFIG_SYS_BMAN_MEM_PHYS,
+		      MAS3_SX|MAS3_SW|MAS3_SR, 0,
+		      0, 9, BOOKE_PAGESZ_1M, 1),
+	SET_TLB_ENTRY(1, CONFIG_SYS_BMAN_MEM_BASE + 0x00100000,
+		      CONFIG_SYS_BMAN_MEM_PHYS + 0x00100000,
+		      MAS3_SX|MAS3_SW|MAS3_SR, MAS2_I|MAS2_G,
+		      0, 10, BOOKE_PAGESZ_1M, 1),
+	SET_TLB_ENTRY(1, CONFIG_SYS_QMAN_MEM_BASE, CONFIG_SYS_QMAN_MEM_PHYS,
+		      MAS3_SX|MAS3_SW|MAS3_SR, 0,
+		      0, 11, BOOKE_PAGESZ_1M, 1),
+	SET_TLB_ENTRY(1, CONFIG_SYS_QMAN_MEM_BASE + 0x00100000,
+		      CONFIG_SYS_QMAN_MEM_PHYS + 0x00100000,
+		      MAS3_SX|MAS3_SW|MAS3_SR, MAS2_I|MAS2_G,
+		      0, 12, BOOKE_PAGESZ_1M, 1),
+#ifdef CONFIG_SYS_DCSRBAR_PHYS
+	SET_TLB_ENTRY(1, CONFIG_SYS_DCSRBAR, CONFIG_SYS_DCSRBAR_PHYS,
+		      MAS3_SX|MAS3_SW|MAS3_SR, MAS2_I|MAS2_G,
+		      0, 13, BOOKE_PAGESZ_4M, 1),
+#endif
+};
+
+int num_tlb_entries = ARRAY_SIZE(tlb_table);