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_checksum(uint8_t *buf, int size)
 798 {
 799         int i;
 800         uint8_t sum = 0;
 801
 802         for (i = 0; i < size - 1; i++)
 803                 sum += buf[i];
 804         buf[size - 1] = 0x100 - sum;
 805 }
 806
 807 static void
 808 ahci_handle_read_log(struct ahci_port *p, int slot, uint8_t *cfis)
 809 {
 810         struct ahci_cmd_hdr *hdr;
 811         uint8_t buf[512];
 812
 813         hdr = (struct ahci_cmd_hdr *)(p->cmd_lst + slot * AHCI_CL_SIZE);
 814         if (p->atapi || hdr->prdtl == 0 || cfis[4] != 0x10 ||
 815             cfis[5] != 0 || cfis[9] != 0 || cfis[12] != 1 || cfis[13] != 0) {
 816                 ahci_write_fis_d2h(p, slot, cfis,
 817                     (ATA_E_ABORT << 8) | ATA_S_READY | ATA_S_ERROR);
 818                 return;
 819         }
 820
 821         memset(buf, 0, sizeof(buf));
 822         memcpy(buf, p->err_cfis, sizeof(p->err_cfis));
 823         ahci_checksum(buf, sizeof(buf));
 824
 825         if (cfis[2] == ATA_READ_LOG_EXT)
 826                 ahci_write_fis_piosetup(p);
 827         write_prdt(p, slot, cfis, (void *)buf, sizeof(buf));
 828         ahci_write_fis_d2h(p, slot, cfis, ATA_S_DSC | ATA_S_READY);
 829 }
 830
 831 static void
 832 handle_identify(struct ahci_port *p, int slot, uint8_t *cfis)
 833 {
 834         struct ahci_cmd_hdr *hdr;
 835
 836         hdr = (struct ahci_cmd_hdr *)(p->cmd_lst + slot * AHCI_CL_SIZE);
 837         if (p->atapi || hdr->prdtl == 0) {
 838                 ahci_write_fis_d2h(p, slot, cfis,
 839                     (ATA_E_ABORT << 8) | ATA_S_READY | ATA_S_ERROR);
 840         } else {
 841                 uint16_t buf[256];
 842                 uint64_t sectors;
 843                 int sectsz, psectsz, psectoff, candelete;
 844                 uint16_t cyl;
 845                 uint8_t sech, heads;
 846
 847                 candelete = blockif_candelete(p->bctx);
 848                 sectsz = blockif_sectsz(p->bctx);
 849                 sectors = blockif_size(p->bctx) / sectsz;
 850                 blockif_chs(p->bctx, &cyl, &heads, &sech);
 851                 blockif_psectsz(p->bctx, &psectsz, &psectoff);
 852                 memset(buf, 0, sizeof(buf));
 853                 buf[0] = 0x0040;
 854                 buf[1] = cyl;
 855                 buf[3] = heads;
 856                 buf[6] = sech;
 857                 /* TODO emulate different serial? */
 858                 ata_string((uint8_t *)(buf+10), "123456", 20);
 859                 ata_string((uint8_t *)(buf+23), "001", 8);
 860                 ata_string((uint8_t *)(buf+27), "BHYVE SATA DISK", 40);
 861                 buf[47] = (0x8000 | 128);
 862                 buf[48] = 0x1;
 863                 buf[49] = (1 << 8 | 1 << 9 | 1 << 11);
 864                 buf[50] = (1 << 14);
 865                 buf[53] = (1 << 1 | 1 << 2);
 866                 if (p->mult_sectors)
 867                         buf[59] = (0x100 | p->mult_sectors);
 868                 if (sectors <= 0x0fffffff) {
 869                         buf[60] = sectors;
 870                         buf[61] = (sectors >> 16);
 871                 } else {
 872                         buf[60] = 0xffff;
 873                         buf[61] = 0x0fff;
 874                 }
 875                 buf[63] = 0x7;
 876                 if (p->xfermode & ATA_WDMA0)
 877                         buf[63] |= (1 << ((p->xfermode & 7) + 8));
 878                 buf[64] = 0x3;
 879                 buf[65] = 120;
 880                 buf[66] = 120;
 881                 buf[67] = 120;
 882                 buf[68] = 120;
 883                 buf[69] = 0;
 884                 buf[75] = 31;
 885                 buf[76] = (ATA_SATA_GEN1 | ATA_SATA_GEN2 | ATA_SATA_GEN3 |
 886                            ATA_SUPPORT_NCQ);
 887                 buf[77] = (ATA_SUPPORT_RCVSND_FPDMA_QUEUED |
 888                            (p->ssts & ATA_SS_SPD_MASK) >> 3);
 889                 buf[80] = 0x3f0;
 890                 buf[81] = 0x28;
 891                 buf[82] = (ATA_SUPPORT_POWERMGT | ATA_SUPPORT_WRITECACHE|
 892                            ATA_SUPPORT_LOOKAHEAD | ATA_SUPPORT_NOP);
 893                 buf[83] = (ATA_SUPPORT_ADDRESS48 | ATA_SUPPORT_FLUSHCACHE |
 894                            ATA_SUPPORT_FLUSHCACHE48 | 1 << 14);
 895                 buf[84] = (1 << 14);
 896                 buf[85] = (ATA_SUPPORT_POWERMGT | ATA_SUPPORT_WRITECACHE|
 897                            ATA_SUPPORT_LOOKAHEAD | ATA_SUPPORT_NOP);
 898                 buf[86] = (ATA_SUPPORT_ADDRESS48 | ATA_SUPPORT_FLUSHCACHE |
 899                            ATA_SUPPORT_FLUSHCACHE48 | 1 << 15);
 900                 buf[87] = (1 << 14);
 901                 buf[88] = 0x7f;
 902                 if (p->xfermode & ATA_UDMA0)
 903                         buf[88] |= (1 << ((p->xfermode & 7) + 8));
 904                 buf[93] = (1 | 1 <<14);
 905                 buf[100] = sectors;
 906                 buf[101] = (sectors >> 16);
 907                 buf[102] = (sectors >> 32);
 908                 buf[103] = (sectors >> 48);
 909                 if (candelete) {
 910                         buf[69] |= ATA_SUPPORT_RZAT | ATA_SUPPORT_DRAT;
 911                         buf[105] = 1;
 912                         buf[169] = ATA_SUPPORT_DSM_TRIM;
 913                 }
 914                 buf[106] = 0x4000;
 915                 buf[209] = 0x4000;
 916                 if (psectsz > sectsz) {
 917                         buf[106] |= 0x2000;
 918                         buf[106] |= ffsl(psectsz / sectsz) - 1;
 919                         buf[209] |= (psectoff / sectsz);
 920                 }
 921                 if (sectsz > 512) {
 922                         buf[106] |= 0x1000;
 923                         buf[117] = sectsz / 2;
 924                         buf[118] = ((sectsz / 2) >> 16);
 925                 }
 926                 buf[119] = (ATA_SUPPORT_RWLOGDMAEXT | 1 << 14);
 927                 buf[120] = (ATA_SUPPORT_RWLOGDMAEXT | 1 << 14);
 928                 buf[222] = 0x1020;
 929                 buf[255] = 0x00a5;
 930                 ahci_checksum((uint8_t *)buf, sizeof(buf));
 931                 ahci_write_fis_piosetup(p);
 932                 write_prdt(p, slot, cfis, (void *)buf, sizeof(buf));
 933                 ahci_write_fis_d2h(p, slot, cfis, ATA_S_DSC | ATA_S_READY);
 934         }
 935 }
 936
 937 static void
 938 handle_atapi_identify(struct ahci_port *p, int slot, uint8_t *cfis)
 939 {
 940         if (!p->atapi) {
 941                 ahci_write_fis_d2h(p, slot, cfis,
 942                     (ATA_E_ABORT << 8) | ATA_S_READY | ATA_S_ERROR);
 943         } else {
 944                 uint16_t buf[256];
 945
 946                 memset(buf, 0, sizeof(buf));
 947                 buf[0] = (2 << 14 | 5 << 8 | 1 << 7 | 2 << 5);
 948                 /* TODO emulate different serial? */
 949                 ata_string((uint8_t *)(buf+10), "123456", 20);
 950                 ata_string((uint8_t *)(buf+23), "001", 8);
 951                 ata_string((uint8_t *)(buf+27), "BHYVE SATA DVD ROM", 40);
 952                 buf[49] = (1 << 9 | 1 << 8);
 953                 buf[50] = (1 << 14 | 1);
 954                 buf[53] = (1 << 2 | 1 << 1);
 955                 buf[62] = 0x3f;
 956                 buf[63] = 7;
 957                 if (p->xfermode & ATA_WDMA0)
 958                         buf[63] |= (1 << ((p->xfermode & 7) + 8));
 959                 buf[64] = 3;
 960                 buf[65] = 120;
 961                 buf[66] = 120;
 962                 buf[67] = 120;
 963                 buf[68] = 120;
 964                 buf[76] = (ATA_SATA_GEN1 | ATA_SATA_GEN2 | ATA_SATA_GEN3);
 965                 buf[77] = ((p->ssts & ATA_SS_SPD_MASK) >> 3);
 966                 buf[78] = (1 << 5);
 967                 buf[80] = 0x3f0;
 968                 buf[82] = (ATA_SUPPORT_POWERMGT | ATA_SUPPORT_PACKET |
 969                            ATA_SUPPORT_RESET | ATA_SUPPORT_NOP);
 970                 buf[83] = (1 << 14);
 971                 buf[84] = (1 << 14);
 972                 buf[85] = (ATA_SUPPORT_POWERMGT | ATA_SUPPORT_PACKET |
 973                            ATA_SUPPORT_RESET | ATA_SUPPORT_NOP);
 974                 buf[87] = (1 << 14);
 975                 buf[88] = 0x7f;
 976                 if (p->xfermode & ATA_UDMA0)
 977                         buf[88] |= (1 << ((p->xfermode & 7) + 8));
 978                 buf[222] = 0x1020;
 979                 buf[255] = 0x00a5;
 980                 ahci_checksum((uint8_t *)buf, sizeof(buf));
 981                 ahci_write_fis_piosetup(p);
 982                 write_prdt(p, slot, cfis, (void *)buf, sizeof(buf));
 983                 ahci_write_fis_d2h(p, slot, cfis, ATA_S_DSC | ATA_S_READY);
 984         }
 985 }
 986
 987 static void
 988 atapi_inquiry(struct ahci_port *p, int slot, uint8_t *cfis)
 989 {
 990         uint8_t buf[36];
 991         uint8_t *acmd;
 992         int len;
 993         uint32_t tfd;
 994
 995         acmd = cfis + 0x40;
 996
 997         if (acmd[1] & 1) {              /* VPD */
 998                 if (acmd[2] == 0) {     /* Supported VPD pages */
 999                         buf[0] = 0x05;
1000                         buf[1] = 0;
1001                         buf[2] = 0;
1002                         buf[3] = 1;
1003                         buf[4] = 0;
1004                         len = 4 + buf[3];
1005                 } else {
1006                         p->sense_key = ATA_SENSE_ILLEGAL_REQUEST;
1007                         p->asc = 0x24;
1008                         tfd = (p->sense_key << 12) | ATA_S_READY | ATA_S_ERROR;
1009                         cfis[4] = (cfis[4] & ~7) | ATA_I_CMD | ATA_I_IN;
1010                         ahci_write_fis_d2h(p, slot, cfis, tfd);
1011                         return;
1012                 }
1013         } else {
1014                 buf[0] = 0x05;
1015                 buf[1] = 0x80;
1016                 buf[2] = 0x00;
1017                 buf[3] = 0x21;
1018                 buf[4] = 31;
1019                 buf[5] = 0;
1020                 buf[6] = 0;
1021                 buf[7] = 0;
1022                 atapi_string(buf + 8, "BHYVE", 8);
1023                 atapi_string(buf + 16, "BHYVE DVD-ROM", 16);
1024                 atapi_string(buf + 32, "001", 4);
1025                 len = sizeof(buf);
1026         }
1027
1028         if (len > acmd[4])
1029                 len = acmd[4];
1030         cfis[4] = (cfis[4] & ~7) | ATA_I_CMD | ATA_I_IN;
1031         write_prdt(p, slot, cfis, buf, len);
1032         ahci_write_fis_d2h(p, slot, cfis, ATA_S_READY | ATA_S_DSC);
1033 }
1034
1035 static void
1036 atapi_read_capacity(struct ahci_port *p, int slot, uint8_t *cfis)
1037 {
1038         uint8_t buf[8];
1039         uint64_t sectors;
1040
1041         sectors = blockif_size(p->bctx) / 2048;
1042         be32enc(buf, sectors - 1);
1043         be32enc(buf + 4, 2048);
1044         cfis[4] = (cfis[4] & ~7) | ATA_I_CMD | ATA_I_IN;
1045         write_prdt(p, slot, cfis, buf, sizeof(buf));
1046         ahci_write_fis_d2h(p, slot, cfis, ATA_S_READY | ATA_S_DSC);
1047 }
1048
1049 static void
1050 atapi_read_toc(struct ahci_port *p, int slot, uint8_t *cfis)
1051 {
1052         uint8_t *acmd;
1053         uint8_t format;
1054         int len;
1055
1056         acmd = cfis + 0x40;
1057
1058         len = be16dec(acmd + 7);
1059         format = acmd[9] >> 6;
1060         switch (format) {
1061         case 0:
1062         {
1063                 int msf, size;
1064                 uint64_t sectors;
1065                 uint8_t start_track, buf[20], *bp;
1066
1067                 msf = (acmd[1] >> 1) & 1;
1068                 start_track = acmd[6];
1069                 if (start_track > 1 && start_track != 0xaa) {
1070                         uint32_t tfd;
1071                         p->sense_key = ATA_SENSE_ILLEGAL_REQUEST;
1072                         p->asc = 0x24;
1073                         tfd = (p->sense_key << 12) | ATA_S_READY | ATA_S_ERROR;
1074                         cfis[4] = (cfis[4] & ~7) | ATA_I_CMD | ATA_I_IN;
1075                         ahci_write_fis_d2h(p, slot, cfis, tfd);
1076                         return;
1077                 }
1078                 bp = buf + 2;
1079                 *bp++ = 1;
1080                 *bp++ = 1;
1081                 if (start_track <= 1) {
1082                         *bp++ = 0;
1083                         *bp++ = 0x14;
1084                         *bp++ = 1;
1085                         *bp++ = 0;
1086                         if (msf) {
1087                                 *bp++ = 0;
1088                                 lba_to_msf(bp, 0);
1089                                 bp += 3;
1090                         } else {
1091                                 *bp++ = 0;
1092                                 *bp++ = 0;
1093                                 *bp++ = 0;
1094                                 *bp++ = 0;
1095                         }
1096                 }
1097                 *bp++ = 0;
1098                 *bp++ = 0x14;
1099                 *bp++ = 0xaa;
1100                 *bp++ = 0;
1101                 sectors = blockif_size(p->bctx) / blockif_sectsz(p->bctx);
1102                 sectors >>= 2;
1103                 if (msf) {
1104                         *bp++ = 0;
1105                         lba_to_msf(bp, sectors);
1106                         bp += 3;
1107                 } else {
1108                         be32enc(bp, sectors);
1109                         bp += 4;
1110                 }
1111                 size = bp - buf;
1112                 be16enc(buf, size - 2);
1113                 if (len > size)
1114                         len = size;
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 1:
1121         {
1122                 uint8_t buf[12];
1123
1124                 memset(buf, 0, sizeof(buf));
1125                 buf[1] = 0xa;
1126                 buf[2] = 0x1;
1127                 buf[3] = 0x1;
1128                 if (len > sizeof(buf))
1129                         len = sizeof(buf);
1130                 write_prdt(p, slot, cfis, buf, len);
1131                 cfis[4] = (cfis[4] & ~7) | ATA_I_CMD | ATA_I_IN;
1132                 ahci_write_fis_d2h(p, slot, cfis, ATA_S_READY | ATA_S_DSC);
1133                 break;
1134         }
1135         case 2:
1136         {
1137                 int msf, size;
1138                 uint64_t sectors;
1139                 uint8_t start_track, *bp, buf[50];
1140
1141                 msf = (acmd[1] >> 1) & 1;
1142                 start_track = acmd[6];
1143                 bp = buf + 2;
1144                 *bp++ = 1;
1145                 *bp++ = 1;
1146
1147                 *bp++ = 1;
1148                 *bp++ = 0x14;
1149                 *bp++ = 0;
1150                 *bp++ = 0xa0;
1151                 *bp++ = 0;
1152                 *bp++ = 0;
1153                 *bp++ = 0;
1154                 *bp++ = 0;
1155                 *bp++ = 1;
1156                 *bp++ = 0;
1157                 *bp++ = 0;
1158
1159                 *bp++ = 1;
1160                 *bp++ = 0x14;
1161                 *bp++ = 0;
1162                 *bp++ = 0xa1;
1163                 *bp++ = 0;
1164                 *bp++ = 0;
1165                 *bp++ = 0;
1166                 *bp++ = 0;
1167                 *bp++ = 1;
1168                 *bp++ = 0;
1169                 *bp++ = 0;
1170
1171                 *bp++ = 1;
1172                 *bp++ = 0x14;
1173                 *bp++ = 0;
1174                 *bp++ = 0xa2;
1175                 *bp++ = 0;
1176                 *bp++ = 0;
1177                 *bp++ = 0;
1178                 sectors = blockif_size(p->bctx) / blockif_sectsz(p->bctx);
1179                 sectors >>= 2;
1180                 if (msf) {
1181                         *bp++ = 0;
1182                         lba_to_msf(bp, sectors);
1183                         bp += 3;
1184                 } else {
1185                         be32enc(bp, sectors);
1186                         bp += 4;
1187                 }
1188
1189                 *bp++ = 1;
1190                 *bp++ = 0x14;
1191                 *bp++ = 0;
1192                 *bp++ = 1;
1193                 *bp++ = 0;
1194                 *bp++ = 0;
1195                 *bp++ = 0;
1196                 if (msf) {
1197                         *bp++ = 0;
1198                         lba_to_msf(bp, 0);
1199                         bp += 3;
1200                 } else {
1201                         *bp++ = 0;
1202                         *bp++ = 0;
1203                         *bp++ = 0;
1204                         *bp++ = 0;
1205                 }
1206
1207                 size = bp - buf;
1208                 be16enc(buf, size - 2);
1209                 if (len > size)
1210                         len = size;
1211                 write_prdt(p, slot, cfis, buf, len);
1212                 cfis[4] = (cfis[4] & ~7) | ATA_I_CMD | ATA_I_IN;
1213                 ahci_write_fis_d2h(p, slot, cfis, ATA_S_READY | ATA_S_DSC);
1214                 break;
1215         }
1216         default:
1217         {
1218                 uint32_t tfd;
1219
1220                 p->sense_key = ATA_SENSE_ILLEGAL_REQUEST;
1221                 p->asc = 0x24;
1222                 tfd = (p->sense_key << 12) | ATA_S_READY | ATA_S_ERROR;
1223                 cfis[4] = (cfis[4] & ~7) | ATA_I_CMD | ATA_I_IN;
1224                 ahci_write_fis_d2h(p, slot, cfis, tfd);
1225                 break;
1226         }
1227         }
1228 }
1229
1230 static void
1231 atapi_report_luns(struct ahci_port *p, int slot, uint8_t *cfis)
1232 {
1233         uint8_t buf[16];
1234
1235         memset(buf, 0, sizeof(buf));
1236         buf[3] = 8;
1237
1238         cfis[4] = (cfis[4] & ~7) | ATA_I_CMD | ATA_I_IN;
1239         write_prdt(p, slot, cfis, buf, sizeof(buf));
1240         ahci_write_fis_d2h(p, slot, cfis, ATA_S_READY | ATA_S_DSC);
1241 }
1242
1243 static void
1244 atapi_read(struct ahci_port *p, int slot, uint8_t *cfis,
1245                 uint32_t done, int seek)
1246 {
1247         struct ahci_ioreq *aior;
1248         struct ahci_cmd_hdr *hdr;
1249         struct ahci_prdt_entry *prdt;
1250         struct blockif_req *breq;
1251         struct pci_ahci_softc *sc;
1252         uint8_t *acmd;
1253         uint64_t lba;
1254         uint32_t len;
1255         int i, err, iovcnt;
1256
1257         sc = p->pr_sc;
1258         acmd = cfis + 0x40;
1259         hdr = (struct ahci_cmd_hdr *)(p->cmd_lst + slot * AHCI_CL_SIZE);
1260         prdt = (struct ahci_prdt_entry *)(cfis + 0x80);
1261
1262         prdt += seek;
1263         lba = be32dec(acmd + 2);
1264         if (acmd[0] == READ_10)
1265                 len = be16dec(acmd + 7);
1266         else
1267                 len = be32dec(acmd + 6);
1268         if (len == 0) {
1269                 cfis[4] = (cfis[4] & ~7) | ATA_I_CMD | ATA_I_IN;
1270                 ahci_write_fis_d2h(p, slot, cfis, ATA_S_READY | ATA_S_DSC);
1271         }
1272         lba *= 2048;
1273         len *= 2048;
1274
1275         /*
1276          * Pull request off free list
1277          */
1278         aior = STAILQ_FIRST(&p->iofhd);
1279         assert(aior != NULL);
1280         STAILQ_REMOVE_HEAD(&p->iofhd, io_flist);
1281         aior->cfis = cfis;
1282         aior->slot = slot;
1283         aior->len = len;
1284         aior->done = done;
1285         breq = &aior->io_req;
1286         breq->br_offset = lba + done;
1287         iovcnt = hdr->prdtl - seek;
1288         if (iovcnt > BLOCKIF_IOV_MAX) {
1289                 aior->prdtl = iovcnt - BLOCKIF_IOV_MAX;
1290                 iovcnt = BLOCKIF_IOV_MAX;
1291         } else
1292                 aior->prdtl = 0;
1293         breq->br_iovcnt = iovcnt;
1294
1295         /*
1296          * Mark this command in-flight.
1297          */
1298         p->pending |= 1 << slot;
1299
1300         /*
1301          * Stuff request onto busy list
1302          */
1303         TAILQ_INSERT_HEAD(&p->iobhd, aior, io_blist);
1304
1305         /*
1306          * Build up the iovec based on the prdt
1307          */
1308         for (i = 0; i < iovcnt; i++) {
1309                 uint32_t dbcsz;
1310
1311                 dbcsz = (prdt->dbc & DBCMASK) + 1;
1312                 breq->br_iov[i].iov_base = paddr_guest2host(ahci_ctx(sc),
1313                     prdt->dba, dbcsz);
1314                 breq->br_iov[i].iov_len = dbcsz;
1315                 aior->done += dbcsz;
1316                 prdt++;
1317         }
1318         err = blockif_read(p->bctx, breq);
1319         assert(err == 0);
1320 }
1321
1322 static void
1323 atapi_request_sense(struct ahci_port *p, int slot, uint8_t *cfis)
1324 {
1325         uint8_t buf[64];
1326         uint8_t *acmd;
1327         int len;
1328
1329         acmd = cfis + 0x40;
1330         len = acmd[4];
1331         if (len > sizeof(buf))
1332                 len = sizeof(buf);
1333         memset(buf, 0, len);
1334         buf[0] = 0x70 | (1 << 7);
1335         buf[2] = p->sense_key;
1336         buf[7] = 10;
1337         buf[12] = p->asc;
1338         write_prdt(p, slot, cfis, buf, len);
1339         cfis[4] = (cfis[4] & ~7) | ATA_I_CMD | ATA_I_IN;
1340         ahci_write_fis_d2h(p, slot, cfis, ATA_S_READY | ATA_S_DSC);
1341 }
1342
1343 static void
1344 atapi_start_stop_unit(struct ahci_port *p, int slot, uint8_t *cfis)
1345 {
1346         uint8_t *acmd = cfis + 0x40;
1347         uint32_t tfd;
1348
1349         switch (acmd[4] & 3) {
1350         case 0:
1351         case 1:
1352         case 3:
1353                 cfis[4] = (cfis[4] & ~7) | ATA_I_CMD | ATA_I_IN;
1354                 tfd = ATA_S_READY | ATA_S_DSC;
1355                 break;
1356         case 2:
1357                 /* TODO eject media */
1358                 cfis[4] = (cfis[4] & ~7) | ATA_I_CMD | ATA_I_IN;
1359                 p->sense_key = ATA_SENSE_ILLEGAL_REQUEST;
1360                 p->asc = 0x53;
1361                 tfd = (p->sense_key << 12) | ATA_S_READY | ATA_S_ERROR;
1362                 break;
1363         }
1364         ahci_write_fis_d2h(p, slot, cfis, tfd);
1365 }
1366
1367 static void
1368 atapi_mode_sense(struct ahci_port *p, int slot, uint8_t *cfis)
1369 {
1370         uint8_t *acmd;
1371         uint32_t tfd;
1372         uint8_t pc, code;
1373         int len;
1374
1375         acmd = cfis + 0x40;
1376         len = be16dec(acmd + 7);
1377         pc = acmd[2] >> 6;
1378         code = acmd[2] & 0x3f;
1379
1380         switch (pc) {
1381         case 0:
1382                 switch (code) {
1383                 case MODEPAGE_RW_ERROR_RECOVERY:
1384                 {
1385                         uint8_t buf[16];
1386
1387                         if (len > sizeof(buf))
1388                                 len = sizeof(buf);
1389
1390                         memset(buf, 0, sizeof(buf));
1391                         be16enc(buf, 16 - 2);
1392                         buf[2] = 0x70;
1393                         buf[8] = 0x01;
1394                         buf[9] = 16 - 10;
1395                         buf[11] = 0x05;
1396                         write_prdt(p, slot, cfis, buf, len);
1397                         tfd = ATA_S_READY | ATA_S_DSC;
1398                         break;
1399                 }
1400                 case MODEPAGE_CD_CAPABILITIES:
1401                 {
1402                         uint8_t buf[30];
1403
1404                         if (len > sizeof(buf))
1405                                 len = sizeof(buf);
1406
1407                         memset(buf, 0, sizeof(buf));
1408                         be16enc(buf, 30 - 2);
1409                         buf[2] = 0x70;
1410                         buf[8] = 0x2A;
1411                         buf[9] = 30 - 10;
1412                         buf[10] = 0x08;
1413                         buf[12] = 0x71;
1414                         be16enc(&buf[18], 2);
1415                         be16enc(&buf[20], 512);
1416                         write_prdt(p, slot, cfis, buf, len);
1417                         tfd = ATA_S_READY | ATA_S_DSC;
1418                         break;
1419                 }
1420                 default:
1421                         goto error;
1422                         break;
1423                 }
1424                 break;
1425         case 3:
1426                 p->sense_key = ATA_SENSE_ILLEGAL_REQUEST;
1427                 p->asc = 0x39;
1428                 tfd = (p->sense_key << 12) | ATA_S_READY | ATA_S_ERROR;
1429                 break;
1430 error:
1431         case 1:
1432         case 2:
1433                 p->sense_key = ATA_SENSE_ILLEGAL_REQUEST;
1434                 p->asc = 0x24;
1435                 tfd = (p->sense_key << 12) | ATA_S_READY | ATA_S_ERROR;
1436                 break;
1437         }
1438         cfis[4] = (cfis[4] & ~7) | ATA_I_CMD | ATA_I_IN;
1439         ahci_write_fis_d2h(p, slot, cfis, tfd);
1440 }
1441
1442 static void
1443 atapi_get_event_status_notification(struct ahci_port *p, int slot,
1444     uint8_t *cfis)
1445 {
1446         uint8_t *acmd;
1447         uint32_t tfd;
1448
1449         acmd = cfis + 0x40;
1450
1451         /* we don't support asynchronous operation */
1452         if (!(acmd[1] & 1)) {
1453                 p->sense_key = ATA_SENSE_ILLEGAL_REQUEST;
1454                 p->asc = 0x24;
1455                 tfd = (p->sense_key << 12) | ATA_S_READY | ATA_S_ERROR;
1456         } else {
1457                 uint8_t buf[8];
1458                 int len;
1459
1460                 len = be16dec(acmd + 7);
1461                 if (len > sizeof(buf))
1462                         len = sizeof(buf);
1463
1464                 memset(buf, 0, sizeof(buf));
1465                 be16enc(buf, 8 - 2);
1466                 buf[2] = 0x04;
1467                 buf[3] = 0x10;
1468                 buf[5] = 0x02;
1469                 write_prdt(p, slot, cfis, buf, len);
1470                 tfd = ATA_S_READY | ATA_S_DSC;
1471         }
1472         cfis[4] = (cfis[4] & ~7) | ATA_I_CMD | ATA_I_IN;
1473         ahci_write_fis_d2h(p, slot, cfis, tfd);
1474 }
1475
1476 static void
1477 handle_packet_cmd(struct ahci_port *p, int slot, uint8_t *cfis)
1478 {
1479         uint8_t *acmd;
1480
1481         acmd = cfis + 0x40;
1482
1483 #ifdef AHCI_DEBUG
1484         {
1485                 int i;
1486                 DPRINTF("ACMD:");
1487                 for (i = 0; i < 16; i++)
1488                         DPRINTF("%02x ", acmd[i]);
1489                 DPRINTF("\n");
1490         }
1491 #endif
1492
1493         switch (acmd[0]) {
1494         case TEST_UNIT_READY:
1495                 cfis[4] = (cfis[4] & ~7) | ATA_I_CMD | ATA_I_IN;
1496                 ahci_write_fis_d2h(p, slot, cfis, ATA_S_READY | ATA_S_DSC);
1497                 break;
1498         case INQUIRY:
1499                 atapi_inquiry(p, slot, cfis);
1500                 break;
1501         case READ_CAPACITY:
1502                 atapi_read_capacity(p, slot, cfis);
1503                 break;
1504         case PREVENT_ALLOW:
1505                 /* TODO */
1506                 cfis[4] = (cfis[4] & ~7) | ATA_I_CMD | ATA_I_IN;
1507                 ahci_write_fis_d2h(p, slot, cfis, ATA_S_READY | ATA_S_DSC);
1508                 break;
1509         case READ_TOC:
1510                 atapi_read_toc(p, slot, cfis);
1511                 break;
1512         case REPORT_LUNS:
1513                 atapi_report_luns(p, slot, cfis);
1514                 break;
1515         case READ_10:
1516         case READ_12:
1517                 atapi_read(p, slot, cfis, 0, 0);
1518                 break;
1519         case REQUEST_SENSE:
1520                 atapi_request_sense(p, slot, cfis);
1521                 break;
1522         case START_STOP_UNIT:
1523                 atapi_start_stop_unit(p, slot, cfis);
1524                 break;
1525         case MODE_SENSE_10:
1526                 atapi_mode_sense(p, slot, cfis);
1527                 break;
1528         case GET_EVENT_STATUS_NOTIFICATION:
1529                 atapi_get_event_status_notification(p, slot, cfis);
1530                 break;
1531         default:
1532                 cfis[4] = (cfis[4] & ~7) | ATA_I_CMD | ATA_I_IN;
1533                 p->sense_key = ATA_SENSE_ILLEGAL_REQUEST;
1534                 p->asc = 0x20;
1535                 ahci_write_fis_d2h(p, slot, cfis, (p->sense_key << 12) |
1536                                 ATA_S_READY | ATA_S_ERROR);
1537                 break;
1538         }
1539 }
1540
1541 static void
1542 ahci_handle_cmd(struct ahci_port *p, int slot, uint8_t *cfis)
1543 {
1544
1545         switch (cfis[2]) {
1546         case ATA_ATA_IDENTIFY:
1547                 handle_identify(p, slot, cfis);
1548                 break;
1549         case ATA_SETFEATURES:
1550         {
1551                 switch (cfis[3]) {
1552                 case ATA_SF_ENAB_SATA_SF:
1553                         switch (cfis[12]) {
1554                         case ATA_SATA_SF_AN:
1555                                 p->tfd = ATA_S_DSC | ATA_S_READY;
1556                                 break;
1557                         default:
1558                                 p->tfd = ATA_S_ERROR | ATA_S_READY;
1559                                 p->tfd |= (ATA_ERROR_ABORT << 8);
1560                                 break;
1561                         }
1562                         break;
1563                 case ATA_SF_ENAB_WCACHE:
1564                 case ATA_SF_DIS_WCACHE:
1565                 case ATA_SF_ENAB_RCACHE:
1566                 case ATA_SF_DIS_RCACHE:
1567                         p->tfd = ATA_S_DSC | ATA_S_READY;
1568                         break;
1569                 case ATA_SF_SETXFER:
1570                 {
1571                         switch (cfis[12] & 0xf8) {
1572                         case ATA_PIO:
1573                         case ATA_PIO0:
1574                                 break;
1575                         case ATA_WDMA0:
1576                         case ATA_UDMA0:
1577                                 p->xfermode = (cfis[12] & 0x7);
1578                                 break;
1579                         }
1580                         p->tfd = ATA_S_DSC | ATA_S_READY;
1581                         break;
1582                 }
1583                 default:
1584                         p->tfd = ATA_S_ERROR | ATA_S_READY;
1585                         p->tfd |= (ATA_ERROR_ABORT << 8);
1586                         break;
1587                 }
1588                 ahci_write_fis_d2h(p, slot, cfis, p->tfd);
1589                 break;
1590         }
1591         case ATA_SET_MULTI:
1592                 if (cfis[12] != 0 &&
1593                         (cfis[12] > 128 || (cfis[12] & (cfis[12] - 1)))) {
1594                         p->tfd = ATA_S_ERROR | ATA_S_READY;
1595                         p->tfd |= (ATA_ERROR_ABORT << 8);
1596                 } else {
1597                         p->mult_sectors = cfis[12];
1598                         p->tfd = ATA_S_DSC | ATA_S_READY;
1599                 }
1600                 ahci_write_fis_d2h(p, slot, cfis, p->tfd);
1601                 break;
1602         case ATA_READ:
1603         case ATA_WRITE:
1604         case ATA_READ48:
1605         case ATA_WRITE48:
1606         case ATA_READ_MUL:
1607         case ATA_WRITE_MUL:
1608         case ATA_READ_MUL48:
1609         case ATA_WRITE_MUL48:
1610         case ATA_READ_DMA:
1611         case ATA_WRITE_DMA:
1612         case ATA_READ_DMA48:
1613         case ATA_WRITE_DMA48:
1614         case ATA_READ_FPDMA_QUEUED:
1615         case ATA_WRITE_FPDMA_QUEUED:
1616                 ahci_handle_dma(p, slot, cfis, 0, 0);
1617                 break;
1618         case ATA_FLUSHCACHE:
1619         case ATA_FLUSHCACHE48:
1620                 ahci_handle_flush(p, slot, cfis);
1621                 break;
1622         case ATA_DATA_SET_MANAGEMENT:
1623                 if (cfis[11] == 0 && cfis[3] == ATA_DSM_TRIM &&
1624                     cfis[13] == 0 && cfis[12] == 1) {
1625                         ahci_handle_dsm_trim(p, slot, cfis, 0);
1626                         break;
1627                 }
1628                 ahci_write_fis_d2h(p, slot, cfis,
1629                     (ATA_E_ABORT << 8) | ATA_S_READY | ATA_S_ERROR);
1630                 break;
1631         case ATA_SEND_FPDMA_QUEUED:
1632                 if ((cfis[13] & 0x1f) == ATA_SFPDMA_DSM &&
1633                     cfis[17] == 0 && cfis[16] == ATA_DSM_TRIM &&
1634                     cfis[11] == 0 && cfis[13] == 1) {
1635                         ahci_handle_dsm_trim(p, slot, cfis, 0);
1636                         break;
1637                 }
1638                 ahci_write_fis_d2h(p, slot, cfis,
1639                     (ATA_E_ABORT << 8) | ATA_S_READY | ATA_S_ERROR);
1640                 break;
1641         case ATA_READ_LOG_EXT:
1642         case ATA_READ_LOG_DMA_EXT:
1643                 ahci_handle_read_log(p, slot, cfis);
1644                 break;
1645         case ATA_NOP:
1646                 ahci_write_fis_d2h(p, slot, cfis,
1647                     (ATA_E_ABORT << 8) | ATA_S_READY | ATA_S_ERROR);
1648                 break;
1649         case ATA_STANDBY_CMD:
1650         case ATA_STANDBY_IMMEDIATE:
1651         case ATA_IDLE_CMD:
1652         case ATA_IDLE_IMMEDIATE:
1653         case ATA_SLEEP:
1654                 ahci_write_fis_d2h(p, slot, cfis, ATA_S_READY | ATA_S_DSC);
1655                 break;
1656         case ATA_ATAPI_IDENTIFY:
1657                 handle_atapi_identify(p, slot, cfis);
1658                 break;
1659         case ATA_PACKET_CMD:
1660                 if (!p->atapi) {
1661                         ahci_write_fis_d2h(p, slot, cfis,
1662                             (ATA_E_ABORT << 8) | ATA_S_READY | ATA_S_ERROR);
1663                 } else
1664                         handle_packet_cmd(p, slot, cfis);
1665                 break;
1666         default:
1667                 WPRINTF("Unsupported cmd:%02x\n", cfis[2]);
1668                 ahci_write_fis_d2h(p, slot, cfis,
1669                     (ATA_E_ABORT << 8) | ATA_S_READY | ATA_S_ERROR);
1670                 break;
1671         }
1672 }
1673
1674 static void
1675 ahci_handle_slot(struct ahci_port *p, int slot)
1676 {
1677         struct ahci_cmd_hdr *hdr;
1678         struct ahci_prdt_entry *prdt;
1679         struct pci_ahci_softc *sc;
1680         uint8_t *cfis;
1681         int cfl;
1682
1683         sc = p->pr_sc;
1684         hdr = (struct ahci_cmd_hdr *)(p->cmd_lst + slot * AHCI_CL_SIZE);
1685         cfl = (hdr->flags & 0x1f) * 4;
1686         cfis = paddr_guest2host(ahci_ctx(sc), hdr->ctba,
1687                         0x80 + hdr->prdtl * sizeof(struct ahci_prdt_entry));
1688         prdt = (struct ahci_prdt_entry *)(cfis + 0x80);
1689
1690 #ifdef AHCI_DEBUG
1691         DPRINTF("\ncfis:");
1692         for (i = 0; i < cfl; i++) {
1693                 if (i % 10 == 0)
1694                         DPRINTF("\n");
1695                 DPRINTF("%02x ", cfis[i]);
1696         }
1697         DPRINTF("\n");
1698
1699         for (i = 0; i < hdr->prdtl; i++) {
1700                 DPRINTF("%d@%08"PRIx64"\n", prdt->dbc & 0x3fffff, prdt->dba);
1701                 prdt++;
1702         }
1703 #endif
1704
1705         if (cfis[0] != FIS_TYPE_REGH2D) {
1706                 WPRINTF("Not a H2D FIS:%02x\n", cfis[0]);
1707                 return;
1708         }
1709
1710         if (cfis[1] & 0x80) {
1711                 ahci_handle_cmd(p, slot, cfis);
1712         } else {
1713                 if (cfis[15] & (1 << 2))
1714                         p->reset = 1;
1715                 else if (p->reset) {
1716                         p->reset = 0;
1717                         ahci_port_reset(p);
1718                 }
1719                 p->ci &= ~(1 << slot);
1720         }
1721 }
1722
1723 static void
1724 ahci_handle_port(struct ahci_port *p)
1725 {
1726         int i;
1727
1728         if (!(p->cmd & AHCI_P_CMD_ST))
1729                 return;
1730
1731         /*
1732          * Search for any new commands to issue ignoring those that
1733          * are already in-flight.
1734          */
1735         for (i = 0; (i < 32) && p->ci; i++) {
1736                 if ((p->ci & (1 << i)) && !(p->pending & (1 << i))) {
1737                         p->cmd &= ~AHCI_P_CMD_CCS_MASK;
1738                         p->cmd |= i << AHCI_P_CMD_CCS_SHIFT;
1739                         ahci_handle_slot(p, i);
1740                 }
1741         }
1742 }
1743
1744 /*
1745  * blockif callback routine - this runs in the context of the blockif
1746  * i/o thread, so the mutex needs to be acquired.
1747  */
1748 static void
1749 ata_ioreq_cb(struct blockif_req *br, int err)
1750 {
1751         struct ahci_cmd_hdr *hdr;
1752         struct ahci_ioreq *aior;
1753         struct ahci_port *p;
1754         struct pci_ahci_softc *sc;
1755         uint32_t tfd;
1756         uint8_t *cfis;
1757         int pending, slot, ncq, dsm;
1758
1759         DPRINTF("%s %d\n", __func__, err);
1760
1761         ncq = 0;
1762         aior = br->br_param;
1763         p = aior->io_pr;
1764         cfis = aior->cfis;
1765         slot = aior->slot;
1766         pending = aior->prdtl;
1767         sc = p->pr_sc;
1768         hdr = (struct ahci_cmd_hdr *)(p->cmd_lst + slot * AHCI_CL_SIZE);
1769
1770         if (cfis[2] == ATA_WRITE_FPDMA_QUEUED ||
1771             cfis[2] == ATA_READ_FPDMA_QUEUED ||
1772             cfis[2] == ATA_SEND_FPDMA_QUEUED)
1773                 ncq = 1;
1774         if (cfis[2] == ATA_DATA_SET_MANAGEMENT ||
1775             (cfis[2] == ATA_SEND_FPDMA_QUEUED &&
1776              (cfis[13] & 0x1f) == ATA_SFPDMA_DSM))
1777                 dsm = 1;
1778
1779         pthread_mutex_lock(&sc->mtx);
1780
1781         /*
1782          * Delete the blockif request from the busy list
1783          */
1784         TAILQ_REMOVE(&p->iobhd, aior, io_blist);
1785
1786         /*
1787          * Move the blockif request back to the free list
1788          */
1789         STAILQ_INSERT_TAIL(&p->iofhd, aior, io_flist);
1790
1791         if (!err)
1792                 hdr->prdbc = aior->done;
1793
1794         if (dsm) {
1795                 if (aior->done != aior->len && !err) {
1796                         ahci_handle_dsm_trim(p, slot, cfis, aior->done);
1797                         goto out;
1798                 }
1799         } else {
1800                 if (pending && !err) {
1801                         ahci_handle_dma(p, slot, cfis, aior->done,
1802                             hdr->prdtl - pending);
1803                         goto out;
1804                 }
1805         }
1806
1807         if (!err && aior->done == aior->len) {
1808                 tfd = ATA_S_READY | ATA_S_DSC;
1809         } else {
1810                 tfd = (ATA_E_ABORT << 8) | ATA_S_READY | ATA_S_ERROR;
1811         }
1812
1813         if (ncq)
1814                 ahci_write_fis_sdb(p, slot, cfis, tfd);
1815         else
1816                 ahci_write_fis_d2h(p, slot, cfis, tfd);
1817
1818         /*
1819          * This command is now complete.
1820          */
1821         p->pending &= ~(1 << slot);
1822
1823         ahci_check_stopped(p);
1824 out:
1825         pthread_mutex_unlock(&sc->mtx);
1826         DPRINTF("%s exit\n", __func__);
1827 }
1828
1829 static void
1830 atapi_ioreq_cb(struct blockif_req *br, int err)
1831 {
1832         struct ahci_cmd_hdr *hdr;
1833         struct ahci_ioreq *aior;
1834         struct ahci_port *p;
1835         struct pci_ahci_softc *sc;
1836         uint8_t *cfis;
1837         uint32_t tfd;
1838         int pending, slot;
1839
1840         DPRINTF("%s %d\n", __func__, err);
1841
1842         aior = br->br_param;
1843         p = aior->io_pr;
1844         cfis = aior->cfis;
1845         slot = aior->slot;
1846         pending = aior->prdtl;
1847         sc = p->pr_sc;
1848         hdr = (struct ahci_cmd_hdr *)(p->cmd_lst + aior->slot * AHCI_CL_SIZE);
1849
1850         pthread_mutex_lock(&sc->mtx);
1851
1852         /*
1853          * Delete the blockif request from the busy list
1854          */
1855         TAILQ_REMOVE(&p->iobhd, aior, io_blist);
1856
1857         /*
1858          * Move the blockif request back to the free list
1859          */
1860         STAILQ_INSERT_TAIL(&p->iofhd, aior, io_flist);
1861
1862         if (!err)
1863                 hdr->prdbc = aior->done;
1864
1865         if (pending && !err) {
1866                 atapi_read(p, slot, cfis, aior->done, hdr->prdtl - pending);
1867                 goto out;
1868         }
1869
1870         if (!err && aior->done == aior->len) {
1871                 tfd = ATA_S_READY | ATA_S_DSC;
1872         } else {
1873                 p->sense_key = ATA_SENSE_ILLEGAL_REQUEST;
1874                 p->asc = 0x21;
1875                 tfd = (p->sense_key << 12) | ATA_S_READY | ATA_S_ERROR;
1876         }
1877
1878         cfis[4] = (cfis[4] & ~7) | ATA_I_CMD | ATA_I_IN;
1879         ahci_write_fis_d2h(p, slot, cfis, tfd);
1880
1881         /*
1882          * This command is now complete.
1883          */
1884         p->pending &= ~(1 << slot);
1885
1886         ahci_check_stopped(p);
1887 out:
1888         pthread_mutex_unlock(&sc->mtx);
1889         DPRINTF("%s exit\n", __func__);
1890 }
1891
1892 static void
1893 pci_ahci_ioreq_init(struct ahci_port *pr)
1894 {
1895         struct ahci_ioreq *vr;
1896         int i;
1897
1898         pr->ioqsz = blockif_queuesz(pr->bctx);
1899         pr->ioreq = calloc(pr->ioqsz, sizeof(struct ahci_ioreq));
1900         STAILQ_INIT(&pr->iofhd);
1901
1902         /*
1903          * Add all i/o request entries to the free queue
1904          */
1905         for (i = 0; i < pr->ioqsz; i++) {
1906                 vr = &pr->ioreq[i];
1907                 vr->io_pr = pr;
1908                 if (!pr->atapi)
1909                         vr->io_req.br_callback = ata_ioreq_cb;
1910                 else
1911                         vr->io_req.br_callback = atapi_ioreq_cb;
1912                 vr->io_req.br_param = vr;
1913                 STAILQ_INSERT_TAIL(&pr->iofhd, vr, io_flist);
1914         }
1915
1916         TAILQ_INIT(&pr->iobhd);
1917 }
1918
1919 static void
1920 pci_ahci_port_write(struct pci_ahci_softc *sc, uint64_t offset, uint64_t value)
1921 {
1922         int port = (offset - AHCI_OFFSET) / AHCI_STEP;
1923         offset = (offset - AHCI_OFFSET) % AHCI_STEP;
1924         struct ahci_port *p = &sc->port[port];
1925
1926         DPRINTF("pci_ahci_port %d: write offset 0x%"PRIx64" value 0x%"PRIx64"\n",
1927                 port, offset, value);
1928
1929         switch (offset) {
1930         case AHCI_P_CLB:
1931                 p->clb = value;
1932                 break;
1933         case AHCI_P_CLBU:
1934                 p->clbu = value;
1935                 break;
1936         case AHCI_P_FB:
1937                 p->fb = value;
1938                 break;
1939         case AHCI_P_FBU:
1940                 p->fbu = value;
1941                 break;
1942         case AHCI_P_IS:
1943                 p->is &= ~value;
1944                 break;
1945         case AHCI_P_IE:
1946                 p->ie = value & 0xFDC000FF;
1947                 ahci_generate_intr(sc);
1948                 break;
1949         case AHCI_P_CMD:
1950         {
1951                 p->cmd = value;
1952
1953                 if (!(value & AHCI_P_CMD_ST)) {
1954                         ahci_port_stop(p);
1955                 } else {
1956                         uint64_t clb;
1957
1958                         p->cmd |= AHCI_P_CMD_CR;
1959                         clb = (uint64_t)p->clbu << 32 | p->clb;
1960                         p->cmd_lst = paddr_guest2host(ahci_ctx(sc), clb,
1961                                         AHCI_CL_SIZE * AHCI_MAX_SLOTS);
1962                 }
1963
1964                 if (value & AHCI_P_CMD_FRE) {
1965                         uint64_t fb;
1966
1967                         p->cmd |= AHCI_P_CMD_FR;
1968                         fb = (uint64_t)p->fbu << 32 | p->fb;
1969                         /* we don't support FBSCP, so rfis size is 256Bytes */
1970                         p->rfis = paddr_guest2host(ahci_ctx(sc), fb, 256);
1971                 } else {
1972                         p->cmd &= ~AHCI_P_CMD_FR;
1973                 }
1974
1975                 if (value & AHCI_P_CMD_CLO) {
1976                         p->tfd = 0;
1977                         p->cmd &= ~AHCI_P_CMD_CLO;
1978                 }
1979
1980                 ahci_handle_port(p);
1981                 break;
1982         }
1983         case AHCI_P_TFD:
1984         case AHCI_P_SIG:
1985         case AHCI_P_SSTS:
1986                 WPRINTF("pci_ahci_port: read only registers 0x%"PRIx64"\n", offset);
1987                 break;
1988         case AHCI_P_SCTL:
1989                 p->sctl = value;
1990                 if (!(p->cmd & AHCI_P_CMD_ST)) {
1991                         if (value & ATA_SC_DET_RESET)
1992                                 ahci_port_reset(p);
1993                 }
1994                 break;
1995         case AHCI_P_SERR:
1996                 p->serr &= ~value;
1997                 break;
1998         case AHCI_P_SACT:
1999                 p->sact |= value;
2000                 break;
2001         case AHCI_P_CI:
2002                 p->ci |= value;
2003                 ahci_handle_port(p);
2004                 break;
2005         case AHCI_P_SNTF:
2006         case AHCI_P_FBS:
2007         default:
2008                 break;
2009         }
2010 }
2011
2012 static void
2013 pci_ahci_host_write(struct pci_ahci_softc *sc, uint64_t offset, uint64_t value)
2014 {
2015         DPRINTF("pci_ahci_host: write offset 0x%"PRIx64" value 0x%"PRIx64"\n",
2016                 offset, value);
2017
2018         switch (offset) {
2019         case AHCI_CAP:
2020         case AHCI_PI:
2021         case AHCI_VS:
2022         case AHCI_CAP2:
2023                 DPRINTF("pci_ahci_host: read only registers 0x%"PRIx64"\n", offset);
2024                 break;
2025         case AHCI_GHC:
2026                 if (value & AHCI_GHC_HR)
2027                         ahci_reset(sc);
2028                 else if (value & AHCI_GHC_IE) {
2029                         sc->ghc |= AHCI_GHC_IE;
2030                         ahci_generate_intr(sc);
2031                 }
2032                 break;
2033         case AHCI_IS:
2034                 sc->is &= ~value;
2035                 ahci_generate_intr(sc);
2036                 break;
2037         default:
2038                 break;
2039         }
2040 }
2041
2042 static void
2043 pci_ahci_write(struct vmctx *ctx, int vcpu, struct pci_devinst *pi,
2044                 int baridx, uint64_t offset, int size, uint64_t value)
2045 {
2046         struct pci_ahci_softc *sc = pi->pi_arg;
2047
2048         assert(baridx == 5);
2049         assert(size == 4);
2050
2051         pthread_mutex_lock(&sc->mtx);
2052
2053         if (offset < AHCI_OFFSET)
2054                 pci_ahci_host_write(sc, offset, value);
2055         else if (offset < AHCI_OFFSET + sc->ports * AHCI_STEP)
2056                 pci_ahci_port_write(sc, offset, value);
2057         else
2058                 WPRINTF("pci_ahci: unknown i/o write offset 0x%"PRIx64"\n", offset);
2059
2060         pthread_mutex_unlock(&sc->mtx);
2061 }
2062
2063 static uint64_t
2064 pci_ahci_host_read(struct pci_ahci_softc *sc, uint64_t offset)
2065 {
2066         uint32_t value;
2067
2068         switch (offset) {
2069         case AHCI_CAP:
2070         case AHCI_GHC:
2071         case AHCI_IS:
2072         case AHCI_PI:
2073         case AHCI_VS:
2074         case AHCI_CCCC:
2075         case AHCI_CCCP:
2076         case AHCI_EM_LOC:
2077         case AHCI_EM_CTL:
2078         case AHCI_CAP2:
2079         {
2080                 uint32_t *p = &sc->cap;
2081                 p += (offset - AHCI_CAP) / sizeof(uint32_t);
2082                 value = *p;
2083                 break;
2084         }
2085         default:
2086                 value = 0;
2087                 break;
2088         }
2089         DPRINTF("pci_ahci_host: read offset 0x%"PRIx64" value 0x%x\n",
2090                 offset, value);
2091
2092         return (value);
2093 }
2094
2095 static uint64_t
2096 pci_ahci_port_read(struct pci_ahci_softc *sc, uint64_t offset)
2097 {
2098         uint32_t value;
2099         int port = (offset - AHCI_OFFSET) / AHCI_STEP;
2100         offset = (offset - AHCI_OFFSET) % AHCI_STEP;
2101
2102         switch (offset) {
2103         case AHCI_P_CLB:
2104         case AHCI_P_CLBU:
2105         case AHCI_P_FB:
2106         case AHCI_P_FBU:
2107         case AHCI_P_IS:
2108         case AHCI_P_IE:
2109         case AHCI_P_CMD:
2110         case AHCI_P_TFD:
2111         case AHCI_P_SIG:
2112         case AHCI_P_SSTS:
2113         case AHCI_P_SCTL:
2114         case AHCI_P_SERR:
2115         case AHCI_P_SACT:
2116         case AHCI_P_CI:
2117         case AHCI_P_SNTF:
2118         case AHCI_P_FBS:
2119         {
2120                 uint32_t *p= &sc->port[port].clb;
2121                 p += (offset - AHCI_P_CLB) / sizeof(uint32_t);
2122                 value = *p;
2123                 break;
2124         }
2125         default:
2126                 value = 0;
2127                 break;
2128         }
2129
2130         DPRINTF("pci_ahci_port %d: read offset 0x%"PRIx64" value 0x%x\n",
2131                 port, offset, value);
2132
2133         return value;
2134 }
2135
2136 static uint64_t
2137 pci_ahci_read(struct vmctx *ctx, int vcpu, struct pci_devinst *pi, int baridx,
2138     uint64_t offset, int size)
2139 {
2140         struct pci_ahci_softc *sc = pi->pi_arg;
2141         uint32_t value;
2142
2143         assert(baridx == 5);
2144         assert(size == 4);
2145
2146         pthread_mutex_lock(&sc->mtx);
2147
2148         if (offset < AHCI_OFFSET)
2149                 value = pci_ahci_host_read(sc, offset);
2150         else if (offset < AHCI_OFFSET + sc->ports * AHCI_STEP)
2151                 value = pci_ahci_port_read(sc, offset);
2152         else {
2153                 value = 0;
2154                 WPRINTF("pci_ahci: unknown i/o read offset 0x%"PRIx64"\n", offset);
2155         }
2156
2157         pthread_mutex_unlock(&sc->mtx);
2158
2159         return (value);
2160 }
2161
2162 static int
2163 pci_ahci_init(struct vmctx *ctx, struct pci_devinst *pi, char *opts, int atapi)
2164 {
2165         char bident[sizeof("XX:X:X")];
2166         struct blockif_ctxt *bctxt;
2167         struct pci_ahci_softc *sc;
2168         int ret, slots;
2169
2170         ret = 0;
2171
2172         if (opts == NULL) {
2173                 fprintf(stderr, "pci_ahci: backing device required\n");
2174                 return (1);
2175         }
2176
2177 #ifdef AHCI_DEBUG
2178         dbg = fopen("/tmp/log", "w+");
2179 #endif
2180
2181         sc = calloc(1, sizeof(struct pci_ahci_softc));
2182         pi->pi_arg = sc;
2183         sc->asc_pi = pi;
2184         sc->ports = MAX_PORTS;
2185
2186         /*
2187          * Only use port 0 for a backing device. All other ports will be
2188          * marked as unused
2189          */
2190         sc->port[0].atapi = atapi;
2191
2192         /*
2193          * Attempt to open the backing image. Use the PCI
2194          * slot/func for the identifier string.
2195          */
2196         snprintf(bident, sizeof(bident), "%d:%d", pi->pi_slot, pi->pi_func);
2197         bctxt = blockif_open(opts, bident);
2198         if (bctxt == NULL) {
2199                 ret = 1;
2200                 goto open_fail;
2201         }
2202         sc->port[0].bctx = bctxt;
2203         sc->port[0].pr_sc = sc;
2204
2205         /*
2206          * Allocate blockif request structures and add them
2207          * to the free list
2208          */
2209         pci_ahci_ioreq_init(&sc->port[0]);
2210
2211         pthread_mutex_init(&sc->mtx, NULL);
2212
2213         /* Intel ICH8 AHCI */
2214         slots = sc->port[0].ioqsz;
2215         if (slots > 32)
2216                 slots = 32;
2217         --slots;
2218         sc->cap = AHCI_CAP_64BIT | AHCI_CAP_SNCQ | AHCI_CAP_SSNTF |
2219             AHCI_CAP_SMPS | AHCI_CAP_SSS | AHCI_CAP_SALP |
2220             AHCI_CAP_SAL | AHCI_CAP_SCLO | (0x3 << AHCI_CAP_ISS_SHIFT)|
2221             AHCI_CAP_PMD | AHCI_CAP_SSC | AHCI_CAP_PSC |
2222             (slots << AHCI_CAP_NCS_SHIFT) | AHCI_CAP_SXS | (sc->ports - 1);
2223
2224         /* Only port 0 implemented */
2225         sc->pi = 1;
2226         sc->vs = 0x10300;
2227         sc->cap2 = AHCI_CAP2_APST;
2228         ahci_reset(sc);
2229
2230         pci_set_cfgdata16(pi, PCIR_DEVICE, 0x2821);
2231         pci_set_cfgdata16(pi, PCIR_VENDOR, 0x8086);
2232         pci_set_cfgdata8(pi, PCIR_CLASS, PCIC_STORAGE);
2233         pci_set_cfgdata8(pi, PCIR_SUBCLASS, PCIS_STORAGE_SATA);
2234         pci_set_cfgdata8(pi, PCIR_PROGIF, PCIP_STORAGE_SATA_AHCI_1_0);
2235         pci_emul_add_msicap(pi, 1);
2236         pci_emul_alloc_bar(pi, 5, PCIBAR_MEM32,
2237             AHCI_OFFSET + sc->ports * AHCI_STEP);
2238
2239         pci_lintr_request(pi);
2240
2241 open_fail:
2242         if (ret) {
2243                 if (sc->port[0].bctx != NULL)
2244                         blockif_close(sc->port[0].bctx);
2245                 free(sc);
2246         }
2247
2248         return (ret);
2249 }
2250
2251 static int
2252 pci_ahci_hd_init(struct vmctx *ctx, struct pci_devinst *pi, char *opts)
2253 {
2254
2255         return (pci_ahci_init(ctx, pi, opts, 0));
2256 }
2257
2258 static int
2259 pci_ahci_atapi_init(struct vmctx *ctx, struct pci_devinst *pi, char *opts)
2260 {
2261
2262         return (pci_ahci_init(ctx, pi, opts, 1));
2263 }
2264
2265 /*
2266  * Use separate emulation names to distinguish drive and atapi devices
2267  */
2268 struct pci_devemu pci_de_ahci_hd = {
2269         .pe_emu =       "ahci-hd",
2270         .pe_init =      pci_ahci_hd_init,
2271         .pe_barwrite =  pci_ahci_write,
2272         .pe_barread =   pci_ahci_read
2273 };
2274 PCI_EMUL_SET(pci_de_ahci_hd);
2275
2276 struct pci_devemu pci_de_ahci_cd = {
2277         .pe_emu =       "ahci-cd",
2278         .pe_init =      pci_ahci_atapi_init,
2279         .pe_barwrite =  pci_ahci_write,
2280         .pe_barread =   pci_ahci_read
2281 };
2282 PCI_EMUL_SET(pci_de_ahci_cd);