usr.sbin/bhyve/pci_ahci.c

   1 /*-
   2  * Copyright (c) 2013  Zhixiang Yu <zcore@freebsd.org>
   3  * All rights reserved.
   4  *
   5  * Redistribution and use in source and binary forms, with or without
   6  * modification, are permitted provided that the following conditions
   7  * are met:
   8  * 1. Redistributions of source code must retain the above copyright
   9  *    notice, this list of conditions and the following disclaimer.
  10  * 2. Redistributions in binary form must reproduce the above copyright
  11  *    notice, this list of conditions and the following disclaimer in the
  12  *    documentation and/or other materials provided with the distribution.
  13  *
  14  * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND
  15  * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
  16  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
  17  * ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
  18  * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
  19  * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
  20  * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
  21  * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
  22  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
  23  * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
  24  * SUCH DAMAGE.
  25  *
  26  * $FreeBSD$
  27  */
  28
  29 #include <sys/cdefs.h>
  30 __FBSDID("$FreeBSD$");
  31
  32 #include <sys/param.h>
  33 #include <sys/linker_set.h>
  34 #include <sys/stat.h>
  35 #include <sys/uio.h>
  36 #include <sys/ioctl.h>
  37 #include <sys/disk.h>
  38 #include <sys/ata.h>
  39 #include <sys/endian.h>
  40
  41 #include <errno.h>
  42 #include <fcntl.h>
  43 #include <stdio.h>
  44 #include <stdlib.h>
  45 #include <stdint.h>
  46 #include <string.h>
  47 #include <strings.h>
  48 #include <unistd.h>
  49 #include <assert.h>
  50 #include <pthread.h>
  51 #include <pthread_np.h>
  52 #include <inttypes.h>
  53
  54 #include "bhyverun.h"
  55 #include "pci_emul.h"
  56 #include "ahci.h"
  57 #include "block_if.h"
  58
  59 #define MAX_PORTS       6       /* Intel ICH8 AHCI supports 6 ports */
  60
  61 #define PxSIG_ATA       0x00000101 /* ATA drive */
  62 #define PxSIG_ATAPI     0xeb140101 /* ATAPI drive */
  63
  64 enum sata_fis_type {
  65         FIS_TYPE_REGH2D         = 0x27, /* Register FIS - host to device */
  66         FIS_TYPE_REGD2H         = 0x34, /* Register FIS - device to host */
  67         FIS_TYPE_DMAACT         = 0x39, /* DMA activate FIS - device to host */
  68         FIS_TYPE_DMASETUP       = 0x41, /* DMA setup FIS - bidirectional */
  69         FIS_TYPE_DATA           = 0x46, /* Data FIS - bidirectional */
  70         FIS_TYPE_BIST           = 0x58, /* BIST activate FIS - bidirectional */
  71         FIS_TYPE_PIOSETUP       = 0x5F, /* PIO setup FIS - device to host */
  72         FIS_TYPE_SETDEVBITS     = 0xA1, /* Set dev bits FIS - device to host */
  73 };
  74
  75 /*
  76  * SCSI opcodes
  77  */
  78 #define TEST_UNIT_READY         0x00
  79 #define REQUEST_SENSE           0x03
  80 #define INQUIRY                 0x12
  81 #define START_STOP_UNIT         0x1B
  82 #define PREVENT_ALLOW           0x1E
  83 #define READ_CAPACITY           0x25
  84 #define READ_10                 0x28
  85 #define POSITION_TO_ELEMENT     0x2B
  86 #define READ_TOC                0x43
  87 #define GET_EVENT_STATUS_NOTIFICATION 0x4A
  88 #define MODE_SENSE_10           0x5A
  89 #define READ_12                 0xA8
  90 #define READ_CD                 0xBE
  91
  92 /*
  93  * SCSI mode page codes
  94  */
  95 #define MODEPAGE_RW_ERROR_RECOVERY      0x01
  96 #define MODEPAGE_CD_CAPABILITIES        0x2A
  97
  98 /*
  99  * ATA commands
 100  */
 101 #define ATA_SF_ENAB_SATA_SF             0x10
 102 #define         ATA_SATA_SF_AN          0x05
 103 #define ATA_SF_DIS_SATA_SF              0x90
 104
 105 /*
 106  * Debug printf
 107  */
 108 #ifdef AHCI_DEBUG
 109 static FILE *dbg;
 110 #define DPRINTF(format, arg...) do{fprintf(dbg, format, ##arg);fflush(dbg);}while(0)
 111 #else
 112 #define DPRINTF(format, arg...)
 113 #endif
 114 #define WPRINTF(format, arg...) printf(format, ##arg)
 115
 116 struct ahci_ioreq {
 117         struct blockif_req io_req;
 118         struct ahci_port *io_pr;
 119         STAILQ_ENTRY(ahci_ioreq) io_flist;
 120         TAILQ_ENTRY(ahci_ioreq) io_blist;
 121         uint8_t *cfis;
 122         uint32_t len;
 123         uint32_t done;
 124         int slot;
 125         int prdtl;
 126 };
 127
 128 struct ahci_port {
 129         struct blockif_ctxt *bctx;
 130         struct pci_ahci_softc *pr_sc;
 131         uint8_t *cmd_lst;
 132         uint8_t *rfis;
 133         int atapi;
 134         int reset;
 135         int mult_sectors;
 136         uint8_t xfermode;
 137         uint8_t err_cfis[20];
 138         uint8_t sense_key;
 139         uint8_t asc;
 140         uint32_t pending;
 141
 142         uint32_t clb;
 143         uint32_t clbu;
 144         uint32_t fb;
 145         uint32_t fbu;
 146         uint32_t is;
 147         uint32_t ie;
 148         uint32_t cmd;
 149         uint32_t unused0;
 150         uint32_t tfd;
 151         uint32_t sig;
 152         uint32_t ssts;
 153         uint32_t sctl;
 154         uint32_t serr;
 155         uint32_t sact;
 156         uint32_t ci;
 157         uint32_t sntf;
 158         uint32_t fbs;
 159
 160         /*
 161          * i/o request info
 162          */
 163         struct ahci_ioreq *ioreq;
 164         int ioqsz;
 165         STAILQ_HEAD(ahci_fhead, ahci_ioreq) iofhd;
 166         TAILQ_HEAD(ahci_bhead, ahci_ioreq) iobhd;
 167 };
 168
 169 struct ahci_cmd_hdr {
 170         uint16_t flags;
 171         uint16_t prdtl;
 172         uint32_t prdbc;
 173         uint64_t ctba;
 174         uint32_t reserved[4];
 175 };
 176
 177 struct ahci_prdt_entry {
 178         uint64_t dba;
 179         uint32_t reserved;
 180 #define DBCMASK         0x3fffff
 181         uint32_t dbc;
 182 };
 183
 184 struct pci_ahci_softc {
 185         struct pci_devinst *asc_pi;
 186         pthread_mutex_t mtx;
 187         int ports;
 188         uint32_t cap;
 189         uint32_t ghc;
 190         uint32_t is;
 191         uint32_t pi;
 192         uint32_t vs;
 193         uint32_t ccc_ctl;
 194         uint32_t ccc_pts;
 195         uint32_t em_loc;
 196         uint32_t em_ctl;
 197         uint32_t cap2;
 198         uint32_t bohc;
 199         uint32_t lintr;
 200         struct ahci_port port[MAX_PORTS];
 201 };
 202 #define ahci_ctx(sc)    ((sc)->asc_pi->pi_vmctx)
 203
 204 static inline void lba_to_msf(uint8_t *buf, int lba)
 205 {
 206         lba += 150;
 207         buf[0] = (lba / 75) / 60;
 208         buf[1] = (lba / 75) % 60;
 209         buf[2] = lba % 75;
 210 }
 211
 212 /*
 213  * generate HBA intr depending on whether or not ports within
 214  * the controller have an interrupt pending.
 215  */
 216 static void
 217 ahci_generate_intr(struct pci_ahci_softc *sc)
 218 {
 219         struct pci_devinst *pi;
 220         int i;
 221
 222         pi = sc->asc_pi;
 223
 224         for (i = 0; i < sc->ports; i++) {
 225                 struct ahci_port *pr;
 226                 pr = &sc->port[i];
 227                 if (pr->is & pr->ie)
 228                         sc->is |= (1 << i);
 229         }
 230
 231         DPRINTF("%s %x\n", __func__, sc->is);
 232
 233         if (sc->is && (sc->ghc & AHCI_GHC_IE)) {
 234                 if (pci_msi_enabled(pi)) {
 235                         /*
 236                          * Generate an MSI interrupt on every edge
 237                          */
 238                         pci_generate_msi(pi, 0);
 239                 } else if (!sc->lintr) {
 240                         /*
 241                          * Only generate a pin-based interrupt if one wasn't
 242                          * in progress
 243                          */
 244                         sc->lintr = 1;
 245                         pci_lintr_assert(pi);
 246                 }
 247         } else if (sc->lintr) {
 248                 /*
 249                  * No interrupts: deassert pin-based signal if it had
 250                  * been asserted
 251                  */
 252                 pci_lintr_deassert(pi);
 253                 sc->lintr = 0;
 254         }
 255 }
 256
 257 static void
 258 ahci_write_fis(struct ahci_port *p, enum sata_fis_type ft, uint8_t *fis)
 259 {
 260         int offset, len, irq;
 261
 262         if (p->rfis == NULL || !(p->cmd & AHCI_P_CMD_FRE))
 263                 return;
 264
 265         switch (ft) {
 266         case FIS_TYPE_REGD2H:
 267                 offset = 0x40;
 268                 len = 20;
 269                 irq = AHCI_P_IX_DHR;
 270                 break;
 271         case FIS_TYPE_SETDEVBITS:
 272                 offset = 0x58;
 273                 len = 8;
 274                 irq = AHCI_P_IX_SDB;
 275                 break;
 276         case FIS_TYPE_PIOSETUP:
 277                 offset = 0x20;
 278                 len = 20;
 279                 irq = 0;
 280                 break;
 281         default:
 282                 WPRINTF("unsupported fis type %d\n", ft);
 283                 return;
 284         }
 285         memcpy(p->rfis + offset, fis, len);
 286         if (irq) {
 287                 p->is |= irq;
 288                 ahci_generate_intr(p->pr_sc);
 289         }
 290 }
 291
 292 static void
 293 ahci_write_fis_piosetup(struct ahci_port *p)
 294 {
 295         uint8_t fis[20];
 296
 297         memset(fis, 0, sizeof(fis));
 298         fis[0] = FIS_TYPE_PIOSETUP;
 299         ahci_write_fis(p, FIS_TYPE_PIOSETUP, fis);
 300 }
 301
 302 static void
 303 ahci_write_fis_sdb(struct ahci_port *p, int slot, uint8_t *cfis, uint32_t tfd)
 304 {
 305         uint8_t fis[8];
 306         uint8_t error;
 307
 308         error = (tfd >> 8) & 0xff;
 309         memset(fis, 0, sizeof(fis));
 310         fis[0] = FIS_TYPE_SETDEVBITS;
 311         fis[1] = (1 << 6);
 312         fis[2] = tfd & 0x77;
 313         fis[3] = error;
 314         if (fis[2] & ATA_S_ERROR) {
 315                 p->is |= AHCI_P_IX_TFE;
 316                 p->err_cfis[0] = slot;
 317                 p->err_cfis[2] = tfd & 0x77;
 318                 p->err_cfis[3] = error;
 319                 memcpy(&p->err_cfis[4], cfis + 4, 16);
 320         } else {
 321                 *(uint32_t *)(fis + 4) = (1 << slot);
 322                 p->sact &= ~(1 << slot);
 323         }
 324         p->tfd = tfd;
 325         ahci_write_fis(p, FIS_TYPE_SETDEVBITS, fis);
 326 }
 327
 328 static void
 329 ahci_write_fis_d2h(struct ahci_port *p, int slot, uint8_t *cfis, uint32_t tfd)
 330 {
 331         uint8_t fis[20];
 332         uint8_t error;
 333
 334         error = (tfd >> 8) & 0xff;
 335         memset(fis, 0, sizeof(fis));
 336         fis[0] = FIS_TYPE_REGD2H;
 337         fis[1] = (1 << 6);
 338         fis[2] = tfd & 0xff;
 339         fis[3] = error;
 340         fis[4] = cfis[4];
 341         fis[5] = cfis[5];
 342         fis[6] = cfis[6];
 343         fis[7] = cfis[7];
 344         fis[8] = cfis[8];
 345         fis[9] = cfis[9];
 346         fis[10] = cfis[10];
 347         fis[11] = cfis[11];
 348         fis[12] = cfis[12];
 349         fis[13] = cfis[13];
 350         if (fis[2] & ATA_S_ERROR) {
 351                 p->is |= AHCI_P_IX_TFE;
 352                 p->err_cfis[0] = 0x80;
 353                 p->err_cfis[2] = tfd & 0xff;
 354                 p->err_cfis[3] = error;
 355                 memcpy(&p->err_cfis[4], cfis + 4, 16);
 356         } else
 357                 p->ci &= ~(1 << slot);
 358         p->tfd = tfd;
 359         ahci_write_fis(p, FIS_TYPE_REGD2H, fis);
 360 }
 361
 362 static void
 363 ahci_write_reset_fis_d2h(struct ahci_port *p)
 364 {
 365         uint8_t fis[20];
 366
 367         memset(fis, 0, sizeof(fis));
 368         fis[0] = FIS_TYPE_REGD2H;
 369         fis[3] = 1;
 370         fis[4] = 1;
 371         if (p->atapi) {
 372                 fis[5] = 0x14;
 373                 fis[6] = 0xeb;
 374         }
 375         fis[12] = 1;
 376         ahci_write_fis(p, FIS_TYPE_REGD2H, fis);
 377 }
 378
 379 static void
 380 ahci_check_stopped(struct ahci_port *p)
 381 {
 382         /*
 383          * If we are no longer processing the command list and nothing
 384          * is in-flight, clear the running bit, the current command
 385          * slot, the command issue and active bits.
 386          */
 387         if (!(p->cmd & AHCI_P_CMD_ST)) {
 388                 if (p->pending == 0) {
 389                         p->cmd &= ~(AHCI_P_CMD_CR | AHCI_P_CMD_CCS_MASK);
 390                         p->ci = 0;
 391                         p->sact = 0;
 392                 }
 393         }
 394 }
 395
 396 static void
 397 ahci_port_stop(struct ahci_port *p)
 398 {
 399         struct ahci_ioreq *aior;
 400         uint8_t *cfis;
 401         int slot;
 402         int ncq;
 403         int error;
 404
 405         assert(pthread_mutex_isowned_np(&p->pr_sc->mtx));
 406
 407         TAILQ_FOREACH(aior, &p->iobhd, io_blist) {
 408                 /*
 409                  * Try to cancel the outstanding blockif request.
 410                  */
 411                 error = blockif_cancel(p->bctx, &aior->io_req);
 412                 if (error != 0)
 413                         continue;
 414
 415                 slot = aior->slot;
 416                 cfis = aior->cfis;
 417                 if (cfis[2] == ATA_WRITE_FPDMA_QUEUED ||
 418                     cfis[2] == ATA_READ_FPDMA_QUEUED)
 419                         ncq = 1;
 420
 421                 if (ncq)
 422                         p->sact &= ~(1 << slot);
 423                 else
 424                         p->ci &= ~(1 << slot);
 425
 426                 /*
 427                  * This command is now done.
 428                  */
 429                 p->pending &= ~(1 << slot);
 430
 431                 /*
 432                  * Delete the blockif request from the busy list
 433                  */
 434                 TAILQ_REMOVE(&p->iobhd, aior, io_blist);
 435
 436                 /*
 437                  * Move the blockif request back to the free list
 438                  */
 439                 STAILQ_INSERT_TAIL(&p->iofhd, aior, io_flist);
 440         }
 441
 442         ahci_check_stopped(p);
 443 }
 444
 445 static void
 446 ahci_port_reset(struct ahci_port *pr)
 447 {
 448         pr->serr = 0;
 449         pr->sact = 0;
 450         pr->xfermode = ATA_UDMA6;
 451         pr->mult_sectors = 128;
 452
 453         if (!pr->bctx) {
 454                 pr->ssts = ATA_SS_DET_NO_DEVICE;
 455                 pr->sig = 0xFFFFFFFF;
 456                 pr->tfd = 0x7F;
 457                 return;
 458         }
 459         pr->ssts = ATA_SS_DET_PHY_ONLINE | ATA_SS_IPM_ACTIVE;
 460         if (pr->sctl & ATA_SC_SPD_MASK)
 461                 pr->ssts |= (pr->sctl & ATA_SC_SPD_MASK);
 462         else
 463                 pr->ssts |= ATA_SS_SPD_GEN3;
 464         pr->tfd = (1 << 8) | ATA_S_DSC | ATA_S_DMA;
 465         if (!pr->atapi) {
 466                 pr->sig = PxSIG_ATA;
 467                 pr->tfd |= ATA_S_READY;
 468         } else
 469                 pr->sig = PxSIG_ATAPI;
 470         ahci_write_reset_fis_d2h(pr);
 471 }
 472
 473 static void
 474 ahci_reset(struct pci_ahci_softc *sc)
 475 {
 476         int i;
 477
 478         sc->ghc = AHCI_GHC_AE;
 479         sc->is = 0;
 480
 481         if (sc->lintr) {
 482                 pci_lintr_deassert(sc->asc_pi);
 483                 sc->lintr = 0;
 484         }
 485
 486         for (i = 0; i < sc->ports; i++) {
 487                 sc->port[i].ie = 0;
 488                 sc->port[i].is = 0;
 489                 sc->port[i].sctl = 0;
 490                 ahci_port_reset(&sc->port[i]);
 491         }
 492 }
 493
 494 static void
 495 ata_string(uint8_t *dest, const char *src, int len)
 496 {
 497         int i;
 498
 499         for (i = 0; i < len; i++) {
 500                 if (*src)
 501                         dest[i ^ 1] = *src++;
 502                 else
 503                         dest[i ^ 1] = ' ';
 504         }
 505 }
 506
 507 static void
 508 atapi_string(uint8_t *dest, const char *src, int len)
 509 {
 510         int i;
 511
 512         for (i = 0; i < len; i++) {
 513                 if (*src)
 514                         dest[i] = *src++;
 515                 else
 516                         dest[i] = ' ';
 517         }
 518 }
 519
 520 static void
 521 ahci_handle_dma(struct ahci_port *p, int slot, uint8_t *cfis, uint32_t done,
 522     int seek)
 523 {
 524         struct ahci_ioreq *aior;
 525         struct blockif_req *breq;
 526         struct pci_ahci_softc *sc;
 527         struct ahci_prdt_entry *prdt;
 528         struct ahci_cmd_hdr *hdr;
 529         uint64_t lba;
 530         uint32_t len;
 531         int i, err, iovcnt, ncq, readop;
 532
 533         sc = p->pr_sc;
 534         prdt = (struct ahci_prdt_entry *)(cfis + 0x80);
 535         hdr = (struct ahci_cmd_hdr *)(p->cmd_lst + slot * AHCI_CL_SIZE);
 536         ncq = 0;
 537         readop = 1;
 538
 539         prdt += seek;
 540         if (cfis[2] == ATA_WRITE || cfis[2] == ATA_WRITE48 ||
 541             cfis[2] == ATA_WRITE_MUL || cfis[2] == ATA_WRITE_MUL48 ||
 542             cfis[2] == ATA_WRITE_DMA || cfis[2] == ATA_WRITE_DMA48 ||
 543             cfis[2] == ATA_WRITE_FPDMA_QUEUED)
 544                 readop = 0;
 545
 546         if (cfis[2] == ATA_WRITE_FPDMA_QUEUED ||
 547             cfis[2] == ATA_READ_FPDMA_QUEUED) {
 548                 lba = ((uint64_t)cfis[10] << 40) |
 549                         ((uint64_t)cfis[9] << 32) |
 550                         ((uint64_t)cfis[8] << 24) |
 551                         ((uint64_t)cfis[6] << 16) |
 552                         ((uint64_t)cfis[5] << 8) |
 553                         cfis[4];
 554                 len = cfis[11] << 8 | cfis[3];
 555                 if (!len)
 556                         len = 65536;
 557                 ncq = 1;
 558         } else if (cfis[2] == ATA_READ48 || cfis[2] == ATA_WRITE48 ||
 559             cfis[2] == ATA_READ_MUL48 || cfis[2] == ATA_WRITE_MUL48 ||
 560             cfis[2] == ATA_READ_DMA48 || cfis[2] == ATA_WRITE_DMA48) {
 561                 lba = ((uint64_t)cfis[10] << 40) |
 562                         ((uint64_t)cfis[9] << 32) |
 563                         ((uint64_t)cfis[8] << 24) |
 564                         ((uint64_t)cfis[6] << 16) |
 565                         ((uint64_t)cfis[5] << 8) |
 566                         cfis[4];
 567                 len = cfis[13] << 8 | cfis[12];
 568                 if (!len)
 569                         len = 65536;
 570         } else {
 571                 lba = ((cfis[7] & 0xf) << 24) | (cfis[6] << 16) |
 572                         (cfis[5] << 8) | cfis[4];
 573                 len = cfis[12];
 574                 if (!len)
 575                         len = 256;
 576         }
 577         lba *= blockif_sectsz(p->bctx);
 578         len *= blockif_sectsz(p->bctx);
 579
 580         /*
 581          * Pull request off free list
 582          */
 583         aior = STAILQ_FIRST(&p->iofhd);
 584         assert(aior != NULL);
 585         STAILQ_REMOVE_HEAD(&p->iofhd, io_flist);
 586         aior->cfis = cfis;
 587         aior->slot = slot;
 588         aior->len = len;
 589         aior->done = done;
 590         breq = &aior->io_req;
 591         breq->br_offset = lba + done;
 592         iovcnt = hdr->prdtl - seek;
 593         if (iovcnt > BLOCKIF_IOV_MAX) {
 594                 aior->prdtl = iovcnt - BLOCKIF_IOV_MAX;
 595                 iovcnt = BLOCKIF_IOV_MAX;
 596         } else
 597                 aior->prdtl = 0;
 598         breq->br_iovcnt = iovcnt;
 599
 600         /*
 601          * Mark this command in-flight.
 602          */
 603         p->pending |= 1 << slot;
 604
 605         /*
 606          * Stuff request onto busy list
 607          */
 608         TAILQ_INSERT_HEAD(&p->iobhd, aior, io_blist);
 609
 610         /*
 611          * Build up the iovec based on the prdt
 612          */
 613         for (i = 0; i < iovcnt; i++) {
 614                 uint32_t dbcsz;
 615
 616                 dbcsz = (prdt->dbc & DBCMASK) + 1;
 617                 breq->br_iov[i].iov_base = paddr_guest2host(ahci_ctx(sc),
 618                     prdt->dba, dbcsz);
 619                 breq->br_iov[i].iov_len = dbcsz;
 620                 aior->done += dbcsz;
 621                 prdt++;
 622         }
 623         if (readop)
 624                 err = blockif_read(p->bctx, breq);
 625         else
 626                 err = blockif_write(p->bctx, breq);
 627         assert(err == 0);
 628
 629         if (ncq)
 630                 p->ci &= ~(1 << slot);
 631 }
 632
 633 static void
 634 ahci_handle_flush(struct ahci_port *p, int slot, uint8_t *cfis)
 635 {
 636         struct ahci_ioreq *aior;
 637         struct blockif_req *breq;
 638         int err;
 639
 640         /*
 641          * Pull request off free list
 642          */
 643         aior = STAILQ_FIRST(&p->iofhd);
 644         assert(aior != NULL);
 645         STAILQ_REMOVE_HEAD(&p->iofhd, io_flist);
 646         aior->cfis = cfis;
 647         aior->slot = slot;
 648         aior->len = 0;
 649         aior->done = 0;
 650         aior->prdtl = 0;
 651         breq = &aior->io_req;
 652
 653         /*
 654          * Mark this command in-flight.
 655          */
 656         p->pending |= 1 << slot;
 657
 658         /*
 659          * Stuff request onto busy list
 660          */
 661         TAILQ_INSERT_HEAD(&p->iobhd, aior, io_blist);
 662
 663         err = blockif_flush(p->bctx, breq);
 664         assert(err == 0);
 665 }
 666
 667 static inline void
 668 read_prdt(struct ahci_port *p, int slot, uint8_t *cfis,
 669                 void *buf, int size)
 670 {
 671         struct ahci_cmd_hdr *hdr;
 672         struct ahci_prdt_entry *prdt;
 673         void *to;
 674         int i, len;
 675
 676         hdr = (struct ahci_cmd_hdr *)(p->cmd_lst + slot * AHCI_CL_SIZE);
 677         len = size;
 678         to = buf;
 679         prdt = (struct ahci_prdt_entry *)(cfis + 0x80);
 680         for (i = 0; i < hdr->prdtl && len; i++) {
 681                 uint8_t *ptr;
 682                 uint32_t dbcsz;
 683                 int sublen;
 684
 685                 dbcsz = (prdt->dbc & DBCMASK) + 1;
 686                 ptr = paddr_guest2host(ahci_ctx(p->pr_sc), prdt->dba, dbcsz);
 687                 sublen = len < dbcsz ? len : dbcsz;
 688                 memcpy(to, ptr, sublen);
 689                 len -= sublen;
 690                 to += sublen;
 691                 prdt++;
 692         }
 693 }
 694
 695 static void
 696 ahci_handle_dsm_trim(struct ahci_port *p, int slot, uint8_t *cfis, uint32_t done)
 697 {
 698         struct ahci_ioreq *aior;
 699         struct blockif_req *breq;
 700         uint8_t *entry;
 701         uint64_t elba;
 702         uint32_t len, elen;
 703         int err;
 704         uint8_t buf[512];
 705
 706         len = (uint16_t)cfis[13] << 8 | cfis[12];
 707         len *= 512;
 708         read_prdt(p, slot, cfis, buf, sizeof(buf));
 709
 710 next:
 711         entry = &buf[done];
 712         elba = ((uint64_t)entry[5] << 40) |
 713                 ((uint64_t)entry[4] << 32) |
 714                 ((uint64_t)entry[3] << 24) |
 715                 ((uint64_t)entry[2] << 16) |
 716                 ((uint64_t)entry[1] << 8) |
 717                 entry[0];
 718         elen = (uint16_t)entry[7] << 8 | entry[6];
 719         done += 8;
 720         if (elen == 0) {
 721                 if (done >= len) {
 722                         ahci_write_fis_d2h(p, slot, cfis, ATA_S_READY | ATA_S_DSC);
 723                         p->pending &= ~(1 << slot);
 724                         ahci_check_stopped(p);
 725                         return;
 726                 }
 727                 goto next;
 728         }
 729
 730         /*
 731          * Pull request off free list
 732          */
 733         aior = STAILQ_FIRST(&p->iofhd);
 734         assert(aior != NULL);
 735         STAILQ_REMOVE_HEAD(&p->iofhd, io_flist);
 736         aior->cfis = cfis;
 737         aior->slot = slot;
 738         aior->len = len;
 739         aior->done = done;
 740         aior->prdtl = 0;
 741
 742         breq = &aior->io_req;
 743         breq->br_offset = elba * blockif_sectsz(p->bctx);
 744         breq->br_iovcnt = 1;
 745         breq->br_iov[0].iov_len = elen * blockif_sectsz(p->bctx);
 746
 747         /*
 748          * Mark this command in-flight.
 749          */
 750         p->pending |= 1 << slot;
 751
 752         /*
 753          * Stuff request onto busy list
 754          */
 755         TAILQ_INSERT_HEAD(&p->iobhd, aior, io_blist);
 756
 757         err = blockif_delete(p->bctx, breq);
 758         assert(err == 0);
 759 }
 760
 761 static inline void
 762 write_prdt(struct ahci_port *p, int slot, uint8_t *cfis,
 763                 void *buf, int size)
 764 {
 765         struct ahci_cmd_hdr *hdr;
 766         struct ahci_prdt_entry *prdt;
 767         void *from;
 768         int i, len;
 769
 770         hdr = (struct ahci_cmd_hdr *)(p->cmd_lst + slot * AHCI_CL_SIZE);
 771         len = size;
 772         from = buf;
 773         prdt = (struct ahci_prdt_entry *)(cfis + 0x80);
 774         for (i = 0; i < hdr->prdtl && len; i++) {
 775                 uint8_t *ptr;
 776                 uint32_t dbcsz;
 777                 int sublen;
 778
 779                 dbcsz = (prdt->dbc & DBCMASK) + 1;
 780                 ptr = paddr_guest2host(ahci_ctx(p->pr_sc), prdt->dba, dbcsz);
 781                 sublen = len < dbcsz ? len : dbcsz;
 782                 memcpy(ptr, from, sublen);
 783                 len -= sublen;
 784                 from += sublen;
 785                 prdt++;
 786         }
 787         hdr->prdbc = size - len;
 788 }
 789
 790 static void
 791 ahci_handle_read_log(struct ahci_port *p, int slot, uint8_t *cfis)
 792 {
 793         struct ahci_cmd_hdr *hdr;
 794         uint8_t buf[512];
 795
 796         hdr = (struct ahci_cmd_hdr *)(p->cmd_lst + slot * AHCI_CL_SIZE);
 797         if (p->atapi || hdr->prdtl == 0 || cfis[4] != 0x10 ||
 798             cfis[5] != 0 || cfis[9] != 0 || cfis[12] != 1 || cfis[13] != 0) {
 799                 ahci_write_fis_d2h(p, slot, cfis,
 800                     (ATA_E_ABORT << 8) | ATA_S_READY | ATA_S_ERROR);
 801                 return;
 802         }
 803
 804         memset(buf, 0, sizeof(buf));
 805         memcpy(buf, p->err_cfis, sizeof(p->err_cfis));
 806
 807         if (cfis[2] == ATA_READ_LOG_EXT)
 808                 ahci_write_fis_piosetup(p);
 809         write_prdt(p, slot, cfis, (void *)buf, sizeof(buf));
 810         ahci_write_fis_d2h(p, slot, cfis, ATA_S_DSC | ATA_S_READY);
 811 }
 812
 813 static void
 814 handle_identify(struct ahci_port *p, int slot, uint8_t *cfis)
 815 {
 816         struct ahci_cmd_hdr *hdr;
 817
 818         hdr = (struct ahci_cmd_hdr *)(p->cmd_lst + slot * AHCI_CL_SIZE);
 819         if (p->atapi || hdr->prdtl == 0) {
 820                 ahci_write_fis_d2h(p, slot, cfis,
 821                     (ATA_E_ABORT << 8) | ATA_S_READY | ATA_S_ERROR);
 822         } else {
 823                 uint16_t buf[256];
 824                 uint64_t sectors;
 825                 int sectsz, psectsz, psectoff, candelete, ro;
 826                 uint16_t cyl;
 827                 uint8_t sech, heads;
 828
 829                 ro = blockif_is_ro(p->bctx);
 830                 candelete = blockif_candelete(p->bctx);
 831                 sectsz = blockif_sectsz(p->bctx);
 832                 sectors = blockif_size(p->bctx) / sectsz;
 833                 blockif_chs(p->bctx, &cyl, &heads, &sech);
 834                 blockif_psectsz(p->bctx, &psectsz, &psectoff);
 835                 memset(buf, 0, sizeof(buf));
 836                 buf[0] = 0x0040;
 837                 buf[1] = cyl;
 838                 buf[3] = heads;
 839                 buf[6] = sech;
 840                 /* TODO emulate different serial? */
 841                 ata_string((uint8_t *)(buf+10), "123456", 20);
 842                 ata_string((uint8_t *)(buf+23), "001", 8);
 843                 ata_string((uint8_t *)(buf+27), "BHYVE SATA DISK", 40);
 844                 buf[47] = (0x8000 | 128);
 845                 buf[48] = 0x1;
 846                 buf[49] = (1 << 8 | 1 << 9 | 1 << 11);
 847                 buf[50] = (1 << 14);
 848                 buf[53] = (1 << 1 | 1 << 2);
 849                 if (p->mult_sectors)
 850                         buf[59] = (0x100 | p->mult_sectors);
 851                 if (sectors <= 0x0fffffff) {
 852                         buf[60] = sectors;
 853                         buf[61] = (sectors >> 16);
 854                 } else {
 855                         buf[60] = 0xffff;
 856                         buf[61] = 0x0fff;
 857                 }
 858                 buf[63] = 0x7;
 859                 if (p->xfermode & ATA_WDMA0)
 860                         buf[63] |= (1 << ((p->xfermode & 7) + 8));
 861                 buf[64] = 0x3;
 862                 buf[65] = 120;
 863                 buf[66] = 120;
 864                 buf[67] = 120;
 865                 buf[68] = 120;
 866                 buf[69] = 0;
 867                 buf[75] = 31;
 868                 buf[76] = (ATA_SATA_GEN1 | ATA_SATA_GEN2 | ATA_SATA_GEN3 |
 869                            ATA_SUPPORT_NCQ);
 870                 buf[80] = 0x1f0;
 871                 buf[81] = 0x28;
 872                 buf[82] = (ATA_SUPPORT_POWERMGT | ATA_SUPPORT_WRITECACHE|
 873                            ATA_SUPPORT_LOOKAHEAD | ATA_SUPPORT_NOP);
 874                 buf[83] = (ATA_SUPPORT_ADDRESS48 | ATA_SUPPORT_FLUSHCACHE |
 875                            ATA_SUPPORT_FLUSHCACHE48 | 1 << 14);
 876                 buf[84] = (1 << 14);
 877                 buf[85] = (ATA_SUPPORT_POWERMGT | ATA_SUPPORT_WRITECACHE|
 878                            ATA_SUPPORT_LOOKAHEAD | ATA_SUPPORT_NOP);
 879                 buf[86] = (ATA_SUPPORT_ADDRESS48 | ATA_SUPPORT_FLUSHCACHE |
 880                            ATA_SUPPORT_FLUSHCACHE48 | 1 << 15);
 881                 buf[87] = (1 << 14);
 882                 buf[88] = 0x7f;
 883                 if (p->xfermode & ATA_UDMA0)
 884                         buf[88] |= (1 << ((p->xfermode & 7) + 8));
 885                 buf[93] = (1 | 1 <<14);
 886                 buf[100] = sectors;
 887                 buf[101] = (sectors >> 16);
 888                 buf[102] = (sectors >> 32);
 889                 buf[103] = (sectors >> 48);
 890                 if (candelete && !ro) {
 891                         buf[69] |= ATA_SUPPORT_RZAT | ATA_SUPPORT_DRAT;
 892                         buf[105] = 1;
 893                         buf[169] = ATA_SUPPORT_DSM_TRIM;
 894                 }
 895                 buf[106] = 0x4000;
 896                 buf[209] = 0x4000;
 897                 if (psectsz > sectsz) {
 898                         buf[106] |= 0x2000;
 899                         buf[106] |= ffsl(psectsz / sectsz) - 1;
 900                         buf[209] |= (psectoff / sectsz);
 901                 }
 902                 if (sectsz > 512) {
 903                         buf[106] |= 0x1000;
 904                         buf[117] = sectsz / 2;
 905                         buf[118] = ((sectsz / 2) >> 16);
 906                 }
 907                 buf[119] = (ATA_SUPPORT_RWLOGDMAEXT | 1 << 14);
 908                 buf[120] = (ATA_SUPPORT_RWLOGDMAEXT | 1 << 14);
 909                 buf[222] = 0x1020;
 910                 ahci_write_fis_piosetup(p);
 911                 write_prdt(p, slot, cfis, (void *)buf, sizeof(buf));
 912                 ahci_write_fis_d2h(p, slot, cfis, ATA_S_DSC | ATA_S_READY);
 913         }
 914 }
 915
 916 static void
 917 handle_atapi_identify(struct ahci_port *p, int slot, uint8_t *cfis)
 918 {
 919         if (!p->atapi) {
 920                 ahci_write_fis_d2h(p, slot, cfis,
 921                     (ATA_E_ABORT << 8) | ATA_S_READY | ATA_S_ERROR);
 922         } else {
 923                 uint16_t buf[256];
 924
 925                 memset(buf, 0, sizeof(buf));
 926                 buf[0] = (2 << 14 | 5 << 8 | 1 << 7 | 2 << 5);
 927                 /* TODO emulate different serial? */
 928                 ata_string((uint8_t *)(buf+10), "123456", 20);
 929                 ata_string((uint8_t *)(buf+23), "001", 8);
 930                 ata_string((uint8_t *)(buf+27), "BHYVE SATA DVD ROM", 40);
 931                 buf[49] = (1 << 9 | 1 << 8);
 932                 buf[50] = (1 << 14 | 1);
 933                 buf[53] = (1 << 2 | 1 << 1);
 934                 buf[62] = 0x3f;
 935                 buf[63] = 7;
 936                 buf[64] = 3;
 937                 buf[65] = 100;
 938                 buf[66] = 100;
 939                 buf[67] = 100;
 940                 buf[68] = 100;
 941                 buf[76] = (1 << 2 | 1 << 1);
 942                 buf[78] = (1 << 5);
 943                 buf[80] = (0x1f << 4);
 944                 buf[82] = (1 << 4);
 945                 buf[83] = (1 << 14);
 946                 buf[84] = (1 << 14);
 947                 buf[85] = (1 << 4);
 948                 buf[87] = (1 << 14);
 949                 buf[88] = (1 << 14 | 0x7f);
 950                 ahci_write_fis_piosetup(p);
 951                 write_prdt(p, slot, cfis, (void *)buf, sizeof(buf));
 952                 ahci_write_fis_d2h(p, slot, cfis, ATA_S_DSC | ATA_S_READY);
 953         }
 954 }
 955
 956 static void
 957 atapi_inquiry(struct ahci_port *p, int slot, uint8_t *cfis)
 958 {
 959         uint8_t buf[36];
 960         uint8_t *acmd;
 961         int len;
 962
 963         acmd = cfis + 0x40;
 964
 965         buf[0] = 0x05;
 966         buf[1] = 0x80;
 967         buf[2] = 0x00;
 968         buf[3] = 0x21;
 969         buf[4] = 31;
 970         buf[5] = 0;
 971         buf[6] = 0;
 972         buf[7] = 0;
 973         atapi_string(buf + 8, "BHYVE", 8);
 974         atapi_string(buf + 16, "BHYVE DVD-ROM", 16);
 975         atapi_string(buf + 32, "001", 4);
 976
 977         len = sizeof(buf);
 978         if (len > acmd[4])
 979                 len = acmd[4];
 980         cfis[4] = (cfis[4] & ~7) | ATA_I_CMD | ATA_I_IN;
 981         write_prdt(p, slot, cfis, buf, len);
 982         ahci_write_fis_d2h(p, slot, cfis, ATA_S_READY | ATA_S_DSC);
 983 }
 984
 985 static void
 986 atapi_read_capacity(struct ahci_port *p, int slot, uint8_t *cfis)
 987 {
 988         uint8_t buf[8];
 989         uint64_t sectors;
 990
 991         sectors = blockif_size(p->bctx) / 2048;
 992         be32enc(buf, sectors - 1);
 993         be32enc(buf + 4, 2048);
 994         cfis[4] = (cfis[4] & ~7) | ATA_I_CMD | ATA_I_IN;
 995         write_prdt(p, slot, cfis, buf, sizeof(buf));
 996         ahci_write_fis_d2h(p, slot, cfis, ATA_S_READY | ATA_S_DSC);
 997 }
 998
 999 static void
1000 atapi_read_toc(struct ahci_port *p, int slot, uint8_t *cfis)
1001 {
1002         uint8_t *acmd;
1003         uint8_t format;
1004         int len;
1005
1006         acmd = cfis + 0x40;
1007
1008         len = be16dec(acmd + 7);
1009         format = acmd[9] >> 6;
1010         switch (format) {
1011         case 0:
1012         {
1013                 int msf, size;
1014                 uint64_t sectors;
1015                 uint8_t start_track, buf[20], *bp;
1016
1017                 msf = (acmd[1] >> 1) & 1;
1018                 start_track = acmd[6];
1019                 if (start_track > 1 && start_track != 0xaa) {
1020                         uint32_t tfd;
1021                         p->sense_key = ATA_SENSE_ILLEGAL_REQUEST;
1022                         p->asc = 0x24;
1023                         tfd = (p->sense_key << 12) | ATA_S_READY | ATA_S_ERROR;
1024                         cfis[4] = (cfis[4] & ~7) | ATA_I_CMD | ATA_I_IN;
1025                         ahci_write_fis_d2h(p, slot, cfis, tfd);
1026                         return;
1027                 }
1028                 bp = buf + 2;
1029                 *bp++ = 1;
1030                 *bp++ = 1;
1031                 if (start_track <= 1) {
1032                         *bp++ = 0;
1033                         *bp++ = 0x14;
1034                         *bp++ = 1;
1035                         *bp++ = 0;
1036                         if (msf) {
1037                                 *bp++ = 0;
1038                                 lba_to_msf(bp, 0);
1039                                 bp += 3;
1040                         } else {
1041                                 *bp++ = 0;
1042                                 *bp++ = 0;
1043                                 *bp++ = 0;
1044                                 *bp++ = 0;
1045                         }
1046                 }
1047                 *bp++ = 0;
1048                 *bp++ = 0x14;
1049                 *bp++ = 0xaa;
1050                 *bp++ = 0;
1051                 sectors = blockif_size(p->bctx) / blockif_sectsz(p->bctx);
1052                 sectors >>= 2;
1053                 if (msf) {
1054                         *bp++ = 0;
1055                         lba_to_msf(bp, sectors);
1056                         bp += 3;
1057                 } else {
1058                         be32enc(bp, sectors);
1059                         bp += 4;
1060                 }
1061                 size = bp - buf;
1062                 be16enc(buf, size - 2);
1063                 if (len > size)
1064                         len = size;
1065                 write_prdt(p, slot, cfis, buf, len);
1066                 cfis[4] = (cfis[4] & ~7) | ATA_I_CMD | ATA_I_IN;
1067                 ahci_write_fis_d2h(p, slot, cfis, ATA_S_READY | ATA_S_DSC);
1068                 break;
1069         }
1070         case 1:
1071         {
1072                 uint8_t buf[12];
1073
1074                 memset(buf, 0, sizeof(buf));
1075                 buf[1] = 0xa;
1076                 buf[2] = 0x1;
1077                 buf[3] = 0x1;
1078                 if (len > sizeof(buf))
1079                         len = sizeof(buf);
1080                 write_prdt(p, slot, cfis, buf, len);
1081                 cfis[4] = (cfis[4] & ~7) | ATA_I_CMD | ATA_I_IN;
1082                 ahci_write_fis_d2h(p, slot, cfis, ATA_S_READY | ATA_S_DSC);
1083                 break;
1084         }
1085         case 2:
1086         {
1087                 int msf, size;
1088                 uint64_t sectors;
1089                 uint8_t start_track, *bp, buf[50];
1090
1091                 msf = (acmd[1] >> 1) & 1;
1092                 start_track = acmd[6];
1093                 bp = buf + 2;
1094                 *bp++ = 1;
1095                 *bp++ = 1;
1096
1097                 *bp++ = 1;
1098                 *bp++ = 0x14;
1099                 *bp++ = 0;
1100                 *bp++ = 0xa0;
1101                 *bp++ = 0;
1102                 *bp++ = 0;
1103                 *bp++ = 0;
1104                 *bp++ = 0;
1105                 *bp++ = 1;
1106                 *bp++ = 0;
1107                 *bp++ = 0;
1108
1109                 *bp++ = 1;
1110                 *bp++ = 0x14;
1111                 *bp++ = 0;
1112                 *bp++ = 0xa1;
1113                 *bp++ = 0;
1114                 *bp++ = 0;
1115                 *bp++ = 0;
1116                 *bp++ = 0;
1117                 *bp++ = 1;
1118                 *bp++ = 0;
1119                 *bp++ = 0;
1120
1121                 *bp++ = 1;
1122                 *bp++ = 0x14;
1123                 *bp++ = 0;
1124                 *bp++ = 0xa2;
1125                 *bp++ = 0;
1126                 *bp++ = 0;
1127                 *bp++ = 0;
1128                 sectors = blockif_size(p->bctx) / blockif_sectsz(p->bctx);
1129                 sectors >>= 2;
1130                 if (msf) {
1131                         *bp++ = 0;
1132                         lba_to_msf(bp, sectors);
1133                         bp += 3;
1134                 } else {
1135                         be32enc(bp, sectors);
1136                         bp += 4;
1137                 }
1138
1139                 *bp++ = 1;
1140                 *bp++ = 0x14;
1141                 *bp++ = 0;
1142                 *bp++ = 1;
1143                 *bp++ = 0;
1144                 *bp++ = 0;
1145                 *bp++ = 0;
1146                 if (msf) {
1147                         *bp++ = 0;
1148                         lba_to_msf(bp, 0);
1149                         bp += 3;
1150                 } else {
1151                         *bp++ = 0;
1152                         *bp++ = 0;
1153                         *bp++ = 0;
1154                         *bp++ = 0;
1155                 }
1156
1157                 size = bp - buf;
1158                 be16enc(buf, size - 2);
1159                 if (len > size)
1160                         len = size;
1161                 write_prdt(p, slot, cfis, buf, len);
1162                 cfis[4] = (cfis[4] & ~7) | ATA_I_CMD | ATA_I_IN;
1163                 ahci_write_fis_d2h(p, slot, cfis, ATA_S_READY | ATA_S_DSC);
1164                 break;
1165         }
1166         default:
1167         {
1168                 uint32_t tfd;
1169
1170                 p->sense_key = ATA_SENSE_ILLEGAL_REQUEST;
1171                 p->asc = 0x24;
1172                 tfd = (p->sense_key << 12) | ATA_S_READY | ATA_S_ERROR;
1173                 cfis[4] = (cfis[4] & ~7) | ATA_I_CMD | ATA_I_IN;
1174                 ahci_write_fis_d2h(p, slot, cfis, tfd);
1175                 break;
1176         }
1177         }
1178 }
1179
1180 static void
1181 atapi_read(struct ahci_port *p, int slot, uint8_t *cfis,
1182                 uint32_t done, int seek)
1183 {
1184         struct ahci_ioreq *aior;
1185         struct ahci_cmd_hdr *hdr;
1186         struct ahci_prdt_entry *prdt;
1187         struct blockif_req *breq;
1188         struct pci_ahci_softc *sc;
1189         uint8_t *acmd;
1190         uint64_t lba;
1191         uint32_t len;
1192         int i, err, iovcnt;
1193
1194         sc = p->pr_sc;
1195         acmd = cfis + 0x40;
1196         hdr = (struct ahci_cmd_hdr *)(p->cmd_lst + slot * AHCI_CL_SIZE);
1197         prdt = (struct ahci_prdt_entry *)(cfis + 0x80);
1198
1199         prdt += seek;
1200         lba = be32dec(acmd + 2);
1201         if (acmd[0] == READ_10)
1202                 len = be16dec(acmd + 7);
1203         else
1204                 len = be32dec(acmd + 6);
1205         if (len == 0) {
1206                 cfis[4] = (cfis[4] & ~7) | ATA_I_CMD | ATA_I_IN;
1207                 ahci_write_fis_d2h(p, slot, cfis, ATA_S_READY | ATA_S_DSC);
1208         }
1209         lba *= 2048;
1210         len *= 2048;
1211
1212         /*
1213          * Pull request off free list
1214          */
1215         aior = STAILQ_FIRST(&p->iofhd);
1216         assert(aior != NULL);
1217         STAILQ_REMOVE_HEAD(&p->iofhd, io_flist);
1218         aior->cfis = cfis;
1219         aior->slot = slot;
1220         aior->len = len;
1221         aior->done = done;
1222         breq = &aior->io_req;
1223         breq->br_offset = lba + done;
1224         iovcnt = hdr->prdtl - seek;
1225         if (iovcnt > BLOCKIF_IOV_MAX) {
1226                 aior->prdtl = iovcnt - BLOCKIF_IOV_MAX;
1227                 iovcnt = BLOCKIF_IOV_MAX;
1228         } else
1229                 aior->prdtl = 0;
1230         breq->br_iovcnt = iovcnt;
1231
1232         /*
1233          * Mark this command in-flight.
1234          */
1235         p->pending |= 1 << slot;
1236
1237         /*
1238          * Stuff request onto busy list
1239          */
1240         TAILQ_INSERT_HEAD(&p->iobhd, aior, io_blist);
1241
1242         /*
1243          * Build up the iovec based on the prdt
1244          */
1245         for (i = 0; i < iovcnt; i++) {
1246                 uint32_t dbcsz;
1247
1248                 dbcsz = (prdt->dbc & DBCMASK) + 1;
1249                 breq->br_iov[i].iov_base = paddr_guest2host(ahci_ctx(sc),
1250                     prdt->dba, dbcsz);
1251                 breq->br_iov[i].iov_len = dbcsz;
1252                 aior->done += dbcsz;
1253                 prdt++;
1254         }
1255         err = blockif_read(p->bctx, breq);
1256         assert(err == 0);
1257 }
1258
1259 static void
1260 atapi_request_sense(struct ahci_port *p, int slot, uint8_t *cfis)
1261 {
1262         uint8_t buf[64];
1263         uint8_t *acmd;
1264         int len;
1265
1266         acmd = cfis + 0x40;
1267         len = acmd[4];
1268         if (len > sizeof(buf))
1269                 len = sizeof(buf);
1270         memset(buf, 0, len);
1271         buf[0] = 0x70 | (1 << 7);
1272         buf[2] = p->sense_key;
1273         buf[7] = 10;
1274         buf[12] = p->asc;
1275         write_prdt(p, slot, cfis, buf, len);
1276         cfis[4] = (cfis[4] & ~7) | ATA_I_CMD | ATA_I_IN;
1277         ahci_write_fis_d2h(p, slot, cfis, ATA_S_READY | ATA_S_DSC);
1278 }
1279
1280 static void
1281 atapi_start_stop_unit(struct ahci_port *p, int slot, uint8_t *cfis)
1282 {
1283         uint8_t *acmd = cfis + 0x40;
1284         uint32_t tfd;
1285
1286         switch (acmd[4] & 3) {
1287         case 0:
1288         case 1:
1289         case 3:
1290                 cfis[4] = (cfis[4] & ~7) | ATA_I_CMD | ATA_I_IN;
1291                 tfd = ATA_S_READY | ATA_S_DSC;
1292                 break;
1293         case 2:
1294                 /* TODO eject media */
1295                 cfis[4] = (cfis[4] & ~7) | ATA_I_CMD | ATA_I_IN;
1296                 p->sense_key = ATA_SENSE_ILLEGAL_REQUEST;
1297                 p->asc = 0x53;
1298                 tfd = (p->sense_key << 12) | ATA_S_READY | ATA_S_ERROR;
1299                 break;
1300         }
1301         ahci_write_fis_d2h(p, slot, cfis, tfd);
1302 }
1303
1304 static void
1305 atapi_mode_sense(struct ahci_port *p, int slot, uint8_t *cfis)
1306 {
1307         uint8_t *acmd;
1308         uint32_t tfd;
1309         uint8_t pc, code;
1310         int len;
1311
1312         acmd = cfis + 0x40;
1313         len = be16dec(acmd + 7);
1314         pc = acmd[2] >> 6;
1315         code = acmd[2] & 0x3f;
1316
1317         switch (pc) {
1318         case 0:
1319                 switch (code) {
1320                 case MODEPAGE_RW_ERROR_RECOVERY:
1321                 {
1322                         uint8_t buf[16];
1323
1324                         if (len > sizeof(buf))
1325                                 len = sizeof(buf);
1326
1327                         memset(buf, 0, sizeof(buf));
1328                         be16enc(buf, 16 - 2);
1329                         buf[2] = 0x70;
1330                         buf[8] = 0x01;
1331                         buf[9] = 16 - 10;
1332                         buf[11] = 0x05;
1333                         write_prdt(p, slot, cfis, buf, len);
1334                         tfd = ATA_S_READY | ATA_S_DSC;
1335                         break;
1336                 }
1337                 case MODEPAGE_CD_CAPABILITIES:
1338                 {
1339                         uint8_t buf[30];
1340
1341                         if (len > sizeof(buf))
1342                                 len = sizeof(buf);
1343
1344                         memset(buf, 0, sizeof(buf));
1345                         be16enc(buf, 30 - 2);
1346                         buf[2] = 0x70;
1347                         buf[8] = 0x2A;
1348                         buf[9] = 30 - 10;
1349                         buf[10] = 0x08;
1350                         buf[12] = 0x71;
1351                         be16enc(&buf[18], 2);
1352                         be16enc(&buf[20], 512);
1353                         write_prdt(p, slot, cfis, buf, len);
1354                         tfd = ATA_S_READY | ATA_S_DSC;
1355                         break;
1356                 }
1357                 default:
1358                         goto error;
1359                         break;
1360                 }
1361                 break;
1362         case 3:
1363                 p->sense_key = ATA_SENSE_ILLEGAL_REQUEST;
1364                 p->asc = 0x39;
1365                 tfd = (p->sense_key << 12) | ATA_S_READY | ATA_S_ERROR;
1366                 break;
1367 error:
1368         case 1:
1369         case 2:
1370                 p->sense_key = ATA_SENSE_ILLEGAL_REQUEST;
1371                 p->asc = 0x24;
1372                 tfd = (p->sense_key << 12) | ATA_S_READY | ATA_S_ERROR;
1373                 break;
1374         }
1375         cfis[4] = (cfis[4] & ~7) | ATA_I_CMD | ATA_I_IN;
1376         ahci_write_fis_d2h(p, slot, cfis, tfd);
1377 }
1378
1379 static void
1380 atapi_get_event_status_notification(struct ahci_port *p, int slot,
1381     uint8_t *cfis)
1382 {
1383         uint8_t *acmd;
1384         uint32_t tfd;
1385
1386         acmd = cfis + 0x40;
1387
1388         /* we don't support asynchronous operation */
1389         if (!(acmd[1] & 1)) {
1390                 p->sense_key = ATA_SENSE_ILLEGAL_REQUEST;
1391                 p->asc = 0x24;
1392                 tfd = (p->sense_key << 12) | ATA_S_READY | ATA_S_ERROR;
1393         } else {
1394                 uint8_t buf[8];
1395                 int len;
1396
1397                 len = be16dec(acmd + 7);
1398                 if (len > sizeof(buf))
1399                         len = sizeof(buf);
1400
1401                 memset(buf, 0, sizeof(buf));
1402                 be16enc(buf, 8 - 2);
1403                 buf[2] = 0x04;
1404                 buf[3] = 0x10;
1405                 buf[5] = 0x02;
1406                 write_prdt(p, slot, cfis, buf, len);
1407                 tfd = ATA_S_READY | ATA_S_DSC;
1408         }
1409         cfis[4] = (cfis[4] & ~7) | ATA_I_CMD | ATA_I_IN;
1410         ahci_write_fis_d2h(p, slot, cfis, tfd);
1411 }
1412
1413 static void
1414 handle_packet_cmd(struct ahci_port *p, int slot, uint8_t *cfis)
1415 {
1416         uint8_t *acmd;
1417
1418         acmd = cfis + 0x40;
1419
1420 #ifdef AHCI_DEBUG
1421         {
1422                 int i;
1423                 DPRINTF("ACMD:");
1424                 for (i = 0; i < 16; i++)
1425                         DPRINTF("%02x ", acmd[i]);
1426                 DPRINTF("\n");
1427         }
1428 #endif
1429
1430         switch (acmd[0]) {
1431         case TEST_UNIT_READY:
1432                 cfis[4] = (cfis[4] & ~7) | ATA_I_CMD | ATA_I_IN;
1433                 ahci_write_fis_d2h(p, slot, cfis, ATA_S_READY | ATA_S_DSC);
1434                 break;
1435         case INQUIRY:
1436                 atapi_inquiry(p, slot, cfis);
1437                 break;
1438         case READ_CAPACITY:
1439                 atapi_read_capacity(p, slot, cfis);
1440                 break;
1441         case PREVENT_ALLOW:
1442                 /* TODO */
1443                 cfis[4] = (cfis[4] & ~7) | ATA_I_CMD | ATA_I_IN;
1444                 ahci_write_fis_d2h(p, slot, cfis, ATA_S_READY | ATA_S_DSC);
1445                 break;
1446         case READ_TOC:
1447                 atapi_read_toc(p, slot, cfis);
1448                 break;
1449         case READ_10:
1450         case READ_12:
1451                 atapi_read(p, slot, cfis, 0, 0);
1452                 break;
1453         case REQUEST_SENSE:
1454                 atapi_request_sense(p, slot, cfis);
1455                 break;
1456         case START_STOP_UNIT:
1457                 atapi_start_stop_unit(p, slot, cfis);
1458                 break;
1459         case MODE_SENSE_10:
1460                 atapi_mode_sense(p, slot, cfis);
1461                 break;
1462         case GET_EVENT_STATUS_NOTIFICATION:
1463                 atapi_get_event_status_notification(p, slot, cfis);
1464                 break;
1465         default:
1466                 cfis[4] = (cfis[4] & ~7) | ATA_I_CMD | ATA_I_IN;
1467                 p->sense_key = ATA_SENSE_ILLEGAL_REQUEST;
1468                 p->asc = 0x20;
1469                 ahci_write_fis_d2h(p, slot, cfis, (p->sense_key << 12) |
1470                                 ATA_S_READY | ATA_S_ERROR);
1471                 break;
1472         }
1473 }
1474
1475 static void
1476 ahci_handle_cmd(struct ahci_port *p, int slot, uint8_t *cfis)
1477 {
1478
1479         switch (cfis[2]) {
1480         case ATA_ATA_IDENTIFY:
1481                 handle_identify(p, slot, cfis);
1482                 break;
1483         case ATA_SETFEATURES:
1484         {
1485                 switch (cfis[3]) {
1486                 case ATA_SF_ENAB_SATA_SF:
1487                         switch (cfis[12]) {
1488                         case ATA_SATA_SF_AN:
1489                                 p->tfd = ATA_S_DSC | ATA_S_READY;
1490                                 break;
1491                         default:
1492                                 p->tfd = ATA_S_ERROR | ATA_S_READY;
1493                                 p->tfd |= (ATA_ERROR_ABORT << 8);
1494                                 break;
1495                         }
1496                         break;
1497                 case ATA_SF_ENAB_WCACHE:
1498                 case ATA_SF_DIS_WCACHE:
1499                 case ATA_SF_ENAB_RCACHE:
1500                 case ATA_SF_DIS_RCACHE:
1501                         p->tfd = ATA_S_DSC | ATA_S_READY;
1502                         break;
1503                 case ATA_SF_SETXFER:
1504                 {
1505                         switch (cfis[12] & 0xf8) {
1506                         case ATA_PIO:
1507                         case ATA_PIO0:
1508                                 break;
1509                         case ATA_WDMA0:
1510                         case ATA_UDMA0:
1511                                 p->xfermode = (cfis[12] & 0x7);
1512                                 break;
1513                         }
1514                         p->tfd = ATA_S_DSC | ATA_S_READY;
1515                         break;
1516                 }
1517                 default:
1518                         p->tfd = ATA_S_ERROR | ATA_S_READY;
1519                         p->tfd |= (ATA_ERROR_ABORT << 8);
1520                         break;
1521                 }
1522                 ahci_write_fis_d2h(p, slot, cfis, p->tfd);
1523                 break;
1524         }
1525         case ATA_SET_MULTI:
1526                 if (cfis[12] != 0 &&
1527                         (cfis[12] > 128 || (cfis[12] & (cfis[12] - 1)))) {
1528                         p->tfd = ATA_S_ERROR | ATA_S_READY;
1529                         p->tfd |= (ATA_ERROR_ABORT << 8);
1530                 } else {
1531                         p->mult_sectors = cfis[12];
1532                         p->tfd = ATA_S_DSC | ATA_S_READY;
1533                 }
1534                 ahci_write_fis_d2h(p, slot, cfis, p->tfd);
1535                 break;
1536         case ATA_READ:
1537         case ATA_WRITE:
1538         case ATA_READ48:
1539         case ATA_WRITE48:
1540         case ATA_READ_MUL:
1541         case ATA_WRITE_MUL:
1542         case ATA_READ_MUL48:
1543         case ATA_WRITE_MUL48:
1544         case ATA_READ_DMA:
1545         case ATA_WRITE_DMA:
1546         case ATA_READ_DMA48:
1547         case ATA_WRITE_DMA48:
1548         case ATA_READ_FPDMA_QUEUED:
1549         case ATA_WRITE_FPDMA_QUEUED:
1550                 ahci_handle_dma(p, slot, cfis, 0, 0);
1551                 break;
1552         case ATA_FLUSHCACHE:
1553         case ATA_FLUSHCACHE48:
1554                 ahci_handle_flush(p, slot, cfis);
1555                 break;
1556         case ATA_DATA_SET_MANAGEMENT:
1557                 if (cfis[11] == 0 && cfis[3] == ATA_DSM_TRIM &&
1558                     cfis[13] == 0 && cfis[12] == 1) {
1559                         ahci_handle_dsm_trim(p, slot, cfis, 0);
1560                         break;
1561                 }
1562                 ahci_write_fis_d2h(p, slot, cfis,
1563                     (ATA_E_ABORT << 8) | ATA_S_READY | ATA_S_ERROR);
1564                 break;
1565         case ATA_READ_LOG_EXT:
1566         case ATA_READ_LOG_DMA_EXT:
1567                 ahci_handle_read_log(p, slot, cfis);
1568                 break;
1569         case ATA_STANDBY_CMD:
1570                 break;
1571         case ATA_NOP:
1572         case ATA_STANDBY_IMMEDIATE:
1573         case ATA_IDLE_IMMEDIATE:
1574         case ATA_SLEEP:
1575                 ahci_write_fis_d2h(p, slot, cfis, ATA_S_READY | ATA_S_DSC);
1576                 break;
1577         case ATA_ATAPI_IDENTIFY:
1578                 handle_atapi_identify(p, slot, cfis);
1579                 break;
1580         case ATA_PACKET_CMD:
1581                 if (!p->atapi) {
1582                         ahci_write_fis_d2h(p, slot, cfis,
1583                             (ATA_E_ABORT << 8) | ATA_S_READY | ATA_S_ERROR);
1584                 } else
1585                         handle_packet_cmd(p, slot, cfis);
1586                 break;
1587         default:
1588                 WPRINTF("Unsupported cmd:%02x\n", cfis[2]);
1589                 ahci_write_fis_d2h(p, slot, cfis,
1590                     (ATA_E_ABORT << 8) | ATA_S_READY | ATA_S_ERROR);
1591                 break;
1592         }
1593 }
1594
1595 static void
1596 ahci_handle_slot(struct ahci_port *p, int slot)
1597 {
1598         struct ahci_cmd_hdr *hdr;
1599         struct ahci_prdt_entry *prdt;
1600         struct pci_ahci_softc *sc;
1601         uint8_t *cfis;
1602         int cfl;
1603
1604         sc = p->pr_sc;
1605         hdr = (struct ahci_cmd_hdr *)(p->cmd_lst + slot * AHCI_CL_SIZE);
1606         cfl = (hdr->flags & 0x1f) * 4;
1607         cfis = paddr_guest2host(ahci_ctx(sc), hdr->ctba,
1608                         0x80 + hdr->prdtl * sizeof(struct ahci_prdt_entry));
1609         prdt = (struct ahci_prdt_entry *)(cfis + 0x80);
1610
1611 #ifdef AHCI_DEBUG
1612         DPRINTF("\ncfis:");
1613         for (i = 0; i < cfl; i++) {
1614                 if (i % 10 == 0)
1615                         DPRINTF("\n");
1616                 DPRINTF("%02x ", cfis[i]);
1617         }
1618         DPRINTF("\n");
1619
1620         for (i = 0; i < hdr->prdtl; i++) {
1621                 DPRINTF("%d@%08"PRIx64"\n", prdt->dbc & 0x3fffff, prdt->dba);
1622                 prdt++;
1623         }
1624 #endif
1625
1626         if (cfis[0] != FIS_TYPE_REGH2D) {
1627                 WPRINTF("Not a H2D FIS:%02x\n", cfis[0]);
1628                 return;
1629         }
1630
1631         if (cfis[1] & 0x80) {
1632                 ahci_handle_cmd(p, slot, cfis);
1633         } else {
1634                 if (cfis[15] & (1 << 2))
1635                         p->reset = 1;
1636                 else if (p->reset) {
1637                         p->reset = 0;
1638                         ahci_port_reset(p);
1639                 }
1640                 p->ci &= ~(1 << slot);
1641         }
1642 }
1643
1644 static void
1645 ahci_handle_port(struct ahci_port *p)
1646 {
1647         int i;
1648
1649         if (!(p->cmd & AHCI_P_CMD_ST))
1650                 return;
1651
1652         /*
1653          * Search for any new commands to issue ignoring those that
1654          * are already in-flight.
1655          */
1656         for (i = 0; (i < 32) && p->ci; i++) {
1657                 if ((p->ci & (1 << i)) && !(p->pending & (1 << i))) {
1658                         p->cmd &= ~AHCI_P_CMD_CCS_MASK;
1659                         p->cmd |= i << AHCI_P_CMD_CCS_SHIFT;
1660                         ahci_handle_slot(p, i);
1661                 }
1662         }
1663 }
1664
1665 /*
1666  * blockif callback routine - this runs in the context of the blockif
1667  * i/o thread, so the mutex needs to be acquired.
1668  */
1669 static void
1670 ata_ioreq_cb(struct blockif_req *br, int err)
1671 {
1672         struct ahci_cmd_hdr *hdr;
1673         struct ahci_ioreq *aior;
1674         struct ahci_port *p;
1675         struct pci_ahci_softc *sc;
1676         uint32_t tfd;
1677         uint8_t *cfis;
1678         int pending, slot, ncq, dsm;
1679
1680         DPRINTF("%s %d\n", __func__, err);
1681
1682         ncq = dsm = 0;
1683         aior = br->br_param;
1684         p = aior->io_pr;
1685         cfis = aior->cfis;
1686         slot = aior->slot;
1687         pending = aior->prdtl;
1688         sc = p->pr_sc;
1689         hdr = (struct ahci_cmd_hdr *)(p->cmd_lst + slot * AHCI_CL_SIZE);
1690
1691         if (cfis[2] == ATA_WRITE_FPDMA_QUEUED ||
1692                         cfis[2] == ATA_READ_FPDMA_QUEUED)
1693                 ncq = 1;
1694         if (cfis[2] == ATA_DATA_SET_MANAGEMENT)
1695                 dsm = 1;
1696
1697         pthread_mutex_lock(&sc->mtx);
1698
1699         /*
1700          * Delete the blockif request from the busy list
1701          */
1702         TAILQ_REMOVE(&p->iobhd, aior, io_blist);
1703
1704         /*
1705          * Move the blockif request back to the free list
1706          */
1707         STAILQ_INSERT_TAIL(&p->iofhd, aior, io_flist);
1708
1709         if (!err)
1710                 hdr->prdbc = aior->done;
1711
1712         if (dsm) {
1713                 if (aior->done != aior->len && !err) {
1714                         ahci_handle_dsm_trim(p, slot, cfis, aior->done);
1715                         goto out;
1716                 }
1717         } else {
1718                 if (pending && !err) {
1719                         ahci_handle_dma(p, slot, cfis, aior->done,
1720                             hdr->prdtl - pending);
1721                         goto out;
1722                 }
1723         }
1724
1725         if (!err && aior->done == aior->len) {
1726                 tfd = ATA_S_READY | ATA_S_DSC;
1727         } else {
1728                 tfd = (ATA_E_ABORT << 8) | ATA_S_READY | ATA_S_ERROR;
1729         }
1730
1731         if (ncq)
1732                 ahci_write_fis_sdb(p, slot, cfis, tfd);
1733         else
1734                 ahci_write_fis_d2h(p, slot, cfis, tfd);
1735
1736         /*
1737          * This command is now complete.
1738          */
1739         p->pending &= ~(1 << slot);
1740
1741         ahci_check_stopped(p);
1742 out:
1743         pthread_mutex_unlock(&sc->mtx);
1744         DPRINTF("%s exit\n", __func__);
1745 }
1746
1747 static void
1748 atapi_ioreq_cb(struct blockif_req *br, int err)
1749 {
1750         struct ahci_cmd_hdr *hdr;
1751         struct ahci_ioreq *aior;
1752         struct ahci_port *p;
1753         struct pci_ahci_softc *sc;
1754         uint8_t *cfis;
1755         uint32_t tfd;
1756         int pending, slot;
1757
1758         DPRINTF("%s %d\n", __func__, err);
1759
1760         aior = br->br_param;
1761         p = aior->io_pr;
1762         cfis = aior->cfis;
1763         slot = aior->slot;
1764         pending = aior->prdtl;
1765         sc = p->pr_sc;
1766         hdr = (struct ahci_cmd_hdr *)(p->cmd_lst + aior->slot * AHCI_CL_SIZE);
1767
1768         pthread_mutex_lock(&sc->mtx);
1769
1770         /*
1771          * Delete the blockif request from the busy list
1772          */
1773         TAILQ_REMOVE(&p->iobhd, aior, io_blist);
1774
1775         /*
1776          * Move the blockif request back to the free list
1777          */
1778         STAILQ_INSERT_TAIL(&p->iofhd, aior, io_flist);
1779
1780         if (!err)
1781                 hdr->prdbc = aior->done;
1782
1783         if (pending && !err) {
1784                 atapi_read(p, slot, cfis, aior->done, hdr->prdtl - pending);
1785                 goto out;
1786         }
1787
1788         if (!err && aior->done == aior->len) {
1789                 tfd = ATA_S_READY | ATA_S_DSC;
1790         } else {
1791                 p->sense_key = ATA_SENSE_ILLEGAL_REQUEST;
1792                 p->asc = 0x21;
1793                 tfd = (p->sense_key << 12) | ATA_S_READY | ATA_S_ERROR;
1794         }
1795
1796         cfis[4] = (cfis[4] & ~7) | ATA_I_CMD | ATA_I_IN;
1797         ahci_write_fis_d2h(p, slot, cfis, tfd);
1798
1799         /*
1800          * This command is now complete.
1801          */
1802         p->pending &= ~(1 << slot);
1803
1804         ahci_check_stopped(p);
1805 out:
1806         pthread_mutex_unlock(&sc->mtx);
1807         DPRINTF("%s exit\n", __func__);
1808 }
1809
1810 static void
1811 pci_ahci_ioreq_init(struct ahci_port *pr)
1812 {
1813         struct ahci_ioreq *vr;
1814         int i;
1815
1816         pr->ioqsz = blockif_queuesz(pr->bctx);
1817         pr->ioreq = calloc(pr->ioqsz, sizeof(struct ahci_ioreq));
1818         STAILQ_INIT(&pr->iofhd);
1819
1820         /*
1821          * Add all i/o request entries to the free queue
1822          */
1823         for (i = 0; i < pr->ioqsz; i++) {
1824                 vr = &pr->ioreq[i];
1825                 vr->io_pr = pr;
1826                 if (!pr->atapi)
1827                         vr->io_req.br_callback = ata_ioreq_cb;
1828                 else
1829                         vr->io_req.br_callback = atapi_ioreq_cb;
1830                 vr->io_req.br_param = vr;
1831                 STAILQ_INSERT_TAIL(&pr->iofhd, vr, io_flist);
1832         }
1833
1834         TAILQ_INIT(&pr->iobhd);
1835 }
1836
1837 static void
1838 pci_ahci_port_write(struct pci_ahci_softc *sc, uint64_t offset, uint64_t value)
1839 {
1840         int port = (offset - AHCI_OFFSET) / AHCI_STEP;
1841         offset = (offset - AHCI_OFFSET) % AHCI_STEP;
1842         struct ahci_port *p = &sc->port[port];
1843
1844         DPRINTF("pci_ahci_port %d: write offset 0x%"PRIx64" value 0x%"PRIx64"\n",
1845                 port, offset, value);
1846
1847         switch (offset) {
1848         case AHCI_P_CLB:
1849                 p->clb = value;
1850                 break;
1851         case AHCI_P_CLBU:
1852                 p->clbu = value;
1853                 break;
1854         case AHCI_P_FB:
1855                 p->fb = value;
1856                 break;
1857         case AHCI_P_FBU:
1858                 p->fbu = value;
1859                 break;
1860         case AHCI_P_IS:
1861                 p->is &= ~value;
1862                 break;
1863         case AHCI_P_IE:
1864                 p->ie = value & 0xFDC000FF;
1865                 ahci_generate_intr(sc);
1866                 break;
1867         case AHCI_P_CMD:
1868         {
1869                 p->cmd = value;
1870
1871                 if (!(value & AHCI_P_CMD_ST)) {
1872                         ahci_port_stop(p);
1873                 } else {
1874                         uint64_t clb;
1875
1876                         p->cmd |= AHCI_P_CMD_CR;
1877                         clb = (uint64_t)p->clbu << 32 | p->clb;
1878                         p->cmd_lst = paddr_guest2host(ahci_ctx(sc), clb,
1879                                         AHCI_CL_SIZE * AHCI_MAX_SLOTS);
1880                 }
1881
1882                 if (value & AHCI_P_CMD_FRE) {
1883                         uint64_t fb;
1884
1885                         p->cmd |= AHCI_P_CMD_FR;
1886                         fb = (uint64_t)p->fbu << 32 | p->fb;
1887                         /* we don't support FBSCP, so rfis size is 256Bytes */
1888                         p->rfis = paddr_guest2host(ahci_ctx(sc), fb, 256);
1889                 } else {
1890                         p->cmd &= ~AHCI_P_CMD_FR;
1891                 }
1892
1893                 if (value & AHCI_P_CMD_CLO) {
1894                         p->tfd = 0;
1895                         p->cmd &= ~AHCI_P_CMD_CLO;
1896                 }
1897
1898                 ahci_handle_port(p);
1899                 break;
1900         }
1901         case AHCI_P_TFD:
1902         case AHCI_P_SIG:
1903         case AHCI_P_SSTS:
1904                 WPRINTF("pci_ahci_port: read only registers 0x%"PRIx64"\n", offset);
1905                 break;
1906         case AHCI_P_SCTL:
1907                 p->sctl = value;
1908                 if (!(p->cmd & AHCI_P_CMD_ST)) {
1909                         if (value & ATA_SC_DET_RESET)
1910                                 ahci_port_reset(p);
1911                 }
1912                 break;
1913         case AHCI_P_SERR:
1914                 p->serr &= ~value;
1915                 break;
1916         case AHCI_P_SACT:
1917                 p->sact |= value;
1918                 break;
1919         case AHCI_P_CI:
1920                 p->ci |= value;
1921                 ahci_handle_port(p);
1922                 break;
1923         case AHCI_P_SNTF:
1924         case AHCI_P_FBS:
1925         default:
1926                 break;
1927         }
1928 }
1929
1930 static void
1931 pci_ahci_host_write(struct pci_ahci_softc *sc, uint64_t offset, uint64_t value)
1932 {
1933         DPRINTF("pci_ahci_host: write offset 0x%"PRIx64" value 0x%"PRIx64"\n",
1934                 offset, value);
1935
1936         switch (offset) {
1937         case AHCI_CAP:
1938         case AHCI_PI:
1939         case AHCI_VS:
1940         case AHCI_CAP2:
1941                 DPRINTF("pci_ahci_host: read only registers 0x%"PRIx64"\n", offset);
1942                 break;
1943         case AHCI_GHC:
1944                 if (value & AHCI_GHC_HR)
1945                         ahci_reset(sc);
1946                 else if (value & AHCI_GHC_IE) {
1947                         sc->ghc |= AHCI_GHC_IE;
1948                         ahci_generate_intr(sc);
1949                 }
1950                 break;
1951         case AHCI_IS:
1952                 sc->is &= ~value;
1953                 ahci_generate_intr(sc);
1954                 break;
1955         default:
1956                 break;
1957         }
1958 }
1959
1960 static void
1961 pci_ahci_write(struct vmctx *ctx, int vcpu, struct pci_devinst *pi,
1962                 int baridx, uint64_t offset, int size, uint64_t value)
1963 {
1964         struct pci_ahci_softc *sc = pi->pi_arg;
1965
1966         assert(baridx == 5);
1967         assert(size == 4);
1968
1969         pthread_mutex_lock(&sc->mtx);
1970
1971         if (offset < AHCI_OFFSET)
1972                 pci_ahci_host_write(sc, offset, value);
1973         else if (offset < AHCI_OFFSET + sc->ports * AHCI_STEP)
1974                 pci_ahci_port_write(sc, offset, value);
1975         else
1976                 WPRINTF("pci_ahci: unknown i/o write offset 0x%"PRIx64"\n", offset);
1977
1978         pthread_mutex_unlock(&sc->mtx);
1979 }
1980
1981 static uint64_t
1982 pci_ahci_host_read(struct pci_ahci_softc *sc, uint64_t offset)
1983 {
1984         uint32_t value;
1985
1986         switch (offset) {
1987         case AHCI_CAP:
1988         case AHCI_GHC:
1989         case AHCI_IS:
1990         case AHCI_PI:
1991         case AHCI_VS:
1992         case AHCI_CCCC:
1993         case AHCI_CCCP:
1994         case AHCI_EM_LOC:
1995         case AHCI_EM_CTL:
1996         case AHCI_CAP2:
1997         {
1998                 uint32_t *p = &sc->cap;
1999                 p += (offset - AHCI_CAP) / sizeof(uint32_t);
2000                 value = *p;
2001                 break;
2002         }
2003         default:
2004                 value = 0;
2005                 break;
2006         }
2007         DPRINTF("pci_ahci_host: read offset 0x%"PRIx64" value 0x%x\n",
2008                 offset, value);
2009
2010         return (value);
2011 }
2012
2013 static uint64_t
2014 pci_ahci_port_read(struct pci_ahci_softc *sc, uint64_t offset)
2015 {
2016         uint32_t value;
2017         int port = (offset - AHCI_OFFSET) / AHCI_STEP;
2018         offset = (offset - AHCI_OFFSET) % AHCI_STEP;
2019
2020         switch (offset) {
2021         case AHCI_P_CLB:
2022         case AHCI_P_CLBU:
2023         case AHCI_P_FB:
2024         case AHCI_P_FBU:
2025         case AHCI_P_IS:
2026         case AHCI_P_IE:
2027         case AHCI_P_CMD:
2028         case AHCI_P_TFD:
2029         case AHCI_P_SIG:
2030         case AHCI_P_SSTS:
2031         case AHCI_P_SCTL:
2032         case AHCI_P_SERR:
2033         case AHCI_P_SACT:
2034         case AHCI_P_CI:
2035         case AHCI_P_SNTF:
2036         case AHCI_P_FBS:
2037         {
2038                 uint32_t *p= &sc->port[port].clb;
2039                 p += (offset - AHCI_P_CLB) / sizeof(uint32_t);
2040                 value = *p;
2041                 break;
2042         }
2043         default:
2044                 value = 0;
2045                 break;
2046         }
2047
2048         DPRINTF("pci_ahci_port %d: read offset 0x%"PRIx64" value 0x%x\n",
2049                 port, offset, value);
2050
2051         return value;
2052 }
2053
2054 static uint64_t
2055 pci_ahci_read(struct vmctx *ctx, int vcpu, struct pci_devinst *pi, int baridx,
2056     uint64_t offset, int size)
2057 {
2058         struct pci_ahci_softc *sc = pi->pi_arg;
2059         uint32_t value;
2060
2061         assert(baridx == 5);
2062         assert(size == 4);
2063
2064         pthread_mutex_lock(&sc->mtx);
2065
2066         if (offset < AHCI_OFFSET)
2067                 value = pci_ahci_host_read(sc, offset);
2068         else if (offset < AHCI_OFFSET + sc->ports * AHCI_STEP)
2069                 value = pci_ahci_port_read(sc, offset);
2070         else {
2071                 value = 0;
2072                 WPRINTF("pci_ahci: unknown i/o read offset 0x%"PRIx64"\n", offset);
2073         }
2074
2075         pthread_mutex_unlock(&sc->mtx);
2076
2077         return (value);
2078 }
2079
2080 static int
2081 pci_ahci_init(struct vmctx *ctx, struct pci_devinst *pi, char *opts, int atapi)
2082 {
2083         char bident[sizeof("XX:X:X")];
2084         struct blockif_ctxt *bctxt;
2085         struct pci_ahci_softc *sc;
2086         int ret, slots;
2087
2088         ret = 0;
2089
2090         if (opts == NULL) {
2091                 fprintf(stderr, "pci_ahci: backing device required\n");
2092                 return (1);
2093         }
2094
2095 #ifdef AHCI_DEBUG
2096         dbg = fopen("/tmp/log", "w+");
2097 #endif
2098
2099         sc = calloc(1, sizeof(struct pci_ahci_softc));
2100         pi->pi_arg = sc;
2101         sc->asc_pi = pi;
2102         sc->ports = MAX_PORTS;
2103
2104         /*
2105          * Only use port 0 for a backing device. All other ports will be
2106          * marked as unused
2107          */
2108         sc->port[0].atapi = atapi;
2109
2110         /*
2111          * Attempt to open the backing image. Use the PCI
2112          * slot/func for the identifier string.
2113          */
2114         snprintf(bident, sizeof(bident), "%d:%d", pi->pi_slot, pi->pi_func);
2115         bctxt = blockif_open(opts, bident);
2116         if (bctxt == NULL) {
2117                 ret = 1;
2118                 goto open_fail;
2119         }
2120         sc->port[0].bctx = bctxt;
2121         sc->port[0].pr_sc = sc;
2122
2123         /*
2124          * Allocate blockif request structures and add them
2125          * to the free list
2126          */
2127         pci_ahci_ioreq_init(&sc->port[0]);
2128
2129         pthread_mutex_init(&sc->mtx, NULL);
2130
2131         /* Intel ICH8 AHCI */
2132         slots = sc->port[0].ioqsz;
2133         if (slots > 32)
2134                 slots = 32;
2135         --slots;
2136         sc->cap = AHCI_CAP_64BIT | AHCI_CAP_SNCQ | AHCI_CAP_SSNTF |
2137             AHCI_CAP_SMPS | AHCI_CAP_SSS | AHCI_CAP_SALP |
2138             AHCI_CAP_SAL | AHCI_CAP_SCLO | (0x3 << AHCI_CAP_ISS_SHIFT)|
2139             AHCI_CAP_PMD | AHCI_CAP_SSC | AHCI_CAP_PSC |
2140             (slots << AHCI_CAP_NCS_SHIFT) | AHCI_CAP_SXS | (sc->ports - 1);
2141
2142         /* Only port 0 implemented */
2143         sc->pi = 1;
2144         sc->vs = 0x10300;
2145         sc->cap2 = AHCI_CAP2_APST;
2146         ahci_reset(sc);
2147
2148         pci_set_cfgdata16(pi, PCIR_DEVICE, 0x2821);
2149         pci_set_cfgdata16(pi, PCIR_VENDOR, 0x8086);
2150         pci_set_cfgdata8(pi, PCIR_CLASS, PCIC_STORAGE);
2151         pci_set_cfgdata8(pi, PCIR_SUBCLASS, PCIS_STORAGE_SATA);
2152         pci_set_cfgdata8(pi, PCIR_PROGIF, PCIP_STORAGE_SATA_AHCI_1_0);
2153         pci_emul_add_msicap(pi, 1);
2154         pci_emul_alloc_bar(pi, 5, PCIBAR_MEM32,
2155             AHCI_OFFSET + sc->ports * AHCI_STEP);
2156
2157         pci_lintr_request(pi);
2158
2159 open_fail:
2160         if (ret) {
2161                 blockif_close(sc->port[0].bctx);
2162                 free(sc);
2163         }
2164
2165         return (ret);
2166 }
2167
2168 static int
2169 pci_ahci_hd_init(struct vmctx *ctx, struct pci_devinst *pi, char *opts)
2170 {
2171
2172         return (pci_ahci_init(ctx, pi, opts, 0));
2173 }
2174
2175 static int
2176 pci_ahci_atapi_init(struct vmctx *ctx, struct pci_devinst *pi, char *opts)
2177 {
2178
2179         return (pci_ahci_init(ctx, pi, opts, 1));
2180 }
2181
2182 /*
2183  * Use separate emulation names to distinguish drive and atapi devices
2184  */
2185 struct pci_devemu pci_de_ahci_hd = {
2186         .pe_emu =       "ahci-hd",
2187         .pe_init =      pci_ahci_hd_init,
2188         .pe_barwrite =  pci_ahci_write,
2189         .pe_barread =   pci_ahci_read
2190 };
2191 PCI_EMUL_SET(pci_de_ahci_hd);
2192
2193 struct pci_devemu pci_de_ahci_cd = {
2194         .pe_emu =       "ahci-cd",
2195         .pe_init =      pci_ahci_atapi_init,
2196         .pe_barwrite =  pci_ahci_write,
2197         .pe_barread =   pci_ahci_read
2198 };
2199 PCI_EMUL_SET(pci_de_ahci_cd);