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