Index: uspace/drv/infrastructure/rootamdm37x/rootamdm37x.c
===================================================================
--- uspace/drv/infrastructure/rootamdm37x/rootamdm37x.c	(revision b3ab8f744761c03302f241aee1e8934acfb98dc0)
+++ uspace/drv/infrastructure/rootamdm37x/rootamdm37x.c	(revision f25f1e6a3f18bef9c31bcd8e9f545a514bb07220)
@@ -55,4 +55,187 @@
 
 typedef struct {
+	uhh_regs_t *uhh;
+	tll_regs_t *tll;
+	struct {
+		core_cm_regs_t *core;
+		clock_control_cm_regs_t *clocks;
+		usbhost_cm_regs_t *usbhost;
+	} cm;
+} amdm37x_t;
+
+static int amdm37x_hw_access_init(amdm37x_t *device)
+{
+	assert(device);
+	int ret = EOK;
+
+	ret = pio_enable((void*)USBHOST_CM_BASE_ADDRESS, USBHOST_CM_SIZE,
+	    (void**)&device->cm.usbhost);
+	if (ret != EOK)
+		return ret;
+
+	ret = pio_enable((void*)CORE_CM_BASE_ADDRESS, CORE_CM_SIZE,
+	    (void**)&device->cm.core);
+	if (ret != EOK)
+		return ret;
+
+	ret = pio_enable((void*)CLOCK_CONTROL_CM_BASE_ADDRESS,
+		    CLOCK_CONTROL_CM_SIZE, (void**)&device->cm.clocks);
+	if (ret != EOK)
+		return ret;
+
+	ret = pio_enable((void*)AMDM37x_USBTLL_BASE_ADDRESS,
+	    AMDM37x_USBTLL_SIZE, (void**)&device->tll);
+	if (ret != EOK)
+		return ret;
+
+	ret = pio_enable((void*)AMDM37x_UHH_BASE_ADDRESS,
+	    AMDM37x_UHH_SIZE, (void**)&device->uhh);
+	if (ret != EOK)
+		return ret;
+
+	return EOK;
+}
+
+static int usb_clocks(amdm37x_t *device, bool on)
+{
+	uint32_t reg;
+
+	/* Set DPLL3 and DPLL4 to automatic */
+	reg = device->cm.clocks->autoidle_pll;
+	reg &= ~(CLOCK_CONTROL_CM_AUTOIDLE_PLL_AUTO_CORE_DPLL_MASK <<
+	    CLOCK_CONTROL_CM_AUTOIDLE_PLL_AUTO_CORE_DPLL_SHIFT);
+	reg &= ~(CLOCK_CONTROL_CM_AUTOIDLE_PLL_AUTO_PERIPH_DPLL_MASK <<
+	    CLOCK_CONTROL_CM_AUTOIDLE_PLL_AUTO_PERIPH_DPLL_SHIFT);
+	reg |= (CLOCK_CONTROL_CM_AUTOIDLE_PLL_AUTO_CORE_DPLL_AUTOMATIC <<
+	    CLOCK_CONTROL_CM_AUTOIDLE_PLL_AUTO_CORE_DPLL_SHIFT);
+	reg |= (CLOCK_CONTROL_CM_AUTOIDLE_PLL_AUTO_PERIPH_DPLL_AUTOMATIC <<
+	    CLOCK_CONTROL_CM_AUTOIDLE_PLL_AUTO_PERIPH_DPLL_SHIFT);
+	device->cm.clocks->autoidle_pll = reg;
+
+	/* Set DPLL5 to automatic */
+	reg = device->cm.clocks->autoidle2_pll;
+	reg &= ~(CLOCK_CONTROL_CM_AUTOIDLE2_PLL_AUTO_PERIPH2_DPLL_MASK <<
+	    CLOCK_CONTROL_CM_AUTOIDLE2_PLL_AUTO_PERIPH2_DPLL_SHIFT);
+	reg |= (CLOCK_CONTROL_CM_AUTOIDLE2_PLL_AUTO_PERIPH2_DPLL_AUTOMATIC <<
+	    CLOCK_CONTROL_CM_AUTOIDLE2_PLL_AUTO_PERIPH2_DPLL_SHIFT);
+	device->cm.clocks->autoidle2_pll = reg;
+
+
+#ifdef DEBUG_CM
+	printf("DPLL5 could be on: %x %x.\n",
+	    device->cm.clocks->idlest_ckgen, device->cm.clocks->idlest2_ckgen);
+#endif
+
+	if (on) {
+		/* Enable interface and function clock for USB TLL */
+		device->cm.core->iclken3 |= CORE_CM_ICLKEN3_EN_USBTLL_FLAG;
+		device->cm.core->fclken3 |= CORE_CM_FCLKEN3_EN_USBTLL_FLAG;
+
+		/* Enable interface and function clock for USB hosts */
+		device->cm.usbhost->iclken |= USBHOST_CM_ICLKEN_EN_USBHOST;
+		device->cm.usbhost->fclken |= USBHOST_CM_FCLKEN_EN_USBHOST1_FLAG;
+		device->cm.usbhost->fclken |= USBHOST_CM_FCLKEN_EN_USBHOST2_FLAG;
+#ifdef DEBUG_CM
+	printf("DPLL5 (and everything else) should be on: %x %x.\n",
+	    device->cm.clocks->idlest_ckgen, device->cm.clocks->idlest2_ckgen);
+#endif
+	} else {
+		/* Disable interface and function clock for USB hosts */
+		device->cm.usbhost->fclken &= ~USBHOST_CM_FCLKEN_EN_USBHOST2_FLAG;
+		device->cm.usbhost->fclken &= ~USBHOST_CM_FCLKEN_EN_USBHOST1_FLAG;
+		device->cm.usbhost->iclken &= ~USBHOST_CM_ICLKEN_EN_USBHOST;
+
+		/* Disable interface and function clock for USB TLL */
+		device->cm.core->fclken3 &= ~CORE_CM_FCLKEN3_EN_USBTLL_FLAG;
+		device->cm.core->iclken3 &= ~CORE_CM_ICLKEN3_EN_USBTLL_FLAG;
+	}
+
+	return EOK;
+}
+
+/** Initialize USB TLL port connections.
+ *
+ * Different modes are on page 3312 of the Manual Figure 22-34.
+ * Select mode than can operate in FS/LS.
+ */
+static int usb_tll_init(amdm37x_t *device)
+{
+
+	/* Reset USB TLL */
+	device->tll->sysconfig |= TLL_SYSCONFIG_SOFTRESET_FLAG;
+	ddf_msg(LVL_DEBUG2, "Waiting for USB TLL reset");
+	while (!(device->tll->sysstatus & TLL_SYSSTATUS_RESET_DONE_FLAG));
+	ddf_msg(LVL_DEBUG, "USB TLL Reset done.");
+
+	{
+	/* Setup idle mode (smart idle) */
+	uint32_t sysc = device->tll->sysconfig;
+	sysc |= TLL_SYSCONFIG_CLOCKACTIVITY_FLAG | TLL_SYSCONFIG_AUTOIDLE_FLAG;
+	sysc = (sysc
+	    & ~(TLL_SYSCONFIG_SIDLE_MODE_MASK << TLL_SYSCONFIG_SIDLE_MODE_SHIFT)
+	    ) | (0x2 << TLL_SYSCONFIG_SIDLE_MODE_SHIFT);
+	device->tll->sysconfig = sysc;
+	ddf_msg(LVL_DEBUG2, "Set TLL->sysconfig (%p) to %x:%x.",
+	    &device->tll->sysconfig, device->tll->sysconfig, sysc);
+	}
+
+	{
+	/* Smart idle for UHH */
+	uint32_t sysc = device->uhh->sysconfig;
+	sysc |= UHH_SYSCONFIG_CLOCKACTIVITY_FLAG | UHH_SYSCONFIG_AUTOIDLE_FLAG;
+	sysc = (sysc
+	    & ~(UHH_SYSCONFIG_SIDLE_MODE_MASK << UHH_SYSCONFIG_SIDLE_MODE_SHIFT)
+	    ) | (0x2 << UHH_SYSCONFIG_SIDLE_MODE_SHIFT);
+	sysc = (sysc
+	    & ~(UHH_SYSCONFIG_MIDLE_MODE_MASK << UHH_SYSCONFIG_MIDLE_MODE_SHIFT)
+	    ) | (0x2 << UHH_SYSCONFIG_MIDLE_MODE_SHIFT);
+	ddf_msg(LVL_DEBUG2, "Set UHH->sysconfig (%p) to %x.",
+	    &device->uhh->sysconfig, device->uhh->sysconfig);
+	device->uhh->sysconfig = sysc;
+
+	/* All ports are connected on BBxM */
+	device->uhh->hostconfig |= (UHH_HOSTCONFIG_P1_CONNECT_STATUS_FLAG
+	    | UHH_HOSTCONFIG_P2_CONNECT_STATUS_FLAG
+	    | UHH_HOSTCONFIG_P3_CONNECT_STATUS_FLAG);
+
+	/* Set all ports to go through TLL(UTMI)
+	 * Direct connection can only work in HS mode */
+	device->uhh->hostconfig |= (UHH_HOSTCONFIG_P1_ULPI_BYPASS_FLAG
+	    | UHH_HOSTCONFIG_P2_ULPI_BYPASS_FLAG
+	    | UHH_HOSTCONFIG_P3_ULPI_BYPASS_FLAG);
+	ddf_msg(LVL_DEBUG2, "Set UHH->hostconfig (%p) to %x.",
+	    &device->uhh->hostconfig, device->uhh->hostconfig);
+	}
+
+	device->tll->shared_conf |= TLL_SHARED_CONF_FCLK_IS_ON_FLAG;
+	ddf_msg(LVL_DEBUG2, "Set shared conf port (%p) to %x.",
+	    &device->tll->shared_conf, device->tll->shared_conf);
+
+	for (unsigned i = 0; i < 3; ++i) {
+		uint32_t ch = device->tll->channel_conf[i];
+		/* Clear Channel mode and FSLS mode */
+		ch &= ~(TLL_CHANNEL_CONF_CHANMODE_MASK
+		    << TLL_CHANNEL_CONF_CHANMODE_SHIFT)
+		    & ~(TLL_CHANNEL_CONF_FSLSMODE_MASK
+		    << TLL_CHANNEL_CONF_FSLSMODE_SHIFT);
+
+		/* Serial mode is the only one capable of FS/LS operation. */
+		ch |= (TLL_CHANNEL_CONF_CHANMODE_UTMI_SERIAL_MODE
+		    << TLL_CHANNEL_CONF_CHANMODE_SHIFT);
+
+		/* Select FS/LS mode, no idea what the difference is
+		 * one of bidirectional modes might be good choice
+		 * 2 = 3pin bidi phy. */
+		ch |= (2 << TLL_CHANNEL_CONF_FSLSMODE_SHIFT);
+
+		/* Write to register */
+		ddf_msg(LVL_DEBUG2, "Setting port %u(%p) to %x.",
+		    i, &device->tll->channel_conf[i], ch);
+		device->tll->channel_conf[i] = ch;
+	}
+	return EOK;
+}
+
+typedef struct {
 	hw_resource_list_t hw_resources;
 } rootamdm37x_fun_t;
@@ -105,5 +288,5 @@
 	.hw_resources = {
 	    .resources = ehci_res,
-	    .count = sizeof(ehci_res)/sizeof(ehci_res[0]),
+	    .count = sizeof(ehci_res) / sizeof(ehci_res[0]),
 	}
 };
@@ -121,185 +304,4 @@
 	.interfaces[HW_RES_DEV_IFACE] = &fun_hw_res_ops
 };
-
-static int usb_clocks(bool on)
-{
-	static usbhost_cm_regs_t *usb_host_cm = NULL;
-	static core_cm_regs_t *l4_core_cm = NULL;
-	static clock_control_cm_regs_t *clock_control_cm = NULL;
-
-	if (!usb_host_cm) {
-		const int ret = pio_enable((void*)USBHOST_CM_BASE_ADDRESS,
-		    USBHOST_CM_SIZE, (void**)&usb_host_cm);
-		if (ret != EOK)
-			return ret;
-	}
-
-	if (!l4_core_cm) {
-		const int ret = pio_enable((void*)CORE_CM_BASE_ADDRESS,
-		    CORE_CM_SIZE, (void**)&l4_core_cm);
-		if (ret != EOK)
-			return ret;
-	}
-
-	if (!clock_control_cm) {
-		const int ret = pio_enable((void*)CLOCK_CONTROL_CM_BASE_ADDRESS,
-		    CLOCK_CONTROL_CM_SIZE, (void**)&clock_control_cm);
-		if (ret != EOK)
-			return ret;
-	}
-
-	assert(l4_core_cm);
-	assert(usb_host_cm);
-	assert(clock_control_cm);
-
-	uint32_t reg;
-
-	/* Set DPLL3 and DPLL4 to automatic */
-	reg = clock_control_cm->autoidle_pll;
-	reg &= ~(CLOCK_CONTROL_CM_AUTOIDLE_PLL_AUTO_CORE_DPLL_MASK <<
-	    CLOCK_CONTROL_CM_AUTOIDLE_PLL_AUTO_CORE_DPLL_SHIFT);
-	reg &= ~(CLOCK_CONTROL_CM_AUTOIDLE_PLL_AUTO_PERIPH_DPLL_MASK <<
-	    CLOCK_CONTROL_CM_AUTOIDLE_PLL_AUTO_PERIPH_DPLL_SHIFT);
-	reg |= (CLOCK_CONTROL_CM_AUTOIDLE_PLL_AUTO_CORE_DPLL_AUTOMATIC <<
-	    CLOCK_CONTROL_CM_AUTOIDLE_PLL_AUTO_CORE_DPLL_SHIFT);
-	reg |= (CLOCK_CONTROL_CM_AUTOIDLE_PLL_AUTO_PERIPH_DPLL_AUTOMATIC <<
-	    CLOCK_CONTROL_CM_AUTOIDLE_PLL_AUTO_PERIPH_DPLL_SHIFT);
-	clock_control_cm->autoidle_pll = reg;
-
-	/* Set DPLL5 to automatic */
-	reg = clock_control_cm->autoidle2_pll;
-	reg &= ~(CLOCK_CONTROL_CM_AUTOIDLE2_PLL_AUTO_PERIPH2_DPLL_MASK <<
-	    CLOCK_CONTROL_CM_AUTOIDLE2_PLL_AUTO_PERIPH2_DPLL_SHIFT);
-	reg |= (CLOCK_CONTROL_CM_AUTOIDLE2_PLL_AUTO_PERIPH2_DPLL_AUTOMATIC <<
-	    CLOCK_CONTROL_CM_AUTOIDLE2_PLL_AUTO_PERIPH2_DPLL_SHIFT);
-	clock_control_cm->autoidle2_pll = reg;
-
-
-#ifdef DEBUG_CM
-	printf("DPLL5 could be on: %x %x.\n",
-	    clock_control_cm->idlest_ckgen, clock_control_cm->idlest2_ckgen);
-#endif
-
-	if (on) {
-		/* Enable interface and function clock for USB TLL */
-		l4_core_cm->iclken3 |= CORE_CM_ICLKEN3_EN_USBTLL_FLAG;
-		l4_core_cm->fclken3 |= CORE_CM_FCLKEN3_EN_USBTLL_FLAG;
-
-		/* Enable interface and function clock for USB hosts */
-		usb_host_cm->iclken |= USBHOST_CM_ICLKEN_EN_USBHOST;
-		usb_host_cm->fclken |= USBHOST_CM_FCLKEN_EN_USBHOST1_FLAG;
-		usb_host_cm->fclken |= USBHOST_CM_FCLKEN_EN_USBHOST2_FLAG;
-#ifdef DEBUG_CM
-	printf("DPLL5 (and everything else) should be on: %x %x.\n",
-	    clock_control_cm->idlest_ckgen, clock_control_cm->idlest2_ckgen);
-#endif
-	} else {
-		/* Disable interface and function clock for USB hosts */
-		usb_host_cm->fclken &= ~USBHOST_CM_FCLKEN_EN_USBHOST2_FLAG;
-		usb_host_cm->fclken &= ~USBHOST_CM_FCLKEN_EN_USBHOST1_FLAG;
-		usb_host_cm->iclken &= ~USBHOST_CM_ICLKEN_EN_USBHOST;
-
-		/* Disable interface and function clock for USB TLL */
-		l4_core_cm->fclken3 &= ~CORE_CM_FCLKEN3_EN_USBTLL_FLAG;
-		l4_core_cm->iclken3 &= ~CORE_CM_ICLKEN3_EN_USBTLL_FLAG;
-	}
-
-	return EOK;
-}
-
-/** Initialize USB TLL port connections.
- *
- * Different modes are on page 3312 of the Manual Figure 22-34.
- * Select mode than can operate in FS/LS.
- */
-static int usb_tll_init()
-{
-	tll_regs_t *usb_tll = NULL;
-	uhh_regs_t *uhh_conf = NULL;
-
-	int ret = pio_enable((void*)AMDM37x_USBTLL_BASE_ADDRESS,
-	    AMDM37x_USBTLL_SIZE, (void**)&usb_tll);
-	if (ret != EOK)
-		return ret;
-
-	ret = pio_enable((void*)AMDM37x_UHH_BASE_ADDRESS,
-	    AMDM37x_UHH_SIZE, (void**)&uhh_conf);
-	if (ret != EOK)
-		return ret;
-
-	/* Reset USB TLL */
-	usb_tll->sysconfig |= TLL_SYSCONFIG_SOFTRESET_FLAG;
-	ddf_msg(LVL_DEBUG2, "Waiting for USB TLL reset");
-	while (!(usb_tll->sysstatus & TLL_SYSSTATUS_RESET_DONE_FLAG));
-	ddf_msg(LVL_DEBUG, "USB TLL Reset done.");
-
-	{
-	/* Setup idle mode (smart idle) */
-	uint32_t sysc = usb_tll->sysconfig;
-	sysc |= TLL_SYSCONFIG_CLOCKACTIVITY_FLAG | TLL_SYSCONFIG_AUTOIDLE_FLAG;
-	sysc = (sysc
-	    & ~(TLL_SYSCONFIG_SIDLE_MODE_MASK << TLL_SYSCONFIG_SIDLE_MODE_SHIFT)
-	    ) | (0x2 << TLL_SYSCONFIG_SIDLE_MODE_SHIFT);
-	usb_tll->sysconfig = sysc;
-	ddf_msg(LVL_DEBUG2, "Set TLL->sysconfig (%p) to %x:%x.",
-	    &usb_tll->sysconfig, usb_tll->sysconfig, sysc);
-	}
-
-	{
-	/* Smart idle for UHH */
-	uint32_t sysc = uhh_conf->sysconfig;
-	sysc |= UHH_SYSCONFIG_CLOCKACTIVITY_FLAG | UHH_SYSCONFIG_AUTOIDLE_FLAG;
-	sysc = (sysc
-	    & ~(UHH_SYSCONFIG_SIDLE_MODE_MASK << UHH_SYSCONFIG_SIDLE_MODE_SHIFT)
-	    ) | (0x2 << UHH_SYSCONFIG_SIDLE_MODE_SHIFT);
-	sysc = (sysc
-	    & ~(UHH_SYSCONFIG_MIDLE_MODE_MASK << UHH_SYSCONFIG_MIDLE_MODE_SHIFT)
-	    ) | (0x2 << UHH_SYSCONFIG_MIDLE_MODE_SHIFT);
-	ddf_msg(LVL_DEBUG2, "Set UHH->sysconfig (%p) to %x.",
-	    &uhh_conf->sysconfig, uhh_conf->sysconfig);
-	uhh_conf->sysconfig = sysc;
-
-	/* All ports are connected on BBxM */
-	uhh_conf->hostconfig |= (UHH_HOSTCONFIG_P1_CONNECT_STATUS_FLAG
-	    | UHH_HOSTCONFIG_P2_CONNECT_STATUS_FLAG
-	    | UHH_HOSTCONFIG_P3_CONNECT_STATUS_FLAG);
-
-	/* Set all ports to go through TLL(UTMI)
-	 * Direct connection can only work in HS mode */
-	uhh_conf->hostconfig |= (UHH_HOSTCONFIG_P1_ULPI_BYPASS_FLAG
-	    | UHH_HOSTCONFIG_P2_ULPI_BYPASS_FLAG
-	    | UHH_HOSTCONFIG_P3_ULPI_BYPASS_FLAG);
-	ddf_msg(LVL_DEBUG2, "Set UHH->hostconfig (%p) to %x.",
-	    &uhh_conf->hostconfig, uhh_conf->hostconfig);
-	}
-
-	usb_tll->shared_conf |= TLL_SHARED_CONF_FCLK_IS_ON_FLAG;
-	ddf_msg(LVL_DEBUG2, "Set shared conf port (%p) to %x.",
-	    &usb_tll->shared_conf, usb_tll->shared_conf);
-
-	for (unsigned i = 0; i < 3; ++i) {
-		uint32_t ch = usb_tll->channel_conf[i];
-		/* Clear Channel mode and FSLS mode */
-		ch &= ~(TLL_CHANNEL_CONF_CHANMODE_MASK
-		    << TLL_CHANNEL_CONF_CHANMODE_SHIFT)
-		    & ~(TLL_CHANNEL_CONF_FSLSMODE_MASK
-		    << TLL_CHANNEL_CONF_FSLSMODE_SHIFT);
-
-		/* Serial mode is the only one capable of FS/LS operation. */
-		ch |= (TLL_CHANNEL_CONF_CHANMODE_UTMI_SERIAL_MODE
-		    << TLL_CHANNEL_CONF_CHANMODE_SHIFT);
-
-		/* Select FS/LS mode, no idea what the difference is
-		 * one of bidirectional modes might be good choice
-		 * 2 = 3pin bidi phy. */
-		ch |= (2 << TLL_CHANNEL_CONF_FSLSMODE_SHIFT);
-
-		/* Write to register */
-		ddf_msg(LVL_DEBUG2, "Setting port %u(%p) to %x.",
-		    i, &usb_tll->channel_conf[i], ch);
-		usb_tll->channel_conf[i] = ch;
-	}
-	return EOK;
-}
 
 static bool rootamdm37x_add_fun(ddf_dev_t *dev, const char *name,
@@ -345,5 +347,15 @@
 static int rootamdm37x_dev_add(ddf_dev_t *dev)
 {
-	int ret = usb_clocks(true);
+	assert(dev);
+	amdm37x_t *device = ddf_dev_data_alloc(dev, sizeof(amdm37x_t));
+	if (!device)
+		return ENOMEM;
+	int ret = amdm37x_hw_access_init(device);
+	if (ret != EOK) {
+		ddf_msg(LVL_FATAL, "Failed to setup hw access!.\n");
+		return ret;
+	}
+
+	ret = usb_clocks(device, true);
 	if (ret != EOK) {
 		ddf_msg(LVL_FATAL, "Failed to enable USB HC clocks!.\n");
@@ -351,8 +363,8 @@
 	}
 
-	ret = usb_tll_init();
+	ret = usb_tll_init(device);
 	if (ret != EOK) {
 		ddf_msg(LVL_FATAL, "Failed to init USB TLL!.\n");
-		usb_clocks(false);
+		usb_clocks(device, false);
 		return ret;
 	}
@@ -396,5 +408,5 @@
 {
 	printf("%s: HelenOS AM/DM37x(OMAP37x) platform driver\n", NAME);
-	ddf_log_init(NAME, LVL_ERROR);
+	ddf_log_init(NAME);
 	return ddf_driver_main(&rootamdm37x_driver);
 }
