core.c

Linux嵌入式平台蓝牙协议栈软件

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->sk_receive_queue));

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

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

		rfcomm_session_close(s, sk->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(&bt_sk(sock->sk)->accept_q))
		return;

	BT_DBG("session %p", s);

	if (sock_create_lite(PF_BLUETOOTH, sock->type, BTPROTO_L2CAP, &nsock))
		return;

	nsock->ops  = sock->ops;

	__module_get(nsock->ops->owner);

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

	/* Set our callbacks */
	nsock->sk->sk_data_ready   = rfcomm_l2data_ready;
	nsock->sk->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->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->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("");

	while (!atomic_read(&terminate)) {
		if (!test_bit(RFCOMM_SCHED_WAKEUP, &rfcomm_event)) {
			/* No pending events. Let's sleep.
			 * Incoming 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 socket *sock;
	struct sock *sk;
	struct rfcomm_session *s;
	int    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 */
	sk = sock->sk;
	lock_sock(sk);
	l2cap_pi(sk)->imtu = RFCOMM_MAX_L2CAP_MTU;
	release_sock(sk);

	/* 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("krfcommd");
	set_user_nice(current, -10);
	current->flags |= PF_NOFREEZE;

	BT_DBG("");

	rfcomm_add_listener(BDADDR_ANY);

	rfcomm_worker();

	rfcomm_kill_listener();

	atomic_dec(&running);
	return 0;
}

/* ---- Proc fs support ---- */
#ifdef CONFIG_PROC_FS
static void *rfcomm_seq_start(struct seq_file *seq, loff_t *pos)
{
	struct rfcomm_session *s;
	struct list_head *pp, *p;
	loff_t l = *pos;

	rfcomm_lock();

	list_for_each(p, &session_list) {
		s = list_entry(p, struct rfcomm_session, list);
		list_for_each(pp, &s->dlcs)
			if (!l--) {
				seq->private = s;
				return pp;
			}
	}
	return NULL;
}

static void *rfcomm_seq_next(struct seq_file *seq, void *e, loff_t *pos)
{
	struct rfcomm_session *s = seq->private;
	struct list_head *pp, *p = e;
	(*pos)++;

	if (p->next != &s->dlcs)
		return p->next;

	list_for_each(p, &session_list) {
		s = list_entry(p, struct rfcomm_session, list);
		__list_for_each(pp, &s->dlcs) {
			seq->private = s;
			return pp;
		}
	}
	return NULL;
}

static void rfcomm_seq_stop(struct seq_file *seq, void *e)
{
	rfcomm_unlock();
}

static int  rfcomm_seq_show(struct seq_file *seq, void *e)
{
	struct rfcomm_session *s = seq->private;
	struct sock *sk = s->sock->sk;
	struct rfcomm_dlc *d = list_entry(e, struct rfcomm_dlc, list);

	seq_printf(seq, "%s %s %ld %d %d %d %d\n",
			batostr(&bt_sk(sk)->src), batostr(&bt_sk(sk)->dst),
			d->state, d->dlci, d->mtu, d->rx_credits, d->tx_credits);
	return 0;
}

static struct seq_operations rfcomm_seq_ops = {
	.start  = rfcomm_seq_start,
	.next   = rfcomm_seq_next,
	.stop   = rfcomm_seq_stop,
	.show   = rfcomm_seq_show 
};

static int rfcomm_seq_open(struct inode *inode, struct file *file)
{
	return seq_open(file, &rfcomm_seq_ops);
}

static struct file_operations rfcomm_seq_fops = {
	.owner	 = THIS_MODULE,
	.open    = rfcomm_seq_open,
	.read    = seq_read,
	.llseek  = seq_lseek,
	.release = seq_release,
};

static int  __init rfcomm_proc_init(void)
{
        struct proc_dir_entry *p;

	proc_bt_rfcomm = proc_mkdir("rfcomm", proc_bt);
	if (proc_bt_rfcomm) {
		proc_bt_rfcomm->owner = THIS_MODULE;

        	p = create_proc_entry("dlc", S_IRUGO, proc_bt_rfcomm);
		if (p)
        		p->proc_fops = &rfcomm_seq_fops;
	}
        return 0;
}

static void __exit rfcomm_proc_cleanup(void)
{
        remove_proc_entry("dlc", proc_bt_rfcomm);

	remove_proc_entry("rfcomm", proc_bt);
}

#else /* CONFIG_PROC_FS */

static int  __init rfcomm_proc_init(void)
{
        return 0;
}

static void __exit rfcomm_proc_cleanup(void)
{
        return;
}
#endif /* CONFIG_PROC_FS */

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

	kernel_thread(rfcomm_run, NULL, CLONE_KERNEL);

	BT_INFO("RFCOMM ver %s", VERSION);

	rfcomm_proc_init();

	rfcomm_init_sockets();

#ifdef CONFIG_BT_RFCOMM_TTY
	rfcomm_init_ttys();
#endif

	return 0;
}

static void __exit rfcomm_exit(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();

#ifdef CONFIG_BT_RFCOMM_TTY
	rfcomm_cleanup_ttys();
#endif

	rfcomm_cleanup_sockets();

	rfcomm_proc_cleanup();
}

module_init(rfcomm_init);
module_exit(rfcomm_exit);

MODULE_AUTHOR("Maxim Krasnyansky <[e-mail]>, Marcel Holtmann <[e-mail]>");
MODULE_DESCRIPTION("Bluetooth RFCOMM ver " VERSION);
MODULE_VERSION(VERSION);
MODULE_LICENSE("GPL");
MODULE_ALIAS("bt-proto-3");