core.c

linux-2.4.29操作系统的源码

		if (xoff_char != RFCOMM_RPN_XOFF_CHAR) {
			BT_DBG("RPN XOFF char mismatch 0x%x", xoff_char);
			xoff_char = RFCOMM_RPN_XOFF_CHAR;
			rpn_mask ^= RFCOMM_RPN_PM_XOFF;
		}
	}

rpn_out:
	rfcomm_send_rpn(s, 0, dlci, 
			bit_rate, data_bits, stop_bits, parity, flow_ctrl,
			xon_char, xoff_char, rpn_mask);

	return 0;
}

static int rfcomm_recv_rls(struct rfcomm_session *s, int cr, struct sk_buff *skb)
{
	struct rfcomm_rls *rls = (void *) skb->data;
	u8 dlci = __get_dlci(rls->dlci);

	BT_DBG("dlci %d cr %d status 0x%x", dlci, cr, rls->status);
	
	if (!cr)
		return 0;

	/* FIXME: We should probably do something with this
	   information here. But for now it's sufficient just
	   to reply -- Bluetooth 1.1 says it's mandatory to 
	   recognise and respond to RLS */

	rfcomm_send_rls(s, 0, dlci, rls->status);

	return 0;
}

static int rfcomm_recv_msc(struct rfcomm_session *s, int cr, struct sk_buff *skb)
{
	struct rfcomm_msc *msc = (void *) skb->data;
	struct rfcomm_dlc *d;
	u8 dlci = __get_dlci(msc->dlci);

	BT_DBG("dlci %d cr %d v24 0x%x", dlci, cr, msc->v24_sig);

	d = rfcomm_dlc_get(s, dlci);
	if (!d)
		return 0;

	if (cr) {
		if (msc->v24_sig & RFCOMM_V24_FC && !d->cfc)
			set_bit(RFCOMM_TX_THROTTLED, &d->flags);
		else
			clear_bit(RFCOMM_TX_THROTTLED, &d->flags);
		
		rfcomm_dlc_lock(d);
		if (d->modem_status)
			d->modem_status(d, msc->v24_sig);
		rfcomm_dlc_unlock(d);
		
		rfcomm_send_msc(s, 0, dlci, msc->v24_sig);

		d->mscex |= RFCOMM_MSCEX_RX;
	} else 
		d->mscex |= RFCOMM_MSCEX_TX;

	return 0;
}

static int rfcomm_recv_mcc(struct rfcomm_session *s, struct sk_buff *skb)
{
	struct rfcomm_mcc *mcc = (void *) skb->data;
	u8 type, cr, len;

	cr   = __test_cr(mcc->type);
	type = __get_mcc_type(mcc->type);
	len  = __get_mcc_len(mcc->len);

	BT_DBG("%p type 0x%x cr %d", s, type, cr);

	skb_pull(skb, 2);

	switch (type) {
	case RFCOMM_PN:
		rfcomm_recv_pn(s, cr, skb);
		break;

	case RFCOMM_RPN:
		rfcomm_recv_rpn(s, cr, len, skb);
		break;

	case RFCOMM_RLS:
		rfcomm_recv_rls(s, cr, skb);
		break;

	case RFCOMM_MSC:
		rfcomm_recv_msc(s, cr, skb);
		break;

	case RFCOMM_FCOFF:
		if (cr) {
			set_bit(RFCOMM_TX_THROTTLED, &s->flags);
			rfcomm_send_fcoff(s, 0);
		}
		break;

	case RFCOMM_FCON:
		if (cr) {
			clear_bit(RFCOMM_TX_THROTTLED, &s->flags);
			rfcomm_send_fcon(s, 0);
		}
		break;

	case RFCOMM_TEST:
		if (cr)
			rfcomm_send_test(s, 0, skb->data, skb->len);
		break;

	case RFCOMM_NSC:
		break;

	default:
		BT_ERR("Unknown control type 0x%02x", type);
		rfcomm_send_nsc(s, cr, type);
		break;
	}
	return 0;
}

static int rfcomm_recv_data(struct rfcomm_session *s, u8 dlci, int pf, struct sk_buff *skb)
{
	struct rfcomm_dlc *d;

	BT_DBG("session %p state %ld dlci %d pf %d", s, s->state, dlci, pf);

	d = rfcomm_dlc_get(s, dlci);
	if (!d) {
		rfcomm_send_dm(s, dlci);
		goto drop;
	}

	if (pf && d->cfc) {
		u8 credits = *(u8 *) skb->data; skb_pull(skb, 1);

		d->tx_credits += credits;
		if (d->tx_credits)
			clear_bit(RFCOMM_TX_THROTTLED, &d->flags);
	}

	if (skb->len && d->state == BT_CONNECTED) {
		rfcomm_dlc_lock(d);
		d->rx_credits--;
		d->data_ready(d, skb);
		rfcomm_dlc_unlock(d);
		return 0;
	}

drop:
	kfree_skb(skb);
	return 0;
}

static int rfcomm_recv_frame(struct rfcomm_session *s, struct sk_buff *skb)
{
	struct rfcomm_hdr *hdr = (void *) skb->data;
	u8 type, dlci, fcs;

	dlci = __get_dlci(hdr->addr);
	type = __get_type(hdr->ctrl);

	/* Trim FCS */
	skb->len--; skb->tail--;
	fcs = *(u8 *) skb->tail;
	
	if (__check_fcs(skb->data, type, fcs)) {
		BT_ERR("bad checksum in packet");
		kfree_skb(skb);
		return -EILSEQ;
	}

	if (__test_ea(hdr->len))
		skb_pull(skb, 3);
	else
		skb_pull(skb, 4);
	
	switch (type) {
	case RFCOMM_SABM:
		if (__test_pf(hdr->ctrl))
			rfcomm_recv_sabm(s, dlci);
		break;

	case RFCOMM_DISC:
		if (__test_pf(hdr->ctrl))
			rfcomm_recv_disc(s, dlci);
		break;

	case RFCOMM_UA:
		if (__test_pf(hdr->ctrl))
			rfcomm_recv_ua(s, dlci);
		break;

	case RFCOMM_DM:
		rfcomm_recv_dm(s, dlci);
		break;

	case RFCOMM_UIH:
		if (dlci)
			return rfcomm_recv_data(s, dlci, __test_pf(hdr->ctrl), skb);

		rfcomm_recv_mcc(s, skb);
		break;

	default:
		BT_ERR("Unknown packet type 0x%02x\n", type);
		break;
	}
	kfree_skb(skb);
	return 0;
}

/* ---- Connection and data processing ---- */

static void rfcomm_process_connect(struct rfcomm_session *s)
{
	struct rfcomm_dlc *d;
	struct list_head *p, *n;

	BT_DBG("session %p state %ld", s, s->state);

	list_for_each_safe(p, n, &s->dlcs) {
		d = list_entry(p, struct rfcomm_dlc, list);
		if (d->state == BT_CONFIG) {
			d->mtu = s->mtu;
			rfcomm_send_pn(s, 1, d);
		}
	}
}

/* Send data queued for the DLC.
 * Return number of frames left in the queue.
 */
static inline int rfcomm_process_tx(struct rfcomm_dlc *d)
{
	struct sk_buff *skb;
	int err;

	BT_DBG("dlc %p state %ld cfc %d rx_credits %d tx_credits %d", 
			d, d->state, d->cfc, d->rx_credits, d->tx_credits);

	/* Send pending MSC */
	if (test_and_clear_bit(RFCOMM_MSC_PENDING, &d->flags))
		rfcomm_send_msc(d->session, 1, d->dlci, d->v24_sig);
	
	if (d->cfc) {
		/* CFC enabled. 
		 * Give them some credits */
		if (!test_bit(RFCOMM_RX_THROTTLED, &d->flags) &&
			       	d->rx_credits <= (d->cfc >> 2)) {
			rfcomm_send_credits(d->session, d->addr, d->cfc - d->rx_credits);
			d->rx_credits = d->cfc;
		}
	} else {
		/* CFC disabled. 
		 * Give ourselves some credits */
		d->tx_credits = 5;
	}

	if (test_bit(RFCOMM_TX_THROTTLED, &d->flags))
		return skb_queue_len(&d->tx_queue);

	while (d->tx_credits && (skb = skb_dequeue(&d->tx_queue))) {
		err = rfcomm_send_frame(d->session, skb->data, skb->len);
		if (err < 0) {
			skb_queue_head(&d->tx_queue, skb);
			break;
		}
		kfree_skb(skb);
		d->tx_credits--;
	}

	if (d->cfc && !d->tx_credits) {
		/* We're out of TX credits.
		 * Set TX_THROTTLED flag to avoid unnesary wakeups by dlc_send. */
		set_bit(RFCOMM_TX_THROTTLED, &d->flags);
	}

	return skb_queue_len(&d->tx_queue);
}

static inline void rfcomm_process_dlcs(struct rfcomm_session *s)
{
	struct rfcomm_dlc *d;
	struct list_head *p, *n;

	BT_DBG("session %p state %ld", s, s->state);

	list_for_each_safe(p, n, &s->dlcs) {
		d = list_entry(p, struct rfcomm_dlc, list);
		if (test_bit(RFCOMM_TIMED_OUT, &d->flags)) {
			__rfcomm_dlc_close(d, ETIMEDOUT);
			continue;
		}

		if (test_bit(RFCOMM_TX_THROTTLED, &s->flags))
			continue;

		if ((d->state == BT_CONNECTED || d->state == BT_DISCONN) &&
				d->mscex == RFCOMM_MSCEX_OK)
			rfcomm_process_tx(d);
	}
}

static inline void rfcomm_process_rx(struct rfcomm_session *s)
{
	struct socket *sock = s->sock;
	struct sock *sk = sock->sk;
	struct sk_buff *skb;

	BT_DBG("session %p state %ld qlen %d", s, s->state, skb_queue_len(&sk->receive_queue));

	/* Get data directly from socket receive queue without copying it. */
	while ((skb = skb_dequeue(&sk->receive_queue))) {
		skb_orphan(skb);
		rfcomm_recv_frame(s, skb);
	}

	if (sk->state == BT_CLOSED) {
		if (!s->initiator)
			rfcomm_session_put(s);

		rfcomm_session_close(s, sk->err);
	}
}

static inline void rfcomm_accept_connection(struct rfcomm_session *s)
{
	struct socket *sock = s->sock, *nsock;
	int err;

	/* Fast check for a new connection.
	 * Avoids unnesesary socket allocations. */
	if (list_empty(&bluez_pi(sock->sk)->accept_q))
		return;

	BT_DBG("session %p", s);

	nsock = sock_alloc();
	if (!nsock)
		return;

	nsock->type = sock->type;
	nsock->ops  = sock->ops;
	
	err = sock->ops->accept(sock, nsock, O_NONBLOCK);
	if (err < 0) {
		sock_release(nsock);
		return;
	}

	/* Set our callbacks */
	nsock->sk->data_ready   = rfcomm_l2data_ready;
	nsock->sk->state_change = rfcomm_l2state_change;

	s = rfcomm_session_add(nsock, BT_OPEN);
	if (s) {
		rfcomm_session_hold(s);
		rfcomm_schedule(RFCOMM_SCHED_RX);
	} else
		sock_release(nsock);
}

static inline void rfcomm_check_connection(struct rfcomm_session *s)
{
	struct sock *sk = s->sock->sk;

	BT_DBG("%p state %ld", s, s->state);

	switch(sk->state) {
	case BT_CONNECTED:
		s->state = BT_CONNECT;

		/* We can adjust MTU on outgoing sessions.
		 * L2CAP MTU minus UIH header and FCS. */
		s->mtu = min(l2cap_pi(sk)->omtu, l2cap_pi(sk)->imtu) - 5;

		rfcomm_send_sabm(s, 0);
		break;

	case BT_CLOSED:
		s->state = BT_CLOSED;
		rfcomm_session_close(s, sk->err);
		break;
	}
}

static inline void rfcomm_process_sessions(void)
{
	struct list_head *p, *n;

	rfcomm_lock();

	list_for_each_safe(p, n, &session_list) {
		struct rfcomm_session *s;
		s = list_entry(p, struct rfcomm_session, list);

		if (s->state == BT_LISTEN) {
			rfcomm_accept_connection(s);
			continue;
		}

		rfcomm_session_hold(s);

		switch (s->state) {
		case BT_BOUND:
			rfcomm_check_connection(s);
			break;

		default:
			rfcomm_process_rx(s);
			break;
		}

		rfcomm_process_dlcs(s);

		rfcomm_session_put(s);
	}
	
	rfcomm_unlock();
}

static void rfcomm_worker(void)
{
	BT_DBG("");

	daemonize(); reparent_to_init();
	set_fs(KERNEL_DS);

	while (!atomic_read(&terminate)) {
		BT_DBG("worker loop event 0x%lx", rfcomm_event);

		if (!test_bit(RFCOMM_SCHED_WAKEUP, &rfcomm_event)) {
			/* No pending events. Let's sleep.
			 * Incomming connections and data will wake us up. */
			set_current_state(TASK_INTERRUPTIBLE);
			schedule();
		}

		/* Process stuff */
		clear_bit(RFCOMM_SCHED_WAKEUP, &rfcomm_event);
		rfcomm_process_sessions();
	}
	set_current_state(TASK_RUNNING);
	return;
}

static int rfcomm_add_listener(bdaddr_t *ba)
{
	struct sockaddr_l2 addr;
	struct l2cap_options opts;
	struct socket *sock;
	struct rfcomm_session *s;
	int    size, err = 0;

	/* Create socket */
	err = rfcomm_l2sock_create(&sock);
	if (err < 0) { 
		BT_ERR("Create socket failed %d", err);
		return err;
	}

	/* Bind socket */
	bacpy(&addr.l2_bdaddr, ba);
	addr.l2_family = AF_BLUETOOTH;
	addr.l2_psm    = htobs(RFCOMM_PSM);
	err = sock->ops->bind(sock, (struct sockaddr *) &addr, sizeof(addr));
	if (err < 0) {
		BT_ERR("Bind failed %d", err);
		goto failed;
	}

	/* Set L2CAP options */
	size = sizeof(opts);
	sock->ops->getsockopt(sock, SOL_L2CAP, L2CAP_OPTIONS, (void *)&opts, &size);

	opts.imtu = RFCOMM_MAX_L2CAP_MTU;
	sock->ops->setsockopt(sock, SOL_L2CAP, L2CAP_OPTIONS, (void *)&opts, size);

	/* Start listening on the socket */
	err = sock->ops->listen(sock, 10);
	if (err) {
		BT_ERR("Listen failed %d", err);
		goto failed;
	}

	/* Add listening session */
	s = rfcomm_session_add(sock, BT_LISTEN);
	if (!s)
		goto failed;

	rfcomm_session_hold(s);
	return 0;
failed:
	sock_release(sock);
	return err;
}

static void rfcomm_kill_listener(void)
{
	struct rfcomm_session *s;
	struct list_head *p, *n;

	BT_DBG("");

	list_for_each_safe(p, n, &session_list) {
		s = list_entry(p, struct rfcomm_session, list);
		rfcomm_session_del(s);
	}
}

static int rfcomm_run(void *unused)
{
	rfcomm_thread = current;

	atomic_inc(&running);

	daemonize(); reparent_to_init();

	sigfillset(&current->blocked);
	set_fs(KERNEL_DS);

	sprintf(current->comm, "krfcommd");

	BT_DBG("");

	rfcomm_add_listener(BDADDR_ANY);

	rfcomm_worker();

	rfcomm_kill_listener();

	atomic_dec(&running);
	return 0;
}

/* ---- Proc fs support ---- */
static int rfcomm_dlc_dump(char *buf)
{
	struct rfcomm_session *s;
	struct sock *sk;
	struct list_head *p, *pp;
	char *ptr = buf;

	rfcomm_lock();

	list_for_each(p, &session_list) {
		s = list_entry(p, struct rfcomm_session, list);
		sk = s->sock->sk;

		list_for_each(pp, &s->dlcs) {
		struct rfcomm_dlc *d;
			d = list_entry(pp, struct rfcomm_dlc, list);

			ptr += sprintf(ptr, "dlc %s %s %ld %d %d %d %d\n",
				batostr(&bluez_pi(sk)->src), batostr(&bluez_pi(sk)->dst),
				d->state, d->dlci, d->mtu, d->rx_credits, d->tx_credits);
		}
	}
	
	rfcomm_unlock();

	return ptr - buf;
}

extern int rfcomm_sock_dump(char *buf);

static int rfcomm_read_proc(char *buf, char **start, off_t offset, int count, int *eof, void *priv)
{
	char *ptr = buf;
	int len;

	BT_DBG("count %d, offset %ld", count, offset);

	ptr += rfcomm_dlc_dump(ptr);
	ptr += rfcomm_sock_dump(ptr);
	len  = ptr - buf;

	if (len <= count + offset)
		*eof = 1;

	*start = buf + offset;
	len -= offset;

	if (len > count)
		len = count;
	if (len < 0)
		len = 0;

	return len;
}

/* ---- Initialization ---- */
int __init rfcomm_init(void)
{
	l2cap_load();

	kernel_thread(rfcomm_run, NULL, CLONE_FS | CLONE_FILES | CLONE_SIGHAND);

	rfcomm_init_sockets();

#ifdef CONFIG_BLUEZ_RFCOMM_TTY
	rfcomm_init_ttys();
#endif

	create_proc_read_entry("bluetooth/rfcomm", 0, 0, rfcomm_read_proc, NULL);

	BT_INFO("BlueZ RFCOMM ver %s", VERSION);
	BT_INFO("Copyright (C) 2002 Maxim Krasnyansky <[e-mail]>");
	BT_INFO("Copyright (C) 2002 Marcel Holtmann <[e-mail]>");
	return 0;
}

void rfcomm_cleanup(void)
{
	/* Terminate working thread.
	 * ie. Set terminate flag and wake it up */
	atomic_inc(&terminate);
	rfcomm_schedule(RFCOMM_SCHED_STATE);

	/* Wait until thread is running */
	while (atomic_read(&running))
		schedule();

	remove_proc_entry("bluetooth/rfcomm", NULL);

#ifdef CONFIG_BLUEZ_RFCOMM_TTY
	rfcomm_cleanup_ttys();
#endif

	rfcomm_cleanup_sockets();
	return;
}

module_init(rfcomm_init);
module_exit(rfcomm_cleanup);

MODULE_AUTHOR("Maxim Krasnyansky <[e-mail]>, Marcel Holtmann <[e-mail]>");
MODULE_DESCRIPTION("BlueZ RFCOMM ver " VERSION);
MODULE_LICENSE("GPL");