Add port multiplier support, has been in snaps for a while with no reported

issues. No actual OKs, but general acknowledgement and 'get it in' from several.

1 files changed, 1055 insertions, 83 deletions

diff --git a/sys/dev/pci/ahci.c b/sys/dev/pci/ahci.c
index 319b210f521..b126a7dc991 100644
--- a/sys/dev/pci/ahci.c
+++ b/sys/dev/pci/ahci.c
@@ -1,7 +1,9 @@
-/*	$OpenBSD: ahci.c,v 1.169 2010/08/31 17:13:44 deraadt Exp $ */
+/*	$OpenBSD: ahci.c,v 1.170 2011/01/26 21:41:00 drahn Exp $ */
 
 /*
  * Copyright (c) 2006 David Gwynne <dlg@openbsd.org>
+ * Copyright (c) 2010 Conformal Systems LLC <info@conformal.com>
+ * Copyright (c) 2010 Jonathan Matthew <jonathan@d14n.org>
  *
  * Permission to use, copy, modify, and distribute this software for any
  * purpose with or without fee is hereby granted, provided that the above
@@ -33,6 +35,7 @@
 #include <dev/pci/pcidevs.h>
 
 #include <dev/ata/atascsi.h>
+#include <dev/ata/pmreg.h>
 
 /* change to AHCI_DEBUG for dmesg spam */
 #define NO_AHCI_DEBUG
@@ -253,6 +256,16 @@ int ahcidebug = AHCI_D_VERBOSE;
 #define  AHCI_PREG_CI_ALL_SLOTS	0xffffffff
 #define AHCI_PREG_SNTF		0x3c /* SNotification */
 
+#define AHCI_PREG_FBS		0x40 /* FIS-based Switching Control */
+#define AHCI_PREG_FBS_DWE	0xf0000	/* Device With Error */
+#define AHCI_PREG_FBS_ADO	0xf000	/* Active Device Optimization */
+#define AHCI_PREG_FBS_DEV	0xf00	/* Device To Issue */
+#define AHCI_PREG_FBS_SDE	(1<<2)	/* Single Device Error */
+#define AHCI_PREG_FBS_DEC	(1<<1)	/* Device Error Clear */
+#define AHCI_PREG_FBS_EN	(1<<0)	/* Enable */
+
+
+
 struct ahci_cmd_hdr {
 	u_int16_t		flags;
 #define AHCI_CMD_LIST_FLAG_CFL		0x001f /* Command FIS Length */
@@ -263,6 +276,7 @@ struct ahci_cmd_hdr {
 #define AHCI_CMD_LIST_FLAG_B		(1<<9) /* BIST */
 #define AHCI_CMD_LIST_FLAG_C		(1<<10) /* Clear Busy upon R_OK */
 #define AHCI_CMD_LIST_FLAG_PMP		0xf000 /* Port Multiplier Port */
+#define AHCI_CMD_LIST_FLAG_PMP_SHIFT	12
 	u_int16_t		prdtl; /* sgl len */
 
 	u_int32_t		prdbc; /* transferred byte count */
@@ -352,6 +366,7 @@ struct ahci_port {
 	volatile u_int32_t	ap_active;
 	volatile u_int32_t	ap_active_cnt;
 	volatile u_int32_t	ap_sactive;
+	volatile u_int32_t	ap_pmp_ncq_port;
 	struct ahci_ccb		*ap_ccbs;
 
 	TAILQ_HEAD(, ahci_ccb)	ap_ccb_free;
@@ -360,7 +375,13 @@ struct ahci_port {
 
 	u_int32_t		ap_state;
 #define AP_S_NORMAL			0
-#define AP_S_FATAL_ERROR		1
+#define AP_S_PMP_PROBE			1
+#define AP_S_PMP_PORT_PROBE		2
+#define AP_S_FATAL_ERROR		3
+
+	int			ap_pmp_ports;
+	int			ap_port;
+	int			ap_pmp_ignore_ifs;
 
 	/* For error recovery. */
 #ifdef DIAGNOSTIC
@@ -394,6 +415,7 @@ struct ahci_softc {
 	int			sc_flags;
 #define AHCI_F_NO_NCQ			(1<<0)
 #define AHCI_F_IGN_FR			(1<<1)
+#define AHCI_F_IPMS_PROBE		(1<<2)	/* IPMS on failed PMP probe */
 
 	u_int			sc_ncmds;
 
@@ -428,6 +450,8 @@ void			ahci_ati_sb_idetoahci(struct ahci_softc *,
 			    struct pci_attach_args *pa);
 int			ahci_ati_sb600_attach(struct ahci_softc *,
 			    struct pci_attach_args *);
+int			ahci_ati_sb700_attach(struct ahci_softc *,
+			    struct pci_attach_args *);
 int			ahci_amd_hudson2_attach(struct ahci_softc *,
 			    struct pci_attach_args *);
 int			ahci_nvidia_mcp_attach(struct ahci_softc *,
@@ -441,6 +465,16 @@ static const struct ahci_device ahci_devices[] = {
 	    NULL,		ahci_ati_sb600_attach },
 	{ PCI_VENDOR_ATI,	PCI_PRODUCT_ATI_SBX00_SATA_1,
 	    NULL,		ahci_ati_sb600_attach },
+	{ PCI_VENDOR_ATI,	PCI_PRODUCT_ATI_SBX00_SATA_2,
+	    NULL,		ahci_ati_sb700_attach },
+	{ PCI_VENDOR_ATI,	PCI_PRODUCT_ATI_SBX00_SATA_3,
+	    NULL,		ahci_ati_sb700_attach },
+	{ PCI_VENDOR_ATI,	PCI_PRODUCT_ATI_SBX00_SATA_4,
+	    NULL,		ahci_ati_sb700_attach },
+	{ PCI_VENDOR_ATI,	PCI_PRODUCT_ATI_SBX00_SATA_5,
+	    NULL,		ahci_ati_sb700_attach },
+	{ PCI_VENDOR_ATI,	PCI_PRODUCT_ATI_SBX00_SATA_6,
+	    NULL,		ahci_ati_sb700_attach },
 
 	{ PCI_VENDOR_NVIDIA,	PCI_PRODUCT_NVIDIA_MCP65_AHCI_2,
 	    NULL,		ahci_nvidia_mcp_attach },
@@ -499,6 +533,7 @@ void			ahci_unmap_regs(struct ahci_softc *);
 int			ahci_map_intr(struct ahci_softc *,
 			    struct pci_attach_args *, pci_intr_handle_t);
 void			ahci_unmap_intr(struct ahci_softc *);
+void			ahci_enable_interrupts(struct ahci_port *);
 
 int			ahci_init(struct ahci_softc *);
 int			ahci_port_alloc(struct ahci_softc *, u_int);
@@ -509,7 +544,11 @@ int			ahci_port_start(struct ahci_port *, int);
 int			ahci_port_stop(struct ahci_port *, int);
 int			ahci_port_clo(struct ahci_port *);
 int			ahci_port_softreset(struct ahci_port *);
-int			ahci_port_portreset(struct ahci_port *);
+int			ahci_port_portreset(struct ahci_port *, int);
+int			ahci_port_signature(struct ahci_port *);
+int			ahci_pmp_port_softreset(struct ahci_port *, int);
+int			ahci_pmp_port_portreset(struct ahci_port *, int);
+int			ahci_pmp_port_probe(struct ahci_port *ap, int pmp_port);
 
 int			ahci_load_prdt(struct ahci_ccb *);
 void			ahci_unload_prdt(struct ahci_ccb *);
@@ -529,7 +568,10 @@ void			ahci_put_ccb(struct ahci_ccb *);
 struct ahci_ccb		*ahci_get_err_ccb(struct ahci_port *);
 void			ahci_put_err_ccb(struct ahci_ccb *);
 
-int			ahci_port_read_ncq_error(struct ahci_port *, int *);
+struct ahci_ccb		*ahci_get_pmp_ccb(struct ahci_port *);
+void			ahci_put_pmp_ccb(struct ahci_ccb *);
+
+int			ahci_port_read_ncq_error(struct ahci_port *, int *, int);
 
 struct ahci_dmamem	*ahci_dmamem_alloc(struct ahci_softc *, size_t);
 void			ahci_dmamem_free(struct ahci_softc *,
@@ -544,6 +586,18 @@ u_int32_t		ahci_pread(struct ahci_port *, bus_size_t);
 void			ahci_pwrite(struct ahci_port *, bus_size_t, u_int32_t);
 int			ahci_pwait_eq(struct ahci_port *, bus_size_t,
 			    u_int32_t, u_int32_t, int);
+void			ahci_flush_tfd(struct ahci_port *ap);
+u_int32_t		ahci_active_mask(struct ahci_port *);
+void			ahci_pmp_probe_timeout(void *);
+
+/* pmp operations */
+int			ahci_pmp_read(struct ahci_port *, int, int,
+			    u_int32_t *);
+int			ahci_pmp_write(struct ahci_port *, int, int, u_int32_t);
+int			ahci_pmp_phy_status(struct ahci_port *, int,
+			    u_int32_t *);
+int 			ahci_pmp_identify(struct ahci_port *, int *);
+
 
 /* Wait for all bits in _b to be cleared */
 #define ahci_pwait_clr(_ap, _r, _b, _n) \
@@ -556,8 +610,8 @@ int			ahci_pwait_eq(struct ahci_port *, bus_size_t,
 
 
 /* provide methods for atascsi to call */
-int			ahci_ata_probe(void *, int);
-void			ahci_ata_free(void *, int);
+int			ahci_ata_probe(void *, int, int);
+void			ahci_ata_free(void *, int, int);
 struct ata_xfer *	ahci_ata_get_xfer(void *, int);
 void			ahci_ata_put_xfer(struct ata_xfer *);
 void			ahci_ata_cmd(struct ata_xfer *);
@@ -571,6 +625,7 @@ struct atascsi_methods ahci_atascsi_methods = {
 
 /* ccb completions */
 void			ahci_ata_cmd_done(struct ahci_ccb *);
+void			ahci_pmp_cmd_done(struct ahci_ccb *);
 void			ahci_ata_cmd_timeout(void *);
 void			ahci_empty_done(struct ahci_ccb *);
 
@@ -632,8 +687,15 @@ ahci_ati_sb600_attach(struct ahci_softc *sc, struct pci_attach_args *pa)
 {
 	ahci_ati_sb_idetoahci(sc, pa);
 
-	sc->sc_flags |= AHCI_F_IGN_FR;
+	sc->sc_flags |= AHCI_F_IGN_FR | AHCI_F_IPMS_PROBE;
+
+	return (0);
+}
 
+int
+ahci_ati_sb700_attach(struct ahci_softc *sc, struct pci_attach_args *pa)
+{
+	sc->sc_flags |= AHCI_F_IPMS_PROBE;
 	return (0);
 }
 
@@ -803,8 +865,18 @@ noccc:
 	aaa.aaa_ncmds = sc->sc_ncmds;
 	aaa.aaa_capability = ASAA_CAP_NEEDS_RESERVED;
 	if (!(sc->sc_flags & AHCI_F_NO_NCQ) &&
-	    (sc->sc_cap & AHCI_REG_CAP_SNCQ))
-		aaa.aaa_capability |= ASAA_CAP_NCQ;
+	    (sc->sc_cap & AHCI_REG_CAP_SNCQ)) {
+		aaa.aaa_capability |= ASAA_CAP_NCQ | ASAA_CAP_PMP_NCQ;
+		/* XXX enabling ASAA_CAP_PMP_NCQ with FBS:
+		 * - some error recovery work required (single device vs port
+		 *   errors)
+		 * - probably need to look at storing our active ccb queue
+		 *   differently so we can group ncq and non-ncq commands
+		 *   for different ports.  as long as we preserve the order for
+		 *   each port, we can reorder commands to get more ncq 
+		 *   commands to run in parallel.
+		 */
+	}
 
 	sc->sc_atascsi = atascsi_attach(&sc->sc_dev, &aaa);
 
@@ -989,6 +1061,22 @@ ahci_init(struct ahci_softc *sc)
 	return (0);
 }
 
+void
+ahci_enable_interrupts(struct ahci_port *ap)
+{
+	ahci_pwrite(ap, AHCI_PREG_IE, AHCI_PREG_IE_TFEE | AHCI_PREG_IE_HBFE |
+	    AHCI_PREG_IE_IFE | AHCI_PREG_IE_OFE | AHCI_PREG_IE_DPE |
+	    AHCI_PREG_IE_UFE |
+	    ((ap->ap_sc->sc_cap & AHCI_REG_CAP_SSNTF) ? AHCI_PREG_IE_IPME : 0) |
+#ifdef AHCI_COALESCE
+	    ((ap->ap_sc->sc_ccc_ports & (1 << ap->ap_port)) ? 0 :
+	     (AHCI_PREG_IE_SDBE | AHCI_PREG_IE_DHRE))
+#else
+	    AHCI_PREG_IE_SDBE | AHCI_PREG_IE_DHRE
+#endif
+	    );
+}
+
 int
 ahci_port_alloc(struct ahci_softc *sc, u_int port)
 {
@@ -1011,6 +1099,7 @@ ahci_port_alloc(struct ahci_softc *sc, u_int port)
 	snprintf(ap->ap_name, sizeof(ap->ap_name), "%s.%d",
 	    DEVNAME(sc), port);
 #endif
+	ap->ap_port = port;
 	sc->sc_ports[port] = ap;
 
 	if (bus_space_subregion(sc->sc_iot, sc->sc_ioh,
@@ -1050,6 +1139,11 @@ ahci_port_alloc(struct ahci_softc *sc, u_int port)
 		ahci_pwrite(ap, AHCI_PREG_SCTL, 0);
 	}
 
+	/* XXX FBS - need to allocate 16x ahci_rfis struct? - but we don't
+	 * know if there's a PMP attached or if the HBA supports FBS yet..
+	 * reallocate when we enable FBS?
+	 */
+
 	/* Allocate RFIS */
 	ap->ap_dmamem_rfis = ahci_dmamem_alloc(sc, sizeof(struct ahci_rfis));
 	if (ap->ap_dmamem_rfis == NULL)
@@ -1137,7 +1231,8 @@ nomem:
 	ahci_pwait_clr(ap, AHCI_PREG_CMD, AHCI_PREG_CMD_ICC, 1);
 
 	/* Reset port */
-	rc = ahci_port_portreset(ap);
+	rc = ahci_port_portreset(ap, 1);
+
 	switch (rc) {
 	case ENODEV:
 		switch (ahci_pread(ap, AHCI_PREG_SSTS) & AHCI_PREG_SSTS_DET) {
@@ -1173,8 +1268,9 @@ nomem:
 	default:
 		break;
 	}
-	DPRINTF(AHCI_D_VERBOSE, "%s: detected device on port %d\n",
-	    DEVNAME(sc), port);
+
+	DPRINTF(AHCI_D_VERBOSE, "%s: detected device on port %d; %d\n",
+	    DEVNAME(sc), port, rc);
 
 	/* Enable command transfers on port */
 	if (ahci_port_start(ap, 0)) {
@@ -1187,17 +1283,7 @@ nomem:
 	ahci_pwrite(ap, AHCI_PREG_IS, ahci_pread(ap, AHCI_PREG_IS));
 	ahci_write(sc, AHCI_REG_IS, 1 << port);
 
-	/* Enable port interrupts */
-	ahci_pwrite(ap, AHCI_PREG_IE, AHCI_PREG_IE_TFEE | AHCI_PREG_IE_HBFE |
-	    AHCI_PREG_IE_IFE | AHCI_PREG_IE_OFE | AHCI_PREG_IE_DPE |
-	    AHCI_PREG_IE_UFE |
-#ifdef AHCI_COALESCE
-	    ((sc->sc_ccc_ports & (1 << port)) ? 0 : (AHCI_PREG_IE_SDBE |
-	    AHCI_PREG_IE_DHRE))
-#else
-	    AHCI_PREG_IE_SDBE | AHCI_PREG_IE_DHRE
-#endif
-	    );
+	ahci_enable_interrupts(ap);
 
 freeport:
 	if (rc != 0)
@@ -1302,7 +1388,7 @@ ahci_port_init(struct ahci_softc *sc, u_int port)
 	ahci_pwait_clr(ap, AHCI_PREG_CMD, AHCI_PREG_CMD_ICC, 1);
 
 	/* Reset port */
-	rc = ahci_port_portreset(ap);
+	rc = ahci_port_portreset(ap, 1);
 	switch (rc) {
 	case ENODEV:
 		switch (ahci_pread(ap, AHCI_PREG_SSTS) & AHCI_PREG_SSTS_DET) {
@@ -1341,6 +1427,27 @@ ahci_port_init(struct ahci_softc *sc, u_int port)
 	DPRINTF(AHCI_D_VERBOSE, "%s: detected device on port %d\n",
 	    DEVNAME(sc), port);
 
+	if (ap->ap_pmp_ports > 0) {
+		int p;
+
+		for (p = 0; p < ap->ap_pmp_ports; p++) {
+			int sig;
+
+			/* might need to do a portreset first here? */
+
+			/* softreset the port */
+			if (ahci_pmp_port_softreset(ap, p)) {
+				printf("%s.%d: unable to probe PMP port due to"
+				    " softreset failure\n", PORTNAME(ap), p);
+				continue;
+			}
+
+			sig = ahci_port_signature(ap);
+			printf("%s.%d: port signature returned %d\n",
+			    PORTNAME(ap), p, sig);
+		}
+	}
+
 	/* Enable command transfers on port */
 	if (ahci_port_start(ap, 0)) {
 		printf("%s: failed to start command DMA on port %d, "
@@ -1352,17 +1459,7 @@ ahci_port_init(struct ahci_softc *sc, u_int port)
 	ahci_pwrite(ap, AHCI_PREG_IS, ahci_pread(ap, AHCI_PREG_IS));
 	ahci_write(sc, AHCI_REG_IS, 1 << port);
 
-	/* Enable port interrupts */
-	ahci_pwrite(ap, AHCI_PREG_IE, AHCI_PREG_IE_TFEE | AHCI_PREG_IE_HBFE |
-	    AHCI_PREG_IE_IFE | AHCI_PREG_IE_OFE | AHCI_PREG_IE_DPE |
-	    AHCI_PREG_IE_UFE |
-#ifdef AHCI_COALESCE
-	    ((sc->sc_ccc_ports & (1 << port)) ? 0 : (AHCI_PREG_IE_SDBE |
-	    AHCI_PREG_IE_DHRE))
-#else
-	    AHCI_PREG_IE_SDBE | AHCI_PREG_IE_DHRE
-#endif
-	    );
+	ahci_enable_interrupts(ap);
 
 reterr:
 	return (rc);
@@ -1373,6 +1470,8 @@ ahci_port_start(struct ahci_port *ap, int fre_only)
 {
 	u_int32_t			r;
 
+	/* XXX FBS: possibly turn FBS on here */
+
 	/* Turn on FRE (and ST) */
 	r = ahci_pread(ap, AHCI_PREG_CMD) & ~AHCI_PREG_CMD_ICC;
 	r |= AHCI_PREG_CMD_FRE;
@@ -1433,6 +1532,8 @@ ahci_port_stop(struct ahci_port *ap, int stop_fis_rx)
 	    ahci_pwait_clr(ap, AHCI_PREG_CMD, AHCI_PREG_CMD_FR, 1))
 		return (2);
 
+	/* XXX FBS: possibly disable FBS here? */
+
 	return (0);
 }
 
@@ -1496,6 +1597,10 @@ ahci_port_softreset(struct ahci_port *ap)
 	/* Clear port errors to permit TFD transfer */
 	ahci_pwrite(ap, AHCI_PREG_SERR, ahci_pread(ap, AHCI_PREG_SERR));
 
+	/* XXX FBS - need to ensure we don't enable FBS here, since we're
+	 * resetting stuff
+	 * (AHCI spec 9.3.8)
+	 */
 	/* Restart port */
 	if (ahci_port_start(ap, 0)) {
 		printf("%s: failed to start port, cannot softreset\n",
@@ -1519,8 +1624,8 @@ ahci_port_softreset(struct ahci_port *ap)
 	bzero(ccb->ccb_cmd_table, sizeof(struct ahci_cmd_table));
 
 	fis = ccb->ccb_cmd_table->cfis;
-	fis[0] = 0x27;	/* Host to device */
-	fis[15] = 0x04;	/* SRST DEVCTL */
+	fis[0] = ATA_FIS_TYPE_H2D;
+	fis[15] = ATA_FIS_CONTROL_SRST;
 
 	cmd_slot->prdtl = 0;
 	cmd_slot->flags = htole16(5);	/* FIS length: 5 DWORDS */
@@ -1533,7 +1638,7 @@ ahci_port_softreset(struct ahci_port *ap)
 		goto err;
 
 	/* Prep second D2H command to read status and complete reset sequence */
-	fis[0] = 0x27;	/* Host to device */
+	fis[0] = ATA_FIS_TYPE_H2D;
 	fis[15] = 0;
 
 	cmd_slot->prdtl = 0;
@@ -1574,13 +1679,336 @@ err:
 	return (rc);
 }
 
+int
+ahci_pmp_port_softreset(struct ahci_port *ap, int pmp_port)
+{
+	struct ahci_ccb		*ccb = NULL;
+	u_int32_t		data;
+	int			count;
+	int			rc;
+	int			s;
+	struct ahci_cmd_hdr	*cmd_slot;
+	u_int8_t		*fis;
+
+	/* XXX FBS: ensure fbs is disabled on ap, since we're resetting
+	 * devices (AHCI section 9.3.8)
+	 */
+
+	s = splbio();
+	/* ignore spurious IFS errors while resetting */
+	printf("%s: now ignoring IFS\n", PORTNAME(ap));
+	ap->ap_pmp_ignore_ifs = 1;
+
+	count = 2;
+	rc = 0;
+	do {
+		if (ccb != NULL) {
+			ahci_put_pmp_ccb(ccb);
+			ccb = NULL;
+		}
+
+		if (ahci_pmp_phy_status(ap, pmp_port, &data)) {
+			printf("%s.%d: unable to clear PHY status\n",
+			    PORTNAME(ap), pmp_port);
+		}
+		ahci_pwrite(ap, AHCI_PREG_SERR, -1);
+		/* maybe don't do this on the first loop: */
+		ahci_pwrite(ap, AHCI_PREG_IS, AHCI_PREG_IS_IFS);
+		ahci_pmp_write(ap, pmp_port, SATA_PMREG_SERR, -1);
+
+		/* send first softreset FIS */
+		ccb = ahci_get_pmp_ccb(ap);
+		cmd_slot = ccb->ccb_cmd_hdr;
+		bzero(ccb->ccb_cmd_table, sizeof(struct ahci_cmd_table));
+
+		fis = ccb->ccb_cmd_table->cfis;
+		fis[0] = ATA_FIS_TYPE_H2D;
+		fis[1] = pmp_port;
+		fis[15] = ATA_FIS_CONTROL_SRST | ATA_FIS_CONTROL_4BIT;
+
+		cmd_slot->prdtl = 0;
+		cmd_slot->flags = htole16(5);	/* FIS length: 5 DWORDS */
+		cmd_slot->flags |= htole16(AHCI_CMD_LIST_FLAG_C);
+		cmd_slot->flags |= htole16(AHCI_CMD_LIST_FLAG_R);
+		cmd_slot->flags |= htole16(pmp_port <<
+		    AHCI_CMD_LIST_FLAG_PMP_SHIFT);
+
+		ccb->ccb_xa.state = ATA_S_PENDING;
+
+		DPRINTF(AHCI_D_VERBOSE, "%s.%d: sending PMP softreset cmd\n",
+		    PORTNAME(ap), pmp_port);
+		if (ahci_poll(ccb, 1000, ahci_pmp_probe_timeout) != 0) {
+			printf("%s.%d: PMP port softreset cmd failed\n",
+			       PORTNAME(ap), pmp_port);
+			rc = EBUSY;
+			if (count > 0) {
+				/* probably delay a while to allow
+				 * it to settle down?
+				 */
+			}
+			continue;
+		}
+
+		/* send signature FIS */
+		bzero(ccb->ccb_cmd_table, sizeof(struct ahci_cmd_table));
+		fis[0] = ATA_FIS_TYPE_H2D;
+		fis[1] = pmp_port;
+		fis[15] = ATA_FIS_CONTROL_4BIT;
+
+		cmd_slot->prdtl = 0;
+		cmd_slot->flags = htole16(5);	/* FIS length: 5 DWORDS */
+		cmd_slot->flags |= htole16(pmp_port <<
+		    AHCI_CMD_LIST_FLAG_PMP_SHIFT);
+
+		DPRINTF(AHCI_D_VERBOSE, "%s.%d: sending PMP probe status cmd\n",
+		    PORTNAME(ap), pmp_port);
+		ccb->ccb_xa.state = ATA_S_PENDING;
+		if (ahci_poll(ccb, 5000, ahci_pmp_probe_timeout) != 0) {
+			DPRINTF(AHCI_D_VERBOSE, "%s.%d: PMP probe status cmd "
+			    "failed\n", PORTNAME(ap), pmp_port);
+			rc = EBUSY;
+			if (count > 0) {
+				/* sleep a while? */
+			}
+			continue;
+		}
+
+		fis[15] = 0;
+		break;
+	} while (count--);
+
+	if (ccb != NULL) {
+		ahci_put_pmp_ccb(ccb);
+		ccb = NULL;
+	}
+
+	/* clean up a bit */
+	ahci_pmp_write(ap, pmp_port, SATA_PMREG_SERR, -1);
+	ahci_pwrite(ap, AHCI_PREG_SERR, -1);
+	ahci_pwrite(ap, AHCI_PREG_IS, AHCI_PREG_IS_IFS);
+	ap->ap_pmp_ignore_ifs = 0;
+	printf("%s: no longer ignoring IFS\n", PORTNAME(ap));
+	splx(s);
+
+	return (rc);
+}
+
+int
+ahci_pmp_port_probe(struct ahci_port *ap, int pmp_port)
+{
+	int sig;
+	
+	ap->ap_state = AP_S_PMP_PORT_PROBE;
+
+	printf("%s.%d: probing pmp port\n", PORTNAME(ap), pmp_port);
+	if (ahci_pmp_port_portreset(ap, pmp_port)) {
+		printf("%s.%d: unable to probe PMP port; portreset failed\n",
+		    PORTNAME(ap), pmp_port);
+		ap->ap_state = AP_S_NORMAL;
+		return (ATA_PORT_T_NONE);
+	}
+
+	if (ahci_pmp_port_softreset(ap, pmp_port)) {
+		printf("%s.%d: unable to probe PMP port due to softreset "
+		    "failure\n", PORTNAME(ap), pmp_port);
+		ap->ap_state = AP_S_NORMAL;
+		return (ATA_PORT_T_NONE);
+	}
+
+	sig = ahci_port_signature(ap);
+	printf("%s.%d: port signature returned %d\n", PORTNAME(ap), pmp_port,
+	    sig);
+	ap->ap_state = AP_S_NORMAL;
+	return (sig);
+}
+
+
+void
+ahci_flush_tfd(struct ahci_port *ap)
+{
+	u_int32_t r;
+
+	r = ahci_pread(ap, AHCI_PREG_SERR);
+	if (r & AHCI_PREG_SERR_DIAG_X)
+		ahci_pwrite(ap, AHCI_PREG_SERR, AHCI_PREG_SERR_DIAG_X);
+}
+
+u_int32_t
+ahci_active_mask(struct ahci_port *ap)
+{
+	u_int32_t mask;
+
+	mask = ahci_pread(ap, AHCI_PREG_CI);
+	if (ap->ap_sc->sc_cap & AHCI_REG_CAP_SNCQ)
+		mask |= ahci_pread(ap, AHCI_PREG_SACT);
+	return mask;
+}
+
+void
+ahci_pmp_probe_timeout(void *cookie)
+{
+	struct ahci_ccb *ccb = cookie;
+	struct ahci_port *ap = ccb->ccb_port;
+	u_int32_t mask;
+
+	DPRINTF(AHCI_D_VERBOSE, "%s: PMP probe cmd timed out\n", PORTNAME(ap));
+	switch (ccb->ccb_xa.state) {
+	case ATA_S_PENDING:
+		TAILQ_REMOVE(&ap->ap_ccb_pending, ccb, ccb_entry);
+		ccb->ccb_xa.state = ATA_S_TIMEOUT;
+		break;
+
+	case ATA_S_ONCHIP:
+	case ATA_S_ERROR:  /* currently mostly here for the ATI SBx00 quirk */
+		/* clear the command on-chip */
+		KASSERT(ap->ap_active == (1 << ccb->ccb_slot) &&
+		    ap->ap_sactive == 0);
+		ahci_port_stop(ap, 0);
+		ahci_port_start(ap, 0);
+
+		if (ahci_active_mask(ap) != 0) {
+			ahci_port_stop(ap, 0);
+			ahci_port_start(ap, 0);
+			mask = ahci_active_mask(ap);
+			if (mask != 0) {
+				printf("%s: ahci_pmp_probe_timeout: failed to "
+				    "clear active cmds: %08x\n", PORTNAME(ap),
+				    mask);
+			}
+		}
+
+		ccb->ccb_xa.state = ATA_S_TIMEOUT;
+		ap->ap_active &= ~(1 << ccb->ccb_slot);
+		KASSERT(ap->ap_active_cnt > 0);
+		--ap->ap_active_cnt;
+		printf("%s: timed out %d, active %x, count %d\n", PORTNAME(ap),
+		    ccb->ccb_slot, ap->ap_active, ap->ap_active_cnt);
+		break;
+
+	default:
+		panic("%s: ahci_pmp_probe_timeout: ccb in bad state %d",
+			PORTNAME(ap), ccb->ccb_xa.state);
+	}
+}
+
+int
+ahci_port_signature(struct ahci_port *ap)
+{
+	u_int32_t sig;
+
+	sig = ahci_pread(ap, AHCI_PREG_SIG);
+	if ((sig & 0xffff0000) == (SATA_SIGNATURE_ATAPI & 0xffff0000))
+		return (ATA_PORT_T_ATAPI);
+	else if ((sig & 0xffff0000) == (SATA_SIGNATURE_PORT_MULTIPLIER &
+	    0xffff0000))
+		return (ATA_PORT_T_PM);
+	else
+		return (ATA_PORT_T_DISK);
+}
+
+int
+ahci_pmp_port_portreset(struct ahci_port *ap, int pmp_port)
+{
+	u_int32_t cmd, data;
+	int loop;
+	int rc = 1;
+	int s;
+
+	s = splbio();
+	DPRINTF(AHCI_D_VERBOSE, "%s.%d: PMP port reset\n", PORTNAME(ap),
+	    pmp_port);
+
+	cmd = ahci_pread(ap, AHCI_PREG_CMD) & ~AHCI_PREG_CMD_ICC;
+
+	/* turn off power management and disable the PHY */
+	data = AHCI_PREG_SCTL_IPM_DISABLED;
+	/* maybe add AHCI_PREG_SCTL_DET_DISABLE */
+	if (ahci_pmp_write(ap, pmp_port, SATA_PMREG_SERR, -1))
+		goto err;
+	if (ahci_pmp_write(ap, pmp_port, SATA_PMREG_SCTL, data))
+		goto err;
+	delay(10000);
+
+	/* start COMRESET */
+	data = AHCI_PREG_SCTL_IPM_DISABLED | AHCI_PREG_SCTL_DET_INIT;
+	if ((ap->ap_sc->sc_dev.dv_cfdata->cf_flags & 0x01) != 0) {
+		DPRINTF(AHCI_D_VERBOSE, "%s.%d: forcing GEN1\n", PORTNAME(ap),
+		    pmp_port);
+		data |= AHCI_PREG_SCTL_SPD_GEN1;
+	} else
+		data |= AHCI_PREG_SCTL_SPD_ANY;
+	
+	if (ahci_pmp_write(ap, pmp_port, SATA_PMREG_SCTL, data))
+		goto err;
+
+	/* give it a while to settle down */
+	delay(100000);
+
+	if (ahci_pmp_phy_status(ap, pmp_port, &data)) {
+		printf("%s.%d: cannot clear PHY status\n", PORTNAME(ap),
+		    pmp_port);
+	}
+
+	/* start trying to negotiate */
+	ahci_pmp_write(ap, pmp_port, SATA_PMREG_SERR, -1);
+	data = AHCI_PREG_SCTL_IPM_DISABLED | AHCI_PREG_SCTL_DET_NONE;
+	if (ahci_pmp_write(ap, pmp_port, SATA_PMREG_SCTL, data))
+		goto err;
+
+	/* give it a while to detect */
+	for (loop = 3; loop; --loop) {
+		if (ahci_pmp_read(ap, pmp_port, SATA_PMREG_SSTS, &data))
+			goto err;
+		if (data & AHCI_PREG_SSTS_DET)
+			break;
+		delay(100000);
+	}
+	if (loop == 0) {
+		printf("%s.%d: port is unplugged\n", PORTNAME(ap), pmp_port);
+		goto err;
+	}
+
+	/* give it even longer to fully negotiate */
+	for (loop = 30; loop; --loop) {
+		if (ahci_pmp_read(ap, pmp_port, SATA_PMREG_SSTS, &data))
+			goto err;
+		if ((data & AHCI_PREG_SSTS_DET) == AHCI_PREG_SSTS_DET_DEV)
+			break;
+		delay(100000);
+	}
+
+	if (loop == 0) {
+		printf("%s.%d: device is not negotiating\n", PORTNAME(ap),
+		    pmp_port);
+		goto err;
+	}
+
+	/* device detected */
+	printf("%s.%d: device detected\n", PORTNAME(ap), pmp_port);
+
+	/* clean up a bit */
+	delay(100000);
+	ahci_pmp_write(ap, pmp_port, SATA_PMREG_SERR, -1);
+	ahci_pwrite(ap, AHCI_PREG_SERR, -1);
+	ahci_pwrite(ap, AHCI_PREG_IS, AHCI_PREG_IS_IFS);
+
+	rc = 0;
+err:
+	splx(s);
+	return (rc);
+}
+
 /* AHCI port reset, Section 10.4.2 */
 int
-ahci_port_portreset(struct ahci_port *ap)
+ahci_port_portreset(struct ahci_port *ap, int pmp)
 {
 	u_int32_t			cmd, r;
-	int				rc;
+	int				rc, count, pmp_rc, s;
+	struct ahci_cmd_hdr		*cmd_slot;
+	struct ahci_ccb			*ccb = NULL;
+	u_int8_t			*fis = NULL;
 
+	s = splbio();
 	DPRINTF(AHCI_D_VERBOSE, "%s: port reset\n", PORTNAME(ap));
 
 	/* Save previous command register state */
@@ -1609,23 +2037,225 @@ ahci_port_portreset(struct ahci_port *ap)
 	if (ahci_pwait_eq(ap, AHCI_PREG_SSTS, AHCI_PREG_SSTS_DET,
 	    AHCI_PREG_SSTS_DET_DEV, 1)) {
 		rc = ENODEV;
+		if (ahci_pread(ap, AHCI_PREG_SSTS) & AHCI_PREG_SSTS_DET) {
+			/* this may be a port multiplier with no device
+			 * on port 0, so still do the pmp check if requested.
+			 */
+		} else {
+			goto err;
+		}
+	} else {
+		/* Clear SERR (incl X bit), so TFD can update */
+		ahci_pwrite(ap, AHCI_PREG_SERR, ahci_pread(ap, AHCI_PREG_SERR));
+
+		/* Wait for device to become ready */
+		if (ahci_pwait_clr(ap, AHCI_PREG_TFD, AHCI_PREG_TFD_STS_BSY |
+		    AHCI_PREG_TFD_STS_DRQ | AHCI_PREG_TFD_STS_ERR, 3)) {
+			/* even if the device doesn't wake up, check if there's
+			 * a port multiplier there
+			 */
+			rc = EBUSY;
+		} else {
+			rc = 0;
+		}
+	}
+
+	if (pmp == 0 ||
+	    !ISSET(ahci_read(ap->ap_sc, AHCI_REG_CAP), AHCI_REG_CAP_SPM)) {
 		goto err;
 	}
 
-	/* Clear SERR (incl X bit), so TFD can update */
-	ahci_pwrite(ap, AHCI_PREG_SERR, ahci_pread(ap, AHCI_PREG_SERR));
+	pmp_rc = 0;
+	count = 2;
+	do {
+		DPRINTF(AHCI_D_VERBOSE, "%s: PMP probe %d\n", PORTNAME(ap),
+		    count);
+		if (ccb != NULL) {
+			ahci_put_pmp_ccb(ccb);
+			ccb = NULL;
+		}
+		ahci_port_stop(ap, 0);
+		ap->ap_state = AP_S_PMP_PROBE;
 
-	/* Wait for device to become ready */
-	if (ahci_pwait_clr(ap, AHCI_PREG_TFD, AHCI_PREG_TFD_STS_BSY |
-	    AHCI_PREG_TFD_STS_DRQ | AHCI_PREG_TFD_STS_ERR, 3)) {
-		rc = EBUSY;
-		goto err;
+		/* set PMA in cmd reg */
+		cmd = ahci_pread(ap, AHCI_PREG_CMD) & ~AHCI_PREG_CMD_ICC;
+		if ((cmd & AHCI_PREG_CMD_PMA) == 0) {
+			cmd |= AHCI_PREG_CMD_PMA;
+			ahci_pwrite(ap, AHCI_PREG_CMD, cmd);
+		}
+
+		/* Flush errors and request CLO unconditionally,
+		 * then start the port
+		 */
+		r = ahci_pread(ap, AHCI_PREG_SERR);
+		if (r & AHCI_PREG_SERR_DIAG_X)
+			ahci_pwrite(ap, AHCI_PREG_SERR,
+			    AHCI_PREG_SERR_DIAG_X);
+		
+		/* Request CLO */
+		ahci_port_clo(ap);
+
+		/* Clear port errors to permit TFD transfer */
+		r = ahci_pread(ap, AHCI_PREG_SERR);
+		ahci_pwrite(ap, AHCI_PREG_SERR, r);
+
+		/* XXX FBS: ensure we don't enable FBS here, since
+		 * we're resetting the port
+		 * (AHCI section 9.3.8)
+		 */
+		/* Restart port */
+		if (ahci_port_start(ap, 0)) {
+			rc = 1;
+			printf("%s: failed to start port, cannot probe PMP\n",
+			    PORTNAME(ap));
+			break;
+		}
+
+		/* Check whether CLO worked */
+		if (ahci_pwait_clr(ap, AHCI_PREG_TFD,
+		    AHCI_PREG_TFD_STS_BSY | AHCI_PREG_TFD_STS_DRQ, 1)) {
+			u_int32_t cap;
+
+			cap = ahci_read(ap->ap_sc, AHCI_REG_CAP);
+			printf("%s: CLO %s, need port reset\n",
+			    PORTNAME(ap),
+			    ISSET(cap, AHCI_REG_CAP_SCLO)
+			    ? "failed" : "unsupported");
+			pmp_rc = EBUSY;
+			break;
+		}
+
+		/* Prep first command with SRST feature &
+		 * clear busy/reset flags
+		 */
+		ccb = ahci_get_pmp_ccb(ap);
+		cmd_slot = ccb->ccb_cmd_hdr;
+		bzero(ccb->ccb_cmd_table,
+		    sizeof(struct ahci_cmd_table));
+
+		fis = ccb->ccb_cmd_table->cfis;
+		fis[0] = ATA_FIS_TYPE_H2D;
+		fis[1] = SATA_PMP_CONTROL_PORT;
+		fis[15] = ATA_FIS_CONTROL_SRST | ATA_FIS_CONTROL_4BIT;
+
+		cmd_slot->prdtl = 0;
+		cmd_slot->flags = htole16(5); /* FIS length: 5 DWORDS */
+		cmd_slot->flags |= htole16(AHCI_CMD_LIST_FLAG_C);
+		cmd_slot->flags |= htole16(AHCI_CMD_LIST_FLAG_R);
+		cmd_slot->flags |= htole16(AHCI_CMD_LIST_FLAG_PMP);
+
+		DPRINTF(AHCI_D_VERBOSE, "%s: sending PMP reset cmd\n",
+		    PORTNAME(ap));
+		ccb->ccb_xa.state = ATA_S_PENDING;
+		if (ahci_poll(ccb, 1000, ahci_pmp_probe_timeout) != 0) {
+			DPRINTF(AHCI_D_VERBOSE, "%s: PMP reset cmd failed\n",
+			    PORTNAME(ap));
+			pmp_rc = EBUSY;
+			continue;
+		}
+
+		if (ahci_pwait_clr(ap, AHCI_PREG_TFD,
+		    AHCI_PREG_TFD_STS_BSY | AHCI_PREG_TFD_STS_DRQ, 1)) {
+			printf("%s: port busy after first PMP probe FIS\n",
+			    PORTNAME(ap));
+		}
+
+		/* clear errors in case the device
+		 * didn't reset cleanly
+		 */
+		ahci_flush_tfd(ap);
+		r = ahci_pread(ap, AHCI_PREG_SERR);
+		ahci_pwrite(ap, AHCI_PREG_SERR, r);
+
+		/* Prep second command to read status and
+		 * complete reset sequence
+		 */
+		bzero(ccb->ccb_cmd_table,
+		    sizeof(struct ahci_cmd_table));
+		fis[0] = ATA_FIS_TYPE_H2D;
+		fis[1] = SATA_PMP_CONTROL_PORT;
+		fis[15] = ATA_FIS_CONTROL_4BIT;
+
+		cmd_slot->prdtl = 0;
+		cmd_slot->flags = htole16(5); /* FIS length: 5 DWORDS */
+		cmd_slot->flags |= htole16(AHCI_CMD_LIST_FLAG_PMP);
+
+		DPRINTF(AHCI_D_VERBOSE, "%s: sending PMP probe status cmd\n",
+		    PORTNAME(ap));
+		ccb->ccb_xa.state = ATA_S_PENDING;
+		if (ahci_poll(ccb, 5000, ahci_pmp_probe_timeout) != 0) {
+			DPRINTF(AHCI_D_VERBOSE, "%s: PMP probe status "
+			    "cmd failed\n", PORTNAME(ap));
+			pmp_rc = EBUSY;
+			continue;
+		}
+
+		/* apparently we need to retry at least once
+		 * to get the right signature
+		 */
+		fis[15] = 0;
+		pmp_rc = 0;
+	} while (--count);
+
+	if (ccb != NULL) {
+		ahci_put_pmp_ccb(ccb);
+		ccb = NULL;
+	}
+
+	if (ap->ap_state == AP_S_PMP_PROBE) {
+		ap->ap_state = AP_S_NORMAL;
+	}
+
+	if (pmp_rc == 0) {
+		if (ahci_port_signature(ap) != ATA_PORT_T_PM) {
+			DPRINTF(AHCI_D_VERBOSE, "%s: device is not a PMP\n",
+			    PORTNAME(ap));
+			pmp_rc = EBUSY;
+		} else {
+			DPRINTF(AHCI_D_VERBOSE, "%s: PMP found\n",
+			    PORTNAME(ap));
+		}
+	}
+
+	if (pmp_rc == 0) {
+		if (ahci_pmp_identify(ap, &ap->ap_pmp_ports)) {
+			pmp_rc = EBUSY;
+		} else {
+			/* XXX enable FBS if available */
+			rc = 0;
+		}
+	}
+
+	/* if PMP detection failed, so turn off the PMA bit and
+	 * reset the port again
+	 */
+	if (pmp_rc != 0) {
+		DPRINTF(AHCI_D_VERBOSE, "%s: no PMP found, resetting "
+		    "the port\n", PORTNAME(ap));
+		ahci_port_stop(ap, 0);
+		ahci_port_clo(ap);
+		cmd = ahci_pread(ap, AHCI_PREG_CMD) & ~AHCI_PREG_CMD_ICC;
+		cmd &= ~AHCI_PREG_CMD_PMA;
+		ahci_pwrite(ap, AHCI_PREG_CMD, cmd);
+
+		ahci_pwrite(ap, AHCI_PREG_IE, 0);
+		ahci_port_stop(ap, 0);
+		if (ap->ap_sc->sc_cap & AHCI_REG_CAP_SSNTF)
+			ahci_pwrite(ap, AHCI_PREG_SNTF, -1);
+		ahci_flush_tfd(ap);
+		ahci_pwrite(ap, AHCI_PREG_SERR, -1);
+
+		ahci_pwrite(ap, AHCI_PREG_IS, -1);
+
+		ahci_enable_interrupts(ap);
+
+		ahci_port_portreset(ap, 0);
 	}
 
-	rc = 0;
 err:
 	/* Restore preserved port state */
 	ahci_pwrite(ap, AHCI_PREG_CMD, cmd);
+	splx(s);
 
 	return (rc);
 }
@@ -1729,6 +2359,16 @@ ahci_poll(struct ahci_ccb *ccb, int timeout, void (*timeout_fn)(void *))
 			splx(s);
 			return (0);
 		}
+		if (ccb->ccb_xa.state == ATA_S_ERROR) {
+			printf("%s: ccb in slot %d errored\n", PORTNAME(ap),
+			    ccb->ccb_slot);
+			/* pretend it timed out? */
+			if (timeout_fn != NULL) {
+				timeout_fn(ccb);
+			}
+			splx(s);
+			return (1);
+		}
 
 		delay(1000);
 	} while (--timeout > 0);
@@ -1763,17 +2403,29 @@ ahci_start(struct ahci_ccb *ccb)
 	bus_dmamap_sync(sc->sc_dmat, AHCI_DMA_MAP(ap->ap_dmamem_rfis), 0,
 	    sizeof(struct ahci_rfis), BUS_DMASYNC_PREREAD);
 
+	/* XXX FBS: need to figure out whether we still need to keep NCQ and
+	 * non-queued commands separate when FBS is in use.  I guess probably
+	 * not?  it's not particularly clear from the spec..
+	 */
+
 	if (ccb->ccb_xa.flags & ATA_F_NCQ) {
 		/* Issue NCQ commands only when there are no outstanding
 		 * standard commands. */
-		if (ap->ap_active != 0 || !TAILQ_EMPTY(&ap->ap_ccb_pending))
+		if (ap->ap_active != 0 || !TAILQ_EMPTY(&ap->ap_ccb_pending) ||
+		    (ap->ap_sactive != 0 &&
+		     ap->ap_pmp_ncq_port != ccb->ccb_xa.pmp_port)) {
 			TAILQ_INSERT_TAIL(&ap->ap_ccb_pending, ccb, ccb_entry);
-		else {
+		} else {
+			/* XXX FBS: if using FBS, set AHCI_PREG_FBS_DEV
+			 * to the port number
+			 */
+
 			KASSERT(ap->ap_active_cnt == 0);
 			ap->ap_sactive |= (1 << ccb->ccb_slot);
 			ccb->ccb_xa.state = ATA_S_ONCHIP;
 			ahci_pwrite(ap, AHCI_PREG_SACT, 1 << ccb->ccb_slot);
 			ahci_pwrite(ap, AHCI_PREG_CI, 1 << ccb->ccb_slot);
+			ap->ap_pmp_ncq_port = ccb->ccb_xa.pmp_port;
 		}
 	} else {
 		/* Wait for all NCQ commands to finish before issuing standard
@@ -1781,6 +2433,10 @@ ahci_start(struct ahci_ccb *ccb)
 		if (ap->ap_sactive != 0 || ap->ap_active_cnt == 2)
 			TAILQ_INSERT_TAIL(&ap->ap_ccb_pending, ccb, ccb_entry);
 		else if (ap->ap_active_cnt < 2) {
+			/* XXX FBS: if using FBS, set AHCI_PREG_FBS_DEV to the
+			 * port number
+			 */
+
 			ap->ap_active |= 1 << ccb->ccb_slot;
 			ccb->ccb_xa.state = ATA_S_ONCHIP;
 			ahci_pwrite(ap, AHCI_PREG_CI, 1 << ccb->ccb_slot);
@@ -1801,19 +2457,32 @@ ahci_issue_pending_ncq_commands(struct ahci_port *ap)
 	if (nextccb == NULL || !(nextccb->ccb_xa.flags & ATA_F_NCQ))
 		return;
 
-	/* Start all the NCQ commands at the head of the pending list. */
+	/* XXX FBS:
+	 * - set AHCI_PREG_FBS_DEV for each command
+	 * - one write to AHCI_PREG_CI per command
+	 */
+
+	/* Start all the NCQ commands at the head of the pending list.
+	 * If a port multiplier is attached to the port, we can only
+	 * issue commands for one of its ports at a time.
+	 */
+	if (ap->ap_sactive != NULL &&
+	    ap->ap_pmp_ncq_port != nextccb->ccb_xa.pmp_port) {
+		return;
+	}
+
+	ap->ap_pmp_ncq_port = nextccb->ccb_xa.pmp_port;
 	do {
 		TAILQ_REMOVE(&ap->ap_ccb_pending, nextccb, ccb_entry);
 		sact_change |= 1 << nextccb->ccb_slot;
 		nextccb->ccb_xa.state = ATA_S_ONCHIP;
 		nextccb = TAILQ_FIRST(&ap->ap_ccb_pending);
-	} while (nextccb && (nextccb->ccb_xa.flags & ATA_F_NCQ));
+	} while (nextccb && (nextccb->ccb_xa.flags & ATA_F_NCQ) &&
+	    (nextccb->ccb_xa.pmp_port == ap->ap_pmp_ncq_port));
 
 	ap->ap_sactive |= sact_change;
 	ahci_pwrite(ap, AHCI_PREG_SACT, sact_change);
 	ahci_pwrite(ap, AHCI_PREG_CI, sact_change);
-
-	return;
 }
 
 void
@@ -1823,12 +2492,18 @@ ahci_issue_pending_commands(struct ahci_port *ap, int last_was_ncq)
 
 	nextccb = TAILQ_FIRST(&ap->ap_ccb_pending);
 	if (nextccb && (nextccb->ccb_xa.flags & ATA_F_NCQ)) {
-		KASSERT(last_was_ncq == 0);	/* otherwise it should have
-						 * been started already. */
+		if (last_was_ncq) {
+			KASSERT(nextccb->ccb_xa.pmp_port !=
+			    ap->ap_pmp_ncq_port);
+			/* otherwise it should have been started already */
+		} else {
+			ap->ap_active_cnt--;
+		}
 
 		/* Issue NCQ commands only when there are no outstanding
-		 * standard commands. */
-		ap->ap_active_cnt--;
+		 * standard commands, and previous NCQ commands for other
+		 * PMP ports have finished.
+		 */
 		if (ap->ap_active == 0)
 			ahci_issue_pending_ncq_commands(ap);
 		else
@@ -1847,6 +2522,7 @@ ahci_issue_pending_commands(struct ahci_port *ap, int last_was_ncq)
 			TAILQ_REMOVE(&ap->ap_ccb_pending, nextccb, ccb_entry);
 			ap->ap_active |= 1 << nextccb->ccb_slot;
 			nextccb->ccb_xa.state = ATA_S_ONCHIP;
+			/* XXX FBS: set AHCI_PREG_FBS_DEV here */
 			ahci_pwrite(ap, AHCI_PREG_CI, 1 << nextccb->ccb_slot);
 			if (last_was_ncq)
 				ap->ap_active_cnt++;
@@ -1911,6 +2587,7 @@ ahci_port_intr(struct ahci_port *ap, u_int32_t ci_mask)
 	struct ahci_softc		*sc = ap->ap_sc;
 	u_int32_t			is, ci_saved, ci_masked, processed = 0;
 	int				slot, need_restart = 0;
+	int				process_error = 0;
 	struct ahci_ccb			*ccb;
 	volatile u_int32_t		*active;
 #ifdef DIAGNOSTIC
@@ -1939,8 +2616,32 @@ ahci_port_intr(struct ahci_port *ap, u_int32_t ci_mask)
 		active = &ap->ap_active;
 	}
 
-	/* Command failed.  See AHCI 1.1 spec 6.2.2.1 and 6.2.2.2. */
 	if (is & AHCI_PREG_IS_TFES) {
+		process_error = 1;
+	} else if (is & AHCI_PREG_IS_DHRS) {
+		u_int32_t tfd;
+		u_int32_t cmd;
+		u_int32_t serr;
+
+		tfd = ahci_pread(ap, AHCI_PREG_TFD);
+		cmd = ahci_pread(ap, AHCI_PREG_CMD);
+		serr = ahci_pread(ap, AHCI_PREG_SERR);
+		if ((tfd & AHCI_PREG_TFD_STS_ERR) &&
+		    (cmd & AHCI_PREG_CMD_CR) == 0) {
+			DPRINTF(AHCI_D_VERBOSE, "%s: DHRS error, TFD: %b, SERR:"
+			    " %b, DIAG: %b\n", PORTNAME(ap), tfd,
+			    AHCI_PFMT_TFD_STS, AHCI_PREG_SERR_ERR(serr),
+			    AHCI_PFMT_SERR_ERR, AHCI_PREG_SERR_DIAG(serr),
+			    AHCI_PFMT_SERR_DIAG);
+			process_error = 1;
+		} else {
+			/* rfis copy back is in the normal execution path */
+			ahci_pwrite(ap, AHCI_PREG_IS, AHCI_PREG_IS_DHRS);
+		}
+	}
+
+	/* Command failed.  See AHCI 1.1 spec 6.2.2.1 and 6.2.2.2. */
+	if (process_error) {
 		u_int32_t		tfd, serr;
 		int			err_slot;
 
@@ -1993,22 +2694,54 @@ ahci_port_intr(struct ahci_port *ap, u_int32_t ci_mask)
 
 		/* If device hasn't cleared its busy status, try to idle it. */
 		if (ISSET(tfd, AHCI_PREG_TFD_STS_BSY | AHCI_PREG_TFD_STS_DRQ)) {
-			printf("%s: attempting to idle device\n", PORTNAME(ap));
-			if (ahci_port_softreset(ap)) {
-				printf("%s: failed to soft reset device\n",
+
+			if (ap->ap_state == AP_S_PMP_PORT_PROBE) {
+				/* can't reset the port here, just make sure
+				 * the probe fails and the port still works.
+				 */
+			} else if (ap->ap_pmp_ports != 0 && err_slot != -1) {
+				printf("%s: error on PMP port %d, idling "
+				    "device\n", PORTNAME(ap),
+				    ccb->ccb_xa.pmp_port);
+				if (ahci_pmp_port_softreset(ap,
+				        ccb->ccb_xa.pmp_port) == 0) {
+					printf("%s: unable to softreset port "
+					    "%d\n", PORTNAME(ap),
+					    ccb->ccb_xa.pmp_port);
+					if (ahci_pmp_port_portreset(ap,
+						ccb->ccb_xa.pmp_port)) {
+						printf("%s: failed to port "
+						    " reset %d, giving up on "
+						    "it\n", PORTNAME(ap),
+						    ccb->ccb_xa.pmp_port);
+						goto fatal;
+					}
+				}
+			} else {
+				printf("%s: attempting to idle device\n",
 				    PORTNAME(ap));
-				if (ahci_port_portreset(ap)) {
-					printf("%s: failed to port reset "
-					    "device, give up on it\n",
-					    PORTNAME(ap));
-					goto fatal;
+				if (ahci_port_softreset(ap)) {
+					printf("%s: failed to soft reset "
+					    "device\n", PORTNAME(ap));
+					if (ahci_port_portreset(ap, 0)) {
+						printf("%s: failed to port "
+						    "reset device, give up on "
+						    "it\n", PORTNAME(ap));
+						goto fatal;
+					}
 				}
 			}
 
 			/* Had to reset device, can't gather extended info. */
 		} else if (ap->ap_sactive) {
-			/* Recover the NCQ error from log page 10h. */
-			ahci_port_read_ncq_error(ap, &err_slot);
+			/* Recover the NCQ error from log page 10h.
+			 * XXX FBS: need to do things to figure out where the
+			 * error came from.  without FBS, we know the PMP port
+			 * responsible because we can only have queued commands
+			 * active for one port at a time.
+			 */
+			ahci_port_read_ncq_error(ap, &err_slot,
+			    ap->ap_pmp_ncq_port);
 			if (err_slot < 0)
 				goto failall;
 
@@ -2026,7 +2759,7 @@ ahci_port_intr(struct ahci_port *ap, u_int32_t ci_mask)
 		 */
 		if (err_slot == -1) {
 			if (ahci_port_softreset(ap) != 0 &&
-			    ahci_port_portreset(ap) != 0) {
+			    ahci_port_portreset(ap, 0) != 0) {
 				printf("%s: couldn't reset after NCQ error, "
 				    "disabling device.\n", PORTNAME(ap));
 				goto fatal;
@@ -2056,6 +2789,35 @@ ahci_port_intr(struct ahci_port *ap, u_int32_t ci_mask)
 #endif
 	}
 
+	/* ATI SBx00 AHCI controllers respond to PMP probes with IPMS interrupts
+	 * when there's a normal SATA device attached.
+	 */
+	if ((ap->ap_state == AP_S_PMP_PROBE) &&
+	    (ap->ap_sc->sc_flags & AHCI_F_IPMS_PROBE) &&
+	    (is & AHCI_PREG_IS_IPMS)) {
+		slot = AHCI_PREG_CMD_CCS(ahci_pread(ap, AHCI_PREG_CMD));
+		DPRINTF(AHCI_D_INTR, "%s: slot %d received IPMS\n",
+		    PORTNAME(ap), slot);
+
+		ccb = &ap->ap_ccbs[slot];
+		ccb->ccb_xa.state = ATA_S_ERROR;
+
+		ahci_pwrite(ap, AHCI_PREG_IS, AHCI_PREG_IS_IPMS);
+		is &= ~AHCI_PREG_IS_IPMS;
+	}
+
+	/* ignore IFS errors while resetting a PMP port */
+	if ((is & AHCI_PREG_IS_IFS) /*&& ap->ap_pmp_ignore_ifs*/) {
+		DPRINTF(AHCI_D_INTR, "%s: ignoring IFS while resetting PMP "
+		    "port\n", PORTNAME(ap));
+
+		need_restart = 1;
+		ahci_pwrite(ap, AHCI_PREG_SERR, -1);
+		ahci_pwrite(ap, AHCI_PREG_IS, AHCI_PREG_IS_IFS);
+		is &= ~AHCI_PREG_IS_IFS;
+		goto failall;
+	}
+
 	/* Check for remaining errors - they are fatal. */
 	if (is & (AHCI_PREG_IS_TFES | AHCI_PREG_IS_HBFS | AHCI_PREG_IS_IFS |
 	    AHCI_PREG_IS_OFS | AHCI_PREG_IS_UFS)) {
@@ -2122,6 +2884,14 @@ failall:
 		    sizeof(struct ahci_rfis), BUS_DMASYNC_POSTREAD);
 
 		*active &= ~(1 << ccb->ccb_slot);
+		/* Copy the rfis into the ccb if we were asked for it */
+		if (ccb->ccb_xa.state == ATA_S_ONCHIP &&
+		    ccb->ccb_xa.flags & ATA_F_GET_RFIS) {
+			memcpy(&ccb->ccb_xa.rfis,
+			       ap->ap_rfis->rfis,
+			       sizeof(struct ata_fis_d2h));
+		}
+
 		ccb->ccb_done(ccb);
 
 		processed |= 1 << ccb->ccb_slot;
@@ -2148,6 +2918,11 @@ failall:
 			    "re-enabling%s slots %08x\n", PORTNAME(ap),
 			    ap->ap_sactive ? " NCQ" : "", ci_saved);
 
+			/* XXX FBS:
+			 * - need to set AHCI_PREG_FBS_DEV for each command
+			 * - can't do multiple commands with a single write to
+			 *   AHCI_PREG_CI
+			 */
 			if (ap->ap_sactive)
 				ahci_pwrite(ap, AHCI_PREG_SACT, ci_saved);
 			ahci_pwrite(ap, AHCI_PREG_CI, ci_saved);
@@ -2266,8 +3041,60 @@ ahci_put_err_ccb(struct ahci_ccb *ccb)
 #endif
 }
 
+struct ahci_ccb *
+ahci_get_pmp_ccb(struct ahci_port *ap)
+{
+	struct ahci_ccb *ccb;
+	u_int32_t sact;
+
+	/* some PMP commands need to be issued on slot 1,
+	 * particularly the command that clears SRST and
+	 * fetches the device signature.
+	 *
+	 * ensure the chip is idle and ccb 1 is available.
+	 */
+	splassert(IPL_BIO);
+
+	sact = ahci_pread(ap, AHCI_PREG_SACT);
+	if (sact != 0)
+		printf("ahci_get_pmp_ccb; SACT %08x != 0\n", sact);
+	KASSERT(ahci_pread(ap, AHCI_PREG_CI) == 0);
+
+	ccb = &ap->ap_ccbs[1];
+	KASSERT(ccb->ccb_xa.state == ATA_S_PUT);
+	ccb->ccb_xa.flags = 0;
+	ccb->ccb_done = ahci_pmp_cmd_done;
+
+	mtx_enter(&ap->ap_ccb_mtx);
+	TAILQ_REMOVE(&ap->ap_ccb_free, ccb, ccb_entry);
+	mtx_leave(&ap->ap_ccb_mtx);
+
+	return ccb;
+}
+
+void
+ahci_put_pmp_ccb(struct ahci_ccb *ccb)
+{
+	struct ahci_port *ap = ccb->ccb_port;
+	u_int32_t sact;
+	
+	/* make sure this is the right ccb */
+	KASSERT(ccb == &ap->ap_ccbs[1]);
+
+	/* No commands may be active on the chip */
+	sact = ahci_pread(ap, AHCI_PREG_SACT);
+	if (sact != 0)
+		printf("ahci_port_err_ccb_restore but SACT %08x != 0?\n", sact);
+	KASSERT(ahci_pread(ap, AHCI_PREG_CI) == 0);
+
+	ccb->ccb_xa.state = ATA_S_PUT;
+	mtx_enter(&ap->ap_ccb_mtx);
+	TAILQ_INSERT_TAIL(&ap->ap_ccb_free, ccb, ccb_entry);
+	mtx_leave(&ap->ap_ccb_mtx);
+}
+
 int
-ahci_port_read_ncq_error(struct ahci_port *ap, int *err_slotp)
+ahci_port_read_ncq_error(struct ahci_port *ap, int *err_slotp, int pmp_port)
 {
 	struct ahci_ccb			*ccb;
 	struct ahci_cmd_hdr		*cmd_slot;
@@ -2294,7 +3121,7 @@ ahci_port_read_ncq_error(struct ahci_port *ap, int *err_slotp)
 
 	fis = (struct ata_fis_h2d *)ccb->ccb_cmd_table->cfis;
 	fis->type = ATA_FIS_TYPE_H2D;
-	fis->flags = ATA_H2D_FLAGS_CMD;
+	fis->flags = ATA_H2D_FLAGS_CMD | pmp_port;
 	fis->command = ATA_C_READ_LOG_EXT;
 	fis->lba_low = 0x10;		/* queued error log page (10h) */
 	fis->sector_count = 1;		/* number of sectors (1) */
@@ -2304,6 +3131,7 @@ ahci_port_read_ncq_error(struct ahci_port *ap, int *err_slotp)
 	fis->device = 0;
 
 	cmd_slot->flags = htole16(5);	/* FIS length: 5 DWORDS */
+	cmd_slot->flags |= htole16(pmp_port << AHCI_CMD_LIST_FLAG_PMP_SHIFT);
 
 	if (ahci_load_prdt(ccb) != 0) {
 		rc = ENOMEM;	/* XXX caller must abort all commands */
@@ -2472,24 +3300,27 @@ ahci_pwait_eq(struct ahci_port *ap, bus_size_t r, u_int32_t mask,
 }
 
 int
-ahci_ata_probe(void *xsc, int port)
+ahci_ata_probe(void *xsc, int port, int lun)
 {
 	struct ahci_softc		*sc = xsc;
 	struct ahci_port		*ap = sc->sc_ports[port];
-	u_int32_t			sig;
 
 	if (ap == NULL)
 		return (ATA_PORT_T_NONE);
 
-	sig = ahci_pread(ap, AHCI_PREG_SIG);
-	if ((sig & 0xffff0000) == (SATA_SIGNATURE_ATAPI & 0xffff0000))
-		return (ATA_PORT_T_ATAPI);
-	else
-		return (ATA_PORT_T_DISK);
+	if (lun != 0) {
+		int pmp_port = lun - 1;
+		if (pmp_port >= ap->ap_pmp_ports) {
+			return (ATA_PORT_T_NONE);
+		}
+		return (ahci_pmp_port_probe(ap, pmp_port));
+	} else {
+		return (ahci_port_signature(ap));
+	}
 }
 
 void
-ahci_ata_free(void *xsc, int port)
+ahci_ata_free(void *xsc, int port, int lun)
 {
 
 }
@@ -2538,6 +3369,8 @@ ahci_ata_cmd(struct ata_xfer *xa)
 
 	cmd_slot = ccb->ccb_cmd_hdr;
 	cmd_slot->flags = htole16(5); /* FIS length (in DWORDs) */
+	cmd_slot->flags |= htole16(xa->pmp_port <<
+	    AHCI_CMD_LIST_FLAG_PMP_SHIFT);
 
 	if (xa->flags & ATA_F_WRITE)
 		cmd_slot->flags |= htole16(AHCI_CMD_LIST_FLAG_W);
@@ -2572,6 +3405,19 @@ failcmd:
 }
 
 void
+ahci_pmp_cmd_done(struct ahci_ccb *ccb)
+{
+	struct ata_xfer			*xa = &ccb->ccb_xa;
+
+	if (xa->state == ATA_S_ONCHIP || xa->state == ATA_S_ERROR)
+		ahci_issue_pending_commands(ccb->ccb_port,
+		    xa->flags & ATA_F_NCQ);
+
+	xa->state = ATA_S_COMPLETE;
+}
+
+
+void
 ahci_ata_cmd_done(struct ahci_ccb *ccb)
 {
 	struct ata_xfer			*xa = &ccb->ccb_xa;
@@ -2649,9 +3495,9 @@ ahci_ata_cmd_timeout(void *arg)
 	/* Reset port to abort running command. */
 	if (ccb_was_started) {
 		DPRINTF(AHCI_D_TIMEOUT, "%s: resetting port to abort%s command "
-		    "in slot %d, active %08x\n", PORTNAME(ap), ncq_cmd ? " NCQ"
-		    : "", ccb->ccb_slot, *active);
-		if (ahci_port_softreset(ap) != 0 && ahci_port_portreset(ap)
+		    "in slot %d, pmp port %d, active %08x\n", PORTNAME(ap),
+		    ncq_cmd ? " NCQ" : "", ccb->ccb_slot, xa->pmp_port, *active);
+		if (ahci_port_softreset(ap) != 0 && ahci_port_portreset(ap, 0)
 		    != 0) {
 			printf("%s: failed to reset port during timeout "
 			    "handling, disabling it\n", PORTNAME(ap));
@@ -2690,3 +3536,129 @@ ahci_empty_done(struct ahci_ccb *ccb)
 {
 	ccb->ccb_xa.state = ATA_S_COMPLETE;
 }
+
+int
+ahci_pmp_read(struct ahci_port *ap, int target, int which, u_int32_t *datap)
+{
+	struct ahci_ccb	*ccb;
+	struct ata_fis_h2d *fis;
+	int error;
+
+	ccb = ahci_get_pmp_ccb(ap);
+	if (ccb == NULL) {
+		printf("%s: NULL ccb!\n", PORTNAME(ap));
+		return (1);
+	}
+	ccb->ccb_xa.flags = ATA_F_POLL | ATA_F_GET_RFIS;
+	ccb->ccb_xa.pmp_port = SATA_PMP_CONTROL_PORT;
+	ccb->ccb_xa.state = ATA_S_PENDING;
+	
+	bzero(ccb->ccb_cmd_table, sizeof(struct ahci_cmd_table));
+	fis = (struct ata_fis_h2d *)ccb->ccb_cmd_table->cfis;
+	fis->type = ATA_FIS_TYPE_H2D;
+	fis->flags = ATA_H2D_FLAGS_CMD | SATA_PMP_CONTROL_PORT;
+	fis->command = ATA_C_READ_PM;
+	fis->features = which;
+	fis->device = target | ATA_H2D_DEVICE_LBA;
+	fis->control = ATA_FIS_CONTROL_4BIT;
+
+	if (ahci_poll(ccb, 1000, ahci_pmp_probe_timeout) != 0) {
+		error = 1;
+	} else {
+		*datap = ccb->ccb_xa.rfis.sector_count |
+		    (ccb->ccb_xa.rfis.lba_low << 8) |
+		    (ccb->ccb_xa.rfis.lba_mid << 16) |
+		    (ccb->ccb_xa.rfis.lba_high << 24);
+		error = 0;
+	}
+	ahci_put_pmp_ccb(ccb);
+	return (error);
+}
+
+int
+ahci_pmp_write(struct ahci_port *ap, int target, int which, u_int32_t data)
+{
+	struct ahci_ccb	*ccb;
+	struct ata_fis_h2d *fis;
+	int error;
+
+	ccb = ahci_get_pmp_ccb(ap);
+	if (ccb == NULL) {
+		printf("%s: NULL ccb!\n", PORTNAME(ap));
+		return (1);
+	}
+	ccb->ccb_xa.flags = ATA_F_POLL;
+	ccb->ccb_xa.pmp_port = SATA_PMP_CONTROL_PORT;
+	ccb->ccb_xa.state = ATA_S_PENDING;
+
+	bzero(ccb->ccb_cmd_table, sizeof(struct ahci_cmd_table));
+	fis = (struct ata_fis_h2d *)ccb->ccb_cmd_table->cfis;
+	fis->type = ATA_FIS_TYPE_H2D;
+	fis->flags = ATA_H2D_FLAGS_CMD | SATA_PMP_CONTROL_PORT;
+	fis->command = ATA_C_WRITE_PM;
+	fis->features = which;
+	fis->device = target | ATA_H2D_DEVICE_LBA;
+	fis->sector_count = (u_int8_t)data;
+	fis->lba_low = (u_int8_t)(data >> 8);
+	fis->lba_mid = (u_int8_t)(data >> 16);
+	fis->lba_high = (u_int8_t)(data >> 24);
+	fis->control = ATA_FIS_CONTROL_4BIT;
+
+	error = ahci_poll(ccb, 1000, ahci_pmp_probe_timeout);
+	ahci_put_pmp_ccb(ccb);
+	return (error);
+}
+
+int
+ahci_pmp_phy_status(struct ahci_port *ap, int target, u_int32_t *datap)
+{
+	int error;
+
+	error = ahci_pmp_read(ap, target, SATA_PMREG_SSTS, datap);
+	if (error == 0)
+		error = ahci_pmp_write(ap, target, SATA_PMREG_SERR, -1);
+	if (error)
+		*datap = 0;
+
+	return (error);
+}
+
+int
+ahci_pmp_identify(struct ahci_port *ap, int *ret_nports)
+{
+	u_int32_t chipid;
+	u_int32_t rev;
+	u_int32_t nports;
+	u_int32_t features;
+	u_int32_t enabled;
+	int s;
+
+	s = splbio();
+
+	if (ahci_pmp_read(ap, 15, 0, &chipid) ||
+	    ahci_pmp_read(ap, 15, 1, &rev) ||
+	    ahci_pmp_read(ap, 15, 2, &nports) ||
+	    ahci_pmp_read(ap, 15, SATA_PMREG_FEA, &features) ||
+	    ahci_pmp_read(ap, 15, SATA_PMREG_FEAEN, &enabled)) {
+		printf("%s: port multiplier identification failed\n",
+		    PORTNAME(ap));
+		splx(s);
+		return (1);
+	}
+	splx(s);
+
+	nports &= 0x0F;
+
+	/* ignore SEMB port on SiI3726 port multiplier chips */
+	if (chipid == 0x37261095) {
+		nports--;
+	}
+
+	printf("%s: port multiplier found: chip=%08x rev=0x%b nports=%d, "
+	    "features: 0x%b, enabled: 0x%b\n", PORTNAME(ap), chipid, rev,
+	    SATA_PFMT_PM_REV, nports, features, SATA_PFMT_PM_FEA, enabled,
+	    SATA_PFMT_PM_FEA);
+
+	*ret_nports = nports;
+	return (0);
+}