diff options
Diffstat (limited to 'net')
55 files changed, 31562 insertions, 92 deletions
diff --git a/net/Kconfig b/net/Kconfig index c58ccb1..b74076a 100644 --- a/net/Kconfig +++ b/net/Kconfig @@ -314,6 +314,7 @@ source "net/can/Kconfig" source "net/irda/Kconfig" source "net/bluetooth/Kconfig" source "net/bluetooth_mgmt/Kconfig" +source "net/bluetooth_tizen/Kconfig" source "net/rxrpc/Kconfig" config FIB_RULES diff --git a/net/Makefile b/net/Makefile index 358bcfd..11fee99 100644 --- a/net/Makefile +++ b/net/Makefile @@ -37,8 +37,12 @@ obj-$(CONFIG_IRDA) += irda/ ifeq ($(CONFIG_BT_MGMT),y) obj-$(CONFIG_BT) += bluetooth_mgmt/ else +ifeq ($(CONFIG_BT_TIZEN),y) +obj-$(CONFIG_BT) += bluetooth_tizen/ +else obj-$(CONFIG_BT) += bluetooth/ endif +endif obj-$(CONFIG_SUNRPC) += sunrpc/ obj-$(CONFIG_AF_RXRPC) += rxrpc/ obj-$(CONFIG_ATM) += atm/ diff --git a/net/bluetooth/l2cap_core.c b/net/bluetooth/l2cap_core.c index 50f95cf..b044c38 100644 --- a/net/bluetooth/l2cap_core.c +++ b/net/bluetooth/l2cap_core.c @@ -56,6 +56,24 @@ #include <net/bluetooth/l2cap.h> #include <net/bluetooth/smp.h> +/* BEGIN SLP_Bluetooth :: fix av chopping issue. */ +#ifdef CONFIG_SLP +#define HCI_BROADCOMM_QOS_PATCH +#endif + +#ifdef HCI_BROADCOMM_QOS_PATCH +#define L2CAP_PSM_AVDTP 25 +#define HCI_BROADCOM_QOS_CMD 0xFC57 /* For bcm4329/bcm4330 chipset */ +#define PRIORITY_NORMAL 0x00 /* Broadcom ACL priority for bcm4330 chipset */ +#define PRIORITY_HIGH 0x01 + +struct hci_cp_broadcom_cmd { + __le16 handle; + __u8 priority; /* Only for bcm4330 chipset */ +} __attribute__ ((__packed__)); +#endif +/* END SLP_Bluetooth */ + int disable_ertm; static u32 l2cap_feat_mask = L2CAP_FEAT_FIXED_CHAN; @@ -2552,6 +2570,20 @@ static inline int l2cap_connect_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hd if (test_and_set_bit(CONF_REQ_SENT, &chan->conf_state)) break; +/* BEGIN SLP_Bluetooth :: fix av chopping issue. */ +#ifdef HCI_BROADCOMM_QOS_PATCH + /* To gurantee the A2DP packet*/ + if (chan->psm == L2CAP_PSM_AVDTP) { + struct hci_cp_broadcom_cmd cp; + cp.handle = cpu_to_le16(conn->hcon->handle); + cp.priority = PRIORITY_HIGH; + + hci_send_cmd(conn->hcon->hdev, HCI_BROADCOM_QOS_CMD, + sizeof(cp), &cp); + } +#endif +/* END SLP_Bluetooth */ + l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ, l2cap_build_conf_req(chan, req), req); chan->num_conf_req++; diff --git a/net/bluetooth/sco.c b/net/bluetooth/sco.c index d3d48b5..db9b289 100644 --- a/net/bluetooth/sco.c +++ b/net/bluetooth/sco.c @@ -890,6 +890,11 @@ static void sco_conn_ready(struct sco_conn *conn) bacpy(&bt_sk(sk)->src, conn->src); bacpy(&bt_sk(sk)->dst, conn->dst); + if (!conn->hcon) { + BT_ERR("conn->hcon = NULL"); + /* to do */ + } + hci_conn_hold(conn->hcon); __sco_chan_add(conn, sk, parent); diff --git a/net/bluetooth_mgmt/Kconfig b/net/bluetooth_mgmt/Kconfig index 002b302..c1fc90c 100644 --- a/net/bluetooth_mgmt/Kconfig +++ b/net/bluetooth_mgmt/Kconfig @@ -3,7 +3,7 @@ # menuconfig BT - tristate "Bluetooth subsystem support" + tristate "Bluetooth subsystem support(Android)" depends on NET && !S390 depends on RFKILL || !RFKILL select CRYPTO diff --git a/net/bluetooth_mgmt/af_bluetooth.c b/net/bluetooth_mgmt/af_bluetooth.c index 7c73a10..c3afcd2 100644 --- a/net/bluetooth_mgmt/af_bluetooth.c +++ b/net/bluetooth_mgmt/af_bluetooth.c @@ -224,8 +224,10 @@ void bt_accept_unlink(struct sock *sk) BT_DBG("sk %p state %d", sk, sk->sk_state); list_del_init(&bt_sk(sk)->accept_q); - bt_sk(sk)->parent->sk_ack_backlog--; - bt_sk(sk)->parent = NULL; + if (bt_sk(sk)->parent != NULL) { + bt_sk(sk)->parent->sk_ack_backlog--; + bt_sk(sk)->parent = NULL; + } sock_put(sk); } EXPORT_SYMBOL(bt_accept_unlink); diff --git a/net/bluetooth_mgmt/hci_core.c b/net/bluetooth_mgmt/hci_core.c index a68c91e..d695b16 100644 --- a/net/bluetooth_mgmt/hci_core.c +++ b/net/bluetooth_mgmt/hci_core.c @@ -45,7 +45,6 @@ #include <linux/timer.h> #include <linux/crypto.h> #include <net/sock.h> -#include <linux/gpio.h> #include <asm/system.h> #include <linux/uaccess.h> @@ -219,6 +218,7 @@ static void hci_init_req(struct hci_dev *hdev, unsigned long opt) if (!test_bit(HCI_QUIRK_NO_RESET, &hdev->quirks)) { set_bit(HCI_RESET, &hdev->flags); hci_send_cmd(hdev, HCI_OP_RESET, 0, NULL); + memset(hdev->eir, 0, sizeof(hdev->eir)); } /* Read Local Supported Features */ @@ -773,6 +773,7 @@ static int hci_dev_do_close(struct hci_dev *hdev) set_bit(HCI_INIT, &hdev->flags); __hci_request(hdev, hci_reset_req, 0, msecs_to_jiffies(250)); + memset(hdev->eir, 0, sizeof(hdev->eir)); clear_bit(HCI_INIT, &hdev->flags); } @@ -1417,18 +1418,7 @@ int hci_remove_ltk(struct hci_dev *hdev, bdaddr_t *bdaddr) static void hci_cmd_timer(unsigned long arg) { struct hci_dev *hdev = (void *) arg; -#ifdef CONFIG_MACH_M0 - int rx_pin = gpio_get_value(GPIO_BT_RXD); - int tx_pin = gpio_get_value(GPIO_BT_TXD); - int cts_pin = gpio_get_value(GPIO_BT_CTS); - int rts_pin = gpio_get_value(GPIO_BT_RTS); - - int bt_host_wake_pin = gpio_get_value(GPIO_BT_HOST_WAKE); - int bt_wake_pin = gpio_get_value(GPIO_BT_WAKE); - int bt_en = gpio_get_value(GPIO_BT_EN); - BT_ERR("rx: %d, tx: %d, cts: %d, rts: %d", rx_pin, tx_pin, cts_pin, rts_pin); - BT_ERR("host_wake: %d, bt_wake: %d, en: %d", bt_host_wake_pin, bt_wake_pin, bt_en); -#endif + BT_ERR("%s command tx timeout", hdev->name); atomic_set(&hdev->cmd_cnt, 1); tasklet_schedule(&hdev->cmd_task); @@ -2298,6 +2288,21 @@ void *hci_sent_cmd_data(struct hci_dev *hdev, __u16 opcode) { struct hci_command_hdr *hdr; + /* SS_BLUETOOTH(is80.hwang) 2012.05.16 */ + /*Check null pointer and opcode */ + #if defined(CONFIG_BT_CSR8811) + if (hdev == NULL) { + BT_ERR("hci_sent_cmd_opcode:: hdev=NULL, opcode=0x%x", opcode); + return NULL; + } + + if (hdev->sent_cmd->data == NULL) { + BT_ERR("hci_sent_cmd_opcode:: hdev->sent_cmd->data=NULL opcode=0x%x", opcode); + return NULL; + } + #endif + /* SS_BLUEZ_BT(is80.hwang) End */ + if (!hdev->sent_cmd) return NULL; diff --git a/net/bluetooth_mgmt/hci_event.c b/net/bluetooth_mgmt/hci_event.c index 7dc2018..9a3e6a6 100644 --- a/net/bluetooth_mgmt/hci_event.c +++ b/net/bluetooth_mgmt/hci_event.c @@ -1111,8 +1111,13 @@ static inline void hci_cs_inquiry(struct hci_dev *hdev, __u8 status) hci_req_complete(hdev, HCI_OP_INQUIRY, status); hci_conn_check_pending(hdev); hci_dev_lock(hdev); - if (test_bit(HCI_MGMT, &hdev->dev_flags)) + if (test_bit(HCI_MGMT, &hdev->dev_flags)) { + /* [GGSM/sc47.yun] P120828-6815. Discovery fail issue */ + BT_ERR("Discovery can't be done with other commands"); + hci_discovery_set_state(hdev, DISCOVERY_STOPPING); + mgmt_start_discovery_failed(hdev, status); + } hci_dev_unlock(hdev); return; } @@ -1899,6 +1904,18 @@ static inline void hci_auth_complete_evt(struct hci_dev *hdev, struct sk_buff *s return; } + /* SS_BLUETOOTH(is80.hwang) 2012.05.18 */ + /* for pin code request issue */ +#if defined(CONFIG_BT_CSR8811) + if (ev->status == 0x06 ) { + BT_ERR("Pin or key missing !!!"); + hci_remove_link_key(hdev, &conn->dst); + hci_dev_unlock(hdev); + return ; + } +#endif + /* SS_BLUEZ_BT(is80.hwang) End */ + if (!ev->status) { if (!(conn->ssp_mode > 0 && hdev->ssp_mode > 0) && test_bit(HCI_CONN_REAUTH_PEND, &conn->flags)) { diff --git a/net/bluetooth_mgmt/sco.c b/net/bluetooth_mgmt/sco.c index 20d7cb7..5e0104c 100644 --- a/net/bluetooth_mgmt/sco.c +++ b/net/bluetooth_mgmt/sco.c @@ -905,6 +905,11 @@ static void sco_conn_ready(struct sco_conn *conn) bacpy(&bt_sk(sk)->src, conn->src); bacpy(&bt_sk(sk)->dst, conn->dst); + if (!conn->hcon) { + BT_ERR("conn->hcon = NULL"); + /* to do */ + } + hci_conn_hold(conn->hcon); __sco_chan_add(conn, sk, parent); diff --git a/net/bluetooth_tizen/Kconfig b/net/bluetooth_tizen/Kconfig new file mode 100644 index 0000000..6d17deb --- /dev/null +++ b/net/bluetooth_tizen/Kconfig @@ -0,0 +1,49 @@ +# +# Bluetooth subsystem configuration +# + +menuconfig BT + tristate "Bluetooth subsystem support(Tizen)" + depends on NET && !S390 + depends on RFKILL || !RFKILL + select CRC16 + select CRYPTO + select CRYPTO_BLKCIPHER + select CRYPTO_AES + select CRYPTO_ECB + help + Bluetooth is low-cost, low-power, short-range wireless technology. + It was designed as a replacement for cables and other short-range + technologies like IrDA. Bluetooth operates in personal area range + that typically extends up to 10 meters. More information about + Bluetooth can be found at <http://www.bluetooth.com/>. + + Linux Bluetooth subsystem consist of several layers: + Bluetooth Core + HCI device and connection manager, scheduler + SCO audio links + L2CAP (Logical Link Control and Adaptation Protocol) + SMP (Security Manager Protocol) on LE (Low Energy) links + HCI Device drivers (Interface to the hardware) + RFCOMM Module (RFCOMM Protocol) + BNEP Module (Bluetooth Network Encapsulation Protocol) + CMTP Module (CAPI Message Transport Protocol) + HIDP Module (Human Interface Device Protocol) + + Say Y here to compile Bluetooth support into the kernel or say M to + compile it as module (bluetooth). + + To use Linux Bluetooth subsystem, you will need several user-space + utilities like hciconfig and bluetoothd. These utilities and updates + to Bluetooth kernel modules are provided in the BlueZ packages. For + more information, see <http://www.bluez.org/>. + +source "net/bluetooth_tizen/rfcomm/Kconfig" + +source "net/bluetooth_tizen/bnep/Kconfig" + +source "net/bluetooth_tizen/cmtp/Kconfig" + +source "net/bluetooth_tizen/hidp/Kconfig" + +source "drivers/bluetooth_tizen/Kconfig" diff --git a/net/bluetooth_tizen/Makefile b/net/bluetooth_tizen/Makefile new file mode 100644 index 0000000..2dc5a57 --- /dev/null +++ b/net/bluetooth_tizen/Makefile @@ -0,0 +1,12 @@ +# +# Makefile for the Linux Bluetooth subsystem. +# + +obj-$(CONFIG_BT) += bluetooth.o +obj-$(CONFIG_BT_RFCOMM) += rfcomm/ +obj-$(CONFIG_BT_BNEP) += bnep/ +obj-$(CONFIG_BT_CMTP) += cmtp/ +obj-$(CONFIG_BT_HIDP) += hidp/ + +bluetooth-y := af_bluetooth.o hci_core.o hci_conn.o hci_event.o mgmt.o \ + hci_sock.o hci_sysfs.o l2cap_core.o l2cap_sock.o smp.o sco.o lib.o diff --git a/net/bluetooth_tizen/af_bluetooth.c b/net/bluetooth_tizen/af_bluetooth.c new file mode 100644 index 0000000..6fb68a9 --- /dev/null +++ b/net/bluetooth_tizen/af_bluetooth.c @@ -0,0 +1,616 @@ +/* + BlueZ - Bluetooth protocol stack for Linux + Copyright (C) 2000-2001 Qualcomm Incorporated + + Written 2000,2001 by Maxim Krasnyansky <maxk@qualcomm.com> + + This program is free software; you can redistribute it and/or modify + it under the terms of the GNU General Public License version 2 as + published by the Free Software Foundation; + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS + OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT OF THIRD PARTY RIGHTS. + IN NO EVENT SHALL THE COPYRIGHT HOLDER(S) AND AUTHOR(S) BE LIABLE FOR ANY + CLAIM, OR ANY SPECIAL INDIRECT OR CONSEQUENTIAL DAMAGES, OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + + ALL LIABILITY, INCLUDING LIABILITY FOR INFRINGEMENT OF ANY PATENTS, + COPYRIGHTS, TRADEMARKS OR OTHER RIGHTS, RELATING TO USE OF THIS + SOFTWARE IS DISCLAIMED. +*/ + +/* Bluetooth address family and sockets. */ + +#include <linux/module.h> + +#include <linux/types.h> +#include <linux/list.h> +#include <linux/errno.h> +#include <linux/kernel.h> +#include <linux/sched.h> +#include <linux/skbuff.h> +#include <linux/init.h> +#include <linux/poll.h> +#include <net/sock.h> +#include <asm/ioctls.h> +#include <linux/kmod.h> + +#include <net/bluetooth/bluetooth.h> + +#define VERSION "2.16" + +/* Bluetooth sockets */ +#define BT_MAX_PROTO 8 +static const struct net_proto_family *bt_proto[BT_MAX_PROTO]; +static DEFINE_RWLOCK(bt_proto_lock); + +static struct lock_class_key bt_lock_key[BT_MAX_PROTO]; +static const char *const bt_key_strings[BT_MAX_PROTO] = { + "sk_lock-AF_BLUETOOTH-BTPROTO_L2CAP", + "sk_lock-AF_BLUETOOTH-BTPROTO_HCI", + "sk_lock-AF_BLUETOOTH-BTPROTO_SCO", + "sk_lock-AF_BLUETOOTH-BTPROTO_RFCOMM", + "sk_lock-AF_BLUETOOTH-BTPROTO_BNEP", + "sk_lock-AF_BLUETOOTH-BTPROTO_CMTP", + "sk_lock-AF_BLUETOOTH-BTPROTO_HIDP", + "sk_lock-AF_BLUETOOTH-BTPROTO_AVDTP", +}; + +static struct lock_class_key bt_slock_key[BT_MAX_PROTO]; +static const char *const bt_slock_key_strings[BT_MAX_PROTO] = { + "slock-AF_BLUETOOTH-BTPROTO_L2CAP", + "slock-AF_BLUETOOTH-BTPROTO_HCI", + "slock-AF_BLUETOOTH-BTPROTO_SCO", + "slock-AF_BLUETOOTH-BTPROTO_RFCOMM", + "slock-AF_BLUETOOTH-BTPROTO_BNEP", + "slock-AF_BLUETOOTH-BTPROTO_CMTP", + "slock-AF_BLUETOOTH-BTPROTO_HIDP", + "slock-AF_BLUETOOTH-BTPROTO_AVDTP", +}; + +void bt_sock_reclassify_lock(struct sock *sk, int proto) +{ + BUG_ON(!sk); + BUG_ON(sock_owned_by_user(sk)); + + sock_lock_init_class_and_name(sk, + bt_slock_key_strings[proto], &bt_slock_key[proto], + bt_key_strings[proto], &bt_lock_key[proto]); +} +EXPORT_SYMBOL(bt_sock_reclassify_lock); + +int bt_sock_register(int proto, const struct net_proto_family *ops) +{ + int err = 0; + + if (proto < 0 || proto >= BT_MAX_PROTO) + return -EINVAL; + + write_lock(&bt_proto_lock); + + if (bt_proto[proto]) + err = -EEXIST; + else + bt_proto[proto] = ops; + + write_unlock(&bt_proto_lock); + + return err; +} +EXPORT_SYMBOL(bt_sock_register); + +int bt_sock_unregister(int proto) +{ + int err = 0; + + if (proto < 0 || proto >= BT_MAX_PROTO) + return -EINVAL; + + write_lock(&bt_proto_lock); + + if (!bt_proto[proto]) + err = -ENOENT; + else + bt_proto[proto] = NULL; + + write_unlock(&bt_proto_lock); + + return err; +} +EXPORT_SYMBOL(bt_sock_unregister); + +static int bt_sock_create(struct net *net, struct socket *sock, int proto, + int kern) +{ + int err; + + if (net != &init_net) + return -EAFNOSUPPORT; + + if (proto < 0 || proto >= BT_MAX_PROTO) + return -EINVAL; + + if (!bt_proto[proto]) + request_module("bt-proto-%d", proto); + + err = -EPROTONOSUPPORT; + + read_lock(&bt_proto_lock); + + if (bt_proto[proto] && try_module_get(bt_proto[proto]->owner)) { + err = bt_proto[proto]->create(net, sock, proto, kern); + if (!err) + bt_sock_reclassify_lock(sock->sk, proto); + module_put(bt_proto[proto]->owner); + } + + read_unlock(&bt_proto_lock); + + return err; +} + +void bt_sock_link(struct bt_sock_list *l, struct sock *sk) +{ + write_lock(&l->lock); + sk_add_node(sk, &l->head); + write_unlock(&l->lock); +} +EXPORT_SYMBOL(bt_sock_link); + +void bt_sock_unlink(struct bt_sock_list *l, struct sock *sk) +{ + write_lock(&l->lock); + sk_del_node_init(sk); + write_unlock(&l->lock); +} +EXPORT_SYMBOL(bt_sock_unlink); + +void bt_accept_enqueue(struct sock *parent, struct sock *sk) +{ + BT_DBG("parent %p, sk %p", parent, sk); + + sock_hold(sk); + list_add_tail(&bt_sk(sk)->accept_q, &bt_sk(parent)->accept_q); + bt_sk(sk)->parent = parent; + parent->sk_ack_backlog++; +} +EXPORT_SYMBOL(bt_accept_enqueue); + +void bt_accept_unlink(struct sock *sk) +{ + BT_DBG("sk %p state %d", sk, sk->sk_state); + + list_del_init(&bt_sk(sk)->accept_q); + bt_sk(sk)->parent->sk_ack_backlog--; + bt_sk(sk)->parent = NULL; + sock_put(sk); +} +EXPORT_SYMBOL(bt_accept_unlink); + +struct sock *bt_accept_dequeue(struct sock *parent, struct socket *newsock) +{ + struct list_head *p, *n; + struct sock *sk; + + BT_DBG("parent %p", parent); + + list_for_each_safe(p, n, &bt_sk(parent)->accept_q) { + sk = (struct sock *) list_entry(p, struct bt_sock, accept_q); + + lock_sock(sk); + + /* FIXME: Is this check still needed */ + if (sk->sk_state == BT_CLOSED) { + release_sock(sk); + bt_accept_unlink(sk); + continue; + } + + if (sk->sk_state == BT_CONNECTED || !newsock || + bt_sk(parent)->defer_setup) { + bt_accept_unlink(sk); + if (newsock) + sock_graft(sk, newsock); + + release_sock(sk); + return sk; + } + + release_sock(sk); + } + + return NULL; +} +EXPORT_SYMBOL(bt_accept_dequeue); + +int bt_sock_recvmsg(struct kiocb *iocb, struct socket *sock, + struct msghdr *msg, size_t len, int flags) +{ + int noblock = flags & MSG_DONTWAIT; + struct sock *sk = sock->sk; + struct sk_buff *skb; + size_t copied; + int err; + + BT_DBG("sock %p sk %p len %zu", sock, sk, len); + + if (flags & (MSG_OOB)) + return -EOPNOTSUPP; + + skb = skb_recv_datagram(sk, flags, noblock, &err); + if (!skb) { + if (sk->sk_shutdown & RCV_SHUTDOWN) + return 0; + return err; + } + + msg->msg_namelen = 0; + + copied = skb->len; + if (len < copied) { + msg->msg_flags |= MSG_TRUNC; + copied = len; + } + + skb_reset_transport_header(skb); + err = skb_copy_datagram_iovec(skb, 0, msg->msg_iov, copied); + if (err == 0) + sock_recv_ts_and_drops(msg, sk, skb); + + skb_free_datagram(sk, skb); + + return err ? : copied; +} +EXPORT_SYMBOL(bt_sock_recvmsg); + +static long bt_sock_data_wait(struct sock *sk, long timeo) +{ + DECLARE_WAITQUEUE(wait, current); + + add_wait_queue(sk_sleep(sk), &wait); + for (;;) { + set_current_state(TASK_INTERRUPTIBLE); + + if (!skb_queue_empty(&sk->sk_receive_queue)) + break; + + if (sk->sk_err || (sk->sk_shutdown & RCV_SHUTDOWN)) + break; + + if (signal_pending(current) || !timeo) + break; + + set_bit(SOCK_ASYNC_WAITDATA, &sk->sk_socket->flags); + release_sock(sk); + timeo = schedule_timeout(timeo); + lock_sock(sk); + clear_bit(SOCK_ASYNC_WAITDATA, &sk->sk_socket->flags); + } + + __set_current_state(TASK_RUNNING); + remove_wait_queue(sk_sleep(sk), &wait); + return timeo; +} + +int bt_sock_stream_recvmsg(struct kiocb *iocb, struct socket *sock, + struct msghdr *msg, size_t size, int flags) +{ + struct sock *sk = sock->sk; + int err = 0; + size_t target, copied = 0; + long timeo; + + if (flags & MSG_OOB) + return -EOPNOTSUPP; + + msg->msg_namelen = 0; + + BT_DBG("sk %p size %zu", sk, size); + + lock_sock(sk); + + target = sock_rcvlowat(sk, flags & MSG_WAITALL, size); + timeo = sock_rcvtimeo(sk, flags & MSG_DONTWAIT); + + do { + struct sk_buff *skb; + int chunk; + + skb = skb_dequeue(&sk->sk_receive_queue); + if (!skb) { + if (copied >= target) + break; + + err = sock_error(sk); + if (err) + break; + if (sk->sk_shutdown & RCV_SHUTDOWN) + break; + + err = -EAGAIN; + if (!timeo) + break; + + timeo = bt_sock_data_wait(sk, timeo); + + if (signal_pending(current)) { + err = sock_intr_errno(timeo); + goto out; + } + continue; + } + + chunk = min_t(unsigned int, skb->len, size); + if (skb_copy_datagram_iovec(skb, 0, msg->msg_iov, chunk)) { + skb_queue_head(&sk->sk_receive_queue, skb); + if (!copied) + copied = -EFAULT; + break; + } + copied += chunk; + size -= chunk; + + sock_recv_ts_and_drops(msg, sk, skb); + + if (!(flags & MSG_PEEK)) { + int skb_len = skb_headlen(skb); + + if (chunk <= skb_len) { + __skb_pull(skb, chunk); + } else { + struct sk_buff *frag; + + __skb_pull(skb, skb_len); + chunk -= skb_len; + + skb_walk_frags(skb, frag) { + if (chunk <= frag->len) { + /* Pulling partial data */ + skb->len -= chunk; + skb->data_len -= chunk; + __skb_pull(frag, chunk); + break; + } else if (frag->len) { + /* Pulling all frag data */ + chunk -= frag->len; + skb->len -= frag->len; + skb->data_len -= frag->len; + __skb_pull(frag, frag->len); + } + } + } + + if (skb->len) { + skb_queue_head(&sk->sk_receive_queue, skb); + break; + } + kfree_skb(skb); + + } else { + /* put message back and return */ + skb_queue_head(&sk->sk_receive_queue, skb); + break; + } + } while (size); + +out: + release_sock(sk); + return copied ? : err; +} +EXPORT_SYMBOL(bt_sock_stream_recvmsg); + +static inline unsigned int bt_accept_poll(struct sock *parent) +{ + struct list_head *p, *n; + struct sock *sk; + + list_for_each_safe(p, n, &bt_sk(parent)->accept_q) { + sk = (struct sock *) list_entry(p, struct bt_sock, accept_q); + if (sk->sk_state == BT_CONNECTED || + (bt_sk(parent)->defer_setup && + sk->sk_state == BT_CONNECT2)) + return POLLIN | POLLRDNORM; + } + + return 0; +} + +unsigned int bt_sock_poll(struct file *file, struct socket *sock, poll_table *wait) +{ + struct sock *sk = sock->sk; + unsigned int mask = 0; + + BT_DBG("sock %p, sk %p", sock, sk); + + poll_wait(file, sk_sleep(sk), wait); + + if (sk->sk_state == BT_LISTEN) + return bt_accept_poll(sk); + + if (sk->sk_err || !skb_queue_empty(&sk->sk_error_queue)) + mask |= POLLERR; + + if (sk->sk_shutdown & RCV_SHUTDOWN) + mask |= POLLRDHUP | POLLIN | POLLRDNORM; + + if (sk->sk_shutdown == SHUTDOWN_MASK) + mask |= POLLHUP; + + if (!skb_queue_empty(&sk->sk_receive_queue)) + mask |= POLLIN | POLLRDNORM; + + if (sk->sk_state == BT_CLOSED) + mask |= POLLHUP; + + if (sk->sk_state == BT_CONNECT || + sk->sk_state == BT_CONNECT2 || + sk->sk_state == BT_CONFIG) + return mask; + + if (!bt_sk(sk)->suspended && sock_writeable(sk)) + mask |= POLLOUT | POLLWRNORM | POLLWRBAND; + else + set_bit(SOCK_ASYNC_NOSPACE, &sk->sk_socket->flags); + + return mask; +} +EXPORT_SYMBOL(bt_sock_poll); + +int bt_sock_ioctl(struct socket *sock, unsigned int cmd, unsigned long arg) +{ + struct sock *sk = sock->sk; + struct sk_buff *skb; + long amount; + int err; + + BT_DBG("sk %p cmd %x arg %lx", sk, cmd, arg); + + switch (cmd) { + case TIOCOUTQ: + if (sk->sk_state == BT_LISTEN) + return -EINVAL; + + amount = sk->sk_sndbuf - sk_wmem_alloc_get(sk); + if (amount < 0) + amount = 0; + err = put_user(amount, (int __user *) arg); + break; + + case TIOCINQ: + if (sk->sk_state == BT_LISTEN) + return -EINVAL; + + lock_sock(sk); + skb = skb_peek(&sk->sk_receive_queue); + amount = skb ? skb->len : 0; + release_sock(sk); + err = put_user(amount, (int __user *) arg); + break; + + case SIOCGSTAMP: + err = sock_get_timestamp(sk, (struct timeval __user *) arg); + break; + + case SIOCGSTAMPNS: + err = sock_get_timestampns(sk, (struct timespec __user *) arg); + break; + + default: + err = -ENOIOCTLCMD; + break; + } + + return err; +} +EXPORT_SYMBOL(bt_sock_ioctl); + +int bt_sock_wait_state(struct sock *sk, int state, unsigned long timeo) +{ + DECLARE_WAITQUEUE(wait, current); + int err = 0; + + BT_DBG("sk %p", sk); + + add_wait_queue(sk_sleep(sk), &wait); + set_current_state(TASK_INTERRUPTIBLE); + while (sk->sk_state != state) { + if (!timeo) { + err = -EINPROGRESS; + break; + } + + if (signal_pending(current)) { + err = sock_intr_errno(timeo); + break; + } + + release_sock(sk); + timeo = schedule_timeout(timeo); + lock_sock(sk); + set_current_state(TASK_INTERRUPTIBLE); + + err = sock_error(sk); + if (err) + break; + } + __set_current_state(TASK_RUNNING); + remove_wait_queue(sk_sleep(sk), &wait); + return err; +} +EXPORT_SYMBOL(bt_sock_wait_state); + +static struct net_proto_family bt_sock_family_ops = { + .owner = THIS_MODULE, + .family = PF_BLUETOOTH, + .create = bt_sock_create, +}; + +static int __init bt_init(void) +{ + int err; + + BT_INFO("Core ver %s", VERSION); + + err = bt_sysfs_init(); + if (err < 0) + return err; + + err = sock_register(&bt_sock_family_ops); + if (err < 0) { + bt_sysfs_cleanup(); + return err; + } + + BT_INFO("HCI device and connection manager initialized"); + + err = hci_sock_init(); + if (err < 0) + goto error; + + err = l2cap_init(); + if (err < 0) + goto sock_err; + + err = sco_init(); + if (err < 0) { + l2cap_exit(); + goto sock_err; + } + + return 0; + +sock_err: + hci_sock_cleanup(); + +error: + sock_unregister(PF_BLUETOOTH); + bt_sysfs_cleanup(); + + return err; +} + +static void __exit bt_exit(void) +{ + + sco_exit(); + + l2cap_exit(); + + hci_sock_cleanup(); + + sock_unregister(PF_BLUETOOTH); + + bt_sysfs_cleanup(); +} + +subsys_initcall(bt_init); +module_exit(bt_exit); + +MODULE_AUTHOR("Marcel Holtmann <marcel@holtmann.org>"); +MODULE_DESCRIPTION("Bluetooth Core ver " VERSION); +MODULE_VERSION(VERSION); +MODULE_LICENSE("GPL"); +MODULE_ALIAS_NETPROTO(PF_BLUETOOTH); diff --git a/net/bluetooth_tizen/bnep/Kconfig b/net/bluetooth_tizen/bnep/Kconfig new file mode 100644 index 0000000..71791fc --- /dev/null +++ b/net/bluetooth_tizen/bnep/Kconfig @@ -0,0 +1,24 @@ +config BT_BNEP + tristate "BNEP protocol support" + depends on BT + select CRC32 + help + BNEP (Bluetooth Network Encapsulation Protocol) is Ethernet + emulation layer on top of Bluetooth. BNEP is required for + Bluetooth PAN (Personal Area Network). + + Say Y here to compile BNEP support into the kernel or say M to + compile it as module (bnep). + +config BT_BNEP_MC_FILTER + bool "Multicast filter support" + depends on BT_BNEP + help + This option enables the multicast filter support for BNEP. + +config BT_BNEP_PROTO_FILTER + bool "Protocol filter support" + depends on BT_BNEP + help + This option enables the protocol filter support for BNEP. + diff --git a/net/bluetooth_tizen/bnep/Makefile b/net/bluetooth_tizen/bnep/Makefile new file mode 100644 index 0000000..c7821e7 --- /dev/null +++ b/net/bluetooth_tizen/bnep/Makefile @@ -0,0 +1,7 @@ +# +# Makefile for the Linux Bluetooth BNEP layer. +# + +obj-$(CONFIG_BT_BNEP) += bnep.o + +bnep-objs := core.o sock.o netdev.o diff --git a/net/bluetooth_tizen/bnep/bnep.h b/net/bluetooth_tizen/bnep/bnep.h new file mode 100644 index 0000000..e7ee531 --- /dev/null +++ b/net/bluetooth_tizen/bnep/bnep.h @@ -0,0 +1,180 @@ +/* + BNEP protocol definition for Linux Bluetooth stack (BlueZ). + Copyright (C) 2002 Maxim Krasnyansky <maxk@qualcomm.com> + + This program is free software; you can redistribute it and/or modify + it under the terms of the GNU General Public License, 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. + + You should have received a copy of the GNU General Public License + along with this program; if not, write to the Free Software + Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA +*/ + +#ifndef _BNEP_H +#define _BNEP_H + +#include <linux/types.h> +#include <linux/crc32.h> +#include <net/bluetooth/bluetooth.h> + +/* Limits */ +#define BNEP_MAX_PROTO_FILTERS 5 +#define BNEP_MAX_MULTICAST_FILTERS 20 + +/* UUIDs */ +#define BNEP_BASE_UUID 0x0000000000001000800000805F9B34FB +#define BNEP_UUID16 0x02 +#define BNEP_UUID32 0x04 +#define BNEP_UUID128 0x16 + +#define BNEP_SVC_PANU 0x1115 +#define BNEP_SVC_NAP 0x1116 +#define BNEP_SVC_GN 0x1117 + +/* Packet types */ +#define BNEP_GENERAL 0x00 +#define BNEP_CONTROL 0x01 +#define BNEP_COMPRESSED 0x02 +#define BNEP_COMPRESSED_SRC_ONLY 0x03 +#define BNEP_COMPRESSED_DST_ONLY 0x04 + +/* Control types */ +#define BNEP_CMD_NOT_UNDERSTOOD 0x00 +#define BNEP_SETUP_CONN_REQ 0x01 +#define BNEP_SETUP_CONN_RSP 0x02 +#define BNEP_FILTER_NET_TYPE_SET 0x03 +#define BNEP_FILTER_NET_TYPE_RSP 0x04 +#define BNEP_FILTER_MULTI_ADDR_SET 0x05 +#define BNEP_FILTER_MULTI_ADDR_RSP 0x06 + +/* Extension types */ +#define BNEP_EXT_CONTROL 0x00 + +/* Response messages */ +#define BNEP_SUCCESS 0x00 + +#define BNEP_CONN_INVALID_DST 0x01 +#define BNEP_CONN_INVALID_SRC 0x02 +#define BNEP_CONN_INVALID_SVC 0x03 +#define BNEP_CONN_NOT_ALLOWED 0x04 + +#define BNEP_FILTER_UNSUPPORTED_REQ 0x01 +#define BNEP_FILTER_INVALID_RANGE 0x02 +#define BNEP_FILTER_INVALID_MCADDR 0x02 +#define BNEP_FILTER_LIMIT_REACHED 0x03 +#define BNEP_FILTER_DENIED_SECURITY 0x04 + +/* L2CAP settings */ +#define BNEP_MTU 1691 +#define BNEP_PSM 0x0f +#define BNEP_FLUSH_TO 0xffff +#define BNEP_CONNECT_TO 15 +#define BNEP_FILTER_TO 15 + +/* Headers */ +#define BNEP_TYPE_MASK 0x7f +#define BNEP_EXT_HEADER 0x80 + +struct bnep_setup_conn_req { + __u8 type; + __u8 ctrl; + __u8 uuid_size; + __u8 service[0]; +} __packed; + +struct bnep_set_filter_req { + __u8 type; + __u8 ctrl; + __be16 len; + __u8 list[0]; +} __packed; + +struct bnep_control_rsp { + __u8 type; + __u8 ctrl; + __be16 resp; +} __packed; + +struct bnep_ext_hdr { + __u8 type; + __u8 len; + __u8 data[0]; +} __packed; + +/* BNEP ioctl defines */ +#define BNEPCONNADD _IOW('B', 200, int) +#define BNEPCONNDEL _IOW('B', 201, int) +#define BNEPGETCONNLIST _IOR('B', 210, int) +#define BNEPGETCONNINFO _IOR('B', 211, int) + +struct bnep_connadd_req { + int sock; /* Connected socket */ + __u32 flags; + __u16 role; + char device[16]; /* Name of the Ethernet device */ +}; + +struct bnep_conndel_req { + __u32 flags; + __u8 dst[ETH_ALEN]; +}; + +struct bnep_conninfo { + __u32 flags; + __u16 role; + __u16 state; + __u8 dst[ETH_ALEN]; + char device[16]; +}; + +struct bnep_connlist_req { + __u32 cnum; + struct bnep_conninfo __user *ci; +}; + +struct bnep_proto_filter { + __u16 start; + __u16 end; +}; + +int bnep_add_connection(struct bnep_connadd_req *req, struct socket *sock); +int bnep_del_connection(struct bnep_conndel_req *req); +int bnep_get_connlist(struct bnep_connlist_req *req); +int bnep_get_conninfo(struct bnep_conninfo *ci); + +/* BNEP sessions */ +struct bnep_session { + struct list_head list; + + unsigned int role; + unsigned long state; + unsigned long flags; + atomic_t terminate; + struct task_struct *task; + + struct ethhdr eh; + struct msghdr msg; + + struct bnep_proto_filter proto_filter[BNEP_MAX_PROTO_FILTERS]; + unsigned long long mc_filter; + + struct socket *sock; + struct net_device *dev; +}; + +void bnep_net_setup(struct net_device *dev); +int bnep_sock_init(void); +void bnep_sock_cleanup(void); + +static inline int bnep_mc_hash(__u8 *addr) +{ + return crc32_be(~0, addr, ETH_ALEN) >> 26; +} + +#endif diff --git a/net/bluetooth_tizen/bnep/core.c b/net/bluetooth_tizen/bnep/core.c new file mode 100644 index 0000000..a779ec7 --- /dev/null +++ b/net/bluetooth_tizen/bnep/core.c @@ -0,0 +1,747 @@ +/* + BNEP implementation for Linux Bluetooth stack (BlueZ). + Copyright (C) 2001-2002 Inventel Systemes + Written 2001-2002 by + Clément Moreau <clement.moreau@inventel.fr> + David Libault <david.libault@inventel.fr> + + Copyright (C) 2002 Maxim Krasnyansky <maxk@qualcomm.com> + + This program is free software; you can redistribute it and/or modify + it under the terms of the GNU General Public License version 2 as + published by the Free Software Foundation; + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS + OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT OF THIRD PARTY RIGHTS. + IN NO EVENT SHALL THE COPYRIGHT HOLDER(S) AND AUTHOR(S) BE LIABLE FOR ANY + CLAIM, OR ANY SPECIAL INDIRECT OR CONSEQUENTIAL DAMAGES, OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + + ALL LIABILITY, INCLUDING LIABILITY FOR INFRINGEMENT OF ANY PATENTS, + COPYRIGHTS, TRADEMARKS OR OTHER RIGHTS, RELATING TO USE OF THIS + SOFTWARE IS DISCLAIMED. +*/ + +#include <linux/module.h> + +#include <linux/kernel.h> +#include <linux/sched.h> +#include <linux/signal.h> +#include <linux/init.h> +#include <linux/wait.h> +#include <linux/freezer.h> +#include <linux/errno.h> +#include <linux/net.h> +#include <linux/slab.h> +#include <linux/kthread.h> +#include <net/sock.h> + +#include <linux/socket.h> +#include <linux/file.h> + +#include <linux/netdevice.h> +#include <linux/etherdevice.h> +#include <linux/skbuff.h> + +#include <asm/unaligned.h> + +#include <net/bluetooth/bluetooth.h> +#include <net/bluetooth/hci_core.h> +#include <net/bluetooth/l2cap.h> + +#include "bnep.h" + +#define VERSION "1.3" + +static bool compress_src = true; +static bool compress_dst = true; + +static LIST_HEAD(bnep_session_list); +static DECLARE_RWSEM(bnep_session_sem); + +static struct bnep_session *__bnep_get_session(u8 *dst) +{ + struct bnep_session *s; + + BT_DBG(""); + + list_for_each_entry(s, &bnep_session_list, list) + if (!compare_ether_addr(dst, s->eh.h_source)) + return s; + + return NULL; +} + +static void __bnep_link_session(struct bnep_session *s) +{ + list_add(&s->list, &bnep_session_list); +} + +static void __bnep_unlink_session(struct bnep_session *s) +{ + list_del(&s->list); +} + +static int bnep_send(struct bnep_session *s, void *data, size_t len) +{ + struct socket *sock = s->sock; + struct kvec iv = { data, len }; + + return kernel_sendmsg(sock, &s->msg, &iv, 1, len); +} + +static int bnep_send_rsp(struct bnep_session *s, u8 ctrl, u16 resp) +{ + struct bnep_control_rsp rsp; + rsp.type = BNEP_CONTROL; + rsp.ctrl = ctrl; + rsp.resp = htons(resp); + return bnep_send(s, &rsp, sizeof(rsp)); +} + +#ifdef CONFIG_BT_BNEP_PROTO_FILTER +static inline void bnep_set_default_proto_filter(struct bnep_session *s) +{ + /* (IPv4, ARP) */ + s->proto_filter[0].start = ETH_P_IP; + s->proto_filter[0].end = ETH_P_ARP; + /* (RARP, AppleTalk) */ + s->proto_filter[1].start = ETH_P_RARP; + s->proto_filter[1].end = ETH_P_AARP; + /* (IPX, IPv6) */ + s->proto_filter[2].start = ETH_P_IPX; + s->proto_filter[2].end = ETH_P_IPV6; +} +#endif + +static int bnep_ctrl_set_netfilter(struct bnep_session *s, __be16 *data, int len) +{ + int n; + + if (len < 2) + return -EILSEQ; + + n = get_unaligned_be16(data); + data++; + len -= 2; + + if (len < n) + return -EILSEQ; + + BT_DBG("filter len %d", n); + +#ifdef CONFIG_BT_BNEP_PROTO_FILTER + n /= 4; + if (n <= BNEP_MAX_PROTO_FILTERS) { + struct bnep_proto_filter *f = s->proto_filter; + int i; + + for (i = 0; i < n; i++) { + f[i].start = get_unaligned_be16(data++); + f[i].end = get_unaligned_be16(data++); + + BT_DBG("proto filter start %d end %d", + f[i].start, f[i].end); + } + + if (i < BNEP_MAX_PROTO_FILTERS) + memset(f + i, 0, sizeof(*f)); + + if (n == 0) + bnep_set_default_proto_filter(s); + + bnep_send_rsp(s, BNEP_FILTER_NET_TYPE_RSP, BNEP_SUCCESS); + } else { + bnep_send_rsp(s, BNEP_FILTER_NET_TYPE_RSP, BNEP_FILTER_LIMIT_REACHED); + } +#else + bnep_send_rsp(s, BNEP_FILTER_NET_TYPE_RSP, BNEP_FILTER_UNSUPPORTED_REQ); +#endif + return 0; +} + +static int bnep_ctrl_set_mcfilter(struct bnep_session *s, u8 *data, int len) +{ + int n; + + if (len < 2) + return -EILSEQ; + + n = get_unaligned_be16(data); + data += 2; + len -= 2; + + if (len < n) + return -EILSEQ; + + BT_DBG("filter len %d", n); + +#ifdef CONFIG_BT_BNEP_MC_FILTER + n /= (ETH_ALEN * 2); + + if (n > 0) { + int i; + + s->mc_filter = 0; + + /* Always send broadcast */ + set_bit(bnep_mc_hash(s->dev->broadcast), (ulong *) &s->mc_filter); + + /* Add address ranges to the multicast hash */ + for (; n > 0; n--) { + u8 a1[6], *a2; + + memcpy(a1, data, ETH_ALEN); + data += ETH_ALEN; + a2 = data; + data += ETH_ALEN; + + BT_DBG("mc filter %s -> %s", + batostr((void *) a1), batostr((void *) a2)); + + /* Iterate from a1 to a2 */ + set_bit(bnep_mc_hash(a1), (ulong *) &s->mc_filter); + while (memcmp(a1, a2, 6) < 0 && s->mc_filter != ~0LL) { + /* Increment a1 */ + i = 5; + while (i >= 0 && ++a1[i--] == 0) + ; + + set_bit(bnep_mc_hash(a1), (ulong *) &s->mc_filter); + } + } + } + + BT_DBG("mc filter hash 0x%llx", s->mc_filter); + + bnep_send_rsp(s, BNEP_FILTER_MULTI_ADDR_RSP, BNEP_SUCCESS); +#else + bnep_send_rsp(s, BNEP_FILTER_MULTI_ADDR_RSP, BNEP_FILTER_UNSUPPORTED_REQ); +#endif + return 0; +} + +static int bnep_rx_control(struct bnep_session *s, void *data, int len) +{ + u8 cmd = *(u8 *)data; + int err = 0; + + data++; + len--; + + switch (cmd) { + case BNEP_CMD_NOT_UNDERSTOOD: + case BNEP_SETUP_CONN_RSP: + case BNEP_FILTER_NET_TYPE_RSP: + case BNEP_FILTER_MULTI_ADDR_RSP: + /* Ignore these for now */ + break; + + case BNEP_FILTER_NET_TYPE_SET: + err = bnep_ctrl_set_netfilter(s, data, len); + break; + + case BNEP_FILTER_MULTI_ADDR_SET: + err = bnep_ctrl_set_mcfilter(s, data, len); + break; + + case BNEP_SETUP_CONN_REQ: + err = bnep_send_rsp(s, BNEP_SETUP_CONN_RSP, BNEP_CONN_NOT_ALLOWED); + break; + + default: { + u8 pkt[3]; + pkt[0] = BNEP_CONTROL; + pkt[1] = BNEP_CMD_NOT_UNDERSTOOD; + pkt[2] = cmd; + bnep_send(s, pkt, sizeof(pkt)); + } + break; + } + + return err; +} + +static int bnep_rx_extension(struct bnep_session *s, struct sk_buff *skb) +{ + struct bnep_ext_hdr *h; + int err = 0; + + do { + h = (void *) skb->data; + if (!skb_pull(skb, sizeof(*h))) { + err = -EILSEQ; + break; + } + + BT_DBG("type 0x%x len %d", h->type, h->len); + + switch (h->type & BNEP_TYPE_MASK) { + case BNEP_EXT_CONTROL: + bnep_rx_control(s, skb->data, skb->len); + break; + + default: + /* Unknown extension, skip it. */ + break; + } + + if (!skb_pull(skb, h->len)) { + err = -EILSEQ; + break; + } + } while (!err && (h->type & BNEP_EXT_HEADER)); + + return err; +} + +static u8 __bnep_rx_hlen[] = { + ETH_HLEN, /* BNEP_GENERAL */ + 0, /* BNEP_CONTROL */ + 2, /* BNEP_COMPRESSED */ + ETH_ALEN + 2, /* BNEP_COMPRESSED_SRC_ONLY */ + ETH_ALEN + 2 /* BNEP_COMPRESSED_DST_ONLY */ +}; + +static inline int bnep_rx_frame(struct bnep_session *s, struct sk_buff *skb) +{ + struct net_device *dev = s->dev; + struct sk_buff *nskb; + u8 type; + + dev->stats.rx_bytes += skb->len; + + type = *(u8 *) skb->data; + skb_pull(skb, 1); + + if ((type & BNEP_TYPE_MASK) >= sizeof(__bnep_rx_hlen)) + goto badframe; + + if ((type & BNEP_TYPE_MASK) == BNEP_CONTROL) { + bnep_rx_control(s, skb->data, skb->len); + kfree_skb(skb); + return 0; + } + + skb_reset_mac_header(skb); + + /* Verify and pull out header */ + if (!skb_pull(skb, __bnep_rx_hlen[type & BNEP_TYPE_MASK])) + goto badframe; + + s->eh.h_proto = get_unaligned((__be16 *) (skb->data - 2)); + + if (type & BNEP_EXT_HEADER) { + if (bnep_rx_extension(s, skb) < 0) + goto badframe; + } + + /* Strip 802.1p header */ + if (ntohs(s->eh.h_proto) == 0x8100) { + if (!skb_pull(skb, 4)) + goto badframe; + s->eh.h_proto = get_unaligned((__be16 *) (skb->data - 2)); + } + + /* We have to alloc new skb and copy data here :(. Because original skb + * may not be modified and because of the alignment requirements. */ + nskb = alloc_skb(2 + ETH_HLEN + skb->len, GFP_KERNEL); + if (!nskb) { + dev->stats.rx_dropped++; + kfree_skb(skb); + return -ENOMEM; + } + skb_reserve(nskb, 2); + + /* Decompress header and construct ether frame */ + switch (type & BNEP_TYPE_MASK) { + case BNEP_COMPRESSED: + memcpy(__skb_put(nskb, ETH_HLEN), &s->eh, ETH_HLEN); + break; + + case BNEP_COMPRESSED_SRC_ONLY: + memcpy(__skb_put(nskb, ETH_ALEN), s->eh.h_dest, ETH_ALEN); + memcpy(__skb_put(nskb, ETH_ALEN), skb_mac_header(skb), ETH_ALEN); + put_unaligned(s->eh.h_proto, (__be16 *) __skb_put(nskb, 2)); + break; + + case BNEP_COMPRESSED_DST_ONLY: + memcpy(__skb_put(nskb, ETH_ALEN), skb_mac_header(skb), + ETH_ALEN); + memcpy(__skb_put(nskb, ETH_ALEN + 2), s->eh.h_source, + ETH_ALEN + 2); + break; + + case BNEP_GENERAL: + memcpy(__skb_put(nskb, ETH_ALEN * 2), skb_mac_header(skb), + ETH_ALEN * 2); + put_unaligned(s->eh.h_proto, (__be16 *) __skb_put(nskb, 2)); + break; + } + + skb_copy_from_linear_data(skb, __skb_put(nskb, skb->len), skb->len); + kfree_skb(skb); + + dev->stats.rx_packets++; + nskb->ip_summed = CHECKSUM_NONE; + nskb->protocol = eth_type_trans(nskb, dev); + netif_rx_ni(nskb); + return 0; + +badframe: + dev->stats.rx_errors++; + kfree_skb(skb); + return 0; +} + +static u8 __bnep_tx_types[] = { + BNEP_GENERAL, + BNEP_COMPRESSED_SRC_ONLY, + BNEP_COMPRESSED_DST_ONLY, + BNEP_COMPRESSED +}; + +static inline int bnep_tx_frame(struct bnep_session *s, struct sk_buff *skb) +{ + struct ethhdr *eh = (void *) skb->data; + struct socket *sock = s->sock; + struct kvec iv[3]; + int len = 0, il = 0; + u8 type = 0; + + BT_DBG("skb %p dev %p type %d", skb, skb->dev, skb->pkt_type); + + if (!skb->dev) { + /* Control frame sent by us */ + goto send; + } + + iv[il++] = (struct kvec) { &type, 1 }; + len++; + + if (compress_src && !compare_ether_addr(eh->h_dest, s->eh.h_source)) + type |= 0x01; + + if (compress_dst && !compare_ether_addr(eh->h_source, s->eh.h_dest)) + type |= 0x02; + + if (type) + skb_pull(skb, ETH_ALEN * 2); + + type = __bnep_tx_types[type]; + switch (type) { + case BNEP_COMPRESSED_SRC_ONLY: + iv[il++] = (struct kvec) { eh->h_source, ETH_ALEN }; + len += ETH_ALEN; + break; + + case BNEP_COMPRESSED_DST_ONLY: + iv[il++] = (struct kvec) { eh->h_dest, ETH_ALEN }; + len += ETH_ALEN; + break; + } + +send: + iv[il++] = (struct kvec) { skb->data, skb->len }; + len += skb->len; + + /* FIXME: linearize skb */ + { + len = kernel_sendmsg(sock, &s->msg, iv, il, len); + } + kfree_skb(skb); + + if (len > 0) { + s->dev->stats.tx_bytes += len; + s->dev->stats.tx_packets++; + return 0; + } + + return len; +} + +static int bnep_session(void *arg) +{ + struct bnep_session *s = arg; + struct net_device *dev = s->dev; + struct sock *sk = s->sock->sk; + struct sk_buff *skb; + wait_queue_t wait; + + BT_DBG(""); + + set_user_nice(current, -15); + + init_waitqueue_entry(&wait, current); + add_wait_queue(sk_sleep(sk), &wait); + while (1) { + set_current_state(TASK_INTERRUPTIBLE); + + if (atomic_read(&s->terminate)) + break; + /* RX */ + while ((skb = skb_dequeue(&sk->sk_receive_queue))) { + skb_orphan(skb); + if (!skb_linearize(skb)) + bnep_rx_frame(s, skb); + else + kfree_skb(skb); + } + + if (sk->sk_state != BT_CONNECTED) + break; + + /* TX */ + while ((skb = skb_dequeue(&sk->sk_write_queue))) + if (bnep_tx_frame(s, skb)) + break; + netif_wake_queue(dev); + + schedule(); + } + __set_current_state(TASK_RUNNING); + remove_wait_queue(sk_sleep(sk), &wait); + + /* Cleanup session */ + down_write(&bnep_session_sem); + + /* Delete network device */ + unregister_netdev(dev); + + /* Wakeup user-space polling for socket errors */ + s->sock->sk->sk_err = EUNATCH; + + wake_up_interruptible(sk_sleep(s->sock->sk)); + + /* Release the socket */ + fput(s->sock->file); + + __bnep_unlink_session(s); + + up_write(&bnep_session_sem); + free_netdev(dev); + module_put_and_exit(0); + return 0; +} + +static struct device *bnep_get_device(struct bnep_session *session) +{ + bdaddr_t *src = &bt_sk(session->sock->sk)->src; + bdaddr_t *dst = &bt_sk(session->sock->sk)->dst; + struct hci_dev *hdev; + struct hci_conn *conn; + + hdev = hci_get_route(dst, src); + if (!hdev) + return NULL; + + conn = hci_conn_hash_lookup_ba(hdev, ACL_LINK, dst); + + hci_dev_put(hdev); + + return conn ? &conn->dev : NULL; +} + +static struct device_type bnep_type = { + .name = "bluetooth", +}; + +int bnep_add_connection(struct bnep_connadd_req *req, struct socket *sock) +{ + struct net_device *dev; + struct bnep_session *s, *ss; + u8 dst[ETH_ALEN], src[ETH_ALEN]; + int err; + + BT_DBG(""); + + baswap((void *) dst, &bt_sk(sock->sk)->dst); + baswap((void *) src, &bt_sk(sock->sk)->src); + + /* session struct allocated as private part of net_device */ + dev = alloc_netdev(sizeof(struct bnep_session), + (*req->device) ? req->device : "bnep%d", + bnep_net_setup); + if (!dev) + return -ENOMEM; + + down_write(&bnep_session_sem); + + ss = __bnep_get_session(dst); + if (ss && ss->state == BT_CONNECTED) { + err = -EEXIST; + goto failed; + } + + s = netdev_priv(dev); + + /* This is rx header therefore addresses are swapped. + * ie. eh.h_dest is our local address. */ + memcpy(s->eh.h_dest, &src, ETH_ALEN); + memcpy(s->eh.h_source, &dst, ETH_ALEN); + memcpy(dev->dev_addr, s->eh.h_dest, ETH_ALEN); + + s->dev = dev; + s->sock = sock; + s->role = req->role; + s->state = BT_CONNECTED; + + s->msg.msg_flags = MSG_NOSIGNAL; + +#ifdef CONFIG_BT_BNEP_MC_FILTER + /* Set default mc filter */ + set_bit(bnep_mc_hash(dev->broadcast), (ulong *) &s->mc_filter); +#endif + +#ifdef CONFIG_BT_BNEP_PROTO_FILTER + /* Set default protocol filter */ + bnep_set_default_proto_filter(s); +#endif + + SET_NETDEV_DEV(dev, bnep_get_device(s)); + SET_NETDEV_DEVTYPE(dev, &bnep_type); + + err = register_netdev(dev); + if (err) + goto failed; + + __bnep_link_session(s); + + __module_get(THIS_MODULE); + s->task = kthread_run(bnep_session, s, "kbnepd %s", dev->name); + if (IS_ERR(s->task)) { + /* Session thread start failed, gotta cleanup. */ + module_put(THIS_MODULE); + unregister_netdev(dev); + __bnep_unlink_session(s); + err = PTR_ERR(s->task); + goto failed; + } + + up_write(&bnep_session_sem); + strcpy(req->device, dev->name); + return 0; + +failed: + up_write(&bnep_session_sem); + free_netdev(dev); + return err; +} + +int bnep_del_connection(struct bnep_conndel_req *req) +{ + struct bnep_session *s; + int err = 0; + + BT_DBG(""); + + down_read(&bnep_session_sem); + + s = __bnep_get_session(req->dst); + if (s) { + atomic_inc(&s->terminate); + wake_up_process(s->task); + } else + err = -ENOENT; + + up_read(&bnep_session_sem); + return err; +} + +static void __bnep_copy_ci(struct bnep_conninfo *ci, struct bnep_session *s) +{ + memset(ci, 0, sizeof(*ci)); + memcpy(ci->dst, s->eh.h_source, ETH_ALEN); + strcpy(ci->device, s->dev->name); + ci->flags = s->flags; + ci->state = s->state; + ci->role = s->role; +} + +int bnep_get_connlist(struct bnep_connlist_req *req) +{ + struct bnep_session *s; + int err = 0, n = 0; + + down_read(&bnep_session_sem); + + list_for_each_entry(s, &bnep_session_list, list) { + struct bnep_conninfo ci; + + __bnep_copy_ci(&ci, s); + + if (copy_to_user(req->ci, &ci, sizeof(ci))) { + err = -EFAULT; + break; + } + + if (++n >= req->cnum) + break; + + req->ci++; + } + req->cnum = n; + + up_read(&bnep_session_sem); + return err; +} + +int bnep_get_conninfo(struct bnep_conninfo *ci) +{ + struct bnep_session *s; + int err = 0; + + down_read(&bnep_session_sem); + + s = __bnep_get_session(ci->dst); + if (s) + __bnep_copy_ci(ci, s); + else + err = -ENOENT; + + up_read(&bnep_session_sem); + return err; +} + +static int __init bnep_init(void) +{ + char flt[50] = ""; + +#ifdef CONFIG_BT_BNEP_PROTO_FILTER + strcat(flt, "protocol "); +#endif + +#ifdef CONFIG_BT_BNEP_MC_FILTER + strcat(flt, "multicast"); +#endif + + BT_INFO("BNEP (Ethernet Emulation) ver %s", VERSION); + if (flt[0]) + BT_INFO("BNEP filters: %s", flt); + + bnep_sock_init(); + return 0; +} + +static void __exit bnep_exit(void) +{ + bnep_sock_cleanup(); +} + +module_init(bnep_init); +module_exit(bnep_exit); + +module_param(compress_src, bool, 0644); +MODULE_PARM_DESC(compress_src, "Compress sources headers"); + +module_param(compress_dst, bool, 0644); +MODULE_PARM_DESC(compress_dst, "Compress destination headers"); + +MODULE_AUTHOR("Marcel Holtmann <marcel@holtmann.org>"); +MODULE_DESCRIPTION("Bluetooth BNEP ver " VERSION); +MODULE_VERSION(VERSION); +MODULE_LICENSE("GPL"); +MODULE_ALIAS("bt-proto-4"); diff --git a/net/bluetooth_tizen/bnep/netdev.c b/net/bluetooth_tizen/bnep/netdev.c new file mode 100644 index 0000000..bc40864 --- /dev/null +++ b/net/bluetooth_tizen/bnep/netdev.c @@ -0,0 +1,238 @@ +/* + BNEP implementation for Linux Bluetooth stack (BlueZ). + Copyright (C) 2001-2002 Inventel Systemes + Written 2001-2002 by + Clément Moreau <clement.moreau@inventel.fr> + David Libault <david.libault@inventel.fr> + + Copyright (C) 2002 Maxim Krasnyansky <maxk@qualcomm.com> + + This program is free software; you can redistribute it and/or modify + it under the terms of the GNU General Public License version 2 as + published by the Free Software Foundation; + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS + OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT OF THIRD PARTY RIGHTS. + IN NO EVENT SHALL THE COPYRIGHT HOLDER(S) AND AUTHOR(S) BE LIABLE FOR ANY + CLAIM, OR ANY SPECIAL INDIRECT OR CONSEQUENTIAL DAMAGES, OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + + ALL LIABILITY, INCLUDING LIABILITY FOR INFRINGEMENT OF ANY PATENTS, + COPYRIGHTS, TRADEMARKS OR OTHER RIGHTS, RELATING TO USE OF THIS + SOFTWARE IS DISCLAIMED. +*/ + +#include <linux/module.h> +#include <linux/slab.h> + +#include <linux/socket.h> +#include <linux/netdevice.h> +#include <linux/etherdevice.h> +#include <linux/skbuff.h> +#include <linux/wait.h> + +#include <asm/unaligned.h> + +#include <net/bluetooth/bluetooth.h> +#include <net/bluetooth/hci_core.h> +#include <net/bluetooth/l2cap.h> + +#include "bnep.h" + +#define BNEP_TX_QUEUE_LEN 20 + +static int bnep_net_open(struct net_device *dev) +{ + netif_start_queue(dev); + return 0; +} + +static int bnep_net_close(struct net_device *dev) +{ + netif_stop_queue(dev); + return 0; +} + +static void bnep_net_set_mc_list(struct net_device *dev) +{ +#ifdef CONFIG_BT_BNEP_MC_FILTER + struct bnep_session *s = netdev_priv(dev); + struct sock *sk = s->sock->sk; + struct bnep_set_filter_req *r; + struct sk_buff *skb; + int size; + + BT_DBG("%s mc_count %d", dev->name, netdev_mc_count(dev)); + + size = sizeof(*r) + (BNEP_MAX_MULTICAST_FILTERS + 1) * ETH_ALEN * 2; + skb = alloc_skb(size, GFP_ATOMIC); + if (!skb) { + BT_ERR("%s Multicast list allocation failed", dev->name); + return; + } + + r = (void *) skb->data; + __skb_put(skb, sizeof(*r)); + + r->type = BNEP_CONTROL; + r->ctrl = BNEP_FILTER_MULTI_ADDR_SET; + + if (dev->flags & (IFF_PROMISC | IFF_ALLMULTI)) { + u8 start[ETH_ALEN] = { 0x01 }; + + /* Request all addresses */ + memcpy(__skb_put(skb, ETH_ALEN), start, ETH_ALEN); + memcpy(__skb_put(skb, ETH_ALEN), dev->broadcast, ETH_ALEN); + r->len = htons(ETH_ALEN * 2); + } else { + struct netdev_hw_addr *ha; + int i, len = skb->len; + + if (dev->flags & IFF_BROADCAST) { + memcpy(__skb_put(skb, ETH_ALEN), dev->broadcast, ETH_ALEN); + memcpy(__skb_put(skb, ETH_ALEN), dev->broadcast, ETH_ALEN); + } + + /* FIXME: We should group addresses here. */ + + i = 0; + netdev_for_each_mc_addr(ha, dev) { + if (i == BNEP_MAX_MULTICAST_FILTERS) + break; + memcpy(__skb_put(skb, ETH_ALEN), ha->addr, ETH_ALEN); + memcpy(__skb_put(skb, ETH_ALEN), ha->addr, ETH_ALEN); + + i++; + } + r->len = htons(skb->len - len); + } + + skb_queue_tail(&sk->sk_write_queue, skb); + wake_up_interruptible(sk_sleep(sk)); +#endif +} + +static int bnep_net_set_mac_addr(struct net_device *dev, void *arg) +{ + BT_DBG("%s", dev->name); + return 0; +} + +static void bnep_net_timeout(struct net_device *dev) +{ + BT_DBG("net_timeout"); + netif_wake_queue(dev); +} + +#ifdef CONFIG_BT_BNEP_MC_FILTER +static inline int bnep_net_mc_filter(struct sk_buff *skb, struct bnep_session *s) +{ + struct ethhdr *eh = (void *) skb->data; + + if ((eh->h_dest[0] & 1) && !test_bit(bnep_mc_hash(eh->h_dest), (ulong *) &s->mc_filter)) + return 1; + return 0; +} +#endif + +#ifdef CONFIG_BT_BNEP_PROTO_FILTER +/* Determine ether protocol. Based on eth_type_trans. */ +static inline u16 bnep_net_eth_proto(struct sk_buff *skb) +{ + struct ethhdr *eh = (void *) skb->data; + u16 proto = ntohs(eh->h_proto); + + if (proto >= 1536) + return proto; + + if (get_unaligned((__be16 *) skb->data) == htons(0xFFFF)) + return ETH_P_802_3; + + return ETH_P_802_2; +} + +static inline int bnep_net_proto_filter(struct sk_buff *skb, struct bnep_session *s) +{ + u16 proto = bnep_net_eth_proto(skb); + struct bnep_proto_filter *f = s->proto_filter; + int i; + + for (i = 0; i < BNEP_MAX_PROTO_FILTERS && f[i].end; i++) { + if (proto >= f[i].start && proto <= f[i].end) + return 0; + } + + BT_DBG("BNEP: filtered skb %p, proto 0x%.4x", skb, proto); + return 1; +} +#endif + +static netdev_tx_t bnep_net_xmit(struct sk_buff *skb, + struct net_device *dev) +{ + struct bnep_session *s = netdev_priv(dev); + struct sock *sk = s->sock->sk; + + BT_DBG("skb %p, dev %p", skb, dev); + +#ifdef CONFIG_BT_BNEP_MC_FILTER + if (bnep_net_mc_filter(skb, s)) { + kfree_skb(skb); + return NETDEV_TX_OK; + } +#endif + +#ifdef CONFIG_BT_BNEP_PROTO_FILTER + if (bnep_net_proto_filter(skb, s)) { + kfree_skb(skb); + return NETDEV_TX_OK; + } +#endif + + /* + * We cannot send L2CAP packets from here as we are potentially in a bh. + * So we have to queue them and wake up session thread which is sleeping + * on the sk_sleep(sk). + */ + dev->trans_start = jiffies; + skb_queue_tail(&sk->sk_write_queue, skb); + wake_up_interruptible(sk_sleep(sk)); + + if (skb_queue_len(&sk->sk_write_queue) >= BNEP_TX_QUEUE_LEN) { + BT_DBG("tx queue is full"); + + /* Stop queuing. + * Session thread will do netif_wake_queue() */ + netif_stop_queue(dev); + } + + return NETDEV_TX_OK; +} + +static const struct net_device_ops bnep_netdev_ops = { + .ndo_open = bnep_net_open, + .ndo_stop = bnep_net_close, + .ndo_start_xmit = bnep_net_xmit, + .ndo_validate_addr = eth_validate_addr, + .ndo_set_rx_mode = bnep_net_set_mc_list, + .ndo_set_mac_address = bnep_net_set_mac_addr, + .ndo_tx_timeout = bnep_net_timeout, + .ndo_change_mtu = eth_change_mtu, + +}; + +void bnep_net_setup(struct net_device *dev) +{ + + memset(dev->broadcast, 0xff, ETH_ALEN); + dev->addr_len = ETH_ALEN; + + ether_setup(dev); + dev->priv_flags &= ~IFF_TX_SKB_SHARING; + dev->netdev_ops = &bnep_netdev_ops; + + dev->watchdog_timeo = HZ * 2; +} diff --git a/net/bluetooth_tizen/bnep/sock.c b/net/bluetooth_tizen/bnep/sock.c new file mode 100644 index 0000000..9f9c8dc --- /dev/null +++ b/net/bluetooth_tizen/bnep/sock.c @@ -0,0 +1,259 @@ +/* + BNEP implementation for Linux Bluetooth stack (BlueZ). + Copyright (C) 2001-2002 Inventel Systemes + Written 2001-2002 by + David Libault <david.libault@inventel.fr> + + Copyright (C) 2002 Maxim Krasnyansky <maxk@qualcomm.com> + + This program is free software; you can redistribute it and/or modify + it under the terms of the GNU General Public License version 2 as + published by the Free Software Foundation; + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS + OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT OF THIRD PARTY RIGHTS. + IN NO EVENT SHALL THE COPYRIGHT HOLDER(S) AND AUTHOR(S) BE LIABLE FOR ANY + CLAIM, OR ANY SPECIAL INDIRECT OR CONSEQUENTIAL DAMAGES, OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + + ALL LIABILITY, INCLUDING LIABILITY FOR INFRINGEMENT OF ANY PATENTS, + COPYRIGHTS, TRADEMARKS OR OTHER RIGHTS, RELATING TO USE OF THIS + SOFTWARE IS DISCLAIMED. +*/ + +#include <linux/module.h> + +#include <linux/types.h> +#include <linux/capability.h> +#include <linux/errno.h> +#include <linux/kernel.h> +#include <linux/poll.h> +#include <linux/fcntl.h> +#include <linux/skbuff.h> +#include <linux/socket.h> +#include <linux/ioctl.h> +#include <linux/file.h> +#include <linux/init.h> +#include <linux/compat.h> +#include <linux/gfp.h> +#include <linux/uaccess.h> +#include <net/sock.h> + +#include <asm/system.h> + +#include "bnep.h" + +static int bnep_sock_release(struct socket *sock) +{ + struct sock *sk = sock->sk; + + BT_DBG("sock %p sk %p", sock, sk); + + if (!sk) + return 0; + + sock_orphan(sk); + sock_put(sk); + return 0; +} + +static int bnep_sock_ioctl(struct socket *sock, unsigned int cmd, unsigned long arg) +{ + struct bnep_connlist_req cl; + struct bnep_connadd_req ca; + struct bnep_conndel_req cd; + struct bnep_conninfo ci; + struct socket *nsock; + void __user *argp = (void __user *)arg; + int err; + + BT_DBG("cmd %x arg %lx", cmd, arg); + + switch (cmd) { + case BNEPCONNADD: + if (!capable(CAP_NET_ADMIN)) + return -EACCES; + + if (copy_from_user(&ca, argp, sizeof(ca))) + return -EFAULT; + + nsock = sockfd_lookup(ca.sock, &err); + if (!nsock) + return err; + + if (nsock->sk->sk_state != BT_CONNECTED) { + sockfd_put(nsock); + return -EBADFD; + } + ca.device[sizeof(ca.device)-1] = 0; + + err = bnep_add_connection(&ca, nsock); + if (!err) { + if (copy_to_user(argp, &ca, sizeof(ca))) + err = -EFAULT; + } else + sockfd_put(nsock); + + return err; + + case BNEPCONNDEL: + if (!capable(CAP_NET_ADMIN)) + return -EACCES; + + if (copy_from_user(&cd, argp, sizeof(cd))) + return -EFAULT; + + return bnep_del_connection(&cd); + + case BNEPGETCONNLIST: + if (copy_from_user(&cl, argp, sizeof(cl))) + return -EFAULT; + + if (cl.cnum <= 0) + return -EINVAL; + + err = bnep_get_connlist(&cl); + if (!err && copy_to_user(argp, &cl, sizeof(cl))) + return -EFAULT; + + return err; + + case BNEPGETCONNINFO: + if (copy_from_user(&ci, argp, sizeof(ci))) + return -EFAULT; + + err = bnep_get_conninfo(&ci); + if (!err && copy_to_user(argp, &ci, sizeof(ci))) + return -EFAULT; + + return err; + + default: + return -EINVAL; + } + + return 0; +} + +#ifdef CONFIG_COMPAT +static int bnep_sock_compat_ioctl(struct socket *sock, unsigned int cmd, unsigned long arg) +{ + if (cmd == BNEPGETCONNLIST) { + struct bnep_connlist_req cl; + u32 uci; + int err; + + if (get_user(cl.cnum, (u32 __user *) arg) || + get_user(uci, (u32 __user *) (arg + 4))) + return -EFAULT; + + cl.ci = compat_ptr(uci); + + if (cl.cnum <= 0) + return -EINVAL; + + err = bnep_get_connlist(&cl); + + if (!err && put_user(cl.cnum, (u32 __user *) arg)) + err = -EFAULT; + + return err; + } + + return bnep_sock_ioctl(sock, cmd, arg); +} +#endif + +static const struct proto_ops bnep_sock_ops = { + .family = PF_BLUETOOTH, + .owner = THIS_MODULE, + .release = bnep_sock_release, + .ioctl = bnep_sock_ioctl, +#ifdef CONFIG_COMPAT + .compat_ioctl = bnep_sock_compat_ioctl, +#endif + .bind = sock_no_bind, + .getname = sock_no_getname, + .sendmsg = sock_no_sendmsg, + .recvmsg = sock_no_recvmsg, + .poll = sock_no_poll, + .listen = sock_no_listen, + .shutdown = sock_no_shutdown, + .setsockopt = sock_no_setsockopt, + .getsockopt = sock_no_getsockopt, + .connect = sock_no_connect, + .socketpair = sock_no_socketpair, + .accept = sock_no_accept, + .mmap = sock_no_mmap +}; + +static struct proto bnep_proto = { + .name = "BNEP", + .owner = THIS_MODULE, + .obj_size = sizeof(struct bt_sock) +}; + +static int bnep_sock_create(struct net *net, struct socket *sock, int protocol, + int kern) +{ + struct sock *sk; + + BT_DBG("sock %p", sock); + + if (sock->type != SOCK_RAW) + return -ESOCKTNOSUPPORT; + + sk = sk_alloc(net, PF_BLUETOOTH, GFP_ATOMIC, &bnep_proto); + if (!sk) + return -ENOMEM; + + sock_init_data(sock, sk); + + sock->ops = &bnep_sock_ops; + + sock->state = SS_UNCONNECTED; + + sock_reset_flag(sk, SOCK_ZAPPED); + + sk->sk_protocol = protocol; + sk->sk_state = BT_OPEN; + + return 0; +} + +static const struct net_proto_family bnep_sock_family_ops = { + .family = PF_BLUETOOTH, + .owner = THIS_MODULE, + .create = bnep_sock_create +}; + +int __init bnep_sock_init(void) +{ + int err; + + err = proto_register(&bnep_proto, 0); + if (err < 0) + return err; + + err = bt_sock_register(BTPROTO_BNEP, &bnep_sock_family_ops); + if (err < 0) + goto error; + + return 0; + +error: + BT_ERR("Can't register BNEP socket"); + proto_unregister(&bnep_proto); + return err; +} + +void __exit bnep_sock_cleanup(void) +{ + if (bt_sock_unregister(BTPROTO_BNEP) < 0) + BT_ERR("Can't unregister BNEP socket"); + + proto_unregister(&bnep_proto); +} diff --git a/net/bluetooth_tizen/cmtp/Kconfig b/net/bluetooth_tizen/cmtp/Kconfig new file mode 100644 index 0000000..94cbf42 --- /dev/null +++ b/net/bluetooth_tizen/cmtp/Kconfig @@ -0,0 +1,11 @@ +config BT_CMTP + tristate "CMTP protocol support" + depends on BT && ISDN_CAPI + help + CMTP (CAPI Message Transport Protocol) is a transport layer + for CAPI messages. CMTP is required for the Bluetooth Common + ISDN Access Profile. + + Say Y here to compile CMTP support into the kernel or say M to + compile it as module (cmtp). + diff --git a/net/bluetooth_tizen/cmtp/Makefile b/net/bluetooth_tizen/cmtp/Makefile new file mode 100644 index 0000000..890a9a5 --- /dev/null +++ b/net/bluetooth_tizen/cmtp/Makefile @@ -0,0 +1,7 @@ +# +# Makefile for the Linux Bluetooth CMTP layer +# + +obj-$(CONFIG_BT_CMTP) += cmtp.o + +cmtp-objs := core.o sock.o capi.o diff --git a/net/bluetooth_tizen/cmtp/capi.c b/net/bluetooth_tizen/cmtp/capi.c new file mode 100644 index 0000000..50f0d13 --- /dev/null +++ b/net/bluetooth_tizen/cmtp/capi.c @@ -0,0 +1,624 @@ +/* + CMTP implementation for Linux Bluetooth stack (BlueZ). + Copyright (C) 2002-2003 Marcel Holtmann <marcel@holtmann.org> + + This program is free software; you can redistribute it and/or modify + it under the terms of the GNU General Public License version 2 as + published by the Free Software Foundation; + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS + OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT OF THIRD PARTY RIGHTS. + IN NO EVENT SHALL THE COPYRIGHT HOLDER(S) AND AUTHOR(S) BE LIABLE FOR ANY + CLAIM, OR ANY SPECIAL INDIRECT OR CONSEQUENTIAL DAMAGES, OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + + ALL LIABILITY, INCLUDING LIABILITY FOR INFRINGEMENT OF ANY PATENTS, + COPYRIGHTS, TRADEMARKS OR OTHER RIGHTS, RELATING TO USE OF THIS + SOFTWARE IS DISCLAIMED. +*/ + +#include <linux/module.h> +#include <linux/proc_fs.h> +#include <linux/seq_file.h> +#include <linux/types.h> +#include <linux/errno.h> +#include <linux/kernel.h> +#include <linux/sched.h> +#include <linux/slab.h> +#include <linux/poll.h> +#include <linux/fcntl.h> +#include <linux/skbuff.h> +#include <linux/socket.h> +#include <linux/ioctl.h> +#include <linux/file.h> +#include <linux/wait.h> +#include <linux/kthread.h> +#include <net/sock.h> + +#include <linux/isdn/capilli.h> +#include <linux/isdn/capicmd.h> +#include <linux/isdn/capiutil.h> + +#include "cmtp.h" + +#define CAPI_INTEROPERABILITY 0x20 + +#define CAPI_INTEROPERABILITY_REQ CAPICMD(CAPI_INTEROPERABILITY, CAPI_REQ) +#define CAPI_INTEROPERABILITY_CONF CAPICMD(CAPI_INTEROPERABILITY, CAPI_CONF) +#define CAPI_INTEROPERABILITY_IND CAPICMD(CAPI_INTEROPERABILITY, CAPI_IND) +#define CAPI_INTEROPERABILITY_RESP CAPICMD(CAPI_INTEROPERABILITY, CAPI_RESP) + +#define CAPI_INTEROPERABILITY_REQ_LEN (CAPI_MSG_BASELEN + 2) +#define CAPI_INTEROPERABILITY_CONF_LEN (CAPI_MSG_BASELEN + 4) +#define CAPI_INTEROPERABILITY_IND_LEN (CAPI_MSG_BASELEN + 2) +#define CAPI_INTEROPERABILITY_RESP_LEN (CAPI_MSG_BASELEN + 2) + +#define CAPI_FUNCTION_REGISTER 0 +#define CAPI_FUNCTION_RELEASE 1 +#define CAPI_FUNCTION_GET_PROFILE 2 +#define CAPI_FUNCTION_GET_MANUFACTURER 3 +#define CAPI_FUNCTION_GET_VERSION 4 +#define CAPI_FUNCTION_GET_SERIAL_NUMBER 5 +#define CAPI_FUNCTION_MANUFACTURER 6 +#define CAPI_FUNCTION_LOOPBACK 7 + + +#define CMTP_MSGNUM 1 +#define CMTP_APPLID 2 +#define CMTP_MAPPING 3 + +static struct cmtp_application *cmtp_application_add(struct cmtp_session *session, __u16 appl) +{ + struct cmtp_application *app = kzalloc(sizeof(*app), GFP_KERNEL); + + BT_DBG("session %p application %p appl %d", session, app, appl); + + if (!app) + return NULL; + + app->state = BT_OPEN; + app->appl = appl; + + list_add_tail(&app->list, &session->applications); + + return app; +} + +static void cmtp_application_del(struct cmtp_session *session, struct cmtp_application *app) +{ + BT_DBG("session %p application %p", session, app); + + if (app) { + list_del(&app->list); + kfree(app); + } +} + +static struct cmtp_application *cmtp_application_get(struct cmtp_session *session, int pattern, __u16 value) +{ + struct cmtp_application *app; + struct list_head *p, *n; + + list_for_each_safe(p, n, &session->applications) { + app = list_entry(p, struct cmtp_application, list); + switch (pattern) { + case CMTP_MSGNUM: + if (app->msgnum == value) + return app; + break; + case CMTP_APPLID: + if (app->appl == value) + return app; + break; + case CMTP_MAPPING: + if (app->mapping == value) + return app; + break; + } + } + + return NULL; +} + +static int cmtp_msgnum_get(struct cmtp_session *session) +{ + session->msgnum++; + + if ((session->msgnum & 0xff) > 200) + session->msgnum = CMTP_INITIAL_MSGNUM + 1; + + return session->msgnum; +} + +static void cmtp_send_capimsg(struct cmtp_session *session, struct sk_buff *skb) +{ + struct cmtp_scb *scb = (void *) skb->cb; + + BT_DBG("session %p skb %p len %d", session, skb, skb->len); + + scb->id = -1; + scb->data = (CAPIMSG_COMMAND(skb->data) == CAPI_DATA_B3); + + skb_queue_tail(&session->transmit, skb); + + wake_up_interruptible(sk_sleep(session->sock->sk)); +} + +static void cmtp_send_interopmsg(struct cmtp_session *session, + __u8 subcmd, __u16 appl, __u16 msgnum, + __u16 function, unsigned char *buf, int len) +{ + struct sk_buff *skb; + unsigned char *s; + + BT_DBG("session %p subcmd 0x%02x appl %d msgnum %d", session, subcmd, appl, msgnum); + + skb = alloc_skb(CAPI_MSG_BASELEN + 6 + len, GFP_ATOMIC); + if (!skb) { + BT_ERR("Can't allocate memory for interoperability packet"); + return; + } + + s = skb_put(skb, CAPI_MSG_BASELEN + 6 + len); + + capimsg_setu16(s, 0, CAPI_MSG_BASELEN + 6 + len); + capimsg_setu16(s, 2, appl); + capimsg_setu8 (s, 4, CAPI_INTEROPERABILITY); + capimsg_setu8 (s, 5, subcmd); + capimsg_setu16(s, 6, msgnum); + + /* Interoperability selector (Bluetooth Device Management) */ + capimsg_setu16(s, 8, 0x0001); + + capimsg_setu8 (s, 10, 3 + len); + capimsg_setu16(s, 11, function); + capimsg_setu8 (s, 13, len); + + if (len > 0) + memcpy(s + 14, buf, len); + + cmtp_send_capimsg(session, skb); +} + +static void cmtp_recv_interopmsg(struct cmtp_session *session, struct sk_buff *skb) +{ + struct capi_ctr *ctrl = &session->ctrl; + struct cmtp_application *application; + __u16 appl, msgnum, func, info; + __u32 controller; + + BT_DBG("session %p skb %p len %d", session, skb, skb->len); + + switch (CAPIMSG_SUBCOMMAND(skb->data)) { + case CAPI_CONF: + if (skb->len < CAPI_MSG_BASELEN + 10) + break; + + func = CAPIMSG_U16(skb->data, CAPI_MSG_BASELEN + 5); + info = CAPIMSG_U16(skb->data, CAPI_MSG_BASELEN + 8); + + switch (func) { + case CAPI_FUNCTION_REGISTER: + msgnum = CAPIMSG_MSGID(skb->data); + + application = cmtp_application_get(session, CMTP_MSGNUM, msgnum); + if (application) { + application->state = BT_CONNECTED; + application->msgnum = 0; + application->mapping = CAPIMSG_APPID(skb->data); + wake_up_interruptible(&session->wait); + } + + break; + + case CAPI_FUNCTION_RELEASE: + appl = CAPIMSG_APPID(skb->data); + + application = cmtp_application_get(session, CMTP_MAPPING, appl); + if (application) { + application->state = BT_CLOSED; + application->msgnum = 0; + wake_up_interruptible(&session->wait); + } + + break; + + case CAPI_FUNCTION_GET_PROFILE: + if (skb->len < CAPI_MSG_BASELEN + 11 + sizeof(capi_profile)) + break; + + controller = CAPIMSG_U16(skb->data, CAPI_MSG_BASELEN + 11); + msgnum = CAPIMSG_MSGID(skb->data); + + if (!info && (msgnum == CMTP_INITIAL_MSGNUM)) { + session->ncontroller = controller; + wake_up_interruptible(&session->wait); + break; + } + + if (!info && ctrl) { + memcpy(&ctrl->profile, + skb->data + CAPI_MSG_BASELEN + 11, + sizeof(capi_profile)); + session->state = BT_CONNECTED; + capi_ctr_ready(ctrl); + } + + break; + + case CAPI_FUNCTION_GET_MANUFACTURER: + if (skb->len < CAPI_MSG_BASELEN + 15) + break; + + controller = CAPIMSG_U32(skb->data, CAPI_MSG_BASELEN + 10); + + if (!info && ctrl) { + int len = min_t(uint, CAPI_MANUFACTURER_LEN, + skb->data[CAPI_MSG_BASELEN + 14]); + + memset(ctrl->manu, 0, CAPI_MANUFACTURER_LEN); + strncpy(ctrl->manu, + skb->data + CAPI_MSG_BASELEN + 15, len); + } + + break; + + case CAPI_FUNCTION_GET_VERSION: + if (skb->len < CAPI_MSG_BASELEN + 32) + break; + + controller = CAPIMSG_U32(skb->data, CAPI_MSG_BASELEN + 12); + + if (!info && ctrl) { + ctrl->version.majorversion = CAPIMSG_U32(skb->data, CAPI_MSG_BASELEN + 16); + ctrl->version.minorversion = CAPIMSG_U32(skb->data, CAPI_MSG_BASELEN + 20); + ctrl->version.majormanuversion = CAPIMSG_U32(skb->data, CAPI_MSG_BASELEN + 24); + ctrl->version.minormanuversion = CAPIMSG_U32(skb->data, CAPI_MSG_BASELEN + 28); + } + + break; + + case CAPI_FUNCTION_GET_SERIAL_NUMBER: + if (skb->len < CAPI_MSG_BASELEN + 17) + break; + + controller = CAPIMSG_U32(skb->data, CAPI_MSG_BASELEN + 12); + + if (!info && ctrl) { + int len = min_t(uint, CAPI_SERIAL_LEN, + skb->data[CAPI_MSG_BASELEN + 16]); + + memset(ctrl->serial, 0, CAPI_SERIAL_LEN); + strncpy(ctrl->serial, + skb->data + CAPI_MSG_BASELEN + 17, len); + } + + break; + } + + break; + + case CAPI_IND: + if (skb->len < CAPI_MSG_BASELEN + 6) + break; + + func = CAPIMSG_U16(skb->data, CAPI_MSG_BASELEN + 3); + + if (func == CAPI_FUNCTION_LOOPBACK) { + int len = min_t(uint, skb->len - CAPI_MSG_BASELEN - 6, + skb->data[CAPI_MSG_BASELEN + 5]); + appl = CAPIMSG_APPID(skb->data); + msgnum = CAPIMSG_MSGID(skb->data); + cmtp_send_interopmsg(session, CAPI_RESP, appl, msgnum, func, + skb->data + CAPI_MSG_BASELEN + 6, len); + } + + break; + } + + kfree_skb(skb); +} + +void cmtp_recv_capimsg(struct cmtp_session *session, struct sk_buff *skb) +{ + struct capi_ctr *ctrl = &session->ctrl; + struct cmtp_application *application; + __u16 appl; + __u32 contr; + + BT_DBG("session %p skb %p len %d", session, skb, skb->len); + + if (skb->len < CAPI_MSG_BASELEN) + return; + + if (CAPIMSG_COMMAND(skb->data) == CAPI_INTEROPERABILITY) { + cmtp_recv_interopmsg(session, skb); + return; + } + + if (session->flags & (1 << CMTP_LOOPBACK)) { + kfree_skb(skb); + return; + } + + appl = CAPIMSG_APPID(skb->data); + contr = CAPIMSG_CONTROL(skb->data); + + application = cmtp_application_get(session, CMTP_MAPPING, appl); + if (application) { + appl = application->appl; + CAPIMSG_SETAPPID(skb->data, appl); + } else { + BT_ERR("Can't find application with id %d", appl); + kfree_skb(skb); + return; + } + + if ((contr & 0x7f) == 0x01) { + contr = (contr & 0xffffff80) | session->num; + CAPIMSG_SETCONTROL(skb->data, contr); + } + + if (!ctrl) { + BT_ERR("Can't find controller %d for message", session->num); + kfree_skb(skb); + return; + } + + capi_ctr_handle_message(ctrl, appl, skb); +} + +static int cmtp_load_firmware(struct capi_ctr *ctrl, capiloaddata *data) +{ + BT_DBG("ctrl %p data %p", ctrl, data); + + return 0; +} + +static void cmtp_reset_ctr(struct capi_ctr *ctrl) +{ + struct cmtp_session *session = ctrl->driverdata; + + BT_DBG("ctrl %p", ctrl); + + capi_ctr_down(ctrl); + + atomic_inc(&session->terminate); + wake_up_process(session->task); +} + +static void cmtp_register_appl(struct capi_ctr *ctrl, __u16 appl, capi_register_params *rp) +{ + DECLARE_WAITQUEUE(wait, current); + struct cmtp_session *session = ctrl->driverdata; + struct cmtp_application *application; + unsigned long timeo = CMTP_INTEROP_TIMEOUT; + unsigned char buf[8]; + int err = 0, nconn, want = rp->level3cnt; + + BT_DBG("ctrl %p appl %d level3cnt %d datablkcnt %d datablklen %d", + ctrl, appl, rp->level3cnt, rp->datablkcnt, rp->datablklen); + + application = cmtp_application_add(session, appl); + if (!application) { + BT_ERR("Can't allocate memory for new application"); + return; + } + + if (want < 0) + nconn = ctrl->profile.nbchannel * -want; + else + nconn = want; + + if (nconn == 0) + nconn = ctrl->profile.nbchannel; + + capimsg_setu16(buf, 0, nconn); + capimsg_setu16(buf, 2, rp->datablkcnt); + capimsg_setu16(buf, 4, rp->datablklen); + + application->state = BT_CONFIG; + application->msgnum = cmtp_msgnum_get(session); + + cmtp_send_interopmsg(session, CAPI_REQ, 0x0000, application->msgnum, + CAPI_FUNCTION_REGISTER, buf, 6); + + add_wait_queue(&session->wait, &wait); + while (1) { + set_current_state(TASK_INTERRUPTIBLE); + + if (!timeo) { + err = -EAGAIN; + break; + } + + if (application->state == BT_CLOSED) { + err = -application->err; + break; + } + + if (application->state == BT_CONNECTED) + break; + + if (signal_pending(current)) { + err = -EINTR; + break; + } + + timeo = schedule_timeout(timeo); + } + set_current_state(TASK_RUNNING); + remove_wait_queue(&session->wait, &wait); + + if (err) { + cmtp_application_del(session, application); + return; + } +} + +static void cmtp_release_appl(struct capi_ctr *ctrl, __u16 appl) +{ + struct cmtp_session *session = ctrl->driverdata; + struct cmtp_application *application; + + BT_DBG("ctrl %p appl %d", ctrl, appl); + + application = cmtp_application_get(session, CMTP_APPLID, appl); + if (!application) { + BT_ERR("Can't find application"); + return; + } + + application->msgnum = cmtp_msgnum_get(session); + + cmtp_send_interopmsg(session, CAPI_REQ, application->mapping, application->msgnum, + CAPI_FUNCTION_RELEASE, NULL, 0); + + wait_event_interruptible_timeout(session->wait, + (application->state == BT_CLOSED), CMTP_INTEROP_TIMEOUT); + + cmtp_application_del(session, application); +} + +static u16 cmtp_send_message(struct capi_ctr *ctrl, struct sk_buff *skb) +{ + struct cmtp_session *session = ctrl->driverdata; + struct cmtp_application *application; + __u16 appl; + __u32 contr; + + BT_DBG("ctrl %p skb %p", ctrl, skb); + + appl = CAPIMSG_APPID(skb->data); + contr = CAPIMSG_CONTROL(skb->data); + + application = cmtp_application_get(session, CMTP_APPLID, appl); + if ((!application) || (application->state != BT_CONNECTED)) { + BT_ERR("Can't find application with id %d", appl); + return CAPI_ILLAPPNR; + } + + CAPIMSG_SETAPPID(skb->data, application->mapping); + + if ((contr & 0x7f) == session->num) { + contr = (contr & 0xffffff80) | 0x01; + CAPIMSG_SETCONTROL(skb->data, contr); + } + + cmtp_send_capimsg(session, skb); + + return CAPI_NOERROR; +} + +static char *cmtp_procinfo(struct capi_ctr *ctrl) +{ + return "CAPI Message Transport Protocol"; +} + +static int cmtp_proc_show(struct seq_file *m, void *v) +{ + struct capi_ctr *ctrl = m->private; + struct cmtp_session *session = ctrl->driverdata; + struct cmtp_application *app; + struct list_head *p, *n; + + seq_printf(m, "%s\n\n", cmtp_procinfo(ctrl)); + seq_printf(m, "addr %s\n", session->name); + seq_printf(m, "ctrl %d\n", session->num); + + list_for_each_safe(p, n, &session->applications) { + app = list_entry(p, struct cmtp_application, list); + seq_printf(m, "appl %d -> %d\n", app->appl, app->mapping); + } + + return 0; +} + +static int cmtp_proc_open(struct inode *inode, struct file *file) +{ + return single_open(file, cmtp_proc_show, PDE(inode)->data); +} + +static const struct file_operations cmtp_proc_fops = { + .owner = THIS_MODULE, + .open = cmtp_proc_open, + .read = seq_read, + .llseek = seq_lseek, + .release = single_release, +}; + +int cmtp_attach_device(struct cmtp_session *session) +{ + unsigned char buf[4]; + long ret; + + BT_DBG("session %p", session); + + capimsg_setu32(buf, 0, 0); + + cmtp_send_interopmsg(session, CAPI_REQ, 0xffff, CMTP_INITIAL_MSGNUM, + CAPI_FUNCTION_GET_PROFILE, buf, 4); + + ret = wait_event_interruptible_timeout(session->wait, + session->ncontroller, CMTP_INTEROP_TIMEOUT); + + BT_INFO("Found %d CAPI controller(s) on device %s", session->ncontroller, session->name); + + if (!ret) + return -ETIMEDOUT; + + if (!session->ncontroller) + return -ENODEV; + + if (session->ncontroller > 1) + BT_INFO("Setting up only CAPI controller 1"); + + session->ctrl.owner = THIS_MODULE; + session->ctrl.driverdata = session; + strcpy(session->ctrl.name, session->name); + + session->ctrl.driver_name = "cmtp"; + session->ctrl.load_firmware = cmtp_load_firmware; + session->ctrl.reset_ctr = cmtp_reset_ctr; + session->ctrl.register_appl = cmtp_register_appl; + session->ctrl.release_appl = cmtp_release_appl; + session->ctrl.send_message = cmtp_send_message; + + session->ctrl.procinfo = cmtp_procinfo; + session->ctrl.proc_fops = &cmtp_proc_fops; + + if (attach_capi_ctr(&session->ctrl) < 0) { + BT_ERR("Can't attach new controller"); + return -EBUSY; + } + + session->num = session->ctrl.cnr; + + BT_DBG("session %p num %d", session, session->num); + + capimsg_setu32(buf, 0, 1); + + cmtp_send_interopmsg(session, CAPI_REQ, 0xffff, cmtp_msgnum_get(session), + CAPI_FUNCTION_GET_MANUFACTURER, buf, 4); + + cmtp_send_interopmsg(session, CAPI_REQ, 0xffff, cmtp_msgnum_get(session), + CAPI_FUNCTION_GET_VERSION, buf, 4); + + cmtp_send_interopmsg(session, CAPI_REQ, 0xffff, cmtp_msgnum_get(session), + CAPI_FUNCTION_GET_SERIAL_NUMBER, buf, 4); + + cmtp_send_interopmsg(session, CAPI_REQ, 0xffff, cmtp_msgnum_get(session), + CAPI_FUNCTION_GET_PROFILE, buf, 4); + + return 0; +} + +void cmtp_detach_device(struct cmtp_session *session) +{ + BT_DBG("session %p", session); + + detach_capi_ctr(&session->ctrl); +} diff --git a/net/bluetooth_tizen/cmtp/cmtp.h b/net/bluetooth_tizen/cmtp/cmtp.h new file mode 100644 index 0000000..c32638d --- /dev/null +++ b/net/bluetooth_tizen/cmtp/cmtp.h @@ -0,0 +1,129 @@ +/* + CMTP implementation for Linux Bluetooth stack (BlueZ). + Copyright (C) 2002-2003 Marcel Holtmann <marcel@holtmann.org> + + This program is free software; you can redistribute it and/or modify + it under the terms of the GNU General Public License version 2 as + published by the Free Software Foundation; + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS + OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT OF THIRD PARTY RIGHTS. + IN NO EVENT SHALL THE COPYRIGHT HOLDER(S) AND AUTHOR(S) BE LIABLE FOR ANY + CLAIM, OR ANY SPECIAL INDIRECT OR CONSEQUENTIAL DAMAGES, OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + + ALL LIABILITY, INCLUDING LIABILITY FOR INFRINGEMENT OF ANY PATENTS, + COPYRIGHTS, TRADEMARKS OR OTHER RIGHTS, RELATING TO USE OF THIS + SOFTWARE IS DISCLAIMED. +*/ + +#ifndef __CMTP_H +#define __CMTP_H + +#include <linux/types.h> +#include <net/bluetooth/bluetooth.h> + +#define BTNAMSIZ 18 + +/* CMTP ioctl defines */ +#define CMTPCONNADD _IOW('C', 200, int) +#define CMTPCONNDEL _IOW('C', 201, int) +#define CMTPGETCONNLIST _IOR('C', 210, int) +#define CMTPGETCONNINFO _IOR('C', 211, int) + +#define CMTP_LOOPBACK 0 + +struct cmtp_connadd_req { + int sock; /* Connected socket */ + __u32 flags; +}; + +struct cmtp_conndel_req { + bdaddr_t bdaddr; + __u32 flags; +}; + +struct cmtp_conninfo { + bdaddr_t bdaddr; + __u32 flags; + __u16 state; + int num; +}; + +struct cmtp_connlist_req { + __u32 cnum; + struct cmtp_conninfo __user *ci; +}; + +int cmtp_add_connection(struct cmtp_connadd_req *req, struct socket *sock); +int cmtp_del_connection(struct cmtp_conndel_req *req); +int cmtp_get_connlist(struct cmtp_connlist_req *req); +int cmtp_get_conninfo(struct cmtp_conninfo *ci); + +/* CMTP session defines */ +#define CMTP_INTEROP_TIMEOUT (HZ * 5) +#define CMTP_INITIAL_MSGNUM 0xff00 + +struct cmtp_session { + struct list_head list; + + struct socket *sock; + + bdaddr_t bdaddr; + + unsigned long state; + unsigned long flags; + + uint mtu; + + char name[BTNAMSIZ]; + + atomic_t terminate; + struct task_struct *task; + + wait_queue_head_t wait; + + int ncontroller; + int num; + struct capi_ctr ctrl; + + struct list_head applications; + + unsigned long blockids; + int msgnum; + + struct sk_buff_head transmit; + + struct sk_buff *reassembly[16]; +}; + +struct cmtp_application { + struct list_head list; + + unsigned long state; + int err; + + __u16 appl; + __u16 mapping; + + __u16 msgnum; +}; + +struct cmtp_scb { + int id; + int data; +}; + +int cmtp_attach_device(struct cmtp_session *session); +void cmtp_detach_device(struct cmtp_session *session); + +void cmtp_recv_capimsg(struct cmtp_session *session, struct sk_buff *skb); + +/* CMTP init defines */ +int cmtp_init_sockets(void); +void cmtp_cleanup_sockets(void); + +#endif /* __CMTP_H */ diff --git a/net/bluetooth_tizen/cmtp/core.c b/net/bluetooth_tizen/cmtp/core.c new file mode 100644 index 0000000..6c9c1fd --- /dev/null +++ b/net/bluetooth_tizen/cmtp/core.c @@ -0,0 +1,500 @@ +/* + CMTP implementation for Linux Bluetooth stack (BlueZ). + Copyright (C) 2002-2003 Marcel Holtmann <marcel@holtmann.org> + + This program is free software; you can redistribute it and/or modify + it under the terms of the GNU General Public License version 2 as + published by the Free Software Foundation; + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS + OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT OF THIRD PARTY RIGHTS. + IN NO EVENT SHALL THE COPYRIGHT HOLDER(S) AND AUTHOR(S) BE LIABLE FOR ANY + CLAIM, OR ANY SPECIAL INDIRECT OR CONSEQUENTIAL DAMAGES, OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + + ALL LIABILITY, INCLUDING LIABILITY FOR INFRINGEMENT OF ANY PATENTS, + COPYRIGHTS, TRADEMARKS OR OTHER RIGHTS, RELATING TO USE OF THIS + SOFTWARE IS DISCLAIMED. +*/ + +#include <linux/module.h> + +#include <linux/types.h> +#include <linux/errno.h> +#include <linux/kernel.h> +#include <linux/sched.h> +#include <linux/slab.h> +#include <linux/poll.h> +#include <linux/fcntl.h> +#include <linux/freezer.h> +#include <linux/skbuff.h> +#include <linux/socket.h> +#include <linux/ioctl.h> +#include <linux/file.h> +#include <linux/init.h> +#include <linux/kthread.h> +#include <net/sock.h> + +#include <linux/isdn/capilli.h> + +#include <net/bluetooth/bluetooth.h> +#include <net/bluetooth/l2cap.h> + +#include "cmtp.h" + +#define VERSION "1.0" + +static DECLARE_RWSEM(cmtp_session_sem); +static LIST_HEAD(cmtp_session_list); + +static struct cmtp_session *__cmtp_get_session(bdaddr_t *bdaddr) +{ + struct cmtp_session *session; + + BT_DBG(""); + + list_for_each_entry(session, &cmtp_session_list, list) + if (!bacmp(bdaddr, &session->bdaddr)) + return session; + + return NULL; +} + +static void __cmtp_link_session(struct cmtp_session *session) +{ + list_add(&session->list, &cmtp_session_list); +} + +static void __cmtp_unlink_session(struct cmtp_session *session) +{ + list_del(&session->list); +} + +static void __cmtp_copy_session(struct cmtp_session *session, struct cmtp_conninfo *ci) +{ + memset(ci, 0, sizeof(*ci)); + bacpy(&ci->bdaddr, &session->bdaddr); + + ci->flags = session->flags; + ci->state = session->state; + + ci->num = session->num; +} + + +static inline int cmtp_alloc_block_id(struct cmtp_session *session) +{ + int i, id = -1; + + for (i = 0; i < 16; i++) + if (!test_and_set_bit(i, &session->blockids)) { + id = i; + break; + } + + return id; +} + +static inline void cmtp_free_block_id(struct cmtp_session *session, int id) +{ + clear_bit(id, &session->blockids); +} + +static inline void cmtp_add_msgpart(struct cmtp_session *session, int id, const unsigned char *buf, int count) +{ + struct sk_buff *skb = session->reassembly[id], *nskb; + int size; + + BT_DBG("session %p buf %p count %d", session, buf, count); + + size = (skb) ? skb->len + count : count; + + nskb = alloc_skb(size, GFP_ATOMIC); + if (!nskb) { + BT_ERR("Can't allocate memory for CAPI message"); + return; + } + + if (skb && (skb->len > 0)) + skb_copy_from_linear_data(skb, skb_put(nskb, skb->len), skb->len); + + memcpy(skb_put(nskb, count), buf, count); + + session->reassembly[id] = nskb; + + kfree_skb(skb); +} + +static inline int cmtp_recv_frame(struct cmtp_session *session, struct sk_buff *skb) +{ + __u8 hdr, hdrlen, id; + __u16 len; + + BT_DBG("session %p skb %p len %d", session, skb, skb->len); + + while (skb->len > 0) { + hdr = skb->data[0]; + + switch (hdr & 0xc0) { + case 0x40: + hdrlen = 2; + len = skb->data[1]; + break; + case 0x80: + hdrlen = 3; + len = skb->data[1] | (skb->data[2] << 8); + break; + default: + hdrlen = 1; + len = 0; + break; + } + + id = (hdr & 0x3c) >> 2; + + BT_DBG("hdr 0x%02x hdrlen %d len %d id %d", hdr, hdrlen, len, id); + + if (hdrlen + len > skb->len) { + BT_ERR("Wrong size or header information in CMTP frame"); + break; + } + + if (len == 0) { + skb_pull(skb, hdrlen); + continue; + } + + switch (hdr & 0x03) { + case 0x00: + cmtp_add_msgpart(session, id, skb->data + hdrlen, len); + cmtp_recv_capimsg(session, session->reassembly[id]); + session->reassembly[id] = NULL; + break; + case 0x01: + cmtp_add_msgpart(session, id, skb->data + hdrlen, len); + break; + default: + if (session->reassembly[id] != NULL) + kfree_skb(session->reassembly[id]); + session->reassembly[id] = NULL; + break; + } + + skb_pull(skb, hdrlen + len); + } + + kfree_skb(skb); + return 0; +} + +static int cmtp_send_frame(struct cmtp_session *session, unsigned char *data, int len) +{ + struct socket *sock = session->sock; + struct kvec iv = { data, len }; + struct msghdr msg; + + BT_DBG("session %p data %p len %d", session, data, len); + + if (!len) + return 0; + + memset(&msg, 0, sizeof(msg)); + + return kernel_sendmsg(sock, &msg, &iv, 1, len); +} + +static void cmtp_process_transmit(struct cmtp_session *session) +{ + struct sk_buff *skb, *nskb; + unsigned char *hdr; + unsigned int size, tail; + + BT_DBG("session %p", session); + + nskb = alloc_skb(session->mtu, GFP_ATOMIC); + if (!nskb) { + BT_ERR("Can't allocate memory for new frame"); + return; + } + + while ((skb = skb_dequeue(&session->transmit))) { + struct cmtp_scb *scb = (void *) skb->cb; + + tail = session->mtu - nskb->len; + if (tail < 5) { + cmtp_send_frame(session, nskb->data, nskb->len); + skb_trim(nskb, 0); + tail = session->mtu; + } + + size = min_t(uint, ((tail < 258) ? (tail - 2) : (tail - 3)), skb->len); + + if (scb->id < 0) { + scb->id = cmtp_alloc_block_id(session); + if (scb->id < 0) { + skb_queue_head(&session->transmit, skb); + break; + } + } + + if (size < 256) { + hdr = skb_put(nskb, 2); + hdr[0] = 0x40 + | ((scb->id << 2) & 0x3c) + | ((skb->len == size) ? 0x00 : 0x01); + hdr[1] = size; + } else { + hdr = skb_put(nskb, 3); + hdr[0] = 0x80 + | ((scb->id << 2) & 0x3c) + | ((skb->len == size) ? 0x00 : 0x01); + hdr[1] = size & 0xff; + hdr[2] = size >> 8; + } + + skb_copy_from_linear_data(skb, skb_put(nskb, size), size); + skb_pull(skb, size); + + if (skb->len > 0) { + skb_queue_head(&session->transmit, skb); + } else { + cmtp_free_block_id(session, scb->id); + if (scb->data) { + cmtp_send_frame(session, nskb->data, nskb->len); + skb_trim(nskb, 0); + } + kfree_skb(skb); + } + } + + cmtp_send_frame(session, nskb->data, nskb->len); + + kfree_skb(nskb); +} + +static int cmtp_session(void *arg) +{ + struct cmtp_session *session = arg; + struct sock *sk = session->sock->sk; + struct sk_buff *skb; + wait_queue_t wait; + + BT_DBG("session %p", session); + + set_user_nice(current, -15); + + init_waitqueue_entry(&wait, current); + add_wait_queue(sk_sleep(sk), &wait); + while (1) { + set_current_state(TASK_INTERRUPTIBLE); + + if (atomic_read(&session->terminate)) + break; + if (sk->sk_state != BT_CONNECTED) + break; + + while ((skb = skb_dequeue(&sk->sk_receive_queue))) { + skb_orphan(skb); + if (!skb_linearize(skb)) + cmtp_recv_frame(session, skb); + else + kfree_skb(skb); + } + + cmtp_process_transmit(session); + + schedule(); + } + __set_current_state(TASK_RUNNING); + remove_wait_queue(sk_sleep(sk), &wait); + + down_write(&cmtp_session_sem); + + if (!(session->flags & (1 << CMTP_LOOPBACK))) + cmtp_detach_device(session); + + fput(session->sock->file); + + __cmtp_unlink_session(session); + + up_write(&cmtp_session_sem); + + kfree(session); + module_put_and_exit(0); + return 0; +} + +int cmtp_add_connection(struct cmtp_connadd_req *req, struct socket *sock) +{ + struct cmtp_session *session, *s; + int i, err; + + BT_DBG(""); + + session = kzalloc(sizeof(struct cmtp_session), GFP_KERNEL); + if (!session) + return -ENOMEM; + + down_write(&cmtp_session_sem); + + s = __cmtp_get_session(&bt_sk(sock->sk)->dst); + if (s && s->state == BT_CONNECTED) { + err = -EEXIST; + goto failed; + } + + bacpy(&session->bdaddr, &bt_sk(sock->sk)->dst); + + session->mtu = min_t(uint, l2cap_pi(sock->sk)->chan->omtu, + l2cap_pi(sock->sk)->chan->imtu); + + BT_DBG("mtu %d", session->mtu); + + sprintf(session->name, "%s", batostr(&bt_sk(sock->sk)->dst)); + + session->sock = sock; + session->state = BT_CONFIG; + + init_waitqueue_head(&session->wait); + + session->msgnum = CMTP_INITIAL_MSGNUM; + + INIT_LIST_HEAD(&session->applications); + + skb_queue_head_init(&session->transmit); + + for (i = 0; i < 16; i++) + session->reassembly[i] = NULL; + + session->flags = req->flags; + + __cmtp_link_session(session); + + __module_get(THIS_MODULE); + session->task = kthread_run(cmtp_session, session, "kcmtpd_ctr_%d", + session->num); + if (IS_ERR(session->task)) { + module_put(THIS_MODULE); + err = PTR_ERR(session->task); + goto unlink; + } + + if (!(session->flags & (1 << CMTP_LOOPBACK))) { + err = cmtp_attach_device(session); + if (err < 0) { + atomic_inc(&session->terminate); + wake_up_process(session->task); + up_write(&cmtp_session_sem); + return err; + } + } + + up_write(&cmtp_session_sem); + return 0; + +unlink: + __cmtp_unlink_session(session); + +failed: + up_write(&cmtp_session_sem); + kfree(session); + return err; +} + +int cmtp_del_connection(struct cmtp_conndel_req *req) +{ + struct cmtp_session *session; + int err = 0; + + BT_DBG(""); + + down_read(&cmtp_session_sem); + + session = __cmtp_get_session(&req->bdaddr); + if (session) { + /* Flush the transmit queue */ + skb_queue_purge(&session->transmit); + + /* Stop session thread */ + atomic_inc(&session->terminate); + wake_up_process(session->task); + } else + err = -ENOENT; + + up_read(&cmtp_session_sem); + return err; +} + +int cmtp_get_connlist(struct cmtp_connlist_req *req) +{ + struct cmtp_session *session; + int err = 0, n = 0; + + BT_DBG(""); + + down_read(&cmtp_session_sem); + + list_for_each_entry(session, &cmtp_session_list, list) { + struct cmtp_conninfo ci; + + __cmtp_copy_session(session, &ci); + + if (copy_to_user(req->ci, &ci, sizeof(ci))) { + err = -EFAULT; + break; + } + + if (++n >= req->cnum) + break; + + req->ci++; + } + req->cnum = n; + + up_read(&cmtp_session_sem); + return err; +} + +int cmtp_get_conninfo(struct cmtp_conninfo *ci) +{ + struct cmtp_session *session; + int err = 0; + + down_read(&cmtp_session_sem); + + session = __cmtp_get_session(&ci->bdaddr); + if (session) + __cmtp_copy_session(session, ci); + else + err = -ENOENT; + + up_read(&cmtp_session_sem); + return err; +} + + +static int __init cmtp_init(void) +{ + BT_INFO("CMTP (CAPI Emulation) ver %s", VERSION); + + cmtp_init_sockets(); + + return 0; +} + +static void __exit cmtp_exit(void) +{ + cmtp_cleanup_sockets(); +} + +module_init(cmtp_init); +module_exit(cmtp_exit); + +MODULE_AUTHOR("Marcel Holtmann <marcel@holtmann.org>"); +MODULE_DESCRIPTION("Bluetooth CMTP ver " VERSION); +MODULE_VERSION(VERSION); +MODULE_LICENSE("GPL"); +MODULE_ALIAS("bt-proto-5"); diff --git a/net/bluetooth_tizen/cmtp/sock.c b/net/bluetooth_tizen/cmtp/sock.c new file mode 100644 index 0000000..1230faa --- /dev/null +++ b/net/bluetooth_tizen/cmtp/sock.c @@ -0,0 +1,253 @@ +/* + CMTP implementation for Linux Bluetooth stack (BlueZ). + Copyright (C) 2002-2003 Marcel Holtmann <marcel@holtmann.org> + + This program is free software; you can redistribute it and/or modify + it under the terms of the GNU General Public License version 2 as + published by the Free Software Foundation; + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS + OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT OF THIRD PARTY RIGHTS. + IN NO EVENT SHALL THE COPYRIGHT HOLDER(S) AND AUTHOR(S) BE LIABLE FOR ANY + CLAIM, OR ANY SPECIAL INDIRECT OR CONSEQUENTIAL DAMAGES, OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + + ALL LIABILITY, INCLUDING LIABILITY FOR INFRINGEMENT OF ANY PATENTS, + COPYRIGHTS, TRADEMARKS OR OTHER RIGHTS, RELATING TO USE OF THIS + SOFTWARE IS DISCLAIMED. +*/ + +#include <linux/module.h> + +#include <linux/types.h> +#include <linux/capability.h> +#include <linux/errno.h> +#include <linux/kernel.h> +#include <linux/poll.h> +#include <linux/fcntl.h> +#include <linux/skbuff.h> +#include <linux/socket.h> +#include <linux/ioctl.h> +#include <linux/file.h> +#include <linux/compat.h> +#include <linux/gfp.h> +#include <linux/uaccess.h> +#include <net/sock.h> + +#include <linux/isdn/capilli.h> + +#include <asm/system.h> + +#include "cmtp.h" + +static int cmtp_sock_release(struct socket *sock) +{ + struct sock *sk = sock->sk; + + BT_DBG("sock %p sk %p", sock, sk); + + if (!sk) + return 0; + + sock_orphan(sk); + sock_put(sk); + + return 0; +} + +static int cmtp_sock_ioctl(struct socket *sock, unsigned int cmd, unsigned long arg) +{ + struct cmtp_connadd_req ca; + struct cmtp_conndel_req cd; + struct cmtp_connlist_req cl; + struct cmtp_conninfo ci; + struct socket *nsock; + void __user *argp = (void __user *)arg; + int err; + + BT_DBG("cmd %x arg %lx", cmd, arg); + + switch (cmd) { + case CMTPCONNADD: + if (!capable(CAP_NET_ADMIN)) + return -EACCES; + + if (copy_from_user(&ca, argp, sizeof(ca))) + return -EFAULT; + + nsock = sockfd_lookup(ca.sock, &err); + if (!nsock) + return err; + + if (nsock->sk->sk_state != BT_CONNECTED) { + sockfd_put(nsock); + return -EBADFD; + } + + err = cmtp_add_connection(&ca, nsock); + if (!err) { + if (copy_to_user(argp, &ca, sizeof(ca))) + err = -EFAULT; + } else + sockfd_put(nsock); + + return err; + + case CMTPCONNDEL: + if (!capable(CAP_NET_ADMIN)) + return -EACCES; + + if (copy_from_user(&cd, argp, sizeof(cd))) + return -EFAULT; + + return cmtp_del_connection(&cd); + + case CMTPGETCONNLIST: + if (copy_from_user(&cl, argp, sizeof(cl))) + return -EFAULT; + + if (cl.cnum <= 0) + return -EINVAL; + + err = cmtp_get_connlist(&cl); + if (!err && copy_to_user(argp, &cl, sizeof(cl))) + return -EFAULT; + + return err; + + case CMTPGETCONNINFO: + if (copy_from_user(&ci, argp, sizeof(ci))) + return -EFAULT; + + err = cmtp_get_conninfo(&ci); + if (!err && copy_to_user(argp, &ci, sizeof(ci))) + return -EFAULT; + + return err; + } + + return -EINVAL; +} + +#ifdef CONFIG_COMPAT +static int cmtp_sock_compat_ioctl(struct socket *sock, unsigned int cmd, unsigned long arg) +{ + if (cmd == CMTPGETCONNLIST) { + struct cmtp_connlist_req cl; + u32 uci; + int err; + + if (get_user(cl.cnum, (u32 __user *) arg) || + get_user(uci, (u32 __user *) (arg + 4))) + return -EFAULT; + + cl.ci = compat_ptr(uci); + + if (cl.cnum <= 0) + return -EINVAL; + + err = cmtp_get_connlist(&cl); + + if (!err && put_user(cl.cnum, (u32 __user *) arg)) + err = -EFAULT; + + return err; + } + + return cmtp_sock_ioctl(sock, cmd, arg); +} +#endif + +static const struct proto_ops cmtp_sock_ops = { + .family = PF_BLUETOOTH, + .owner = THIS_MODULE, + .release = cmtp_sock_release, + .ioctl = cmtp_sock_ioctl, +#ifdef CONFIG_COMPAT + .compat_ioctl = cmtp_sock_compat_ioctl, +#endif + .bind = sock_no_bind, + .getname = sock_no_getname, + .sendmsg = sock_no_sendmsg, + .recvmsg = sock_no_recvmsg, + .poll = sock_no_poll, + .listen = sock_no_listen, + .shutdown = sock_no_shutdown, + .setsockopt = sock_no_setsockopt, + .getsockopt = sock_no_getsockopt, + .connect = sock_no_connect, + .socketpair = sock_no_socketpair, + .accept = sock_no_accept, + .mmap = sock_no_mmap +}; + +static struct proto cmtp_proto = { + .name = "CMTP", + .owner = THIS_MODULE, + .obj_size = sizeof(struct bt_sock) +}; + +static int cmtp_sock_create(struct net *net, struct socket *sock, int protocol, + int kern) +{ + struct sock *sk; + + BT_DBG("sock %p", sock); + + if (sock->type != SOCK_RAW) + return -ESOCKTNOSUPPORT; + + sk = sk_alloc(net, PF_BLUETOOTH, GFP_ATOMIC, &cmtp_proto); + if (!sk) + return -ENOMEM; + + sock_init_data(sock, sk); + + sock->ops = &cmtp_sock_ops; + + sock->state = SS_UNCONNECTED; + + sock_reset_flag(sk, SOCK_ZAPPED); + + sk->sk_protocol = protocol; + sk->sk_state = BT_OPEN; + + return 0; +} + +static const struct net_proto_family cmtp_sock_family_ops = { + .family = PF_BLUETOOTH, + .owner = THIS_MODULE, + .create = cmtp_sock_create +}; + +int cmtp_init_sockets(void) +{ + int err; + + err = proto_register(&cmtp_proto, 0); + if (err < 0) + return err; + + err = bt_sock_register(BTPROTO_CMTP, &cmtp_sock_family_ops); + if (err < 0) + goto error; + + return 0; + +error: + BT_ERR("Can't register CMTP socket"); + proto_unregister(&cmtp_proto); + return err; +} + +void cmtp_cleanup_sockets(void) +{ + if (bt_sock_unregister(BTPROTO_CMTP) < 0) + BT_ERR("Can't unregister CMTP socket"); + + proto_unregister(&cmtp_proto); +} diff --git a/net/bluetooth_tizen/hci_conn.c b/net/bluetooth_tizen/hci_conn.c new file mode 100644 index 0000000..459227d --- /dev/null +++ b/net/bluetooth_tizen/hci_conn.c @@ -0,0 +1,984 @@ +/* + BlueZ - Bluetooth protocol stack for Linux + Copyright (c) 2000-2001, 2010, Code Aurora Forum. All rights reserved. + + Written 2000,2001 by Maxim Krasnyansky <maxk@qualcomm.com> + + This program is free software; you can redistribute it and/or modify + it under the terms of the GNU General Public License version 2 as + published by the Free Software Foundation; + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS + OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT OF THIRD PARTY RIGHTS. + IN NO EVENT SHALL THE COPYRIGHT HOLDER(S) AND AUTHOR(S) BE LIABLE FOR ANY + CLAIM, OR ANY SPECIAL INDIRECT OR CONSEQUENTIAL DAMAGES, OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + + ALL LIABILITY, INCLUDING LIABILITY FOR INFRINGEMENT OF ANY PATENTS, + COPYRIGHTS, TRADEMARKS OR OTHER RIGHTS, RELATING TO USE OF THIS + SOFTWARE IS DISCLAIMED. +*/ + +/* Bluetooth HCI connection handling. */ + +#include <linux/module.h> + +#include <linux/types.h> +#include <linux/errno.h> +#include <linux/kernel.h> +#include <linux/slab.h> +#include <linux/poll.h> +#include <linux/fcntl.h> +#include <linux/init.h> +#include <linux/skbuff.h> +#include <linux/interrupt.h> +#include <linux/notifier.h> +#include <net/sock.h> + +#include <asm/system.h> +#include <linux/uaccess.h> +#include <asm/unaligned.h> + +#include <net/bluetooth/bluetooth.h> +#include <net/bluetooth/hci_core.h> + +static void hci_le_connect(struct hci_conn *conn) +{ + struct hci_dev *hdev = conn->hdev; + struct hci_cp_le_create_conn cp; + + conn->state = BT_CONNECT; + conn->out = true; + conn->link_mode |= HCI_LM_MASTER; + conn->sec_level = BT_SECURITY_LOW; + + memset(&cp, 0, sizeof(cp)); + cp.scan_interval = cpu_to_le16(0x0060); + cp.scan_window = cpu_to_le16(0x0030); + bacpy(&cp.peer_addr, &conn->dst); + cp.peer_addr_type = conn->dst_type; + cp.conn_interval_min = cpu_to_le16(0x0028); + cp.conn_interval_max = cpu_to_le16(0x0038); + cp.supervision_timeout = cpu_to_le16(0x002a); + cp.min_ce_len = cpu_to_le16(0x0000); + cp.max_ce_len = cpu_to_le16(0x0000); + + hci_send_cmd(hdev, HCI_OP_LE_CREATE_CONN, sizeof(cp), &cp); +} + +static void hci_le_connect_cancel(struct hci_conn *conn) +{ + hci_send_cmd(conn->hdev, HCI_OP_LE_CREATE_CONN_CANCEL, 0, NULL); +} + +void hci_acl_connect(struct hci_conn *conn) +{ + struct hci_dev *hdev = conn->hdev; + struct inquiry_entry *ie; + struct hci_cp_create_conn cp; + + BT_DBG("hcon %p", conn); + + conn->state = BT_CONNECT; + conn->out = true; + + conn->link_mode = HCI_LM_MASTER; + + conn->attempt++; + + conn->link_policy = hdev->link_policy; + + memset(&cp, 0, sizeof(cp)); + bacpy(&cp.bdaddr, &conn->dst); + cp.pscan_rep_mode = 0x02; + + ie = hci_inquiry_cache_lookup(hdev, &conn->dst); + if (ie) { + if (inquiry_entry_age(ie) <= INQUIRY_ENTRY_AGE_MAX) { + cp.pscan_rep_mode = ie->data.pscan_rep_mode; + cp.pscan_mode = ie->data.pscan_mode; + cp.clock_offset = ie->data.clock_offset | + cpu_to_le16(0x8000); + } + + memcpy(conn->dev_class, ie->data.dev_class, 3); + if (ie->data.ssp_mode > 0) + set_bit(HCI_CONN_SSP_ENABLED, &conn->flags); + } + + cp.pkt_type = cpu_to_le16(conn->pkt_type); + if (lmp_rswitch_capable(hdev) && !(hdev->link_mode & HCI_LM_MASTER)) + cp.role_switch = 0x01; + else + cp.role_switch = 0x00; + + hci_send_cmd(hdev, HCI_OP_CREATE_CONN, sizeof(cp), &cp); +} + +static void hci_acl_connect_cancel(struct hci_conn *conn) +{ + struct hci_cp_create_conn_cancel cp; + + BT_DBG("%p", conn); + + if (conn->hdev->hci_ver < BLUETOOTH_VER_1_2) + return; + + bacpy(&cp.bdaddr, &conn->dst); + hci_send_cmd(conn->hdev, HCI_OP_CREATE_CONN_CANCEL, sizeof(cp), &cp); +} + +void hci_acl_disconn(struct hci_conn *conn, __u8 reason) +{ + struct hci_cp_disconnect cp; + + BT_DBG("%p", conn); + + conn->state = BT_DISCONN; + + cp.handle = cpu_to_le16(conn->handle); + cp.reason = reason; + hci_send_cmd(conn->hdev, HCI_OP_DISCONNECT, sizeof(cp), &cp); +} + +void hci_add_sco(struct hci_conn *conn, __u16 handle) +{ + struct hci_dev *hdev = conn->hdev; + struct hci_cp_add_sco cp; + + BT_DBG("%p", conn); + + conn->state = BT_CONNECT; + conn->out = true; + + conn->attempt++; + + cp.handle = cpu_to_le16(handle); + cp.pkt_type = cpu_to_le16(conn->pkt_type); + + hci_send_cmd(hdev, HCI_OP_ADD_SCO, sizeof(cp), &cp); +} + +void hci_setup_sync(struct hci_conn *conn, __u16 handle) +{ + struct hci_dev *hdev = conn->hdev; + struct hci_cp_setup_sync_conn cp; + + BT_DBG("%p", conn); + + conn->state = BT_CONNECT; + conn->out = true; + + conn->attempt++; + + cp.handle = cpu_to_le16(handle); + cp.pkt_type = cpu_to_le16(conn->pkt_type); + + cp.tx_bandwidth = cpu_to_le32(0x00001f40); + cp.rx_bandwidth = cpu_to_le32(0x00001f40); + cp.max_latency = cpu_to_le16(0xffff); + cp.voice_setting = cpu_to_le16(hdev->voice_setting); + cp.retrans_effort = 0xff; + + hci_send_cmd(hdev, HCI_OP_SETUP_SYNC_CONN, sizeof(cp), &cp); +} + +void hci_le_conn_update(struct hci_conn *conn, u16 min, u16 max, + u16 latency, u16 to_multiplier) +{ + struct hci_cp_le_conn_update cp; + struct hci_dev *hdev = conn->hdev; + + memset(&cp, 0, sizeof(cp)); + + cp.handle = cpu_to_le16(conn->handle); + cp.conn_interval_min = cpu_to_le16(min); + cp.conn_interval_max = cpu_to_le16(max); + cp.conn_latency = cpu_to_le16(latency); + cp.supervision_timeout = cpu_to_le16(to_multiplier); + cp.min_ce_len = cpu_to_le16(0x0001); + cp.max_ce_len = cpu_to_le16(0x0001); + + hci_send_cmd(hdev, HCI_OP_LE_CONN_UPDATE, sizeof(cp), &cp); +} +EXPORT_SYMBOL(hci_le_conn_update); + +void hci_le_start_enc(struct hci_conn *conn, __le16 ediv, __u8 rand[8], + __u8 ltk[16]) +{ + struct hci_dev *hdev = conn->hdev; + struct hci_cp_le_start_enc cp; + + BT_DBG("%p", conn); + + memset(&cp, 0, sizeof(cp)); + + cp.handle = cpu_to_le16(conn->handle); + memcpy(cp.ltk, ltk, sizeof(cp.ltk)); + cp.ediv = ediv; + memcpy(cp.rand, rand, sizeof(cp.rand)); + + hci_send_cmd(hdev, HCI_OP_LE_START_ENC, sizeof(cp), &cp); +} +EXPORT_SYMBOL(hci_le_start_enc); + +void hci_le_ltk_reply(struct hci_conn *conn, u8 ltk[16]) +{ + struct hci_dev *hdev = conn->hdev; + struct hci_cp_le_ltk_reply cp; + + BT_DBG("%p", conn); + + memset(&cp, 0, sizeof(cp)); + + cp.handle = cpu_to_le16(conn->handle); + memcpy(cp.ltk, ltk, sizeof(ltk)); + + hci_send_cmd(hdev, HCI_OP_LE_LTK_REPLY, sizeof(cp), &cp); +} +EXPORT_SYMBOL(hci_le_ltk_reply); + +void hci_le_ltk_neg_reply(struct hci_conn *conn) +{ + struct hci_dev *hdev = conn->hdev; + struct hci_cp_le_ltk_neg_reply cp; + + BT_DBG("%p", conn); + + memset(&cp, 0, sizeof(cp)); + + cp.handle = cpu_to_le16(conn->handle); + + hci_send_cmd(hdev, HCI_OP_LE_LTK_NEG_REPLY, sizeof(cp), &cp); +} + +/* Device _must_ be locked */ +void hci_sco_setup(struct hci_conn *conn, __u8 status) +{ + struct hci_conn *sco = conn->link; + + BT_DBG("%p", conn); + + if (!sco) + return; + + if (!status) { + if (lmp_esco_capable(conn->hdev)) + hci_setup_sync(sco, conn->handle); + else + hci_add_sco(sco, conn->handle); + } else { + hci_proto_connect_cfm(sco, status); + hci_conn_del(sco); + } +} + +static void hci_conn_timeout(struct work_struct *work) +{ + struct hci_conn *conn = container_of(work, struct hci_conn, + disc_work.work); + __u8 reason; + + BT_DBG("conn %p state %s", conn, state_to_string(conn->state)); + + if (atomic_read(&conn->refcnt)) + return; + + switch (conn->state) { + case BT_CONNECT: + case BT_CONNECT2: + if (conn->out) { + if (conn->type == ACL_LINK) + hci_acl_connect_cancel(conn); + else if (conn->type == LE_LINK) + hci_le_connect_cancel(conn); + } + break; + case BT_CONFIG: + case BT_CONNECTED: + reason = hci_proto_disconn_ind(conn); + hci_acl_disconn(conn, reason); + break; + default: + conn->state = BT_CLOSED; + break; + } +} + +/* Enter sniff mode */ +static void hci_conn_enter_sniff_mode(struct hci_conn *conn) +{ + struct hci_dev *hdev = conn->hdev; + + BT_DBG("conn %p mode %d", conn, conn->mode); + + if (test_bit(HCI_RAW, &hdev->flags)) + return; + + if (!lmp_sniff_capable(hdev) || !lmp_sniff_capable(conn)) + return; + + if (conn->mode != HCI_CM_ACTIVE || !(conn->link_policy & HCI_LP_SNIFF)) + return; + + if (lmp_sniffsubr_capable(hdev) && lmp_sniffsubr_capable(conn)) { + struct hci_cp_sniff_subrate cp; + cp.handle = cpu_to_le16(conn->handle); + cp.max_latency = cpu_to_le16(0); + cp.min_remote_timeout = cpu_to_le16(0); + cp.min_local_timeout = cpu_to_le16(0); + hci_send_cmd(hdev, HCI_OP_SNIFF_SUBRATE, sizeof(cp), &cp); + } + + if (!test_and_set_bit(HCI_CONN_MODE_CHANGE_PEND, &conn->flags)) { + struct hci_cp_sniff_mode cp; + cp.handle = cpu_to_le16(conn->handle); + cp.max_interval = cpu_to_le16(hdev->sniff_max_interval); + cp.min_interval = cpu_to_le16(hdev->sniff_min_interval); + cp.attempt = cpu_to_le16(4); + cp.timeout = cpu_to_le16(1); + hci_send_cmd(hdev, HCI_OP_SNIFF_MODE, sizeof(cp), &cp); + } +} + +static void hci_conn_idle(unsigned long arg) +{ + struct hci_conn *conn = (void *) arg; + + BT_DBG("conn %p mode %d", conn, conn->mode); + + hci_conn_enter_sniff_mode(conn); +} + +static void hci_conn_auto_accept(unsigned long arg) +{ + struct hci_conn *conn = (void *) arg; + struct hci_dev *hdev = conn->hdev; + + hci_send_cmd(hdev, HCI_OP_USER_CONFIRM_REPLY, sizeof(conn->dst), + &conn->dst); +} + +struct hci_conn *hci_conn_add(struct hci_dev *hdev, int type, bdaddr_t *dst) +{ + struct hci_conn *conn; + + BT_DBG("%s dst %s", hdev->name, batostr(dst)); + + conn = kzalloc(sizeof(struct hci_conn), GFP_KERNEL); + if (!conn) + return NULL; + + bacpy(&conn->dst, dst); + conn->hdev = hdev; + conn->type = type; + conn->mode = HCI_CM_ACTIVE; + conn->state = BT_OPEN; + conn->auth_type = HCI_AT_GENERAL_BONDING; + conn->io_capability = hdev->io_capability; + conn->remote_auth = 0xff; + conn->key_type = 0xff; + + set_bit(HCI_CONN_POWER_SAVE, &conn->flags); + conn->disc_timeout = HCI_DISCONN_TIMEOUT; + + switch (type) { + case ACL_LINK: + conn->pkt_type = hdev->pkt_type & ACL_PTYPE_MASK; + break; + case SCO_LINK: + if (lmp_esco_capable(hdev)) + conn->pkt_type = (hdev->esco_type & SCO_ESCO_MASK) | + (hdev->esco_type & EDR_ESCO_MASK); + else + conn->pkt_type = hdev->pkt_type & SCO_PTYPE_MASK; + break; + case ESCO_LINK: + conn->pkt_type = hdev->esco_type & ~EDR_ESCO_MASK; + break; + } + + skb_queue_head_init(&conn->data_q); + + INIT_LIST_HEAD(&conn->chan_list); + + INIT_DELAYED_WORK(&conn->disc_work, hci_conn_timeout); + setup_timer(&conn->idle_timer, hci_conn_idle, (unsigned long)conn); + setup_timer(&conn->auto_accept_timer, hci_conn_auto_accept, + (unsigned long) conn); + + atomic_set(&conn->refcnt, 0); + + hci_dev_hold(hdev); + + hci_conn_hash_add(hdev, conn); + if (hdev->notify) + hdev->notify(hdev, HCI_NOTIFY_CONN_ADD); + + atomic_set(&conn->devref, 0); + + hci_conn_init_sysfs(conn); + + return conn; +} + +int hci_conn_del(struct hci_conn *conn) +{ + struct hci_dev *hdev = conn->hdev; + + BT_DBG("%s conn %p handle %d", hdev->name, conn, conn->handle); + + del_timer(&conn->idle_timer); + + cancel_delayed_work_sync(&conn->disc_work); + + del_timer(&conn->auto_accept_timer); + + if (conn->type == ACL_LINK) { + struct hci_conn *sco = conn->link; + if (sco) + sco->link = NULL; + + /* Unacked frames */ + hdev->acl_cnt += conn->sent; + } else if (conn->type == LE_LINK) { + if (hdev->le_pkts) + hdev->le_cnt += conn->sent; + else + hdev->acl_cnt += conn->sent; + } else { + struct hci_conn *acl = conn->link; + if (acl) { + acl->link = NULL; + hci_conn_put(acl); + } + } + + + hci_chan_list_flush(conn); + + hci_conn_hash_del(hdev, conn); + if (hdev->notify) + hdev->notify(hdev, HCI_NOTIFY_CONN_DEL); + + skb_queue_purge(&conn->data_q); + + hci_conn_put_device(conn); + + hci_dev_put(hdev); + + if (conn->handle == 0) + kfree(conn); + + return 0; +} + +struct hci_dev *hci_get_route(bdaddr_t *dst, bdaddr_t *src) +{ + int use_src = bacmp(src, BDADDR_ANY); + struct hci_dev *hdev = NULL, *d; + + BT_DBG("%s -> %s", batostr(src), batostr(dst)); + + read_lock(&hci_dev_list_lock); + + list_for_each_entry(d, &hci_dev_list, list) { + if (!test_bit(HCI_UP, &d->flags) || test_bit(HCI_RAW, &d->flags)) + continue; + + /* Simple routing: + * No source address - find interface with bdaddr != dst + * Source address - find interface with bdaddr == src + */ + + if (use_src) { + if (!bacmp(&d->bdaddr, src)) { + hdev = d; break; + } + } else { + if (bacmp(&d->bdaddr, dst)) { + hdev = d; break; + } + } + } + + if (hdev) + hdev = hci_dev_hold(hdev); + + read_unlock(&hci_dev_list_lock); + return hdev; +} +EXPORT_SYMBOL(hci_get_route); + +/* Create SCO, ACL or LE connection. + * Device _must_ be locked */ +struct hci_conn *hci_connect(struct hci_dev *hdev, int type, bdaddr_t *dst, __u8 sec_level, __u8 auth_type) +{ + struct hci_conn *acl; + struct hci_conn *sco; + struct hci_conn *le; + + BT_DBG("%s dst %s", hdev->name, batostr(dst)); + + if (type == LE_LINK) { + struct adv_entry *entry; + + le = hci_conn_hash_lookup_ba(hdev, LE_LINK, dst); + if (le) + return ERR_PTR(-EBUSY); + + entry = hci_find_adv_entry(hdev, dst); + if (!entry) + return ERR_PTR(-EHOSTUNREACH); + + le = hci_conn_add(hdev, LE_LINK, dst); + if (!le) + return ERR_PTR(-ENOMEM); + + le->dst_type = entry->bdaddr_type; + + hci_le_connect(le); + + hci_conn_hold(le); + + return le; + } + + acl = hci_conn_hash_lookup_ba(hdev, ACL_LINK, dst); + if (!acl) { + acl = hci_conn_add(hdev, ACL_LINK, dst); + if (!acl) + return ERR_PTR(-ENOMEM); + } + + hci_conn_hold(acl); + + if (acl->state == BT_OPEN || acl->state == BT_CLOSED) { + acl->sec_level = BT_SECURITY_LOW; + acl->pending_sec_level = sec_level; + acl->auth_type = auth_type; + hci_acl_connect(acl); + } + + if (type == ACL_LINK) + return acl; + + sco = hci_conn_hash_lookup_ba(hdev, type, dst); + if (!sco) { + sco = hci_conn_add(hdev, type, dst); + if (!sco) { + hci_conn_put(acl); + return ERR_PTR(-ENOMEM); + } + } + + acl->link = sco; + sco->link = acl; + + hci_conn_hold(sco); + + if (acl->state == BT_CONNECTED && + (sco->state == BT_OPEN || sco->state == BT_CLOSED)) { + set_bit(HCI_CONN_POWER_SAVE, &acl->flags); + hci_conn_enter_active_mode(acl, BT_POWER_FORCE_ACTIVE_ON); + + if (test_bit(HCI_CONN_MODE_CHANGE_PEND, &acl->flags)) { + /* defer SCO setup until mode change completed */ + set_bit(HCI_CONN_SCO_SETUP_PEND, &acl->flags); + return sco; + } + + hci_sco_setup(acl, 0x00); + } + + return sco; +} +EXPORT_SYMBOL(hci_connect); + +/* Check link security requirement */ +int hci_conn_check_link_mode(struct hci_conn *conn) +{ + BT_DBG("conn %p", conn); + + if (hci_conn_ssp_enabled(conn) && !(conn->link_mode & HCI_LM_ENCRYPT)) + return 0; + + return 1; +} +EXPORT_SYMBOL(hci_conn_check_link_mode); + +/* Authenticate remote device */ +static int hci_conn_auth(struct hci_conn *conn, __u8 sec_level, __u8 auth_type) +{ + BT_DBG("conn %p", conn); + + if (conn->pending_sec_level > sec_level) + sec_level = conn->pending_sec_level; + + if (sec_level > conn->sec_level) + conn->pending_sec_level = sec_level; + else if (conn->link_mode & HCI_LM_AUTH) + return 1; + + /* Make sure we preserve an existing MITM requirement*/ + auth_type |= (conn->auth_type & 0x01); + + conn->auth_type = auth_type; + + if (!test_and_set_bit(HCI_CONN_AUTH_PEND, &conn->flags)) { + struct hci_cp_auth_requested cp; + + /* encrypt must be pending if auth is also pending */ + set_bit(HCI_CONN_ENCRYPT_PEND, &conn->flags); + + cp.handle = cpu_to_le16(conn->handle); + hci_send_cmd(conn->hdev, HCI_OP_AUTH_REQUESTED, + sizeof(cp), &cp); + if (conn->key_type != 0xff) + set_bit(HCI_CONN_REAUTH_PEND, &conn->flags); + } + + return 0; +} + +/* Encrypt the the link */ +static void hci_conn_encrypt(struct hci_conn *conn) +{ + BT_DBG("conn %p", conn); + + if (!test_and_set_bit(HCI_CONN_ENCRYPT_PEND, &conn->flags)) { + struct hci_cp_set_conn_encrypt cp; + cp.handle = cpu_to_le16(conn->handle); + cp.encrypt = 0x01; + hci_send_cmd(conn->hdev, HCI_OP_SET_CONN_ENCRYPT, sizeof(cp), + &cp); + } +} + +/* Enable security */ +int hci_conn_security(struct hci_conn *conn, __u8 sec_level, __u8 auth_type) +{ + BT_DBG("conn %p", conn); + + /* For sdp we don't need the link key. */ + if (sec_level == BT_SECURITY_SDP) + return 1; + + /* For non 2.1 devices and low security level we don't need the link + key. */ + if (sec_level == BT_SECURITY_LOW && !hci_conn_ssp_enabled(conn)) + return 1; + + /* For other security levels we need the link key. */ + if (!(conn->link_mode & HCI_LM_AUTH)) + goto auth; + + /* An authenticated combination key has sufficient security for any + security level. */ + if (conn->key_type == HCI_LK_AUTH_COMBINATION) + goto encrypt; + + /* An unauthenticated combination key has sufficient security for + security level 1 and 2. */ + if (conn->key_type == HCI_LK_UNAUTH_COMBINATION && + (sec_level == BT_SECURITY_MEDIUM || + sec_level == BT_SECURITY_LOW)) + goto encrypt; + + /* A combination key has always sufficient security for the security + levels 1 or 2. High security level requires the combination key + is generated using maximum PIN code length (16). + For pre 2.1 units. */ + if (conn->key_type == HCI_LK_COMBINATION && + (sec_level != BT_SECURITY_HIGH || + conn->pin_length == 16)) + goto encrypt; + +auth: + if (test_bit(HCI_CONN_ENCRYPT_PEND, &conn->flags)) + return 0; + + if (!hci_conn_auth(conn, sec_level, auth_type)) + return 0; + +encrypt: + if (conn->link_mode & HCI_LM_ENCRYPT) + return 1; + + hci_conn_encrypt(conn); + return 0; +} +EXPORT_SYMBOL(hci_conn_security); + +/* Check secure link requirement */ +int hci_conn_check_secure(struct hci_conn *conn, __u8 sec_level) +{ + BT_DBG("conn %p", conn); + + if (sec_level != BT_SECURITY_HIGH) + return 1; /* Accept if non-secure is required */ + + if (conn->sec_level == BT_SECURITY_HIGH) + return 1; + + return 0; /* Reject not secure link */ +} +EXPORT_SYMBOL(hci_conn_check_secure); + +/* Change link key */ +int hci_conn_change_link_key(struct hci_conn *conn) +{ + BT_DBG("conn %p", conn); + + if (!test_and_set_bit(HCI_CONN_AUTH_PEND, &conn->flags)) { + struct hci_cp_change_conn_link_key cp; + cp.handle = cpu_to_le16(conn->handle); + hci_send_cmd(conn->hdev, HCI_OP_CHANGE_CONN_LINK_KEY, + sizeof(cp), &cp); + } + + return 0; +} +EXPORT_SYMBOL(hci_conn_change_link_key); + +/* Switch role */ +int hci_conn_switch_role(struct hci_conn *conn, __u8 role) +{ + BT_DBG("conn %p", conn); + + if (!role && conn->link_mode & HCI_LM_MASTER) + return 1; + + if (!test_and_set_bit(HCI_CONN_RSWITCH_PEND, &conn->flags)) { + struct hci_cp_switch_role cp; + bacpy(&cp.bdaddr, &conn->dst); + cp.role = role; + hci_send_cmd(conn->hdev, HCI_OP_SWITCH_ROLE, sizeof(cp), &cp); + } + + return 0; +} +EXPORT_SYMBOL(hci_conn_switch_role); + +/* Enter active mode */ +void hci_conn_enter_active_mode(struct hci_conn *conn, __u8 force_active) +{ + struct hci_dev *hdev = conn->hdev; + + BT_DBG("conn %p mode %d", conn, conn->mode); + + if (test_bit(HCI_RAW, &hdev->flags)) + return; + + if (conn->mode != HCI_CM_SNIFF) + goto timer; + + if (!test_bit(HCI_CONN_POWER_SAVE, &conn->flags) && !force_active) + goto timer; + + if (!test_and_set_bit(HCI_CONN_MODE_CHANGE_PEND, &conn->flags)) { + struct hci_cp_exit_sniff_mode cp; + cp.handle = cpu_to_le16(conn->handle); + hci_send_cmd(hdev, HCI_OP_EXIT_SNIFF_MODE, sizeof(cp), &cp); + } + +timer: + if (hdev->idle_timeout > 0) + mod_timer(&conn->idle_timer, + jiffies + msecs_to_jiffies(hdev->idle_timeout)); +} + +/* Drop all connection on the device */ +void hci_conn_hash_flush(struct hci_dev *hdev) +{ + struct hci_conn_hash *h = &hdev->conn_hash; + struct hci_conn *c, *n; + + BT_DBG("hdev %s", hdev->name); + + list_for_each_entry_safe(c, n, &h->list, list) { + c->state = BT_CLOSED; + + hci_proto_disconn_cfm(c, HCI_ERROR_LOCAL_HOST_TERM); + hci_conn_del(c); + } +} + +/* Check pending connect attempts */ +void hci_conn_check_pending(struct hci_dev *hdev) +{ + struct hci_conn *conn; + + BT_DBG("hdev %s", hdev->name); + + hci_dev_lock(hdev); + + conn = hci_conn_hash_lookup_state(hdev, ACL_LINK, BT_CONNECT2); + if (conn) + hci_acl_connect(conn); + + hci_dev_unlock(hdev); +} + +void hci_conn_hold_device(struct hci_conn *conn) +{ + atomic_inc(&conn->devref); +} +EXPORT_SYMBOL(hci_conn_hold_device); + +void hci_conn_put_device(struct hci_conn *conn) +{ + if (atomic_dec_and_test(&conn->devref)) + hci_conn_del_sysfs(conn); +} +EXPORT_SYMBOL(hci_conn_put_device); + +int hci_get_conn_list(void __user *arg) +{ + register struct hci_conn *c; + struct hci_conn_list_req req, *cl; + struct hci_conn_info *ci; + struct hci_dev *hdev; + int n = 0, size, err; + + if (copy_from_user(&req, arg, sizeof(req))) + return -EFAULT; + + if (!req.conn_num || req.conn_num > (PAGE_SIZE * 2) / sizeof(*ci)) + return -EINVAL; + + size = sizeof(req) + req.conn_num * sizeof(*ci); + + cl = kmalloc(size, GFP_KERNEL); + if (!cl) + return -ENOMEM; + + hdev = hci_dev_get(req.dev_id); + if (!hdev) { + kfree(cl); + return -ENODEV; + } + + ci = cl->conn_info; + + hci_dev_lock(hdev); + list_for_each_entry(c, &hdev->conn_hash.list, list) { + bacpy(&(ci + n)->bdaddr, &c->dst); + (ci + n)->handle = c->handle; + (ci + n)->type = c->type; + (ci + n)->out = c->out; + (ci + n)->state = c->state; + (ci + n)->link_mode = c->link_mode; + if (++n >= req.conn_num) + break; + } + hci_dev_unlock(hdev); + + cl->dev_id = hdev->id; + cl->conn_num = n; + size = sizeof(req) + n * sizeof(*ci); + + hci_dev_put(hdev); + + err = copy_to_user(arg, cl, size); + kfree(cl); + + return err ? -EFAULT : 0; +} + +int hci_get_conn_info(struct hci_dev *hdev, void __user *arg) +{ + struct hci_conn_info_req req; + struct hci_conn_info ci; + struct hci_conn *conn; + char __user *ptr = arg + sizeof(req); + + if (copy_from_user(&req, arg, sizeof(req))) + return -EFAULT; + + hci_dev_lock(hdev); + conn = hci_conn_hash_lookup_ba(hdev, req.type, &req.bdaddr); + if (conn) { + bacpy(&ci.bdaddr, &conn->dst); + ci.handle = conn->handle; + ci.type = conn->type; + ci.out = conn->out; + ci.state = conn->state; + ci.link_mode = conn->link_mode; + } + hci_dev_unlock(hdev); + + if (!conn) + return -ENOENT; + + return copy_to_user(ptr, &ci, sizeof(ci)) ? -EFAULT : 0; +} + +int hci_get_auth_info(struct hci_dev *hdev, void __user *arg) +{ + struct hci_auth_info_req req; + struct hci_conn *conn; + + if (copy_from_user(&req, arg, sizeof(req))) + return -EFAULT; + + hci_dev_lock(hdev); + conn = hci_conn_hash_lookup_ba(hdev, ACL_LINK, &req.bdaddr); + if (conn) + req.type = conn->auth_type; + hci_dev_unlock(hdev); + + if (!conn) + return -ENOENT; + + return copy_to_user(arg, &req, sizeof(req)) ? -EFAULT : 0; +} + +struct hci_chan *hci_chan_create(struct hci_conn *conn) +{ + struct hci_dev *hdev = conn->hdev; + struct hci_chan *chan; + + BT_DBG("%s conn %p", hdev->name, conn); + + chan = kzalloc(sizeof(struct hci_chan), GFP_KERNEL); + if (!chan) + return NULL; + + chan->conn = conn; + skb_queue_head_init(&chan->data_q); + + list_add_rcu(&chan->list, &conn->chan_list); + + return chan; +} + +int hci_chan_del(struct hci_chan *chan) +{ + struct hci_conn *conn = chan->conn; + struct hci_dev *hdev = conn->hdev; + + BT_DBG("%s conn %p chan %p", hdev->name, conn, chan); + + list_del_rcu(&chan->list); + + synchronize_rcu(); + + skb_queue_purge(&chan->data_q); + kfree(chan); + + return 0; +} + +void hci_chan_list_flush(struct hci_conn *conn) +{ + struct hci_chan *chan, *n; + + BT_DBG("conn %p", conn); + + list_for_each_entry_safe(chan, n, &conn->chan_list, list) + hci_chan_del(chan); +} diff --git a/net/bluetooth_tizen/hci_core.c b/net/bluetooth_tizen/hci_core.c new file mode 100644 index 0000000..1c69c54 --- /dev/null +++ b/net/bluetooth_tizen/hci_core.c @@ -0,0 +1,2966 @@ +/* + BlueZ - Bluetooth protocol stack for Linux + Copyright (C) 2000-2001 Qualcomm Incorporated + Copyright (C) 2011 ProFUSION Embedded Systems + + Written 2000,2001 by Maxim Krasnyansky <maxk@qualcomm.com> + + This program is free software; you can redistribute it and/or modify + it under the terms of the GNU General Public License version 2 as + published by the Free Software Foundation; + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS + OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT OF THIRD PARTY RIGHTS. + IN NO EVENT SHALL THE COPYRIGHT HOLDER(S) AND AUTHOR(S) BE LIABLE FOR ANY + CLAIM, OR ANY SPECIAL INDIRECT OR CONSEQUENTIAL DAMAGES, OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + + ALL LIABILITY, INCLUDING LIABILITY FOR INFRINGEMENT OF ANY PATENTS, + COPYRIGHTS, TRADEMARKS OR OTHER RIGHTS, RELATING TO USE OF THIS + SOFTWARE IS DISCLAIMED. +*/ + +/* Bluetooth HCI core. */ + +#include <linux/jiffies.h> +#include <linux/module.h> +#include <linux/kmod.h> + +#include <linux/types.h> +#include <linux/errno.h> +#include <linux/kernel.h> +#include <linux/sched.h> +#include <linux/slab.h> +#include <linux/poll.h> +#include <linux/fcntl.h> +#include <linux/init.h> +#include <linux/skbuff.h> +#include <linux/workqueue.h> +#include <linux/interrupt.h> +#include <linux/notifier.h> +#include <linux/rfkill.h> +#include <linux/timer.h> +#include <linux/crypto.h> +#include <net/sock.h> + +#include <asm/system.h> +#include <linux/uaccess.h> +#include <asm/unaligned.h> + +#include <net/bluetooth/bluetooth.h> +#include <net/bluetooth/hci_core.h> + +#define AUTO_OFF_TIMEOUT 2000 + +static void hci_rx_work(struct work_struct *work); +static void hci_cmd_work(struct work_struct *work); +static void hci_tx_work(struct work_struct *work); + +/* HCI device list */ +LIST_HEAD(hci_dev_list); +DEFINE_RWLOCK(hci_dev_list_lock); + +/* HCI callback list */ +LIST_HEAD(hci_cb_list); +DEFINE_RWLOCK(hci_cb_list_lock); + +/* HCI notifiers list */ +static ATOMIC_NOTIFIER_HEAD(hci_notifier); + +/* ---- HCI notifications ---- */ + +int hci_register_notifier(struct notifier_block *nb) +{ + return atomic_notifier_chain_register(&hci_notifier, nb); +} + +int hci_unregister_notifier(struct notifier_block *nb) +{ + return atomic_notifier_chain_unregister(&hci_notifier, nb); +} + +static void hci_notify(struct hci_dev *hdev, int event) +{ + atomic_notifier_call_chain(&hci_notifier, event, hdev); +} + +/* ---- HCI requests ---- */ + +void hci_req_complete(struct hci_dev *hdev, __u16 cmd, int result) +{ + BT_DBG("%s command 0x%04x result 0x%2.2x", hdev->name, cmd, result); + + /* If this is the init phase check if the completed command matches + * the last init command, and if not just return. + */ + if (test_bit(HCI_INIT, &hdev->flags) && hdev->init_last_cmd != cmd) { + struct hci_command_hdr *sent = (void *) hdev->sent_cmd->data; + struct sk_buff *skb; + + /* Some CSR based controllers generate a spontaneous + * reset complete event during init and any pending + * command will never be completed. In such a case we + * need to resend whatever was the last sent + * command. + */ + + if (cmd != HCI_OP_RESET || sent->opcode == HCI_OP_RESET) + return; + + skb = skb_clone(hdev->sent_cmd, GFP_ATOMIC); + if (skb) { + skb_queue_head(&hdev->cmd_q, skb); + queue_work(hdev->workqueue, &hdev->cmd_work); + } + + return; + } + + if (hdev->req_status == HCI_REQ_PEND) { + hdev->req_result = result; + hdev->req_status = HCI_REQ_DONE; + wake_up_interruptible(&hdev->req_wait_q); + } +} + +static void hci_req_cancel(struct hci_dev *hdev, int err) +{ + BT_DBG("%s err 0x%2.2x", hdev->name, err); + + if (hdev->req_status == HCI_REQ_PEND) { + hdev->req_result = err; + hdev->req_status = HCI_REQ_CANCELED; + wake_up_interruptible(&hdev->req_wait_q); + } +} + +/* Execute request and wait for completion. */ +static int __hci_request(struct hci_dev *hdev, void (*req)(struct hci_dev *hdev, unsigned long opt), + unsigned long opt, __u32 timeout) +{ + DECLARE_WAITQUEUE(wait, current); + int err = 0; + + BT_DBG("%s start", hdev->name); + + hdev->req_status = HCI_REQ_PEND; + + add_wait_queue(&hdev->req_wait_q, &wait); + set_current_state(TASK_INTERRUPTIBLE); + + req(hdev, opt); + schedule_timeout(timeout); + + remove_wait_queue(&hdev->req_wait_q, &wait); + + if (signal_pending(current)) + return -EINTR; + + switch (hdev->req_status) { + case HCI_REQ_DONE: + err = -bt_to_errno(hdev->req_result); + break; + + case HCI_REQ_CANCELED: + err = -hdev->req_result; + break; + + default: + err = -ETIMEDOUT; + break; + } + + hdev->req_status = hdev->req_result = 0; + + BT_DBG("%s end: err %d", hdev->name, err); + + return err; +} + +static inline int hci_request(struct hci_dev *hdev, void (*req)(struct hci_dev *hdev, unsigned long opt), + unsigned long opt, __u32 timeout) +{ + int ret; + + if (!test_bit(HCI_UP, &hdev->flags)) + return -ENETDOWN; + + /* Serialize all requests */ + hci_req_lock(hdev); + ret = __hci_request(hdev, req, opt, timeout); + hci_req_unlock(hdev); + + return ret; +} + +static void hci_reset_req(struct hci_dev *hdev, unsigned long opt) +{ + BT_DBG("%s %ld", hdev->name, opt); + + /* Reset device */ + set_bit(HCI_RESET, &hdev->flags); + hci_send_cmd(hdev, HCI_OP_RESET, 0, NULL); +} + +static void bredr_init(struct hci_dev *hdev) +{ + struct hci_cp_delete_stored_link_key cp; + __le16 param; + __u8 flt_type; + + hdev->flow_ctl_mode = HCI_FLOW_CTL_MODE_PACKET_BASED; + + /* Mandatory initialization */ + + /* Reset */ + if (!test_bit(HCI_QUIRK_NO_RESET, &hdev->quirks)) { + set_bit(HCI_RESET, &hdev->flags); + hci_send_cmd(hdev, HCI_OP_RESET, 0, NULL); + } + + /* Read Local Supported Features */ + hci_send_cmd(hdev, HCI_OP_READ_LOCAL_FEATURES, 0, NULL); + + /* Read Local Version */ + hci_send_cmd(hdev, HCI_OP_READ_LOCAL_VERSION, 0, NULL); + + /* Read Buffer Size (ACL mtu, max pkt, etc.) */ + hci_send_cmd(hdev, HCI_OP_READ_BUFFER_SIZE, 0, NULL); + + /* Read BD Address */ + hci_send_cmd(hdev, HCI_OP_READ_BD_ADDR, 0, NULL); + + /* Read Class of Device */ + hci_send_cmd(hdev, HCI_OP_READ_CLASS_OF_DEV, 0, NULL); + + /* Read Local Name */ + hci_send_cmd(hdev, HCI_OP_READ_LOCAL_NAME, 0, NULL); + + /* Read Voice Setting */ + hci_send_cmd(hdev, HCI_OP_READ_VOICE_SETTING, 0, NULL); + + /* Optional initialization */ + + /* Clear Event Filters */ + flt_type = HCI_FLT_CLEAR_ALL; + hci_send_cmd(hdev, HCI_OP_SET_EVENT_FLT, 1, &flt_type); + + /* Connection accept timeout ~20 secs */ + param = cpu_to_le16(0x7d00); + hci_send_cmd(hdev, HCI_OP_WRITE_CA_TIMEOUT, 2, ¶m); + + bacpy(&cp.bdaddr, BDADDR_ANY); + cp.delete_all = 1; + hci_send_cmd(hdev, HCI_OP_DELETE_STORED_LINK_KEY, sizeof(cp), &cp); +} + +static void amp_init(struct hci_dev *hdev) +{ + hdev->flow_ctl_mode = HCI_FLOW_CTL_MODE_BLOCK_BASED; + + /* Reset */ + hci_send_cmd(hdev, HCI_OP_RESET, 0, NULL); + + /* Read Local Version */ + hci_send_cmd(hdev, HCI_OP_READ_LOCAL_VERSION, 0, NULL); +} + +static void hci_init_req(struct hci_dev *hdev, unsigned long opt) +{ + struct sk_buff *skb; + + BT_DBG("%s %ld", hdev->name, opt); + + /* Driver initialization */ + + /* Special commands */ + while ((skb = skb_dequeue(&hdev->driver_init))) { + bt_cb(skb)->pkt_type = HCI_COMMAND_PKT; + skb->dev = (void *) hdev; + + skb_queue_tail(&hdev->cmd_q, skb); + queue_work(hdev->workqueue, &hdev->cmd_work); + } + skb_queue_purge(&hdev->driver_init); + + switch (hdev->dev_type) { + case HCI_BREDR: + bredr_init(hdev); + break; + + case HCI_AMP: + amp_init(hdev); + break; + + default: + BT_ERR("Unknown device type %d", hdev->dev_type); + break; + } + +} + +static void hci_le_init_req(struct hci_dev *hdev, unsigned long opt) +{ + BT_DBG("%s", hdev->name); + + /* Read LE buffer size */ + hci_send_cmd(hdev, HCI_OP_LE_READ_BUFFER_SIZE, 0, NULL); +} + +static void hci_scan_req(struct hci_dev *hdev, unsigned long opt) +{ + __u8 scan = opt; + + BT_DBG("%s %x", hdev->name, scan); + + /* Inquiry and Page scans */ + hci_send_cmd(hdev, HCI_OP_WRITE_SCAN_ENABLE, 1, &scan); +} + +static void hci_auth_req(struct hci_dev *hdev, unsigned long opt) +{ + __u8 auth = opt; + + BT_DBG("%s %x", hdev->name, auth); + + /* Authentication */ + hci_send_cmd(hdev, HCI_OP_WRITE_AUTH_ENABLE, 1, &auth); +} + +static void hci_encrypt_req(struct hci_dev *hdev, unsigned long opt) +{ + __u8 encrypt = opt; + + BT_DBG("%s %x", hdev->name, encrypt); + + /* Encryption */ + hci_send_cmd(hdev, HCI_OP_WRITE_ENCRYPT_MODE, 1, &encrypt); +} + +static void hci_linkpol_req(struct hci_dev *hdev, unsigned long opt) +{ + __le16 policy = cpu_to_le16(opt); + + BT_DBG("%s %x", hdev->name, policy); + + /* Default link policy */ + hci_send_cmd(hdev, HCI_OP_WRITE_DEF_LINK_POLICY, 2, &policy); +} + +/* Get HCI device by index. + * Device is held on return. */ +struct hci_dev *hci_dev_get(int index) +{ + struct hci_dev *hdev = NULL, *d; + + BT_DBG("%d", index); + + if (index < 0) + return NULL; + + read_lock(&hci_dev_list_lock); + list_for_each_entry(d, &hci_dev_list, list) { + if (d->id == index) { + hdev = hci_dev_hold(d); + break; + } + } + read_unlock(&hci_dev_list_lock); + return hdev; +} + +/* ---- Inquiry support ---- */ + +bool hci_discovery_active(struct hci_dev *hdev) +{ + struct discovery_state *discov = &hdev->discovery; + + switch (discov->state) { + case DISCOVERY_FINDING: + case DISCOVERY_RESOLVING: + return true; + + default: + return false; + } +} + +void hci_discovery_set_state(struct hci_dev *hdev, int state) +{ + BT_DBG("%s state %u -> %u", hdev->name, hdev->discovery.state, state); + + if (hdev->discovery.state == state) + return; + + switch (state) { + case DISCOVERY_STOPPED: + if (hdev->discovery.state != DISCOVERY_STARTING) + mgmt_discovering(hdev, 0); + hdev->discovery.type = 0; + break; + case DISCOVERY_STARTING: + break; + case DISCOVERY_FINDING: + mgmt_discovering(hdev, 1); + break; + case DISCOVERY_RESOLVING: + break; + case DISCOVERY_STOPPING: + break; + } + + hdev->discovery.state = state; +} + +static void inquiry_cache_flush(struct hci_dev *hdev) +{ + struct discovery_state *cache = &hdev->discovery; + struct inquiry_entry *p, *n; + + list_for_each_entry_safe(p, n, &cache->all, all) { + list_del(&p->all); + kfree(p); + } + + INIT_LIST_HEAD(&cache->unknown); + INIT_LIST_HEAD(&cache->resolve); +} + +struct inquiry_entry *hci_inquiry_cache_lookup(struct hci_dev *hdev, bdaddr_t *bdaddr) +{ + struct discovery_state *cache = &hdev->discovery; + struct inquiry_entry *e; + + BT_DBG("cache %p, %s", cache, batostr(bdaddr)); + + list_for_each_entry(e, &cache->all, all) { + if (!bacmp(&e->data.bdaddr, bdaddr)) + return e; + } + + return NULL; +} + +struct inquiry_entry *hci_inquiry_cache_lookup_unknown(struct hci_dev *hdev, + bdaddr_t *bdaddr) +{ + struct discovery_state *cache = &hdev->discovery; + struct inquiry_entry *e; + + BT_DBG("cache %p, %s", cache, batostr(bdaddr)); + + list_for_each_entry(e, &cache->unknown, list) { + if (!bacmp(&e->data.bdaddr, bdaddr)) + return e; + } + + return NULL; +} + +struct inquiry_entry *hci_inquiry_cache_lookup_resolve(struct hci_dev *hdev, + bdaddr_t *bdaddr, + int state) +{ + struct discovery_state *cache = &hdev->discovery; + struct inquiry_entry *e; + + BT_DBG("cache %p bdaddr %s state %d", cache, batostr(bdaddr), state); + + list_for_each_entry(e, &cache->resolve, list) { + if (!bacmp(bdaddr, BDADDR_ANY) && e->name_state == state) + return e; + if (!bacmp(&e->data.bdaddr, bdaddr)) + return e; + } + + return NULL; +} + +void hci_inquiry_cache_update_resolve(struct hci_dev *hdev, + struct inquiry_entry *ie) +{ + struct discovery_state *cache = &hdev->discovery; + struct list_head *pos = &cache->resolve; + struct inquiry_entry *p; + + list_del(&ie->list); + + list_for_each_entry(p, &cache->resolve, list) { + if (p->name_state != NAME_PENDING && + abs(p->data.rssi) >= abs(ie->data.rssi)) + break; + pos = &p->list; + } + + list_add(&ie->list, pos); +} + +bool hci_inquiry_cache_update(struct hci_dev *hdev, struct inquiry_data *data, + bool name_known, bool *ssp) +{ + struct discovery_state *cache = &hdev->discovery; + struct inquiry_entry *ie; + + BT_DBG("cache %p, %s", cache, batostr(&data->bdaddr)); + + if (ssp) + *ssp = data->ssp_mode; + + ie = hci_inquiry_cache_lookup(hdev, &data->bdaddr); + if (ie) { + if (ie->data.ssp_mode && ssp) + *ssp = true; + + if (ie->name_state == NAME_NEEDED && + data->rssi != ie->data.rssi) { + ie->data.rssi = data->rssi; + hci_inquiry_cache_update_resolve(hdev, ie); + } + + goto update; + } + + /* Entry not in the cache. Add new one. */ + ie = kzalloc(sizeof(struct inquiry_entry), GFP_ATOMIC); + if (!ie) + return false; + + list_add(&ie->all, &cache->all); + + if (name_known) { + ie->name_state = NAME_KNOWN; + } else { + ie->name_state = NAME_NOT_KNOWN; + list_add(&ie->list, &cache->unknown); + } + +update: + if (name_known && ie->name_state != NAME_KNOWN && + ie->name_state != NAME_PENDING) { + ie->name_state = NAME_KNOWN; + list_del(&ie->list); + } + + memcpy(&ie->data, data, sizeof(*data)); + ie->timestamp = jiffies; + cache->timestamp = jiffies; + + if (ie->name_state == NAME_NOT_KNOWN) + return false; + + return true; +} + +static int inquiry_cache_dump(struct hci_dev *hdev, int num, __u8 *buf) +{ + struct discovery_state *cache = &hdev->discovery; + struct inquiry_info *info = (struct inquiry_info *) buf; + struct inquiry_entry *e; + int copied = 0; + + list_for_each_entry(e, &cache->all, all) { + struct inquiry_data *data = &e->data; + + if (copied >= num) + break; + + bacpy(&info->bdaddr, &data->bdaddr); + info->pscan_rep_mode = data->pscan_rep_mode; + info->pscan_period_mode = data->pscan_period_mode; + info->pscan_mode = data->pscan_mode; + memcpy(info->dev_class, data->dev_class, 3); + info->clock_offset = data->clock_offset; + + info++; + copied++; + } + + BT_DBG("cache %p, copied %d", cache, copied); + return copied; +} + +static void hci_inq_req(struct hci_dev *hdev, unsigned long opt) +{ + struct hci_inquiry_req *ir = (struct hci_inquiry_req *) opt; + struct hci_cp_inquiry cp; + + BT_DBG("%s", hdev->name); + + if (test_bit(HCI_INQUIRY, &hdev->flags)) + return; + + /* Start Inquiry */ + memcpy(&cp.lap, &ir->lap, 3); + cp.length = ir->length; + cp.num_rsp = ir->num_rsp; + hci_send_cmd(hdev, HCI_OP_INQUIRY, sizeof(cp), &cp); +} + +int hci_inquiry(void __user *arg) +{ + __u8 __user *ptr = arg; + struct hci_inquiry_req ir; + struct hci_dev *hdev; + int err = 0, do_inquiry = 0, max_rsp; + long timeo; + __u8 *buf; + + if (copy_from_user(&ir, ptr, sizeof(ir))) + return -EFAULT; + + hdev = hci_dev_get(ir.dev_id); + if (!hdev) + return -ENODEV; + + hci_dev_lock(hdev); + if (inquiry_cache_age(hdev) > INQUIRY_CACHE_AGE_MAX || + inquiry_cache_empty(hdev) || + ir.flags & IREQ_CACHE_FLUSH) { + inquiry_cache_flush(hdev); + do_inquiry = 1; + } + hci_dev_unlock(hdev); + + timeo = ir.length * msecs_to_jiffies(2000); + + if (do_inquiry) { + err = hci_request(hdev, hci_inq_req, (unsigned long)&ir, timeo); + if (err < 0) + goto done; + } + + /* for unlimited number of responses we will use buffer with 255 entries */ + max_rsp = (ir.num_rsp == 0) ? 255 : ir.num_rsp; + + /* cache_dump can't sleep. Therefore we allocate temp buffer and then + * copy it to the user space. + */ + buf = kmalloc(sizeof(struct inquiry_info) * max_rsp, GFP_KERNEL); + if (!buf) { + err = -ENOMEM; + goto done; + } + + hci_dev_lock(hdev); + ir.num_rsp = inquiry_cache_dump(hdev, max_rsp, buf); + hci_dev_unlock(hdev); + + BT_DBG("num_rsp %d", ir.num_rsp); + + if (!copy_to_user(ptr, &ir, sizeof(ir))) { + ptr += sizeof(ir); + if (copy_to_user(ptr, buf, sizeof(struct inquiry_info) * + ir.num_rsp)) + err = -EFAULT; + } else + err = -EFAULT; + + kfree(buf); + +done: + hci_dev_put(hdev); + return err; +} + +/* ---- HCI ioctl helpers ---- */ + +int hci_dev_open(__u16 dev) +{ + struct hci_dev *hdev; + int ret = 0; + + hdev = hci_dev_get(dev); + if (!hdev) + return -ENODEV; + + BT_DBG("%s %p", hdev->name, hdev); + + hci_req_lock(hdev); + + if (test_bit(HCI_UNREGISTER, &hdev->dev_flags)) { + ret = -ENODEV; + goto done; + } + + if (hdev->rfkill && rfkill_blocked(hdev->rfkill)) { + ret = -ERFKILL; + goto done; + } + + if (test_bit(HCI_UP, &hdev->flags)) { + ret = -EALREADY; + goto done; + } + + if (test_bit(HCI_QUIRK_RAW_DEVICE, &hdev->quirks)) + set_bit(HCI_RAW, &hdev->flags); + + /* Treat all non BR/EDR controllers as raw devices if + enable_hs is not set */ + if (hdev->dev_type != HCI_BREDR && !enable_hs) + set_bit(HCI_RAW, &hdev->flags); + + if (hdev->open(hdev)) { + ret = -EIO; + goto done; + } + + if (!test_bit(HCI_RAW, &hdev->flags)) { + atomic_set(&hdev->cmd_cnt, 1); + set_bit(HCI_INIT, &hdev->flags); + hdev->init_last_cmd = 0; + + ret = __hci_request(hdev, hci_init_req, 0, + msecs_to_jiffies(HCI_INIT_TIMEOUT)); + + if (lmp_host_le_capable(hdev)) + ret = __hci_request(hdev, hci_le_init_req, 0, + msecs_to_jiffies(HCI_INIT_TIMEOUT)); + + clear_bit(HCI_INIT, &hdev->flags); + } + + if (!ret) { + hci_dev_hold(hdev); + set_bit(HCI_UP, &hdev->flags); + hci_notify(hdev, HCI_DEV_UP); + if (!test_bit(HCI_SETUP, &hdev->dev_flags)) { + hci_dev_lock(hdev); + mgmt_powered(hdev, 1); + hci_dev_unlock(hdev); + } + } else { + /* Init failed, cleanup */ + flush_work(&hdev->tx_work); + flush_work(&hdev->cmd_work); + flush_work(&hdev->rx_work); + + skb_queue_purge(&hdev->cmd_q); + skb_queue_purge(&hdev->rx_q); + + if (hdev->flush) + hdev->flush(hdev); + + if (hdev->sent_cmd) { + kfree_skb(hdev->sent_cmd); + hdev->sent_cmd = NULL; + } + + hdev->close(hdev); + hdev->flags = 0; + } + +done: + hci_req_unlock(hdev); + hci_dev_put(hdev); + return ret; +} + +static int hci_dev_do_close(struct hci_dev *hdev) +{ + BT_DBG("%s %p", hdev->name, hdev); + + cancel_work_sync(&hdev->le_scan); + + hci_req_cancel(hdev, ENODEV); + hci_req_lock(hdev); + + if (!test_and_clear_bit(HCI_UP, &hdev->flags)) { + del_timer_sync(&hdev->cmd_timer); + hci_req_unlock(hdev); + return 0; + } + + /* Flush RX and TX works */ + flush_work(&hdev->tx_work); + flush_work(&hdev->rx_work); + + if (hdev->discov_timeout > 0) { + cancel_delayed_work(&hdev->discov_off); + hdev->discov_timeout = 0; + clear_bit(HCI_DISCOVERABLE, &hdev->dev_flags); + } + + if (test_and_clear_bit(HCI_SERVICE_CACHE, &hdev->dev_flags)) + cancel_delayed_work(&hdev->service_cache); + + cancel_delayed_work_sync(&hdev->le_scan_disable); + + hci_dev_lock(hdev); + inquiry_cache_flush(hdev); + hci_conn_hash_flush(hdev); + hci_dev_unlock(hdev); + + hci_notify(hdev, HCI_DEV_DOWN); + + if (hdev->flush) + hdev->flush(hdev); + + /* Reset device */ + skb_queue_purge(&hdev->cmd_q); + atomic_set(&hdev->cmd_cnt, 1); + if (!test_bit(HCI_RAW, &hdev->flags) && + test_bit(HCI_QUIRK_NO_RESET, &hdev->quirks)) { + set_bit(HCI_INIT, &hdev->flags); + __hci_request(hdev, hci_reset_req, 0, + msecs_to_jiffies(250)); + clear_bit(HCI_INIT, &hdev->flags); + } + + /* flush cmd work */ + flush_work(&hdev->cmd_work); + + /* Drop queues */ + skb_queue_purge(&hdev->rx_q); + skb_queue_purge(&hdev->cmd_q); + skb_queue_purge(&hdev->raw_q); + + /* Drop last sent command */ + if (hdev->sent_cmd) { + del_timer_sync(&hdev->cmd_timer); + kfree_skb(hdev->sent_cmd); + hdev->sent_cmd = NULL; + } + + /* After this point our queues are empty + * and no tasks are scheduled. */ + hdev->close(hdev); + + if (!test_and_clear_bit(HCI_AUTO_OFF, &hdev->dev_flags)) { + hci_dev_lock(hdev); + mgmt_powered(hdev, 0); + hci_dev_unlock(hdev); + } + + /* Clear flags */ + hdev->flags = 0; + + memset(hdev->eir, 0, sizeof(hdev->eir)); + memset(hdev->dev_class, 0, sizeof(hdev->dev_class)); + + hci_req_unlock(hdev); + + hci_dev_put(hdev); + return 0; +} + +int hci_dev_close(__u16 dev) +{ + struct hci_dev *hdev; + int err; + + hdev = hci_dev_get(dev); + if (!hdev) + return -ENODEV; + + if (test_and_clear_bit(HCI_AUTO_OFF, &hdev->dev_flags)) + cancel_delayed_work(&hdev->power_off); + + err = hci_dev_do_close(hdev); + + hci_dev_put(hdev); + return err; +} + +int hci_dev_reset(__u16 dev) +{ + struct hci_dev *hdev; + int ret = 0; + + hdev = hci_dev_get(dev); + if (!hdev) + return -ENODEV; + + hci_req_lock(hdev); + + if (!test_bit(HCI_UP, &hdev->flags)) + goto done; + + /* Drop queues */ + skb_queue_purge(&hdev->rx_q); + skb_queue_purge(&hdev->cmd_q); + + hci_dev_lock(hdev); + inquiry_cache_flush(hdev); + hci_conn_hash_flush(hdev); + hci_dev_unlock(hdev); + + if (hdev->flush) + hdev->flush(hdev); + + atomic_set(&hdev->cmd_cnt, 1); + hdev->acl_cnt = 0; hdev->sco_cnt = 0; hdev->le_cnt = 0; + + if (!test_bit(HCI_RAW, &hdev->flags)) + ret = __hci_request(hdev, hci_reset_req, 0, + msecs_to_jiffies(HCI_INIT_TIMEOUT)); + +done: + hci_req_unlock(hdev); + hci_dev_put(hdev); + return ret; +} + +int hci_dev_reset_stat(__u16 dev) +{ + struct hci_dev *hdev; + int ret = 0; + + hdev = hci_dev_get(dev); + if (!hdev) + return -ENODEV; + + memset(&hdev->stat, 0, sizeof(struct hci_dev_stats)); + + hci_dev_put(hdev); + + return ret; +} + +int hci_dev_cmd(unsigned int cmd, void __user *arg) +{ + struct hci_dev *hdev; + struct hci_dev_req dr; + int err = 0; + + if (copy_from_user(&dr, arg, sizeof(dr))) + return -EFAULT; + + hdev = hci_dev_get(dr.dev_id); + if (!hdev) + return -ENODEV; + + switch (cmd) { + case HCISETAUTH: + err = hci_request(hdev, hci_auth_req, dr.dev_opt, + msecs_to_jiffies(HCI_INIT_TIMEOUT)); + break; + + case HCISETENCRYPT: + if (!lmp_encrypt_capable(hdev)) { + err = -EOPNOTSUPP; + break; + } + + if (!test_bit(HCI_AUTH, &hdev->flags)) { + /* Auth must be enabled first */ + err = hci_request(hdev, hci_auth_req, dr.dev_opt, + msecs_to_jiffies(HCI_INIT_TIMEOUT)); + if (err) + break; + } + + err = hci_request(hdev, hci_encrypt_req, dr.dev_opt, + msecs_to_jiffies(HCI_INIT_TIMEOUT)); + break; + + case HCISETSCAN: + err = hci_request(hdev, hci_scan_req, dr.dev_opt, + msecs_to_jiffies(HCI_INIT_TIMEOUT)); + break; + + case HCISETLINKPOL: + err = hci_request(hdev, hci_linkpol_req, dr.dev_opt, + msecs_to_jiffies(HCI_INIT_TIMEOUT)); + break; + + case HCISETLINKMODE: + hdev->link_mode = ((__u16) dr.dev_opt) & + (HCI_LM_MASTER | HCI_LM_ACCEPT); + break; + + case HCISETPTYPE: + hdev->pkt_type = (__u16) dr.dev_opt; + break; + + case HCISETACLMTU: + hdev->acl_mtu = *((__u16 *) &dr.dev_opt + 1); + hdev->acl_pkts = *((__u16 *) &dr.dev_opt + 0); + break; + + case HCISETSCOMTU: + hdev->sco_mtu = *((__u16 *) &dr.dev_opt + 1); + hdev->sco_pkts = *((__u16 *) &dr.dev_opt + 0); + break; + + default: + err = -EINVAL; + break; + } + + hci_dev_put(hdev); + return err; +} + +int hci_get_dev_list(void __user *arg) +{ + struct hci_dev *hdev; + struct hci_dev_list_req *dl; + struct hci_dev_req *dr; + int n = 0, size, err; + __u16 dev_num; + + if (get_user(dev_num, (__u16 __user *) arg)) + return -EFAULT; + + if (!dev_num || dev_num > (PAGE_SIZE * 2) / sizeof(*dr)) + return -EINVAL; + + size = sizeof(*dl) + dev_num * sizeof(*dr); + + dl = kzalloc(size, GFP_KERNEL); + if (!dl) + return -ENOMEM; + + dr = dl->dev_req; + + read_lock(&hci_dev_list_lock); + list_for_each_entry(hdev, &hci_dev_list, list) { + if (test_and_clear_bit(HCI_AUTO_OFF, &hdev->dev_flags)) + cancel_delayed_work(&hdev->power_off); + + if (!test_bit(HCI_MGMT, &hdev->dev_flags)) + set_bit(HCI_PAIRABLE, &hdev->dev_flags); + + (dr + n)->dev_id = hdev->id; + (dr + n)->dev_opt = hdev->flags; + + if (++n >= dev_num) + break; + } + read_unlock(&hci_dev_list_lock); + + dl->dev_num = n; + size = sizeof(*dl) + n * sizeof(*dr); + + err = copy_to_user(arg, dl, size); + kfree(dl); + + return err ? -EFAULT : 0; +} + +int hci_get_dev_info(void __user *arg) +{ + struct hci_dev *hdev; + struct hci_dev_info di; + int err = 0; + + if (copy_from_user(&di, arg, sizeof(di))) + return -EFAULT; + + hdev = hci_dev_get(di.dev_id); + if (!hdev) + return -ENODEV; + + if (test_and_clear_bit(HCI_AUTO_OFF, &hdev->dev_flags)) + cancel_delayed_work_sync(&hdev->power_off); + + if (!test_bit(HCI_MGMT, &hdev->dev_flags)) + set_bit(HCI_PAIRABLE, &hdev->dev_flags); + + strcpy(di.name, hdev->name); + di.bdaddr = hdev->bdaddr; + di.type = (hdev->bus & 0x0f) | (hdev->dev_type << 4); + di.flags = hdev->flags; + di.pkt_type = hdev->pkt_type; + di.acl_mtu = hdev->acl_mtu; + di.acl_pkts = hdev->acl_pkts; + di.sco_mtu = hdev->sco_mtu; + di.sco_pkts = hdev->sco_pkts; + di.link_policy = hdev->link_policy; + di.link_mode = hdev->link_mode; + + memcpy(&di.stat, &hdev->stat, sizeof(di.stat)); + memcpy(&di.features, &hdev->features, sizeof(di.features)); + + if (copy_to_user(arg, &di, sizeof(di))) + err = -EFAULT; + + hci_dev_put(hdev); + + return err; +} + +/* ---- Interface to HCI drivers ---- */ + +static int hci_rfkill_set_block(void *data, bool blocked) +{ + struct hci_dev *hdev = data; + + BT_DBG("%p name %s blocked %d", hdev, hdev->name, blocked); + + if (!blocked) + return 0; + + hci_dev_do_close(hdev); + + return 0; +} + +static const struct rfkill_ops hci_rfkill_ops = { + .set_block = hci_rfkill_set_block, +}; + +/* Alloc HCI device */ +struct hci_dev *hci_alloc_dev(void) +{ + struct hci_dev *hdev; + + hdev = kzalloc(sizeof(struct hci_dev), GFP_KERNEL); + if (!hdev) + return NULL; + + hci_init_sysfs(hdev); + skb_queue_head_init(&hdev->driver_init); + + return hdev; +} +EXPORT_SYMBOL(hci_alloc_dev); + +/* Free HCI device */ +void hci_free_dev(struct hci_dev *hdev) +{ + skb_queue_purge(&hdev->driver_init); + + /* will free via device release */ + put_device(&hdev->dev); +} +EXPORT_SYMBOL(hci_free_dev); + +static void hci_power_on(struct work_struct *work) +{ + struct hci_dev *hdev = container_of(work, struct hci_dev, power_on); + + BT_DBG("%s", hdev->name); + + if (hci_dev_open(hdev->id) < 0) + return; + + if (test_bit(HCI_AUTO_OFF, &hdev->dev_flags)) + schedule_delayed_work(&hdev->power_off, + msecs_to_jiffies(AUTO_OFF_TIMEOUT)); + + if (test_and_clear_bit(HCI_SETUP, &hdev->dev_flags)) + mgmt_index_added(hdev); +} + +static void hci_power_off(struct work_struct *work) +{ + struct hci_dev *hdev = container_of(work, struct hci_dev, + power_off.work); + + BT_DBG("%s", hdev->name); + + hci_dev_do_close(hdev); +} + +static void hci_discov_off(struct work_struct *work) +{ + struct hci_dev *hdev; + u8 scan = SCAN_PAGE; + + hdev = container_of(work, struct hci_dev, discov_off.work); + + BT_DBG("%s", hdev->name); + + hci_dev_lock(hdev); + + hci_send_cmd(hdev, HCI_OP_WRITE_SCAN_ENABLE, sizeof(scan), &scan); + + hdev->discov_timeout = 0; + + hci_dev_unlock(hdev); +} + +int hci_uuids_clear(struct hci_dev *hdev) +{ + struct list_head *p, *n; + + list_for_each_safe(p, n, &hdev->uuids) { + struct bt_uuid *uuid; + + uuid = list_entry(p, struct bt_uuid, list); + + list_del(p); + kfree(uuid); + } + + return 0; +} + +int hci_link_keys_clear(struct hci_dev *hdev) +{ + struct list_head *p, *n; + + list_for_each_safe(p, n, &hdev->link_keys) { + struct link_key *key; + + key = list_entry(p, struct link_key, list); + + list_del(p); + kfree(key); + } + + return 0; +} + +int hci_smp_ltks_clear(struct hci_dev *hdev) +{ + struct smp_ltk *k, *tmp; + + list_for_each_entry_safe(k, tmp, &hdev->long_term_keys, list) { + list_del(&k->list); + kfree(k); + } + + return 0; +} + +struct link_key *hci_find_link_key(struct hci_dev *hdev, bdaddr_t *bdaddr) +{ + struct link_key *k; + + list_for_each_entry(k, &hdev->link_keys, list) + if (bacmp(bdaddr, &k->bdaddr) == 0) + return k; + + return NULL; +} + +static bool hci_persistent_key(struct hci_dev *hdev, struct hci_conn *conn, + u8 key_type, u8 old_key_type) +{ + /* Legacy key */ + if (key_type < 0x03) + return true; + + /* Debug keys are insecure so don't store them persistently */ + if (key_type == HCI_LK_DEBUG_COMBINATION) + return false; + + /* Changed combination key and there's no previous one */ + if (key_type == HCI_LK_CHANGED_COMBINATION && old_key_type == 0xff) + return false; + + /* Security mode 3 case */ + if (!conn) + return true; + + /* Neither local nor remote side had no-bonding as requirement */ + if (conn->auth_type > 0x01 && conn->remote_auth > 0x01) + return true; + + /* Local side had dedicated bonding as requirement */ + if (conn->auth_type == 0x02 || conn->auth_type == 0x03) + return true; + + /* Remote side had dedicated bonding as requirement */ + if (conn->remote_auth == 0x02 || conn->remote_auth == 0x03) + return true; + + /* If none of the above criteria match, then don't store the key + * persistently */ + return false; +} + +struct smp_ltk *hci_find_ltk(struct hci_dev *hdev, __le16 ediv, u8 rand[8]) +{ + struct smp_ltk *k; + + list_for_each_entry(k, &hdev->long_term_keys, list) { + if (k->ediv != ediv || + memcmp(rand, k->rand, sizeof(k->rand))) + continue; + + return k; + } + + return NULL; +} +EXPORT_SYMBOL(hci_find_ltk); + +struct smp_ltk *hci_find_ltk_by_addr(struct hci_dev *hdev, bdaddr_t *bdaddr, + u8 addr_type) +{ + struct smp_ltk *k; + + list_for_each_entry(k, &hdev->long_term_keys, list) + if (addr_type == k->bdaddr_type && + bacmp(bdaddr, &k->bdaddr) == 0) + return k; + + return NULL; +} +EXPORT_SYMBOL(hci_find_ltk_by_addr); + +int hci_add_link_key(struct hci_dev *hdev, struct hci_conn *conn, int new_key, + bdaddr_t *bdaddr, u8 *val, u8 type, u8 pin_len) +{ + struct link_key *key, *old_key; + u8 old_key_type; + bool persistent; + + old_key = hci_find_link_key(hdev, bdaddr); + if (old_key) { + old_key_type = old_key->type; + key = old_key; + } else { + old_key_type = conn ? conn->key_type : 0xff; + key = kzalloc(sizeof(*key), GFP_ATOMIC); + if (!key) + return -ENOMEM; + list_add(&key->list, &hdev->link_keys); + } + + BT_DBG("%s key for %s type %u", hdev->name, batostr(bdaddr), type); + + /* Some buggy controller combinations generate a changed + * combination key for legacy pairing even when there's no + * previous key */ + if (type == HCI_LK_CHANGED_COMBINATION && + (!conn || conn->remote_auth == 0xff) && + old_key_type == 0xff) { + type = HCI_LK_COMBINATION; + if (conn) + conn->key_type = type; + } + + bacpy(&key->bdaddr, bdaddr); + memcpy(key->val, val, 16); + key->pin_len = pin_len; + + if (type == HCI_LK_CHANGED_COMBINATION) + key->type = old_key_type; + else + key->type = type; + + if (!new_key) + return 0; + + persistent = hci_persistent_key(hdev, conn, type, old_key_type); + + mgmt_new_link_key(hdev, key, persistent); + + if (conn) + conn->flush_key = !persistent; + + return 0; +} + +int hci_add_ltk(struct hci_dev *hdev, bdaddr_t *bdaddr, u8 addr_type, u8 type, + int new_key, u8 authenticated, u8 tk[16], u8 enc_size, u16 + ediv, u8 rand[8]) +{ + struct smp_ltk *key, *old_key; + + if (!(type & HCI_SMP_STK) && !(type & HCI_SMP_LTK)) + return 0; + + old_key = hci_find_ltk_by_addr(hdev, bdaddr, addr_type); + if (old_key) + key = old_key; + else { + key = kzalloc(sizeof(*key), GFP_ATOMIC); + if (!key) + return -ENOMEM; + list_add(&key->list, &hdev->long_term_keys); + } + + bacpy(&key->bdaddr, bdaddr); + key->bdaddr_type = addr_type; + memcpy(key->val, tk, sizeof(key->val)); + key->authenticated = authenticated; + key->ediv = ediv; + key->enc_size = enc_size; + key->type = type; + memcpy(key->rand, rand, sizeof(key->rand)); + + if (!new_key) + return 0; + + if (type & HCI_SMP_LTK) + mgmt_new_ltk(hdev, key, 1); + + return 0; +} + +int hci_remove_link_key(struct hci_dev *hdev, bdaddr_t *bdaddr) +{ + struct link_key *key; + + key = hci_find_link_key(hdev, bdaddr); + if (!key) + return -ENOENT; + + BT_DBG("%s removing %s", hdev->name, batostr(bdaddr)); + + list_del(&key->list); + kfree(key); + + return 0; +} + +int hci_remove_ltk(struct hci_dev *hdev, bdaddr_t *bdaddr) +{ + struct smp_ltk *k, *tmp; + + list_for_each_entry_safe(k, tmp, &hdev->long_term_keys, list) { + if (bacmp(bdaddr, &k->bdaddr)) + continue; + + BT_DBG("%s removing %s", hdev->name, batostr(bdaddr)); + + list_del(&k->list); + kfree(k); + } + + return 0; +} + +/* HCI command timer function */ +static void hci_cmd_timer(unsigned long arg) +{ + struct hci_dev *hdev = (void *) arg; + + BT_ERR("%s command tx timeout", hdev->name); + atomic_set(&hdev->cmd_cnt, 1); + queue_work(hdev->workqueue, &hdev->cmd_work); +} + +struct oob_data *hci_find_remote_oob_data(struct hci_dev *hdev, + bdaddr_t *bdaddr) +{ + struct oob_data *data; + + list_for_each_entry(data, &hdev->remote_oob_data, list) + if (bacmp(bdaddr, &data->bdaddr) == 0) + return data; + + return NULL; +} + +int hci_remove_remote_oob_data(struct hci_dev *hdev, bdaddr_t *bdaddr) +{ + struct oob_data *data; + + data = hci_find_remote_oob_data(hdev, bdaddr); + if (!data) + return -ENOENT; + + BT_DBG("%s removing %s", hdev->name, batostr(bdaddr)); + + list_del(&data->list); + kfree(data); + + return 0; +} + +int hci_remote_oob_data_clear(struct hci_dev *hdev) +{ + struct oob_data *data, *n; + + list_for_each_entry_safe(data, n, &hdev->remote_oob_data, list) { + list_del(&data->list); + kfree(data); + } + + return 0; +} + +int hci_add_remote_oob_data(struct hci_dev *hdev, bdaddr_t *bdaddr, u8 *hash, + u8 *randomizer) +{ + struct oob_data *data; + + data = hci_find_remote_oob_data(hdev, bdaddr); + + if (!data) { + data = kmalloc(sizeof(*data), GFP_ATOMIC); + if (!data) + return -ENOMEM; + + bacpy(&data->bdaddr, bdaddr); + list_add(&data->list, &hdev->remote_oob_data); + } + + memcpy(data->hash, hash, sizeof(data->hash)); + memcpy(data->randomizer, randomizer, sizeof(data->randomizer)); + + BT_DBG("%s for %s", hdev->name, batostr(bdaddr)); + + return 0; +} + +struct bdaddr_list *hci_blacklist_lookup(struct hci_dev *hdev, bdaddr_t *bdaddr) +{ + struct bdaddr_list *b; + + list_for_each_entry(b, &hdev->blacklist, list) + if (bacmp(bdaddr, &b->bdaddr) == 0) + return b; + + return NULL; +} + +int hci_blacklist_clear(struct hci_dev *hdev) +{ + struct list_head *p, *n; + + list_for_each_safe(p, n, &hdev->blacklist) { + struct bdaddr_list *b; + + b = list_entry(p, struct bdaddr_list, list); + + list_del(p); + kfree(b); + } + + return 0; +} + +int hci_blacklist_add(struct hci_dev *hdev, bdaddr_t *bdaddr, u8 type) +{ + struct bdaddr_list *entry; + + if (bacmp(bdaddr, BDADDR_ANY) == 0) + return -EBADF; + + if (hci_blacklist_lookup(hdev, bdaddr)) + return -EEXIST; + + entry = kzalloc(sizeof(struct bdaddr_list), GFP_KERNEL); + if (!entry) + return -ENOMEM; + + bacpy(&entry->bdaddr, bdaddr); + + list_add(&entry->list, &hdev->blacklist); + + return mgmt_device_blocked(hdev, bdaddr, type); +} + +int hci_blacklist_del(struct hci_dev *hdev, bdaddr_t *bdaddr, u8 type) +{ + struct bdaddr_list *entry; + + if (bacmp(bdaddr, BDADDR_ANY) == 0) + return hci_blacklist_clear(hdev); + + entry = hci_blacklist_lookup(hdev, bdaddr); + if (!entry) + return -ENOENT; + + list_del(&entry->list); + kfree(entry); + + return mgmt_device_unblocked(hdev, bdaddr, type); +} + +static void hci_clear_adv_cache(struct work_struct *work) +{ + struct hci_dev *hdev = container_of(work, struct hci_dev, + adv_work.work); + + hci_dev_lock(hdev); + + hci_adv_entries_clear(hdev); + + hci_dev_unlock(hdev); +} + +int hci_adv_entries_clear(struct hci_dev *hdev) +{ + struct adv_entry *entry, *tmp; + + list_for_each_entry_safe(entry, tmp, &hdev->adv_entries, list) { + list_del(&entry->list); + kfree(entry); + } + + BT_DBG("%s adv cache cleared", hdev->name); + + return 0; +} + +struct adv_entry *hci_find_adv_entry(struct hci_dev *hdev, bdaddr_t *bdaddr) +{ + struct adv_entry *entry; + + list_for_each_entry(entry, &hdev->adv_entries, list) + if (bacmp(bdaddr, &entry->bdaddr) == 0) + return entry; + + return NULL; +} + +static inline int is_connectable_adv(u8 evt_type) +{ + if (evt_type == ADV_IND || evt_type == ADV_DIRECT_IND) + return 1; + + return 0; +} + +int hci_add_adv_entry(struct hci_dev *hdev, + struct hci_ev_le_advertising_info *ev) { struct adv_entry *entry; if (!is_connectable_adv(ev->evt_type)) + return -EINVAL; + + /* Only new entries should be added to adv_entries. So, if + * bdaddr was found, don't add it. */ + if (hci_find_adv_entry(hdev, &ev->bdaddr)) + return 0; + + entry = kzalloc(sizeof(*entry), GFP_KERNEL); + if (!entry) + return -ENOMEM; + + bacpy(&entry->bdaddr, &ev->bdaddr); + entry->bdaddr_type = ev->bdaddr_type; + + list_add(&entry->list, &hdev->adv_entries); + + BT_DBG("%s adv entry added: address %s type %u", hdev->name, + batostr(&entry->bdaddr), entry->bdaddr_type); + + return 0; +} + +static void le_scan_param_req(struct hci_dev *hdev, unsigned long opt) +{ + struct le_scan_params *param = (struct le_scan_params *) opt; + struct hci_cp_le_set_scan_param cp; + + memset(&cp, 0, sizeof(cp)); + cp.type = param->type; + cp.interval = cpu_to_le16(param->interval); + cp.window = cpu_to_le16(param->window); + + hci_send_cmd(hdev, HCI_OP_LE_SET_SCAN_PARAM, sizeof(cp), &cp); +} + +static void le_scan_enable_req(struct hci_dev *hdev, unsigned long opt) +{ + struct hci_cp_le_set_scan_enable cp; + + memset(&cp, 0, sizeof(cp)); + cp.enable = 1; + + hci_send_cmd(hdev, HCI_OP_LE_SET_SCAN_ENABLE, sizeof(cp), &cp); +} + +static int hci_do_le_scan(struct hci_dev *hdev, u8 type, u16 interval, + u16 window, int timeout) +{ + long timeo = msecs_to_jiffies(3000); + struct le_scan_params param; + int err; + + BT_DBG("%s", hdev->name); + + if (test_bit(HCI_LE_SCAN, &hdev->dev_flags)) + return -EINPROGRESS; + + param.type = type; + param.interval = interval; + param.window = window; + + hci_req_lock(hdev); + + err = __hci_request(hdev, le_scan_param_req, (unsigned long) ¶m, + timeo); + if (!err) + err = __hci_request(hdev, le_scan_enable_req, 0, timeo); + + hci_req_unlock(hdev); + + if (err < 0) + return err; + + schedule_delayed_work(&hdev->le_scan_disable, + msecs_to_jiffies(timeout)); + + return 0; +} + +static void le_scan_disable_work(struct work_struct *work) +{ + struct hci_dev *hdev = container_of(work, struct hci_dev, + le_scan_disable.work); + struct hci_cp_le_set_scan_enable cp; + + BT_DBG("%s", hdev->name); + + memset(&cp, 0, sizeof(cp)); + + hci_send_cmd(hdev, HCI_OP_LE_SET_SCAN_ENABLE, sizeof(cp), &cp); +} + +static void le_scan_work(struct work_struct *work) +{ + struct hci_dev *hdev = container_of(work, struct hci_dev, le_scan); + struct le_scan_params *param = &hdev->le_scan_params; + + BT_DBG("%s", hdev->name); + + hci_do_le_scan(hdev, param->type, param->interval, param->window, + param->timeout); +} + +int hci_le_scan(struct hci_dev *hdev, u8 type, u16 interval, u16 window, + int timeout) +{ + struct le_scan_params *param = &hdev->le_scan_params; + + BT_DBG("%s", hdev->name); + + if (work_busy(&hdev->le_scan)) + return -EINPROGRESS; + + param->type = type; + param->interval = interval; + param->window = window; + param->timeout = timeout; + + queue_work(system_long_wq, &hdev->le_scan); + + return 0; +} + +/* Register HCI device */ +int hci_register_dev(struct hci_dev *hdev) +{ + struct list_head *head = &hci_dev_list, *p; + int i, id, error; + + BT_DBG("%p name %s bus %d", hdev, hdev->name, hdev->bus); + + if (!hdev->open || !hdev->close) + return -EINVAL; + + /* Do not allow HCI_AMP devices to register at index 0, + * so the index can be used as the AMP controller ID. + */ + id = (hdev->dev_type == HCI_BREDR) ? 0 : 1; + + write_lock(&hci_dev_list_lock); + + /* Find first available device id */ + list_for_each(p, &hci_dev_list) { + if (list_entry(p, struct hci_dev, list)->id != id) + break; + head = p; id++; + } + + sprintf(hdev->name, "hci%d", id); + hdev->id = id; + list_add_tail(&hdev->list, head); + + mutex_init(&hdev->lock); + + hdev->flags = 0; + hdev->dev_flags = 0; + hdev->pkt_type = (HCI_DM1 | HCI_DH1 | HCI_HV1); + hdev->esco_type = (ESCO_HV1); + hdev->link_mode = (HCI_LM_ACCEPT); + hdev->io_capability = 0x03; /* No Input No Output */ + + hdev->idle_timeout = 0; + hdev->sniff_max_interval = 800; + hdev->sniff_min_interval = 80; + + INIT_WORK(&hdev->rx_work, hci_rx_work); + INIT_WORK(&hdev->cmd_work, hci_cmd_work); + INIT_WORK(&hdev->tx_work, hci_tx_work); + + + skb_queue_head_init(&hdev->rx_q); + skb_queue_head_init(&hdev->cmd_q); + skb_queue_head_init(&hdev->raw_q); + + setup_timer(&hdev->cmd_timer, hci_cmd_timer, (unsigned long) hdev); + + for (i = 0; i < NUM_REASSEMBLY; i++) + hdev->reassembly[i] = NULL; + + init_waitqueue_head(&hdev->req_wait_q); + mutex_init(&hdev->req_lock); + + discovery_init(hdev); + + hci_conn_hash_init(hdev); + + INIT_LIST_HEAD(&hdev->mgmt_pending); + + INIT_LIST_HEAD(&hdev->blacklist); + + INIT_LIST_HEAD(&hdev->uuids); + + INIT_LIST_HEAD(&hdev->link_keys); + INIT_LIST_HEAD(&hdev->long_term_keys); + + INIT_LIST_HEAD(&hdev->remote_oob_data); + + INIT_LIST_HEAD(&hdev->adv_entries); + + INIT_DELAYED_WORK(&hdev->adv_work, hci_clear_adv_cache); + INIT_WORK(&hdev->power_on, hci_power_on); + INIT_DELAYED_WORK(&hdev->power_off, hci_power_off); + + INIT_DELAYED_WORK(&hdev->discov_off, hci_discov_off); + + memset(&hdev->stat, 0, sizeof(struct hci_dev_stats)); + + atomic_set(&hdev->promisc, 0); + + INIT_WORK(&hdev->le_scan, le_scan_work); + + INIT_DELAYED_WORK(&hdev->le_scan_disable, le_scan_disable_work); + + write_unlock(&hci_dev_list_lock); + + hdev->workqueue = alloc_workqueue(hdev->name, WQ_HIGHPRI | WQ_UNBOUND | + WQ_MEM_RECLAIM, 1); + if (!hdev->workqueue) { + error = -ENOMEM; + goto err; + } + + error = hci_add_sysfs(hdev); + if (error < 0) + goto err_wqueue; + + hdev->rfkill = rfkill_alloc(hdev->name, &hdev->dev, + RFKILL_TYPE_BLUETOOTH, &hci_rfkill_ops, hdev); + if (hdev->rfkill) { + if (rfkill_register(hdev->rfkill) < 0) { + rfkill_destroy(hdev->rfkill); + hdev->rfkill = NULL; + } + } + + set_bit(HCI_AUTO_OFF, &hdev->dev_flags); + set_bit(HCI_SETUP, &hdev->dev_flags); + schedule_work(&hdev->power_on); + + hci_notify(hdev, HCI_DEV_REG); + hci_dev_hold(hdev); + + return id; + +err_wqueue: + destroy_workqueue(hdev->workqueue); +err: + write_lock(&hci_dev_list_lock); + list_del(&hdev->list); + write_unlock(&hci_dev_list_lock); + + return error; +} +EXPORT_SYMBOL(hci_register_dev); + +/* Unregister HCI device */ +void hci_unregister_dev(struct hci_dev *hdev) +{ + int i; + + BT_DBG("%p name %s bus %d", hdev, hdev->name, hdev->bus); + + set_bit(HCI_UNREGISTER, &hdev->dev_flags); + + write_lock(&hci_dev_list_lock); + list_del(&hdev->list); + write_unlock(&hci_dev_list_lock); + + hci_dev_do_close(hdev); + + for (i = 0; i < NUM_REASSEMBLY; i++) + kfree_skb(hdev->reassembly[i]); + + if (!test_bit(HCI_INIT, &hdev->flags) && + !test_bit(HCI_SETUP, &hdev->dev_flags)) { + hci_dev_lock(hdev); + mgmt_index_removed(hdev); + hci_dev_unlock(hdev); + } + + /* mgmt_index_removed should take care of emptying the + * pending list */ + BUG_ON(!list_empty(&hdev->mgmt_pending)); + + hci_notify(hdev, HCI_DEV_UNREG); + + if (hdev->rfkill) { + rfkill_unregister(hdev->rfkill); + rfkill_destroy(hdev->rfkill); + } + + hci_del_sysfs(hdev); + + cancel_delayed_work_sync(&hdev->adv_work); + + destroy_workqueue(hdev->workqueue); + + hci_dev_lock(hdev); + hci_blacklist_clear(hdev); + hci_uuids_clear(hdev); + hci_link_keys_clear(hdev); + hci_smp_ltks_clear(hdev); + hci_remote_oob_data_clear(hdev); + hci_adv_entries_clear(hdev); + hci_dev_unlock(hdev); + + hci_dev_put(hdev); +} +EXPORT_SYMBOL(hci_unregister_dev); + +/* Suspend HCI device */ +int hci_suspend_dev(struct hci_dev *hdev) +{ + hci_notify(hdev, HCI_DEV_SUSPEND); + return 0; +} +EXPORT_SYMBOL(hci_suspend_dev); + +/* Resume HCI device */ +int hci_resume_dev(struct hci_dev *hdev) +{ + hci_notify(hdev, HCI_DEV_RESUME); + return 0; +} +EXPORT_SYMBOL(hci_resume_dev); + +/* Receive frame from HCI drivers */ +int hci_recv_frame(struct sk_buff *skb) +{ + struct hci_dev *hdev = (struct hci_dev *) skb->dev; + if (!hdev || (!test_bit(HCI_UP, &hdev->flags) + && !test_bit(HCI_INIT, &hdev->flags))) { + kfree_skb(skb); + return -ENXIO; + } + + /* Incomming skb */ + bt_cb(skb)->incoming = 1; + + /* Time stamp */ + __net_timestamp(skb); + + skb_queue_tail(&hdev->rx_q, skb); + queue_work(hdev->workqueue, &hdev->rx_work); + + return 0; +} +EXPORT_SYMBOL(hci_recv_frame); + +static int hci_reassembly(struct hci_dev *hdev, int type, void *data, + int count, __u8 index) +{ + int len = 0; + int hlen = 0; + int remain = count; + struct sk_buff *skb; + struct bt_skb_cb *scb; + + if ((type < HCI_ACLDATA_PKT || type > HCI_EVENT_PKT) || + index >= NUM_REASSEMBLY) + return -EILSEQ; + + skb = hdev->reassembly[index]; + + if (!skb) { + switch (type) { + case HCI_ACLDATA_PKT: + len = HCI_MAX_FRAME_SIZE; + hlen = HCI_ACL_HDR_SIZE; + break; + case HCI_EVENT_PKT: + len = HCI_MAX_EVENT_SIZE; + hlen = HCI_EVENT_HDR_SIZE; + break; + case HCI_SCODATA_PKT: + len = HCI_MAX_SCO_SIZE; + hlen = HCI_SCO_HDR_SIZE; + break; + } + + skb = bt_skb_alloc(len, GFP_ATOMIC); + if (!skb) + return -ENOMEM; + + scb = (void *) skb->cb; + scb->expect = hlen; + scb->pkt_type = type; + + skb->dev = (void *) hdev; + hdev->reassembly[index] = skb; + } + + while (count) { + scb = (void *) skb->cb; + len = min_t(uint, scb->expect, count); + + memcpy(skb_put(skb, len), data, len); + + count -= len; + data += len; + scb->expect -= len; + remain = count; + + switch (type) { + case HCI_EVENT_PKT: + if (skb->len == HCI_EVENT_HDR_SIZE) { + struct hci_event_hdr *h = hci_event_hdr(skb); + scb->expect = h->plen; + + if (skb_tailroom(skb) < scb->expect) { + kfree_skb(skb); + hdev->reassembly[index] = NULL; + return -ENOMEM; + } + } + break; + + case HCI_ACLDATA_PKT: + if (skb->len == HCI_ACL_HDR_SIZE) { + struct hci_acl_hdr *h = hci_acl_hdr(skb); + scb->expect = __le16_to_cpu(h->dlen); + + if (skb_tailroom(skb) < scb->expect) { + kfree_skb(skb); + hdev->reassembly[index] = NULL; + return -ENOMEM; + } + } + break; + + case HCI_SCODATA_PKT: + if (skb->len == HCI_SCO_HDR_SIZE) { + struct hci_sco_hdr *h = hci_sco_hdr(skb); + scb->expect = h->dlen; + + if (skb_tailroom(skb) < scb->expect) { + kfree_skb(skb); + hdev->reassembly[index] = NULL; + return -ENOMEM; + } + } + break; + } + + if (scb->expect == 0) { + /* Complete frame */ + + bt_cb(skb)->pkt_type = type; + hci_recv_frame(skb); + + hdev->reassembly[index] = NULL; + return remain; + } + } + + return remain; +} + +int hci_recv_fragment(struct hci_dev *hdev, int type, void *data, int count) +{ + int rem = 0; + + if (type < HCI_ACLDATA_PKT || type > HCI_EVENT_PKT) + return -EILSEQ; + + while (count) { + rem = hci_reassembly(hdev, type, data, count, type - 1); + if (rem < 0) + return rem; + + data += (count - rem); + count = rem; + } + + return rem; +} +EXPORT_SYMBOL(hci_recv_fragment); + +#define STREAM_REASSEMBLY 0 + +int hci_recv_stream_fragment(struct hci_dev *hdev, void *data, int count) +{ + int type; + int rem = 0; + + while (count) { + struct sk_buff *skb = hdev->reassembly[STREAM_REASSEMBLY]; + + if (!skb) { + struct { char type; } *pkt; + + /* Start of the frame */ + pkt = data; + type = pkt->type; + + data++; + count--; + } else + type = bt_cb(skb)->pkt_type; + + rem = hci_reassembly(hdev, type, data, count, + STREAM_REASSEMBLY); + if (rem < 0) + return rem; + + data += (count - rem); + count = rem; + } + + return rem; +} +EXPORT_SYMBOL(hci_recv_stream_fragment); + +/* ---- Interface to upper protocols ---- */ + +int hci_register_cb(struct hci_cb *cb) +{ + BT_DBG("%p name %s", cb, cb->name); + + write_lock(&hci_cb_list_lock); + list_add(&cb->list, &hci_cb_list); + write_unlock(&hci_cb_list_lock); + + return 0; +} +EXPORT_SYMBOL(hci_register_cb); + +int hci_unregister_cb(struct hci_cb *cb) +{ + BT_DBG("%p name %s", cb, cb->name); + + write_lock(&hci_cb_list_lock); + list_del(&cb->list); + write_unlock(&hci_cb_list_lock); + + return 0; +} +EXPORT_SYMBOL(hci_unregister_cb); + +static int hci_send_frame(struct sk_buff *skb) +{ + struct hci_dev *hdev = (struct hci_dev *) skb->dev; + + if (!hdev) { + kfree_skb(skb); + return -ENODEV; + } + + BT_DBG("%s type %d len %d", hdev->name, bt_cb(skb)->pkt_type, skb->len); + + /* Time stamp */ + __net_timestamp(skb); + + /* Send copy to monitor */ + hci_send_to_monitor(hdev, skb); + + if (atomic_read(&hdev->promisc)) { + /* Send copy to the sockets */ + hci_send_to_sock(hdev, skb); + } + + /* Get rid of skb owner, prior to sending to the driver. */ + skb_orphan(skb); + + return hdev->send(skb); +} + +/* Send HCI command */ +int hci_send_cmd(struct hci_dev *hdev, __u16 opcode, __u32 plen, void *param) +{ + int len = HCI_COMMAND_HDR_SIZE + plen; + struct hci_command_hdr *hdr; + struct sk_buff *skb; + + BT_DBG("%s opcode 0x%x plen %d", hdev->name, opcode, plen); + + skb = bt_skb_alloc(len, GFP_ATOMIC); + if (!skb) { + BT_ERR("%s no memory for command", hdev->name); + return -ENOMEM; + } + + hdr = (struct hci_command_hdr *) skb_put(skb, HCI_COMMAND_HDR_SIZE); + hdr->opcode = cpu_to_le16(opcode); + hdr->plen = plen; + + if (plen) + memcpy(skb_put(skb, plen), param, plen); + + BT_DBG("skb len %d", skb->len); + + bt_cb(skb)->pkt_type = HCI_COMMAND_PKT; + skb->dev = (void *) hdev; + + if (test_bit(HCI_INIT, &hdev->flags)) + hdev->init_last_cmd = opcode; + + skb_queue_tail(&hdev->cmd_q, skb); + queue_work(hdev->workqueue, &hdev->cmd_work); + + return 0; +} + +/* Get data from the previously sent command */ +void *hci_sent_cmd_data(struct hci_dev *hdev, __u16 opcode) +{ + struct hci_command_hdr *hdr; + + if (!hdev->sent_cmd) + return NULL; + + hdr = (void *) hdev->sent_cmd->data; + + if (hdr->opcode != cpu_to_le16(opcode)) + return NULL; + + BT_DBG("%s opcode 0x%x", hdev->name, opcode); + + return hdev->sent_cmd->data + HCI_COMMAND_HDR_SIZE; +} + +/* Send ACL data */ +static void hci_add_acl_hdr(struct sk_buff *skb, __u16 handle, __u16 flags) +{ + struct hci_acl_hdr *hdr; + int len = skb->len; + + skb_push(skb, HCI_ACL_HDR_SIZE); + skb_reset_transport_header(skb); + hdr = (struct hci_acl_hdr *)skb_transport_header(skb); + hdr->handle = cpu_to_le16(hci_handle_pack(handle, flags)); + hdr->dlen = cpu_to_le16(len); +} + +static void hci_queue_acl(struct hci_conn *conn, struct sk_buff_head *queue, + struct sk_buff *skb, __u16 flags) +{ + struct hci_dev *hdev = conn->hdev; + struct sk_buff *list; + + list = skb_shinfo(skb)->frag_list; + if (!list) { + /* Non fragmented */ + BT_DBG("%s nonfrag skb %p len %d", hdev->name, skb, skb->len); + + skb_queue_tail(queue, skb); + } else { + /* Fragmented */ + BT_DBG("%s frag %p len %d", hdev->name, skb, skb->len); + + skb_shinfo(skb)->frag_list = NULL; + + /* Queue all fragments atomically */ + spin_lock(&queue->lock); + + __skb_queue_tail(queue, skb); + + flags &= ~ACL_START; + flags |= ACL_CONT; + do { + skb = list; list = list->next; + + skb->dev = (void *) hdev; + bt_cb(skb)->pkt_type = HCI_ACLDATA_PKT; + hci_add_acl_hdr(skb, conn->handle, flags); + + BT_DBG("%s frag %p len %d", hdev->name, skb, skb->len); + + __skb_queue_tail(queue, skb); + } while (list); + + spin_unlock(&queue->lock); + } +} + +void hci_send_acl(struct hci_chan *chan, struct sk_buff *skb, __u16 flags) +{ + struct hci_conn *conn = chan->conn; + struct hci_dev *hdev = conn->hdev; + + BT_DBG("%s chan %p flags 0x%x", hdev->name, chan, flags); + + skb->dev = (void *) hdev; + bt_cb(skb)->pkt_type = HCI_ACLDATA_PKT; + hci_add_acl_hdr(skb, conn->handle, flags); + + hci_queue_acl(conn, &chan->data_q, skb, flags); + + queue_work(hdev->workqueue, &hdev->tx_work); +} +EXPORT_SYMBOL(hci_send_acl); + +/* Send SCO data */ +void hci_send_sco(struct hci_conn *conn, struct sk_buff *skb) +{ + struct hci_dev *hdev = conn->hdev; + struct hci_sco_hdr hdr; + + BT_DBG("%s len %d", hdev->name, skb->len); + + hdr.handle = cpu_to_le16(conn->handle); + hdr.dlen = skb->len; + + skb_push(skb, HCI_SCO_HDR_SIZE); + skb_reset_transport_header(skb); + memcpy(skb_transport_header(skb), &hdr, HCI_SCO_HDR_SIZE); + + skb->dev = (void *) hdev; + bt_cb(skb)->pkt_type = HCI_SCODATA_PKT; + + skb_queue_tail(&conn->data_q, skb); + queue_work(hdev->workqueue, &hdev->tx_work); +} +EXPORT_SYMBOL(hci_send_sco); + +/* ---- HCI TX task (outgoing data) ---- */ + +/* HCI Connection scheduler */ +static inline struct hci_conn *hci_low_sent(struct hci_dev *hdev, __u8 type, int *quote) +{ + struct hci_conn_hash *h = &hdev->conn_hash; + struct hci_conn *conn = NULL, *c; + int num = 0, min = ~0; + + /* We don't have to lock device here. Connections are always + * added and removed with TX task disabled. */ + + rcu_read_lock(); + + list_for_each_entry_rcu(c, &h->list, list) { + if (c->type != type || skb_queue_empty(&c->data_q)) + continue; + + if (c->state != BT_CONNECTED && c->state != BT_CONFIG) + continue; + + num++; + + if (c->sent < min) { + min = c->sent; + conn = c; + } + + if (hci_conn_num(hdev, type) == num) + break; + } + + rcu_read_unlock(); + + if (conn) { + int cnt, q; + + switch (conn->type) { + case ACL_LINK: + cnt = hdev->acl_cnt; + break; + case SCO_LINK: + case ESCO_LINK: + cnt = hdev->sco_cnt; + break; + case LE_LINK: + cnt = hdev->le_mtu ? hdev->le_cnt : hdev->acl_cnt; + break; + default: + cnt = 0; + BT_ERR("Unknown link type"); + } + + q = cnt / num; + *quote = q ? q : 1; + } else + *quote = 0; + + BT_DBG("conn %p quote %d", conn, *quote); + return conn; +} + +static inline void hci_link_tx_to(struct hci_dev *hdev, __u8 type) +{ + struct hci_conn_hash *h = &hdev->conn_hash; + struct hci_conn *c; + + BT_ERR("%s link tx timeout", hdev->name); + + rcu_read_lock(); + + /* Kill stalled connections */ + list_for_each_entry_rcu(c, &h->list, list) { + if (c->type == type && c->sent) { + BT_ERR("%s killing stalled connection %s", + hdev->name, batostr(&c->dst)); + hci_acl_disconn(c, 0x13); + } + } + + rcu_read_unlock(); +} + +static inline struct hci_chan *hci_chan_sent(struct hci_dev *hdev, __u8 type, + int *quote) +{ + struct hci_conn_hash *h = &hdev->conn_hash; + struct hci_chan *chan = NULL; + int num = 0, min = ~0, cur_prio = 0; + struct hci_conn *conn; + int cnt, q, conn_num = 0; + + BT_DBG("%s", hdev->name); + + rcu_read_lock(); + + list_for_each_entry_rcu(conn, &h->list, list) { + struct hci_chan *tmp; + + if (conn->type != type) + continue; + + if (conn->state != BT_CONNECTED && conn->state != BT_CONFIG) + continue; + + conn_num++; + + list_for_each_entry_rcu(tmp, &conn->chan_list, list) { + struct sk_buff *skb; + + if (skb_queue_empty(&tmp->data_q)) + continue; + + skb = skb_peek(&tmp->data_q); + if (skb->priority < cur_prio) + continue; + + if (skb->priority > cur_prio) { + num = 0; + min = ~0; + cur_prio = skb->priority; + } + + num++; + + if (conn->sent < min) { + min = conn->sent; + chan = tmp; + } + } + + if (hci_conn_num(hdev, type) == conn_num) + break; + } + + rcu_read_unlock(); + + if (!chan) + return NULL; + + switch (chan->conn->type) { + case ACL_LINK: + cnt = hdev->acl_cnt; + break; + case SCO_LINK: + case ESCO_LINK: + cnt = hdev->sco_cnt; + break; + case LE_LINK: + cnt = hdev->le_mtu ? hdev->le_cnt : hdev->acl_cnt; + break; + default: + cnt = 0; + BT_ERR("Unknown link type"); + } + + q = cnt / num; + *quote = q ? q : 1; + BT_DBG("chan %p quote %d", chan, *quote); + return chan; +} + +static void hci_prio_recalculate(struct hci_dev *hdev, __u8 type) +{ + struct hci_conn_hash *h = &hdev->conn_hash; + struct hci_conn *conn; + int num = 0; + + BT_DBG("%s", hdev->name); + + rcu_read_lock(); + + list_for_each_entry_rcu(conn, &h->list, list) { + struct hci_chan *chan; + + if (conn->type != type) + continue; + + if (conn->state != BT_CONNECTED && conn->state != BT_CONFIG) + continue; + + num++; + + list_for_each_entry_rcu(chan, &conn->chan_list, list) { + struct sk_buff *skb; + + if (chan->sent) { + chan->sent = 0; + continue; + } + + if (skb_queue_empty(&chan->data_q)) + continue; + + skb = skb_peek(&chan->data_q); + if (skb->priority >= HCI_PRIO_MAX - 1) + continue; + + skb->priority = HCI_PRIO_MAX - 1; + + BT_DBG("chan %p skb %p promoted to %d", chan, skb, + skb->priority); + } + + if (hci_conn_num(hdev, type) == num) + break; + } + + rcu_read_unlock(); + +} + +static inline int __get_blocks(struct hci_dev *hdev, struct sk_buff *skb) +{ + /* Calculate count of blocks used by this packet */ + return DIV_ROUND_UP(skb->len - HCI_ACL_HDR_SIZE, hdev->block_len); +} + +static inline void __check_timeout(struct hci_dev *hdev, unsigned int cnt) +{ + if (!test_bit(HCI_RAW, &hdev->flags)) { + /* ACL tx timeout must be longer than maximum + * link supervision timeout (40.9 seconds) */ + if (!cnt && time_after(jiffies, hdev->acl_last_tx + + msecs_to_jiffies(HCI_ACL_TX_TIMEOUT))) + hci_link_tx_to(hdev, ACL_LINK); + } +} + +static inline void hci_sched_acl_pkt(struct hci_dev *hdev) +{ + unsigned int cnt = hdev->acl_cnt; + struct hci_chan *chan; + struct sk_buff *skb; + int quote; + + __check_timeout(hdev, cnt); + + while (hdev->acl_cnt && + (chan = hci_chan_sent(hdev, ACL_LINK, "e))) { + u32 priority = (skb_peek(&chan->data_q))->priority; + while (quote-- && (skb = skb_peek(&chan->data_q))) { + BT_DBG("chan %p skb %p len %d priority %u", chan, skb, + skb->len, skb->priority); + + /* Stop if priority has changed */ + if (skb->priority < priority) + break; + + skb = skb_dequeue(&chan->data_q); + + hci_conn_enter_active_mode(chan->conn, + bt_cb(skb)->force_active); + + hci_send_frame(skb); + hdev->acl_last_tx = jiffies; + + hdev->acl_cnt--; + chan->sent++; + chan->conn->sent++; + } + } + + if (cnt != hdev->acl_cnt) + hci_prio_recalculate(hdev, ACL_LINK); +} + +static inline void hci_sched_acl_blk(struct hci_dev *hdev) +{ + unsigned int cnt = hdev->block_cnt; + struct hci_chan *chan; + struct sk_buff *skb; + int quote; + + __check_timeout(hdev, cnt); + + while (hdev->block_cnt > 0 && + (chan = hci_chan_sent(hdev, ACL_LINK, "e))) { + u32 priority = (skb_peek(&chan->data_q))->priority; + while (quote > 0 && (skb = skb_peek(&chan->data_q))) { + int blocks; + + BT_DBG("chan %p skb %p len %d priority %u", chan, skb, + skb->len, skb->priority); + + /* Stop if priority has changed */ + if (skb->priority < priority) + break; + + skb = skb_dequeue(&chan->data_q); + + blocks = __get_blocks(hdev, skb); + if (blocks > hdev->block_cnt) + return; + + hci_conn_enter_active_mode(chan->conn, + bt_cb(skb)->force_active); + + hci_send_frame(skb); + hdev->acl_last_tx = jiffies; + + hdev->block_cnt -= blocks; + quote -= blocks; + + chan->sent += blocks; + chan->conn->sent += blocks; + } + } + + if (cnt != hdev->block_cnt) + hci_prio_recalculate(hdev, ACL_LINK); +} + +static inline void hci_sched_acl(struct hci_dev *hdev) +{ + BT_DBG("%s", hdev->name); + + if (!hci_conn_num(hdev, ACL_LINK)) + return; + + switch (hdev->flow_ctl_mode) { + case HCI_FLOW_CTL_MODE_PACKET_BASED: + hci_sched_acl_pkt(hdev); + break; + + case HCI_FLOW_CTL_MODE_BLOCK_BASED: + hci_sched_acl_blk(hdev); + break; + } +} + +/* Schedule SCO */ +static inline void hci_sched_sco(struct hci_dev *hdev) +{ + struct hci_conn *conn; + struct sk_buff *skb; + int quote; + + BT_DBG("%s", hdev->name); + + if (!hci_conn_num(hdev, SCO_LINK)) + return; + + while (hdev->sco_cnt && (conn = hci_low_sent(hdev, SCO_LINK, "e))) { + while (quote-- && (skb = skb_dequeue(&conn->data_q))) { + BT_DBG("skb %p len %d", skb, skb->len); + hci_send_frame(skb); + + conn->sent++; + if (conn->sent == ~0) + conn->sent = 0; + } + } +} + +static inline void hci_sched_esco(struct hci_dev *hdev) +{ + struct hci_conn *conn; + struct sk_buff *skb; + int quote; + + BT_DBG("%s", hdev->name); + + if (!hci_conn_num(hdev, ESCO_LINK)) + return; + + while (hdev->sco_cnt && (conn = hci_low_sent(hdev, ESCO_LINK, "e))) { + while (quote-- && (skb = skb_dequeue(&conn->data_q))) { + BT_DBG("skb %p len %d", skb, skb->len); + hci_send_frame(skb); + + conn->sent++; + if (conn->sent == ~0) + conn->sent = 0; + } + } +} + +static inline void hci_sched_le(struct hci_dev *hdev) +{ + struct hci_chan *chan; + struct sk_buff *skb; + int quote, cnt, tmp; + + BT_DBG("%s", hdev->name); + + if (!hci_conn_num(hdev, LE_LINK)) + return; + + if (!test_bit(HCI_RAW, &hdev->flags)) { + /* LE tx timeout must be longer than maximum + * link supervision timeout (40.9 seconds) */ + if (!hdev->le_cnt && hdev->le_pkts && + time_after(jiffies, hdev->le_last_tx + HZ * 45)) + hci_link_tx_to(hdev, LE_LINK); + } + + cnt = hdev->le_pkts ? hdev->le_cnt : hdev->acl_cnt; + tmp = cnt; + while (cnt && (chan = hci_chan_sent(hdev, LE_LINK, "e))) { + u32 priority = (skb_peek(&chan->data_q))->priority; + while (quote-- && (skb = skb_peek(&chan->data_q))) { + BT_DBG("chan %p skb %p len %d priority %u", chan, skb, + skb->len, skb->priority); + + /* Stop if priority has changed */ + if (skb->priority < priority) + break; + + skb = skb_dequeue(&chan->data_q); + + hci_send_frame(skb); + hdev->le_last_tx = jiffies; + + cnt--; + chan->sent++; + chan->conn->sent++; + } + } + + if (hdev->le_pkts) + hdev->le_cnt = cnt; + else + hdev->acl_cnt = cnt; + + if (cnt != tmp) + hci_prio_recalculate(hdev, LE_LINK); +} + +static void hci_tx_work(struct work_struct *work) +{ + struct hci_dev *hdev = container_of(work, struct hci_dev, tx_work); + struct sk_buff *skb; + + BT_DBG("%s acl %d sco %d le %d", hdev->name, hdev->acl_cnt, + hdev->sco_cnt, hdev->le_cnt); + + /* Schedule queues and send stuff to HCI driver */ + + hci_sched_acl(hdev); + + hci_sched_sco(hdev); + + hci_sched_esco(hdev); + + hci_sched_le(hdev); + + /* Send next queued raw (unknown type) packet */ + while ((skb = skb_dequeue(&hdev->raw_q))) + hci_send_frame(skb); +} + +/* ----- HCI RX task (incoming data processing) ----- */ + +/* ACL data packet */ +static inline void hci_acldata_packet(struct hci_dev *hdev, struct sk_buff *skb) +{ + struct hci_acl_hdr *hdr = (void *) skb->data; + struct hci_conn *conn; + __u16 handle, flags; + + skb_pull(skb, HCI_ACL_HDR_SIZE); + + handle = __le16_to_cpu(hdr->handle); + flags = hci_flags(handle); + handle = hci_handle(handle); + + BT_DBG("%s len %d handle 0x%x flags 0x%x", hdev->name, skb->len, handle, flags); + + hdev->stat.acl_rx++; + + hci_dev_lock(hdev); + conn = hci_conn_hash_lookup_handle(hdev, handle); + hci_dev_unlock(hdev); + + if (conn) { + hci_conn_enter_active_mode(conn, BT_POWER_FORCE_ACTIVE_OFF); + + hci_dev_lock(hdev); + if (test_bit(HCI_MGMT, &hdev->dev_flags) && + !test_and_set_bit(HCI_CONN_MGMT_CONNECTED, &conn->flags)) + mgmt_device_connected(hdev, &conn->dst, conn->type, + conn->dst_type, 0, NULL, 0, + conn->dev_class); + hci_dev_unlock(hdev); + + /* Send to upper protocol */ + l2cap_recv_acldata(conn, skb, flags); + return; + } else { + BT_ERR("%s ACL packet for unknown connection handle %d", + hdev->name, handle); + } + + kfree_skb(skb); +} + +/* SCO data packet */ +static inline void hci_scodata_packet(struct hci_dev *hdev, struct sk_buff *skb) +{ + struct hci_sco_hdr *hdr = (void *) skb->data; + struct hci_conn *conn; + __u16 handle; + + skb_pull(skb, HCI_SCO_HDR_SIZE); + + handle = __le16_to_cpu(hdr->handle); + + BT_DBG("%s len %d handle 0x%x", hdev->name, skb->len, handle); + + hdev->stat.sco_rx++; + + hci_dev_lock(hdev); + conn = hci_conn_hash_lookup_handle(hdev, handle); + hci_dev_unlock(hdev); + + if (conn) { + /* Send to upper protocol */ + sco_recv_scodata(conn, skb); + return; + } else { + BT_ERR("%s SCO packet for unknown connection handle %d", + hdev->name, handle); + } + + kfree_skb(skb); +} + +static void hci_rx_work(struct work_struct *work) +{ + struct hci_dev *hdev = container_of(work, struct hci_dev, rx_work); + struct sk_buff *skb; + + BT_DBG("%s", hdev->name); + + while ((skb = skb_dequeue(&hdev->rx_q))) { + /* Send copy to monitor */ + hci_send_to_monitor(hdev, skb); + + if (atomic_read(&hdev->promisc)) { + /* Send copy to the sockets */ + hci_send_to_sock(hdev, skb); + } + + if (test_bit(HCI_RAW, &hdev->flags)) { + kfree_skb(skb); + continue; + } + + if (test_bit(HCI_INIT, &hdev->flags)) { + /* Don't process data packets in this states. */ + switch (bt_cb(skb)->pkt_type) { + case HCI_ACLDATA_PKT: + case HCI_SCODATA_PKT: + kfree_skb(skb); + continue; + } + } + + /* Process frame */ + switch (bt_cb(skb)->pkt_type) { + case HCI_EVENT_PKT: + BT_DBG("%s Event packet", hdev->name); + hci_event_packet(hdev, skb); + break; + + case HCI_ACLDATA_PKT: + BT_DBG("%s ACL data packet", hdev->name); + hci_acldata_packet(hdev, skb); + break; + + case HCI_SCODATA_PKT: + BT_DBG("%s SCO data packet", hdev->name); + hci_scodata_packet(hdev, skb); + break; + + default: + kfree_skb(skb); + break; + } + } +} + +static void hci_cmd_work(struct work_struct *work) +{ + struct hci_dev *hdev = container_of(work, struct hci_dev, cmd_work); + struct sk_buff *skb; + + BT_DBG("%s cmd %d", hdev->name, atomic_read(&hdev->cmd_cnt)); + + /* Send queued commands */ + if (atomic_read(&hdev->cmd_cnt)) { + skb = skb_dequeue(&hdev->cmd_q); + if (!skb) + return; + + kfree_skb(hdev->sent_cmd); + + hdev->sent_cmd = skb_clone(skb, GFP_ATOMIC); + if (hdev->sent_cmd) { + atomic_dec(&hdev->cmd_cnt); + hci_send_frame(skb); + if (test_bit(HCI_RESET, &hdev->flags)) + del_timer(&hdev->cmd_timer); + else + mod_timer(&hdev->cmd_timer, + jiffies + msecs_to_jiffies(HCI_CMD_TIMEOUT)); + } else { + skb_queue_head(&hdev->cmd_q, skb); + queue_work(hdev->workqueue, &hdev->cmd_work); + } + } +} + +int hci_do_inquiry(struct hci_dev *hdev, u8 length) +{ + /* General inquiry access code (GIAC) */ + u8 lap[3] = { 0x33, 0x8b, 0x9e }; + struct hci_cp_inquiry cp; + + BT_DBG("%s", hdev->name); + + if (test_bit(HCI_INQUIRY, &hdev->flags)) + return -EINPROGRESS; + + inquiry_cache_flush(hdev); + + memset(&cp, 0, sizeof(cp)); + memcpy(&cp.lap, lap, sizeof(cp.lap)); + cp.length = length; + + return hci_send_cmd(hdev, HCI_OP_INQUIRY, sizeof(cp), &cp); +} + +int hci_cancel_inquiry(struct hci_dev *hdev) +{ + BT_DBG("%s", hdev->name); + + if (!test_bit(HCI_INQUIRY, &hdev->flags)) + return -EPERM; + + return hci_send_cmd(hdev, HCI_OP_INQUIRY_CANCEL, 0, NULL); +} diff --git a/net/bluetooth_tizen/hci_event.c b/net/bluetooth_tizen/hci_event.c new file mode 100644 index 0000000..17aaa11 --- /dev/null +++ b/net/bluetooth_tizen/hci_event.c @@ -0,0 +1,3604 @@ +/* + BlueZ - Bluetooth protocol stack for Linux + Copyright (c) 2000-2001, 2010, Code Aurora Forum. All rights reserved. + + Written 2000,2001 by Maxim Krasnyansky <maxk@qualcomm.com> + + This program is free software; you can redistribute it and/or modify + it under the terms of the GNU General Public License version 2 as + published by the Free Software Foundation; + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS + OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT OF THIRD PARTY RIGHTS. + IN NO EVENT SHALL THE COPYRIGHT HOLDER(S) AND AUTHOR(S) BE LIABLE FOR ANY + CLAIM, OR ANY SPECIAL INDIRECT OR CONSEQUENTIAL DAMAGES, OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + + ALL LIABILITY, INCLUDING LIABILITY FOR INFRINGEMENT OF ANY PATENTS, + COPYRIGHTS, TRADEMARKS OR OTHER RIGHTS, RELATING TO USE OF THIS + SOFTWARE IS DISCLAIMED. +*/ + +/* Bluetooth HCI event handling. */ + +#include <linux/module.h> + +#include <linux/types.h> +#include <linux/errno.h> +#include <linux/kernel.h> +#include <linux/slab.h> +#include <linux/poll.h> +#include <linux/fcntl.h> +#include <linux/init.h> +#include <linux/skbuff.h> +#include <linux/interrupt.h> +#include <linux/notifier.h> +#include <net/sock.h> + +#include <asm/system.h> +#include <linux/uaccess.h> +#include <asm/unaligned.h> + +#include <net/bluetooth/bluetooth.h> +#include <net/bluetooth/hci_core.h> + +/* Handle HCI Event packets */ + +static void hci_cc_inquiry_cancel(struct hci_dev *hdev, struct sk_buff *skb) +{ + __u8 status = *((__u8 *) skb->data); + + BT_DBG("%s status 0x%x", hdev->name, status); + + if (status) { + hci_dev_lock(hdev); + mgmt_stop_discovery_failed(hdev, status); + hci_dev_unlock(hdev); + return; + } + + clear_bit(HCI_INQUIRY, &hdev->flags); + + hci_dev_lock(hdev); + hci_discovery_set_state(hdev, DISCOVERY_STOPPED); + hci_dev_unlock(hdev); + + hci_req_complete(hdev, HCI_OP_INQUIRY_CANCEL, status); + + hci_conn_check_pending(hdev); +} + +static void hci_cc_exit_periodic_inq(struct hci_dev *hdev, struct sk_buff *skb) +{ + __u8 status = *((__u8 *) skb->data); + + BT_DBG("%s status 0x%x", hdev->name, status); + + if (status) + return; + + hci_conn_check_pending(hdev); +} + +static void hci_cc_remote_name_req_cancel(struct hci_dev *hdev, struct sk_buff *skb) +{ + BT_DBG("%s", hdev->name); +} + +static void hci_cc_role_discovery(struct hci_dev *hdev, struct sk_buff *skb) +{ + struct hci_rp_role_discovery *rp = (void *) skb->data; + struct hci_conn *conn; + + BT_DBG("%s status 0x%x", hdev->name, rp->status); + + if (rp->status) + return; + + hci_dev_lock(hdev); + + conn = hci_conn_hash_lookup_handle(hdev, __le16_to_cpu(rp->handle)); + if (conn) { + if (rp->role) + conn->link_mode &= ~HCI_LM_MASTER; + else + conn->link_mode |= HCI_LM_MASTER; + } + + hci_dev_unlock(hdev); +} + +static void hci_cc_read_link_policy(struct hci_dev *hdev, struct sk_buff *skb) +{ + struct hci_rp_read_link_policy *rp = (void *) skb->data; + struct hci_conn *conn; + + BT_DBG("%s status 0x%x", hdev->name, rp->status); + + if (rp->status) + return; + + hci_dev_lock(hdev); + + conn = hci_conn_hash_lookup_handle(hdev, __le16_to_cpu(rp->handle)); + if (conn) + conn->link_policy = __le16_to_cpu(rp->policy); + + hci_dev_unlock(hdev); +} + +static void hci_cc_write_link_policy(struct hci_dev *hdev, struct sk_buff *skb) +{ + struct hci_rp_write_link_policy *rp = (void *) skb->data; + struct hci_conn *conn; + void *sent; + + BT_DBG("%s status 0x%x", hdev->name, rp->status); + + if (rp->status) + return; + + sent = hci_sent_cmd_data(hdev, HCI_OP_WRITE_LINK_POLICY); + if (!sent) + return; + + hci_dev_lock(hdev); + + conn = hci_conn_hash_lookup_handle(hdev, __le16_to_cpu(rp->handle)); + if (conn) + conn->link_policy = get_unaligned_le16(sent + 2); + + hci_dev_unlock(hdev); +} + +static void hci_cc_read_def_link_policy(struct hci_dev *hdev, struct sk_buff *skb) +{ + struct hci_rp_read_def_link_policy *rp = (void *) skb->data; + + BT_DBG("%s status 0x%x", hdev->name, rp->status); + + if (rp->status) + return; + + hdev->link_policy = __le16_to_cpu(rp->policy); +} + +static void hci_cc_write_def_link_policy(struct hci_dev *hdev, struct sk_buff *skb) +{ + __u8 status = *((__u8 *) skb->data); + void *sent; + + BT_DBG("%s status 0x%x", hdev->name, status); + + sent = hci_sent_cmd_data(hdev, HCI_OP_WRITE_DEF_LINK_POLICY); + if (!sent) + return; + + if (!status) + hdev->link_policy = get_unaligned_le16(sent); + + hci_req_complete(hdev, HCI_OP_WRITE_DEF_LINK_POLICY, status); +} + +static void hci_cc_reset(struct hci_dev *hdev, struct sk_buff *skb) +{ + __u8 status = *((__u8 *) skb->data); + + BT_DBG("%s status 0x%x", hdev->name, status); + + clear_bit(HCI_RESET, &hdev->flags); + + hci_req_complete(hdev, HCI_OP_RESET, status); + + /* Reset all non-persistent flags */ + hdev->dev_flags &= ~(BIT(HCI_LE_SCAN) | BIT(HCI_PENDING_CLASS)); + + hdev->discovery.state = DISCOVERY_STOPPED; +} + +static void hci_cc_write_local_name(struct hci_dev *hdev, struct sk_buff *skb) +{ + __u8 status = *((__u8 *) skb->data); + void *sent; + + BT_DBG("%s status 0x%x", hdev->name, status); + + sent = hci_sent_cmd_data(hdev, HCI_OP_WRITE_LOCAL_NAME); + if (!sent) + return; + + hci_dev_lock(hdev); + + if (test_bit(HCI_MGMT, &hdev->dev_flags)) + mgmt_set_local_name_complete(hdev, sent, status); + else if (!status) + memcpy(hdev->dev_name, sent, HCI_MAX_NAME_LENGTH); + + hci_dev_unlock(hdev); + + hci_req_complete(hdev, HCI_OP_WRITE_LOCAL_NAME, status); +} + +static void hci_cc_read_local_name(struct hci_dev *hdev, struct sk_buff *skb) +{ + struct hci_rp_read_local_name *rp = (void *) skb->data; + + BT_DBG("%s status 0x%x", hdev->name, rp->status); + + if (rp->status) + return; + + if (test_bit(HCI_SETUP, &hdev->dev_flags)) + memcpy(hdev->dev_name, rp->name, HCI_MAX_NAME_LENGTH); +} + +static void hci_cc_write_auth_enable(struct hci_dev *hdev, struct sk_buff *skb) +{ + __u8 status = *((__u8 *) skb->data); + void *sent; + + BT_DBG("%s status 0x%x", hdev->name, status); + + sent = hci_sent_cmd_data(hdev, HCI_OP_WRITE_AUTH_ENABLE); + if (!sent) + return; + + if (!status) { + __u8 param = *((__u8 *) sent); + + if (param == AUTH_ENABLED) + set_bit(HCI_AUTH, &hdev->flags); + else + clear_bit(HCI_AUTH, &hdev->flags); + } + + if (test_bit(HCI_MGMT, &hdev->dev_flags)) + mgmt_auth_enable_complete(hdev, status); + + hci_req_complete(hdev, HCI_OP_WRITE_AUTH_ENABLE, status); +} + +static void hci_cc_write_encrypt_mode(struct hci_dev *hdev, struct sk_buff *skb) +{ + __u8 status = *((__u8 *) skb->data); + void *sent; + + BT_DBG("%s status 0x%x", hdev->name, status); + + sent = hci_sent_cmd_data(hdev, HCI_OP_WRITE_ENCRYPT_MODE); + if (!sent) + return; + + if (!status) { + __u8 param = *((__u8 *) sent); + + if (param) + set_bit(HCI_ENCRYPT, &hdev->flags); + else + clear_bit(HCI_ENCRYPT, &hdev->flags); + } + + hci_req_complete(hdev, HCI_OP_WRITE_ENCRYPT_MODE, status); +} + +static void hci_cc_write_scan_enable(struct hci_dev *hdev, struct sk_buff *skb) +{ + __u8 param, status = *((__u8 *) skb->data); + int old_pscan, old_iscan; + void *sent; + + BT_DBG("%s status 0x%x", hdev->name, status); + + sent = hci_sent_cmd_data(hdev, HCI_OP_WRITE_SCAN_ENABLE); + if (!sent) + return; + + param = *((__u8 *) sent); + + hci_dev_lock(hdev); + + if (status != 0) { + mgmt_write_scan_failed(hdev, param, status); + hdev->discov_timeout = 0; + goto done; + } + + old_pscan = test_and_clear_bit(HCI_PSCAN, &hdev->flags); + old_iscan = test_and_clear_bit(HCI_ISCAN, &hdev->flags); + + if (param & SCAN_INQUIRY) { + set_bit(HCI_ISCAN, &hdev->flags); + if (!old_iscan) + mgmt_discoverable(hdev, 1); + if (hdev->discov_timeout > 0) { + int to = msecs_to_jiffies(hdev->discov_timeout * 1000); + queue_delayed_work(hdev->workqueue, &hdev->discov_off, + to); + } + } else if (old_iscan) + mgmt_discoverable(hdev, 0); + + if (param & SCAN_PAGE) { + set_bit(HCI_PSCAN, &hdev->flags); + if (!old_pscan) + mgmt_connectable(hdev, 1); + } else if (old_pscan) + mgmt_connectable(hdev, 0); + +done: + hci_dev_unlock(hdev); + hci_req_complete(hdev, HCI_OP_WRITE_SCAN_ENABLE, status); +} + +static void hci_cc_read_class_of_dev(struct hci_dev *hdev, struct sk_buff *skb) +{ + struct hci_rp_read_class_of_dev *rp = (void *) skb->data; + + BT_DBG("%s status 0x%x", hdev->name, rp->status); + + if (rp->status) + return; + + memcpy(hdev->dev_class, rp->dev_class, 3); + + BT_DBG("%s class 0x%.2x%.2x%.2x", hdev->name, + hdev->dev_class[2], hdev->dev_class[1], hdev->dev_class[0]); +} + +static void hci_cc_write_class_of_dev(struct hci_dev *hdev, struct sk_buff *skb) +{ + __u8 status = *((__u8 *) skb->data); + void *sent; + + BT_DBG("%s status 0x%x", hdev->name, status); + + sent = hci_sent_cmd_data(hdev, HCI_OP_WRITE_CLASS_OF_DEV); + if (!sent) + return; + + hci_dev_lock(hdev); + + if (status == 0) + memcpy(hdev->dev_class, sent, 3); + + if (test_bit(HCI_MGMT, &hdev->dev_flags)) + mgmt_set_class_of_dev_complete(hdev, sent, status); + + hci_dev_unlock(hdev); +} + +static void hci_cc_read_voice_setting(struct hci_dev *hdev, struct sk_buff *skb) +{ + struct hci_rp_read_voice_setting *rp = (void *) skb->data; + __u16 setting; + + BT_DBG("%s status 0x%x", hdev->name, rp->status); + + if (rp->status) + return; + + setting = __le16_to_cpu(rp->voice_setting); + + if (hdev->voice_setting == setting) + return; + + hdev->voice_setting = setting; + + BT_DBG("%s voice setting 0x%04x", hdev->name, setting); + + if (hdev->notify) + hdev->notify(hdev, HCI_NOTIFY_VOICE_SETTING); +} + +static void hci_cc_write_voice_setting(struct hci_dev *hdev, struct sk_buff *skb) +{ + __u8 status = *((__u8 *) skb->data); + __u16 setting; + void *sent; + + BT_DBG("%s status 0x%x", hdev->name, status); + + if (status) + return; + + sent = hci_sent_cmd_data(hdev, HCI_OP_WRITE_VOICE_SETTING); + if (!sent) + return; + + setting = get_unaligned_le16(sent); + + if (hdev->voice_setting == setting) + return; + + hdev->voice_setting = setting; + + BT_DBG("%s voice setting 0x%04x", hdev->name, setting); + + if (hdev->notify) + hdev->notify(hdev, HCI_NOTIFY_VOICE_SETTING); +} + +static void hci_cc_host_buffer_size(struct hci_dev *hdev, struct sk_buff *skb) +{ + __u8 status = *((__u8 *) skb->data); + + BT_DBG("%s status 0x%x", hdev->name, status); + + hci_req_complete(hdev, HCI_OP_HOST_BUFFER_SIZE, status); +} + +static void hci_cc_write_ssp_mode(struct hci_dev *hdev, struct sk_buff *skb) +{ + __u8 status = *((__u8 *) skb->data); + void *sent; + + BT_DBG("%s status 0x%x", hdev->name, status); + + sent = hci_sent_cmd_data(hdev, HCI_OP_WRITE_SSP_MODE); + if (!sent) + return; + + if (test_bit(HCI_MGMT, &hdev->dev_flags)) + mgmt_ssp_enable_complete(hdev, *((u8 *) sent), status); + else if (!status) { + if (*((u8 *) sent)) + set_bit(HCI_SSP_ENABLED, &hdev->dev_flags); + else + clear_bit(HCI_SSP_ENABLED, &hdev->dev_flags); + } +} + +static u8 hci_get_inquiry_mode(struct hci_dev *hdev) +{ + if (hdev->features[6] & LMP_EXT_INQ) + return 2; + + if (hdev->features[3] & LMP_RSSI_INQ) + return 1; + + if (hdev->manufacturer == 11 && hdev->hci_rev == 0x00 && + hdev->lmp_subver == 0x0757) + return 1; + + if (hdev->manufacturer == 15) { + if (hdev->hci_rev == 0x03 && hdev->lmp_subver == 0x6963) + return 1; + if (hdev->hci_rev == 0x09 && hdev->lmp_subver == 0x6963) + return 1; + if (hdev->hci_rev == 0x00 && hdev->lmp_subver == 0x6965) + return 1; + } + + if (hdev->manufacturer == 31 && hdev->hci_rev == 0x2005 && + hdev->lmp_subver == 0x1805) + return 1; + + return 0; +} + +static void hci_setup_inquiry_mode(struct hci_dev *hdev) +{ + u8 mode; + + mode = hci_get_inquiry_mode(hdev); + + hci_send_cmd(hdev, HCI_OP_WRITE_INQUIRY_MODE, 1, &mode); +} + +static void hci_setup_event_mask(struct hci_dev *hdev) +{ + /* The second byte is 0xff instead of 0x9f (two reserved bits + * disabled) since a Broadcom 1.2 dongle doesn't respond to the + * command otherwise */ + u8 events[8] = { 0xff, 0xff, 0xfb, 0xff, 0x00, 0x00, 0x00, 0x00 }; + + /* CSR 1.1 dongles does not accept any bitfield so don't try to set + * any event mask for pre 1.2 devices */ + if (hdev->hci_ver < BLUETOOTH_VER_1_2) + return; + + events[4] |= 0x01; /* Flow Specification Complete */ + events[4] |= 0x02; /* Inquiry Result with RSSI */ + events[4] |= 0x04; /* Read Remote Extended Features Complete */ + events[5] |= 0x08; /* Synchronous Connection Complete */ + events[5] |= 0x10; /* Synchronous Connection Changed */ + + if (hdev->features[3] & LMP_RSSI_INQ) + events[4] |= 0x02; /* Inquiry Result with RSSI */ + + if (hdev->features[5] & LMP_SNIFF_SUBR) + events[5] |= 0x20; /* Sniff Subrating */ + + if (hdev->features[5] & LMP_PAUSE_ENC) + events[5] |= 0x80; /* Encryption Key Refresh Complete */ + + if (hdev->features[6] & LMP_EXT_INQ) + events[5] |= 0x40; /* Extended Inquiry Result */ + + if (hdev->features[6] & LMP_NO_FLUSH) + events[7] |= 0x01; /* Enhanced Flush Complete */ + + if (hdev->features[7] & LMP_LSTO) + events[6] |= 0x80; /* Link Supervision Timeout Changed */ + + if (hdev->features[6] & LMP_SIMPLE_PAIR) { + events[6] |= 0x01; /* IO Capability Request */ + events[6] |= 0x02; /* IO Capability Response */ + events[6] |= 0x04; /* User Confirmation Request */ + events[6] |= 0x08; /* User Passkey Request */ + events[6] |= 0x10; /* Remote OOB Data Request */ + events[6] |= 0x20; /* Simple Pairing Complete */ + events[7] |= 0x04; /* User Passkey Notification */ + events[7] |= 0x08; /* Keypress Notification */ + events[7] |= 0x10; /* Remote Host Supported + * Features Notification */ + } + + if (hdev->features[4] & LMP_LE) + events[7] |= 0x20; /* LE Meta-Event */ + + hci_send_cmd(hdev, HCI_OP_SET_EVENT_MASK, sizeof(events), events); +} + +static void hci_setup(struct hci_dev *hdev) +{ + if (hdev->dev_type != HCI_BREDR) + return; + + hci_setup_event_mask(hdev); + + if (hdev->hci_ver > BLUETOOTH_VER_1_1) + hci_send_cmd(hdev, HCI_OP_READ_LOCAL_COMMANDS, 0, NULL); + + if (hdev->features[6] & LMP_SIMPLE_PAIR) { + if (test_bit(HCI_SSP_ENABLED, &hdev->dev_flags)) { + u8 mode = 0x01; + hci_send_cmd(hdev, HCI_OP_WRITE_SSP_MODE, + sizeof(mode), &mode); + } else { + struct hci_cp_write_eir cp; + + memset(hdev->eir, 0, sizeof(hdev->eir)); + memset(&cp, 0, sizeof(cp)); + + hci_send_cmd(hdev, HCI_OP_WRITE_EIR, sizeof(cp), &cp); + } + } + + if (hdev->features[3] & LMP_RSSI_INQ) + hci_setup_inquiry_mode(hdev); + + if (hdev->features[7] & LMP_INQ_TX_PWR) + hci_send_cmd(hdev, HCI_OP_READ_INQ_RSP_TX_POWER, 0, NULL); + + if (hdev->features[7] & LMP_EXTFEATURES) { + struct hci_cp_read_local_ext_features cp; + + cp.page = 0x01; + hci_send_cmd(hdev, HCI_OP_READ_LOCAL_EXT_FEATURES, sizeof(cp), + &cp); + } + + if (test_bit(HCI_LINK_SECURITY, &hdev->dev_flags)) { + u8 enable = 1; + hci_send_cmd(hdev, HCI_OP_WRITE_AUTH_ENABLE, sizeof(enable), + &enable); + } +} + +static void hci_cc_read_local_version(struct hci_dev *hdev, struct sk_buff *skb) +{ + struct hci_rp_read_local_version *rp = (void *) skb->data; + + BT_DBG("%s status 0x%x", hdev->name, rp->status); + + if (rp->status) + goto done; + + hdev->hci_ver = rp->hci_ver; + hdev->hci_rev = __le16_to_cpu(rp->hci_rev); + hdev->lmp_ver = rp->lmp_ver; + hdev->manufacturer = __le16_to_cpu(rp->manufacturer); + hdev->lmp_subver = __le16_to_cpu(rp->lmp_subver); + + BT_DBG("%s manufacturer %d hci ver %d:%d", hdev->name, + hdev->manufacturer, + hdev->hci_ver, hdev->hci_rev); + + if (test_bit(HCI_INIT, &hdev->flags)) + hci_setup(hdev); + +done: + hci_req_complete(hdev, HCI_OP_READ_LOCAL_VERSION, rp->status); +} + +static void hci_setup_link_policy(struct hci_dev *hdev) +{ + u16 link_policy = 0; + + if (hdev->features[0] & LMP_RSWITCH) + link_policy |= HCI_LP_RSWITCH; + if (hdev->features[0] & LMP_HOLD) + link_policy |= HCI_LP_HOLD; + if (hdev->features[0] & LMP_SNIFF) + link_policy |= HCI_LP_SNIFF; + if (hdev->features[1] & LMP_PARK) + link_policy |= HCI_LP_PARK; + + link_policy = cpu_to_le16(link_policy); + hci_send_cmd(hdev, HCI_OP_WRITE_DEF_LINK_POLICY, sizeof(link_policy), + &link_policy); +} + +static void hci_cc_read_local_commands(struct hci_dev *hdev, struct sk_buff *skb) +{ + struct hci_rp_read_local_commands *rp = (void *) skb->data; + + BT_DBG("%s status 0x%x", hdev->name, rp->status); + + if (rp->status) + goto done; + + memcpy(hdev->commands, rp->commands, sizeof(hdev->commands)); + + if (test_bit(HCI_INIT, &hdev->flags) && (hdev->commands[5] & 0x10)) + hci_setup_link_policy(hdev); + +done: + hci_req_complete(hdev, HCI_OP_READ_LOCAL_COMMANDS, rp->status); +} + +static void hci_cc_read_local_features(struct hci_dev *hdev, struct sk_buff *skb) +{ + struct hci_rp_read_local_features *rp = (void *) skb->data; + + BT_DBG("%s status 0x%x", hdev->name, rp->status); + + if (rp->status) + return; + + memcpy(hdev->features, rp->features, 8); + + /* Adjust default settings according to features + * supported by device. */ + + if (hdev->features[0] & LMP_3SLOT) + hdev->pkt_type |= (HCI_DM3 | HCI_DH3); + + if (hdev->features[0] & LMP_5SLOT) + hdev->pkt_type |= (HCI_DM5 | HCI_DH5); + + if (hdev->features[1] & LMP_HV2) { + hdev->pkt_type |= (HCI_HV2); + hdev->esco_type |= (ESCO_HV2); + } + + if (hdev->features[1] & LMP_HV3) { + hdev->pkt_type |= (HCI_HV3); + hdev->esco_type |= (ESCO_HV3); + } + + if (hdev->features[3] & LMP_ESCO) + hdev->esco_type |= (ESCO_EV3); + + if (hdev->features[4] & LMP_EV4) + hdev->esco_type |= (ESCO_EV4); + + if (hdev->features[4] & LMP_EV5) + hdev->esco_type |= (ESCO_EV5); + + if (hdev->features[5] & LMP_EDR_ESCO_2M) + hdev->esco_type |= (ESCO_2EV3); + + if (hdev->features[5] & LMP_EDR_ESCO_3M) + hdev->esco_type |= (ESCO_3EV3); + + if (hdev->features[5] & LMP_EDR_3S_ESCO) + hdev->esco_type |= (ESCO_2EV5 | ESCO_3EV5); + + BT_DBG("%s features 0x%.2x%.2x%.2x%.2x%.2x%.2x%.2x%.2x", hdev->name, + hdev->features[0], hdev->features[1], + hdev->features[2], hdev->features[3], + hdev->features[4], hdev->features[5], + hdev->features[6], hdev->features[7]); +} + +static void hci_set_le_support(struct hci_dev *hdev) +{ + struct hci_cp_write_le_host_supported cp; + + memset(&cp, 0, sizeof(cp)); + + if (enable_le && test_bit(HCI_LE_ENABLED, &hdev->dev_flags)) { + cp.le = 1; + cp.simul = !!(hdev->features[6] & LMP_SIMUL_LE_BR); + } + + if (cp.le != !!(hdev->host_features[0] & LMP_HOST_LE)) + hci_send_cmd(hdev, HCI_OP_WRITE_LE_HOST_SUPPORTED, sizeof(cp), + &cp); +} + +static void hci_cc_read_local_ext_features(struct hci_dev *hdev, + struct sk_buff *skb) +{ + struct hci_rp_read_local_ext_features *rp = (void *) skb->data; + + BT_DBG("%s status 0x%x", hdev->name, rp->status); + + if (rp->status) + goto done; + + switch (rp->page) { + case 0: + memcpy(hdev->features, rp->features, 8); + break; + case 1: + memcpy(hdev->host_features, rp->features, 8); + break; + } + + if (test_bit(HCI_INIT, &hdev->flags) && hdev->features[4] & LMP_LE) + hci_set_le_support(hdev); + +done: + hci_req_complete(hdev, HCI_OP_READ_LOCAL_EXT_FEATURES, rp->status); +} + +static void hci_cc_read_flow_control_mode(struct hci_dev *hdev, + struct sk_buff *skb) +{ + struct hci_rp_read_flow_control_mode *rp = (void *) skb->data; + + BT_DBG("%s status 0x%x", hdev->name, rp->status); + + if (rp->status) + return; + + hdev->flow_ctl_mode = rp->mode; + + hci_req_complete(hdev, HCI_OP_READ_FLOW_CONTROL_MODE, rp->status); +} + +static void hci_cc_read_buffer_size(struct hci_dev *hdev, struct sk_buff *skb) +{ + struct hci_rp_read_buffer_size *rp = (void *) skb->data; + + BT_DBG("%s status 0x%x", hdev->name, rp->status); + + if (rp->status) + return; + + hdev->acl_mtu = __le16_to_cpu(rp->acl_mtu); + hdev->sco_mtu = rp->sco_mtu; + hdev->acl_pkts = __le16_to_cpu(rp->acl_max_pkt); + hdev->sco_pkts = __le16_to_cpu(rp->sco_max_pkt); + + if (test_bit(HCI_QUIRK_FIXUP_BUFFER_SIZE, &hdev->quirks)) { + hdev->sco_mtu = 64; + hdev->sco_pkts = 8; + } + + hdev->acl_cnt = hdev->acl_pkts; + hdev->sco_cnt = hdev->sco_pkts; + + BT_DBG("%s acl mtu %d:%d sco mtu %d:%d", hdev->name, + hdev->acl_mtu, hdev->acl_pkts, + hdev->sco_mtu, hdev->sco_pkts); +} + +static void hci_cc_read_bd_addr(struct hci_dev *hdev, struct sk_buff *skb) +{ + struct hci_rp_read_bd_addr *rp = (void *) skb->data; + + BT_DBG("%s status 0x%x", hdev->name, rp->status); + + if (!rp->status) + bacpy(&hdev->bdaddr, &rp->bdaddr); + + hci_req_complete(hdev, HCI_OP_READ_BD_ADDR, rp->status); +} + +static void hci_cc_read_data_block_size(struct hci_dev *hdev, + struct sk_buff *skb) +{ + struct hci_rp_read_data_block_size *rp = (void *) skb->data; + + BT_DBG("%s status 0x%x", hdev->name, rp->status); + + if (rp->status) + return; + + hdev->block_mtu = __le16_to_cpu(rp->max_acl_len); + hdev->block_len = __le16_to_cpu(rp->block_len); + hdev->num_blocks = __le16_to_cpu(rp->num_blocks); + + hdev->block_cnt = hdev->num_blocks; + + BT_DBG("%s blk mtu %d cnt %d len %d", hdev->name, hdev->block_mtu, + hdev->block_cnt, hdev->block_len); + + hci_req_complete(hdev, HCI_OP_READ_DATA_BLOCK_SIZE, rp->status); +} + +static void hci_cc_write_ca_timeout(struct hci_dev *hdev, struct sk_buff *skb) +{ + __u8 status = *((__u8 *) skb->data); + + BT_DBG("%s status 0x%x", hdev->name, status); + + hci_req_complete(hdev, HCI_OP_WRITE_CA_TIMEOUT, status); +} + +static void hci_cc_read_local_amp_info(struct hci_dev *hdev, + struct sk_buff *skb) +{ + struct hci_rp_read_local_amp_info *rp = (void *) skb->data; + + BT_DBG("%s status 0x%x", hdev->name, rp->status); + + if (rp->status) + return; + + hdev->amp_status = rp->amp_status; + hdev->amp_total_bw = __le32_to_cpu(rp->total_bw); + hdev->amp_max_bw = __le32_to_cpu(rp->max_bw); + hdev->amp_min_latency = __le32_to_cpu(rp->min_latency); + hdev->amp_max_pdu = __le32_to_cpu(rp->max_pdu); + hdev->amp_type = rp->amp_type; + hdev->amp_pal_cap = __le16_to_cpu(rp->pal_cap); + hdev->amp_assoc_size = __le16_to_cpu(rp->max_assoc_size); + hdev->amp_be_flush_to = __le32_to_cpu(rp->be_flush_to); + hdev->amp_max_flush_to = __le32_to_cpu(rp->max_flush_to); + + hci_req_complete(hdev, HCI_OP_READ_LOCAL_AMP_INFO, rp->status); +} + +static void hci_cc_delete_stored_link_key(struct hci_dev *hdev, + struct sk_buff *skb) +{ + __u8 status = *((__u8 *) skb->data); + + BT_DBG("%s status 0x%x", hdev->name, status); + + hci_req_complete(hdev, HCI_OP_DELETE_STORED_LINK_KEY, status); +} + +static void hci_cc_set_event_mask(struct hci_dev *hdev, struct sk_buff *skb) +{ + __u8 status = *((__u8 *) skb->data); + + BT_DBG("%s status 0x%x", hdev->name, status); + + hci_req_complete(hdev, HCI_OP_SET_EVENT_MASK, status); +} + +static void hci_cc_write_inquiry_mode(struct hci_dev *hdev, + struct sk_buff *skb) +{ + __u8 status = *((__u8 *) skb->data); + + BT_DBG("%s status 0x%x", hdev->name, status); + + hci_req_complete(hdev, HCI_OP_WRITE_INQUIRY_MODE, status); +} + +static void hci_cc_read_inq_rsp_tx_power(struct hci_dev *hdev, + struct sk_buff *skb) +{ + __u8 status = *((__u8 *) skb->data); + + BT_DBG("%s status 0x%x", hdev->name, status); + + hci_req_complete(hdev, HCI_OP_READ_INQ_RSP_TX_POWER, status); +} + +static void hci_cc_set_event_flt(struct hci_dev *hdev, struct sk_buff *skb) +{ + __u8 status = *((__u8 *) skb->data); + + BT_DBG("%s status 0x%x", hdev->name, status); + + hci_req_complete(hdev, HCI_OP_SET_EVENT_FLT, status); +} + +static void hci_cc_pin_code_reply(struct hci_dev *hdev, struct sk_buff *skb) +{ + struct hci_rp_pin_code_reply *rp = (void *) skb->data; + struct hci_cp_pin_code_reply *cp; + struct hci_conn *conn; + + BT_DBG("%s status 0x%x", hdev->name, rp->status); + + hci_dev_lock(hdev); + + if (test_bit(HCI_MGMT, &hdev->dev_flags)) + mgmt_pin_code_reply_complete(hdev, &rp->bdaddr, rp->status); + + if (rp->status != 0) + goto unlock; + + cp = hci_sent_cmd_data(hdev, HCI_OP_PIN_CODE_REPLY); + if (!cp) + goto unlock; + + conn = hci_conn_hash_lookup_ba(hdev, ACL_LINK, &cp->bdaddr); + if (conn) + conn->pin_length = cp->pin_len; + +unlock: + hci_dev_unlock(hdev); +} + +static void hci_cc_pin_code_neg_reply(struct hci_dev *hdev, struct sk_buff *skb) +{ + struct hci_rp_pin_code_neg_reply *rp = (void *) skb->data; + + BT_DBG("%s status 0x%x", hdev->name, rp->status); + + hci_dev_lock(hdev); + + if (test_bit(HCI_MGMT, &hdev->dev_flags)) + mgmt_pin_code_neg_reply_complete(hdev, &rp->bdaddr, + rp->status); + + hci_dev_unlock(hdev); +} + +static void hci_cc_le_read_buffer_size(struct hci_dev *hdev, + struct sk_buff *skb) +{ + struct hci_rp_le_read_buffer_size *rp = (void *) skb->data; + + BT_DBG("%s status 0x%x", hdev->name, rp->status); + + if (rp->status) + return; + + hdev->le_mtu = __le16_to_cpu(rp->le_mtu); + hdev->le_pkts = rp->le_max_pkt; + + hdev->le_cnt = hdev->le_pkts; + + BT_DBG("%s le mtu %d:%d", hdev->name, hdev->le_mtu, hdev->le_pkts); + + hci_req_complete(hdev, HCI_OP_LE_READ_BUFFER_SIZE, rp->status); +} + +static void hci_cc_user_confirm_reply(struct hci_dev *hdev, struct sk_buff *skb) +{ + struct hci_rp_user_confirm_reply *rp = (void *) skb->data; + + BT_DBG("%s status 0x%x", hdev->name, rp->status); + + hci_dev_lock(hdev); + + if (test_bit(HCI_MGMT, &hdev->dev_flags)) + mgmt_user_confirm_reply_complete(hdev, &rp->bdaddr, ACL_LINK, 0, + rp->status); + + hci_dev_unlock(hdev); +} + +static void hci_cc_user_confirm_neg_reply(struct hci_dev *hdev, + struct sk_buff *skb) +{ + struct hci_rp_user_confirm_reply *rp = (void *) skb->data; + + BT_DBG("%s status 0x%x", hdev->name, rp->status); + + hci_dev_lock(hdev); + + if (test_bit(HCI_MGMT, &hdev->dev_flags)) + mgmt_user_confirm_neg_reply_complete(hdev, &rp->bdaddr, + ACL_LINK, 0, rp->status); + + hci_dev_unlock(hdev); +} + +static void hci_cc_user_passkey_reply(struct hci_dev *hdev, struct sk_buff *skb) +{ + struct hci_rp_user_confirm_reply *rp = (void *) skb->data; + + BT_DBG("%s status 0x%x", hdev->name, rp->status); + + hci_dev_lock(hdev); + + if (test_bit(HCI_MGMT, &hdev->dev_flags)) + mgmt_user_passkey_reply_complete(hdev, &rp->bdaddr, ACL_LINK, + 0, rp->status); + + hci_dev_unlock(hdev); +} + +static void hci_cc_user_passkey_neg_reply(struct hci_dev *hdev, + struct sk_buff *skb) +{ + struct hci_rp_user_confirm_reply *rp = (void *) skb->data; + + BT_DBG("%s status 0x%x", hdev->name, rp->status); + + hci_dev_lock(hdev); + + if (test_bit(HCI_MGMT, &hdev->dev_flags)) + mgmt_user_passkey_neg_reply_complete(hdev, &rp->bdaddr, + ACL_LINK, 0, rp->status); + + hci_dev_unlock(hdev); +} + +static void hci_cc_read_local_oob_data_reply(struct hci_dev *hdev, + struct sk_buff *skb) +{ + struct hci_rp_read_local_oob_data *rp = (void *) skb->data; + + BT_DBG("%s status 0x%x", hdev->name, rp->status); + + hci_dev_lock(hdev); + mgmt_read_local_oob_data_reply_complete(hdev, rp->hash, + rp->randomizer, rp->status); + hci_dev_unlock(hdev); +} + +static void hci_cc_le_set_scan_param(struct hci_dev *hdev, struct sk_buff *skb) +{ + __u8 status = *((__u8 *) skb->data); + + BT_DBG("%s status 0x%x", hdev->name, status); + + hci_req_complete(hdev, HCI_OP_LE_SET_SCAN_PARAM, status); + + if (status) { + hci_dev_lock(hdev); + mgmt_start_discovery_failed(hdev, status); + hci_dev_unlock(hdev); + return; + } +} + +static void hci_cc_le_set_scan_enable(struct hci_dev *hdev, + struct sk_buff *skb) +{ + struct hci_cp_le_set_scan_enable *cp; + __u8 status = *((__u8 *) skb->data); + + BT_DBG("%s status 0x%x", hdev->name, status); + + cp = hci_sent_cmd_data(hdev, HCI_OP_LE_SET_SCAN_ENABLE); + if (!cp) + return; + + switch (cp->enable) { + case LE_SCANNING_ENABLED: + hci_req_complete(hdev, HCI_OP_LE_SET_SCAN_ENABLE, status); + + if (status) { + hci_dev_lock(hdev); + mgmt_start_discovery_failed(hdev, status); + hci_dev_unlock(hdev); + return; + } + + set_bit(HCI_LE_SCAN, &hdev->dev_flags); + + cancel_delayed_work_sync(&hdev->adv_work); + + hci_dev_lock(hdev); + hci_adv_entries_clear(hdev); + hci_discovery_set_state(hdev, DISCOVERY_FINDING); + hci_dev_unlock(hdev); + break; + + case LE_SCANNING_DISABLED: + if (status) + return; + + clear_bit(HCI_LE_SCAN, &hdev->dev_flags); + + schedule_delayed_work(&hdev->adv_work, ADV_CLEAR_TIMEOUT); + + if (hdev->discovery.type == DISCOV_TYPE_INTERLEAVED) { + mgmt_interleaved_discovery(hdev); + } else { + hci_dev_lock(hdev); + hci_discovery_set_state(hdev, DISCOVERY_STOPPED); + hci_dev_unlock(hdev); + } + + break; + + default: + BT_ERR("Used reserved LE_Scan_Enable param %d", cp->enable); + break; + } +} + +static void hci_cc_le_ltk_reply(struct hci_dev *hdev, struct sk_buff *skb) +{ + struct hci_rp_le_ltk_reply *rp = (void *) skb->data; + + BT_DBG("%s status 0x%x", hdev->name, rp->status); + + if (rp->status) + return; + + hci_req_complete(hdev, HCI_OP_LE_LTK_REPLY, rp->status); +} + +static void hci_cc_le_ltk_neg_reply(struct hci_dev *hdev, struct sk_buff *skb) +{ + struct hci_rp_le_ltk_neg_reply *rp = (void *) skb->data; + + BT_DBG("%s status 0x%x", hdev->name, rp->status); + + if (rp->status) + return; + + hci_req_complete(hdev, HCI_OP_LE_LTK_NEG_REPLY, rp->status); +} + +static inline void hci_cc_write_le_host_supported(struct hci_dev *hdev, + struct sk_buff *skb) +{ + struct hci_cp_write_le_host_supported *sent; + __u8 status = *((__u8 *) skb->data); + + BT_DBG("%s status 0x%x", hdev->name, status); + + sent = hci_sent_cmd_data(hdev, HCI_OP_WRITE_LE_HOST_SUPPORTED); + if (!sent) + return; + + if (!status) { + if (sent->le) + hdev->host_features[0] |= LMP_HOST_LE; + else + hdev->host_features[0] &= ~LMP_HOST_LE; + } + + if (test_bit(HCI_MGMT, &hdev->dev_flags) && + !test_bit(HCI_INIT, &hdev->flags)) + mgmt_le_enable_complete(hdev, sent->le, status); + + hci_req_complete(hdev, HCI_OP_WRITE_LE_HOST_SUPPORTED, status); +} + +static inline void hci_cs_inquiry(struct hci_dev *hdev, __u8 status) +{ + BT_DBG("%s status 0x%x", hdev->name, status); + + if (status) { + hci_req_complete(hdev, HCI_OP_INQUIRY, status); + hci_conn_check_pending(hdev); + hci_dev_lock(hdev); + if (test_bit(HCI_MGMT, &hdev->dev_flags)) + mgmt_start_discovery_failed(hdev, status); + hci_dev_unlock(hdev); + return; + } + + set_bit(HCI_INQUIRY, &hdev->flags); + + hci_dev_lock(hdev); + hci_discovery_set_state(hdev, DISCOVERY_FINDING); + hci_dev_unlock(hdev); +} + +static inline void hci_cs_create_conn(struct hci_dev *hdev, __u8 status) +{ + struct hci_cp_create_conn *cp; + struct hci_conn *conn; + + BT_DBG("%s status 0x%x", hdev->name, status); + + cp = hci_sent_cmd_data(hdev, HCI_OP_CREATE_CONN); + if (!cp) + return; + + hci_dev_lock(hdev); + + conn = hci_conn_hash_lookup_ba(hdev, ACL_LINK, &cp->bdaddr); + + BT_DBG("%s bdaddr %s conn %p", hdev->name, batostr(&cp->bdaddr), conn); + + if (status) { + if (conn && conn->state == BT_CONNECT) { + if (status != 0x0c || conn->attempt > 2) { + conn->state = BT_CLOSED; + hci_proto_connect_cfm(conn, status); + hci_conn_del(conn); + } else + conn->state = BT_CONNECT2; + } + } else { + if (!conn) { + conn = hci_conn_add(hdev, ACL_LINK, &cp->bdaddr); + if (conn) { + conn->out = true; + conn->link_mode |= HCI_LM_MASTER; + } else + BT_ERR("No memory for new connection"); + } + } + + hci_dev_unlock(hdev); +} + +static void hci_cs_add_sco(struct hci_dev *hdev, __u8 status) +{ + struct hci_cp_add_sco *cp; + struct hci_conn *acl, *sco; + __u16 handle; + + BT_DBG("%s status 0x%x", hdev->name, status); + + if (!status) + return; + + cp = hci_sent_cmd_data(hdev, HCI_OP_ADD_SCO); + if (!cp) + return; + + handle = __le16_to_cpu(cp->handle); + + BT_DBG("%s handle %d", hdev->name, handle); + + hci_dev_lock(hdev); + + acl = hci_conn_hash_lookup_handle(hdev, handle); + if (acl) { + sco = acl->link; + if (sco) { + sco->state = BT_CLOSED; + + hci_proto_connect_cfm(sco, status); + hci_conn_del(sco); + } + } + + hci_dev_unlock(hdev); +} + +static void hci_cs_auth_requested(struct hci_dev *hdev, __u8 status) +{ + struct hci_cp_auth_requested *cp; + struct hci_conn *conn; + + BT_DBG("%s status 0x%x", hdev->name, status); + + if (!status) + return; + + cp = hci_sent_cmd_data(hdev, HCI_OP_AUTH_REQUESTED); + if (!cp) + return; + + hci_dev_lock(hdev); + + conn = hci_conn_hash_lookup_handle(hdev, __le16_to_cpu(cp->handle)); + if (conn) { + if (conn->state == BT_CONFIG) { + hci_proto_connect_cfm(conn, status); + hci_conn_put(conn); + } + } + + hci_dev_unlock(hdev); +} + +static void hci_cs_set_conn_encrypt(struct hci_dev *hdev, __u8 status) +{ + struct hci_cp_set_conn_encrypt *cp; + struct hci_conn *conn; + + BT_DBG("%s status 0x%x", hdev->name, status); + + if (!status) + return; + + cp = hci_sent_cmd_data(hdev, HCI_OP_SET_CONN_ENCRYPT); + if (!cp) + return; + + hci_dev_lock(hdev); + + conn = hci_conn_hash_lookup_handle(hdev, __le16_to_cpu(cp->handle)); + if (conn) { + if (conn->state == BT_CONFIG) { + hci_proto_connect_cfm(conn, status); + hci_conn_put(conn); + } + } + + hci_dev_unlock(hdev); +} + +static int hci_outgoing_auth_needed(struct hci_dev *hdev, + struct hci_conn *conn) +{ + if (conn->state != BT_CONFIG || !conn->out) + return 0; + + if (conn->pending_sec_level == BT_SECURITY_SDP) + return 0; + + /* Only request authentication for SSP connections or non-SSP + * devices with sec_level HIGH or if MITM protection is requested */ + if (!hci_conn_ssp_enabled(conn) && + conn->pending_sec_level != BT_SECURITY_HIGH && + !(conn->auth_type & 0x01)) + return 0; + + return 1; +} + +static inline int hci_resolve_name(struct hci_dev *hdev, + struct inquiry_entry *e) +{ + struct hci_cp_remote_name_req cp; + + memset(&cp, 0, sizeof(cp)); + + bacpy(&cp.bdaddr, &e->data.bdaddr); + cp.pscan_rep_mode = e->data.pscan_rep_mode; + cp.pscan_mode = e->data.pscan_mode; + cp.clock_offset = e->data.clock_offset; + + return hci_send_cmd(hdev, HCI_OP_REMOTE_NAME_REQ, sizeof(cp), &cp); +} + +static bool hci_resolve_next_name(struct hci_dev *hdev) +{ + struct discovery_state *discov = &hdev->discovery; + struct inquiry_entry *e; + + if (list_empty(&discov->resolve)) + return false; + + e = hci_inquiry_cache_lookup_resolve(hdev, BDADDR_ANY, NAME_NEEDED); + if (hci_resolve_name(hdev, e) == 0) { + e->name_state = NAME_PENDING; + return true; + } + + return false; +} + +static void hci_check_pending_name(struct hci_dev *hdev, struct hci_conn *conn, + bdaddr_t *bdaddr, u8 *name, u8 name_len) +{ + struct discovery_state *discov = &hdev->discovery; + struct inquiry_entry *e; + + if (conn && !test_and_set_bit(HCI_CONN_MGMT_CONNECTED, &conn->flags)) + mgmt_device_connected(hdev, bdaddr, ACL_LINK, 0x00, 0, name, + name_len, conn->dev_class); + + if (discov->state == DISCOVERY_STOPPED) + return; + + if (discov->state == DISCOVERY_STOPPING) + goto discov_complete; + + if (discov->state != DISCOVERY_RESOLVING) + return; + + e = hci_inquiry_cache_lookup_resolve(hdev, bdaddr, NAME_PENDING); + if (e) { + e->name_state = NAME_KNOWN; + list_del(&e->list); + if (name) + mgmt_remote_name(hdev, bdaddr, ACL_LINK, 0x00, + e->data.rssi, name, name_len); + } + + if (hci_resolve_next_name(hdev)) + return; + +discov_complete: + hci_discovery_set_state(hdev, DISCOVERY_STOPPED); +} + +static void hci_cs_remote_name_req(struct hci_dev *hdev, __u8 status) +{ + struct hci_cp_remote_name_req *cp; + struct hci_conn *conn; + + BT_DBG("%s status 0x%x", hdev->name, status); + + /* If successful wait for the name req complete event before + * checking for the need to do authentication */ + if (!status) + return; + + cp = hci_sent_cmd_data(hdev, HCI_OP_REMOTE_NAME_REQ); + if (!cp) + return; + + hci_dev_lock(hdev); + + conn = hci_conn_hash_lookup_ba(hdev, ACL_LINK, &cp->bdaddr); + + if (test_bit(HCI_MGMT, &hdev->dev_flags)) + hci_check_pending_name(hdev, conn, &cp->bdaddr, NULL, 0); + + if (!conn) + goto unlock; + + if (!hci_outgoing_auth_needed(hdev, conn)) + goto unlock; + + if (!test_and_set_bit(HCI_CONN_AUTH_PEND, &conn->flags)) { + struct hci_cp_auth_requested cp; + cp.handle = __cpu_to_le16(conn->handle); + hci_send_cmd(hdev, HCI_OP_AUTH_REQUESTED, sizeof(cp), &cp); + } + +unlock: + hci_dev_unlock(hdev); +} + +static void hci_cs_read_remote_features(struct hci_dev *hdev, __u8 status) +{ + struct hci_cp_read_remote_features *cp; + struct hci_conn *conn; + + BT_DBG("%s status 0x%x", hdev->name, status); + + if (!status) + return; + + cp = hci_sent_cmd_data(hdev, HCI_OP_READ_REMOTE_FEATURES); + if (!cp) + return; + + hci_dev_lock(hdev); + + conn = hci_conn_hash_lookup_handle(hdev, __le16_to_cpu(cp->handle)); + if (conn) { + if (conn->state == BT_CONFIG) { + hci_proto_connect_cfm(conn, status); + hci_conn_put(conn); + } + } + + hci_dev_unlock(hdev); +} + +static void hci_cs_read_remote_ext_features(struct hci_dev *hdev, __u8 status) +{ + struct hci_cp_read_remote_ext_features *cp; + struct hci_conn *conn; + + BT_DBG("%s status 0x%x", hdev->name, status); + + if (!status) + return; + + cp = hci_sent_cmd_data(hdev, HCI_OP_READ_REMOTE_EXT_FEATURES); + if (!cp) + return; + + hci_dev_lock(hdev); + + conn = hci_conn_hash_lookup_handle(hdev, __le16_to_cpu(cp->handle)); + if (conn) { + if (conn->state == BT_CONFIG) { + hci_proto_connect_cfm(conn, status); + hci_conn_put(conn); + } + } + + hci_dev_unlock(hdev); +} + +static void hci_cs_setup_sync_conn(struct hci_dev *hdev, __u8 status) +{ + struct hci_cp_setup_sync_conn *cp; + struct hci_conn *acl, *sco; + __u16 handle; + + BT_DBG("%s status 0x%x", hdev->name, status); + + if (!status) + return; + + cp = hci_sent_cmd_data(hdev, HCI_OP_SETUP_SYNC_CONN); + if (!cp) + return; + + handle = __le16_to_cpu(cp->handle); + + BT_DBG("%s handle %d", hdev->name, handle); + + hci_dev_lock(hdev); + + acl = hci_conn_hash_lookup_handle(hdev, handle); + if (acl) { + sco = acl->link; + if (sco) { + sco->state = BT_CLOSED; + + hci_proto_connect_cfm(sco, status); + hci_conn_del(sco); + } + } + + hci_dev_unlock(hdev); +} + +static void hci_cs_sniff_mode(struct hci_dev *hdev, __u8 status) +{ + struct hci_cp_sniff_mode *cp; + struct hci_conn *conn; + + BT_DBG("%s status 0x%x", hdev->name, status); + + if (!status) + return; + + cp = hci_sent_cmd_data(hdev, HCI_OP_SNIFF_MODE); + if (!cp) + return; + + hci_dev_lock(hdev); + + conn = hci_conn_hash_lookup_handle(hdev, __le16_to_cpu(cp->handle)); + if (conn) { + clear_bit(HCI_CONN_MODE_CHANGE_PEND, &conn->flags); + + if (test_and_clear_bit(HCI_CONN_SCO_SETUP_PEND, &conn->flags)) + hci_sco_setup(conn, status); + } + + hci_dev_unlock(hdev); +} + +static void hci_cs_exit_sniff_mode(struct hci_dev *hdev, __u8 status) +{ + struct hci_cp_exit_sniff_mode *cp; + struct hci_conn *conn; + + BT_DBG("%s status 0x%x", hdev->name, status); + + if (!status) + return; + + cp = hci_sent_cmd_data(hdev, HCI_OP_EXIT_SNIFF_MODE); + if (!cp) + return; + + hci_dev_lock(hdev); + + conn = hci_conn_hash_lookup_handle(hdev, __le16_to_cpu(cp->handle)); + if (conn) { + clear_bit(HCI_CONN_MODE_CHANGE_PEND, &conn->flags); + + if (test_and_clear_bit(HCI_CONN_SCO_SETUP_PEND, &conn->flags)) + hci_sco_setup(conn, status); + } + + hci_dev_unlock(hdev); +} + +static void hci_cs_disconnect(struct hci_dev *hdev, u8 status) +{ + struct hci_cp_disconnect *cp; + struct hci_conn *conn; + + if (!status) + return; + + cp = hci_sent_cmd_data(hdev, HCI_OP_DISCONNECT); + if (!cp) + return; + + hci_dev_lock(hdev); + + conn = hci_conn_hash_lookup_handle(hdev, __le16_to_cpu(cp->handle)); + if (conn) + mgmt_disconnect_failed(hdev, &conn->dst, conn->type, + conn->dst_type, status); + + hci_dev_unlock(hdev); +} + +static void hci_cs_le_create_conn(struct hci_dev *hdev, __u8 status) +{ + struct hci_cp_le_create_conn *cp; + struct hci_conn *conn; + + BT_DBG("%s status 0x%x", hdev->name, status); + + cp = hci_sent_cmd_data(hdev, HCI_OP_LE_CREATE_CONN); + if (!cp) + return; + + hci_dev_lock(hdev); + + conn = hci_conn_hash_lookup_ba(hdev, LE_LINK, &cp->peer_addr); + + BT_DBG("%s bdaddr %s conn %p", hdev->name, batostr(&cp->peer_addr), + conn); + + if (status) { + if (conn && conn->state == BT_CONNECT) { + conn->state = BT_CLOSED; + hci_proto_connect_cfm(conn, status); + hci_conn_del(conn); + } + } else { + if (!conn) { + conn = hci_conn_add(hdev, LE_LINK, &cp->peer_addr); + if (conn) { + conn->dst_type = cp->peer_addr_type; + conn->out = true; + } else { + BT_ERR("No memory for new connection"); + } + } + } + + hci_dev_unlock(hdev); +} + +static void hci_cs_le_start_enc(struct hci_dev *hdev, u8 status) +{ + BT_DBG("%s status 0x%x", hdev->name, status); +} + +static inline void hci_inquiry_complete_evt(struct hci_dev *hdev, struct sk_buff *skb) +{ + __u8 status = *((__u8 *) skb->data); + struct discovery_state *discov = &hdev->discovery; + struct inquiry_entry *e; + + BT_DBG("%s status %d", hdev->name, status); + + hci_req_complete(hdev, HCI_OP_INQUIRY, status); + + hci_conn_check_pending(hdev); + + if (!test_and_clear_bit(HCI_INQUIRY, &hdev->flags)) + return; + + if (!test_bit(HCI_MGMT, &hdev->dev_flags)) + return; + + hci_dev_lock(hdev); + + if (discov->state != DISCOVERY_FINDING) + goto unlock; + + if (list_empty(&discov->resolve)) { + hci_discovery_set_state(hdev, DISCOVERY_STOPPED); + goto unlock; + } + + e = hci_inquiry_cache_lookup_resolve(hdev, BDADDR_ANY, NAME_NEEDED); + if (e && hci_resolve_name(hdev, e) == 0) { + e->name_state = NAME_PENDING; + hci_discovery_set_state(hdev, DISCOVERY_RESOLVING); + } else { + hci_discovery_set_state(hdev, DISCOVERY_STOPPED); + } + +unlock: + hci_dev_unlock(hdev); +} + +static inline void hci_inquiry_result_evt(struct hci_dev *hdev, struct sk_buff *skb) +{ + struct inquiry_data data; + struct inquiry_info *info = (void *) (skb->data + 1); + int num_rsp = *((__u8 *) skb->data); + + BT_DBG("%s num_rsp %d", hdev->name, num_rsp); + + if (!num_rsp) + return; + + hci_dev_lock(hdev); + + for (; num_rsp; num_rsp--, info++) { + bool name_known, ssp; + + bacpy(&data.bdaddr, &info->bdaddr); + data.pscan_rep_mode = info->pscan_rep_mode; + data.pscan_period_mode = info->pscan_period_mode; + data.pscan_mode = info->pscan_mode; + memcpy(data.dev_class, info->dev_class, 3); + data.clock_offset = info->clock_offset; + data.rssi = 0x00; + data.ssp_mode = 0x00; + + name_known = hci_inquiry_cache_update(hdev, &data, false, &ssp); + mgmt_device_found(hdev, &info->bdaddr, ACL_LINK, 0x00, + info->dev_class, 0, !name_known, ssp, NULL, + 0); + } + + hci_dev_unlock(hdev); +} + +static inline void hci_conn_complete_evt(struct hci_dev *hdev, struct sk_buff *skb) +{ + struct hci_ev_conn_complete *ev = (void *) skb->data; + struct hci_conn *conn; + + BT_DBG("%s", hdev->name); + + hci_dev_lock(hdev); + + conn = hci_conn_hash_lookup_ba(hdev, ev->link_type, &ev->bdaddr); + if (!conn) { + if (ev->link_type != SCO_LINK) + goto unlock; + + conn = hci_conn_hash_lookup_ba(hdev, ESCO_LINK, &ev->bdaddr); + if (!conn) + goto unlock; + + conn->type = SCO_LINK; + } + + if (!ev->status) { + conn->handle = __le16_to_cpu(ev->handle); + + if (conn->type == ACL_LINK) { + conn->state = BT_CONFIG; + hci_conn_hold(conn); + conn->disc_timeout = HCI_DISCONN_TIMEOUT; + } else + conn->state = BT_CONNECTED; + + hci_conn_hold_device(conn); + hci_conn_add_sysfs(conn); + + if (test_bit(HCI_AUTH, &hdev->flags)) + conn->link_mode |= HCI_LM_AUTH; + + if (test_bit(HCI_ENCRYPT, &hdev->flags)) + conn->link_mode |= HCI_LM_ENCRYPT; + + /* Get remote features */ + if (conn->type == ACL_LINK) { + struct hci_cp_read_remote_features cp; + cp.handle = ev->handle; + hci_send_cmd(hdev, HCI_OP_READ_REMOTE_FEATURES, + sizeof(cp), &cp); + } + + /* Set packet type for incoming connection */ + if (!conn->out && hdev->hci_ver < BLUETOOTH_VER_2_0) { + struct hci_cp_change_conn_ptype cp; + cp.handle = ev->handle; + cp.pkt_type = cpu_to_le16(conn->pkt_type); + hci_send_cmd(hdev, HCI_OP_CHANGE_CONN_PTYPE, sizeof(cp), + &cp); + } + } else { + conn->state = BT_CLOSED; + if (conn->type == ACL_LINK) + mgmt_connect_failed(hdev, &ev->bdaddr, conn->type, + conn->dst_type, ev->status); + } + + if (conn->type == ACL_LINK) + hci_sco_setup(conn, ev->status); + + if (ev->status) { + hci_proto_connect_cfm(conn, ev->status); + hci_conn_del(conn); + } else if (ev->link_type != ACL_LINK) + hci_proto_connect_cfm(conn, ev->status); + +unlock: + hci_dev_unlock(hdev); + + hci_conn_check_pending(hdev); +} + +static inline void hci_conn_request_evt(struct hci_dev *hdev, struct sk_buff *skb) +{ + struct hci_ev_conn_request *ev = (void *) skb->data; + int mask = hdev->link_mode; + + BT_DBG("%s bdaddr %s type 0x%x", hdev->name, + batostr(&ev->bdaddr), ev->link_type); + + mask |= hci_proto_connect_ind(hdev, &ev->bdaddr, ev->link_type); + + if ((mask & HCI_LM_ACCEPT) && + !hci_blacklist_lookup(hdev, &ev->bdaddr)) { + /* Connection accepted */ + struct inquiry_entry *ie; + struct hci_conn *conn; + + hci_dev_lock(hdev); + + ie = hci_inquiry_cache_lookup(hdev, &ev->bdaddr); + if (ie) + memcpy(ie->data.dev_class, ev->dev_class, 3); + + conn = hci_conn_hash_lookup_ba(hdev, ev->link_type, &ev->bdaddr); + if (!conn) { + conn = hci_conn_add(hdev, ev->link_type, &ev->bdaddr); + if (!conn) { + BT_ERR("No memory for new connection"); + hci_dev_unlock(hdev); + return; + } + } + + memcpy(conn->dev_class, ev->dev_class, 3); + conn->state = BT_CONNECT; + + hci_dev_unlock(hdev); + + if (ev->link_type == ACL_LINK || !lmp_esco_capable(hdev)) { + struct hci_cp_accept_conn_req cp; + + bacpy(&cp.bdaddr, &ev->bdaddr); + + if (lmp_rswitch_capable(hdev) && (mask & HCI_LM_MASTER)) + cp.role = 0x00; /* Become master */ + else + cp.role = 0x01; /* Remain slave */ + + hci_send_cmd(hdev, HCI_OP_ACCEPT_CONN_REQ, sizeof(cp), + &cp); + } else { + struct hci_cp_accept_sync_conn_req cp; + + bacpy(&cp.bdaddr, &ev->bdaddr); + cp.pkt_type = cpu_to_le16(conn->pkt_type); + + cp.tx_bandwidth = cpu_to_le32(0x00001f40); + cp.rx_bandwidth = cpu_to_le32(0x00001f40); + cp.max_latency = cpu_to_le16(0xffff); + cp.content_format = cpu_to_le16(hdev->voice_setting); + cp.retrans_effort = 0xff; + + hci_send_cmd(hdev, HCI_OP_ACCEPT_SYNC_CONN_REQ, + sizeof(cp), &cp); + } + } else { + /* Connection rejected */ + struct hci_cp_reject_conn_req cp; + + bacpy(&cp.bdaddr, &ev->bdaddr); + cp.reason = HCI_ERROR_REJ_BAD_ADDR; + hci_send_cmd(hdev, HCI_OP_REJECT_CONN_REQ, sizeof(cp), &cp); + } +} + +static inline void hci_disconn_complete_evt(struct hci_dev *hdev, struct sk_buff *skb) +{ + struct hci_ev_disconn_complete *ev = (void *) skb->data; + struct hci_conn *conn; + + BT_DBG("%s status %d", hdev->name, ev->status); + + hci_dev_lock(hdev); + + conn = hci_conn_hash_lookup_handle(hdev, __le16_to_cpu(ev->handle)); + if (!conn) + goto unlock; + + if (ev->status == 0) + conn->state = BT_CLOSED; + + if (test_and_clear_bit(HCI_CONN_MGMT_CONNECTED, &conn->flags) && + (conn->type == ACL_LINK || conn->type == LE_LINK)) { + if (ev->status != 0) + mgmt_disconnect_failed(hdev, &conn->dst, conn->type, + conn->dst_type, ev->status); + else + mgmt_device_disconnected(hdev, &conn->dst, conn->type, + conn->dst_type); + } + + if (ev->status == 0) { + if (conn->type == ACL_LINK && conn->flush_key) + hci_remove_link_key(hdev, &conn->dst); + hci_proto_disconn_cfm(conn, ev->reason); + hci_conn_del(conn); + } + +unlock: + hci_dev_unlock(hdev); +} + +static inline void hci_auth_complete_evt(struct hci_dev *hdev, struct sk_buff *skb) +{ + struct hci_ev_auth_complete *ev = (void *) skb->data; + struct hci_conn *conn; + + BT_DBG("%s status %d", hdev->name, ev->status); + + hci_dev_lock(hdev); + + conn = hci_conn_hash_lookup_handle(hdev, __le16_to_cpu(ev->handle)); + if (!conn) + goto unlock; + + if (!ev->status) { + if (!hci_conn_ssp_enabled(conn) && + test_bit(HCI_CONN_REAUTH_PEND, &conn->flags)) { + BT_INFO("re-auth of legacy device is not possible."); + } else { + conn->link_mode |= HCI_LM_AUTH; + conn->sec_level = conn->pending_sec_level; + } + } else { + mgmt_auth_failed(hdev, &conn->dst, conn->type, conn->dst_type, + ev->status); + } + + clear_bit(HCI_CONN_AUTH_PEND, &conn->flags); + clear_bit(HCI_CONN_REAUTH_PEND, &conn->flags); + + if (conn->state == BT_CONFIG) { + if (!ev->status && hci_conn_ssp_enabled(conn)) { + struct hci_cp_set_conn_encrypt cp; + cp.handle = ev->handle; + cp.encrypt = 0x01; + hci_send_cmd(hdev, HCI_OP_SET_CONN_ENCRYPT, sizeof(cp), + &cp); + } else { + conn->state = BT_CONNECTED; + hci_proto_connect_cfm(conn, ev->status); + hci_conn_put(conn); + } + } else { + hci_auth_cfm(conn, ev->status); + + hci_conn_hold(conn); + conn->disc_timeout = HCI_DISCONN_TIMEOUT; + hci_conn_put(conn); + } + + if (test_bit(HCI_CONN_ENCRYPT_PEND, &conn->flags)) { + if (!ev->status) { + struct hci_cp_set_conn_encrypt cp; + cp.handle = ev->handle; + cp.encrypt = 0x01; + hci_send_cmd(hdev, HCI_OP_SET_CONN_ENCRYPT, sizeof(cp), + &cp); + } else { + clear_bit(HCI_CONN_ENCRYPT_PEND, &conn->flags); + hci_encrypt_cfm(conn, ev->status, 0x00); + } + } + +unlock: + hci_dev_unlock(hdev); +} + +static inline void hci_remote_name_evt(struct hci_dev *hdev, struct sk_buff *skb) +{ + struct hci_ev_remote_name *ev = (void *) skb->data; + struct hci_conn *conn; + + BT_DBG("%s", hdev->name); + + hci_conn_check_pending(hdev); + + hci_dev_lock(hdev); + + conn = hci_conn_hash_lookup_ba(hdev, ACL_LINK, &ev->bdaddr); + + if (!test_bit(HCI_MGMT, &hdev->dev_flags)) + goto check_auth; + + if (ev->status == 0) + hci_check_pending_name(hdev, conn, &ev->bdaddr, ev->name, + strnlen(ev->name, HCI_MAX_NAME_LENGTH)); + else + hci_check_pending_name(hdev, conn, &ev->bdaddr, NULL, 0); + +check_auth: + if (!conn) + goto unlock; + + if (!hci_outgoing_auth_needed(hdev, conn)) + goto unlock; + + if (!test_and_set_bit(HCI_CONN_AUTH_PEND, &conn->flags)) { + struct hci_cp_auth_requested cp; + cp.handle = __cpu_to_le16(conn->handle); + hci_send_cmd(hdev, HCI_OP_AUTH_REQUESTED, sizeof(cp), &cp); + } + +unlock: + hci_dev_unlock(hdev); +} + +static inline void hci_encrypt_change_evt(struct hci_dev *hdev, struct sk_buff *skb) +{ + struct hci_ev_encrypt_change *ev = (void *) skb->data; + struct hci_conn *conn; + + BT_DBG("%s status %d", hdev->name, ev->status); + + hci_dev_lock(hdev); + + conn = hci_conn_hash_lookup_handle(hdev, __le16_to_cpu(ev->handle)); + if (conn) { + if (!ev->status) { + if (ev->encrypt) { + /* Encryption implies authentication */ + conn->link_mode |= HCI_LM_AUTH; + conn->link_mode |= HCI_LM_ENCRYPT; + conn->sec_level = conn->pending_sec_level; + } else + conn->link_mode &= ~HCI_LM_ENCRYPT; + } + + clear_bit(HCI_CONN_ENCRYPT_PEND, &conn->flags); + + if (ev->status && conn->state == BT_CONNECTED) { + hci_acl_disconn(conn, 0x13); + hci_conn_put(conn); + goto unlock; + } + + if (conn->state == BT_CONFIG) { + if (!ev->status) + conn->state = BT_CONNECTED; + + hci_proto_connect_cfm(conn, ev->status); + hci_conn_put(conn); + } else + hci_encrypt_cfm(conn, ev->status, ev->encrypt); + } + +unlock: + hci_dev_unlock(hdev); +} + +static inline void hci_change_link_key_complete_evt(struct hci_dev *hdev, struct sk_buff *skb) +{ + struct hci_ev_change_link_key_complete *ev = (void *) skb->data; + struct hci_conn *conn; + + BT_DBG("%s status %d", hdev->name, ev->status); + + hci_dev_lock(hdev); + + conn = hci_conn_hash_lookup_handle(hdev, __le16_to_cpu(ev->handle)); + if (conn) { + if (!ev->status) + conn->link_mode |= HCI_LM_SECURE; + + clear_bit(HCI_CONN_AUTH_PEND, &conn->flags); + + hci_key_change_cfm(conn, ev->status); + } + + hci_dev_unlock(hdev); +} + +static inline void hci_remote_features_evt(struct hci_dev *hdev, struct sk_buff *skb) +{ + struct hci_ev_remote_features *ev = (void *) skb->data; + struct hci_conn *conn; + + BT_DBG("%s status %d", hdev->name, ev->status); + + hci_dev_lock(hdev); + + conn = hci_conn_hash_lookup_handle(hdev, __le16_to_cpu(ev->handle)); + if (!conn) + goto unlock; + + if (!ev->status) + memcpy(conn->features, ev->features, 8); + + if (conn->state != BT_CONFIG) + goto unlock; + + if (!ev->status && lmp_ssp_capable(hdev) && lmp_ssp_capable(conn)) { + struct hci_cp_read_remote_ext_features cp; + cp.handle = ev->handle; + cp.page = 0x01; + hci_send_cmd(hdev, HCI_OP_READ_REMOTE_EXT_FEATURES, + sizeof(cp), &cp); + goto unlock; + } + + if (!ev->status && !test_bit(HCI_CONN_MGMT_CONNECTED, &conn->flags)) { + struct hci_cp_remote_name_req cp; + memset(&cp, 0, sizeof(cp)); + bacpy(&cp.bdaddr, &conn->dst); + cp.pscan_rep_mode = 0x02; + hci_send_cmd(hdev, HCI_OP_REMOTE_NAME_REQ, sizeof(cp), &cp); + } else if (!test_and_set_bit(HCI_CONN_MGMT_CONNECTED, &conn->flags)) + mgmt_device_connected(hdev, &conn->dst, conn->type, + conn->dst_type, 0, NULL, 0, + conn->dev_class); + + if (!hci_outgoing_auth_needed(hdev, conn)) { + conn->state = BT_CONNECTED; + hci_proto_connect_cfm(conn, ev->status); + hci_conn_put(conn); + } + +unlock: + hci_dev_unlock(hdev); +} + +static inline void hci_remote_version_evt(struct hci_dev *hdev, struct sk_buff *skb) +{ + BT_DBG("%s", hdev->name); +} + +static inline void hci_qos_setup_complete_evt(struct hci_dev *hdev, struct sk_buff *skb) +{ + BT_DBG("%s", hdev->name); +} + +static inline void hci_cmd_complete_evt(struct hci_dev *hdev, struct sk_buff *skb) +{ + struct hci_ev_cmd_complete *ev = (void *) skb->data; + __u16 opcode; + + skb_pull(skb, sizeof(*ev)); + + opcode = __le16_to_cpu(ev->opcode); + + switch (opcode) { + case HCI_OP_INQUIRY_CANCEL: + hci_cc_inquiry_cancel(hdev, skb); + break; + + case HCI_OP_EXIT_PERIODIC_INQ: + hci_cc_exit_periodic_inq(hdev, skb); + break; + + case HCI_OP_REMOTE_NAME_REQ_CANCEL: + hci_cc_remote_name_req_cancel(hdev, skb); + break; + + case HCI_OP_ROLE_DISCOVERY: + hci_cc_role_discovery(hdev, skb); + break; + + case HCI_OP_READ_LINK_POLICY: + hci_cc_read_link_policy(hdev, skb); + break; + + case HCI_OP_WRITE_LINK_POLICY: + hci_cc_write_link_policy(hdev, skb); + break; + + case HCI_OP_READ_DEF_LINK_POLICY: + hci_cc_read_def_link_policy(hdev, skb); + break; + + case HCI_OP_WRITE_DEF_LINK_POLICY: + hci_cc_write_def_link_policy(hdev, skb); + break; + + case HCI_OP_RESET: + hci_cc_reset(hdev, skb); + break; + + case HCI_OP_WRITE_LOCAL_NAME: + hci_cc_write_local_name(hdev, skb); + break; + + case HCI_OP_READ_LOCAL_NAME: + hci_cc_read_local_name(hdev, skb); + break; + + case HCI_OP_WRITE_AUTH_ENABLE: + hci_cc_write_auth_enable(hdev, skb); + break; + + case HCI_OP_WRITE_ENCRYPT_MODE: + hci_cc_write_encrypt_mode(hdev, skb); + break; + + case HCI_OP_WRITE_SCAN_ENABLE: + hci_cc_write_scan_enable(hdev, skb); + break; + + case HCI_OP_READ_CLASS_OF_DEV: + hci_cc_read_class_of_dev(hdev, skb); + break; + + case HCI_OP_WRITE_CLASS_OF_DEV: + hci_cc_write_class_of_dev(hdev, skb); + break; + + case HCI_OP_READ_VOICE_SETTING: + hci_cc_read_voice_setting(hdev, skb); + break; + + case HCI_OP_WRITE_VOICE_SETTING: + hci_cc_write_voice_setting(hdev, skb); + break; + + case HCI_OP_HOST_BUFFER_SIZE: + hci_cc_host_buffer_size(hdev, skb); + break; + + case HCI_OP_WRITE_SSP_MODE: + hci_cc_write_ssp_mode(hdev, skb); + break; + + case HCI_OP_READ_LOCAL_VERSION: + hci_cc_read_local_version(hdev, skb); + break; + + case HCI_OP_READ_LOCAL_COMMANDS: + hci_cc_read_local_commands(hdev, skb); + break; + + case HCI_OP_READ_LOCAL_FEATURES: + hci_cc_read_local_features(hdev, skb); + break; + + case HCI_OP_READ_LOCAL_EXT_FEATURES: + hci_cc_read_local_ext_features(hdev, skb); + break; + + case HCI_OP_READ_BUFFER_SIZE: + hci_cc_read_buffer_size(hdev, skb); + break; + + case HCI_OP_READ_BD_ADDR: + hci_cc_read_bd_addr(hdev, skb); + break; + + case HCI_OP_READ_DATA_BLOCK_SIZE: + hci_cc_read_data_block_size(hdev, skb); + break; + + case HCI_OP_WRITE_CA_TIMEOUT: + hci_cc_write_ca_timeout(hdev, skb); + break; + + case HCI_OP_READ_FLOW_CONTROL_MODE: + hci_cc_read_flow_control_mode(hdev, skb); + break; + + case HCI_OP_READ_LOCAL_AMP_INFO: + hci_cc_read_local_amp_info(hdev, skb); + break; + + case HCI_OP_DELETE_STORED_LINK_KEY: + hci_cc_delete_stored_link_key(hdev, skb); + break; + + case HCI_OP_SET_EVENT_MASK: + hci_cc_set_event_mask(hdev, skb); + break; + + case HCI_OP_WRITE_INQUIRY_MODE: + hci_cc_write_inquiry_mode(hdev, skb); + break; + + case HCI_OP_READ_INQ_RSP_TX_POWER: + hci_cc_read_inq_rsp_tx_power(hdev, skb); + break; + + case HCI_OP_SET_EVENT_FLT: + hci_cc_set_event_flt(hdev, skb); + break; + + case HCI_OP_PIN_CODE_REPLY: + hci_cc_pin_code_reply(hdev, skb); + break; + + case HCI_OP_PIN_CODE_NEG_REPLY: + hci_cc_pin_code_neg_reply(hdev, skb); + break; + + case HCI_OP_READ_LOCAL_OOB_DATA: + hci_cc_read_local_oob_data_reply(hdev, skb); + break; + + case HCI_OP_LE_READ_BUFFER_SIZE: + hci_cc_le_read_buffer_size(hdev, skb); + break; + + case HCI_OP_USER_CONFIRM_REPLY: + hci_cc_user_confirm_reply(hdev, skb); + break; + + case HCI_OP_USER_CONFIRM_NEG_REPLY: + hci_cc_user_confirm_neg_reply(hdev, skb); + break; + + case HCI_OP_USER_PASSKEY_REPLY: + hci_cc_user_passkey_reply(hdev, skb); + break; + + case HCI_OP_USER_PASSKEY_NEG_REPLY: + hci_cc_user_passkey_neg_reply(hdev, skb); + break; + + case HCI_OP_LE_SET_SCAN_PARAM: + hci_cc_le_set_scan_param(hdev, skb); + break; + + case HCI_OP_LE_SET_SCAN_ENABLE: + hci_cc_le_set_scan_enable(hdev, skb); + break; + + case HCI_OP_LE_LTK_REPLY: + hci_cc_le_ltk_reply(hdev, skb); + break; + + case HCI_OP_LE_LTK_NEG_REPLY: + hci_cc_le_ltk_neg_reply(hdev, skb); + break; + + case HCI_OP_WRITE_LE_HOST_SUPPORTED: + hci_cc_write_le_host_supported(hdev, skb); + break; + + default: + BT_DBG("%s opcode 0x%x", hdev->name, opcode); + break; + } + + if (ev->opcode != HCI_OP_NOP) + del_timer(&hdev->cmd_timer); + + if (ev->ncmd) { + atomic_set(&hdev->cmd_cnt, 1); + if (!skb_queue_empty(&hdev->cmd_q)) + queue_work(hdev->workqueue, &hdev->cmd_work); + } +} + +static inline void hci_cmd_status_evt(struct hci_dev *hdev, struct sk_buff *skb) +{ + struct hci_ev_cmd_status *ev = (void *) skb->data; + __u16 opcode; + + skb_pull(skb, sizeof(*ev)); + + opcode = __le16_to_cpu(ev->opcode); + + switch (opcode) { + case HCI_OP_INQUIRY: + hci_cs_inquiry(hdev, ev->status); + break; + + case HCI_OP_CREATE_CONN: + hci_cs_create_conn(hdev, ev->status); + break; + + case HCI_OP_ADD_SCO: + hci_cs_add_sco(hdev, ev->status); + break; + + case HCI_OP_AUTH_REQUESTED: + hci_cs_auth_requested(hdev, ev->status); + break; + + case HCI_OP_SET_CONN_ENCRYPT: + hci_cs_set_conn_encrypt(hdev, ev->status); + break; + + case HCI_OP_REMOTE_NAME_REQ: + hci_cs_remote_name_req(hdev, ev->status); + break; + + case HCI_OP_READ_REMOTE_FEATURES: + hci_cs_read_remote_features(hdev, ev->status); + break; + + case HCI_OP_READ_REMOTE_EXT_FEATURES: + hci_cs_read_remote_ext_features(hdev, ev->status); + break; + + case HCI_OP_SETUP_SYNC_CONN: + hci_cs_setup_sync_conn(hdev, ev->status); + break; + + case HCI_OP_SNIFF_MODE: + hci_cs_sniff_mode(hdev, ev->status); + break; + + case HCI_OP_EXIT_SNIFF_MODE: + hci_cs_exit_sniff_mode(hdev, ev->status); + break; + + case HCI_OP_DISCONNECT: + hci_cs_disconnect(hdev, ev->status); + break; + + case HCI_OP_LE_CREATE_CONN: + hci_cs_le_create_conn(hdev, ev->status); + break; + + case HCI_OP_LE_START_ENC: + hci_cs_le_start_enc(hdev, ev->status); + break; + + default: + BT_DBG("%s opcode 0x%x", hdev->name, opcode); + break; + } + + if (ev->opcode != HCI_OP_NOP) + del_timer(&hdev->cmd_timer); + + if (ev->ncmd && !test_bit(HCI_RESET, &hdev->flags)) { + atomic_set(&hdev->cmd_cnt, 1); + if (!skb_queue_empty(&hdev->cmd_q)) + queue_work(hdev->workqueue, &hdev->cmd_work); + } +} + +static inline void hci_role_change_evt(struct hci_dev *hdev, struct sk_buff *skb) +{ + struct hci_ev_role_change *ev = (void *) skb->data; + struct hci_conn *conn; + + BT_DBG("%s status %d", hdev->name, ev->status); + + hci_dev_lock(hdev); + + conn = hci_conn_hash_lookup_ba(hdev, ACL_LINK, &ev->bdaddr); + if (conn) { + if (!ev->status) { + if (ev->role) + conn->link_mode &= ~HCI_LM_MASTER; + else + conn->link_mode |= HCI_LM_MASTER; + } + + clear_bit(HCI_CONN_RSWITCH_PEND, &conn->flags); + + hci_role_switch_cfm(conn, ev->status, ev->role); + } + + hci_dev_unlock(hdev); +} + +static inline void hci_num_comp_pkts_evt(struct hci_dev *hdev, struct sk_buff *skb) +{ + struct hci_ev_num_comp_pkts *ev = (void *) skb->data; + int i; + + if (hdev->flow_ctl_mode != HCI_FLOW_CTL_MODE_PACKET_BASED) { + BT_ERR("Wrong event for mode %d", hdev->flow_ctl_mode); + return; + } + + if (skb->len < sizeof(*ev) || skb->len < sizeof(*ev) + + ev->num_hndl * sizeof(struct hci_comp_pkts_info)) { + BT_DBG("%s bad parameters", hdev->name); + return; + } + + BT_DBG("%s num_hndl %d", hdev->name, ev->num_hndl); + + for (i = 0; i < ev->num_hndl; i++) { + struct hci_comp_pkts_info *info = &ev->handles[i]; + struct hci_conn *conn; + __u16 handle, count; + + handle = __le16_to_cpu(info->handle); + count = __le16_to_cpu(info->count); + + conn = hci_conn_hash_lookup_handle(hdev, handle); + if (!conn) + continue; + + conn->sent -= count; + + switch (conn->type) { + case ACL_LINK: + hdev->acl_cnt += count; + if (hdev->acl_cnt > hdev->acl_pkts) + hdev->acl_cnt = hdev->acl_pkts; + break; + + case LE_LINK: + if (hdev->le_pkts) { + hdev->le_cnt += count; + if (hdev->le_cnt > hdev->le_pkts) + hdev->le_cnt = hdev->le_pkts; + } else { + hdev->acl_cnt += count; + if (hdev->acl_cnt > hdev->acl_pkts) + hdev->acl_cnt = hdev->acl_pkts; + } + break; + + case SCO_LINK: + hdev->sco_cnt += count; + if (hdev->sco_cnt > hdev->sco_pkts) + hdev->sco_cnt = hdev->sco_pkts; + break; + + default: + BT_ERR("Unknown type %d conn %p", conn->type, conn); + break; + } + } + + queue_work(hdev->workqueue, &hdev->tx_work); +} + +static inline void hci_num_comp_blocks_evt(struct hci_dev *hdev, + struct sk_buff *skb) +{ + struct hci_ev_num_comp_blocks *ev = (void *) skb->data; + int i; + + if (hdev->flow_ctl_mode != HCI_FLOW_CTL_MODE_BLOCK_BASED) { + BT_ERR("Wrong event for mode %d", hdev->flow_ctl_mode); + return; + } + + if (skb->len < sizeof(*ev) || skb->len < sizeof(*ev) + + ev->num_hndl * sizeof(struct hci_comp_blocks_info)) { + BT_DBG("%s bad parameters", hdev->name); + return; + } + + BT_DBG("%s num_blocks %d num_hndl %d", hdev->name, ev->num_blocks, + ev->num_hndl); + + for (i = 0; i < ev->num_hndl; i++) { + struct hci_comp_blocks_info *info = &ev->handles[i]; + struct hci_conn *conn; + __u16 handle, block_count; + + handle = __le16_to_cpu(info->handle); + block_count = __le16_to_cpu(info->blocks); + + conn = hci_conn_hash_lookup_handle(hdev, handle); + if (!conn) + continue; + + conn->sent -= block_count; + + switch (conn->type) { + case ACL_LINK: + hdev->block_cnt += block_count; + if (hdev->block_cnt > hdev->num_blocks) + hdev->block_cnt = hdev->num_blocks; + break; + + default: + BT_ERR("Unknown type %d conn %p", conn->type, conn); + break; + } + } + + queue_work(hdev->workqueue, &hdev->tx_work); +} + +static inline void hci_mode_change_evt(struct hci_dev *hdev, struct sk_buff *skb) +{ + struct hci_ev_mode_change *ev = (void *) skb->data; + struct hci_conn *conn; + + BT_DBG("%s status %d", hdev->name, ev->status); + + hci_dev_lock(hdev); + + conn = hci_conn_hash_lookup_handle(hdev, __le16_to_cpu(ev->handle)); + if (conn) { + conn->mode = ev->mode; + conn->interval = __le16_to_cpu(ev->interval); + + if (!test_and_clear_bit(HCI_CONN_MODE_CHANGE_PEND, &conn->flags)) { + if (conn->mode == HCI_CM_ACTIVE) + set_bit(HCI_CONN_POWER_SAVE, &conn->flags); + else + clear_bit(HCI_CONN_POWER_SAVE, &conn->flags); + } + + if (test_and_clear_bit(HCI_CONN_SCO_SETUP_PEND, &conn->flags)) + hci_sco_setup(conn, ev->status); + } + + hci_dev_unlock(hdev); +} + +static inline void hci_pin_code_request_evt(struct hci_dev *hdev, struct sk_buff *skb) +{ + struct hci_ev_pin_code_req *ev = (void *) skb->data; + struct hci_conn *conn; + + BT_DBG("%s", hdev->name); + + hci_dev_lock(hdev); + + conn = hci_conn_hash_lookup_ba(hdev, ACL_LINK, &ev->bdaddr); + if (!conn) + goto unlock; + + if (conn->state == BT_CONNECTED) { + hci_conn_hold(conn); + conn->disc_timeout = HCI_PAIRING_TIMEOUT; + hci_conn_put(conn); + } + + if (!test_bit(HCI_PAIRABLE, &hdev->dev_flags)) + hci_send_cmd(hdev, HCI_OP_PIN_CODE_NEG_REPLY, + sizeof(ev->bdaddr), &ev->bdaddr); + else if (test_bit(HCI_MGMT, &hdev->dev_flags)) { + u8 secure; + + if (conn->pending_sec_level == BT_SECURITY_HIGH) + secure = 1; + else + secure = 0; + + mgmt_pin_code_request(hdev, &ev->bdaddr, secure); + } + +unlock: + hci_dev_unlock(hdev); +} + +static inline void hci_link_key_request_evt(struct hci_dev *hdev, struct sk_buff *skb) +{ + struct hci_ev_link_key_req *ev = (void *) skb->data; + struct hci_cp_link_key_reply cp; + struct hci_conn *conn; + struct link_key *key; + + BT_DBG("%s", hdev->name); + + if (!test_bit(HCI_LINK_KEYS, &hdev->dev_flags)) + return; + + hci_dev_lock(hdev); + + key = hci_find_link_key(hdev, &ev->bdaddr); + if (!key) { + BT_DBG("%s link key not found for %s", hdev->name, + batostr(&ev->bdaddr)); + goto not_found; + } + + BT_DBG("%s found key type %u for %s", hdev->name, key->type, + batostr(&ev->bdaddr)); + + if (!test_bit(HCI_DEBUG_KEYS, &hdev->dev_flags) && + key->type == HCI_LK_DEBUG_COMBINATION) { + BT_DBG("%s ignoring debug key", hdev->name); + goto not_found; + } + + conn = hci_conn_hash_lookup_ba(hdev, ACL_LINK, &ev->bdaddr); + if (conn) { + if (key->type == HCI_LK_UNAUTH_COMBINATION && + conn->auth_type != 0xff && + (conn->auth_type & 0x01)) { + BT_DBG("%s ignoring unauthenticated key", hdev->name); + goto not_found; + } + + if (key->type == HCI_LK_COMBINATION && key->pin_len < 16 && + conn->pending_sec_level == BT_SECURITY_HIGH) { + BT_DBG("%s ignoring key unauthenticated for high \ + security", hdev->name); + goto not_found; + } + + conn->key_type = key->type; + conn->pin_length = key->pin_len; + } + + bacpy(&cp.bdaddr, &ev->bdaddr); + memcpy(cp.link_key, key->val, 16); + + hci_send_cmd(hdev, HCI_OP_LINK_KEY_REPLY, sizeof(cp), &cp); + + hci_dev_unlock(hdev); + + return; + +not_found: + hci_send_cmd(hdev, HCI_OP_LINK_KEY_NEG_REPLY, 6, &ev->bdaddr); + hci_dev_unlock(hdev); +} + +static inline void hci_link_key_notify_evt(struct hci_dev *hdev, struct sk_buff *skb) +{ + struct hci_ev_link_key_notify *ev = (void *) skb->data; + struct hci_conn *conn; + u8 pin_len = 0; + + BT_DBG("%s", hdev->name); + + hci_dev_lock(hdev); + + conn = hci_conn_hash_lookup_ba(hdev, ACL_LINK, &ev->bdaddr); + if (conn) { + hci_conn_hold(conn); + conn->disc_timeout = HCI_DISCONN_TIMEOUT; + pin_len = conn->pin_length; + + if (ev->key_type != HCI_LK_CHANGED_COMBINATION) + conn->key_type = ev->key_type; + + hci_conn_put(conn); + } + + if (test_bit(HCI_LINK_KEYS, &hdev->dev_flags)) + hci_add_link_key(hdev, conn, 1, &ev->bdaddr, ev->link_key, + ev->key_type, pin_len); + + hci_dev_unlock(hdev); +} + +static inline void hci_clock_offset_evt(struct hci_dev *hdev, struct sk_buff *skb) +{ + struct hci_ev_clock_offset *ev = (void *) skb->data; + struct hci_conn *conn; + + BT_DBG("%s status %d", hdev->name, ev->status); + + hci_dev_lock(hdev); + + conn = hci_conn_hash_lookup_handle(hdev, __le16_to_cpu(ev->handle)); + if (conn && !ev->status) { + struct inquiry_entry *ie; + + ie = hci_inquiry_cache_lookup(hdev, &conn->dst); + if (ie) { + ie->data.clock_offset = ev->clock_offset; + ie->timestamp = jiffies; + } + } + + hci_dev_unlock(hdev); +} + +static inline void hci_pkt_type_change_evt(struct hci_dev *hdev, struct sk_buff *skb) +{ + struct hci_ev_pkt_type_change *ev = (void *) skb->data; + struct hci_conn *conn; + + BT_DBG("%s status %d", hdev->name, ev->status); + + hci_dev_lock(hdev); + + conn = hci_conn_hash_lookup_handle(hdev, __le16_to_cpu(ev->handle)); + if (conn && !ev->status) + conn->pkt_type = __le16_to_cpu(ev->pkt_type); + + hci_dev_unlock(hdev); +} + +static inline void hci_pscan_rep_mode_evt(struct hci_dev *hdev, struct sk_buff *skb) +{ + struct hci_ev_pscan_rep_mode *ev = (void *) skb->data; + struct inquiry_entry *ie; + + BT_DBG("%s", hdev->name); + + hci_dev_lock(hdev); + + ie = hci_inquiry_cache_lookup(hdev, &ev->bdaddr); + if (ie) { + ie->data.pscan_rep_mode = ev->pscan_rep_mode; + ie->timestamp = jiffies; + } + + hci_dev_unlock(hdev); +} + +static inline void hci_inquiry_result_with_rssi_evt(struct hci_dev *hdev, struct sk_buff *skb) +{ + struct inquiry_data data; + int num_rsp = *((__u8 *) skb->data); + bool name_known, ssp; + + BT_DBG("%s num_rsp %d", hdev->name, num_rsp); + + if (!num_rsp) + return; + + hci_dev_lock(hdev); + + if ((skb->len - 1) / num_rsp != sizeof(struct inquiry_info_with_rssi)) { + struct inquiry_info_with_rssi_and_pscan_mode *info; + info = (void *) (skb->data + 1); + + for (; num_rsp; num_rsp--, info++) { + bacpy(&data.bdaddr, &info->bdaddr); + data.pscan_rep_mode = info->pscan_rep_mode; + data.pscan_period_mode = info->pscan_period_mode; + data.pscan_mode = info->pscan_mode; + memcpy(data.dev_class, info->dev_class, 3); + data.clock_offset = info->clock_offset; + data.rssi = info->rssi; + data.ssp_mode = 0x00; + + name_known = hci_inquiry_cache_update(hdev, &data, + false, &ssp); + mgmt_device_found(hdev, &info->bdaddr, ACL_LINK, 0x00, + info->dev_class, info->rssi, + !name_known, ssp, NULL, 0); + } + } else { + struct inquiry_info_with_rssi *info = (void *) (skb->data + 1); + + for (; num_rsp; num_rsp--, info++) { + bacpy(&data.bdaddr, &info->bdaddr); + data.pscan_rep_mode = info->pscan_rep_mode; + data.pscan_period_mode = info->pscan_period_mode; + data.pscan_mode = 0x00; + memcpy(data.dev_class, info->dev_class, 3); + data.clock_offset = info->clock_offset; + data.rssi = info->rssi; + data.ssp_mode = 0x00; + name_known = hci_inquiry_cache_update(hdev, &data, + false, &ssp); + mgmt_device_found(hdev, &info->bdaddr, ACL_LINK, 0x00, + info->dev_class, info->rssi, + !name_known, ssp, NULL, 0); + } + } + + hci_dev_unlock(hdev); +} + +static inline void hci_remote_ext_features_evt(struct hci_dev *hdev, struct sk_buff *skb) +{ + struct hci_ev_remote_ext_features *ev = (void *) skb->data; + struct hci_conn *conn; + + BT_DBG("%s", hdev->name); + + hci_dev_lock(hdev); + + conn = hci_conn_hash_lookup_handle(hdev, __le16_to_cpu(ev->handle)); + if (!conn) + goto unlock; + + if (!ev->status && ev->page == 0x01) { + struct inquiry_entry *ie; + + ie = hci_inquiry_cache_lookup(hdev, &conn->dst); + if (ie) + ie->data.ssp_mode = (ev->features[0] & LMP_HOST_SSP); + + if (ev->features[0] & LMP_HOST_SSP) + set_bit(HCI_CONN_SSP_ENABLED, &conn->flags); + } + + if (conn->state != BT_CONFIG) + goto unlock; + + if (!ev->status && !test_bit(HCI_CONN_MGMT_CONNECTED, &conn->flags)) { + struct hci_cp_remote_name_req cp; + memset(&cp, 0, sizeof(cp)); + bacpy(&cp.bdaddr, &conn->dst); + cp.pscan_rep_mode = 0x02; + hci_send_cmd(hdev, HCI_OP_REMOTE_NAME_REQ, sizeof(cp), &cp); + } else if (!test_and_set_bit(HCI_CONN_MGMT_CONNECTED, &conn->flags)) + mgmt_device_connected(hdev, &conn->dst, conn->type, + conn->dst_type, 0, NULL, 0, + conn->dev_class); + + if (!hci_outgoing_auth_needed(hdev, conn)) { + conn->state = BT_CONNECTED; + hci_proto_connect_cfm(conn, ev->status); + hci_conn_put(conn); + } + +unlock: + hci_dev_unlock(hdev); +} + +static inline void hci_sync_conn_complete_evt(struct hci_dev *hdev, struct sk_buff *skb) +{ + struct hci_ev_sync_conn_complete *ev = (void *) skb->data; + struct hci_conn *conn; + + BT_DBG("%s status %d", hdev->name, ev->status); + + hci_dev_lock(hdev); + + conn = hci_conn_hash_lookup_ba(hdev, ev->link_type, &ev->bdaddr); + if (!conn) { + if (ev->link_type == ESCO_LINK) + goto unlock; + + conn = hci_conn_hash_lookup_ba(hdev, ESCO_LINK, &ev->bdaddr); + if (!conn) + goto unlock; + + conn->type = SCO_LINK; + } + + switch (ev->status) { + case 0x00: + conn->handle = __le16_to_cpu(ev->handle); + conn->state = BT_CONNECTED; + + hci_conn_hold_device(conn); + hci_conn_add_sysfs(conn); + break; + + case 0x11: /* Unsupported Feature or Parameter Value */ + case 0x1c: /* SCO interval rejected */ + case 0x1a: /* Unsupported Remote Feature */ + case 0x1f: /* Unspecified error */ + if (conn->out && conn->attempt < 2) { + conn->pkt_type = (hdev->esco_type & SCO_ESCO_MASK) | + (hdev->esco_type & EDR_ESCO_MASK); + hci_setup_sync(conn, conn->link->handle); + goto unlock; + } + /* fall through */ + + default: + conn->state = BT_CLOSED; + break; + } + + hci_proto_connect_cfm(conn, ev->status); + if (ev->status) + hci_conn_del(conn); + +unlock: + hci_dev_unlock(hdev); +} + +static inline void hci_sync_conn_changed_evt(struct hci_dev *hdev, struct sk_buff *skb) +{ + BT_DBG("%s", hdev->name); +} + +static inline void hci_sniff_subrate_evt(struct hci_dev *hdev, struct sk_buff *skb) +{ + struct hci_ev_sniff_subrate *ev = (void *) skb->data; + + BT_DBG("%s status %d", hdev->name, ev->status); +} + +static inline void hci_extended_inquiry_result_evt(struct hci_dev *hdev, struct sk_buff *skb) +{ + struct inquiry_data data; + struct extended_inquiry_info *info = (void *) (skb->data + 1); + int num_rsp = *((__u8 *) skb->data); + size_t eir_len; + + BT_DBG("%s num_rsp %d", hdev->name, num_rsp); + + if (!num_rsp) + return; + + hci_dev_lock(hdev); + + for (; num_rsp; num_rsp--, info++) { + bool name_known, ssp; + + bacpy(&data.bdaddr, &info->bdaddr); + data.pscan_rep_mode = info->pscan_rep_mode; + data.pscan_period_mode = info->pscan_period_mode; + data.pscan_mode = 0x00; + memcpy(data.dev_class, info->dev_class, 3); + data.clock_offset = info->clock_offset; + data.rssi = info->rssi; + data.ssp_mode = 0x01; + + if (test_bit(HCI_MGMT, &hdev->dev_flags)) + name_known = eir_has_data_type(info->data, + sizeof(info->data), + EIR_NAME_COMPLETE); + else + name_known = true; + + name_known = hci_inquiry_cache_update(hdev, &data, name_known, + &ssp); + eir_len = eir_get_length(info->data, sizeof(info->data)); + mgmt_device_found(hdev, &info->bdaddr, ACL_LINK, 0x00, + info->dev_class, info->rssi, !name_known, + ssp, info->data, eir_len); + } + + hci_dev_unlock(hdev); +} + +static inline u8 hci_get_auth_req(struct hci_conn *conn) +{ + /* If remote requests dedicated bonding follow that lead */ + if (conn->remote_auth == 0x02 || conn->remote_auth == 0x03) { + /* If both remote and local IO capabilities allow MITM + * protection then require it, otherwise don't */ + if (conn->remote_cap == 0x03 || conn->io_capability == 0x03) + return 0x02; + else + return 0x03; + } + + /* If remote requests no-bonding follow that lead */ + if (conn->remote_auth == 0x00 || conn->remote_auth == 0x01) + return conn->remote_auth | (conn->auth_type & 0x01); + + return conn->auth_type; +} + +static inline void hci_io_capa_request_evt(struct hci_dev *hdev, struct sk_buff *skb) +{ + struct hci_ev_io_capa_request *ev = (void *) skb->data; + struct hci_conn *conn; + + BT_DBG("%s", hdev->name); + + hci_dev_lock(hdev); + + conn = hci_conn_hash_lookup_ba(hdev, ACL_LINK, &ev->bdaddr); + if (!conn) + goto unlock; + + hci_conn_hold(conn); + + if (!test_bit(HCI_MGMT, &hdev->dev_flags)) + goto unlock; + + if (test_bit(HCI_PAIRABLE, &hdev->dev_flags) || + (conn->remote_auth & ~0x01) == HCI_AT_NO_BONDING) { + struct hci_cp_io_capability_reply cp; + + bacpy(&cp.bdaddr, &ev->bdaddr); + /* Change the IO capability from KeyboardDisplay + * to DisplayYesNo as it is not supported by BT spec. */ + cp.capability = (conn->io_capability == 0x04) ? + 0x01 : conn->io_capability; + conn->auth_type = hci_get_auth_req(conn); + cp.authentication = conn->auth_type; + + if ((conn->out || test_bit(HCI_CONN_REMOTE_OOB, &conn->flags)) && + hci_find_remote_oob_data(hdev, &conn->dst)) + cp.oob_data = 0x01; + else + cp.oob_data = 0x00; + + hci_send_cmd(hdev, HCI_OP_IO_CAPABILITY_REPLY, + sizeof(cp), &cp); + } else { + struct hci_cp_io_capability_neg_reply cp; + + bacpy(&cp.bdaddr, &ev->bdaddr); + cp.reason = HCI_ERROR_PAIRING_NOT_ALLOWED; + + hci_send_cmd(hdev, HCI_OP_IO_CAPABILITY_NEG_REPLY, + sizeof(cp), &cp); + } + +unlock: + hci_dev_unlock(hdev); +} + +static inline void hci_io_capa_reply_evt(struct hci_dev *hdev, struct sk_buff *skb) +{ + struct hci_ev_io_capa_reply *ev = (void *) skb->data; + struct hci_conn *conn; + + BT_DBG("%s", hdev->name); + + hci_dev_lock(hdev); + + conn = hci_conn_hash_lookup_ba(hdev, ACL_LINK, &ev->bdaddr); + if (!conn) + goto unlock; + + conn->remote_cap = ev->capability; + conn->remote_auth = ev->authentication; + if (ev->oob_data) + set_bit(HCI_CONN_REMOTE_OOB, &conn->flags); + +unlock: + hci_dev_unlock(hdev); +} + +static inline void hci_user_confirm_request_evt(struct hci_dev *hdev, + struct sk_buff *skb) +{ + struct hci_ev_user_confirm_req *ev = (void *) skb->data; + int loc_mitm, rem_mitm, confirm_hint = 0; + struct hci_conn *conn; + + BT_DBG("%s", hdev->name); + + hci_dev_lock(hdev); + + if (!test_bit(HCI_MGMT, &hdev->dev_flags)) + goto unlock; + + conn = hci_conn_hash_lookup_ba(hdev, ACL_LINK, &ev->bdaddr); + if (!conn) + goto unlock; + + loc_mitm = (conn->auth_type & 0x01); + rem_mitm = (conn->remote_auth & 0x01); + + /* If we require MITM but the remote device can't provide that + * (it has NoInputNoOutput) then reject the confirmation + * request. The only exception is when we're dedicated bonding + * initiators (connect_cfm_cb set) since then we always have the MITM + * bit set. */ + if (!conn->connect_cfm_cb && loc_mitm && conn->remote_cap == 0x03) { + BT_DBG("Rejecting request: remote device can't provide MITM"); + hci_send_cmd(hdev, HCI_OP_USER_CONFIRM_NEG_REPLY, + sizeof(ev->bdaddr), &ev->bdaddr); + goto unlock; + } + + /* If no side requires MITM protection; auto-accept */ + if ((!loc_mitm || conn->remote_cap == 0x03) && + (!rem_mitm || conn->io_capability == 0x03)) { + + /* If we're not the initiators request authorization to + * proceed from user space (mgmt_user_confirm with + * confirm_hint set to 1). */ + if (!test_bit(HCI_CONN_AUTH_PEND, &conn->flags)) { + BT_DBG("Confirming auto-accept as acceptor"); + confirm_hint = 1; + goto confirm; + } + + BT_DBG("Auto-accept of user confirmation with %ums delay", + hdev->auto_accept_delay); + + if (hdev->auto_accept_delay > 0) { + int delay = msecs_to_jiffies(hdev->auto_accept_delay); + mod_timer(&conn->auto_accept_timer, jiffies + delay); + goto unlock; + } + + hci_send_cmd(hdev, HCI_OP_USER_CONFIRM_REPLY, + sizeof(ev->bdaddr), &ev->bdaddr); + goto unlock; + } + +confirm: + mgmt_user_confirm_request(hdev, &ev->bdaddr, ACL_LINK, 0, ev->passkey, + confirm_hint); + +unlock: + hci_dev_unlock(hdev); +} + +static inline void hci_user_passkey_request_evt(struct hci_dev *hdev, + struct sk_buff *skb) +{ + struct hci_ev_user_passkey_req *ev = (void *) skb->data; + + BT_DBG("%s", hdev->name); + + hci_dev_lock(hdev); + + if (test_bit(HCI_MGMT, &hdev->dev_flags)) + mgmt_user_passkey_request(hdev, &ev->bdaddr, ACL_LINK, 0); + + hci_dev_unlock(hdev); +} + +static inline void hci_simple_pair_complete_evt(struct hci_dev *hdev, struct sk_buff *skb) +{ + struct hci_ev_simple_pair_complete *ev = (void *) skb->data; + struct hci_conn *conn; + + BT_DBG("%s", hdev->name); + + hci_dev_lock(hdev); + + conn = hci_conn_hash_lookup_ba(hdev, ACL_LINK, &ev->bdaddr); + if (!conn) + goto unlock; + + /* To avoid duplicate auth_failed events to user space we check + * the HCI_CONN_AUTH_PEND flag which will be set if we + * initiated the authentication. A traditional auth_complete + * event gets always produced as initiator and is also mapped to + * the mgmt_auth_failed event */ + if (!test_bit(HCI_CONN_AUTH_PEND, &conn->flags) && ev->status != 0) + mgmt_auth_failed(hdev, &conn->dst, conn->type, conn->dst_type, + ev->status); + + hci_conn_put(conn); + +unlock: + hci_dev_unlock(hdev); +} + +static inline void hci_remote_host_features_evt(struct hci_dev *hdev, struct sk_buff *skb) +{ + struct hci_ev_remote_host_features *ev = (void *) skb->data; + struct inquiry_entry *ie; + + BT_DBG("%s", hdev->name); + + hci_dev_lock(hdev); + + ie = hci_inquiry_cache_lookup(hdev, &ev->bdaddr); + if (ie) + ie->data.ssp_mode = (ev->features[0] & LMP_HOST_SSP); + + hci_dev_unlock(hdev); +} + +static inline void hci_remote_oob_data_request_evt(struct hci_dev *hdev, + struct sk_buff *skb) +{ + struct hci_ev_remote_oob_data_request *ev = (void *) skb->data; + struct oob_data *data; + + BT_DBG("%s", hdev->name); + + hci_dev_lock(hdev); + + if (!test_bit(HCI_MGMT, &hdev->dev_flags)) + goto unlock; + + data = hci_find_remote_oob_data(hdev, &ev->bdaddr); + if (data) { + struct hci_cp_remote_oob_data_reply cp; + + bacpy(&cp.bdaddr, &ev->bdaddr); + memcpy(cp.hash, data->hash, sizeof(cp.hash)); + memcpy(cp.randomizer, data->randomizer, sizeof(cp.randomizer)); + + hci_send_cmd(hdev, HCI_OP_REMOTE_OOB_DATA_REPLY, sizeof(cp), + &cp); + } else { + struct hci_cp_remote_oob_data_neg_reply cp; + + bacpy(&cp.bdaddr, &ev->bdaddr); + hci_send_cmd(hdev, HCI_OP_REMOTE_OOB_DATA_NEG_REPLY, sizeof(cp), + &cp); + } + +unlock: + hci_dev_unlock(hdev); +} + +static inline void hci_le_conn_complete_evt(struct hci_dev *hdev, struct sk_buff *skb) +{ + struct hci_ev_le_conn_complete *ev = (void *) skb->data; + struct hci_conn *conn; + + BT_DBG("%s status %d", hdev->name, ev->status); + + hci_dev_lock(hdev); + + conn = hci_conn_hash_lookup_ba(hdev, LE_LINK, &ev->bdaddr); + if (!conn) { + conn = hci_conn_add(hdev, LE_LINK, &ev->bdaddr); + if (!conn) { + BT_ERR("No memory for new connection"); + hci_dev_unlock(hdev); + return; + } + + conn->dst_type = ev->bdaddr_type; + } + + if (ev->status) { + mgmt_connect_failed(hdev, &ev->bdaddr, conn->type, + conn->dst_type, ev->status); + hci_proto_connect_cfm(conn, ev->status); + conn->state = BT_CLOSED; + hci_conn_del(conn); + goto unlock; + } + + if (!test_and_set_bit(HCI_CONN_MGMT_CONNECTED, &conn->flags)) + mgmt_device_connected(hdev, &ev->bdaddr, conn->type, + conn->dst_type, 0, NULL, 0, NULL); + + conn->sec_level = BT_SECURITY_LOW; + conn->handle = __le16_to_cpu(ev->handle); + conn->state = BT_CONNECTED; + + hci_conn_hold_device(conn); + hci_conn_add_sysfs(conn); + + hci_proto_connect_cfm(conn, ev->status); + +unlock: + hci_dev_unlock(hdev); +} + +static inline void hci_le_adv_report_evt(struct hci_dev *hdev, + struct sk_buff *skb) +{ + u8 num_reports = skb->data[0]; + void *ptr = &skb->data[1]; + s8 rssi; + + hci_dev_lock(hdev); + + while (num_reports--) { + struct hci_ev_le_advertising_info *ev = ptr; + + hci_add_adv_entry(hdev, ev); + + rssi = ev->data[ev->length]; + mgmt_device_found(hdev, &ev->bdaddr, LE_LINK, ev->bdaddr_type, + NULL, rssi, 0, 1, ev->data, ev->length); + + ptr += sizeof(*ev) + ev->length + 1; + } + + hci_dev_unlock(hdev); +} + +static inline void hci_le_ltk_request_evt(struct hci_dev *hdev, + struct sk_buff *skb) +{ + struct hci_ev_le_ltk_req *ev = (void *) skb->data; + struct hci_cp_le_ltk_reply cp; + struct hci_cp_le_ltk_neg_reply neg; + struct hci_conn *conn; + struct smp_ltk *ltk; + + BT_DBG("%s handle %d", hdev->name, cpu_to_le16(ev->handle)); + + hci_dev_lock(hdev); + + conn = hci_conn_hash_lookup_handle(hdev, __le16_to_cpu(ev->handle)); + if (conn == NULL) + goto not_found; + + ltk = hci_find_ltk(hdev, ev->ediv, ev->random); + if (ltk == NULL) + goto not_found; + + memcpy(cp.ltk, ltk->val, sizeof(ltk->val)); + cp.handle = cpu_to_le16(conn->handle); + + if (ltk->authenticated) + conn->sec_level = BT_SECURITY_HIGH; + + hci_send_cmd(hdev, HCI_OP_LE_LTK_REPLY, sizeof(cp), &cp); + + if (ltk->type & HCI_SMP_STK) { + list_del(<k->list); + kfree(ltk); + } + + hci_dev_unlock(hdev); + + return; + +not_found: + neg.handle = ev->handle; + hci_send_cmd(hdev, HCI_OP_LE_LTK_NEG_REPLY, sizeof(neg), &neg); + hci_dev_unlock(hdev); +} + +static inline void hci_le_meta_evt(struct hci_dev *hdev, struct sk_buff *skb) +{ + struct hci_ev_le_meta *le_ev = (void *) skb->data; + + skb_pull(skb, sizeof(*le_ev)); + + switch (le_ev->subevent) { + case HCI_EV_LE_CONN_COMPLETE: + hci_le_conn_complete_evt(hdev, skb); + break; + + case HCI_EV_LE_ADVERTISING_REPORT: + hci_le_adv_report_evt(hdev, skb); + break; + + case HCI_EV_LE_LTK_REQ: + hci_le_ltk_request_evt(hdev, skb); + break; + + default: + break; + } +} + +void hci_event_packet(struct hci_dev *hdev, struct sk_buff *skb) +{ + struct hci_event_hdr *hdr = (void *) skb->data; + __u8 event = hdr->evt; + + skb_pull(skb, HCI_EVENT_HDR_SIZE); + + switch (event) { + case HCI_EV_INQUIRY_COMPLETE: + hci_inquiry_complete_evt(hdev, skb); + break; + + case HCI_EV_INQUIRY_RESULT: + hci_inquiry_result_evt(hdev, skb); + break; + + case HCI_EV_CONN_COMPLETE: + hci_conn_complete_evt(hdev, skb); + break; + + case HCI_EV_CONN_REQUEST: + hci_conn_request_evt(hdev, skb); + break; + + case HCI_EV_DISCONN_COMPLETE: + hci_disconn_complete_evt(hdev, skb); + break; + + case HCI_EV_AUTH_COMPLETE: + hci_auth_complete_evt(hdev, skb); + break; + + case HCI_EV_REMOTE_NAME: + hci_remote_name_evt(hdev, skb); + break; + + case HCI_EV_ENCRYPT_CHANGE: + hci_encrypt_change_evt(hdev, skb); + break; + + case HCI_EV_CHANGE_LINK_KEY_COMPLETE: + hci_change_link_key_complete_evt(hdev, skb); + break; + + case HCI_EV_REMOTE_FEATURES: + hci_remote_features_evt(hdev, skb); + break; + + case HCI_EV_REMOTE_VERSION: + hci_remote_version_evt(hdev, skb); + break; + + case HCI_EV_QOS_SETUP_COMPLETE: + hci_qos_setup_complete_evt(hdev, skb); + break; + + case HCI_EV_CMD_COMPLETE: + hci_cmd_complete_evt(hdev, skb); + break; + + case HCI_EV_CMD_STATUS: + hci_cmd_status_evt(hdev, skb); + break; + + case HCI_EV_ROLE_CHANGE: + hci_role_change_evt(hdev, skb); + break; + + case HCI_EV_NUM_COMP_PKTS: + hci_num_comp_pkts_evt(hdev, skb); + break; + + case HCI_EV_MODE_CHANGE: + hci_mode_change_evt(hdev, skb); + break; + + case HCI_EV_PIN_CODE_REQ: + hci_pin_code_request_evt(hdev, skb); + break; + + case HCI_EV_LINK_KEY_REQ: + hci_link_key_request_evt(hdev, skb); + break; + + case HCI_EV_LINK_KEY_NOTIFY: + hci_link_key_notify_evt(hdev, skb); + break; + + case HCI_EV_CLOCK_OFFSET: + hci_clock_offset_evt(hdev, skb); + break; + + case HCI_EV_PKT_TYPE_CHANGE: + hci_pkt_type_change_evt(hdev, skb); + break; + + case HCI_EV_PSCAN_REP_MODE: + hci_pscan_rep_mode_evt(hdev, skb); + break; + + case HCI_EV_INQUIRY_RESULT_WITH_RSSI: + hci_inquiry_result_with_rssi_evt(hdev, skb); + break; + + case HCI_EV_REMOTE_EXT_FEATURES: + hci_remote_ext_features_evt(hdev, skb); + break; + + case HCI_EV_SYNC_CONN_COMPLETE: + hci_sync_conn_complete_evt(hdev, skb); + break; + + case HCI_EV_SYNC_CONN_CHANGED: + hci_sync_conn_changed_evt(hdev, skb); + break; + + case HCI_EV_SNIFF_SUBRATE: + hci_sniff_subrate_evt(hdev, skb); + break; + + case HCI_EV_EXTENDED_INQUIRY_RESULT: + hci_extended_inquiry_result_evt(hdev, skb); + break; + + case HCI_EV_IO_CAPA_REQUEST: + hci_io_capa_request_evt(hdev, skb); + break; + + case HCI_EV_IO_CAPA_REPLY: + hci_io_capa_reply_evt(hdev, skb); + break; + + case HCI_EV_USER_CONFIRM_REQUEST: + hci_user_confirm_request_evt(hdev, skb); + break; + + case HCI_EV_USER_PASSKEY_REQUEST: + hci_user_passkey_request_evt(hdev, skb); + break; + + case HCI_EV_SIMPLE_PAIR_COMPLETE: + hci_simple_pair_complete_evt(hdev, skb); + break; + + case HCI_EV_REMOTE_HOST_FEATURES: + hci_remote_host_features_evt(hdev, skb); + break; + + case HCI_EV_LE_META: + hci_le_meta_evt(hdev, skb); + break; + + case HCI_EV_REMOTE_OOB_DATA_REQUEST: + hci_remote_oob_data_request_evt(hdev, skb); + break; + + case HCI_EV_NUM_COMP_BLOCKS: + hci_num_comp_blocks_evt(hdev, skb); + break; + + default: + BT_DBG("%s event 0x%x", hdev->name, event); + break; + } + + kfree_skb(skb); + hdev->stat.evt_rx++; +} + +/* Generate internal stack event */ +void hci_si_event(struct hci_dev *hdev, int type, int dlen, void *data) +{ + struct hci_event_hdr *hdr; + struct hci_ev_stack_internal *ev; + struct sk_buff *skb; + + skb = bt_skb_alloc(HCI_EVENT_HDR_SIZE + sizeof(*ev) + dlen, GFP_ATOMIC); + if (!skb) + return; + + hdr = (void *) skb_put(skb, HCI_EVENT_HDR_SIZE); + hdr->evt = HCI_EV_STACK_INTERNAL; + hdr->plen = sizeof(*ev) + dlen; + + ev = (void *) skb_put(skb, sizeof(*ev) + dlen); + ev->type = type; + memcpy(ev->data, data, dlen); + + bt_cb(skb)->incoming = 1; + __net_timestamp(skb); + + bt_cb(skb)->pkt_type = HCI_EVENT_PKT; + skb->dev = (void *) hdev; + hci_send_to_sock(hdev, skb); + kfree_skb(skb); +} diff --git a/net/bluetooth_tizen/hci_sock.c b/net/bluetooth_tizen/hci_sock.c new file mode 100644 index 0000000..63c1516 --- /dev/null +++ b/net/bluetooth_tizen/hci_sock.c @@ -0,0 +1,1102 @@ +/* + BlueZ - Bluetooth protocol stack for Linux + Copyright (C) 2000-2001 Qualcomm Incorporated + + Written 2000,2001 by Maxim Krasnyansky <maxk@qualcomm.com> + + This program is free software; you can redistribute it and/or modify + it under the terms of the GNU General Public License version 2 as + published by the Free Software Foundation; + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS + OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT OF THIRD PARTY RIGHTS. + IN NO EVENT SHALL THE COPYRIGHT HOLDER(S) AND AUTHOR(S) BE LIABLE FOR ANY + CLAIM, OR ANY SPECIAL INDIRECT OR CONSEQUENTIAL DAMAGES, OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + + ALL LIABILITY, INCLUDING LIABILITY FOR INFRINGEMENT OF ANY PATENTS, + COPYRIGHTS, TRADEMARKS OR OTHER RIGHTS, RELATING TO USE OF THIS + SOFTWARE IS DISCLAIMED. +*/ + +/* Bluetooth HCI sockets. */ + +#include <linux/module.h> + +#include <linux/types.h> +#include <linux/capability.h> +#include <linux/errno.h> +#include <linux/kernel.h> +#include <linux/slab.h> +#include <linux/poll.h> +#include <linux/fcntl.h> +#include <linux/init.h> +#include <linux/skbuff.h> +#include <linux/workqueue.h> +#include <linux/interrupt.h> +#include <linux/compat.h> +#include <linux/socket.h> +#include <linux/ioctl.h> +#include <net/sock.h> + +#include <asm/system.h> +#include <linux/uaccess.h> +#include <asm/unaligned.h> + +#include <net/bluetooth/bluetooth.h> +#include <net/bluetooth/hci_core.h> +#include <net/bluetooth/hci_mon.h> + +static atomic_t monitor_promisc = ATOMIC_INIT(0); + +/* ----- HCI socket interface ----- */ + +static inline int hci_test_bit(int nr, void *addr) +{ + return *((__u32 *) addr + (nr >> 5)) & ((__u32) 1 << (nr & 31)); +} + +/* Security filter */ +static struct hci_sec_filter hci_sec_filter = { + /* Packet types */ + 0x10, + /* Events */ + { 0x1000d9fe, 0x0000b00c }, + /* Commands */ + { + { 0x0 }, + /* OGF_LINK_CTL */ + { 0xbe000006, 0x00000001, 0x00000000, 0x00 }, + /* OGF_LINK_POLICY */ + { 0x00005200, 0x00000000, 0x00000000, 0x00 }, + /* OGF_HOST_CTL */ + { 0xaab00200, 0x2b402aaa, 0x05220154, 0x00 }, + /* OGF_INFO_PARAM */ + { 0x000002be, 0x00000000, 0x00000000, 0x00 }, + /* OGF_STATUS_PARAM */ + { 0x000000ea, 0x00000000, 0x00000000, 0x00 } + } +}; + +static struct bt_sock_list hci_sk_list = { + .lock = __RW_LOCK_UNLOCKED(hci_sk_list.lock) +}; + +/* Send frame to RAW socket */ +void hci_send_to_sock(struct hci_dev *hdev, struct sk_buff *skb) +{ + struct sock *sk; + struct hlist_node *node; + struct sk_buff *skb_copy = NULL; + + BT_DBG("hdev %p len %d", hdev, skb->len); + + read_lock(&hci_sk_list.lock); + + sk_for_each(sk, node, &hci_sk_list.head) { + struct hci_filter *flt; + struct sk_buff *nskb; + + if (sk->sk_state != BT_BOUND || hci_pi(sk)->hdev != hdev) + continue; + + /* Don't send frame to the socket it came from */ + if (skb->sk == sk) + continue; + + if (hci_pi(sk)->channel != HCI_CHANNEL_RAW) + continue; + + /* Apply filter */ + flt = &hci_pi(sk)->filter; + + if (!test_bit((bt_cb(skb)->pkt_type == HCI_VENDOR_PKT) ? + 0 : (bt_cb(skb)->pkt_type & HCI_FLT_TYPE_BITS), &flt->type_mask)) + continue; + + if (bt_cb(skb)->pkt_type == HCI_EVENT_PKT) { + register int evt = (*(__u8 *)skb->data & HCI_FLT_EVENT_BITS); + + if (!hci_test_bit(evt, &flt->event_mask)) + continue; + + if (flt->opcode && + ((evt == HCI_EV_CMD_COMPLETE && + flt->opcode != + get_unaligned((__le16 *)(skb->data + 3))) || + (evt == HCI_EV_CMD_STATUS && + flt->opcode != + get_unaligned((__le16 *)(skb->data + 4))))) + continue; + } + + if (!skb_copy) { + /* Create a private copy with headroom */ + skb_copy = __pskb_copy(skb, 1, GFP_ATOMIC); + if (!skb_copy) + continue; + + /* Put type byte before the data */ + memcpy(skb_push(skb_copy, 1), &bt_cb(skb)->pkt_type, 1); + } + + nskb = skb_clone(skb_copy, GFP_ATOMIC); + if (!nskb) + continue; + + if (sock_queue_rcv_skb(sk, nskb)) + kfree_skb(nskb); + } + + read_unlock(&hci_sk_list.lock); + + kfree_skb(skb_copy); +} + +/* Send frame to control socket */ +void hci_send_to_control(struct sk_buff *skb, struct sock *skip_sk) +{ + struct sock *sk; + struct hlist_node *node; + + BT_DBG("len %d", skb->len); + + read_lock(&hci_sk_list.lock); + + sk_for_each(sk, node, &hci_sk_list.head) { + struct sk_buff *nskb; + + /* Skip the original socket */ + if (sk == skip_sk) + continue; + + if (sk->sk_state != BT_BOUND) + continue; + + if (hci_pi(sk)->channel != HCI_CHANNEL_CONTROL) + continue; + + nskb = skb_clone(skb, GFP_ATOMIC); + if (!nskb) + continue; + + if (sock_queue_rcv_skb(sk, nskb)) + kfree_skb(nskb); + } + + read_unlock(&hci_sk_list.lock); +} + +/* Send frame to monitor socket */ +void hci_send_to_monitor(struct hci_dev *hdev, struct sk_buff *skb) +{ + struct sock *sk; + struct hlist_node *node; + struct sk_buff *skb_copy = NULL; + __le16 opcode; + + if (!atomic_read(&monitor_promisc)) + return; + + BT_DBG("hdev %p len %d", hdev, skb->len); + + switch (bt_cb(skb)->pkt_type) { + case HCI_COMMAND_PKT: + opcode = __constant_cpu_to_le16(HCI_MON_COMMAND_PKT); + break; + case HCI_EVENT_PKT: + opcode = __constant_cpu_to_le16(HCI_MON_EVENT_PKT); + break; + case HCI_ACLDATA_PKT: + if (bt_cb(skb)->incoming) + opcode = __constant_cpu_to_le16(HCI_MON_ACL_RX_PKT); + else + opcode = __constant_cpu_to_le16(HCI_MON_ACL_TX_PKT); + break; + case HCI_SCODATA_PKT: + if (bt_cb(skb)->incoming) + opcode = __constant_cpu_to_le16(HCI_MON_SCO_RX_PKT); + else + opcode = __constant_cpu_to_le16(HCI_MON_SCO_TX_PKT); + break; + default: + return; + } + + read_lock(&hci_sk_list.lock); + + sk_for_each(sk, node, &hci_sk_list.head) { + struct sk_buff *nskb; + + if (sk->sk_state != BT_BOUND) + continue; + + if (hci_pi(sk)->channel != HCI_CHANNEL_MONITOR) + continue; + + if (!skb_copy) { + struct hci_mon_hdr *hdr; + + /* Create a private copy with headroom */ + skb_copy = __pskb_copy(skb, HCI_MON_HDR_SIZE, GFP_ATOMIC); + if (!skb_copy) + continue; + + /* Put header before the data */ + hdr = (void *) skb_push(skb_copy, HCI_MON_HDR_SIZE); + hdr->opcode = opcode; + hdr->index = cpu_to_le16(hdev->id); + hdr->len = cpu_to_le16(skb->len); + } + + nskb = skb_clone(skb_copy, GFP_ATOMIC); + if (!nskb) + continue; + + if (sock_queue_rcv_skb(sk, nskb)) + kfree_skb(nskb); + } + + read_unlock(&hci_sk_list.lock); + + kfree_skb(skb_copy); +} + +static void send_monitor_event(struct sk_buff *skb) +{ + struct sock *sk; + struct hlist_node *node; + + BT_DBG("len %d", skb->len); + + read_lock(&hci_sk_list.lock); + + sk_for_each(sk, node, &hci_sk_list.head) { + struct sk_buff *nskb; + + if (sk->sk_state != BT_BOUND) + continue; + + if (hci_pi(sk)->channel != HCI_CHANNEL_MONITOR) + continue; + + nskb = skb_clone(skb, GFP_ATOMIC); + if (!nskb) + continue; + + if (sock_queue_rcv_skb(sk, nskb)) + kfree_skb(nskb); + } + + read_unlock(&hci_sk_list.lock); +} + +static struct sk_buff *create_monitor_event(struct hci_dev *hdev, int event) +{ + struct hci_mon_hdr *hdr; + struct hci_mon_new_index *ni; + struct sk_buff *skb; + __le16 opcode; + + switch (event) { + case HCI_DEV_REG: + skb = bt_skb_alloc(HCI_MON_NEW_INDEX_SIZE, GFP_ATOMIC); + if (!skb) + return NULL; + + ni = (void *) skb_put(skb, HCI_MON_NEW_INDEX_SIZE); + ni->type = hdev->dev_type; + ni->bus = hdev->bus; + bacpy(&ni->bdaddr, &hdev->bdaddr); + memcpy(ni->name, hdev->name, 8); + + opcode = __constant_cpu_to_le16(HCI_MON_NEW_INDEX); + break; + + case HCI_DEV_UNREG: + skb = bt_skb_alloc(0, GFP_ATOMIC); + if (!skb) + return NULL; + + opcode = __constant_cpu_to_le16(HCI_MON_DEL_INDEX); + break; + + default: + return NULL; + } + + __net_timestamp(skb); + + hdr = (void *) skb_push(skb, HCI_MON_HDR_SIZE); + hdr->opcode = opcode; + hdr->index = cpu_to_le16(hdev->id); + hdr->len = cpu_to_le16(skb->len - HCI_MON_HDR_SIZE); + + return skb; +} + +static void send_monitor_replay(struct sock *sk) +{ + struct hci_dev *hdev; + + read_lock(&hci_dev_list_lock); + + list_for_each_entry(hdev, &hci_dev_list, list) { + struct sk_buff *skb; + + skb = create_monitor_event(hdev, HCI_DEV_REG); + if (!skb) + continue; + + if (sock_queue_rcv_skb(sk, skb)) + kfree_skb(skb); + } + + read_unlock(&hci_dev_list_lock); +} + +static int hci_sock_release(struct socket *sock) +{ + struct sock *sk = sock->sk; + struct hci_dev *hdev; + + BT_DBG("sock %p sk %p", sock, sk); + + if (!sk) + return 0; + + hdev = hci_pi(sk)->hdev; + + if (hci_pi(sk)->channel == HCI_CHANNEL_MONITOR) + atomic_dec(&monitor_promisc); + + bt_sock_unlink(&hci_sk_list, sk); + + if (hdev) { + atomic_dec(&hdev->promisc); + hci_dev_put(hdev); + } + + sock_orphan(sk); + + skb_queue_purge(&sk->sk_receive_queue); + skb_queue_purge(&sk->sk_write_queue); + + sock_put(sk); + return 0; +} + +static int hci_sock_blacklist_add(struct hci_dev *hdev, void __user *arg) +{ + bdaddr_t bdaddr; + int err; + + if (copy_from_user(&bdaddr, arg, sizeof(bdaddr))) + return -EFAULT; + + hci_dev_lock(hdev); + + err = hci_blacklist_add(hdev, &bdaddr, 0); + + hci_dev_unlock(hdev); + + return err; +} + +static int hci_sock_blacklist_del(struct hci_dev *hdev, void __user *arg) +{ + bdaddr_t bdaddr; + int err; + + if (copy_from_user(&bdaddr, arg, sizeof(bdaddr))) + return -EFAULT; + + hci_dev_lock(hdev); + + err = hci_blacklist_del(hdev, &bdaddr, 0); + + hci_dev_unlock(hdev); + + return err; +} + +/* Ioctls that require bound socket */ +static inline int hci_sock_bound_ioctl(struct sock *sk, unsigned int cmd, unsigned long arg) +{ + struct hci_dev *hdev = hci_pi(sk)->hdev; + + if (!hdev) + return -EBADFD; + + switch (cmd) { + case HCISETRAW: + if (!capable(CAP_NET_ADMIN)) + return -EACCES; + + if (test_bit(HCI_QUIRK_RAW_DEVICE, &hdev->quirks)) + return -EPERM; + + if (arg) + set_bit(HCI_RAW, &hdev->flags); + else + clear_bit(HCI_RAW, &hdev->flags); + + return 0; + + case HCIGETCONNINFO: + return hci_get_conn_info(hdev, (void __user *) arg); + + case HCIGETAUTHINFO: + return hci_get_auth_info(hdev, (void __user *) arg); + + case HCIBLOCKADDR: + if (!capable(CAP_NET_ADMIN)) + return -EACCES; + return hci_sock_blacklist_add(hdev, (void __user *) arg); + + case HCIUNBLOCKADDR: + if (!capable(CAP_NET_ADMIN)) + return -EACCES; + return hci_sock_blacklist_del(hdev, (void __user *) arg); + + default: + if (hdev->ioctl) + return hdev->ioctl(hdev, cmd, arg); + return -EINVAL; + } +} + +static int hci_sock_ioctl(struct socket *sock, unsigned int cmd, unsigned long arg) +{ + struct sock *sk = sock->sk; + void __user *argp = (void __user *) arg; + int err; + + BT_DBG("cmd %x arg %lx", cmd, arg); + + switch (cmd) { + case HCIGETDEVLIST: + return hci_get_dev_list(argp); + + case HCIGETDEVINFO: + return hci_get_dev_info(argp); + + case HCIGETCONNLIST: + return hci_get_conn_list(argp); + + case HCIDEVUP: + if (!capable(CAP_NET_ADMIN)) + return -EACCES; + return hci_dev_open(arg); + + case HCIDEVDOWN: + if (!capable(CAP_NET_ADMIN)) + return -EACCES; + return hci_dev_close(arg); + + case HCIDEVRESET: + if (!capable(CAP_NET_ADMIN)) + return -EACCES; + return hci_dev_reset(arg); + + case HCIDEVRESTAT: + if (!capable(CAP_NET_ADMIN)) + return -EACCES; + return hci_dev_reset_stat(arg); + + case HCISETSCAN: + case HCISETAUTH: + case HCISETENCRYPT: + case HCISETPTYPE: + case HCISETLINKPOL: + case HCISETLINKMODE: + case HCISETACLMTU: + case HCISETSCOMTU: + if (!capable(CAP_NET_ADMIN)) + return -EACCES; + return hci_dev_cmd(cmd, argp); + + case HCIINQUIRY: + return hci_inquiry(argp); + + default: + lock_sock(sk); + err = hci_sock_bound_ioctl(sk, cmd, arg); + release_sock(sk); + return err; + } +} + +static int hci_sock_bind(struct socket *sock, struct sockaddr *addr, int addr_len) +{ + struct sockaddr_hci haddr; + struct sock *sk = sock->sk; + struct hci_dev *hdev = NULL; + int len, err = 0; + + BT_DBG("sock %p sk %p", sock, sk); + + if (!addr) + return -EINVAL; + + memset(&haddr, 0, sizeof(haddr)); + len = min_t(unsigned int, sizeof(haddr), addr_len); + memcpy(&haddr, addr, len); + + if (haddr.hci_family != AF_BLUETOOTH) + return -EINVAL; + + lock_sock(sk); + + if (sk->sk_state == BT_BOUND) { + err = -EALREADY; + goto done; + } + + switch (haddr.hci_channel) { + case HCI_CHANNEL_RAW: + if (hci_pi(sk)->hdev) { + err = -EALREADY; + goto done; + } + + if (haddr.hci_dev != HCI_DEV_NONE) { + hdev = hci_dev_get(haddr.hci_dev); + if (!hdev) { + err = -ENODEV; + goto done; + } + + atomic_inc(&hdev->promisc); + } + + hci_pi(sk)->hdev = hdev; + break; + + case HCI_CHANNEL_CONTROL: + if (haddr.hci_dev != HCI_DEV_NONE) { + err = -EINVAL; + goto done; + } + + if (!capable(CAP_NET_ADMIN)) { + err = -EPERM; + goto done; + } + + break; + + case HCI_CHANNEL_MONITOR: + if (haddr.hci_dev != HCI_DEV_NONE) { + err = -EINVAL; + goto done; + } + + if (!capable(CAP_NET_RAW)) { + err = -EPERM; + goto done; + } + + send_monitor_replay(sk); + + atomic_inc(&monitor_promisc); + break; + + default: + err = -EINVAL; + goto done; + } + + + hci_pi(sk)->channel = haddr.hci_channel; + sk->sk_state = BT_BOUND; + +done: + release_sock(sk); + return err; +} + +static int hci_sock_getname(struct socket *sock, struct sockaddr *addr, int *addr_len, int peer) +{ + struct sockaddr_hci *haddr = (struct sockaddr_hci *) addr; + struct sock *sk = sock->sk; + struct hci_dev *hdev = hci_pi(sk)->hdev; + + BT_DBG("sock %p sk %p", sock, sk); + + if (!hdev) + return -EBADFD; + + lock_sock(sk); + + *addr_len = sizeof(*haddr); + haddr->hci_family = AF_BLUETOOTH; + haddr->hci_dev = hdev->id; + + release_sock(sk); + return 0; +} + +static inline void hci_sock_cmsg(struct sock *sk, struct msghdr *msg, struct sk_buff *skb) +{ + __u32 mask = hci_pi(sk)->cmsg_mask; + + if (mask & HCI_CMSG_DIR) { + int incoming = bt_cb(skb)->incoming; + put_cmsg(msg, SOL_HCI, HCI_CMSG_DIR, sizeof(incoming), &incoming); + } + + if (mask & HCI_CMSG_TSTAMP) { +#ifdef CONFIG_COMPAT + struct compat_timeval ctv; +#endif + struct timeval tv; + void *data; + int len; + + skb_get_timestamp(skb, &tv); + + data = &tv; + len = sizeof(tv); +#ifdef CONFIG_COMPAT + if (msg->msg_flags & MSG_CMSG_COMPAT) { + ctv.tv_sec = tv.tv_sec; + ctv.tv_usec = tv.tv_usec; + data = &ctv; + len = sizeof(ctv); + } +#endif + + put_cmsg(msg, SOL_HCI, HCI_CMSG_TSTAMP, len, data); + } +} + +static int hci_sock_recvmsg(struct kiocb *iocb, struct socket *sock, + struct msghdr *msg, size_t len, int flags) +{ + int noblock = flags & MSG_DONTWAIT; + struct sock *sk = sock->sk; + struct sk_buff *skb; + int copied, err; + + BT_DBG("sock %p, sk %p", sock, sk); + + if (flags & (MSG_OOB)) + return -EOPNOTSUPP; + + if (sk->sk_state == BT_CLOSED) + return 0; + + skb = skb_recv_datagram(sk, flags, noblock, &err); + if (!skb) + return err; + + msg->msg_namelen = 0; + + copied = skb->len; + if (len < copied) { + msg->msg_flags |= MSG_TRUNC; + copied = len; + } + + skb_reset_transport_header(skb); + err = skb_copy_datagram_iovec(skb, 0, msg->msg_iov, copied); + + switch (hci_pi(sk)->channel) { + case HCI_CHANNEL_RAW: + hci_sock_cmsg(sk, msg, skb); + break; + case HCI_CHANNEL_CONTROL: + case HCI_CHANNEL_MONITOR: + sock_recv_timestamp(msg, sk, skb); + break; + } + + skb_free_datagram(sk, skb); + + return err ? : copied; +} + +static int hci_sock_sendmsg(struct kiocb *iocb, struct socket *sock, + struct msghdr *msg, size_t len) +{ + struct sock *sk = sock->sk; + struct hci_dev *hdev; + struct sk_buff *skb; + int err; + + BT_DBG("sock %p sk %p", sock, sk); + + if (msg->msg_flags & MSG_OOB) + return -EOPNOTSUPP; + + if (msg->msg_flags & ~(MSG_DONTWAIT|MSG_NOSIGNAL|MSG_ERRQUEUE)) + return -EINVAL; + + if (len < 4 || len > HCI_MAX_FRAME_SIZE) + return -EINVAL; + + lock_sock(sk); + + switch (hci_pi(sk)->channel) { + case HCI_CHANNEL_RAW: + break; + case HCI_CHANNEL_CONTROL: + err = mgmt_control(sk, msg, len); + goto done; + case HCI_CHANNEL_MONITOR: + err = -EOPNOTSUPP; + goto done; + default: + err = -EINVAL; + goto done; + } + + hdev = hci_pi(sk)->hdev; + if (!hdev) { + err = -EBADFD; + goto done; + } + + if (!test_bit(HCI_UP, &hdev->flags)) { + err = -ENETDOWN; + goto done; + } + + skb = bt_skb_send_alloc(sk, len, msg->msg_flags & MSG_DONTWAIT, &err); + if (!skb) + goto done; + + if (memcpy_fromiovec(skb_put(skb, len), msg->msg_iov, len)) { + err = -EFAULT; + goto drop; + } + + bt_cb(skb)->pkt_type = *((unsigned char *) skb->data); + skb_pull(skb, 1); + skb->dev = (void *) hdev; + + if (bt_cb(skb)->pkt_type == HCI_COMMAND_PKT) { + u16 opcode = get_unaligned_le16(skb->data); + u16 ogf = hci_opcode_ogf(opcode); + u16 ocf = hci_opcode_ocf(opcode); + + if (((ogf > HCI_SFLT_MAX_OGF) || + !hci_test_bit(ocf & HCI_FLT_OCF_BITS, &hci_sec_filter.ocf_mask[ogf])) && + !capable(CAP_NET_RAW)) { + err = -EPERM; + goto drop; + } + + if (test_bit(HCI_RAW, &hdev->flags) || (ogf == 0x3f)) { + skb_queue_tail(&hdev->raw_q, skb); + queue_work(hdev->workqueue, &hdev->tx_work); + } else { + skb_queue_tail(&hdev->cmd_q, skb); + queue_work(hdev->workqueue, &hdev->cmd_work); + } + } else { + if (!capable(CAP_NET_RAW)) { + err = -EPERM; + goto drop; + } + + skb_queue_tail(&hdev->raw_q, skb); + queue_work(hdev->workqueue, &hdev->tx_work); + } + + err = len; + +done: + release_sock(sk); + return err; + +drop: + kfree_skb(skb); + goto done; +} + +static int hci_sock_setsockopt(struct socket *sock, int level, int optname, char __user *optval, unsigned int len) +{ + struct hci_ufilter uf = { .opcode = 0 }; + struct sock *sk = sock->sk; + int err = 0, opt = 0; + + BT_DBG("sk %p, opt %d", sk, optname); + + lock_sock(sk); + + if (hci_pi(sk)->channel != HCI_CHANNEL_RAW) { + err = -EINVAL; + goto done; + } + + switch (optname) { + case HCI_DATA_DIR: + if (get_user(opt, (int __user *)optval)) { + err = -EFAULT; + break; + } + + if (opt) + hci_pi(sk)->cmsg_mask |= HCI_CMSG_DIR; + else + hci_pi(sk)->cmsg_mask &= ~HCI_CMSG_DIR; + break; + + case HCI_TIME_STAMP: + if (get_user(opt, (int __user *)optval)) { + err = -EFAULT; + break; + } + + if (opt) + hci_pi(sk)->cmsg_mask |= HCI_CMSG_TSTAMP; + else + hci_pi(sk)->cmsg_mask &= ~HCI_CMSG_TSTAMP; + break; + + case HCI_FILTER: + { + struct hci_filter *f = &hci_pi(sk)->filter; + + uf.type_mask = f->type_mask; + uf.opcode = f->opcode; + uf.event_mask[0] = *((u32 *) f->event_mask + 0); + uf.event_mask[1] = *((u32 *) f->event_mask + 1); + } + + len = min_t(unsigned int, len, sizeof(uf)); + if (copy_from_user(&uf, optval, len)) { + err = -EFAULT; + break; + } + + if (!capable(CAP_NET_RAW)) { + uf.type_mask &= hci_sec_filter.type_mask; + uf.event_mask[0] &= *((u32 *) hci_sec_filter.event_mask + 0); + uf.event_mask[1] &= *((u32 *) hci_sec_filter.event_mask + 1); + } + + { + struct hci_filter *f = &hci_pi(sk)->filter; + + f->type_mask = uf.type_mask; + f->opcode = uf.opcode; + *((u32 *) f->event_mask + 0) = uf.event_mask[0]; + *((u32 *) f->event_mask + 1) = uf.event_mask[1]; + } + break; + + default: + err = -ENOPROTOOPT; + break; + } + +done: + release_sock(sk); + return err; +} + +static int hci_sock_getsockopt(struct socket *sock, int level, int optname, char __user *optval, int __user *optlen) +{ + struct hci_ufilter uf; + struct sock *sk = sock->sk; + int len, opt, err = 0; + + BT_DBG("sk %p, opt %d", sk, optname); + + if (get_user(len, optlen)) + return -EFAULT; + + lock_sock(sk); + + if (hci_pi(sk)->channel != HCI_CHANNEL_RAW) { + err = -EINVAL; + goto done; + } + + switch (optname) { + case HCI_DATA_DIR: + if (hci_pi(sk)->cmsg_mask & HCI_CMSG_DIR) + opt = 1; + else + opt = 0; + + if (put_user(opt, optval)) + err = -EFAULT; + break; + + case HCI_TIME_STAMP: + if (hci_pi(sk)->cmsg_mask & HCI_CMSG_TSTAMP) + opt = 1; + else + opt = 0; + + if (put_user(opt, optval)) + err = -EFAULT; + break; + + case HCI_FILTER: + { + struct hci_filter *f = &hci_pi(sk)->filter; + + uf.type_mask = f->type_mask; + uf.opcode = f->opcode; + uf.event_mask[0] = *((u32 *) f->event_mask + 0); + uf.event_mask[1] = *((u32 *) f->event_mask + 1); + } + + len = min_t(unsigned int, len, sizeof(uf)); + if (copy_to_user(optval, &uf, len)) + err = -EFAULT; + break; + + default: + err = -ENOPROTOOPT; + break; + } + +done: + release_sock(sk); + return err; +} + +static const struct proto_ops hci_sock_ops = { + .family = PF_BLUETOOTH, + .owner = THIS_MODULE, + .release = hci_sock_release, + .bind = hci_sock_bind, + .getname = hci_sock_getname, + .sendmsg = hci_sock_sendmsg, + .recvmsg = hci_sock_recvmsg, + .ioctl = hci_sock_ioctl, + .poll = datagram_poll, + .listen = sock_no_listen, + .shutdown = sock_no_shutdown, + .setsockopt = hci_sock_setsockopt, + .getsockopt = hci_sock_getsockopt, + .connect = sock_no_connect, + .socketpair = sock_no_socketpair, + .accept = sock_no_accept, + .mmap = sock_no_mmap +}; + +static struct proto hci_sk_proto = { + .name = "HCI", + .owner = THIS_MODULE, + .obj_size = sizeof(struct hci_pinfo) +}; + +static int hci_sock_create(struct net *net, struct socket *sock, int protocol, + int kern) +{ + struct sock *sk; + + BT_DBG("sock %p", sock); + + if (sock->type != SOCK_RAW) + return -ESOCKTNOSUPPORT; + + sock->ops = &hci_sock_ops; + + sk = sk_alloc(net, PF_BLUETOOTH, GFP_ATOMIC, &hci_sk_proto); + if (!sk) + return -ENOMEM; + + sock_init_data(sock, sk); + + sock_reset_flag(sk, SOCK_ZAPPED); + + sk->sk_protocol = protocol; + + sock->state = SS_UNCONNECTED; + sk->sk_state = BT_OPEN; + + bt_sock_link(&hci_sk_list, sk); + return 0; +} + +static int hci_sock_dev_event(struct notifier_block *this, + unsigned long event, void *ptr) +{ + struct hci_dev *hdev = (struct hci_dev *) ptr; + struct hci_ev_si_device ev; + + BT_DBG("hdev %s event %ld", hdev->name, event); + + /* Send event to sockets */ + ev.event = event; + ev.dev_id = hdev->id; + hci_si_event(NULL, HCI_EV_SI_DEVICE, sizeof(ev), &ev); + + if (event == HCI_DEV_UNREG) { + struct sock *sk; + struct hlist_node *node; + + /* Detach sockets from device */ + read_lock(&hci_sk_list.lock); + sk_for_each(sk, node, &hci_sk_list.head) { + bh_lock_sock_nested(sk); + if (hci_pi(sk)->hdev == hdev) { + hci_pi(sk)->hdev = NULL; + sk->sk_err = EPIPE; + sk->sk_state = BT_OPEN; + sk->sk_state_change(sk); + + hci_dev_put(hdev); + } + bh_unlock_sock(sk); + } + read_unlock(&hci_sk_list.lock); + } + + return NOTIFY_DONE; +} + +static const struct net_proto_family hci_sock_family_ops = { + .family = PF_BLUETOOTH, + .owner = THIS_MODULE, + .create = hci_sock_create, +}; + +static struct notifier_block hci_sock_nblock = { + .notifier_call = hci_sock_dev_event +}; + +int __init hci_sock_init(void) +{ + int err; + + err = proto_register(&hci_sk_proto, 0); + if (err < 0) + return err; + + err = bt_sock_register(BTPROTO_HCI, &hci_sock_family_ops); + if (err < 0) + goto error; + + hci_register_notifier(&hci_sock_nblock); + + BT_INFO("HCI socket layer initialized"); + + return 0; + +error: + BT_ERR("HCI socket registration failed"); + proto_unregister(&hci_sk_proto); + return err; +} + +void hci_sock_cleanup(void) +{ + if (bt_sock_unregister(BTPROTO_HCI) < 0) + BT_ERR("HCI socket unregistration failed"); + + hci_unregister_notifier(&hci_sock_nblock); + + proto_unregister(&hci_sk_proto); +} diff --git a/net/bluetooth_tizen/hci_sysfs.c b/net/bluetooth_tizen/hci_sysfs.c new file mode 100644 index 0000000..bc15429 --- /dev/null +++ b/net/bluetooth_tizen/hci_sysfs.c @@ -0,0 +1,588 @@ +/* Bluetooth HCI driver model support. */ + +#include <linux/kernel.h> +#include <linux/slab.h> +#include <linux/init.h> +#include <linux/debugfs.h> +#include <linux/seq_file.h> +#include <linux/module.h> + +#include <net/bluetooth/bluetooth.h> +#include <net/bluetooth/hci_core.h> + +static struct class *bt_class; + +struct dentry *bt_debugfs; +EXPORT_SYMBOL_GPL(bt_debugfs); + +static inline char *link_typetostr(int type) +{ + switch (type) { + case ACL_LINK: + return "ACL"; + case SCO_LINK: + return "SCO"; + case ESCO_LINK: + return "eSCO"; + case LE_LINK: + return "LE"; + default: + return "UNKNOWN"; + } +} + +static ssize_t show_link_type(struct device *dev, struct device_attribute *attr, char *buf) +{ + struct hci_conn *conn = to_hci_conn(dev); + return sprintf(buf, "%s\n", link_typetostr(conn->type)); +} + +static ssize_t show_link_address(struct device *dev, struct device_attribute *attr, char *buf) +{ + struct hci_conn *conn = to_hci_conn(dev); + return sprintf(buf, "%s\n", batostr(&conn->dst)); +} + +static ssize_t show_link_features(struct device *dev, struct device_attribute *attr, char *buf) +{ + struct hci_conn *conn = to_hci_conn(dev); + + return sprintf(buf, "0x%02x%02x%02x%02x%02x%02x%02x%02x\n", + conn->features[0], conn->features[1], + conn->features[2], conn->features[3], + conn->features[4], conn->features[5], + conn->features[6], conn->features[7]); +} + +#define LINK_ATTR(_name, _mode, _show, _store) \ +struct device_attribute link_attr_##_name = __ATTR(_name, _mode, _show, _store) + +static LINK_ATTR(type, S_IRUGO, show_link_type, NULL); +static LINK_ATTR(address, S_IRUGO, show_link_address, NULL); +static LINK_ATTR(features, S_IRUGO, show_link_features, NULL); + +static struct attribute *bt_link_attrs[] = { + &link_attr_type.attr, + &link_attr_address.attr, + &link_attr_features.attr, + NULL +}; + +static struct attribute_group bt_link_group = { + .attrs = bt_link_attrs, +}; + +static const struct attribute_group *bt_link_groups[] = { + &bt_link_group, + NULL +}; + +static void bt_link_release(struct device *dev) +{ + struct hci_conn *conn = to_hci_conn(dev); + kfree(conn); +} + +static struct device_type bt_link = { + .name = "link", + .groups = bt_link_groups, + .release = bt_link_release, +}; + +/* + * The rfcomm tty device will possibly retain even when conn + * is down, and sysfs doesn't support move zombie device, + * so we should move the device before conn device is destroyed. + */ +static int __match_tty(struct device *dev, void *data) +{ + return !strncmp(dev_name(dev), "rfcomm", 6); +} + +void hci_conn_init_sysfs(struct hci_conn *conn) +{ + struct hci_dev *hdev = conn->hdev; + + BT_DBG("conn %p", conn); + + conn->dev.type = &bt_link; + conn->dev.class = bt_class; + conn->dev.parent = &hdev->dev; + + device_initialize(&conn->dev); +} + +void hci_conn_add_sysfs(struct hci_conn *conn) +{ + struct hci_dev *hdev = conn->hdev; + + BT_DBG("conn %p", conn); + + dev_set_name(&conn->dev, "%s:%d", hdev->name, conn->handle); + + if (device_add(&conn->dev) < 0) { + BT_ERR("Failed to register connection device"); + return; + } + + hci_dev_hold(hdev); +} + +void hci_conn_del_sysfs(struct hci_conn *conn) +{ + struct hci_dev *hdev = conn->hdev; + + if (!device_is_registered(&conn->dev)) + return; + + while (1) { + struct device *dev; + + dev = device_find_child(&conn->dev, NULL, __match_tty); + if (!dev) + break; + device_move(dev, NULL, DPM_ORDER_DEV_LAST); + put_device(dev); + } + + device_del(&conn->dev); + put_device(&conn->dev); + + hci_dev_put(hdev); +} + +static inline char *host_bustostr(int bus) +{ + switch (bus) { + case HCI_VIRTUAL: + return "VIRTUAL"; + case HCI_USB: + return "USB"; + case HCI_PCCARD: + return "PCCARD"; + case HCI_UART: + return "UART"; + case HCI_RS232: + return "RS232"; + case HCI_PCI: + return "PCI"; + case HCI_SDIO: + return "SDIO"; + default: + return "UNKNOWN"; + } +} + +static inline char *host_typetostr(int type) +{ + switch (type) { + case HCI_BREDR: + return "BR/EDR"; + case HCI_AMP: + return "AMP"; + default: + return "UNKNOWN"; + } +} + +static ssize_t show_bus(struct device *dev, struct device_attribute *attr, char *buf) +{ + struct hci_dev *hdev = to_hci_dev(dev); + return sprintf(buf, "%s\n", host_bustostr(hdev->bus)); +} + +static ssize_t show_type(struct device *dev, struct device_attribute *attr, char *buf) +{ + struct hci_dev *hdev = to_hci_dev(dev); + return sprintf(buf, "%s\n", host_typetostr(hdev->dev_type)); +} + +static ssize_t show_name(struct device *dev, struct device_attribute *attr, char *buf) +{ + struct hci_dev *hdev = to_hci_dev(dev); + char name[HCI_MAX_NAME_LENGTH + 1]; + int i; + + for (i = 0; i < HCI_MAX_NAME_LENGTH; i++) + name[i] = hdev->dev_name[i]; + + name[HCI_MAX_NAME_LENGTH] = '\0'; + return sprintf(buf, "%s\n", name); +} + +static ssize_t show_class(struct device *dev, struct device_attribute *attr, char *buf) +{ + struct hci_dev *hdev = to_hci_dev(dev); + return sprintf(buf, "0x%.2x%.2x%.2x\n", + hdev->dev_class[2], hdev->dev_class[1], hdev->dev_class[0]); +} + +static ssize_t show_address(struct device *dev, struct device_attribute *attr, char *buf) +{ + struct hci_dev *hdev = to_hci_dev(dev); + return sprintf(buf, "%s\n", batostr(&hdev->bdaddr)); +} + +static ssize_t show_features(struct device *dev, struct device_attribute *attr, char *buf) +{ + struct hci_dev *hdev = to_hci_dev(dev); + + return sprintf(buf, "0x%02x%02x%02x%02x%02x%02x%02x%02x\n", + hdev->features[0], hdev->features[1], + hdev->features[2], hdev->features[3], + hdev->features[4], hdev->features[5], + hdev->features[6], hdev->features[7]); +} + +static ssize_t show_manufacturer(struct device *dev, struct device_attribute *attr, char *buf) +{ + struct hci_dev *hdev = to_hci_dev(dev); + return sprintf(buf, "%d\n", hdev->manufacturer); +} + +static ssize_t show_hci_version(struct device *dev, struct device_attribute *attr, char *buf) +{ + struct hci_dev *hdev = to_hci_dev(dev); + return sprintf(buf, "%d\n", hdev->hci_ver); +} + +static ssize_t show_hci_revision(struct device *dev, struct device_attribute *attr, char *buf) +{ + struct hci_dev *hdev = to_hci_dev(dev); + return sprintf(buf, "%d\n", hdev->hci_rev); +} + +static ssize_t show_idle_timeout(struct device *dev, struct device_attribute *attr, char *buf) +{ + struct hci_dev *hdev = to_hci_dev(dev); + return sprintf(buf, "%d\n", hdev->idle_timeout); +} + +static ssize_t store_idle_timeout(struct device *dev, struct device_attribute *attr, const char *buf, size_t count) +{ + struct hci_dev *hdev = to_hci_dev(dev); + unsigned int val; + int rv; + + rv = kstrtouint(buf, 0, &val); + if (rv < 0) + return rv; + + if (val != 0 && (val < 500 || val > 3600000)) + return -EINVAL; + + hdev->idle_timeout = val; + + return count; +} + +static ssize_t show_sniff_max_interval(struct device *dev, struct device_attribute *attr, char *buf) +{ + struct hci_dev *hdev = to_hci_dev(dev); + return sprintf(buf, "%d\n", hdev->sniff_max_interval); +} + +static ssize_t store_sniff_max_interval(struct device *dev, struct device_attribute *attr, const char *buf, size_t count) +{ + struct hci_dev *hdev = to_hci_dev(dev); + u16 val; + int rv; + + rv = kstrtou16(buf, 0, &val); + if (rv < 0) + return rv; + + if (val == 0 || val % 2 || val < hdev->sniff_min_interval) + return -EINVAL; + + hdev->sniff_max_interval = val; + + return count; +} + +static ssize_t show_sniff_min_interval(struct device *dev, struct device_attribute *attr, char *buf) +{ + struct hci_dev *hdev = to_hci_dev(dev); + return sprintf(buf, "%d\n", hdev->sniff_min_interval); +} + +static ssize_t store_sniff_min_interval(struct device *dev, struct device_attribute *attr, const char *buf, size_t count) +{ + struct hci_dev *hdev = to_hci_dev(dev); + u16 val; + int rv; + + rv = kstrtou16(buf, 0, &val); + if (rv < 0) + return rv; + + if (val == 0 || val % 2 || val > hdev->sniff_max_interval) + return -EINVAL; + + hdev->sniff_min_interval = val; + + return count; +} + +static DEVICE_ATTR(bus, S_IRUGO, show_bus, NULL); +static DEVICE_ATTR(type, S_IRUGO, show_type, NULL); +static DEVICE_ATTR(name, S_IRUGO, show_name, NULL); +static DEVICE_ATTR(class, S_IRUGO, show_class, NULL); +static DEVICE_ATTR(address, S_IRUGO, show_address, NULL); +static DEVICE_ATTR(features, S_IRUGO, show_features, NULL); +static DEVICE_ATTR(manufacturer, S_IRUGO, show_manufacturer, NULL); +static DEVICE_ATTR(hci_version, S_IRUGO, show_hci_version, NULL); +static DEVICE_ATTR(hci_revision, S_IRUGO, show_hci_revision, NULL); + +static DEVICE_ATTR(idle_timeout, S_IRUGO | S_IWUSR, + show_idle_timeout, store_idle_timeout); +static DEVICE_ATTR(sniff_max_interval, S_IRUGO | S_IWUSR, + show_sniff_max_interval, store_sniff_max_interval); +static DEVICE_ATTR(sniff_min_interval, S_IRUGO | S_IWUSR, + show_sniff_min_interval, store_sniff_min_interval); + +static struct attribute *bt_host_attrs[] = { + &dev_attr_bus.attr, + &dev_attr_type.attr, + &dev_attr_name.attr, + &dev_attr_class.attr, + &dev_attr_address.attr, + &dev_attr_features.attr, + &dev_attr_manufacturer.attr, + &dev_attr_hci_version.attr, + &dev_attr_hci_revision.attr, + &dev_attr_idle_timeout.attr, + &dev_attr_sniff_max_interval.attr, + &dev_attr_sniff_min_interval.attr, + NULL +}; + +static struct attribute_group bt_host_group = { + .attrs = bt_host_attrs, +}; + +static const struct attribute_group *bt_host_groups[] = { + &bt_host_group, + NULL +}; + +static void bt_host_release(struct device *dev) +{ + struct hci_dev *hdev = to_hci_dev(dev); + kfree(hdev); + module_put(THIS_MODULE); +} + +static struct device_type bt_host = { + .name = "host", + .groups = bt_host_groups, + .release = bt_host_release, +}; + +static int inquiry_cache_show(struct seq_file *f, void *p) +{ + struct hci_dev *hdev = f->private; + struct discovery_state *cache = &hdev->discovery; + struct inquiry_entry *e; + + hci_dev_lock(hdev); + + list_for_each_entry(e, &cache->all, all) { + struct inquiry_data *data = &e->data; + seq_printf(f, "%s %d %d %d 0x%.2x%.2x%.2x 0x%.4x %d %d %u\n", + batostr(&data->bdaddr), + data->pscan_rep_mode, data->pscan_period_mode, + data->pscan_mode, data->dev_class[2], + data->dev_class[1], data->dev_class[0], + __le16_to_cpu(data->clock_offset), + data->rssi, data->ssp_mode, e->timestamp); + } + + hci_dev_unlock(hdev); + + return 0; +} + +static int inquiry_cache_open(struct inode *inode, struct file *file) +{ + return single_open(file, inquiry_cache_show, inode->i_private); +} + +static const struct file_operations inquiry_cache_fops = { + .open = inquiry_cache_open, + .read = seq_read, + .llseek = seq_lseek, + .release = single_release, +}; + +static int blacklist_show(struct seq_file *f, void *p) +{ + struct hci_dev *hdev = f->private; + struct bdaddr_list *b; + + hci_dev_lock(hdev); + + list_for_each_entry(b, &hdev->blacklist, list) + seq_printf(f, "%s\n", batostr(&b->bdaddr)); + + hci_dev_unlock(hdev); + + return 0; +} + +static int blacklist_open(struct inode *inode, struct file *file) +{ + return single_open(file, blacklist_show, inode->i_private); +} + +static const struct file_operations blacklist_fops = { + .open = blacklist_open, + .read = seq_read, + .llseek = seq_lseek, + .release = single_release, +}; + +static void print_bt_uuid(struct seq_file *f, u8 *uuid) +{ + u32 data0, data4; + u16 data1, data2, data3, data5; + + memcpy(&data0, &uuid[0], 4); + memcpy(&data1, &uuid[4], 2); + memcpy(&data2, &uuid[6], 2); + memcpy(&data3, &uuid[8], 2); + memcpy(&data4, &uuid[10], 4); + memcpy(&data5, &uuid[14], 2); + + seq_printf(f, "%.8x-%.4x-%.4x-%.4x-%.8x%.4x\n", + ntohl(data0), ntohs(data1), ntohs(data2), + ntohs(data3), ntohl(data4), ntohs(data5)); +} + +static int uuids_show(struct seq_file *f, void *p) +{ + struct hci_dev *hdev = f->private; + struct bt_uuid *uuid; + + hci_dev_lock(hdev); + + list_for_each_entry(uuid, &hdev->uuids, list) + print_bt_uuid(f, uuid->uuid); + + hci_dev_unlock(hdev); + + return 0; +} + +static int uuids_open(struct inode *inode, struct file *file) +{ + return single_open(file, uuids_show, inode->i_private); +} + +static const struct file_operations uuids_fops = { + .open = uuids_open, + .read = seq_read, + .llseek = seq_lseek, + .release = single_release, +}; + +static int auto_accept_delay_set(void *data, u64 val) +{ + struct hci_dev *hdev = data; + + hci_dev_lock(hdev); + + hdev->auto_accept_delay = val; + + hci_dev_unlock(hdev); + + return 0; +} + +static int auto_accept_delay_get(void *data, u64 *val) +{ + struct hci_dev *hdev = data; + + hci_dev_lock(hdev); + + *val = hdev->auto_accept_delay; + + hci_dev_unlock(hdev); + + return 0; +} + +DEFINE_SIMPLE_ATTRIBUTE(auto_accept_delay_fops, auto_accept_delay_get, + auto_accept_delay_set, "%llu\n"); + +void hci_init_sysfs(struct hci_dev *hdev) +{ + struct device *dev = &hdev->dev; + + dev->type = &bt_host; + dev->class = bt_class; + + __module_get(THIS_MODULE); + device_initialize(dev); +} + +int hci_add_sysfs(struct hci_dev *hdev) +{ + struct device *dev = &hdev->dev; + int err; + + BT_DBG("%p name %s bus %d", hdev, hdev->name, hdev->bus); + + dev->parent = hdev->parent; + dev_set_name(dev, "%s", hdev->name); + + err = device_add(dev); + if (err < 0) + return err; + + if (!bt_debugfs) + return 0; + + hdev->debugfs = debugfs_create_dir(hdev->name, bt_debugfs); + if (!hdev->debugfs) + return 0; + + debugfs_create_file("inquiry_cache", 0444, hdev->debugfs, + hdev, &inquiry_cache_fops); + + debugfs_create_file("blacklist", 0444, hdev->debugfs, + hdev, &blacklist_fops); + + debugfs_create_file("uuids", 0444, hdev->debugfs, hdev, &uuids_fops); + + debugfs_create_file("auto_accept_delay", 0444, hdev->debugfs, hdev, + &auto_accept_delay_fops); + return 0; +} + +void hci_del_sysfs(struct hci_dev *hdev) +{ + BT_DBG("%p name %s bus %d", hdev, hdev->name, hdev->bus); + + debugfs_remove_recursive(hdev->debugfs); + + device_del(&hdev->dev); +} + +int __init bt_sysfs_init(void) +{ + bt_debugfs = debugfs_create_dir("bluetooth", NULL); + + bt_class = class_create(THIS_MODULE, "bluetooth"); + if (IS_ERR(bt_class)) + return PTR_ERR(bt_class); + + return 0; +} + +void bt_sysfs_cleanup(void) +{ + class_destroy(bt_class); + + debugfs_remove_recursive(bt_debugfs); +} diff --git a/net/bluetooth_tizen/hidp/Kconfig b/net/bluetooth_tizen/hidp/Kconfig new file mode 100644 index 0000000..4deaca7 --- /dev/null +++ b/net/bluetooth_tizen/hidp/Kconfig @@ -0,0 +1,12 @@ +config BT_HIDP + tristate "HIDP protocol support" + depends on BT && INPUT && HID_SUPPORT + select HID + help + HIDP (Human Interface Device Protocol) is a transport layer + for HID reports. HIDP is required for the Bluetooth Human + Interface Device Profile. + + Say Y here to compile HIDP support into the kernel or say M to + compile it as module (hidp). + diff --git a/net/bluetooth_tizen/hidp/Makefile b/net/bluetooth_tizen/hidp/Makefile new file mode 100644 index 0000000..a9ee115 --- /dev/null +++ b/net/bluetooth_tizen/hidp/Makefile @@ -0,0 +1,7 @@ +# +# Makefile for the Linux Bluetooth HIDP layer +# + +obj-$(CONFIG_BT_HIDP) += hidp.o + +hidp-objs := core.o sock.o diff --git a/net/bluetooth_tizen/hidp/core.c b/net/bluetooth_tizen/hidp/core.c new file mode 100644 index 0000000..d478be1 --- /dev/null +++ b/net/bluetooth_tizen/hidp/core.c @@ -0,0 +1,1242 @@ +/* + HIDP implementation for Linux Bluetooth stack (BlueZ). + Copyright (C) 2003-2004 Marcel Holtmann <marcel@holtmann.org> + + This program is free software; you can redistribute it and/or modify + it under the terms of the GNU General Public License version 2 as + published by the Free Software Foundation; + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS + OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT OF THIRD PARTY RIGHTS. + IN NO EVENT SHALL THE COPYRIGHT HOLDER(S) AND AUTHOR(S) BE LIABLE FOR ANY + CLAIM, OR ANY SPECIAL INDIRECT OR CONSEQUENTIAL DAMAGES, OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + + ALL LIABILITY, INCLUDING LIABILITY FOR INFRINGEMENT OF ANY PATENTS, + COPYRIGHTS, TRADEMARKS OR OTHER RIGHTS, RELATING TO USE OF THIS + SOFTWARE IS DISCLAIMED. +*/ + +#include <linux/module.h> + +#include <linux/types.h> +#include <linux/errno.h> +#include <linux/kernel.h> +#include <linux/sched.h> +#include <linux/slab.h> +#include <linux/poll.h> +#include <linux/freezer.h> +#include <linux/fcntl.h> +#include <linux/skbuff.h> +#include <linux/socket.h> +#include <linux/ioctl.h> +#include <linux/file.h> +#include <linux/init.h> +#include <linux/wait.h> +#include <linux/mutex.h> +#include <linux/kthread.h> +#include <net/sock.h> + +#include <linux/input.h> +#include <linux/hid.h> +#include <linux/hidraw.h> + +#include <net/bluetooth/bluetooth.h> +#include <net/bluetooth/hci_core.h> +#include <net/bluetooth/l2cap.h> + +#include "hidp.h" + +#define VERSION "1.2" + +static DECLARE_RWSEM(hidp_session_sem); +static LIST_HEAD(hidp_session_list); + +static unsigned char hidp_keycode[256] = { + 0, 0, 0, 0, 30, 48, 46, 32, 18, 33, 34, 35, 23, 36, + 37, 38, 50, 49, 24, 25, 16, 19, 31, 20, 22, 47, 17, 45, + 21, 44, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 28, 1, + 14, 15, 57, 12, 13, 26, 27, 43, 43, 39, 40, 41, 51, 52, + 53, 58, 59, 60, 61, 62, 63, 64, 65, 66, 67, 68, 87, 88, + 99, 70, 119, 110, 102, 104, 111, 107, 109, 106, 105, 108, 103, 69, + 98, 55, 74, 78, 96, 79, 80, 81, 75, 76, 77, 71, 72, 73, + 82, 83, 86, 127, 116, 117, 183, 184, 185, 186, 187, 188, 189, 190, + 191, 192, 193, 194, 134, 138, 130, 132, 128, 129, 131, 137, 133, 135, + 136, 113, 115, 114, 0, 0, 0, 121, 0, 89, 93, 124, 92, 94, + 95, 0, 0, 0, 122, 123, 90, 91, 85, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 29, 42, 56, 125, 97, 54, 100, 126, 164, 166, 165, 163, 161, 115, + 114, 113, 150, 158, 159, 128, 136, 177, 178, 176, 142, 152, 173, 140 +}; + +static unsigned char hidp_mkeyspat[] = { 0x01, 0x01, 0x01, 0x01, 0x01, 0x01 }; + +static struct hidp_session *__hidp_get_session(bdaddr_t *bdaddr) +{ + struct hidp_session *session; + + BT_DBG(""); + + list_for_each_entry(session, &hidp_session_list, list) { + if (!bacmp(bdaddr, &session->bdaddr)) + return session; + } + + return NULL; +} + +static void __hidp_link_session(struct hidp_session *session) +{ + list_add(&session->list, &hidp_session_list); +} + +static void __hidp_unlink_session(struct hidp_session *session) +{ + hci_conn_put_device(session->conn); + + list_del(&session->list); +} + +static void __hidp_copy_session(struct hidp_session *session, struct hidp_conninfo *ci) +{ + memset(ci, 0, sizeof(*ci)); + bacpy(&ci->bdaddr, &session->bdaddr); + + ci->flags = session->flags; + ci->state = session->state; + + ci->vendor = 0x0000; + ci->product = 0x0000; + ci->version = 0x0000; + + if (session->input) { + ci->vendor = session->input->id.vendor; + ci->product = session->input->id.product; + ci->version = session->input->id.version; + if (session->input->name) + strncpy(ci->name, session->input->name, 128); + else + strncpy(ci->name, "HID Boot Device", 128); + } + + if (session->hid) { + ci->vendor = session->hid->vendor; + ci->product = session->hid->product; + ci->version = session->hid->version; + strncpy(ci->name, session->hid->name, 128); + } +} + +static int hidp_queue_event(struct hidp_session *session, struct input_dev *dev, + unsigned int type, unsigned int code, int value) +{ + unsigned char newleds; + struct sk_buff *skb; + + BT_DBG("session %p type %d code %d value %d", session, type, code, value); + + if (type != EV_LED) + return -1; + + newleds = (!!test_bit(LED_KANA, dev->led) << 3) | + (!!test_bit(LED_COMPOSE, dev->led) << 3) | + (!!test_bit(LED_SCROLLL, dev->led) << 2) | + (!!test_bit(LED_CAPSL, dev->led) << 1) | + (!!test_bit(LED_NUML, dev->led)); + + if (session->leds == newleds) + return 0; + + session->leds = newleds; + + skb = alloc_skb(3, GFP_ATOMIC); + if (!skb) { + BT_ERR("Can't allocate memory for new frame"); + return -ENOMEM; + } + + *skb_put(skb, 1) = HIDP_TRANS_DATA | HIDP_DATA_RTYPE_OUPUT; + *skb_put(skb, 1) = 0x01; + *skb_put(skb, 1) = newleds; + + skb_queue_tail(&session->intr_transmit, skb); + + hidp_schedule(session); + + return 0; +} + +static int hidp_hidinput_event(struct input_dev *dev, unsigned int type, unsigned int code, int value) +{ + struct hid_device *hid = input_get_drvdata(dev); + struct hidp_session *session = hid->driver_data; + + return hidp_queue_event(session, dev, type, code, value); +} + +static int hidp_input_event(struct input_dev *dev, unsigned int type, unsigned int code, int value) +{ + struct hidp_session *session = input_get_drvdata(dev); + + return hidp_queue_event(session, dev, type, code, value); +} + +static void hidp_input_report(struct hidp_session *session, struct sk_buff *skb) +{ + struct input_dev *dev = session->input; + unsigned char *keys = session->keys; + unsigned char *udata = skb->data + 1; + signed char *sdata = skb->data + 1; + int i, size = skb->len - 1; + + switch (skb->data[0]) { + case 0x01: /* Keyboard report */ + for (i = 0; i < 8; i++) + input_report_key(dev, hidp_keycode[i + 224], (udata[0] >> i) & 1); + + /* If all the key codes have been set to 0x01, it means + * too many keys were pressed at the same time. */ + if (!memcmp(udata + 2, hidp_mkeyspat, 6)) + break; + + for (i = 2; i < 8; i++) { + if (keys[i] > 3 && memscan(udata + 2, keys[i], 6) == udata + 8) { + if (hidp_keycode[keys[i]]) + input_report_key(dev, hidp_keycode[keys[i]], 0); + else + BT_ERR("Unknown key (scancode %#x) released.", keys[i]); + } + + if (udata[i] > 3 && memscan(keys + 2, udata[i], 6) == keys + 8) { + if (hidp_keycode[udata[i]]) + input_report_key(dev, hidp_keycode[udata[i]], 1); + else + BT_ERR("Unknown key (scancode %#x) pressed.", udata[i]); + } + } + + memcpy(keys, udata, 8); + break; + + case 0x02: /* Mouse report */ + input_report_key(dev, BTN_LEFT, sdata[0] & 0x01); + input_report_key(dev, BTN_RIGHT, sdata[0] & 0x02); + input_report_key(dev, BTN_MIDDLE, sdata[0] & 0x04); + input_report_key(dev, BTN_SIDE, sdata[0] & 0x08); + input_report_key(dev, BTN_EXTRA, sdata[0] & 0x10); + + input_report_rel(dev, REL_X, sdata[1]); + input_report_rel(dev, REL_Y, sdata[2]); + + if (size > 3) + input_report_rel(dev, REL_WHEEL, sdata[3]); + break; + } + + input_sync(dev); +} + +static int __hidp_send_ctrl_message(struct hidp_session *session, + unsigned char hdr, unsigned char *data, int size) +{ + struct sk_buff *skb; + + BT_DBG("session %p data %p size %d", session, data, size); + + if (atomic_read(&session->terminate)) + return -EIO; + + skb = alloc_skb(size + 1, GFP_ATOMIC); + if (!skb) { + BT_ERR("Can't allocate memory for new frame"); + return -ENOMEM; + } + + *skb_put(skb, 1) = hdr; + if (data && size > 0) + memcpy(skb_put(skb, size), data, size); + + skb_queue_tail(&session->ctrl_transmit, skb); + + return 0; +} + +static inline int hidp_send_ctrl_message(struct hidp_session *session, + unsigned char hdr, unsigned char *data, int size) +{ + int err; + + err = __hidp_send_ctrl_message(session, hdr, data, size); + + hidp_schedule(session); + + return err; +} + +static int hidp_queue_report(struct hidp_session *session, + unsigned char *data, int size) +{ + struct sk_buff *skb; + + BT_DBG("session %p hid %p data %p size %d", session, session->hid, data, size); + + skb = alloc_skb(size + 1, GFP_ATOMIC); + if (!skb) { + BT_ERR("Can't allocate memory for new frame"); + return -ENOMEM; + } + + *skb_put(skb, 1) = 0xa2; + if (size > 0) + memcpy(skb_put(skb, size), data, size); + + skb_queue_tail(&session->intr_transmit, skb); + + hidp_schedule(session); + + return 0; +} + +static int hidp_send_report(struct hidp_session *session, struct hid_report *report) +{ + unsigned char buf[32]; + int rsize; + + rsize = ((report->size - 1) >> 3) + 1 + (report->id > 0); + if (rsize > sizeof(buf)) + return -EIO; + + hid_output_report(report, buf); + + return hidp_queue_report(session, buf, rsize); +} + +static int hidp_get_raw_report(struct hid_device *hid, + unsigned char report_number, + unsigned char *data, size_t count, + unsigned char report_type) +{ + struct hidp_session *session = hid->driver_data; + struct sk_buff *skb; + size_t len; + int numbered_reports = hid->report_enum[report_type].numbered; + int ret; + + switch (report_type) { + case HID_FEATURE_REPORT: + report_type = HIDP_TRANS_GET_REPORT | HIDP_DATA_RTYPE_FEATURE; + break; + case HID_INPUT_REPORT: + report_type = HIDP_TRANS_GET_REPORT | HIDP_DATA_RTYPE_INPUT; + break; + case HID_OUTPUT_REPORT: + report_type = HIDP_TRANS_GET_REPORT | HIDP_DATA_RTYPE_OUPUT; + break; + default: + return -EINVAL; + } + + if (mutex_lock_interruptible(&session->report_mutex)) + return -ERESTARTSYS; + + /* Set up our wait, and send the report request to the device. */ + session->waiting_report_type = report_type & HIDP_DATA_RTYPE_MASK; + session->waiting_report_number = numbered_reports ? report_number : -1; + set_bit(HIDP_WAITING_FOR_RETURN, &session->flags); + data[0] = report_number; + ret = hidp_send_ctrl_message(hid->driver_data, report_type, data, 1); + if (ret) + goto err; + + /* Wait for the return of the report. The returned report + gets put in session->report_return. */ + while (test_bit(HIDP_WAITING_FOR_RETURN, &session->flags)) { + int res; + + res = wait_event_interruptible_timeout(session->report_queue, + !test_bit(HIDP_WAITING_FOR_RETURN, &session->flags), + 5*HZ); + if (res == 0) { + /* timeout */ + ret = -EIO; + goto err; + } + if (res < 0) { + /* signal */ + ret = -ERESTARTSYS; + goto err; + } + } + + skb = session->report_return; + if (skb) { + len = skb->len < count ? skb->len : count; + memcpy(data, skb->data, len); + + kfree_skb(skb); + session->report_return = NULL; + } else { + /* Device returned a HANDSHAKE, indicating protocol error. */ + len = -EIO; + } + + clear_bit(HIDP_WAITING_FOR_RETURN, &session->flags); + mutex_unlock(&session->report_mutex); + + return len; + +err: + clear_bit(HIDP_WAITING_FOR_RETURN, &session->flags); + mutex_unlock(&session->report_mutex); + return ret; +} + +static int hidp_output_raw_report(struct hid_device *hid, unsigned char *data, size_t count, + unsigned char report_type) +{ + struct hidp_session *session = hid->driver_data; + int ret; + + switch (report_type) { + case HID_FEATURE_REPORT: + report_type = HIDP_TRANS_SET_REPORT | HIDP_DATA_RTYPE_FEATURE; + break; + case HID_OUTPUT_REPORT: + report_type = HIDP_TRANS_SET_REPORT | HIDP_DATA_RTYPE_OUPUT; + break; + default: + return -EINVAL; + } + + if (mutex_lock_interruptible(&session->report_mutex)) + return -ERESTARTSYS; + + /* Set up our wait, and send the report request to the device. */ + set_bit(HIDP_WAITING_FOR_SEND_ACK, &session->flags); + ret = hidp_send_ctrl_message(hid->driver_data, report_type, data, + count); + if (ret) + goto err; + + /* Wait for the ACK from the device. */ + while (test_bit(HIDP_WAITING_FOR_SEND_ACK, &session->flags)) { + int res; + + res = wait_event_interruptible_timeout(session->report_queue, + !test_bit(HIDP_WAITING_FOR_SEND_ACK, &session->flags), + 10*HZ); + if (res == 0) { + /* timeout */ + ret = -EIO; + goto err; + } + if (res < 0) { + /* signal */ + ret = -ERESTARTSYS; + goto err; + } + } + + if (!session->output_report_success) { + ret = -EIO; + goto err; + } + + ret = count; + +err: + clear_bit(HIDP_WAITING_FOR_SEND_ACK, &session->flags); + mutex_unlock(&session->report_mutex); + return ret; +} + +static void hidp_idle_timeout(unsigned long arg) +{ + struct hidp_session *session = (struct hidp_session *) arg; + + atomic_inc(&session->terminate); + wake_up_process(session->task); +} + +static void hidp_set_timer(struct hidp_session *session) +{ + if (session->idle_to > 0) + mod_timer(&session->timer, jiffies + HZ * session->idle_to); +} + +static inline void hidp_del_timer(struct hidp_session *session) +{ + if (session->idle_to > 0) + del_timer(&session->timer); +} + +static void hidp_process_handshake(struct hidp_session *session, + unsigned char param) +{ + BT_DBG("session %p param 0x%02x", session, param); + session->output_report_success = 0; /* default condition */ + + switch (param) { + case HIDP_HSHK_SUCCESSFUL: + /* FIXME: Call into SET_ GET_ handlers here */ + session->output_report_success = 1; + break; + + case HIDP_HSHK_NOT_READY: + case HIDP_HSHK_ERR_INVALID_REPORT_ID: + case HIDP_HSHK_ERR_UNSUPPORTED_REQUEST: + case HIDP_HSHK_ERR_INVALID_PARAMETER: + if (test_and_clear_bit(HIDP_WAITING_FOR_RETURN, &session->flags)) + wake_up_interruptible(&session->report_queue); + + /* FIXME: Call into SET_ GET_ handlers here */ + break; + + case HIDP_HSHK_ERR_UNKNOWN: + break; + + case HIDP_HSHK_ERR_FATAL: + /* Device requests a reboot, as this is the only way this error + * can be recovered. */ + __hidp_send_ctrl_message(session, + HIDP_TRANS_HID_CONTROL | HIDP_CTRL_SOFT_RESET, NULL, 0); + break; + + default: + __hidp_send_ctrl_message(session, + HIDP_TRANS_HANDSHAKE | HIDP_HSHK_ERR_INVALID_PARAMETER, NULL, 0); + break; + } + + /* Wake up the waiting thread. */ + if (test_and_clear_bit(HIDP_WAITING_FOR_SEND_ACK, &session->flags)) + wake_up_interruptible(&session->report_queue); +} + +static void hidp_process_hid_control(struct hidp_session *session, + unsigned char param) +{ + BT_DBG("session %p param 0x%02x", session, param); + + if (param == HIDP_CTRL_VIRTUAL_CABLE_UNPLUG) { + /* Flush the transmit queues */ + skb_queue_purge(&session->ctrl_transmit); + skb_queue_purge(&session->intr_transmit); + + atomic_inc(&session->terminate); + wake_up_process(current); + } +} + +/* Returns true if the passed-in skb should be freed by the caller. */ +static int hidp_process_data(struct hidp_session *session, struct sk_buff *skb, + unsigned char param) +{ + int done_with_skb = 1; + BT_DBG("session %p skb %p len %d param 0x%02x", session, skb, skb->len, param); + + switch (param) { + case HIDP_DATA_RTYPE_INPUT: + hidp_set_timer(session); + + if (session->input) + hidp_input_report(session, skb); + + if (session->hid) + hid_input_report(session->hid, HID_INPUT_REPORT, skb->data, skb->len, 0); + break; + + case HIDP_DATA_RTYPE_OTHER: + case HIDP_DATA_RTYPE_OUPUT: + case HIDP_DATA_RTYPE_FEATURE: + break; + + default: + __hidp_send_ctrl_message(session, + HIDP_TRANS_HANDSHAKE | HIDP_HSHK_ERR_INVALID_PARAMETER, NULL, 0); + } + + if (test_bit(HIDP_WAITING_FOR_RETURN, &session->flags) && + param == session->waiting_report_type) { + if (session->waiting_report_number < 0 || + session->waiting_report_number == skb->data[0]) { + /* hidp_get_raw_report() is waiting on this report. */ + session->report_return = skb; + done_with_skb = 0; + clear_bit(HIDP_WAITING_FOR_RETURN, &session->flags); + wake_up_interruptible(&session->report_queue); + } + } + + return done_with_skb; +} + +static void hidp_recv_ctrl_frame(struct hidp_session *session, + struct sk_buff *skb) +{ + unsigned char hdr, type, param; + int free_skb = 1; + + BT_DBG("session %p skb %p len %d", session, skb, skb->len); + + hdr = skb->data[0]; + skb_pull(skb, 1); + + type = hdr & HIDP_HEADER_TRANS_MASK; + param = hdr & HIDP_HEADER_PARAM_MASK; + + switch (type) { + case HIDP_TRANS_HANDSHAKE: + hidp_process_handshake(session, param); + break; + + case HIDP_TRANS_HID_CONTROL: + hidp_process_hid_control(session, param); + break; + + case HIDP_TRANS_DATA: + free_skb = hidp_process_data(session, skb, param); + break; + + default: + __hidp_send_ctrl_message(session, + HIDP_TRANS_HANDSHAKE | HIDP_HSHK_ERR_UNSUPPORTED_REQUEST, NULL, 0); + break; + } + + if (free_skb) + kfree_skb(skb); +} + +static void hidp_recv_intr_frame(struct hidp_session *session, + struct sk_buff *skb) +{ + unsigned char hdr; + + BT_DBG("session %p skb %p len %d", session, skb, skb->len); + + hdr = skb->data[0]; + skb_pull(skb, 1); + + if (hdr == (HIDP_TRANS_DATA | HIDP_DATA_RTYPE_INPUT)) { + hidp_set_timer(session); + + if (session->input) + hidp_input_report(session, skb); + + if (session->hid) { + hid_input_report(session->hid, HID_INPUT_REPORT, skb->data, skb->len, 1); + BT_DBG("report len %d", skb->len); + } + } else { + BT_DBG("Unsupported protocol header 0x%02x", hdr); + } + + kfree_skb(skb); +} + +static int hidp_send_frame(struct socket *sock, unsigned char *data, int len) +{ + struct kvec iv = { data, len }; + struct msghdr msg; + + BT_DBG("sock %p data %p len %d", sock, data, len); + + if (!len) + return 0; + + memset(&msg, 0, sizeof(msg)); + + return kernel_sendmsg(sock, &msg, &iv, 1, len); +} + +static void hidp_process_intr_transmit(struct hidp_session *session) +{ + struct sk_buff *skb; + + BT_DBG("session %p", session); + + while ((skb = skb_dequeue(&session->intr_transmit))) { + if (hidp_send_frame(session->intr_sock, skb->data, skb->len) < 0) { + skb_queue_head(&session->intr_transmit, skb); + break; + } + + hidp_set_timer(session); + kfree_skb(skb); + } +} + +static void hidp_process_ctrl_transmit(struct hidp_session *session) +{ + struct sk_buff *skb; + + BT_DBG("session %p", session); + + while ((skb = skb_dequeue(&session->ctrl_transmit))) { + if (hidp_send_frame(session->ctrl_sock, skb->data, skb->len) < 0) { + skb_queue_head(&session->ctrl_transmit, skb); + break; + } + + hidp_set_timer(session); + kfree_skb(skb); + } +} + +static int hidp_session(void *arg) +{ + struct hidp_session *session = arg; + struct sock *ctrl_sk = session->ctrl_sock->sk; + struct sock *intr_sk = session->intr_sock->sk; + struct sk_buff *skb; + wait_queue_t ctrl_wait, intr_wait; + + BT_DBG("session %p", session); + + __module_get(THIS_MODULE); + set_user_nice(current, -15); + + init_waitqueue_entry(&ctrl_wait, current); + init_waitqueue_entry(&intr_wait, current); + add_wait_queue(sk_sleep(ctrl_sk), &ctrl_wait); + add_wait_queue(sk_sleep(intr_sk), &intr_wait); + session->waiting_for_startup = 0; + wake_up_interruptible(&session->startup_queue); + set_current_state(TASK_INTERRUPTIBLE); + while (!atomic_read(&session->terminate)) { + if (ctrl_sk->sk_state != BT_CONNECTED || + intr_sk->sk_state != BT_CONNECTED) + break; + + while ((skb = skb_dequeue(&intr_sk->sk_receive_queue))) { + skb_orphan(skb); + if (!skb_linearize(skb)) + hidp_recv_intr_frame(session, skb); + else + kfree_skb(skb); + } + + hidp_process_intr_transmit(session); + + while ((skb = skb_dequeue(&ctrl_sk->sk_receive_queue))) { + skb_orphan(skb); + if (!skb_linearize(skb)) + hidp_recv_ctrl_frame(session, skb); + else + kfree_skb(skb); + } + + hidp_process_ctrl_transmit(session); + + schedule(); + set_current_state(TASK_INTERRUPTIBLE); + } + set_current_state(TASK_RUNNING); + remove_wait_queue(sk_sleep(intr_sk), &intr_wait); + remove_wait_queue(sk_sleep(ctrl_sk), &ctrl_wait); + + clear_bit(HIDP_WAITING_FOR_SEND_ACK, &session->flags); + clear_bit(HIDP_WAITING_FOR_RETURN, &session->flags); + wake_up_interruptible(&session->report_queue); + + down_write(&hidp_session_sem); + + hidp_del_timer(session); + + if (session->input) { + input_unregister_device(session->input); + session->input = NULL; + } + + if (session->hid) { + hid_destroy_device(session->hid); + session->hid = NULL; + } + + /* Wakeup user-space polling for socket errors */ + session->intr_sock->sk->sk_err = EUNATCH; + session->ctrl_sock->sk->sk_err = EUNATCH; + + hidp_schedule(session); + + fput(session->intr_sock->file); + + wait_event_timeout(*(sk_sleep(ctrl_sk)), + (ctrl_sk->sk_state == BT_CLOSED), msecs_to_jiffies(500)); + + fput(session->ctrl_sock->file); + + __hidp_unlink_session(session); + + up_write(&hidp_session_sem); + + kfree(session->rd_data); + kfree(session); + module_put_and_exit(0); + return 0; +} + +static struct hci_conn *hidp_get_connection(struct hidp_session *session) +{ + bdaddr_t *src = &bt_sk(session->ctrl_sock->sk)->src; + bdaddr_t *dst = &bt_sk(session->ctrl_sock->sk)->dst; + struct hci_conn *conn; + struct hci_dev *hdev; + + hdev = hci_get_route(dst, src); + if (!hdev) + return NULL; + + hci_dev_lock(hdev); + conn = hci_conn_hash_lookup_ba(hdev, ACL_LINK, dst); + if (conn) + hci_conn_hold_device(conn); + hci_dev_unlock(hdev); + + hci_dev_put(hdev); + + return conn; +} + +static int hidp_setup_input(struct hidp_session *session, + struct hidp_connadd_req *req) +{ + struct input_dev *input; + int i; + + input = input_allocate_device(); + if (!input) + return -ENOMEM; + + session->input = input; + + input_set_drvdata(input, session); + + input->name = "Bluetooth HID Boot Protocol Device"; + + input->id.bustype = BUS_BLUETOOTH; + input->id.vendor = req->vendor; + input->id.product = req->product; + input->id.version = req->version; + + if (req->subclass & 0x40) { + set_bit(EV_KEY, input->evbit); + set_bit(EV_LED, input->evbit); + set_bit(EV_REP, input->evbit); + + set_bit(LED_NUML, input->ledbit); + set_bit(LED_CAPSL, input->ledbit); + set_bit(LED_SCROLLL, input->ledbit); + set_bit(LED_COMPOSE, input->ledbit); + set_bit(LED_KANA, input->ledbit); + + for (i = 0; i < sizeof(hidp_keycode); i++) + set_bit(hidp_keycode[i], input->keybit); + clear_bit(0, input->keybit); + } + + if (req->subclass & 0x80) { + input->evbit[0] = BIT_MASK(EV_KEY) | BIT_MASK(EV_REL); + input->keybit[BIT_WORD(BTN_MOUSE)] = BIT_MASK(BTN_LEFT) | + BIT_MASK(BTN_RIGHT) | BIT_MASK(BTN_MIDDLE); + input->relbit[0] = BIT_MASK(REL_X) | BIT_MASK(REL_Y); + input->keybit[BIT_WORD(BTN_MOUSE)] |= BIT_MASK(BTN_SIDE) | + BIT_MASK(BTN_EXTRA); + input->relbit[0] |= BIT_MASK(REL_WHEEL); + } + + input->dev.parent = &session->conn->dev; + + input->event = hidp_input_event; + + return 0; +} + +static int hidp_open(struct hid_device *hid) +{ + return 0; +} + +static void hidp_close(struct hid_device *hid) +{ +} + +static int hidp_parse(struct hid_device *hid) +{ + struct hidp_session *session = hid->driver_data; + + return hid_parse_report(session->hid, session->rd_data, + session->rd_size); +} + +static int hidp_start(struct hid_device *hid) +{ + struct hidp_session *session = hid->driver_data; + struct hid_report *report; + + if (hid->quirks & HID_QUIRK_NO_INIT_REPORTS) + return 0; + + list_for_each_entry(report, &hid->report_enum[HID_INPUT_REPORT]. + report_list, list) + hidp_send_report(session, report); + + list_for_each_entry(report, &hid->report_enum[HID_FEATURE_REPORT]. + report_list, list) + hidp_send_report(session, report); + + return 0; +} + +static void hidp_stop(struct hid_device *hid) +{ + struct hidp_session *session = hid->driver_data; + + skb_queue_purge(&session->ctrl_transmit); + skb_queue_purge(&session->intr_transmit); + + hid->claimed = 0; +} + +static struct hid_ll_driver hidp_hid_driver = { + .parse = hidp_parse, + .start = hidp_start, + .stop = hidp_stop, + .open = hidp_open, + .close = hidp_close, + .hidinput_input_event = hidp_hidinput_event, +}; + +/* This function sets up the hid device. It does not add it + to the HID system. That is done in hidp_add_connection(). */ +static int hidp_setup_hid(struct hidp_session *session, + struct hidp_connadd_req *req) +{ + struct hid_device *hid; + int err; + + session->rd_data = kzalloc(req->rd_size, GFP_KERNEL); + if (!session->rd_data) + return -ENOMEM; + + if (copy_from_user(session->rd_data, req->rd_data, req->rd_size)) { + err = -EFAULT; + goto fault; + } + session->rd_size = req->rd_size; + + hid = hid_allocate_device(); + if (IS_ERR(hid)) { + err = PTR_ERR(hid); + goto fault; + } + + session->hid = hid; + + hid->driver_data = session; + + hid->bus = BUS_BLUETOOTH; + hid->vendor = req->vendor; + hid->product = req->product; + hid->version = req->version; + hid->country = req->country; + + strncpy(hid->name, req->name, 128); + strncpy(hid->phys, batostr(&bt_sk(session->ctrl_sock->sk)->src), 64); + strncpy(hid->uniq, batostr(&bt_sk(session->ctrl_sock->sk)->dst), 64); + + hid->dev.parent = &session->conn->dev; + hid->ll_driver = &hidp_hid_driver; + + hid->hid_get_raw_report = hidp_get_raw_report; + hid->hid_output_raw_report = hidp_output_raw_report; + + return 0; + +fault: + kfree(session->rd_data); + session->rd_data = NULL; + + return err; +} + +int hidp_add_connection(struct hidp_connadd_req *req, struct socket *ctrl_sock, struct socket *intr_sock) +{ + struct hidp_session *session, *s; + int vendor, product; + int err; + + BT_DBG(""); + + if (bacmp(&bt_sk(ctrl_sock->sk)->src, &bt_sk(intr_sock->sk)->src) || + bacmp(&bt_sk(ctrl_sock->sk)->dst, &bt_sk(intr_sock->sk)->dst)) + return -ENOTUNIQ; + + BT_DBG("rd_data %p rd_size %d", req->rd_data, req->rd_size); + + down_write(&hidp_session_sem); + + s = __hidp_get_session(&bt_sk(ctrl_sock->sk)->dst); + if (s && s->state == BT_CONNECTED) { + up_write(&hidp_session_sem); + return -EEXIST; + } + + session = kzalloc(sizeof(struct hidp_session), GFP_KERNEL); + if (!session) { + up_write(&hidp_session_sem); + return -ENOMEM; + } + + bacpy(&session->bdaddr, &bt_sk(ctrl_sock->sk)->dst); + + session->ctrl_mtu = min_t(uint, l2cap_pi(ctrl_sock->sk)->chan->omtu, + l2cap_pi(ctrl_sock->sk)->chan->imtu); + session->intr_mtu = min_t(uint, l2cap_pi(intr_sock->sk)->chan->omtu, + l2cap_pi(intr_sock->sk)->chan->imtu); + + BT_DBG("ctrl mtu %d intr mtu %d", session->ctrl_mtu, session->intr_mtu); + + session->ctrl_sock = ctrl_sock; + session->intr_sock = intr_sock; + session->state = BT_CONNECTED; + + session->conn = hidp_get_connection(session); + if (!session->conn) { + err = -ENOTCONN; + goto failed; + } + + setup_timer(&session->timer, hidp_idle_timeout, (unsigned long)session); + + skb_queue_head_init(&session->ctrl_transmit); + skb_queue_head_init(&session->intr_transmit); + + mutex_init(&session->report_mutex); + init_waitqueue_head(&session->report_queue); + init_waitqueue_head(&session->startup_queue); + session->waiting_for_startup = 1; + session->flags = req->flags & (1 << HIDP_BLUETOOTH_VENDOR_ID); + session->idle_to = req->idle_to; + + __hidp_link_session(session); + + if (req->rd_size > 0) { + err = hidp_setup_hid(session, req); + if (err) + goto purge; + } + + if (!session->hid) { + err = hidp_setup_input(session, req); + if (err < 0) + goto purge; + } + + hidp_set_timer(session); + + if (session->hid) { + vendor = session->hid->vendor; + product = session->hid->product; + } else if (session->input) { + vendor = session->input->id.vendor; + product = session->input->id.product; + } else { + vendor = 0x0000; + product = 0x0000; + } + + session->task = kthread_run(hidp_session, session, "khidpd_%04x%04x", + vendor, product); + if (IS_ERR(session->task)) { + err = PTR_ERR(session->task); + goto unlink; + } + + while (session->waiting_for_startup) { + wait_event_interruptible(session->startup_queue, + !session->waiting_for_startup); + } + + if (session->hid) + err = hid_add_device(session->hid); + else + err = input_register_device(session->input); + + if (err < 0) { + atomic_inc(&session->terminate); + wake_up_process(session->task); + up_write(&hidp_session_sem); + return err; + } + + if (session->input) { + hidp_send_ctrl_message(session, + HIDP_TRANS_SET_PROTOCOL | HIDP_PROTO_BOOT, NULL, 0); + session->flags |= (1 << HIDP_BOOT_PROTOCOL_MODE); + + session->leds = 0xff; + hidp_input_event(session->input, EV_LED, 0, 0); + } + + up_write(&hidp_session_sem); + return 0; + +unlink: + hidp_del_timer(session); + + if (session->input) { + input_unregister_device(session->input); + session->input = NULL; + } + + if (session->hid) { + hid_destroy_device(session->hid); + session->hid = NULL; + } + + kfree(session->rd_data); + session->rd_data = NULL; + +purge: + __hidp_unlink_session(session); + + skb_queue_purge(&session->ctrl_transmit); + skb_queue_purge(&session->intr_transmit); + +failed: + up_write(&hidp_session_sem); + + kfree(session); + return err; +} + +int hidp_del_connection(struct hidp_conndel_req *req) +{ + struct hidp_session *session; + int err = 0; + + BT_DBG(""); + + down_read(&hidp_session_sem); + + session = __hidp_get_session(&req->bdaddr); + if (session) { + if (req->flags & (1 << HIDP_VIRTUAL_CABLE_UNPLUG)) { + hidp_send_ctrl_message(session, + HIDP_TRANS_HID_CONTROL | HIDP_CTRL_VIRTUAL_CABLE_UNPLUG, NULL, 0); + } else { + /* Flush the transmit queues */ + skb_queue_purge(&session->ctrl_transmit); + skb_queue_purge(&session->intr_transmit); + + atomic_inc(&session->terminate); + wake_up_process(session->task); + } + } else + err = -ENOENT; + + up_read(&hidp_session_sem); + return err; +} + +int hidp_get_connlist(struct hidp_connlist_req *req) +{ + struct hidp_session *session; + int err = 0, n = 0; + + BT_DBG(""); + + down_read(&hidp_session_sem); + + list_for_each_entry(session, &hidp_session_list, list) { + struct hidp_conninfo ci; + + __hidp_copy_session(session, &ci); + + if (copy_to_user(req->ci, &ci, sizeof(ci))) { + err = -EFAULT; + break; + } + + if (++n >= req->cnum) + break; + + req->ci++; + } + req->cnum = n; + + up_read(&hidp_session_sem); + return err; +} + +int hidp_get_conninfo(struct hidp_conninfo *ci) +{ + struct hidp_session *session; + int err = 0; + + down_read(&hidp_session_sem); + + session = __hidp_get_session(&ci->bdaddr); + if (session) + __hidp_copy_session(session, ci); + else + err = -ENOENT; + + up_read(&hidp_session_sem); + return err; +} + +static const struct hid_device_id hidp_table[] = { + { HID_BLUETOOTH_DEVICE(HID_ANY_ID, HID_ANY_ID) }, + { } +}; + +static struct hid_driver hidp_driver = { + .name = "generic-bluetooth", + .id_table = hidp_table, +}; + +static int __init hidp_init(void) +{ + int ret; + + BT_INFO("HIDP (Human Interface Emulation) ver %s", VERSION); + + ret = hid_register_driver(&hidp_driver); + if (ret) + goto err; + + ret = hidp_init_sockets(); + if (ret) + goto err_drv; + + return 0; +err_drv: + hid_unregister_driver(&hidp_driver); +err: + return ret; +} + +static void __exit hidp_exit(void) +{ + hidp_cleanup_sockets(); + hid_unregister_driver(&hidp_driver); +} + +module_init(hidp_init); +module_exit(hidp_exit); + +MODULE_AUTHOR("Marcel Holtmann <marcel@holtmann.org>"); +MODULE_DESCRIPTION("Bluetooth HIDP ver " VERSION); +MODULE_VERSION(VERSION); +MODULE_LICENSE("GPL"); +MODULE_ALIAS("bt-proto-6"); diff --git a/net/bluetooth_tizen/hidp/hidp.h b/net/bluetooth_tizen/hidp/hidp.h new file mode 100644 index 0000000..af1bcc8 --- /dev/null +++ b/net/bluetooth_tizen/hidp/hidp.h @@ -0,0 +1,191 @@ +/* + HIDP implementation for Linux Bluetooth stack (BlueZ). + Copyright (C) 2003-2004 Marcel Holtmann <marcel@holtmann.org> + + This program is free software; you can redistribute it and/or modify + it under the terms of the GNU General Public License version 2 as + published by the Free Software Foundation; + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS + OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT OF THIRD PARTY RIGHTS. + IN NO EVENT SHALL THE COPYRIGHT HOLDER(S) AND AUTHOR(S) BE LIABLE FOR ANY + CLAIM, OR ANY SPECIAL INDIRECT OR CONSEQUENTIAL DAMAGES, OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + + ALL LIABILITY, INCLUDING LIABILITY FOR INFRINGEMENT OF ANY PATENTS, + COPYRIGHTS, TRADEMARKS OR OTHER RIGHTS, RELATING TO USE OF THIS + SOFTWARE IS DISCLAIMED. +*/ + +#ifndef __HIDP_H +#define __HIDP_H + +#include <linux/types.h> +#include <net/bluetooth/bluetooth.h> + +/* HIDP header masks */ +#define HIDP_HEADER_TRANS_MASK 0xf0 +#define HIDP_HEADER_PARAM_MASK 0x0f + +/* HIDP transaction types */ +#define HIDP_TRANS_HANDSHAKE 0x00 +#define HIDP_TRANS_HID_CONTROL 0x10 +#define HIDP_TRANS_GET_REPORT 0x40 +#define HIDP_TRANS_SET_REPORT 0x50 +#define HIDP_TRANS_GET_PROTOCOL 0x60 +#define HIDP_TRANS_SET_PROTOCOL 0x70 +#define HIDP_TRANS_GET_IDLE 0x80 +#define HIDP_TRANS_SET_IDLE 0x90 +#define HIDP_TRANS_DATA 0xa0 +#define HIDP_TRANS_DATC 0xb0 + +/* HIDP handshake results */ +#define HIDP_HSHK_SUCCESSFUL 0x00 +#define HIDP_HSHK_NOT_READY 0x01 +#define HIDP_HSHK_ERR_INVALID_REPORT_ID 0x02 +#define HIDP_HSHK_ERR_UNSUPPORTED_REQUEST 0x03 +#define HIDP_HSHK_ERR_INVALID_PARAMETER 0x04 +#define HIDP_HSHK_ERR_UNKNOWN 0x0e +#define HIDP_HSHK_ERR_FATAL 0x0f + +/* HIDP control operation parameters */ +#define HIDP_CTRL_NOP 0x00 +#define HIDP_CTRL_HARD_RESET 0x01 +#define HIDP_CTRL_SOFT_RESET 0x02 +#define HIDP_CTRL_SUSPEND 0x03 +#define HIDP_CTRL_EXIT_SUSPEND 0x04 +#define HIDP_CTRL_VIRTUAL_CABLE_UNPLUG 0x05 + +/* HIDP data transaction headers */ +#define HIDP_DATA_RTYPE_MASK 0x03 +#define HIDP_DATA_RSRVD_MASK 0x0c +#define HIDP_DATA_RTYPE_OTHER 0x00 +#define HIDP_DATA_RTYPE_INPUT 0x01 +#define HIDP_DATA_RTYPE_OUPUT 0x02 +#define HIDP_DATA_RTYPE_FEATURE 0x03 + +/* HIDP protocol header parameters */ +#define HIDP_PROTO_BOOT 0x00 +#define HIDP_PROTO_REPORT 0x01 + +/* HIDP ioctl defines */ +#define HIDPCONNADD _IOW('H', 200, int) +#define HIDPCONNDEL _IOW('H', 201, int) +#define HIDPGETCONNLIST _IOR('H', 210, int) +#define HIDPGETCONNINFO _IOR('H', 211, int) + +#define HIDP_VIRTUAL_CABLE_UNPLUG 0 +#define HIDP_BOOT_PROTOCOL_MODE 1 +#define HIDP_BLUETOOTH_VENDOR_ID 9 +#define HIDP_WAITING_FOR_RETURN 10 +#define HIDP_WAITING_FOR_SEND_ACK 11 + +struct hidp_connadd_req { + int ctrl_sock; /* Connected control socket */ + int intr_sock; /* Connected interrupt socket */ + __u16 parser; + __u16 rd_size; + __u8 __user *rd_data; + __u8 country; + __u8 subclass; + __u16 vendor; + __u16 product; + __u16 version; + __u32 flags; + __u32 idle_to; + char name[128]; +}; + +struct hidp_conndel_req { + bdaddr_t bdaddr; + __u32 flags; +}; + +struct hidp_conninfo { + bdaddr_t bdaddr; + __u32 flags; + __u16 state; + __u16 vendor; + __u16 product; + __u16 version; + char name[128]; +}; + +struct hidp_connlist_req { + __u32 cnum; + struct hidp_conninfo __user *ci; +}; + +int hidp_add_connection(struct hidp_connadd_req *req, struct socket *ctrl_sock, struct socket *intr_sock); +int hidp_del_connection(struct hidp_conndel_req *req); +int hidp_get_connlist(struct hidp_connlist_req *req); +int hidp_get_conninfo(struct hidp_conninfo *ci); + +/* HIDP session defines */ +struct hidp_session { + struct list_head list; + + struct hci_conn *conn; + + struct socket *ctrl_sock; + struct socket *intr_sock; + + bdaddr_t bdaddr; + + unsigned long state; + unsigned long flags; + unsigned long idle_to; + + uint ctrl_mtu; + uint intr_mtu; + + atomic_t terminate; + struct task_struct *task; + + unsigned char keys[8]; + unsigned char leds; + + struct input_dev *input; + + struct hid_device *hid; + + struct timer_list timer; + + struct sk_buff_head ctrl_transmit; + struct sk_buff_head intr_transmit; + + /* Used in hidp_get_raw_report() */ + int waiting_report_type; /* HIDP_DATA_RTYPE_* */ + int waiting_report_number; /* -1 for not numbered */ + struct mutex report_mutex; + struct sk_buff *report_return; + wait_queue_head_t report_queue; + + /* Used in hidp_output_raw_report() */ + int output_report_success; /* boolean */ + + /* Report descriptor */ + __u8 *rd_data; + uint rd_size; + + wait_queue_head_t startup_queue; + int waiting_for_startup; +}; + +static inline void hidp_schedule(struct hidp_session *session) +{ + struct sock *ctrl_sk = session->ctrl_sock->sk; + struct sock *intr_sk = session->intr_sock->sk; + + wake_up_interruptible(sk_sleep(ctrl_sk)); + wake_up_interruptible(sk_sleep(intr_sk)); +} + +/* HIDP init defines */ +extern int __init hidp_init_sockets(void); +extern void __exit hidp_cleanup_sockets(void); + +#endif /* __HIDP_H */ diff --git a/net/bluetooth_tizen/hidp/sock.c b/net/bluetooth_tizen/hidp/sock.c new file mode 100644 index 0000000..73a32d7 --- /dev/null +++ b/net/bluetooth_tizen/hidp/sock.c @@ -0,0 +1,305 @@ +/* + HIDP implementation for Linux Bluetooth stack (BlueZ). + Copyright (C) 2003-2004 Marcel Holtmann <marcel@holtmann.org> + + This program is free software; you can redistribute it and/or modify + it under the terms of the GNU General Public License version 2 as + published by the Free Software Foundation; + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS + OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT OF THIRD PARTY RIGHTS. + IN NO EVENT SHALL THE COPYRIGHT HOLDER(S) AND AUTHOR(S) BE LIABLE FOR ANY + CLAIM, OR ANY SPECIAL INDIRECT OR CONSEQUENTIAL DAMAGES, OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + + ALL LIABILITY, INCLUDING LIABILITY FOR INFRINGEMENT OF ANY PATENTS, + COPYRIGHTS, TRADEMARKS OR OTHER RIGHTS, RELATING TO USE OF THIS + SOFTWARE IS DISCLAIMED. +*/ + +#include <linux/module.h> + +#include <linux/types.h> +#include <linux/capability.h> +#include <linux/errno.h> +#include <linux/kernel.h> +#include <linux/poll.h> +#include <linux/fcntl.h> +#include <linux/skbuff.h> +#include <linux/socket.h> +#include <linux/ioctl.h> +#include <linux/file.h> +#include <linux/init.h> +#include <linux/compat.h> +#include <linux/gfp.h> +#include <net/sock.h> + +#include "hidp.h" + +static int hidp_sock_release(struct socket *sock) +{ + struct sock *sk = sock->sk; + + BT_DBG("sock %p sk %p", sock, sk); + + if (!sk) + return 0; + + sock_orphan(sk); + sock_put(sk); + + return 0; +} + +static int hidp_sock_ioctl(struct socket *sock, unsigned int cmd, unsigned long arg) +{ + void __user *argp = (void __user *) arg; + struct hidp_connadd_req ca; + struct hidp_conndel_req cd; + struct hidp_connlist_req cl; + struct hidp_conninfo ci; + struct socket *csock; + struct socket *isock; + int err; + + BT_DBG("cmd %x arg %lx", cmd, arg); + + switch (cmd) { + case HIDPCONNADD: + if (!capable(CAP_NET_ADMIN)) + return -EACCES; + + if (copy_from_user(&ca, argp, sizeof(ca))) + return -EFAULT; + + csock = sockfd_lookup(ca.ctrl_sock, &err); + if (!csock) + return err; + + isock = sockfd_lookup(ca.intr_sock, &err); + if (!isock) { + sockfd_put(csock); + return err; + } + + if (csock->sk->sk_state != BT_CONNECTED || + isock->sk->sk_state != BT_CONNECTED) { + sockfd_put(csock); + sockfd_put(isock); + return -EBADFD; + } + + err = hidp_add_connection(&ca, csock, isock); + if (!err) { + if (copy_to_user(argp, &ca, sizeof(ca))) + err = -EFAULT; + } else { + sockfd_put(csock); + sockfd_put(isock); + } + + return err; + + case HIDPCONNDEL: + if (!capable(CAP_NET_ADMIN)) + return -EACCES; + + if (copy_from_user(&cd, argp, sizeof(cd))) + return -EFAULT; + + return hidp_del_connection(&cd); + + case HIDPGETCONNLIST: + if (copy_from_user(&cl, argp, sizeof(cl))) + return -EFAULT; + + if (cl.cnum <= 0) + return -EINVAL; + + err = hidp_get_connlist(&cl); + if (!err && copy_to_user(argp, &cl, sizeof(cl))) + return -EFAULT; + + return err; + + case HIDPGETCONNINFO: + if (copy_from_user(&ci, argp, sizeof(ci))) + return -EFAULT; + + err = hidp_get_conninfo(&ci); + if (!err && copy_to_user(argp, &ci, sizeof(ci))) + return -EFAULT; + + return err; + } + + return -EINVAL; +} + +#ifdef CONFIG_COMPAT +struct compat_hidp_connadd_req { + int ctrl_sock; /* Connected control socket */ + int intr_sock; /* Connected interrupt socket */ + __u16 parser; + __u16 rd_size; + compat_uptr_t rd_data; + __u8 country; + __u8 subclass; + __u16 vendor; + __u16 product; + __u16 version; + __u32 flags; + __u32 idle_to; + char name[128]; +}; + +static int hidp_sock_compat_ioctl(struct socket *sock, unsigned int cmd, unsigned long arg) +{ + if (cmd == HIDPGETCONNLIST) { + struct hidp_connlist_req cl; + u32 uci; + int err; + + if (get_user(cl.cnum, (u32 __user *) arg) || + get_user(uci, (u32 __user *) (arg + 4))) + return -EFAULT; + + cl.ci = compat_ptr(uci); + + if (cl.cnum <= 0) + return -EINVAL; + + err = hidp_get_connlist(&cl); + + if (!err && put_user(cl.cnum, (u32 __user *) arg)) + err = -EFAULT; + + return err; + } else if (cmd == HIDPCONNADD) { + struct compat_hidp_connadd_req ca; + struct hidp_connadd_req __user *uca; + + uca = compat_alloc_user_space(sizeof(*uca)); + + if (copy_from_user(&ca, (void __user *) arg, sizeof(ca))) + return -EFAULT; + + if (put_user(ca.ctrl_sock, &uca->ctrl_sock) || + put_user(ca.intr_sock, &uca->intr_sock) || + put_user(ca.parser, &uca->parser) || + put_user(ca.rd_size, &uca->rd_size) || + put_user(compat_ptr(ca.rd_data), &uca->rd_data) || + put_user(ca.country, &uca->country) || + put_user(ca.subclass, &uca->subclass) || + put_user(ca.vendor, &uca->vendor) || + put_user(ca.product, &uca->product) || + put_user(ca.version, &uca->version) || + put_user(ca.flags, &uca->flags) || + put_user(ca.idle_to, &uca->idle_to) || + copy_to_user(&uca->name[0], &ca.name[0], 128)) + return -EFAULT; + + arg = (unsigned long) uca; + + /* Fall through. We don't actually write back any _changes_ + to the structure anyway, so there's no need to copy back + into the original compat version */ + } + + return hidp_sock_ioctl(sock, cmd, arg); +} +#endif + +static const struct proto_ops hidp_sock_ops = { + .family = PF_BLUETOOTH, + .owner = THIS_MODULE, + .release = hidp_sock_release, + .ioctl = hidp_sock_ioctl, +#ifdef CONFIG_COMPAT + .compat_ioctl = hidp_sock_compat_ioctl, +#endif + .bind = sock_no_bind, + .getname = sock_no_getname, + .sendmsg = sock_no_sendmsg, + .recvmsg = sock_no_recvmsg, + .poll = sock_no_poll, + .listen = sock_no_listen, + .shutdown = sock_no_shutdown, + .setsockopt = sock_no_setsockopt, + .getsockopt = sock_no_getsockopt, + .connect = sock_no_connect, + .socketpair = sock_no_socketpair, + .accept = sock_no_accept, + .mmap = sock_no_mmap +}; + +static struct proto hidp_proto = { + .name = "HIDP", + .owner = THIS_MODULE, + .obj_size = sizeof(struct bt_sock) +}; + +static int hidp_sock_create(struct net *net, struct socket *sock, int protocol, + int kern) +{ + struct sock *sk; + + BT_DBG("sock %p", sock); + + if (sock->type != SOCK_RAW) + return -ESOCKTNOSUPPORT; + + sk = sk_alloc(net, PF_BLUETOOTH, GFP_ATOMIC, &hidp_proto); + if (!sk) + return -ENOMEM; + + sock_init_data(sock, sk); + + sock->ops = &hidp_sock_ops; + + sock->state = SS_UNCONNECTED; + + sock_reset_flag(sk, SOCK_ZAPPED); + + sk->sk_protocol = protocol; + sk->sk_state = BT_OPEN; + + return 0; +} + +static const struct net_proto_family hidp_sock_family_ops = { + .family = PF_BLUETOOTH, + .owner = THIS_MODULE, + .create = hidp_sock_create +}; + +int __init hidp_init_sockets(void) +{ + int err; + + err = proto_register(&hidp_proto, 0); + if (err < 0) + return err; + + err = bt_sock_register(BTPROTO_HIDP, &hidp_sock_family_ops); + if (err < 0) + goto error; + + return 0; + +error: + BT_ERR("Can't register HIDP socket"); + proto_unregister(&hidp_proto); + return err; +} + +void __exit hidp_cleanup_sockets(void) +{ + if (bt_sock_unregister(BTPROTO_HIDP) < 0) + BT_ERR("Can't unregister HIDP socket"); + + proto_unregister(&hidp_proto); +} diff --git a/net/bluetooth_tizen/l2cap_core.c b/net/bluetooth_tizen/l2cap_core.c new file mode 100644 index 0000000..48268ab --- /dev/null +++ b/net/bluetooth_tizen/l2cap_core.c @@ -0,0 +1,4803 @@ +/* + BlueZ - Bluetooth protocol stack for Linux + Copyright (C) 2000-2001 Qualcomm Incorporated + Copyright (C) 2009-2010 Gustavo F. Padovan <gustavo@padovan.org> + Copyright (C) 2010 Google Inc. + Copyright (C) 2011 ProFUSION Embedded Systems + + Written 2000,2001 by Maxim Krasnyansky <maxk@qualcomm.com> + + This program is free software; you can redistribute it and/or modify + it under the terms of the GNU General Public License version 2 as + published by the Free Software Foundation; + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS + OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT OF THIRD PARTY RIGHTS. + IN NO EVENT SHALL THE COPYRIGHT HOLDER(S) AND AUTHOR(S) BE LIABLE FOR ANY + CLAIM, OR ANY SPECIAL INDIRECT OR CONSEQUENTIAL DAMAGES, OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + + ALL LIABILITY, INCLUDING LIABILITY FOR INFRINGEMENT OF ANY PATENTS, + COPYRIGHTS, TRADEMARKS OR OTHER RIGHTS, RELATING TO USE OF THIS + SOFTWARE IS DISCLAIMED. +*/ + +/* Bluetooth L2CAP core. */ + +#include <linux/module.h> + +#include <linux/types.h> +#include <linux/capability.h> +#include <linux/errno.h> +#include <linux/kernel.h> +#include <linux/sched.h> +#include <linux/slab.h> +#include <linux/poll.h> +#include <linux/fcntl.h> +#include <linux/init.h> +#include <linux/interrupt.h> +#include <linux/socket.h> +#include <linux/skbuff.h> +#include <linux/list.h> +#include <linux/device.h> +#include <linux/debugfs.h> +#include <linux/seq_file.h> +#include <linux/uaccess.h> +#include <linux/crc16.h> +#include <net/sock.h> + +#include <asm/system.h> +#include <asm/unaligned.h> + +#include <net/bluetooth/bluetooth.h> +#include <net/bluetooth/hci_core.h> +#include <net/bluetooth/l2cap.h> +#include <net/bluetooth/smp.h> + +bool disable_ertm; + +static u32 l2cap_feat_mask = L2CAP_FEAT_FIXED_CHAN; +static u8 l2cap_fixed_chan[8] = { L2CAP_FC_L2CAP, }; + +static LIST_HEAD(chan_list); +static DEFINE_RWLOCK(chan_list_lock); + +static struct sk_buff *l2cap_build_cmd(struct l2cap_conn *conn, + u8 code, u8 ident, u16 dlen, void *data); +static void l2cap_send_cmd(struct l2cap_conn *conn, u8 ident, u8 code, u16 len, + void *data); +static int l2cap_build_conf_req(struct l2cap_chan *chan, void *data); +static void l2cap_send_disconn_req(struct l2cap_conn *conn, + struct l2cap_chan *chan, int err); + +/* ---- L2CAP channels ---- */ + +static struct l2cap_chan *__l2cap_get_chan_by_dcid(struct l2cap_conn *conn, u16 cid) +{ + struct l2cap_chan *c; + + list_for_each_entry(c, &conn->chan_l, list) { + if (c->dcid == cid) + return c; + } + return NULL; +} + +static struct l2cap_chan *__l2cap_get_chan_by_scid(struct l2cap_conn *conn, u16 cid) +{ + struct l2cap_chan *c; + + list_for_each_entry(c, &conn->chan_l, list) { + if (c->scid == cid) + return c; + } + return NULL; +} + +/* Find channel with given SCID. + * Returns locked channel. */ +static struct l2cap_chan *l2cap_get_chan_by_scid(struct l2cap_conn *conn, u16 cid) +{ + struct l2cap_chan *c; + + mutex_lock(&conn->chan_lock); + c = __l2cap_get_chan_by_scid(conn, cid); + if (c) + l2cap_chan_lock(c); + mutex_unlock(&conn->chan_lock); + + return c; +} + +static struct l2cap_chan *__l2cap_get_chan_by_ident(struct l2cap_conn *conn, u8 ident) +{ + struct l2cap_chan *c; + + list_for_each_entry(c, &conn->chan_l, list) { + if (c->ident == ident) + return c; + } + return NULL; +} + +static inline struct l2cap_chan *l2cap_get_chan_by_ident(struct l2cap_conn *conn, u8 ident) +{ + struct l2cap_chan *c; + + mutex_lock(&conn->chan_lock); + c = __l2cap_get_chan_by_ident(conn, ident); + mutex_unlock(&conn->chan_lock); + + return c; +} + +static struct l2cap_chan *__l2cap_global_chan_by_addr(__le16 psm, bdaddr_t *src) +{ + struct l2cap_chan *c; + + list_for_each_entry(c, &chan_list, global_l) { + if (c->sport == psm && !bacmp(&bt_sk(c->sk)->src, src)) + return c; + } + return NULL; +} + +int l2cap_add_psm(struct l2cap_chan *chan, bdaddr_t *src, __le16 psm) +{ + int err; + + write_lock(&chan_list_lock); + + if (psm && __l2cap_global_chan_by_addr(psm, src)) { + err = -EADDRINUSE; + goto done; + } + + if (psm) { + chan->psm = psm; + chan->sport = psm; + err = 0; + } else { + u16 p; + + err = -EINVAL; + for (p = 0x1001; p < 0x1100; p += 2) + if (!__l2cap_global_chan_by_addr(cpu_to_le16(p), src)) { + chan->psm = cpu_to_le16(p); + chan->sport = cpu_to_le16(p); + err = 0; + break; + } + } + +done: + write_unlock(&chan_list_lock); + return err; +} + +int l2cap_add_scid(struct l2cap_chan *chan, __u16 scid) +{ + write_lock(&chan_list_lock); + + chan->scid = scid; + + write_unlock(&chan_list_lock); + + return 0; +} + +static u16 l2cap_alloc_cid(struct l2cap_conn *conn) +{ + u16 cid = L2CAP_CID_DYN_START; + + for (; cid < L2CAP_CID_DYN_END; cid++) { + if (!__l2cap_get_chan_by_scid(conn, cid)) + return cid; + } + + return 0; +} + +static void __l2cap_state_change(struct l2cap_chan *chan, int state) +{ + BT_DBG("chan %p %s -> %s", chan, state_to_string(chan->state), + state_to_string(state)); + + chan->state = state; + chan->ops->state_change(chan->data, state); +} + +static void l2cap_state_change(struct l2cap_chan *chan, int state) +{ + struct sock *sk = chan->sk; + + lock_sock(sk); + __l2cap_state_change(chan, state); + release_sock(sk); +} + +static inline void __l2cap_chan_set_err(struct l2cap_chan *chan, int err) +{ + struct sock *sk = chan->sk; + + sk->sk_err = err; +} + +static inline void l2cap_chan_set_err(struct l2cap_chan *chan, int err) +{ + struct sock *sk = chan->sk; + + lock_sock(sk); + __l2cap_chan_set_err(chan, err); + release_sock(sk); +} + +static void l2cap_chan_timeout(struct work_struct *work) +{ + struct l2cap_chan *chan = container_of(work, struct l2cap_chan, + chan_timer.work); + struct l2cap_conn *conn = chan->conn; + int reason; + + BT_DBG("chan %p state %s", chan, state_to_string(chan->state)); + + mutex_lock(&conn->chan_lock); + l2cap_chan_lock(chan); + + if (chan->state == BT_CONNECTED || chan->state == BT_CONFIG) + reason = ECONNREFUSED; + else if (chan->state == BT_CONNECT && + chan->sec_level != BT_SECURITY_SDP) + reason = ECONNREFUSED; + else + reason = ETIMEDOUT; + + l2cap_chan_close(chan, reason); + + l2cap_chan_unlock(chan); + + chan->ops->close(chan->data); + mutex_unlock(&conn->chan_lock); + + l2cap_chan_put(chan); +} + +struct l2cap_chan *l2cap_chan_create(struct sock *sk) +{ + struct l2cap_chan *chan; + + chan = kzalloc(sizeof(*chan), GFP_ATOMIC); + if (!chan) + return NULL; + + mutex_init(&chan->lock); + + chan->sk = sk; + + write_lock(&chan_list_lock); + list_add(&chan->global_l, &chan_list); + write_unlock(&chan_list_lock); + + INIT_DELAYED_WORK(&chan->chan_timer, l2cap_chan_timeout); + + chan->state = BT_OPEN; + + atomic_set(&chan->refcnt, 1); + + BT_DBG("sk %p chan %p", sk, chan); + + return chan; +} + +void l2cap_chan_destroy(struct l2cap_chan *chan) +{ + write_lock(&chan_list_lock); + list_del(&chan->global_l); + write_unlock(&chan_list_lock); + + l2cap_chan_put(chan); +} + +void __l2cap_chan_add(struct l2cap_conn *conn, struct l2cap_chan *chan) +{ + BT_DBG("conn %p, psm 0x%2.2x, dcid 0x%4.4x", conn, + chan->psm, chan->dcid); + + conn->disc_reason = HCI_ERROR_REMOTE_USER_TERM; + + chan->conn = conn; + + switch (chan->chan_type) { + case L2CAP_CHAN_CONN_ORIENTED: + if (conn->hcon->type == LE_LINK) { + /* LE connection */ + chan->omtu = L2CAP_LE_DEFAULT_MTU; + chan->scid = L2CAP_CID_LE_DATA; + chan->dcid = L2CAP_CID_LE_DATA; + } else { + /* Alloc CID for connection-oriented socket */ + chan->scid = l2cap_alloc_cid(conn); + chan->omtu = L2CAP_DEFAULT_MTU; + } + break; + + case L2CAP_CHAN_CONN_LESS: + /* Connectionless socket */ + chan->scid = L2CAP_CID_CONN_LESS; + chan->dcid = L2CAP_CID_CONN_LESS; + chan->omtu = L2CAP_DEFAULT_MTU; + break; + + default: + /* Raw socket can send/recv signalling messages only */ + chan->scid = L2CAP_CID_SIGNALING; + chan->dcid = L2CAP_CID_SIGNALING; + chan->omtu = L2CAP_DEFAULT_MTU; + } + + chan->local_id = L2CAP_BESTEFFORT_ID; + chan->local_stype = L2CAP_SERV_BESTEFFORT; + chan->local_msdu = L2CAP_DEFAULT_MAX_SDU_SIZE; + chan->local_sdu_itime = L2CAP_DEFAULT_SDU_ITIME; + chan->local_acc_lat = L2CAP_DEFAULT_ACC_LAT; + chan->local_flush_to = L2CAP_DEFAULT_FLUSH_TO; + + l2cap_chan_hold(chan); + + list_add(&chan->list, &conn->chan_l); +} + +void l2cap_chan_add(struct l2cap_conn *conn, struct l2cap_chan *chan) +{ + mutex_lock(&conn->chan_lock); + __l2cap_chan_add(conn, chan); + mutex_unlock(&conn->chan_lock); +} + +static void l2cap_chan_del(struct l2cap_chan *chan, int err) +{ + struct sock *sk = chan->sk; + struct l2cap_conn *conn = chan->conn; + struct sock *parent = bt_sk(sk)->parent; + + __clear_chan_timer(chan); + + BT_DBG("chan %p, conn %p, err %d", chan, conn, err); + + if (conn) { + /* Delete from channel list */ + list_del(&chan->list); + + l2cap_chan_put(chan); + + chan->conn = NULL; + hci_conn_put(conn->hcon); + } + + lock_sock(sk); + + __l2cap_state_change(chan, BT_CLOSED); + sock_set_flag(sk, SOCK_ZAPPED); + + if (err) + __l2cap_chan_set_err(chan, err); + + if (parent) { + bt_accept_unlink(sk); + parent->sk_data_ready(parent, 0); + } else + sk->sk_state_change(sk); + + release_sock(sk); + + if (!(test_bit(CONF_OUTPUT_DONE, &chan->conf_state) && + test_bit(CONF_INPUT_DONE, &chan->conf_state))) + return; + + skb_queue_purge(&chan->tx_q); + + if (chan->mode == L2CAP_MODE_ERTM) { + struct srej_list *l, *tmp; + + __clear_retrans_timer(chan); + __clear_monitor_timer(chan); + __clear_ack_timer(chan); + + skb_queue_purge(&chan->srej_q); + + list_for_each_entry_safe(l, tmp, &chan->srej_l, list) { + list_del(&l->list); + kfree(l); + } + } +} + +static void l2cap_chan_cleanup_listen(struct sock *parent) +{ + struct sock *sk; + + BT_DBG("parent %p", parent); + + /* Close not yet accepted channels */ + while ((sk = bt_accept_dequeue(parent, NULL))) { + struct l2cap_chan *chan = l2cap_pi(sk)->chan; + + l2cap_chan_lock(chan); + __clear_chan_timer(chan); + l2cap_chan_close(chan, ECONNRESET); + l2cap_chan_unlock(chan); + + chan->ops->close(chan->data); + } +} + +void l2cap_chan_close(struct l2cap_chan *chan, int reason) +{ + struct l2cap_conn *conn = chan->conn; + struct sock *sk = chan->sk; + + BT_DBG("chan %p state %s sk %p", chan, + state_to_string(chan->state), sk); + + switch (chan->state) { + case BT_LISTEN: + lock_sock(sk); + l2cap_chan_cleanup_listen(sk); + + __l2cap_state_change(chan, BT_CLOSED); + sock_set_flag(sk, SOCK_ZAPPED); + release_sock(sk); + break; + + case BT_CONNECTED: + case BT_CONFIG: + if (chan->chan_type == L2CAP_CHAN_CONN_ORIENTED && + conn->hcon->type == ACL_LINK) { + __clear_chan_timer(chan); + __set_chan_timer(chan, sk->sk_sndtimeo); + l2cap_send_disconn_req(conn, chan, reason); + } else + l2cap_chan_del(chan, reason); + break; + + case BT_CONNECT2: + if (chan->chan_type == L2CAP_CHAN_CONN_ORIENTED && + conn->hcon->type == ACL_LINK) { + struct l2cap_conn_rsp rsp; + __u16 result; + + if (bt_sk(sk)->defer_setup) + result = L2CAP_CR_SEC_BLOCK; + else + result = L2CAP_CR_BAD_PSM; + l2cap_state_change(chan, BT_DISCONN); + + rsp.scid = cpu_to_le16(chan->dcid); + rsp.dcid = cpu_to_le16(chan->scid); + rsp.result = cpu_to_le16(result); + rsp.status = cpu_to_le16(L2CAP_CS_NO_INFO); + l2cap_send_cmd(conn, chan->ident, L2CAP_CONN_RSP, + sizeof(rsp), &rsp); + } + + l2cap_chan_del(chan, reason); + break; + + case BT_CONNECT: + case BT_DISCONN: + l2cap_chan_del(chan, reason); + break; + + default: + lock_sock(sk); + sock_set_flag(sk, SOCK_ZAPPED); + release_sock(sk); + break; + } +} + +static inline u8 l2cap_get_auth_type(struct l2cap_chan *chan) +{ + if (chan->chan_type == L2CAP_CHAN_RAW) { + switch (chan->sec_level) { + case BT_SECURITY_HIGH: + return HCI_AT_DEDICATED_BONDING_MITM; + case BT_SECURITY_MEDIUM: + return HCI_AT_DEDICATED_BONDING; + default: + return HCI_AT_NO_BONDING; + } + } else if (chan->psm == cpu_to_le16(0x0001)) { + if (chan->sec_level == BT_SECURITY_LOW) + chan->sec_level = BT_SECURITY_SDP; + + if (chan->sec_level == BT_SECURITY_HIGH) + return HCI_AT_NO_BONDING_MITM; + else + return HCI_AT_NO_BONDING; + } else { + switch (chan->sec_level) { + case BT_SECURITY_HIGH: + return HCI_AT_GENERAL_BONDING_MITM; + case BT_SECURITY_MEDIUM: + return HCI_AT_GENERAL_BONDING; + default: + return HCI_AT_NO_BONDING; + } + } +} + +/* Service level security */ +int l2cap_chan_check_security(struct l2cap_chan *chan) +{ + struct l2cap_conn *conn = chan->conn; + __u8 auth_type; + + auth_type = l2cap_get_auth_type(chan); + + return hci_conn_security(conn->hcon, chan->sec_level, auth_type); +} + +static u8 l2cap_get_ident(struct l2cap_conn *conn) +{ + u8 id; + + /* Get next available identificator. + * 1 - 128 are used by kernel. + * 129 - 199 are reserved. + * 200 - 254 are used by utilities like l2ping, etc. + */ + + spin_lock(&conn->lock); + + if (++conn->tx_ident > 128) + conn->tx_ident = 1; + + id = conn->tx_ident; + + spin_unlock(&conn->lock); + + return id; +} + +static void l2cap_send_cmd(struct l2cap_conn *conn, u8 ident, u8 code, u16 len, void *data) +{ + struct sk_buff *skb = l2cap_build_cmd(conn, code, ident, len, data); + u8 flags; + + BT_DBG("code 0x%2.2x", code); + + if (!skb) + return; + + if (lmp_no_flush_capable(conn->hcon->hdev)) + flags = ACL_START_NO_FLUSH; + else + flags = ACL_START; + + bt_cb(skb)->force_active = BT_POWER_FORCE_ACTIVE_ON; + skb->priority = HCI_PRIO_MAX; + + hci_send_acl(conn->hchan, skb, flags); +} + +static void l2cap_do_send(struct l2cap_chan *chan, struct sk_buff *skb) +{ + struct hci_conn *hcon = chan->conn->hcon; + u16 flags; + + BT_DBG("chan %p, skb %p len %d priority %u", chan, skb, skb->len, + skb->priority); + + if (!test_bit(FLAG_FLUSHABLE, &chan->flags) && + lmp_no_flush_capable(hcon->hdev)) + flags = ACL_START_NO_FLUSH; + else + flags = ACL_START; + + bt_cb(skb)->force_active = test_bit(FLAG_FORCE_ACTIVE, &chan->flags); + hci_send_acl(chan->conn->hchan, skb, flags); +} + +static inline void l2cap_send_sframe(struct l2cap_chan *chan, u32 control) +{ + struct sk_buff *skb; + struct l2cap_hdr *lh; + struct l2cap_conn *conn = chan->conn; + int count, hlen; + + if (chan->state != BT_CONNECTED) + return; + + if (test_bit(FLAG_EXT_CTRL, &chan->flags)) + hlen = L2CAP_EXT_HDR_SIZE; + else + hlen = L2CAP_ENH_HDR_SIZE; + + if (chan->fcs == L2CAP_FCS_CRC16) + hlen += L2CAP_FCS_SIZE; + + BT_DBG("chan %p, control 0x%8.8x", chan, control); + + count = min_t(unsigned int, conn->mtu, hlen); + + control |= __set_sframe(chan); + + if (test_and_clear_bit(CONN_SEND_FBIT, &chan->conn_state)) + control |= __set_ctrl_final(chan); + + if (test_and_clear_bit(CONN_SEND_PBIT, &chan->conn_state)) + control |= __set_ctrl_poll(chan); + + skb = bt_skb_alloc(count, GFP_ATOMIC); + if (!skb) + return; + + lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE); + lh->len = cpu_to_le16(hlen - L2CAP_HDR_SIZE); + lh->cid = cpu_to_le16(chan->dcid); + + __put_control(chan, control, skb_put(skb, __ctrl_size(chan))); + + if (chan->fcs == L2CAP_FCS_CRC16) { + u16 fcs = crc16(0, (u8 *)lh, count - L2CAP_FCS_SIZE); + put_unaligned_le16(fcs, skb_put(skb, L2CAP_FCS_SIZE)); + } + + skb->priority = HCI_PRIO_MAX; + l2cap_do_send(chan, skb); +} + +static inline void l2cap_send_rr_or_rnr(struct l2cap_chan *chan, u32 control) +{ + if (test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) { + control |= __set_ctrl_super(chan, L2CAP_SUPER_RNR); + set_bit(CONN_RNR_SENT, &chan->conn_state); + } else + control |= __set_ctrl_super(chan, L2CAP_SUPER_RR); + + control |= __set_reqseq(chan, chan->buffer_seq); + + l2cap_send_sframe(chan, control); +} + +static inline int __l2cap_no_conn_pending(struct l2cap_chan *chan) +{ + return !test_bit(CONF_CONNECT_PEND, &chan->conf_state); +} + +static void l2cap_send_conn_req(struct l2cap_chan *chan) +{ + struct l2cap_conn *conn = chan->conn; + struct l2cap_conn_req req; + + req.scid = cpu_to_le16(chan->scid); + req.psm = chan->psm; + + chan->ident = l2cap_get_ident(conn); + + set_bit(CONF_CONNECT_PEND, &chan->conf_state); + + l2cap_send_cmd(conn, chan->ident, L2CAP_CONN_REQ, sizeof(req), &req); +} + +static void l2cap_do_start(struct l2cap_chan *chan) +{ + struct l2cap_conn *conn = chan->conn; + + if (conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_SENT) { + if (!(conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_DONE)) + return; + + if (l2cap_chan_check_security(chan) && + __l2cap_no_conn_pending(chan)) + l2cap_send_conn_req(chan); + } else { + struct l2cap_info_req req; + req.type = cpu_to_le16(L2CAP_IT_FEAT_MASK); + + conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_SENT; + conn->info_ident = l2cap_get_ident(conn); + + schedule_delayed_work(&conn->info_timer, L2CAP_INFO_TIMEOUT); + + l2cap_send_cmd(conn, conn->info_ident, + L2CAP_INFO_REQ, sizeof(req), &req); + } +} + +static inline int l2cap_mode_supported(__u8 mode, __u32 feat_mask) +{ + u32 local_feat_mask = l2cap_feat_mask; + if (!disable_ertm) + local_feat_mask |= L2CAP_FEAT_ERTM | L2CAP_FEAT_STREAMING; + + switch (mode) { + case L2CAP_MODE_ERTM: + return L2CAP_FEAT_ERTM & feat_mask & local_feat_mask; + case L2CAP_MODE_STREAMING: + return L2CAP_FEAT_STREAMING & feat_mask & local_feat_mask; + default: + return 0x00; + } +} + +static void l2cap_send_disconn_req(struct l2cap_conn *conn, struct l2cap_chan *chan, int err) +{ + struct sock *sk = chan->sk; + struct l2cap_disconn_req req; + + if (!conn) + return; + + if (chan->mode == L2CAP_MODE_ERTM) { + __clear_retrans_timer(chan); + __clear_monitor_timer(chan); + __clear_ack_timer(chan); + } + + req.dcid = cpu_to_le16(chan->dcid); + req.scid = cpu_to_le16(chan->scid); + l2cap_send_cmd(conn, l2cap_get_ident(conn), + L2CAP_DISCONN_REQ, sizeof(req), &req); + + lock_sock(sk); + __l2cap_state_change(chan, BT_DISCONN); + __l2cap_chan_set_err(chan, err); + release_sock(sk); +} + +/* ---- L2CAP connections ---- */ +static void l2cap_conn_start(struct l2cap_conn *conn) +{ + struct l2cap_chan *chan, *tmp; + + BT_DBG("conn %p", conn); + + mutex_lock(&conn->chan_lock); + + list_for_each_entry_safe(chan, tmp, &conn->chan_l, list) { + struct sock *sk = chan->sk; + + l2cap_chan_lock(chan); + + if (chan->chan_type != L2CAP_CHAN_CONN_ORIENTED) { + l2cap_chan_unlock(chan); + continue; + } + + if (chan->state == BT_CONNECT) { + if (!l2cap_chan_check_security(chan) || + !__l2cap_no_conn_pending(chan)) { + l2cap_chan_unlock(chan); + continue; + } + + if (!l2cap_mode_supported(chan->mode, conn->feat_mask) + && test_bit(CONF_STATE2_DEVICE, + &chan->conf_state)) { + l2cap_chan_close(chan, ECONNRESET); + l2cap_chan_unlock(chan); + continue; + } + + l2cap_send_conn_req(chan); + + } else if (chan->state == BT_CONNECT2) { + struct l2cap_conn_rsp rsp; + char buf[128]; + rsp.scid = cpu_to_le16(chan->dcid); + rsp.dcid = cpu_to_le16(chan->scid); + + if (l2cap_chan_check_security(chan)) { + lock_sock(sk); + if (bt_sk(sk)->defer_setup) { + struct sock *parent = bt_sk(sk)->parent; + rsp.result = cpu_to_le16(L2CAP_CR_PEND); + rsp.status = cpu_to_le16(L2CAP_CS_AUTHOR_PEND); + if (parent) + parent->sk_data_ready(parent, 0); + + } else { + __l2cap_state_change(chan, BT_CONFIG); + rsp.result = cpu_to_le16(L2CAP_CR_SUCCESS); + rsp.status = cpu_to_le16(L2CAP_CS_NO_INFO); + } + release_sock(sk); + } else { + rsp.result = cpu_to_le16(L2CAP_CR_PEND); + rsp.status = cpu_to_le16(L2CAP_CS_AUTHEN_PEND); + } + + l2cap_send_cmd(conn, chan->ident, L2CAP_CONN_RSP, + sizeof(rsp), &rsp); + + if (test_bit(CONF_REQ_SENT, &chan->conf_state) || + rsp.result != L2CAP_CR_SUCCESS) { + l2cap_chan_unlock(chan); + continue; + } + + set_bit(CONF_REQ_SENT, &chan->conf_state); + l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ, + l2cap_build_conf_req(chan, buf), buf); + chan->num_conf_req++; + } + + l2cap_chan_unlock(chan); + } + + mutex_unlock(&conn->chan_lock); +} + +/* Find socket with cid and source bdaddr. + * Returns closest match, locked. + */ +static struct l2cap_chan *l2cap_global_chan_by_scid(int state, __le16 cid, bdaddr_t *src) +{ + struct l2cap_chan *c, *c1 = NULL; + + read_lock(&chan_list_lock); + + list_for_each_entry(c, &chan_list, global_l) { + struct sock *sk = c->sk; + + if (state && c->state != state) + continue; + + if (c->scid == cid) { + /* Exact match. */ + if (!bacmp(&bt_sk(sk)->src, src)) { + read_unlock(&chan_list_lock); + return c; + } + + /* Closest match */ + if (!bacmp(&bt_sk(sk)->src, BDADDR_ANY)) + c1 = c; + } + } + + read_unlock(&chan_list_lock); + + return c1; +} + +static void l2cap_le_conn_ready(struct l2cap_conn *conn) +{ + struct sock *parent, *sk; + struct l2cap_chan *chan, *pchan; + + BT_DBG(""); + + /* Check if we have socket listening on cid */ + pchan = l2cap_global_chan_by_scid(BT_LISTEN, L2CAP_CID_LE_DATA, + conn->src); + if (!pchan) + return; + + parent = pchan->sk; + + lock_sock(parent); + + /* Check for backlog size */ + if (sk_acceptq_is_full(parent)) { + BT_DBG("backlog full %d", parent->sk_ack_backlog); + goto clean; + } + + chan = pchan->ops->new_connection(pchan->data); + if (!chan) + goto clean; + + sk = chan->sk; + + hci_conn_hold(conn->hcon); + + bacpy(&bt_sk(sk)->src, conn->src); + bacpy(&bt_sk(sk)->dst, conn->dst); + + bt_accept_enqueue(parent, sk); + + l2cap_chan_add(conn, chan); + + __set_chan_timer(chan, sk->sk_sndtimeo); + + __l2cap_state_change(chan, BT_CONNECTED); + parent->sk_data_ready(parent, 0); + +clean: + release_sock(parent); +} + +static void l2cap_chan_ready(struct l2cap_chan *chan) +{ + struct sock *sk = chan->sk; + struct sock *parent; + + lock_sock(sk); + + parent = bt_sk(sk)->parent; + + BT_DBG("sk %p, parent %p", sk, parent); + + chan->conf_state = 0; + __clear_chan_timer(chan); + + __l2cap_state_change(chan, BT_CONNECTED); + sk->sk_state_change(sk); + + if (parent) + parent->sk_data_ready(parent, 0); + + release_sock(sk); +} + +static void l2cap_conn_ready(struct l2cap_conn *conn) +{ + struct l2cap_chan *chan; + + BT_DBG("conn %p", conn); + + if (!conn->hcon->out && conn->hcon->type == LE_LINK) + l2cap_le_conn_ready(conn); + + if (conn->hcon->out && conn->hcon->type == LE_LINK) + smp_conn_security(conn, conn->hcon->pending_sec_level); + + mutex_lock(&conn->chan_lock); + + list_for_each_entry(chan, &conn->chan_l, list) { + + l2cap_chan_lock(chan); + + if (conn->hcon->type == LE_LINK) { + if (smp_conn_security(conn, chan->sec_level)) + l2cap_chan_ready(chan); + + } else if (chan->chan_type != L2CAP_CHAN_CONN_ORIENTED) { + struct sock *sk = chan->sk; + __clear_chan_timer(chan); + lock_sock(sk); + __l2cap_state_change(chan, BT_CONNECTED); + sk->sk_state_change(sk); + release_sock(sk); + + } else if (chan->state == BT_CONNECT) + l2cap_do_start(chan); + + l2cap_chan_unlock(chan); + } + + mutex_unlock(&conn->chan_lock); +} + +/* Notify sockets that we cannot guaranty reliability anymore */ +static void l2cap_conn_unreliable(struct l2cap_conn *conn, int err) +{ + struct l2cap_chan *chan; + + BT_DBG("conn %p", conn); + + mutex_lock(&conn->chan_lock); + + list_for_each_entry(chan, &conn->chan_l, list) { + if (test_bit(FLAG_FORCE_RELIABLE, &chan->flags)) + __l2cap_chan_set_err(chan, err); + } + + mutex_unlock(&conn->chan_lock); +} + +static void l2cap_info_timeout(struct work_struct *work) +{ + struct l2cap_conn *conn = container_of(work, struct l2cap_conn, + info_timer.work); + + conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE; + conn->info_ident = 0; + + l2cap_conn_start(conn); +} + +static void l2cap_conn_del(struct hci_conn *hcon, int err) +{ + struct l2cap_conn *conn = hcon->l2cap_data; + struct l2cap_chan *chan, *l; + + if (!conn) + return; + + BT_DBG("hcon %p conn %p, err %d", hcon, conn, err); + + kfree_skb(conn->rx_skb); + + mutex_lock(&conn->chan_lock); + + /* Kill channels */ + list_for_each_entry_safe(chan, l, &conn->chan_l, list) { + l2cap_chan_lock(chan); + + l2cap_chan_del(chan, err); + + l2cap_chan_unlock(chan); + + chan->ops->close(chan->data); + } + + mutex_unlock(&conn->chan_lock); + + hci_chan_del(conn->hchan); + + if (conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_SENT) + cancel_delayed_work_sync(&conn->info_timer); + + if (test_and_clear_bit(HCI_CONN_LE_SMP_PEND, &hcon->flags)) { + cancel_delayed_work_sync(&conn->security_timer); + smp_chan_destroy(conn); + } + + hcon->l2cap_data = NULL; + kfree(conn); +} + +static void security_timeout(struct work_struct *work) +{ + struct l2cap_conn *conn = container_of(work, struct l2cap_conn, + security_timer.work); + + l2cap_conn_del(conn->hcon, ETIMEDOUT); +} + +static struct l2cap_conn *l2cap_conn_add(struct hci_conn *hcon, u8 status) +{ + struct l2cap_conn *conn = hcon->l2cap_data; + struct hci_chan *hchan; + + if (conn || status) + return conn; + + hchan = hci_chan_create(hcon); + if (!hchan) + return NULL; + + conn = kzalloc(sizeof(struct l2cap_conn), GFP_ATOMIC); + if (!conn) { + hci_chan_del(hchan); + return NULL; + } + + hcon->l2cap_data = conn; + conn->hcon = hcon; + conn->hchan = hchan; + + BT_DBG("hcon %p conn %p hchan %p", hcon, conn, hchan); + + if (hcon->hdev->le_mtu && hcon->type == LE_LINK) + conn->mtu = hcon->hdev->le_mtu; + else + conn->mtu = hcon->hdev->acl_mtu; + + conn->src = &hcon->hdev->bdaddr; + conn->dst = &hcon->dst; + + conn->feat_mask = 0; + + spin_lock_init(&conn->lock); + mutex_init(&conn->chan_lock); + + INIT_LIST_HEAD(&conn->chan_l); + + if (hcon->type == LE_LINK) + INIT_DELAYED_WORK(&conn->security_timer, security_timeout); + else + INIT_DELAYED_WORK(&conn->info_timer, l2cap_info_timeout); + + conn->disc_reason = HCI_ERROR_REMOTE_USER_TERM; + + return conn; +} + +/* ---- Socket interface ---- */ + +/* Find socket with psm and source bdaddr. + * Returns closest match. + */ +static struct l2cap_chan *l2cap_global_chan_by_psm(int state, __le16 psm, bdaddr_t *src) +{ + struct l2cap_chan *c, *c1 = NULL; + + read_lock(&chan_list_lock); + + list_for_each_entry(c, &chan_list, global_l) { + struct sock *sk = c->sk; + + if (state && c->state != state) + continue; + + if (c->psm == psm) { + /* Exact match. */ + if (!bacmp(&bt_sk(sk)->src, src)) { + read_unlock(&chan_list_lock); + return c; + } + + /* Closest match */ + if (!bacmp(&bt_sk(sk)->src, BDADDR_ANY)) + c1 = c; + } + } + + read_unlock(&chan_list_lock); + + return c1; +} + +int l2cap_chan_connect(struct l2cap_chan *chan, __le16 psm, u16 cid, bdaddr_t *dst) +{ + struct sock *sk = chan->sk; + bdaddr_t *src = &bt_sk(sk)->src; + struct l2cap_conn *conn; + struct hci_conn *hcon; + struct hci_dev *hdev; + __u8 auth_type; + int err; + + BT_DBG("%s -> %s psm 0x%2.2x", batostr(src), batostr(dst), + chan->psm); + + hdev = hci_get_route(dst, src); + if (!hdev) + return -EHOSTUNREACH; + + hci_dev_lock(hdev); + + l2cap_chan_lock(chan); + + /* PSM must be odd and lsb of upper byte must be 0 */ + if ((__le16_to_cpu(psm) & 0x0101) != 0x0001 && !cid && + chan->chan_type != L2CAP_CHAN_RAW) { + err = -EINVAL; + goto done; + } + + if (chan->chan_type == L2CAP_CHAN_CONN_ORIENTED && !(psm || cid)) { + err = -EINVAL; + goto done; + } + + switch (chan->mode) { + case L2CAP_MODE_BASIC: + break; + case L2CAP_MODE_ERTM: + case L2CAP_MODE_STREAMING: + if (!disable_ertm) + break; + /* fall through */ + default: + err = -ENOTSUPP; + goto done; + } + + lock_sock(sk); + + switch (sk->sk_state) { + case BT_CONNECT: + case BT_CONNECT2: + case BT_CONFIG: + /* Already connecting */ + err = 0; + release_sock(sk); + goto done; + + case BT_CONNECTED: + /* Already connected */ + err = -EISCONN; + release_sock(sk); + goto done; + + case BT_OPEN: + case BT_BOUND: + /* Can connect */ + break; + + default: + err = -EBADFD; + release_sock(sk); + goto done; + } + + /* Set destination address and psm */ + bacpy(&bt_sk(sk)->dst, dst); + + release_sock(sk); + + chan->psm = psm; + chan->dcid = cid; + + auth_type = l2cap_get_auth_type(chan); + + if (chan->dcid == L2CAP_CID_LE_DATA) + hcon = hci_connect(hdev, LE_LINK, dst, + chan->sec_level, auth_type); + else + hcon = hci_connect(hdev, ACL_LINK, dst, + chan->sec_level, auth_type); + + if (IS_ERR(hcon)) { + err = PTR_ERR(hcon); + goto done; + } + + conn = l2cap_conn_add(hcon, 0); + if (!conn) { + hci_conn_put(hcon); + err = -ENOMEM; + goto done; + } + + /* Update source addr of the socket */ + bacpy(src, conn->src); + + l2cap_chan_unlock(chan); + l2cap_chan_add(conn, chan); + l2cap_chan_lock(chan); + + l2cap_state_change(chan, BT_CONNECT); + __set_chan_timer(chan, sk->sk_sndtimeo); + + if (hcon->state == BT_CONNECTED) { + if (chan->chan_type != L2CAP_CHAN_CONN_ORIENTED) { + __clear_chan_timer(chan); + if (l2cap_chan_check_security(chan)) + l2cap_state_change(chan, BT_CONNECTED); + } else + l2cap_do_start(chan); + } + + err = 0; + +done: + l2cap_chan_unlock(chan); + hci_dev_unlock(hdev); + hci_dev_put(hdev); + return err; +} + +int __l2cap_wait_ack(struct sock *sk) +{ + struct l2cap_chan *chan = l2cap_pi(sk)->chan; + DECLARE_WAITQUEUE(wait, current); + int err = 0; + int timeo = HZ/5; + + add_wait_queue(sk_sleep(sk), &wait); + set_current_state(TASK_INTERRUPTIBLE); + while (chan->unacked_frames > 0 && chan->conn) { + if (!timeo) + timeo = HZ/5; + + if (signal_pending(current)) { + err = sock_intr_errno(timeo); + break; + } + + release_sock(sk); + timeo = schedule_timeout(timeo); + lock_sock(sk); + set_current_state(TASK_INTERRUPTIBLE); + + err = sock_error(sk); + if (err) + break; + } + set_current_state(TASK_RUNNING); + remove_wait_queue(sk_sleep(sk), &wait); + return err; +} + +static void l2cap_monitor_timeout(struct work_struct *work) +{ + struct l2cap_chan *chan = container_of(work, struct l2cap_chan, + monitor_timer.work); + + BT_DBG("chan %p", chan); + + l2cap_chan_lock(chan); + + if (chan->retry_count >= chan->remote_max_tx) { + l2cap_send_disconn_req(chan->conn, chan, ECONNABORTED); + l2cap_chan_unlock(chan); + l2cap_chan_put(chan); + return; + } + + chan->retry_count++; + __set_monitor_timer(chan); + + l2cap_send_rr_or_rnr(chan, L2CAP_CTRL_POLL); + l2cap_chan_unlock(chan); + l2cap_chan_put(chan); +} + +static void l2cap_retrans_timeout(struct work_struct *work) +{ + struct l2cap_chan *chan = container_of(work, struct l2cap_chan, + retrans_timer.work); + + BT_DBG("chan %p", chan); + + l2cap_chan_lock(chan); + + chan->retry_count = 1; + __set_monitor_timer(chan); + + set_bit(CONN_WAIT_F, &chan->conn_state); + + l2cap_send_rr_or_rnr(chan, L2CAP_CTRL_POLL); + + l2cap_chan_unlock(chan); + l2cap_chan_put(chan); +} + +static void l2cap_drop_acked_frames(struct l2cap_chan *chan) +{ + struct sk_buff *skb; + + while ((skb = skb_peek(&chan->tx_q)) && + chan->unacked_frames) { + if (bt_cb(skb)->tx_seq == chan->expected_ack_seq) + break; + + skb = skb_dequeue(&chan->tx_q); + kfree_skb(skb); + + chan->unacked_frames--; + } + + if (!chan->unacked_frames) + __clear_retrans_timer(chan); +} + +static void l2cap_streaming_send(struct l2cap_chan *chan) +{ + struct sk_buff *skb; + u32 control; + u16 fcs; + + while ((skb = skb_dequeue(&chan->tx_q))) { + control = __get_control(chan, skb->data + L2CAP_HDR_SIZE); + control |= __set_txseq(chan, chan->next_tx_seq); + __put_control(chan, control, skb->data + L2CAP_HDR_SIZE); + + if (chan->fcs == L2CAP_FCS_CRC16) { + fcs = crc16(0, (u8 *)skb->data, + skb->len - L2CAP_FCS_SIZE); + put_unaligned_le16(fcs, + skb->data + skb->len - L2CAP_FCS_SIZE); + } + + l2cap_do_send(chan, skb); + + chan->next_tx_seq = __next_seq(chan, chan->next_tx_seq); + } +} + +static void l2cap_retransmit_one_frame(struct l2cap_chan *chan, u16 tx_seq) +{ + struct sk_buff *skb, *tx_skb; + u16 fcs; + u32 control; + + skb = skb_peek(&chan->tx_q); + if (!skb) + return; + + while (bt_cb(skb)->tx_seq != tx_seq) { + if (skb_queue_is_last(&chan->tx_q, skb)) + return; + + skb = skb_queue_next(&chan->tx_q, skb); + } + + if (chan->remote_max_tx && + bt_cb(skb)->retries == chan->remote_max_tx) { + l2cap_send_disconn_req(chan->conn, chan, ECONNABORTED); + return; + } + + tx_skb = skb_clone(skb, GFP_ATOMIC); + bt_cb(skb)->retries++; + + control = __get_control(chan, tx_skb->data + L2CAP_HDR_SIZE); + control &= __get_sar_mask(chan); + + if (test_and_clear_bit(CONN_SEND_FBIT, &chan->conn_state)) + control |= __set_ctrl_final(chan); + + control |= __set_reqseq(chan, chan->buffer_seq); + control |= __set_txseq(chan, tx_seq); + + __put_control(chan, control, tx_skb->data + L2CAP_HDR_SIZE); + + if (chan->fcs == L2CAP_FCS_CRC16) { + fcs = crc16(0, (u8 *)tx_skb->data, + tx_skb->len - L2CAP_FCS_SIZE); + put_unaligned_le16(fcs, + tx_skb->data + tx_skb->len - L2CAP_FCS_SIZE); + } + + l2cap_do_send(chan, tx_skb); +} + +static int l2cap_ertm_send(struct l2cap_chan *chan) +{ + struct sk_buff *skb, *tx_skb; + u16 fcs; + u32 control; + int nsent = 0; + + if (chan->state != BT_CONNECTED) + return -ENOTCONN; + + while ((skb = chan->tx_send_head) && (!l2cap_tx_window_full(chan))) { + + if (chan->remote_max_tx && + bt_cb(skb)->retries == chan->remote_max_tx) { + l2cap_send_disconn_req(chan->conn, chan, ECONNABORTED); + break; + } + + tx_skb = skb_clone(skb, GFP_ATOMIC); + + bt_cb(skb)->retries++; + + control = __get_control(chan, tx_skb->data + L2CAP_HDR_SIZE); + control &= __get_sar_mask(chan); + + if (test_and_clear_bit(CONN_SEND_FBIT, &chan->conn_state)) + control |= __set_ctrl_final(chan); + + control |= __set_reqseq(chan, chan->buffer_seq); + control |= __set_txseq(chan, chan->next_tx_seq); + + __put_control(chan, control, tx_skb->data + L2CAP_HDR_SIZE); + + if (chan->fcs == L2CAP_FCS_CRC16) { + fcs = crc16(0, (u8 *)skb->data, + tx_skb->len - L2CAP_FCS_SIZE); + put_unaligned_le16(fcs, skb->data + + tx_skb->len - L2CAP_FCS_SIZE); + } + + l2cap_do_send(chan, tx_skb); + + __set_retrans_timer(chan); + + bt_cb(skb)->tx_seq = chan->next_tx_seq; + + chan->next_tx_seq = __next_seq(chan, chan->next_tx_seq); + + if (bt_cb(skb)->retries == 1) { + chan->unacked_frames++; + + if (!nsent++) + __clear_ack_timer(chan); + } + + chan->frames_sent++; + + if (skb_queue_is_last(&chan->tx_q, skb)) + chan->tx_send_head = NULL; + else + chan->tx_send_head = skb_queue_next(&chan->tx_q, skb); + } + + return nsent; +} + +static int l2cap_retransmit_frames(struct l2cap_chan *chan) +{ + int ret; + + if (!skb_queue_empty(&chan->tx_q)) + chan->tx_send_head = chan->tx_q.next; + + chan->next_tx_seq = chan->expected_ack_seq; + ret = l2cap_ertm_send(chan); + return ret; +} + +static void __l2cap_send_ack(struct l2cap_chan *chan) +{ + u32 control = 0; + + control |= __set_reqseq(chan, chan->buffer_seq); + + if (test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) { + control |= __set_ctrl_super(chan, L2CAP_SUPER_RNR); + set_bit(CONN_RNR_SENT, &chan->conn_state); + l2cap_send_sframe(chan, control); + return; + } + + if (l2cap_ertm_send(chan) > 0) + return; + + control |= __set_ctrl_super(chan, L2CAP_SUPER_RR); + l2cap_send_sframe(chan, control); +} + +static void l2cap_send_ack(struct l2cap_chan *chan) +{ + __clear_ack_timer(chan); + __l2cap_send_ack(chan); +} + +static void l2cap_send_srejtail(struct l2cap_chan *chan) +{ + struct srej_list *tail; + u32 control; + + control = __set_ctrl_super(chan, L2CAP_SUPER_SREJ); + control |= __set_ctrl_final(chan); + + tail = list_entry((&chan->srej_l)->prev, struct srej_list, list); + control |= __set_reqseq(chan, tail->tx_seq); + + l2cap_send_sframe(chan, control); +} + +static inline int l2cap_skbuff_fromiovec(struct l2cap_chan *chan, + struct msghdr *msg, int len, + int count, struct sk_buff *skb) +{ + struct l2cap_conn *conn = chan->conn; + struct sk_buff **frag; + int err, sent = 0; + + if (memcpy_fromiovec(skb_put(skb, count), msg->msg_iov, count)) + return -EFAULT; + + sent += count; + len -= count; + + /* Continuation fragments (no L2CAP header) */ + frag = &skb_shinfo(skb)->frag_list; + while (len) { + count = min_t(unsigned int, conn->mtu, len); + + *frag = chan->ops->alloc_skb(chan, count, + msg->msg_flags & MSG_DONTWAIT, + &err); + + if (!*frag) + return err; + if (memcpy_fromiovec(skb_put(*frag, count), msg->msg_iov, count)) + return -EFAULT; + + (*frag)->priority = skb->priority; + + sent += count; + len -= count; + + frag = &(*frag)->next; + } + + return sent; +} + +static struct sk_buff *l2cap_create_connless_pdu(struct l2cap_chan *chan, + struct msghdr *msg, size_t len, + u32 priority) +{ + struct l2cap_conn *conn = chan->conn; + struct sk_buff *skb; + int err, count, hlen = L2CAP_HDR_SIZE + L2CAP_PSMLEN_SIZE; + struct l2cap_hdr *lh; + + BT_DBG("chan %p len %d priority %u", chan, (int)len, priority); + + count = min_t(unsigned int, (conn->mtu - hlen), len); + + skb = chan->ops->alloc_skb(chan, count + hlen, + msg->msg_flags & MSG_DONTWAIT, &err); + + if (!skb) + return ERR_PTR(err); + + skb->priority = priority; + + /* Create L2CAP header */ + lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE); + lh->cid = cpu_to_le16(chan->dcid); + lh->len = cpu_to_le16(len + (hlen - L2CAP_HDR_SIZE)); + put_unaligned_le16(chan->psm, skb_put(skb, 2)); + + err = l2cap_skbuff_fromiovec(chan, msg, len, count, skb); + if (unlikely(err < 0)) { + kfree_skb(skb); + return ERR_PTR(err); + } + return skb; +} + +static struct sk_buff *l2cap_create_basic_pdu(struct l2cap_chan *chan, + struct msghdr *msg, size_t len, + u32 priority) +{ + struct l2cap_conn *conn = chan->conn; + struct sk_buff *skb; + int err, count, hlen = L2CAP_HDR_SIZE; + struct l2cap_hdr *lh; + + BT_DBG("chan %p len %d", chan, (int)len); + + count = min_t(unsigned int, (conn->mtu - hlen), len); + + skb = chan->ops->alloc_skb(chan, count + hlen, + msg->msg_flags & MSG_DONTWAIT, &err); + + if (!skb) + return ERR_PTR(err); + + skb->priority = priority; + + /* Create L2CAP header */ + lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE); + lh->cid = cpu_to_le16(chan->dcid); + lh->len = cpu_to_le16(len + (hlen - L2CAP_HDR_SIZE)); + + err = l2cap_skbuff_fromiovec(chan, msg, len, count, skb); + if (unlikely(err < 0)) { + kfree_skb(skb); + return ERR_PTR(err); + } + return skb; +} + +static struct sk_buff *l2cap_create_iframe_pdu(struct l2cap_chan *chan, + struct msghdr *msg, size_t len, + u32 control, u16 sdulen) +{ + struct l2cap_conn *conn = chan->conn; + struct sk_buff *skb; + int err, count, hlen; + struct l2cap_hdr *lh; + + BT_DBG("chan %p len %d", chan, (int)len); + + if (!conn) + return ERR_PTR(-ENOTCONN); + + if (test_bit(FLAG_EXT_CTRL, &chan->flags)) + hlen = L2CAP_EXT_HDR_SIZE; + else + hlen = L2CAP_ENH_HDR_SIZE; + + if (sdulen) + hlen += L2CAP_SDULEN_SIZE; + + if (chan->fcs == L2CAP_FCS_CRC16) + hlen += L2CAP_FCS_SIZE; + + count = min_t(unsigned int, (conn->mtu - hlen), len); + + skb = chan->ops->alloc_skb(chan, count + hlen, + msg->msg_flags & MSG_DONTWAIT, &err); + + if (!skb) + return ERR_PTR(err); + + /* Create L2CAP header */ + lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE); + lh->cid = cpu_to_le16(chan->dcid); + lh->len = cpu_to_le16(len + (hlen - L2CAP_HDR_SIZE)); + + __put_control(chan, control, skb_put(skb, __ctrl_size(chan))); + + if (sdulen) + put_unaligned_le16(sdulen, skb_put(skb, L2CAP_SDULEN_SIZE)); + + err = l2cap_skbuff_fromiovec(chan, msg, len, count, skb); + if (unlikely(err < 0)) { + kfree_skb(skb); + return ERR_PTR(err); + } + + if (chan->fcs == L2CAP_FCS_CRC16) + put_unaligned_le16(0, skb_put(skb, L2CAP_FCS_SIZE)); + + bt_cb(skb)->retries = 0; + return skb; +} + +static int l2cap_sar_segment_sdu(struct l2cap_chan *chan, struct msghdr *msg, size_t len) +{ + struct sk_buff *skb; + struct sk_buff_head sar_queue; + u32 control; + size_t size = 0; + + skb_queue_head_init(&sar_queue); + control = __set_ctrl_sar(chan, L2CAP_SAR_START); + skb = l2cap_create_iframe_pdu(chan, msg, chan->remote_mps, control, len); + if (IS_ERR(skb)) + return PTR_ERR(skb); + + __skb_queue_tail(&sar_queue, skb); + len -= chan->remote_mps; + size += chan->remote_mps; + + while (len > 0) { + size_t buflen; + + if (len > chan->remote_mps) { + control = __set_ctrl_sar(chan, L2CAP_SAR_CONTINUE); + buflen = chan->remote_mps; + } else { + control = __set_ctrl_sar(chan, L2CAP_SAR_END); + buflen = len; + } + + skb = l2cap_create_iframe_pdu(chan, msg, buflen, control, 0); + if (IS_ERR(skb)) { + skb_queue_purge(&sar_queue); + return PTR_ERR(skb); + } + + __skb_queue_tail(&sar_queue, skb); + len -= buflen; + size += buflen; + } + skb_queue_splice_tail(&sar_queue, &chan->tx_q); + if (chan->tx_send_head == NULL) + chan->tx_send_head = sar_queue.next; + + return size; +} + +int l2cap_chan_send(struct l2cap_chan *chan, struct msghdr *msg, size_t len, + u32 priority) +{ + struct sk_buff *skb; + u32 control; + int err; + + /* Connectionless channel */ + if (chan->chan_type == L2CAP_CHAN_CONN_LESS) { + skb = l2cap_create_connless_pdu(chan, msg, len, priority); + if (IS_ERR(skb)) + return PTR_ERR(skb); + + l2cap_do_send(chan, skb); + return len; + } + + switch (chan->mode) { + case L2CAP_MODE_BASIC: + /* Check outgoing MTU */ + if (len > chan->omtu) + return -EMSGSIZE; + + /* Create a basic PDU */ + skb = l2cap_create_basic_pdu(chan, msg, len, priority); + if (IS_ERR(skb)) + return PTR_ERR(skb); + + l2cap_do_send(chan, skb); + err = len; + break; + + case L2CAP_MODE_ERTM: + case L2CAP_MODE_STREAMING: + /* Entire SDU fits into one PDU */ + if (len <= chan->remote_mps) { + control = __set_ctrl_sar(chan, L2CAP_SAR_UNSEGMENTED); + skb = l2cap_create_iframe_pdu(chan, msg, len, control, + 0); + if (IS_ERR(skb)) + return PTR_ERR(skb); + + __skb_queue_tail(&chan->tx_q, skb); + + if (chan->tx_send_head == NULL) + chan->tx_send_head = skb; + + } else { + /* Segment SDU into multiples PDUs */ + err = l2cap_sar_segment_sdu(chan, msg, len); + if (err < 0) + return err; + } + + if (chan->mode == L2CAP_MODE_STREAMING) { + l2cap_streaming_send(chan); + err = len; + break; + } + + if (test_bit(CONN_REMOTE_BUSY, &chan->conn_state) && + test_bit(CONN_WAIT_F, &chan->conn_state)) { + err = len; + break; + } + + err = l2cap_ertm_send(chan); + if (err >= 0) + err = len; + + break; + + default: + BT_DBG("bad state %1.1x", chan->mode); + err = -EBADFD; + } + + return err; +} + +/* Copy frame to all raw sockets on that connection */ +static void l2cap_raw_recv(struct l2cap_conn *conn, struct sk_buff *skb) +{ + struct sk_buff *nskb; + struct l2cap_chan *chan; + + BT_DBG("conn %p", conn); + + mutex_lock(&conn->chan_lock); + + list_for_each_entry(chan, &conn->chan_l, list) { + struct sock *sk = chan->sk; + if (chan->chan_type != L2CAP_CHAN_RAW) + continue; + + /* Don't send frame to the socket it came from */ + if (skb->sk == sk) + continue; + nskb = skb_clone(skb, GFP_ATOMIC); + if (!nskb) + continue; + + if (chan->ops->recv(chan->data, nskb)) + kfree_skb(nskb); + } + + mutex_unlock(&conn->chan_lock); +} + +/* ---- L2CAP signalling commands ---- */ +static struct sk_buff *l2cap_build_cmd(struct l2cap_conn *conn, + u8 code, u8 ident, u16 dlen, void *data) +{ + struct sk_buff *skb, **frag; + struct l2cap_cmd_hdr *cmd; + struct l2cap_hdr *lh; + int len, count; + + BT_DBG("conn %p, code 0x%2.2x, ident 0x%2.2x, len %d", + conn, code, ident, dlen); + + len = L2CAP_HDR_SIZE + L2CAP_CMD_HDR_SIZE + dlen; + count = min_t(unsigned int, conn->mtu, len); + + skb = bt_skb_alloc(count, GFP_ATOMIC); + if (!skb) + return NULL; + + lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE); + lh->len = cpu_to_le16(L2CAP_CMD_HDR_SIZE + dlen); + + if (conn->hcon->type == LE_LINK) + lh->cid = cpu_to_le16(L2CAP_CID_LE_SIGNALING); + else + lh->cid = cpu_to_le16(L2CAP_CID_SIGNALING); + + cmd = (struct l2cap_cmd_hdr *) skb_put(skb, L2CAP_CMD_HDR_SIZE); + cmd->code = code; + cmd->ident = ident; + cmd->len = cpu_to_le16(dlen); + + if (dlen) { + count -= L2CAP_HDR_SIZE + L2CAP_CMD_HDR_SIZE; + memcpy(skb_put(skb, count), data, count); + data += count; + } + + len -= skb->len; + + /* Continuation fragments (no L2CAP header) */ + frag = &skb_shinfo(skb)->frag_list; + while (len) { + count = min_t(unsigned int, conn->mtu, len); + + *frag = bt_skb_alloc(count, GFP_ATOMIC); + if (!*frag) + goto fail; + + memcpy(skb_put(*frag, count), data, count); + + len -= count; + data += count; + + frag = &(*frag)->next; + } + + return skb; + +fail: + kfree_skb(skb); + return NULL; +} + +static inline int l2cap_get_conf_opt(void **ptr, int *type, int *olen, unsigned long *val) +{ + struct l2cap_conf_opt *opt = *ptr; + int len; + + len = L2CAP_CONF_OPT_SIZE + opt->len; + *ptr += len; + + *type = opt->type; + *olen = opt->len; + + switch (opt->len) { + case 1: + *val = *((u8 *) opt->val); + break; + + case 2: + *val = get_unaligned_le16(opt->val); + break; + + case 4: + *val = get_unaligned_le32(opt->val); + break; + + default: + *val = (unsigned long) opt->val; + break; + } + + BT_DBG("type 0x%2.2x len %d val 0x%lx", *type, opt->len, *val); + return len; +} + +static void l2cap_add_conf_opt(void **ptr, u8 type, u8 len, unsigned long val) +{ + struct l2cap_conf_opt *opt = *ptr; + + BT_DBG("type 0x%2.2x len %d val 0x%lx", type, len, val); + + opt->type = type; + opt->len = len; + + switch (len) { + case 1: + *((u8 *) opt->val) = val; + break; + + case 2: + put_unaligned_le16(val, opt->val); + break; + + case 4: + put_unaligned_le32(val, opt->val); + break; + + default: + memcpy(opt->val, (void *) val, len); + break; + } + + *ptr += L2CAP_CONF_OPT_SIZE + len; +} + +static void l2cap_add_opt_efs(void **ptr, struct l2cap_chan *chan) +{ + struct l2cap_conf_efs efs; + + switch (chan->mode) { + case L2CAP_MODE_ERTM: + efs.id = chan->local_id; + efs.stype = chan->local_stype; + efs.msdu = cpu_to_le16(chan->local_msdu); + efs.sdu_itime = cpu_to_le32(chan->local_sdu_itime); + efs.acc_lat = cpu_to_le32(L2CAP_DEFAULT_ACC_LAT); + efs.flush_to = cpu_to_le32(L2CAP_DEFAULT_FLUSH_TO); + break; + + case L2CAP_MODE_STREAMING: + efs.id = 1; + efs.stype = L2CAP_SERV_BESTEFFORT; + efs.msdu = cpu_to_le16(chan->local_msdu); + efs.sdu_itime = cpu_to_le32(chan->local_sdu_itime); + efs.acc_lat = 0; + efs.flush_to = 0; + break; + + default: + return; + } + + l2cap_add_conf_opt(ptr, L2CAP_CONF_EFS, sizeof(efs), + (unsigned long) &efs); +} + +static void l2cap_ack_timeout(struct work_struct *work) +{ + struct l2cap_chan *chan = container_of(work, struct l2cap_chan, + ack_timer.work); + + BT_DBG("chan %p", chan); + + l2cap_chan_lock(chan); + + __l2cap_send_ack(chan); + + l2cap_chan_unlock(chan); + + l2cap_chan_put(chan); +} + +static inline void l2cap_ertm_init(struct l2cap_chan *chan) +{ + chan->expected_ack_seq = 0; + chan->unacked_frames = 0; + chan->buffer_seq = 0; + chan->num_acked = 0; + chan->frames_sent = 0; + + INIT_DELAYED_WORK(&chan->retrans_timer, l2cap_retrans_timeout); + INIT_DELAYED_WORK(&chan->monitor_timer, l2cap_monitor_timeout); + INIT_DELAYED_WORK(&chan->ack_timer, l2cap_ack_timeout); + + skb_queue_head_init(&chan->srej_q); + + INIT_LIST_HEAD(&chan->srej_l); +} + +static inline __u8 l2cap_select_mode(__u8 mode, __u16 remote_feat_mask) +{ + switch (mode) { + case L2CAP_MODE_STREAMING: + case L2CAP_MODE_ERTM: + if (l2cap_mode_supported(mode, remote_feat_mask)) + return mode; + /* fall through */ + default: + return L2CAP_MODE_BASIC; + } +} + +static inline bool __l2cap_ews_supported(struct l2cap_chan *chan) +{ + return enable_hs && chan->conn->feat_mask & L2CAP_FEAT_EXT_WINDOW; +} + +static inline bool __l2cap_efs_supported(struct l2cap_chan *chan) +{ + return enable_hs && chan->conn->feat_mask & L2CAP_FEAT_EXT_FLOW; +} + +static inline void l2cap_txwin_setup(struct l2cap_chan *chan) +{ + if (chan->tx_win > L2CAP_DEFAULT_TX_WINDOW && + __l2cap_ews_supported(chan)) { + /* use extended control field */ + set_bit(FLAG_EXT_CTRL, &chan->flags); + chan->tx_win_max = L2CAP_DEFAULT_EXT_WINDOW; + } else { + chan->tx_win = min_t(u16, chan->tx_win, + L2CAP_DEFAULT_TX_WINDOW); + chan->tx_win_max = L2CAP_DEFAULT_TX_WINDOW; + } +} + +static int l2cap_build_conf_req(struct l2cap_chan *chan, void *data) +{ + struct l2cap_conf_req *req = data; + struct l2cap_conf_rfc rfc = { .mode = chan->mode }; + void *ptr = req->data; + u16 size; + + BT_DBG("chan %p", chan); + + if (chan->num_conf_req || chan->num_conf_rsp) + goto done; + + switch (chan->mode) { + case L2CAP_MODE_STREAMING: + case L2CAP_MODE_ERTM: + if (test_bit(CONF_STATE2_DEVICE, &chan->conf_state)) + break; + + if (__l2cap_efs_supported(chan)) + set_bit(FLAG_EFS_ENABLE, &chan->flags); + + /* fall through */ + default: + chan->mode = l2cap_select_mode(rfc.mode, chan->conn->feat_mask); + break; + } + +done: + if (chan->imtu != L2CAP_DEFAULT_MTU) + l2cap_add_conf_opt(&ptr, L2CAP_CONF_MTU, 2, chan->imtu); + + switch (chan->mode) { + case L2CAP_MODE_BASIC: + if (!(chan->conn->feat_mask & L2CAP_FEAT_ERTM) && + !(chan->conn->feat_mask & L2CAP_FEAT_STREAMING)) + break; + + rfc.mode = L2CAP_MODE_BASIC; + rfc.txwin_size = 0; + rfc.max_transmit = 0; + rfc.retrans_timeout = 0; + rfc.monitor_timeout = 0; + rfc.max_pdu_size = 0; + + l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, sizeof(rfc), + (unsigned long) &rfc); + break; + + case L2CAP_MODE_ERTM: + rfc.mode = L2CAP_MODE_ERTM; + rfc.max_transmit = chan->max_tx; + rfc.retrans_timeout = 0; + rfc.monitor_timeout = 0; + + size = min_t(u16, L2CAP_DEFAULT_MAX_PDU_SIZE, chan->conn->mtu - + L2CAP_EXT_HDR_SIZE - + L2CAP_SDULEN_SIZE - + L2CAP_FCS_SIZE); + rfc.max_pdu_size = cpu_to_le16(size); + + l2cap_txwin_setup(chan); + + rfc.txwin_size = min_t(u16, chan->tx_win, + L2CAP_DEFAULT_TX_WINDOW); + + l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, sizeof(rfc), + (unsigned long) &rfc); + + if (test_bit(FLAG_EFS_ENABLE, &chan->flags)) + l2cap_add_opt_efs(&ptr, chan); + + if (!(chan->conn->feat_mask & L2CAP_FEAT_FCS)) + break; + + if (chan->fcs == L2CAP_FCS_NONE || + test_bit(CONF_NO_FCS_RECV, &chan->conf_state)) { + chan->fcs = L2CAP_FCS_NONE; + l2cap_add_conf_opt(&ptr, L2CAP_CONF_FCS, 1, chan->fcs); + } + + if (test_bit(FLAG_EXT_CTRL, &chan->flags)) + l2cap_add_conf_opt(&ptr, L2CAP_CONF_EWS, 2, + chan->tx_win); + break; + + case L2CAP_MODE_STREAMING: + rfc.mode = L2CAP_MODE_STREAMING; + rfc.txwin_size = 0; + rfc.max_transmit = 0; + rfc.retrans_timeout = 0; + rfc.monitor_timeout = 0; + + size = min_t(u16, L2CAP_DEFAULT_MAX_PDU_SIZE, chan->conn->mtu - + L2CAP_EXT_HDR_SIZE - + L2CAP_SDULEN_SIZE - + L2CAP_FCS_SIZE); + rfc.max_pdu_size = cpu_to_le16(size); + + l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, sizeof(rfc), + (unsigned long) &rfc); + + if (test_bit(FLAG_EFS_ENABLE, &chan->flags)) + l2cap_add_opt_efs(&ptr, chan); + + if (!(chan->conn->feat_mask & L2CAP_FEAT_FCS)) + break; + + if (chan->fcs == L2CAP_FCS_NONE || + test_bit(CONF_NO_FCS_RECV, &chan->conf_state)) { + chan->fcs = L2CAP_FCS_NONE; + l2cap_add_conf_opt(&ptr, L2CAP_CONF_FCS, 1, chan->fcs); + } + break; + } + + req->dcid = cpu_to_le16(chan->dcid); + req->flags = cpu_to_le16(0); + + return ptr - data; +} + +static int l2cap_parse_conf_req(struct l2cap_chan *chan, void *data) +{ + struct l2cap_conf_rsp *rsp = data; + void *ptr = rsp->data; + void *req = chan->conf_req; + int len = chan->conf_len; + int type, hint, olen; + unsigned long val; + struct l2cap_conf_rfc rfc = { .mode = L2CAP_MODE_BASIC }; + struct l2cap_conf_efs efs; + u8 remote_efs = 0; + u16 mtu = L2CAP_DEFAULT_MTU; + u16 result = L2CAP_CONF_SUCCESS; + u16 size; + + BT_DBG("chan %p", chan); + + while (len >= L2CAP_CONF_OPT_SIZE) { + len -= l2cap_get_conf_opt(&req, &type, &olen, &val); + + hint = type & L2CAP_CONF_HINT; + type &= L2CAP_CONF_MASK; + + switch (type) { + case L2CAP_CONF_MTU: + mtu = val; + break; + + case L2CAP_CONF_FLUSH_TO: + chan->flush_to = val; + break; + + case L2CAP_CONF_QOS: + break; + + case L2CAP_CONF_RFC: + if (olen == sizeof(rfc)) + memcpy(&rfc, (void *) val, olen); + break; + + case L2CAP_CONF_FCS: + if (val == L2CAP_FCS_NONE) + set_bit(CONF_NO_FCS_RECV, &chan->conf_state); + break; + + case L2CAP_CONF_EFS: + remote_efs = 1; + if (olen == sizeof(efs)) + memcpy(&efs, (void *) val, olen); + break; + + case L2CAP_CONF_EWS: + if (!enable_hs) + return -ECONNREFUSED; + + set_bit(FLAG_EXT_CTRL, &chan->flags); + set_bit(CONF_EWS_RECV, &chan->conf_state); + chan->tx_win_max = L2CAP_DEFAULT_EXT_WINDOW; + chan->remote_tx_win = val; + break; + + default: + if (hint) + break; + + result = L2CAP_CONF_UNKNOWN; + *((u8 *) ptr++) = type; + break; + } + } + + if (chan->num_conf_rsp || chan->num_conf_req > 1) + goto done; + + switch (chan->mode) { + case L2CAP_MODE_STREAMING: + case L2CAP_MODE_ERTM: + if (!test_bit(CONF_STATE2_DEVICE, &chan->conf_state)) { + chan->mode = l2cap_select_mode(rfc.mode, + chan->conn->feat_mask); + break; + } + + if (remote_efs) { + if (__l2cap_efs_supported(chan)) + set_bit(FLAG_EFS_ENABLE, &chan->flags); + else + return -ECONNREFUSED; + } + + if (chan->mode != rfc.mode) + return -ECONNREFUSED; + + break; + } + +done: + if (chan->mode != rfc.mode) { + result = L2CAP_CONF_UNACCEPT; + rfc.mode = chan->mode; + + if (chan->num_conf_rsp == 1) + return -ECONNREFUSED; + + l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, + sizeof(rfc), (unsigned long) &rfc); + } + + if (result == L2CAP_CONF_SUCCESS) { + /* Configure output options and let the other side know + * which ones we don't like. */ + + if (mtu < L2CAP_DEFAULT_MIN_MTU) + result = L2CAP_CONF_UNACCEPT; + else { + chan->omtu = mtu; + set_bit(CONF_MTU_DONE, &chan->conf_state); + } + l2cap_add_conf_opt(&ptr, L2CAP_CONF_MTU, 2, chan->omtu); + + if (remote_efs) { + if (chan->local_stype != L2CAP_SERV_NOTRAFIC && + efs.stype != L2CAP_SERV_NOTRAFIC && + efs.stype != chan->local_stype) { + + result = L2CAP_CONF_UNACCEPT; + + if (chan->num_conf_req >= 1) + return -ECONNREFUSED; + + l2cap_add_conf_opt(&ptr, L2CAP_CONF_EFS, + sizeof(efs), + (unsigned long) &efs); + } else { + /* Send PENDING Conf Rsp */ + result = L2CAP_CONF_PENDING; + set_bit(CONF_LOC_CONF_PEND, &chan->conf_state); + } + } + + switch (rfc.mode) { + case L2CAP_MODE_BASIC: + chan->fcs = L2CAP_FCS_NONE; + set_bit(CONF_MODE_DONE, &chan->conf_state); + break; + + case L2CAP_MODE_ERTM: + if (!test_bit(CONF_EWS_RECV, &chan->conf_state)) + chan->remote_tx_win = rfc.txwin_size; + else + rfc.txwin_size = L2CAP_DEFAULT_TX_WINDOW; + + chan->remote_max_tx = rfc.max_transmit; + + size = min_t(u16, le16_to_cpu(rfc.max_pdu_size), + chan->conn->mtu - + L2CAP_EXT_HDR_SIZE - + L2CAP_SDULEN_SIZE - + L2CAP_FCS_SIZE); + rfc.max_pdu_size = cpu_to_le16(size); + chan->remote_mps = size; + + rfc.retrans_timeout = + le16_to_cpu(L2CAP_DEFAULT_RETRANS_TO); + rfc.monitor_timeout = + le16_to_cpu(L2CAP_DEFAULT_MONITOR_TO); + + set_bit(CONF_MODE_DONE, &chan->conf_state); + + l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, + sizeof(rfc), (unsigned long) &rfc); + + if (test_bit(FLAG_EFS_ENABLE, &chan->flags)) { + chan->remote_id = efs.id; + chan->remote_stype = efs.stype; + chan->remote_msdu = le16_to_cpu(efs.msdu); + chan->remote_flush_to = + le32_to_cpu(efs.flush_to); + chan->remote_acc_lat = + le32_to_cpu(efs.acc_lat); + chan->remote_sdu_itime = + le32_to_cpu(efs.sdu_itime); + l2cap_add_conf_opt(&ptr, L2CAP_CONF_EFS, + sizeof(efs), (unsigned long) &efs); + } + break; + + case L2CAP_MODE_STREAMING: + size = min_t(u16, le16_to_cpu(rfc.max_pdu_size), + chan->conn->mtu - + L2CAP_EXT_HDR_SIZE - + L2CAP_SDULEN_SIZE - + L2CAP_FCS_SIZE); + rfc.max_pdu_size = cpu_to_le16(size); + chan->remote_mps = size; + + set_bit(CONF_MODE_DONE, &chan->conf_state); + + l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, + sizeof(rfc), (unsigned long) &rfc); + + break; + + default: + result = L2CAP_CONF_UNACCEPT; + + memset(&rfc, 0, sizeof(rfc)); + rfc.mode = chan->mode; + } + + if (result == L2CAP_CONF_SUCCESS) + set_bit(CONF_OUTPUT_DONE, &chan->conf_state); + } + rsp->scid = cpu_to_le16(chan->dcid); + rsp->result = cpu_to_le16(result); + rsp->flags = cpu_to_le16(0x0000); + + return ptr - data; +} + +static int l2cap_parse_conf_rsp(struct l2cap_chan *chan, void *rsp, int len, void *data, u16 *result) +{ + struct l2cap_conf_req *req = data; + void *ptr = req->data; + int type, olen; + unsigned long val; + struct l2cap_conf_rfc rfc = { .mode = L2CAP_MODE_BASIC }; + struct l2cap_conf_efs efs; + + BT_DBG("chan %p, rsp %p, len %d, req %p", chan, rsp, len, data); + + while (len >= L2CAP_CONF_OPT_SIZE) { + len -= l2cap_get_conf_opt(&rsp, &type, &olen, &val); + + switch (type) { + case L2CAP_CONF_MTU: + if (val < L2CAP_DEFAULT_MIN_MTU) { + *result = L2CAP_CONF_UNACCEPT; + chan->imtu = L2CAP_DEFAULT_MIN_MTU; + } else + chan->imtu = val; + l2cap_add_conf_opt(&ptr, L2CAP_CONF_MTU, 2, chan->imtu); + break; + + case L2CAP_CONF_FLUSH_TO: + chan->flush_to = val; + l2cap_add_conf_opt(&ptr, L2CAP_CONF_FLUSH_TO, + 2, chan->flush_to); + break; + + case L2CAP_CONF_RFC: + if (olen == sizeof(rfc)) + memcpy(&rfc, (void *)val, olen); + + if (test_bit(CONF_STATE2_DEVICE, &chan->conf_state) && + rfc.mode != chan->mode) + return -ECONNREFUSED; + + chan->fcs = 0; + + l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, + sizeof(rfc), (unsigned long) &rfc); + break; + + case L2CAP_CONF_EWS: + chan->tx_win = min_t(u16, val, + L2CAP_DEFAULT_EXT_WINDOW); + l2cap_add_conf_opt(&ptr, L2CAP_CONF_EWS, 2, + chan->tx_win); + break; + + case L2CAP_CONF_EFS: + if (olen == sizeof(efs)) + memcpy(&efs, (void *)val, olen); + + if (chan->local_stype != L2CAP_SERV_NOTRAFIC && + efs.stype != L2CAP_SERV_NOTRAFIC && + efs.stype != chan->local_stype) + return -ECONNREFUSED; + + l2cap_add_conf_opt(&ptr, L2CAP_CONF_EFS, + sizeof(efs), (unsigned long) &efs); + break; + } + } + + if (chan->mode == L2CAP_MODE_BASIC && chan->mode != rfc.mode) + return -ECONNREFUSED; + + chan->mode = rfc.mode; + + if (*result == L2CAP_CONF_SUCCESS || *result == L2CAP_CONF_PENDING) { + switch (rfc.mode) { + case L2CAP_MODE_ERTM: + chan->retrans_timeout = le16_to_cpu(rfc.retrans_timeout); + chan->monitor_timeout = le16_to_cpu(rfc.monitor_timeout); + chan->mps = le16_to_cpu(rfc.max_pdu_size); + + if (test_bit(FLAG_EFS_ENABLE, &chan->flags)) { + chan->local_msdu = le16_to_cpu(efs.msdu); + chan->local_sdu_itime = + le32_to_cpu(efs.sdu_itime); + chan->local_acc_lat = le32_to_cpu(efs.acc_lat); + chan->local_flush_to = + le32_to_cpu(efs.flush_to); + } + break; + + case L2CAP_MODE_STREAMING: + chan->mps = le16_to_cpu(rfc.max_pdu_size); + } + } + + req->dcid = cpu_to_le16(chan->dcid); + req->flags = cpu_to_le16(0x0000); + + return ptr - data; +} + +static int l2cap_build_conf_rsp(struct l2cap_chan *chan, void *data, u16 result, u16 flags) +{ + struct l2cap_conf_rsp *rsp = data; + void *ptr = rsp->data; + + BT_DBG("chan %p", chan); + + rsp->scid = cpu_to_le16(chan->dcid); + rsp->result = cpu_to_le16(result); + rsp->flags = cpu_to_le16(flags); + + return ptr - data; +} + +void __l2cap_connect_rsp_defer(struct l2cap_chan *chan) +{ + struct l2cap_conn_rsp rsp; + struct l2cap_conn *conn = chan->conn; + u8 buf[128]; + + rsp.scid = cpu_to_le16(chan->dcid); + rsp.dcid = cpu_to_le16(chan->scid); + rsp.result = cpu_to_le16(L2CAP_CR_SUCCESS); + rsp.status = cpu_to_le16(L2CAP_CS_NO_INFO); + l2cap_send_cmd(conn, chan->ident, + L2CAP_CONN_RSP, sizeof(rsp), &rsp); + + if (test_and_set_bit(CONF_REQ_SENT, &chan->conf_state)) + return; + + l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ, + l2cap_build_conf_req(chan, buf), buf); + chan->num_conf_req++; +} + +static void l2cap_conf_rfc_get(struct l2cap_chan *chan, void *rsp, int len) +{ + int type, olen; + unsigned long val; + struct l2cap_conf_rfc rfc; + + BT_DBG("chan %p, rsp %p, len %d", chan, rsp, len); + + if ((chan->mode != L2CAP_MODE_ERTM) && (chan->mode != L2CAP_MODE_STREAMING)) + return; + + while (len >= L2CAP_CONF_OPT_SIZE) { + len -= l2cap_get_conf_opt(&rsp, &type, &olen, &val); + + switch (type) { + case L2CAP_CONF_RFC: + if (olen == sizeof(rfc)) + memcpy(&rfc, (void *)val, olen); + goto done; + } + } + + /* Use sane default values in case a misbehaving remote device + * did not send an RFC option. + */ + rfc.mode = chan->mode; + rfc.retrans_timeout = cpu_to_le16(L2CAP_DEFAULT_RETRANS_TO); + rfc.monitor_timeout = cpu_to_le16(L2CAP_DEFAULT_MONITOR_TO); + rfc.max_pdu_size = cpu_to_le16(chan->imtu); + + BT_ERR("Expected RFC option was not found, using defaults"); + +done: + switch (rfc.mode) { + case L2CAP_MODE_ERTM: + chan->retrans_timeout = le16_to_cpu(rfc.retrans_timeout); + chan->monitor_timeout = le16_to_cpu(rfc.monitor_timeout); + chan->mps = le16_to_cpu(rfc.max_pdu_size); + break; + case L2CAP_MODE_STREAMING: + chan->mps = le16_to_cpu(rfc.max_pdu_size); + } +} + +static inline int l2cap_command_rej(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data) +{ + struct l2cap_cmd_rej_unk *rej = (struct l2cap_cmd_rej_unk *) data; + + if (rej->reason != L2CAP_REJ_NOT_UNDERSTOOD) + return 0; + + if ((conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_SENT) && + cmd->ident == conn->info_ident) { + cancel_delayed_work(&conn->info_timer); + + conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE; + conn->info_ident = 0; + + l2cap_conn_start(conn); + } + + return 0; +} + +static inline int l2cap_connect_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data) +{ + struct l2cap_conn_req *req = (struct l2cap_conn_req *) data; + struct l2cap_conn_rsp rsp; + struct l2cap_chan *chan = NULL, *pchan; + struct sock *parent, *sk = NULL; + int result, status = L2CAP_CS_NO_INFO; + + u16 dcid = 0, scid = __le16_to_cpu(req->scid); + __le16 psm = req->psm; + + BT_DBG("psm 0x%2.2x scid 0x%4.4x", psm, scid); + + /* Check if we have socket listening on psm */ + pchan = l2cap_global_chan_by_psm(BT_LISTEN, psm, conn->src); + if (!pchan) { + result = L2CAP_CR_BAD_PSM; + goto sendresp; + } + + parent = pchan->sk; + + mutex_lock(&conn->chan_lock); + lock_sock(parent); + + /* Check if the ACL is secure enough (if not SDP) */ + if (psm != cpu_to_le16(0x0001) && + !hci_conn_check_link_mode(conn->hcon)) { + conn->disc_reason = HCI_ERROR_AUTH_FAILURE; + result = L2CAP_CR_SEC_BLOCK; + goto response; + } + + result = L2CAP_CR_NO_MEM; + + /* Check for backlog size */ + if (sk_acceptq_is_full(parent)) { + BT_DBG("backlog full %d", parent->sk_ack_backlog); + goto response; + } + + chan = pchan->ops->new_connection(pchan->data); + if (!chan) + goto response; + + sk = chan->sk; + + /* Check if we already have channel with that dcid */ + if (__l2cap_get_chan_by_dcid(conn, scid)) { + sock_set_flag(sk, SOCK_ZAPPED); + chan->ops->close(chan->data); + goto response; + } + + hci_conn_hold(conn->hcon); + + bacpy(&bt_sk(sk)->src, conn->src); + bacpy(&bt_sk(sk)->dst, conn->dst); + chan->psm = psm; + chan->dcid = scid; + + bt_accept_enqueue(parent, sk); + + __l2cap_chan_add(conn, chan); + + dcid = chan->scid; + + __set_chan_timer(chan, sk->sk_sndtimeo); + + chan->ident = cmd->ident; + + if (conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_DONE) { + if (l2cap_chan_check_security(chan)) { + if (bt_sk(sk)->defer_setup) { + __l2cap_state_change(chan, BT_CONNECT2); + result = L2CAP_CR_PEND; + status = L2CAP_CS_AUTHOR_PEND; + parent->sk_data_ready(parent, 0); + } else { + __l2cap_state_change(chan, BT_CONFIG); + result = L2CAP_CR_SUCCESS; + status = L2CAP_CS_NO_INFO; + } + } else { + __l2cap_state_change(chan, BT_CONNECT2); + result = L2CAP_CR_PEND; + status = L2CAP_CS_AUTHEN_PEND; + } + } else { + __l2cap_state_change(chan, BT_CONNECT2); + result = L2CAP_CR_PEND; + status = L2CAP_CS_NO_INFO; + } + +response: + release_sock(parent); + mutex_unlock(&conn->chan_lock); + +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_cmd(conn, cmd->ident, L2CAP_CONN_RSP, sizeof(rsp), &rsp); + + if (result == L2CAP_CR_PEND && status == L2CAP_CS_NO_INFO) { + struct l2cap_info_req info; + info.type = cpu_to_le16(L2CAP_IT_FEAT_MASK); + + conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_SENT; + conn->info_ident = l2cap_get_ident(conn); + + schedule_delayed_work(&conn->info_timer, L2CAP_INFO_TIMEOUT); + + l2cap_send_cmd(conn, conn->info_ident, + L2CAP_INFO_REQ, sizeof(info), &info); + } + + if (chan && !test_bit(CONF_REQ_SENT, &chan->conf_state) && + result == L2CAP_CR_SUCCESS) { + u8 buf[128]; + set_bit(CONF_REQ_SENT, &chan->conf_state); + l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ, + l2cap_build_conf_req(chan, buf), buf); + chan->num_conf_req++; + } + + return 0; +} + +static inline int l2cap_connect_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data) +{ + struct l2cap_conn_rsp *rsp = (struct l2cap_conn_rsp *) data; + u16 scid, dcid, result, status; + struct l2cap_chan *chan; + u8 req[128]; + int err; + + scid = __le16_to_cpu(rsp->scid); + dcid = __le16_to_cpu(rsp->dcid); + result = __le16_to_cpu(rsp->result); + status = __le16_to_cpu(rsp->status); + + BT_DBG("dcid 0x%4.4x scid 0x%4.4x result 0x%2.2x status 0x%2.2x", + dcid, scid, result, status); + + mutex_lock(&conn->chan_lock); + + if (scid) { + chan = __l2cap_get_chan_by_scid(conn, scid); + if (!chan) { + err = -EFAULT; + goto unlock; + } + } else { + chan = __l2cap_get_chan_by_ident(conn, cmd->ident); + if (!chan) { + err = -EFAULT; + goto unlock; + } + } + + err = 0; + + l2cap_chan_lock(chan); + + switch (result) { + case L2CAP_CR_SUCCESS: + l2cap_state_change(chan, BT_CONFIG); + chan->ident = 0; + chan->dcid = dcid; + clear_bit(CONF_CONNECT_PEND, &chan->conf_state); + + if (test_and_set_bit(CONF_REQ_SENT, &chan->conf_state)) + break; + + l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ, + l2cap_build_conf_req(chan, req), req); + chan->num_conf_req++; + break; + + case L2CAP_CR_PEND: + set_bit(CONF_CONNECT_PEND, &chan->conf_state); + break; + + default: + l2cap_chan_del(chan, ECONNREFUSED); + break; + } + + l2cap_chan_unlock(chan); + +unlock: + mutex_unlock(&conn->chan_lock); + + return err; +} + +static inline void set_default_fcs(struct l2cap_chan *chan) +{ + /* FCS is enabled only in ERTM or streaming mode, if one or both + * sides request it. + */ + if (chan->mode != L2CAP_MODE_ERTM && chan->mode != L2CAP_MODE_STREAMING) + chan->fcs = L2CAP_FCS_NONE; + else if (!test_bit(CONF_NO_FCS_RECV, &chan->conf_state)) + chan->fcs = L2CAP_FCS_CRC16; +} + +static inline int l2cap_config_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u16 cmd_len, u8 *data) +{ + struct l2cap_conf_req *req = (struct l2cap_conf_req *) data; + u16 dcid, flags; + u8 rsp[64]; + struct l2cap_chan *chan; + int len; + + dcid = __le16_to_cpu(req->dcid); + flags = __le16_to_cpu(req->flags); + + BT_DBG("dcid 0x%4.4x flags 0x%2.2x", dcid, flags); + + chan = l2cap_get_chan_by_scid(conn, dcid); + if (!chan) + return -ENOENT; + + if (chan->state != BT_CONFIG && chan->state != BT_CONNECT2) { + struct l2cap_cmd_rej_cid rej; + + rej.reason = cpu_to_le16(L2CAP_REJ_INVALID_CID); + rej.scid = cpu_to_le16(chan->scid); + rej.dcid = cpu_to_le16(chan->dcid); + + l2cap_send_cmd(conn, cmd->ident, L2CAP_COMMAND_REJ, + sizeof(rej), &rej); + goto unlock; + } + + /* Reject if config buffer is too small. */ + len = cmd_len - sizeof(*req); + if (len < 0 || chan->conf_len + len > sizeof(chan->conf_req)) { + l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP, + l2cap_build_conf_rsp(chan, rsp, + L2CAP_CONF_REJECT, flags), rsp); + goto unlock; + } + + /* Store config. */ + memcpy(chan->conf_req + chan->conf_len, req->data, len); + chan->conf_len += len; + + if (flags & 0x0001) { + /* Incomplete config. Send empty response. */ + l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP, + l2cap_build_conf_rsp(chan, rsp, + L2CAP_CONF_SUCCESS, 0x0001), rsp); + goto unlock; + } + + /* Complete config. */ + len = l2cap_parse_conf_req(chan, rsp); + if (len < 0) { + l2cap_send_disconn_req(conn, chan, ECONNRESET); + goto unlock; + } + + l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP, len, rsp); + chan->num_conf_rsp++; + + /* Reset config buffer. */ + chan->conf_len = 0; + + if (!test_bit(CONF_OUTPUT_DONE, &chan->conf_state)) + goto unlock; + + if (test_bit(CONF_INPUT_DONE, &chan->conf_state)) { + set_default_fcs(chan); + + l2cap_state_change(chan, BT_CONNECTED); + + chan->next_tx_seq = 0; + chan->expected_tx_seq = 0; + skb_queue_head_init(&chan->tx_q); + if (chan->mode == L2CAP_MODE_ERTM) + l2cap_ertm_init(chan); + + l2cap_chan_ready(chan); + goto unlock; + } + + if (!test_and_set_bit(CONF_REQ_SENT, &chan->conf_state)) { + u8 buf[64]; + l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ, + l2cap_build_conf_req(chan, buf), buf); + chan->num_conf_req++; + } + + /* Got Conf Rsp PENDING from remote side and asume we sent + Conf Rsp PENDING in the code above */ + if (test_bit(CONF_REM_CONF_PEND, &chan->conf_state) && + test_bit(CONF_LOC_CONF_PEND, &chan->conf_state)) { + + /* check compatibility */ + + clear_bit(CONF_LOC_CONF_PEND, &chan->conf_state); + set_bit(CONF_OUTPUT_DONE, &chan->conf_state); + + l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP, + l2cap_build_conf_rsp(chan, rsp, + L2CAP_CONF_SUCCESS, 0x0000), rsp); + } + +unlock: + l2cap_chan_unlock(chan); + return 0; +} + +static inline int l2cap_config_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data) +{ + struct l2cap_conf_rsp *rsp = (struct l2cap_conf_rsp *)data; + u16 scid, flags, result; + struct l2cap_chan *chan; + int len = cmd->len - sizeof(*rsp); + + scid = __le16_to_cpu(rsp->scid); + flags = __le16_to_cpu(rsp->flags); + result = __le16_to_cpu(rsp->result); + + BT_DBG("scid 0x%4.4x flags 0x%2.2x result 0x%2.2x", + scid, flags, result); + + chan = l2cap_get_chan_by_scid(conn, scid); + if (!chan) + return 0; + + switch (result) { + case L2CAP_CONF_SUCCESS: + l2cap_conf_rfc_get(chan, rsp->data, len); + clear_bit(CONF_REM_CONF_PEND, &chan->conf_state); + break; + + case L2CAP_CONF_PENDING: + set_bit(CONF_REM_CONF_PEND, &chan->conf_state); + + if (test_bit(CONF_LOC_CONF_PEND, &chan->conf_state)) { + char buf[64]; + + len = l2cap_parse_conf_rsp(chan, rsp->data, len, + buf, &result); + if (len < 0) { + l2cap_send_disconn_req(conn, chan, ECONNRESET); + goto done; + } + + /* check compatibility */ + + clear_bit(CONF_LOC_CONF_PEND, &chan->conf_state); + set_bit(CONF_OUTPUT_DONE, &chan->conf_state); + + l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP, + l2cap_build_conf_rsp(chan, buf, + L2CAP_CONF_SUCCESS, 0x0000), buf); + } + goto done; + + case L2CAP_CONF_UNACCEPT: + if (chan->num_conf_rsp <= L2CAP_CONF_MAX_CONF_RSP) { + char req[64]; + + if (len > sizeof(req) - sizeof(struct l2cap_conf_req)) { + l2cap_send_disconn_req(conn, chan, ECONNRESET); + goto done; + } + + /* throw out any old stored conf requests */ + result = L2CAP_CONF_SUCCESS; + len = l2cap_parse_conf_rsp(chan, rsp->data, len, + req, &result); + if (len < 0) { + l2cap_send_disconn_req(conn, chan, ECONNRESET); + goto done; + } + + l2cap_send_cmd(conn, l2cap_get_ident(conn), + L2CAP_CONF_REQ, len, req); + chan->num_conf_req++; + if (result != L2CAP_CONF_SUCCESS) + goto done; + break; + } + + default: + l2cap_chan_set_err(chan, ECONNRESET); + + __set_chan_timer(chan, L2CAP_DISC_REJ_TIMEOUT); + l2cap_send_disconn_req(conn, chan, ECONNRESET); + goto done; + } + + if (flags & 0x01) + goto done; + + set_bit(CONF_INPUT_DONE, &chan->conf_state); + + if (test_bit(CONF_OUTPUT_DONE, &chan->conf_state)) { + set_default_fcs(chan); + + l2cap_state_change(chan, BT_CONNECTED); + chan->next_tx_seq = 0; + chan->expected_tx_seq = 0; + skb_queue_head_init(&chan->tx_q); + if (chan->mode == L2CAP_MODE_ERTM) + l2cap_ertm_init(chan); + + l2cap_chan_ready(chan); + } + +done: + l2cap_chan_unlock(chan); + return 0; +} + +static inline int l2cap_disconnect_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data) +{ + struct l2cap_disconn_req *req = (struct l2cap_disconn_req *) data; + struct l2cap_disconn_rsp rsp; + u16 dcid, scid; + struct l2cap_chan *chan; + 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); + + mutex_lock(&conn->chan_lock); + + chan = __l2cap_get_chan_by_scid(conn, dcid); + if (!chan) { + mutex_unlock(&conn->chan_lock); + return 0; + } + + l2cap_chan_lock(chan); + + sk = chan->sk; + + rsp.dcid = cpu_to_le16(chan->scid); + rsp.scid = cpu_to_le16(chan->dcid); + l2cap_send_cmd(conn, cmd->ident, L2CAP_DISCONN_RSP, sizeof(rsp), &rsp); + + lock_sock(sk); + sk->sk_shutdown = SHUTDOWN_MASK; + release_sock(sk); + + l2cap_chan_del(chan, ECONNRESET); + + l2cap_chan_unlock(chan); + + chan->ops->close(chan->data); + + mutex_unlock(&conn->chan_lock); + + return 0; +} + +static inline int l2cap_disconnect_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data) +{ + struct l2cap_disconn_rsp *rsp = (struct l2cap_disconn_rsp *) data; + u16 dcid, scid; + struct l2cap_chan *chan; + + scid = __le16_to_cpu(rsp->scid); + dcid = __le16_to_cpu(rsp->dcid); + + BT_DBG("dcid 0x%4.4x scid 0x%4.4x", dcid, scid); + + mutex_lock(&conn->chan_lock); + + chan = __l2cap_get_chan_by_scid(conn, scid); + if (!chan) { + mutex_unlock(&conn->chan_lock); + return 0; + } + + l2cap_chan_lock(chan); + + l2cap_chan_del(chan, 0); + + l2cap_chan_unlock(chan); + + chan->ops->close(chan->data); + + mutex_unlock(&conn->chan_lock); + + return 0; +} + +static inline int l2cap_information_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data) +{ + struct l2cap_info_req *req = (struct l2cap_info_req *) data; + u16 type; + + type = __le16_to_cpu(req->type); + + BT_DBG("type 0x%4.4x", type); + + if (type == L2CAP_IT_FEAT_MASK) { + u8 buf[8]; + u32 feat_mask = l2cap_feat_mask; + struct l2cap_info_rsp *rsp = (struct l2cap_info_rsp *) buf; + rsp->type = cpu_to_le16(L2CAP_IT_FEAT_MASK); + rsp->result = cpu_to_le16(L2CAP_IR_SUCCESS); + if (!disable_ertm) + feat_mask |= L2CAP_FEAT_ERTM | L2CAP_FEAT_STREAMING + | L2CAP_FEAT_FCS; + if (enable_hs) + feat_mask |= L2CAP_FEAT_EXT_FLOW + | L2CAP_FEAT_EXT_WINDOW; + + put_unaligned_le32(feat_mask, rsp->data); + l2cap_send_cmd(conn, cmd->ident, + L2CAP_INFO_RSP, sizeof(buf), buf); + } else if (type == L2CAP_IT_FIXED_CHAN) { + u8 buf[12]; + struct l2cap_info_rsp *rsp = (struct l2cap_info_rsp *) buf; + + if (enable_hs) + l2cap_fixed_chan[0] |= L2CAP_FC_A2MP; + else + l2cap_fixed_chan[0] &= ~L2CAP_FC_A2MP; + + rsp->type = cpu_to_le16(L2CAP_IT_FIXED_CHAN); + rsp->result = cpu_to_le16(L2CAP_IR_SUCCESS); + memcpy(rsp->data, l2cap_fixed_chan, sizeof(l2cap_fixed_chan)); + l2cap_send_cmd(conn, cmd->ident, + L2CAP_INFO_RSP, sizeof(buf), buf); + } else { + struct l2cap_info_rsp rsp; + rsp.type = cpu_to_le16(type); + rsp.result = cpu_to_le16(L2CAP_IR_NOTSUPP); + l2cap_send_cmd(conn, cmd->ident, + L2CAP_INFO_RSP, sizeof(rsp), &rsp); + } + + return 0; +} + +static inline int l2cap_information_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data) +{ + struct l2cap_info_rsp *rsp = (struct l2cap_info_rsp *) data; + u16 type, result; + + type = __le16_to_cpu(rsp->type); + result = __le16_to_cpu(rsp->result); + + BT_DBG("type 0x%4.4x result 0x%2.2x", type, result); + + /* L2CAP Info req/rsp are unbound to channels, add extra checks */ + if (cmd->ident != conn->info_ident || + conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_DONE) + return 0; + + cancel_delayed_work(&conn->info_timer); + + if (result != L2CAP_IR_SUCCESS) { + conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE; + conn->info_ident = 0; + + l2cap_conn_start(conn); + + return 0; + } + + switch (type) { + case L2CAP_IT_FEAT_MASK: + conn->feat_mask = get_unaligned_le32(rsp->data); + + if (conn->feat_mask & L2CAP_FEAT_FIXED_CHAN) { + struct l2cap_info_req req; + req.type = cpu_to_le16(L2CAP_IT_FIXED_CHAN); + + conn->info_ident = l2cap_get_ident(conn); + + l2cap_send_cmd(conn, conn->info_ident, + L2CAP_INFO_REQ, sizeof(req), &req); + } else { + conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE; + conn->info_ident = 0; + + l2cap_conn_start(conn); + } + break; + + case L2CAP_IT_FIXED_CHAN: + conn->fixed_chan_mask = rsp->data[0]; + conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE; + conn->info_ident = 0; + + l2cap_conn_start(conn); + break; + } + + return 0; +} + +static inline int l2cap_create_channel_req(struct l2cap_conn *conn, + struct l2cap_cmd_hdr *cmd, u16 cmd_len, + void *data) +{ + struct l2cap_create_chan_req *req = data; + struct l2cap_create_chan_rsp rsp; + u16 psm, scid; + + if (cmd_len != sizeof(*req)) + return -EPROTO; + + if (!enable_hs) + return -EINVAL; + + psm = le16_to_cpu(req->psm); + scid = le16_to_cpu(req->scid); + + BT_DBG("psm %d, scid %d, amp_id %d", psm, scid, req->amp_id); + + /* Placeholder: Always reject */ + rsp.dcid = 0; + rsp.scid = cpu_to_le16(scid); + rsp.result = L2CAP_CR_NO_MEM; + rsp.status = L2CAP_CS_NO_INFO; + + l2cap_send_cmd(conn, cmd->ident, L2CAP_CREATE_CHAN_RSP, + sizeof(rsp), &rsp); + + return 0; +} + +static inline int l2cap_create_channel_rsp(struct l2cap_conn *conn, + struct l2cap_cmd_hdr *cmd, void *data) +{ + BT_DBG("conn %p", conn); + + return l2cap_connect_rsp(conn, cmd, data); +} + +static void l2cap_send_move_chan_rsp(struct l2cap_conn *conn, u8 ident, + u16 icid, u16 result) +{ + struct l2cap_move_chan_rsp rsp; + + BT_DBG("icid %d, result %d", icid, result); + + rsp.icid = cpu_to_le16(icid); + rsp.result = cpu_to_le16(result); + + l2cap_send_cmd(conn, ident, L2CAP_MOVE_CHAN_RSP, sizeof(rsp), &rsp); +} + +static void l2cap_send_move_chan_cfm(struct l2cap_conn *conn, + struct l2cap_chan *chan, u16 icid, u16 result) +{ + struct l2cap_move_chan_cfm cfm; + u8 ident; + + BT_DBG("icid %d, result %d", icid, result); + + ident = l2cap_get_ident(conn); + if (chan) + chan->ident = ident; + + cfm.icid = cpu_to_le16(icid); + cfm.result = cpu_to_le16(result); + + l2cap_send_cmd(conn, ident, L2CAP_MOVE_CHAN_CFM, sizeof(cfm), &cfm); +} + +static void l2cap_send_move_chan_cfm_rsp(struct l2cap_conn *conn, u8 ident, + u16 icid) +{ + struct l2cap_move_chan_cfm_rsp rsp; + + BT_DBG("icid %d", icid); + + rsp.icid = cpu_to_le16(icid); + l2cap_send_cmd(conn, ident, L2CAP_MOVE_CHAN_CFM_RSP, sizeof(rsp), &rsp); +} + +static inline int l2cap_move_channel_req(struct l2cap_conn *conn, + struct l2cap_cmd_hdr *cmd, u16 cmd_len, void *data) +{ + struct l2cap_move_chan_req *req = data; + u16 icid = 0; + u16 result = L2CAP_MR_NOT_ALLOWED; + + if (cmd_len != sizeof(*req)) + return -EPROTO; + + icid = le16_to_cpu(req->icid); + + BT_DBG("icid %d, dest_amp_id %d", icid, req->dest_amp_id); + + if (!enable_hs) + return -EINVAL; + + /* Placeholder: Always refuse */ + l2cap_send_move_chan_rsp(conn, cmd->ident, icid, result); + + return 0; +} + +static inline int l2cap_move_channel_rsp(struct l2cap_conn *conn, + struct l2cap_cmd_hdr *cmd, u16 cmd_len, void *data) +{ + struct l2cap_move_chan_rsp *rsp = data; + u16 icid, result; + + if (cmd_len != sizeof(*rsp)) + return -EPROTO; + + icid = le16_to_cpu(rsp->icid); + result = le16_to_cpu(rsp->result); + + BT_DBG("icid %d, result %d", icid, result); + + /* Placeholder: Always unconfirmed */ + l2cap_send_move_chan_cfm(conn, NULL, icid, L2CAP_MC_UNCONFIRMED); + + return 0; +} + +static inline int l2cap_move_channel_confirm(struct l2cap_conn *conn, + struct l2cap_cmd_hdr *cmd, u16 cmd_len, void *data) +{ + struct l2cap_move_chan_cfm *cfm = data; + u16 icid, result; + + if (cmd_len != sizeof(*cfm)) + return -EPROTO; + + icid = le16_to_cpu(cfm->icid); + result = le16_to_cpu(cfm->result); + + BT_DBG("icid %d, result %d", icid, result); + + l2cap_send_move_chan_cfm_rsp(conn, cmd->ident, icid); + + return 0; +} + +static inline int l2cap_move_channel_confirm_rsp(struct l2cap_conn *conn, + struct l2cap_cmd_hdr *cmd, u16 cmd_len, void *data) +{ + struct l2cap_move_chan_cfm_rsp *rsp = data; + u16 icid; + + if (cmd_len != sizeof(*rsp)) + return -EPROTO; + + icid = le16_to_cpu(rsp->icid); + + BT_DBG("icid %d", icid); + + return 0; +} + +static inline int l2cap_check_conn_param(u16 min, u16 max, u16 latency, + u16 to_multiplier) +{ + u16 max_latency; + + if (min > max || min < 6 || max > 3200) + return -EINVAL; + + if (to_multiplier < 10 || to_multiplier > 3200) + return -EINVAL; + + if (max >= to_multiplier * 8) + return -EINVAL; + + max_latency = (to_multiplier * 8 / max) - 1; + if (latency > 499 || latency > max_latency) + return -EINVAL; + + return 0; +} + +static inline int l2cap_conn_param_update_req(struct l2cap_conn *conn, + struct l2cap_cmd_hdr *cmd, u8 *data) +{ + struct hci_conn *hcon = conn->hcon; + struct l2cap_conn_param_update_req *req; + struct l2cap_conn_param_update_rsp rsp; + u16 min, max, latency, to_multiplier, cmd_len; + int err; + + if (!(hcon->link_mode & HCI_LM_MASTER)) + return -EINVAL; + + cmd_len = __le16_to_cpu(cmd->len); + if (cmd_len != sizeof(struct l2cap_conn_param_update_req)) + return -EPROTO; + + req = (struct l2cap_conn_param_update_req *) data; + min = __le16_to_cpu(req->min); + max = __le16_to_cpu(req->max); + latency = __le16_to_cpu(req->latency); + to_multiplier = __le16_to_cpu(req->to_multiplier); + + BT_DBG("min 0x%4.4x max 0x%4.4x latency: 0x%4.4x Timeout: 0x%4.4x", + min, max, latency, to_multiplier); + + memset(&rsp, 0, sizeof(rsp)); + + err = l2cap_check_conn_param(min, max, latency, to_multiplier); + if (err) + rsp.result = cpu_to_le16(L2CAP_CONN_PARAM_REJECTED); + else + rsp.result = cpu_to_le16(L2CAP_CONN_PARAM_ACCEPTED); + + l2cap_send_cmd(conn, cmd->ident, L2CAP_CONN_PARAM_UPDATE_RSP, + sizeof(rsp), &rsp); + + if (!err) + hci_le_conn_update(hcon, min, max, latency, to_multiplier); + + return 0; +} + +static inline int l2cap_bredr_sig_cmd(struct l2cap_conn *conn, + struct l2cap_cmd_hdr *cmd, u16 cmd_len, u8 *data) +{ + int err = 0; + + switch (cmd->code) { + case L2CAP_COMMAND_REJ: + l2cap_command_rej(conn, cmd, data); + break; + + 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, cmd_len, 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_ECHO_REQ: + l2cap_send_cmd(conn, cmd->ident, L2CAP_ECHO_RSP, cmd_len, data); + break; + + case L2CAP_ECHO_RSP: + break; + + case L2CAP_INFO_REQ: + err = l2cap_information_req(conn, cmd, data); + break; + + case L2CAP_INFO_RSP: + err = l2cap_information_rsp(conn, cmd, data); + break; + + case L2CAP_CREATE_CHAN_REQ: + err = l2cap_create_channel_req(conn, cmd, cmd_len, data); + break; + + case L2CAP_CREATE_CHAN_RSP: + err = l2cap_create_channel_rsp(conn, cmd, data); + break; + + case L2CAP_MOVE_CHAN_REQ: + err = l2cap_move_channel_req(conn, cmd, cmd_len, data); + break; + + case L2CAP_MOVE_CHAN_RSP: + err = l2cap_move_channel_rsp(conn, cmd, cmd_len, data); + break; + + case L2CAP_MOVE_CHAN_CFM: + err = l2cap_move_channel_confirm(conn, cmd, cmd_len, data); + break; + + case L2CAP_MOVE_CHAN_CFM_RSP: + err = l2cap_move_channel_confirm_rsp(conn, cmd, cmd_len, data); + break; + + default: + BT_ERR("Unknown BR/EDR signaling command 0x%2.2x", cmd->code); + err = -EINVAL; + break; + } + + return err; +} + +static inline int l2cap_le_sig_cmd(struct l2cap_conn *conn, + struct l2cap_cmd_hdr *cmd, u8 *data) +{ + switch (cmd->code) { + case L2CAP_COMMAND_REJ: + return 0; + + case L2CAP_CONN_PARAM_UPDATE_REQ: + return l2cap_conn_param_update_req(conn, cmd, data); + + case L2CAP_CONN_PARAM_UPDATE_RSP: + return 0; + + default: + BT_ERR("Unknown LE signaling command 0x%2.2x", cmd->code); + return -EINVAL; + } +} + +static inline void l2cap_sig_channel(struct l2cap_conn *conn, + struct sk_buff *skb) +{ + u8 *data = skb->data; + int len = skb->len; + struct l2cap_cmd_hdr cmd; + int err; + + l2cap_raw_recv(conn, skb); + + while (len >= L2CAP_CMD_HDR_SIZE) { + u16 cmd_len; + 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; + } + + if (conn->hcon->type == LE_LINK) + err = l2cap_le_sig_cmd(conn, &cmd, data); + else + err = l2cap_bredr_sig_cmd(conn, &cmd, cmd_len, data); + + if (err) { + struct l2cap_cmd_rej_unk rej; + + BT_ERR("Wrong link type (%d)", err); + + /* FIXME: Map err to a valid reason */ + rej.reason = cpu_to_le16(L2CAP_REJ_NOT_UNDERSTOOD); + l2cap_send_cmd(conn, cmd.ident, L2CAP_COMMAND_REJ, sizeof(rej), &rej); + } + + data += cmd_len; + len -= cmd_len; + } + + kfree_skb(skb); +} + +static int l2cap_check_fcs(struct l2cap_chan *chan, struct sk_buff *skb) +{ + u16 our_fcs, rcv_fcs; + int hdr_size; + + if (test_bit(FLAG_EXT_CTRL, &chan->flags)) + hdr_size = L2CAP_EXT_HDR_SIZE; + else + hdr_size = L2CAP_ENH_HDR_SIZE; + + if (chan->fcs == L2CAP_FCS_CRC16) { + skb_trim(skb, skb->len - L2CAP_FCS_SIZE); + rcv_fcs = get_unaligned_le16(skb->data + skb->len); + our_fcs = crc16(0, skb->data - hdr_size, skb->len + hdr_size); + + if (our_fcs != rcv_fcs) + return -EBADMSG; + } + return 0; +} + +static inline void l2cap_send_i_or_rr_or_rnr(struct l2cap_chan *chan) +{ + u32 control = 0; + + chan->frames_sent = 0; + + control |= __set_reqseq(chan, chan->buffer_seq); + + if (test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) { + control |= __set_ctrl_super(chan, L2CAP_SUPER_RNR); + l2cap_send_sframe(chan, control); + set_bit(CONN_RNR_SENT, &chan->conn_state); + } + + if (test_bit(CONN_REMOTE_BUSY, &chan->conn_state)) + l2cap_retransmit_frames(chan); + + l2cap_ertm_send(chan); + + if (!test_bit(CONN_LOCAL_BUSY, &chan->conn_state) && + chan->frames_sent == 0) { + control |= __set_ctrl_super(chan, L2CAP_SUPER_RR); + l2cap_send_sframe(chan, control); + } +} + +static int l2cap_add_to_srej_queue(struct l2cap_chan *chan, struct sk_buff *skb, u16 tx_seq, u8 sar) +{ + struct sk_buff *next_skb; + int tx_seq_offset, next_tx_seq_offset; + + bt_cb(skb)->tx_seq = tx_seq; + bt_cb(skb)->sar = sar; + + next_skb = skb_peek(&chan->srej_q); + + tx_seq_offset = __seq_offset(chan, tx_seq, chan->buffer_seq); + + while (next_skb) { + if (bt_cb(next_skb)->tx_seq == tx_seq) + return -EINVAL; + + next_tx_seq_offset = __seq_offset(chan, + bt_cb(next_skb)->tx_seq, chan->buffer_seq); + + if (next_tx_seq_offset > tx_seq_offset) { + __skb_queue_before(&chan->srej_q, next_skb, skb); + return 0; + } + + if (skb_queue_is_last(&chan->srej_q, next_skb)) + next_skb = NULL; + else + next_skb = skb_queue_next(&chan->srej_q, next_skb); + } + + __skb_queue_tail(&chan->srej_q, skb); + + return 0; +} + +static void append_skb_frag(struct sk_buff *skb, + struct sk_buff *new_frag, struct sk_buff **last_frag) +{ + /* skb->len reflects data in skb as well as all fragments + * skb->data_len reflects only data in fragments + */ + if (!skb_has_frag_list(skb)) + skb_shinfo(skb)->frag_list = new_frag; + + new_frag->next = NULL; + + (*last_frag)->next = new_frag; + *last_frag = new_frag; + + skb->len += new_frag->len; + skb->data_len += new_frag->len; + skb->truesize += new_frag->truesize; +} + +static int l2cap_reassemble_sdu(struct l2cap_chan *chan, struct sk_buff *skb, u32 control) +{ + int err = -EINVAL; + + switch (__get_ctrl_sar(chan, control)) { + case L2CAP_SAR_UNSEGMENTED: + if (chan->sdu) + break; + + err = chan->ops->recv(chan->data, skb); + break; + + case L2CAP_SAR_START: + if (chan->sdu) + break; + + chan->sdu_len = get_unaligned_le16(skb->data); + skb_pull(skb, L2CAP_SDULEN_SIZE); + + if (chan->sdu_len > chan->imtu) { + err = -EMSGSIZE; + break; + } + + if (skb->len >= chan->sdu_len) + break; + + chan->sdu = skb; + chan->sdu_last_frag = skb; + + skb = NULL; + err = 0; + break; + + case L2CAP_SAR_CONTINUE: + if (!chan->sdu) + break; + + append_skb_frag(chan->sdu, skb, + &chan->sdu_last_frag); + skb = NULL; + + if (chan->sdu->len >= chan->sdu_len) + break; + + err = 0; + break; + + case L2CAP_SAR_END: + if (!chan->sdu) + break; + + append_skb_frag(chan->sdu, skb, + &chan->sdu_last_frag); + skb = NULL; + + if (chan->sdu->len != chan->sdu_len) + break; + + err = chan->ops->recv(chan->data, chan->sdu); + + if (!err) { + /* Reassembly complete */ + chan->sdu = NULL; + chan->sdu_last_frag = NULL; + chan->sdu_len = 0; + } + break; + } + + if (err) { + kfree_skb(skb); + kfree_skb(chan->sdu); + chan->sdu = NULL; + chan->sdu_last_frag = NULL; + chan->sdu_len = 0; + } + + return err; +} + +static void l2cap_ertm_enter_local_busy(struct l2cap_chan *chan) +{ + BT_DBG("chan %p, Enter local busy", chan); + + set_bit(CONN_LOCAL_BUSY, &chan->conn_state); + + __set_ack_timer(chan); +} + +static void l2cap_ertm_exit_local_busy(struct l2cap_chan *chan) +{ + u32 control; + + if (!test_bit(CONN_RNR_SENT, &chan->conn_state)) + goto done; + + control = __set_reqseq(chan, chan->buffer_seq); + control |= __set_ctrl_poll(chan); + control |= __set_ctrl_super(chan, L2CAP_SUPER_RR); + l2cap_send_sframe(chan, control); + chan->retry_count = 1; + + __clear_retrans_timer(chan); + __set_monitor_timer(chan); + + set_bit(CONN_WAIT_F, &chan->conn_state); + +done: + clear_bit(CONN_LOCAL_BUSY, &chan->conn_state); + clear_bit(CONN_RNR_SENT, &chan->conn_state); + + BT_DBG("chan %p, Exit local busy", chan); +} + +void l2cap_chan_busy(struct l2cap_chan *chan, int busy) +{ + if (chan->mode == L2CAP_MODE_ERTM) { + if (busy) + l2cap_ertm_enter_local_busy(chan); + else + l2cap_ertm_exit_local_busy(chan); + } +} + +static void l2cap_check_srej_gap(struct l2cap_chan *chan, u16 tx_seq) +{ + struct sk_buff *skb; + u32 control; + + while ((skb = skb_peek(&chan->srej_q)) && + !test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) { + int err; + + if (bt_cb(skb)->tx_seq != tx_seq) + break; + + skb = skb_dequeue(&chan->srej_q); + control = __set_ctrl_sar(chan, bt_cb(skb)->sar); + err = l2cap_reassemble_sdu(chan, skb, control); + + if (err < 0) { + l2cap_send_disconn_req(chan->conn, chan, ECONNRESET); + break; + } + + chan->buffer_seq_srej = __next_seq(chan, chan->buffer_seq_srej); + tx_seq = __next_seq(chan, tx_seq); + } +} + +static void l2cap_resend_srejframe(struct l2cap_chan *chan, u16 tx_seq) +{ + struct srej_list *l, *tmp; + u32 control; + + list_for_each_entry_safe(l, tmp, &chan->srej_l, list) { + if (l->tx_seq == tx_seq) { + list_del(&l->list); + kfree(l); + return; + } + control = __set_ctrl_super(chan, L2CAP_SUPER_SREJ); + control |= __set_reqseq(chan, l->tx_seq); + l2cap_send_sframe(chan, control); + list_del(&l->list); + list_add_tail(&l->list, &chan->srej_l); + } +} + +static int l2cap_send_srejframe(struct l2cap_chan *chan, u16 tx_seq) +{ + struct srej_list *new; + u32 control; + + while (tx_seq != chan->expected_tx_seq) { + control = __set_ctrl_super(chan, L2CAP_SUPER_SREJ); + control |= __set_reqseq(chan, chan->expected_tx_seq); + l2cap_send_sframe(chan, control); + + new = kzalloc(sizeof(struct srej_list), GFP_ATOMIC); + if (!new) + return -ENOMEM; + + new->tx_seq = chan->expected_tx_seq; + + chan->expected_tx_seq = __next_seq(chan, chan->expected_tx_seq); + + list_add_tail(&new->list, &chan->srej_l); + } + + chan->expected_tx_seq = __next_seq(chan, chan->expected_tx_seq); + + return 0; +} + +static inline int l2cap_data_channel_iframe(struct l2cap_chan *chan, u32 rx_control, struct sk_buff *skb) +{ + u16 tx_seq = __get_txseq(chan, rx_control); + u16 req_seq = __get_reqseq(chan, rx_control); + u8 sar = __get_ctrl_sar(chan, rx_control); + int tx_seq_offset, expected_tx_seq_offset; + int num_to_ack = (chan->tx_win/6) + 1; + int err = 0; + + BT_DBG("chan %p len %d tx_seq %d rx_control 0x%8.8x", chan, skb->len, + tx_seq, rx_control); + + if (__is_ctrl_final(chan, rx_control) && + test_bit(CONN_WAIT_F, &chan->conn_state)) { + __clear_monitor_timer(chan); + if (chan->unacked_frames > 0) + __set_retrans_timer(chan); + clear_bit(CONN_WAIT_F, &chan->conn_state); + } + + chan->expected_ack_seq = req_seq; + l2cap_drop_acked_frames(chan); + + tx_seq_offset = __seq_offset(chan, tx_seq, chan->buffer_seq); + + /* invalid tx_seq */ + if (tx_seq_offset >= chan->tx_win) { + l2cap_send_disconn_req(chan->conn, chan, ECONNRESET); + goto drop; + } + + if (test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) { + if (!test_bit(CONN_RNR_SENT, &chan->conn_state)) + l2cap_send_ack(chan); + goto drop; + } + + if (tx_seq == chan->expected_tx_seq) + goto expected; + + if (test_bit(CONN_SREJ_SENT, &chan->conn_state)) { + struct srej_list *first; + + first = list_first_entry(&chan->srej_l, + struct srej_list, list); + if (tx_seq == first->tx_seq) { + l2cap_add_to_srej_queue(chan, skb, tx_seq, sar); + l2cap_check_srej_gap(chan, tx_seq); + + list_del(&first->list); + kfree(first); + + if (list_empty(&chan->srej_l)) { + chan->buffer_seq = chan->buffer_seq_srej; + clear_bit(CONN_SREJ_SENT, &chan->conn_state); + l2cap_send_ack(chan); + BT_DBG("chan %p, Exit SREJ_SENT", chan); + } + } else { + struct srej_list *l; + + /* duplicated tx_seq */ + if (l2cap_add_to_srej_queue(chan, skb, tx_seq, sar) < 0) + goto drop; + + list_for_each_entry(l, &chan->srej_l, list) { + if (l->tx_seq == tx_seq) { + l2cap_resend_srejframe(chan, tx_seq); + return 0; + } + } + + err = l2cap_send_srejframe(chan, tx_seq); + if (err < 0) { + l2cap_send_disconn_req(chan->conn, chan, -err); + return err; + } + } + } else { + expected_tx_seq_offset = __seq_offset(chan, + chan->expected_tx_seq, chan->buffer_seq); + + /* duplicated tx_seq */ + if (tx_seq_offset < expected_tx_seq_offset) + goto drop; + + set_bit(CONN_SREJ_SENT, &chan->conn_state); + + BT_DBG("chan %p, Enter SREJ", chan); + + INIT_LIST_HEAD(&chan->srej_l); + chan->buffer_seq_srej = chan->buffer_seq; + + __skb_queue_head_init(&chan->srej_q); + l2cap_add_to_srej_queue(chan, skb, tx_seq, sar); + + /* Set P-bit only if there are some I-frames to ack. */ + if (__clear_ack_timer(chan)) + set_bit(CONN_SEND_PBIT, &chan->conn_state); + + err = l2cap_send_srejframe(chan, tx_seq); + if (err < 0) { + l2cap_send_disconn_req(chan->conn, chan, -err); + return err; + } + } + return 0; + +expected: + chan->expected_tx_seq = __next_seq(chan, chan->expected_tx_seq); + + if (test_bit(CONN_SREJ_SENT, &chan->conn_state)) { + bt_cb(skb)->tx_seq = tx_seq; + bt_cb(skb)->sar = sar; + __skb_queue_tail(&chan->srej_q, skb); + return 0; + } + + err = l2cap_reassemble_sdu(chan, skb, rx_control); + chan->buffer_seq = __next_seq(chan, chan->buffer_seq); + + if (err < 0) { + l2cap_send_disconn_req(chan->conn, chan, ECONNRESET); + return err; + } + + if (__is_ctrl_final(chan, rx_control)) { + if (!test_and_clear_bit(CONN_REJ_ACT, &chan->conn_state)) + l2cap_retransmit_frames(chan); + } + + + chan->num_acked = (chan->num_acked + 1) % num_to_ack; + if (chan->num_acked == num_to_ack - 1) + l2cap_send_ack(chan); + else + __set_ack_timer(chan); + + return 0; + +drop: + kfree_skb(skb); + return 0; +} + +static inline void l2cap_data_channel_rrframe(struct l2cap_chan *chan, u32 rx_control) +{ + BT_DBG("chan %p, req_seq %d ctrl 0x%8.8x", chan, + __get_reqseq(chan, rx_control), rx_control); + + chan->expected_ack_seq = __get_reqseq(chan, rx_control); + l2cap_drop_acked_frames(chan); + + if (__is_ctrl_poll(chan, rx_control)) { + set_bit(CONN_SEND_FBIT, &chan->conn_state); + if (test_bit(CONN_SREJ_SENT, &chan->conn_state)) { + if (test_bit(CONN_REMOTE_BUSY, &chan->conn_state) && + (chan->unacked_frames > 0)) + __set_retrans_timer(chan); + + clear_bit(CONN_REMOTE_BUSY, &chan->conn_state); + l2cap_send_srejtail(chan); + } else { + l2cap_send_i_or_rr_or_rnr(chan); + } + + } else if (__is_ctrl_final(chan, rx_control)) { + clear_bit(CONN_REMOTE_BUSY, &chan->conn_state); + + if (!test_and_clear_bit(CONN_REJ_ACT, &chan->conn_state)) + l2cap_retransmit_frames(chan); + + } else { + if (test_bit(CONN_REMOTE_BUSY, &chan->conn_state) && + (chan->unacked_frames > 0)) + __set_retrans_timer(chan); + + clear_bit(CONN_REMOTE_BUSY, &chan->conn_state); + if (test_bit(CONN_SREJ_SENT, &chan->conn_state)) + l2cap_send_ack(chan); + else + l2cap_ertm_send(chan); + } +} + +static inline void l2cap_data_channel_rejframe(struct l2cap_chan *chan, u32 rx_control) +{ + u16 tx_seq = __get_reqseq(chan, rx_control); + + BT_DBG("chan %p, req_seq %d ctrl 0x%8.8x", chan, tx_seq, rx_control); + + clear_bit(CONN_REMOTE_BUSY, &chan->conn_state); + + chan->expected_ack_seq = tx_seq; + l2cap_drop_acked_frames(chan); + + if (__is_ctrl_final(chan, rx_control)) { + if (!test_and_clear_bit(CONN_REJ_ACT, &chan->conn_state)) + l2cap_retransmit_frames(chan); + } else { + l2cap_retransmit_frames(chan); + + if (test_bit(CONN_WAIT_F, &chan->conn_state)) + set_bit(CONN_REJ_ACT, &chan->conn_state); + } +} +static inline void l2cap_data_channel_srejframe(struct l2cap_chan *chan, u32 rx_control) +{ + u16 tx_seq = __get_reqseq(chan, rx_control); + + BT_DBG("chan %p, req_seq %d ctrl 0x%8.8x", chan, tx_seq, rx_control); + + clear_bit(CONN_REMOTE_BUSY, &chan->conn_state); + + if (__is_ctrl_poll(chan, rx_control)) { + chan->expected_ack_seq = tx_seq; + l2cap_drop_acked_frames(chan); + + set_bit(CONN_SEND_FBIT, &chan->conn_state); + l2cap_retransmit_one_frame(chan, tx_seq); + + l2cap_ertm_send(chan); + + if (test_bit(CONN_WAIT_F, &chan->conn_state)) { + chan->srej_save_reqseq = tx_seq; + set_bit(CONN_SREJ_ACT, &chan->conn_state); + } + } else if (__is_ctrl_final(chan, rx_control)) { + if (test_bit(CONN_SREJ_ACT, &chan->conn_state) && + chan->srej_save_reqseq == tx_seq) + clear_bit(CONN_SREJ_ACT, &chan->conn_state); + else + l2cap_retransmit_one_frame(chan, tx_seq); + } else { + l2cap_retransmit_one_frame(chan, tx_seq); + if (test_bit(CONN_WAIT_F, &chan->conn_state)) { + chan->srej_save_reqseq = tx_seq; + set_bit(CONN_SREJ_ACT, &chan->conn_state); + } + } +} + +static inline void l2cap_data_channel_rnrframe(struct l2cap_chan *chan, u32 rx_control) +{ + u16 tx_seq = __get_reqseq(chan, rx_control); + + BT_DBG("chan %p, req_seq %d ctrl 0x%8.8x", chan, tx_seq, rx_control); + + set_bit(CONN_REMOTE_BUSY, &chan->conn_state); + chan->expected_ack_seq = tx_seq; + l2cap_drop_acked_frames(chan); + + if (__is_ctrl_poll(chan, rx_control)) + set_bit(CONN_SEND_FBIT, &chan->conn_state); + + if (!test_bit(CONN_SREJ_SENT, &chan->conn_state)) { + __clear_retrans_timer(chan); + if (__is_ctrl_poll(chan, rx_control)) + l2cap_send_rr_or_rnr(chan, L2CAP_CTRL_FINAL); + return; + } + + if (__is_ctrl_poll(chan, rx_control)) { + l2cap_send_srejtail(chan); + } else { + rx_control = __set_ctrl_super(chan, L2CAP_SUPER_RR); + l2cap_send_sframe(chan, rx_control); + } +} + +static inline int l2cap_data_channel_sframe(struct l2cap_chan *chan, u32 rx_control, struct sk_buff *skb) +{ + BT_DBG("chan %p rx_control 0x%8.8x len %d", chan, rx_control, skb->len); + + if (__is_ctrl_final(chan, rx_control) && + test_bit(CONN_WAIT_F, &chan->conn_state)) { + __clear_monitor_timer(chan); + if (chan->unacked_frames > 0) + __set_retrans_timer(chan); + clear_bit(CONN_WAIT_F, &chan->conn_state); + } + + switch (__get_ctrl_super(chan, rx_control)) { + case L2CAP_SUPER_RR: + l2cap_data_channel_rrframe(chan, rx_control); + break; + + case L2CAP_SUPER_REJ: + l2cap_data_channel_rejframe(chan, rx_control); + break; + + case L2CAP_SUPER_SREJ: + l2cap_data_channel_srejframe(chan, rx_control); + break; + + case L2CAP_SUPER_RNR: + l2cap_data_channel_rnrframe(chan, rx_control); + break; + } + + kfree_skb(skb); + return 0; +} + +static int l2cap_ertm_data_rcv(struct l2cap_chan *chan, struct sk_buff *skb) +{ + u32 control; + u16 req_seq; + int len, next_tx_seq_offset, req_seq_offset; + + control = __get_control(chan, skb->data); + skb_pull(skb, __ctrl_size(chan)); + len = skb->len; + + /* + * We can just drop the corrupted I-frame here. + * Receiver will miss it and start proper recovery + * procedures and ask retransmission. + */ + if (l2cap_check_fcs(chan, skb)) + goto drop; + + if (__is_sar_start(chan, control) && !__is_sframe(chan, control)) + len -= L2CAP_SDULEN_SIZE; + + if (chan->fcs == L2CAP_FCS_CRC16) + len -= L2CAP_FCS_SIZE; + + if (len > chan->mps) { + l2cap_send_disconn_req(chan->conn, chan, ECONNRESET); + goto drop; + } + + req_seq = __get_reqseq(chan, control); + + req_seq_offset = __seq_offset(chan, req_seq, chan->expected_ack_seq); + + next_tx_seq_offset = __seq_offset(chan, chan->next_tx_seq, + chan->expected_ack_seq); + + /* check for invalid req-seq */ + if (req_seq_offset > next_tx_seq_offset) { + l2cap_send_disconn_req(chan->conn, chan, ECONNRESET); + goto drop; + } + + if (!__is_sframe(chan, control)) { + if (len < 0) { + l2cap_send_disconn_req(chan->conn, chan, ECONNRESET); + goto drop; + } + + l2cap_data_channel_iframe(chan, control, skb); + } else { + if (len != 0) { + BT_ERR("%d", len); + l2cap_send_disconn_req(chan->conn, chan, ECONNRESET); + goto drop; + } + + l2cap_data_channel_sframe(chan, control, skb); + } + + return 0; + +drop: + kfree_skb(skb); + return 0; +} + +static inline int l2cap_data_channel(struct l2cap_conn *conn, u16 cid, struct sk_buff *skb) +{ + struct l2cap_chan *chan; + u32 control; + u16 tx_seq; + int len; + + chan = l2cap_get_chan_by_scid(conn, cid); + if (!chan) { + BT_DBG("unknown cid 0x%4.4x", cid); + /* Drop packet and return */ + kfree_skb(skb); + return 0; + } + + BT_DBG("chan %p, len %d", chan, skb->len); + + if (chan->state != BT_CONNECTED) + goto drop; + + switch (chan->mode) { + case L2CAP_MODE_BASIC: + /* If socket recv buffers overflows we drop data here + * which is *bad* because L2CAP has to be reliable. + * But we don't have any other choice. L2CAP doesn't + * provide flow control mechanism. */ + + if (chan->imtu < skb->len) + goto drop; + + if (!chan->ops->recv(chan->data, skb)) + goto done; + break; + + case L2CAP_MODE_ERTM: + l2cap_ertm_data_rcv(chan, skb); + + goto done; + + case L2CAP_MODE_STREAMING: + control = __get_control(chan, skb->data); + skb_pull(skb, __ctrl_size(chan)); + len = skb->len; + + if (l2cap_check_fcs(chan, skb)) + goto drop; + + if (__is_sar_start(chan, control)) + len -= L2CAP_SDULEN_SIZE; + + if (chan->fcs == L2CAP_FCS_CRC16) + len -= L2CAP_FCS_SIZE; + + if (len > chan->mps || len < 0 || __is_sframe(chan, control)) + goto drop; + + tx_seq = __get_txseq(chan, control); + + if (chan->expected_tx_seq != tx_seq) { + /* Frame(s) missing - must discard partial SDU */ + kfree_skb(chan->sdu); + chan->sdu = NULL; + chan->sdu_last_frag = NULL; + chan->sdu_len = 0; + + /* TODO: Notify userland of missing data */ + } + + chan->expected_tx_seq = __next_seq(chan, tx_seq); + + if (l2cap_reassemble_sdu(chan, skb, control) == -EMSGSIZE) + l2cap_send_disconn_req(chan->conn, chan, ECONNRESET); + + goto done; + + default: + BT_DBG("chan %p: bad mode 0x%2.2x", chan, chan->mode); + break; + } + +drop: + kfree_skb(skb); + +done: + l2cap_chan_unlock(chan); + + return 0; +} + +static inline int l2cap_conless_channel(struct l2cap_conn *conn, __le16 psm, struct sk_buff *skb) +{ + struct l2cap_chan *chan; + + chan = l2cap_global_chan_by_psm(0, psm, conn->src); + if (!chan) + goto drop; + + BT_DBG("chan %p, len %d", chan, skb->len); + + if (chan->state != BT_BOUND && chan->state != BT_CONNECTED) + goto drop; + + if (chan->imtu < skb->len) + goto drop; + + if (!chan->ops->recv(chan->data, skb)) + return 0; + +drop: + kfree_skb(skb); + + return 0; +} + +static inline int l2cap_att_channel(struct l2cap_conn *conn, __le16 cid, struct sk_buff *skb) +{ + struct l2cap_chan *chan; + + chan = l2cap_global_chan_by_scid(0, cid, conn->src); + if (!chan) + goto drop; + + BT_DBG("chan %p, len %d", chan, skb->len); + + if (chan->state != BT_BOUND && chan->state != BT_CONNECTED) + goto drop; + + if (chan->imtu < skb->len) + goto drop; + + if (!chan->ops->recv(chan->data, skb)) + return 0; + +drop: + kfree_skb(skb); + + return 0; +} + +static void l2cap_recv_frame(struct l2cap_conn *conn, struct sk_buff *skb) +{ + struct l2cap_hdr *lh = (void *) skb->data; + u16 cid, len; + __le16 psm; + + skb_pull(skb, L2CAP_HDR_SIZE); + cid = __le16_to_cpu(lh->cid); + len = __le16_to_cpu(lh->len); + + if (len != skb->len) { + kfree_skb(skb); + return; + } + + BT_DBG("len %d, cid 0x%4.4x", len, cid); + + switch (cid) { + case L2CAP_CID_LE_SIGNALING: + case L2CAP_CID_SIGNALING: + l2cap_sig_channel(conn, skb); + break; + + case L2CAP_CID_CONN_LESS: + psm = get_unaligned_le16(skb->data); + skb_pull(skb, 2); + l2cap_conless_channel(conn, psm, skb); + break; + + case L2CAP_CID_LE_DATA: + l2cap_att_channel(conn, cid, skb); + break; + + case L2CAP_CID_SMP: + if (smp_sig_channel(conn, skb)) + l2cap_conn_del(conn->hcon, EACCES); + break; + + default: + l2cap_data_channel(conn, cid, skb); + break; + } +} + +/* ---- L2CAP interface with lower layer (HCI) ---- */ + +int l2cap_connect_ind(struct hci_dev *hdev, bdaddr_t *bdaddr) +{ + int exact = 0, lm1 = 0, lm2 = 0; + struct l2cap_chan *c; + + BT_DBG("hdev %s, bdaddr %s", hdev->name, batostr(bdaddr)); + + /* Find listening sockets and check their link_mode */ + read_lock(&chan_list_lock); + list_for_each_entry(c, &chan_list, global_l) { + struct sock *sk = c->sk; + + if (c->state != BT_LISTEN) + continue; + + if (!bacmp(&bt_sk(sk)->src, &hdev->bdaddr)) { + lm1 |= HCI_LM_ACCEPT; + if (test_bit(FLAG_ROLE_SWITCH, &c->flags)) + lm1 |= HCI_LM_MASTER; + exact++; + } else if (!bacmp(&bt_sk(sk)->src, BDADDR_ANY)) { + lm2 |= HCI_LM_ACCEPT; + if (test_bit(FLAG_ROLE_SWITCH, &c->flags)) + lm2 |= HCI_LM_MASTER; + } + } + read_unlock(&chan_list_lock); + + return exact ? lm1 : lm2; +} + +int l2cap_connect_cfm(struct hci_conn *hcon, u8 status) +{ + struct l2cap_conn *conn; + + BT_DBG("hcon %p bdaddr %s status %d", hcon, batostr(&hcon->dst), status); + + if (!status) { + conn = l2cap_conn_add(hcon, status); + if (conn) + l2cap_conn_ready(conn); + } else + l2cap_conn_del(hcon, bt_to_errno(status)); + + return 0; +} + +int l2cap_disconn_ind(struct hci_conn *hcon) +{ + struct l2cap_conn *conn = hcon->l2cap_data; + + BT_DBG("hcon %p", hcon); + + if (!conn) + return HCI_ERROR_REMOTE_USER_TERM; + return conn->disc_reason; +} + +int l2cap_disconn_cfm(struct hci_conn *hcon, u8 reason) +{ + BT_DBG("hcon %p reason %d", hcon, reason); + + l2cap_conn_del(hcon, bt_to_errno(reason)); + return 0; +} + +static inline void l2cap_check_encryption(struct l2cap_chan *chan, u8 encrypt) +{ + if (chan->chan_type != L2CAP_CHAN_CONN_ORIENTED) + return; + + if (encrypt == 0x00) { + if (chan->sec_level == BT_SECURITY_MEDIUM) { + __clear_chan_timer(chan); + __set_chan_timer(chan, L2CAP_ENC_TIMEOUT); + } else if (chan->sec_level == BT_SECURITY_HIGH) + l2cap_chan_close(chan, ECONNREFUSED); + } else { + if (chan->sec_level == BT_SECURITY_MEDIUM) + __clear_chan_timer(chan); + } +} + +int l2cap_security_cfm(struct hci_conn *hcon, u8 status, u8 encrypt) +{ + struct l2cap_conn *conn = hcon->l2cap_data; + struct l2cap_chan *chan; + + if (!conn) + return 0; + + BT_DBG("conn %p", conn); + + if (hcon->type == LE_LINK) { + smp_distribute_keys(conn, 0); + cancel_delayed_work(&conn->security_timer); + } + + mutex_lock(&conn->chan_lock); + + list_for_each_entry(chan, &conn->chan_l, list) { + l2cap_chan_lock(chan); + + BT_DBG("chan->scid %d", chan->scid); + + if (chan->scid == L2CAP_CID_LE_DATA) { + if (!status && encrypt) { + chan->sec_level = hcon->sec_level; + l2cap_chan_ready(chan); + } + + l2cap_chan_unlock(chan); + continue; + } + + if (test_bit(CONF_CONNECT_PEND, &chan->conf_state)) { + l2cap_chan_unlock(chan); + continue; + } + + if (!status && (chan->state == BT_CONNECTED || + chan->state == BT_CONFIG)) { + struct sock *sk = chan->sk; + + bt_sk(sk)->suspended = false; + sk->sk_state_change(sk); + + l2cap_check_encryption(chan, encrypt); + l2cap_chan_unlock(chan); + continue; + } + + if (chan->state == BT_CONNECT) { + if (!status) { + l2cap_send_conn_req(chan); + } else { + __clear_chan_timer(chan); + __set_chan_timer(chan, L2CAP_DISC_TIMEOUT); + } + } else if (chan->state == BT_CONNECT2) { + struct sock *sk = chan->sk; + struct l2cap_conn_rsp rsp; + __u16 res, stat; + + lock_sock(sk); + + if (!status) { + if (bt_sk(sk)->defer_setup) { + struct sock *parent = bt_sk(sk)->parent; + res = L2CAP_CR_PEND; + stat = L2CAP_CS_AUTHOR_PEND; + if (parent) + parent->sk_data_ready(parent, 0); + } else { + __l2cap_state_change(chan, BT_CONFIG); + res = L2CAP_CR_SUCCESS; + stat = L2CAP_CS_NO_INFO; + } + } else { + __l2cap_state_change(chan, BT_DISCONN); + __set_chan_timer(chan, L2CAP_DISC_TIMEOUT); + res = L2CAP_CR_SEC_BLOCK; + stat = L2CAP_CS_NO_INFO; + } + + release_sock(sk); + + rsp.scid = cpu_to_le16(chan->dcid); + rsp.dcid = cpu_to_le16(chan->scid); + rsp.result = cpu_to_le16(res); + rsp.status = cpu_to_le16(stat); + l2cap_send_cmd(conn, chan->ident, L2CAP_CONN_RSP, + sizeof(rsp), &rsp); + } + + l2cap_chan_unlock(chan); + } + + mutex_unlock(&conn->chan_lock); + + return 0; +} + +int l2cap_recv_acldata(struct hci_conn *hcon, struct sk_buff *skb, u16 flags) +{ + struct l2cap_conn *conn = hcon->l2cap_data; + + if (!conn) + conn = l2cap_conn_add(hcon, 0); + + if (!conn) + goto drop; + + BT_DBG("conn %p len %d flags 0x%x", conn, skb->len, flags); + + if (!(flags & ACL_CONT)) { + struct l2cap_hdr *hdr; + int len; + + if (conn->rx_len) { + BT_ERR("Unexpected start frame (len %d)", skb->len); + kfree_skb(conn->rx_skb); + conn->rx_skb = NULL; + conn->rx_len = 0; + l2cap_conn_unreliable(conn, ECOMM); + } + + /* Start fragment always begin with Basic L2CAP header */ + if (skb->len < L2CAP_HDR_SIZE) { + BT_ERR("Frame is too short (len %d)", skb->len); + l2cap_conn_unreliable(conn, ECOMM); + goto drop; + } + + hdr = (struct l2cap_hdr *) skb->data; + len = __le16_to_cpu(hdr->len) + L2CAP_HDR_SIZE; + + if (len == skb->len) { + /* Complete frame received */ + l2cap_recv_frame(conn, skb); + return 0; + } + + BT_DBG("Start: total len %d, frag len %d", len, skb->len); + + if (skb->len > len) { + BT_ERR("Frame is too long (len %d, expected len %d)", + skb->len, len); + l2cap_conn_unreliable(conn, ECOMM); + goto drop; + } + + /* Allocate skb for the complete frame (with header) */ + conn->rx_skb = bt_skb_alloc(len, GFP_ATOMIC); + if (!conn->rx_skb) + goto drop; + + skb_copy_from_linear_data(skb, skb_put(conn->rx_skb, skb->len), + skb->len); + conn->rx_len = len - skb->len; + } else { + BT_DBG("Cont: frag len %d (expecting %d)", skb->len, conn->rx_len); + + if (!conn->rx_len) { + BT_ERR("Unexpected continuation frame (len %d)", skb->len); + l2cap_conn_unreliable(conn, ECOMM); + goto drop; + } + + if (skb->len > conn->rx_len) { + BT_ERR("Fragment is too long (len %d, expected %d)", + skb->len, conn->rx_len); + kfree_skb(conn->rx_skb); + conn->rx_skb = NULL; + conn->rx_len = 0; + l2cap_conn_unreliable(conn, ECOMM); + goto drop; + } + + skb_copy_from_linear_data(skb, skb_put(conn->rx_skb, skb->len), + skb->len); + conn->rx_len -= skb->len; + + if (!conn->rx_len) { + /* Complete frame received */ + l2cap_recv_frame(conn, conn->rx_skb); + conn->rx_skb = NULL; + } + } + +drop: + kfree_skb(skb); + return 0; +} + +static int l2cap_debugfs_show(struct seq_file *f, void *p) +{ + struct l2cap_chan *c; + + read_lock(&chan_list_lock); + + list_for_each_entry(c, &chan_list, global_l) { + struct sock *sk = c->sk; + + seq_printf(f, "%s %s %d %d 0x%4.4x 0x%4.4x %d %d %d %d\n", + batostr(&bt_sk(sk)->src), + batostr(&bt_sk(sk)->dst), + c->state, __le16_to_cpu(c->psm), + c->scid, c->dcid, c->imtu, c->omtu, + c->sec_level, c->mode); + } + + read_unlock(&chan_list_lock); + + return 0; +} + +static int l2cap_debugfs_open(struct inode *inode, struct file *file) +{ + return single_open(file, l2cap_debugfs_show, inode->i_private); +} + +static const struct file_operations l2cap_debugfs_fops = { + .open = l2cap_debugfs_open, + .read = seq_read, + .llseek = seq_lseek, + .release = single_release, +}; + +static struct dentry *l2cap_debugfs; + +int __init l2cap_init(void) +{ + int err; + + err = l2cap_init_sockets(); + if (err < 0) + return err; + + if (bt_debugfs) { + l2cap_debugfs = debugfs_create_file("l2cap", 0444, + bt_debugfs, NULL, &l2cap_debugfs_fops); + if (!l2cap_debugfs) + BT_ERR("Failed to create L2CAP debug file"); + } + + return 0; +} + +void l2cap_exit(void) +{ + debugfs_remove(l2cap_debugfs); + l2cap_cleanup_sockets(); +} + +module_param(disable_ertm, bool, 0644); +MODULE_PARM_DESC(disable_ertm, "Disable enhanced retransmission mode"); diff --git a/net/bluetooth_tizen/l2cap_sock.c b/net/bluetooth_tizen/l2cap_sock.c new file mode 100644 index 0000000..2cae7af --- /dev/null +++ b/net/bluetooth_tizen/l2cap_sock.c @@ -0,0 +1,1156 @@ +/* + BlueZ - Bluetooth protocol stack for Linux + Copyright (C) 2000-2001 Qualcomm Incorporated + Copyright (C) 2009-2010 Gustavo F. Padovan <gustavo@padovan.org> + Copyright (C) 2010 Google Inc. + Copyright (C) 2011 ProFUSION Embedded Systems + + Written 2000,2001 by Maxim Krasnyansky <maxk@qualcomm.com> + + This program is free software; you can redistribute it and/or modify + it under the terms of the GNU General Public License version 2 as + published by the Free Software Foundation; + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS + OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT OF THIRD PARTY RIGHTS. + IN NO EVENT SHALL THE COPYRIGHT HOLDER(S) AND AUTHOR(S) BE LIABLE FOR ANY + CLAIM, OR ANY SPECIAL INDIRECT OR CONSEQUENTIAL DAMAGES, OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + + ALL LIABILITY, INCLUDING LIABILITY FOR INFRINGEMENT OF ANY PATENTS, + COPYRIGHTS, TRADEMARKS OR OTHER RIGHTS, RELATING TO USE OF THIS + SOFTWARE IS DISCLAIMED. +*/ + +/* Bluetooth L2CAP sockets. */ + +#include <linux/security.h> + +#include <net/bluetooth/bluetooth.h> +#include <net/bluetooth/hci_core.h> +#include <net/bluetooth/l2cap.h> +#include <net/bluetooth/smp.h> + +static const struct proto_ops l2cap_sock_ops; +static void l2cap_sock_init(struct sock *sk, struct sock *parent); +static struct sock *l2cap_sock_alloc(struct net *net, struct socket *sock, int proto, gfp_t prio); + +static int l2cap_sock_bind(struct socket *sock, struct sockaddr *addr, int alen) +{ + struct sock *sk = sock->sk; + struct l2cap_chan *chan = l2cap_pi(sk)->chan; + struct sockaddr_l2 la; + int len, err = 0; + + BT_DBG("sk %p", sk); + + if (!addr || addr->sa_family != AF_BLUETOOTH) + return -EINVAL; + + memset(&la, 0, sizeof(la)); + len = min_t(unsigned int, sizeof(la), alen); + memcpy(&la, addr, len); + + if (la.l2_cid && la.l2_psm) + return -EINVAL; + + lock_sock(sk); + + if (sk->sk_state != BT_OPEN) { + err = -EBADFD; + goto done; + } + + if (la.l2_psm) { + __u16 psm = __le16_to_cpu(la.l2_psm); + + /* PSM must be odd and lsb of upper byte must be 0 */ + if ((psm & 0x0101) != 0x0001) { + err = -EINVAL; + goto done; + } + + /* Restrict usage of well-known PSMs */ + if (psm < 0x1001 && !capable(CAP_NET_BIND_SERVICE)) { + err = -EACCES; + goto done; + } + } + + if (la.l2_cid) + err = l2cap_add_scid(chan, __le16_to_cpu(la.l2_cid)); + else + err = l2cap_add_psm(chan, &la.l2_bdaddr, la.l2_psm); + + if (err < 0) + goto done; + + if (__le16_to_cpu(la.l2_psm) == 0x0001 || + __le16_to_cpu(la.l2_psm) == 0x0003) + chan->sec_level = BT_SECURITY_SDP; + + bacpy(&bt_sk(sk)->src, &la.l2_bdaddr); + + chan->state = BT_BOUND; + sk->sk_state = BT_BOUND; + +done: + release_sock(sk); + return err; +} + +static int l2cap_sock_connect(struct socket *sock, struct sockaddr *addr, int alen, int flags) +{ + struct sock *sk = sock->sk; + struct l2cap_chan *chan = l2cap_pi(sk)->chan; + struct sockaddr_l2 la; + int len, err = 0; + + BT_DBG("sk %p", sk); + + if (!addr || alen < sizeof(addr->sa_family) || + addr->sa_family != AF_BLUETOOTH) + return -EINVAL; + + memset(&la, 0, sizeof(la)); + len = min_t(unsigned int, sizeof(la), alen); + memcpy(&la, addr, len); + + if (la.l2_cid && la.l2_psm) + return -EINVAL; + + err = l2cap_chan_connect(chan, la.l2_psm, __le16_to_cpu(la.l2_cid), + &la.l2_bdaddr); + if (err) + return err; + + lock_sock(sk); + + err = bt_sock_wait_state(sk, BT_CONNECTED, + sock_sndtimeo(sk, flags & O_NONBLOCK)); + + release_sock(sk); + + return err; +} + +static int l2cap_sock_listen(struct socket *sock, int backlog) +{ + struct sock *sk = sock->sk; + struct l2cap_chan *chan = l2cap_pi(sk)->chan; + int err = 0; + + BT_DBG("sk %p backlog %d", sk, backlog); + + lock_sock(sk); + + if ((sock->type != SOCK_SEQPACKET && sock->type != SOCK_STREAM) + || sk->sk_state != BT_BOUND) { + err = -EBADFD; + goto done; + } + + switch (chan->mode) { + case L2CAP_MODE_BASIC: + break; + case L2CAP_MODE_ERTM: + case L2CAP_MODE_STREAMING: + if (!disable_ertm) + break; + /* fall through */ + default: + err = -ENOTSUPP; + goto done; + } + + sk->sk_max_ack_backlog = backlog; + sk->sk_ack_backlog = 0; + + chan->state = BT_LISTEN; + sk->sk_state = BT_LISTEN; + +done: + release_sock(sk); + return err; +} + +static int l2cap_sock_accept(struct socket *sock, struct socket *newsock, int flags) +{ + DECLARE_WAITQUEUE(wait, current); + struct sock *sk = sock->sk, *nsk; + long timeo; + int err = 0; + + lock_sock_nested(sk, SINGLE_DEPTH_NESTING); + + timeo = sock_rcvtimeo(sk, flags & O_NONBLOCK); + + BT_DBG("sk %p timeo %ld", sk, timeo); + + /* Wait for an incoming connection. (wake-one). */ + add_wait_queue_exclusive(sk_sleep(sk), &wait); + while (1) { + set_current_state(TASK_INTERRUPTIBLE); + + if (sk->sk_state != BT_LISTEN) { + err = -EBADFD; + break; + } + + nsk = bt_accept_dequeue(sk, newsock); + if (nsk) + break; + + if (!timeo) { + err = -EAGAIN; + break; + } + + if (signal_pending(current)) { + err = sock_intr_errno(timeo); + break; + } + + release_sock(sk); + timeo = schedule_timeout(timeo); + lock_sock_nested(sk, SINGLE_DEPTH_NESTING); + } + __set_current_state(TASK_RUNNING); + remove_wait_queue(sk_sleep(sk), &wait); + + if (err) + goto done; + + newsock->state = SS_CONNECTED; + + BT_DBG("new socket %p", nsk); + +done: + release_sock(sk); + return err; +} + +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; + struct l2cap_chan *chan = l2cap_pi(sk)->chan; + + BT_DBG("sock %p, sk %p", sock, sk); + + addr->sa_family = AF_BLUETOOTH; + *len = sizeof(struct sockaddr_l2); + + if (peer) { + la->l2_psm = chan->psm; + bacpy(&la->l2_bdaddr, &bt_sk(sk)->dst); + la->l2_cid = cpu_to_le16(chan->dcid); + } else { + la->l2_psm = chan->sport; + bacpy(&la->l2_bdaddr, &bt_sk(sk)->src); + la->l2_cid = cpu_to_le16(chan->scid); + } + + return 0; +} + +static int l2cap_sock_getsockopt_old(struct socket *sock, int optname, char __user *optval, int __user *optlen) +{ + struct sock *sk = sock->sk; + struct l2cap_chan *chan = l2cap_pi(sk)->chan; + struct l2cap_options opts; + struct l2cap_conninfo cinfo; + int len, err = 0; + u32 opt; + + BT_DBG("sk %p", sk); + + if (get_user(len, optlen)) + return -EFAULT; + + lock_sock(sk); + + switch (optname) { + case L2CAP_OPTIONS: + memset(&opts, 0, sizeof(opts)); + opts.imtu = chan->imtu; + opts.omtu = chan->omtu; + opts.flush_to = chan->flush_to; + opts.mode = chan->mode; + opts.fcs = chan->fcs; + opts.max_tx = chan->max_tx; + opts.txwin_size = chan->tx_win; + + len = min_t(unsigned int, len, sizeof(opts)); + if (copy_to_user(optval, (char *) &opts, len)) + err = -EFAULT; + + break; + + case L2CAP_LM: + switch (chan->sec_level) { + case BT_SECURITY_LOW: + opt = L2CAP_LM_AUTH; + break; + case BT_SECURITY_MEDIUM: + opt = L2CAP_LM_AUTH | L2CAP_LM_ENCRYPT; + break; + case BT_SECURITY_HIGH: + opt = L2CAP_LM_AUTH | L2CAP_LM_ENCRYPT | + L2CAP_LM_SECURE; + break; + default: + opt = 0; + break; + } + + if (test_bit(FLAG_ROLE_SWITCH, &chan->flags)) + opt |= L2CAP_LM_MASTER; + + if (test_bit(FLAG_FORCE_RELIABLE, &chan->flags)) + opt |= L2CAP_LM_RELIABLE; + + if (put_user(opt, (u32 __user *) optval)) + err = -EFAULT; + break; + + case L2CAP_CONNINFO: + if (sk->sk_state != BT_CONNECTED && + !(sk->sk_state == BT_CONNECT2 && + bt_sk(sk)->defer_setup)) { + err = -ENOTCONN; + break; + } + + memset(&cinfo, 0, sizeof(cinfo)); + cinfo.hci_handle = chan->conn->hcon->handle; + memcpy(cinfo.dev_class, chan->conn->hcon->dev_class, 3); + + len = min_t(unsigned int, len, sizeof(cinfo)); + if (copy_to_user(optval, (char *) &cinfo, len)) + err = -EFAULT; + + break; + + default: + err = -ENOPROTOOPT; + break; + } + + release_sock(sk); + return err; +} + +static int l2cap_sock_getsockopt(struct socket *sock, int level, int optname, char __user *optval, int __user *optlen) +{ + struct sock *sk = sock->sk; + struct l2cap_chan *chan = l2cap_pi(sk)->chan; + struct bt_security sec; + struct bt_power pwr; + int len, err = 0; + + BT_DBG("sk %p", sk); + + if (level == SOL_L2CAP) + return l2cap_sock_getsockopt_old(sock, optname, optval, optlen); + + if (level != SOL_BLUETOOTH) + return -ENOPROTOOPT; + + if (get_user(len, optlen)) + return -EFAULT; + + lock_sock(sk); + + switch (optname) { + case BT_SECURITY: + if (chan->chan_type != L2CAP_CHAN_CONN_ORIENTED && + chan->chan_type != L2CAP_CHAN_RAW) { + err = -EINVAL; + break; + } + + memset(&sec, 0, sizeof(sec)); + if (chan->conn) + sec.level = chan->conn->hcon->sec_level; + else + sec.level = chan->sec_level; + + if (sk->sk_state == BT_CONNECTED) + sec.key_size = chan->conn->hcon->enc_key_size; + + len = min_t(unsigned int, len, sizeof(sec)); + if (copy_to_user(optval, (char *) &sec, len)) + err = -EFAULT; + + break; + + case BT_DEFER_SETUP: + if (sk->sk_state != BT_BOUND && sk->sk_state != BT_LISTEN) { + err = -EINVAL; + break; + } + + if (put_user(bt_sk(sk)->defer_setup, (u32 __user *) optval)) + err = -EFAULT; + + break; + + case BT_FLUSHABLE: + if (put_user(test_bit(FLAG_FLUSHABLE, &chan->flags), + (u32 __user *) optval)) + err = -EFAULT; + + break; + + case BT_POWER: + if (sk->sk_type != SOCK_SEQPACKET && sk->sk_type != SOCK_STREAM + && sk->sk_type != SOCK_RAW) { + err = -EINVAL; + break; + } + + pwr.force_active = test_bit(FLAG_FORCE_ACTIVE, &chan->flags); + + len = min_t(unsigned int, len, sizeof(pwr)); + if (copy_to_user(optval, (char *) &pwr, len)) + err = -EFAULT; + + break; + + case BT_CHANNEL_POLICY: + if (!enable_hs) { + err = -ENOPROTOOPT; + break; + } + + if (put_user(chan->chan_policy, (u32 __user *) optval)) + err = -EFAULT; + break; + + default: + err = -ENOPROTOOPT; + break; + } + + release_sock(sk); + return err; +} + +static int l2cap_sock_setsockopt_old(struct socket *sock, int optname, char __user *optval, unsigned int optlen) +{ + struct sock *sk = sock->sk; + struct l2cap_chan *chan = l2cap_pi(sk)->chan; + struct l2cap_options opts; + int len, err = 0; + u32 opt; + + BT_DBG("sk %p", sk); + + lock_sock(sk); + + switch (optname) { + case L2CAP_OPTIONS: + if (sk->sk_state == BT_CONNECTED) { + err = -EINVAL; + break; + } + + opts.imtu = chan->imtu; + opts.omtu = chan->omtu; + opts.flush_to = chan->flush_to; + opts.mode = chan->mode; + opts.fcs = chan->fcs; + opts.max_tx = chan->max_tx; + opts.txwin_size = chan->tx_win; + + len = min_t(unsigned int, sizeof(opts), optlen); + if (copy_from_user((char *) &opts, optval, len)) { + err = -EFAULT; + break; + } + + if (opts.txwin_size > L2CAP_DEFAULT_EXT_WINDOW) { + err = -EINVAL; + break; + } + + chan->mode = opts.mode; + switch (chan->mode) { + case L2CAP_MODE_BASIC: + clear_bit(CONF_STATE2_DEVICE, &chan->conf_state); + break; + case L2CAP_MODE_ERTM: + case L2CAP_MODE_STREAMING: + if (!disable_ertm) + break; + /* fall through */ + default: + err = -EINVAL; + break; + } + + chan->imtu = opts.imtu; + chan->omtu = opts.omtu; + chan->fcs = opts.fcs; + chan->max_tx = opts.max_tx; + chan->tx_win = opts.txwin_size; + break; + + case L2CAP_LM: + if (get_user(opt, (u32 __user *) optval)) { + err = -EFAULT; + break; + } + + if (opt & L2CAP_LM_AUTH) + chan->sec_level = BT_SECURITY_LOW; + if (opt & L2CAP_LM_ENCRYPT) + chan->sec_level = BT_SECURITY_MEDIUM; + if (opt & L2CAP_LM_SECURE) + chan->sec_level = BT_SECURITY_HIGH; + + if (opt & L2CAP_LM_MASTER) + set_bit(FLAG_ROLE_SWITCH, &chan->flags); + else + clear_bit(FLAG_ROLE_SWITCH, &chan->flags); + + if (opt & L2CAP_LM_RELIABLE) + set_bit(FLAG_FORCE_RELIABLE, &chan->flags); + else + clear_bit(FLAG_FORCE_RELIABLE, &chan->flags); + break; + + default: + err = -ENOPROTOOPT; + break; + } + + release_sock(sk); + return err; +} + +static int l2cap_sock_setsockopt(struct socket *sock, int level, int optname, char __user *optval, unsigned int optlen) +{ + struct sock *sk = sock->sk; + struct l2cap_chan *chan = l2cap_pi(sk)->chan; + struct bt_security sec; + struct bt_power pwr; + struct l2cap_conn *conn; + int len, err = 0; + u32 opt; + + BT_DBG("sk %p", sk); + + if (level == SOL_L2CAP) + return l2cap_sock_setsockopt_old(sock, optname, optval, optlen); + + if (level != SOL_BLUETOOTH) + return -ENOPROTOOPT; + + lock_sock(sk); + + switch (optname) { + case BT_SECURITY: + if (chan->chan_type != L2CAP_CHAN_CONN_ORIENTED && + chan->chan_type != L2CAP_CHAN_RAW) { + err = -EINVAL; + break; + } + + sec.level = BT_SECURITY_LOW; + + len = min_t(unsigned int, sizeof(sec), optlen); + if (copy_from_user((char *) &sec, optval, len)) { + err = -EFAULT; + break; + } + + if (sec.level < BT_SECURITY_LOW || + sec.level > BT_SECURITY_HIGH) { + err = -EINVAL; + break; + } + + chan->sec_level = sec.level; + + if (!chan->conn) + break; + + conn = chan->conn; + + /*change security for LE channels */ + if (chan->scid == L2CAP_CID_LE_DATA) { + if (!conn->hcon->out) { + err = -EINVAL; + break; + } + + if (smp_conn_security(conn, sec.level)) + break; + sk->sk_state = BT_CONFIG; + chan->state = BT_CONFIG; + + /* or for ACL link */ + } else if ((sk->sk_state == BT_CONNECT2 && + bt_sk(sk)->defer_setup) || + sk->sk_state == BT_CONNECTED) { + if (!l2cap_chan_check_security(chan)) + bt_sk(sk)->suspended = true; + else + sk->sk_state_change(sk); + } + else { + err = -EINVAL; + } + break; + + case BT_DEFER_SETUP: + if (sk->sk_state != BT_BOUND && sk->sk_state != BT_LISTEN) { + err = -EINVAL; + break; + } + + if (get_user(opt, (u32 __user *) optval)) { + err = -EFAULT; + break; + } + + bt_sk(sk)->defer_setup = opt; + break; + + case BT_FLUSHABLE: + if (get_user(opt, (u32 __user *) optval)) { + err = -EFAULT; + break; + } + + if (opt > BT_FLUSHABLE_ON) { + err = -EINVAL; + break; + } + + if (opt == BT_FLUSHABLE_OFF) { + struct l2cap_conn *conn = chan->conn; + /* proceed further only when we have l2cap_conn and + No Flush support in the LM */ + if (!conn || !lmp_no_flush_capable(conn->hcon->hdev)) { + err = -EINVAL; + break; + } + } + + if (opt) + set_bit(FLAG_FLUSHABLE, &chan->flags); + else + clear_bit(FLAG_FLUSHABLE, &chan->flags); + break; + + case BT_POWER: + if (chan->chan_type != L2CAP_CHAN_CONN_ORIENTED && + chan->chan_type != L2CAP_CHAN_RAW) { + err = -EINVAL; + break; + } + + pwr.force_active = BT_POWER_FORCE_ACTIVE_ON; + + len = min_t(unsigned int, sizeof(pwr), optlen); + if (copy_from_user((char *) &pwr, optval, len)) { + err = -EFAULT; + break; + } + + if (pwr.force_active) + set_bit(FLAG_FORCE_ACTIVE, &chan->flags); + else + clear_bit(FLAG_FORCE_ACTIVE, &chan->flags); + break; + + case BT_CHANNEL_POLICY: + if (!enable_hs) { + err = -ENOPROTOOPT; + break; + } + + if (get_user(opt, (u32 __user *) optval)) { + err = -EFAULT; + break; + } + + if (opt > BT_CHANNEL_POLICY_AMP_PREFERRED) { + err = -EINVAL; + break; + } + + if (chan->mode != L2CAP_MODE_ERTM && + chan->mode != L2CAP_MODE_STREAMING) { + err = -EOPNOTSUPP; + break; + } + + chan->chan_policy = (u8) opt; + break; + + default: + err = -ENOPROTOOPT; + break; + } + + release_sock(sk); + return err; +} + +static int l2cap_sock_sendmsg(struct kiocb *iocb, struct socket *sock, struct msghdr *msg, size_t len) +{ + struct sock *sk = sock->sk; + struct l2cap_chan *chan = l2cap_pi(sk)->chan; + int err; + + BT_DBG("sock %p, sk %p", sock, sk); + + err = sock_error(sk); + if (err) + return err; + + if (msg->msg_flags & MSG_OOB) + return -EOPNOTSUPP; + + l2cap_chan_lock(chan); + + if (sk->sk_state != BT_CONNECTED) { + l2cap_chan_unlock(chan); + return -ENOTCONN; + } + + err = l2cap_chan_send(chan, msg, len, sk->sk_priority); + l2cap_chan_unlock(chan); + + return err; +} + +static int l2cap_sock_recvmsg(struct kiocb *iocb, struct socket *sock, struct msghdr *msg, size_t len, int flags) +{ + struct sock *sk = sock->sk; + struct l2cap_pinfo *pi = l2cap_pi(sk); + int err; + + lock_sock(sk); + + if (sk->sk_state == BT_CONNECT2 && bt_sk(sk)->defer_setup) { + sk->sk_state = BT_CONFIG; + pi->chan->state = BT_CONFIG; + + __l2cap_connect_rsp_defer(pi->chan); + release_sock(sk); + return 0; + } + + release_sock(sk); + + if (sock->type == SOCK_STREAM) + err = bt_sock_stream_recvmsg(iocb, sock, msg, len, flags); + else + err = bt_sock_recvmsg(iocb, sock, msg, len, flags); + + if (pi->chan->mode != L2CAP_MODE_ERTM) + return err; + + /* Attempt to put pending rx data in the socket buffer */ + + lock_sock(sk); + + if (!test_bit(CONN_LOCAL_BUSY, &pi->chan->conn_state)) + goto done; + + if (pi->rx_busy_skb) { + if (!sock_queue_rcv_skb(sk, pi->rx_busy_skb)) + pi->rx_busy_skb = NULL; + else + goto done; + } + + /* Restore data flow when half of the receive buffer is + * available. This avoids resending large numbers of + * frames. + */ + if (atomic_read(&sk->sk_rmem_alloc) <= sk->sk_rcvbuf >> 1) + l2cap_chan_busy(pi->chan, 0); + +done: + release_sock(sk); + return err; +} + +/* Kill socket (only if zapped and orphan) + * Must be called on unlocked socket. + */ +static void l2cap_sock_kill(struct sock *sk) +{ + if (!sock_flag(sk, SOCK_ZAPPED) || sk->sk_socket || + sock_flag(sk, SOCK_DEAD)) + return; + + BT_DBG("sk %p state %s", sk, state_to_string(sk->sk_state)); + + /* Kill poor orphan */ + + l2cap_chan_destroy(l2cap_pi(sk)->chan); + sock_set_flag(sk, SOCK_DEAD); + sock_put(sk); +} + +static int l2cap_sock_shutdown(struct socket *sock, int how) +{ + struct sock *sk = sock->sk; + struct l2cap_chan *chan; + struct l2cap_conn *conn; + int err = 0; + + BT_DBG("sock %p, sk %p", sock, sk); + + if (!sk) + return 0; + + chan = l2cap_pi(sk)->chan; + conn = chan->conn; + + if (conn) + mutex_lock(&conn->chan_lock); + + l2cap_chan_lock(chan); + lock_sock(sk); + + if (!sk->sk_shutdown) { + if (chan->mode == L2CAP_MODE_ERTM) + err = __l2cap_wait_ack(sk); + + sk->sk_shutdown = SHUTDOWN_MASK; + + release_sock(sk); + l2cap_chan_close(chan, 0); + lock_sock(sk); + + if (sock_flag(sk, SOCK_LINGER) && sk->sk_lingertime) + err = bt_sock_wait_state(sk, BT_CLOSED, + sk->sk_lingertime); + } + + if (!err && sk->sk_err) + err = -sk->sk_err; + + release_sock(sk); + l2cap_chan_unlock(chan); + + if (conn) + mutex_unlock(&conn->chan_lock); + + return err; +} + +static int l2cap_sock_release(struct socket *sock) +{ + struct sock *sk = sock->sk; + int err; + + BT_DBG("sock %p, sk %p", sock, sk); + + if (!sk) + return 0; + + err = l2cap_sock_shutdown(sock, 2); + + sock_orphan(sk); + l2cap_sock_kill(sk); + return err; +} + +static struct l2cap_chan *l2cap_sock_new_connection_cb(void *data) +{ + struct sock *sk, *parent = data; + + sk = l2cap_sock_alloc(sock_net(parent), NULL, BTPROTO_L2CAP, + GFP_ATOMIC); + if (!sk) + return NULL; + + bt_sock_reclassify_lock(sk, BTPROTO_L2CAP); + + l2cap_sock_init(sk, parent); + + return l2cap_pi(sk)->chan; +} + +static int l2cap_sock_recv_cb(void *data, struct sk_buff *skb) +{ + int err; + struct sock *sk = data; + struct l2cap_pinfo *pi = l2cap_pi(sk); + + lock_sock(sk); + + if (pi->rx_busy_skb) { + err = -ENOMEM; + goto done; + } + + err = sock_queue_rcv_skb(sk, skb); + + /* For ERTM, handle one skb that doesn't fit into the recv + * buffer. This is important to do because the data frames + * have already been acked, so the skb cannot be discarded. + * + * Notify the l2cap core that the buffer is full, so the + * LOCAL_BUSY state is entered and no more frames are + * acked and reassembled until there is buffer space + * available. + */ + if (err < 0 && pi->chan->mode == L2CAP_MODE_ERTM) { + pi->rx_busy_skb = skb; + l2cap_chan_busy(pi->chan, 1); + err = 0; + } + +done: + release_sock(sk); + + return err; +} + +static void l2cap_sock_close_cb(void *data) +{ + struct sock *sk = data; + + l2cap_sock_kill(sk); +} + +static void l2cap_sock_state_change_cb(void *data, int state) +{ + struct sock *sk = data; + + sk->sk_state = state; +} + +static struct sk_buff *l2cap_sock_alloc_skb_cb(struct l2cap_chan *chan, + unsigned long len, int nb, + int *err) +{ + struct sk_buff *skb; + + l2cap_chan_unlock(chan); + + skb = bt_skb_send_alloc(chan->sk, len, nb, err); + + l2cap_chan_lock(chan); + + return skb; +} + +static struct l2cap_ops l2cap_chan_ops = { + .name = "L2CAP Socket Interface", + .new_connection = l2cap_sock_new_connection_cb, + .recv = l2cap_sock_recv_cb, + .close = l2cap_sock_close_cb, + .state_change = l2cap_sock_state_change_cb, + .alloc_skb = l2cap_sock_alloc_skb_cb, +}; + +static void l2cap_sock_destruct(struct sock *sk) +{ + BT_DBG("sk %p", sk); + + if (l2cap_pi(sk)->rx_busy_skb) { + kfree_skb(l2cap_pi(sk)->rx_busy_skb); + l2cap_pi(sk)->rx_busy_skb = NULL; + } + + skb_queue_purge(&sk->sk_receive_queue); + skb_queue_purge(&sk->sk_write_queue); +} + +static void l2cap_sock_init(struct sock *sk, struct sock *parent) +{ + struct l2cap_pinfo *pi = l2cap_pi(sk); + struct l2cap_chan *chan = pi->chan; + + BT_DBG("sk %p", sk); + + if (parent) { + struct l2cap_chan *pchan = l2cap_pi(parent)->chan; + + sk->sk_type = parent->sk_type; + bt_sk(sk)->defer_setup = bt_sk(parent)->defer_setup; + + chan->chan_type = pchan->chan_type; + chan->imtu = pchan->imtu; + chan->omtu = pchan->omtu; + chan->conf_state = pchan->conf_state; + chan->mode = pchan->mode; + chan->fcs = pchan->fcs; + chan->max_tx = pchan->max_tx; + chan->tx_win = pchan->tx_win; + chan->tx_win_max = pchan->tx_win_max; + chan->sec_level = pchan->sec_level; + chan->flags = pchan->flags; + + security_sk_clone(parent, sk); + } else { + + switch (sk->sk_type) { + case SOCK_RAW: + chan->chan_type = L2CAP_CHAN_RAW; + break; + case SOCK_DGRAM: + chan->chan_type = L2CAP_CHAN_CONN_LESS; + break; + case SOCK_SEQPACKET: + case SOCK_STREAM: + chan->chan_type = L2CAP_CHAN_CONN_ORIENTED; + break; + } + + chan->imtu = L2CAP_DEFAULT_MTU; + chan->omtu = 0; + if (!disable_ertm && sk->sk_type == SOCK_STREAM) { + chan->mode = L2CAP_MODE_ERTM; + set_bit(CONF_STATE2_DEVICE, &chan->conf_state); + } else { + chan->mode = L2CAP_MODE_BASIC; + } + chan->max_tx = L2CAP_DEFAULT_MAX_TX; + chan->fcs = L2CAP_FCS_CRC16; + chan->tx_win = L2CAP_DEFAULT_TX_WINDOW; + chan->tx_win_max = L2CAP_DEFAULT_TX_WINDOW; + chan->sec_level = BT_SECURITY_LOW; + chan->flags = 0; + set_bit(FLAG_FORCE_ACTIVE, &chan->flags); + } + + /* Default config options */ + chan->flush_to = L2CAP_DEFAULT_FLUSH_TO; + + chan->data = sk; + chan->ops = &l2cap_chan_ops; +} + +static struct proto l2cap_proto = { + .name = "L2CAP", + .owner = THIS_MODULE, + .obj_size = sizeof(struct l2cap_pinfo) +}; + +static struct sock *l2cap_sock_alloc(struct net *net, struct socket *sock, int proto, gfp_t prio) +{ + struct sock *sk; + struct l2cap_chan *chan; + + sk = sk_alloc(net, PF_BLUETOOTH, prio, &l2cap_proto); + if (!sk) + return NULL; + + sock_init_data(sock, sk); + INIT_LIST_HEAD(&bt_sk(sk)->accept_q); + + sk->sk_destruct = l2cap_sock_destruct; + sk->sk_sndtimeo = L2CAP_CONN_TIMEOUT; + + sock_reset_flag(sk, SOCK_ZAPPED); + + sk->sk_protocol = proto; + sk->sk_state = BT_OPEN; + + chan = l2cap_chan_create(sk); + if (!chan) { + l2cap_sock_kill(sk); + return NULL; + } + + l2cap_pi(sk)->chan = chan; + + return sk; +} + +static int l2cap_sock_create(struct net *net, struct socket *sock, int protocol, + int kern) +{ + struct sock *sk; + + BT_DBG("sock %p", sock); + + sock->state = SS_UNCONNECTED; + + if (sock->type != SOCK_SEQPACKET && sock->type != SOCK_STREAM && + sock->type != SOCK_DGRAM && sock->type != SOCK_RAW) + return -ESOCKTNOSUPPORT; + + if (sock->type == SOCK_RAW && !kern && !capable(CAP_NET_RAW)) + return -EPERM; + + sock->ops = &l2cap_sock_ops; + + sk = l2cap_sock_alloc(net, sock, protocol, GFP_ATOMIC); + if (!sk) + return -ENOMEM; + + l2cap_sock_init(sk, NULL); + return 0; +} + +static const struct proto_ops l2cap_sock_ops = { + .family = PF_BLUETOOTH, + .owner = THIS_MODULE, + .release = l2cap_sock_release, + .bind = l2cap_sock_bind, + .connect = l2cap_sock_connect, + .listen = l2cap_sock_listen, + .accept = l2cap_sock_accept, + .getname = l2cap_sock_getname, + .sendmsg = l2cap_sock_sendmsg, + .recvmsg = l2cap_sock_recvmsg, + .poll = bt_sock_poll, + .ioctl = bt_sock_ioctl, + .mmap = sock_no_mmap, + .socketpair = sock_no_socketpair, + .shutdown = l2cap_sock_shutdown, + .setsockopt = l2cap_sock_setsockopt, + .getsockopt = l2cap_sock_getsockopt +}; + +static const struct net_proto_family l2cap_sock_family_ops = { + .family = PF_BLUETOOTH, + .owner = THIS_MODULE, + .create = l2cap_sock_create, +}; + +int __init l2cap_init_sockets(void) +{ + int err; + + err = proto_register(&l2cap_proto, 0); + if (err < 0) + return err; + + err = bt_sock_register(BTPROTO_L2CAP, &l2cap_sock_family_ops); + if (err < 0) + goto error; + + BT_INFO("L2CAP socket layer initialized"); + + return 0; + +error: + BT_ERR("L2CAP socket registration failed"); + proto_unregister(&l2cap_proto); + return err; +} + +void l2cap_cleanup_sockets(void) +{ + if (bt_sock_unregister(BTPROTO_L2CAP) < 0) + BT_ERR("L2CAP socket unregistration failed"); + + proto_unregister(&l2cap_proto); +} diff --git a/net/bluetooth_tizen/lib.c b/net/bluetooth_tizen/lib.c new file mode 100644 index 0000000..5066288 --- /dev/null +++ b/net/bluetooth_tizen/lib.c @@ -0,0 +1,192 @@ +/* + BlueZ - Bluetooth protocol stack for Linux + Copyright (C) 2000-2001 Qualcomm Incorporated + + Written 2000,2001 by Maxim Krasnyansky <maxk@qualcomm.com> + + This program is free software; you can redistribute it and/or modify + it under the terms of the GNU General Public License version 2 as + published by the Free Software Foundation; + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS + OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT OF THIRD PARTY RIGHTS. + IN NO EVENT SHALL THE COPYRIGHT HOLDER(S) AND AUTHOR(S) BE LIABLE FOR ANY + CLAIM, OR ANY SPECIAL INDIRECT OR CONSEQUENTIAL DAMAGES, OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + + ALL LIABILITY, INCLUDING LIABILITY FOR INFRINGEMENT OF ANY PATENTS, + COPYRIGHTS, TRADEMARKS OR OTHER RIGHTS, RELATING TO USE OF THIS + SOFTWARE IS DISCLAIMED. +*/ + +/* Bluetooth kernel library. */ + +#define pr_fmt(fmt) "Bluetooth: " fmt + +#include <linux/module.h> + +#include <linux/kernel.h> +#include <linux/stddef.h> +#include <linux/string.h> +#include <asm/errno.h> + +#include <net/bluetooth/bluetooth.h> + +void baswap(bdaddr_t *dst, bdaddr_t *src) +{ + unsigned char *d = (unsigned char *) dst; + unsigned char *s = (unsigned char *) src; + unsigned int i; + + for (i = 0; i < 6; i++) + d[i] = s[5 - i]; +} +EXPORT_SYMBOL(baswap); + +char *batostr(bdaddr_t *ba) +{ + static char str[2][18]; + static int i = 1; + + i ^= 1; + sprintf(str[i], "%2.2X:%2.2X:%2.2X:%2.2X:%2.2X:%2.2X", + ba->b[5], ba->b[4], ba->b[3], + ba->b[2], ba->b[1], ba->b[0]); + + return str[i]; +} +EXPORT_SYMBOL(batostr); + +/* Bluetooth error codes to Unix errno mapping */ +int bt_to_errno(__u16 code) +{ + switch (code) { + case 0: + return 0; + + case 0x01: + return EBADRQC; + + case 0x02: + return ENOTCONN; + + case 0x03: + return EIO; + + case 0x04: + return EHOSTDOWN; + + case 0x05: + return EACCES; + + case 0x06: + return EBADE; + + case 0x07: + return ENOMEM; + + case 0x08: + return ETIMEDOUT; + + case 0x09: + return EMLINK; + + case 0x0a: + return EMLINK; + + case 0x0b: + return EALREADY; + + case 0x0c: + return EBUSY; + + case 0x0d: + case 0x0e: + case 0x0f: + return ECONNREFUSED; + + case 0x10: + return ETIMEDOUT; + + case 0x11: + case 0x27: + case 0x29: + case 0x20: + return EOPNOTSUPP; + + case 0x12: + return EINVAL; + + case 0x13: + case 0x14: + case 0x15: + return ECONNRESET; + + case 0x16: + return ECONNABORTED; + + case 0x17: + return ELOOP; + + case 0x18: + return EACCES; + + case 0x1a: + return EPROTONOSUPPORT; + + case 0x1b: + return ECONNREFUSED; + + case 0x19: + case 0x1e: + case 0x23: + case 0x24: + case 0x25: + return EPROTO; + + default: + return ENOSYS; + } +} +EXPORT_SYMBOL(bt_to_errno); + +int bt_info(const char *format, ...) +{ + struct va_format vaf; + va_list args; + int r; + + va_start(args, format); + + vaf.fmt = format; + vaf.va = &args; + + r = pr_info("%pV", &vaf); + + va_end(args); + + return r; +} +EXPORT_SYMBOL(bt_info); + +int bt_err(const char *format, ...) +{ + struct va_format vaf; + va_list args; + int r; + + va_start(args, format); + + vaf.fmt = format; + vaf.va = &args; + + r = pr_err("%pV", &vaf); + + va_end(args); + + return r; +} +EXPORT_SYMBOL(bt_err); diff --git a/net/bluetooth_tizen/mgmt.c b/net/bluetooth_tizen/mgmt.c new file mode 100644 index 0000000..4bb03b1 --- /dev/null +++ b/net/bluetooth_tizen/mgmt.c @@ -0,0 +1,3599 @@ +/* + BlueZ - Bluetooth protocol stack for Linux + + Copyright (C) 2010 Nokia Corporation + Copyright (C) 2011-2012 Intel Corporation + + This program is free software; you can redistribute it and/or modify + it under the terms of the GNU General Public License version 2 as + published by the Free Software Foundation; + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS + OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT OF THIRD PARTY RIGHTS. + IN NO EVENT SHALL THE COPYRIGHT HOLDER(S) AND AUTHOR(S) BE LIABLE FOR ANY + CLAIM, OR ANY SPECIAL INDIRECT OR CONSEQUENTIAL DAMAGES, OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + + ALL LIABILITY, INCLUDING LIABILITY FOR INFRINGEMENT OF ANY PATENTS, + COPYRIGHTS, TRADEMARKS OR OTHER RIGHTS, RELATING TO USE OF THIS + SOFTWARE IS DISCLAIMED. +*/ + +/* Bluetooth HCI Management interface */ + +#include <linux/kernel.h> +#include <linux/uaccess.h> +#include <linux/module.h> +#include <asm/unaligned.h> + +#include <net/bluetooth/bluetooth.h> +#include <net/bluetooth/hci_core.h> +#include <net/bluetooth/mgmt.h> +#include <net/bluetooth/smp.h> + +bool enable_hs; +bool enable_le; + +#define MGMT_VERSION 1 +#define MGMT_REVISION 0 + +static const u16 mgmt_commands[] = { + MGMT_OP_READ_INDEX_LIST, + MGMT_OP_READ_INFO, + MGMT_OP_SET_POWERED, + MGMT_OP_SET_DISCOVERABLE, + MGMT_OP_SET_CONNECTABLE, + MGMT_OP_SET_FAST_CONNECTABLE, + MGMT_OP_SET_PAIRABLE, + MGMT_OP_SET_LINK_SECURITY, + MGMT_OP_SET_SSP, + MGMT_OP_SET_HS, + MGMT_OP_SET_LE, + MGMT_OP_SET_DEV_CLASS, + MGMT_OP_SET_LOCAL_NAME, + MGMT_OP_ADD_UUID, + MGMT_OP_REMOVE_UUID, + MGMT_OP_LOAD_LINK_KEYS, + MGMT_OP_LOAD_LONG_TERM_KEYS, + MGMT_OP_DISCONNECT, + MGMT_OP_GET_CONNECTIONS, + MGMT_OP_PIN_CODE_REPLY, + MGMT_OP_PIN_CODE_NEG_REPLY, + MGMT_OP_SET_IO_CAPABILITY, + MGMT_OP_PAIR_DEVICE, + MGMT_OP_CANCEL_PAIR_DEVICE, + MGMT_OP_UNPAIR_DEVICE, + MGMT_OP_USER_CONFIRM_REPLY, + MGMT_OP_USER_CONFIRM_NEG_REPLY, + MGMT_OP_USER_PASSKEY_REPLY, + MGMT_OP_USER_PASSKEY_NEG_REPLY, + MGMT_OP_READ_LOCAL_OOB_DATA, + MGMT_OP_ADD_REMOTE_OOB_DATA, + MGMT_OP_REMOVE_REMOTE_OOB_DATA, + MGMT_OP_START_DISCOVERY, + MGMT_OP_STOP_DISCOVERY, + MGMT_OP_CONFIRM_NAME, + MGMT_OP_BLOCK_DEVICE, + MGMT_OP_UNBLOCK_DEVICE, +}; + +static const u16 mgmt_events[] = { + MGMT_EV_CONTROLLER_ERROR, + MGMT_EV_INDEX_ADDED, + MGMT_EV_INDEX_REMOVED, + MGMT_EV_NEW_SETTINGS, + MGMT_EV_CLASS_OF_DEV_CHANGED, + MGMT_EV_LOCAL_NAME_CHANGED, + MGMT_EV_NEW_LINK_KEY, + MGMT_EV_NEW_LONG_TERM_KEY, + MGMT_EV_DEVICE_CONNECTED, + MGMT_EV_DEVICE_DISCONNECTED, + MGMT_EV_CONNECT_FAILED, + MGMT_EV_PIN_CODE_REQUEST, + MGMT_EV_USER_CONFIRM_REQUEST, + MGMT_EV_USER_PASSKEY_REQUEST, + MGMT_EV_AUTH_FAILED, + MGMT_EV_DEVICE_FOUND, + MGMT_EV_DISCOVERING, + MGMT_EV_DEVICE_BLOCKED, + MGMT_EV_DEVICE_UNBLOCKED, + MGMT_EV_DEVICE_UNPAIRED, +}; + +/* + * These LE scan and inquiry parameters were chosen according to LE General + * Discovery Procedure specification. + */ +#define LE_SCAN_TYPE 0x01 +#define LE_SCAN_WIN 0x12 +#define LE_SCAN_INT 0x12 +#define LE_SCAN_TIMEOUT_LE_ONLY 10240 /* TGAP(gen_disc_scan_min) */ +#define LE_SCAN_TIMEOUT_BREDR_LE 5120 /* TGAP(100)/2 */ + +#define INQUIRY_LEN_BREDR 0x08 /* TGAP(100) */ +#define INQUIRY_LEN_BREDR_LE 0x04 /* TGAP(100)/2 */ + +#define CACHE_TIMEOUT msecs_to_jiffies(2 * 1000) + +#define hdev_is_powered(hdev) (test_bit(HCI_UP, &hdev->flags) && \ + !test_bit(HCI_AUTO_OFF, &hdev->dev_flags)) + +struct pending_cmd { + struct list_head list; + u16 opcode; + int index; + void *param; + struct sock *sk; + void *user_data; +}; + +/* HCI to MGMT error code conversion table */ +static u8 mgmt_status_table[] = { + MGMT_STATUS_SUCCESS, + MGMT_STATUS_UNKNOWN_COMMAND, /* Unknown Command */ + MGMT_STATUS_NOT_CONNECTED, /* No Connection */ + MGMT_STATUS_FAILED, /* Hardware Failure */ + MGMT_STATUS_CONNECT_FAILED, /* Page Timeout */ + MGMT_STATUS_AUTH_FAILED, /* Authentication Failed */ + MGMT_STATUS_NOT_PAIRED, /* PIN or Key Missing */ + MGMT_STATUS_NO_RESOURCES, /* Memory Full */ + MGMT_STATUS_TIMEOUT, /* Connection Timeout */ + MGMT_STATUS_NO_RESOURCES, /* Max Number of Connections */ + MGMT_STATUS_NO_RESOURCES, /* Max Number of SCO Connections */ + MGMT_STATUS_ALREADY_CONNECTED, /* ACL Connection Exists */ + MGMT_STATUS_BUSY, /* Command Disallowed */ + MGMT_STATUS_NO_RESOURCES, /* Rejected Limited Resources */ + MGMT_STATUS_REJECTED, /* Rejected Security */ + MGMT_STATUS_REJECTED, /* Rejected Personal */ + MGMT_STATUS_TIMEOUT, /* Host Timeout */ + MGMT_STATUS_NOT_SUPPORTED, /* Unsupported Feature */ + MGMT_STATUS_INVALID_PARAMS, /* Invalid Parameters */ + MGMT_STATUS_DISCONNECTED, /* OE User Ended Connection */ + MGMT_STATUS_NO_RESOURCES, /* OE Low Resources */ + MGMT_STATUS_DISCONNECTED, /* OE Power Off */ + MGMT_STATUS_DISCONNECTED, /* Connection Terminated */ + MGMT_STATUS_BUSY, /* Repeated Attempts */ + MGMT_STATUS_REJECTED, /* Pairing Not Allowed */ + MGMT_STATUS_FAILED, /* Unknown LMP PDU */ + MGMT_STATUS_NOT_SUPPORTED, /* Unsupported Remote Feature */ + MGMT_STATUS_REJECTED, /* SCO Offset Rejected */ + MGMT_STATUS_REJECTED, /* SCO Interval Rejected */ + MGMT_STATUS_REJECTED, /* Air Mode Rejected */ + MGMT_STATUS_INVALID_PARAMS, /* Invalid LMP Parameters */ + MGMT_STATUS_FAILED, /* Unspecified Error */ + MGMT_STATUS_NOT_SUPPORTED, /* Unsupported LMP Parameter Value */ + MGMT_STATUS_FAILED, /* Role Change Not Allowed */ + MGMT_STATUS_TIMEOUT, /* LMP Response Timeout */ + MGMT_STATUS_FAILED, /* LMP Error Transaction Collision */ + MGMT_STATUS_FAILED, /* LMP PDU Not Allowed */ + MGMT_STATUS_REJECTED, /* Encryption Mode Not Accepted */ + MGMT_STATUS_FAILED, /* Unit Link Key Used */ + MGMT_STATUS_NOT_SUPPORTED, /* QoS Not Supported */ + MGMT_STATUS_TIMEOUT, /* Instant Passed */ + MGMT_STATUS_NOT_SUPPORTED, /* Pairing Not Supported */ + MGMT_STATUS_FAILED, /* Transaction Collision */ + MGMT_STATUS_INVALID_PARAMS, /* Unacceptable Parameter */ + MGMT_STATUS_REJECTED, /* QoS Rejected */ + MGMT_STATUS_NOT_SUPPORTED, /* Classification Not Supported */ + MGMT_STATUS_REJECTED, /* Insufficient Security */ + MGMT_STATUS_INVALID_PARAMS, /* Parameter Out Of Range */ + MGMT_STATUS_BUSY, /* Role Switch Pending */ + MGMT_STATUS_FAILED, /* Slot Violation */ + MGMT_STATUS_FAILED, /* Role Switch Failed */ + MGMT_STATUS_INVALID_PARAMS, /* EIR Too Large */ + MGMT_STATUS_NOT_SUPPORTED, /* Simple Pairing Not Supported */ + MGMT_STATUS_BUSY, /* Host Busy Pairing */ + MGMT_STATUS_REJECTED, /* Rejected, No Suitable Channel */ + MGMT_STATUS_BUSY, /* Controller Busy */ + MGMT_STATUS_INVALID_PARAMS, /* Unsuitable Connection Interval */ + MGMT_STATUS_TIMEOUT, /* Directed Advertising Timeout */ + MGMT_STATUS_AUTH_FAILED, /* Terminated Due to MIC Failure */ + MGMT_STATUS_CONNECT_FAILED, /* Connection Establishment Failed */ + MGMT_STATUS_CONNECT_FAILED, /* MAC Connection Failed */ +}; + +static u8 mgmt_status(u8 hci_status) +{ + if (hci_status < ARRAY_SIZE(mgmt_status_table)) + return mgmt_status_table[hci_status]; + + return MGMT_STATUS_FAILED; +} + +static int cmd_status(struct sock *sk, u16 index, u16 cmd, u8 status) +{ + struct sk_buff *skb; + struct mgmt_hdr *hdr; + struct mgmt_ev_cmd_status *ev; + int err; + + BT_DBG("sock %p, index %u, cmd %u, status %u", sk, index, cmd, status); + + skb = alloc_skb(sizeof(*hdr) + sizeof(*ev), GFP_ATOMIC); + if (!skb) + return -ENOMEM; + + hdr = (void *) skb_put(skb, sizeof(*hdr)); + + hdr->opcode = cpu_to_le16(MGMT_EV_CMD_STATUS); + hdr->index = cpu_to_le16(index); + hdr->len = cpu_to_le16(sizeof(*ev)); + + ev = (void *) skb_put(skb, sizeof(*ev)); + ev->status = status; + put_unaligned_le16(cmd, &ev->opcode); + + err = sock_queue_rcv_skb(sk, skb); + if (err < 0) + kfree_skb(skb); + + return err; +} + +static int cmd_complete(struct sock *sk, u16 index, u16 cmd, u8 status, + void *rp, size_t rp_len) +{ + struct sk_buff *skb; + struct mgmt_hdr *hdr; + struct mgmt_ev_cmd_complete *ev; + int err; + + BT_DBG("sock %p", sk); + + skb = alloc_skb(sizeof(*hdr) + sizeof(*ev) + rp_len, GFP_ATOMIC); + if (!skb) + return -ENOMEM; + + hdr = (void *) skb_put(skb, sizeof(*hdr)); + + hdr->opcode = cpu_to_le16(MGMT_EV_CMD_COMPLETE); + hdr->index = cpu_to_le16(index); + hdr->len = cpu_to_le16(sizeof(*ev) + rp_len); + + ev = (void *) skb_put(skb, sizeof(*ev) + rp_len); + put_unaligned_le16(cmd, &ev->opcode); + ev->status = status; + + if (rp) + memcpy(ev->data, rp, rp_len); + + err = sock_queue_rcv_skb(sk, skb); + if (err < 0) + kfree_skb(skb); + + return err; +} + +static int read_version(struct sock *sk, struct hci_dev *hdev, void *data, + u16 data_len) +{ + struct mgmt_rp_read_version rp; + + BT_DBG("sock %p", sk); + + rp.version = MGMT_VERSION; + put_unaligned_le16(MGMT_REVISION, &rp.revision); + + return cmd_complete(sk, MGMT_INDEX_NONE, MGMT_OP_READ_VERSION, 0, &rp, + sizeof(rp)); +} + +static int read_commands(struct sock *sk, struct hci_dev *hdev, void *data, + u16 data_len) +{ + struct mgmt_rp_read_commands *rp; + u16 num_commands = ARRAY_SIZE(mgmt_commands); + u16 num_events = ARRAY_SIZE(mgmt_events); + u16 *opcode; + size_t rp_size; + int i, err; + + BT_DBG("sock %p", sk); + + rp_size = sizeof(*rp) + ((num_commands + num_events) * sizeof(u16)); + + rp = kmalloc(rp_size, GFP_KERNEL); + if (!rp) + return -ENOMEM; + + put_unaligned_le16(num_commands, &rp->num_commands); + put_unaligned_le16(num_events, &rp->num_events); + + for (i = 0, opcode = rp->opcodes; i < num_commands; i++, opcode++) + put_unaligned_le16(mgmt_commands[i], opcode); + + for (i = 0; i < num_events; i++, opcode++) + put_unaligned_le16(mgmt_events[i], opcode); + + err = cmd_complete(sk, MGMT_INDEX_NONE, MGMT_OP_READ_COMMANDS, 0, rp, + rp_size); + kfree(rp); + + return err; +} + +static int read_index_list(struct sock *sk, struct hci_dev *hdev, void *data, + u16 data_len) +{ + struct mgmt_rp_read_index_list *rp; + struct list_head *p; + struct hci_dev *d; + size_t rp_len; + u16 count; + int i, err; + + BT_DBG("sock %p", sk); + + read_lock(&hci_dev_list_lock); + + count = 0; + list_for_each(p, &hci_dev_list) { + count++; + } + + rp_len = sizeof(*rp) + (2 * count); + rp = kmalloc(rp_len, GFP_ATOMIC); + if (!rp) { + read_unlock(&hci_dev_list_lock); + return -ENOMEM; + } + + put_unaligned_le16(count, &rp->num_controllers); + + i = 0; + list_for_each_entry(d, &hci_dev_list, list) { + if (test_bit(HCI_SETUP, &d->dev_flags)) + continue; + + put_unaligned_le16(d->id, &rp->index[i++]); + BT_DBG("Added hci%u", d->id); + } + + read_unlock(&hci_dev_list_lock); + + err = cmd_complete(sk, MGMT_INDEX_NONE, MGMT_OP_READ_INDEX_LIST, 0, rp, + rp_len); + + kfree(rp); + + return err; +} + +static u32 get_supported_settings(struct hci_dev *hdev) +{ + u32 settings = 0; + + settings |= MGMT_SETTING_POWERED; + settings |= MGMT_SETTING_CONNECTABLE; + settings |= MGMT_SETTING_FAST_CONNECTABLE; + settings |= MGMT_SETTING_DISCOVERABLE; + settings |= MGMT_SETTING_PAIRABLE; + + if (hdev->features[6] & LMP_SIMPLE_PAIR) + settings |= MGMT_SETTING_SSP; + + if (!(hdev->features[4] & LMP_NO_BREDR)) { + settings |= MGMT_SETTING_BREDR; + settings |= MGMT_SETTING_LINK_SECURITY; + } + + if (enable_hs) + settings |= MGMT_SETTING_HS; + + if (enable_le) { + if (hdev->features[4] & LMP_LE) + settings |= MGMT_SETTING_LE; + } + + return settings; +} + +static u32 get_current_settings(struct hci_dev *hdev) +{ + u32 settings = 0; + + if (hdev_is_powered(hdev)) + settings |= MGMT_SETTING_POWERED; + + if (test_bit(HCI_CONNECTABLE, &hdev->dev_flags)) + settings |= MGMT_SETTING_CONNECTABLE; + + if (test_bit(HCI_DISCOVERABLE, &hdev->dev_flags)) + settings |= MGMT_SETTING_DISCOVERABLE; + + if (test_bit(HCI_PAIRABLE, &hdev->dev_flags)) + settings |= MGMT_SETTING_PAIRABLE; + + if (!(hdev->features[4] & LMP_NO_BREDR)) + settings |= MGMT_SETTING_BREDR; + + if (test_bit(HCI_LE_ENABLED, &hdev->dev_flags)) + settings |= MGMT_SETTING_LE; + + if (test_bit(HCI_LINK_SECURITY, &hdev->dev_flags)) + settings |= MGMT_SETTING_LINK_SECURITY; + + if (test_bit(HCI_SSP_ENABLED, &hdev->dev_flags)) + settings |= MGMT_SETTING_SSP; + + if (test_bit(HCI_HS_ENABLED, &hdev->dev_flags)) + settings |= MGMT_SETTING_HS; + + return settings; +} + +#define PNP_INFO_SVCLASS_ID 0x1200 + +static u8 bluetooth_base_uuid[] = { + 0xFB, 0x34, 0x9B, 0x5F, 0x80, 0x00, 0x00, 0x80, + 0x00, 0x10, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, +}; + +static u16 get_uuid16(u8 *uuid128) +{ + u32 val; + int i; + + for (i = 0; i < 12; i++) { + if (bluetooth_base_uuid[i] != uuid128[i]) + return 0; + } + + memcpy(&val, &uuid128[12], 4); + + val = le32_to_cpu(val); + if (val > 0xffff) + return 0; + + return (u16) val; +} + +static void create_eir(struct hci_dev *hdev, u8 *data) +{ + u8 *ptr = data; + u16 eir_len = 0; + u16 uuid16_list[HCI_MAX_EIR_LENGTH / sizeof(u16)]; + int i, truncated = 0; + struct bt_uuid *uuid; + size_t name_len; + + name_len = strlen(hdev->dev_name); + + if (name_len > 0) { + /* EIR Data type */ + if (name_len > 48) { + name_len = 48; + ptr[1] = EIR_NAME_SHORT; + } else + ptr[1] = EIR_NAME_COMPLETE; + + /* EIR Data length */ + ptr[0] = name_len + 1; + + memcpy(ptr + 2, hdev->dev_name, name_len); + + eir_len += (name_len + 2); + ptr += (name_len + 2); + } + + memset(uuid16_list, 0, sizeof(uuid16_list)); + + /* Group all UUID16 types */ + list_for_each_entry(uuid, &hdev->uuids, list) { + u16 uuid16; + + uuid16 = get_uuid16(uuid->uuid); + if (uuid16 == 0) + return; + + if (uuid16 < 0x1100) + continue; + + if (uuid16 == PNP_INFO_SVCLASS_ID) + continue; + + /* Stop if not enough space to put next UUID */ + if (eir_len + 2 + sizeof(u16) > HCI_MAX_EIR_LENGTH) { + truncated = 1; + break; + } + + /* Check for duplicates */ + for (i = 0; uuid16_list[i] != 0; i++) + if (uuid16_list[i] == uuid16) + break; + + if (uuid16_list[i] == 0) { + uuid16_list[i] = uuid16; + eir_len += sizeof(u16); + } + } + + if (uuid16_list[0] != 0) { + u8 *length = ptr; + + /* EIR Data type */ + ptr[1] = truncated ? EIR_UUID16_SOME : EIR_UUID16_ALL; + + ptr += 2; + eir_len += 2; + + for (i = 0; uuid16_list[i] != 0; i++) { + *ptr++ = (uuid16_list[i] & 0x00ff); + *ptr++ = (uuid16_list[i] & 0xff00) >> 8; + } + + /* EIR Data length */ + *length = (i * sizeof(u16)) + 1; + } +} + +static int update_eir(struct hci_dev *hdev) +{ + struct hci_cp_write_eir cp; + + if (!hdev_is_powered(hdev)) + return 0; + + if (!(hdev->features[6] & LMP_EXT_INQ)) + return 0; + + if (!test_bit(HCI_SSP_ENABLED, &hdev->dev_flags)) + return 0; + + if (test_bit(HCI_SERVICE_CACHE, &hdev->dev_flags)) + return 0; + + memset(&cp, 0, sizeof(cp)); + + create_eir(hdev, cp.data); + + if (memcmp(cp.data, hdev->eir, sizeof(cp.data)) == 0) + return 0; + + memcpy(hdev->eir, cp.data, sizeof(cp.data)); + + return hci_send_cmd(hdev, HCI_OP_WRITE_EIR, sizeof(cp), &cp); +} + +static u8 get_service_classes(struct hci_dev *hdev) +{ + struct bt_uuid *uuid; + u8 val = 0; + + list_for_each_entry(uuid, &hdev->uuids, list) + val |= uuid->svc_hint; + + return val; +} + +static int update_class(struct hci_dev *hdev) +{ + u8 cod[3]; + int err; + + BT_DBG("%s", hdev->name); + + if (!hdev_is_powered(hdev)) + return 0; + + if (test_bit(HCI_SERVICE_CACHE, &hdev->dev_flags)) + return 0; + + cod[0] = hdev->minor_class; + cod[1] = hdev->major_class; + cod[2] = get_service_classes(hdev); + + if (memcmp(cod, hdev->dev_class, 3) == 0) + return 0; + + err = hci_send_cmd(hdev, HCI_OP_WRITE_CLASS_OF_DEV, sizeof(cod), cod); + if (err == 0) + set_bit(HCI_PENDING_CLASS, &hdev->dev_flags); + + return err; +} + +static void service_cache_off(struct work_struct *work) +{ + struct hci_dev *hdev = container_of(work, struct hci_dev, + service_cache.work); + + if (!test_and_clear_bit(HCI_SERVICE_CACHE, &hdev->dev_flags)) + return; + + hci_dev_lock(hdev); + + update_eir(hdev); + update_class(hdev); + + hci_dev_unlock(hdev); +} + +static void mgmt_init_hdev(struct sock *sk, struct hci_dev *hdev) +{ + if (test_and_set_bit(HCI_MGMT, &hdev->dev_flags)) + return; + + INIT_DELAYED_WORK(&hdev->service_cache, service_cache_off); + + /* Non-mgmt controlled devices get this bit set + * implicitly so that pairing works for them, however + * for mgmt we require user-space to explicitly enable + * it + */ + clear_bit(HCI_PAIRABLE, &hdev->dev_flags); +} + +static int read_controller_info(struct sock *sk, struct hci_dev *hdev, + void *data, u16 data_len) +{ + struct mgmt_rp_read_info rp; + + BT_DBG("sock %p %s", sk, hdev->name); + + hci_dev_lock(hdev); + + memset(&rp, 0, sizeof(rp)); + + bacpy(&rp.bdaddr, &hdev->bdaddr); + + rp.version = hdev->hci_ver; + + put_unaligned_le16(hdev->manufacturer, &rp.manufacturer); + + rp.supported_settings = cpu_to_le32(get_supported_settings(hdev)); + rp.current_settings = cpu_to_le32(get_current_settings(hdev)); + + memcpy(rp.dev_class, hdev->dev_class, 3); + + memcpy(rp.name, hdev->dev_name, sizeof(hdev->dev_name)); + memcpy(rp.short_name, hdev->short_name, sizeof(hdev->short_name)); + + hci_dev_unlock(hdev); + + return cmd_complete(sk, hdev->id, MGMT_OP_READ_INFO, 0, &rp, + sizeof(rp)); +} + +static void mgmt_pending_free(struct pending_cmd *cmd) +{ + sock_put(cmd->sk); + kfree(cmd->param); + kfree(cmd); +} + +static struct pending_cmd *mgmt_pending_add(struct sock *sk, u16 opcode, + struct hci_dev *hdev, void *data, + u16 len) +{ + struct pending_cmd *cmd; + + cmd = kmalloc(sizeof(*cmd), GFP_ATOMIC); + if (!cmd) + return NULL; + + cmd->opcode = opcode; + cmd->index = hdev->id; + + cmd->param = kmalloc(len, GFP_ATOMIC); + if (!cmd->param) { + kfree(cmd); + return NULL; + } + + if (data) + memcpy(cmd->param, data, len); + + cmd->sk = sk; + sock_hold(sk); + + list_add(&cmd->list, &hdev->mgmt_pending); + + return cmd; +} + +static void mgmt_pending_foreach(u16 opcode, struct hci_dev *hdev, + void (*cb)(struct pending_cmd *cmd, void *data), + void *data) +{ + struct list_head *p, *n; + + list_for_each_safe(p, n, &hdev->mgmt_pending) { + struct pending_cmd *cmd; + + cmd = list_entry(p, struct pending_cmd, list); + + if (opcode > 0 && cmd->opcode != opcode) + continue; + + cb(cmd, data); + } +} + +static struct pending_cmd *mgmt_pending_find(u16 opcode, struct hci_dev *hdev) +{ + struct pending_cmd *cmd; + + list_for_each_entry(cmd, &hdev->mgmt_pending, list) { + if (cmd->opcode == opcode) + return cmd; + } + + return NULL; +} + +static void mgmt_pending_remove(struct pending_cmd *cmd) +{ + list_del(&cmd->list); + mgmt_pending_free(cmd); +} + +static int send_settings_rsp(struct sock *sk, u16 opcode, struct hci_dev *hdev) +{ + __le32 settings = cpu_to_le32(get_current_settings(hdev)); + + return cmd_complete(sk, hdev->id, opcode, 0, &settings, + sizeof(settings)); +} + +static int set_powered(struct sock *sk, struct hci_dev *hdev, void *data, + u16 len) +{ + struct mgmt_mode *cp = data; + struct pending_cmd *cmd; + int err; + + BT_DBG("request for %s", hdev->name); + + hci_dev_lock(hdev); + + if (test_and_clear_bit(HCI_AUTO_OFF, &hdev->dev_flags)) { + cancel_delayed_work(&hdev->power_off); + + if (cp->val) { + err = send_settings_rsp(sk, MGMT_OP_SET_POWERED, hdev); + mgmt_powered(hdev, 1); + goto failed; + } + } + + if (!!cp->val == hdev_is_powered(hdev)) { + err = send_settings_rsp(sk, MGMT_OP_SET_POWERED, hdev); + goto failed; + } + + if (mgmt_pending_find(MGMT_OP_SET_POWERED, hdev)) { + err = cmd_status(sk, hdev->id, MGMT_OP_SET_POWERED, + MGMT_STATUS_BUSY); + goto failed; + } + + cmd = mgmt_pending_add(sk, MGMT_OP_SET_POWERED, hdev, data, len); + if (!cmd) { + err = -ENOMEM; + goto failed; + } + + if (cp->val) + schedule_work(&hdev->power_on); + else + schedule_work(&hdev->power_off.work); + + err = 0; + +failed: + hci_dev_unlock(hdev); + return err; +} + +static int mgmt_event(u16 event, struct hci_dev *hdev, void *data, u16 data_len, + struct sock *skip_sk) +{ + struct sk_buff *skb; + struct mgmt_hdr *hdr; + + skb = alloc_skb(sizeof(*hdr) + data_len, GFP_ATOMIC); + if (!skb) + return -ENOMEM; + + hdr = (void *) skb_put(skb, sizeof(*hdr)); + hdr->opcode = cpu_to_le16(event); + if (hdev) + hdr->index = cpu_to_le16(hdev->id); + else + hdr->index = cpu_to_le16(MGMT_INDEX_NONE); + hdr->len = cpu_to_le16(data_len); + + if (data) + memcpy(skb_put(skb, data_len), data, data_len); + + /* Time stamp */ + __net_timestamp(skb); + + hci_send_to_control(skb, skip_sk); + kfree_skb(skb); + + return 0; +} + +static int new_settings(struct hci_dev *hdev, struct sock *skip) +{ + __le32 ev; + + ev = cpu_to_le32(get_current_settings(hdev)); + + return mgmt_event(MGMT_EV_NEW_SETTINGS, hdev, &ev, sizeof(ev), skip); +} + +static int set_discoverable(struct sock *sk, struct hci_dev *hdev, void *data, + u16 len) +{ + struct mgmt_cp_set_discoverable *cp = data; + struct pending_cmd *cmd; + u16 timeout; + u8 scan; + int err; + + BT_DBG("request for %s", hdev->name); + + timeout = get_unaligned_le16(&cp->timeout); + if (!cp->val && timeout > 0) + return cmd_status(sk, hdev->id, MGMT_OP_SET_DISCOVERABLE, + MGMT_STATUS_INVALID_PARAMS); + + hci_dev_lock(hdev); + + if (!hdev_is_powered(hdev) && timeout > 0) { + err = cmd_status(sk, hdev->id, MGMT_OP_SET_DISCOVERABLE, + MGMT_STATUS_NOT_POWERED); + goto failed; + } + + if (mgmt_pending_find(MGMT_OP_SET_DISCOVERABLE, hdev) || + mgmt_pending_find(MGMT_OP_SET_CONNECTABLE, hdev)) { + err = cmd_status(sk, hdev->id, MGMT_OP_SET_DISCOVERABLE, + MGMT_STATUS_BUSY); + goto failed; + } + + if (!test_bit(HCI_CONNECTABLE, &hdev->dev_flags)) { + err = cmd_status(sk, hdev->id, MGMT_OP_SET_DISCOVERABLE, + MGMT_STATUS_REJECTED); + goto failed; + } + + if (!hdev_is_powered(hdev)) { + bool changed = false; + + if (!!cp->val != test_bit(HCI_DISCOVERABLE, &hdev->dev_flags)) { + change_bit(HCI_DISCOVERABLE, &hdev->dev_flags); + changed = true; + } + + err = send_settings_rsp(sk, MGMT_OP_SET_DISCOVERABLE, hdev); + if (err < 0) + goto failed; + + if (changed) + err = new_settings(hdev, sk); + + goto failed; + } + + if (!!cp->val == test_bit(HCI_DISCOVERABLE, &hdev->dev_flags)) { + if (hdev->discov_timeout > 0) { + cancel_delayed_work(&hdev->discov_off); + hdev->discov_timeout = 0; + } + + if (cp->val && timeout > 0) { + hdev->discov_timeout = timeout; + queue_delayed_work(hdev->workqueue, &hdev->discov_off, + msecs_to_jiffies(hdev->discov_timeout * 1000)); + } + + err = send_settings_rsp(sk, MGMT_OP_SET_DISCOVERABLE, hdev); + goto failed; + } + + cmd = mgmt_pending_add(sk, MGMT_OP_SET_DISCOVERABLE, hdev, data, len); + if (!cmd) { + err = -ENOMEM; + goto failed; + } + + scan = SCAN_PAGE; + + if (cp->val) + scan |= SCAN_INQUIRY; + else + cancel_delayed_work(&hdev->discov_off); + + err = hci_send_cmd(hdev, HCI_OP_WRITE_SCAN_ENABLE, 1, &scan); + if (err < 0) + mgmt_pending_remove(cmd); + + if (cp->val) + hdev->discov_timeout = timeout; + +failed: + hci_dev_unlock(hdev); + return err; +} + +static int set_connectable(struct sock *sk, struct hci_dev *hdev, void *data, + u16 len) +{ + struct mgmt_mode *cp = data; + struct pending_cmd *cmd; + u8 scan; + int err; + + BT_DBG("request for %s", hdev->name); + + hci_dev_lock(hdev); + + if (!hdev_is_powered(hdev)) { + bool changed = false; + + if (!!cp->val != test_bit(HCI_CONNECTABLE, &hdev->dev_flags)) + changed = true; + + if (cp->val) { + set_bit(HCI_CONNECTABLE, &hdev->dev_flags); + } else { + clear_bit(HCI_CONNECTABLE, &hdev->dev_flags); + clear_bit(HCI_DISCOVERABLE, &hdev->dev_flags); + } + + err = send_settings_rsp(sk, MGMT_OP_SET_CONNECTABLE, hdev); + if (err < 0) + goto failed; + + if (changed) + err = new_settings(hdev, sk); + + goto failed; + } + + if (mgmt_pending_find(MGMT_OP_SET_DISCOVERABLE, hdev) || + mgmt_pending_find(MGMT_OP_SET_CONNECTABLE, hdev)) { + err = cmd_status(sk, hdev->id, MGMT_OP_SET_CONNECTABLE, + MGMT_STATUS_BUSY); + goto failed; + } + + if (!!cp->val == test_bit(HCI_PSCAN, &hdev->flags)) { + err = send_settings_rsp(sk, MGMT_OP_SET_CONNECTABLE, hdev); + goto failed; + } + + cmd = mgmt_pending_add(sk, MGMT_OP_SET_CONNECTABLE, hdev, data, len); + if (!cmd) { + err = -ENOMEM; + goto failed; + } + + if (cp->val) { + scan = SCAN_PAGE; + } else { + scan = 0; + + if (test_bit(HCI_ISCAN, &hdev->flags) && + hdev->discov_timeout > 0) + cancel_delayed_work(&hdev->discov_off); + } + + err = hci_send_cmd(hdev, HCI_OP_WRITE_SCAN_ENABLE, 1, &scan); + if (err < 0) + mgmt_pending_remove(cmd); + +failed: + hci_dev_unlock(hdev); + return err; +} + +static int set_pairable(struct sock *sk, struct hci_dev *hdev, void *data, + u16 len) +{ + struct mgmt_mode *cp = data; + int err; + + BT_DBG("request for %s", hdev->name); + + hci_dev_lock(hdev); + + if (cp->val) + set_bit(HCI_PAIRABLE, &hdev->dev_flags); + else + clear_bit(HCI_PAIRABLE, &hdev->dev_flags); + + err = send_settings_rsp(sk, MGMT_OP_SET_PAIRABLE, hdev); + if (err < 0) + goto failed; + + err = new_settings(hdev, sk); + +failed: + hci_dev_unlock(hdev); + return err; +} + +static int set_link_security(struct sock *sk, struct hci_dev *hdev, void *data, + u16 len) +{ + struct mgmt_mode *cp = data; + struct pending_cmd *cmd; + u8 val; + int err; + + BT_DBG("request for %s", hdev->name); + + hci_dev_lock(hdev); + + if (!hdev_is_powered(hdev)) { + bool changed = false; + + if (!!cp->val != test_bit(HCI_LINK_SECURITY, + &hdev->dev_flags)) { + change_bit(HCI_LINK_SECURITY, &hdev->dev_flags); + changed = true; + } + + err = send_settings_rsp(sk, MGMT_OP_SET_LINK_SECURITY, hdev); + if (err < 0) + goto failed; + + if (changed) + err = new_settings(hdev, sk); + + goto failed; + } + + if (mgmt_pending_find(MGMT_OP_SET_LINK_SECURITY, hdev)) { + err = cmd_status(sk, hdev->id, MGMT_OP_SET_LINK_SECURITY, + MGMT_STATUS_BUSY); + goto failed; + } + + val = !!cp->val; + + if (test_bit(HCI_AUTH, &hdev->flags) == val) { + err = send_settings_rsp(sk, MGMT_OP_SET_LINK_SECURITY, hdev); + goto failed; + } + + cmd = mgmt_pending_add(sk, MGMT_OP_SET_LINK_SECURITY, hdev, data, len); + if (!cmd) { + err = -ENOMEM; + goto failed; + } + + err = hci_send_cmd(hdev, HCI_OP_WRITE_AUTH_ENABLE, sizeof(val), &val); + if (err < 0) { + mgmt_pending_remove(cmd); + goto failed; + } + +failed: + hci_dev_unlock(hdev); + return err; +} + +static int set_ssp(struct sock *sk, struct hci_dev *hdev, void *data, u16 len) +{ + struct mgmt_mode *cp = data; + struct pending_cmd *cmd; + u8 val; + int err; + + BT_DBG("request for %s", hdev->name); + + hci_dev_lock(hdev); + + if (!(hdev->features[6] & LMP_SIMPLE_PAIR)) { + err = cmd_status(sk, hdev->id, MGMT_OP_SET_SSP, + MGMT_STATUS_NOT_SUPPORTED); + goto failed; + } + + val = !!cp->val; + + if (!hdev_is_powered(hdev)) { + bool changed = false; + + if (val != test_bit(HCI_SSP_ENABLED, &hdev->dev_flags)) { + change_bit(HCI_SSP_ENABLED, &hdev->dev_flags); + changed = true; + } + + err = send_settings_rsp(sk, MGMT_OP_SET_SSP, hdev); + if (err < 0) + goto failed; + + if (changed) + err = new_settings(hdev, sk); + + goto failed; + } + + if (mgmt_pending_find(MGMT_OP_SET_SSP, hdev)) { + err = cmd_status(sk, hdev->id, MGMT_OP_SET_SSP, + MGMT_STATUS_BUSY); + goto failed; + } + + if (test_bit(HCI_SSP_ENABLED, &hdev->dev_flags) == val) { + err = send_settings_rsp(sk, MGMT_OP_SET_SSP, hdev); + goto failed; + } + + cmd = mgmt_pending_add(sk, MGMT_OP_SET_SSP, hdev, data, len); + if (!cmd) { + err = -ENOMEM; + goto failed; + } + + err = hci_send_cmd(hdev, HCI_OP_WRITE_SSP_MODE, sizeof(val), &val); + if (err < 0) { + mgmt_pending_remove(cmd); + goto failed; + } + +failed: + hci_dev_unlock(hdev); + return err; +} + +static int set_hs(struct sock *sk, struct hci_dev *hdev, void *data, u16 len) +{ + struct mgmt_mode *cp = data; + + BT_DBG("request for %s", hdev->name); + + if (!enable_hs) + return cmd_status(sk, hdev->id, MGMT_OP_SET_HS, + MGMT_STATUS_NOT_SUPPORTED); + + if (cp->val) + set_bit(HCI_HS_ENABLED, &hdev->dev_flags); + else + clear_bit(HCI_HS_ENABLED, &hdev->dev_flags); + + return send_settings_rsp(sk, MGMT_OP_SET_HS, hdev); +} + +static int set_le(struct sock *sk, struct hci_dev *hdev, void *data, u16 len) +{ + struct mgmt_mode *cp = data; + struct hci_cp_write_le_host_supported hci_cp; + struct pending_cmd *cmd; + int err; + u8 val, enabled; + + BT_DBG("request for %s", hdev->name); + + hci_dev_lock(hdev); + + if (!enable_le || !(hdev->features[4] & LMP_LE)) { + err = cmd_status(sk, hdev->id, MGMT_OP_SET_LE, + MGMT_STATUS_NOT_SUPPORTED); + goto unlock; + } + + val = !!cp->val; + enabled = !!(hdev->host_features[0] & LMP_HOST_LE); + + if (!hdev_is_powered(hdev) || val == enabled) { + bool changed = false; + + if (val != test_bit(HCI_LE_ENABLED, &hdev->dev_flags)) { + change_bit(HCI_LE_ENABLED, &hdev->dev_flags); + changed = true; + } + + err = send_settings_rsp(sk, MGMT_OP_SET_LE, hdev); + if (err < 0) + goto unlock; + + if (changed) + err = new_settings(hdev, sk); + + goto unlock; + } + + if (mgmt_pending_find(MGMT_OP_SET_LE, hdev)) { + err = cmd_status(sk, hdev->id, MGMT_OP_SET_LE, + MGMT_STATUS_BUSY); + goto unlock; + } + + cmd = mgmt_pending_add(sk, MGMT_OP_SET_LE, hdev, data, len); + if (!cmd) { + err = -ENOMEM; + goto unlock; + } + + memset(&hci_cp, 0, sizeof(hci_cp)); + + if (val) { + hci_cp.le = val; + hci_cp.simul = !!(hdev->features[6] & LMP_SIMUL_LE_BR); + } + + err = hci_send_cmd(hdev, HCI_OP_WRITE_LE_HOST_SUPPORTED, sizeof(hci_cp), + &hci_cp); + if (err < 0) { + mgmt_pending_remove(cmd); + goto unlock; + } + +unlock: + hci_dev_unlock(hdev); + return err; +} + +static int add_uuid(struct sock *sk, struct hci_dev *hdev, void *data, u16 len) +{ + struct mgmt_cp_add_uuid *cp = data; + struct pending_cmd *cmd; + struct bt_uuid *uuid; + int err; + + BT_DBG("request for %s", hdev->name); + + hci_dev_lock(hdev); + + if (test_bit(HCI_PENDING_CLASS, &hdev->dev_flags)) { + err = cmd_status(sk, hdev->id, MGMT_OP_ADD_UUID, + MGMT_STATUS_BUSY); + goto failed; + } + + uuid = kmalloc(sizeof(*uuid), GFP_ATOMIC); + if (!uuid) { + err = -ENOMEM; + goto failed; + } + + memcpy(uuid->uuid, cp->uuid, 16); + uuid->svc_hint = cp->svc_hint; + + list_add(&uuid->list, &hdev->uuids); + + err = update_class(hdev); + if (err < 0) + goto failed; + + err = update_eir(hdev); + if (err < 0) + goto failed; + + if (!test_bit(HCI_PENDING_CLASS, &hdev->dev_flags)) { + err = cmd_complete(sk, hdev->id, MGMT_OP_ADD_UUID, 0, + hdev->dev_class, 3); + goto failed; + } + + cmd = mgmt_pending_add(sk, MGMT_OP_ADD_UUID, hdev, data, len); + if (!cmd) { + err = -ENOMEM; + goto failed; + } + +failed: + hci_dev_unlock(hdev); + return err; +} + +static bool enable_service_cache(struct hci_dev *hdev) +{ + if (!hdev_is_powered(hdev)) + return false; + + if (!test_and_set_bit(HCI_SERVICE_CACHE, &hdev->dev_flags)) { + schedule_delayed_work(&hdev->service_cache, CACHE_TIMEOUT); + return true; + } + + return false; +} + +static int remove_uuid(struct sock *sk, struct hci_dev *hdev, void *data, + u16 len) +{ + struct mgmt_cp_remove_uuid *cp = data; + struct pending_cmd *cmd; + struct list_head *p, *n; + u8 bt_uuid_any[] = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 }; + int err, found; + + BT_DBG("request for %s", hdev->name); + + hci_dev_lock(hdev); + + if (test_bit(HCI_PENDING_CLASS, &hdev->dev_flags)) { + err = cmd_status(sk, hdev->id, MGMT_OP_REMOVE_UUID, + MGMT_STATUS_BUSY); + goto unlock; + } + + if (memcmp(cp->uuid, bt_uuid_any, 16) == 0) { + err = hci_uuids_clear(hdev); + + if (enable_service_cache(hdev)) { + err = cmd_complete(sk, hdev->id, MGMT_OP_REMOVE_UUID, + 0, hdev->dev_class, 3); + goto unlock; + } + + goto update_class; + } + + found = 0; + + list_for_each_safe(p, n, &hdev->uuids) { + struct bt_uuid *match = list_entry(p, struct bt_uuid, list); + + if (memcmp(match->uuid, cp->uuid, 16) != 0) + continue; + + list_del(&match->list); + found++; + } + + if (found == 0) { + err = cmd_status(sk, hdev->id, MGMT_OP_REMOVE_UUID, + MGMT_STATUS_INVALID_PARAMS); + goto unlock; + } + +update_class: + err = update_class(hdev); + if (err < 0) + goto unlock; + + err = update_eir(hdev); + if (err < 0) + goto unlock; + + if (!test_bit(HCI_PENDING_CLASS, &hdev->dev_flags)) { + err = cmd_complete(sk, hdev->id, MGMT_OP_REMOVE_UUID, 0, + hdev->dev_class, 3); + goto unlock; + } + + cmd = mgmt_pending_add(sk, MGMT_OP_REMOVE_UUID, hdev, data, len); + if (!cmd) { + err = -ENOMEM; + goto unlock; + } + +unlock: + hci_dev_unlock(hdev); + return err; +} + +static int set_dev_class(struct sock *sk, struct hci_dev *hdev, void *data, + u16 len) +{ + struct mgmt_cp_set_dev_class *cp = data; + struct pending_cmd *cmd; + int err; + + BT_DBG("request for %s", hdev->name); + + hci_dev_lock(hdev); + + if (test_bit(HCI_PENDING_CLASS, &hdev->dev_flags)) { + err = cmd_status(sk, hdev->id, MGMT_OP_SET_DEV_CLASS, + MGMT_STATUS_BUSY); + goto unlock; + } + + hdev->major_class = cp->major; + hdev->minor_class = cp->minor; + + if (!hdev_is_powered(hdev)) { + err = cmd_complete(sk, hdev->id, MGMT_OP_SET_DEV_CLASS, 0, + hdev->dev_class, 3); + goto unlock; + } + + if (test_and_clear_bit(HCI_SERVICE_CACHE, &hdev->dev_flags)) { + hci_dev_unlock(hdev); + cancel_delayed_work_sync(&hdev->service_cache); + hci_dev_lock(hdev); + update_eir(hdev); + } + + err = update_class(hdev); + if (err < 0) + goto unlock; + + if (!test_bit(HCI_PENDING_CLASS, &hdev->dev_flags)) { + err = cmd_complete(sk, hdev->id, MGMT_OP_SET_DEV_CLASS, 0, + hdev->dev_class, 3); + goto unlock; + } + + cmd = mgmt_pending_add(sk, MGMT_OP_SET_DEV_CLASS, hdev, data, len); + if (!cmd) { + err = -ENOMEM; + goto unlock; + } + +unlock: + hci_dev_unlock(hdev); + return err; +} + +static int load_link_keys(struct sock *sk, struct hci_dev *hdev, void *data, + u16 len) +{ + struct mgmt_cp_load_link_keys *cp = data; + u16 key_count, expected_len; + int i; + + key_count = get_unaligned_le16(&cp->key_count); + + expected_len = sizeof(*cp) + key_count * + sizeof(struct mgmt_link_key_info); + if (expected_len != len) { + BT_ERR("load_link_keys: expected %u bytes, got %u bytes", + len, expected_len); + return cmd_status(sk, hdev->id, MGMT_OP_LOAD_LINK_KEYS, + MGMT_STATUS_INVALID_PARAMS); + } + + BT_DBG("%s debug_keys %u key_count %u", hdev->name, cp->debug_keys, + key_count); + + hci_dev_lock(hdev); + + hci_link_keys_clear(hdev); + + set_bit(HCI_LINK_KEYS, &hdev->dev_flags); + + if (cp->debug_keys) + set_bit(HCI_DEBUG_KEYS, &hdev->dev_flags); + else + clear_bit(HCI_DEBUG_KEYS, &hdev->dev_flags); + + for (i = 0; i < key_count; i++) { + struct mgmt_link_key_info *key = &cp->keys[i]; + + hci_add_link_key(hdev, NULL, 0, &key->addr.bdaddr, key->val, + key->type, key->pin_len); + } + + cmd_complete(sk, hdev->id, MGMT_OP_LOAD_LINK_KEYS, 0, NULL, 0); + + hci_dev_unlock(hdev); + + return 0; +} + +static int device_unpaired(struct hci_dev *hdev, bdaddr_t *bdaddr, + u8 addr_type, struct sock *skip_sk) +{ + struct mgmt_ev_device_unpaired ev; + + bacpy(&ev.addr.bdaddr, bdaddr); + ev.addr.type = addr_type; + + return mgmt_event(MGMT_EV_DEVICE_UNPAIRED, hdev, &ev, sizeof(ev), + skip_sk); +} + +static int unpair_device(struct sock *sk, struct hci_dev *hdev, void *data, + u16 len) +{ + struct mgmt_cp_unpair_device *cp = data; + struct mgmt_rp_unpair_device rp; + struct hci_cp_disconnect dc; + struct pending_cmd *cmd; + struct hci_conn *conn; + int err; + + hci_dev_lock(hdev); + + memset(&rp, 0, sizeof(rp)); + bacpy(&rp.addr.bdaddr, &cp->addr.bdaddr); + rp.addr.type = cp->addr.type; + + if (!hdev_is_powered(hdev)) { + err = cmd_complete(sk, hdev->id, MGMT_OP_UNPAIR_DEVICE, + MGMT_STATUS_NOT_POWERED, &rp, sizeof(rp)); + goto unlock; + } + + if (cp->addr.type == MGMT_ADDR_BREDR) + err = hci_remove_link_key(hdev, &cp->addr.bdaddr); + else + err = hci_remove_ltk(hdev, &cp->addr.bdaddr); + + if (err < 0) { + err = cmd_complete(sk, hdev->id, MGMT_OP_UNPAIR_DEVICE, + MGMT_STATUS_NOT_PAIRED, &rp, sizeof(rp)); + goto unlock; + } + + if (cp->disconnect) { + if (cp->addr.type == MGMT_ADDR_BREDR) + conn = hci_conn_hash_lookup_ba(hdev, ACL_LINK, + &cp->addr.bdaddr); + else + conn = hci_conn_hash_lookup_ba(hdev, LE_LINK, + &cp->addr.bdaddr); + } else { + conn = NULL; + } + + if (!conn) { + err = cmd_complete(sk, hdev->id, MGMT_OP_UNPAIR_DEVICE, 0, + &rp, sizeof(rp)); + device_unpaired(hdev, &cp->addr.bdaddr, cp->addr.type, sk); + goto unlock; + } + + cmd = mgmt_pending_add(sk, MGMT_OP_UNPAIR_DEVICE, hdev, cp, + sizeof(*cp)); + if (!cmd) { + err = -ENOMEM; + goto unlock; + } + + put_unaligned_le16(conn->handle, &dc.handle); + dc.reason = 0x13; /* Remote User Terminated Connection */ + err = hci_send_cmd(hdev, HCI_OP_DISCONNECT, sizeof(dc), &dc); + if (err < 0) + mgmt_pending_remove(cmd); + +unlock: + hci_dev_unlock(hdev); + return err; +} + +static int disconnect(struct sock *sk, struct hci_dev *hdev, void *data, + u16 len) +{ + struct mgmt_cp_disconnect *cp = data; + struct hci_cp_disconnect dc; + struct pending_cmd *cmd; + struct hci_conn *conn; + int err; + + BT_DBG(""); + + hci_dev_lock(hdev); + + if (!test_bit(HCI_UP, &hdev->flags)) { + err = cmd_status(sk, hdev->id, MGMT_OP_DISCONNECT, + MGMT_STATUS_NOT_POWERED); + goto failed; + } + + if (mgmt_pending_find(MGMT_OP_DISCONNECT, hdev)) { + err = cmd_status(sk, hdev->id, MGMT_OP_DISCONNECT, + MGMT_STATUS_BUSY); + goto failed; + } + + if (cp->addr.type == MGMT_ADDR_BREDR) + conn = hci_conn_hash_lookup_ba(hdev, ACL_LINK, &cp->addr.bdaddr); + else + conn = hci_conn_hash_lookup_ba(hdev, LE_LINK, &cp->addr.bdaddr); + + if (!conn) { + err = cmd_status(sk, hdev->id, MGMT_OP_DISCONNECT, + MGMT_STATUS_NOT_CONNECTED); + goto failed; + } + + cmd = mgmt_pending_add(sk, MGMT_OP_DISCONNECT, hdev, data, len); + if (!cmd) { + err = -ENOMEM; + goto failed; + } + + put_unaligned_le16(conn->handle, &dc.handle); + dc.reason = 0x13; /* Remote User Terminated Connection */ + + err = hci_send_cmd(hdev, HCI_OP_DISCONNECT, sizeof(dc), &dc); + if (err < 0) + mgmt_pending_remove(cmd); + +failed: + hci_dev_unlock(hdev); + return err; +} + +static u8 link_to_mgmt(u8 link_type, u8 addr_type) +{ + switch (link_type) { + case LE_LINK: + switch (addr_type) { + case ADDR_LE_DEV_PUBLIC: + return MGMT_ADDR_LE_PUBLIC; + case ADDR_LE_DEV_RANDOM: + return MGMT_ADDR_LE_RANDOM; + default: + return MGMT_ADDR_INVALID; + } + case ACL_LINK: + return MGMT_ADDR_BREDR; + default: + return MGMT_ADDR_INVALID; + } +} + +static int get_connections(struct sock *sk, struct hci_dev *hdev, void *data, + u16 data_len) +{ + struct mgmt_rp_get_connections *rp; + struct hci_conn *c; + size_t rp_len; + int err; + u16 i; + + BT_DBG(""); + + hci_dev_lock(hdev); + + if (!hdev_is_powered(hdev)) { + err = cmd_status(sk, hdev->id, MGMT_OP_GET_CONNECTIONS, + MGMT_STATUS_NOT_POWERED); + goto unlock; + } + + i = 0; + list_for_each_entry(c, &hdev->conn_hash.list, list) { + if (test_bit(HCI_CONN_MGMT_CONNECTED, &c->flags)) + i++; + } + + rp_len = sizeof(*rp) + (i * sizeof(struct mgmt_addr_info)); + rp = kmalloc(rp_len, GFP_ATOMIC); + if (!rp) { + err = -ENOMEM; + goto unlock; + } + + i = 0; + list_for_each_entry(c, &hdev->conn_hash.list, list) { + if (!test_bit(HCI_CONN_MGMT_CONNECTED, &c->flags)) + continue; + bacpy(&rp->addr[i].bdaddr, &c->dst); + rp->addr[i].type = link_to_mgmt(c->type, c->dst_type); + if (rp->addr[i].type == MGMT_ADDR_INVALID) + continue; + i++; + } + + put_unaligned_le16(i, &rp->conn_count); + + /* Recalculate length in case of filtered SCO connections, etc */ + rp_len = sizeof(*rp) + (i * sizeof(struct mgmt_addr_info)); + + err = cmd_complete(sk, hdev->id, MGMT_OP_GET_CONNECTIONS, 0, rp, + rp_len); + + kfree(rp); + +unlock: + hci_dev_unlock(hdev); + return err; +} + +static int send_pin_code_neg_reply(struct sock *sk, struct hci_dev *hdev, + struct mgmt_cp_pin_code_neg_reply *cp) +{ + struct pending_cmd *cmd; + int err; + + cmd = mgmt_pending_add(sk, MGMT_OP_PIN_CODE_NEG_REPLY, hdev, cp, + sizeof(*cp)); + if (!cmd) + return -ENOMEM; + + err = hci_send_cmd(hdev, HCI_OP_PIN_CODE_NEG_REPLY, + sizeof(cp->addr.bdaddr), &cp->addr.bdaddr); + if (err < 0) + mgmt_pending_remove(cmd); + + return err; +} + +static int pin_code_reply(struct sock *sk, struct hci_dev *hdev, void *data, + u16 len) +{ + struct hci_conn *conn; + struct mgmt_cp_pin_code_reply *cp = data; + struct hci_cp_pin_code_reply reply; + struct pending_cmd *cmd; + int err; + + BT_DBG(""); + + hci_dev_lock(hdev); + + if (!hdev_is_powered(hdev)) { + err = cmd_status(sk, hdev->id, MGMT_OP_PIN_CODE_REPLY, + MGMT_STATUS_NOT_POWERED); + goto failed; + } + + conn = hci_conn_hash_lookup_ba(hdev, ACL_LINK, &cp->addr.bdaddr); + if (!conn) { + err = cmd_status(sk, hdev->id, MGMT_OP_PIN_CODE_REPLY, + MGMT_STATUS_NOT_CONNECTED); + goto failed; + } + + if (conn->pending_sec_level == BT_SECURITY_HIGH && cp->pin_len != 16) { + struct mgmt_cp_pin_code_neg_reply ncp; + + memcpy(&ncp.addr, &cp->addr, sizeof(ncp.addr)); + + BT_ERR("PIN code is not 16 bytes long"); + + err = send_pin_code_neg_reply(sk, hdev, &ncp); + if (err >= 0) + err = cmd_status(sk, hdev->id, MGMT_OP_PIN_CODE_REPLY, + MGMT_STATUS_INVALID_PARAMS); + + goto failed; + } + + cmd = mgmt_pending_add(sk, MGMT_OP_PIN_CODE_REPLY, hdev, data, len); + if (!cmd) { + err = -ENOMEM; + goto failed; + } + + bacpy(&reply.bdaddr, &cp->addr.bdaddr); + reply.pin_len = cp->pin_len; + memcpy(reply.pin_code, cp->pin_code, sizeof(reply.pin_code)); + + err = hci_send_cmd(hdev, HCI_OP_PIN_CODE_REPLY, sizeof(reply), &reply); + if (err < 0) + mgmt_pending_remove(cmd); + +failed: + hci_dev_unlock(hdev); + return err; +} + +static int pin_code_neg_reply(struct sock *sk, struct hci_dev *hdev, + void *data, u16 len) +{ + struct mgmt_cp_pin_code_neg_reply *cp = data; + int err; + + BT_DBG(""); + + hci_dev_lock(hdev); + + if (!hdev_is_powered(hdev)) { + err = cmd_status(sk, hdev->id, MGMT_OP_PIN_CODE_NEG_REPLY, + MGMT_STATUS_NOT_POWERED); + goto failed; + } + + err = send_pin_code_neg_reply(sk, hdev, cp); + +failed: + hci_dev_unlock(hdev); + return err; +} + +static int set_io_capability(struct sock *sk, struct hci_dev *hdev, void *data, + u16 len) +{ + struct mgmt_cp_set_io_capability *cp = data; + + BT_DBG(""); + + hci_dev_lock(hdev); + + hdev->io_capability = cp->io_capability; + + BT_DBG("%s IO capability set to 0x%02x", hdev->name, + hdev->io_capability); + + hci_dev_unlock(hdev); + + return cmd_complete(sk, hdev->id, MGMT_OP_SET_IO_CAPABILITY, 0, NULL, + 0); +} + +static inline struct pending_cmd *find_pairing(struct hci_conn *conn) +{ + struct hci_dev *hdev = conn->hdev; + struct pending_cmd *cmd; + + list_for_each_entry(cmd, &hdev->mgmt_pending, list) { + if (cmd->opcode != MGMT_OP_PAIR_DEVICE) + continue; + + if (cmd->user_data != conn) + continue; + + return cmd; + } + + return NULL; +} + +static void pairing_complete(struct pending_cmd *cmd, u8 status) +{ + struct mgmt_rp_pair_device rp; + struct hci_conn *conn = cmd->user_data; + + bacpy(&rp.addr.bdaddr, &conn->dst); + rp.addr.type = link_to_mgmt(conn->type, conn->dst_type); + + cmd_complete(cmd->sk, cmd->index, MGMT_OP_PAIR_DEVICE, status, + &rp, sizeof(rp)); + + /* So we don't get further callbacks for this connection */ + conn->connect_cfm_cb = NULL; + conn->security_cfm_cb = NULL; + conn->disconn_cfm_cb = NULL; + + hci_conn_put(conn); + + mgmt_pending_remove(cmd); +} + +static void pairing_complete_cb(struct hci_conn *conn, u8 status) +{ + struct pending_cmd *cmd; + + BT_DBG("status %u", status); + + cmd = find_pairing(conn); + if (!cmd) + BT_DBG("Unable to find a pending command"); + else + pairing_complete(cmd, mgmt_status(status)); +} + +static int pair_device(struct sock *sk, struct hci_dev *hdev, void *data, + u16 len) +{ + struct mgmt_cp_pair_device *cp = data; + struct mgmt_rp_pair_device rp; + struct pending_cmd *cmd; + u8 sec_level, auth_type; + struct hci_conn *conn; + int err; + + BT_DBG(""); + + hci_dev_lock(hdev); + + if (!hdev_is_powered(hdev)) { + err = cmd_status(sk, hdev->id, MGMT_OP_PAIR_DEVICE, + MGMT_STATUS_NOT_POWERED); + goto unlock; + } + + sec_level = BT_SECURITY_MEDIUM; + if (cp->io_cap == 0x03) + auth_type = HCI_AT_DEDICATED_BONDING; + else + auth_type = HCI_AT_DEDICATED_BONDING_MITM; + + if (cp->addr.type == MGMT_ADDR_BREDR) + conn = hci_connect(hdev, ACL_LINK, &cp->addr.bdaddr, sec_level, + auth_type); + else + conn = hci_connect(hdev, LE_LINK, &cp->addr.bdaddr, sec_level, + auth_type); + + memset(&rp, 0, sizeof(rp)); + bacpy(&rp.addr.bdaddr, &cp->addr.bdaddr); + rp.addr.type = cp->addr.type; + + if (IS_ERR(conn)) { + err = cmd_complete(sk, hdev->id, MGMT_OP_PAIR_DEVICE, + MGMT_STATUS_CONNECT_FAILED, &rp, + sizeof(rp)); + goto unlock; + } + + if (conn->connect_cfm_cb) { + hci_conn_put(conn); + err = cmd_complete(sk, hdev->id, MGMT_OP_PAIR_DEVICE, + MGMT_STATUS_BUSY, &rp, sizeof(rp)); + goto unlock; + } + + cmd = mgmt_pending_add(sk, MGMT_OP_PAIR_DEVICE, hdev, data, len); + if (!cmd) { + err = -ENOMEM; + hci_conn_put(conn); + goto unlock; + } + + /* For LE, just connecting isn't a proof that the pairing finished */ + if (cp->addr.type == MGMT_ADDR_BREDR) + conn->connect_cfm_cb = pairing_complete_cb; + + conn->security_cfm_cb = pairing_complete_cb; + conn->disconn_cfm_cb = pairing_complete_cb; + conn->io_capability = cp->io_cap; + cmd->user_data = conn; + + if (conn->state == BT_CONNECTED && + hci_conn_security(conn, sec_level, auth_type)) + pairing_complete(cmd, 0); + + err = 0; + +unlock: + hci_dev_unlock(hdev); + return err; +} + +static int cancel_pair_device(struct sock *sk, struct hci_dev *hdev, void *data, + u16 len) +{ + struct mgmt_addr_info *addr = data; + struct pending_cmd *cmd; + struct hci_conn *conn; + int err; + + BT_DBG(""); + + hci_dev_lock(hdev); + + if (!hdev_is_powered(hdev)) { + err = cmd_status(sk, hdev->id, MGMT_OP_CANCEL_PAIR_DEVICE, + MGMT_STATUS_NOT_POWERED); + goto unlock; + } + + cmd = mgmt_pending_find(MGMT_OP_PAIR_DEVICE, hdev); + if (!cmd) { + err = cmd_status(sk, hdev->id, MGMT_OP_CANCEL_PAIR_DEVICE, + MGMT_STATUS_INVALID_PARAMS); + goto unlock; + } + + conn = cmd->user_data; + + if (bacmp(&addr->bdaddr, &conn->dst) != 0) { + err = cmd_status(sk, hdev->id, MGMT_OP_CANCEL_PAIR_DEVICE, + MGMT_STATUS_INVALID_PARAMS); + goto unlock; + } + + pairing_complete(cmd, MGMT_STATUS_CANCELLED); + + err = cmd_complete(sk, hdev->id, MGMT_OP_CANCEL_PAIR_DEVICE, 0, + addr, sizeof(*addr)); +unlock: + hci_dev_unlock(hdev); + return err; +} + +static int user_pairing_resp(struct sock *sk, struct hci_dev *hdev, + bdaddr_t *bdaddr, u8 type, u16 mgmt_op, + u16 hci_op, __le32 passkey) +{ + struct pending_cmd *cmd; + struct hci_conn *conn; + int err; + + hci_dev_lock(hdev); + + if (!hdev_is_powered(hdev)) { + err = cmd_status(sk, hdev->id, mgmt_op, + MGMT_STATUS_NOT_POWERED); + goto done; + } + + if (type == MGMT_ADDR_BREDR) + conn = hci_conn_hash_lookup_ba(hdev, ACL_LINK, bdaddr); + else + conn = hci_conn_hash_lookup_ba(hdev, LE_LINK, bdaddr); + + if (!conn) { + err = cmd_status(sk, hdev->id, mgmt_op, + MGMT_STATUS_NOT_CONNECTED); + goto done; + } + + if (type == MGMT_ADDR_LE_PUBLIC || type == MGMT_ADDR_LE_RANDOM) { + /* Continue with pairing via SMP */ + err = smp_user_confirm_reply(conn, mgmt_op, passkey); + + if (!err) + err = cmd_status(sk, hdev->id, mgmt_op, + MGMT_STATUS_SUCCESS); + else + err = cmd_status(sk, hdev->id, mgmt_op, + MGMT_STATUS_FAILED); + + goto done; + } + + cmd = mgmt_pending_add(sk, mgmt_op, hdev, bdaddr, sizeof(*bdaddr)); + if (!cmd) { + err = -ENOMEM; + goto done; + } + + /* Continue with pairing via HCI */ + if (hci_op == HCI_OP_USER_PASSKEY_REPLY) { + struct hci_cp_user_passkey_reply cp; + + bacpy(&cp.bdaddr, bdaddr); + cp.passkey = passkey; + err = hci_send_cmd(hdev, hci_op, sizeof(cp), &cp); + } else + err = hci_send_cmd(hdev, hci_op, sizeof(*bdaddr), bdaddr); + + if (err < 0) + mgmt_pending_remove(cmd); + +done: + hci_dev_unlock(hdev); + return err; +} + +static int user_confirm_reply(struct sock *sk, struct hci_dev *hdev, void *data, + u16 len) +{ + struct mgmt_cp_user_confirm_reply *cp = data; + + BT_DBG(""); + + if (len != sizeof(*cp)) + return cmd_status(sk, hdev->id, MGMT_OP_USER_CONFIRM_REPLY, + MGMT_STATUS_INVALID_PARAMS); + + return user_pairing_resp(sk, hdev, &cp->addr.bdaddr, cp->addr.type, + MGMT_OP_USER_CONFIRM_REPLY, + HCI_OP_USER_CONFIRM_REPLY, 0); +} + +static int user_confirm_neg_reply(struct sock *sk, struct hci_dev *hdev, + void *data, u16 len) +{ + struct mgmt_cp_user_confirm_neg_reply *cp = data; + + BT_DBG(""); + + return user_pairing_resp(sk, hdev, &cp->addr.bdaddr, cp->addr.type, + MGMT_OP_USER_CONFIRM_NEG_REPLY, + HCI_OP_USER_CONFIRM_NEG_REPLY, 0); +} + +static int user_passkey_reply(struct sock *sk, struct hci_dev *hdev, void *data, + u16 len) +{ + struct mgmt_cp_user_passkey_reply *cp = data; + + BT_DBG(""); + + return user_pairing_resp(sk, hdev, &cp->addr.bdaddr, cp->addr.type, + MGMT_OP_USER_PASSKEY_REPLY, + HCI_OP_USER_PASSKEY_REPLY, cp->passkey); +} + +static int user_passkey_neg_reply(struct sock *sk, struct hci_dev *hdev, + void *data, u16 len) +{ + struct mgmt_cp_user_passkey_neg_reply *cp = data; + + BT_DBG(""); + + return user_pairing_resp(sk, hdev, &cp->addr.bdaddr, cp->addr.type, + MGMT_OP_USER_PASSKEY_NEG_REPLY, + HCI_OP_USER_PASSKEY_NEG_REPLY, 0); +} + +static int update_name(struct hci_dev *hdev, const char *name) +{ + struct hci_cp_write_local_name cp; + + memcpy(cp.name, name, sizeof(cp.name)); + + return hci_send_cmd(hdev, HCI_OP_WRITE_LOCAL_NAME, sizeof(cp), &cp); +} + +static int set_local_name(struct sock *sk, struct hci_dev *hdev, void *data, + u16 len) +{ + struct mgmt_cp_set_local_name *cp = data; + struct pending_cmd *cmd; + int err; + + BT_DBG(""); + + hci_dev_lock(hdev); + + memcpy(hdev->short_name, cp->short_name, sizeof(hdev->short_name)); + + if (!hdev_is_powered(hdev)) { + memcpy(hdev->dev_name, cp->name, sizeof(hdev->dev_name)); + + err = cmd_complete(sk, hdev->id, MGMT_OP_SET_LOCAL_NAME, 0, + data, len); + if (err < 0) + goto failed; + + err = mgmt_event(MGMT_EV_LOCAL_NAME_CHANGED, hdev, data, len, + sk); + + goto failed; + } + + cmd = mgmt_pending_add(sk, MGMT_OP_SET_LOCAL_NAME, hdev, data, len); + if (!cmd) { + err = -ENOMEM; + goto failed; + } + + err = update_name(hdev, cp->name); + if (err < 0) + mgmt_pending_remove(cmd); + +failed: + hci_dev_unlock(hdev); + return err; +} + +static int read_local_oob_data(struct sock *sk, struct hci_dev *hdev, + void *data, u16 data_len) +{ + struct pending_cmd *cmd; + int err; + + BT_DBG("%s", hdev->name); + + hci_dev_lock(hdev); + + if (!hdev_is_powered(hdev)) { + err = cmd_status(sk, hdev->id, MGMT_OP_READ_LOCAL_OOB_DATA, + MGMT_STATUS_NOT_POWERED); + goto unlock; + } + + if (!(hdev->features[6] & LMP_SIMPLE_PAIR)) { + err = cmd_status(sk, hdev->id, MGMT_OP_READ_LOCAL_OOB_DATA, + MGMT_STATUS_NOT_SUPPORTED); + goto unlock; + } + + if (mgmt_pending_find(MGMT_OP_READ_LOCAL_OOB_DATA, hdev)) { + err = cmd_status(sk, hdev->id, MGMT_OP_READ_LOCAL_OOB_DATA, + MGMT_STATUS_BUSY); + goto unlock; + } + + cmd = mgmt_pending_add(sk, MGMT_OP_READ_LOCAL_OOB_DATA, hdev, NULL, 0); + if (!cmd) { + err = -ENOMEM; + goto unlock; + } + + err = hci_send_cmd(hdev, HCI_OP_READ_LOCAL_OOB_DATA, 0, NULL); + if (err < 0) + mgmt_pending_remove(cmd); + +unlock: + hci_dev_unlock(hdev); + return err; +} + +static int add_remote_oob_data(struct sock *sk, struct hci_dev *hdev, + void *data, u16 len) +{ + struct mgmt_cp_add_remote_oob_data *cp = data; + u8 status; + int err; + + BT_DBG("%s ", hdev->name); + + hci_dev_lock(hdev); + + if (!hdev_is_powered(hdev)) { + err = cmd_complete(sk, hdev->id, MGMT_OP_ADD_REMOTE_OOB_DATA, + MGMT_STATUS_NOT_POWERED, &cp->addr, + sizeof(cp->addr)); + goto unlock; + } + + err = hci_add_remote_oob_data(hdev, &cp->addr.bdaddr, cp->hash, + cp->randomizer); + if (err < 0) + status = MGMT_STATUS_FAILED; + else + status = 0; + + err = cmd_complete(sk, hdev->id, MGMT_OP_ADD_REMOTE_OOB_DATA, status, + &cp->addr, sizeof(cp->addr)); + +unlock: + hci_dev_unlock(hdev); + return err; +} + +static int remove_remote_oob_data(struct sock *sk, struct hci_dev *hdev, + void *data, u16 len) +{ + struct mgmt_cp_remove_remote_oob_data *cp = data; + u8 status; + int err; + + BT_DBG("%s", hdev->name); + + hci_dev_lock(hdev); + + if (!hdev_is_powered(hdev)) { + err = cmd_complete(sk, hdev->id, + MGMT_OP_REMOVE_REMOTE_OOB_DATA, + MGMT_STATUS_NOT_POWERED, &cp->addr, + sizeof(cp->addr)); + goto unlock; + } + + err = hci_remove_remote_oob_data(hdev, &cp->addr.bdaddr); + if (err < 0) + status = MGMT_STATUS_INVALID_PARAMS; + else + status = 0; + + err = cmd_complete(sk, hdev->id, MGMT_OP_REMOVE_REMOTE_OOB_DATA, + status, &cp->addr, sizeof(cp->addr)); + +unlock: + hci_dev_unlock(hdev); + return err; +} + +int mgmt_interleaved_discovery(struct hci_dev *hdev) +{ + int err; + + BT_DBG("%s", hdev->name); + + hci_dev_lock(hdev); + + err = hci_do_inquiry(hdev, INQUIRY_LEN_BREDR_LE); + if (err < 0) + hci_discovery_set_state(hdev, DISCOVERY_STOPPED); + + hci_dev_unlock(hdev); + + return err; +} + +static int start_discovery(struct sock *sk, struct hci_dev *hdev, + void *data, u16 len) +{ + struct mgmt_cp_start_discovery *cp = data; + struct pending_cmd *cmd; + int err; + + BT_DBG("%s", hdev->name); + + hci_dev_lock(hdev); + + if (!hdev_is_powered(hdev)) { + err = cmd_status(sk, hdev->id, MGMT_OP_START_DISCOVERY, + MGMT_STATUS_NOT_POWERED); + goto failed; + } + + if (hdev->discovery.state != DISCOVERY_STOPPED) { + err = cmd_status(sk, hdev->id, MGMT_OP_START_DISCOVERY, + MGMT_STATUS_BUSY); + goto failed; + } + + cmd = mgmt_pending_add(sk, MGMT_OP_START_DISCOVERY, hdev, NULL, 0); + if (!cmd) { + err = -ENOMEM; + goto failed; + } + + hdev->discovery.type = cp->type; + + switch (hdev->discovery.type) { + case DISCOV_TYPE_BREDR: + if (lmp_bredr_capable(hdev)) + err = hci_do_inquiry(hdev, INQUIRY_LEN_BREDR); + else + err = -ENOTSUPP; + break; + + case DISCOV_TYPE_LE: + if (lmp_host_le_capable(hdev)) + err = hci_le_scan(hdev, LE_SCAN_TYPE, LE_SCAN_INT, + LE_SCAN_WIN, LE_SCAN_TIMEOUT_LE_ONLY); + else + err = -ENOTSUPP; + break; + + case DISCOV_TYPE_INTERLEAVED: + if (lmp_host_le_capable(hdev) && lmp_bredr_capable(hdev)) + err = hci_le_scan(hdev, LE_SCAN_TYPE, LE_SCAN_INT, + LE_SCAN_WIN, + LE_SCAN_TIMEOUT_BREDR_LE); + else + err = -ENOTSUPP; + break; + + default: + err = -EINVAL; + } + + if (err < 0) + mgmt_pending_remove(cmd); + else + hci_discovery_set_state(hdev, DISCOVERY_STARTING); + +failed: + hci_dev_unlock(hdev); + return err; +} + +static int stop_discovery(struct sock *sk, struct hci_dev *hdev, void *data, + u16 len) +{ + struct mgmt_cp_stop_discovery *mgmt_cp = data; + struct pending_cmd *cmd; + struct hci_cp_remote_name_req_cancel cp; + struct inquiry_entry *e; + int err; + + BT_DBG("%s", hdev->name); + + hci_dev_lock(hdev); + + if (!hci_discovery_active(hdev)) { + err = cmd_complete(sk, hdev->id, MGMT_OP_STOP_DISCOVERY, + MGMT_STATUS_REJECTED, &mgmt_cp->type, + sizeof(mgmt_cp->type)); + goto unlock; + } + + if (hdev->discovery.type != mgmt_cp->type) { + err = cmd_complete(sk, hdev->id, MGMT_OP_STOP_DISCOVERY, + MGMT_STATUS_INVALID_PARAMS, &mgmt_cp->type, + sizeof(mgmt_cp->type)); + goto unlock; + } + + cmd = mgmt_pending_add(sk, MGMT_OP_STOP_DISCOVERY, hdev, NULL, 0); + if (!cmd) { + err = -ENOMEM; + goto unlock; + } + + if (hdev->discovery.state == DISCOVERY_FINDING) { + err = hci_cancel_inquiry(hdev); + if (err < 0) + mgmt_pending_remove(cmd); + else + hci_discovery_set_state(hdev, DISCOVERY_STOPPING); + goto unlock; + } + + e = hci_inquiry_cache_lookup_resolve(hdev, BDADDR_ANY, NAME_PENDING); + if (!e) { + mgmt_pending_remove(cmd); + err = cmd_complete(sk, hdev->id, MGMT_OP_STOP_DISCOVERY, 0, + &mgmt_cp->type, sizeof(mgmt_cp->type)); + hci_discovery_set_state(hdev, DISCOVERY_STOPPED); + goto unlock; + } + + bacpy(&cp.bdaddr, &e->data.bdaddr); + err = hci_send_cmd(hdev, HCI_OP_REMOTE_NAME_REQ_CANCEL, sizeof(cp), + &cp); + if (err < 0) + mgmt_pending_remove(cmd); + else + hci_discovery_set_state(hdev, DISCOVERY_STOPPING); + +unlock: + hci_dev_unlock(hdev); + return err; +} + +static int confirm_name(struct sock *sk, struct hci_dev *hdev, void *data, + u16 len) +{ + struct mgmt_cp_confirm_name *cp = data; + struct inquiry_entry *e; + int err; + + BT_DBG("%s", hdev->name); + + hci_dev_lock(hdev); + + if (!hci_discovery_active(hdev)) { + err = cmd_status(sk, hdev->id, MGMT_OP_CONFIRM_NAME, + MGMT_STATUS_FAILED); + goto failed; + } + + e = hci_inquiry_cache_lookup_unknown(hdev, &cp->addr.bdaddr); + if (!e) { + err = cmd_status(sk, hdev->id, MGMT_OP_CONFIRM_NAME, + MGMT_STATUS_INVALID_PARAMS); + goto failed; + } + + if (cp->name_known) { + e->name_state = NAME_KNOWN; + list_del(&e->list); + } else { + e->name_state = NAME_NEEDED; + hci_inquiry_cache_update_resolve(hdev, e); + } + + err = 0; + +failed: + hci_dev_unlock(hdev); + return err; +} + +static int block_device(struct sock *sk, struct hci_dev *hdev, void *data, + u16 len) +{ + struct mgmt_cp_block_device *cp = data; + u8 status; + int err; + + BT_DBG("%s", hdev->name); + + hci_dev_lock(hdev); + + err = hci_blacklist_add(hdev, &cp->addr.bdaddr, cp->addr.type); + if (err < 0) + status = MGMT_STATUS_FAILED; + else + status = 0; + + err = cmd_complete(sk, hdev->id, MGMT_OP_BLOCK_DEVICE, status, + &cp->addr, sizeof(cp->addr)); + + hci_dev_unlock(hdev); + + return err; +} + +static int unblock_device(struct sock *sk, struct hci_dev *hdev, void *data, + u16 len) +{ + struct mgmt_cp_unblock_device *cp = data; + u8 status; + int err; + + BT_DBG("%s", hdev->name); + + hci_dev_lock(hdev); + + err = hci_blacklist_del(hdev, &cp->addr.bdaddr, cp->addr.type); + if (err < 0) + status = MGMT_STATUS_INVALID_PARAMS; + else + status = 0; + + err = cmd_complete(sk, hdev->id, MGMT_OP_UNBLOCK_DEVICE, status, + &cp->addr, sizeof(cp->addr)); + + hci_dev_unlock(hdev); + + return err; +} + +static int set_fast_connectable(struct sock *sk, struct hci_dev *hdev, + void *data, u16 len) +{ + struct mgmt_mode *cp = data; + struct hci_cp_write_page_scan_activity acp; + u8 type; + int err; + + BT_DBG("%s", hdev->name); + + if (!hdev_is_powered(hdev)) + return cmd_status(sk, hdev->id, MGMT_OP_SET_FAST_CONNECTABLE, + MGMT_STATUS_NOT_POWERED); + + if (!test_bit(HCI_CONNECTABLE, &hdev->dev_flags)) + return cmd_status(sk, hdev->id, MGMT_OP_SET_FAST_CONNECTABLE, + MGMT_STATUS_REJECTED); + + hci_dev_lock(hdev); + + if (cp->val) { + type = PAGE_SCAN_TYPE_INTERLACED; + + /* 22.5 msec page scan interval */ + acp.interval = __constant_cpu_to_le16(0x0024); + } else { + type = PAGE_SCAN_TYPE_STANDARD; /* default */ + + /* default 1.28 sec page scan */ + acp.interval = __constant_cpu_to_le16(0x0800); + } + + /* default 11.25 msec page scan window */ + acp.window = __constant_cpu_to_le16(0x0012); + + err = hci_send_cmd(hdev, HCI_OP_WRITE_PAGE_SCAN_ACTIVITY, sizeof(acp), + &acp); + if (err < 0) { + err = cmd_status(sk, hdev->id, MGMT_OP_SET_FAST_CONNECTABLE, + MGMT_STATUS_FAILED); + goto done; + } + + err = hci_send_cmd(hdev, HCI_OP_WRITE_PAGE_SCAN_TYPE, 1, &type); + if (err < 0) { + err = cmd_status(sk, hdev->id, MGMT_OP_SET_FAST_CONNECTABLE, + MGMT_STATUS_FAILED); + goto done; + } + + err = cmd_complete(sk, hdev->id, MGMT_OP_SET_FAST_CONNECTABLE, 0, + NULL, 0); +done: + hci_dev_unlock(hdev); + return err; +} + +static int load_long_term_keys(struct sock *sk, struct hci_dev *hdev, + void *cp_data, u16 len) +{ + struct mgmt_cp_load_long_term_keys *cp = cp_data; + u16 key_count, expected_len; + int i; + + key_count = get_unaligned_le16(&cp->key_count); + + expected_len = sizeof(*cp) + key_count * + sizeof(struct mgmt_ltk_info); + if (expected_len != len) { + BT_ERR("load_keys: expected %u bytes, got %u bytes", + len, expected_len); + return cmd_status(sk, hdev->id, MGMT_OP_LOAD_LONG_TERM_KEYS, + EINVAL); + } + + BT_DBG("%s key_count %u", hdev->name, key_count); + + hci_dev_lock(hdev); + + hci_smp_ltks_clear(hdev); + + for (i = 0; i < key_count; i++) { + struct mgmt_ltk_info *key = &cp->keys[i]; + u8 type; + + if (key->master) + type = HCI_SMP_LTK; + else + type = HCI_SMP_LTK_SLAVE; + + hci_add_ltk(hdev, &key->addr.bdaddr, key->addr.type, + type, 0, key->authenticated, key->val, + key->enc_size, key->ediv, key->rand); + } + + hci_dev_unlock(hdev); + + return 0; +} + +struct mgmt_handler { + int (*func) (struct sock *sk, struct hci_dev *hdev, void *data, + u16 data_len); + bool var_len; + size_t data_len; +} mgmt_handlers[] = { + { NULL }, /* 0x0000 (no command) */ + { read_version, false, MGMT_READ_VERSION_SIZE }, + { read_commands, false, MGMT_READ_COMMANDS_SIZE }, + { read_index_list, false, MGMT_READ_INDEX_LIST_SIZE }, + { read_controller_info, false, MGMT_READ_INFO_SIZE }, + { set_powered, false, MGMT_SETTING_SIZE }, + { set_discoverable, false, MGMT_SET_DISCOVERABLE_SIZE }, + { set_connectable, false, MGMT_SETTING_SIZE }, + { set_fast_connectable, false, MGMT_SETTING_SIZE }, + { set_pairable, false, MGMT_SETTING_SIZE }, + { set_link_security, false, MGMT_SETTING_SIZE }, + { set_ssp, false, MGMT_SETTING_SIZE }, + { set_hs, false, MGMT_SETTING_SIZE }, + { set_le, false, MGMT_SETTING_SIZE }, + { set_dev_class, false, MGMT_SET_DEV_CLASS_SIZE }, + { set_local_name, false, MGMT_SET_LOCAL_NAME_SIZE }, + { add_uuid, false, MGMT_ADD_UUID_SIZE }, + { remove_uuid, false, MGMT_REMOVE_UUID_SIZE }, + { load_link_keys, true, MGMT_LOAD_LINK_KEYS_SIZE }, + { load_long_term_keys, true, MGMT_LOAD_LONG_TERM_KEYS_SIZE }, + { disconnect, false, MGMT_DISCONNECT_SIZE }, + { get_connections, false, MGMT_GET_CONNECTIONS_SIZE }, + { pin_code_reply, false, MGMT_PIN_CODE_REPLY_SIZE }, + { pin_code_neg_reply, false, MGMT_PIN_CODE_NEG_REPLY_SIZE }, + { set_io_capability, false, MGMT_SET_IO_CAPABILITY_SIZE }, + { pair_device, false, MGMT_PAIR_DEVICE_SIZE }, + { cancel_pair_device, false, MGMT_CANCEL_PAIR_DEVICE_SIZE }, + { unpair_device, false, MGMT_UNPAIR_DEVICE_SIZE }, + { user_confirm_reply, false, MGMT_USER_CONFIRM_REPLY_SIZE }, + { user_confirm_neg_reply, false, MGMT_USER_CONFIRM_NEG_REPLY_SIZE }, + { user_passkey_reply, false, MGMT_USER_PASSKEY_REPLY_SIZE }, + { user_passkey_neg_reply, false, MGMT_USER_PASSKEY_NEG_REPLY_SIZE }, + { read_local_oob_data, false, MGMT_READ_LOCAL_OOB_DATA_SIZE }, + { add_remote_oob_data, false, MGMT_ADD_REMOTE_OOB_DATA_SIZE }, + { remove_remote_oob_data, false, MGMT_REMOVE_REMOTE_OOB_DATA_SIZE }, + { start_discovery, false, MGMT_START_DISCOVERY_SIZE }, + { stop_discovery, false, MGMT_STOP_DISCOVERY_SIZE }, + { confirm_name, false, MGMT_CONFIRM_NAME_SIZE }, + { block_device, false, MGMT_BLOCK_DEVICE_SIZE }, + { unblock_device, false, MGMT_UNBLOCK_DEVICE_SIZE }, +}; + + +int mgmt_control(struct sock *sk, struct msghdr *msg, size_t msglen) +{ + void *buf; + u8 *cp; + struct mgmt_hdr *hdr; + u16 opcode, index, len; + struct hci_dev *hdev = NULL; + struct mgmt_handler *handler; + int err; + + BT_DBG("got %zu bytes", msglen); + + if (msglen < sizeof(*hdr)) + return -EINVAL; + + buf = kmalloc(msglen, GFP_KERNEL); + if (!buf) + return -ENOMEM; + + if (memcpy_fromiovec(buf, msg->msg_iov, msglen)) { + err = -EFAULT; + goto done; + } + + hdr = buf; + opcode = get_unaligned_le16(&hdr->opcode); + index = get_unaligned_le16(&hdr->index); + len = get_unaligned_le16(&hdr->len); + + if (len != msglen - sizeof(*hdr)) { + err = -EINVAL; + goto done; + } + + if (index != MGMT_INDEX_NONE) { + hdev = hci_dev_get(index); + if (!hdev) { + err = cmd_status(sk, index, opcode, + MGMT_STATUS_INVALID_INDEX); + goto done; + } + } + + if (opcode >= ARRAY_SIZE(mgmt_handlers) || + mgmt_handlers[opcode].func == NULL) { + BT_DBG("Unknown op %u", opcode); + err = cmd_status(sk, index, opcode, + MGMT_STATUS_UNKNOWN_COMMAND); + goto done; + } + + if ((hdev && opcode < MGMT_OP_READ_INFO) || + (!hdev && opcode >= MGMT_OP_READ_INFO)) { + err = cmd_status(sk, index, opcode, + MGMT_STATUS_INVALID_INDEX); + goto done; + } + + handler = &mgmt_handlers[opcode]; + + if ((handler->var_len && len < handler->data_len) || + (!handler->var_len && len != handler->data_len)) { + err = cmd_status(sk, index, opcode, + MGMT_STATUS_INVALID_PARAMS); + goto done; + } + + if (hdev) + mgmt_init_hdev(sk, hdev); + + cp = buf + sizeof(*hdr); + + err = handler->func(sk, hdev, cp, len); + if (err < 0) + goto done; + + err = msglen; + +done: + if (hdev) + hci_dev_put(hdev); + + kfree(buf); + return err; +} + +static void cmd_status_rsp(struct pending_cmd *cmd, void *data) +{ + u8 *status = data; + + cmd_status(cmd->sk, cmd->index, cmd->opcode, *status); + mgmt_pending_remove(cmd); +} + +int mgmt_index_added(struct hci_dev *hdev) +{ + return mgmt_event(MGMT_EV_INDEX_ADDED, hdev, NULL, 0, NULL); +} + +int mgmt_index_removed(struct hci_dev *hdev) +{ + u8 status = MGMT_STATUS_INVALID_INDEX; + + mgmt_pending_foreach(0, hdev, cmd_status_rsp, &status); + + return mgmt_event(MGMT_EV_INDEX_REMOVED, hdev, NULL, 0, NULL); +} + +struct cmd_lookup { + struct sock *sk; + struct hci_dev *hdev; + u8 mgmt_status; +}; + +static void settings_rsp(struct pending_cmd *cmd, void *data) +{ + struct cmd_lookup *match = data; + + send_settings_rsp(cmd->sk, cmd->opcode, match->hdev); + + list_del(&cmd->list); + + if (match->sk == NULL) { + match->sk = cmd->sk; + sock_hold(match->sk); + } + + mgmt_pending_free(cmd); +} + +int mgmt_powered(struct hci_dev *hdev, u8 powered) +{ + struct cmd_lookup match = { NULL, hdev }; + int err; + + if (!test_bit(HCI_MGMT, &hdev->dev_flags)) + return 0; + + mgmt_pending_foreach(MGMT_OP_SET_POWERED, hdev, settings_rsp, &match); + + if (powered) { + u8 scan = 0; + + if (test_bit(HCI_CONNECTABLE, &hdev->dev_flags)) + scan |= SCAN_PAGE; + if (test_bit(HCI_DISCOVERABLE, &hdev->dev_flags)) + scan |= SCAN_INQUIRY; + + if (scan) + hci_send_cmd(hdev, HCI_OP_WRITE_SCAN_ENABLE, 1, &scan); + + update_class(hdev); + update_name(hdev, hdev->dev_name); + update_eir(hdev); + } else { + u8 status = MGMT_STATUS_NOT_POWERED; + mgmt_pending_foreach(0, hdev, cmd_status_rsp, &status); + } + + err = new_settings(hdev, match.sk); + + if (match.sk) + sock_put(match.sk); + + return err; +} + +int mgmt_discoverable(struct hci_dev *hdev, u8 discoverable) +{ + struct cmd_lookup match = { NULL, hdev }; + bool changed = false; + int err = 0; + + if (discoverable) { + if (!test_and_set_bit(HCI_DISCOVERABLE, &hdev->dev_flags)) + changed = true; + } else { + if (test_and_clear_bit(HCI_DISCOVERABLE, &hdev->dev_flags)) + changed = true; + } + + mgmt_pending_foreach(MGMT_OP_SET_DISCOVERABLE, hdev, settings_rsp, + &match); + + if (changed) + err = new_settings(hdev, match.sk); + + if (match.sk) + sock_put(match.sk); + + return err; +} + +int mgmt_connectable(struct hci_dev *hdev, u8 connectable) +{ + struct cmd_lookup match = { NULL, hdev }; + bool changed = false; + int err = 0; + + if (connectable) { + if (!test_and_set_bit(HCI_CONNECTABLE, &hdev->dev_flags)) + changed = true; + } else { + if (test_and_clear_bit(HCI_CONNECTABLE, &hdev->dev_flags)) + changed = true; + } + + mgmt_pending_foreach(MGMT_OP_SET_CONNECTABLE, hdev, settings_rsp, + &match); + + if (changed) + err = new_settings(hdev, match.sk); + + if (match.sk) + sock_put(match.sk); + + return err; +} + +int mgmt_write_scan_failed(struct hci_dev *hdev, u8 scan, u8 status) +{ + u8 mgmt_err = mgmt_status(status); + + if (scan & SCAN_PAGE) + mgmt_pending_foreach(MGMT_OP_SET_CONNECTABLE, hdev, + cmd_status_rsp, &mgmt_err); + + if (scan & SCAN_INQUIRY) + mgmt_pending_foreach(MGMT_OP_SET_DISCOVERABLE, hdev, + cmd_status_rsp, &mgmt_err); + + return 0; +} + +int mgmt_new_link_key(struct hci_dev *hdev, struct link_key *key, bool persistent) +{ + struct mgmt_ev_new_link_key ev; + + memset(&ev, 0, sizeof(ev)); + + ev.store_hint = persistent; + bacpy(&ev.key.addr.bdaddr, &key->bdaddr); + ev.key.addr.type = MGMT_ADDR_BREDR; + ev.key.type = key->type; + memcpy(ev.key.val, key->val, 16); + ev.key.pin_len = key->pin_len; + + return mgmt_event(MGMT_EV_NEW_LINK_KEY, hdev, &ev, sizeof(ev), NULL); +} + +int mgmt_new_ltk(struct hci_dev *hdev, struct smp_ltk *key, u8 persistent) +{ + struct mgmt_ev_new_long_term_key ev; + + memset(&ev, 0, sizeof(ev)); + + ev.store_hint = persistent; + bacpy(&ev.key.addr.bdaddr, &key->bdaddr); + ev.key.addr.type = key->bdaddr_type; + ev.key.authenticated = key->authenticated; + ev.key.enc_size = key->enc_size; + ev.key.ediv = key->ediv; + + if (key->type == HCI_SMP_LTK) + ev.key.master = 1; + + memcpy(ev.key.rand, key->rand, sizeof(key->rand)); + memcpy(ev.key.val, key->val, sizeof(key->val)); + + return mgmt_event(MGMT_EV_NEW_LONG_TERM_KEY, hdev, &ev, sizeof(ev), + NULL); +} + +int mgmt_device_connected(struct hci_dev *hdev, bdaddr_t *bdaddr, u8 link_type, + u8 addr_type, u32 flags, u8 *name, u8 name_len, + u8 *dev_class) +{ + char buf[512]; + struct mgmt_ev_device_connected *ev = (void *) buf; + u16 eir_len = 0; + + bacpy(&ev->addr.bdaddr, bdaddr); + ev->addr.type = link_to_mgmt(link_type, addr_type); + + ev->flags = __cpu_to_le32(flags); + + if (name_len > 0) + eir_len = eir_append_data(ev->eir, 0, EIR_NAME_COMPLETE, + name, name_len); + + if (dev_class && memcmp(dev_class, "\0\0\0", 3) != 0) + eir_len = eir_append_data(ev->eir, eir_len, + EIR_CLASS_OF_DEV, dev_class, 3); + + put_unaligned_le16(eir_len, &ev->eir_len); + + return mgmt_event(MGMT_EV_DEVICE_CONNECTED, hdev, buf, + sizeof(*ev) + eir_len, NULL); +} + +static void disconnect_rsp(struct pending_cmd *cmd, void *data) +{ + struct mgmt_cp_disconnect *cp = cmd->param; + struct sock **sk = data; + struct mgmt_rp_disconnect rp; + + bacpy(&rp.addr.bdaddr, &cp->addr.bdaddr); + rp.addr.type = cp->addr.type; + + cmd_complete(cmd->sk, cmd->index, MGMT_OP_DISCONNECT, 0, &rp, + sizeof(rp)); + + *sk = cmd->sk; + sock_hold(*sk); + + mgmt_pending_remove(cmd); +} + +static void unpair_device_rsp(struct pending_cmd *cmd, void *data) +{ + struct hci_dev *hdev = data; + struct mgmt_cp_unpair_device *cp = cmd->param; + struct mgmt_rp_unpair_device rp; + + memset(&rp, 0, sizeof(rp)); + bacpy(&rp.addr.bdaddr, &cp->addr.bdaddr); + rp.addr.type = cp->addr.type; + + device_unpaired(hdev, &cp->addr.bdaddr, cp->addr.type, cmd->sk); + + cmd_complete(cmd->sk, cmd->index, cmd->opcode, 0, &rp, sizeof(rp)); + + mgmt_pending_remove(cmd); +} + +int mgmt_device_disconnected(struct hci_dev *hdev, bdaddr_t *bdaddr, + u8 link_type, u8 addr_type) +{ + struct mgmt_addr_info ev; + struct sock *sk = NULL; + int err; + + mgmt_pending_foreach(MGMT_OP_DISCONNECT, hdev, disconnect_rsp, &sk); + + bacpy(&ev.bdaddr, bdaddr); + ev.type = link_to_mgmt(link_type, addr_type); + + err = mgmt_event(MGMT_EV_DEVICE_DISCONNECTED, hdev, &ev, sizeof(ev), + sk); + + if (sk) + sock_put(sk); + + mgmt_pending_foreach(MGMT_OP_UNPAIR_DEVICE, hdev, unpair_device_rsp, + hdev); + + return err; +} + +int mgmt_disconnect_failed(struct hci_dev *hdev, bdaddr_t *bdaddr, + u8 link_type, u8 addr_type, u8 status) +{ + struct mgmt_rp_disconnect rp; + struct pending_cmd *cmd; + int err; + + cmd = mgmt_pending_find(MGMT_OP_DISCONNECT, hdev); + if (!cmd) + return -ENOENT; + + bacpy(&rp.addr.bdaddr, bdaddr); + rp.addr.type = link_to_mgmt(link_type, addr_type); + + err = cmd_complete(cmd->sk, cmd->index, MGMT_OP_DISCONNECT, + mgmt_status(status), &rp, sizeof(rp)); + + mgmt_pending_remove(cmd); + + mgmt_pending_foreach(MGMT_OP_UNPAIR_DEVICE, hdev, unpair_device_rsp, + hdev); + return err; +} + +int mgmt_connect_failed(struct hci_dev *hdev, bdaddr_t *bdaddr, u8 link_type, + u8 addr_type, u8 status) +{ + struct mgmt_ev_connect_failed ev; + + bacpy(&ev.addr.bdaddr, bdaddr); + ev.addr.type = link_to_mgmt(link_type, addr_type); + ev.status = mgmt_status(status); + + return mgmt_event(MGMT_EV_CONNECT_FAILED, hdev, &ev, sizeof(ev), NULL); +} + +int mgmt_pin_code_request(struct hci_dev *hdev, bdaddr_t *bdaddr, u8 secure) +{ + struct mgmt_ev_pin_code_request ev; + + bacpy(&ev.addr.bdaddr, bdaddr); + ev.addr.type = MGMT_ADDR_BREDR; + ev.secure = secure; + + return mgmt_event(MGMT_EV_PIN_CODE_REQUEST, hdev, &ev, sizeof(ev), + NULL); +} + +int mgmt_pin_code_reply_complete(struct hci_dev *hdev, bdaddr_t *bdaddr, + u8 status) +{ + struct pending_cmd *cmd; + struct mgmt_rp_pin_code_reply rp; + int err; + + cmd = mgmt_pending_find(MGMT_OP_PIN_CODE_REPLY, hdev); + if (!cmd) + return -ENOENT; + + bacpy(&rp.addr.bdaddr, bdaddr); + rp.addr.type = MGMT_ADDR_BREDR; + + err = cmd_complete(cmd->sk, hdev->id, MGMT_OP_PIN_CODE_REPLY, + mgmt_status(status), &rp, sizeof(rp)); + + mgmt_pending_remove(cmd); + + return err; +} + +int mgmt_pin_code_neg_reply_complete(struct hci_dev *hdev, bdaddr_t *bdaddr, + u8 status) +{ + struct pending_cmd *cmd; + struct mgmt_rp_pin_code_reply rp; + int err; + + cmd = mgmt_pending_find(MGMT_OP_PIN_CODE_NEG_REPLY, hdev); + if (!cmd) + return -ENOENT; + + bacpy(&rp.addr.bdaddr, bdaddr); + rp.addr.type = MGMT_ADDR_BREDR; + + err = cmd_complete(cmd->sk, hdev->id, MGMT_OP_PIN_CODE_NEG_REPLY, + mgmt_status(status), &rp, sizeof(rp)); + + mgmt_pending_remove(cmd); + + return err; +} + +int mgmt_user_confirm_request(struct hci_dev *hdev, bdaddr_t *bdaddr, + u8 link_type, u8 addr_type, __le32 value, + u8 confirm_hint) +{ + struct mgmt_ev_user_confirm_request ev; + + BT_DBG("%s", hdev->name); + + bacpy(&ev.addr.bdaddr, bdaddr); + ev.addr.type = link_to_mgmt(link_type, addr_type); + ev.confirm_hint = confirm_hint; + put_unaligned_le32(value, &ev.value); + + return mgmt_event(MGMT_EV_USER_CONFIRM_REQUEST, hdev, &ev, sizeof(ev), + NULL); +} + +int mgmt_user_passkey_request(struct hci_dev *hdev, bdaddr_t *bdaddr, + u8 link_type, u8 addr_type) +{ + struct mgmt_ev_user_passkey_request ev; + + BT_DBG("%s", hdev->name); + + bacpy(&ev.addr.bdaddr, bdaddr); + ev.addr.type = link_to_mgmt(link_type, addr_type); + + return mgmt_event(MGMT_EV_USER_PASSKEY_REQUEST, hdev, &ev, sizeof(ev), + NULL); +} + +static int user_pairing_resp_complete(struct hci_dev *hdev, bdaddr_t *bdaddr, + u8 link_type, u8 addr_type, u8 status, + u8 opcode) +{ + struct pending_cmd *cmd; + struct mgmt_rp_user_confirm_reply rp; + int err; + + cmd = mgmt_pending_find(opcode, hdev); + if (!cmd) + return -ENOENT; + + bacpy(&rp.addr.bdaddr, bdaddr); + rp.addr.type = link_to_mgmt(link_type, addr_type); + err = cmd_complete(cmd->sk, hdev->id, opcode, mgmt_status(status), + &rp, sizeof(rp)); + + mgmt_pending_remove(cmd); + + return err; +} + +int mgmt_user_confirm_reply_complete(struct hci_dev *hdev, bdaddr_t *bdaddr, + u8 link_type, u8 addr_type, u8 status) +{ + return user_pairing_resp_complete(hdev, bdaddr, link_type, addr_type, + status, MGMT_OP_USER_CONFIRM_REPLY); +} + +int mgmt_user_confirm_neg_reply_complete(struct hci_dev *hdev, bdaddr_t *bdaddr, + u8 link_type, u8 addr_type, u8 status) +{ + return user_pairing_resp_complete(hdev, bdaddr, link_type, addr_type, + status, MGMT_OP_USER_CONFIRM_NEG_REPLY); +} + +int mgmt_user_passkey_reply_complete(struct hci_dev *hdev, bdaddr_t *bdaddr, + u8 link_type, u8 addr_type, u8 status) +{ + return user_pairing_resp_complete(hdev, bdaddr, link_type, addr_type, + status, MGMT_OP_USER_PASSKEY_REPLY); +} + +int mgmt_user_passkey_neg_reply_complete(struct hci_dev *hdev, bdaddr_t *bdaddr, + u8 link_type, u8 addr_type, u8 status) +{ + return user_pairing_resp_complete(hdev, bdaddr, link_type, addr_type, + status, MGMT_OP_USER_PASSKEY_NEG_REPLY); +} + +int mgmt_auth_failed(struct hci_dev *hdev, bdaddr_t *bdaddr, u8 link_type, + u8 addr_type, u8 status) +{ + struct mgmt_ev_auth_failed ev; + + bacpy(&ev.addr.bdaddr, bdaddr); + ev.addr.type = link_to_mgmt(link_type, addr_type); + ev.status = mgmt_status(status); + + return mgmt_event(MGMT_EV_AUTH_FAILED, hdev, &ev, sizeof(ev), NULL); +} + +int mgmt_auth_enable_complete(struct hci_dev *hdev, u8 status) +{ + struct cmd_lookup match = { NULL, hdev }; + bool changed = false; + int err = 0; + + if (status) { + u8 mgmt_err = mgmt_status(status); + mgmt_pending_foreach(MGMT_OP_SET_LINK_SECURITY, hdev, + cmd_status_rsp, &mgmt_err); + return 0; + } + + if (test_bit(HCI_AUTH, &hdev->flags)) { + if (!test_and_set_bit(HCI_LINK_SECURITY, &hdev->dev_flags)) + changed = true; + } else { + if (test_and_clear_bit(HCI_LINK_SECURITY, &hdev->dev_flags)) + changed = true; + } + + mgmt_pending_foreach(MGMT_OP_SET_LINK_SECURITY, hdev, settings_rsp, + &match); + + if (changed) + err = new_settings(hdev, match.sk); + + if (match.sk) + sock_put(match.sk); + + return err; +} + +static int clear_eir(struct hci_dev *hdev) +{ + struct hci_cp_write_eir cp; + + if (!(hdev->features[6] & LMP_EXT_INQ)) + return 0; + + memset(hdev->eir, 0, sizeof(hdev->eir)); + + memset(&cp, 0, sizeof(cp)); + + return hci_send_cmd(hdev, HCI_OP_WRITE_EIR, sizeof(cp), &cp); +} + +int mgmt_ssp_enable_complete(struct hci_dev *hdev, u8 enable, u8 status) +{ + struct cmd_lookup match = { NULL, hdev }; + bool changed = false; + int err = 0; + + if (status) { + u8 mgmt_err = mgmt_status(status); + + if (enable && test_and_clear_bit(HCI_SSP_ENABLED, + &hdev->dev_flags)) + err = new_settings(hdev, NULL); + + mgmt_pending_foreach(MGMT_OP_SET_SSP, hdev, cmd_status_rsp, + &mgmt_err); + + return err; + } + + if (enable) { + if (!test_and_set_bit(HCI_SSP_ENABLED, &hdev->dev_flags)) + changed = true; + } else { + if (test_and_clear_bit(HCI_SSP_ENABLED, &hdev->dev_flags)) + changed = true; + } + + mgmt_pending_foreach(MGMT_OP_SET_SSP, hdev, settings_rsp, &match); + + if (changed) + err = new_settings(hdev, match.sk); + + if (match.sk) + sock_put(match.sk); + + if (test_bit(HCI_SSP_ENABLED, &hdev->dev_flags)) + update_eir(hdev); + else + clear_eir(hdev); + + return err; +} + +static void class_rsp(struct pending_cmd *cmd, void *data) +{ + struct cmd_lookup *match = data; + + cmd_complete(cmd->sk, cmd->index, cmd->opcode, match->mgmt_status, + match->hdev->dev_class, 3); + + list_del(&cmd->list); + + if (match->sk == NULL) { + match->sk = cmd->sk; + sock_hold(match->sk); + } + + mgmt_pending_free(cmd); +} + +int mgmt_set_class_of_dev_complete(struct hci_dev *hdev, u8 *dev_class, + u8 status) +{ + struct cmd_lookup match = { NULL, hdev, mgmt_status(status) }; + int err = 0; + + clear_bit(HCI_PENDING_CLASS, &hdev->dev_flags); + + mgmt_pending_foreach(MGMT_OP_SET_DEV_CLASS, hdev, class_rsp, &match); + mgmt_pending_foreach(MGMT_OP_ADD_UUID, hdev, class_rsp, &match); + mgmt_pending_foreach(MGMT_OP_REMOVE_UUID, hdev, class_rsp, &match); + + if (!status) + err = mgmt_event(MGMT_EV_CLASS_OF_DEV_CHANGED, hdev, dev_class, + 3, NULL); + + if (match.sk) + sock_put(match.sk); + + return err; +} + +int mgmt_set_local_name_complete(struct hci_dev *hdev, u8 *name, u8 status) +{ + struct pending_cmd *cmd; + struct mgmt_cp_set_local_name ev; + bool changed = false; + int err = 0; + + if (memcmp(name, hdev->dev_name, sizeof(hdev->dev_name)) != 0) { + memcpy(hdev->dev_name, name, sizeof(hdev->dev_name)); + changed = true; + } + + memset(&ev, 0, sizeof(ev)); + memcpy(ev.name, name, HCI_MAX_NAME_LENGTH); + memcpy(ev.short_name, hdev->short_name, HCI_MAX_SHORT_NAME_LENGTH); + + cmd = mgmt_pending_find(MGMT_OP_SET_LOCAL_NAME, hdev); + if (!cmd) + goto send_event; + + /* Always assume that either the short or the complete name has + * changed if there was a pending mgmt command */ + changed = true; + + if (status) { + err = cmd_status(cmd->sk, hdev->id, MGMT_OP_SET_LOCAL_NAME, + mgmt_status(status)); + goto failed; + } + + err = cmd_complete(cmd->sk, hdev->id, MGMT_OP_SET_LOCAL_NAME, 0, &ev, + sizeof(ev)); + if (err < 0) + goto failed; + +send_event: + if (changed) + err = mgmt_event(MGMT_EV_LOCAL_NAME_CHANGED, hdev, &ev, + sizeof(ev), cmd ? cmd->sk : NULL); + + update_eir(hdev); + +failed: + if (cmd) + mgmt_pending_remove(cmd); + return err; +} + +int mgmt_read_local_oob_data_reply_complete(struct hci_dev *hdev, u8 *hash, + u8 *randomizer, u8 status) +{ + struct pending_cmd *cmd; + int err; + + BT_DBG("%s status %u", hdev->name, status); + + cmd = mgmt_pending_find(MGMT_OP_READ_LOCAL_OOB_DATA, hdev); + if (!cmd) + return -ENOENT; + + if (status) { + err = cmd_status(cmd->sk, hdev->id, MGMT_OP_READ_LOCAL_OOB_DATA, + mgmt_status(status)); + } else { + struct mgmt_rp_read_local_oob_data rp; + + memcpy(rp.hash, hash, sizeof(rp.hash)); + memcpy(rp.randomizer, randomizer, sizeof(rp.randomizer)); + + err = cmd_complete(cmd->sk, hdev->id, + MGMT_OP_READ_LOCAL_OOB_DATA, 0, &rp, + sizeof(rp)); + } + + mgmt_pending_remove(cmd); + + return err; +} + +int mgmt_le_enable_complete(struct hci_dev *hdev, u8 enable, u8 status) +{ + struct cmd_lookup match = { NULL, hdev }; + bool changed = false; + int err = 0; + + if (status) { + u8 mgmt_err = mgmt_status(status); + + if (enable && test_and_clear_bit(HCI_LE_ENABLED, + &hdev->dev_flags)) + err = new_settings(hdev, NULL); + + mgmt_pending_foreach(MGMT_OP_SET_LE, hdev, + cmd_status_rsp, &mgmt_err); + + return err; + } + + if (enable) { + if (!test_and_set_bit(HCI_LE_ENABLED, &hdev->dev_flags)) + changed = true; + } else { + if (test_and_clear_bit(HCI_LE_ENABLED, &hdev->dev_flags)) + changed = true; + } + + mgmt_pending_foreach(MGMT_OP_SET_LE, hdev, settings_rsp, &match); + + if (changed) + err = new_settings(hdev, match.sk); + + if (match.sk) + sock_put(match.sk); + + return err; +} + +int mgmt_device_found(struct hci_dev *hdev, bdaddr_t *bdaddr, u8 link_type, + u8 addr_type, u8 *dev_class, s8 rssi, u8 cfm_name, u8 + ssp, u8 *eir, u16 eir_len) +{ + char buf[512]; + struct mgmt_ev_device_found *ev = (void *) buf; + size_t ev_size; + + /* Leave 5 bytes for a potential CoD field */ + if (sizeof(*ev) + eir_len + 5 > sizeof(buf)) + return -EINVAL; + + memset(buf, 0, sizeof(buf)); + + bacpy(&ev->addr.bdaddr, bdaddr); + ev->addr.type = link_to_mgmt(link_type, addr_type); + ev->rssi = rssi; + if (cfm_name) + ev->flags[0] |= MGMT_DEV_FOUND_CONFIRM_NAME; + if (!ssp) + ev->flags[0] |= MGMT_DEV_FOUND_LEGACY_PAIRING; + + if (eir_len > 0) + memcpy(ev->eir, eir, eir_len); + + if (dev_class && !eir_has_data_type(ev->eir, eir_len, EIR_CLASS_OF_DEV)) + eir_len = eir_append_data(ev->eir, eir_len, EIR_CLASS_OF_DEV, + dev_class, 3); + + put_unaligned_le16(eir_len, &ev->eir_len); + + ev_size = sizeof(*ev) + eir_len; + + return mgmt_event(MGMT_EV_DEVICE_FOUND, hdev, ev, ev_size, NULL); +} + +int mgmt_remote_name(struct hci_dev *hdev, bdaddr_t *bdaddr, u8 link_type, + u8 addr_type, s8 rssi, u8 *name, u8 name_len) +{ + struct mgmt_ev_device_found *ev; + char buf[sizeof(*ev) + HCI_MAX_NAME_LENGTH + 2]; + u16 eir_len; + + ev = (struct mgmt_ev_device_found *) buf; + + memset(buf, 0, sizeof(buf)); + + bacpy(&ev->addr.bdaddr, bdaddr); + ev->addr.type = link_to_mgmt(link_type, addr_type); + ev->rssi = rssi; + + eir_len = eir_append_data(ev->eir, 0, EIR_NAME_COMPLETE, name, + name_len); + + put_unaligned_le16(eir_len, &ev->eir_len); + + return mgmt_event(MGMT_EV_DEVICE_FOUND, hdev, ev, + sizeof(*ev) + eir_len, NULL); +} + +int mgmt_start_discovery_failed(struct hci_dev *hdev, u8 status) +{ + struct pending_cmd *cmd; + u8 type; + int err; + + hci_discovery_set_state(hdev, DISCOVERY_STOPPED); + + cmd = mgmt_pending_find(MGMT_OP_START_DISCOVERY, hdev); + if (!cmd) + return -ENOENT; + + type = hdev->discovery.type; + + err = cmd_complete(cmd->sk, hdev->id, cmd->opcode, mgmt_status(status), + &type, sizeof(type)); + mgmt_pending_remove(cmd); + + return err; +} + +int mgmt_stop_discovery_failed(struct hci_dev *hdev, u8 status) +{ + struct pending_cmd *cmd; + int err; + + cmd = mgmt_pending_find(MGMT_OP_STOP_DISCOVERY, hdev); + if (!cmd) + return -ENOENT; + + err = cmd_complete(cmd->sk, hdev->id, cmd->opcode, mgmt_status(status), + &hdev->discovery.type, sizeof(hdev->discovery.type)); + mgmt_pending_remove(cmd); + + return err; +} + +int mgmt_discovering(struct hci_dev *hdev, u8 discovering) +{ + struct mgmt_ev_discovering ev; + struct pending_cmd *cmd; + + BT_DBG("%s discovering %u", hdev->name, discovering); + + if (discovering) + cmd = mgmt_pending_find(MGMT_OP_START_DISCOVERY, hdev); + else + cmd = mgmt_pending_find(MGMT_OP_STOP_DISCOVERY, hdev); + + if (cmd != NULL) { + u8 type = hdev->discovery.type; + + cmd_complete(cmd->sk, hdev->id, cmd->opcode, 0, &type, + sizeof(type)); + mgmt_pending_remove(cmd); + } + + memset(&ev, 0, sizeof(ev)); + ev.type = hdev->discovery.type; + ev.discovering = discovering; + + return mgmt_event(MGMT_EV_DISCOVERING, hdev, &ev, sizeof(ev), NULL); +} + +int mgmt_device_blocked(struct hci_dev *hdev, bdaddr_t *bdaddr, u8 type) +{ + struct pending_cmd *cmd; + struct mgmt_ev_device_blocked ev; + + cmd = mgmt_pending_find(MGMT_OP_BLOCK_DEVICE, hdev); + + bacpy(&ev.addr.bdaddr, bdaddr); + ev.addr.type = type; + + return mgmt_event(MGMT_EV_DEVICE_BLOCKED, hdev, &ev, sizeof(ev), + cmd ? cmd->sk : NULL); +} + +int mgmt_device_unblocked(struct hci_dev *hdev, bdaddr_t *bdaddr, u8 type) +{ + struct pending_cmd *cmd; + struct mgmt_ev_device_unblocked ev; + + cmd = mgmt_pending_find(MGMT_OP_UNBLOCK_DEVICE, hdev); + + bacpy(&ev.addr.bdaddr, bdaddr); + ev.addr.type = type; + + return mgmt_event(MGMT_EV_DEVICE_UNBLOCKED, hdev, &ev, sizeof(ev), + cmd ? cmd->sk : NULL); +} + +module_param(enable_hs, bool, 0644); +MODULE_PARM_DESC(enable_hs, "Enable High Speed support"); + +module_param(enable_le, bool, 0644); +MODULE_PARM_DESC(enable_le, "Enable Low Energy support"); diff --git a/net/bluetooth_tizen/rfcomm/Kconfig b/net/bluetooth_tizen/rfcomm/Kconfig new file mode 100644 index 0000000..22e718b --- /dev/null +++ b/net/bluetooth_tizen/rfcomm/Kconfig @@ -0,0 +1,17 @@ +config BT_RFCOMM + tristate "RFCOMM protocol support" + depends on BT + help + RFCOMM provides connection oriented stream transport. RFCOMM + support is required for Dialup Networking, OBEX and other Bluetooth + applications. + + Say Y here to compile RFCOMM support into the kernel or say M to + compile it as module (rfcomm). + +config BT_RFCOMM_TTY + bool "RFCOMM TTY support" + depends on BT_RFCOMM + help + This option enables TTY emulation support for RFCOMM channels. + diff --git a/net/bluetooth_tizen/rfcomm/Makefile b/net/bluetooth_tizen/rfcomm/Makefile new file mode 100644 index 0000000..fe07988 --- /dev/null +++ b/net/bluetooth_tizen/rfcomm/Makefile @@ -0,0 +1,8 @@ +# +# Makefile for the Linux Bluetooth RFCOMM layer. +# + +obj-$(CONFIG_BT_RFCOMM) += rfcomm.o + +rfcomm-y := core.o sock.o +rfcomm-$(CONFIG_BT_RFCOMM_TTY) += tty.o diff --git a/net/bluetooth_tizen/rfcomm/core.c b/net/bluetooth_tizen/rfcomm/core.c new file mode 100644 index 0000000..8a60238 --- /dev/null +++ b/net/bluetooth_tizen/rfcomm/core.c @@ -0,0 +1,2245 @@ +/* + RFCOMM implementation for Linux Bluetooth stack (BlueZ). + Copyright (C) 2002 Maxim Krasnyansky <maxk@qualcomm.com> + Copyright (C) 2002 Marcel Holtmann <marcel@holtmann.org> + + This program is free software; you can redistribute it and/or modify + it under the terms of the GNU General Public License version 2 as + published by the Free Software Foundation; + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS + OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT OF THIRD PARTY RIGHTS. + IN NO EVENT SHALL THE COPYRIGHT HOLDER(S) AND AUTHOR(S) BE LIABLE FOR ANY + CLAIM, OR ANY SPECIAL INDIRECT OR CONSEQUENTIAL DAMAGES, OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + + ALL LIABILITY, INCLUDING LIABILITY FOR INFRINGEMENT OF ANY PATENTS, + COPYRIGHTS, TRADEMARKS OR OTHER RIGHTS, RELATING TO USE OF THIS + SOFTWARE IS DISCLAIMED. +*/ + +/* + * Bluetooth RFCOMM core. + */ + +#include <linux/module.h> +#include <linux/errno.h> +#include <linux/kernel.h> +#include <linux/sched.h> +#include <linux/signal.h> +#include <linux/init.h> +#include <linux/wait.h> +#include <linux/device.h> +#include <linux/debugfs.h> +#include <linux/seq_file.h> +#include <linux/net.h> +#include <linux/mutex.h> +#include <linux/kthread.h> +#include <linux/slab.h> + +#include <net/sock.h> +#include <linux/uaccess.h> +#include <asm/unaligned.h> + +#include <net/bluetooth/bluetooth.h> +#include <net/bluetooth/hci_core.h> +#include <net/bluetooth/l2cap.h> +#include <net/bluetooth/rfcomm.h> + +#define VERSION "1.11" + +static bool disable_cfc; +static bool l2cap_ertm; +static int channel_mtu = -1; +static unsigned int l2cap_mtu = RFCOMM_MAX_L2CAP_MTU; + +static struct task_struct *rfcomm_thread; + +static DEFINE_MUTEX(rfcomm_mutex); +#define rfcomm_lock() mutex_lock(&rfcomm_mutex) +#define rfcomm_unlock() mutex_unlock(&rfcomm_mutex) + + +static LIST_HEAD(session_list); + +static int rfcomm_send_frame(struct rfcomm_session *s, u8 *data, int len); +static int rfcomm_send_sabm(struct rfcomm_session *s, u8 dlci); +static int rfcomm_send_disc(struct rfcomm_session *s, u8 dlci); +static int rfcomm_queue_disc(struct rfcomm_dlc *d); +static int rfcomm_send_nsc(struct rfcomm_session *s, int cr, u8 type); +static int rfcomm_send_pn(struct rfcomm_session *s, int cr, struct rfcomm_dlc *d); +static int rfcomm_send_msc(struct rfcomm_session *s, int cr, u8 dlci, u8 v24_sig); +static int rfcomm_send_test(struct rfcomm_session *s, int cr, u8 *pattern, int len); +static int rfcomm_send_credits(struct rfcomm_session *s, u8 addr, u8 credits); +static void rfcomm_make_uih(struct sk_buff *skb, u8 addr); + +static void rfcomm_process_connect(struct rfcomm_session *s); + +static struct rfcomm_session *rfcomm_session_create(bdaddr_t *src, + bdaddr_t *dst, + u8 sec_level, + int *err); +static struct rfcomm_session *rfcomm_session_get(bdaddr_t *src, bdaddr_t *dst); +static void rfcomm_session_del(struct rfcomm_session *s); + +/* ---- RFCOMM frame parsing macros ---- */ +#define __get_dlci(b) ((b & 0xfc) >> 2) +#define __get_channel(b) ((b & 0xf8) >> 3) +#define __get_dir(b) ((b & 0x04) >> 2) +#define __get_type(b) ((b & 0xef)) + +#define __test_ea(b) ((b & 0x01)) +#define __test_cr(b) ((b & 0x02)) +#define __test_pf(b) ((b & 0x10)) + +#define __addr(cr, dlci) (((dlci & 0x3f) << 2) | (cr << 1) | 0x01) +#define __ctrl(type, pf) (((type & 0xef) | (pf << 4))) +#define __dlci(dir, chn) (((chn & 0x1f) << 1) | dir) +#define __srv_channel(dlci) (dlci >> 1) +#define __dir(dlci) (dlci & 0x01) + +#define __len8(len) (((len) << 1) | 1) +#define __len16(len) ((len) << 1) + +/* MCC macros */ +#define __mcc_type(cr, type) (((type << 2) | (cr << 1) | 0x01)) +#define __get_mcc_type(b) ((b & 0xfc) >> 2) +#define __get_mcc_len(b) ((b & 0xfe) >> 1) + +/* RPN macros */ +#define __rpn_line_settings(data, stop, parity) ((data & 0x3) | ((stop & 0x1) << 2) | ((parity & 0x7) << 3)) +#define __get_rpn_data_bits(line) ((line) & 0x3) +#define __get_rpn_stop_bits(line) (((line) >> 2) & 0x1) +#define __get_rpn_parity(line) (((line) >> 3) & 0x7) + +static inline void rfcomm_schedule(void) +{ + if (!rfcomm_thread) + return; + wake_up_process(rfcomm_thread); +} + +static inline void rfcomm_session_put(struct rfcomm_session *s) +{ + if (atomic_dec_and_test(&s->refcnt)) + rfcomm_session_del(s); +} + +/* ---- RFCOMM FCS computation ---- */ + +/* reversed, 8-bit, poly=0x07 */ +static unsigned char rfcomm_crc_table[256] = { + 0x00, 0x91, 0xe3, 0x72, 0x07, 0x96, 0xe4, 0x75, + 0x0e, 0x9f, 0xed, 0x7c, 0x09, 0x98, 0xea, 0x7b, + 0x1c, 0x8d, 0xff, 0x6e, 0x1b, 0x8a, 0xf8, 0x69, + 0x12, 0x83, 0xf1, 0x60, 0x15, 0x84, 0xf6, 0x67, + + 0x38, 0xa9, 0xdb, 0x4a, 0x3f, 0xae, 0xdc, 0x4d, + 0x36, 0xa7, 0xd5, 0x44, 0x31, 0xa0, 0xd2, 0x43, + 0x24, 0xb5, 0xc7, 0x56, 0x23, 0xb2, 0xc0, 0x51, + 0x2a, 0xbb, 0xc9, 0x58, 0x2d, 0xbc, 0xce, 0x5f, + + 0x70, 0xe1, 0x93, 0x02, 0x77, 0xe6, 0x94, 0x05, + 0x7e, 0xef, 0x9d, 0x0c, 0x79, 0xe8, 0x9a, 0x0b, + 0x6c, 0xfd, 0x8f, 0x1e, 0x6b, 0xfa, 0x88, 0x19, + 0x62, 0xf3, 0x81, 0x10, 0x65, 0xf4, 0x86, 0x17, + + 0x48, 0xd9, 0xab, 0x3a, 0x4f, 0xde, 0xac, 0x3d, + 0x46, 0xd7, 0xa5, 0x34, 0x41, 0xd0, 0xa2, 0x33, + 0x54, 0xc5, 0xb7, 0x26, 0x53, 0xc2, 0xb0, 0x21, + 0x5a, 0xcb, 0xb9, 0x28, 0x5d, 0xcc, 0xbe, 0x2f, + + 0xe0, 0x71, 0x03, 0x92, 0xe7, 0x76, 0x04, 0x95, + 0xee, 0x7f, 0x0d, 0x9c, 0xe9, 0x78, 0x0a, 0x9b, + 0xfc, 0x6d, 0x1f, 0x8e, 0xfb, 0x6a, 0x18, 0x89, + 0xf2, 0x63, 0x11, 0x80, 0xf5, 0x64, 0x16, 0x87, + + 0xd8, 0x49, 0x3b, 0xaa, 0xdf, 0x4e, 0x3c, 0xad, + 0xd6, 0x47, 0x35, 0xa4, 0xd1, 0x40, 0x32, 0xa3, + 0xc4, 0x55, 0x27, 0xb6, 0xc3, 0x52, 0x20, 0xb1, + 0xca, 0x5b, 0x29, 0xb8, 0xcd, 0x5c, 0x2e, 0xbf, + + 0x90, 0x01, 0x73, 0xe2, 0x97, 0x06, 0x74, 0xe5, + 0x9e, 0x0f, 0x7d, 0xec, 0x99, 0x08, 0x7a, 0xeb, + 0x8c, 0x1d, 0x6f, 0xfe, 0x8b, 0x1a, 0x68, 0xf9, + 0x82, 0x13, 0x61, 0xf0, 0x85, 0x14, 0x66, 0xf7, + + 0xa8, 0x39, 0x4b, 0xda, 0xaf, 0x3e, 0x4c, 0xdd, + 0xa6, 0x37, 0x45, 0xd4, 0xa1, 0x30, 0x42, 0xd3, + 0xb4, 0x25, 0x57, 0xc6, 0xb3, 0x22, 0x50, 0xc1, + 0xba, 0x2b, 0x59, 0xc8, 0xbd, 0x2c, 0x5e, 0xcf +}; + +/* CRC on 2 bytes */ +#define __crc(data) (rfcomm_crc_table[rfcomm_crc_table[0xff ^ data[0]] ^ data[1]]) + +/* FCS on 2 bytes */ +static inline u8 __fcs(u8 *data) +{ + return 0xff - __crc(data); +} + +/* FCS on 3 bytes */ +static inline u8 __fcs2(u8 *data) +{ + return 0xff - rfcomm_crc_table[__crc(data) ^ data[2]]; +} + +/* Check FCS */ +static inline int __check_fcs(u8 *data, int type, u8 fcs) +{ + u8 f = __crc(data); + + if (type != RFCOMM_UIH) + f = rfcomm_crc_table[f ^ data[2]]; + + return rfcomm_crc_table[f ^ fcs] != 0xcf; +} + +/* ---- L2CAP callbacks ---- */ +static void rfcomm_l2state_change(struct sock *sk) +{ + BT_DBG("%p state %d", sk, sk->sk_state); + rfcomm_schedule(); +} + +static void rfcomm_l2data_ready(struct sock *sk, int bytes) +{ + BT_DBG("%p bytes %d", sk, bytes); + rfcomm_schedule(); +} + +static int rfcomm_l2sock_create(struct socket **sock) +{ + int err; + + BT_DBG(""); + + err = sock_create_kern(PF_BLUETOOTH, SOCK_SEQPACKET, BTPROTO_L2CAP, sock); + if (!err) { + struct sock *sk = (*sock)->sk; + sk->sk_data_ready = rfcomm_l2data_ready; + sk->sk_state_change = rfcomm_l2state_change; + } + return err; +} + +static inline int rfcomm_check_security(struct rfcomm_dlc *d) +{ + struct sock *sk = d->session->sock->sk; + struct l2cap_conn *conn = l2cap_pi(sk)->chan->conn; + + __u8 auth_type; + + switch (d->sec_level) { + case BT_SECURITY_HIGH: + auth_type = HCI_AT_GENERAL_BONDING_MITM; + break; + case BT_SECURITY_MEDIUM: + auth_type = HCI_AT_GENERAL_BONDING; + break; + default: + auth_type = HCI_AT_NO_BONDING; + break; + } + + return hci_conn_security(conn->hcon, d->sec_level, auth_type); +} + +static void rfcomm_session_timeout(unsigned long arg) +{ + struct rfcomm_session *s = (void *) arg; + + BT_DBG("session %p state %ld", s, s->state); + + set_bit(RFCOMM_TIMED_OUT, &s->flags); + rfcomm_schedule(); +} + +static void rfcomm_session_set_timer(struct rfcomm_session *s, long timeout) +{ + BT_DBG("session %p state %ld timeout %ld", s, s->state, timeout); + + if (!mod_timer(&s->timer, jiffies + timeout)) + rfcomm_session_hold(s); +} + +static void rfcomm_session_clear_timer(struct rfcomm_session *s) +{ + BT_DBG("session %p state %ld", s, s->state); + + if (timer_pending(&s->timer) && del_timer(&s->timer)) + rfcomm_session_put(s); +} + +/* ---- RFCOMM DLCs ---- */ +static void rfcomm_dlc_timeout(unsigned long arg) +{ + struct rfcomm_dlc *d = (void *) arg; + + BT_DBG("dlc %p state %ld", d, d->state); + + set_bit(RFCOMM_TIMED_OUT, &d->flags); + rfcomm_dlc_put(d); + rfcomm_schedule(); +} + +static void rfcomm_dlc_set_timer(struct rfcomm_dlc *d, long timeout) +{ + BT_DBG("dlc %p state %ld timeout %ld", d, d->state, timeout); + + if (!mod_timer(&d->timer, jiffies + timeout)) + rfcomm_dlc_hold(d); +} + +static void rfcomm_dlc_clear_timer(struct rfcomm_dlc *d) +{ + BT_DBG("dlc %p state %ld", d, d->state); + + if (timer_pending(&d->timer) && del_timer(&d->timer)) + rfcomm_dlc_put(d); +} + +static void rfcomm_dlc_clear_state(struct rfcomm_dlc *d) +{ + BT_DBG("%p", d); + + d->state = BT_OPEN; + d->flags = 0; + d->mscex = 0; + d->sec_level = BT_SECURITY_LOW; + d->mtu = RFCOMM_DEFAULT_MTU; + d->v24_sig = RFCOMM_V24_RTC | RFCOMM_V24_RTR | RFCOMM_V24_DV; + + d->cfc = RFCOMM_CFC_DISABLED; + d->rx_credits = RFCOMM_DEFAULT_CREDITS; +} + +struct rfcomm_dlc *rfcomm_dlc_alloc(gfp_t prio) +{ + struct rfcomm_dlc *d = kzalloc(sizeof(*d), prio); + + if (!d) + return NULL; + + setup_timer(&d->timer, rfcomm_dlc_timeout, (unsigned long)d); + + skb_queue_head_init(&d->tx_queue); + spin_lock_init(&d->lock); + atomic_set(&d->refcnt, 1); + + rfcomm_dlc_clear_state(d); + + BT_DBG("%p", d); + + return d; +} + +void rfcomm_dlc_free(struct rfcomm_dlc *d) +{ + BT_DBG("%p", d); + + skb_queue_purge(&d->tx_queue); + kfree(d); +} + +static void rfcomm_dlc_link(struct rfcomm_session *s, struct rfcomm_dlc *d) +{ + BT_DBG("dlc %p session %p", d, s); + + rfcomm_session_hold(s); + + rfcomm_session_clear_timer(s); + rfcomm_dlc_hold(d); + list_add(&d->list, &s->dlcs); + d->session = s; +} + +static void rfcomm_dlc_unlink(struct rfcomm_dlc *d) +{ + struct rfcomm_session *s = d->session; + + BT_DBG("dlc %p refcnt %d session %p", d, atomic_read(&d->refcnt), s); + + list_del(&d->list); + d->session = NULL; + rfcomm_dlc_put(d); + + if (list_empty(&s->dlcs)) + rfcomm_session_set_timer(s, RFCOMM_IDLE_TIMEOUT); + + rfcomm_session_put(s); +} + +static struct rfcomm_dlc *rfcomm_dlc_get(struct rfcomm_session *s, u8 dlci) +{ + struct rfcomm_dlc *d; + + list_for_each_entry(d, &s->dlcs, list) + if (d->dlci == dlci) + return d; + + return NULL; +} + +static int __rfcomm_dlc_open(struct rfcomm_dlc *d, bdaddr_t *src, bdaddr_t *dst, u8 channel) +{ + struct rfcomm_session *s; + int err = 0; + u8 dlci; + + BT_DBG("dlc %p state %ld %s %s channel %d", + d, d->state, batostr(src), batostr(dst), channel); + + if (channel < 1 || channel > 30) + return -EINVAL; + + if (d->state != BT_OPEN && d->state != BT_CLOSED) + return 0; + + s = rfcomm_session_get(src, dst); + if (!s) { + s = rfcomm_session_create(src, dst, d->sec_level, &err); + if (!s) + return err; + } + + dlci = __dlci(!s->initiator, channel); + + /* Check if DLCI already exists */ + if (rfcomm_dlc_get(s, dlci)) + return -EBUSY; + + rfcomm_dlc_clear_state(d); + + d->dlci = dlci; + d->addr = __addr(s->initiator, dlci); + d->priority = 7; + + d->state = BT_CONFIG; + rfcomm_dlc_link(s, d); + + d->out = 1; + + d->mtu = s->mtu; + d->cfc = (s->cfc == RFCOMM_CFC_UNKNOWN) ? 0 : s->cfc; + + if (s->state == BT_CONNECTED) { + if (rfcomm_check_security(d)) + rfcomm_send_pn(s, 1, d); + else + set_bit(RFCOMM_AUTH_PENDING, &d->flags); + } + + rfcomm_dlc_set_timer(d, RFCOMM_CONN_TIMEOUT); + + return 0; +} + +int rfcomm_dlc_open(struct rfcomm_dlc *d, bdaddr_t *src, bdaddr_t *dst, u8 channel) +{ + int r; + + rfcomm_lock(); + + r = __rfcomm_dlc_open(d, src, dst, channel); + + rfcomm_unlock(); + return r; +} + +static int __rfcomm_dlc_close(struct rfcomm_dlc *d, int err) +{ + struct rfcomm_session *s = d->session; + if (!s) + return 0; + + BT_DBG("dlc %p state %ld dlci %d err %d session %p", + d, d->state, d->dlci, err, s); + + switch (d->state) { + case BT_CONNECT: + case BT_CONFIG: + if (test_and_clear_bit(RFCOMM_DEFER_SETUP, &d->flags)) { + set_bit(RFCOMM_AUTH_REJECT, &d->flags); + rfcomm_schedule(); + break; + } + /* Fall through */ + + case BT_CONNECTED: + d->state = BT_DISCONN; + if (skb_queue_empty(&d->tx_queue)) { + rfcomm_send_disc(s, d->dlci); + rfcomm_dlc_set_timer(d, RFCOMM_DISC_TIMEOUT); + } else { + rfcomm_queue_disc(d); + rfcomm_dlc_set_timer(d, RFCOMM_DISC_TIMEOUT * 2); + } + break; + + case BT_OPEN: + case BT_CONNECT2: + if (test_and_clear_bit(RFCOMM_DEFER_SETUP, &d->flags)) { + set_bit(RFCOMM_AUTH_REJECT, &d->flags); + rfcomm_schedule(); + break; + } + /* Fall through */ + + default: + rfcomm_dlc_clear_timer(d); + + rfcomm_dlc_lock(d); + d->state = BT_CLOSED; + d->state_change(d, err); + rfcomm_dlc_unlock(d); + + skb_queue_purge(&d->tx_queue); + rfcomm_dlc_unlink(d); + } + + return 0; +} + +int rfcomm_dlc_close(struct rfcomm_dlc *d, int err) +{ + int r; + + rfcomm_lock(); + + r = __rfcomm_dlc_close(d, err); + + rfcomm_unlock(); + return r; +} + +int rfcomm_dlc_send(struct rfcomm_dlc *d, struct sk_buff *skb) +{ + int len = skb->len; + + if (d->state != BT_CONNECTED) + return -ENOTCONN; + + BT_DBG("dlc %p mtu %d len %d", d, d->mtu, len); + + if (len > d->mtu) + return -EINVAL; + + rfcomm_make_uih(skb, d->addr); + skb_queue_tail(&d->tx_queue, skb); + + if (!test_bit(RFCOMM_TX_THROTTLED, &d->flags)) + rfcomm_schedule(); + return len; +} + +void __rfcomm_dlc_throttle(struct rfcomm_dlc *d) +{ + BT_DBG("dlc %p state %ld", d, d->state); + + if (!d->cfc) { + d->v24_sig |= RFCOMM_V24_FC; + set_bit(RFCOMM_MSC_PENDING, &d->flags); + } + rfcomm_schedule(); +} + +void __rfcomm_dlc_unthrottle(struct rfcomm_dlc *d) +{ + BT_DBG("dlc %p state %ld", d, d->state); + + if (!d->cfc) { + d->v24_sig &= ~RFCOMM_V24_FC; + set_bit(RFCOMM_MSC_PENDING, &d->flags); + } + rfcomm_schedule(); +} + +/* + Set/get modem status functions use _local_ status i.e. what we report + to the other side. + Remote status is provided by dlc->modem_status() callback. + */ +int rfcomm_dlc_set_modem_status(struct rfcomm_dlc *d, u8 v24_sig) +{ + BT_DBG("dlc %p state %ld v24_sig 0x%x", + d, d->state, v24_sig); + + if (test_bit(RFCOMM_RX_THROTTLED, &d->flags)) + v24_sig |= RFCOMM_V24_FC; + else + v24_sig &= ~RFCOMM_V24_FC; + + d->v24_sig = v24_sig; + + if (!test_and_set_bit(RFCOMM_MSC_PENDING, &d->flags)) + rfcomm_schedule(); + + return 0; +} + +int rfcomm_dlc_get_modem_status(struct rfcomm_dlc *d, u8 *v24_sig) +{ + BT_DBG("dlc %p state %ld v24_sig 0x%x", + d, d->state, d->v24_sig); + + *v24_sig = d->v24_sig; + return 0; +} + +/* ---- RFCOMM sessions ---- */ +static struct rfcomm_session *rfcomm_session_add(struct socket *sock, int state) +{ + struct rfcomm_session *s = kzalloc(sizeof(*s), GFP_KERNEL); + + if (!s) + return NULL; + + BT_DBG("session %p sock %p", s, sock); + + setup_timer(&s->timer, rfcomm_session_timeout, (unsigned long) s); + + INIT_LIST_HEAD(&s->dlcs); + s->state = state; + s->sock = sock; + + s->mtu = RFCOMM_DEFAULT_MTU; + s->cfc = disable_cfc ? RFCOMM_CFC_DISABLED : RFCOMM_CFC_UNKNOWN; + + /* Do not increment module usage count for listening sessions. + * Otherwise we won't be able to unload the module. */ + if (state != BT_LISTEN) + if (!try_module_get(THIS_MODULE)) { + kfree(s); + return NULL; + } + + list_add(&s->list, &session_list); + + return s; +} + +static void rfcomm_session_del(struct rfcomm_session *s) +{ + int state = s->state; + + BT_DBG("session %p state %ld", s, s->state); + + list_del(&s->list); + + if (state == BT_CONNECTED) + rfcomm_send_disc(s, 0); + + rfcomm_session_clear_timer(s); + sock_release(s->sock); + kfree(s); + + if (state != BT_LISTEN) + module_put(THIS_MODULE); +} + +static struct rfcomm_session *rfcomm_session_get(bdaddr_t *src, bdaddr_t *dst) +{ + struct rfcomm_session *s; + struct list_head *p, *n; + struct bt_sock *sk; + list_for_each_safe(p, n, &session_list) { + s = list_entry(p, struct rfcomm_session, list); + sk = bt_sk(s->sock->sk); + + if ((!bacmp(src, BDADDR_ANY) || !bacmp(&sk->src, src)) && + !bacmp(&sk->dst, dst)) + return s; + } + return NULL; +} + +static void rfcomm_session_close(struct rfcomm_session *s, int err) +{ + struct rfcomm_dlc *d; + struct list_head *p, *n; + + BT_DBG("session %p state %ld err %d", s, s->state, err); + + rfcomm_session_hold(s); + + s->state = BT_CLOSED; + + /* Close all dlcs */ + list_for_each_safe(p, n, &s->dlcs) { + d = list_entry(p, struct rfcomm_dlc, list); + d->state = BT_CLOSED; + __rfcomm_dlc_close(d, err); + } + + rfcomm_session_clear_timer(s); + rfcomm_session_put(s); +} + +static struct rfcomm_session *rfcomm_session_create(bdaddr_t *src, + bdaddr_t *dst, + u8 sec_level, + int *err) +{ + struct rfcomm_session *s = NULL; + struct sockaddr_l2 addr; + struct socket *sock; + struct sock *sk; + + BT_DBG("%s %s", batostr(src), batostr(dst)); + + *err = rfcomm_l2sock_create(&sock); + if (*err < 0) + return NULL; + + bacpy(&addr.l2_bdaddr, src); + addr.l2_family = AF_BLUETOOTH; + addr.l2_psm = 0; + addr.l2_cid = 0; + *err = kernel_bind(sock, (struct sockaddr *) &addr, sizeof(addr)); + if (*err < 0) + goto failed; + + /* Set L2CAP options */ + sk = sock->sk; + lock_sock(sk); + l2cap_pi(sk)->chan->imtu = l2cap_mtu; + l2cap_pi(sk)->chan->sec_level = sec_level; + if (l2cap_ertm) + l2cap_pi(sk)->chan->mode = L2CAP_MODE_ERTM; + release_sock(sk); + + s = rfcomm_session_add(sock, BT_BOUND); + if (!s) { + *err = -ENOMEM; + goto failed; + } + + s->initiator = 1; + + bacpy(&addr.l2_bdaddr, dst); + addr.l2_family = AF_BLUETOOTH; + addr.l2_psm = cpu_to_le16(RFCOMM_PSM); + addr.l2_cid = 0; + *err = kernel_connect(sock, (struct sockaddr *) &addr, sizeof(addr), O_NONBLOCK); + if (*err == 0 || *err == -EINPROGRESS) + return s; + + rfcomm_session_del(s); + return NULL; + +failed: + sock_release(sock); + return NULL; +} + +void rfcomm_session_getaddr(struct rfcomm_session *s, bdaddr_t *src, bdaddr_t *dst) +{ + struct sock *sk = s->sock->sk; + if (src) + bacpy(src, &bt_sk(sk)->src); + if (dst) + bacpy(dst, &bt_sk(sk)->dst); +} + +/* ---- RFCOMM frame sending ---- */ +static int rfcomm_send_frame(struct rfcomm_session *s, u8 *data, int len) +{ + struct kvec iv = { data, len }; + struct msghdr msg; + + BT_DBG("session %p len %d", s, len); + + memset(&msg, 0, sizeof(msg)); + + return kernel_sendmsg(s->sock, &msg, &iv, 1, len); +} + +static int rfcomm_send_cmd(struct rfcomm_session *s, struct rfcomm_cmd *cmd) +{ + BT_DBG("%p cmd %u", s, cmd->ctrl); + + return rfcomm_send_frame(s, (void *) cmd, sizeof(*cmd)); +} + +static int rfcomm_send_sabm(struct rfcomm_session *s, u8 dlci) +{ + struct rfcomm_cmd cmd; + + BT_DBG("%p dlci %d", s, dlci); + + cmd.addr = __addr(s->initiator, dlci); + cmd.ctrl = __ctrl(RFCOMM_SABM, 1); + cmd.len = __len8(0); + cmd.fcs = __fcs2((u8 *) &cmd); + + return rfcomm_send_cmd(s, &cmd); +} + +static int rfcomm_send_ua(struct rfcomm_session *s, u8 dlci) +{ + struct rfcomm_cmd cmd; + + BT_DBG("%p dlci %d", s, dlci); + + cmd.addr = __addr(!s->initiator, dlci); + cmd.ctrl = __ctrl(RFCOMM_UA, 1); + cmd.len = __len8(0); + cmd.fcs = __fcs2((u8 *) &cmd); + + return rfcomm_send_cmd(s, &cmd); +} + +static int rfcomm_send_disc(struct rfcomm_session *s, u8 dlci) +{ + struct rfcomm_cmd cmd; + + BT_DBG("%p dlci %d", s, dlci); + + cmd.addr = __addr(s->initiator, dlci); + cmd.ctrl = __ctrl(RFCOMM_DISC, 1); + cmd.len = __len8(0); + cmd.fcs = __fcs2((u8 *) &cmd); + + return rfcomm_send_cmd(s, &cmd); +} + +static int rfcomm_queue_disc(struct rfcomm_dlc *d) +{ + struct rfcomm_cmd *cmd; + struct sk_buff *skb; + + BT_DBG("dlc %p dlci %d", d, d->dlci); + + skb = alloc_skb(sizeof(*cmd), GFP_KERNEL); + if (!skb) + return -ENOMEM; + + cmd = (void *) __skb_put(skb, sizeof(*cmd)); + cmd->addr = d->addr; + cmd->ctrl = __ctrl(RFCOMM_DISC, 1); + cmd->len = __len8(0); + cmd->fcs = __fcs2((u8 *) cmd); + + skb_queue_tail(&d->tx_queue, skb); + rfcomm_schedule(); + return 0; +} + +static int rfcomm_send_dm(struct rfcomm_session *s, u8 dlci) +{ + struct rfcomm_cmd cmd; + + BT_DBG("%p dlci %d", s, dlci); + + cmd.addr = __addr(!s->initiator, dlci); + cmd.ctrl = __ctrl(RFCOMM_DM, 1); + cmd.len = __len8(0); + cmd.fcs = __fcs2((u8 *) &cmd); + + return rfcomm_send_cmd(s, &cmd); +} + +static int rfcomm_send_nsc(struct rfcomm_session *s, int cr, u8 type) +{ + struct rfcomm_hdr *hdr; + struct rfcomm_mcc *mcc; + u8 buf[16], *ptr = buf; + + BT_DBG("%p cr %d type %d", s, cr, type); + + hdr = (void *) ptr; ptr += sizeof(*hdr); + hdr->addr = __addr(s->initiator, 0); + hdr->ctrl = __ctrl(RFCOMM_UIH, 0); + hdr->len = __len8(sizeof(*mcc) + 1); + + mcc = (void *) ptr; ptr += sizeof(*mcc); + mcc->type = __mcc_type(cr, RFCOMM_NSC); + mcc->len = __len8(1); + + /* Type that we didn't like */ + *ptr = __mcc_type(cr, type); ptr++; + + *ptr = __fcs(buf); ptr++; + + return rfcomm_send_frame(s, buf, ptr - buf); +} + +static int rfcomm_send_pn(struct rfcomm_session *s, int cr, struct rfcomm_dlc *d) +{ + struct rfcomm_hdr *hdr; + struct rfcomm_mcc *mcc; + struct rfcomm_pn *pn; + u8 buf[16], *ptr = buf; + + BT_DBG("%p cr %d dlci %d mtu %d", s, cr, d->dlci, d->mtu); + + hdr = (void *) ptr; ptr += sizeof(*hdr); + hdr->addr = __addr(s->initiator, 0); + hdr->ctrl = __ctrl(RFCOMM_UIH, 0); + hdr->len = __len8(sizeof(*mcc) + sizeof(*pn)); + + mcc = (void *) ptr; ptr += sizeof(*mcc); + mcc->type = __mcc_type(cr, RFCOMM_PN); + mcc->len = __len8(sizeof(*pn)); + + pn = (void *) ptr; ptr += sizeof(*pn); + pn->dlci = d->dlci; + pn->priority = d->priority; + pn->ack_timer = 0; + pn->max_retrans = 0; + + if (s->cfc) { + pn->flow_ctrl = cr ? 0xf0 : 0xe0; + pn->credits = RFCOMM_DEFAULT_CREDITS; + } else { + pn->flow_ctrl = 0; + pn->credits = 0; + } + + if (cr && channel_mtu >= 0) + pn->mtu = cpu_to_le16(channel_mtu); + else + pn->mtu = cpu_to_le16(d->mtu); + + *ptr = __fcs(buf); ptr++; + + return rfcomm_send_frame(s, buf, ptr - buf); +} + +int rfcomm_send_rpn(struct rfcomm_session *s, int cr, u8 dlci, + u8 bit_rate, u8 data_bits, u8 stop_bits, + u8 parity, u8 flow_ctrl_settings, + u8 xon_char, u8 xoff_char, u16 param_mask) +{ + struct rfcomm_hdr *hdr; + struct rfcomm_mcc *mcc; + struct rfcomm_rpn *rpn; + u8 buf[16], *ptr = buf; + + BT_DBG("%p cr %d dlci %d bit_r 0x%x data_b 0x%x stop_b 0x%x parity 0x%x" + " flwc_s 0x%x xon_c 0x%x xoff_c 0x%x p_mask 0x%x", + s, cr, dlci, bit_rate, data_bits, stop_bits, parity, + flow_ctrl_settings, xon_char, xoff_char, param_mask); + + hdr = (void *) ptr; ptr += sizeof(*hdr); + hdr->addr = __addr(s->initiator, 0); + hdr->ctrl = __ctrl(RFCOMM_UIH, 0); + hdr->len = __len8(sizeof(*mcc) + sizeof(*rpn)); + + mcc = (void *) ptr; ptr += sizeof(*mcc); + mcc->type = __mcc_type(cr, RFCOMM_RPN); + mcc->len = __len8(sizeof(*rpn)); + + rpn = (void *) ptr; ptr += sizeof(*rpn); + rpn->dlci = __addr(1, dlci); + rpn->bit_rate = bit_rate; + rpn->line_settings = __rpn_line_settings(data_bits, stop_bits, parity); + rpn->flow_ctrl = flow_ctrl_settings; + rpn->xon_char = xon_char; + rpn->xoff_char = xoff_char; + rpn->param_mask = cpu_to_le16(param_mask); + + *ptr = __fcs(buf); ptr++; + + return rfcomm_send_frame(s, buf, ptr - buf); +} + +static int rfcomm_send_rls(struct rfcomm_session *s, int cr, u8 dlci, u8 status) +{ + struct rfcomm_hdr *hdr; + struct rfcomm_mcc *mcc; + struct rfcomm_rls *rls; + u8 buf[16], *ptr = buf; + + BT_DBG("%p cr %d status 0x%x", s, cr, status); + + hdr = (void *) ptr; ptr += sizeof(*hdr); + hdr->addr = __addr(s->initiator, 0); + hdr->ctrl = __ctrl(RFCOMM_UIH, 0); + hdr->len = __len8(sizeof(*mcc) + sizeof(*rls)); + + mcc = (void *) ptr; ptr += sizeof(*mcc); + mcc->type = __mcc_type(cr, RFCOMM_RLS); + mcc->len = __len8(sizeof(*rls)); + + rls = (void *) ptr; ptr += sizeof(*rls); + rls->dlci = __addr(1, dlci); + rls->status = status; + + *ptr = __fcs(buf); ptr++; + + return rfcomm_send_frame(s, buf, ptr - buf); +} + +static int rfcomm_send_msc(struct rfcomm_session *s, int cr, u8 dlci, u8 v24_sig) +{ + struct rfcomm_hdr *hdr; + struct rfcomm_mcc *mcc; + struct rfcomm_msc *msc; + u8 buf[16], *ptr = buf; + + BT_DBG("%p cr %d v24 0x%x", s, cr, v24_sig); + + hdr = (void *) ptr; ptr += sizeof(*hdr); + hdr->addr = __addr(s->initiator, 0); + hdr->ctrl = __ctrl(RFCOMM_UIH, 0); + hdr->len = __len8(sizeof(*mcc) + sizeof(*msc)); + + mcc = (void *) ptr; ptr += sizeof(*mcc); + mcc->type = __mcc_type(cr, RFCOMM_MSC); + mcc->len = __len8(sizeof(*msc)); + + msc = (void *) ptr; ptr += sizeof(*msc); + msc->dlci = __addr(1, dlci); + msc->v24_sig = v24_sig | 0x01; + + *ptr = __fcs(buf); ptr++; + + return rfcomm_send_frame(s, buf, ptr - buf); +} + +static int rfcomm_send_fcoff(struct rfcomm_session *s, int cr) +{ + struct rfcomm_hdr *hdr; + struct rfcomm_mcc *mcc; + u8 buf[16], *ptr = buf; + + BT_DBG("%p cr %d", s, cr); + + hdr = (void *) ptr; ptr += sizeof(*hdr); + hdr->addr = __addr(s->initiator, 0); + hdr->ctrl = __ctrl(RFCOMM_UIH, 0); + hdr->len = __len8(sizeof(*mcc)); + + mcc = (void *) ptr; ptr += sizeof(*mcc); + mcc->type = __mcc_type(cr, RFCOMM_FCOFF); + mcc->len = __len8(0); + + *ptr = __fcs(buf); ptr++; + + return rfcomm_send_frame(s, buf, ptr - buf); +} + +static int rfcomm_send_fcon(struct rfcomm_session *s, int cr) +{ + struct rfcomm_hdr *hdr; + struct rfcomm_mcc *mcc; + u8 buf[16], *ptr = buf; + + BT_DBG("%p cr %d", s, cr); + + hdr = (void *) ptr; ptr += sizeof(*hdr); + hdr->addr = __addr(s->initiator, 0); + hdr->ctrl = __ctrl(RFCOMM_UIH, 0); + hdr->len = __len8(sizeof(*mcc)); + + mcc = (void *) ptr; ptr += sizeof(*mcc); + mcc->type = __mcc_type(cr, RFCOMM_FCON); + mcc->len = __len8(0); + + *ptr = __fcs(buf); ptr++; + + return rfcomm_send_frame(s, buf, ptr - buf); +} + +static int rfcomm_send_test(struct rfcomm_session *s, int cr, u8 *pattern, int len) +{ + struct socket *sock = s->sock; + struct kvec iv[3]; + struct msghdr msg; + unsigned char hdr[5], crc[1]; + + if (len > 125) + return -EINVAL; + + BT_DBG("%p cr %d", s, cr); + + hdr[0] = __addr(s->initiator, 0); + hdr[1] = __ctrl(RFCOMM_UIH, 0); + hdr[2] = 0x01 | ((len + 2) << 1); + hdr[3] = 0x01 | ((cr & 0x01) << 1) | (RFCOMM_TEST << 2); + hdr[4] = 0x01 | (len << 1); + + crc[0] = __fcs(hdr); + + iv[0].iov_base = hdr; + iv[0].iov_len = 5; + iv[1].iov_base = pattern; + iv[1].iov_len = len; + iv[2].iov_base = crc; + iv[2].iov_len = 1; + + memset(&msg, 0, sizeof(msg)); + + return kernel_sendmsg(sock, &msg, iv, 3, 6 + len); +} + +static int rfcomm_send_credits(struct rfcomm_session *s, u8 addr, u8 credits) +{ + struct rfcomm_hdr *hdr; + u8 buf[16], *ptr = buf; + + BT_DBG("%p addr %d credits %d", s, addr, credits); + + hdr = (void *) ptr; ptr += sizeof(*hdr); + hdr->addr = addr; + hdr->ctrl = __ctrl(RFCOMM_UIH, 1); + hdr->len = __len8(0); + + *ptr = credits; ptr++; + + *ptr = __fcs(buf); ptr++; + + return rfcomm_send_frame(s, buf, ptr - buf); +} + +static void rfcomm_make_uih(struct sk_buff *skb, u8 addr) +{ + struct rfcomm_hdr *hdr; + int len = skb->len; + u8 *crc; + + if (len > 127) { + hdr = (void *) skb_push(skb, 4); + put_unaligned(cpu_to_le16(__len16(len)), (__le16 *) &hdr->len); + } else { + hdr = (void *) skb_push(skb, 3); + hdr->len = __len8(len); + } + hdr->addr = addr; + hdr->ctrl = __ctrl(RFCOMM_UIH, 0); + + crc = skb_put(skb, 1); + *crc = __fcs((void *) hdr); +} + +/* ---- RFCOMM frame reception ---- */ +static int rfcomm_recv_ua(struct rfcomm_session *s, u8 dlci) +{ + BT_DBG("session %p state %ld dlci %d", s, s->state, dlci); + + if (dlci) { + /* Data channel */ + struct rfcomm_dlc *d = rfcomm_dlc_get(s, dlci); + if (!d) { + rfcomm_send_dm(s, dlci); + return 0; + } + + switch (d->state) { + case BT_CONNECT: + rfcomm_dlc_clear_timer(d); + + rfcomm_dlc_lock(d); + d->state = BT_CONNECTED; + d->state_change(d, 0); + rfcomm_dlc_unlock(d); + + rfcomm_send_msc(s, 1, dlci, d->v24_sig); + break; + + case BT_DISCONN: + d->state = BT_CLOSED; + __rfcomm_dlc_close(d, 0); + + if (list_empty(&s->dlcs)) { + s->state = BT_DISCONN; + rfcomm_send_disc(s, 0); + rfcomm_session_clear_timer(s); + } + + break; + } + } else { + /* Control channel */ + switch (s->state) { + case BT_CONNECT: + s->state = BT_CONNECTED; + rfcomm_process_connect(s); + break; + + case BT_DISCONN: + /* rfcomm_session_put is called later so don't do + * anything here otherwise we will mess up the session + * reference counter: + * + * (a) when we are the initiator dlc_unlink will drive + * the reference counter to 0 (there is no initial put + * after session_add) + * + * (b) when we are not the initiator rfcomm_rx_process + * will explicitly call put to balance the initial hold + * done after session add. + */ + break; + } + } + return 0; +} + +static int rfcomm_recv_dm(struct rfcomm_session *s, u8 dlci) +{ + int err = 0; + + BT_DBG("session %p state %ld dlci %d", s, s->state, dlci); + + if (dlci) { + /* Data DLC */ + struct rfcomm_dlc *d = rfcomm_dlc_get(s, dlci); + if (d) { + if (d->state == BT_CONNECT || d->state == BT_CONFIG) + err = ECONNREFUSED; + else + err = ECONNRESET; + + d->state = BT_CLOSED; + __rfcomm_dlc_close(d, err); + } + } else { + if (s->state == BT_CONNECT) + err = ECONNREFUSED; + else + err = ECONNRESET; + + s->state = BT_CLOSED; + rfcomm_session_close(s, err); + } + return 0; +} + +static int rfcomm_recv_disc(struct rfcomm_session *s, u8 dlci) +{ + int err = 0; + + BT_DBG("session %p state %ld dlci %d", s, s->state, dlci); + + if (dlci) { + struct rfcomm_dlc *d = rfcomm_dlc_get(s, dlci); + if (d) { + rfcomm_send_ua(s, dlci); + + if (d->state == BT_CONNECT || d->state == BT_CONFIG) + err = ECONNREFUSED; + else + err = ECONNRESET; + + d->state = BT_CLOSED; + __rfcomm_dlc_close(d, err); + } else + rfcomm_send_dm(s, dlci); + + } else { + rfcomm_send_ua(s, 0); + + if (s->state == BT_CONNECT) + err = ECONNREFUSED; + else + err = ECONNRESET; + + s->state = BT_CLOSED; + rfcomm_session_close(s, err); + } + + return 0; +} + +void rfcomm_dlc_accept(struct rfcomm_dlc *d) +{ + struct sock *sk = d->session->sock->sk; + struct l2cap_conn *conn = l2cap_pi(sk)->chan->conn; + + BT_DBG("dlc %p", d); + + rfcomm_send_ua(d->session, d->dlci); + + rfcomm_dlc_clear_timer(d); + + rfcomm_dlc_lock(d); + d->state = BT_CONNECTED; + d->state_change(d, 0); + rfcomm_dlc_unlock(d); + + if (d->role_switch) + hci_conn_switch_role(conn->hcon, 0x00); + + rfcomm_send_msc(d->session, 1, d->dlci, d->v24_sig); +} + +static void rfcomm_check_accept(struct rfcomm_dlc *d) +{ + if (rfcomm_check_security(d)) { + if (d->defer_setup) { + set_bit(RFCOMM_DEFER_SETUP, &d->flags); + rfcomm_dlc_set_timer(d, RFCOMM_AUTH_TIMEOUT); + + rfcomm_dlc_lock(d); + d->state = BT_CONNECT2; + d->state_change(d, 0); + rfcomm_dlc_unlock(d); + } else + rfcomm_dlc_accept(d); + } else { + set_bit(RFCOMM_AUTH_PENDING, &d->flags); + rfcomm_dlc_set_timer(d, RFCOMM_AUTH_TIMEOUT); + } +} + +static int rfcomm_recv_sabm(struct rfcomm_session *s, u8 dlci) +{ + struct rfcomm_dlc *d; + u8 channel; + + BT_DBG("session %p state %ld dlci %d", s, s->state, dlci); + + if (!dlci) { + rfcomm_send_ua(s, 0); + + if (s->state == BT_OPEN) { + s->state = BT_CONNECTED; + rfcomm_process_connect(s); + } + return 0; + } + + /* Check if DLC exists */ + d = rfcomm_dlc_get(s, dlci); + if (d) { + if (d->state == BT_OPEN) { + /* DLC was previously opened by PN request */ + rfcomm_check_accept(d); + } + return 0; + } + + /* Notify socket layer about incoming connection */ + channel = __srv_channel(dlci); + if (rfcomm_connect_ind(s, channel, &d)) { + d->dlci = dlci; + d->addr = __addr(s->initiator, dlci); + rfcomm_dlc_link(s, d); + + rfcomm_check_accept(d); + } else { + rfcomm_send_dm(s, dlci); + } + + return 0; +} + +static int rfcomm_apply_pn(struct rfcomm_dlc *d, int cr, struct rfcomm_pn *pn) +{ + struct rfcomm_session *s = d->session; + + BT_DBG("dlc %p state %ld dlci %d mtu %d fc 0x%x credits %d", + d, d->state, d->dlci, pn->mtu, pn->flow_ctrl, pn->credits); + + if ((pn->flow_ctrl == 0xf0 && s->cfc != RFCOMM_CFC_DISABLED) || + pn->flow_ctrl == 0xe0) { + d->cfc = RFCOMM_CFC_ENABLED; + d->tx_credits = pn->credits; + } else { + d->cfc = RFCOMM_CFC_DISABLED; + set_bit(RFCOMM_TX_THROTTLED, &d->flags); + } + + if (s->cfc == RFCOMM_CFC_UNKNOWN) + s->cfc = d->cfc; + + d->priority = pn->priority; + + d->mtu = __le16_to_cpu(pn->mtu); + + if (cr && d->mtu > s->mtu) + d->mtu = s->mtu; + + return 0; +} + +static int rfcomm_recv_pn(struct rfcomm_session *s, int cr, struct sk_buff *skb) +{ + struct rfcomm_pn *pn = (void *) skb->data; + struct rfcomm_dlc *d; + u8 dlci = pn->dlci; + + BT_DBG("session %p state %ld dlci %d", s, s->state, dlci); + + if (!dlci) + return 0; + + d = rfcomm_dlc_get(s, dlci); + if (d) { + if (cr) { + /* PN request */ + rfcomm_apply_pn(d, cr, pn); + rfcomm_send_pn(s, 0, d); + } else { + /* PN response */ + switch (d->state) { + case BT_CONFIG: + rfcomm_apply_pn(d, cr, pn); + + d->state = BT_CONNECT; + rfcomm_send_sabm(s, d->dlci); + break; + } + } + } else { + u8 channel = __srv_channel(dlci); + + if (!cr) + return 0; + + /* PN request for non existing DLC. + * Assume incoming connection. */ + if (rfcomm_connect_ind(s, channel, &d)) { + d->dlci = dlci; + d->addr = __addr(s->initiator, dlci); + rfcomm_dlc_link(s, d); + + rfcomm_apply_pn(d, cr, pn); + + d->state = BT_OPEN; + rfcomm_send_pn(s, 0, d); + } else { + rfcomm_send_dm(s, dlci); + } + } + return 0; +} + +static int rfcomm_recv_rpn(struct rfcomm_session *s, int cr, int len, struct sk_buff *skb) +{ + struct rfcomm_rpn *rpn = (void *) skb->data; + u8 dlci = __get_dlci(rpn->dlci); + + u8 bit_rate = 0; + u8 data_bits = 0; + u8 stop_bits = 0; + u8 parity = 0; + u8 flow_ctrl = 0; + u8 xon_char = 0; + u8 xoff_char = 0; + u16 rpn_mask = RFCOMM_RPN_PM_ALL; + + BT_DBG("dlci %d cr %d len 0x%x bitr 0x%x line 0x%x flow 0x%x xonc 0x%x xoffc 0x%x pm 0x%x", + dlci, cr, len, rpn->bit_rate, rpn->line_settings, rpn->flow_ctrl, + rpn->xon_char, rpn->xoff_char, rpn->param_mask); + + if (!cr) + return 0; + + if (len == 1) { + /* This is a request, return default (according to ETSI TS 07.10) settings */ + bit_rate = RFCOMM_RPN_BR_9600; + data_bits = RFCOMM_RPN_DATA_8; + stop_bits = RFCOMM_RPN_STOP_1; + parity = RFCOMM_RPN_PARITY_NONE; + flow_ctrl = RFCOMM_RPN_FLOW_NONE; + xon_char = RFCOMM_RPN_XON_CHAR; + xoff_char = RFCOMM_RPN_XOFF_CHAR; + goto rpn_out; + } + + /* Check for sane values, ignore/accept bit_rate, 8 bits, 1 stop bit, + * no parity, no flow control lines, normal XON/XOFF chars */ + + if (rpn->param_mask & cpu_to_le16(RFCOMM_RPN_PM_BITRATE)) { + bit_rate = rpn->bit_rate; + if (bit_rate > RFCOMM_RPN_BR_230400) { + BT_DBG("RPN bit rate mismatch 0x%x", bit_rate); + bit_rate = RFCOMM_RPN_BR_9600; + rpn_mask ^= RFCOMM_RPN_PM_BITRATE; + } + } + + if (rpn->param_mask & cpu_to_le16(RFCOMM_RPN_PM_DATA)) { + data_bits = __get_rpn_data_bits(rpn->line_settings); + if (data_bits != RFCOMM_RPN_DATA_8) { + BT_DBG("RPN data bits mismatch 0x%x", data_bits); + data_bits = RFCOMM_RPN_DATA_8; + rpn_mask ^= RFCOMM_RPN_PM_DATA; + } + } + + if (rpn->param_mask & cpu_to_le16(RFCOMM_RPN_PM_STOP)) { + stop_bits = __get_rpn_stop_bits(rpn->line_settings); + if (stop_bits != RFCOMM_RPN_STOP_1) { + BT_DBG("RPN stop bits mismatch 0x%x", stop_bits); + stop_bits = RFCOMM_RPN_STOP_1; + rpn_mask ^= RFCOMM_RPN_PM_STOP; + } + } + + if (rpn->param_mask & cpu_to_le16(RFCOMM_RPN_PM_PARITY)) { + parity = __get_rpn_parity(rpn->line_settings); + if (parity != RFCOMM_RPN_PARITY_NONE) { + BT_DBG("RPN parity mismatch 0x%x", parity); + parity = RFCOMM_RPN_PARITY_NONE; + rpn_mask ^= RFCOMM_RPN_PM_PARITY; + } + } + + if (rpn->param_mask & cpu_to_le16(RFCOMM_RPN_PM_FLOW)) { + flow_ctrl = rpn->flow_ctrl; + if (flow_ctrl != RFCOMM_RPN_FLOW_NONE) { + BT_DBG("RPN flow ctrl mismatch 0x%x", flow_ctrl); + flow_ctrl = RFCOMM_RPN_FLOW_NONE; + rpn_mask ^= RFCOMM_RPN_PM_FLOW; + } + } + + if (rpn->param_mask & cpu_to_le16(RFCOMM_RPN_PM_XON)) { + xon_char = rpn->xon_char; + if (xon_char != RFCOMM_RPN_XON_CHAR) { + BT_DBG("RPN XON char mismatch 0x%x", xon_char); + xon_char = RFCOMM_RPN_XON_CHAR; + rpn_mask ^= RFCOMM_RPN_PM_XON; + } + } + + if (rpn->param_mask & cpu_to_le16(RFCOMM_RPN_PM_XOFF)) { + xoff_char = rpn->xoff_char; + if (xoff_char != RFCOMM_RPN_XOFF_CHAR) { + BT_DBG("RPN XOFF char mismatch 0x%x", xoff_char); + xoff_char = RFCOMM_RPN_XOFF_CHAR; + rpn_mask ^= RFCOMM_RPN_PM_XOFF; + } + } + +rpn_out: + rfcomm_send_rpn(s, 0, dlci, bit_rate, data_bits, stop_bits, + parity, flow_ctrl, xon_char, xoff_char, rpn_mask); + + return 0; +} + +static int rfcomm_recv_rls(struct rfcomm_session *s, int cr, struct sk_buff *skb) +{ + struct rfcomm_rls *rls = (void *) skb->data; + u8 dlci = __get_dlci(rls->dlci); + + BT_DBG("dlci %d cr %d status 0x%x", dlci, cr, rls->status); + + if (!cr) + return 0; + + /* We should probably do something with this information here. But + * for now it's sufficient just to reply -- Bluetooth 1.1 says it's + * mandatory to recognise and respond to RLS */ + + rfcomm_send_rls(s, 0, dlci, rls->status); + + return 0; +} + +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); + + d->remote_v24_sig = msc->v24_sig; + + 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_pointer(skb); + + 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", 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; + if (rfcomm_check_security(d)) { + rfcomm_send_pn(s, 1, d); + } else { + set_bit(RFCOMM_AUTH_PENDING, &d->flags); + rfcomm_dlc_set_timer(d, RFCOMM_AUTH_TIMEOUT); + } + } + } +} + +/* 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_ENC_DROP, &d->flags)) { + __rfcomm_dlc_close(d, ECONNREFUSED); + continue; + } + + if (test_and_clear_bit(RFCOMM_AUTH_ACCEPT, &d->flags)) { + rfcomm_dlc_clear_timer(d); + if (d->out) { + rfcomm_send_pn(s, 1, d); + rfcomm_dlc_set_timer(d, RFCOMM_CONN_TIMEOUT); + } else { + if (d->defer_setup) { + set_bit(RFCOMM_DEFER_SETUP, &d->flags); + rfcomm_dlc_set_timer(d, RFCOMM_AUTH_TIMEOUT); + + rfcomm_dlc_lock(d); + d->state = BT_CONNECT2; + d->state_change(d, 0); + rfcomm_dlc_unlock(d); + } else + rfcomm_dlc_accept(d); + } + continue; + } else if (test_and_clear_bit(RFCOMM_AUTH_REJECT, &d->flags)) { + rfcomm_dlc_clear_timer(d); + if (!d->out) + rfcomm_send_dm(s, d->dlci); + else + d->state = BT_CLOSED; + __rfcomm_dlc_close(d, ECONNREFUSED); + continue; + } + + if (test_bit(RFCOMM_SEC_PENDING, &d->flags)) + 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); + if (!skb_linearize(skb)) + rfcomm_recv_frame(s, skb); + else + kfree_skb(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); + + err = kernel_accept(sock, &nsock, O_NONBLOCK); + if (err < 0) + 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); + + /* We should adjust MTU on incoming sessions. + * L2CAP MTU minus UIH header and FCS. */ + s->mtu = min(l2cap_pi(nsock->sk)->chan->omtu, + l2cap_pi(nsock->sk)->chan->imtu) - 5; + + rfcomm_schedule(); + } 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)->chan->omtu, l2cap_pi(sk)->chan->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 (test_and_clear_bit(RFCOMM_TIMED_OUT, &s->flags)) { + s->state = BT_DISCONN; + rfcomm_send_disc(s, 0); + rfcomm_session_put(s); + continue; + } + + 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 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 = cpu_to_le16(RFCOMM_PSM); + addr.l2_cid = 0; + err = kernel_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)->chan->imtu = l2cap_mtu; + release_sock(sk); + + /* Start listening on the socket */ + err = kernel_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) +{ + BT_DBG(""); + + set_user_nice(current, -10); + + rfcomm_add_listener(BDADDR_ANY); + + while (1) { + set_current_state(TASK_INTERRUPTIBLE); + + if (kthread_should_stop()) + break; + + /* Process stuff */ + rfcomm_process_sessions(); + + schedule(); + } + __set_current_state(TASK_RUNNING); + + rfcomm_kill_listener(); + + return 0; +} + +static void rfcomm_security_cfm(struct hci_conn *conn, u8 status, u8 encrypt) +{ + struct rfcomm_session *s; + struct rfcomm_dlc *d; + struct list_head *p, *n; + + BT_DBG("conn %p status 0x%02x encrypt 0x%02x", conn, status, encrypt); + + s = rfcomm_session_get(&conn->hdev->bdaddr, &conn->dst); + if (!s) + return; + + rfcomm_session_hold(s); + + list_for_each_safe(p, n, &s->dlcs) { + d = list_entry(p, struct rfcomm_dlc, list); + + if (test_and_clear_bit(RFCOMM_SEC_PENDING, &d->flags)) { + rfcomm_dlc_clear_timer(d); + if (status || encrypt == 0x00) { + set_bit(RFCOMM_ENC_DROP, &d->flags); + continue; + } + } + + if (d->state == BT_CONNECTED && !status && encrypt == 0x00) { + if (d->sec_level == BT_SECURITY_MEDIUM) { + set_bit(RFCOMM_SEC_PENDING, &d->flags); + rfcomm_dlc_set_timer(d, RFCOMM_AUTH_TIMEOUT); + continue; + } else if (d->sec_level == BT_SECURITY_HIGH) { + set_bit(RFCOMM_ENC_DROP, &d->flags); + continue; + } + } + + if (!test_and_clear_bit(RFCOMM_AUTH_PENDING, &d->flags)) + continue; + + if (!status && hci_conn_check_secure(conn, d->sec_level)) + set_bit(RFCOMM_AUTH_ACCEPT, &d->flags); + else + set_bit(RFCOMM_AUTH_REJECT, &d->flags); + } + + rfcomm_session_put(s); + + rfcomm_schedule(); +} + +static struct hci_cb rfcomm_cb = { + .name = "RFCOMM", + .security_cfm = rfcomm_security_cfm +}; + +static int rfcomm_dlc_debugfs_show(struct seq_file *f, void *x) +{ + struct rfcomm_session *s; + + rfcomm_lock(); + + list_for_each_entry(s, &session_list, list) { + struct rfcomm_dlc *d; + list_for_each_entry(d, &s->dlcs, list) { + struct sock *sk = s->sock->sk; + + seq_printf(f, "%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); + } + } + + rfcomm_unlock(); + + return 0; +} + +static int rfcomm_dlc_debugfs_open(struct inode *inode, struct file *file) +{ + return single_open(file, rfcomm_dlc_debugfs_show, inode->i_private); +} + +static const struct file_operations rfcomm_dlc_debugfs_fops = { + .open = rfcomm_dlc_debugfs_open, + .read = seq_read, + .llseek = seq_lseek, + .release = single_release, +}; + +static struct dentry *rfcomm_dlc_debugfs; + +/* ---- Initialization ---- */ +static int __init rfcomm_init(void) +{ + int err; + + hci_register_cb(&rfcomm_cb); + + rfcomm_thread = kthread_run(rfcomm_run, NULL, "krfcommd"); + if (IS_ERR(rfcomm_thread)) { + err = PTR_ERR(rfcomm_thread); + goto unregister; + } + + if (bt_debugfs) { + rfcomm_dlc_debugfs = debugfs_create_file("rfcomm_dlc", 0444, + bt_debugfs, NULL, &rfcomm_dlc_debugfs_fops); + if (!rfcomm_dlc_debugfs) + BT_ERR("Failed to create RFCOMM debug file"); + } + + err = rfcomm_init_ttys(); + if (err < 0) + goto stop; + + err = rfcomm_init_sockets(); + if (err < 0) + goto cleanup; + + BT_INFO("RFCOMM ver %s", VERSION); + + return 0; + +cleanup: + rfcomm_cleanup_ttys(); + +stop: + kthread_stop(rfcomm_thread); + +unregister: + hci_unregister_cb(&rfcomm_cb); + + return err; +} + +static void __exit rfcomm_exit(void) +{ + debugfs_remove(rfcomm_dlc_debugfs); + + hci_unregister_cb(&rfcomm_cb); + + kthread_stop(rfcomm_thread); + + rfcomm_cleanup_ttys(); + + rfcomm_cleanup_sockets(); +} + +module_init(rfcomm_init); +module_exit(rfcomm_exit); + +module_param(disable_cfc, bool, 0644); +MODULE_PARM_DESC(disable_cfc, "Disable credit based flow control"); + +module_param(channel_mtu, int, 0644); +MODULE_PARM_DESC(channel_mtu, "Default MTU for the RFCOMM channel"); + +module_param(l2cap_mtu, uint, 0644); +MODULE_PARM_DESC(l2cap_mtu, "Default MTU for the L2CAP connection"); + +module_param(l2cap_ertm, bool, 0644); +MODULE_PARM_DESC(l2cap_ertm, "Use L2CAP ERTM mode for connection"); + +MODULE_AUTHOR("Marcel Holtmann <marcel@holtmann.org>"); +MODULE_DESCRIPTION("Bluetooth RFCOMM ver " VERSION); +MODULE_VERSION(VERSION); +MODULE_LICENSE("GPL"); +MODULE_ALIAS("bt-proto-3"); diff --git a/net/bluetooth_tizen/rfcomm/sock.c b/net/bluetooth_tizen/rfcomm/sock.c new file mode 100644 index 0000000..22169c3 --- /dev/null +++ b/net/bluetooth_tizen/rfcomm/sock.c @@ -0,0 +1,1078 @@ +/* + RFCOMM implementation for Linux Bluetooth stack (BlueZ). + Copyright (C) 2002 Maxim Krasnyansky <maxk@qualcomm.com> + Copyright (C) 2002 Marcel Holtmann <marcel@holtmann.org> + + This program is free software; you can redistribute it and/or modify + it under the terms of the GNU General Public License version 2 as + published by the Free Software Foundation; + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS + OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT OF THIRD PARTY RIGHTS. + IN NO EVENT SHALL THE COPYRIGHT HOLDER(S) AND AUTHOR(S) BE LIABLE FOR ANY + CLAIM, OR ANY SPECIAL INDIRECT OR CONSEQUENTIAL DAMAGES, OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + + ALL LIABILITY, INCLUDING LIABILITY FOR INFRINGEMENT OF ANY PATENTS, + COPYRIGHTS, TRADEMARKS OR OTHER RIGHTS, RELATING TO USE OF THIS + SOFTWARE IS DISCLAIMED. +*/ + +/* + * RFCOMM sockets. + */ + +#include <linux/module.h> + +#include <linux/types.h> +#include <linux/errno.h> +#include <linux/kernel.h> +#include <linux/sched.h> +#include <linux/slab.h> +#include <linux/poll.h> +#include <linux/fcntl.h> +#include <linux/init.h> +#include <linux/interrupt.h> +#include <linux/socket.h> +#include <linux/skbuff.h> +#include <linux/list.h> +#include <linux/device.h> +#include <linux/debugfs.h> +#include <linux/seq_file.h> +#include <linux/security.h> +#include <net/sock.h> + +#include <asm/system.h> +#include <linux/uaccess.h> + +#include <net/bluetooth/bluetooth.h> +#include <net/bluetooth/hci_core.h> +#include <net/bluetooth/l2cap.h> +#include <net/bluetooth/rfcomm.h> + +static const struct proto_ops rfcomm_sock_ops; + +static struct bt_sock_list rfcomm_sk_list = { + .lock = __RW_LOCK_UNLOCKED(rfcomm_sk_list.lock) +}; + +static void rfcomm_sock_close(struct sock *sk); +static void rfcomm_sock_kill(struct sock *sk); + +/* ---- DLC callbacks ---- + * + * called under rfcomm_dlc_lock() + */ +static void rfcomm_sk_data_ready(struct rfcomm_dlc *d, struct sk_buff *skb) +{ + struct sock *sk = d->owner; + if (!sk) + return; + + atomic_add(skb->len, &sk->sk_rmem_alloc); + skb_queue_tail(&sk->sk_receive_queue, skb); + sk->sk_data_ready(sk, skb->len); + + if (atomic_read(&sk->sk_rmem_alloc) >= sk->sk_rcvbuf) + rfcomm_dlc_throttle(d); +} + +static void rfcomm_sk_state_change(struct rfcomm_dlc *d, int err) +{ + struct sock *sk = d->owner, *parent; + unsigned long flags; + + if (!sk) + return; + + BT_DBG("dlc %p state %ld err %d", d, d->state, err); + + local_irq_save(flags); + bh_lock_sock(sk); + + if (err) + sk->sk_err = err; + + sk->sk_state = d->state; + + parent = bt_sk(sk)->parent; + if (parent) { + if (d->state == BT_CLOSED) { + sock_set_flag(sk, SOCK_ZAPPED); + bt_accept_unlink(sk); + } + parent->sk_data_ready(parent, 0); + } else { + if (d->state == BT_CONNECTED) + rfcomm_session_getaddr(d->session, &bt_sk(sk)->src, NULL); + sk->sk_state_change(sk); + } + + bh_unlock_sock(sk); + local_irq_restore(flags); + + if (parent && sock_flag(sk, SOCK_ZAPPED)) { + /* We have to drop DLC lock here, otherwise + * rfcomm_sock_destruct() will dead lock. */ + rfcomm_dlc_unlock(d); + rfcomm_sock_kill(sk); + rfcomm_dlc_lock(d); + } +} + +/* ---- Socket functions ---- */ +static struct sock *__rfcomm_get_sock_by_addr(u8 channel, bdaddr_t *src) +{ + struct sock *sk = NULL; + struct hlist_node *node; + + sk_for_each(sk, node, &rfcomm_sk_list.head) { + if (rfcomm_pi(sk)->channel == channel && + !bacmp(&bt_sk(sk)->src, src)) + break; + } + + return node ? sk : NULL; +} + +/* Find socket with channel and source bdaddr. + * Returns closest match. + */ +static struct sock *rfcomm_get_sock_by_channel(int state, u8 channel, bdaddr_t *src) +{ + struct sock *sk = NULL, *sk1 = NULL; + struct hlist_node *node; + + read_lock(&rfcomm_sk_list.lock); + + sk_for_each(sk, node, &rfcomm_sk_list.head) { + if (state && sk->sk_state != state) + continue; + + if (rfcomm_pi(sk)->channel == channel) { + /* Exact match. */ + if (!bacmp(&bt_sk(sk)->src, src)) + break; + + /* Closest match */ + if (!bacmp(&bt_sk(sk)->src, BDADDR_ANY)) + sk1 = sk; + } + } + + read_unlock(&rfcomm_sk_list.lock); + + return node ? sk : sk1; +} + +static void rfcomm_sock_destruct(struct sock *sk) +{ + struct rfcomm_dlc *d = rfcomm_pi(sk)->dlc; + + BT_DBG("sk %p dlc %p", sk, d); + + skb_queue_purge(&sk->sk_receive_queue); + skb_queue_purge(&sk->sk_write_queue); + + rfcomm_dlc_lock(d); + rfcomm_pi(sk)->dlc = NULL; + + /* Detach DLC if it's owned by this socket */ + if (d->owner == sk) + d->owner = NULL; + rfcomm_dlc_unlock(d); + + rfcomm_dlc_put(d); +} + +static void rfcomm_sock_cleanup_listen(struct sock *parent) +{ + struct sock *sk; + + BT_DBG("parent %p", parent); + + /* Close not yet accepted dlcs */ + while ((sk = bt_accept_dequeue(parent, NULL))) { + rfcomm_sock_close(sk); + rfcomm_sock_kill(sk); + } + + parent->sk_state = BT_CLOSED; + sock_set_flag(parent, SOCK_ZAPPED); +} + +/* Kill socket (only if zapped and orphan) + * Must be called on unlocked socket. + */ +static void rfcomm_sock_kill(struct sock *sk) +{ + if (!sock_flag(sk, SOCK_ZAPPED) || sk->sk_socket) + return; + + BT_DBG("sk %p state %d refcnt %d", sk, sk->sk_state, atomic_read(&sk->sk_refcnt)); + + /* Kill poor orphan */ + bt_sock_unlink(&rfcomm_sk_list, sk); + sock_set_flag(sk, SOCK_DEAD); + sock_put(sk); +} + +static void __rfcomm_sock_close(struct sock *sk) +{ + struct rfcomm_dlc *d = rfcomm_pi(sk)->dlc; + + BT_DBG("sk %p state %d socket %p", sk, sk->sk_state, sk->sk_socket); + + switch (sk->sk_state) { + case BT_LISTEN: + rfcomm_sock_cleanup_listen(sk); + break; + + case BT_CONNECT: + case BT_CONNECT2: + case BT_CONFIG: + case BT_CONNECTED: + rfcomm_dlc_close(d, 0); + + default: + sock_set_flag(sk, SOCK_ZAPPED); + break; + } +} + +/* Close socket. + * Must be called on unlocked socket. + */ +static void rfcomm_sock_close(struct sock *sk) +{ + lock_sock(sk); + __rfcomm_sock_close(sk); + release_sock(sk); +} + +static void rfcomm_sock_init(struct sock *sk, struct sock *parent) +{ + struct rfcomm_pinfo *pi = rfcomm_pi(sk); + + BT_DBG("sk %p", sk); + + if (parent) { + sk->sk_type = parent->sk_type; + pi->dlc->defer_setup = bt_sk(parent)->defer_setup; + + pi->sec_level = rfcomm_pi(parent)->sec_level; + pi->role_switch = rfcomm_pi(parent)->role_switch; + + security_sk_clone(parent, sk); + } else { + pi->dlc->defer_setup = 0; + + pi->sec_level = BT_SECURITY_LOW; + pi->role_switch = 0; + } + + pi->dlc->sec_level = pi->sec_level; + pi->dlc->role_switch = pi->role_switch; +} + +static struct proto rfcomm_proto = { + .name = "RFCOMM", + .owner = THIS_MODULE, + .obj_size = sizeof(struct rfcomm_pinfo) +}; + +static struct sock *rfcomm_sock_alloc(struct net *net, struct socket *sock, int proto, gfp_t prio) +{ + struct rfcomm_dlc *d; + struct sock *sk; + + sk = sk_alloc(net, PF_BLUETOOTH, prio, &rfcomm_proto); + if (!sk) + return NULL; + + sock_init_data(sock, sk); + INIT_LIST_HEAD(&bt_sk(sk)->accept_q); + + d = rfcomm_dlc_alloc(prio); + if (!d) { + sk_free(sk); + return NULL; + } + + d->data_ready = rfcomm_sk_data_ready; + d->state_change = rfcomm_sk_state_change; + + rfcomm_pi(sk)->dlc = d; + d->owner = sk; + + sk->sk_destruct = rfcomm_sock_destruct; + sk->sk_sndtimeo = RFCOMM_CONN_TIMEOUT; + + sk->sk_sndbuf = RFCOMM_MAX_CREDITS * RFCOMM_DEFAULT_MTU * 10; + sk->sk_rcvbuf = RFCOMM_MAX_CREDITS * RFCOMM_DEFAULT_MTU * 10; + + sock_reset_flag(sk, SOCK_ZAPPED); + + sk->sk_protocol = proto; + sk->sk_state = BT_OPEN; + + bt_sock_link(&rfcomm_sk_list, sk); + + BT_DBG("sk %p", sk); + return sk; +} + +static int rfcomm_sock_create(struct net *net, struct socket *sock, + int protocol, int kern) +{ + struct sock *sk; + + BT_DBG("sock %p", sock); + + sock->state = SS_UNCONNECTED; + + if (sock->type != SOCK_STREAM && sock->type != SOCK_RAW) + return -ESOCKTNOSUPPORT; + + sock->ops = &rfcomm_sock_ops; + + sk = rfcomm_sock_alloc(net, sock, protocol, GFP_ATOMIC); + if (!sk) + return -ENOMEM; + + rfcomm_sock_init(sk, NULL); + return 0; +} + +static int rfcomm_sock_bind(struct socket *sock, struct sockaddr *addr, int addr_len) +{ + struct sockaddr_rc *sa = (struct sockaddr_rc *) addr; + struct sock *sk = sock->sk; + int err = 0; + + BT_DBG("sk %p %s", sk, batostr(&sa->rc_bdaddr)); + + if (!addr || addr->sa_family != AF_BLUETOOTH) + return -EINVAL; + + lock_sock(sk); + + if (sk->sk_state != BT_OPEN) { + err = -EBADFD; + goto done; + } + + if (sk->sk_type != SOCK_STREAM) { + err = -EINVAL; + goto done; + } + + write_lock(&rfcomm_sk_list.lock); + + if (sa->rc_channel && __rfcomm_get_sock_by_addr(sa->rc_channel, &sa->rc_bdaddr)) { + err = -EADDRINUSE; + } else { + /* Save source address */ + bacpy(&bt_sk(sk)->src, &sa->rc_bdaddr); + rfcomm_pi(sk)->channel = sa->rc_channel; + sk->sk_state = BT_BOUND; + } + + write_unlock(&rfcomm_sk_list.lock); + +done: + release_sock(sk); + return err; +} + +static int rfcomm_sock_connect(struct socket *sock, struct sockaddr *addr, int alen, int flags) +{ + struct sockaddr_rc *sa = (struct sockaddr_rc *) addr; + struct sock *sk = sock->sk; + struct rfcomm_dlc *d = rfcomm_pi(sk)->dlc; + int err = 0; + + BT_DBG("sk %p", sk); + + if (alen < sizeof(struct sockaddr_rc) || + addr->sa_family != AF_BLUETOOTH) + return -EINVAL; + + lock_sock(sk); + + if (sk->sk_state != BT_OPEN && sk->sk_state != BT_BOUND) { + err = -EBADFD; + goto done; + } + + if (sk->sk_type != SOCK_STREAM) { + err = -EINVAL; + goto done; + } + + sk->sk_state = BT_CONNECT; + bacpy(&bt_sk(sk)->dst, &sa->rc_bdaddr); + rfcomm_pi(sk)->channel = sa->rc_channel; + + d->sec_level = rfcomm_pi(sk)->sec_level; + d->role_switch = rfcomm_pi(sk)->role_switch; + + err = rfcomm_dlc_open(d, &bt_sk(sk)->src, &sa->rc_bdaddr, sa->rc_channel); + if (!err) + err = bt_sock_wait_state(sk, BT_CONNECTED, + sock_sndtimeo(sk, flags & O_NONBLOCK)); + +done: + release_sock(sk); + return err; +} + +static int rfcomm_sock_listen(struct socket *sock, int backlog) +{ + struct sock *sk = sock->sk; + int err = 0; + + BT_DBG("sk %p backlog %d", sk, backlog); + + lock_sock(sk); + + if (sk->sk_state != BT_BOUND) { + err = -EBADFD; + goto done; + } + + if (sk->sk_type != SOCK_STREAM) { + err = -EINVAL; + goto done; + } + + if (!rfcomm_pi(sk)->channel) { + bdaddr_t *src = &bt_sk(sk)->src; + u8 channel; + + err = -EINVAL; + + write_lock(&rfcomm_sk_list.lock); + + for (channel = 1; channel < 31; channel++) + if (!__rfcomm_get_sock_by_addr(channel, src)) { + rfcomm_pi(sk)->channel = channel; + err = 0; + break; + } + + write_unlock(&rfcomm_sk_list.lock); + + if (err < 0) + goto done; + } + + sk->sk_max_ack_backlog = backlog; + sk->sk_ack_backlog = 0; + sk->sk_state = BT_LISTEN; + +done: + release_sock(sk); + return err; +} + +static int rfcomm_sock_accept(struct socket *sock, struct socket *newsock, int flags) +{ + DECLARE_WAITQUEUE(wait, current); + struct sock *sk = sock->sk, *nsk; + long timeo; + int err = 0; + + lock_sock(sk); + + if (sk->sk_type != SOCK_STREAM) { + err = -EINVAL; + goto done; + } + + timeo = sock_rcvtimeo(sk, flags & O_NONBLOCK); + + BT_DBG("sk %p timeo %ld", sk, timeo); + + /* Wait for an incoming connection. (wake-one). */ + add_wait_queue_exclusive(sk_sleep(sk), &wait); + while (1) { + set_current_state(TASK_INTERRUPTIBLE); + + if (sk->sk_state != BT_LISTEN) { + err = -EBADFD; + break; + } + + nsk = bt_accept_dequeue(sk, newsock); + if (nsk) + break; + + if (!timeo) { + err = -EAGAIN; + break; + } + + if (signal_pending(current)) { + err = sock_intr_errno(timeo); + break; + } + + release_sock(sk); + timeo = schedule_timeout(timeo); + lock_sock(sk); + } + __set_current_state(TASK_RUNNING); + remove_wait_queue(sk_sleep(sk), &wait); + + if (err) + goto done; + + newsock->state = SS_CONNECTED; + + BT_DBG("new socket %p", nsk); + +done: + release_sock(sk); + return err; +} + +static int rfcomm_sock_getname(struct socket *sock, struct sockaddr *addr, int *len, int peer) +{ + struct sockaddr_rc *sa = (struct sockaddr_rc *) addr; + struct sock *sk = sock->sk; + + BT_DBG("sock %p, sk %p", sock, sk); + + sa->rc_family = AF_BLUETOOTH; + sa->rc_channel = rfcomm_pi(sk)->channel; + if (peer) + bacpy(&sa->rc_bdaddr, &bt_sk(sk)->dst); + else + bacpy(&sa->rc_bdaddr, &bt_sk(sk)->src); + + *len = sizeof(struct sockaddr_rc); + return 0; +} + +static int rfcomm_sock_sendmsg(struct kiocb *iocb, struct socket *sock, + struct msghdr *msg, size_t len) +{ + struct sock *sk = sock->sk; + struct rfcomm_dlc *d = rfcomm_pi(sk)->dlc; + struct sk_buff *skb; + int sent = 0; + + if (test_bit(RFCOMM_DEFER_SETUP, &d->flags)) + return -ENOTCONN; + + if (msg->msg_flags & MSG_OOB) + return -EOPNOTSUPP; + + if (sk->sk_shutdown & SEND_SHUTDOWN) + return -EPIPE; + + BT_DBG("sock %p, sk %p", sock, sk); + + lock_sock(sk); + + while (len) { + size_t size = min_t(size_t, len, d->mtu); + int err; + + skb = sock_alloc_send_skb(sk, size + RFCOMM_SKB_RESERVE, + msg->msg_flags & MSG_DONTWAIT, &err); + if (!skb) { + if (sent == 0) + sent = err; + break; + } + skb_reserve(skb, RFCOMM_SKB_HEAD_RESERVE); + + err = memcpy_fromiovec(skb_put(skb, size), msg->msg_iov, size); + if (err) { + kfree_skb(skb); + if (sent == 0) + sent = err; + break; + } + + skb->priority = sk->sk_priority; + + err = rfcomm_dlc_send(d, skb); + if (err < 0) { + kfree_skb(skb); + if (sent == 0) + sent = err; + break; + } + + sent += size; + len -= size; + } + + release_sock(sk); + + return sent; +} + +static int rfcomm_sock_recvmsg(struct kiocb *iocb, struct socket *sock, + struct msghdr *msg, size_t size, int flags) +{ + struct sock *sk = sock->sk; + struct rfcomm_dlc *d = rfcomm_pi(sk)->dlc; + int len; + + if (test_and_clear_bit(RFCOMM_DEFER_SETUP, &d->flags)) { + rfcomm_dlc_accept(d); + return 0; + } + + len = bt_sock_stream_recvmsg(iocb, sock, msg, size, flags); + + lock_sock(sk); + if (!(flags & MSG_PEEK) && len > 0) + atomic_sub(len, &sk->sk_rmem_alloc); + + if (atomic_read(&sk->sk_rmem_alloc) <= (sk->sk_rcvbuf >> 2)) + rfcomm_dlc_unthrottle(rfcomm_pi(sk)->dlc); + release_sock(sk); + + return len; +} + +static int rfcomm_sock_setsockopt_old(struct socket *sock, int optname, char __user *optval, unsigned int optlen) +{ + struct sock *sk = sock->sk; + int err = 0; + u32 opt; + + BT_DBG("sk %p", sk); + + lock_sock(sk); + + switch (optname) { + case RFCOMM_LM: + if (get_user(opt, (u32 __user *) optval)) { + err = -EFAULT; + break; + } + + if (opt & RFCOMM_LM_AUTH) + rfcomm_pi(sk)->sec_level = BT_SECURITY_LOW; + if (opt & RFCOMM_LM_ENCRYPT) + rfcomm_pi(sk)->sec_level = BT_SECURITY_MEDIUM; + if (opt & RFCOMM_LM_SECURE) + rfcomm_pi(sk)->sec_level = BT_SECURITY_HIGH; + + rfcomm_pi(sk)->role_switch = (opt & RFCOMM_LM_MASTER); + break; + + default: + err = -ENOPROTOOPT; + break; + } + + release_sock(sk); + return err; +} + +static int rfcomm_sock_setsockopt(struct socket *sock, int level, int optname, char __user *optval, unsigned int optlen) +{ + struct sock *sk = sock->sk; + struct bt_security sec; + int err = 0; + size_t len; + u32 opt; + + BT_DBG("sk %p", sk); + + if (level == SOL_RFCOMM) + return rfcomm_sock_setsockopt_old(sock, optname, optval, optlen); + + if (level != SOL_BLUETOOTH) + return -ENOPROTOOPT; + + lock_sock(sk); + + switch (optname) { + case BT_SECURITY: + if (sk->sk_type != SOCK_STREAM) { + err = -EINVAL; + break; + } + + sec.level = BT_SECURITY_LOW; + + len = min_t(unsigned int, sizeof(sec), optlen); + if (copy_from_user((char *) &sec, optval, len)) { + err = -EFAULT; + break; + } + + if (sec.level > BT_SECURITY_HIGH) { + err = -EINVAL; + break; + } + + rfcomm_pi(sk)->sec_level = sec.level; + break; + + case BT_DEFER_SETUP: + if (sk->sk_state != BT_BOUND && sk->sk_state != BT_LISTEN) { + err = -EINVAL; + break; + } + + if (get_user(opt, (u32 __user *) optval)) { + err = -EFAULT; + break; + } + + bt_sk(sk)->defer_setup = opt; + break; + + default: + err = -ENOPROTOOPT; + break; + } + + release_sock(sk); + return err; +} + +static int rfcomm_sock_getsockopt_old(struct socket *sock, int optname, char __user *optval, int __user *optlen) +{ + struct sock *sk = sock->sk; + struct rfcomm_conninfo cinfo; + struct l2cap_conn *conn = l2cap_pi(sk)->chan->conn; + int len, err = 0; + u32 opt; + + BT_DBG("sk %p", sk); + + if (get_user(len, optlen)) + return -EFAULT; + + lock_sock(sk); + + switch (optname) { + case RFCOMM_LM: + switch (rfcomm_pi(sk)->sec_level) { + case BT_SECURITY_LOW: + opt = RFCOMM_LM_AUTH; + break; + case BT_SECURITY_MEDIUM: + opt = RFCOMM_LM_AUTH | RFCOMM_LM_ENCRYPT; + break; + case BT_SECURITY_HIGH: + opt = RFCOMM_LM_AUTH | RFCOMM_LM_ENCRYPT | + RFCOMM_LM_SECURE; + break; + default: + opt = 0; + break; + } + + if (rfcomm_pi(sk)->role_switch) + opt |= RFCOMM_LM_MASTER; + + if (put_user(opt, (u32 __user *) optval)) + err = -EFAULT; + break; + + case RFCOMM_CONNINFO: + if (sk->sk_state != BT_CONNECTED && + !rfcomm_pi(sk)->dlc->defer_setup) { + err = -ENOTCONN; + break; + } + + memset(&cinfo, 0, sizeof(cinfo)); + cinfo.hci_handle = conn->hcon->handle; + memcpy(cinfo.dev_class, conn->hcon->dev_class, 3); + + len = min_t(unsigned int, len, sizeof(cinfo)); + if (copy_to_user(optval, (char *) &cinfo, len)) + err = -EFAULT; + + break; + + default: + err = -ENOPROTOOPT; + break; + } + + release_sock(sk); + return err; +} + +static int rfcomm_sock_getsockopt(struct socket *sock, int level, int optname, char __user *optval, int __user *optlen) +{ + struct sock *sk = sock->sk; + struct bt_security sec; + int len, err = 0; + + BT_DBG("sk %p", sk); + + if (level == SOL_RFCOMM) + return rfcomm_sock_getsockopt_old(sock, optname, optval, optlen); + + if (level != SOL_BLUETOOTH) + return -ENOPROTOOPT; + + if (get_user(len, optlen)) + return -EFAULT; + + lock_sock(sk); + + switch (optname) { + case BT_SECURITY: + if (sk->sk_type != SOCK_STREAM) { + err = -EINVAL; + break; + } + + sec.level = rfcomm_pi(sk)->sec_level; + + len = min_t(unsigned int, len, sizeof(sec)); + if (copy_to_user(optval, (char *) &sec, len)) + err = -EFAULT; + + break; + + case BT_DEFER_SETUP: + if (sk->sk_state != BT_BOUND && sk->sk_state != BT_LISTEN) { + err = -EINVAL; + break; + } + + if (put_user(bt_sk(sk)->defer_setup, (u32 __user *) optval)) + err = -EFAULT; + + break; + + default: + err = -ENOPROTOOPT; + break; + } + + release_sock(sk); + return err; +} + +static int rfcomm_sock_ioctl(struct socket *sock, unsigned int cmd, unsigned long arg) +{ + struct sock *sk __maybe_unused = sock->sk; + int err; + + BT_DBG("sk %p cmd %x arg %lx", sk, cmd, arg); + + err = bt_sock_ioctl(sock, cmd, arg); + + if (err == -ENOIOCTLCMD) { +#ifdef CONFIG_BT_RFCOMM_TTY + lock_sock(sk); + err = rfcomm_dev_ioctl(sk, cmd, (void __user *) arg); + release_sock(sk); +#else + err = -EOPNOTSUPP; +#endif + } + + return err; +} + +static int rfcomm_sock_shutdown(struct socket *sock, int how) +{ + struct sock *sk = sock->sk; + int err = 0; + + BT_DBG("sock %p, sk %p", sock, sk); + + if (!sk) + return 0; + + lock_sock(sk); + if (!sk->sk_shutdown) { + sk->sk_shutdown = SHUTDOWN_MASK; + __rfcomm_sock_close(sk); + + if (sock_flag(sk, SOCK_LINGER) && sk->sk_lingertime) + err = bt_sock_wait_state(sk, BT_CLOSED, sk->sk_lingertime); + } + release_sock(sk); + return err; +} + +static int rfcomm_sock_release(struct socket *sock) +{ + struct sock *sk = sock->sk; + int err; + + BT_DBG("sock %p, sk %p", sock, sk); + + if (!sk) + return 0; + + err = rfcomm_sock_shutdown(sock, 2); + + sock_orphan(sk); + rfcomm_sock_kill(sk); + return err; +} + +/* ---- RFCOMM core layer callbacks ---- + * + * called under rfcomm_lock() + */ +int rfcomm_connect_ind(struct rfcomm_session *s, u8 channel, struct rfcomm_dlc **d) +{ + struct sock *sk, *parent; + bdaddr_t src, dst; + int result = 0; + + BT_DBG("session %p channel %d", s, channel); + + rfcomm_session_getaddr(s, &src, &dst); + + /* Check if we have socket listening on channel */ + parent = rfcomm_get_sock_by_channel(BT_LISTEN, channel, &src); + if (!parent) + return 0; + + bh_lock_sock(parent); + + /* Check for backlog size */ + if (sk_acceptq_is_full(parent)) { + BT_DBG("backlog full %d", parent->sk_ack_backlog); + goto done; + } + + sk = rfcomm_sock_alloc(sock_net(parent), NULL, BTPROTO_RFCOMM, GFP_ATOMIC); + if (!sk) + goto done; + + bt_sock_reclassify_lock(sk, BTPROTO_RFCOMM); + + rfcomm_sock_init(sk, parent); + bacpy(&bt_sk(sk)->src, &src); + bacpy(&bt_sk(sk)->dst, &dst); + rfcomm_pi(sk)->channel = channel; + + sk->sk_state = BT_CONFIG; + bt_accept_enqueue(parent, sk); + + /* Accept connection and return socket DLC */ + *d = rfcomm_pi(sk)->dlc; + result = 1; + +done: + bh_unlock_sock(parent); + + if (bt_sk(parent)->defer_setup) + parent->sk_state_change(parent); + + return result; +} + +static int rfcomm_sock_debugfs_show(struct seq_file *f, void *p) +{ + struct sock *sk; + struct hlist_node *node; + + read_lock(&rfcomm_sk_list.lock); + + sk_for_each(sk, node, &rfcomm_sk_list.head) { + seq_printf(f, "%s %s %d %d\n", + batostr(&bt_sk(sk)->src), + batostr(&bt_sk(sk)->dst), + sk->sk_state, rfcomm_pi(sk)->channel); + } + + read_unlock(&rfcomm_sk_list.lock); + + return 0; +} + +static int rfcomm_sock_debugfs_open(struct inode *inode, struct file *file) +{ + return single_open(file, rfcomm_sock_debugfs_show, inode->i_private); +} + +static const struct file_operations rfcomm_sock_debugfs_fops = { + .open = rfcomm_sock_debugfs_open, + .read = seq_read, + .llseek = seq_lseek, + .release = single_release, +}; + +static struct dentry *rfcomm_sock_debugfs; + +static const struct proto_ops rfcomm_sock_ops = { + .family = PF_BLUETOOTH, + .owner = THIS_MODULE, + .release = rfcomm_sock_release, + .bind = rfcomm_sock_bind, + .connect = rfcomm_sock_connect, + .listen = rfcomm_sock_listen, + .accept = rfcomm_sock_accept, + .getname = rfcomm_sock_getname, + .sendmsg = rfcomm_sock_sendmsg, + .recvmsg = rfcomm_sock_recvmsg, + .shutdown = rfcomm_sock_shutdown, + .setsockopt = rfcomm_sock_setsockopt, + .getsockopt = rfcomm_sock_getsockopt, + .ioctl = rfcomm_sock_ioctl, + .poll = bt_sock_poll, + .socketpair = sock_no_socketpair, + .mmap = sock_no_mmap +}; + +static const struct net_proto_family rfcomm_sock_family_ops = { + .family = PF_BLUETOOTH, + .owner = THIS_MODULE, + .create = rfcomm_sock_create +}; + +int __init rfcomm_init_sockets(void) +{ + int err; + + err = proto_register(&rfcomm_proto, 0); + if (err < 0) + return err; + + err = bt_sock_register(BTPROTO_RFCOMM, &rfcomm_sock_family_ops); + if (err < 0) + goto error; + + if (bt_debugfs) { + rfcomm_sock_debugfs = debugfs_create_file("rfcomm", 0444, + bt_debugfs, NULL, &rfcomm_sock_debugfs_fops); + if (!rfcomm_sock_debugfs) + BT_ERR("Failed to create RFCOMM debug file"); + } + + BT_INFO("RFCOMM socket layer initialized"); + + return 0; + +error: + BT_ERR("RFCOMM socket layer registration failed"); + proto_unregister(&rfcomm_proto); + return err; +} + +void __exit rfcomm_cleanup_sockets(void) +{ + debugfs_remove(rfcomm_sock_debugfs); + + if (bt_sock_unregister(BTPROTO_RFCOMM) < 0) + BT_ERR("RFCOMM socket layer unregistration failed"); + + proto_unregister(&rfcomm_proto); +} diff --git a/net/bluetooth_tizen/rfcomm/tty.c b/net/bluetooth_tizen/rfcomm/tty.c new file mode 100644 index 0000000..4bf54b3 --- /dev/null +++ b/net/bluetooth_tizen/rfcomm/tty.c @@ -0,0 +1,1188 @@ +/* + RFCOMM implementation for Linux Bluetooth stack (BlueZ). + Copyright (C) 2002 Maxim Krasnyansky <maxk@qualcomm.com> + Copyright (C) 2002 Marcel Holtmann <marcel@holtmann.org> + + This program is free software; you can redistribute it and/or modify + it under the terms of the GNU General Public License version 2 as + published by the Free Software Foundation; + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS + OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT OF THIRD PARTY RIGHTS. + IN NO EVENT SHALL THE COPYRIGHT HOLDER(S) AND AUTHOR(S) BE LIABLE FOR ANY + CLAIM, OR ANY SPECIAL INDIRECT OR CONSEQUENTIAL DAMAGES, OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + + ALL LIABILITY, INCLUDING LIABILITY FOR INFRINGEMENT OF ANY PATENTS, + COPYRIGHTS, TRADEMARKS OR OTHER RIGHTS, RELATING TO USE OF THIS + SOFTWARE IS DISCLAIMED. +*/ + +/* + * RFCOMM TTY. + */ + +#include <linux/module.h> + +#include <linux/tty.h> +#include <linux/tty_driver.h> +#include <linux/tty_flip.h> + +#include <linux/capability.h> +#include <linux/slab.h> +#include <linux/skbuff.h> +#include <linux/workqueue.h> + +#include <net/bluetooth/bluetooth.h> +#include <net/bluetooth/hci_core.h> +#include <net/bluetooth/rfcomm.h> + +#define RFCOMM_TTY_MAGIC 0x6d02 /* magic number for rfcomm struct */ +#define RFCOMM_TTY_PORTS RFCOMM_MAX_DEV /* whole lotta rfcomm devices */ +#define RFCOMM_TTY_MAJOR 216 /* device node major id of the usb/bluetooth.c driver */ +#define RFCOMM_TTY_MINOR 0 + +static struct tty_driver *rfcomm_tty_driver; + +struct rfcomm_dev { + struct list_head list; + atomic_t refcnt; + + char name[12]; + int id; + unsigned long flags; + atomic_t opened; + int err; + + bdaddr_t src; + bdaddr_t dst; + u8 channel; + + uint modem_status; + + struct rfcomm_dlc *dlc; + struct tty_struct *tty; + wait_queue_head_t wait; + struct work_struct wakeup_task; + + struct device *tty_dev; + + atomic_t wmem_alloc; + + struct sk_buff_head pending; +}; + +static LIST_HEAD(rfcomm_dev_list); +static DEFINE_SPINLOCK(rfcomm_dev_lock); + +static void rfcomm_dev_data_ready(struct rfcomm_dlc *dlc, struct sk_buff *skb); +static void rfcomm_dev_state_change(struct rfcomm_dlc *dlc, int err); +static void rfcomm_dev_modem_status(struct rfcomm_dlc *dlc, u8 v24_sig); + +static void rfcomm_tty_wakeup(struct work_struct *work); + +/* ---- Device functions ---- */ +static void rfcomm_dev_destruct(struct rfcomm_dev *dev) +{ + struct rfcomm_dlc *dlc = dev->dlc; + + BT_DBG("dev %p dlc %p", dev, dlc); + + /* Refcount should only hit zero when called from rfcomm_dev_del() + which will have taken us off the list. Everything else are + refcounting bugs. */ + BUG_ON(!list_empty(&dev->list)); + + rfcomm_dlc_lock(dlc); + /* Detach DLC if it's owned by this dev */ + if (dlc->owner == dev) + dlc->owner = NULL; + rfcomm_dlc_unlock(dlc); + + rfcomm_dlc_put(dlc); + + tty_unregister_device(rfcomm_tty_driver, dev->id); + + kfree(dev); + + /* It's safe to call module_put() here because socket still + holds reference to this module. */ + module_put(THIS_MODULE); +} + +static inline void rfcomm_dev_hold(struct rfcomm_dev *dev) +{ + atomic_inc(&dev->refcnt); +} + +static inline void rfcomm_dev_put(struct rfcomm_dev *dev) +{ + /* The reason this isn't actually a race, as you no + doubt have a little voice screaming at you in your + head, is that the refcount should never actually + reach zero unless the device has already been taken + off the list, in rfcomm_dev_del(). And if that's not + true, we'll hit the BUG() in rfcomm_dev_destruct() + anyway. */ + if (atomic_dec_and_test(&dev->refcnt)) + rfcomm_dev_destruct(dev); +} + +static struct rfcomm_dev *__rfcomm_dev_get(int id) +{ + struct rfcomm_dev *dev; + + list_for_each_entry(dev, &rfcomm_dev_list, list) + if (dev->id == id) + return dev; + + return NULL; +} + +static inline struct rfcomm_dev *rfcomm_dev_get(int id) +{ + struct rfcomm_dev *dev; + + spin_lock(&rfcomm_dev_lock); + + dev = __rfcomm_dev_get(id); + + if (dev) { + if (test_bit(RFCOMM_TTY_RELEASED, &dev->flags)) + dev = NULL; + else + rfcomm_dev_hold(dev); + } + + spin_unlock(&rfcomm_dev_lock); + + return dev; +} + +static struct device *rfcomm_get_device(struct rfcomm_dev *dev) +{ + struct hci_dev *hdev; + struct hci_conn *conn; + + hdev = hci_get_route(&dev->dst, &dev->src); + if (!hdev) + return NULL; + + conn = hci_conn_hash_lookup_ba(hdev, ACL_LINK, &dev->dst); + + hci_dev_put(hdev); + + return conn ? &conn->dev : NULL; +} + +static ssize_t show_address(struct device *tty_dev, struct device_attribute *attr, char *buf) +{ + struct rfcomm_dev *dev = dev_get_drvdata(tty_dev); + return sprintf(buf, "%s\n", batostr(&dev->dst)); +} + +static ssize_t show_channel(struct device *tty_dev, struct device_attribute *attr, char *buf) +{ + struct rfcomm_dev *dev = dev_get_drvdata(tty_dev); + return sprintf(buf, "%d\n", dev->channel); +} + +static DEVICE_ATTR(address, S_IRUGO, show_address, NULL); +static DEVICE_ATTR(channel, S_IRUGO, show_channel, NULL); + +static int rfcomm_dev_add(struct rfcomm_dev_req *req, struct rfcomm_dlc *dlc) +{ + struct rfcomm_dev *dev, *entry; + struct list_head *head = &rfcomm_dev_list; + int err = 0; + + BT_DBG("id %d channel %d", req->dev_id, req->channel); + + dev = kzalloc(sizeof(struct rfcomm_dev), GFP_KERNEL); + if (!dev) + return -ENOMEM; + + spin_lock(&rfcomm_dev_lock); + + if (req->dev_id < 0) { + dev->id = 0; + + list_for_each_entry(entry, &rfcomm_dev_list, list) { + if (entry->id != dev->id) + break; + + dev->id++; + head = &entry->list; + } + } else { + dev->id = req->dev_id; + + list_for_each_entry(entry, &rfcomm_dev_list, list) { + if (entry->id == dev->id) { + err = -EADDRINUSE; + goto out; + } + + if (entry->id > dev->id - 1) + break; + + head = &entry->list; + } + } + + if ((dev->id < 0) || (dev->id > RFCOMM_MAX_DEV - 1)) { + err = -ENFILE; + goto out; + } + + sprintf(dev->name, "rfcomm%d", dev->id); + + list_add(&dev->list, head); + atomic_set(&dev->refcnt, 1); + + bacpy(&dev->src, &req->src); + bacpy(&dev->dst, &req->dst); + dev->channel = req->channel; + + dev->flags = req->flags & + ((1 << RFCOMM_RELEASE_ONHUP) | (1 << RFCOMM_REUSE_DLC)); + + atomic_set(&dev->opened, 0); + + init_waitqueue_head(&dev->wait); + INIT_WORK(&dev->wakeup_task, rfcomm_tty_wakeup); + + skb_queue_head_init(&dev->pending); + + rfcomm_dlc_lock(dlc); + + if (req->flags & (1 << RFCOMM_REUSE_DLC)) { + struct sock *sk = dlc->owner; + struct sk_buff *skb; + + BUG_ON(!sk); + + rfcomm_dlc_throttle(dlc); + + while ((skb = skb_dequeue(&sk->sk_receive_queue))) { + skb_orphan(skb); + skb_queue_tail(&dev->pending, skb); + atomic_sub(skb->len, &sk->sk_rmem_alloc); + } + } + + dlc->data_ready = rfcomm_dev_data_ready; + dlc->state_change = rfcomm_dev_state_change; + dlc->modem_status = rfcomm_dev_modem_status; + + dlc->owner = dev; + dev->dlc = dlc; + + rfcomm_dev_modem_status(dlc, dlc->remote_v24_sig); + + rfcomm_dlc_unlock(dlc); + + /* It's safe to call __module_get() here because socket already + holds reference to this module. */ + __module_get(THIS_MODULE); + +out: + spin_unlock(&rfcomm_dev_lock); + + if (err < 0) + goto free; + + dev->tty_dev = tty_register_device(rfcomm_tty_driver, dev->id, NULL); + + if (IS_ERR(dev->tty_dev)) { + err = PTR_ERR(dev->tty_dev); + list_del(&dev->list); + goto free; + } + + dev_set_drvdata(dev->tty_dev, dev); + + if (device_create_file(dev->tty_dev, &dev_attr_address) < 0) + BT_ERR("Failed to create address attribute"); + + if (device_create_file(dev->tty_dev, &dev_attr_channel) < 0) + BT_ERR("Failed to create channel attribute"); + + return dev->id; + +free: + kfree(dev); + return err; +} + +static void rfcomm_dev_del(struct rfcomm_dev *dev) +{ + BT_DBG("dev %p", dev); + + BUG_ON(test_and_set_bit(RFCOMM_TTY_RELEASED, &dev->flags)); + + if (atomic_read(&dev->opened) > 0) + return; + + spin_lock(&rfcomm_dev_lock); + list_del_init(&dev->list); + spin_unlock(&rfcomm_dev_lock); + + rfcomm_dev_put(dev); +} + +/* ---- Send buffer ---- */ +static inline unsigned int rfcomm_room(struct rfcomm_dlc *dlc) +{ + /* We can't let it be zero, because we don't get a callback + when tx_credits becomes nonzero, hence we'd never wake up */ + return dlc->mtu * (dlc->tx_credits?:1); +} + +static void rfcomm_wfree(struct sk_buff *skb) +{ + struct rfcomm_dev *dev = (void *) skb->sk; + atomic_sub(skb->truesize, &dev->wmem_alloc); + if (test_bit(RFCOMM_TTY_ATTACHED, &dev->flags)) + queue_work(system_nrt_wq, &dev->wakeup_task); + rfcomm_dev_put(dev); +} + +static inline void rfcomm_set_owner_w(struct sk_buff *skb, struct rfcomm_dev *dev) +{ + rfcomm_dev_hold(dev); + atomic_add(skb->truesize, &dev->wmem_alloc); + skb->sk = (void *) dev; + skb->destructor = rfcomm_wfree; +} + +static struct sk_buff *rfcomm_wmalloc(struct rfcomm_dev *dev, unsigned long size, gfp_t priority) +{ + if (atomic_read(&dev->wmem_alloc) < rfcomm_room(dev->dlc)) { + struct sk_buff *skb = alloc_skb(size, priority); + if (skb) { + rfcomm_set_owner_w(skb, dev); + return skb; + } + } + return NULL; +} + +/* ---- Device IOCTLs ---- */ + +#define NOCAP_FLAGS ((1 << RFCOMM_REUSE_DLC) | (1 << RFCOMM_RELEASE_ONHUP)) + +static int rfcomm_create_dev(struct sock *sk, void __user *arg) +{ + struct rfcomm_dev_req req; + struct rfcomm_dlc *dlc; + int id; + + if (copy_from_user(&req, arg, sizeof(req))) + return -EFAULT; + + BT_DBG("sk %p dev_id %d flags 0x%x", sk, req.dev_id, req.flags); + + if (req.flags != NOCAP_FLAGS && !capable(CAP_NET_ADMIN)) + return -EPERM; + + if (req.flags & (1 << RFCOMM_REUSE_DLC)) { + /* Socket must be connected */ + if (sk->sk_state != BT_CONNECTED) + return -EBADFD; + + dlc = rfcomm_pi(sk)->dlc; + rfcomm_dlc_hold(dlc); + } else { + dlc = rfcomm_dlc_alloc(GFP_KERNEL); + if (!dlc) + return -ENOMEM; + } + + id = rfcomm_dev_add(&req, dlc); + if (id < 0) { + rfcomm_dlc_put(dlc); + return id; + } + + if (req.flags & (1 << RFCOMM_REUSE_DLC)) { + /* DLC is now used by device. + * Socket must be disconnected */ + sk->sk_state = BT_CLOSED; + } + + return id; +} + +static int rfcomm_release_dev(void __user *arg) +{ + struct rfcomm_dev_req req; + struct rfcomm_dev *dev; + + if (copy_from_user(&req, arg, sizeof(req))) + return -EFAULT; + + BT_DBG("dev_id %d flags 0x%x", req.dev_id, req.flags); + + dev = rfcomm_dev_get(req.dev_id); + if (!dev) + return -ENODEV; + + if (dev->flags != NOCAP_FLAGS && !capable(CAP_NET_ADMIN)) { + rfcomm_dev_put(dev); + return -EPERM; + } + + if (req.flags & (1 << RFCOMM_HANGUP_NOW)) + rfcomm_dlc_close(dev->dlc, 0); + + /* Shut down TTY synchronously before freeing rfcomm_dev */ + if (dev->tty) + tty_vhangup(dev->tty); + + if (!test_bit(RFCOMM_RELEASE_ONHUP, &dev->flags)) + rfcomm_dev_del(dev); + rfcomm_dev_put(dev); + return 0; +} + +static int rfcomm_get_dev_list(void __user *arg) +{ + struct rfcomm_dev *dev; + struct rfcomm_dev_list_req *dl; + struct rfcomm_dev_info *di; + int n = 0, size, err; + u16 dev_num; + + BT_DBG(""); + + if (get_user(dev_num, (u16 __user *) arg)) + return -EFAULT; + + if (!dev_num || dev_num > (PAGE_SIZE * 4) / sizeof(*di)) + return -EINVAL; + + size = sizeof(*dl) + dev_num * sizeof(*di); + + dl = kmalloc(size, GFP_KERNEL); + if (!dl) + return -ENOMEM; + + di = dl->dev_info; + + spin_lock(&rfcomm_dev_lock); + + list_for_each_entry(dev, &rfcomm_dev_list, list) { + if (test_bit(RFCOMM_TTY_RELEASED, &dev->flags)) + continue; + (di + n)->id = dev->id; + (di + n)->flags = dev->flags; + (di + n)->state = dev->dlc->state; + (di + n)->channel = dev->channel; + bacpy(&(di + n)->src, &dev->src); + bacpy(&(di + n)->dst, &dev->dst); + if (++n >= dev_num) + break; + } + + spin_unlock(&rfcomm_dev_lock); + + dl->dev_num = n; + size = sizeof(*dl) + n * sizeof(*di); + + err = copy_to_user(arg, dl, size); + kfree(dl); + + return err ? -EFAULT : 0; +} + +static int rfcomm_get_dev_info(void __user *arg) +{ + struct rfcomm_dev *dev; + struct rfcomm_dev_info di; + int err = 0; + + BT_DBG(""); + + if (copy_from_user(&di, arg, sizeof(di))) + return -EFAULT; + + dev = rfcomm_dev_get(di.id); + if (!dev) + return -ENODEV; + + di.flags = dev->flags; + di.channel = dev->channel; + di.state = dev->dlc->state; + bacpy(&di.src, &dev->src); + bacpy(&di.dst, &dev->dst); + + if (copy_to_user(arg, &di, sizeof(di))) + err = -EFAULT; + + rfcomm_dev_put(dev); + return err; +} + +int rfcomm_dev_ioctl(struct sock *sk, unsigned int cmd, void __user *arg) +{ + BT_DBG("cmd %d arg %p", cmd, arg); + + switch (cmd) { + case RFCOMMCREATEDEV: + return rfcomm_create_dev(sk, arg); + + case RFCOMMRELEASEDEV: + return rfcomm_release_dev(arg); + + case RFCOMMGETDEVLIST: + return rfcomm_get_dev_list(arg); + + case RFCOMMGETDEVINFO: + return rfcomm_get_dev_info(arg); + } + + return -EINVAL; +} + +/* ---- DLC callbacks ---- */ +static void rfcomm_dev_data_ready(struct rfcomm_dlc *dlc, struct sk_buff *skb) +{ + struct rfcomm_dev *dev = dlc->owner; + struct tty_struct *tty; + + if (!dev) { + kfree_skb(skb); + return; + } + + tty = dev->tty; + if (!tty || !skb_queue_empty(&dev->pending)) { + skb_queue_tail(&dev->pending, skb); + return; + } + + BT_DBG("dlc %p tty %p len %d", dlc, tty, skb->len); + + tty_insert_flip_string(tty, skb->data, skb->len); + tty_flip_buffer_push(tty); + + kfree_skb(skb); +} + +static void rfcomm_dev_state_change(struct rfcomm_dlc *dlc, int err) +{ + struct rfcomm_dev *dev = dlc->owner; + if (!dev) + return; + + BT_DBG("dlc %p dev %p err %d", dlc, dev, err); + + dev->err = err; + wake_up_interruptible(&dev->wait); + + if (dlc->state == BT_CLOSED) { + if (!dev->tty) { + if (test_bit(RFCOMM_RELEASE_ONHUP, &dev->flags)) { + /* Drop DLC lock here to avoid deadlock + * 1. rfcomm_dev_get will take rfcomm_dev_lock + * but in rfcomm_dev_add there's lock order: + * rfcomm_dev_lock -> dlc lock + * 2. rfcomm_dev_put will deadlock if it's + * the last reference + */ + rfcomm_dlc_unlock(dlc); + if (rfcomm_dev_get(dev->id) == NULL) { + rfcomm_dlc_lock(dlc); + return; + } + + rfcomm_dev_del(dev); + rfcomm_dev_put(dev); + rfcomm_dlc_lock(dlc); + } + } else + tty_hangup(dev->tty); + } +} + +static void rfcomm_dev_modem_status(struct rfcomm_dlc *dlc, u8 v24_sig) +{ + struct rfcomm_dev *dev = dlc->owner; + if (!dev) + return; + + BT_DBG("dlc %p dev %p v24_sig 0x%02x", dlc, dev, v24_sig); + + if ((dev->modem_status & TIOCM_CD) && !(v24_sig & RFCOMM_V24_DV)) { + if (dev->tty && !C_CLOCAL(dev->tty)) + tty_hangup(dev->tty); + } + + dev->modem_status = + ((v24_sig & RFCOMM_V24_RTC) ? (TIOCM_DSR | TIOCM_DTR) : 0) | + ((v24_sig & RFCOMM_V24_RTR) ? (TIOCM_RTS | TIOCM_CTS) : 0) | + ((v24_sig & RFCOMM_V24_IC) ? TIOCM_RI : 0) | + ((v24_sig & RFCOMM_V24_DV) ? TIOCM_CD : 0); +} + +/* ---- TTY functions ---- */ +static void rfcomm_tty_wakeup(struct work_struct *work) +{ + struct rfcomm_dev *dev = container_of(work, struct rfcomm_dev, + wakeup_task); + struct tty_struct *tty = dev->tty; + if (!tty) + return; + + BT_DBG("dev %p tty %p", dev, tty); + tty_wakeup(tty); +} + +static void rfcomm_tty_copy_pending(struct rfcomm_dev *dev) +{ + struct tty_struct *tty = dev->tty; + struct sk_buff *skb; + int inserted = 0; + + if (!tty) + return; + + BT_DBG("dev %p tty %p", dev, tty); + + rfcomm_dlc_lock(dev->dlc); + + while ((skb = skb_dequeue(&dev->pending))) { + inserted += tty_insert_flip_string(tty, skb->data, skb->len); + kfree_skb(skb); + } + + rfcomm_dlc_unlock(dev->dlc); + + if (inserted > 0) + tty_flip_buffer_push(tty); +} + +static int rfcomm_tty_open(struct tty_struct *tty, struct file *filp) +{ + DECLARE_WAITQUEUE(wait, current); + struct rfcomm_dev *dev; + struct rfcomm_dlc *dlc; + int err, id; + + id = tty->index; + + BT_DBG("tty %p id %d", tty, id); + + /* We don't leak this refcount. For reasons which are not entirely + clear, the TTY layer will call our ->close() method even if the + open fails. We decrease the refcount there, and decreasing it + here too would cause breakage. */ + dev = rfcomm_dev_get(id); + if (!dev) + return -ENODEV; + + BT_DBG("dev %p dst %s channel %d opened %d", dev, batostr(&dev->dst), + dev->channel, atomic_read(&dev->opened)); + + if (atomic_inc_return(&dev->opened) > 1) + return 0; + + dlc = dev->dlc; + + /* Attach TTY and open DLC */ + + rfcomm_dlc_lock(dlc); + tty->driver_data = dev; + dev->tty = tty; + rfcomm_dlc_unlock(dlc); + set_bit(RFCOMM_TTY_ATTACHED, &dev->flags); + + err = rfcomm_dlc_open(dlc, &dev->src, &dev->dst, dev->channel); + if (err < 0) + return err; + + /* Wait for DLC to connect */ + add_wait_queue(&dev->wait, &wait); + while (1) { + set_current_state(TASK_INTERRUPTIBLE); + + if (dlc->state == BT_CLOSED) { + err = -dev->err; + break; + } + + if (dlc->state == BT_CONNECTED) + break; + + if (signal_pending(current)) { + err = -EINTR; + break; + } + + tty_unlock(); + schedule(); + tty_lock(); + } + set_current_state(TASK_RUNNING); + remove_wait_queue(&dev->wait, &wait); + + if (err == 0) + device_move(dev->tty_dev, rfcomm_get_device(dev), + DPM_ORDER_DEV_AFTER_PARENT); + + rfcomm_tty_copy_pending(dev); + + rfcomm_dlc_unthrottle(dev->dlc); + + return err; +} + +static void rfcomm_tty_close(struct tty_struct *tty, struct file *filp) +{ + struct rfcomm_dev *dev = (struct rfcomm_dev *) tty->driver_data; + if (!dev) + return; + + BT_DBG("tty %p dev %p dlc %p opened %d", tty, dev, dev->dlc, + atomic_read(&dev->opened)); + + if (atomic_dec_and_test(&dev->opened)) { + if (dev->tty_dev->parent) + device_move(dev->tty_dev, NULL, DPM_ORDER_DEV_LAST); + + /* Close DLC and dettach TTY */ + rfcomm_dlc_close(dev->dlc, 0); + + clear_bit(RFCOMM_TTY_ATTACHED, &dev->flags); + cancel_work_sync(&dev->wakeup_task); + + rfcomm_dlc_lock(dev->dlc); + tty->driver_data = NULL; + dev->tty = NULL; + rfcomm_dlc_unlock(dev->dlc); + + if (test_bit(RFCOMM_TTY_RELEASED, &dev->flags)) { + spin_lock(&rfcomm_dev_lock); + list_del_init(&dev->list); + spin_unlock(&rfcomm_dev_lock); + + rfcomm_dev_put(dev); + } + } + + rfcomm_dev_put(dev); +} + +static int rfcomm_tty_write(struct tty_struct *tty, const unsigned char *buf, int count) +{ + struct rfcomm_dev *dev = (struct rfcomm_dev *) tty->driver_data; + struct rfcomm_dlc *dlc = dev->dlc; + struct sk_buff *skb; + int err = 0, sent = 0, size; + + BT_DBG("tty %p count %d", tty, count); + + while (count) { + size = min_t(uint, count, dlc->mtu); + + skb = rfcomm_wmalloc(dev, size + RFCOMM_SKB_RESERVE, GFP_ATOMIC); + + if (!skb) + break; + + skb_reserve(skb, RFCOMM_SKB_HEAD_RESERVE); + + memcpy(skb_put(skb, size), buf + sent, size); + + err = rfcomm_dlc_send(dlc, skb); + if (err < 0) { + kfree_skb(skb); + break; + } + + sent += size; + count -= size; + } + + return sent ? sent : err; +} + +static int rfcomm_tty_write_room(struct tty_struct *tty) +{ + struct rfcomm_dev *dev = (struct rfcomm_dev *) tty->driver_data; + int room; + + BT_DBG("tty %p", tty); + + if (!dev || !dev->dlc) + return 0; + + room = rfcomm_room(dev->dlc) - atomic_read(&dev->wmem_alloc); + if (room < 0) + room = 0; + + return room; +} + +static int rfcomm_tty_ioctl(struct tty_struct *tty, unsigned int cmd, unsigned long arg) +{ + BT_DBG("tty %p cmd 0x%02x", tty, cmd); + + switch (cmd) { + case TCGETS: + BT_DBG("TCGETS is not supported"); + return -ENOIOCTLCMD; + + case TCSETS: + BT_DBG("TCSETS is not supported"); + return -ENOIOCTLCMD; + + case TIOCMIWAIT: + BT_DBG("TIOCMIWAIT"); + break; + + case TIOCGSERIAL: + BT_ERR("TIOCGSERIAL is not supported"); + return -ENOIOCTLCMD; + + case TIOCSSERIAL: + BT_ERR("TIOCSSERIAL is not supported"); + return -ENOIOCTLCMD; + + case TIOCSERGSTRUCT: + BT_ERR("TIOCSERGSTRUCT is not supported"); + return -ENOIOCTLCMD; + + case TIOCSERGETLSR: + BT_ERR("TIOCSERGETLSR is not supported"); + return -ENOIOCTLCMD; + + case TIOCSERCONFIG: + BT_ERR("TIOCSERCONFIG is not supported"); + return -ENOIOCTLCMD; + + default: + return -ENOIOCTLCMD; /* ioctls which we must ignore */ + + } + + return -ENOIOCTLCMD; +} + +static void rfcomm_tty_set_termios(struct tty_struct *tty, struct ktermios *old) +{ + struct ktermios *new = tty->termios; + int old_baud_rate = tty_termios_baud_rate(old); + int new_baud_rate = tty_termios_baud_rate(new); + + u8 baud, data_bits, stop_bits, parity, x_on, x_off; + u16 changes = 0; + + struct rfcomm_dev *dev = (struct rfcomm_dev *) tty->driver_data; + + BT_DBG("tty %p termios %p", tty, old); + + if (!dev || !dev->dlc || !dev->dlc->session) + return; + + /* Handle turning off CRTSCTS */ + if ((old->c_cflag & CRTSCTS) && !(new->c_cflag & CRTSCTS)) + BT_DBG("Turning off CRTSCTS unsupported"); + + /* Parity on/off and when on, odd/even */ + if (((old->c_cflag & PARENB) != (new->c_cflag & PARENB)) || + ((old->c_cflag & PARODD) != (new->c_cflag & PARODD))) { + changes |= RFCOMM_RPN_PM_PARITY; + BT_DBG("Parity change detected."); + } + + /* Mark and space parity are not supported! */ + if (new->c_cflag & PARENB) { + if (new->c_cflag & PARODD) { + BT_DBG("Parity is ODD"); + parity = RFCOMM_RPN_PARITY_ODD; + } else { + BT_DBG("Parity is EVEN"); + parity = RFCOMM_RPN_PARITY_EVEN; + } + } else { + BT_DBG("Parity is OFF"); + parity = RFCOMM_RPN_PARITY_NONE; + } + + /* Setting the x_on / x_off characters */ + if (old->c_cc[VSTOP] != new->c_cc[VSTOP]) { + BT_DBG("XOFF custom"); + x_on = new->c_cc[VSTOP]; + changes |= RFCOMM_RPN_PM_XON; + } else { + BT_DBG("XOFF default"); + x_on = RFCOMM_RPN_XON_CHAR; + } + + if (old->c_cc[VSTART] != new->c_cc[VSTART]) { + BT_DBG("XON custom"); + x_off = new->c_cc[VSTART]; + changes |= RFCOMM_RPN_PM_XOFF; + } else { + BT_DBG("XON default"); + x_off = RFCOMM_RPN_XOFF_CHAR; + } + + /* Handle setting of stop bits */ + if ((old->c_cflag & CSTOPB) != (new->c_cflag & CSTOPB)) + changes |= RFCOMM_RPN_PM_STOP; + + /* POSIX does not support 1.5 stop bits and RFCOMM does not + * support 2 stop bits. So a request for 2 stop bits gets + * translated to 1.5 stop bits */ + if (new->c_cflag & CSTOPB) + stop_bits = RFCOMM_RPN_STOP_15; + else + stop_bits = RFCOMM_RPN_STOP_1; + + /* Handle number of data bits [5-8] */ + if ((old->c_cflag & CSIZE) != (new->c_cflag & CSIZE)) + changes |= RFCOMM_RPN_PM_DATA; + + switch (new->c_cflag & CSIZE) { + case CS5: + data_bits = RFCOMM_RPN_DATA_5; + break; + case CS6: + data_bits = RFCOMM_RPN_DATA_6; + break; + case CS7: + data_bits = RFCOMM_RPN_DATA_7; + break; + case CS8: + data_bits = RFCOMM_RPN_DATA_8; + break; + default: + data_bits = RFCOMM_RPN_DATA_8; + break; + } + + /* Handle baudrate settings */ + if (old_baud_rate != new_baud_rate) + changes |= RFCOMM_RPN_PM_BITRATE; + + switch (new_baud_rate) { + case 2400: + baud = RFCOMM_RPN_BR_2400; + break; + case 4800: + baud = RFCOMM_RPN_BR_4800; + break; + case 7200: + baud = RFCOMM_RPN_BR_7200; + break; + case 9600: + baud = RFCOMM_RPN_BR_9600; + break; + case 19200: + baud = RFCOMM_RPN_BR_19200; + break; + case 38400: + baud = RFCOMM_RPN_BR_38400; + break; + case 57600: + baud = RFCOMM_RPN_BR_57600; + break; + case 115200: + baud = RFCOMM_RPN_BR_115200; + break; + case 230400: + baud = RFCOMM_RPN_BR_230400; + break; + default: + /* 9600 is standard accordinag to the RFCOMM specification */ + baud = RFCOMM_RPN_BR_9600; + break; + + } + + if (changes) + rfcomm_send_rpn(dev->dlc->session, 1, dev->dlc->dlci, baud, + data_bits, stop_bits, parity, + RFCOMM_RPN_FLOW_NONE, x_on, x_off, changes); +} + +static void rfcomm_tty_throttle(struct tty_struct *tty) +{ + struct rfcomm_dev *dev = (struct rfcomm_dev *) tty->driver_data; + + BT_DBG("tty %p dev %p", tty, dev); + + rfcomm_dlc_throttle(dev->dlc); +} + +static void rfcomm_tty_unthrottle(struct tty_struct *tty) +{ + struct rfcomm_dev *dev = (struct rfcomm_dev *) tty->driver_data; + + BT_DBG("tty %p dev %p", tty, dev); + + rfcomm_dlc_unthrottle(dev->dlc); +} + +static int rfcomm_tty_chars_in_buffer(struct tty_struct *tty) +{ + struct rfcomm_dev *dev = (struct rfcomm_dev *) tty->driver_data; + + BT_DBG("tty %p dev %p", tty, dev); + + if (!dev || !dev->dlc) + return 0; + + if (!skb_queue_empty(&dev->dlc->tx_queue)) + return dev->dlc->mtu; + + return 0; +} + +static void rfcomm_tty_flush_buffer(struct tty_struct *tty) +{ + struct rfcomm_dev *dev = (struct rfcomm_dev *) tty->driver_data; + + BT_DBG("tty %p dev %p", tty, dev); + + if (!dev || !dev->dlc) + return; + + skb_queue_purge(&dev->dlc->tx_queue); + tty_wakeup(tty); +} + +static void rfcomm_tty_send_xchar(struct tty_struct *tty, char ch) +{ + BT_DBG("tty %p ch %c", tty, ch); +} + +static void rfcomm_tty_wait_until_sent(struct tty_struct *tty, int timeout) +{ + BT_DBG("tty %p timeout %d", tty, timeout); +} + +static void rfcomm_tty_hangup(struct tty_struct *tty) +{ + struct rfcomm_dev *dev = (struct rfcomm_dev *) tty->driver_data; + + BT_DBG("tty %p dev %p", tty, dev); + + if (!dev) + return; + + rfcomm_tty_flush_buffer(tty); + + if (test_bit(RFCOMM_RELEASE_ONHUP, &dev->flags)) { + if (rfcomm_dev_get(dev->id) == NULL) + return; + rfcomm_dev_del(dev); + rfcomm_dev_put(dev); + } +} + +static int rfcomm_tty_tiocmget(struct tty_struct *tty) +{ + struct rfcomm_dev *dev = (struct rfcomm_dev *) tty->driver_data; + + BT_DBG("tty %p dev %p", tty, dev); + + return dev->modem_status; +} + +static int rfcomm_tty_tiocmset(struct tty_struct *tty, unsigned int set, unsigned int clear) +{ + struct rfcomm_dev *dev = (struct rfcomm_dev *) tty->driver_data; + struct rfcomm_dlc *dlc = dev->dlc; + u8 v24_sig; + + BT_DBG("tty %p dev %p set 0x%02x clear 0x%02x", tty, dev, set, clear); + + rfcomm_dlc_get_modem_status(dlc, &v24_sig); + + if (set & TIOCM_DSR || set & TIOCM_DTR) + v24_sig |= RFCOMM_V24_RTC; + if (set & TIOCM_RTS || set & TIOCM_CTS) + v24_sig |= RFCOMM_V24_RTR; + if (set & TIOCM_RI) + v24_sig |= RFCOMM_V24_IC; + if (set & TIOCM_CD) + v24_sig |= RFCOMM_V24_DV; + + if (clear & TIOCM_DSR || clear & TIOCM_DTR) + v24_sig &= ~RFCOMM_V24_RTC; + if (clear & TIOCM_RTS || clear & TIOCM_CTS) + v24_sig &= ~RFCOMM_V24_RTR; + if (clear & TIOCM_RI) + v24_sig &= ~RFCOMM_V24_IC; + if (clear & TIOCM_CD) + v24_sig &= ~RFCOMM_V24_DV; + + rfcomm_dlc_set_modem_status(dlc, v24_sig); + + return 0; +} + +/* ---- TTY structure ---- */ + +static const struct tty_operations rfcomm_ops = { + .open = rfcomm_tty_open, + .close = rfcomm_tty_close, + .write = rfcomm_tty_write, + .write_room = rfcomm_tty_write_room, + .chars_in_buffer = rfcomm_tty_chars_in_buffer, + .flush_buffer = rfcomm_tty_flush_buffer, + .ioctl = rfcomm_tty_ioctl, + .throttle = rfcomm_tty_throttle, + .unthrottle = rfcomm_tty_unthrottle, + .set_termios = rfcomm_tty_set_termios, + .send_xchar = rfcomm_tty_send_xchar, + .hangup = rfcomm_tty_hangup, + .wait_until_sent = rfcomm_tty_wait_until_sent, + .tiocmget = rfcomm_tty_tiocmget, + .tiocmset = rfcomm_tty_tiocmset, +}; + +int __init rfcomm_init_ttys(void) +{ + int error; + + rfcomm_tty_driver = alloc_tty_driver(RFCOMM_TTY_PORTS); + if (!rfcomm_tty_driver) + return -ENOMEM; + + rfcomm_tty_driver->driver_name = "rfcomm"; + rfcomm_tty_driver->name = "rfcomm"; + rfcomm_tty_driver->major = RFCOMM_TTY_MAJOR; + rfcomm_tty_driver->minor_start = RFCOMM_TTY_MINOR; + rfcomm_tty_driver->type = TTY_DRIVER_TYPE_SERIAL; + rfcomm_tty_driver->subtype = SERIAL_TYPE_NORMAL; + rfcomm_tty_driver->flags = TTY_DRIVER_REAL_RAW | TTY_DRIVER_DYNAMIC_DEV; + rfcomm_tty_driver->init_termios = tty_std_termios; + rfcomm_tty_driver->init_termios.c_cflag = B9600 | CS8 | CREAD | HUPCL | CLOCAL; + rfcomm_tty_driver->init_termios.c_lflag &= ~ICANON; + tty_set_operations(rfcomm_tty_driver, &rfcomm_ops); + + error = tty_register_driver(rfcomm_tty_driver); + if (error) { + BT_ERR("Can't register RFCOMM TTY driver"); + put_tty_driver(rfcomm_tty_driver); + return error; + } + + BT_INFO("RFCOMM TTY layer initialized"); + + return 0; +} + +void rfcomm_cleanup_ttys(void) +{ + tty_unregister_driver(rfcomm_tty_driver); + put_tty_driver(rfcomm_tty_driver); +} diff --git a/net/bluetooth_tizen/sco.c b/net/bluetooth_tizen/sco.c new file mode 100644 index 0000000..8bf26d1 --- /dev/null +++ b/net/bluetooth_tizen/sco.c @@ -0,0 +1,1061 @@ +/* + BlueZ - Bluetooth protocol stack for Linux + Copyright (C) 2000-2001 Qualcomm Incorporated + + Written 2000,2001 by Maxim Krasnyansky <maxk@qualcomm.com> + + This program is free software; you can redistribute it and/or modify + it under the terms of the GNU General Public License version 2 as + published by the Free Software Foundation; + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS + OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT OF THIRD PARTY RIGHTS. + IN NO EVENT SHALL THE COPYRIGHT HOLDER(S) AND AUTHOR(S) BE LIABLE FOR ANY + CLAIM, OR ANY SPECIAL INDIRECT OR CONSEQUENTIAL DAMAGES, OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + + ALL LIABILITY, INCLUDING LIABILITY FOR INFRINGEMENT OF ANY PATENTS, + COPYRIGHTS, TRADEMARKS OR OTHER RIGHTS, RELATING TO USE OF THIS + SOFTWARE IS DISCLAIMED. +*/ + +/* Bluetooth SCO sockets. */ + +#include <linux/module.h> + +#include <linux/types.h> +#include <linux/errno.h> +#include <linux/kernel.h> +#include <linux/sched.h> +#include <linux/slab.h> +#include <linux/poll.h> +#include <linux/fcntl.h> +#include <linux/init.h> +#include <linux/interrupt.h> +#include <linux/socket.h> +#include <linux/skbuff.h> +#include <linux/device.h> +#include <linux/debugfs.h> +#include <linux/seq_file.h> +#include <linux/list.h> +#include <linux/security.h> +#include <net/sock.h> + +#include <asm/system.h> +#include <linux/uaccess.h> + +#include <net/bluetooth/bluetooth.h> +#include <net/bluetooth/hci_core.h> +#include <net/bluetooth/sco.h> + +static bool disable_esco; + +static const struct proto_ops sco_sock_ops; + +static struct bt_sock_list sco_sk_list = { + .lock = __RW_LOCK_UNLOCKED(sco_sk_list.lock) +}; + +static void __sco_chan_add(struct sco_conn *conn, struct sock *sk, struct sock *parent); +static void sco_chan_del(struct sock *sk, int err); + +static int sco_conn_del(struct hci_conn *conn, int err); + +static void sco_sock_close(struct sock *sk); +static void sco_sock_kill(struct sock *sk); + +/* ---- SCO timers ---- */ +static void sco_sock_timeout(unsigned long arg) +{ + struct sock *sk = (struct sock *) arg; + + BT_DBG("sock %p state %d", sk, sk->sk_state); + + bh_lock_sock(sk); + sk->sk_err = ETIMEDOUT; + sk->sk_state_change(sk); + bh_unlock_sock(sk); + + sco_sock_kill(sk); + sock_put(sk); +} + +static void sco_sock_set_timer(struct sock *sk, long timeout) +{ + BT_DBG("sock %p state %d timeout %ld", sk, sk->sk_state, timeout); + sk_reset_timer(sk, &sk->sk_timer, jiffies + timeout); +} + +static void sco_sock_clear_timer(struct sock *sk) +{ + BT_DBG("sock %p state %d", sk, sk->sk_state); + sk_stop_timer(sk, &sk->sk_timer); +} + +/* ---- SCO connections ---- */ +static struct sco_conn *sco_conn_add(struct hci_conn *hcon, __u8 status) +{ + struct hci_dev *hdev = hcon->hdev; + struct sco_conn *conn = hcon->sco_data; + + if (conn || status) + return conn; + + conn = kzalloc(sizeof(struct sco_conn), GFP_ATOMIC); + if (!conn) + return NULL; + + spin_lock_init(&conn->lock); + + hcon->sco_data = conn; + conn->hcon = hcon; + + conn->src = &hdev->bdaddr; + conn->dst = &hcon->dst; + + if (hdev->sco_mtu > 0) + conn->mtu = hdev->sco_mtu; + else + conn->mtu = 60; + + BT_DBG("hcon %p conn %p", hcon, conn); + + return conn; +} + +static inline struct sock *sco_chan_get(struct sco_conn *conn) +{ + struct sock *sk = NULL; + sco_conn_lock(conn); + sk = conn->sk; + sco_conn_unlock(conn); + return sk; +} + +static int sco_conn_del(struct hci_conn *hcon, int err) +{ + struct sco_conn *conn = hcon->sco_data; + struct sock *sk; + + if (!conn) + return 0; + + BT_DBG("hcon %p conn %p, err %d", hcon, conn, err); + + /* Kill socket */ + sk = sco_chan_get(conn); + if (sk) { + bh_lock_sock(sk); + sco_sock_clear_timer(sk); + sco_chan_del(sk, err); + bh_unlock_sock(sk); + sco_sock_kill(sk); + } + + hcon->sco_data = NULL; + kfree(conn); + return 0; +} + +static inline int sco_chan_add(struct sco_conn *conn, struct sock *sk, struct sock *parent) +{ + int err = 0; + + sco_conn_lock(conn); + if (conn->sk) + err = -EBUSY; + else + __sco_chan_add(conn, sk, parent); + + sco_conn_unlock(conn); + return err; +} + +static int sco_connect(struct sock *sk) +{ + bdaddr_t *src = &bt_sk(sk)->src; + bdaddr_t *dst = &bt_sk(sk)->dst; + struct sco_conn *conn; + struct hci_conn *hcon; + struct hci_dev *hdev; + int err, type; + + BT_DBG("%s -> %s", batostr(src), batostr(dst)); + + hdev = hci_get_route(dst, src); + if (!hdev) + return -EHOSTUNREACH; + + hci_dev_lock(hdev); + + if (lmp_esco_capable(hdev) && !disable_esco) + type = ESCO_LINK; + else + type = SCO_LINK; + + hcon = hci_connect(hdev, type, dst, BT_SECURITY_LOW, HCI_AT_NO_BONDING); + if (IS_ERR(hcon)) { + err = PTR_ERR(hcon); + goto done; + } + + conn = sco_conn_add(hcon, 0); + if (!conn) { + hci_conn_put(hcon); + err = -ENOMEM; + 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->sk_state = BT_CONNECTED; + } else { + sk->sk_state = BT_CONNECT; + sco_sock_set_timer(sk, sk->sk_sndtimeo); + } + +done: + hci_dev_unlock(hdev); + hci_dev_put(hdev); + return err; +} + +static inline int sco_send_frame(struct sock *sk, struct msghdr *msg, int len) +{ + struct sco_conn *conn = sco_pi(sk)->conn; + struct sk_buff *skb; + int err, count; + + /* Check outgoing MTU */ + if (len > conn->mtu) + return -EINVAL; + + BT_DBG("sk %p len %d", sk, len); + + count = min_t(unsigned int, conn->mtu, len); + skb = bt_skb_send_alloc(sk, count, + msg->msg_flags & MSG_DONTWAIT, &err); + if (!skb) + return err; + + if (memcpy_fromiovec(skb_put(skb, count), msg->msg_iov, count)) { + kfree_skb(skb); + return -EFAULT; + } + + hci_send_sco(conn->hcon, skb); + + return count; +} + +static inline void sco_recv_frame(struct sco_conn *conn, struct sk_buff *skb) +{ + struct sock *sk = sco_chan_get(conn); + + if (!sk) + goto drop; + + BT_DBG("sk %p len %d", sk, skb->len); + + if (sk->sk_state != BT_CONNECTED) + goto drop; + + if (!sock_queue_rcv_skb(sk, skb)) + return; + +drop: + kfree_skb(skb); +} + +/* -------- Socket interface ---------- */ +static struct sock *__sco_get_sock_by_addr(bdaddr_t *ba) +{ + struct sock *sk; + struct hlist_node *node; + + sk_for_each(sk, node, &sco_sk_list.head) + if (!bacmp(&bt_sk(sk)->src, ba)) + goto found; + sk = NULL; +found: + return sk; +} + +/* Find socket listening on source bdaddr. + * Returns closest match. + */ +static struct sock *sco_get_sock_listen(bdaddr_t *src) +{ + struct sock *sk = NULL, *sk1 = NULL; + struct hlist_node *node; + + read_lock(&sco_sk_list.lock); + + sk_for_each(sk, node, &sco_sk_list.head) { + if (sk->sk_state != BT_LISTEN) + continue; + + /* Exact match. */ + if (!bacmp(&bt_sk(sk)->src, src)) + break; + + /* Closest match */ + if (!bacmp(&bt_sk(sk)->src, BDADDR_ANY)) + sk1 = sk; + } + + read_unlock(&sco_sk_list.lock); + + return node ? sk : sk1; +} + +static void sco_sock_destruct(struct sock *sk) +{ + BT_DBG("sk %p", sk); + + skb_queue_purge(&sk->sk_receive_queue); + skb_queue_purge(&sk->sk_write_queue); +} + +static void sco_sock_cleanup_listen(struct sock *parent) +{ + struct sock *sk; + + BT_DBG("parent %p", parent); + + /* Close not yet accepted channels */ + while ((sk = bt_accept_dequeue(parent, NULL))) { + sco_sock_close(sk); + sco_sock_kill(sk); + } + + parent->sk_state = BT_CLOSED; + sock_set_flag(parent, SOCK_ZAPPED); +} + +/* Kill socket (only if zapped and orphan) + * Must be called on unlocked socket. + */ +static void sco_sock_kill(struct sock *sk) +{ + if (!sock_flag(sk, SOCK_ZAPPED) || sk->sk_socket) + return; + + BT_DBG("sk %p state %d", sk, sk->sk_state); + + /* Kill poor orphan */ + bt_sock_unlink(&sco_sk_list, sk); + sock_set_flag(sk, SOCK_DEAD); + sock_put(sk); +} + +static void __sco_sock_close(struct sock *sk) +{ + BT_DBG("sk %p state %d socket %p", sk, sk->sk_state, sk->sk_socket); + + switch (sk->sk_state) { + case BT_LISTEN: + sco_sock_cleanup_listen(sk); + break; + + case BT_CONNECTED: + case BT_CONFIG: + if (sco_pi(sk)->conn) { + sk->sk_state = BT_DISCONN; + sco_sock_set_timer(sk, SCO_DISCONN_TIMEOUT); + hci_conn_put(sco_pi(sk)->conn->hcon); + sco_pi(sk)->conn->hcon = NULL; + } else + sco_chan_del(sk, ECONNRESET); + break; + + case BT_CONNECT: + case BT_DISCONN: + sco_chan_del(sk, ECONNRESET); + break; + + default: + sock_set_flag(sk, SOCK_ZAPPED); + break; + } +} + +/* Must be called on unlocked socket. */ +static void sco_sock_close(struct sock *sk) +{ + sco_sock_clear_timer(sk); + lock_sock(sk); + __sco_sock_close(sk); + release_sock(sk); + sco_sock_kill(sk); +} + +static void sco_sock_init(struct sock *sk, struct sock *parent) +{ + BT_DBG("sk %p", sk); + + if (parent) { + sk->sk_type = parent->sk_type; + security_sk_clone(parent, sk); + } +} + +static struct proto sco_proto = { + .name = "SCO", + .owner = THIS_MODULE, + .obj_size = sizeof(struct sco_pinfo) +}; + +static struct sock *sco_sock_alloc(struct net *net, struct socket *sock, int proto, gfp_t prio) +{ + struct sock *sk; + + sk = sk_alloc(net, PF_BLUETOOTH, prio, &sco_proto); + if (!sk) + return NULL; + + sock_init_data(sock, sk); + INIT_LIST_HEAD(&bt_sk(sk)->accept_q); + + sk->sk_destruct = sco_sock_destruct; + sk->sk_sndtimeo = SCO_CONN_TIMEOUT; + + sock_reset_flag(sk, SOCK_ZAPPED); + + sk->sk_protocol = proto; + sk->sk_state = BT_OPEN; + + setup_timer(&sk->sk_timer, sco_sock_timeout, (unsigned long)sk); + + bt_sock_link(&sco_sk_list, sk); + return sk; +} + +static int sco_sock_create(struct net *net, struct socket *sock, int protocol, + int kern) +{ + struct sock *sk; + + BT_DBG("sock %p", sock); + + sock->state = SS_UNCONNECTED; + + if (sock->type != SOCK_SEQPACKET) + return -ESOCKTNOSUPPORT; + + sock->ops = &sco_sock_ops; + + sk = sco_sock_alloc(net, sock, protocol, GFP_ATOMIC); + if (!sk) + return -ENOMEM; + + sco_sock_init(sk, NULL); + return 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->sk_state != BT_OPEN) { + err = -EBADFD; + goto done; + } + + write_lock(&sco_sk_list.lock); + + if (bacmp(src, BDADDR_ANY) && __sco_get_sock_by_addr(src)) { + err = -EADDRINUSE; + } else { + /* Save source address */ + bacpy(&bt_sk(sk)->src, &sa->sco_bdaddr); + sk->sk_state = BT_BOUND; + } + + write_unlock(&sco_sk_list.lock); + +done: + release_sock(sk); + return err; +} + +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 (alen < sizeof(struct sockaddr_sco) || + addr->sa_family != AF_BLUETOOTH) + return -EINVAL; + + if (sk->sk_state != BT_OPEN && sk->sk_state != BT_BOUND) + return -EBADFD; + + if (sk->sk_type != SOCK_SEQPACKET) + return -EINVAL; + + lock_sock(sk); + + /* Set destination address and psm */ + bacpy(&bt_sk(sk)->dst, &sa->sco_bdaddr); + + err = sco_connect(sk); + if (err) + goto done; + + err = bt_sock_wait_state(sk, BT_CONNECTED, + sock_sndtimeo(sk, flags & O_NONBLOCK)); + +done: + release_sock(sk); + return err; +} + +static int sco_sock_listen(struct socket *sock, int backlog) +{ + struct sock *sk = sock->sk; + int err = 0; + + BT_DBG("sk %p backlog %d", sk, backlog); + + lock_sock(sk); + + if (sk->sk_state != BT_BOUND || sock->type != SOCK_SEQPACKET) { + err = -EBADFD; + goto done; + } + + sk->sk_max_ack_backlog = backlog; + sk->sk_ack_backlog = 0; + sk->sk_state = BT_LISTEN; + +done: + release_sock(sk); + return err; +} + +static int sco_sock_accept(struct socket *sock, struct socket *newsock, int flags) +{ + DECLARE_WAITQUEUE(wait, current); + struct sock *sk = sock->sk, *ch; + long timeo; + int err = 0; + + lock_sock(sk); + + timeo = sock_rcvtimeo(sk, flags & O_NONBLOCK); + + BT_DBG("sk %p timeo %ld", sk, timeo); + + /* Wait for an incoming connection. (wake-one). */ + add_wait_queue_exclusive(sk_sleep(sk), &wait); + while (1) { + set_current_state(TASK_INTERRUPTIBLE); + + if (sk->sk_state != BT_LISTEN) { + err = -EBADFD; + break; + } + + ch = bt_accept_dequeue(sk, newsock); + if (ch) + break; + + if (!timeo) { + err = -EAGAIN; + break; + } + + if (signal_pending(current)) { + err = sock_intr_errno(timeo); + break; + } + + release_sock(sk); + timeo = schedule_timeout(timeo); + lock_sock(sk); + } + __set_current_state(TASK_RUNNING); + remove_wait_queue(sk_sleep(sk), &wait); + + if (err) + goto done; + + newsock->state = SS_CONNECTED; + + BT_DBG("new socket %p", ch); + +done: + release_sock(sk); + return err; +} + +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, &bt_sk(sk)->dst); + else + bacpy(&sa->sco_bdaddr, &bt_sk(sk)->src); + + return 0; +} + +static int sco_sock_sendmsg(struct kiocb *iocb, struct socket *sock, + struct msghdr *msg, size_t len) +{ + struct sock *sk = sock->sk; + int err; + + BT_DBG("sock %p, sk %p", sock, sk); + + err = sock_error(sk); + if (err) + return err; + + if (msg->msg_flags & MSG_OOB) + return -EOPNOTSUPP; + + lock_sock(sk); + + if (sk->sk_state == BT_CONNECTED) + err = sco_send_frame(sk, msg, len); + else + err = -ENOTCONN; + + release_sock(sk); + return err; +} + +static int sco_sock_setsockopt(struct socket *sock, int level, int optname, char __user *optval, unsigned int optlen) +{ + struct sock *sk = sock->sk; + int err = 0; + + BT_DBG("sk %p", sk); + + lock_sock(sk); + + switch (optname) { + default: + err = -ENOPROTOOPT; + break; + } + + release_sock(sk); + return err; +} + +static int sco_sock_getsockopt_old(struct socket *sock, int optname, char __user *optval, int __user *optlen) +{ + struct sock *sk = sock->sk; + struct sco_options opts; + struct sco_conninfo cinfo; + int len, err = 0; + + BT_DBG("sk %p", sk); + + if (get_user(len, optlen)) + return -EFAULT; + + lock_sock(sk); + + switch (optname) { + case SCO_OPTIONS: + if (sk->sk_state != BT_CONNECTED) { + err = -ENOTCONN; + break; + } + + opts.mtu = sco_pi(sk)->conn->mtu; + + BT_DBG("mtu %d", opts.mtu); + + len = min_t(unsigned int, len, sizeof(opts)); + if (copy_to_user(optval, (char *)&opts, len)) + err = -EFAULT; + + break; + + case SCO_CONNINFO: + if (sk->sk_state != BT_CONNECTED) { + err = -ENOTCONN; + break; + } + + memset(&cinfo, 0, sizeof(cinfo)); + cinfo.hci_handle = sco_pi(sk)->conn->hcon->handle; + memcpy(cinfo.dev_class, sco_pi(sk)->conn->hcon->dev_class, 3); + + len = min_t(unsigned int, len, sizeof(cinfo)); + if (copy_to_user(optval, (char *)&cinfo, len)) + err = -EFAULT; + + break; + + default: + err = -ENOPROTOOPT; + break; + } + + release_sock(sk); + return err; +} + +static int sco_sock_getsockopt(struct socket *sock, int level, int optname, char __user *optval, int __user *optlen) +{ + struct sock *sk = sock->sk; + int len, err = 0; + + BT_DBG("sk %p", sk); + + if (level == SOL_SCO) + return sco_sock_getsockopt_old(sock, optname, optval, optlen); + + if (get_user(len, optlen)) + return -EFAULT; + + lock_sock(sk); + + switch (optname) { + default: + err = -ENOPROTOOPT; + break; + } + + release_sock(sk); + return err; +} + +static int sco_sock_shutdown(struct socket *sock, int how) +{ + struct sock *sk = sock->sk; + int err = 0; + + BT_DBG("sock %p, sk %p", sock, sk); + + if (!sk) + return 0; + + lock_sock(sk); + if (!sk->sk_shutdown) { + sk->sk_shutdown = SHUTDOWN_MASK; + sco_sock_clear_timer(sk); + __sco_sock_close(sk); + + if (sock_flag(sk, SOCK_LINGER) && sk->sk_lingertime) + err = bt_sock_wait_state(sk, BT_CLOSED, + sk->sk_lingertime); + } + release_sock(sk); + return err; +} + +static int sco_sock_release(struct socket *sock) +{ + struct sock *sk = sock->sk; + int err = 0; + + BT_DBG("sock %p, sk %p", sock, sk); + + if (!sk) + return 0; + + sco_sock_close(sk); + + if (sock_flag(sk, SOCK_LINGER) && sk->sk_lingertime) { + lock_sock(sk); + err = bt_sock_wait_state(sk, BT_CLOSED, sk->sk_lingertime); + release_sock(sk); + } + + sock_orphan(sk); + sco_sock_kill(sk); + return err; +} + +static void __sco_chan_add(struct sco_conn *conn, struct sock *sk, struct sock *parent) +{ + BT_DBG("conn %p", conn); + + sco_pi(sk)->conn = conn; + conn->sk = sk; + + if (parent) + bt_accept_enqueue(parent, sk); +} + +/* Delete channel. + * Must be called on the locked socket. */ +static void sco_chan_del(struct sock *sk, int err) +{ + struct sco_conn *conn; + + conn = sco_pi(sk)->conn; + + BT_DBG("sk %p, conn %p, err %d", sk, conn, err); + + if (conn) { + sco_conn_lock(conn); + conn->sk = NULL; + sco_pi(sk)->conn = NULL; + sco_conn_unlock(conn); + + if (conn->hcon) + hci_conn_put(conn->hcon); + } + + sk->sk_state = BT_CLOSED; + sk->sk_err = err; + sk->sk_state_change(sk); + + sock_set_flag(sk, SOCK_ZAPPED); +} + +static void sco_conn_ready(struct sco_conn *conn) +{ + struct sock *parent; + struct sock *sk = conn->sk; + + BT_DBG("conn %p", conn); + + sco_conn_lock(conn); + + if (sk) { + sco_sock_clear_timer(sk); + bh_lock_sock(sk); + sk->sk_state = BT_CONNECTED; + sk->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(sock_net(parent), NULL, + BTPROTO_SCO, GFP_ATOMIC); + if (!sk) { + bh_unlock_sock(parent); + goto done; + } + + sco_sock_init(sk, parent); + + bacpy(&bt_sk(sk)->src, conn->src); + bacpy(&bt_sk(sk)->dst, conn->dst); + + hci_conn_hold(conn->hcon); + __sco_chan_add(conn, sk, parent); + + sk->sk_state = BT_CONNECTED; + + /* Wake up parent */ + parent->sk_data_ready(parent, 1); + + bh_unlock_sock(parent); + } + +done: + sco_conn_unlock(conn); +} + +/* ----- SCO interface with lower layer (HCI) ----- */ +int sco_connect_ind(struct hci_dev *hdev, bdaddr_t *bdaddr) +{ + register struct sock *sk; + struct hlist_node *node; + int lm = 0; + + BT_DBG("hdev %s, bdaddr %s", hdev->name, batostr(bdaddr)); + + /* Find listening sockets */ + read_lock(&sco_sk_list.lock); + sk_for_each(sk, node, &sco_sk_list.head) { + if (sk->sk_state != BT_LISTEN) + continue; + + if (!bacmp(&bt_sk(sk)->src, &hdev->bdaddr) || + !bacmp(&bt_sk(sk)->src, BDADDR_ANY)) { + lm |= HCI_LM_ACCEPT; + break; + } + } + read_unlock(&sco_sk_list.lock); + + return lm; +} + +int sco_connect_cfm(struct hci_conn *hcon, __u8 status) +{ + BT_DBG("hcon %p bdaddr %s status %d", hcon, batostr(&hcon->dst), status); + if (!status) { + struct sco_conn *conn; + + conn = sco_conn_add(hcon, status); + if (conn) + sco_conn_ready(conn); + } else + sco_conn_del(hcon, bt_to_errno(status)); + + return 0; +} + +int sco_disconn_cfm(struct hci_conn *hcon, __u8 reason) +{ + BT_DBG("hcon %p reason %d", hcon, reason); + + sco_conn_del(hcon, bt_to_errno(reason)); + return 0; +} + +int sco_recv_scodata(struct hci_conn *hcon, struct sk_buff *skb) +{ + struct sco_conn *conn = hcon->sco_data; + + if (!conn) + goto drop; + + BT_DBG("conn %p len %d", conn, skb->len); + + if (skb->len) { + sco_recv_frame(conn, skb); + return 0; + } + +drop: + kfree_skb(skb); + return 0; +} + +static int sco_debugfs_show(struct seq_file *f, void *p) +{ + struct sock *sk; + struct hlist_node *node; + + read_lock(&sco_sk_list.lock); + + sk_for_each(sk, node, &sco_sk_list.head) { + seq_printf(f, "%s %s %d\n", batostr(&bt_sk(sk)->src), + batostr(&bt_sk(sk)->dst), sk->sk_state); + } + + read_unlock(&sco_sk_list.lock); + + return 0; +} + +static int sco_debugfs_open(struct inode *inode, struct file *file) +{ + return single_open(file, sco_debugfs_show, inode->i_private); +} + +static const struct file_operations sco_debugfs_fops = { + .open = sco_debugfs_open, + .read = seq_read, + .llseek = seq_lseek, + .release = single_release, +}; + +static struct dentry *sco_debugfs; + +static const struct proto_ops sco_sock_ops = { + .family = PF_BLUETOOTH, + .owner = THIS_MODULE, + .release = sco_sock_release, + .bind = sco_sock_bind, + .connect = sco_sock_connect, + .listen = sco_sock_listen, + .accept = sco_sock_accept, + .getname = sco_sock_getname, + .sendmsg = sco_sock_sendmsg, + .recvmsg = bt_sock_recvmsg, + .poll = bt_sock_poll, + .ioctl = bt_sock_ioctl, + .mmap = sock_no_mmap, + .socketpair = sock_no_socketpair, + .shutdown = sco_sock_shutdown, + .setsockopt = sco_sock_setsockopt, + .getsockopt = sco_sock_getsockopt +}; + +static const struct net_proto_family sco_sock_family_ops = { + .family = PF_BLUETOOTH, + .owner = THIS_MODULE, + .create = sco_sock_create, +}; + +int __init sco_init(void) +{ + int err; + + err = proto_register(&sco_proto, 0); + if (err < 0) + return err; + + err = bt_sock_register(BTPROTO_SCO, &sco_sock_family_ops); + if (err < 0) { + BT_ERR("SCO socket registration failed"); + goto error; + } + + if (bt_debugfs) { + sco_debugfs = debugfs_create_file("sco", 0444, + bt_debugfs, NULL, &sco_debugfs_fops); + if (!sco_debugfs) + BT_ERR("Failed to create SCO debug file"); + } + + BT_INFO("SCO socket layer initialized"); + + return 0; + +error: + proto_unregister(&sco_proto); + return err; +} + +void __exit sco_exit(void) +{ + debugfs_remove(sco_debugfs); + + if (bt_sock_unregister(BTPROTO_SCO) < 0) + BT_ERR("SCO socket unregistration failed"); + + proto_unregister(&sco_proto); +} + +module_param(disable_esco, bool, 0644); +MODULE_PARM_DESC(disable_esco, "Disable eSCO connection creation"); diff --git a/net/bluetooth_tizen/smp.c b/net/bluetooth_tizen/smp.c new file mode 100644 index 0000000..deb1198 --- /dev/null +++ b/net/bluetooth_tizen/smp.c @@ -0,0 +1,1003 @@ +/* + BlueZ - Bluetooth protocol stack for Linux + Copyright (C) 2011 Nokia Corporation and/or its subsidiary(-ies). + + This program is free software; you can redistribute it and/or modify + it under the terms of the GNU General Public License version 2 as + published by the Free Software Foundation; + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS + OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT OF THIRD PARTY RIGHTS. + IN NO EVENT SHALL THE COPYRIGHT HOLDER(S) AND AUTHOR(S) BE LIABLE FOR ANY + CLAIM, OR ANY SPECIAL INDIRECT OR CONSEQUENTIAL DAMAGES, OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + + ALL LIABILITY, INCLUDING LIABILITY FOR INFRINGEMENT OF ANY PATENTS, + COPYRIGHTS, TRADEMARKS OR OTHER RIGHTS, RELATING TO USE OF THIS + SOFTWARE IS DISCLAIMED. +*/ + +#include <net/bluetooth/bluetooth.h> +#include <net/bluetooth/hci_core.h> +#include <net/bluetooth/l2cap.h> +#include <net/bluetooth/mgmt.h> +#include <net/bluetooth/smp.h> +#include <linux/crypto.h> +#include <linux/scatterlist.h> +#include <crypto/b128ops.h> + +#define SMP_TIMEOUT msecs_to_jiffies(30000) + +static inline void swap128(u8 src[16], u8 dst[16]) +{ + int i; + for (i = 0; i < 16; i++) + dst[15 - i] = src[i]; +} + +static inline void swap56(u8 src[7], u8 dst[7]) +{ + int i; + for (i = 0; i < 7; i++) + dst[6 - i] = src[i]; +} + +static int smp_e(struct crypto_blkcipher *tfm, const u8 *k, u8 *r) +{ + struct blkcipher_desc desc; + struct scatterlist sg; + int err, iv_len; + unsigned char iv[128]; + + if (tfm == NULL) { + BT_ERR("tfm %p", tfm); + return -EINVAL; + } + + desc.tfm = tfm; + desc.flags = 0; + + err = crypto_blkcipher_setkey(tfm, k, 16); + if (err) { + BT_ERR("cipher setkey failed: %d", err); + return err; + } + + sg_init_one(&sg, r, 16); + + iv_len = crypto_blkcipher_ivsize(tfm); + if (iv_len) { + memset(&iv, 0xff, iv_len); + crypto_blkcipher_set_iv(tfm, iv, iv_len); + } + + err = crypto_blkcipher_encrypt(&desc, &sg, &sg, 16); + if (err) + BT_ERR("Encrypt data error %d", err); + + return err; +} + +static int smp_c1(struct crypto_blkcipher *tfm, u8 k[16], u8 r[16], + u8 preq[7], u8 pres[7], u8 _iat, bdaddr_t *ia, + u8 _rat, bdaddr_t *ra, u8 res[16]) +{ + u8 p1[16], p2[16]; + int err; + + memset(p1, 0, 16); + + /* p1 = pres || preq || _rat || _iat */ + swap56(pres, p1); + swap56(preq, p1 + 7); + p1[14] = _rat; + p1[15] = _iat; + + memset(p2, 0, 16); + + /* p2 = padding || ia || ra */ + baswap((bdaddr_t *) (p2 + 4), ia); + baswap((bdaddr_t *) (p2 + 10), ra); + + /* res = r XOR p1 */ + u128_xor((u128 *) res, (u128 *) r, (u128 *) p1); + + /* res = e(k, res) */ + err = smp_e(tfm, k, res); + if (err) { + BT_ERR("Encrypt data error"); + return err; + } + + /* res = res XOR p2 */ + u128_xor((u128 *) res, (u128 *) res, (u128 *) p2); + + /* res = e(k, res) */ + err = smp_e(tfm, k, res); + if (err) + BT_ERR("Encrypt data error"); + + return err; +} + +static int smp_s1(struct crypto_blkcipher *tfm, u8 k[16], + u8 r1[16], u8 r2[16], u8 _r[16]) +{ + int err; + + /* Just least significant octets from r1 and r2 are considered */ + memcpy(_r, r1 + 8, 8); + memcpy(_r + 8, r2 + 8, 8); + + err = smp_e(tfm, k, _r); + if (err) + BT_ERR("Encrypt data error"); + + return err; +} + +static int smp_rand(u8 *buf) +{ + get_random_bytes(buf, 16); + + return 0; +} + +static struct sk_buff *smp_build_cmd(struct l2cap_conn *conn, u8 code, + u16 dlen, void *data) +{ + struct sk_buff *skb; + struct l2cap_hdr *lh; + int len; + + len = L2CAP_HDR_SIZE + sizeof(code) + dlen; + + if (len > conn->mtu) + return NULL; + + skb = bt_skb_alloc(len, GFP_ATOMIC); + if (!skb) + return NULL; + + lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE); + lh->len = cpu_to_le16(sizeof(code) + dlen); + lh->cid = cpu_to_le16(L2CAP_CID_SMP); + + memcpy(skb_put(skb, sizeof(code)), &code, sizeof(code)); + + memcpy(skb_put(skb, dlen), data, dlen); + + return skb; +} + +static void smp_send_cmd(struct l2cap_conn *conn, u8 code, u16 len, void *data) +{ + struct sk_buff *skb = smp_build_cmd(conn, code, len, data); + + BT_DBG("code 0x%2.2x", code); + + if (!skb) + return; + + skb->priority = HCI_PRIO_MAX; + hci_send_acl(conn->hchan, skb, 0); + + cancel_delayed_work_sync(&conn->security_timer); + schedule_delayed_work(&conn->security_timer, SMP_TIMEOUT); +} + +static __u8 authreq_to_seclevel(__u8 authreq) +{ + if (authreq & SMP_AUTH_MITM) + return BT_SECURITY_HIGH; + else + return BT_SECURITY_MEDIUM; +} + +static __u8 seclevel_to_authreq(__u8 sec_level) +{ + switch (sec_level) { + case BT_SECURITY_HIGH: + return SMP_AUTH_MITM | SMP_AUTH_BONDING; + case BT_SECURITY_MEDIUM: + return SMP_AUTH_BONDING; + default: + return SMP_AUTH_NONE; + } +} + +static void build_pairing_cmd(struct l2cap_conn *conn, + struct smp_cmd_pairing *req, + struct smp_cmd_pairing *rsp, + __u8 authreq) +{ + u8 dist_keys = 0; + + if (test_bit(HCI_PAIRABLE, &conn->hcon->hdev->dev_flags)) { + dist_keys = SMP_DIST_ENC_KEY; + authreq |= SMP_AUTH_BONDING; + } else { + authreq &= ~SMP_AUTH_BONDING; + } + + if (rsp == NULL) { + req->io_capability = conn->hcon->io_capability; + req->oob_flag = SMP_OOB_NOT_PRESENT; + req->max_key_size = SMP_MAX_ENC_KEY_SIZE; + req->init_key_dist = 0; + req->resp_key_dist = dist_keys; + req->auth_req = authreq; + return; + } + + rsp->io_capability = conn->hcon->io_capability; + rsp->oob_flag = SMP_OOB_NOT_PRESENT; + rsp->max_key_size = SMP_MAX_ENC_KEY_SIZE; + rsp->init_key_dist = 0; + rsp->resp_key_dist = req->resp_key_dist & dist_keys; + rsp->auth_req = authreq; +} + +static u8 check_enc_key_size(struct l2cap_conn *conn, __u8 max_key_size) +{ + struct smp_chan *smp = conn->smp_chan; + + if ((max_key_size > SMP_MAX_ENC_KEY_SIZE) || + (max_key_size < SMP_MIN_ENC_KEY_SIZE)) + return SMP_ENC_KEY_SIZE; + + smp->enc_key_size = max_key_size; + + return 0; +} + +static void smp_failure(struct l2cap_conn *conn, u8 reason, u8 send) +{ + struct hci_conn *hcon = conn->hcon; + + if (send) + smp_send_cmd(conn, SMP_CMD_PAIRING_FAIL, sizeof(reason), + &reason); + + clear_bit(HCI_CONN_ENCRYPT_PEND, &conn->hcon->flags); + mgmt_auth_failed(conn->hcon->hdev, conn->dst, hcon->type, + hcon->dst_type, reason); + + if (test_and_clear_bit(HCI_CONN_LE_SMP_PEND, &conn->hcon->flags)) { + cancel_delayed_work_sync(&conn->security_timer); + smp_chan_destroy(conn); + } +} + +#define JUST_WORKS 0x00 +#define JUST_CFM 0x01 +#define REQ_PASSKEY 0x02 +#define CFM_PASSKEY 0x03 +#define REQ_OOB 0x04 +#define OVERLAP 0xFF + +static const u8 gen_method[5][5] = { + { JUST_WORKS, JUST_CFM, REQ_PASSKEY, JUST_WORKS, REQ_PASSKEY }, + { JUST_WORKS, JUST_CFM, REQ_PASSKEY, JUST_WORKS, REQ_PASSKEY }, + { CFM_PASSKEY, CFM_PASSKEY, REQ_PASSKEY, JUST_WORKS, CFM_PASSKEY }, + { JUST_WORKS, JUST_CFM, JUST_WORKS, JUST_WORKS, JUST_CFM }, + { CFM_PASSKEY, CFM_PASSKEY, REQ_PASSKEY, JUST_WORKS, OVERLAP }, +}; + +static int tk_request(struct l2cap_conn *conn, u8 remote_oob, u8 auth, + u8 local_io, u8 remote_io) +{ + struct hci_conn *hcon = conn->hcon; + struct smp_chan *smp = conn->smp_chan; + u8 method; + u32 passkey = 0; + int ret = 0; + + /* Initialize key for JUST WORKS */ + memset(smp->tk, 0, sizeof(smp->tk)); + clear_bit(SMP_FLAG_TK_VALID, &smp->smp_flags); + + BT_DBG("tk_request: auth:%d lcl:%d rem:%d", auth, local_io, remote_io); + + /* If neither side wants MITM, use JUST WORKS */ + /* If either side has unknown io_caps, use JUST WORKS */ + /* Otherwise, look up method from the table */ + if (!(auth & SMP_AUTH_MITM) || + local_io > SMP_IO_KEYBOARD_DISPLAY || + remote_io > SMP_IO_KEYBOARD_DISPLAY) + method = JUST_WORKS; + else + method = gen_method[remote_io][local_io]; + + /* If not bonding, don't ask user to confirm a Zero TK */ + if (!(auth & SMP_AUTH_BONDING) && method == JUST_CFM) + method = JUST_WORKS; + + /* If Just Works, Continue with Zero TK */ + if (method == JUST_WORKS) { + set_bit(SMP_FLAG_TK_VALID, &smp->smp_flags); + return 0; + } + + /* Not Just Works/Confirm results in MITM Authentication */ + if (method != JUST_CFM) + set_bit(SMP_FLAG_MITM_AUTH, &smp->smp_flags); + + /* If both devices have Keyoard-Display I/O, the master + * Confirms and the slave Enters the passkey. + */ + if (method == OVERLAP) { + if (hcon->link_mode & HCI_LM_MASTER) + method = CFM_PASSKEY; + else + method = REQ_PASSKEY; + } + + /* Generate random passkey. Not valid until confirmed. */ + if (method == CFM_PASSKEY) { + u8 key[16]; + + memset(key, 0, sizeof(key)); + get_random_bytes(&passkey, sizeof(passkey)); + passkey %= 1000000; + put_unaligned_le32(passkey, key); + swap128(key, smp->tk); + BT_DBG("PassKey: %d", passkey); + } + + hci_dev_lock(hcon->hdev); + + if (method == REQ_PASSKEY) + ret = mgmt_user_passkey_request(hcon->hdev, conn->dst, + hcon->type, hcon->dst_type); + else + ret = mgmt_user_confirm_request(hcon->hdev, conn->dst, + hcon->type, hcon->dst_type, + cpu_to_le32(passkey), 0); + + hci_dev_unlock(hcon->hdev); + + return ret; +} + +static void confirm_work(struct work_struct *work) +{ + struct smp_chan *smp = container_of(work, struct smp_chan, confirm); + struct l2cap_conn *conn = smp->conn; + struct crypto_blkcipher *tfm; + struct smp_cmd_pairing_confirm cp; + int ret; + u8 res[16], reason; + + BT_DBG("conn %p", conn); + + tfm = crypto_alloc_blkcipher("ecb(aes)", 0, CRYPTO_ALG_ASYNC); + if (IS_ERR(tfm)) { + reason = SMP_UNSPECIFIED; + goto error; + } + + smp->tfm = tfm; + + if (conn->hcon->out) + ret = smp_c1(tfm, smp->tk, smp->prnd, smp->preq, smp->prsp, 0, + conn->src, conn->hcon->dst_type, conn->dst, res); + else + ret = smp_c1(tfm, smp->tk, smp->prnd, smp->preq, smp->prsp, + conn->hcon->dst_type, conn->dst, 0, conn->src, + res); + if (ret) { + reason = SMP_UNSPECIFIED; + goto error; + } + + clear_bit(SMP_FLAG_CFM_PENDING, &smp->smp_flags); + + swap128(res, cp.confirm_val); + smp_send_cmd(smp->conn, SMP_CMD_PAIRING_CONFIRM, sizeof(cp), &cp); + + return; + +error: + smp_failure(conn, reason, 1); +} + +static void random_work(struct work_struct *work) +{ + struct smp_chan *smp = container_of(work, struct smp_chan, random); + struct l2cap_conn *conn = smp->conn; + struct hci_conn *hcon = conn->hcon; + struct crypto_blkcipher *tfm = smp->tfm; + u8 reason, confirm[16], res[16], key[16]; + int ret; + + if (IS_ERR_OR_NULL(tfm)) { + reason = SMP_UNSPECIFIED; + goto error; + } + + BT_DBG("conn %p %s", conn, conn->hcon->out ? "master" : "slave"); + + if (hcon->out) + ret = smp_c1(tfm, smp->tk, smp->rrnd, smp->preq, smp->prsp, 0, + conn->src, hcon->dst_type, conn->dst, res); + else + ret = smp_c1(tfm, smp->tk, smp->rrnd, smp->preq, smp->prsp, + hcon->dst_type, conn->dst, 0, conn->src, res); + if (ret) { + reason = SMP_UNSPECIFIED; + goto error; + } + + swap128(res, confirm); + + if (memcmp(smp->pcnf, confirm, sizeof(smp->pcnf)) != 0) { + BT_ERR("Pairing failed (confirmation values mismatch)"); + reason = SMP_CONFIRM_FAILED; + goto error; + } + + if (hcon->out) { + u8 stk[16], rand[8]; + __le16 ediv; + + memset(rand, 0, sizeof(rand)); + ediv = 0; + + smp_s1(tfm, smp->tk, smp->rrnd, smp->prnd, key); + swap128(key, stk); + + memset(stk + smp->enc_key_size, 0, + SMP_MAX_ENC_KEY_SIZE - smp->enc_key_size); + + if (test_and_set_bit(HCI_CONN_ENCRYPT_PEND, &hcon->flags)) { + reason = SMP_UNSPECIFIED; + goto error; + } + + hci_le_start_enc(hcon, ediv, rand, stk); + hcon->enc_key_size = smp->enc_key_size; + } else { + u8 stk[16], r[16], rand[8]; + __le16 ediv; + + memset(rand, 0, sizeof(rand)); + ediv = 0; + + swap128(smp->prnd, r); + smp_send_cmd(conn, SMP_CMD_PAIRING_RANDOM, sizeof(r), r); + + smp_s1(tfm, smp->tk, smp->prnd, smp->rrnd, key); + swap128(key, stk); + + memset(stk + smp->enc_key_size, 0, + SMP_MAX_ENC_KEY_SIZE - smp->enc_key_size); + + hci_add_ltk(hcon->hdev, conn->dst, hcon->dst_type, + HCI_SMP_STK_SLAVE, 0, 0, stk, smp->enc_key_size, + ediv, rand); + } + + return; + +error: + smp_failure(conn, reason, 1); +} + +static struct smp_chan *smp_chan_create(struct l2cap_conn *conn) +{ + struct smp_chan *smp; + + smp = kzalloc(sizeof(struct smp_chan), GFP_ATOMIC); + if (!smp) + return NULL; + + INIT_WORK(&smp->confirm, confirm_work); + INIT_WORK(&smp->random, random_work); + + smp->conn = conn; + conn->smp_chan = smp; + conn->hcon->smp_conn = conn; + + hci_conn_hold(conn->hcon); + + return smp; +} + +void smp_chan_destroy(struct l2cap_conn *conn) +{ + struct smp_chan *smp = conn->smp_chan; + + BUG_ON(!smp); + + if (smp->tfm) + crypto_free_blkcipher(smp->tfm); + + kfree(smp); + conn->smp_chan = NULL; + conn->hcon->smp_conn = NULL; + hci_conn_put(conn->hcon); +} + +int smp_user_confirm_reply(struct hci_conn *hcon, u16 mgmt_op, __le32 passkey) +{ + struct l2cap_conn *conn = hcon->smp_conn; + struct smp_chan *smp; + u32 value; + u8 key[16]; + + BT_DBG(""); + + if (!conn) + return -ENOTCONN; + + smp = conn->smp_chan; + + switch (mgmt_op) { + case MGMT_OP_USER_PASSKEY_REPLY: + value = le32_to_cpu(passkey); + memset(key, 0, sizeof(key)); + BT_DBG("PassKey: %d", value); + put_unaligned_le32(value, key); + swap128(key, smp->tk); + /* Fall Through */ + case MGMT_OP_USER_CONFIRM_REPLY: + set_bit(SMP_FLAG_TK_VALID, &smp->smp_flags); + break; + case MGMT_OP_USER_PASSKEY_NEG_REPLY: + case MGMT_OP_USER_CONFIRM_NEG_REPLY: + smp_failure(conn, SMP_PASSKEY_ENTRY_FAILED, 1); + return 0; + default: + smp_failure(conn, SMP_PASSKEY_ENTRY_FAILED, 1); + return -EOPNOTSUPP; + } + + /* If it is our turn to send Pairing Confirm, do so now */ + if (test_bit(SMP_FLAG_CFM_PENDING, &smp->smp_flags)) + queue_work(hcon->hdev->workqueue, &smp->confirm); + + return 0; +} + +static u8 smp_cmd_pairing_req(struct l2cap_conn *conn, struct sk_buff *skb) +{ + struct smp_cmd_pairing rsp, *req = (void *) skb->data; + struct smp_chan *smp; + u8 key_size; + u8 auth = SMP_AUTH_NONE; + int ret; + + BT_DBG("conn %p", conn); + + if (conn->hcon->link_mode & HCI_LM_MASTER) + return SMP_CMD_NOTSUPP; + + if (!test_and_set_bit(HCI_CONN_LE_SMP_PEND, &conn->hcon->flags)) + smp = smp_chan_create(conn); + + smp = conn->smp_chan; + + smp->preq[0] = SMP_CMD_PAIRING_REQ; + memcpy(&smp->preq[1], req, sizeof(*req)); + skb_pull(skb, sizeof(*req)); + + /* We didn't start the pairing, so match remote */ + if (req->auth_req & SMP_AUTH_BONDING) + auth = req->auth_req; + + conn->hcon->pending_sec_level = authreq_to_seclevel(auth); + + build_pairing_cmd(conn, req, &rsp, auth); + + key_size = min(req->max_key_size, rsp.max_key_size); + if (check_enc_key_size(conn, key_size)) + return SMP_ENC_KEY_SIZE; + + ret = smp_rand(smp->prnd); + if (ret) + return SMP_UNSPECIFIED; + + smp->prsp[0] = SMP_CMD_PAIRING_RSP; + memcpy(&smp->prsp[1], &rsp, sizeof(rsp)); + + smp_send_cmd(conn, SMP_CMD_PAIRING_RSP, sizeof(rsp), &rsp); + + /* Request setup of TK */ + ret = tk_request(conn, 0, auth, rsp.io_capability, req->io_capability); + if (ret) + return SMP_UNSPECIFIED; + + return 0; +} + +static u8 smp_cmd_pairing_rsp(struct l2cap_conn *conn, struct sk_buff *skb) +{ + struct smp_cmd_pairing *req, *rsp = (void *) skb->data; + struct smp_chan *smp = conn->smp_chan; + struct hci_dev *hdev = conn->hcon->hdev; + u8 key_size, auth = SMP_AUTH_NONE; + int ret; + + BT_DBG("conn %p", conn); + + if (!(conn->hcon->link_mode & HCI_LM_MASTER)) + return SMP_CMD_NOTSUPP; + + skb_pull(skb, sizeof(*rsp)); + + req = (void *) &smp->preq[1]; + + key_size = min(req->max_key_size, rsp->max_key_size); + if (check_enc_key_size(conn, key_size)) + return SMP_ENC_KEY_SIZE; + + ret = smp_rand(smp->prnd); + if (ret) + return SMP_UNSPECIFIED; + + smp->prsp[0] = SMP_CMD_PAIRING_RSP; + memcpy(&smp->prsp[1], rsp, sizeof(*rsp)); + + if ((req->auth_req & SMP_AUTH_BONDING) && + (rsp->auth_req & SMP_AUTH_BONDING)) + auth = SMP_AUTH_BONDING; + + auth |= (req->auth_req | rsp->auth_req) & SMP_AUTH_MITM; + + ret = tk_request(conn, 0, auth, rsp->io_capability, req->io_capability); + if (ret) + return SMP_UNSPECIFIED; + + set_bit(SMP_FLAG_CFM_PENDING, &smp->smp_flags); + + /* Can't compose response until we have been confirmed */ + if (!test_bit(SMP_FLAG_TK_VALID, &smp->smp_flags)) + return 0; + + queue_work(hdev->workqueue, &smp->confirm); + + return 0; +} + +static u8 smp_cmd_pairing_confirm(struct l2cap_conn *conn, struct sk_buff *skb) +{ + struct smp_chan *smp = conn->smp_chan; + struct hci_dev *hdev = conn->hcon->hdev; + + BT_DBG("conn %p %s", conn, conn->hcon->out ? "master" : "slave"); + + memcpy(smp->pcnf, skb->data, sizeof(smp->pcnf)); + skb_pull(skb, sizeof(smp->pcnf)); + + if (conn->hcon->out) { + u8 random[16]; + + swap128(smp->prnd, random); + smp_send_cmd(conn, SMP_CMD_PAIRING_RANDOM, sizeof(random), + random); + } else if (test_bit(SMP_FLAG_TK_VALID, &smp->smp_flags)) { + queue_work(hdev->workqueue, &smp->confirm); + } else { + set_bit(SMP_FLAG_CFM_PENDING, &smp->smp_flags); + } + + return 0; +} + +static u8 smp_cmd_pairing_random(struct l2cap_conn *conn, struct sk_buff *skb) +{ + struct smp_chan *smp = conn->smp_chan; + struct hci_dev *hdev = conn->hcon->hdev; + + BT_DBG("conn %p", conn); + + swap128(skb->data, smp->rrnd); + skb_pull(skb, sizeof(smp->rrnd)); + + queue_work(hdev->workqueue, &smp->random); + + return 0; +} + +static u8 smp_ltk_encrypt(struct l2cap_conn *conn) +{ + struct smp_ltk *key; + struct hci_conn *hcon = conn->hcon; + + key = hci_find_ltk_by_addr(hcon->hdev, conn->dst, hcon->dst_type); + if (!key) + return 0; + + if (test_and_set_bit(HCI_CONN_ENCRYPT_PEND, &hcon->flags)) + return 1; + + hci_le_start_enc(hcon, key->ediv, key->rand, key->val); + hcon->enc_key_size = key->enc_size; + + return 1; + +} +static u8 smp_cmd_security_req(struct l2cap_conn *conn, struct sk_buff *skb) +{ + struct smp_cmd_security_req *rp = (void *) skb->data; + struct smp_cmd_pairing cp; + struct hci_conn *hcon = conn->hcon; + struct smp_chan *smp; + + BT_DBG("conn %p", conn); + + hcon->pending_sec_level = authreq_to_seclevel(rp->auth_req); + + if (smp_ltk_encrypt(conn)) + return 0; + + if (test_and_set_bit(HCI_CONN_LE_SMP_PEND, &hcon->flags)) + return 0; + + smp = smp_chan_create(conn); + + skb_pull(skb, sizeof(*rp)); + + memset(&cp, 0, sizeof(cp)); + build_pairing_cmd(conn, &cp, NULL, rp->auth_req); + + smp->preq[0] = SMP_CMD_PAIRING_REQ; + memcpy(&smp->preq[1], &cp, sizeof(cp)); + + smp_send_cmd(conn, SMP_CMD_PAIRING_REQ, sizeof(cp), &cp); + + return 0; +} + +int smp_conn_security(struct l2cap_conn *conn, __u8 sec_level) +{ + struct hci_conn *hcon = conn->hcon; + struct smp_chan *smp = conn->smp_chan; + __u8 authreq; + + BT_DBG("conn %p hcon %p level 0x%2.2x", conn, hcon, sec_level); + + if (!lmp_host_le_capable(hcon->hdev)) + return 1; + + if (sec_level == BT_SECURITY_LOW) + return 1; + + if (hcon->sec_level >= sec_level) + return 1; + + if (hcon->link_mode & HCI_LM_MASTER) + if (smp_ltk_encrypt(conn)) + goto done; + + if (test_and_set_bit(HCI_CONN_LE_SMP_PEND, &hcon->flags)) + return 0; + + smp = smp_chan_create(conn); + if (!smp) + return 1; + + authreq = seclevel_to_authreq(sec_level); + + if (hcon->link_mode & HCI_LM_MASTER) { + struct smp_cmd_pairing cp; + + build_pairing_cmd(conn, &cp, NULL, authreq); + smp->preq[0] = SMP_CMD_PAIRING_REQ; + memcpy(&smp->preq[1], &cp, sizeof(cp)); + + smp_send_cmd(conn, SMP_CMD_PAIRING_REQ, sizeof(cp), &cp); + } else { + struct smp_cmd_security_req cp; + cp.auth_req = authreq; + smp_send_cmd(conn, SMP_CMD_SECURITY_REQ, sizeof(cp), &cp); + } + +done: + hcon->pending_sec_level = sec_level; + + return 0; +} + +static int smp_cmd_encrypt_info(struct l2cap_conn *conn, struct sk_buff *skb) +{ + struct smp_cmd_encrypt_info *rp = (void *) skb->data; + struct smp_chan *smp = conn->smp_chan; + + skb_pull(skb, sizeof(*rp)); + + memcpy(smp->tk, rp->ltk, sizeof(smp->tk)); + + return 0; +} + +static int smp_cmd_master_ident(struct l2cap_conn *conn, struct sk_buff *skb) +{ + struct smp_cmd_master_ident *rp = (void *) skb->data; + struct smp_chan *smp = conn->smp_chan; + struct hci_dev *hdev = conn->hcon->hdev; + struct hci_conn *hcon = conn->hcon; + u8 authenticated; + + skb_pull(skb, sizeof(*rp)); + + hci_dev_lock(hdev); + authenticated = (conn->hcon->sec_level == BT_SECURITY_HIGH); + hci_add_ltk(conn->hcon->hdev, conn->dst, hcon->dst_type, + HCI_SMP_LTK, 1, authenticated, smp->tk, smp->enc_key_size, + rp->ediv, rp->rand); + smp_distribute_keys(conn, 1); + hci_dev_unlock(hdev); + + return 0; +} + +int smp_sig_channel(struct l2cap_conn *conn, struct sk_buff *skb) +{ + __u8 code = skb->data[0]; + __u8 reason; + int err = 0; + + if (!lmp_host_le_capable(conn->hcon->hdev)) { + err = -ENOTSUPP; + reason = SMP_PAIRING_NOTSUPP; + goto done; + } + + skb_pull(skb, sizeof(code)); + + switch (code) { + case SMP_CMD_PAIRING_REQ: + reason = smp_cmd_pairing_req(conn, skb); + break; + + case SMP_CMD_PAIRING_FAIL: + smp_failure(conn, skb->data[0], 0); + reason = 0; + err = -EPERM; + break; + + case SMP_CMD_PAIRING_RSP: + reason = smp_cmd_pairing_rsp(conn, skb); + break; + + case SMP_CMD_SECURITY_REQ: + reason = smp_cmd_security_req(conn, skb); + break; + + case SMP_CMD_PAIRING_CONFIRM: + reason = smp_cmd_pairing_confirm(conn, skb); + break; + + case SMP_CMD_PAIRING_RANDOM: + reason = smp_cmd_pairing_random(conn, skb); + break; + + case SMP_CMD_ENCRYPT_INFO: + reason = smp_cmd_encrypt_info(conn, skb); + break; + + case SMP_CMD_MASTER_IDENT: + reason = smp_cmd_master_ident(conn, skb); + break; + + case SMP_CMD_IDENT_INFO: + case SMP_CMD_IDENT_ADDR_INFO: + case SMP_CMD_SIGN_INFO: + /* Just ignored */ + reason = 0; + break; + + default: + BT_DBG("Unknown command code 0x%2.2x", code); + + reason = SMP_CMD_NOTSUPP; + err = -EOPNOTSUPP; + goto done; + } + +done: + if (reason) + smp_failure(conn, reason, 1); + + kfree_skb(skb); + return err; +} + +int smp_distribute_keys(struct l2cap_conn *conn, __u8 force) +{ + struct smp_cmd_pairing *req, *rsp; + struct smp_chan *smp = conn->smp_chan; + __u8 *keydist; + + BT_DBG("conn %p force %d", conn, force); + + if (!test_bit(HCI_CONN_LE_SMP_PEND, &conn->hcon->flags)) + return 0; + + rsp = (void *) &smp->prsp[1]; + + /* The responder sends its keys first */ + if (!force && conn->hcon->out && (rsp->resp_key_dist & 0x07)) + return 0; + + req = (void *) &smp->preq[1]; + + if (conn->hcon->out) { + keydist = &rsp->init_key_dist; + *keydist &= req->init_key_dist; + } else { + keydist = &rsp->resp_key_dist; + *keydist &= req->resp_key_dist; + } + + + BT_DBG("keydist 0x%x", *keydist); + + if (*keydist & SMP_DIST_ENC_KEY) { + struct smp_cmd_encrypt_info enc; + struct smp_cmd_master_ident ident; + struct hci_conn *hcon = conn->hcon; + u8 authenticated; + __le16 ediv; + + get_random_bytes(enc.ltk, sizeof(enc.ltk)); + get_random_bytes(&ediv, sizeof(ediv)); + get_random_bytes(ident.rand, sizeof(ident.rand)); + + smp_send_cmd(conn, SMP_CMD_ENCRYPT_INFO, sizeof(enc), &enc); + + authenticated = hcon->sec_level == BT_SECURITY_HIGH; + hci_add_ltk(conn->hcon->hdev, conn->dst, hcon->dst_type, + HCI_SMP_LTK_SLAVE, 1, authenticated, + enc.ltk, smp->enc_key_size, ediv, ident.rand); + + ident.ediv = cpu_to_le16(ediv); + + smp_send_cmd(conn, SMP_CMD_MASTER_IDENT, sizeof(ident), &ident); + + *keydist &= ~SMP_DIST_ENC_KEY; + } + + if (*keydist & SMP_DIST_ID_KEY) { + struct smp_cmd_ident_addr_info addrinfo; + struct smp_cmd_ident_info idinfo; + + /* Send a dummy key */ + get_random_bytes(idinfo.irk, sizeof(idinfo.irk)); + + smp_send_cmd(conn, SMP_CMD_IDENT_INFO, sizeof(idinfo), &idinfo); + + /* Just public address */ + memset(&addrinfo, 0, sizeof(addrinfo)); + bacpy(&addrinfo.bdaddr, conn->src); + + smp_send_cmd(conn, SMP_CMD_IDENT_ADDR_INFO, sizeof(addrinfo), + &addrinfo); + + *keydist &= ~SMP_DIST_ID_KEY; + } + + if (*keydist & SMP_DIST_SIGN) { + struct smp_cmd_sign_info sign; + + /* Send a dummy key */ + get_random_bytes(sign.csrk, sizeof(sign.csrk)); + + smp_send_cmd(conn, SMP_CMD_SIGN_INFO, sizeof(sign), &sign); + + *keydist &= ~SMP_DIST_SIGN; + } + + if (conn->hcon->out || force) { + clear_bit(HCI_CONN_LE_SMP_PEND, &conn->hcon->flags); + cancel_delayed_work_sync(&conn->security_timer); + smp_chan_destroy(conn); + } + + return 0; +} diff --git a/net/core/skbuff.c b/net/core/skbuff.c index 4821df8..63fa02e 100644 --- a/net/core/skbuff.c +++ b/net/core/skbuff.c @@ -697,7 +697,7 @@ struct sk_buff *skb_copy(const struct sk_buff *skb, gfp_t gfp_mask) EXPORT_SYMBOL(skb_copy); /** - * pskb_copy - create copy of an sk_buff with private head. + * __pskb_copy - create copy of an sk_buff with private head. * @skb: buffer to copy * @gfp_mask: allocation priority * @@ -709,16 +709,16 @@ EXPORT_SYMBOL(skb_copy); * The returned buffer has a reference count of 1. */ -struct sk_buff *pskb_copy(struct sk_buff *skb, gfp_t gfp_mask) +struct sk_buff *__pskb_copy(struct sk_buff *skb, int headroom, gfp_t gfp_mask) { - unsigned int size = skb_end_pointer(skb) - skb->head; + unsigned int size = skb_headlen(skb) + headroom; struct sk_buff *n = alloc_skb(size, gfp_mask); if (!n) goto out; /* Set the data pointer */ - skb_reserve(n, skb_headroom(skb)); + skb_reserve(n, headroom); /* Set the tail pointer and length */ skb_put(n, skb_headlen(skb)); /* Copy the bytes */ @@ -747,7 +747,7 @@ struct sk_buff *pskb_copy(struct sk_buff *skb, gfp_t gfp_mask) out: return n; } -EXPORT_SYMBOL(pskb_copy); +EXPORT_SYMBOL(__pskb_copy); /** * pskb_expand_head - reallocate header of &sk_buff diff --git a/net/ipv4/tcp_output.c b/net/ipv4/tcp_output.c index faf257b..b764e2a 100644 --- a/net/ipv4/tcp_output.c +++ b/net/ipv4/tcp_output.c @@ -2138,7 +2138,16 @@ int tcp_retransmit_skb(struct sock *sk, struct sk_buff *skb) */ TCP_SKB_CB(skb)->when = tcp_time_stamp; - err = tcp_transmit_skb(sk, skb, 1, GFP_ATOMIC); + /* make sure skb->data is aligned on arches that require it */ + if (unlikely(NET_IP_ALIGN && ((unsigned long)skb->data & 3))) { + struct sk_buff *nskb = __pskb_copy(skb, MAX_TCP_HEADER, + GFP_ATOMIC); + err = nskb ? tcp_transmit_skb(sk, nskb, 0, GFP_ATOMIC) : + -ENOBUFS; + } else { + err = tcp_transmit_skb(sk, skb, 1, GFP_ATOMIC); + } + if (err == 0) { /* Update global TCP statistics. */ diff --git a/net/ipv6/route.c b/net/ipv6/route.c index d6b1d50..7ef5d08 100644 --- a/net/ipv6/route.c +++ b/net/ipv6/route.c @@ -235,7 +235,8 @@ static inline struct rt6_info *ip6_dst_alloc(struct dst_ops *ops, if (rt != NULL) memset(&rt->rt6i_table, 0, - sizeof(*rt) - sizeof(struct dst_entry)); + sizeof(*rt) - sizeof(struct dst_entry)); + return rt; } diff --git a/net/netfilter/xt_IDLETIMER.c b/net/netfilter/xt_IDLETIMER.c index 3bdd443..43a16d0 100644 --- a/net/netfilter/xt_IDLETIMER.c +++ b/net/netfilter/xt_IDLETIMER.c @@ -5,6 +5,7 @@ * After timer expires a kevent will be sent. * * Copyright (C) 2004, 2010 Nokia Corporation + * * Written by Timo Teras <ext-timo.teras@nokia.com> * * Converted to x_tables and reworked for upstream inclusion @@ -38,8 +39,10 @@ #include <linux/netfilter/xt_IDLETIMER.h> #include <linux/kdev_t.h> #include <linux/kobject.h> +#include <linux/skbuff.h> #include <linux/workqueue.h> #include <linux/sysfs.h> +#include <net/net_namespace.h> struct idletimer_tg_attr { struct attribute attr; @@ -56,6 +59,8 @@ struct idletimer_tg { struct idletimer_tg_attr attr; unsigned int refcnt; + bool send_nl_msg; + bool active; }; static LIST_HEAD(idletimer_tg_list); @@ -63,6 +68,32 @@ static DEFINE_MUTEX(list_mutex); static struct kobject *idletimer_tg_kobj; +static void notify_netlink_uevent(const char *iface, struct idletimer_tg *timer) +{ + char iface_msg[NLMSG_MAX_SIZE]; + char state_msg[NLMSG_MAX_SIZE]; + char *envp[] = { iface_msg, state_msg, NULL }; + int res; + + res = snprintf(iface_msg, NLMSG_MAX_SIZE, "INTERFACE=%s", + iface); + if (NLMSG_MAX_SIZE <= res) { + pr_err("message too long (%d)", res); + return; + } + res = snprintf(state_msg, NLMSG_MAX_SIZE, "STATE=%s", + timer->active ? "active" : "inactive"); + if (NLMSG_MAX_SIZE <= res) { + pr_err("message too long (%d)", res); + return; + } + pr_debug("putting nlmsg: <%s> <%s>\n", iface_msg, state_msg); + kobject_uevent_env(idletimer_tg_kobj, KOBJ_CHANGE, envp); + return; + + +} + static struct idletimer_tg *__idletimer_tg_find_by_label(const char *label) { @@ -83,6 +114,7 @@ static ssize_t idletimer_tg_show(struct kobject *kobj, struct attribute *attr, { struct idletimer_tg *timer; unsigned long expires = 0; + unsigned long now = jiffies; mutex_lock(&list_mutex); @@ -92,11 +124,15 @@ static ssize_t idletimer_tg_show(struct kobject *kobj, struct attribute *attr, mutex_unlock(&list_mutex); - if (time_after(expires, jiffies)) + if (time_after(expires, now)) return sprintf(buf, "%u\n", - jiffies_to_msecs(expires - jiffies) / 1000); + jiffies_to_msecs(expires - now) / 1000); - return sprintf(buf, "0\n"); + if (timer->send_nl_msg) + return sprintf(buf, "0 %d\n", + jiffies_to_msecs(now - expires) / 1000); + else + return sprintf(buf, "0\n"); } static void idletimer_tg_work(struct work_struct *work) @@ -105,6 +141,9 @@ static void idletimer_tg_work(struct work_struct *work) work); sysfs_notify(idletimer_tg_kobj, NULL, timer->attr.attr.name); + + if (timer->send_nl_msg) + notify_netlink_uevent(timer->attr.attr.name, timer); } static void idletimer_tg_expired(unsigned long data) @@ -113,6 +152,7 @@ static void idletimer_tg_expired(unsigned long data) pr_debug("timer %s expired\n", timer->attr.attr.name); + timer->active = false; schedule_work(&timer->work); } @@ -147,6 +187,8 @@ static int idletimer_tg_create(struct idletimer_tg_info *info) setup_timer(&info->timer->timer, idletimer_tg_expired, (unsigned long) info->timer); info->timer->refcnt = 1; + info->timer->send_nl_msg = (info->send_nl_msg == 0) ? false : true; + info->timer->active = true; mod_timer(&info->timer->timer, msecs_to_jiffies(info->timeout * 1000) + jiffies); @@ -170,14 +212,24 @@ static unsigned int idletimer_tg_target(struct sk_buff *skb, const struct xt_action_param *par) { const struct idletimer_tg_info *info = par->targinfo; + unsigned long now = jiffies; pr_debug("resetting timer %s, timeout period %u\n", info->label, info->timeout); BUG_ON(!info->timer); + info->timer->active = true; + + if (time_before(info->timer->timer.expires, now)) { + schedule_work(&info->timer->work); + pr_debug("Starting timer %s (Expired, Jiffies): %lu, %lu\n", + info->label, info->timer->timer.expires, now); + } + + /* TODO: Avoid modifying timers on each packet */ mod_timer(&info->timer->timer, - msecs_to_jiffies(info->timeout * 1000) + jiffies); + msecs_to_jiffies(info->timeout * 1000) + now); return XT_CONTINUE; } @@ -186,8 +238,9 @@ static int idletimer_tg_checkentry(const struct xt_tgchk_param *par) { struct idletimer_tg_info *info = par->targinfo; int ret; + unsigned long now = jiffies; - pr_debug("checkentry targinfo%s\n", info->label); + pr_debug("checkentry targinfo %s\n", info->label); if (info->timeout == 0) { pr_debug("timeout value is zero\n"); @@ -206,8 +259,17 @@ static int idletimer_tg_checkentry(const struct xt_tgchk_param *par) info->timer = __idletimer_tg_find_by_label(info->label); if (info->timer) { info->timer->refcnt++; + info->timer->active = true; + + if (time_before(info->timer->timer.expires, now)) { + schedule_work(&info->timer->work); + pr_debug("Starting Checkentry timer" + "(Expired, Jiffies): %lu, %lu\n", + info->timer->timer.expires, now); + } + mod_timer(&info->timer->timer, - msecs_to_jiffies(info->timeout * 1000) + jiffies); + msecs_to_jiffies(info->timeout * 1000) + now); pr_debug("increased refcnt of timer %s to %u\n", info->label, info->timer->refcnt); @@ -221,6 +283,7 @@ static int idletimer_tg_checkentry(const struct xt_tgchk_param *par) } mutex_unlock(&list_mutex); + return 0; } @@ -242,7 +305,7 @@ static void idletimer_tg_destroy(const struct xt_tgdtor_param *par) kfree(info->timer); } else { pr_debug("decreased refcnt of timer %s to %u\n", - info->label, info->timer->refcnt); + info->label, info->timer->refcnt); } mutex_unlock(&list_mutex); @@ -250,6 +313,7 @@ static void idletimer_tg_destroy(const struct xt_tgdtor_param *par) static struct xt_target idletimer_tg __read_mostly = { .name = "IDLETIMER", + .revision = 1, .family = NFPROTO_UNSPEC, .target = idletimer_tg_target, .targetsize = sizeof(struct idletimer_tg_info), @@ -315,3 +379,4 @@ MODULE_DESCRIPTION("Xtables: idle time monitor"); MODULE_LICENSE("GPL v2"); MODULE_ALIAS("ipt_IDLETIMER"); MODULE_ALIAS("ip6t_IDLETIMER"); +MODULE_ALIAS("arpt_IDLETIMER"); diff --git a/net/netfilter/xt_qtaguid.c b/net/netfilter/xt_qtaguid.c index e04635b..e4a14f6 100644 --- a/net/netfilter/xt_qtaguid.c +++ b/net/netfilter/xt_qtaguid.c @@ -26,6 +26,10 @@ #include <net/tcp.h> #include <net/udp.h> +#if defined(CONFIG_IP6_NF_IPTABLES) || defined(CONFIG_IP6_NF_IPTABLES_MODULE) +#include <linux/netfilter_ipv6/ip6_tables.h> +#endif + #include <linux/netfilter/xt_socket.h> #include "xt_qtaguid_internal.h" #include "xt_qtaguid_print.h" @@ -110,8 +114,15 @@ module_param_named(debug_mask, qtaguid_debug_mask, uint, S_IRUGO | S_IWUSR); /*---------------------------------------------------------------------------*/ static const char *iface_stat_procdirname = "iface_stat"; static struct proc_dir_entry *iface_stat_procdir; +/* + * The iface_stat_all* will go away once userspace gets use to the new fields + * that have a format line. + */ static const char *iface_stat_all_procfilename = "iface_stat_all"; static struct proc_dir_entry *iface_stat_all_procfile; +static const char *iface_stat_fmt_procfilename = "iface_stat_fmt"; +static struct proc_dir_entry *iface_stat_fmt_procfile; + /* * Ordering of locks: @@ -124,9 +135,9 @@ static struct proc_dir_entry *iface_stat_all_procfile; * Notice how sock_tag_list_lock is held sometimes when uid_tag_data_tree_lock * is acquired. * - * Call tree with all lock holders as of 2011-09-25: + * Call tree with all lock holders as of 2012-04-27: * - * iface_stat_all_proc_read() + * iface_stat_fmt_proc_read() * iface_stat_list_lock * (struct iface_stat) * @@ -777,13 +788,14 @@ done: return iface_entry; } -static int iface_stat_all_proc_read(char *page, char **num_items_returned, +static int iface_stat_fmt_proc_read(char *page, char **num_items_returned, off_t items_to_skip, int char_count, int *eof, void *data) { char *outp = page; int item_index = 0; int len; + int fmt = (int)data; /* The data is just 1 (old) or 2 (uses fmt) */ struct iface_stat *iface_entry; struct rtnl_link_stats64 dev_stats, *stats; struct rtnl_link_stats64 no_dev_stats = {0}; @@ -793,14 +805,32 @@ static int iface_stat_all_proc_read(char *page, char **num_items_returned, return 0; } - CT_DEBUG("qtaguid:proc iface_stat_all " + CT_DEBUG("qtaguid:proc iface_stat_fmt " + "pid=%u tgid=%u uid=%u " "page=%p *num_items_returned=%p off=%ld " - "char_count=%d *eof=%d\n", page, *num_items_returned, + "char_count=%d *eof=%d\n", + current->pid, current->tgid, current_fsuid(), + page, *num_items_returned, items_to_skip, char_count, *eof); if (*eof) return 0; + if (fmt == 2 && item_index++ >= items_to_skip) { + len = snprintf(outp, char_count, + "ifname " + "total_skb_rx_bytes total_skb_rx_packets " + "total_skb_tx_bytes total_skb_tx_packets\n" + ); + if (len >= char_count) { + *outp = '\0'; + return outp - page; + } + outp += len; + char_count -= len; + (*num_items_returned)++; + } + /* * This lock will prevent iface_stat_update() from changing active, * and in turn prevent an interface from unregistering itself. @@ -816,18 +846,37 @@ static int iface_stat_all_proc_read(char *page, char **num_items_returned, } else { stats = &no_dev_stats; } - len = snprintf(outp, char_count, - "%s %d " - "%llu %llu %llu %llu " - "%llu %llu %llu %llu\n", - iface_entry->ifname, - iface_entry->active, - iface_entry->totals[IFS_RX].bytes, - iface_entry->totals[IFS_RX].packets, - iface_entry->totals[IFS_TX].bytes, - iface_entry->totals[IFS_TX].packets, - stats->rx_bytes, stats->rx_packets, - stats->tx_bytes, stats->tx_packets); + /* + * If the meaning of the data changes, then update the fmtX + * string. + */ + if (fmt == 1) { + len = snprintf( + outp, char_count, + "%s %d " + "%llu %llu %llu %llu " + "%llu %llu %llu %llu\n", + iface_entry->ifname, + iface_entry->active, + iface_entry->totals_via_dev[IFS_RX].bytes, + iface_entry->totals_via_dev[IFS_RX].packets, + iface_entry->totals_via_dev[IFS_TX].bytes, + iface_entry->totals_via_dev[IFS_TX].packets, + stats->rx_bytes, stats->rx_packets, + stats->tx_bytes, stats->tx_packets + ); + } else { + len = snprintf( + outp, char_count, + "%s " + "%llu %llu %llu %llu\n", + iface_entry->ifname, + iface_entry->totals_via_skb[IFS_RX].bytes, + iface_entry->totals_via_skb[IFS_RX].packets, + iface_entry->totals_via_skb[IFS_TX].bytes, + iface_entry->totals_via_skb[IFS_TX].packets + ); + } if (len >= char_count) { spin_unlock_bh(&iface_stat_list_lock); *outp = '\0'; @@ -861,13 +910,17 @@ static void iface_create_proc_worker(struct work_struct *work) new_iface->proc_ptr = proc_entry; create_proc_read_entry("tx_bytes", proc_iface_perms, proc_entry, - read_proc_u64, &new_iface->totals[IFS_TX].bytes); + read_proc_u64, + &new_iface->totals_via_dev[IFS_TX].bytes); create_proc_read_entry("rx_bytes", proc_iface_perms, proc_entry, - read_proc_u64, &new_iface->totals[IFS_RX].bytes); + read_proc_u64, + &new_iface->totals_via_dev[IFS_RX].bytes); create_proc_read_entry("tx_packets", proc_iface_perms, proc_entry, - read_proc_u64, &new_iface->totals[IFS_TX].packets); + read_proc_u64, + &new_iface->totals_via_dev[IFS_TX].packets); create_proc_read_entry("rx_packets", proc_iface_perms, proc_entry, - read_proc_u64, &new_iface->totals[IFS_RX].packets); + read_proc_u64, + &new_iface->totals_via_dev[IFS_RX].packets); create_proc_read_entry("active", proc_iface_perms, proc_entry, read_proc_bool, &new_iface->active); @@ -971,11 +1024,13 @@ static void iface_check_stats_reset_and_adjust(struct net_device *net_dev, "iface reset its stats unexpectedly\n", __func__, net_dev->name); - iface->totals[IFS_TX].bytes += iface->last_known[IFS_TX].bytes; - iface->totals[IFS_TX].packets += + iface->totals_via_dev[IFS_TX].bytes += + iface->last_known[IFS_TX].bytes; + iface->totals_via_dev[IFS_TX].packets += iface->last_known[IFS_TX].packets; - iface->totals[IFS_RX].bytes += iface->last_known[IFS_RX].bytes; - iface->totals[IFS_RX].packets += + iface->totals_via_dev[IFS_RX].bytes += + iface->last_known[IFS_RX].bytes; + iface->totals_via_dev[IFS_RX].packets += iface->last_known[IFS_RX].packets; iface->last_known_valid = false; IF_DEBUG("qtaguid: %s(%s): iface=%p " @@ -1143,6 +1198,27 @@ static struct sock_tag *get_sock_stat(const struct sock *sk) return sock_tag_entry; } +static int ipx_proto(const struct sk_buff *skb, + struct xt_action_param *par) +{ + int thoff, tproto; + + switch (par->family) { + case NFPROTO_IPV6: + tproto = ipv6_find_hdr(skb, &thoff, -1, NULL); + if (tproto < 0) + MT_DEBUG("%s(): transport header not found in ipv6" + " skb=%p\n", __func__, skb); + break; + case NFPROTO_IPV4: + tproto = ip_hdr(skb)->protocol; + break; + default: + tproto = IPPROTO_RAW; + } + return tproto; +} + static void data_counters_update(struct data_counters *dc, int set, enum ifs_tx_rx direction, int proto, int bytes) @@ -1203,10 +1279,10 @@ static void iface_stat_update(struct net_device *net_dev, bool stash_only) spin_unlock_bh(&iface_stat_list_lock); return; } - entry->totals[IFS_TX].bytes += stats->tx_bytes; - entry->totals[IFS_TX].packets += stats->tx_packets; - entry->totals[IFS_RX].bytes += stats->rx_bytes; - entry->totals[IFS_RX].packets += stats->rx_packets; + entry->totals_via_dev[IFS_TX].bytes += stats->tx_bytes; + entry->totals_via_dev[IFS_TX].packets += stats->tx_packets; + entry->totals_via_dev[IFS_RX].bytes += stats->rx_bytes; + entry->totals_via_dev[IFS_RX].packets += stats->rx_packets; /* We don't need the last_known[] anymore */ entry->last_known_valid = false; _iface_stat_set_active(entry, net_dev, false); @@ -1216,6 +1292,67 @@ static void iface_stat_update(struct net_device *net_dev, bool stash_only) spin_unlock_bh(&iface_stat_list_lock); } +/* + * Update stats for the specified interface from the skb. + * Do nothing if the entry + * does not exist (when a device was never configured with an IP address). + * Called on each sk. + */ +static void iface_stat_update_from_skb(const struct sk_buff *skb, + struct xt_action_param *par) +{ + struct iface_stat *entry; + const struct net_device *el_dev; + enum ifs_tx_rx direction = par->in ? IFS_RX : IFS_TX; + int bytes = skb->len; + + if (!skb->dev) { + MT_DEBUG("qtaguid[%d]: no skb->dev\n", par->hooknum); + el_dev = par->in ? : par->out; + } else { + const struct net_device *other_dev; + el_dev = skb->dev; + other_dev = par->in ? : par->out; + if (el_dev != other_dev) { + MT_DEBUG("qtaguid[%d]: skb->dev=%p %s vs " + "par->(in/out)=%p %s\n", + par->hooknum, el_dev, el_dev->name, other_dev, + other_dev->name); + } + } + + if (unlikely(!el_dev)) { + pr_err("qtaguid[%d]: %s(): no par->in/out?!!\n", + par->hooknum, __func__); + BUG(); + } else if (unlikely(!el_dev->name)) { + pr_err("qtaguid[%d]: %s(): no dev->name?!!\n", + par->hooknum, __func__); + BUG(); + } else { + int proto = ipx_proto(skb, par); + MT_DEBUG("qtaguid[%d]: dev name=%s type=%d fam=%d proto=%d\n", + par->hooknum, el_dev->name, el_dev->type, + par->family, proto); + } + + spin_lock_bh(&iface_stat_list_lock); + entry = get_iface_entry(el_dev->name); + if (entry == NULL) { + IF_DEBUG("qtaguid: iface_stat: %s(%s): not tracked\n", + __func__, el_dev->name); + spin_unlock_bh(&iface_stat_list_lock); + return; + } + + IF_DEBUG("qtaguid: %s(%s): entry=%p\n", __func__, + el_dev->name, entry); + + entry->totals_via_skb[direction].bytes += bytes; + entry->totals_via_skb[direction].packets++; + spin_unlock_bh(&iface_stat_list_lock); +} + static void tag_stat_update(struct tag_stat *tag_entry, enum ifs_tx_rx direction, int proto, int bytes) { @@ -1330,8 +1467,19 @@ static void if_tag_stat_update(const char *ifname, uid_t uid, } if (acct_tag) { + /* Create the child {acct_tag, uid_tag} and hook up parent. */ new_tag_stat = create_if_tag_stat(iface_entry, tag); new_tag_stat->parent_counters = uid_tag_counters; + } else { + /* + * For new_tag_stat to be still NULL here would require: + * {0, uid_tag} exists + * and {acct_tag, uid_tag} doesn't exist + * AND acct_tag == 0. + * Impossible. This reassures us that new_tag_stat + * below will always be assigned. + */ + BUG_ON(!new_tag_stat); } if (new_tag_stat) @@ -1457,18 +1605,31 @@ static int __init iface_stat_init(struct proc_dir_entry *parent_procdir) parent_procdir); if (!iface_stat_all_procfile) { pr_err("qtaguid: iface_stat: init " - " failed to create stat_all proc entry\n"); + " failed to create stat_old proc entry\n"); err = -1; goto err_zap_entry; } - iface_stat_all_procfile->read_proc = iface_stat_all_proc_read; + iface_stat_all_procfile->read_proc = iface_stat_fmt_proc_read; + iface_stat_all_procfile->data = (void *)1; /* fmt1 */ + + iface_stat_fmt_procfile = create_proc_entry(iface_stat_fmt_procfilename, + proc_iface_perms, + parent_procdir); + if (!iface_stat_fmt_procfile) { + pr_err("qtaguid: iface_stat: init " + " failed to create stat_all proc entry\n"); + err = -1; + goto err_zap_all_stats_entry; + } + iface_stat_fmt_procfile->read_proc = iface_stat_fmt_proc_read; + iface_stat_fmt_procfile->data = (void *)2; /* fmt2 */ err = register_netdevice_notifier(&iface_netdev_notifier_blk); if (err) { pr_err("qtaguid: iface_stat: init " "failed to register dev event handler\n"); - goto err_zap_all_stats_entry; + goto err_zap_all_stats_entries; } err = register_inetaddr_notifier(&iface_inetaddr_notifier_blk); if (err) { @@ -1492,6 +1653,8 @@ err_unreg_ip4_addr: unregister_inetaddr_notifier(&iface_inetaddr_notifier_blk); err_unreg_nd: unregister_netdevice_notifier(&iface_netdev_notifier_blk); +err_zap_all_stats_entries: + remove_proc_entry(iface_stat_fmt_procfilename, parent_procdir); err_zap_all_stats_entry: remove_proc_entry(iface_stat_all_procfilename, parent_procdir); err_zap_entry: @@ -1571,15 +1734,15 @@ static void account_for_uid(const struct sk_buff *skb, } else if (unlikely(!el_dev->name)) { pr_info("qtaguid[%d]: no dev->name?!!\n", par->hooknum); } else { - MT_DEBUG("qtaguid[%d]: dev name=%s type=%d\n", - par->hooknum, - el_dev->name, - el_dev->type); + int proto = ipx_proto(skb, par); + MT_DEBUG("qtaguid[%d]: dev name=%s type=%d fam=%d proto=%d\n", + par->hooknum, el_dev->name, el_dev->type, + par->family, proto); if_tag_stat_update(el_dev->name, uid, skb->sk ? skb->sk : alternate_sk, par->in ? IFS_RX : IFS_TX, - ip_hdr(skb)->protocol, skb->len); + proto, skb->len); } } @@ -1604,8 +1767,22 @@ static bool qtaguid_mt(const struct sk_buff *skb, struct xt_action_param *par) goto ret_res; } - sk = skb->sk; + switch (par->hooknum) { + case NF_INET_PRE_ROUTING: + case NF_INET_POST_ROUTING: + atomic64_inc(&qtu_events.match_calls_prepost); + iface_stat_update_from_skb(skb, par); + /* + * We are done in pre/post. The skb will get processed + * further alter. + */ + res = (info->match ^ info->invert); + goto ret_res; + break; + /* default: Fall through and do UID releated work */ + } + sk = skb->sk; if (sk == NULL) { /* * A missing sk->sk_socket happens when packets are in-flight @@ -1624,8 +1801,8 @@ static bool qtaguid_mt(const struct sk_buff *skb, struct xt_action_param *par) } else { atomic64_inc(&qtu_events.match_found_sk); } - MT_DEBUG("qtaguid[%d]: sk=%p got_sock=%d proto=%d\n", - par->hooknum, sk, got_sock, ip_hdr(skb)->protocol); + MT_DEBUG("qtaguid[%d]: sk=%p got_sock=%d fam=%d proto=%d\n", + par->hooknum, sk, got_sock, par->family, ipx_proto(skb, par)); if (sk != NULL) { MT_DEBUG("qtaguid[%d]: sk=%p->sk_socket=%p->file=%p\n", par->hooknum, sk, sk->sk_socket, @@ -1780,8 +1957,10 @@ static int qtaguid_ctrl_proc_read(char *page, char **num_items_returned, if (*eof) return 0; - CT_DEBUG("qtaguid: proc ctrl page=%p off=%ld char_count=%d *eof=%d\n", - page, items_to_skip, char_count, *eof); + CT_DEBUG("qtaguid: proc ctrl pid=%u tgid=%u uid=%u " + "page=%p off=%ld char_count=%d *eof=%d\n", + current->pid, current->tgid, current_fsuid(), + page, items_to_skip, char_count, *eof); spin_lock_bh(&sock_tag_list_lock); for (node = rb_first(&sock_tag_tree); @@ -1825,6 +2004,7 @@ static int qtaguid_ctrl_proc_read(char *page, char **num_items_returned, "delete_cmds=%llu " "iface_events=%llu " "match_calls=%llu " + "match_calls_prepost=%llu " "match_found_sk=%llu " "match_found_sk_in_ct=%llu " "match_found_no_sk_in_ct=%llu " @@ -1836,6 +2016,7 @@ static int qtaguid_ctrl_proc_read(char *page, char **num_items_returned, atomic64_read(&qtu_events.delete_cmds), atomic64_read(&qtu_events.iface_events), atomic64_read(&qtu_events.match_calls), + atomic64_read(&qtu_events.match_calls_prepost), atomic64_read(&qtu_events.match_found_sk), atomic64_read(&qtu_events.match_found_sk_in_ct), atomic64_read( @@ -2109,7 +2290,9 @@ static int ctrl_cmd_tag(const char *input) el_socket = sockfd_lookup(sock_fd, &res); /* This locks the file */ if (!el_socket) { pr_info("qtaguid: ctrl_tag(%s): failed to lookup" - " sock_fd=%d err=%d\n", input, sock_fd, res); + " sock_fd=%d err=%d pid=%u tgid=%u uid=%u\n", + input, sock_fd, res, current->pid, current->tgid, + current_fsuid()); goto err; } CT_DEBUG("qtaguid: ctrl_tag(%s): socket->...->f_count=%ld ->sk=%p\n", @@ -2254,7 +2437,9 @@ static int ctrl_cmd_untag(const char *input) el_socket = sockfd_lookup(sock_fd, &res); /* This locks the file */ if (!el_socket) { pr_info("qtaguid: ctrl_untag(%s): failed to lookup" - " sock_fd=%d err=%d\n", input, sock_fd, res); + " sock_fd=%d err=%d pid=%u tgid=%u uid=%u\n", + input, sock_fd, res, current->pid, current->tgid, + current_fsuid()); goto err; } CT_DEBUG("qtaguid: ctrl_untag(%s): socket->...->f_count=%ld ->sk=%p\n", @@ -2330,6 +2515,9 @@ static int qtaguid_ctrl_parse(const char *input, int count) char cmd; int res; + CT_DEBUG("qtaguid: ctrl(%s): pid=%u tgid=%u uid=%u\n", + input, current->pid, current->tgid, current_fsuid()); + cmd = input[0]; /* Collect params for commands */ switch (cmd) { @@ -2506,9 +2694,12 @@ static int qtaguid_stats_proc_read(char *page, char **num_items_returned, return len; } - CT_DEBUG("qtaguid:proc stats page=%p *num_items_returned=%p off=%ld " - "char_count=%d *eof=%d\n", page, *num_items_returned, - items_to_skip, char_count, *eof); + CT_DEBUG("qtaguid:proc stats pid=%u tgid=%u uid=%u " + "page=%p *num_items_returned=%p off=%ld " + "char_count=%d *eof=%d\n", + current->pid, current->tgid, current_fsuid(), + page, *num_items_returned, + items_to_skip, char_count, *eof); if (*eof) return 0; diff --git a/net/netfilter/xt_qtaguid_internal.h b/net/netfilter/xt_qtaguid_internal.h index 02479d6..d79f838 100644 --- a/net/netfilter/xt_qtaguid_internal.h +++ b/net/netfilter/xt_qtaguid_internal.h @@ -202,7 +202,8 @@ struct iface_stat { /* net_dev is only valid for active iface_stat */ struct net_device *net_dev; - struct byte_packet_counters totals[IFS_MAX_DIRECTIONS]; + struct byte_packet_counters totals_via_dev[IFS_MAX_DIRECTIONS]; + struct byte_packet_counters totals_via_skb[IFS_MAX_DIRECTIONS]; /* * We keep the last_known, because some devices reset their counters * just before NETDEV_UP, while some will reset just before @@ -254,6 +255,8 @@ struct qtaguid_event_counts { atomic64_t iface_events; /* Number of NETDEV_* events handled */ atomic64_t match_calls; /* Number of times iptables called mt */ + /* Number of times iptables called mt from pre or post routing hooks */ + atomic64_t match_calls_prepost; /* * match_found_sk_*: numbers related to the netfilter matching * function finding a sock for the sk_buff. diff --git a/net/netfilter/xt_qtaguid_print.c b/net/netfilter/xt_qtaguid_print.c index 3917678..8cbd8e4 100644 --- a/net/netfilter/xt_qtaguid_print.c +++ b/net/netfilter/xt_qtaguid_print.c @@ -183,7 +183,11 @@ char *pp_iface_stat(struct iface_stat *is) res = kasprintf(GFP_ATOMIC, "iface_stat@%p{" "list=list_head{...}, " "ifname=%s, " - "total={rx={bytes=%llu, " + "total_dev={rx={bytes=%llu, " + "packets=%llu}, " + "tx={bytes=%llu, " + "packets=%llu}}, " + "total_skb={rx={bytes=%llu, " "packets=%llu}, " "tx={bytes=%llu, " "packets=%llu}}, " @@ -198,10 +202,14 @@ char *pp_iface_stat(struct iface_stat *is) "tag_stat_tree=rb_root{...}}", is, is->ifname, - is->totals[IFS_RX].bytes, - is->totals[IFS_RX].packets, - is->totals[IFS_TX].bytes, - is->totals[IFS_TX].packets, + is->totals_via_dev[IFS_RX].bytes, + is->totals_via_dev[IFS_RX].packets, + is->totals_via_dev[IFS_TX].bytes, + is->totals_via_dev[IFS_TX].packets, + is->totals_via_skb[IFS_RX].bytes, + is->totals_via_skb[IFS_RX].packets, + is->totals_via_skb[IFS_TX].bytes, + is->totals_via_skb[IFS_TX].packets, is->last_known_valid, is->last_known[IFS_RX].bytes, is->last_known[IFS_RX].packets, diff --git a/net/wireless/nl80211.c b/net/wireless/nl80211.c index 055e000..320c3fe 100644 --- a/net/wireless/nl80211.c +++ b/net/wireless/nl80211.c @@ -175,6 +175,15 @@ static const struct nla_policy nl80211_policy[NL80211_ATTR_MAX+1] = { [NL80211_ATTR_WOWLAN_TRIGGERS] = { .type = NLA_NESTED }, [NL80211_ATTR_STA_PLINK_STATE] = { .type = NLA_U8 }, [NL80211_ATTR_SCHED_SCAN_INTERVAL] = { .type = NLA_U32 }, + [NL80211_ATTR_REKEY_DATA] = { .type = NLA_NESTED }, + [NL80211_ATTR_SCAN_SUPP_RATES] = { .type = NLA_NESTED }, + [NL80211_ATTR_HIDDEN_SSID] = { .type = NLA_U32 }, + [NL80211_ATTR_IE_PROBE_RESP] = { .type = NLA_BINARY, + .len = IEEE80211_MAX_DATA_LEN }, + [NL80211_ATTR_IE_ASSOC_RESP] = { .type = NLA_BINARY, + .len = IEEE80211_MAX_DATA_LEN }, + [NL80211_ATTR_ROAM_SUPPORT] = { .type = NLA_FLAG }, + [NL80211_ATTR_SCHED_SCAN_MATCH] = { .type = NLA_NESTED }, }; /* policy for the key attributes */ @@ -205,6 +214,12 @@ nl80211_wowlan_policy[NUM_NL80211_WOWLAN_TRIG] = { [NL80211_WOWLAN_TRIG_PKT_PATTERN] = { .type = NLA_NESTED }, }; +static const struct nla_policy +nl80211_match_policy[NL80211_SCHED_SCAN_MATCH_ATTR_MAX + 1] = { + [NL80211_ATTR_SCHED_SCAN_MATCH_SSID] = { .type = NLA_BINARY, + .len = IEEE80211_MAX_SSID_LEN }, +}; + /* ifidx get helper */ static int nl80211_get_ifidx(struct netlink_callback *cb) { @@ -682,8 +697,14 @@ static int nl80211_send_wiphy(struct sk_buff *msg, u32 pid, u32 seq, int flags, dev->wiphy.coverage_class); NLA_PUT_U8(msg, NL80211_ATTR_MAX_NUM_SCAN_SSIDS, dev->wiphy.max_scan_ssids); + NLA_PUT_U8(msg, NL80211_ATTR_MAX_NUM_SCHED_SCAN_SSIDS, + dev->wiphy.max_sched_scan_ssids); NLA_PUT_U16(msg, NL80211_ATTR_MAX_SCAN_IE_LEN, dev->wiphy.max_scan_ie_len); + NLA_PUT_U16(msg, NL80211_ATTR_MAX_SCHED_SCAN_IE_LEN, + dev->wiphy.max_sched_scan_ie_len); + NLA_PUT_U8(msg, NL80211_ATTR_MAX_MATCH_SETS, + dev->wiphy.max_match_sets); if (dev->wiphy.flags & WIPHY_FLAG_IBSS_RSN) NLA_PUT_FLAG(msg, NL80211_ATTR_SUPPORT_IBSS_RSN); @@ -2213,7 +2234,7 @@ static int nl80211_send_station(struct sk_buff *msg, u32 pid, u32 seq, } nla_nest_end(msg, sinfoattr); - if (sinfo->assoc_req_ies) + if (sinfo->filled & STATION_INFO_ASSOC_REQ_IES) NLA_PUT(msg, NL80211_ATTR_IE, sinfo->assoc_req_ies_len, sinfo->assoc_req_ies); @@ -3458,10 +3479,11 @@ static int nl80211_start_sched_scan(struct sk_buff *skb, struct net_device *dev = info->user_ptr[1]; struct nlattr *attr; struct wiphy *wiphy; - int err, tmp, n_ssids = 0, n_channels, i; + int err, tmp, n_ssids = 0, n_match_sets = 0, n_channels, i; u32 interval; enum ieee80211_band band; size_t ie_len; + struct nlattr *tb[NL80211_SCHED_SCAN_MATCH_ATTR_MAX + 1]; if (!(rdev->wiphy.flags & WIPHY_FLAG_SUPPORTS_SCHED_SCAN) || !rdev->ops->sched_scan_start) @@ -3497,7 +3519,16 @@ static int nl80211_start_sched_scan(struct sk_buff *skb, tmp) n_ssids++; - if (n_ssids > wiphy->max_scan_ssids) + if (n_ssids > wiphy->max_sched_scan_ssids) + return -EINVAL; + + if (info->attrs[NL80211_ATTR_SCHED_SCAN_MATCH]) + nla_for_each_nested(attr, + info->attrs[NL80211_ATTR_SCHED_SCAN_MATCH], + tmp) + n_match_sets++; + + if (n_match_sets > wiphy->max_match_sets) return -EINVAL; if (info->attrs[NL80211_ATTR_IE]) @@ -3505,7 +3536,7 @@ static int nl80211_start_sched_scan(struct sk_buff *skb, else ie_len = 0; - if (ie_len > wiphy->max_scan_ie_len) + if (ie_len > wiphy->max_sched_scan_ie_len) return -EINVAL; mutex_lock(&rdev->sched_scan_mtx); @@ -3517,6 +3548,7 @@ static int nl80211_start_sched_scan(struct sk_buff *skb, request = kzalloc(sizeof(*request) + sizeof(*request->ssids) * n_ssids + + sizeof(*request->match_sets) * n_match_sets + sizeof(*request->channels) * n_channels + ie_len, GFP_KERNEL); if (!request) { @@ -3534,6 +3566,18 @@ static int nl80211_start_sched_scan(struct sk_buff *skb, request->ie = (void *)(request->channels + n_channels); } + if (n_match_sets) { + if (request->ie) + request->match_sets = (void *)(request->ie + ie_len); + else if (request->ssids) + request->match_sets = + (void *)(request->ssids + n_ssids); + else + request->match_sets = + (void *)(request->channels + n_channels); + } + request->n_match_sets = n_match_sets; + i = 0; if (info->attrs[NL80211_ATTR_SCAN_FREQUENCIES]) { /* user specified, bail out if channel not found */ @@ -3598,6 +3642,31 @@ static int nl80211_start_sched_scan(struct sk_buff *skb, } } + i = 0; + if (info->attrs[NL80211_ATTR_SCHED_SCAN_MATCH]) { + nla_for_each_nested(attr, + info->attrs[NL80211_ATTR_SCHED_SCAN_MATCH], + tmp) { + struct nlattr *ssid; + + nla_parse(tb, NL80211_SCHED_SCAN_MATCH_ATTR_MAX, + nla_data(attr), nla_len(attr), + nl80211_match_policy); + ssid = tb[NL80211_ATTR_SCHED_SCAN_MATCH_SSID]; + if (ssid) { + if (nla_len(ssid) > IEEE80211_MAX_SSID_LEN) { + err = -EINVAL; + goto out_free; + } + memcpy(request->match_sets[i].ssid.ssid, + nla_data(ssid), nla_len(ssid)); + request->match_sets[i].ssid.ssid_len = + nla_len(ssid); + } + i++; + } + } + if (info->attrs[NL80211_ATTR_IE]) { request->ie_len = nla_len(info->attrs[NL80211_ATTR_IE]); memcpy((void *)request->ie, diff --git a/net/wireless_ath/nl80211.c b/net/wireless_ath/nl80211.c index 360e49c..b11cabe 100755 --- a/net/wireless_ath/nl80211.c +++ b/net/wireless_ath/nl80211.c @@ -214,6 +214,7 @@ static const struct nla_policy nl80211_policy[NL80211_ATTR_MAX+1] = { [NL80211_ATTR_BTCOEX_DATA] = { .type = NLA_BINARY, .len = IEEE80211_MAX_DATA_LEN }, [NL80211_ATTR_PRIV_CMD] = { .type = NLA_NUL_STRING, .len = 128 }, + [NL80211_ATTR_PRIV_EVENT] = { .type = NLA_NUL_STRING, .len = 128 }, }; @@ -6515,7 +6516,10 @@ static struct genl_ops nl80211_ops[] = { .doit = nl80211_testmode_do, .dumpit = nl80211_testmode_dump, .policy = nl80211_policy, +#ifdef CONFIG_MACH_PX +#else .flags = GENL_ADMIN_PERM, +#endif .internal_flags = NL80211_FLAG_NEED_WIPHY | NL80211_FLAG_NEED_RTNL, }, @@ -7969,6 +7973,46 @@ void cfg80211_report_obss_beacon(struct wiphy *wiphy, } EXPORT_SYMBOL(cfg80211_report_obss_beacon); +void cfg80211_priv_event(struct net_device *dev, + const char *priv_event, gfp_t gfp) +{ + struct wireless_dev *wdev = dev->ieee80211_ptr; + struct cfg80211_registered_device *rdev = wiphy_to_dev(wdev->wiphy); + struct sk_buff *msg; + void *hdr; + int err; + + msg = nlmsg_new(NLMSG_GOODSIZE, gfp); + if (!msg) + return; + + hdr = nl80211hdr_put(msg, 0, 0, 0, NL80211_CMD_PRIV_EVENT); + if (!hdr) { + nlmsg_free(msg); + return; + } + + NLA_PUT_U32(msg, NL80211_ATTR_IFINDEX, dev->ifindex); + NLA_PUT_STRING(msg, NL80211_ATTR_PRIV_EVENT, priv_event); + + err = genlmsg_end(msg, hdr); + if (err < 0) { + nlmsg_free(msg); + return; + } + + genlmsg_multicast_netns(wiphy_net(&rdev->wiphy), msg, 0, + nl80211_mlme_mcgrp.id, gfp); + return; + + nla_put_failure: + genlmsg_cancel(msg, hdr); + nlmsg_free(msg); + +} +EXPORT_SYMBOL(cfg80211_priv_event); + + static int nl80211_netlink_notify(struct notifier_block * nb, unsigned long state, void *_notify) diff --git a/net/xfrm/xfrm_policy.c b/net/xfrm/xfrm_policy.c index 7803eb6..1b906c3 100644 --- a/net/xfrm/xfrm_policy.c +++ b/net/xfrm/xfrm_policy.c @@ -1349,14 +1349,16 @@ static inline struct xfrm_dst *xfrm_alloc_dst(struct net *net, int family) BUG(); } xdst = dst_alloc(dst_ops, NULL, 0, 0, 0); - memset(&xdst->u.rt6.rt6i_table, 0, sizeof(*xdst) - sizeof(struct dst_entry)); - xfrm_policy_put_afinfo(afinfo); - if (likely(xdst)) + if (likely(xdst)) { + memset(&xdst->u.rt6.rt6i_table, 0, + sizeof(*xdst) - sizeof(struct dst_entry)); xdst->flo.ops = &xfrm_bundle_fc_ops; - else + } else xdst = ERR_PTR(-ENOBUFS); + xfrm_policy_put_afinfo(afinfo); + return xdst; } |