/* Copyright (c) 2010,2011 Code Aurora Forum. All rights reserved. Copyright (c) 2011,2012 Intel Corp. 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 #include #include #include #include /* Global AMP Manager list */ LIST_HEAD(amp_mgr_list); DEFINE_MUTEX(amp_mgr_list_lock); /* A2MP build & send command helper functions */ static struct a2mp_cmd *__a2mp_build(u8 code, u8 ident, u16 len, void *data) { struct a2mp_cmd *cmd; int plen; plen = sizeof(*cmd) + len; cmd = kzalloc(plen, GFP_KERNEL); if (!cmd) return NULL; cmd->code = code; cmd->ident = ident; cmd->len = cpu_to_le16(len); memcpy(cmd->data, data, len); return cmd; } void a2mp_send(struct amp_mgr *mgr, u8 code, u8 ident, u16 len, void *data) { struct l2cap_chan *chan = mgr->a2mp_chan; struct a2mp_cmd *cmd; u16 total_len = len + sizeof(*cmd); struct kvec iv; struct msghdr msg; cmd = __a2mp_build(code, ident, len, data); if (!cmd) return; iv.iov_base = cmd; iv.iov_len = total_len; memset(&msg, 0, sizeof(msg)); msg.msg_iov = (struct iovec *) &iv; msg.msg_iovlen = 1; l2cap_chan_send(chan, &msg, total_len, 0); kfree(cmd); } static u8 __next_ident(struct amp_mgr *mgr) { if (++mgr->ident == 0) mgr->ident = 1; return mgr->ident; } static inline void __a2mp_cl_bredr(struct a2mp_cl *cl) { cl->id = 0; cl->type = 0; cl->status = 1; } /* hci_dev_list shall be locked */ static void __a2mp_add_cl(struct amp_mgr *mgr, struct a2mp_cl *cl, u8 num_ctrl) { int i = 0; struct hci_dev *hdev; __a2mp_cl_bredr(cl); list_for_each_entry(hdev, &hci_dev_list, list) { /* Iterate through AMP controllers */ if (hdev->id == HCI_BREDR_ID) continue; /* Starting from second entry */ if (++i >= num_ctrl) return; cl[i].id = hdev->id; cl[i].type = hdev->amp_type; cl[i].status = hdev->amp_status; } } /* Processing A2MP messages */ static int a2mp_command_rej(struct amp_mgr *mgr, struct sk_buff *skb, struct a2mp_cmd *hdr) { struct a2mp_cmd_rej *rej = (void *) skb->data; if (le16_to_cpu(hdr->len) < sizeof(*rej)) return -EINVAL; BT_DBG("ident %d reason %d", hdr->ident, le16_to_cpu(rej->reason)); skb_pull(skb, sizeof(*rej)); return 0; } static int a2mp_discover_req(struct amp_mgr *mgr, struct sk_buff *skb, struct a2mp_cmd *hdr) { struct a2mp_discov_req *req = (void *) skb->data; u16 len = le16_to_cpu(hdr->len); struct a2mp_discov_rsp *rsp; u16 ext_feat; u8 num_ctrl; if (len < sizeof(*req)) return -EINVAL; skb_pull(skb, sizeof(*req)); ext_feat = le16_to_cpu(req->ext_feat); BT_DBG("mtu %d efm 0x%4.4x", le16_to_cpu(req->mtu), ext_feat); /* check that packet is not broken for now */ while (ext_feat & A2MP_FEAT_EXT) { if (len < sizeof(ext_feat)) return -EINVAL; ext_feat = get_unaligned_le16(skb->data); BT_DBG("efm 0x%4.4x", ext_feat); len -= sizeof(ext_feat); skb_pull(skb, sizeof(ext_feat)); } read_lock(&hci_dev_list_lock); num_ctrl = __hci_num_ctrl(); len = num_ctrl * sizeof(struct a2mp_cl) + sizeof(*rsp); rsp = kmalloc(len, GFP_ATOMIC); if (!rsp) { read_unlock(&hci_dev_list_lock); return -ENOMEM; } rsp->mtu = __constant_cpu_to_le16(L2CAP_A2MP_DEFAULT_MTU); rsp->ext_feat = 0; __a2mp_add_cl(mgr, rsp->cl, num_ctrl); read_unlock(&hci_dev_list_lock); a2mp_send(mgr, A2MP_DISCOVER_RSP, hdr->ident, len, rsp); kfree(rsp); return 0; } static int a2mp_discover_rsp(struct amp_mgr *mgr, struct sk_buff *skb, struct a2mp_cmd *hdr) { struct a2mp_discov_rsp *rsp = (void *) skb->data; u16 len = le16_to_cpu(hdr->len); struct a2mp_cl *cl; u16 ext_feat; if (len < sizeof(*rsp)) return -EINVAL; len -= sizeof(*rsp); skb_pull(skb, sizeof(*rsp)); ext_feat = le16_to_cpu(rsp->ext_feat); BT_DBG("mtu %d efm 0x%4.4x", le16_to_cpu(rsp->mtu), ext_feat); /* check that packet is not broken for now */ while (ext_feat & A2MP_FEAT_EXT) { if (len < sizeof(ext_feat)) return -EINVAL; ext_feat = get_unaligned_le16(skb->data); BT_DBG("efm 0x%4.4x", ext_feat); len -= sizeof(ext_feat); skb_pull(skb, sizeof(ext_feat)); } cl = (void *) skb->data; while (len >= sizeof(*cl)) { BT_DBG("Remote AMP id %d type %d status %d", cl->id, cl->type, cl->status); if (cl->id != HCI_BREDR_ID && cl->type == HCI_AMP) { struct a2mp_info_req req; req.id = cl->id; a2mp_send(mgr, A2MP_GETINFO_REQ, __next_ident(mgr), sizeof(req), &req); } len -= sizeof(*cl); cl = (void *) skb_pull(skb, sizeof(*cl)); } return 0; } static int a2mp_change_notify(struct amp_mgr *mgr, struct sk_buff *skb, struct a2mp_cmd *hdr) { struct a2mp_cl *cl = (void *) skb->data; while (skb->len >= sizeof(*cl)) { BT_DBG("Controller id %d type %d status %d", cl->id, cl->type, cl->status); cl = (struct a2mp_cl *) skb_pull(skb, sizeof(*cl)); } /* TODO send A2MP_CHANGE_RSP */ return 0; } static int a2mp_getinfo_req(struct amp_mgr *mgr, struct sk_buff *skb, struct a2mp_cmd *hdr) { struct a2mp_info_req *req = (void *) skb->data; struct hci_dev *hdev; if (le16_to_cpu(hdr->len) < sizeof(*req)) return -EINVAL; BT_DBG("id %d", req->id); hdev = hci_dev_get(req->id); if (!hdev) { struct a2mp_info_rsp rsp; rsp.id = req->id; rsp.status = A2MP_STATUS_INVALID_CTRL_ID; a2mp_send(mgr, A2MP_GETINFO_RSP, hdr->ident, sizeof(rsp), &rsp); } if (hdev->dev_type != HCI_BREDR) { mgr->state = READ_LOC_AMP_INFO; hci_send_cmd(hdev, HCI_OP_READ_LOCAL_AMP_INFO, 0, NULL); } hci_dev_put(hdev); skb_pull(skb, sizeof(*req)); return 0; } static int a2mp_getampassoc_req(struct amp_mgr *mgr, struct sk_buff *skb, struct a2mp_cmd *hdr) { struct a2mp_amp_assoc_req *req = (void *) skb->data; struct hci_dev *hdev; struct amp_mgr *tmp; if (le16_to_cpu(hdr->len) < sizeof(*req)) return -EINVAL; BT_DBG("id %d", req->id); /* Make sure that other request is not processed */ tmp = amp_mgr_lookup_by_state(READ_LOC_AMP_ASSOC); hdev = hci_dev_get(req->id); if (!hdev || hdev->amp_type == HCI_BREDR || tmp) { struct a2mp_amp_assoc_rsp rsp; rsp.id = req->id; if (tmp) { rsp.status = A2MP_STATUS_COLLISION_OCCURED; amp_mgr_put(tmp); } else { rsp.status = A2MP_STATUS_INVALID_CTRL_ID; } a2mp_send(mgr, A2MP_GETAMPASSOC_RSP, hdr->ident, sizeof(rsp), &rsp); goto done; } amp_read_loc_assoc(hdev, mgr); done: if (hdev) hci_dev_put(hdev); skb_pull(skb, sizeof(*req)); return 0; } static int a2mp_createphyslink_req(struct amp_mgr *mgr, struct sk_buff *skb, struct a2mp_cmd *hdr) { struct a2mp_physlink_req *req = (void *) skb->data; struct a2mp_physlink_rsp rsp; struct hci_dev *hdev; struct hci_conn *hcon; if (le16_to_cpu(hdr->len) < sizeof(*req)) return -EINVAL; BT_DBG("local_id %d, remote_id %d", req->local_id, req->remote_id); rsp.local_id = req->remote_id; rsp.remote_id = req->local_id; hdev = hci_dev_get(req->remote_id); if (!hdev || hdev->amp_type != HCI_AMP) { rsp.status = A2MP_STATUS_INVALID_CTRL_ID; goto send_rsp; } /* TODO process physlink create */ hcon = phylink_add(hdev, mgr, req->local_id); if (hcon) { BT_DBG("hcon %p", hcon); rsp.status = A2MP_STATUS_SUCCESS; } else { rsp.status = A2MP_STATUS_UNABLE_START_LINK_CREATION; } send_rsp: if (hdev) hci_dev_put(hdev); a2mp_send(mgr, A2MP_CREATEPHYSLINK_RSP, hdr->ident, sizeof(rsp), &rsp); skb_pull(skb, le16_to_cpu(hdr->len)); return 0; } static int a2mp_discphyslink_req(struct amp_mgr *mgr, struct sk_buff *skb, struct a2mp_cmd *hdr) { struct a2mp_physlink_req *req = (void *) skb->data; struct a2mp_physlink_rsp rsp; struct hci_dev *hdev; struct hci_conn *hcon; if (le16_to_cpu(hdr->len) < sizeof(*req)) return -EINVAL; BT_DBG("local_id %d remote_id %d", req->local_id, req->remote_id); rsp.local_id = req->remote_id; rsp.remote_id = req->local_id; rsp.status = A2MP_STATUS_SUCCESS; hdev = hci_dev_get(req->remote_id); if (!hdev) { rsp.status = A2MP_STATUS_INVALID_CTRL_ID; goto send_rsp; } hcon = hci_conn_hash_lookup_ba(hdev, AMP_LINK, mgr->l2cap_conn->dst); if (!hcon) { BT_ERR("No phys link exist"); rsp.status = A2MP_STATUS_NO_PHYSICAL_LINK_EXISTS; goto clean; } /* TODO Disconnect Phys Link here */ clean: hci_dev_put(hdev); send_rsp: a2mp_send(mgr, A2MP_DISCONNPHYSLINK_RSP, hdr->ident, sizeof(rsp), &rsp); skb_pull(skb, sizeof(*req)); return 0; } static inline int a2mp_cmd_rsp(struct amp_mgr *mgr, struct sk_buff *skb, struct a2mp_cmd *hdr) { BT_DBG("ident %d code 0x%2.2x", hdr->ident, hdr->code); skb_pull(skb, le16_to_cpu(hdr->len)); return 0; } /* Handle A2MP signalling */ static int a2mp_chan_recv_cb(struct l2cap_chan *chan, struct sk_buff *skb) { struct a2mp_cmd *hdr; struct amp_mgr *mgr = chan->data; int err = 0; amp_mgr_get(mgr); while (skb->len >= sizeof(*hdr)) { u16 len; hdr = (void *) skb->data; len = le16_to_cpu(hdr->len); BT_DBG("code 0x%2.2x id %d len %u", hdr->code, hdr->ident, len); skb_pull(skb, sizeof(*hdr)); if (len > skb->len || !hdr->ident) { err = -EINVAL; break; } mgr->ident = hdr->ident; switch (hdr->code) { case A2MP_COMMAND_REJ: a2mp_command_rej(mgr, skb, hdr); break; case A2MP_DISCOVER_REQ: err = a2mp_discover_req(mgr, skb, hdr); break; case A2MP_CHANGE_NOTIFY: err = a2mp_change_notify(mgr, skb, hdr); break; case A2MP_GETINFO_REQ: err = a2mp_getinfo_req(mgr, skb, hdr); break; case A2MP_GETAMPASSOC_REQ: err = a2mp_getampassoc_req(mgr, skb, hdr); break; case A2MP_CREATEPHYSLINK_REQ: err = a2mp_createphyslink_req(mgr, skb, hdr); break; case A2MP_DISCONNPHYSLINK_REQ: err = a2mp_discphyslink_req(mgr, skb, hdr); break; case A2MP_DISCOVER_RSP: err = a2mp_discover_rsp(mgr, skb, hdr); break; case A2MP_CHANGE_RSP: case A2MP_GETINFO_RSP: case A2MP_GETAMPASSOC_RSP: case A2MP_CREATEPHYSLINK_RSP: case A2MP_DISCONNPHYSLINK_RSP: err = a2mp_cmd_rsp(mgr, skb, hdr); break; default: BT_ERR("Unknown A2MP sig cmd 0x%2.2x", hdr->code); err = -EINVAL; break; } } if (err) { struct a2mp_cmd_rej rej; rej.reason = __constant_cpu_to_le16(0); hdr = (void *) skb->data; BT_DBG("Send A2MP Rej: cmd 0x%2.2x err %d", hdr->code, err); a2mp_send(mgr, A2MP_COMMAND_REJ, hdr->ident, sizeof(rej), &rej); } /* Always free skb and return success error code to prevent from sending L2CAP Disconnect over A2MP channel */ kfree_skb(skb); amp_mgr_put(mgr); return 0; } static void a2mp_chan_close_cb(struct l2cap_chan *chan) { l2cap_chan_put(chan); } static void a2mp_chan_state_change_cb(struct l2cap_chan *chan, int state) { struct amp_mgr *mgr = chan->data; if (!mgr) return; BT_DBG("chan %p state %s", chan, state_to_string(state)); chan->state = state; switch (state) { case BT_CLOSED: if (mgr) amp_mgr_put(mgr); break; } } static struct sk_buff *a2mp_chan_alloc_skb_cb(struct l2cap_chan *chan, unsigned long len, int nb) { return bt_skb_alloc(len, GFP_KERNEL); } static struct l2cap_ops a2mp_chan_ops = { .name = "L2CAP A2MP channel", .recv = a2mp_chan_recv_cb, .close = a2mp_chan_close_cb, .state_change = a2mp_chan_state_change_cb, .alloc_skb = a2mp_chan_alloc_skb_cb, /* Not implemented for A2MP */ .new_connection = l2cap_chan_no_new_connection, .teardown = l2cap_chan_no_teardown, .ready = l2cap_chan_no_ready, }; static struct l2cap_chan *a2mp_chan_open(struct l2cap_conn *conn) { struct l2cap_chan *chan; int err; chan = l2cap_chan_create(); if (!chan) return NULL; BT_DBG("chan %p", chan); chan->chan_type = L2CAP_CHAN_CONN_FIX_A2MP; chan->flush_to = L2CAP_DEFAULT_FLUSH_TO; chan->ops = &a2mp_chan_ops; l2cap_chan_set_defaults(chan); chan->remote_max_tx = chan->max_tx; chan->remote_tx_win = chan->tx_win; chan->retrans_timeout = L2CAP_DEFAULT_RETRANS_TO; chan->monitor_timeout = L2CAP_DEFAULT_MONITOR_TO; skb_queue_head_init(&chan->tx_q); chan->mode = L2CAP_MODE_ERTM; err = l2cap_ertm_init(chan); if (err < 0) { l2cap_chan_del(chan, 0); return NULL; } chan->conf_state = 0; l2cap_chan_add(conn, chan); chan->remote_mps = chan->omtu; chan->mps = chan->omtu; chan->state = BT_CONNECTED; return chan; } /* AMP Manager functions */ void amp_mgr_get(struct amp_mgr *mgr) { BT_DBG("mgr %p orig refcnt %d", mgr, atomic_read(&mgr->kref.refcount)); kref_get(&mgr->kref); } static void amp_mgr_destroy(struct kref *kref) { struct amp_mgr *mgr = container_of(kref, struct amp_mgr, kref); BT_DBG("mgr %p", mgr); mutex_lock(&_mgr_list_lock); list_del(&mgr->list); mutex_unlock(&_mgr_list_lock); amp_ctrl_list_flush(mgr); kfree(mgr); } int amp_mgr_put(struct amp_mgr *mgr) { BT_DBG("mgr %p orig refcnt %d", mgr, atomic_read(&mgr->kref.refcount)); return kref_put(&mgr->kref, &_mgr_destroy); } static struct amp_mgr *amp_mgr_create(struct l2cap_conn *conn) { struct amp_mgr *mgr; struct l2cap_chan *chan; mgr = kzalloc(sizeof(*mgr), GFP_KERNEL); if (!mgr) return NULL; BT_DBG("conn %p mgr %p", conn, mgr); mgr->l2cap_conn = conn; chan = a2mp_chan_open(conn); if (!chan) { kfree(mgr); return NULL; } mgr->a2mp_chan = chan; chan->data = mgr; conn->hcon->amp_mgr = mgr; kref_init(&mgr->kref); /* Remote AMP ctrl list initialization */ INIT_LIST_HEAD(&mgr->amp_ctrls); mutex_init(&mgr->amp_ctrls_lock); mutex_lock(&_mgr_list_lock); list_add(&mgr->list, &_mgr_list); mutex_unlock(&_mgr_list_lock); return mgr; } struct l2cap_chan *a2mp_channel_create(struct l2cap_conn *conn, struct sk_buff *skb) { struct amp_mgr *mgr; mgr = amp_mgr_create(conn); if (!mgr) { BT_ERR("Could not create AMP manager"); return NULL; } BT_DBG("mgr: %p chan %p", mgr, mgr->a2mp_chan); return mgr->a2mp_chan; } struct amp_mgr *amp_mgr_lookup_by_state(u8 state) { struct amp_mgr *mgr; mutex_lock(&_mgr_list_lock); list_for_each_entry(mgr, &_mgr_list, list) { if (mgr->state == state) { amp_mgr_get(mgr); mutex_unlock(&_mgr_list_lock); return mgr; } } mutex_unlock(&_mgr_list_lock); return NULL; } void a2mp_send_getinfo_rsp(struct hci_dev *hdev) { struct amp_mgr *mgr; struct a2mp_info_rsp rsp; mgr = amp_mgr_lookup_by_state(READ_LOC_AMP_INFO); if (!mgr) return; BT_DBG("%s mgr %p", hdev->name, mgr); rsp.id = hdev->id; rsp.status = A2MP_STATUS_INVALID_CTRL_ID; if (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); } a2mp_send(mgr, A2MP_GETINFO_RSP, mgr->ident, sizeof(rsp), &rsp); amp_mgr_put(mgr); } void a2mp_send_getampassoc_rsp(struct hci_dev *hdev, u8 status) { struct amp_mgr *mgr; struct amp_assoc *loc_assoc = &hdev->loc_assoc; struct a2mp_amp_assoc_rsp *rsp; size_t len; mgr = amp_mgr_lookup_by_state(READ_LOC_AMP_ASSOC); if (!mgr) return; BT_DBG("%s mgr %p", hdev->name, mgr); len = sizeof(struct a2mp_amp_assoc_rsp) + loc_assoc->len; rsp = kzalloc(len, GFP_KERNEL); if (!rsp) { amp_mgr_put(mgr); return; } rsp->id = hdev->id; if (status) { rsp->status = A2MP_STATUS_INVALID_CTRL_ID; } else { rsp->status = A2MP_STATUS_SUCCESS; memcpy(rsp->amp_assoc, loc_assoc->data, loc_assoc->len); } a2mp_send(mgr, A2MP_GETAMPASSOC_RSP, mgr->ident, len, rsp); amp_mgr_put(mgr); kfree(rsp); }