Restart cycle master after bus manager election.

This should fix the IR(and maybe IT) problem when
the host becames the bus manager.
- rename fw_noderesolve() to fw_noderesolve_eui64() and add
	fw_noderesolve_nodeid().

4 files changed, 74 insertions, 25 deletions

diff --git a/sys/dev/firewire/firewire.c b/sys/dev/firewire/firewire.c
index 0b93285..5158e12 100644
--- a/sys/dev/firewire/firewire.c
+++ b/sys/dev/firewire/firewire.c
@@ -54,6 +54,7 @@
 
 #include <dev/firewire/firewire.h>
 #include <dev/firewire/firewirereg.h>
+#include <dev/firewire/fwmem.h>
 #include <dev/firewire/iec13213.h>
 #include <dev/firewire/iec68113.h>
 
@@ -91,6 +92,7 @@ static void fw_vmaccess __P((struct fw_xfer *));
 #endif
 struct fw_xfer *asyreqq __P((struct firewire_comm *, u_int8_t, u_int8_t, u_int8_t,
 	u_int32_t, u_int32_t, void (*)__P((struct fw_xfer *))));
+static int fw_bmr __P((struct firewire_comm *));
 
 static device_method_t firewire_methods[] = {
 	/* Device interface */
@@ -122,18 +124,40 @@ static driver_t firewire_driver = {
 };
 
 /*
- * To lookup node id. from EUI64.
+ * Lookup fwdev by node id.
  */
 struct fw_device *
-fw_noderesolve(struct firewire_comm *fc, struct fw_eui64 eui)
+fw_noderesolve_nodeid(struct firewire_comm *fc, int dst)
 {
 	struct fw_device *fwdev;
-	for(fwdev = TAILQ_FIRST(&fc->devices); fwdev != NULL;
-		fwdev = TAILQ_NEXT(fwdev, link)){
-		if(fwdev->eui.hi == eui.hi && fwdev->eui.lo == eui.lo){
+	int s;
+
+	s = splfw();
+	TAILQ_FOREACH(fwdev, &fc->devices, link)
+		if (fwdev->dst == dst)
 			break;
-		}
-	}
+	splx(s);
+
+	if(fwdev == NULL) return NULL;
+	if(fwdev->status == FWDEVINVAL) return NULL;
+	return fwdev;
+}
+
+/*
+ * Lookup fwdev by EUI64.
+ */
+struct fw_device *
+fw_noderesolve_eui64(struct firewire_comm *fc, struct fw_eui64 eui)
+{
+	struct fw_device *fwdev;
+	int s;
+
+	s = splfw();
+	TAILQ_FOREACH(fwdev, &fc->devices, link)
+		if (fwdev->eui.hi == eui.hi && fwdev->eui.lo == eui.lo)
+			break;
+	splx(s);
+
 	if(fwdev == NULL) return NULL;
 	if(fwdev->status == FWDEVINVAL) return NULL;
 	return fwdev;
@@ -851,19 +875,14 @@ fw_xfer_free( struct fw_xfer* xfer)
 	free(xfer, M_DEVBUF);
 }
 
-/*
- * Callback for PHY configuration. 
- */
 static void
-fw_phy_config_callback(struct fw_xfer *xfer)
+fw_asy_callback_free(struct fw_xfer *xfer)
 {
 #if 0
-	printf("phy_config done state=%d resp=%d\n",
+	printf("asyreq done state=%d resp=%d\n",
 				xfer->state, xfer->resp);
 #endif
 	fw_xfer_free(xfer);
-	/* XXX need bus reset ?? */
-	/* sc->fc->ibr(xfer->fc);  LOOP */
 }
 
 /*
@@ -885,7 +904,7 @@ fw_phy_config(struct firewire_comm *fc, int root_node, int gap_count)
 	xfer->send.off = 0;
 	xfer->fc = fc;
 	xfer->retry_req = fw_asybusy;
-	xfer->act.hand = fw_phy_config_callback;
+	xfer->act.hand = fw_asy_callback_free;
 
 	xfer->send.buf = malloc(sizeof(u_int32_t),
 					M_DEVBUF, M_NOWAIT | M_ZERO);
@@ -1041,9 +1060,9 @@ void fw_sidrcv(struct firewire_comm* fc, caddr_t buf, u_int len, u_int off)
 #endif
 	}
 	free(buf, M_DEVBUF);
-	/* Optimize gap_count, if I am BMGR */
 	if(fc->irm == ((CSRARC(fc, NODE_IDS) >> 16 ) & 0x3f)){
-		fw_phy_config(fc, -1, gap_cnt[fc->max_hop]);
+		/* I am BMGR */
+		fw_bmr(fc);
 	}
 	callout_reset(&fc->busprobe_callout, hz/4,
 			(void *)fw_bus_probe, (void *)fc);
@@ -1851,10 +1870,7 @@ fw_try_bmr_callback(struct fw_xfer *xfer)
 		CSRARC(fc, BUS_MGR_ID));
 	if(bmr == fc->nodeid){
 		printf("(me)\n");
-/* If I am bus manager, optimize gapcount */
-		if(fc->max_hop <= MAX_GAPHOP ){
-			fw_phy_config(fc, -1, gap_cnt[fc->max_hop]);
-		}
+		fw_bmr(fc);
 	}else{
 		printf("\n");
 	}
@@ -2008,5 +2024,36 @@ fw_crc16(u_int32_t *ptr, u_int32_t len){
 	return((u_int16_t) crc);
 }
 
+int
+fw_bmr(struct firewire_comm *fc)
+{
+	struct fw_device fwdev;
+	int cmstr;
+
+	/* XXX Assume that the current root node is cycle master capable */
+	cmstr = fc->max_node;
+	/* If I am the bus manager, optimize gapcount */
+	if(fc->max_hop <= MAX_GAPHOP ){
+		fw_phy_config(fc, (fc->max_node > 0)?cmstr:-1,
+						gap_cnt[fc->max_hop]);
+	}
+	/* If we are the cycle master, nothing to do */
+	if (cmstr == fc->nodeid)
+		return 0;
+	/* Bus probe has not finished, make dummy fwdev for cmstr */
+	bzero(&fwdev, sizeof(fwdev));
+	fwdev.fc = fc;
+	fwdev.dst = cmstr;
+	fwdev.speed = 0;
+	fwdev.maxrec = 8; /* 512 */
+	fwdev.status = FWDEVINIT;
+	/* Set cmstr bit on the cycle master */
+	fwmem_write_quad(&fwdev, NULL, 0/*spd*/,
+		0xffff, 0xf0000000 | STATE_SET, 1 << 16,
+		fw_asy_callback_free);
+
+	return 0;
+}
+
 DRIVER_MODULE(firewire,fwohci,firewire_driver,firewire_devclass,0,0);
 MODULE_VERSION(firewire, 1);
diff --git a/sys/dev/firewire/firewirereg.h b/sys/dev/firewire/firewirereg.h
index 57fbfdf..1a74811 100644
--- a/sys/dev/firewire/firewirereg.h
+++ b/sys/dev/firewire/firewirereg.h
@@ -320,7 +320,8 @@ u_int16_t fw_crc16 __P((u_int32_t *, u_int32_t));
 void fw_xfer_timeout __P((void *));
 void fw_xfer_done __P((struct fw_xfer *));
 void fw_asy_callback __P((struct fw_xfer *));
-struct fw_device *fw_noderesolve __P((struct firewire_comm *, struct fw_eui64));
+struct fw_device *fw_noderesolve_nodeid __P((struct firewire_comm *, int));
+struct fw_device *fw_noderesolve_eui64 __P((struct firewire_comm *, struct fw_eui64));
 struct fw_bind *fw_bindlookup __P((struct firewire_comm *, u_int32_t, u_int32_t));
 
 
diff --git a/sys/dev/firewire/fwdev.c b/sys/dev/firewire/fwdev.c
index 8474049..0a74018 100644
--- a/sys/dev/firewire/fwdev.c
+++ b/sys/dev/firewire/fwdev.c
@@ -745,7 +745,8 @@ fw_ioctl (dev_t dev, u_long cmd, caddr_t data, int flag, fw_proc *td)
 			xfer->dst = ntohs(fp->mode.hdr.dst);
 			break;
 		case FWASREQEUI:
-			fwdev = fw_noderesolve(sc->fc, asyreq->req.dst.eui);
+			fwdev = fw_noderesolve_eui64(sc->fc,
+						asyreq->req.dst.eui);
 			if (fwdev == NULL) {
 				device_printf(sc->fc->bdev,
 					"cannot find node\n");
diff --git a/sys/dev/firewire/fwmem.c b/sys/dev/firewire/fwmem.c
index ed0eed9..478bcbf 100644
--- a/sys/dev/firewire/fwmem.c
+++ b/sys/dev/firewire/fwmem.c
@@ -158,7 +158,7 @@ fwmem_write_quad(
 		return NULL;
 
 	fp = (struct fw_pkt *)xfer->send.buf;
-	fp->mode.wreqq.tcode = FWTCODE_RREQQ;
+	fp->mode.wreqq.tcode = FWTCODE_WREQQ;
 	fp->mode.wreqq.dst = htons(xfer->dst);
 	fp->mode.wreqq.dest_hi = htons(dst_hi);
 	fp->mode.wreqq.dest_lo = htonl(dst_lo);
@@ -240,7 +240,7 @@ fwmem_read (dev_t dev, struct uio *uio, int ioflag)
 	int len;
 
 	sc = devclass_get_softc(firewire_devclass, unit);
-	fwdev = fw_noderesolve(sc->fc, fwmem_eui64);
+	fwdev = fw_noderesolve_eui64(sc->fc, fwmem_eui64);
 	if (fwdev == NULL) {
 		printf("fwmem: no such device ID:%08x%08x\n",
 			fwmem_eui64.hi, fwmem_eui64.lo);