From 8a45d99b2af793b404bf2d63c0653286de50e41f Mon Sep 17 00:00:00 2001 From: Hidetoshi Shimokawa Date: Tue, 4 Feb 2003 17:09:59 +0000 Subject: [PATCH] - Implement write part. - Use quad access for aligned 4 byte access. --- sys/dev/firewire/fwmem.c | 150 +++++++++++++++++++++++++++++++-------- sys/dev/firewire/fwmem.h | 2 + 2 files changed, 122 insertions(+), 30 deletions(-) diff --git a/sys/dev/firewire/fwmem.c b/sys/dev/firewire/fwmem.c index b755dd2bb62b..f19b296c23cb 100644 --- a/sys/dev/firewire/fwmem.c +++ b/sys/dev/firewire/fwmem.c @@ -210,6 +210,43 @@ fwmem_read_block( return NULL; } +struct fw_xfer * +fwmem_write_block( + struct fw_device *fwdev, + caddr_t sc, + u_int8_t spd, + u_int16_t dst_hi, + u_int32_t dst_lo, + int len, + char *data, + void (*hand)(struct fw_xfer *)) +{ + struct fw_xfer *xfer; + struct fw_pkt *fp; + + xfer = fwmem_xfer_req(fwdev, sc, spd, 16 + roundup(len, 4), hand); + if (xfer == NULL) + return NULL; + + fp = (struct fw_pkt *)xfer->send.buf; + fp->mode.wreqb.tcode = FWTCODE_RREQB; + fp->mode.wreqb.dst = htons(xfer->dst); + fp->mode.wreqb.dest_hi = htons(dst_hi); + fp->mode.wreqb.dest_lo = htonl(dst_lo); + fp->mode.wreqb.len = htons(len); + bcopy(data, &fp->mode.wreqb.payload[0], len); + + if (fwmem_debug) + printf("fwmem_write_block: %d %04x:%08x %d\n", fwdev->dst, + dst_hi, dst_lo, len); + if (fw_asyreq(xfer->fc, -1, xfer) == 0) + return xfer; + + fw_xfer_free(xfer); + return NULL; +} + + int fwmem_open (dev_t dev, int flags, int fmt, fw_proc *td) { @@ -232,7 +269,7 @@ fwmem_read (dev_t dev, struct uio *uio, int ioflag) struct firewire_softc *sc; struct fw_device *fwdev; struct fw_xfer *xfer; - int err = 0, pad; + int err = 0; int unit = DEV2UNIT(dev); u_int16_t dst_hi; u_int32_t dst_lo; @@ -247,50 +284,103 @@ fwmem_read (dev_t dev, struct uio *uio, int ioflag) return EINVAL; } - pad = uio->uio_offset % 4; - if (fwmem_debug && pad != 0) - printf("unaligned\n"); while(uio->uio_resid > 0) { offset = uio->uio_offset; - offset -= pad; dst_hi = (offset >> 32) & 0xffff; dst_lo = offset & 0xffffffff; -#if USE_QUAD - xfer = fwmem_read_quad(fwdev, NULL, fwmem_speed, - dst_hi, dst_lo, fw_asy_callback); - if (xfer == NULL) - return EINVAL; - err = tsleep((caddr_t)xfer, FWPRI, "fwmem", hz); - if (err !=0 || xfer->resp != 0 || xfer->recv.buf == NULL) - return EINVAL; /* XXX */ - err = uiomove(xfer->recv.buf + xfer->recv.off + 4*3 + pad, - 4 - pad, uio); -#else len = uio->uio_resid; - if (len > MAXLEN) - len = MAXLEN; - xfer = fwmem_read_block(fwdev, NULL, fwmem_speed, + if (len == 4 && (dst_lo & 3) == 0) { + xfer = fwmem_read_quad(fwdev, NULL, fwmem_speed, + dst_hi, dst_lo, fw_asy_callback); + if (xfer == NULL) + return EINVAL; + err = tsleep((caddr_t)xfer, FWPRI, "fwmrq", hz); + if (err !=0 || xfer->resp != 0 + || xfer->recv.buf == NULL) + return EINVAL; /* XXX */ + err = uiomove(xfer->recv.buf + xfer->recv.off + 4*3, + 4, uio); + } else { + if (len > MAXLEN) + len = MAXLEN; + xfer = fwmem_read_block(fwdev, NULL, fwmem_speed, dst_hi, dst_lo, len, fw_asy_callback); - if (xfer == NULL) - return EINVAL; - err = tsleep((caddr_t)xfer, FWPRI, "fwmem", hz); - if (err != 0 || xfer->resp != 0 || xfer->recv.buf == NULL) - return EINVAL; /* XXX */ - err = uiomove(xfer->recv.buf + xfer->recv.off + 4*4 + pad, - len - pad, uio); -#endif + if (xfer == NULL) + return EINVAL; + err = tsleep((caddr_t)xfer, FWPRI, "fwmrb", hz); + if (err != 0 || xfer->resp != 0 + || xfer->recv.buf == NULL) + return EINVAL; /* XXX */ + err = uiomove(xfer->recv.buf + xfer->recv.off + 4*4, + len, uio); + } + fw_xfer_free(xfer); if (err) return err; - fw_xfer_free(xfer); - pad = 0; } return err; } int fwmem_write (dev_t dev, struct uio *uio, int ioflag) { - return EINVAL; + struct firewire_softc *sc; + struct fw_device *fwdev; + struct fw_xfer *xfer; + int err = 0; + int unit = DEV2UNIT(dev); + u_int16_t dst_hi; + u_int32_t dst_lo, quad; + char *data; + off_t offset; + int len; + + sc = devclass_get_softc(firewire_devclass, unit); + 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); + return EINVAL; + } + + data = malloc(MAXLEN, M_FW, 0); + if (data == NULL) + return ENOMEM; + + while(uio->uio_resid > 0) { + offset = uio->uio_offset; + dst_hi = (offset >> 32) & 0xffff; + dst_lo = offset & 0xffffffff; + len = uio->uio_resid; + if (len == 4 && (dst_lo & 3) == 0) { + err = uiomove((char *)&quad, sizeof(quad), uio); + xfer = fwmem_write_quad(fwdev, NULL, fwmem_speed, + dst_hi, dst_lo, quad, fw_asy_callback); + if (xfer == NULL) + return EINVAL; + err = tsleep((caddr_t)xfer, FWPRI, "fwmwq", hz); + if (err !=0 || xfer->resp != 0 + || xfer->recv.buf == NULL) + return EINVAL; /* XXX */ + } else { + if (len > MAXLEN) + len = MAXLEN; + err = uiomove(data, len, uio); + if (err) + return err; + xfer = fwmem_write_block(fwdev, NULL, fwmem_speed, + dst_hi, dst_lo, len, data, fw_asy_callback); + if (xfer == NULL) + return EINVAL; + err = tsleep((caddr_t)xfer, FWPRI, "fwmwb", hz); + if (err != 0 || xfer->resp != 0 + || xfer->recv.buf == NULL) + return EINVAL; /* XXX */ + fw_xfer_free(xfer); + } + } + return err; } + int fwmem_ioctl (dev_t dev, u_long cmd, caddr_t data, int flag, fw_proc *td) { diff --git a/sys/dev/firewire/fwmem.h b/sys/dev/firewire/fwmem.h index 5f395081f22e..0e2e83cab34f 100644 --- a/sys/dev/firewire/fwmem.h +++ b/sys/dev/firewire/fwmem.h @@ -40,6 +40,8 @@ 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 *)); +struct fw_xfer *fwmem_write_block(struct fw_device *, caddr_t, u_int8_t, + u_int16_t, u_int32_t, int, char *, void (*)(struct fw_xfer *)); d_open_t fwmem_open; d_close_t fwmem_close;