3 files changed, 21 insertions, 4 deletions
diff --git a/firmware/src/os/dbgu.c b/firmware/src/os/dbgu.c
index a3a5941..e71b62e 100644
--- a/firmware/src/os/dbgu.c
+++ b/firmware/src/os/dbgu.c
@@ -85,10 +85,10 @@ static void DBGU_irq_handler(u_int32_t sr)
 	case '0':		//* info
 		AT91F_DBGU_Frame("Clear Pull up\n\r");
 		// Set
-		AT91F_PIO_ClearOutput(AT91C_BASE_PIOA, OPENPCD_PIO_UDP_PUP);
+		udp_pullup_on();
 		break;
 	case '1':		//* info
-		AT91F_PIO_SetOutput(AT91C_BASE_PIOA, OPENPCD_PIO_UDP_PUP);
+		udp_pullup_off();
 		AT91F_DBGU_Printk("Set Pull up\n\r");
 		// Reset Application
 		Send_reset();
diff --git a/firmware/src/os/pcd_enumerate.c b/firmware/src/os/pcd_enumerate.c
index 24b2874..f201a32 100644
--- a/firmware/src/os/pcd_enumerate.c
+++ b/firmware/src/os/pcd_enumerate.c
@@ -487,6 +487,18 @@ out:
 	AT91F_AIC_ClearIt(AT91C_BASE_AIC, AT91C_ID_UDP);
 }
 
+void udp_pullup_on(void)
+{
+	AT91F_PIO_SetOutput(AT91C_BASE_PIOA, OPENPCD_PIO_UDP_PUP);
+	AT91F_PIO_ClearOutput(AT91C_BASE_PIOA, OPENPCD_PIO_UDP_PUPv4);
+}
+
+void udp_pullup_off(void)
+{
+	AT91F_PIO_ClearOutput(AT91C_BASE_PIOA, OPENPCD_PIO_UDP_PUP);
+	AT91F_PIO_SetOutput(AT91C_BASE_PIOA, OPENPCD_PIO_UDP_PUPv4);
+}
+
 /* Open USB Device Port  */
 void udp_open(void)
 {
@@ -508,16 +520,18 @@ void udp_open(void)
 
 	/* Set the Pull up resistor */
 	AT91F_PIO_SetOutput(AT91C_BASE_PIOA, OPENPCD_PIO_UDP_PUP);
+	AT91F_PIO_ClearOutput(AT91C_BASE_PIOA, OPENPCD_PIO_UDP_PUPv4);
+	udp_pullup_on();
 }
 
 void udp_reset(void)
 {
 	volatile int i;
 
-	AT91F_PIO_ClearOutput(AT91C_BASE_PIOA, OPENPCD_PIO_UDP_PUP);
+	udp_pullup_off();
 	for (i = 0; i < 0xffff; i++)
 		;
-	AT91F_PIO_SetOutput(AT91C_BASE_PIOA, OPENPCD_PIO_UDP_PUP);
+	udp_pullup_on();
 }
 
 #ifdef DEBUG_UDP_EP0
diff --git a/firmware/src/os/pcd_enumerate.h b/firmware/src/os/pcd_enumerate.h
index b2af3a1..ae15dc0 100644
--- a/firmware/src/os/pcd_enumerate.h
+++ b/firmware/src/os/pcd_enumerate.h
@@ -21,6 +21,9 @@ extern int udp_refill_ep(int ep);
 extern void udp_unthrottle(void);
 extern void udp_reset(void);
 
+extern void udp_pullup_off(void);
+extern void udp_pullup_on(void);
+
 struct ep_ctx {
 	atomic_t pkts_in_transit;
 	struct {