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