commonlib: Move drivers/storage into commonlib/storage

Move drivers/storage into commonlib/storage to enable access by
libpayload and indirectly by payloads.

* Remove SD/MMC specific include files from include/device
* Remove files from drivers/storage
* Add SD/MMC specific include files to commonlib/include
* Add files to commonlib/storage
* Fix header file references
* Add subdir entry in commonlib/Makefile.inc to build the SD/MMC driver
* Add Kconfig source for commonlib/storage
* Rename *DEVICE* to *COMMONLIB*
* Rename *DRIVERS_STORAGE* to *COMMONLIB_STORAGE*

TEST=Build and run on Galileo Gen2

Change-Id: I4339e4378491db9a0da1f2dc34e1906a5ba31ad6
Signed-off-by: Lee Leahy <Leroy.P.Leahy@intel.com>
Reviewed-on: https://review.coreboot.org/19672
Tested-by: build bot (Jenkins) <no-reply@coreboot.org>
Reviewed-by: Patrick Georgi <pgeorgi@google.com>

18 files changed, 3779 insertions, 0 deletions

diff --git a/src/commonlib/storage/Kconfig b/src/commonlib/storage/Kconfig
new file mode 100644
index 0000000000..c7bb70d399
--- /dev/null
+++ b/src/commonlib/storage/Kconfig
@@ -0,0 +1,105 @@
+##
+## This file is part of the coreboot project.
+##
+## Copyright (C) 2017 Intel Corp.
+##
+## 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.
+##
+
+config COMMONLIB_STORAGE
+	bool
+	default n
+
+if COMMONLIB_STORAGE
+
+config COMMONLIB_STORAGE_MMC
+	bool "Enable MultiMediaCard (MMC) and eMMC device support"
+	default n
+
+config COMMONLIB_STORAGE_SD
+	bool "Enable Secure Digital (SD) memory card support"
+	default n
+
+config STORAGE_ERASE
+	bool "Support SD/MMC erase operations"
+	default n
+	help
+	  Select to enable SD/MMC erase oprations
+
+config STORAGE_EARLY_ERASE
+	bool "Enable erase operations in bootblock and verstage"
+	default n
+	depends on STORAGE_ERASE
+
+config STORAGE_WRITE
+	bool "Support SD/MMC write operations"
+	default n
+	help
+	  Select to enable SD/MMC write oprations
+
+config STORAGE_EARLY_WRITE
+	bool "Enable write operations in bootblock and verstage"
+	default n
+	depends on STORAGE_WRITE
+
+config SD_MMC_DEBUG
+	bool "Debug SD/MMC card/devices operations"
+	default n
+	help
+	  Display overview of SD/MMC card/device operations
+
+config SD_MMC_TRACE
+	bool "Trace SD/MMC card/device operations"
+	default n
+	help
+	  Display details of SD/MMC card/device operations
+
+config SDHC_DEBUG
+	bool "Debug SD/MMC controller settings"
+	default n
+	help
+	  Display clock speed and bus width settings
+
+config SDHC_TRACE
+	bool "Trace SD/MMC controller operations"
+	default n
+	help
+	  Display the operations performed by the SD/MMC controller
+
+config SDHCI_CONTROLLER
+	bool "Support SD host controller"
+	default n
+
+if SDHCI_CONTROLLER
+
+config SDHCI_ADMA_IN_BOOTBLOCK
+	bool
+	default n
+	help
+	  Determine if bootblock is able to use ADMA2 or ADMA64
+
+config SDHCI_ADMA_IN_ROMSTAGE
+	bool
+	default n
+	help
+	  Determine if romstage is able to use ADMA2 or ADMA64
+
+config SDHCI_ADMA_IN_VERSTAGE
+	bool
+	default n
+	help
+	  Determine if verstage is able to use ADMA2 or ADMA64
+
+config SDHCI_BOUNCE_BUFFER
+	bool "Use DMA bounce buffer for SD/MMC controller"
+	default n
+
+endif # SDHCI_CONTROLLER
+endif # COMMONLIB_STORAGE
diff --git a/src/commonlib/storage/Makefile.inc b/src/commonlib/storage/Makefile.inc
new file mode 100644
index 0000000000..af5137e287
--- /dev/null
+++ b/src/commonlib/storage/Makefile.inc
@@ -0,0 +1,107 @@
+#
+# This file is part of the coreboot project.
+#
+# Copyright (C) 2017 Intel Corporation.
+#
+# 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.
+#
+
+ifeq ($(CONFIG_COMMONLIB_STORAGE),y)
+
+bootblock-y += sd_mmc.c
+bootblock-y += storage.c
+
+verstage-y += sd_mmc.c
+verstage-y += storage.c
+
+romstage-y += sd_mmc.c
+romstage-y += storage.c
+
+postcar-y += sd_mmc.c
+postcar-y += storage.c
+
+ramstage-y += sd_mmc.c
+ramstage-y += storage.c
+
+# Determine the type of controller being used
+ifeq ($(CONFIG_SDHCI_CONTROLLER),y)
+bootblock-y += pci_sdhci.c
+bootblock-y += sdhci.c
+bootblock-$(CONFIG_SDHCI_ADMA_IN_BOOTBLOCK) += sdhci_adma.c
+bootblock-y += sdhci_display.c
+
+verstage-y += pci_sdhci.c
+verstage-y += sdhci.c
+verstage-$(CONFIG_SDHCI_ADMA_IN_VERSTAGE) += sdhci_adma.c
+verstage-y += sdhci_display.c
+
+romstage-y += pci_sdhci.c
+romstage-y += sdhci.c
+romstage-$(CONFIG_SDHCI_ADMA_IN_ROMSTAGE) += sdhci_adma.c
+romstage-y += sdhci_display.c
+
+postcar-y += pci_sdhci.c
+postcar-y += sdhci.c
+postcar-y += sdhci_adma.c
+postcar-y += sdhci_display.c
+
+ramstage-y += pci_sdhci.c
+ramstage-y += sdhci.c
+ramstage-y += sdhci_adma.c
+ramstage-y += sdhci_display.c
+
+# Determine if the bounce buffer is necessary
+ifeq ($(CONFIG_SDHCI_BOUNCE_BUFFER),y)
+bootblock-y += bouncebuf.c
+verstage-y += bouncebuf.c
+romstage-y += bouncebuf.c
+postcar-y += bouncebuf.c
+ramstage-y += bouncebuf.c
+endif # CONFIG_SDHCI_BOUNCE_BUFFER
+
+endif # CONFIG_SDHCI_CONTROLLER
+
+# Determine if MultiMediaCards or embedded MMC devices are supported
+ifeq ($(CONFIG_COMMONLIB_STORAGE_MMC),y)
+bootblock-y += mmc.c
+verstage-y += mmc.c
+romstage-y += mmc.c
+postcar-y += mmc.c
+ramstage-y += mmc.c
+endif # CONFIG_COMMONLIB_STORAGE_MMC
+
+# Determine if Secure Digital cards are supported
+ifeq ($(CONFIG_COMMONLIB_STORAGE_SD),y)
+bootblock-y += sd.c
+verstage-y += sd.c
+romstage-y += sd.c
+postcar-y += sd.c
+ramstage-y += sd.c
+endif # CONFIG_COMMONLIB_STORAGE_SD
+
+# Determine if erase operations are supported
+ifeq ($(CONFIG_STORAGE_ERASE),y)
+bootblock-$(CONFIG_STORAGE_EARLY_ERASE) += storage_erase.c
+verstage-$(CONFIG_STORAGE_EARLY_ERASE) += storage_erase.c
+romstage-y += storage_erase.c
+postcar-y += storage_erase.c
+ramstage-y += storage_erase.c
+endif # CONFIG_STORAGE_ERASE
+
+# Determine if write operations are supported
+ifeq ($(CONFIG_STORAGE_WRITE),y)
+bootblock-$(CONFIG_STORAGE_EARLY_WRITE) += storage_write.c
+verstage-$(CONFIG_STORAGE_EARLY_WRITE) += storage_write.c
+romstage-y += storage_write.c
+postcar-y += storage_write.c
+ramstage-y += storage_write.c
+endif # CONFIG_STORAGE_WRITE
+
+endif # CONFIG_COMMONLIB_STORAGE
diff --git a/src/commonlib/storage/bouncebuf.c b/src/commonlib/storage/bouncebuf.c
new file mode 100644
index 0000000000..c8125aaa42
--- /dev/null
+++ b/src/commonlib/storage/bouncebuf.c
@@ -0,0 +1,92 @@
+/*
+ * Generic bounce buffer implementation
+ *
+ * Copyright (C) 2012 Marek Vasut <marex@denx.de>
+ * Copyright 2013 Google Inc.  All rights reserved.
+ *
+ * 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.
+ */
+
+#include <arch/cache.h>
+#include <console/console.h>
+#include "bouncebuf.h"
+#include <halt.h>
+#include "storage.h"
+#include <string.h>
+#include <commonlib/stdlib.h>
+
+static int addr_aligned(struct bounce_buffer *state)
+{
+	const uint32_t align_mask = ARCH_DMA_MINALIGN - 1;
+
+	// Check if start is aligned
+	if ((uintptr_t)state->user_buffer & align_mask) {
+		sdhc_debug("Unaligned buffer address %p\n", state->user_buffer);
+		return 0;
+	}
+
+	// Check if length is aligned
+	if (state->len != state->len_aligned) {
+		sdhc_debug("Unaligned buffer length %zd\n", state->len);
+		return 0;
+	}
+
+	// Aligned
+	return 1;
+}
+
+int bounce_buffer_start(struct bounce_buffer *state, void *data,
+			size_t len, unsigned int flags)
+{
+	state->user_buffer = data;
+	state->bounce_buffer = data;
+	state->len = len;
+	state->len_aligned = ROUND(len, ARCH_DMA_MINALIGN);
+	state->flags = flags;
+
+	if (!addr_aligned(state)) {
+		state->bounce_buffer = memalign(ARCH_DMA_MINALIGN,
+						state->len_aligned);
+		if (!state->bounce_buffer)
+			return -1;
+
+		if (state->flags & GEN_BB_READ)
+			memcpy(state->bounce_buffer, state->user_buffer,
+				state->len);
+	}
+
+	/*
+	 * Flush data to RAM so DMA reads can pick it up,
+	 * and any CPU writebacks don't race with DMA writes
+	 */
+	dcache_clean_invalidate_by_mva(state->bounce_buffer,
+				       state->len_aligned);
+	return 0;
+}
+
+int bounce_buffer_stop(struct bounce_buffer *state)
+{
+	if (state->flags & GEN_BB_WRITE) {
+		// Invalidate cache so that CPU can see any newly DMA'd data
+		dcache_invalidate_by_mva(state->bounce_buffer,
+					 state->len_aligned);
+	}
+
+	if (state->bounce_buffer == state->user_buffer)
+		return 0;
+
+	if (state->flags & GEN_BB_WRITE)
+		memcpy(state->user_buffer, state->bounce_buffer, state->len);
+
+	free(state->bounce_buffer);
+
+	return 0;
+}
diff --git a/src/commonlib/storage/bouncebuf.h b/src/commonlib/storage/bouncebuf.h
new file mode 100644
index 0000000000..27d92e826b
--- /dev/null
+++ b/src/commonlib/storage/bouncebuf.h
@@ -0,0 +1,96 @@
+/*
+ * Generic bounce buffer implementation
+ *
+ * Copyright (C) 2012 Marek Vasut <marex@denx.de>
+ * Copyright 2013 Google Inc.  All rights reserved.
+ *
+ * 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.
+ */
+
+#ifndef __COMMONLIB_STORAGE_BOUNCEBUF_H__
+#define __COMMONLIB_STORAGE_BOUNCEBUF_H__
+
+#include <stddef.h>
+#include <stdint.h>
+#include <stdlib.h>
+
+/*
+ * GEN_BB_READ -- Data are read from the buffer eg. by DMA hardware.
+ * The source buffer is copied into the bounce buffer (if unaligned, otherwise
+ * the source buffer is used directly) upon start() call, then the operation
+ * requiring the aligned transfer happens, then the bounce buffer is lost upon
+ * stop() call.
+ */
+#define GEN_BB_READ	(1 << 0)
+/*
+ * GEN_BB_WRITE -- Data are written into the buffer eg. by DMA hardware.
+ * The source buffer starts in an undefined state upon start() call, then the
+ * operation requiring the aligned transfer happens, then the bounce buffer is
+ * copied into the destination buffer (if unaligned, otherwise destination
+ * buffer is used directly) upon stop() call.
+ */
+#define GEN_BB_WRITE	(1 << 1)
+/*
+ * GEN_BB_RW -- Data are read and written into the buffer eg. by DMA hardware.
+ * The source buffer is copied into the bounce buffer (if unaligned, otherwise
+ * the source buffer is used directly) upon start() call, then the  operation
+ * requiring the aligned transfer happens, then the bounce buffer is  copied
+ * into the destination buffer (if unaligned, otherwise destination buffer is
+ * used directly) upon stop() call.
+ */
+#define GEN_BB_RW	(GEN_BB_READ | GEN_BB_WRITE)
+
+struct bounce_buffer {
+	/* Copy of data parameter passed to start() */
+	void *user_buffer;
+	/*
+	 * DMA-aligned buffer. This field is always set to the value that
+	 * should be used for DMA; either equal to .user_buffer, or to a
+	 * freshly allocated aligned buffer.
+	 */
+	void *bounce_buffer;
+	/* Copy of len parameter passed to start() */
+	size_t len;
+	/* DMA-aligned buffer length */
+	size_t len_aligned;
+	/* Copy of flags parameter passed to start() */
+	unsigned int flags;
+};
+
+/**
+ * bounce_buffer_start() -- Start the bounce buffer session
+ * state:	stores state passed between bounce_buffer_{start,stop}
+ * data:	pointer to buffer to be aligned
+ * len:		length of the buffer
+ * flags:	flags describing the transaction, see above.
+ */
+int bounce_buffer_start(struct bounce_buffer *state, void *data,
+			size_t len, unsigned int flags);
+/**
+ * bounce_buffer_stop() -- Finish the bounce buffer session
+ * state:	stores state passed between bounce_buffer_{start,stop}
+ */
+int bounce_buffer_stop(struct bounce_buffer *state);
+
+// TODO(hungte) Eliminate the alignment stuff below and replace them with a
+// better and centralized way to handler non-cache/aligned memory.
+// Helper macros for alignment.
+#define DMA_MINALIGN (64)
+#define ROUND(a, b) (((a) + (b) - 1) & ~((b) - 1))
+#define ALLOC_CACHE_ALIGN_BUFFER(type, name, size)                   \
+	char __##name[ROUND(size * sizeof(type), DMA_MINALIGN) +     \
+		      DMA_MINALIGN - 1];                             \
+	type *name = (type *) ALIGN((uintptr_t)__##name, DMA_MINALIGN)
+#ifndef ARCH_DMA_MINALIGN
+#define ARCH_DMA_MINALIGN (DMA_MINALIGN)
+#endif
+
+#endif // __COMMONLIB_STORAGE_BOUNCEBUF_H__
diff --git a/src/commonlib/storage/mmc.c b/src/commonlib/storage/mmc.c
new file mode 100644
index 0000000000..4289b01f33
--- /dev/null
+++ b/src/commonlib/storage/mmc.c
@@ -0,0 +1,545 @@
+/*
+ * Copyright 2008, Freescale Semiconductor, Inc
+ * Andy Fleming
+ *
+ * Copyright 2013 Google Inc.  All rights reserved.
+ * Copyright 2017 Intel Corporation
+ *
+ * MultiMediaCard (MMC) and eMMC specific support code
+ * This code is controller independent
+ *
+ * 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.
+ */
+
+#include <commonlib/storage.h>
+#include <console/console.h>
+#include "sd_mmc.h"
+#include "mmc.h"
+#include "sd_mmc.h"
+#include "storage.h"
+#include <string.h>
+
+/* We pass in the cmd since otherwise the init seems to fail */
+static int mmc_send_op_cond_iter(struct storage_media *media,
+	struct mmc_command *cmd, int use_arg)
+{
+	struct sd_mmc_ctrlr *ctrlr = media->ctrlr;
+
+	cmd->cmdidx = MMC_CMD_SEND_OP_COND;
+	cmd->resp_type = CARD_RSP_R3;
+
+	/* Set the controller's operating conditions */
+	if (use_arg) {
+		uint32_t mask = media->op_cond_response &
+			(OCR_VOLTAGE_MASK | OCR_ACCESS_MODE);
+		cmd->cmdarg = ctrlr->voltages & mask;
+
+		/* Always request high capacity if supported by the
+		 * controller
+		 */
+		if (ctrlr->caps & DRVR_CAP_HC)
+			cmd->cmdarg |= OCR_HCS;
+	}
+	cmd->flags = 0;
+	int err = ctrlr->send_cmd(ctrlr, cmd, NULL);
+	if (err)
+		return err;
+
+	media->op_cond_response = cmd->response[0];
+	return 0;
+}
+
+int mmc_send_op_cond(struct storage_media *media)
+{
+	struct mmc_command cmd;
+	int max_iters = 2;
+
+	/* Ask the card for its operating conditions */
+	cmd.cmdarg = 0;
+	for (int i = 0; i < max_iters; i++) {
+		int err = mmc_send_op_cond_iter(media, &cmd, i != 0);
+		if (err)
+			return err;
+
+		// OCR_BUSY is active low, this bit set means
+		// "initialization complete".
+		if (media->op_cond_response & OCR_BUSY)
+			return 0;
+	}
+	return CARD_IN_PROGRESS;
+}
+
+int mmc_complete_op_cond(struct storage_media *media)
+{
+	struct mmc_command cmd;
+	struct stopwatch sw;
+
+	stopwatch_init_msecs_expire(&sw, MMC_INIT_TIMEOUT_US_MS);
+	while (1) {
+		// CMD1 queries whether initialization is done.
+		int err = mmc_send_op_cond_iter(media, &cmd, 1);
+		if (err)
+			return err;
+
+		// OCR_BUSY means "initialization complete".
+		if (media->op_cond_response & OCR_BUSY)
+			break;
+
+		// Check if init timeout has expired.
+		if (stopwatch_expired(&sw))
+			return CARD_UNUSABLE_ERR;
+
+		udelay(100);
+	}
+
+	media->version = MMC_VERSION_UNKNOWN;
+	media->ocr = cmd.response[0];
+
+	media->high_capacity = ((media->ocr & OCR_HCS) == OCR_HCS);
+	media->rca = 0;
+	return 0;
+}
+
+int mmc_send_ext_csd(struct sd_mmc_ctrlr *ctrlr, unsigned char *ext_csd)
+{
+	struct mmc_command cmd;
+	struct mmc_data data;
+	int rv;
+
+	/* Get the Card Status Register */
+	cmd.cmdidx = MMC_CMD_SEND_EXT_CSD;
+	cmd.resp_type = CARD_RSP_R1;
+	cmd.cmdarg = 0;
+	cmd.flags = 0;
+
+	data.dest = (char *)ext_csd;
+	data.blocks = 1;
+	data.blocksize = 512;
+	data.flags = DATA_FLAG_READ;
+
+	rv = ctrlr->send_cmd(ctrlr, &cmd, &data);
+
+	if (!rv && IS_ENABLED(CONFIG_SD_MMC_TRACE)) {
+		int i, size;
+
+		size = data.blocks * data.blocksize;
+		sd_mmc_trace("\t%p ext_csd:", ctrlr);
+		for (i = 0; i < size; i++) {
+			if (!(i % 32))
+				sd_mmc_trace("\n");
+			sd_mmc_trace(" %2.2x", ext_csd[i]);
+		}
+		sd_mmc_trace("\n");
+	}
+	return rv;
+}
+
+static int mmc_switch(struct storage_media *media, uint8_t index, uint8_t value)
+{
+	struct mmc_command cmd;
+	struct sd_mmc_ctrlr *ctrlr = media->ctrlr;
+
+	cmd.cmdidx = MMC_CMD_SWITCH;
+	cmd.resp_type = CARD_RSP_R1b;
+	cmd.cmdarg = ((MMC_SWITCH_MODE_WRITE_BYTE << 24) |
+			   (index << 16) | (value << 8));
+	cmd.flags = 0;
+
+	int ret = ctrlr->send_cmd(ctrlr, &cmd, NULL);
+
+	/* Waiting for the ready status */
+	sd_mmc_send_status(media, SD_MMC_IO_RETRIES);
+	return ret;
+
+}
+
+static void mmc_recalculate_clock(struct storage_media *media)
+{
+	uint32_t clock;
+
+	clock = CLOCK_26MHZ;
+	if (media->caps & DRVR_CAP_HS) {
+		if ((media->caps & DRVR_CAP_HS200) ||
+		    (media->caps & DRVR_CAP_HS400))
+			clock = CLOCK_200MHZ;
+		else if (media->caps & DRVR_CAP_HS52)
+			clock = CLOCK_52MHZ;
+	}
+	SET_CLOCK(media->ctrlr, clock);
+}
+
+static int mmc_select_hs(struct storage_media *media)
+{
+	int ret;
+
+	/* Switch the MMC device into high speed mode */
+	ret = mmc_switch(media, EXT_CSD_HS_TIMING, EXT_CSD_TIMING_HS);
+	if (ret) {
+		sd_mmc_error("Timing switch to high speed failed\n");
+		return ret;
+	}
+	sdhc_debug("SDHCI switched MMC to high speed\n");
+
+	/* Increase the controller clock speed */
+	SET_TIMING(media->ctrlr, BUS_TIMING_MMC_HS);
+	media->caps |= DRVR_CAP_HS52 | DRVR_CAP_HS;
+	mmc_recalculate_clock(media);
+	ret = sd_mmc_send_status(media, SD_MMC_IO_RETRIES);
+	return ret;
+}
+
+static int mmc_send_tunning_seq(struct sd_mmc_ctrlr *ctrlr, char *buffer)
+{
+	struct mmc_command cmd;
+	struct mmc_data data;
+
+	/* Request the device send the tuning sequence to the host */
+	cmd.cmdidx = MMC_CMD_AUTO_TUNING_SEQUENCE;
+	cmd.resp_type = CARD_RSP_R1;
+	cmd.cmdarg = 0;
+	cmd.flags = CMD_FLAG_IGNORE_INHIBIT;
+
+	data.dest = buffer;
+	data.blocks = 1;
+	data.blocksize = (ctrlr->bus_width == 8) ? 128 : 64;
+	data.flags = DATA_FLAG_READ;
+	return ctrlr->send_cmd(ctrlr, &cmd, &data);
+}
+
+static int mmc_bus_tuning(struct storage_media *media)
+{
+	ALLOC_CACHE_ALIGN_BUFFER(char, buffer, 128);
+	struct sd_mmc_ctrlr *ctrlr = media->ctrlr;
+	int index;
+	int successful;
+
+	/* Request the device send the tuning sequence up to 40 times */
+	ctrlr->tuning_start(ctrlr, 0);
+	for (index = 0; index < 40; index++) {
+		mmc_send_tunning_seq(ctrlr, buffer);
+		if (ctrlr->is_tuning_complete(ctrlr, &successful)) {
+			if (successful)
+				return 0;
+			break;
+		}
+	}
+	sd_mmc_error("Bus tuning failed!\n");
+	return -1;
+}
+
+static int mmc_select_hs400(struct storage_media *media)
+{
+	uint8_t bus_width;
+	uint32_t caps;
+	struct sd_mmc_ctrlr *ctrlr = media->ctrlr;
+	int ret;
+	uint32_t timing;
+
+	/* Switch the MMC device into high speed mode */
+	ret = mmc_select_hs(media);
+	if (ret)
+		return ret;
+
+	/* Switch MMC device to 8-bit DDR with strobe */
+	bus_width = EXT_CSD_DDR_BUS_WIDTH_8;
+	caps = DRVR_CAP_HS400 | DRVR_CAP_HS52 | DRVR_CAP_HS;
+	timing = BUS_TIMING_MMC_HS400;
+	if ((ctrlr->caps & DRVR_CAP_ENHANCED_STROBE)
+		&& (media->caps & DRVR_CAP_ENHANCED_STROBE)) {
+		bus_width |= EXT_CSD_BUS_WIDTH_STROBE;
+		caps |= DRVR_CAP_ENHANCED_STROBE;
+		timing = BUS_TIMING_MMC_HS400ES;
+	}
+	ret = mmc_switch(media, EXT_CSD_BUS_WIDTH, bus_width);
+	if (ret) {
+		sd_mmc_error("Switching bus width for HS400 failed\n");
+		return ret;
+	}
+	sdhc_debug("SDHCI switched MMC to 8-bit DDR\n");
+
+	/* Set controller to 8-bit mode */
+	SET_BUS_WIDTH(ctrlr, 8);
+	media->caps |= EXT_CSD_BUS_WIDTH_8;
+
+	/* Switch MMC device to HS400 */
+	ret = mmc_switch(media, EXT_CSD_HS_TIMING, EXT_CSD_TIMING_HS400);
+	if (ret) {
+		sd_mmc_error("Switch to HS400 timing failed\n");
+		return ret;
+	}
+
+	/* Set controller to 200 MHz and use receive strobe */
+	SET_TIMING(ctrlr, timing);
+	media->caps |= caps;
+	mmc_recalculate_clock(media);
+	ret = sd_mmc_send_status(media, SD_MMC_IO_RETRIES);
+	return ret;
+}
+
+static int mmc_select_hs200(struct storage_media *media)
+{
+	struct sd_mmc_ctrlr *ctrlr = media->ctrlr;
+	int ret;
+
+	/* Switch the MMC device to 8-bit SDR */
+	ret = mmc_switch(media, EXT_CSD_BUS_WIDTH, EXT_CSD_BUS_WIDTH_8);
+	if (ret) {
+		sd_mmc_error("Switching bus width for HS200 failed\n");
+		return ret;
+	}
+
+	/* Set controller to 8-bit mode */
+	SET_BUS_WIDTH(ctrlr, 8);
+	media->caps |= EXT_CSD_BUS_WIDTH_8;
+
+	/* Switch to HS200 */
+	ret = mmc_switch(media, EXT_CSD_HS_TIMING, EXT_CSD_TIMING_HS200);
+
+	if (ret) {
+		sd_mmc_error("Switch to HS200 failed\n");
+		return ret;
+	}
+	sdhc_debug("SDHCI switched MMC to 8-bit SDR\n");
+
+	/* Set controller to 200 MHz */
+	SET_TIMING(ctrlr, BUS_TIMING_MMC_HS200);
+	media->caps |= DRVR_CAP_HS200 | DRVR_CAP_HS52 | DRVR_CAP_HS;
+	mmc_recalculate_clock(media);
+
+	/* Tune the receive sampling point for the bus */
+	if ((!ret) && (ctrlr->caps & DRVR_CAP_HS200_TUNING))
+		ret = mmc_bus_tuning(media);
+	return ret;
+}
+
+int mmc_change_freq(struct storage_media *media)
+{
+	struct sd_mmc_ctrlr *ctrlr = media->ctrlr;
+	int err;
+	ALLOC_CACHE_ALIGN_BUFFER(unsigned char, ext_csd, 512);
+
+	media->caps = 0;
+
+	/* Only version 4 supports high-speed */
+	if (media->version < MMC_VERSION_4)
+		return 0;
+
+	err = mmc_send_ext_csd(ctrlr, ext_csd);
+	if (err)
+		return err;
+
+	if ((ctrlr->caps & DRVR_CAP_HS400) &&
+	    (ext_csd[EXT_CSD_CARD_TYPE] & MMC_HS400))
+		err = mmc_select_hs400(media);
+	else if ((ctrlr->caps & DRVR_CAP_HS200) &&
+		 (ext_csd[EXT_CSD_CARD_TYPE] & MMC_HS_200MHZ))
+		err = mmc_select_hs200(media);
+	else
+		err = mmc_select_hs(media);
+
+	return err;
+}
+
+int mmc_set_bus_width(struct storage_media *media)
+{
+	struct sd_mmc_ctrlr *ctrlr = media->ctrlr;
+	int err;
+	int width;
+
+	ALLOC_CACHE_ALIGN_BUFFER(unsigned char, ext_csd, EXT_CSD_SIZE);
+	ALLOC_CACHE_ALIGN_BUFFER(unsigned char, test_csd, EXT_CSD_SIZE);
+
+	/* Set the bus width */
+	err = 0;
+	for (width = EXT_CSD_BUS_WIDTH_8; width >= 0; width--) {
+		/* If HS200 is switched, Bus Width has been 8-bit */
+		if ((media->caps & DRVR_CAP_HS200) ||
+		    (media->caps & DRVR_CAP_HS400))
+			break;
+
+		/* Set the card to use 4 bit*/
+		err = mmc_switch(media, EXT_CSD_BUS_WIDTH, width);
+		if (err)
+			continue;
+
+		if (!width) {
+			SET_BUS_WIDTH(ctrlr, 1);
+			break;
+		}
+		SET_BUS_WIDTH(ctrlr, 4 * width);
+
+		err = mmc_send_ext_csd(ctrlr, test_csd);
+		if (!err &&
+		    (ext_csd[EXT_CSD_PARTITIONING_SUPPORT] ==
+		    test_csd[EXT_CSD_PARTITIONING_SUPPORT]) &&
+		    (ext_csd[EXT_CSD_ERASE_GROUP_DEF] ==
+		    test_csd[EXT_CSD_ERASE_GROUP_DEF]) &&
+		    (ext_csd[EXT_CSD_REV] ==
+		    test_csd[EXT_CSD_REV]) &&
+		    (ext_csd[EXT_CSD_HC_ERASE_GRP_SIZE] ==
+		    test_csd[EXT_CSD_HC_ERASE_GRP_SIZE]) &&
+		    memcmp(&ext_csd[EXT_CSD_SEC_CNT],
+			   &test_csd[EXT_CSD_SEC_CNT], 4) == 0) {
+			media->caps |= width;
+			break;
+		}
+	}
+	return err;
+}
+
+int mmc_update_capacity(struct storage_media *media)
+{
+	uint64_t capacity;
+	struct sd_mmc_ctrlr *ctrlr = media->ctrlr;
+	int err;
+	ALLOC_CACHE_ALIGN_BUFFER(unsigned char, ext_csd, EXT_CSD_SIZE);
+	uint32_t erase_size;
+	uint32_t hc_erase_size;
+	uint64_t hc_wp_size;
+	int index;
+
+	if (media->version < MMC_VERSION_4)
+		return 0;
+
+	/* check  ext_csd version and capacity */
+	err = mmc_send_ext_csd(ctrlr, ext_csd);
+	if (err)
+		return err;
+
+	if (ext_csd[EXT_CSD_REV] < 2)
+		return 0;
+
+	/* Determine if the device supports enhanced strobe */
+	media->caps |= ext_csd[EXT_CSD_STROBE_SUPPORT]
+		? DRVR_CAP_ENHANCED_STROBE : 0;
+
+	/* Determine the eMMC device information */
+	media->partition_config = ext_csd[EXT_CSD_PART_CONF]
+		& EXT_CSD_PART_ACCESS_MASK;
+
+	/* Determine the user partition size
+	 *
+	 * According to the JEDEC Standard, the value of
+	 * ext_csd's capacity is valid if the value is
+	 * more than 2GB
+	 */
+	capacity = (ext_csd[EXT_CSD_SEC_CNT + 0] << 0 |
+		    ext_csd[EXT_CSD_SEC_CNT + 1] << 8 |
+		    ext_csd[EXT_CSD_SEC_CNT + 2] << 16 |
+		    ext_csd[EXT_CSD_SEC_CNT + 3] << 24);
+	capacity *= 512;
+	if ((capacity >> 20) > 2 * 1024)
+		media->capacity[MMC_PARTITION_USER] = capacity;
+
+	/* Determine the boot parition sizes */
+	hc_erase_size = ext_csd[224] * 512 * KiB;
+	capacity = ext_csd[EXT_CSD_BOOT_SIZE_MULT] * 128 * KiB;
+	media->capacity[MMC_PARTITION_BOOT_1] = capacity;
+	media->capacity[MMC_PARTITION_BOOT_2] = capacity;
+
+	/* Determine the RPMB size */
+	hc_wp_size = ext_csd[EXT_CSD_HC_WP_GRP_SIZE] * hc_erase_size;
+	capacity = 128 * KiB * ext_csd[EXT_CSD_RPMB_SIZE_MULT];
+	media->capacity[MMC_PARTITION_RPMB] = capacity;
+
+	/* Determine the general partition sizes */
+	capacity = (ext_csd[EXT_CSD_GP_SIZE_MULT_GP0 + 2] << 16)
+		| (ext_csd[EXT_CSD_GP_SIZE_MULT_GP0 + 1] << 8)
+		| ext_csd[EXT_CSD_GP_SIZE_MULT_GP0];
+	capacity *= hc_wp_size;
+	media->capacity[MMC_PARTITION_GP1] = capacity;
+
+	capacity = (ext_csd[EXT_CSD_GP_SIZE_MULT_GP1 + 2] << 16)
+		| (ext_csd[EXT_CSD_GP_SIZE_MULT_GP1 + 1] << 8)
+		| ext_csd[EXT_CSD_GP_SIZE_MULT_GP1];
+	capacity *= hc_wp_size;
+	media->capacity[MMC_PARTITION_GP2] = capacity;
+
+	capacity = (ext_csd[EXT_CSD_GP_SIZE_MULT_GP2 + 2] << 16)
+		| (ext_csd[EXT_CSD_GP_SIZE_MULT_GP2 + 1] << 8)
+		| ext_csd[EXT_CSD_GP_SIZE_MULT_GP2];
+	capacity *= hc_wp_size;
+	media->capacity[MMC_PARTITION_GP3] = capacity;
+
+	capacity = (ext_csd[EXT_CSD_GP_SIZE_MULT_GP3 + 2] << 16)
+		| (ext_csd[EXT_CSD_GP_SIZE_MULT_GP3 + 1] << 8)
+		| ext_csd[EXT_CSD_GP_SIZE_MULT_GP3];
+	capacity *= hc_wp_size;
+	media->capacity[MMC_PARTITION_GP4] = capacity;
+
+	/* Determine the erase size */
+	erase_size = (sd_mmc_extract_uint32_bits(media->csd,
+		81, 5) + 1) *
+		(sd_mmc_extract_uint32_bits(media->csd, 86, 5)
+		+ 1);
+	for (index = MMC_PARTITION_BOOT_1; index <= MMC_PARTITION_GP4;
+		index++) {
+		if (media->capacity[index] != 0) {
+			/* Enable the partitions  */
+			err = mmc_switch(media, EXT_CSD_ERASE_GROUP_DEF,
+				EXT_CSD_PARTITION_ENABLE);
+			if (err) {
+				sdhc_error("Failed to enable partition access\n");
+				return err;
+			}
+
+			/* Use HC erase group size */
+			erase_size = hc_erase_size / media->write_bl_len;
+			break;
+		}
+	}
+	media->erase_blocks = erase_size;
+	media->trim_mult = ext_csd[EXT_CSD_TRIM_MULT];
+
+	return 0;
+}
+
+int mmc_set_partition(struct storage_media *media,
+	unsigned int partition_number)
+{
+	uint8_t partition_config;
+
+	/* Validate the partition number */
+	if ((partition_number > MMC_PARTITION_GP4)
+		|| (!media->capacity[partition_number]))
+		return -1;
+
+	/* Update the partition register */
+	partition_config = media->partition_config;
+	partition_config &= ~EXT_CSD_PART_ACCESS_MASK;
+	partition_config |= partition_number;
+
+	/* Select the new partition */
+	int ret = mmc_switch(media, EXT_CSD_PART_CONF, partition_config);
+	if (!ret)
+		media->partition_config = partition_config;
+
+	return ret;
+}
+
+const char *mmc_partition_name(struct storage_media *media,
+	unsigned int partition_number)
+{
+	static const char * const partition_name[8] = {
+		"User",		/* 0 */
+		"Boot 1",	/* 1 */
+		"Boot 2",	/* 2 */
+		"RPMB",		/* 3 */
+		"GP 1",		/* 4 */
+		"GP 2",		/* 5 */
+		"GP 3",		/* 6 */
+		"GP 4"		/* 7 */
+	};
+
+	if (partition_number >= ARRAY_SIZE(partition_name))
+		return "";
+	return partition_name[partition_number];
+}
diff --git a/src/commonlib/storage/mmc.h b/src/commonlib/storage/mmc.h
new file mode 100644
index 0000000000..87d16353c8
--- /dev/null
+++ b/src/commonlib/storage/mmc.h
@@ -0,0 +1,54 @@
+/*
+ * Copyright 2008,2010 Freescale Semiconductor, Inc
+ * Andy Fleming
+ *
+ * Copyright 2013 Google Inc.  All rights reserved.
+ * Copyright 2017 Intel Corporation
+ *
+ * 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.
+ */
+
+#ifndef __COMMONLIB_STORAGE_MMC_H__
+#define __COMMONLIB_STORAGE_MMC_H__
+
+#include <commonlib/sd_mmc_ctrlr.h>
+
+#define MMC_HS_TIMING		0x00000100
+#define MMC_HS_52MHZ		0x2
+#define MMC_HS_200MHZ		0x10
+#define MMC_HS400		0x40
+
+#define SECURE_ERASE		0x80000000
+
+#define MMC_STATUS_MASK		(~0x0206BF7F)
+#define MMC_STATUS_RDY_FOR_DATA (1 << 8)
+#define MMC_STATUS_CURR_STATE	(0xf << 9)
+#define MMC_STATUS_ERROR	(1 << 19)
+
+#define MMC_SWITCH_MODE_CMD_SET		0x00 /* Change the command set */
+#define MMC_SWITCH_MODE_SET_BITS	0x01 /* Set bits in EXT_CSD byte
+						addressed by index which are
+						1 in value field */
+#define MMC_SWITCH_MODE_CLEAR_BITS	0x02 /* Clear bits in EXT_CSD byte
+						addressed by index, which are
+						1 in value field */
+#define MMC_SWITCH_MODE_WRITE_BYTE	0x03 /* Set target byte to value */
+
+#define R1_ILLEGAL_COMMAND		(1 << 22)
+#define R1_APP_CMD			(1 << 5)
+
+#define MMC_INIT_TIMEOUT_US	(1000 * 1000)
+#define MMC_INIT_TIMEOUT_US_MS	1000
+
+int storage_block_setup_media(struct storage_media *media,
+	struct sd_mmc_ctrlr *ctrlr);
+
+#endif /* __COMMONLIB_STORAGE_MMC_H__ */
diff --git a/src/commonlib/storage/pci_sdhci.c b/src/commonlib/storage/pci_sdhci.c
new file mode 100644
index 0000000000..c53ab001a9
--- /dev/null
+++ b/src/commonlib/storage/pci_sdhci.c
@@ -0,0 +1,64 @@
+/*
+ * Copyright 2013 Google Inc.
+ * Copyright 2017 Intel Corporation
+ *
+ * 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.
+ */
+
+#include <rules.h>
+#if ENV_RAMSTAGE
+#define __SIMPLE_DEVICE__		1
+#endif
+
+#include <assert.h>
+#include <commonlib/sdhci.h>
+#include <console/console.h>
+#include <device/pci.h>
+#include "sd_mmc.h"
+#include "storage.h"
+#include <string.h>
+
+/* Initialize an SDHCI port */
+int sdhci_controller_init(struct sdhci_ctrlr *sdhci_ctrlr, void *ioaddr)
+{
+	memset(sdhci_ctrlr, 0, sizeof(*sdhci_ctrlr));
+	sdhci_ctrlr->ioaddr = ioaddr;
+	return add_sdhci(sdhci_ctrlr);
+}
+
+struct sd_mmc_ctrlr *new_mem_sdhci_controller(void *ioaddr)
+{
+	struct sdhci_ctrlr *sdhci_ctrlr;
+
+	sdhci_ctrlr = malloc(sizeof(*sdhci_ctrlr));
+	if (sdhci_ctrlr == NULL)
+		return NULL;
+
+	if (sdhci_controller_init(sdhci_ctrlr, ioaddr)) {
+		free(sdhci_ctrlr);
+		sdhci_ctrlr = NULL;
+	}
+	return &sdhci_ctrlr->sd_mmc_ctrlr;
+}
+
+struct sd_mmc_ctrlr *new_pci_sdhci_controller(uint32_t dev)
+{
+	uint32_t addr;
+
+	addr = pci_read_config32(dev, PCI_BASE_ADDRESS_0);
+	if (addr == ((uint32_t)~0)) {
+		sdhc_error("Error: PCI SDHCI not found\n");
+		return NULL;
+	}
+
+	addr &= ~0xf;
+	return new_mem_sdhci_controller((void *)addr);
+}
diff --git a/src/commonlib/storage/sd.c b/src/commonlib/storage/sd.c
new file mode 100644
index 0000000000..bd23b7f70c
--- /dev/null
+++ b/src/commonlib/storage/sd.c
@@ -0,0 +1,302 @@
+/*
+ * Copyright 2008, Freescale Semiconductor, Inc
+ * Andy Fleming
+ *
+ * Copyright 2013 Google Inc.  All rights reserved.
+ * Copyright 2017 Intel Corporation
+ *
+ * Secure Digital (SD) card specific support code
+ * This code is controller independent
+ *
+ * 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.
+ */
+
+#include <assert.h>
+#include <commonlib/sd_mmc_ctrlr.h>
+#include <commonlib/storage.h>
+#include <delay.h>
+#include <endian.h>
+#include "sd_mmc.h"
+#include "storage.h"
+#include <string.h>
+#include <timer.h>
+
+int sd_send_if_cond(struct storage_media *media)
+{
+	struct mmc_command cmd;
+	struct sd_mmc_ctrlr *ctrlr = media->ctrlr;
+
+	cmd.cmdidx = SD_CMD_SEND_IF_COND;
+	/* Set if controller supports voltages between 2.7 and 3.6 V. */
+	cmd.cmdarg = ((ctrlr->voltages & 0xff8000) != 0) << 8 | 0xaa;
+	cmd.resp_type = CARD_RSP_R7;
+	cmd.flags = 0;
+	int err = ctrlr->send_cmd(ctrlr, &cmd, NULL);
+	if (err)
+		return err;
+
+	if ((cmd.response[0] & 0xff) != 0xaa)
+		return CARD_UNUSABLE_ERR;
+	media->version = SD_VERSION_2;
+	return 0;
+}
+
+int sd_send_op_cond(struct storage_media *media)
+{
+	int err;
+	struct mmc_command cmd;
+	struct sd_mmc_ctrlr *ctrlr = media->ctrlr;
+
+	int tries = SD_MMC_IO_RETRIES;
+	while (tries--) {
+		cmd.cmdidx = MMC_CMD_APP_CMD;
+		cmd.resp_type = CARD_RSP_R1;
+		cmd.cmdarg = 0;
+		cmd.flags = 0;
+
+		err = ctrlr->send_cmd(ctrlr, &cmd, NULL);
+		if (err)
+			return err;
+
+		cmd.cmdidx = SD_CMD_APP_SEND_OP_COND;
+		cmd.resp_type = CARD_RSP_R3;
+
+		/*
+		 * Most cards do not answer if some reserved bits
+		 * in the ocr are set. However, Some controller
+		 * can set bit 7 (reserved for low voltages), but
+		 * how to manage low voltages SD card is not yet
+		 * specified.
+		 */
+		cmd.cmdarg = (ctrlr->voltages & 0xff8000);
+
+		if (media->version == SD_VERSION_2)
+			cmd.cmdarg |= OCR_HCS;
+
+		err = ctrlr->send_cmd(ctrlr, &cmd, NULL);
+		if (err)
+			return err;
+
+		// OCR_BUSY means "initialization complete".
+		if (cmd.response[0] & OCR_BUSY)
+			break;
+
+		udelay(100);
+	}
+	if (tries < 0)
+		return CARD_UNUSABLE_ERR;
+
+	if (media->version != SD_VERSION_2)
+		media->version = SD_VERSION_1_0;
+
+	media->ocr = cmd.response[0];
+	media->high_capacity = ((media->ocr & OCR_HCS) == OCR_HCS);
+	media->rca = 0;
+	return 0;
+}
+
+static int sd_switch(struct sd_mmc_ctrlr *ctrlr, int mode, int group,
+	uint8_t value, uint8_t *resp)
+{
+	/* Switch the frequency */
+	struct mmc_command cmd;
+	cmd.cmdidx = SD_CMD_SWITCH_FUNC;
+	cmd.resp_type = CARD_RSP_R1;
+	cmd.cmdarg = (mode << 31) | (0xffffff & ~(0xf << (group * 4))) |
+		     (value << (group * 4));
+	cmd.flags = 0;
+
+	struct mmc_data data;
+	data.dest = (char *)resp;
+	data.blocksize = 64;
+	data.blocks = 1;
+	data.flags = DATA_FLAG_READ;
+
+	return ctrlr->send_cmd(ctrlr, &cmd, &data);
+}
+
+static void sd_recalculate_clock(struct storage_media *media)
+{
+	uint32_t clock = 1;
+
+	if (media->caps & DRVR_CAP_HS)
+		clock = CLOCK_50MHZ;
+	else
+		clock = CLOCK_25MHZ;
+	SET_CLOCK(media->ctrlr, clock);
+}
+
+int sd_change_freq(struct storage_media *media)
+{
+	int delay;
+	int err, timeout;
+	struct mmc_command cmd;
+	struct sd_mmc_ctrlr *ctrlr = media->ctrlr;
+	struct mmc_data data;
+	ALLOC_CACHE_ALIGN_BUFFER(uint32_t, scr, 2);
+	ALLOC_CACHE_ALIGN_BUFFER(uint32_t, switch_status, 16);
+
+	media->caps = 0;
+
+	/* Read the SCR to find out if this card supports higher speeds */
+	cmd.cmdidx = MMC_CMD_APP_CMD;
+	cmd.resp_type = CARD_RSP_R1;
+	cmd.cmdarg = media->rca << 16;
+	cmd.flags = 0;
+
+	err = ctrlr->send_cmd(ctrlr, &cmd, NULL);
+	if (err)
+		return err;
+
+	cmd.cmdidx = SD_CMD_APP_SEND_SCR;
+	cmd.resp_type = CARD_RSP_R1;
+	cmd.cmdarg = 0;
+	cmd.flags = 0;
+
+	timeout = 3;
+	while (timeout--) {
+		data.dest = (char *)scr;
+		data.blocksize = 8;
+		data.blocks = 1;
+		data.flags = DATA_FLAG_READ;
+		err = ctrlr->send_cmd(ctrlr, &cmd, &data);
+		if (!err)
+			break;
+	}
+	if (err) {
+		sd_mmc_error("%s returning %d\n", __func__, err);
+		return err;
+	}
+
+	media->scr[0] = be32toh(scr[0]);
+	media->scr[1] = be32toh(scr[1]);
+
+	switch ((media->scr[0] >> 24) & 0xf) {
+	case 0:
+		media->version = SD_VERSION_1_0;
+		break;
+	case 1:
+		media->version = SD_VERSION_1_10;
+		break;
+	case 2:
+		media->version = SD_VERSION_2;
+		break;
+	default:
+		media->version = SD_VERSION_1_0;
+		break;
+	}
+
+	if (media->scr[0] & SD_DATA_4BIT)
+		media->caps |= DRVR_CAP_4BIT;
+
+	/* Version 1.0 doesn't support switching */
+	if (media->version == SD_VERSION_1_0)
+		goto out;
+
+	timeout = 4;
+	while (timeout--) {
+		err = sd_switch(ctrlr, SD_SWITCH_CHECK, 0, 1,
+				(uint8_t *)switch_status);
+		if (err)
+			return err;
+
+		/* The high-speed function is busy.  Try again */
+		if (!(ntohl(switch_status[7]) & SD_HIGHSPEED_BUSY))
+			break;
+	}
+
+	/* If high-speed isn't supported, we return */
+	if (!(ntohl(switch_status[3]) & SD_HIGHSPEED_SUPPORTED))
+		goto out;
+
+	/*
+	 * If the controller doesn't support SD_HIGHSPEED, do not switch the
+	 * card to HIGHSPEED mode even if the card support SD_HIGHSPPED.
+	 * This can avoid a further problem when the card runs in different
+	 * mode than the controller.
+	 */
+	if (!((ctrlr->caps & DRVR_CAP_HS52) && (ctrlr->caps & DRVR_CAP_HS)))
+		goto out;
+
+	/* Give the card time to recover afer the switch operation.  Wait for
+	 * 9 (>= 8) clock cycles receiving the switch status.
+	 */
+	delay = (9000000 + ctrlr->bus_hz - 1) / ctrlr->bus_hz;
+	udelay(delay);
+
+	/* Switch to high speed */
+	err = sd_switch(ctrlr, SD_SWITCH_SWITCH, 0, 1,
+			(uint8_t *)switch_status);
+	if (err)
+		return err;
+
+	/* Give the card time to perform the switch operation.  Wait for 9
+	 * (>= 8) clock cycles receiving the switch status.
+	 */
+	udelay(delay);
+
+	if ((ntohl(switch_status[4]) & 0x0f000000) == 0x01000000) {
+		media->caps |= DRVR_CAP_HS;
+		SET_TIMING(ctrlr, BUS_TIMING_SD_HS);
+	}
+
+out:
+	sd_recalculate_clock(media);
+	return 0;
+}
+
+int sd_set_bus_width(struct storage_media *media)
+{
+	int err;
+	struct mmc_command cmd;
+	struct sd_mmc_ctrlr *ctrlr = media->ctrlr;
+
+	if (media->caps & DRVR_CAP_4BIT) {
+		cmd.cmdidx = MMC_CMD_APP_CMD;
+		cmd.resp_type = CARD_RSP_R1;
+		cmd.cmdarg = media->rca << 16;
+		cmd.flags = 0;
+
+		err = ctrlr->send_cmd(ctrlr, &cmd, NULL);
+		if (err)
+			return err;
+
+		cmd.cmdidx = SD_CMD_APP_SET_BUS_WIDTH;
+		cmd.resp_type = CARD_RSP_R1;
+		cmd.cmdarg = 2;
+		cmd.flags = 0;
+		err = ctrlr->send_cmd(ctrlr, &cmd, NULL);
+		if (err)
+			return err;
+
+		SET_BUS_WIDTH(ctrlr, 4);
+	}
+	return 0;
+}
+
+
+int sd_set_partition(struct storage_media *media,
+	unsigned int partition_number)
+{
+	/* Validate the partition number */
+	if (partition_number)
+		return -1;
+
+	/* Update the partition number */
+	media->partition_config = partition_number;
+	return 0;
+}
+
+const char *sd_partition_name(struct storage_media *media,
+	unsigned int partition_number)
+{
+	return "";
+}
diff --git a/src/commonlib/storage/sd_mmc.c b/src/commonlib/storage/sd_mmc.c
new file mode 100644
index 0000000000..0ca53b7d20
--- /dev/null
+++ b/src/commonlib/storage/sd_mmc.c
@@ -0,0 +1,270 @@
+/*
+ * Copyright 2008, Freescale Semiconductor, Inc
+ * Andy Fleming
+ *
+ * Copyright 2013 Google Inc.  All rights reserved.
+ * Copyright 2017 Intel Corporation
+ *
+ * MultiMediaCard (MMC), eMMC and Secure Digital (SD) common initialization
+ * code which brings the card into the standby state.  This code is controller
+ * independent.
+ *
+ * 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.
+ */
+
+#include <assert.h>
+#include <commonlib/storage.h>
+#include <delay.h>
+#include <endian.h>
+#include "mmc.h"
+#include "sd_mmc.h"
+#include "storage.h"
+#include <string.h>
+#include <timer.h>
+
+uint64_t sd_mmc_extract_uint32_bits(const uint32_t *array, int start, int count)
+{
+	int i;
+	uint64_t value = 0;
+
+	for (i = 0; i < count; i++, start++) {
+		value <<= 1;
+		value |= (array[start / 32] >> (31 - (start % 32))) & 0x1;
+	}
+	return value;
+}
+
+static uint32_t sd_mmc_calculate_transfer_speed(uint32_t csd0)
+{
+	uint32_t mult, freq;
+
+	/* frequency bases, divided by 10 to be nice to platforms without
+	 * floating point */
+	static const int fbase[] = {
+		10000,
+		100000,
+		1000000,
+		10000000,
+	};
+	/* Multiplier values for TRAN_SPEED. Multiplied by 10 to be nice
+	 * to platforms without floating point. */
+	static const int multipliers[] = {
+		0,  // reserved
+		10,
+		12,
+		13,
+		15,
+		20,
+		25,
+		30,
+		35,
+		40,
+		45,
+		50,
+		55,
+		60,
+		70,
+		80,
+	};
+
+	/* divide frequency by 10, since the mults are 10x bigger */
+	freq = fbase[csd0 & 0x7];
+	mult = multipliers[(csd0 >> 3) & 0xf];
+	return freq * mult;
+}
+
+static int sd_mmc_go_idle(struct storage_media *media)
+{
+	struct sd_mmc_ctrlr *ctrlr = media->ctrlr;
+
+	// Some cards can't accept idle commands without delay.
+	if (ctrlr->mdelay_before_cmd0)
+		mdelay(ctrlr->mdelay_before_cmd0);
+
+	struct mmc_command cmd;
+	cmd.cmdidx = MMC_CMD_GO_IDLE_STATE;
+	cmd.cmdarg = 0;
+	cmd.resp_type = CARD_RSP_NONE;
+	cmd.flags = 0;
+
+	int err = ctrlr->send_cmd(ctrlr, &cmd, NULL);
+	if (err)
+		return err;
+
+	// Some cards need more than half second to respond to next command (ex,
+	// SEND_OP_COND).
+	if (ctrlr->mdelay_after_cmd0)
+		mdelay(ctrlr->mdelay_after_cmd0);
+
+	return 0;
+}
+
+int sd_mmc_send_status(struct storage_media *media, ssize_t tries)
+{
+	struct mmc_command cmd;
+	struct sd_mmc_ctrlr *ctrlr = media->ctrlr;
+
+	cmd.cmdidx = MMC_CMD_SEND_STATUS;
+	cmd.resp_type = CARD_RSP_R1;
+	cmd.cmdarg = media->rca << 16;
+	cmd.flags = 0;
+
+	while (tries--) {
+		int err = ctrlr->send_cmd(ctrlr, &cmd, NULL);
+		if (err)
+			return err;
+		else if (cmd.response[0] & MMC_STATUS_RDY_FOR_DATA)
+			break;
+		else if (cmd.response[0] & MMC_STATUS_MASK) {
+			sd_mmc_error("Status Error: %#8.8x\n", cmd.response[0]);
+			return CARD_COMM_ERR;
+		}
+
+		udelay(100);
+	}
+
+	sd_mmc_trace("CURR STATE:%d\n",
+		  (cmd.response[0] & MMC_STATUS_CURR_STATE) >> 9);
+
+	if (tries < 0) {
+		sd_mmc_error("Timeout waiting card ready\n");
+		return CARD_TIMEOUT;
+	}
+	return 0;
+}
+
+int sd_mmc_set_blocklen(struct sd_mmc_ctrlr *ctrlr, int len)
+{
+	struct mmc_command cmd;
+	cmd.cmdidx = MMC_CMD_SET_BLOCKLEN;
+	cmd.resp_type = CARD_RSP_R1;
+	cmd.cmdarg = len;
+	cmd.flags = 0;
+
+	return ctrlr->send_cmd(ctrlr, &cmd, NULL);
+}
+
+int sd_mmc_enter_standby(struct storage_media *media)
+{
+	struct mmc_command cmd;
+	struct sd_mmc_ctrlr *ctrlr = media->ctrlr;
+	int err;
+
+	SET_BUS_WIDTH(ctrlr, 1);
+	SET_CLOCK(ctrlr, 1);
+
+	/* Reset the Card */
+	err = sd_mmc_go_idle(media);
+	if (err)
+		return err;
+
+	/* Test for SD version 2 */
+	err = CARD_TIMEOUT;
+	if (IS_ENABLED(CONFIG_COMMONLIB_STORAGE_SD)) {
+		err = sd_send_if_cond(media);
+
+		/* Get SD card operating condition */
+		if (!err)
+			err = sd_send_op_cond(media);
+	}
+
+	/* If the command timed out, we check for an MMC card */
+	if (IS_ENABLED(CONFIG_COMMONLIB_STORAGE_MMC) && (err == CARD_TIMEOUT)) {
+		/* Some cards seem to need this */
+		sd_mmc_go_idle(media);
+
+		err = mmc_send_op_cond(media);
+		if (err == CARD_IN_PROGRESS)
+			err = mmc_complete_op_cond(media);
+	}
+
+	if (err) {
+		sd_mmc_error(
+			"Card did not respond to voltage select!\n");
+		return CARD_UNUSABLE_ERR;
+	}
+
+	/* Put the Card in Identify Mode */
+	cmd.cmdidx = MMC_CMD_ALL_SEND_CID;
+	cmd.resp_type = CARD_RSP_R2;
+	cmd.cmdarg = 0;
+	cmd.flags = 0;
+	err = ctrlr->send_cmd(ctrlr, &cmd, NULL);
+	if (err)
+		return err;
+	memcpy(media->cid, cmd.response, sizeof(media->cid));
+
+	/*
+	 * For MMC cards, set the Relative Address.
+	 * For SD cards, get the Relatvie Address.
+	 * This also puts the cards into Standby State
+	 */
+	cmd.cmdidx = SD_CMD_SEND_RELATIVE_ADDR;
+	cmd.cmdarg = media->rca << 16;
+	cmd.resp_type = CARD_RSP_R6;
+	cmd.flags = 0;
+	err = ctrlr->send_cmd(ctrlr, &cmd, NULL);
+	if (err)
+		return err;
+	if (IS_SD(media))
+		media->rca = (cmd.response[0] >> 16) & 0xffff;
+
+	/* Get the Card-Specific Data */
+	cmd.cmdidx = MMC_CMD_SEND_CSD;
+	cmd.resp_type = CARD_RSP_R2;
+	cmd.cmdarg = media->rca << 16;
+	cmd.flags = 0;
+	err = ctrlr->send_cmd(ctrlr, &cmd, NULL);
+
+	/* Waiting for the ready status */
+	sd_mmc_send_status(media, SD_MMC_IO_RETRIES);
+	if (err)
+		return err;
+
+	memcpy(media->csd, cmd.response, sizeof(media->csd));
+	if (media->version == MMC_VERSION_UNKNOWN) {
+		int version = sd_mmc_extract_uint32_bits(media->csd, 2, 4);
+		switch (version) {
+		case 0:
+			media->version = MMC_VERSION_1_2;
+			break;
+		case 1:
+			media->version = MMC_VERSION_1_4;
+			break;
+		case 2:
+			media->version = MMC_VERSION_2_2;
+			break;
+		case 3:
+			media->version = MMC_VERSION_3;
+			break;
+		case 4:
+			media->version = MMC_VERSION_4;
+			break;
+		default:
+			media->version = MMC_VERSION_1_2;
+			break;
+		}
+	}
+	media->tran_speed = sd_mmc_calculate_transfer_speed(media->csd[0]);
+
+	/* Determine the read and write block lengths */
+	media->read_bl_len = 1 << sd_mmc_extract_uint32_bits(media->csd, 44, 4);
+	if (IS_SD(media))
+		media->write_bl_len = media->read_bl_len;
+	else
+		media->write_bl_len =
+			1 << sd_mmc_extract_uint32_bits(media->csd, 102, 4);
+
+	sd_mmc_debug("mmc media info: version=%#x, tran_speed=%d\n",
+	      media->version, (int)media->tran_speed);
+
+	return 0;
+}
diff --git a/src/commonlib/storage/sd_mmc.h b/src/commonlib/storage/sd_mmc.h
new file mode 100644
index 0000000000..d54bbc7537
--- /dev/null
+++ b/src/commonlib/storage/sd_mmc.h
@@ -0,0 +1,99 @@
+/*
+ * Copyright 2017 Intel Corporation
+ *
+ * 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.
+ */
+
+#ifndef __COMMONLIB_STORAGE_SD_MMC_H__
+#define __COMMONLIB_STORAGE_SD_MMC_H__
+
+#include <commonlib/sd_mmc_ctrlr.h>
+#include <commonlib/storage.h>
+#include <stddef.h>
+
+#define SD_MMC_IO_RETRIES	1000
+
+#define IS_SD(x)		(x->version & SD_VERSION_SD)
+
+#define SET_BUS_WIDTH(ctrlr, width)		\
+	do {					\
+		ctrlr->bus_width = width;	\
+		ctrlr->set_ios(ctrlr);		\
+	} while (0)
+
+#define SET_CLOCK(ctrlr, clock_hz)		\
+	do {					\
+		ctrlr->request_hz = clock_hz;	\
+		ctrlr->set_ios(ctrlr);		\
+	} while (0)
+
+#define SET_TIMING(ctrlr, timing_value)		\
+	do {					\
+		ctrlr->timing = timing_value;	\
+		ctrlr->set_ios(ctrlr);		\
+	} while (0)
+
+/* Common support routines */
+int sd_mmc_enter_standby(struct storage_media *media);
+uint64_t sd_mmc_extract_uint32_bits(const uint32_t *array, int start,
+	int count);
+int sd_mmc_send_status(struct storage_media *media, ssize_t tries);
+int sd_mmc_set_blocklen(struct sd_mmc_ctrlr *ctrlr, int len);
+
+/* MMC support routines */
+int mmc_change_freq(struct storage_media *media);
+int mmc_complete_op_cond(struct storage_media *media);
+const char *mmc_partition_name(struct storage_media *media,
+	unsigned int partition_number);
+int mmc_send_ext_csd(struct sd_mmc_ctrlr *ctrlr, unsigned char *ext_csd);
+int mmc_send_op_cond(struct storage_media *media);
+int mmc_set_bus_width(struct storage_media *media);
+int mmc_set_partition(struct storage_media *media,
+	unsigned int partition_number);
+int mmc_update_capacity(struct storage_media *media);
+
+/* SD card support routines */
+int sd_change_freq(struct storage_media *media);
+const char *sd_partition_name(struct storage_media *media,
+	unsigned int partition_number);
+int sd_send_if_cond(struct storage_media *media);
+int sd_send_op_cond(struct storage_media *media);
+int sd_set_bus_width(struct storage_media *media);
+int sd_set_partition(struct storage_media *media,
+	unsigned int partition_number);
+
+/* Controller debug functions */
+#define sdhc_debug(format...) \
+	do {						\
+		if (IS_ENABLED(CONFIG_SDHC_DEBUG))	\
+			printk(BIOS_DEBUG, format);	\
+	} while (0)
+#define sdhc_trace(format...) \
+	do {						\
+		if (IS_ENABLED(CONFIG_SDHC_TRACE))	\
+			printk(BIOS_DEBUG, format);	\
+	} while (0)
+#define sdhc_error(format...) printk(BIOS_ERR, "ERROR: " format)
+
+/* Card/device debug functions */
+#define sd_mmc_debug(format...) \
+	do {						\
+		if (IS_ENABLED(CONFIG_SD_MMC_DEBUG))	\
+			printk(BIOS_DEBUG, format);	\
+	} while (0)
+#define sd_mmc_trace(format...) \
+	do {						\
+		if (IS_ENABLED(CONFIG_SD_MMC_TRACE))	\
+			printk(BIOS_DEBUG, format);	\
+	} while (0)
+#define sd_mmc_error(format...) printk(BIOS_ERR, "ERROR: " format)
+
+#endif /* __COMMONLIB_STORAGE_SD_MMC_H__ */
diff --git a/src/commonlib/storage/sdhci.c b/src/commonlib/storage/sdhci.c
new file mode 100644
index 0000000000..bac510e12a
--- /dev/null
+++ b/src/commonlib/storage/sdhci.c
@@ -0,0 +1,813 @@
+/*
+ * Copyright 2011, Marvell Semiconductor Inc.
+ * Lei Wen <leiwen@marvell.com>
+ *
+ * Copyright 2017 Intel Corporation
+ *
+ * Secure Digital (SD) Host Controller interface specific code
+ *
+ * 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.
+ */
+
+#include <assert.h>
+#include "bouncebuf.h"
+#include <console/console.h>
+#include <commonlib/sd_mmc_ctrlr.h>
+#include <commonlib/sdhci.h>
+#include <commonlib/storage.h>
+#include <delay.h>
+#include <endian.h>
+#include <halt.h>
+#include "sdhci.h"
+#include "sd_mmc.h"
+#include "storage.h"
+#include <string.h>
+#include <timer.h>
+#include <commonlib/stdlib.h>
+
+#define DMA_AVAILABLE	((CONFIG_SDHCI_ADMA_IN_BOOTBLOCK && ENV_BOOTBLOCK) \
+			|| (CONFIG_SDHCI_ADMA_IN_VERSTAGE && ENV_VERSTAGE) \
+			|| (CONFIG_SDHCI_ADMA_IN_ROMSTAGE && ENV_ROMSTAGE) \
+			|| ENV_POSTCAR || ENV_RAMSTAGE)
+
+__attribute__((weak)) void *dma_malloc(size_t length_in_bytes)
+{
+	return malloc(length_in_bytes);
+}
+
+void sdhci_reset(struct sdhci_ctrlr *sdhci_ctrlr, u8 mask)
+{
+	struct stopwatch sw;
+
+	/* Wait max 100 ms */
+	stopwatch_init_msecs_expire(&sw, 100);
+
+	sdhci_writeb(sdhci_ctrlr, mask, SDHCI_SOFTWARE_RESET);
+	while (sdhci_readb(sdhci_ctrlr, SDHCI_SOFTWARE_RESET) & mask) {
+		if (stopwatch_expired(&sw)) {
+			sdhc_error("Reset 0x%x never completed.\n", (int)mask);
+			return;
+		}
+		udelay(1000);
+	}
+}
+
+void sdhci_cmd_done(struct sdhci_ctrlr *sdhci_ctrlr, struct mmc_command *cmd)
+{
+	int i;
+	if (cmd->resp_type & CARD_RSP_136) {
+		/* CRC is stripped so we need to do some shifting. */
+		for (i = 0; i < 4; i++) {
+			cmd->response[i] = sdhci_readl(sdhci_ctrlr,
+					SDHCI_RESPONSE + (3-i)*4) << 8;
+			if (i != 3)
+				cmd->response[i] |= sdhci_readb(sdhci_ctrlr,
+						SDHCI_RESPONSE + (3-i)*4-1);
+		}
+		sdhc_log_response(4, &cmd->response[0]);
+		sdhc_trace("Response: 0x%08x.%08x.%08x.%08x\n",
+			cmd->response[3], cmd->response[2], cmd->response[1],
+			cmd->response[0]);
+	} else {
+		cmd->response[0] = sdhci_readl(sdhci_ctrlr, SDHCI_RESPONSE);
+		sdhc_log_response(1, &cmd->response[0]);
+		sdhc_trace("Response: 0x%08x\n", cmd->response[0]);
+	}
+}
+
+static int sdhci_transfer_data(struct sdhci_ctrlr *sdhci_ctrlr,
+	struct mmc_data *data, unsigned int start_addr)
+{
+	uint32_t block_count;
+	uint32_t *buffer;
+	uint32_t *buffer_end;
+	uint32_t ps;
+	uint32_t ps_mask;
+	uint32_t stat;
+	struct stopwatch sw;
+
+	block_count = 0;
+	buffer = (uint32_t *)data->dest;
+	ps_mask = (data->flags & DATA_FLAG_READ)
+		? SDHCI_DATA_AVAILABLE : SDHCI_SPACE_AVAILABLE;
+	stopwatch_init_msecs_expire(&sw, 100);
+	do {
+		/* Stop transfers if there is an error */
+		stat = sdhci_readl(sdhci_ctrlr, SDHCI_INT_STATUS);
+		sdhci_writel(sdhci_ctrlr, stat, SDHCI_INT_STATUS);
+		if (stat & SDHCI_INT_ERROR) {
+			sdhc_error("Error detected in status(0x%X)!\n", stat);
+			return -1;
+		}
+
+		/* Determine if the buffer is ready to move data */
+		ps = sdhci_readl(sdhci_ctrlr, SDHCI_PRESENT_STATE);
+		if (!(ps & ps_mask)) {
+			if (stopwatch_expired(&sw)) {
+				sdhc_error("Transfer data timeout\n");
+				return -1;
+			}
+			udelay(1);
+			continue;
+		}
+
+		/* Transfer a block of data */
+		buffer_end = &buffer[data->blocksize >> 2];
+		if (data->flags == DATA_FLAG_READ)
+			while (buffer_end > buffer)
+				*buffer++ = sdhci_readl(sdhci_ctrlr,
+					SDHCI_BUFFER);
+		else
+			while (buffer_end > buffer)
+				sdhci_writel(sdhci_ctrlr, *buffer++,
+					SDHCI_BUFFER);
+		if (++block_count >= data->blocks)
+			break;
+	} while (!(stat & SDHCI_INT_DATA_END));
+	return 0;
+}
+
+static int sdhci_send_command_bounced(struct sd_mmc_ctrlr *ctrlr,
+	struct mmc_command *cmd, struct mmc_data *data,
+	struct bounce_buffer *bbstate)
+{
+	struct sdhci_ctrlr *sdhci_ctrlr = (struct sdhci_ctrlr *)ctrlr;
+	u16 mode = 0;
+	unsigned int stat = 0;
+	int ret = 0;
+	u32 mask, flags;
+	unsigned int timeout, start_addr = 0;
+	struct stopwatch sw;
+
+	/* Wait max 1 s */
+	timeout = 1000;
+
+	sdhci_writel(sdhci_ctrlr, SDHCI_INT_ALL_MASK, SDHCI_INT_STATUS);
+	mask = SDHCI_CMD_INHIBIT | SDHCI_DATA_INHIBIT;
+
+	/* We shouldn't wait for data inihibit for stop commands, even
+	   though they might use busy signaling */
+	if (cmd->flags & CMD_FLAG_IGNORE_INHIBIT)
+		mask &= ~SDHCI_DATA_INHIBIT;
+
+	while (sdhci_readl(sdhci_ctrlr, SDHCI_PRESENT_STATE) & mask) {
+		if (timeout == 0) {
+			sdhc_trace("Cmd: %2d, Arg: 0x%08x, not sent\n",
+					cmd->cmdidx, cmd->cmdarg);
+			sdhc_error("Controller never released inhibit bit(s), "
+			       "present state %#8.8x.\n",
+			       sdhci_readl(sdhci_ctrlr, SDHCI_PRESENT_STATE));
+			return CARD_COMM_ERR;
+		}
+		timeout--;
+		udelay(1000);
+	}
+
+	mask = SDHCI_INT_RESPONSE;
+	if (!(cmd->resp_type & CARD_RSP_PRESENT))
+		flags = SDHCI_CMD_RESP_NONE;
+	else if (cmd->resp_type & CARD_RSP_136)
+		flags = SDHCI_CMD_RESP_LONG;
+	else if (cmd->resp_type & CARD_RSP_BUSY) {
+		flags = SDHCI_CMD_RESP_SHORT_BUSY;
+		mask |= SDHCI_INT_DATA_END;
+	} else
+		flags = SDHCI_CMD_RESP_SHORT;
+
+	if (cmd->resp_type & CARD_RSP_CRC)
+		flags |= SDHCI_CMD_CRC;
+	if (cmd->resp_type & CARD_RSP_OPCODE)
+		flags |= SDHCI_CMD_INDEX;
+	if (data)
+		flags |= SDHCI_CMD_DATA;
+
+	/* Set Transfer mode regarding to data flag */
+	if (data) {
+		sdhci_writew(sdhci_ctrlr,
+			SDHCI_MAKE_BLKSZ(SDHCI_DEFAULT_BOUNDARY_ARG,
+			data->blocksize), SDHCI_BLOCK_SIZE);
+
+		if (data->flags == DATA_FLAG_READ)
+			mode |= SDHCI_TRNS_READ;
+
+		if (data->blocks > 1)
+			mode |= SDHCI_TRNS_BLK_CNT_EN |
+				SDHCI_TRNS_MULTI | SDHCI_TRNS_ACMD12;
+
+		sdhci_writew(sdhci_ctrlr, data->blocks, SDHCI_BLOCK_COUNT);
+
+		if (DMA_AVAILABLE && (ctrlr->caps & DRVR_CAP_AUTO_CMD12)
+			&& (cmd->cmdidx != MMC_CMD_AUTO_TUNING_SEQUENCE)) {
+			if (sdhci_setup_adma(sdhci_ctrlr, data))
+				return -1;
+			mode |= SDHCI_TRNS_DMA;
+		}
+		sdhci_writew(sdhci_ctrlr, mode, SDHCI_TRANSFER_MODE);
+	}
+
+	sdhc_trace("Cmd: %2d, Arg: 0x%08x\n", cmd->cmdidx, cmd->cmdarg);
+	sdhci_writel(sdhci_ctrlr, cmd->cmdarg, SDHCI_ARGUMENT);
+	sdhci_writew(sdhci_ctrlr, SDHCI_MAKE_CMD(cmd->cmdidx, flags),
+		SDHCI_COMMAND);
+	sdhc_log_command_issued();
+
+	if (DMA_AVAILABLE && (mode & SDHCI_TRNS_DMA))
+		return sdhci_complete_adma(sdhci_ctrlr, cmd);
+
+	stopwatch_init_msecs_expire(&sw, 2550);
+	do {
+		stat = sdhci_readl(sdhci_ctrlr, SDHCI_INT_STATUS);
+		if (stat & SDHCI_INT_ERROR) {
+			sdhc_trace("Error - IntStatus: 0x%08x\n", stat);
+			break;
+		}
+
+		if (stat & SDHCI_INT_DATA_AVAIL) {
+			sdhci_writel(sdhci_ctrlr, stat, SDHCI_INT_STATUS);
+			return 0;
+		}
+
+		/* Apply max timeout for R1b-type CMD defined in eMMC ext_csd
+		   except for erase ones */
+		if (stopwatch_expired(&sw)) {
+			if (ctrlr->caps & DRVR_CAP_BROKEN_R1B)
+				return 0;
+			sdhc_error(
+				"Timeout for status update!  IntStatus: 0x%08x\n",
+				stat);
+			return CARD_TIMEOUT;
+		}
+	} while ((stat & mask) != mask);
+
+	if ((stat & (SDHCI_INT_ERROR | mask)) == mask) {
+		if (cmd->cmdidx)
+			sdhci_cmd_done(sdhci_ctrlr, cmd);
+		sdhci_writel(sdhci_ctrlr, mask, SDHCI_INT_STATUS);
+	} else
+		ret = -1;
+
+	if (!ret && data)
+		ret = sdhci_transfer_data(sdhci_ctrlr, data, start_addr);
+
+	if (ctrlr->udelay_wait_after_cmd)
+		udelay(ctrlr->udelay_wait_after_cmd);
+
+	stat = sdhci_readl(sdhci_ctrlr, SDHCI_INT_STATUS);
+	sdhci_writel(sdhci_ctrlr, SDHCI_INT_ALL_MASK, SDHCI_INT_STATUS);
+
+	if (!ret)
+		return 0;
+
+	sdhci_reset(sdhci_ctrlr, SDHCI_RESET_CMD);
+	sdhci_reset(sdhci_ctrlr, SDHCI_RESET_DATA);
+	if (stat & SDHCI_INT_TIMEOUT) {
+		sdhc_error("CMD%d timeout, IntStatus: 0x%08x\n", cmd->cmdidx,
+			stat);
+		return CARD_TIMEOUT;
+	}
+
+	sdhc_error("CMD%d failed, IntStatus: 0x%08x\n", cmd->cmdidx, stat);
+	return CARD_COMM_ERR;
+}
+
+__attribute__((weak)) void sdhc_log_command(struct mmc_command *cmd)
+{
+}
+
+__attribute__((weak)) void sdhc_log_command_issued(void)
+{
+}
+
+__attribute__((weak)) void sdhc_log_response(uint32_t entries,
+	uint32_t *response)
+{
+}
+
+__attribute__((weak)) void sdhc_log_ret(int ret)
+{
+}
+
+static void sdhci_led_control(struct sd_mmc_ctrlr *ctrlr, int on)
+{
+	uint8_t host_ctrl;
+	struct sdhci_ctrlr *sdhci_ctrlr = (struct sdhci_ctrlr *)ctrlr;
+
+	host_ctrl = sdhci_readb(sdhci_ctrlr, SDHCI_HOST_CONTROL);
+	host_ctrl &= ~SDHCI_CTRL_LED;
+	if (on)
+		host_ctrl |= SDHCI_CTRL_LED;
+	sdhci_writeb(sdhci_ctrlr, host_ctrl, SDHCI_HOST_CONTROL);
+}
+
+static int sdhci_send_command(struct sd_mmc_ctrlr *ctrlr,
+	struct mmc_command *cmd, struct mmc_data *data)
+{
+	void *buf;
+	unsigned int bbflags;
+	size_t len;
+	struct bounce_buffer *bbstate = NULL;
+	struct bounce_buffer bbstate_val;
+	int ret;
+
+	sdhc_log_command(cmd);
+
+	if (IS_ENABLED(CONFIG_SDHCI_BOUNCE_BUFFER) && data) {
+		if (data->flags & DATA_FLAG_READ) {
+			buf = data->dest;
+			bbflags = GEN_BB_WRITE;
+		} else {
+			buf = (void *)data->src;
+			bbflags = GEN_BB_READ;
+		}
+		len = data->blocks * data->blocksize;
+
+		/*
+		 * on some platform(like rk3399 etc) need to worry about
+		 * cache coherency, so check the buffer, if not dma
+		 * coherent, use bounce_buffer to do DMA management.
+		 */
+		if (!dma_coherent(buf)) {
+			bbstate = &bbstate_val;
+			if (bounce_buffer_start(bbstate, buf, len, bbflags)) {
+				sdhc_error(
+					"ERROR: Failed to get bounce buffer.\n");
+				return -1;
+			}
+		}
+	}
+
+	sdhci_led_control(ctrlr, 1);
+	ret = sdhci_send_command_bounced(ctrlr, cmd, data, bbstate);
+	sdhci_led_control(ctrlr, 0);
+	sdhc_log_ret(ret);
+
+	if (IS_ENABLED(CONFIG_SDHCI_BOUNCE_BUFFER) && bbstate)
+		bounce_buffer_stop(bbstate);
+
+	return ret;
+}
+
+static int sdhci_set_clock(struct sdhci_ctrlr *sdhci_ctrlr, unsigned int clock)
+{
+	struct sd_mmc_ctrlr *ctrlr = &sdhci_ctrlr->sd_mmc_ctrlr;
+	unsigned int actual, div, clk, timeout;
+
+	/* Turn off the clock if requested */
+	actual = clock;
+	if (actual == 0) {
+		sdhci_writew(sdhci_ctrlr, 0, SDHCI_CLOCK_CONTROL);
+		sdhc_debug("SDHCI bus clock: Off\n");
+		return 0;
+	}
+
+	/* Compute the divisor for the new clock frequency */
+	actual = MIN(actual, ctrlr->f_max);
+	actual = MAX(actual, ctrlr->f_min);
+	if (ctrlr->clock_base <= actual)
+		div = 0;
+	else {
+		/* Version 3.00 divisors must be a multiple of 2. */
+		if ((ctrlr->version & SDHCI_SPEC_VER_MASK)
+			>= SDHCI_SPEC_300) {
+			div = MIN(((ctrlr->clock_base + actual - 1)
+				/ actual), SDHCI_MAX_DIV_SPEC_300);
+			actual = ctrlr->clock_base / div;
+			div += 1;
+		} else {
+			/* Version 2.00 divisors must be a power of 2. */
+			for (div = 1; div < SDHCI_MAX_DIV_SPEC_200; div *= 2) {
+				if ((ctrlr->clock_base / div) <= actual)
+					break;
+			}
+			actual = ctrlr->clock_base / div;
+		}
+		div >>= 1;
+	}
+
+	/* Set the new clock frequency */
+	if (actual != ctrlr->bus_hz) {
+		/* Turn off the clock */
+		sdhci_writew(sdhci_ctrlr, 0, SDHCI_CLOCK_CONTROL);
+
+		/* Set the new clock frequency */
+		clk = (div & SDHCI_DIV_MASK) << SDHCI_DIVIDER_SHIFT;
+		clk |= ((div & SDHCI_DIV_HI_MASK) >> SDHCI_DIV_MASK_LEN)
+			<< SDHCI_DIVIDER_HI_SHIFT;
+		clk |= SDHCI_CLOCK_INT_EN;
+		sdhci_writew(sdhci_ctrlr, clk, SDHCI_CLOCK_CONTROL);
+
+		/* Display the requested clock frequency */
+		sdhc_debug("SDHCI bus clock: %d.%03d MHz\n",
+				actual / 1000000,
+				(actual / 1000) % 1000);
+
+		/* Wait max 20 ms */
+		timeout = 20;
+		while (!((clk = sdhci_readw(sdhci_ctrlr, SDHCI_CLOCK_CONTROL))
+			& SDHCI_CLOCK_INT_STABLE)) {
+			if (timeout == 0) {
+				sdhc_error(
+					"Internal clock never stabilised.\n");
+				return -1;
+			}
+			timeout--;
+			udelay(1000);
+		}
+
+		clk |= SDHCI_CLOCK_CARD_EN;
+		sdhci_writew(sdhci_ctrlr, clk, SDHCI_CLOCK_CONTROL);
+		ctrlr->bus_hz = actual;
+	}
+	return 0;
+}
+
+/* Find leftmost set bit in a 32 bit integer */
+static int fls(u32 x)
+{
+	int r = 32;
+
+	if (!x)
+		return 0;
+	if (!(x & 0xffff0000u)) {
+		x <<= 16;
+		r -= 16;
+	}
+	if (!(x & 0xff000000u)) {
+		x <<= 8;
+		r -= 8;
+	}
+	if (!(x & 0xf0000000u)) {
+		x <<= 4;
+		r -= 4;
+	}
+	if (!(x & 0xc0000000u)) {
+		x <<= 2;
+		r -= 2;
+	}
+	if (!(x & 0x80000000u)) {
+		x <<= 1;
+		r -= 1;
+	}
+	return r;
+}
+
+static void sdhci_set_power(struct sdhci_ctrlr *sdhci_ctrlr,
+	unsigned short power)
+{
+	struct sd_mmc_ctrlr *ctrlr = &sdhci_ctrlr->sd_mmc_ctrlr;
+	u8 pwr = 0;
+	u8 pwr_ctrl;
+	const char *voltage;
+
+	if (power != (unsigned short)-1) {
+		switch (1 << power) {
+		case MMC_VDD_165_195:
+			voltage = "1.8";
+			pwr = SDHCI_POWER_180;
+			break;
+		case MMC_VDD_29_30:
+		case MMC_VDD_30_31:
+			voltage = "3.0";
+			pwr = SDHCI_POWER_300;
+			break;
+		case MMC_VDD_32_33:
+		case MMC_VDD_33_34:
+			voltage = "3.3";
+			pwr = SDHCI_POWER_330;
+			break;
+		}
+	}
+
+	/* Determine the power state */
+	pwr_ctrl = sdhci_readb(sdhci_ctrlr, SDHCI_POWER_CONTROL);
+	if (pwr == 0) {
+		if (pwr_ctrl & SDHCI_POWER_ON)
+			sdhc_debug("SDHCI voltage: Off\n");
+		sdhci_writeb(sdhci_ctrlr, 0, SDHCI_POWER_CONTROL);
+		return;
+	}
+
+	/* Determine if the power has changed */
+	if (pwr_ctrl != (pwr | SDHCI_POWER_ON)) {
+		sdhc_debug("SDHCI voltage: %s Volts\n", voltage);
+
+		/* Select the voltage */
+		if (ctrlr->caps & DRVR_CAP_NO_SIMULT_VDD_AND_POWER)
+			sdhci_writeb(sdhci_ctrlr, pwr, SDHCI_POWER_CONTROL);
+
+		/* Apply power to the SD/MMC device */
+		pwr |= SDHCI_POWER_ON;
+		sdhci_writeb(sdhci_ctrlr, pwr, SDHCI_POWER_CONTROL);
+	}
+}
+
+const u16 speed_driver_voltage[] = {
+	0, /*  0: BUS_TIMING_LEGACY */
+	0, /*  1: BUS_TIMING_MMC_HS */
+	0, /*  2: BUS_TIMING_SD_HS */
+	SDHCI_CTRL_UHS_SDR12 | SDHCI_CTRL_VDD_180, /* 3: BUS_TIMING_UHS_SDR12 */
+	SDHCI_CTRL_UHS_SDR25 | SDHCI_CTRL_VDD_180, /* 4: BUS_TIMING_UHS_SDR25 */
+	SDHCI_CTRL_UHS_SDR50 | SDHCI_CTRL_VDD_180, /* 5: BUS_TIMING_UHS_SDR50 */
+	/*  6: BUS_TIMING_UHS_SDR104 */
+	SDHCI_CTRL_UHS_SDR104 | SDHCI_CTRL_DRV_TYPE_A | SDHCI_CTRL_VDD_180,
+	SDHCI_CTRL_UHS_DDR50 | SDHCI_CTRL_VDD_180, /* 7: BUS_TIMING_UHS_DDR50 */
+	SDHCI_CTRL_UHS_DDR50 | SDHCI_CTRL_VDD_180, /* 8: BUS_TIMING_MMC_DDR52 */
+	/*  9: BUS_TIMING_MMC_HS200 */
+	SDHCI_CTRL_UHS_SDR104 | SDHCI_CTRL_DRV_TYPE_A | SDHCI_CTRL_VDD_180,
+	/* 10: BUS_TIMING_MMC_HS400 */
+	SDHCI_CTRL_HS400 | SDHCI_CTRL_DRV_TYPE_A | SDHCI_CTRL_VDD_180,
+	/* 11: BUS_TIMING_MMC_HS400ES */
+	SDHCI_CTRL_HS400 | SDHCI_CTRL_DRV_TYPE_A | SDHCI_CTRL_VDD_180
+};
+
+static void sdhci_set_uhs_signaling(struct sdhci_ctrlr *sdhci_ctrlr,
+	uint32_t timing)
+{
+	u16 ctrl_2;
+
+	/* Select bus speed mode, driver and VDD 1.8 volt support */
+	ctrl_2 = sdhci_readw(sdhci_ctrlr, SDHCI_HOST_CONTROL2);
+	ctrl_2 &= ~(SDHCI_CTRL_UHS_MASK | SDHCI_CTRL_DRV_TYPE_MASK
+		| SDHCI_CTRL_VDD_180);
+	if (timing < ARRAY_SIZE(speed_driver_voltage))
+		ctrl_2 |= speed_driver_voltage[timing];
+	sdhci_writew(sdhci_ctrlr, ctrl_2, SDHCI_HOST_CONTROL2);
+}
+
+static void sdhci_set_ios(struct sd_mmc_ctrlr *ctrlr)
+{
+	struct sdhci_ctrlr *sdhci_ctrlr = (struct sdhci_ctrlr *)ctrlr;
+	u32 ctrl;
+	u32 previous_ctrl;
+	u32 bus_width;
+	int version;
+
+	/* Set the clock frequency */
+	if (ctrlr->bus_hz != ctrlr->request_hz)
+		sdhci_set_clock(sdhci_ctrlr, ctrlr->request_hz);
+
+	/* Switch to 1.8 volt for HS200 */
+	if (ctrlr->caps & DRVR_CAP_1V8_VDD)
+		if (ctrlr->bus_hz == CLOCK_200MHZ)
+			sdhci_set_power(sdhci_ctrlr, MMC_VDD_165_195_SHIFT);
+
+	/* Determine the new bus width */
+	bus_width = 1;
+	ctrl = sdhci_readb(sdhci_ctrlr, SDHCI_HOST_CONTROL);
+	previous_ctrl = ctrl;
+	ctrl &= ~SDHCI_CTRL_4BITBUS;
+	version = ctrlr->version & SDHCI_SPEC_VER_MASK;
+	if (version >= SDHCI_SPEC_300)
+		ctrl &= ~SDHCI_CTRL_8BITBUS;
+
+	if ((ctrlr->bus_width == 8) && (version >= SDHCI_SPEC_300)) {
+		ctrl |= SDHCI_CTRL_8BITBUS;
+		bus_width = 8;
+	} else if (ctrlr->bus_width == 4) {
+		ctrl |= SDHCI_CTRL_4BITBUS;
+		bus_width = 4;
+	}
+
+	if (!(ctrlr->timing == BUS_TIMING_LEGACY) &&
+	    !(ctrlr->caps & DRVR_CAP_NO_HISPD_BIT))
+		ctrl |= SDHCI_CTRL_HISPD;
+	else
+		ctrl &= ~SDHCI_CTRL_HISPD;
+
+	sdhci_set_uhs_signaling(sdhci_ctrlr, ctrlr->timing);
+
+	if (DMA_AVAILABLE) {
+		if (ctrlr->caps & DRVR_CAP_AUTO_CMD12) {
+			ctrl &= ~SDHCI_CTRL_DMA_MASK;
+			if (ctrlr->caps & DRVR_CAP_DMA_64BIT)
+				ctrl |= SDHCI_CTRL_ADMA64;
+			else
+				ctrl |= SDHCI_CTRL_ADMA32;
+		}
+	}
+
+	/* Set the new bus width */
+	if (IS_ENABLED(CONFIG_SDHC_DEBUG)
+		&& ((ctrl ^ previous_ctrl) & (SDHCI_CTRL_4BITBUS
+		| ((version >= SDHCI_SPEC_300) ? SDHCI_CTRL_8BITBUS : 0))))
+		sdhc_debug("SDHCI bus width: %d bit%s\n", bus_width,
+			(bus_width != 1) ? "s" : "");
+	sdhci_writeb(sdhci_ctrlr, ctrl, SDHCI_HOST_CONTROL);
+}
+
+static void sdhci_tuning_start(struct sd_mmc_ctrlr *ctrlr, int retune)
+{
+	uint16_t host_ctrl2;
+	struct sdhci_ctrlr *sdhci_ctrlr = (struct sdhci_ctrlr *)ctrlr;
+
+	/* Start the bus tuning */
+	host_ctrl2 = sdhci_readw(sdhci_ctrlr, SDHCI_HOST_CONTROL2);
+	host_ctrl2 &= ~SDHCI_CTRL_TUNED_CLK;
+	host_ctrl2 |= (retune ? SDHCI_CTRL_TUNED_CLK : 0)
+		| SDHCI_CTRL_EXEC_TUNING;
+	sdhci_writew(sdhci_ctrlr, host_ctrl2, SDHCI_HOST_CONTROL2);
+}
+
+static int sdhci_is_tuning_complete(struct sd_mmc_ctrlr *ctrlr, int *successful)
+{
+	uint16_t host_ctrl2;
+	struct sdhci_ctrlr *sdhci_ctrlr = (struct sdhci_ctrlr *)ctrlr;
+
+	/* Determine if the bus tuning has completed */
+	host_ctrl2 = sdhci_readw(sdhci_ctrlr, SDHCI_HOST_CONTROL2);
+	*successful = ((host_ctrl2 & SDHCI_CTRL_TUNED_CLK) != 0);
+	return ((host_ctrl2 & SDHCI_CTRL_EXEC_TUNING) == 0);
+}
+
+/* Prepare SDHCI controller to be initialized */
+static int sdhci_pre_init(struct sdhci_ctrlr *sdhci_ctrlr)
+{
+	struct sd_mmc_ctrlr *ctrlr = &sdhci_ctrlr->sd_mmc_ctrlr;
+	unsigned int caps, caps_1;
+
+	/* Get controller version and capabilities */
+	ctrlr->version = sdhci_readw(sdhci_ctrlr, SDHCI_HOST_VERSION) & 0xff;
+	caps = sdhci_readl(sdhci_ctrlr, SDHCI_CAPABILITIES);
+	caps_1 = sdhci_readl(sdhci_ctrlr, SDHCI_CAPABILITIES_1);
+
+	/* Determine the supported voltages */
+	if (caps & SDHCI_CAN_VDD_330)
+		ctrlr->voltages |= MMC_VDD_32_33 | MMC_VDD_33_34;
+	if (caps & SDHCI_CAN_VDD_300)
+		ctrlr->voltages |= MMC_VDD_29_30 | MMC_VDD_30_31;
+	if (caps & SDHCI_CAN_VDD_180)
+		ctrlr->voltages |= MMC_VDD_165_195;
+
+	/* Get the controller's base clock frequency */
+	if ((ctrlr->version & SDHCI_SPEC_VER_MASK) >= SDHCI_SPEC_300)
+		ctrlr->clock_base = (caps & SDHCI_CLOCK_V3_BASE_MASK)
+			>> SDHCI_CLOCK_BASE_SHIFT;
+	else
+		ctrlr->clock_base = (caps & SDHCI_CLOCK_BASE_MASK)
+			>> SDHCI_CLOCK_BASE_SHIFT;
+	ctrlr->clock_base *= 1000000;
+	ctrlr->f_max = ctrlr->clock_base;
+
+	/* Determine the controller's clock frequency range */
+	ctrlr->f_min = 0;
+	if ((ctrlr->version & SDHCI_SPEC_VER_MASK) >= SDHCI_SPEC_300)
+		ctrlr->f_min =
+			ctrlr->clock_base / SDHCI_MAX_DIV_SPEC_300;
+	else
+		ctrlr->f_min =
+			ctrlr->clock_base / SDHCI_MAX_DIV_SPEC_200;
+
+	/* Determine the controller's modes of operation */
+	ctrlr->caps |= DRVR_CAP_HS52 | DRVR_CAP_HS;
+	if (ctrlr->clock_base >= CLOCK_200MHZ) {
+		ctrlr->caps |= DRVR_CAP_HS200 | DRVR_CAP_HS200_TUNING;
+		if (caps_1 & SDHCI_SUPPORT_HS400)
+			ctrlr->caps |= DRVR_CAP_HS400
+				| DRVR_CAP_ENHANCED_STROBE;
+	}
+
+	/* Determine the bus widths the controller supports */
+	ctrlr->caps |= DRVR_CAP_4BIT;
+	if (caps & SDHCI_CAN_DO_8BIT)
+		ctrlr->caps |= DRVR_CAP_8BIT;
+
+	/* Determine the controller's DMA support */
+	if (caps & SDHCI_CAN_DO_ADMA2)
+		ctrlr->caps |= DRVR_CAP_AUTO_CMD12;
+	if (DMA_AVAILABLE && (caps & SDHCI_CAN_64BIT))
+		ctrlr->caps |= DRVR_CAP_DMA_64BIT;
+
+	/* Specify the modes that the driver stack supports */
+	ctrlr->caps |= DRVR_CAP_HC;
+
+	/* Let the SOC adjust the configuration to handle controller quirks */
+	soc_sd_mmc_controller_quirks(&sdhci_ctrlr->sd_mmc_ctrlr);
+	if (ctrlr->clock_base == 0) {
+		sdhc_error("Hardware doesn't specify base clock frequency\n");
+		return -1;
+	}
+	if (!ctrlr->f_max)
+		ctrlr->f_max = ctrlr->clock_base;
+
+	/* Display the results */
+	sdhc_trace("0x%08x: ctrlr->caps\n", ctrlr->caps);
+	sdhc_trace("%d.%03d MHz: ctrlr->clock_base\n",
+		ctrlr->clock_base / 1000000,
+		(ctrlr->clock_base / 1000) % 1000);
+	sdhc_trace("%d.%03d MHz: ctrlr->f_max\n",
+		ctrlr->f_max / 1000000,
+		(ctrlr->f_max / 1000) % 1000);
+	sdhc_trace("%d.%03d MHz: ctrlr->f_min\n",
+		ctrlr->f_min / 1000000,
+		(ctrlr->f_min / 1000) % 1000);
+	sdhc_trace("0x%08x: ctrlr->voltages\n", ctrlr->voltages);
+
+	sdhci_reset(sdhci_ctrlr, SDHCI_RESET_ALL);
+
+	return 0;
+}
+
+__attribute__((weak)) void soc_sd_mmc_controller_quirks(struct sd_mmc_ctrlr
+	*ctrlr)
+{
+}
+
+static int sdhci_init(struct sdhci_ctrlr *sdhci_ctrlr)
+{
+	struct sd_mmc_ctrlr *ctrlr = &sdhci_ctrlr->sd_mmc_ctrlr;
+	int rv;
+
+	/* Only initialize the controller upon reset or card insertion */
+	if (ctrlr->initialized)
+		return 0;
+
+	sdhc_debug("SDHCI Controller Base Address: 0x%p\n",
+			sdhci_ctrlr->ioaddr);
+
+	rv = sdhci_pre_init(sdhci_ctrlr);
+	if (rv)
+		return rv; /* The error has been already reported */
+
+	sdhci_set_power(sdhci_ctrlr, fls(ctrlr->voltages) - 1);
+
+	if (ctrlr->caps & DRVR_CAP_NO_CD) {
+		unsigned int status;
+
+		sdhci_writel(sdhci_ctrlr, SDHCI_CTRL_CD_TEST_INS
+			| SDHCI_CTRL_CD_TEST, SDHCI_HOST_CONTROL);
+
+		status = sdhci_readl(sdhci_ctrlr, SDHCI_PRESENT_STATE);
+		while ((!(status & SDHCI_CARD_PRESENT)) ||
+		    (!(status & SDHCI_CARD_STATE_STABLE)) ||
+		    (!(status & SDHCI_CARD_DETECT_PIN_LEVEL)))
+			status = sdhci_readl(sdhci_ctrlr, SDHCI_PRESENT_STATE);
+	}
+
+	/* Enable only interrupts served by the SD controller */
+	sdhci_writel(sdhci_ctrlr, SDHCI_INT_DATA_MASK | SDHCI_INT_CMD_MASK,
+		     SDHCI_INT_ENABLE);
+	/* Mask all sdhci interrupt sources */
+	sdhci_writel(sdhci_ctrlr, 0x0, SDHCI_SIGNAL_ENABLE);
+
+	/* Set timeout to maximum, shouldn't happen if everything's right. */
+	sdhci_writeb(sdhci_ctrlr, 0xe, SDHCI_TIMEOUT_CONTROL);
+
+	mdelay(10);
+	ctrlr->initialized = 1;
+	return 0;
+}
+
+static int sdhci_update(struct sdhci_ctrlr *sdhci_ctrlr)
+{
+	struct sd_mmc_ctrlr *ctrlr = &sdhci_ctrlr->sd_mmc_ctrlr;
+
+	if (ctrlr->caps & DRVR_CAP_REMOVABLE) {
+		int present = (sdhci_readl(sdhci_ctrlr, SDHCI_PRESENT_STATE) &
+			       SDHCI_CARD_PRESENT) != 0;
+
+		if (!present) {
+			/* A card was present indicate the controller needs
+			 * initialization on the next call.
+			 */
+			ctrlr->initialized = 0;
+			return 0;
+		}
+	}
+
+	/* A card is present, get it ready. */
+	if (sdhci_init(sdhci_ctrlr))
+		return -1;
+	return 0;
+}
+
+void sdhci_update_pointers(struct sdhci_ctrlr *sdhci_ctrlr)
+{
+	struct sd_mmc_ctrlr *ctrlr = &sdhci_ctrlr->sd_mmc_ctrlr;
+
+	/* Update the routine pointers */
+	ctrlr->send_cmd = &sdhci_send_command;
+	ctrlr->set_ios = &sdhci_set_ios;
+	ctrlr->tuning_start = &sdhci_tuning_start;
+	ctrlr->is_tuning_complete = &sdhci_is_tuning_complete;
+}
+
+int add_sdhci(struct sdhci_ctrlr *sdhci_ctrlr)
+{
+	struct sd_mmc_ctrlr *ctrlr = &sdhci_ctrlr->sd_mmc_ctrlr;
+
+	sdhci_update_pointers(sdhci_ctrlr);
+
+	/* TODO(vbendeb): check if SDHCI spec allows to retrieve this value. */
+	ctrlr->b_max = 65535;
+
+	/* Initialize the SDHC controller */
+	return sdhci_update(sdhci_ctrlr);
+}
diff --git a/src/commonlib/storage/sdhci.h b/src/commonlib/storage/sdhci.h
new file mode 100644
index 0000000000..5dd5391f38
--- /dev/null
+++ b/src/commonlib/storage/sdhci.h
@@ -0,0 +1,281 @@
+/*
+ * Copyright 2011, Marvell Semiconductor Inc.
+ * Lei Wen <leiwen@marvell.com>
+ *
+ * Copyright 2017 Intel Corporation
+ *
+ * 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.
+ */
+#ifndef __COMMONLIB_STORAGE_SDHCI_H__
+#define __COMMONLIB_STORAGE_SDHCI_H__
+
+#include <arch/io.h>
+#include <commonlib/sd_mmc_ctrlr.h>
+
+/*
+ * Controller registers
+ */
+
+#define SDHCI_DMA_ADDRESS	0x00
+
+#define SDHCI_BLOCK_SIZE	0x04
+#define  SDHCI_MAKE_BLKSZ(dma, blksz) (((dma & 0x7) << 12) | (blksz & 0xFFF))
+
+#define SDHCI_BLOCK_COUNT	0x06
+
+#define SDHCI_ARGUMENT		0x08
+
+#define SDHCI_TRANSFER_MODE	0x0C
+#define  SDHCI_TRNS_DMA		0x01
+#define  SDHCI_TRNS_BLK_CNT_EN	0x02
+#define  SDHCI_TRNS_ACMD12	0x04
+#define  SDHCI_TRNS_READ	0x10
+#define  SDHCI_TRNS_MULTI	0x20
+
+#define SDHCI_COMMAND		0x0E
+#define  SDHCI_CMD_RESP_MASK	0x03
+#define  SDHCI_CMD_CRC		0x08
+#define  SDHCI_CMD_INDEX	0x10
+#define  SDHCI_CMD_DATA		0x20
+#define  SDHCI_CMD_ABORTCMD	0xC0
+
+#define  SDHCI_CMD_RESP_NONE	0x00
+#define  SDHCI_CMD_RESP_LONG	0x01
+#define  SDHCI_CMD_RESP_SHORT	0x02
+#define  SDHCI_CMD_RESP_SHORT_BUSY 0x03
+
+#define SDHCI_MAKE_CMD(c, f) (((c & 0xff) << 8) | (f & 0xff))
+#define SDHCI_GET_CMD(c) ((c>>8) & 0x3f)
+
+#define SDHCI_RESPONSE		0x10
+
+#define SDHCI_BUFFER		0x20
+
+#define SDHCI_PRESENT_STATE	0x24
+#define  SDHCI_CMD_INHIBIT	0x00000001
+#define  SDHCI_DATA_INHIBIT	0x00000002
+#define  SDHCI_DOING_WRITE	0x00000100
+#define  SDHCI_DOING_READ	0x00000200
+#define  SDHCI_SPACE_AVAILABLE	0x00000400
+#define  SDHCI_DATA_AVAILABLE	0x00000800
+#define  SDHCI_CARD_PRESENT	0x00010000
+#define  SDHCI_CARD_STATE_STABLE	0x00020000
+#define  SDHCI_CARD_DETECT_PIN_LEVEL	0x00040000
+#define  SDHCI_WRITE_PROTECT	0x00080000
+
+#define SDHCI_HOST_CONTROL	0x28
+#define  SDHCI_CTRL_LED		0x01
+#define  SDHCI_CTRL_4BITBUS	0x02
+#define  SDHCI_CTRL_HISPD	0x04
+#define  SDHCI_CTRL_DMA_MASK	0x18
+#define   SDHCI_CTRL_SDMA	0x00
+#define   SDHCI_CTRL_ADMA1	0x08
+#define   SDHCI_CTRL_ADMA32	0x10
+#define   SDHCI_CTRL_ADMA64	0x18
+#define  SDHCI_CTRL_8BITBUS	0x20
+#define  SDHCI_CTRL_CD_TEST_INS	0x40
+#define  SDHCI_CTRL_CD_TEST	0x80
+
+#define SDHCI_POWER_CONTROL	0x29
+#define  SDHCI_POWER_ON		0x01
+#define  SDHCI_POWER_180	0x0A
+#define  SDHCI_POWER_300	0x0C
+#define  SDHCI_POWER_330	0x0E
+
+#define SDHCI_BLOCK_GAP_CONTROL	0x2A
+
+#define SDHCI_WAKE_UP_CONTROL	0x2B
+#define  SDHCI_WAKE_ON_INT	0x01
+#define  SDHCI_WAKE_ON_INSERT	0x02
+#define  SDHCI_WAKE_ON_REMOVE	0x04
+
+#define SDHCI_CLOCK_CONTROL	0x2C
+#define  SDHCI_DIVIDER_SHIFT	8
+#define  SDHCI_DIVIDER_HI_SHIFT	6
+#define  SDHCI_DIV_MASK	0xFF
+#define  SDHCI_DIV_MASK_LEN	8
+#define  SDHCI_DIV_HI_MASK	0x300
+#define  SDHCI_CLOCK_CARD_EN	0x0004
+#define  SDHCI_CLOCK_INT_STABLE	0x0002
+#define  SDHCI_CLOCK_INT_EN	0x0001
+
+#define SDHCI_TIMEOUT_CONTROL	0x2E
+
+#define SDHCI_SOFTWARE_RESET	0x2F
+#define  SDHCI_RESET_ALL	0x01
+#define  SDHCI_RESET_CMD	0x02
+#define  SDHCI_RESET_DATA	0x04
+
+#define SDHCI_INT_STATUS	0x30
+#define SDHCI_INT_ENABLE	0x34
+#define SDHCI_SIGNAL_ENABLE	0x38
+#define  SDHCI_INT_RESPONSE	0x00000001
+#define  SDHCI_INT_DATA_END	0x00000002
+#define  SDHCI_INT_DMA_END	0x00000008
+#define  SDHCI_INT_SPACE_AVAIL	0x00000010
+#define  SDHCI_INT_DATA_AVAIL	0x00000020
+#define  SDHCI_INT_CARD_INSERT	0x00000040
+#define  SDHCI_INT_CARD_REMOVE	0x00000080
+#define  SDHCI_INT_CARD_INT	0x00000100
+#define  SDHCI_INT_ERROR	0x00008000
+#define  SDHCI_INT_TIMEOUT	0x00010000
+#define  SDHCI_INT_CRC		0x00020000
+#define  SDHCI_INT_END_BIT	0x00040000
+#define  SDHCI_INT_INDEX	0x00080000
+#define  SDHCI_INT_DATA_TIMEOUT	0x00100000
+#define  SDHCI_INT_DATA_CRC	0x00200000
+#define  SDHCI_INT_DATA_END_BIT	0x00400000
+#define  SDHCI_INT_BUS_POWER	0x00800000
+#define  SDHCI_INT_ACMD12ERR	0x01000000
+#define  SDHCI_INT_ADMA_ERROR	0x02000000
+
+#define  SDHCI_INT_NORMAL_MASK	0x00007FFF
+#define  SDHCI_INT_ERROR_MASK	0xFFFF8000
+
+#define  SDHCI_INT_CMD_MASK	(SDHCI_INT_RESPONSE | SDHCI_INT_TIMEOUT \
+				| SDHCI_INT_CRC | SDHCI_INT_END_BIT \
+				| SDHCI_INT_INDEX)
+#define  SDHCI_INT_DATA_MASK	(SDHCI_INT_DATA_END | SDHCI_INT_DMA_END \
+				| SDHCI_INT_DATA_AVAIL | SDHCI_INT_SPACE_AVAIL \
+				| SDHCI_INT_DATA_TIMEOUT | SDHCI_INT_DATA_CRC \
+				| SDHCI_INT_DATA_END_BIT | SDHCI_INT_ADMA_ERROR)
+#define SDHCI_INT_ALL_MASK	((unsigned int)-1)
+
+#define SDHCI_ACMD12_ERR	0x3C
+
+#define SDHCI_HOST_CONTROL2             0x3E
+#define  SDHCI_CTRL_UHS_MASK            0x0007
+#define   SDHCI_CTRL_UHS_SDR12          0x0000
+#define   SDHCI_CTRL_UHS_SDR25          0x0001
+#define   SDHCI_CTRL_UHS_SDR50          0x0002
+#define   SDHCI_CTRL_UHS_SDR104         0x0003
+#define   SDHCI_CTRL_UHS_DDR50          0x0004
+#define   SDHCI_CTRL_HS400		0x0005 /* reserved value in SDIO spec */
+#define  SDHCI_CTRL_VDD_180             0x0008
+#define  SDHCI_CTRL_DRV_TYPE_MASK       0x0030
+#define   SDHCI_CTRL_DRV_TYPE_B         0x0000
+#define   SDHCI_CTRL_DRV_TYPE_A         0x0010
+#define   SDHCI_CTRL_DRV_TYPE_C         0x0020
+#define   SDHCI_CTRL_DRV_TYPE_D         0x0030
+#define  SDHCI_CTRL_EXEC_TUNING         0x0040
+#define  SDHCI_CTRL_TUNED_CLK           0x0080
+#define  SDHCI_CTRL_PRESET_VAL_ENABLE   0x8000
+
+#define SDHCI_CAPABILITIES	0x40
+#define  SDHCI_TIMEOUT_CLK_MASK	0x0000003F
+#define  SDHCI_TIMEOUT_CLK_SHIFT 0
+#define  SDHCI_TIMEOUT_CLK_UNIT	0x00000080
+#define  SDHCI_CLOCK_BASE_MASK	0x00003F00
+#define  SDHCI_CLOCK_V3_BASE_MASK	0x0000FF00
+#define  SDHCI_CLOCK_BASE_SHIFT	8
+#define  SDHCI_MAX_BLOCK_MASK	0x00030000
+#define  SDHCI_MAX_BLOCK_SHIFT  16
+#define  SDHCI_CAN_DO_8BIT	0x00040000
+#define  SDHCI_CAN_DO_ADMA2	0x00080000
+#define  SDHCI_CAN_DO_ADMA1	0x00100000
+#define  SDHCI_CAN_DO_HISPD	0x00200000
+#define  SDHCI_CAN_DO_SDMA	0x00400000
+#define  SDHCI_CAN_VDD_330	0x01000000
+#define  SDHCI_CAN_VDD_300	0x02000000
+#define  SDHCI_CAN_VDD_180	0x04000000
+#define  SDHCI_CAN_64BIT	0x10000000
+
+#define SDHCI_CAPABILITIES_1	0x44
+#define SDHCI_SUPPORT_HS400	0x80000000
+
+#define SDHCI_MAX_CURRENT	0x48
+
+/* 4C-4F reserved for more max current */
+
+#define SDHCI_SET_ACMD12_ERROR	0x50
+#define SDHCI_SET_INT_ERROR	0x52
+
+#define SDHCI_ADMA_ERROR	0x54
+
+/* 55-57 reserved */
+
+#define SDHCI_ADMA_ADDRESS	0x58
+
+/* 60-FB reserved */
+
+#define SDHCI_SLOT_INT_STATUS	0xFC
+
+#define SDHCI_HOST_VERSION	0xFE
+#define  SDHCI_VENDOR_VER_MASK	0xFF00
+#define  SDHCI_VENDOR_VER_SHIFT	8
+#define  SDHCI_SPEC_VER_MASK	0x00FF
+#define  SDHCI_SPEC_VER_SHIFT	0
+#define   SDHCI_SPEC_100	0
+#define   SDHCI_SPEC_200	1
+#define   SDHCI_SPEC_300	2
+
+/*
+ * End of controller registers.
+ */
+
+#define SDHCI_MAX_DIV_SPEC_200	256
+#define SDHCI_MAX_DIV_SPEC_300	2046
+
+/*
+ * Controller SDMA buffer boundary. Valid values from 4K to 512K in powers of 2.
+ */
+#define SDHCI_DEFAULT_BOUNDARY_SIZE	(512 * 1024)
+#define SDHCI_DEFAULT_BOUNDARY_ARG	(7)
+
+#define SDHCI_MAX_PER_DESCRIPTOR 0x10000
+
+/* ADMA descriptor attributes */
+#define SDHCI_ADMA_VALID (1 << 0)
+#define SDHCI_ADMA_END (1 << 1)
+#define SDHCI_ADMA_INT (1 << 2)
+#define SDHCI_ACT_NOP (0 << 4)
+#define SDHCI_ACT_TRAN (2 << 4)
+#define SDHCI_ACT_LINK (3 << 4)
+
+static inline void sdhci_writel(struct sdhci_ctrlr *sdhci_ctrlr, u32 val,
+	int reg)
+{
+	write32(sdhci_ctrlr->ioaddr + reg, val);
+}
+
+static inline void sdhci_writew(struct sdhci_ctrlr *sdhci_ctrlr, u16 val,
+	int reg)
+{
+	write16(sdhci_ctrlr->ioaddr + reg, val);
+}
+
+static inline void sdhci_writeb(struct sdhci_ctrlr *sdhci_ctrlr, u8 val,
+	int reg)
+{
+	write8(sdhci_ctrlr->ioaddr + reg, val);
+}
+static inline u32 sdhci_readl(struct sdhci_ctrlr *sdhci_ctrlr, int reg)
+{
+	return read32(sdhci_ctrlr->ioaddr + reg);
+}
+
+static inline u16 sdhci_readw(struct sdhci_ctrlr *sdhci_ctrlr, int reg)
+{
+	return read16(sdhci_ctrlr->ioaddr + reg);
+}
+
+static inline u8 sdhci_readb(struct sdhci_ctrlr *sdhci_ctrlr, int reg)
+{
+	return read8(sdhci_ctrlr->ioaddr + reg);
+}
+
+void sdhci_reset(struct sdhci_ctrlr *sdhci_ctrlr, u8 mask);
+void sdhci_cmd_done(struct sdhci_ctrlr *sdhci_ctrlr, struct mmc_command *cmd);
+int sdhci_setup_adma(struct sdhci_ctrlr *sdhci_ctrlr, struct mmc_data *data);
+int sdhci_complete_adma(struct sdhci_ctrlr *sdhci_ctrlr,
+	struct mmc_command *cmd);
+
+#endif /* __COMMONLIB_STORAGE_SDHCI_H__ */
diff --git a/src/commonlib/storage/sdhci_adma.c b/src/commonlib/storage/sdhci_adma.c
new file mode 100644
index 0000000000..e95742b35b
--- /dev/null
+++ b/src/commonlib/storage/sdhci_adma.c
@@ -0,0 +1,194 @@
+/*
+ * Copyright 2011, Marvell Semiconductor Inc.
+ * Lei Wen <leiwen@marvell.com>
+ *
+ * Copyright 2017 Intel Corporation
+ *
+ * Secure Digital (SD) Host Controller interface DMA support code
+ *
+ * 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.
+ */
+
+#include <assert.h>
+#include <commonlib/sdhci.h>
+#include <commonlib/storage.h>
+#include <delay.h>
+#include <endian.h>
+#include "sdhci.h"
+#include "sd_mmc.h"
+#include "storage.h"
+#include <string.h>
+#include <timer.h>
+
+static void sdhci_alloc_adma_descs(struct sdhci_ctrlr *sdhci_ctrlr,
+	u32 need_descriptors)
+{
+	if (sdhci_ctrlr->adma_descs) {
+		if (sdhci_ctrlr->adma_desc_count < need_descriptors) {
+			/* Previously allocated array is too small */
+			free(sdhci_ctrlr->adma_descs);
+			sdhci_ctrlr->adma_desc_count = 0;
+			sdhci_ctrlr->adma_descs = NULL;
+		}
+	}
+
+	/* use dma_malloc() to make sure we get the coherent/uncached memory */
+	if (!sdhci_ctrlr->adma_descs) {
+		sdhci_ctrlr->adma_descs = malloc(need_descriptors
+			* sizeof(*sdhci_ctrlr->adma_descs));
+		if (sdhci_ctrlr->adma_descs == NULL)
+			die("fail to malloc adma_descs\n");
+		sdhci_ctrlr->adma_desc_count = need_descriptors;
+	}
+
+	memset(sdhci_ctrlr->adma_descs, 0, sizeof(*sdhci_ctrlr->adma_descs)
+		* need_descriptors);
+}
+
+static void sdhci_alloc_adma64_descs(struct sdhci_ctrlr *sdhci_ctrlr,
+	u32 need_descriptors)
+{
+	if (sdhci_ctrlr->adma64_descs) {
+		if (sdhci_ctrlr->adma_desc_count < need_descriptors) {
+			/* Previously allocated array is too small */
+			free(sdhci_ctrlr->adma64_descs);
+			sdhci_ctrlr->adma_desc_count = 0;
+			sdhci_ctrlr->adma64_descs = NULL;
+		}
+	}
+
+	/* use dma_malloc() to make sure we get the coherent/uncached memory */
+	if (!sdhci_ctrlr->adma64_descs) {
+		sdhci_ctrlr->adma64_descs = malloc(need_descriptors
+			* sizeof(*sdhci_ctrlr->adma64_descs));
+		if (sdhci_ctrlr->adma64_descs == NULL)
+			die("fail to malloc adma64_descs\n");
+
+		sdhci_ctrlr->adma_desc_count = need_descriptors;
+	}
+
+	memset(sdhci_ctrlr->adma64_descs, 0, sizeof(*sdhci_ctrlr->adma64_descs)
+		* need_descriptors);
+}
+
+int sdhci_setup_adma(struct sdhci_ctrlr *sdhci_ctrlr, struct mmc_data *data)
+{
+	int i, togo, need_descriptors;
+	int dma64;
+	char *buffer_data;
+	u16 attributes;
+
+	togo = data->blocks * data->blocksize;
+	if (!togo) {
+		sdhc_error("%s: MmcData corrupted: %d blocks of %d bytes\n",
+		       __func__, data->blocks, data->blocksize);
+		return -1;
+	}
+
+	need_descriptors = 1 +  togo / SDHCI_MAX_PER_DESCRIPTOR;
+	dma64 = sdhci_ctrlr->sd_mmc_ctrlr.caps & DRVR_CAP_DMA_64BIT;
+	if (dma64)
+		sdhci_alloc_adma64_descs(sdhci_ctrlr, need_descriptors);
+	else
+		sdhci_alloc_adma_descs(sdhci_ctrlr, need_descriptors);
+	buffer_data = data->dest;
+
+	/* Now set up the descriptor chain. */
+	for (i = 0; togo; i++) {
+		unsigned int desc_length;
+
+		if (togo < SDHCI_MAX_PER_DESCRIPTOR)
+			desc_length = togo;
+		else
+			desc_length = SDHCI_MAX_PER_DESCRIPTOR;
+		togo -= desc_length;
+
+		attributes = SDHCI_ADMA_VALID | SDHCI_ACT_TRAN;
+		if (togo == 0)
+			attributes |= SDHCI_ADMA_END;
+
+		if (dma64) {
+			sdhci_ctrlr->adma64_descs[i].addr =
+				(uintptr_t)buffer_data;
+			sdhci_ctrlr->adma64_descs[i].addr_hi = 0;
+			sdhci_ctrlr->adma64_descs[i].length = desc_length;
+			sdhci_ctrlr->adma64_descs[i].attributes = attributes;
+
+		} else {
+			sdhci_ctrlr->adma_descs[i].addr =
+				(uintptr_t)buffer_data;
+			sdhci_ctrlr->adma_descs[i].length = desc_length;
+			sdhci_ctrlr->adma_descs[i].attributes = attributes;
+		}
+
+		buffer_data += desc_length;
+	}
+
+	if (dma64)
+		sdhci_writel(sdhci_ctrlr, (uintptr_t) sdhci_ctrlr->adma64_descs,
+			     SDHCI_ADMA_ADDRESS);
+	else
+		sdhci_writel(sdhci_ctrlr, (uintptr_t) sdhci_ctrlr->adma_descs,
+			     SDHCI_ADMA_ADDRESS);
+
+	return 0;
+}
+
+int sdhci_complete_adma(struct sdhci_ctrlr *sdhci_ctrlr,
+	struct mmc_command *cmd)
+{
+	int retry;
+	u32 stat = 0, mask;
+
+	mask = SDHCI_INT_RESPONSE | SDHCI_INT_ERROR;
+
+	retry = 10000; /* Command should be done in way less than 10 ms. */
+	while (--retry) {
+		stat = sdhci_readl(sdhci_ctrlr, SDHCI_INT_STATUS);
+		if (stat & mask)
+			break;
+		udelay(1);
+	}
+
+	sdhci_writel(sdhci_ctrlr, SDHCI_INT_RESPONSE, SDHCI_INT_STATUS);
+
+	if (retry && !(stat & SDHCI_INT_ERROR)) {
+		/* Command OK, let's wait for data transfer completion. */
+		mask = SDHCI_INT_DATA_END |
+			SDHCI_INT_ERROR | SDHCI_INT_ADMA_ERROR;
+
+		/* Transfer should take 10 seconds tops. */
+		retry = 10 * 1000 * 1000;
+		while (--retry) {
+			stat = sdhci_readl(sdhci_ctrlr, SDHCI_INT_STATUS);
+			if (stat & mask)
+				break;
+			udelay(1);
+		}
+
+		sdhci_writel(sdhci_ctrlr, stat, SDHCI_INT_STATUS);
+		if (retry && !(stat & SDHCI_INT_ERROR)) {
+			sdhci_cmd_done(sdhci_ctrlr, cmd);
+			return 0;
+		}
+	}
+
+	sdhc_error("%s: transfer error, stat %#x, adma error %#x, retry %d\n",
+	       __func__, stat, sdhci_readl(sdhci_ctrlr, SDHCI_ADMA_ERROR),
+		retry);
+
+	sdhci_reset(sdhci_ctrlr, SDHCI_RESET_CMD);
+	sdhci_reset(sdhci_ctrlr, SDHCI_RESET_DATA);
+
+	if (stat & SDHCI_INT_TIMEOUT)
+		return CARD_TIMEOUT;
+	return CARD_COMM_ERR;
+}
diff --git a/src/commonlib/storage/sdhci_display.c b/src/commonlib/storage/sdhci_display.c
new file mode 100644
index 0000000000..d05e35a8d7
--- /dev/null
+++ b/src/commonlib/storage/sdhci_display.c
@@ -0,0 +1,113 @@
+/*
+ * Copyright 2011, Marvell Semiconductor Inc.
+ * Lei Wen <leiwen@marvell.com>
+ *
+ * Copyright 2017 Intel Corporation
+ *
+ * Secure Digital (SD) Host Controller interface specific code
+ *
+ * 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.
+ */
+
+#include <commonlib/sd_mmc_ctrlr.h>
+#include <commonlib/sdhci.h>
+#include <commonlib/storage.h>
+#include <console/console.h>
+#include "sdhci.h"
+#include "sd_mmc.h"
+#include "storage.h"
+
+static void sdhci_display_bus_width(struct sdhci_ctrlr *sdhci_ctrlr)
+{
+	if (IS_ENABLED(CONFIG_SDHC_DEBUG)) {
+		int bits;
+		uint8_t host_ctrl;
+		uint16_t host2;
+		const char *rate;
+		uint16_t timing;
+
+		/* Display the bus width */
+		host_ctrl = sdhci_readb(sdhci_ctrlr, SDHCI_HOST_CONTROL);
+		host2 = sdhci_readw(sdhci_ctrlr, SDHCI_HOST_CONTROL2);
+		timing = host2 & SDHCI_CTRL_UHS_MASK;
+		bits = 1;
+		if (host_ctrl & SDHCI_CTRL_8BITBUS)
+			bits = 8;
+		else if (host_ctrl & SDHCI_CTRL_4BITBUS)
+			bits = 4;
+		rate = "SDR";
+		if ((timing == SDHCI_CTRL_UHS_DDR50)
+			|| (timing == SDHCI_CTRL_HS400))
+			rate = "DDR";
+		sdhc_debug("SDHCI bus width: %d bit%s %s\n", bits,
+			(bits != 1) ? "s" : "", rate);
+	}
+}
+
+static void sdhci_display_clock(struct sdhci_ctrlr *sdhci_ctrlr)
+{
+	if (IS_ENABLED(CONFIG_SDHC_DEBUG)) {
+		uint16_t clk_ctrl;
+		uint32_t clock;
+		uint32_t divisor;
+
+		/* Display the clock */
+		clk_ctrl = sdhci_readw(sdhci_ctrlr, SDHCI_CLOCK_CONTROL);
+		sdhc_debug("SDHCI bus clock: ");
+		if (clk_ctrl & SDHCI_CLOCK_CARD_EN) {
+			divisor = (clk_ctrl >> SDHCI_DIVIDER_SHIFT)
+				& SDHCI_DIV_MASK;
+			divisor |= ((clk_ctrl >> SDHCI_DIVIDER_SHIFT)
+				<< SDHCI_DIV_MASK_LEN) & SDHCI_DIV_HI_MASK;
+			divisor <<= 1;
+			clock = sdhci_ctrlr->sd_mmc_ctrlr.clock_base;
+			if (divisor)
+				clock /= divisor;
+			sdhc_debug("%d.%03d MHz\n", clock / 1000000,
+				(clock / 1000) % 1000);
+		} else
+			sdhc_debug("Off\n");
+	}
+}
+
+static void sdhci_display_voltage(struct sdhci_ctrlr *sdhci_ctrlr)
+{
+	if (IS_ENABLED(CONFIG_SDHC_DEBUG)) {
+		u8 pwr_ctrl;
+		const char *voltage;
+		const char *voltage_table[8] = {
+			"Unknown",	/* 0 */
+			"Unknown",	/* 1 */
+			"Unknown",	/* 2 */
+			"Unknown",	/* 3 */
+			"Unknown",	/* 4 */
+			"1.8",		/* 5 */
+			"3.0",		/* 6 */
+			"3.3",		/* 7 */
+		};
+
+		pwr_ctrl = sdhci_readb(sdhci_ctrlr, SDHCI_POWER_CONTROL);
+		if (pwr_ctrl & SDHCI_POWER_ON) {
+			voltage = voltage_table[(pwr_ctrl & SDHCI_POWER_330)
+				>> 1];
+			sdhc_debug("SDHCI voltage: %s Volts\n", voltage);
+		} else
+			sdhc_debug("SDHCI voltage: Off\n");
+	}
+}
+
+void sdhci_display_setup(struct sdhci_ctrlr *sdhci_ctrlr)
+{
+	/* Display the controller setup */
+	sdhci_display_voltage(sdhci_ctrlr);
+	sdhci_display_clock(sdhci_ctrlr);
+	sdhci_display_bus_width(sdhci_ctrlr);
+}
diff --git a/src/commonlib/storage/storage.c b/src/commonlib/storage/storage.c
new file mode 100644
index 0000000000..d2b566f71e
--- /dev/null
+++ b/src/commonlib/storage/storage.c
@@ -0,0 +1,358 @@
+/*
+ * Copyright 2008, Freescale Semiconductor, Inc
+ * Andy Fleming
+ *
+ * Copyright 2013 Google Inc.  All rights reserved.
+ * Copyright 2017 Intel Corporation
+ *
+ * MultiMediaCard (MMC), eMMC and Secure Digital (SD) common code which
+ * transitions the card from the standby state to the transfer state.  The
+ * common code supports read operations, erase and write operations are in
+ * a separate modules.  This code is controller independent.
+ *
+ * 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.
+ */
+
+#include <assert.h>
+#include <commonlib/storage.h>
+#include "sd_mmc.h"
+#include "storage.h"
+#include <string.h>
+
+#define DECIMAL_CAPACITY_MULTIPLIER	1000ULL
+#define HEX_CAPACITY_MULTIPLIER		1024ULL
+
+struct capacity {
+	const char * const units;
+	uint64_t bytes;
+};
+
+static void display_capacity(struct storage_media *media, int partition_number)
+{
+	uint64_t capacity;
+	uint64_t decimal_divisor;
+	const char *decimal_units;
+	uint64_t hex_divisor;
+	const char *hex_units;
+	int index;
+	const char *name;
+	const char *separator;
+	const struct capacity decimal_list[] = {
+		{"TB", DECIMAL_CAPACITY_MULTIPLIER * DECIMAL_CAPACITY_MULTIPLIER
+			* DECIMAL_CAPACITY_MULTIPLIER
+			* DECIMAL_CAPACITY_MULTIPLIER},
+		{"GB", DECIMAL_CAPACITY_MULTIPLIER * DECIMAL_CAPACITY_MULTIPLIER
+			* DECIMAL_CAPACITY_MULTIPLIER},
+		{"MB", DECIMAL_CAPACITY_MULTIPLIER
+			* DECIMAL_CAPACITY_MULTIPLIER},
+		{"KB", DECIMAL_CAPACITY_MULTIPLIER},
+		{"B", 1}
+	};
+	const struct capacity hex_list[] = {
+		{"TiB", HEX_CAPACITY_MULTIPLIER * HEX_CAPACITY_MULTIPLIER
+			* HEX_CAPACITY_MULTIPLIER * HEX_CAPACITY_MULTIPLIER},
+		{"GiB", HEX_CAPACITY_MULTIPLIER * HEX_CAPACITY_MULTIPLIER
+			* HEX_CAPACITY_MULTIPLIER},
+		{"MiB", HEX_CAPACITY_MULTIPLIER * HEX_CAPACITY_MULTIPLIER},
+		{"KiB", HEX_CAPACITY_MULTIPLIER},
+		{"B", 1}
+	};
+
+	/* Get the partition name */
+	capacity = media->capacity[partition_number];
+	name = storage_partition_name(media, partition_number);
+	separator = "";
+	if (IS_ENABLED(CONFIG_COMMONLIB_STORAGE_MMC) && !IS_SD(media))
+		separator = ": ";
+
+	/* Determine the decimal divisor for the capacity */
+	for (index = 0; index < ARRAY_SIZE(decimal_list) - 1; index++) {
+		if (capacity >= decimal_list[index].bytes)
+			break;
+	}
+	decimal_divisor = decimal_list[index].bytes;
+	decimal_units = decimal_list[index].units;
+
+	/* Determine the hex divisor for the capacity */
+	for (index = 0; index < ARRAY_SIZE(hex_list) - 1; index++) {
+		if (capacity >= hex_list[index].bytes)
+			break;
+	}
+	hex_divisor = hex_list[index].bytes;
+	hex_units = hex_list[index].units;
+
+	/* Display the capacity */
+	sdhc_debug("%3lld.%03lld %sytes (%3lld.%03lld %sytes)%s%s\n",
+		capacity / decimal_divisor,
+		(capacity / (decimal_divisor / 1000)) % 1000,
+		decimal_units,
+		capacity / hex_divisor,
+		((capacity / (hex_divisor / 1024)) * 1000 / 1024) % 1000,
+		hex_units,
+		separator,
+		name);
+}
+
+void storage_display_setup(struct storage_media *media)
+{
+	int partition_number;
+
+	/* Display the device info */
+	sd_mmc_debug("Man %06x Snr %u ",
+	       media->cid[0] >> 24,
+	       (((media->cid[2] & 0xffff) << 16) |
+		((media->cid[3] >> 16) & 0xffff)));
+	sd_mmc_debug("Product %c%c%c%c", media->cid[0] & 0xff,
+	       (media->cid[1] >> 24), (media->cid[1] >> 16) & 0xff,
+	       (media->cid[1] >> 8) & 0xff);
+	if (!IS_SD(media)) /* eMMC product string is longer */
+		sd_mmc_debug("%c%c", media->cid[1] & 0xff,
+		       (media->cid[2] >> 24) & 0xff);
+	sd_mmc_debug(" Revision %d.%d\n", (media->cid[2] >> 20) & 0xf,
+	       (media->cid[2] >> 16) & 0xf);
+
+	/* Display the erase block size */
+	sdhc_debug("Erase block size: 0x%08x\n", media->erase_blocks
+		* media->write_bl_len);
+
+	/* Display the partition capacities */
+	if (IS_ENABLED(CONFIG_SDHC_DEBUG)) {
+		for (partition_number = 0; partition_number
+			< ARRAY_SIZE(media->capacity); partition_number++) {
+			if (!media->capacity[partition_number])
+				continue;
+			display_capacity(media, partition_number);
+		}
+	}
+}
+
+int storage_startup(struct storage_media *media)
+{
+	int err;
+	uint64_t capacity;
+	uint64_t cmult, csize;
+	struct mmc_command cmd;
+	struct sd_mmc_ctrlr *ctrlr = media->ctrlr;
+
+	/* Determine the storage capacity */
+	if (media->high_capacity) {
+		cmult = 8;
+		csize = sd_mmc_extract_uint32_bits(media->csd, 58, 22);
+	} else {
+		csize = sd_mmc_extract_uint32_bits(media->csd, 54, 12);
+		cmult = sd_mmc_extract_uint32_bits(media->csd, 78, 3);
+	}
+	capacity = (csize + 1) << (cmult + 2);
+	capacity *= media->read_bl_len;
+	media->capacity[0] = capacity;
+
+	/* Limit the block size to 512 bytes */
+	if (media->read_bl_len > 512)
+		media->read_bl_len = 512;
+	if (media->write_bl_len > 512)
+		media->write_bl_len = 512;
+
+	/* Get the erase size in blocks */
+	media->erase_blocks =
+		(sd_mmc_extract_uint32_bits(media->csd, 47, 3) + 1)
+		* (sd_mmc_extract_uint32_bits(media->csd, 42, 5) + 1);
+
+	/* Select the card, and put it into Transfer Mode */
+	cmd.cmdidx = MMC_CMD_SELECT_CARD;
+	cmd.resp_type = CARD_RSP_R1;
+	cmd.cmdarg = media->rca << 16;
+	cmd.flags = 0;
+	err = ctrlr->send_cmd(ctrlr, &cmd, NULL);
+	if (err)
+		return err;
+
+	/* Increase the bus frequency */
+	if (IS_ENABLED(CONFIG_COMMONLIB_STORAGE_SD) && IS_SD(media))
+		err = sd_change_freq(media);
+	else if (IS_ENABLED(CONFIG_COMMONLIB_STORAGE_MMC)) {
+		err = mmc_change_freq(media);
+		if (!err)
+			mmc_update_capacity(media);
+	}
+	if (err)
+		return err;
+
+	/* Restrict card's capabilities by what the controller can do */
+	media->caps &= ctrlr->caps;
+
+	/* Increase the bus width if possible */
+	if (IS_ENABLED(CONFIG_COMMONLIB_STORAGE_SD) && IS_SD(media))
+		err = sd_set_bus_width(media);
+	else if (IS_ENABLED(CONFIG_COMMONLIB_STORAGE_MMC))
+		err = mmc_set_bus_width(media);
+	if (err)
+		return err;
+
+	/* Display the card setup */
+	storage_display_setup(media);
+	return 0;
+}
+
+int storage_setup_media(struct storage_media *media, struct sd_mmc_ctrlr *ctrlr)
+{
+	int err;
+
+	memset(media, 0, sizeof(*media));
+	media->ctrlr = ctrlr;
+
+	err = sd_mmc_enter_standby(media);
+	if (err)
+		return err;
+	return storage_startup(media);
+}
+
+static int storage_read(struct storage_media *media, void *dest, uint32_t start,
+	uint32_t block_count)
+{
+	struct mmc_command cmd;
+	struct sd_mmc_ctrlr *ctrlr = media->ctrlr;
+
+	cmd.resp_type = CARD_RSP_R1;
+	cmd.flags = 0;
+
+	if (block_count > 1)
+		cmd.cmdidx = MMC_CMD_READ_MULTIPLE_BLOCK;
+	else
+		cmd.cmdidx = MMC_CMD_READ_SINGLE_BLOCK;
+
+	if (media->high_capacity)
+		cmd.cmdarg = start;
+	else
+		cmd.cmdarg = start * media->read_bl_len;
+
+	struct mmc_data data;
+	data.dest = dest;
+	data.blocks = block_count;
+	data.blocksize = media->read_bl_len;
+	data.flags = DATA_FLAG_READ;
+
+	if (ctrlr->send_cmd(ctrlr, &cmd, &data))
+		return 0;
+
+	if ((block_count > 1) && !(ctrlr->caps
+		& DRVR_CAP_AUTO_CMD12)) {
+		cmd.cmdidx = MMC_CMD_STOP_TRANSMISSION;
+		cmd.cmdarg = 0;
+		cmd.resp_type = CARD_RSP_R1b;
+		cmd.flags = CMD_FLAG_IGNORE_INHIBIT;
+		if (ctrlr->send_cmd(ctrlr, &cmd, NULL)) {
+			sd_mmc_error("Failed to send stop cmd\n");
+			return 0;
+		}
+
+		/* Waiting for the ready status */
+		sd_mmc_send_status(media, SD_MMC_IO_RETRIES);
+	}
+
+	return block_count;
+}
+
+/////////////////////////////////////////////////////////////////////////////
+// BlockDevice utilities and callbacks
+
+int storage_block_setup(struct storage_media *media, uint64_t start,
+	uint64_t count, int is_read)
+{
+	struct sd_mmc_ctrlr *ctrlr = media->ctrlr;
+	int partition_number;
+
+	if (count == 0)
+		return 0;
+
+	uint32_t bl_len = is_read ? media->read_bl_len :
+		media->write_bl_len;
+
+	/* Validate the block range */
+	partition_number = media->partition_config & EXT_CSD_PART_ACCESS_MASK;
+	if (((start * bl_len) > media->capacity[partition_number])
+		|| (((start + count) * bl_len) >
+		media->capacity[partition_number])) {
+		sd_mmc_error("Block range exceeds device capacity\n");
+		return 0;
+	}
+
+	/*
+	 * CMD16 only applies to single data rate mode, and block
+	 * length for double data rate is always 512 bytes.
+	 */
+	if ((ctrlr->timing == BUS_TIMING_UHS_DDR50) ||
+	    (ctrlr->timing == BUS_TIMING_MMC_DDR52) ||
+	    (ctrlr->timing == BUS_TIMING_MMC_HS400) ||
+	    (ctrlr->timing == BUS_TIMING_MMC_HS400ES))
+		return 1;
+	if (sd_mmc_set_blocklen(ctrlr, bl_len))
+		return 0;
+
+	return 1;
+}
+
+uint64_t storage_block_read(struct storage_media *media, uint64_t start,
+	uint64_t count, void *buffer)
+{
+	uint8_t *dest = (uint8_t *)buffer;
+
+	if (storage_block_setup(media, start, count, 1) == 0)
+		return 0;
+
+	uint64_t todo = count;
+	struct sd_mmc_ctrlr *ctrlr = media->ctrlr;
+	do {
+		uint32_t cur = (uint32_t)MIN(todo, ctrlr->b_max);
+		if (storage_read(media, dest, start, cur) != cur)
+			return 0;
+		todo -= cur;
+		sd_mmc_trace("%s: Got %d blocks, more %d (total %d) to go.\n",
+			  __func__, (int)cur, (int)todo, (int)count);
+		start += cur;
+		dest += cur * media->read_bl_len;
+	} while (todo > 0);
+	return count;
+}
+
+int storage_set_partition(struct storage_media *media,
+	unsigned int partition_number)
+{
+	int err;
+
+	/* Select the partition */
+	err = -1;
+	if (IS_ENABLED(CONFIG_COMMONLIB_STORAGE_SD) && IS_SD(media))
+		err = sd_set_partition(media, partition_number);
+	else if (IS_ENABLED(CONFIG_COMMONLIB_STORAGE_MMC))
+		err = mmc_set_partition(media, partition_number);
+	if (err)
+		sd_mmc_error("Invalid partition number!\n");
+	return err;
+}
+
+const char *storage_partition_name(struct storage_media *media,
+	unsigned int partition_number)
+{
+	const char *name;
+
+	/* Get the partition name */
+	name = NULL;
+	if (IS_ENABLED(CONFIG_COMMONLIB_STORAGE_SD) && IS_SD(media))
+		name = sd_partition_name(media, partition_number);
+	else if (IS_ENABLED(CONFIG_COMMONLIB_STORAGE_MMC))
+		name = mmc_partition_name(media, partition_number);
+	return name;
+}
+
+unsigned int storage_get_current_partition(struct storage_media *media)
+{
+	return media->partition_config & EXT_CSD_PART_ACCESS_MASK;
+}
diff --git a/src/commonlib/storage/storage.h b/src/commonlib/storage/storage.h
new file mode 100644
index 0000000000..645b6c1abd
--- /dev/null
+++ b/src/commonlib/storage/storage.h
@@ -0,0 +1,37 @@
+/*
+ * Copyright 2017 Intel Corporation
+ *
+ * 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.
+ */
+
+#ifndef __COMMONLIB_STORAGE_STORAGE_H__
+#define __COMMONLIB_STORAGE_STORAGE_H__
+
+#include <stdint.h>
+#include <commonlib/storage.h>
+
+#define DMA_MINALIGN (64)
+#define ROUND(a, b) (((a) + (b) - 1) & ~((b) - 1))
+#define ALLOC_CACHE_ALIGN_BUFFER(type, name, size)                   \
+	char __##name[ROUND(size * sizeof(type), DMA_MINALIGN) +     \
+		      DMA_MINALIGN - 1];                             \
+	type *name = (type *) ALIGN((uintptr_t)__##name, DMA_MINALIGN)
+
+/* NOOPs mirroring ARM's cache API, since x86 devices usually cache snoop */
+#define dcache_invalidate_by_mva(addr, len)
+#define dcache_clean_invalidate_by_mva(addr, len)
+
+/* Storage support routines */
+int storage_startup(struct storage_media *media);
+int storage_block_setup(struct storage_media *media, uint64_t start,
+	uint64_t count, int is_read);
+
+#endif /* __COMMONLIB_STORAGE_STORAGE_H__ */
diff --git a/src/commonlib/storage/storage_erase.c b/src/commonlib/storage/storage_erase.c
new file mode 100644
index 0000000000..004a200154
--- /dev/null
+++ b/src/commonlib/storage/storage_erase.c
@@ -0,0 +1,95 @@
+/*
+ * Copyright 2008, Freescale Semiconductor, Inc
+ * Andy Fleming
+ *
+ * Copyright 2013 Google Inc.  All rights reserved.
+ * Copyright 2017 Intel Corporation
+ *
+ * MultiMediaCard (MMC), eMMC and Secure Digital (SD) erase support code.
+ * This code is controller independent.
+ *
+ * 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.
+ */
+
+#include <console/console.h>
+#include "sd_mmc.h"
+#include "storage.h"
+
+uint64_t storage_block_erase(struct storage_media *media, uint64_t start,
+	uint64_t count)
+{
+	struct mmc_command cmd;
+	struct sd_mmc_ctrlr *ctrlr = media->ctrlr;
+
+	if (storage_block_setup(media, start, count, 0) == 0)
+		return 0;
+
+	cmd.cmdidx = MMC_CMD_ERASE_GROUP_START;
+	cmd.resp_type = CARD_RSP_R1;
+	cmd.cmdarg = start;
+	cmd.flags = 0;
+
+	if (ctrlr->send_cmd(ctrlr, &cmd, NULL))
+		return 0;
+
+	cmd.cmdidx = MMC_CMD_ERASE_GROUP_END;
+	cmd.cmdarg = start + count - 1;
+	cmd.resp_type = CARD_RSP_R1;
+	cmd.flags = 0;
+
+	if (ctrlr->send_cmd(ctrlr, &cmd, NULL))
+		return 0;
+
+	cmd.cmdidx = MMC_CMD_ERASE;
+	cmd.cmdarg = MMC_TRIM_ARG;	/* just unmap blocks */
+	cmd.resp_type = CARD_RSP_R1;
+	cmd.flags = 0;
+
+	if (ctrlr->send_cmd(ctrlr, &cmd, NULL))
+		return 0;
+
+	size_t erase_blocks;
+	/*
+	 * Timeout for TRIM operation on one erase group is defined as:
+	 * TRIM timeout = 300ms x TRIM_MULT
+	 *
+	 * This timeout is expressed in units of 100us to sd_mmc_send_status.
+	 *
+	 * Hence, timeout_per_erase_block = TRIM timeout * 1000us/100us;
+	 */
+	size_t timeout_per_erase_block = (media->trim_mult * 300) * 10;
+	int err = 0;
+
+	erase_blocks = ALIGN_UP(count, media->erase_blocks)
+		/ media->erase_blocks;
+
+	while (erase_blocks) {
+		/*
+		 * To avoid overflow of timeout value, loop in calls to
+		 * sd_mmc_send_status for erase_blocks number of times.
+		 */
+		err = sd_mmc_send_status(media, timeout_per_erase_block);
+
+		/* Send status successful, erase action complete. */
+		if (err == 0)
+			break;
+
+		erase_blocks--;
+	}
+
+	/* Total timeout done. Still status not successful. */
+	if (err) {
+		sd_mmc_error("TRIM operation not successful within timeout.\n");
+		return 0;
+	}
+
+	return count;
+}
diff --git a/src/commonlib/storage/storage_write.c b/src/commonlib/storage/storage_write.c
new file mode 100644
index 0000000000..ae9fbc2c5e
--- /dev/null
+++ b/src/commonlib/storage/storage_write.c
@@ -0,0 +1,154 @@
+/*
+ * Copyright 2008, Freescale Semiconductor, Inc
+ * Andy Fleming
+ *
+ * Copyright 2013 Google Inc.  All rights reserved.
+ * Copyright 2017 Intel Corporation
+ *
+ * MultiMediaCard (MMC), eMMC and Secure Digital (SD) write support code.
+ * This code is controller independent.
+ *
+ * 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.
+ */
+
+#include <console/console.h>
+#include "sd_mmc.h"
+#include "storage.h"
+#include <string.h>
+
+static uint32_t storage_write(struct storage_media *media, uint32_t start,
+	uint64_t block_count, const void *src)
+{
+	struct mmc_command cmd;
+	struct sd_mmc_ctrlr *ctrlr = media->ctrlr;
+
+	cmd.resp_type = CARD_RSP_R1;
+	cmd.flags = 0;
+
+	if (block_count > 1)
+		cmd.cmdidx = MMC_CMD_WRITE_MULTIPLE_BLOCK;
+	else
+		cmd.cmdidx = MMC_CMD_WRITE_SINGLE_BLOCK;
+
+	if (media->high_capacity)
+		cmd.cmdarg = start;
+	else
+		cmd.cmdarg = start * media->write_bl_len;
+
+	struct mmc_data data;
+	data.src = src;
+	data.blocks = block_count;
+	data.blocksize = media->write_bl_len;
+	data.flags = DATA_FLAG_WRITE;
+
+	if (ctrlr->send_cmd(ctrlr, &cmd, &data)) {
+		sd_mmc_error("Write failed\n");
+		return 0;
+	}
+
+	/* SPI multiblock writes terminate using a special
+	 * token, not a STOP_TRANSMISSION request.
+	 */
+	if ((block_count > 1) && !(ctrlr->caps
+		& DRVR_CAP_AUTO_CMD12)) {
+		cmd.cmdidx = MMC_CMD_STOP_TRANSMISSION;
+		cmd.cmdarg = 0;
+		cmd.resp_type = CARD_RSP_R1b;
+		cmd.flags = CMD_FLAG_IGNORE_INHIBIT;
+		if (ctrlr->send_cmd(ctrlr, &cmd, NULL)) {
+			sd_mmc_error("Failed to send stop cmd\n");
+			return 0;
+		}
+
+		/* Waiting for the ready status */
+		sd_mmc_send_status(media, SD_MMC_IO_RETRIES);
+	}
+
+	return block_count;
+}
+
+uint64_t storage_block_write(struct storage_media *media, uint64_t start,
+	uint64_t count, const void *buffer)
+{
+	const uint8_t *src = (const uint8_t *)buffer;
+
+	if (storage_block_setup(media, start, count, 0) == 0)
+		return 0;
+
+	uint64_t todo = count;
+	struct sd_mmc_ctrlr *ctrlr = media->ctrlr;
+	do {
+		uint64_t cur = MIN(todo, ctrlr->b_max);
+		if (storage_write(media, start, cur, src) != cur)
+			return 0;
+		todo -= cur;
+		start += cur;
+		src += cur * media->write_bl_len;
+	} while (todo > 0);
+	return count;
+}
+
+uint64_t storage_block_fill_write(struct storage_media *media, uint64_t start,
+	uint64_t count, uint32_t fill_pattern)
+{
+	if (storage_block_setup(media, start, count, 0) == 0)
+		return 0;
+
+	struct sd_mmc_ctrlr *ctrlr = media->ctrlr;
+	uint64_t block_size = media->write_bl_len;
+	/*
+	 * We allocate max 4 MiB buffer on heap and set it to fill_pattern and
+	 * perform mmc_write operation using this 4MiB buffer until requested
+	 * size on disk is written by the fill byte.
+	 *
+	 * 4MiB was chosen after repeating several experiments with the max
+	 * buffer size to be used. Using 1 lba i.e. block_size buffer results in
+	 * very large fill_write time. On the other hand, choosing 4MiB, 8MiB or
+	 * even 128 Mib resulted in similar write times. With 2MiB, the
+	 * fill_write time increased by several seconds. So, 4MiB was chosen as
+	 * the default max buffer size.
+	 */
+	uint64_t heap_lba = (4 * MiB) / block_size;
+	/*
+	 * Actual allocated buffer size is minimum of three entities:
+	 * 1) 4MiB equivalent in lba
+	 * 2) count: Number of lbas to overwrite
+	 * 3) ctrlr->b_max: Max lbas that the block device allows write
+	 * operation on at a time.
+	 */
+	uint64_t buffer_lba = MIN(MIN(heap_lba, count), ctrlr->b_max);
+
+	uint64_t buffer_bytes = buffer_lba * block_size;
+	uint64_t buffer_words = buffer_bytes / sizeof(uint32_t);
+	uint32_t *buffer = malloc(buffer_bytes);
+	uint32_t *ptr = buffer;
+
+	for ( ; buffer_words ; buffer_words--)
+		*ptr++ = fill_pattern;
+
+	uint64_t todo = count;
+	int ret = 0;
+
+	do {
+		uint64_t curr_lba = MIN(buffer_lba, todo);
+
+		if (storage_write(media, start, curr_lba, buffer) != curr_lba)
+			goto cleanup;
+		todo -= curr_lba;
+		start += curr_lba;
+	} while (todo > 0);
+
+	ret = count;
+
+cleanup:
+	free(buffer);
+	return ret;
+}