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