libpayload: New AHCI, ATA and ATAPI drivers

This adds a new interface for storage devices. A driver for ATA and
ATAPI drives on AHCI host controllers comes along.

The interface is very simple and was designed to match FILO's needs.
It consists of three functions:

  void storage_initialize(void);
  Initializes controllers. Should be called once at startup.

  storage_poll_t storage_probe(size_t dev_num);
     with typedef enum {
            POLL_NO_DEVICE      = -2,
            POLL_ERROR          = -1,
            POLL_NO_MEDIUM      =  0,
            POLL_MEDIUM_PRESENT =  1,
          } storage_poll_t;
  Looks for a drive with number dev_num (drives are counted from
  zero) and polls for a medium in the drive if appropriate.

  int storage_read_blocks512(size_t dev_num,
                             u64 start, size_t count,
                             unsigned char *buf);
  Reads count blocks of 512 bytes from block start of drive dev_num
  into buf.

Change-Id: I1c85796b7f8e379ff3817a61b1837636b57e182b
Signed-off-by: Nico Huber <nico.huber@secunet.com>
Reviewed-on: http://review.coreboot.org/1622
Tested-by: build bot (Jenkins)
Reviewed-by: Stefan Reinauer <stefan.reinauer@coreboot.org>
Reviewed-by: Peter Stuge <peter@stuge.se>

6 files changed, 1297 insertions, 0 deletions

diff --git a/payloads/libpayload/drivers/Makefile.inc b/payloads/libpayload/drivers/Makefile.inc
index 360a258948..80c9a5b075 100644
--- a/payloads/libpayload/drivers/Makefile.inc
+++ b/payloads/libpayload/drivers/Makefile.inc
@@ -52,6 +52,11 @@ libc-$(CONFIG_GEODELX_VIDEO_CONSOLE) += video/font8x16.c
 libc-$(CONFIG_COREBOOT_VIDEO_CONSOLE) += video/corebootfb.c
 libc-$(CONFIG_COREBOOT_VIDEO_CONSOLE) += video/font8x16.c
 
+libc-$(CONFIG_STORAGE) += storage/storage.c
+libc-$(CONFIG_STORAGE_ATA) += storage/ata.c
+libc-$(CONFIG_STORAGE_ATAPI) += storage/atapi.c
+libc-$(CONFIG_STORAGE_AHCI) += storage/ahci.c
+
 # USB stack
 libc-$(CONFIG_USB) += usb/usbinit.c
 libc-$(CONFIG_USB) += usb/usb.c
diff --git a/payloads/libpayload/drivers/storage/ahci.c b/payloads/libpayload/drivers/storage/ahci.c
new file mode 100644
index 0000000000..b33cc708b6
--- /dev/null
+++ b/payloads/libpayload/drivers/storage/ahci.c
@@ -0,0 +1,541 @@
+/*
+ * This file is part of the libpayload project.
+ *
+ * Copyright (C) 2012 secunet Security Networks AG
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ * 3. The name of the author may not be used to endorse or promote products
+ *    derived from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
+ * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
+ * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
+ * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
+ * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
+ * SUCH DAMAGE.
+ */
+
+//#define DEBUG_STATUS
+
+#include <stdlib.h>
+#include <stdint.h>
+#include <string.h>
+#include <libpayload.h>
+#include <pci.h>
+#include <storage/ata.h>
+#include <storage/ahci.h>
+
+#include "ahci_private.h"
+
+
+#ifdef DEBUG_STATUS
+static inline u32 _ahci_clear_status(volatile u32 *const reg,
+				     const char *const r,
+				     const char *const f)
+{
+	const u32 bits = *reg;
+	if (bits)
+		*reg = bits;
+	printf("ahci: %s: %s == 0x%08x\n", f, r, bits);
+
+	return bits;
+}
+#define ahci_clear_status(p, r) _ahci_clear_status(&(p)->r, #r, __func__)
+#else
+static inline u32 _ahci_clear_status(volatile u32 *const reg)
+{
+	const u32 bits = *reg;
+	if (bits)
+		*reg = bits;
+	return bits;
+}
+#define ahci_clear_status(p, r) _ahci_clear_status(&(p)->r)
+#endif
+
+
+static inline int ahci_port_is_active(const hba_port_t *const port)
+{
+	return (port->sata_status & (HBA_PxSSTS_IPM_MASK | HBA_PxSSTS_DET_MASK))
+		== (HBA_PxSSTS_IPM_ACTIVE | HBA_PxSSTS_DET_ESTABLISHED);
+}
+
+static int ahci_cmdengine_start(hba_port_t *const port)
+{
+	/* Wait for the controller to clear CR.
+	   This shouldn't take too long, but we should time out nevertheless. */
+	int timeout = 1000; /* Time out after 1000 * 1us == 1ms. */
+	while ((port->cmd_stat & HBA_PxCMD_CR) && timeout--)
+		udelay(1);
+	if (timeout < 0) {
+		printf("ahci: Timeout during start of command engine.\n");
+		return 1;
+	}
+
+	port->cmd_stat |= HBA_PxCMD_FRE;
+	port->cmd_stat |= HBA_PxCMD_ST;
+	return 0;
+}
+
+static int ahci_cmdengine_stop(hba_port_t *const port)
+{
+	port->cmd_stat &= ~HBA_PxCMD_ST;
+
+	/* Wait for the controller to clear FR and CR.
+	   This shouldn't take too long, but we should time out nevertheless. */
+	int timeout = 1000; /* Time out after 1000 * 1us == 1ms. */
+	while ((port->cmd_stat & (HBA_PxCMD_FR | HBA_PxCMD_CR)) && timeout--)
+		udelay(1);
+	if (timeout < 0) {
+		printf("ahci: Timeout during stopping of command engine.\n");
+		return 1;
+	}
+
+	port->cmd_stat &= ~HBA_PxCMD_FRE;
+	return 0;
+}
+
+/** Do minimal error recovery. */
+static int ahci_error_recovery(ahci_dev_t *const dev, const u32 intr_status)
+{
+	/* Command engine has to be restarted.
+	   We don't call ahci_cmdengine_stop() here as it also checks
+	   HBA_PxCMD_FR which won't clear on fatal errors. */
+	dev->port->cmd_stat &= ~HBA_PxCMD_ST;
+
+	/* Always clear sata_error. */
+	ahci_clear_status(dev->port, sata_error);
+
+	/* Perform COMRESET if appropriate. */
+	const u32 tfd = dev->port->taskfile_data;
+	if ((tfd & (HBA_PxTFD_BSY | HBA_PxTFD_DRQ)) |
+			(intr_status & HBA_PxIS_PCS)) {
+		const u32 sctl = dev->port->sata_control & ~HBA_PxSCTL_DET_MASK;
+		dev->port->sata_control = sctl | HBA_PxSCTL_DET_COMRESET;
+		mdelay(1);
+		dev->port->sata_control = sctl;
+	}
+
+	if (ahci_port_is_active(dev->port))
+		/* Start command engine. */
+		return ahci_cmdengine_start(dev->port);
+	else
+		return -1;
+}
+
+/** Give a buffer with even address. */
+static u8 *ahci_prdbuf_init(ahci_dev_t *const dev,
+			    u8 *const user_buf, const size_t len,
+			    const int out)
+{
+	if ((u32)user_buf & 1) {
+		printf("ahci: Odd buffer pointer (%p).\n", user_buf);
+		if (dev->buf) /* orphaned buffer */
+			free((void *)dev->buf - *(dev->buf - 1));
+		dev->buf = malloc(len + 2);
+		if (!dev->buf)
+			return NULL;
+		dev->user_buf = user_buf;
+		dev->write_back = !out;
+		dev->buflen = len;
+		if ((u32)dev->buf & 1) {
+			dev->buf[0] = 1;
+			dev->buf += 1;
+		} else {
+			dev->buf[0] = 1;
+			dev->buf[1] = 2;
+			dev->buf += 2;
+		}
+		if (out)
+			memcpy(dev->buf, user_buf, len);
+		return dev->buf;
+	} else {
+		return user_buf;
+	}
+}
+
+static void ahci_prdbuf_finalize(ahci_dev_t *const dev)
+{
+	if (dev->buf) {
+		if (dev->write_back)
+			memcpy(dev->user_buf, dev->buf, dev->buflen);
+		free((void *)dev->buf - *(dev->buf - 1));
+	}
+	dev->buf = NULL;
+	dev->user_buf = NULL;
+	dev->write_back = 0;
+	dev->buflen = 0;
+}
+
+static ssize_t ahci_cmdslot_exec(ahci_dev_t *const dev)
+{
+	const int slotnum = 0; /* We always use the first slot. */
+
+	if (!(dev->port->cmd_stat & HBA_PxCMD_CR))
+		return -1;
+
+	/* Trigger command execution. */
+	dev->port->cmd_issue |= (1 << slotnum);
+
+	/* Wait for the controller to finish command execution. */
+	int timeout = 50000; /* Time out after 50000 * 100us == 5s. */
+	while ((dev->port->cmd_issue & (1 << slotnum)) &&
+			!(dev->port->intr_status & HBA_PxIS_TFES) &&
+			timeout--)
+		udelay(100);
+	if (timeout < 0) {
+		printf("ahci: Timeout during command execution.\n");
+		return -1;
+	}
+
+	ahci_prdbuf_finalize(dev);
+
+	const u32 intr_status = ahci_clear_status(dev->port, intr_status);
+	if (intr_status & (HBA_PxIS_FATAL | HBA_PxIS_PCS)) {
+		ahci_error_recovery(dev, intr_status);
+		return -1;
+	} else {
+		return dev->cmdlist[slotnum].prd_bytes;
+	}
+}
+
+static size_t ahci_cmdslot_prepare(ahci_dev_t *const dev,
+				   u8 *const user_buf, size_t buf_len,
+				   const int out)
+{
+	const int slotnum = 0; /* We always use the first slot. */
+
+	size_t read_count = 0;
+
+	memset((void *)&dev->cmdlist[slotnum],
+			'\0', sizeof(dev->cmdlist[slotnum]));
+	memset((void *)dev->cmdtable,
+			'\0', sizeof(*dev->cmdtable));
+	dev->cmdlist[slotnum].cmd = CMD_CFL(FIS_H2D_FIS_LEN);
+	dev->cmdlist[slotnum].cmdtable_base = virt_to_phys(dev->cmdtable);
+
+	if (buf_len > 0) {
+		size_t prdt_len;
+		u8 *buf;
+		int i;
+
+		prdt_len = ((buf_len - 1) >> BYTES_PER_PRD_SHIFT) + 1;
+		const size_t max_prdt_len = ARRAY_SIZE(dev->cmdtable->prdt);
+		if (prdt_len > max_prdt_len) {
+			prdt_len = max_prdt_len;
+			buf_len = prdt_len << BYTES_PER_PRD_SHIFT;
+		}
+
+		dev->cmdlist[slotnum].prdt_length = prdt_len;
+		read_count = buf_len;
+
+		buf = ahci_prdbuf_init(dev, user_buf, buf_len, out);
+		if (!buf)
+			return 0;
+		for (i = 0; i < prdt_len; ++i) {
+			const size_t bytes =
+				(buf_len < BYTES_PER_PRD)
+				? buf_len : BYTES_PER_PRD;
+			dev->cmdtable->prdt[i].data_base = virt_to_phys(buf);
+			dev->cmdtable->prdt[i].flags = PRD_TABLE_BYTES(bytes);
+			buf_len -= bytes;
+			buf += bytes;
+		}
+	}
+
+	return read_count;
+}
+
+static ssize_t ahci_ata_read_sectors(ata_dev_t *const ata_dev,
+				     const lba_t start, size_t count,
+				     u8 *const buf)
+{
+	ahci_dev_t *const dev = (ahci_dev_t *)ata_dev;
+
+	if (count == 0)
+		return 0;
+
+	if (ata_dev->read_cmd == ATA_READ_DMA) {
+		if (start >= (1 << 28)) {
+		       printf("ahci: Sector is not 28-bit addressable.\n");
+		       return -1;
+		} else if (count > 256) {
+		       printf("ahci: Sector count too high (max. 256).\n");
+		       count = 256;
+		}
+#ifdef CONFIG_STORAGE_64BIT_LBA
+	} else if (ata_dev->read_cmd == ATA_READ_DMA_EXT) {
+		if (start >= (1ULL << 48)) {
+			printf("ahci: Sector is not 48-bit addressable.\n");
+			return -1;
+		} else if (count > (64 * 1024)) {
+		       printf("ahci: Sector count too high (max. 65536).\n");
+		       count = 64 * 1024;
+		}
+#endif
+	} else {
+		printf("ahci: Unsupported ATA read command (0x%x).\n",
+			ata_dev->read_cmd);
+		return -1;
+	}
+
+	const size_t bytes = count << ata_dev->sector_size_shift;
+	const size_t bytes_feasible = ahci_cmdslot_prepare(dev, buf, bytes, 0);
+	const size_t sectors = bytes_feasible >> ata_dev->sector_size_shift;
+
+	dev->cmdtable->fis[ 0] = FIS_HOST_TO_DEVICE;
+	dev->cmdtable->fis[ 1] = FIS_H2D_CMD;
+	dev->cmdtable->fis[ 2] = ata_dev->read_cmd;
+	dev->cmdtable->fis[ 4] = (start >>  0) & 0xff;
+	dev->cmdtable->fis[ 5] = (start >>  8) & 0xff;
+	dev->cmdtable->fis[ 6] = (start >> 16) & 0xff;
+	dev->cmdtable->fis[ 7] = FIS_H2D_DEV_LBA;
+	dev->cmdtable->fis[ 8] = (start >> 24) & 0xff;
+#ifdef CONFIG_STORAGE_64BIT_LBA
+	if (ata_dev->read_cmd == ATA_READ_DMA_EXT) {
+		dev->cmdtable->fis[ 9] = (start >> 32) & 0xff;
+		dev->cmdtable->fis[10] = (start >> 40) & 0xff;
+	}
+#endif
+	dev->cmdtable->fis[12] = (sectors >>  0) & 0xff;
+	dev->cmdtable->fis[13] = (sectors >>  8) & 0xff;
+
+	if (ahci_cmdslot_exec(dev) < 0)
+		return -1;
+	else
+		return dev->cmdlist->prd_bytes >> ata_dev->sector_size_shift;
+}
+
+static ssize_t ahci_packet_read_cmd(atapi_dev_t *const _dev,
+				    const u8 *const cmd, const size_t cmdlen,
+				    u8 *const buf, const size_t buflen)
+{
+	ahci_dev_t *const dev = (ahci_dev_t *)_dev;
+
+	if ((cmdlen != 12) && (cmdlen != 16)) {
+		printf("ahci: Only 12- and 16-byte packet commands allowed.\n");
+		return -1;
+	}
+
+	const size_t len = ahci_cmdslot_prepare(dev, buf, buflen, 0);
+	u16 byte_limit = MIN(len, 63 * 1024); /* like Linux */
+	if (byte_limit & 1) ++byte_limit; /* even limit */
+
+	dev->cmdlist[0].cmd |= CMD_ATAPI;
+	dev->cmdtable->fis[0] = FIS_HOST_TO_DEVICE;
+	dev->cmdtable->fis[1] = FIS_H2D_CMD;
+	dev->cmdtable->fis[2] = ATA_PACKET;
+	dev->cmdtable->fis[5] = byte_limit & 0xff;
+	dev->cmdtable->fis[6] = byte_limit >> 8;
+	memcpy((void *)dev->cmdtable->atapi_cmd, cmd, cmdlen);
+
+	return ahci_cmdslot_exec(dev);
+}
+
+static int ahci_identify_device(ata_dev_t *const ata_dev, u8 *const buf)
+{
+	ahci_dev_t *const dev = (ahci_dev_t *)ata_dev;
+
+	ahci_cmdslot_prepare(dev, buf, 512, 0);
+
+	dev->cmdtable->fis[0] = FIS_HOST_TO_DEVICE;
+	dev->cmdtable->fis[1] = FIS_H2D_CMD;
+	dev->cmdtable->fis[2] = ata_dev->identify_cmd;
+
+	if ((ahci_cmdslot_exec(dev) < 0) || (dev->cmdlist->prd_bytes != 512))
+		return -1;
+	else
+		return 0;
+}
+
+static int ahci_dev_init(hba_ctrl_t *const ctrl,
+			 hba_port_t *const port,
+			 const int portnum)
+{
+	int ret = 1;
+
+	const int ncs = HBA_CAPS_DECODE_NCS(ctrl->caps);
+
+	/* Allocate command list, one command table and received FIS. */
+	cmd_t *const cmdlist = memalign(1024, ncs * sizeof(cmd_t));
+	cmdtable_t *const cmdtable = memalign(128, sizeof(cmdtable_t));
+	rcvd_fis_t *const rcvd_fis = memalign(256, sizeof(rcvd_fis_t));
+	/* Allocate our device structure. */
+	ahci_dev_t *const dev = calloc(1, sizeof(ahci_dev_t));
+	if (!cmdlist || !cmdtable || !rcvd_fis || !dev)
+		goto _cleanup_ret;
+	memset((void *)cmdlist, '\0', ncs * sizeof(cmd_t));
+	memset((void *)cmdtable, '\0', sizeof(*cmdtable));
+	memset((void *)rcvd_fis, '\0', sizeof(*rcvd_fis));
+
+	/* Set command list base and received FIS base. */
+	if (ahci_cmdengine_stop(port))
+		return 1;
+	port->cmdlist_base = virt_to_phys(cmdlist);
+	port->frameinfo_base = virt_to_phys(rcvd_fis);
+	if (ahci_cmdengine_start(port))
+		return 1;
+	/* Put port into active state. */
+	port->cmd_stat |= HBA_PxCMD_ICC_ACTIVE;
+
+	dev->ctrl = ctrl;
+	dev->port = port;
+	dev->cmdlist = cmdlist;
+	dev->cmdtable = cmdtable;
+	dev->rcvd_fis = rcvd_fis;
+
+	/* Wait for D2H Register FIS with device' signature. */
+	int timeout = 200; /* Time out after 200 * 10ms == 2s. */
+	while ((port->taskfile_data & HBA_PxTFD_BSY) && timeout--)
+		mdelay(10);
+
+	/* Initialize device or fall through to clean up. */
+	switch (port->signature) {
+	case HBA_PxSIG_ATA:
+		printf("ahci: ATA drive on port #%d.\n", portnum);
+#ifdef CONFIG_STORAGE_ATA
+		dev->ata_dev.identify = ahci_identify_device;
+		dev->ata_dev.read_sectors = ahci_ata_read_sectors;
+		return ata_attach_device(&dev->ata_dev, PORT_TYPE_SATA);
+#endif
+		break;
+	case HBA_PxSIG_ATAPI:
+		printf("ahci: ATAPI drive on port #%d.\n", portnum);
+#ifdef CONFIG_STORAGE_ATAPI
+		dev->atapi_dev.identify = ahci_identify_device;
+		dev->atapi_dev.packet_read_cmd = ahci_packet_read_cmd;
+		return atapi_attach_device(&dev->atapi_dev, PORT_TYPE_SATA);
+#endif
+		break;
+	default:
+		printf("ahci: Unsupported device (signature == 0x%08x) "
+				"on port #%d.\n", port->signature, portnum);
+		break;
+	}
+	ret = 2;
+
+_cleanup_ret:
+	/* Clean up (not reached for initialized devices). */
+	if (dev)
+		free(dev);
+	if (!ahci_cmdengine_stop(port)) {
+		port->cmdlist_base = 0;
+		port->frameinfo_base = 0;
+		if (rcvd_fis)
+			free((void *)rcvd_fis);
+		if (cmdtable)
+			free((void *)cmdtable);
+		if (cmdlist)
+			free((void *)cmdlist);
+	}
+	return ret;
+}
+
+static void ahci_port_probe(hba_ctrl_t *const ctrl,
+			    hba_port_t *const port,
+			    const int portnum)
+{
+	/* If staggered spin-up is supported, spin-up device. */
+	if (ctrl->caps & HBA_CAPS_SSS) {
+		port->cmd_stat |= HBA_PxCMD_SUD;
+	}
+
+	/* Wait 1s if we just told the device to spin up or
+	           if it's the first port. */
+	if ((ctrl->caps & HBA_CAPS_SSS) ||
+			!(ctrl->ports_impl & ((1 << (portnum - 1)) - 1))) {
+		/* Wait for port to become active. */
+		int timeout = 100; /* Time out after 100 * 100us == 10ms. */
+		while (!ahci_port_is_active(port) && timeout--)
+			udelay(100);
+	}
+	if (!ahci_port_is_active(port))
+		return;
+
+	ahci_clear_status(port, sata_error);
+	ahci_clear_status(port, intr_status);
+
+	ahci_dev_init(ctrl, port, portnum);
+}
+
+#ifdef CONFIG_STORAGE_AHCI_ONLY_TESTED
+static u32 working_controllers[] = {
+	0x8086 | 0x2929 << 16,
+};
+#endif
+static void ahci_init_pci(pcidev_t dev)
+{
+	int i;
+
+	const u16 class = pci_read_config16(dev, 0xa);
+	if (class != 0x0106)
+		return;
+	const u16 vendor = pci_read_config16(dev, 0x00);
+	const u16 device = pci_read_config16(dev, 0x02);
+
+#ifdef CONFIG_STORAGE_AHCI_ONLY_TESTED
+	const u32 vendor_device = pci_read_config32(dev, 0x0);
+	for (i = 0; i < ARRAY_SIZE(working_controllers); ++i)
+		if (vendor_device == working_controllers[i])
+			break;
+	if (i == ARRAY_SIZE(working_controllers)) {
+		printf("ahci: Not using untested SATA controller "
+			"%02x:%02x.%02x (%04x:%04x).\n", PCI_BUS(dev),
+			PCI_SLOT(dev), PCI_FUNC(dev), vendor, device);
+		return;
+	}
+#endif
+
+	printf("ahci: Found SATA controller %02x:%02x.%02x (%04x:%04x).\n",
+		PCI_BUS(dev), PCI_SLOT(dev), PCI_FUNC(dev), vendor, device);
+
+	hba_ctrl_t *const ctrl = phys_to_virt(
+			pci_read_config32(dev, 0x24) & ~0x3ff);
+	hba_port_t *const ports = ctrl->ports;
+
+	/* Reset host controller. */
+	ctrl->global_ctrl |= HBA_CTRL_RESET;
+	/* Reset has to be finished after 1s. */
+	delay(1);
+	if (ctrl->global_ctrl & HBA_CTRL_RESET) {
+		printf("ahci: ERROR: "
+			"Controller reset didn't finish within 1s.\n");
+		return;
+	}
+
+	/* Set AHCI access mode. */
+	ctrl->global_ctrl |= HBA_CTRL_AHCI_EN;
+
+	/* Probe for devices. */
+	for (i = 0; i < 32; ++i) {
+		if (ctrl->ports_impl & (1 << i))
+			ahci_port_probe(ctrl, &ports[i], i + 1);
+	}
+}
+
+void ahci_initialize(void)
+{
+	int bus, dev, func;
+
+	for (bus = 0; bus < 256; ++bus) {
+		for (dev = 0; dev < 32; ++dev) {
+			const u16 class =
+				pci_read_config16(PCI_DEV(bus, dev, 0), 0xa);
+			if (class != 0xffff) {
+				for (func = 0; func < 8; ++func)
+					ahci_init_pci(PCI_DEV(bus, dev, func));
+			}
+		}
+	}
+}
diff --git a/payloads/libpayload/drivers/storage/ahci_private.h b/payloads/libpayload/drivers/storage/ahci_private.h
new file mode 100644
index 0000000000..ba1471b5d5
--- /dev/null
+++ b/payloads/libpayload/drivers/storage/ahci_private.h
@@ -0,0 +1,201 @@
+/*
+ * This file is part of the libpayload project.
+ *
+ * Copyright (C) 2012 secunet Security Networks AG
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ * 3. The name of the author may not be used to endorse or promote products
+ *    derived from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
+ * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
+ * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
+ * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
+ * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
+ * SUCH DAMAGE.
+ */
+
+#ifndef _AHCI_PRIVATE_H
+#define _AHCI_PRIVATE_H
+
+#include <stdint.h>
+#include <storage/ata.h>
+#include <storage/atapi.h>
+
+typedef volatile struct {
+	u64 cmdlist_base;
+	u64 frameinfo_base;
+	u32 intr_status;
+	u32 intr_enable;
+	u32 cmd_stat;
+	u32 _reserved0;
+	u32 taskfile_data;
+	u32 signature;
+	u32 sata_status;
+	u32 sata_control;
+	u32 sata_error;
+	u32 sata_active;
+	u32 cmd_issue;
+	u32 sata_notify;
+	u32 fis_based_switch;
+	u32 _reserved1[11];
+	u32 _vendor[4];
+} hba_port_t;
+
+#define HBA_PxIS_TFES	(1 << 30)	/* TFES - Task File Error Status */
+#define HBA_PxIS_HBFS	(1 << 29)	/* HBFS - Host Bus Fatal Error Status */
+#define HBA_PxIS_HBDS	(1 << 28)	/* HBDS - Host Bus Data Error Status */
+#define HBA_PxIS_IFS	(1 << 27)	/* IFS - Interface Fatal Error Status */
+#define HBA_PxIS_FATAL	(HBA_PxIS_TFES | HBA_PxIS_HBFS | \
+				HBA_PxIS_HBDS | HBA_PxIS_IFS)
+#define HBA_PxIS_PCS	(1 <<  6)	/* PCS - Port Connect Change Status */
+
+#define HBA_PxCMD_ICC_SHIFT	28
+#define HBA_PxCMD_ICC_MASK	(0xf << HBA_PxCMD_ICC_SHIFT)
+#define HBA_PxCMD_ICC_ACTIVE	(0x1 << HBA_PxCMD_ICC_SHIFT)
+#define HBA_PxCMD_CR		(1 << 15) /* CR - Command list Running */
+#define HBA_PxCMD_FR		(1 << 14) /* FR - FIS receive Running */
+#define HBA_PxCMD_FRE		(1 <<  4) /* FRE - FIS Receive Enable */
+#define HBA_PxCMD_SUD		(1 <<  1) /* SUD - Spin-Up Device */
+#define HBA_PxCMD_ST		(1 <<  0) /* ST - Start (command processing) */
+
+#define HBA_PxTFD_BSY		(1 <<  7)
+#define HBA_PxTFD_DRQ		(1 <<  3)
+
+#define HBA_PxSSTS_IPM_SHIFT		8
+#define HBA_PxSSTS_IPM_MASK		(0xf << HBA_PxSSTS_IPM_SHIFT)
+#define HBA_PxSSTS_IPM_ACTIVE		(1 << HBA_PxSSTS_IPM_SHIFT)
+#define HBA_PxSSTS_DET_SHIFT		0
+#define HBA_PxSSTS_DET_MASK		(0xf << HBA_PxSSTS_DET_SHIFT)
+#define HBA_PxSSTS_DET_ESTABLISHED	(3 << HBA_PxSSTS_DET_SHIFT)
+
+#define HBA_PxSCTL_DET_SHIFT		0
+#define HBA_PxSCTL_DET_MASK		(0xf << HBA_PxSCTL_DET_SHIFT)
+#define HBA_PxSCTL_DET_COMRESET		(0x1 << HBA_PxSCTL_DET_SHIFT)
+
+#define	HBA_PxSIG_ATA	0x00000101 /* SATA drive */
+#define	HBA_PxSIG_ATAPI	0xeb140101 /* SATAPI drive */
+#define	HBA_PxSIG_SEMB	0xc33c0101 /* Enclosure management bridge */
+#define	HBA_PxSIG_PM	0x96690101 /* Port multiplier */
+
+typedef volatile struct {
+	u32 caps;
+	u32 global_ctrl;
+	u32 intr_status;
+	u32 ports_impl;
+	u32 version;
+	u32 ccc_ctrl;		/* CCC - Command Completion Coalescing */
+	u32 ccc_ports;
+	u32 em_location;	/* EM - Enclosure Management */
+	u32 em_ctrl;
+	u32 ext_caps;
+	u32 handoff_ctrl_stat;
+	u32 _reserved0[13];
+	u32 _reserved_nvmchi[16];
+	u32 _vendor[24];
+	hba_port_t ports[32];
+} hba_ctrl_t;
+
+#define HBA_CAPS_SSS		(1 << 27) /* SSS - Supports Staggered Spin-up */
+#define HBA_CAPS_NCS_SHIFT	8	/* NCS - Number of Command Slots */
+#define HBA_CAPS_NCS_MASK	(0x1f << HBA_CAPS_NCS_SHIFT)
+
+#define HBA_CAPS_DECODE_NCS(caps)	(((caps & HBA_CAPS_NCS_MASK)	\
+					  >> HBA_CAPS_NCS_SHIFT)	\
+					 + 1)
+
+#define HBA_CTRL_AHCI_EN	(1 << 31)
+#define HBA_CTRL_INTR_EN	(1 <<  1)
+#define HBA_CTRL_RESET		(1 <<  0)
+
+
+typedef volatile struct {
+	u8 dma_setup_fis[28];
+	u8 _reserved0[4];
+	u8 pio_setup_fis[20];
+	u8 _reserved1[12];
+	u8 d2h_register_fis[20];
+	u8 _reserved2[4];
+	u8 set_device_bits_fis[8];
+	u8 unknown_fis[64];
+	u8 _reserved3[96];
+} rcvd_fis_t;
+
+typedef volatile struct {
+	u16 cmd;
+	u16 prdt_length;
+	u32 prd_bytes;
+	u64 cmdtable_base;
+	u8 _reserved[16];
+} cmd_t;
+
+#define CMD_PMP_SHIFT	12		/* PMP - Port Multiplier Port */
+#define CMD_PMP_MASK	(0xf << CMD_PMP_SHIFT)
+#define CMD_PMP(x)	((x << CMD_PMP_SHIFT) & CMD_PMP_MASK)
+#define CMD_CBuROK	(1 << 10)	/* C - Clear Busy upon R_OK */
+#define CMD_BIST	(1 <<  9)	/* B - BIST - Built-In Selft Test */
+#define CMD_RESET	(1 <<  8)	/* R - Reset */
+#define CMD_PREFETCH	(1 <<  7)	/* P - Prefetch (PRDTs or ATAPI cmd) */
+#define CMD_WRITE	(1 <<  6)	/* W - Write (host to device) */
+#define CMD_ATAPI	(1 <<  5)	/* A - ATAPI cmd */
+#define CMD_CFL_SHIFT	0		/* CFL - Command FIS Length */
+#define CMD_CFL_MASK	(0xf << CMD_CFL_SHIFT)
+#define CMD_CFL(x)	((((x) >> 2) << CMD_CFL_SHIFT) & CMD_CFL_MASK)
+
+typedef volatile struct {
+	u8 fis[64];
+	u8 atapi_cmd[16];
+	u8 _reserved0[48];
+	struct {
+		u64 data_base;
+		u32 _reserved0;
+		u32 flags;
+	} prdt[8]; /* Can be up to 65,535 prds,
+		      but implementation needs multiple of 128 bytes. */
+} cmdtable_t;
+
+#define BYTES_PER_PRD_SHIFT	20
+#define BYTES_PER_PRD		(4 << 20)
+
+enum {
+	FIS_HOST_TO_DEVICE	= 0x27,
+};
+#define FIS_H2D_CMD	(1 << 7)
+#define FIS_H2D_FIS_LEN	20
+#define FIS_H2D_DEV_LBA	(1 << 6)
+
+#define PRD_TABLE_I		(1 << 31) /* I - Interrupt on Completion */
+#define PRD_TABLE_BYTES_MASK	0x3fffff
+#define PRD_TABLE_BYTES(x)	(((x) - 1) & PRD_TABLE_BYTES_MASK)
+
+typedef struct {
+	union {
+		ata_dev_t ata_dev;
+		atapi_dev_t atapi_dev;
+	};
+
+	hba_ctrl_t *ctrl;
+	hba_port_t *port;
+
+	cmd_t *cmdlist;
+	cmdtable_t *cmdtable;
+	rcvd_fis_t *rcvd_fis;
+
+	u8 *buf, *user_buf;
+	int write_back;
+	size_t buflen;
+} ahci_dev_t;
+
+#endif
diff --git a/payloads/libpayload/drivers/storage/ata.c b/payloads/libpayload/drivers/storage/ata.c
new file mode 100644
index 0000000000..e7d45fd8cd
--- /dev/null
+++ b/payloads/libpayload/drivers/storage/ata.c
@@ -0,0 +1,233 @@
+/*
+ * This file is part of the libpayload project.
+ *
+ * Copyright (C) 2012 secunet Security Networks AG
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ * 3. The name of the author may not be used to endorse or promote products
+ *    derived from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
+ * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
+ * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
+ * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
+ * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
+ * SUCH DAMAGE.
+ */
+
+#include <libpayload.h>
+#include <stdint.h>
+#include <string.h>
+#include <stdio.h>
+
+#include <storage/ata.h>
+
+
+/** Reads non-sector-aligned blocks of 512 bytes. */
+static ssize_t ata_read_unaligned(ata_dev_t *const dev,
+				  const lba_t blk_start, size_t blk_count,
+				  u8 *blk_buf)
+{
+	ssize_t ret = 0;
+	lba_t sec_start;
+	size_t blk_residue;
+
+	u8 *const sec_buf = malloc(dev->sector_size);
+	if (sec_buf == NULL)
+		return -1;
+
+	const size_t shift = dev->sector_size_shift - 9;
+	const size_t mask = (dev->sector_size >> 9) - 1;
+
+	/* Sector aligned start block. */
+	const lba_t blk_aligned = blk_start & ~mask;
+
+	/* First sector to read from. */
+	sec_start = blk_aligned >> shift;
+
+	/* Calculate and read residue before sector aligned blocks. */
+	blk_residue = MIN(blk_start - blk_aligned, blk_count);
+	if (blk_residue) {
+		if (dev->read_sectors(dev, sec_start, 1, sec_buf) != 1)
+			goto _free_ret;
+
+		const size_t bytes = blk_residue << 9;
+		memcpy(blk_buf, sec_buf + (dev->sector_size - bytes), bytes);
+		blk_count -= blk_residue;
+		blk_buf += bytes;
+		++sec_start;
+
+		ret = blk_residue;
+		if (blk_count == 0)
+			goto _free_ret;
+	}
+
+	/* Read all sector aligned blocks. */
+	const size_t sec_count = (blk_count & ~mask) >> shift;
+	const int sec_read = dev->read_sectors(
+				dev, sec_start, sec_count, blk_buf);
+	if (sec_read < 0)
+		goto _free_ret;
+	ret += sec_read << shift;
+	if (sec_read != sec_count)
+		goto _free_ret;
+
+	/* Calculate and read residue. */
+	blk_residue = blk_count & mask;
+	if (blk_residue) {
+		sec_start += sec_read;
+		blk_buf += sec_read << dev->sector_size_shift;
+
+		if (dev->read_sectors(dev, sec_start, 1, sec_buf) != 1)
+			goto _free_ret;
+
+		const size_t bytes = blk_residue << 9;
+		memcpy(blk_buf, sec_buf, bytes);
+		ret += blk_residue;
+	}
+
+_free_ret:
+	free(sec_buf);
+	return ret;
+}
+
+static ssize_t ata_read512(storage_dev_t *_dev,
+			   const lba_t start, const size_t count,
+			   unsigned char *const buf)
+{
+	ata_dev_t *const dev = (ata_dev_t *)_dev;
+
+	if (dev->read_sectors == NULL) {
+		printf("ata: No read support implemented.\n");
+		return -1;
+	}
+
+	if (dev->sector_size == 512) {
+		return dev->read_sectors(dev, start, count, buf);
+	} else if (dev->sector_size > 512) {
+		/* Sector size has to be a power of two. */
+		const size_t mask = (dev->sector_size >> 9) - 1;
+		if (!(start & mask) && !(count & mask)) {
+			const size_t shift = dev->sector_size_shift - 9;
+			const ssize_t ret = dev->read_sectors(dev,
+					start >> shift, count >> shift, buf);
+			if (ret < 0)
+				return ret;
+			else
+				return ret << shift;
+		} else {
+			return ata_read_unaligned(dev, start, count, buf);
+		}
+	} else {
+		printf("ata: No support for sectors smaller than 512 bytes.\n");
+		return -1;
+	}
+}
+
+static ssize_t ata_write512(storage_dev_t *const dev,
+			    const lba_t start, const size_t count,
+			    const unsigned char *const buf)
+{
+	printf("ata: No write support implemented.\n");
+	return -1;
+}
+
+void ata_initialize_storage_ops(ata_dev_t *const dev)
+{
+	dev->storage_dev.read_blocks512 = ata_read512;
+	dev->storage_dev.write_blocks512 = ata_write512;
+}
+
+int ata_set_sector_size(ata_dev_t *const dev, u32 sector_size)
+{
+	if (!sector_size || (sector_size & (sector_size - 1))) {
+		printf("ata: Sector size is not a power of two (%u).\n",
+								sector_size);
+		return -1;
+	}
+	dev->sector_size = sector_size;
+	dev->sector_size_shift = 0;
+	while (sector_size >>= 1)
+		++dev->sector_size_shift;
+
+	return 0;
+}
+
+static int ata_decode_sector_size(ata_dev_t *const dev, const u16 *const id)
+{
+	u32 size;
+	if ((id[ATA_ID_SECTOR_SIZE] & ((3 << 14) | (1 << 12)))
+			!= ((1 << 14) | (1 << 12)))
+		size = DEFAULT_ATA_SECTOR_SIZE;
+	else
+		size = (id[ATA_ID_LOGICAL_SECTOR_SIZE] |
+			(id[ATA_ID_LOGICAL_SECTOR_SIZE + 1] << 16)) << 1;
+
+	return ata_set_sector_size(dev, size);
+}
+
+/**
+ * Copies n-1 bytes from src to dest swapping each two bytes, removes
+ * trailing spaces and terminates dest with '\0'.
+ */
+char *ata_strncpy(char *const dest, const u16 *const src, const size_t n)
+{
+	int i;
+
+	for (i = 0; i < (n - 1); i += 2) {
+		dest[i] = ((const char *)src)[i + 1];
+		dest[i + 1] = ((const char *)src)[i];
+	}
+
+	for (i = n - 2; i >= 0; --i)
+		if (dest[i] != ' ')
+			break;
+	dest[i + 1] = '\0';
+
+	return dest;
+}
+
+int ata_attach_device(ata_dev_t *const dev, const storage_port_t port_type)
+{
+	u16 id[256];
+
+	dev->identify_cmd = ATA_IDENTIFY_DEVICE;
+	if (dev->identify(dev, (u8 *)id))
+		return -1;
+
+	char fw[9], model[41];
+	ata_strncpy(fw, id + 23, sizeof(fw));
+	ata_strncpy(model, id + 27, sizeof(model));
+	printf("ata: Identified %s [%s]\n", model, fw);
+
+#ifdef CONFIG_STORAGE_64BIT_LBA
+	if (id[ATA_CMDS_AND_FEATURE_SETS + 1] & (1 << 10)) {
+		printf("ata: Support for LBA-48 enabled.\n");
+		dev->read_cmd = ATA_READ_DMA_EXT;
+	} else {
+		dev->read_cmd = ATA_READ_DMA;
+	}
+#else
+	dev->read_cmd = ATA_READ_DMA;
+#endif
+
+	if (ata_decode_sector_size(dev, id))
+		return -1;
+
+	dev->storage_dev.port_type = port_type;
+	ata_initialize_storage_ops(dev);
+
+	return storage_attach_device(&dev->storage_dev);
+}
diff --git a/payloads/libpayload/drivers/storage/atapi.c b/payloads/libpayload/drivers/storage/atapi.c
new file mode 100644
index 0000000000..82d14cee61
--- /dev/null
+++ b/payloads/libpayload/drivers/storage/atapi.c
@@ -0,0 +1,206 @@
+/*
+ * This file is part of the libpayload project.
+ *
+ * Copyright (C) 2012 secunet Security Networks AG
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ * 3. The name of the author may not be used to endorse or promote products
+ *    derived from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
+ * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
+ * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
+ * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
+ * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
+ * SUCH DAMAGE.
+ */
+
+#include <stdint.h>
+#include <stdio.h>
+#include <libpayload.h>
+
+#include <storage/ata.h>
+#include <storage/atapi.h>
+
+
+static int atapi_request_sense(atapi_dev_t *const dev)
+{
+	u8 cdb[12] = { 0, };
+	cdb[0] = ATAPI_REQUEST_SENSE;
+	cdb[4] = sizeof(dev->sense_data);
+	return dev->packet_read_cmd(dev, cdb, sizeof(cdb),
+			dev->sense_data, sizeof(dev->sense_data));
+}
+
+static ssize_t atapi_packet_read_cmd(atapi_dev_t *const dev,
+				     const u8 *const cmd, const size_t cmdlen,
+				     u8 *const buf, const size_t buflen)
+{
+	int retries = 10;
+	ssize_t ret;
+
+	do {
+		ret = dev->packet_read_cmd(dev, cmd, cmdlen, buf, buflen);
+		if (ret >= 0)
+			return ret;
+
+		ret = atapi_request_sense(dev);
+		if (ret < 0) {
+			printf("atapi: Requesting sense failed.\n");
+			return -1;
+		}
+
+		switch (dev->sense_data[2] & 0xf) {
+		case ATAPI_SENSE_UNIT_ATTENTION:
+			/* Nothing to do. */
+			break;
+		case ATAPI_SENSE_NOT_READY:
+			switch (dev->sense_data[12]) {
+			case ATAPI_ADDITIONAL_SENSE_LOGICAL_UNIT_NOT_READY:
+				/* Device wants more time. */
+				delay(3);
+				break;
+			case ATAPI_ADDITIONAL_SENSE_MEDIUM_NOT_PRESENT:
+				printf("atapi: No medium present.\n");
+				dev->medium_present = 0;
+				return -1;
+			}
+			break;
+		default:
+			return -1;
+		}
+	} while (retries--);
+
+	/* No more retries. */
+	return -1;
+}
+
+static ssize_t atapi_packet_read_sectors(ata_dev_t *const _dev,
+					 const lba_t start, size_t count,
+					 u8 *const buf)
+{
+	atapi_dev_t *const dev = (atapi_dev_t *)_dev;
+
+	if (start >= (1ULL << 32)) {
+		printf("atapi: Sector is not 32-bit addressable.\n");
+		return -1;
+	} else if (count >= (64 * 1024)) {
+	       printf("ahci: Sector count too high (max. 65535).\n");
+	       count = (64 * 1024) - 1;
+	}
+
+	u8 cdb[12] = { 0, };
+	cdb[0] = ATAPI_READ_10;
+	cdb[2] = (start >> 24) & 0xff;
+	cdb[3] = (start >> 16) & 0xff;
+	cdb[4] = (start >>  8) & 0xff;
+	cdb[5] = (start >>  0) & 0xff;
+	cdb[7] = (count >>  8) & 0xff;
+	cdb[8] = (count >>  0) & 0xff;
+	const ssize_t ret = atapi_packet_read_cmd(dev, cdb, sizeof(cdb),
+				buf, count << dev->ata_dev.sector_size_shift);
+	if (ret < 0)
+		return ret;
+	else
+		return ret >> dev->ata_dev.sector_size_shift;
+}
+
+static storage_poll_t atapi_poll(storage_dev_t *const _dev)
+{
+	atapi_dev_t *const dev = (atapi_dev_t *)_dev;
+
+	u8 cdb[12] = { 0, };
+	u8 capacity[8];
+
+	if (dev->medium_present)
+		return POLL_MEDIUM_PRESENT;
+
+	cdb[0] = ATAPI_TEST_UNIT_READY;
+	const int ret = atapi_packet_read_cmd(dev, cdb, sizeof(cdb), NULL, 0);
+	if (!ret) {
+		printf("atapi: Found medium.\n");
+		dev->medium_present = 1;
+	} else if (dev->medium_present) {
+		return POLL_ERROR;
+	}
+
+	if (dev->medium_present) {
+		cdb[0] = ATAPI_START_STOP_UNIT;
+		cdb[4] = 0x01; /* Start Disc, read TOC. */
+		if (atapi_packet_read_cmd(dev, cdb, sizeof(cdb), NULL, 0))
+			goto _error_ret;
+		cdb[4] = 0x00;
+	}
+
+	cdb[0] = ATAPI_PREVENT_ALLOW_MEDIUM_REMOVAL;
+	cdb[2] = 0x02; /* Clear persistent prevent removal bit. */
+	atapi_packet_read_cmd(dev, cdb, sizeof(cdb), NULL, 0);
+	cdb[2] = 0x00; /* Clear prevent removal bit. */
+	atapi_packet_read_cmd(dev, cdb, sizeof(cdb), NULL, 0);
+
+	/* If we don't have a medium, we're done. */
+	if (!dev->medium_present)
+		return POLL_NO_MEDIUM;
+
+	/* Read capacity and sector size. */
+	cdb[0] = ATAPI_READ_CAPACITY;
+	if (atapi_packet_read_cmd(dev, cdb, sizeof(cdb),
+				  (u8 *)capacity, sizeof(capacity))
+			!= sizeof(capacity))
+		goto _error_ret;
+	const u32 sector_size = (capacity[4] << 24) |
+				(capacity[5] << 16) |
+				(capacity[6] <<  8) |
+				(capacity[7] <<  0);
+	if (ata_set_sector_size(&dev->ata_dev, sector_size)) {
+		dev->medium_present = 0;
+		return POLL_MEDIUM_ERROR;
+	}
+
+	return POLL_MEDIUM_PRESENT;
+
+_error_ret:
+	dev->medium_present = 0;
+	return POLL_ERROR;
+}
+
+int atapi_attach_device(atapi_dev_t *const dev, const storage_port_t port_type)
+{
+	u16 id[256];
+
+	dev->ata_dev.identify_cmd = ATA_IDENTIFY_PACKET_DEVICE;
+	if (dev->identify(&dev->ata_dev, (u8 *)id))
+		return -1;
+
+	char fw[9], model[41];
+	ata_strncpy(fw, id + 23, sizeof(fw));
+	ata_strncpy(model, id + 27, sizeof(model));
+	printf("atapi: Identified %s [%s]\n", model, fw);
+
+	/* Initialize to sane values. */
+	dev->ata_dev.sector_size = 2048;
+	dev->ata_dev.sector_size_shift = 11;
+
+	dev->medium_present = 0;
+
+	/* Reuse ata procedures. */
+	ata_initialize_storage_ops(&dev->ata_dev);
+
+	dev->ata_dev.read_sectors = atapi_packet_read_sectors;
+	dev->ata_dev.storage_dev.port_type = port_type;
+	dev->ata_dev.storage_dev.poll = atapi_poll;
+
+	return storage_attach_device(&dev->ata_dev.storage_dev);
+}
diff --git a/payloads/libpayload/drivers/storage/storage.c b/payloads/libpayload/drivers/storage/storage.c
new file mode 100644
index 0000000000..3e9c375201
--- /dev/null
+++ b/payloads/libpayload/drivers/storage/storage.c
@@ -0,0 +1,111 @@
+/*
+ * This file is part of the libpayload project.
+ *
+ * Copyright (C) 2012 secunet Security Networks AG
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ * 3. The name of the author may not be used to endorse or promote products
+ *    derived from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
+ * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
+ * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
+ * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
+ * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
+ * SUCH DAMAGE.
+ */
+
+#include <libpayload.h>
+#ifdef CONFIG_STORAGE_AHCI
+# include <storage/ahci.h>
+#endif
+#include <storage/storage.h>
+
+
+static storage_dev_t **devices = NULL;
+static size_t devices_length = 0;
+static size_t dev_count = 0;
+
+int storage_attach_device(storage_dev_t *const dev)
+{
+	if (dev_count == devices_length) {
+		const size_t new_len =
+			(0 == devices_length) ? 4 : devices_length << 1;
+		storage_dev_t **const new_devices =
+			realloc(devices, new_len * sizeof(storage_dev_t *));
+		if (!new_devices)
+			return -1;
+		devices = new_devices;
+		memset(devices + devices_length, '\0',
+			(new_len - devices_length) * sizeof(storage_dev_t *));
+		devices_length = new_len;
+	}
+	devices[dev_count++] = dev;
+
+	return 0;
+}
+
+/**
+ * Probe for drive with given number
+ *
+ * Looks for a drive with number dev_num and polls for a medium
+ * in the drive if appropriate.
+ *
+ * @dev_num device number counted from 0
+ */
+storage_poll_t storage_probe(const size_t dev_num)
+{
+	if (dev_num >= dev_count)
+		return POLL_NO_DEVICE;
+	else if (devices[dev_num]->poll)
+		return devices[dev_num]->poll(devices[dev_num]);
+	else
+		return POLL_MEDIUM_PRESENT;
+}
+
+/**
+ * Read 512-byte blocks
+ *
+ * Reads count blocks of 512 bytes from block start of drive dev_num
+ * into buf.
+ *
+ * @dev_num device number counted from 0
+ * @start number of first block to read from
+ * @count number of blocks to read
+ * @buf buffer where the read data should be written
+ */
+ssize_t storage_read_blocks512(const size_t dev_num,
+			       const lba_t start, const size_t count,
+			       unsigned char *const buf)
+{
+	if ((dev_num < dev_count) && devices[dev_num]->read_blocks512)
+		return devices[dev_num]->read_blocks512(
+				devices[dev_num], start, count, buf);
+	else
+		return -1;
+}
+
+/**
+ * Initializes storage controllers
+ *
+ * This function should be called once at startup to bring up supported
+ * strorage controllers.
+ */
+void storage_initialize(void)
+{
+#ifdef CONFIG_STORAGE_AHCI
+	ahci_initialize();
+#endif
+}