Add preliminary support for the Serial Bus Protocol II (SBP-2) standard.

As well as a first rough implementation of a SCSI over FireWire support,
following the SBP-2 standard.

5 files changed, 2683 insertions, 0 deletions

diff --git a/sys/dev/ieee1394/fwscsi.c b/sys/dev/ieee1394/fwscsi.c
new file mode 100644
index 00000000000..00328086ba5
--- /dev/null
+++ b/sys/dev/ieee1394/fwscsi.c
@@ -0,0 +1,1163 @@
+/*	$OpenBSD: fwscsi.c,v 1.1 2002/12/13 02:57:50 tdeval Exp $	*/
+
+/*
+ * Copyright (c) 2002 Thierry Deval.  All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
+ * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
+ * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
+ * IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
+ * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+ * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+ * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
+ * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <sys/param.h>
+#include <sys/kernel.h>
+#include <sys/device.h>
+#include <sys/systm.h>
+#include <sys/malloc.h>
+#include <sys/buf.h>
+#include <sys/uio.h>
+#include <sys/types.h>
+#include <sys/proc.h>
+#if 1	/* NO_THREAD */
+#include <sys/kthread.h>
+#endif	/* NO_THREAD */
+#include <sys/timeout.h>
+
+#include <dev/rndvar.h>
+#include <machine/bus.h>
+
+#ifdef	__NetBSD__
+#include <dev/scsipi/scsi_all.h>
+#include <dev/scsipi/scsipi_all.h>
+#include <dev/scsipi/scsiconf.h>
+#else
+#include <scsi/scsi_all.h>
+#include <scsi/scsiconf.h>
+#endif
+
+#include <dev/std/ieee1212reg.h>
+#include <dev/std/ieee1212var.h>
+#include <dev/std/sbp2reg.h>
+#include <dev/std/sbp2var.h>
+#include <dev/ieee1394/ieee1394reg.h>
+#include <dev/ieee1394/ieee1394var.h>
+#include <dev/ieee1394/fwohcivar.h>
+#include <dev/ieee1394/fwnodevar.h>
+#include <dev/ieee1394/fwnodereg.h>
+
+#ifdef	MALLOC_DEBUG
+#include <dev/ieee1394/malloc_debug.h>		/* MPRINTF(x,y) */
+#else	/* !MALLOC_DEBUG */
+#define	MPRINTF(x,y)
+#endif	/* MALLOC_DEBUG */
+
+#ifdef	FWSCSI_DEBUG
+#include <sys/syslog.h>
+extern int log_open;
+int fwscsi_oldlog;
+#define	DPRINTF(x)	if (fwscsidebug&3) do {				\
+	fwscsi_oldlog = log_open; log_open = 1;				\
+	addlog x; log_open = fwscsi_oldlog;				\
+} while (0)
+#define	DPRINTFN(n,x)	if ((fwscsidebug&3)>(n)) do {			\
+	fwscsi_oldlog = log_open; log_open = 1;				\
+	addlog x; log_open = fwscsi_oldlog;				\
+} while (0)
+int	fwscsidebug = 0;
+#else	/* FWSCSI_DEBUG */
+#define	DPRINTF(x)
+#define	DPRINTFN(n,x)
+#endif	/* ! FWSCSI_DEBUG */
+
+#ifdef	__NetBSD__
+int  fwscsi_match(struct device *, struct cfdata *, void *);
+#else
+int  fwscsi_match(struct device *, void *, void *);
+#endif
+void fwscsi_attach(struct device *, struct device *, void *);
+#if 0	/* NO_THREAD */
+void fwscsi_init(void *);
+#else	/* NO_THREAD */
+void fwscsi_config_thread(void *);
+void fwscsi_login_cb(void *, struct sbp2_status_notification *);
+#endif	/* NO_THREAD */
+void fwscsi_agent_init(void *);
+void fwscsi_status_notify(void *, struct sbp2_status_notification *);
+int  fwscsi_detach(struct device *, int);
+#ifdef	__NetBSD__
+void fwscsi_scsipi_request(struct scsipi_channel *, scsipi_adapter_req_t,
+    void *);
+void fwscsi_scsipi_minphys(struct buf *);
+#else
+int  fwscsi_scsi_cmd(struct scsi_xfer *);
+void fwscsi_minphys(struct buf *);
+#endif
+//void fwscsi_cmd_notify(struct sbp2_status_notification *);
+#if 0	/* NO_THREAD */
+void fwscsi_command_wait(void *);
+#else	/* NO_THREAD */
+void fwscsi_command_timeout(void *);
+void fwscsi_command_wait(void *, struct sbp2_status_notification *);
+#endif	/* NO_THREAD */
+void fwscsi_command_data(struct ieee1394_abuf *, int);
+
+typedef struct fwscsi_orb_data {
+	u_int32_t	data_hash;
+	size_t		data_len;
+	caddr_t		data_addr;
+	struct ieee1394_abuf *data_ab;
+	TAILQ_ENTRY(fwscsi_orb_data) data_chain;
+} fwscsi_orb_data;
+
+typedef struct fwscsi_status {
+	u_int8_t	flags;
+	u_int8_t	status;
+	u_int16_t	orb_offset_hi;
+	u_int32_t	orb_offset_lo;
+	u_int8_t	scsi_status;
+	u_int8_t	sense_key;
+#define	FWSCSI_INFO_VALID	0x80
+#define	FWSCSI_MEI		0x70
+#define	FWSCSI_SENSE_KEY	0x0F
+	u_int8_t	sense_code;
+	u_int8_t	sense_qual;
+	u_int32_t	information;
+	u_int32_t	cmd_spec_info;
+	u_int32_t	sense_key_info;
+	u_int32_t	vendor_info[2];
+} fwscsi_status;
+
+#ifdef	__OpenBSD__
+struct scsi_adapter fwscsi_switch = {
+	fwscsi_scsi_cmd,	/* scsi_cmd */
+	fwscsi_minphys,		/* scsi_minphys */
+	NULL,			/* open_target_lu */
+	NULL,			/* close_target_lu */
+	NULL			/* ioctl */
+};
+
+struct scsi_device fwscsi_dev = {
+	NULL,			/* Use default error handler */
+	NULL,			/* have a queue, served by this */
+	NULL,			/* have no async handler */
+	NULL			/* Use default 'done' routine */
+};
+#endif
+
+typedef struct fwscsi_softc {
+	struct device sc_dev;
+#ifdef	__NetBSD__
+	struct scsipi_adapter sc_adapter;
+	struct scsipi_channel sc_channel;
+#else
+	struct scsi_link sc_link;
+#endif
+
+	struct fwnode_softc *sc_fwnode;
+	struct p1212_dir **sc_unitdir;
+
+	u_int8_t sc_speed;
+	u_int32_t sc_maxpayload;
+
+	u_int64_t sc_csrbase;
+	u_int64_t sc_mgmtreg;
+	int sc_loginid;
+	int sc_lun;
+
+	struct device *sc_bus;
+
+	TAILQ_HEAD(, fwscsi_orb_data) sc_data;
+} fwscsi_softc;
+
+struct cfattach fwscsi_ca = {
+	sizeof(struct fwscsi_softc), fwscsi_match, fwscsi_attach, fwscsi_detach
+};
+
+#ifdef	__OpenBSD__
+struct cfdriver fwscsi_cd = {
+	NULL, "fwscsi", DV_DULL
+};
+#endif
+
+struct fwscsi_orb_data *fwscsi_datafind(struct fwscsi_softc *, u_int32_t);
+struct fwscsi_orb_data *
+fwscsi_datafind(struct fwscsi_softc *sc, u_int32_t hash)
+{
+	struct fwscsi_orb_data *data;
+
+	TAILQ_FOREACH(data, &sc->sc_data, data_chain) {
+		if (data->data_hash == hash)
+			break;
+	}
+
+	return (data);
+}
+
+#ifdef	__NetBSD__
+int
+fwscsi_match(struct device *parent, struct cfdata *match, void *aux)
+#else
+int
+fwscsi_match(struct device *parent, void *match, void *aux)
+#endif
+{
+	struct p1212_key **key;
+	struct p1212_dir **udirs = aux;
+
+	key = p1212_find(*udirs, P1212_KEYTYPE_Immediate,
+	    P1212_KEYVALUE_Unit_Spec_Id, 0);
+	if (!key || key[0]->val != SBP2_UNIT_SPEC_ID) {
+		if (key != NULL) {
+			free(key, M_DEVBUF);
+			//MPRINTF_OLD("free(DEVBUF)", key);
+			key = NULL;	/* XXX */
+		}
+		return 0;
+	}
+	free(key, M_DEVBUF);
+	//MPRINTF_OLD("free(DEVBUF)", key);
+	key = NULL;	/* XXX */
+
+	key = p1212_find(*udirs, P1212_KEYTYPE_Immediate,
+	    P1212_KEYVALUE_Unit_Sw_Version, 0);
+	if (!key || key[0]->val != SBP2_UNIT_SW_VERSION) {
+		if (key != NULL) {
+			free(key, M_DEVBUF);
+			//MPRINTF_OLD("free(DEVBUF)", key);
+			key = NULL;	/* XXX */
+		}
+		return 0;
+	}
+	free(key, M_DEVBUF);
+	//MPRINTF_OLD("free(DEVBUF)", key);
+	key = NULL;	/* XXX */
+
+	key = p1212_find(*udirs, P1212_KEYTYPE_Immediate,
+	    SBP2_KEYVALUE_Command_Set_Spec_Id, 0);
+	if (!key || key[0]->val != 0x00609E) {
+		if (key != NULL) {
+			free(key, M_DEVBUF);
+			//MPRINTF_OLD("free(DEVBUF)", key);
+			key = NULL;	/* XXX */
+		}
+		return 0;
+	}
+	free(key, M_DEVBUF);
+	//MPRINTF_OLD("free(DEVBUF)", key);
+	key = NULL;	/* XXX */
+
+	key = p1212_find(*udirs, P1212_KEYTYPE_Immediate,
+	    SBP2_KEYVALUE_Command_Set, 0);
+	if (!key || key[0]->val != 0x0104D8) {
+		if (key != NULL) {
+			free(key, M_DEVBUF);
+			//MPRINTF_OLD("free(DEVBUF)", key);
+			key = NULL;	/* XXX */
+		}
+		return 0;
+	}
+	free(key, M_DEVBUF);
+	//MPRINTF_OLD("free(DEVBUF)", key);
+	key = NULL;	/* XXX */
+
+	return 1;
+}
+
+void
+fwscsi_attach(struct device *parent, struct device *self, void *aux)
+{
+	struct ieee1394_softc *psc = (struct ieee1394_softc *)parent;
+	struct ieee1394_softc *buspsc =
+	    (struct ieee1394_softc *)parent->dv_parent;
+	struct fwscsi_softc *sc = (struct fwscsi_softc *)self;
+	struct p1212_dir **udir = (struct p1212_dir **)aux;
+	int lun, n;
+#if 1	/* NO_THREAD */
+	struct sbp2_login_orb *login_orb;
+#if 0
+	struct sbp2_status_block *status = NULL;
+#endif
+//	int error;
+#endif	/* NO_THREAD */
+
+	DPRINTF(("%s: cpl = %d(%08x)\n", __func__, cpl, cpl));
+
+#ifdef	__NetBSD__
+	sc->sc_adapter.adapt_dev = &sc->sc_dev;
+	sc->sc_adapter.adapt_nchannels = 1;
+	sc->sc_adapter.adapt_max_periph = 1;
+	sc->sc_adapter.adapt_request = fwscsi_scsipi_request;
+	sc->sc_adapter.adapt_minphys = fwscsi_scsipi_minphys;
+	sc->sc_adapter.adapt_openings = 8;
+
+	sc->sc_channel.chan_adapter = &sc->sc_adapter;
+	sc->sc_channel.chan_bustype = &scsi_bustype;
+	sc->sc_channel.chan_channel = 0;
+	sc->sc_channel.chan_flags = SCSIPI_CHAN_CANGROW | SCSIPI_CHAN_NOSETTLE;
+	sc->sc_channel.chan_ntargets = 2;
+	sc->sc_channel.chan_nluns = SBP2_MAX_LUNS;
+	sc->sc_channel.chan_id = 1;
+#else	/* __NetBSD__ */
+	sc->sc_link.adapter_target = 7;
+	sc->sc_link.adapter_buswidth = 8;
+	sc->sc_link.openings = 2;
+	sc->sc_link.device = &fwscsi_dev;
+	sc->sc_link.device_softc = sc;
+	sc->sc_link.adapter = &fwscsi_switch;
+	sc->sc_link.adapter_softc = sc;
+	sc->sc_link.flags |= SDEV_ATAPI;
+#endif	/* ! __NetBSD__ */
+
+	sc->sc_fwnode = (struct fwnode_softc *)parent;
+
+	n = 0;
+	while (udir[n++]) {};
+	sc->sc_unitdir = malloc(n * sizeof(*sc->sc_unitdir), M_DEVBUF,
+	    M_WAITOK);
+	//MPRINTF_OLD("malloc(DEVBUF)", sc->sc_unitdir);
+	bcopy(udir, sc->sc_unitdir, n * sizeof(*sc->sc_unitdir));
+
+	sc->sc_speed = psc->sc1394_link_speed;
+	sc->sc_maxpayload = buspsc->sc1394_max_receive - 1;
+
+	sc->sc_loginid = 0;
+	sc->sc_lun = 0;
+
+	printf("\n");
+	lun = sbp2_init(sc->sc_fwnode, *sc->sc_unitdir);
+	if (lun < 0) {
+		DPRINTF(("%s: initialization failure... (-1)\n", __func__));
+		return;
+	}
+	sc->sc_lun = lun;
+
+#if 0	/* NO_THREAD */
+	if (kthread_create(fwscsi_init, sc, NULL, "%s",
+	    sc->sc_dev.dv_xname))
+	{
+		printf("%s: unable to create init thread\n",
+		    sc->sc_dev.dv_xname);
+	}
+}
+
+void
+fwscsi_init(void *aux)
+{
+	struct device *dev;
+	struct sbp2_login_orb *login_orb;
+	struct fwscsi_softc *sc = (struct fwscsi_softc *)aux;
+#if 0
+	struct sbp2_status_block *status = NULL;
+#endif
+	int error;
+#endif	/* NO_THREAD */
+
+#ifdef	FWSCSI_DEBUG
+	if (fwscsidebug & 4)
+		Debugger();
+#endif	/* FWSCSI_DEBUG */
+
+	login_orb = malloc(sizeof(struct sbp2_login_orb), M_1394DATA, M_WAITOK);
+	//MPRINTF_OLD("malloc(1394DATA)", login_orb);
+	bzero(login_orb, sizeof(struct sbp2_login_orb));
+
+	login_orb->lun = htons(sc->sc_lun);
+#if 0	/* NO_THREAD */
+	sbp2_login(sc->sc_fwnode, login_orb, fwscsi_status_notify);
+#else	/* NO_THREAD */
+	sbp2_login(sc->sc_fwnode, login_orb, fwscsi_login_cb, (void *)sc);
+}
+
+void
+fwscsi_login_cb(void *arg, struct sbp2_status_notification *notification)
+{
+	struct fwscsi_softc * const sc = arg;
+	struct sbp2_login_orb *login_orb =
+	    (struct sbp2_login_orb *)notification->origin;
+#ifdef	FWSCSI_DEBUG
+	struct sbp2_status_block *status = notification->status;
+	int i;
+
+	DPRINTF(("%s: cpl = %d(%08x)\n", __func__, cpl, cpl));
+
+	DPRINTF(("%s: origin=0x%08x csr=0x%016qx", __func__,
+	    (u_int32_t)login_orb,
+	    ((u_int64_t)(ntohs(status->orb_offset_hi)) << 32) +
+	    ntohl(status->orb_offset_lo)));
+
+	for (i = 0; i < sizeof(*status); i++) {
+		DPRINTFN(1, ("%s %02.2x", (i % 16)?"":"\n   ",
+		    ((u_int8_t *)status)[i]));
+	}
+	DPRINTF(("\n"));
+#endif	/* FWSCSI_DEBUG */
+
+	if (login_orb != NULL) {
+		free(login_orb, M_1394DATA);
+		//MPRINTF_OLD("free(1394DATA)", login_orb);
+		login_orb = NULL;	/* XXX */
+	}
+
+	TAILQ_INIT(&sc->sc_data);
+	sc->sc_bus = NULL;
+
+#ifdef	FWSCSI_DEBUG
+	if (fwscsidebug & 4)
+		Debugger();
+#endif	/* FWSCSI_DEBUG */
+
+	if (kthread_create(fwscsi_config_thread, sc, NULL, "%s",
+	    sc->sc_dev.dv_xname))
+	{
+		printf("%s: unable to create config thread\n",
+		    sc->sc_dev.dv_xname);
+	}
+}
+
+void
+fwscsi_config_thread(void *arg)
+{
+	struct device *dev;
+	struct fwscsi_softc * const sc = arg;
+
+#ifdef	__NetBSD__
+	dev = config_found(&sc->sc_dev, &sc->sc_channel, scsiprint);
+#else
+	dev = config_found(&sc->sc_dev, &sc->sc_link, scsiprint);
+#endif
+
+	sc->sc_bus = dev;
+
+	DPRINTF(("%s: exiting...\n", __func__));
+
+	kthread_exit(0);
+#endif	/* NO_THREAD */
+
+#if 0	/* NO_THREAD */
+	error = tsleep(login_orb, PRIBIO, "sbplogin", 5*hz);
+
+	if (error == EWOULDBLOCK) {
+		DPRINTF(("%s: SBP Login failure\n", __func__));
+		free(login_orb, M_1394DATA);
+		//MPRINTF_OLD("free(1394DATA)", login_orb);
+		login_orb = NULL;	/* XXX */
+		kthread_exit(1);
+	}
+
+#if 0
+	status = ((struct sbp2_status_block **)login_orb)[0];
+	if (status != NULL) {
+		free(status, M_1394DATA);
+		//MPRINTF_OLD("free(1394DATA)", status);
+		status = NULL;	/* XXX */
+	}
+#endif
+	free(login_orb, M_1394DATA);
+	//MPRINTF_OLD("free(1394DATA)", login_orb);
+	login_orb = NULL;	/* XXX */
+
+	TAILQ_INIT(&sc->sc_data);
+
+#ifdef	FWSCSI_DEBUG
+	if (fwscsidebug & 4)
+		Debugger();
+#endif	/* FWSCSI_DEBUG */
+
+#ifdef	__NetBSD__
+	dev = config_found(&sc->sc_dev, &sc->sc_channel, scsiprint);
+#else
+	dev = config_found(&sc->sc_dev, &sc->sc_link, scsiprint);
+#endif
+
+	sc->sc_bus = dev;
+
+	DPRINTF(("%s: exiting...\n", __func__));
+	kthread_exit(0);
+#endif	/* NO_THREAD */
+}
+
+void
+fwscsi_status_notify(void *arg, struct sbp2_status_notification *notification)
+{
+	struct sbp2_status_block *status = notification->status;
+	void *wakemeup = notification->origin;
+
+	DPRINTF(("%s: origin=0x%08x csr=0x%016qx\n", __func__,
+	    (u_int32_t)wakemeup,
+	    ((u_int64_t)(ntohs(status->orb_offset_hi)) << 32) +
+	    ntohl(status->orb_offset_lo)));
+
+	if (wakemeup != NULL) {
+		((void **)wakemeup)[0] = status;
+		DPRINTF(("%s: Wake-up 0x%08x\n", __func__,
+		    (u_int32_t)wakemeup));
+		wakeup(wakemeup);
+	}
+}
+
+int
+fwscsi_detach(struct device *self, int flags)
+{
+	struct fwscsi_softc *sc = (struct fwscsi_softc *)self;
+	int s, rv;
+
+	DPRINTF(("%s: cpl = %d(%08x)\n", __func__, cpl, cpl));
+
+	rv = 0;
+
+	if (sc->sc_bus) {
+		DPRINTF(("%s: detach %s\n", __func__,
+		    sc->sc_bus->dv_xname));
+		s = splbio();
+		rv += config_detach(sc->sc_bus, flags);
+		splx(s);
+	}
+	sbp2_clean(sc->sc_fwnode, *sc->sc_unitdir, 0);
+	free(sc->sc_unitdir, M_DEVBUF);
+	//MPRINTF_OLD("free(DEVBUF)", sc->sc_unitdir);
+	sc->sc_unitdir = NULL;	/* XXX */
+
+	return (rv);
+}
+
+#ifdef	__NetBSD__
+void
+fwscsi_scsipi_request(struct scsipi_channel *channel,
+    scsipi_adapter_req_t req, void *arg)
+{
+	/*struct scsipi_adapter *adapt = channel->chan_adapter;
+	struct fwscsi_softc *sc = (struct fwscsi_softc *)adapt->adapt_dev;*/
+	struct scsipi_xfer *xs = arg;
+	int i;
+
+	DPRINTF(("Called fwscsi_scsipi_request\n"));
+
+	switch (req) {
+	case ADAPTER_REQ_RUN_XFER:
+		xs->error = XS_DRIVER_STUFFUP;
+		DPRINTF(("Got req_run_xfer\n"));
+		DPRINTF(("xs control: 0x%08x, timeout: %d\n", xs->xs_control,
+		    xs->timeout));
+		DPRINTF(("opcode: 0x%02x\n", (u_int8_t)xs->cmd->opcode));
+		for (i = 0; i < 15; i++)
+			DPRINTF(("0x%02.2x ",(u_int8_t)xs->cmd->bytes[i]));
+		DPRINTF(("\n"));
+		scsipi_done(xs);
+		break;
+	case ADAPTER_REQ_GROW_RESOURCES:
+		DPRINTF(("Got req_grow_resources\n"));
+		break;
+	case ADAPTER_REQ_SET_XFER_MODE:
+		DPRINTF(("Got set xfer mode\n"));
+		break;
+	default:
+		panic("Unknown request: %d\n", (int)req);
+	}
+}
+#else
+int
+fwscsi_scsi_cmd(struct scsi_xfer *xs)
+{
+	struct sbp2_command_orb *cmd_orb;
+	struct fwscsi_orb_data *data_elm;
+	struct fwscsi_softc *sc =
+	    (struct fwscsi_softc *)xs->sc_link->adapter_softc;
+	struct fwnode_softc *fwsc = sc->sc_fwnode;
+	struct fwohci_softc *ohsc;
+	struct ieee1394_abuf *data_ab;
+	u_int32_t dhash;
+	u_int16_t options, host_id;
+	size_t datalen;
+	int datashift, s;
+#ifdef	FWSCSI_DEBUG
+	struct uio *data_uio;
+	int i;
+
+	DPRINTF(("%s: cpl:%d(%x), xs:0x%08x\n", __func__, cpl, cpl, xs));
+	DPRINTF(("  flags:%05x retries:%d timeout:%d target:%d lun:%d\n",
+	    xs->flags, xs->retries, xs->timeout,
+	    xs->sc_link->target, xs->sc_link->lun));
+	DPRINTF(("  cmd[%d]:", xs->cmdlen));
+	for (i=0; i<xs->cmdlen; i++)
+		DPRINTF((" %02.2x", ((u_int8_t *)xs->cmd)[i]));
+	DPRINTF(("\n  data[%u]:0x%08x", xs->datalen, (caddr_t)xs->data));
+	if (xs->flags & SCSI_DATA_UIO) {
+		data_uio = (struct uio *)xs->data;
+		DPRINTF(("/UIO:0x%08x(%d)/0x%08x", (u_int32_t)data_uio->uio_iov,
+		    data_uio->uio_iovcnt, data_uio->uio_resid));
+	}
+	if (xs->bp != NULL)
+		DPRINTF((" buf[%u/%u]:0x%08x", xs->bp->b_bcount,
+		    xs->bp->b_bufsize, (u_int32_t)xs->bp->b_data));
+	DPRINTF(("\n"));
+	if (xs->flags & SCSI_DATA_UIO) {
+		for (i=0; i<data_uio->uio_iovcnt; i++)
+			DPRINTFN(1,("  uio_segment[%d]: 0x%p(%d)\n", i,
+			    (void *)data_uio->uio_iov[i].iov_base, 
+			    data_uio->uio_iov[i].iov_len));
+	}
+#endif	/* FWSCSI_DEBUG */
+
+	s = splbio();
+
+#if 1	/* NO_THREAD */
+	/* Always reset xs->stimeout, lest we timeout_del() with trash */
+	timeout_set(&xs->stimeout, fwscsi_command_timeout, (void *)xs);
+#endif	/* NO_THREAD */
+	bzero(&xs->sense, sizeof(struct scsi_mode_sense));
+
+	if (xs->sc_link->target != sc->sc_lun || xs->sc_link->lun != 0) {
+		DPRINTF(("    device not available...\n"));
+#if 0
+		xs->error = XS_SENSE;
+#else
+		xs->error = XS_SELTIMEOUT;
+#endif
+		xs->status = SCSI_CHECK;
+		xs->flags |= ITSDONE | SCSI_SILENT;
+		xs->sense.flags = SKEY_ILLEGAL_REQUEST;
+		xs->sense.add_sense_code = 0x25; /* LOGIC UNIT NOT SUPPORTED */
+		xs->sense.error_code = 0x70;
+		scsi_done(xs);
+		splx(s);
+		return (COMPLETE);
+	}
+
+	cmd_orb = malloc(sizeof(struct sbp2_command_orb) + 8,
+	    M_1394DATA, M_NOWAIT);
+	if (cmd_orb == NULL) {
+		printf("%s: can't alloc cmd_orb for target %d lun %d\n",
+		    sc->sc_dev.dv_xname, xs->sc_link->target,
+		    xs->sc_link->lun);
+		xs->error = XS_DRIVER_STUFFUP;
+		splx(s);
+		return (TRY_AGAIN_LATER);
+	}
+	//MPRINTF_OLD("malloc(1394DATA)", cmd_orb);
+	bzero(cmd_orb, sizeof(struct sbp2_command_orb) + 8);
+
+	options = 0x8000 | ((fwsc->sc_sc1394.sc1394_link_speed & 0x7) << 8);
+
+	datalen = 0;
+	if (xs->flags & (SCSI_DATA_IN | SCSI_DATA_OUT)) {
+		ohsc = (struct fwohci_softc *)
+		    ((struct device *)fwsc)->dv_parent;
+		host_id = ohsc->sc_nodeid;
+		DPRINTFN(1, ("%s: host=0x%04hx data=0x%08x[%d/%d]",
+		    __func__, host_id, (u_int32_t)(xs->data),
+		    xs->datalen, xs->resid));
+		datashift = 0;
+		datalen = xs->datalen / 256;
+		while (datalen) {
+			datashift++;
+			datalen /= 2;
+		}
+		do {
+			dhash = arc4random() & (~(1 << datashift) + 1);
+		} while (!dhash || fwscsi_datafind(sc, dhash) != NULL);
+
+		MALLOC(data_elm, struct fwscsi_orb_data *, sizeof(*data_elm),
+		    M_1394CTL, M_NOWAIT);
+		//MPRINTF_OLD("MALLOC(1394CTL)", data_elm);
+		if (data_elm == NULL) {
+			printf("%s: can't alloc data_elm for target %d lun"
+			    " %d\n", sc->sc_dev.dv_xname, xs->sc_link->target,
+			    xs->sc_link->lun);
+			xs->error = XS_DRIVER_STUFFUP;
+			free(cmd_orb, M_1394DATA);
+			//MPRINTF_OLD("free(1394DATA)", cmd_orb);
+			splx(s);
+			return (TRY_AGAIN_LATER);
+		}
+		bzero(data_elm, sizeof(*data_elm));
+
+		data_elm->data_hash = dhash;
+		data_elm->data_addr = xs->data;
+		data_elm->data_len = xs->datalen;
+
+		MALLOC(data_ab, struct ieee1394_abuf *, sizeof(*data_ab),
+		    M_1394DATA, M_NOWAIT);
+		//MPRINTF_OLD("MALLOC(1394DATA)", data_ab);
+		if (data_ab == NULL) {
+			printf("%s: can't alloc data_ab for target %d lun"
+			    " %d\n", sc->sc_dev.dv_xname, xs->sc_link->target,
+			    xs->sc_link->lun);
+			xs->error = XS_DRIVER_STUFFUP;
+			free(data_elm, M_1394CTL);
+			//MPRINTF_OLD("FREE(1394CTL)", data_elm);
+			free(cmd_orb, M_1394DATA);
+			//MPRINTF_OLD("free(1394DATA)", cmd_orb);
+			splx(s);
+			return (TRY_AGAIN_LATER);
+		}
+		bzero(data_ab, sizeof(*data_ab));
+
+		data_ab->ab_req = (struct ieee1394_softc *)sc->sc_fwnode;
+		data_ab->ab_retlen = 0;
+		datalen = roundup(xs->datalen, 4);
+		data_ab->ab_length = datalen & 0xffff;
+		data_ab->ab_addr = SBP2_CMD_DATA + ((u_int64_t)dhash << 8);
+		data_ab->ab_cb = fwscsi_command_data;
+		data_ab->ab_cbarg = xs;
+
+		TAILQ_INSERT_TAIL(&sc->sc_data, data_elm, data_chain);
+		xs->data = (u_char *)data_elm;
+
+		/* Check direction of data transfer */
+		if (xs->flags & SCSI_DATA_OUT) {
+			data_ab->ab_tcode =
+			    IEEE1394_TCODE_READ_REQUEST_DATABLOCK;
+#if 1
+			options |=
+			    ((sc->sc_fwnode->sc_sc1394.sc1394_max_receive - 1)
+			     & 0xF) << 4;
+#else
+			options |= 0x9 << 4;	/* 2048 max payload	*/
+#endif
+			DPRINTFN(1, (" -- OUT(%d/%X)\n", datalen,
+			    (options >> 4) & 0xf));
+		} else {
+			data_ab->ab_tcode =
+			    IEEE1394_TCODE_WRITE_REQUEST_DATABLOCK;
+			options |= 0x0800;
+#if 0
+			options |= (sc->sc_maxpayload & 0xF) << 4;
+			options |= ((sc->sc_fwnode->sc_sc1394.sc1394_max_receive -1) & 0xF) << 4;
+#else
+			options |= 0x9 << 4;	/* 2048 max payload	*/
+#endif
+			DPRINTFN(1, (" -- IN(%d/%X)\n", datalen,
+			    (options >> 4) & 0xf));
+		}
+#ifdef	FWSCSI_DEBUG
+		for (i=0; i<xs->datalen; i++) {
+			DPRINTFN(2, ("%s %02.2x", (i % 16)?"":"\n   ",
+			    data_elm->data_addr[i]));
+		}
+		DPRINTFN(2, ("\n"));
+#endif	/* FWSCSI_DEBUG */
+		sc->sc_fwnode->sc1394_inreg(data_ab, TRUE);
+		data_elm->data_ab = data_ab;
+
+		cmd_orb->data_descriptor.node_id = htons(host_id);
+		data_ab->ab_addr = SBP2_CMD_DATA + ((u_int64_t)dhash << 8);
+		cmd_orb->data_descriptor.hi = htons((SBP2_CMD_DATA >> 32) +
+		    ((dhash >> 24) & 0xFF));
+		cmd_orb->data_descriptor.lo = htonl((dhash << 8) & 0xFFFFFFFF);
+		cmd_orb->data_size = htons(datalen & 0xFFFF);
+	}
+
+	cmd_orb->options = htons(options);
+
+	bcopy(xs->cmd, (void*)(&cmd_orb->command_block[0]), xs->cmdlen);
+	xs->cmd = (struct scsi_generic *) cmd_orb;
+
+#if 0	/* NO_THREAD */
+	if (kthread_create(fwscsi_command_wait, xs, NULL, "%s",
+	    sc->sc_dev.dv_xname))
+	{
+		printf("%s: unable to create event thread\n",
+		    sc->sc_dev.dv_xname);
+		return (TRY_AGAIN_LATER);
+	}
+#endif	/* NO_THREAD */
+
+#if 0	/* NO_THREAD */
+	sbp2_command_add(sc->sc_fwnode, sc->sc_lun, cmd_orb, 8, xs->data,
+	    fwscsi_status_notify);
+#else	/* NO_THREAD */
+	timeout_add(&xs->stimeout, (xs->timeout * hz) / 1000);
+	sbp2_command_add(sc->sc_fwnode, sc->sc_lun, cmd_orb, 8, xs->data,
+	    fwscsi_command_wait, (void *)xs);
+#endif	/* NO_THREAD */
+
+	splx(s);
+	return (SUCCESSFULLY_QUEUED);
+}
+
+#if 1	/* NO_THREAD */
+void
+fwscsi_command_timeout(void *arg)
+{
+	struct sbp2_status_notification notification;
+	struct sbp2_status_block *status;
+
+	DPRINTF(("%s: cpl = %d(%08x)\n", __func__, cpl, cpl));
+
+	MALLOC(status, struct sbp2_status_block *, sizeof(*status),
+	    M_1394DATA, M_WAITOK);
+	bzero(status, sizeof(*status));
+
+	status->flags = 0x41;
+	status->flags |= SBP2_STATUS_RESP_TRANS_FAILURE | SBP2_STATUS_DEAD;
+	status->status = SBP2_STATUS_OBJECT_ORB | SBP2_STATUS_SERIAL_TIMEOUT;
+
+	notification.origin = ((struct scsi_xfer *)arg)->cmd;
+	notification.status = status;
+	fwscsi_command_wait(arg, &notification);
+}
+#endif	/* NO_THREAD */
+
+void
+#if 0	/* NO_THREAD */
+fwscsi_command_wait(void *aux)
+#else	/* NO_THREAD */
+fwscsi_command_wait(void *aux, struct sbp2_status_notification *notification)
+#endif	/* NO_THREAD */
+{
+	struct scsi_xfer *xs = (struct scsi_xfer *)aux;
+	struct fwscsi_orb_data *data_elm = (struct fwscsi_orb_data *)xs->data;
+	struct fwscsi_softc *sc = xs->sc_link->adapter_softc;
+	struct sbp2_command_orb *cmd_orb;
+	struct ieee1394_abuf *data_ab;
+	struct fwscsi_status *status = NULL;
+	u_int32_t tmp;
+#if 0	/* NO_THREAD */
+	int error;
+#endif	/* NO_THREAD */
+	int s;
+#if 1	/* NO_THREAD */
+#ifdef	FWSCSI_DEBUG
+	void *orb = notification->origin;
+	int i;
+#endif	/* FWSCSI_DEBUG */
+
+	DPRINTF(("%s: cpl = %d(%08x)\n", __func__, cpl, cpl));
+
+#if 1	/* NO_THREAD */
+	s = splbio();
+	timeout_del(&xs->stimeout);
+	splx(s);
+#endif	/* NO_THREAD */
+	status = (struct fwscsi_status *)notification->status;
+
+	DPRINTF(("%s: origin=0x%08x csr=0x%016qx", __func__,
+	    (u_int32_t)orb,
+	    ((u_int64_t)(ntohs(status->orb_offset_hi)) << 32) +
+	    ntohl(status->orb_offset_lo)));
+#ifdef	FWSCSI_DEBUG
+	for (i = 0; i < sizeof(*status); i++) {
+		DPRINTFN(2, ("%s %02.2x", (i % 16)?"":"\n   ",
+		    ((u_int8_t *)status)[i]));
+	}
+	DPRINTFN(2, ("\n"));
+#endif	/* FWSCSI_DEBUG */
+#endif	/* NO_THREAD */
+
+	cmd_orb = (struct sbp2_command_orb *)(xs->cmd);
+	xs->cmd = &xs->cmdstore;
+
+#if 0	/* NO_THREAD */
+	error = tsleep(cmd_orb, PRIBIO, "sbpcmd", (xs->timeout * hz) / 1000);
+#endif	/* NO_THREAD */
+
+	if (data_elm != NULL) {
+		data_ab = data_elm->data_ab;
+		if (data_ab) {
+			sc->sc_fwnode->sc1394_unreg(data_ab, TRUE);
+			if ((void *)data_ab->ab_data > (void *)1) { /* XXX */
+				free(data_ab->ab_data, M_1394DATA);
+				//MPRINTF_OLD("free(1394DATA)", data_ab->ab_data);
+				data_ab->ab_data = NULL;	/* XXX */
+			}
+			FREE(data_ab, M_1394DATA);
+			//MPRINTF_OLD("FREE(1394DATA)", data_ab);
+			data_ab = NULL;	/* XXX */
+		}
+
+		xs->data = data_elm->data_addr;
+		s = splbio();
+		TAILQ_REMOVE(&sc->sc_data, data_elm, data_chain);
+		splx(s);
+		FREE(data_elm, M_1394CTL);
+		//MPRINTF_OLD("FREE(1394CTL)", data_elm);
+		data_elm = NULL;	/* XXX */
+	}
+
+#if 0	/* NO_THREAD */
+	if (error == EWOULDBLOCK) {
+		DPRINTF(("%s: Command Timeout.\n", __func__));
+		DPRINTF(("  -> XS_SELTIMEOUT\n"));
+		xs->error = XS_SELTIMEOUT;
+		cmd_orb = NULL;
+	} else {
+		DPRINTF(("%s: Command returned.\n", __func__));
+		status = ((struct fwscsi_status **)cmd_orb)[0];
+	}
+#endif	/* NO_THREAD */
+
+	if (status != NULL &&
+	    ((status->flags & SBP2_STATUS_RESPONSE_MASK) ==
+	        SBP2_STATUS_RESP_REQ_COMPLETE ||
+	     (status->flags & SBP2_STATUS_RESPONSE_MASK) ==
+	        SBP2_STATUS_RESP_VENDOR) &&
+	    status->status != 0) {
+		DPRINTF(("%s: sbp_status 0x%02x, scsi_status 0x%02x\n",
+		    __func__, status->status, status->scsi_status));
+		xs->error = XS_SENSE;
+		xs->status = status->scsi_status & 0x3F;
+		xs->sense.error_code = 0x70;
+		xs->sense.flags = (status->sense_key & FWSCSI_SENSE_KEY) |
+		    ((status->sense_key & FWSCSI_MEI) << 1);
+		if (status->sense_key & FWSCSI_INFO_VALID) {
+			tmp = ntohl(status->information);
+			bcopy(&tmp, &xs->sense.info, sizeof(u_int32_t));
+			xs->sense.error_code |= 0x80;
+		}
+		xs->sense.add_sense_code = status->sense_code;
+		xs->sense.add_sense_code_qual = status->sense_qual;
+		tmp = ntohl(status->sense_key_info);
+		bcopy(&tmp, &xs->sense.fru, sizeof(u_int32_t));
+		tmp = ntohl(status->cmd_spec_info);
+		bcopy(&tmp, &xs->sense.cmd_spec_info, sizeof(u_int32_t));
+		tmp = ntohl(status->vendor_info[0]);
+		bcopy(&tmp, &xs->sense.extra_bytes[0], sizeof(u_int32_t));
+		tmp = ntohl(status->vendor_info[1]);
+		bcopy(&tmp, &xs->sense.extra_bytes[4], sizeof(u_int32_t));
+	} else if (status != NULL &&
+	    ((status->flags & SBP2_STATUS_RESPONSE_MASK) ==
+	        SBP2_STATUS_RESP_TRANS_FAILURE ||
+	     (status->flags & SBP2_STATUS_RESPONSE_MASK) ==
+	        SBP2_STATUS_RESP_ILLEGAL_REQ)) {
+
+		DPRINTF(("%s: device error (flags 0x%02x, status 0x%02x)\n",
+		    __func__, status->flags, status->status));
+		xs->error = XS_SENSE;
+		xs->status = SCSI_CHECK;
+		xs->flags |= ITSDONE;
+		if ((status->flags & SBP2_STATUS_RESPONSE_MASK) ==
+		    SBP2_STATUS_RESP_TRANS_FAILURE) {
+			xs->sense.flags = SKEY_HARDWARE_ERROR;
+			xs->sense.add_sense_code = status->status;
+		} else {
+			xs->sense.flags = SKEY_ILLEGAL_REQUEST;
+			xs->sense.add_sense_code = 0x0;
+		}
+
+		FREE(status, M_1394DATA);
+		//MPRINTF_OLD("FREE(1394DATA)", status);
+		status = NULL;	/* XXX */
+		sbp2_command_del(sc->sc_fwnode, sc->sc_lun, cmd_orb);
+		free(cmd_orb, M_1394DATA);
+		//MPRINTF_OLD("free(1394DATA)", cmd_orb);
+		cmd_orb = NULL;	/* XXX */
+
+		s = splbio();
+		scsi_done(xs);
+		splx(s);
+
+#if 0	/* NO_THREAD */
+		kthread_exit(0);
+#else	/* NO_THREAD */
+		return;
+#endif	/* NO_THREAD */
+	}
+
+	/*
+	 * Now, if we've come here with no error code, i.e. we've kept the
+	 * initial XS_NOERROR, and the status code signals that we should
+	 * check sense, we'll need to set up a request sense cmd block and
+	 * push the command back into the ready queue *before* any other
+	 * commands for this target/lunit, else we lose the sense info.
+	 * We don't support chk sense conditions for the request sense cmd.
+	 */
+	if (xs->error == XS_NOERROR) {
+		if (status->flags & SBP2_STATUS_RESPONSE_MASK) {
+			DPRINTF(("  -> XS_SENSE\n"));
+			xs->error = XS_SENSE;
+		} else if (status->flags & SBP2_STATUS_DEAD) {
+			DPRINTF(("  -> XS_DRIVER_STUFFUP\n"));
+			xs->error = XS_DRIVER_STUFFUP;
+		} else {
+			DPRINTF(("  -> XS_NOERROR\n"));
+		}
+	}
+
+	if (status != NULL) {
+		DPRINTF(("%s: Free status(0x%08x)\n", __func__,
+		    (u_int32_t)status));
+		FREE(status, M_1394DATA);
+		//MPRINTF_OLD("FREE(1394DATA)", status);
+		status = NULL;	/* XXX */
+	}
+	if (cmd_orb != NULL) {
+		DPRINTF(("%s: Nullify orb(0x%08x)\n", __func__,
+		    (u_int32_t)cmd_orb));
+		cmd_orb->options = htons(SBP2_DUMMY_TYPE);
+		sbp2_command_del(sc->sc_fwnode, sc->sc_lun, cmd_orb);
+		free(cmd_orb, M_1394DATA);
+		//MPRINTF_OLD("free(1394DATA)", cmd_orb);
+		cmd_orb = NULL;	/* XXX */
+	}
+
+	xs->flags |= ITSDONE;
+	xs->resid = 0;
+	s = splbio();
+	scsi_done(xs);
+	splx(s);
+
+#if 0	/* NO_THREAD */
+	kthread_exit(0);
+#endif	/* NO_THREAD */
+}
+
+void
+fwscsi_command_data(struct ieee1394_abuf *ab, int rcode)
+{
+	struct fwscsi_orb_data *data_elm;
+	struct ieee1394_abuf *data_ab;
+	struct fwnode_softc *sc = (struct fwnode_softc *)ab->ab_req;
+	struct scsi_xfer *xs = ab->ab_cbarg;
+	size_t datalen;
+	caddr_t dataptr;
+#ifdef	FWSCSI_DEBUG
+	int i;
+#endif	/* FWSCSI_DEBUG */
+
+	DPRINTF(("%s: cpl = %d(%08x)\n", __func__, cpl, cpl));
+
+	if (rcode || (xs == NULL)) {
+#ifdef	FWSCSI_DEBUG
+		DPRINTF(("%s: Bad return code: %d\n", __func__, rcode));
+#endif	/* FWSCSI_DEBUG */
+		if ((void *)ab->ab_data > (void *)1) {		/* XXX */
+			free(ab->ab_data, M_1394DATA);
+			//MPRINTF_OLD("free(1394DATA)", ab->ab_data);
+			ab->ab_data = NULL;
+		}
+		return;
+	}
+
+	data_elm = (struct fwscsi_orb_data *)xs->data;
+
+	datalen = MIN(ab->ab_retlen, ab->ab_length);
+	dataptr = data_elm->data_addr + (size_t)((ab->ab_addr & 0xFFFFFFFFFF) -
+	    ((u_int64_t)data_elm->data_hash << 8));
+
+	if ((dataptr < data_elm->data_addr) ||
+	    ((dataptr + datalen) >
+	     (data_elm->data_addr + roundup(data_elm->data_len, 4)))) {
+#ifdef	FWSCSI_DEBUG
+		DPRINTF(("%s: Data (0x%08x[%d]) out of range (0x%08x[%d])\n",
+		    __func__, dataptr, datalen, data_elm->data_addr,
+		    data_elm->data_len));
+#endif	/* FWSCSI_DEBUG */
+		if ((void *)ab->ab_data > (void *)1) {		/* XXX */
+			free(ab->ab_data, M_1394DATA);
+			//MPRINTF_OLD("free(1394DATA)", ab->ab_data);
+			ab->ab_data = NULL;
+		}
+		return;
+	}
+
+	DPRINTF(("%s: tcode:%d data:0x%08x[%d/%d/%d/%d]",
+	    __func__, ab->ab_tcode, dataptr, ab->ab_length,
+	    ab->ab_retlen, data_elm->data_len, xs->resid));
+
+	switch (ab->ab_tcode) {
+
+	/* Got a read so allocate the buffer and write out the response. */
+	case	IEEE1394_TCODE_READ_REQUEST_DATABLOCK:
+
+		MALLOC(data_ab, struct ieee1394_abuf *, sizeof(*data_ab),
+		    M_1394DATA, M_WAITOK);
+		//MPRINTF_OLD("MALLOC(1394DATA)", data_ab);
+		bcopy(ab, data_ab, sizeof(*data_ab));
+
+		data_ab->ab_data = malloc(datalen, M_1394DATA, M_WAITOK);
+		//MPRINTF_OLD("malloc(1394DATA)", data_ab->ab_data);
+		bzero(data_ab->ab_data, datalen);
+		bcopy(dataptr, data_ab->ab_data,
+		    MIN(data_elm->data_len, datalen));
+
+		data_ab->ab_retlen = 0;
+		data_ab->ab_cb = NULL;
+		data_ab->ab_cbarg = NULL;
+		data_ab->ab_tcode = IEEE1394_TCODE_READ_RESPONSE_DATABLOCK;
+		data_ab->ab_length = datalen;
+
+#ifdef	FWSCSI_DEBUG
+		for (i = 0; i < data_ab->ab_length; i++) {
+			DPRINTFN(1, ("%s %02.2x", (i % 16)?"":"\n   ",
+			    ((u_int8_t *)data_ab->ab_data)[i]));
+		}
+#endif	/* FWSCSI_DEBUG */
+
+		sc->sc1394_write(data_ab);
+
+		break;
+
+	case	IEEE1394_TCODE_WRITE_REQUEST_DATABLOCK:
+
+#ifdef	FWSCSI_DEBUG
+		for (i = 0; i < ab->ab_retlen; i++) {
+			DPRINTFN(1, ("%s %02.2x", (i % 16)?"":"\n   ",
+			    ((u_int8_t *)ab->ab_data)[i]));
+		}
+#endif	/* FWSCSI_DEBUG */
+
+		bcopy(ab->ab_data, dataptr, datalen);
+
+		break;
+
+	default:
+		break;
+	}
+	DPRINTF(("\n"));
+
+#if	0
+	if (xs->resid > datalen) {
+		xs->resid -= datalen;
+		DPRINTFN(1, ("%s: Wait more", __func__));
+	} else {
+		if (xs->resid != datalen)
+			xs->resid = xs->datalen = data_elm->data_len;
+		DPRINTFN(1, ("%s: Data block complete", __func__));
+	}
+#else
+	if (xs->resid <= datalen) {
+		xs->resid = 0;
+		DPRINTFN(1, ("%s: Data block complete", __func__));
+	} else {
+		xs->resid -= datalen;
+		DPRINTFN(1, ("%s: Wait more", __func__));
+	}
+#endif
+	DPRINTFN(1, (" -- resid = %d\n", xs->resid));
+}
+#endif
+
+#ifdef	__NetBSD__
+void
+fwscsi_scsipi_minphys(struct buf *buf)
+#else
+void
+fwscsi_minphys(struct buf *buf)
+#endif
+{
+	DPRINTF(("%s: cpl = %d(%08x)\n", __func__, cpl, cpl));
+
+#if 1
+	if (buf->b_bcount > SBP2_MAX_TRANS)
+		buf->b_bcount = SBP2_MAX_TRANS;
+#else
+	if (buf->b_bcount > 512)
+		buf->b_bcount = 512;
+#endif
+	minphys(buf);
+}
diff --git a/sys/dev/std/SBP2.roadmap b/sys/dev/std/SBP2.roadmap
new file mode 100644
index 00000000000..55be1d19517
--- /dev/null
+++ b/sys/dev/std/SBP2.roadmap
@@ -0,0 +1,149 @@
+- configuration rom
+  - p1212 takes care of this, though it might be useful to compile the info into
+    a direct usable form (a struct)
+
+- access control
+  - target login_descriptors : context of a login
+    - lun
+    - login_owner_id
+    - login_owner_EUI_64
+    - status_fifo_addr
+    - exclusive_flag
+    - fetch_agent_csr
+    - login_id
+    - reconnect_hold
+  - login procedure
+    - 8-byte (login_request_addr) block-write transaction to the
+      management_agent_csr
+    - target reads the initiator's EUI64 with 2 quadlet-read transactions
+    - target reads the login_request_orb at login_request_addr
+    - if initiator's EUI64 exists in login_owner_EUI_64 : access denied
+    - if initiator wants exclusive and lun already used : access denied
+    - if exclusive_flag set on lun's login_descriptor : access denied
+    - if no login_descriptor is available : resources unavailable
+    - if it's ok, target return a valid login_response
+  - reconnection procedure
+    - 8-byte (reconnect_request_addr) block-write transaction to the
+      management_agent_csr
+    - target reads the initiator's EUI64 with 2 quadlet-read transactions
+    - target reads the reconnect_request_orb at reconnect_request_addr
+    - a completion status is sent back to the initiator
+  - logout procedure
+    - 8-byte (logout_request_addr) block-write transaction to the
+      management_agent_csr
+    - a completion status is sent back to the initiator
+- command execution
+  - requests and request-lists
+    - fetch agent initialization
+      - initiator allocates a dummy_orb, with a null pointer as next_orb,
+        of at least the minimum orb size
+      - initiator resets target fetch agent with a quadlet-write to
+        AGENT_RESET register
+      - initiator writes dummy_orb_addr with 8-byte block-write to
+        ORB_POINTER register
+      - fetch agent transition to ACTIVE state
+      - fetch agent reads the terminating dummy_orb and transition to
+        SUSPENDED state
+    - dynamic appends to request lists
+      - initiator constructs a linked-list of orbs in system memory, the
+        last orb's next_orb is null
+      - initiator updates the current last orb's next_orb to the first orb
+        of the linked-list
+      - initiator transmits a quadlet-write to the target's DOORBELL register
+      if the target is already in SUSPENDED state, the initiator may write
+      the first orb to ORB_POINTER register, and no DOORBELL is necessary
+  - data transfer
+  - completion status
+  - unsolicited status
+- task management
+  - task sets
+  - basic task management model
+    - all tasks in a task set share the same execution characteristics :
+      reorderable/ordered
+    - reorderable/ordered feature is target dependent, see config_rom
+    - each task is identified by its orb's serial bus address
+    - all targets must at least implement the abort task, abort task set and
+      target reset management functions
+  - error conditions : entire task set is cleared on error condition
+    - fetch agent transitions to dead state
+    - target waits for sent status completion of all the completed tasks
+    - then target sends status completion for the faulty task, with dead bit
+  - task management requests
+    - abort task
+      - modify the rq_fmt to 3 in the task to be aborted's orb
+      - setup an ABORT_TASK management orb with the task orb's serial bus addr
+      - if target fetch an aborted task, sends REQUEST_COMPLETE on dummy_orb
+      - initiator may not reuse the memory of the task to abort until
+        completion has been received
+    - abort task set
+      - initiator sends an ABORT_TASK_SET management orb with login_id to target
+      - fetch agent transitions to dead state
+      - target waits for sent status completion of all the completed tasks
+      - then target sends status completion into the supplied status buffer
+      - initiator may not reuse the memory of any task in the task set until
+        a completion status is received for the abort task set management task
+    - logical unit reset (optional)
+      - initiator sends a LOGICAL_UNIT_RESET management orb with login_id to
+        target
+      - fetch agent transitions to dead state for all the lun's fetch agents
+      - target creates a logical unit attention condition to all the initiators
+        logged in to that lun (except the one initiating the reset)
+      - then target sends status completion into the supplied status buffer
+      - initiator may not reuse the memory of any task in the task set until
+        a completion status is received for the logical unit reset management
+        task
+    - target reset
+      - initiator sends a TARGET_RESET management orb with login_id to target
+      - fetch agent transitions to dead state for all the fetch agents
+      - target creates a logical unit attention condition to all the initiators
+        except the one initiating the reset
+      - then target sends status completion into the supplied status buffer
+      - initiator may not reuse the memory of any task in the task set until
+        a completion status is received for the target reset management task
+
+- data structures
+  - ORB : rq_fmt_dep:128|notify:1|rq_fmt:2|rq_fmt_dep:29|rq_fmt_dep:nq
+    rq_fmt = 0|std, 1|rsvd, 2|vend, 3|nop
+    - Dummy ORB : next_ORB:64|ignore:64|notify:1|3:2|ignore:29|ignore:nq
+    - Command ORB : next_ORB:64|data_descr:64|notify:1|0:2|rsvd:1|direction:1|
+                    speed:3|max_payload:4|page_table_present:1|page_size:3|
+                    data_size:16|command_block:nq
+      speed = 0|S100, 1|S200, 2|S400, 3|S800, 4|S1600, 5|S3200
+      max payload = 2 ^ (max_payload + 2)
+      page size = 2 ^ (page_size + 8), if page_size != 0
+      (page_table_present):(page_size == 0)?unrestricted:normalized
+      (page_table_present):data_size = # of table elements
+                          ?data_size = data_descriptor size
+    - Management ORB : func_dep:128|notify:1|0:2|rsvd:9|function:4|func_dep:16|
+                       func_dep:32|status_fifo:64
+      function = 0|LOGIN, 1|QUERY_LOGINS, 3|RECONNECT, 4|SET_PASSWORD,
+                 7|LOGOUT, B|ABORT_TASK, C|ABORT_TASK_SET,
+                 E|LOGICAL_UNIT_RESET, F|TARGET_RESET
+      - Access ORB
+        - LOGIN ORB : password:64|login_response:64|notify:1|0:2|exclusive:1|
+                      rsvd:4|reconnect:4|0:4|lun:16|password_length:16|
+                      login_response_length:16|status_fifo:64
+          login_response_length >= 12
+          reconnect timeout = 2 ^ reconnect
+          - login response : length:16|login_ID:16|command_block_agent:64|
+                             rsvd:16|reconnect_hold:16
+            reconnect_hold <= 2 ^ reconnect - 1
+        - QUERY_LOGINS ORB : rsvd:64|query_response:64|n0..1:16|lun:16|rsvd:16|
+                             query_response_length:16|status_fifo:64
+          - query response : length:16|max_logins:16|
+                             (node_ID:16|login_ID:16|initiator_EUI_64:64)*n
+        - RECONNECT ORB : rsvd:128|n0..3:16|login_ID:16|rsvd:32|status_fifo:64
+        - LOGOUT ORB : rsvd:128|n0..7:16|login_ID:16|rsvd:32|status_fifo:64
+      - Task Management ORB : ORB_offset:064|rsvd:64|n0..fn:16|login_ID:16|
+                              rsvd:32|status_fifo:64
+        fn = B|ABORT_TASK, C|ABORT_TASK_SET,
+             E|LOGICAL_UNIT_RESET, F|TARGET_RESET
+        if ABORT_TASK, ORB_offset is relevant
+  - page tables
+    - unrestricted page table : segment_length:16|segment_base:48
+    - normalized page table : segment_length:16|segment_base..segment_offset:48
+  - status block : src:2|resp:2|dead:1|len:3|sbp_status:8|ORB_offset:48|cmd_dep
+  - address pointer : node_ID:16|offset:46|rsvd:2
+  - ORB pointer : null:1|rsvd:15|offset:46|rsvd:2
+- CSR
+
diff --git a/sys/dev/std/sbp2.c b/sys/dev/std/sbp2.c
new file mode 100644
index 00000000000..8c7e311cecc
--- /dev/null
+++ b/sys/dev/std/sbp2.c
@@ -0,0 +1,1081 @@
+/*	$OpenBSD: sbp2.c,v 1.1 2002/12/13 02:57:56 tdeval Exp $	*/
+
+/*
+ * Copyright (c) 2002 Thierry Deval.  All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
+ * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
+ * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
+ * IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
+ * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+ * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+ * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
+ * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/*
+ * sbp2.c
+ *
+ * Basic implementation of the ANSI NCITS 325-1998 Serial Bus Protocol2 (SBP-2).
+ *
+ * ANSI NCITS 325-1998
+ * Information Technology - Serial Bus Protocol 2 (SBP-2)
+ *
+ * Defines a protocol for the transport of commands and data over high
+ * performance serial bus, as specified in American National Standard for
+ * High Performance Serial Bus, ANSI/IEEE 1394-1995. The transport protocol,
+ * Serial Bus Protocol 2 or SBP-2, requires implementations to conform to the
+ * requirements of the aforementioned standard as well as to International
+ * Standard for Control and Status Register (CSR) Architecture for
+ * Microcomputer Buses, ISO/IEC 13213:1994 (IEEE 1212-1994), and permits the
+ * exchange of commands, data and status between initiators and targets
+ * connected to Serial Bus.
+ */
+
+#include <sys/param.h>
+#include <sys/systm.h>
+#include <sys/socket.h>
+#ifdef	__NetBSD__
+#include <sys/callout.h>
+#else
+#include <sys/timeout.h>
+#endif
+#include <sys/device.h>
+#include <sys/kernel.h>
+#include <sys/malloc.h>
+#include <sys/mbuf.h>
+#ifdef	__OpenBSD__
+#include <sys/endian.h>
+#endif
+
+#if __NetBSD_Version__ >= 105010000 || !defined(__NetBSD__)
+#include <uvm/uvm_extern.h>
+#else
+#include <vm/vm.h>
+#endif
+
+#include <machine/bus.h>
+#include <machine/intr.h>
+
+#include <dev/rndvar.h>
+
+#include <dev/ieee1394/ieee1394reg.h>
+#include <dev/ieee1394/fwohcireg.h>
+#include <dev/ieee1394/fwnodereg.h>
+#include <dev/std/ieee1212reg.h>
+#include <dev/std/sbp2reg.h>
+
+#include <dev/ieee1394/ieee1394var.h>
+#include <dev/ieee1394/fwohcivar.h>
+#include <dev/ieee1394/fwnodevar.h>
+#include <dev/std/ieee1212var.h>
+#include <dev/std/sbp2var.h>
+
+int sbp2_print_data(struct p1212_data *);
+int sbp2_print_dir(struct p1212_dir *);
+
+void sbp2_login_send(struct ieee1394_abuf *, int);
+void sbp2_status_resp(struct ieee1394_abuf *, int);
+void sbp2_command_send(struct ieee1394_abuf *, int);
+
+#ifdef	MALLOC_DEBUG
+#include <dev/ieee1394/malloc_debug.h>		/* MPRINTF(x,y) */
+#else	/* !MALLOC_DEBUG */
+#define	MPRINTF(x,y)
+#endif	/* MALLOC_DEBUG */
+
+#ifdef	SBP2_DEBUG
+#include <sys/syslog.h>
+extern int log_open;
+int sbp2_oldlog;
+#define	DPRINTF(x)	do {						\
+	if (sbp2debug) {						\
+		sbp2_oldlog = log_open; log_open = 1;			\
+		addlog x; log_open = sbp2_oldlog;			\
+	}								\
+} while (0)
+#define	DPRINTFN(n,x)	do {						\
+	if (sbp2debug>(n)) {						\
+		sbp2_oldlog = log_open; log_open = 1;			\
+		addlog x; log_open = sbp2_oldlog;			\
+	}								\
+} while (0)
+int	sbp2debug = 0;
+#else	/* SBP2_DEBUG */
+#define	DPRINTF(x)
+#define	DPRINTFN(n,x)
+#endif	/* ! SBP2_DEBUG */
+
+typedef struct sbp2_orb_element {
+	u_int32_t		  elm_hash;
+	struct ieee1394_abuf	 *elm_orb_ab;
+	struct sbp2_command_orb	 *elm_orb;
+	size_t			  elm_orblen;
+	void			 *elm_data;
+	size_t			  elm_datasize;
+	void			(*elm_cb)(void *,
+					  struct sbp2_status_notification *);
+	void			 *elm_cbarg;
+	TAILQ_ENTRY(sbp2_orb_element) elm_chain;
+} sbp2_orb_element;
+
+typedef struct sbp2_account {
+	struct fwnode_softc	 *ac_softc;
+	u_int16_t		  ac_lun;
+	u_int16_t		  ac_login;
+	u_int16_t		  ac_reconnect_hold;
+	u_int16_t		  ac_valid;
+	u_int64_t		  ac_mgmt_agent;
+	u_int64_t		  ac_fetch_agent;
+	u_int64_t		  ac_response;
+	void			 *ac_response_block;
+	u_int64_t		  ac_status_fifo;
+	struct ieee1394_abuf	 *ac_status_ab;
+	struct sbp2_status_block *ac_status_block;
+	void			(*ac_cb)(void *,
+					 struct sbp2_status_notification *);
+	void			 *ac_cbarg;
+	struct sbp2_task_management_orb *ac_mgmt_orb;
+	TAILQ_HEAD(sbp2_orb_tq, sbp2_orb_element) ac_orb_head;
+	SLIST_ENTRY(sbp2_account) ac_chain;
+} sbp2_account;
+
+static SLIST_HEAD(, sbp2_account) sbp2_ac_head;
+static int sbp2_ac_valid;
+
+struct sbp2_account *sbp2_acfind(struct fwnode_softc *, int);
+struct sbp2_orb_element *sbp2_elfind_hash(struct sbp2_account *, u_int32_t);
+struct sbp2_orb_element *sbp2_elfind_orb(struct sbp2_account *,
+    struct sbp2_command_orb *);
+
+struct sbp2_account *
+sbp2_acfind(struct fwnode_softc *sc, int lun)
+{
+	struct sbp2_account *ac;
+
+	SLIST_FOREACH(ac, &sbp2_ac_head, ac_chain) {
+		if (ac != NULL && ac->ac_softc == sc && ac->ac_lun == lun)
+			break;
+	}
+
+	return (ac);
+}
+
+struct sbp2_orb_element *
+sbp2_elfind_hash(struct sbp2_account *ac, u_int32_t hash)
+{
+	struct sbp2_orb_element *elm;
+
+	TAILQ_FOREACH(elm, &ac->ac_orb_head, elm_chain) {
+		if (elm->elm_hash == hash)
+			break;
+	}
+
+	return (elm);
+}
+
+struct sbp2_orb_element *
+sbp2_elfind_orb(struct sbp2_account *ac, struct sbp2_command_orb *orb)
+{
+	struct sbp2_orb_element *elm;
+
+	TAILQ_FOREACH(elm, &ac->ac_orb_head, elm_chain) {
+		if (elm->elm_orb == orb)
+			break;
+	}
+
+	return (elm);
+}
+
+int
+sbp2_print_data(struct p1212_data *data)
+{
+	struct p1212_key *key = (struct p1212_key *)data;
+
+	switch (key->key_value) {
+	case SBP2_KEYVALUE_Command_Set:
+		DPRINTF(("SBP2 Command Set: "));
+		if (key->val == 0x104d8)
+			DPRINTF(("SCSI 2\n"));
+		else
+			DPRINTF(("0x%08x\n", key->val));
+		break;
+	case SBP2_KEYVALUE_Unit_Characteristics:
+		DPRINTF(("SBP2 Unit Characteristics: 0x%08x\n", key->val));
+		break;
+	case SBP2_KEYVALUE_Command_Set_Revision:
+		DPRINTF(("SBP2 Command Set Revision: 0x%08x\n", key->val));
+		break;
+	case SBP2_KEYVALUE_Command_Set_Spec_Id:
+		DPRINTF(("SBP2 Command Set Spec Id: 0x%08x\n", key->val));
+		break;
+	case SBP2_KEYVALUE_Firmware_Revision:
+		DPRINTF(("SBP2 Firmware Revision: 0x%08x\n", key->val));
+		break;
+	case SBP2_KEYVALUE_Reconnect_Timeout:
+		DPRINTF(("SBP2 Reconnect Timeout: 0x%08x\n", key->val));
+		break;
+	case SBP2_KEYVALUE_Unit_Unique_Id:
+		DPRINTF(("SBP2 Unit Unique Id: 0x%08x\n", key->val));
+		break;
+	case P1212_KEYVALUE_Unit_Dependent_Info:
+		if (key->key_type == P1212_KEYTYPE_Immediate)
+			DPRINTF(("SBP2 Logical Unit Number: 0x%08x\n", key->val));
+		else if (key->key_type == P1212_KEYTYPE_Offset)
+			DPRINTF(("SBP2 Management Agent: 0x%08x\n", key->val));
+		break;
+	default:
+		return 0;
+	}
+	return 1;
+}
+
+int
+sbp2_print_dir(struct p1212_dir *dir)
+{
+	u_int8_t dir_type = ((struct p1212_key *)dir)->key_type;
+
+	switch(dir_type) {
+	case SBP2_KEYVALUE_Logical_Unit_Directory:
+		DPRINTF(("Logical Unit "));
+		break;
+	default:
+		return 0;
+	}
+	return 1;
+}
+
+int
+sbp2_init(struct fwnode_softc *sc, struct p1212_dir *unitdir)
+{
+	struct p1212_key **key;
+	struct p1212_dir *dir;
+	struct sbp2_account *ac;
+	int loc;
+
+	key = p1212_find(unitdir, P1212_KEYTYPE_Offset,
+	    SBP2_KEYVALUE_Management_Agent, 0);
+	if (key == NULL) return (-1);
+
+	if (!sbp2_ac_valid) {
+		SLIST_INIT(&sbp2_ac_head);
+		sbp2_ac_valid = 1;
+	}
+
+	MALLOC(ac, struct sbp2_account *, sizeof(*ac), M_1394CTL, M_WAITOK);
+	//MPRINTF_OLD("MALLOC(1394CTL)", ac);
+	bzero(ac, sizeof(*ac));
+
+	loc = key[0]->val;
+	DPRINTF(("%s: Node %d: UID %02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x\n",
+	    __func__, sc->sc_sc1394.sc1394_node_id,
+	    sc->sc_sc1394.sc1394_guid[0], sc->sc_sc1394.sc1394_guid[1],
+	    sc->sc_sc1394.sc1394_guid[2], sc->sc_sc1394.sc1394_guid[3],
+	    sc->sc_sc1394.sc1394_guid[4], sc->sc_sc1394.sc1394_guid[5],
+	    sc->sc_sc1394.sc1394_guid[6], sc->sc_sc1394.sc1394_guid[7]));
+	free(key, M_DEVBUF);
+	//MPRINTF_OLD("free(DEVBUF)", key);
+	key = NULL;	/* XXX */
+
+	ac->ac_softc = sc;
+	ac->ac_login = sc->sc_sc1394.sc1394_node_id;
+	ac->ac_mgmt_agent = CSR_BASE + 4 * loc;
+	DPRINTF(("%s: mgmt_agent = 0x%016qx\n", __func__, ac->ac_mgmt_agent));
+
+	if ((key = p1212_find(unitdir, P1212_KEYTYPE_Immediate,
+	    SBP2_KEYVALUE_Logical_Unit_Number, 0)) != NULL) {
+		ac->ac_lun = (*key)->val & 0xffff;
+		free(key, M_DEVBUF);
+		//MPRINTF_OLD("free(DEVBUF)", key);
+		key = NULL;	/* XXX */
+	} else if ((key = p1212_find(unitdir, P1212_KEYTYPE_Directory,
+	    SBP2_KEYVALUE_Logical_Unit_Directory, 0)) != NULL) {
+		dir = (struct p1212_dir *)*key;
+		free(key, M_DEVBUF);
+		//MPRINTF_OLD("free(DEVBUF)", key);
+		key = NULL;	/* XXX */
+		key = p1212_find(dir, P1212_KEYTYPE_Immediate,
+		    SBP2_KEYVALUE_Logical_Unit_Number, 0);
+		if (key != NULL) {
+			ac->ac_lun = (*key)->val & 0xffff;
+			free(key, M_DEVBUF);
+			//MPRINTF_OLD("free(DEVBUF)", key);
+			key = NULL;	/* XXX */
+		}
+	}
+	DPRINTF(("%s: lun = %d\n", __func__, ac->ac_lun));
+
+	TAILQ_INIT(&ac->ac_orb_head);
+
+	SLIST_INSERT_HEAD(&sbp2_ac_head, ac, ac_chain);
+
+	return (ac->ac_lun);
+}
+
+void
+sbp2_clean(struct fwnode_softc *sc, struct p1212_dir *unitdir, int logout)
+{
+	struct sbp2_account *ac;
+	struct sbp2_orb_element *elm;
+	struct p1212_key **key;
+	struct p1212_dir **dir,*d;
+	int lun,i;
+
+	DPRINTF(("%s: start\n", __func__));
+#ifdef	SBP2_DEBUG
+	if (sbp2debug)
+		p1212_print(unitdir);
+#endif
+
+	if ((key = p1212_find(unitdir, P1212_KEYTYPE_Immediate,
+	    SBP2_KEYVALUE_Logical_Unit_Number, 0)) != NULL) {
+		lun = (*key)->val & 0xffff;
+		if ((ac = sbp2_acfind(sc, lun)) != NULL) {
+			DPRINTF(("%s: clean lun %d\n", __func__, lun));
+			i = 0;
+			TAILQ_FOREACH_REVERSE(elm, &ac->ac_orb_head, elm_chain,
+			    sbp2_orb_tq) {
+				DPRINTF(("%s%d", i++?" ":"", i));
+				if (elm != NULL) {
+					TAILQ_REMOVE(&ac->ac_orb_head, elm,
+					    elm_chain);
+					FREE(elm, M_1394CTL);
+					//MPRINTF_OLD("FREE(1394CTL)", elm);
+					//elm = NULL;	/* XXX */
+				}
+			}
+			if (i) DPRINTF(("\n"));
+			SLIST_REMOVE(&sbp2_ac_head, ac, sbp2_account, ac_chain);
+			if (ac->ac_status_ab) {
+				sc->sc1394_unreg(ac->ac_status_ab, FALSE);
+				FREE(ac->ac_status_ab, M_1394DATA);
+				//MPRINTF_OLD("FREE(1394DATA)", ac->ac_status_ab);
+				ac->ac_status_ab = NULL;	/* XXX */
+			}
+			if (ac->ac_status_block) {
+				FREE(ac->ac_status_block, M_1394DATA);
+				//MPRINTF_OLD("FREE(1394DATA)", ac->ac_status_block);
+				ac->ac_status_block = NULL;	/* XXX */
+			}
+			FREE(ac, M_1394CTL);
+			//MPRINTF_OLD("FREE(1394CTL)", ac);
+			ac = NULL;	/* XXX */
+		}
+		free(key, M_DEVBUF);
+		//MPRINTF_OLD("free(DEVBUF)", key);
+		key = NULL;	/* XXX */
+	} else if ((key = p1212_find(unitdir, P1212_KEYTYPE_Directory,
+	    SBP2_KEYVALUE_Logical_Unit_Directory, 0)) != NULL) {
+		dir = (struct p1212_dir **)key;
+		i = 0;
+		d = dir[i++];
+		while (d != NULL) {
+			key = p1212_find(d, P1212_KEYTYPE_Immediate,
+			    SBP2_KEYVALUE_Logical_Unit_Number, 0);
+			if (key != NULL) {
+				lun = (*key)->val & 0xffff;
+				if ((ac = sbp2_acfind(sc, lun)) != NULL) {
+					DPRINTF(("%s: clean lun %d\n",
+					    __func__, lun));
+					TAILQ_FOREACH(elm, &ac->ac_orb_head,
+					    elm_chain) {
+						if (elm != NULL) {
+							FREE(elm, M_1394CTL);
+							//MPRINTF_OLD("FREE(1394CTL)", elm);
+							elm = NULL;	/* XXX */
+						}
+					}
+					SLIST_REMOVE(&sbp2_ac_head, ac,
+					    sbp2_account, ac_chain);
+					if (ac->ac_status_ab) {
+						sc->sc1394_unreg(ac
+						    ->ac_status_ab, FALSE);
+						FREE(ac->ac_status_ab,
+						    M_1394DATA);
+						//MPRINTF_OLD("FREE(1394DATA)", ac->ac_status_ab);
+						ac->ac_status_ab = NULL;	/* XXX */
+					}
+					if (ac->ac_status_block) {
+						FREE(ac->ac_status_block,
+						    M_1394DATA);
+						//MPRINTF_OLD("FREE(1394DATA)", ac->ac_status_block);
+						ac->ac_status_block = NULL;	/* XXX */
+					}
+					FREE(ac, M_1394CTL);
+					//MPRINTF_OLD("FREE(1394CTL)", ac);
+					ac = NULL;	/* XXX */
+				}
+				free(key, M_DEVBUF);
+				//MPRINTF_OLD("free(DEVBUF)", key);
+				key = NULL;	/* XXX */
+			}
+			d = dir[i++];
+		}
+		free(dir, M_DEVBUF);
+		//MPRINTF_OLD("free(DEVBUF)", dir);
+		dir = NULL;	/* XXX */
+	} else {
+		DPRINTF(("%s: no LUN in configrom 0x%08x ???\n", __func__,
+		    (u_int32_t)sc->sc_configrom->root));
+	}
+
+	DPRINTF(("%s: end\n", __func__));
+}
+
+void
+sbp2_login(struct fwnode_softc *sc, struct sbp2_login_orb *orb,
+    void (*cb)(void *, struct sbp2_status_notification *), void *cbarg)
+{
+	struct ieee1394_abuf *ab, *ab2;
+	struct sbp2_account *ac;
+
+	if ((ac = sbp2_acfind(sc, ntohs(orb->lun))) == NULL) {
+		DPRINTF(("%s: destination not initialized\n", __func__));
+		return;
+	}
+
+	DPRINTF(("%s:", __func__));
+
+	ac->ac_mgmt_orb = (struct sbp2_task_management_orb *)orb;
+	if (cb != NULL) {
+		ac->ac_cb = cb;
+		ac->ac_cbarg = cbarg;
+	}
+
+	MALLOC(ab, struct ieee1394_abuf *, sizeof(*ab), M_1394DATA, M_WAITOK);
+	//MPRINTF_OLD("MALLOC(1394DATA)", ab);
+	bzero(ab, sizeof(*ab));
+	MALLOC(ab2, struct ieee1394_abuf *, sizeof(*ab2), M_1394DATA, M_WAITOK);
+	//MPRINTF_OLD("MALLOC(1394DATA)", ab2);
+	bzero(ab2, sizeof(*ab2));
+
+	ab->ab_req = (struct ieee1394_softc *)sc;
+	ab->ab_length = 8;
+	ab->ab_retlen = 0;
+	ab->ab_cb = NULL;
+	ab->ab_cbarg = NULL;
+	ab->ab_addr = ac->ac_mgmt_agent;
+	ab->ab_tcode = IEEE1394_TCODE_WRITE_REQUEST_DATABLOCK;
+
+	ab->ab_data = malloc(8, M_1394DATA, M_WAITOK);
+	//MPRINTF_OLD("malloc(1394DATA)", ab->ab_data);
+	ab->ab_data[0] = htonl((u_int32_t)(SBP2_LOGIN_ORB >> 32));
+	ab->ab_data[1] = htonl((u_int32_t)(SBP2_LOGIN_ORB & 0xFFFFFFFF));
+	DPRINTF((" CSR = 0x%016qx", ac->ac_mgmt_agent));
+	DPRINTF((", ORB = 0x%016qx\n", SBP2_LOGIN_ORB + (u_int32_t)orb));
+
+	ab2->ab_length = sizeof(struct sbp2_login_orb);
+	ab2->ab_tcode = IEEE1394_TCODE_READ_REQUEST_DATABLOCK;
+	ab2->ab_retlen = 0;
+	ab2->ab_data = NULL;
+	ab2->ab_addr = SBP2_LOGIN_ORB;
+	ab2->ab_cb = sbp2_login_send;
+	ab2->ab_cbarg = ac;
+	ab2->ab_req = (struct ieee1394_softc *)sc;
+
+	sc->sc1394_inreg(ab2, FALSE);
+	sc->sc1394_write(ab);
+	DPRINTF(("%s: LOGIN submitted\n", __func__));
+	return;
+}
+
+void
+sbp2_login_send(struct ieee1394_abuf *ab, int rcode)
+{
+	struct fwnode_softc *sc = (struct fwnode_softc *)ab->ab_req;
+	struct sbp2_account *ac = ab->ab_cbarg;
+	struct sbp2_login_orb *login_orb;
+	struct ieee1394_abuf *stat_ab, *resp_ab, *orb_ab;
+#ifdef	SBP2_DEBUG
+	int i;
+#endif	/* SBP2_DEBUG */
+
+	/* Got a read so allocate the buffer and write out the response. */
+
+	if (rcode || (ac == NULL)) {
+		DPRINTF(("%s: Bad return code: %d\n", __func__, rcode));
+		if (ab->ab_data) {
+			free(ab->ab_data, M_1394DATA);
+			//MPRINTF_OLD("free(1394DATA)", ab->ab_data);
+			ab->ab_data = NULL;	/* XXX */
+		}
+		FREE(ab, M_1394DATA);
+		//MPRINTF_OLD("FREE(1394DATA)", ab);
+		ab = NULL;	/* XXX */
+		return;
+	}
+
+	MALLOC(orb_ab, struct ieee1394_abuf *, sizeof(*orb_ab),
+	    M_1394DATA, M_WAITOK);
+	//MPRINTF_OLD("MALLOC(1394DATA)", orb_ab);
+	bcopy(ab, orb_ab, sizeof(*orb_ab));
+
+	sc->sc1394_unreg(ab, FALSE);
+
+	if (ab->ab_data) {
+		free(ab->ab_data, M_1394DATA);
+		//MPRINTF_OLD("free(1394DATA)", ab->ab_data);
+		ab->ab_data = NULL;	/* XXX */
+		orb_ab->ab_data = NULL;
+	}
+	FREE(ab, M_1394DATA);
+	//MPRINTF_OLD("FREE(1394DATA)", ab);
+	ab = NULL;	/* XXX */
+
+	MALLOC(resp_ab, struct ieee1394_abuf *, sizeof(*resp_ab),
+	    M_1394DATA, M_WAITOK);
+	//MPRINTF_OLD("MALLOC(1394DATA)", resp_ab);
+	MALLOC(stat_ab, struct ieee1394_abuf *, sizeof(*stat_ab),
+	    M_1394DATA, M_WAITOK);
+	//MPRINTF_OLD("MALLOC(1394DATA)", stat_ab);
+	bzero(resp_ab, sizeof(*resp_ab));
+	bzero(stat_ab, sizeof(*stat_ab));
+
+	resp_ab->ab_length = sizeof(struct sbp2_login_response);
+	resp_ab->ab_tcode = IEEE1394_TCODE_WRITE_REQUEST_DATABLOCK;
+	resp_ab->ab_retlen = 0;
+	resp_ab->ab_data = NULL;
+	resp_ab->ab_addr = SBP2_LOGIN_RESP;
+	resp_ab->ab_cb = sbp2_status_resp;
+	resp_ab->ab_cbarg = ac;
+	resp_ab->ab_req = orb_ab->ab_req;
+
+	sc->sc1394_inreg(resp_ab, FALSE);
+
+	stat_ab->ab_length = sizeof(struct sbp2_status_block);
+	stat_ab->ab_tcode = IEEE1394_TCODE_WRITE_REQUEST_DATABLOCK;
+	stat_ab->ab_retlen = 0;
+	stat_ab->ab_data = NULL;
+	stat_ab->ab_addr = SBP2_LOGIN_STATUS;
+	stat_ab->ab_cb = sbp2_status_resp;
+	stat_ab->ab_cbarg = ac;
+	stat_ab->ab_req = orb_ab->ab_req;
+
+	ac->ac_status_ab = stat_ab;
+	sc->sc1394_inreg(stat_ab, FALSE);
+
+	/* Fill in a login packet. First 2 quads are 0 for password. */
+	login_orb = (struct sbp2_login_orb *)ac->ac_mgmt_orb;
+
+	/* Addr for response. */
+	login_orb->login_response.hi = htons((SBP2_LOGIN_RESP >> 32) & 0xffff);
+	login_orb->login_response.lo = htonl(SBP2_LOGIN_RESP & 0xffffffff);
+	DPRINTF(("%s: RESP_ORB = 0x%016qx", __func__, SBP2_LOGIN_RESP));
+
+	/* Set notify and exclusive use bits. Login to lun 0 (XXX) */
+	login_orb->options = htons(0x8000);
+	login_orb->lun = htons(ac->ac_lun);
+
+	/* Password length (0) and login response length (16) */
+	login_orb->password_length = htons(0);
+	login_orb->login_response_length = htons(16);
+
+	/* Addr for status packet. */
+	login_orb->status_fifo.hi = htons((SBP2_LOGIN_STATUS >> 32) & 0xffff);
+	login_orb->status_fifo.lo = htonl(SBP2_LOGIN_STATUS & 0xffffffff);
+	DPRINTF((", STATUS_ORB = 0x%016qx", SBP2_LOGIN_STATUS));
+
+	orb_ab->ab_data = malloc(sizeof(*login_orb), M_1394DATA, M_WAITOK);
+	//MPRINTF_OLD("malloc(1394DATA)", orb_ab->ab_data);
+	bcopy(login_orb, orb_ab->ab_data, sizeof(*login_orb));
+#ifdef	SBP2_DEBUG
+	for (i = 0; i < (sizeof(*login_orb) / 4); i++) {
+		if ((i % 8) == 0) DPRINTFN(2, ("\n   "));
+		DPRINTFN(2, (" %08x", ntohl(orb_ab->ab_data[i])));
+	}
+#endif	/* SBP2_DEBUG */
+	DPRINTF(("\n"));
+
+	orb_ab->ab_retlen = 0;
+	orb_ab->ab_cb = NULL;
+	orb_ab->ab_cbarg = NULL;
+	orb_ab->ab_tcode = IEEE1394_TCODE_READ_RESPONSE_DATABLOCK;
+	orb_ab->ab_length = sizeof(struct sbp2_login_orb);
+
+	sc->sc1394_write(orb_ab);
+}
+
+void
+sbp2_status_resp(struct ieee1394_abuf *ab, int rcode)
+{
+	struct fwnode_softc *sc = (struct fwnode_softc *)ab->ab_req;
+	struct sbp2_account *ac = ab->ab_cbarg;
+	struct sbp2_login_response *login_resp;
+	struct sbp2_status_block *cmd_status;
+	struct sbp2_status_notification *status_notify;
+	struct sbp2_orb_element *elm;
+	int resp, src, status, len, dead;
+	u_int64_t csr;
+#ifdef	SBP2_DEBUG
+	int i;
+#endif	/* SBP2_DEBUG */
+
+	if (rcode || (ac == NULL)) {
+		DPRINTF(("%s: Bad return code: %d\n", __func__, rcode));
+		if (ab->ab_data) {
+			free(ab->ab_data, M_1394DATA);
+			//MPRINTF_OLD("free(1394DATA)", ab->ab_data);
+			ab->ab_data = NULL;	/* XXX */
+		}
+		if (ab->ab_addr != SBP2_LOGIN_STATUS) {
+			FREE(ab, M_1394DATA);
+			//MPRINTF_OLD("FREE(1394DATA)", ab);
+			ab = NULL;	/* XXX */
+		}
+		return;
+	}
+
+#ifdef	SBP2_DEBUG
+	DPRINTF(("%s: CSR = 0x%016qx", __func__, (quad_t)ab->ab_addr));
+	for (i = 0; i < (ab->ab_retlen / 4); i++) {
+		if ((i % 8) == 0) DPRINTFN(2, ("\n   "));
+		DPRINTFN(2, (" %08x", ntohl(ab->ab_data[i])));
+	}
+	DPRINTF(("\n"));
+#endif	/* SBP2_DEBUG */
+
+	if (ab->ab_addr == SBP2_LOGIN_RESP) {
+		login_resp = (struct sbp2_login_response *)ab->ab_data;
+
+//		ac->ac_response = ab->ab_addr;
+		ac->ac_response_block = login_resp;
+		ac->ac_login = ntohs(login_resp->login_id);
+		ac->ac_fetch_agent =
+		    ((u_int64_t)(ntohs(login_resp->fetch_agent.hi)) << 32) +
+		    ntohl(login_resp->fetch_agent.lo);
+		ac->ac_reconnect_hold = ntohs(login_resp->reconnect_hold);
+
+		DPRINTF(("Got a valid response\n"));
+		DPRINTF(("Login ID : 0x%04x, Command Agent : 0x%016qx\n",
+		    ac->ac_login, ac->ac_fetch_agent));
+
+		ac->ac_valid |= 1;
+	}
+	if (ab->ab_addr == SBP2_LOGIN_STATUS) {
+		MALLOC(cmd_status, struct sbp2_status_block *,
+		    sizeof(*cmd_status), M_1394DATA, M_WAITOK);
+		//MPRINTF_OLD("MALLOC(1394DATA)", cmd_status);
+		bzero(cmd_status, sizeof(*cmd_status));
+		bcopy(ab->ab_data, cmd_status, ab->ab_retlen);
+
+		/* Reset status ab for subsequent notifications. */
+		ab->ab_retlen = 0;
+		ab->ab_length = sizeof(*cmd_status);
+
+		src = (cmd_status->flags >> 6) & 0x3;
+		resp = (cmd_status->flags >> 4) & 0x3;
+		dead = (cmd_status->flags >> 3) & 0x1;
+		len = cmd_status->flags & 0x7;
+		status = cmd_status->status;
+		csr = ((u_int64_t)(ntohs(cmd_status->orb_offset_hi)) << 32) +
+		    ntohl(cmd_status->orb_offset_lo);
+		DPRINTF(("status -- src: %d, resp: %d, dead: %d, len: %d, "
+		    "status: %d\nstatus -- csr: 0x%016qx\n", src, resp, dead,
+		    (len + 1) * 4, status, (quad_t)csr));
+		if (ac->ac_valid & 4) {
+			DPRINTF(("Notify callback\n"));
+			elm = sbp2_elfind_hash(ac,
+			    (u_int32_t)(csr & 0xFFFFFFFF));
+			if (elm == NULL) {
+				DPRINTF(("%s: no element found for hash"
+				    " 0x%08x\n", __func__,
+				    (u_int32_t)(csr & 0xFFFFFFFF)));
+				FREE(cmd_status, M_1394DATA);
+				//MPRINTF_OLD("FREE(1394DATA)", cmd_status);
+				cmd_status = NULL;	/* XXX */
+				goto leave;
+			}
+			MALLOC(status_notify, struct sbp2_status_notification *,
+			    sizeof(*status_notify), M_1394CTL, M_WAITOK);
+			//MPRINTF_OLD("MALLOC(1394CTL)", status_notify);
+			status_notify->origin = elm->elm_orb;
+			status_notify->status = cmd_status;
+			elm->elm_cb(elm->elm_cbarg, status_notify);
+			FREE(status_notify, M_1394CTL);
+			//MPRINTF_OLD("FREE(1394CTL)", status_notify);
+			status_notify = NULL;	/* XXX */
+		} else if (((src & 2) == 0) && (resp == 0) && (dead == 0) &&
+		    (status == 0) && (csr == SBP2_LOGIN_ORB)) {
+			if (ac->ac_status_block) {
+				FREE(ac->ac_status_block, M_1394DATA);
+				//MPRINTF_OLD("FREE(1394DATA)", ac->ac_status_block);
+				ac->ac_status_block = NULL;	/* XXX */
+			}
+			ac->ac_status_block = cmd_status;
+			DPRINTF(("Got a valid status\n"));
+			ac->ac_valid |= 2;
+		} else {
+			FREE(cmd_status, M_1394DATA);
+			//MPRINTF_OLD("FREE(1394DATA)", cmd_status);
+			cmd_status = NULL;	/* XXX */
+		}
+	}
+	if (ac->ac_valid == 3) {
+		DPRINTF(("Valid response : notify callback\n"));
+		if (ac->ac_cb != NULL) {
+			MALLOC(status_notify, struct sbp2_status_notification *,
+			    sizeof(*status_notify), M_1394CTL, M_WAITOK);
+			//MPRINTF_OLD("MALLOC(1394CTL)", status_notify);
+			status_notify->origin = ac->ac_mgmt_orb;
+			status_notify->status = cmd_status;
+			ac->ac_cb(ac->ac_cbarg, status_notify);
+			FREE(status_notify, M_1394CTL);
+			//MPRINTF_OLD("FREE(1394CTL)", status_notify);
+			status_notify = NULL;	/* XXX */
+		}
+		ac->ac_valid |= 4;
+	}
+
+	/*
+	 * Leave the handler for status since unsolicited status will get sent
+	 * to the addr specified in the login packet.
+	 */
+
+leave:
+	if (ab->ab_data != NULL) {
+		free(ab->ab_data, M_1394DATA);
+		//MPRINTF_OLD("free(1394DATA)", ab->ab_data);
+		ab->ab_data = NULL;
+	}
+
+	if (ab->ab_addr != SBP2_LOGIN_STATUS) {
+		sc->sc1394_unreg(ab, FALSE);
+		FREE(ab, M_1394DATA);
+		//MPRINTF_OLD("FREE(1394DATA)", ab);
+		ab = NULL;	/* XXX */
+	}
+
+}
+
+void
+sbp2_query_logins(struct fwnode_softc *sc, struct sbp2_query_logins_orb *orb,
+    void (*cb)(struct sbp2_status_notification *))
+{
+}
+
+void
+sbp2_command_add(struct fwnode_softc *sc, int lun,
+    struct sbp2_command_orb *orb, int qlen, void *data,
+    void (*cb)(void *, struct sbp2_status_notification *), void *cbarg)
+{
+	struct ieee1394_abuf *ab, *ab2;
+	struct sbp2_account *ac;
+	struct sbp2_orb_element *elm, *elast;
+	u_int32_t ehash;
+
+	if ((ac = sbp2_acfind(sc, lun)) == NULL) {
+		DPRINTF(("%s: destination not initialized\n", __func__));
+		return;
+	}
+
+	DPRINTF(("%s:", __func__));
+
+	/* Initialise orb address hash. */
+	do {
+		ehash = arc4random() & 0xFFFFFFFC;	/* "quadlet" addr */
+	} while (sbp2_elfind_hash(ac, ehash) != NULL);
+
+	orb->next_orb.flag = htons(SBP2_NULL_ORB);
+	elast = TAILQ_LAST(&ac->ac_orb_head, sbp2_orb_tq);
+	if (elast != TAILQ_END(&ac->ac_orb_head)) {
+		elast->elm_orb->next_orb.hi = htons(SBP2_CMD_ORB >> 32);
+		elast->elm_orb->next_orb.lo = htonl(ehash);
+	}
+
+	MALLOC(elm, struct sbp2_orb_element *, sizeof(*elm),
+	    M_1394CTL, M_WAITOK);
+	//MPRINTF_OLD("MALLOC(1394CTL)", elm);
+	bzero(elm, sizeof(*elm));
+	elm->elm_hash = ehash;
+	elm->elm_orb = orb;
+	elm->elm_orblen = qlen;
+	elm->elm_data = data;
+	elm->elm_datasize = ntohs(orb->data_size);
+	elm->elm_cb = cb;
+	elm->elm_cbarg = cbarg;
+	TAILQ_INSERT_TAIL(&ac->ac_orb_head, elm, elm_chain);
+
+	MALLOC(ab2, struct ieee1394_abuf *, sizeof(*ab2), M_1394DATA, M_WAITOK);
+	//MPRINTF_OLD("MALLOC(1394DATA)", ab2);
+	bzero(ab2, sizeof(*ab2));
+
+	ab2->ab_length = sizeof(u_int32_t) * qlen;
+	ab2->ab_tcode = IEEE1394_TCODE_READ_REQUEST_DATABLOCK;
+	ab2->ab_retlen = 0;
+	ab2->ab_data = NULL;
+	ab2->ab_addr = SBP2_CMD_ORB + ehash;
+	ab2->ab_cb = sbp2_command_send;
+	ab2->ab_cbarg = ac;
+	ab2->ab_req = (struct ieee1394_softc *)sc;
+
+	elm->elm_orb_ab = ab2;
+	sc->sc1394_inreg(ab2, FALSE);
+
+	if (ac->ac_valid & 8) {
+		sbp2_agent_tickle(sc, lun);
+	} else {
+		MALLOC(ab, struct ieee1394_abuf *, sizeof(*ab),
+		    M_1394DATA, M_WAITOK);
+		//MPRINTF_OLD("MALLOC(1394DATA)", ab);
+		bzero(ab, sizeof(*ab));
+
+		ab->ab_req = (struct ieee1394_softc *)sc;
+		ab->ab_length = 8;
+		ab->ab_retlen = 0;
+		ab->ab_cb = NULL;
+		ab->ab_cbarg = NULL;
+		ab->ab_addr = ac->ac_fetch_agent + SBP2_ORB_POINTER;
+		ab->ab_tcode = IEEE1394_TCODE_WRITE_REQUEST_DATABLOCK;
+
+		ab->ab_data = malloc(8, M_1394DATA, M_WAITOK);
+		//MPRINTF_OLD("malloc(1394DATA)", ab->ab_data);
+		ab->ab_data[0] = htonl((u_int32_t)(SBP2_CMD_ORB >> 32));
+		ab->ab_data[1] = htonl(ehash);
+		DPRINTF((" CSR = 0x%016qx", ab->ab_addr));
+		DPRINTF((", ORB = 0x%016qx\n", SBP2_CMD_ORB + ehash));
+		ac->ac_valid |= 8;
+
+		sbp2_agent_reset(sc, lun);
+
+		sc->sc1394_write(ab);
+	}
+
+	DPRINTF(("%s: COMMAND submitted\n", __func__));
+
+	return;
+}
+
+void
+sbp2_command_del(struct fwnode_softc *sc, int lun, struct sbp2_command_orb *orb)
+{
+	struct sbp2_account *ac;
+	struct sbp2_orb_element *elm;
+
+	if ((ac = sbp2_acfind(sc, lun)) == NULL) {
+		DPRINTF(("%s: destination not initialized\n", __func__));
+		return;
+	}
+
+	DPRINTF(("%s:", __func__));
+
+	if ((elm = sbp2_elfind_orb(ac, orb)) == NULL) {
+#ifdef	SBP2_DEBUG
+		DPRINTF((" ORB not found: 0x%08x\n", (u_int32_t)orb));
+#endif	/* SBP2_DEBUG */
+		return;
+	}
+
+	DPRINTF((" orb=0x%08x len=%d data=0x%08x size=%d\n",
+	    (u_int32_t)(elm->elm_orb), elm->elm_orblen,
+	    (u_int32_t)(elm->elm_data), elm->elm_datasize));
+
+	if (elm->elm_orb_ab != NULL) {
+		sc->sc1394_unreg(elm->elm_orb_ab, FALSE);
+		if (elm->elm_orb_ab->ab_data != NULL) {
+			free(elm->elm_orb_ab->ab_data, M_1394DATA);
+			//MPRINTF_OLD("free(1394DATA)", elm->elm_orb_ab->ab_data);
+			elm->elm_orb_ab->ab_data = NULL;	/* XXX */
+		}
+		FREE(elm->elm_orb_ab, M_1394DATA);
+		//MPRINTF_OLD("FREE(1394DATA)", elm->elm_orb_ab);
+		elm->elm_orb_ab = NULL;	/* XXX */
+	}
+
+	TAILQ_REMOVE(&ac->ac_orb_head, elm, elm_chain);
+	FREE(elm, M_1394CTL);
+	//MPRINTF_OLD("FREE(1394CTL)", elm);
+	elm = NULL;	/* XXX */
+
+	if (TAILQ_EMPTY(&ac->ac_orb_head))
+		ac->ac_valid &= ~8;
+}
+
+void
+sbp2_command_send(struct ieee1394_abuf *ab, int rcode)
+{
+	struct fwnode_softc *sc = (struct fwnode_softc *)ab->ab_req;
+	struct sbp2_account *ac = ab->ab_cbarg;
+	struct sbp2_orb_element *elm, *next_elm;
+	struct sbp2_command_orb *cmd_orb, *next_orb;
+	struct ieee1394_abuf *cmd_ab;
+	int i;
+
+	/* Got a read so allocate the buffer and write out the response. */
+
+	if (rcode || (ac == NULL)) {
+#ifdef	SBP2_DEBUG
+		DPRINTF(("%s: Bad return code: %d\n", __func__, rcode));
+#endif	/* SBP2_DEBUG */
+		if (ab->ab_data) {
+			free(ab->ab_data, M_1394DATA);
+			//MPRINTF_OLD("free(1394DATA)", ab->ab_data);
+			ab->ab_data = NULL;	/* XXX */
+		}
+		FREE(ab, M_1394DATA);
+		//MPRINTF_OLD("FREE(1394DATA)", ab);
+		ab = NULL;	/* XXX */
+		return;
+	}
+
+	if (ab->ab_data != NULL) {
+		free(ab->ab_data, M_1394DATA);
+		//MPRINTF_OLD("free(1394DATA)", ab->ab_data);
+		ab->ab_data = NULL;
+	}
+
+	if ((elm = sbp2_elfind_hash(ac, (u_int32_t)(ab->ab_addr & 0xFFFFFFFF)))
+	    == NULL) {
+#ifdef	SBP2_DEBUG
+		DPRINTF(("%s: ORB not found: 0x%016qx\n", __func__,
+		    ab->ab_addr));
+#endif	/* SBP2_DEBUG */
+		if (ab->ab_data != NULL) {
+			free(ab->ab_data, M_1394DATA);
+			//MPRINTF_OLD("free(1394DATA)", ab->ab_data);
+			ab->ab_data = NULL;	/* XXX */
+		}
+		FREE(ab, M_1394DATA);
+		//MPRINTF_OLD("FREE(1394DATA)", ab);
+		ab = NULL;	/* XXX */
+		return;
+	}
+
+	DPRINTF(("%s: orb=0x%08x len=%d data=0x%08x size=%d (l=%d rl=%d)\n",
+	    __func__, (u_int32_t)(elm->elm_orb), elm->elm_orblen,
+	    (u_int32_t)(elm->elm_data), elm->elm_datasize,
+	    ab->ab_length, ab->ab_retlen));
+
+	MALLOC(cmd_ab, struct ieee1394_abuf *, sizeof(*cmd_ab),
+	    M_1394DATA, M_WAITOK);
+	//MPRINTF_OLD("MALLOC(1394DATA)", cmd_ab);
+	bcopy(ab, cmd_ab, sizeof(*cmd_ab));
+
+	/* Fill in a command packet. */
+	cmd_orb = elm->elm_orb;
+	if ((next_elm = TAILQ_NEXT(elm, elm_chain))
+	    != TAILQ_END(&ac->ac_orb_head)) {
+		next_orb = next_elm->elm_orb;
+		cmd_orb->next_orb.flag = 0x0000;
+		cmd_orb->next_orb.hi = htons(SBP2_CMD_ORB >> 32);
+		cmd_orb->next_orb.lo = htonl(next_elm->elm_hash);
+	} else {
+		cmd_orb->next_orb.flag = htons(SBP2_NULL_ORB);
+		cmd_orb->next_orb.hi = 0x0000;
+		cmd_orb->next_orb.lo = 0x00000000;
+	}
+
+	cmd_ab->ab_retlen = 0;
+	cmd_ab->ab_cb = NULL;
+	cmd_ab->ab_cbarg = NULL;
+	cmd_ab->ab_tcode = IEEE1394_TCODE_READ_RESPONSE_DATABLOCK;
+	cmd_ab->ab_length = ab->ab_retlen;
+
+	cmd_ab->ab_data = malloc(elm->elm_orblen * 4, M_1394DATA, M_WAITOK);
+	//MPRINTF_OLD("malloc(1394DATA)", cmd_ab->ab_data);
+	bcopy(cmd_orb, cmd_ab->ab_data, elm->elm_orblen * 4);
+	for (i = 0; i < elm->elm_orblen; i++) {
+		if ((i % 8) == 0) DPRINTF(("   "));
+		DPRINTF((" %08x", ntohl(cmd_ab->ab_data[i])));
+		if ((i % 8) == 7 && i != (elm->elm_orblen - 1)) DPRINTF(("\n"));
+	}
+	DPRINTF(("\n"));
+
+	sc->sc1394_write(cmd_ab);
+}
+
+void
+sbp2_agent_reset(struct fwnode_softc *sc, int lun)
+{
+	struct ieee1394_abuf *ab;
+	struct sbp2_account *ac;
+
+	if ((ac = sbp2_acfind(sc, lun)) == NULL) {
+		DPRINTF(("%s: destination not initialized\n", __func__));
+		return;
+	}
+
+	DPRINTF(("%s:", __func__));
+
+	MALLOC(ab, struct ieee1394_abuf *, sizeof(*ab), M_1394DATA, M_WAITOK);
+	//MPRINTF_OLD("MALLOC(1394DATA)", ab);
+	bzero(ab, sizeof(*ab));
+
+	ab->ab_req = (struct ieee1394_softc *)sc;
+	ab->ab_length = 4;
+	ab->ab_retlen = 0;
+	ab->ab_cb = NULL;
+	ab->ab_cbarg = NULL;
+	ab->ab_addr = ac->ac_fetch_agent + SBP2_AGENT_RESET;
+	ab->ab_tcode = IEEE1394_TCODE_WRITE_REQUEST_QUADLET;
+
+	ab->ab_data = malloc(4, M_1394DATA, M_WAITOK);
+	//MPRINTF_OLD("malloc(1394DATA)", ab->ab_data);
+	ab->ab_data[0] = 0;
+	DPRINTF((" CSR = 0x%016qx\n", ab->ab_addr));
+
+	sc->sc1394_write(ab);
+	DPRINTF(("%s: AGENT_RESET submitted\n", __func__));
+
+	return;
+}
+
+void
+sbp2_agent_tickle(struct fwnode_softc *sc, int lun)
+{
+	struct ieee1394_abuf *ab;
+	struct sbp2_account *ac;
+
+	if ((ac = sbp2_acfind(sc, lun)) == NULL) {
+		DPRINTF(("%s: destination not initialized\n", __func__));
+		return;
+	}
+
+	DPRINTF(("%s:", __func__));
+
+	MALLOC(ab, struct ieee1394_abuf *, sizeof(*ab), M_1394DATA, M_WAITOK);
+	//MPRINTF_OLD("MALLOC(1394DATA)", ab);
+	bzero(ab, sizeof(*ab));
+
+	ab->ab_req = (struct ieee1394_softc *)sc;
+	ab->ab_length = 4;
+	ab->ab_retlen = 0;
+	ab->ab_cb = NULL;
+	ab->ab_cbarg = NULL;
+	ab->ab_addr = ac->ac_fetch_agent + SBP2_DOORBEL;
+	ab->ab_tcode = IEEE1394_TCODE_WRITE_REQUEST_QUADLET;
+
+	ab->ab_data = malloc(4, M_1394DATA, M_WAITOK);
+	//MPRINTF_OLD("malloc(1394DATA)", ab->ab_data);
+	ab->ab_data[0] = 0;
+	DPRINTF((" CSR = 0x%016qx\n", ab->ab_addr));
+
+	sc->sc1394_write(ab);
+	DPRINTF(("%s: DOORBEL submitted\n", __func__));
+
+	return;
+}
+
+int
+sbp2_agent_state(struct fwnode_softc *sc, int lun)
+{
+	return (0);
+}
+
+void
+sbp2_task_management(struct fwnode_softc *sc,
+    struct sbp2_task_management_orb *orb,
+    void (*cb)(struct sbp2_status_notification *))
+{
+}
+
diff --git a/sys/dev/std/sbp2reg.h b/sys/dev/std/sbp2reg.h
new file mode 100644
index 00000000000..6611d30d94b
--- /dev/null
+++ b/sys/dev/std/sbp2reg.h
@@ -0,0 +1,102 @@
+/*	$OpenBSD: sbp2reg.h,v 1.1 2002/12/13 02:57:56 tdeval Exp $	*/
+
+/*
+ * Copyright (c) 2002 Thierry Deval.  All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
+ * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
+ * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
+ * IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
+ * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+ * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+ * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
+ * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef	_DEV_STD_SBP2REG_H_
+#define	_DEV_STD_SBP2REG_H_
+
+#define	SBP2_UNIT_SPEC_ID			0x00609E	/* NCITS OUI */
+#define	SBP2_UNIT_SW_VERSION			0x010483
+
+/*	0x38/8, oui/24					Unit Directory
+ */
+#define	SBP2_KEYVALUE_Command_Set_Spec_Id	0x38	/* immediate (OUI) */
+
+/*	0x39/8, oui/24					Unit Directory
+ */
+#define	SBP2_KEYVALUE_Command_Set		0x39	/* immediate (OUI) */
+
+/*	0x54/8, offset/24				Unit Directory
+ */
+#define	SBP2_KEYVALUE_Management_Agent \
+		P1212_KEYVALUE_Unit_Dependent_Info	/* offset */
+
+/*	0x3A/8, reserved/8,				Unit Directory
+ *	msg_ORB_timeout/8, ORB_size/8
+ */
+#define	SBP2_KEYVALUE_Unit_Characteristics	0x3A	/* immediate */
+
+/*	0x3B/8, reserver/8, max_reconnect_hold/16	Unit Directory
+ */
+#define	SBP2_KEYVALUE_Command_Set_Revision	0x3B	/* immediate */
+
+/*	0x3C/8, firmware_revision/24			Unit Directory
+ */
+#define	SBP2_KEYVALUE_Firmware_Revision		0x3C	/* immediate */
+
+/*	0x3D/8, reserver/8, max_reconnect_hold/16	Unit Directory
+ */
+#define	SBP2_KEYVALUE_Reconnect_Timeout		0x3D	/* immediate */
+
+/*	0xD4/8, indirect_offset/24			Unit Directory
+ */
+#define	SBP2_KEYVALUE_Logical_Unit_Directory \
+		P1212_KEYVALUE_Unit_Dependent_Info	/* directory */
+
+/*	0x14/8, reserved/1, ordered/1, reserved/1	Unit Directory
+ *	device_type/5, lun/16
+ */
+#define	SBP2_KEYVALUE_Logical_Unit_Number \
+		P1212_KEYVALUE_Unit_Dependent_Info	/* immediate */
+
+/*	0x8D/8, indirect_offset/24			Unit Directory
+ */
+#define	SBP2_KEYVALUE_Unit_Unique_Id \
+		P1212_KEYVALUE_Node_Unique_Id		/* leaf */
+
+#define	SBP2_ORB_LOGIN				0
+#define	SBP2_ORB_QUERY_LOGINS			1
+#define	SBP2_ORB_RECONNECT			3
+#define	SBP2_ORB_LOGOUT				7
+
+#define	SBP2_NULL_ORB_PTR			0x8000000000000000
+
+#define	SBP2_LOGIN_ORB				0x0000400000000000
+#define	SBP2_LOGIN_RESP				0x0000400000000020
+#define	SBP2_LOGIN_STATUS			0x0000400000000030
+
+#define	SBP2_CMD_ORB				0x0000440000000000
+#define	SBP2_CMD_DATA				0x0000450000000000
+
+#define	SBP2_AGENT_STATE			0x00
+#define	SBP2_AGENT_RESET			0x04
+#define	SBP2_ORB_POINTER			0x08
+#define	SBP2_DOORBEL				0x10
+#define	SBP2_UNSOLICITED_STATUS_ENABLE		0x14
+
+#define	SBP2_MAX_TRANS				(1024 * 1024)
+#define	SBP2_MAX_LUNS				8
+
+#endif	/* _DEV_STD_SBP2REG_H_ */
diff --git a/sys/dev/std/sbp2var.h b/sys/dev/std/sbp2var.h
new file mode 100644
index 00000000000..fe85b5ea434
--- /dev/null
+++ b/sys/dev/std/sbp2var.h
@@ -0,0 +1,188 @@
+/*	$OpenBSD: sbp2var.h,v 1.1 2002/12/13 02:57:56 tdeval Exp $	*/
+
+/*
+ * Copyright (c) 2002 Thierry Deval.  All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
+ * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
+ * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
+ * IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
+ * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+ * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+ * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
+ * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef	_DEV_STD_SBP2VAR_H_
+#define	_DEV_STD_SBP2VAR_H_
+
+struct fwnode_softc;
+struct fwnode_device_cap;
+struct p1212_dir;
+
+typedef struct sbp2_addr {
+	u_int16_t	node_id;
+	u_int16_t	hi;
+	u_int32_t	lo;
+} sbp2_addr_t;
+
+typedef struct sbp2_orb {
+	u_int16_t	flag;
+#define	SBP2_NULL_ORB				0x8000
+	u_int16_t	hi;
+	u_int32_t	lo;
+} sbp2_orb_t;
+
+typedef struct sbp2_pte_u {
+	u_int16_t	segmentLength;
+	u_int16_t	segmentBaseAddress_Hi;
+	u_int32_t	segmentBaseAddress_Lo;
+} sbp2_pte_u;
+
+typedef struct sbp2_pte_n {
+	u_int16_t	segmentLength;
+	u_int16_t	segmentBaseAddress_Hi;
+	u_int32_t	segmentBaseAddress_Offset;
+} sbp2_pte_n;
+
+typedef struct sbp2_command_orb {
+	sbp2_orb_t	next_orb;
+	sbp2_addr_t	data_descriptor;
+	u_int16_t	options;
+#define	SBP2_DUMMY_TYPE				0x6000
+	u_int16_t	data_size;
+	u_int32_t	command_block[1];	/* most certainly more */
+} sbp2_command_orb;
+
+typedef struct sbp2_task_management_orb {
+	sbp2_orb_t	orb_offset;
+	u_int32_t	reserved[2];
+	u_int16_t	options;
+	u_int16_t	login_id;
+	u_int32_t	reserved2;
+	sbp2_addr_t	status_fifo;
+} sbp2_task_management_orb;
+
+typedef struct sbp2_login_orb {
+	sbp2_addr_t	password;
+	sbp2_addr_t	login_response;
+	u_int16_t	options;
+	u_int16_t	lun;
+	u_int16_t	password_length;
+	u_int16_t	login_response_length;
+	sbp2_addr_t	status_fifo;
+} sbp2_login_orb;
+
+typedef struct sbp2_login_response {
+	u_int16_t	length;
+	u_int16_t	login_id;
+	sbp2_addr_t	fetch_agent;
+	u_int16_t	reserved;
+	u_int16_t	reconnect_hold;
+} sbp2_login_response;
+
+typedef struct sbp2_query_logins_orb {
+	u_int32_t	reserved[2];
+	sbp2_addr_t	query_response;
+	u_int16_t	options;
+	u_int16_t	lun;
+	u_int16_t	reserved2;
+	u_int16_t	query_response_length;
+	sbp2_addr_t	status_fifo;
+} sbp2_query_logins_orb;
+
+typedef struct sbp2_query_logins_response {
+	u_int16_t	length;
+	u_int16_t	max_logins;
+	struct {
+		u_int16_t	node_id;
+		u_int16_t	login_id;
+		sbp2_addr_t	initiator;
+	}		login_info[1];		/* most certainly more */
+} sbp2_query_logins_response;
+
+typedef struct sbp2_status_block {
+	u_int8_t	flags;
+#define	SBP2_STATUS_SRC_MASK			0xC0
+#define	SBP2_STATUS_RESPONSE_MASK		0x30
+#define	SBP2_STATUS_RESP_REQ_COMPLETE		0x00
+#define	SBP2_STATUS_RESP_TRANS_FAILURE		0x10
+#define	SBP2_STATUS_RESP_ILLEGAL_REQ		0x20
+#define	SBP2_STATUS_RESP_VENDOR			0x30
+#define	SBP2_STATUS_DEAD			0x08
+#define	SBP2_STATUS_LEN_MASK			0x07
+	u_int8_t	status;
+#define	SBP2_STATUS_NONE			0x00
+#define	SBP2_STATUS_UNSUPPORTED_TYPE		0x01
+#define	SBP2_STATUS_UNSUPPORTED_SPEED		0x02
+#define	SBP2_STATUS_UNSUPPORTED_PGSIZ		0x03
+#define	SBP2_STATUS_ACCESS_DENIED		0x04
+#define	SBP2_STATUS_UNSUPPORTED_LUN		0x05
+#define	SBP2_STATUS_MAX_PAYLOAD_SMALL		0x06
+#define	SBP2_STATUS_RESOURCE_UNAVAIL		0x08
+#define	SBP2_STATUS_FUNCTION_REJECTED		0x09
+#define	SBP2_STATUS_INVALID_LOGINID		0x0A
+#define	SBP2_STATUS_DUMMY_ORB_COMPLETE		0x0B
+#define	SBP2_STATUS_REQUEST_ABORTED		0x0C
+#define	SBP2_STATUS_UNSPECIFIED			0xFF
+#define	SBP2_STATUS_OBJECT_MASK			0xC0
+#define	SBP2_STATUS_OBJECT_ORB			0x00
+#define	SBP2_STATUS_OBJECT_DATA_BUF		0x40
+#define	SBP2_STATUS_OBJECT_PAGE_TABLE		0x80
+#define	SBP2_STATUS_OBJECT_UNSPECIFIED		0xC0
+#define	SBP2_STATUS_SERIAL_MASK			0x0F
+#define	SBP2_STATUS_SERIAL_ACK_MISSING		0x00
+#define	SBP2_STATUS_SERIAL_TIMEOUT		0x02
+#define	SBP2_STATUS_SERIAL_ACK_BUSY_X		0x04
+#define	SBP2_STATUS_SERIAL_ACK_BUSY_A		0x05
+#define	SBP2_STATUS_SERIAL_ACK_BUSY_B		0x06
+#define	SBP2_STATUS_SERIAL_TARDY_RETRY		0x0B
+#define	SBP2_STATUS_SERIAL_CONFLICT		0x0C
+#define	SBP2_STATUS_SERIAL_DATA			0x0D
+#define	SBP2_STATUS_SERIAL_TYPE			0x0E
+#define	SBP2_STATUS_SERIAL_ADDRESS		0x0F
+	u_int16_t	orb_offset_hi;
+	u_int32_t	orb_offset_lo;
+	u_int32_t	status_info[6];
+} sbp2_status_block;
+
+
+/*
+ * A Status Notification structure containing both a reference to the
+ * original ORB, and the returned status info.
+ */
+typedef struct sbp2_status_notification {
+	void *origin;
+	struct sbp2_status_block *status;
+} sbp2_status_notification;
+
+
+int  sbp2_match(struct device *, void *, void *);
+int  sbp2_init(struct fwnode_softc *, struct p1212_dir *);
+void sbp2_clean(struct fwnode_softc *, struct p1212_dir *, int);
+void sbp2_login(struct fwnode_softc *, struct sbp2_login_orb *,
+    void (*)(void *, struct sbp2_status_notification *), void *);
+void sbp2_query_logins(struct fwnode_softc *, struct sbp2_query_logins_orb *,
+    void (*)(struct sbp2_status_notification *));
+void sbp2_command_add(struct fwnode_softc *, int, struct sbp2_command_orb *,
+    int, void *, void (*)(void *, struct sbp2_status_notification *), void *);
+void sbp2_command_del(struct fwnode_softc *, int, struct sbp2_command_orb *);
+void sbp2_agent_reset(struct fwnode_softc *, int);
+void sbp2_agent_tickle(struct fwnode_softc *, int);
+int  sbp2_agent_state(struct fwnode_softc *, int);
+void sbp2_task_management(struct fwnode_softc *,
+    struct sbp2_task_management_orb *,
+    void (*)(struct sbp2_status_notification *));
+
+#endif	/* _DEV_STD_SBP2VAR_H_ */