4 files changed, 97 insertions, 31 deletions

diff --git a/sys/dev/firewire/firewire.c b/sys/dev/firewire/firewire.c
index 71a69fc4a428..9c43eda86602 100644
--- a/sys/dev/firewire/firewire.c
+++ b/sys/dev/firewire/firewire.c
@@ -106,7 +106,6 @@ static device_t firewire_add_child   __P((device_t, int, const char *, int));
 static struct fw_bind *fw_bindlookup __P((struct firewire_comm *, u_int32_t, u_int32_t));
 static void fw_try_bmr __P((void *));
 static void fw_try_bmr_callback __P((struct fw_xfer *));
-static u_int16_t fw_noderesolve __P((struct firewire_comm *, struct fw_eui64));
 static void fw_asystart __P((struct fw_xfer *));
 static int fw_get_tlabel __P((struct firewire_comm *, struct fw_xfer *));
 static void fw_bus_probe __P((struct firewire_comm *));
@@ -977,13 +976,14 @@ 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:
-			xfer->dst = fw_noderesolve(sc->fc, asyreq->req.dst.eui);
-			if(xfer->dst == FWNODE_INVAL ){
+			fwdev = fw_noderesolve(sc->fc, asyreq->req.dst.eui);
+			if (fwdev == NULL) {
 				printf("%s:cannot found node\n",
 					device_get_nameunit(sc->fc->dev));
 				err = EINVAL;
 				goto error;
 			}
+			xfer->dst = fwdev->dst;
 			fp->mode.hdr.dst = htons(FWLOCALBUS | xfer->dst);
 			break;
 		case FWASRESTL:
@@ -1154,7 +1154,7 @@ fw_poll(dev_t dev, int events, fw_proc *td)
 /*
  * To lookup node id. from EUI64.
  */
-u_int16_t
+struct fw_device *
 fw_noderesolve(struct firewire_comm *fc, struct fw_eui64 eui)
 {
 	struct fw_device *fwdev;
@@ -1164,9 +1164,9 @@ fw_noderesolve(struct firewire_comm *fc, struct fw_eui64 eui)
 			break;
 		}
 	}
-	if(fwdev == NULL) return FWNODE_INVAL;
-	if(fwdev->status != FWDEVATTACHED) return FWNODE_INVAL;
-	return fwdev->dst;
+	if(fwdev == NULL) return NULL;
+	if(fwdev->status == FWDEVINVAL) return NULL;
+	return fwdev;
 }
 /*
  * Async. request procedure for userland application.
@@ -2182,6 +2182,7 @@ loop:
 		fwdev = malloc(sizeof(struct fw_device), M_DEVBUF, M_DONTWAIT);
 		if(fwdev == NULL)
 			return;
+		fwdev->fc = fc;
 		fwdev->rommax = 0;
 		fwdev->dst = fc->ongonode;
 		fwdev->eui.hi = fc->ongoeui.hi; fwdev->eui.lo = fc->ongoeui.lo;
diff --git a/sys/dev/firewire/firewirereg.h b/sys/dev/firewire/firewirereg.h
index d84e25c3637f..17e1aead6996 100644
--- a/sys/dev/firewire/firewirereg.h
+++ b/sys/dev/firewire/firewirereg.h
@@ -58,14 +58,16 @@ struct fw_device{
 	int rommax;	/* offset from 0xffff f000 0000 */
 	u_int32_t csrrom[CSRROMSIZE/4];
 	int rcnt;
-	device_t dev;
+	struct firewire_comm *fc;
 	u_int32_t status;
 #define FWDEVINIT	1
 #define FWDEVATTACHED	2
 #define FWDEVINVAL	3
 	TAILQ_ENTRY(fw_device) link;
+#if 0
 	LIST_HEAD(, fw_xfer) txqueue;
 	LIST_HEAD(, fw_xfer) rxqueue;
+#endif
 };
 struct firewire_softc {
 #if __FreeBSD_version >= 500000
@@ -310,6 +312,7 @@ 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));
 
 extern int firewire_debug;
 extern devclass_t firewire_devclass;
diff --git a/sys/dev/firewire/fwmem.c b/sys/dev/firewire/fwmem.c
index 15181a6e3352..c2130e58ecf2 100644
--- a/sys/dev/firewire/fwmem.c
+++ b/sys/dev/firewire/fwmem.c
@@ -54,12 +54,15 @@
 #include <dev/firewire/firewirereg.h>
 #include <dev/firewire/fwmem.h>
 
-static int fwmem_node=0, fwmem_speed=2, fwmem_debug=0;
+static int fwmem_speed=2, fwmem_debug=0;
+static struct fw_eui64 fwmem_eui64;
 SYSCTL_DECL(_hw_firewire);
 SYSCTL_NODE(_hw_firewire, OID_AUTO, fwmem, CTLFLAG_RD, 0,
 	"Firewire Memory Access");
-SYSCTL_INT(_hw_firewire_fwmem, OID_AUTO, node, CTLFLAG_RW, &fwmem_node, 0,
-	"Fwmem target node");
+SYSCTL_UINT(_hw_firewire_fwmem, OID_AUTO, eui64_hi, CTLFLAG_RW,
+	&fwmem_eui64.hi, 0, "Fwmem target EUI64 high");
+SYSCTL_UINT(_hw_firewire_fwmem, OID_AUTO, eui64_low, CTLFLAG_RW,
+	&fwmem_eui64.lo, 0, "Fwmem target EUI64 low");
 SYSCTL_INT(_hw_firewire_fwmem, OID_AUTO, speed, CTLFLAG_RW, &fwmem_speed, 0,
 	"Fwmem link speed");
 SYSCTL_INT(_debug, OID_AUTO, fwmem_debug, CTLFLAG_RW, &fwmem_debug, 0,
@@ -67,9 +70,9 @@ SYSCTL_INT(_debug, OID_AUTO, fwmem_debug, CTLFLAG_RW, &fwmem_debug, 0,
 
 struct fw_xfer *
 fwmem_read_quad(
-	struct firewire_comm *fc,
+	struct fw_device *fwdev,
+	caddr_t	sc,
 	u_int8_t spd,
-	int dst,
 	u_int16_t dst_hi,
 	u_int32_t dst_lo,
 	void (*hand)(struct fw_xfer *))
@@ -81,8 +84,8 @@ fwmem_read_quad(
 	if (xfer == NULL)
 		return NULL;
 
-	xfer->fc = fc;
-	xfer->dst = FWLOCALBUS | dst;
+	xfer->fc = fwdev->fc;
+	xfer->dst = FWLOCALBUS | fwdev->dst;
 	xfer->spd = spd;
 	xfer->send.len = 12;
 	xfer->send.buf = malloc(xfer->send.len, M_DEVBUF, M_NOWAIT | M_ZERO);
@@ -102,9 +105,59 @@ fwmem_read_quad(
 	fp->mode.rreqq.dest_lo = htonl(dst_lo);
 
 	if (fwmem_debug)
-		printf("fwmem: %d %04x:%08x\n", dst, dst_hi, dst_lo);
+		printf("fwmem: %d %04x:%08x\n", fwdev->dst, dst_hi, dst_lo);
 
-	if (fw_asyreq(fc, -1, xfer) == 0)
+	if (fw_asyreq(xfer->fc, -1, xfer) == 0)
+		return xfer;
+
+error:
+	fw_xfer_free(xfer);
+	return NULL;
+}
+
+struct fw_xfer *
+fwmem_write_quad(
+	struct fw_device *fwdev,
+	caddr_t	sc,
+	u_int8_t spd,
+	u_int16_t dst_hi,
+	u_int32_t dst_lo,
+	u_int32_t data,
+	void (*hand)(struct fw_xfer *))
+{
+	struct fw_xfer *xfer;
+	struct fw_pkt *fp;
+
+	xfer = fw_xfer_alloc();
+	if (xfer == NULL)
+		return NULL;
+
+	xfer->fc = fwdev->fc;
+	xfer->dst = FWLOCALBUS | fwdev->dst;
+	xfer->spd = spd;
+	xfer->send.len = 16;
+	xfer->send.buf = malloc(xfer->send.len, M_DEVBUF, M_NOWAIT | M_ZERO);
+
+	if (xfer->send.buf == NULL)
+		goto error;
+
+	xfer->send.off = 0; 
+	xfer->act.hand = hand;
+	xfer->retry_req = fw_asybusy;
+	xfer->sc = sc;
+
+	fp = (struct fw_pkt *)xfer->send.buf;
+	fp->mode.wreqq.tcode = FWTCODE_RREQQ;
+	fp->mode.wreqq.dst = htons(xfer->dst);
+	fp->mode.wreqq.dest_hi = htons(dst_hi);
+	fp->mode.wreqq.dest_lo = htonl(dst_lo);
+
+	fp->mode.wreqq.data = htonl(data);
+
+	if (fwmem_debug)
+		printf("fwmem: %d %04x:%08x\n", fwdev->dst, dst_hi, dst_lo);
+
+	if (fw_asyreq(xfer->fc, -1, xfer) == 0)
 		return xfer;
 
 error:
@@ -114,9 +167,9 @@ error:
 
 struct fw_xfer *
 fwmem_read_block(
-	struct firewire_comm *fc,
+	struct fw_device *fwdev,
+	caddr_t	sc,
 	u_int8_t spd,
-	int dst,
 	u_int16_t dst_hi,
 	u_int32_t dst_lo,
 	int len,
@@ -129,8 +182,8 @@ fwmem_read_block(
 	if (xfer == NULL)
 		return NULL;
 
-	xfer->fc = fc;
-	xfer->dst = FWLOCALBUS | dst;
+	xfer->fc = fwdev->fc;
+	xfer->dst = FWLOCALBUS | fwdev->dst;
 	xfer->spd = spd;
 	xfer->send.len = 16;
 	xfer->send.buf = malloc(xfer->send.len, M_DEVBUF, M_NOWAIT | M_ZERO);
@@ -151,8 +204,9 @@ fwmem_read_block(
 	fp->mode.rreqb.len = htons(len);
 
 	if (fwmem_debug)
-		printf("fwmem: %d %04x:%08x %d\n", dst, dst_hi, dst_lo, len);
-	if (fw_asyreq(fc, -1, xfer) == 0)
+		printf("fwmem: %d %04x:%08x %d\n", fwdev->dst,
+				dst_hi, dst_lo, len);
+	if (fw_asyreq(xfer->fc, -1, xfer) == 0)
 		return xfer;
 
 error:
@@ -180,7 +234,7 @@ int
 fwmem_read (dev_t dev, struct uio *uio, int ioflag)
 {
 	struct firewire_softc *sc;
-	struct firewire_comm *fc;
+	struct fw_device *fwdev;
 	struct fw_xfer *xfer;
 	int err = 0, pad;
         int unit = DEV2UNIT(dev);
@@ -190,7 +244,12 @@ fwmem_read (dev_t dev, struct uio *uio, int ioflag)
 	int len;
 
 	sc = devclass_get_softc(firewire_devclass, unit);
-	fc = sc->fc;
+	fwdev = fw_noderesolve(sc->fc, fwmem_eui64);
+	if (fwdev == NULL) {
+		printf("fwmem: no such device ID:%08x%08x\n",
+			fwmem_eui64.hi, fwmem_eui64.lo);
+		return EINVAL;
+	}
 
 	pad = uio->uio_offset % 4;
 	if  (fwmem_debug && pad != 0)
@@ -201,7 +260,7 @@ fwmem_read (dev_t dev, struct uio *uio, int ioflag)
 		dst_hi = (offset >> 32) & 0xffff;
 		dst_lo = offset & 0xffffffff;
 #if USE_QUAD
-		xfer = fwmem_read_quad(fc, fwmem_speed, fwmem_node,
+		xfer = fwmem_read_quad(fwdev, NULL, fwmem_speed,
 				dst_hi, dst_lo, fw_asy_callback);
 		if (xfer == NULL)
 			return EINVAL;
@@ -214,7 +273,7 @@ fwmem_read (dev_t dev, struct uio *uio, int ioflag)
 		len = uio->uio_resid;
 		if (len > MAXLEN)
 			len = MAXLEN;
-		xfer = fwmem_read_block(fc, fwmem_speed, fwmem_node,
+		xfer = fwmem_read_block(fwdev, NULL, fwmem_speed,
 				dst_hi, dst_lo, len, fw_asy_callback);
 		if (xfer == NULL)
 			return EINVAL;
diff --git a/sys/dev/firewire/fwmem.h b/sys/dev/firewire/fwmem.h
index 753fbd5c1886..5f395081f22e 100644
--- a/sys/dev/firewire/fwmem.h
+++ b/sys/dev/firewire/fwmem.h
@@ -34,10 +34,13 @@
  * $FreeBSD$
  */
 
-struct fw_xfer *fwmem_read_quad(struct firewire_comm *, u_int8_t, int,
-			u_int16_t, u_int32_t, void (*)(struct fw_xfer *));
-struct fw_xfer *fwmem_read_block(struct firewire_comm *, u_int8_t, int,
-			u_int16_t, u_int32_t, int, void (*)(struct fw_xfer *));
+struct fw_xfer *fwmem_read_quad(struct fw_device *, caddr_t, u_int8_t,
+	u_int16_t, u_int32_t, void (*)(struct fw_xfer *));
+struct fw_xfer *fwmem_write_quad(struct fw_device *, caddr_t, u_int8_t,
+	u_int16_t, u_int32_t, u_int32_t, void (*)(struct fw_xfer *));
+struct fw_xfer *fwmem_read_block(struct fw_device *, caddr_t, u_int8_t,
+	u_int16_t, u_int32_t, int, void (*)(struct fw_xfer *));
+
 d_open_t	fwmem_open;
 d_close_t	fwmem_close;
 d_ioctl_t	fwmem_ioctl;