3 files changed, 163 insertions, 3 deletions

diff --git a/sys/cam/ata/ata_pmp.c b/sys/cam/ata/ata_pmp.c
index b60a4a7596b1..0835a2bf228f 100644
--- a/sys/cam/ata/ata_pmp.c
+++ b/sys/cam/ata/ata_pmp.c
@@ -101,6 +101,7 @@ struct pmp_softc {
 	int			events;
 #define PMP_EV_RESET	1
 #define PMP_EV_RESCAN	2
+	u_int			caps;
 	struct task		sysctl_task;
 	struct sysctl_ctx_list	sysctl_ctx;
 	struct sysctl_oid	*sysctl_tree;
@@ -457,6 +458,14 @@ pmpstart(struct cam_periph *periph, union ccb *start_ccb)
 		ata_pm_read_cmd(ataio, 2, 15);
 		break;
 	case PMP_STATE_PRECONFIG:
+		/* Get/update host SATA capabilities. */
+		bzero(&cts, sizeof(cts));
+		xpt_setup_ccb(&cts.ccb_h, periph->path, CAM_PRIORITY_NONE);
+		cts.ccb_h.func_code = XPT_GET_TRAN_SETTINGS;
+		cts.type = CTS_TYPE_CURRENT_SETTINGS;
+		xpt_action((union ccb *)&cts);
+		if (cts.xport_specific.sata.valid & CTS_SATA_VALID_CAPS)
+			softc->caps = cts.xport_specific.sata.caps;
 		cam_fill_ataio(ataio,
 		      pmp_retry_count,
 		      pmpdone,
@@ -644,14 +653,16 @@ pmpdone(struct cam_periph *periph, union ccb *done_ccb)
 		    (done_ccb->ataio.res.lba_mid << 16) +
 		    (done_ccb->ataio.res.lba_low << 8) +
 		    done_ccb->ataio.res.sector_count;
-		if ((res & 0xf0f) == 0x103 && (res & 0x0f0) != 0) {
+		if (((res & 0xf0f) == 0x103 && (res & 0x0f0) != 0) ||
+		    (res & 0x600) != 0) {
 			if (bootverbose) {
 				printf("%s%d: port %d status: %08x\n",
 				    periph->periph_name, periph->unit_number,
 				    softc->pm_step, res);
 			}
-			/* Report device speed. */
-			if (xpt_create_path(&dpath, periph,
+			/* Report device speed if it is online. */
+			if ((res & 0xf0f) == 0x103 &&
+			    xpt_create_path(&dpath, periph,
 			    xpt_path_path_id(periph->path),
 			    softc->pm_step, 0) == CAM_REQ_CMP) {
 				bzero(&cts, sizeof(cts));
@@ -660,6 +671,9 @@ pmpdone(struct cam_periph *periph, union ccb *done_ccb)
 				cts.type = CTS_TYPE_CURRENT_SETTINGS;
 				cts.xport_specific.sata.revision = (res & 0x0f0) >> 4;
 				cts.xport_specific.sata.valid = CTS_SATA_VALID_REVISION;
+				cts.xport_specific.sata.caps = softc->caps &
+				    (CTS_SATA_CAPS_H_PMREQ | CTS_SATA_CAPS_H_DMAAA);
+				cts.xport_specific.sata.valid |= CTS_SATA_VALID_CAPS;
 				xpt_action((union ccb *)&cts);
 				xpt_free_path(dpath);
 			}
diff --git a/sys/cam/ata/ata_xpt.c b/sys/cam/ata/ata_xpt.c
index bd748cd4f453..478b0060ce45 100644
--- a/sys/cam/ata/ata_xpt.c
+++ b/sys/cam/ata/ata_xpt.c
@@ -88,6 +88,9 @@ typedef enum {
 	PROBE_IDENTIFY,
 	PROBE_SPINUP,
 	PROBE_SETMODE,
+	PROBE_SETPM,
+	PROBE_SETAPST,
+	PROBE_SETDMAAA,
 	PROBE_SET_MULTI,
 	PROBE_INQUIRY,
 	PROBE_FULL_INQUIRY,
@@ -101,6 +104,9 @@ static char *probe_action_text[] = {
 	"PROBE_IDENTIFY",
 	"PROBE_SPINUP",
 	"PROBE_SETMODE",
+	"PROBE_SETPM",
+	"PROBE_SETAPST",
+	"PROBE_SETDMAAA",
 	"PROBE_SET_MULTI",
 	"PROBE_INQUIRY",
 	"PROBE_FULL_INQUIRY",
@@ -132,6 +138,7 @@ typedef struct {
 	uint32_t	pm_prv;
 	int		restart;
 	int		spinup;
+	u_int		caps;
 	struct cam_periph *periph;
 } probe_softc;
 
@@ -393,6 +400,45 @@ negotiate:
 		ata_28bit_cmd(ataio, ATA_SETFEATURES, ATA_SF_SETXFER, 0, mode);
 		break;
 	}
+	case PROBE_SETPM:
+		cam_fill_ataio(ataio,
+		    1,
+		    probedone,
+		    CAM_DIR_NONE,
+		    0,
+		    NULL,
+		    0,
+		    30*1000);
+		ata_28bit_cmd(ataio, ATA_SETFEATURES,
+		    (softc->caps & CTS_SATA_CAPS_H_PMREQ) ? 0x10 : 0x90,
+		    0, 0x03);
+		break;
+	case PROBE_SETAPST:
+		cam_fill_ataio(ataio,
+		    1,
+		    probedone,
+		    CAM_DIR_NONE,
+		    0,
+		    NULL,
+		    0,
+		    30*1000);
+		ata_28bit_cmd(ataio, ATA_SETFEATURES,
+		    (softc->caps & CTS_SATA_CAPS_H_APST) ? 0x10 : 0x90,
+		    0, 0x07);
+		break;
+	case PROBE_SETDMAAA:
+		cam_fill_ataio(ataio,
+		    1,
+		    probedone,
+		    CAM_DIR_NONE,
+		    0,
+		    NULL,
+		    0,
+		    30*1000);
+		ata_28bit_cmd(ataio, ATA_SETFEATURES,
+		    (softc->caps & CTS_SATA_CAPS_H_DMAAA) ? 0x10 : 0x90,
+		    0, 0x02);
+		break;
 	case PROBE_SET_MULTI:
 	{
 		u_int sectors, bytecount;
@@ -685,6 +731,7 @@ probedone(struct cam_periph *periph, union ccb *done_ccb)
 	probe_softc *softc;
 	struct cam_path *path;
 	u_int32_t  priority;
+	u_int caps;
 	int found = 1;
 
 	CAM_DEBUG(done_ccb->ccb_h.path, CAM_DEBUG_TRACE, ("probedone\n"));
@@ -879,6 +926,67 @@ noerror:
 		xpt_schedule(periph, priority);
 		return;
 	case PROBE_SETMODE:
+		if (path->device->transport != XPORT_SATA)
+			goto notsata;
+		/* Set supported bits. */
+		bzero(&cts, sizeof(cts));
+		xpt_setup_ccb(&cts.ccb_h, path, CAM_PRIORITY_NONE);
+		cts.ccb_h.func_code = XPT_GET_TRAN_SETTINGS;
+		cts.type = CTS_TYPE_CURRENT_SETTINGS;
+		xpt_action((union ccb *)&cts);
+		if (cts.xport_specific.sata.valid & CTS_SATA_VALID_CAPS)
+			caps = cts.xport_specific.sata.caps & CTS_SATA_CAPS_H;
+		else
+			caps = 0;
+		if (ident_buf->satacapabilities != 0xffff) {
+			if (ident_buf->satacapabilities & ATA_SUPPORT_IFPWRMNGTRCV)
+				caps |= CTS_SATA_CAPS_D_PMREQ;
+			if (ident_buf->satacapabilities & ATA_SUPPORT_HAPST)
+				caps |= CTS_SATA_CAPS_D_APST;
+		}
+		/* Mask unwanted bits. */
+		bzero(&cts, sizeof(cts));
+		xpt_setup_ccb(&cts.ccb_h, path, CAM_PRIORITY_NONE);
+		cts.ccb_h.func_code = XPT_GET_TRAN_SETTINGS;
+		cts.type = CTS_TYPE_USER_SETTINGS;
+		xpt_action((union ccb *)&cts);
+		if (cts.xport_specific.sata.valid & CTS_SATA_VALID_CAPS)
+			caps &= cts.xport_specific.sata.caps;
+		/* Store result to SIM. */
+		bzero(&cts, sizeof(cts));
+		xpt_setup_ccb(&cts.ccb_h, path, CAM_PRIORITY_NONE);
+		cts.ccb_h.func_code = XPT_SET_TRAN_SETTINGS;
+		cts.type = CTS_TYPE_CURRENT_SETTINGS;
+		cts.xport_specific.sata.caps = caps;
+		cts.xport_specific.sata.valid = CTS_SATA_VALID_CAPS;
+		xpt_action((union ccb *)&cts);
+		softc->caps = caps;
+		if (ident_buf->satasupport & ATA_SUPPORT_IFPWRMNGT) {
+			PROBE_SET_ACTION(softc, PROBE_SETPM);
+			xpt_release_ccb(done_ccb);
+			xpt_schedule(periph, priority);
+			return;
+		}
+		/* FALLTHROUGH */
+	case PROBE_SETPM:
+		if (ident_buf->satacapabilities != 0xffff &&
+		    ident_buf->satacapabilities & ATA_SUPPORT_DAPST) {
+			PROBE_SET_ACTION(softc, PROBE_SETAPST);
+			xpt_release_ccb(done_ccb);
+			xpt_schedule(periph, priority);
+			return;
+		}
+		/* FALLTHROUGH */
+	case PROBE_SETAPST:
+		if (ident_buf->satasupport & ATA_SUPPORT_AUTOACTIVATE) {
+			PROBE_SET_ACTION(softc, PROBE_SETDMAAA);
+			xpt_release_ccb(done_ccb);
+			xpt_schedule(periph, priority);
+			return;
+		}
+		/* FALLTHROUGH */
+	case PROBE_SETDMAAA:
+notsata:
 		if (path->device->protocol == PROTO_ATA) {
 			PROBE_SET_ACTION(softc, PROBE_SET_MULTI);
 		} else {
@@ -964,6 +1072,35 @@ noerror:
 		snprintf(ident_buf->revision, sizeof(ident_buf->revision),
 		    "%04x", softc->pm_prv);
 		path->device->flags |= CAM_DEV_IDENTIFY_DATA_VALID;
+		/* Set supported bits. */
+		bzero(&cts, sizeof(cts));
+		xpt_setup_ccb(&cts.ccb_h, path, CAM_PRIORITY_NONE);
+		cts.ccb_h.func_code = XPT_GET_TRAN_SETTINGS;
+		cts.type = CTS_TYPE_CURRENT_SETTINGS;
+		xpt_action((union ccb *)&cts);
+		if (cts.xport_specific.sata.valid & CTS_SATA_VALID_CAPS)
+			caps = cts.xport_specific.sata.caps & CTS_SATA_CAPS_H;
+		else
+			caps = 0;
+		/* All PMPs must support PM requests. */
+		caps |= CTS_SATA_CAPS_D_PMREQ;
+		/* Mask unwanted bits. */
+		bzero(&cts, sizeof(cts));
+		xpt_setup_ccb(&cts.ccb_h, path, CAM_PRIORITY_NONE);
+		cts.ccb_h.func_code = XPT_GET_TRAN_SETTINGS;
+		cts.type = CTS_TYPE_USER_SETTINGS;
+		xpt_action((union ccb *)&cts);
+		if (cts.xport_specific.sata.valid & CTS_SATA_VALID_CAPS)
+			caps &= cts.xport_specific.sata.caps;
+		/* Store result to SIM. */
+		bzero(&cts, sizeof(cts));
+		xpt_setup_ccb(&cts.ccb_h, path, CAM_PRIORITY_NONE);
+		cts.ccb_h.func_code = XPT_SET_TRAN_SETTINGS;
+		cts.type = CTS_TYPE_CURRENT_SETTINGS;
+		cts.xport_specific.sata.caps = caps;
+		cts.xport_specific.sata.valid = CTS_SATA_VALID_CAPS;
+		xpt_action((union ccb *)&cts);
+		softc->caps = caps;
 		if (periph->path->device->flags & CAM_DEV_UNCONFIGURED) {
 			path->device->flags &= ~CAM_DEV_UNCONFIGURED;
 			xpt_acquire_device(path->device);
diff --git a/sys/cam/cam_ccb.h b/sys/cam/cam_ccb.h
index 2810c545f71a..c5ac35e98462 100644
--- a/sys/cam/cam_ccb.h
+++ b/sys/cam/cam_ccb.h
@@ -837,12 +837,21 @@ struct ccb_trans_settings_sata {
 #define	CTS_SATA_VALID_PM		0x08
 #define	CTS_SATA_VALID_TAGS		0x10
 #define	CTS_SATA_VALID_ATAPI		0x20
+#define	CTS_SATA_VALID_CAPS		0x40
 	int		mode;		/* Legacy PATA mode */
 	u_int 		bytecount;	/* Length of PIO transaction */
 	int		revision;	/* SATA revision */
 	u_int 		pm_present;	/* PM is present (XPT->SIM) */
 	u_int 		tags;		/* Number of allowed tags */
 	u_int 		atapi;		/* Length of ATAPI CDB */
+	u_int 		caps;		/* Device and host SATA caps. */
+#define	CTS_SATA_CAPS_H			0x0000ffff
+#define	CTS_SATA_CAPS_H_PMREQ		0x00000001
+#define	CTS_SATA_CAPS_H_APST		0x00000002
+#define	CTS_SATA_CAPS_H_DMAAA		0x00000010 /* Auto-activation */
+#define	CTS_SATA_CAPS_D			0xffff0000
+#define	CTS_SATA_CAPS_D_PMREQ		0x00010000
+#define	CTS_SATA_CAPS_D_APST		0x00020000
 };
 
 /* Get/Set transfer rate/width/disconnection/tag queueing settings */