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>
diff --git a/payloads/libpayload/drivers/Makefile.inc b/payloads/libpayload/drivers/Makefile.inc
index 360a258..80c9a5b 100644
--- a/payloads/libpayload/drivers/Makefile.inc
+++ b/payloads/libpayload/drivers/Makefile.inc
@@ -52,6 +52,11 @@
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 0000000..b33cc70
--- /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 0000000..ba1471b
--- /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 0000000..e7d45fd
--- /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 0000000..82d14ce
--- /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 0000000..3e9c375
--- /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
+}