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