diff --git a/arch/powerpc/cpu/mpc85xx/Makefile b/arch/powerpc/cpu/mpc85xx/Makefile
index 33e93c8..aad50f3 100644
--- a/arch/powerpc/cpu/mpc85xx/Makefile
+++ b/arch/powerpc/cpu/mpc85xx/Makefile
@@ -66,6 +66,7 @@ COBJS-$(CONFIG_PPC_P2041)	+= ddr-gen3.o
 COBJS-$(CONFIG_PPC_P3041)	+= ddr-gen3.o
 COBJS-$(CONFIG_PPC_P4080)	+= ddr-gen3.o
 COBJS-$(CONFIG_PPC_P5020)	+= ddr-gen3.o
+COBJS-$(CONFIG_PPC_P5040)	+= ddr-gen3.o
 COBJS-$(CONFIG_BSC9131)		+= ddr-gen3.o
 
 COBJS-$(CONFIG_CPM2)	+= ether_fcc.o
@@ -80,6 +81,7 @@ COBJS-$(CONFIG_PPC_P2041) += p2041_ids.o
 COBJS-$(CONFIG_PPC_P3041) += p3041_ids.o
 COBJS-$(CONFIG_PPC_P4080) += p4080_ids.o
 COBJS-$(CONFIG_PPC_P5020) += p5020_ids.o
+COBJS-$(CONFIG_PPC_P5040) += p5040_ids.o
 
 COBJS-$(CONFIG_QE)	+= qe_io.o
 COBJS-$(CONFIG_CPM2)	+= serial_scc.o
@@ -110,6 +112,7 @@ COBJS-$(CONFIG_PPC_P2041) += p2041_serdes.o
 COBJS-$(CONFIG_PPC_P3041) += p3041_serdes.o
 COBJS-$(CONFIG_PPC_P4080) += p4080_serdes.o
 COBJS-$(CONFIG_PPC_P5020) += p5020_serdes.o
+COBJS-$(CONFIG_PPC_P5040) += p5040_serdes.o
 
 COBJS	= $(COBJS-y)
 COBJS	+= cpu.o
diff --git a/arch/powerpc/cpu/mpc85xx/fsl_corenet_serdes.c b/arch/powerpc/cpu/mpc85xx/fsl_corenet_serdes.c
index 2a68060..e6b1b1b 100644
--- a/arch/powerpc/cpu/mpc85xx/fsl_corenet_serdes.c
+++ b/arch/powerpc/cpu/mpc85xx/fsl_corenet_serdes.c
@@ -92,10 +92,17 @@ static const struct {
 	{ 17, 163, FSL_SRDS_BANK_2 },
 	{ 18, 164, FSL_SRDS_BANK_2 },
 	{ 19, 165, FSL_SRDS_BANK_2 },
+#ifdef CONFIG_PPC_P4080
 	{ 20, 170, FSL_SRDS_BANK_3 },
 	{ 21, 171, FSL_SRDS_BANK_3 },
 	{ 22, 172, FSL_SRDS_BANK_3 },
 	{ 23, 173, FSL_SRDS_BANK_3 },
+#else
+	{ 20, 166, FSL_SRDS_BANK_3 },
+	{ 21, 167, FSL_SRDS_BANK_3 },
+	{ 22, 168, FSL_SRDS_BANK_3 },
+	{ 23, 169, FSL_SRDS_BANK_3 },
+#endif
 };
 
 int serdes_get_lane_idx(int lane)
@@ -493,6 +500,9 @@ void fsl_serdes_init(void)
 	ccsr_gur_t *gur = (void *)(CONFIG_SYS_MPC85xx_GUTS_ADDR);
 	int cfg;
 	serdes_corenet_t *srds_regs;
+#ifdef CONFIG_PPC_P5040
+	serdes_corenet_t *srds2_regs;
+#endif
 	int lane, bank, idx;
 	int have_bank[SRDS_MAX_BANK] = {};
 #ifdef CONFIG_SYS_P4080_ERRATUM_SERDES8
@@ -574,6 +584,34 @@ void fsl_serdes_init(void)
 		}
 	}
 
+#ifdef CONFIG_PPC_P5040
+	/*
+	 * Lanes on bank 4 on P5040 are commented-out, but for some SERDES
+	 * protocols, these lanes are routed to SATA.  We use serdes_prtcl_map
+	 * to decide whether a protocol is supported on a given lane, so SATA
+	 * will be identified as not supported, and therefore not initialized.
+	 * So for protocols which use SATA on bank4, we add SATA support in
+	 * serdes_prtcl_map.
+	 */
+	switch (cfg) {
+	case 0x0:
+	case 0x1:
+	case 0x2:
+	case 0x3:
+	case 0x4:
+	case 0x5:
+	case 0x6:
+	case 0x7:
+		serdes_prtcl_map |= 1 << SATA1 | 1 << SATA2;
+		break;
+	default:
+		srds2_regs = (void *)CONFIG_SYS_FSL_CORENET_SERDES2_ADDR;
+
+		/* We don't need bank 4, so power it down */
+		setbits_be32(&srds2_regs->bank[0].rstctl, SRDS_RSTCTL_SDPD);
+	}
+#endif
+
 	soc_serdes_init();
 
 #ifdef CONFIG_SYS_P4080_ERRATUM_SERDES8
@@ -617,6 +655,38 @@ void fsl_serdes_init(void)
 		}
 	}
 
+#ifdef CONFIG_SYS_FSL_ERRATUM_A004699
+	/*
+	 * To avoid the situation that resulted in the P4080 erratum
+	 * SERDES-8, a given SerDes bank will use the PLLs from the previous
+	 * bank if one of the PLL frequencies is a multiple of the other.  For
+	 * instance, if bank 3 is running at 2.5GHz and bank 2 is at 1.25GHz,
+	 * then bank 3 will use bank 2's PLL.  P5040 Erratum A-004699 says
+	 * that, in this situation, lane synchronization is not initiated.  So
+	 * when we detect a bank with a "borrowed" PLL, we have to manually
+	 * initiate lane synchronization.
+	 */
+	for (bank = FSL_SRDS_BANK_2; bank <= FSL_SRDS_BANK_3; bank++) {
+		/* Determine the first lane for this bank */
+		unsigned int lane;
+
+		for (lane = 0; lane < SRDS_MAX_LANES; lane++)
+			if (lanes[lane].bank == bank)
+				break;
+		idx = lanes[lane].idx;
+
+		/*
+		 * Check if the PLL for the bank is borrowed.  The UOTHL
+		 * bit of the first lane will tell us that.
+		 */
+		if (in_be32(&srds_regs->lane[idx].gcr0) & SRDS_GCR0_UOTHL) {
+			/* Manually start lane synchronization */
+			setbits_be32(&srds_regs->bank[bank].pllcr0,
+				     SRDS_PLLCR0_PVCOCNT_EN);
+		}
+	}
+#endif
+
 #if defined(CONFIG_SYS_P4080_ERRATUM_SERDES8) || defined (CONFIG_SYS_P4080_ERRATUM_SERDES9)
 	for (lane = 0; lane < SRDS_MAX_LANES; lane++) {
 		enum srds_prtcl lane_prtcl;
diff --git a/arch/powerpc/cpu/mpc85xx/fsl_corenet_serdes.h b/arch/powerpc/cpu/mpc85xx/fsl_corenet_serdes.h
index f261351..c82060d 100644
--- a/arch/powerpc/cpu/mpc85xx/fsl_corenet_serdes.h
+++ b/arch/powerpc/cpu/mpc85xx/fsl_corenet_serdes.h
@@ -22,6 +22,11 @@
 #ifndef __FSL_CORENET_SERDES_H
 #define __FSL_CORENET_SERDES_H
 
+/*
+ * Note: For P5040, the fourth SerDes bank is on SerDes2, but U-boot currently
+ * only supports one SerDes controller.  For now, pretend that we have three
+ * banks and 18 lanes on the P5040.
+ */
 #define SRDS_MAX_LANES		18
 #define SRDS_MAX_BANK		3
 
diff --git a/arch/powerpc/cpu/mpc85xx/p5040_ids.c b/arch/powerpc/cpu/mpc85xx/p5040_ids.c
new file mode 100644
index 0000000..878ee3e
--- /dev/null
+++ b/arch/powerpc/cpu/mpc85xx/p5040_ids.c
@@ -0,0 +1,127 @@
+/*
+ * Copyright 2010-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_portals.h>
+#include <asm/fsl_liodn.h>
+
+#ifdef CONFIG_SYS_DPAA_QBMAN
+struct qportal_info qp_info[CONFIG_SYS_QMAN_NUM_PORTALS] = {
+	/* dqrr liodn, frame data liodn, liodn off, sdest */
+	SET_QP_INFO(1, 2, 1, 0),
+	SET_QP_INFO(3, 4, 2, 1),
+	SET_QP_INFO(5, 6, 3, 2),
+	SET_QP_INFO(7, 8, 4, 3),
+	SET_QP_INFO(9, 10, 5, 0),
+	SET_QP_INFO(11, 12, 6, 1),
+	SET_QP_INFO(13, 14, 7, 2),
+	SET_QP_INFO(15, 16, 8, 3),
+	SET_QP_INFO(17, 18, 9, 0),	/* for now, set sdest to 0 */
+	SET_QP_INFO(19, 20, 10, 0),	/* for now, set sdest to 0 */
+};
+#endif
+
+struct liodn_id_table liodn_tbl[] = {
+#ifdef CONFIG_SYS_DPAA_QBMAN
+	SET_QMAN_LIODN(31),
+	SET_BMAN_LIODN(32),
+#endif
+
+	SET_SDHC_LIODN(1, 64),
+
+	SET_USB_LIODN(1, "fsl-usb2-mph", 93),
+	SET_USB_LIODN(2, "fsl-usb2-dr", 94),
+
+	SET_SATA_LIODN(1, 95),
+	SET_SATA_LIODN(2, 96),
+
+	SET_PCI_LIODN_BASE(CONFIG_SYS_FSL_PCIE_COMPAT, 1, 195),
+	SET_PCI_LIODN_BASE(CONFIG_SYS_FSL_PCIE_COMPAT, 2, 196),
+	SET_PCI_LIODN_BASE(CONFIG_SYS_FSL_PCIE_COMPAT, 3, 197),
+
+	SET_DMA_LIODN(1, 193),
+	SET_DMA_LIODN(2, 194),
+};
+int liodn_tbl_sz = ARRAY_SIZE(liodn_tbl);
+
+#ifdef CONFIG_SYS_DPAA_FMAN
+struct liodn_id_table fman1_liodn_tbl[] = {
+	SET_FMAN_RX_1G_LIODN(1, 0, 6),
+	SET_FMAN_RX_1G_LIODN(1, 1, 7),
+	SET_FMAN_RX_1G_LIODN(1, 2, 8),
+	SET_FMAN_RX_1G_LIODN(1, 3, 9),
+	SET_FMAN_RX_1G_LIODN(1, 4, 10),
+	SET_FMAN_RX_10G_LIODN(1, 0, 11),
+};
+int fman1_liodn_tbl_sz = ARRAY_SIZE(fman1_liodn_tbl);
+
+#if (CONFIG_SYS_NUM_FMAN == 2)
+struct liodn_id_table fman2_liodn_tbl[] = {
+	SET_FMAN_RX_1G_LIODN(2, 0, 12),
+	SET_FMAN_RX_1G_LIODN(2, 1, 13),
+	SET_FMAN_RX_1G_LIODN(2, 2, 14),
+	SET_FMAN_RX_1G_LIODN(2, 3, 15),
+	SET_FMAN_RX_1G_LIODN(2, 4, 16),
+	SET_FMAN_RX_10G_LIODN(2, 0, 17),
+};
+int fman2_liodn_tbl_sz = ARRAY_SIZE(fman2_liodn_tbl);
+#endif
+#endif
+
+struct liodn_id_table sec_liodn_tbl[] = {
+	SET_SEC_JR_LIODN_ENTRY(0, 129, 130),
+	SET_SEC_JR_LIODN_ENTRY(1, 131, 132),
+	SET_SEC_JR_LIODN_ENTRY(2, 133, 134),
+	SET_SEC_JR_LIODN_ENTRY(3, 135, 136),
+	SET_SEC_RTIC_LIODN_ENTRY(a, 89),
+	SET_SEC_RTIC_LIODN_ENTRY(b, 90),
+	SET_SEC_RTIC_LIODN_ENTRY(c, 91),
+	SET_SEC_RTIC_LIODN_ENTRY(d, 92),
+	SET_SEC_DECO_LIODN_ENTRY(0, 139, 140),
+	SET_SEC_DECO_LIODN_ENTRY(1, 141, 142),
+	SET_SEC_DECO_LIODN_ENTRY(2, 143, 144),
+	SET_SEC_DECO_LIODN_ENTRY(3, 145, 146),
+};
+int sec_liodn_tbl_sz = ARRAY_SIZE(sec_liodn_tbl);
+
+#ifdef CONFIG_SYS_FSL_RAID_ENGINE
+struct liodn_id_table raide_liodn_tbl[] = {
+	SET_RAID_ENGINE_JQ_LIODN_ENTRY(0, 0, 60),
+	SET_RAID_ENGINE_JQ_LIODN_ENTRY(0, 1, 61),
+	SET_RAID_ENGINE_JQ_LIODN_ENTRY(1, 0, 62),
+	SET_RAID_ENGINE_JQ_LIODN_ENTRY(1, 1, 63),
+};
+int raide_liodn_tbl_sz = ARRAY_SIZE(raide_liodn_tbl);
+#endif
+
+struct liodn_id_table liodn_bases[] = {
+	[FSL_HW_PORTAL_SEC]  = SET_LIODN_BASE_2(64, 101),
+#ifdef CONFIG_SYS_DPAA_FMAN
+	[FSL_HW_PORTAL_FMAN1] = SET_LIODN_BASE_1(32),
+#endif
+#if (CONFIG_SYS_NUM_FMAN == 2)
+	[FSL_HW_PORTAL_FMAN2] = SET_LIODN_BASE_1(160),
+#endif
+#ifdef CONFIG_SYS_FSL_RAID_ENGINE
+	[FSL_HW_PORTAL_RAID_ENGINE]  = SET_LIODN_BASE_1(49),
+#endif
+};
diff --git a/arch/powerpc/cpu/mpc85xx/p5040_serdes.c b/arch/powerpc/cpu/mpc85xx/p5040_serdes.c
new file mode 100644
index 0000000..890b88e
--- /dev/null
+++ b/arch/powerpc/cpu/mpc85xx/p5040_serdes.c
@@ -0,0 +1,117 @@
+/*
+ * Copyright 2009-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_serdes.h>
+#include <asm/processor.h>
+#include <asm/io.h>
+#include "fsl_corenet_serdes.h"
+
+/*
+ * Note: For P5040, the fourth SerDes bank (with two lanes) is on SerDes2, but
+ * U-boot only supports one SerDes controller.  Therefore, we ignore bank 4 in
+ * this table.  This works because most of the SerDes code is for errata
+ * work-arounds, and there are no P5040 errata that effect bank 4.
+ */
+
+static u8 serdes_cfg_tbl[][SRDS_MAX_LANES] = {
+	[0x00] = {PCIE1, PCIE1, PCIE1, PCIE1, PCIE2, PCIE2, PCIE2, PCIE2,
+		SGMII_FM2_DTSEC3, SGMII_FM2_DTSEC4, SGMII_FM1_DTSEC1,
+		SGMII_FM1_DTSEC2, SGMII_FM1_DTSEC3, SGMII_FM1_DTSEC4,
+		XAUI_FM2, XAUI_FM2, XAUI_FM2, XAUI_FM2, /* SATA1, SATA2, */ },
+	[0x01] = {PCIE1, PCIE1, PCIE1, PCIE1, PCIE2, PCIE2, PCIE2, PCIE2,
+		SGMII_FM1_DTSEC3, SGMII_FM2_DTSEC4, XAUI_FM1, XAUI_FM1,
+		XAUI_FM1, XAUI_FM1, XAUI_FM2, XAUI_FM2, XAUI_FM2,
+		XAUI_FM2, /* SATA1, SATA2 */ },
+	[0x02] = {PCIE1, PCIE1, PCIE1, PCIE1, PCIE2, PCIE2, SGMII_FM1_DTSEC3,
+		SGMII_FM1_DTSEC4, SGMII_FM2_DTSEC3, SGMII_FM2_DTSEC4,
+		XAUI_FM1, XAUI_FM1, XAUI_FM1, XAUI_FM1, XAUI_FM2, XAUI_FM2,
+		XAUI_FM2, XAUI_FM2, /* SATA1, SATA2 */ },
+	[0x03] = {PCIE1, PCIE1, PCIE1, PCIE1, PCIE2, PCIE2, SGMII_FM2_DTSEC1,
+		SGMII_FM2_DTSEC2, SGMII_FM2_DTSEC3, SGMII_FM2_DTSEC4,
+		SGMII_FM1_DTSEC1, SGMII_FM1_DTSEC2, SGMII_FM1_DTSEC3,
+		SGMII_FM1_DTSEC4, XAUI_FM2, XAUI_FM2, XAUI_FM2, XAUI_FM2,
+		/* SATA1, SATA2 */ },
+	[0x04] = {PCIE1, PCIE1, PCIE1, PCIE1, PCIE2, PCIE3, SGMII_FM2_DTSEC1,
+		SGMII_FM2_DTSEC2, SGMII_FM2_DTSEC3, SGMII_FM2_DTSEC4,
+		SGMII_FM1_DTSEC1, SGMII_FM1_DTSEC2, SGMII_FM1_DTSEC3,
+		SGMII_FM1_DTSEC4, XAUI_FM2, XAUI_FM2, XAUI_FM2, XAUI_FM2,
+		/* SATA1, SATA2 */ },
+	[0x05] = {PCIE1, PCIE1, PCIE1, PCIE1, PCIE2, PCIE3, SGMII_FM1_DTSEC3,
+		SGMII_FM1_DTSEC4, SGMII_FM2_DTSEC3, SGMII_FM2_DTSEC4,
+		XAUI_FM1, XAUI_FM1, XAUI_FM1, XAUI_FM1, XAUI_FM2, XAUI_FM2,
+		XAUI_FM2, XAUI_FM2, /* SATA1, SATA2 */ },
+	[0x06] = {PCIE1, PCIE1, PCIE1, PCIE1, PCIE1, PCIE1, PCIE1, PCIE1,
+		SGMII_FM2_DTSEC3, SGMII_FM2_DTSEC4, SGMII_FM1_DTSEC1,
+		SGMII_FM1_DTSEC2, SGMII_FM1_DTSEC3, SGMII_FM1_DTSEC4,
+		XAUI_FM2, XAUI_FM2, XAUI_FM2, XAUI_FM2, /* SATA1, SATA2 */ },
+	[0x07] = {PCIE1, PCIE1, PCIE1, PCIE1, PCIE1, PCIE1, PCIE1, PCIE1,
+		SGMII_FM1_DTSEC3, SGMII_FM2_DTSEC4, XAUI_FM1, XAUI_FM1,
+		XAUI_FM1, XAUI_FM1, XAUI_FM2, XAUI_FM2, XAUI_FM2,
+		XAUI_FM2, /* SATA1, SATA2 */ },
+	[0x11] = {PCIE1, PCIE1, PCIE1, PCIE1, PCIE2, PCIE2, PCIE2, PCIE2,
+		AURORA, AURORA, SGMII_FM1_DTSEC1, SGMII_FM1_DTSEC2,
+		SGMII_FM1_DTSEC3, SGMII_FM1_DTSEC4, NONE, NONE, SATA1, SATA2,
+		/* NONE, NONE */ },
+	[0x15] = {PCIE1, PCIE1, PCIE1, PCIE1, PCIE2, PCIE2, PCIE2, PCIE2,
+		AURORA, AURORA, XAUI_FM1, XAUI_FM1, XAUI_FM1, XAUI_FM1,
+		NONE, NONE, SATA1, SATA2, /* NONE, NONE */ },
+	[0x2a] = {PCIE1, PCIE1, PCIE3, PCIE3, PCIE2, PCIE2, PCIE2, PCIE2,
+		AURORA, AURORA, SGMII_FM1_DTSEC1, SGMII_FM1_DTSEC2,
+		SGMII_FM1_DTSEC3, SGMII_FM1_DTSEC4, XAUI_FM2, XAUI_FM2,
+		XAUI_FM2, XAUI_FM2, /* NONE, NONE */ },
+	[0x34] = {PCIE1, PCIE1, PCIE1, PCIE1, SGMII_FM1_DTSEC1,
+		SGMII_FM1_DTSEC2, SGMII_FM1_DTSEC3, SGMII_FM1_DTSEC4, AURORA,
+		AURORA, XAUI_FM1, XAUI_FM1, XAUI_FM1, XAUI_FM1, NONE,
+		NONE, SATA1, SATA2, /* NONE, NONE */ },
+	[0x35] = {PCIE1, PCIE1, PCIE1, PCIE1, PCIE2, PCIE2,
+		SGMII_FM1_DTSEC3, SGMII_FM1_DTSEC4, AURORA, AURORA, XAUI_FM1,
+		XAUI_FM1, XAUI_FM1, XAUI_FM1, NONE, NONE, SATA1, SATA2,
+		/* NONE, NONE */ },
+	[0x36] = {PCIE1, PCIE1, PCIE3, PCIE3, SGMII_FM1_DTSEC1,
+		SGMII_FM1_DTSEC2, SGMII_FM1_DTSEC3, SGMII_FM1_DTSEC4, AURORA,
+		AURORA, XAUI_FM1, XAUI_FM1, XAUI_FM1, XAUI_FM1, NONE,
+		NONE, SATA1, SATA2, /* NONE, NONE */ },
+};
+
+enum srds_prtcl serdes_get_prtcl(int cfg, int lane)
+{
+	if (!serdes_lane_enabled(lane))
+		return NONE;
+
+	return serdes_cfg_tbl[cfg][lane];
+}
+
+int is_serdes_prtcl_valid(u32 prtcl)
+{
+	int i;
+
+	if (prtcl > ARRAY_SIZE(serdes_cfg_tbl))
+		return 0;
+
+	for (i = 0; i < SRDS_MAX_LANES; i++) {
+		if (serdes_cfg_tbl[prtcl][i] != NONE)
+			return 1;
+	}
+
+	return 0;
+}
diff --git a/arch/powerpc/cpu/mpc85xx/speed.c b/arch/powerpc/cpu/mpc85xx/speed.c
index abfeb26..6dd9e8d 100644
--- a/arch/powerpc/cpu/mpc85xx/speed.c
+++ b/arch/powerpc/cpu/mpc85xx/speed.c
@@ -112,6 +112,8 @@ void get_sys_info (sys_info_t * sysInfo)
 #define HWA_ASYNC_DIV	0x04000000
 #if (CONFIG_SYS_FSL_NUM_CC_PLLS == 2)
 #define HWA_CC_PLL	1
+#elif (CONFIG_SYS_FSL_NUM_CC_PLLS == 3)
+#define HWA_CC_PLL	2
 #elif (CONFIG_SYS_FSL_NUM_CC_PLLS == 4)
 #define HWA_CC_PLL	2
 #else
diff --git a/arch/powerpc/cpu/mpc8xxx/cpu.c b/arch/powerpc/cpu/mpc8xxx/cpu.c
index 78a8f92..fb78784 100644
--- a/arch/powerpc/cpu/mpc8xxx/cpu.c
+++ b/arch/powerpc/cpu/mpc8xxx/cpu.c
@@ -73,6 +73,8 @@ struct cpu_type cpu_type_list [] = {
 	CPU_TYPE_ENTRY(P4080, P4080, 8),
 	CPU_TYPE_ENTRY(P5010, P5010, 1),
 	CPU_TYPE_ENTRY(P5020, P5020, 2),
+	CPU_TYPE_ENTRY(P5021, P5021, 2),
+	CPU_TYPE_ENTRY(P5040, P5040, 4),
 	CPU_TYPE_ENTRY(BSC9130, 9130, 1),
 	CPU_TYPE_ENTRY(BSC9131, 9131, 1),
 #elif defined(CONFIG_MPC86xx)
diff --git a/arch/powerpc/include/asm/config_mpc85xx.h b/arch/powerpc/include/asm/config_mpc85xx.h
index aa27741..f8a1c99 100644
--- a/arch/powerpc/include/asm/config_mpc85xx.h
+++ b/arch/powerpc/include/asm/config_mpc85xx.h
@@ -433,6 +433,35 @@
 #define CONFIG_SYS_FSL_ERRATUM_A004510_SVR_REV	0x10
 #define CONFIG_SYS_FSL_CORENET_SNOOPVEC_COREONLY 0xc0000000
 
+#elif defined(CONFIG_PPC_P5040)
+#define CONFIG_SYS_FSL_QORIQ_CHASSIS1
+#define CONFIG_MAX_CPUS			4
+#define CONFIG_SYS_FSL_NUM_CC_PLLS	3
+#define CONFIG_SYS_FSL_NUM_LAWS		32
+#define CONFIG_SYS_FSL_SEC_COMPAT	4
+#define CONFIG_SYS_NUM_FMAN		2
+#define CONFIG_SYS_NUM_FM1_DTSEC	5
+#define CONFIG_SYS_NUM_FM1_10GEC	1
+#define CONFIG_SYS_NUM_FM2_DTSEC	5
+#define CONFIG_SYS_NUM_FM2_10GEC	1
+#define CONFIG_NUM_DDR_CONTROLLERS	2
+#define CONFIG_SYS_FM_MURAM_SIZE	0x28000
+#define CONFIG_SYS_FSL_TBCLK_DIV	16
+#define CONFIG_SYS_FSL_PCIE_COMPAT	"fsl,qoriq-pcie-v2.4"
+#define CONFIG_SYS_CCSRBAR_DEFAULT	0xfe000000
+#define CONFIG_SYS_FSL_USB1_PHY_ENABLE
+#define CONFIG_SYS_FSL_USB2_PHY_ENABLE
+#define CONFIG_SYS_FSL_USB_INTERNAL_UTMI_PHY
+#define CONFIG_SYS_FSL_ERRATUM_ESDHC111
+#define CONFIG_SYS_FSL_ERRATUM_USB138
+#define CONFIG_SYS_FSL_ERRATUM_DDR_A003
+#define CONFIG_SYS_FSL_ERRATUM_DDR_A003474
+#define CONFIG_SYS_FSL_ERRATUM_A004699
+#define CONFIG_SYS_FSL_ELBC_MULTIBIT_ECC
+#define CONFIG_SYS_FSL_ERRATUM_A004510
+#define CONFIG_SYS_FSL_ERRATUM_A004510_SVR_REV	0x10
+#define CONFIG_SYS_FSL_CORENET_SNOOPVEC_COREONLY 0xf0000000
+
 #elif defined(CONFIG_BSC9131)
 #define CONFIG_MAX_CPUS			1
 #define CONFIG_FSL_SDHC_V2_3
diff --git a/arch/powerpc/include/asm/immap_85xx.h b/arch/powerpc/include/asm/immap_85xx.h
index 6c11178..7c35b41 100644
--- a/arch/powerpc/include/asm/immap_85xx.h
+++ b/arch/powerpc/include/asm/immap_85xx.h
@@ -1761,6 +1761,7 @@ typedef struct ccsr_gur {
 #define FSL_CORENET_RCWSR5_DDR_SYNC		0x00000080
 #define FSL_CORENET_RCWSR5_DDR_SYNC_SHIFT		 7
 #define FSL_CORENET_RCWSR5_SRDS_EN		0x00002000
+#define FSL_CORENET_RCWSR5_SRDS2_EN		0x00001000
 #define FSL_CORENET_RCWSR6_BOOT_LOC	0x0f800000
 #define FSL_CORENET_RCWSRn_SRDS_LPD_B2		0x3c000000 /* bits 162..165 */
 #define FSL_CORENET_RCWSRn_SRDS_LPD_B3		0x003c0000 /* bits 170..173 */
@@ -1786,6 +1787,15 @@ typedef struct ccsr_gur {
 #define FSL_CORENET_RCWSR11_EC2_FM1_DTSEC5_MII		0x00100000
 #define FSL_CORENET_RCWSR11_EC2_FM1_DTSEC5_NONE		0x00180000
 #endif
+#if defined(CONFIG_PPC_P5040)
+#define FSL_CORENET_RCWSR11_EC1_FM1_DTSEC5_RGMII        0x00000000
+#define FSL_CORENET_RCWSR11_EC1_FM1_DTSEC5_MII          0x00800000
+#define FSL_CORENET_RCWSR11_EC1_FM1_DTSEC5_NONE         0x00c00000
+#define FSL_CORENET_RCWSR11_EC2                 0x00180000 /* bits 363..364 */
+#define FSL_CORENET_RCWSR11_EC2_FM2_DTSEC5_RGMII        0x00000000
+#define FSL_CORENET_RCWSR11_EC2_FM2_DTSEC5_MII          0x00100000
+#define FSL_CORENET_RCWSR11_EC2_FM2_DTSEC5_NONE         0x00180000
+#endif
 	u8	res18[192];
 	u32	scratchrw[4];	/* Scratch Read/Write */
 	u8	res19[240];
@@ -2395,6 +2405,7 @@ typedef struct serdes_corenet {
 #define SRDS_RSTCTL_SDPD	0x00000020
 		u32	pllcr0; /* PLL Control Register 0 */
 #define SRDS_PLLCR0_RFCK_SEL_MASK	0x30000000
+#define SRDS_PLLCR0_PVCOCNT_EN		0x02000000
 #define SRDS_PLLCR0_RFCK_SEL_100	0x00000000
 #define SRDS_PLLCR0_RFCK_SEL_125	0x10000000
 #define SRDS_PLLCR0_RFCK_SEL_156_25	0x20000000
@@ -2423,6 +2434,7 @@ typedef struct serdes_corenet {
 		u32	gcr0;	/* General Control Register 0 */
 #define SRDS_GCR0_RRST			0x00400000
 #define SRDS_GCR0_1STLANE		0x00010000
+#define SRDS_GCR0_UOTHL			0x00100000
 		u32	gcr1;	/* General Control Register 1 */
 #define SRDS_GCR1_REIDL_CTL_MASK	0x001f0000
 #define SRDS_GCR1_REIDL_CTL_PCIE	0x00100000
@@ -2627,6 +2639,7 @@ struct ccsr_rman {
 #define CONFIG_SYS_FSL_CORENET_CLK_OFFSET	0xE1000
 #define CONFIG_SYS_FSL_CORENET_RCPM_OFFSET	0xE2000
 #define CONFIG_SYS_FSL_CORENET_SERDES_OFFSET	0xEA000
+#define CONFIG_SYS_FSL_CORENET_SERDES2_OFFSET	0xEB000
 #define CONFIG_SYS_FSL_CPC_OFFSET		0x10000
 #define CONFIG_SYS_MPC85xx_DMA1_OFFSET		0x100000
 #define CONFIG_SYS_MPC85xx_DMA2_OFFSET		0x101000
@@ -2777,6 +2790,8 @@ struct ccsr_rman {
 	(CONFIG_SYS_IMMR + CONFIG_SYS_MPC85xx_SERDES2_OFFSET)
 #define CONFIG_SYS_FSL_CORENET_SERDES_ADDR \
 	(CONFIG_SYS_IMMR + CONFIG_SYS_FSL_CORENET_SERDES_OFFSET)
+#define CONFIG_SYS_FSL_CORENET_SERDES2_ADDR \
+	(CONFIG_SYS_IMMR + CONFIG_SYS_FSL_CORENET_SERDES2_OFFSET)
 #define CONFIG_SYS_MPC85xx_USB_ADDR \
 	(CONFIG_SYS_IMMR + CONFIG_SYS_MPC85xx_USB_OFFSET)
 #define CONFIG_SYS_MPC85xx_USB1_PHY_ADDR \
diff --git a/arch/powerpc/include/asm/processor.h b/arch/powerpc/include/asm/processor.h
index 36695e2..fd0160a 100644
--- a/arch/powerpc/include/asm/processor.h
+++ b/arch/powerpc/include/asm/processor.h
@@ -1095,6 +1095,8 @@
 #define SVR_P4080	0x820000
 #define SVR_P5010	0x822100
 #define SVR_P5020	0x822000
+#define SVR_P5021	0X820500
+#define SVR_P5040	0x820400
 
 #define SVR_8610	0x80A000
 #define SVR_8641	0x809000