Bluetooth: LE_ADV_IND filtering for GFRM200
Change-Id: I915332328f17629af5236359c45d49375fe8dc63
diff --git a/include/net/bluetooth/hci_core.h b/include/net/bluetooth/hci_core.h
index a056c2b..4ce220d 100644
--- a/include/net/bluetooth/hci_core.h
+++ b/include/net/bluetooth/hci_core.h
@@ -199,6 +199,7 @@
bdaddr_t public_addr;
bdaddr_t random_addr;
bdaddr_t static_addr;
+ bdaddr_t le_adv_ind_filter;
__u8 adv_addr_type;
__u8 dev_name[HCI_MAX_NAME_LENGTH];
__u8 short_name[HCI_MAX_SHORT_NAME_LENGTH];
@@ -1225,6 +1226,41 @@
return false;
}
+static inline bool eir_has_data(u8 *data, size_t data_len, u8 type, u8 *mdata,
+ size_t mdata_len)
+{
+ size_t parsed = 0;
+
+ if (data_len < 2)
+ return false;
+
+ while (parsed < data_len - 1) {
+ u8 field_len = data[0];
+
+ if (field_len == 0)
+ break;
+
+ parsed += field_len + 1;
+
+ if (parsed > data_len)
+ break;
+
+ if (data[1] != type)
+ goto next;
+
+ if ((field_len - 1) != mdata_len)
+ goto next;
+
+ if (!memcmp(&data[2], mdata, mdata_len))
+ return true;
+
+ next:
+ data += field_len + 1;
+ }
+
+ return false;
+}
+
static inline bool hci_bdaddr_is_rpa(bdaddr_t *bdaddr, u8 addr_type)
{
if (addr_type != ADDR_LE_DEV_RANDOM)
diff --git a/net/bluetooth/hci_debugfs.c b/net/bluetooth/hci_debugfs.c
index 7db4220..df84060 100644
--- a/net/bluetooth/hci_debugfs.c
+++ b/net/bluetooth/hci_debugfs.c
@@ -22,6 +22,8 @@
*/
#include <linux/debugfs.h>
+#include <linux/kernel.h>
+#include <linux/ctype.h>
#include <net/bluetooth/bluetooth.h>
#include <net/bluetooth/hci_core.h>
@@ -793,6 +795,87 @@
.llseek = default_llseek,
};
+static int bachk(const char *str)
+{
+ if (!str)
+ return -1;
+
+ if (strlen(str) != 17)
+ return -1;
+
+ while (*str) {
+ if (!isxdigit(*str++))
+ return -1;
+
+ if (!isxdigit(*str++))
+ return -1;
+
+ if (*str == 0)
+ break;
+
+ if (*str++ != ':')
+ return -1;
+ }
+
+ return 0;
+}
+
+static int str2ba(const char *str, bdaddr_t *ba)
+{
+ int i;
+
+ if (bachk(str) < 0) {
+ memset(ba, 0, sizeof(*ba));
+ return -1;
+ }
+
+ for (i = 5; i >= 0; i--, str += 3)
+ ba->b[i] = simple_strtol(str, NULL, 16);
+
+ return 0;
+}
+
+static ssize_t le_adv_ind_filter_read(struct file *file,
+ char __user *user_buf,
+ size_t count, loff_t *ppos)
+{
+ struct hci_dev *hdev = file->private_data;
+ char buf[20];
+
+ snprintf(buf, sizeof(buf), "%pMR\n", &hdev->le_adv_ind_filter);
+ return simple_read_from_buffer(user_buf, count, ppos, buf, 18);
+}
+
+static ssize_t le_adv_ind_filter_write(struct file *file,
+ const char __user *user_buf,
+ size_t count, loff_t *ppos)
+{
+ struct hci_dev *hdev = file->private_data;
+ char buf[20];
+
+ if (count != 17 && count != 18)
+ return -EINVAL;
+
+ if (copy_from_user(buf, user_buf, 17))
+ return -EFAULT;
+
+ buf[17] = '\0';
+
+ if (str2ba(buf, &hdev->le_adv_ind_filter) < 0)
+ return -EINVAL;
+
+ BT_INFO("%s: le_adv_ind_filter=%pMR", hdev->name, &hdev->le_adv_ind_filter);
+
+ return count;
+}
+
+static const struct file_operations le_adv_ind_filter_fops = {
+ .open = simple_open,
+ .read = le_adv_ind_filter_read,
+ .write = le_adv_ind_filter_write,
+ .llseek = default_llseek,
+};
+
static int white_list_show(struct seq_file *f, void *ptr)
{
struct hci_dev *hdev = f->private;
@@ -1127,6 +1210,9 @@
debugfs_create_file("quirk_simultaneous_discovery", 0644,
hdev->debugfs, hdev,
&quirk_simultaneous_discovery_fops);
+
+ debugfs_create_file("le_adv_ind_filter", 0644, hdev->debugfs, hdev,
+ &le_adv_ind_filter_fops);
}
void hci_debugfs_create_conn(struct hci_conn *conn)
diff --git a/net/bluetooth/hci_event.c b/net/bluetooth/hci_event.c
index 7b61be7..677f604 100644
--- a/net/bluetooth/hci_event.c
+++ b/net/bluetooth/hci_event.c
@@ -4712,6 +4712,19 @@
bool match;
u32 flags;
+ /*
+ * GFRM200: Drop connectable undirected advertisements to the floor
+ * except when required for pairing. When BlueZ initiates pairing,
+ * it expects to receive LE_ADV_IND from the peripheral device in
+ * order to establish the LE connection for pairing.
+ */
+ if (type == LE_ADV_IND &&
+ eir_has_data(data, len, EIR_NAME_SHORT, "GFRM200", 7) &&
+ bacmp(bdaddr, &hdev->le_adv_ind_filter)) {
+ BT_INFO("%s: Drop LE_ADV_IND from GFRM200 [%pMR]", hdev->name, bdaddr);
+ return;
+ }
+
/* If the direct address is present, then this report is from
* a LE Direct Advertising Report event. In that case it is
* important to see if the address is matching the local
