diff options
| -rw-r--r-- | lib/mgmt.h | 28 | ||||
| -rw-r--r-- | monitor/packet.c | 4 | ||||
| -rw-r--r-- | src/advertising.c | 222 |
3 files changed, 237 insertions, 17 deletions
diff --git a/lib/mgmt.h b/lib/mgmt.h index 59608e7ea..7ab4fb797 100644 --- a/lib/mgmt.h +++ b/lib/mgmt.h @@ -503,6 +503,10 @@ struct mgmt_rp_add_advertising { #define MGMT_ADV_FLAG_SEC_1M (1 << 7) #define MGMT_ADV_FLAG_SEC_2M (1 << 8) #define MGMT_ADV_FLAG_SEC_CODED (1 << 9) +#define MGMT_ADV_PARAM_DURATION (1 << 12) +#define MGMT_ADV_PARAM_TIMEOUT (1 << 13) +#define MGMT_ADV_PARAM_INTERVALS (1 << 14) +#define MGMT_ADV_PARAM_TX_POWER (1 << 15) #define MGMT_OP_REMOVE_ADVERTISING 0x003F struct mgmt_cp_remove_advertising { @@ -701,8 +705,32 @@ struct mgmt_rp_remove_adv_monitor { } __packed; #define MGMT_OP_ADD_EXT_ADV_PARAMS 0x0054 +struct mgmt_cp_add_ext_adv_params { + uint8_t instance; + uint32_t flags; + uint16_t duration; + uint16_t timeout; + uint32_t min_interval; + uint32_t max_interval; + int8_t tx_power; +} __packed; +struct mgmt_rp_add_ext_adv_params { + uint8_t instance; + int8_t tx_power; + uint8_t max_adv_data_len; + uint8_t max_scan_rsp_len; +} __packed; #define MGMT_OP_ADD_EXT_ADV_DATA 0x0055 +struct mgmt_cp_add_ext_adv_data { + uint8_t instance; + uint8_t adv_data_len; + uint8_t scan_rsp_len; + uint8_t data[0]; +} __packed; +struct mgmt_rp_add_ext_adv_data { + uint8_t instance; +} __packed; #define MGMT_EV_CMD_COMPLETE 0x0001 struct mgmt_ev_cmd_complete { diff --git a/monitor/packet.c b/monitor/packet.c index dcbed9f0f..a97f2af52 100644 --- a/monitor/packet.c +++ b/monitor/packet.c @@ -6980,8 +6980,8 @@ static void le_set_ext_adv_params_cmd(const void *data, uint8_t size) print_peer_addr_type("Peer address type", cmd->peer_addr_type); print_addr("Peer address", cmd->peer_addr, cmd->peer_addr_type); print_adv_filter_policy("Filter policy", cmd->filter_policy); - if (cmd->tx_power == 0xff) - print_field("TX power: Host has no preference (0xff)"); + if (cmd->tx_power == 0x7f) + print_field("TX power: Host has no preference (0x7f)"); else print_power_level(cmd->tx_power, NULL); diff --git a/src/advertising.c b/src/advertising.c index 446ad1165..538c788a5 100644 --- a/src/advertising.c +++ b/src/advertising.c @@ -82,6 +82,7 @@ struct btd_adv_client { uint32_t min_interval; uint32_t max_interval; int8_t tx_power; + mgmt_request_func_t refresh_done_func; }; struct dbus_obj_match { @@ -788,19 +789,9 @@ static uint8_t *generate_scan_rsp(struct btd_adv_client *client, return bt_ad_generate(client->scan, len); } -static int refresh_adv(struct btd_adv_client *client, mgmt_request_func_t func, - unsigned int *mgmt_id) +static int get_adv_flags(struct btd_adv_client *client) { - struct mgmt_cp_add_advertising *cp; - uint8_t param_len; - uint8_t *adv_data; - size_t adv_data_len; - uint8_t *scan_rsp; - size_t scan_rsp_len = -1; uint32_t flags = 0; - unsigned int mgmt_ret; - - DBG("Refreshing advertisement: %s", client->path); if (client->type == AD_TYPE_PERIPHERAL) { flags = MGMT_ADV_FLAG_CONNECTABLE; @@ -812,6 +803,26 @@ static int refresh_adv(struct btd_adv_client *client, mgmt_request_func_t func, flags |= client->flags; + return flags; +} + +static int refresh_legacy_adv(struct btd_adv_client *client, + mgmt_request_func_t func, + unsigned int *mgmt_id) +{ + struct mgmt_cp_add_advertising *cp; + uint8_t param_len; + uint8_t *adv_data; + size_t adv_data_len; + uint8_t *scan_rsp; + size_t scan_rsp_len = -1; + uint32_t flags = 0; + unsigned int mgmt_ret; + + DBG("Refreshing advertisement: %s", client->path); + + flags = get_adv_flags(client); + adv_data = generate_adv_data(client, &flags, &adv_data_len); if (!adv_data || (adv_data_len > calc_max_adv_len(client, flags))) { error("Advertising data too long or couldn't be generated."); @@ -864,6 +875,74 @@ static int refresh_adv(struct btd_adv_client *client, mgmt_request_func_t func, return 0; } +static void add_adv_params_callback(uint8_t status, uint16_t length, + const void *param, void *user_data); + +static int refresh_extended_adv(struct btd_adv_client *client, + mgmt_request_func_t func, unsigned int *mgmt_id) +{ + struct mgmt_cp_add_ext_adv_params cp; + uint32_t flags = 0; + unsigned int mgmt_ret = 0; + + DBG("Refreshing advertisement parameters: %s", client->path); + + flags = get_adv_flags(client); + + memset(&cp, 0, sizeof(cp)); + cp.instance = client->instance; + + /* Not all advertising instances will use all possible parameters. The + * included_params bit field tells the kernel which parameters are + * relevant, and sensible defaults will be used for the rest + */ + + if (client->duration) { + cp.duration = client->duration; + flags |= MGMT_ADV_PARAM_DURATION; + } + + if (client->min_interval && client->max_interval) { + cp.min_interval = client->min_interval; + cp.max_interval = client->max_interval; + flags |= MGMT_ADV_PARAM_INTERVALS; + } + + if (client->tx_power != ADV_TX_POWER_NO_PREFERENCE) { + cp.tx_power = client->tx_power; + flags |= MGMT_ADV_PARAM_TX_POWER; + } + + cp.flags = htobl(flags); + + mgmt_ret = mgmt_send(client->manager->mgmt, MGMT_OP_ADD_EXT_ADV_PARAMS, + client->manager->mgmt_index, sizeof(cp), &cp, + add_adv_params_callback, client, NULL); + + if (!mgmt_ret) { + error("Failed to request extended advertising parameters"); + return -EINVAL; + } + + /* Store callback, called after we set advertising data */ + client->refresh_done_func = func; + + if (mgmt_id) + *mgmt_id = mgmt_ret; + + + return 0; +} + +static int refresh_advertisement(struct btd_adv_client *client, + mgmt_request_func_t func, unsigned int *mgmt_id) +{ + if (client->manager->extended_add_cmds) + return refresh_extended_adv(client, func, mgmt_id); + + return refresh_legacy_adv(client, func, mgmt_id); +} + static gboolean client_discoverable_timeout(void *user_data) { struct btd_adv_client *client = user_data; @@ -874,7 +953,7 @@ static gboolean client_discoverable_timeout(void *user_data) bt_ad_clear_flags(client->data); - refresh_adv(client, NULL, NULL); + refresh_advertisement(client, NULL, NULL); return FALSE; } @@ -1070,7 +1149,8 @@ static void properties_changed(GDBusProxy *proxy, const char *name, continue; if (parser->func(iter, client)) { - refresh_adv(client, NULL, NULL); + refresh_advertisement(client, NULL, NULL); + break; } } @@ -1133,6 +1213,112 @@ done: add_client_complete(client, status); } +static void add_adv_params_callback(uint8_t status, uint16_t length, + const void *param, void *user_data) +{ + struct btd_adv_client *client = user_data; + const struct mgmt_rp_add_ext_adv_params *rp = param; + struct mgmt_cp_add_ext_adv_data *cp = NULL; + uint8_t param_len; + uint8_t *adv_data = NULL; + size_t adv_data_len; + uint8_t *scan_rsp = NULL; + size_t scan_rsp_len = -1; + uint32_t flags = 0; + unsigned int mgmt_ret; + dbus_int16_t tx_power; + + if (status) + goto fail; + + if (!param || length < sizeof(*rp)) { + status = MGMT_STATUS_FAILED; + goto fail; + } + + DBG("Refreshing advertisement data: %s", client->path); + + /* Update tx power held by client */ + tx_power = rp->tx_power; + if (tx_power != ADV_TX_POWER_NO_PREFERENCE) + g_dbus_proxy_set_property_basic(client->proxy, "TxPower", + DBUS_TYPE_INT16, &tx_power, NULL, NULL, NULL); + + client->instance = rp->instance; + + flags = get_adv_flags(client); + + adv_data = generate_adv_data(client, &flags, &adv_data_len); + if (!adv_data || (adv_data_len > rp->max_adv_data_len)) { + error("Advertising data too long or couldn't be generated."); + goto fail; + } + + scan_rsp = generate_scan_rsp(client, &flags, &scan_rsp_len); + if ((!scan_rsp && scan_rsp_len) || + scan_rsp_len > rp->max_scan_rsp_len) { + error("Scan data couldn't be generated."); + goto fail; + } + + param_len = sizeof(struct mgmt_cp_add_advertising) + adv_data_len + + scan_rsp_len; + + cp = malloc0(param_len); + if (!cp) { + error("Couldn't allocate for MGMT!"); + goto fail; + } + + cp->instance = client->instance; + cp->adv_data_len = adv_data_len; + cp->scan_rsp_len = scan_rsp_len; + memcpy(cp->data, adv_data, adv_data_len); + memcpy(cp->data + adv_data_len, scan_rsp, scan_rsp_len); + + free(adv_data); + free(scan_rsp); + adv_data = NULL; + scan_rsp = NULL; + + /* Submit request to update instance data */ + mgmt_ret = mgmt_send(client->manager->mgmt, MGMT_OP_ADD_EXT_ADV_DATA, + client->manager->mgmt_index, param_len, cp, + client->refresh_done_func, client, NULL); + + /* Clear the callback */ + client->refresh_done_func = NULL; + + if (!mgmt_ret) { + error("Failed to add Advertising Data"); + goto fail; + } + + if (client->add_adv_id) + client->add_adv_id = mgmt_ret; + + free(cp); + cp = NULL; + + return; + +fail: + if (adv_data) + free(adv_data); + + if (scan_rsp) + free(scan_rsp); + + if (cp) + free(cp); + + if (!status) + status = -EINVAL; + + /* Failure for any reason ends this advertising request */ + add_client_complete(client, status); +} + static DBusMessage *parse_advertisement(struct btd_adv_client *client) { struct adv_parser *parser; @@ -1191,7 +1377,9 @@ static DBusMessage *parse_advertisement(struct btd_adv_client *client) goto fail; } - err = refresh_adv(client, add_adv_callback, &client->add_adv_id); + err = refresh_advertisement(client, add_adv_callback, + &client->add_adv_id); + if (!err) return NULL; @@ -1270,6 +1458,8 @@ static struct btd_adv_client *client_create(struct btd_adv_manager *manager, client->min_interval = 0; client->max_interval = 0; + client->refresh_done_func = NULL; + return client; fail: @@ -1586,7 +1776,9 @@ void btd_adv_manager_destroy(struct btd_adv_manager *manager) static void manager_refresh(void *data, void *user_data) { - refresh_adv(data, user_data, NULL); + struct btd_adv_client *client = data; + + refresh_advertisement(client, user_data, NULL); } void btd_adv_manager_refresh(struct btd_adv_manager *manager) |