static inline int l2cap_disconnect_req(struct l2cap_conn *conn, l2cap_cmd_hdr *cmd, __u8 *data)
{
l2cap_disconn_req *req = (l2cap_disconn_req *) data;
l2cap_disconn_rsp rsp;
__u16 dcid, scid;
struct sock *sk;
scid = __le16_to_cpu(req->scid);
dcid = __le16_to_cpu(req->dcid);
BT_DBG("scid 0x%4.4x dcid 0x%4.4x", scid, dcid);
if (!(sk = l2cap_get_chan_by_scid(&conn->chan_list, dcid)))
return 0;
rsp.dcid = __cpu_to_le16(l2cap_pi(sk)->scid);
rsp.scid = __cpu_to_le16(l2cap_pi(sk)->dcid);
l2cap_send_rsp(conn, cmd->ident, L2CAP_DISCONN_RSP, L2CAP_DISCONN_RSP_SIZE, &rsp);
sk->shutdown = SHUTDOWN_MASK;
l2cap_chan_del(sk, ECONNRESET);
bh_unlock_sock(sk);
l2cap_sock_kill(sk);
return 0;
}
static inline int l2cap_config_req(struct l2cap_conn *conn, l2cap_cmd_hdr *cmd, __u8 *data)
{
l2cap_conf_req * req = (l2cap_conf_req *) data;
__u16 dcid, flags;
__u8 rsp[64];
struct sock *sk;
int result;
dcid = __le16_to_cpu(req->dcid);
flags = __le16_to_cpu(req->flags);
BT_DBG("dcid 0x%4.4x flags 0x%2.2x", dcid, flags);
if (!(sk = l2cap_get_chan_by_scid(&conn->chan_list, dcid)))
return -ENOENT;
l2cap_parse_conf_req(sk, req->data, cmd->len - L2CAP_CONF_REQ_SIZE);
if (flags & 0x0001) {
/* Incomplete config. Send empty response. */
l2cap_send_rsp(conn, cmd->ident, L2CAP_CONF_RSP, l2cap_build_conf_rsp(sk, rsp, NULL), rsp);
goto unlock;
}
/* Complete config. */
l2cap_send_rsp(conn, cmd->ident, L2CAP_CONF_RSP, l2cap_build_conf_rsp(sk, rsp, &result), rsp);
if (result)
goto unlock;
/* Output config done */
l2cap_pi(sk)->conf_state |= L2CAP_CONF_OUTPUT_DONE;
if (l2cap_pi(sk)->conf_state & L2CAP_CONF_INPUT_DONE) {
sk->state = BT_CONNECTED;
l2cap_chan_ready(sk);
} else if (!(l2cap_pi(sk)->conf_state & L2CAP_CONF_REQ_SENT)) {
char req[64];
l2cap_send_req(conn, L2CAP_CONF_REQ, l2cap_build_conf_req(sk, req), req);
}
unlock:
bh_unlock_sock(sk);
return 0;
}
static inline int l2cap_information_req(struct l2cap_conn *conn, l2cap_cmd_hdr *cmd, u8 *data)
{
l2cap_info_req *req = (l2cap_info_req *) data;
l2cap_info_rsp rsp;
u16 type;
type = __le16_to_cpu(req->type);
BT_DBG("type 0x%4.4x", type);
rsp.type = __cpu_to_le16(type);
rsp.result = __cpu_to_le16(L2CAP_IR_NOTSUPP);
l2cap_send_rsp(conn, cmd->ident, L2CAP_INFO_RSP, sizeof(rsp), &rsp);
return 0;
}
static int l2cap_auth_cfm(struct hci_conn *hcon, __u8 status)
{
struct l2cap_chan_list *l;
struct l2cap_conn *conn;
l2cap_conn_rsp rsp;
struct sock *sk;
int result;
if (!(conn = hcon->l2cap_data))
return 0;
l = &conn->chan_list;
BT_DBG("conn %p", conn);
read_lock(&l->lock);
for (sk = l->head; sk; sk = l2cap_pi(sk)->next_c) {
bh_lock_sock(sk);
if (sk->state != BT_CONNECT2 ||
(l2cap_pi(sk)->link_mode & L2CAP_LM_ENCRYPT)) {
bh_unlock_sock(sk);
continue;
}
if (!status) {
sk->state = BT_CONFIG;
result = 0;
} else {
sk->state = BT_DISCONN;
l2cap_sock_set_timer(sk, HZ/10);
result = L2CAP_CR_SEC_BLOCK;
}
rsp.scid = __cpu_to_le16(l2cap_pi(sk)->dcid);
rsp.dcid = __cpu_to_le16(l2cap_pi(sk)->scid);
rsp.result = __cpu_to_le16(result);
rsp.status = __cpu_to_le16(0);
l2cap_send_rsp(conn, l2cap_pi(sk)->ident, L2CAP_CONN_RSP,
L2CAP_CONN_RSP_SIZE, &rsp);
bh_unlock_sock(sk);
}
read_unlock(&l->lock);
return 0;
}
static inline void l2cap_sig_channel(struct l2cap_conn *conn, struct sk_buff *skb)
{
__u8 *data = skb->data;
int len = skb->len;
l2cap_cmd_hdr cmd;
int err = 0;
l2cap_raw_recv(conn, skb);
while (len >= L2CAP_CMD_HDR_SIZE) {
memcpy(&cmd, data, L2CAP_CMD_HDR_SIZE);
data += L2CAP_CMD_HDR_SIZE;
len -= L2CAP_CMD_HDR_SIZE;
cmd.len = __le16_to_cpu(cmd.len);
BT_DBG("code 0x%2.2x len %d id 0x%2.2x", cmd.code, cmd.len, cmd.ident);
if (cmd.len > len || !cmd.ident) {
BT_DBG("corrupted command");
break;
}
switch (cmd.code) {
case L2CAP_CONN_REQ:
err = l2cap_connect_req(conn, &cmd, data);
break;
case L2CAP_CONN_RSP:
err = l2cap_connect_rsp(conn, &cmd, data);
break;
case L2CAP_CONF_REQ:
err = l2cap_config_req(conn, &cmd, data);
break;
case L2CAP_CONF_RSP:
err = l2cap_config_rsp(conn, &cmd, data);
break;
case L2CAP_DISCONN_REQ:
err = l2cap_disconnect_req(conn, &cmd, data);
break;
case L2CAP_DISCONN_RSP:
err = l2cap_disconnect_rsp(conn, &cmd, data);
break;
case L2CAP_COMMAND_REJ:
/* FIXME: We should process this */
break;
case L2CAP_ECHO_REQ:
l2cap_send_rsp(conn, cmd.ident, L2CAP_ECHO_RSP, cmd.len, data);
break;
case L2CAP_ECHO_RSP:
case L2CAP_INFO_REQ:
case L2CAP_INFO_RSP:
break;
default:
BT_ERR("Unknown signaling command 0x%2.2x", cmd.code);
err = -EINVAL;
break;
};
if (err) {
l2cap_cmd_rej rej;
BT_DBG("error %d", err);
/* FIXME: Map err to a valid reason. */
rej.reason = __cpu_to_le16(0);
l2cap_send_rsp(conn, cmd.ident, L2CAP_COMMAND_REJ, L2CAP_CMD_REJ_SIZE, &rej);
}
data += cmd.len;
len -= cmd.len;
}
kfree_skb(skb);
}
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;
}