soc/marvell/mvmap2315: Add load_validate driver

Load_validate: it loads and validates images from
flash using hash values stored in the BDB.

Testing: booted successfully.

Change-Id: I0b00e8c60ed76622d03cb232d5c4273b4077aae7
Signed-off-by: Hakim Giydan <hgiydan@marvell.com>
Reviewed-on: https://review.coreboot.org/16148
Tested-by: build bot (Jenkins)
Reviewed-by: Martin Roth <martinroth@google.com>

11 files changed, 526 insertions, 2 deletions

diff --git a/src/soc/marvell/mvmap2315/Makefile.inc b/src/soc/marvell/mvmap2315/Makefile.inc
index d7b23762a2..a79d5a8e19 100644
--- a/src/soc/marvell/mvmap2315/Makefile.inc
+++ b/src/soc/marvell/mvmap2315/Makefile.inc
@@ -16,11 +16,14 @@
 ifeq ($(CONFIG_SOC_MARVELL_MVMAP2315),y)
 
 bootblock-y += bootblock.c
+bootblock-y += bdb.c
 bootblock-y += clock.c
+bootblock-y += digest.c
 bootblock-y += fiq.S
 bootblock-y += gic.c
 bootblock-y += gpio.c
 bootblock-y += flash.c
+bootblock-y += load_validate.c
 bootblock-y += media.c
 bootblock-y += pinmux.c
 bootblock-y += reset.c
diff --git a/src/soc/marvell/mvmap2315/bdb.c b/src/soc/marvell/mvmap2315/bdb.c
new file mode 100644
index 0000000000..6be414cb39
--- /dev/null
+++ b/src/soc/marvell/mvmap2315/bdb.c
@@ -0,0 +1,79 @@
+/*
+ * This file is part of the coreboot project.
+ *
+ * Copyright (C) 2016 Marvell, 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; version 2 of the License.
+ *
+ * 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.
+ */
+
+#include <stddef.h>
+#include <stdint.h>
+#include <stdlib.h>
+
+#include <arch/io.h>
+#include <console/console.h>
+#include <soc/bdb.h>
+
+void set_bdb_pointers(u8 *start_addr, struct bdb_pointer *bdb_in)
+{
+	bdb_in->bdb_h
+		= (struct bdb_header *)start_addr;
+
+	bdb_in->bdb_k
+		= (struct bdb_key *)(start_addr
+			+ (bdb_in->bdb_h->struct_size));
+
+	bdb_in->bdb_oem_0
+		= (u8 *)((u32)(bdb_in->bdb_k)
+			+ (bdb_in->bdb_k->struct_size));
+
+	bdb_in->sub_k
+		= (struct bdb_key *)((u32)(bdb_in->bdb_oem_0)
+			+ (bdb_in->bdb_h->oem_area_0_size));
+
+	bdb_in->bdb_h_s
+		= (struct bdb_sig *)((u32)(bdb_in->bdb_oem_0)
+			+ (bdb_in->bdb_h->signed_size));
+
+	bdb_in->bdb_d
+		= (struct bdb_data *)((u32)(bdb_in->bdb_h_s)
+			+ (bdb_in->bdb_h_s->struct_size));
+
+	bdb_in->oem_1
+		= (u8 *)((u32)(bdb_in->bdb_d)
+			+ (bdb_in->bdb_d->struct_size));
+
+	bdb_in->bdb_hash
+		= (struct bdb_hash *)((u32)(bdb_in->oem_1)
+			+ (bdb_in->bdb_d->oem_area_1_size));
+
+	bdb_in->bdb_s
+		= (struct bdb_sig *)((u32)(bdb_in->bdb_d)
+			+ (bdb_in->bdb_d->signed_size));
+}
+
+struct bdb_hash *find_bdb_image(struct bdb_pointer *bdb_info, u32 image_type)
+{
+	int i;
+
+	if (!bdb_info) {
+		printk(BIOS_DEBUG, "can't find BDB\n");
+		return NULL;
+	}
+
+	for (i = 0; i < bdb_info->bdb_d->num_hashes; i++) {
+		if (bdb_info->bdb_hash[i].type == image_type)
+			return &bdb_info->bdb_hash[i];
+	}
+
+	printk(BIOS_DEBUG, "can't find image with type %d in the BDB\n",
+	       image_type);
+	return NULL;
+}
diff --git a/src/soc/marvell/mvmap2315/bootblock.c b/src/soc/marvell/mvmap2315/bootblock.c
index 5aa9eb547b..47bf470abb 100644
--- a/src/soc/marvell/mvmap2315/bootblock.c
+++ b/src/soc/marvell/mvmap2315/bootblock.c
@@ -17,11 +17,15 @@
 #include <stdint.h>
 #include <stdlib.h>
 
+#include <arch/io.h>
 #include <bootblock_common.h>
 #include <console/console.h>
 #include <timestamp.h>
 #include <console/uart.h>
+#include <soc/addressmap.h>
+#include <soc/bdb.h>
 #include <soc/gic.h>
+#include <soc/load_validate.h>
 #include <soc/uart.h>
 
 void bootblock_soc_early_init(void)
@@ -37,10 +41,32 @@ void bootblock_soc_early_init(void)
 
 void bootblock_soc_init(void)
 {
+	struct bdb_pointer bdb_info;
+
+	write32((void *)MVMAP2315_BOOTBLOCK_CB1, 0);
+	write32((void *)MVMAP2315_BOOTBLOCK_CB2, 0);
+
+	set_bdb_pointers((u8 *)MVMAP2315_BDB_LCM_BASE, &bdb_info);
+
+	printk(BIOS_DEBUG, "loading and validating APMU firmware.\n");
+	load_and_validate(&bdb_info, APMU_FIRMWARE);
+
+	printk(BIOS_DEBUG, "loading and validating MCU firmware.\n");
+	load_and_validate(&bdb_info, MCU_FIRMWARE);
+
 	/* initializing UART1 to free UART0 to be used by romstage */
 	uart_num = 1;
 	uart_init(uart_num);
 
+	while (read32((void *)MVMAP2315_BOOTBLOCK_CB1) != 0x4)
+		;
+
+	printk(BIOS_DEBUG, "loading and validating AP_RW firmware.\n");
+	load_and_validate(&bdb_info, AP_RW_FIRMWARE);
+
+	write32((void *)MVMAP2315_BOOTBLOCK_CB2, 0x4)
+		;
+
 	while (1)
 		;
 }
diff --git a/src/soc/marvell/mvmap2315/digest.c b/src/soc/marvell/mvmap2315/digest.c
new file mode 100644
index 0000000000..35cd5b1c4d
--- /dev/null
+++ b/src/soc/marvell/mvmap2315/digest.c
@@ -0,0 +1,64 @@
+/*
+ * This file is part of the coreboot project.
+ *
+ * Copyright (C) 2016 Marvell, 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; version 2 of the License.
+ *
+ * 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.
+ */
+
+#include <stddef.h>
+#include <stdint.h>
+#include <stdlib.h>
+
+#include <arch/io.h>
+#include <console/console.h>
+#include <soc/digest.h>
+#include <string.h>
+
+struct digest_ops digest_callbacks = {
+	.init = (security_init_F) MVMAP2315_DIGEST_INIT,
+	.sha_hash = (sha_hash_F) MVMAP2315_DIGEST_SHA_HASH,
+};
+
+u32 digest_init(void)
+{
+	int rc;
+
+	rc = digest_callbacks.init(0);
+
+	if (rc)
+		printk(BIOS_DEBUG, "digest_init failed with rc=%x.\n", rc);
+
+	return rc;
+}
+
+u32 digest_sha_hash(const u8 *msg, u32 msg_len, u8 *digest, u32 digest_len)
+{
+	int rc;
+
+	rc = digest_callbacks.sha_hash(msg, msg_len, digest, digest_len);
+
+	if (rc)
+		printk(BIOS_DEBUG, "digest_sha_hash failed with rc=%x.\n", rc);
+
+	return rc;
+}
+
+u32 digest_cmp(const u8 *msg, u8 *digest, u32 digest_len)
+{
+	int rc;
+
+	rc = memcmp(msg, digest, digest_len);
+
+	if (rc)
+		printk(BIOS_DEBUG, "image hash doesn't match BDB expected");
+
+	return rc;
+}
diff --git a/src/soc/marvell/mvmap2315/include/soc/addressmap.h b/src/soc/marvell/mvmap2315/include/soc/addressmap.h
index f646b00e5a..1b3843421a 100644
--- a/src/soc/marvell/mvmap2315/include/soc/addressmap.h
+++ b/src/soc/marvell/mvmap2315/include/soc/addressmap.h
@@ -22,6 +22,9 @@
 
 #define MVMAP2315_CBFS_BASE		0x00400000
 
+#define MVMAP2315_BOOTBLOCK_CB1		0xE0009510
+#define MVMAP2315_BOOTBLOCK_CB2		0xE0009514
+
 #define MVMAP2315_PINMUX_BASE		0xE0140000
 #define MVMAP2315_TIMER0_BASE		0xE1020000
 #define MVMAP2315_GPIOF_BASE		0xE0142000
@@ -35,6 +38,7 @@
 #define MVMAP2315_APMU_CLK_BASE		0xE0125400
 #define MVMAP2315_GENTIMER_BASE		0xE0137000
 #define MVMAP2315_MPMU_CLK_BASE		0xEF000800
+#define MVMAP2315_MCU_SECCONFIG_BASE	0xED600000
 
 #define MVMAP2315_RAM_BASE		0x00000000
 #define MVMAP2315_DEVICE_BASE		0x80000000
diff --git a/src/soc/marvell/mvmap2315/include/soc/bdb.h b/src/soc/marvell/mvmap2315/include/soc/bdb.h
new file mode 100644
index 0000000000..f182a44ad2
--- /dev/null
+++ b/src/soc/marvell/mvmap2315/include/soc/bdb.h
@@ -0,0 +1,112 @@
+/*
+ * This file is part of the coreboot project.
+ *
+ * Copyright (C) 2016 Marvell, 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; version 2 of the License.
+ *
+ * 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.
+ */
+
+#ifndef __SOC_MARVELL_MVMAP2315_BDB_H__
+#define __SOC_MARVELL_MVMAP2315_BDB_H__
+
+#include <stdint.h>
+
+#define MVMAP2315_BDB_LCM_BASE		0xE0000000
+
+enum {
+	BDB_RESERVED = 0,
+	SP_RW_FIRMWARE = 1,
+	AP_RW_FIRMWARE = 2,
+	MCU_FIRMWARE = 3,
+	APMU_FIRMWARE = 4,
+	KERNEL_IMAGE = 128,
+	KERNEL_COMMAND_LINE = 129,
+	SIXTEEN_BIT_VMLINUX_HEADER = 130
+};
+
+struct bdb_header {
+	u32 struct_magic;
+	u8 struct_major_version;
+	u8 struct_minor_version;
+	u16 struct_size;
+	u64 bdb_load_address;
+	u32 bdb_size;
+	u32 signed_size;
+	u32 oem_area_0_size;
+	u8 reserved0[8];
+} __attribute__ ((packed));
+
+struct bdb_key {
+	u32 struct_magic;
+	u8 struct_major_version;
+	u8 struct_minor_version;
+	u16 struct_size;
+	u8 hash_alg;
+	u8 sig_alg;
+	u8 reserved0[2];
+	u32 key_version;
+	char description[128];
+	u8 key_data[];
+} __attribute__ ((packed));
+
+struct bdb_sig {
+	u32 struct_magic;
+	u8 struct_major_version;
+	u8 struct_minor_version;
+	u16 struct_size;
+	u8 hash_alg;
+	u8 sig_alg;
+	u8 reserved0[2];
+	u32 signed_size;
+	char description[128];
+	u8 sig_SOC_MARVELL_MVMAP2315_data[];
+} __attribute__ ((packed));
+
+struct bdb_data {
+	u32 struct_magic;
+	u8 struct_major_version;
+	u8 struct_minor_version;
+	u16 struct_size;
+	u32 data_version;
+	u32 oem_area_1_size;
+	u8 num_hashes;
+	u8 hash_entry_size;
+	u8 reserved0[2];
+	u32 signed_size;
+	u8 reserved1[8];
+	char description[128];
+} __attribute__ ((packed));
+
+struct bdb_hash {
+	u64 offset;
+	u32 size;
+	u8 partition;
+	u8 type;
+	u8 reserved0[2];
+	u64 load_address;
+	u8 digest[32];
+} __attribute__ ((packed));
+
+struct bdb_pointer {
+	struct bdb_header *bdb_h;
+	struct bdb_key	*bdb_k;
+	u8 *bdb_oem_0;
+	struct bdb_key *sub_k;
+	struct bdb_sig *bdb_h_s;
+	struct bdb_data *bdb_d;
+	u8 *oem_1;
+	struct bdb_hash *bdb_hash;
+	struct bdb_sig *bdb_s;
+} __attribute__ ((packed));
+
+void set_bdb_pointers(u8 *start_addr, struct bdb_pointer *bdb_in);
+struct bdb_hash *find_bdb_image(struct bdb_pointer *bdb_info, u32 image_type);
+
+#endif /* __SOC_MARVELL_MVMAP2315_BDB_H__ */
diff --git a/src/soc/marvell/mvmap2315/include/soc/digest.h b/src/soc/marvell/mvmap2315/include/soc/digest.h
new file mode 100644
index 0000000000..e81af17e1a
--- /dev/null
+++ b/src/soc/marvell/mvmap2315/include/soc/digest.h
@@ -0,0 +1,37 @@
+/*
+ * This file is part of the coreboot project.
+ *
+ * Copyright (C) 2016 Marvell, 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; version 2 of the License.
+ *
+ * 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.
+ */
+
+#ifndef __SOC_MARVELL_MVMAP2315_DIGEST_H__
+#define __SOC_MARVELL_MVMAP2315_DIGEST_H__
+
+#include <stdint.h>
+
+#define MVMAP2315_DIGEST_INIT		0xFFE00040
+#define MVMAP2315_DIGEST_SHA_HASH	0xFFE00044
+
+typedef u32 (*security_init_F)(u32 adv_ver);
+typedef u32 (*sha_hash_F)(const u8 *msg, u32 msg_len, u8 *digest,
+			  u32 digest_len);
+
+struct digest_ops {
+	security_init_F init;
+	sha_hash_F sha_hash;
+};
+
+u32 digest_init(void);
+u32 digest_sha_hash(const u8 *msg, u32 msg_len, u8 *digest, u32 digest_len);
+u32 digest_cmp(const u8 *msg, u8 *digest, u32 digest_len);
+
+#endif /* __SOC_MARVELL_MVMAP2315_DIGEST_H__ */
diff --git a/src/soc/marvell/mvmap2315/include/soc/load_validate.h b/src/soc/marvell/mvmap2315/include/soc/load_validate.h
new file mode 100644
index 0000000000..ba7f808809
--- /dev/null
+++ b/src/soc/marvell/mvmap2315/include/soc/load_validate.h
@@ -0,0 +1,68 @@
+/*
+ * This file is part of the coreboot project.
+ *
+ * Copyright (C) 2016 Marvell, 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; version 2 of the License.
+ *
+ * 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.
+ */
+
+#ifndef __SOC_MARVELL_MVMAP2315_LOAD_VALIDATE_H__
+#define __SOC_MARVELL_MVMAP2315_LOAD_VALIDATE_H__
+
+#include <stdint.h>
+
+#include <soc/addressmap.h>
+#include <types.h>
+
+enum {
+	PRIMARY_BDB = 0,
+	ALTERNATE_BDB = 1,
+	RECOVERY_BDB = 2,
+};
+
+#define MVMAP2315_BDB_TYPE		(BIT(0) | BIT(1))
+struct mvmap2315_bootrom_info {
+	u32 sp_ro_status;
+	u32 flash_media;
+};
+
+struct mvmap2315_mcu_secconfig_regs {
+	u8 _reserved0[0x04];
+	u32 strap_override_sec_cfg;
+	u32 sec_mcu_cfg_sec_cfg;
+	u32 lcm_mcu_cfg_sec_cfg0;
+	u32 lcm_mcu_cfg_sec_cfg1;
+	u32 lcm_mcu_cfg_sec_cfg2;
+	u8 _reserved1[0xe8];
+	u32 dap_debug_disable;
+	u32 boot_avs_status;
+	u32 pwr_clr_in;
+	u32 pwr_clr_recovery;
+	u32 pwr_clr_bdb;
+	u32 pwr_clr_fail_a;
+	u32 pwr_clr_fail_b;
+	u32 rst_clr_dev;
+	u32 rst_clr_wp1;
+	u32 rst_clr_wp2;
+	u32 rst_clr_lp;
+	u32 boot_gpio_out;
+	u32 boot_gpio_in;
+	u32 boot_hw_lockdown_nvm;
+	u32 boot_hw_lockdown_pinmux;
+	u32 boot_callback_pointer;
+};
+
+check_member(mvmap2315_mcu_secconfig_regs, boot_callback_pointer, 0x13c);
+static struct mvmap2315_mcu_secconfig_regs * const mvmap2315_mcu_secconfig
+					= (void *)MVMAP2315_MCU_SECCONFIG_BASE;
+
+void load_and_validate(struct bdb_pointer *bdb_info, u32 image_type);
+
+#endif /* __SOC_MARVELL_MVMAP2315_LOAD_VALIDATE_H__ */
diff --git a/src/soc/marvell/mvmap2315/include/soc/memlayout.ld b/src/soc/marvell/mvmap2315/include/soc/memlayout.ld
index e0f497aa28..ecac4b2c1f 100644
--- a/src/soc/marvell/mvmap2315/include/soc/memlayout.ld
+++ b/src/soc/marvell/mvmap2315/include/soc/memlayout.ld
@@ -46,8 +46,9 @@ SECTIONS
 #endif
 
 	PRERAM_CBFS_CACHE(0xE0009400, 256)
-	PRERAM_CBMEM_CONSOLE(0xE0009500, 8K)
-	REGION(fiq_stack, 0xE000B500, 2560, 1)
+	REGION(bootblock_CB, 0xE0009500, 256, 1)
+	PRERAM_CBMEM_CONSOLE(0xE0009600, 8K)
+	REGION(fiq_stack, 0xE000B600, 2560, 1)
 	BOOTBLOCK(0xE000C000, 16K)
 	ROMSTAGE(0xE0010000, 64K)
 	REGION(apmu, 0xE0020000, 64K, 1)
diff --git a/src/soc/marvell/mvmap2315/load_validate.c b/src/soc/marvell/mvmap2315/load_validate.c
new file mode 100644
index 0000000000..422c57f8b7
--- /dev/null
+++ b/src/soc/marvell/mvmap2315/load_validate.c
@@ -0,0 +1,123 @@
+/*
+ * This file is part of the coreboot project.
+ *
+ * Copyright (C) 2016 Marvell, 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; version 2 of the License.
+ *
+ * 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.
+ */
+
+#include <stddef.h>
+#include <stdint.h>
+#include <stdlib.h>
+
+#include <arch/io.h>
+#include <console/console.h>
+#include <soc/bdb.h>
+#include <soc/digest.h>
+#include <soc/flash.h>
+#include <soc/load_validate.h>
+#include <soc/reset.h>
+
+static void set_flash_parameters(struct flash_params *flash_info,
+				 struct bdb_hash *image_info)
+{
+	(*flash_info).offset
+		= (u32)(image_info->offset);
+	(*flash_info).buff
+		= (u32)(image_info->load_address);
+	(*flash_info).size = image_info->size;
+	(*flash_info).id = 0x0;
+	(*flash_info).partition = (u32)image_info->partition;
+}
+
+static void image_failure(void)
+{
+	struct mvmap2315_bootrom_info *bootrom_info
+		= (void *)read32(
+			&mvmap2315_mcu_secconfig->boot_callback_pointer);
+
+	printk(BIOS_DEBUG, "Resetting system!!\n");
+
+	if ((bootrom_info->sp_ro_status & MVMAP2315_BDB_TYPE)
+					== PRIMARY_BDB) {
+		write32(&mvmap2315_mcu_secconfig->pwr_clr_fail_a, 1);
+
+		if (read32(
+		&mvmap2315_mcu_secconfig->pwr_clr_fail_b)) {
+			printk(BIOS_DEBUG, "primary & ALT BDB boot failed\n");
+			printk(BIOS_DEBUG, "Rebooting to recovery mode...\n");
+			write32(&mvmap2315_mcu_secconfig->pwr_clr_recovery, 1);
+		} else {
+			printk(BIOS_DEBUG, "primary BDB boot failed\n");
+			printk(BIOS_DEBUG, "Rebooting using AlT BDB...\n");
+			write32(&mvmap2315_mcu_secconfig->pwr_clr_bdb, 1);
+		}
+	} else if ((bootrom_info->sp_ro_status & MVMAP2315_BDB_TYPE)
+					== ALTERNATE_BDB) {
+		write32(&mvmap2315_mcu_secconfig->pwr_clr_fail_b, 1);
+
+		if (read32(
+		&mvmap2315_mcu_secconfig->pwr_clr_fail_a)) {
+			printk(BIOS_DEBUG, "primary & ALT BDB boot failed\n");
+			printk(BIOS_DEBUG, "Rebooting to recovery mode...\n");
+			write32(&mvmap2315_mcu_secconfig->pwr_clr_recovery, 1);
+		} else {
+			printk(BIOS_DEBUG, "AlT BDB boot failed\n");
+			printk(BIOS_DEBUG, "Rebooting using primary BDB...\n");
+			write32(&mvmap2315_mcu_secconfig->pwr_clr_bdb, 0);
+		}
+	} else if ((bootrom_info->sp_ro_status & MVMAP2315_BDB_TYPE)
+					== RECOVERY_BDB) {
+		printk(BIOS_DEBUG, "Recovery image has errors!!!\n");
+		printk(BIOS_DEBUG, "Re-entering recovery mode...\n");
+		write32(&mvmap2315_mcu_secconfig->pwr_clr_recovery, 1);
+	}
+
+	mvmap2315_reset();
+}
+
+void load_and_validate(struct bdb_pointer *bdb_info, u32 image_type)
+{
+	struct bdb_hash *image_info;
+	struct flash_params flash_info;
+	struct mvmap2315_bootrom_info *bootrom_info
+		= (void *)read32(
+			&mvmap2315_mcu_secconfig->boot_callback_pointer);
+	u8 image_digest[32];
+
+	image_info = find_bdb_image(bdb_info, image_type);
+
+	if (!image_info)
+		image_failure();
+
+	set_flash_parameters(&flash_info, image_info);
+
+	if (flash_init(bootrom_info->flash_media, MVMAP2315_MMC_CLK_MHZ))
+		image_failure();
+
+	if (flash_partition(bootrom_info->flash_media, &flash_info))
+		image_failure();
+
+	if (flash_read(bootrom_info->flash_media, &flash_info))
+		image_failure();
+
+	if (flash_shutdown(bootrom_info->flash_media))
+		image_failure();
+
+	if (digest_init())
+		image_failure();
+
+	if (digest_sha_hash(((u8 *)(flash_info.buff)), image_info->size,
+			    &image_digest[0], 32))
+		image_failure();
+
+	if (digest_cmp(&image_digest[0], &image_info->digest[0], 32))
+		image_failure();
+}
diff --git a/src/soc/marvell/mvmap2315/romstage.c b/src/soc/marvell/mvmap2315/romstage.c
index dfd3bcf91a..cffa8d0bff 100644
--- a/src/soc/marvell/mvmap2315/romstage.c
+++ b/src/soc/marvell/mvmap2315/romstage.c
@@ -17,10 +17,12 @@
 #include <stdint.h>
 #include <stdlib.h>
 
+#include <arch/io.h>
 #include <arch/exception.h>
 #include <cbmem.h>
 #include <console/console.h>
 #include <program_loading.h>
+#include <soc/addressmap.h>
 #include <soc/clock.h>
 #include <soc/mmu_operations.h>
 
@@ -32,6 +34,11 @@ void main(void)
 
 	clock_init();
 
+	write32((void *)MVMAP2315_BOOTBLOCK_CB1, 0x4);
+
+	while (read32((void *)MVMAP2315_BOOTBLOCK_CB2) != 0x4)
+		;
+
 	mvmap2315_mmu_init();
 
 	cbmem_initialize_empty();