Commit b4a221ea authored by Archie Pusaka's avatar Archie Pusaka Committed by Marcel Holtmann
Browse files

Bluetooth: advmon offload MSFT add rssi support 



MSFT needs rssi parameter for monitoring advertisement packet,
therefore we should supply them from mgmt. This adds a new opcode
to add advertisement monitor with rssi parameters.

Signed-off-by: default avatarArchie Pusaka <apusaka@chromium.org>
Reviewed-by: default avatarManish Mandlik <mmandlik@chromium.org>
Reviewed-by: default avatarMiao-chen Chou <mcchou@chromium.org>
Reviewed-by: default avatarYun-Hao Chung <howardchung@google.com>
Signed-off-by: default avatarMarcel Holtmann <marcel@holtmann.org>
parent 7f9f2c3f

include/net/bluetooth/hci_core.h

+9 −0
Original line number Diff line number Diff line
@@ -250,8 +250,17 @@ struct adv_pattern { 
	__u8 value[HCI_MAX_AD_LENGTH];

};



struct adv_rssi_thresholds {

	__s8 low_threshold;

	__s8 high_threshold;

	__u16 low_threshold_timeout;

	__u16 high_threshold_timeout;

	__u8 sampling_period;

};



struct adv_monitor {

	struct list_head patterns;

	struct adv_rssi_thresholds rssi;

	bool		active;

	__u16		handle;

};

include/net/bluetooth/mgmt.h

+16 −0
Original line number Diff line number Diff line
@@ -821,6 +821,22 @@ struct mgmt_rp_add_ext_adv_data { 
	__u8	instance;

} __packed;



struct mgmt_adv_rssi_thresholds {

	__s8	high_threshold;

	__le16	high_threshold_timeout;

	__s8	low_threshold;

	__le16	low_threshold_timeout;

	__u8	sampling_period;

} __packed;



#define MGMT_OP_ADD_ADV_PATTERNS_MONITOR_RSSI	0x0056

struct mgmt_cp_add_adv_patterns_monitor_rssi {

	struct mgmt_adv_rssi_thresholds rssi;

	__u8	pattern_count;

	struct mgmt_adv_pattern patterns[];

} __packed;

#define MGMT_ADD_ADV_PATTERNS_MONITOR_RSSI_SIZE	8



#define MGMT_EV_CMD_COMPLETE		0x0001

struct mgmt_ev_cmd_complete {

	__le16	opcode;

net/bluetooth/mgmt.c

+153 −72
Original line number Diff line number Diff line
@@ -124,6 +124,7 @@ static const u16 mgmt_commands[] = { 
	MGMT_OP_REMOVE_ADV_MONITOR,

	MGMT_OP_ADD_EXT_ADV_PARAMS,

	MGMT_OP_ADD_EXT_ADV_DATA,

	MGMT_OP_ADD_ADV_PATTERNS_MONITOR_RSSI,

};



static const u16 mgmt_events[] = {
@@ -4225,106 +4226,183 @@ static int read_adv_mon_features(struct sock *sk, struct hci_dev *hdev, 
	return err;

}



static int add_adv_patterns_monitor(struct sock *sk, struct hci_dev *hdev,

				    void *data, u16 len)

static int __add_adv_patterns_monitor(struct sock *sk, struct hci_dev *hdev,

				      struct adv_monitor *m, u8 status, u16 op)

{

	struct mgmt_cp_add_adv_patterns_monitor *cp = data;

	struct mgmt_rp_add_adv_patterns_monitor rp;

	struct adv_monitor *m = NULL;

	struct adv_pattern *p = NULL;

	unsigned int mp_cnt = 0, prev_adv_monitors_cnt;

	__u8 cp_ofst = 0, cp_len = 0;

	int err, i;



	BT_DBG("request for %s", hdev->name);

	unsigned int prev_adv_monitors_cnt;

	int err;



	if (len <= sizeof(*cp) || cp->pattern_count == 0) {

		err = mgmt_cmd_status(sk, hdev->id,

				      MGMT_OP_ADD_ADV_PATTERNS_MONITOR,

				      MGMT_STATUS_INVALID_PARAMS);

	if (status)

		goto failed;

	}



	m = kmalloc(sizeof(*m), GFP_KERNEL);

	if (!m) {

		err = -ENOMEM;

		goto failed;

	hci_dev_lock(hdev);



	prev_adv_monitors_cnt = hdev->adv_monitors_cnt;



	err = hci_add_adv_monitor(hdev, m);

	if (err) {

		if (err == -ENOSPC)

			status = MGMT_STATUS_NO_RESOURCES;

		else

			status = MGMT_STATUS_FAILED;



		goto unlock;

	}



	INIT_LIST_HEAD(&m->patterns);

	m->active = false;

	if (hdev->adv_monitors_cnt > prev_adv_monitors_cnt)

		mgmt_adv_monitor_added(sk, hdev, m->handle);



	for (i = 0; i < cp->pattern_count; i++) {

		if (++mp_cnt > HCI_MAX_ADV_MONITOR_NUM_PATTERNS) {

			err = mgmt_cmd_status(sk, hdev->id,

					      MGMT_OP_ADD_ADV_PATTERNS_MONITOR,

					      MGMT_STATUS_INVALID_PARAMS);

			goto failed;

	hci_dev_unlock(hdev);



	rp.monitor_handle = cpu_to_le16(m->handle);



	return mgmt_cmd_complete(sk, hdev->id, op,

				 MGMT_STATUS_SUCCESS, &rp, sizeof(rp));



unlock:

	hci_dev_unlock(hdev);



failed:

	hci_free_adv_monitor(m);

	return mgmt_cmd_status(sk, hdev->id, op, status);

}



		cp_ofst = cp->patterns[i].offset;

		cp_len = cp->patterns[i].length;

		if (cp_ofst >= HCI_MAX_AD_LENGTH ||

		    cp_len > HCI_MAX_AD_LENGTH ||

		    (cp_ofst + cp_len) > HCI_MAX_AD_LENGTH) {

			err = mgmt_cmd_status(sk, hdev->id,

					      MGMT_OP_ADD_ADV_PATTERNS_MONITOR,

					      MGMT_STATUS_INVALID_PARAMS);

			goto failed;

static void parse_adv_monitor_rssi(struct adv_monitor *m,

				   struct mgmt_adv_rssi_thresholds *rssi)

{

	if (rssi) {

		m->rssi.low_threshold = rssi->low_threshold;

		m->rssi.low_threshold_timeout =

		    __le16_to_cpu(rssi->low_threshold_timeout);

		m->rssi.high_threshold = rssi->high_threshold;

		m->rssi.high_threshold_timeout =

		    __le16_to_cpu(rssi->high_threshold_timeout);

		m->rssi.sampling_period = rssi->sampling_period;

	} else {

		/* Default values. These numbers are the least constricting

		 * parameters for MSFT API to work, so it behaves as if there

		 * are no rssi parameter to consider. May need to be changed

		 * if other API are to be supported.

		 */

		m->rssi.low_threshold = -127;

		m->rssi.low_threshold_timeout = 60;

		m->rssi.high_threshold = -127;

		m->rssi.high_threshold_timeout = 0;

		m->rssi.sampling_period = 0;

	}

}



static u8 parse_adv_monitor_pattern(struct adv_monitor *m, u8 pattern_count,

				    struct mgmt_adv_pattern *patterns)

{

	u8 offset = 0, length = 0;

	struct adv_pattern *p = NULL;

	unsigned int mp_cnt = 0;

	int i;



	for (i = 0; i < pattern_count; i++) {

		if (++mp_cnt > HCI_MAX_ADV_MONITOR_NUM_PATTERNS)

			return MGMT_STATUS_INVALID_PARAMS;



		offset = patterns[i].offset;

		length = patterns[i].length;

		if (offset >= HCI_MAX_AD_LENGTH ||

		    length > HCI_MAX_AD_LENGTH ||

		    (offset + length) > HCI_MAX_AD_LENGTH)

			return MGMT_STATUS_INVALID_PARAMS;



		p = kmalloc(sizeof(*p), GFP_KERNEL);

		if (!p) {

			err = -ENOMEM;

			goto failed;

		}

		if (!p)

			return MGMT_STATUS_NO_RESOURCES;



		p->ad_type = cp->patterns[i].ad_type;

		p->offset = cp->patterns[i].offset;

		p->length = cp->patterns[i].length;

		memcpy(p->value, cp->patterns[i].value, p->length);

		p->ad_type = patterns[i].ad_type;

		p->offset = patterns[i].offset;

		p->length = patterns[i].length;

		memcpy(p->value, patterns[i].value, p->length);



		INIT_LIST_HEAD(&p->list);

		list_add(&p->list, &m->patterns);

	}



	if (mp_cnt != cp->pattern_count) {

		err = mgmt_cmd_status(sk, hdev->id,

				      MGMT_OP_ADD_ADV_PATTERNS_MONITOR,

				      MGMT_STATUS_INVALID_PARAMS);

		goto failed;

	if (mp_cnt != pattern_count)

		return MGMT_STATUS_INVALID_PARAMS;



	return MGMT_STATUS_SUCCESS;

}



	hci_dev_lock(hdev);

static int add_adv_patterns_monitor(struct sock *sk, struct hci_dev *hdev,

				    void *data, u16 len)

{

	struct mgmt_cp_add_adv_patterns_monitor *cp = data;

	struct adv_monitor *m = NULL;

	u8 status = MGMT_STATUS_SUCCESS;

	size_t expected_size = sizeof(*cp);



	prev_adv_monitors_cnt = hdev->adv_monitors_cnt;

	BT_DBG("request for %s", hdev->name);



	err = hci_add_adv_monitor(hdev, m);

	if (err) {

		if (err == -ENOSPC) {

			mgmt_cmd_status(sk, hdev->id,

					MGMT_OP_ADD_ADV_PATTERNS_MONITOR,

					MGMT_STATUS_NO_RESOURCES);

	if (len <= sizeof(*cp)) {

		status = MGMT_STATUS_INVALID_PARAMS;

		goto done;

	}

		goto unlock;



	expected_size += cp->pattern_count * sizeof(struct mgmt_adv_pattern);

	if (len != expected_size) {

		status = MGMT_STATUS_INVALID_PARAMS;

		goto done;

	}



	if (hdev->adv_monitors_cnt > prev_adv_monitors_cnt)

		mgmt_adv_monitor_added(sk, hdev, m->handle);

	m = kzalloc(sizeof(*m), GFP_KERNEL);

	if (!m) {

		status = MGMT_STATUS_NO_RESOURCES;

		goto done;

	}



	hci_dev_unlock(hdev);

	INIT_LIST_HEAD(&m->patterns);



	rp.monitor_handle = cpu_to_le16(m->handle);

	parse_adv_monitor_rssi(m, NULL);

	status = parse_adv_monitor_pattern(m, cp->pattern_count, cp->patterns);



	return mgmt_cmd_complete(sk, hdev->id, MGMT_OP_ADD_ADV_PATTERNS_MONITOR,

				 MGMT_STATUS_SUCCESS, &rp, sizeof(rp));

done:

	return __add_adv_patterns_monitor(sk, hdev, m, status,

					  MGMT_OP_ADD_ADV_PATTERNS_MONITOR);

}



unlock:

	hci_dev_unlock(hdev);

static int add_adv_patterns_monitor_rssi(struct sock *sk, struct hci_dev *hdev,

					 void *data, u16 len)

{

	struct mgmt_cp_add_adv_patterns_monitor_rssi *cp = data;

	struct adv_monitor *m = NULL;

	u8 status = MGMT_STATUS_SUCCESS;

	size_t expected_size = sizeof(*cp);



failed:

	hci_free_adv_monitor(m);

	return err;

	BT_DBG("request for %s", hdev->name);



	if (len <= sizeof(*cp)) {

		status = MGMT_STATUS_INVALID_PARAMS;

		goto done;

	}



	expected_size += cp->pattern_count * sizeof(struct mgmt_adv_pattern);

	if (len != expected_size) {

		status = MGMT_STATUS_INVALID_PARAMS;

		goto done;

	}



	m = kzalloc(sizeof(*m), GFP_KERNEL);

	if (!m) {

		status = MGMT_STATUS_NO_RESOURCES;

		goto done;

	}



	INIT_LIST_HEAD(&m->patterns);



	parse_adv_monitor_rssi(m, &cp->rssi);

	status = parse_adv_monitor_pattern(m, cp->pattern_count, cp->patterns);



done:

	return __add_adv_patterns_monitor(sk, hdev, m, status,

					 MGMT_OP_ADD_ADV_PATTERNS_MONITOR_RSSI);

}



static int remove_adv_monitor(struct sock *sk, struct hci_dev *hdev,
@@ -8242,6 +8320,9 @@ static const struct hci_mgmt_handler mgmt_handlers[] = { 
						HCI_MGMT_VAR_LEN },

	{ add_ext_adv_data,        MGMT_ADD_EXT_ADV_DATA_SIZE,

						HCI_MGMT_VAR_LEN },

	{ add_adv_patterns_monitor_rssi,

				   MGMT_ADD_ADV_PATTERNS_MONITOR_RSSI_SIZE,

						HCI_MGMT_VAR_LEN },

};



void mgmt_index_added(struct hci_dev *hdev)