1 files changed, 171 insertions, 9 deletions

diff --git a/qemu/tests/libqos/ahci.c b/qemu/tests/libqos/ahci.c
index cf66b3e32..ac6c155c8 100644
--- a/qemu/tests/libqos/ahci.c
+++ b/qemu/tests/libqos/ahci.c
@@ -22,6 +22,7 @@
  * THE SOFTWARE.
  */
 
+#include "qemu/osdep.h"
 #include <glib.h>
 
 #include "libqtest.h"
@@ -74,7 +75,11 @@ AHCICommandProp ahci_command_properties[] = {
                                  .lba48 = true, .write = true, .ncq = true },
     { .cmd = CMD_READ_MAX,       .lba28 = true },
     { .cmd = CMD_READ_MAX_EXT,   .lba48 = true },
-    { .cmd = CMD_FLUSH_CACHE,    .data = false }
+    { .cmd = CMD_FLUSH_CACHE,    .data = false },
+    { .cmd = CMD_PACKET,         .data = true,  .size = 16,
+                                 .atapi = true, .pio = true },
+    { .cmd = CMD_PACKET_ID,      .data = true,  .pio = true,
+                                 .size = 512,   .read = true }
 };
 
 struct AHCICommand {
@@ -90,7 +95,7 @@ struct AHCICommand {
     /* Data to be transferred to the guest */
     AHCICommandHeader header;
     RegH2DFIS fis;
-    void *atapi_cmd;
+    unsigned char *atapi_cmd;
 };
 
 /**
@@ -110,6 +115,11 @@ void ahci_free(AHCIQState *ahci, uint64_t addr)
     qfree(ahci->parent, addr);
 }
 
+bool is_atapi(AHCIQState *ahci, uint8_t port)
+{
+    return ahci_px_rreg(ahci, port, AHCI_PX_SIG) == AHCI_SIGNATURE_CDROM;
+}
+
 /**
  * Locate, verify, and return a handle to the AHCI device.
  */
@@ -592,6 +602,82 @@ inline unsigned size_to_prdtl(unsigned bytes, unsigned bytes_per_prd)
     return (bytes + bytes_per_prd - 1) / bytes_per_prd;
 }
 
+const AHCIOpts default_opts = { .size = 0 };
+
+/**
+ * ahci_exec: execute a given command on a specific
+ * AHCI port.
+ *
+ * @ahci: The device to send the command to
+ * @port: The port number of the SATA device we wish
+ *        to have execute this command
+ * @op:   The S/ATA command to execute, or if opts.atapi
+ *        is true, the SCSI command code.
+ * @opts: Optional arguments to modify execution behavior.
+ */
+void ahci_exec(AHCIQState *ahci, uint8_t port,
+               uint8_t op, const AHCIOpts *opts_in)
+{
+    AHCICommand *cmd;
+    int rc;
+    AHCIOpts *opts;
+
+    opts = g_memdup((opts_in == NULL ? &default_opts : opts_in),
+                    sizeof(AHCIOpts));
+
+    /* No guest buffer provided, create one. */
+    if (opts->size && !opts->buffer) {
+        opts->buffer = ahci_alloc(ahci, opts->size);
+        g_assert(opts->buffer);
+        qmemset(opts->buffer, 0x00, opts->size);
+    }
+
+    /* Command creation */
+    if (opts->atapi) {
+        cmd = ahci_atapi_command_create(op);
+        if (opts->atapi_dma) {
+            ahci_command_enable_atapi_dma(cmd);
+        }
+    } else {
+        cmd = ahci_command_create(op);
+    }
+    ahci_command_adjust(cmd, opts->lba, opts->buffer,
+                        opts->size, opts->prd_size);
+
+    if (opts->pre_cb) {
+        rc = opts->pre_cb(ahci, cmd, opts);
+        g_assert_cmpint(rc, ==, 0);
+    }
+
+    /* Write command to memory and issue it */
+    ahci_command_commit(ahci, cmd, port);
+    ahci_command_issue_async(ahci, cmd);
+    if (opts->error) {
+        qmp_eventwait("STOP");
+    }
+    if (opts->mid_cb) {
+        rc = opts->mid_cb(ahci, cmd, opts);
+        g_assert_cmpint(rc, ==, 0);
+    }
+    if (opts->error) {
+        qmp_async("{'execute':'cont' }");
+        qmp_eventwait("RESUME");
+    }
+
+    /* Wait for command to complete and verify sanity */
+    ahci_command_wait(ahci, cmd);
+    ahci_command_verify(ahci, cmd);
+    if (opts->post_cb) {
+        rc = opts->post_cb(ahci, cmd, opts);
+        g_assert_cmpint(rc, ==, 0);
+    }
+    ahci_command_free(cmd);
+    if (opts->buffer != opts_in->buffer) {
+        ahci_free(ahci, opts->buffer);
+    }
+    g_free(opts);
+}
+
 /* Issue a command, expecting it to fail and STOP the VM */
 AHCICommand *ahci_guest_io_halt(AHCIQState *ahci, uint8_t port,
                                 uint8_t ide_cmd, uint64_t buffer,
@@ -659,16 +745,16 @@ void ahci_io(AHCIQState *ahci, uint8_t port, uint8_t ide_cmd,
     props = ahci_command_find(ide_cmd);
     g_assert(props);
     ptr = ahci_alloc(ahci, bufsize);
-    g_assert(ptr);
+    g_assert(!bufsize || ptr);
     qmemset(ptr, 0x00, bufsize);
 
-    if (props->write) {
+    if (bufsize && props->write) {
         bufwrite(ptr, buffer, bufsize);
     }
 
     ahci_guest_io(ahci, port, ide_cmd, ptr, bufsize, sector);
 
-    if (props->read) {
+    if (bufsize && props->read) {
         bufread(ptr, buffer, bufsize);
     }
 
@@ -731,6 +817,18 @@ static void command_table_init(AHCICommand *cmd)
     memset(fis->aux, 0x00, ARRAY_SIZE(fis->aux));
 }
 
+void ahci_command_enable_atapi_dma(AHCICommand *cmd)
+{
+    RegH2DFIS *fis = &(cmd->fis);
+    g_assert(cmd->props->atapi);
+    fis->feature_low |= 0x01;
+    cmd->interrupts &= ~AHCI_PX_IS_PSS;
+    cmd->props->dma = true;
+    cmd->props->pio = false;
+    /* BUG: We expect the DMA Setup interrupt for DMA commands */
+    /* cmd->interrupts |= AHCI_PX_IS_DSS; */
+}
+
 AHCICommand *ahci_command_create(uint8_t command_name)
 {
     AHCICommandProp *props = ahci_command_find(command_name);
@@ -742,10 +840,10 @@ AHCICommand *ahci_command_create(uint8_t command_name)
     g_assert(!(props->lba28 && props->lba48));
     g_assert(!(props->read && props->write));
     g_assert(!props->size || props->data);
-    g_assert(!props->ncq || (props->ncq && props->lba48));
+    g_assert(!props->ncq || props->lba48);
 
     /* Defaults and book-keeping */
-    cmd->props = props;
+    cmd->props = g_memdup(props, sizeof(AHCICommandProp));
     cmd->name = command_name;
     cmd->xbytes = props->size;
     cmd->prd_size = 4096;
@@ -767,8 +865,23 @@ AHCICommand *ahci_command_create(uint8_t command_name)
     return cmd;
 }
 
+AHCICommand *ahci_atapi_command_create(uint8_t scsi_cmd)
+{
+    AHCICommand *cmd = ahci_command_create(CMD_PACKET);
+    cmd->atapi_cmd = g_malloc0(16);
+    cmd->atapi_cmd[0] = scsi_cmd;
+    /* ATAPI needs a PIO transfer chunk size set inside of the LBA registers.
+     * The block/sector size is a natural default. */
+    cmd->fis.lba_lo[1] = ATAPI_SECTOR_SIZE >> 8 & 0xFF;
+    cmd->fis.lba_lo[2] = ATAPI_SECTOR_SIZE & 0xFF;
+
+    return cmd;
+}
+
 void ahci_command_free(AHCICommand *cmd)
 {
+    g_free(cmd->atapi_cmd);
+    g_free(cmd->props);
     g_free(cmd);
 }
 
@@ -782,10 +895,34 @@ void ahci_command_clr_flags(AHCICommand *cmd, uint16_t cmdh_flags)
     cmd->header.flags &= ~cmdh_flags;
 }
 
+static void ahci_atapi_command_set_offset(AHCICommand *cmd, uint64_t lba)
+{
+    unsigned char *cbd = cmd->atapi_cmd;
+    g_assert(cbd);
+
+    switch (cbd[0]) {
+    case CMD_ATAPI_READ_10:
+        g_assert_cmpuint(lba, <=, UINT32_MAX);
+        stl_be_p(&cbd[2], lba);
+        break;
+    default:
+        /* SCSI doesn't have uniform packet formats,
+         * so you have to add support for it manually. Sorry! */
+        g_assert_not_reached();
+    }
+}
+
 void ahci_command_set_offset(AHCICommand *cmd, uint64_t lba_sect)
 {
     RegH2DFIS *fis = &(cmd->fis);
-    if (cmd->props->lba28) {
+
+    if (cmd->props->atapi) {
+        ahci_atapi_command_set_offset(cmd, lba_sect);
+        return;
+    } else if (!cmd->props->data && !lba_sect) {
+        /* Not meaningful, ignore. */
+        return;
+    } else if (cmd->props->lba28) {
         g_assert_cmphex(lba_sect, <=, 0xFFFFFFF);
     } else if (cmd->props->lba48 || cmd->props->ncq) {
         g_assert_cmphex(lba_sect, <=, 0xFFFFFFFFFFFF);
@@ -811,6 +948,24 @@ void ahci_command_set_buffer(AHCICommand *cmd, uint64_t buffer)
     cmd->buffer = buffer;
 }
 
+static void ahci_atapi_set_size(AHCICommand *cmd, uint64_t xbytes)
+{
+    unsigned char *cbd = cmd->atapi_cmd;
+    uint64_t nsectors = xbytes / 2048;
+    g_assert(cbd);
+
+    switch (cbd[0]) {
+    case CMD_ATAPI_READ_10:
+        g_assert_cmpuint(nsectors, <=, UINT16_MAX);
+        stw_be_p(&cbd[7], nsectors);
+        break;
+    default:
+        /* SCSI doesn't have uniform packet formats,
+         * so you have to add support for it manually. Sorry! */
+        g_assert_not_reached();
+    }
+}
+
 void ahci_command_set_sizes(AHCICommand *cmd, uint64_t xbytes,
                             unsigned prd_size)
 {
@@ -829,6 +984,8 @@ void ahci_command_set_sizes(AHCICommand *cmd, uint64_t xbytes,
         NCQFIS *nfis = (NCQFIS *)&(cmd->fis);
         nfis->sector_low = sect_count & 0xFF;
         nfis->sector_hi = (sect_count >> 8) & 0xFF;
+    } else if (cmd->props->atapi) {
+        ahci_atapi_set_size(cmd, xbytes);
     } else {
         cmd->fis.count = sect_count;
     }
@@ -877,9 +1034,14 @@ void ahci_command_commit(AHCIQState *ahci, AHCICommand *cmd, uint8_t port)
     g_assert((table_ptr & 0x7F) == 0x00);
     cmd->header.ctba = table_ptr;
 
-    /* Commit the command header and command FIS */
+    /* Commit the command header (part of the Command List Buffer) */
     ahci_set_command_header(ahci, port, cmd->slot, &(cmd->header));
+    /* Now, write the command table (FIS, ACMD, and PRDT) -- FIS first, */
     ahci_write_fis(ahci, cmd);
+    /* Then ATAPI CMD, if needed */
+    if (cmd->props->atapi) {
+        memwrite(table_ptr + 0x40, cmd->atapi_cmd, 16);
+    }
 
     /* Construct and write the PRDs to the command table */
     g_assert_cmphex(prdtl, ==, cmd->header.prdtl);