1/*-
2 * Copyright (c) 2003 Hidetoshi Shimokawa
3 * Copyright (c) 1998-2002 Katsushi Kobayashi and Hidetoshi Shimokawa
4 * All rights reserved.
5 *
6 * Redistribution and use in source and binary forms, with or without
7 * modification, are permitted provided that the following conditions
8 * are met:
9 * 1. Redistributions of source code must retain the above copyright
10 *    notice, this list of conditions and the following disclaimer.
11 * 2. Redistributions in binary form must reproduce the above copyright
12 *    notice, this list of conditions and the following disclaimer in the
13 *    documentation and/or other materials provided with the distribution.
14 * 3. All advertising materials mentioning features or use of this software
15 *    must display the acknowledgement as bellow:
16 *
17 *    This product includes software developed by K. Kobayashi and H. Shimokawa
18 *
19 * 4. The name of the author may not be used to endorse or promote products
20 *    derived from this software without specific prior written permission.
21 *
22 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
23 * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
24 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
25 * DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT,
26 * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
27 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
28 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
29 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,
30 * STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *
34 * $FreeBSD: src/sys/dev/firewire/firewire.c,v 1.100 2007/07/20 03:42:57 simokawa Exp $
35 *
36 */
37#include <OS.h>
38#include <Drivers.h>
39#include <KernelExport.h>
40#include <SupportDefs.h>
41#include <ByteOrder.h>
42#include <malloc.h>
43#include <string.h>
44#include <stdio.h>
45#include <sys/param.h>
46#include <sys/types.h>
47#include <sys/uio.h>
48#include <dpc.h>
49
50#include "fwdebug.h"
51#include "fwglue.h"
52#include "queue.h"
53#include "firewire.h"
54#include "iec13213.h"
55#include "firewirereg.h"
56#include "fwmem.h"
57#include "iec68113.h"
58#include "timer.h"
59#include "util.h"
60
61#define PROBE_THREAD_PRIORITY 80
62
63int firewire_debug=1, try_bmr=1, hold_count=0;
64
65#define FW_MAXASYRTY 4
66
67static void firewire_xfer_timeout(void *);
68static void fw_try_bmr (void *);
69static void fw_try_bmr_callback (struct fw_xfer *);
70static void fw_asystart (struct fw_xfer *);
71static int fw_get_tlabel (struct firewire_comm *, struct fw_xfer *);
72static void fw_bus_probe (void *);
73static void fw_attach_dev (struct firewire_comm *);
74static int32 fw_bus_probe_thread(void *);
75#ifdef FW_VMACCESS
76static void fw_vmaccess (struct fw_xfer *);
77#endif
78static int fw_bmr (struct firewire_comm *);
79static void fw_dump_hdr(struct fw_pkt *, const char *);
80
81
82extern const char *const linkspeed[];
83const char *const linkspeed[] = {
84	"S100", "S200", "S400", "S800",
85	"S1600", "S3200", "undef", "undef"
86};
87
88static const char *const tcode_str[] = {
89	"WREQQ", "WREQB", "WRES",   "undef",
90	"RREQQ", "RREQB", "RRESQ",  "RRESB",
91	"CYCS",  "LREQ",  "STREAM", "LRES",
92	"undef", "undef", "PHY",    "undef"
93};
94
95/* IEEE-1394a Table C-2 Gap count as a function of hops*/
96#define MAX_GAPHOP 15
97u_int gap_cnt[] = { 5,  5,  7,  8, 10, 13, 16, 18,
98		   21, 24, 26, 29, 32, 35, 37, 40};
99
100
101/*
102 * Lookup fwdev by node id.
103 */
104struct fw_device *
105fw_noderesolve_nodeid(struct firewire_comm *fc, int dst)
106{
107	struct fw_device *fwdev;
108	int s;
109
110	s = splfw();
111	STAILQ_FOREACH(fwdev, &fc->devices, link)
112		if (fwdev->dst == dst && fwdev->status != FWDEVINVAL)
113			break;
114	splx(s);
115
116	return fwdev;
117}
118
119/*
120 * Lookup fwdev by EUI64.
121 */
122struct fw_device *
123fw_noderesolve_eui64(struct firewire_comm *fc, struct fw_eui64 *eui)
124{
125	struct fw_device *fwdev;
126	int s;
127
128	s = splfw();
129	FW_GLOCK(fc);
130	STAILQ_FOREACH(fwdev, &fc->devices, link)
131		if (FW_EUI64_EQUAL(fwdev->eui, *eui))
132			break;
133	FW_GUNLOCK(fc);
134	splx(s);
135
136	if(fwdev == NULL) return NULL;
137	if(fwdev->status == FWDEVINVAL) return NULL;
138	return fwdev;
139}
140
141/*
142 * Async. request procedure for userland application.
143 */
144int
145fw_asyreq(struct firewire_comm *fc, int sub, struct fw_xfer *xfer)
146{
147	int err = 0;
148	struct fw_xferq *xferq;
149	int len;
150	struct fw_pkt *fp;
151	int tcode;
152	struct tcode_info *info;
153
154	if(xfer == NULL) return EINVAL;
155	if(xfer->hand == NULL){
156		printf("hand == NULL\n");
157		return EINVAL;
158	}
159	fp = &xfer->send.hdr;
160
161	tcode = fp->mode.common.tcode & 0xf;
162	info = &fc->tcode[tcode];
163	if (info->flag == 0) {
164		printf("invalid tcode=%x\n", tcode);
165		return EINVAL;
166	}
167
168	/* XXX allow bus explore packets only after bus rest */
169	if ((fc->status < FWBUSEXPLORE) &&
170	    ((tcode != FWTCODE_RREQQ) || (fp->mode.rreqq.dest_hi != 0xffff) ||
171	    (fp->mode.rreqq.dest_lo  < 0xf0000000) ||
172	    (fp->mode.rreqq.dest_lo >= 0xf0001000))) {
173		xfer->resp = EAGAIN;
174		xfer->flag = FWXF_BUSY;
175		return (EAGAIN);
176	}
177
178	if (info->flag & FWTI_REQ)
179		xferq = fc->atq;
180	else
181		xferq = fc->ats;
182	len = info->hdr_len;
183	if (xfer->send.pay_len > MAXREC(fc->maxrec)) {
184		printf("send.pay_len > maxrec\n");
185		return EINVAL;
186	}
187	if (info->flag & FWTI_BLOCK_STR)
188		len = fp->mode.stream.len;
189	else if (info->flag & FWTI_BLOCK_ASY)
190		len = fp->mode.rresb.len;
191	else
192		len = 0;
193	if (len != xfer->send.pay_len){
194		printf("len(%d) != send.pay_len(%d) %s(%x)\n",
195		    len, xfer->send.pay_len, tcode_str[tcode], tcode);
196		return EINVAL;
197	}
198
199	if(xferq->start == NULL){
200		printf("xferq->start == NULL\n");
201		return EINVAL;
202	}
203	if(!(xferq->queued < xferq->maxq)){
204		device_printf(fc->bdev, "Discard a packet (queued=%d)\n",
205			xferq->queued);
206		return EAGAIN;
207	}
208
209	xfer->tl = -1;
210	if (info->flag & FWTI_TLABEL) {
211		if (fw_get_tlabel(fc, xfer) < 0)
212			return EAGAIN;
213	}
214
215	xfer->resp = 0;
216	xfer->fc = fc;
217	xfer->q = xferq;
218
219	fw_asystart(xfer);
220	return err;
221}
222/*
223 * Wakeup blocked process.
224 */
225void
226fw_xferwake(struct fw_xfer *xfer)
227{
228//	struct mtx *lock = &xfer->fc->wait_lock;
229	mutex *lock = &xfer->fc->wait_lock;
230
231	mtx_lock(lock);
232	xfer->flag |= FWXF_WAKE;
233	mtx_unlock(lock);
234
235//	wakeup(xfer);
236	release_sem(xfer->Sem);
237	return;
238}
239
240int
241fw_xferwait(struct fw_xfer *xfer)
242{
243//	struct mtx *lock = &xfer->fc->wait_lock;
244	mutex *lock = &xfer->fc->wait_lock;
245	int err = 0;
246
247	mtx_lock(lock);
248	if ((xfer->flag & FWXF_WAKE) == 0){
249//		err = msleep((void *)xfer, lock, PWAIT|PCATCH, "fw_xferwait", 0);
250		mtx_unlock(lock);
251		err = acquire_sem(xfer->Sem);
252		return err;
253	}
254	mtx_unlock(lock);
255
256	return (err);
257}
258
259/*
260 * Async. request with given xfer structure.
261 */
262static void
263fw_asystart(struct fw_xfer *xfer)
264{
265	struct firewire_comm *fc = xfer->fc;
266	int s;
267	s = splfw();
268	/* Protect from interrupt/timeout */
269	FW_GLOCK(fc);
270	xfer->flag = FWXF_INQ;
271	STAILQ_INSERT_TAIL(&xfer->q->q, xfer, link);
272#if 0
273	xfer->q->queued ++;
274#endif
275	FW_GUNLOCK(fc);
276	splx(s);
277	/* XXX just queue for mbuf */
278//	if (xfer->mbuf == NULL)
279		xfer->q->start(fc);
280	return;
281}
282
283
284static void
285firewire_xfer_timeout(void *arg)
286{
287	struct firewire_comm *fc = (struct firewire_comm *)arg;
288	struct fw_xfer *xfer, *txfer;
289//	struct timeval tv;
290//	struct timeval split_timeout;
291	bigtime_t tv;
292	STAILQ_HEAD(, fw_xfer) xfer_timeout;
293	int i, s;
294
295//	split_timeout.tv_sec = 0;
296//	split_timeout.tv_usec = 200 * 1000;	 /* 200 msec */
297
298//	microtime(&tv);
299//	timevalsub(&tv, &split_timeout);
300	tv = system_time();
301	tv -= 200*1000;
302	STAILQ_INIT(&xfer_timeout);
303
304	s = splfw();
305	mtx_lock(&fc->tlabel_lock);
306	for (i = 0; i < 0x40; i ++) {
307		while ((xfer = STAILQ_FIRST(&fc->tlabels[i])) != NULL) {
308			if ((xfer->flag & FWXF_SENT) == 0)
309				/* not sent yet */
310				break;
311//			if (timevalcmp(&xfer->tv, &tv, >))
312			if (xfer->tv > tv)
313				/* the rests are newer than this */
314				break;
315			device_printf(fc->bdev,
316				"split transaction timeout: "
317				"tl=0x%x flag=0x%02x\n", i, xfer->flag);
318			fw_dump_hdr(&xfer->send.hdr, "send");
319			xfer->resp = ETIMEDOUT;
320			STAILQ_REMOVE_HEAD(&fc->tlabels[i], tlabel);
321			STAILQ_INSERT_TAIL(&xfer_timeout, xfer, tlabel);
322		}
323	}
324	mtx_unlock(&fc->tlabel_lock);
325	splx(s);
326	fc->timeout(fc);
327
328	STAILQ_FOREACH_SAFE(xfer, &xfer_timeout, tlabel, txfer)
329		xfer->hand(xfer);
330}
331
332#define WATCHDOG_HZ 10
333static void
334firewire_watchdog(void *arg)
335{
336	struct firewire_comm *fc;
337	static int watchdog_clock = 0;
338
339	fc = (struct firewire_comm *)arg;
340
341	/*
342	 * At boot stage, the device interrupt is disabled and
343	 * We encounter a timeout easily. To avoid this,
344	 * ignore clock interrupt for a while.
345	 */
346	if (watchdog_clock > WATCHDOG_HZ * 15)
347//		taskqueue_enqueue(fc->taskqueue, &fc->task_timeout);
348		firewire_xfer_timeout(fc);
349	else
350		watchdog_clock ++;
351
352//	callout_reset(&fc->timeout_callout, hz / WATCHDOG_HZ,
353//			(void *)firewire_watchdog, (void *)fc);
354}
355
356#if 0//to do
357status_t
358firewire_add_child(struct firewire_softc *sc, const char *childname,
359		struct firewire_notify_hooks *hooks)
360{
361	struct firewire_child_info *element, *info;
362	if (!childname)
363		return B_BAD_VALUE;
364
365	element = sc->fc->childList;
366	while (element) {
367		if (strcmp(element->child_name, childname) == 0) {
368			// we already have an entry for this child
369			return B_OK;
370		}
371
372		element = element->link;
373	}
374
375	info = malloc(sizeof(struct firewire_child_info));
376	if (!info) {
377		return B_NO_MEMORY;
378	}
379
380	info->child_name = strdup(childname);
381	info->notify_hooks.device_attach = hooks->device_attach;
382	info->notify_hooks.device_detach = hooks->device_detach;
383	info->cookie = NULL;
384	info->link = NULL;
385
386	element = sc->fc->childList;
387	if (element) {
388		while (element->link)
389			element = element->link;
390		element->link = info;
391	} else
392		sc->fc->childList = info;
393
394	hooks->device_attach(sc, &info->cookie);
395	return B_OK;
396}
397
398status_t
399firewire_remove_child(struct firewire_softc *sc, const char *childname)
400{
401
402	struct firewire_child_info *element = sc->fc->childList;
403	struct firewire_child_info *temp = element;
404	while (element) {
405		if (strcmp(element->child_name, childname) == 0) {
406			// trigger the device removed hook
407			element->notify_hooks.device_detach(sc, element->cookie);
408			if(temp->link == NULL)
409				sc->fc->childList = NULL;//childList has only a node
410			else
411				temp->link = element->link;
412
413			free(element);
414			return B_OK;
415		}
416
417		temp = element;
418		element = element->link;
419	}
420
421	return B_NAME_NOT_FOUND;
422}
423#endif
424
425/*
426 * The attach routine.
427 */
428int
429firewire_attach(struct firewire_comm *fc, struct firewire_softc *sc)
430{
431	sc->fc = fc;
432	fc->status = (uint)FWBUSNOTREADY;
433
434//	unit = device_get_unit(dev);
435	if( fc->nisodma > FWMAXNDMA) fc->nisodma = FWMAXNDMA;
436
437//	fwdev_makedev(sc);
438
439	fc->crom_src_buf = (struct crom_src_buf *)malloc(
440				sizeof(struct crom_src_buf));
441	if (fc->crom_src_buf == NULL) {
442		device_printf(fc->dev, "%s: Malloc Failure crom src buff\n", __func__);
443		return ENOMEM;
444	}
445	memset(fc->crom_src_buf, 0, sizeof(struct crom_src_buf));
446	fc->topology_map = (struct fw_topology_map *)malloc(
447				sizeof(struct fw_topology_map));
448	if (fc->topology_map == NULL) {
449		device_printf(fc->dev, "%s: Malloc Failure topology map\n", __func__);
450		free(fc->crom_src_buf);
451		return ENOMEM;
452	}
453	memset(fc->topology_map, 0, sizeof(struct fw_topology_map));
454	fc->speed_map = (struct fw_speed_map *)malloc(
455				sizeof(struct fw_speed_map));
456	if (fc->speed_map == NULL) {
457		device_printf(fc->dev, "%s: Malloc Failure speed map\n", __func__);
458		free(fc->crom_src_buf);
459		free(fc->topology_map);
460		return ENOMEM;
461	}
462	memset(fc->speed_map, 0, sizeof(struct fw_speed_map));
463
464	mtx_init(&fc->wait_lock, "fwwait", NULL, MTX_DEF);
465	mtx_init(&fc->tlabel_lock, "fwtlabel", NULL, MTX_DEF);
466//	CALLOUT_INIT(&fc->timeout_callout);
467//	CALLOUT_INIT(&fc->bmr_callout);
468//	CALLOUT_INIT(&fc->busprobe_callout);
469//	TASK_INIT(&fc->task_timeout, 0, firewire_xfer_timeout, (void *)fc);
470
471//	callout_reset(&sc->fc->timeout_callout, hz,
472//			(void *)firewire_watchdog, (void *)sc->fc);
473	fc->timeout_callout = create_timer(firewire_watchdog, fc,
474			hz/WATCHDOG_HZ, B_PERIODIC_TIMER);
475
476	/* create thread */
477//	kproc_create(fw_bus_probe_thread, (void *)fc, &fc->probe_thread,
478//		0, 0, "fw%d_probe", unit);
479	fc->probe_thread = spawn_kernel_thread(fw_bus_probe_thread,
480			"fw_probe", PROBE_THREAD_PRIORITY, fc);
481	if(fc->probe_thread < B_OK)
482		dprintf("can not Create bus probe thread\n");
483	resume_thread(fc->probe_thread);
484
485	/* Locate our children */
486//	bus_generic_probe(dev);
487
488	/* launch attachement of the added children */
489//	bus_generic_attach(dev);
490/*	firewire_driver_info *element = sc->childList;
491	while(element){
492		element->notify_hooks.device_attach(sc, element->cookie);
493		element = element->link;
494	}*/
495
496	/* bus_reset */
497	FW_GLOCK(fc);
498	fw_busreset(fc, (uint)FWBUSNOTREADY);
499	FW_GUNLOCK(fc);
500	fc->ibr(fc);
501
502	return 0;
503}
504
505/*
506 * Dettach it.
507 */
508void
509firewire_detach(struct firewire_softc *sc)
510{
511	struct firewire_comm *fc;
512	struct fw_device *fwdev, *fwdev_next;
513	struct firewire_child_info *element, *temp;
514//	int err;
515
516	fc = sc->fc;
517	mtx_lock(&fc->wait_lock);
518	fc->status = (uint)FWBUSDETACH;
519//	wakeup(fc);
520	release_sem(fc->Sem);
521/*	if (msleep(fc->probe_thread, &fc->wait_lock, PWAIT, "fwthr", hz * 60))
522		printf("firewire probe thread didn't die\n");*/
523	mtx_unlock(&fc->wait_lock);
524	DELAY(hz*60);
525
526	if (fc->arq !=0 && fc->arq->maxq > 0)
527		fw_drain_txq(fc);
528/*	if ((err = fwdev_destroydev(sc)) != 0)
529		return err;
530
531	if ((err = bus_generic_detach(dev)) != 0)
532		return err;*/
533
534	element = sc->fc->childList;
535	while(element){
536		element->notify_hooks.device_detach(sc, element->cookie);
537		temp = element;
538		element = element->link;
539		free(temp);
540	}
541	sc->fc->childList = NULL;
542
543/*	callout_stop(&fc->timeout_callout);
544	callout_stop(&fc->bmr_callout);
545	callout_stop(&fc->busprobe_callout);*/
546	delete_timer(fc->timeout_callout);
547	delete_timer(fc->bmr_callout);
548	delete_timer(fc->busprobe_callout);
549
550
551	/* XXX xfer_free and untimeout on all xfers */
552	for (fwdev = STAILQ_FIRST(&fc->devices); fwdev != NULL;
553							fwdev = fwdev_next) {
554		fwdev_next = STAILQ_NEXT(fwdev, link);
555		free(fwdev);
556	}
557	free(fc->topology_map);
558	free(fc->speed_map);
559	free(fc->crom_src_buf);
560
561	mtx_destroy(&fc->tlabel_lock);
562	mtx_destroy(&fc->wait_lock);
563//	return(0);
564}
565#if 0
566static int
567firewire_shutdown( device_t dev )
568{
569	return 0;
570}
571#endif
572
573
574static void
575fw_xferq_drain(struct fw_xferq *xferq)
576{
577	struct fw_xfer *xfer;
578
579	while ((xfer = STAILQ_FIRST(&xferq->q)) != NULL) {
580		STAILQ_REMOVE_HEAD(&xferq->q, link);
581#if 0
582		xferq->queued --;
583#endif
584		xfer->resp = EAGAIN;
585		xfer->flag = FWXF_SENTERR;
586		fw_xfer_done(xfer);
587	}
588}
589
590void
591fw_drain_txq(struct firewire_comm *fc)
592{
593	struct fw_xfer *xfer, *txfer;
594	STAILQ_HEAD(, fw_xfer) xfer_drain;
595	int i;
596
597	STAILQ_INIT(&xfer_drain);
598
599	FW_GLOCK(fc);
600	fw_xferq_drain(fc->atq);
601	fw_xferq_drain(fc->ats);
602	for(i = 0; i < fc->nisodma; i++)
603		fw_xferq_drain(fc->it[i]);
604	FW_GUNLOCK(fc);
605
606	mtx_lock(&fc->tlabel_lock);
607	for (i = 0; i < 0x40; i ++)
608		while ((xfer = STAILQ_FIRST(&fc->tlabels[i])) != NULL) {
609			if (firewire_debug)
610				printf("tl=%d flag=%d\n", i, xfer->flag);
611			xfer->resp = EAGAIN;
612			STAILQ_REMOVE_HEAD(&fc->tlabels[i], tlabel);
613			STAILQ_INSERT_TAIL(&xfer_drain, xfer, tlabel);
614		}
615	mtx_unlock(&fc->tlabel_lock);
616
617	STAILQ_FOREACH_SAFE(xfer, &xfer_drain, tlabel, txfer)
618		xfer->hand(xfer);
619}
620
621static void
622fw_reset_csr(struct firewire_comm *fc)
623{
624	int i;
625
626	CSRARC(fc, STATE_CLEAR)
627			= 1 << 23 | 0 << 17 | 1 << 16 | 1 << 15 | 1 << 14 ;
628	CSRARC(fc, STATE_SET) = CSRARC(fc, STATE_CLEAR);
629	CSRARC(fc, NODE_IDS) = 0x3f;
630
631	CSRARC(fc, TOPO_MAP + 8) = 0;
632	fc->irm = (uint)-1;
633
634	fc->max_node = (uint)-1;
635
636	for(i = 2; i < 0x100/4 - 2 ; i++){
637		CSRARC(fc, SPED_MAP + i * 4) = 0;
638	}
639	CSRARC(fc, STATE_CLEAR) = 1 << 23 | 0 << 17 | 1 << 16 | 1 << 15 | 1 << 14 ;
640	CSRARC(fc, STATE_SET) = CSRARC(fc, STATE_CLEAR);
641	CSRARC(fc, RESET_START) = 0;
642	CSRARC(fc, SPLIT_TIMEOUT_HI) = 0;
643	CSRARC(fc, SPLIT_TIMEOUT_LO) = 800 << 19;
644	CSRARC(fc, CYCLE_TIME) = 0x0;
645	CSRARC(fc, BUS_TIME) = 0x0;
646	CSRARC(fc, BUS_MGR_ID) = 0x3f;
647	CSRARC(fc, BANDWIDTH_AV) = 4915;
648	CSRARC(fc, CHANNELS_AV_HI) = 0xffffffff;
649	CSRARC(fc, CHANNELS_AV_LO) = 0xffffffff;
650	CSRARC(fc, IP_CHANNELS) = (1 << 31);
651
652	CSRARC(fc, CONF_ROM) = 0x04 << 24;
653	CSRARC(fc, CONF_ROM + 4) = 0x31333934; /* means strings 1394 */
654	CSRARC(fc, CONF_ROM + 8) = 1 << 31 | 1 << 30 | 1 << 29 |
655				1 << 28 | 0xff << 16 | 0x09 << 8;
656	CSRARC(fc, CONF_ROM + 0xc) = 0;
657
658/* DV depend CSRs see blue book */
659	CSRARC(fc, oPCR) &= ~DV_BROADCAST_ON;
660	CSRARC(fc, iPCR) &= ~DV_BROADCAST_ON;
661
662	CSRARC(fc, STATE_CLEAR) &= ~(1 << 23 | 1 << 15 | 1 << 14 );
663	CSRARC(fc, STATE_SET) = CSRARC(fc, STATE_CLEAR);
664}
665
666static void
667fw_init_crom(struct firewire_comm *fc)
668{
669	struct crom_src *src;
670
671	src = &fc->crom_src_buf->src;
672	bzero(src, sizeof(struct crom_src));
673
674	/* BUS info sample */
675	src->hdr.info_len = 4;
676
677	src->businfo.bus_name = CSR_BUS_NAME_IEEE1394;
678
679	src->businfo.irmc = 1;
680	src->businfo.cmc = 1;
681	src->businfo.isc = 1;
682	src->businfo.bmc = 1;
683	src->businfo.pmc = 0;
684	src->businfo.cyc_clk_acc = 100;
685	src->businfo.max_rec = fc->maxrec;
686	src->businfo.max_rom = MAXROM_4;
687#define FW_GENERATION_CHANGEABLE 2
688	src->businfo.generation = FW_GENERATION_CHANGEABLE;
689	src->businfo.link_spd = fc->speed;
690
691	src->businfo.eui64.hi = fc->eui.hi;
692	src->businfo.eui64.lo = fc->eui.lo;
693
694	STAILQ_INIT(&src->chunk_list);
695
696	fc->crom_src = src;
697	fc->crom_root = &fc->crom_src_buf->root;
698}
699
700static void
701fw_reset_crom(struct firewire_comm *fc)
702{
703	struct crom_src_buf *buf;
704	struct crom_src *src;
705	struct crom_chunk *root;
706
707	buf =  fc->crom_src_buf;
708	src = fc->crom_src;
709	root = fc->crom_root;
710
711	STAILQ_INIT(&src->chunk_list);
712
713	bzero(root, sizeof(struct crom_chunk));
714	crom_add_chunk(src, NULL, root, 0);
715	crom_add_entry(root, CSRKEY_NCAP, 0x0083c0); /* XXX */
716	/* private company_id */
717	crom_add_entry(root, CSRKEY_VENDOR, CSRVAL_VENDOR_PRIVATE);
718#if 0
719#ifdef __DragonFly__
720	crom_add_simple_text(src, root, &buf->vendor, "DragonFly Project");
721	crom_add_entry(root, CSRKEY_HW, __DragonFly_cc_version);
722#else
723	crom_add_simple_text(src, root, &buf->vendor, "FreeBSD Project");
724	crom_add_entry(root, CSRKEY_HW, __FreeBSD_version);
725#endif
726#endif
727	crom_add_simple_text(src, root, &buf->vendor, "HAIKU Project");
728	crom_add_entry(root, CSRKEY_HW, B_CUR_DRIVER_API_VERSION);
729	crom_add_simple_text(src, root, &buf->hw, "");
730}
731
732/*
733 * Called after bus reset.
734 */
735void
736fw_busreset(struct firewire_comm *fc, uint32_t new_status)
737{
738	struct firewire_dev_comm *fdc;
739	struct crom_src *src;
740//	device_t *devlistp;
741	uint32_t *newrom;
742//	int i, devcnt;
743	struct firewire_child_info *element = fc->childList;
744
745	FW_GLOCK_ASSERT(fc);
746	if (fc->status == FWBUSMGRELECT)
747//		callout_stop(&fc->bmr_callout);
748		delete_timer(fc->bmr_callout);
749	fc->status = new_status;
750	fw_reset_csr(fc);
751
752	if ((int32)fc->status == FWBUSNOTREADY)
753		fw_init_crom(fc);
754
755	fw_reset_crom(fc);
756
757
758/*	if (device_get_children(fc->bdev, &devlistp, &devcnt) == 0) {
759		for( i = 0 ; i < devcnt ; i++)
760			if (device_get_state(devlistp[i]) >= DS_ATTACHED)  {
761				fdc = device_get_softc(devlistp[i]);
762				if (fdc->post_busreset != NULL)
763					fdc->post_busreset(fdc);
764			}
765		free(devlistp);
766	}*/
767
768	while(element){
769//		fdc = &element->cookie.fd;
770		fdc = (struct firewire_dev_comm *)element->cookie;
771		if (fdc->post_busreset != NULL)
772			fdc->post_busreset(fdc);
773		element = element->link;
774	}
775
776	src = &fc->crom_src_buf->src;
777        /*
778         * If the old config rom needs to be overwritten,
779         * bump the businfo.generation indicator to
780         * indicate that we need to be reprobed
781         * See 1394a-2000 8.3.2.5.4 for more details.
782         * generation starts at 2 and rolls over at 0xF
783         * back to 2.
784         *
785         * A generation of 0 indicates a device
786         * that is not 1394a-2000 compliant.
787         * A generation of 1 indicates a device that
788         * does not change it's Bus Info Block or
789         * Configuration ROM.
790         */
791#define FW_MAX_GENERATION 0xF
792	newrom = (uint32_t*)malloc(CROMSIZE);
793	memset(newrom, 0, CROMSIZE);
794	src = &fc->crom_src_buf->src;
795	crom_load(src, newrom, CROMSIZE);
796	if (bcmp(newrom, fc->config_rom, CROMSIZE) != 0) {
797		if ( src->businfo.generation++ > FW_MAX_GENERATION )
798			src->businfo.generation = FW_GENERATION_CHANGEABLE;
799		bcopy(newrom, (void *)fc->config_rom, CROMSIZE);
800	}
801	free(newrom);
802}
803
804/* Call once after reboot */
805void fw_init(struct firewire_comm *fc)
806{
807	int i;
808	char tempname[16];
809#ifdef FW_VMACCESS
810	struct fw_xfer *xfer;
811	struct fw_bind *fwb;
812#endif
813
814	fc->arq->queued = 0;
815	fc->ars->queued = 0;
816	fc->atq->queued = 0;
817	fc->ats->queued = 0;
818
819	fc->arq->buf = NULL;
820	fc->ars->buf = NULL;
821	fc->atq->buf = NULL;
822	fc->ats->buf = NULL;
823
824	fc->arq->flag = 0;
825	fc->ars->flag = 0;
826	fc->atq->flag = 0;
827	fc->ats->flag = 0;
828
829	STAILQ_INIT(&fc->atq->q);
830	STAILQ_INIT(&fc->ats->q);
831
832	fc->Sem = create_sem(0, "fc Sem");
833
834	for( i = 0 ; i < fc->nisodma ; i ++ ){
835		fc->it[i]->queued = 0;
836		fc->ir[i]->queued = 0;
837
838		fc->it[i]->start = NULL;
839		fc->ir[i]->start = NULL;
840
841		fc->it[i]->buf = NULL;
842		fc->ir[i]->buf = NULL;
843
844		fc->it[i]->flag = FWXFERQ_STREAM;
845		fc->ir[i]->flag = FWXFERQ_STREAM;
846
847		STAILQ_INIT(&fc->it[i]->q);
848		STAILQ_INIT(&fc->ir[i]->q);
849
850		snprintf(tempname, sizeof(tempname), "it%d ready sem", i);
851		fc->it[i]->Sem = create_sem(0, tempname);
852		snprintf(tempname, sizeof(tempname), "ir%d ready sem", i);
853		fc->ir[i]->Sem = create_sem(0, tempname);
854	}
855
856	fc->arq->maxq = FWMAXQUEUE;
857	fc->ars->maxq = FWMAXQUEUE;
858	fc->atq->maxq = FWMAXQUEUE;
859	fc->ats->maxq = FWMAXQUEUE;
860
861	for( i = 0 ; i < fc->nisodma ; i++){
862		fc->ir[i]->maxq = FWMAXQUEUE;
863		fc->it[i]->maxq = FWMAXQUEUE;
864	}
865	CSRARC(fc, TOPO_MAP) = 0x3f1 << 16;
866	CSRARC(fc, TOPO_MAP + 4) = 1;
867	CSRARC(fc, SPED_MAP) = 0x3f1 << 16;
868	CSRARC(fc, SPED_MAP + 4) = 1;
869
870	STAILQ_INIT(&fc->devices);
871
872/* Initialize Async handlers */
873	STAILQ_INIT(&fc->binds);
874	for( i = 0 ; i < 0x40 ; i++){
875		STAILQ_INIT(&fc->tlabels[i]);
876	}
877
878/* DV depend CSRs see blue book */
879#if 0
880	CSRARC(fc, oMPR) = 0x3fff0001; /* # output channel = 1 */
881	CSRARC(fc, oPCR) = 0x8000007a;
882	for(i = 4 ; i < 0x7c/4 ; i+=4){
883		CSRARC(fc, i + oPCR) = 0x8000007a;
884	}
885
886	CSRARC(fc, iMPR) = 0x00ff0001; /* # input channel = 1 */
887	CSRARC(fc, iPCR) = 0x803f0000;
888	for(i = 4 ; i < 0x7c/4 ; i+=4){
889		CSRARC(fc, i + iPCR) = 0x0;
890	}
891#endif
892
893	fc->crom_src_buf = NULL;
894
895#ifdef FW_VMACCESS
896	xfer = fw_xfer_alloc();
897	if(xfer == NULL) return;
898
899	fwb = (struct fw_bind *)malloc(sizeof (struct fw_bind), M_FW, M_NOWAIT);
900	if(fwb == NULL){
901		fw_xfer_free(xfer);
902		return;
903	}
904	xfer->hand = fw_vmaccess;
905	xfer->fc = fc;
906	xfer->sc = NULL;
907
908	fwb->start_hi = 0x2;
909	fwb->start_lo = 0;
910	fwb->addrlen = 0xffffffff;
911	fwb->xfer = xfer;
912	fw_bindadd(fc, fwb);
913#endif
914}
915
916#define BIND_CMP(addr, fwb) (((addr) < (fwb)->start)?-1:\
917    ((fwb)->end < (addr))?1:0)
918
919/*
920 * To lookup bound process from IEEE1394 address.
921 */
922struct fw_bind *
923fw_bindlookup(struct firewire_comm *fc, uint16_t dest_hi, uint32_t dest_lo)
924{
925	u_int64_t addr;
926	struct fw_bind *tfw, *r = NULL;
927
928	addr = ((u_int64_t)dest_hi << 32) | dest_lo;
929	FW_GLOCK(fc);
930	STAILQ_FOREACH(tfw, &fc->binds, fclist)
931		if (BIND_CMP(addr, tfw) == 0) {
932			r = tfw;
933			break;
934		}
935	FW_GUNLOCK(fc);
936	return(r);
937}
938
939/*
940 * To bind IEEE1394 address block to process.
941 */
942int
943fw_bindadd(struct firewire_comm *fc, struct fw_bind *fwb)
944{
945	struct fw_bind *tfw, *prev = NULL;
946	int r = 0;
947
948	if (fwb->start > fwb->end) {
949		printf("%s: invalid range\n", __func__);
950		return EINVAL;
951	}
952
953	FW_GLOCK(fc);
954	STAILQ_FOREACH(tfw, &fc->binds, fclist) {
955		if (fwb->end < tfw->start)
956			break;
957		prev = tfw;
958	}
959	if (prev == NULL)
960		STAILQ_INSERT_HEAD(&fc->binds, fwb, fclist);
961	else if (prev->end < fwb->start)
962		STAILQ_INSERT_AFTER(&fc->binds, prev, fwb, fclist);
963	else {
964		printf("%s: bind failed\n", __func__);
965		r = EBUSY;
966	}
967	FW_GUNLOCK(fc);
968	return (r);
969}
970
971/*
972 * To free IEEE1394 address block.
973 */
974int
975fw_bindremove(struct firewire_comm *fc, struct fw_bind *fwb)
976{
977#if 0
978	struct fw_xfer *xfer, *next;
979#endif
980	struct fw_bind *tfw;
981	int s;
982
983	s = splfw();
984	FW_GLOCK(fc);
985	STAILQ_FOREACH(tfw, &fc->binds, fclist)
986		if (tfw == fwb) {
987			STAILQ_REMOVE(&fc->binds, fwb, fw_bind, fclist);
988			goto found;
989		}
990
991	printf("%s: no such binding\n", __func__);
992	FW_GUNLOCK(fc);
993	splx(s);
994	return (1);
995found:
996#if 0
997	/* shall we do this? */
998	for (xfer = STAILQ_FIRST(&fwb->xferlist); xfer != NULL; xfer = next) {
999		next = STAILQ_NEXT(xfer, link);
1000		fw_xfer_free(xfer);
1001	}
1002	STAILQ_INIT(&fwb->xferlist);
1003#endif
1004	FW_GUNLOCK(fc);
1005
1006	splx(s);
1007	return 0;
1008}
1009
1010int
1011fw_xferlist_add(struct fw_xferlist *q, int slen, int rlen, int n,
1012    struct firewire_comm *fc, void *sc, void (*hand)(struct fw_xfer *))
1013{
1014	int i, s;
1015	struct fw_xfer *xfer;
1016
1017	for (i = 0; i < n; i++) {
1018		xfer = fw_xfer_alloc_buf(slen, rlen);
1019		if (xfer == NULL)
1020			return (n);
1021		xfer->fc = fc;
1022		xfer->sc = (caddr_t)sc;
1023		xfer->hand = hand;
1024		s = splfw();
1025		STAILQ_INSERT_TAIL(q, xfer, link);
1026		splx(s);
1027	}
1028	return (n);
1029}
1030
1031void
1032fw_xferlist_remove(struct fw_xferlist *q)
1033{
1034	struct fw_xfer *xfer, *next;
1035
1036	for (xfer = STAILQ_FIRST(q); xfer != NULL; xfer = next) {
1037                next = STAILQ_NEXT(xfer, link);
1038                fw_xfer_free_buf(xfer);
1039        }
1040        STAILQ_INIT(q);
1041}
1042/*
1043 * dump packet header
1044 */
1045static void
1046fw_dump_hdr(struct fw_pkt *fp, const char *prefix)
1047{
1048	printf("%s: dst=0x%02x tl=0x%02x rt=%d tcode=0x%x pri=0x%x "
1049	    "src=0x%03x\n", prefix,
1050	    fp->mode.hdr.dst & 0x3f,
1051	    fp->mode.hdr.tlrt >> 2, fp->mode.hdr.tlrt & 3,
1052	    fp->mode.hdr.tcode, fp->mode.hdr.pri,
1053	    fp->mode.hdr.src);
1054}
1055
1056/*
1057 * To free transaction label.
1058 */
1059static void
1060fw_tl_free(struct firewire_comm *fc, struct fw_xfer *xfer)
1061{
1062	struct fw_xfer *txfer;
1063	int s;
1064
1065	if (xfer->tl < 0)
1066		return;
1067
1068	s = splfw();
1069	mtx_lock(&fc->tlabel_lock);
1070#if 1	/* make sure the label is allocated */
1071	STAILQ_FOREACH(txfer, &fc->tlabels[xfer->tl], tlabel)
1072		if(txfer == xfer)
1073			break;
1074	if (txfer == NULL) {
1075		printf("%s: the xfer is not in the queue "
1076		    "(tlabel=%d, flag=0x%x)\n",
1077		    __FUNCTION__, xfer->tl, xfer->flag);
1078		fw_dump_hdr(&xfer->send.hdr, "send");
1079		fw_dump_hdr(&xfer->recv.hdr, "recv");
1080//		kdb_backtrace();
1081		mtx_unlock(&fc->tlabel_lock);
1082		splx(s);
1083		return;
1084	}
1085#endif
1086
1087	STAILQ_REMOVE(&fc->tlabels[xfer->tl], xfer, fw_xfer, tlabel);
1088	mtx_unlock(&fc->tlabel_lock);
1089	splx(s);
1090	return;
1091}
1092
1093/*
1094 * To obtain XFER structure by transaction label.
1095 */
1096static struct fw_xfer *
1097fw_tl2xfer(struct firewire_comm *fc, int node, int tlabel, int tcode)
1098{
1099	struct fw_xfer *xfer;
1100	int s;
1101	int req;
1102
1103	s = splfw();
1104
1105	mtx_lock(&fc->tlabel_lock);
1106	STAILQ_FOREACH(xfer, &fc->tlabels[tlabel], tlabel)
1107		if((uint)xfer->send.hdr.mode.hdr.dst == (uint)node) {
1108			mtx_unlock(&fc->tlabel_lock);
1109			splx(s);
1110			KASSERT(xfer->tl == tlabel,
1111				("xfer->tl 0x%x != 0x%x", xfer->tl, tlabel));
1112			/* extra sanity check */
1113			req = xfer->send.hdr.mode.hdr.tcode;
1114			if (xfer->fc->tcode[req].valid_res != tcode) {
1115				printf("%s: invalid response tcode "
1116				    "(0x%x for 0x%x)\n", __FUNCTION__,
1117				    tcode, req);
1118				return(NULL);
1119			}
1120
1121			if (firewire_debug > 2)
1122				printf("fw_tl2xfer: found tl=%d\n", tlabel);
1123			return(xfer);
1124		}
1125	mtx_unlock(&fc->tlabel_lock);
1126	if (firewire_debug > 1)
1127		printf("fw_tl2xfer: not found tl=%d\n", tlabel);
1128	splx(s);
1129	return(NULL);
1130}
1131
1132/*
1133 * To allocate IEEE1394 XFER structure.
1134 */
1135struct fw_xfer *
1136fw_xfer_alloc()
1137{
1138	struct fw_xfer *xfer;
1139
1140	xfer = (fw_xfer*)malloc(sizeof(struct fw_xfer));
1141	if (xfer == NULL)
1142		return xfer;
1143	memset(xfer, 0, sizeof(struct fw_xfer));
1144
1145	xfer->Sem = create_sem(0, "xfer ready sem");
1146
1147//	xfer->malloc = type;
1148
1149	return xfer;
1150}
1151
1152struct fw_xfer *
1153fw_xfer_alloc_buf(int send_len, int recv_len)
1154{
1155	struct fw_xfer *xfer;
1156	void *send_virt, *send_phy, *recv_virt, *recv_phy;
1157
1158	xfer = fw_xfer_alloc();
1159	if (xfer == NULL)
1160		return(NULL);
1161	xfer->send.pay_len = send_len;
1162	xfer->recv.pay_len = recv_len;
1163	if (send_len > 0) {
1164/*		xfer->send.payload = malloc(send_len);
1165		if (xfer->send.payload == NULL) {
1166			fw_xfer_free(xfer);
1167			return(NULL);
1168		}
1169		memset(xfer->send.payload 0, send_len);*/
1170		xfer->send.payArea = alloc_mem(&send_virt, &send_phy,
1171				send_len, 0, "firewire tx buf");
1172		if (xfer->send.payArea < B_OK){
1173			fw_xfer_free(xfer);
1174			return(NULL);
1175		}
1176		xfer->send.payload = (uint32_t *)send_virt;
1177		xfer->send.bus_addr = (bus_addr_t)(addr_t)send_phy;
1178	}
1179	if (recv_len > 0) {
1180/*		xfer->recv.payload = malloc(recv_len);
1181		if (xfer->recv.payload == NULL) {
1182			if (xfer->send.payload != NULL)
1183				free(xfer->send.payload);
1184			fw_xfer_free(xfer);
1185			return(NULL);
1186		}*/
1187		xfer->recv.payArea = alloc_mem(&recv_virt, &recv_phy,
1188				recv_len, 0, "firewire rx buf");
1189		if (xfer->recv.payArea < B_OK){
1190			if (xfer->send.payArea > B_OK){
1191				delete_area(xfer->send.payArea);
1192				xfer->send.payArea = -1;
1193			}
1194
1195			fw_xfer_free(xfer);
1196			return(NULL);
1197		}
1198		xfer->recv.payload = (uint32_t *)recv_virt;
1199		xfer->recv.bus_addr = (bus_addr_t)(addr_t)recv_phy;
1200	}
1201	return(xfer);
1202}
1203
1204/*
1205 * IEEE1394 XFER post process.
1206 */
1207void
1208fw_xfer_done(struct fw_xfer *xfer)
1209{
1210	if (xfer->hand == NULL) {
1211		printf("hand == NULL\n");
1212		return;
1213	}
1214
1215	if (xfer->fc == NULL)
1216		panic("fw_xfer_done: why xfer->fc is NULL?");
1217
1218	fw_tl_free(xfer->fc, xfer);
1219	xfer->hand(xfer);
1220}
1221
1222void
1223fw_xfer_unload(struct fw_xfer* xfer)
1224{
1225	int s;
1226
1227	if(xfer == NULL ) return;
1228	if(xfer->flag & FWXF_INQ){
1229		printf("fw_xfer_free FWXF_INQ\n");
1230		s = splfw();
1231		FW_GLOCK(xfer->fc);
1232		STAILQ_REMOVE(&xfer->q->q, xfer, fw_xfer, link);
1233#if 0
1234		xfer->q->queued --;
1235#endif
1236		FW_GUNLOCK(xfer->fc);
1237		splx(s);
1238	}
1239	if (xfer->fc != NULL) {
1240#if 1
1241		if(xfer->flag & FWXF_START)
1242			/*
1243			 * This could happen if:
1244			 *  1. We call fwohci_arcv() before fwohci_txd().
1245			 *  2. firewire_watch() is called.
1246			 */
1247			printf("fw_xfer_free FWXF_START\n");
1248#endif
1249	}
1250	xfer->flag = FWXF_INIT;
1251	xfer->resp = 0;
1252}
1253/*
1254 * To free IEEE1394 XFER structure.
1255 */
1256void
1257fw_xfer_free_buf( struct fw_xfer* xfer)
1258{
1259	if (xfer == NULL) {
1260		printf("%s: xfer == NULL\n", __func__);
1261		return;
1262	}
1263	fw_xfer_unload(xfer);
1264/*	if(xfer->send.payload != NULL){
1265		free(xfer->send.payload);
1266	}
1267	if(xfer->recv.payload != NULL){
1268		free(xfer->recv.payload);
1269	}*/
1270	if (xfer->send.payArea > B_OK){
1271		delete_area(xfer->send.payArea);
1272		xfer->send.payArea = -1;
1273	}
1274	if (xfer->recv.payArea > B_OK){
1275		delete_area(xfer->recv.payArea);
1276		xfer->recv.payArea = -1;
1277	}
1278	delete_sem(xfer->Sem);
1279	free(xfer);
1280}
1281
1282void
1283fw_xfer_free( struct fw_xfer* xfer)
1284{
1285	if (xfer == NULL) {
1286		printf("%s: xfer == NULL\n", __func__);
1287		return;
1288	}
1289	delete_sem(xfer->Sem);
1290	fw_xfer_unload(xfer);
1291	free(xfer);
1292}
1293
1294void
1295fw_asy_callback_free(struct fw_xfer *xfer)
1296{
1297#if 0
1298	printf("asyreq done flag=0x%02x resp=%d\n",
1299				xfer->flag, xfer->resp);
1300#endif
1301	fw_xfer_free(xfer);
1302}
1303
1304/*
1305 * To configure PHY.
1306 */
1307static void
1308fw_phy_config(struct firewire_comm *fc, int root_node, int gap_count)
1309{
1310	struct fw_xfer *xfer;
1311	struct fw_pkt *fp;
1312
1313	fc->status = FWBUSPHYCONF;
1314
1315	xfer = fw_xfer_alloc();
1316	if (xfer == NULL)
1317		return;
1318	xfer->fc = fc;
1319	xfer->hand = fw_asy_callback_free;
1320
1321	fp = &xfer->send.hdr;
1322	fp->mode.ld[1] = 0;
1323	if (root_node >= 0)
1324		fp->mode.ld[1] |= (root_node & 0x3f) << 24 | 1 << 23;
1325	if (gap_count >= 0)
1326		fp->mode.ld[1] |= 1 << 22 | (gap_count & 0x3f) << 16;
1327	fp->mode.ld[2] = ~fp->mode.ld[1];
1328/* XXX Dangerous, how to pass PHY packet to device driver */
1329	fp->mode.common.tcode |= FWTCODE_PHY;
1330
1331	if (firewire_debug)
1332		device_printf(fc->bdev, "%s: root_node=%d gap_count=%d\n",
1333			 __func__, root_node, gap_count);
1334	fw_asyreq(fc, -1, xfer);
1335}
1336
1337/*
1338 * Dump self ID.
1339 */
1340static void
1341fw_print_sid(uint32_t sid)
1342{
1343	union fw_self_id *s;
1344	s = (union fw_self_id *) &sid;
1345	if ( s->p0.sequel ) {
1346		if ( s->p1.sequence_num == FW_SELF_ID_PAGE0 ) {
1347			printf("node:%d p3:%d p4:%d p5:%d p6:%d p7:%d"
1348				"p8:%d p9:%d p10:%d\n",
1349				s->p1.phy_id, s->p1.port3, s->p1.port4,
1350				s->p1.port5, s->p1.port6, s->p1.port7,
1351				s->p1.port8, s->p1.port9, s->p1.port10);
1352		} else if (s->p2.sequence_num == FW_SELF_ID_PAGE1 ){
1353			printf("node:%d p11:%d p12:%d p13:%d p14:%d p15:%d\n",
1354				s->p2.phy_id, s->p2.port11, s->p2.port12,
1355				s->p2.port13, s->p2.port14, s->p2.port15);
1356		} else {
1357			printf("node:%d Unknown Self ID Page number %d\n",
1358				s->p1.phy_id, s->p1.sequence_num);
1359		}
1360	} else {
1361		printf("node:%d link:%d gap:%d spd:%d con:%d pwr:%d"
1362			" p0:%d p1:%d p2:%d i:%d m:%d\n",
1363			s->p0.phy_id, s->p0.link_active, s->p0.gap_count,
1364			s->p0.phy_speed, s->p0.contender,
1365			s->p0.power_class, s->p0.port0, s->p0.port1,
1366			s->p0.port2, s->p0.initiated_reset, s->p0.more_packets);
1367	}
1368}
1369
1370/*
1371 * To receive self ID.
1372 */
1373void fw_sidrcv(struct firewire_comm* fc, uint32_t *sid, u_int len)
1374{
1375	uint32_t *p;
1376	union fw_self_id *self_id;
1377	u_int i, j, node, c_port = 0, i_branch = 0;
1378
1379	fc->sid_cnt = len /(sizeof(uint32_t) * 2);
1380	fc->max_node = fc->nodeid & 0x3f;
1381	CSRARC(fc, NODE_IDS) = ((uint32_t)fc->nodeid) << 16;
1382	fc->status = FWBUSCYMELECT;
1383	fc->topology_map->crc_len = 2;
1384	fc->topology_map->generation ++;
1385	fc->topology_map->self_id_count = 0;
1386	fc->topology_map->node_count = 0;
1387	fc->speed_map->generation ++;
1388	fc->speed_map->crc_len = 1 + (64*64 + 3) / 4;
1389	self_id = &fc->topology_map->self_id[0];
1390	for(i = 0; i < fc->sid_cnt; i ++){
1391		if (sid[1] != ~sid[0]) {
1392			device_printf(fc->bdev, "%s:"
1393				"ERROR invalid self-id packet\n", __func__);
1394			sid += 2;
1395			continue;
1396		}
1397		*self_id = *((union fw_self_id *)sid);
1398		(void)(fc->topology_map->crc_len++);
1399		if(self_id->p0.sequel == 0){
1400			(void)(fc->topology_map->node_count++);
1401			c_port = 0;
1402			if (firewire_debug)
1403				fw_print_sid(sid[0]);
1404
1405			node = self_id->p0.phy_id;
1406			if(fc->max_node < node){
1407				fc->max_node = self_id->p0.phy_id;
1408			}
1409			/* XXX I'm not sure this is the right speed_map */
1410			fc->speed_map->speed[node][node]
1411					= self_id->p0.phy_speed;
1412			for (j = 0; j < node; j ++) {
1413				fc->speed_map->speed[j][node]
1414					= fc->speed_map->speed[node][j]
1415					= min_c(fc->speed_map->speed[j][j],
1416							self_id->p0.phy_speed);
1417			}
1418			if ((fc->irm == (u_int)-1 || self_id->p0.phy_id > fc->irm) &&
1419			  (self_id->p0.link_active && self_id->p0.contender)) {
1420				fc->irm = self_id->p0.phy_id;
1421			}
1422			if(self_id->p0.port0 >= 0x2){
1423				c_port++;
1424			}
1425			if(self_id->p0.port1 >= 0x2){
1426				c_port++;
1427			}
1428			if(self_id->p0.port2 >= 0x2){
1429				c_port++;
1430			}
1431		}
1432		if(c_port > 2){
1433			i_branch += (c_port - 2);
1434		}
1435		sid += 2;
1436		self_id++;
1437		(void)(fc->topology_map->self_id_count++);
1438	}
1439	/* CRC */
1440	fc->topology_map->crc = fw_crc16(
1441			(uint32_t *)&fc->topology_map->generation,
1442			fc->topology_map->crc_len * 4);
1443	fc->speed_map->crc = fw_crc16(
1444			(uint32_t *)&fc->speed_map->generation,
1445			fc->speed_map->crc_len * 4);
1446	/* byteswap and copy to CSR */
1447	p = (uint32_t *)fc->topology_map;
1448	for (i = 0; i <= fc->topology_map->crc_len; i++)
1449		CSRARC(fc, TOPO_MAP + i * 4) = htonl(*p++);
1450	p = (uint32_t *)fc->speed_map;
1451	CSRARC(fc, SPED_MAP) = htonl(*p++);
1452	CSRARC(fc, SPED_MAP + 4) = htonl(*p++);
1453	/* don't byte-swap uint8_t array */
1454	bcopy(p, &CSRARC(fc, SPED_MAP + 8), (fc->speed_map->crc_len - 1)*4);
1455
1456	fc->max_hop = fc->max_node - i_branch;
1457	device_printf(fc->bdev, "%d nodes, maxhop <= %d %s irm(%d) %s\n",
1458		fc->max_node + 1, fc->max_hop,
1459		(fc->irm == (u_int)-1) ? "Not IRM capable" : "cable IRM",
1460		fc->irm,
1461		(fc->irm == fc->nodeid) ? " (me) " : "");
1462
1463	if (try_bmr && (fc->irm != (u_int)-1) && (CSRARC(fc, BUS_MGR_ID) == 0x3f)) {
1464		if (fc->irm == fc->nodeid) {
1465			fc->status = FWBUSMGRDONE;
1466			CSRARC(fc, BUS_MGR_ID) = fc->set_bmr(fc, fc->irm);
1467			fw_bmr(fc);
1468		} else {
1469			fc->status = FWBUSMGRELECT;
1470//			callout_reset(&fc->bmr_callout, hz/8,
1471//				(void *)fw_try_bmr, (void *)fc);
1472			fc->bmr_callout = create_timer(fw_try_bmr, fc,
1473					hz/8, B_ONE_SHOT_RELATIVE_TIMER);
1474		}
1475	} else
1476		fc->status = FWBUSMGRDONE;
1477
1478//	callout_reset(&fc->busprobe_callout, hz/4,
1479//			(void *)fw_bus_probe, (void *)fc);
1480	fc->busprobe_callout = create_timer(fw_bus_probe, fc,
1481			hz/4, B_ONE_SHOT_RELATIVE_TIMER);
1482}
1483
1484/*
1485 * To probe devices on the IEEE1394 bus.
1486 */
1487static void
1488fw_bus_probe(void *arg)
1489{
1490	int s;
1491	struct firewire_comm *fc = (struct firewire_comm *)arg;
1492	struct fw_device *fwdev;
1493
1494	s = splfw();
1495	fc->status = FWBUSEXPLORE;
1496
1497	/* Invalidate all devices, just after bus reset. */
1498	if (firewire_debug)
1499		device_printf(fc->bdev, "%s:"
1500			"iterate and invalidate all nodes\n",
1501			__func__);
1502	STAILQ_FOREACH(fwdev, &fc->devices, link)
1503		if (fwdev->status != FWDEVINVAL) {
1504			fwdev->status = FWDEVINVAL;
1505			fwdev->rcnt = 0;
1506			if (firewire_debug)
1507				device_printf(fc->bdev, "%s:"
1508					"Invalidate Dev ID: %08x%08x\n",
1509					__func__, fwdev->eui.hi, fwdev->eui.lo);
1510		} else {
1511			if (firewire_debug)
1512				device_printf(fc->bdev, "%s:"
1513					"Dev ID: %08x%08x already invalid\n",
1514					__func__, fwdev->eui.hi, fwdev->eui.lo);
1515
1516		}
1517	splx(s);
1518
1519//	wakeup((void *)fc);
1520	release_sem(fc->Sem);
1521}
1522
1523static int
1524fw_explore_read_quads(struct fw_device *fwdev, int offset,
1525    uint32_t *quad, int length)
1526{
1527	struct fw_xfer *xfer;
1528	uint32_t tmp;
1529	int i, error;
1530
1531	for (i = 0; i < length; i ++, offset += sizeof(uint32_t)) {
1532		xfer = fwmem_read_quad(fwdev, NULL, (uint8_t)-1,
1533		    0xffff, 0xf0000000 | offset, (void *)&tmp,
1534		    fw_xferwake);
1535		if (xfer == NULL)
1536			return (-1);
1537		fw_xferwait(xfer);
1538
1539		if (xfer->resp == 0)
1540			quad[i] = ntohl(tmp);
1541
1542		error = xfer->resp;
1543		fw_xfer_free(xfer);
1544		if (error)
1545			return (error);
1546	}
1547	return (0);
1548}
1549
1550
1551static int
1552fw_explore_csrblock(struct fw_device *fwdev, int offset, int recur)
1553{
1554	int err, i, off;
1555	struct csrdirectory *dir;
1556	struct csrreg *reg;
1557
1558	dir = (struct csrdirectory *)&fwdev->csrrom[offset/sizeof(uint32_t)];
1559	err = fw_explore_read_quads(fwdev, CSRROMOFF + offset,
1560	    (uint32_t *)dir, 1);
1561	if (err)
1562		return (-1);
1563
1564	offset += sizeof(uint32_t);
1565	reg = (struct csrreg *)&fwdev->csrrom[offset/sizeof(uint32_t)];
1566	err = fw_explore_read_quads(fwdev, CSRROMOFF + offset,
1567	    (uint32_t *)reg, dir->crc_len);
1568	if (err)
1569		return (-1);
1570
1571	/* XXX check CRC */
1572
1573	off = CSRROMOFF + offset + sizeof(uint32_t) * (dir->crc_len - 1);
1574	if (fwdev->rommax < off)
1575		fwdev->rommax = off;
1576
1577	if (recur == 0)
1578		return (0);
1579
1580	for (i = 0; (uint)i < dir->crc_len; i ++, offset += sizeof(uint32_t)) {
1581		if ((reg[i].key & CSRTYPE_MASK) == CSRTYPE_D)
1582			recur = 1;
1583		else if ((reg[i].key & CSRTYPE_MASK) == CSRTYPE_L)
1584			recur = 0;
1585		else
1586			continue;
1587
1588		off = offset + reg[i].val * sizeof(uint32_t);
1589		if (off > CROMSIZE) {
1590			printf("%s: invalid offset %d\n", __FUNCTION__, off);
1591			return(-1);
1592		}
1593		err = fw_explore_csrblock(fwdev, off, recur);
1594		if (err)
1595			return (-1);
1596	}
1597	return (0);
1598}
1599
1600static int
1601fw_explore_node(struct fw_device *dfwdev)
1602{
1603	struct firewire_comm *fc;
1604	struct fw_device *fwdev, *pfwdev, *tfwdev;
1605	uint32_t *csr;
1606	struct csrhdr *hdr;
1607	struct bus_info *binfo;
1608	int err, node;
1609	uint32_t speed_test = 0;
1610
1611	fc = dfwdev->fc;
1612	csr = dfwdev->csrrom;
1613	node = dfwdev->dst;
1614
1615	/* First quad */
1616	err = fw_explore_read_quads(dfwdev, CSRROMOFF, &csr[0], 1);
1617	if (err) {
1618		device_printf(fc->bdev, "%s: node%d: explore_read_quads failure\n",
1619			__func__, node);
1620		dfwdev->status = FWDEVINVAL;
1621		return (-1);
1622	}
1623	hdr = (struct csrhdr *)&csr[0];
1624	if (hdr->info_len != 4) {
1625		if (firewire_debug)
1626			device_printf(fc->bdev, "%s:"
1627				" node%d: wrong bus info len(%d)\n",
1628			       	__func__, node, hdr->info_len);
1629		dfwdev->status = FWDEVINVAL;
1630		return (-1);
1631	}
1632
1633	/* bus info */
1634	err = fw_explore_read_quads(dfwdev, CSRROMOFF + 0x04, &csr[1], 4);
1635	if (err) {
1636		device_printf(fc->bdev, "%s: node%d: error reading 0x04\n",
1637			__func__, node);
1638		dfwdev->status = FWDEVINVAL;
1639		return (-1);
1640	}
1641	binfo = (struct bus_info *)&csr[1];
1642	if (binfo->bus_name != CSR_BUS_NAME_IEEE1394) {
1643		device_printf(fc->bdev, "%s: node%d: invalid bus name 0x%08x\n",
1644			__func__, node, binfo->bus_name);
1645		dfwdev->status = FWDEVINVAL;
1646		return (-1);
1647	}
1648
1649	if (firewire_debug)
1650		device_printf(fc->bdev, "%s: node(%d) BUS INFO BLOCK:\n"
1651			"irmc(%d) cmc(%d) isc(%d) bmc(%d) pmc(%d) "
1652			"cyc_clk_acc(%d) max_rec(%d) max_rom(%d) "
1653			"generation(%d) link_spd(%d)\n",
1654			__func__, node,
1655			binfo->irmc, binfo->cmc, binfo->isc,
1656			binfo->bmc, binfo->pmc, binfo->cyc_clk_acc,
1657			binfo->max_rec, binfo->max_rom,
1658			binfo->generation, binfo->link_spd);
1659
1660	STAILQ_FOREACH(fwdev, &fc->devices, link)
1661		if (FW_EUI64_EQUAL(fwdev->eui, binfo->eui64))
1662			break;
1663	if (fwdev == NULL) {
1664		/* new device */
1665		fwdev = (fw_device*)malloc(sizeof(struct fw_device));
1666		if (fwdev == NULL) {
1667			device_printf(fc->bdev, "%s: node%d: no memory\n",
1668				__func__, node);
1669			return (-1);
1670		}
1671		memset(fwdev, 0, sizeof(struct fw_device));
1672		fwdev->fc = fc;
1673		fwdev->eui = binfo->eui64;
1674		fwdev->dst = dfwdev->dst;
1675		fwdev->maxrec = dfwdev->maxrec;
1676		fwdev->status = dfwdev->status;
1677
1678		/*
1679		 * Pre-1394a-2000 didn't have link_spd in
1680		 * the Bus Info block, so try and use the
1681		 * speed map value.
1682		 * 1394a-2000 compliant devices only use
1683		 * the Bus Info Block link spd value, so
1684		 * ignore the speed map alltogether. SWB
1685		 */
1686		if ( binfo->link_spd == FWSPD_S100 /* 0 */) {
1687			device_printf(fc->bdev, "%s: "
1688				"Pre 1394a-2000 detected\n",
1689				__func__);
1690			fwdev->speed = fc->speed_map->speed[fc->nodeid][node];
1691		} else
1692			fwdev->speed = binfo->link_spd;
1693		/*
1694		 * Test this speed with a read to the CSRROM.
1695		 * If it fails, slow down the speed and retry.
1696		 */
1697		while (fwdev->speed > FWSPD_S100 /* 0 */) {
1698			err = fw_explore_read_quads(fwdev, CSRROMOFF,
1699            				&speed_test, 1);
1700			if (err) {
1701				device_printf(fc->bdev, "%s: fwdev->speed(%s)"
1702					" decremented due to negotiation\n",
1703					 __func__,
1704					  linkspeed[fwdev->speed]);
1705				fwdev->speed--;
1706			} else
1707				break;
1708
1709		}
1710		/* inesrt into sorted fwdev list */
1711		pfwdev = NULL;
1712		STAILQ_FOREACH(tfwdev, &fc->devices, link) {
1713			if (tfwdev->eui.hi > fwdev->eui.hi ||
1714				(tfwdev->eui.hi == fwdev->eui.hi &&
1715				tfwdev->eui.lo > fwdev->eui.lo))
1716				break;
1717			pfwdev = tfwdev;
1718		}
1719		if (pfwdev == NULL)
1720			STAILQ_INSERT_HEAD(&fc->devices, fwdev, link);
1721		else
1722			STAILQ_INSERT_AFTER(&fc->devices, pfwdev, fwdev, link);
1723
1724		device_printf(fc->bdev, "New %s device ID:%08x%08x\n",
1725		    linkspeed[fwdev->speed],
1726		    fwdev->eui.hi, fwdev->eui.lo);
1727	} else {
1728		fwdev->dst = node;
1729		fwdev->status = FWDEVINIT;
1730		/* unchanged ? */
1731		if (bcmp(&csr[0], &fwdev->csrrom[0], sizeof(uint32_t) * 5) == 0) {
1732			if (firewire_debug)
1733				device_printf(fc->dev, "node%d: crom unchanged\n", node);
1734			return (0);
1735		}
1736	}
1737
1738	bzero(&fwdev->csrrom[0], CROMSIZE);
1739
1740	/* copy first quad and bus info block */
1741	bcopy(&csr[0], &fwdev->csrrom[0], sizeof(uint32_t) * 5);
1742	fwdev->rommax = CSRROMOFF + sizeof(uint32_t) * 4;
1743
1744	err = fw_explore_csrblock(fwdev, 0x14, 1); /* root directory */
1745
1746	if (err) {
1747		if (firewire_debug)
1748			device_printf(fc->dev, "%s:"
1749				" explore csrblock failed err(%d)\n",
1750				__func__, err);
1751		fwdev->status = FWDEVINVAL;
1752		fwdev->csrrom[0] = 0;
1753	}
1754	return (err);
1755
1756}
1757
1758/*
1759 * Find the self_id packet for a node, ignoring sequels.
1760 */
1761static union fw_self_id *
1762fw_find_self_id(struct firewire_comm *fc, int node)
1763{
1764	uint32_t i;
1765	union fw_self_id *s;
1766
1767	for (i = 0; i < fc->topology_map->self_id_count; i++) {
1768		s = &fc->topology_map->self_id[i];
1769		if (s->p0.sequel)
1770			continue;
1771		if ((int)s->p0.phy_id == node)
1772			return s;
1773	}
1774	return 0;
1775}
1776
1777static void
1778fw_explore(struct firewire_comm *fc)
1779{
1780	int node, err, s, i, todo, todo2, trys;
1781	char nodes[63];
1782	struct fw_device dfwdev;
1783	union fw_self_id *fwsid;
1784
1785	todo = 0;
1786	/* setup dummy fwdev */
1787	dfwdev.fc = fc;
1788	dfwdev.speed = 0;
1789	dfwdev.maxrec = 8; /* 512 */
1790	dfwdev.status = FWDEVINIT;
1791
1792	for (node = 0; (uint)node <= fc->max_node; node ++) {
1793		/* We don't probe myself and linkdown nodes */
1794		if (node == fc->nodeid) {
1795			if (firewire_debug)
1796				device_printf(fc->bdev, "%s:"
1797					" found myself node(%d) fc->nodeid(%d)"
1798					" fc->max_node(%d)\n",
1799					__func__, node, fc->nodeid, fc->max_node);
1800
1801			continue;
1802		} else if (firewire_debug) {
1803			device_printf(fc->bdev, "%s:"
1804				"node(%d) fc->max_node(%d) found\n",
1805				__func__, node, fc->max_node);
1806		}
1807
1808		fwsid = fw_find_self_id(fc, node);
1809		if (!fwsid || !fwsid->p0.link_active) {
1810			if (firewire_debug)
1811				device_printf(fc->bdev, "%s: node%d: link down\n",
1812					__func__, node);
1813			continue;
1814		}
1815		nodes[todo++] = node;
1816	}
1817
1818	s = splfw();
1819	for (trys = 0; todo > 0 && trys < 3; trys ++) {
1820		todo2 = 0;
1821		for (i = 0; i < todo; i ++) {
1822			dfwdev.dst = nodes[i];
1823			err = fw_explore_node(&dfwdev);
1824			if (err)
1825				nodes[todo2++] = nodes[i];
1826			if (firewire_debug)
1827				device_printf(fc->bdev, "%s: node %d, err = %d\n",
1828					__func__, node, err);
1829		}
1830		todo = todo2;
1831	}
1832	splx(s);
1833}
1834
1835
1836static int32
1837fw_bus_probe_thread(void *arg)
1838{
1839	struct firewire_comm *fc;
1840
1841	fc = (struct firewire_comm *)arg;
1842	mtx_lock(&fc->wait_lock);
1843	while (fc->status != (uint32_t)FWBUSDETACH) {
1844		if (fc->status == FWBUSEXPLORE) {
1845			mtx_unlock(&fc->wait_lock);
1846			fw_explore(fc);
1847			fc->status = FWBUSEXPDONE;
1848			if (firewire_debug)
1849				printf("bus_explore done\n");
1850			fw_attach_dev(fc);
1851			mtx_lock(&fc->wait_lock);
1852		}
1853//		msleep((void *)fc, &fc->wait_lock, PWAIT|PCATCH, "-", 0);
1854		mtx_unlock(&fc->wait_lock);
1855		acquire_sem(fc->Sem);
1856		mtx_lock(&fc->wait_lock);
1857
1858	}
1859	mtx_unlock(&fc->wait_lock);
1860//	kproc_exit(0);
1861	return 0;
1862}
1863
1864/*
1865 * To attach sub-devices layer onto IEEE1394 bus.
1866 */
1867static void
1868fw_attach_dev(struct firewire_comm *fc)
1869{
1870	struct fw_device *fwdev, *next;
1871//	int i, err;
1872//	device_t *devlistp;
1873//	int devcnt;
1874	struct firewire_dev_comm *fdc;
1875	struct firewire_child_info *element = fc->childList;
1876
1877	for (fwdev = STAILQ_FIRST(&fc->devices); fwdev != NULL; fwdev = next) {
1878		next = STAILQ_NEXT(fwdev, link);
1879		if (fwdev->status == FWDEVINIT) {
1880			fwdev->status = FWDEVATTACHED;
1881		} else if (fwdev->status == FWDEVINVAL) {
1882			fwdev->rcnt ++;
1883			if (firewire_debug)
1884				device_printf(fc->bdev, "%s:"
1885					"fwdev->rcnt(%d), hold_count(%d)\n",
1886					__func__, fwdev->rcnt, hold_count);
1887			if (fwdev->rcnt > hold_count) {
1888				/*
1889				 * Remove devices which have not been seen
1890				 * for a while.
1891				 */
1892				device_printf(fc->bdev, "%s:"
1893					"Removing missing device ID:%08x%08x\n",
1894					__func__, fwdev->eui.hi, fwdev->eui.lo);
1895				STAILQ_REMOVE(&fc->devices, fwdev, fw_device,
1896				    link);
1897				free(fwdev);
1898			}
1899		}
1900	}
1901
1902/*	err = device_get_children(fc->bdev, &devlistp, &devcnt);
1903	if( err == 0 ) {
1904		for( i = 0 ; i < devcnt ; i++){
1905			if (device_get_state(devlistp[i]) >= DS_ATTACHED)  {
1906				fdc = device_get_softc(devlistp[i]);
1907				if (fdc->post_explore != NULL)
1908					fdc->post_explore(fdc);
1909			}
1910		}
1911		free(devlistp, M_TEMP);
1912	}*/
1913
1914	while(element){
1915//		fdc = &element->cookie.fd;
1916		fdc = (struct firewire_dev_comm *)element->cookie;
1917		if (fdc->post_explore != NULL)
1918			fdc->post_explore(fdc);
1919		element = element->link;
1920	}
1921
1922
1923	return;
1924}
1925
1926/*
1927 * To allocate unique transaction label.
1928 */
1929static int
1930fw_get_tlabel(struct firewire_comm *fc, struct fw_xfer *xfer)
1931{
1932	u_int dst, new_tlabel;
1933	struct fw_xfer *txfer;
1934	int s;
1935
1936	dst = xfer->send.hdr.mode.hdr.dst & 0x3f;
1937	s = splfw();
1938	mtx_lock(&fc->tlabel_lock);
1939	new_tlabel = (fc->last_tlabel[dst] + 1) & 0x3f;
1940	STAILQ_FOREACH(txfer, &fc->tlabels[new_tlabel], tlabel)
1941		if ((txfer->send.hdr.mode.hdr.dst & 0x3f) == dst)
1942				break;
1943	if(txfer == NULL) {
1944		fc->last_tlabel[dst] = new_tlabel;
1945		STAILQ_INSERT_TAIL(&fc->tlabels[new_tlabel], xfer, tlabel);
1946		mtx_unlock(&fc->tlabel_lock);
1947		splx(s);
1948		xfer->tl = new_tlabel;
1949		xfer->send.hdr.mode.hdr.tlrt = new_tlabel << 2;
1950		if (firewire_debug > 1)
1951			printf("fw_get_tlabel: dst=%d tl=%d\n", dst, new_tlabel);
1952		return (new_tlabel);
1953	}
1954	mtx_unlock(&fc->tlabel_lock);
1955	splx(s);
1956
1957	if (firewire_debug > 1)
1958		printf("fw_get_tlabel: no free tlabel\n");
1959	return (-1);
1960}
1961
1962static void
1963fw_rcv_copy(struct fw_rcv_buf *rb)
1964{
1965	struct fw_pkt *pkt;
1966	u_char *p;
1967	struct tcode_info *tinfo;
1968	u_int res, i, len, plen;
1969
1970	rb->xfer->recv.spd = rb->spd;
1971
1972	pkt = (struct fw_pkt *)rb->vec->iov_base;
1973	tinfo = &rb->fc->tcode[pkt->mode.hdr.tcode];
1974
1975	/* Copy header */
1976	p = (u_char *)&rb->xfer->recv.hdr;
1977	bcopy(rb->vec->iov_base, p, tinfo->hdr_len);
1978	rb->vec->iov_base = (u_char *)rb->vec->iov_base + tinfo->hdr_len;
1979	rb->vec->iov_len -= tinfo->hdr_len;
1980
1981	/* Copy payload */
1982	p = (u_char *)rb->xfer->recv.payload;
1983	res = rb->xfer->recv.pay_len;
1984
1985	/* special handling for RRESQ */
1986	if (pkt->mode.hdr.tcode == FWTCODE_RRESQ &&
1987	    p != NULL && res >= sizeof(uint32_t)) {
1988		*(uint32_t *)p = pkt->mode.rresq.data;
1989		rb->xfer->recv.pay_len = sizeof(uint32_t);
1990		return;
1991	}
1992
1993	if ((tinfo->flag & FWTI_BLOCK_ASY) == 0)
1994		return;
1995
1996	plen = pkt->mode.rresb.len;
1997
1998	for (i = 0; i < rb->nvec; i++, rb->vec++) {
1999		len = MIN(rb->vec->iov_len, plen);
2000		if (res < len) {
2001			device_printf(rb->fc->bdev, "%s:"
2002				" rcv buffer(%d) is %d bytes short.\n",
2003				__func__, rb->xfer->recv.pay_len, len - res);
2004			len = res;
2005		}
2006		bcopy(rb->vec->iov_base, p, len);
2007		p += len;
2008		res -= len;
2009		plen -= len;
2010		if (res == 0 || plen == 0)
2011			break;
2012	}
2013	rb->xfer->recv.pay_len -= res;
2014
2015}
2016
2017/*
2018 * Generic packet receiving process.
2019 */
2020void
2021fw_rcv(struct fw_rcv_buf *rb)
2022{
2023	struct fw_pkt *fp, *resfp;
2024	struct fw_bind *bind;
2025	int tcode;
2026	int i, len, oldstate;
2027#if 0
2028	{
2029		uint32_t *qld;
2030		int i;
2031		qld = (uint32_t *)buf;
2032		printf("spd %d len:%d\n", spd, len);
2033		for( i = 0 ; i <= len && i < 32; i+= 4){
2034			printf("0x%08x ", ntohl(qld[i/4]));
2035			if((i % 16) == 15) printf("\n");
2036		}
2037		if((i % 16) != 15) printf("\n");
2038	}
2039#endif
2040	fp = (struct fw_pkt *)rb->vec[0].iov_base;
2041	tcode = fp->mode.common.tcode;
2042	switch (tcode) {
2043	case FWTCODE_WRES:
2044	case FWTCODE_RRESQ:
2045	case FWTCODE_RRESB:
2046	case FWTCODE_LRES:
2047		rb->xfer = fw_tl2xfer(rb->fc, fp->mode.hdr.src,
2048				fp->mode.hdr.tlrt >> 2, fp->mode.hdr.tcode);
2049		if(rb->xfer == NULL) {
2050			device_printf(rb->fc->bdev, "%s: "
2051				"unknown response "
2052			    	"%s(%x) src=0x%x tl=0x%x rt=%d data=0x%x\n",
2053				__func__,
2054			    	tcode_str[tcode], tcode,
2055				fp->mode.hdr.src,
2056				fp->mode.hdr.tlrt >> 2,
2057				fp->mode.hdr.tlrt & 3,
2058				fp->mode.rresq.data);
2059#if 0
2060			printf("try ad-hoc work around!!\n");
2061			rb->xfer = fw_tl2xfer(rb->fc, fp->mode.hdr.src,
2062					(fp->mode.hdr.tlrt >> 2)^3);
2063			if (rb->xfer == NULL) {
2064				printf("no use...\n");
2065				return;
2066			}
2067#else
2068			return;
2069#endif
2070		}
2071		fw_rcv_copy(rb);
2072		if (rb->xfer->recv.hdr.mode.wres.rtcode != RESP_CMP)
2073			rb->xfer->resp = EIO;
2074		else
2075			rb->xfer->resp = 0;
2076		/* make sure the packet is drained in AT queue */
2077		oldstate = rb->xfer->flag;
2078		rb->xfer->flag = FWXF_RCVD;
2079		switch (oldstate) {
2080		case FWXF_SENT:
2081			fw_xfer_done(rb->xfer);
2082			break;
2083		case FWXF_START:
2084#if 0
2085			if (firewire_debug)
2086				printf("not sent yet tl=%x\n", rb->xfer->tl);
2087#endif
2088			break;
2089		default:
2090			device_printf(rb->fc->bdev, "%s: "
2091				"unexpected flag 0x%02x\n", __func__, rb->xfer->flag);
2092
2093		}
2094		return;
2095	case FWTCODE_WREQQ:
2096	case FWTCODE_WREQB:
2097	case FWTCODE_RREQQ:
2098	case FWTCODE_RREQB:
2099	case FWTCODE_LREQ:
2100		bind = fw_bindlookup(rb->fc, fp->mode.rreqq.dest_hi,
2101			fp->mode.rreqq.dest_lo);
2102		if(bind == NULL){
2103			device_printf(rb->fc->bdev, "%s: "
2104				"Unknown service addr 0x%04x:0x%08x %s(%x)"
2105			    " src=0x%x data=%u\n",
2106				__func__,
2107				fp->mode.wreqq.dest_hi,
2108				fp->mode.wreqq.dest_lo,
2109				tcode_str[tcode], tcode,
2110				fp->mode.hdr.src,
2111				ntohl(fp->mode.wreqq.data));
2112
2113			if (rb->fc->status == FWBUSINIT) {
2114				device_printf(rb->fc->bdev, "%s: "
2115					"cannot respond(bus reset)!\n",
2116					__func__);
2117				return;
2118			}
2119			rb->xfer = fw_xfer_alloc();
2120			if(rb->xfer == NULL){
2121				return;
2122			}
2123			rb->xfer->send.spd = rb->spd;
2124			rb->xfer->send.pay_len = 0;
2125			resfp = &rb->xfer->send.hdr;
2126			switch (tcode) {
2127			case FWTCODE_WREQQ:
2128			case FWTCODE_WREQB:
2129				resfp->mode.hdr.tcode = FWTCODE_WRES;
2130				break;
2131			case FWTCODE_RREQQ:
2132				resfp->mode.hdr.tcode = FWTCODE_RRESQ;
2133				break;
2134			case FWTCODE_RREQB:
2135				resfp->mode.hdr.tcode = FWTCODE_RRESB;
2136				break;
2137			case FWTCODE_LREQ:
2138				resfp->mode.hdr.tcode = FWTCODE_LRES;
2139				break;
2140			}
2141			resfp->mode.hdr.dst = fp->mode.hdr.src;
2142			resfp->mode.hdr.tlrt = fp->mode.hdr.tlrt;
2143			resfp->mode.hdr.pri = fp->mode.hdr.pri;
2144			resfp->mode.rresb.rtcode = RESP_ADDRESS_ERROR;
2145			resfp->mode.rresb.extcode = 0;
2146			resfp->mode.rresb.len = 0;
2147/*
2148			rb->xfer->hand = fw_xferwake;
2149*/
2150			rb->xfer->hand = fw_xfer_free;
2151			if(fw_asyreq(rb->fc, -1, rb->xfer)){
2152				fw_xfer_free(rb->xfer);
2153				return;
2154			}
2155			return;
2156		}
2157		len = 0;
2158		for (i = 0; (uint)i < rb->nvec; i ++)
2159			len += rb->vec[i].iov_len;
2160		rb->xfer = STAILQ_FIRST(&bind->xferlist);
2161		if (rb->xfer == NULL) {
2162			device_printf(rb->fc->bdev, "%s: "
2163				"Discard a packet for this bind.\n",
2164				__func__);
2165			return;
2166		}
2167		STAILQ_REMOVE_HEAD(&bind->xferlist, link);
2168		fw_rcv_copy(rb);
2169		rb->xfer->hand(rb->xfer);
2170		return;
2171#if 0 /* shouldn't happen ?? or for GASP */
2172	case FWTCODE_STREAM:
2173	{
2174		struct fw_xferq *xferq;
2175
2176		xferq = rb->fc->ir[sub];
2177#if 0
2178		printf("stream rcv dma %d len %d off %d spd %d\n",
2179			sub, len, off, spd);
2180#endif
2181		if(xferq->queued >= xferq->maxq) {
2182			printf("receive queue is full\n");
2183			return;
2184		}
2185		/* XXX get xfer from xfer queue, we don't need copy for
2186			per packet mode */
2187		rb->xfer = fw_xfer_alloc_buf(M_FWXFER, 0, /* XXX */
2188						vec[0].iov_len);
2189		if (rb->xfer == NULL)
2190			return;
2191		fw_rcv_copy(rb)
2192		s = splfw();
2193		xferq->queued++;
2194		STAILQ_INSERT_TAIL(&xferq->q, rb->xfer, link);
2195		splx(s);
2196		sc = device_get_softc(rb->fc->bdev);
2197#if defined(__DragonFly__) || __FreeBSD_version < 500000
2198		if (&xferq->rsel.si_pid != 0)
2199#else
2200		if (SEL_WAITING(&xferq->rsel))
2201#endif
2202			selwakeuppri(&xferq->rsel, FWPRI);
2203		if (xferq->flag & FWXFERQ_WAKEUP) {
2204			xferq->flag &= ~FWXFERQ_WAKEUP;
2205			wakeup((caddr_t)xferq);
2206		}
2207		if (xferq->flag & FWXFERQ_HANDLER) {
2208			xferq->hand(xferq);
2209		}
2210		return;
2211		break;
2212	}
2213#endif
2214	default:
2215		device_printf(rb->fc->bdev,"%s: unknown tcode %d\n",
2216				__func__, tcode);
2217		break;
2218	}
2219}
2220
2221/*
2222 * Post process for Bus Manager election process.
2223 */
2224static void
2225fw_try_bmr_callback(struct fw_xfer *xfer)
2226{
2227	struct firewire_comm *fc;
2228	int bmr;
2229
2230	if (xfer == NULL)
2231		return;
2232	fc = xfer->fc;
2233	if (xfer->resp != 0)
2234		goto error;
2235	if (xfer->recv.payload == NULL)
2236		goto error;
2237	if (xfer->recv.hdr.mode.lres.rtcode != FWRCODE_COMPLETE)
2238		goto error;
2239
2240	bmr = ntohl(xfer->recv.payload[0]);
2241	if (bmr == 0x3f)
2242		bmr = fc->nodeid;
2243
2244	CSRARC(fc, BUS_MGR_ID) = fc->set_bmr(fc, bmr & 0x3f);
2245	fw_xfer_free_buf(xfer);
2246	fw_bmr(fc);
2247	return;
2248
2249error:
2250	device_printf(fc->bdev, "bus manager election failed\n");
2251	fw_xfer_free_buf(xfer);
2252}
2253
2254
2255/*
2256 * To candidate Bus Manager election process.
2257 */
2258static void
2259fw_try_bmr(void *arg)
2260{
2261	struct fw_xfer *xfer;
2262	struct firewire_comm *fc = (struct firewire_comm *)arg;
2263	struct fw_pkt *fp;
2264	int err = 0;
2265
2266	xfer = fw_xfer_alloc_buf(8, 4);
2267	if(xfer == NULL){
2268		return;
2269	}
2270	xfer->send.spd = 0;
2271	fc->status = FWBUSMGRELECT;
2272
2273	fp = &xfer->send.hdr;
2274	fp->mode.lreq.dest_hi = 0xffff;
2275	fp->mode.lreq.tlrt = 0;
2276	fp->mode.lreq.tcode = FWTCODE_LREQ;
2277	fp->mode.lreq.pri = 0;
2278	fp->mode.lreq.src = 0;
2279	fp->mode.lreq.len = 8;
2280	fp->mode.lreq.extcode = EXTCODE_CMP_SWAP;
2281	fp->mode.lreq.dst = FWLOCALBUS | fc->irm;
2282	fp->mode.lreq.dest_lo = 0xf0000000 | BUS_MGR_ID;
2283	xfer->send.payload[0] = htonl(0x3f);
2284	xfer->send.payload[1] = htonl(fc->nodeid);
2285	xfer->hand = fw_try_bmr_callback;
2286
2287	err = fw_asyreq(fc, -1, xfer);
2288	if(err){
2289		fw_xfer_free_buf(xfer);
2290		return;
2291	}
2292	return;
2293}
2294
2295#ifdef FW_VMACCESS
2296/*
2297 * Software implementation for physical memory block access.
2298 * XXX:Too slow, usef for debug purpose only.
2299 */
2300static void
2301fw_vmaccess(struct fw_xfer *xfer){
2302	struct fw_pkt *rfp, *sfp = NULL;
2303	uint32_t *ld = (uint32_t *)xfer->recv.buf;
2304
2305	printf("vmaccess spd:%2x len:%03x data:%08x %08x %08x %08x\n",
2306			xfer->spd, xfer->recv.len, ntohl(ld[0]), ntohl(ld[1]), ntohl(ld[2]), ntohl(ld[3]));
2307	printf("vmaccess          data:%08x %08x %08x %08x\n", ntohl(ld[4]), ntohl(ld[5]), ntohl(ld[6]), ntohl(ld[7]));
2308	if(xfer->resp != 0){
2309		fw_xfer_free( xfer);
2310		return;
2311	}
2312	if(xfer->recv.buf == NULL){
2313		fw_xfer_free( xfer);
2314		return;
2315	}
2316	rfp = (struct fw_pkt *)xfer->recv.buf;
2317	switch(rfp->mode.hdr.tcode){
2318		/* XXX need fix for 64bit arch */
2319		case FWTCODE_WREQB:
2320			xfer->send.buf = malloc(12, M_FW, M_NOWAIT);
2321			xfer->send.len = 12;
2322			sfp = (struct fw_pkt *)xfer->send.buf;
2323			bcopy(rfp->mode.wreqb.payload,
2324				(caddr_t)ntohl(rfp->mode.wreqb.dest_lo), ntohs(rfp->mode.wreqb.len));
2325			sfp->mode.wres.tcode = FWTCODE_WRES;
2326			sfp->mode.wres.rtcode = 0;
2327			break;
2328		case FWTCODE_WREQQ:
2329			xfer->send.buf = malloc(12, M_FW, M_NOWAIT);
2330			xfer->send.len = 12;
2331			sfp->mode.wres.tcode = FWTCODE_WRES;
2332			*((uint32_t *)(ntohl(rfp->mode.wreqb.dest_lo))) = rfp->mode.wreqq.data;
2333			sfp->mode.wres.rtcode = 0;
2334			break;
2335		case FWTCODE_RREQB:
2336			xfer->send.buf = malloc(16 + rfp->mode.rreqb.len, M_FW, M_NOWAIT);
2337			xfer->send.len = 16 + ntohs(rfp->mode.rreqb.len);
2338			sfp = (struct fw_pkt *)xfer->send.buf;
2339			bcopy((caddr_t)ntohl(rfp->mode.rreqb.dest_lo),
2340				sfp->mode.rresb.payload, (uint16_t)ntohs(rfp->mode.rreqb.len));
2341			sfp->mode.rresb.tcode = FWTCODE_RRESB;
2342			sfp->mode.rresb.len = rfp->mode.rreqb.len;
2343			sfp->mode.rresb.rtcode = 0;
2344			sfp->mode.rresb.extcode = 0;
2345			break;
2346		case FWTCODE_RREQQ:
2347			xfer->send.buf = malloc(16, M_FW, M_NOWAIT);
2348			xfer->send.len = 16;
2349			sfp = (struct fw_pkt *)xfer->send.buf;
2350			sfp->mode.rresq.data = *(uint32_t *)(ntohl(rfp->mode.rreqq.dest_lo));
2351			sfp->mode.wres.tcode = FWTCODE_RRESQ;
2352			sfp->mode.rresb.rtcode = 0;
2353			break;
2354		default:
2355			fw_xfer_free( xfer);
2356			return;
2357	}
2358	sfp->mode.hdr.dst = rfp->mode.hdr.src;
2359	xfer->dst = ntohs(rfp->mode.hdr.src);
2360	xfer->hand = fw_xfer_free;
2361
2362	sfp->mode.hdr.tlrt = rfp->mode.hdr.tlrt;
2363	sfp->mode.hdr.pri = 0;
2364
2365	fw_asyreq(xfer->fc, -1, xfer);
2366/**/
2367	return;
2368}
2369#endif
2370
2371/*
2372 * CRC16 check-sum for IEEE1394 register blocks.
2373 */
2374uint16_t
2375fw_crc16(uint32_t *ptr, uint32_t len){
2376	uint32_t i, sum, crc = 0;
2377	int shift;
2378	len = (len + 3) & ~3;
2379	for(i = 0 ; i < len ; i+= 4){
2380		for( shift = 28 ; shift >= 0 ; shift -= 4){
2381			sum = ((crc >> 12) ^ (ptr[i/4] >> shift)) & 0xf;
2382			crc = (crc << 4) ^ ( sum << 12 ) ^ ( sum << 5) ^ sum;
2383		}
2384		crc &= 0xffff;
2385	}
2386	return((uint16_t) crc);
2387}
2388
2389/*
2390 * Find the root node, if it is not
2391 * Cycle Master Capable, then we should
2392 * override this and become the Cycle
2393 * Master
2394 */
2395static int
2396fw_bmr(struct firewire_comm *fc)
2397{
2398	struct fw_device fwdev;
2399	union fw_self_id *self_id;
2400	int cmstr;
2401	uint32_t quad;
2402
2403	/* Check to see if the current root node is cycle master capable */
2404	self_id = fw_find_self_id(fc, fc->max_node);
2405	if (fc->max_node > 0) {
2406		/* XXX check cmc bit of businfo block rather than contender */
2407		if (self_id->p0.link_active && self_id->p0.contender)
2408			cmstr = fc->max_node;
2409		else {
2410			device_printf(fc->bdev,
2411				"root node is not cycle master capable\n");
2412			/* XXX shall we be the cycle master? */
2413			cmstr = fc->nodeid;
2414			/* XXX need bus reset */
2415		}
2416	} else
2417		cmstr = -1;
2418
2419		device_printf(fc->bdev, "bus manager %d %s\n",
2420			CSRARC(fc, BUS_MGR_ID),
2421			(CSRARC(fc, BUS_MGR_ID) != fc->nodeid) ? "(me)" : "");
2422	if(CSRARC(fc, BUS_MGR_ID) != fc->nodeid) {
2423		/* We are not the bus manager */
2424		return(0);
2425	}
2426
2427	/* Optimize gapcount */
2428	if(fc->max_hop <= MAX_GAPHOP )
2429		fw_phy_config(fc, cmstr, gap_cnt[fc->max_hop]);
2430	/* If we are the cycle master, nothing to do */
2431	if (cmstr == fc->nodeid || cmstr == -1)
2432		return 0;
2433	/* Bus probe has not finished, make dummy fwdev for cmstr */
2434	bzero(&fwdev, sizeof(fwdev));
2435	fwdev.fc = fc;
2436	fwdev.dst = cmstr;
2437	fwdev.speed = 0;
2438	fwdev.maxrec = 8; /* 512 */
2439	fwdev.status = FWDEVINIT;
2440	/* Set cmstr bit on the cycle master */
2441	quad = htonl(1 << 8);
2442	fwmem_write_quad(&fwdev, NULL, 0/*spd*/,
2443		0xffff, 0xf0000000 | STATE_SET, &quad, fw_asy_callback_free);
2444
2445	return 0;
2446}
2447
2448int
2449fw_open_isodma(struct firewire_comm *fc, int tx)
2450{
2451	struct fw_xferq **xferqa;
2452	struct fw_xferq *xferq;
2453	int i;
2454
2455	if (tx)
2456		xferqa = &fc->it[0];
2457	else
2458		xferqa = &fc->ir[0];
2459
2460	FW_GLOCK(fc);
2461	for (i = 0; i < fc->nisodma; i ++) {
2462		xferq = xferqa[i];
2463		if ((xferq->flag & FWXFERQ_OPEN) == 0) {
2464			xferq->flag |= FWXFERQ_OPEN;
2465			break;
2466		}
2467	}
2468	if (i == fc->nisodma) {
2469		printf("no free dma channel (tx=%d)\n", tx);
2470		i = -1;
2471	}
2472	FW_GUNLOCK(fc);
2473	return (i);
2474}
2475