mpc85xx: Add gdsys ControlCenter Digital board

The gdsys ControlCenter Digital board is based on a Freescale P1022 QorIQ SOC.
It boots from SPI-Flash but can be configured to boot from SD-card for
factory programming and testing.
On board peripherals include:
- 2x GbE
- Lattice ECP3 FPGA connected via PCIe
- mSATA RAID1
- USB host
- DisplayPort video output
- Atmel TPM

Signed-off-by: Dirk Eibach <dirk.eibach@gdsys.cc>
Signed-off-by: Reinhard Pfau <reinhard.pfau@gdsys.cc>
Signed-off-by: Andy Fleming <afleming@freescale.com>

12 files changed, 2171 insertions, 0 deletions

diff --git a/board/gdsys/common/Makefile b/board/gdsys/common/Makefile
index 05dd65df7ac..13a9c5dc738 100644
--- a/board/gdsys/common/Makefile
+++ b/board/gdsys/common/Makefile
@@ -33,6 +33,7 @@ COBJS-$(CONFIG_IO) += miiphybb.o
 COBJS-$(CONFIG_IO64) += miiphybb.o
 COBJS-$(CONFIG_IOCON) += osd.o
 COBJS-$(CONFIG_DLVISION_10G) += osd.o
+COBJS-$(CONFIG_CONTROLCENTERD) += dp501.o
 
 COBJS   := $(COBJS-y)
 SOBJS   =
diff --git a/board/gdsys/common/dp501.c b/board/gdsys/common/dp501.c
new file mode 100644
index 00000000000..9aa4e3f4bdc
--- /dev/null
+++ b/board/gdsys/common/dp501.c
@@ -0,0 +1,107 @@
+/*
+ * (C) Copyright 2012
+ * Dirk Eibach,  Guntermann & Drunck GmbH, dirk.eibach@gdsys.cc
+ *
+ * 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
+ */
+
+/* Parade Technologies Inc. DP501 DisplayPort DVI/HDMI Transmitter */
+
+#include <common.h>
+#include <asm/io.h>
+#include <errno.h>
+#include <i2c.h>
+
+static void dp501_setbits(u8 addr, u8 reg, u8 mask)
+{
+	u8 val;
+
+	val = i2c_reg_read(addr, reg);
+	setbits_8(&val, mask);
+	i2c_reg_write(addr, reg, val);
+}
+
+static void dp501_clrbits(u8 addr, u8 reg, u8 mask)
+{
+	u8 val;
+
+	val = i2c_reg_read(addr, reg);
+	clrbits_8(&val, mask);
+	i2c_reg_write(addr, reg, val);
+}
+
+static int dp501_detect_cable_adapter(u8 addr)
+{
+	u8 val = i2c_reg_read(addr, 0x00);
+
+	return !(val & 0x04);
+}
+
+static void dp501_link_training(u8 addr)
+{
+	u8 val;
+
+	val = i2c_reg_read(addr, 0x51);
+	i2c_reg_write(addr, 0x5d, val); /* set link_bw */
+	val = i2c_reg_read(addr, 0x52);
+	i2c_reg_write(addr, 0x5e, val); /* set lane_cnt */
+	val = i2c_reg_read(addr, 0x53);
+	i2c_reg_write(addr, 0x5c, val); /* set downspread_ctl */
+
+	i2c_reg_write(addr, 0x5f, 0x0d); /* start training */
+}
+
+void dp501_powerup(u8 addr)
+{
+	dp501_clrbits(addr, 0x0a, 0x30); /* power on encoder */
+	i2c_reg_write(addr, 0x27, 0x30); /* Hardware auto detect DVO timing */
+	dp501_setbits(addr, 0x72, 0x80); /* DPCD read enable */
+	dp501_setbits(addr, 0x30, 0x20); /* RS polynomial select */
+	i2c_reg_write(addr, 0x71, 0x20); /* Enable Aux burst write */
+	dp501_setbits(addr, 0x78, 0x30); /* Disable HPD2 IRQ */
+	dp501_clrbits(addr, 0x2f, 0x40); /* Link FIFO reset selection */
+	i2c_reg_write(addr, 0x24, 0xc0); /* SDR mode 0, ext. H/VSYNC */
+	i2c_reg_write(addr + 2, 0x24, 0x02); /* clock input single ended */
+
+	if (dp501_detect_cable_adapter(addr)) {
+		printf("DVI/HDMI cable adapter detected\n");
+		i2c_reg_write(addr, 0x5e, 0x04); /* enable 4 channel */
+		dp501_clrbits(addr, 0x00, 0x08); /* DVI/HDMI HDCP operation */
+	} else {
+		printf("no DVI/HDMI cable adapter detected\n");
+		i2c_reg_write(addr + 2, 0x00, 0x18); /* driving strength */
+		i2c_reg_write(addr + 2, 0x03, 0x06); /* driving strength */
+		i2c_reg_write(addr, 0x2c, 0x00); /* configure N value */
+		i2c_reg_write(addr, 0x2d, 0x00); /* configure N value */
+		i2c_reg_write(addr, 0x2e, 0x0c); /* configure N value */
+		i2c_reg_write(addr, 0x76, 0xff); /* clear all interrupt */
+		dp501_setbits(addr, 0x78, 0x03); /* clear all interrupt */
+		i2c_reg_write(addr, 0x75, 0xf8); /* aux channel reset */
+		i2c_reg_write(addr, 0x75, 0x00); /* clear aux channel reset */
+		i2c_reg_write(addr, 0x87, 0x70); /* set retry counter as 7 */
+		dp501_setbits(addr, 0x00, 0x08); /* for DP HDCP operation */
+
+		dp501_link_training(addr);
+	}
+}
+
+void dp501_powerdown(u8 addr)
+{
+	dp501_setbits(addr, 0x0a, 0x30); /* power down encoder, standby mode */
+}
diff --git a/board/gdsys/common/dp501.h b/board/gdsys/common/dp501.h
new file mode 100644
index 00000000000..8dc3215dcf0
--- /dev/null
+++ b/board/gdsys/common/dp501.h
@@ -0,0 +1,30 @@
+/*
+ * (C) Copyright 2012
+ * Dirk Eibach,  Guntermann & Drunck GmbH, dirk.eibach@gdsys.cc
+ *
+ * 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
+ */
+
+#ifndef _DP501_H_
+#define _DP501_H_
+
+void dp501_powerup(u8 addr);
+void dp501_powerdown(u8 addr);
+
+#endif
diff --git a/board/gdsys/p1022/Makefile b/board/gdsys/p1022/Makefile
new file mode 100644
index 00000000000..17f602f7ef7
--- /dev/null
+++ b/board/gdsys/p1022/Makefile
@@ -0,0 +1,37 @@
+#
+# Copyright 2010 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 as published by the Free
+# Software Foundation; either version 2 of the License, or (at your option)
+# any later version.
+#
+
+include $(TOPDIR)/config.mk
+
+LIB	= $(obj)lib$(BOARD).o
+
+# COBJS-y	+= $(BOARD).o
+COBJS-y	+= law.o
+COBJS-y	+= ddr.o
+COBJS-y	+= tlb.o
+COBJS-y	+= sdhc_boot.o
+COBJS-$(CONFIG_CONTROLCENTERD) += controlcenterd.o controlcenterd-id.o
+
+COBJS-$(CONFIG_FSL_DIU_FB) += diu.o
+
+SRCS	:= $(SOBJS:.o=.S) $(COBJS-y:.o=.c)
+OBJS	:= $(addprefix $(obj),$(COBJS-y))
+SOBJS	:= $(addprefix $(obj),$(SOBJS))
+
+$(LIB):	$(OBJS) $(SOBJS)
+	$(call cmd_link_o_target, $(OBJS))
+
+#########################################################################
+
+# defines $(obj).depend target
+include $(SRCTREE)/rules.mk
+
+sinclude $(obj).depend
+
+#########################################################################
diff --git a/board/gdsys/p1022/controlcenterd-id.c b/board/gdsys/p1022/controlcenterd-id.c
new file mode 100644
index 00000000000..3fca3c53b2e
--- /dev/null
+++ b/board/gdsys/p1022/controlcenterd-id.c
@@ -0,0 +1,1224 @@
+/*
+ * (C) Copyright 2013
+ * Reinhard Pfau, Guntermann & Drunck GmbH, reinhard.pfau@gdsys.cc
+ *
+ * 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., 51 Franklin Street, Fifth Floor, Boston,
+ * MA 02110-1301, USA.
+ */
+
+/* TODO: some more #ifdef's to avoid unneeded code for stage 1 / stage 2 */
+
+#ifdef CCDM_ID_DEBUG
+#define DEBUG
+#endif
+
+#include <common.h>
+#include <malloc.h>
+#include <fs.h>
+#include <i2c.h>
+#include <mmc.h>
+#include <tpm.h>
+#include <sha1.h>
+#include <asm/byteorder.h>
+#include <asm/unaligned.h>
+#include <pca9698.h>
+
+#undef CCDM_FIRST_STAGE
+#undef CCDM_SECOND_STAGE
+#undef CCDM_AUTO_FIRST_STAGE
+
+#ifdef CONFIG_DEVELOP
+#define CCDM_DEVELOP
+#endif
+
+#ifdef CONFIG_TRAILBLAZER
+#define CCDM_FIRST_STAGE
+#undef CCDM_SECOND_STAGE
+#else
+#undef CCDM_FIRST_STAGE
+#define CCDM_SECOND_STAGE
+#endif
+
+#if defined(CCDM_DEVELOP) && defined(CCDM_SECOND_STAGE) && \
+	!defined(CCCM_FIRST_STAGE)
+#define CCDM_AUTO_FIRST_STAGE
+#endif
+
+/* enums from TCG specs */
+enum {
+	/* capability areas */
+	TPM_CAP_NV_INDEX	= 0x00000011,
+	TPM_CAP_HANDLE		= 0x00000014,
+	/* resource types */
+	TPM_RT_KEY	= 0x00000001,
+};
+
+/* CCDM specific contants */
+enum {
+	/* NV indices */
+	NV_COMMON_DATA_INDEX	= 0x40000001,
+	/* magics for key blob chains */
+	MAGIC_KEY_PROGRAM	= 0x68726500,
+	MAGIC_HMAC		= 0x68616300,
+	MAGIC_END_OF_CHAIN	= 0x00000000,
+	/* sizes */
+	NV_COMMON_DATA_MIN_SIZE	= 3 * sizeof(uint64_t) + 2 * sizeof(uint16_t),
+};
+
+/* other constants */
+enum {
+	ESDHC_BOOT_IMAGE_SIG_OFS	= 0x40,
+	ESDHC_BOOT_IMAGE_SIZE_OFS	= 0x48,
+	ESDHC_BOOT_IMAGE_ADDR_OFS	= 0x50,
+	ESDHC_BOOT_IMAGE_TARGET_OFS	= 0x58,
+	ESDHC_BOOT_IMAGE_ENTRY_OFS	= 0x60,
+};
+
+struct key_program {
+	uint32_t magic;
+	uint32_t code_crc;
+	uint32_t code_size;
+	uint8_t code[];
+};
+
+struct h_reg {
+	bool valid;
+	uint8_t digest[20];
+};
+
+
+enum access_mode {
+	HREG_NONE	= 0,
+	HREG_RD		= 1,
+	HREG_WR		= 2,
+	HREG_RDWR	= 3,
+};
+
+/* register constants */
+enum {
+	FIX_HREG_DEVICE_ID_HASH	= 0,
+	FIX_HREG_SELF_HASH	= 1,
+	FIX_HREG_STAGE2_HASH	= 2,
+	FIX_HREG_VENDOR		= 3,
+	COUNT_FIX_HREGS
+};
+
+
+/* hre opcodes */
+enum {
+	/* opcodes w/o data */
+	HRE_NOP		= 0x00,
+	HRE_SYNC	= HRE_NOP,
+	HRE_CHECK0	= 0x01,
+	/* opcodes w/o data, w/ sync dst */
+	/* opcodes w/ data */
+	HRE_LOAD	= 0x81,
+	/* opcodes w/data, w/sync dst */
+	HRE_XOR		= 0xC1,
+	HRE_AND		= 0xC2,
+	HRE_OR		= 0xC3,
+	HRE_EXTEND	= 0xC4,
+	HRE_LOADKEY	= 0xC5,
+};
+
+/* hre errors */
+enum {
+	HRE_E_OK	= 0,
+	HRE_E_TPM_FAILURE,
+	HRE_E_INVALID_HREG,
+};
+
+static uint64_t device_id;
+static uint64_t device_cl;
+static uint64_t device_type;
+
+static uint32_t platform_key_handle;
+
+static void(*bl2_entry)(void);
+
+static struct h_reg pcr_hregs[24];
+static struct h_reg fix_hregs[COUNT_FIX_HREGS];
+static struct h_reg var_hregs[8];
+static uint32_t hre_tpm_err;
+static int hre_err = HRE_E_OK;
+
+#define IS_PCR_HREG(spec) ((spec) & 0x20)
+#define IS_FIX_HREG(spec) (((spec) & 0x38) == 0x08)
+#define IS_VAR_HREG(spec) (((spec) & 0x38) == 0x10)
+#define HREG_IDX(spec) ((spec) & (IS_PCR_HREG(spec) ? 0x1f : 0x7))
+
+
+static const uint8_t prg_stage1_prepare[] = {
+	0x00, 0x20, 0x00, 0x00, /* opcode: SYNC f0 */
+	0x00, 0x24, 0x00, 0x00, /* opcode: SYNC f1 */
+	0x01, 0x80, 0x00, 0x00, /* opcode: CHECK0 PCR0 */
+	0x81, 0x22, 0x00, 0x00, /* opcode: LOAD PCR0, f0 */
+	0x01, 0x84, 0x00, 0x00, /* opcode: CHECK0 PCR1 */
+	0x81, 0x26, 0x10, 0x00, /* opcode: LOAD PCR1, f1 */
+	0x01, 0x88, 0x00, 0x00, /* opcode: CHECK0 PCR2 */
+	0x81, 0x2a, 0x20, 0x00, /* opcode: LOAD PCR2, f2 */
+	0x01, 0x8c, 0x00, 0x00, /* opcode: CHECK0 PCR3 */
+	0x81, 0x2e, 0x30, 0x00, /* opcode: LOAD PCR3, f3 */
+};
+
+static const uint8_t prg_stage2_prepare[] = {
+	0x00, 0x80, 0x00, 0x00, /* opcode: SYNC PCR0 */
+	0x00, 0x84, 0x00, 0x00, /* opcode: SYNC PCR1 */
+	0x00, 0x88, 0x00, 0x00, /* opcode: SYNC PCR2 */
+	0x00, 0x8c, 0x00, 0x00, /* opcode: SYNC PCR3 */
+	0x00, 0x90, 0x00, 0x00, /* opcode: SYNC PCR4 */
+};
+
+static const uint8_t prg_stage2_success[] = {
+	0x81, 0x02, 0x40, 0x14, /* opcode: LOAD PCR4, #<20B data> */
+	0x48, 0xfd, 0x95, 0x17, 0xe7, 0x54, 0x6b, 0x68, /* data */
+	0x92, 0x31, 0x18, 0x05, 0xf8, 0x58, 0x58, 0x3c, /* data */
+	0xe4, 0xd2, 0x81, 0xe0, /* data */
+};
+
+static const uint8_t prg_stage_fail[] = {
+	0x81, 0x01, 0x00, 0x14, /* opcode: LOAD v0, #<20B data> */
+	0xc0, 0x32, 0xad, 0xc1, 0xff, 0x62, 0x9c, 0x9b, /* data */
+	0x66, 0xf2, 0x27, 0x49, 0xad, 0x66, 0x7e, 0x6b, /* data */
+	0xea, 0xdf, 0x14, 0x4b, /* data */
+	0x81, 0x42, 0x30, 0x00, /* opcode: LOAD PCR3, v0 */
+	0x81, 0x42, 0x40, 0x00, /* opcode: LOAD PCR4, v0 */
+};
+
+static const uint8_t vendor[] = "Guntermann & Drunck";
+
+
+/**
+ * @brief read a bunch of data from MMC into memory.
+ *
+ * @param mmc	pointer to the mmc structure to use.
+ * @param src	offset where the data starts on MMC/SD device (in bytes).
+ * @param dst	pointer to the location where the read data should be stored.
+ * @param size	number of bytes to read from the MMC/SD device.
+ * @return number of bytes read or -1 on error.
+ */
+static int ccdm_mmc_read(struct mmc *mmc, u64 src, u8 *dst, int size)
+{
+	int result = 0;
+	u32 blk_len, ofs;
+	ulong block_no, n, cnt;
+	u8 *tmp_buf = NULL;
+
+	if (size <= 0)
+		goto end;
+
+	blk_len = mmc->read_bl_len;
+	tmp_buf = malloc(blk_len);
+	if (!tmp_buf)
+		goto failure;
+	block_no = src / blk_len;
+	ofs = src % blk_len;
+
+	if (ofs) {
+		n = mmc->block_dev.block_read(mmc->block_dev.dev, block_no++, 1,
+			tmp_buf);
+		if (!n)
+			goto failure;
+		result = min(size, blk_len - ofs);
+		memcpy(dst, tmp_buf + ofs, result);
+		dst += result;
+		size -= result;
+	}
+	cnt = size / blk_len;
+	if (cnt) {
+		n = mmc->block_dev.block_read(mmc->block_dev.dev, block_no, cnt,
+			dst);
+		if (n != cnt)
+			goto failure;
+		size -= cnt * blk_len;
+		result += cnt * blk_len;
+		dst += cnt * blk_len;
+		block_no += cnt;
+	}
+	if (size) {
+		n = mmc->block_dev.block_read(mmc->block_dev.dev, block_no++, 1,
+			tmp_buf);
+		if (!n)
+			goto failure;
+		memcpy(dst, tmp_buf, size);
+		result += size;
+	}
+	goto end;
+failure:
+	result = -1;
+end:
+	if (tmp_buf)
+		free(tmp_buf);
+	return result;
+}
+
+/**
+ * @brief returns a location where the 2nd stage bootloader can be(/ is) placed.
+ *
+ * @return pointer to the location for/of the 2nd stage bootloader
+ */
+static u8 *get_2nd_stage_bl_location(ulong target_addr)
+{
+	ulong addr;
+#ifdef CCDM_SECOND_STAGE
+	addr = getenv_ulong("loadaddr", 16, CONFIG_LOADADDR);
+#else
+	addr = target_addr;
+#endif
+	return (u8 *)(addr);
+}
+
+
+#ifdef CCDM_SECOND_STAGE
+/**
+ * @brief returns a location where the image can be(/ is) placed.
+ *
+ * @return pointer to the location for/of the image
+ */
+static u8 *get_image_location(void)
+{
+	ulong addr;
+	/* TODO use other area? */
+	addr = getenv_ulong("loadaddr", 16, CONFIG_LOADADDR);
+	return (u8 *)(addr);
+}
+#endif
+
+/**
+ * @brief get the size of a given (TPM) NV area
+ * @param index	NV index of the area to get size for
+ * @param size	pointer to the size
+ * @return 0 on success, != 0 on error
+ */
+static int get_tpm_nv_size(uint32_t index, uint32_t *size)
+{
+	uint32_t err;
+	uint8_t info[72];
+	uint8_t *ptr;
+	uint16_t v16;
+
+	err = tpm_get_capability(TPM_CAP_NV_INDEX, index,
+		info, sizeof(info));
+	if (err) {
+		printf("tpm_get_capability(CAP_NV_INDEX, %08x) failed: %u\n",
+		       index, err);
+		return 1;
+	}
+
+	/* skip tag and nvIndex */
+	ptr = info + 6;
+	/* skip 2 pcr info fields */
+	v16 = get_unaligned_be16(ptr);
+	ptr += 2 + v16 + 1 + 20;
+	v16 = get_unaligned_be16(ptr);
+	ptr += 2 + v16 + 1 + 20;
+	/* skip permission and flags */
+	ptr += 6 + 3;
+
+	*size = get_unaligned_be32(ptr);
+	return 0;
+}
+
+/**
+ * @brief search for a key by usage auth and pub key hash.
+ * @param auth	usage auth of the key to search for
+ * @param pubkey_digest	(SHA1) hash of the pub key structure of the key
+ * @param[out] handle	the handle of the key iff found
+ * @return 0 if key was found in TPM; != 0 if not.
+ */
+static int find_key(const uint8_t auth[20], const uint8_t pubkey_digest[20],
+		uint32_t *handle)
+{
+	uint16_t key_count;
+	uint32_t key_handles[10];
+	uint8_t buf[288];
+	uint8_t *ptr;
+	uint32_t err;
+	uint8_t digest[20];
+	size_t buf_len;
+	unsigned int i;
+
+	/* fetch list of already loaded keys in the TPM */
+	err = tpm_get_capability(TPM_CAP_HANDLE, TPM_RT_KEY, buf, sizeof(buf));
+	if (err)
+		return -1;
+	key_count = get_unaligned_be16(buf);
+	ptr = buf + 2;
+	for (i = 0; i < key_count; ++i, ptr += 4)
+		key_handles[i] = get_unaligned_be32(ptr);
+
+	/* now search a(/ the) key which we can access with the given auth */
+	for (i = 0; i < key_count; ++i) {
+		buf_len = sizeof(buf);
+		err = tpm_get_pub_key_oiap(key_handles[i], auth, buf, &buf_len);
+		if (err && err != TPM_AUTHFAIL)
+			return -1;
+		if (err)
+			continue;
+		sha1_csum(buf, buf_len, digest);
+		if (!memcmp(digest, pubkey_digest, 20)) {
+			*handle = key_handles[i];
+			return 0;
+		}
+	}
+	return 1;
+}
+
+/**
+ * @brief read CCDM common data from TPM NV
+ * @return 0 if CCDM common data was found and read, !=0 if something failed.
+ */
+static int read_common_data(void)
+{
+	uint32_t size;
+	uint32_t err;
+	uint8_t buf[256];
+	sha1_context ctx;
+
+	if (get_tpm_nv_size(NV_COMMON_DATA_INDEX, &size) ||
+	    size < NV_COMMON_DATA_MIN_SIZE)
+		return 1;
+	err = tpm_nv_read_value(NV_COMMON_DATA_INDEX,
+		buf, min(sizeof(buf), size));
+	if (err) {
+		printf("tpm_nv_read_value() failed: %u\n", err);
+		return 1;
+	}
+
+	device_id = get_unaligned_be64(buf);
+	device_cl = get_unaligned_be64(buf + 8);
+	device_type = get_unaligned_be64(buf + 16);
+
+	sha1_starts(&ctx);
+	sha1_update(&ctx, buf, 24);
+	sha1_finish(&ctx, fix_hregs[FIX_HREG_DEVICE_ID_HASH].digest);
+	fix_hregs[FIX_HREG_DEVICE_ID_HASH].valid = true;
+
+	platform_key_handle = get_unaligned_be32(buf + 24);
+
+	return 0;
+}
+
+/**
+ * @brief compute hash of bootloader itself.
+ * @param[out] dst	hash register where the hash should be stored
+ * @return 0 on success, != 0 on failure.
+ *
+ * @note MUST be called at a time where the boot loader is accessible at the
+ * configured location (; so take care when code is reallocated).
+ */
+static int compute_self_hash(struct h_reg *dst)
+{
+	sha1_csum((const uint8_t *)CONFIG_SYS_MONITOR_BASE,
+		  CONFIG_SYS_MONITOR_LEN, dst->digest);
+	dst->valid = true;
+	return 0;
+}
+
+int ccdm_compute_self_hash(void)
+{
+	if (!fix_hregs[FIX_HREG_SELF_HASH].valid)
+		compute_self_hash(&fix_hregs[FIX_HREG_SELF_HASH]);
+	return 0;
+}
+
+/**
+ * @brief compute the hash of the 2nd stage boot loader (on SD card)
+ * @param[out] dst	hash register to store the computed hash
+ * @return 0 on success, != 0 on failure
+ *
+ * Determines the size and location of the 2nd stage boot loader on SD card,
+ * loads the 2nd stage boot loader and computes the (SHA1) hash value.
+ * Within the 1st stage boot loader, the 2nd stage boot loader is loaded at
+ * the desired memory location and the variable @a bl2_entry is set.
+ *
+ * @note This sets the variable @a bl2_entry to the entry point when the
+ * 2nd stage boot loader is loaded at its configured memory location.
+ */
+static int compute_second_stage_hash(struct h_reg *dst)
+{
+	int result = 0;
+	u32 code_len, code_offset, target_addr, exec_entry;
+	struct mmc *mmc;
+	u8 *load_addr = NULL;
+	u8 buf[128];
+
+	mmc = find_mmc_device(0);
+	if (!mmc)
+		goto failure;
+	mmc_init(mmc);
+
+	if (ccdm_mmc_read(mmc, 0, buf, sizeof(buf)) < 0)
+		goto failure;
+
+	code_offset = *(u32 *)(buf + ESDHC_BOOT_IMAGE_ADDR_OFS);
+	code_len = *(u32 *)(buf + ESDHC_BOOT_IMAGE_SIZE_OFS);
+	target_addr = *(u32 *)(buf + ESDHC_BOOT_IMAGE_TARGET_OFS);
+	exec_entry =  *(u32 *)(buf + ESDHC_BOOT_IMAGE_ENTRY_OFS);
+
+	load_addr = get_2nd_stage_bl_location(target_addr);
+	if (load_addr == (u8 *)target_addr)
+		bl2_entry = (void(*)(void))exec_entry;
+
+	if (ccdm_mmc_read(mmc, code_offset, load_addr, code_len) < 0)
+		goto failure;
+
+	sha1_csum(load_addr, code_len, dst->digest);
+	dst->valid = true;
+
+	goto end;
+failure:
+	result = 1;
+	bl2_entry = NULL;
+end:
+	return result;
+}
+
+/**
+ * @brief get pointer to  hash register by specification
+ * @param spec	specification of a hash register
+ * @return pointer to hash register or NULL if @a spec does not qualify a
+ * valid hash register; NULL else.
+ */
+static struct h_reg *get_hreg(uint8_t spec)
+{
+	uint8_t idx;
+
+	idx = HREG_IDX(spec);
+	if (IS_FIX_HREG(spec)) {
+		if (idx < ARRAY_SIZE(fix_hregs))
+			return fix_hregs + idx;
+		hre_err = HRE_E_INVALID_HREG;
+	} else if (IS_PCR_HREG(spec)) {
+		if (idx < ARRAY_SIZE(pcr_hregs))
+			return pcr_hregs + idx;
+		hre_err = HRE_E_INVALID_HREG;
+	} else if (IS_VAR_HREG(spec)) {
+		if (idx < ARRAY_SIZE(var_hregs))
+			return var_hregs + idx;
+		hre_err = HRE_E_INVALID_HREG;
+	}
+	return NULL;
+}
+
+/**
+ * @brief get pointer of a hash register by specification and usage.
+ * @param spec	specification of a hash register
+ * @param mode	access mode (read or write or read/write)
+ * @return pointer to hash register if found and valid; NULL else.
+ *
+ * This func uses @a get_reg() to determine the hash register for a given spec.
+ * If a register is found it is validated according to the desired access mode.
+ * The value of automatic registers (PCR register and fixed registers) is
+ * loaded or computed on read access.
+ */
+static struct h_reg *access_hreg(uint8_t spec, enum access_mode mode)
+{
+	struct h_reg *result;
+
+	result = get_hreg(spec);
+	if (!result)
+		return NULL;
+
+	if (mode & HREG_WR) {
+		if (IS_FIX_HREG(spec)) {
+			hre_err = HRE_E_INVALID_HREG;
+			return NULL;
+		}
+	}
+	if (mode & HREG_RD) {
+		if (!result->valid) {
+			if (IS_PCR_HREG(spec)) {
+				hre_tpm_err = tpm_pcr_read(HREG_IDX(spec),
+					result->digest, 20);
+				result->valid = (hre_tpm_err == TPM_SUCCESS);
+			} else if (IS_FIX_HREG(spec)) {
+				switch (HREG_IDX(spec)) {
+				case FIX_HREG_DEVICE_ID_HASH:
+					read_common_data();
+					break;
+				case FIX_HREG_SELF_HASH:
+					ccdm_compute_self_hash();
+					break;
+				case FIX_HREG_STAGE2_HASH:
+					compute_second_stage_hash(result);
+					break;
+				case FIX_HREG_VENDOR:
+					memcpy(result->digest, vendor, 20);
+					result->valid = true;
+					break;
+				}
+			} else {
+				result->valid = true;
+			}
+		}
+		if (!result->valid) {
+			hre_err = HRE_E_INVALID_HREG;
+			return NULL;
+		}
+	}
+
+	return result;
+}
+
+static void *compute_and(void *_dst, const void *_src, size_t n)
+{
+	uint8_t *dst = _dst;
+	const uint8_t *src = _src;
+	size_t i;
+
+	for (i = n; i-- > 0; )
+		*dst++ &= *src++;
+
+	return _dst;
+}
+
+static void *compute_or(void *_dst, const void *_src, size_t n)
+{
+	uint8_t *dst = _dst;
+	const uint8_t *src = _src;
+	size_t i;
+
+	for (i = n; i-- > 0; )
+		*dst++ |= *src++;
+
+	return _dst;
+}
+
+static void *compute_xor(void *_dst, const void *_src, size_t n)
+{
+	uint8_t *dst = _dst;
+	const uint8_t *src = _src;
+	size_t i;
+
+	for (i = n; i-- > 0; )
+		*dst++ ^= *src++;
+
+	return _dst;
+}
+
+static void *compute_extend(void *_dst, const void *_src, size_t n)
+{
+	uint8_t digest[20];
+	sha1_context ctx;
+
+	sha1_starts(&ctx);
+	sha1_update(&ctx, _dst, n);
+	sha1_update(&ctx, _src, n);
+	sha1_finish(&ctx, digest);
+	memcpy(_dst, digest, min(n, sizeof(digest)));
+
+	return _dst;
+}
+
+static int hre_op_loadkey(struct h_reg *src_reg, struct h_reg *dst_reg,
+		const void *key, size_t key_size)
+{
+	uint32_t parent_handle;
+	uint32_t key_handle;
+
+	if (!src_reg || !dst_reg || !src_reg->valid || !dst_reg->valid)
+		return -1;
+	if (find_key(src_reg->digest, dst_reg->digest, &parent_handle))
+		return -1;
+	hre_tpm_err = tpm_load_key2_oiap(parent_handle, key, key_size,
+		src_reg->digest, &key_handle);
+	if (hre_tpm_err) {
+		hre_err = HRE_E_TPM_FAILURE;
+		return -1;
+	}
+	/* TODO remember key handle somehow? */
+
+	return 0;
+}
+
+/**
+ * @brief executes the next opcode on the hash register engine.
+ * @param[in,out] ip	pointer to the opcode (instruction pointer)
+ * @param[in,out] code_size	(remaining) size of the code
+ * @return new instruction pointer on success, NULL on error.
+ */
+static const uint8_t *hre_execute_op(const uint8_t **ip, size_t *code_size)
+{
+	bool dst_modified = false;
+	uint32_t ins;
+	uint8_t opcode;
+	uint8_t src_spec;
+	uint8_t dst_spec;
+	uint16_t data_size;
+	struct h_reg *src_reg, *dst_reg;
+	uint8_t buf[20];
+	const uint8_t *src_buf, *data;
+	uint8_t *ptr;
+	int i;
+	void * (*bin_func)(void *, const void *, size_t);
+
+	if (*code_size < 4)
+		return NULL;
+
+	ins = get_unaligned_be32(*ip);
+	opcode = **ip;
+	data = *ip + 4;
+	src_spec = (ins >> 18) & 0x3f;
+	dst_spec = (ins >> 12) & 0x3f;
+	data_size = (ins & 0x7ff);
+
+	debug("HRE: ins=%08x (op=%02x, s=%02x, d=%02x, L=%d)\n", ins,
+	      opcode, src_spec, dst_spec, data_size);
+
+	if ((opcode & 0x80) && (data_size + 4) > *code_size)
+		return NULL;
+
+	src_reg = access_hreg(src_spec, HREG_RD);
+	if (hre_err || hre_tpm_err)
+		return NULL;
+	dst_reg = access_hreg(dst_spec, (opcode & 0x40) ? HREG_RDWR : HREG_WR);
+	if (hre_err || hre_tpm_err)
+		return NULL;
+
+	switch (opcode) {
+	case HRE_NOP:
+		goto end;
+	case HRE_CHECK0:
+		if (src_reg) {
+			for (i = 0; i < 20; ++i) {
+				if (src_reg->digest[i])
+					return NULL;
+			}
+		}
+		break;
+	case HRE_LOAD:
+		bin_func = memcpy;
+		goto do_bin_func;
+	case HRE_XOR:
+		bin_func = compute_xor;
+		goto do_bin_func;
+	case HRE_AND:
+		bin_func = compute_and;
+		goto do_bin_func;
+	case HRE_OR:
+		bin_func = compute_or;
+		goto do_bin_func;
+	case HRE_EXTEND:
+		bin_func = compute_extend;
+do_bin_func:
+		if (!dst_reg)
+			return NULL;
+		if (src_reg) {
+			src_buf = src_reg->digest;
+		} else {
+			if (!data_size) {
+				memset(buf, 0, 20);
+				src_buf = buf;
+			} else if (data_size == 1) {
+				memset(buf, *data, 20);
+				src_buf = buf;
+			} else if (data_size >= 20) {
+				src_buf = data;
+			} else {
+				src_buf = buf;
+				for (ptr = (uint8_t *)src_buf, i = 20; i > 0;
+					i -= data_size, ptr += data_size)
+					memcpy(ptr, data, min(i, data_size));
+			}
+		}
+		bin_func(dst_reg->digest, src_buf, 20);
+		dst_reg->valid = true;
+		dst_modified = true;
+		break;
+	case HRE_LOADKEY:
+		if (hre_op_loadkey(src_reg, dst_reg, data, data_size))
+			return NULL;
+		break;
+	default:
+		return NULL;
+	}
+
+	if (dst_reg && dst_modified && IS_PCR_HREG(dst_spec)) {
+		hre_tpm_err = tpm_extend(HREG_IDX(dst_spec), dst_reg->digest,
+			dst_reg->digest);
+		if (hre_tpm_err) {
+			hre_err = HRE_E_TPM_FAILURE;
+			return NULL;
+		}
+	}
+end:
+	*ip += 4;
+	*code_size -= 4;
+	if (opcode & 0x80) {
+		*ip += data_size;
+		*code_size -= data_size;
+	}
+
+	return *ip;
+}
+
+/**
+ * @brief runs a program on the hash register engine.
+ * @param code		pointer to the (HRE) code.
+ * @param code_size	size of the code (in bytes).
+ * @return 0 on success, != 0 on failure.
+ */
+static int hre_run_program(const uint8_t *code, size_t code_size)
+{
+	size_t code_left;
+	const uint8_t *ip = code;
+
+	code_left = code_size;
+	hre_tpm_err = 0;
+	hre_err = HRE_E_OK;
+	while (code_left > 0)
+		if (!hre_execute_op(&ip, &code_left))
+			return -1;
+
+	return hre_err;
+}
+
+static int check_hmac(struct key_program *hmac,
+	const uint8_t *data, size_t data_size)
+{
+	uint8_t key[20], computed_hmac[20];
+	uint32_t type;
+
+	type = get_unaligned_be32(hmac->code);
+	if (type != 0)
+		return 1;
+	memset(key, 0, sizeof(key));
+	compute_extend(key, pcr_hregs[1].digest, 20);
+	compute_extend(key, pcr_hregs[2].digest, 20);
+	compute_extend(key, pcr_hregs[3].digest, 20);
+	compute_extend(key, pcr_hregs[4].digest, 20);
+
+	sha1_hmac(key, sizeof(key), data, data_size, computed_hmac);
+
+	return memcmp(computed_hmac, hmac->code + 4, 20);
+}
+
+static int verify_program(struct key_program *prg)
+{
+	uint32_t crc;
+	crc = crc32(0, prg->code, prg->code_size);
+
+	if (crc != prg->code_crc) {
+		printf("HRC crc mismatch: %08x != %08x\n",
+		       crc, prg->code_crc);
+		return 1;
+	}
+	return 0;
+}
+
+#if defined(CCDM_FIRST_STAGE) || (defined CCDM_AUTO_FIRST_STAGE)
+static struct key_program *load_sd_key_program(void)
+{
+	u32 code_len, code_offset;
+	struct mmc *mmc;
+	u8 buf[128];
+	struct key_program *result = NULL, *hmac = NULL;
+	struct key_program header;
+
+	mmc = find_mmc_device(0);
+	if (!mmc)
+		return NULL;
+	mmc_init(mmc);
+
+	if (ccdm_mmc_read(mmc, 0, buf, sizeof(buf)) <= 0)
+		goto failure;
+
+	code_offset = *(u32 *)(buf + ESDHC_BOOT_IMAGE_ADDR_OFS);
+	code_len = *(u32 *)(buf + ESDHC_BOOT_IMAGE_SIZE_OFS);
+
+	code_offset += code_len;
+	/* TODO: the following needs to be the size of the 2nd stage env */
+	code_offset += CONFIG_ENV_SIZE;
+
+	if (ccdm_mmc_read(mmc, code_offset, buf, 4*3) < 0)
+		goto failure;
+
+	header.magic = get_unaligned_be32(buf);
+	header.code_crc = get_unaligned_be32(buf + 4);
+	header.code_size = get_unaligned_be32(buf + 8);
+
+	if (header.magic != MAGIC_KEY_PROGRAM)
+		goto failure;
+
+	result = malloc(sizeof(struct key_program) + header.code_size);
+	if (!result)
+		goto failure;
+	*result = header;
+
+	printf("load key program chunk from SD card (%u bytes) ",
+	       header.code_size);
+	code_offset += 12;
+	if (ccdm_mmc_read(mmc, code_offset, result->code, header.code_size)
+		< 0)
+		goto failure;
+	code_offset += header.code_size;
+	puts("\n");
+
+	if (verify_program(result))
+		goto failure;
+
+	if (ccdm_mmc_read(mmc, code_offset, buf, 4*3) < 0)
+		goto failure;
+
+	header.magic = get_unaligned_be32(buf);
+	header.code_crc = get_unaligned_be32(buf + 4);
+	header.code_size = get_unaligned_be32(buf + 8);
+
+	if (header.magic == MAGIC_HMAC) {
+		puts("check integrity\n");
+		hmac = malloc(sizeof(struct key_program) + header.code_size);
+		if (!hmac)
+			goto failure;
+		*hmac = header;
+		code_offset += 12;
+		if (ccdm_mmc_read(mmc, code_offset, hmac->code,
+				  hmac->code_size) < 0)
+			goto failure;
+		if (verify_program(hmac))
+			goto failure;
+		if (check_hmac(hmac, result->code, result->code_size)) {
+			puts("key program integrity could not be verified\n");
+			goto failure;
+		}
+		puts("key program verified\n");
+	}
+
+	goto end;
+failure:
+	if (result)
+		free(result);
+	result = NULL;
+end:
+	if (hmac)
+		free(hmac);
+
+	return result;
+}
+#endif
+
+#ifdef CCDM_SECOND_STAGE
+/**
+ * @brief load a key program from file system.
+ * @param ifname	interface of the file system
+ * @param dev_part_str	device part of the file system
+ * @param fs_type	tyep of the file system
+ * @param path		path of the file to load.
+ * @return the loaded structure or NULL on failure.
+ */
+static struct key_program *load_key_chunk(const char *ifname,
+	const char *dev_part_str, int fs_type,
+	const char *path)
+{
+	struct key_program *result = NULL;
+	struct key_program header;
+	uint32_t crc;
+	uint8_t buf[12];
+	int i;
+
+	if (fs_set_blk_dev(ifname, dev_part_str, fs_type))
+		goto failure;
+	i = fs_read(path, (ulong)buf, 0, 12);
+	if (i < 12)
+		goto failure;
+	header.magic = get_unaligned_be32(buf);
+	header.code_crc = get_unaligned_be32(buf + 4);
+	header.code_size = get_unaligned_be32(buf + 8);
+
+	if (header.magic != MAGIC_HMAC && header.magic != MAGIC_KEY_PROGRAM)
+		goto failure;
+
+	result = malloc(sizeof(struct key_program) + header.code_size);
+	if (!result)
+		goto failure;
+	if (fs_set_blk_dev(ifname, dev_part_str, fs_type))
+		goto failure;
+	i = fs_read(path, (ulong)result, 0,
+		sizeof(struct key_program) + header.code_size);
+	if (i <= 0)
+		goto failure;
+	*result = header;
+
+	crc = crc32(0, result->code, result->code_size);
+
+	if (crc != result->code_crc) {
+		printf("%s: HRC crc mismatch: %08x != %08x\n",
+		       path, crc, result->code_crc);
+		goto failure;
+	}
+	goto end;
+failure:
+	if (result) {
+		free(result);
+		result = NULL;
+	}
+end:
+	return result;
+}
+#endif
+
+#if defined(CCDM_FIRST_STAGE) || (defined CCDM_AUTO_FIRST_STAGE)
+static int first_stage_actions(void)
+{
+	int result = 0;
+	struct key_program *sd_prg = NULL;
+
+	puts("CCDM S1: start actions\n");
+#ifndef CCDM_SECOND_STAGE
+	if (tpm_continue_self_test())
+		goto failure;
+#else
+	tpm_continue_self_test();
+#endif
+	mdelay(37);
+
+	if (hre_run_program(prg_stage1_prepare, sizeof(prg_stage1_prepare)))
+		goto failure;
+
+	sd_prg = load_sd_key_program();
+	if (sd_prg) {
+		if (hre_run_program(sd_prg->code, sd_prg->code_size))
+			goto failure;
+		puts("SD code run successfully\n");
+	} else {
+		puts("no key program found on SD\n");
+		goto failure;
+	}
+	goto end;
+failure:
+	result = 1;
+end:
+	if (sd_prg)
+		free(sd_prg);
+	printf("CCDM S1: actions done (%d)\n", result);
+	return result;
+}
+#endif
+
+#ifdef CCDM_FIRST_STAGE
+static int first_stage_init(void)
+{
+	int res = 0;
+	puts("CCDM S1\n");
+	if (tpm_init() || tpm_startup(TPM_ST_CLEAR))
+		return 1;
+	res = first_stage_actions();
+#ifndef CCDM_SECOND_STAGE
+	if (!res) {
+		if (bl2_entry)
+			(*bl2_entry)();
+		res = 1;
+	}
+#endif
+	return res;
+}
+#endif
+
+#ifdef CCDM_SECOND_STAGE
+static int second_stage_init(void)
+{
+	static const char mac_suffix[] = ".mac";
+	bool did_first_stage_run = true;
+	int result = 0;
+	char *cptr, *mmcdev = NULL;
+	struct key_program *hmac_blob = NULL;
+	const char *image_path = "/ccdm.itb";
+	char *mac_path = NULL;
+	ulong image_addr;
+	size_t image_size;
+	uint32_t err;
+
+	printf("CCDM S2\n");
+	if (tpm_init())
+		return 1;
+	err = tpm_startup(TPM_ST_CLEAR);
+	if (err != TPM_INVALID_POSTINIT)
+		did_first_stage_run = false;
+
+#ifdef CCDM_AUTO_FIRST_STAGE
+	if (!did_first_stage_run && first_stage_actions())
+		goto failure;
+#else
+	if (!did_first_stage_run)
+		goto failure;
+#endif
+
+	if (hre_run_program(prg_stage2_prepare, sizeof(prg_stage2_prepare)))
+		goto failure;
+
+	/* run "prepboot" from env to get "mmcdev" set */
+	cptr = getenv("prepboot");
+	if (cptr && !run_command(cptr, 0))
+		mmcdev = getenv("mmcdev");
+	if (!mmcdev)
+		goto failure;
+
+	cptr = getenv("ramdiskimage");
+	if (cptr)
+		image_path = cptr;
+
+	mac_path = malloc(strlen(image_path) + strlen(mac_suffix) + 1);
+	if (mac_path == NULL)
+		goto failure;
+	strcpy(mac_path, image_path);
+	strcat(mac_path, mac_suffix);
+
+	/* read image from mmcdev (ccdm.itb) */
+	image_addr = (ulong)get_image_location();
+	if (fs_set_blk_dev("mmc", mmcdev, FS_TYPE_EXT))
+		goto failure;
+	image_size = fs_read(image_path, image_addr, 0, 0);
+	if (image_size <= 0)
+		goto failure;
+	printf("CCDM image found on %s, %d bytes\n", mmcdev, image_size);
+
+	hmac_blob = load_key_chunk("mmc", mmcdev, FS_TYPE_EXT, mac_path);
+	if (!hmac_blob) {
+		puts("failed to load mac file\n");
+		goto failure;
+	}
+	if (verify_program(hmac_blob)) {
+		puts("corrupted mac file\n");
+		goto failure;
+	}
+	if (check_hmac(hmac_blob, (u8 *)image_addr, image_size)) {
+		puts("image integrity could not be verified\n");
+		goto failure;
+	}
+	puts("CCDM image OK\n");
+
+	hre_run_program(prg_stage2_success, sizeof(prg_stage2_success));
+
+	goto end;
+failure:
+	result = 1;
+	hre_run_program(prg_stage_fail, sizeof(prg_stage_fail));
+end:
+	if (hmac_blob)
+		free(hmac_blob);
+	if (mac_path)
+		free(mac_path);
+
+	return result;
+}
+#endif
+
+int show_self_hash(void)
+{
+	struct h_reg *hash_ptr;
+#ifdef CCDM_SECOND_STAGE
+	struct h_reg hash;
+
+	hash_ptr = &hash;
+	if (compute_self_hash(hash_ptr))
+		return 1;
+#else
+	hash_ptr = &fix_hregs[FIX_HREG_SELF_HASH];
+#endif
+	puts("self hash: ");
+	if (hash_ptr && hash_ptr->valid)
+		print_buffer(0, hash_ptr->digest, 1, 20, 20);
+	else
+		puts("INVALID\n");
+
+	return 0;
+}
+
+/**
+ * @brief let the system hang.
+ *
+ * Called on error.
+ * Will stop the boot process; display a message and signal the error condition
+ * by blinking the "status" and the "finder" LED of the controller board.
+ *
+ * @note the develop version runs the blink cycle 2 times and then returns.
+ * The release version never returns.
+ */
+static void ccdm_hang(void)
+{
+	static const u64 f0 = 0x0ba3bb8ba2e880; /* blink code "finder" LED */
+	static const u64 s0 = 0x00f0f0f0f0f0f0; /* blink code "status" LED */
+	u64 f, s;
+	int i;
+#ifdef CCDM_DEVELOP
+	int j;
+#endif
+
+	I2C_SET_BUS(0);
+	pca9698_direction_output(0x22, 0, 0); /* Finder */
+	pca9698_direction_output(0x22, 4, 0); /* Status */
+
+	puts("### ERROR ### Please RESET the board ###\n");
+	bootstage_error(BOOTSTAGE_ID_NEED_RESET);
+#ifdef CCDM_DEVELOP
+	puts("*** ERROR ******** THIS WOULD HANG ******** ERROR ***\n");
+	puts("** but we continue since this is a DEVELOP version **\n");
+	puts("*** ERROR ******** THIS WOULD HANG ******** ERROR ***\n");
+	for (j = 2; j-- > 0;) {
+		putc('#');
+#else
+	for (;;) {
+#endif
+		f = f0;
+		s = s0;
+		for (i = 54; i-- > 0;) {
+			pca9698_set_value(0x22, 0, !(f & 1));
+			pca9698_set_value(0x22, 4, (s & 1));
+			f >>= 1;
+			s >>= 1;
+			mdelay(120);
+		}
+	}
+	puts("\ncontinue...\n");
+}
+
+int startup_ccdm_id_module(void)
+{
+	int result = 0;
+	unsigned int orig_i2c_bus;
+
+	orig_i2c_bus = I2C_GET_BUS();
+	I2C_SET_BUS(1);
+
+	/* goto end; */
+
+#ifdef CCDM_DEVELOP
+	show_self_hash();
+#endif
+#ifdef CCDM_FIRST_STAGE
+	result = first_stage_init();
+	if (result) {
+		puts("1st stage init failed\n");
+		goto failure;
+	}
+#endif
+#ifdef CCDM_SECOND_STAGE
+	result = second_stage_init();
+	if (result) {
+		puts("2nd stage init failed\n");
+		goto failure;
+	}
+#endif
+
+	goto end;
+failure:
+	result = 1;
+end:
+	I2C_SET_BUS(orig_i2c_bus);
+	if (result)
+		ccdm_hang();
+
+	return result;
+}
diff --git a/board/gdsys/p1022/controlcenterd-id.h b/board/gdsys/p1022/controlcenterd-id.h
new file mode 100644
index 00000000000..de4770ac8de
--- /dev/null
+++ b/board/gdsys/p1022/controlcenterd-id.h
@@ -0,0 +1,29 @@
+/*
+ * (C) Copyright 2013
+ * Reinhard Pfau, Guntermann & Drunck GmbH, reinhard.pfau@gdsys.cc
+ *
+ * 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., 51 Franklin Street, Fifth Floor, Boston,
+ * MA 02110-1301, USA.
+ */
+
+#ifndef __CONTROLCENTER_ID_H
+#define __CONTROLCENTER_ID_H
+
+int ccdm_compute_self_hash(void);
+int startup_ccdm_id_module(void);
+
+int show_self_hash(void);
+
+#endif /* __CONTROLCENTER_ID_H */
diff --git a/board/gdsys/p1022/controlcenterd.c b/board/gdsys/p1022/controlcenterd.c
new file mode 100644
index 00000000000..81c22bc94cd
--- /dev/null
+++ b/board/gdsys/p1022/controlcenterd.c
@@ -0,0 +1,425 @@
+/*
+ * (C) Copyright 2013
+ * Dirk Eibach,  Guntermann & Drunck GmbH, dirk.eibach@gdsys.cc
+ *
+ * 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/processor.h>
+#include <asm/mmu.h>
+#include <asm/cache.h>
+#include <asm/immap_85xx.h>
+#include <asm/fsl_pci.h>
+#include <asm/fsl_ddr_sdram.h>
+#include <asm/fsl_serdes.h>
+#include <asm/io.h>
+#include <libfdt.h>
+#include <fdt_support.h>
+#include <fsl_mdio.h>
+#include <tsec.h>
+#include <asm/fsl_law.h>
+#include <netdev.h>
+#include <i2c.h>
+#include <pca9698.h>
+#include <watchdog.h>
+#include "../common/dp501.h"
+#include "controlcenterd-id.h"
+
+DECLARE_GLOBAL_DATA_PTR;
+
+enum {
+	HWVER_100 = 0,
+	HWVER_110 = 1,
+	HWVER_120 = 2,
+};
+
+struct ihs_fpga {
+	u32 reflection_low;	/* 0x0000 */
+	u32 versions;		/* 0x0004 */
+	u32 fpga_version;	/* 0x0008 */
+	u32 fpga_features;	/* 0x000c */
+};
+
+#ifndef CONFIG_TRAILBLAZER
+static struct pci_device_id hydra_supported[] = {
+	{ 0x6d5e, 0xcdc0 },
+	{}
+};
+
+static void hydra_initialize(void);
+#endif
+
+int board_early_init_f(void)
+{
+	ccsr_gur_t *gur = (void *)CONFIG_SYS_MPC85xx_GUTS_ADDR;
+	ccsr_gpio_t *pgpio = (void *)(CONFIG_SYS_MPC85xx_GPIO3_ADDR);
+
+	/* Reset eLBC_DIU and SPI_eLBC in case we are booting from SD */
+	clrsetbits_be32(&gur->pmuxcr, 0x00600000, 0x80000000);
+
+	/* Set pmuxcr to allow both i2c1 and i2c2 */
+	setbits_be32(&gur->pmuxcr, 0x00001000);
+
+	/* Set pmuxcr to enable GPIO 3_11-3_13 */
+	setbits_be32(&gur->pmuxcr, 0x00000010);
+
+	/* Set pmuxcr to enable GPIO 2_31,3_9+10 */
+	setbits_be32(&gur->pmuxcr, 0x00000020);
+
+	/* Set pmuxcr to enable GPIO 2_28-2_30 */
+	setbits_be32(&gur->pmuxcr, 0x000000c0);
+
+	/* Set pmuxcr to enable GPIO 3_20-3_22 */
+	setbits_be32(&gur->pmuxcr2, 0x03000000);
+
+	/* Set pmuxcr to enable IRQ0-2 */
+	clrbits_be32(&gur->pmuxcr, 0x00000300);
+
+	/* Set pmuxcr to disable IRQ3-11 */
+	setbits_be32(&gur->pmuxcr, 0x000000F0);
+
+	/* Read back the register to synchronize the write. */
+	in_be32(&gur->pmuxcr);
+
+	/* Set the pin muxing to enable ETSEC2. */
+	clrbits_be32(&gur->pmuxcr2, 0x001F8000);
+
+#ifdef CONFIG_TRAILBLAZER
+	/*
+	 * GPIO3_10 SPERRTRIGGER
+	 */
+	setbits_be32(&pgpio->gpdir, 0x00200000);
+	clrbits_be32(&pgpio->gpdat, 0x00200000);
+	udelay(100);
+	setbits_be32(&pgpio->gpdat, 0x00200000);
+	udelay(100);
+	clrbits_be32(&pgpio->gpdat, 0x00200000);
+#endif
+
+	/*
+	 * GPIO3_11 CPU-TO-FPGA-RESET#
+	 */
+	setbits_be32(&pgpio->gpdir, 0x00100000);
+	clrbits_be32(&pgpio->gpdat, 0x00100000);
+
+	/*
+	 * GPIO3_21 CPU-STATUS-WATCHDOG-TRIGGER#
+	 */
+	setbits_be32(&pgpio->gpdir, 0x00000400);
+
+	return 0;
+}
+
+int checkboard(void)
+{
+	printf("Board: ControlCenter DIGITAL\n");
+
+	return 0;
+}
+
+int misc_init_r(void)
+{
+	return 0;
+}
+
+/*
+ * A list of PCI and SATA slots
+ */
+enum slot_id {
+	SLOT_PCIE1 = 1,
+	SLOT_PCIE2,
+	SLOT_PCIE3,
+	SLOT_PCIE4,
+	SLOT_PCIE5,
+	SLOT_SATA1,
+	SLOT_SATA2
+};
+
+/*
+ * This array maps the slot identifiers to their names on the P1022DS board.
+ */
+static const char * const slot_names[] = {
+	[SLOT_PCIE1] = "Slot 1",
+	[SLOT_PCIE2] = "Slot 2",
+	[SLOT_PCIE3] = "Slot 3",
+	[SLOT_PCIE4] = "Slot 4",
+	[SLOT_PCIE5] = "Mini-PCIe",
+	[SLOT_SATA1] = "SATA 1",
+	[SLOT_SATA2] = "SATA 2",
+};
+
+/*
+ * This array maps a given SERDES configuration and SERDES device to the PCI or
+ * SATA slot that it connects to.  This mapping is hard-coded in the FPGA.
+ */
+static u8 serdes_dev_slot[][SATA2 + 1] = {
+	[0x01] = { [PCIE3] = SLOT_PCIE4, [PCIE2] = SLOT_PCIE5 },
+	[0x02] = { [SATA1] = SLOT_SATA1, [SATA2] = SLOT_SATA2 },
+	[0x09] = { [PCIE1] = SLOT_PCIE1, [PCIE3] = SLOT_PCIE4,
+		   [PCIE2] = SLOT_PCIE5 },
+	[0x16] = { [PCIE1] = SLOT_PCIE1, [PCIE3] = SLOT_PCIE2,
+		   [PCIE2] = SLOT_PCIE3,
+		   [SATA1] = SLOT_SATA1, [SATA2] = SLOT_SATA2 },
+	[0x17] = { [PCIE1] = SLOT_PCIE1, [PCIE3] = SLOT_PCIE2,
+		   [PCIE2] = SLOT_PCIE3 },
+	[0x1a] = { [PCIE1] = SLOT_PCIE1, [PCIE2] = SLOT_PCIE3,
+		   [PCIE2] = SLOT_PCIE3,
+		   [SATA1] = SLOT_SATA1, [SATA2] = SLOT_SATA2 },
+	[0x1c] = { [PCIE1] = SLOT_PCIE1,
+		   [SATA1] = SLOT_SATA1, [SATA2] = SLOT_SATA2 },
+	[0x1e] = { [PCIE1] = SLOT_PCIE1, [PCIE3] = SLOT_PCIE3 },
+	[0x1f] = { [PCIE1] = SLOT_PCIE1 },
+};
+
+
+/*
+ * Returns the name of the slot to which the PCIe or SATA controller is
+ * connected
+ */
+const char *board_serdes_name(enum srds_prtcl device)
+{
+	ccsr_gur_t *gur = (void *)CONFIG_SYS_MPC85xx_GUTS_ADDR;
+	u32 pordevsr = in_be32(&gur->pordevsr);
+	unsigned int srds_cfg = (pordevsr & MPC85xx_PORDEVSR_IO_SEL) >>
+				MPC85xx_PORDEVSR_IO_SEL_SHIFT;
+	enum slot_id slot = serdes_dev_slot[srds_cfg][device];
+	const char *name = slot_names[slot];
+
+	if (name)
+		return name;
+	else
+		return "Nothing";
+}
+
+void hw_watchdog_reset(void)
+{
+	ccsr_gpio_t *pgpio = (void *)(CONFIG_SYS_MPC85xx_GPIO3_ADDR);
+
+	clrbits_be32(&pgpio->gpdat, 0x00000400);
+	setbits_be32(&pgpio->gpdat, 0x00000400);
+}
+
+#ifdef CONFIG_TRAILBLAZER
+int do_bootd(cmd_tbl_t *cmdtp, int flag, int argc, char * const argv[])
+{
+	int rcode = 0;
+
+	if (run_command(getenv("bootcmd"), flag) < 0)
+		rcode = 1;
+	return rcode;
+}
+
+int board_early_init_r(void)
+{
+	ccsr_gpio_t *pgpio = (void *)(CONFIG_SYS_MPC85xx_GPIO3_ADDR);
+
+	/*
+	 * GPIO3_12 PPC_SYSTEMREADY#
+	 */
+	setbits_be32(&pgpio->gpdir, 0x00080000);
+	setbits_be32(&pgpio->gpodr, 0x00080000);
+	clrbits_be32(&pgpio->gpdat, 0x00080000);
+
+	return ccdm_compute_self_hash();
+}
+
+int last_stage_init(void)
+{
+	startup_ccdm_id_module();
+	return 0;
+}
+
+#else
+void pci_init_board(void)
+{
+	fsl_pcie_init_board(0);
+
+	hydra_initialize();
+}
+
+int board_early_init_r(void)
+{
+	unsigned int k = 0;
+	ccsr_gpio_t *pgpio = (void *)(CONFIG_SYS_MPC85xx_GPIO3_ADDR);
+
+	/* wait for FPGA configuration to finish */
+	while (!pca9698_get_value(0x22, 11) && (k++ < 30))
+		udelay(100000);
+
+	if (k > 30) {
+		puts("FPGA configuration timed out.\n");
+	} else {
+		/* clear FPGA reset */
+		udelay(1000);
+		setbits_be32(&pgpio->gpdat, 0x00100000);
+	}
+
+	/* give time for PCIe link training */
+	udelay(100000);
+
+	/*
+	 * GPIO3_12 PPC_SYSTEMREADY#
+	 */
+	setbits_be32(&pgpio->gpdir, 0x00080000);
+	setbits_be32(&pgpio->gpodr, 0x00080000);
+	clrbits_be32(&pgpio->gpdat, 0x00080000);
+
+	return 0;
+}
+
+int last_stage_init(void)
+{
+	/* Turn on Parade DP501 */
+	pca9698_direction_output(0x22, 7, 1);
+	udelay(500000);
+
+	dp501_powerup(0x08);
+
+	startup_ccdm_id_module();
+
+	return 0;
+}
+
+/*
+ * Initialize on-board and/or PCI Ethernet devices
+ *
+ * Returns:
+ *      <0, error
+ *       0, no ethernet devices found
+ *      >0, number of ethernet devices initialized
+ */
+int board_eth_init(bd_t *bis)
+{
+	struct fsl_pq_mdio_info mdio_info;
+	struct tsec_info_struct tsec_info[2];
+	unsigned int num = 0;
+
+#ifdef CONFIG_TSEC1
+	SET_STD_TSEC_INFO(tsec_info[num], 1);
+	num++;
+#endif
+#ifdef CONFIG_TSEC2
+	SET_STD_TSEC_INFO(tsec_info[num], 2);
+	num++;
+#endif
+
+	mdio_info.regs = (struct tsec_mii_mng *)CONFIG_SYS_MDIO_BASE_ADDR;
+	mdio_info.name = DEFAULT_MII_NAME;
+	fsl_pq_mdio_init(bis, &mdio_info);
+
+	return tsec_eth_init(bis, tsec_info, num) + pci_eth_init(bis);
+}
+
+#ifdef CONFIG_OF_BOARD_SETUP
+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_HAS_FSL_DR_USB
+	fdt_fixup_dr_usb(blob, bd);
+#endif
+
+	FT_FSL_PCI_SETUP;
+}
+#endif
+
+static void hydra_initialize(void)
+{
+	unsigned int i;
+	pci_dev_t devno;
+
+	/* Find and probe all the matching PCI devices */
+	for (i = 0; (devno = pci_find_devices(hydra_supported, i)) >= 0; i++) {
+		u32 val;
+		struct ihs_fpga *fpga;
+		u32 versions;
+		u32 fpga_version;
+		u32 fpga_features;
+
+		unsigned hardware_version;
+		unsigned feature_uart_channels;
+		unsigned feature_sb_channels;
+
+		/* Try to enable I/O accesses and bus-mastering */
+		val = PCI_COMMAND_MEMORY | PCI_COMMAND_MASTER;
+		pci_write_config_dword(devno, PCI_COMMAND, val);
+
+		/* Make sure it worked */
+		pci_read_config_dword(devno, PCI_COMMAND, &val);
+		if (!(val & PCI_COMMAND_MEMORY)) {
+			puts("Can't enable I/O memory\n");
+			continue;
+		}
+		if (!(val & PCI_COMMAND_MASTER)) {
+			puts("Can't enable bus-mastering\n");
+			continue;
+		}
+
+		/* read FPGA details */
+		fpga = pci_map_bar(devno, PCI_BASE_ADDRESS_0,
+			PCI_REGION_MEM);
+
+		versions = readl(fpga->versions);
+		fpga_version = readl(fpga->fpga_version);
+		fpga_features = readl(fpga->fpga_features);
+
+		hardware_version = versions & 0xf;
+		feature_uart_channels = (fpga_features >> 6) & 0x1f;
+		feature_sb_channels = fpga_features & 0x1f;
+
+		printf("FPGA%d: ", i);
+
+		switch (hardware_version) {
+		case HWVER_100:
+			printf("HW-Ver 1.00\n");
+			break;
+
+		case HWVER_110:
+			printf("HW-Ver 1.10\n");
+			break;
+
+		case HWVER_120:
+			printf("HW-Ver 1.20\n");
+			break;
+
+		default:
+			printf("HW-Ver %d(not supported)\n",
+			       hardware_version);
+			break;
+		}
+
+		printf("       FPGA V %d.%02d, features:",
+		       fpga_version / 100, fpga_version % 100);
+
+		printf(" %d uart channel(s)", feature_uart_channels);
+		printf(" %d sideband channel(s)\n", feature_sb_channels);
+	}
+}
+#endif
diff --git a/board/gdsys/p1022/ddr.c b/board/gdsys/p1022/ddr.c
new file mode 100644
index 00000000000..4a652de4306
--- /dev/null
+++ b/board/gdsys/p1022/ddr.c
@@ -0,0 +1,71 @@
+/*
+ * Copyright 2010 Freescale Semiconductor, Inc.
+ * Authors: Srikanth Srinivasan <srikanth.srinivasan@freescale.com>
+ *          Timur Tabi <timur@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.
+ */
+
+#include <common.h>
+#include <i2c.h>
+
+#include <asm/fsl_ddr_sdram.h>
+#include <asm/fsl_ddr_dimm_params.h>
+
+void fsl_ddr_board_options(memctl_options_t *popts, dimm_params_t *pdimm,
+			   unsigned int ctrl_num)
+{
+	unsigned int i;
+
+	if (ctrl_num) {
+		printf("Wrong parameter for controller number %d", ctrl_num);
+		return;
+	}
+	if (!pdimm->n_ranks)
+		return;
+
+	/* set odt_rd_cfg and odt_wr_cfg. */
+	for (i = 0; i < CONFIG_CHIP_SELECTS_PER_CTRL; i++) {
+		popts->cs_local_opts[i].odt_rd_cfg = 0;
+		popts->cs_local_opts[i].odt_wr_cfg = 1;
+	}
+
+	popts->clk_adjust = 5;
+	popts->cpo_override = 0x1f;
+	popts->write_data_delay = 2;
+	popts->half_strength_driver_enable = 1;
+
+	/* Per AN4039, enable ZQ calibration. */
+	popts->zq_en = 1;
+}
+
+#ifdef CONFIG_SPD_EEPROM
+/*
+ * we only have a "fake" SPD-EEPROM here, which has 16 bit addresses
+ */
+void get_spd(generic_spd_eeprom_t *spd, u8 i2c_address)
+{
+	int ret = i2c_read(i2c_address, 0, 2, (uchar *)spd,
+				sizeof(generic_spd_eeprom_t));
+
+	if (ret) {
+		if (i2c_address ==
+#ifdef SPD_EEPROM_ADDRESS
+				SPD_EEPROM_ADDRESS
+#elif defined(SPD_EEPROM_ADDRESS1)
+				SPD_EEPROM_ADDRESS1
+#endif
+				) {
+			printf("DDR: failed to read SPD from address %u\n",
+			       i2c_address);
+		} else {
+			debug("DDR: failed to read SPD from address %u\n",
+			      i2c_address);
+		}
+		memset(spd, 0, sizeof(generic_spd_eeprom_t));
+	}
+}
+#endif
diff --git a/board/gdsys/p1022/diu.c b/board/gdsys/p1022/diu.c
new file mode 100644
index 00000000000..52ac1e6ecc1
--- /dev/null
+++ b/board/gdsys/p1022/diu.c
@@ -0,0 +1,87 @@
+/*
+ * Copyright 2010-2011 Freescale Semiconductor, Inc.
+ * Authors: Timur Tabi <timur@freescale.com>
+ *
+ * FSL DIU Framebuffer driver
+ *
+ * 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.
+ */
+
+#include <common.h>
+#include <command.h>
+#include <linux/ctype.h>
+#include <asm/io.h>
+#include <stdio_dev.h>
+#include <video_fb.h>
+#include <fsl_diu_fb.h>
+
+#define PMUXCR_ELBCDIU_MASK	0xc0000000
+#define PMUXCR_ELBCDIU_NOR16	0x80000000
+#define PMUXCR_ELBCDIU_DIU	0x40000000
+
+/*
+ * DIU Area Descriptor
+ *
+ * Note that we need to byte-swap the value before it's written to the AD
+ * register.  So even though the registers don't look like they're in the same
+ * bit positions as they are on the MPC8610, the same value is written to the
+ * AD register on the MPC8610 and on the P1022.
+ */
+#define AD_BYTE_F		0x10000000
+#define AD_ALPHA_C_SHIFT	25
+#define AD_BLUE_C_SHIFT		23
+#define AD_GREEN_C_SHIFT	21
+#define AD_RED_C_SHIFT		19
+#define AD_PIXEL_S_SHIFT	16
+#define AD_COMP_3_SHIFT		12
+#define AD_COMP_2_SHIFT		8
+#define AD_COMP_1_SHIFT		4
+#define AD_COMP_0_SHIFT		0
+
+/*
+ * Variables used by the DIU/LBC switching code.  It's safe to makes these
+ * global, because the DIU requires DDR, so we'll only run this code after
+ * relocation.
+ */
+static u32 pmuxcr;
+
+void diu_set_pixel_clock(unsigned int pixclock)
+{
+	ccsr_gur_t *gur = (void *)(CONFIG_SYS_MPC85xx_GUTS_ADDR);
+	unsigned long speed_ccb, temp;
+	u32 pixval;
+
+	speed_ccb = get_bus_freq(0);
+	temp = 1000000000 / pixclock;
+	temp *= 1000;
+	pixval = speed_ccb / temp;
+	debug("DIU pixval = %u\n", pixval);
+
+	/* Modify PXCLK in GUTS CLKDVDR */
+	temp = in_be32(&gur->clkdvdr) & 0x2000FFFF;
+	out_be32(&gur->clkdvdr, temp);			/* turn off clock */
+	out_be32(&gur->clkdvdr, temp | 0x80000000 | ((pixval & 0x1F) << 16));
+}
+
+int platform_diu_init(unsigned int xres, unsigned int yres, const char *port)
+{
+	ccsr_gur_t *gur = (void *)(CONFIG_SYS_MPC85xx_GUTS_ADDR);
+	u32 pixel_format;
+
+	pixel_format = cpu_to_le32(AD_BYTE_F | (3 << AD_ALPHA_C_SHIFT) |
+		(0 << AD_BLUE_C_SHIFT) | (1 << AD_GREEN_C_SHIFT) |
+		(2 << AD_RED_C_SHIFT) | (8 << AD_COMP_3_SHIFT) |
+		(8 << AD_COMP_2_SHIFT) | (8 << AD_COMP_1_SHIFT) |
+		(8 << AD_COMP_0_SHIFT) | (3 << AD_PIXEL_S_SHIFT));
+
+	printf("DIU:   Switching to %ux%u\n", xres, yres);
+
+	/* Set PMUXCR to switch the muxed pins from the LBC to the DIU */
+	clrsetbits_be32(&gur->pmuxcr, PMUXCR_ELBCDIU_MASK, PMUXCR_ELBCDIU_DIU);
+	pmuxcr = in_be32(&gur->pmuxcr);
+
+	return fsl_diu_init(xres, yres, pixel_format, 0);
+}
diff --git a/board/gdsys/p1022/law.c b/board/gdsys/p1022/law.c
new file mode 100644
index 00000000000..96f38f7c6fe
--- /dev/null
+++ b/board/gdsys/p1022/law.c
@@ -0,0 +1,20 @@
+/*
+ * Copyright 2010 Freescale Semiconductor, Inc.
+ * Authors: Srikanth Srinivasan <srikanth.srinivasan@freescale.com>
+ *          Timur Tabi <timur@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.
+ */
+
+#include <common.h>
+#include <asm/fsl_law.h>
+#include <asm/mmu.h>
+
+struct law_entry law_table[] = {
+	SET_LAW(CONFIG_SYS_ELBC_BASE_PHYS, LAW_SIZE_1M, LAW_TRGT_IF_LBC),
+};
+
+int num_law_entries = ARRAY_SIZE(law_table);
diff --git a/board/gdsys/p1022/sdhc_boot.c b/board/gdsys/p1022/sdhc_boot.c
new file mode 100644
index 00000000000..e4323181fc6
--- /dev/null
+++ b/board/gdsys/p1022/sdhc_boot.c
@@ -0,0 +1,63 @@
+/*
+ * 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 <mmc.h>
+#include <malloc.h>
+
+/*
+ * The environment variables are written to just after the u-boot image
+ * on SDCard, so we must read the MBR to get the start address and code
+ * length of the u-boot image, then calculate the address of the env.
+ */
+#define ESDHC_BOOT_IMAGE_SIZE	0x48
+#define ESDHC_BOOT_IMAGE_ADDR	0x50
+
+int mmc_get_env_addr(struct mmc *mmc, u32 *env_addr)
+{
+	u8 *tmp_buf;
+	u32 blklen, code_offset, code_len, n;
+
+	blklen = mmc->read_bl_len;
+	tmp_buf = malloc(blklen);
+	if (!tmp_buf)
+		return 1;
+
+	/* read out the first block, get the config data information */
+	n = mmc->block_dev.block_read(mmc->block_dev.dev, 0, 1, tmp_buf);
+	if (!n) {
+		free(tmp_buf);
+		return 1;
+	}
+
+	/* Get the Source Address, from offset 0x50 */
+	code_offset = *(u32 *)(tmp_buf + ESDHC_BOOT_IMAGE_ADDR);
+
+	/* Get the code size from offset 0x48 */
+	code_len = *(u32 *)(tmp_buf + ESDHC_BOOT_IMAGE_SIZE);
+
+	*env_addr = code_offset + code_len;
+
+	free(tmp_buf);
+
+	return 0;
+}
diff --git a/board/gdsys/p1022/tlb.c b/board/gdsys/p1022/tlb.c
new file mode 100644
index 00000000000..9cad6927833
--- /dev/null
+++ b/board/gdsys/p1022/tlb.c
@@ -0,0 +1,77 @@
+/*
+ * Copyright 2010 Freescale Semiconductor, Inc.
+ * Authors: Srikanth Srinivasan <srikanth.srinivasan@freescale.com>
+ *          Timur Tabi <timur@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.
+ */
+
+#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,
+		      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 + 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 + 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 + 12 * 1024,
+		      MAS3_SX|MAS3_SW|MAS3_SR, 0,
+		      0, 0, BOOKE_PAGESZ_4K, 0),
+
+	/* TLB 1 */
+	/* *I*** - Covers boot page */
+	SET_TLB_ENTRY(1, 0xfffff000, 0xfffff000,
+		      MAS3_SX|MAS3_SW|MAS3_SR, MAS2_I,
+		      0, 0, BOOKE_PAGESZ_4K, 1),
+
+	/* *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_1M, 1),
+
+	/* *I*G* - eLBC */
+	SET_TLB_ENTRY(1, CONFIG_SYS_ELBC_BASE, CONFIG_SYS_ELBC_BASE_PHYS,
+		      MAS3_SX|MAS3_SW|MAS3_SR, MAS2_I|MAS2_G,
+		      0, 2, BOOKE_PAGESZ_1M, 1),
+
+#if defined(CONFIG_TRAILBLAZER)
+	/* *I*G - L2SRAM */
+	SET_TLB_ENTRY(1, CONFIG_SYS_INIT_L2_ADDR, CONFIG_SYS_INIT_L2_ADDR_PHYS,
+		      MAS3_SX|MAS3_SW|MAS3_SR, MAS2_I|MAS2_G,
+		      0, 9, BOOKE_PAGESZ_256K, 1),
+#else
+	/* *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_256M, 1),
+	SET_TLB_ENTRY(1, CONFIG_SYS_PCIE1_MEM_VIRT + 0x10000000,
+		      CONFIG_SYS_PCIE1_MEM_PHYS + 0x10000000,
+		      MAS3_SX|MAS3_SW|MAS3_SR, MAS2_I|MAS2_G,
+		      0, 4, 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, 5, BOOKE_PAGESZ_256K, 1),
+
+#ifdef CONFIG_SYS_RAMBOOT
+	SET_TLB_ENTRY(1, CONFIG_SYS_DDR_SDRAM_BASE, CONFIG_SYS_DDR_SDRAM_BASE,
+		      MAS3_SX|MAS3_SW|MAS3_SR, 0,
+		      0, 6, BOOKE_PAGESZ_1G, 1),
+#endif
+#endif
+};
+
+int num_tlb_entries = ARRAY_SIZE(tlb_table);