6759202 ata: S3 suspend to ram hangs in pci_save_config_regs()

6759210 uhci: suspend to ram on Toshiba Tecra S1 hangs in uhci_cpr_suspend()
6759217 uppc: state of legacy pic must be saved / restored to support suspend to ram
6761267 pci interrupt routing entries not restored after wakeup from S3 sleep
6759220 ipw2100: needs DDI_SUSPEND / DDI_RESUME support, for suspend to ram
6761274 acpi-crs property not freed after wakeup from s3 suspend to ram
6766845 Tecra S1: ata0 "timeout: abort request" errors for PATA HDD after S3 resume
Contributed by Juergen Keil <jk@tools.de>.

10 files changed, 352 insertions, 55 deletions

diff --git a/usr/src/uts/common/io/ipw/ipw2100.c b/usr/src/uts/common/io/ipw/ipw2100.c
index a5feab7846..e882cae4b1 100644
--- a/usr/src/uts/common/io/ipw/ipw2100.c
+++ b/usr/src/uts/common/io/ipw/ipw2100.c
@@ -1,5 +1,5 @@
 /*
- * Copyright 2008 Sun Microsystems, Inc.  All rights reserved.
+ * Copyright 2009 Sun Microsystems, Inc.  All rights reserved.
  * Use is subject to license terms.
  */
 
@@ -172,6 +172,12 @@ static int	ipw_wificfg_disassoc(struct ipw2100_softc *sc,
     wldp_t *outfp);
 
 /*
+ * Suspend / Resume operations
+ */
+static int	ipw2100_cpr_suspend(struct ipw2100_softc *sc);
+static int	ipw2100_cpr_resume(struct ipw2100_softc *sc);
+
+/*
  * Mac Call Back entries
  */
 mac_callbacks_t	ipw2100_m_callbacks = {
@@ -254,7 +260,17 @@ ipw2100_attach(dev_info_t *dip, ddi_attach_cmd_t cmd)
 	wifi_data_t		wd = { 0 };
 	mac_register_t		*macp;
 
-	if (cmd != DDI_ATTACH) {
+	switch (cmd) {
+	case DDI_ATTACH:
+		break;
+	case DDI_RESUME:
+		sc = ddi_get_soft_state(ipw2100_ssp, ddi_get_instance(dip));
+		if (sc == NULL) {
+			err = DDI_FAILURE;
+			goto fail1;
+		}
+		return (ipw2100_cpr_resume(sc));
+	default:
 		err = DDI_FAILURE;
 		goto fail1;
 	}
@@ -508,8 +524,14 @@ ipw2100_detach(dev_info_t *dip, ddi_detach_cmd_t cmd)
 
 	ASSERT(sc != NULL);
 
-	if (cmd != DDI_DETACH)
+	switch (cmd) {
+	case DDI_DETACH:
+		break;
+	case DDI_SUSPEND:
+		return (ipw2100_cpr_suspend(sc));
+	default:
 		return (DDI_FAILURE);
+	}
 
 	/*
 	 * Destroy the mf_thread
@@ -523,7 +545,7 @@ ipw2100_detach(dev_info_t *dip, ddi_detach_cmd_t cmd)
 	mutex_exit(&sc->sc_mflock);
 
 	/*
-	 * Unregiste from the MAC layer subsystem
+	 * Unregister from the MAC layer subsystem
 	 */
 	err = mac_unregister(sc->sc_ic.ic_mach);
 	if (err != DDI_SUCCESS)
@@ -558,6 +580,103 @@ ipw2100_detach(dev_info_t *dip, ddi_detach_cmd_t cmd)
 	return (DDI_SUCCESS);
 }
 
+int
+ipw2100_cpr_suspend(struct ipw2100_softc *sc)
+{
+	IPW2100_DBG(IPW2100_DBG_INIT, (sc->sc_dip, CE_CONT,
+	    "ipw2100_cpr_suspend(): enter\n"));
+
+	/*
+	 * Destroy the mf_thread
+	 */
+	mutex_enter(&sc->sc_mflock);
+	sc->sc_mfthread_switch = 0;
+	while (sc->sc_mf_thread != NULL) {
+		if (cv_wait_sig(&sc->sc_mfthread_cv, &sc->sc_mflock) == 0)
+			break;
+	}
+	mutex_exit(&sc->sc_mflock);
+
+	/*
+	 * stop the hardware; this mask all interrupts
+	 */
+	ipw2100_stop(sc);
+	sc->sc_flags &= ~IPW2100_FLAG_RUNNING;
+	sc->sc_suspended = 1;
+
+	(void) ipw2100_free_firmware(sc);
+	ipw2100_ring_free(sc);
+
+	return (DDI_SUCCESS);
+}
+
+int
+ipw2100_cpr_resume(struct ipw2100_softc *sc)
+{
+	struct ieee80211com	*ic = &sc->sc_ic;
+	dev_info_t		*dip = sc->sc_dip;
+	int			err;
+
+	IPW2100_DBG(IPW2100_DBG_INIT, (sc->sc_dip, CE_CONT,
+	    "ipw2100_cpr_resume(): enter\n"));
+
+	/*
+	 * Reset the chip
+	 */
+	err = ipw2100_chip_reset(sc);
+	if (err != DDI_SUCCESS) {
+		IPW2100_WARN((dip, CE_WARN,
+		    "ipw2100_attach(): reset failed\n"));
+		return (DDI_FAILURE);
+	}
+
+	/*
+	 * Get the hw conf, including MAC address, then init all rings.
+	 */
+	/* ipw2100_hwconf_get(sc); */
+	err = ipw2100_ring_init(sc);
+	if (err != DDI_SUCCESS) {
+		IPW2100_WARN((dip, CE_WARN,
+		    "ipw2100_attach(): "
+		    "unable to allocate and initialize rings\n"));
+		return (DDI_FAILURE);
+	}
+
+	/*
+	 * Cache firmware, always return true
+	 */
+	(void) ipw2100_cache_firmware(sc);
+
+	/*
+	 * Notify link is down now
+	 */
+	mac_link_update(ic->ic_mach, LINK_STATE_DOWN);
+
+	/*
+	 * create the mf thread to handle the link status,
+	 * recovery fatal error, etc.
+	 */
+	sc->sc_mfthread_switch = 1;
+	if (sc->sc_mf_thread == NULL)
+		sc->sc_mf_thread = thread_create((caddr_t)NULL, 0,
+		    ipw2100_thread, sc, 0, &p0, TS_RUN, minclsyspri);
+
+	/*
+	 * enable all interrupts
+	 */
+	sc->sc_suspended = 0;
+	ipw2100_csr_put32(sc, IPW2100_CSR_INTR_MASK, IPW2100_INTR_MASK_ALL);
+
+	/*
+	 * initialize ipw2100 hardware
+	 */
+	(void) ipw2100_init(sc);
+
+	sc->sc_flags |= IPW2100_FLAG_RUNNING;
+
+	return (DDI_SUCCESS);
+}
+
 /*
  * quiesce(9E) entry point.
  * This function is called when the system is single-threaded at high
@@ -977,6 +1096,9 @@ ipw2100_init(struct ipw2100_softc *sc)
 {
 	int	err;
 
+	IPW2100_DBG(IPW2100_DBG_INIT, (sc->sc_dip, CE_CONT,
+	    "ipw2100_init(): enter\n"));
+
 	/*
 	 * no firmware is available, return fail directly
 	 */
@@ -998,6 +1120,8 @@ ipw2100_init(struct ipw2100_softc *sc)
 	/*
 	 * load microcode
 	 */
+	IPW2100_DBG(IPW2100_DBG_INIT, (sc->sc_dip, CE_CONT,
+	    "ipw2100_init(): loading microcode\n"));
 	err = ipw2100_load_uc(sc);
 	if (err != DDI_SUCCESS) {
 		IPW2100_WARN((sc->sc_dip, CE_WARN,
@@ -1012,6 +1136,8 @@ ipw2100_init(struct ipw2100_softc *sc)
 	/*
 	 * load firmware
 	 */
+	IPW2100_DBG(IPW2100_DBG_INIT, (sc->sc_dip, CE_CONT,
+	    "ipw2100_init(): loading firmware\n"));
 	err = ipw2100_load_fw(sc);
 	if (err != DDI_SUCCESS) {
 		IPW2100_WARN((sc->sc_dip, CE_WARN,
@@ -2537,6 +2663,9 @@ ipw2100_intr(caddr_t arg)
 	struct ipw2100_bd *rxbd;
 #endif
 
+	if (sc->sc_suspended)
+		return (DDI_INTR_UNCLAIMED);
+
 	ireg = ipw2100_csr_get32(sc, IPW2100_CSR_INTR);
 
 	if (!(ireg & IPW2100_INTR_MASK_ALL))
diff --git a/usr/src/uts/common/io/ipw/ipw2100_impl.h b/usr/src/uts/common/io/ipw/ipw2100_impl.h
index f0de7961d5..7207468323 100644
--- a/usr/src/uts/common/io/ipw/ipw2100_impl.h
+++ b/usr/src/uts/common/io/ipw/ipw2100_impl.h
@@ -1,5 +1,5 @@
 /*
- * Copyright 2008 Sun Microsystems, Inc.  All rights reserved.
+ * Copyright 2009 Sun Microsystems, Inc.  All rights reserved.
  * Use is subject to license terms.
  */
 
@@ -99,6 +99,7 @@ struct ipw2100_softc {
 	int	(*sc_newstate)(struct ieee80211com *,
 	    enum ieee80211_state, int);
 	int			sc_authmode;
+	int			sc_suspended;
 	/* CSR */
 	ddi_acc_handle_t	sc_ioh;
 	caddr_t			sc_regs;
diff --git a/usr/src/uts/common/io/pci-ide/pci-ide.c b/usr/src/uts/common/io/pci-ide/pci-ide.c
index 7156aaa091..87f228c170 100644
--- a/usr/src/uts/common/io/pci-ide/pci-ide.c
+++ b/usr/src/uts/common/io/pci-ide/pci-ide.c
@@ -19,7 +19,7 @@
  * CDDL HEADER END
  */
 /*
- * Copyright 2008 Sun Microsystems, Inc.  All rights reserved.
+ * Copyright 2009 Sun Microsystems, Inc.  All rights reserved.
  * Use is subject to license terms.
  */
 
@@ -381,6 +381,13 @@ pciide_ddi_ctlops(dev_info_t *dip, dev_info_t *rdip, ddi_ctl_enum_t ctlop,
 
 		return (rc);
 
+	case DDI_CTLOPS_ATTACH:
+	case DDI_CTLOPS_DETACH:
+		/*
+		 * Don't pass child ide ATTACH/DETACH to parent
+		 */
+		return (DDI_SUCCESS);
+
 	default:
 		return (ddi_ctlops(dip, rdip, ctlop, arg, result));
 	}
diff --git a/usr/src/uts/common/io/usb/hcd/uhci/uhci.c b/usr/src/uts/common/io/usb/hcd/uhci/uhci.c
index 4da8376838..0dfca00755 100644
--- a/usr/src/uts/common/io/usb/hcd/uhci/uhci.c
+++ b/usr/src/uts/common/io/usb/hcd/uhci/uhci.c
@@ -19,7 +19,7 @@
  * CDDL HEADER END
  */
 /*
- * Copyright 2008 Sun Microsystems, Inc.  All rights reserved.
+ * Copyright 2009 Sun Microsystems, Inc.  All rights reserved.
  * Use is subject to license terms.
  */
 
@@ -850,6 +850,9 @@ uhci_cleanup(uhci_state_t *uhcip)
 static int
 uhci_cpr_suspend(uhci_state_t	*uhcip)
 {
+	uint16_t	cmd_reg;
+	int		i;
+
 	USB_DPRINTF_L4(PRINT_MASK_ATTA, uhcip->uhci_log_hdl,
 	    "uhci_cpr_suspend:");
 
@@ -861,20 +864,36 @@ uhci_cpr_suspend(uhci_state_t	*uhcip)
 
 	mutex_enter(&uhcip->uhci_int_mutex);
 
-	/* Disable interrupts */
-	Set_OpReg16(USBINTR, DISABLE_ALL_INTRS);
+	/* Stop the Host Controller */
+	cmd_reg = Get_OpReg16(USBCMD);
+	cmd_reg &= ~USBCMD_REG_HC_RUN;
+	Set_OpReg16(USBCMD, cmd_reg);
 
-	mutex_exit(&uhcip->uhci_int_mutex);
+	/*
+	 * Wait for the duration of an SOF period until the host controller
+	 * reaches the stopped state, indicated by the HCHalted bit in the
+	 * USB status register.
+	 */
+	for (i = 0; i <= UHCI_TIMEWAIT / 1000; i++) {
+		if (Get_OpReg16(USBSTS) & USBSTS_REG_HC_HALTED)
+			break;
+		drv_usecwait(1000);
+	}
 
-	/* Wait for SOF time to handle the scheduled interrupt */
-	delay(drv_usectohz(UHCI_TIMEWAIT));
+	USB_DPRINTF_L3(PRINT_MASK_ATTA, uhcip->uhci_log_hdl,
+	    "uhci_cpr_suspend: waited %d milliseconds for hc to halt", i);
 
-	mutex_enter(&uhcip->uhci_int_mutex);
-	/* Stop the Host Controller */
-	Set_OpReg16(USBCMD, 0);
+	/* Disable interrupts */
+	Set_OpReg16(USBINTR, DISABLE_ALL_INTRS);
+
+	/* Clear any scheduled pending interrupts */
+	Set_OpReg16(USBSTS, USBSTS_REG_HC_HALTED |
+	    USBSTS_REG_HC_PROCESS_ERR | USBSTS_REG_HOST_SYS_ERR |
+	    USBSTS_REG_RESUME_DETECT | USBSTS_REG_USB_ERR_INTR |
+	    USBSTS_REG_USB_INTR);
 
 	/* Set Global Suspend bit */
-	Set_OpReg16(USBCMD, USBCMD_REG_ENER_GBL_SUSPEND);
+	Set_OpReg16(USBCMD, USBCMD_REG_ENTER_GBL_SUSPEND);
 
 	/* Set host controller soft state to suspend */
 	uhcip->uhci_hc_soft_state = UHCI_CTLR_SUSPEND_STATE;
diff --git a/usr/src/uts/common/sys/usb/hcd/uhci/uhci.h b/usr/src/uts/common/sys/usb/hcd/uhci/uhci.h
index 0b547ff4e9..a275e13bfe 100644
--- a/usr/src/uts/common/sys/usb/hcd/uhci/uhci.h
+++ b/usr/src/uts/common/sys/usb/hcd/uhci/uhci.h
@@ -19,7 +19,7 @@
  * CDDL HEADER END
  */
 /*
- * Copyright 2008 Sun Microsystems, Inc.  All rights reserved.
+ * Copyright 2009 Sun Microsystems, Inc.  All rights reserved.
  * Use is subject to license terms.
  */
 
@@ -68,7 +68,7 @@ typedef volatile struct hcr_regs {
 #define	USBCMD_REG_CONFIG_FLAG		0x0040
 #define	USBCMD_REG_SW_DEBUG		0x0020
 #define	USBCMD_REG_FGBL_RESUME		0x0010
-#define	USBCMD_REG_ENER_GBL_SUSPEND	0x0008
+#define	USBCMD_REG_ENTER_GBL_SUSPEND	0x0008
 #define	USBCMD_REG_GBL_RESET		0x0004
 #define	USBCMD_REG_HC_RESET		0x0002
 #define	USBCMD_REG_HC_RUN		0x0001
diff --git a/usr/src/uts/i86pc/io/psm/uppc.c b/usr/src/uts/i86pc/io/psm/uppc.c
index 2fd3f860c5..0b6faee267 100644
--- a/usr/src/uts/i86pc/io/psm/uppc.c
+++ b/usr/src/uts/i86pc/io/psm/uppc.c
@@ -19,12 +19,10 @@
  * CDDL HEADER END
  */
 /*
- * Copyright 2007 Sun Microsystems, Inc.  All rights reserved.
+ * Copyright 2009 Sun Microsystems, Inc.  All rights reserved.
  * Use is subject to license terms.
  */
 
-#pragma ident	"%Z%%M%	%I%	%E% SMI"
-
 #define	PSMI_1_6
 
 #include <sys/mutex.h>
@@ -36,6 +34,7 @@
 #include <sys/promif.h>
 #include <sys/psm.h>
 #include <sys/pit.h>
+#include <sys/apic.h>
 #include <sys/psm_common.h>
 #include <sys/atomic.h>
 #include <sys/archsystm.h>
@@ -47,6 +46,7 @@
  */
 static void uppc_softinit(void);
 static void uppc_picinit();
+static int uppc_post_cpu_start(void);
 static int uppc_clkinit(int);
 static int uppc_addspl(int irqno, int ipl, int min_ipl, int max_ipl);
 static int uppc_delspl(int irqno, int ipl, int min_ipl, int max_ipl);
@@ -56,6 +56,7 @@ static int uppc_probe(void);
 static int uppc_translate_irq(dev_info_t *dip, int irqno);
 static void uppc_shutdown(int cmd, int fcn);
 static void uppc_preshutdown(int cmd, int fcn);
+static int uppc_state(psm_state_request_t *request);
 static int uppc_init_acpi(void);
 static void uppc_setspl(int);
 static int uppc_intr_enter(int, int *);
@@ -154,7 +155,7 @@ static struct	psm_ops uppc_ops = {
 
 	uppc_get_next_processorid,		/* psm_get_next_processorid */
 	(int (*)(processorid_t, caddr_t))NULL,	/* psm_cpu_start	*/
-	(int (*)(void))NULL,			/* psm_post_cpu_start	*/
+	uppc_post_cpu_start,			/* psm_post_cpu_start	*/
 	uppc_shutdown,				/* psm_shutdown		*/
 	(int (*)(int, int))NULL,		/* psm_get_ipivect	*/
 	(void (*)(processorid_t, int))NULL,	/* psm_send_ipi		*/
@@ -170,12 +171,14 @@ static struct	psm_ops uppc_ops = {
 	uppc_preshutdown,			/* psm_preshutdown	*/
 
 	(int (*)(dev_info_t *, ddi_intr_handle_impl_t *,
-	    psm_intr_op_t, int *))NULL		/* psm_intr_ops		*/
+	    psm_intr_op_t, int *))NULL,		/* psm_intr_ops		*/
+
+	uppc_state				/* psm_state		*/
 };
 
 
 static struct	psm_info uppc_info = {
-	PSM_INFO_VER01_5,	/* version				*/
+	PSM_INFO_VER01_6,	/* version				*/
 	PSM_OWN_SYS_DEFAULT,	/* ownership				*/
 	(struct psm_ops *)&uppc_ops, /* operation			*/
 	"uppc",			/* machine name				*/
@@ -282,6 +285,27 @@ uppc_picinit()
 		(void) uppc_addspl(uppc_sci, SCI_IPL, SCI_IPL, SCI_IPL);
 }
 
+static int
+uppc_post_cpu_start(void)
+{
+	/*
+	 * On uppc machines psm_post_cpu_start is called during S3 resume
+	 * on the boot cpu from assembly, using the ap_mlsetup vector.
+	 */
+
+	/*
+	 * Init master and slave pic
+	 */
+	picsetup();
+
+	/*
+	 * program timer 0
+	 */
+	(void) uppc_clkinit(hz);
+
+	return (PSM_SUCCESS);
+}
+
 /*ARGSUSED3*/
 static int
 uppc_addspl(int irqno, int ipl, int min_ipl, int max_ipl)
@@ -486,6 +510,112 @@ uppc_shutdown(int cmd, int fcn)
 	(void) acpi_poweroff();
 }
 
+
+static int
+uppc_acpi_enter_picmode(void)
+{
+	ACPI_OBJECT_LIST	arglist;
+	ACPI_OBJECT		arg;
+	ACPI_STATUS		status;
+
+	/* Setup parameter object */
+	arglist.Count = 1;
+	arglist.Pointer = &arg;
+	arg.Type = ACPI_TYPE_INTEGER;
+	arg.Integer.Value = ACPI_PIC_MODE;
+
+	status = AcpiEvaluateObject(NULL, "\\_PIC", &arglist, NULL);
+	if (ACPI_FAILURE(status))
+		return (PSM_FAILURE);
+	else
+		return (PSM_SUCCESS);
+}
+
+
+struct pic_state {
+	int8_t		mmask;
+	int8_t		smask;
+	uint16_t	elcr;
+};
+
+
+static void
+pic_save_state(struct pic_state *sp)
+{
+	struct standard_pic *pp;
+	int	vecno;
+
+	/*
+	 * Only the PIC masks and the ELCR can be saved;
+	 * other 8259 state is write-only
+	 */
+
+	/*
+	 * save current master and slave interrupt mask
+	 */
+	pp = &pics0;
+	sp->smask = pp->c_curmask[0];
+	sp->mmask = pp->c_curmask[1];
+
+	/*
+	 * save edge/level configuration for isa interrupts
+	 */
+	sp->elcr = 0;
+	for (vecno = 0; vecno <= MAX_ISA_IRQ; vecno++)
+		sp->elcr |= psm_get_elcr(vecno) << vecno;
+}
+
+static void
+pic_restore_state(struct pic_state *sp)
+{
+	int	vecno;
+
+	/* Restore master and slave interrupt masks */
+	outb(SIMR_PORT, sp->smask);
+	outb(MIMR_PORT, sp->mmask);
+
+	/* Read master to allow pics to settle */
+	(void) inb(MIMR_PORT);
+
+	/* Restore edge/level configuration for isa interupts */
+	for (vecno = 0; vecno <= MAX_ISA_IRQ; vecno++)
+		psm_set_elcr(vecno, sp->elcr & (1 << vecno));
+
+	/* Reenter PIC mode before restoring LNK devices */
+	(void) uppc_acpi_enter_picmode();
+
+	/* Restore ACPI link device mappings */
+	acpi_restore_link_devices();
+}
+
+static int
+uppc_state(psm_state_request_t *rp)
+{
+	switch (rp->psr_cmd) {
+	case PSM_STATE_ALLOC:
+		rp->req.psm_state_req.psr_state =
+		    kmem_zalloc(sizeof (struct pic_state), KM_NOSLEEP);
+		if (rp->req.psm_state_req.psr_state == NULL)
+			return (ENOMEM);
+		rp->req.psm_state_req.psr_state_size =
+		    sizeof (struct pic_state);
+		return (0);
+	case PSM_STATE_FREE:
+		kmem_free(rp->req.psm_state_req.psr_state,
+		    rp->req.psm_state_req.psr_state_size);
+		return (0);
+	case PSM_STATE_SAVE:
+		pic_save_state(rp->req.psm_state_req.psr_state);
+		return (0);
+	case PSM_STATE_RESTORE:
+		pic_restore_state(rp->req.psm_state_req.psr_state);
+		return (0);
+	default:
+		return (EINVAL);
+	}
+}
+
+
 static int
 uppc_acpi_translate_pci_irq(dev_info_t *dip, int busid, int devid,
     int ipin, int *pci_irqp, iflag_t *intr_flagp)
diff --git a/usr/src/uts/intel/io/acpica/acpica.c b/usr/src/uts/intel/io/acpica/acpica.c
index 25b6f1dab7..283e06a164 100644
--- a/usr/src/uts/intel/io/acpica/acpica.c
+++ b/usr/src/uts/intel/io/acpica/acpica.c
@@ -19,7 +19,7 @@
  * CDDL HEADER END
  */
 /*
- * Copyright 2008 Sun Microsystems, Inc.  All rights reserved.
+ * Copyright 2009 Sun Microsystems, Inc.  All rights reserved.
  * Use is subject to license terms.
  */
 /*
@@ -697,5 +697,5 @@ acpica_ddi_restore_resources(dev_info_t *dip)
 	resbuf.Length = proplen;
 	(void) AcpiSetCurrentResources(devobj, &resbuf);
 	ddi_prop_free(propdata);
-	(void) ddi_prop_remove(DDI_DEV_T_ANY, dip, "acpi-crs");
+	(void) ddi_prop_remove(DDI_DEV_T_NONE, dip, "acpi-crs");
 }
diff --git a/usr/src/uts/intel/io/dktp/controller/ata/ata_common.c b/usr/src/uts/intel/io/dktp/controller/ata/ata_common.c
index 6de3774329..b18f3a12bf 100644
--- a/usr/src/uts/intel/io/dktp/controller/ata/ata_common.c
+++ b/usr/src/uts/intel/io/dktp/controller/ata/ata_common.c
@@ -20,7 +20,7 @@
  */
 
 /*
- * Copyright 2008 Sun Microsystems, Inc.  All rights reserved.
+ * Copyright 2009 Sun Microsystems, Inc.  All rights reserved.
  * Use is subject to license terms.
  */
 
@@ -3551,12 +3551,12 @@ ata_resume_drive(ata_drv_t *ata_drvp)
 
 	if (!ATAPIDRV(ata_drvp)) {
 		/* Reset Ultra DMA mode */
-		(void) ata_set_dma_mode(ata_ctlp, ata_drvp);
+		ata_reset_dma_mode(ata_drvp);
 		if (!ata_disk_setup_parms(ata_ctlp, ata_drvp))
 			return;
 	} else {
 		(void) atapi_init_drive(ata_drvp);
-		(void) atapi_reset_dma_mode(ata_drvp);
+		atapi_reset_dma_mode(ata_drvp);
 	}
 	(void) ata_set_feature(ata_ctlp, ata_drvp, ATSF_DIS_REVPOD, 0);
 
@@ -3858,3 +3858,36 @@ ata_set_dma_mode(ata_ctl_t *ata_ctlp, ata_drv_t *ata_drvp)
 
 	return (rval);
 }
+
+/*
+ * Reset Ultra DMA mode / MWDMA mode
+ */
+void
+ata_reset_dma_mode(ata_drv_t *ata_drvp)
+{
+	uint8_t	subcmd;
+	int	mode;
+	ata_ctl_t *ata_ctlp = ata_drvp->ad_ctlp;
+
+	switch (ata_drvp->ad_dma_cap) {
+	case ATA_DMA_ULTRAMODE:
+		subcmd = ATF_XFRMOD_UDMA;
+		for (mode = 0; mode <= 6; mode++) {
+			if (ata_drvp->ad_dma_mode & (1 << (mode + 8)))
+				break;
+		}
+		break;
+	case ATA_DMA_MWORDMODE:
+		subcmd = ATF_XFRMOD_MDMA;
+		mode = ((ata_drvp->ad_dma_mode & ATAC_MDMA_2_SEL) ==
+		    ATAC_MDMA_2_SEL ? 2 :
+		    (ata_drvp->ad_dma_mode & ATAC_MDMA_1_SEL) ==
+		    ATAC_MDMA_1_SEL ? 1 : 0);
+		break;
+	default:
+		return;
+	}
+
+	(void) ata_set_feature(ata_ctlp, ata_drvp, ATSF_SET_XFRMOD,
+	    (subcmd | mode));
+}
diff --git a/usr/src/uts/intel/io/dktp/controller/ata/ata_common.h b/usr/src/uts/intel/io/dktp/controller/ata/ata_common.h
index 79617958e7..2df68e08bf 100644
--- a/usr/src/uts/intel/io/dktp/controller/ata/ata_common.h
+++ b/usr/src/uts/intel/io/dktp/controller/ata/ata_common.h
@@ -20,7 +20,7 @@
  */
 
 /*
- * Copyright 2008 Sun Microsystems, Inc.  All rights reserved.
+ * Copyright 2009 Sun Microsystems, Inc.  All rights reserved.
  * Use is subject to license terms.
  */
 
@@ -684,6 +684,7 @@ int	ata_wait3(ddi_acc_handle_t io_hdl, caddr_t ioaddr, uchar_t onbits1,
 int	ata_test_lba_support(struct ata_id *aidp);
 void	ata_nsecwait(clock_t count);
 int	ata_set_dma_mode(ata_ctl_t *ata_ctlp, ata_drv_t *ata_drvp);
+void	ata_reset_dma_mode(ata_drv_t *ata_drvp);
 void	atapi_reset_dma_mode(ata_drv_t *ata_drvp);
 
 
diff --git a/usr/src/uts/intel/io/dktp/controller/ata/atapi.c b/usr/src/uts/intel/io/dktp/controller/ata/atapi.c
index d5a2b7d5c8..1715c516e0 100644
--- a/usr/src/uts/intel/io/dktp/controller/ata/atapi.c
+++ b/usr/src/uts/intel/io/dktp/controller/ata/atapi.c
@@ -20,7 +20,7 @@
  */
 
 /*
- * Copyright 2008 Sun Microsystems, Inc.  All rights reserved.
+ * Copyright 2009 Sun Microsystems, Inc.  All rights reserved.
  * Use is subject to license terms.
  */
 
@@ -1208,31 +1208,8 @@ atapi_init_arq(
 void
 atapi_reset_dma_mode(ata_drv_t *ata_drvp)
 {
-	uint8_t	subcmd;
-	int	mode;
 	ata_ctl_t *ata_ctlp = ata_drvp->ad_ctlp;
 
-	switch (ata_drvp->ad_dma_cap) {
-	case ATA_DMA_ULTRAMODE:
-		subcmd = ATF_XFRMOD_UDMA;
-		for (mode = 0; mode <= 6; mode++) {
-			if (ata_drvp->ad_dma_mode & (1 << (mode + 8)))
-				break;
-		}
-		break;
-	case ATA_DMA_MWORDMODE:
-		subcmd = ATF_XFRMOD_MDMA;
-		mode = ((ata_drvp->ad_dma_mode & ATAC_MDMA_2_SEL) ==
-		    ATAC_MDMA_2_SEL ? 2 :
-		    (ata_drvp->ad_dma_mode & ATAC_MDMA_1_SEL) ==
-		    ATAC_MDMA_1_SEL ? 1 : 0);
-		break;
-	default:
-		return;
-	}
-
-	(void) ata_set_feature(ata_ctlp, ata_drvp, ATSF_SET_XFRMOD,
-	    (subcmd | mode));
-
+	ata_reset_dma_mode(ata_drvp);
 	(void) atapi_id_update(ata_ctlp, ata_drvp, NULL);
 }