/* Delete channel.
* Must be called on the locked socket. */
static void l2cap_chan_del(struct sock *sk, int err)
{
struct l2cap_conn *conn = l2cap_pi(sk)->conn;
struct sock *parent = bluez_pi(sk)->parent;
l2cap_sock_clear_timer(sk);
BT_DBG("sk %p, conn %p, err %d", sk, conn, err);
if (conn) {
/* Unlink from channel list */
l2cap_chan_unlink(&conn->chan_list, sk);
l2cap_pi(sk)->conn = NULL;
hci_conn_put(conn->hcon);
}
sk->state = BT_CLOSED;
sk->zapped = 1;
if (err)
sk->err = err;
if (parent)
parent->data_ready(parent, 0);
else
sk->state_change(sk);
}
static int sco_sock_connect(struct socket *sock, struct sockaddr *addr, int alen, int flags)
{
struct sockaddr_sco *sa = (struct sockaddr_sco *) addr;
struct sock *sk = sock->sk;
int err = 0;
BT_DBG("sk %p", sk);
if (addr->sa_family != AF_BLUETOOTH || alen < sizeof(struct sockaddr_sco))
return -EINVAL;
if (sk->state != BT_OPEN && sk->state != BT_BOUND)
return -EBADFD;
if (sk->type != SOCK_SEQPACKET)
return -EINVAL;
lock_sock(sk);
/* Set destination address and psm */
bacpy(&bluez_pi(sk)->dst, &sa->sco_bdaddr);
if ((err = sco_connect(sk)))
goto done;
err = bluez_sock_wait_state(sk, BT_CONNECTED,
sock_sndtimeo(sk, flags & O_NONBLOCK));
done:
release_sock(sk);
return err;
}
static int l2cap_sock_bind(struct socket *sock, struct sockaddr *addr, int addr_len)
{
struct sockaddr_l2 *la = (struct sockaddr_l2 *) addr;
struct sock *sk = sock->sk;
int err = 0;
BT_DBG("sk %p, %s %d", sk, batostr(&la->l2_bdaddr), la->l2_psm);
if (!addr || addr->sa_family != AF_BLUETOOTH)
return -EINVAL;
lock_sock(sk);
if (sk->state != BT_OPEN) {
err = -EBADFD;
goto done;
}
write_lock_bh(&l2cap_sk_list.lock);
if (la->l2_psm && __l2cap_get_sock_by_addr(la->l2_psm, &la->l2_bdaddr)) {
err = -EADDRINUSE;
} else {
/* Save source address */
bacpy(&bluez_pi(sk)->src, &la->l2_bdaddr);
l2cap_pi(sk)->psm = la->l2_psm;
sk->sport = la->l2_psm;
sk->state = BT_BOUND;
}
write_unlock_bh(&l2cap_sk_list.lock);
done:
release_sock(sk);
return err;
}
int sco_connect(struct sock *sk)
{
bdaddr_t *src = &bluez_pi(sk)->src;
bdaddr_t *dst = &bluez_pi(sk)->dst;
struct sco_conn *conn;
struct hci_conn *hcon;
struct hci_dev *hdev;
int err = 0;
BT_DBG("%s -> %s", batostr(src), batostr(dst));
if (!(hdev = hci_get_route(dst, src)))
return -EHOSTUNREACH;
hci_dev_lock_bh(hdev);
err = -ENOMEM;
hcon = hci_connect(hdev, SCO_LINK, dst);
if (!hcon)
goto done;
conn = sco_conn_add(hcon, 0);
if (!conn) {
hci_conn_put(hcon);
goto done;
}
/* Update source addr of the socket */
bacpy(src, conn->src);
err = sco_chan_add(conn, sk, NULL);
if (err)
goto done;
if (hcon->state == BT_CONNECTED) {
sco_sock_clear_timer(sk);
sk->state = BT_CONNECTED;
} else {
sk->state = BT_CONNECT;
sco_sock_set_timer(sk, sk->sndtimeo);
}
done:
hci_dev_unlock_bh(hdev);
hci_dev_put(hdev);
return err;
}
static void sco_conn_ready(struct sco_conn *conn)
{
struct sock *parent, *sk;
BT_DBG("conn %p", conn);
sco_conn_lock(conn);
if ((sk = conn->sk)) {
sco_sock_clear_timer(sk);
bh_lock_sock(sk);
sk->state = BT_CONNECTED;
sk->state_change(sk);
bh_unlock_sock(sk);
} else {
parent = sco_get_sock_listen(conn->src);
if (!parent)
goto done;
bh_lock_sock(parent);
sk = sco_sock_alloc(NULL, BTPROTO_SCO, GFP_ATOMIC);
if (!sk) {
bh_unlock_sock(parent);
goto done;
}
sco_sock_init(sk, parent);
bacpy(&bluez_pi(sk)->src, conn->src);
bacpy(&bluez_pi(sk)->dst, conn->dst);
hci_conn_hold(conn->hcon);
__sco_chan_add(conn, sk, parent);
sk->state = BT_CONNECTED;
/* Wake up parent */
parent->data_ready(parent, 1);
bh_unlock_sock(parent);
}
done:
sco_conn_unlock(conn);
}
static int sco_sock_getname(struct socket *sock, struct sockaddr *addr, int *len, int peer)
{
struct sockaddr_sco *sa = (struct sockaddr_sco *) addr;
struct sock *sk = sock->sk;
BT_DBG("sock %p, sk %p", sock, sk);
addr->sa_family = AF_BLUETOOTH;
*len = sizeof(struct sockaddr_sco);
if (peer)
bacpy(&sa->sco_bdaddr, &bluez_pi(sk)->dst);
else
bacpy(&sa->sco_bdaddr, &bluez_pi(sk)->src);
return 0;
}
static int l2cap_sock_getname(struct socket *sock, struct sockaddr *addr, int *len, int peer)
{
struct sockaddr_l2 *la = (struct sockaddr_l2 *) addr;
struct sock *sk = sock->sk;
BT_DBG("sock %p, sk %p", sock, sk);
addr->sa_family = AF_BLUETOOTH;
*len = sizeof(struct sockaddr_l2);
if (peer)
bacpy(&la->l2_bdaddr, &bluez_pi(sk)->dst);
else
bacpy(&la->l2_bdaddr, &bluez_pi(sk)->src);
la->l2_psm = l2cap_pi(sk)->psm;
return 0;
}
/* -------- Socket interface ---------- */
static struct sock *__l2cap_get_sock_by_addr(__u16 psm, bdaddr_t *src)
{
struct sock *sk;
for (sk = l2cap_sk_list.head; sk; sk = sk->next) {
if (sk->sport == psm && !bacmp(&bluez_pi(sk)->src, src))
break;
}
return sk;
}
static int l2cap_sock_connect(struct socket *sock, struct sockaddr *addr, int alen, int flags)
{
struct sockaddr_l2 *la = (struct sockaddr_l2 *) addr;
struct sock *sk = sock->sk;
int err = 0;
lock_sock(sk);
BT_DBG("sk %p", sk);
if (addr->sa_family != AF_BLUETOOTH || alen < sizeof(struct sockaddr_l2)) {
err = -EINVAL;
goto done;
}
if (sk->type == SOCK_SEQPACKET && !la->l2_psm) {
err = -EINVAL;
goto done;
}
switch(sk->state) {
case BT_CONNECT:
case BT_CONNECT2:
case BT_CONFIG:
/* Already connecting */
goto wait;
case BT_CONNECTED:
/* Already connected */
goto done;
case BT_OPEN:
case BT_BOUND:
/* Can connect */
break;
default:
err = -EBADFD;
goto done;
}
/* Set destination address and psm */
bacpy(&bluez_pi(sk)->dst, &la->l2_bdaddr);
l2cap_pi(sk)->psm = la->l2_psm;
if ((err = l2cap_do_connect(sk)))
goto done;
wait:
err = bluez_sock_wait_state(sk, BT_CONNECTED,
sock_sndtimeo(sk, flags & O_NONBLOCK));
done:
release_sock(sk);
return err;
}
/* Find socket with psm and source bdaddr.
* Returns closest match.
*/
static struct sock *__l2cap_get_sock_by_psm(int state, __u16 psm, bdaddr_t *src)
{
struct sock *sk, *sk1 = NULL;
for (sk = l2cap_sk_list.head; sk; sk = sk->next) {
if (state && sk->state != state)
continue;
if (l2cap_pi(sk)->psm == psm) {
/* Exact match. */
if (!bacmp(&bluez_pi(sk)->src, src))
break;
/* Closest match */
if (!bacmp(&bluez_pi(sk)->src, BDADDR_ANY))
sk1 = sk;
}
}
return sk ? sk : sk1;
}
/* -------- Socket interface ---------- */
static struct sock *__sco_get_sock_by_addr(bdaddr_t *ba)
{
struct sock *sk;
for (sk = sco_sk_list.head; sk; sk = sk->next) {
if (!bacmp(&bluez_pi(sk)->src, ba))
break;
}
return sk;
}
/* ----- Proc fs support ------ */
static int l2cap_sock_dump(char *buf, struct bluez_sock_list *list)
{
struct l2cap_pinfo *pi;
struct sock *sk;
char *ptr = buf;
read_lock_bh(&list->lock);
for (sk = list->head; sk; sk = sk->next) {
pi = l2cap_pi(sk);
ptr += sprintf(ptr, "%s %s %d %d 0x%4.4x 0x%4.4x %d %d 0x%x\n",
batostr(&bluez_pi(sk)->src), batostr(&bluez_pi(sk)->dst),
sk->state, pi->psm, pi->scid, pi->dcid, pi->imtu, pi->omtu,
pi->link_mode);
}
read_unlock_bh(&list->lock);
ptr += sprintf(ptr, "\n");
return ptr - buf;
}
/* ----- Proc fs support ------ */
static int sco_sock_dump(char *buf, struct bluez_sock_list *list)
{
struct sco_pinfo *pi;
struct sock *sk;
char *ptr = buf;
write_lock_bh(&list->lock);
for (sk = list->head; sk; sk = sk->next) {
pi = sco_pi(sk);
ptr += sprintf(ptr, "%s %s %d\n",
batostr(&bluez_pi(sk)->src), batostr(&bluez_pi(sk)->dst),
sk->state);
}
write_unlock_bh(&list->lock);
ptr += sprintf(ptr, "\n");
return ptr - buf;
}
/* Find socket listening on source bdaddr.
* Returns closest match.
*/
static struct sock *sco_get_sock_listen(bdaddr_t *src)
{
struct sock *sk, *sk1 = NULL;
read_lock(&sco_sk_list.lock);
for (sk = sco_sk_list.head; sk; sk = sk->next) {
if (sk->state != BT_LISTEN)
continue;
/* Exact match. */
if (!bacmp(&bluez_pi(sk)->src, src))
break;
/* Closest match */
if (!bacmp(&bluez_pi(sk)->src, BDADDR_ANY))
sk1 = sk;
}
read_unlock(&sco_sk_list.lock);
return sk ? sk : sk1;
}
static void l2cap_chan_ready(struct sock *sk)
{
struct sock *parent = bluez_pi(sk)->parent;
BT_DBG("sk %p, parent %p", sk, parent);
l2cap_pi(sk)->conf_state = 0;
l2cap_sock_clear_timer(sk);
if (!parent) {
/* Outgoing channel.
* Wake up socket sleeping on connect.
*/
sk->state = BT_CONNECTED;
sk->state_change(sk);
} else {
/* Incomming channel.
* Wake up socket sleeping on accept.
*/
parent->data_ready(parent, 0);
}
}
static int sco_sock_bind(struct socket *sock, struct sockaddr *addr, int addr_len)
{
struct sockaddr_sco *sa = (struct sockaddr_sco *) addr;
struct sock *sk = sock->sk;
bdaddr_t *src = &sa->sco_bdaddr;
int err = 0;
BT_DBG("sk %p %s", sk, batostr(&sa->sco_bdaddr));
if (!addr || addr->sa_family != AF_BLUETOOTH)
return -EINVAL;
lock_sock(sk);
if (sk->state != BT_OPEN) {
err = -EBADFD;
goto done;
}
write_lock_bh(&sco_sk_list.lock);
if (bacmp(src, BDADDR_ANY) && __sco_get_sock_by_addr(src)) {
err = -EADDRINUSE;
} else {
/* Save source address */
bacpy(&bluez_pi(sk)->src, &sa->sco_bdaddr);
sk->state = BT_BOUND;
}
write_unlock_bh(&sco_sk_list.lock);
done:
release_sock(sk);
return err;
}
static inline int l2cap_connect_req(struct l2cap_conn *conn, l2cap_cmd_hdr *cmd, __u8 *data)
{
struct l2cap_chan_list *list = &conn->chan_list;
l2cap_conn_req *req = (l2cap_conn_req *) data;
l2cap_conn_rsp rsp;
struct sock *sk, *parent;
int result = 0, status = 0;
__u16 dcid = 0, scid = __le16_to_cpu(req->scid);
__u16 psm = req->psm;
BT_DBG("psm 0x%2.2x scid 0x%4.4x", psm, scid);
/* Check if we have socket listening on psm */
parent = l2cap_get_sock_by_psm(BT_LISTEN, psm, conn->src);
if (!parent) {
result = L2CAP_CR_BAD_PSM;
goto sendresp;
}
result = L2CAP_CR_NO_MEM;
/* Check for backlog size */
if (parent->ack_backlog > parent->max_ack_backlog) {
BT_DBG("backlog full %d", parent->ack_backlog);
goto response;
}
sk = l2cap_sock_alloc(NULL, BTPROTO_L2CAP, GFP_ATOMIC);
if (!sk)
goto response;
write_lock(&list->lock);
/* Check if we already have channel with that dcid */
if (__l2cap_get_chan_by_dcid(list, scid)) {
write_unlock(&list->lock);
sk->zapped = 1;
l2cap_sock_kill(sk);
goto response;
}
hci_conn_hold(conn->hcon);
l2cap_sock_init(sk, parent);
bacpy(&bluez_pi(sk)->src, conn->src);
bacpy(&bluez_pi(sk)->dst, conn->dst);
l2cap_pi(sk)->psm = psm;
l2cap_pi(sk)->dcid = scid;
__l2cap_chan_add(conn, sk, parent);
dcid = l2cap_pi(sk)->scid;
l2cap_sock_set_timer(sk, sk->sndtimeo);
/* Service level security */
result = L2CAP_CR_PEND;
status = L2CAP_CS_AUTHEN_PEND;
sk->state = BT_CONNECT2;
l2cap_pi(sk)->ident = cmd->ident;
if (l2cap_pi(sk)->link_mode & L2CAP_LM_ENCRYPT) {
if (!hci_conn_encrypt(conn->hcon))
goto done;
} else if (l2cap_pi(sk)->link_mode & L2CAP_LM_AUTH) {
if (!hci_conn_auth(conn->hcon))
goto done;
}
sk->state = BT_CONFIG;
result = status = 0;
done:
write_unlock(&list->lock);
response:
bh_unlock_sock(parent);
sendresp:
rsp.scid = __cpu_to_le16(scid);
rsp.dcid = __cpu_to_le16(dcid);
rsp.result = __cpu_to_le16(result);
rsp.status = __cpu_to_le16(status);
l2cap_send_rsp(conn, cmd->ident, L2CAP_CONN_RSP, L2CAP_CONN_RSP_SIZE, &rsp);
return 0;
}
