Yet more patches -current

Index: firewire.c
===================================================================
--- firewire.c	(revision 188508)
+++ firewire.c	(working copy)
@@ -31,7 +31,7 @@
  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  * POSSIBILITY OF SUCH DAMAGE.
  * 
- * $FreeBSD$
+ * $FreeBSD: head/sys/dev/firewire/firewire.c 187993 2009-02-01 23:28:52Z sbruno $
  *
  */
 
@@ -77,7 +77,7 @@
 	struct crom_chunk hw;
 };
 
-int firewire_debug=0, try_bmr=1, hold_count=3;
+int firewire_debug=0, try_bmr=1, hold_count=0;
 SYSCTL_INT(_debug, OID_AUTO, firewire_debug, CTLFLAG_RW, &firewire_debug, 0,
 	"FireWire driver debug flag");
 SYSCTL_NODE(_hw, OID_AUTO, firewire, CTLFLAG_RD, 0, "FireWire Subsystem");
@@ -434,14 +434,14 @@
 				sizeof(struct crom_src_buf),
 				M_FW, M_NOWAIT | M_ZERO);
 	if (fc->crom_src_buf == NULL) {
-		device_printf(fc->dev, "%s: Malloc Failure crom src buff\n", __func__);
+		device_printf(fc->bdev, "%s: Malloc Failure crom src buff\n", __func__);
 		return ENOMEM;
 	}
 	fc->topology_map = (struct fw_topology_map *)malloc(
 				sizeof(struct fw_topology_map),
 				M_FW, M_NOWAIT | M_ZERO);
 	if (fc->topology_map == NULL) {
-		device_printf(fc->dev, "%s: Malloc Failure topology map\n", __func__);
+		device_printf(fc->bdev, "%s: Malloc Failure topology map\n", __func__);
 		free(fc->crom_src_buf, M_FW);
 		return ENOMEM;
 	}
@@ -449,7 +449,7 @@
 				sizeof(struct fw_speed_map),
 				M_FW, M_NOWAIT | M_ZERO);
 	if (fc->speed_map == NULL) {
-		device_printf(fc->dev, "%s: Malloc Failure speed map\n", __func__);
+		device_printf(fc->bdev, "%s: Malloc Failure speed map\n", __func__);
 		free(fc->crom_src_buf, M_FW);
 		free(fc->topology_map, M_FW);
 		return ENOMEM;
@@ -1257,12 +1257,11 @@
 	fp->mode.common.tcode |= FWTCODE_PHY;
 
 	if (firewire_debug)
-		printf("send phy_config root_node=%d gap_count=%d\n",
-						root_node, gap_count);
+		device_printf(fc->bdev, "%s: root_node=%d gap_count=%d\n",
+					__func__, root_node, gap_count);
 	fw_asyreq(fc, -1, xfer);
 }
 
-#if 0
 /*
  * Dump self ID. 
  */
@@ -1278,7 +1277,6 @@
 		s->p0.power_class, s->p0.port0, s->p0.port1,
 		s->p0.port2, s->p0.initiated_reset, s->p0.more_packets);
 }
-#endif
 
 /*
  * To receive self ID. 
@@ -1302,7 +1300,8 @@
 	self_id = &fc->topology_map->self_id[0];
 	for(i = 0; i < fc->sid_cnt; i ++){
 		if (sid[1] != ~sid[0]) {
-			printf("fw_sidrcv: invalid self-id packet\n");
+			device_printf(fc->bdev, "%s: ERROR invalid self-id packet\n",
+						__func__);
 			sid += 2;
 			continue;
 		}
@@ -1311,9 +1310,8 @@
 		if(self_id->p0.sequel == 0){
 			fc->topology_map->node_count ++;
 			c_port = 0;
-#if 0
-			fw_print_sid(sid[0]);
-#endif
+			if (firewire_debug)
+				fw_print_sid(sid[0]);
 			node = self_id->p0.phy_id;
 			if(fc->max_node < node){
 				fc->max_node = self_id->p0.phy_id;
@@ -1348,7 +1346,6 @@
 		self_id++;
 		fc->topology_map->self_id_count ++;
 	}
-	device_printf(fc->bdev, "%d nodes", fc->max_node + 1);
 	/* CRC */
 	fc->topology_map->crc = fw_crc16(
 			(uint32_t *)&fc->topology_map->generation,
@@ -1367,17 +1364,12 @@
 	bcopy(p, &CSRARC(fc, SPED_MAP + 8), (fc->speed_map->crc_len - 1)*4);
 
 	fc->max_hop = fc->max_node - i_branch;
-	printf(", maxhop <= %d", fc->max_hop);
+	device_printf(fc->bdev, "%d nodes, maxhop <= %d %s irm(%d) %s\n",
+			fc->max_node + 1, fc->max_hop,
+			(fc->irm == -1) ? "Not IRM capable" : "cable IRM",
+			fc->irm,
+			(fc->irm == fc->nodeid) ? " (me) " : "");
 		
-	if(fc->irm == -1 ){
-		printf(", Not found IRM capable node");
-	}else{
-		printf(", cable IRM = %d", fc->irm);
-		if (fc->irm == fc->nodeid)
-			printf(" (me)");
-	}
-	printf("\n");
-
 	if (try_bmr && (fc->irm != -1) && (CSRARC(fc, BUS_MGR_ID) == 0x3f)) {
 		if (fc->irm == fc->nodeid) {
 			fc->status = FWBUSMGRDONE;
@@ -1408,10 +1400,23 @@
 	fc->status = FWBUSEXPLORE;
 
 	/* Invalidate all devices, just after bus reset. */
+	if (firewire_debug)
+		device_printf(fc->bdev, "%s:"
+			"iterate and invalidate all nodes\n",
+			__func__);
 	STAILQ_FOREACH(fwdev, &fc->devices, link)
 		if (fwdev->status != FWDEVINVAL) {
 			fwdev->status = FWDEVINVAL;
 			fwdev->rcnt = 0;
+			if (firewire_debug)
+				device_printf(fc->bdev, "%s:"
+					"Invalidate Dev ID: %08x%08x\n",
+					__func__, fwdev->eui.hi, fwdev->eui.lo);
+		} else {
+			if (firewire_debug)
+				device_printf(fc->bdev, "%s:"
+					"Dev ID: %08x%08x already invalid\n",
+					__func__, fwdev->eui.hi, fwdev->eui.lo);
 		}
 	splx(s);
 
@@ -1420,13 +1425,13 @@
 
 static int
 fw_explore_read_quads(struct fw_device *fwdev, int offset,
-    uint32_t *quad, int n)
+    uint32_t *quad, int length)
 {
 	struct fw_xfer *xfer;
 	uint32_t tmp;
 	int i, error;
 
-	for (i = 0; i < n; i ++, offset += sizeof(uint32_t)) {
+	for (i = 0; i < length; i ++, offset += sizeof(uint32_t)) {
 		xfer = fwmem_read_quad(fwdev, NULL, -1,
 		    0xffff, 0xf0000000 | offset, (void *)&tmp,
 		    fw_xferwake);
@@ -1503,7 +1508,8 @@
 	uint32_t *csr;
 	struct csrhdr *hdr;
 	struct bus_info *binfo;
-	int err, node, spd;
+	int err, node;
+	uint32_t speed_test = 0;
 
 	fc = dfwdev->fc;
 	csr = dfwdev->csrrom;
@@ -1511,28 +1517,36 @@
 
 	/* First quad */
 	err = fw_explore_read_quads(dfwdev, CSRROMOFF, &csr[0], 1);
-	if (err)
+	if (err) {
+		device_printf(fc->bdev, "%s: node%d: explore_read_quads failure\n",
+		    __func__, node);
+		dfwdev->status = FWDEVINVAL;
 		return (-1);
+	}
 	hdr = (struct csrhdr *)&csr[0];
 	if (hdr->info_len != 4) {
 		if (firewire_debug)
-			printf("node%d: wrong bus info len(%d)\n",
-			    node, hdr->info_len);
+			device_printf(fc->bdev, "%s: node%d: wrong bus info len(%d)\n",
+			    __func__, node, hdr->info_len);
+		dfwdev->status = FWDEVINVAL;
 		return (-1);
 	}
 
 	/* bus info */
 	err = fw_explore_read_quads(dfwdev, CSRROMOFF + 0x04, &csr[1], 4);
-	if (err)
+	if (err) {
+		device_printf(fc->bdev, "%s: node%d: error reading 0x04\n",
+		    __func__, node);
+		dfwdev->status = FWDEVINVAL;
 		return (-1);
+	}
 	binfo = (struct bus_info *)&csr[1];
 	if (binfo->bus_name != CSR_BUS_NAME_IEEE1394) {
-		if (firewire_debug)
-			printf("node%d: invalid bus name 0x%08x\n",
-			    node, binfo->bus_name);
+		device_printf(fc->bdev, "%s: node%d: invalid bus name 0x%08x\n",
+		    __func__, node, binfo->bus_name);
+		dfwdev->status = FWDEVINVAL;
 		return (-1);
 	}
-	spd = fc->speed_map->speed[fc->nodeid][node];
 	STAILQ_FOREACH(fwdev, &fc->devices, link)
 		if (FW_EUI64_EQUAL(fwdev->eui, binfo->eui64))
 			break;
@@ -1541,12 +1555,46 @@
 		fwdev = malloc(sizeof(struct fw_device), M_FW,
 						M_NOWAIT | M_ZERO);
 		if (fwdev == NULL) {
-			if (firewire_debug)
-				printf("node%d: no memory\n", node);
+			device_printf(fc->bdev, "%s: node%d: no memory\n",
+					__func__, node);
 			return (-1);
 		}
 		fwdev->fc = fc;
 		fwdev->eui = binfo->eui64;
+		/*
+ 		 * Pre-1394a-2000 didn't have link_spd in
+ 		 * the Bus Info block, so try and use the 
+ 		 * speed map value.
+ 		 * 1394a-2000 compliant devices only use
+ 		 * the Bus Info Block link spd value, so
+ 		 * ignore the speed map alltogether. SWB
+ 		 */
+		if ( binfo->link_spd == FWSPD_S100 /* 0 */) {
+			device_printf(fc->bdev, "%s"
+				"Pre 1394a-2000 detected\n",
+				__func__);
+			fwdev->speed = fc->speed_map->speed[fc->nodeid][node];
+		} else
+			fwdev->speed = binfo->link_spd;
+		/*
+		 * Test this speed with a read to the CSRROM.
+		 * If it fails, slow down the speed and retry.
+		 */
+		while (fwdev->speed > 0) {
+			err = fw_explore_read_quads(fwdev, CSRROMOFF,
+            				&speed_test, 1);
+			if (err)
+				fwdev->speed--;
+			else
+				break;
+				
+		}
+		if (fwdev->speed != binfo->link_spd)
+			device_printf(fc->bdev, "%s: fwdev->speed(%s)"
+						" set lower than binfo->link_spd(%s)\n",
+						__func__,
+						linkspeed[fwdev->speed],
+						linkspeed[binfo->link_spd]);
 		/* inesrt into sorted fwdev list */
 		pfwdev = NULL;
 		STAILQ_FOREACH(tfwdev, &fc->devices, link) {
@@ -1562,17 +1610,16 @@
 			STAILQ_INSERT_AFTER(&fc->devices, pfwdev, fwdev, link);
 
 		device_printf(fc->bdev, "New %s device ID:%08x%08x\n",
-		    linkspeed[spd],
+		    linkspeed[fwdev->speed],
 		    fwdev->eui.hi, fwdev->eui.lo);
 	}
 	fwdev->dst = node;
 	fwdev->status = FWDEVINIT;
-	fwdev->speed = spd;
 
 	/* unchanged ? */
 	if (bcmp(&csr[0], &fwdev->csrrom[0], sizeof(uint32_t) * 5) == 0) {
 		if (firewire_debug)
-			device_printf(fc->dev, "node%d: crom unchanged\n", node);
+			device_printf(fc->bdev, "node%d: crom unchanged\n", node);
 		return (0);
 	}
 
@@ -1628,12 +1675,22 @@
 
 	for (node = 0; node <= fc->max_node; node ++) {
 		/* We don't probe myself and linkdown nodes */
-		if (node == fc->nodeid)
+		if (node == fc->nodeid) {
+			if (firewire_debug)
+				device_printf(fc->bdev, "%s:"
+					"found myself node(%d) fc->nodeid(%d) fc->max_node(%d)\n",
+					__func__, node, fc->nodeid, fc->max_node);
 			continue;
+		} else if (firewire_debug) {
+			device_printf(fc->bdev, "%s:"
+				"node(%d) fc->max_node(%d) found\n",
+				__func__, node, fc->max_node);
+		}
 		fwsid = fw_find_self_id(fc, node);
 		if (!fwsid || !fwsid->p0.link_active) {
 			if (firewire_debug)
-				printf("node%d: link down\n", node);
+				device_printf(fc->bdev, "%s: node%d: link down\n",
+							__func__, node);
 			continue;
 		}
 		nodes[todo++] = node;
@@ -1648,8 +1705,8 @@
 			if (err)
 				nodes[todo2++] = nodes[i];
 			if (firewire_debug)
-				printf("%s: node %d, err = %d\n",
-					__FUNCTION__, node, err);
+				device_printf(fc->bdev, "%s: node %d, err = %d\n",
+					__func__, node, err);
 		}
 		todo = todo2;
 	}
@@ -1670,9 +1727,9 @@
 			mtx_unlock(&fc->wait_lock);
 			fw_explore(fc);
 			fc->status = FWBUSEXPDONE;
+			fw_attach_dev(fc);
 			if (firewire_debug)
-				printf("bus_explore done\n");
-			fw_attach_dev(fc);
+				device_printf(fc->bdev, "%s done\n", __func__);
 			mtx_lock(&fc->wait_lock);
 		}
 		msleep((void *)fc, &fc->wait_lock, PWAIT|PCATCH, "-", 0);
@@ -1699,11 +1756,19 @@
 			fwdev->status = FWDEVATTACHED;
 		} else if (fwdev->status == FWDEVINVAL) {
 			fwdev->rcnt ++;
+			if (firewire_debug)
+				device_printf(fc->bdev, "%s:"
+					"fwdev->rcnt(%d), hold_count(%d)\n",
+					__func__, fwdev->rcnt, hold_count);
 			if (fwdev->rcnt > hold_count) {
 				/*
 				 * Remove devices which have not been seen
 				 * for a while.
 				 */
+				device_printf(fc->bdev, "%s:"
+					"Device no longer on the bus, removing."
+					"device ID:%08x%08x",
+					__func__, fwdev->eui.hi, fwdev->eui.lo);
 				STAILQ_REMOVE(&fc->devices, fwdev, fw_device,
 				    link);
 				free(fwdev, M_FW);
@@ -1712,16 +1777,16 @@
 	}
 
 	err = device_get_children(fc->bdev, &devlistp, &devcnt);
-	if( err != 0 )
-		return;
-	for( i = 0 ; i < devcnt ; i++){
-		if (device_get_state(devlistp[i]) >= DS_ATTACHED)  {
-			fdc = device_get_softc(devlistp[i]);
-			if (fdc->post_explore != NULL)
-				fdc->post_explore(fdc);
+	if( err == 0 ) {
+		for( i = 0 ; i < devcnt ; i++){
+			if (device_get_state(devlistp[i]) >= DS_ATTACHED)  {
+				fdc = device_get_softc(devlistp[i]);
+				if (fdc->post_explore != NULL)
+					fdc->post_explore(fdc);
+			}
 		}
+		free(devlistp, M_TEMP);
 	}
-	free(devlistp, M_TEMP);
 
 	return;
 }
@@ -1801,8 +1866,9 @@
 	for (i = 0; i < rb->nvec; i++, rb->vec++) {
 		len = MIN(rb->vec->iov_len, plen);
 		if (res < len) {
-			printf("rcv buffer(%d) is %d bytes short.\n",
-			    rb->xfer->recv.pay_len, len - res);
+			device_printf(rb->fc->bdev, "%s:"
+				" rcv buffer(%d) is %d bytes short.\n",
+				__func__, rb->xfer->recv.pay_len, len - res);
 			len = res;
 		}
 		bcopy(rb->vec->iov_base, p, len);
@@ -1849,13 +1915,15 @@
 		rb->xfer = fw_tl2xfer(rb->fc, fp->mode.hdr.src,
 				fp->mode.hdr.tlrt >> 2, fp->mode.hdr.tcode);
 		if(rb->xfer == NULL) {
-			printf("fw_rcv: unknown response "
-			    "%s(%x) src=0x%x tl=0x%x rt=%d data=0x%x\n",
-			    tcode_str[tcode], tcode,
-			    fp->mode.hdr.src,
-			    fp->mode.hdr.tlrt >> 2,
-			    fp->mode.hdr.tlrt & 3,
-			    fp->mode.rresq.data);
+			device_printf(rb->fc->bdev, "%s: "
+				"unknown response "
+			    	"%s(%x) src=0x%x tl=0x%x rt=%d data=0x%x\n",
+				__func__,
+			    	tcode_str[tcode], tcode,
+				fp->mode.hdr.src,
+				fp->mode.hdr.tlrt >> 2,
+				fp->mode.hdr.tlrt & 3,
+				fp->mode.rresq.data);
 #if 0
 			printf("try ad-hoc work around!!\n");
 			rb->xfer = fw_tl2xfer(rb->fc, fp->mode.hdr.src,
@@ -1887,7 +1955,8 @@
 #endif
 			break;
 		default:
-			printf("unexpected flag 0x%02x\n", rb->xfer->flag);
+			device_printf(rb->fc->bdev, "%s: "
+				"unexpected flag 0x%02x\n", __func__, rb->xfer->flag);
 		}
 		return;
 	case FWTCODE_WREQQ:
@@ -1898,17 +1967,23 @@
 		bind = fw_bindlookup(rb->fc, fp->mode.rreqq.dest_hi,
 			fp->mode.rreqq.dest_lo);
 		if(bind == NULL){
-			printf("Unknown service addr 0x%04x:0x%08x %s(%x)"
+			device_printf(rb->fc->bdev, "%s: "
+				"Unknown service addr 0x%04x:0x%08x %s(%x)"
 #if defined(__DragonFly__) || __FreeBSD_version < 500000
-			    " src=0x%x data=%lx\n",
+				" src=0x%x data=%lx\n",
 #else
-			    " src=0x%x data=%x\n",
+				" src=0x%x data=%x\n",
 #endif
-			    fp->mode.wreqq.dest_hi, fp->mode.wreqq.dest_lo,
-			    tcode_str[tcode], tcode,
-			    fp->mode.hdr.src, ntohl(fp->mode.wreqq.data));
+				__func__,
+				fp->mode.wreqq.dest_hi,
+				fp->mode.wreqq.dest_lo,
+				tcode_str[tcode], tcode,
+				fp->mode.hdr.src,
+				ntohl(fp->mode.wreqq.data));
+
 			if (rb->fc->status == FWBUSINIT) {
-				printf("fw_rcv: cannot respond(bus reset)!\n");
+				device_printf(rb->fc->bdev, "%s: cannot respond(bus reset)!\n",
+						__func__);
 				return;
 			}
 			rb->xfer = fw_xfer_alloc(M_FWXFER);
@@ -1955,7 +2030,9 @@
 		rb->xfer = STAILQ_FIRST(&bind->xferlist);
 		if (rb->xfer == NULL) {
 #if 1
-			printf("Discard a packet for this bind.\n");
+			device_printf(rb->fc->bdev, "%s: "
+				"Discard a packet for this bind.\n",
+				__func__);
 #endif
 			return;
 		}
@@ -2007,7 +2084,8 @@
 	}
 #endif
 	default:
-		printf("fw_rcv: unknow tcode %d\n", tcode);
+		device_printf(rb->fc->bdev,"%s: unknown tcode %d\n",
+				__func__, tcode);
 		break;
 	}
 }