2 files changed, 56 insertions, 0 deletions
diff --git a/board/siemens/smartweb/smartweb.c b/board/siemens/smartweb/smartweb.c
index cf8a7f5b06..2d424882a9 100644
--- a/board/siemens/smartweb/smartweb.c
+++ b/board/siemens/smartweb/smartweb.c
@@ -25,6 +25,7 @@
 #include <asm/arch/at91_pmc.h>
 #include <asm/arch/at91_spi.h>
 #include <spi.h>
+#include <asm/arch/clk.h>
 #include <asm/arch/gpio.h>
 #include <watchdog.h>
 #ifdef CONFIG_MACB
@@ -108,6 +109,29 @@ static void smartweb_macb_hw_init(void)
 }
 #endif /* CONFIG_MACB */
 
+#ifdef CONFIG_USB_GADGET_AT91
+#include <linux/usb/at91_udc.h>
+
+void at91_udp_hw_init(void)
+{
+	at91_pmc_t *pmc = (at91_pmc_t *)ATMEL_BASE_PMC;
+
+	/* Enable PLLB */
+	writel(get_pllb_init(), &pmc->pllbr);
+	while ((readl(&pmc->sr) & AT91_PMC_LOCKB) != AT91_PMC_LOCKB)
+		;
+
+	/* Enable UDPCK clock, MCK is enabled in at91_clock_init() */
+	at91_periph_clk_enable(ATMEL_ID_UDP);
+
+	writel(AT91SAM926x_PMC_UDP, &pmc->scer);
+}
+
+struct at91_udc_data board_udc_data  = {
+	.baseaddr = ATMEL_BASE_UDP0,
+};
+#endif
+
 int board_early_init_f(void)
 {
 	/* enable this here, as we have SPL without serial support */
@@ -134,6 +158,11 @@ int board_init(void)
 	at91_set_gpio_output(AT91_PIN_PC10, 0);
 	at91_set_gpio_output(AT91_PIN_PC11, 1);
 
+#ifdef CONFIG_USB_GADGET_AT91
+	at91_udp_hw_init();
+	at91_udc_probe(&board_udc_data);
+#endif
+
 	return 0;
 }
 
diff --git a/board/siemens/taurus/taurus.c b/board/siemens/taurus/taurus.c
index edeb15ba48..72c5e6083d 100644
--- a/board/siemens/taurus/taurus.c
+++ b/board/siemens/taurus/taurus.c
@@ -285,6 +285,29 @@ void spi_cs_deactivate(struct spi_slave *slave)
 	at91_set_gpio_value(TAURUS_SPI_CS_PIN, 1);
 }
 
+#ifdef CONFIG_USB_GADGET_AT91
+#include <linux/usb/at91_udc.h>
+
+void at91_udp_hw_init(void)
+{
+	at91_pmc_t *pmc = (at91_pmc_t *)ATMEL_BASE_PMC;
+
+	/* Enable PLLB */
+	writel(get_pllb_init(), &pmc->pllbr);
+	while ((readl(&pmc->sr) & AT91_PMC_LOCKB) != AT91_PMC_LOCKB)
+		;
+
+	/* Enable UDPCK clock, MCK is enabled in at91_clock_init() */
+	at91_periph_clk_enable(ATMEL_ID_UDP);
+
+	writel(AT91SAM926x_PMC_UDP, &pmc->scer);
+}
+
+struct at91_udc_data board_udc_data  = {
+	.baseaddr = ATMEL_BASE_UDP0,
+};
+#endif
+
 int board_init(void)
 {
 	/* adress of boot parameters */
@@ -297,6 +320,10 @@ int board_init(void)
 	taurus_macb_hw_init();
 #endif
 	at91_spi0_hw_init(TAURUS_SPI_MASK);
+#ifdef CONFIG_USB_GADGET_AT91
+	at91_udp_hw_init();
+	at91_udc_probe(&board_udc_data);
+#endif
 
 	return 0;
 }