tests/libqos/ahci.c

   1 /*
   2  * libqos AHCI functions
   3  *
   4  * Copyright (c) 2014 John Snow <[email protected]>
   5  *
   6  * Permission is hereby granted, free of charge, to any person obtaining a copy
   7  * of this software and associated documentation files (the "Software"), to deal
   8  * in the Software without restriction, including without limitation the rights
   9  * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
  10  * copies of the Software, and to permit persons to whom the Software is
  11  * furnished to do so, subject to the following conditions:
  12  *
  13  * The above copyright notice and this permission notice shall be included in
  14  * all copies or substantial portions of the Software.
  15  *
  16  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
  17  * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
  18  * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
  19  * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
  20  * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
  21  * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
  22  * THE SOFTWARE.
  23  */
  24
  25 #include "qemu/osdep.h"
  26
  27 #include "libqtest.h"
  28 #include "libqos/ahci.h"
  29 #include "libqos/pci-pc.h"
  30
  31 #include "qemu-common.h"
  32 #include "qemu/host-utils.h"
  33
  34 #include "hw/pci/pci_ids.h"
  35 #include "hw/pci/pci_regs.h"
  36
  37 typedef struct AHCICommandProp {
  38     uint8_t  cmd;        /* Command Code */
  39     bool     data;       /* Data transfer command? */
  40     bool     pio;
  41     bool     dma;
  42     bool     lba28;
  43     bool     lba48;
  44     bool     read;
  45     bool     write;
  46     bool     atapi;
  47     bool     ncq;
  48     uint64_t size;       /* Static transfer size, for commands like IDENTIFY. */
  49     uint32_t interrupts; /* Expected interrupts for this command. */
  50 } AHCICommandProp;
  51
  52 AHCICommandProp ahci_command_properties[] = {
  53     { .cmd = CMD_READ_PIO,       .data = true,  .pio = true,
  54                                  .lba28 = true, .read = true },
  55     { .cmd = CMD_WRITE_PIO,      .data = true,  .pio = true,
  56                                  .lba28 = true, .write = true },
  57     { .cmd = CMD_READ_PIO_EXT,   .data = true,  .pio = true,
  58                                  .lba48 = true, .read = true },
  59     { .cmd = CMD_WRITE_PIO_EXT,  .data = true,  .pio = true,
  60                                  .lba48 = true, .write = true },
  61     { .cmd = CMD_READ_DMA,       .data = true,  .dma = true,
  62                                  .lba28 = true, .read = true },
  63     { .cmd = CMD_WRITE_DMA,      .data = true,  .dma = true,
  64                                  .lba28 = true, .write = true },
  65     { .cmd = CMD_READ_DMA_EXT,   .data = true,  .dma = true,
  66                                  .lba48 = true, .read = true },
  67     { .cmd = CMD_WRITE_DMA_EXT,  .data = true,  .dma = true,
  68                                  .lba48 = true, .write = true },
  69     { .cmd = CMD_IDENTIFY,       .data = true,  .pio = true,
  70                                  .size = 512,   .read = true },
  71     { .cmd = READ_FPDMA_QUEUED,  .data = true,  .dma = true,
  72                                  .lba48 = true, .read = true, .ncq = true },
  73     { .cmd = WRITE_FPDMA_QUEUED, .data = true,  .dma = true,
  74                                  .lba48 = true, .write = true, .ncq = true },
  75     { .cmd = CMD_READ_MAX,       .lba28 = true },
  76     { .cmd = CMD_READ_MAX_EXT,   .lba48 = true },
  77     { .cmd = CMD_FLUSH_CACHE,    .data = false },
  78     { .cmd = CMD_PACKET,         .data = true,  .size = 16,
  79                                  .atapi = true, .pio = true },
  80     { .cmd = CMD_PACKET_ID,      .data = true,  .pio = true,
  81                                  .size = 512,   .read = true }
  82 };
  83
  84 struct AHCICommand {
  85     /* Test Management Data */
  86     uint8_t name;
  87     uint8_t port;
  88     uint8_t slot;
  89     uint8_t errors;
  90     uint32_t interrupts;
  91     uint64_t xbytes;
  92     uint32_t prd_size;
  93     uint64_t buffer;
  94     AHCICommandProp *props;
  95     /* Data to be transferred to the guest */
  96     AHCICommandHeader header;
  97     RegH2DFIS fis;
  98     unsigned char *atapi_cmd;
  99 };
 100
 101 /**
 102  * Allocate space in the guest using information in the AHCIQState object.
 103  */
 104 uint64_t ahci_alloc(AHCIQState *ahci, size_t bytes)
 105 {
 106     g_assert(ahci);
 107     g_assert(ahci->parent);
 108     return qmalloc(ahci->parent, bytes);
 109 }
 110
 111 void ahci_free(AHCIQState *ahci, uint64_t addr)
 112 {
 113     g_assert(ahci);
 114     g_assert(ahci->parent);
 115     qfree(ahci->parent, addr);
 116 }
 117
 118 bool is_atapi(AHCIQState *ahci, uint8_t port)
 119 {
 120     return ahci_px_rreg(ahci, port, AHCI_PX_SIG) == AHCI_SIGNATURE_CDROM;
 121 }
 122
 123 /**
 124  * Locate, verify, and return a handle to the AHCI device.
 125  */
 126 QPCIDevice *get_ahci_device(uint32_t *fingerprint)
 127 {
 128     QPCIDevice *ahci;
 129     uint32_t ahci_fingerprint;
 130     QPCIBus *pcibus;
 131
 132     pcibus = qpci_init_pc(NULL);
 133
 134     /* Find the AHCI PCI device and verify it's the right one. */
 135     ahci = qpci_device_find(pcibus, QPCI_DEVFN(0x1F, 0x02));
 136     g_assert(ahci != NULL);
 137
 138     ahci_fingerprint = qpci_config_readl(ahci, PCI_VENDOR_ID);
 139
 140     switch (ahci_fingerprint) {
 141     case AHCI_INTEL_ICH9:
 142         break;
 143     default:
 144         /* Unknown device. */
 145         g_assert_not_reached();
 146     }
 147
 148     if (fingerprint) {
 149         *fingerprint = ahci_fingerprint;
 150     }
 151     return ahci;
 152 }
 153
 154 void free_ahci_device(QPCIDevice *dev)
 155 {
 156     QPCIBus *pcibus = dev ? dev->bus : NULL;
 157
 158     /* libqos doesn't have a function for this, so free it manually */
 159     g_free(dev);
 160     qpci_free_pc(pcibus);
 161 }
 162
 163 /* Free all memory in-use by the AHCI device. */
 164 void ahci_clean_mem(AHCIQState *ahci)
 165 {
 166     uint8_t port, slot;
 167
 168     for (port = 0; port < 32; ++port) {
 169         if (ahci->port[port].fb) {
 170             ahci_free(ahci, ahci->port[port].fb);
 171             ahci->port[port].fb = 0;
 172         }
 173         if (ahci->port[port].clb) {
 174             for (slot = 0; slot < 32; slot++) {
 175                 ahci_destroy_command(ahci, port, slot);
 176             }
 177             ahci_free(ahci, ahci->port[port].clb);
 178             ahci->port[port].clb = 0;
 179         }
 180     }
 181 }
 182
 183 /*** Logical Device Initialization ***/
 184
 185 /**
 186  * Start the PCI device and sanity-check default operation.
 187  */
 188 void ahci_pci_enable(AHCIQState *ahci)
 189 {
 190     uint8_t reg;
 191
 192     start_ahci_device(ahci);
 193
 194     switch (ahci->fingerprint) {
 195     case AHCI_INTEL_ICH9:
 196         /* ICH9 has a register at PCI 0x92 that
 197          * acts as a master port enabler mask. */
 198         reg = qpci_config_readb(ahci->dev, 0x92);
 199         reg |= 0x3F;
 200         qpci_config_writeb(ahci->dev, 0x92, reg);
 201         /* 0...0111111b -- bit significant, ports 0-5 enabled. */
 202         ASSERT_BIT_SET(qpci_config_readb(ahci->dev, 0x92), 0x3F);
 203         break;
 204     }
 205
 206 }
 207
 208 /**
 209  * Map BAR5/ABAR, and engage the PCI device.
 210  */
 211 void start_ahci_device(AHCIQState *ahci)
 212 {
 213     /* Map AHCI's ABAR (BAR5) */
 214     ahci->hba_bar = qpci_iomap(ahci->dev, 5, &ahci->barsize);
 215
 216     /* turns on pci.cmd.iose, pci.cmd.mse and pci.cmd.bme */
 217     qpci_device_enable(ahci->dev);
 218 }
 219
 220 /**
 221  * Test and initialize the AHCI's HBA memory areas.
 222  * Initialize and start any ports with devices attached.
 223  * Bring the HBA into the idle state.
 224  */
 225 void ahci_hba_enable(AHCIQState *ahci)
 226 {
 227     /* Bits of interest in this section:
 228      * GHC.AE     Global Host Control / AHCI Enable
 229      * PxCMD.ST   Port Command: Start
 230      * PxCMD.SUD  "Spin Up Device"
 231      * PxCMD.POD  "Power On Device"
 232      * PxCMD.FRE  "FIS Receive Enable"
 233      * PxCMD.FR   "FIS Receive Running"
 234      * PxCMD.CR   "Command List Running"
 235      */
 236     uint32_t reg, ports_impl;
 237     uint16_t i;
 238     uint8_t num_cmd_slots;
 239
 240     g_assert(ahci != NULL);
 241
 242     /* Set GHC.AE to 1 */
 243     ahci_set(ahci, AHCI_GHC, AHCI_GHC_AE);
 244     reg = ahci_rreg(ahci, AHCI_GHC);
 245     ASSERT_BIT_SET(reg, AHCI_GHC_AE);
 246
 247     /* Cache CAP and CAP2. */
 248     ahci->cap = ahci_rreg(ahci, AHCI_CAP);
 249     ahci->cap2 = ahci_rreg(ahci, AHCI_CAP2);
 250
 251     /* Read CAP.NCS, how many command slots do we have? */
 252     num_cmd_slots = ((ahci->cap & AHCI_CAP_NCS) >> ctzl(AHCI_CAP_NCS)) + 1;
 253     g_test_message("Number of Command Slots: %u", num_cmd_slots);
 254
 255     /* Determine which ports are implemented. */
 256     ports_impl = ahci_rreg(ahci, AHCI_PI);
 257
 258     for (i = 0; ports_impl; ports_impl >>= 1, ++i) {
 259         if (!(ports_impl & 0x01)) {
 260             continue;
 261         }
 262
 263         g_test_message("Initializing port %u", i);
 264
 265         reg = ahci_px_rreg(ahci, i, AHCI_PX_CMD);
 266         if (BITCLR(reg, AHCI_PX_CMD_ST | AHCI_PX_CMD_CR |
 267                    AHCI_PX_CMD_FRE | AHCI_PX_CMD_FR)) {
 268             g_test_message("port is idle");
 269         } else {
 270             g_test_message("port needs to be idled");
 271             ahci_px_clr(ahci, i, AHCI_PX_CMD,
 272                         (AHCI_PX_CMD_ST | AHCI_PX_CMD_FRE));
 273             /* The port has 500ms to disengage. */
 274             usleep(500000);
 275             reg = ahci_px_rreg(ahci, i, AHCI_PX_CMD);
 276             ASSERT_BIT_CLEAR(reg, AHCI_PX_CMD_CR);
 277             ASSERT_BIT_CLEAR(reg, AHCI_PX_CMD_FR);
 278             g_test_message("port is now idle");
 279             /* The spec does allow for possibly needing a PORT RESET
 280              * or HBA reset if we fail to idle the port. */
 281         }
 282
 283         /* Allocate Memory for the Command List Buffer & FIS Buffer */
 284         /* PxCLB space ... 0x20 per command, as in 4.2.2 p 36 */
 285         ahci->port[i].clb = ahci_alloc(ahci, num_cmd_slots * 0x20);
 286         qmemset(ahci->port[i].clb, 0x00, num_cmd_slots * 0x20);
 287         g_test_message("CLB: 0x%08" PRIx64, ahci->port[i].clb);
 288         ahci_px_wreg(ahci, i, AHCI_PX_CLB, ahci->port[i].clb);
 289         g_assert_cmphex(ahci->port[i].clb, ==,
 290                         ahci_px_rreg(ahci, i, AHCI_PX_CLB));
 291
 292         /* PxFB space ... 0x100, as in 4.2.1 p 35 */
 293         ahci->port[i].fb = ahci_alloc(ahci, 0x100);
 294         qmemset(ahci->port[i].fb, 0x00, 0x100);
 295         g_test_message("FB: 0x%08" PRIx64, ahci->port[i].fb);
 296         ahci_px_wreg(ahci, i, AHCI_PX_FB, ahci->port[i].fb);
 297         g_assert_cmphex(ahci->port[i].fb, ==,
 298                         ahci_px_rreg(ahci, i, AHCI_PX_FB));
 299
 300         /* Clear PxSERR, PxIS, then IS.IPS[x] by writing '1's. */
 301         ahci_px_wreg(ahci, i, AHCI_PX_SERR, 0xFFFFFFFF);
 302         ahci_px_wreg(ahci, i, AHCI_PX_IS, 0xFFFFFFFF);
 303         ahci_wreg(ahci, AHCI_IS, (1 << i));
 304
 305         /* Verify Interrupts Cleared */
 306         reg = ahci_px_rreg(ahci, i, AHCI_PX_SERR);
 307         g_assert_cmphex(reg, ==, 0);
 308
 309         reg = ahci_px_rreg(ahci, i, AHCI_PX_IS);
 310         g_assert_cmphex(reg, ==, 0);
 311
 312         reg = ahci_rreg(ahci, AHCI_IS);
 313         ASSERT_BIT_CLEAR(reg, (1 << i));
 314
 315         /* Enable All Interrupts: */
 316         ahci_px_wreg(ahci, i, AHCI_PX_IE, 0xFFFFFFFF);
 317         reg = ahci_px_rreg(ahci, i, AHCI_PX_IE);
 318         g_assert_cmphex(reg, ==, ~((uint32_t)AHCI_PX_IE_RESERVED));
 319
 320         /* Enable the FIS Receive Engine. */
 321         ahci_px_set(ahci, i, AHCI_PX_CMD, AHCI_PX_CMD_FRE);
 322         reg = ahci_px_rreg(ahci, i, AHCI_PX_CMD);
 323         ASSERT_BIT_SET(reg, AHCI_PX_CMD_FR);
 324
 325         /* AHCI 1.3 spec: if !STS.BSY, !STS.DRQ and PxSSTS.DET indicates
 326          * physical presence, a device is present and may be started. However,
 327          * PxSERR.DIAG.X /may/ need to be cleared a priori. */
 328         reg = ahci_px_rreg(ahci, i, AHCI_PX_SERR);
 329         if (BITSET(reg, AHCI_PX_SERR_DIAG_X)) {
 330             ahci_px_set(ahci, i, AHCI_PX_SERR, AHCI_PX_SERR_DIAG_X);
 331         }
 332
 333         reg = ahci_px_rreg(ahci, i, AHCI_PX_TFD);
 334         if (BITCLR(reg, AHCI_PX_TFD_STS_BSY | AHCI_PX_TFD_STS_DRQ)) {
 335             reg = ahci_px_rreg(ahci, i, AHCI_PX_SSTS);
 336             if ((reg & AHCI_PX_SSTS_DET) == SSTS_DET_ESTABLISHED) {
 337                 /* Device Found: set PxCMD.ST := 1 */
 338                 ahci_px_set(ahci, i, AHCI_PX_CMD, AHCI_PX_CMD_ST);
 339                 ASSERT_BIT_SET(ahci_px_rreg(ahci, i, AHCI_PX_CMD),
 340                                AHCI_PX_CMD_CR);
 341                 g_test_message("Started Device %u", i);
 342             } else if ((reg & AHCI_PX_SSTS_DET)) {
 343                 /* Device present, but in some unknown state. */
 344                 g_assert_not_reached();
 345             }
 346         }
 347     }
 348
 349     /* Enable GHC.IE */
 350     ahci_set(ahci, AHCI_GHC, AHCI_GHC_IE);
 351     reg = ahci_rreg(ahci, AHCI_GHC);
 352     ASSERT_BIT_SET(reg, AHCI_GHC_IE);
 353
 354     ahci->enabled = true;
 355     /* TODO: The device should now be idling and waiting for commands.
 356      * In the future, a small test-case to inspect the Register D2H FIS
 357      * and clear the initial interrupts might be good. */
 358 }
 359
 360 /**
 361  * Pick the first implemented and running port
 362  */
 363 unsigned ahci_port_select(AHCIQState *ahci)
 364 {
 365     uint32_t ports, reg;
 366     unsigned i;
 367
 368     ports = ahci_rreg(ahci, AHCI_PI);
 369     for (i = 0; i < 32; ports >>= 1, ++i) {
 370         if (ports == 0) {
 371             i = 32;
 372         }
 373
 374         if (!(ports & 0x01)) {
 375             continue;
 376         }
 377
 378         reg = ahci_px_rreg(ahci, i, AHCI_PX_CMD);
 379         if (BITSET(reg, AHCI_PX_CMD_ST)) {
 380             break;
 381         }
 382     }
 383     g_assert(i < 32);
 384     return i;
 385 }
 386
 387 /**
 388  * Clear a port's interrupts and status information prior to a test.
 389  */
 390 void ahci_port_clear(AHCIQState *ahci, uint8_t port)
 391 {
 392     uint32_t reg;
 393
 394     /* Clear out this port's interrupts (ignore the init register d2h fis) */
 395     reg = ahci_px_rreg(ahci, port, AHCI_PX_IS);
 396     ahci_px_wreg(ahci, port, AHCI_PX_IS, reg);
 397     g_assert_cmphex(ahci_px_rreg(ahci, port, AHCI_PX_IS), ==, 0);
 398
 399     /* Wipe the FIS-Receive Buffer */
 400     qmemset(ahci->port[port].fb, 0x00, 0x100);
 401 }
 402
 403 /**
 404  * Check a port for errors.
 405  */
 406 void ahci_port_check_error(AHCIQState *ahci, uint8_t port,
 407                            uint32_t imask, uint8_t emask)
 408 {
 409     uint32_t reg;
 410
 411     /* The upper 9 bits of the IS register all indicate errors. */
 412     reg = ahci_px_rreg(ahci, port, AHCI_PX_IS);
 413     reg &= ~imask;
 414     reg >>= 23;
 415     g_assert_cmphex(reg, ==, 0);
 416
 417     /* The Sata Error Register should be empty. */
 418     reg = ahci_px_rreg(ahci, port, AHCI_PX_SERR);
 419     g_assert_cmphex(reg, ==, 0);
 420
 421     /* The TFD also has two error sections. */
 422     reg = ahci_px_rreg(ahci, port, AHCI_PX_TFD);
 423     if (!emask) {
 424         ASSERT_BIT_CLEAR(reg, AHCI_PX_TFD_STS_ERR);
 425     } else {
 426         ASSERT_BIT_SET(reg, AHCI_PX_TFD_STS_ERR);
 427     }
 428     ASSERT_BIT_CLEAR(reg, AHCI_PX_TFD_ERR & (~emask << 8));
 429     ASSERT_BIT_SET(reg, AHCI_PX_TFD_ERR & (emask << 8));
 430 }
 431
 432 void ahci_port_check_interrupts(AHCIQState *ahci, uint8_t port,
 433                                 uint32_t intr_mask)
 434 {
 435     uint32_t reg;
 436
 437     /* Check for expected interrupts */
 438     reg = ahci_px_rreg(ahci, port, AHCI_PX_IS);
 439     ASSERT_BIT_SET(reg, intr_mask);
 440
 441     /* Clear expected interrupts and assert all interrupts now cleared. */
 442     ahci_px_wreg(ahci, port, AHCI_PX_IS, intr_mask);
 443     g_assert_cmphex(ahci_px_rreg(ahci, port, AHCI_PX_IS), ==, 0);
 444 }
 445
 446 void ahci_port_check_nonbusy(AHCIQState *ahci, uint8_t port, uint8_t slot)
 447 {
 448     uint32_t reg;
 449
 450     /* Assert that the command slot is no longer busy (NCQ) */
 451     reg = ahci_px_rreg(ahci, port, AHCI_PX_SACT);
 452     ASSERT_BIT_CLEAR(reg, (1 << slot));
 453
 454     /* Non-NCQ */
 455     reg = ahci_px_rreg(ahci, port, AHCI_PX_CI);
 456     ASSERT_BIT_CLEAR(reg, (1 << slot));
 457
 458     /* And assert that we are generally not busy. */
 459     reg = ahci_px_rreg(ahci, port, AHCI_PX_TFD);
 460     ASSERT_BIT_CLEAR(reg, AHCI_PX_TFD_STS_BSY);
 461     ASSERT_BIT_CLEAR(reg, AHCI_PX_TFD_STS_DRQ);
 462 }
 463
 464 void ahci_port_check_d2h_sanity(AHCIQState *ahci, uint8_t port, uint8_t slot)
 465 {
 466     RegD2HFIS *d2h = g_malloc0(0x20);
 467     uint32_t reg;
 468
 469     memread(ahci->port[port].fb + 0x40, d2h, 0x20);
 470     g_assert_cmphex(d2h->fis_type, ==, 0x34);
 471
 472     reg = ahci_px_rreg(ahci, port, AHCI_PX_TFD);
 473     g_assert_cmphex((reg & AHCI_PX_TFD_ERR) >> 8, ==, d2h->error);
 474     g_assert_cmphex((reg & AHCI_PX_TFD_STS), ==, d2h->status);
 475
 476     g_free(d2h);
 477 }
 478
 479 void ahci_port_check_pio_sanity(AHCIQState *ahci, uint8_t port,
 480                                 uint8_t slot, size_t buffsize)
 481 {
 482     PIOSetupFIS *pio = g_malloc0(0x20);
 483
 484     /* We cannot check the Status or E_Status registers, because
 485      * the status may have again changed between the PIO Setup FIS
 486      * and the conclusion of the command with the D2H Register FIS. */
 487     memread(ahci->port[port].fb + 0x20, pio, 0x20);
 488     g_assert_cmphex(pio->fis_type, ==, 0x5f);
 489
 490     /* BUG: PIO Setup FIS as utilized by QEMU tries to fit the entire
 491      * transfer size in a uint16_t field. The maximum transfer size can
 492      * eclipse this; the field is meant to convey the size of data per
 493      * each Data FIS, not the entire operation as a whole. For now,
 494      * we will sanity check the broken case where applicable. */
 495     if (buffsize <= UINT16_MAX) {
 496         g_assert_cmphex(le16_to_cpu(pio->tx_count), ==, buffsize);
 497     }
 498
 499     g_free(pio);
 500 }
 501
 502 void ahci_port_check_cmd_sanity(AHCIQState *ahci, AHCICommand *cmd)
 503 {
 504     AHCICommandHeader cmdh;
 505
 506     ahci_get_command_header(ahci, cmd->port, cmd->slot, &cmdh);
 507     /* Physical Region Descriptor Byte Count is not required to work for NCQ. */
 508     if (!cmd->props->ncq) {
 509         g_assert_cmphex(cmd->xbytes, ==, cmdh.prdbc);
 510     }
 511 }
 512
 513 /* Get the command in #slot of port #port. */
 514 void ahci_get_command_header(AHCIQState *ahci, uint8_t port,
 515                              uint8_t slot, AHCICommandHeader *cmd)
 516 {
 517     uint64_t ba = ahci->port[port].clb;
 518     ba += slot * sizeof(AHCICommandHeader);
 519     memread(ba, cmd, sizeof(AHCICommandHeader));
 520
 521     cmd->flags = le16_to_cpu(cmd->flags);
 522     cmd->prdtl = le16_to_cpu(cmd->prdtl);
 523     cmd->prdbc = le32_to_cpu(cmd->prdbc);
 524     cmd->ctba = le64_to_cpu(cmd->ctba);
 525 }
 526
 527 /* Set the command in #slot of port #port. */
 528 void ahci_set_command_header(AHCIQState *ahci, uint8_t port,
 529                              uint8_t slot, AHCICommandHeader *cmd)
 530 {
 531     AHCICommandHeader tmp = { .flags = 0 };
 532     uint64_t ba = ahci->port[port].clb;
 533     ba += slot * sizeof(AHCICommandHeader);
 534
 535     tmp.flags = cpu_to_le16(cmd->flags);
 536     tmp.prdtl = cpu_to_le16(cmd->prdtl);
 537     tmp.prdbc = cpu_to_le32(cmd->prdbc);
 538     tmp.ctba = cpu_to_le64(cmd->ctba);
 539
 540     memwrite(ba, &tmp, sizeof(AHCICommandHeader));
 541 }
 542
 543 void ahci_destroy_command(AHCIQState *ahci, uint8_t port, uint8_t slot)
 544 {
 545     AHCICommandHeader cmd;
 546
 547     /* Obtain the Nth Command Header */
 548     ahci_get_command_header(ahci, port, slot, &cmd);
 549     if (cmd.ctba == 0) {
 550         /* No address in it, so just return -- it's empty. */
 551         goto tidy;
 552     }
 553
 554     /* Free the Table */
 555     ahci_free(ahci, cmd.ctba);
 556
 557  tidy:
 558     /* NULL the header. */
 559     memset(&cmd, 0x00, sizeof(cmd));
 560     ahci_set_command_header(ahci, port, slot, &cmd);
 561     ahci->port[port].ctba[slot] = 0;
 562     ahci->port[port].prdtl[slot] = 0;
 563 }
 564
 565 void ahci_write_fis(AHCIQState *ahci, AHCICommand *cmd)
 566 {
 567     RegH2DFIS tmp = cmd->fis;
 568     uint64_t addr = cmd->header.ctba;
 569
 570     /* NCQ commands use exclusively 8 bit fields and needs no adjustment.
 571      * Only the count field needs to be adjusted for non-NCQ commands.
 572      * The auxiliary FIS fields are defined per-command and are not currently
 573      * implemented in libqos/ahci.o, but may or may not need to be flipped. */
 574     if (!cmd->props->ncq) {
 575         tmp.count = cpu_to_le16(tmp.count);
 576     }
 577
 578     memwrite(addr, &tmp, sizeof(tmp));
 579 }
 580
 581 unsigned ahci_pick_cmd(AHCIQState *ahci, uint8_t port)
 582 {
 583     unsigned i;
 584     unsigned j;
 585     uint32_t reg;
 586
 587     reg = ahci_px_rreg(ahci, port, AHCI_PX_CI);
 588
 589     /* Pick the least recently used command slot that's available */
 590     for (i = 0; i < 32; ++i) {
 591         j = ((ahci->port[port].next + i) % 32);
 592         if (reg & (1 << j)) {
 593             continue;
 594         }
 595         ahci_destroy_command(ahci, port, j);
 596         ahci->port[port].next = (j + 1) % 32;
 597         return j;
 598     }
 599
 600     g_test_message("All command slots were busy.");
 601     g_assert_not_reached();
 602 }
 603
 604 inline unsigned size_to_prdtl(unsigned bytes, unsigned bytes_per_prd)
 605 {
 606     /* Each PRD can describe up to 4MiB */
 607     g_assert_cmphex(bytes_per_prd, <=, 4096 * 1024);
 608     g_assert_cmphex(bytes_per_prd & 0x01, ==, 0x00);
 609     return (bytes + bytes_per_prd - 1) / bytes_per_prd;
 610 }
 611
 612 const AHCIOpts default_opts = { .size = 0 };
 613
 614 /**
 615  * ahci_exec: execute a given command on a specific
 616  * AHCI port.
 617  *
 618  * @ahci: The device to send the command to
 619  * @port: The port number of the SATA device we wish
 620  *        to have execute this command
 621  * @op:   The S/ATA command to execute, or if opts.atapi
 622  *        is true, the SCSI command code.
 623  * @opts: Optional arguments to modify execution behavior.
 624  */
 625 void ahci_exec(AHCIQState *ahci, uint8_t port,
 626                uint8_t op, const AHCIOpts *opts_in)
 627 {
 628     AHCICommand *cmd;
 629     int rc;
 630     AHCIOpts *opts;
 631
 632     opts = g_memdup((opts_in == NULL ? &default_opts : opts_in),
 633                     sizeof(AHCIOpts));
 634
 635     /* No guest buffer provided, create one. */
 636     if (opts->size && !opts->buffer) {
 637         opts->buffer = ahci_alloc(ahci, opts->size);
 638         g_assert(opts->buffer);
 639         qmemset(opts->buffer, 0x00, opts->size);
 640     }
 641
 642     /* Command creation */
 643     if (opts->atapi) {
 644         uint16_t bcl = opts->set_bcl ? opts->bcl : ATAPI_SECTOR_SIZE;
 645         cmd = ahci_atapi_command_create(op, bcl);
 646         if (opts->atapi_dma) {
 647             ahci_command_enable_atapi_dma(cmd);
 648         }
 649     } else {
 650         cmd = ahci_command_create(op);
 651     }
 652     ahci_command_adjust(cmd, opts->lba, opts->buffer,
 653                         opts->size, opts->prd_size);
 654
 655     if (opts->pre_cb) {
 656         rc = opts->pre_cb(ahci, cmd, opts);
 657         g_assert_cmpint(rc, ==, 0);
 658     }
 659
 660     /* Write command to memory and issue it */
 661     ahci_command_commit(ahci, cmd, port);
 662     ahci_command_issue_async(ahci, cmd);
 663     if (opts->error) {
 664         qmp_eventwait("STOP");
 665     }
 666     if (opts->mid_cb) {
 667         rc = opts->mid_cb(ahci, cmd, opts);
 668         g_assert_cmpint(rc, ==, 0);
 669     }
 670     if (opts->error) {
 671         qmp_async("{'execute':'cont' }");
 672         qmp_eventwait("RESUME");
 673     }
 674
 675     /* Wait for command to complete and verify sanity */
 676     ahci_command_wait(ahci, cmd);
 677     ahci_command_verify(ahci, cmd);
 678     if (opts->post_cb) {
 679         rc = opts->post_cb(ahci, cmd, opts);
 680         g_assert_cmpint(rc, ==, 0);
 681     }
 682     ahci_command_free(cmd);
 683     if (opts->buffer != opts_in->buffer) {
 684         ahci_free(ahci, opts->buffer);
 685     }
 686     g_free(opts);
 687 }
 688
 689 /* Issue a command, expecting it to fail and STOP the VM */
 690 AHCICommand *ahci_guest_io_halt(AHCIQState *ahci, uint8_t port,
 691                                 uint8_t ide_cmd, uint64_t buffer,
 692                                 size_t bufsize, uint64_t sector)
 693 {
 694     AHCICommand *cmd;
 695
 696     cmd = ahci_command_create(ide_cmd);
 697     ahci_command_adjust(cmd, sector, buffer, bufsize, 0);
 698     ahci_command_commit(ahci, cmd, port);
 699     ahci_command_issue_async(ahci, cmd);
 700     qmp_eventwait("STOP");
 701
 702     return cmd;
 703 }
 704
 705 /* Resume a previously failed command and verify/finalize */
 706 void ahci_guest_io_resume(AHCIQState *ahci, AHCICommand *cmd)
 707 {
 708     /* Complete the command */
 709     qmp_async("{'execute':'cont' }");
 710     qmp_eventwait("RESUME");
 711     ahci_command_wait(ahci, cmd);
 712     ahci_command_verify(ahci, cmd);
 713     ahci_command_free(cmd);
 714 }
 715
 716 /* Given a guest buffer address, perform an IO operation */
 717 void ahci_guest_io(AHCIQState *ahci, uint8_t port, uint8_t ide_cmd,
 718                    uint64_t buffer, size_t bufsize, uint64_t sector)
 719 {
 720     AHCICommand *cmd;
 721     cmd = ahci_command_create(ide_cmd);
 722     ahci_command_set_buffer(cmd, buffer);
 723     ahci_command_set_size(cmd, bufsize);
 724     if (sector) {
 725         ahci_command_set_offset(cmd, sector);
 726     }
 727     ahci_command_commit(ahci, cmd, port);
 728     ahci_command_issue(ahci, cmd);
 729     ahci_command_verify(ahci, cmd);
 730     ahci_command_free(cmd);
 731 }
 732
 733 static AHCICommandProp *ahci_command_find(uint8_t command_name)
 734 {
 735     int i;
 736
 737     for (i = 0; i < ARRAY_SIZE(ahci_command_properties); i++) {
 738         if (ahci_command_properties[i].cmd == command_name) {
 739             return &ahci_command_properties[i];
 740         }
 741     }
 742
 743     return NULL;
 744 }
 745
 746 /* Given a HOST buffer, create a buffer address and perform an IO operation. */
 747 void ahci_io(AHCIQState *ahci, uint8_t port, uint8_t ide_cmd,
 748              void *buffer, size_t bufsize, uint64_t sector)
 749 {
 750     uint64_t ptr;
 751     AHCICommandProp *props;
 752
 753     props = ahci_command_find(ide_cmd);
 754     g_assert(props);
 755     ptr = ahci_alloc(ahci, bufsize);
 756     g_assert(!bufsize || ptr);
 757     qmemset(ptr, 0x00, bufsize);
 758
 759     if (bufsize && props->write) {
 760         bufwrite(ptr, buffer, bufsize);
 761     }
 762
 763     ahci_guest_io(ahci, port, ide_cmd, ptr, bufsize, sector);
 764
 765     if (bufsize && props->read) {
 766         bufread(ptr, buffer, bufsize);
 767     }
 768
 769     ahci_free(ahci, ptr);
 770 }
 771
 772 /**
 773  * Initializes a basic command header in memory.
 774  * We assume that this is for an ATA command using RegH2DFIS.
 775  */
 776 static void command_header_init(AHCICommand *cmd)
 777 {
 778     AHCICommandHeader *hdr = &cmd->header;
 779     AHCICommandProp *props = cmd->props;
 780
 781     hdr->flags = 5;             /* RegH2DFIS is 5 DW long. Must be < 32 */
 782     hdr->flags |= CMDH_CLR_BSY; /* Clear the BSY bit when done */
 783     if (props->write) {
 784         hdr->flags |= CMDH_WRITE;
 785     }
 786     if (props->atapi) {
 787         hdr->flags |= CMDH_ATAPI;
 788     }
 789     /* Other flags: PREFETCH, RESET, and BIST */
 790     hdr->prdtl = size_to_prdtl(cmd->xbytes, cmd->prd_size);
 791     hdr->prdbc = 0;
 792     hdr->ctba = 0;
 793 }
 794
 795 static void command_table_init(AHCICommand *cmd)
 796 {
 797     RegH2DFIS *fis = &(cmd->fis);
 798     uint16_t sect_count = (cmd->xbytes / AHCI_SECTOR_SIZE);
 799
 800     fis->fis_type = REG_H2D_FIS;
 801     fis->flags = REG_H2D_FIS_CMD; /* "Command" bit */
 802     fis->command = cmd->name;
 803
 804     if (cmd->props->ncq) {
 805         NCQFIS *ncqfis = (NCQFIS *)fis;
 806         /* NCQ is weird and re-uses FIS frames for unrelated data.
 807          * See SATA 3.2, 13.6.4.1 READ FPDMA QUEUED for an example. */
 808         ncqfis->sector_low = sect_count & 0xFF;
 809         ncqfis->sector_hi = (sect_count >> 8) & 0xFF;
 810         ncqfis->device = NCQ_DEVICE_MAGIC;
 811         /* Force Unit Access is bit 7 in the device register */
 812         ncqfis->tag = 0;  /* bits 3-7 are the NCQ tag */
 813         ncqfis->prio = 0; /* bits 6,7 are a prio tag */
 814         /* RARC bit is bit 0 of TAG field */
 815     } else {
 816         fis->feature_low = 0x00;
 817         fis->feature_high = 0x00;
 818         if (cmd->props->lba28 || cmd->props->lba48) {
 819             fis->device = ATA_DEVICE_LBA;
 820         }
 821         fis->count = (cmd->xbytes / AHCI_SECTOR_SIZE);
 822     }
 823     fis->icc = 0x00;
 824     fis->control = 0x00;
 825     memset(fis->aux, 0x00, ARRAY_SIZE(fis->aux));
 826 }
 827
 828 void ahci_command_enable_atapi_dma(AHCICommand *cmd)
 829 {
 830     RegH2DFIS *fis = &(cmd->fis);
 831     g_assert(cmd->props->atapi);
 832     fis->feature_low |= 0x01;
 833     cmd->interrupts &= ~AHCI_PX_IS_PSS;
 834     cmd->props->dma = true;
 835     cmd->props->pio = false;
 836     /* BUG: We expect the DMA Setup interrupt for DMA commands */
 837     /* cmd->interrupts |= AHCI_PX_IS_DSS; */
 838 }
 839
 840 AHCICommand *ahci_command_create(uint8_t command_name)
 841 {
 842     AHCICommandProp *props = ahci_command_find(command_name);
 843     AHCICommand *cmd;
 844
 845     g_assert(props);
 846     cmd = g_malloc0(sizeof(AHCICommand));
 847     g_assert(!(props->dma && props->pio));
 848     g_assert(!(props->lba28 && props->lba48));
 849     g_assert(!(props->read && props->write));
 850     g_assert(!props->size || props->data);
 851     g_assert(!props->ncq || props->lba48);
 852
 853     /* Defaults and book-keeping */
 854     cmd->props = g_memdup(props, sizeof(AHCICommandProp));
 855     cmd->name = command_name;
 856     cmd->xbytes = props->size;
 857     cmd->prd_size = 4096;
 858     cmd->buffer = 0xabad1dea;
 859
 860     if (!cmd->props->ncq) {
 861         cmd->interrupts = AHCI_PX_IS_DHRS;
 862     }
 863     /* BUG: We expect the DPS interrupt for data commands */
 864     /* cmd->interrupts |= props->data ? AHCI_PX_IS_DPS : 0; */
 865     /* BUG: We expect the DMA Setup interrupt for DMA commands */
 866     /* cmd->interrupts |= props->dma ? AHCI_PX_IS_DSS : 0; */
 867     cmd->interrupts |= props->pio ? AHCI_PX_IS_PSS : 0;
 868     cmd->interrupts |= props->ncq ? AHCI_PX_IS_SDBS : 0;
 869
 870     command_header_init(cmd);
 871     command_table_init(cmd);
 872
 873     return cmd;
 874 }
 875
 876 AHCICommand *ahci_atapi_command_create(uint8_t scsi_cmd, uint16_t bcl)
 877 {
 878     AHCICommand *cmd = ahci_command_create(CMD_PACKET);
 879     cmd->atapi_cmd = g_malloc0(16);
 880     cmd->atapi_cmd[0] = scsi_cmd;
 881     stw_le_p(&cmd->fis.lba_lo[1], bcl);
 882     return cmd;
 883 }
 884
 885 void ahci_atapi_test_ready(AHCIQState *ahci, uint8_t port,
 886                            bool ready, uint8_t expected_sense)
 887 {
 888     AHCICommand *cmd = ahci_atapi_command_create(CMD_ATAPI_TEST_UNIT_READY, 0);
 889     ahci_command_set_size(cmd, 0);
 890     if (!ready) {
 891         cmd->interrupts |= AHCI_PX_IS_TFES;
 892         cmd->errors |= expected_sense << 4;
 893     }
 894     ahci_command_commit(ahci, cmd, port);
 895     ahci_command_issue(ahci, cmd);
 896     ahci_command_verify(ahci, cmd);
 897     ahci_command_free(cmd);
 898 }
 899
 900 static int copy_buffer(AHCIQState *ahci, AHCICommand *cmd,
 901                         const AHCIOpts *opts)
 902 {
 903     unsigned char *rx = opts->opaque;
 904     bufread(opts->buffer, rx, opts->size);
 905     return 0;
 906 }
 907
 908 void ahci_atapi_get_sense(AHCIQState *ahci, uint8_t port,
 909                           uint8_t *sense, uint8_t *asc)
 910 {
 911     unsigned char *rx;
 912     AHCIOpts opts = {
 913         .size = 18,
 914         .atapi = true,
 915         .post_cb = copy_buffer,
 916     };
 917     rx = g_malloc(18);
 918     opts.opaque = rx;
 919
 920     ahci_exec(ahci, port, CMD_ATAPI_REQUEST_SENSE, &opts);
 921
 922     *sense = rx[2];
 923     *asc = rx[12];
 924
 925     g_free(rx);
 926 }
 927
 928 void ahci_atapi_eject(AHCIQState *ahci, uint8_t port)
 929 {
 930     AHCICommand *cmd = ahci_atapi_command_create(CMD_ATAPI_START_STOP_UNIT, 0);
 931     ahci_command_set_size(cmd, 0);
 932
 933     cmd->atapi_cmd[4] = 0x02; /* loej = true */
 934     ahci_command_commit(ahci, cmd, port);
 935     ahci_command_issue(ahci, cmd);
 936     ahci_command_verify(ahci, cmd);
 937     ahci_command_free(cmd);
 938 }
 939
 940 void ahci_atapi_load(AHCIQState *ahci, uint8_t port)
 941 {
 942     AHCICommand *cmd = ahci_atapi_command_create(CMD_ATAPI_START_STOP_UNIT, 0);
 943     ahci_command_set_size(cmd, 0);
 944
 945     cmd->atapi_cmd[4] = 0x03; /* loej,start = true */
 946     ahci_command_commit(ahci, cmd, port);
 947     ahci_command_issue(ahci, cmd);
 948     ahci_command_verify(ahci, cmd);
 949     ahci_command_free(cmd);
 950 }
 951
 952 void ahci_command_free(AHCICommand *cmd)
 953 {
 954     g_free(cmd->atapi_cmd);
 955     g_free(cmd->props);
 956     g_free(cmd);
 957 }
 958
 959 void ahci_command_set_flags(AHCICommand *cmd, uint16_t cmdh_flags)
 960 {
 961     cmd->header.flags |= cmdh_flags;
 962 }
 963
 964 void ahci_command_clr_flags(AHCICommand *cmd, uint16_t cmdh_flags)
 965 {
 966     cmd->header.flags &= ~cmdh_flags;
 967 }
 968
 969 static void ahci_atapi_command_set_offset(AHCICommand *cmd, uint64_t lba)
 970 {
 971     unsigned char *cbd = cmd->atapi_cmd;
 972     g_assert(cbd);
 973
 974     switch (cbd[0]) {
 975     case CMD_ATAPI_READ_10:
 976     case CMD_ATAPI_READ_CD:
 977         g_assert_cmpuint(lba, <=, UINT32_MAX);
 978         stl_be_p(&cbd[2], lba);
 979         break;
 980     case CMD_ATAPI_REQUEST_SENSE:
 981     case CMD_ATAPI_TEST_UNIT_READY:
 982     case CMD_ATAPI_START_STOP_UNIT:
 983         g_assert_cmpuint(lba, ==, 0x00);
 984         break;
 985     default:
 986         /* SCSI doesn't have uniform packet formats,
 987          * so you have to add support for it manually. Sorry! */
 988         fprintf(stderr, "The Libqos AHCI driver does not support the "
 989                 "set_offset operation for ATAPI command 0x%02x, "
 990                 "please add support.\n",
 991                 cbd[0]);
 992         g_assert_not_reached();
 993     }
 994 }
 995
 996 void ahci_command_set_offset(AHCICommand *cmd, uint64_t lba_sect)
 997 {
 998     RegH2DFIS *fis = &(cmd->fis);
 999
1000     if (cmd->props->atapi) {
1001         ahci_atapi_command_set_offset(cmd, lba_sect);
1002         return;
1003     } else if (!cmd->props->data && !lba_sect) {
1004         /* Not meaningful, ignore. */
1005         return;
1006     } else if (cmd->props->lba28) {
1007         g_assert_cmphex(lba_sect, <=, 0xFFFFFFF);
1008     } else if (cmd->props->lba48 || cmd->props->ncq) {
1009         g_assert_cmphex(lba_sect, <=, 0xFFFFFFFFFFFF);
1010     } else {
1011         /* Can't set offset if we don't know the format. */
1012         g_assert_not_reached();
1013     }
1014
1015     /* LBA28 uses the low nibble of the device/control register for LBA24:27 */
1016     fis->lba_lo[0] = (lba_sect & 0xFF);
1017     fis->lba_lo[1] = (lba_sect >> 8) & 0xFF;
1018     fis->lba_lo[2] = (lba_sect >> 16) & 0xFF;
1019     if (cmd->props->lba28) {
1020         fis->device = (fis->device & 0xF0) | ((lba_sect >> 24) & 0x0F);
1021     }
1022     fis->lba_hi[0] = (lba_sect >> 24) & 0xFF;
1023     fis->lba_hi[1] = (lba_sect >> 32) & 0xFF;
1024     fis->lba_hi[2] = (lba_sect >> 40) & 0xFF;
1025 }
1026
1027 void ahci_command_set_buffer(AHCICommand *cmd, uint64_t buffer)
1028 {
1029     cmd->buffer = buffer;
1030 }
1031
1032 static void ahci_atapi_set_size(AHCICommand *cmd, uint64_t xbytes)
1033 {
1034     unsigned char *cbd = cmd->atapi_cmd;
1035     uint64_t nsectors = xbytes / 2048;
1036     uint32_t tmp;
1037     g_assert(cbd);
1038
1039     switch (cbd[0]) {
1040     case CMD_ATAPI_READ_10:
1041         g_assert_cmpuint(nsectors, <=, UINT16_MAX);
1042         stw_be_p(&cbd[7], nsectors);
1043         break;
1044     case CMD_ATAPI_READ_CD:
1045         /* 24bit BE store */
1046         g_assert_cmpuint(nsectors, <, 1ULL << 24);
1047         tmp = nsectors;
1048         cbd[6] = (tmp & 0xFF0000) >> 16;
1049         cbd[7] = (tmp & 0xFF00) >> 8;
1050         cbd[8] = (tmp & 0xFF);
1051         break;
1052     case CMD_ATAPI_REQUEST_SENSE:
1053         g_assert_cmpuint(xbytes, <=, UINT8_MAX);
1054         cbd[4] = (uint8_t)xbytes;
1055         break;
1056     case CMD_ATAPI_TEST_UNIT_READY:
1057     case CMD_ATAPI_START_STOP_UNIT:
1058         g_assert_cmpuint(xbytes, ==, 0);
1059         break;
1060     default:
1061         /* SCSI doesn't have uniform packet formats,
1062          * so you have to add support for it manually. Sorry! */
1063         fprintf(stderr, "The Libqos AHCI driver does not support the set_size "
1064                 "operation for ATAPI command 0x%02x, please add support.\n",
1065                 cbd[0]);
1066         g_assert_not_reached();
1067     }
1068 }
1069
1070 void ahci_command_set_sizes(AHCICommand *cmd, uint64_t xbytes,
1071                             unsigned prd_size)
1072 {
1073     uint16_t sect_count;
1074
1075     /* Each PRD can describe up to 4MiB, and must not be odd. */
1076     g_assert_cmphex(prd_size, <=, 4096 * 1024);
1077     g_assert_cmphex(prd_size & 0x01, ==, 0x00);
1078     if (prd_size) {
1079         cmd->prd_size = prd_size;
1080     }
1081     cmd->xbytes = xbytes;
1082     sect_count = (cmd->xbytes / AHCI_SECTOR_SIZE);
1083
1084     if (cmd->props->ncq) {
1085         NCQFIS *nfis = (NCQFIS *)&(cmd->fis);
1086         nfis->sector_low = sect_count & 0xFF;
1087         nfis->sector_hi = (sect_count >> 8) & 0xFF;
1088     } else if (cmd->props->atapi) {
1089         ahci_atapi_set_size(cmd, xbytes);
1090     } else {
1091         cmd->fis.count = sect_count;
1092     }
1093     cmd->header.prdtl = size_to_prdtl(cmd->xbytes, cmd->prd_size);
1094 }
1095
1096 void ahci_command_set_size(AHCICommand *cmd, uint64_t xbytes)
1097 {
1098     ahci_command_set_sizes(cmd, xbytes, cmd->prd_size);
1099 }
1100
1101 void ahci_command_set_prd_size(AHCICommand *cmd, unsigned prd_size)
1102 {
1103     ahci_command_set_sizes(cmd, cmd->xbytes, prd_size);
1104 }
1105
1106 void ahci_command_adjust(AHCICommand *cmd, uint64_t offset, uint64_t buffer,
1107                          uint64_t xbytes, unsigned prd_size)
1108 {
1109     ahci_command_set_sizes(cmd, xbytes, prd_size);
1110     ahci_command_set_buffer(cmd, buffer);
1111     ahci_command_set_offset(cmd, offset);
1112 }
1113
1114 void ahci_command_commit(AHCIQState *ahci, AHCICommand *cmd, uint8_t port)
1115 {
1116     uint16_t i, prdtl;
1117     uint64_t table_size, table_ptr, remaining;
1118     PRD prd;
1119
1120     /* This command is now tied to this port/command slot */
1121     cmd->port = port;
1122     cmd->slot = ahci_pick_cmd(ahci, port);
1123
1124     if (cmd->props->ncq) {
1125         NCQFIS *nfis = (NCQFIS *)&cmd->fis;
1126         nfis->tag = (cmd->slot << 3) & 0xFC;
1127     }
1128
1129     /* Create a buffer for the command table */
1130     prdtl = size_to_prdtl(cmd->xbytes, cmd->prd_size);
1131     table_size = CMD_TBL_SIZ(prdtl);
1132     table_ptr = ahci_alloc(ahci, table_size);
1133     g_assert(table_ptr);
1134     /* AHCI 1.3: Must be aligned to 0x80 */
1135     g_assert((table_ptr & 0x7F) == 0x00);
1136     cmd->header.ctba = table_ptr;
1137
1138     /* Commit the command header (part of the Command List Buffer) */
1139     ahci_set_command_header(ahci, port, cmd->slot, &(cmd->header));
1140     /* Now, write the command table (FIS, ACMD, and PRDT) -- FIS first, */
1141     ahci_write_fis(ahci, cmd);
1142     /* Then ATAPI CMD, if needed */
1143     if (cmd->props->atapi) {
1144         memwrite(table_ptr + 0x40, cmd->atapi_cmd, 16);
1145     }
1146
1147     /* Construct and write the PRDs to the command table */
1148     g_assert_cmphex(prdtl, ==, cmd->header.prdtl);
1149     remaining = cmd->xbytes;
1150     for (i = 0; i < prdtl; ++i) {
1151         prd.dba = cpu_to_le64(cmd->buffer + (cmd->prd_size * i));
1152         prd.res = 0;
1153         if (remaining > cmd->prd_size) {
1154             /* Note that byte count is 0-based. */
1155             prd.dbc = cpu_to_le32(cmd->prd_size - 1);
1156             remaining -= cmd->prd_size;
1157         } else {
1158             /* Again, dbc is 0-based. */
1159             prd.dbc = cpu_to_le32(remaining - 1);
1160             remaining = 0;
1161         }
1162         prd.dbc |= cpu_to_le32(0x80000000); /* Request DPS Interrupt */
1163
1164         /* Commit the PRD entry to the Command Table */
1165         memwrite(table_ptr + 0x80 + (i * sizeof(PRD)),
1166                  &prd, sizeof(PRD));
1167     }
1168
1169     /* Bookmark the PRDTL and CTBA values */
1170     ahci->port[port].ctba[cmd->slot] = table_ptr;
1171     ahci->port[port].prdtl[cmd->slot] = prdtl;
1172 }
1173
1174 void ahci_command_issue_async(AHCIQState *ahci, AHCICommand *cmd)
1175 {
1176     if (cmd->props->ncq) {
1177         ahci_px_wreg(ahci, cmd->port, AHCI_PX_SACT, (1 << cmd->slot));
1178     }
1179
1180     ahci_px_wreg(ahci, cmd->port, AHCI_PX_CI, (1 << cmd->slot));
1181 }
1182
1183 void ahci_command_wait(AHCIQState *ahci, AHCICommand *cmd)
1184 {
1185     /* We can't rely on STS_BSY until the command has started processing.
1186      * Therefore, we also use the Command Issue bit as indication of
1187      * a command in-flight. */
1188
1189 #define RSET(REG, MASK) (BITSET(ahci_px_rreg(ahci, cmd->port, (REG)), (MASK)))
1190
1191     while (RSET(AHCI_PX_TFD, AHCI_PX_TFD_STS_BSY) ||
1192            RSET(AHCI_PX_CI, 1 << cmd->slot) ||
1193            (cmd->props->ncq && RSET(AHCI_PX_SACT, 1 << cmd->slot))) {
1194         usleep(50);
1195     }
1196
1197 }
1198
1199 void ahci_command_issue(AHCIQState *ahci, AHCICommand *cmd)
1200 {
1201     ahci_command_issue_async(ahci, cmd);
1202     ahci_command_wait(ahci, cmd);
1203 }
1204
1205 void ahci_command_verify(AHCIQState *ahci, AHCICommand *cmd)
1206 {
1207     uint8_t slot = cmd->slot;
1208     uint8_t port = cmd->port;
1209
1210     ahci_port_check_error(ahci, port, cmd->interrupts, cmd->errors);
1211     ahci_port_check_interrupts(ahci, port, cmd->interrupts);
1212     ahci_port_check_nonbusy(ahci, port, slot);
1213     ahci_port_check_cmd_sanity(ahci, cmd);
1214     if (cmd->interrupts & AHCI_PX_IS_DHRS) {
1215         ahci_port_check_d2h_sanity(ahci, port, slot);
1216     }
1217     if (cmd->props->pio) {
1218         ahci_port_check_pio_sanity(ahci, port, slot, cmd->xbytes);
1219     }
1220 }
1221
1222 uint8_t ahci_command_slot(AHCICommand *cmd)
1223 {
1224     return cmd->slot;
1225 }