zynq: fix usb by implementing delay() and give proper port speed in portstatus

2 files changed, 37 insertions, 3 deletions

diff --git a/sys/src/9/zynq/timer.c b/sys/src/9/zynq/timer.c
index 585fe295a..4edd2962d 100644
--- a/sys/src/9/zynq/timer.c
+++ b/sys/src/9/zynq/timer.c
@@ -23,13 +23,19 @@ enum {
 uvlong timerhz;
 
 void
-delay(int)
+microdelay(int n)
 {
+	ulong now;
+
+	now = µs();
+	while(µs() - now < n);
 }
 
 void
-microdelay(int)
+delay(int n)
 {
+	while(--n >= 0)
+		microdelay(1000);
 }
 
 uvlong
diff --git a/sys/src/9/zynq/usbehcizynq.c b/sys/src/9/zynq/usbehcizynq.c
index d5ec5b005..c93dbe0f0 100644
--- a/sys/src/9/zynq/usbehcizynq.c
+++ b/sys/src/9/zynq/usbehcizynq.c
@@ -102,6 +102,29 @@ dmaflush(int clean, void *data, ulong len)
 	}
 }
 
+static int (*ehciportstatus)(Hci*,int);
+
+static int
+portstatus(Hci *hp, int port)
+{
+	Ctlr *ctlr;
+	Eopio *opio;
+	int r, sts;
+
+	ctlr = hp->aux;
+	opio = ctlr->opio;
+	r = (*ehciportstatus)(hp, port);
+	if(r & HPpresent){
+		sts = opio->portsc[port-1];
+		r &= ~(HPhigh|HPslow);
+		if(sts & (1<<9))
+			r |= HPhigh;
+		else if(sts & 1<<26)
+			r |= HPslow;
+	}
+	return r;
+}
+
 static int
 reset(Hci *hp)
 {
@@ -124,7 +147,7 @@ reset(Hci *hp)
 	ctlr->r = vmap(ctlr->base, 0x1F0);
 	ctlr->opio = (Eopio *) ((uchar *) ctlr->r + 0x140);
 	ctlr->capio = (void *) ctlr->base;
-	hp->nports = 1;
+	hp->nports = 1;	
 
 	ctlr->tdalloc = tdalloc;
 	ctlr->dmaalloc = dmaalloc;
@@ -136,6 +159,11 @@ reset(Hci *hp)
 	ctlr->r[ULPI] = 1<<30 | 1<<29 | 0x0B << 16 | 3<<5;
 	ehcimeminit(ctlr);
 	ehcilinkage(hp);
+
+	/* hook portstatus */
+	ehciportstatus = hp->portstatus;
+	hp->portstatus = portstatus;
+
 	if(hp->interrupt != nil)
 		intrenable(hp->irq, hp->interrupt, hp, LEVEL, hp->type);
 	return 0;