Implement the reconnect callback. This will be called whenever an SBP2 node's

node_id changes, following a BusReset (happens when you add/remove a device
to/from the FireWire chain).

Re-work the ORB management to support concurrent node accesses.

1 files changed, 440 insertions, 167 deletions

diff --git a/sys/dev/std/sbp2.c b/sys/dev/std/sbp2.c
index 8cd7242ac15..d8b0c915c23 100644
--- a/sys/dev/std/sbp2.c
+++ b/sys/dev/std/sbp2.c
@@ -1,4 +1,4 @@
-/*	$OpenBSD: sbp2.c,v 1.2 2002/12/13 21:35:11 tdeval Exp $	*/
+/*	$OpenBSD: sbp2.c,v 1.3 2002/12/30 11:48:34 tdeval Exp $	*/
 
 /*
  * Copyright (c) 2002 Thierry Deval.  All rights reserved.
@@ -82,12 +82,16 @@
 #include <dev/std/ieee1212var.h>
 #include <dev/std/sbp2var.h>
 
-int sbp2_print_data(struct p1212_data *);
-int sbp2_print_dir(struct p1212_dir *);
+#if 0
+void sbp2_print(struct p1212_dir *);
+void sbp2_print_node(struct p1212_key *, void *);
+#endif
 
 void sbp2_login_send(struct ieee1394_abuf *, int);
 void sbp2_status_resp(struct ieee1394_abuf *, int);
 void sbp2_command_send(struct ieee1394_abuf *, int);
+void sbp2_reconnect(struct ieee1394_softc *);
+void sbp2_reconnect_send(struct ieee1394_abuf *, int);
 
 #ifdef	SBP2_DEBUG
 #include <sys/syslog.h>
@@ -119,11 +123,34 @@ int	sbp2debug = 0;
 #define	MPRINTF(x,y)
 #endif	/* ! SBP2_DEBUG */
 
+typedef struct sbp2_account {
+	struct fwnode_softc	 *ac_softc;
+	struct ieee1394_abuf	 *ac_status_ab;
+	struct sbp2_task_management_orb *ac_mgmt_orb;
+	struct sbp2_status_block *ac_status_block;
+	void			 *ac_response_block;
+	void			(*ac_cb)(void *,
+					 struct sbp2_status_notification *);
+	void			 *ac_cbarg;
+	u_int64_t		  ac_mgmt_agent;
+	u_int64_t		  ac_fetch_agent;
+	u_int64_t		  ac_response;
+	u_int64_t		  ac_status_fifo;
+	u_int16_t		  ac_nodeid;
+	u_int16_t		  ac_lun;
+	u_int16_t		  ac_login;
+	u_int16_t		  ac_reconnect_hold;
+	u_int16_t		  ac_valid;
+	SLIST_ENTRY(sbp2_account) ac_chain;
+} sbp2_account;
+static SLIST_HEAD(, sbp2_account) sbp2_ac_head;
+
 typedef struct sbp2_orb_element {
-	u_int32_t		  elm_hash;
+	struct sbp2_account	 *elm_ac;
 	struct ieee1394_abuf	 *elm_orb_ab;
 	struct sbp2_command_orb	 *elm_orb;
 	size_t			  elm_orblen;
+	u_int32_t		  elm_hash;
 	void			 *elm_data;
 	size_t			  elm_datasize;
 	void			(*elm_cb)(void *,
@@ -131,35 +158,15 @@ typedef struct sbp2_orb_element {
 	void			 *elm_cbarg;
 	TAILQ_ENTRY(sbp2_orb_element) elm_chain;
 } sbp2_orb_element;
+static TAILQ_HEAD(sbp2_orb_tq, sbp2_orb_element) sbp2_elm_head;
 
-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_orb_element *sbp2_elfind_hash(u_int32_t);
+struct sbp2_orb_element *sbp2_elfind_orb(struct sbp2_command_orb *);
+struct sbp2_orb_element *sbp2_elfind_first(struct sbp2_account *);
+struct sbp2_orb_element *sbp2_elfind_last(struct sbp2_account *);
 
 struct sbp2_account *
 sbp2_acfind(struct fwnode_softc *sc, int lun)
@@ -175,11 +182,11 @@ sbp2_acfind(struct fwnode_softc *sc, int lun)
 }
 
 struct sbp2_orb_element *
-sbp2_elfind_hash(struct sbp2_account *ac, u_int32_t hash)
+sbp2_elfind_hash(u_int32_t hash)
 {
 	struct sbp2_orb_element *elm;
 
-	TAILQ_FOREACH(elm, &ac->ac_orb_head, elm_chain) {
+	TAILQ_FOREACH(elm, &sbp2_elm_head, elm_chain) {
 		if (elm->elm_hash == hash)
 			break;
 	}
@@ -188,11 +195,11 @@ sbp2_elfind_hash(struct sbp2_account *ac, u_int32_t hash)
 }
 
 struct sbp2_orb_element *
-sbp2_elfind_orb(struct sbp2_account *ac, struct sbp2_command_orb *orb)
+sbp2_elfind_orb(struct sbp2_command_orb *orb)
 {
 	struct sbp2_orb_element *elm;
 
-	TAILQ_FOREACH(elm, &ac->ac_orb_head, elm_chain) {
+	TAILQ_FOREACH(elm, &sbp2_elm_head, elm_chain) {
 		if (elm->elm_orb == orb)
 			break;
 	}
@@ -200,7 +207,33 @@ sbp2_elfind_orb(struct sbp2_account *ac, struct sbp2_command_orb *orb)
 	return (elm);
 }
 
-int
+struct sbp2_orb_element *
+sbp2_elfind_first(struct sbp2_account *ac)
+{
+	struct sbp2_orb_element *elm;
+
+	TAILQ_FOREACH(elm, &sbp2_elm_head, elm_chain) {
+		if (elm->elm_ac == ac)
+			break;
+	}
+
+	return (elm);
+}
+
+struct sbp2_orb_element *
+sbp2_elfind_last(struct sbp2_account *ac)
+{
+	struct sbp2_orb_element *elm;
+
+	TAILQ_FOREACH_REVERSE(elm, &sbp2_elm_head, elm_chain, sbp2_orb_tq) {
+		if (elm->elm_ac == ac)
+			break;
+	}
+
+	return (elm);
+}
+
+void
 sbp2_print_data(struct p1212_data *data)
 {
 	struct p1212_key *key = (struct p1212_key *)data;
@@ -208,7 +241,7 @@ sbp2_print_data(struct p1212_data *data)
 	switch (key->key_value) {
 	case SBP2_KEYVALUE_Command_Set:
 		DPRINTF(("SBP2 Command Set: "));
-		if (key->val == 0x104d8)
+		if (key->val == 0x104D8)
 			DPRINTF(("SCSI 2\n"));
 		else
 			DPRINTF(("0x%08x\n", key->val));
@@ -238,12 +271,10 @@ sbp2_print_data(struct p1212_data *data)
 			DPRINTF(("SBP2 Management Agent: 0x%08x\n", key->val));
 		break;
 	default:
-		return 0;
 	}
-	return 1;
 }
 
-int
+void
 sbp2_print_dir(struct p1212_dir *dir)
 {
 	u_int8_t dir_type = ((struct p1212_key *)dir)->key_type;
@@ -253,9 +284,7 @@ sbp2_print_dir(struct p1212_dir *dir)
 		DPRINTF(("Logical Unit "));
 		break;
 	default:
-		return 0;
 	}
-	return 1;
 }
 
 int
@@ -266,12 +295,18 @@ sbp2_init(struct fwnode_softc *sc, struct p1212_dir *unitdir)
 	struct sbp2_account *ac;
 	int loc;
 
+#ifdef	SBP2_DEBUG
+	if (sbp2debug > 1)
+		p1212_print(unitdir);
+#endif
+
 	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);
+		TAILQ_INIT(&sbp2_elm_head);
 		sbp2_ac_valid = 1;
 	}
 
@@ -280,8 +315,9 @@ sbp2_init(struct fwnode_softc *sc, struct p1212_dir *unitdir)
 	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,
+	DPRINTF(("%s: Node %d (sc 0x%08x):"
+	    " UID %02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x\n",
+	    __func__, sc->sc_sc1394.sc1394_node_id, sc,
 	    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],
@@ -291,13 +327,14 @@ sbp2_init(struct fwnode_softc *sc, struct p1212_dir *unitdir)
 	key = NULL;	/* XXX */
 
 	ac->ac_softc = sc;
-	ac->ac_login = sc->sc_sc1394.sc1394_node_id;
+	ac->ac_nodeid = sc->sc_sc1394.sc1394_node_id;
+	ac->ac_login = 0;
 	ac->ac_mgmt_agent = CSR_BASE + 4 * loc;
-	DPRINTF(("%s: mgmt_agent = 0x%016qx\n", __func__, ac->ac_mgmt_agent));
+	DPRINTF(("%s: mgmt_agent = 0x%012qx\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;
+		ac->ac_lun = (*key)->val & 0xFFFF;
 		free(key, M_DEVBUF);
 		MPRINTF("free(DEVBUF)", key);
 		key = NULL;	/* XXX */
@@ -310,15 +347,13 @@ sbp2_init(struct fwnode_softc *sc, struct p1212_dir *unitdir)
 		key = p1212_find(dir, P1212_KEYTYPE_Immediate,
 		    SBP2_KEYVALUE_Logical_Unit_Number, 0);
 		if (key != NULL) {
-			ac->ac_lun = (*key)->val & 0xffff;
+			ac->ac_lun = (*key)->val & 0xFFFF;
 			free(key, M_DEVBUF);
 			MPRINTF("free(DEVBUF)", key);
 			key = NULL;	/* XXX */
 		}
 	}
-	DPRINTF(("%s: lun = %d\n", __func__, ac->ac_lun));
-
-	TAILQ_INIT(&ac->ac_orb_head);
+	DPRINTF(("%s: lun = %04x\n", __func__, ac->ac_lun));
 
 	SLIST_INSERT_HEAD(&sbp2_ac_head, ac, ac_chain);
 
@@ -335,22 +370,18 @@ sbp2_clean(struct fwnode_softc *sc, struct p1212_dir *unitdir, int logout)
 	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;
+		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,
+			TAILQ_FOREACH_REVERSE(elm, &sbp2_elm_head, elm_chain,
 			    sbp2_orb_tq) {
 				DPRINTF(("%s%d", i++?" ":"", i));
-				if (elm != NULL) {
-					TAILQ_REMOVE(&ac->ac_orb_head, elm,
+				if (elm != NULL && elm->elm_ac == ac) {
+					TAILQ_REMOVE(&sbp2_elm_head, elm,
 					    elm_chain);
 					FREE(elm, M_1394CTL);
 					MPRINTF("FREE(1394CTL)", elm);
@@ -386,13 +417,14 @@ sbp2_clean(struct fwnode_softc *sc, struct p1212_dir *unitdir, int logout)
 			key = p1212_find(d, P1212_KEYTYPE_Immediate,
 			    SBP2_KEYVALUE_Logical_Unit_Number, 0);
 			if (key != NULL) {
-				lun = (*key)->val & 0xffff;
+				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,
+					TAILQ_FOREACH(elm, &sbp2_elm_head,
 					    elm_chain) {
-						if (elm != NULL) {
+						if (elm != NULL &&
+						    elm->elm_ac == ac) {
 							FREE(elm, M_1394CTL);
 							MPRINTF("FREE(1394CTL)", elm);
 							elm = NULL;	/* XXX */
@@ -441,6 +473,7 @@ sbp2_login(struct fwnode_softc *sc, struct sbp2_login_orb *orb,
 {
 	struct ieee1394_abuf *ab, *ab2;
 	struct sbp2_account *ac;
+	u_int64_t addr;
 
 	if ((ac = sbp2_acfind(sc, ntohs(orb->lun))) == NULL) {
 		DPRINTF(("%s: destination not initialized\n", __func__));
@@ -472,16 +505,20 @@ sbp2_login(struct fwnode_softc *sc, struct sbp2_login_orb *orb,
 
 	ab->ab_data = malloc(8, M_1394DATA, M_WAITOK);
 	MPRINTF("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));
+
+	addr = SBP2_MGMT_ORB +
+	    ((u_int64_t)ac->ac_nodeid << SBP2_NODE_SHIFT) +
+	    ((u_int64_t)ac->ac_lun << SBP2_LUN_SHIFT);
+	ab->ab_data[0] = htonl((u_int32_t)(addr >> 32));
+	ab->ab_data[1] = htonl((u_int32_t)(addr & 0xFFFFFFFF));
+	DPRINTF((" CSR = 0x%012qx", ac->ac_mgmt_agent));
+	DPRINTF((", ORB = 0x%012qx\n", addr));
 
 	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_addr = addr;
 	ab2->ab_cb = sbp2_login_send;
 	ab2->ab_cbarg = ac;
 	ab2->ab_req = (struct ieee1394_softc *)sc;
@@ -489,7 +526,6 @@ sbp2_login(struct fwnode_softc *sc, struct sbp2_login_orb *orb,
 	sc->sc1394_inreg(ab2, FALSE);
 	sc->sc1394_write(ab);
 	DPRINTF(("%s: LOGIN submitted\n", __func__));
-	return;
 }
 
 void
@@ -499,6 +535,7 @@ sbp2_login_send(struct ieee1394_abuf *ab, int rcode)
 	struct sbp2_account *ac = ab->ab_cbarg;
 	struct sbp2_login_orb *login_orb;
 	struct ieee1394_abuf *stat_ab, *resp_ab, *orb_ab;
+	u_int64_t addr;
 #ifdef	SBP2_DEBUG
 	int i;
 #endif	/* SBP2_DEBUG */
@@ -544,22 +581,49 @@ sbp2_login_send(struct ieee1394_abuf *ab, int rcode)
 	bzero(resp_ab, sizeof(*resp_ab));
 	bzero(stat_ab, sizeof(*stat_ab));
 
+	/* 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. */
+	addr = SBP2_RESP_BLOCK +
+	    ((u_int64_t)ac->ac_nodeid << SBP2_NODE_SHIFT) +
+	    ((u_int64_t)ac->ac_lun << SBP2_LUN_SHIFT);
 	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_addr = addr;
 	resp_ab->ab_cb = sbp2_status_resp;
 	resp_ab->ab_cbarg = ac;
 	resp_ab->ab_req = orb_ab->ab_req;
 
+	login_orb->login_response.hi = htons((u_int16_t)(addr >> 32));
+	login_orb->login_response.lo = htonl((u_int32_t)(addr & 0xFFFFFFFF));
+	DPRINTF(("%s: RESP_ORB = 0x%012qx", __func__, addr));
+
 	sc->sc1394_inreg(resp_ab, FALSE);
 
+	/* Set notify and exclusive use bits. */
+	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. */
+#if 0
+	addr = SBP2_STATUS_BLOCK +
+	    ((u_int64_t)ac->ac_nodeid << SBP2_NODE_SHIFT) +
+	    ((u_int64_t)ac->ac_lun << SBP2_LUN_SHIFT);
+#else
+	addr = SBP2_STATUS_BLOCK;
+#endif
 	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_addr = addr;
 	stat_ab->ab_cb = sbp2_status_resp;
 	stat_ab->ab_cbarg = ac;
 	stat_ab->ab_req = orb_ab->ab_req;
@@ -567,26 +631,9 @@ sbp2_login_send(struct ieee1394_abuf *ab, int rcode)
 	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));
+	login_orb->status_fifo.hi = htons((u_int16_t)(addr >> 32));
+	login_orb->status_fifo.lo = htonl((u_int32_t)(addr & 0xFFFFFFFF));
+	DPRINTF((", STATUS_ORB = 0x%012qx", addr));
 
 	orb_ab->ab_data = malloc(sizeof(*login_orb), M_1394DATA, M_WAITOK);
 	MPRINTF("malloc(1394DATA)", orb_ab->ab_data);
@@ -618,19 +665,41 @@ sbp2_status_resp(struct ieee1394_abuf *ab, int rcode)
 	struct sbp2_status_notification *status_notify;
 	struct sbp2_orb_element *elm;
 	int resp, src, status, len, dead;
-	u_int64_t csr;
+	u_int64_t csr, stat_addr = 0;
 #ifdef	SBP2_DEBUG
 	int i;
 #endif	/* SBP2_DEBUG */
 
-	if (rcode || (ac == NULL)) {
+	if (!ac) {
+		DPRINTF(("%s: Callback Arg is NULL\n", __func__));
+
+		if (ab->ab_data) {
+			free(ab->ab_data, M_1394DATA);
+			MPRINTF("free(1394DATA)", ab->ab_data);
+			ab->ab_data = NULL;	/* XXX */
+		}
+		FREE(ab, M_1394DATA);
+		MPRINTF("FREE(1394DATA)", ab);
+		ab = NULL;	/* XXX */
+		return;
+	}
+#if 0
+	stat_addr = SBP2_STATUS_BLOCK +
+	    ((u_int64_t)ac->ac_nodeid << SBP2_NODE_SHIFT) +
+	    ((u_int64_t)ac->ac_lun << SBP2_LUN_SHIFT);
+#else
+	stat_addr = SBP2_STATUS_BLOCK;
+#endif
+
+	if (rcode) {
 		DPRINTF(("%s: Bad return code: %d\n", __func__, rcode));
+
 		if (ab->ab_data) {
 			free(ab->ab_data, M_1394DATA);
 			MPRINTF("free(1394DATA)", ab->ab_data);
 			ab->ab_data = NULL;	/* XXX */
 		}
-		if (ab->ab_addr != SBP2_LOGIN_STATUS) {
+		if (ab->ab_addr != stat_addr) {
 			FREE(ab, M_1394DATA);
 			MPRINTF("FREE(1394DATA)", ab);
 			ab = NULL;	/* XXX */
@@ -639,7 +708,8 @@ sbp2_status_resp(struct ieee1394_abuf *ab, int rcode)
 	}
 
 #ifdef	SBP2_DEBUG
-	DPRINTF(("%s: CSR = 0x%016qx", __func__, (quad_t)ab->ab_addr));
+	DPRINTF(("%s: CSR = 0x%012qx, ac = 0x%08x", __func__,
+	    (quad_t)ab->ab_addr, (u_int32_t)ac));
 	for (i = 0; i < (ab->ab_retlen / 4); i++) {
 		if ((i % 8) == 0) DPRINTFN(2, ("\n   "));
 		DPRINTFN(2, (" %08x", ntohl(ab->ab_data[i])));
@@ -647,24 +717,7 @@ sbp2_status_resp(struct ieee1394_abuf *ab, int rcode)
 	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) {
+	if (ab->ab_addr == stat_addr) {
 		MALLOC(cmd_status, struct sbp2_status_block *,
 		    sizeof(*cmd_status), M_1394DATA, M_WAITOK);
 		MPRINTF("MALLOC(1394DATA)", cmd_status);
@@ -682,17 +735,17 @@ sbp2_status_resp(struct ieee1394_abuf *ab, int rcode)
 		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,
+		DPRINTF(("  status -- src: %d, resp: %d, dead: %d, len: %d, "
+		    "status: %d\n  status -- csr: 0x%012qx\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));
+			elm = sbp2_elfind_hash((u_int32_t)(csr >>
+			    (SBP2_NODE_SHIFT - 8 * sizeof(u_int32_t))));
 			if (elm == NULL) {
 				DPRINTF(("%s: no element found for hash"
 				    " 0x%08x\n", __func__,
-				    (u_int32_t)(csr & 0xFFFFFFFF)));
+				    (u_int32_t)(csr & 0xFFFFFFFC)));
 				FREE(cmd_status, M_1394DATA);
 				MPRINTF("FREE(1394DATA)", cmd_status);
 				cmd_status = NULL;	/* XXX */
@@ -708,7 +761,9 @@ sbp2_status_resp(struct ieee1394_abuf *ab, int rcode)
 			MPRINTF("FREE(1394CTL)", status_notify);
 			status_notify = NULL;	/* XXX */
 		} else if (((src & 2) == 0) && (resp == 0) && (dead == 0) &&
-		    (status == 0) && (csr == SBP2_LOGIN_ORB)) {
+		    (status == 0) && (csr == (SBP2_MGMT_ORB +
+		     ((u_int64_t)ac->ac_nodeid << SBP2_NODE_SHIFT) +
+		     ((u_int64_t)ac->ac_lun << SBP2_LUN_SHIFT)))) {
 			if (ac->ac_status_block) {
 				FREE(ac->ac_status_block, M_1394DATA);
 				MPRINTF("FREE(1394DATA)", ac->ac_status_block);
@@ -722,8 +777,28 @@ sbp2_status_resp(struct ieee1394_abuf *ab, int rcode)
 			MPRINTF("FREE(1394DATA)", cmd_status);
 			cmd_status = NULL;	/* XXX */
 		}
+
+	} else if (ab->ab_addr == (SBP2_RESP_BLOCK +
+	    ((u_int64_t)ac->ac_nodeid << SBP2_NODE_SHIFT) +
+	    ((u_int64_t)ac->ac_lun << SBP2_LUN_SHIFT))) {
+		login_resp = (struct sbp2_login_response *)ab->ab_data;
+
+		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);
+
+		sc->sc_sc1394.sc1394_callback.cb1394_busreset = sbp2_reconnect;
+
+		DPRINTF(("Got a valid response\n"));
+		DPRINTF(("Login ID : 0x%04x, Command Agent : 0x%012qx\n",
+		    ac->ac_login, ac->ac_fetch_agent));
+
+		ac->ac_valid |= 1;
 	}
-	if (ac->ac_valid == 3) {
+	if ((ac->ac_valid & 7) == 3) {
 		DPRINTF(("Valid response : notify callback\n"));
 		if (ac->ac_cb != NULL) {
 			MALLOC(status_notify, struct sbp2_status_notification *,
@@ -751,7 +826,7 @@ leave:
 		ab->ab_data = NULL;
 	}
 
-	if (ab->ab_addr != SBP2_LOGIN_STATUS) {
+	if (ab->ab_addr != stat_addr) {
 		sc->sc1394_unreg(ab, FALSE);
 		FREE(ab, M_1394DATA);
 		MPRINTF("FREE(1394DATA)", ab);
@@ -762,7 +837,7 @@ leave:
 
 void
 sbp2_query_logins(struct fwnode_softc *sc, struct sbp2_query_logins_orb *orb,
-    void (*cb)(struct sbp2_status_notification *))
+    void (*cb)(void *, struct sbp2_status_notification *), void *arg)
 {
 }
 
@@ -774,6 +849,7 @@ sbp2_command_add(struct fwnode_softc *sc, int lun,
 	struct ieee1394_abuf *ab, *ab2;
 	struct sbp2_account *ac;
 	struct sbp2_orb_element *elm, *elast;
+	u_int64_t addr;
 	u_int32_t ehash;
 
 	if ((ac = sbp2_acfind(sc, lun)) == NULL) {
@@ -781,19 +857,17 @@ sbp2_command_add(struct fwnode_softc *sc, int lun,
 		return;
 	}
 
-	DPRINTF(("%s:", __func__));
+	DPRINTF(("%s:\n", __func__));
 
 	/* Initialise orb address hash. */
 	do {
-		ehash = arc4random() & 0xFFFFFFFC;	/* "quadlet" addr */
-	} while (sbp2_elfind_hash(ac, ehash) != NULL);
+		ehash = arc4random();
+	} while (sbp2_elfind_hash(ehash) != TAILQ_END(&sbp2_elm_head));
 
+	addr = SBP2_CMD_ORB +
+	    ((u_int64_t)sc->sc_sc1394.sc1394_node_id << SBP2_NODE_SHIFT) +
+	    ((u_int64_t)ehash << (SBP2_NODE_SHIFT - 8 * sizeof(u_int32_t)));
 	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);
@@ -806,7 +880,7 @@ sbp2_command_add(struct fwnode_softc *sc, int lun,
 	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);
+	TAILQ_INSERT_TAIL(&sbp2_elm_head, elm, elm_chain);
 
 	MALLOC(ab2, struct ieee1394_abuf *, sizeof(*ab2), M_1394DATA, M_WAITOK);
 	MPRINTF("MALLOC(1394DATA)", ab2);
@@ -816,7 +890,7 @@ sbp2_command_add(struct fwnode_softc *sc, int lun,
 	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_addr = addr;
 	ab2->ab_cb = sbp2_command_send;
 	ab2->ab_cbarg = ac;
 	ab2->ab_req = (struct ieee1394_softc *)sc;
@@ -824,7 +898,14 @@ sbp2_command_add(struct fwnode_softc *sc, int lun,
 	elm->elm_orb_ab = ab2;
 	sc->sc1394_inreg(ab2, FALSE);
 
-	if (ac->ac_valid & 8) {
+	elast = sbp2_elfind_last(ac);
+	if (elast != TAILQ_END(&sbp2_elm_head)) {
+		DPRINTF(("%s: chaining to orb 0x%08x", __func__,
+		    elast->elm_orb));
+		elast->elm_orb->next_orb.flag = 0;
+		elast->elm_orb->next_orb.hi = htons((u_int16_t)(addr >> 32));
+		elast->elm_orb->next_orb.lo =
+		    htonl((u_int32_t)(addr & 0xFFFFFFFF));
 		sbp2_agent_tickle(sc, lun);
 	} else {
 		MALLOC(ab, struct ieee1394_abuf *, sizeof(*ab),
@@ -842,17 +923,19 @@ sbp2_command_add(struct fwnode_softc *sc, int lun,
 
 		ab->ab_data = malloc(8, M_1394DATA, M_WAITOK);
 		MPRINTF("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));
+		ab->ab_data[0] = htonl((u_int32_t)(addr >> 32));
+		ab->ab_data[1] = htonl((u_int32_t)(addr & 0xFFFFFFFF));
 		ac->ac_valid |= 8;
 
 		sbp2_agent_reset(sc, lun);
 
+		DPRINTF(("%s: CSR = 0x%012qx", __func__, ab->ab_addr));
 		sc->sc1394_write(ab);
 	}
 
+	DPRINTF((", ORB = 0x%012qx", addr));
+	DPRINTF((", orb = 0x%08x", (u_int32_t)orb));
+	DPRINTF((", ac = 0x%08x\n", (u_int32_t)ac));
 	DPRINTF(("%s: COMMAND submitted\n", __func__));
 
 	return;
@@ -871,15 +954,15 @@ sbp2_command_del(struct fwnode_softc *sc, int lun, struct sbp2_command_orb *orb)
 
 	DPRINTF(("%s:", __func__));
 
-	if ((elm = sbp2_elfind_orb(ac, orb)) == NULL) {
+	if ((elm = sbp2_elfind_orb(orb)) == TAILQ_END(&sbp2_elm_head)) {
 #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,
+	DPRINTF((" orb=0x%08x hash=0x%08x len=%d data=0x%08x size=%d\n",
+	    (u_int32_t)(elm->elm_orb), elm->elm_hash, elm->elm_orblen,
 	    (u_int32_t)(elm->elm_data), elm->elm_datasize));
 
 	if (elm->elm_orb_ab != NULL) {
@@ -894,12 +977,12 @@ sbp2_command_del(struct fwnode_softc *sc, int lun, struct sbp2_command_orb *orb)
 		elm->elm_orb_ab = NULL;	/* XXX */
 	}
 
-	TAILQ_REMOVE(&ac->ac_orb_head, elm, elm_chain);
+	TAILQ_REMOVE(&sbp2_elm_head, elm, elm_chain);
 	FREE(elm, M_1394CTL);
 	MPRINTF("FREE(1394CTL)", elm);
 	elm = NULL;	/* XXX */
 
-	if (TAILQ_EMPTY(&ac->ac_orb_head))
+	if (sbp2_elfind_last(ac) == TAILQ_END(&sbp2_elm_head))
 		ac->ac_valid &= ~8;
 }
 
@@ -908,8 +991,7 @@ 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 sbp2_orb_element *elm;
 	struct ieee1394_abuf *cmd_ab;
 	int i;
 
@@ -936,10 +1018,11 @@ sbp2_command_send(struct ieee1394_abuf *ab, int rcode)
 		ab->ab_data = NULL;
 	}
 
-	if ((elm = sbp2_elfind_hash(ac, (u_int32_t)(ab->ab_addr & 0xFFFFFFFF)))
-	    == NULL) {
+	if ((elm = sbp2_elfind_hash((u_int32_t)(ab->ab_addr >>
+	     (SBP2_NODE_SHIFT - 8 * sizeof(u_int32_t)))))
+	    == TAILQ_END(&sbp2_elm_head)) {
 #ifdef	SBP2_DEBUG
-		DPRINTF(("%s: ORB not found: 0x%016qx\n", __func__,
+		DPRINTF(("%s: ORB not found: 0x%012qx\n", __func__,
 		    ab->ab_addr));
 #endif	/* SBP2_DEBUG */
 		if (ab->ab_data != NULL) {
@@ -953,29 +1036,15 @@ sbp2_command_send(struct ieee1394_abuf *ab, int rcode)
 		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));
+	DPRINTF(("%s: orb=0x%08x hash=0x%08x len=%d data=0x%08x size=%d\n",
+	    __func__, (u_int32_t)(elm->elm_orb), elm->elm_hash,
+	    elm->elm_orblen, (u_int32_t)(elm->elm_data), elm->elm_datasize));
 
 	MALLOC(cmd_ab, struct ieee1394_abuf *, sizeof(*cmd_ab),
 	    M_1394DATA, M_WAITOK);
 	MPRINTF("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;
@@ -985,7 +1054,7 @@ sbp2_command_send(struct ieee1394_abuf *ab, int rcode)
 
 	cmd_ab->ab_data = malloc(elm->elm_orblen * 4, M_1394DATA, M_WAITOK);
 	MPRINTF("malloc(1394DATA)", cmd_ab->ab_data);
-	bcopy(cmd_orb, cmd_ab->ab_data, elm->elm_orblen * 4);
+	bcopy(elm->elm_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])));
@@ -997,6 +1066,212 @@ sbp2_command_send(struct ieee1394_abuf *ab, int rcode)
 }
 
 void
+sbp2_reconnect(struct ieee1394_softc *sc)
+{
+	struct ieee1394_abuf *ab, *orb_ab, *stat_ab;
+	struct sbp2_account *ac;
+	struct sbp2_reconnect_orb *orb;
+	struct fwnode_softc *fwsc = (struct fwnode_softc *)sc;
+	u_int16_t old_nodeid;
+	u_int64_t addr;
+
+	DPRINTF(("%s:", __func__));
+
+	SLIST_FOREACH(ac, &sbp2_ac_head, ac_chain) {
+		if (ac != NULL && ac->ac_softc == fwsc) {
+
+			MALLOC(ab, struct ieee1394_abuf *, sizeof(*ab),
+			    M_1394DATA, M_NOWAIT);
+			MPRINTF("MALLOC(1394DATA)", ab);
+			if (!ab) {
+				printf("%s: memory allocation failure.\n",
+				    __func__);
+				return;
+			}
+			bzero(ab, sizeof(*ab));
+			ab->ab_data = malloc(8, M_1394DATA, M_NOWAIT);
+			MPRINTF("malloc(1394DATA)", ab->ab_data);
+			if (!ab->ab_data) {
+				printf("%s: memory allocation failure.\n",
+				    __func__);
+				FREE(ab, M_1394DATA);
+				MPRINTF("FREE(1394DATA)", ab);
+				return;
+			}
+			MALLOC(orb_ab, struct ieee1394_abuf *, sizeof(*orb_ab),
+			    M_1394DATA, M_NOWAIT);
+			MPRINTF("MALLOC(1394DATA)", orb_ab);
+			if (!orb_ab) {
+				printf("%s: memory allocation failure.\n",
+				    __func__);
+				free(ab->ab_data, M_1394DATA);
+				MPRINTF("free(1394DATA)", ab->ab_data);
+				FREE(ab, M_1394DATA);
+				MPRINTF("FREE(1394DATA)", ab);
+				return;
+			}
+			bzero(orb_ab, sizeof(*orb_ab));
+			MALLOC(stat_ab, struct ieee1394_abuf *,
+			    sizeof(*stat_ab), M_1394DATA, M_NOWAIT);
+			MPRINTF("MALLOC(1394DATA)", stat_ab);
+			if (!stat_ab) {
+				printf("%s: memory allocation failure.\n",
+				    __func__);
+				FREE(orb_ab, M_1394DATA);
+				MPRINTF("FREE(1394DATA)", orb_ab);
+				free(ab->ab_data, M_1394DATA);
+				MPRINTF("free(1394DATA)", ab->ab_data);
+				FREE(ab, M_1394DATA);
+				MPRINTF("FREE(1394DATA)", ab);
+				return;
+			}
+			bzero(stat_ab, sizeof(*stat_ab));
+			orb = malloc(sizeof(*orb), M_1394DATA, M_NOWAIT);
+			MPRINTF("malloc(1394DATA)", orb);
+			if (!orb) {
+				printf("%s: memory allocation failure.\n",
+				    __func__);
+				FREE(stat_ab, M_1394DATA);
+				MPRINTF("FREE(1394DATA)", stat_ab);
+				FREE(orb_ab, M_1394DATA);
+				MPRINTF("FREE(1394DATA)", orb_ab);
+				free(ab->ab_data, M_1394DATA);
+				MPRINTF("free(1394DATA)", ab->ab_data);
+				FREE(ab, M_1394DATA);
+				MPRINTF("FREE(1394DATA)", ab);
+				return;
+			}
+			bzero(orb, sizeof(*orb));
+
+			old_nodeid = ac->ac_nodeid;
+			ac->ac_nodeid = sc->sc1394_node_id;
+			//ac->ac_valid = 1;
+
+			/* Re-register the status block with the new nodeid. */
+			sc->sc1394_node_id = old_nodeid;
+			fwsc->sc1394_unreg(ac->ac_status_ab, FALSE);
+			sc->sc1394_node_id = ac->ac_nodeid;
+			fwsc->sc1394_inreg(ac->ac_status_ab, FALSE);
+
+			/* Register a transient status block. */
+			addr = SBP2_STATUS_BLOCK +
+			    ((u_int64_t)ac->ac_nodeid << SBP2_NODE_SHIFT) +
+			    ((u_int64_t)ac->ac_lun << SBP2_LUN_SHIFT) +
+			    SBP2_RECONNECT_OFFSET;
+			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 = addr;
+			stat_ab->ab_cb = sbp2_status_resp;
+			stat_ab->ab_cbarg = ac;
+			stat_ab->ab_req = sc;
+
+			fwsc->sc1394_inreg(stat_ab, FALSE);
+
+			/* Construct the RECONNECT orb. */
+			orb->options = htons(SBP2_ORB_RECONNECT);
+			orb->login_id = htons(ac->ac_login);
+			orb->status_fifo.hi =
+			    htons((u_int16_t)(addr >> 32));
+			orb->status_fifo.lo =
+			    htonl((u_int32_t)(addr & 0xFFFFFFFF));
+
+			addr = SBP2_MGMT_ORB +
+			    ((u_int64_t)ac->ac_nodeid << SBP2_NODE_SHIFT) +
+			    ((u_int64_t)ac->ac_lun << SBP2_LUN_SHIFT);
+			orb_ab->ab_length = sizeof(struct sbp2_reconnect_orb);
+			orb_ab->ab_tcode =
+			    IEEE1394_TCODE_READ_REQUEST_DATABLOCK;
+			orb_ab->ab_retlen = 0;
+			orb_ab->ab_data = NULL;
+			orb_ab->ab_addr = addr;
+			orb_ab->ab_cb = sbp2_reconnect_send;
+			orb_ab->ab_cbarg = orb;
+			orb_ab->ab_req = sc;
+
+			fwsc->sc1394_inreg(orb_ab, FALSE);
+
+			/* Invoque the RECONNECT management command. */
+			ab->ab_req = 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[0] = htonl((u_int32_t)(addr >> 32));
+			ab->ab_data[1] = htonl((u_int32_t)(addr & 0xFFFFFFFF));
+			DPRINTF((" CSR = 0x%012qx", ac->ac_mgmt_agent));
+			DPRINTF((", ORB = 0x%012qx\n", addr));
+
+			fwsc->sc1394_write(ab);
+			DPRINTF(("%s: %04x RECONNECT submitted\n", __func__,
+			    ac->ac_login));
+		}
+	}
+}
+
+void
+sbp2_reconnect_send(struct ieee1394_abuf *ab, int rcode)
+{
+	struct ieee1394_abuf *orb_ab;
+	struct fwnode_softc *sc = (struct fwnode_softc *)ab->ab_req;
+
+	if (rcode) {
+		DPRINTF(("%s: Bad return code: %d\n", __func__, rcode));
+		if (ab->ab_data > (u_int32_t *)1) {
+			free(ab->ab_data, M_1394DATA);
+			MPRINTF("free(1394DATA)", ab->ab_data);
+			ab->ab_data = NULL;	/* XXX */
+		}
+		FREE(ab, M_1394DATA);
+		MPRINTF("FREE(1394DATA)", ab);
+		ab = NULL;	/* XXX */
+		return;
+	}
+
+	MALLOC(orb_ab, struct ieee1394_abuf *, sizeof(*orb_ab),
+	    M_1394DATA, M_NOWAIT);
+	MPRINTF("MALLOC(1394DATA)", orb_ab);
+	if (!orb_ab) {
+		printf("%s: memory allocation failure.\n",
+		    __func__);
+		return;
+	}
+
+	bcopy(ab, orb_ab, sizeof(*orb_ab));
+
+	sc->sc1394_unreg(ab, FALSE);
+	if (ab->ab_data) {
+		free(ab->ab_data, M_1394DATA);
+		MPRINTF("free(1394DATA)", ab->ab_data);
+		ab->ab_data = NULL;	/* XXX */
+		orb_ab->ab_data = NULL;
+	}
+	FREE(ab, M_1394DATA);
+	MPRINTF("FREE(1394DATA)", ab);
+	ab = NULL;	/* XXX */
+
+	if (!orb_ab->ab_cbarg) {
+		DPRINTF(("%s: orb lost !\n", __func__));
+		FREE(orb_ab, M_1394DATA);
+		MPRINTF("FREE(1394DATA)", orb_ab);
+		orb_ab = NULL;	/* XXX */
+		return;
+	}
+	orb_ab->ab_data = (u_int32_t *)orb_ab->ab_cbarg;
+	orb_ab->ab_length = sizeof(struct sbp2_reconnect_orb);
+	orb_ab->ab_retlen = 0;
+	orb_ab->ab_cb = NULL;
+	orb_ab->ab_cbarg = NULL;
+	orb_ab->ab_tcode = IEEE1394_TCODE_READ_RESPONSE_DATABLOCK;
+
+	sc->sc1394_write(orb_ab);
+}
+
+void
 sbp2_agent_reset(struct fwnode_softc *sc, int lun)
 {
 	struct ieee1394_abuf *ab;
@@ -1024,12 +1299,11 @@ sbp2_agent_reset(struct fwnode_softc *sc, int lun)
 	ab->ab_data = malloc(4, M_1394DATA, M_WAITOK);
 	MPRINTF("malloc(1394DATA)", ab->ab_data);
 	ab->ab_data[0] = 0;
-	DPRINTF((" CSR = 0x%016qx\n", ab->ab_addr));
+	DPRINTF((" CSR = 0x%012qx\n", ab->ab_addr));
 
 	sc->sc1394_write(ab);
 	DPRINTF(("%s: AGENT_RESET submitted\n", __func__));
 
-	return;
 }
 
 void
@@ -1060,12 +1334,11 @@ sbp2_agent_tickle(struct fwnode_softc *sc, int lun)
 	ab->ab_data = malloc(4, M_1394DATA, M_WAITOK);
 	MPRINTF("malloc(1394DATA)", ab->ab_data);
 	ab->ab_data[0] = 0;
-	DPRINTF((" CSR = 0x%016qx\n", ab->ab_addr));
+	DPRINTF((" CSR = 0x%012qx\n", ab->ab_addr));
 
 	sc->sc1394_write(ab);
 	DPRINTF(("%s: DOORBEL submitted\n", __func__));
 
-	return;
 }
 
 int
@@ -1077,7 +1350,7 @@ sbp2_agent_state(struct fwnode_softc *sc, int lun)
 void
 sbp2_task_management(struct fwnode_softc *sc,
     struct sbp2_task_management_orb *orb,
-    void (*cb)(struct sbp2_status_notification *))
+    void (*cb)(void *, struct sbp2_status_notification *), void *arg)
 {
 }