blob: ba638d166b0c7fd4669e8f0f1479f2d3cedd758e [file] [log] [blame]
/*
Copyright (c) 2010-2012 Code Aurora Forum. All rights reserved.
This program is free software; you can redistribute it and/or modify
it under the terms of the GNU General Public License version 2 and
only version 2 as published by the Free Software Foundation.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
*/
#include <linux/interrupt.h>
#include <linux/module.h>
#include <linux/types.h>
#include <linux/errno.h>
#include <linux/kernel.h>
#include <linux/skbuff.h>
#include <linux/list.h>
#include <linux/workqueue.h>
#include <linux/timer.h>
#include <linux/crypto.h>
#include <linux/scatterlist.h>
#include <linux/err.h>
#include <crypto/hash.h>
#include <net/bluetooth/bluetooth.h>
#include <net/bluetooth/hci_core.h>
#include <net/bluetooth/l2cap.h>
#include <net/bluetooth/amp.h>
static struct workqueue_struct *amp_workqueue;
LIST_HEAD(amp_mgr_list);
DEFINE_RWLOCK(amp_mgr_list_lock);
static int send_a2mp(struct socket *sock, u8 *data, int len);
static void ctx_timeout(unsigned long data);
static void launch_ctx(struct amp_mgr *mgr);
static int execute_ctx(struct amp_ctx *ctx, u8 evt_type, void *data);
static int kill_ctx(struct amp_ctx *ctx);
static int cancel_ctx(struct amp_ctx *ctx);
static struct socket *open_fixed_channel(bdaddr_t *src, bdaddr_t *dst);
static void remove_amp_mgr(struct amp_mgr *mgr)
{
BT_DBG("mgr %p", mgr);
write_lock(&amp_mgr_list_lock);
list_del(&mgr->list);
write_unlock(&amp_mgr_list_lock);
read_lock(&mgr->ctx_list_lock);
while (!list_empty(&mgr->ctx_list)) {
struct amp_ctx *ctx;
ctx = list_first_entry(&mgr->ctx_list, struct amp_ctx, list);
read_unlock(&mgr->ctx_list_lock);
BT_DBG("kill ctx %p", ctx);
kill_ctx(ctx);
read_lock(&mgr->ctx_list_lock);
}
read_unlock(&mgr->ctx_list_lock);
kfree(mgr->ctrls);
kfree(mgr);
}
static struct amp_mgr *get_amp_mgr_sk(struct sock *sk)
{
struct amp_mgr *mgr;
struct amp_mgr *found = NULL;
read_lock(&amp_mgr_list_lock);
list_for_each_entry(mgr, &amp_mgr_list, list) {
if ((mgr->a2mp_sock) && (mgr->a2mp_sock->sk == sk)) {
found = mgr;
break;
}
}
read_unlock(&amp_mgr_list_lock);
return found;
}
static struct amp_mgr *get_create_amp_mgr(struct hci_conn *hcon,
struct sk_buff *skb)
{
struct amp_mgr *mgr;
write_lock(&amp_mgr_list_lock);
list_for_each_entry(mgr, &amp_mgr_list, list) {
if (mgr->l2cap_conn == hcon->l2cap_data) {
BT_DBG("found %p", mgr);
write_unlock(&amp_mgr_list_lock);
goto gc_finished;
}
}
write_unlock(&amp_mgr_list_lock);
mgr = kzalloc(sizeof(*mgr), GFP_ATOMIC);
if (!mgr)
return NULL;
mgr->l2cap_conn = hcon->l2cap_data;
mgr->next_ident = 1;
INIT_LIST_HEAD(&mgr->ctx_list);
rwlock_init(&mgr->ctx_list_lock);
mgr->skb = skb;
BT_DBG("hcon %p mgr %p", hcon, mgr);
mgr->a2mp_sock = open_fixed_channel(&hcon->hdev->bdaddr, &hcon->dst);
if (!mgr->a2mp_sock) {
kfree(mgr);
return NULL;
}
write_lock(&amp_mgr_list_lock);
list_add(&(mgr->list), &amp_mgr_list);
write_unlock(&amp_mgr_list_lock);
gc_finished:
return mgr;
}
static struct amp_ctrl *get_ctrl(struct amp_mgr *mgr, u8 remote_id)
{
if ((mgr->ctrls) && (mgr->ctrls->id == remote_id))
return mgr->ctrls;
else
return NULL;
}
static struct amp_ctrl *get_create_ctrl(struct amp_mgr *mgr, u8 id)
{
struct amp_ctrl *ctrl;
BT_DBG("mgr %p, id %d", mgr, id);
if ((mgr->ctrls) && (mgr->ctrls->id == id))
ctrl = mgr->ctrls;
else {
kfree(mgr->ctrls);
ctrl = kzalloc(sizeof(struct amp_ctrl), GFP_ATOMIC);
if (ctrl) {
ctrl->mgr = mgr;
ctrl->id = id;
}
mgr->ctrls = ctrl;
}
return ctrl;
}
static struct amp_ctx *create_ctx(u8 type, u8 state)
{
struct amp_ctx *ctx = NULL;
ctx = kzalloc(sizeof(*ctx), GFP_ATOMIC);
if (ctx) {
ctx->type = type;
ctx->state = state;
init_timer(&(ctx->timer));
ctx->timer.function = ctx_timeout;
ctx->timer.data = (unsigned long) ctx;
}
BT_DBG("ctx %p, type %d", ctx, type);
return ctx;
}
static inline void start_ctx(struct amp_mgr *mgr, struct amp_ctx *ctx)
{
BT_DBG("ctx %p", ctx);
write_lock(&mgr->ctx_list_lock);
list_add(&ctx->list, &mgr->ctx_list);
write_unlock(&mgr->ctx_list_lock);
ctx->mgr = mgr;
execute_ctx(ctx, AMP_INIT, 0);
}
static void destroy_ctx(struct amp_ctx *ctx)
{
struct amp_mgr *mgr = ctx->mgr;
BT_DBG("ctx %p deferred %p", ctx, ctx->deferred);
del_timer(&ctx->timer);
write_lock(&mgr->ctx_list_lock);
list_del(&ctx->list);
write_unlock(&mgr->ctx_list_lock);
if (ctx->deferred)
execute_ctx(ctx->deferred, AMP_INIT, 0);
kfree(ctx);
}
static struct amp_ctx *get_ctx_mgr(struct amp_mgr *mgr, u8 type)
{
struct amp_ctx *fnd = NULL;
struct amp_ctx *ctx;
read_lock(&mgr->ctx_list_lock);
list_for_each_entry(ctx, &mgr->ctx_list, list) {
if (ctx->type == type) {
fnd = ctx;
break;
}
}
read_unlock(&mgr->ctx_list_lock);
return fnd;
}
static struct amp_ctx *get_ctx_type(struct amp_ctx *cur, u8 type)
{
struct amp_mgr *mgr = cur->mgr;
struct amp_ctx *fnd = NULL;
struct amp_ctx *ctx;
read_lock(&mgr->ctx_list_lock);
list_for_each_entry(ctx, &mgr->ctx_list, list) {
if ((ctx->type == type) && (ctx != cur)) {
fnd = ctx;
break;
}
}
read_unlock(&mgr->ctx_list_lock);
return fnd;
}
static struct amp_ctx *get_ctx_a2mp(struct amp_mgr *mgr, u8 ident)
{
struct amp_ctx *fnd = NULL;
struct amp_ctx *ctx;
read_lock(&mgr->ctx_list_lock);
list_for_each_entry(ctx, &mgr->ctx_list, list) {
if ((ctx->evt_type & AMP_A2MP_RSP) &&
(ctx->rsp_ident == ident)) {
fnd = ctx;
break;
}
}
read_unlock(&mgr->ctx_list_lock);
return fnd;
}
static struct amp_ctx *get_ctx_hdev(struct hci_dev *hdev, u8 evt_type,
u16 evt_value)
{
struct amp_mgr *mgr;
struct amp_ctx *fnd = NULL;
read_lock(&amp_mgr_list_lock);
list_for_each_entry(mgr, &amp_mgr_list, list) {
struct amp_ctx *ctx;
read_lock(&mgr->ctx_list_lock);
list_for_each_entry(ctx, &mgr->ctx_list, list) {
struct hci_dev *ctx_hdev;
ctx_hdev = hci_dev_get(ctx->id);
if ((ctx_hdev == hdev) && (ctx->evt_type & evt_type)) {
switch (evt_type) {
case AMP_HCI_CMD_STATUS:
case AMP_HCI_CMD_CMPLT:
if (ctx->opcode == evt_value)
fnd = ctx;
break;
case AMP_HCI_EVENT:
if (ctx->evt_code == (u8) evt_value)
fnd = ctx;
break;
}
}
if (ctx_hdev)
hci_dev_put(ctx_hdev);
if (fnd)
break;
}
read_unlock(&mgr->ctx_list_lock);
}
read_unlock(&amp_mgr_list_lock);
return fnd;
}
static inline u8 next_ident(struct amp_mgr *mgr)
{
if (++mgr->next_ident == 0)
mgr->next_ident = 1;
return mgr->next_ident;
}
static inline void send_a2mp_cmd2(struct amp_mgr *mgr, u8 ident, u8 code,
u16 len, void *data, u16 len2, void *data2)
{
struct a2mp_cmd_hdr *hdr;
int plen;
u8 *p, *cmd;
BT_DBG("ident %d code 0x%02x", ident, code);
if (!mgr->a2mp_sock)
return;
plen = sizeof(*hdr) + len + len2;
cmd = kzalloc(plen, GFP_ATOMIC);
if (!cmd)
return;
hdr = (struct a2mp_cmd_hdr *) cmd;
hdr->code = code;
hdr->ident = ident;
hdr->len = cpu_to_le16(len+len2);
p = cmd + sizeof(*hdr);
memcpy(p, data, len);
p += len;
memcpy(p, data2, len2);
send_a2mp(mgr->a2mp_sock, cmd, plen);
kfree(cmd);
}
static inline void send_a2mp_cmd(struct amp_mgr *mgr, u8 ident,
u8 code, u16 len, void *data)
{
send_a2mp_cmd2(mgr, ident, code, len, data, 0, NULL);
}
static inline int command_rej(struct amp_mgr *mgr, struct sk_buff *skb)
{
struct a2mp_cmd_hdr *hdr = (struct a2mp_cmd_hdr *) skb->data;
struct a2mp_cmd_rej *rej;
struct amp_ctx *ctx;
BT_DBG("ident %d code %d", hdr->ident, hdr->code);
rej = (struct a2mp_cmd_rej *) skb_pull(skb, sizeof(*hdr));
if (skb->len < sizeof(*rej))
return -EINVAL;
BT_DBG("reason %d", le16_to_cpu(rej->reason));
ctx = get_ctx_a2mp(mgr, hdr->ident);
if (ctx)
kill_ctx(ctx);
skb_pull(skb, sizeof(*rej));
return 0;
}
static int send_a2mp_cl(struct amp_mgr *mgr, u8 ident, u8 code, u16 len,
void *msg)
{
struct a2mp_cl clist[16];
struct a2mp_cl *cl;
struct hci_dev *hdev;
int num_ctrls = 1, id;
cl = clist;
cl->id = 0;
cl->type = 0;
cl->status = 1;
for (id = 0; id < 16; ++id) {
hdev = hci_dev_get(id);
if (hdev) {
if ((hdev->amp_type != HCI_BREDR) &&
test_bit(HCI_UP, &hdev->flags)) {
(cl + num_ctrls)->id = hdev->id;
(cl + num_ctrls)->type = hdev->amp_type;
(cl + num_ctrls)->status = hdev->amp_status;
++num_ctrls;
}
hci_dev_put(hdev);
}
}
send_a2mp_cmd2(mgr, ident, code, len, msg,
num_ctrls*sizeof(*cl), clist);
return 0;
}
static void send_a2mp_change_notify(void)
{
struct amp_mgr *mgr;
list_for_each_entry(mgr, &amp_mgr_list, list) {
if (mgr->discovered)
send_a2mp_cl(mgr, next_ident(mgr),
A2MP_CHANGE_NOTIFY, 0, NULL);
}
}
static inline int discover_req(struct amp_mgr *mgr, struct sk_buff *skb)
{
struct a2mp_cmd_hdr *hdr = (struct a2mp_cmd_hdr *) skb->data;
struct a2mp_discover_req *req;
u16 *efm;
struct a2mp_discover_rsp rsp;
req = (struct a2mp_discover_req *) skb_pull(skb, sizeof(*hdr));
if (skb->len < sizeof(*req))
return -EINVAL;
efm = (u16 *) skb_pull(skb, sizeof(*req));
BT_DBG("mtu %d efm 0x%4.4x", le16_to_cpu(req->mtu),
le16_to_cpu(req->ext_feat));
while (le16_to_cpu(req->ext_feat) & 0x8000) {
if (skb->len < sizeof(*efm))
return -EINVAL;
req->ext_feat = *efm;
BT_DBG("efm 0x%4.4x", le16_to_cpu(req->ext_feat));
efm = (u16 *) skb_pull(skb, sizeof(*efm));
}
rsp.mtu = cpu_to_le16(L2CAP_A2MP_DEFAULT_MTU);
rsp.ext_feat = 0;
mgr->discovered = 1;
return send_a2mp_cl(mgr, hdr->ident, A2MP_DISCOVER_RSP,
sizeof(rsp), &rsp);
}
static inline int change_notify(struct amp_mgr *mgr, struct sk_buff *skb)
{
struct a2mp_cmd_hdr *hdr = (struct a2mp_cmd_hdr *) skb->data;
struct a2mp_cl *cl;
cl = (struct a2mp_cl *) skb_pull(skb, sizeof(*hdr));
while (skb->len >= sizeof(*cl)) {
struct amp_ctrl *ctrl;
if (cl->id != 0) {
ctrl = get_create_ctrl(mgr, cl->id);
if (ctrl != NULL) {
ctrl->type = cl->type;
ctrl->status = cl->status;
}
}
cl = (struct a2mp_cl *) skb_pull(skb, sizeof(*cl));
}
/* TODO find controllers in manager that were not on received */
/* controller list and destroy them */
send_a2mp_cmd(mgr, hdr->ident, A2MP_CHANGE_RSP, 0, NULL);
return 0;
}
static inline int getinfo_req(struct amp_mgr *mgr, struct sk_buff *skb)
{
struct a2mp_cmd_hdr *hdr = (struct a2mp_cmd_hdr *) skb->data;
u8 *data;
int id;
struct hci_dev *hdev;
struct a2mp_getinfo_rsp rsp;
data = (u8 *) skb_pull(skb, sizeof(*hdr));
if (le16_to_cpu(hdr->len) < sizeof(*data))
return -EINVAL;
if (skb->len < sizeof(*data))
return -EINVAL;
id = *data;
skb_pull(skb, sizeof(*data));
rsp.id = id;
rsp.status = 1;
BT_DBG("id %d", id);
hdev = hci_dev_get(id);
if (hdev && hdev->amp_type != HCI_BREDR) {
rsp.status = 0;
rsp.total_bw = cpu_to_le32(hdev->amp_total_bw);
rsp.max_bw = cpu_to_le32(hdev->amp_max_bw);
rsp.min_latency = cpu_to_le32(hdev->amp_min_latency);
rsp.pal_cap = cpu_to_le16(hdev->amp_pal_cap);
rsp.assoc_size = cpu_to_le16(hdev->amp_assoc_size);
}
send_a2mp_cmd(mgr, hdr->ident, A2MP_GETINFO_RSP, sizeof(rsp), &rsp);
if (hdev)
hci_dev_put(hdev);
return 0;
}
static void create_physical(struct l2cap_conn *conn, struct sock *sk)
{
struct amp_mgr *mgr;
struct amp_ctx *ctx = NULL;
BT_DBG("conn %p", conn);
mgr = get_create_amp_mgr(conn->hcon, NULL);
if (!mgr)
goto cp_finished;
BT_DBG("mgr %p", mgr);
ctx = create_ctx(AMP_CREATEPHYSLINK, AMP_CPL_INIT);
if (!ctx)
goto cp_finished;
ctx->sk = sk;
sock_hold(sk);
start_ctx(mgr, ctx);
return;
cp_finished:
l2cap_amp_physical_complete(-ENOMEM, 0, 0, sk);
}
static void accept_physical(struct l2cap_conn *lcon, u8 id, struct sock *sk)
{
struct amp_mgr *mgr;
struct hci_dev *hdev;
struct hci_conn *conn;
struct amp_ctx *aplctx = NULL;
u8 remote_id = 0;
int result = -EINVAL;
BT_DBG("lcon %p", lcon);
hdev = hci_dev_get(id);
if (!hdev)
goto ap_finished;
BT_DBG("hdev %p", hdev);
mgr = get_create_amp_mgr(lcon->hcon, NULL);
if (!mgr)
goto ap_finished;
BT_DBG("mgr %p", mgr);
conn = hci_conn_hash_lookup_ba(hdev, ACL_LINK,
&mgr->l2cap_conn->hcon->dst);
if (conn) {
BT_DBG("conn %p", hdev);
result = 0;
remote_id = conn->dst_id;
goto ap_finished;
}
aplctx = get_ctx_mgr(mgr, AMP_ACCEPTPHYSLINK);
if (!aplctx)
goto ap_finished;
aplctx->sk = sk;
sock_hold(sk);
return;
ap_finished:
if (hdev)
hci_dev_put(hdev);
l2cap_amp_physical_complete(result, id, remote_id, sk);
}
static int getampassoc_req(struct amp_mgr *mgr, struct sk_buff *skb)
{
struct a2mp_cmd_hdr *hdr = (struct a2mp_cmd_hdr *) skb->data;
struct amp_ctx *ctx;
struct a2mp_getampassoc_req *req;
if (hdr->len < sizeof(*req))
return -EINVAL;
req = (struct a2mp_getampassoc_req *) skb_pull(skb, sizeof(*hdr));
skb_pull(skb, sizeof(*req));
ctx = create_ctx(AMP_GETAMPASSOC, AMP_GAA_INIT);
if (!ctx)
return -ENOMEM;
ctx->id = req->id;
ctx->d.gaa.req_ident = hdr->ident;
ctx->hdev = hci_dev_get(ctx->id);
if (ctx->hdev)
ctx->d.gaa.assoc = kmalloc(ctx->hdev->amp_assoc_size,
GFP_ATOMIC);
start_ctx(mgr, ctx);
return 0;
}
static u8 getampassoc_handler(struct amp_ctx *ctx, u8 evt_type, void *data)
{
struct sk_buff *skb = (struct sk_buff *) data;
struct hci_cp_read_local_amp_assoc cp;
struct hci_rp_read_local_amp_assoc *rp;
struct a2mp_getampassoc_rsp rsp;
u16 rem_len;
u16 frag_len;
rsp.status = 1;
if ((evt_type == AMP_KILLED) || (!ctx->hdev) || (!ctx->d.gaa.assoc))
goto gaa_finished;
switch (ctx->state) {
case AMP_GAA_INIT:
ctx->state = AMP_GAA_RLAA_COMPLETE;
ctx->evt_type = AMP_HCI_CMD_CMPLT;
ctx->opcode = HCI_OP_READ_LOCAL_AMP_ASSOC;
ctx->d.gaa.len_so_far = 0;
cp.phy_handle = 0;
cp.len_so_far = 0;
cp.max_len = ctx->hdev->amp_assoc_size;
hci_send_cmd(ctx->hdev, ctx->opcode, sizeof(cp), &cp);
break;
case AMP_GAA_RLAA_COMPLETE:
if (skb->len < 4)
goto gaa_finished;
rp = (struct hci_rp_read_local_amp_assoc *) skb->data;
if (rp->status)
goto gaa_finished;
rem_len = le16_to_cpu(rp->rem_len);
skb_pull(skb, 4);
frag_len = skb->len;
if (ctx->d.gaa.len_so_far + rem_len <=
ctx->hdev->amp_assoc_size) {
struct hci_cp_read_local_amp_assoc cp;
u8 *assoc = ctx->d.gaa.assoc + ctx->d.gaa.len_so_far;
memcpy(assoc, rp->frag, frag_len);
ctx->d.gaa.len_so_far += rem_len;
rem_len -= frag_len;
if (rem_len == 0) {
rsp.status = 0;
goto gaa_finished;
}
/* more assoc data to read */
cp.phy_handle = 0;
cp.len_so_far = ctx->d.gaa.len_so_far;
cp.max_len = ctx->hdev->amp_assoc_size;
hci_send_cmd(ctx->hdev, ctx->opcode, sizeof(cp), &cp);
}
break;
default:
goto gaa_finished;
break;
}
return 0;
gaa_finished:
rsp.id = ctx->id;
send_a2mp_cmd2(ctx->mgr, ctx->d.gaa.req_ident, A2MP_GETAMPASSOC_RSP,
sizeof(rsp), &rsp,
ctx->d.gaa.len_so_far, ctx->d.gaa.assoc);
kfree(ctx->d.gaa.assoc);
if (ctx->hdev)
hci_dev_put(ctx->hdev);
return 1;
}
struct hmac_sha256_result {
struct completion completion;
int err;
};
static void hmac_sha256_final(struct crypto_async_request *req, int err)
{
struct hmac_sha256_result *r = req->data;
if (err == -EINPROGRESS)
return;
r->err = err;
complete(&r->completion);
}
int hmac_sha256(u8 *key, u8 ksize, char *plaintext, u8 psize,
u8 *output, u8 outlen)
{
int ret = 0;
struct crypto_ahash *tfm;
struct scatterlist sg;
struct ahash_request *req;
struct hmac_sha256_result tresult;
void *hash_buff = NULL;
unsigned char hash_result[64];
int i;
memset(output, 0, outlen);
init_completion(&tresult.completion);
tfm = crypto_alloc_ahash("hmac(sha256)", CRYPTO_ALG_TYPE_AHASH,
CRYPTO_ALG_TYPE_AHASH_MASK);
if (IS_ERR(tfm)) {
BT_DBG("crypto_alloc_ahash failed");
ret = PTR_ERR(tfm);
goto err_tfm;
}
req = ahash_request_alloc(tfm, GFP_KERNEL);
if (!req) {
BT_DBG("failed to allocate request for hmac(sha256)");
ret = -ENOMEM;
goto err_req;
}
ahash_request_set_callback(req, CRYPTO_TFM_REQ_MAY_BACKLOG,
hmac_sha256_final, &tresult);
hash_buff = kzalloc(psize, GFP_KERNEL);
if (!hash_buff) {
BT_DBG("failed to kzalloc hash_buff");
ret = -ENOMEM;
goto err_hash_buf;
}
memset(hash_result, 0, 64);
memcpy(hash_buff, plaintext, psize);
sg_init_one(&sg, hash_buff, psize);
if (ksize) {
crypto_ahash_clear_flags(tfm, ~0);
ret = crypto_ahash_setkey(tfm, key, ksize);
if (ret) {
BT_DBG("crypto_ahash_setkey failed");
goto err_setkey;
}
}
ahash_request_set_crypt(req, &sg, hash_result, psize);
ret = crypto_ahash_digest(req);
BT_DBG("ret 0x%x", ret);
switch (ret) {
case 0:
for (i = 0; i < outlen; i++)
output[i] = hash_result[i];
break;
case -EINPROGRESS:
case -EBUSY:
ret = wait_for_completion_interruptible(&tresult.completion);
if (!ret && !tresult.err) {
INIT_COMPLETION(tresult.completion);
break;
} else {
BT_DBG("wait_for_completion_interruptible failed");
if (!ret)
ret = tresult.err;
goto out;
}
default:
goto out;
}
out:
err_setkey:
kfree(hash_buff);
err_hash_buf:
ahash_request_free(req);
err_req:
crypto_free_ahash(tfm);
err_tfm:
return ret;
}
static void show_key(u8 *k)
{
int i = 0;
for (i = 0; i < 32; i += 8)
BT_DBG(" %02x %02x %02x %02x %02x %02x %02x %02x",
*(k+i+0), *(k+i+1), *(k+i+2), *(k+i+3),
*(k+i+4), *(k+i+5), *(k+i+6), *(k+i+7));
}
static int physlink_security(struct hci_conn *conn, u8 *data, u8 *len, u8 *type)
{
u8 bt2_key[32];
u8 gamp_key[32];
u8 b802_key[32];
int result;
if (!hci_conn_check_link_mode(conn))
return -EACCES;
BT_DBG("key_type %d", conn->key_type);
if (conn->key_type < 3)
return -EACCES;
*type = conn->key_type;
*len = 32;
memcpy(&bt2_key[0], conn->link_key, 16);
memcpy(&bt2_key[16], conn->link_key, 16);
result = hmac_sha256(bt2_key, 32, "gamp", 4, gamp_key, 32);
if (result)
goto ps_finished;
if (conn->key_type == 3) {
BT_DBG("gamp_key");
show_key(gamp_key);
memcpy(data, gamp_key, 32);
goto ps_finished;
}
result = hmac_sha256(gamp_key, 32, "802b", 4, b802_key, 32);
if (result)
goto ps_finished;
BT_DBG("802b_key");
show_key(b802_key);
memcpy(data, b802_key, 32);
ps_finished:
return result;
}
static u8 amp_next_handle;
static inline u8 physlink_handle(struct hci_dev *hdev)
{
/* TODO amp_next_handle should be part of hci_dev */
if (amp_next_handle == 0)
amp_next_handle = 1;
return amp_next_handle++;
}
/* Start an Accept Physical Link sequence */
static int createphyslink_req(struct amp_mgr *mgr, struct sk_buff *skb)
{
struct a2mp_cmd_hdr *hdr = (struct a2mp_cmd_hdr *) skb->data;
struct amp_ctx *ctx = NULL;
struct a2mp_createphyslink_req *req;
if (hdr->len < sizeof(*req))
return -EINVAL;
req = (struct a2mp_createphyslink_req *) skb_pull(skb, sizeof(*hdr));
skb_pull(skb, sizeof(*req));
BT_DBG("local_id %d, remote_id %d", req->local_id, req->remote_id);
/* initialize the context */
ctx = create_ctx(AMP_ACCEPTPHYSLINK, AMP_APL_INIT);
if (!ctx)
return -ENOMEM;
ctx->d.apl.req_ident = hdr->ident;
ctx->d.apl.remote_id = req->local_id;
ctx->id = req->remote_id;
/* add the supplied remote assoc to the context */
ctx->d.apl.remote_assoc = kmalloc(skb->len, GFP_ATOMIC);
if (ctx->d.apl.remote_assoc)
memcpy(ctx->d.apl.remote_assoc, skb->data, skb->len);
ctx->d.apl.len_so_far = 0;
ctx->d.apl.rem_len = skb->len;
skb_pull(skb, skb->len);
ctx->hdev = hci_dev_get(ctx->id);
start_ctx(mgr, ctx);
return 0;
}
static u8 acceptphyslink_handler(struct amp_ctx *ctx, u8 evt_type, void *data)
{
struct sk_buff *skb = data;
struct hci_cp_accept_phys_link acp;
struct hci_cp_write_remote_amp_assoc wcp;
struct hci_rp_write_remote_amp_assoc *wrp;
struct hci_ev_cmd_status *cs = data;
struct hci_ev_phys_link_complete *ev;
struct a2mp_createphyslink_rsp rsp;
struct amp_ctx *cplctx;
struct amp_ctx *aplctx;
u16 frag_len;
struct hci_conn *conn;
int result;
BT_DBG("state %d", ctx->state);
result = -EINVAL;
rsp.status = 1; /* Invalid Controller ID */
if (!ctx->hdev || !test_bit(HCI_UP, &ctx->hdev->flags))
goto apl_finished;
if (evt_type == AMP_KILLED) {
result = -EAGAIN;
rsp.status = 4; /* Disconnect request received */
goto apl_finished;
}
if (!ctx->d.apl.remote_assoc) {
result = -ENOMEM;
rsp.status = 2; /* Unable to Start */
goto apl_finished;
}
switch (ctx->state) {
case AMP_APL_INIT:
BT_DBG("local_id %d, remote_id %d",
ctx->id, ctx->d.apl.remote_id);
conn = hci_conn_hash_lookup_id(ctx->hdev,
&ctx->mgr->l2cap_conn->hcon->dst,
ctx->d.apl.remote_id);
if (conn) {
result = -EEXIST;
rsp.status = 5; /* Already Exists */
goto apl_finished;
}
aplctx = get_ctx_type(ctx, AMP_ACCEPTPHYSLINK);
if ((aplctx) &&
(aplctx->d.cpl.remote_id == ctx->d.apl.remote_id)) {
BT_DBG("deferred to %p", aplctx);
aplctx->deferred = ctx;
break;
}
cplctx = get_ctx_type(ctx, AMP_CREATEPHYSLINK);
if ((cplctx) &&
(cplctx->d.cpl.remote_id == ctx->d.apl.remote_id)) {
struct hci_conn *bcon = ctx->mgr->l2cap_conn->hcon;
BT_DBG("local %s remote %s",
batostr(&bcon->hdev->bdaddr),
batostr(&bcon->dst));
if ((cplctx->state < AMP_CPL_PL_COMPLETE) ||
(bacmp(&bcon->hdev->bdaddr, &bcon->dst) < 0)) {
BT_DBG("COLLISION LOSER");
cplctx->deferred = ctx;
cancel_ctx(cplctx);
break;
} else {
BT_DBG("COLLISION WINNER");
result = -EISCONN;
rsp.status = 3; /* Collision */
goto apl_finished;
}
}
result = physlink_security(ctx->mgr->l2cap_conn->hcon, acp.data,
&acp.key_len, &acp.type);
if (result) {
BT_DBG("SECURITY");
rsp.status = 6; /* Security Violation */
goto apl_finished;
}
ctx->d.apl.phy_handle = physlink_handle(ctx->hdev);
ctx->state = AMP_APL_APL_STATUS;
ctx->evt_type = AMP_HCI_CMD_STATUS;
ctx->opcode = HCI_OP_ACCEPT_PHYS_LINK;
acp.phy_handle = ctx->d.apl.phy_handle;
hci_send_cmd(ctx->hdev, ctx->opcode, sizeof(acp), &acp);
break;
case AMP_APL_APL_STATUS:
if (cs->status != 0)
goto apl_finished;
/* PAL will accept link, send a2mp response */
rsp.local_id = ctx->id;
rsp.remote_id = ctx->d.apl.remote_id;
rsp.status = 0;
send_a2mp_cmd(ctx->mgr, ctx->d.apl.req_ident,
A2MP_CREATEPHYSLINK_RSP, sizeof(rsp), &rsp);
/* send the first assoc fragment */
wcp.phy_handle = ctx->d.apl.phy_handle;
wcp.len_so_far = cpu_to_le16(ctx->d.apl.len_so_far);
wcp.rem_len = cpu_to_le16(ctx->d.apl.rem_len);
frag_len = min_t(u16, 248, ctx->d.apl.rem_len);
memcpy(wcp.frag, ctx->d.apl.remote_assoc, frag_len);
ctx->state = AMP_APL_WRA_COMPLETE;
ctx->evt_type = AMP_HCI_CMD_CMPLT;
ctx->opcode = HCI_OP_WRITE_REMOTE_AMP_ASSOC;
hci_send_cmd(ctx->hdev, ctx->opcode, 5+frag_len, &wcp);
break;
case AMP_APL_WRA_COMPLETE:
/* received write remote amp assoc command complete event */
wrp = (struct hci_rp_write_remote_amp_assoc *) skb->data;
if (wrp->status != 0)
goto apl_finished;
if (wrp->phy_handle != ctx->d.apl.phy_handle)
goto apl_finished;
/* update progress */
frag_len = min_t(u16, 248, ctx->d.apl.rem_len);
ctx->d.apl.len_so_far += frag_len;
ctx->d.apl.rem_len -= frag_len;
if (ctx->d.apl.rem_len > 0) {
u8 *assoc;
/* another assoc fragment to send */
wcp.phy_handle = ctx->d.apl.phy_handle;
wcp.len_so_far = cpu_to_le16(ctx->d.apl.len_so_far);
wcp.rem_len = cpu_to_le16(ctx->d.apl.rem_len);
frag_len = min_t(u16, 248, ctx->d.apl.rem_len);
assoc = ctx->d.apl.remote_assoc + ctx->d.apl.len_so_far;
memcpy(wcp.frag, assoc, frag_len);
hci_send_cmd(ctx->hdev, ctx->opcode, 5+frag_len, &wcp);
break;
}
/* wait for physical link complete event */
ctx->state = AMP_APL_PL_COMPLETE;
ctx->evt_type = AMP_HCI_EVENT;
ctx->evt_code = HCI_EV_PHYS_LINK_COMPLETE;
break;
case AMP_APL_PL_COMPLETE:
/* physical link complete event received */
if (skb->len < sizeof(*ev))
goto apl_finished;
ev = (struct hci_ev_phys_link_complete *) skb->data;
if (ev->phy_handle != ctx->d.apl.phy_handle)
break;
if (ev->status != 0)
goto apl_finished;
conn = hci_conn_hash_lookup_handle(ctx->hdev, ev->phy_handle);
if (!conn)
goto apl_finished;
result = 0;
BT_DBG("PL_COMPLETE phy_handle %x", ev->phy_handle);
conn->dst_id = ctx->d.apl.remote_id;
bacpy(&conn->dst, &ctx->mgr->l2cap_conn->hcon->dst);
goto apl_finished;
break;
default:
goto apl_finished;
break;
}
return 0;
apl_finished:
if (ctx->sk)
l2cap_amp_physical_complete(result, ctx->id,
ctx->d.apl.remote_id, ctx->sk);
if ((result) && (ctx->state < AMP_APL_PL_COMPLETE)) {
rsp.local_id = ctx->id;
rsp.remote_id = ctx->d.apl.remote_id;
send_a2mp_cmd(ctx->mgr, ctx->d.apl.req_ident,
A2MP_CREATEPHYSLINK_RSP, sizeof(rsp), &rsp);
}
kfree(ctx->d.apl.remote_assoc);
if (ctx->sk)
sock_put(ctx->sk);
if (ctx->hdev)
hci_dev_put(ctx->hdev);
return 1;
}
static void cancel_cpl_ctx(struct amp_ctx *ctx, u8 reason)
{
struct hci_cp_disconn_phys_link dcp;
ctx->state = AMP_CPL_PL_CANCEL;
ctx->evt_type = AMP_HCI_EVENT;
ctx->evt_code = HCI_EV_DISCONN_PHYS_LINK_COMPLETE;
dcp.phy_handle = ctx->d.cpl.phy_handle;
dcp.reason = reason;
hci_send_cmd(ctx->hdev, HCI_OP_DISCONN_PHYS_LINK, sizeof(dcp), &dcp);
}
static u8 createphyslink_handler(struct amp_ctx *ctx, u8 evt_type, void *data)
{
struct amp_ctrl *ctrl;
struct sk_buff *skb = data;
struct a2mp_cmd_hdr *hdr;
struct hci_ev_cmd_status *cs = data;
struct amp_ctx *cplctx;
struct a2mp_discover_req dreq;
struct a2mp_discover_rsp *drsp;
u16 *efm;
struct a2mp_getinfo_req greq;
struct a2mp_getinfo_rsp *grsp;
struct a2mp_cl *cl;
struct a2mp_getampassoc_req areq;
struct a2mp_getampassoc_rsp *arsp;
struct hci_cp_create_phys_link cp;
struct hci_cp_write_remote_amp_assoc wcp;
struct hci_rp_write_remote_amp_assoc *wrp;
struct hci_ev_channel_selected *cev;
struct hci_cp_read_local_amp_assoc rcp;
struct hci_rp_read_local_amp_assoc *rrp;
struct a2mp_createphyslink_req creq;
struct a2mp_createphyslink_rsp *crsp;
struct hci_ev_phys_link_complete *pev;
struct hci_ev_disconn_phys_link_complete *dev;
u8 *assoc, *rassoc, *lassoc;
u16 frag_len;
u16 rem_len;
int result = -EAGAIN;
struct hci_conn *conn;
BT_DBG("state %d", ctx->state);
if (evt_type == AMP_KILLED)
goto cpl_finished;
if (evt_type == AMP_CANCEL) {
if ((ctx->state < AMP_CPL_CPL_STATUS) ||
((ctx->state == AMP_CPL_PL_COMPLETE) &&
!(ctx->evt_type & AMP_HCI_EVENT)))
goto cpl_finished;
cancel_cpl_ctx(ctx, 0x16);
return 0;
}
switch (ctx->state) {
case AMP_CPL_INIT:
cplctx = get_ctx_type(ctx, AMP_CREATEPHYSLINK);
if (cplctx) {
BT_DBG("deferred to %p", cplctx);
cplctx->deferred = ctx;
break;
}
ctx->state = AMP_CPL_DISC_RSP;
ctx->evt_type = AMP_A2MP_RSP;
ctx->rsp_ident = next_ident(ctx->mgr);
dreq.mtu = cpu_to_le16(L2CAP_A2MP_DEFAULT_MTU);
dreq.ext_feat = 0;
send_a2mp_cmd(ctx->mgr, ctx->rsp_ident, A2MP_DISCOVER_REQ,
sizeof(dreq), &dreq);
break;
case AMP_CPL_DISC_RSP:
drsp = (struct a2mp_discover_rsp *) skb_pull(skb, sizeof(*hdr));
if (skb->len < (sizeof(*drsp))) {
result = -EINVAL;
goto cpl_finished;
}
efm = (u16 *) skb_pull(skb, sizeof(*drsp));
BT_DBG("mtu %d efm 0x%4.4x", le16_to_cpu(drsp->mtu),
le16_to_cpu(drsp->ext_feat));
while (le16_to_cpu(drsp->ext_feat) & 0x8000) {
if (skb->len < sizeof(*efm)) {
result = -EINVAL;
goto cpl_finished;
}
drsp->ext_feat = *efm;
BT_DBG("efm 0x%4.4x", le16_to_cpu(drsp->ext_feat));
efm = (u16 *) skb_pull(skb, sizeof(*efm));
}
cl = (struct a2mp_cl *) efm;
/* find the first remote and local controller with the
* same type
*/
greq.id = 0;
result = -ENODEV;
while (skb->len >= sizeof(*cl)) {
if ((cl->id != 0) && (greq.id == 0)) {
struct hci_dev *hdev;
hdev = hci_dev_get_type(cl->type);
if (hdev) {
struct hci_conn *conn;
ctx->hdev = hdev;
ctx->id = hdev->id;
ctx->d.cpl.remote_id = cl->id;
conn = hci_conn_hash_lookup_ba(hdev,
ACL_LINK,
&ctx->mgr->l2cap_conn->hcon->dst);
if (conn) {
BT_DBG("PL_COMPLETE exists %x",
(int) conn->handle);
result = 0;
}
ctrl = get_create_ctrl(ctx->mgr,
cl->id);
if (ctrl) {
ctrl->type = cl->type;
ctrl->status = cl->status;
}
greq.id = cl->id;
}
}
cl = (struct a2mp_cl *) skb_pull(skb, sizeof(*cl));
}
if ((!greq.id) || (!result))
goto cpl_finished;
ctx->state = AMP_CPL_GETINFO_RSP;
ctx->evt_type = AMP_A2MP_RSP;
ctx->rsp_ident = next_ident(ctx->mgr);
send_a2mp_cmd(ctx->mgr, ctx->rsp_ident, A2MP_GETINFO_REQ,
sizeof(greq), &greq);
break;
case AMP_CPL_GETINFO_RSP:
if (skb->len < sizeof(*grsp))
goto cpl_finished;
grsp = (struct a2mp_getinfo_rsp *) skb_pull(skb, sizeof(*hdr));
skb_pull(skb, sizeof(*grsp));
if (grsp->status)
goto cpl_finished;
if (grsp->id != ctx->d.cpl.remote_id)
goto cpl_finished;
ctrl = get_ctrl(ctx->mgr, grsp->id);
if (!ctrl)
goto cpl_finished;
ctrl->status = grsp->status;
ctrl->total_bw = le32_to_cpu(grsp->total_bw);
ctrl->max_bw = le32_to_cpu(grsp->max_bw);
ctrl->min_latency = le32_to_cpu(grsp->min_latency);
ctrl->pal_cap = le16_to_cpu(grsp->pal_cap);
ctrl->max_assoc_size = le16_to_cpu(grsp->assoc_size);
ctx->d.cpl.max_len = ctrl->max_assoc_size;
/* setup up GAA request */
areq.id = ctx->d.cpl.remote_id;
/* advance context state */
ctx->state = AMP_CPL_GAA_RSP;
ctx->evt_type = AMP_A2MP_RSP;
ctx->rsp_ident = next_ident(ctx->mgr);
send_a2mp_cmd(ctx->mgr, ctx->rsp_ident, A2MP_GETAMPASSOC_REQ,
sizeof(areq), &areq);
break;
case AMP_CPL_GAA_RSP:
if (skb->len < sizeof(*arsp))
goto cpl_finished;
hdr = (void *) skb->data;
arsp = (void *) skb_pull(skb, sizeof(*hdr));
if (arsp->status != 0)
goto cpl_finished;
/* store away remote assoc */
assoc = (u8 *) skb_pull(skb, sizeof(*arsp));
ctx->d.cpl.len_so_far = 0;
ctx->d.cpl.rem_len = hdr->len - sizeof(*arsp);
skb_pull(skb, ctx->d.cpl.rem_len);
rassoc = kmalloc(ctx->d.cpl.rem_len, GFP_ATOMIC);
if (!rassoc)
goto cpl_finished;
memcpy(rassoc, assoc, ctx->d.cpl.rem_len);
ctx->d.cpl.remote_assoc = rassoc;
/* set up CPL command */
ctx->d.cpl.phy_handle = physlink_handle(ctx->hdev);
cp.phy_handle = ctx->d.cpl.phy_handle;
if (physlink_security(ctx->mgr->l2cap_conn->hcon, cp.data,
&cp.key_len, &cp.type)) {
result = -EPERM;
goto cpl_finished;
}
/* advance context state */
ctx->state = AMP_CPL_CPL_STATUS;
ctx->evt_type = AMP_HCI_CMD_STATUS;
ctx->opcode = HCI_OP_CREATE_PHYS_LINK;
hci_send_cmd(ctx->hdev, ctx->opcode, sizeof(cp), &cp);
break;
case AMP_CPL_CPL_STATUS:
/* received create physical link command status */
if (cs->status != 0)
goto cpl_finished;
/* send the first assoc fragment */
wcp.phy_handle = ctx->d.cpl.phy_handle;
wcp.len_so_far = ctx->d.cpl.len_so_far;
wcp.rem_len = cpu_to_le16(ctx->d.cpl.rem_len);
frag_len = min_t(u16, 248, ctx->d.cpl.rem_len);
memcpy(wcp.frag, ctx->d.cpl.remote_assoc, frag_len);
ctx->state = AMP_CPL_WRA_COMPLETE;
ctx->evt_type = AMP_HCI_CMD_CMPLT;
ctx->opcode = HCI_OP_WRITE_REMOTE_AMP_ASSOC;
hci_send_cmd(ctx->hdev, ctx->opcode, 5+frag_len, &wcp);
break;
case AMP_CPL_WRA_COMPLETE:
/* received write remote amp assoc command complete event */
if (skb->len < sizeof(*wrp))
goto cpl_finished;
wrp = (struct hci_rp_write_remote_amp_assoc *) skb->data;
if (wrp->status != 0)
goto cpl_finished;
if (wrp->phy_handle != ctx->d.cpl.phy_handle)
goto cpl_finished;
/* update progress */
frag_len = min_t(u16, 248, ctx->d.cpl.rem_len);
ctx->d.cpl.len_so_far += frag_len;
ctx->d.cpl.rem_len -= frag_len;
if (ctx->d.cpl.rem_len > 0) {
/* another assoc fragment to send */
wcp.phy_handle = ctx->d.cpl.phy_handle;
wcp.len_so_far = cpu_to_le16(ctx->d.cpl.len_so_far);
wcp.rem_len = cpu_to_le16(ctx->d.cpl.rem_len);
frag_len = min_t(u16, 248, ctx->d.cpl.rem_len);
memcpy(wcp.frag,
ctx->d.cpl.remote_assoc + ctx->d.cpl.len_so_far,
frag_len);
hci_send_cmd(ctx->hdev, ctx->opcode, 5+frag_len, &wcp);
break;
}
/* now wait for channel selected event */
ctx->state = AMP_CPL_CHANNEL_SELECT;
ctx->evt_type = AMP_HCI_EVENT;
ctx->evt_code = HCI_EV_CHANNEL_SELECTED;
break;
case AMP_CPL_CHANNEL_SELECT:
/* received channel selection event */
if (skb->len < sizeof(*cev))
goto cpl_finished;
cev = (void *) skb->data;
/* TODO - PK This check is valid but Libra PAL returns 0 for handle during
Create Physical Link collision scenario
if (cev->phy_handle != ctx->d.cpl.phy_handle)
goto cpl_finished;
*/
/* request the first local assoc fragment */
rcp.phy_handle = ctx->d.cpl.phy_handle;
rcp.len_so_far = 0;
rcp.max_len = ctx->d.cpl.max_len;
lassoc = kmalloc(ctx->d.cpl.max_len, GFP_ATOMIC);
if (!lassoc)
goto cpl_finished;
ctx->d.cpl.local_assoc = lassoc;
ctx->d.cpl.len_so_far = 0;
ctx->state = AMP_CPL_RLA_COMPLETE;
ctx->evt_type = AMP_HCI_CMD_CMPLT;
ctx->opcode = HCI_OP_READ_LOCAL_AMP_ASSOC;
hci_send_cmd(ctx->hdev, ctx->opcode, sizeof(rcp), &rcp);
break;
case AMP_CPL_RLA_COMPLETE:
/* received read local amp assoc command complete event */
if (skb->len < 4)
goto cpl_finished;
rrp = (struct hci_rp_read_local_amp_assoc *) skb->data;
if (rrp->status)
goto cpl_finished;
if (rrp->phy_handle != ctx->d.cpl.phy_handle)
goto cpl_finished;
rem_len = le16_to_cpu(rrp->rem_len);
skb_pull(skb, 4);
frag_len = skb->len;
if (ctx->d.cpl.len_so_far + rem_len > ctx->d.cpl.max_len)
goto cpl_finished;
/* save this fragment in context */
lassoc = ctx->d.cpl.local_assoc + ctx->d.cpl.len_so_far;
memcpy(lassoc, rrp->frag, frag_len);
ctx->d.cpl.len_so_far += frag_len;
rem_len -= frag_len;
if (rem_len > 0) {
/* request another local assoc fragment */
rcp.phy_handle = ctx->d.cpl.phy_handle;
rcp.len_so_far = ctx->d.cpl.len_so_far;
rcp.max_len = ctx->d.cpl.max_len;
hci_send_cmd(ctx->hdev, ctx->opcode, sizeof(rcp), &rcp);
} else {
creq.local_id = ctx->id;
creq.remote_id = ctx->d.cpl.remote_id;
/* wait for A2MP rsp AND phys link complete event */
ctx->state = AMP_CPL_PL_COMPLETE;
ctx->evt_type = AMP_A2MP_RSP | AMP_HCI_EVENT;
ctx->rsp_ident = next_ident(ctx->mgr);
ctx->evt_code = HCI_EV_PHYS_LINK_COMPLETE;
send_a2mp_cmd2(ctx->mgr, ctx->rsp_ident,
A2MP_CREATEPHYSLINK_REQ, sizeof(creq), &creq,
ctx->d.cpl.len_so_far, ctx->d.cpl.local_assoc);
}
break;
case AMP_CPL_PL_COMPLETE:
if (evt_type == AMP_A2MP_RSP) {
/* create physical link response received */
ctx->evt_type &= ~AMP_A2MP_RSP;
if (skb->len < sizeof(*crsp))
goto cpl_finished;
crsp = (void *) skb_pull(skb, sizeof(*hdr));
if ((crsp->local_id != ctx->d.cpl.remote_id) ||
(crsp->remote_id != ctx->id) ||
(crsp->status != 0)) {
cancel_cpl_ctx(ctx, 0x13);
break;
}
/* notify Qualcomm PAL */
if (ctx->hdev->manufacturer == 0x001d)
hci_send_cmd(ctx->hdev,
hci_opcode_pack(0x3f, 0x00), 0, NULL);
}
if (evt_type == AMP_HCI_EVENT) {
ctx->evt_type &= ~AMP_HCI_EVENT;
/* physical link complete event received */
if (skb->len < sizeof(*pev))
goto cpl_finished;
pev = (void *) skb->data;
if (pev->phy_handle != ctx->d.cpl.phy_handle)
break;
if (pev->status != 0)
goto cpl_finished;
}
if (ctx->evt_type)
break;
conn = hci_conn_hash_lookup_handle(ctx->hdev,
ctx->d.cpl.phy_handle);
if (!conn)
goto cpl_finished;
result = 0;
BT_DBG("PL_COMPLETE phy_handle %x", ctx->d.cpl.phy_handle);
bacpy(&conn->dst, &ctx->mgr->l2cap_conn->hcon->dst);
conn->dst_id = ctx->d.cpl.remote_id;
conn->out = 1;
goto cpl_finished;
break;
case AMP_CPL_PL_CANCEL:
dev = (void *) skb->data;
BT_DBG("PL_COMPLETE cancelled %x", dev->phy_handle);
result = -EISCONN;
goto cpl_finished;
break;
default:
goto cpl_finished;
break;
}
return 0;
cpl_finished:
l2cap_amp_physical_complete(result, ctx->id, ctx->d.cpl.remote_id,
ctx->sk);
if (ctx->sk)
sock_put(ctx->sk);
if (ctx->hdev)
hci_dev_put(ctx->hdev);
kfree(ctx->d.cpl.remote_assoc);
kfree(ctx->d.cpl.local_assoc);
return 1;
}
static int disconnphyslink_req(struct amp_mgr *mgr, struct sk_buff *skb)
{
struct a2mp_cmd_hdr *hdr = (void *) skb->data;
struct a2mp_disconnphyslink_req *req;
struct a2mp_disconnphyslink_rsp rsp;
struct hci_dev *hdev;
struct hci_conn *conn;
struct amp_ctx *aplctx;
BT_DBG("mgr %p skb %p", mgr, skb);
if (hdr->len < sizeof(*req))
return -EINVAL;
req = (void *) skb_pull(skb, sizeof(*hdr));
skb_pull(skb, sizeof(*req));
rsp.local_id = req->remote_id;
rsp.remote_id = req->local_id;
rsp.status = 0;
BT_DBG("local_id %d remote_id %d",
(int) rsp.local_id, (int) rsp.remote_id);
hdev = hci_dev_get(rsp.local_id);
if (!hdev) {
rsp.status = 1; /* Invalid Controller ID */
goto dpl_finished;
}
BT_DBG("hdev %p", hdev);
conn = hci_conn_hash_lookup_ba(hdev, ACL_LINK,
&mgr->l2cap_conn->hcon->dst);
if (!conn) {
aplctx = get_ctx_mgr(mgr, AMP_ACCEPTPHYSLINK);
if (aplctx) {
kill_ctx(aplctx);
rsp.status = 0;
goto dpl_finished;
}
rsp.status = 2; /* No Physical Link exists */
goto dpl_finished;
}
BT_DBG("conn %p", conn);
hci_disconnect(conn, 0x13);
dpl_finished:
send_a2mp_cmd(mgr, hdr->ident,
A2MP_DISCONNPHYSLINK_RSP, sizeof(rsp), &rsp);
if (hdev)
hci_dev_put(hdev);
return 0;
}
static int execute_ctx(struct amp_ctx *ctx, u8 evt_type, void *data)
{
struct amp_mgr *mgr = ctx->mgr;
u8 finished = 0;
if (!mgr->connected)
return 0;
switch (ctx->type) {
case AMP_GETAMPASSOC:
finished = getampassoc_handler(ctx, evt_type, data);
break;
case AMP_CREATEPHYSLINK:
finished = createphyslink_handler(ctx, evt_type, data);
break;
case AMP_ACCEPTPHYSLINK:
finished = acceptphyslink_handler(ctx, evt_type, data);
break;
}
if (!finished)
mod_timer(&(ctx->timer), jiffies +
msecs_to_jiffies(A2MP_RSP_TIMEOUT));
else
destroy_ctx(ctx);
return finished;
}
static int cancel_ctx(struct amp_ctx *ctx)
{
return execute_ctx(ctx, AMP_CANCEL, 0);
}
static int kill_ctx(struct amp_ctx *ctx)
{
return execute_ctx(ctx, AMP_KILLED, 0);
}
static void ctx_timeout_worker(struct work_struct *w)
{
struct amp_work_ctx_timeout *work = (struct amp_work_ctx_timeout *) w;
struct amp_ctx *ctx = work->ctx;
kill_ctx(ctx);
kfree(work);
}
static void ctx_timeout(unsigned long data)
{
struct amp_ctx *ctx = (struct amp_ctx *) data;
struct amp_work_ctx_timeout *work;
BT_DBG("ctx %p", ctx);
work = kmalloc(sizeof(*work), GFP_ATOMIC);
if (work) {
INIT_WORK((struct work_struct *) work, ctx_timeout_worker);
work->ctx = ctx;
if (queue_work(amp_workqueue, (struct work_struct *) work) == 0)
kfree(work);
}
}
static void launch_ctx(struct amp_mgr *mgr)
{
struct amp_ctx *ctx = NULL;
BT_DBG("mgr %p", mgr);
read_lock(&mgr->ctx_list_lock);
if (!list_empty(&mgr->ctx_list))
ctx = list_first_entry(&mgr->ctx_list, struct amp_ctx, list);
read_unlock(&mgr->ctx_list_lock);
BT_DBG("ctx %p", ctx);
if (ctx)
execute_ctx(ctx, AMP_INIT, NULL);
}
static inline int a2mp_rsp(struct amp_mgr *mgr, struct sk_buff *skb)
{
struct amp_ctx *ctx;
struct a2mp_cmd_hdr *hdr = (struct a2mp_cmd_hdr *) skb->data;
u16 hdr_len = le16_to_cpu(hdr->len);
/* find context waiting for A2MP rsp with this rsp's identifier */
BT_DBG("ident %d code %d", hdr->ident, hdr->code);
ctx = get_ctx_a2mp(mgr, hdr->ident);
if (ctx) {
execute_ctx(ctx, AMP_A2MP_RSP, skb);
} else {
BT_DBG("context not found");
skb_pull(skb, sizeof(*hdr));
if (hdr_len > skb->len)
hdr_len = skb->len;
skb_pull(skb, hdr_len);
}
return 0;
}
/* L2CAP-A2MP interface */
static void a2mp_receive(struct sock *sk, struct sk_buff *skb)
{
struct a2mp_cmd_hdr *hdr = (struct a2mp_cmd_hdr *) skb->data;
int len;
int err = 0;
struct amp_mgr *mgr;
mgr = get_amp_mgr_sk(sk);
if (!mgr)
goto a2mp_finished;
len = skb->len;
while (len >= sizeof(*hdr)) {
struct a2mp_cmd_hdr *hdr = (struct a2mp_cmd_hdr *) skb->data;
u16 clen = le16_to_cpu(hdr->len);
BT_DBG("code 0x%02x id %d len %d", hdr->code, hdr->ident, clen);
if (clen > len || !hdr->ident) {
err = -EINVAL;
break;
}
switch (hdr->code) {
case A2MP_COMMAND_REJ:
command_rej(mgr, skb);
break;
case A2MP_DISCOVER_REQ:
err = discover_req(mgr, skb);
break;
case A2MP_CHANGE_NOTIFY:
err = change_notify(mgr, skb);
break;
case A2MP_GETINFO_REQ:
err = getinfo_req(mgr, skb);
break;
case A2MP_GETAMPASSOC_REQ:
err = getampassoc_req(mgr, skb);
break;
case A2MP_CREATEPHYSLINK_REQ:
err = createphyslink_req(mgr, skb);
break;
case A2MP_DISCONNPHYSLINK_REQ:
err = disconnphyslink_req(mgr, skb);
break;
case A2MP_CHANGE_RSP:
case A2MP_DISCOVER_RSP:
case A2MP_GETINFO_RSP:
case A2MP_GETAMPASSOC_RSP:
case A2MP_CREATEPHYSLINK_RSP:
case A2MP_DISCONNPHYSLINK_RSP:
err = a2mp_rsp(mgr, skb);
break;
default:
BT_ERR("Unknown A2MP signaling command 0x%2.2x",
hdr->code);
skb_pull(skb, sizeof(*hdr));
err = -EINVAL;
break;
}
len = skb->len;
}
a2mp_finished:
if (err && mgr) {
struct a2mp_cmd_rej rej;
rej.reason = cpu_to_le16(0);
send_a2mp_cmd(mgr, hdr->ident, A2MP_COMMAND_REJ,
sizeof(rej), &rej);
}
}
/* L2CAP-A2MP interface */
static int send_a2mp(struct socket *sock, u8 *data, int len)
{
struct kvec iv = { data, len };
struct msghdr msg;
memset(&msg, 0, sizeof(msg));
return kernel_sendmsg(sock, &msg, &iv, 1, len);
}
static void data_ready_worker(struct work_struct *w)
{
struct amp_work_data_ready *work = (struct amp_work_data_ready *) w;
struct sock *sk = work->sk;
struct sk_buff *skb;
/* skb_dequeue() is thread-safe */
while ((skb = skb_dequeue(&sk->sk_receive_queue))) {
a2mp_receive(sk, skb);
kfree_skb(skb);
}
sock_put(work->sk);
kfree(work);
}
static void data_ready(struct sock *sk, int bytes)
{
struct amp_work_data_ready *work;
work = kmalloc(sizeof(*work), GFP_ATOMIC);
if (work) {
INIT_WORK((struct work_struct *) work, data_ready_worker);
sock_hold(sk);
work->sk = sk;
work->bytes = bytes;
if (!queue_work(amp_workqueue, (struct work_struct *) work)) {
kfree(work);
sock_put(sk);
}
}
}
static void state_change_worker(struct work_struct *w)
{
struct amp_work_state_change *work = (struct amp_work_state_change *) w;
struct amp_mgr *mgr;
switch (work->sk->sk_state) {
case BT_CONNECTED:
/* socket is up */
BT_DBG("CONNECTED");
mgr = get_amp_mgr_sk(work->sk);
if (mgr) {
mgr->connected = 1;
if (mgr->skb) {
l2cap_recv_deferred_frame(work->sk, mgr->skb);
mgr->skb = NULL;
}
launch_ctx(mgr);
}
break;
case BT_CLOSED:
/* connection is gone */
BT_DBG("CLOSED");
mgr = get_amp_mgr_sk(work->sk);
if (mgr) {
if (!sock_flag(work->sk, SOCK_DEAD))
sock_release(mgr->a2mp_sock);
mgr->a2mp_sock = NULL;
remove_amp_mgr(mgr);
}
break;
default:
/* something else happened */
break;
}
sock_put(work->sk);
kfree(work);
}
static void state_change(struct sock *sk)
{
struct amp_work_state_change *work;
work = kmalloc(sizeof(*work), GFP_ATOMIC);
if (work) {
INIT_WORK((struct work_struct *) work, state_change_worker);
sock_hold(sk);
work->sk = sk;
if (!queue_work(amp_workqueue, (struct work_struct *) work)) {
kfree(work);
sock_put(sk);
}
}
}
static struct socket *open_fixed_channel(bdaddr_t *src, bdaddr_t *dst)
{
int err;
struct socket *sock;
struct sockaddr_l2 addr;
struct sock *sk;
struct l2cap_options opts = {L2CAP_A2MP_DEFAULT_MTU,
L2CAP_A2MP_DEFAULT_MTU, L2CAP_DEFAULT_FLUSH_TO,
L2CAP_MODE_ERTM, 1, 0xFF, 1};
err = sock_create_kern(PF_BLUETOOTH, SOCK_SEQPACKET,
BTPROTO_L2CAP, &sock);
if (err) {
BT_ERR("sock_create_kern failed %d", err);
return NULL;
}
sk = sock->sk;
sk->sk_data_ready = data_ready;
sk->sk_state_change = state_change;
memset(&addr, 0, sizeof(addr));
bacpy(&addr.l2_bdaddr, src);
addr.l2_family = AF_BLUETOOTH;
addr.l2_cid = L2CAP_CID_A2MP;
err = kernel_bind(sock, (struct sockaddr *) &addr, sizeof(addr));
if (err) {
BT_ERR("kernel_bind failed %d", err);
sock_release(sock);
return NULL;
}
l2cap_fixed_channel_config(sk, &opts);
memset(&addr, 0, sizeof(addr));
bacpy(&addr.l2_bdaddr, dst);
addr.l2_family = AF_BLUETOOTH;
addr.l2_cid = L2CAP_CID_A2MP;
err = kernel_connect(sock, (struct sockaddr *) &addr, sizeof(addr),
O_NONBLOCK);
if ((err == 0) || (err == -EINPROGRESS))
return sock;
else {
BT_ERR("kernel_connect failed %d", err);
sock_release(sock);
return NULL;
}
}
static void conn_ind_worker(struct work_struct *w)
{
struct amp_work_conn_ind *work = (struct amp_work_conn_ind *) w;
struct hci_conn *hcon = work->hcon;
struct sk_buff *skb = work->skb;
struct amp_mgr *mgr;
mgr = get_create_amp_mgr(hcon, skb);
BT_DBG("mgr %p", mgr);
hci_conn_put(hcon);
kfree(work);
}
static void create_physical_worker(struct work_struct *w)
{
struct amp_work_create_physical *work =
(struct amp_work_create_physical *) w;
create_physical(work->conn, work->sk);
sock_put(work->sk);
kfree(work);
}
static void accept_physical_worker(struct work_struct *w)
{
struct amp_work_accept_physical *work =
(struct amp_work_accept_physical *) w;
accept_physical(work->conn, work->id, work->sk);
sock_put(work->sk);
kfree(work);
}
/* L2CAP Fixed Channel interface */
void amp_conn_ind(struct hci_conn *hcon, struct sk_buff *skb)
{
struct amp_work_conn_ind *work;
BT_DBG("hcon %p, skb %p", hcon, skb);
work = kmalloc(sizeof(*work), GFP_ATOMIC);
if (work) {
INIT_WORK((struct work_struct *) work, conn_ind_worker);
hci_conn_hold(hcon);
work->hcon = hcon;
work->skb = skb;
if (!queue_work(amp_workqueue, (struct work_struct *) work)) {
hci_conn_put(hcon);
kfree(work);
}
}
}
/* L2CAP Physical Link interface */
void amp_create_physical(struct l2cap_conn *conn, struct sock *sk)
{
struct amp_work_create_physical *work;
BT_DBG("conn %p", conn);
work = kmalloc(sizeof(*work), GFP_ATOMIC);
if (work) {
INIT_WORK((struct work_struct *) work, create_physical_worker);
work->conn = conn;
work->sk = sk;
sock_hold(sk);
if (!queue_work(amp_workqueue, (struct work_struct *) work)) {
sock_put(sk);
kfree(work);
}
}
}
void amp_accept_physical(struct l2cap_conn *conn, u8 id, struct sock *sk)
{
struct amp_work_accept_physical *work;
BT_DBG("conn %p", conn);
work = kmalloc(sizeof(*work), GFP_ATOMIC);
if (work) {
INIT_WORK((struct work_struct *) work, accept_physical_worker);
work->conn = conn;
work->sk = sk;
work->id = id;
sock_hold(sk);
if (!queue_work(amp_workqueue, (struct work_struct *) work)) {
sock_put(sk);
kfree(work);
}
}
}
/* HCI interface */
static void amp_cmd_cmplt_worker(struct work_struct *w)
{
struct amp_work_cmd_cmplt *work = (struct amp_work_cmd_cmplt *) w;
struct hci_dev *hdev = work->hdev;
u16 opcode = work->opcode;
struct sk_buff *skb = work->skb;
struct amp_ctx *ctx;
ctx = get_ctx_hdev(hdev, AMP_HCI_CMD_CMPLT, opcode);
if (ctx)
execute_ctx(ctx, AMP_HCI_CMD_CMPLT, skb);
kfree_skb(skb);
kfree(w);
}
static void amp_cmd_cmplt_evt(struct hci_dev *hdev, u16 opcode,
struct sk_buff *skb)
{
struct amp_work_cmd_cmplt *work;
struct sk_buff *skbc;
BT_DBG("hdev %p opcode 0x%x skb %p len %d",
hdev, opcode, skb, skb->len);
skbc = skb_clone(skb, GFP_ATOMIC);
if (!skbc)
return;
work = kmalloc(sizeof(*work), GFP_ATOMIC);
if (work) {
INIT_WORK((struct work_struct *) work, amp_cmd_cmplt_worker);
work->hdev = hdev;
work->opcode = opcode;
work->skb = skbc;
if (queue_work(amp_workqueue, (struct work_struct *) work) == 0)
kfree(work);
}
}
static void amp_cmd_status_worker(struct work_struct *w)
{
struct amp_work_cmd_status *work = (struct amp_work_cmd_status *) w;
struct hci_dev *hdev = work->hdev;
u16 opcode = work->opcode;
u8 status = work->status;
struct amp_ctx *ctx;
ctx = get_ctx_hdev(hdev, AMP_HCI_CMD_STATUS, opcode);
if (ctx)
execute_ctx(ctx, AMP_HCI_CMD_STATUS, &status);
kfree(w);
}
static void amp_cmd_status_evt(struct hci_dev *hdev, u16 opcode, u8 status)
{
struct amp_work_cmd_status *work;
BT_DBG("hdev %p opcode 0x%x status %d", hdev, opcode, status);
work = kmalloc(sizeof(*work), GFP_ATOMIC);
if (work) {
INIT_WORK((struct work_struct *) work, amp_cmd_status_worker);
work->hdev = hdev;
work->opcode = opcode;
work->status = status;
if (queue_work(amp_workqueue, (struct work_struct *) work) == 0)
kfree(work);
}
}
static void amp_event_worker(struct work_struct *w)
{
struct amp_work_event *work = (struct amp_work_event *) w;
struct hci_dev *hdev = work->hdev;
u8 event = work->event;
struct sk_buff *skb = work->skb;
struct amp_ctx *ctx;
if (event == HCI_EV_AMP_STATUS_CHANGE) {
struct hci_ev_amp_status_change *ev;
if (skb->len < sizeof(*ev))
goto amp_event_finished;
ev = (void *) skb->data;
if (ev->status != 0)
goto amp_event_finished;
if (ev->amp_status == hdev->amp_status)
goto amp_event_finished;
hdev->amp_status = ev->amp_status;
send_a2mp_change_notify();
goto amp_event_finished;
}
ctx = get_ctx_hdev(hdev, AMP_HCI_EVENT, (u16) event);
if (ctx)
execute_ctx(ctx, AMP_HCI_EVENT, skb);
amp_event_finished:
kfree_skb(skb);
kfree(w);
}
static void amp_evt(struct hci_dev *hdev, u8 event, struct sk_buff *skb)
{
struct amp_work_event *work;
struct sk_buff *skbc;
BT_DBG("hdev %p event 0x%x skb %p", hdev, event, skb);
skbc = skb_clone(skb, GFP_ATOMIC);
if (!skbc)
return;
work = kmalloc(sizeof(*work), GFP_ATOMIC);
if (work) {
INIT_WORK((struct work_struct *) work, amp_event_worker);
work->hdev = hdev;
work->event = event;
work->skb = skbc;
if (queue_work(amp_workqueue, (struct work_struct *) work) == 0)
kfree(work);
}
}
static void amp_dev_event_worker(struct work_struct *w)
{
send_a2mp_change_notify();
kfree(w);
}
static int amp_dev_event(struct notifier_block *this, unsigned long event,
void *ptr)
{
struct hci_dev *hdev = (struct hci_dev *) ptr;
struct amp_work_event *work;
if (hdev->amp_type == HCI_BREDR)
return NOTIFY_DONE;
switch (event) {
case HCI_DEV_UNREG:
case HCI_DEV_REG:
case HCI_DEV_UP:
case HCI_DEV_DOWN:
BT_DBG("hdev %p event %ld", hdev, event);
work = kmalloc(sizeof(*work), GFP_ATOMIC);
if (work) {
INIT_WORK((struct work_struct *) work,
amp_dev_event_worker);
if (queue_work(amp_workqueue,
(struct work_struct *) work) == 0)
kfree(work);
}
}
return NOTIFY_DONE;
}
/* L2CAP module init continued */
static struct notifier_block amp_notifier = {
.notifier_call = amp_dev_event
};
static struct amp_mgr_cb hci_amp = {
.amp_cmd_complete_event = amp_cmd_cmplt_evt,
.amp_cmd_status_event = amp_cmd_status_evt,
.amp_event = amp_evt
};
int amp_init(void)
{
hci_register_amp(&hci_amp);
hci_register_notifier(&amp_notifier);
amp_next_handle = 1;
amp_workqueue = create_singlethread_workqueue("a2mp");
if (!amp_workqueue)
return -EPERM;
return 0;
}
void amp_exit(void)
{
hci_unregister_amp(&hci_amp);
hci_unregister_notifier(&amp_notifier);
flush_workqueue(amp_workqueue);
destroy_workqueue(amp_workqueue);
}