4 files changed, 92 insertions, 61 deletions

diff --git a/sys/dev/firewire/firewire.c b/sys/dev/firewire/firewire.c
index 861006f95153..ab4ed4682577 100644
--- a/sys/dev/firewire/firewire.c
+++ b/sys/dev/firewire/firewire.c
@@ -505,7 +505,7 @@ firewire_shutdown( device_t dev )
 
 
 static void
-firewire_xferq_drain(struct fw_xferq *xferq)
+fw_xferq_drain(struct fw_xferq *xferq)
 {
 	struct fw_xfer *xfer;
 
@@ -524,6 +524,17 @@ firewire_xferq_drain(struct fw_xferq *xferq)
 	}
 }
 
+void
+fw_drain_txq(struct firewire_comm *fc)
+{
+	int i;
+
+	fw_xferq_drain(fc->atq);
+	fw_xferq_drain(fc->ats);
+	for(i = 0; i < fc->nisodma; i++)
+		fw_xferq_drain(fc->it[i]);
+}
+
 /*
  * Called after bus reset.
  */
@@ -540,12 +551,6 @@ fw_busreset(struct firewire_comm *fc)
 		break;
 	}
 	fc->status = FWBUSRESET;
-/* XXX: discard all queued packet */
-	firewire_xferq_drain(fc->atq);
-	firewire_xferq_drain(fc->ats);
-	for(i = 0; i < fc->nisodma; i++)
-		firewire_xferq_drain(fc->it[i]);
-
 	CSRARC(fc, STATE_CLEAR)
 			= 1 << 23 | 0 << 17 | 1 << 16 | 1 << 15 | 1 << 14 ;
 	CSRARC(fc, STATE_SET) = CSRARC(fc, STATE_CLEAR);
@@ -1772,7 +1777,7 @@ fw_rcv(struct firewire_comm* fc, caddr_t buf, u_int len, u_int sub, u_int off, u
 				ntohl(fp->mode.rreqq.dest_lo),
 				fp->mode.common.tcode);
 			if (fc->status == FWBUSRESET) {
-				printf("fw_rcv: cannot response(bus reset)!\n");
+				printf("fw_rcv: cannot respond(bus reset)!\n");
 				goto err;
 			}
 			xfer = fw_xfer_alloc(M_FWXFER);
diff --git a/sys/dev/firewire/firewirereg.h b/sys/dev/firewire/firewirereg.h
index 4631e0037047..34f4cbbc9cfd 100644
--- a/sys/dev/firewire/firewirereg.h
+++ b/sys/dev/firewire/firewirereg.h
@@ -326,6 +326,7 @@ void fw_asy_callback __P((struct fw_xfer *));
 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));
+void fw_drain_txq __P((struct firewire_comm *));
 
 
 extern int firewire_debug;
diff --git a/sys/dev/firewire/fwohci.c b/sys/dev/firewire/fwohci.c
index 86640ed1c768..3e48b377cb71 100644
--- a/sys/dev/firewire/fwohci.c
+++ b/sys/dev/firewire/fwohci.c
@@ -1774,27 +1774,10 @@ fwohci_intr_body(struct fwohci_softc *sc, u_int32_t stat, int count)
 		OWRITE(sc,  OHCI_ATSCTLCLR, OHCI_CNTL_DMA_RUN);
 		sc->atrs.xferq.flag &= ~FWXFERQ_RUNNING;
 
-#if 0
-		for( i = 0 ; i < fc->nisodma ; i ++ ){
-			OWRITE(sc,  OHCI_IRCTLCLR(i), OHCI_CNTL_DMA_RUN);
-			OWRITE(sc,  OHCI_ITCTLCLR(i), OHCI_CNTL_DMA_RUN);
-		}
-
-#endif
-		fw_busreset(fc);
-
-		/* XXX need to wait DMA to stop */
 #ifndef ACK_ALL
 		OWRITE(sc, FWOHCI_INTSTATCLR, OHCI_INT_PHY_BUS_R);
 #endif
-#if 0
-		/* pending all pre-bus_reset packets */
-		fwohci_txd(sc, &sc->atrq);
-		fwohci_txd(sc, &sc->atrs);
-		fwohci_arcv(sc, &sc->arrs, -1);
-		fwohci_arcv(sc, &sc->arrq, -1);
-#endif
-
+		fw_busreset(fc);
 
 		OWRITE(sc, OHCI_AREQHI, 1 << 31);
 		/* XXX insecure ?? */
@@ -1902,6 +1885,7 @@ fwohci_intr_body(struct fwohci_softc *sc, u_int32_t stat, int count)
 		fwohci_txd(sc, &sc->atrs);
 		fwohci_arcv(sc, &sc->arrs, -1);
 		fwohci_arcv(sc, &sc->arrq, -1);
+		fw_drain_txq(fc);
 #endif
 		fw_sidrcv(fc, buf, plen, 0);
 	}
@@ -2564,11 +2548,11 @@ device_printf(sc->fc.dev, "%04x %2x 0x%08x 0x%08x 0x%08x 0x%08x\n", len,
 	fwohci_irx_enable(fc, dmach);
 }
 
-#define PLEN(x)	(((ntohs(x))+0x3) & ~0x3)
+#define PLEN(x)	roundup2(ntohs(x), sizeof(u_int32_t))
 static int
-fwohci_get_plen(struct fwohci_softc *sc, struct fw_pkt *fp, int hlen)
+fwohci_get_plen(struct fwohci_softc *sc, struct fwohci_dbch *dbch, struct fw_pkt *fp, int hlen)
 {
-	int i;
+	int i, r;
 
 	for( i = 4; i < hlen ; i+=4){
 		fp->mode.ld[i/4] = htonl(fp->mode.ld[i/4]);
@@ -2576,32 +2560,49 @@ fwohci_get_plen(struct fwohci_softc *sc, struct fw_pkt *fp, int hlen)
 
 	switch(fp->mode.common.tcode){
 	case FWTCODE_RREQQ:
-		return sizeof(fp->mode.rreqq) + sizeof(u_int32_t);
+		r = sizeof(fp->mode.rreqq) + sizeof(u_int32_t);
+		break;
 	case FWTCODE_WRES:
-		return sizeof(fp->mode.wres) + sizeof(u_int32_t);
+		r = sizeof(fp->mode.wres) + sizeof(u_int32_t);
+		break;
 	case FWTCODE_WREQQ:
-		return sizeof(fp->mode.wreqq) + sizeof(u_int32_t);
+		r = sizeof(fp->mode.wreqq) + sizeof(u_int32_t);
+		break;
 	case FWTCODE_RREQB:
-		return sizeof(fp->mode.rreqb) + sizeof(u_int32_t);
+		r = sizeof(fp->mode.rreqb) + sizeof(u_int32_t);
+		break;
 	case FWTCODE_RRESQ:
-		return sizeof(fp->mode.rresq) + sizeof(u_int32_t);
+		r = sizeof(fp->mode.rresq) + sizeof(u_int32_t);
+		break;
 	case FWTCODE_WREQB:
-		return sizeof(struct fw_asyhdr) + PLEN(fp->mode.wreqb.len)
+		r = sizeof(struct fw_asyhdr) + PLEN(fp->mode.wreqb.len)
 						+ sizeof(u_int32_t);
+		break;
 	case FWTCODE_LREQ:
-		return sizeof(struct fw_asyhdr) + PLEN(fp->mode.lreq.len)
+		r = sizeof(struct fw_asyhdr) + PLEN(fp->mode.lreq.len)
 						+ sizeof(u_int32_t);
+		break;
 	case FWTCODE_RRESB:
-		return sizeof(struct fw_asyhdr) + PLEN(fp->mode.rresb.len)
+		r = sizeof(struct fw_asyhdr) + PLEN(fp->mode.rresb.len)
 						+ sizeof(u_int32_t);
+		break;
 	case FWTCODE_LRES:
-		return sizeof(struct fw_asyhdr) + PLEN(fp->mode.lres.len)
+		r = sizeof(struct fw_asyhdr) + PLEN(fp->mode.lres.len)
 						+ sizeof(u_int32_t);
+		break;
 	case FWOHCITCODE_PHY:
-		return 16;
+		r = 16;
+		break;
+	default:
+		device_printf(sc->fc.dev, "Unknown tcode %d\n",
+						fp->mode.common.tcode);
+		r = 0;
 	}
-	device_printf(sc->fc.dev, "Unknown tcode %d\n", fp->mode.common.tcode);
-	return 0;
+	if (r > dbch->xferq.psize) {
+		device_printf(sc->fc.dev, "Invalid packet length %d\n", r);
+		/* panic ? */
+	}
+	return r;
 }
 
 static void
@@ -2655,7 +2656,8 @@ fwohci_arcv(struct fwohci_softc *sc, struct fwohci_dbch *dbch, int count)
 #endif
 					fp=(struct fw_pkt *)dbch->frag.buf;
 					dbch->frag.plen
-						= fwohci_get_plen(sc, fp, hlen);
+						= fwohci_get_plen(sc,
+							dbch, fp, hlen);
 					if (dbch->frag.plen == 0)
 						goto out;
 				}
@@ -2695,7 +2697,8 @@ fwohci_arcv(struct fwohci_softc *sc, struct fwohci_dbch *dbch, int count)
 					goto out;
 				}
 				if (len >= hlen) {
-					plen = fwohci_get_plen(sc, fp, hlen);
+					plen = fwohci_get_plen(sc,
+							dbch, fp, hlen);
 					if (plen == 0)
 						goto out;
 					plen = (plen + 3) & ~3;
@@ -2705,8 +2708,7 @@ fwohci_arcv(struct fwohci_softc *sc, struct fwohci_dbch *dbch, int count)
 					len -= hlen;
 				}
 				if(resCount > 0 || len > 0){
-					buf = malloc( dbch->xferq.psize,
-							M_FW, M_NOWAIT);
+					buf = malloc(plen, M_FW, M_NOWAIT);
 					if(buf == NULL){
 						printf("cannot malloc!\n");
 						free(db_tr->buf, M_FW);
diff --git a/sys/dev/firewire/sbp.c b/sys/dev/firewire/sbp.c
index e182477d4fe2..f8bc93b77ff1 100644
--- a/sys/dev/firewire/sbp.c
+++ b/sys/dev/firewire/sbp.c
@@ -474,7 +474,7 @@ END_DEBUG
 		reg = crom_search_key(&cc, CROM_LUN);
 		if (reg == NULL)
 			break;
-		lun = reg->val & 0xff;
+		lun = reg->val & 0xffff;
 SBP_DEBUG(0)
 		printf("target %d lun %d found\n", target->target_id, lun);
 END_DEBUG
@@ -482,10 +482,11 @@ END_DEBUG
 			maxlun = lun;
 		crom_next(&cc);
 	}
-	target->num_lun = maxlun + 1;
-	if (maxlun < 0) {
+	if (maxlun < 0)
 		printf("no lun found!\n");
-	}
+	if (maxlun >= SBP_NUM_LUNS)
+		maxlun = SBP_NUM_LUNS;
+	target->num_lun = maxlun + 1;
 	target->luns = (struct sbp_dev *) malloc(
 				sizeof(struct sbp_dev) * target->num_lun, 
 				M_SBP, M_NOWAIT | M_ZERO);
@@ -501,13 +502,17 @@ END_DEBUG
 		reg = crom_search_key(&cc, CROM_LUN);
 		if (reg == NULL)
 			break;
-		lun = reg->val & 0xff;
+		lun = reg->val & 0xffff;
+		if (lun >= SBP_NUM_LUNS) {
+			printf("too large lun %d\n", lun);
+			continue;
+		}
 		target->luns[lun].status = SBP_DEV_RESET;
-		target->luns[lun].type = (reg->val & 0x0f00) >> 16;
+		target->luns[lun].type = (reg->val & 0xf0000) >> 16;
 		crom_next(&cc);
-	}
-	return target;
-}
+	    }
+	    return target;
+    }
 
 static void
 sbp_get_text_leaf(struct fw_device *fwdev, int key, char *buf, int len)
@@ -519,15 +524,16 @@ sbp_get_text_leaf(struct fw_device *fwdev, int key, char *buf, int len)
 	u_int32_t *src, *dst;
 
 	chdr = (struct csrhdr *)&fwdev->csrrom[0];
-	creg = (struct csrreg *)chdr;
-	creg += chdr->info_len;
-	for( i = chdr->info_len + 4; i <= fwdev->rommax; i+=4){
+	/* skip crom header, bus info and root directory */
+	creg = (struct csrreg *)chdr + chdr->info_len + 2;
+	/* search unitl the one before the last. */
+	for (i = chdr->info_len + 2; i < fwdev->rommax / 4; i++) {
 		if((creg++)->key == key){
 			found = 1;
 			break;
 		}
 	}
-	if (!found) {
+	if (!found || creg->key != CROM_TEXTLEAF) {
 		strncpy(buf, nullstr, len);
 		return;
 	}
@@ -780,7 +786,13 @@ END_DEBUG
 static void
 sbp_cam_scan_lun(struct sbp_dev *sdev)
 {
-	union ccb *ccb = malloc(sizeof(union ccb), M_SBP, M_ZERO);
+	union ccb *ccb;
+
+	ccb = malloc(sizeof(union ccb), M_SBP, M_NOWAIT | M_ZERO);
+	if (ccb == NULL) {
+		printf("sbp_cam_scan_lun: malloc failed\n");
+		return;
+	}
 
 SBP_DEBUG(0)
 	sbp_show_sdev_info(sdev, 2);
@@ -840,9 +852,20 @@ sbp_ping_unit(struct sbp_dev *sdev)
 	union ccb *ccb;
 	struct scsi_inquiry_data *inq_buf;
 
-	ccb = malloc(sizeof(union ccb), M_SBP, M_ZERO);
+
+	ccb = malloc(sizeof(union ccb), M_SBP, M_NOWAIT | M_ZERO);
+	if (ccb == NULL) {
+		printf("sbp_ping_unit: malloc failed\n");
+		return;
+	}
+
 	inq_buf = (struct scsi_inquiry_data *)
-			malloc(sizeof(*inq_buf), M_SBP, 0);
+			malloc(sizeof(*inq_buf), M_SBP, M_NOWAIT);
+	if (inq_buf == NULL) {
+		free(ccb, M_SBP);
+		printf("sbp_ping_unit: malloc failed\n");
+		return;
+	}
 
 SBP_DEBUG(0)
 	sbp_show_sdev_info(sdev, 2);