Merge branch 'master' of git://git.denx.de/u-boot-mpc85xx

14 files changed, 878 insertions, 84 deletions

diff --git a/board/freescale/t1040qds/Makefile b/board/freescale/t1040qds/Makefile
index 93af9eb..c7470d7 100644
--- a/board/freescale/t1040qds/Makefile
+++ b/board/freescale/t1040qds/Makefile
@@ -9,3 +9,4 @@ obj-y	+= ddr.o
 obj-$(CONFIG_PCI)     += pci.o
 obj-y	+= law.o
 obj-y	+= tlb.o
+obj-y	+= eth.o
diff --git a/board/freescale/t1040qds/README b/board/freescale/t1040qds/README
index 6d380ae..8160ca0 100644
--- a/board/freescale/t1040qds/README
+++ b/board/freescale/t1040qds/README
@@ -121,14 +121,14 @@ NOR Flash memory Map on T1040QDS
 0xEFF40000      0xEFFFFFFF      u-boot (current bank)            768KB
 0xEFF20000      0xEFF3FFFF      u-boot env (current bank)        128KB
 0xEFF00000      0xEFF1FFFF      FMAN Ucode (current bank)        128KB
-0xED300000      0xEFF3FFFF      rootfs (alt bank)                44MB + 256KB
-0xEC800000      0xEC8FFFF       Hardware device tree (alt bank)  1MB
+0xED300000      0xEFEFFFFF      rootfs (alt bank)                44MB
+0xEC800000      0xEC8FFFFF      Hardware device tree (alt bank)  1MB
 0xEC020000      0xEC7FFFFF      Linux.uImage (alt bank)          7MB + 875KB
 0xEC000000      0xEC01FFFF      RCW (alt bank)                   128KB
 0xEBF40000      0xEBFFFFFF      u-boot (alt bank)                768KB
 0xEBF20000      0xEBF3FFFF      u-boot env (alt bank)            128KB
 0xEBF00000      0xEBF1FFFF      FMAN ucode (alt bank)            128KB
-0xE9300000      0xEBF3FFFF      rootfs (current bank)            44MB + 256KB
+0xE9300000      0xEBEFFFFF      rootfs (current bank)            44MB
 0xE8800000      0xE88FFFFF      Hardware device tree (cur bank)  11MB + 512KB
 0xE8020000      0xE86FFFFF      Linux.uImage (current bank)      7MB + 875KB
 0xE8000000      0xE801FFFF      RCW (current bank)               128KB
diff --git a/board/freescale/t1040qds/eth.c b/board/freescale/t1040qds/eth.c
new file mode 100644
index 0000000..3077b4a
--- /dev/null
+++ b/board/freescale/t1040qds/eth.c
@@ -0,0 +1,492 @@
+/*
+ * Copyright 2013 Freescale Semiconductor, Inc.
+ *
+ * SPDX-License-Identifier:	GPL-2.0+
+ */
+
+/*
+ * The RGMII PHYs are provided by the two on-board PHY connected to
+ * dTSEC instances 4 and 5. The SGMII PHYs are provided by one on-board
+ * PHY or by the standard four-port SGMII riser card (VSC).
+ */
+
+#include <common.h>
+#include <netdev.h>
+#include <asm/fsl_serdes.h>
+#include <asm/immap_85xx.h>
+#include <fm_eth.h>
+#include <fsl_mdio.h>
+#include <malloc.h>
+#include <asm/fsl_dtsec.h>
+
+#include "../common/fman.h"
+#include "../common/qixis.h"
+
+#include "t1040qds_qixis.h"
+
+#ifdef CONFIG_FMAN_ENET
+ /* - In T1040 there are only 8 SERDES lanes, spread across 2 SERDES banks.
+ *   Bank 1 -> Lanes A, B, C, D
+ *   Bank 2 -> Lanes E, F, G, H
+ */
+
+ /* Mapping of 8 SERDES lanes to T1040 QDS board slots. A value of '0' here
+  * means that the mapping must be determined dynamically, or that the lane
+  * maps to something other than a board slot.
+  */
+static u8 lane_to_slot[] = {
+	0, 0, 0, 0, 0, 0, 0, 0
+};
+
+/* On the Vitesse VSC8234XHG SGMII riser card there are 4 SGMII PHYs
+ * housed.
+ */
+static int riser_phy_addr[] = {
+	CONFIG_SYS_FM1_DTSEC1_RISER_PHY_ADDR,
+	CONFIG_SYS_FM1_DTSEC2_RISER_PHY_ADDR,
+	CONFIG_SYS_FM1_DTSEC3_RISER_PHY_ADDR,
+	CONFIG_SYS_FM1_DTSEC4_RISER_PHY_ADDR,
+};
+
+/* Slot2 does not have EMI connections */
+#define EMI_NONE	0xFFFFFFFF
+#define EMI1_RGMII0	0
+#define EMI1_RGMII1	1
+#define EMI1_SLOT1	2
+#define EMI1_SLOT3	3
+#define EMI1_SLOT4	4
+#define EMI1_SLOT5	5
+#define EMI1_SLOT6	6
+#define EMI1_SLOT7	7
+#define EMI2		8
+
+static int mdio_mux[NUM_FM_PORTS];
+
+static const char * const mdio_names[] = {
+	"T1040_QDS_MDIO0",
+	"T1040_QDS_MDIO1",
+	"T1040_QDS_MDIO2",
+	"T1040_QDS_MDIO3",
+	"T1040_QDS_MDIO4",
+	"T1040_QDS_MDIO5",
+	"T1040_QDS_MDIO6",
+	"T1040_QDS_MDIO7",
+};
+
+struct t1040_qds_mdio {
+	u8 muxval;
+	struct mii_dev *realbus;
+};
+
+static const char *t1040_qds_mdio_name_for_muxval(u8 muxval)
+{
+	return mdio_names[muxval];
+}
+
+struct mii_dev *mii_dev_for_muxval(u8 muxval)
+{
+	struct mii_dev *bus;
+	const char *name = t1040_qds_mdio_name_for_muxval(muxval);
+
+	if (!name) {
+		printf("No bus for muxval %x\n", muxval);
+		return NULL;
+	}
+
+	bus = miiphy_get_dev_by_name(name);
+
+	if (!bus) {
+		printf("No bus by name %s\n", name);
+		return NULL;
+	}
+
+	return bus;
+}
+
+static void t1040_qds_mux_mdio(u8 muxval)
+{
+	u8 brdcfg4;
+	if (muxval <= 7) {
+		brdcfg4 = QIXIS_READ(brdcfg[4]);
+		brdcfg4 &= ~BRDCFG4_EMISEL_MASK;
+		brdcfg4 |= (muxval << BRDCFG4_EMISEL_SHIFT);
+		QIXIS_WRITE(brdcfg[4], brdcfg4);
+	}
+}
+
+static int t1040_qds_mdio_read(struct mii_dev *bus, int addr, int devad,
+				int regnum)
+{
+	struct t1040_qds_mdio *priv = bus->priv;
+
+	t1040_qds_mux_mdio(priv->muxval);
+
+	return priv->realbus->read(priv->realbus, addr, devad, regnum);
+}
+
+static int t1040_qds_mdio_write(struct mii_dev *bus, int addr, int devad,
+				int regnum, u16 value)
+{
+	struct t1040_qds_mdio *priv = bus->priv;
+
+	t1040_qds_mux_mdio(priv->muxval);
+
+	return priv->realbus->write(priv->realbus, addr, devad, regnum, value);
+}
+
+static int t1040_qds_mdio_reset(struct mii_dev *bus)
+{
+	struct t1040_qds_mdio *priv = bus->priv;
+
+	return priv->realbus->reset(priv->realbus);
+}
+
+static int t1040_qds_mdio_init(char *realbusname, u8 muxval)
+{
+	struct t1040_qds_mdio *pmdio;
+	struct mii_dev *bus = mdio_alloc();
+
+	if (!bus) {
+		printf("Failed to allocate t1040_qds MDIO bus\n");
+		return -1;
+	}
+
+	pmdio = malloc(sizeof(*pmdio));
+	if (!pmdio) {
+		printf("Failed to allocate t1040_qds private data\n");
+		free(bus);
+		return -1;
+	}
+
+	bus->read = t1040_qds_mdio_read;
+	bus->write = t1040_qds_mdio_write;
+	bus->reset = t1040_qds_mdio_reset;
+	sprintf(bus->name, t1040_qds_mdio_name_for_muxval(muxval));
+
+	pmdio->realbus = miiphy_get_dev_by_name(realbusname);
+
+	if (!pmdio->realbus) {
+		printf("No bus with name %s\n", realbusname);
+		free(bus);
+		free(pmdio);
+		return -1;
+	}
+
+	pmdio->muxval = muxval;
+	bus->priv = pmdio;
+
+	return mdio_register(bus);
+}
+
+/*
+ * Initialize the lane_to_slot[] array.
+ *
+ * On the T1040QDS board the mapping is controlled by ?? register.
+ */
+static void initialize_lane_to_slot(void)
+{
+	ccsr_gur_t *gur = (void *)CONFIG_SYS_MPC85xx_GUTS_ADDR;
+	int serdes1_prtcl = (in_be32(&gur->rcwsr[4]) &
+				FSL_CORENET2_RCWSR4_SRDS1_PRTCL)
+		>> FSL_CORENET2_RCWSR4_SRDS1_PRTCL_SHIFT;
+
+	QIXIS_WRITE(cms[0], 0x07);
+
+	switch (serdes1_prtcl) {
+	case 0x60:
+	case 0x66:
+	case 0x67:
+	case 0x69:
+		lane_to_slot[1] = 7;
+		lane_to_slot[2] = 6;
+		lane_to_slot[3] = 5;
+		break;
+	case 0x86:
+		lane_to_slot[1] = 7;
+		lane_to_slot[2] = 7;
+		lane_to_slot[3] = 7;
+		break;
+	case 0x87:
+		lane_to_slot[1] = 7;
+		lane_to_slot[2] = 7;
+		lane_to_slot[3] = 7;
+		lane_to_slot[7] = 7;
+		break;
+	case 0x89:
+		lane_to_slot[1] = 7;
+		lane_to_slot[2] = 7;
+		lane_to_slot[3] = 7;
+		lane_to_slot[7] = 7;
+		break;
+	case 0x8d:
+		lane_to_slot[1] = 7;
+		lane_to_slot[2] = 7;
+		lane_to_slot[3] = 7;
+		lane_to_slot[5] = 3;
+		lane_to_slot[6] = 3;
+		lane_to_slot[7] = 3;
+		break;
+	case 0x8F:
+	case 0x85:
+		lane_to_slot[1] = 7;
+		lane_to_slot[2] = 6;
+		lane_to_slot[3] = 5;
+		lane_to_slot[6] = 3;
+		lane_to_slot[7] = 3;
+		break;
+	case 0xA5:
+		lane_to_slot[1] = 7;
+		lane_to_slot[6] = 3;
+		lane_to_slot[7] = 3;
+		break;
+	case 0xA7:
+		lane_to_slot[1] = 7;
+		lane_to_slot[7] = 7;
+		break;
+	case 0xAA:
+		lane_to_slot[1] = 7;
+		lane_to_slot[6] = 7;
+		lane_to_slot[7] = 7;
+		break;
+	case 0x40:
+		lane_to_slot[2] = 7;
+		lane_to_slot[3] = 7;
+		break;
+	default:
+		printf("qds: Fman: Unsupported SerDes Protocol 0x%02x\n",
+		       serdes1_prtcl);
+		break;
+	}
+}
+
+/*
+ * Given the following ...
+ *
+ * 1) A pointer to an Fman Ethernet node (as identified by the 'compat'
+ * compatible string and 'addr' physical address)
+ *
+ * 2) An Fman port
+ *
+ * ... update the phy-handle property of the Ethernet node to point to the
+ * right PHY. This assumes that we already know the PHY for each port.
+ *
+ * The offset of the Fman Ethernet node is also passed in for convenience, but
+ * it is not used, and we recalculate the offset anyway.
+ *
+ * Note that what we call "Fman ports" (enum fm_port) is really an Fman MAC.
+ * Inside the Fman, "ports" are things that connect to MACs. We only call them
+ * ports in U-Boot because on previous Ethernet devices (e.g. Gianfar), MACs
+ * and ports are the same thing.
+ *
+ */
+void board_ft_fman_fixup_port(void *fdt, char *compat, phys_addr_t addr,
+			      enum fm_port port, int offset)
+{
+	phy_interface_t intf = fm_info_get_enet_if(port);
+	char phy[16];
+
+	/* The RGMII PHY is identified by the MAC connected to it */
+	if (intf == PHY_INTERFACE_MODE_RGMII) {
+		sprintf(phy, "rgmii_phy%u", port == FM1_DTSEC4 ? 1 : 2);
+		fdt_set_phy_handle(fdt, compat, addr, phy);
+	}
+
+	/* The SGMII PHY is identified by the MAC connected to it */
+	if (intf == PHY_INTERFACE_MODE_SGMII) {
+		int lane = serdes_get_first_lane(FSL_SRDS_1, SGMII_FM1_DTSEC1
+						 + port);
+		u8 slot;
+		if (lane < 0)
+			return;
+		slot = lane_to_slot[lane];
+		if (slot) {
+			/* Slot housing a SGMII riser card */
+			sprintf(phy, "phy_s%x_%02x", slot,
+				(fm_info_get_phy_address(port - FM1_DTSEC1)-
+				CONFIG_SYS_FM1_DTSEC1_RISER_PHY_ADDR + 1));
+			fdt_set_phy_handle(fdt, compat, addr, phy);
+		}
+	}
+}
+
+void fdt_fixup_board_enet(void *fdt)
+{
+	int i, lane, idx;
+
+	for (i = FM1_DTSEC1; i < FM1_DTSEC1 + CONFIG_SYS_NUM_FM1_DTSEC; i++) {
+		idx = i - FM1_DTSEC1;
+		switch (fm_info_get_enet_if(i)) {
+		case PHY_INTERFACE_MODE_SGMII:
+			lane = serdes_get_first_lane(FSL_SRDS_1,
+						     SGMII_FM1_DTSEC1 + idx);
+			if (lane < 0)
+				break;
+
+			switch (mdio_mux[i]) {
+			case EMI1_SLOT3:
+				fdt_status_okay_by_alias(fdt, "emi1_slot3");
+				break;
+			case EMI1_SLOT5:
+				fdt_status_okay_by_alias(fdt, "emi1_slot5");
+				break;
+			case EMI1_SLOT6:
+				fdt_status_okay_by_alias(fdt, "emi1_slot6");
+				break;
+			case EMI1_SLOT7:
+				fdt_status_okay_by_alias(fdt, "emi1_slot7");
+				break;
+			}
+		break;
+		case PHY_INTERFACE_MODE_RGMII:
+			if (i == FM1_DTSEC4)
+				fdt_status_okay_by_alias(fdt, "emi1_rgmii0");
+
+			if (i == FM1_DTSEC5)
+				fdt_status_okay_by_alias(fdt, "emi1_rgmii1");
+			break;
+		default:
+			break;
+		}
+	}
+}
+#endif /* #ifdef CONFIG_FMAN_ENET */
+
+static void set_brdcfg9_for_gtx_clk(void)
+{
+	u8 brdcfg9;
+	brdcfg9 = QIXIS_READ(brdcfg[9]);
+	brdcfg9 |= (1 << 5);
+	QIXIS_WRITE(brdcfg[9], brdcfg9);
+}
+
+void t1040_handle_phy_interface_sgmii(int i)
+{
+	int lane, idx, slot;
+	idx = i - FM1_DTSEC1;
+	lane = serdes_get_first_lane(FSL_SRDS_1,
+			SGMII_FM1_DTSEC1 + idx);
+
+	if (lane < 0)
+		return;
+	slot = lane_to_slot[lane];
+
+	switch (slot) {
+	case 1:
+		mdio_mux[i] = EMI1_SLOT1;
+		fm_info_set_mdio(i, mii_dev_for_muxval(mdio_mux[i]));
+		break;
+	case 3:
+		if (FM1_DTSEC4 == i)
+			fm_info_set_phy_address(i, riser_phy_addr[0]);
+		if (FM1_DTSEC5 == i)
+			fm_info_set_phy_address(i, riser_phy_addr[1]);
+
+		mdio_mux[i] = EMI1_SLOT3;
+
+		fm_info_set_mdio(i, mii_dev_for_muxval(mdio_mux[i]));
+		break;
+	case 4:
+		mdio_mux[i] = EMI1_SLOT4;
+		fm_info_set_mdio(i, mii_dev_for_muxval(mdio_mux[i]));
+		break;
+	case 5:
+		/* Slot housing a SGMII riser card? */
+		fm_info_set_phy_address(i, riser_phy_addr[0]);
+		mdio_mux[i] = EMI1_SLOT5;
+		fm_info_set_mdio(i, mii_dev_for_muxval(mdio_mux[i]));
+		break;
+	case 6:
+		/* Slot housing a SGMII riser card? */
+		fm_info_set_phy_address(i, riser_phy_addr[0]);
+		mdio_mux[i] = EMI1_SLOT6;
+		fm_info_set_mdio(i, mii_dev_for_muxval(mdio_mux[i]));
+		break;
+	case 7:
+		if (FM1_DTSEC1 == i)
+			fm_info_set_phy_address(i, riser_phy_addr[0]);
+		if (FM1_DTSEC2 == i)
+			fm_info_set_phy_address(i, riser_phy_addr[1]);
+		if (FM1_DTSEC3 == i)
+			fm_info_set_phy_address(i, riser_phy_addr[2]);
+
+		mdio_mux[i] = EMI1_SLOT7;
+		fm_info_set_mdio(i, mii_dev_for_muxval(mdio_mux[i]));
+		break;
+	default:
+		break;
+	}
+	fm_info_set_mdio(i, mii_dev_for_muxval(mdio_mux[i]));
+}
+void t1040_handle_phy_interface_rgmii(int i)
+{
+	fm_info_set_phy_address(i, i == FM1_DTSEC5 ?
+			CONFIG_SYS_FM1_DTSEC5_PHY_ADDR :
+			CONFIG_SYS_FM1_DTSEC4_PHY_ADDR);
+	mdio_mux[i] = (i == FM1_DTSEC5) ? EMI1_RGMII1 :
+		EMI1_RGMII0;
+	fm_info_set_mdio(i, mii_dev_for_muxval(mdio_mux[i]));
+}
+
+int board_eth_init(bd_t *bis)
+{
+#ifdef CONFIG_FMAN_ENET
+	struct memac_mdio_info memac_mdio_info;
+	unsigned int i;
+
+	printf("Initializing Fman\n");
+	set_brdcfg9_for_gtx_clk();
+
+	initialize_lane_to_slot();
+
+	/* Initialize the mdio_mux array so we can recognize empty elements */
+	for (i = 0; i < NUM_FM_PORTS; i++)
+		mdio_mux[i] = EMI_NONE;
+
+	memac_mdio_info.regs =
+		(struct memac_mdio_controller *)CONFIG_SYS_FM1_DTSEC_MDIO_ADDR;
+	memac_mdio_info.name = DEFAULT_FM_MDIO_NAME;
+
+	/* Register the real 1G MDIO bus */
+	fm_memac_mdio_init(bis, &memac_mdio_info);
+
+	/* Register the muxing front-ends to the MDIO buses */
+	t1040_qds_mdio_init(DEFAULT_FM_MDIO_NAME, EMI1_RGMII0);
+	t1040_qds_mdio_init(DEFAULT_FM_MDIO_NAME, EMI1_RGMII1);
+	t1040_qds_mdio_init(DEFAULT_FM_MDIO_NAME, EMI1_SLOT1);
+	t1040_qds_mdio_init(DEFAULT_FM_MDIO_NAME, EMI1_SLOT3);
+	t1040_qds_mdio_init(DEFAULT_FM_MDIO_NAME, EMI1_SLOT4);
+	t1040_qds_mdio_init(DEFAULT_FM_MDIO_NAME, EMI1_SLOT5);
+	t1040_qds_mdio_init(DEFAULT_FM_MDIO_NAME, EMI1_SLOT6);
+	t1040_qds_mdio_init(DEFAULT_FM_MDIO_NAME, EMI1_SLOT7);
+
+	/*
+	 * Program on board RGMII PHY addresses. If the SGMII Riser
+	 * card used, we'll override the PHY address later. For any DTSEC that
+	 * is RGMII, we'll also override its PHY address later. We assume that
+	 * DTSEC4 and DTSEC5 are used for RGMII.
+	 */
+	fm_info_set_phy_address(FM1_DTSEC4, CONFIG_SYS_FM1_DTSEC4_PHY_ADDR);
+	fm_info_set_phy_address(FM1_DTSEC5, CONFIG_SYS_FM1_DTSEC5_PHY_ADDR);
+
+	for (i = FM1_DTSEC1; i < FM1_DTSEC1 + CONFIG_SYS_NUM_FM1_DTSEC; i++) {
+		switch (fm_info_get_enet_if(i)) {
+		case PHY_INTERFACE_MODE_QSGMII:
+			break;
+		case PHY_INTERFACE_MODE_SGMII:
+			t1040_handle_phy_interface_sgmii(i);
+			break;
+
+		case PHY_INTERFACE_MODE_RGMII:
+			/* Only DTSEC4 and DTSEC5 can be routed to RGMII */
+			t1040_handle_phy_interface_rgmii(i);
+			break;
+		default:
+			break;
+		}
+	}
+
+	cpu_eth_init(bis);
+#endif
+
+	return pci_eth_init(bis);
+}
diff --git a/board/freescale/t1040qds/t1040qds.c b/board/freescale/t1040qds/t1040qds.c
index de3ea5c..3dec447 100644
--- a/board/freescale/t1040qds/t1040qds.c
+++ b/board/freescale/t1040qds/t1040qds.c
@@ -223,6 +223,7 @@ void ft_board_setup(void *blob, bd_t *bd)
 
 #ifdef CONFIG_SYS_DPAA_FMAN
 	fdt_fixup_fman_ethernet(blob);
+	fdt_fixup_board_enet(blob);
 #endif
 }
 
diff --git a/board/freescale/t104xrdb/Makefile b/board/freescale/t104xrdb/Makefile
index 76c0c94..e51fb7a 100644
--- a/board/freescale/t104xrdb/Makefile
+++ b/board/freescale/t104xrdb/Makefile
@@ -7,6 +7,7 @@
 
 obj-y	+= t104xrdb.o
 obj-y	+= ddr.o
+obj-y	+= eth.o
 obj-$(CONFIG_PCI)	+= pci.o
 obj-y	+= law.o
 obj-y	+= tlb.o
diff --git a/board/freescale/t104xrdb/README b/board/freescale/t104xrdb/README
index 11e03bd..1da52bb 100644
--- a/board/freescale/t104xrdb/README
+++ b/board/freescale/t104xrdb/README
@@ -164,14 +164,14 @@ NOR Flash memory Map
 0xEFF40000      0xEFFFFFFF      u-boot (current bank)            768KB
 0xEFF20000      0xEFF3FFFF      u-boot env (current bank)        128KB
 0xEFF00000      0xEFF1FFFF      FMAN Ucode (current bank)        128KB
-0xED300000      0xEFF3FFFF      rootfs (alt bank)                44MB + 256KB
-0xEC800000      0xEC8FFFF       Hardware device tree (alt bank)  1MB
+0xED300000      0xEFEFFFFF      rootfs (alt bank)                44MB
+0xEC800000      0xEC8FFFFF      Hardware device tree (alt bank)  1MB
 0xEC020000      0xEC7FFFFF      Linux.uImage (alt bank)          7MB + 875KB
 0xEC000000      0xEC01FFFF      RCW (alt bank)                   128KB
 0xEBF40000      0xEBFFFFFF      u-boot (alt bank)                768KB
 0xEBF20000      0xEBF3FFFF      u-boot env (alt bank)            128KB
 0xEBF00000      0xEBF1FFFF      FMAN ucode (alt bank)            128KB
-0xE9300000      0xEBF3FFFF      rootfs (current bank)            44MB + 256KB
+0xE9300000      0xEBEFFFFF      rootfs (current bank)            44MB
 0xE8800000      0xE88FFFFF      Hardware device tree (cur bank)  11MB + 512KB
 0xE8020000      0xE86FFFFF      Linux.uImage (current bank)      7MB + 875KB
 0xE8000000      0xE801FFFF      RCW (current bank)               128KB
diff --git a/board/freescale/t104xrdb/eth.c b/board/freescale/t104xrdb/eth.c
new file mode 100644
index 0000000..0188fd4
--- /dev/null
+++ b/board/freescale/t104xrdb/eth.c
@@ -0,0 +1,72 @@
+/*
+ * Copyright 2014 Freescale Semiconductor, Inc.
+ *
+ * SPDX-License-Identifier:	GPL-2.0+
+ */
+
+#include <common.h>
+#include <netdev.h>
+#include <asm/immap_85xx.h>
+#include <fm_eth.h>
+#include <fsl_mdio.h>
+#include <malloc.h>
+#include <asm/fsl_dtsec.h>
+
+#include "../common/fman.h"
+
+int board_eth_init(bd_t *bis)
+{
+#ifdef CONFIG_FMAN_ENET
+	struct memac_mdio_info memac_mdio_info;
+	unsigned int i;
+	int phy_addr = 0;
+	printf("Initializing Fman\n");
+
+	memac_mdio_info.regs =
+		(struct memac_mdio_controller *)CONFIG_SYS_FM1_DTSEC_MDIO_ADDR;
+	memac_mdio_info.name = DEFAULT_FM_MDIO_NAME;
+
+	/* Register the real 1G MDIO bus */
+	fm_memac_mdio_init(bis, &memac_mdio_info);
+
+	/*
+	 * Program on board RGMII, SGMII PHY addresses.
+	 */
+	for (i = FM1_DTSEC1; i < FM1_DTSEC1 + CONFIG_SYS_NUM_FM1_DTSEC; i++) {
+		int idx = i - FM1_DTSEC1;
+
+		switch (fm_info_get_enet_if(i)) {
+#ifdef CONFIG_T1040RDB
+		case PHY_INTERFACE_MODE_SGMII:
+			/* T1040RDB only supports SGMII on DTSEC3 */
+			fm_info_set_phy_address(FM1_DTSEC3,
+						CONFIG_SYS_SGMII1_PHY_ADDR);
+#endif
+		case PHY_INTERFACE_MODE_RGMII:
+			if (FM1_DTSEC4 == i)
+				phy_addr = CONFIG_SYS_RGMII1_PHY_ADDR;
+			if (FM1_DTSEC5 == i)
+				phy_addr = CONFIG_SYS_RGMII2_PHY_ADDR;
+			fm_info_set_phy_address(i, phy_addr);
+			break;
+		case PHY_INTERFACE_MODE_QSGMII:
+			fm_info_set_phy_address(i, 0);
+			break;
+		case PHY_INTERFACE_MODE_NONE:
+			fm_info_set_phy_address(i, 0);
+			break;
+		default:
+			printf("Fman1: DTSEC%u set to unknown interface %i\n",
+			       idx + 1, fm_info_get_enet_if(i));
+			fm_info_set_phy_address(i, 0);
+			break;
+		}
+		fm_info_set_mdio(i,
+				 miiphy_get_dev_by_name(DEFAULT_FM_MDIO_NAME));
+	}
+
+	cpu_eth_init(bis);
+#endif
+
+	return pci_eth_init(bis);
+}
diff --git a/board/keymile/kmp204x/Makefile b/board/keymile/kmp204x/Makefile
index 3e69ee2..c57ca08 100644
--- a/board/keymile/kmp204x/Makefile
+++ b/board/keymile/kmp204x/Makefile
@@ -8,5 +8,5 @@
 # SPDX-License-Identifier:	GPL-2.0+
 #
 
-obj-y	:= kmp204x.o ddr.o eth.o tlb.o pci.o law.o \
+obj-y	:= kmp204x.o ddr.o eth.o tlb.o pci.o law.o qrio.o \
 	../common/common.o ../common/ivm.o
diff --git a/board/keymile/kmp204x/kmp204x.c b/board/keymile/kmp204x/kmp204x.c
index f02642a..95a19cd 100644
--- a/board/keymile/kmp204x/kmp204x.c
+++ b/board/keymile/kmp204x/kmp204x.c
@@ -33,12 +33,51 @@ int checkboard(void)
 	return 0;
 }
 
-/* TODO: implement the I2C deblocking function */
-int i2c_make_abort(void)
+/* I2C deblocking uses the algorithm defined in board/keymile/common/common.c
+ * 2 dedicated QRIO GPIOs externally pull the SCL and SDA lines
+ * For I2C only the low state is activly driven and high state is pulled-up
+ * by a resistor. Therefore the deblock GPIOs are used
+ *  -> as an active output to drive a low state
+ *  -> as an open-drain input to have a pulled-up high state
+ */
+
+/* QRIO GPIOs used for deblocking */
+#define DEBLOCK_PORT1	GPIO_A
+#define DEBLOCK_SCL1	20
+#define DEBLOCK_SDA1	21
+
+/* By default deblock GPIOs are floating */
+static void i2c_deblock_gpio_cfg(void)
+{
+	/* set I2C bus 1 deblocking GPIOs input, but 0 value for open drain */
+	qrio_gpio_direction_input(DEBLOCK_PORT1, DEBLOCK_SCL1);
+	qrio_gpio_direction_input(DEBLOCK_PORT1, DEBLOCK_SDA1);
+
+	qrio_set_gpio(DEBLOCK_PORT1, DEBLOCK_SCL1, 0);
+	qrio_set_gpio(DEBLOCK_PORT1, DEBLOCK_SDA1, 0);
+}
+
+void set_sda(int state)
+{
+	qrio_set_opendrain_gpio(DEBLOCK_PORT1, DEBLOCK_SDA1, state);
+}
+
+void set_scl(int state)
+{
+	qrio_set_opendrain_gpio(DEBLOCK_PORT1, DEBLOCK_SCL1, state);
+}
+
+int get_sda(void)
+{
+	return qrio_get_gpio(DEBLOCK_PORT1, DEBLOCK_SDA1);
+}
+
+int get_scl(void)
 {
-	return 1;
+	return qrio_get_gpio(DEBLOCK_PORT1, DEBLOCK_SCL1);
 }
 
+
 #define ZL30158_RST	8
 #define ZL30343_RST	9
 
@@ -62,6 +101,7 @@ int board_early_init_f(void)
 
 int board_early_init_r(void)
 {
+	int ret = 0;
 	/* Flush d-cache and invalidate i-cache of any FLASH data */
 	flush_dcache();
 	invalidate_icache();
@@ -69,7 +109,11 @@ int board_early_init_r(void)
 	set_liodns();
 	setup_portals();
 
-	return 0;
+	ret = trigger_fpga_config();
+	if (ret)
+		printf("error triggering PCIe FPGA config\n");
+
+	return ret;
 }
 
 unsigned long get_board_sys_clk(unsigned long dummy)
@@ -77,80 +121,12 @@ unsigned long get_board_sys_clk(unsigned long dummy)
 	return 66666666;
 }
 
-#define WDMASK_OFF	0x16
-
-static void qrio_wdmask(u8 bit, bool wden)
-{
-	u16 wdmask;
-	void __iomem *qrio_base = (void *)CONFIG_SYS_QRIO_BASE;
-
-	wdmask = in_be16(qrio_base + WDMASK_OFF);
-
-	if (wden)
-		wdmask |= (1 << bit);
-	else
-		wdmask &= ~(1 << bit);
-
-	out_be16(qrio_base + WDMASK_OFF, wdmask);
-}
-
-#define PRST_OFF	0x1a
-
-void qrio_prst(u8 bit, bool en, bool wden)
-{
-	u16 prst;
-	void __iomem *qrio_base = (void *)CONFIG_SYS_QRIO_BASE;
-
-	qrio_wdmask(bit, wden);
-
-	prst = in_be16(qrio_base + PRST_OFF);
-
-	if (en)
-		prst &= ~(1 << bit);
-	else
-		prst |= (1 << bit);
-
-	out_be16(qrio_base + PRST_OFF, prst);
-}
-
-#define PRSTCFG_OFF	0x1c
-
-void qrio_prstcfg(u8 bit, u8 mode)
-{
-	u32 prstcfg;
-	u8 i;
-	void __iomem *qrio_base = (void *)CONFIG_SYS_QRIO_BASE;
-
-	prstcfg = in_be32(qrio_base + PRSTCFG_OFF);
-
-	for (i = 0; i < 2; i++) {
-		if (mode & (1<<i))
-			set_bit(2*bit+i, &prstcfg);
-		else
-			clear_bit(2*bit+i, &prstcfg);
-	}
-
-	out_be32(qrio_base + PRSTCFG_OFF, prstcfg);
-}
-
-
-#define BOOTCOUNT_OFF	0x12
-
-void bootcount_store(ulong counter)
+int misc_init_f(void)
 {
-	u8 val;
-	void __iomem *qrio_base = (void *)CONFIG_SYS_QRIO_BASE;
-
-	val = (counter <= 255) ? (u8)counter : 255;
-	out_8(qrio_base + BOOTCOUNT_OFF, val);
-}
+	/* configure QRIO pis for i2c deblocking */
+	i2c_deblock_gpio_cfg();
 
-ulong bootcount_load(void)
-{
-	u8 val;
-	void __iomem *qrio_base = (void *)CONFIG_SYS_QRIO_BASE;
-	val = in_8(qrio_base + BOOTCOUNT_OFF);
-	return val;
+	return 0;
 }
 
 #define NUM_SRDS_BANKS	2
diff --git a/board/keymile/kmp204x/kmp204x.h b/board/keymile/kmp204x/kmp204x.h
index b6ba672..0267596 100644
--- a/board/keymile/kmp204x/kmp204x.h
+++ b/board/keymile/kmp204x/kmp204x.h
@@ -5,6 +5,16 @@
  * SPDX-License-Identifier:	GPL-2.0+
  */
 
+/* QRIO GPIO ports */
+#define GPIO_A			0x40
+#define GPIO_B			0x60
+
+int qrio_get_gpio(u8 port_off, u8 gpio_nr);
+void qrio_set_opendrain_gpio(u8 port_off, u8 gpio_nr, u8 val);
+void qrio_set_gpio(u8 port_off, u8 gpio_nr, bool value);
+void qrio_gpio_direction_output(u8 port_off, u8 gpio_nr, bool value);
+void qrio_gpio_direction_input(u8 port_off, u8 gpio_nr);
+
 #define PRSTCFG_POWUP_UNIT_CORE_RST	0x0
 #define PRSTCFG_POWUP_UNIT_RST		0x1
 #define PRSTCFG_POWUP_RST		0x3
diff --git a/board/keymile/kmp204x/pbi.cfg b/board/keymile/kmp204x/pbi.cfg
index f38dcf9..9af8bd5 100644
--- a/board/keymile/kmp204x/pbi.cfg
+++ b/board/keymile/kmp204x/pbi.cfg
@@ -8,6 +8,16 @@
 #
 
 #PBI commands
+#Workaround for A-006559 needed for rev 2.0 of P2041 silicon
+#Freescale's errarta sheet suggests it may be done with PBI
+09000010 00000000
+09000014 00000000
+09000018 81d00000
+09021008 0000f000
+09021028 0000f000
+09021048 0000f000
+09021068 0000f000
+09000018 00000000
 #Initialize CPC1 as 1MB SRAM
 09010000 00200400
 09138000 00000000
diff --git a/board/keymile/kmp204x/pci.c b/board/keymile/kmp204x/pci.c
index ec20c8a..a484eb5 100644
--- a/board/keymile/kmp204x/pci.c
+++ b/board/keymile/kmp204x/pci.c
@@ -14,18 +14,103 @@
 #include <libfdt.h>
 #include <fdt_support.h>
 #include <asm/fsl_serdes.h>
+#include <asm/errno.h>
 
 #include "kmp204x.h"
 
+#define PROM_SEL_L	11
+/* control the PROM_SEL_L signal*/
+static void toggle_fpga_eeprom_bus(bool cpu_own)
+{
+	qrio_gpio_direction_output(GPIO_A, PROM_SEL_L, !cpu_own);
+}
+
+#define CONF_SEL_L	10
+#define FPGA_PROG_L	19
+#define FPGA_DONE	18
+#define FPGA_INIT_L	17
+
+int trigger_fpga_config(void)
+{
+	int ret = 0, init_l;
+	/* approx 10ms */
+	u32 timeout = 10000;
+
+	/* make sure the FPGA_can access the EEPROM */
+	toggle_fpga_eeprom_bus(false);
+
+	/* assert CONF_SEL_L to be able to drive FPGA_PROG_L */
+	qrio_gpio_direction_output(GPIO_A, CONF_SEL_L, 0);
+
+	/* trigger the config start */
+	qrio_gpio_direction_output(GPIO_A, FPGA_PROG_L, 0);
+
+	/* small delay for INIT_L line */
+	udelay(10);
+
+	/* wait for FPGA_INIT to be asserted */
+	do {
+		init_l = qrio_get_gpio(GPIO_A, FPGA_INIT_L);
+		if (timeout-- == 0) {
+			printf("FPGA_INIT timeout\n");
+			ret = -EFAULT;
+			break;
+		}
+		udelay(10);
+	} while (init_l);
+
+	/* deassert FPGA_PROG, config should start */
+	qrio_set_gpio(GPIO_A, FPGA_PROG_L, 1);
+
+	return ret;
+}
+
+/* poll the FPGA_DONE signal and give the EEPROM back to the QorIQ */
+static int wait_for_fpga_config(void)
+{
+	int ret = 0, done;
+	/* approx 5 s */
+	u32 timeout = 500000;
+
+	printf("PCIe FPGA config:");
+	do {
+		done = qrio_get_gpio(GPIO_A, FPGA_DONE);
+		if (timeout-- == 0) {
+			printf(" FPGA_DONE timeout\n");
+			ret = -EFAULT;
+			goto err_out;
+		}
+		udelay(10);
+	} while (!done);
+
+	printf(" done\n");
+
+err_out:
+	/* deactive CONF_SEL and give the CPU conf EEPROM access */
+	qrio_set_gpio(GPIO_A, CONF_SEL_L, 1);
+	toggle_fpga_eeprom_bus(true);
+
+	return ret;
+}
+
 #define PCIE_SW_RST	14
+#define PEXHC_SW_RST	13
 #define HOOPER_SW_RST	12
 
 void pci_init_board(void)
 {
+	/* first wait for the PCIe FPGA to be configured
+	 * it has been triggered earlier in board_early_init_r */
+	int ret = wait_for_fpga_config();
+	if (ret)
+		printf("error finishing PCIe FPGA config\n");
+
 	qrio_prst(PCIE_SW_RST, false, false);
+	qrio_prst(PEXHC_SW_RST, false, false);
 	qrio_prst(HOOPER_SW_RST, false, false);
 	/* Hooper is not direcly PCIe capable */
 	mdelay(50);
+
 	fsl_pcie_init_board(0);
 }
 
diff --git a/board/keymile/kmp204x/qrio.c b/board/keymile/kmp204x/qrio.c
new file mode 100644
index 0000000..49f9aa2
--- /dev/null
+++ b/board/keymile/kmp204x/qrio.c
@@ -0,0 +1,146 @@
+/*
+ * (C) Copyright 2013 Keymile AG
+ * Valentin Longchamp <valentin.longchamp@keymile.com>
+ *
+ * SPDX-License-Identifier:	GPL-2.0+
+ */
+
+#include <common.h>
+
+#include "../common/common.h"
+#include "kmp204x.h"
+
+/* QRIO GPIO register offsets */
+#define DIRECT_OFF		0x18
+#define GPRT_OFF		0x1c
+
+int qrio_get_gpio(u8 port_off, u8 gpio_nr)
+{
+	u32 gprt;
+
+	void __iomem *qrio_base = (void *)CONFIG_SYS_QRIO_BASE;
+
+	gprt = in_be32(qrio_base + port_off + GPRT_OFF);
+
+	return (gprt >> gpio_nr) & 1U;
+}
+
+void qrio_set_gpio(u8 port_off, u8 gpio_nr, bool value)
+{
+	u32 gprt, mask;
+
+	void __iomem *qrio_base = (void *)CONFIG_SYS_QRIO_BASE;
+
+	mask = 1U << gpio_nr;
+
+	gprt = in_be32(qrio_base + port_off + GPRT_OFF);
+	if (value)
+		gprt |= mask;
+	else
+		gprt &= ~mask;
+
+	out_be32(qrio_base + port_off + GPRT_OFF, gprt);
+}
+
+void qrio_gpio_direction_output(u8 port_off, u8 gpio_nr, bool value)
+{
+	u32 direct, mask;
+
+	void __iomem *qrio_base = (void *)CONFIG_SYS_QRIO_BASE;
+
+	mask = 1U << gpio_nr;
+
+	direct = in_be32(qrio_base + port_off + DIRECT_OFF);
+	direct |= mask;
+	out_be32(qrio_base + port_off + DIRECT_OFF, direct);
+
+	qrio_set_gpio(port_off, gpio_nr, value);
+}
+
+void qrio_gpio_direction_input(u8 port_off, u8 gpio_nr)
+{
+	u32 direct, mask;
+
+	void __iomem *qrio_base = (void *)CONFIG_SYS_QRIO_BASE;
+
+	mask = 1U << gpio_nr;
+
+	direct = in_be32(qrio_base + port_off + DIRECT_OFF);
+	direct &= ~mask;
+	out_be32(qrio_base + port_off + DIRECT_OFF, direct);
+}
+
+void qrio_set_opendrain_gpio(u8 port_off, u8 gpio_nr, u8 val)
+{
+	u32 direct, mask;
+
+	void __iomem *qrio_base = (void *)CONFIG_SYS_QRIO_BASE;
+
+	mask = 1U << gpio_nr;
+
+	direct = in_be32(qrio_base + port_off + DIRECT_OFF);
+	if (val == 0)
+		/* set to output -> GPIO drives low */
+		direct |= mask;
+	else
+		/* set to input -> GPIO floating */
+		direct &= ~mask;
+
+	out_be32(qrio_base + port_off + DIRECT_OFF, direct);
+}
+
+#define WDMASK_OFF	0x16
+
+static void qrio_wdmask(u8 bit, bool wden)
+{
+	u16 wdmask;
+	void __iomem *qrio_base = (void *)CONFIG_SYS_QRIO_BASE;
+
+	wdmask = in_be16(qrio_base + WDMASK_OFF);
+
+	if (wden)
+		wdmask |= (1 << bit);
+	else
+		wdmask &= ~(1 << bit);
+
+	out_be16(qrio_base + WDMASK_OFF, wdmask);
+}
+
+#define PRST_OFF	0x1a
+
+void qrio_prst(u8 bit, bool en, bool wden)
+{
+	u16 prst;
+	void __iomem *qrio_base = (void *)CONFIG_SYS_QRIO_BASE;
+
+	qrio_wdmask(bit, wden);
+
+	prst = in_be16(qrio_base + PRST_OFF);
+
+	if (en)
+		prst &= ~(1 << bit);
+	else
+		prst |= (1 << bit);
+
+	out_be16(qrio_base + PRST_OFF, prst);
+}
+
+#define PRSTCFG_OFF	0x1c
+
+void qrio_prstcfg(u8 bit, u8 mode)
+{
+	u32 prstcfg;
+	u8 i;
+	void __iomem *qrio_base = (void *)CONFIG_SYS_QRIO_BASE;
+
+	prstcfg = in_be32(qrio_base + PRSTCFG_OFF);
+
+	for (i = 0; i < 2; i++) {
+		if (mode & (1<<i))
+			set_bit(2*bit+i, &prstcfg);
+		else
+			clear_bit(2*bit+i, &prstcfg);
+	}
+
+	out_be32(qrio_base + PRSTCFG_OFF, prstcfg);
+}
diff --git a/board/keymile/kmp204x/rcw_kmp204x.cfg b/board/keymile/kmp204x/rcw_kmp204x.cfg
index f2b7fe3..2d4c48c 100644
--- a/board/keymile/kmp204x/rcw_kmp204x.cfg
+++ b/board/keymile/kmp204x/rcw_kmp204x.cfg
@@ -7,5 +7,5 @@ aa55aa55 010e0100
 #64 bytes RCW data
 14600000 00000000 28200000 00000000
 148E70CF CFC02000 58000000 41000000
-00000000 00000000 00000000 F4428002
+00000000 00000000 00000000 F0428002
 00000000 00000000 00000000 00000000