diff -pruN 5.65-1/bootstrap-configure 5.66-1/bootstrap-configure --- 5.65-1/bootstrap-configure 2022-07-24 20:59:52.000000000 +0000 +++ 5.66-1/bootstrap-configure 2022-11-10 20:22:09.000000000 +0000 @@ -33,4 +33,5 @@ fi --enable-ubsan \ --enable-cups \ --enable-library \ + --enable-admin \ --disable-datafiles $* diff -pruN 5.65-1/ChangeLog 5.66-1/ChangeLog --- 5.65-1/ChangeLog 2022-07-24 20:59:52.000000000 +0000 +++ 5.66-1/ChangeLog 2022-11-10 20:22:09.000000000 +0000 @@ -1,3 +1,11 @@ +ver 5.66: + Fix issue with A2DP and transport connection collisions. + Fix issue with allowing application specific error codes. + Fix issue with not setting initiator flag correctly. + Fix issue with HoG Report MAP size handling. + Add initial support for Basic Audio Profile. + Add initial support for Volume Control Profile. + ver 5.65: Fix issue with A2DP cache invalidation handling. Fix issue with A2DP and not initialized SEP codec. diff -pruN 5.65-1/client/admin.c 5.66-1/client/admin.c --- 5.65-1/client/admin.c 2022-07-24 20:59:52.000000000 +0000 +++ 5.66-1/client/admin.c 2022-11-10 20:22:09.000000000 +0000 @@ -31,20 +31,22 @@ #include "admin.h" #define _GNU_SOURCE +static DBusConnection *dbus_conn; +static GList *admin_proxies; static GDBusProxy *set_proxy; static GDBusProxy *status_proxy; -void admin_policy_set_set_proxy(GDBusProxy *proxy) +static void admin_policy_set_set_proxy(GDBusProxy *proxy) { set_proxy = proxy; } -void admin_policy_set_status_proxy(GDBusProxy *proxy) +static void admin_policy_set_status_proxy(GDBusProxy *proxy) { status_proxy = proxy; } -void admin_policy_read_service_allowlist(DBusConnection *dbus_conn) +static void admin_policy_read_service_allowlist(DBusConnection *dbus_conn) { DBusMessageIter iter, subiter; char *uuid = NULL; @@ -111,8 +113,7 @@ static void set_service_reply(DBusMessag return bt_shell_noninteractive_quit(EXIT_FAILURE); } -void admin_policy_set_service_allowlist(DBusConnection *dbus_connd, - int argc, char *argv[]) +static void admin_policy_set_service_allowlist(int argc, char *argv[]) { struct uuid_list_data data; @@ -131,3 +132,89 @@ void admin_policy_set_service_allowlist( return bt_shell_noninteractive_quit(EXIT_FAILURE); } } + +static void cmd_admin_allow(int argc, char *argv[]) +{ + if (argc <= 1) { + admin_policy_read_service_allowlist(dbus_conn); + return; + } + + if (strcmp(argv[1], "clear") == 0) + argc--; + + admin_policy_set_service_allowlist(argc - 1, argv + 1); +} + +static const struct bt_shell_menu admin_menu = { + .name = "admin", + .desc = "Admin Policy Submenu", + .entries = { + { "allow", "[clear/uuid1 uuid2 ...]", cmd_admin_allow, + "Allow service UUIDs and block rest of them"}, + {} }, +}; + +static void admin_policy_status_added(GDBusProxy *proxy) +{ + admin_proxies = g_list_append(admin_proxies, proxy); + admin_policy_set_status_proxy(proxy); +} + +static void proxy_added(GDBusProxy *proxy, void *user_data) +{ + const char *interface; + + interface = g_dbus_proxy_get_interface(proxy); + + if (!strcmp(interface, "org.bluez.AdminPolicySet1")) + admin_policy_set_set_proxy(proxy); + else if (!strcmp(interface, "org.bluez.AdminPolicyStatus1")) + admin_policy_status_added(proxy); +} + +static void admin_policy_status_removed(GDBusProxy *proxy) +{ + admin_proxies = g_list_remove(admin_proxies, proxy); + admin_policy_set_status_proxy(NULL); +} + +static void proxy_removed(GDBusProxy *proxy, void *user_data) +{ + const char *interface; + + interface = g_dbus_proxy_get_interface(proxy); + + if (!strcmp(interface, "org.bluez.AdminPolicySet1")) + admin_policy_set_set_proxy(NULL); + else if (!strcmp(interface, "org.bluez.AdminPolicyStatus1")) + admin_policy_status_removed(proxy); +} + +static GDBusClient *client; + +static void disconnect_handler(DBusConnection *connection, void *user_data) +{ + g_list_free_full(admin_proxies, NULL); + admin_proxies = NULL; +} + +void admin_add_submenu(void) +{ + bt_shell_add_submenu(&admin_menu); + + dbus_conn = bt_shell_get_env("DBUS_CONNECTION"); + if (!dbus_conn || client) + return; + + client = g_dbus_client_new(dbus_conn, "org.bluez", "/org/bluez"); + g_dbus_client_set_proxy_handlers(client, proxy_added, proxy_removed, + NULL, NULL); + g_dbus_client_set_disconnect_watch(client, disconnect_handler, NULL); +} + +void admin_remove_submenu(void) +{ + g_dbus_client_unref(client); + client = NULL; +} diff -pruN 5.65-1/client/admin.h 5.66-1/client/admin.h --- 5.65-1/client/admin.h 2022-07-24 20:59:52.000000000 +0000 +++ 5.66-1/client/admin.h 2022-11-10 20:22:09.000000000 +0000 @@ -17,9 +17,5 @@ * */ -void admin_policy_set_set_proxy(GDBusProxy *proxy); -void admin_policy_set_status_proxy(GDBusProxy *proxy); - -void admin_policy_read_service_allowlist(DBusConnection *dbus_conn); -void admin_policy_set_service_allowlist(DBusConnection *dbus_conn, - int argc, char *argv[]); +void admin_add_submenu(void); +void admin_remove_submenu(void); diff -pruN 5.65-1/client/advertising.c 5.66-1/client/advertising.c --- 5.65-1/client/advertising.c 2022-07-24 20:59:52.000000000 +0000 +++ 5.66-1/client/advertising.c 2022-11-10 20:22:09.000000000 +0000 @@ -329,7 +329,8 @@ static gboolean get_includes(const GDBus return TRUE; } -static gboolean local_name_exits(const GDBusPropertyTable *property, void *data) +static gboolean local_name_exists(const GDBusPropertyTable *property, + void *data) { return ad.local_name ? TRUE : FALSE; } @@ -342,7 +343,8 @@ static gboolean get_local_name(const GDB return TRUE; } -static gboolean appearance_exits(const GDBusPropertyTable *property, void *data) +static gboolean appearance_exists(const GDBusPropertyTable *property, + void *data) { return ad.local_appearance != UINT16_MAX ? TRUE : FALSE; } @@ -356,7 +358,7 @@ static gboolean get_appearance(const GDB return TRUE; } -static gboolean duration_exits(const GDBusPropertyTable *property, void *data) +static gboolean duration_exists(const GDBusPropertyTable *property, void *data) { return ad.duration; } @@ -369,7 +371,7 @@ static gboolean get_duration(const GDBus return TRUE; } -static gboolean timeout_exits(const GDBusPropertyTable *property, void *data) +static gboolean timeout_exists(const GDBusPropertyTable *property, void *data) { return ad.timeout; } @@ -420,7 +422,7 @@ static gboolean get_discoverable(const G return TRUE; } -static gboolean discoverable_timeout_exits(const GDBusPropertyTable *property, +static gboolean discoverable_timeout_exists(const GDBusPropertyTable *property, void *data) { return ad.discoverable_to; @@ -435,7 +437,7 @@ static gboolean get_discoverable_timeout return TRUE; } -static gboolean secondary_exits(const GDBusPropertyTable *property, void *data) +static gboolean secondary_exists(const GDBusPropertyTable *property, void *data) { return ad.secondary ? TRUE : FALSE; } @@ -449,7 +451,7 @@ static gboolean get_secondary(const GDBu return TRUE; } -static gboolean min_interval_exits(const GDBusPropertyTable *property, +static gboolean min_interval_exists(const GDBusPropertyTable *property, void *data) { return ad.min_interval; @@ -464,7 +466,7 @@ static gboolean get_min_interval(const G return TRUE; } -static gboolean max_interval_exits(const GDBusPropertyTable *property, +static gboolean max_interval_exists(const GDBusPropertyTable *property, void *data) { return ad.max_interval; @@ -488,15 +490,15 @@ static const GDBusPropertyTable ad_props { "Data", "a{yv}", get_data, NULL, data_exists }, { "Discoverable", "b", get_discoverable, NULL, discoverable_exists }, { "DiscoverableTimeout", "q", get_discoverable_timeout, NULL, - discoverable_timeout_exits }, + discoverable_timeout_exists }, { "Includes", "as", get_includes, NULL, includes_exists }, - { "LocalName", "s", get_local_name, NULL, local_name_exits }, - { "Appearance", "q", get_appearance, NULL, appearance_exits }, - { "Duration", "q", get_duration, NULL, duration_exits }, - { "Timeout", "q", get_timeout, NULL, timeout_exits }, - { "MinInterval", "u", get_min_interval, NULL, min_interval_exits }, - { "MaxInterval", "u", get_max_interval, NULL, max_interval_exits }, - { "SecondaryChannel", "s", get_secondary, NULL, secondary_exits }, + { "LocalName", "s", get_local_name, NULL, local_name_exists }, + { "Appearance", "q", get_appearance, NULL, appearance_exists }, + { "Duration", "q", get_duration, NULL, duration_exists }, + { "Timeout", "q", get_timeout, NULL, timeout_exists }, + { "MinInterval", "u", get_min_interval, NULL, min_interval_exists }, + { "MaxInterval", "u", get_max_interval, NULL, max_interval_exists }, + { "SecondaryChannel", "s", get_secondary, NULL, secondary_exists }, { } }; @@ -758,7 +760,7 @@ void ad_advertise_data(DBusConnection *c struct ad_data data; if (argc < 2 || !strlen(argv[1])) { - if (ad.manufacturer.data.len) { + if (ad.data.data.len) { bt_shell_printf("Type: 0x%02x\n", ad.data.type); bt_shell_hexdump(ad.data.data.data, ad.data.data.len); } diff -pruN 5.65-1/client/gatt.c 5.66-1/client/gatt.c --- 5.65-1/client/gatt.c 2022-07-24 20:59:52.000000000 +0000 +++ 5.66-1/client/gatt.c 2022-11-10 20:22:09.000000000 +0000 @@ -2143,7 +2143,8 @@ static int write_value(size_t *dst_len, *dst_value = g_realloc(*dst_value, *dst_len); } - memcpy(*dst_value + offset, src_val, src_len); + if (src_val && src_len) + memcpy(*dst_value + offset, src_val, src_len); return 0; } @@ -2518,7 +2519,7 @@ static DBusMessage *chrc_start_notify(DB chrc->notifying = true; bt_shell_printf("[" COLORED_CHG "] Attribute %s (%s) notifications " - "enabled", chrc->path, bt_uuidstr_to_str(chrc->uuid)); + "enabled\n", chrc->path, bt_uuidstr_to_str(chrc->uuid)); g_dbus_emit_property_changed(conn, chrc->path, CHRC_INTERFACE, "Notifying"); @@ -2538,7 +2539,8 @@ static DBusMessage *chrc_stop_notify(DBu chrc->notifying = false; bt_shell_printf("[" COLORED_CHG "] Attribute %s (%s) notifications " - "disabled", chrc->path, bt_uuidstr_to_str(chrc->uuid)); + "disabled\n", chrc->path, + bt_uuidstr_to_str(chrc->uuid)); g_dbus_emit_property_changed(conn, chrc->path, CHRC_INTERFACE, "Notifying"); @@ -3002,17 +3004,20 @@ static void proxy_property_changed(GDBus chrc->path, bt_uuidstr_to_str(chrc->uuid), name); if (!strcmp(name, "Value")) { - DBusMessageIter array; - uint8_t *value; - int len; + uint8_t *value = '\0'; /* don't pass NULL to write_value() */ + int len = 0; + + if (iter && dbus_message_iter_get_arg_type(iter) == + DBUS_TYPE_ARRAY) { + DBusMessageIter array; - if (dbus_message_iter_get_arg_type(iter) == DBUS_TYPE_ARRAY) { dbus_message_iter_recurse(iter, &array); dbus_message_iter_get_fixed_array(&array, &value, &len); - write_value(&chrc->value_len, &chrc->value, value, len, - 0, chrc->max_val_len); - bt_shell_hexdump(value, len); } + + write_value(&chrc->value_len, &chrc->value, value, len, + 0, chrc->max_val_len); + bt_shell_hexdump(value, len); } g_dbus_emit_property_changed(conn, chrc->path, CHRC_INTERFACE, name); diff -pruN 5.65-1/client/main.c 5.66-1/client/main.c --- 5.65-1/client/main.c 2022-07-24 20:59:52.000000000 +0000 +++ 5.66-1/client/main.c 2022-11-10 20:22:09.000000000 +0000 @@ -57,7 +57,6 @@ static GDBusProxy *default_dev; static GDBusProxy *default_attr; static GList *ctrl_list; static GList *battery_proxies; -static GList *admin_devices_proxies; static const char *agent_arguments[] = { "on", @@ -563,26 +562,6 @@ static void admon_manager_added(GDBusPro adv_monitor_register_app(dbus_conn); } -static void admin_policy_set_added(GDBusProxy *proxy) -{ - admin_policy_set_set_proxy(proxy); -} - -static void admin_policy_status_added(GDBusProxy *proxy) -{ - struct adapter *adapter; - - adapter = find_ctrl(ctrl_list, g_dbus_proxy_get_path(proxy)); - - if (!adapter) { - admin_devices_proxies = g_list_append(admin_devices_proxies, - proxy); - return; - } - - admin_policy_set_status_proxy(proxy); -} - static void proxy_added(GDBusProxy *proxy, void *user_data) { const char *interface; @@ -618,10 +597,6 @@ static void proxy_added(GDBusProxy *prox } else if (!strcmp(interface, "org.bluez.AdvertisementMonitorManager1")) { admon_manager_added(proxy); - } else if (!strcmp(interface, "org.bluez.AdminPolicySet1")) { - admin_policy_set_added(proxy); - } else if (!strcmp(interface, "org.bluez.AdminPolicyStatus1")) { - admin_policy_status_added(proxy); } } @@ -678,26 +653,6 @@ static void adapter_removed(GDBusProxy * } } -static void admin_policy_set_removed(GDBusProxy *proxy) -{ - admin_policy_set_set_proxy(NULL); -} - -static void admin_policy_status_removed(GDBusProxy *proxy) -{ - struct adapter *adapter; - - adapter = find_ctrl(ctrl_list, g_dbus_proxy_get_path(proxy)); - - if (!adapter) { - admin_devices_proxies = g_list_remove(admin_devices_proxies, - proxy); - return; - } - - admin_policy_set_status_proxy(NULL); -} - static void proxy_removed(GDBusProxy *proxy, void *user_data) { const char *interface; @@ -738,10 +693,6 @@ static void proxy_removed(GDBusProxy *pr } else if (!strcmp(interface, "org.bluez.AdvertisementMonitorManager1")) { adv_monitor_remove_manager(dbus_conn); - } else if (!strcmp(interface, "org.bluez.AdminPolicySet1")) { - admin_policy_set_removed(proxy); - } else if (!strcmp(interface, "org.bluez.AdminPolicyStatus1")) { - admin_policy_status_removed(proxy); } } @@ -1030,6 +981,7 @@ static void cmd_show(int argc, char *arg print_property(adapter->proxy, "Alias"); print_property(adapter->proxy, "Class"); print_property(adapter->proxy, "Powered"); + print_property(adapter->proxy, "PowerState"); print_property(adapter->proxy, "Discoverable"); print_property(adapter->proxy, "DiscoverableTimeout"); print_property(adapter->proxy, "Pairable"); @@ -1772,7 +1724,6 @@ static struct GDBusProxy *find_device(in static void cmd_info(int argc, char *argv[]) { GDBusProxy *proxy; - GDBusProxy *admin_proxy; GDBusProxy *battery_proxy; DBusMessageIter iter; const char *address; @@ -1819,12 +1770,8 @@ static void cmd_info(int argc, char *arg battery_proxy = find_proxies_by_path(battery_proxies, g_dbus_proxy_get_path(proxy)); - admin_proxy = find_proxies_by_path(admin_devices_proxies, - g_dbus_proxy_get_path(proxy)); print_property_with_label(battery_proxy, "Percentage", "Battery Percentage"); - print_property_with_label(admin_proxy, "AffectedByPolicy", - "Affected by Policy"); return bt_shell_noninteractive_quit(EXIT_SUCCESS); } @@ -2569,7 +2516,7 @@ static void cmd_advertise(int argc, char if (!default_ctrl || !default_ctrl->ad_proxy) { bt_shell_printf("LEAdvertisingManager not found\n"); - bt_shell_noninteractive_quit(EXIT_FAILURE); + return bt_shell_noninteractive_quit(EXIT_FAILURE); } if (enable == TRUE) @@ -2948,22 +2895,6 @@ static void cmd_adv_monitor_get_supporte adv_monitor_get_supported_info(); } -static void cmd_admin_allow(int argc, char *argv[]) -{ - if (check_default_ctrl() == FALSE) - return bt_shell_noninteractive_quit(EXIT_FAILURE); - - if (argc <= 1) { - admin_policy_read_service_allowlist(dbus_conn); - return; - } - - if (strcmp(argv[1], "clear") == 0) - argc--; - - admin_policy_set_service_allowlist(dbus_conn, argc - 1, argv + 1); -} - static const struct bt_shell_menu advertise_menu = { .name = "advertise", .desc = "Advertise Options Submenu", @@ -3118,15 +3049,6 @@ static const struct bt_shell_menu gatt_m { } }, }; -static const struct bt_shell_menu admin_menu = { - .name = "admin", - .desc = "Admin Policy Submenu", - .entries = { - { "allow", "[clear/uuid1 uuid2 ...]", cmd_admin_allow, - "Allow service UUIDs and block rest of them"}, - {} }, -}; - static const struct bt_shell_menu main_menu = { .name = "main", .entries = { @@ -3188,23 +3110,27 @@ static const struct bt_shell_menu main_m static const struct option options[] = { { "agent", required_argument, 0, 'a' }, + { "endpoints", no_argument, 0, 'e' }, { 0, 0, 0, 0 } }; static const char *agent_option; +static const char *endpoint_option; static const char **optargs[] = { - &agent_option + &agent_option, + &endpoint_option }; static const char *help[] = { - "Register agent handler: " + "Register agent handler: ", + "Register Media endpoints" }; static const struct bt_shell_opt opt = { .options = options, .optno = sizeof(options) / sizeof(struct option), - .optstr = "a:", + .optstr = "a:e", .optarg = optargs, .help = help, }; @@ -3225,7 +3151,6 @@ int main(int argc, char *argv[]) bt_shell_add_submenu(&advertise_monitor_menu); bt_shell_add_submenu(&scan_menu); bt_shell_add_submenu(&gatt_menu); - bt_shell_add_submenu(&admin_menu); bt_shell_set_prompt(PROMPT_OFF); if (agent_option) @@ -3238,6 +3163,11 @@ int main(int argc, char *argv[]) bt_shell_set_env("DBUS_CONNECTION", dbus_conn); + if (endpoint_option) + bt_shell_set_env("AUTO_REGISTER_ENDPOINT", + (void *)endpoint_option); + + admin_add_submenu(); player_add_submenu(); client = g_dbus_client_new(dbus_conn, "org.bluez", "/org/bluez"); @@ -3253,6 +3183,7 @@ int main(int argc, char *argv[]) status = bt_shell_run(); + admin_remove_submenu(); player_remove_submenu(); g_dbus_client_unref(client); diff -pruN 5.65-1/client/player.c 5.66-1/client/player.c --- 5.65-1/client/player.c 2022-07-24 20:59:52.000000000 +0000 +++ 5.66-1/client/player.c 2022-11-10 20:22:09.000000000 +0000 @@ -15,7 +15,7 @@ #define _GNU_SOURCE #include #include -#include +#include #include #include #include @@ -33,6 +33,7 @@ #include "lib/uuid.h" #include "profiles/audio/a2dp-codecs.h" +#include "src/shared/lc3.h" #include "src/shared/util.h" #include "src/shared/shell.h" @@ -64,6 +65,9 @@ struct endpoint { uint8_t codec; struct iovec *caps; bool auto_accept; + bool acquiring; + uint8_t cig; + uint8_t cis; char *transport; DBusMessage *msg; }; @@ -507,41 +511,6 @@ static char *proxy_description(GDBusProx title, path); } -static void print_media(GDBusProxy *proxy, const char *description) -{ - char *str; - - str = proxy_description(proxy, "Media", description); - - bt_shell_printf("%s\n", str); - - g_free(str); -} - -static void print_player(GDBusProxy *proxy, const char *description) -{ - char *str; - - str = proxy_description(proxy, "Player", description); - - bt_shell_printf("%s%s\n", str, - default_player == proxy ? "[default]" : ""); - - g_free(str); -} - -static void cmd_list(int argc, char *arg[]) -{ - GList *l; - - for (l = players; l; l = g_list_next(l)) { - GDBusProxy *proxy = l->data; - print_player(proxy, NULL); - } - - return bt_shell_noninteractive_quit(EXIT_SUCCESS); -} - static void print_iter(const char *label, const char *name, DBusMessageIter *iter) { @@ -624,6 +593,39 @@ static void print_property(GDBusProxy *p print_iter("\t", name, &iter); } +static void print_media(GDBusProxy *proxy, const char *description) +{ + char *str; + + str = proxy_description(proxy, "Media", description); + + bt_shell_printf("%s\n", str); + print_property(proxy, "SupportedUUIDs"); + + g_free(str); +} + +static void print_player(void *data, void *user_data) +{ + GDBusProxy *proxy = data; + const char *description = user_data; + char *str; + + str = proxy_description(proxy, "Player", description); + + bt_shell_printf("%s%s\n", str, + default_player == proxy ? "[default]" : ""); + + g_free(str); +} + +static void cmd_list(int argc, char *arg[]) +{ + g_list_foreach(players, print_player, NULL); + + return bt_shell_noninteractive_quit(EXIT_SUCCESS); +} + static void cmd_show_item(int argc, char *argv[]) { GDBusProxy *proxy; @@ -1148,7 +1150,7 @@ struct codec_capabilities { #define data(args...) ((const unsigned char[]) { args }) -#define SBC_DATA(args...) \ +#define CODEC_DATA(args...) \ { \ .iov_base = (void *)data(args), \ .iov_len = sizeof(data(args)), \ @@ -1161,6 +1163,13 @@ struct codec_capabilities { .data = _data, \ } +#define LC3_DATA(_freq, _duration, _chan_count, _len_min, _len_max) \ + CODEC_DATA(0x03, LC3_FREQ, _freq, _freq >> 8, \ + 0x02, LC3_DURATION, _duration, \ + 0x02, LC3_CHAN_COUNT, _chan_count, \ + 0x05, LC3_FRAME_LEN, _len_min, _len_min >> 8, _len_max, \ + _len_max >> 8) + static const struct capabilities { const char *uuid; uint8_t codec_id; @@ -1175,7 +1184,7 @@ static const struct capabilities { * Bitpool Range: 2-64 */ CODEC_CAPABILITIES(A2DP_SOURCE_UUID, A2DP_CODEC_SBC, - SBC_DATA(0xff, 0xff, 2, 64)), + CODEC_DATA(0xff, 0xff, 2, 64)), /* A2DP SBC Sink: * * Channel Modes: Mono DualChannel Stereo JointStereo @@ -1185,13 +1194,45 @@ static const struct capabilities { * Bitpool Range: 2-64 */ CODEC_CAPABILITIES(A2DP_SINK_UUID, A2DP_CODEC_SBC, - SBC_DATA(0xff, 0xff, 2, 64)), + CODEC_DATA(0xff, 0xff, 2, 64)), + /* PAC LC3 Sink: + * + * Frequencies: 8Khz 11Khz 16Khz 22Khz 24Khz 32Khz 44.1Khz 48Khz + * Duration: 7.5 ms 10 ms + * Channel count: 3 + * Frame length: 30-240 + */ + CODEC_CAPABILITIES(PAC_SINK_UUID, LC3_ID, + LC3_DATA(LC3_FREQ_ANY, LC3_DURATION_ANY, + 3u, 30, 240)), + /* PAC LC3 Source: + * + * Frequencies: 8Khz 11Khz 16Khz 22Khz 24Khz 32Khz 44.1Khz 48Khz + * Duration: 7.5 ms 10 ms + * Channel count: 3 + * Frame length: 30-240 + */ + CODEC_CAPABILITIES(PAC_SOURCE_UUID, LC3_ID, + LC3_DATA(LC3_FREQ_ANY, LC3_DURATION_ANY, + 3u, 30, 240)), +}; + +struct codec_qos { + uint32_t interval; + uint8_t framing; + char *phy; + uint16_t sdu; + uint8_t rtn; + uint16_t latency; + uint32_t delay; }; struct codec_preset { const char *name; const struct iovec data; + const struct codec_qos qos; bool is_default; + uint8_t latency; }; #define SBC_PRESET(_name, _data) \ @@ -1216,32 +1257,212 @@ static struct codec_preset sbc_presets[] * mono, and 512kb/s for two-channel modes. */ SBC_PRESET("MQ_MONO_44_1", - SBC_DATA(0x28, 0x15, 2, SBC_BITPOOL_MQ_MONO_44100)), + CODEC_DATA(0x28, 0x15, 2, SBC_BITPOOL_MQ_MONO_44100)), SBC_PRESET("MQ_MONO_48", - SBC_DATA(0x18, 0x15, 2, SBC_BITPOOL_MQ_MONO_48000)), + CODEC_DATA(0x18, 0x15, 2, SBC_BITPOOL_MQ_MONO_48000)), SBC_PRESET("MQ_STEREO_44_1", - SBC_DATA(0x21, 0x15, 2, SBC_BITPOOL_MQ_JOINT_STEREO_44100)), + CODEC_DATA(0x21, 0x15, 2, SBC_BITPOOL_MQ_JOINT_STEREO_44100)), SBC_PRESET("MQ_STEREO_48", - SBC_DATA(0x11, 0x15, 2, SBC_BITPOOL_MQ_JOINT_STEREO_48000)), + CODEC_DATA(0x11, 0x15, 2, SBC_BITPOOL_MQ_JOINT_STEREO_48000)), SBC_PRESET("HQ_MONO_44_1", - SBC_DATA(0x28, 0x15, 2, SBC_BITPOOL_HQ_MONO_44100)), + CODEC_DATA(0x28, 0x15, 2, SBC_BITPOOL_HQ_MONO_44100)), SBC_PRESET("HQ_MONO_48", - SBC_DATA(0x18, 0x15, 2, SBC_BITPOOL_HQ_MONO_48000)), + CODEC_DATA(0x18, 0x15, 2, SBC_BITPOOL_HQ_MONO_48000)), SBC_DEFAULT_PRESET("HQ_STEREO_44_1", - SBC_DATA(0x21, 0x15, 2, SBC_BITPOOL_HQ_JOINT_STEREO_44100)), + CODEC_DATA(0x21, 0x15, 2, SBC_BITPOOL_HQ_JOINT_STEREO_44100)), SBC_PRESET("HQ_STEREO_48", - SBC_DATA(0x11, 0x15, 2, SBC_BITPOOL_HQ_JOINT_STEREO_48000)), + CODEC_DATA(0x11, 0x15, 2, SBC_BITPOOL_HQ_JOINT_STEREO_48000)), /* Higher bitrates not recommended by A2DP spec, it dual channel to * avoid going above 53 bitpool: * * https://habr.com/en/post/456476/ * https://gitlab.freedesktop.org/pulseaudio/pulseaudio/-/issues/1092 */ - SBC_PRESET("XQ_DUAL_44_1", SBC_DATA(0x24, 0x15, 2, 43)), - SBC_PRESET("XQ_DUAL_48", SBC_DATA(0x14, 0x15, 2, 39)), + SBC_PRESET("XQ_DUAL_44_1", CODEC_DATA(0x24, 0x15, 2, 43)), + SBC_PRESET("XQ_DUAL_48", CODEC_DATA(0x14, 0x15, 2, 39)), /* Ultra high bitpool that fits in 512 kbps mandatory bitrate */ - SBC_PRESET("UQ_STEREO_44_1", SBC_DATA(0x21, 0x15, 2, 64)), - SBC_PRESET("UQ_STEREO_48", SBC_DATA(0x11, 0x15, 2, 58)), + SBC_PRESET("UQ_STEREO_44_1", CODEC_DATA(0x21, 0x15, 2, 64)), + SBC_PRESET("UQ_STEREO_48", CODEC_DATA(0x11, 0x15, 2, 58)), +}; + +#define QOS_CONFIG(_interval, _framing, _phy, _sdu, _rtn, _latency, _delay) \ + { \ + .interval = _interval, \ + .framing = _framing, \ + .phy = _phy, \ + .sdu = _sdu, \ + .rtn = _rtn, \ + .latency = _latency, \ + .delay = _delay, \ + } + +#define QOS_UNFRAMED(_interval, _phy, _sdu, _rtn, _latency, _delay) \ + QOS_CONFIG(_interval, 0x00, _phy, _sdu, _rtn, _latency, _delay) + +#define QOS_FRAMED(_interval, _phy, _sdu, _rtn, _latency, _delay) \ + QOS_CONFIG(_interval, 0x01, _phy, _sdu, _rtn, _latency, _delay) + +#define QOS_UNFRAMED_1M(_interval, _sdu, _rtn, _latency, _delay) \ + QOS_UNFRAMED(_interval, "1M", _sdu, _rtn, _latency, _delay) \ + +#define QOS_FRAMED_1M(_interval, _sdu, _rtn, _latency, _delay) \ + QOS_FRAMED(_interval, "1M", _sdu, _rtn, _latency, _delay) \ + +#define QOS_UNFRAMED_2M(_interval, _sdu, _rtn, _latency, _delay) \ + QOS_UNFRAMED(_interval, "2M", _sdu, _rtn, _latency, _delay) \ + +#define QOS_FRAMED_2M(_interval, _sdu, _rtn, _latency, _delay) \ + QOS_FRAMED(_interval, "2M", _sdu, _rtn, _latency, _delay) \ + +#define LC3_7_5_UNFRAMED(_sdu, _rtn, _latency, _delay) \ + QOS_UNFRAMED(7500u, "2M", _sdu, _rtn, _latency, _delay) + +#define LC3_7_5_FRAMED(_sdu, _rtn, _latency, _delay) \ + QOS_FRAMED(7500u, "2M", _sdu, _rtn, _latency, _delay) + +#define LC3_10_UNFRAMED(_sdu, _rtn, _latency, _delay) \ + QOS_UNFRAMED_2M(10000u, _sdu, _rtn, _latency, _delay) + +#define LC3_10_FRAMED(_sdu, _rtn, _latency, _delay) \ + QOS_FRAMED_2M(10000u, _sdu, _rtn, _latency, _delay) + +#define LC3_PRESET_DATA(_freq, _duration, _len) \ + CODEC_DATA(0x02, LC3_CONFIG_FREQ, _freq, \ + 0x02, LC3_CONFIG_DURATION, _duration, \ + 0x03, LC3_CONFIG_FRAME_LEN, _len, _len >> 8) + +#define LC3_PRESET_8KHZ(_duration, _len) \ + LC3_PRESET_DATA(LC3_CONFIG_FREQ_8KHZ, _duration, _len) + +#define LC3_PRESET_11KHZ(_duration, _len) \ + LC3_PRESET_DATA(LC3_CONFIG_FREQ_11KHZ, _duration, _len) + +#define LC3_PRESET_16KHZ(_duration, _len) \ + LC3_PRESET_DATA(LC3_CONFIG_FREQ_16KHZ, _duration, _len) + +#define LC3_PRESET_22KHZ(_duration, _len) \ + LC3_PRESET_DATA(LC3_CONFIG_FREQ_22KHZ, _duration, _len) + +#define LC3_PRESET_24KHZ(_duration, _len) \ + LC3_PRESET_DATA(LC3_CONFIG_FREQ_24KHZ, _duration, _len) + +#define LC3_PRESET_32KHZ(_duration, _len) \ + LC3_PRESET_DATA(LC3_CONFIG_FREQ_32KHZ, _duration, _len) + +#define LC3_PRESET_44KHZ(_duration, _len) \ + LC3_PRESET_DATA(LC3_CONFIG_FREQ_44KHZ, _duration, _len) + +#define LC3_PRESET_48KHZ(_duration, _len) \ + LC3_PRESET_DATA(LC3_CONFIG_FREQ_48KHZ, _duration, _len) + +#define LC3_PRESET_LL(_name, _data, _qos) \ + { \ + .name = _name, \ + .data = _data, \ + .qos = _qos, \ + .latency = 0x01, \ + } + +#define LC3_PRESET(_name, _data, _qos) \ + { \ + .name = _name, \ + .data = _data, \ + .qos = _qos, \ + .latency = 0x02, \ + } + +#define LC3_PRESET_HR(_name, _data, _qos) \ + { \ + .name = _name, \ + .data = _data, \ + .qos = _qos, \ + .latency = 0x03, \ + } + +#define LC3_DEFAULT_PRESET(_name, _data, _qos) \ + { \ + .name = _name, \ + .data = _data, \ + .is_default = true, \ + .qos = _qos, \ + .latency = 0x02, \ + } + +static struct codec_preset lc3_presets[] = { + /* Table 4.43: QoS configuration support setting requirements */ + LC3_PRESET("8_1_1", + LC3_PRESET_8KHZ(LC3_CONFIG_DURATION_7_5, 26u), + LC3_7_5_UNFRAMED(26u, 2u, 8u, 40000u)), + LC3_PRESET("8_2_1", + LC3_PRESET_8KHZ(LC3_CONFIG_DURATION_10, 30u), + LC3_10_UNFRAMED(30u, 2u, 10u, 40000u)), + LC3_PRESET("16_1_1", + LC3_PRESET_16KHZ(LC3_CONFIG_DURATION_7_5, 30u), + LC3_7_5_UNFRAMED(30u, 2u, 8u, 40000u)), + LC3_DEFAULT_PRESET("16_2_1", + LC3_PRESET_16KHZ(LC3_CONFIG_DURATION_10, 40u), + LC3_10_UNFRAMED(40u, 2u, 10u, 40000u)), + LC3_PRESET("24_1_1", + LC3_PRESET_24KHZ(LC3_CONFIG_DURATION_7_5, 45u), + LC3_7_5_UNFRAMED(45u, 2u, 8u, 40000u)), + LC3_PRESET("24_2_1", + LC3_PRESET_24KHZ(LC3_CONFIG_DURATION_10, 60u), + LC3_10_UNFRAMED(60u, 2u, 10u, 40000u)), + LC3_PRESET("32_1_1", + LC3_PRESET_32KHZ(LC3_CONFIG_DURATION_7_5, 60u), + LC3_7_5_UNFRAMED(60u, 2u, 8u, 40000u)), + LC3_PRESET("32_2_1", + LC3_PRESET_32KHZ(LC3_CONFIG_DURATION_10, 80u), + LC3_10_UNFRAMED(80u, 2u, 10u, 40000u)), + LC3_PRESET("44_1_1", + LC3_PRESET_44KHZ(LC3_CONFIG_DURATION_7_5, 98u), + QOS_FRAMED_2M(8163u, 98u, 5u, 24u, 40000u)), + LC3_PRESET("44_2_1", + LC3_PRESET_44KHZ(LC3_CONFIG_DURATION_10, 130u), + QOS_FRAMED_2M(10884u, 130u, 5u, 31u, 40000u)), + LC3_PRESET("48_1_1", + LC3_PRESET_48KHZ(LC3_CONFIG_DURATION_7_5, 75u), + LC3_7_5_UNFRAMED(75u, 5u, 15u, 40000u)), + LC3_PRESET("48_2_1", + LC3_PRESET_48KHZ(LC3_CONFIG_DURATION_10, 100u), + LC3_10_UNFRAMED(100u, 5u, 20u, 40000u)), + LC3_PRESET("48_3_1", + LC3_PRESET_48KHZ(LC3_CONFIG_DURATION_7_5, 90u), + LC3_7_5_UNFRAMED(90u, 5u, 15u, 40000u)), + LC3_PRESET("48_4_1", + LC3_PRESET_48KHZ(LC3_CONFIG_DURATION_10, 120u), + LC3_10_UNFRAMED(120u, 5u, 20u, 40000u)), + LC3_PRESET("48_5_1", + LC3_PRESET_48KHZ(LC3_CONFIG_DURATION_7_5, 117u), + LC3_7_5_UNFRAMED(117u, 5u, 15u, 40000u)), + LC3_PRESET("48_6_1", + LC3_PRESET_48KHZ(LC3_CONFIG_DURATION_10, 155u), + LC3_10_UNFRAMED(155u, 5u, 20u, 40000u)), + /* QoS Configuration settings for high reliability audio data */ + LC3_PRESET_HR("44_1_2", + LC3_PRESET_44KHZ(LC3_CONFIG_DURATION_7_5, 98u), + QOS_FRAMED_2M(8163u, 98u, 23u, 54u, 40000u)), + LC3_PRESET_HR("44_2_2", + LC3_PRESET_44KHZ(LC3_CONFIG_DURATION_10, 130u), + QOS_FRAMED_2M(10884u, 130u, 23u, 71u, 40000u)), + LC3_PRESET_HR("48_1_2", + LC3_PRESET_48KHZ(LC3_CONFIG_DURATION_7_5, 75u), + LC3_7_5_UNFRAMED(75u, 23u, 45u, 40000u)), + LC3_PRESET_HR("48_2_2", + LC3_PRESET_48KHZ(LC3_CONFIG_DURATION_10, 100u), + LC3_10_UNFRAMED(100u, 23u, 60u, 40000u)), + LC3_PRESET_HR("48_3_2", + LC3_PRESET_48KHZ(LC3_CONFIG_DURATION_7_5, 90u), + LC3_7_5_UNFRAMED(90u, 23u, 45u, 40000u)), + LC3_PRESET_HR("48_4_2", + LC3_PRESET_48KHZ(LC3_CONFIG_DURATION_10, 120u), + LC3_10_UNFRAMED(120u, 23u, 60u, 40000u)), + LC3_PRESET_HR("48_5_2", + LC3_PRESET_48KHZ(LC3_CONFIG_DURATION_7_5, 117u), + LC3_7_5_UNFRAMED(117u, 23u, 45u, 40000u)), + LC3_PRESET_HR("48_6_2", + LC3_PRESET_48KHZ(LC3_CONFIG_DURATION_10, 155u), + LC3_10_UNFRAMED(155u, 23u, 60u, 40000u)), }; #define PRESET(_uuid, _presets) \ @@ -1258,6 +1479,8 @@ static const struct preset { } presets[] = { PRESET(A2DP_SOURCE_UUID, sbc_presets), PRESET(A2DP_SINK_UUID, sbc_presets), + PRESET(PAC_SINK_UUID, lc3_presets), + PRESET(PAC_SOURCE_UUID, lc3_presets), }; static struct codec_preset *find_preset(const char *uuid, const char *name) @@ -1400,10 +1623,7 @@ static DBusMessage *endpoint_select_conf } p = find_preset(ep->uuid, NULL); - if (!p) - NULL; - - if (p->data.iov_base) { + if (!p) { reply = g_dbus_create_error(msg, "org.bluez.Error.Rejected", NULL); return reply; @@ -1419,6 +1639,190 @@ static DBusMessage *endpoint_select_conf return reply; } +struct endpoint_config { + GDBusProxy *proxy; + struct endpoint *ep; + struct iovec *caps; + uint8_t target_latency; + const struct codec_qos *qos; +}; + +static void append_properties(DBusMessageIter *iter, + struct endpoint_config *cfg) +{ + DBusMessageIter dict; + struct codec_qos *qos = (void *)cfg->qos; + const char *key = "Capabilities"; + + dbus_message_iter_open_container(iter, DBUS_TYPE_ARRAY, "{sv}", &dict); + + bt_shell_printf("Capabilities: "); + bt_shell_hexdump(cfg->caps->iov_base, cfg->caps->iov_len); + + g_dbus_dict_append_basic_array(&dict, DBUS_TYPE_STRING, &key, + DBUS_TYPE_BYTE, &cfg->caps->iov_base, + cfg->caps->iov_len); + + if (!qos) + goto done; + + if (cfg->target_latency) { + bt_shell_printf("TargetLatency 0x%02x\n", qos->interval); + g_dbus_dict_append_entry(&dict, "TargetLatency", + DBUS_TYPE_BYTE, &cfg->target_latency); + } + + if (cfg->ep->cig != BT_ISO_QOS_CIG_UNSET) { + bt_shell_printf("CIG 0x%2.2x\n", cfg->ep->cig); + g_dbus_dict_append_entry(&dict, "CIG", DBUS_TYPE_BYTE, + &cfg->ep->cig); + } + + if (cfg->ep->cis != BT_ISO_QOS_CIS_UNSET) { + bt_shell_printf("CIS 0x%2.2x\n", cfg->ep->cis); + g_dbus_dict_append_entry(&dict, "CIS", DBUS_TYPE_BYTE, + &cfg->ep->cis); + } + + bt_shell_printf("Interval %u\n", qos->interval); + + g_dbus_dict_append_entry(&dict, "Interval", DBUS_TYPE_UINT32, + &qos->interval); + + bt_shell_printf("Framing %s\n", qos->framing ? "true" : "false"); + + g_dbus_dict_append_entry(&dict, "Framing", DBUS_TYPE_BOOLEAN, + &qos->framing); + + bt_shell_printf("PHY %s\n", qos->phy); + + g_dbus_dict_append_entry(&dict, "PHY", DBUS_TYPE_STRING, &qos->phy); + + bt_shell_printf("SDU %u\n", cfg->qos->sdu); + + g_dbus_dict_append_entry(&dict, "SDU", DBUS_TYPE_UINT16, &qos->sdu); + + bt_shell_printf("Retransmissions %u\n", qos->rtn); + + g_dbus_dict_append_entry(&dict, "Retransmissions", DBUS_TYPE_BYTE, + &qos->rtn); + + bt_shell_printf("Latency %u\n", qos->latency); + + g_dbus_dict_append_entry(&dict, "Latency", DBUS_TYPE_UINT16, + &qos->latency); + + bt_shell_printf("Delay %u\n", qos->delay); + + g_dbus_dict_append_entry(&dict, "Delay", DBUS_TYPE_UINT32, + &qos->delay); + +done: + dbus_message_iter_close_container(iter, &dict); +} + +static struct iovec *iov_append(struct iovec **iov, const void *data, + size_t len) +{ + if (!*iov) { + *iov = new0(struct iovec, 1); + (*iov)->iov_base = new0(uint8_t, UINT8_MAX); + } + + if (data && len) { + memcpy((*iov)->iov_base + (*iov)->iov_len, data, len); + (*iov)->iov_len += len; + } + + return *iov; +} + +static DBusMessage *endpoint_select_properties_reply(struct endpoint *ep, + DBusMessage *msg, + struct codec_preset *preset) +{ + DBusMessage *reply; + DBusMessageIter iter; + struct endpoint_config *cfg; + + reply = dbus_message_new_method_return(msg); + if (!reply) + return NULL; + + cfg = new0(struct endpoint_config, 1); + cfg->ep = ep; + + /* Copy capabilities */ + iov_append(&cfg->caps, preset->data.iov_base, preset->data.iov_len); + cfg->target_latency = preset->latency; + + if (preset->qos.phy) + /* Set QoS parameters */ + cfg->qos = &preset->qos; + + dbus_message_iter_init_append(reply, &iter); + + append_properties(&iter, cfg); + + free(cfg); + + return reply; +} + +static void select_properties_response(const char *input, void *user_data) +{ + struct endpoint *ep = user_data; + struct codec_preset *p; + DBusMessage *reply; + + p = find_preset(ep->uuid, input); + if (p) { + reply = endpoint_select_properties_reply(ep, ep->msg, p); + goto done; + } + + bt_shell_printf("Preset %s not found\n", input); + reply = g_dbus_create_error(ep->msg, "org.bluez.Error.Rejected", NULL); + +done: + g_dbus_send_message(dbus_conn, reply); + dbus_message_unref(ep->msg); + ep->msg = NULL; +} + +static DBusMessage *endpoint_select_properties(DBusConnection *conn, + DBusMessage *msg, void *user_data) +{ + struct endpoint *ep = user_data; + struct codec_preset *p; + DBusMessageIter args; + DBusMessage *reply; + + dbus_message_iter_init(msg, &args); + + bt_shell_printf("Endpoint: SelectProperties\n"); + print_iter("\t", "Properties", &args); + + if (!ep->auto_accept) { + ep->msg = dbus_message_ref(msg); + bt_shell_prompt_input("Endpoint", "Enter preset/configuration:", + select_properties_response, ep); + return NULL; + } + + p = find_preset(ep->uuid, NULL); + if (!p) + NULL; + + reply = endpoint_select_properties_reply(ep, msg, p); + if (!reply) + return NULL; + + bt_shell_printf("Auto Accepting using %s...\n", p->name); + + return reply; +} + static DBusMessage *endpoint_clear_configuration(DBusConnection *conn, DBusMessage *msg, void *user_data) { @@ -1478,7 +1882,12 @@ static const GDBusMethodTable endpoint_m NULL, endpoint_set_configuration) }, { GDBUS_ASYNC_METHOD("SelectConfiguration", GDBUS_ARGS({ "caps", "ay" } ), - NULL, endpoint_select_configuration) }, + GDBUS_ARGS({ "cfg", "ay" } ), + endpoint_select_configuration) }, + { GDBUS_ASYNC_METHOD("SelectProperties", + GDBUS_ARGS({ "properties", "a{sv}" } ), + GDBUS_ARGS({ "properties", "a{sv}" } ), + endpoint_select_properties) }, { GDBUS_ASYNC_METHOD("ClearConfiguration", GDBUS_ARGS({ "transport", "o" } ), NULL, endpoint_clear_configuration) }, @@ -1625,18 +2034,64 @@ fail: } +static void endpoint_cis(const char *input, void *user_data) +{ + struct endpoint *ep = user_data; + char *endptr = NULL; + int value; + + if (!strcasecmp(input, "a") || !strcasecmp(input, "auto")) { + ep->cis = BT_ISO_QOS_CIS_UNSET; + } else { + value = strtol(input, &endptr, 0); + + if (!endptr || *endptr != '\0' || value > UINT8_MAX) { + bt_shell_printf("Invalid argument: %s\n", input); + return bt_shell_noninteractive_quit(EXIT_FAILURE); + } + + ep->cis = value; + } + + endpoint_register(ep); +} + +static void endpoint_cig(const char *input, void *user_data) +{ + struct endpoint *ep = user_data; + char *endptr = NULL; + int value; + + if (!strcasecmp(input, "a") || !strcasecmp(input, "auto")) { + ep->cig = BT_ISO_QOS_CIG_UNSET; + } else { + value = strtol(input, &endptr, 0); + + if (!endptr || *endptr != '\0' || value > UINT8_MAX) { + bt_shell_printf("Invalid argument: %s\n", input); + return bt_shell_noninteractive_quit(EXIT_FAILURE); + } + + ep->cig = value; + } + + bt_shell_prompt_input(ep->path, "CIS (auto/value):", endpoint_cis, ep); +} + static void endpoint_auto_accept(const char *input, void *user_data) { struct endpoint *ep = user_data; - if (!strcasecmp(input, "y") || !strcasecmp(input, "yes")) + if (!strcasecmp(input, "y") || !strcasecmp(input, "yes")) { ep->auto_accept = true; - else if (!strcasecmp(input, "n") || !strcasecmp(input, "no")) + } else if (!strcasecmp(input, "n") || !strcasecmp(input, "no")) { ep->auto_accept = false; - else + } else { bt_shell_printf("Invalid input for Auto Accept\n"); + return bt_shell_noninteractive_quit(EXIT_FAILURE); + } - endpoint_register(ep); + bt_shell_prompt_input(ep->path, "CIG (auto/value):", endpoint_cig, ep); } static void endpoint_set_capabilities(const char *input, void *user_data) @@ -1694,22 +2149,6 @@ static const struct capabilities *find_c return NULL; } -static struct iovec *iov_append(struct iovec **iov, const void *data, - size_t len) -{ - if (!*iov) { - *iov = new0(struct iovec, 1); - (*iov)->iov_base = new0(uint8_t, UINT8_MAX); - } - - if (data && len) { - memcpy((*iov)->iov_base + (*iov)->iov_len, data, len); - (*iov)->iov_len += len; - } - - return *iov; -} - static void cmd_register_endpoint(int argc, char *argv[]) { struct endpoint *ep; @@ -1799,31 +2238,14 @@ static void cmd_unregister_endpoint(int return bt_shell_noninteractive_quit(EXIT_SUCCESS); } -struct endpoint_config { - GDBusProxy *proxy; - struct endpoint *ep; - struct iovec *caps; -}; - static void config_endpoint_setup(DBusMessageIter *iter, void *user_data) { struct endpoint_config *cfg = user_data; - DBusMessageIter dict; - const char *key = "Capabilities"; dbus_message_iter_append_basic(iter, DBUS_TYPE_OBJECT_PATH, &cfg->ep->path); - dbus_message_iter_open_container(iter, DBUS_TYPE_ARRAY, "{sv}", &dict); - - bt_shell_printf("Capabilities: "); - bt_shell_hexdump(cfg->caps->iov_base, cfg->caps->iov_len); - - g_dbus_dict_append_basic_array(&dict, DBUS_TYPE_STRING, &key, - DBUS_TYPE_BYTE, &cfg->caps->iov_base, - cfg->caps->iov_len); - - dbus_message_iter_close_container(iter, &dict); + append_properties(iter, cfg); } static void config_endpoint_reply(DBusMessage *message, void *user_data) @@ -1906,6 +2328,9 @@ static void cmd_config_endpoint(int argc iov_append(&cfg->caps, preset->data.iov_base, preset->data.iov_len); + /* Set QoS parameters */ + cfg->qos = &preset->qos; + endpoint_set_config(cfg); return; } @@ -1986,11 +2411,62 @@ static const struct bt_shell_menu endpoi {} }, }; +static struct endpoint *endpoint_new(const struct capabilities *cap) +{ + struct endpoint *ep; + + ep = new0(struct endpoint, 1); + ep->uuid = g_strdup(cap->uuid); + ep->codec = cap->codec_id; + ep->path = g_strdup_printf("%s/ep%u", BLUEZ_MEDIA_ENDPOINT_PATH, + g_list_length(local_endpoints)); + /* Copy capabilities */ + iov_append(&ep->caps, cap->data.iov_base, cap->data.iov_len); + local_endpoints = g_list_append(local_endpoints, ep); + + return ep; +} + +static void register_endpoints(GDBusProxy *proxy) +{ + struct endpoint *ep; + DBusMessageIter iter, array; + + if (!g_dbus_proxy_get_property(proxy, "SupportedUUIDs", &iter)) + return; + + dbus_message_iter_recurse(&iter, &array); + while (dbus_message_iter_get_arg_type(&array) == DBUS_TYPE_STRING) { + const char *uuid; + size_t i; + + dbus_message_iter_get_basic(&array, &uuid); + + for (i = 0; i < ARRAY_SIZE(caps); i++) { + const struct capabilities *cap = &caps[i]; + + if (strcasecmp(cap->uuid, uuid)) + continue; + + ep = endpoint_new(cap); + ep->auto_accept = true; + ep->cig = BT_ISO_QOS_CIG_UNSET; + ep->cis = BT_ISO_QOS_CIS_UNSET; + endpoint_register(ep); + } + + dbus_message_iter_next(&array); + } +} + static void media_added(GDBusProxy *proxy) { medias = g_list_append(medias, proxy); print_media(proxy, COLORED_NEW); + + if (bt_shell_get_env("AUTO_REGISTER_ENDPOINT")) + register_endpoints(proxy); } static void player_added(GDBusProxy *proxy) @@ -2213,6 +2689,30 @@ static struct endpoint *find_ep_by_trans return NULL; } +static struct endpoint *find_link_by_proxy(GDBusProxy *proxy) +{ + DBusMessageIter iter, array; + + if (!g_dbus_proxy_get_property(proxy, "Links", &iter)) + return NULL; + + dbus_message_iter_recurse(&iter, &array); + + while (dbus_message_iter_get_arg_type(&array) == + DBUS_TYPE_OBJECT_PATH) { + const char *transport; + struct endpoint *link; + + dbus_message_iter_get_basic(&array, &transport); + + link = find_ep_by_transport(transport); + if (link) + return link; + } + + return NULL; +} + static void transport_close(struct transport *transport) { if (transport->fd < 0) @@ -2294,10 +2794,19 @@ static void transport_new(GDBusProxy *pr static void acquire_reply(DBusMessage *message, void *user_data) { GDBusProxy *proxy = user_data; + struct endpoint *ep, *link; DBusError error; int sk; uint16_t mtu[2]; + ep = find_ep_by_transport(g_dbus_proxy_get_path(proxy)); + if (ep) { + ep->acquiring = false; + link = find_link_by_proxy(proxy); + if (link) + link->acquiring = false; + } + dbus_error_init(&error); if (dbus_set_error_from_message(&error, message) == TRUE) { @@ -2328,11 +2837,22 @@ static void acquire_reply(DBusMessage *m static void transport_acquire(const char *input, void *user_data) { GDBusProxy *proxy = user_data; + struct endpoint *ep, *link; if (!strcasecmp(input, "y") || !strcasecmp(input, "yes")) { - if (!g_dbus_proxy_method_call(proxy, "Acquire", NULL, + if (g_dbus_proxy_method_call(proxy, "Acquire", NULL, acquire_reply, proxy, NULL)) - bt_shell_printf("Failed acquire transport\n"); + return; + bt_shell_printf("Failed acquire transport\n"); + } + + /* Reset acquiring */ + ep = find_ep_by_transport(g_dbus_proxy_get_path(proxy)); + if (ep) { + ep->acquiring = false; + link = find_link_by_proxy(proxy); + if (link) + link->acquiring = false; } } @@ -2340,7 +2860,7 @@ static void transport_property_changed(G DBusMessageIter *iter) { char *str; - struct endpoint *ep; + struct endpoint *ep, *link; str = proxy_description(proxy, "Transport", COLORED_CHG); print_iter(str, name, iter); @@ -2358,14 +2878,29 @@ static void transport_property_changed(G * endpoint. */ ep = find_ep_by_transport(g_dbus_proxy_get_path(proxy)); - if (!ep) + if (!ep || ep->acquiring) return; + ep->acquiring = true; + + link = find_link_by_proxy(proxy); + if (link) { + bt_shell_printf("Link %s found\n", link->transport); + /* If link already acquiring wait it to be complete */ + if (link->acquiring) + return; + link->acquiring = true; + } + if (ep->auto_accept) { - bt_shell_printf("Auto Accepting...\n"); + bt_shell_printf("Auto Acquiring...\n"); if (!g_dbus_proxy_method_call(proxy, "Acquire", NULL, - acquire_reply, proxy, NULL)) + acquire_reply, proxy, NULL)) { bt_shell_printf("Failed acquire transport\n"); + ep->acquiring = false; + if (link) + link->acquiring = false; + } return; } @@ -2431,6 +2966,15 @@ static void cmd_show_transport(int argc, print_property(proxy, "Volume"); print_property(proxy, "Endpoint"); + print_property(proxy, "Interval"); + print_property(proxy, "Framing"); + print_property(proxy, "SDU"); + print_property(proxy, "Retransmissions"); + print_property(proxy, "Latency"); + print_property(proxy, "Location"); + print_property(proxy, "Metadata"); + print_property(proxy, "Links"); + return bt_shell_noninteractive_quit(EXIT_SUCCESS); } @@ -2450,23 +2994,27 @@ static struct transport *find_transport( static void cmd_acquire_transport(int argc, char *argv[]) { GDBusProxy *proxy; + int i; - proxy = g_dbus_proxy_lookup(transports, NULL, argv[1], + for (i = 1; i < argc; i++) { + proxy = g_dbus_proxy_lookup(transports, NULL, argv[i], BLUEZ_MEDIA_TRANSPORT_INTERFACE); - if (!proxy) { - bt_shell_printf("Transport %s not found\n", argv[1]); - return bt_shell_noninteractive_quit(EXIT_FAILURE); - } + if (!proxy) { + bt_shell_printf("Transport %s not found\n", argv[i]); + return bt_shell_noninteractive_quit(EXIT_FAILURE); + } - if (find_transport(proxy)) { - bt_shell_printf("Transport %s already acquired\n", argv[1]); - return bt_shell_noninteractive_quit(EXIT_FAILURE); - } + if (find_transport(proxy)) { + bt_shell_printf("Transport %s already acquired\n", + argv[i]); + return bt_shell_noninteractive_quit(EXIT_FAILURE); + } - if (!g_dbus_proxy_method_call(proxy, "Acquire", NULL, - acquire_reply, proxy, NULL)) { - bt_shell_printf("Failed acquire transport\n"); - return bt_shell_noninteractive_quit(EXIT_FAILURE); + if (!g_dbus_proxy_method_call(proxy, "Acquire", NULL, + acquire_reply, proxy, NULL)) { + bt_shell_printf("Failed acquire transport\n"); + return bt_shell_noninteractive_quit(EXIT_FAILURE); + } } return bt_shell_noninteractive_quit(EXIT_SUCCESS); @@ -2496,25 +3044,29 @@ static void release_reply(DBusMessage *m static void cmd_release_transport(int argc, char *argv[]) { GDBusProxy *proxy; - struct transport *transport; + int i; - proxy = g_dbus_proxy_lookup(transports, NULL, argv[1], + for (i = 1; i < argc; i++) { + struct transport *transport; + + proxy = g_dbus_proxy_lookup(transports, NULL, argv[i], BLUEZ_MEDIA_TRANSPORT_INTERFACE); - if (!proxy) { - bt_shell_printf("Transport %s not found\n", argv[1]); - return bt_shell_noninteractive_quit(EXIT_FAILURE); - } + if (!proxy) { + bt_shell_printf("Transport %s not found\n", argv[1]); + return bt_shell_noninteractive_quit(EXIT_FAILURE); + } - transport = find_transport(proxy); - if (!transport) { - bt_shell_printf("Transport %s not acquired\n", argv[1]); - return bt_shell_noninteractive_quit(EXIT_FAILURE); - } + transport = find_transport(proxy); + if (!transport) { + bt_shell_printf("Transport %s not acquired\n", argv[i]); + return bt_shell_noninteractive_quit(EXIT_FAILURE); + } - if (!g_dbus_proxy_method_call(proxy, "Release", NULL, + if (!g_dbus_proxy_method_call(proxy, "Release", NULL, release_reply, transport, NULL)) { - bt_shell_printf("Failed release transport\n"); - return bt_shell_noninteractive_quit(EXIT_FAILURE); + bt_shell_printf("Failed release transport\n"); + return bt_shell_noninteractive_quit(EXIT_FAILURE); + } } return bt_shell_noninteractive_quit(EXIT_SUCCESS); @@ -2538,9 +3090,56 @@ static int open_file(const char *filenam return fd; } -static int transport_send(struct transport *transport, int fd) +#define NSEC_USEC(_t) (_t / 1000L) +#define SEC_USEC(_t) (_t * 1000000L) +#define TS_USEC(_ts) (SEC_USEC((_ts)->tv_sec) + NSEC_USEC((_ts)->tv_nsec)) + +static void send_wait(struct timespec *t_start, uint32_t us) +{ + struct timespec t_now; + struct timespec t_diff; + int64_t delta_us; + + /* Skip sleep at start */ + if (!us) + return; + + if (clock_gettime(CLOCK_MONOTONIC, &t_now) < 0) { + bt_shell_printf("clock_gettime: %s (%d)", strerror(errno), + errno); + return; + } + + t_diff.tv_sec = t_now.tv_sec - t_start->tv_sec; + t_diff.tv_nsec = t_now.tv_nsec - t_start->tv_nsec; + + delta_us = us - TS_USEC(&t_diff); + + if (delta_us < 0) { + bt_shell_printf("Send is behind: %" PRId64 " us - skip sleep", + delta_us); + delta_us = 1000; + } + + usleep(delta_us); + + if (clock_gettime(CLOCK_MONOTONIC, t_start) < 0) + bt_shell_printf("clock_gettime: %s (%d)", strerror(errno), + errno); +} + +static int transport_send(struct transport *transport, int fd, + struct bt_iso_qos *qos) { + struct timespec t_start; uint8_t *buf; + uint32_t num = 0; + + if (qos && clock_gettime(CLOCK_MONOTONIC, &t_start) < 0) { + bt_shell_printf("clock_gettime: %s (%d)", strerror(errno), + errno); + return -errno; + } buf = malloc(transport->mtu[1]); if (!buf) { @@ -2548,6 +3147,10 @@ static int transport_send(struct transpo return -ENOMEM; } + /* num of packets = latency (ms) / interval (us) */ + if (qos) + num = (qos->out.latency * 1000 / qos->out.interval); + for (transport->seq = 0; ; transport->seq++) { ssize_t ret; int queued; @@ -2573,6 +3176,11 @@ static int transport_send(struct transpo bt_shell_printf("[seq %d] send: %zd bytes " "(TIOCOUTQ %d bytes)\n", transport->seq, ret, queued); + + if (qos) { + if (transport->seq && !((transport->seq + 1) % num)) + send_wait(&t_start, num * qos->out.interval); + } } free(buf); @@ -2583,6 +3191,8 @@ static void cmd_send_transport(int argc, GDBusProxy *proxy; struct transport *transport; int fd, err; + struct bt_iso_qos qos; + socklen_t len; proxy = g_dbus_proxy_lookup(transports, NULL, argv[1], BLUEZ_MEDIA_TRANSPORT_INTERFACE); @@ -2606,7 +3216,14 @@ static void cmd_send_transport(int argc, bt_shell_printf("Sending ...\n"); - err = transport_send(transport, fd); + /* Read QoS if available */ + memset(&qos, 0, sizeof(qos)); + len = sizeof(qos); + if (getsockopt(transport->sk, SOL_BLUETOOTH, BT_ISO_QOS, &qos, + &len) < 0) + err = transport_send(transport, fd, NULL); + else + err = transport_send(transport, fd, &qos); close(fd); @@ -2707,10 +3324,10 @@ static const struct bt_shell_menu transp { "show", "", cmd_show_transport, "Transport information", transport_generator }, - { "acquire", "", cmd_acquire_transport, + { "acquire", " [transport1...]", cmd_acquire_transport, "Acquire Transport", transport_generator }, - { "release", "", cmd_release_transport, + { "release", " [transport1...]", cmd_release_transport, "Release Transport", transport_generator }, { "send", " ", cmd_send_transport, diff -pruN 5.65-1/completion/zsh/_bluetoothctl 5.66-1/completion/zsh/_bluetoothctl --- 5.65-1/completion/zsh/_bluetoothctl 2022-07-24 20:59:52.000000000 +0000 +++ 5.66-1/completion/zsh/_bluetoothctl 2022-11-10 20:22:09.000000000 +0000 @@ -68,8 +68,7 @@ _bluetoothctl() { '(info)'{-m,--monitor}'[Enable monitor output]' \ + 'command' \ '(info):command:->command' \ - '(info):: :->argument' \ - ': :_message "no more arguments"' + '(info):: :->argument' if [[ $state == "command" ]]; then _describe -t commands 'command' all_commands diff -pruN 5.65-1/configure.ac 5.66-1/configure.ac --- 5.65-1/configure.ac 2022-07-24 20:59:52.000000000 +0000 +++ 5.66-1/configure.ac 2022-11-10 20:22:09.000000000 +0000 @@ -1,6 +1,6 @@ # SPDX-License-Identifier: GPL-2.0 AC_PREREQ(2.60) -AC_INIT(bluez, 5.65) +AC_INIT(bluez, 5.66) AM_INIT_AUTOMAKE([foreign subdir-objects color-tests silent-rules tar-pax no-dist-gzip dist-xz]) @@ -195,6 +195,18 @@ AC_ARG_ENABLE(health, AS_HELP_STRING([-- [enable health profiles]), [enable_health=${enableval}]) AM_CONDITIONAL(HEALTH, test "${enable_health}" = "yes") +AC_ARG_ENABLE(bap, AS_HELP_STRING([--disable-bap], + [disable BAP profile]), [enable_bap=${enableval}]) +AM_CONDITIONAL(BAP, test "${enable_bap}" != "no") + +AC_ARG_ENABLE(mcp, AS_HELP_STRING([--disable-mcp], + [disable MCP profile]), [enable_mcp=${enableval}]) +AM_CONDITIONAL(MCP, test "${enable_mcp}" != "no") + +AC_ARG_ENABLE(vcp, AS_HELP_STRING([--disable-vcp], + [disable VCP profile]), [enable_vcp=${enableval}]) +AM_CONDITIONAL(VCP, test "${enable_vcp}" != "no") + AC_ARG_ENABLE(tools, AS_HELP_STRING([--disable-tools], [disable Bluetooth tools]), [enable_tools=${enableval}]) AM_CONDITIONAL(TOOLS, test "${enable_tools}" != "no") diff -pruN 5.65-1/debian/changelog 5.66-1/debian/changelog --- 5.65-1/debian/changelog 2022-08-03 01:57:23.000000000 +0000 +++ 5.66-1/debian/changelog 2022-11-16 18:44:23.000000000 +0000 @@ -1,3 +1,10 @@ +bluez (5.66-1) unstable; urgency=medium + + * Update to 5.66. + * Refresh d/patches/Fix-typo.patch. + + -- Nobuhiro Iwamatsu Thu, 17 Nov 2022 03:44:23 +0900 + bluez (5.65-1) unstable; urgency=medium * Update to 5.65. diff -pruN 5.65-1/debian/patches/Fix-typo.patch 5.66-1/debian/patches/Fix-typo.patch --- 5.65-1/debian/patches/Fix-typo.patch 2022-08-03 01:57:23.000000000 +0000 +++ 5.66-1/debian/patches/Fix-typo.patch 2022-11-16 18:44:23.000000000 +0000 @@ -1,6 +1,6 @@ -From 7b9990be13f483bb2daa03d1a350f95bf9aa819f Mon Sep 17 00:00:00 2001 +From 940f5fede5165f199072bf2fa462ab1302d8e233 Mon Sep 17 00:00:00 2001 From: Nobuhiro Iwamatsu -Date: Tue, 14 Dec 2021 15:18:02 +0900 +Date: Fri, 18 Nov 2022 08:24:30 +0900 Subject: [PATCH] Fix typo This commit fixes following typo: @@ -34,12 +34,15 @@ This commit fixes following typo: wich -> which Signed-off-by: Nobuhiro Iwamatsu + +Signed-off-by: Nobuhiro Iwamatsu --- android/tester-main.c | 6 +++--- attrib/att.c | 2 +- emulator/btdev.c | 2 +- lib/bluetooth.c | 2 +- mesh/net.c | 2 +- + monitor/att.c | 2 +- monitor/avctp.c | 6 +++--- monitor/ll.c | 2 +- obexd/plugins/bluetooth.c | 2 +- @@ -50,10 +53,9 @@ Signed-off-by: Nobuhiro Iwamatsu msg_cache, match_cache, &tst); +@@ -1031,7 +1031,7 @@ static bool msg_in_cache(struct mesh_net *net, uint16_t src, uint32_t seq, + msg = l_queue_find(net->msg_cache, match_cache, &tst); if (msg) { - l_debug("Supressing duplicate %4.4x + %6.6x + %8.8x", + l_debug("Suppressing duplicate %4.4x + %6.6x + %8.8x", src, seq, mic); - l_queue_push_head(net->msg_cache, msg); return true; + } +diff --git a/monitor/att.c b/monitor/att.c +index efd840d51..3a8aeb063 100644 +--- a/monitor/att.c ++++ b/monitor/att.c +@@ -1827,7 +1827,7 @@ static void print_vcs_flag(const struct l2cap_frame *frame) + print_text(COLOR_ERROR, "Volume Flag: invalid size"); + goto done; + } +- print_field(" Volume Falg: %u", vol_flag); ++ print_field(" Volume Flag: %u", vol_flag); + + done: + if (frame->size) diff --git a/monitor/avctp.c b/monitor/avctp.c -index dc03195a8..73ef50c17 100644 +index fb2628282..7a8c124ab 100644 --- a/monitor/avctp.c +++ b/monitor/avctp.c @@ -1314,13 +1314,13 @@ static bool avrcp_get_play_status(struct avctp_frame *avctp_frame, @@ -204,10 +219,10 @@ index d232d3fd5..0db6db303 100644 for (l = profiles; l; l = l->next) { struct bluetooth_profile *profile = l->data; diff --git a/obexd/plugins/pcsuite.c b/obexd/plugins/pcsuite.c -index b2232ea09..fd69cae00 100644 +index f5a9d9ae8..f3707d104 100644 --- a/obexd/plugins/pcsuite.c +++ b/obexd/plugins/pcsuite.c -@@ -444,7 +444,7 @@ static ssize_t backup_write(void *object, const void *buf, size_t count) +@@ -446,7 +446,7 @@ static ssize_t backup_write(void *object, const void *buf, size_t count) if (obj->fd != -1) { ret = write(obj->fd, buf, count); @@ -230,10 +245,10 @@ index 01741fe62..dae2d3ecb 100644 struct agent { char *bus_name; diff --git a/plugins/policy.c b/plugins/policy.c -index 051db82e1..f790b7a77 100644 +index 0bbdbfc88..5daddaac2 100644 --- a/plugins/policy.c +++ b/plugins/policy.c -@@ -277,7 +277,7 @@ static void sink_cb(struct btd_service *service, btd_service_state_t old_state, +@@ -278,7 +278,7 @@ static void sink_cb(struct btd_service *service, btd_service_state_t old_state, } /* Check if service initiate the connection then proceed @@ -242,7 +257,7 @@ index 051db82e1..f790b7a77 100644 */ if (btd_service_is_initiator(service)) policy_connect(data, controller); -@@ -428,7 +428,7 @@ static void source_cb(struct btd_service *service, +@@ -429,7 +429,7 @@ static void source_cb(struct btd_service *service, } /* Check if service initiate the connection then proceed @@ -252,7 +267,7 @@ index 051db82e1..f790b7a77 100644 if (btd_service_is_initiator(service)) policy_connect(data, target); diff --git a/profiles/battery/battery.c b/profiles/battery/battery.c -index 176d127f6..2efe9beb8 100644 +index 02d024d92..d236cdcb7 100644 --- a/profiles/battery/battery.c +++ b/profiles/battery/battery.c @@ -151,7 +151,7 @@ static void read_initial_battery_level_cb(bool success, @@ -265,7 +280,7 @@ index 176d127f6..2efe9beb8 100644 return; } diff --git a/profiles/gap/gas.c b/profiles/gap/gas.c -index ea3249be9..780f749aa 100644 +index 400818d67..9bb1a2247 100644 --- a/profiles/gap/gas.c +++ b/profiles/gap/gas.c @@ -91,7 +91,7 @@ static void read_device_name_cb(bool success, uint8_t att_ecode, @@ -278,10 +293,10 @@ index ea3249be9..780f749aa 100644 return; } diff --git a/profiles/health/mcap.c b/profiles/health/mcap.c -index 5161ef77c..1097ca70f 100644 +index 5d2bac3d9..1601b99d6 100644 --- a/profiles/health/mcap.c +++ b/profiles/health/mcap.c -@@ -2854,7 +2854,7 @@ static void proc_sync_set_req(struct mcap_mcl *mcl, uint8_t *cmd, uint32_t len) +@@ -2867,7 +2867,7 @@ static void proc_sync_set_req(struct mcap_mcl *mcl, uint8_t *cmd, uint32_t len) return; } @@ -291,10 +306,10 @@ index 5161ef77c..1097ca70f 100644 if (phase2_delay > 61*1000) { diff --git a/profiles/midi/midi.c b/profiles/midi/midi.c -index 737d1b5f6..43203efbe 100644 +index 40064df3a..4c660da47 100644 --- a/profiles/midi/midi.c +++ b/profiles/midi/midi.c -@@ -309,7 +309,7 @@ static int midi_accept(struct btd_service *service) +@@ -318,7 +318,7 @@ static int midi_accept(struct btd_service *service) err = snd_seq_client_id(midi->seq_handle); if (err < 0) { @@ -304,10 +319,10 @@ index 737d1b5f6..43203efbe 100644 } midi->seq_client_id = err; diff --git a/src/adapter.c b/src/adapter.c -index 5846f0396..4601e02c8 100644 +index 8fb2acdc8..711487a14 100644 --- a/src/adapter.c +++ b/src/adapter.c -@@ -4012,7 +4012,7 @@ static void set_privacy_complete(uint8_t status, uint16_t length, +@@ -4196,7 +4196,7 @@ static void set_privacy_complete(uint8_t status, uint16_t length, return; } @@ -316,7 +331,7 @@ index 5846f0396..4601e02c8 100644 } static int set_privacy(struct btd_adapter *adapter, uint8_t privacy) -@@ -5346,7 +5346,7 @@ void adapter_auto_connect_add(struct btd_adapter *adapter, +@@ -5538,7 +5538,7 @@ void adapter_auto_connect_add(struct btd_adapter *adapter, bdaddr_type = btd_device_get_bdaddr_type(device); if (bdaddr_type == BDADDR_BREDR) { @@ -325,7 +340,7 @@ index 5846f0396..4601e02c8 100644 return; } -@@ -5494,7 +5494,7 @@ void adapter_auto_connect_remove(struct btd_adapter *adapter, +@@ -5651,7 +5651,7 @@ void adapter_auto_connect_remove(struct btd_adapter *adapter, bdaddr_type = btd_device_get_bdaddr_type(device); if (bdaddr_type == BDADDR_BREDR) { @@ -334,24 +349,6 @@ index 5846f0396..4601e02c8 100644 return; } -@@ -9760,7 +9760,7 @@ static void read_info_complete(uint8_t status, uint16_t length, - case BT_MODE_BREDR: - if (!(adapter->supported_settings & MGMT_SETTING_BREDR)) { - btd_error(adapter->dev_id, -- "Ignoring adapter withouth BR/EDR support"); -+ "Ignoring adapter without BR/EDR support"); - goto failed; - } - -@@ -9774,7 +9774,7 @@ static void read_info_complete(uint8_t status, uint16_t length, - case BT_MODE_LE: - if (!(adapter->supported_settings & MGMT_SETTING_LE)) { - btd_error(adapter->dev_id, -- "Ignoring adapter withouth LE support"); -+ "Ignoring adapter without LE support"); - goto failed; - } - diff --git a/src/profile.c b/src/profile.c index e1bebf1ee..5b651ce7b 100644 --- a/src/profile.c @@ -375,10 +372,10 @@ index e1bebf1ee..5b651ce7b 100644 } diff --git a/src/shared/util.c b/src/shared/util.c -index 81b20d86f..205d3f35d 100644 +index 0a0308cb0..9eee200d4 100644 --- a/src/shared/util.c +++ b/src/shared/util.c -@@ -205,7 +205,7 @@ static const struct { +@@ -246,7 +246,7 @@ static const struct { { 0x111d, "Imaging Referenced Objects" }, { 0x111e, "Handsfree" }, { 0x111f, "Handsfree Audio Gateway" }, @@ -387,24 +384,11 @@ index 81b20d86f..205d3f35d 100644 { 0x1121, "Reflected UI" }, { 0x1122, "Basic Printing" }, { 0x1123, "Printing Status" }, -diff --git a/tools/btattach.rst b/tools/btattach.rst -index 787d5c49e..d4f3f7f0c 100644 ---- a/tools/btattach.rst -+++ b/tools/btattach.rst -@@ -60,7 +60,7 @@ OPTIONS - - * - qca - ---S baudrate, --speed baudrate Specify wich baudrate to use -+-S baudrate, --speed baudrate Specify which baudrate to use - - -N, --noflowctl Disable flow control - diff --git a/tools/btmgmt.c b/tools/btmgmt.c -index 42ef9acef..3f84659bb 100644 +index 29f86091f..866d65afe 100644 --- a/tools/btmgmt.c +++ b/tools/btmgmt.c -@@ -5527,7 +5527,7 @@ static const struct bt_shell_menu main_menu = { +@@ -5895,7 +5895,7 @@ static const struct bt_shell_menu main_menu = { { "ssp", "", cmd_ssp, "Toggle SSP mode" }, { "sc", "", @@ -413,7 +397,7 @@ index 42ef9acef..3f84659bb 100644 { "hs", "", cmd_hs, "Toggle HS support" }, { "le", "", -@@ -5589,7 +5589,7 @@ static const struct bt_shell_menu main_menu = { +@@ -5957,7 +5957,7 @@ static const struct bt_shell_menu main_menu = { { "ext-config", "", cmd_ext_config, "External configuration" }, { "debug-keys", "", @@ -458,10 +442,10 @@ index 0d6272da9..4ba33f87e 100644 } diff --git a/tools/l2cap-tester.c b/tools/l2cap-tester.c -index d78b1e29c..3200ed074 100644 +index 3f0464013..c3a30628a 100644 --- a/tools/l2cap-tester.c +++ b/tools/l2cap-tester.c -@@ -1908,7 +1908,7 @@ static void test_getpeername_not_connected(const void *test_data) +@@ -1909,7 +1909,7 @@ static void test_getpeername_not_connected(const void *test_data) } if (errno != ENOTCONN) { @@ -471,10 +455,10 @@ index d78b1e29c..3200ed074 100644 tester_test_failed(); goto done; diff --git a/tools/l2test.c b/tools/l2test.c -index fbaca747e..fb208c0c3 100644 +index 5aae4b687..4baaba5e0 100644 --- a/tools/l2test.c +++ b/tools/l2test.c -@@ -901,7 +901,7 @@ static void recv_mode(int sk) +@@ -902,7 +902,7 @@ static void recv_mode(int sk) /* Check sequence */ sq = get_le32(buf); if (seq != sq) { @@ -483,7 +467,7 @@ index fbaca747e..fb208c0c3 100644 seq = sq; } seq++; -@@ -909,14 +909,14 @@ static void recv_mode(int sk) +@@ -910,14 +910,14 @@ static void recv_mode(int sk) /* Check length */ l = get_le16(buf + 4); if (len != l) { @@ -501,10 +485,10 @@ index fbaca747e..fb208c0c3 100644 total += len; diff --git a/tools/mgmt-tester.c b/tools/mgmt-tester.c -index 8bddf6b03..38f670324 100644 +index a56c38173..29df74007 100644 --- a/tools/mgmt-tester.c +++ b/tools/mgmt-tester.c -@@ -12343,10 +12343,10 @@ int main(int argc, char *argv[]) +@@ -14015,10 +14015,10 @@ int main(int argc, char *argv[]) test_bredrle50("Set PHY 2m Success", &set_phy_2m_success, NULL, test_command_generic); @@ -580,7 +564,7 @@ index e73a6317e..1480e4eef 100644 raw_dump(level, frm); } diff --git a/tools/parser/hci.c b/tools/parser/hci.c -index db7d32c01..a0b62f915 100644 +index db7d32c01..2dac62ce9 100644 --- a/tools/parser/hci.c +++ b/tools/parser/hci.c @@ -1175,7 +1175,7 @@ static inline void qos_setup_dump(int level, struct frame *frm) @@ -588,7 +572,7 @@ index db7d32c01..a0b62f915 100644 printf("Token rate: %d\n", btohl(cp->qos.token_rate)); p_indent(level, frm); - printf("Peak bandwith: %d\n", btohl(cp->qos.peak_bandwidth)); -+ printf("Peak bandwidth/: %d\n", btohl(cp->qos.peak_bandwidth)); ++ printf("Peak bandwidth: %d\n", btohl(cp->qos.peak_bandwidth)); p_indent(level, frm); printf("Latency: %d\n", btohl(cp->qos.latency)); p_indent(level, frm); @@ -597,7 +581,7 @@ index db7d32c01..a0b62f915 100644 printf("Token rate: %d\n", btohl(evt->qos.token_rate)); p_indent(level, frm); - printf("Peak bandwith: %d\n", btohl(evt->qos.peak_bandwidth)); -+ printf("Peak bandwidth/: %d\n", btohl(evt->qos.peak_bandwidth)); ++ printf("Peak bandwidth: %d\n", btohl(evt->qos.peak_bandwidth)); p_indent(level, frm); printf("Latency: %d\n", btohl(evt->qos.latency)); p_indent(level, frm); @@ -606,7 +590,7 @@ index db7d32c01..a0b62f915 100644 printf("Token rate: %d\n", btohl(evt->qos.token_rate)); p_indent(level, frm); - printf("Peak bandwith: %d\n", btohl(evt->qos.peak_bandwidth)); -+ printf("Peak bandwidth/: %d\n", btohl(evt->qos.peak_bandwidth)); ++ printf("Peak bandwidth: %d\n", btohl(evt->qos.peak_bandwidth)); p_indent(level, frm); printf("Latency: %d\n", btohl(evt->qos.latency)); p_indent(level, frm); @@ -655,10 +639,10 @@ index 733795ac6..10d51de4d 100644 } diff --git a/tools/rctest.c b/tools/rctest.c -index 9eb8210d6..56e9c4f05 100644 +index d31180880..1eb781218 100644 --- a/tools/rctest.c +++ b/tools/rctest.c -@@ -509,7 +509,7 @@ static void recv_mode(int sk) +@@ -510,7 +510,7 @@ static void recv_mode(int sk) /* Check sequence */ sq = btohl(*(uint32_t *) buf); if (seq != sq) { @@ -667,7 +651,7 @@ index 9eb8210d6..56e9c4f05 100644 seq = sq; } seq++; -@@ -517,14 +517,14 @@ static void recv_mode(int sk) +@@ -518,14 +518,14 @@ static void recv_mode(int sk) /* Check length */ l = btohs(*(uint16_t *) (buf + 4)); if (r != l) { @@ -685,5 +669,5 @@ index 9eb8210d6..56e9c4f05 100644 #endif total += r; -- -2.33.1 +2.36.1 diff -pruN 5.65-1/doc/adapter-api.txt 5.66-1/doc/adapter-api.txt --- 5.65-1/doc/adapter-api.txt 2022-07-24 20:59:52.000000000 +0000 +++ 5.66-1/doc/adapter-api.txt 2022-11-10 20:22:09.000000000 +0000 @@ -269,6 +269,21 @@ Properties string Address [readonly] restart or unplugging of the adapter it will reset back to false. + string PowerState [readonly, experimental] + + The power state of an adapter. + + The power state will show whether the adapter is + turning off, or turning on, as well as being on + or off. + + Possible values: + "on" - powered on + "off" - powered off + "off-enabling" - transitioning from "off" to "on" + "on-disabling" - transitioning from "on" to "off" + "off-blocked" - blocked by rfkill + boolean Discoverable [readwrite] Switch an adapter to discoverable or non-discoverable diff -pruN 5.65-1/doc/ci.config 5.66-1/doc/ci.config --- 5.65-1/doc/ci.config 1970-01-01 00:00:00.000000000 +0000 +++ 5.66-1/doc/ci.config 2022-11-10 20:22:09.000000000 +0000 @@ -0,0 +1,122 @@ +############################################################# +# # +# This config file is for testing bluetooth build only. # +# # +############################################################# + +CONFIG_VIRTIO=y +CONFIG_VIRTIO_PCI=y + +CONFIG_NET=y +CONFIG_INET=y + +CONFIG_NET_9P=y +CONFIG_NET_9P_VIRTIO=y + +CONFIG_9P_FS=y +CONFIG_9P_FS_POSIX_ACL=y + +CONFIG_GPIOLIB=y + +CONFIG_SERIAL_8250=y +CONFIG_SERIAL_8250_CONSOLE=y +CONFIG_SERIAL_8250_PCI=y +CONFIG_SERIAL_8250_NR_UARTS=4 + +CONFIG_SERIAL_DEV_BUS=y + +CONFIG_TMPFS=y +CONFIG_TMPFS_POSIX_ACL=y +CONFIG_TMPFS_XATTR=y + +CONFIG_DEVTMPFS=y +CONFIG_DEBUG_FS=y + +CONFIG_MMC=y + +CONFIG_RPMSG=y +CONFIG_QCOM_WCNSS_CTRL=y + +CONFIG_PCMCIA=y + +CONFIG_ISDN_CAPI=y + +CONFIG_6LOWPAN=y + +CONFIG_LEDS_CLASS=y + +CONFIG_USB=y + +CONFIG_BT=y +CONFIG_BT_BREDR=y +CONFIG_BT_RFCOMM=y +CONFIG_BT_RFCOMM_TTY=y +CONFIG_BT_BNEP=y +CONFIG_BT_BNEP_MC_FILTER=y +CONFIG_BT_BNEP_PROTO_FILTER=y +CONFIG_BT_HIDP=y +CONFIG_BT_LE=y +CONFIG_BT_MSFTEXT=y +CONFIG_BT_HS=y +CONFIG_BT_CMTP=y +CONFIG_BT_6LOWPAN=y +CONFIG_BT_LEDS=y +CONFIG_BT_FEATURE_DEBUG=y + +CONFIG_BT_HCIVHCI=y + +CONFIG_BT_HCIBTUSB=y +CONFIG_BT_HCIBTUSB_AUTOSUSPEND=y +CONFIG_BT_HCIBTUSB_MTK=y +CONFIG_BT_HCIBCM203X=y +CONFIG_BT_HCIBPA10X=y +CONFIG_BT_MRVL=y +CONFIG_BT_ATH3K=y + +CONFIG_BT_HCIUART=y +CONFIG_BT_HCIUART_SERDEV=y +CONFIG_BT_HCIUART_H4=y +CONFIG_BT_HCIUART_BCSP=y +CONFIG_BT_HCIUART_ATH3K=y +CONFIG_BT_HCIUART_AG6XX=y +CONFIG_BT_HCIUART_NOKIA=y +CONFIG_BT_HCIUART_LL=y +CONFIG_BT_HCIUART_3WIRE=y +CONFIG_BT_HCIUART_INTEL=y +CONFIG_BT_HCIUART_BCM=y +CONFIG_BT_HCIUART_RTL=y +CONFIG_BT_HCIUART_QCA=y +CONFIG_BT_HCIUART_MRVL=y +CONFIG_BT_MTKUART=y + +CONFIG_BT_HCIBFUSB=y + +CONFIG_BT_HCIBTSDIO=y +CONFIG_BT_MRVL_SDIO=y +CONFIG_BT_MTKSDIO=y + +CONFIG_BT_HCIDTL1=y +CONFIG_BT_HCIBT3C=y +CONFIG_BT_HCIBLUECARD=y + +CONFIG_BT_QCOMSMD=y + +CONFIG_BT_VIRTIO=y + +CONFIG_CRYPTO_CMAC=y +CONFIG_CRYPTO_USER_API=y +CONFIG_CRYPTO_USER_API_HASH=y +CONFIG_CRYPTO_USER_API_SKCIPHER=y + +CONFIG_UNIX=y + +CONFIG_UHID=y + +CONFIG_LOCKDEP_SUPPORT=y +CONFIG_DEBUG_SPINLOCK=y +CONFIG_DEBUG_LOCK_ALLOC=y +CONFIG_PROVE_LOCKING=y +CONFIG_LOCKDEP=y +CONFIG_DEBUG_MUTEXES=y + +CONFIG_OF=y diff -pruN 5.65-1/doc/gatt-api.txt 5.66-1/doc/gatt-api.txt --- 5.65-1/doc/gatt-api.txt 2022-07-24 20:59:52.000000000 +0000 +++ 5.66-1/doc/gatt-api.txt 2022-11-10 20:22:09.000000000 +0000 @@ -79,13 +79,15 @@ Methods array{byte} ReadValue(dict opti "mtu": Exchanged MTU (Server only) "device": Object Device (Server only) - Possible Errors: org.bluez.Error.Failed + Possible Errors: org.bluez.Error.Failed(string ecode) org.bluez.Error.InProgress org.bluez.Error.NotPermitted org.bluez.Error.NotAuthorized org.bluez.Error.InvalidOffset org.bluez.Error.NotSupported + Possible Error Code: string 0x80 - 0x9f + void WriteValue(array{byte} value, dict options) Issues a request to write the value of the @@ -105,13 +107,15 @@ Methods array{byte} ReadValue(dict opti authorization request - Possible Errors: org.bluez.Error.Failed + Possible Errors: org.bluez.Error.Failed(string ecode) org.bluez.Error.InProgress org.bluez.Error.NotPermitted org.bluez.Error.InvalidValueLength org.bluez.Error.NotAuthorized org.bluez.Error.NotSupported + Possible Error Code: string 0x80 - 0x9f + fd, uint16 AcquireWrite(dict options) [optional] Acquire file descriptor and MTU for writing. Only diff -pruN 5.65-1/doc/media-api.txt 5.66-1/doc/media-api.txt --- 5.65-1/doc/media-api.txt 2022-07-24 20:59:52.000000000 +0000 +++ 5.66-1/doc/media-api.txt 2022-11-10 20:22:09.000000000 +0000 @@ -24,6 +24,9 @@ Methods void RegisterEndpoint(object en UUID of the profile which the endpoint is for. + UUID must be in the list of + SupportedUUIDS. + byte Codec: Assigned number of codec that the @@ -87,6 +90,12 @@ Methods void RegisterEndpoint(object en Possible errors: org.bluez.Error.InvalidArguments org.bluez.Error.DoesNotExist + +Properties array{string} SupportedUUIDs [readonly]: + + List of 128-bit UUIDs that represents the supported + Endpoint registration. + Media Control hierarchy ======================= @@ -564,7 +573,18 @@ Methods void SetConfiguration(object tr endpoint oject which will be configured and the properties must contain the following properties: - array{byte} Capabilities + array{byte} Capabilities [Mandatory] + array{byte} Metadata [ISO only] + byte CIG [ISO only] + byte CIS [ISO only] + uint32 Interval [ISO only] + bool Framing [ISO only] + string PHY [ISO only] + uint16 SDU [ISO only] + byte Retransmissions [ISO only] + uint16 Latency [ISO only] + uint32 Delay [ISO only] + uint8 TargetLatency [ISO Latency] array{byte} SelectConfiguration(array{byte} capabilities) @@ -578,6 +598,21 @@ Methods void SetConfiguration(object tr configuration since on success the configuration is send back as parameter of SetConfiguration. + dict SelectProperties(dict properties) + + Select preferable properties from the supported + properties: + object Endpoint [ISO only] + Refer to SetConfiguration for the list of + other possible properties. + + Returns propeties which can be used to setup + a transport. + + Note: There is no need to cache the selected + properties since on success the configuration is + send back as parameter of SetConfiguration. + void ClearConfiguration(object transport) Clear transport configuration. @@ -613,6 +648,46 @@ Properties string UUID [readonly, option Indicates if endpoint supports Delay Reporting. + byte Framing [ISO only] + + Indicates endpoint support framing. + + byte PHY [ISO only] + + Indicates endpoint supported PHY. + + uint16_t MaximumLatency [ISO only] + + Indicates endpoint maximum latency. + + uint32_t MinimumDelay [ISO only] + + Indicates endpoint minimum presentation delay. + + uint32_t MaximumDelay [ISO only] + + Indicates endpoint maximum presentation delay. + + uint32_t PreferredMinimumDelay [ISO only] + + Indicates endpoint preferred minimum presentation delay. + + uint32_t PreferredMinimumDelay [ISO only] + + Indicates endpoint preferred minimum presentation delay. + + uint32 Location [ISO only] + + Indicates endpoint supported locations. + + uint16 SupportedContext [ISO only] + + Indicates endpoint supported audio context. + + uint16 Context [ISO only] + + Indicates endpoint available audio context. + MediaTransport1 hierarchy ========================= @@ -689,3 +764,16 @@ Properties object Device [readonly] Endpoint object which the transport is associated with. + + uint32 Location [readonly, ISO only, experimental] + + Indicates transport Audio Location. + + array{byte} Metadata [ISO Only, experimental] + + Indicates transport Metadata. + + array{object} Links [readonly, optional, ISO only, experimental] + + Linked transport objects which the transport is + associated with. diff -pruN 5.65-1/doc/mgmt-api.txt 5.66-1/doc/mgmt-api.txt --- 5.65-1/doc/mgmt-api.txt 2022-07-24 20:59:52.000000000 +0000 +++ 5.66-1/doc/mgmt-api.txt 2022-11-10 20:22:09.000000000 +0000 @@ -3861,45 +3861,132 @@ Add Advertisement Patterns Monitor With Invalid Parameters -Set Quality Report Command -========================== +Set Mesh Receiver Command +========================= Command Code: 0x0057 Controller Index: - Command Parameters: Quality_Report (1 Octet) - Return Parameters: Current_Settings (4 Octets) + Command Parameters: Enable (1 Octets) + Window (2 Octets) + Period (2 Octets) + Num AD Types (1 Octets) + AD Types { } + + This command Enables or Disables Mesh Receiving. When enabled passive + scanning remains enabled for this controller. + + The Window/Period values are used to set the Scan Parameters when no + other scanning is being done. + + Num AD Types and AD Types parameter, filter Advertising and Scan + responses by AD type. Reponses that do not contain at least one of the + requested AD types will be ignored. Otherwise they will be delivered + with the Mesh Device Found event. + + Possible errors: Failed + No Resources + Invalid Parameters + - This command is used to enable and disable the controller's quality - report feature. The allowed values for the Quality_Report command - parameter are 0x00 and 0x01. All other values will return Invalid - Parameters. - - The value 0x00 disables the Quality Report, and the value 0x01 - enables the Quality Report feature. - - This command is only available for the controllers that support - either AOSP Bluetooth quality report or Intel telemetry event. - It is supported if the supported_settings indicate support for it. - - This command requires to use a valid controller index. Otherwise, - an Invalid Index status will be returned. - - The command is sent to the controller to enable/disable the quality - report feature, and generates a Command Complete event on success. - If the controller failed to execute the action, a Failed status will - be returned. - - The quality report state is maintained by the kernel over the adapter - power cycle. When the adapter is powered off, the quality report - feature is disabled by the kernel. When the adapter is powered on, it - is enabled again by the kernel if it was enabled before. +Read Mesh Features Command +========================== + + Command Code: 0x0058 + Controller Index: + Command Parameters: + Return Parameters: Index (2 Octets) + Max Handles (1 Octets) + Used Handles (1 Octets) + Handle { } + + This command is used to both verify that Outbound Mesh packet + support is enabled, and to indicate the number of packets that + can and are simultaneously queued. + + Index identifies the HCI Controller that this information is valid for. + + Max Handles indicates the maximum number of packets that may be queued. + + Used Handles indicates the number of packets awaiting transmission. + + Handle is an array of the currently outstanding packets. Possible errors: Failed - Invalid Index + No Resources Invalid Parameters - Not Supported +Transmit Mesh Packet Command +============================ + + Command Code: 0x0059 + Controller Index: + Command Parameters: Addr (6 octets) + Addr Type (1 Octets) + Instant (8 Octets) + Delay (2 Octets) + Count (1 Octets) + Data Length (1 Octets) + Data (variable) + + Return Parameters: Handle (1 Octets) + + This command sends a Mesh Packet as a NONCONN LE Advertisement. + + The Addr + Addr Type parameters specifify the address to use in the + outbound advertising packet. If BD_ADDR_ANY and LE_RANDOM is set, the + kernel will create a single use non-resolvable address. + + The Instant parameter is used in combination with the Delay + parameter, to finely time the sending of the Advertising packet. It + should be set to the Instant value tag of a received incoming + Mesh Device Found Event. It is only useful in POLL-RESPONSE situations + where a response must be sent within a negotiated time window. The value + of the Instant parameter should not be interpreted by the host, and + only has meaning to the controller. + + The Delay parameter, if 0x0000, will cause the packet to be sent + at the earliest opportunity. If non-Zero, and the controller supports + delayed delivery, the Instant and Delay parameters will be used + to delay the outbound packet. While the Instant is not defined, the + Delay is specified in milliseconds. + + The Count parameter must be sent to a non-Zero value indicating the + number of times this packet will be sent before transmission completes. + If the Delay parameter is non-Zero, then Count must be 1 only. + + The Data parameter is an octet array of the AD Type and Mesh Packet. + + This command will return immediately, and if it succeeds, will generate + a Mesh Packet Transmission Complete event when after the packet has been + sent. + + Possible errors: Failed + Busy + No Resources + Invalid Parameters + + +Cancel Transmit Mesh Packet Command +=================================== + + Command Code: 0x005A + Controller Index: + Command Parameters: Handle (1 Octets) + + This command may be used to cancel an outbound transmission request. + + The Handle parameter is the returned handle from a successful Transmit + Mesh Packet request. If Zero is specified as the handle, all outstanding + send requests are canceled. + + For each mesh packet canceled, the Mesh Packet Transmission Complete + event will be generated, regardless of whether the packet was sent + successfully. + + Possible errors: Failed + Invalid Parameters + Command Complete Event ====================== @@ -5022,20 +5109,55 @@ Advertisement Monitor Device Lost Event This event will be sent to all management sockets. -Quality Report Event -==================== +Mesh Device Found Event +======================= Event code: 0x0031 Controller Index: - Event Parameters: Quality_Spec (1 Octet) - Report_Len (2 Octets) - Report (0-65535 Octets) - - This event carries the Bluetooth quality report sent by the - controller. - - Possible values for the Quality_Spec parameter: - 0 AOSP Bluetooth Quality Report Event - 1 Intel Telemetry Event + Event Parameters: Address (6 Octets) + Address_Type (1 Octet) + RSSI (1 Octet) + Instant (8 Octets) + Flags (4 Octets) + AD_Data_Length (2 Octets) + AD_Data (0-65535 Octets) + + This event indicates that the controller has received an Advertisement + or Scan Result containing an AD Type matching the Mesh scan set. + + The address of the sending device is returned, and must be a valid LE + Address_Type. + + Possible values for the Address_Type parameter: + 0 Reserved (not in use) + 1 LE Public + 2 LE Random + + The RSSI field is a signed octet, and is the RSSI reported by the + receiving controller. + + The Instant field is 64 bit value that represents the instant in time + the packet was received. It's value is not intended to be interpretted + by the host, and is only useful if the host wants to make a timed + response to the received packet. (i.e. a Poll/Response) + + AD_Length and AD_Data contains the Info structure of Advertising and + Scan rsults. To receive this event, AD filters must be requested with + the Set Mesh Receiver command command, specifying which AD Types to + return. All AD structures will be received in this event if any of the + filtered AD Types are present. + + This event will be sent to all management sockets. + + +Mesh Packet Transmit Complete Event +=================================== + + Event code: 0x0032 + Controller Index: + Event Parameters: Handle (1 Octets) + + This event indicates that a requested outbound Mesh packet has + completed and no longer occupies a transmit slot. This event will be sent to all management sockets. diff -pruN 5.65-1/emulator/btdev.c 5.66-1/emulator/btdev.c --- 5.65-1/emulator/btdev.c 2022-07-24 20:59:52.000000000 +0000 +++ 5.66-1/emulator/btdev.c 2022-11-10 20:22:09.000000000 +0000 @@ -6960,6 +6960,16 @@ const uint8_t *btdev_get_bdaddr(struct b return btdev->bdaddr; } +bool btdev_set_bdaddr(struct btdev *btdev, const uint8_t *bdaddr) +{ + if (!btdev || !bdaddr) + return false; + + memcpy(btdev->bdaddr, bdaddr, sizeof(btdev->bdaddr)); + + return true; +} + uint8_t *btdev_get_features(struct btdev *btdev) { return btdev->features; diff -pruN 5.65-1/emulator/btdev.h 5.66-1/emulator/btdev.h --- 5.65-1/emulator/btdev.h 2022-07-24 20:59:52.000000000 +0000 +++ 5.66-1/emulator/btdev.h 2022-11-10 20:22:09.000000000 +0000 @@ -72,6 +72,8 @@ bool btdev_set_debug(struct btdev *btdev void *user_data, btdev_destroy_func_t destroy); const uint8_t *btdev_get_bdaddr(struct btdev *btdev); +bool btdev_set_bdaddr(struct btdev *btdev, const uint8_t *bdaddr); + uint8_t *btdev_get_features(struct btdev *btdev); uint8_t btdev_get_scan_enable(struct btdev *btdev); diff -pruN 5.65-1/emulator/bthost.c 5.66-1/emulator/bthost.c --- 5.65-1/emulator/bthost.c 2022-07-24 20:59:52.000000000 +0000 +++ 5.66-1/emulator/bthost.c 2022-11-10 20:22:09.000000000 +0000 @@ -137,6 +137,7 @@ struct rfcomm_chan_hook { struct iso_hook { bthost_cid_hook_func_t func; void *user_data; + bthost_destroy_func_t destroy; }; struct btconn { @@ -306,6 +307,9 @@ static void btconn_free(struct btconn *c free(hook); } + if (conn->iso_hook && conn->iso_hook->destroy) + conn->iso_hook->destroy(conn->iso_hook->user_data); + free(conn->iso_hook); free(conn->recv_data); free(conn); @@ -676,7 +680,8 @@ void bthost_add_cid_hook(struct bthost * } void bthost_add_iso_hook(struct bthost *bthost, uint16_t handle, - bthost_cid_hook_func_t func, void *user_data) + bthost_iso_hook_func_t func, void *user_data, + bthost_destroy_func_t destroy) { struct iso_hook *hook; struct btconn *conn; @@ -693,6 +698,7 @@ void bthost_add_iso_hook(struct bthost * hook->func = func; hook->user_data = user_data; + hook->destroy = destroy; conn->iso_hook = hook; } @@ -3131,25 +3137,29 @@ bool bthost_search_ext_adv_addr(struct b } void bthost_set_cig_params(struct bthost *bthost, uint8_t cig_id, - uint8_t cis_id) + uint8_t cis_id, const struct bt_iso_qos *qos) { struct bt_hci_cmd_le_set_cig_params *cp; cp = malloc(sizeof(*cp) + sizeof(*cp->cis)); memset(cp, 0, sizeof(*cp) + sizeof(*cp->cis)); cp->cig_id = cig_id; - put_le24(10000, cp->c_interval); - put_le24(10000, cp->p_interval); - cp->c_latency = cpu_to_le16(10); - cp->p_latency = cpu_to_le16(10); + put_le24(qos->in.interval ? qos->in.interval : qos->out.interval, + cp->c_interval); + put_le24(qos->out.interval ? qos->out.interval : qos->in.interval, + cp->p_interval); + cp->c_latency = cpu_to_le16(qos->in.latency ? qos->in.latency : + qos->out.latency); + cp->p_latency = cpu_to_le16(qos->out.latency ? qos->out.latency : + qos->in.latency); cp->num_cis = 0x01; cp->cis[0].cis_id = cis_id; - cp->cis[0].c_sdu = 40; - cp->cis[0].p_sdu = 40; - cp->cis[0].c_phy = 0x02; - cp->cis[0].p_phy = 0x02; - cp->cis[0].c_rtn = 2; - cp->cis[0].p_rtn = 2; + cp->cis[0].c_sdu = qos->in.sdu; + cp->cis[0].p_sdu = qos->out.sdu; + cp->cis[0].c_phy = qos->in.phy ? qos->in.phy : qos->out.phy; + cp->cis[0].p_phy = qos->out.phy ? qos->out.phy : qos->in.phy; + cp->cis[0].c_rtn = qos->in.rtn; + cp->cis[0].p_rtn = qos->out.rtn; send_command(bthost, BT_HCI_CMD_LE_SET_CIG_PARAMS, cp, sizeof(*cp) + sizeof(*cp->cis)); diff -pruN 5.65-1/emulator/bthost.h 5.66-1/emulator/bthost.h --- 5.65-1/emulator/bthost.h 2022-07-24 20:59:52.000000000 +0000 +++ 5.66-1/emulator/bthost.h 2022-11-10 20:22:09.000000000 +0000 @@ -12,6 +12,8 @@ #include #include +#include "lib/bluetooth.h" + typedef void (*bthost_send_func) (const struct iovec *iov, int iovlen, void *user_data); @@ -69,7 +71,8 @@ typedef void (*bthost_iso_hook_func_t)(c void *user_data); void bthost_add_iso_hook(struct bthost *bthost, uint16_t handle, - bthost_iso_hook_func_t func, void *user_data); + bthost_iso_hook_func_t func, void *user_data, + bthost_destroy_func_t destroy); void bthost_send_cid(struct bthost *bthost, uint16_t handle, uint16_t cid, const void *data, uint16_t len); @@ -101,7 +104,7 @@ void bthost_create_big(struct bthost *bt bool bthost_search_ext_adv_addr(struct bthost *bthost, const uint8_t *addr); void bthost_set_cig_params(struct bthost *bthost, uint8_t cig_id, - uint8_t cis_id); + uint8_t cis_id, const struct bt_iso_qos *qos); void bthost_create_cis(struct bthost *bthost, uint16_t cis_handle, uint16_t acl_handle); diff -pruN 5.65-1/emulator/vhci.c 5.66-1/emulator/vhci.c --- 5.65-1/emulator/vhci.c 2022-07-24 20:59:52.000000000 +0000 +++ 5.66-1/emulator/vhci.c 2022-11-10 20:22:09.000000000 +0000 @@ -257,3 +257,13 @@ int vhci_set_emu_opcode(struct vhci *vhc { return btdev_set_emu_opcode(vhci->btdev, opcode); } + +int vhci_set_force_static_address(struct vhci *vhci, bool enable) +{ + char val; + + val = (enable) ? 'Y' : 'N'; + + return vhci_debugfs_write(vhci, "force_static_address", &val, + sizeof(val)); +} diff -pruN 5.65-1/emulator/vhci.h 5.66-1/emulator/vhci.h --- 5.65-1/emulator/vhci.h 2022-07-24 20:59:52.000000000 +0000 +++ 5.66-1/emulator/vhci.h 2022-11-10 20:22:09.000000000 +0000 @@ -28,3 +28,4 @@ int vhci_set_force_wakeup(struct vhci *v int vhci_set_msft_opcode(struct vhci *vhci, uint16_t opcode); int vhci_set_aosp_capable(struct vhci *vhci, bool enable); int vhci_set_emu_opcode(struct vhci *vhci, uint16_t opcode); +int vhci_set_force_static_address(struct vhci *vhci, bool enable); diff -pruN 5.65-1/lib/mgmt.h 5.66-1/lib/mgmt.h --- 5.65-1/lib/mgmt.h 2022-07-24 20:59:52.000000000 +0000 +++ 5.66-1/lib/mgmt.h 2022-11-10 20:22:09.000000000 +0000 @@ -760,6 +760,38 @@ struct mgmt_cp_add_adv_patterns_monitor_ struct mgmt_adv_pattern patterns[0]; } __packed; +#define MGMT_OP_SET_MESH_RECEIVER 0x0057 +struct mgmt_cp_set_mesh { + uint8_t enable; + uint16_t window; + uint16_t period; + uint8_t num_ad_types; + uint8_t ad_types[]; +} __packed; + +#define MGMT_OP_MESH_READ_FEATURES 0x0058 +struct mgmt_rp_mesh_read_features { + uint16_t index; + uint8_t max_handles; + uint8_t used_handles; + uint8_t handles[]; +} __packed; + +#define MGMT_OP_MESH_SEND 0x0059 +struct mgmt_cp_mesh_send { + struct mgmt_addr_info addr; + uint64_t instant; + uint16_t delay; + uint8_t cnt; + uint8_t adv_data_len; + uint8_t adv_data[]; +} __packed; + +#define MGMT_OP_MESH_SEND_CANCEL 0x005A +struct mgmt_cp_mesh_send_cancel { + uint8_t handle; +} __packed; + #define MGMT_EV_CMD_COMPLETE 0x0001 struct mgmt_ev_cmd_complete { uint16_t opcode; @@ -1035,6 +1067,21 @@ struct mgmt_ev_adv_monitor_device_lost { struct mgmt_addr_info addr; } __packed; +#define MGMT_EV_MESH_DEVICE_FOUND 0x0031 +struct mgmt_ev_mesh_device_found { + struct mgmt_addr_info addr; + int8_t rssi; + uint64_t instant; + uint32_t flags; + uint16_t eir_len; + uint8_t eir[]; +} __packed; + +#define MGMT_EV_MESH_PACKET_CMPLT 0x0032 +struct mgmt_ev_mesh_pkt_cmplt { + uint8_t handle; +} __packed; + static const char *mgmt_op[] = { "<0x0000>", "Read Version", @@ -1123,6 +1170,10 @@ static const char *mgmt_op[] = { "Add Extended Advertisement Parameters", /* 0x0054 */ "Add Extended Advertisement Data", "Add Advertisement Patterns Monitor RSSI", + "Set Mesh Receiver", + "Read Mesh Features", + "Mesh Send", + "Mesh Send Cancel", }; static const char *mgmt_ev[] = { @@ -1175,6 +1226,8 @@ static const char *mgmt_ev[] = { "Controller Resume", "Advertisement Monitor Device Found", /* 0x002f */ "Advertisement Monitor Device Lost", + "Mesh Packet Found", + "Mesh Packet Complete", }; static const char *mgmt_status[] = { diff -pruN 5.65-1/lib/uuid.h 5.66-1/lib/uuid.h --- 5.65-1/lib/uuid.h 2022-07-24 20:59:52.000000000 +0000 +++ 5.66-1/lib/uuid.h 2022-11-10 20:22:09.000000000 +0000 @@ -146,6 +146,46 @@ extern "C" { /* GATT Server Supported features */ #define GATT_CHARAC_SERVER_FEAT 0x2B3A +/* TODO: Update these on final UUID is given */ +#define PACS_UUID 0x1850 +#define PAC_SINK_CHRC_UUID 0x2bc9 +#define PAC_SINK_UUID "00002bc9-0000-1000-8000-00805f9b34fb" +#define PAC_SINK_LOC_CHRC_UUID 0x2bca + +#define PAC_SOURCE_CHRC_UUID 0x2bcb +#define PAC_SOURCE_UUID "00002bcb-0000-1000-8000-00805f9b34fb" +#define PAC_SOURCE_LOC_CHRC_UUID 0x2bcc + +#define PAC_CONTEXT 0x2bcd +#define PAC_SUPPORTED_CONTEXT 0x2bce + +#define ASCS_UUID 0x184e +#define ASE_SINK_UUID 0x2bc4 +#define ASE_SOURCE_UUID 0x2bc5 +#define ASE_CP_UUID 0x2bc6 + +#define VCS_UUID 0x1844 +#define VOL_OFFSET_CS_UUID 0x1845 +#define AUDIO_INPUT_CS_UUID 0x1843 +#define VOL_STATE_CHRC_UUID 0x2B7D +#define VOL_CP_CHRC_UUID 0x2B7E +#define VOL_FLAG_CHRC_UUID 0x2B7F + +#define GMCS_UUID 0x1849 +#define MEDIA_PLAYER_NAME_CHRC_UUID 0x2b93 +#define MEDIA_TRACK_CHNGD_CHRC_UUID 0x2b96 +#define MEDIA_TRACK_TITLE_CHRC_UUID 0x2b97 +#define MEDIA_TRACK_DURATION_CHRC_UUID 0x2b98 +#define MEDIA_TRACK_POSTION_CHRC_UUID 0x2b99 +#define MEDIA_PLAYBACK_SPEED_CHRC_UUID 0x2b9a +#define MEDIA_SEEKING_SPEED_CHRC_UUID 0x2b9b +#define MEDIA_PLAYING_ORDER_CHRC_UUID 0x2ba1 +#define MEDIA_PLAY_ORDER_SUPPRTD_CHRC_UUID 0x2ba2 +#define MEDIA_STATE_CHRC_UUID 0x2ba3 +#define MEDIA_CP_CHRC_UUID 0x2ba4 +#define MEDIA_CP_OP_SUPPORTED_CHRC_UUID 0x2ba5 +#define MEDIA_CONTENT_CONTROL_ID_CHRC_UUID 0x2bba + typedef struct { enum { BT_UUID_UNSPEC = 0, diff -pruN 5.65-1/Makefile.am 5.66-1/Makefile.am --- 5.65-1/Makefile.am 2022-07-24 20:59:52.000000000 +0000 +++ 5.66-1/Makefile.am 2022-11-10 20:22:09.000000000 +0000 @@ -82,7 +82,7 @@ pkginclude_HEADERS += $(lib_headers) lib_LTLIBRARIES += lib/libbluetooth.la lib_libbluetooth_la_SOURCES = $(lib_headers) $(lib_sources) -lib_libbluetooth_la_LDFLAGS = $(AM_LDFLAGS) -version-info 22:7:19 +lib_libbluetooth_la_LDFLAGS = $(AM_LDFLAGS) -version-info 22:8:19 lib_libbluetooth_la_DEPENDENCIES = $(local_headers) endif @@ -230,7 +230,10 @@ shared_sources = src/shared/io.h src/sha src/shared/gatt-db.h src/shared/gatt-db.c \ src/shared/gap.h src/shared/gap.c \ src/shared/log.h src/shared/log.c \ - src/shared/tty.h + src/shared/bap.h src/shared/bap.c src/shared/ascs.h \ + src/shared/mcs.h src/shared/mcp.h src/shared/mcp.c \ + src/shared/vcp.c src/shared/vcp.h \ + src/shared/lc3.h src/shared/tty.h if READLINE shared_sources += src/shared/shell.c src/shared/shell.h @@ -407,6 +410,11 @@ EXTRA_DIST += tools/magic.btsnoop AM_CPPFLAGS += $(DBUS_CFLAGS) $(GLIB_CFLAGS) -I$(builddir)/lib +unit_tests += unit/test-tester + +unit_test_tester_SOURCES = unit/test-tester.c +unit_test_tester_LDADD = src/libshared-glib.la lib/libbluetooth-internal.la \ + $(GLIB_LIBS) unit_tests += unit/test-eir diff -pruN 5.65-1/Makefile.mesh 5.66-1/Makefile.mesh --- 5.65-1/Makefile.mesh 2022-07-24 20:59:52.000000000 +0000 +++ 5.66-1/Makefile.mesh 2022-11-10 20:22:09.000000000 +0000 @@ -13,12 +13,11 @@ endif mesh_sources = mesh/mesh.h mesh/mesh.c \ mesh/net-keys.h mesh/net-keys.c \ mesh/mesh-io.h mesh/mesh-io.c \ - mesh/mesh-mgmt.c mesh/mesh-mgmt.h \ + mesh/mesh-mgmt.h mesh/mesh-mgmt.c \ mesh/error.h mesh/mesh-io-api.h \ - mesh/mesh-io-generic.h \ - mesh/mesh-io-generic.c \ - mesh/mesh-io-unit.h \ - mesh/mesh-io-unit.c \ + mesh/mesh-io-unit.h mesh/mesh-io-unit.c \ + mesh/mesh-io-mgmt.h mesh/mesh-io-mgmt.c \ + mesh/mesh-io-generic.h mesh/mesh-io-generic.c \ mesh/net.h mesh/net.c \ mesh/crypto.h mesh/crypto.c \ mesh/friend.h mesh/friend.c \ diff -pruN 5.65-1/Makefile.plugins 5.66-1/Makefile.plugins --- 5.65-1/Makefile.plugins 2022-07-24 20:59:52.000000000 +0000 +++ 5.66-1/Makefile.plugins 2022-11-10 20:22:09.000000000 +0000 @@ -116,3 +116,18 @@ plugins_sixaxis_la_LDFLAGS = $(AM_LDFLAG plugins_sixaxis_la_LIBADD = $(UDEV_LIBS) plugins_sixaxis_la_CFLAGS = $(AM_CFLAGS) -fvisibility=hidden endif + +if BAP +builtin_modules += bap +builtin_sources += profiles/audio/bap.c +endif + +if MCP +builtin_modules += mcp +builtin_sources += profiles/audio/mcp.c +endif + +if VCP +builtin_modules += vcp +builtin_sources += profiles/audio/vcp.c +endif diff -pruN 5.65-1/Makefile.tools 5.66-1/Makefile.tools --- 5.65-1/Makefile.tools 2022-07-24 20:59:52.000000000 +0000 +++ 5.66-1/Makefile.tools 2022-11-10 20:22:09.000000000 +0000 @@ -86,7 +86,8 @@ noinst_PROGRAMS += emulator/btvirt emula tools/l2cap-tester tools/sco-tester \ tools/smp-tester tools/hci-tester \ tools/rfcomm-tester tools/bnep-tester \ - tools/userchan-tester tools/iso-tester + tools/userchan-tester tools/iso-tester \ + tools/mesh-tester tools/ioctl-tester emulator_btvirt_SOURCES = emulator/main.c monitor/bt.h \ emulator/serial.h emulator/serial.c \ @@ -127,6 +128,15 @@ tools_mgmt_tester_SOURCES = tools/mgmt-t tools_mgmt_tester_LDADD = lib/libbluetooth-internal.la \ src/libshared-glib.la $(GLIB_LIBS) +tools_mesh_tester_SOURCES = tools/mesh-tester.c monitor/bt.h \ + emulator/hciemu.h emulator/hciemu.c \ + emulator/vhci.h emulator/vhci.c \ + emulator/btdev.h emulator/btdev.c \ + emulator/bthost.h emulator/bthost.c \ + emulator/smp.c +tools_mesh_tester_LDADD = lib/libbluetooth-internal.la \ + src/libshared-glib.la $(GLIB_LIBS) + tools_l2cap_tester_SOURCES = tools/l2cap-tester.c monitor/bt.h \ emulator/hciemu.h emulator/hciemu.c \ emulator/vhci.h emulator/vhci.c \ @@ -203,6 +213,15 @@ tools_iso_tester_SOURCES = tools/iso-tes emulator/smp.c tools_iso_tester_LDADD = lib/libbluetooth-internal.la \ src/libshared-glib.la $(GLIB_LIBS) + +tools_ioctl_tester_SOURCES = tools/ioctl-tester.c monitor/bt.h \ + emulator/hciemu.h emulator/hciemu.c \ + emulator/vhci.h emulator/vhci.c \ + emulator/btdev.h emulator/btdev.c \ + emulator/bthost.h emulator/bthost.c \ + emulator/smp.c +tools_ioctl_tester_LDADD = lib/libbluetooth-internal.la \ + src/libshared-glib.la $(GLIB_LIBS) endif if TOOLS @@ -329,6 +348,8 @@ man_MANS += tools/rctest.1 tools/l2ping. endif if MESH + +if DEPRECATED bin_PROGRAMS += tools/meshctl tools_meshctl_SOURCES = tools/meshctl.c \ @@ -353,6 +374,9 @@ tools_meshctl_LDADD = gdbus/libgdbus-int lib/libbluetooth-internal.la \ $(GLIB_LIBS) $(DBUS_LIBS) -ljson-c -lreadline +EXTRA_DIST += tools/mesh-gatt/local_node.json tools/mesh-gatt/prov_db.json +endif + bin_PROGRAMS += tools/mesh-cfgclient tools_mesh_cfgclient_SOURCES = tools/mesh-cfgclient.c \ @@ -376,8 +400,6 @@ tools_mesh_cfgtest_LDADD = lib/libblueto $(ell_ldadd) endif -EXTRA_DIST += tools/mesh-gatt/local_node.json tools/mesh-gatt/prov_db.json - if DEPRECATED bin_PROGRAMS += tools/hciattach tools/hciconfig tools/hcitool tools/hcidump \ tools/rfcomm tools/sdptool tools/ciptool diff -pruN 5.65-1/mesh/appkey.c 5.66-1/mesh/appkey.c --- 5.65-1/mesh/appkey.c 2022-07-24 20:59:52.000000000 +0000 +++ 5.66-1/mesh/appkey.c 2022-11-10 20:22:09.000000000 +0000 @@ -296,7 +296,9 @@ int appkey_key_add(struct mesh_net *net, key = l_queue_find(app_keys, match_key_index, L_UINT_TO_PTR(app_idx)); if (key) { - if (memcmp(new_key, key->key, 16) == 0) + if (key->net_idx != net_idx) + return MESH_STATUS_INVALID_NETKEY; + else if (memcmp(new_key, key->key, 16) == 0) return MESH_STATUS_SUCCESS; else return MESH_STATUS_IDX_ALREADY_STORED; diff -pruN 5.65-1/mesh/cfgmod-server.c 5.66-1/mesh/cfgmod-server.c --- 5.65-1/mesh/cfgmod-server.c 2022-07-24 20:59:52.000000000 +0000 +++ 5.66-1/mesh/cfgmod-server.c 2022-11-10 20:22:09.000000000 +0000 @@ -110,8 +110,6 @@ static uint16_t config_pub_set(struct me pkt += (virt ? 14 : 0); idx = l_get_le16(pkt + 4); - if (idx > CREDFLAG_MASK) - return 0; cred_flag = !!(CREDFLAG_MASK & idx); idx &= APP_IDX_MASK; @@ -288,6 +286,10 @@ static uint16_t cfg_virt_sub_add_msg(str label, true, addr, opcode)) msg[n] = MESH_STATUS_STORAGE_FAIL; + /* If processing failed, set addr field to zero in reply */ + if (msg[n] != MESH_STATUS_SUCCESS) + addr = UNASSIGNED_ADDRESS; + l_put_le16(ele_addr, msg + n + 1); l_put_le16(addr, msg + n + 3); @@ -434,6 +436,10 @@ static uint16_t cfg_key_refresh_phase(st return 0; } + if (pkt[2] == KEY_REFRESH_TRANS_THREE && + phase == KEY_REFRESH_PHASE_NONE) + goto done; + status = mesh_net_key_refresh_phase_set(net, idx, pkt[2]); l_debug("Set KR Phase: net=%3.3x transition=%d", idx, pkt[2]); @@ -453,14 +459,14 @@ done: static uint8_t uint32_to_log(uint32_t value) { uint32_t val = 1; - uint8_t ret = 1; + uint8_t ret = 0; if (!value) return 0; else if (value > 0x10000) return 0xff; - while (val < value) { + while (val <= value) { val <<= 1; ret++; } @@ -468,7 +474,7 @@ static uint8_t uint32_to_log(uint32_t va return ret; } -static uint16_t hb_subscription_get(struct mesh_node *node, int status) +static uint16_t hb_subscription_status(struct mesh_node *node, int status) { struct mesh_net *net = node_get_net(node); struct mesh_net_heartbeat_sub *sub = mesh_net_get_heartbeat_sub(net); @@ -493,13 +499,35 @@ static uint16_t hb_subscription_get(stru l_put_le16(sub->dst, msg + n); n += 2; msg[n++] = uint32_to_log(time_now.tv_sec); - msg[n++] = uint32_to_log(sub->count); - msg[n++] = sub->count ? sub->min_hops : 0; + msg[n++] = sub->count != 0xffff ? uint32_to_log(sub->count) : 0xff; + msg[n++] = sub->min_hops; msg[n++] = sub->max_hops; return n; } +static uint16_t hb_subscription_get(struct mesh_node *node, int status) +{ + struct mesh_net *net = node_get_net(node); + struct mesh_net_heartbeat_sub *sub = mesh_net_get_heartbeat_sub(net); + + /* + * MshPRFv1.0.1 section 4.4.1.2.16, Heartbeat Subscription state: + * If this is a GET request and the source or destination is unassigned, + * all fields shall be set to zero in the status reply. + */ + if (IS_UNASSIGNED(sub->src) || IS_UNASSIGNED(sub->dst)) { + uint16_t n; + + n = mesh_model_opcode_set(OP_CONFIG_HEARTBEAT_SUB_STATUS, msg); + memset(msg + n, 0, 9); + n += 9; + return n; + } + + return hb_subscription_status(node, status); +} + static uint16_t hb_subscription_set(struct mesh_node *node, const uint8_t *pkt) { uint16_t src, dst; @@ -523,7 +551,7 @@ static uint16_t hb_subscription_set(stru status = mesh_net_set_heartbeat_sub(net, src, dst, period_log); - return hb_subscription_get(node, status); + return hb_subscription_status(node, status); } static uint16_t hb_publication_get(struct mesh_node *node, int status) @@ -536,7 +564,7 @@ static uint16_t hb_publication_get(struc msg[n++] = status; l_put_le16(pub->dst, msg + n); n += 2; - msg[n++] = uint32_to_log(pub->count); + msg[n++] = pub->count != 0xffff ? uint32_to_log(pub->count) : 0xff; msg[n++] = uint32_to_log(pub->period); msg[n++] = pub->ttl; l_put_le16(pub->features, msg + n); @@ -573,7 +601,17 @@ static uint16_t hb_publication_set(struc status = mesh_net_set_heartbeat_pub(net, dst, features, net_idx, ttl, count_log, period_log); - return hb_publication_get(node, status); + if (status != MESH_STATUS_SUCCESS) { + uint16_t n; + + n = mesh_model_opcode_set(OP_CONFIG_HEARTBEAT_PUB_STATUS, msg); + msg[n++] = status; + memcpy(msg + n, pkt, 9); + n += 9; + + return n; + } else + return hb_publication_get(node, status); } static void node_reset(void *user_data) diff -pruN 5.65-1/mesh/friend.c 5.66-1/mesh/friend.c --- 5.65-1/mesh/friend.c 2022-07-24 20:59:52.000000000 +0000 +++ 5.66-1/mesh/friend.c 2022-11-10 20:22:09.000000000 +0000 @@ -25,7 +25,7 @@ #define MAX_FRND_GROUPS 20 #define FRND_RELAY_WINDOW 250 /* 250 ms */ -#define FRND_CACHE_SIZE 16 +#define FRND_CACHE_SIZE FRND_CACHE_MAX #define FRND_SUB_LIST_SIZE 8 #define RESPONSE_DELAY (100 - 12) /* 100 ms - 12ms hw delay */ diff -pruN 5.65-1/mesh/main.c 5.66-1/mesh/main.c --- 5.65-1/mesh/main.c 2022-07-24 20:59:52.000000000 +0000 +++ 5.66-1/mesh/main.c 2022-11-10 20:22:09.000000000 +0000 @@ -123,6 +123,12 @@ static void disconnect_callback(void *us l_main_quit(); } +static void kill_to(struct l_timeout *timeout, void *user_data) +{ + l_timeout_remove(timeout); + l_main_quit(); +} + static void signal_handler(uint32_t signo, void *user_data) { static bool terminated; @@ -131,13 +137,44 @@ static void signal_handler(uint32_t sign return; l_info("Terminating"); - l_main_quit(); + + mesh_cleanup(true); + + if (io_type != MESH_IO_TYPE_UNIT_TEST) + l_timeout_create(1, kill_to, NULL, NULL); + else + l_main_quit(); + terminated = true; } static bool parse_io(const char *optarg, enum mesh_io_type *type, void **opts) { - if (strstr(optarg, "generic") == optarg) { + if (strstr(optarg, "auto") == optarg) { + int *index = l_new(int, 1); + + *type = MESH_IO_TYPE_AUTO; + *opts = index; + + optarg += strlen("auto"); + if (!*optarg) { + *index = MGMT_INDEX_NONE; + return true; + } + + if (*optarg != ':') + return false; + + optarg++; + + if (sscanf(optarg, "hci%d", index) == 1) + return true; + + if (sscanf(optarg, "%d", index) == 1) + return true; + + return false; + } else if (strstr(optarg, "generic") == optarg) { int *index = l_new(int, 1); *type = MESH_IO_TYPE_GENERIC; @@ -251,7 +288,7 @@ int main(int argc, char *argv[]) } if (!io) - io = l_strdup_printf("generic"); + io = l_strdup_printf("auto"); if (!parse_io(io, &io_type, &io_opts)) { l_error("Invalid io: %s", io); @@ -295,7 +332,7 @@ done: l_free(io); l_free(io_opts); - mesh_cleanup(); + mesh_cleanup(false); l_dbus_destroy(dbus); l_main_exit(); diff -pruN 5.65-1/mesh/mesh.c 5.66-1/mesh/mesh.c --- 5.65-1/mesh/mesh.c 2022-07-24 20:59:52.000000000 +0000 +++ 5.66-1/mesh/mesh.c 2022-11-10 20:22:09.000000000 +0000 @@ -324,11 +324,15 @@ static void free_pending_join_call(bool join_pending = NULL; } -void mesh_cleanup(void) +void mesh_cleanup(bool signaled) { struct l_dbus_message *reply; mesh_io_destroy(mesh.io); + mesh.io = NULL; + + if (signaled) + return; if (join_pending) { diff -pruN 5.65-1/mesh/mesh-config-json.c 5.66-1/mesh/mesh-config-json.c --- 5.65-1/mesh/mesh-config-json.c 2022-07-24 20:59:52.000000000 +0000 +++ 5.66-1/mesh/mesh-config-json.c 2022-11-10 20:22:09.000000000 +0000 @@ -1961,8 +1961,10 @@ bool mesh_config_comp_page_add(struct me len = (size * 2) + 3; buf = l_malloc(len); ret = snprintf(buf, len, "%2.2x", page); - if (ret < 0) + if (ret < 0) { + l_free(buf); return false; + } hex2str(data, size, buf + 2, len - 2); diff -pruN 5.65-1/mesh/mesh.h 5.66-1/mesh/mesh.h --- 5.65-1/mesh/mesh.h 2022-07-24 20:59:52.000000000 +0000 +++ 5.66-1/mesh/mesh.h 2022-11-10 20:22:09.000000000 +0000 @@ -28,7 +28,7 @@ typedef void (*prov_rx_cb_t)(void *user_ bool mesh_init(const char *config_dir, const char *mesh_conf_fname, enum mesh_io_type type, void *opts, mesh_ready_func_t cb, void *user_data); -void mesh_cleanup(void); +void mesh_cleanup(bool signaled); bool mesh_dbus_init(struct l_dbus *dbus); const char *mesh_status_str(uint8_t err); diff -pruN 5.65-1/mesh/mesh-io-api.h 5.66-1/mesh/mesh-io-api.h --- 5.65-1/mesh/mesh-io-api.h 2022-07-24 20:59:52.000000000 +0000 +++ 5.66-1/mesh/mesh-io-api.h 2022-11-10 20:22:09.000000000 +0000 @@ -10,8 +10,7 @@ struct mesh_io_private; -typedef bool (*mesh_io_init_t)(struct mesh_io *io, void *opts, - mesh_io_ready_func_t cb, void *user_data); +typedef bool (*mesh_io_init_t)(struct mesh_io *io, void *opts, void *user_data); typedef bool (*mesh_io_destroy_t)(struct mesh_io *io); typedef bool (*mesh_io_caps_t)(struct mesh_io *io, struct mesh_io_caps *caps); typedef bool (*mesh_io_send_t)(struct mesh_io *io, @@ -36,9 +35,13 @@ struct mesh_io_api { }; struct mesh_io { - enum mesh_io_type type; - const struct mesh_io_api *api; + int index; + int favored_index; + mesh_io_ready_func_t ready; + struct l_queue *rx_regs; struct mesh_io_private *pvt; + void *user_data; + const struct mesh_io_api *api; }; struct mesh_io_table { diff -pruN 5.65-1/mesh/mesh-io.c 5.66-1/mesh/mesh-io.c --- 5.65-1/mesh/mesh-io.c 2022-07-24 20:59:52.000000000 +0000 +++ 5.66-1/mesh/mesh-io.c 2022-11-10 20:22:09.000000000 +0000 @@ -15,95 +15,161 @@ #include #include "lib/bluetooth.h" +#include "lib/mgmt.h" +#include "src/shared/mgmt.h" #include "mesh/mesh-defs.h" +#include "mesh/mesh-mgmt.h" #include "mesh/mesh-io.h" #include "mesh/mesh-io-api.h" /* List of Mesh-IO Type headers */ +#include "mesh/mesh-io-mgmt.h" #include "mesh/mesh-io-generic.h" #include "mesh/mesh-io-unit.h" +struct mesh_io_reg { + mesh_io_recv_func_t cb; + void *user_data; + uint8_t len; + uint8_t filter[]; +} packed; + /* List of Supported Mesh-IO Types */ static const struct mesh_io_table table[] = { - {MESH_IO_TYPE_GENERIC, &mesh_io_generic}, + {MESH_IO_TYPE_MGMT, &mesh_io_mgmt}, + {MESH_IO_TYPE_GENERIC, &mesh_io_generic}, {MESH_IO_TYPE_UNIT_TEST, &mesh_io_unit}, }; -static struct l_queue *io_list; +static struct mesh_io *default_io; -static bool match_by_io(const void *a, const void *b) +static const struct mesh_io_api *io_api(enum mesh_io_type type) { - return a == b; + uint16_t i; + + for (i = 0; i < L_ARRAY_SIZE(table); i++) { + if (table[i].type == type) + return table[i].api; + } + + return NULL; } -static bool match_by_type(const void *a, const void *b) +static void refresh_rx(void *a, void *b) { - const struct mesh_io *io = a; - const enum mesh_io_type type = L_PTR_TO_UINT(b); + struct mesh_io_reg *rx_reg = a; + struct mesh_io *io = b; - return io->type == type; + if (io->api && io->api->reg) + io->api->reg(io, rx_reg->filter, rx_reg->len, rx_reg->cb, + rx_reg->user_data); } -struct mesh_io *mesh_io_new(enum mesh_io_type type, void *opts, - mesh_io_ready_func_t cb, void *user_data) +static void ctl_alert(int index, bool up, bool pwr, bool mesh, void *user_data) { + enum mesh_io_type type = L_PTR_TO_UINT(user_data); const struct mesh_io_api *api = NULL; - struct mesh_io *io; - uint16_t i; - for (i = 0; i < L_ARRAY_SIZE(table); i++) { - if (table[i].type == type) { - api = table[i].api; - break; + l_warn("up:%d pwr: %d mesh: %d", up, pwr, mesh); + + /* If specific IO controller requested, honor it */ + if (default_io->favored_index != MGMT_INDEX_NONE && + default_io->favored_index != index) + return; + + if (!up && default_io->index == index) { + /* Our controller has disappeared */ + if (default_io->api && default_io->api->destroy) { + default_io->api->destroy(default_io); + default_io->api = NULL; } + + /* Re-enumerate controllers */ + mesh_mgmt_list(ctl_alert, user_data); + return; + } + + /* If we already have an API, keep using it */ + if (!up || default_io->api) + return; + + if (mesh && type != MESH_IO_TYPE_GENERIC) + api = io_api(MESH_IO_TYPE_MGMT); + + else if (!pwr) + api = io_api(MESH_IO_TYPE_GENERIC); + + if (api) { + default_io->index = index; + default_io->api = api; + api->init(default_io, &index, default_io->user_data); + + l_queue_foreach(default_io->rx_regs, refresh_rx, default_io); } +} - io = l_queue_find(io_list, match_by_type, L_UINT_TO_PTR(type)); +static void free_io(struct mesh_io *io) +{ + if (io) { + if (io->api && io->api->destroy) + io->api->destroy(io); + + l_queue_destroy(io->rx_regs, l_free); + io->rx_regs = NULL; + l_free(io); + l_warn("Destroy %p", io); + } +} + +struct mesh_io *mesh_io_new(enum mesh_io_type type, void *opts, + mesh_io_ready_func_t cb, void *user_data) +{ + const struct mesh_io_api *api = NULL; - if (!api || !api->init || io) + /* Only allow one IO */ + if (default_io) return NULL; - io = l_new(struct mesh_io, 1); + default_io = l_new(struct mesh_io, 1); + default_io->ready = cb; + default_io->user_data = user_data; + default_io->favored_index = *(int *) opts; + default_io->rx_regs = l_queue_new(); + + if (type >= MESH_IO_TYPE_AUTO) { + if (!mesh_mgmt_list(ctl_alert, L_UINT_TO_PTR(type))) + goto fail; - io->type = type; - io->api = api; + return default_io; + } + + api = io_api(type); - if (!api->init(io, opts, cb, user_data)) + if (!api || !api->init) goto fail; - if (!io_list) - io_list = l_queue_new(); + default_io->api = api; - if (l_queue_push_head(io_list, io)) - return io; + if (!api->init(default_io, opts, user_data)) + goto fail; -fail: - if (api->destroy) - api->destroy(io); + return default_io; - l_free(io); +fail: + free_io(default_io); + default_io = NULL; return NULL; } void mesh_io_destroy(struct mesh_io *io) { - io = l_queue_remove_if(io_list, match_by_io, io); - - if (io && io->api && io->api->destroy) - io->api->destroy(io); - - l_free(io); - - if (l_queue_isempty(io_list)) { - l_queue_destroy(io_list, NULL); - io_list = NULL; - } } bool mesh_io_get_caps(struct mesh_io *io, struct mesh_io_caps *caps) { - io = l_queue_find(io_list, match_by_io, io); + if (io != default_io) + return false; if (io && io->api && io->api->caps) return io->api->caps(io, caps); @@ -115,7 +181,17 @@ bool mesh_io_register_recv_cb(struct mes uint8_t len, mesh_io_recv_func_t cb, void *user_data) { - io = l_queue_find(io_list, match_by_io, io); + struct mesh_io_reg *rx_reg; + + if (io != default_io) + return false; + + rx_reg = l_malloc(sizeof(struct mesh_io_reg) + len); + rx_reg->cb = cb; + rx_reg->len = len; + rx_reg->user_data = user_data; + memcpy(rx_reg->filter, filter, len); + l_queue_push_head(io->rx_regs, rx_reg); if (io && io->api && io->api->reg) return io->api->reg(io, filter, len, cb, user_data); @@ -123,10 +199,24 @@ bool mesh_io_register_recv_cb(struct mes return false; } +static bool by_filter(const void *a, const void *b) +{ + const struct mesh_io_reg *rx_reg = a; + const uint8_t *filter = b; + + return rx_reg->filter[0] == filter[0]; +} + bool mesh_io_deregister_recv_cb(struct mesh_io *io, const uint8_t *filter, uint8_t len) { - io = l_queue_find(io_list, match_by_io, io); + struct mesh_io_reg *rx_reg; + + if (io != default_io) + return false; + + rx_reg = l_queue_remove_if(io->rx_regs, by_filter, filter); + l_free(rx_reg); if (io && io->api && io->api->dereg) return io->api->dereg(io, filter, len); @@ -137,10 +227,11 @@ bool mesh_io_deregister_recv_cb(struct m bool mesh_io_send(struct mesh_io *io, struct mesh_io_send_info *info, const uint8_t *data, uint16_t len) { - io = l_queue_find(io_list, match_by_io, io); + if (io && io != default_io) + return false; if (!io) - io = l_queue_peek_head(io_list); + io = default_io; if (io && io->api && io->api->send) return io->api->send(io, info, data, len); @@ -151,7 +242,11 @@ bool mesh_io_send(struct mesh_io *io, st bool mesh_io_send_cancel(struct mesh_io *io, const uint8_t *pattern, uint8_t len) { - io = l_queue_find(io_list, match_by_io, io); + if (io && io != default_io) + return false; + + if (!io) + io = default_io; if (io && io->api && io->api->cancel) return io->api->cancel(io, pattern, len); diff -pruN 5.65-1/mesh/mesh-io-generic.c 5.66-1/mesh/mesh-io-generic.c --- 5.65-1/mesh/mesh-io-generic.c 2022-07-24 20:59:52.000000000 +0000 +++ 5.66-1/mesh/mesh-io-generic.c 2022-11-10 20:22:09.000000000 +0000 @@ -19,6 +19,7 @@ #include "monitor/bt.h" #include "src/shared/hci.h" +#include "src/shared/mgmt.h" #include "lib/bluetooth.h" #include "lib/mgmt.h" @@ -29,14 +30,12 @@ #include "mesh/mesh-io-generic.h" struct mesh_io_private { + struct mesh_io *io; struct bt_hci *hci; - void *user_data; - mesh_io_ready_func_t ready_callback; struct l_timeout *tx_timeout; struct l_queue *rx_regs; struct l_queue *tx_pkts; struct tx_pkt *tx; - uint16_t index; uint16_t interval; bool sending; bool active; @@ -385,16 +384,13 @@ static void hci_init(void *user_data) { struct mesh_io *io = user_data; bool result = true; - bool restarted = false; - if (io->pvt->hci) { - restarted = true; + if (io->pvt->hci) bt_hci_unref(io->pvt->hci); - } - io->pvt->hci = bt_hci_new_user_channel(io->pvt->index); + io->pvt->hci = bt_hci_new_user_channel(io->index); if (!io->pvt->hci) { - l_error("Failed to start mesh io (hci %u): %s", io->pvt->index, + l_error("Failed to start mesh io (hci %u): %s", io->index, strerror(errno)); result = false; } @@ -405,47 +401,26 @@ static void hci_init(void *user_data) bt_hci_register(io->pvt->hci, BT_HCI_EVT_LE_META_EVENT, event_callback, io, NULL); - l_debug("Started mesh on hci %u", io->pvt->index); + l_debug("Started mesh on hci %u", io->index); - if (restarted) - restart_scan(io->pvt); + restart_scan(io->pvt); } - if (io->pvt->ready_callback) - io->pvt->ready_callback(io->pvt->user_data, result); + if (io->ready) + io->ready(io->user_data, result); } -static void read_info(int index, void *user_data) -{ - struct mesh_io *io = user_data; - - if (io->pvt->index != MGMT_INDEX_NONE && - index != io->pvt->index) { - l_debug("Ignore index %d", index); - return; - } - - io->pvt->index = index; - hci_init(io); -} - -static bool dev_init(struct mesh_io *io, void *opts, - mesh_io_ready_func_t cb, void *user_data) +static bool dev_init(struct mesh_io *io, void *opts, void *user_data) { if (!io || io->pvt) return false; io->pvt = l_new(struct mesh_io_private, 1); - io->pvt->index = *(int *)opts; io->pvt->rx_regs = l_queue_new(); io->pvt->tx_pkts = l_queue_new(); - io->pvt->ready_callback = cb; - io->pvt->user_data = user_data; - - if (io->pvt->index == MGMT_INDEX_NONE) - return mesh_mgmt_list(read_info, io); + io->pvt->io = io; l_idle_oneshot(hci_init, io, NULL); diff -pruN 5.65-1/mesh/mesh-io.h 5.66-1/mesh/mesh-io.h --- 5.65-1/mesh/mesh-io.h 2022-07-24 20:59:52.000000000 +0000 +++ 5.66-1/mesh/mesh-io.h 2022-11-10 20:22:09.000000000 +0000 @@ -14,8 +14,10 @@ struct mesh_io; enum mesh_io_type { MESH_IO_TYPE_NONE = 0, + MESH_IO_TYPE_UNIT_TEST, + MESH_IO_TYPE_AUTO, /* If MGMT required, add after here */ + MESH_IO_TYPE_MGMT, MESH_IO_TYPE_GENERIC, - MESH_IO_TYPE_UNIT_TEST }; enum mesh_io_timing_type { diff -pruN 5.65-1/mesh/mesh-io-mgmt.c 5.66-1/mesh/mesh-io-mgmt.c --- 5.65-1/mesh/mesh-io-mgmt.c 1970-01-01 00:00:00.000000000 +0000 +++ 5.66-1/mesh/mesh-io-mgmt.c 2022-11-10 20:22:09.000000000 +0000 @@ -0,0 +1,788 @@ +// SPDX-License-Identifier: LGPL-2.1-or-later +/* + * + * BlueZ - Bluetooth protocol stack for Linux + * + * Copyright (C) 2020 Intel Corporation. All rights reserved. + * + * + */ + +#ifdef HAVE_CONFIG_H +#include +#endif + +#include +#include +#include +#include +#include + +#include "monitor/bt.h" +#include "lib/bluetooth.h" +#include "lib/bluetooth.h" +#include "lib/mgmt.h" +#include "src/shared/mgmt.h" + +#include "mesh/mesh-defs.h" +#include "mesh/util.h" +#include "mesh/mesh-mgmt.h" +#include "mesh/mesh-io.h" +#include "mesh/mesh-io-api.h" +#include "mesh/mesh-io-mgmt.h" + +struct mesh_io_private { + struct mesh_io *io; + void *user_data; + struct l_timeout *tx_timeout; + struct l_queue *dup_filters; + struct l_queue *rx_regs; + struct l_queue *tx_pkts; + struct tx_pkt *tx; + unsigned int tx_id; + unsigned int rx_id; + uint16_t send_idx; + uint16_t interval; + uint8_t handle; + bool sending; + bool active; +}; + +struct pvt_rx_reg { + mesh_io_recv_func_t cb; + void *user_data; + uint8_t len; + uint8_t filter[0]; +}; + +struct process_data { + struct mesh_io_private *pvt; + const uint8_t *data; + uint8_t len; + struct mesh_io_recv_info info; +}; + +struct tx_pkt { + struct mesh_io_send_info info; + bool delete; + uint8_t len; + uint8_t pkt[30]; +}; + +struct tx_pattern { + const uint8_t *data; + uint8_t len; +}; + +#define DUP_FILTER_TIME 1000 +/* Accept one instance of unique message a second */ +struct dup_filter { + uint64_t data; + uint32_t instant; + uint8_t addr[6]; +} __packed; + +static struct mesh_io_private *pvt; + +static uint32_t get_instant(void) +{ + struct timeval tm; + uint32_t instant; + + gettimeofday(&tm, NULL); + instant = tm.tv_sec * 1000; + instant += tm.tv_usec / 1000; + + return instant; +} + +static uint32_t instant_remaining_ms(uint32_t instant) +{ + instant -= get_instant(); + + return instant; +} + +static bool find_by_addr(const void *a, const void *b) +{ + const struct dup_filter *filter = a; + + return !memcmp(filter->addr, b, 6); +} + +static void filter_timeout(struct l_timeout *timeout, void *user_data) +{ + struct dup_filter *filter; + uint32_t instant, delta; + + if (!pvt) + goto done; + + instant = get_instant(); + + filter = l_queue_peek_tail(pvt->dup_filters); + while (filter) { + delta = instant - filter->instant; + if (delta >= DUP_FILTER_TIME) { + l_queue_remove(pvt->dup_filters, filter); + l_free(filter); + } else { + l_timeout_modify(timeout, 1); + return; + } + + filter = l_queue_peek_tail(pvt->dup_filters); + } + +done: + l_timeout_remove(timeout); +} + +/* Ignore consequtive duplicate advertisements within timeout period */ +static bool filter_dups(const uint8_t *addr, const uint8_t *adv, + uint32_t instant) +{ + struct dup_filter *filter; + uint32_t instant_delta; + uint64_t data = l_get_be64(adv); + + filter = l_queue_remove_if(pvt->dup_filters, find_by_addr, addr); + if (!filter) { + filter = l_new(struct dup_filter, 1); + memcpy(filter->addr, addr, 6); + } + + /* Start filter expiration timer */ + if (!l_queue_length(pvt->dup_filters)) + l_timeout_create(1, filter_timeout, NULL, NULL); + + l_queue_push_head(pvt->dup_filters, filter); + instant_delta = instant - filter->instant; + + if (instant_delta >= DUP_FILTER_TIME || data != filter->data) { + filter->instant = instant; + filter->data = data; + return false; + } + + return true; +} + +static void process_rx_callbacks(void *v_reg, void *v_rx) +{ + struct pvt_rx_reg *rx_reg = v_reg; + struct process_data *rx = v_rx; + + if (!memcmp(rx->data, rx_reg->filter, rx_reg->len)) + rx_reg->cb(rx_reg->user_data, &rx->info, rx->data, rx->len); +} + +static void process_rx(struct mesh_io_private *pvt, int8_t rssi, + uint32_t instant, const uint8_t *addr, + const uint8_t *data, uint8_t len) +{ + struct process_data rx = { + .pvt = pvt, + .data = data, + .len = len, + .info.instant = instant, + .info.addr = addr, + .info.chan = 7, + .info.rssi = rssi, + }; + + print_packet("RX", data, len); + l_queue_foreach(pvt->rx_regs, process_rx_callbacks, &rx); +} + +static void send_cmplt(uint16_t index, uint16_t length, + const void *param, void *user_data) +{ + print_packet("Mesh Send Complete", param, length); +} + +static void event_device_found(uint16_t index, uint16_t length, + const void *param, void *user_data) +{ + const struct mgmt_ev_mesh_device_found *ev = param; + struct mesh_io *io = user_data; + const uint8_t *adv; + const uint8_t *addr; + uint32_t instant; + uint16_t adv_len; + uint16_t len = 0; + + if (ev->addr.type < 1 || ev->addr.type > 2) + return; + + instant = get_instant(); + adv = ev->eir; + adv_len = ev->eir_len; + addr = ev->addr.bdaddr.b; + + if (filter_dups(addr, adv, instant)) + return; + + while (len < adv_len - 1) { + uint8_t field_len = adv[0]; + + /* Check for the end of advertising data */ + if (field_len == 0) + break; + + len += field_len + 1; + + /* Do not continue data parsing if got incorrect length */ + if (len > adv_len) + break; + + if (adv[1] >= 0x29 && adv[1] <= 0x2B) + process_rx(io->pvt, ev->rssi, instant, addr, adv + 1, + adv[0]); + + adv += field_len + 1; + } +} + +static bool simple_match(const void *a, const void *b) +{ + return a == b; +} + +static bool find_by_ad_type(const void *a, const void *b) +{ + const struct tx_pkt *tx = a; + uint8_t ad_type = L_PTR_TO_UINT(b); + + return !ad_type || ad_type == tx->pkt[0]; +} + +static bool find_by_pattern(const void *a, const void *b) +{ + const struct tx_pkt *tx = a; + const struct tx_pattern *pattern = b; + + if (tx->len < pattern->len) + return false; + + return (!memcmp(tx->pkt, pattern->data, pattern->len)); +} + +static bool find_active(const void *a, const void *b) +{ + const struct pvt_rx_reg *rx_reg = a; + + /* Mesh specific AD types do *not* require active scanning, + * so do not turn on Active Scanning on their account. + */ + if (rx_reg->filter[0] < MESH_AD_TYPE_PROVISION || + rx_reg->filter[0] > MESH_AD_TYPE_BEACON) + return true; + + return false; +} + +static void mesh_up(uint8_t status, uint16_t length, + const void *param, void *user_data) +{ + int index = L_PTR_TO_UINT(user_data); + + l_debug("HCI%d Mesh up status: %d", index, status); +} + +static void le_up(uint8_t status, uint16_t length, + const void *param, void *user_data) +{ + int index = L_PTR_TO_UINT(user_data); + + l_debug("HCI%d LE up status: %d", index, status); +} + +static void ctl_up(uint8_t status, uint16_t length, + const void *param, void *user_data) +{ + int index = L_PTR_TO_UINT(user_data); + uint16_t len; + struct mgmt_cp_set_mesh *mesh; + uint8_t mesh_ad_types[] = { MESH_AD_TYPE_NETWORK, + MESH_AD_TYPE_BEACON, MESH_AD_TYPE_PROVISION }; + + l_debug("HCI%d is up status: %d", index, status); + if (status) + return; + + len = sizeof(struct mgmt_cp_set_mesh) + sizeof(mesh_ad_types); + mesh = l_malloc(len); + + mesh->enable = 1; + mesh->window = L_CPU_TO_LE16(0x1000); + mesh->period = L_CPU_TO_LE16(0x1000); + mesh->num_ad_types = sizeof(mesh_ad_types); + memcpy(mesh->ad_types, mesh_ad_types, sizeof(mesh_ad_types)); + + mesh_mgmt_send(MGMT_OP_SET_MESH_RECEIVER, index, len, mesh, + mesh_up, L_UINT_TO_PTR(index), NULL); + l_debug("done %d mesh startup", index); + + l_free(mesh); + + if (pvt->send_idx == MGMT_INDEX_NONE) { + pvt->send_idx = index; + if (pvt && pvt->io && pvt->io->ready) { + pvt->io->ready(pvt->io->user_data, true); + pvt->io->ready = NULL; + } + } +} + +static void read_info_cb(uint8_t status, uint16_t length, + const void *param, void *user_data) +{ + unsigned char le[] = { 0x01 }; + int index = L_PTR_TO_UINT(user_data); + const struct mgmt_rp_read_info *rp = param; + uint32_t current_settings, supported_settings; + + l_debug("hci %u status 0x%02x", index, status); + + if (!pvt) + return; + + if (status != MGMT_STATUS_SUCCESS) { + l_error("Failed to read info for hci index %u: %s (0x%02x)", + index, mgmt_errstr(status), status); + return; + } + + if (length < sizeof(*rp)) { + l_error("Read info response too short"); + return; + } + + current_settings = btohl(rp->current_settings); + supported_settings = btohl(rp->supported_settings); + + if (!(supported_settings & MGMT_SETTING_LE)) { + l_info("Controller hci %u does not support LE", index); + return; + } + + if (!(current_settings & MGMT_SETTING_POWERED)) { + unsigned char power[] = { 0x01 }; + + /* TODO: Initialize this HCI controller */ + l_info("Controller hci %u not in use", index); + + mesh_mgmt_send(MGMT_OP_SET_LE, index, + sizeof(le), &le, + le_up, L_UINT_TO_PTR(index), NULL); + + mesh_mgmt_send(MGMT_OP_SET_POWERED, index, + sizeof(power), &power, + ctl_up, L_UINT_TO_PTR(index), NULL); + } else { + + l_info("Controller hci %u already in use (%x)", + index, current_settings); + + /* Share this controller with bluetoothd */ + mesh_mgmt_send(MGMT_OP_SET_LE, index, + sizeof(le), &le, + ctl_up, L_UINT_TO_PTR(index), NULL); + + } +} + +static bool dev_init(struct mesh_io *io, void *opts, void *user_data) +{ + uint16_t index = *(int *)opts; + + if (!io || pvt) + return false; + + pvt = l_new(struct mesh_io_private, 1); + + pvt->send_idx = MGMT_INDEX_NONE; + + mesh_mgmt_send(MGMT_OP_READ_INFO, index, 0, NULL, + read_info_cb, L_UINT_TO_PTR(index), NULL); + + pvt->rx_id = mesh_mgmt_register(MGMT_EV_MESH_DEVICE_FOUND, + MGMT_INDEX_NONE, event_device_found, io, NULL); + pvt->tx_id = mesh_mgmt_register(MGMT_EV_MESH_PACKET_CMPLT, + MGMT_INDEX_NONE, send_cmplt, io, NULL); + + pvt->dup_filters = l_queue_new(); + pvt->rx_regs = l_queue_new(); + pvt->tx_pkts = l_queue_new(); + + pvt->io = io; + io->pvt = pvt; + + return true; +} + +static void free_rx_reg(void *user_data) +{ + struct pvt_rx_reg *rx_reg = user_data; + + l_free(rx_reg); +} + + +static bool dev_destroy(struct mesh_io *io) +{ + unsigned char param[] = { 0x00 }; + + if (io->pvt != pvt) + return true; + + mesh_mgmt_send(MGMT_OP_SET_POWERED, io->index, sizeof(param), ¶m, + NULL, NULL, NULL); + + mesh_mgmt_unregister(pvt->rx_id); + mesh_mgmt_unregister(pvt->tx_id); + l_timeout_remove(pvt->tx_timeout); + l_queue_destroy(pvt->dup_filters, l_free); + l_queue_destroy(pvt->rx_regs, free_rx_reg); + l_queue_destroy(pvt->tx_pkts, l_free); + io->pvt = NULL; + l_free(pvt); + pvt = NULL; + + return true; +} + +static bool dev_caps(struct mesh_io *io, struct mesh_io_caps *caps) +{ + struct mesh_io_private *pvt = io->pvt; + + if (!pvt || !caps) + return false; + + caps->max_num_filters = 255; + caps->window_accuracy = 50; + + return true; +} + +static void send_cancel(struct mesh_io_private *pvt) +{ + struct mgmt_cp_mesh_send_cancel remove; + + if (!pvt) + return; + + if (pvt->handle) { + remove.handle = pvt->handle; + l_debug("Cancel TX"); + mesh_mgmt_send(MGMT_OP_MESH_SEND_CANCEL, pvt->send_idx, + sizeof(remove), &remove, + NULL, NULL, NULL); + } +} + +static void tx_to(struct l_timeout *timeout, void *user_data); +static void send_queued(uint8_t status, uint16_t length, + const void *param, void *user_data) +{ + struct tx_pkt *tx = user_data; + + if (status) + l_debug("Mesh Send Failed: %d", status); + else if (param && length >= 1) + pvt->handle = *(uint8_t *) param; + + if (tx->delete) { + l_queue_remove_if(pvt->tx_pkts, simple_match, tx); + l_free(tx); + pvt->tx = NULL; + } +} + +static void send_pkt(struct mesh_io_private *pvt, struct tx_pkt *tx, + uint16_t interval) +{ + uint8_t buffer[sizeof(struct mgmt_cp_mesh_send) + tx->len + 1]; + struct mgmt_cp_mesh_send *send = (void *) buffer; + uint16_t index; + size_t len; + + if (!pvt) + return; + + index = pvt->send_idx; + + len = sizeof(buffer); + memset(send, 0, len); + send->addr.type = BDADDR_LE_RANDOM; + send->instant = 0; + send->delay = 0; + send->cnt = 1; + send->adv_data_len = tx->len + 1; + send->adv_data[0] = tx->len; + memcpy(send->adv_data + 1, tx->pkt, tx->len); + mesh_mgmt_send(MGMT_OP_MESH_SEND, index, + len, send, send_queued, tx, NULL); + print_packet("Mesh Send Start", tx->pkt, tx->len); + pvt->tx = tx; +} + +static void tx_to(struct l_timeout *timeout, void *user_data) +{ + struct mesh_io_private *pvt = user_data; + struct tx_pkt *tx; + uint16_t ms; + uint8_t count; + + if (!pvt) + return; + + tx = l_queue_pop_head(pvt->tx_pkts); + if (!tx) { + l_timeout_remove(timeout); + pvt->tx_timeout = NULL; + send_cancel(pvt); + pvt->tx = NULL; + return; + } + + if (tx->info.type == MESH_IO_TIMING_TYPE_GENERAL) { + ms = tx->info.u.gen.interval; + count = tx->info.u.gen.cnt; + if (count != MESH_IO_TX_COUNT_UNLIMITED) + tx->info.u.gen.cnt--; + } else { + ms = 25; + count = 1; + } + + tx->delete = !!(count == 1); + + send_pkt(pvt, tx, ms); + + if (count == 1) { + /* Recalculate wakeup if we are responding to POLL */ + tx = l_queue_peek_head(pvt->tx_pkts); + + if (tx && tx->info.type == MESH_IO_TIMING_TYPE_POLL_RSP) { + ms = instant_remaining_ms(tx->info.u.poll_rsp.instant + + tx->info.u.poll_rsp.delay); + } + } else + l_queue_push_tail(pvt->tx_pkts, tx); + + if (timeout) { + pvt->tx_timeout = timeout; + l_timeout_modify_ms(timeout, ms); + } else + pvt->tx_timeout = l_timeout_create_ms(ms, tx_to, pvt, NULL); +} + +static void tx_worker(void *user_data) +{ + struct mesh_io_private *pvt = user_data; + struct tx_pkt *tx; + uint32_t delay; + + tx = l_queue_peek_head(pvt->tx_pkts); + if (!tx) + return; + + switch (tx->info.type) { + case MESH_IO_TIMING_TYPE_GENERAL: + if (tx->info.u.gen.min_delay == tx->info.u.gen.max_delay) + delay = tx->info.u.gen.min_delay; + else { + l_getrandom(&delay, sizeof(delay)); + delay %= tx->info.u.gen.max_delay - + tx->info.u.gen.min_delay; + delay += tx->info.u.gen.min_delay; + } + break; + + case MESH_IO_TIMING_TYPE_POLL: + if (tx->info.u.poll.min_delay == tx->info.u.poll.max_delay) + delay = tx->info.u.poll.min_delay; + else { + l_getrandom(&delay, sizeof(delay)); + delay %= tx->info.u.poll.max_delay - + tx->info.u.poll.min_delay; + delay += tx->info.u.poll.min_delay; + } + break; + + case MESH_IO_TIMING_TYPE_POLL_RSP: + /* Delay until Instant + Delay */ + delay = instant_remaining_ms(tx->info.u.poll_rsp.instant + + tx->info.u.poll_rsp.delay); + if (delay > 255) + delay = 0; + break; + + default: + return; + } + + if (!delay) + tx_to(pvt->tx_timeout, pvt); + else if (pvt->tx_timeout) + l_timeout_modify_ms(pvt->tx_timeout, delay); + else + pvt->tx_timeout = l_timeout_create_ms(delay, tx_to, pvt, NULL); +} + +static bool send_tx(struct mesh_io *io, struct mesh_io_send_info *info, + const uint8_t *data, uint16_t len) +{ + struct tx_pkt *tx; + bool sending = false; + + if (!info || !data || !len || len > sizeof(tx->pkt)) + return false; + + tx = l_new(struct tx_pkt, 1); + + memcpy(&tx->info, info, sizeof(tx->info)); + memcpy(&tx->pkt, data, len); + tx->len = len; + + if (info->type == MESH_IO_TIMING_TYPE_POLL_RSP) + l_queue_push_head(pvt->tx_pkts, tx); + else { + if (pvt->tx) + sending = true; + else + sending = !l_queue_isempty(pvt->tx_pkts); + + l_queue_push_tail(pvt->tx_pkts, tx); + } + + if (!sending) { + l_timeout_remove(pvt->tx_timeout); + pvt->tx_timeout = NULL; + l_idle_oneshot(tx_worker, pvt, NULL); + } + + return true; +} + +static bool tx_cancel(struct mesh_io *io, const uint8_t *data, uint8_t len) +{ + struct mesh_io_private *pvt = io->pvt; + struct tx_pkt *tx; + + if (!data) + return false; + + if (len == 1) { + do { + tx = l_queue_remove_if(pvt->tx_pkts, find_by_ad_type, + L_UINT_TO_PTR(data[0])); + l_free(tx); + + if (tx == pvt->tx) + pvt->tx = NULL; + + } while (tx); + } else { + struct tx_pattern pattern = { + .data = data, + .len = len + }; + + do { + tx = l_queue_remove_if(pvt->tx_pkts, find_by_pattern, + &pattern); + l_free(tx); + + if (tx == pvt->tx) + pvt->tx = NULL; + + } while (tx); + } + + if (l_queue_isempty(pvt->tx_pkts)) { + send_cancel(pvt); + l_timeout_remove(pvt->tx_timeout); + pvt->tx_timeout = NULL; + } + + return true; +} + +static bool find_by_filter(const void *a, const void *b) +{ + const struct pvt_rx_reg *rx_reg = a; + const uint8_t *filter = b; + + return !memcmp(rx_reg->filter, filter, rx_reg->len); +} + +static bool recv_register(struct mesh_io *io, const uint8_t *filter, + uint8_t len, mesh_io_recv_func_t cb, void *user_data) +{ + struct pvt_rx_reg *rx_reg; + bool active = false; + + if (!cb || !filter || !len || io->pvt != pvt) + return false; + + rx_reg = l_queue_remove_if(pvt->rx_regs, find_by_filter, filter); + + free_rx_reg(rx_reg); + rx_reg = l_malloc(sizeof(*rx_reg) + len); + + memcpy(rx_reg->filter, filter, len); + rx_reg->len = len; + rx_reg->cb = cb; + rx_reg->user_data = user_data; + + l_queue_push_head(pvt->rx_regs, rx_reg); + + /* Look for any AD types requiring Active Scanning */ + if (l_queue_find(pvt->rx_regs, find_active, NULL)) + active = true; + + if (pvt->active != active) { + pvt->active = active; + /* TODO: Request active or passive scanning */ + } + + return true; +} + +static bool recv_deregister(struct mesh_io *io, const uint8_t *filter, + uint8_t len) +{ + struct pvt_rx_reg *rx_reg; + bool active = false; + + if (io->pvt != pvt) + return false; + + rx_reg = l_queue_remove_if(pvt->rx_regs, find_by_filter, filter); + + free_rx_reg(rx_reg); + + /* Look for any AD types requiring Active Scanning */ + if (l_queue_find(pvt->rx_regs, find_active, NULL)) + active = true; + + if (active != pvt->active) { + pvt->active = active; + /* TODO: Request active or passive scanning */ + } + + return true; +} + +const struct mesh_io_api mesh_io_mgmt = { + .init = dev_init, + .destroy = dev_destroy, + .caps = dev_caps, + .send = send_tx, + .reg = recv_register, + .dereg = recv_deregister, + .cancel = tx_cancel, +}; diff -pruN 5.65-1/mesh/mesh-io-mgmt.h 5.66-1/mesh/mesh-io-mgmt.h --- 5.65-1/mesh/mesh-io-mgmt.h 1970-01-01 00:00:00.000000000 +0000 +++ 5.66-1/mesh/mesh-io-mgmt.h 2022-11-10 20:22:09.000000000 +0000 @@ -0,0 +1,11 @@ +/* SPDX-License-Identifier: LGPL-2.1-or-later */ +/* + * + * BlueZ - Bluetooth protocol stack for Linux + * + * Copyright (C) 2018 Intel Corporation. All rights reserved. + * + * + */ + +extern const struct mesh_io_api mesh_io_mgmt; diff -pruN 5.65-1/mesh/mesh-io-unit.c 5.66-1/mesh/mesh-io-unit.c --- 5.65-1/mesh/mesh-io-unit.c 2022-07-24 20:59:52.000000000 +0000 +++ 5.66-1/mesh/mesh-io-unit.c 2022-11-10 20:22:09.000000000 +0000 @@ -25,13 +25,13 @@ #include "mesh/dbus.h" #include "mesh/mesh-io.h" #include "mesh/mesh-io-api.h" -#include "mesh/mesh-io-generic.h" +#include "mesh/mesh-io-unit.h" struct mesh_io_private { + struct mesh_io *io; struct l_io *sio; void *user_data; char *unique_name; - mesh_io_ready_func_t ready_callback; struct l_timeout *tx_timeout; struct l_queue *rx_regs; struct l_queue *tx_pkts; @@ -203,14 +203,13 @@ static void unit_up(void *user_data) l_debug("Started io-unit"); - if (pvt->ready_callback) - pvt->ready_callback(pvt->user_data, true); + if (pvt->io && pvt->io->ready) + pvt->io->ready(pvt->user_data, true); l_timeout_create_ms(1, get_name, pvt, NULL); } -static bool unit_init(struct mesh_io *io, void *opt, - mesh_io_ready_func_t cb, void *user_data) +static bool unit_init(struct mesh_io *io, void *opt, void *user_data) { struct mesh_io_private *pvt; char *sk_path; @@ -247,7 +246,7 @@ static bool unit_init(struct mesh_io *io pvt->rx_regs = l_queue_new(); pvt->tx_pkts = l_queue_new(); - pvt->ready_callback = cb; + pvt->io = io; pvt->user_data = user_data; io->pvt = pvt; diff -pruN 5.65-1/mesh/mesh-mgmt.c 5.66-1/mesh/mesh-mgmt.c --- 5.65-1/mesh/mesh-mgmt.c 2022-07-24 20:59:52.000000000 +0000 +++ 5.66-1/mesh/mesh-mgmt.c 2022-11-10 20:22:09.000000000 +0000 @@ -12,35 +12,78 @@ #include #endif +#include + #include "lib/bluetooth.h" #include "lib/mgmt.h" #include "src/shared/mgmt.h" -#include "ell/queue.h" -#include "ell/log.h" -#include "ell/util.h" - #include "mesh/mesh-mgmt.h" -struct read_info_reg { - mesh_mgmt_read_info_func_t cb; - void *user_data; +struct mesh_controler { + int index; + bool mesh_support; + bool powered; }; -struct read_info_req { - int index; - struct mesh_io *io; +static mesh_mgmt_read_info_func_t ctl_info; +static struct mgmt *mgmt_mesh; +static struct l_queue *ctl_list; +static void *list_user_data; +static bool mesh_detected; + +static const uint8_t set_exp_feat_param_mesh[] = { + 0x76, 0x6e, 0xf3, 0xe8, 0x24, 0x5f, 0x05, 0xbf, /* UUID - Mesh */ + 0x8d, 0x4d, 0x03, 0x7a, 0xd7, 0x63, 0xe4, 0x2c, + 0x01, /* Action - enable */ }; -static struct mgmt *mgmt_mesh; -static struct l_queue *read_info_regs; +static bool by_index(const void *a, const void *b) +{ + const struct mesh_controler *ctl = a; + int index = L_PTR_TO_UINT(b); + + return ctl->index == index; +} -static void process_read_info_req(void *data, void *user_data) +static void index_removed(uint16_t index, uint16_t length, const void *param, + void *user_data); +static void features_cb(uint8_t status, uint16_t length, + const void *param, void *user_data) { - struct read_info_reg *reg = data; int index = L_PTR_TO_UINT(user_data); + struct mesh_controler *ctl; + - reg->cb(index, reg->user_data); + ctl = l_queue_find(ctl_list, by_index, L_UINT_TO_PTR(index)); + if (!ctl) + return; + + l_debug("Status: %d, Length: %d", status, length); + if (status != MGMT_STATUS_NOT_SUPPORTED && + status != MGMT_STATUS_UNKNOWN_COMMAND) { + ctl->mesh_support = true; + if (!mesh_detected) { + mgmt_register(mgmt_mesh, MGMT_EV_INDEX_REMOVED, + MGMT_INDEX_NONE, index_removed, + NULL, NULL); + } + mesh_detected = true; + } else + l_debug("Kernel mesh not supported for hci%u", index); + + if (ctl_info) + ctl_info(index, true, ctl->powered, ctl->mesh_support, + list_user_data); +} + +static void set_exp_mesh_cb(uint8_t status, uint16_t length, + const void *param, void *user_data) +{ + int index = L_PTR_TO_UINT(user_data); + + mesh_mgmt_send(MGMT_OP_MESH_READ_FEATURES, index, 0, NULL, + features_cb, L_UINT_TO_PTR(index), NULL); } static void read_info_cb(uint8_t status, uint16_t length, @@ -49,12 +92,25 @@ static void read_info_cb(uint8_t status, int index = L_PTR_TO_UINT(user_data); const struct mgmt_rp_read_info *rp = param; uint32_t current_settings, supported_settings; + struct mesh_controler *ctl; l_debug("hci %u status 0x%02x", index, status); + ctl = l_queue_find(ctl_list, by_index, L_UINT_TO_PTR(index)); + if (!ctl) + return; + if (status != MGMT_STATUS_SUCCESS) { + ctl = l_queue_remove_if(ctl_list, by_index, + L_UINT_TO_PTR(index)); l_error("Failed to read info for hci index %u: %s (0x%02x)", index, mgmt_errstr(status), status); + + l_warn("Hci dev %d removal detected", index); + if (ctl && ctl_info) + ctl_info(index, false, false, false, list_user_data); + + l_free(ctl); return; } @@ -69,23 +125,36 @@ static void read_info_cb(uint8_t status, l_debug("settings: supp %8.8x curr %8.8x", supported_settings, current_settings); - if (current_settings & MGMT_SETTING_POWERED) { - l_info("Controller hci %u is in use", index); - return; - } - if (!(supported_settings & MGMT_SETTING_LE)) { l_info("Controller hci %u does not support LE", index); + l_queue_remove(ctl_list, ctl); + l_free(ctl); return; } - l_queue_foreach(read_info_regs, process_read_info_req, - L_UINT_TO_PTR(index)); + if (current_settings & MGMT_SETTING_POWERED) + ctl->powered = true; + + mesh_mgmt_send(MGMT_OP_SET_EXP_FEATURE, index, + sizeof(set_exp_feat_param_mesh), + set_exp_feat_param_mesh, + set_exp_mesh_cb, L_UINT_TO_PTR(index), NULL); } static void index_added(uint16_t index, uint16_t length, const void *param, void *user_data) { + struct mesh_controler *ctl = l_queue_find(ctl_list, by_index, + L_UINT_TO_PTR(index)); + + if (!ctl) { + ctl = l_new(struct mesh_controler, 1); + ctl->index = index; + l_queue_push_head(ctl_list, ctl); + } else { + ctl->mesh_support = ctl->powered = false; + } + mgmt_send(mgmt_mesh, MGMT_OP_READ_INFO, index, 0, NULL, read_info_cb, L_UINT_TO_PTR(index), NULL); } @@ -93,7 +162,9 @@ static void index_added(uint16_t index, static void index_removed(uint16_t index, uint16_t length, const void *param, void *user_data) { - l_warn("Hci dev %4.4x removed", index); + mgmt_send(mgmt_mesh, MGMT_OP_READ_INFO, index, 0, NULL, + read_info_cb, L_UINT_TO_PTR(index), NULL); + } static void read_index_list_cb(uint8_t status, uint16_t length, @@ -133,8 +204,8 @@ static void read_index_list_cb(uint8_t s static bool mesh_mgmt_init(void) { - if (!read_info_regs) - read_info_regs = l_queue_new(); + if (!ctl_list) + ctl_list = l_queue_new(); if (!mgmt_mesh) { mgmt_mesh = mgmt_new_default(); @@ -146,8 +217,6 @@ static bool mesh_mgmt_init(void) mgmt_register(mgmt_mesh, MGMT_EV_INDEX_ADDED, MGMT_INDEX_NONE, index_added, NULL, NULL); - mgmt_register(mgmt_mesh, MGMT_EV_INDEX_REMOVED, - MGMT_INDEX_NONE, index_removed, NULL, NULL); } return true; @@ -155,16 +224,11 @@ static bool mesh_mgmt_init(void) bool mesh_mgmt_list(mesh_mgmt_read_info_func_t cb, void *user_data) { - struct read_info_reg *reg; - if (!mesh_mgmt_init()) return false; - reg = l_new(struct read_info_reg, 1); - reg->cb = cb; - reg->user_data = user_data; - - l_queue_push_tail(read_info_regs, reg); + ctl_info = cb; + list_user_data = user_data; /* Use MGMT to find a candidate controller */ l_debug("send read index_list"); @@ -175,3 +239,35 @@ bool mesh_mgmt_list(mesh_mgmt_read_info_ return true; } + +void mesh_mgmt_destroy(void) +{ + mgmt_unref(mgmt_mesh); + mgmt_mesh = NULL; + ctl_info = NULL; + list_user_data = NULL; + l_queue_destroy(ctl_list, l_free); + ctl_list = NULL; +} + +unsigned int mesh_mgmt_send(uint16_t opcode, uint16_t index, + uint16_t length, const void *param, + mgmt_request_func_t callback, + void *user_data, mgmt_destroy_func_t destroy) +{ + return mgmt_send_timeout(mgmt_mesh, opcode, index, length, param, + callback, user_data, destroy, 0); +} + +unsigned int mesh_mgmt_register(uint16_t event, uint16_t index, + mgmt_notify_func_t callback, + void *user_data, mgmt_destroy_func_t destroy) +{ + return mgmt_register(mgmt_mesh, event, index, callback, + user_data, destroy); +} + +bool mesh_mgmt_unregister(unsigned int id) +{ + return mgmt_unregister(mgmt_mesh, id); +} diff -pruN 5.65-1/mesh/mesh-mgmt.h 5.66-1/mesh/mesh-mgmt.h --- 5.65-1/mesh/mesh-mgmt.h 2022-07-24 20:59:52.000000000 +0000 +++ 5.66-1/mesh/mesh-mgmt.h 2022-11-10 20:22:09.000000000 +0000 @@ -9,6 +9,16 @@ */ #include -typedef void (*mesh_mgmt_read_info_func_t)(int index, void *user_data); +typedef void (*mesh_mgmt_read_info_func_t)(int index, bool added, bool powered, + bool mesh, void *user_data); bool mesh_mgmt_list(mesh_mgmt_read_info_func_t cb, void *user_data); +unsigned int mesh_mgmt_send(uint16_t opcode, uint16_t index, + uint16_t length, const void *param, + mgmt_request_func_t callback, + void *user_data, mgmt_destroy_func_t destroy); +unsigned int mesh_mgmt_register(uint16_t event, uint16_t index, + mgmt_notify_func_t callback, + void *user_data, mgmt_destroy_func_t destroy); +bool mesh_mgmt_unregister(unsigned int id); +void mesh_mgmt_destroy(void); diff -pruN 5.65-1/mesh/model.c 5.66-1/mesh/model.c --- 5.65-1/mesh/model.c 2022-07-24 20:59:52.000000000 +0000 +++ 5.66-1/mesh/model.c 2022-11-10 20:22:09.000000000 +0000 @@ -1063,7 +1063,8 @@ int mesh_model_pub_set(struct mesh_node if (!mod->pub_enabled || (mod->cbs && !(mod->cbs->pub))) return MESH_STATUS_INVALID_PUB_PARAM; - if (!appkey_have_key(node_get_net(node), idx)) + if (!appkey_have_key(node_get_net(node), idx) || + !has_binding(mod->bindings, idx)) return MESH_STATUS_INVALID_APPKEY; /* diff -pruN 5.65-1/mesh/net.c 5.66-1/mesh/net.c --- 5.65-1/mesh/net.c 2022-07-24 20:59:52.000000000 +0000 +++ 5.66-1/mesh/net.c 2022-11-10 20:22:09.000000000 +0000 @@ -46,6 +46,7 @@ #define SEG_TO 2 #define MSG_TO 60 +#define SAR_DEL 10 #define DEFAULT_TRANSMIT_COUNT 1 #define DEFAULT_TRANSMIT_INTERVAL 100 @@ -166,6 +167,7 @@ struct mesh_sar { bool segmented; bool frnd; bool frnd_cred; + bool delete; uint8_t ttl; uint8_t last_seg; uint8_t key_aid; @@ -1026,12 +1028,11 @@ static bool msg_in_cache(struct mesh_net .mic = mic, }; - msg = l_queue_remove_if(net->msg_cache, match_cache, &tst); + msg = l_queue_find(net->msg_cache, match_cache, &tst); if (msg) { l_debug("Supressing duplicate %4.4x + %6.6x + %8.8x", src, seq, mic); - l_queue_push_head(net->msg_cache, msg); return true; } @@ -1493,14 +1494,27 @@ static void inseg_to(struct l_timeout *s static void inmsg_to(struct l_timeout *msg_timeout, void *user_data) { struct mesh_net *net = user_data; - struct mesh_sar *sar = l_queue_remove_if(net->sar_in, + struct mesh_sar *sar = l_queue_find(net->sar_in, match_msg_timeout, msg_timeout); - l_timeout_remove(msg_timeout); - if (!sar) + if (!sar) { + l_timeout_remove(msg_timeout); return; + } - sar->msg_timeout = NULL; + if (!sar->delete) { + /* + * Incomplete timer expired, cancel SAR and start + * delete timer + */ + l_timeout_remove(sar->seg_timeout); + sar->seg_timeout = NULL; + sar->delete = true; + l_timeout_modify(sar->msg_timeout, SAR_DEL); + return; + } + + l_queue_remove(net->sar_in, sar); mesh_sar_free(sar); } @@ -1763,7 +1777,8 @@ not_for_friend: return true; } -static uint16_t key_id_to_net_idx(struct mesh_net *net, uint32_t net_key_id) +static uint16_t key_id_to_net_idx(struct mesh_net *net, + uint32_t net_key_id, bool *frnd) { struct mesh_subnet *subnet; struct mesh_friend *friend; @@ -1771,6 +1786,9 @@ static uint16_t key_id_to_net_idx(struct if (!net) return NET_IDX_INVALID; + if (frnd) + *frnd = false; + subnet = l_queue_find(net->subnets, match_key_id, L_UINT_TO_PTR(net_key_id)); @@ -1780,8 +1798,12 @@ static uint16_t key_id_to_net_idx(struct friend = l_queue_find(net->friends, match_friend_key_id, L_UINT_TO_PTR(net_key_id)); - if (friend) + if (friend) { + if (frnd) + *frnd = true; + return friend->net_idx; + } friend = l_queue_find(net->negotiations, match_friend_key_id, L_UINT_TO_PTR(net_key_id)); @@ -1956,7 +1978,9 @@ static bool seg_rxed(struct mesh_net *ne /* Re-Send ACK for full msg */ send_net_ack(net, sar_in, expected); return true; - } + } else if (sar_in->delete) + /* Ignore cancelled */ + return false; } else { uint16_t len = MAX_SEG_TO_LEN(segN); @@ -2006,6 +2030,10 @@ static bool seg_rxed(struct mesh_net *ne /* Kill Inter-Seg timeout */ l_timeout_remove(sar_in->seg_timeout); sar_in->seg_timeout = NULL; + + /* Start delete timer */ + sar_in->delete = true; + l_timeout_modify(sar_in->msg_timeout, SAR_DEL); return true; } @@ -2059,7 +2087,7 @@ static bool ctl_received(struct mesh_net break; case NET_OP_FRND_POLL: - if (len != 1 || ttl) + if (len != 1 || ttl || pkt[0] > 1) return false; print_packet("Rx-NET_OP_FRND_POLL", pkt, len); @@ -2078,7 +2106,7 @@ static bool ctl_received(struct mesh_net return false; print_packet("Rx-NET_OP_FRND_REQUEST", pkt, len); - net_idx = key_id_to_net_idx(net, net_key_id); + net_idx = key_id_to_net_idx(net, net_key_id, NULL); friend_request(net, net_idx, src, pkt[0], pkt[1], l_get_be32(pkt + 1) & 0xffffff, l_get_be16(pkt + 5), pkt[7], @@ -2269,7 +2297,8 @@ static void send_msg_pkt(struct mesh_net } static enum _relay_advice packet_received(void *user_data, - uint32_t net_key_id, uint32_t iv_index, + uint32_t net_key_id, uint16_t net_idx, + bool frnd, uint32_t iv_index, const void *data, uint8_t size, int8_t rssi) { struct mesh_net *net = user_data; @@ -2278,16 +2307,11 @@ static enum _relay_advice packet_receive uint8_t net_ttl, key_aid, net_segO, net_segN, net_opcode; uint32_t net_seq, cache_cookie; uint16_t net_src, net_dst, net_seqZero; - uint16_t net_idx; uint8_t packet[31]; bool net_ctl, net_segmented, net_szmic, net_relay; memcpy(packet + 2, data, size); - net_idx = key_id_to_net_idx(net, net_key_id); - if (net_idx == NET_IDX_INVALID) - return RELAY_NONE; - print_packet("RX: Network [clr] :", packet + 2, size); if (!mesh_crypto_packet_parse(packet + 2, size, &net_ctl, &net_ttl, @@ -2389,6 +2413,13 @@ static enum _relay_advice packet_receive return RELAY_DISALLOWED; } + /* + * Messages that are encrypted with friendship credentials + * should *always* be relayed + */ + if (frnd) + return RELAY_ALWAYS; + /* If relay not enable, or no more hops allowed */ if (!net->relay.enable || net_ttl < 0x02) return RELAY_NONE; @@ -2414,7 +2445,9 @@ static void net_rx(void *net_ptr, void * uint8_t *out; size_t out_size; uint32_t net_key_id; + uint16_t net_idx; int8_t rssi = 0; + bool frnd; bool ivi_net = !!(net->iv_index & 1); bool ivi_pkt = !!(data->data[0] & 0x80); @@ -2438,9 +2471,21 @@ static void net_rx(void *net_ptr, void * rssi = data->info->rssi; } - relay_advice = packet_received(net, net_key_id, iv_index, out, out_size, - rssi); + net_idx = key_id_to_net_idx(net, net_key_id, &frnd); + + if (net_idx == NET_IDX_INVALID) + return; + + relay_advice = packet_received(net, net_key_id, net_idx, frnd, + iv_index, out, out_size, rssi); if (relay_advice > data->relay_advice) { + /* + * If packet was encrypted with friendship credentials, + * relay it using master credentials + */ + if (frnd && !mesh_net_get_key(net, false, net_idx, &net_key_id)) + return; + data->iv_index = iv_index; data->relay_advice = relay_advice; data->net_key_id = net_key_id; @@ -2613,7 +2658,8 @@ static bool update_kr_state(struct mesh_ { /* Figure out the key refresh phase */ if (kr) { - if (id == subnet->net_key_upd) { + if (subnet->kr_phase == KEY_REFRESH_PHASE_ONE && + id == subnet->net_key_upd) { l_debug("Beacon based KR phase 2 change"); return (key_refresh_phase_two(subnet->net, subnet->idx) == MESH_STATUS_SUCCESS); @@ -2671,6 +2717,10 @@ static bool update_iv_ivu_state(struct m if (iv_index == net->iv_index) return false; + /* Ignore beacon with invalid IV index value */ + if (net->iv_update && iv_index == net->iv_index + 1) + return false; + if (!net->iv_update) { l_debug("iv_upd_state = IV_UPD_UPDATING"); net->iv_upd_state = IV_UPD_UPDATING; @@ -2698,6 +2748,7 @@ static bool update_iv_ivu_state(struct m net->iv_index = iv_index; net->iv_update = ivu; + queue_friend_update(net); return true; } @@ -2708,7 +2759,7 @@ static void process_beacon(void *net_ptr struct net_beacon_data *beacon_data = user_data; uint32_t ivi; bool ivu, kr, local_kr; - struct mesh_subnet *subnet; + struct mesh_subnet *subnet, *primary_subnet; ivi = beacon_data->ivi; @@ -2723,6 +2774,17 @@ static void process_beacon(void *net_ptr if (!subnet) return; + /* + * @MshPRFv1.0.1 section 3.10.5: IV Update procedure + * If this node is a member of a primary subnet and receives a Secure + * Network beacon on a secondary subnet with an IV Index greater than + * the last known IV Index of the primary subnet, the Secure Network + * beacon shall be ignored. + */ + primary_subnet = get_primary_subnet(net); + if (primary_subnet && subnet != primary_subnet && ivi > net->iv_index) + return; + /* Get IVU and KR boolean bits from beacon */ ivu = beacon_data->ivu; kr = beacon_data->kr; @@ -2739,7 +2801,7 @@ static void process_beacon(void *net_ptr ivu != net->iv_update) updated |= update_iv_ivu_state(net, ivi, ivu); - if (kr != local_kr) + if (kr != local_kr || beacon_data->net_key_id != subnet->net_key_cur) updated |= update_kr_state(subnet, kr, beacon_data->net_key_id); if (updated) @@ -3067,6 +3129,13 @@ void mesh_net_send_seg(struct mesh_net * uint8_t segO = (hdr >> SEGO_HDR_SHIFT) & SEG_MASK; uint8_t segN = (hdr >> SEGN_HDR_SHIFT) & SEG_MASK; + /* + * MshPRFv1.0.1 section 3.4.5.2, Interface output filter: + * If TTL is set to 1, message shall be dropped. + */ + if (ttl == 1) + return; + /* TODO: Only used for current POLLed segments to LPNs */ l_debug("SEQ: %6.6x", seq + segO); @@ -3135,6 +3204,13 @@ bool mesh_net_app_send(struct mesh_net * (dst >= net->src_addr && dst <= net->last_addr)) return true; + /* + * MshPRFv1.0.1 section 3.4.5.2, Interface output filter: + * If TTL is set to 1, message shall be dropped. + */ + if (ttl == 1) + return true; + /* Setup OTA Network send */ payload = mesh_sar_new(msg_len); memcpy(payload->buf, msg, msg_len); @@ -3206,6 +3282,13 @@ void mesh_net_ack_send(struct mesh_net * uint8_t pkt_len; uint8_t pkt[30]; + /* + * MshPRFv1.0.1 section 3.4.5.2, Interface output filter: + * If TTL is set to 1, message shall be dropped. + */ + if (ttl == 1) + return; + hdr = NET_OP_SEG_ACKNOWLEDGE << OPCODE_HDR_SHIFT; hdr |= rly << RELAY_HDR_SHIFT; hdr |= (seqZero & SEQ_ZERO_MASK) << SEQ_ZERO_HDR_SHIFT; @@ -3264,6 +3347,13 @@ void mesh_net_transport_send(struct mesh if (*msg & 0xc0 || (9 + msg_len + 8 > 29)) return; + /* + * MshPRFv1.0.1 section 3.4.5.2, Interface output filter: + * If TTL is set to 1, message shall be dropped. + */ + if (ttl == 1) + return; + /* Enqueue for Friend if forwardable and from us */ if (!net_key_id && src >= net->src_addr && src <= net->last_addr) { uint32_t hdr = msg[0] << OPCODE_HDR_SHIFT; @@ -3580,24 +3670,14 @@ int mesh_net_set_heartbeat_sub(struct me return MESH_STATUS_UNSPECIFIED_ERROR; /* Check if the subscription should be disabled */ - if (IS_UNASSIGNED(src) || IS_UNASSIGNED(dst)) { + if (IS_UNASSIGNED(src) || IS_UNASSIGNED(dst) || !period_log) { if (IS_GROUP(sub->dst)) mesh_net_dst_unreg(net, sub->dst); + /* Preserve collected data, but disable */ sub->enabled = false; sub->dst = UNASSIGNED_ADDRESS; sub->src = UNASSIGNED_ADDRESS; - sub->count = 0; - sub->period = 0; - sub->min_hops = 0; - sub->max_hops = 0; - - } else if (!period_log && src == sub->src && dst == sub->dst) { - if (IS_GROUP(sub->dst)) - mesh_net_dst_unreg(net, sub->dst); - - /* Preserve collected data, but disable */ - sub->enabled = false; sub->period = 0; } else { @@ -3609,12 +3689,12 @@ int mesh_net_set_heartbeat_sub(struct me mesh_net_dst_reg(net, dst); } - sub->enabled = !!period_log; + sub->enabled = true; sub->src = src; sub->dst = dst; sub->count = 0; sub->period = log_to_uint32(period_log); - sub->min_hops = 0x00; + sub->min_hops = 0x7f; sub->max_hops = 0x00; gettimeofday(&time_now, NULL); sub->start = time_now.tv_sec; @@ -3628,8 +3708,6 @@ int mesh_net_set_heartbeat_sub(struct me return MESH_STATUS_SUCCESS; } - sub->min_hops = 0xff; - if (!sub->timer) sub->timer = l_timeout_create(sub->period, hb_sub_timeout_func, net, NULL); diff -pruN 5.65-1/mesh/node.c 5.66-1/mesh/node.c --- 5.65-1/mesh/node.c 2022-07-24 20:59:52.000000000 +0000 +++ 5.66-1/mesh/node.c 2022-11-10 20:22:09.000000000 +0000 @@ -1270,8 +1270,8 @@ static bool add_local_node(struct mesh_n MESH_STATUS_SUCCESS) return false; - if (!mesh_config_net_key_set_phase(node->cfg, net_key_idx, - KEY_REFRESH_PHASE_TWO)) + if (mesh_net_key_refresh_phase_set(node->net, net_key_idx, + KEY_REFRESH_PHASE_TWO) != MESH_STATUS_SUCCESS) return false; } diff -pruN 5.65-1/mesh/pb-adv.c 5.66-1/mesh/pb-adv.c --- 5.65-1/mesh/pb-adv.c 2022-07-24 20:59:52.000000000 +0000 +++ 5.66-1/mesh/pb-adv.c 2022-11-10 20:22:09.000000000 +0000 @@ -23,6 +23,8 @@ #include "mesh/provision.h" #include "mesh/pb-adv.h" +#include "mesh/util.h" + struct pb_adv_session { mesh_prov_open_func_t open_cb; @@ -158,7 +160,7 @@ static void send_adv_segs(struct pb_adv_ l_debug("max_seg: %2.2x", max_seg); l_debug("size: %2.2x, CRC: %2.2x", size, buf[9]); - pb_adv_send(session, MESH_IO_TX_COUNT_UNLIMITED, 200, + pb_adv_send(session, MESH_IO_TX_COUNT_UNLIMITED, 500, buf, init_size + 10); consumed = init_size; @@ -174,7 +176,7 @@ static void send_adv_segs(struct pb_adv_ buf[6] = (i << 2) | 0x02; memcpy(buf + 7, data + consumed, seg_size); - pb_adv_send(session, MESH_IO_TX_COUNT_UNLIMITED, 200, + pb_adv_send(session, MESH_IO_TX_COUNT_UNLIMITED, 500, buf, seg_size + 7); consumed += seg_size; @@ -270,7 +272,8 @@ static void send_ack(struct pb_adv_sessi ack.trans_num = trans_num; ack.opcode = PB_ADV_ACK; - pb_adv_send(session, 1, 100, &ack, sizeof(ack)); + pb_adv_send(session, MESH_IO_TX_COUNT_UNLIMITED, 500, + &ack, sizeof(ack)); } static void send_close_ind(struct pb_adv_session *session, uint8_t reason) diff -pruN 5.65-1/mesh/prov-acceptor.c 5.66-1/mesh/prov-acceptor.c --- 5.65-1/mesh/prov-acceptor.c 2022-07-24 20:59:52.000000000 +0000 +++ 5.66-1/mesh/prov-acceptor.c 2022-11-10 20:22:09.000000000 +0000 @@ -70,6 +70,7 @@ struct mesh_prov_acceptor { uint8_t material; uint8_t expected; int8_t previous; + bool failed; struct conf_input conf_inputs; uint8_t calc_key[16]; uint8_t salt[16]; @@ -383,6 +384,47 @@ static void send_rand(struct mesh_prov_a prov_send(prov, &msg, sizeof(msg)); } +static bool prov_start_check(struct prov_start *start, + struct mesh_net_prov_caps *caps) +{ + if (start->algorithm || start->pub_key > 1 || start->auth_method > 3) + return false; + + if (start->pub_key && !caps->pub_type) + return false; + + switch (start->auth_method) { + case 0: /* No OOB */ + if (start->auth_action != 0 || start->auth_size != 0) + return false; + + break; + + case 1: /* Static OOB */ + if (!caps->static_type || start->auth_action != 0 || + start->auth_size != 0) + return false; + + break; + + case 2: /* Output OOB */ + if (!(caps->output_action & (1 << start->auth_action)) || + start->auth_size == 0) + return false; + + break; + + case 3: /* Input OOB */ + if (!(caps->input_action & (1 << start->auth_action)) || + start->auth_size == 0) + return false; + + break; + } + + return true; +} + static void acp_prov_rx(void *user_data, const uint8_t *data, uint16_t len) { struct mesh_prov_acceptor *rx_prov = user_data; @@ -399,17 +441,23 @@ static void acp_prov_rx(void *user_data, l_debug("Provisioning packet received type: %2.2x (%u octets)", type, len); + if (type >= L_ARRAY_SIZE(expected_pdu_size)) { + l_error("Unknown PDU type: %2.2x", type); + fail.reason = PROV_ERR_INVALID_PDU; + goto failure; + } + if (type == prov->previous) { l_error("Ignore repeated %2.2x packet", type); return; - } else if (type > prov->expected || type < prov->previous) { + } else if (prov->failed || type > prov->expected || + type < prov->previous) { l_error("Expected %2.2x, Got:%2.2x", prov->expected, type); fail.reason = PROV_ERR_UNEXPECTED_PDU; goto failure; } - if (type >= L_ARRAY_SIZE(expected_pdu_size) || - len != expected_pdu_size[type]) { + if (len != expected_pdu_size[type]) { l_error("Expected PDU size %d, Got %d (type: %2.2x)", len, expected_pdu_size[type], type); fail.reason = PROV_ERR_INVALID_FORMAT; @@ -426,22 +474,16 @@ static void acp_prov_rx(void *user_data, memcpy(&prov->conf_inputs.start, data, sizeof(prov->conf_inputs.start)); - if (prov->conf_inputs.start.algorithm || - prov->conf_inputs.start.pub_key > 1 || - prov->conf_inputs.start.auth_method > 3) { + if (!prov_start_check(&prov->conf_inputs.start, + &prov->conf_inputs.caps)) { fail.reason = PROV_ERR_INVALID_FORMAT; goto failure; } if (prov->conf_inputs.start.pub_key) { - if (prov->conf_inputs.caps.pub_type) { - /* Prompt Agent for Private Key of OOB */ - mesh_agent_request_private_key(prov->agent, - priv_key_cb, prov); - } else { - fail.reason = PROV_ERR_INVALID_PDU; - goto failure; - } + /* Prompt Agent for Private Key of OOB */ + mesh_agent_request_private_key(prov->agent, + priv_key_cb, prov); } else { /* Ephemeral Public Key requested */ ecc_make_key(prov->conf_inputs.dev_pub_key, @@ -643,6 +685,8 @@ static void acp_prov_rx(void *user_data, failure: fail.opcode = PROV_FAILED; prov_send(prov, &fail, sizeof(fail)); + prov->failed = true; + prov->previous = -1; if (prov->cmplt) prov->cmplt(prov->caller_data, fail.reason, NULL); prov->cmplt = NULL; @@ -702,6 +746,7 @@ bool acceptor_start(uint8_t num_ele, uin prov->cmplt = complete_cb; prov->ob = l_queue_new(); prov->previous = -1; + prov->failed = false; prov->out_opcode = PROV_NONE; prov->caller_data = caller_data; diff -pruN 5.65-1/monitor/analyze.c 5.66-1/monitor/analyze.c --- 5.65-1/monitor/analyze.c 2022-07-24 20:59:52.000000000 +0000 +++ 5.66-1/monitor/analyze.c 2022-11-10 20:22:09.000000000 +0000 @@ -172,15 +172,18 @@ static void conn_destroy(void *data) print_field("%lu RX packets", conn->rx_num); print_field("%lu TX packets", conn->tx_num); print_field("%lu TX completed packets", conn->tx_num_comp); - print_field("%ld msec min latency", - conn->tx_lat_min.tv_sec * 1000 + - conn->tx_lat_min.tv_usec / 1000); - print_field("%ld msec max latency", - conn->tx_lat_max.tv_sec * 1000 + - conn->tx_lat_max.tv_usec / 1000); - print_field("%ld msec median latency", - conn->tx_lat_med.tv_sec * 1000 + - conn->tx_lat_med.tv_usec / 1000); + print_field("%lld msec min latency", + (long long) + (conn->tx_lat_min.tv_sec * 1000 + + conn->tx_lat_min.tv_usec / 1000)); + print_field("%lld msec max latency", + (long long) + (conn->tx_lat_max.tv_sec * 1000 + + conn->tx_lat_max.tv_usec / 1000)); + print_field("%lld msec median latency", + (long long) + (conn->tx_lat_med.tv_sec * 1000 + + conn->tx_lat_med.tv_usec / 1000)); print_field("%u octets TX min packet size", conn->tx_pkt_min); print_field("%u octets TX max packet size", conn->tx_pkt_max); print_field("%u octets TX median packet size", conn->tx_pkt_med); @@ -372,12 +375,8 @@ static void del_index(struct timeval *tv static void command_pkt(struct timeval *tv, uint16_t index, const void *data, uint16_t size) { - const struct bt_hci_cmd_hdr *hdr = data; struct hci_dev *dev; - data += sizeof(*hdr); - size -= sizeof(*hdr); - dev = dev_lookup(index); if (!dev) return; @@ -392,9 +391,6 @@ static void evt_conn_complete(struct hci const struct bt_hci_evt_conn_complete *evt = data; struct hci_conn *conn; - data += sizeof(*evt); - size -= sizeof(*evt); - if (evt->status) return; @@ -412,9 +408,6 @@ static void evt_disconnect_complete(stru const struct bt_hci_evt_disconnect_complete *evt = data; struct hci_conn *conn; - data += sizeof(*evt); - size -= sizeof(*evt); - if (evt->status) return; @@ -519,13 +512,6 @@ static void evt_num_completed_packets(st static void evt_le_meta_event(struct hci_dev *dev, struct timeval *tv, const void *data, uint16_t size) { - uint8_t subtype = get_u8(data); - - data += sizeof(subtype); - size -= sizeof(subtype); - - switch (subtype) { - } } static void event_pkt(struct timeval *tv, uint16_t index, @@ -620,12 +606,8 @@ static void acl_pkt(struct timeval *tv, static void sco_pkt(struct timeval *tv, uint16_t index, const void *data, uint16_t size) { - const struct bt_hci_sco_hdr *hdr = data; struct hci_dev *dev; - data += sizeof(*hdr); - size -= sizeof(*hdr); - dev = dev_lookup(index); if (!dev) return; @@ -640,9 +622,6 @@ static void info_index(struct timeval *t const struct btsnoop_opcode_index_info *hdr = data; struct hci_dev *dev; - data += sizeof(*hdr); - size -= sizeof(*hdr); - dev = dev_lookup(index); if (!dev) return; @@ -701,12 +680,8 @@ static void ctrl_msg(struct timeval *tv, static void iso_pkt(struct timeval *tv, uint16_t index, const void *data, uint16_t size) { - const struct bt_hci_iso_hdr *hdr = data; struct hci_dev *dev; - data += sizeof(*hdr); - size -= sizeof(*hdr); - dev = dev_lookup(index); if (!dev) return; diff -pruN 5.65-1/monitor/att.c 5.66-1/monitor/att.c --- 5.65-1/monitor/att.c 2022-07-24 20:59:52.000000000 +0000 +++ 5.66-1/monitor/att.c 2022-11-10 20:22:09.000000000 +0000 @@ -14,6 +14,7 @@ #endif #define _GNU_SOURCE +#include #include #include #include @@ -21,6 +22,9 @@ #include #include #include +#include + +#include #include "lib/bluetooth.h" #include "lib/uuid.h" @@ -39,6 +43,21 @@ #include "l2cap.h" #include "att.h" +struct att_read { + struct gatt_db_attribute *attr; + bool in; + uint16_t chan; + void (*func)(const struct l2cap_frame *frame); +}; + +struct att_conn_data { + struct gatt_db *ldb; + struct timespec ldb_mtim; + struct gatt_db *rdb; + struct timespec rdb_mtim; + struct queue *reads; +}; + static void print_uuid(const char *label, const void *data, uint16_t size) { const char *str; @@ -73,27 +92,66 @@ static void print_handle_range(const cha get_le16(data), get_le16(data + 2)); } +static bool match_read_frame(const void *data, const void *match_data) +{ + const struct att_read *read = data; + const struct l2cap_frame *frame = match_data; + + /* Read frame and response frame shall be in the opposite direction to + * match. + */ + if (read->in == frame->in) + return false; + + return read->chan == frame->chan; +} + static void print_data_list(const char *label, uint8_t length, - const void *data, uint16_t size) + const struct l2cap_frame *frame) { + struct packet_conn_data *conn; + struct att_conn_data *data; + struct att_read *read; uint8_t count; if (length == 0) return; - count = size / length; + conn = packet_get_conn_data(frame->handle); + if (conn) { + data = conn->data; + if (data) + read = queue_remove_if(data->reads, match_read_frame, + (void *)frame); + else + read = NULL; + } else + read = NULL; + + count = frame->size / length; print_field("%s: %u entr%s", label, count, count == 1 ? "y" : "ies"); - while (size >= length) { - print_field("Handle: 0x%4.4x", get_le16(data)); - print_hex_field("Value", data + 2, length - 2); + while (frame->size >= length) { + if (!l2cap_frame_print_le16((void *)frame, "Handle")) + break; - data += length; - size -= length; + print_hex_field("Value", frame->data, length - 2); + + if (read) { + struct l2cap_frame f; + + l2cap_frame_clone_size(&f, frame, length - 2); + + read->func(&f); + } + + if (!l2cap_frame_pull((void *)frame, frame, length - 2)) + break; } - packet_hexdump(data, size); + packet_hexdump(frame->data, frame->size); + free(read); } static void print_attribute_info(uint16_t type, const void *data, uint16_t len) @@ -215,6 +273,46 @@ static void att_error_response(const str print_field("Error: %s (0x%2.2x)", str, pdu->error); } +static const struct bitfield_data chrc_prop_table[] = { + { 0, "Broadcast (0x01)" }, + { 1, "Read (0x02)" }, + { 2, "Write Without Response (0x04)" }, + { 3, "Write (0x08)" }, + { 4, "Notify (0x10)" }, + { 5, "Indicate (0x20)" }, + { 6, "Authorize (0x40)" }, + { 6, "Extended Properties (0x80)" }, + { } +}; + +static void print_chrc(const struct l2cap_frame *frame) +{ + uint8_t prop; + uint8_t mask; + + if (!l2cap_frame_get_u8((void *)frame, &prop)) { + print_text(COLOR_ERROR, "Property: invalid size"); + return; + } + + print_field(" Properties: 0x%2.2x", prop); + + mask = print_bitfield(6, prop, chrc_prop_table); + if (mask) + print_text(COLOR_WHITE_BG, " Unknown fields (0x%2.2x)", + mask); + + if (!l2cap_frame_print_le16((void *)frame, " Value Handle")) + return; + + print_uuid(" Value UUID", frame->data, frame->size); +} + +static void chrc_read(const struct l2cap_frame *frame) +{ + print_chrc(frame); +} + static const struct bitfield_data ccc_value_table[] = { { 0, "Notification (0x01)" }, { 1, "Indication (0x02)" }, @@ -1590,215 +1688,945 @@ static void pac_context_notify(const str print_pac_context(frame); } -#define GATT_HANDLER(_uuid, _read, _write, _notify) \ -{ \ - .uuid = { \ - .type = BT_UUID16, \ - .value.u16 = _uuid, \ - }, \ - .read = _read, \ - .write = _write, \ - .notify = _notify \ -} +static void print_vcs_state(const struct l2cap_frame *frame) +{ + uint8_t vol_set, mute, chng_ctr; -struct gatt_handler { - bt_uuid_t uuid; - void (*read)(const struct l2cap_frame *frame); - void (*write)(const struct l2cap_frame *frame); - void (*notify)(const struct l2cap_frame *frame); -} gatt_handlers[] = { - GATT_HANDLER(0x2902, ccc_read, ccc_write, NULL), - GATT_HANDLER(0x2bc4, ase_read, NULL, ase_notify), - GATT_HANDLER(0x2bc5, ase_read, NULL, ase_notify), - GATT_HANDLER(0x2bc6, NULL, ase_cp_write, ase_cp_notify), - GATT_HANDLER(0x2bc9, pac_read, NULL, pac_notify), - GATT_HANDLER(0x2bca, pac_loc_read, NULL, pac_loc_notify), - GATT_HANDLER(0x2bcb, pac_read, NULL, pac_notify), - GATT_HANDLER(0x2bcc, pac_loc_read, NULL, pac_loc_notify), - GATT_HANDLER(0x2bcd, pac_context_read, NULL, pac_context_notify), - GATT_HANDLER(0x2bce, pac_context_read, NULL, pac_context_notify), -}; + if (!l2cap_frame_get_u8((void *)frame, &vol_set)) { + print_text(COLOR_ERROR, "Volume Settings: invalid size"); + goto done; + } + print_field(" Volume Setting: %u", vol_set); -static struct gatt_handler *get_handler(struct gatt_db_attribute *attr) -{ - const bt_uuid_t *uuid = gatt_db_attribute_get_type(attr); - size_t i; + if (!l2cap_frame_get_u8((void *)frame, &mute)) { + print_text(COLOR_ERROR, "Mute Filed: invalid size"); + goto done; + } - for (i = 0; i < ARRAY_SIZE(gatt_handlers); i++) { - struct gatt_handler *handler = &gatt_handlers[i]; + switch (mute) { + case 0x00: + print_field(" Not Muted: %u", mute); + break; + case 0x01: + print_field(" Muted: %u", mute); + break; + default: + print_field(" Unknown Mute Value: %u", mute); + break; + } - if (!bt_uuid_cmp(&handler->uuid, uuid)) - return handler; + if (!l2cap_frame_get_u8((void *)frame, &chng_ctr)) { + print_text(COLOR_ERROR, "Change Counter: invalid size"); + goto done; } + print_field(" Change Counter: %u", chng_ctr); - return NULL; +done: + if (frame->size) + print_hex_field(" Data", frame->data, frame->size); } -static void att_exchange_mtu_req(const struct l2cap_frame *frame) +static void vol_state_read(const struct l2cap_frame *frame) { - const struct bt_l2cap_att_exchange_mtu_req *pdu = frame->data; + print_vcs_state(frame); +} - print_field("Client RX MTU: %d", le16_to_cpu(pdu->mtu)); +static void vol_state_notify(const struct l2cap_frame *frame) +{ + print_vcs_state(frame); } -static void att_exchange_mtu_rsp(const struct l2cap_frame *frame) +static bool vcs_config_cmd(const struct l2cap_frame *frame) { - const struct bt_l2cap_att_exchange_mtu_rsp *pdu = frame->data; + if (!l2cap_frame_print_u8((void *)frame, " Change Counter")) + return false; - print_field("Server RX MTU: %d", le16_to_cpu(pdu->mtu)); + return true; } -static void att_find_info_req(const struct l2cap_frame *frame) +static bool vcs_absolute_cmd(const struct l2cap_frame *frame) { - print_handle_range("Handle range", frame->data); + if (!l2cap_frame_print_u8((void *)frame, " Change Counter")) + return false; + + if (!l2cap_frame_print_u8((void *)frame, " Volume Setting")) + return false; + + return true; } -static const char *att_format_str(uint8_t format) -{ - switch (format) { - case 0x01: - return "UUID-16"; - case 0x02: - return "UUID-128"; - default: - return "unknown"; - } +#define VCS_CMD(_op, _desc, _func) \ +[_op] = { \ + .desc = _desc, \ + .func = _func, \ } -static uint16_t print_info_data_16(const void *data, uint16_t len) +struct vcs_cmd { + const char *desc; + bool (*func)(const struct l2cap_frame *frame); +} vcs_cmd_table[] = { + /* Opcode = 0x00 (Relative Volume Down) */ + VCS_CMD(0x00, "Relative Volume Down", vcs_config_cmd), + /* Opcode = 0x01 (Relative Volume Up) */ + VCS_CMD(0x01, "Relative Volume Up", vcs_config_cmd), + /* Opcode = 0x02 (Unmute/Relative Volume Down) */ + VCS_CMD(0x02, "Unmute/Relative Volume Down", vcs_config_cmd), + /* Opcode = 0x03 (Unmute/Relative Volume Up) */ + VCS_CMD(0x03, "Unmute/Relative Volume Up", vcs_config_cmd), + /* Opcode = 0x04 (Set Absolute Volume) */ + VCS_CMD(0x04, "Set Absolute Volume", vcs_absolute_cmd), + /* Opcode = 0x05 (Unmute) */ + VCS_CMD(0x05, "Unmute", vcs_config_cmd), + /* Opcode = 0x06 (Mute) */ + VCS_CMD(0x06, "Mute", vcs_config_cmd), +}; + +static struct vcs_cmd *vcs_get_cmd(uint8_t op) { - while (len >= 4) { - print_field("Handle: 0x%4.4x", get_le16(data)); - print_uuid("UUID", data + 2, 2); - data += 4; - len -= 4; - } + if (op > ARRAY_SIZE(vcs_cmd_table)) + return NULL; - return len; + return &vcs_cmd_table[op]; } -static uint16_t print_info_data_128(const void *data, uint16_t len) +static void print_vcs_cmd(const struct l2cap_frame *frame) { - while (len >= 18) { - print_field("Handle: 0x%4.4x", get_le16(data)); - print_uuid("UUID", data + 2, 16); - data += 18; - len -= 18; + uint8_t op; + struct vcs_cmd *cmd; + + if (!l2cap_frame_get_u8((void *)frame, &op)) { + print_text(COLOR_ERROR, "opcode: invalid size"); + goto done; } - return len; + cmd = vcs_get_cmd(op); + if (!cmd) { + print_field(" Opcode: Reserved (0x%2.2x)", op); + goto done; + } + + print_field(" Opcode: %s (0x%2.2x)", cmd->desc, op); + if (!cmd->func(frame)) + print_field(" Unknown Opcode"); + +done: + if (frame->size) + print_hex_field(" Data", frame->data, frame->size); } -static void att_find_info_rsp(const struct l2cap_frame *frame) +static void vol_cp_write(const struct l2cap_frame *frame) { - const uint8_t *format = frame->data; - uint16_t len; + print_vcs_cmd(frame); +} - print_field("Format: %s (0x%2.2x)", att_format_str(*format), *format); +static void print_vcs_flag(const struct l2cap_frame *frame) +{ + uint8_t vol_flag; - if (*format == 0x01) - len = print_info_data_16(frame->data + 1, frame->size - 1); - else if (*format == 0x02) - len = print_info_data_128(frame->data + 1, frame->size - 1); - else - len = frame->size - 1; + if (!l2cap_frame_get_u8((void *)frame, &vol_flag)) { + print_text(COLOR_ERROR, "Volume Flag: invalid size"); + goto done; + } + print_field(" Volume Falg: %u", vol_flag); - packet_hexdump(frame->data + (frame->size - len), len); +done: + if (frame->size) + print_hex_field(" Data", frame->data, frame->size); } -static void att_find_by_type_val_req(const struct l2cap_frame *frame) +static void vol_flag_read(const struct l2cap_frame *frame) { - uint16_t type; - - print_handle_range("Handle range", frame->data); + print_vcs_flag(frame); +} - type = get_le16(frame->data + 4); - print_attribute_info(type, frame->data + 6, frame->size - 6); +static void vol_flag_notify(const struct l2cap_frame *frame) +{ + print_vcs_flag(frame); } -static void att_find_by_type_val_rsp(const struct l2cap_frame *frame) +static char *name2utf8(const uint8_t *name, uint16_t len) { - const uint8_t *ptr = frame->data; - uint16_t len = frame->size; + char utf8_name[HCI_MAX_NAME_LENGTH + 2]; + int i; - while (len >= 4) { - print_handle_range("Handle range", ptr); - ptr += 4; - len -= 4; + if (g_utf8_validate((const char *) name, len, NULL)) + return g_strndup((char *) name, len); + + len = MIN(len, sizeof(utf8_name) - 1); + + memset(utf8_name, 0, sizeof(utf8_name)); + strncpy(utf8_name, (char *) name, len); + + /* Assume ASCII, and replace all non-ASCII with spaces */ + for (i = 0; utf8_name[i] != '\0'; i++) { + if (!isascii(utf8_name[i])) + utf8_name[i] = ' '; } - packet_hexdump(ptr, len); -} + /* Remove leading and trailing whitespace characters */ + g_strstrip(utf8_name); -static void att_read_type_req(const struct l2cap_frame *frame) -{ - print_handle_range("Handle range", frame->data); - print_uuid("Attribute type", frame->data + 4, frame->size - 4); + return g_strdup(utf8_name); } -static void att_read_type_rsp(const struct l2cap_frame *frame) +static void print_mp_name(const struct l2cap_frame *frame) { - const struct bt_l2cap_att_read_group_type_rsp *pdu = frame->data; + char *name; - print_field("Attribute data length: %d", pdu->length); - print_data_list("Attribute data list", pdu->length, - frame->data + 1, frame->size - 1); -} + name = name2utf8((uint8_t *)frame->data, frame->size); -struct att_read { - struct gatt_db_attribute *attr; - bool in; - uint16_t chan; - void (*func)(const struct l2cap_frame *frame); -}; + print_field(" Media Player Name: %s", name); +} -struct att_conn_data { - struct gatt_db *ldb; - struct gatt_db *rdb; - struct queue *reads; -}; +static void mp_name_read(const struct l2cap_frame *frame) +{ + print_mp_name(frame); +} -static void att_conn_data_free(void *data) +static void mp_name_notify(const struct l2cap_frame *frame) { - struct att_conn_data *att_data = data; + print_mp_name(frame); +} - gatt_db_unref(att_data->rdb); - gatt_db_unref(att_data->ldb); - queue_destroy(att_data->reads, free); - free(att_data); +static void print_track_changed(const struct l2cap_frame *frame) +{ + print_field(" Track Changed"); } -static void load_gatt_db(struct packet_conn_data *conn) +static void track_changed_notify(const struct l2cap_frame *frame) +{ + print_track_changed(frame); +} + +static void print_track_title(const struct l2cap_frame *frame) +{ + char *name; + + name = name2utf8((uint8_t *)frame->data, frame->size); + + print_field(" Track Title: %s", name); +} + +static void track_title_read(const struct l2cap_frame *frame) +{ + print_track_title(frame); +} + +static void track_title_notify(const struct l2cap_frame *frame) +{ + print_track_title(frame); +} + +static void print_track_duration(const struct l2cap_frame *frame) +{ + int32_t duration; + + if (!l2cap_frame_get_le32((void *)frame, (uint32_t *)&duration)) { + print_text(COLOR_ERROR, " Track Duration: invalid size"); + goto done; + } + + print_field(" Track Duration: %u", duration); + +done: + if (frame->size) + print_hex_field(" Data", frame->data, frame->size); +} + +static void track_duration_read(const struct l2cap_frame *frame) +{ + print_track_duration(frame); +} + +static void track_duration_notify(const struct l2cap_frame *frame) +{ + print_track_duration(frame); +} + +static void print_track_position(const struct l2cap_frame *frame) +{ + int32_t position; + + if (!l2cap_frame_get_le32((void *)frame, (uint32_t *)&position)) { + print_text(COLOR_ERROR, " Track Position: invalid size"); + goto done; + } + + print_field(" Track Position: %u", position); + +done: + if (frame->size) + print_hex_field(" Data", frame->data, frame->size); +} + +static void track_position_read(const struct l2cap_frame *frame) +{ + print_track_position(frame); +} + +static void track_position_write(const struct l2cap_frame *frame) +{ + print_track_position(frame); +} + +static void track_position_notify(const struct l2cap_frame *frame) +{ + print_track_position(frame); +} + +static void print_playback_speed(const struct l2cap_frame *frame) +{ + int8_t playback_speed; + + if (!l2cap_frame_get_u8((void *)frame, (uint8_t *)&playback_speed)) { + print_text(COLOR_ERROR, " Playback Speed: invalid size"); + goto done; + } + + print_field(" Playback Speed: %u", playback_speed); + +done: + if (frame->size) + print_hex_field(" Data", frame->data, frame->size); +} + +static void playback_speed_read(const struct l2cap_frame *frame) +{ + print_playback_speed(frame); +} + +static void playback_speed_write(const struct l2cap_frame *frame) +{ + print_playback_speed(frame); +} + +static void playback_speed_notify(const struct l2cap_frame *frame) +{ + print_playback_speed(frame); +} + +static void print_seeking_speed(const struct l2cap_frame *frame) +{ + int8_t seeking_speed; + + if (!l2cap_frame_get_u8((void *)frame, (uint8_t *)&seeking_speed)) { + print_text(COLOR_ERROR, " Seeking Speed: invalid size"); + goto done; + } + + print_field(" Seeking Speed: %u", seeking_speed); + +done: + if (frame->size) + print_hex_field(" Data", frame->data, frame->size); +} + +static void seeking_speed_read(const struct l2cap_frame *frame) +{ + print_seeking_speed(frame); +} + +static void seeking_speed_notify(const struct l2cap_frame *frame) +{ + print_seeking_speed(frame); +} + +static const char *play_order_str(uint8_t order) +{ + switch (order) { + case 0x01: + return "Single once"; + case 0x02: + return "Single repeat"; + case 0x03: + return "In order once"; + case 0x04: + return "In order repeat"; + case 0x05: + return "Oldest once"; + case 0x06: + return "Oldest repeat"; + case 0x07: + return "Newest once"; + case 0x08: + return "Newest repeat"; + case 0x09: + return "Shuffle once"; + case 0x0A: + return "Shuffle repeat"; + default: + return "RFU"; + } +} + +static void print_playing_order(const struct l2cap_frame *frame) +{ + int8_t playing_order; + + if (!l2cap_frame_get_u8((void *)frame, (uint8_t *)&playing_order)) { + print_text(COLOR_ERROR, " Playing Order: invalid size"); + goto done; + } + + print_field(" Playing Order: %s", play_order_str(playing_order)); + +done: + if (frame->size) + print_hex_field(" Data", frame->data, frame->size); +} + +static void playing_order_read(const struct l2cap_frame *frame) +{ + print_playing_order(frame); +} + +static void playing_order_write(const struct l2cap_frame *frame) +{ + print_playing_order(frame); +} + +static void playing_order_notify(const struct l2cap_frame *frame) +{ + print_playing_order(frame); +} + +static const struct bitfield_data playing_orders_table[] = { + { 0, "Single once (0x0001)" }, + { 1, "Single repeat (0x0002)" }, + { 2, "In order once (0x0004)" }, + { 3, "In Order Repeat (0x0008)" }, + { 4, "Oldest once (0x0010)" }, + { 5, "Oldest repeat (0x0020)" }, + { 6, "Newest once (0x0040)" }, + { 7, "Newest repeat (0x0080)" }, + { 8, "Shuffle once (0x0100)" }, + { 9, "Shuffle repeat (0x0200)" }, + { 10, "RFU (0x0400)" }, + { 11, "RFU (0x0800)" }, + { 12, "RFU (0x1000)" }, + { 13, "RFU (0x2000)" }, + { 14, "RFU (0x4000)" }, + { 15, "RFU (0x8000)" }, + { } +}; + +static void print_playing_orders_supported(const struct l2cap_frame *frame) +{ + uint16_t supported_orders; + uint16_t mask; + + if (!l2cap_frame_get_le16((void *)frame, &supported_orders)) { + print_text(COLOR_ERROR, + " Supported Playing Orders: invalid size"); + goto done; + } + + print_field(" Supported Playing Orders: 0x%4.4x", + supported_orders); + + mask = print_bitfield(8, supported_orders, playing_orders_table); + if (mask) + print_text(COLOR_WHITE_BG, " Unknown fields (0x%4.4x)", + mask); + +done: + if (frame->size) + print_hex_field(" Data", frame->data, frame->size); +} + +static void playing_orders_supported_read(const struct l2cap_frame *frame) +{ + print_playing_orders_supported(frame); +} + +static const char *media_state_str(uint8_t state) +{ + switch (state) { + case 0x00: + return "Inactive"; + case 0x01: + return "Playing"; + case 0x02: + return "Paused"; + case 0x03: + return "Seeking"; + default: + return "RFU"; + } +} + +static void print_media_state(const struct l2cap_frame *frame) +{ + int8_t state; + + if (!l2cap_frame_get_u8((void *)frame, (uint8_t *)&state)) { + print_text(COLOR_ERROR, " Media State: invalid size"); + goto done; + } + + print_field(" Media State: %s", media_state_str(state)); + +done: + if (frame->size) + print_hex_field(" Data", frame->data, frame->size); +} + +static void media_state_read(const struct l2cap_frame *frame) +{ + print_media_state(frame); +} + +static void media_state_notify(const struct l2cap_frame *frame) +{ + print_media_state(frame); +} + +struct media_cp_opcode { + uint8_t opcode; + const char *opcode_str; +} media_cp_opcode_table[] = { + {0x01, "Play"}, + {0x02, "Pause"}, + {0x03, "Fast Rewind"}, + {0x04, "Fast Forward"}, + {0x05, "Stop"}, + {0x10, "Move Relative"}, + {0x20, "Previous Segment"}, + {0x21, "Next Segment"}, + {0x22, "First Segment"}, + {0x23, "Last Segment"}, + {0x24, "Goto Segment"}, + {0x30, "Previous Track"}, + {0x31, "Next Track"}, + {0x32, "First Track"}, + {0x33, "Last Track"}, + {0x34, "Goto Track"}, + {0x40, "Previous Group"}, + {0x41, "Next Group"}, + {0x42, "First Group"}, + {0x43, "Last Group"}, + {0x44, "Goto Group"}, +}; + +static const char *cp_opcode_str(uint8_t opcode) +{ + size_t i; + + for (i = 0; i < ARRAY_SIZE(media_cp_opcode_table); i++) { + const char *str = media_cp_opcode_table[i].opcode_str; + + if (opcode == media_cp_opcode_table[i].opcode) + return str; + } + + return "RFU"; +} + +static void print_media_cp(const struct l2cap_frame *frame) +{ + int8_t opcode; + + if (!l2cap_frame_get_u8((void *)frame, (uint8_t *)&opcode)) { + print_text(COLOR_ERROR, " Media Control Point: invalid size"); + goto done; + } + + print_field(" Media Control Point: %s", cp_opcode_str(opcode)); + +done: + if (frame->size) + print_hex_field(" Data", frame->data, frame->size); +} + +static void media_cp_write(const struct l2cap_frame *frame) +{ + print_media_cp(frame); +} + +static void media_cp_notify(const struct l2cap_frame *frame) +{ + print_media_cp(frame); +} + +static const struct bitfield_data supported_opcodes_table[] = { + {0, "Play (0x00000001)" }, + {1, "Pause (0x00000002)" }, + {2, "Fast Rewind (0x00000004)" }, + {3, "Fast Forward (0x00000008)" }, + {4, "Stop (0x00000010)" }, + {5, "Move Relative (0x00000020)" }, + {6, "Previous Segment (0x00000040)" }, + {7, "Next Segment (0x00000080)" }, + {8, "First Segment (0x00000100)" }, + {9, "Last Segment (0x00000200)" }, + {10, "Goto Segment (0x00000400)" }, + {11, "Previous Track (0x00000800)" }, + {12, "Next Track (0x00001000)" }, + {13, "First Track (0x00002000)" }, + {14, "Last Track (0x00004000)" }, + {15, "Goto Track (0x00008000)" }, + {16, "Previous Group (0x00010000)" }, + {17, "Next Group (0x00020000)" }, + {18, "First Group (0x00040000)" }, + {19, "Last Group (0x00080000)" }, + {20, "Goto Group (0x00100000)" }, + {21, "RFU (0x00200000)" }, + {22, "RFU (0x00400000)" }, + {23, "RFU (0x00800000)" }, + {24, "RFU (0x01000000)" }, + {25, "RFU (0x02000000)" }, + {26, "RFU (0x04000000)" }, + {27, "RFU (0x08000000)" }, + {28, "RFU (0x10000000)" }, + {29, "RFU (0x20000000)" }, + {30, "RFU (0x40000000)" }, + {31, "RFU (0x80000000)" }, + { } +}; + +static void print_media_cp_op_supported(const struct l2cap_frame *frame) +{ + uint32_t supported_opcodes; + uint32_t mask; + + if (!l2cap_frame_get_le32((void *)frame, &supported_opcodes)) { + print_text(COLOR_ERROR, " value: invalid size"); + goto done; + } + + print_field(" Supported Opcodes: 0x%8.8x", supported_opcodes); + + mask = print_bitfield(8, supported_opcodes, supported_opcodes_table); + if (mask) + print_text(COLOR_WHITE_BG, " Unknown fields (0x%4.4x)", + mask); + +done: + if (frame->size) + print_hex_field(" Data", frame->data, frame->size); +} + +static void media_cp_op_supported_read(const struct l2cap_frame *frame) +{ + print_media_cp_op_supported(frame); +} + +static void media_cp_op_supported_notify(const struct l2cap_frame *frame) +{ + print_media_cp_op_supported(frame); +} + +static void print_content_control_id(const struct l2cap_frame *frame) +{ + int8_t ccid; + + if (!l2cap_frame_get_u8((void *)frame, (uint8_t *)&ccid)) { + print_text(COLOR_ERROR, " Content Control ID: invalid size"); + goto done; + } + + print_field(" Content Control ID: 0x%2.2x", ccid); + +done: + if (frame->size) + print_hex_field(" Data", frame->data, frame->size); +} + +static void content_control_id_read(const struct l2cap_frame *frame) +{ + print_content_control_id(frame); +} + +#define GATT_HANDLER(_uuid, _read, _write, _notify) \ +{ \ + .uuid = { \ + .type = BT_UUID16, \ + .value.u16 = _uuid, \ + }, \ + .read = _read, \ + .write = _write, \ + .notify = _notify \ +} + +struct gatt_handler { + bt_uuid_t uuid; + void (*read)(const struct l2cap_frame *frame); + void (*write)(const struct l2cap_frame *frame); + void (*notify)(const struct l2cap_frame *frame); +} gatt_handlers[] = { + GATT_HANDLER(0x2803, chrc_read, NULL, NULL), + GATT_HANDLER(0x2902, ccc_read, ccc_write, NULL), + GATT_HANDLER(0x2bc4, ase_read, NULL, ase_notify), + GATT_HANDLER(0x2bc5, ase_read, NULL, ase_notify), + GATT_HANDLER(0x2bc6, NULL, ase_cp_write, ase_cp_notify), + GATT_HANDLER(0x2bc9, pac_read, NULL, pac_notify), + GATT_HANDLER(0x2bca, pac_loc_read, NULL, pac_loc_notify), + GATT_HANDLER(0x2bcb, pac_read, NULL, pac_notify), + GATT_HANDLER(0x2bcc, pac_loc_read, NULL, pac_loc_notify), + GATT_HANDLER(0x2bcd, pac_context_read, NULL, pac_context_notify), + GATT_HANDLER(0x2bce, pac_context_read, NULL, pac_context_notify), + GATT_HANDLER(0x2b7d, vol_state_read, NULL, vol_state_notify), + GATT_HANDLER(0x2b7e, NULL, vol_cp_write, NULL), + GATT_HANDLER(0x2b7f, vol_flag_read, NULL, vol_flag_notify), + GATT_HANDLER(0x2b93, mp_name_read, NULL, mp_name_notify), + GATT_HANDLER(0x2b96, NULL, NULL, track_changed_notify), + GATT_HANDLER(0x2b97, track_title_read, NULL, track_title_notify), + GATT_HANDLER(0x2b98, track_duration_read, NULL, track_duration_notify), + GATT_HANDLER(0x2b99, track_position_read, track_position_write, + track_position_notify), + GATT_HANDLER(0x2b9a, playback_speed_read, playback_speed_write, + playback_speed_notify), + GATT_HANDLER(0x2b9b, seeking_speed_read, NULL, seeking_speed_notify), + GATT_HANDLER(0x2ba1, playing_order_read, playing_order_write, + playing_order_notify), + GATT_HANDLER(0x2ba2, playing_orders_supported_read, NULL, NULL), + GATT_HANDLER(0x2ba3, media_state_read, NULL, media_state_notify), + GATT_HANDLER(0x2ba4, NULL, media_cp_write, media_cp_notify), + GATT_HANDLER(0x2ba5, media_cp_op_supported_read, NULL, + media_cp_op_supported_notify), + GATT_HANDLER(0x2bba, content_control_id_read, NULL, NULL), +}; + +static struct gatt_handler *get_handler_uuid(const bt_uuid_t *uuid) +{ + size_t i; + + for (i = 0; i < ARRAY_SIZE(gatt_handlers); i++) { + struct gatt_handler *handler = &gatt_handlers[i]; + + if (!bt_uuid_cmp(&handler->uuid, uuid)) + return handler; + } + + return NULL; +} + +static struct gatt_handler *get_handler(struct gatt_db_attribute *attr) +{ + return get_handler_uuid(gatt_db_attribute_get_type(attr)); +} + +static void att_exchange_mtu_req(const struct l2cap_frame *frame) +{ + const struct bt_l2cap_att_exchange_mtu_req *pdu = frame->data; + + print_field("Client RX MTU: %d", le16_to_cpu(pdu->mtu)); +} + +static void att_exchange_mtu_rsp(const struct l2cap_frame *frame) +{ + const struct bt_l2cap_att_exchange_mtu_rsp *pdu = frame->data; + + print_field("Server RX MTU: %d", le16_to_cpu(pdu->mtu)); +} + +static void att_find_info_req(const struct l2cap_frame *frame) +{ + print_handle_range("Handle range", frame->data); +} + +static const char *att_format_str(uint8_t format) +{ + switch (format) { + case 0x01: + return "UUID-16"; + case 0x02: + return "UUID-128"; + default: + return "unknown"; + } +} + +static uint16_t print_info_data_16(const void *data, uint16_t len) +{ + while (len >= 4) { + print_field("Handle: 0x%4.4x", get_le16(data)); + print_uuid("UUID", data + 2, 2); + data += 4; + len -= 4; + } + + return len; +} + +static uint16_t print_info_data_128(const void *data, uint16_t len) +{ + while (len >= 18) { + print_field("Handle: 0x%4.4x", get_le16(data)); + print_uuid("UUID", data + 2, 16); + data += 18; + len -= 18; + } + + return len; +} + +static void att_find_info_rsp(const struct l2cap_frame *frame) +{ + const uint8_t *format = frame->data; + uint16_t len; + + print_field("Format: %s (0x%2.2x)", att_format_str(*format), *format); + + if (*format == 0x01) + len = print_info_data_16(frame->data + 1, frame->size - 1); + else if (*format == 0x02) + len = print_info_data_128(frame->data + 1, frame->size - 1); + else + len = frame->size - 1; + + packet_hexdump(frame->data + (frame->size - len), len); +} + +static void att_find_by_type_val_req(const struct l2cap_frame *frame) +{ + uint16_t type; + + print_handle_range("Handle range", frame->data); + + type = get_le16(frame->data + 4); + print_attribute_info(type, frame->data + 6, frame->size - 6); +} + +static void att_find_by_type_val_rsp(const struct l2cap_frame *frame) +{ + const uint8_t *ptr = frame->data; + uint16_t len = frame->size; + + while (len >= 4) { + print_handle_range("Handle range", ptr); + ptr += 4; + len -= 4; + } + + packet_hexdump(ptr, len); +} + +static int bt_uuid_from_data(bt_uuid_t *uuid, const void *data, uint16_t size) +{ + uint128_t u128; + + switch (size) { + case 2: + return bt_uuid16_create(uuid, get_le16(data)); + case 4: + return bt_uuid32_create(uuid, get_le32(data)); + case 16: + memcpy(u128.data, data, sizeof(u128.data)); + return bt_uuid128_create(uuid, u128); + } + + return -EINVAL; +} + +static void att_conn_data_free(void *data) +{ + struct att_conn_data *att_data = data; + + gatt_db_unref(att_data->rdb); + gatt_db_unref(att_data->ldb); + queue_destroy(att_data->reads, free); + free(att_data); +} + +static struct att_conn_data *att_get_conn_data(struct packet_conn_data *conn) { struct att_conn_data *data = conn->data; - char filename[PATH_MAX]; - char local[18]; - char peer[18]; - if (!data) { - data = new0(struct att_conn_data, 1); - data->rdb = gatt_db_new(); - data->ldb = gatt_db_new(); - conn->data = data; - conn->destroy = att_conn_data_free; + if (data) + return data; + + data = new0(struct att_conn_data, 1); + data->rdb = gatt_db_new(); + data->ldb = gatt_db_new(); + conn->data = data; + conn->destroy = att_conn_data_free; + + return data; +} + +static void att_read_type_req(const struct l2cap_frame *frame) +{ + bt_uuid_t uuid; + struct packet_conn_data *conn; + struct att_conn_data *data; + struct att_read *read; + struct gatt_handler *handler; + + print_handle_range("Handle range", frame->data); + print_uuid("Attribute type", frame->data + 4, frame->size - 4); + + if (bt_uuid_from_data(&uuid, frame->data + 4, frame->size - 4)) + return; + + handler = get_handler_uuid(&uuid); + if (!handler || !handler->read) + return; + + conn = packet_get_conn_data(frame->handle); + data = att_get_conn_data(conn); + + if (!data->reads) + data->reads = queue_new(); + + read = new0(struct att_read, 1); + read->in = frame->in; + read->chan = frame->chan; + read->func = handler->read; + + queue_push_tail(data->reads, read); +} + +static void att_read_type_rsp(const struct l2cap_frame *frame) +{ + uint8_t len; + + if (!l2cap_frame_get_u8((void *)frame, &len)) { + print_text(COLOR_ERROR, "invalid size"); + return; } - if (!gatt_db_isempty(data->ldb) && !gatt_db_isempty(data->rdb)) + print_field("Attribute data length: %d", len); + print_data_list("Attribute data list", len, frame); +} + +static void gatt_load_db(struct gatt_db *db, const char *filename, + struct timespec *mtim) +{ + struct stat st; + + if (lstat(filename, &st)) return; + if (!gatt_db_isempty(db)) { + /* Check if file has been modified since last time */ + if (st.st_mtim.tv_sec == mtim->tv_sec && + st.st_mtim.tv_nsec == mtim->tv_nsec) + return; + /* Clear db before reloading */ + gatt_db_clear(db); + } + + *mtim = st.st_mtim; + + btd_settings_gatt_db_load(db, filename); +} + +static void load_gatt_db(struct packet_conn_data *conn) +{ + struct att_conn_data *data = att_get_conn_data(conn); + char filename[PATH_MAX]; + char local[18]; + char peer[18]; + ba2str((bdaddr_t *)conn->src, local); ba2str((bdaddr_t *)conn->dst, peer); - if (gatt_db_isempty(data->ldb)) { - create_filename(filename, PATH_MAX, "/%s/attributes", local); - btd_settings_gatt_db_load(data->ldb, filename); - } + create_filename(filename, PATH_MAX, "/%s/attributes", local); + gatt_load_db(data->ldb, filename, &data->ldb_mtim); - if (gatt_db_isempty(data->rdb)) { - create_filename(filename, PATH_MAX, "/%s/cache/%s", local, - peer); - btd_settings_gatt_db_load(data->rdb, filename); - } + create_filename(filename, PATH_MAX, "/%s/cache/%s", local, peer); + gatt_load_db(data->rdb, filename, &data->rdb_mtim); } static struct gatt_db_attribute *get_attribute(const struct l2cap_frame *frame, @@ -1916,20 +2744,6 @@ static void att_read_req(const struct l2 queue_push_tail(data->reads, read); } -static bool match_read_frame(const void *data, const void *match_data) -{ - const struct att_read *read = data; - const struct l2cap_frame *frame = match_data; - - /* Read frame and response frame shall be in the opposite direction to - * match. - */ - if (read->in == frame->in) - return false; - - return read->chan == frame->chan; -} - static void att_read_rsp(const struct l2cap_frame *frame) { struct packet_conn_data *conn; @@ -2096,7 +2910,7 @@ static void print_notify(const struct l2 struct gatt_handler *handler; struct l2cap_frame clone; - print_handle(frame, handle, false); + print_handle(frame, handle, true); print_hex_field(" Data", frame->data, len); if (len > frame->size) { diff -pruN 5.65-1/monitor/ellisys.c 5.66-1/monitor/ellisys.c --- 5.65-1/monitor/ellisys.c 2022-07-24 20:59:52.000000000 +0000 +++ 5.66-1/monitor/ellisys.c 2022-11-10 20:22:09.000000000 +0000 @@ -131,6 +131,12 @@ void ellisys_inject_hci(struct timeval * case BTSNOOP_OPCODE_SCO_RX_PKT: msg[20] = 0x83; break; + case BTSNOOP_OPCODE_ISO_TX_PKT: + msg[20] = 0x05; + break; + case BTSNOOP_OPCODE_ISO_RX_PKT: + msg[20] = 0x85; + break; default: return; } diff -pruN 5.65-1/monitor/l2cap.h 5.66-1/monitor/l2cap.h --- 5.65-1/monitor/l2cap.h 2022-07-24 20:59:52.000000000 +0000 +++ 5.66-1/monitor/l2cap.h 2022-11-10 20:22:09.000000000 +0000 @@ -31,8 +31,9 @@ void l2cap_frame_init(struct l2cap_frame uint16_t cid, uint16_t psm, const void *data, uint16_t size); -static inline void l2cap_frame_clone(struct l2cap_frame *frame, - const struct l2cap_frame *source) +static inline void l2cap_frame_clone_size(struct l2cap_frame *frame, + const struct l2cap_frame *source, + uint16_t size) { if (frame != source) { frame->index = source->index; @@ -44,10 +45,16 @@ static inline void l2cap_frame_clone(str frame->chan = source->chan; frame->mode = source->mode; frame->data = source->data; - frame->size = source->size; + frame->size = size; } } +static inline void l2cap_frame_clone(struct l2cap_frame *frame, + const struct l2cap_frame *source) +{ + l2cap_frame_clone_size(frame, source, source->size); +} + static inline void *l2cap_frame_pull(struct l2cap_frame *frame, const struct l2cap_frame *source, uint16_t len) { diff -pruN 5.65-1/monitor/packet.c 5.66-1/monitor/packet.c --- 5.65-1/monitor/packet.c 2022-07-24 20:59:52.000000000 +0000 +++ 5.66-1/monitor/packet.c 2022-11-10 20:22:09.000000000 +0000 @@ -384,8 +384,9 @@ static void print_packet(struct timeval } if (filter_mask & PACKET_FILTER_SHOW_TIME) { - n = sprintf(ts_str + ts_pos, " %02d:%02d:%02d.%06lu", - tm.tm_hour, tm.tm_min, tm.tm_sec, tv->tv_usec); + n = sprintf(ts_str + ts_pos, " %02d:%02d:%02d.%06llu", + tm.tm_hour, tm.tm_min, tm.tm_sec, + (long long)tv->tv_usec); if (n > 0) { ts_pos += n; ts_len += n; @@ -393,8 +394,9 @@ static void print_packet(struct timeval } if (filter_mask & PACKET_FILTER_SHOW_TIME_OFFSET) { - n = sprintf(ts_str + ts_pos, " %lu.%06lu", - tv->tv_sec - time_offset, tv->tv_usec); + n = sprintf(ts_str + ts_pos, " %llu.%06llu", + (long long)(tv->tv_sec - time_offset), + (long long)tv->tv_usec); if (n > 0) { ts_pos += n; ts_len += n; @@ -403,12 +405,7 @@ static void print_packet(struct timeval } if (use_color()) { - n = sprintf(ts_str + ts_pos, "%s", COLOR_OFF); - if (n > 0) - ts_pos += n; - } - - if (use_color()) { + sprintf(ts_str + ts_pos, "%s", COLOR_OFF); n = sprintf(line + pos, "%s", color); if (n > 0) pos += n; @@ -449,10 +446,8 @@ static void print_packet(struct timeval if (extra) { n = sprintf(line + pos, " %s", extra); - if (n > 0) { - pos += n; + if (n > 0) len += n; - } } if (ts_len > 0) { @@ -2390,6 +2385,12 @@ void packet_print_version(const char *la case 0x0a: str = "Bluetooth 5.1"; break; + case 0x0b: + str = "Bluetooth 5.2"; + break; + case 0x0c: + str = "Bluetooth 5.3"; + break; default: str = "Reserved"; break; @@ -2663,6 +2664,13 @@ static const struct bitfield_data featur { 30, "Isochronous Broadcaster" }, { 31, "Synchronized Receiver" }, { 32, "Isochronous Channels (Host Support)" }, + { 33, "LE Power Control Request" }, + { 34, "LE Power Control Request" }, + { 35, "LE Path Loss Monitoring" }, + { 36, "Periodic Advertising ADI support" }, + { 37, "Connection Subrating" }, + { 38, "Connection Subrating (Host Support)" }, + { 39, "Channel Classification" }, { } }; @@ -7472,7 +7480,6 @@ static void print_le_phys_preference(uin " (0x%2.2x)", mask); print_field("TX PHYs preference: 0x%2.2x", tx_phys); - mask = tx_phys; mask = print_bitfield(2, tx_phys, le_phys); if (mask) @@ -7480,7 +7487,6 @@ static void print_le_phys_preference(uin " (0x%2.2x)", mask); print_field("RX PHYs preference: 0x%2.2x", rx_phys); - mask = rx_phys; mask = print_bitfield(2, rx_phys, le_phys); if (mask) @@ -9828,7 +9834,7 @@ static const char *get_supported_command return NULL; } -static const char *current_vendor_str(void) +static const char *current_vendor_str(uint16_t ocf) { uint16_t manufacturer, msft_opcode; @@ -9840,7 +9846,8 @@ static const char *current_vendor_str(vo msft_opcode = BT_HCI_CMD_NOP; } - if (msft_opcode != BT_HCI_CMD_NOP) + if (msft_opcode != BT_HCI_CMD_NOP && + cmd_opcode_ocf(msft_opcode) == ocf) return "Microsoft"; switch (manufacturer) { @@ -9884,22 +9891,16 @@ static const struct vendor_ocf *current_ static const struct vendor_evt *current_vendor_evt(const void *data, int *consumed_size) { - uint16_t manufacturer, msft_opcode; + uint16_t manufacturer; uint8_t evt = *((const uint8_t *) data); /* A regular vendor event consumes 1 byte. */ *consumed_size = 1; - if (index_current < MAX_INDEX) { + if (index_current < MAX_INDEX) manufacturer = index_list[index_current].manufacturer; - msft_opcode = index_list[index_current].msft_opcode; - } else { + else manufacturer = fallback_manufacturer; - msft_opcode = BT_HCI_CMD_NOP; - } - - if (msft_opcode != BT_HCI_CMD_NOP) - return NULL; switch (manufacturer) { case 2: @@ -9911,6 +9912,27 @@ static const struct vendor_evt *current_ return NULL; } +static const char *current_vendor_evt_str(void) +{ + uint16_t manufacturer; + + if (index_current < MAX_INDEX) + manufacturer = index_list[index_current].manufacturer; + else + manufacturer = fallback_manufacturer; + + switch (manufacturer) { + case 2: + return "Intel"; + case 15: + return "Broadcom"; + case 93: + return "Realtek"; + } + + return NULL; +} + static void inquiry_complete_evt(uint16_t index, const void *data, uint8_t size) { const struct bt_hci_evt_inquiry_complete *evt = data; @@ -10091,7 +10113,7 @@ static void cmd_complete_evt(uint16_t in const struct vendor_ocf *vnd = current_vendor_ocf(ocf); if (vnd) { - const char *str = current_vendor_str(); + const char *str = current_vendor_str(ocf); if (str) { snprintf(vendor_str, sizeof(vendor_str), @@ -10183,7 +10205,7 @@ static void cmd_status_evt(uint16_t inde const struct vendor_ocf *vnd = current_vendor_ocf(ocf); if (vnd) { - const char *str = current_vendor_str(); + const char *str = current_vendor_str(ocf); if (str) { snprintf(vendor_str, sizeof(vendor_str), @@ -11237,10 +11259,11 @@ static void le_pa_report_evt(uint16_t in break; default: str = "Reserved"; - color_on = COLOR_RED; break; } + print_field("CTE Type: %s (0x%2x)", str, evt->cte_type); + switch (evt->data_status) { case 0x00: str = "Complete"; @@ -11624,7 +11647,7 @@ static void vendor_evt(uint16_t index, c const struct vendor_evt *vnd = current_vendor_evt(data, &consumed_size); if (vnd) { - const char *str = current_vendor_str(); + const char *str = current_vendor_evt_str(); if (str) { snprintf(vendor_str, sizeof(vendor_str), @@ -12026,7 +12049,7 @@ void packet_hci_command(struct timeval * const struct vendor_ocf *vnd = current_vendor_ocf(ocf); if (vnd) { - const char *str = current_vendor_str(); + const char *str = current_vendor_str(ocf); if (str) { snprintf(vendor_str, sizeof(vendor_str), @@ -14191,6 +14214,74 @@ static void mgmt_remove_adv_monitor_patt print_field("Handle: %d", handle); } +static void mgmt_set_mesh_receiver_cmd(const void *data, uint16_t size) +{ + uint8_t enable = get_u8(data); + uint16_t window = get_le16(data + 1); + uint16_t period = get_le16(data + 3); + uint8_t num_ad_types = get_u8(data + 5); + const uint8_t *ad_types = data + 6; + + print_field("Enable: %d", enable); + print_field("Window: %d", window); + print_field("Period: %d", period); + print_field("Num AD Types: %d", num_ad_types); + size -= 6; + + while (size--) + print_field(" AD Type: %d", *ad_types++); +} + +static void mgmt_read_mesh_features_rsp(const void *data, uint16_t size) +{ + uint16_t index = get_le16(data); + uint8_t max_handles = get_u8(data + 2); + uint8_t used_handles = get_u8(data + 3); + const uint8_t *handles = data + 4; + + print_field("Index: %d", index); + print_field("Max Handles: %d", max_handles); + print_field("Used Handles: %d", used_handles); + size -= 4; + + while (size--) + print_field(" Used Handle: %d", *handles++); +} + +static void mgmt_mesh_send_cmd(const void *data, uint16_t size) +{ + const uint8_t *addr = data; + uint8_t addr_type = get_u8(data + 6); + uint64_t instant = get_le64(data + 7); + uint16_t delay = get_le16(data + 15); + uint8_t cnt = get_u8(data + 17); + uint8_t adv_data_len = get_u8(data + 18); + + data += 19; + size -= 19; + print_bdaddr(addr); + print_field("Addr Type: %d", addr_type); + print_field("Instant: 0x%16.16" PRIx64, instant); + print_field("Delay: %d", delay); + print_field("Count: %d", cnt); + print_field("Data Length: %d", adv_data_len); + print_hex_field("Data", data, size); +} + +static void mgmt_mesh_send_rsp(const void *data, uint16_t size) +{ + uint8_t handle = get_u8(data); + + print_field("Handle: %d", handle); +} + +static void mgmt_mesh_send_cancel_cmd(const void *data, uint16_t size) +{ + uint8_t handle = get_u8(data); + + print_field("Handle: %d", handle); +} + struct mgmt_data { uint16_t opcode; const char *str; @@ -14448,6 +14539,18 @@ static const struct mgmt_data mgmt_comma mgmt_add_adv_monitor_patterns_rssi_cmd, 8, false, mgmt_add_adv_monitor_patterns_rsp, 2, true}, + { 0x0057, "Set Mesh Receiver", + mgmt_set_mesh_receiver_cmd, 6, false, + mgmt_null_rsp, 0, true}, + { 0x0058, "Read Mesh Features", + mgmt_null_cmd, 0, true, + mgmt_read_mesh_features_rsp, 4, false}, + { 0x0059, "Mesh Send", + mgmt_mesh_send_cmd, 19, false, + mgmt_mesh_send_rsp, 1, true}, + { 0x0056, "Mesh Send Cancel", + mgmt_mesh_send_cancel_cmd, 1, true, + mgmt_null_rsp, 0, true}, { } }; @@ -14914,6 +15017,64 @@ static void mgmt_controller_resume_evt(c mgmt_print_address(data, addr_type); } +static void mgmt_adv_monitor_device_found_evt(const void *data, uint16_t size) +{ + uint16_t handle = get_le16(data); + const uint8_t *addr = data + 2; + uint8_t addr_type = get_u8(data + 8); + int8_t rssi = get_s8(data + 9); + uint32_t flags = get_le32(data + 10); + uint16_t ad_data_len = get_le16(data + 14); + const uint8_t *ad_data = data + 16; + + print_field("Handle: %d", handle); + print_bdaddr(addr); + print_field("Addr Type: %d", addr_type); + print_field("RSSI: %d", rssi); + mgmt_print_device_flags(flags); + print_field("AD Data Len: %d", ad_data_len); + size -= 16; + print_hex_field("AD Data", ad_data, size); +} + +static void mgmt_adv_monitor_device_lost_evt(const void *data, uint16_t size) +{ + uint16_t handle = get_le16(data); + const uint8_t *addr = data + 2; + uint8_t addr_type = get_u8(data + 8); + + print_field("Handle: %d", handle); + print_bdaddr(addr); + print_field("Addr Type: %d", addr_type); +} + +static void mgmt_mesh_device_found_evt(const void *data, uint16_t size) +{ + const uint8_t *addr = data; + uint8_t addr_type = get_u8(data + 6); + int8_t rssi = get_s8(data + 7); + uint64_t instant = get_le64(data + 8); + uint32_t flags = get_le32(data + 16); + uint16_t eir_len = get_le16(data + 20); + const uint8_t *eir_data = data + 22; + + print_bdaddr(addr); + print_field("Addr Type: %d", addr_type); + print_field("RSSI: %d", rssi); + print_field("Instant: 0x%16.16" PRIx64, instant); + mgmt_print_device_flags(flags); + print_field("EIR Length: %d", eir_len); + size -= 22; + print_hex_field("EIR Data", eir_data, size); +} + +static void mgmt_mesh_packet_cmplt_evt(const void *data, uint16_t size) +{ + uint8_t handle = get_u8(data); + + print_field("Handle: %d", handle); +} + static const struct mgmt_data mgmt_event_table[] = { { 0x0001, "Command Complete", mgmt_command_complete_evt, 3, false }, @@ -15003,6 +15164,14 @@ static const struct mgmt_data mgmt_event mgmt_controller_suspend_evt, 1, true }, { 0x002e, "Controller Resumed", mgmt_controller_resume_evt, 8, true }, + { 0x002f, "ADV Monitor Device Found", + mgmt_adv_monitor_device_found_evt, 16, false }, + { 0x0030, "ADV Monitor Device Lost", + mgmt_adv_monitor_device_lost_evt, 9, true }, + { 0x0031, "Mesh Device Found", + mgmt_mesh_device_found_evt, 22, false }, + { 0x0032, "Mesh Packet Complete", + mgmt_mesh_packet_cmplt_evt, 1, true }, { } }; diff -pruN 5.65-1/plugins/sixaxis.c 5.66-1/plugins/sixaxis.c --- 5.65-1/plugins/sixaxis.c 2022-07-24 20:59:52.000000000 +0000 +++ 5.66-1/plugins/sixaxis.c 2022-11-10 20:22:09.000000000 +0000 @@ -294,7 +294,6 @@ static void agent_auth_cb(DBusError *der } remove_device = false; - btd_device_set_trusted(closure->device, true); btd_device_set_temporary(closure->device, false); if (closure->type == CABLE_PAIRING_SIXAXIS) @@ -336,10 +335,9 @@ static bool setup_device(int fd, const c * connected eg. to charge up battery. */ device = btd_adapter_find_device(adapter, &device_bdaddr, BDADDR_BREDR); - if (device != NULL && - btd_device_is_connected(device) && - g_slist_find_custom(btd_device_get_uuids(device), HID_UUID, - (GCompareFunc)strcasecmp)) { + if (device && btd_device_has_uuid(device, HID_UUID) && + (btd_device_is_connected(device) || + btd_device_is_trusted(device))) { char device_addr[18]; ba2str(&device_bdaddr, device_addr); DBG("device %s already known, skipping", device_addr); @@ -352,7 +350,6 @@ static bool setup_device(int fd, const c btd_device_device_set_name(device, cp->name); btd_device_set_pnpid(device, cp->source, cp->vid, cp->pid, cp->version); - btd_device_set_trusted(device, false); btd_device_set_temporary(device, true); closure = g_new0(struct authentication_closure, 1); diff -pruN 5.65-1/profiles/audio/a2dp.c 5.66-1/profiles/audio/a2dp.c --- 5.65-1/profiles/audio/a2dp.c 2022-07-24 20:59:52.000000000 +0000 +++ 5.66-1/profiles/audio/a2dp.c 2022-11-10 20:22:09.000000000 +0000 @@ -2522,7 +2522,8 @@ static void confirm_cb(GIOChannel *io, g if (!setup || !setup->stream) goto drop; - if (setup->io) { + if (setup->io || avdtp_stream_get_transport(setup->stream, + NULL, NULL, NULL, NULL)) { error("transport channel already exists"); goto drop; } diff -pruN 5.65-1/profiles/audio/bap.c 5.66-1/profiles/audio/bap.c --- 5.65-1/profiles/audio/bap.c 1970-01-01 00:00:00.000000000 +0000 +++ 5.66-1/profiles/audio/bap.c 2022-11-10 20:22:09.000000000 +0000 @@ -0,0 +1,1326 @@ +// SPDX-License-Identifier: GPL-2.0-or-later +/* + * + * BlueZ - Bluetooth protocol stack for Linux + * + * Copyright (C) 2022 Intel Corporation. All rights reserved. + * + * + */ + +#ifdef HAVE_CONFIG_H +#include +#endif + +#define _GNU_SOURCE + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include "gdbus/gdbus.h" + +#include "lib/bluetooth.h" +#include "lib/hci.h" +#include "lib/sdp.h" +#include "lib/uuid.h" + +#include "src/dbus-common.h" +#include "src/shared/util.h" +#include "src/shared/att.h" +#include "src/shared/queue.h" +#include "src/shared/gatt-db.h" +#include "src/shared/gatt-client.h" +#include "src/shared/gatt-server.h" +#include "src/shared/bap.h" + +#include "btio/btio.h" +#include "src/plugin.h" +#include "src/adapter.h" +#include "src/gatt-database.h" +#include "src/device.h" +#include "src/profile.h" +#include "src/service.h" +#include "src/log.h" +#include "src/error.h" + +#define PACS_UUID_STR "00001850-0000-1000-8000-00805f9b34fb" +#define MEDIA_ENDPOINT_INTERFACE "org.bluez.MediaEndpoint1" + +struct bap_ep { + char *path; + struct bap_data *data; + struct bt_bap_pac *lpac; + struct bt_bap_pac *rpac; + struct bt_bap_stream *stream; + GIOChannel *io; + unsigned int io_id; + bool recreate; + struct iovec *caps; + struct iovec *metadata; + struct bt_bap_qos qos; + unsigned int id; + DBusMessage *msg; +}; + +struct bap_data { + struct btd_device *device; + struct btd_service *service; + struct bt_bap *bap; + unsigned int ready_id; + unsigned int state_id; + unsigned int pac_id; + struct queue *srcs; + struct queue *snks; + struct queue *streams; + GIOChannel *listen_io; +}; + +static struct queue *sessions; + +static void bap_debug(const char *str, void *user_data) +{ + DBG_IDX(0xffff, "%s", str); +} + +static void ep_unregister(void *data) +{ + struct bap_ep *ep = data; + + DBG("ep %p path %s", ep, ep->path); + + g_dbus_unregister_interface(btd_get_dbus_connection(), ep->path, + MEDIA_ENDPOINT_INTERFACE); +} + +static void bap_data_free(struct bap_data *data) +{ + if (data->listen_io) { + g_io_channel_shutdown(data->listen_io, TRUE, NULL); + g_io_channel_unref(data->listen_io); + } + + if (data->service) { + btd_service_set_user_data(data->service, NULL); + bt_bap_set_user_data(data->bap, NULL); + } + + queue_destroy(data->snks, ep_unregister); + queue_destroy(data->srcs, ep_unregister); + queue_destroy(data->streams, NULL); + bt_bap_ready_unregister(data->bap, data->ready_id); + bt_bap_state_unregister(data->bap, data->state_id); + bt_bap_pac_unregister(data->pac_id); + bt_bap_unref(data->bap); + free(data); +} + +static void bap_data_remove(struct bap_data *data) +{ + DBG("data %p", data); + + if (!queue_remove(sessions, data)) + return; + + bap_data_free(data); + + if (queue_isempty(sessions)) { + queue_destroy(sessions, NULL); + sessions = NULL; + } +} + +static void bap_remove(struct btd_service *service) +{ + struct btd_device *device = btd_service_get_device(service); + struct bap_data *data; + char addr[18]; + + ba2str(device_get_address(device), addr); + DBG("%s", addr); + + data = btd_service_get_user_data(service); + if (!data) { + error("BAP service not handled by profile"); + return; + } + + bap_data_remove(data); +} + +static gboolean get_uuid(const GDBusPropertyTable *property, + DBusMessageIter *iter, void *data) +{ + struct bap_ep *ep = data; + const char *uuid; + + if (queue_find(ep->data->snks, NULL, ep)) + uuid = PAC_SINK_UUID; + else + uuid = PAC_SOURCE_UUID; + + dbus_message_iter_append_basic(iter, DBUS_TYPE_STRING, &uuid); + + return TRUE; +} + +static gboolean get_codec(const GDBusPropertyTable *property, + DBusMessageIter *iter, void *data) +{ + struct bap_ep *ep = data; + uint8_t codec; + + bt_bap_pac_get_codec(ep->rpac, &codec, NULL, NULL); + + dbus_message_iter_append_basic(iter, DBUS_TYPE_BYTE, &codec); + + return TRUE; +} + +static gboolean get_capabilities(const GDBusPropertyTable *property, + DBusMessageIter *iter, void *data) +{ + struct bap_ep *ep = data; + DBusMessageIter array; + struct iovec *d; + + bt_bap_pac_get_codec(ep->rpac, NULL, &d, NULL); + + dbus_message_iter_open_container(iter, DBUS_TYPE_ARRAY, + DBUS_TYPE_BYTE_AS_STRING, &array); + + dbus_message_iter_append_fixed_array(&array, DBUS_TYPE_BYTE, + &d->iov_base, d->iov_len); + + dbus_message_iter_close_container(iter, &array); + + return TRUE; +} + +static gboolean get_device(const GDBusPropertyTable *property, + DBusMessageIter *iter, void *data) +{ + struct bap_ep *ep = data; + const char *path; + + path = device_get_path(ep->data->device); + + dbus_message_iter_append_basic(iter, DBUS_TYPE_OBJECT_PATH, &path); + + return TRUE; +} + +static const GDBusPropertyTable ep_properties[] = { + { "UUID", "s", get_uuid, NULL, NULL, + G_DBUS_PROPERTY_FLAG_EXPERIMENTAL }, + { "Codec", "y", get_codec, NULL, NULL, + G_DBUS_PROPERTY_FLAG_EXPERIMENTAL }, + { "Capabilities", "ay", get_capabilities, NULL, NULL, + G_DBUS_PROPERTY_FLAG_EXPERIMENTAL }, + { "Device", "o", get_device, NULL, NULL, + G_DBUS_PROPERTY_FLAG_EXPERIMENTAL }, + { } +}; + +static int parse_array(DBusMessageIter *iter, struct iovec **iov) +{ + DBusMessageIter array; + + if (!iov) + return 0; + + if (!(*iov)) + *iov = new0(struct iovec, 1); + + dbus_message_iter_recurse(iter, &array); + dbus_message_iter_get_fixed_array(&array, &(*iov)->iov_base, + (int *)&(*iov)->iov_len); + return 0; +} + +static int parse_properties(DBusMessageIter *props, struct iovec **caps, + struct iovec **metadata, struct bt_bap_qos *qos) +{ + const char *key; + + while (dbus_message_iter_get_arg_type(props) == DBUS_TYPE_DICT_ENTRY) { + DBusMessageIter value, entry; + int var; + + dbus_message_iter_recurse(props, &entry); + dbus_message_iter_get_basic(&entry, &key); + + dbus_message_iter_next(&entry); + dbus_message_iter_recurse(&entry, &value); + + var = dbus_message_iter_get_arg_type(&value); + + if (!strcasecmp(key, "Capabilities")) { + if (var != DBUS_TYPE_ARRAY) + goto fail; + + if (parse_array(&value, caps)) + goto fail; + } else if (!strcasecmp(key, "Metadata")) { + if (var != DBUS_TYPE_ARRAY) + goto fail; + + if (parse_array(&value, metadata)) + goto fail; + } else if (!strcasecmp(key, "CIG")) { + if (var != DBUS_TYPE_BYTE) + goto fail; + + dbus_message_iter_get_basic(&value, &qos->cig_id); + } else if (!strcasecmp(key, "CIS")) { + if (var != DBUS_TYPE_BYTE) + goto fail; + + dbus_message_iter_get_basic(&value, &qos->cis_id); + } else if (!strcasecmp(key, "Interval")) { + if (var != DBUS_TYPE_UINT32) + goto fail; + + dbus_message_iter_get_basic(&value, &qos->interval); + } else if (!strcasecmp(key, "Framing")) { + dbus_bool_t val; + + if (var != DBUS_TYPE_BOOLEAN) + goto fail; + + dbus_message_iter_get_basic(&value, &val); + + qos->framing = val; + } else if (!strcasecmp(key, "PHY")) { + const char *str; + + if (var != DBUS_TYPE_STRING) + goto fail; + + dbus_message_iter_get_basic(&value, &str); + + if (!strcasecmp(str, "1M")) + qos->phy = 0x01; + else if (!strcasecmp(str, "2M")) + qos->phy = 0x02; + else + goto fail; + } else if (!strcasecmp(key, "SDU")) { + if (var != DBUS_TYPE_UINT16) + goto fail; + + dbus_message_iter_get_basic(&value, &qos->sdu); + } else if (!strcasecmp(key, "Retransmissions")) { + if (var != DBUS_TYPE_BYTE) + goto fail; + + dbus_message_iter_get_basic(&value, &qos->rtn); + } else if (!strcasecmp(key, "Latency")) { + if (var != DBUS_TYPE_UINT16) + goto fail; + + dbus_message_iter_get_basic(&value, &qos->latency); + } else if (!strcasecmp(key, "Delay")) { + if (var != DBUS_TYPE_UINT32) + goto fail; + + dbus_message_iter_get_basic(&value, &qos->delay); + } else if (!strcasecmp(key, "TargetLatency")) { + if (var != DBUS_TYPE_BYTE) + goto fail; + + dbus_message_iter_get_basic(&value, + &qos->target_latency); + } + + dbus_message_iter_next(props); + } + + return 0; + +fail: + DBG("Failed parsing %s", key); + + if (*caps) { + free(*caps); + *caps = NULL; + } + + return -EINVAL; +} + +static void qos_cb(struct bt_bap_stream *stream, uint8_t code, uint8_t reason, + void *user_data) +{ + struct bap_ep *ep = user_data; + DBusMessage *reply; + + DBG("stream %p code 0x%02x reason 0x%02x", stream, code, reason); + + if (!ep->msg) + return; + + if (!code) + reply = dbus_message_new_method_return(ep->msg); + else + reply = btd_error_failed(ep->msg, "Unable to configure"); + + g_dbus_send_message(btd_get_dbus_connection(), reply); + + dbus_message_unref(ep->msg); + ep->msg = NULL; +} + +static void config_cb(struct bt_bap_stream *stream, + uint8_t code, uint8_t reason, + void *user_data) +{ + struct bap_ep *ep = user_data; + DBusMessage *reply; + + DBG("stream %p code 0x%02x reason 0x%02x", stream, code, reason); + + ep->id = 0; + + if (!code) + return; + + if (!ep->msg) + return; + + reply = btd_error_failed(ep->msg, "Unable to configure"); + g_dbus_send_message(btd_get_dbus_connection(), reply); + + dbus_message_unref(ep->msg); + ep->msg = NULL; +} + +static void bap_io_close(struct bap_ep *ep) +{ + int fd; + + if (ep->io_id) { + g_source_remove(ep->io_id); + ep->io_id = 0; + } + + if (!ep->io) + return; + + + DBG("ep %p", ep); + + fd = g_io_channel_unix_get_fd(ep->io); + close(fd); + + g_io_channel_unref(ep->io); + ep->io = NULL; +} + +static DBusMessage *set_configuration(DBusConnection *conn, DBusMessage *msg, + void *data) +{ + struct bap_ep *ep = data; + const char *path; + DBusMessageIter args, props; + + if (ep->msg) + return btd_error_busy(msg); + + dbus_message_iter_init(msg, &args); + + dbus_message_iter_get_basic(&args, &path); + dbus_message_iter_next(&args); + + dbus_message_iter_recurse(&args, &props); + if (dbus_message_iter_get_arg_type(&props) != DBUS_TYPE_DICT_ENTRY) + return btd_error_invalid_args(msg); + + /* Disconnect IO if connecting since QoS is going to be reconfigured */ + if (bt_bap_stream_io_is_connecting(ep->stream, NULL)) { + bap_io_close(ep); + bt_bap_stream_io_connecting(ep->stream, -1); + } + + /* Mark CIG and CIS to be auto assigned */ + ep->qos.cig_id = BT_ISO_QOS_CIG_UNSET; + ep->qos.cis_id = BT_ISO_QOS_CIS_UNSET; + + if (parse_properties(&props, &ep->caps, &ep->metadata, &ep->qos) < 0) { + DBG("Unable to parse properties"); + return btd_error_invalid_args(msg); + } + + /* TODO: Check if stream capabilities match add support for Latency + * and PHY. + */ + if (ep->stream) + ep->id = bt_bap_stream_config(ep->stream, &ep->qos, ep->caps, + config_cb, ep); + else + ep->stream = bt_bap_config(ep->data->bap, ep->lpac, ep->rpac, + &ep->qos, ep->caps, + config_cb, ep); + + if (!ep->stream) { + DBG("Unable to config stream"); + free(ep->caps); + ep->caps = NULL; + return btd_error_invalid_args(msg); + } + + bt_bap_stream_set_user_data(ep->stream, ep->path); + ep->msg = dbus_message_ref(msg); + + return NULL; +} + +static const GDBusMethodTable ep_methods[] = { + { GDBUS_EXPERIMENTAL_ASYNC_METHOD("SetConfiguration", + GDBUS_ARGS({ "endpoint", "o" }, + { "properties", "a{sv}" } ), + NULL, set_configuration) }, + { }, +}; + +static void ep_free(void *data) +{ + struct bap_ep *ep = data; + + if (ep->id) + bt_bap_stream_cancel(ep->stream, ep->id); + + bap_io_close(ep); + + free(ep->caps); + free(ep->path); + free(ep); +} + +static struct bap_ep *ep_register(struct btd_service *service, + struct bt_bap_pac *lpac, + struct bt_bap_pac *rpac) +{ + struct btd_device *device = btd_service_get_device(service); + struct bap_data *data = btd_service_get_user_data(service); + struct bap_ep *ep; + struct queue *queue; + int i, err; + const char *suffix; + + switch (bt_bap_pac_get_type(rpac)) { + case BT_BAP_SINK: + queue = data->snks; + i = queue_length(data->snks); + suffix = "sink"; + break; + case BT_BAP_SOURCE: + queue = data->srcs; + i = queue_length(data->srcs); + suffix = "source"; + break; + default: + return NULL; + } + + ep = new0(struct bap_ep, 1); + ep->data = data; + ep->lpac = lpac; + ep->rpac = rpac; + + err = asprintf(&ep->path, "%s/pac_%s%d", device_get_path(device), + suffix, i); + if (err < 0) { + error("Could not allocate path for remote pac %s/pac%d", + device_get_path(device), i); + free(ep); + return NULL; + } + + if (g_dbus_register_interface(btd_get_dbus_connection(), + ep->path, MEDIA_ENDPOINT_INTERFACE, + ep_methods, NULL, ep_properties, + ep, ep_free) == FALSE) { + error("Could not register remote ep %s", ep->path); + ep_free(ep); + return NULL; + } + + bt_bap_pac_set_user_data(rpac, ep->path); + + DBG("ep %p lpac %p rpac %p path %s", ep, ep->lpac, ep->rpac, ep->path); + + queue_push_tail(queue, ep); + + return ep; +} + +static void select_cb(struct bt_bap_pac *pac, int err, struct iovec *caps, + struct iovec *metadata, struct bt_bap_qos *qos, + void *user_data) +{ + struct bap_ep *ep = user_data; + + if (err) { + error("err %d", err); + return; + } + + ep->caps = caps; + ep->metadata = metadata; + ep->qos = *qos; + + /* TODO: Check if stream capabilities match add support for Latency + * and PHY. + */ + if (ep->stream) + ep->id = bt_bap_stream_config(ep->stream, &ep->qos, ep->caps, + config_cb, ep); + else + ep->stream = bt_bap_config(ep->data->bap, ep->lpac, ep->rpac, + &ep->qos, ep->caps, + config_cb, ep); + + if (!ep->stream) { + DBG("Unable to config stream"); + free(ep->caps); + ep->caps = NULL; + } + + bt_bap_stream_set_user_data(ep->stream, ep->path); +} + +static bool pac_found(struct bt_bap_pac *lpac, struct bt_bap_pac *rpac, + void *user_data) +{ + struct btd_service *service = user_data; + struct bap_ep *ep; + + DBG("lpac %p rpac %p", lpac, rpac); + + ep = ep_register(service, lpac, rpac); + if (!ep) { + error("Unable to register endpoint for pac %p", rpac); + return true; + } + + /* TODO: Cache LRU? */ + if (btd_service_is_initiator(service)) + bt_bap_select(lpac, rpac, select_cb, ep); + + return true; +} + +static void bap_ready(struct bt_bap *bap, void *user_data) +{ + struct btd_service *service = user_data; + + DBG("bap %p", bap); + + bt_bap_foreach_pac(bap, BT_BAP_SOURCE, pac_found, service); + bt_bap_foreach_pac(bap, BT_BAP_SINK, pac_found, service); +} + +static bool match_ep_by_stream(const void *data, const void *user_data) +{ + const struct bap_ep *ep = data; + const struct bt_bap_stream *stream = user_data; + + return ep->stream == stream; +} + +static struct bap_ep *bap_find_ep_by_stream(struct bap_data *data, + struct bt_bap_stream *stream) +{ + struct bap_ep *ep; + + ep = queue_find(data->snks, match_ep_by_stream, stream); + if (ep) + return ep; + + return queue_find(data->srcs, match_ep_by_stream, stream); +} + +static void iso_connect_cb(GIOChannel *chan, GError *err, gpointer user_data) +{ + struct bt_bap_stream *stream = user_data; + int fd; + + if (err) { + error("%s", err->message); + bt_bap_stream_set_io(stream, -1); + return; + } + + DBG("ISO connected"); + + fd = g_io_channel_unix_get_fd(chan); + + if (bt_bap_stream_set_io(stream, fd)) { + g_io_channel_set_close_on_unref(chan, FALSE); + return; + } + + error("Unable to set IO"); + bt_bap_stream_set_io(stream, -1); +} + +static void bap_iso_qos(struct bt_bap_qos *qos, struct bt_iso_io_qos *io) +{ + if (!qos) + return; + + io->interval = qos->interval; + io->latency = qos->latency; + io->sdu = qos->sdu; + io->phy = qos->phy; + io->rtn = qos->rtn; +} + +static bool match_stream_qos(const void *data, const void *user_data) +{ + const struct bt_bap_stream *stream = data; + const struct bt_iso_qos *iso_qos = user_data; + struct bt_bap_qos *qos; + + qos = bt_bap_stream_get_qos((void *)stream); + + if (iso_qos->cig != qos->cig_id) + return false; + + return iso_qos->cis == qos->cis_id; +} + +static void iso_confirm_cb(GIOChannel *io, void *user_data) +{ + struct bap_data *data = user_data; + struct bt_bap_stream *stream; + struct bt_iso_qos qos; + char address[18]; + GError *err = NULL; + + bt_io_get(io, &err, + BT_IO_OPT_DEST, address, + BT_IO_OPT_QOS, &qos, + BT_IO_OPT_INVALID); + if (err) { + error("%s", err->message); + g_error_free(err); + goto drop; + } + + DBG("ISO: incoming connect from %s (CIG 0x%02x CIS 0x%02x)", + address, qos.cig, qos.cis); + + stream = queue_remove_if(data->streams, match_stream_qos, &qos); + if (!stream) { + error("No matching stream found"); + goto drop; + } + + if (!bt_io_accept(io, iso_connect_cb, stream, NULL, &err)) { + error("bt_io_accept: %s", err->message); + g_error_free(err); + goto drop; + } + + return; + +drop: + g_io_channel_shutdown(io, TRUE, NULL); +} + +static void bap_accept_io(struct bap_data *data, struct bt_bap_stream *stream, + int fd, int defer) +{ + char c; + struct pollfd pfd; + socklen_t len; + + if (fd < 0 || defer) + return; + + /* Check if socket has DEFER_SETUP set */ + len = sizeof(defer); + if (getsockopt(fd, SOL_BLUETOOTH, BT_DEFER_SETUP, &defer, &len) < 0) + /* Ignore errors since the fd may be connected already */ + return; + + if (!defer) + return; + + DBG("stream %p fd %d defer %s", stream, fd, defer ? "true" : "false"); + + memset(&pfd, 0, sizeof(pfd)); + pfd.fd = fd; + pfd.events = POLLOUT; + + if (poll(&pfd, 1, 0) < 0) { + error("poll: %s (%d)", strerror(errno), errno); + goto fail; + } + + if (!(pfd.revents & POLLOUT)) { + if (read(fd, &c, 1) < 0) { + error("read: %s (%d)", strerror(errno), errno); + goto fail; + } + } + + return; + +fail: + close(fd); +} + +static void bap_create_io(struct bap_data *data, struct bap_ep *ep, + struct bt_bap_stream *stream, int defer); + +static gboolean bap_io_recreate(void *user_data) +{ + struct bap_ep *ep = user_data; + + DBG("ep %p", ep); + + ep->io_id = 0; + + bap_create_io(ep->data, ep, ep->stream, true); + + return FALSE; +} + +static gboolean bap_io_disconnected(GIOChannel *io, GIOCondition cond, + gpointer user_data) +{ + struct bap_ep *ep = user_data; + + DBG("ep %p recreate %s", ep, ep->recreate ? "true" : "false"); + + ep->io_id = 0; + + bap_io_close(ep); + + /* Check if connecting recreate IO */ + if (ep->recreate) { + ep->recreate = false; + ep->io_id = g_idle_add(bap_io_recreate, ep); + } + + return FALSE; +} + +static void bap_connect_io_cb(GIOChannel *chan, GError *err, gpointer user_data) +{ + struct bap_ep *ep = user_data; + + if (!ep->stream) + return; + + iso_connect_cb(chan, err, ep->stream); +} + +static void bap_connect_io(struct bap_data *data, struct bap_ep *ep, + struct bt_bap_stream *stream, + struct bt_iso_qos *qos, int defer) +{ + struct btd_adapter *adapter = device_get_adapter(data->device); + GIOChannel *io; + GError *err = NULL; + int fd; + + /* If IO already set skip creating it again */ + if (bt_bap_stream_get_io(stream)) + return; + + if (bt_bap_stream_io_is_connecting(stream, &fd)) { + bap_accept_io(data, stream, fd, defer); + return; + } + + /* If IO channel still up wait for it to be disconnected and then + * recreate. + */ + if (ep->io) { + ep->recreate = true; + return; + } + + if (ep->io_id) { + g_source_remove(ep->io_id); + ep->io_id = 0; + } + + DBG("ep %p stream %p defer %s", ep, stream, defer ? "true" : "false"); + + io = bt_io_connect(bap_connect_io_cb, ep, NULL, &err, + BT_IO_OPT_SOURCE_BDADDR, + btd_adapter_get_address(adapter), + BT_IO_OPT_DEST_BDADDR, + device_get_address(ep->data->device), + BT_IO_OPT_DEST_TYPE, + device_get_le_address_type(ep->data->device), + BT_IO_OPT_MODE, BT_IO_MODE_ISO, + BT_IO_OPT_QOS, qos, + BT_IO_OPT_DEFER_TIMEOUT, defer, + BT_IO_OPT_INVALID); + if (!io) { + error("%s", err->message); + g_error_free(err); + return; + } + + ep->io_id = g_io_add_watch(io, G_IO_HUP | G_IO_ERR | G_IO_NVAL, + bap_io_disconnected, ep); + + ep->io = io; + + bt_bap_stream_io_connecting(stream, g_io_channel_unix_get_fd(io)); +} + +static void bap_listen_io(struct bap_data *data, struct bt_bap_stream *stream, + struct bt_iso_qos *qos) +{ + struct btd_adapter *adapter = device_get_adapter(data->device); + GIOChannel *io; + GError *err = NULL; + + DBG("stream %p", stream); + + /* If IO already set skip creating it again */ + if (bt_bap_stream_get_io(stream) || data->listen_io) + return; + + io = bt_io_listen(NULL, iso_confirm_cb, data, NULL, &err, + BT_IO_OPT_SOURCE_BDADDR, + btd_adapter_get_address(adapter), + BT_IO_OPT_DEST_BDADDR, + device_get_address(data->device), + BT_IO_OPT_DEST_TYPE, + device_get_le_address_type(data->device), + BT_IO_OPT_MODE, BT_IO_MODE_ISO, + BT_IO_OPT_QOS, qos, + BT_IO_OPT_INVALID); + if (!io) { + error("%s", err->message); + g_error_free(err); + return; + } + + data->listen_io = io; +} + +static void bap_create_io(struct bap_data *data, struct bap_ep *ep, + struct bt_bap_stream *stream, int defer) +{ + struct bt_bap_qos *qos[2] = {}; + struct bt_iso_qos iso_qos; + + DBG("ep %p stream %p defer %s", ep, stream, defer ? "true" : "false"); + + if (!data->streams) + data->streams = queue_new(); + + if (!queue_find(data->streams, NULL, stream)) + queue_push_tail(data->streams, stream); + + if (!bt_bap_stream_io_get_qos(stream, &qos[0], &qos[1])) { + error("bt_bap_stream_get_qos_links: failed"); + return; + } + + memset(&iso_qos, 0, sizeof(iso_qos)); + iso_qos.cig = qos[0] ? qos[0]->cig_id : qos[1]->cig_id; + iso_qos.cis = qos[0] ? qos[0]->cis_id : qos[1]->cis_id; + + bap_iso_qos(qos[0], &iso_qos.in); + bap_iso_qos(qos[1], &iso_qos.out); + + if (ep) + bap_connect_io(data, ep, stream, &iso_qos, defer); + else + bap_listen_io(data, stream, &iso_qos); +} + +static void bap_state(struct bt_bap_stream *stream, uint8_t old_state, + uint8_t new_state, void *user_data) +{ + struct bap_data *data = user_data; + struct bap_ep *ep; + + DBG("stream %p: %s(%u) -> %s(%u)", stream, + bt_bap_stream_statestr(old_state), old_state, + bt_bap_stream_statestr(new_state), new_state); + + if (new_state == old_state) + return; + + ep = bap_find_ep_by_stream(data, stream); + + switch (new_state) { + case BT_BAP_STREAM_STATE_IDLE: + /* Release stream if idle */ + if (ep) + bap_io_close(ep); + else + queue_remove(data->streams, stream); + break; + case BT_BAP_STREAM_STATE_CONFIG: + if (ep && !ep->id) { + bap_create_io(data, ep, stream, true); + if (!ep->io) { + error("Unable to create io"); + bt_bap_stream_release(stream, NULL, NULL); + return; + } + + + /* Wait QoS response to respond */ + ep->id = bt_bap_stream_qos(stream, &ep->qos, qos_cb, + ep); + if (!ep->id) { + error("Failed to Configure QoS"); + bt_bap_stream_release(stream, NULL, NULL); + } + } + break; + case BT_BAP_STREAM_STATE_QOS: + bap_create_io(data, ep, stream, true); + break; + case BT_BAP_STREAM_STATE_ENABLING: + if (ep) + bap_create_io(data, ep, stream, false); + break; + } +} + +static void pac_added(struct bt_bap_pac *pac, void *user_data) +{ + struct btd_service *service = user_data; + struct bap_data *data; + + DBG("pac %p", pac); + + if (btd_service_get_state(service) != BTD_SERVICE_STATE_CONNECTED) + return; + + data = btd_service_get_user_data(service); + + bt_bap_foreach_pac(data->bap, BT_BAP_SOURCE, pac_found, service); + bt_bap_foreach_pac(data->bap, BT_BAP_SINK, pac_found, service); +} + +static bool ep_match_rpac(const void *data, const void *match_data) +{ + const struct bap_ep *ep = data; + const struct bt_bap_pac *pac = match_data; + + return ep->rpac == pac; +} + +static void pac_removed(struct bt_bap_pac *pac, void *user_data) +{ + struct btd_service *service = user_data; + struct bap_data *data; + struct queue *queue; + struct bap_ep *ep; + + DBG("pac %p", pac); + + if (btd_service_get_state(service) != BTD_SERVICE_STATE_CONNECTED) + return; + + data = btd_service_get_user_data(service); + + switch (bt_bap_pac_get_type(pac)) { + case BT_BAP_SINK: + queue = data->srcs; + break; + case BT_BAP_SOURCE: + queue = data->snks; + break; + default: + return; + } + + ep = queue_remove_if(queue, ep_match_rpac, pac); + if (!ep) + return; + + ep_unregister(ep); +} + +static struct bap_data *bap_data_new(struct btd_device *device) +{ + struct bap_data *data; + + data = new0(struct bap_data, 1); + data->device = device; + data->srcs = queue_new(); + data->snks = queue_new(); + + return data; +} + +static void bap_data_add(struct bap_data *data) +{ + DBG("data %p", data); + + if (queue_find(sessions, NULL, data)) { + error("data %p already added", data); + return; + } + + bt_bap_set_debug(data->bap, bap_debug, NULL, NULL); + + if (!sessions) + sessions = queue_new(); + + queue_push_tail(sessions, data); + + if (data->service) + btd_service_set_user_data(data->service, data); +} + +static bool match_data(const void *data, const void *match_data) +{ + const struct bap_data *bdata = data; + const struct bt_bap *bap = match_data; + + return bdata->bap == bap; +} + +static void bap_connecting(struct bt_bap_stream *stream, bool state, int fd, + void *user_data) +{ + struct bap_data *data = user_data; + struct bap_ep *ep; + GIOChannel *io; + + if (!state) + return; + + ep = bap_find_ep_by_stream(data, stream); + if (!ep) + return; + + ep->recreate = false; + + if (!ep->io) { + io = g_io_channel_unix_new(fd); + ep->io = io; + } else + io = ep->io; + + g_io_channel_set_close_on_unref(io, FALSE); + + /* Attempt to get CIG/CIS if they have not been set */ + if (ep->qos.cig_id == BT_ISO_QOS_CIG_UNSET || + ep->qos.cis_id == BT_ISO_QOS_CIS_UNSET) { + struct bt_iso_qos qos; + GError *err = NULL; + + if (!bt_io_get(io, &err, BT_IO_OPT_QOS, &qos, + BT_IO_OPT_INVALID)) { + error("%s", err->message); + g_error_free(err); + g_io_channel_unref(io); + return; + } + + ep->qos.cig_id = qos.cig; + ep->qos.cis_id = qos.cis; + } + + DBG("stream %p fd %d: CIG 0x%02x CIS 0x%02x", stream, fd, + ep->qos.cig_id, ep->qos.cis_id); +} + +static void bap_attached(struct bt_bap *bap, void *user_data) +{ + struct bap_data *data; + struct bt_att *att; + struct btd_device *device; + + DBG("%p", bap); + + data = queue_find(sessions, match_data, bap); + if (data) + return; + + att = bt_bap_get_att(bap); + if (!att) + return; + + device = btd_adapter_find_device_by_fd(bt_att_get_fd(att)); + if (!device) { + error("Unable to find device"); + return; + } + + data = bap_data_new(device); + data->bap = bap; + + bap_data_add(data); + + data->state_id = bt_bap_state_register(data->bap, bap_state, + bap_connecting, data, NULL); +} + +static void bap_detached(struct bt_bap *bap, void *user_data) +{ + struct bap_data *data; + + DBG("%p", bap); + + data = queue_find(sessions, match_data, bap); + if (!data) { + error("Unable to find bap session"); + return; + } + + /* If there is a service it means there is PACS thus we can keep + * instance allocated. + */ + if (data->service) + return; + + bap_data_remove(data); +} + +static int bap_probe(struct btd_service *service) +{ + struct btd_device *device = btd_service_get_device(service); + struct btd_adapter *adapter = device_get_adapter(device); + struct btd_gatt_database *database = btd_adapter_get_database(adapter); + struct bap_data *data = btd_service_get_user_data(service); + char addr[18]; + + ba2str(device_get_address(device), addr); + DBG("%s", addr); + + if (!btd_adapter_has_exp_feature(adapter, EXP_FEAT_ISO_SOCKET)) { + error("BAP requires ISO Socket which is not enabled"); + return -ENOTSUP; + } + + /* Ignore, if we were probed for this device already */ + if (data) { + error("Profile probed twice for the same device!"); + return -EINVAL; + } + + data = bap_data_new(device); + data->service = service; + + data->bap = bt_bap_new(btd_gatt_database_get_db(database), + btd_device_get_gatt_db(device)); + if (!data->bap) { + error("Unable to create BAP instance"); + free(data); + return -EINVAL; + } + + bap_data_add(data); + + data->ready_id = bt_bap_ready_register(data->bap, bap_ready, service, + NULL); + data->state_id = bt_bap_state_register(data->bap, bap_state, + bap_connecting, data, NULL); + data->pac_id = bt_bap_pac_register(pac_added, pac_removed, service, + NULL); + + bt_bap_set_user_data(data->bap, service); + + return 0; +} + +static int bap_accept(struct btd_service *service) +{ + struct btd_device *device = btd_service_get_device(service); + struct bt_gatt_client *client = btd_device_get_gatt_client(device); + struct bap_data *data = btd_service_get_user_data(service); + char addr[18]; + + ba2str(device_get_address(device), addr); + DBG("%s", addr); + + if (!data) { + error("BAP service not handled by profile"); + return -EINVAL; + } + + if (!bt_bap_attach(data->bap, client)) { + error("BAP unable to attach"); + return -EINVAL; + } + + btd_service_connecting_complete(service, 0); + + return 0; +} + +static bool ep_remove(const void *data, const void *match_data) +{ + ep_unregister((void *)data); + + return true; +} + +static int bap_disconnect(struct btd_service *service) +{ + struct bap_data *data = btd_service_get_user_data(service); + + queue_remove_all(data->snks, ep_remove, NULL, NULL); + queue_remove_all(data->srcs, ep_remove, NULL, NULL); + + bt_bap_detach(data->bap); + + btd_service_disconnecting_complete(service, 0); + + return 0; +} + +static struct btd_profile bap_profile = { + .name = "bap", + .priority = BTD_PROFILE_PRIORITY_MEDIUM, + .remote_uuid = PACS_UUID_STR, + .device_probe = bap_probe, + .device_remove = bap_remove, + .accept = bap_accept, + .disconnect = bap_disconnect, +}; + +static unsigned int bap_id = 0; + +static int bap_init(void) +{ + if (!(g_dbus_get_flags() & G_DBUS_FLAG_ENABLE_EXPERIMENTAL)) { + warn("D-Bus experimental not enabled"); + return -ENOTSUP; + } + + btd_profile_register(&bap_profile); + bap_id = bt_bap_register(bap_attached, bap_detached, NULL); + + return 0; +} + +static void bap_exit(void) +{ + if (g_dbus_get_flags() & G_DBUS_FLAG_ENABLE_EXPERIMENTAL) { + btd_profile_unregister(&bap_profile); + bt_bap_unregister(bap_id); + } +} + +BLUETOOTH_PLUGIN_DEFINE(bap, VERSION, BLUETOOTH_PLUGIN_PRIORITY_DEFAULT, + bap_init, bap_exit) diff -pruN 5.65-1/profiles/audio/mcp.c 5.66-1/profiles/audio/mcp.c --- 5.65-1/profiles/audio/mcp.c 1970-01-01 00:00:00.000000000 +0000 +++ 5.66-1/profiles/audio/mcp.c 2022-11-10 20:22:09.000000000 +0000 @@ -0,0 +1,430 @@ +// SPDX-License-Identifier: GPL-2.0-or-later +/* + * + * BlueZ - Bluetooth protocol stack for Linux + * + * Copyright (C) 2020 Intel Corporation. All rights reserved. + * + * + */ +#ifdef HAVE_CONFIG_H +#include +#endif + +#define _GNU_SOURCE + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include "gdbus/gdbus.h" + +#include "lib/bluetooth.h" +#include "lib/hci.h" +#include "lib/sdp.h" +#include "lib/uuid.h" + +#include "src/dbus-common.h" +#include "src/shared/util.h" +#include "src/shared/att.h" +#include "src/shared/queue.h" +#include "src/shared/gatt-db.h" +#include "src/shared/gatt-client.h" +#include "src/shared/gatt-server.h" +#include "src/shared/mcp.h" +#include "src/shared/mcs.h" + +#include "btio/btio.h" +#include "src/plugin.h" +#include "src/adapter.h" +#include "src/gatt-database.h" +#include "src/device.h" +#include "src/profile.h" +#include "src/service.h" +#include "src/log.h" +#include "src/error.h" +#include "player.h" + +#define GMCS_UUID_STR "00001849-0000-1000-8000-00805f9b34fb" + +struct mcp_data { + struct btd_device *device; + struct btd_service *service; + struct bt_mcp *mcp; + unsigned int state_id; + + struct media_player *mp; +}; + +static void mcp_debug(const char *str, void *user_data) +{ + DBG_IDX(0xffff, "%s", str); +} + +static char *name2utf8(const uint8_t *name, uint16_t len) +{ + char utf8_name[HCI_MAX_NAME_LENGTH + 2]; + int i; + + if (g_utf8_validate((const char *) name, len, NULL)) + return g_strndup((char *) name, len); + + len = MIN(len, sizeof(utf8_name) - 1); + + memset(utf8_name, 0, sizeof(utf8_name)); + strncpy(utf8_name, (char *) name, len); + + /* Assume ASCII, and replace all non-ASCII with spaces */ + for (i = 0; utf8_name[i] != '\0'; i++) { + if (!isascii(utf8_name[i])) + utf8_name[i] = ' '; + } + + /* Remove leading and trailing whitespace characters */ + g_strstrip(utf8_name); + + return g_strdup(utf8_name); +} + +static const char *mcp_status_val_to_string(uint8_t status) +{ + switch (status) { + case BT_MCS_STATUS_PLAYING: + return "playing"; + case BT_MCS_STATUS_PAUSED: + return "paused"; + case BT_MCS_STATUS_INACTIVE: + return "stopped"; + case BT_MCS_STATUS_SEEKING: + /* TODO: find a way for fwd/rvs seeking, probably by storing + * control point operation sent before + */ + return "forward-seek"; + default: + return "error"; + } +} + +static struct mcp_data *mcp_data_new(struct btd_device *device) +{ + struct mcp_data *data; + + data = new0(struct mcp_data, 1); + data->device = device; + + return data; +} + +static void cb_player_name(struct bt_mcp *mcp, const uint8_t *value, + uint16_t length) +{ + char *name; + struct media_player *mp = bt_mcp_get_user_data(mcp); + + name = name2utf8(value, length); + DBG("Media Player Name %s", (const char *)name); + + media_player_set_name(mp, name); + + g_free(name); +} + +static void cb_track_changed(struct bt_mcp *mcp) +{ + DBG("Track Changed"); + /* Since track changed has happened + * track title notification is expected + */ +} + +static void cb_track_title(struct bt_mcp *mcp, const uint8_t *value, + uint16_t length) +{ + char *name; + uint16_t len; + struct media_player *mp = bt_mcp_get_user_data(mcp); + + name = name2utf8(value, length); + len = strlen(name); + + DBG("Track Title %s", (const char *)name); + + media_player_set_metadata(mp, NULL, "Title", name, len); + media_player_metadata_changed(mp); + + g_free(name); +} + +static void cb_track_duration(struct bt_mcp *mcp, int32_t duration) +{ + struct media_player *mp = bt_mcp_get_user_data(mcp); + unsigned char buf[10]; + + /* MCP defines duration is int32 but api takes it as uint32 */ + sprintf((char *)buf, "%d", duration); + media_player_set_metadata(mp, NULL, "Duration", buf, sizeof(buf)); + media_player_metadata_changed(mp); +} + +static void cb_track_position(struct bt_mcp *mcp, int32_t duration) +{ + struct media_player *mp = bt_mcp_get_user_data(mcp); + + /* MCP defines duration is int32 but api takes it as uint32 */ + media_player_set_position(mp, duration); +} + +static void cb_media_state(struct bt_mcp *mcp, uint8_t status) +{ + struct media_player *mp = bt_mcp_get_user_data(mcp); + + media_player_set_status(mp, mcp_status_val_to_string(status)); +} + +static const struct bt_mcp_event_callback cbs = { + .player_name = &cb_player_name, + .track_changed = &cb_track_changed, + .track_title = &cb_track_title, + .track_duration = &cb_track_duration, + .track_position = &cb_track_position, + .playback_speed = NULL, + .seeking_speed = NULL, + .play_order = NULL, + .play_order_supported = NULL, + .media_state = &cb_media_state, + .content_control_id = NULL, +}; + +static int ct_play(struct media_player *mp, void *user_data) +{ + struct bt_mcp *mcp = user_data; + + return bt_mcp_play(mcp); +} + +static int ct_pause(struct media_player *mp, void *user_data) +{ + struct bt_mcp *mcp = user_data; + + return bt_mcp_pause(mcp); +} + +static int ct_stop(struct media_player *mp, void *user_data) +{ + struct bt_mcp *mcp = user_data; + + return bt_mcp_stop(mcp); +} + +static const struct media_player_callback ct_cbs = { + .set_setting = NULL, + .play = &ct_play, + .pause = &ct_pause, + .stop = &ct_stop, + .next = NULL, + .previous = NULL, + .fast_forward = NULL, + .rewind = NULL, + .press = NULL, + .hold = NULL, + .release = NULL, + .list_items = NULL, + .change_folder = NULL, + .search = NULL, + .play_item = NULL, + .add_to_nowplaying = NULL, + .total_items = NULL, +}; + +static int mcp_probe(struct btd_service *service) +{ + struct btd_device *device = btd_service_get_device(service); + struct btd_adapter *adapter = device_get_adapter(device); + struct btd_gatt_database *database = btd_adapter_get_database(adapter); + struct mcp_data *data = btd_service_get_user_data(service); + char addr[18]; + + ba2str(device_get_address(device), addr); + DBG("%s", addr); + + /* Ignore, if we were probed for this device already */ + if (data) { + error("Profile probed twice for the same device!"); + return -EINVAL; + } + + data = mcp_data_new(device); + data->service = service; + + data->mcp = bt_mcp_new(btd_gatt_database_get_db(database), + btd_device_get_gatt_db(device)); + + bt_mcp_set_debug(data->mcp, mcp_debug, NULL, NULL); + btd_service_set_user_data(service, data); + + return 0; +} + +static void mcp_data_free(struct mcp_data *data) +{ + DBG(""); + + if (data->service) { + btd_service_set_user_data(data->service, NULL); + bt_mcp_set_user_data(data->mcp, NULL); + } + + if (data->mp) { + media_player_destroy(data->mp); + data->mp = NULL; + } + + bt_mcp_unref(data->mcp); + free(data); +} + +static void mcp_data_remove(struct mcp_data *data) +{ + DBG("data %p", data); + + mcp_data_free(data); +} + +static void mcp_remove(struct btd_service *service) +{ + struct btd_device *device = btd_service_get_device(service); + struct mcp_data *data; + char addr[18]; + + ba2str(device_get_address(device), addr); + DBG("%s", addr); + + data = btd_service_get_user_data(service); + if (!data) { + error("MCP service not handled by profile"); + return; + } + + mcp_data_remove(data); +} + +static int mcp_accept(struct btd_service *service) +{ + struct btd_device *device = btd_service_get_device(service); + struct bt_gatt_client *client = btd_device_get_gatt_client(device); + struct mcp_data *data = btd_service_get_user_data(service); + char addr[18]; + + ba2str(device_get_address(device), addr); + DBG("%s", addr); + + bt_mcp_attach(data->mcp, client); + + data->mp = media_player_controller_create(device_get_path(device), 0); + if (data->mp == NULL) { + DBG("Unable to create Media Player"); + return -EINVAL; + } + + media_player_set_callbacks(data->mp, &ct_cbs, data->mcp); + + bt_mcp_set_user_data(data->mcp, data->mp); + bt_mcp_set_event_callbacks(data->mcp, &cbs, data->mp); + btd_service_connecting_complete(service, 0); + + return 0; +} + +static int mcp_connect(struct btd_service *service) +{ + struct btd_device *device = btd_service_get_device(service); + char addr[18]; + + ba2str(device_get_address(device), addr); + DBG("%s", addr); + + return 0; +} + +static int mcp_disconnect(struct btd_service *service) +{ + struct btd_device *device = btd_service_get_device(service); + struct mcp_data *data = btd_service_get_user_data(service); + char addr[18]; + + ba2str(device_get_address(device), addr); + DBG("%s", addr); + + if (data->mp) { + media_player_destroy(data->mp); + data->mp = NULL; + } + + bt_mcp_detach(data->mcp); + + btd_service_disconnecting_complete(service, 0); + + return 0; +} + +static int media_control_server_probe(struct btd_profile *p, + struct btd_adapter *adapter) +{ + struct btd_gatt_database *database = btd_adapter_get_database(adapter); + + bt_mcp_register(btd_gatt_database_get_db(database)); + + return 0; +} + +static void media_control_server_remove(struct btd_profile *p, + struct btd_adapter *adapter) +{ + +} + +static struct btd_profile mcp_profile = { + .name = "mcp", + .priority = BTD_PROFILE_PRIORITY_MEDIUM, + .remote_uuid = GMCS_UUID_STR, + .device_probe = mcp_probe, + .device_remove = mcp_remove, + .accept = mcp_accept, + .connect = mcp_connect, + .disconnect = mcp_disconnect, + + .adapter_probe = media_control_server_probe, + .adapter_remove = media_control_server_remove, +}; + +static int mcp_init(void) +{ + DBG(""); + + if (!(g_dbus_get_flags() & G_DBUS_FLAG_ENABLE_EXPERIMENTAL)) { + warn("D-Bus experimental not enabled"); + return -ENOTSUP; + } + + btd_profile_register(&mcp_profile); + return 0; +} + +static void mcp_exit(void) +{ + DBG(""); + + if (g_dbus_get_flags() & G_DBUS_FLAG_ENABLE_EXPERIMENTAL) + btd_profile_unregister(&mcp_profile); +} + +BLUETOOTH_PLUGIN_DEFINE(mcp, VERSION, BLUETOOTH_PLUGIN_PRIORITY_DEFAULT, + mcp_init, mcp_exit) diff -pruN 5.65-1/profiles/audio/media.c 5.66-1/profiles/audio/media.c --- 5.65-1/profiles/audio/media.c 2022-07-24 20:59:52.000000000 +0000 +++ 5.66-1/profiles/audio/media.c 2022-11-10 20:22:09.000000000 +0000 @@ -31,12 +31,16 @@ #include "src/device.h" #include "src/dbus-common.h" #include "src/profile.h" +#include "src/service.h" #include "src/uuid-helper.h" #include "src/log.h" #include "src/error.h" +#include "src/gatt-database.h" #include "src/shared/util.h" #include "src/shared/queue.h" +#include "src/shared/att.h" +#include "src/shared/bap.h" #include "avdtp.h" #include "media.h" @@ -81,10 +85,14 @@ struct endpoint_request { struct media_endpoint { struct a2dp_sep *sep; + struct bt_bap_pac *pac; + void *stream; char *sender; /* Endpoint DBus bus id */ char *path; /* Endpoint object path */ char *uuid; /* Endpoint property UUID */ uint8_t codec; /* Endpoint codec */ + bool delay_reporting;/* Endpoint delay_reporting */ + struct bt_bap_pac_qos qos; /* Endpoint qos */ uint8_t *capabilities; /* Endpoint property capabilities */ size_t size; /* Endpoint capabilities size */ guint hs_watch; @@ -161,6 +169,12 @@ static void media_endpoint_destroy(struc g_slist_free_full(endpoint->transports, (GDestroyNotify) media_transport_destroy); + endpoint->transports = NULL; + + if (endpoint->pac) { + bt_bap_remove_pac(endpoint->pac); + endpoint->pac = NULL; + } g_dbus_remove_watch(btd_get_dbus_connection(), endpoint->watch); g_free(endpoint->capabilities); @@ -286,6 +300,7 @@ static void endpoint_reply(DBusPendingCa struct endpoint_request *request = user_data; struct media_endpoint *endpoint = request->endpoint; DBusMessage *reply; + DBusMessageIter args, props; DBusError err; gboolean value; void *ret = NULL; @@ -318,7 +333,7 @@ static void endpoint_reply(DBusPendingCa } if (dbus_message_is_method_call(request->msg, MEDIA_ENDPOINT_INTERFACE, - "SelectConfiguration")) { + "SelectConfiguration")) { DBusMessageIter args, array; uint8_t *configuration; @@ -330,7 +345,14 @@ static void endpoint_reply(DBusPendingCa ret = configuration; goto done; - } else if (!dbus_message_get_args(reply, &err, DBUS_TYPE_INVALID)) { + } else if (dbus_message_is_method_call(request->msg, + MEDIA_ENDPOINT_INTERFACE, + "SelectProperties")) { + dbus_message_iter_init(reply, &args); + dbus_message_iter_recurse(&args, &props); + ret = &props; + goto done; + } else if (!dbus_message_get_args(reply, &err, DBUS_TYPE_INVALID)) { error("Wrong reply signature: %s", err.message); dbus_error_free(&err); goto done; @@ -496,7 +518,7 @@ static gboolean set_configuration(struct transport = media_transport_create(device, a2dp_setup_remote_path(data->setup), - configuration, size, endpoint); + configuration, size, endpoint, NULL); if (transport == NULL) return FALSE; @@ -671,32 +693,483 @@ static void a2dp_destroy_endpoint(void * release_endpoint(endpoint); } -static gboolean endpoint_init_a2dp_source(struct media_endpoint *endpoint, - gboolean delay_reporting, - int *err) +static bool endpoint_init_a2dp_source(struct media_endpoint *endpoint, int *err) { endpoint->sep = a2dp_add_sep(endpoint->adapter->btd_adapter, AVDTP_SEP_TYPE_SOURCE, endpoint->codec, - delay_reporting, &a2dp_endpoint, - endpoint, a2dp_destroy_endpoint, err); + endpoint->delay_reporting, + &a2dp_endpoint, endpoint, + a2dp_destroy_endpoint, err); if (endpoint->sep == NULL) - return FALSE; + return false; - return TRUE; + return true; } -static gboolean endpoint_init_a2dp_sink(struct media_endpoint *endpoint, - gboolean delay_reporting, - int *err) +static bool endpoint_init_a2dp_sink(struct media_endpoint *endpoint, int *err) { endpoint->sep = a2dp_add_sep(endpoint->adapter->btd_adapter, AVDTP_SEP_TYPE_SINK, endpoint->codec, - delay_reporting, &a2dp_endpoint, - endpoint, a2dp_destroy_endpoint, err); + endpoint->delay_reporting, + &a2dp_endpoint, endpoint, + a2dp_destroy_endpoint, err); if (endpoint->sep == NULL) + return false; + + return true; +} + +struct pac_select_data { + struct bt_bap_pac *pac; + bt_bap_pac_select_t cb; + void *user_data; +}; + +static int parse_array(DBusMessageIter *iter, struct iovec **iov) +{ + DBusMessageIter array; + + if (!iov) + return 0; + + if (!(*iov)) + *iov = new0(struct iovec, 1); + + dbus_message_iter_recurse(iter, &array); + dbus_message_iter_get_fixed_array(&array, &(*iov)->iov_base, + (int *)&(*iov)->iov_len); + return 0; +} + +static int parse_select_properties(DBusMessageIter *props, struct iovec **caps, + struct iovec **metadata, + struct bt_bap_qos *qos) +{ + const char *key; + + while (dbus_message_iter_get_arg_type(props) == DBUS_TYPE_DICT_ENTRY) { + DBusMessageIter value, entry; + int var; + + dbus_message_iter_recurse(props, &entry); + dbus_message_iter_get_basic(&entry, &key); + + dbus_message_iter_next(&entry); + dbus_message_iter_recurse(&entry, &value); + + var = dbus_message_iter_get_arg_type(&value); + + if (!strcasecmp(key, "Capabilities")) { + if (var != DBUS_TYPE_ARRAY) + goto fail; + + if (parse_array(&value, caps)) + goto fail; + } else if (!strcasecmp(key, "Metadata")) { + if (var != DBUS_TYPE_ARRAY) + goto fail; + + if (parse_array(&value, metadata)) + goto fail; + } else if (!strcasecmp(key, "CIG")) { + if (var != DBUS_TYPE_BYTE) + goto fail; + + dbus_message_iter_get_basic(&value, &qos->cig_id); + } else if (!strcasecmp(key, "CIS")) { + if (var != DBUS_TYPE_BYTE) + goto fail; + + dbus_message_iter_get_basic(&value, &qos->cis_id); + } else if (!strcasecmp(key, "Interval")) { + if (var != DBUS_TYPE_UINT32) + goto fail; + + dbus_message_iter_get_basic(&value, &qos->interval); + } else if (!strcasecmp(key, "Framing")) { + dbus_bool_t val; + + if (var != DBUS_TYPE_BOOLEAN) + goto fail; + + dbus_message_iter_get_basic(&value, &val); + + qos->framing = val; + } else if (!strcasecmp(key, "PHY")) { + const char *str; + + if (var != DBUS_TYPE_STRING) + goto fail; + + dbus_message_iter_get_basic(&value, &str); + + if (!strcasecmp(str, "1M")) + qos->phy = 0x01; + else if (!strcasecmp(str, "2M")) + qos->phy = 0x02; + else + goto fail; + } else if (!strcasecmp(key, "SDU")) { + if (var != DBUS_TYPE_UINT16) + goto fail; + + dbus_message_iter_get_basic(&value, &qos->sdu); + } else if (!strcasecmp(key, "Retransmissions")) { + if (var != DBUS_TYPE_BYTE) + goto fail; + + dbus_message_iter_get_basic(&value, &qos->rtn); + } else if (!strcasecmp(key, "Latency")) { + if (var != DBUS_TYPE_UINT16) + goto fail; + + dbus_message_iter_get_basic(&value, &qos->latency); + } else if (!strcasecmp(key, "Delay")) { + if (var != DBUS_TYPE_UINT32) + goto fail; + + dbus_message_iter_get_basic(&value, &qos->delay); + } else if (!strcasecmp(key, "TargetLatency")) { + if (var != DBUS_TYPE_BYTE) + goto fail; + + dbus_message_iter_get_basic(&value, + &qos->target_latency); + } + + dbus_message_iter_next(props); + } + + return 0; + +fail: + DBG("Failed parsing %s", key); + + if (*caps) { + free(*caps); + *caps = NULL; + } + + return -EINVAL; +} + +static void pac_select_cb(struct media_endpoint *endpoint, void *ret, int size, + void *user_data) +{ + struct pac_select_data *data = user_data; + DBusMessageIter *iter = ret; + int err; + struct iovec *caps = NULL, *metadata = NULL; + struct bt_bap_qos qos; + + if (!ret) { + err = -EPERM; + goto done; + } + + if (dbus_message_iter_get_arg_type(iter) != DBUS_TYPE_DICT_ENTRY) { + DBG("Unexpected argument type: %c != %c", + dbus_message_iter_get_arg_type(iter), + DBUS_TYPE_DICT_ENTRY); + err = -EINVAL; + goto done; + } + + memset(&qos, 0, sizeof(qos)); + + /* Mark CIG and CIS to be auto assigned */ + qos.cig_id = BT_ISO_QOS_CIG_UNSET; + qos.cis_id = BT_ISO_QOS_CIS_UNSET; + + err = parse_select_properties(iter, &caps, &metadata, &qos); + if (err < 0) + DBG("Unable to parse properties"); + +done: + data->cb(data->pac, err, caps, metadata, &qos, data->user_data); +} + +static int pac_select(struct bt_bap_pac *lpac, struct bt_bap_pac *rpac, + struct bt_bap_pac_qos *qos, + bt_bap_pac_select_t cb, void *cb_data, void *user_data) +{ + struct media_endpoint *endpoint = user_data; + struct iovec *caps; + struct iovec *metadata; + const char *endpoint_path; + struct pac_select_data *data; + DBusMessage *msg; + DBusMessageIter iter, dict; + const char *key = "Capabilities"; + + bt_bap_pac_get_codec(rpac, NULL, &caps, &metadata); + if (!caps) + return -EINVAL; + + msg = dbus_message_new_method_call(endpoint->sender, endpoint->path, + MEDIA_ENDPOINT_INTERFACE, + "SelectProperties"); + if (msg == NULL) { + error("Couldn't allocate D-Bus message"); + return -ENOMEM; + } + + data = new0(struct pac_select_data, 1); + data->pac = lpac; + data->cb = cb; + data->user_data = cb_data; + + dbus_message_iter_init_append(msg, &iter); + + dbus_message_iter_open_container(&iter, DBUS_TYPE_ARRAY, "{sv}", &dict); + + endpoint_path = bt_bap_pac_get_user_data(rpac); + if (endpoint_path) + g_dbus_dict_append_entry(&dict, "Endpoint", + DBUS_TYPE_OBJECT_PATH, &endpoint_path); + + g_dbus_dict_append_basic_array(&dict, DBUS_TYPE_STRING, &key, + DBUS_TYPE_BYTE, &caps->iov_base, + caps->iov_len); + + if (metadata) { + key = "Metadata"; + g_dbus_dict_append_basic_array(&dict, DBUS_TYPE_STRING, &key, + DBUS_TYPE_BYTE, + &metadata->iov_base, + metadata->iov_len); + } + + if (qos && qos->phy) { + g_dbus_dict_append_entry(&dict, "Framing", DBUS_TYPE_BYTE, + &qos->framing); + + g_dbus_dict_append_entry(&dict, "PHY", DBUS_TYPE_BYTE, + &qos->phy); + + g_dbus_dict_append_entry(&dict, "Latency", DBUS_TYPE_UINT16, + &qos->latency); + + g_dbus_dict_append_entry(&dict, "MinimumDelay", + DBUS_TYPE_UINT32, &qos->pd_min); + + g_dbus_dict_append_entry(&dict, "MaximumDelay", + DBUS_TYPE_UINT32, &qos->pd_max); + + g_dbus_dict_append_entry(&dict, "PreferredMinimumDelay", + DBUS_TYPE_UINT32, &qos->ppd_min); + + g_dbus_dict_append_entry(&dict, "PreferredMaximumDelay", + DBUS_TYPE_UINT32, &qos->ppd_min); + } + + dbus_message_iter_close_container(&iter, &dict); + + return media_endpoint_async_call(msg, endpoint, NULL, pac_select_cb, + data, free); +} + +struct pac_config_data { + struct bt_bap_stream *stream; + bt_bap_pac_config_t cb; + void *user_data; +}; + +static int transport_cmp(gconstpointer data, gconstpointer user_data) +{ + const struct media_transport *transport = data; + const char *path = user_data; + + if (g_str_has_prefix(media_transport_get_path((void *)transport), path)) + return 0; + + return -1; +} + +static struct media_transport *find_transport(struct media_endpoint *endpoint, + const char *path) +{ + GSList *match; + + if (!path) + return NULL; + + match = g_slist_find_custom(endpoint->transports, path, transport_cmp); + if (match == NULL) + return NULL; + + return match->data; +} + +static void pac_config_cb(struct media_endpoint *endpoint, void *ret, int size, + void *user_data) +{ + struct pac_config_data *data = user_data; + gboolean *ret_value = ret; + + if (ret_value) + endpoint->stream = data->stream; + + data->cb(data->stream, ret_value ? 0 : -EINVAL); +} + +static int pac_config(struct bt_bap_stream *stream, struct iovec *cfg, + struct bt_bap_qos *qos, bt_bap_pac_config_t cb, + void *user_data) +{ + struct media_endpoint *endpoint = user_data; + DBusConnection *conn = btd_get_dbus_connection(); + struct pac_config_data *data; + struct media_transport *transport; + DBusMessage *msg; + DBusMessageIter iter; + const char *path; + + path = bt_bap_stream_get_user_data(stream); + + DBG("endpoint %p path %s", endpoint, path); + + transport = find_transport(endpoint, path); + if (!transport) { + struct bt_bap *bap = bt_bap_stream_get_session(stream); + struct btd_service *service = bt_bap_get_user_data(bap); + struct btd_device *device; + + if (service) + device = btd_service_get_device(service); + else { + struct bt_att *att = bt_bap_get_att(bap); + int fd = bt_att_get_fd(att); + + device = btd_adapter_find_device_by_fd(fd); + } + + if (!device) { + error("Unable to find device"); + return -EINVAL; + } + + transport = media_transport_create(device, path, cfg->iov_base, + cfg->iov_len, endpoint, + stream); + if (!transport) + return -EINVAL; + + path = media_transport_get_path(transport); + bt_bap_stream_set_user_data(stream, (void *)path); + } + + msg = dbus_message_new_method_call(endpoint->sender, endpoint->path, + MEDIA_ENDPOINT_INTERFACE, + "SetConfiguration"); + if (msg == NULL) { + error("Couldn't allocate D-Bus message"); + media_transport_destroy(transport); return FALSE; + } - return TRUE; + data = new0(struct pac_config_data, 1); + data->stream = stream; + data->cb = cb; + data->user_data = user_data; + + endpoint->transports = g_slist_append(endpoint->transports, transport); + + dbus_message_iter_init_append(msg, &iter); + + path = media_transport_get_path(transport); + dbus_message_iter_append_basic(&iter, DBUS_TYPE_OBJECT_PATH, &path); + + g_dbus_get_properties(conn, path, "org.bluez.MediaTransport1", &iter); + + return media_endpoint_async_call(msg, endpoint, transport, + pac_config_cb, data, free); +} + +static void pac_clear(struct bt_bap_stream *stream, void *user_data) +{ + struct media_endpoint *endpoint = user_data; + + endpoint->stream = NULL; + + while (endpoint->transports != NULL) + clear_configuration(endpoint, endpoint->transports->data); +} + +static struct bt_bap_pac_ops pac_ops = { + .select = pac_select, + .config = pac_config, + .clear = pac_clear, +}; + +static void bap_debug(const char *str, void *user_data) +{ + DBG("%s", str); +} + +static bool endpoint_init_pac(struct media_endpoint *endpoint, uint8_t type, + int *err) +{ + struct btd_gatt_database *database; + struct gatt_db *db; + struct iovec data; + char *name; + + if (!(g_dbus_get_flags() & G_DBUS_FLAG_ENABLE_EXPERIMENTAL)) { + warn("D-Bus experimental not enabled"); + *err = -ENOTSUP; + return false; + } + + database = btd_adapter_get_database(endpoint->adapter->btd_adapter); + if (!database) { + error("Adapter database not found"); + return false; + } + + if (!bap_print_cc(endpoint->capabilities, endpoint->size, bap_debug, + NULL)) { + error("Unable to parse endpoint capabilities"); + return false; + } + + db = btd_gatt_database_get_db(database); + + data.iov_base = endpoint->capabilities; + data.iov_len = endpoint->size; + + /* TODO: Add support for metadata */ + + if (asprintf(&name, "%s:%s", endpoint->sender, endpoint->path) < 0) { + error("Could not allocate name for pac %s:%s", + endpoint->sender, endpoint->path); + return false; + } + + endpoint->pac = bt_bap_add_pac(db, name, type, endpoint->codec, + &endpoint->qos, &data, NULL); + if (!endpoint->pac) { + error("Unable to create PAC"); + return false; + } + + bt_bap_pac_set_ops(endpoint->pac, &pac_ops, endpoint); + + DBG("PAC %s registered", name); + + free(name); + + return true; +} + +static bool endpoint_init_pac_sink(struct media_endpoint *endpoint, int *err) +{ + return endpoint_init_pac(endpoint, BT_BAP_SINK, err); +} + +static bool endpoint_init_pac_source(struct media_endpoint *endpoint, int *err) +{ + return endpoint_init_pac(endpoint, BT_BAP_SOURCE, err); } static bool endpoint_properties_exists(const char *uuid, @@ -781,24 +1254,58 @@ static bool endpoint_properties_get(cons return true; } -static struct media_endpoint *media_endpoint_create(struct media_adapter *adapter, +static bool endpoint_supported(struct btd_adapter *adapter) +{ + return true; +} + +static bool experimental_endpoint_supported(struct btd_adapter *adapter) +{ + if (!btd_adapter_has_exp_feature(adapter, EXP_FEAT_ISO_SOCKET)) + return false; + + return g_dbus_get_flags() & G_DBUS_FLAG_ENABLE_EXPERIMENTAL; +} + +static struct media_endpoint_init { + const char *uuid; + bool (*func)(struct media_endpoint *endpoint, int *err); + bool (*supported)(struct btd_adapter *adapter); +} init_table[] = { + { A2DP_SOURCE_UUID, endpoint_init_a2dp_source, endpoint_supported }, + { A2DP_SINK_UUID, endpoint_init_a2dp_sink, endpoint_supported }, + { PAC_SINK_UUID, endpoint_init_pac_sink, + experimental_endpoint_supported }, + { PAC_SOURCE_UUID, endpoint_init_pac_source, + experimental_endpoint_supported }, +}; + +static struct media_endpoint * +media_endpoint_create(struct media_adapter *adapter, const char *sender, const char *path, const char *uuid, gboolean delay_reporting, uint8_t codec, + struct bt_bap_pac_qos *qos, uint8_t *capabilities, int size, int *err) { struct media_endpoint *endpoint; - gboolean succeeded; + struct media_endpoint_init *init; + size_t i; + bool succeeded = false; endpoint = g_new0(struct media_endpoint, 1); endpoint->sender = g_strdup(sender); endpoint->path = g_strdup(path); endpoint->uuid = g_strdup(uuid); endpoint->codec = codec; + endpoint->delay_reporting = delay_reporting; + + if (qos) + endpoint->qos = *qos; if (size > 0) { endpoint->capabilities = g_new(uint8_t, size); @@ -808,26 +1315,20 @@ static struct media_endpoint *media_endp endpoint->adapter = adapter; - if (strcasecmp(uuid, A2DP_SOURCE_UUID) == 0) - succeeded = endpoint_init_a2dp_source(endpoint, - delay_reporting, err); - else if (strcasecmp(uuid, A2DP_SINK_UUID) == 0) - succeeded = endpoint_init_a2dp_sink(endpoint, - delay_reporting, err); - else if (strcasecmp(uuid, HFP_AG_UUID) == 0 || - strcasecmp(uuid, HSP_AG_UUID) == 0) - succeeded = TRUE; - else if (strcasecmp(uuid, HFP_HS_UUID) == 0 || - strcasecmp(uuid, HSP_HS_UUID) == 0) - succeeded = TRUE; - else { - succeeded = FALSE; + for (i = 0; i < ARRAY_SIZE(init_table); i++) { + init = &init_table[i]; - if (err) - *err = -EINVAL; + if (!init->supported(adapter->btd_adapter)) + continue; + + if (!strcasecmp(init->uuid, uuid)) { + succeeded = init->func(endpoint, err); + break; + } } if (!succeeded) { + error("Unable initialize endpoint for UUID %s", uuid); media_endpoint_destroy(endpoint); return NULL; } @@ -853,6 +1354,7 @@ static struct media_endpoint *media_endp static int parse_properties(DBusMessageIter *props, const char **uuid, gboolean *delay_reporting, uint8_t *codec, + struct bt_bap_pac_qos *qos, uint8_t **capabilities, int *size) { gboolean has_uuid = FALSE; @@ -893,6 +1395,34 @@ static int parse_properties(DBusMessageI dbus_message_iter_recurse(&value, &array); dbus_message_iter_get_fixed_array(&array, capabilities, size); + } else if (strcasecmp(key, "Framing") == 0) { + if (var != DBUS_TYPE_BYTE) + return -EINVAL; + dbus_message_iter_get_basic(&value, &qos->framing); + } else if (strcasecmp(key, "PHY") == 0) { + if (var != DBUS_TYPE_BYTE) + return -EINVAL; + dbus_message_iter_get_basic(&value, &qos->phy); + } else if (strcasecmp(key, "RTN") == 0) { + if (var != DBUS_TYPE_BYTE) + return -EINVAL; + dbus_message_iter_get_basic(&value, &qos->rtn); + } else if (strcasecmp(key, "MinimumDelay") == 0) { + if (var != DBUS_TYPE_UINT16) + return -EINVAL; + dbus_message_iter_get_basic(&value, &qos->pd_min); + } else if (strcasecmp(key, "MaximumDelay") == 0) { + if (var != DBUS_TYPE_UINT16) + return -EINVAL; + dbus_message_iter_get_basic(&value, &qos->pd_max); + } else if (strcasecmp(key, "PreferredMinimumDelay") == 0) { + if (var != DBUS_TYPE_UINT16) + return -EINVAL; + dbus_message_iter_get_basic(&value, &qos->pd_min); + } else if (strcasecmp(key, "PreferredMaximumDelay") == 0) { + if (var != DBUS_TYPE_UINT16) + return -EINVAL; + dbus_message_iter_get_basic(&value, &qos->pd_max); } dbus_message_iter_next(props); @@ -908,8 +1438,9 @@ static DBusMessage *register_endpoint(DB DBusMessageIter args, props; const char *sender, *path, *uuid; gboolean delay_reporting = FALSE; - uint8_t codec; - uint8_t *capabilities; + uint8_t codec = 0; + struct bt_bap_pac_qos qos = {}; + uint8_t *capabilities = NULL; int size = 0; int err; @@ -927,12 +1458,13 @@ static DBusMessage *register_endpoint(DB if (dbus_message_iter_get_arg_type(&props) != DBUS_TYPE_DICT_ENTRY) return btd_error_invalid_args(msg); - if (parse_properties(&props, &uuid, &delay_reporting, &codec, + if (parse_properties(&props, &uuid, &delay_reporting, &codec, &qos, &capabilities, &size) < 0) return btd_error_invalid_args(msg); if (media_endpoint_create(adapter, sender, path, uuid, delay_reporting, - codec, capabilities, size, &err) == NULL) { + codec, &qos, capabilities, size, + &err) == NULL) { if (err == -EPROTONOSUPPORT) return btd_error_not_supported(msg); else @@ -1958,6 +2490,7 @@ static void app_register_endpoint(void * const char *uuid; gboolean delay_reporting = FALSE; uint8_t codec; + struct bt_bap_pac_qos qos; uint8_t *capabilities = NULL; int size = 0; DBusMessageIter iter, array; @@ -2002,9 +2535,60 @@ static void app_register_endpoint(void * dbus_message_iter_get_fixed_array(&array, &capabilities, &size); } + /* Parse QoS preferences */ + memset(&qos, 0, sizeof(qos)); + if (g_dbus_proxy_get_property(proxy, "Framing", &iter)) { + if (dbus_message_iter_get_arg_type(&iter) != DBUS_TYPE_BYTE) + goto fail; + + dbus_message_iter_get_basic(&iter, &qos.framing); + } + + if (g_dbus_proxy_get_property(proxy, "PHY", &iter)) { + if (dbus_message_iter_get_arg_type(&iter) != DBUS_TYPE_BYTE) + goto fail; + + dbus_message_iter_get_basic(&iter, &qos.phy); + } + + if (g_dbus_proxy_get_property(proxy, "Latency", &iter)) { + if (dbus_message_iter_get_arg_type(&iter) != DBUS_TYPE_UINT16) + goto fail; + + dbus_message_iter_get_basic(&iter, &qos.latency); + } + + if (g_dbus_proxy_get_property(proxy, "MinimumDelay", &iter)) { + if (dbus_message_iter_get_arg_type(&iter) != DBUS_TYPE_UINT32) + goto fail; + + dbus_message_iter_get_basic(&iter, &qos.pd_min); + } + + if (g_dbus_proxy_get_property(proxy, "MaximumDelay", &iter)) { + if (dbus_message_iter_get_arg_type(&iter) != DBUS_TYPE_UINT32) + goto fail; + + dbus_message_iter_get_basic(&iter, &qos.pd_max); + } + + if (g_dbus_proxy_get_property(proxy, "PreferredMinimumDelay", &iter)) { + if (dbus_message_iter_get_arg_type(&iter) != DBUS_TYPE_UINT32) + goto fail; + + dbus_message_iter_get_basic(&iter, &qos.ppd_min); + } + + if (g_dbus_proxy_get_property(proxy, "PreferredMaximumDelay", &iter)) { + if (dbus_message_iter_get_arg_type(&iter) != DBUS_TYPE_UINT32) + goto fail; + + dbus_message_iter_get_basic(&iter, &qos.ppd_min); + } + endpoint = media_endpoint_create(app->adapter, app->sender, path, uuid, - delay_reporting, codec, capabilities, - size, &app->err); + delay_reporting, codec, &qos, + capabilities, size, &app->err); if (!endpoint) { error("Unable to register endpoint %s:%s: %s", app->sender, path, strerror(-app->err)); @@ -2390,17 +2974,56 @@ static const GDBusMethodTable media_meth { }, }; +static gboolean supported_uuids(const GDBusPropertyTable *property, + DBusMessageIter *iter, void *data) +{ + struct media_adapter *adapter = data; + DBusMessageIter entry; + size_t i; + + dbus_message_iter_open_container(iter, DBUS_TYPE_ARRAY, + DBUS_TYPE_STRING_AS_STRING, &entry); + + for (i = 0; i < ARRAY_SIZE(init_table); i++) { + struct media_endpoint_init *init = &init_table[i]; + + if (init->supported(adapter->btd_adapter)) + dbus_message_iter_append_basic(&entry, DBUS_TYPE_STRING, + &init->uuid); + } + + dbus_message_iter_close_container(iter, &entry); + + return TRUE; +} + +static const GDBusPropertyTable media_properties[] = { + { "SupportedUUIDs", "as", supported_uuids }, + { } +}; + static void path_free(void *data) { struct media_adapter *adapter = data; + GSList *l; queue_destroy(adapter->apps, app_free); - while (adapter->endpoints) - release_endpoint(adapter->endpoints->data); + for (l = adapter->endpoints; l;) { + struct media_endpoint *endpoint = l->data; + + l = g_slist_next(l); - while (adapter->players) - media_player_destroy(adapter->players->data); + release_endpoint(endpoint); + } + + for (l = adapter->players; l;) { + struct media_player *mp = l->data; + + l = g_slist_next(l); + + media_player_destroy(mp); + } adapters = g_slist_remove(adapters, adapter); @@ -2419,7 +3042,7 @@ int media_register(struct btd_adapter *b if (!g_dbus_register_interface(btd_get_dbus_connection(), adapter_get_path(btd_adapter), MEDIA_INTERFACE, - media_methods, NULL, NULL, + media_methods, NULL, media_properties, adapter, path_free)) { error("D-Bus failed to register %s path", adapter_get_path(btd_adapter)); diff -pruN 5.65-1/profiles/audio/transport.c 5.66-1/profiles/audio/transport.c --- 5.65-1/profiles/audio/transport.c 2022-07-24 20:59:52.000000000 +0000 +++ 5.66-1/profiles/audio/transport.c 2022-11-10 20:22:09.000000000 +0000 @@ -23,6 +23,7 @@ #include "lib/uuid.h" #include "gdbus/gdbus.h" +#include "btio/btio.h" #include "src/adapter.h" #include "src/device.h" @@ -30,7 +31,10 @@ #include "src/log.h" #include "src/error.h" +#include "src/shared/util.h" #include "src/shared/queue.h" +#include "src/shared/bap.h" +#include "src/shared/io.h" #include "avdtp.h" #include "media.h" @@ -76,6 +80,19 @@ struct a2dp_transport { int8_t volume; }; +struct bap_transport { + struct bt_bap_stream *stream; + unsigned int state_id; + bool linked; + uint32_t interval; + uint8_t framing; + uint8_t phy; + uint16_t sdu; + uint8_t rtn; + uint16_t latency; + uint32_t delay; +}; + struct media_transport { char *path; /* Transport object path */ struct btd_device *device; /* Transport device */ @@ -97,6 +114,8 @@ struct media_transport { struct media_owner *owner); void (*cancel) (struct media_transport *transport, guint id); + void (*set_state) (struct media_transport *transport, + transport_state_t state); GDestroyNotify destroy; void *data; }; @@ -134,6 +153,29 @@ static gboolean state_in_use(transport_s return FALSE; } +static struct media_transport * +find_transport_by_bap_stream(const struct bt_bap_stream *stream) +{ + GSList *l; + + for (l = transports; l; l = g_slist_next(l)) { + struct media_transport *transport = l->data; + const char *uuid = media_endpoint_get_uuid(transport->endpoint); + struct bap_transport *bap; + + if (strcasecmp(uuid, PAC_SINK_UUID) && + strcasecmp(uuid, PAC_SOURCE_UUID)) + continue; + + bap = transport->data; + + if (bap->stream == stream) + return transport; + } + + return NULL; +} + static void transport_set_state(struct media_transport *transport, transport_state_t state) { @@ -155,6 +197,10 @@ static void transport_set_state(struct m transport->path, MEDIA_TRANSPORT_INTERFACE, "State"); + + /* Update transport specific data */ + if (transport->set_state) + transport->set_state(transport, state); } void media_transport_destroy(struct media_transport *transport) @@ -236,9 +282,29 @@ static void media_owner_free(struct medi g_free(owner); } +static void linked_transport_remove_owner(void *data, void *user_data) +{ + struct bt_bap_stream *stream = data; + struct media_owner *owner = user_data; + struct media_transport *transport; + + transport = find_transport_by_bap_stream(stream); + if (!transport) { + error("Unable to find transport"); + return; + } + + DBG("Transport %s Owner %s", transport->path, owner->name); + transport->owner = NULL; +} + static void media_transport_remove_owner(struct media_transport *transport) { struct media_owner *owner = transport->owner; + struct bap_transport *bap = transport->data; + + if (!transport->owner) + return; DBG("Transport %s Owner %s", transport->path, owner->name); @@ -247,6 +313,9 @@ static void media_transport_remove_owner media_request_reply(owner->pending, EIO); transport->owner = NULL; + if (bap->linked) + queue_foreach(bt_bap_stream_io_get_links(bap->stream), + linked_transport_remove_owner, owner); if (owner->watch) g_dbus_remove_watch(btd_get_dbus_connection(), owner->watch); @@ -404,11 +473,34 @@ static void media_owner_exit(DBusConnect media_transport_remove_owner(owner->transport); } +static void linked_transport_set_owner(void *data, void *user_data) +{ + struct bt_bap_stream *stream = data; + struct media_owner *owner = user_data; + struct media_transport *transport; + + transport = find_transport_by_bap_stream(stream); + if (!transport) { + error("Unable to find transport"); + return; + } + + DBG("Transport %s Owner %s", transport->path, owner->name); + transport->owner = owner; +} + static void media_transport_set_owner(struct media_transport *transport, struct media_owner *owner) { + struct bap_transport *bap = transport->data; + DBG("Transport %s Owner %s", transport->path, owner->name); transport->owner = owner; + + if (bap->linked) + queue_foreach(bt_bap_stream_io_get_links(bap->stream), + linked_transport_set_owner, owner); + owner->transport = transport; owner->watch = g_dbus_add_disconnect_watch(btd_get_dbus_connection(), owner->name, @@ -597,7 +689,8 @@ static gboolean get_state(const GDBusPro return TRUE; } -static gboolean delay_exists(const GDBusPropertyTable *property, void *data) +static gboolean delay_reporting_exists(const GDBusPropertyTable *property, + void *data) { struct media_transport *transport = data; struct a2dp_transport *a2dp = transport->data; @@ -605,7 +698,7 @@ static gboolean delay_exists(const GDBus return a2dp->delay != 0; } -static gboolean get_delay(const GDBusPropertyTable *property, +static gboolean get_delay_reporting(const GDBusPropertyTable *property, DBusMessageIter *iter, void *data) { struct media_transport *transport = data; @@ -709,19 +802,181 @@ static const GDBusMethodTable transport_ { }, }; -static const GDBusPropertyTable transport_properties[] = { +static const GDBusPropertyTable a2dp_properties[] = { { "Device", "o", get_device }, { "UUID", "s", get_uuid }, { "Codec", "y", get_codec }, { "Configuration", "ay", get_configuration }, { "State", "s", get_state }, - { "Delay", "q", get_delay, NULL, delay_exists }, + { "Delay", "q", get_delay_reporting, NULL, delay_reporting_exists }, { "Volume", "q", get_volume, set_volume, volume_exists }, { "Endpoint", "o", get_endpoint, NULL, endpoint_exists, G_DBUS_PROPERTY_FLAG_EXPERIMENTAL }, { } }; +static gboolean get_interval(const GDBusPropertyTable *property, + DBusMessageIter *iter, void *data) +{ + struct media_transport *transport = data; + struct bap_transport *bap = transport->data; + + dbus_message_iter_append_basic(iter, DBUS_TYPE_UINT32, &bap->interval); + + return TRUE; +} + +static gboolean get_framing(const GDBusPropertyTable *property, + DBusMessageIter *iter, void *data) +{ + struct media_transport *transport = data; + struct bap_transport *bap = transport->data; + dbus_bool_t val = bap->framing; + + dbus_message_iter_append_basic(iter, DBUS_TYPE_BOOLEAN, &val); + + return TRUE; +} + +static gboolean get_sdu(const GDBusPropertyTable *property, + DBusMessageIter *iter, void *data) +{ + struct media_transport *transport = data; + struct bap_transport *bap = transport->data; + + dbus_message_iter_append_basic(iter, DBUS_TYPE_UINT16, &bap->sdu); + + return TRUE; +} + +static gboolean get_retransmissions(const GDBusPropertyTable *property, + DBusMessageIter *iter, void *data) +{ + struct media_transport *transport = data; + struct bap_transport *bap = transport->data; + + dbus_message_iter_append_basic(iter, DBUS_TYPE_BYTE, &bap->rtn); + + return TRUE; +} + +static gboolean get_latency(const GDBusPropertyTable *property, + DBusMessageIter *iter, void *data) +{ + struct media_transport *transport = data; + struct bap_transport *bap = transport->data; + + dbus_message_iter_append_basic(iter, DBUS_TYPE_UINT16, &bap->latency); + + return TRUE; +} + +static gboolean get_delay(const GDBusPropertyTable *property, + DBusMessageIter *iter, void *data) +{ + struct media_transport *transport = data; + struct bap_transport *bap = transport->data; + + dbus_message_iter_append_basic(iter, DBUS_TYPE_UINT32, &bap->delay); + + return TRUE; +} + +static gboolean get_location(const GDBusPropertyTable *property, + DBusMessageIter *iter, void *data) +{ + struct media_transport *transport = data; + struct bap_transport *bap = transport->data; + uint32_t location = bt_bap_stream_get_location(bap->stream); + + dbus_message_iter_append_basic(iter, DBUS_TYPE_UINT32, &location); + + return TRUE; +} + +static gboolean get_metadata(const GDBusPropertyTable *property, + DBusMessageIter *iter, void *data) +{ + struct media_transport *transport = data; + struct bap_transport *bap = transport->data; + struct iovec *meta = bt_bap_stream_get_metadata(bap->stream); + DBusMessageIter array; + + dbus_message_iter_open_container(iter, DBUS_TYPE_ARRAY, + DBUS_TYPE_BYTE_AS_STRING, &array); + + if (meta) + dbus_message_iter_append_fixed_array(&array, DBUS_TYPE_BYTE, + &meta->iov_base, + meta->iov_len); + + dbus_message_iter_close_container(iter, &array); + + return TRUE; +} + +static gboolean links_exists(const GDBusPropertyTable *property, void *data) +{ + struct media_transport *transport = data; + struct bap_transport *bap = transport->data; + + return bap->linked; +} + +static void append_links(void *data, void *user_data) +{ + struct bt_bap_stream *stream = data; + DBusMessageIter *array = user_data; + struct media_transport *transport; + + transport = find_transport_by_bap_stream(stream); + if (!transport) { + error("Unable to find transport"); + return; + } + + dbus_message_iter_append_basic(array, DBUS_TYPE_OBJECT_PATH, + &transport->path); +} + +static gboolean get_links(const GDBusPropertyTable *property, + DBusMessageIter *iter, void *data) +{ + struct media_transport *transport = data; + struct bap_transport *bap = transport->data; + struct queue *links = bt_bap_stream_io_get_links(bap->stream); + DBusMessageIter array; + + dbus_message_iter_open_container(iter, DBUS_TYPE_ARRAY, + DBUS_TYPE_OBJECT_PATH_AS_STRING, + &array); + + queue_foreach(links, append_links, &array); + + dbus_message_iter_close_container(iter, &array); + + return TRUE; +} + +static const GDBusPropertyTable bap_properties[] = { + { "Device", "o", get_device }, + { "UUID", "s", get_uuid }, + { "Codec", "y", get_codec }, + { "Configuration", "ay", get_configuration }, + { "State", "s", get_state }, + { "Interval", "u", get_interval }, + { "Framing", "b", get_framing }, + { "SDU", "q", get_sdu }, + { "Retransmissions", "y", get_retransmissions }, + { "Latency", "q", get_latency }, + { "Delay", "u", get_delay }, + { "Endpoint", "o", get_endpoint, NULL, endpoint_exists }, + { "Location", "u", get_location }, + { "Metadata", "ay", get_metadata }, + { "Links", "ao", get_links, NULL, links_exists }, + { } +}; + static void destroy_a2dp(void *data) { struct a2dp_transport *a2dp = data; @@ -842,15 +1097,338 @@ static int media_transport_init_sink(str return 0; } +static void bap_enable_complete(struct bt_bap_stream *stream, + uint8_t code, uint8_t reason, + void *user_data) +{ + struct media_owner *owner = user_data; + + if (code) + media_transport_remove_owner(owner->transport); +} + +static gboolean resume_complete(void *data) +{ + struct media_transport *transport = data; + struct media_owner *owner = transport->owner; + + if (!owner) + return FALSE; + + if (transport->fd < 0) { + media_transport_remove_owner(transport); + return FALSE; + } + + if (owner->pending) { + gboolean ret; + + ret = g_dbus_send_reply(btd_get_dbus_connection(), + owner->pending->msg, + DBUS_TYPE_UNIX_FD, &transport->fd, + DBUS_TYPE_UINT16, &transport->imtu, + DBUS_TYPE_UINT16, &transport->omtu, + DBUS_TYPE_INVALID); + if (!ret) { + media_transport_remove_owner(transport); + return FALSE; + } + } + + media_owner_remove(owner); + + transport_set_state(transport, TRANSPORT_STATE_ACTIVE); + + return FALSE; +} + +static void bap_update_links(const struct media_transport *transport); + +static bool match_link_transport(const void *data, const void *user_data) +{ + const struct bt_bap_stream *stream = data; + const struct media_transport *transport; + + transport = find_transport_by_bap_stream(stream); + if (!transport) + return false; + + bap_update_links(transport); + + return true; +} + +static void bap_update_links(const struct media_transport *transport) +{ + struct bap_transport *bap = transport->data; + struct queue *links = bt_bap_stream_io_get_links(bap->stream); + + if (bap->linked == !queue_isempty(links)) + return; + + bap->linked = !queue_isempty(links); + + /* Check if the links transport has been create yet */ + if (bap->linked && !queue_find(links, match_link_transport, NULL)) { + bap->linked = false; + return; + } + + g_dbus_emit_property_changed(btd_get_dbus_connection(), transport->path, + MEDIA_TRANSPORT_INTERFACE, + "Links"); + + DBG("stream %p linked %s", bap->stream, bap->linked ? "true" : "false"); +} + +static guint resume_bap(struct media_transport *transport, + struct media_owner *owner) +{ + struct bap_transport *bap = transport->data; + guint id; + + if (!bap->stream) + return 0; + + bap_update_links(transport); + + switch (bt_bap_stream_get_state(bap->stream)) { + case BT_BAP_STREAM_STATE_ENABLING: + bap_enable_complete(bap->stream, 0x00, 0x00, owner); + if (owner->pending) + return owner->pending->id; + return 0; + case BT_BAP_STREAM_STATE_STREAMING: + return g_idle_add(resume_complete, transport); + } + + id = bt_bap_stream_enable(bap->stream, bap->linked, NULL, + bap_enable_complete, owner); + if (!id) + return 0; + + if (transport->state == TRANSPORT_STATE_IDLE) + transport_set_state(transport, TRANSPORT_STATE_REQUESTING); + + return id; +} + +static void bap_stop_complete(struct bt_bap_stream *stream, + uint8_t code, uint8_t reason, + void *user_data) +{ + struct media_owner *owner = user_data; + struct media_request *req = owner->pending; + struct media_transport *transport = owner->transport; + + /* Release always succeeds */ + if (req) { + req->id = 0; + media_request_reply(req, 0); + media_owner_remove(owner); + } + + transport_set_state(transport, TRANSPORT_STATE_IDLE); + media_transport_remove_owner(transport); +} + +static void bap_disable_complete(struct bt_bap_stream *stream, + uint8_t code, uint8_t reason, + void *user_data) +{ + bap_stop_complete(stream, code, reason, user_data); +} + +static guint suspend_bap(struct media_transport *transport, + struct media_owner *owner) +{ + struct bap_transport *bap = transport->data; + bt_bap_stream_func_t func = NULL; + + if (!bap->stream) + return 0; + + if (owner) + func = bap_disable_complete; + else + transport_set_state(transport, TRANSPORT_STATE_IDLE); + + bap_update_links(transport); + + return bt_bap_stream_disable(bap->stream, bap->linked, func, owner); +} + +static void cancel_bap(struct media_transport *transport, guint id) +{ + struct bap_transport *bap = transport->data; + + if (!bap->stream) + return; + + bt_bap_stream_cancel(bap->stream, id); +} + +static void link_set_state(void *data, void *user_data) +{ + struct bt_bap_stream *stream = data; + transport_state_t state = PTR_TO_UINT(user_data); + struct media_transport *transport; + + transport = find_transport_by_bap_stream(stream); + if (!transport) { + error("Unable to find transport"); + return; + } + + transport_set_state(transport, state); +} + +static void set_state_bap(struct media_transport *transport, + transport_state_t state) +{ + struct bap_transport *bap = transport->data; + + if (!bap->linked) + return; + + /* Update links */ + queue_foreach(bt_bap_stream_io_get_links(bap->stream), link_set_state, + UINT_TO_PTR(state)); +} + +static void bap_state_changed(struct bt_bap_stream *stream, uint8_t old_state, + uint8_t new_state, void *user_data) +{ + struct media_transport *transport = user_data; + struct bap_transport *bap = transport->data; + struct media_owner *owner = transport->owner; + struct io *io; + GIOChannel *chan; + GError *err = NULL; + int fd; + uint16_t imtu, omtu; + + if (bap->stream != stream) + return; + + DBG("stream %p: %s(%u) -> %s(%u)", stream, + bt_bap_stream_statestr(old_state), old_state, + bt_bap_stream_statestr(new_state), new_state); + + switch (new_state) { + case BT_BAP_STREAM_STATE_IDLE: + case BT_BAP_STREAM_STATE_CONFIG: + case BT_BAP_STREAM_STATE_QOS: + /* If a request is pending wait it to complete */ + if (owner && owner->pending) + return; + bap_update_links(transport); + transport_update_playing(transport, FALSE); + return; + case BT_BAP_STREAM_STATE_DISABLING: + return; + case BT_BAP_STREAM_STATE_ENABLING: + if (!bt_bap_stream_get_io(stream)) + return; + break; + case BT_BAP_STREAM_STATE_STREAMING: + break; + } + + io = bt_bap_stream_get_io(stream); + if (!io) { + error("Unable to get stream IO"); + /* TODO: Fail if IO has not been established */ + goto done; + } + + fd = io_get_fd(io); + if (fd < 0) { + error("Unable to get IO fd"); + goto done; + } + + chan = g_io_channel_unix_new(fd); + + if (!bt_io_get(chan, &err, BT_IO_OPT_OMTU, &omtu, + BT_IO_OPT_IMTU, &imtu, + BT_IO_OPT_INVALID)) { + error("%s", err->message); + goto done; + } + + g_io_channel_unref(chan); + + media_transport_set_fd(transport, fd, imtu, omtu); + transport_update_playing(transport, TRUE); + +done: + resume_complete(transport); +} + +static void bap_connecting(struct bt_bap_stream *stream, bool state, int fd, + void *user_data) +{ + struct media_transport *transport = user_data; + struct bap_transport *bap = transport->data; + + if (bap->stream != stream) + return; + + bap_update_links(transport); +} + +static void free_bap(void *data) +{ + struct bap_transport *bap = data; + + bt_bap_state_unregister(bt_bap_stream_get_session(bap->stream), + bap->state_id); + free(bap); +} + +static int media_transport_init_bap(struct media_transport *transport, + void *stream) +{ + struct bt_bap_qos *qos; + struct bap_transport *bap; + + qos = bt_bap_stream_get_qos(stream); + + bap = new0(struct bap_transport, 1); + bap->stream = stream; + bap->interval = qos->interval; + bap->framing = qos->framing; + bap->phy = qos->phy; + bap->rtn = qos->rtn; + bap->latency = qos->latency; + bap->delay = qos->delay; + bap->state_id = bt_bap_state_register(bt_bap_stream_get_session(stream), + bap_state_changed, + bap_connecting, + transport, NULL); + + transport->data = bap; + transport->resume = resume_bap; + transport->suspend = suspend_bap; + transport->cancel = cancel_bap; + transport->set_state = set_state_bap; + transport->destroy = free_bap; + + return 0; +} + struct media_transport *media_transport_create(struct btd_device *device, const char *remote_endpoint, uint8_t *configuration, - size_t size, void *data) + size_t size, void *data, + void *stream) { struct media_endpoint *endpoint = data; struct media_transport *transport; const char *uuid; static int fd = 0; + const GDBusPropertyTable *properties; transport = g_new0(struct media_transport, 1); transport->device = device; @@ -868,15 +1446,22 @@ struct media_transport *media_transport_ if (strcasecmp(uuid, A2DP_SOURCE_UUID) == 0) { if (media_transport_init_source(transport) < 0) goto fail; + properties = a2dp_properties; } else if (strcasecmp(uuid, A2DP_SINK_UUID) == 0) { if (media_transport_init_sink(transport) < 0) goto fail; + properties = a2dp_properties; + } else if (!strcasecmp(uuid, PAC_SINK_UUID) || + !strcasecmp(uuid, PAC_SOURCE_UUID)) { + if (media_transport_init_bap(transport, stream) < 0) + goto fail; + properties = bap_properties; } else goto fail; if (g_dbus_register_interface(btd_get_dbus_connection(), transport->path, MEDIA_TRANSPORT_INTERFACE, - transport_methods, NULL, transport_properties, + transport_methods, NULL, properties, transport, media_transport_free) == FALSE) { error("Could not register transport %s", transport->path); goto fail; diff -pruN 5.65-1/profiles/audio/transport.h 5.66-1/profiles/audio/transport.h --- 5.65-1/profiles/audio/transport.h 2022-07-24 20:59:52.000000000 +0000 +++ 5.66-1/profiles/audio/transport.h 2022-11-10 20:22:09.000000000 +0000 @@ -14,7 +14,8 @@ struct media_transport; struct media_transport *media_transport_create(struct btd_device *device, const char *remote_endpoint, uint8_t *configuration, - size_t size, void *data); + size_t size, void *data, + void *stream); void media_transport_destroy(struct media_transport *transport); const char *media_transport_get_path(struct media_transport *transport); diff -pruN 5.65-1/profiles/audio/vcp.c 5.66-1/profiles/audio/vcp.c --- 5.65-1/profiles/audio/vcp.c 1970-01-01 00:00:00.000000000 +0000 +++ 5.66-1/profiles/audio/vcp.c 2022-11-10 20:22:09.000000000 +0000 @@ -0,0 +1,318 @@ +// SPDX-License-Identifier: GPL-2.0-or-later +/* + * + * BlueZ - Bluetooth protocol stack for Linux + * + * Copyright (C) 2022 Intel Corporation. All rights reserved. + * + * + */ + +#ifdef HAVE_CONFIG_H +#include +#endif + +#define _GNU_SOURCE + +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include "gdbus/gdbus.h" + +#include "lib/bluetooth.h" +#include "lib/hci.h" +#include "lib/sdp.h" +#include "lib/uuid.h" + +#include "src/dbus-common.h" +#include "src/shared/util.h" +#include "src/shared/att.h" +#include "src/shared/queue.h" +#include "src/shared/gatt-db.h" +#include "src/shared/gatt-client.h" +#include "src/shared/gatt-server.h" +#include "src/shared/vcp.h" + +#include "btio/btio.h" +#include "src/plugin.h" +#include "src/adapter.h" +#include "src/gatt-database.h" +#include "src/device.h" +#include "src/profile.h" +#include "src/service.h" +#include "src/log.h" +#include "src/error.h" + +#define VCS_UUID_STR "00001844-0000-1000-8000-00805f9b34fb" +#define MEDIA_ENDPOINT_INTERFACE "org.bluez.MediaEndpoint1" + +struct vcp_data { + struct btd_device *device; + struct btd_service *service; + struct bt_vcp *vcp; +}; + +static struct queue *sessions; + +static void vcp_debug(const char *str, void *user_data) +{ + DBG_IDX(0xffff, "%s", str); +} + +static int vcp_disconnect(struct btd_service *service) +{ + DBG(""); + return 0; +} + +static struct vcp_data *vcp_data_new(struct btd_device *device) +{ + struct vcp_data *data; + + data = new0(struct vcp_data, 1); + data->device = device; + + return data; +} + +static void vcp_data_add(struct vcp_data *data) +{ + DBG("data %p", data); + + if (queue_find(sessions, NULL, data)) { + error("data %p already added", data); + return; + } + + bt_vcp_set_debug(data->vcp, vcp_debug, NULL, NULL); + + if (!sessions) + sessions = queue_new(); + + queue_push_tail(sessions, data); + + if (data->service) + btd_service_set_user_data(data->service, data); +} + +static bool match_data(const void *data, const void *match_data) +{ + const struct vcp_data *vdata = data; + const struct bt_vcp *vcp = match_data; + + return vdata->vcp == vcp; +} + +static void vcp_data_free(struct vcp_data *data) +{ + if (data->service) { + btd_service_set_user_data(data->service, NULL); + bt_vcp_set_user_data(data->vcp, NULL); + } + + bt_vcp_unref(data->vcp); + free(data); +} + +static void vcp_data_remove(struct vcp_data *data) +{ + DBG("data %p", data); + + if (!queue_remove(sessions, data)) + return; + + vcp_data_free(data); + + if (queue_isempty(sessions)) { + queue_destroy(sessions, NULL); + sessions = NULL; + } +} + +static void vcp_detached(struct bt_vcp *vcp, void *user_data) +{ + struct vcp_data *data; + + DBG("%p", vcp); + + data = queue_find(sessions, match_data, vcp); + if (!data) { + error("Unable to find vcp session"); + return; + } + + vcp_data_remove(data); +} + +static void vcp_attached(struct bt_vcp *vcp, void *user_data) +{ + struct vcp_data *data; + struct bt_att *att; + struct btd_device *device; + + DBG("%p", vcp); + + data = queue_find(sessions, match_data, vcp); + if (data) + return; + + att = bt_vcp_get_att(vcp); + if (!att) + return; + + device = btd_adapter_find_device_by_fd(bt_att_get_fd(att)); + if (!device) { + error("Unable to find device"); + return; + } + + data = vcp_data_new(device); + data->vcp = vcp; + + vcp_data_add(data); +} + +static int vcp_probe(struct btd_service *service) +{ + struct btd_device *device = btd_service_get_device(service); + struct btd_adapter *adapter = device_get_adapter(device); + struct btd_gatt_database *database = btd_adapter_get_database(adapter); + struct vcp_data *data = btd_service_get_user_data(service); + char addr[18]; + + ba2str(device_get_address(device), addr); + DBG("%s", addr); + + /* Ignore, if we were probed for this device already */ + if (data) { + error("Profile probed twice for the same device!"); + return -EINVAL; + } + + data = vcp_data_new(device); + data->service = service; + + data->vcp = bt_vcp_new(btd_gatt_database_get_db(database), + btd_device_get_gatt_db(device)); + if (!data->vcp) { + error("Unable to create VCP instance"); + free(data); + return -EINVAL; + } + + vcp_data_add(data); + + bt_vcp_set_user_data(data->vcp, service); + + return 0; +} + +static void vcp_remove(struct btd_service *service) +{ + struct btd_device *device = btd_service_get_device(service); + struct vcp_data *data; + char addr[18]; + + ba2str(device_get_address(device), addr); + DBG("%s", addr); + + data = btd_service_get_user_data(service); + if (!data) { + error("VCP service not handled by profile"); + return; + } + + vcp_data_remove(data); +} + +static int vcp_accept(struct btd_service *service) +{ + struct btd_device *device = btd_service_get_device(service); + struct bt_gatt_client *client = btd_device_get_gatt_client(device); + struct vcp_data *data = btd_service_get_user_data(service); + char addr[18]; + + ba2str(device_get_address(device), addr); + DBG("%s", addr); + + if (!data) { + error("VCP service not handled by profile"); + return -EINVAL; + } + + if (!bt_vcp_attach(data->vcp, client)) { + error("VCP unable to attach"); + return -EINVAL; + } + + btd_service_connecting_complete(service, 0); + + return 0; +} + +static int vcp_server_probe(struct btd_profile *p, + struct btd_adapter *adapter) +{ + struct btd_gatt_database *database = btd_adapter_get_database(adapter); + + DBG("VCP path %s", adapter_get_path(adapter)); + + bt_vcp_add_db(btd_gatt_database_get_db(database)); + + return 0; +} + +static void vcp_server_remove(struct btd_profile *p, + struct btd_adapter *adapter) +{ + DBG("VCP remove Adapter"); +} + +static struct btd_profile vcp_profile = { + .name = "vcp", + .priority = BTD_PROFILE_PRIORITY_MEDIUM, + .remote_uuid = VCS_UUID_STR, + + .device_probe = vcp_probe, + .device_remove = vcp_remove, + + .accept = vcp_accept, + .disconnect = vcp_disconnect, + + .adapter_probe = vcp_server_probe, + .adapter_remove = vcp_server_remove, +}; + +static unsigned int vcp_id = 0; + +static int vcp_init(void) +{ + if (!(g_dbus_get_flags() & G_DBUS_FLAG_ENABLE_EXPERIMENTAL)) { + warn("D-Bus experimental not enabled"); + return -ENOTSUP; + } + + btd_profile_register(&vcp_profile); + vcp_id = bt_vcp_register(vcp_attached, vcp_detached, NULL); + + return 0; +} + +static void vcp_exit(void) +{ + if (g_dbus_get_flags() & G_DBUS_FLAG_ENABLE_EXPERIMENTAL) { + btd_profile_unregister(&vcp_profile); + bt_vcp_unregister(vcp_id); + } +} + +BLUETOOTH_PLUGIN_DEFINE(vcp, VERSION, BLUETOOTH_PLUGIN_PRIORITY_DEFAULT, + vcp_init, vcp_exit) diff -pruN 5.65-1/profiles/input/hog-lib.c 5.66-1/profiles/input/hog-lib.c --- 5.65-1/profiles/input/hog-lib.c 2022-07-24 20:59:52.000000000 +0000 +++ 5.66-1/profiles/input/hog-lib.c 2022-11-10 20:22:09.000000000 +0000 @@ -64,7 +64,6 @@ #define HOG_PROTO_MODE_BOOT 0 #define HOG_PROTO_MODE_REPORT 1 -#define HOG_REPORT_MAP_MAX_SIZE 512 #define HID_INFO_SIZE 4 #define ATT_NOTIFICATION_HEADER_SIZE 3 @@ -103,11 +102,6 @@ struct bt_hog { struct queue *input; }; -struct report_map { - uint8_t value[HOG_REPORT_MAP_MAX_SIZE]; - size_t length; -}; - struct report { struct bt_hog *hog; bool numbered; @@ -596,6 +590,9 @@ static struct report *report_new(struct struct report *report; GSList *l; + if (!chr) + return NULL; + /* Skip if report already exists */ l = g_slist_find_custom(hog->reports, chr, report_chrc_cmp); if (l) @@ -636,6 +633,9 @@ static void external_service_char_cb(uin chr = l->data; next = l->next ? l->next->data : NULL; + if (!chr) + continue; + DBG("0x%04x UUID: %s properties: %02x", chr->handle, chr->uuid, chr->properties); @@ -1096,7 +1096,7 @@ static void report_map_read_cb(guint8 st { struct gatt_request *req = user_data; struct bt_hog *hog = req->user_data; - uint8_t value[HOG_REPORT_MAP_MAX_SIZE]; + uint8_t *value; ssize_t vlen; remove_gatt_req(req, status); @@ -1106,10 +1106,12 @@ static void report_map_read_cb(guint8 st return; } - vlen = dec_read_resp(pdu, plen, value, sizeof(value)); + value = new0(uint8_t, plen); + + vlen = dec_read_resp(pdu, plen, value, plen); if (vlen < 0) { error("ATT protocol error"); - return; + goto done; } uhid_create(hog, value, vlen); @@ -1120,6 +1122,9 @@ static void report_map_read_cb(guint8 st NULL, db_report_map_write_value_cb, NULL); } + +done: + free(value); } static void read_report_map(struct bt_hog *hog) @@ -1233,6 +1238,9 @@ static void char_discovered_cb(uint8_t s chr = l->data; next = l->next ? l->next->data : NULL; + if (!chr) + continue; + DBG("0x%04x UUID: %s properties: %02x", chr->handle, chr->uuid, chr->properties); @@ -1394,7 +1402,7 @@ static void db_report_map_read_value_cb( int err, const uint8_t *value, size_t length, void *user_data) { - struct report_map *map = user_data; + struct iovec *map = user_data; if (err) { error("Error reading report map from gatt db %s", @@ -1405,8 +1413,9 @@ static void db_report_map_read_value_cb( if (!length) return; - map->length = length < sizeof(map->value) ? length : sizeof(map->value); - memcpy(map->value, value, map->length); + + map->iov_len = length; + map->iov_base = (void *) value; } static void foreach_hog_chrc(struct gatt_db_attribute *attr, void *user_data) @@ -1415,7 +1424,7 @@ static void foreach_hog_chrc(struct gatt bt_uuid_t uuid, report_uuid, report_map_uuid, info_uuid; bt_uuid_t proto_mode_uuid, ctrlpt_uuid; uint16_t handle, value_handle; - struct report_map report_map = {0}; + struct iovec map = {}; gatt_db_attribute_get_char_data(attr, &handle, &value_handle, NULL, NULL, &uuid); @@ -1438,14 +1447,14 @@ static void foreach_hog_chrc(struct gatt gatt_db_attribute_read(hog->report_map_attr, 0, BT_ATT_OP_READ_REQ, NULL, db_report_map_read_value_cb, - &report_map); + &map); } - if (report_map.length) { + if (map.iov_len) { /* Report map found in the cache, straight to creating * UHID to optimize reconnection. */ - uhid_create(hog, report_map.value, report_map.length); + uhid_create(hog, map.iov_base, map.iov_len); } gatt_db_service_foreach_desc(attr, foreach_hog_external, hog); diff -pruN 5.65-1/README 5.66-1/README --- 5.65-1/README 2022-07-24 20:59:52.000000000 +0000 +++ 5.66-1/README 2022-11-10 20:22:09.000000000 +0000 @@ -79,6 +79,7 @@ may need to be enabled, and the kernel r CONFIG_CRYPTO_USER_API CONFIG_CRYPTO_USER_API_AEAD CONFIG_CRYPTO_USER_API_HASH + CONFIG_CRYPTO_USER_API_SKCIPHER CONFIG_CRYPTO_AES CONFIG_CRYPTO_CCM diff -pruN 5.65-1/src/adapter.c 5.66-1/src/adapter.c --- 5.65-1/src/adapter.c 2022-07-24 20:59:52.000000000 +0000 +++ 5.66-1/src/adapter.c 2022-11-10 20:22:09.000000000 +0000 @@ -239,6 +239,14 @@ struct btd_adapter_pin_cb_iter { /* When the iterator reaches the end, it is NULL and attempt is 0 */ }; +enum { + ADAPTER_POWER_STATE_OFF, + ADAPTER_POWER_STATE_ON, + ADAPTER_POWER_STATE_ON_DISABLING, + ADAPTER_POWER_STATE_OFF_ENABLING, + ADAPTER_POWER_STATE_OFF_BLOCKED, +}; + struct btd_adapter { int ref_count; @@ -252,6 +260,7 @@ struct btd_adapter { char *short_name; /* controller short name */ uint32_t supported_settings; /* controller supported settings */ uint32_t pending_settings; /* pending controller settings */ + uint32_t power_state; /* the power state */ uint32_t current_settings; /* current controller settings */ char *path; /* adapter object path */ @@ -325,6 +334,24 @@ struct btd_adapter { struct queue *exps; }; +static char *adapter_power_state_str(uint32_t power_state) +{ + switch (power_state) { + case ADAPTER_POWER_STATE_OFF: + return "off"; + case ADAPTER_POWER_STATE_ON: + return "on"; + case ADAPTER_POWER_STATE_ON_DISABLING: + return "on-disabling"; + case ADAPTER_POWER_STATE_OFF_ENABLING: + return "off-enabling"; + case ADAPTER_POWER_STATE_OFF_BLOCKED: + return "off-blocked"; + } + DBG("Invalid power state %d", power_state); + return ""; +} + typedef enum { ADAPTER_AUTHORIZE_DISCONNECTED = 0, ADAPTER_AUTHORIZE_CHECK_CONNECTED @@ -618,6 +645,29 @@ static void settings_changed(struct btd_ } } +static void adapter_set_power_state(struct btd_adapter *adapter, uint32_t value) +{ + if (adapter->power_state == value) + return; + + DBG("%s", adapter_power_state_str(value)); + adapter->power_state = value; + g_dbus_emit_property_changed(dbus_conn, adapter->path, + ADAPTER_INTERFACE, "PowerState"); +} + +static void reset_power_state_target(struct btd_adapter *adapter, + uint32_t value) +{ + if (value && + adapter->power_state == ADAPTER_POWER_STATE_OFF_ENABLING) { + adapter_set_power_state(adapter, ADAPTER_POWER_STATE_ON); + } else if (!value && + adapter->power_state == ADAPTER_POWER_STATE_ON_DISABLING) { + adapter_set_power_state(adapter, ADAPTER_POWER_STATE_OFF); + } +} + static void new_settings_callback(uint16_t index, uint16_t length, const void *param, void *user_data) { @@ -635,19 +685,39 @@ static void new_settings_callback(uint16 if (settings == adapter->current_settings) return; + if ((adapter->current_settings ^ settings) & MGMT_SETTING_POWERED) { + reset_power_state_target(adapter, + settings & MGMT_SETTING_POWERED ? + 0x01 : 0x00); + } + DBG("Settings: 0x%08x", settings); settings_changed(adapter, settings); } +struct set_mode_data { + struct btd_adapter *adapter; + uint32_t setting; + uint8_t value; +}; + static void set_mode_complete(uint8_t status, uint16_t length, const void *param, void *user_data) { - struct btd_adapter *adapter = user_data; + struct set_mode_data *data = user_data; + struct btd_adapter *adapter = data->adapter; if (status != MGMT_STATUS_SUCCESS) { btd_error(adapter->dev_id, "Failed to set mode: %s (0x%02x)", mgmt_errstr(status), status); + if (status == MGMT_STATUS_RFKILLED) + adapter_set_power_state(adapter, + ADAPTER_POWER_STATE_OFF_BLOCKED); + adapter->pending_settings &= ~data->setting; + if (status != MGMT_STATUS_RFKILLED && + data->setting & MGMT_SETTING_POWERED) + reset_power_state_target(adapter, data->value); return; } @@ -677,6 +747,7 @@ static bool set_mode(struct btd_adapter { struct mgmt_mode cp; uint32_t setting = 0; + struct set_mode_data *data; memset(&cp, 0, sizeof(cp)); cp.val = mode; @@ -684,6 +755,11 @@ static bool set_mode(struct btd_adapter switch (opcode) { case MGMT_OP_SET_POWERED: setting = MGMT_SETTING_POWERED; + if (adapter->power_state != ADAPTER_POWER_STATE_OFF_BLOCKED) { + adapter_set_power_state(adapter, mode ? + ADAPTER_POWER_STATE_OFF_ENABLING : + ADAPTER_POWER_STATE_ON_DISABLING); + } break; case MGMT_OP_SET_CONNECTABLE: setting = MGMT_SETTING_CONNECTABLE; @@ -699,15 +775,26 @@ static bool set_mode(struct btd_adapter break; } - adapter->pending_settings |= setting; - DBG("sending set mode command for index %u", adapter->dev_id); + data = g_new0(struct set_mode_data, 1); + data->adapter = adapter; + data->setting = setting; + data->value = mode; + if (mgmt_send(adapter->mgmt, opcode, adapter->dev_id, sizeof(cp), &cp, - set_mode_complete, adapter, NULL) > 0) + set_mode_complete, data, g_free) > 0) { + adapter->pending_settings |= setting; return true; - + } + g_free(data); + if (setting == MGMT_SETTING_POWERED) { + /* cancel the earlier setting */ + adapter_set_power_state(adapter, mode ? + ADAPTER_POWER_STATE_OFF : + ADAPTER_POWER_STATE_ON); + } btd_error(adapter->dev_id, "Failed to set mode for index %u", adapter->dev_id); @@ -718,6 +805,7 @@ static bool set_discoverable(struct btd_ uint16_t timeout) { struct mgmt_cp_set_discoverable cp; + struct set_mode_data *data; memset(&cp, 0, sizeof(cp)); cp.val = mode; @@ -734,11 +822,16 @@ static bool set_discoverable(struct btd_ mode); } + data = g_new0(struct set_mode_data, 1); + data->adapter = adapter; + data->setting = 0; + if (mgmt_send(adapter->mgmt, MGMT_OP_SET_DISCOVERABLE, adapter->dev_id, sizeof(cp), &cp, - set_mode_complete, adapter, NULL) > 0) + set_mode_complete, data, g_free) > 0) return true; + g_free(data); btd_error(adapter->dev_id, "Failed to set mode for index %u", adapter->dev_id); @@ -1364,6 +1457,39 @@ struct btd_device *btd_adapter_get_devic return adapter_create_device(adapter, addr, addr_type); } +struct btd_device *btd_adapter_find_device_by_fd(int fd) +{ + bdaddr_t src, dst; + uint8_t dst_type; + GIOChannel *io = NULL; + GError *gerr = NULL; + struct btd_adapter *adapter; + + io = g_io_channel_unix_new(fd); + if (!io) + return NULL; + + bt_io_get(io, &gerr, + BT_IO_OPT_SOURCE_BDADDR, &src, + BT_IO_OPT_DEST_BDADDR, &dst, + BT_IO_OPT_DEST_TYPE, &dst_type, + BT_IO_OPT_INVALID); + if (gerr) { + error("bt_io_get: %s", gerr->message); + g_error_free(gerr); + g_io_channel_unref(io); + return NULL; + } + + g_io_channel_unref(io); + + adapter = adapter_find(&src); + if (!adapter) + return NULL; + + return btd_adapter_find_device(adapter, &dst, dst_type); +} + sdp_list_t *btd_adapter_get_services(struct btd_adapter *adapter) { return adapter->services; @@ -2877,7 +3003,9 @@ static gboolean property_get_mode(struct struct property_set_data { struct btd_adapter *adapter; + uint32_t setting; GDBusPendingPropertySet id; + uint8_t value; }; static void property_set_mode_complete(uint8_t status, uint16_t length, @@ -2894,13 +3022,21 @@ static void property_set_mode_complete(u btd_error(adapter->dev_id, "Failed to set mode: %s (0x%02x)", mgmt_errstr(status), status); - if (status == MGMT_STATUS_RFKILLED) + if (status == MGMT_STATUS_RFKILLED) { dbus_err = ERROR_INTERFACE ".Blocked"; - else + adapter_set_power_state(adapter, + ADAPTER_POWER_STATE_OFF_BLOCKED); + } else { dbus_err = ERROR_INTERFACE ".Failed"; + } g_dbus_pending_property_error(data->id, dbus_err, mgmt_errstr(status)); + + adapter->pending_settings &= ~data->setting; + if (status != MGMT_STATUS_RFKILLED && + data->setting & MGMT_SETTING_POWERED) + reset_power_state_target(adapter, data->value); return; } @@ -2969,8 +3105,6 @@ static void property_set_mode(struct btd mode = (enable == TRUE) ? 0x01 : 0x00; - adapter->pending_settings |= setting; - switch (setting) { case MGMT_SETTING_POWERED: opcode = MGMT_OP_SET_POWERED; @@ -3024,13 +3158,31 @@ static void property_set_mode(struct btd goto failed; data->adapter = adapter; + data->setting = setting; data->id = id; + data->setting = setting; + data->value = mode; + + if (setting == MGMT_SETTING_POWERED && + adapter->power_state != ADAPTER_POWER_STATE_OFF_BLOCKED) { + adapter_set_power_state(adapter, mode ? + ADAPTER_POWER_STATE_OFF_ENABLING : + ADAPTER_POWER_STATE_ON_DISABLING); + } if (mgmt_send(adapter->mgmt, opcode, adapter->dev_id, len, param, - property_set_mode_complete, data, g_free) > 0) + property_set_mode_complete, data, g_free) > 0) { + adapter->pending_settings |= setting; return; + } g_free(data); + if (setting == MGMT_SETTING_POWERED) { + /* cancel the earlier setting */ + adapter_set_power_state(adapter, mode ? + ADAPTER_POWER_STATE_OFF : + ADAPTER_POWER_STATE_ON); + } failed: btd_error(adapter->dev_id, "Failed to set mode for index %u", @@ -3062,6 +3214,18 @@ static void property_set_powered(const G property_set_mode(adapter, MGMT_SETTING_POWERED, iter, id); } +static gboolean property_get_power_state(const GDBusPropertyTable *property, + DBusMessageIter *iter, void *user_data) +{ + struct btd_adapter *adapter = user_data; + const char *str; + + str = adapter_power_state_str(adapter->power_state); + dbus_message_iter_append_basic(iter, DBUS_TYPE_STRING, &str); + + return TRUE; +} + static gboolean property_get_discoverable(const GDBusPropertyTable *property, DBusMessageIter *iter, void *user_data) { @@ -3347,7 +3511,7 @@ static gboolean property_get_experimenta return TRUE; } -static gboolean property_experimental_exits(const GDBusPropertyTable *property, +static gboolean property_experimental_exists(const GDBusPropertyTable *property, void *data) { struct btd_adapter *adapter = data; @@ -3700,6 +3864,8 @@ static const GDBusPropertyTable adapter_ { "Alias", "s", property_get_alias, property_set_alias }, { "Class", "u", property_get_class }, { "Powered", "b", property_get_powered, property_set_powered }, + { "PowerState", "s", property_get_power_state, NULL, NULL, + G_DBUS_PROPERTY_FLAG_EXPERIMENTAL }, { "Discoverable", "b", property_get_discoverable, property_set_discoverable }, { "DiscoverableTimeout", "u", property_get_discoverable_timeout, @@ -3713,7 +3879,7 @@ static const GDBusPropertyTable adapter_ property_exists_modalias }, { "Roles", "as", property_get_roles }, { "ExperimentalFeatures", "as", property_get_experimental, NULL, - property_experimental_exits }, + property_experimental_exists }, { } }; @@ -5395,10 +5561,12 @@ void adapter_set_device_flags(struct btd mgmt_request_func_t func, void *user_data) { struct mgmt_cp_set_device_flags cp; + uint32_t supported = btd_device_get_supported_flags(device); const bdaddr_t *bdaddr; uint8_t bdaddr_type; - if (!btd_has_kernel_features(KERNEL_CONN_CONTROL)) + if (!btd_has_kernel_features(KERNEL_CONN_CONTROL) || + (supported | flags) != supported) return; bdaddr = device_get_address(device); @@ -5504,6 +5672,7 @@ static void adapter_start(struct btd_ada { g_dbus_emit_property_changed(dbus_conn, adapter->path, ADAPTER_INTERFACE, "Powered"); + adapter_set_power_state(adapter, ADAPTER_POWER_STATE_ON); DBG("adapter %s has been enabled", adapter->path); @@ -6623,6 +6792,7 @@ static void load_config(struct btd_adapt static struct btd_adapter *btd_adapter_new(uint16_t index) { struct btd_adapter *adapter; + int blocked; adapter = g_try_new0(struct btd_adapter, 1); if (!adapter) @@ -6631,6 +6801,9 @@ static struct btd_adapter *btd_adapter_n adapter->dev_id = index; adapter->mgmt = mgmt_ref(mgmt_primary); adapter->pincode_requested = false; + blocked = rfkill_get_blocked(index); + if (blocked > 0) + adapter->power_state = ADAPTER_POWER_STATE_OFF_BLOCKED; /* * Setup default configuration values. These are either adapter @@ -6656,6 +6829,9 @@ static struct btd_adapter *btd_adapter_n DBG("Modalias: %s", adapter->modalias); DBG("Discoverable timeout: %u seconds", adapter->discoverable_timeout); DBG("Pairable timeout: %u seconds", adapter->pairable_timeout); + if (blocked > 0) + DBG("Power state: %s", + adapter_power_state_str(adapter->power_state)); adapter->auths = g_queue_new(); adapter->exps = queue_new(); @@ -7247,6 +7423,9 @@ static void adapter_stop(struct btd_adap g_dbus_emit_property_changed(dbus_conn, adapter->path, ADAPTER_INTERFACE, "Powered"); + g_dbus_emit_property_changed(dbus_conn, adapter->path, + ADAPTER_INTERFACE, + "PowerState"); DBG("adapter %s has been disabled", adapter->path); } @@ -7334,7 +7513,7 @@ static gboolean process_auth_queue(gpoin goto next; } - if (device_is_trusted(device) == TRUE) { + if (btd_device_is_trusted(device) == TRUE) { auth->cb(NULL, auth->user_data); goto next; } @@ -7523,7 +7702,18 @@ int btd_cancel_authorization(guint id) int btd_adapter_restore_powered(struct btd_adapter *adapter) { - if (btd_adapter_get_powered(adapter)) + bool powered; + + powered = btd_adapter_get_powered(adapter); + if (adapter->power_state == ADAPTER_POWER_STATE_OFF_BLOCKED && + rfkill_get_blocked(adapter->dev_id) == 0) { + adapter_set_power_state(adapter, + powered ? + ADAPTER_POWER_STATE_ON : + ADAPTER_POWER_STATE_OFF); + } + + if (powered) return 0; set_mode(adapter, MGMT_OP_SET_POWERED, 0x01); @@ -7531,6 +7721,12 @@ int btd_adapter_restore_powered(struct b return 0; } +int btd_adapter_set_blocked(struct btd_adapter *adapter) +{ + adapter_set_power_state(adapter, ADAPTER_POWER_STATE_OFF_BLOCKED); + return 0; +} + void btd_adapter_register_pin_cb(struct btd_adapter *adapter, btd_adapter_pin_cb_t cb) { diff -pruN 5.65-1/src/adapter.h 5.66-1/src/adapter.h --- 5.65-1/src/adapter.h 2022-07-24 20:59:52.000000000 +0000 +++ 5.66-1/src/adapter.h 2022-11-10 20:22:09.000000000 +0000 @@ -86,6 +86,7 @@ struct btd_device *btd_adapter_find_devi uint8_t dst_type); struct btd_device *btd_adapter_find_device_by_path(struct btd_adapter *adapter, const char *path); +struct btd_device *btd_adapter_find_device_by_fd(int fd); void btd_adapter_update_found_device(struct btd_adapter *adapter, const bdaddr_t *bdaddr, @@ -143,6 +144,7 @@ guint btd_request_authorization_cable_co int btd_cancel_authorization(guint id); int btd_adapter_restore_powered(struct btd_adapter *adapter); +int btd_adapter_set_blocked(struct btd_adapter *adapter); typedef ssize_t (*btd_adapter_pin_cb_t) (struct btd_adapter *adapter, struct btd_device *dev, char *out, bool *display, diff -pruN 5.65-1/src/advertising.c 5.66-1/src/advertising.c --- 5.65-1/src/advertising.c 2022-07-24 20:59:52.000000000 +0000 +++ 5.66-1/src/advertising.c 2022-11-10 20:22:09.000000000 +0000 @@ -1047,15 +1047,21 @@ static bool parse_secondary(DBusMessageI const char *str; struct adv_secondary *sec; + if (!iter) { + /* Reset secondary channels */ + client->flags &= ~MGMT_ADV_FLAG_SEC_MASK; + return true; + } + if (dbus_message_iter_get_arg_type(iter) != DBUS_TYPE_STRING) return false; /* Reset secondary channels before parsing */ - client->flags &= 0xfe00; + client->flags &= ~MGMT_ADV_FLAG_SEC_MASK; dbus_message_iter_get_basic(iter, &str); - for (sec = secondary; sec && sec->name; sec++) { + for (sec = secondary; sec->name; sec++) { if (strcmp(str, sec->name)) continue; @@ -1081,8 +1087,10 @@ static bool parse_min_interval(DBusMessa if (!(g_dbus_get_flags() & G_DBUS_FLAG_ENABLE_EXPERIMENTAL)) return true; - if (!iter) + if (!iter) { + client->min_interval = 0; return false; + } if (dbus_message_iter_get_arg_type(iter) != DBUS_TYPE_UINT32) return false; @@ -1112,8 +1120,10 @@ static bool parse_max_interval(DBusMessa if (!(g_dbus_get_flags() & G_DBUS_FLAG_ENABLE_EXPERIMENTAL)) return true; - if (!iter) + if (!iter) { + client->max_interval = 0; return false; + } if (dbus_message_iter_get_arg_type(iter) != DBUS_TYPE_UINT32) return false; @@ -1143,8 +1153,10 @@ static bool parse_tx_power(DBusMessageIt if (!(g_dbus_get_flags() & G_DBUS_FLAG_ENABLE_EXPERIMENTAL)) return true; - if (!iter) + if (!iter) { + client->tx_power = ADV_TX_POWER_NO_PREFERENCE; return false; + } if (dbus_message_iter_get_arg_type(iter) != DBUS_TYPE_INT16) return false; diff -pruN 5.65-1/src/battery.c 5.66-1/src/battery.c --- 5.65-1/src/battery.c 2022-07-24 20:59:52.000000000 +0000 +++ 5.66-1/src/battery.c 2022-11-10 20:22:09.000000000 +0000 @@ -252,7 +252,7 @@ static void provided_battery_property_ch DBusMessageIter *iter, void *user_data) { - uint8_t percentage; + uint8_t percentage = 0; const char *export_path; DBusMessageIter dev_iter; @@ -264,10 +264,12 @@ static void provided_battery_property_ch if (strcmp(name, "Percentage") != 0) return; - if (dbus_message_iter_get_arg_type(iter) != DBUS_TYPE_BYTE) - return; + if (iter) { + if (dbus_message_iter_get_arg_type(iter) != DBUS_TYPE_BYTE) + return; - dbus_message_iter_get_basic(iter, &percentage); + dbus_message_iter_get_basic(iter, &percentage); + } DBG("battery percentage changed on %s, percentage = %d", g_dbus_proxy_get_path(proxy), percentage); diff -pruN 5.65-1/src/btd.h 5.66-1/src/btd.h --- 5.65-1/src/btd.h 2022-07-24 20:59:52.000000000 +0000 +++ 5.66-1/src/btd.h 2022-11-10 20:22:09.000000000 +0000 @@ -144,6 +144,7 @@ void plugin_cleanup(void); void rfkill_init(void); void rfkill_exit(void); +int rfkill_get_blocked(uint16_t index); GKeyFile *btd_get_main_conf(void); bool btd_kernel_experimental_enabled(const char *uuid); diff -pruN 5.65-1/src/device.c 5.66-1/src/device.c --- 5.65-1/src/device.c 2022-07-24 20:59:52.000000000 +0000 +++ 5.66-1/src/device.c 2022-11-10 20:22:09.000000000 +0000 @@ -257,7 +257,7 @@ struct btd_device { sdp_list_t *tmp_records; - gboolean trusted; + bool trusted; gboolean blocked; gboolean auto_connect; gboolean disable_auto_connect; @@ -301,7 +301,7 @@ static bool get_initiator(struct btd_dev if (dev->bredr_state.connected) return dev->bredr_state.initiator; - return false; + return dev->att_io ? true : false; } static GSList *find_service_with_profile(GSList *list, struct btd_profile *p) @@ -826,7 +826,7 @@ bool device_is_bonded(struct btd_device return state->bonded; } -gboolean device_is_trusted(struct btd_device *device) +bool btd_device_is_trusted(struct btd_device *device) { return device->trusted; } @@ -1163,7 +1163,7 @@ static gboolean dev_property_get_trusted DBusMessageIter *iter, void *data) { struct btd_device *device = data; - gboolean val = device_is_trusted(device); + gboolean val = btd_device_is_trusted(device); dbus_message_iter_append_basic(iter, DBUS_TYPE_BOOLEAN, &val); @@ -3731,9 +3731,12 @@ static void device_add_gatt_services(str static void device_accept_gatt_profiles(struct btd_device *device) { GSList *l; + bool initiator = get_initiator(device); + + DBG("initiator %s", initiator ? "true" : "false"); for (l = device->services; l != NULL; l = g_slist_next(l)) - service_accept(l->data, get_initiator(device)); + service_accept(l->data, initiator); } static void device_remove_gatt_service(struct btd_device *device, @@ -4509,6 +4512,12 @@ GSList *btd_device_get_uuids(struct btd_ return device->uuids; } +bool btd_device_has_uuid(struct btd_device *device, const char *uuid) +{ + return g_slist_find_custom(device->uuids, uuid, + (GCompareFunc)strcasecmp); +} + struct probe_data { struct btd_device *dev; GSList *uuids; @@ -4574,7 +4583,8 @@ void device_probe_profile(gpointer a, gp device->services = g_slist_append(device->services, service); - if (!profile->auto_connect || !device->general_connect) + if (!profile->auto_connect || (!btd_device_is_connected(device) && + !device->general_connect)) return; device->pending = g_slist_append(device->pending, service); @@ -5361,6 +5371,9 @@ static void att_connect_cb(GIOChannel *i goto done; } + /* Update connected state */ + device->le_state.connected = true; + if (!device_attach_att(device, io)) goto done; @@ -5414,6 +5427,9 @@ int device_connect_le(struct btd_device DBG("Connection attempt to: %s", addr); + /* Set as initiator */ + dev->le_state.initiator = true; + if (dev->le_state.paired) sec_level = BT_IO_SEC_MEDIUM; else @@ -5451,8 +5467,6 @@ int device_connect_le(struct btd_device /* Keep this, so we can cancel the connection */ dev->att_io = io; - /* Set as initiator */ - dev->le_state.initiator = true; return 0; } @@ -6030,7 +6044,7 @@ void device_bonding_complete(struct btd_ * treated as a newly discovered device. */ if (!device_is_paired(device, bdaddr_type) && - !device_is_trusted(device)) + !btd_device_is_trusted(device)) btd_device_set_temporary(device, true); device_bonding_failed(device, status); @@ -6814,6 +6828,11 @@ uint32_t btd_device_get_current_flags(st return dev->current_flags; } +uint32_t btd_device_get_supported_flags(struct btd_device *dev) +{ + return dev->supported_flags; +} + /* This event is sent immediately after add device on all mgmt sockets. * Afterwards, it is only sent to mgmt sockets other than the one which called * set_device_flags. diff -pruN 5.65-1/src/device.h 5.66-1/src/device.h --- 5.65-1/src/device.h 2022-07-24 20:59:52.000000000 +0000 +++ 5.66-1/src/device.h 2022-11-10 20:22:09.000000000 +0000 @@ -54,6 +54,7 @@ struct device_addr_type { int device_addr_type_cmp(gconstpointer a, gconstpointer b); GSList *btd_device_get_uuids(struct btd_device *device); +bool btd_device_has_uuid(struct btd_device *device, const char *uuid); void device_probe_profiles(struct btd_device *device, GSList *profiles); void btd_device_set_record(struct btd_device *device, const char *uuid, @@ -88,7 +89,7 @@ gboolean device_is_temporary(struct btd_ bool device_is_connectable(struct btd_device *device); bool device_is_paired(struct btd_device *device, uint8_t bdaddr_type); bool device_is_bonded(struct btd_device *device, uint8_t bdaddr_type); -gboolean device_is_trusted(struct btd_device *device); +bool btd_device_is_trusted(struct btd_device *device); void device_set_paired(struct btd_device *dev, uint8_t bdaddr_type); void device_set_unpaired(struct btd_device *dev, uint8_t bdaddr_type); void btd_device_set_temporary(struct btd_device *device, bool temporary); @@ -176,6 +177,7 @@ int device_discover_services(struct btd_ int btd_device_connect_services(struct btd_device *dev, GSList *services); uint32_t btd_device_get_current_flags(struct btd_device *dev); +uint32_t btd_device_get_supported_flags(struct btd_device *dev); void btd_device_flags_changed(struct btd_device *dev, uint32_t supported_flags, uint32_t current_flags); diff -pruN 5.65-1/src/gatt-database.c 5.66-1/src/gatt-database.c --- 5.65-1/src/gatt-database.c 2022-07-24 20:59:52.000000000 +0000 +++ 5.66-1/src/gatt-database.c 2022-11-10 20:22:09.000000000 +0000 @@ -1238,7 +1238,8 @@ static void populate_gatt_service(struct NULL, NULL, database); database->svc_chngd_ccc = service_add_ccc(service, database, NULL, NULL, - 0, NULL); + BT_ATT_PERM_READ | + BT_ATT_PERM_WRITE, NULL); bt_uuid16_create(&uuid, GATT_CHARAC_CLI_FEAT); database->cli_feat = gatt_db_service_add_characteristic(service, @@ -1726,8 +1727,10 @@ static bool parse_chrc_flags(DBusMessage *perm |= BT_ATT_PERM_WRITE; } else if (!strcmp("notify", flag)) { *props |= BT_GATT_CHRC_PROP_NOTIFY; + *ccc_perm |= BT_ATT_PERM_WRITE; } else if (!strcmp("indicate", flag)) { *props |= BT_GATT_CHRC_PROP_INDICATE; + *ccc_perm |= BT_ATT_PERM_WRITE; } else if (!strcmp("authenticated-signed-writes", flag)) { *props |= BT_GATT_CHRC_PROP_AUTH; *perm |= BT_ATT_PERM_WRITE; @@ -2192,27 +2195,43 @@ static bool parse_handle(GDBusProxy *pro return true; } -static uint8_t dbus_error_to_att_ecode(const char *error_name, uint8_t perm_err) +static uint8_t dbus_error_to_att_ecode(const char *name, const char *msg, + uint8_t perm_err) { - if (strcmp(error_name, ERROR_INTERFACE ".Failed") == 0) - return 0x80; /* For now return this "application error" */ + if (strcmp(name, ERROR_INTERFACE ".Failed") == 0) { + char *endptr = NULL; + uint32_t ecode; + + ecode = strtol(msg, &endptr, 0); + + /* If message doesn't set an error code just use 0x80 */ + if (!endptr || *endptr != '\0') + return 0x80; + + if (ecode < 0x80 || ecode > 0x9f) { + error("Invalid error code: %s", msg); + return BT_ATT_ERROR_UNLIKELY; + } + + return ecode; + } - if (strcmp(error_name, ERROR_INTERFACE ".NotSupported") == 0) + if (strcmp(name, ERROR_INTERFACE ".NotSupported") == 0) return BT_ATT_ERROR_REQUEST_NOT_SUPPORTED; - if (strcmp(error_name, ERROR_INTERFACE ".NotAuthorized") == 0) + if (strcmp(name, ERROR_INTERFACE ".NotAuthorized") == 0) return BT_ATT_ERROR_AUTHORIZATION; - if (strcmp(error_name, ERROR_INTERFACE ".InvalidValueLength") == 0) + if (strcmp(name, ERROR_INTERFACE ".InvalidValueLength") == 0) return BT_ATT_ERROR_INVALID_ATTRIBUTE_VALUE_LEN; - if (strcmp(error_name, ERROR_INTERFACE ".InvalidOffset") == 0) + if (strcmp(name, ERROR_INTERFACE ".InvalidOffset") == 0) return BT_ATT_ERROR_INVALID_OFFSET; - if (strcmp(error_name, ERROR_INTERFACE ".InProgress") == 0) + if (strcmp(name, ERROR_INTERFACE ".InProgress") == 0) return BT_ERROR_ALREADY_IN_PROGRESS; - if (strcmp(error_name, ERROR_INTERFACE ".NotPermitted") == 0) + if (strcmp(name, ERROR_INTERFACE ".NotPermitted") == 0) return perm_err; return BT_ATT_ERROR_UNLIKELY; @@ -2236,7 +2255,7 @@ static void read_reply_cb(DBusMessage *m if (dbus_set_error_from_message(&err, message) == TRUE) { DBG("Failed to read value: %s: %s", err.name, err.message); - ecode = dbus_error_to_att_ecode(err.name, + ecode = dbus_error_to_att_ecode(err.name, err.message, BT_ATT_ERROR_READ_NOT_PERMITTED); dbus_error_free(&err); goto done; @@ -2415,7 +2434,7 @@ static void write_reply_cb(DBusMessage * if (dbus_set_error_from_message(&err, message) == TRUE) { DBG("Failed to write value: %s: %s", err.name, err.message); - ecode = dbus_error_to_att_ecode(err.name, + ecode = dbus_error_to_att_ecode(err.name, err.message, BT_ATT_ERROR_WRITE_NOT_PERMITTED); dbus_error_free(&err); goto done; @@ -2610,7 +2629,7 @@ static void acquire_write_reply(DBusMess error("Failed to acquire write: %s\n", err.name); - ecode = dbus_error_to_att_ecode(err.name, + ecode = dbus_error_to_att_ecode(err.name, err.message, BT_ATT_ERROR_WRITE_NOT_PERMITTED); dbus_error_free(&err); @@ -2848,17 +2867,19 @@ static void property_changed_cb(GDBusPro if (strcmp(name, "Value")) return; - if (dbus_message_iter_get_arg_type(iter) != DBUS_TYPE_ARRAY) { - DBG("Malformed \"Value\" property received"); - return; - } + if (iter) { + if (dbus_message_iter_get_arg_type(iter) != DBUS_TYPE_ARRAY) { + DBG("Malformed \"Value\" property received"); + return; + } - dbus_message_iter_recurse(iter, &array); - dbus_message_iter_get_fixed_array(&array, &value, &len); + dbus_message_iter_recurse(iter, &array); + dbus_message_iter_get_fixed_array(&array, &value, &len); - if (len < 0) { - DBG("Malformed \"Value\" property received"); - return; + if (len < 0) { + DBG("Malformed \"Value\" property received"); + return; + } } /* Truncate the value if it's too large */ @@ -2880,6 +2901,9 @@ static bool database_add_ccc(struct exte !(chrc->props & BT_GATT_CHRC_PROP_INDICATE)) return true; + /* Always set read/write permissions */ + chrc->ccc_perm |= BT_ATT_PERM_WRITE | BT_ATT_PERM_READ; + chrc->ccc = service_add_ccc(service->attrib, service->app->database, ccc_write_cb, chrc, chrc->ccc_perm, NULL); if (!chrc->ccc) { @@ -2989,7 +3013,7 @@ static void desc_write_cb(struct gatt_db } if (opcode == BT_ATT_OP_PREP_WRITE_REQ) { - if (!device_is_trusted(device) && !desc->prep_authorized && + if (!btd_device_is_trusted(device) && !desc->prep_authorized && desc->req_prep_authorization) send_write(att, attrib, desc->proxy, desc->pending_writes, id, value, len, @@ -3120,7 +3144,7 @@ static void chrc_write_cb(struct gatt_db queue = NULL; if (opcode == BT_ATT_OP_PREP_WRITE_REQ) { - if (!device_is_trusted(device) && !chrc->prep_authorized && + if (!btd_device_is_trusted(device) && !chrc->prep_authorized && chrc->req_prep_authorization) send_write(att, attrib, chrc->proxy, queue, id, value, len, offset, @@ -3516,8 +3540,8 @@ static void register_characteristic(void { struct gatt_app *app = user_data; GDBusProxy *proxy = data; - const char *iface = g_dbus_proxy_get_interface(proxy); - const char *path = g_dbus_proxy_get_path(proxy); + const char *iface; + const char *path; if (app->failed) return; diff -pruN 5.65-1/src/rfkill.c 5.66-1/src/rfkill.c --- 5.65-1/src/rfkill.c 2022-07-24 20:59:52.000000000 +0000 +++ 5.66-1/src/rfkill.c 2022-11-10 20:22:09.000000000 +0000 @@ -55,12 +55,71 @@ struct rfkill_event { }; #define RFKILL_EVENT_SIZE_V1 8 +static int get_adapter_id_for_rfkill(int rfkill_id) +{ + char sysname[PATH_MAX]; + int namefd; + + snprintf(sysname, sizeof(sysname) - 1, + "/sys/class/rfkill/rfkill%u/name", rfkill_id); + + namefd = open(sysname, O_RDONLY); + if (namefd < 0) + return -1; + + memset(sysname, 0, sizeof(sysname)); + + if (read(namefd, sysname, sizeof(sysname) - 1) < 4) { + close(namefd); + return -1; + } + + close(namefd); + + if (g_str_has_prefix(sysname, "hci") == FALSE) + return -1; + + return atoi(sysname + 3); +} + +int rfkill_get_blocked(uint16_t index) +{ + int fd; + int blocked = -1; + + fd = open("/dev/rfkill", O_RDWR); + if (fd < 0) { + DBG("Failed to open RFKILL control device"); + return -1; + } + + while (1) { + struct rfkill_event event = { 0 }; + int id; + ssize_t len; + + len = read(fd, &event, sizeof(event)); + if (len < RFKILL_EVENT_SIZE_V1) + break; + + id = get_adapter_id_for_rfkill(event.idx); + + if (index == id) { + blocked = event.soft || event.hard; + break; + } + } + close(fd); + + return blocked; +} + static gboolean rfkill_event(GIOChannel *chan, GIOCondition cond, gpointer data) { struct rfkill_event event = { 0 }; struct btd_adapter *adapter; - char sysname[PATH_MAX]; + bool blocked = false; ssize_t len; int fd, id; @@ -84,7 +143,7 @@ static gboolean rfkill_event(GIOChannel event.soft, event.hard); if (event.soft || event.hard) - return TRUE; + blocked = true; if (event.op != RFKILL_OP_CHANGE) return TRUE; @@ -93,26 +152,7 @@ static gboolean rfkill_event(GIOChannel event.type != RFKILL_TYPE_ALL) return TRUE; - snprintf(sysname, sizeof(sysname) - 1, - "/sys/class/rfkill/rfkill%u/name", event.idx); - - fd = open(sysname, O_RDONLY); - if (fd < 0) - return TRUE; - - memset(sysname, 0, sizeof(sysname)); - - if (read(fd, sysname, sizeof(sysname) - 1) < 4) { - close(fd); - return TRUE; - } - - close(fd); - - if (g_str_has_prefix(sysname, "hci") == FALSE) - return TRUE; - - id = atoi(sysname + 3); + id = get_adapter_id_for_rfkill(event.idx); if (id < 0) return TRUE; @@ -122,7 +162,10 @@ static gboolean rfkill_event(GIOChannel DBG("RFKILL unblock for hci%d", id); - btd_adapter_restore_powered(adapter); + if (blocked) + btd_adapter_set_blocked(adapter); + else + btd_adapter_restore_powered(adapter); return TRUE; } diff -pruN 5.65-1/src/settings.c 5.66-1/src/settings.c --- 5.65-1/src/settings.c 2022-07-24 20:59:52.000000000 +0000 +++ 5.66-1/src/settings.c 2022-11-10 20:22:09.000000000 +0000 @@ -269,6 +269,7 @@ static int gatt_db_load(struct gatt_db * &primary, &uuid); bt_uuid_to_string(&uuid, uuid_str, sizeof(uuid_str)); + ret = 0; } else if (g_str_equal(type, GATT_INCLUDE_UUID_STR)) { ret = load_incl(db, *handle, value, current_service); } else if (g_str_equal(type, GATT_CHARAC_UUID_STR)) { diff -pruN 5.65-1/src/shared/ascs.h 5.66-1/src/shared/ascs.h --- 5.65-1/src/shared/ascs.h 1970-01-01 00:00:00.000000000 +0000 +++ 5.66-1/src/shared/ascs.h 2022-11-10 20:22:09.000000000 +0000 @@ -0,0 +1,196 @@ +/* SPDX-License-Identifier: LGPL-2.1-or-later */ +/* + * + * BlueZ - Bluetooth protocol stack for Linux + * + * Copyright (C) 2022 Intel Corporation. All rights reserved. + * + */ + +/* Response Status Code */ +#define BT_ASCS_RSP_SUCCESS 0x00 +#define BT_ASCS_RSP_NOT_SUPPORTED 0x01 +#define BT_ASCS_RSP_TRUNCATED 0x02 +#define BT_ASCS_RSP_INVALID_ASE 0x03 +#define BT_ASCS_RSP_INVALID_ASE_STATE 0x04 +#define BT_ASCS_RSP_INVALID_DIR 0x05 +#define BT_ASCS_RSP_CAP_UNSUPPORTED 0x06 +#define BT_ASCS_RSP_CONF_UNSUPPORTED 0x07 +#define BT_ASCS_RSP_CONF_REJECTED 0x08 +#define BT_ASCS_RSP_CONF_INVALID 0x09 +#define BT_ASCS_RSP_METADATA_UNSUPPORTED 0x0a +#define BT_ASCS_RSP_METADATA_REJECTED 0x0b +#define BT_ASCS_RSP_METADATA_INVALID 0x0c +#define BT_ASCS_RSP_NO_MEM 0x0d +#define BT_ASCS_RSP_UNSPECIFIED 0x0e + +/* Response Reasons */ +#define BT_ASCS_REASON_NONE 0x00 +#define BT_ASCS_REASON_CODEC 0x01 +#define BT_ASCS_REASON_CODEC_DATA 0x02 +#define BT_ASCS_REASON_INTERVAL 0x03 +#define BT_ASCS_REASON_FRAMING 0x04 +#define BT_ASCS_REASON_PHY 0x05 +#define BT_ASCS_REASON_SDU 0x06 +#define BT_ASCS_REASON_RTN 0x07 +#define BT_ASCS_REASON_LATENCY 0x08 +#define BT_ASCS_REASON_PD 0x09 +#define BT_ASCS_REASON_CIS 0x0a + +/* Transport QoS Packing */ +#define BT_ASCS_QOS_PACKING_SEQ 0x00 +#define BT_ASCS_QOS_PACKING_INT 0x01 + +/* Transport QoS Framing */ +#define BT_ASCS_QOS_FRAMING_UNFRAMED 0x00 +#define BT_ASCS_QOS_FRAMING_FRAMED 0x01 + +/* ASE characteristic states */ +#define BT_ASCS_ASE_STATE_IDLE 0x00 +#define BT_ASCS_ASE_STATE_CONFIG 0x01 +#define BT_ASCS_ASE_STATE_QOS 0x02 +#define BT_ASCS_ASE_STATE_ENABLING 0x03 +#define BT_ASCS_ASE_STATE_STREAMING 0x04 +#define BT_ASCS_ASE_STATE_DISABLING 0x05 +#define BT_ASCS_ASE_STATE_RELEASING 0x06 + +struct bt_ascs_ase_rsp { + uint8_t ase; + uint8_t code; + uint8_t reason; +} __packed; + +struct bt_ascs_cp_rsp { + uint8_t op; + uint8_t num_ase; + struct bt_ascs_ase_rsp rsp[0]; +} __packed; + +struct bt_ascs_ase_status { + uint8_t id; + uint8_t state; + uint8_t params[0]; +} __packed; + +/* ASE_State = 0x01 (Codec Configured), defined in Table 4.7. */ +struct bt_ascs_ase_status_config { + uint8_t framing; + uint8_t phy; + uint8_t rtn; + uint16_t latency; + uint8_t pd_min[3]; + uint8_t pd_max[3]; + uint8_t ppd_min[3]; + uint8_t ppd_max[3]; + struct bt_bap_codec codec; + uint8_t cc_len; + /* LTV-formatted Codec-Specific Configuration */ + struct bt_ltv cc[0]; +} __packed; + +/* ASE_State = 0x02 (QoS Configured), defined in Table 4.8. */ +struct bt_ascs_ase_status_qos { + uint8_t cig_id; + uint8_t cis_id; + uint8_t interval[3]; + uint8_t framing; + uint8_t phy; + uint16_t sdu; + uint8_t rtn; + uint16_t latency; + uint8_t pd[3]; +} __packed; + +/* ASE_Status = 0x03 (Enabling), 0x04 (Streaming), or 0x05 (Disabling) + * defined in Table 4.9. + */ +struct bt_ascs_ase_status_metadata { + uint8_t cig_id; + uint8_t cis_id; + uint8_t len; + uint8_t data[0]; +} __packed; + +struct bt_ascs_ase_hdr { + uint8_t op; + uint8_t num; +} __packed; + +#define BT_ASCS_CONFIG 0x01 + +#define BT_ASCS_CONFIG_LATENCY_LOW 0x01 +#define BT_ASCS_CONFIG_LATENCY_MEDIUM 0x02 +#define BT_ASCS_CONFIG_LATENCY_HIGH 0x03 + +#define BT_ASCS_CONFIG_PHY_LE_1M 0x01 +#define BT_ASCS_CONFIG_PHY_LE_2M 0x02 +#define BT_ASCS_CONFIG_PHY_LE_CODED 0x03 + +struct bt_ascs_codec_config { + uint8_t len; + uint8_t type; + uint8_t data[0]; +} __packed; + +struct bt_ascs_config { + uint8_t ase; /* ASE ID */ + uint8_t latency; /* Target Latency */ + uint8_t phy; /* Target PHY */ + struct bt_bap_codec codec; /* Codec ID */ + uint8_t cc_len; /* Codec Specific Config Length */ + /* LTV-formatted Codec-Specific Configuration */ + struct bt_ascs_codec_config cc[0]; +} __packed; + +#define BT_ASCS_QOS 0x02 + +struct bt_ascs_qos { + uint8_t ase; /* ASE ID */ + uint8_t cig; /* CIG ID*/ + uint8_t cis; /* CIG ID*/ + uint8_t interval[3]; /* Frame interval */ + uint8_t framing; /* Frame framing */ + uint8_t phy; /* PHY */ + uint16_t sdu; /* Maximum SDU Size */ + uint8_t rtn; /* Retransmission Effort */ + uint16_t latency; /* Transport Latency */ + uint8_t pd[3]; /* Presentation Delay */ +} __packed; + +#define BT_ASCS_ENABLE 0x03 + +struct bt_ascs_metadata { + uint8_t ase; /* ASE ID */ + uint8_t len; /* Metadata length */ + uint8_t data[0]; /* LTV-formatted Metadata */ +} __packed; + +struct bt_ascs_enable { + struct bt_ascs_metadata meta; /* Metadata */ +} __packed; + +#define BT_ASCS_START 0x04 + +struct bt_ascs_start { + uint8_t ase; /* ASE ID */ +} __packed; + +#define BT_ASCS_DISABLE 0x05 + +struct bt_ascs_disable { + uint8_t ase; /* ASE ID */ +} __packed; + +#define BT_ASCS_STOP 0x06 + +struct bt_ascs_stop { + uint8_t ase; /* ASE ID */ +} __packed; + +#define BT_ASCS_METADATA 0x07 + +#define BT_ASCS_RELEASE 0x08 + +struct bt_ascs_release { + uint8_t ase; /* ASE ID */ +} __packed; diff -pruN 5.65-1/src/shared/bap.c 5.66-1/src/shared/bap.c --- 5.65-1/src/shared/bap.c 1970-01-01 00:00:00.000000000 +0000 +++ 5.66-1/src/shared/bap.c 2022-11-10 20:22:09.000000000 +0000 @@ -0,0 +1,4801 @@ +// SPDX-License-Identifier: LGPL-2.1-or-later +/* + * + * BlueZ - Bluetooth protocol stack for Linux + * + * Copyright (C) 2022 Intel Corporation. All rights reserved. + * + */ + +#define _GNU_SOURCE +#include +#include +#include +#include +#include +#include + +#include "lib/bluetooth.h" +#include "lib/uuid.h" + +#include "src/shared/io.h" +#include "src/shared/queue.h" +#include "src/shared/util.h" +#include "src/shared/timeout.h" +#include "src/shared/att.h" +#include "src/shared/gatt-db.h" +#include "src/shared/gatt-server.h" +#include "src/shared/gatt-client.h" +#include "src/shared/bap.h" +#include "src/shared/ascs.h" + +/* Maximum number of ASE(s) */ +#define NUM_SINKS 2 +#define NUM_SOURCE 2 +#define NUM_ASES (NUM_SINKS + NUM_SOURCE) +#define ASE_UUID(_id) (_id < NUM_SINKS ? ASE_SINK_UUID : ASE_SOURCE_UUID) +#define DBG(_bap, fmt, arg...) \ + bap_debug(_bap, "%s:%s() " fmt, __FILE__, __func__, ## arg) + +#define LTV(_type, _bytes...) \ + { \ + .len = 1 + sizeof((uint8_t []) { _bytes }), \ + .type = _type, \ + .data = { _bytes }, \ + } + +#define BAP_PROCESS_TIMEOUT 10 + +struct bt_bap_pac_changed { + bt_bap_pac_func_t added; + bt_bap_pac_func_t removed; + bt_bap_destroy_func_t destroy; + void *data; +}; + +struct bt_bap_ready { + unsigned int id; + bt_bap_ready_func_t func; + bt_bap_destroy_func_t destroy; + void *data; +}; + +struct bt_bap_state { + unsigned int id; + bt_bap_state_func_t func; + bt_bap_connecting_func_t connecting; + bt_bap_destroy_func_t destroy; + void *data; +}; + +struct bt_bap_cb { + unsigned int id; + bt_bap_func_t attached; + bt_bap_func_t detached; + void *user_data; +}; + +struct bt_pacs { + struct bt_bap_db *bdb; + struct gatt_db_attribute *service; + struct gatt_db_attribute *sink; + struct gatt_db_attribute *sink_ccc; + struct gatt_db_attribute *sink_loc; + struct gatt_db_attribute *sink_loc_ccc; + struct gatt_db_attribute *source; + struct gatt_db_attribute *source_ccc; + struct gatt_db_attribute *source_loc; + struct gatt_db_attribute *source_loc_ccc; + struct gatt_db_attribute *context; + struct gatt_db_attribute *context_ccc; + struct gatt_db_attribute *supported_context; + struct gatt_db_attribute *supported_context_ccc; +}; + +struct bt_ase { + struct bt_ascs *ascs; + uint8_t id; + struct gatt_db_attribute *attr; + struct gatt_db_attribute *ccc; +}; + +struct bt_ascs { + struct bt_bap_db *bdb; + struct gatt_db_attribute *service; + struct bt_ase *ase[NUM_ASES]; + struct gatt_db_attribute *ase_cp; + struct gatt_db_attribute *ase_cp_ccc; +}; + +struct bt_bap_db { + struct gatt_db *db; + struct bt_pacs *pacs; + struct bt_ascs *ascs; + struct queue *sinks; + struct queue *sources; + struct queue *endpoints; +}; + +struct bt_bap_req { + unsigned int id; + struct bt_bap_stream *stream; + uint8_t op; + struct queue *group; + struct iovec *iov; + size_t len; + bt_bap_stream_func_t func; + void *user_data; +}; + +typedef void (*bap_func_t)(struct bt_bap *bap, bool success, uint8_t att_ecode, + const uint8_t *value, uint16_t length, + void *user_data); + +struct bt_bap_pending { + unsigned int id; + struct bt_bap *bap; + bap_func_t func; + void *user_data; +}; + +typedef void (*bap_notify_t)(struct bt_bap *bap, uint16_t value_handle, + const uint8_t *value, uint16_t length, + void *user_data); + +struct bt_bap_notify { + unsigned int id; + struct bt_bap *bap; + bap_notify_t func; + void *user_data; +}; + +struct bt_bap { + int ref_count; + struct bt_bap_db *ldb; + struct bt_bap_db *rdb; + struct bt_gatt_client *client; + struct bt_att *att; + struct bt_bap_req *req; + unsigned int cp_id; + + unsigned int process_id; + unsigned int disconn_id; + struct queue *reqs; + struct queue *pending; + struct queue *notify; + struct queue *streams; + + struct queue *ready_cbs; + struct queue *state_cbs; + + bt_bap_debug_func_t debug_func; + bt_bap_destroy_func_t debug_destroy; + void *debug_data; + void *user_data; +}; + +struct bt_bap_pac { + struct bt_bap_db *bdb; + char *name; + uint8_t type; + uint32_t locations; + uint16_t contexts; + struct bt_bap_codec codec; + struct bt_bap_pac_qos qos; + struct iovec *data; + struct iovec *metadata; + struct bt_bap_pac_ops *ops; + void *user_data; +}; + +struct bt_bap_endpoint { + struct bt_bap_db *bdb; + struct bt_bap_stream *stream; + struct gatt_db_attribute *attr; + uint8_t id; + uint8_t dir; + uint8_t old_state; + uint8_t state; + unsigned int state_id; +}; + +struct bt_bap_stream_io { + struct bt_bap *bap; + int ref_count; + struct io *io; + bool connecting; +}; + +struct bt_bap_stream { + struct bt_bap *bap; + struct bt_bap_endpoint *ep; + struct queue *pacs; + struct bt_bap_pac *lpac; + struct bt_bap_pac *rpac; + struct iovec *cc; + struct iovec *meta; + struct bt_bap_qos qos; + struct queue *links; + struct bt_bap_stream_io *io; + bool client; + void *user_data; +}; + +/* TODO: Figure out the capabilities types */ +#define BT_CODEC_CAP_PARAMS 0x01 +#define BT_CODEC_CAP_DRM 0x0a +#define BT_CODEC_CAP_DRM_VALUE 0x0b + +struct bt_pac_metadata { + uint8_t len; + uint8_t data[0]; +} __packed; + +struct bt_pac { + struct bt_bap_codec codec; /* Codec ID */ + uint8_t cc_len; /* Codec Capabilities Length */ + struct bt_ltv cc[0]; /* Codec Specific Capabilities */ + struct bt_pac_metadata meta[0]; /* Metadata */ +} __packed; + +struct bt_pacs_read_rsp { + uint8_t num_pac; + struct bt_pac pac[0]; +} __packed; + +struct bt_pacs_context { + uint16_t snk; + uint16_t src; +} __packed; + +/* Contains local bt_bap_db */ +static struct queue *bap_db; +static struct queue *pac_cbs; +static struct queue *bap_cbs; +static struct queue *sessions; + +static bool bap_db_match(const void *data, const void *match_data) +{ + const struct bt_bap_db *bdb = data; + const struct gatt_db *db = match_data; + + return (bdb->db == db); +} + +static void *iov_add(struct iovec *iov, size_t len) +{ + void *data; + + data = iov->iov_base + iov->iov_len; + iov->iov_len += len; + + return data; +} + +static void *iov_add_mem(struct iovec *iov, size_t len, const void *d) +{ + void *data; + + data = iov->iov_base + iov->iov_len; + iov->iov_len += len; + + memcpy(data, d, len); + + return data; +} + +static void iov_free(void *data) +{ + struct iovec *iov = data; + + if (!iov) + return; + + free(iov->iov_base); + free(iov); +} + +static void iov_memcpy(struct iovec *iov, void *src, size_t len) +{ + iov->iov_base = realloc(iov->iov_base, len); + iov->iov_len = len; + memcpy(iov->iov_base, src, len); +} + +static int iov_memcmp(struct iovec *iov1, struct iovec *iov2) +{ + if (!iov1) + return 1; + + if (!iov2) + return -1; + + if (iov1->iov_len != iov2->iov_len) + return iov1->iov_len - iov2->iov_len; + + return memcmp(iov1->iov_base, iov2->iov_base, iov1->iov_len); +} + +static struct iovec *iov_dup(struct iovec *iov, size_t len) +{ + struct iovec *dup; + size_t i; + + if (!iov) + return NULL; + + dup = new0(struct iovec, len); + + for (i = 0; i < len; i++) + iov_memcpy(&dup[i], iov[i].iov_base, iov[i].iov_len); + + return dup; +} + +unsigned int bt_bap_pac_register(bt_bap_pac_func_t added, + bt_bap_pac_func_t removed, void *user_data, + bt_bap_destroy_func_t destroy) +{ + struct bt_bap_pac_changed *changed; + + changed = new0(struct bt_bap_pac_changed, 1); + changed->added = added; + changed->removed = removed; + changed->destroy = destroy; + changed->data = user_data; + + if (!pac_cbs) + pac_cbs = queue_new(); + + queue_push_tail(pac_cbs, changed); + + return queue_length(pac_cbs); +} + +static void pac_changed_free(void *data) +{ + struct bt_bap_pac_changed *changed = data; + + if (changed->destroy) + changed->destroy(changed->data); + + free(changed); +} + +struct match_pac_id { + unsigned int id; + unsigned int index; +}; + +static bool match_index(const void *data, const void *match_data) +{ + struct match_pac_id *match = (void *)match_data; + + match->index++; + + return match->id == match->index; +} + +bool bt_bap_pac_unregister(unsigned int id) +{ + struct bt_bap_pac_changed *changed; + struct match_pac_id match; + + memset(&match, 0, sizeof(match)); + match.id = id; + + changed = queue_remove_if(pac_cbs, match_index, &match); + if (!changed) + return false; + + pac_changed_free(changed); + + if (queue_isempty(pac_cbs)) { + queue_destroy(pac_cbs, NULL); + pac_cbs = NULL; + } + + return true; +} + +static void pac_foreach(void *data, void *user_data) +{ + struct bt_bap_pac *pac = data; + struct iovec *iov = user_data; + struct bt_pacs_read_rsp *rsp; + struct bt_pac *p; + struct bt_pac_metadata *meta; + + if (!iov->iov_len) { + rsp = iov_add(iov, sizeof(*rsp)); + rsp->num_pac = 0; + } else + rsp = iov->iov_base; + + rsp->num_pac++; + + p = iov_add(iov, sizeof(*p)); + p->codec.id = pac->codec.id; + + if (pac->data) { + p->cc_len = pac->data->iov_len; + iov_add_mem(iov, p->cc_len, pac->data->iov_base); + } else + p->cc_len = 0; + + meta = iov_add(iov, sizeof(*meta)); + + if (pac->metadata) { + meta->len = pac->metadata->iov_len; + iov_add_mem(iov, meta->len, pac->metadata->iov_base); + } else + meta->len = 0; +} + +static void pacs_sink_read(struct gatt_db_attribute *attrib, + unsigned int id, uint16_t offset, + uint8_t opcode, struct bt_att *att, + void *user_data) +{ + struct bt_pacs *pacs = user_data; + struct bt_bap_db *bdb = pacs->bdb; + struct iovec iov; + uint8_t value[512]; + + memset(value, 0, sizeof(value)); + + iov.iov_base = value; + iov.iov_len = 0; + + queue_foreach(bdb->sinks, pac_foreach, &iov); + + gatt_db_attribute_read_result(attrib, id, 0, iov.iov_base, + iov.iov_len); +} + +static void pacs_sink_loc_read(struct gatt_db_attribute *attrib, + unsigned int id, uint16_t offset, + uint8_t opcode, struct bt_att *att, + void *user_data) +{ + uint32_t value = 0x00000003; + + gatt_db_attribute_read_result(attrib, id, 0, (void *) &value, + sizeof(value)); +} + +static void pacs_source_read(struct gatt_db_attribute *attrib, + unsigned int id, uint16_t offset, + uint8_t opcode, struct bt_att *att, + void *user_data) +{ + struct bt_pacs *pacs = user_data; + struct bt_bap_db *bdb = pacs->bdb; + struct iovec iov; + uint8_t value[512]; + + memset(value, 0, sizeof(value)); + + iov.iov_base = value; + iov.iov_len = 0; + + queue_foreach(bdb->sources, pac_foreach, &iov); + + gatt_db_attribute_read_result(attrib, id, 0, iov.iov_base, + iov.iov_len); +} + +static void pacs_source_loc_read(struct gatt_db_attribute *attrib, + unsigned int id, uint16_t offset, + uint8_t opcode, struct bt_att *att, + void *user_data) +{ + uint32_t value = 0x00000001; + + gatt_db_attribute_read_result(attrib, id, 0, (void *) &value, + sizeof(value)); +} + +static void pacs_context_read(struct gatt_db_attribute *attrib, + unsigned int id, uint16_t offset, + uint8_t opcode, struct bt_att *att, + void *user_data) +{ + struct bt_pacs_context ctx = { + .snk = 0x0fff, + .src = 0x000e + }; + + gatt_db_attribute_read_result(attrib, id, 0, (void *) &ctx, + sizeof(ctx)); +} + +static void pacs_supported_context_read(struct gatt_db_attribute *attrib, + unsigned int id, uint16_t offset, + uint8_t opcode, struct bt_att *att, + void *user_data) +{ + struct bt_pacs_context ctx = { + .snk = 0x0fff, + .src = 0x000e + }; + + gatt_db_attribute_read_result(attrib, id, 0, (void *) &ctx, + sizeof(ctx)); +} + +static struct bt_pacs *pacs_new(struct gatt_db *db) +{ + struct bt_pacs *pacs; + bt_uuid_t uuid; + + if (!db) + return NULL; + + pacs = new0(struct bt_pacs, 1); + + /* Populate DB with PACS attributes */ + bt_uuid16_create(&uuid, PACS_UUID); + pacs->service = gatt_db_add_service(db, &uuid, true, 19); + + bt_uuid16_create(&uuid, PAC_SINK_CHRC_UUID); + pacs->sink = gatt_db_service_add_characteristic(pacs->service, &uuid, + BT_ATT_PERM_READ, + BT_GATT_CHRC_PROP_READ | + BT_GATT_CHRC_PROP_NOTIFY, + pacs_sink_read, NULL, + pacs); + + pacs->sink_ccc = gatt_db_service_add_ccc(pacs->service, + BT_ATT_PERM_READ | BT_ATT_PERM_WRITE); + + bt_uuid16_create(&uuid, PAC_SINK_LOC_CHRC_UUID); + pacs->sink_loc = gatt_db_service_add_characteristic(pacs->service, + &uuid, BT_ATT_PERM_READ, + BT_GATT_CHRC_PROP_READ | + BT_GATT_CHRC_PROP_NOTIFY, + pacs_sink_loc_read, NULL, + pacs); + + pacs->sink_loc_ccc = gatt_db_service_add_ccc(pacs->service, + BT_ATT_PERM_READ | BT_ATT_PERM_WRITE); + + bt_uuid16_create(&uuid, PAC_SOURCE_CHRC_UUID); + pacs->sink = gatt_db_service_add_characteristic(pacs->service, &uuid, + BT_ATT_PERM_READ, + BT_GATT_CHRC_PROP_READ | + BT_GATT_CHRC_PROP_NOTIFY, + pacs_source_read, NULL, + pacs); + + pacs->sink_ccc = gatt_db_service_add_ccc(pacs->service, + BT_ATT_PERM_READ | BT_ATT_PERM_WRITE); + + bt_uuid16_create(&uuid, PAC_SOURCE_LOC_CHRC_UUID); + pacs->source_loc = gatt_db_service_add_characteristic(pacs->service, + &uuid, BT_ATT_PERM_READ, + BT_GATT_CHRC_PROP_READ | + BT_GATT_CHRC_PROP_NOTIFY, + pacs_source_loc_read, NULL, + pacs); + + pacs->source_loc_ccc = gatt_db_service_add_ccc(pacs->service, + BT_ATT_PERM_READ | BT_ATT_PERM_WRITE); + + bt_uuid16_create(&uuid, PAC_CONTEXT); + pacs->context = gatt_db_service_add_characteristic(pacs->service, + &uuid, BT_ATT_PERM_READ, + BT_GATT_CHRC_PROP_READ | + BT_GATT_CHRC_PROP_NOTIFY, + pacs_context_read, NULL, pacs); + + pacs->context_ccc = gatt_db_service_add_ccc(pacs->service, + BT_ATT_PERM_READ | BT_ATT_PERM_WRITE); + + bt_uuid16_create(&uuid, PAC_SUPPORTED_CONTEXT); + pacs->supported_context = + gatt_db_service_add_characteristic(pacs->service, &uuid, + BT_ATT_PERM_READ, + BT_GATT_CHRC_PROP_READ | + BT_GATT_CHRC_PROP_NOTIFY, + pacs_supported_context_read, NULL, + pacs); + + pacs->supported_context_ccc = gatt_db_service_add_ccc(pacs->service, + BT_ATT_PERM_READ | BT_ATT_PERM_WRITE); + + gatt_db_service_set_active(pacs->service, true); + + return pacs; +} + +static void bap_debug(struct bt_bap *bap, const char *format, ...) +{ + va_list ap; + + if (!bap || !format || !bap->debug_func) + return; + + va_start(ap, format); + util_debug_va(bap->debug_func, bap->debug_data, format, ap); + va_end(ap); +} + +static void bap_disconnected(int err, void *user_data) +{ + struct bt_bap *bap = user_data; + + bap->disconn_id = 0; + + DBG(bap, "bap %p disconnected err %d", bap, err); + + bt_bap_detach(bap); +} + +static struct bt_bap *bap_get_session(struct bt_att *att, struct gatt_db *db) +{ + const struct queue_entry *entry; + struct bt_bap *bap; + + for (entry = queue_get_entries(sessions); entry; entry = entry->next) { + struct bt_bap *bap = entry->data; + + if (att == bt_bap_get_att(bap)) + return bap; + } + + bap = bt_bap_new(db, NULL); + bap->att = att; + + bt_bap_attach(bap, NULL); + + return bap; +} + +static bool bap_endpoint_match(const void *data, const void *match_data) +{ + const struct bt_bap_endpoint *ep = data; + const struct gatt_db_attribute *attr = match_data; + + return (ep->attr == attr); +} + +static struct bt_bap_endpoint *bap_endpoint_new(struct bt_bap_db *bdb, + struct gatt_db_attribute *attr) +{ + struct bt_bap_endpoint *ep; + bt_uuid_t uuid, source, sink; + + if (!gatt_db_attribute_get_char_data(attr, NULL, NULL, NULL, NULL, + &uuid)) + return NULL; + + ep = new0(struct bt_bap_endpoint, 1); + ep->bdb = bdb; + ep->attr = attr; + + bt_uuid16_create(&source, ASE_SOURCE_UUID); + bt_uuid16_create(&sink, ASE_SINK_UUID); + + if (!bt_uuid_cmp(&source, &uuid)) + ep->dir = BT_BAP_SOURCE; + else if (!bt_uuid_cmp(&sink, &uuid)) + ep->dir = BT_BAP_SINK; + + return ep; +} + +static struct bt_bap_endpoint *bap_get_endpoint(struct bt_bap_db *db, + struct gatt_db_attribute *attr) +{ + struct bt_bap_endpoint *ep; + + if (!db || !attr) + return NULL; + + ep = queue_find(db->endpoints, bap_endpoint_match, attr); + if (ep) + return ep; + + ep = bap_endpoint_new(db, attr); + if (!ep) + return NULL; + + queue_push_tail(db->endpoints, ep); + + return ep; +} + +static bool bap_endpoint_match_id(const void *data, const void *match_data) +{ + const struct bt_bap_endpoint *ep = data; + uint8_t id = PTR_TO_UINT(match_data); + + return (ep->id == id); +} + +static struct bt_bap_endpoint *bap_get_endpoint_id(struct bt_bap *bap, + struct bt_bap_db *db, + uint8_t id) +{ + struct bt_bap_endpoint *ep; + struct gatt_db_attribute *attr = NULL; + size_t i; + + if (!bap || !db) + return NULL; + + ep = queue_find(db->endpoints, bap_endpoint_match_id, UINT_TO_PTR(id)); + if (ep) + return ep; + + for (i = 0; i < ARRAY_SIZE(db->ascs->ase); i++) { + struct bt_ase *ase = db->ascs->ase[i]; + + if (id) { + if (ase->id != id) + continue; + attr = ase->attr; + break; + } + + ep = queue_find(db->endpoints, bap_endpoint_match, ase->attr); + if (!ep) { + attr = ase->attr; + break; + } + } + + if (!attr) + return NULL; + + ep = bap_endpoint_new(db, attr); + if (!ep) + return NULL; + + ep->id = id; + queue_push_tail(db->endpoints, ep); + + return ep; +} + +static void ascs_ase_read(struct gatt_db_attribute *attrib, + unsigned int id, uint16_t offset, + uint8_t opcode, struct bt_att *att, + void *user_data) +{ + struct bt_ase *ase = user_data; + struct bt_bap *bap = bap_get_session(att, ase->ascs->bdb->db); + struct bt_bap_endpoint *ep = bap_get_endpoint(bap->ldb, attrib); + struct bt_ascs_ase_status rsp; + + if (!ase || !bap || !ep) { + gatt_db_attribute_read_result(attrib, id, BT_ATT_ERROR_UNLIKELY, + NULL, 0); + return; + } + + memset(&rsp, 0, sizeof(rsp)); + + /* Initialize Endpoint ID with ASE ID */ + if (ase->id != ep->id) + ep->id = ase->id; + + rsp.id = ep->id; + rsp.state = ep->state; + + gatt_db_attribute_read_result(attrib, id, 0, (void *) &rsp, + sizeof(rsp)); +} + +static void ase_new(struct bt_ascs *ascs, int i) +{ + struct bt_ase *ase; + bt_uuid_t uuid; + + if (!ascs) + return; + + ase = new0(struct bt_ase, 1); + ase->ascs = ascs; + ase->id = i + 1; + + bt_uuid16_create(&uuid, ASE_UUID(i)); + ase->attr = gatt_db_service_add_characteristic(ascs->service, &uuid, + BT_ATT_PERM_READ, + BT_GATT_CHRC_PROP_READ | + BT_GATT_CHRC_PROP_NOTIFY, + ascs_ase_read, NULL, + ase); + + ase->ccc = gatt_db_service_add_ccc(ascs->service, + BT_ATT_PERM_READ | BT_ATT_PERM_WRITE); + + ascs->ase[i] = ase; +} + +static void *iov_pull_mem(struct iovec *iov, size_t len) +{ + void *data = iov->iov_base; + + if (iov->iov_len < len) + return NULL; + + iov->iov_base += len; + iov->iov_len -= len; + + return data; +} + +static bool bap_codec_equal(const struct bt_bap_codec *c1, + const struct bt_bap_codec *c2) +{ + /* Compare CID and VID if id is 0xff */ + if (c1->id == 0xff) + return !memcmp(c1, c2, sizeof(*c1)); + + return c1->id == c2->id; +} + +static struct bt_bap_stream *bap_stream_new(struct bt_bap *bap, + struct bt_bap_endpoint *ep, + struct bt_bap_pac *lpac, + struct bt_bap_pac *rpac, + struct iovec *data, + bool client) +{ + struct bt_bap_stream *stream; + + stream = new0(struct bt_bap_stream, 1); + stream->bap = bap; + stream->ep = ep; + ep->stream = stream; + stream->lpac = lpac; + stream->rpac = rpac; + stream->cc = iov_dup(data, 1); + stream->client = client; + + queue_push_tail(bap->streams, stream); + + return stream; +} + +static void stream_notify_config(struct bt_bap_stream *stream) +{ + struct bt_bap_endpoint *ep = stream->ep; + struct bt_bap_pac *lpac = stream->lpac; + struct bt_ascs_ase_status *status; + struct bt_ascs_ase_status_config *config; + size_t len; + + DBG(stream->bap, "stream %p", stream); + + len = sizeof(*status) + sizeof(*config) + stream->cc->iov_len; + status = malloc(len); + + memset(status, 0, len); + status->id = ep->id; + status->state = ep->state; + + /* Initialize preffered settings if not set */ + if (!lpac->qos.phy) + lpac->qos.phy = 0x02; + + if (!lpac->qos.rtn) + lpac->qos.rtn = 0x05; + + if (!lpac->qos.latency) + lpac->qos.latency = 10; + + if (!lpac->qos.pd_min) + lpac->qos.pd_min = 20000; + + if (!lpac->qos.pd_max) + lpac->qos.pd_max = 40000; + + if (!lpac->qos.ppd_min) + lpac->qos.ppd_min = lpac->qos.pd_min; + + if (!lpac->qos.ppd_max) + lpac->qos.ppd_max = lpac->qos.pd_max; + + /* TODO:Add support for setting preffered settings on bt_bap_pac */ + config = (void *)status->params; + config->framing = lpac->qos.framing; + config->phy = lpac->qos.phy; + config->rtn = lpac->qos.rtn; + config->latency = cpu_to_le16(lpac->qos.latency); + put_le24(lpac->qos.pd_min, config->pd_min); + put_le24(lpac->qos.pd_max, config->pd_max); + put_le24(lpac->qos.ppd_min, config->ppd_min); + put_le24(lpac->qos.ppd_max, config->ppd_max); + config->codec = lpac->codec; + config->cc_len = stream->cc->iov_len; + memcpy(config->cc, stream->cc->iov_base, stream->cc->iov_len); + + gatt_db_attribute_notify(ep->attr, (void *) status, len, + bt_bap_get_att(stream->bap)); + + free(status); +} + +static void stream_notify_qos(struct bt_bap_stream *stream) +{ + struct bt_bap_endpoint *ep = stream->ep; + struct bt_ascs_ase_status *status; + struct bt_ascs_ase_status_qos *qos; + size_t len; + + DBG(stream->bap, "stream %p", stream); + + len = sizeof(*status) + sizeof(*qos); + status = malloc(len); + + memset(status, 0, len); + status->id = ep->id; + status->state = ep->state; + + qos = (void *)status->params; + qos->cis_id = stream->qos.cis_id; + qos->cig_id = stream->qos.cig_id; + put_le24(stream->qos.interval, qos->interval); + qos->framing = stream->qos.framing; + qos->phy = stream->qos.phy; + qos->sdu = cpu_to_le16(stream->qos.sdu); + qos->rtn = stream->qos.rtn; + qos->latency = cpu_to_le16(stream->qos.latency); + put_le24(stream->qos.delay, qos->pd); + + gatt_db_attribute_notify(ep->attr, (void *) status, len, + bt_bap_get_att(stream->bap)); + + free(status); +} + +static void stream_notify_metadata(struct bt_bap_stream *stream) +{ + struct bt_bap_endpoint *ep = stream->ep; + struct bt_ascs_ase_status *status; + struct bt_ascs_ase_status_metadata *meta; + size_t len; + size_t meta_len = 0; + + DBG(stream->bap, "stream %p", stream); + + if (stream->meta) + meta_len = stream->meta->iov_len; + + len = sizeof(*status) + sizeof(*meta) + meta_len; + status = malloc(len); + + memset(status, 0, len); + status->id = ep->id; + status->state = ep->state; + + meta = (void *)status->params; + meta->cis_id = stream->qos.cis_id; + meta->cig_id = stream->qos.cig_id; + + if (stream->meta) { + meta->len = stream->meta->iov_len; + memcpy(meta->data, stream->meta->iov_base, meta->len); + } + + gatt_db_attribute_notify(ep->attr, (void *) status, len, + bt_bap_get_att(stream->bap)); + + free(status); +} + +static void bap_stream_clear_cfm(struct bt_bap_stream *stream) +{ + if (!stream->lpac->ops || !stream->lpac->ops->clear) + return; + + stream->lpac->ops->clear(stream, stream->lpac->user_data); +} + +static int stream_io_get_fd(struct bt_bap_stream_io *io) +{ + if (!io) + return -1; + + return io_get_fd(io->io); +} + +static void stream_io_free(void *data) +{ + struct bt_bap_stream_io *io = data; + int fd; + + fd = stream_io_get_fd(io); + + DBG(io->bap, "fd %d", fd); + + io_destroy(io->io); + free(io); + + /* Shutdown using SHUT_WR as SHUT_RDWR cause the socket to HUP + * immediately instead of waiting for Disconnect Complete event. + */ + shutdown(fd, SHUT_WR); +} + +static void stream_io_unref(struct bt_bap_stream_io *io) +{ + if (!io) + return; + + if (__sync_sub_and_fetch(&io->ref_count, 1)) + return; + + stream_io_free(io); +} + +static void bap_stream_unlink(void *data, void *user_data) +{ + struct bt_bap_stream *link = data; + struct bt_bap_stream *stream = user_data; + + queue_remove(link->links, stream); +} + +static void bap_stream_free(void *data) +{ + struct bt_bap_stream *stream = data; + + if (stream->ep) + stream->ep->stream = NULL; + + queue_foreach(stream->links, bap_stream_unlink, stream); + queue_destroy(stream->links, NULL); + stream_io_unref(stream->io); + iov_free(stream->cc); + iov_free(stream->meta); + free(stream); +} + +static void bap_stream_detach(struct bt_bap_stream *stream) +{ + struct bt_bap_endpoint *ep = stream->ep; + + if (!ep) + return; + + DBG(stream->bap, "stream %p ep %p", stream, ep); + + queue_remove(stream->bap->streams, stream); + bap_stream_clear_cfm(stream); + + stream->ep = NULL; + ep->stream = NULL; + bap_stream_free(stream); +} + +static void bap_stream_io_link(void *data, void *user_data) +{ + struct bt_bap_stream *stream = data; + struct bt_bap_stream *link = user_data; + + bt_bap_stream_io_link(stream, link); +} + +static void bap_stream_update_io_links(struct bt_bap_stream *stream) +{ + struct bt_bap *bap = stream->bap; + + DBG(bap, "stream %p", stream); + + queue_foreach(bap->streams, bap_stream_io_link, stream); +} + +static struct bt_bap_stream_io *stream_io_ref(struct bt_bap_stream_io *io) +{ + if (!io) + return NULL; + + __sync_fetch_and_add(&io->ref_count, 1); + + return io; +} + +static struct bt_bap_stream_io *stream_io_new(struct bt_bap *bap, int fd) +{ + struct io *io; + struct bt_bap_stream_io *sio; + + io = io_new(fd); + if (!io) + return NULL; + + DBG(bap, "fd %d", fd); + + sio = new0(struct bt_bap_stream_io, 1); + sio->bap = bap; + sio->io = io; + + return stream_io_ref(sio); +} + +static void stream_find_io(void *data, void *user_data) +{ + struct bt_bap_stream *stream = data; + struct bt_bap_stream_io **io = user_data; + + if (*io) + return; + + *io = stream->io; +} + +static struct bt_bap_stream_io *stream_get_io(struct bt_bap_stream *stream) +{ + struct bt_bap_stream_io *io; + + if (!stream) + return NULL; + + if (stream->io) + return stream->io; + + io = NULL; + queue_foreach(stream->links, stream_find_io, &io); + + return io; +} + +static bool stream_io_disconnected(struct io *io, void *user_data); + +static bool bap_stream_io_attach(struct bt_bap_stream *stream, int fd, + bool connecting) +{ + struct bt_bap_stream_io *io; + + io = stream_get_io(stream); + if (io) { + if (fd == stream_io_get_fd(io)) { + if (!stream->io) + stream->io = stream_io_ref(io); + + io->connecting = connecting; + return true; + } + + DBG(stream->bap, "stream %p io already set", stream); + return false; + } + + DBG(stream->bap, "stream %p connecting %s", stream, + connecting ? "true" : "false"); + + io = stream_io_new(stream->bap, fd); + if (!io) + return false; + + io->connecting = connecting; + stream->io = io; + io_set_disconnect_handler(io->io, stream_io_disconnected, stream, NULL); + + return true; +} + +static bool match_stream_io(const void *data, const void *user_data) +{ + const struct bt_bap_stream *stream = data; + const struct bt_bap_stream_io *io = user_data; + + if (!stream->io) + return false; + + return stream->io == io; +} + +static bool bap_stream_io_detach(struct bt_bap_stream *stream) +{ + struct bt_bap_stream *link; + struct bt_bap_stream_io *io; + + if (!stream->io) + return false; + + DBG(stream->bap, "stream %p", stream); + + io = stream->io; + stream->io = NULL; + + link = queue_find(stream->links, match_stream_io, io); + if (link) { + /* Detach link if in QoS state */ + if (link->ep->state == BT_ASCS_ASE_STATE_QOS) + bap_stream_io_detach(link); + } + + stream_io_unref(io); + + return true; +} + +static void bap_stream_set_io(void *data, void *user_data) +{ + struct bt_bap_stream *stream = data; + int fd = PTR_TO_INT(user_data); + bool ret; + + if (fd >= 0) + ret = bap_stream_io_attach(stream, fd, false); + else + ret = bap_stream_io_detach(stream); + + if (!ret) + return; + + switch (stream->ep->state) { + case BT_BAP_STREAM_STATE_ENABLING: + if (fd < 0) + bt_bap_stream_disable(stream, false, NULL, NULL); + else + bt_bap_stream_start(stream, NULL, NULL); + break; + case BT_BAP_STREAM_STATE_DISABLING: + if (fd < 0) + bt_bap_stream_stop(stream, NULL, NULL); + break; + } +} + +static void bap_stream_state_changed(struct bt_bap_stream *stream) +{ + struct bt_bap *bap = stream->bap; + const struct queue_entry *entry; + + DBG(bap, "stream %p dir 0x%02x: %s -> %s", stream, + bt_bap_stream_get_dir(stream), + bt_bap_stream_statestr(stream->ep->old_state), + bt_bap_stream_statestr(stream->ep->state)); + + bt_bap_ref(bap); + + /* Pre notification updates */ + switch (stream->ep->state) { + case BT_ASCS_ASE_STATE_IDLE: + break; + case BT_ASCS_ASE_STATE_CONFIG: + bap_stream_update_io_links(stream); + break; + case BT_ASCS_ASE_STATE_DISABLING: + bap_stream_io_detach(stream); + break; + case BT_ASCS_ASE_STATE_QOS: + if (stream->io && !stream->io->connecting) + bap_stream_io_detach(stream); + else + bap_stream_update_io_links(stream); + break; + case BT_ASCS_ASE_STATE_ENABLING: + case BT_ASCS_ASE_STATE_STREAMING: + break; + } + + for (entry = queue_get_entries(bap->state_cbs); entry; + entry = entry->next) { + struct bt_bap_state *state = entry->data; + + if (state->func) + state->func(stream, stream->ep->old_state, + stream->ep->state, state->data); + } + + /* Post notification updates */ + switch (stream->ep->state) { + case BT_ASCS_ASE_STATE_IDLE: + bap_stream_detach(stream); + break; + case BT_ASCS_ASE_STATE_QOS: + break; + case BT_ASCS_ASE_STATE_ENABLING: + if (bt_bap_stream_get_io(stream)) + bt_bap_stream_start(stream, NULL, NULL); + break; + case BT_ASCS_ASE_STATE_DISABLING: + if (!bt_bap_stream_get_io(stream)) + bt_bap_stream_stop(stream, NULL, NULL); + break; + } + + bt_bap_unref(bap); +} + +static void stream_set_state(struct bt_bap_stream *stream, uint8_t state) +{ + struct bt_bap_endpoint *ep = stream->ep; + + ep->old_state = ep->state; + ep->state = state; + + if (stream->client) + goto done; + + switch (ep->state) { + case BT_ASCS_ASE_STATE_IDLE: + break; + case BT_ASCS_ASE_STATE_CONFIG: + stream_notify_config(stream); + break; + case BT_ASCS_ASE_STATE_QOS: + stream_notify_qos(stream); + break; + case BT_ASCS_ASE_STATE_ENABLING: + case BT_ASCS_ASE_STATE_STREAMING: + case BT_ASCS_ASE_STATE_DISABLING: + stream_notify_metadata(stream); + break; + } + +done: + bap_stream_state_changed(stream); +} + +static void ascs_ase_rsp_add(struct iovec *iov, uint8_t id, + uint8_t code, uint8_t reason) +{ + struct bt_ascs_cp_rsp *cp; + struct bt_ascs_ase_rsp *rsp; + + if (!iov) + return; + + cp = iov->iov_base; + + if (cp->num_ase == 0xff) + return; + + switch (code) { + /* If the Response_Code value is 0x01 or 0x02, Number_of_ASEs shall be + * set to 0xFF. + */ + case BT_ASCS_RSP_NOT_SUPPORTED: + case BT_ASCS_RSP_TRUNCATED: + cp->num_ase = 0xff; + break; + default: + cp->num_ase++; + break; + } + + iov->iov_len += sizeof(*rsp); + iov->iov_base = realloc(iov->iov_base, iov->iov_len); + + rsp = iov->iov_base + (iov->iov_len - sizeof(*rsp)); + rsp->ase = id; + rsp->code = code; + rsp->reason = reason; +} + +static void ascs_ase_rsp_add_errno(struct iovec *iov, uint8_t id, int err) +{ + struct bt_ascs_cp_rsp *rsp = iov->iov_base; + + switch (err) { + case -ENOBUFS: + case -ENOMEM: + return ascs_ase_rsp_add(iov, id, BT_ASCS_RSP_NO_MEM, + BT_ASCS_REASON_NONE); + case -EINVAL: + switch (rsp->op) { + case BT_ASCS_CONFIG: + /* Fallthrough */ + case BT_ASCS_QOS: + return ascs_ase_rsp_add(iov, id, + BT_ASCS_RSP_CONF_INVALID, + BT_ASCS_REASON_NONE); + case BT_ASCS_ENABLE: + /* Fallthrough */ + case BT_ASCS_METADATA: + return ascs_ase_rsp_add(iov, id, + BT_ASCS_RSP_METADATA_INVALID, + BT_ASCS_REASON_NONE); + default: + return ascs_ase_rsp_add(iov, id, + BT_ASCS_RSP_UNSPECIFIED, + BT_ASCS_REASON_NONE); + } + case -ENOTSUP: + switch (rsp->op) { + case BT_ASCS_CONFIG: + /* Fallthrough */ + case BT_ASCS_QOS: + return ascs_ase_rsp_add(iov, id, + BT_ASCS_RSP_CONF_UNSUPPORTED, + BT_ASCS_REASON_NONE); + case BT_ASCS_ENABLE: + /* Fallthrough */ + case BT_ASCS_METADATA: + return ascs_ase_rsp_add(iov, id, + BT_ASCS_RSP_METADATA_UNSUPPORTED, + BT_ASCS_REASON_NONE); + default: + return ascs_ase_rsp_add(iov, id, + BT_ASCS_RSP_NOT_SUPPORTED, + BT_ASCS_REASON_NONE); + } + case -EBADMSG: + return ascs_ase_rsp_add(iov, id, BT_ASCS_RSP_INVALID_ASE_STATE, + BT_ASCS_REASON_NONE); + case -ENOMSG: + return ascs_ase_rsp_add(iov, id, BT_ASCS_RSP_TRUNCATED, + BT_ASCS_REASON_NONE); + default: + return ascs_ase_rsp_add(iov, id, BT_ASCS_RSP_UNSPECIFIED, + BT_ASCS_REASON_NONE); + } +} + +static void ascs_ase_rsp_success(struct iovec *iov, uint8_t id) +{ + return ascs_ase_rsp_add(iov, id, BT_ASCS_RSP_SUCCESS, + BT_ASCS_REASON_NONE); +} + +static void ep_config_cb(struct bt_bap_stream *stream, int err) +{ + if (err) + return; + + stream_set_state(stream, BT_BAP_STREAM_STATE_CONFIG); +} + +static uint8_t stream_config(struct bt_bap_stream *stream, struct iovec *cc, + struct iovec *rsp) +{ + struct bt_bap_pac *pac = stream->lpac; + + DBG(stream->bap, "stream %p", stream); + + /* TODO: Wait for pac->ops response */ + ascs_ase_rsp_success(rsp, stream->ep->id); + + if (!iov_memcmp(stream->cc, cc)) { + stream_set_state(stream, BT_BAP_STREAM_STATE_CONFIG); + return 0; + } + + iov_free(stream->cc); + stream->cc = iov_dup(cc, 1); + + if (pac->ops && pac->ops->config) + pac->ops->config(stream, cc, NULL, ep_config_cb, + pac->user_data); + + return 0; +} + +static uint8_t ep_config(struct bt_bap_endpoint *ep, struct bt_bap *bap, + struct bt_ascs_config *req, + struct iovec *iov, struct iovec *rsp) +{ + struct iovec cc; + const struct queue_entry *e; + + DBG(bap, "ep %p id 0x%02x dir 0x%02x", ep, ep->id, ep->dir); + + switch (ep->state) { + /* Valid only if ASE_State field = 0x00 (Idle) */ + case BT_ASCS_ASE_STATE_IDLE: + /* or 0x01 (Codec Configured) */ + case BT_ASCS_ASE_STATE_CONFIG: + /* or 0x02 (QoS Configured) */ + case BT_ASCS_ASE_STATE_QOS: + break; + default: + DBG(bap, "Invalid state %s", bt_bap_stream_statestr(ep->state)); + ascs_ase_rsp_add(rsp, req->ase, + BT_ASCS_RSP_INVALID_ASE_STATE, + BT_ASCS_REASON_NONE); + return 0; + } + + if (iov->iov_len < req->cc_len) + return BT_ATT_ERROR_INVALID_ATTRIBUTE_VALUE_LEN; + + cc.iov_base = iov_pull_mem(iov, req->cc_len); + cc.iov_len = req->cc_len; + + if (!bap_print_cc(cc.iov_base, cc.iov_len, bap->debug_func, + bap->debug_data)) { + ascs_ase_rsp_add(rsp, req->ase, + BT_ASCS_RSP_CONF_INVALID, + BT_ASCS_REASON_CODEC_DATA); + return 0; + } + + switch (ep->dir) { + case BT_BAP_SINK: + e = queue_get_entries(bap->ldb->sinks); + break; + case BT_BAP_SOURCE: + e = queue_get_entries(bap->ldb->sources); + break; + default: + e = NULL; + } + + for (; e; e = e->next) { + struct bt_bap_pac *pac = e->data; + + if (!bap_codec_equal(&req->codec, &pac->codec)) + continue; + + if (!ep->stream) + ep->stream = bap_stream_new(bap, ep, pac, NULL, NULL, + false); + + break; + } + + if (!e) { + ascs_ase_rsp_add(rsp, req->ase, + BT_ASCS_RSP_CONF_INVALID, + BT_ASCS_REASON_CODEC); + return 0; + } + + return stream_config(ep->stream, &cc, rsp); +} + +static uint8_t ascs_config(struct bt_ascs *ascs, struct bt_bap *bap, + struct iovec *iov, struct iovec *rsp) +{ + struct bt_bap_endpoint *ep; + struct bt_ascs_config *req; + + req = iov_pull_mem(iov, sizeof(*req)); + + DBG(bap, "codec 0x%02x phy 0x%02x latency %u", req->codec.id, req->phy, + req->latency); + + ep = bap_get_endpoint_id(bap, bap->ldb, req->ase); + if (!ep) { + DBG(bap, "Invalid ASE ID 0x%02x", req->ase); + ascs_ase_rsp_add(rsp, req->ase, + BT_ASCS_RSP_INVALID_ASE, BT_ASCS_REASON_NONE); + return 0; + } + + return ep_config(ep, bap, req, iov, rsp); +} + +static uint8_t stream_qos(struct bt_bap_stream *stream, struct bt_bap_qos *qos, + struct iovec *rsp) +{ + DBG(stream->bap, "stream %p", stream); + + ascs_ase_rsp_success(rsp, stream->ep->id); + + if (memcmp(&stream->qos, qos, sizeof(*qos))) + stream->qos = *qos; + + stream_set_state(stream, BT_BAP_STREAM_STATE_QOS); + + return 0; +} + +static uint8_t ep_qos(struct bt_bap_endpoint *ep, struct bt_bap *bap, + struct bt_bap_qos *qos, struct iovec *rsp) +{ + DBG(bap, "ep %p id 0x%02x dir 0x%02x", ep, ep->id, ep->dir); + + switch (ep->state) { + /* Valid only if ASE_State field = 0x01 (Codec Configured) */ + case BT_ASCS_ASE_STATE_CONFIG: + /* or 0x02 (QoS Configured) */ + case BT_ASCS_ASE_STATE_QOS: + break; + default: + DBG(bap, "Invalid state %s", bt_bap_stream_statestr(ep->state)); + ascs_ase_rsp_add(rsp, ep->id, + BT_ASCS_RSP_INVALID_ASE_STATE, + BT_ASCS_REASON_NONE); + return 0; + } + + if (!ep->stream) { + DBG(bap, "No stream found"); + ascs_ase_rsp_add(rsp, ep->id, + BT_ASCS_RSP_INVALID_ASE_STATE, + BT_ASCS_REASON_NONE); + return 0; + } + + return stream_qos(ep->stream, qos, rsp); +} + +static uint8_t ascs_qos(struct bt_ascs *ascs, struct bt_bap *bap, + struct iovec *iov, struct iovec *rsp) +{ + struct bt_bap_endpoint *ep; + struct bt_ascs_qos *req; + struct bt_bap_qos qos; + + req = iov_pull_mem(iov, sizeof(*req)); + + memset(&qos, 0, sizeof(qos)); + + qos.cig_id = req->cig; + qos.cis_id = req->cis; + qos.interval = get_le24(req->interval); + qos.framing = req->framing; + qos.phy = req->phy; + qos.sdu = le16_to_cpu(req->sdu); + qos.rtn = req->rtn; + qos.latency = le16_to_cpu(req->latency); + qos.delay = get_le24(req->pd); + + DBG(bap, "CIG 0x%02x CIS 0x%02x interval %u framing 0x%02x " + "phy 0x%02x SDU %u rtn %u latency %u pd %u", + req->cig, req->cis, qos.interval, qos.framing, qos.phy, + qos.sdu, qos.rtn, qos.latency, qos.delay); + + ep = bap_get_endpoint_id(bap, bap->ldb, req->ase); + if (!ep) { + DBG(bap, "%s: Invalid ASE ID 0x%02x", req->ase); + ascs_ase_rsp_add(rsp, req->ase, + BT_ASCS_RSP_INVALID_ASE, BT_ASCS_REASON_NONE); + return 0; + } + + return ep_qos(ep, bap, &qos, rsp); +} + +static uint8_t stream_enable(struct bt_bap_stream *stream, struct iovec *meta, + struct iovec *rsp) +{ + DBG(stream->bap, "stream %p", stream); + + ascs_ase_rsp_success(rsp, stream->ep->id); + + iov_free(stream->meta); + stream->meta = iov_dup(meta, 1); + + stream_set_state(stream, BT_BAP_STREAM_STATE_ENABLING); + + /* Sink can autonomously for to Streaming state if io already exits */ + if (stream->io && stream->ep->dir == BT_BAP_SINK) + stream_set_state(stream, BT_BAP_STREAM_STATE_STREAMING); + + return 0; +} + +static bool bap_print_ltv(const char *label, void *data, size_t len, + util_debug_func_t func, void *user_data) +{ + struct iovec iov = { + .iov_base = data, + .iov_len = len, + }; + int i; + + util_debug(func, user_data, "Length %zu", iov.iov_len); + + for (i = 0; iov.iov_len > 1; i++) { + struct bt_ltv *ltv = iov_pull_mem(&iov, sizeof(*ltv)); + uint8_t *data; + + if (!ltv) { + util_debug(func, user_data, "Unable to parse %s", + label); + return false; + } + + util_debug(func, user_data, "%s #%u: len %u type %u", + label, i, ltv->len, ltv->type); + + data = iov_pull_mem(&iov, ltv->len - 1); + if (!data) { + util_debug(func, user_data, "Unable to parse %s", + label); + return false; + } + + util_hexdump(' ', ltv->value, ltv->len - 1, func, user_data); + } + + return true; +} + +static bool bap_print_metadata(void *data, size_t len, util_debug_func_t func, + void *user_data) +{ + return bap_print_ltv("Metadata", data, len, func, user_data); +} + +static uint8_t ep_enable(struct bt_bap_endpoint *ep, struct bt_bap *bap, + struct bt_ascs_enable *req, struct iovec *iov, + struct iovec *rsp) +{ + struct iovec meta; + + DBG(bap, "ep %p id 0x%02x dir 0x%02x", ep, ep->id, ep->dir); + + switch (ep->state) { + /* Valid only if ASE_State field = 0x02 (QoS Configured) */ + case BT_ASCS_ASE_STATE_QOS: + break; + default: + DBG(bap, "Invalid state %s", bt_bap_stream_statestr(ep->state)); + ascs_ase_rsp_add(rsp, ep->id, + BT_ASCS_RSP_INVALID_ASE_STATE, + BT_ASCS_REASON_NONE); + return 0; + } + + meta.iov_base = iov_pull_mem(iov, req->meta.len); + meta.iov_len = req->meta.len; + + if (!bap_print_metadata(meta.iov_base, meta.iov_len, bap->debug_func, + bap->debug_data)) { + ascs_ase_rsp_add(rsp, ep->id, + BT_ASCS_RSP_METADATA_INVALID, + BT_ASCS_REASON_NONE); + return 0; + } + + if (!ep->stream) { + DBG(bap, "No stream found"); + ascs_ase_rsp_add(rsp, ep->id, + BT_ASCS_RSP_INVALID_ASE_STATE, + BT_ASCS_REASON_NONE); + return 0; + } + + return stream_enable(ep->stream, &meta, rsp); +} + +static uint8_t ascs_enable(struct bt_ascs *ascs, struct bt_bap *bap, + struct iovec *iov, struct iovec *rsp) +{ + struct bt_bap_endpoint *ep; + struct bt_ascs_enable *req; + + req = iov_pull_mem(iov, sizeof(*req)); + + ep = bap_get_endpoint_id(bap, bap->ldb, req->meta.ase); + if (!ep) { + DBG(bap, "Invalid ASE ID 0x%02x", req->meta.ase); + ascs_ase_rsp_add(rsp, req->meta.ase, + BT_ASCS_RSP_INVALID_ASE, BT_ASCS_REASON_NONE); + return 0; + } + + return ep_enable(ep, bap, req, iov, rsp); +} + +static uint8_t stream_start(struct bt_bap_stream *stream, struct iovec *rsp) +{ + DBG(stream->bap, "stream %p", stream); + + ascs_ase_rsp_success(rsp, stream->ep->id); + + stream_set_state(stream, BT_BAP_STREAM_STATE_STREAMING); + + return 0; +} + +static uint8_t ep_start(struct bt_bap_endpoint *ep, struct iovec *rsp) +{ + struct bt_bap_stream *stream = ep->stream; + + DBG(stream->bap, "ep %p id 0x%02x dir 0x%02x", ep, ep->id, ep->dir); + + switch (ep->state) { + /* Valid only if ASE_State field = 0x03 (Enabling) */ + case BT_ASCS_ASE_STATE_ENABLING: + break; + default: + DBG(ep->stream->bap, "Invalid state %s", + bt_bap_stream_statestr(ep->state)); + ascs_ase_rsp_add(rsp, ep->id, + BT_ASCS_RSP_INVALID_ASE_STATE, + BT_ASCS_REASON_NONE); + return 0; + } + + /* If the ASE_ID written by the client represents a Sink ASE, the + * server shall not accept the Receiver Start Ready operation for that + * ASE. The server shall send a notification of the ASE Control Point + * characteristic to the client, and the server shall set the + * Response_Code value for that ASE to 0x05 (Invalid ASE direction). + */ + if (ep->dir == BT_BAP_SINK) { + ascs_ase_rsp_add(rsp, ep->id, + BT_ASCS_RSP_INVALID_DIR, BT_ASCS_REASON_NONE); + return 0; + } + + return stream_start(ep->stream, rsp); +} + +static uint8_t ascs_start(struct bt_ascs *ascs, struct bt_bap *bap, + struct iovec *iov, struct iovec *rsp) +{ + struct bt_bap_endpoint *ep; + struct bt_ascs_start *req; + + req = iov_pull_mem(iov, sizeof(*req)); + + ep = bap_get_endpoint_id(bap, bap->ldb, req->ase); + if (!ep) { + DBG(bap, "Invalid ASE ID 0x%02x", req->ase); + ascs_ase_rsp_add(rsp, req->ase, + BT_ASCS_RSP_INVALID_ASE, BT_ASCS_REASON_NONE); + return 0; + } + + if (!ep->stream) { + DBG(bap, "No stream found for %p", ep); + ascs_ase_rsp_add(rsp, ep->id, + BT_ASCS_RSP_INVALID_ASE_STATE, + BT_ASCS_REASON_NONE); + return 0; + } + + return ep_start(ep, rsp); +} + +static uint8_t stream_disable(struct bt_bap_stream *stream, struct iovec *rsp) +{ + DBG(stream->bap, "stream %p", stream); + + if (!stream || stream->ep->state == BT_BAP_STREAM_STATE_QOS || + stream->ep->state == BT_BAP_STREAM_STATE_IDLE) + return 0; + + ascs_ase_rsp_success(rsp, stream->ep->id); + + /* Sink can autonomously transit to QOS while source needs to go to + * Disabling until BT_ASCS_STOP is received. + */ + if (stream->ep->dir == BT_BAP_SINK) + stream_set_state(stream, BT_BAP_STREAM_STATE_QOS); + else + stream_set_state(stream, BT_BAP_STREAM_STATE_DISABLING); + + return 0; +} + +static uint8_t ep_disable(struct bt_bap_endpoint *ep, struct iovec *rsp) +{ + struct bt_bap_stream *stream = ep->stream; + + DBG(stream->bap, "ep %p id 0x%02x dir 0x%02x", ep, ep->id, ep->dir); + + switch (ep->state) { + /* Valid only if ASE_State field = 0x03 (Enabling) */ + case BT_ASCS_ASE_STATE_ENABLING: + /* or 0x04 (Streaming) */ + case BT_ASCS_ASE_STATE_STREAMING: + break; + default: + DBG(stream->bap, "Invalid state %s", + bt_bap_stream_statestr(ep->state)); + ascs_ase_rsp_add(rsp, ep->id, + BT_ASCS_RSP_INVALID_ASE_STATE, + BT_ASCS_REASON_NONE); + return 0; + } + + return stream_disable(ep->stream, rsp); +} + +static uint8_t ascs_disable(struct bt_ascs *ascs, struct bt_bap *bap, + struct iovec *iov, struct iovec *rsp) +{ + struct bt_bap_endpoint *ep; + struct bt_ascs_disable *req; + + req = iov_pull_mem(iov, sizeof(*req)); + + ep = bap_get_endpoint_id(bap, bap->ldb, req->ase); + if (!ep) { + DBG(bap, "Invalid ASE ID 0x%02x", req->ase); + ascs_ase_rsp_add(rsp, req->ase, + BT_ASCS_RSP_INVALID_ASE, BT_ASCS_REASON_NONE); + return 0; + } + + if (!ep->stream) { + DBG(bap, "No stream found"); + ascs_ase_rsp_add(rsp, ep->id, + BT_ASCS_RSP_INVALID_ASE_STATE, + BT_ASCS_REASON_NONE); + return 0; + } + + return ep_disable(ep, rsp); +} + +static uint8_t stream_stop(struct bt_bap_stream *stream, struct iovec *rsp) +{ + DBG(stream->bap, "stream %p", stream); + + if (!stream) + return 0; + + ascs_ase_rsp_success(rsp, stream->ep->id); + + stream_set_state(stream, BT_BAP_STREAM_STATE_QOS); + + return 0; +} + +static uint8_t ep_stop(struct bt_bap_endpoint *ep, struct iovec *rsp) +{ + struct bt_bap_stream *stream = ep->stream; + + DBG(stream->bap, "ep %p id 0x%02x dir 0x%02x", ep, ep->id, ep->dir); + + switch (ep->state) { + /* Valid only if ASE_State field = 0x05 (Disabling) */ + case BT_ASCS_ASE_STATE_DISABLING: + break; + default: + DBG(stream->bap, "Invalid state %s", + bt_bap_stream_statestr(ep->state)); + ascs_ase_rsp_add(rsp, ep->id, + BT_ASCS_RSP_INVALID_ASE_STATE, + BT_ASCS_REASON_NONE); + return 0; + } + + /* If the ASE_ID written by the client represents a Sink ASE, the + * server shall not accept the Receiver Stop Ready operation for that + * ASE. The server shall send a notification of the ASE Control Point + * characteristic to the client, and the server shall set the + * Response_Code value for that ASE to 0x05 (Invalid ASE direction). + */ + if (ep->dir == BT_BAP_SINK) { + ascs_ase_rsp_add(rsp, ep->id, + BT_ASCS_RSP_INVALID_DIR, BT_ASCS_REASON_NONE); + return 0; + } + + return stream_stop(ep->stream, rsp); +} + +static uint8_t ascs_stop(struct bt_ascs *ascs, struct bt_bap *bap, + struct iovec *iov, struct iovec *rsp) +{ + struct bt_bap_endpoint *ep; + struct bt_ascs_stop *req; + + req = iov_pull_mem(iov, sizeof(*req)); + + ep = bap_get_endpoint_id(bap, bap->ldb, req->ase); + if (!ep) { + DBG(bap, "Invalid ASE ID 0x%02x", req->ase); + ascs_ase_rsp_add(rsp, req->ase, + BT_ASCS_RSP_INVALID_ASE, BT_ASCS_REASON_NONE); + return 0; + } + + if (!ep->stream) { + DBG(bap, "No stream found"); + ascs_ase_rsp_add(rsp, ep->id, + BT_ASCS_RSP_INVALID_ASE_STATE, + BT_ASCS_REASON_NONE); + return 0; + } + + return ep_stop(ep, rsp); +} + +static uint8_t stream_metadata(struct bt_bap_stream *stream, struct iovec *meta, + struct iovec *rsp) +{ + DBG(stream->bap, "stream %p", stream); + + ascs_ase_rsp_success(rsp, stream->ep->id); + + iov_free(stream->meta); + stream->meta = iov_dup(meta, 1); + + return 0; +} + +static uint8_t ep_metadata(struct bt_bap_endpoint *ep, struct iovec *meta, + struct iovec *rsp) +{ + struct bt_bap_stream *stream = ep->stream; + + DBG(stream->bap, "ep %p id 0x%02x dir 0x%02x", ep, ep->id, ep->dir); + + switch (ep->state) { + /* Valid only if ASE_State field = 0x03 (Enabling) */ + case BT_ASCS_ASE_STATE_ENABLING: + /* or 0x04 (Streaming) */ + case BT_ASCS_ASE_STATE_STREAMING: + break; + default: + DBG(stream->bap, "Invalid state %s", + bt_bap_stream_statestr(ep->state)); + ascs_ase_rsp_add(rsp, ep->id, + BT_ASCS_RSP_INVALID_ASE_STATE, + BT_ASCS_REASON_NONE); + return 0; + } + + return stream_metadata(ep->stream, meta, rsp); +} + +static uint8_t ascs_metadata(struct bt_ascs *ascs, struct bt_bap *bap, + struct iovec *iov, struct iovec *rsp) +{ + struct bt_bap_endpoint *ep; + struct bt_ascs_metadata *req; + + req = iov_pull_mem(iov, sizeof(*req)); + + ep = bap_get_endpoint_id(bap, bap->ldb, req->ase); + if (!ep) { + DBG(bap, "Invalid ASE ID 0x%02x", req->ase); + ascs_ase_rsp_add(rsp, req->ase, + BT_ASCS_RSP_INVALID_ASE, BT_ASCS_REASON_NONE); + return 0; + } + + if (!ep->stream) { + DBG(bap, "No stream found"); + ascs_ase_rsp_add(rsp, ep->id, + BT_ASCS_RSP_INVALID_ASE_STATE, + BT_ASCS_REASON_NONE); + return 0; + } + + return ep_metadata(ep, iov, rsp); +} + +static uint8_t stream_release(struct bt_bap_stream *stream, struct iovec *rsp) +{ + struct bt_bap_pac *pac; + + DBG(stream->bap, "stream %p", stream); + + ascs_ase_rsp_success(rsp, stream->ep->id); + + pac = stream->lpac; + if (pac->ops && pac->ops->clear) + pac->ops->clear(stream, pac->user_data); + + stream_set_state(stream, BT_BAP_STREAM_STATE_IDLE); + + return 0; +} + +static uint8_t ascs_release(struct bt_ascs *ascs, struct bt_bap *bap, + struct iovec *iov, struct iovec *rsp) +{ + struct bt_bap_endpoint *ep; + struct bt_ascs_release *req; + + req = iov_pull_mem(iov, sizeof(*req)); + + ep = bap_get_endpoint_id(bap, bap->ldb, req->ase); + if (!ep) { + DBG(bap, "Invalid ASE ID 0x%02x", req->ase); + ascs_ase_rsp_add(rsp, req->ase, + BT_ASCS_RSP_INVALID_ASE, BT_ASCS_REASON_NONE); + return 0; + } + + if (!ep->stream) { + DBG(bap, "No stream found"); + ascs_ase_rsp_add(rsp, ep->id, + BT_ASCS_RSP_INVALID_ASE_STATE, + BT_ASCS_REASON_NONE); + return 0; + } + + return stream_release(ep->stream, rsp); +} + +#define ASCS_OP(_str, _op, _size, _func) \ + { \ + .str = _str, \ + .op = _op, \ + .size = _size, \ + .func = _func, \ + } + +struct ascs_op_handler { + const char *str; + uint8_t op; + size_t size; + uint8_t (*func)(struct bt_ascs *ascs, struct bt_bap *bap, + struct iovec *iov, struct iovec *rsp); +} handlers[] = { + ASCS_OP("Codec Config", BT_ASCS_CONFIG, + sizeof(struct bt_ascs_config), ascs_config), + ASCS_OP("QoS Config", BT_ASCS_QOS, + sizeof(struct bt_ascs_qos), ascs_qos), + ASCS_OP("Enable", BT_ASCS_ENABLE, sizeof(struct bt_ascs_enable), + ascs_enable), + ASCS_OP("Receiver Start Ready", BT_ASCS_START, + sizeof(struct bt_ascs_start), ascs_start), + ASCS_OP("Disable", BT_ASCS_DISABLE, + sizeof(struct bt_ascs_disable), ascs_disable), + ASCS_OP("Receiver Stop Ready", BT_ASCS_STOP, + sizeof(struct bt_ascs_stop), ascs_stop), + ASCS_OP("Update Metadata", BT_ASCS_METADATA, + sizeof(struct bt_ascs_metadata), ascs_metadata), + ASCS_OP("Release", BT_ASCS_RELEASE, + sizeof(struct bt_ascs_release), ascs_release), + {} +}; + +static struct iovec *ascs_ase_cp_rsp_new(uint8_t op) +{ + struct bt_ascs_cp_rsp *rsp; + struct iovec *iov; + + iov = new0(struct iovec, 1); + rsp = new0(struct bt_ascs_cp_rsp, 1); + rsp->op = op; + iov->iov_base = rsp; + iov->iov_len = sizeof(*rsp); + + return iov; +} + +static void ascs_ase_cp_write(struct gatt_db_attribute *attrib, + unsigned int id, uint16_t offset, + const uint8_t *value, size_t len, + uint8_t opcode, struct bt_att *att, + void *user_data) +{ + struct bt_ascs *ascs = user_data; + struct bt_bap *bap = bap_get_session(att, ascs->bdb->db); + struct iovec iov = { + .iov_base = (void *) value, + .iov_len = len, + }; + struct bt_ascs_ase_hdr *hdr; + struct ascs_op_handler *handler; + uint8_t ret = BT_ATT_ERROR_REQUEST_NOT_SUPPORTED; + struct iovec *rsp; + + if (offset) { + DBG(bap, "invalid offset %u", offset); + gatt_db_attribute_write_result(attrib, id, + BT_ATT_ERROR_INVALID_OFFSET); + return; + } + + if (len < sizeof(*hdr)) { + DBG(bap, "invalid len %u < %u sizeof(*hdr)", len, + sizeof(*hdr)); + gatt_db_attribute_write_result(attrib, id, + BT_ATT_ERROR_INVALID_ATTRIBUTE_VALUE_LEN); + return; + } + + hdr = iov_pull_mem(&iov, sizeof(*hdr)); + rsp = ascs_ase_cp_rsp_new(hdr->op); + + for (handler = handlers; handler && handler->str; handler++) { + if (handler->op != hdr->op) + continue; + + if (iov.iov_len < hdr->num * handler->size) { + DBG(bap, "invalid len %u < %u " + "hdr->num * handler->size", len, + hdr->num * handler->size); + ret = BT_ATT_ERROR_INVALID_ATTRIBUTE_VALUE_LEN; + goto respond; + } + + break; + } + + if (handler && handler->str) { + int i; + + DBG(bap, "%s", handler->str); + + for (i = 0; i < hdr->num; i++) + ret = handler->func(ascs, bap, &iov, rsp); + } else { + DBG(bap, "Unknown opcode 0x%02x", hdr->op); + ascs_ase_rsp_add_errno(rsp, 0x00, -ENOTSUP); + } + +respond: + if (ret == BT_ATT_ERROR_INVALID_ATTRIBUTE_VALUE_LEN) + ascs_ase_rsp_add_errno(rsp, 0x00, -ENOMSG); + + gatt_db_attribute_notify(attrib, rsp->iov_base, rsp->iov_len, att); + gatt_db_attribute_write_result(attrib, id, ret); + + iov_free(rsp); +} + +static struct bt_ascs *ascs_new(struct gatt_db *db) +{ + struct bt_ascs *ascs; + bt_uuid_t uuid; + int i; + + if (!db) + return NULL; + + ascs = new0(struct bt_ascs, 1); + + /* Populate DB with ASCS attributes */ + bt_uuid16_create(&uuid, ASCS_UUID); + ascs->service = gatt_db_add_service(db, &uuid, true, + 4 + (NUM_ASES * 3)); + + for (i = 0; i < NUM_ASES; i++) + ase_new(ascs, i); + + bt_uuid16_create(&uuid, ASE_CP_UUID); + ascs->ase_cp = gatt_db_service_add_characteristic(ascs->service, + &uuid, + BT_ATT_PERM_WRITE, + BT_GATT_CHRC_PROP_WRITE | + BT_GATT_CHRC_PROP_NOTIFY, + NULL, ascs_ase_cp_write, + ascs); + + ascs->ase_cp_ccc = gatt_db_service_add_ccc(ascs->service, + BT_ATT_PERM_READ | BT_ATT_PERM_WRITE); + + gatt_db_service_set_active(ascs->service, true); + + return ascs; +} + +static struct bt_bap_db *bap_db_new(struct gatt_db *db) +{ + struct bt_bap_db *bdb; + + if (!db) + return NULL; + + bdb = new0(struct bt_bap_db, 1); + bdb->db = gatt_db_ref(db); + bdb->sinks = queue_new(); + bdb->sources = queue_new(); + bdb->endpoints = queue_new(); + + if (!bap_db) + bap_db = queue_new(); + + bdb->pacs = pacs_new(db); + bdb->pacs->bdb = bdb; + + bdb->ascs = ascs_new(db); + bdb->ascs->bdb = bdb; + + queue_push_tail(bap_db, bdb); + + return bdb; +} + +static struct bt_bap_db *bap_get_db(struct gatt_db *db) +{ + struct bt_bap_db *bdb; + + bdb = queue_find(bap_db, bap_db_match, db); + if (bdb) + return bdb; + + return bap_db_new(db); +} + +static struct bt_pacs *bap_get_pacs(struct bt_bap *bap) +{ + if (!bap) + return NULL; + + if (bap->rdb->pacs) + return bap->rdb->pacs; + + bap->rdb->pacs = new0(struct bt_pacs, 1); + bap->rdb->pacs->bdb = bap->rdb; + + return bap->rdb->pacs; +} + +static struct bt_ascs *bap_get_ascs(struct bt_bap *bap) +{ + if (!bap) + return NULL; + + if (bap->rdb->ascs) + return bap->rdb->ascs; + + bap->rdb->ascs = new0(struct bt_ascs, 1); + bap->rdb->ascs->bdb = bap->rdb; + + return bap->rdb->ascs; +} + +static struct bt_bap_pac *bap_pac_new(struct bt_bap_db *bdb, const char *name, + uint8_t type, + struct bt_bap_codec *codec, + struct bt_bap_pac_qos *qos, + struct iovec *data, + struct iovec *metadata) +{ + struct bt_bap_pac *pac; + + pac = new0(struct bt_bap_pac, 1); + pac->bdb = bdb; + pac->name = name ? strdup(name) : NULL; + pac->type = type; + pac->codec = *codec; + pac->data = iov_dup(data, 1); + pac->metadata = iov_dup(metadata, 1); + + if (qos) + pac->qos = *qos; + + return pac; +} + +static void bap_pac_free(void *data) +{ + struct bt_bap_pac *pac = data; + + free(pac->name); + iov_free(pac->metadata); + iov_free(pac->data); + free(pac); +} + +static void bap_add_sink(struct bt_bap_pac *pac) +{ + struct iovec iov; + uint8_t value[512]; + + queue_push_tail(pac->bdb->sinks, pac); + + memset(value, 0, sizeof(value)); + + iov.iov_base = value; + iov.iov_len = 0; + + queue_foreach(pac->bdb->sinks, pac_foreach, &iov); + + gatt_db_attribute_notify(pac->bdb->pacs->sink, iov.iov_base, + iov.iov_len, NULL); +} + +static void bap_add_source(struct bt_bap_pac *pac) +{ + struct iovec iov; + uint8_t value[512]; + + queue_push_tail(pac->bdb->sources, pac); + + memset(value, 0, sizeof(value)); + + iov.iov_base = value; + iov.iov_len = 0; + + queue_foreach(pac->bdb->sinks, pac_foreach, &iov); + + gatt_db_attribute_notify(pac->bdb->pacs->source, iov.iov_base, + iov.iov_len, NULL); +} + +static void notify_pac_added(void *data, void *user_data) +{ + struct bt_bap_pac_changed *changed = data; + struct bt_bap_pac *pac = user_data; + + if (changed->added) + changed->added(pac, changed->data); +} + +struct bt_bap_pac *bt_bap_add_vendor_pac(struct gatt_db *db, + const char *name, uint8_t type, + uint8_t id, uint16_t cid, uint16_t vid, + struct bt_bap_pac_qos *qos, + struct iovec *data, + struct iovec *metadata) +{ + struct bt_bap_db *bdb; + struct bt_bap_pac *pac; + struct bt_bap_codec codec; + + if (!db) + return NULL; + + bdb = bap_get_db(db); + if (!bdb) + return NULL; + + codec.id = id; + codec.cid = cid; + codec.vid = vid; + + pac = bap_pac_new(bdb, name, type, &codec, qos, data, metadata); + + switch (type) { + case BT_BAP_SINK: + bap_add_sink(pac); + break; + case BT_BAP_SOURCE: + bap_add_source(pac); + break; + default: + bap_pac_free(pac); + return NULL; + } + + queue_foreach(pac_cbs, notify_pac_added, pac); + + return pac; +} + +struct bt_bap_pac *bt_bap_add_pac(struct gatt_db *db, const char *name, + uint8_t type, uint8_t id, + struct bt_bap_pac_qos *qos, + struct iovec *data, + struct iovec *metadata) +{ + return bt_bap_add_vendor_pac(db, name, type, id, 0x0000, 0x0000, qos, + data, metadata); +} + +uint8_t bt_bap_pac_get_type(struct bt_bap_pac *pac) +{ + if (!pac) + return 0x00; + + return pac->type; +} + +static void notify_pac_removed(void *data, void *user_data) +{ + struct bt_bap_pac_changed *changed = data; + struct bt_bap_pac *pac = user_data; + + if (changed->removed) + changed->removed(pac, changed->data); +} + +bool bt_bap_pac_set_ops(struct bt_bap_pac *pac, struct bt_bap_pac_ops *ops, + void *user_data) +{ + if (!pac) + return false; + + pac->ops = ops; + pac->user_data = user_data; + + return true; +} + +static bool match_stream_lpac(const void *data, const void *user_data) +{ + const struct bt_bap_stream *stream = data; + const struct bt_bap_pac *pac = user_data; + + return stream->lpac == pac; +} + +static void remove_streams(void *data, void *user_data) +{ + struct bt_bap *bap = data; + struct bt_bap_pac *pac = user_data; + struct bt_bap_stream *stream; + + stream = queue_remove_if(bap->streams, match_stream_lpac, pac); + if (stream) + bt_bap_stream_release(stream, NULL, NULL); +} + +bool bt_bap_remove_pac(struct bt_bap_pac *pac) +{ + if (!pac) + return false; + + if (queue_remove_if(pac->bdb->sinks, NULL, pac)) + goto found; + + if (queue_remove_if(pac->bdb->sources, NULL, pac)) + goto found; + + return false; + +found: + queue_foreach(sessions, remove_streams, pac); + queue_foreach(pac_cbs, notify_pac_removed, pac); + bap_pac_free(pac); + return true; +} + +static void bap_db_free(void *data) +{ + struct bt_bap_db *bdb = data; + + if (!bdb) + return; + + queue_destroy(bdb->sinks, bap_pac_free); + queue_destroy(bdb->sources, bap_pac_free); + queue_destroy(bdb->endpoints, free); + gatt_db_unref(bdb->db); + + free(bdb->pacs); + free(bdb->ascs); + free(bdb); +} + +static void bap_ready_free(void *data) +{ + struct bt_bap_ready *ready = data; + + if (ready->destroy) + ready->destroy(ready->data); + + free(ready); +} + +static void bap_state_free(void *data) +{ + struct bt_bap_state *state = data; + + if (state->destroy) + state->destroy(state->data); + + free(state); +} + +static void bap_req_free(void *data) +{ + struct bt_bap_req *req = data; + size_t i; + + queue_destroy(req->group, bap_req_free); + + for (i = 0; i < req->len; i++) + free(req->iov[i].iov_base); + + free(req->iov); + free(req); +} + +static void bap_detached(void *data, void *user_data) +{ + struct bt_bap_cb *cb = data; + struct bt_bap *bap = user_data; + + cb->detached(bap, cb->user_data); +} + +static void bap_free(void *data) +{ + struct bt_bap *bap = data; + + bt_bap_detach(bap); + + bap_db_free(bap->rdb); + + queue_destroy(bap->ready_cbs, bap_ready_free); + queue_destroy(bap->state_cbs, bap_state_free); + + queue_destroy(bap->reqs, bap_req_free); + queue_destroy(bap->pending, NULL); + queue_destroy(bap->notify, NULL); + queue_destroy(bap->streams, bap_stream_free); + + free(bap); +} + +unsigned int bt_bap_register(bt_bap_func_t attached, bt_bap_func_t detached, + void *user_data) +{ + struct bt_bap_cb *cb; + static unsigned int id; + + if (!attached && !detached) + return 0; + + if (!bap_cbs) + bap_cbs = queue_new(); + + cb = new0(struct bt_bap_cb, 1); + cb->id = ++id ? id : ++id; + cb->attached = attached; + cb->detached = detached; + cb->user_data = user_data; + + queue_push_tail(bap_cbs, cb); + + return cb->id; +} + +static bool match_id(const void *data, const void *match_data) +{ + const struct bt_bap_cb *cb = data; + unsigned int id = PTR_TO_UINT(match_data); + + return (cb->id == id); +} + +bool bt_bap_unregister(unsigned int id) +{ + struct bt_bap_cb *cb; + + cb = queue_remove_if(bap_cbs, match_id, UINT_TO_PTR(id)); + if (!cb) + return false; + + free(cb); + + return true; +} + +static void bap_attached(void *data, void *user_data) +{ + struct bt_bap_cb *cb = data; + struct bt_bap *bap = user_data; + + cb->attached(bap, cb->user_data); +} + +struct bt_bap *bt_bap_new(struct gatt_db *ldb, struct gatt_db *rdb) +{ + struct bt_bap *bap; + struct bt_bap_db *bdb; + + if (!ldb) + return NULL; + + bdb = bap_get_db(ldb); + if (!bdb) + return NULL; + + bap = new0(struct bt_bap, 1); + bap->ldb = bdb; + bap->reqs = queue_new(); + bap->pending = queue_new(); + bap->notify = queue_new(); + bap->ready_cbs = queue_new(); + bap->streams = queue_new(); + bap->state_cbs = queue_new(); + + if (!rdb) + goto done; + + bdb = new0(struct bt_bap_db, 1); + bdb->db = gatt_db_ref(rdb); + bdb->sinks = queue_new(); + bdb->sources = queue_new(); + bdb->endpoints = queue_new(); + + bap->rdb = bdb; + +done: + return bt_bap_ref(bap); +} + +bool bt_bap_set_user_data(struct bt_bap *bap, void *user_data) +{ + if (!bap) + return false; + + bap->user_data = user_data; + + return true; +} + +void *bt_bap_get_user_data(struct bt_bap *bap) +{ + if (!bap) + return NULL; + + return bap->user_data; +} + +struct bt_att *bt_bap_get_att(struct bt_bap *bap) +{ + if (!bap) + return NULL; + + if (bap->att) + return bap->att; + + return bt_gatt_client_get_att(bap->client); +} + +struct bt_bap *bt_bap_ref(struct bt_bap *bap) +{ + if (!bap) + return NULL; + + __sync_fetch_and_add(&bap->ref_count, 1); + + return bap; +} + +void bt_bap_unref(struct bt_bap *bap) +{ + if (!bap) + return; + + if (__sync_sub_and_fetch(&bap->ref_count, 1)) + return; + + bap_free(bap); +} + +static void bap_notify_ready(struct bt_bap *bap) +{ + const struct queue_entry *entry; + + if (!queue_isempty(bap->pending)) + return; + + bt_bap_ref(bap); + + for (entry = queue_get_entries(bap->ready_cbs); entry; + entry = entry->next) { + struct bt_bap_ready *ready = entry->data; + + ready->func(bap, ready->data); + } + + bt_bap_unref(bap); +} + +bool bap_print_cc(void *data, size_t len, util_debug_func_t func, + void *user_data) +{ + return bap_print_ltv("CC", data, len, func, user_data); +} + +static void bap_parse_pacs(struct bt_bap *bap, uint8_t type, + struct queue *queue, + const uint8_t *value, + uint16_t len) +{ + struct bt_pacs_read_rsp *rsp; + struct iovec iov = { + .iov_base = (void *) value, + .iov_len = len, + }; + int i; + + rsp = iov_pull_mem(&iov, sizeof(*rsp)); + if (!rsp) { + DBG(bap, "Unable to parse PAC"); + return; + } + + DBG(bap, "PAC(s) %u", rsp->num_pac); + + for (i = 0; i < rsp->num_pac; i++) { + struct bt_bap_pac *pac; + struct bt_pac *p; + struct bt_ltv *cc; + struct bt_pac_metadata *meta; + struct iovec data, metadata; + + p = iov_pull_mem(&iov, sizeof(*p)); + if (!p) { + DBG(bap, "Unable to parse PAC"); + return; + } + + pac = NULL; + + if (!bap_print_cc(iov.iov_base, p->cc_len, bap->debug_func, + bap->debug_data)) + return; + + cc = iov_pull_mem(&iov, p->cc_len); + if (!cc) { + DBG(bap, "Unable to parse PAC codec capabilities"); + return; + } + + meta = iov_pull_mem(&iov, sizeof(*meta)); + if (!meta) { + DBG(bap, "Unable to parse PAC metadata"); + return; + } + + data.iov_len = p->cc_len; + data.iov_base = cc; + + metadata.iov_len = meta->len; + metadata.iov_base = meta->data; + + iov_pull_mem(&iov, meta->len); + + pac = bap_pac_new(bap->rdb, NULL, type, &p->codec, NULL, &data, + &metadata); + if (!pac) + continue; + + DBG(bap, "PAC #%u: type %u codec 0x%02x cc_len %u meta_len %u", + i, type, p->codec.id, p->cc_len, meta->len); + + queue_push_tail(queue, pac); + } +} + +static void read_source_pac(struct bt_bap *bap, bool success, uint8_t att_ecode, + const uint8_t *value, uint16_t length, + void *user_data) +{ + if (!success) { + DBG(bap, "Unable to read Source PAC: error 0x%02x", att_ecode); + return; + } + + bap_parse_pacs(bap, BT_BAP_SOURCE, bap->rdb->sources, value, length); +} + +static void read_sink_pac(struct bt_bap *bap, bool success, uint8_t att_ecode, + const uint8_t *value, uint16_t length, + void *user_data) +{ + if (!success) { + DBG(bap, "Unable to read Sink PAC: error 0x%02x", att_ecode); + return; + } + + bap_parse_pacs(bap, BT_BAP_SINK, bap->rdb->sinks, value, length); +} + +static void read_source_pac_loc(struct bt_bap *bap, bool success, + uint8_t att_ecode, const uint8_t *value, + uint16_t length, void *user_data) +{ + struct bt_pacs *pacs = bap_get_pacs(bap); + + if (!success) { + DBG(bap, "Unable to read Source PAC Location: error 0x%02x", + att_ecode); + return; + } + + gatt_db_attribute_write(pacs->source_loc, 0, value, length, 0, NULL, + NULL, NULL); +} + +static void read_sink_pac_loc(struct bt_bap *bap, bool success, + uint8_t att_ecode, const uint8_t *value, + uint16_t length, void *user_data) +{ + struct bt_pacs *pacs = bap_get_pacs(bap); + + if (!success) { + DBG(bap, "Unable to read Sink PAC Location: error 0x%02x", + att_ecode); + return; + } + + gatt_db_attribute_write(pacs->sink_loc, 0, value, length, 0, NULL, + NULL, NULL); +} + +static void read_pac_context(struct bt_bap *bap, bool success, + uint8_t att_ecode, const uint8_t *value, + uint16_t length, void *user_data) +{ + struct bt_pacs *pacs = bap_get_pacs(bap); + + if (!success) { + DBG(bap, "Unable to read PAC Context: error 0x%02x", att_ecode); + return; + } + + gatt_db_attribute_write(pacs->context, 0, value, length, 0, NULL, + NULL, NULL); +} + +static void read_pac_supported_context(struct bt_bap *bap, bool success, + uint8_t att_ecode, const uint8_t *value, + uint16_t length, void *user_data) +{ + struct bt_pacs *pacs = bap_get_pacs(bap); + + if (!success) { + DBG(bap, "Unable to read PAC Supproted Context: error 0x%02x", + att_ecode); + return; + } + + gatt_db_attribute_write(pacs->supported_context, 0, value, length, 0, + NULL, NULL, NULL); +} + +static void bap_pending_destroy(void *data) +{ + struct bt_bap_pending *pending = data; + struct bt_bap *bap = pending->bap; + + if (queue_remove_if(bap->pending, NULL, pending)) + free(pending); + + bap_notify_ready(bap); +} + +static void bap_pending_complete(bool success, uint8_t att_ecode, + const uint8_t *value, uint16_t length, + void *user_data) +{ + struct bt_bap_pending *pending = user_data; + + if (pending->func) + pending->func(pending->bap, success, att_ecode, value, length, + pending->user_data); +} + +static void bap_read_value(struct bt_bap *bap, uint16_t value_handle, + bap_func_t func, void *user_data) +{ + struct bt_bap_pending *pending; + + pending = new0(struct bt_bap_pending, 1); + pending->bap = bap; + pending->func = func; + pending->user_data = user_data; + + pending->id = bt_gatt_client_read_value(bap->client, value_handle, + bap_pending_complete, pending, + bap_pending_destroy); + if (!pending->id) { + DBG(bap, "Unable to send Read request"); + free(pending); + return; + } + + queue_push_tail(bap->pending, pending); +} + +static void foreach_pacs_char(struct gatt_db_attribute *attr, void *user_data) +{ + struct bt_bap *bap = user_data; + uint16_t value_handle; + bt_uuid_t uuid, uuid_sink, uuid_source, uuid_sink_loc, uuid_source_loc; + bt_uuid_t uuid_context, uuid_supported_context; + struct bt_pacs *pacs; + + if (!gatt_db_attribute_get_char_data(attr, NULL, &value_handle, + NULL, NULL, &uuid)) + return; + + bt_uuid16_create(&uuid_sink, PAC_SINK_CHRC_UUID); + bt_uuid16_create(&uuid_source, PAC_SOURCE_CHRC_UUID); + bt_uuid16_create(&uuid_sink_loc, PAC_SINK_LOC_CHRC_UUID); + bt_uuid16_create(&uuid_source_loc, PAC_SOURCE_LOC_CHRC_UUID); + bt_uuid16_create(&uuid_context, PAC_CONTEXT); + bt_uuid16_create(&uuid_supported_context, PAC_SUPPORTED_CONTEXT); + + if (!bt_uuid_cmp(&uuid, &uuid_sink)) { + DBG(bap, "Sink PAC found: handle 0x%04x", value_handle); + + pacs = bap_get_pacs(bap); + if (!pacs || pacs->sink) + return; + + pacs->sink = attr; + bap_read_value(bap, value_handle, read_sink_pac, bap); + } + + if (!bt_uuid_cmp(&uuid, &uuid_source)) { + DBG(bap, "Source PAC found: handle 0x%04x", value_handle); + + pacs = bap_get_pacs(bap); + if (!pacs || pacs->source) + return; + + pacs->source = attr; + bap_read_value(bap, value_handle, read_source_pac, NULL); + } + + if (!bt_uuid_cmp(&uuid, &uuid_sink_loc)) { + DBG(bap, "Sink PAC Location found: handle 0x%04x", + value_handle); + + pacs = bap_get_pacs(bap); + if (!pacs || pacs->sink_loc) + return; + + pacs->sink_loc = attr; + bap_read_value(bap, value_handle, read_sink_pac_loc, NULL); + } + + if (!bt_uuid_cmp(&uuid, &uuid_source_loc)) { + DBG(bap, "Source PAC Location found: handle 0x%04x", + value_handle); + + pacs = bap_get_pacs(bap); + if (!pacs || pacs->source_loc) + return; + + pacs->source_loc = attr; + bap_read_value(bap, value_handle, read_source_pac_loc, NULL); + } + + if (!bt_uuid_cmp(&uuid, &uuid_context)) { + DBG(bap, "PAC Context found: handle 0x%04x", value_handle); + + pacs = bap_get_pacs(bap); + if (!pacs || pacs->context) + return; + + pacs->context = attr; + bap_read_value(bap, value_handle, read_pac_context, NULL); + } + + if (!bt_uuid_cmp(&uuid, &uuid_supported_context)) { + DBG(bap, "PAC Supported Context found: handle 0x%04x", + value_handle); + + pacs = bap_get_pacs(bap); + if (!pacs || pacs->supported_context) + return; + + pacs->supported_context = attr; + bap_read_value(bap, value_handle, read_pac_supported_context, + NULL); + } +} + +static void foreach_pacs_service(struct gatt_db_attribute *attr, + void *user_data) +{ + struct bt_bap *bap = user_data; + struct bt_pacs *pacs = bap_get_pacs(bap); + + pacs->service = attr; + + gatt_db_service_foreach_char(attr, foreach_pacs_char, bap); +} + +struct match_pac { + struct bt_bap_codec codec; + struct bt_bap_pac *lpac; + struct bt_bap_pac *rpac; + struct bt_bap_endpoint *ep; +}; + +static bool match_stream_pac(struct bt_bap_pac *lpac, struct bt_bap_pac *rpac, + void *user_data) +{ + struct match_pac *match = user_data; + + if (!bap_codec_equal(&match->codec, &lpac->codec)) + return true; + + match->lpac = lpac; + match->rpac = rpac; + + return false; +} + +static void ep_status_config(struct bt_bap *bap, struct bt_bap_endpoint *ep, + struct iovec *iov) +{ + struct bt_ascs_ase_status_config *cfg; + struct bt_ltv *cc; + uint32_t pd_min, pd_max, ppd_min, ppd_max; + int i; + + cfg = iov_pull_mem(iov, sizeof(*cfg)); + if (!cfg) { + DBG(bap, "Unable to parse Config Status"); + return; + } + + pd_min = get_le24(cfg->pd_min); + pd_max = get_le24(cfg->pd_max); + ppd_min = get_le24(cfg->ppd_min); + ppd_max = get_le24(cfg->ppd_max); + + DBG(bap, "codec 0x%02x framing 0x%02x phy 0x%02x rtn %u " + "latency %u pd %u - %u ppd %u - %u", cfg->codec.id, + cfg->framing, cfg->phy, cfg->rtn, + le16_to_cpu(cfg->latency), + pd_min, pd_max, ppd_min, ppd_max); + + if (iov->iov_len < cfg->cc_len) { + DBG(bap, "Unable to parse Config Status: len %zu < %u cc_len", + iov->iov_len, cfg->cc_len); + return; + } + + for (i = 0; iov->iov_len >= sizeof(*cc); i++) { + cc = iov_pull_mem(iov, sizeof(*cc)); + if (!cc) + break; + + DBG(bap, "Codec Config #%u: type 0x%02x len %u", i, + cc->type, cc->len); + + iov_pull_mem(iov, cc->len - 1); + } + + /* Any previously applied codec configuration may be cached by the + * server. + */ + if (!ep->stream) { + struct match_pac match; + + match.lpac = NULL; + match.rpac = NULL; + match.codec.id = cfg->codec.id; + match.codec.cid = le16_to_cpu(cfg->codec.cid); + match.codec.vid = le16_to_cpu(cfg->codec.vid); + + bt_bap_foreach_pac(bap, ep->dir, match_stream_pac, &match); + if (!match.lpac || !match.rpac) + return; + + bap_stream_new(bap, ep, match.lpac, match.rpac, NULL, true); + } + + if (!ep->stream) + return; + + /* Set preferred settings */ + if (ep->stream->rpac) { + ep->stream->rpac->qos.framing = cfg->framing; + ep->stream->rpac->qos.phy = cfg->phy; + ep->stream->rpac->qos.rtn = cfg->rtn; + ep->stream->rpac->qos.latency = le16_to_cpu(cfg->latency); + ep->stream->rpac->qos.pd_min = pd_min; + ep->stream->rpac->qos.pd_max = pd_max; + ep->stream->rpac->qos.ppd_min = ppd_min; + ep->stream->rpac->qos.ppd_max = ppd_max; + } + + if (!ep->stream->cc) + ep->stream->cc = new0(struct iovec, 1); + + iov_memcpy(ep->stream->cc, cfg->cc, cfg->cc_len); +} + +static void bap_stream_config_cfm_cb(struct bt_bap_stream *stream, int err) +{ + struct bt_bap *bap = stream->bap; + + if (err) { + DBG(bap, "Config Confirmation failed: %d", err); + bt_bap_stream_release(stream, NULL, NULL); + return; + } +} + +static void bap_stream_config_cfm(struct bt_bap_stream *stream) +{ + int err; + + if (!stream->lpac->ops || !stream->lpac->ops->config) + return; + + err = stream->lpac->ops->config(stream, stream->cc, &stream->qos, + bap_stream_config_cfm_cb, + stream->lpac->user_data); + if (err < 0) { + DBG(stream->bap, "Unable to send Config Confirmation: %d", + err); + bt_bap_stream_release(stream, NULL, NULL); + } +} + +static void ep_status_qos(struct bt_bap *bap, struct bt_bap_endpoint *ep, + struct iovec *iov) +{ + struct bt_ascs_ase_status_qos *qos; + uint32_t interval; + uint32_t pd; + uint16_t sdu; + uint16_t latency; + + qos = iov_pull_mem(iov, sizeof(*qos)); + if (!qos) { + DBG(bap, "Unable to parse QoS Status"); + return; + } + + interval = get_le24(qos->interval); + pd = get_le24(qos->pd); + sdu = le16_to_cpu(qos->sdu); + latency = le16_to_cpu(qos->latency); + + DBG(bap, "CIG 0x%02x CIS 0x%02x interval %u framing 0x%02x " + "phy 0x%02x rtn %u latency %u pd %u", qos->cig_id, + qos->cis_id, interval, qos->framing, qos->phy, + qos->rtn, latency, pd); + + if (!ep->stream) + return; + + ep->stream->qos.interval = interval; + ep->stream->qos.framing = qos->framing; + ep->stream->qos.phy = qos->phy; + ep->stream->qos.sdu = sdu; + ep->stream->qos.rtn = qos->rtn; + ep->stream->qos.latency = latency; + ep->stream->qos.delay = pd; + + if (ep->old_state == BT_ASCS_ASE_STATE_CONFIG) + bap_stream_config_cfm(ep->stream); +} + +static void ep_status_metadata(struct bt_bap *bap, struct bt_bap_endpoint *ep, + struct iovec *iov) +{ + struct bt_ascs_ase_status_metadata *meta; + + meta = iov_pull_mem(iov, sizeof(*meta)); + if (!meta) { + DBG(bap, "Unable to parse Metadata Status"); + return; + } + + DBG(bap, "CIS 0x%02x CIG 0x%02x metadata len %u", + meta->cis_id, meta->cig_id, meta->len); +} + +static void bap_ep_set_status(struct bt_bap *bap, struct bt_bap_endpoint *ep, + const uint8_t *value, uint16_t length) +{ + struct bt_ascs_ase_status *rsp; + struct iovec iov = { + .iov_base = (void *) value, + .iov_len = length, + }; + + rsp = iov_pull_mem(&iov, sizeof(*rsp)); + if (!rsp) + return; + + ep->id = rsp->id; + ep->old_state = ep->state; + ep->state = rsp->state; + + DBG(bap, "ASE status: ep %p id 0x%02x handle 0x%04x state %s " + "len %zu", ep, ep->id, + gatt_db_attribute_get_handle(ep->attr), + bt_bap_stream_statestr(ep->state), iov.iov_len); + + switch (ep->state) { + case BT_ASCS_ASE_STATE_IDLE: + break; + case BT_ASCS_ASE_STATE_CONFIG: + ep_status_config(bap, ep, &iov); + break; + case BT_ASCS_ASE_STATE_QOS: + ep_status_qos(bap, ep, &iov); + break; + case BT_ASCS_ASE_STATE_ENABLING: + case BT_ASCS_ASE_STATE_STREAMING: + case BT_ASCS_ASE_STATE_DISABLING: + ep_status_metadata(bap, ep, &iov); + break; + case BT_ASCS_ASE_STATE_RELEASING: + break; + } + + /* Only notifify if there is a stream */ + if (!ep->stream) + return; + + bap_stream_state_changed(ep->stream); +} + +static void read_ase_status(struct bt_bap *bap, bool success, uint8_t att_ecode, + const uint8_t *value, uint16_t length, + void *user_data) +{ + struct bt_bap_endpoint *ep = user_data; + + if (!success) + return; + + bap_ep_set_status(bap, ep, value, length); +} + +static void bap_register(uint16_t att_ecode, void *user_data) +{ + struct bt_bap_notify *notify = user_data; + + if (att_ecode) + DBG(notify->bap, "ASE register failed: 0x%04x", att_ecode); +} + +static void bap_endpoint_notify(struct bt_bap *bap, uint16_t value_handle, + const uint8_t *value, uint16_t length, + void *user_data) +{ + struct bt_bap_endpoint *ep = user_data; + + bap_ep_set_status(bap, ep, value, length); +} + +static void bap_notify(uint16_t value_handle, const uint8_t *value, + uint16_t length, void *user_data) +{ + struct bt_bap_notify *notify = user_data; + + if (notify->func) + notify->func(notify->bap, value_handle, value, length, + notify->user_data); +} + +static void bap_notify_destroy(void *data) +{ + struct bt_bap_notify *notify = data; + struct bt_bap *bap = notify->bap; + + if (queue_remove_if(bap->notify, NULL, notify)) + free(notify); +} + +static unsigned int bap_register_notify(struct bt_bap *bap, + uint16_t value_handle, + bap_notify_t func, + void *user_data) +{ + struct bt_bap_notify *notify; + + notify = new0(struct bt_bap_notify, 1); + notify->bap = bap; + notify->func = func; + notify->user_data = user_data; + + notify->id = bt_gatt_client_register_notify(bap->client, + value_handle, bap_register, + bap_notify, notify, + bap_notify_destroy); + if (!notify->id) { + DBG(bap, "Unable to register for notifications"); + free(notify); + return 0; + } + + queue_push_tail(bap->notify, notify); + + return notify->id; +} + +static void bap_endpoint_attach(struct bt_bap *bap, struct bt_bap_endpoint *ep) +{ + uint16_t value_handle; + + if (!gatt_db_attribute_get_char_data(ep->attr, NULL, &value_handle, + NULL, NULL, NULL)) + return; + + DBG(bap, "ASE handle 0x%04x", value_handle); + + bap_read_value(bap, value_handle, read_ase_status, ep); + + ep->state_id = bap_register_notify(bap, value_handle, + bap_endpoint_notify, ep); +} + +static void append_group(void *data, void *user_data) +{ + struct bt_bap_req *req = data; + struct iovec *iov = user_data; + size_t i; + + for (i = 0; i < req->len; i++) + iov_add_mem(iov, req->iov[i].iov_len, req->iov[i].iov_base); +} + +static bool bap_send(struct bt_bap *bap, struct bt_bap_req *req) +{ + struct bt_ascs *ascs = bap_get_ascs(bap); + int ret; + uint16_t handle; + uint8_t buf[64]; + struct bt_ascs_ase_hdr hdr; + struct iovec iov = { + .iov_base = buf, + .iov_len = 0, + }; + size_t i; + + if (!gatt_db_attribute_get_char_data(ascs->ase_cp, NULL, &handle, + NULL, NULL, NULL)) + return false; + + hdr.op = req->op; + hdr.num = 1 + queue_length(req->group); + + iov_add_mem(&iov, sizeof(hdr), &hdr); + + for (i = 0; i < req->len; i++) + iov_add_mem(&iov, req->iov[i].iov_len, req->iov[i].iov_base); + + /* Append the request group with the same opcode */ + queue_foreach(req->group, append_group, &iov); + + ret = bt_gatt_client_write_without_response(bap->client, handle, + false, iov.iov_base, + iov.iov_len); + if (!ret) + return false; + + bap->req = req; + + return false; +} + +static bool bap_process_queue(void *data) +{ + struct bt_bap *bap = data; + struct bt_bap_req *req; + + if (bap->process_id) { + timeout_remove(bap->process_id); + bap->process_id = 0; + } + + while ((req = queue_pop_head(bap->reqs))) { + if (!bap_send(bap, req)) + break; + } + + return false; +} + +static bool match_req(const void *data, const void *match_data) +{ + const struct bt_bap_req *pend = data; + const struct bt_bap_req *req = match_data; + + return pend->op == req->op; +} + +static bool bap_queue_req(struct bt_bap *bap, struct bt_bap_req *req) +{ + struct bt_bap_req *pend; + struct queue *queue; + + pend = queue_find(bap->reqs, match_req, req); + if (pend) { + if (!pend->group) + pend->group = queue_new(); + /* Group requests with the same opcode */ + queue = pend->group; + } else { + queue = bap->reqs; + } + + DBG(bap, "req %p (op 0x%2.2x) queue %p", req, req->op, queue); + + if (!queue_push_tail(queue, req)) { + DBG(bap, "Unable to queue request"); + return false; + } + + /* Only attempot to process queue if there is no outstanding request + * and it has not been scheduled. + */ + if (!bap->req && !bap->process_id) + bap->process_id = timeout_add(BAP_PROCESS_TIMEOUT, + bap_process_queue, bap, NULL); + + return true; +} + +static void bap_req_complete(struct bt_bap_req *req, + const struct bt_ascs_ase_rsp *rsp) +{ + struct queue *group; + + if (!req->func) + goto done; + + if (rsp) + req->func(req->stream, rsp->code, rsp->reason, req->user_data); + else + req->func(req->stream, BT_ASCS_RSP_UNSPECIFIED, 0x00, + req->user_data); + +done: + /* Detach from request so it can be freed separately */ + group = req->group; + req->group = NULL; + + queue_foreach(group, (queue_foreach_func_t)bap_req_complete, + (void *)rsp); + + queue_destroy(group, NULL); + + bap_req_free(req); +} + +static void bap_cp_notify(struct bt_bap *bap, uint16_t value_handle, + const uint8_t *value, uint16_t length, + void *user_data) +{ + const struct bt_ascs_cp_rsp *rsp = (void *)value; + const struct bt_ascs_ase_rsp *ase_rsp = NULL; + struct bt_bap_req *req; + int i; + + if (!bap->req) + return; + + req = bap->req; + bap->req = NULL; + + if (length < sizeof(*rsp)) { + DBG(bap, "Invalid ASE CP notification: length %u < %zu", + length, sizeof(*rsp)); + goto done; + } + + if (rsp->op != req->op) { + DBG(bap, "Invalid ASE CP notification: op 0x%02x != 0x%02x", + rsp->op, req->op); + goto done; + } + + length -= sizeof(*rsp); + + if (rsp->num_ase == 0xff) { + ase_rsp = rsp->rsp; + goto done; + } + + for (i = 0; i < rsp->num_ase; i++) { + if (length < sizeof(*ase_rsp)) { + DBG(bap, "Invalid ASE CP notification: length %u < %zu", + length, sizeof(*ase_rsp)); + goto done; + } + + ase_rsp = &rsp->rsp[i]; + } + +done: + bap_req_complete(req, ase_rsp); + bap_process_queue(bap); +} + +static void bap_cp_attach(struct bt_bap *bap) +{ + uint16_t value_handle; + struct bt_ascs *ascs = bap_get_ascs(bap); + + if (!gatt_db_attribute_get_char_data(ascs->ase_cp, NULL, + &value_handle, + NULL, NULL, NULL)) + return; + + DBG(bap, "ASE CP handle 0x%04x", value_handle); + + bap->cp_id = bap_register_notify(bap, value_handle, bap_cp_notify, + NULL); +} + +static void foreach_ascs_char(struct gatt_db_attribute *attr, void *user_data) +{ + struct bt_bap *bap = user_data; + uint16_t value_handle; + bt_uuid_t uuid, uuid_sink, uuid_source, uuid_cp; + struct bt_ascs *ascs; + + if (!gatt_db_attribute_get_char_data(attr, NULL, &value_handle, + NULL, NULL, &uuid)) + return; + + bt_uuid16_create(&uuid_sink, ASE_SINK_UUID); + bt_uuid16_create(&uuid_source, ASE_SOURCE_UUID); + bt_uuid16_create(&uuid_cp, ASE_CP_UUID); + + if (!bt_uuid_cmp(&uuid, &uuid_sink) || + !bt_uuid_cmp(&uuid, &uuid_source)) { + struct bt_bap_endpoint *ep; + + ep = bap_get_endpoint(bap->rdb, attr); + if (!ep) + return; + + if (!bt_uuid_cmp(&uuid, &uuid_sink)) + DBG(bap, "ASE Sink found: handle 0x%04x", value_handle); + else + DBG(bap, "ASE Source found: handle 0x%04x", + value_handle); + + bap_endpoint_attach(bap, ep); + + return; + } + + if (!bt_uuid_cmp(&uuid, &uuid_cp)) { + ascs = bap_get_ascs(bap); + if (!ascs || ascs->ase_cp) + return; + + ascs->ase_cp = attr; + + DBG(bap, "ASE Control Point found: handle 0x%04x", + value_handle); + + bap_cp_attach(bap); + } +} + +static void foreach_ascs_service(struct gatt_db_attribute *attr, + void *user_data) +{ + struct bt_bap *bap = user_data; + struct bt_ascs *ascs = bap_get_ascs(bap); + + ascs->service = attr; + + gatt_db_service_set_claimed(attr, true); + + gatt_db_service_foreach_char(attr, foreach_ascs_char, bap); +} + +static void bap_endpoint_foreach(void *data, void *user_data) +{ + struct bt_bap_endpoint *ep = data; + struct bt_bap *bap = user_data; + + bap_endpoint_attach(bap, ep); +} + +static void bap_attach_att(struct bt_bap *bap, struct bt_att *att) +{ + if (bap->disconn_id) { + if (att == bt_bap_get_att(bap)) + return; + bt_att_unregister_disconnect(bap->att, bap->disconn_id); + } + + bap->disconn_id = bt_att_register_disconnect(bap->att, + bap_disconnected, + bap, NULL); +} + +bool bt_bap_attach(struct bt_bap *bap, struct bt_gatt_client *client) +{ + bt_uuid_t uuid; + + if (queue_find(sessions, NULL, bap)) { + /* If instance already been set but there is no client proceed + * to clone it otherwise considered it already attached. + */ + if (client && !bap->client) + goto clone; + return true; + } + + if (!sessions) + sessions = queue_new(); + + queue_push_tail(sessions, bap); + + queue_foreach(bap_cbs, bap_attached, bap); + + if (!client) { + bap_attach_att(bap, bap->att); + return true; + } + + if (bap->client) + return false; + +clone: + bap->client = bt_gatt_client_clone(client); + if (!bap->client) + return false; + + bap_attach_att(bap, bt_gatt_client_get_att(client)); + + if (bap->rdb->pacs) { + uint16_t value_handle; + struct bt_pacs *pacs = bap->rdb->pacs; + + /* Resume reading sinks if supported */ + if (pacs->sink && queue_isempty(bap->rdb->sinks)) { + if (gatt_db_attribute_get_char_data(pacs->sink, + NULL, &value_handle, + NULL, NULL, NULL)) { + bap_read_value(bap, value_handle, + read_sink_pac, bap); + } + } + + /* Resume reading sources if supported */ + if (pacs->source && queue_isempty(bap->rdb->sources)) { + if (gatt_db_attribute_get_char_data(pacs->source, + NULL, &value_handle, + NULL, NULL, NULL)) { + bap_read_value(bap, value_handle, + read_source_pac, bap); + } + } + + queue_foreach(bap->rdb->endpoints, bap_endpoint_foreach, bap); + + bap_cp_attach(bap); + + bap_notify_ready(bap); + + return true; + } + + bt_uuid16_create(&uuid, PACS_UUID); + gatt_db_foreach_service(bap->rdb->db, &uuid, foreach_pacs_service, bap); + + bt_uuid16_create(&uuid, ASCS_UUID); + gatt_db_foreach_service(bap->rdb->db, &uuid, foreach_ascs_service, bap); + + return true; +} + +static void stream_foreach_detach(void *data, void *user_data) +{ + struct bt_bap_stream *stream = data; + + stream_set_state(stream, BT_BAP_STREAM_STATE_IDLE); +} + +void bt_bap_detach(struct bt_bap *bap) +{ + DBG(bap, "%p", bap); + + if (!queue_remove(sessions, bap)) + return; + + bt_gatt_client_unref(bap->client); + bap->client = NULL; + + bt_att_unregister_disconnect(bap->att, bap->disconn_id); + bap->att = NULL; + + queue_foreach(bap->streams, stream_foreach_detach, bap); + queue_foreach(bap_cbs, bap_detached, bap); +} + +bool bt_bap_set_debug(struct bt_bap *bap, bt_bap_debug_func_t func, + void *user_data, bt_bap_destroy_func_t destroy) +{ + if (!bap) + return false; + + if (bap->debug_destroy) + bap->debug_destroy(bap->debug_data); + + bap->debug_func = func; + bap->debug_destroy = destroy; + bap->debug_data = user_data; + + return true; +} + +unsigned int bt_bap_ready_register(struct bt_bap *bap, + bt_bap_ready_func_t func, void *user_data, + bt_bap_destroy_func_t destroy) +{ + struct bt_bap_ready *ready; + static unsigned int id; + + if (!bap) + return 0; + + ready = new0(struct bt_bap_ready, 1); + ready->id = ++id ? id : ++id; + ready->func = func; + ready->destroy = destroy; + ready->data = user_data; + + queue_push_tail(bap->ready_cbs, ready); + + return ready->id; +} + +static bool match_ready_id(const void *data, const void *match_data) +{ + const struct bt_bap_ready *ready = data; + unsigned int id = PTR_TO_UINT(match_data); + + return (ready->id == id); +} + +bool bt_bap_ready_unregister(struct bt_bap *bap, unsigned int id) +{ + struct bt_bap_ready *ready; + + ready = queue_remove_if(bap->ready_cbs, match_ready_id, + UINT_TO_PTR(id)); + if (!ready) + return false; + + bap_ready_free(ready); + + return true; +} + +unsigned int bt_bap_state_register(struct bt_bap *bap, + bt_bap_state_func_t func, + bt_bap_connecting_func_t connecting, + void *user_data, bt_bap_destroy_func_t destroy) +{ + struct bt_bap_state *state; + static unsigned int id; + + if (!bap) + return 0; + + state = new0(struct bt_bap_state, 1); + state->id = ++id ? id : ++id; + state->func = func; + state->connecting = connecting; + state->destroy = destroy; + state->data = user_data; + + queue_push_tail(bap->state_cbs, state); + + return state->id; +} + +static bool match_state_id(const void *data, const void *match_data) +{ + const struct bt_bap_state *state = data; + unsigned int id = PTR_TO_UINT(match_data); + + return (state->id == id); +} + +bool bt_bap_state_unregister(struct bt_bap *bap, unsigned int id) +{ + struct bt_bap_state *state; + + if (!bap) + return false; + + state = queue_remove_if(bap->state_cbs, match_state_id, + UINT_TO_PTR(id)); + if (!state) + return false; + + bap_state_free(state); + + return false; +} + +const char *bt_bap_stream_statestr(uint8_t state) +{ + switch (state) { + case BT_BAP_STREAM_STATE_IDLE: + return "idle"; + case BT_BAP_STREAM_STATE_CONFIG: + return "config"; + case BT_BAP_STREAM_STATE_QOS: + return "qos"; + case BT_BAP_STREAM_STATE_ENABLING: + return "enabling"; + case BT_BAP_STREAM_STATE_STREAMING: + return "streaming"; + case BT_BAP_STREAM_STATE_DISABLING: + return "disabling"; + case BT_BAP_STREAM_STATE_RELEASING: + return "releasing"; + } + + return "unknown"; +} + +static void bap_foreach_pac(struct queue *l, struct queue *r, + bt_bap_pac_foreach_t func, void *user_data) +{ + const struct queue_entry *el; + + for (el = queue_get_entries(l); el; el = el->next) { + struct bt_bap_pac *lpac = el->data; + const struct queue_entry *er; + + for (er = queue_get_entries(r); er; er = er->next) { + struct bt_bap_pac *rpac = er->data; + + if (!bap_codec_equal(&lpac->codec, &rpac->codec)) + continue; + + if (!func(lpac, rpac, user_data)) + return; + } + } +} + +void bt_bap_foreach_pac(struct bt_bap *bap, uint8_t type, + bt_bap_pac_foreach_t func, void *user_data) +{ + if (!bap || !func || !bap->rdb || queue_isempty(bap_db)) + return; + + switch (type) { + case BT_BAP_SINK: + return bap_foreach_pac(bap->ldb->sources, bap->rdb->sinks, + func, user_data); + case BT_BAP_SOURCE: + return bap_foreach_pac(bap->ldb->sinks, bap->rdb->sources, + func, user_data); + } +} + +int bt_bap_pac_get_vendor_codec(struct bt_bap_pac *pac, uint8_t *id, + uint16_t *cid, uint16_t *vid, + struct iovec **data, struct iovec **metadata) +{ + if (!pac) + return -EINVAL; + + if (id) + *id = pac->codec.id; + + if (cid) + *cid = pac->codec.cid; + + if (vid) + *vid = pac->codec.cid; + + if (data) + *data = pac->data; + + if (metadata) + *metadata = pac->metadata; + + return 0; +} + +int bt_bap_pac_get_codec(struct bt_bap_pac *pac, uint8_t *id, + struct iovec **data, struct iovec **metadata) +{ + return bt_bap_pac_get_vendor_codec(pac, id, NULL, NULL, data, metadata); +} + +void bt_bap_pac_set_user_data(struct bt_bap_pac *pac, void *user_data) +{ + pac->user_data = user_data; +} + +void *bt_bap_pac_get_user_data(struct bt_bap_pac *pac) +{ + return pac->user_data; +} + +static bool find_ep_unused(const void *data, const void *user_data) +{ + const struct bt_bap_endpoint *ep = data; + const struct match_pac *match = user_data; + + if (ep->stream) + return false; + + return ep->dir == match->rpac->type; +} + +static bool find_ep_pacs(const void *data, const void *user_data) +{ + const struct bt_bap_endpoint *ep = data; + const struct match_pac *match = user_data; + + if (!ep->stream) + return false; + + if (ep->stream->lpac != match->lpac) + return false; + + return ep->stream->rpac == match->rpac; +} + +static struct bt_bap_req *bap_req_new(struct bt_bap_stream *stream, + uint8_t op, struct iovec *iov, + size_t len, + bt_bap_stream_func_t func, + void *user_data) +{ + struct bt_bap_req *req; + static unsigned int id; + + req = new0(struct bt_bap_req, 1); + req->id = ++id; + req->stream = stream; + req->op = op; + req->iov = iov_dup(iov, len); + req->len = len; + req->func = func; + req->user_data = user_data; + + return req; +} + +static bool bap_stream_valid(struct bt_bap_stream *stream) +{ + if (!stream || !stream->bap) + return false; + + return queue_find(stream->bap->streams, NULL, stream); +} + +unsigned int bt_bap_stream_config(struct bt_bap_stream *stream, + struct bt_bap_qos *qos, + struct iovec *data, + bt_bap_stream_func_t func, + void *user_data) +{ + struct iovec iov[2]; + struct bt_ascs_config config; + uint8_t iovlen = 1; + struct bt_bap_req *req; + + if (!bap_stream_valid(stream)) + return 0; + + if (!stream->client) { + stream_config(stream, data, NULL); + return 0; + } + + memset(&config, 0, sizeof(config)); + + config.ase = stream->ep->id; + config.latency = qos->target_latency; + config.phy = qos->phy; + config.codec = stream->rpac->codec; + + iov[0].iov_base = &config; + iov[0].iov_len = sizeof(config); + + if (data) { + if (!bap_print_cc(data->iov_base, data->iov_len, + stream->bap->debug_func, + stream->bap->debug_data)) + return 0; + + config.cc_len = data->iov_len; + iov[1] = *data; + iovlen++; + } + + req = bap_req_new(stream, BT_ASCS_CONFIG, iov, iovlen, func, user_data); + + if (!bap_queue_req(stream->bap, req)) { + bap_req_free(req); + return 0; + } + + stream->qos = *qos; + + return req->id; +} + +static bool match_pac(struct bt_bap_pac *lpac, struct bt_bap_pac *rpac, + void *user_data) +{ + struct match_pac *match = user_data; + + if (match->lpac && match->lpac != lpac) + return true; + + if (match->rpac && match->rpac != rpac) + return true; + + match->lpac = lpac; + match->rpac = rpac; + + return false; +} + +int bt_bap_select(struct bt_bap_pac *lpac, struct bt_bap_pac *rpac, + bt_bap_pac_select_t func, void *user_data) +{ + if (!lpac || !rpac || !func) + return -EINVAL; + + if (!lpac->ops || !lpac->ops->select) + return -EOPNOTSUPP; + + lpac->ops->select(lpac, rpac, &rpac->qos, + func, user_data, lpac->user_data); + + return 0; +} + +struct bt_bap_stream *bt_bap_config(struct bt_bap *bap, + struct bt_bap_pac *lpac, + struct bt_bap_pac *rpac, + struct bt_bap_qos *pqos, + struct iovec *data, + bt_bap_stream_func_t func, + void *user_data) +{ + struct bt_bap_stream *stream; + struct bt_bap_endpoint *ep; + struct match_pac match; + int id; + + if (!bap || !bap->rdb || queue_isempty(bap->rdb->endpoints)) + return NULL; + + if (lpac && rpac) { + if (!bap_codec_equal(&lpac->codec, &rpac->codec)) + return NULL; + } else { + uint8_t type; + + match.lpac = lpac; + match.rpac = rpac; + memset(&match.codec, 0, sizeof(match.codec)); + + if (rpac) + type = rpac->type; + else if (lpac) { + switch(lpac->type) { + case BT_BAP_SINK: + type = BT_BAP_SOURCE; + break; + case BT_BAP_SOURCE: + type = BT_BAP_SINK; + break; + default: + return NULL; + } + } else + return NULL; + + bt_bap_foreach_pac(bap, type, match_pac, &match); + if (!match.lpac || !match.rpac) + return NULL; + + lpac = match.lpac; + rpac = match.rpac; + } + + match.lpac = lpac; + match.rpac = rpac; + + /* Check for existing stream */ + ep = queue_find(bap->rdb->endpoints, find_ep_pacs, &match); + if (!ep) { + /* Check for unused ASE */ + ep = queue_find(bap->rdb->endpoints, find_ep_unused, &match); + if (!ep) { + DBG(bap, "Unable to find unused ASE"); + return NULL; + } + } + + stream = ep->stream; + if (!stream) + stream = bap_stream_new(bap, ep, lpac, rpac, data, true); + + id = bt_bap_stream_config(stream, pqos, data, func, user_data); + if (!id) { + DBG(bap, "Unable to config stream"); + queue_remove(bap->streams, stream); + ep->stream = NULL; + free(stream); + return NULL; + } + + return stream; +} + +struct bt_bap *bt_bap_stream_get_session(struct bt_bap_stream *stream) +{ + if (!stream) + return NULL; + + return stream->bap; +} + +uint8_t bt_bap_stream_get_state(struct bt_bap_stream *stream) +{ + if (!stream) + return BT_BAP_STREAM_STATE_IDLE; + + return stream->ep->state; +} + +bool bt_bap_stream_set_user_data(struct bt_bap_stream *stream, void *user_data) +{ + if (!stream) + return false; + + stream->user_data = user_data; + + return true; +} + +void *bt_bap_stream_get_user_data(struct bt_bap_stream *stream) +{ + if (!stream) + return NULL; + + return stream->user_data; +} + +unsigned int bt_bap_stream_qos(struct bt_bap_stream *stream, + struct bt_bap_qos *data, + bt_bap_stream_func_t func, + void *user_data) +{ + struct iovec iov; + struct bt_ascs_qos qos; + struct bt_bap_req *req; + + if (!bap_stream_valid(stream)) + return 0; + + if (!stream->client) { + stream_qos(stream, data, NULL); + return 0; + } + + memset(&qos, 0, sizeof(qos)); + + /* TODO: Figure out how to pass these values around */ + qos.ase = stream->ep->id; + qos.cig = data->cig_id; + qos.cis = data->cis_id; + put_le24(data->interval, qos.interval); + qos.framing = data->framing; + qos.phy = data->phy; + qos.sdu = cpu_to_le16(data->sdu); + qos.rtn = data->rtn; + qos.latency = cpu_to_le16(data->latency); + put_le24(data->delay, qos.pd); + + iov.iov_base = &qos; + iov.iov_len = sizeof(qos); + + req = bap_req_new(stream, BT_ASCS_QOS, &iov, 1, func, user_data); + + if (!bap_queue_req(stream->bap, req)) { + bap_req_free(req); + return 0; + } + + stream->qos = *data; + + return req->id; +} + +static int bap_stream_metadata(struct bt_bap_stream *stream, uint8_t op, + struct iovec *data, + bt_bap_stream_func_t func, + void *user_data) +{ + struct iovec iov[2]; + struct bt_ascs_metadata meta; + struct bt_bap_req *req; + struct metadata { + uint8_t len; + uint8_t type; + uint8_t data[2]; + } ctx = LTV(0x02, 0x01, 0x00); /* Context = Unspecified */ + + memset(&meta, 0, sizeof(meta)); + + meta.ase = stream->ep->id; + + iov[0].iov_base = &meta; + iov[0].iov_len = sizeof(meta); + + if (data) + iov[1] = *data; + else { + iov[1].iov_base = &ctx; + iov[1].iov_len = sizeof(ctx); + } + + meta.len = iov[1].iov_len; + + req = bap_req_new(stream, op, iov, 2, func, user_data); + + if (!bap_queue_req(stream->bap, req)) { + bap_req_free(req); + return 0; + } + + return req->id; +} + +static void bap_stream_enable_link(void *data, void *user_data) +{ + struct bt_bap_stream *stream = data; + struct iovec *metadata = user_data; + + bap_stream_metadata(stream, BT_ASCS_ENABLE, metadata, NULL, NULL); +} + +unsigned int bt_bap_stream_enable(struct bt_bap_stream *stream, + bool enable_links, + struct iovec *metadata, + bt_bap_stream_func_t func, + void *user_data) +{ + int ret; + + if (!bap_stream_valid(stream)) + return 0; + + if (!stream->client) { + stream_enable(stream, metadata, NULL); + return 0; + } + + ret = bap_stream_metadata(stream, BT_ASCS_ENABLE, metadata, func, + user_data); + if (!ret || !enable_links) + return ret; + + queue_foreach(stream->links, bap_stream_enable_link, metadata); + + return ret; +} + +unsigned int bt_bap_stream_start(struct bt_bap_stream *stream, + bt_bap_stream_func_t func, + void *user_data) +{ + struct iovec iov; + struct bt_ascs_start start; + struct bt_bap_req *req; + + if (!bap_stream_valid(stream)) + return 0; + + if (!stream->client) { + if (stream->ep->dir == BT_BAP_SINK) + stream_start(stream, NULL); + return 0; + } + + if (stream->ep->dir == BT_BAP_SINK) + return 0; + + memset(&start, 0, sizeof(start)); + + start.ase = stream->ep->id; + + iov.iov_base = &start; + iov.iov_len = sizeof(start); + + req = bap_req_new(stream, BT_ASCS_START, &iov, 1, func, user_data); + + if (!bap_queue_req(stream->bap, req)) { + bap_req_free(req); + return 0; + } + + return req->id; +} + +static void bap_stream_disable_link(void *data, void *user_data) +{ + struct bt_bap_stream *stream = data; + struct bt_bap_req *req; + struct iovec iov; + struct bt_ascs_disable disable; + + memset(&disable, 0, sizeof(disable)); + + disable.ase = stream->ep->id; + + iov.iov_base = &disable; + iov.iov_len = sizeof(disable); + + req = bap_req_new(stream, BT_ASCS_DISABLE, &iov, 1, NULL, NULL); + + if (!bap_queue_req(stream->bap, req)) + bap_req_free(req); +} + +unsigned int bt_bap_stream_disable(struct bt_bap_stream *stream, + bool disable_links, + bt_bap_stream_func_t func, + void *user_data) +{ + struct iovec iov; + struct bt_ascs_disable disable; + struct bt_bap_req *req; + + if (!bap_stream_valid(stream)) + return 0; + + if (!stream->client) { + stream_disable(stream, NULL); + return 0; + } + + memset(&disable, 0, sizeof(disable)); + + disable.ase = stream->ep->id; + + iov.iov_base = &disable; + iov.iov_len = sizeof(disable); + + req = bap_req_new(stream, BT_ASCS_DISABLE, &iov, 1, func, user_data); + + if (!bap_queue_req(stream->bap, req)) { + bap_req_free(req); + return 0; + } + + if (disable_links) + queue_foreach(stream->links, bap_stream_disable_link, NULL); + + return req->id; +} + +unsigned int bt_bap_stream_stop(struct bt_bap_stream *stream, + bt_bap_stream_func_t func, + void *user_data) +{ + struct iovec iov; + struct bt_ascs_stop stop; + struct bt_bap_req *req; + + if (!bap_stream_valid(stream)) + return 0; + + if (!stream->client) { + if (stream->ep->dir == BT_BAP_SINK) + stream_stop(stream, NULL); + return 0; + } + + if (stream->ep->dir == BT_BAP_SINK) + return 0; + + memset(&stop, 0, sizeof(stop)); + + stop.ase = stream->ep->id; + + iov.iov_base = &stop; + iov.iov_len = sizeof(stop); + + req = bap_req_new(stream, BT_ASCS_STOP, &iov, 1, func, user_data); + + if (!bap_queue_req(stream->bap, req)) { + bap_req_free(req); + return 0; + } + + return req->id; +} + +unsigned int bt_bap_stream_metadata(struct bt_bap_stream *stream, + struct iovec *metadata, + bt_bap_stream_func_t func, + void *user_data) +{ + if (!stream) + return 0; + + if (!stream->client) { + stream_metadata(stream, metadata, NULL); + return 0; + } + + return bap_stream_metadata(stream, BT_ASCS_METADATA, metadata, func, + user_data); +} + +unsigned int bt_bap_stream_release(struct bt_bap_stream *stream, + bt_bap_stream_func_t func, + void *user_data) +{ + struct iovec iov; + struct bt_ascs_release rel; + struct bt_bap_req *req; + + if (!stream) + return 0; + + if (!stream->client) { + stream_release(stream, NULL); + return 0; + } + + memset(&req, 0, sizeof(req)); + + rel.ase = stream->ep->id; + + iov.iov_base = &rel; + iov.iov_len = sizeof(rel); + + req = bap_req_new(stream, BT_ASCS_RELEASE, &iov, 1, func, user_data); + + if (!bap_queue_req(stream->bap, req)) { + bap_req_free(req); + return 0; + } + + return req->id; +} + +uint8_t bt_bap_stream_get_dir(struct bt_bap_stream *stream) +{ + if (!stream) + return 0x00; + + return stream->ep->dir; +} + +uint32_t bt_bap_stream_get_location(struct bt_bap_stream *stream) +{ + struct bt_bap_pac *pac; + + if (!stream) + return 0x00000000; + + pac = stream->rpac ? stream->rpac : stream->lpac; + + return pac->locations; +} + +struct iovec *bt_bap_stream_get_config(struct bt_bap_stream *stream) +{ + if (!stream) + return NULL; + + return stream->cc; +} + +struct bt_bap_qos *bt_bap_stream_get_qos(struct bt_bap_stream *stream) +{ + if (!stream) + return NULL; + + return &stream->qos; +} + +struct iovec *bt_bap_stream_get_metadata(struct bt_bap_stream *stream) +{ + if (!stream) + return NULL; + + return stream->meta; +} + +struct io *bt_bap_stream_get_io(struct bt_bap_stream *stream) +{ + struct bt_bap_stream_io *io; + + io = stream_get_io(stream); + if (!io || io->connecting) + return NULL; + + return io->io; +} + +static bool stream_io_disconnected(struct io *io, void *user_data) +{ + struct bt_bap_stream *stream = user_data; + + DBG(stream->bap, "stream %p io disconnected", stream); + + bt_bap_stream_set_io(stream, -1); + + return false; +} + +bool bt_bap_stream_set_io(struct bt_bap_stream *stream, int fd) +{ + if (!stream || (fd >= 0 && stream->io && !stream->io->connecting)) + return false; + + bap_stream_set_io(stream, INT_TO_PTR(fd)); + + queue_foreach(stream->links, bap_stream_set_io, INT_TO_PTR(fd)); + + return true; +} + +static bool match_req_id(const void *data, const void *match_data) +{ + const struct bt_bap_req *req = data; + unsigned int id = PTR_TO_UINT(match_data); + + return (req->id == id); +} + +int bt_bap_stream_cancel(struct bt_bap_stream *stream, unsigned int id) +{ + struct bt_bap_req *req; + + if (!stream) + return -EINVAL; + + if (stream->bap->req && stream->bap->req->id == id) { + req = stream->bap->req; + stream->bap->req = NULL; + bap_req_free(req); + return 0; + } + + req = queue_remove_if(stream->bap->reqs, match_req_id, + UINT_TO_PTR(id)); + if (!req) + return 0; + + bap_req_free(req); + + return 0; +} + +int bt_bap_stream_io_link(struct bt_bap_stream *stream, + struct bt_bap_stream *link) +{ + struct bt_bap *bap = stream->bap; + + if (!stream || !link || stream == link) + return -EINVAL; + + if (queue_find(stream->links, NULL, link)) + return -EALREADY; + + if (stream->client != link->client || + stream->qos.cig_id != link->qos.cig_id || + stream->qos.cis_id != link->qos.cis_id) + return -EINVAL; + + if (!stream->links) + stream->links = queue_new(); + + if (!link->links) + link->links = queue_new(); + + queue_push_tail(stream->links, link); + queue_push_tail(link->links, stream); + + /* Link IOs if already set on stream/link */ + if (stream->io && !link->io) + link->io = stream_io_ref(stream->io); + else if (link->io && !stream->io) + stream->io = stream_io_ref(link->io); + + DBG(bap, "stream %p link %p", stream, link); + + return 0; +} + +struct queue *bt_bap_stream_io_get_links(struct bt_bap_stream *stream) +{ + if (!stream) + return NULL; + + return stream->links; +} + +static void bap_stream_get_in_qos(void *data, void *user_data) +{ + struct bt_bap_stream *stream = data; + struct bt_bap_qos **qos = user_data; + + if (!qos || *qos || stream->ep->dir != BT_BAP_SOURCE || + !stream->qos.sdu) + return; + + *qos = &stream->qos; +} + +static void bap_stream_get_out_qos(void *data, void *user_data) +{ + struct bt_bap_stream *stream = data; + struct bt_bap_qos **qos = user_data; + + if (!qos || *qos || stream->ep->dir != BT_BAP_SINK || !stream->qos.sdu) + return; + + *qos = &stream->qos; +} + +bool bt_bap_stream_io_get_qos(struct bt_bap_stream *stream, + struct bt_bap_qos **in, + struct bt_bap_qos **out) +{ + if (!stream || (!in && !out)) + return false; + + switch (stream->ep->dir) { + case BT_BAP_SOURCE: + bap_stream_get_in_qos(stream, in); + queue_foreach(stream->links, bap_stream_get_out_qos, out); + break; + case BT_BAP_SINK: + bap_stream_get_out_qos(stream, out); + queue_foreach(stream->links, bap_stream_get_in_qos, in); + break; + default: + return false; + } + + DBG(stream->bap, "in %p out %p", in ? *in : NULL, out ? *out : NULL); + + return in && out; +} + +static void bap_stream_get_dir(void *data, void *user_data) +{ + struct bt_bap_stream *stream = data; + uint8_t *dir = user_data; + + *dir |= stream->ep->dir; +} + +uint8_t bt_bap_stream_io_dir(struct bt_bap_stream *stream) +{ + uint8_t dir; + + if (!stream) + return 0x00; + + dir = stream->ep->dir; + + queue_foreach(stream->links, bap_stream_get_dir, &dir); + + return dir; +} + +static void bap_stream_io_connecting(void *data, void *user_data) +{ + struct bt_bap_stream *stream = data; + int fd = PTR_TO_INT(user_data); + const struct queue_entry *entry; + + if (fd >= 0) + bap_stream_io_attach(stream, fd, true); + else + bap_stream_io_detach(stream); + + for (entry = queue_get_entries(stream->bap->state_cbs); entry; + entry = entry->next) { + struct bt_bap_state *state = entry->data; + + if (state->connecting) + state->connecting(stream, stream->io ? true : false, + fd, state->data); + } +} + +int bt_bap_stream_io_connecting(struct bt_bap_stream *stream, int fd) +{ + if (!stream) + return -EINVAL; + + bap_stream_io_connecting(stream, INT_TO_PTR(fd)); + + queue_foreach(stream->links, bap_stream_io_connecting, INT_TO_PTR(fd)); + + return 0; +} + +bool bt_bap_stream_io_is_connecting(struct bt_bap_stream *stream, int *fd) +{ + struct bt_bap_stream_io *io; + + if (!stream) + return false; + + io = stream_get_io(stream); + if (!io) + return false; + + if (fd) + *fd = stream_io_get_fd(io); + + return io->connecting; +} diff -pruN 5.65-1/src/shared/bap.h 5.66-1/src/shared/bap.h --- 5.65-1/src/shared/bap.h 1970-01-01 00:00:00.000000000 +0000 +++ 5.66-1/src/shared/bap.h 2022-11-10 20:22:09.000000000 +0000 @@ -0,0 +1,270 @@ +/* SPDX-License-Identifier: LGPL-2.1-or-later */ +/* + * + * BlueZ - Bluetooth protocol stack for Linux + * + * Copyright (C) 2022 Intel Corporation. All rights reserved. + * + */ + +#include +#include + +#ifndef __packed +#define __packed __attribute__((packed)) +#endif + +#define BT_BAP_SINK 0x01 +#define BT_BAP_SOURCE 0x02 + +#define BT_BAP_STREAM_STATE_IDLE 0x00 +#define BT_BAP_STREAM_STATE_CONFIG 0x01 +#define BT_BAP_STREAM_STATE_QOS 0x02 +#define BT_BAP_STREAM_STATE_ENABLING 0x03 +#define BT_BAP_STREAM_STATE_STREAMING 0x04 +#define BT_BAP_STREAM_STATE_DISABLING 0x05 +#define BT_BAP_STREAM_STATE_RELEASING 0x06 + +#define BT_BAP_CONFIG_LATENCY_LOW 0x01 +#define BT_BAP_CONFIG_LATENCY_BALACED 0x02 +#define BT_BAP_CONFIG_LATENCY_HIGH 0x03 + +#define BT_BAP_CONFIG_PHY_1M 0x01 +#define BT_BAP_CONFIG_PHY_2M 0x02 +#define BT_BAP_CONFIG_PHY_CODEC 0x03 + +struct bt_bap; +struct bt_bap_pac; +struct bt_bap_stream; + +struct bt_bap_codec { + uint8_t id; + uint16_t vid; + uint16_t cid; +} __packed; + +struct bt_ltv { + uint8_t len; + uint8_t type; + uint8_t value[0]; +} __packed; + +struct bt_bap_qos { + uint8_t cig_id; + uint8_t cis_id; + uint32_t interval; /* Frame interval */ + uint8_t framing; /* Frame framing */ + uint8_t phy; /* PHY */ + uint16_t sdu; /* Maximum SDU Size */ + uint8_t rtn; /* Retransmission Effort */ + uint16_t latency; /* Transport Latency */ + uint32_t delay; /* Presentation Delay */ + uint8_t target_latency; /* Target Latency */ +}; + +typedef void (*bt_bap_ready_func_t)(struct bt_bap *bap, void *user_data); +typedef void (*bt_bap_destroy_func_t)(void *user_data); +typedef void (*bt_bap_debug_func_t)(const char *str, void *user_data); +typedef void (*bt_bap_pac_func_t)(struct bt_bap_pac *pac, void *user_data); +typedef bool (*bt_bap_pac_foreach_t)(struct bt_bap_pac *lpac, + struct bt_bap_pac *rpac, + void *user_data); +typedef void (*bt_bap_pac_select_t)(struct bt_bap_pac *pac, int err, + struct iovec *caps, + struct iovec *metadata, + struct bt_bap_qos *qos, + void *user_data); +typedef void (*bt_bap_pac_config_t)(struct bt_bap_stream *stream, int err); +typedef void (*bt_bap_state_func_t)(struct bt_bap_stream *stream, + uint8_t old_state, uint8_t new_state, + void *user_data); +typedef void (*bt_bap_connecting_func_t)(struct bt_bap_stream *stream, + bool state, int fd, + void *user_data); +typedef void (*bt_bap_stream_func_t)(struct bt_bap_stream *stream, + uint8_t code, uint8_t reason, + void *user_data); +typedef void (*bt_bap_func_t)(struct bt_bap *bap, void *user_data); + +/* Local PAC related functions */ + +unsigned int bt_bap_pac_register(bt_bap_pac_func_t added, + bt_bap_pac_func_t removed, void *user_data, + bt_bap_destroy_func_t destroy); +bool bt_bap_pac_unregister(unsigned int id); + +struct bt_bap_pac_qos { + uint8_t framing; + uint8_t phy; + uint8_t rtn; + uint16_t latency; + uint32_t pd_min; + uint32_t pd_max; + uint32_t ppd_min; + uint32_t ppd_max; +}; + +struct bt_bap_pac *bt_bap_add_vendor_pac(struct gatt_db *db, + const char *name, uint8_t type, + uint8_t id, uint16_t cid, uint16_t vid, + struct bt_bap_pac_qos *qos, + struct iovec *data, + struct iovec *metadata); + +struct bt_bap_pac *bt_bap_add_pac(struct gatt_db *db, const char *name, + uint8_t type, uint8_t id, + struct bt_bap_pac_qos *qos, + struct iovec *data, + struct iovec *metadata); + +struct bt_bap_pac_ops { + int (*select)(struct bt_bap_pac *lpac, struct bt_bap_pac *rpac, + struct bt_bap_pac_qos *qos, + bt_bap_pac_select_t cb, void *cb_data, void *user_data); + int (*config)(struct bt_bap_stream *stream, struct iovec *cfg, + struct bt_bap_qos *qos, bt_bap_pac_config_t cb, + void *user_data); + void (*clear)(struct bt_bap_stream *stream, void *user_data); +}; + +bool bt_bap_pac_set_ops(struct bt_bap_pac *pac, struct bt_bap_pac_ops *ops, + void *user_data); + +bool bt_bap_remove_pac(struct bt_bap_pac *pac); + +uint8_t bt_bap_pac_get_type(struct bt_bap_pac *pac); + +struct bt_bap_stream *bt_bap_pac_get_stream(struct bt_bap_pac *pac); + +/* Session related function */ +unsigned int bt_bap_register(bt_bap_func_t added, bt_bap_func_t removed, + void *user_data); +bool bt_bap_unregister(unsigned int id); + +struct bt_bap *bt_bap_new(struct gatt_db *ldb, struct gatt_db *rdb); + +bool bt_bap_set_user_data(struct bt_bap *bap, void *user_data); + +void *bt_bap_get_user_data(struct bt_bap *bap); + +struct bt_att *bt_bap_get_att(struct bt_bap *bap); + +struct bt_bap *bt_bap_ref(struct bt_bap *bap); +void bt_bap_unref(struct bt_bap *bap); + +bool bt_bap_attach(struct bt_bap *bap, struct bt_gatt_client *client); +void bt_bap_detach(struct bt_bap *bap); + +bool bt_bap_set_debug(struct bt_bap *bap, bt_bap_debug_func_t cb, + void *user_data, bt_bap_destroy_func_t destroy); + +bool bap_print_cc(void *data, size_t len, util_debug_func_t func, + void *user_data); + +unsigned int bt_bap_ready_register(struct bt_bap *bap, + bt_bap_ready_func_t func, void *user_data, + bt_bap_destroy_func_t destroy); +bool bt_bap_ready_unregister(struct bt_bap *bap, unsigned int id); + +unsigned int bt_bap_state_register(struct bt_bap *bap, + bt_bap_state_func_t func, + bt_bap_connecting_func_t connecting, + void *user_data, bt_bap_destroy_func_t destroy); +bool bt_bap_state_unregister(struct bt_bap *bap, unsigned int id); + +const char *bt_bap_stream_statestr(uint8_t state); + +void bt_bap_foreach_pac(struct bt_bap *bap, uint8_t type, + bt_bap_pac_foreach_t func, void *user_data); + +int bt_bap_pac_get_vendor_codec(struct bt_bap_pac *pac, uint8_t *id, + uint16_t *cid, uint16_t *vid, + struct iovec **data, struct iovec **metadata); + +int bt_bap_pac_get_codec(struct bt_bap_pac *pac, uint8_t *id, + struct iovec **data, struct iovec **metadata); + +void bt_bap_pac_set_user_data(struct bt_bap_pac *pac, void *user_data); +void *bt_bap_pac_get_user_data(struct bt_bap_pac *pac); + +/* Stream related functions */ +int bt_bap_select(struct bt_bap_pac *lpac, struct bt_bap_pac *rpac, + bt_bap_pac_select_t func, void *user_data); + +struct bt_bap_stream *bt_bap_config(struct bt_bap *bap, + struct bt_bap_pac *lpac, + struct bt_bap_pac *rpac, + struct bt_bap_qos *pqos, + struct iovec *data, + bt_bap_stream_func_t func, + void *user_data); + +struct bt_bap *bt_bap_stream_get_session(struct bt_bap_stream *stream); +uint8_t bt_bap_stream_get_state(struct bt_bap_stream *stream); + +bool bt_bap_stream_set_user_data(struct bt_bap_stream *stream, void *user_data); + +void *bt_bap_stream_get_user_data(struct bt_bap_stream *stream); + +unsigned int bt_bap_stream_config(struct bt_bap_stream *stream, + struct bt_bap_qos *pqos, + struct iovec *data, + bt_bap_stream_func_t func, + void *user_data); + +unsigned int bt_bap_stream_qos(struct bt_bap_stream *stream, + struct bt_bap_qos *qos, + bt_bap_stream_func_t func, + void *user_data); + +unsigned int bt_bap_stream_enable(struct bt_bap_stream *stream, + bool enable_links, + struct iovec *metadata, + bt_bap_stream_func_t func, + void *user_data); + +unsigned int bt_bap_stream_start(struct bt_bap_stream *stream, + bt_bap_stream_func_t func, + void *user_data); + +unsigned int bt_bap_stream_disable(struct bt_bap_stream *stream, + bool disable_links, + bt_bap_stream_func_t func, + void *user_data); + +unsigned int bt_bap_stream_stop(struct bt_bap_stream *stream, + bt_bap_stream_func_t func, + void *user_data); + +unsigned int bt_bap_stream_metadata(struct bt_bap_stream *stream, + struct iovec *metadata, + bt_bap_stream_func_t func, + void *user_data); + +unsigned int bt_bap_stream_release(struct bt_bap_stream *stream, + bt_bap_stream_func_t func, + void *user_data); + +uint8_t bt_bap_stream_get_dir(struct bt_bap_stream *stream); +uint32_t bt_bap_stream_get_location(struct bt_bap_stream *stream); +struct iovec *bt_bap_stream_get_config(struct bt_bap_stream *stream); +struct bt_bap_qos *bt_bap_stream_get_qos(struct bt_bap_stream *stream); +struct iovec *bt_bap_stream_get_metadata(struct bt_bap_stream *stream); + +struct io *bt_bap_stream_get_io(struct bt_bap_stream *stream); + +bool bt_bap_stream_set_io(struct bt_bap_stream *stream, int fd); + +int bt_bap_stream_cancel(struct bt_bap_stream *stream, unsigned int id); + +int bt_bap_stream_io_link(struct bt_bap_stream *stream, + struct bt_bap_stream *link); +struct queue *bt_bap_stream_io_get_links(struct bt_bap_stream *stream); +bool bt_bap_stream_io_get_qos(struct bt_bap_stream *stream, + struct bt_bap_qos **in, + struct bt_bap_qos **out); + +uint8_t bt_bap_stream_io_dir(struct bt_bap_stream *stream); + +int bt_bap_stream_io_connecting(struct bt_bap_stream *stream, int fd); +bool bt_bap_stream_io_is_connecting(struct bt_bap_stream *stream, int *fd); diff -pruN 5.65-1/src/shared/gatt-client.c 5.66-1/src/shared/gatt-client.c --- 5.65-1/src/shared/gatt-client.c 2022-07-24 20:59:52.000000000 +0000 +++ 5.66-1/src/shared/gatt-client.c 2022-11-10 20:22:09.000000000 +0000 @@ -211,22 +211,6 @@ static void notify_data_unref(void *data free(notify_data); } -static void find_ccc(struct gatt_db_attribute *attr, void *user_data) -{ - struct gatt_db_attribute **ccc_ptr = user_data; - bt_uuid_t uuid; - - if (*ccc_ptr) - return; - - bt_uuid16_create(&uuid, GATT_CLIENT_CHARAC_CFG_UUID); - - if (bt_uuid_cmp(&uuid, gatt_db_attribute_get_type(attr))) - return; - - *ccc_ptr = attr; -} - static bool match_notify_chrc(const void *data, const void *user_data) { const struct notify_data *notify_data = data; @@ -273,24 +257,25 @@ static void chrc_removed(struct gatt_db_ } static struct notify_chrc *notify_chrc_create(struct bt_gatt_client *client, - uint16_t value_handle) + uint16_t handle) { struct gatt_db_attribute *attr, *ccc; struct notify_chrc *chrc; - bt_uuid_t uuid; + uint16_t value_handle; uint8_t properties; - /* Check that chrc_value_handle belongs to a known characteristic */ - attr = gatt_db_get_attribute(client->db, value_handle - 1); + /* Check that there is an attribute with handle */ + attr = gatt_db_get_attribute(client->db, handle); if (!attr) return NULL; - bt_uuid16_create(&uuid, GATT_CHARAC_UUID); - if (bt_uuid_cmp(&uuid, gatt_db_attribute_get_type(attr))) + if (!gatt_db_attribute_get_char_data(attr, NULL, &value_handle, + &properties, NULL, NULL)) return NULL; - if (!gatt_db_attribute_get_char_data(attr, NULL, NULL, &properties, - NULL, NULL)) + /* Check that there is an attribute with value_handle */ + attr = gatt_db_get_attribute(client->db, value_handle); + if (!attr) return NULL; chrc = new0(struct notify_chrc, 1); @@ -301,13 +286,7 @@ static struct notify_chrc *notify_chrc_c return NULL; } - /* - * Find the CCC characteristic. Some characteristics that allow - * notifications may not have a CCC descriptor. We treat these as - * automatically successful. - */ - ccc = NULL; - gatt_db_service_foreach_desc(attr, find_ccc, &ccc); + ccc = gatt_db_attribute_get_ccc(attr); if (ccc) chrc->ccc_handle = gatt_db_attribute_get_handle(ccc); diff -pruN 5.65-1/src/shared/gatt-db.c 5.66-1/src/shared/gatt-db.c --- 5.65-1/src/shared/gatt-db.c 2022-07-24 20:59:52.000000000 +0000 +++ 5.66-1/src/shared/gatt-db.c 2022-11-10 20:22:09.000000000 +0000 @@ -709,21 +709,24 @@ struct gatt_db_attribute *gatt_db_insert if (service) { const bt_uuid_t *type; bt_uuid_t value; + struct gatt_db_attribute *attr = service->attributes[0]; + + if (!attr) + return NULL; if (primary) type = &primary_service_uuid; else type = &secondary_service_uuid; - gatt_db_attribute_get_service_uuid(service->attributes[0], - &value); + gatt_db_attribute_get_service_uuid(attr, &value); /* Check if service match */ - if (!bt_uuid_cmp(&service->attributes[0]->uuid, type) && + if (!bt_uuid_cmp(&attr->uuid, type) && !bt_uuid_cmp(&value, uuid) && service->num_handles == num_handles && - service->attributes[0]->handle == handle) - return service->attributes[0]; + attr->handle == handle) + return attr; return NULL; } @@ -1066,7 +1069,7 @@ gatt_db_service_add_ccc(struct gatt_db_a struct gatt_db_attribute *value; uint16_t handle = 0; - if (!attrib) + if (!attrib || !permissions) return NULL; db = attrib->service->db; @@ -1328,6 +1331,7 @@ unsigned int gatt_db_find_by_type_value( { struct find_by_type_value_data data; + memset(&data, 0, sizeof(data)); data.func = func; data.user_data = user_data; data.value = value; diff -pruN 5.65-1/src/shared/lc3.h 5.66-1/src/shared/lc3.h --- 5.65-1/src/shared/lc3.h 1970-01-01 00:00:00.000000000 +0000 +++ 5.66-1/src/shared/lc3.h 2022-11-10 20:22:09.000000000 +0000 @@ -0,0 +1,112 @@ +/* SPDX-License-Identifier: LGPL-2.1-or-later */ +/* + * + * BlueZ - Bluetooth protocol stack for Linux + * + * Copyright (C) 2022 Intel Corporation. All rights reserved. + * + */ + +#define LTV(_type, _bytes...) \ + { \ + .len = 1 + sizeof((uint8_t []) { _bytes }), \ + .type = _type, \ + .data = { _bytes }, \ + } + +#define LC3_ID 0x06 + +#define LC3_BASE 0x01 + +#define LC3_FREQ (LC3_BASE) +#define LC3_FREQ_8KHZ BIT(0) +#define LC3_FREQ_11KHZ BIT(1) +#define LC3_FREQ_16KHZ BIT(2) +#define LC3_FREQ_22KHZ BIT(3) +#define LC3_FREQ_24KHZ BIT(4) +#define LC3_FREQ_32KHZ BIT(5) +#define LC3_FREQ_44KHZ BIT(6) +#define LC3_FREQ_48KHZ BIT(7) +#define LC3_FREQ_ANY (LC3_FREQ_8KHZ | \ + LC3_FREQ_11KHZ | \ + LC3_FREQ_16KHZ | \ + LC3_FREQ_22KHZ | \ + LC3_FREQ_24KHZ | \ + LC3_FREQ_32KHZ | \ + LC3_FREQ_44KHZ | \ + LC3_FREQ_48KHZ) + +#define LC3_DURATION (LC3_BASE + 1) +#define LC3_DURATION_7_5 BIT(0) +#define LC3_DURATION_10 BIT(1) +#define LC3_DURATION_ANY (LC3_DURATION_7_5 | LC3_DURATION_10) +#define LC3_DURATION_PREFER_7_5 BIT(4) +#define LC3_DURATION_PREFER_10 BIT(5) + + +#define LC3_CHAN_COUNT (LC3_BASE + 2) +#define LC3_CHAN_COUNT_SUPPORT BIT(0) + +#define LC3_FRAME_LEN (LC3_BASE + 3) + +#define LC3_FRAME_COUNT (LC3_BASE + 4) + +#define LC3_CAPABILITIES(_freq, _duration, _chan_count, _len_min, _len_max) \ + { \ + LTV(LC3_FREQ, _freq), \ + LTV(LC3_DURATION, _duration), \ + LTV(LC3_CHAN_COUNT, _chan_count), \ + LTV(LC3_FRAME_LEN, _len_min, _len_min >> 8, \ + _len_max, _len_max >> 8), \ + } + +#define LC3_CONFIG_BASE 0x01 + +#define LC3_CONFIG_FREQ (LC3_CONFIG_BASE) +#define LC3_CONFIG_FREQ_8KHZ 0x01 +#define LC3_CONFIG_FREQ_11KHZ 0x02 +#define LC3_CONFIG_FREQ_16KHZ 0x03 +#define LC3_CONFIG_FREQ_22KHZ 0x04 +#define LC3_CONFIG_FREQ_24KHZ 0x05 +#define LC3_CONFIG_FREQ_32KHZ 0x06 +#define LC3_CONFIG_FREQ_44KHZ 0x07 +#define LC3_CONFIG_FREQ_48KHZ 0x08 + +#define LC3_CONFIG_DURATION (LC3_CONFIG_BASE + 1) +#define LC3_CONFIG_DURATION_7_5 0x00 +#define LC3_CONFIG_DURATION_10 0x01 + +#define LC3_CONFIG_CHAN_ALLOC (LC3_CONFIG_BASE + 2) + +#define LC3_CONFIG_FRAME_LEN (LC3_CONFIG_BASE + 3) + +#define LC3_CONFIG(_freq, _duration, _len) \ + { \ + LTV(LC3_CONFIG_FREQ, _freq), \ + LTV(LC3_CONFIG_DURATION, _duration), \ + LTV(LC3_CONFIG_FRAME_LEN, _len, _len >> 8), \ + } + +#define LC3_CONFIG_8KHZ(_duration, _len) \ + LC3_CONFIG(LC3_CONFIG_FREQ_8KHZ, _duration, _len) + +#define LC3_CONFIG_11KHZ(_duration, _len) \ + LC3_CONFIG(LC3_CONFIG_FREQ_11KHZ, _duration, _len) + +#define LC3_CONFIG_16KHZ(_duration, _len) \ + LC3_CONFIG(LC3_CONFIG_FREQ_16KHZ, _duration, _len) + +#define LC3_CONFIG_22KHZ(_duration, _len) \ + LC3_CONFIG(LC3_CONFIG_FREQ_22KHZ, _duration, _len) + +#define LC3_CONFIG_24KHZ(_duration, _len) \ + LC3_CONFIG(LC3_CONFIG_FREQ_24KHZ, _duration, _len) + +#define LC3_CONFIG_32KHZ(_duration, _len) \ + LC3_CONFIG(LC3_CONFIG_FREQ_32KHZ, _duration, _len) + +#define LC3_CONFIG_44KHZ(_duration, _len) \ + LC3_CONFIG(LC3_CONFIG_FREQ_44KHZ, _duration, _len) + +#define LC3_CONFIG_48KHZ(_duration, _len) \ + LC3_CONFIG(LC3_CONFIG_FREQ_48KHZ, _duration, _len) diff -pruN 5.65-1/src/shared/mcp.c 5.66-1/src/shared/mcp.c --- 5.65-1/src/shared/mcp.c 1970-01-01 00:00:00.000000000 +0000 +++ 5.66-1/src/shared/mcp.c 2022-11-10 20:22:09.000000000 +0000 @@ -0,0 +1,1419 @@ +// SPDX-License-Identifier: LGPL-2.1-or-later +/* + * + * BlueZ - Bluetooth protocol stack for Linux + * + * Copyright (C) 2022 Intel Corporation. All rights reserved. + * + */ + +#define _GNU_SOURCE +#include +#include +#include +#include +#include +#include + +#include "lib/bluetooth.h" +#include "lib/uuid.h" +#include "lib/hci.h" + +#include "src/shared/queue.h" +#include "src/shared/util.h" +#include "src/shared/timeout.h" +#include "src/shared/att.h" +#include "src/shared/gatt-db.h" +#include "src/shared/gatt-server.h" +#include "src/shared/gatt-client.h" +#include "src/shared/mcp.h" +#include "src/shared/mcs.h" + +#define DBG(_mcp, fmt, arg...) \ + mcp_debug(_mcp, "%s:%s() " fmt, __FILE__, __func__, ## arg) + +struct bt_mcp_db { + struct gatt_db *db; + struct bt_mcs *mcs; +}; + +struct bt_mcp_pending { + unsigned int id; + struct bt_mcp *mcp; + bt_gatt_client_read_callback_t func; + void *user_data; +}; + +struct event_callback { + const struct bt_mcp_event_callback *cbs; + void *user_data; +}; + +struct bt_mcp_session_info { + uint8_t content_control_id; + uint32_t cp_op_supported; +}; + +struct bt_mcp { + int ref_count; + struct bt_gatt_client *client; + struct bt_mcp_db *ldb; + struct bt_mcp_db *rdb; + unsigned int mp_name_id; + unsigned int track_changed_id; + unsigned int track_title_id; + unsigned int track_duration_id; + unsigned int track_position_id; + unsigned int media_state_id; + unsigned int media_cp_id; + unsigned int media_cp_op_supported_id; + + struct bt_mcp_session_info session; + struct event_callback *cb; + + struct queue *pending; + + bt_mcp_debug_func_t debug_func; + bt_mcp_destroy_func_t debug_destroy; + void *debug_data; + void *user_data; +}; + +struct bt_mcs { + struct bt_mcp_db *mdb; + struct gatt_db_attribute *service; + struct gatt_db_attribute *mp_name; + struct gatt_db_attribute *track_changed; + struct gatt_db_attribute *track_changed_ccc; + struct gatt_db_attribute *track_title; + struct gatt_db_attribute *track_duration; + struct gatt_db_attribute *track_position; + struct gatt_db_attribute *playback_speed; + struct gatt_db_attribute *seeking_speed; + struct gatt_db_attribute *play_order; + struct gatt_db_attribute *play_order_supported; + struct gatt_db_attribute *media_state; + struct gatt_db_attribute *media_state_ccc; + struct gatt_db_attribute *media_cp; + struct gatt_db_attribute *media_cp_ccc; + struct gatt_db_attribute *media_cp_op_supportd; + struct gatt_db_attribute *content_control_id; + struct gatt_db_attribute *content_control_id_ccc; +}; + +static struct queue *mcp_db; + +static void mcp_debug(struct bt_mcp *mcp, const char *format, ...) +{ + va_list ap; + + if (!mcp || !format || !mcp->debug_func) + return; + + va_start(ap, format); + util_debug_va(mcp->debug_func, mcp->debug_data, format, ap); + va_end(ap); +} + +static bool mcp_db_match(const void *data, const void *match_data) +{ + const struct bt_mcp_db *mdb = data; + const struct gatt_db *db = match_data; + + return (mdb->db == db); +} + +static void mcp_db_free(void *data) +{ + struct bt_mcp_db *bdb = data; + + if (!bdb) + return; + + gatt_db_unref(bdb->db); + + free(bdb->mcs); + free(bdb); +} + +static void mcp_free(void *data) +{ + struct bt_mcp *mcp = data; + + DBG(mcp, ""); + + bt_mcp_detach(mcp); + + mcp_db_free(mcp->rdb); + + queue_destroy(mcp->pending, NULL); + + free(mcp); +} + +struct bt_mcp *bt_mcp_ref(struct bt_mcp *mcp) +{ + if (!mcp) + return NULL; + + __sync_fetch_and_add(&mcp->ref_count, 1); + + return mcp; +} + +void bt_mcp_unref(struct bt_mcp *mcp) +{ + if (!mcp) + return; + + if (__sync_sub_and_fetch(&mcp->ref_count, 1)) + return; + + mcp_free(mcp); +} + +bool bt_mcp_set_user_data(struct bt_mcp *mcp, void *user_data) +{ + if (!mcp) + return false; + + mcp->user_data = user_data; + + return true; +} + +void *bt_mcp_get_user_data(struct bt_mcp *mcp) +{ + if (!mcp) + return NULL; + + return mcp->user_data; +} + +bool bt_mcp_set_debug(struct bt_mcp *mcp, bt_mcp_debug_func_t func, + void *user_data, bt_mcp_destroy_func_t destroy) +{ + if (!mcp) + return false; + + if (mcp->debug_destroy) + mcp->debug_destroy(mcp->debug_data); + + mcp->debug_func = func; + mcp->debug_destroy = destroy; + mcp->debug_data = user_data; + + return true; +} + +static void mcs_mp_name_read(struct gatt_db_attribute *attrib, + unsigned int id, uint16_t offset, + uint8_t opcode, struct bt_att *att, + void *user_data) +{ + char mp_name[] = ""; + struct iovec iov; + + iov.iov_base = mp_name; + iov.iov_len = sizeof(mp_name); + + gatt_db_attribute_read_result(attrib, id, 0, iov.iov_base, + iov.iov_len); +} + +static void mcs_track_title_read(struct gatt_db_attribute *attrib, + unsigned int id, uint16_t offset, + uint8_t opcode, struct bt_att *att, + void *user_data) +{ + char track_title[] = ""; + struct iovec iov; + + iov.iov_base = track_title; + iov.iov_len = 0; + + gatt_db_attribute_read_result(attrib, id, 0, iov.iov_base, + iov.iov_len); +} + +static void mcs_track_duration_read(struct gatt_db_attribute *attrib, + unsigned int id, uint16_t offset, + uint8_t opcode, struct bt_att *att, + void *user_data) +{ + int32_t track_duration = 0xFFFFFFFF; + struct iovec iov; + + iov.iov_base = &track_duration; + iov.iov_len = sizeof(track_duration); + + gatt_db_attribute_read_result(attrib, id, 0, iov.iov_base, + iov.iov_len); +} + +static void mcs_track_position_read(struct gatt_db_attribute *attrib, + unsigned int id, uint16_t offset, + uint8_t opcode, struct bt_att *att, + void *user_data) +{ + int32_t track_position = 0xFFFFFFFF; + struct iovec iov; + + iov.iov_base = &track_position; + iov.iov_len = sizeof(track_position); + + gatt_db_attribute_read_result(attrib, id, 0, iov.iov_base, + iov.iov_len); +} + +static void mcs_track_position_write(struct gatt_db_attribute *attrib, + unsigned int id, uint16_t offset, + const uint8_t *value, size_t len, + uint8_t opcode, struct bt_att *att, + void *user_data) +{ + gatt_db_attribute_write_result(attrib, id, + BT_ATT_ERROR_INSUFFICIENT_RESOURCES); +} + +static void mcs_playback_speed_read(struct gatt_db_attribute *attrib, + unsigned int id, uint16_t offset, + uint8_t opcode, struct bt_att *att, + void *user_data) +{ + int8_t playback_speed = 0x00; + struct iovec iov; + + iov.iov_base = &playback_speed; + iov.iov_len = sizeof(playback_speed); + + gatt_db_attribute_read_result(attrib, id, 0, iov.iov_base, + iov.iov_len); +} + +static void mcs_playback_speed_write(struct gatt_db_attribute *attrib, + unsigned int id, uint16_t offset, + const uint8_t *value, size_t len, + uint8_t opcode, struct bt_att *att, + void *user_data) +{ + gatt_db_attribute_write_result(attrib, id, + BT_ATT_ERROR_INSUFFICIENT_RESOURCES); +} + +static void mcs_seeking_speed_read(struct gatt_db_attribute *attrib, + unsigned int id, uint16_t offset, + uint8_t opcode, struct bt_att *att, + void *user_data) +{ + int8_t seeking_speed = 0x00; + struct iovec iov; + + iov.iov_base = &seeking_speed; + iov.iov_len = sizeof(seeking_speed); + + gatt_db_attribute_read_result(attrib, id, 0, iov.iov_base, + iov.iov_len); +} + +static void mcs_playing_order_read(struct gatt_db_attribute *attrib, + unsigned int id, uint16_t offset, + uint8_t opcode, struct bt_att *att, + void *user_data) +{ + uint8_t playing_order = 0x01; + struct iovec iov; + + iov.iov_base = &playing_order; + iov.iov_len = sizeof(playing_order); + + gatt_db_attribute_read_result(attrib, id, 0, iov.iov_base, + iov.iov_len); +} + +static void mcs_playing_order_write(struct gatt_db_attribute *attrib, + unsigned int id, uint16_t offset, + const uint8_t *value, size_t len, + uint8_t opcode, struct bt_att *att, + void *user_data) +{ + gatt_db_attribute_write_result(attrib, id, + BT_ATT_ERROR_INSUFFICIENT_RESOURCES); +} + +static void mcs_playing_order_supported_read(struct gatt_db_attribute *attrib, + unsigned int id, uint16_t offset, + uint8_t opcode, struct bt_att *att, + void *user_data) +{ + uint16_t playing_order_supported = 0x01; + struct iovec iov; + + iov.iov_base = &playing_order_supported; + iov.iov_len = sizeof(playing_order_supported); + + gatt_db_attribute_read_result(attrib, id, 0, iov.iov_base, + iov.iov_len); +} + +static void mcs_media_state_read(struct gatt_db_attribute *attrib, + unsigned int id, uint16_t offset, + uint8_t opcode, struct bt_att *att, + void *user_data) +{ + uint8_t media_state = 0x00; + struct iovec iov; + + iov.iov_base = &media_state; + iov.iov_len = sizeof(media_state); + + gatt_db_attribute_read_result(attrib, id, 0, iov.iov_base, + iov.iov_len); +} + +static void mcs_media_cp_write(struct gatt_db_attribute *attrib, + unsigned int id, uint16_t offset, + const uint8_t *value, size_t len, + uint8_t opcode, struct bt_att *att, + void *user_data) +{ + gatt_db_attribute_write_result(attrib, id, + BT_ATT_ERROR_INSUFFICIENT_RESOURCES); +} + +static void mcs_media_cp_op_supported_read(struct gatt_db_attribute *attrib, + unsigned int id, uint16_t offset, + uint8_t opcode, struct bt_att *att, + void *user_data) +{ + uint32_t cp_op_supported = 0x00000000; + struct iovec iov; + + iov.iov_base = &cp_op_supported; + iov.iov_len = sizeof(cp_op_supported); + + gatt_db_attribute_read_result(attrib, id, 0, iov.iov_base, + iov.iov_len); +} + +static void mcs_media_content_control_id_read(struct gatt_db_attribute *attrib, + unsigned int id, uint16_t offset, + uint8_t opcode, struct bt_att *att, + void *user_data) +{ + uint8_t content_control_id = 0x00; + struct iovec iov; + + iov.iov_base = &content_control_id; + iov.iov_len = sizeof(content_control_id); + + gatt_db_attribute_read_result(attrib, id, 0, iov.iov_base, + iov.iov_len); +} + +static struct bt_mcs *mcs_new(struct gatt_db *db) +{ + struct bt_mcs *mcs; + bt_uuid_t uuid; + + if (!db) + return NULL; + + mcs = new0(struct bt_mcs, 1); + + /* Populate DB with MCS attributes */ + bt_uuid16_create(&uuid, GMCS_UUID); + mcs->service = gatt_db_add_service(db, &uuid, true, 31); + + bt_uuid16_create(&uuid, MEDIA_PLAYER_NAME_CHRC_UUID); + mcs->mp_name = gatt_db_service_add_characteristic(mcs->service, &uuid, + BT_ATT_PERM_READ, + BT_GATT_CHRC_PROP_READ, + mcs_mp_name_read, NULL, + mcs); + + bt_uuid16_create(&uuid, MEDIA_TRACK_CHNGD_CHRC_UUID); + mcs->track_changed = gatt_db_service_add_characteristic(mcs->service, + &uuid, + BT_ATT_PERM_NONE, + BT_GATT_CHRC_PROP_NOTIFY, + NULL, NULL, + mcs); + + mcs->track_changed_ccc = gatt_db_service_add_ccc(mcs->service, + BT_ATT_PERM_READ | BT_ATT_PERM_WRITE); + + bt_uuid16_create(&uuid, MEDIA_TRACK_TITLE_CHRC_UUID); + mcs->track_title = gatt_db_service_add_characteristic(mcs->service, + &uuid, + BT_ATT_PERM_READ, + BT_GATT_CHRC_PROP_READ, + mcs_track_title_read, NULL, + mcs); + + bt_uuid16_create(&uuid, MEDIA_TRACK_DURATION_CHRC_UUID); + mcs->track_duration = gatt_db_service_add_characteristic(mcs->service, + &uuid, + BT_ATT_PERM_READ, + BT_GATT_CHRC_PROP_READ, + mcs_track_duration_read, NULL, + mcs); + + bt_uuid16_create(&uuid, MEDIA_TRACK_POSTION_CHRC_UUID); + mcs->track_position = gatt_db_service_add_characteristic(mcs->service, + &uuid, + BT_ATT_PERM_READ | BT_ATT_PERM_WRITE, + BT_GATT_CHRC_PROP_READ | + BT_GATT_CHRC_PROP_WRITE | + BT_GATT_CHRC_PROP_WRITE_WITHOUT_RESP, + mcs_track_position_read, + mcs_track_position_write, + mcs); + + bt_uuid16_create(&uuid, MEDIA_PLAYBACK_SPEED_CHRC_UUID); + mcs->playback_speed = gatt_db_service_add_characteristic(mcs->service, + &uuid, + BT_ATT_PERM_READ | BT_ATT_PERM_WRITE, + BT_GATT_CHRC_PROP_READ | + BT_GATT_CHRC_PROP_WRITE | + BT_GATT_CHRC_PROP_WRITE_WITHOUT_RESP, + mcs_playback_speed_read, + mcs_playback_speed_write, + mcs); + + bt_uuid16_create(&uuid, MEDIA_SEEKING_SPEED_CHRC_UUID); + mcs->seeking_speed = gatt_db_service_add_characteristic(mcs->service, + &uuid, + BT_ATT_PERM_READ, + BT_GATT_CHRC_PROP_READ, + mcs_seeking_speed_read, NULL, + mcs); + + bt_uuid16_create(&uuid, MEDIA_PLAYING_ORDER_CHRC_UUID); + mcs->play_order = gatt_db_service_add_characteristic(mcs->service, + &uuid, + BT_ATT_PERM_READ | BT_ATT_PERM_WRITE, + BT_GATT_CHRC_PROP_READ | + BT_GATT_CHRC_PROP_WRITE | + BT_GATT_CHRC_PROP_WRITE_WITHOUT_RESP, + mcs_playing_order_read, + mcs_playing_order_write, + mcs); + + bt_uuid16_create(&uuid, MEDIA_PLAY_ORDER_SUPPRTD_CHRC_UUID); + mcs->play_order_supported = gatt_db_service_add_characteristic( + mcs->service, + &uuid, + BT_ATT_PERM_READ, + BT_GATT_CHRC_PROP_READ, + mcs_playing_order_supported_read, NULL, + mcs); + + bt_uuid16_create(&uuid, MEDIA_STATE_CHRC_UUID); + mcs->media_state = gatt_db_service_add_characteristic(mcs->service, + &uuid, + BT_ATT_PERM_READ, + BT_GATT_CHRC_PROP_READ | + BT_GATT_CHRC_PROP_NOTIFY, + mcs_media_state_read, NULL, + mcs); + + mcs->media_state_ccc = gatt_db_service_add_ccc(mcs->service, + BT_ATT_PERM_READ | BT_ATT_PERM_WRITE); + + bt_uuid16_create(&uuid, MEDIA_CP_CHRC_UUID); + mcs->media_cp = gatt_db_service_add_characteristic(mcs->service, &uuid, + BT_ATT_PERM_WRITE, + BT_GATT_CHRC_PROP_WRITE | + BT_GATT_CHRC_PROP_NOTIFY | + BT_GATT_CHRC_PROP_WRITE_WITHOUT_RESP, + NULL, mcs_media_cp_write, + mcs); + + mcs->media_cp_ccc = gatt_db_service_add_ccc(mcs->service, + BT_ATT_PERM_READ | BT_ATT_PERM_WRITE); + + bt_uuid16_create(&uuid, MEDIA_CP_OP_SUPPORTED_CHRC_UUID); + mcs->media_cp_op_supportd = gatt_db_service_add_characteristic( + mcs->service, + &uuid, + BT_ATT_PERM_READ, + BT_GATT_CHRC_PROP_READ, + mcs_media_cp_op_supported_read, NULL, + mcs); + + bt_uuid16_create(&uuid, MEDIA_CONTENT_CONTROL_ID_CHRC_UUID); + mcs->content_control_id = gatt_db_service_add_characteristic( + mcs->service, + &uuid, + BT_ATT_PERM_READ, + BT_GATT_CHRC_PROP_READ | + BT_GATT_CHRC_PROP_NOTIFY, + mcs_media_content_control_id_read, + NULL, + mcs); + + mcs->content_control_id_ccc = gatt_db_service_add_ccc(mcs->service, + BT_ATT_PERM_READ | BT_ATT_PERM_WRITE); + + gatt_db_service_set_active(mcs->service, false); + + return mcs; +} + +static struct bt_mcs *mcp_get_mcs(struct bt_mcp *mcp) +{ + if (!mcp) + return NULL; + + if (mcp->rdb->mcs) + return mcp->rdb->mcs; + + mcp->rdb->mcs = new0(struct bt_mcs, 1); + mcp->rdb->mcs->mdb = mcp->rdb; + + return mcp->rdb->mcs; +} + +static unsigned int mcp_send(struct bt_mcp *mcp, uint8_t operation) +{ + struct bt_mcs *mcs = mcp_get_mcs(mcp); + int ret; + uint16_t handle; + + DBG(mcp, "mcs %p", mcs); + + if (!mcp->client) + return -1; + + if (!gatt_db_attribute_get_char_data(mcs->media_cp, NULL, &handle, + NULL, NULL, NULL)) + return -1; + + ret = bt_gatt_client_write_without_response(mcp->client, handle, false, + &operation, sizeof(uint8_t)); + if (!ret) + return -1; + + return 0; +} + +unsigned int bt_mcp_play(struct bt_mcp *mcp) +{ + if (!(mcp->session.cp_op_supported & BT_MCS_CMD_PLAY_SUPPORTED)) + return -ENOTSUP; + + DBG(mcp, "mcp %p", mcp); + + return mcp_send(mcp, BT_MCS_CMD_PLAY); +} + +unsigned int bt_mcp_pause(struct bt_mcp *mcp) +{ + if (!(mcp->session.cp_op_supported & BT_MCS_CMD_PAUSE_SUPPORTED)) + return -ENOTSUP; + + DBG(mcp, "mcp %p", mcp); + + return mcp_send(mcp, BT_MCS_CMD_PAUSE); +} + +unsigned int bt_mcp_stop(struct bt_mcp *mcp) +{ + if (!(mcp->session.cp_op_supported & BT_MCS_CMD_STOP_SUPPORTED)) + return -ENOTSUP; + + DBG(mcp, "mcp %p", mcp); + + return mcp_send(mcp, BT_MCS_CMD_STOP); +} + +static void mcp_mp_set_player_name(struct bt_mcp *mcp, const uint8_t *value, + uint16_t length) +{ + struct event_callback *cb = mcp->cb; + + if (cb && cb->cbs && cb->cbs->player_name) + cb->cbs->player_name(mcp, value, length); +} + +static void mcp_mp_set_track_title(struct bt_mcp *mcp, const uint8_t *value, + uint16_t length) +{ + struct event_callback *cb = mcp->cb; + + if (cb && cb->cbs && cb->cbs->track_title) + cb->cbs->track_title(mcp, value, length); +} + +static void mcp_mp_set_title_duration(struct bt_mcp *mcp, int32_t duration) +{ + struct event_callback *cb = mcp->cb; + + DBG(mcp, "Track Duration 0x%08x", duration); + + if (cb && cb->cbs && cb->cbs->track_duration) + cb->cbs->track_duration(mcp, duration); +} + +static void mcp_mp_set_title_position(struct bt_mcp *mcp, int32_t position) +{ + struct event_callback *cb = mcp->cb; + + DBG(mcp, "Track Position 0x%08x", position); + + if (cb && cb->cbs && cb->cbs->track_position) + cb->cbs->track_position(mcp, position); +} + +static void mcp_mp_set_media_state(struct bt_mcp *mcp, uint8_t state) +{ + struct event_callback *cb = mcp->cb; + + DBG(mcp, "Media State 0x%02x", state); + + if (cb && cb->cbs && cb->cbs->media_state) + cb->cbs->media_state(mcp, state); +} + +static void read_media_player_name(bool success, uint8_t att_ecode, + const uint8_t *value, uint16_t length, + void *user_data) +{ + struct bt_mcp *mcp = user_data; + + if (!success) { + DBG(mcp, "Unable to read media player name: error 0x%02x", + att_ecode); + return; + } + + if (!length) + return; + + mcp_mp_set_player_name(mcp, value, length); +} + +static void read_track_title(bool success, uint8_t att_ecode, + const uint8_t *value, uint16_t length, + void *user_data) +{ + struct bt_mcp *mcp = user_data; + + if (!success) { + DBG(mcp, "Unable to read track title: error 0x%02x", + att_ecode); + return; + } + + if (!length) + return; + + mcp_mp_set_track_title(mcp, value, length); +} + +static void read_track_duration(bool success, uint8_t att_ecode, + const uint8_t *value, uint16_t length, + void *user_data) +{ + struct bt_mcp *mcp = user_data; + int32_t duration; + + if (!success) { + DBG(mcp, "Unable to read track duration: error 0x%02x", + att_ecode); + return; + } + + if (length != sizeof(duration)) + DBG(mcp, "Wrong length received Length : %u", length); + + memcpy(&duration, value, length); + mcp_mp_set_title_duration(mcp, duration); +} + +static void read_track_position(bool success, uint8_t att_ecode, + const uint8_t *value, uint16_t length, + void *user_data) +{ + struct bt_mcp *mcp = user_data; + int32_t position; + + if (!success) { + DBG(mcp, "Unable to read track position: error 0x%02x", + att_ecode); + return; + } + + if (length != sizeof(position)) + DBG(mcp, "Wrong length received Length : %u", length); + + memcpy(&position, value, length); + mcp_mp_set_title_position(mcp, position); +} + +static void read_media_state(bool success, uint8_t att_ecode, + const uint8_t *value, uint16_t length, + void *user_data) +{ + struct bt_mcp *mcp = user_data; + + if (!success) { + DBG(mcp, "Unable to read media state: error 0x%02x", + att_ecode); + return; + } + + if (length != sizeof(uint8_t)) + DBG(mcp, "Wrong length received Length : %u", length); + + mcp_mp_set_media_state(mcp, *value); +} + +static void read_media_cp_op_supported(bool success, uint8_t att_ecode, + const uint8_t *value, uint16_t length, + void *user_data) +{ + struct bt_mcp *mcp = user_data; + + if (!success) { + DBG(mcp, "Unable to read media CP OP supported: error 0x%02x", + att_ecode); + return; + } + + if (length != sizeof(uint32_t)) + DBG(mcp, "Wrong length received Length : %u", length); + + memcpy(&mcp->session.cp_op_supported, value, sizeof(uint32_t)); + DBG(mcp, "Media Control Point Opcodes Supported 0x%08x", + mcp->session.cp_op_supported); +} + +static void read_content_control_id(bool success, uint8_t att_ecode, + const uint8_t *value, uint16_t length, + void *user_data) +{ + struct bt_mcp *mcp = user_data; + + if (!success) { + DBG(mcp, "Unable to read content control id: error 0x%02x", + att_ecode); + return; + } + + if (length != sizeof(uint8_t)) + DBG(mcp, "Wrong length received Length : %u", length); + + DBG(mcp, "Content Control ID 0x%02x", *value); +} + +static void mcp_pending_destroy(void *data) +{ + struct bt_mcp_pending *pending = data; + struct bt_mcp *mcp = pending->mcp; + + queue_remove_if(mcp->pending, NULL, pending); +} + +static void mcp_pending_complete(bool success, uint8_t att_ecode, + const uint8_t *value, uint16_t length, + void *user_data) +{ + struct bt_mcp_pending *pending = user_data; + + if (pending->func) + pending->func(success, att_ecode, value, length, + pending->user_data); +} + +static void mcp_read_value(struct bt_mcp *mcp, uint16_t value_handle, + bt_gatt_client_read_callback_t func, + void *user_data) +{ + struct bt_mcp_pending *pending; + + pending = new0(struct bt_mcp_pending, 1); + pending->mcp = mcp; + pending->func = func; + pending->user_data = user_data; + + pending->id = bt_gatt_client_read_value(mcp->client, value_handle, + mcp_pending_complete, pending, + mcp_pending_destroy); + if (!pending->id) { + DBG(mcp, "Unable to send Read request"); + free(pending); + return; + } + + queue_push_tail(mcp->pending, pending); +} + +static void mcp_mp_name_register(uint16_t att_ecode, void *user_data) +{ + struct bt_mcp *mcp = user_data; + + if (att_ecode) + DBG(mcp, "Media Player Name notification failed: 0x%04x", + att_ecode); +} + +static void mcp_mp_name_notify(uint16_t value_handle, const uint8_t *value, + uint16_t length, void *user_data) +{ + struct bt_mcp *mcp = user_data; + + if (!length) + return; + + mcp_mp_set_player_name(mcp, value, length); +} + +static void mcp_track_changed_register(uint16_t att_ecode, void *user_data) +{ + struct bt_mcp *mcp = user_data; + + if (att_ecode) + DBG(mcp, "Media Track Changed notification failed: 0x%04x", + att_ecode); +} + +static void mcp_track_changed_notify(uint16_t value_handle, + const uint8_t *value, uint16_t length, void *user_data) +{ + struct bt_mcp *mcp = user_data; + struct event_callback *cb = mcp->cb; + + DBG(mcp, "Track Changed"); + + if (cb && cb->cbs && cb->cbs->track_changed) + cb->cbs->track_changed(mcp); +} + +static void mcp_track_title_register(uint16_t att_ecode, void *user_data) +{ + struct bt_mcp *mcp = user_data; + + if (att_ecode) + DBG(mcp, "Media Track Title notification failed: 0x%04x", + att_ecode); +} + +static void mcp_track_title_notify(uint16_t value_handle, + const uint8_t *value, uint16_t length, void *user_data) +{ + struct bt_mcp *mcp = user_data; + + mcp_mp_set_track_title(mcp, value, length); +} + +static void mcp_track_duration_register(uint16_t att_ecode, void *user_data) +{ + struct bt_mcp *mcp = user_data; + + if (att_ecode) + DBG(mcp, "Media Track Duration notification failed: 0x%04x", + att_ecode); +} + +static void mcp_track_duration_notify(uint16_t value_handle, + const uint8_t *value, uint16_t length, void *user_data) +{ + struct bt_mcp *mcp = user_data; + int32_t duration; + + memcpy(&duration, value, sizeof(int32_t)); + mcp_mp_set_title_duration(mcp, duration); +} + +static void mcp_track_position_register(uint16_t att_ecode, void *user_data) +{ + struct bt_mcp *mcp = user_data; + + if (att_ecode) + DBG(mcp, "Media Track Position notification failed: 0x%04x", + att_ecode); +} + +static void mcp_track_position_notify(uint16_t value_handle, + const uint8_t *value, uint16_t length, void *user_data) +{ + struct bt_mcp *mcp = user_data; + int32_t position; + + memcpy(&position, value, sizeof(int32_t)); + mcp_mp_set_title_position(mcp, position); +} + +static void mcp_media_state_register(uint16_t att_ecode, void *user_data) +{ + struct bt_mcp *mcp = user_data; + + if (att_ecode) + DBG(mcp, "Media Media State notification failed: 0x%04x", + att_ecode); +} + +static void mcp_media_state_notify(uint16_t value_handle, + const uint8_t *value, uint16_t length, void *user_data) +{ + struct bt_mcp *mcp = user_data; + + mcp_mp_set_media_state(mcp, *value); +} + +static void mcp_media_cp_register(uint16_t att_ecode, void *user_data) +{ + struct bt_mcp *mcp = user_data; + + if (att_ecode) + DBG(mcp, "Media Media CP notification failed: 0x%04x", + att_ecode); +} + +static void mcp_media_cp_notify(uint16_t value_handle, const uint8_t *value, + uint16_t length, void *user_data) +{ + struct bt_mcp *mcp = user_data; + + DBG(mcp, "Media CP Notification"); +} + +static void mcp_media_cp_op_supported_register(uint16_t att_ecode, + void *user_data) +{ + struct bt_mcp *mcp = user_data; + + if (att_ecode) + DBG(mcp, "Media Media CP OP Supported notify failed: 0x%04x", + att_ecode); +} + +static void mcp_media_cp_op_supported_notify(uint16_t value_handle, + const uint8_t *value, uint16_t length, void *user_data) +{ + struct bt_mcp *mcp = user_data; + + memcpy(&mcp->session.cp_op_supported, value, sizeof(uint32_t)); + DBG(mcp, "Media CP Opcodes Supported Notification 0x%08x", + mcp->session.cp_op_supported); +} + +static void bt_mcp_mp_name_attach(struct bt_mcp *mcp) +{ + uint16_t value_handle; + struct bt_mcs *mcs = mcp_get_mcs(mcp); + + if (!gatt_db_attribute_get_char_data(mcs->mp_name, NULL, &value_handle, + NULL, NULL, NULL)) + return; + + DBG(mcp, "Media Player handle 0x%04x", value_handle); + + mcp_read_value(mcp, value_handle, read_media_player_name, mcp); + + mcp->mp_name_id = bt_gatt_client_register_notify(mcp->client, + value_handle, mcp_mp_name_register, + mcp_mp_name_notify, mcp, NULL); +} + +static void bt_mcp_track_changed_attach(struct bt_mcp *mcp) +{ + uint16_t value_handle; + struct bt_mcs *mcs = mcp_get_mcs(mcp); + + if (!gatt_db_attribute_get_char_data(mcs->track_changed, NULL, + &value_handle, NULL, NULL, NULL)) + return; + + DBG(mcp, "Track Changed handle 0x%04x", value_handle); + + mcp->track_changed_id = bt_gatt_client_register_notify(mcp->client, + value_handle, mcp_track_changed_register, + mcp_track_changed_notify, mcp, NULL); +} + +static void bt_mcp_track_title_attach(struct bt_mcp *mcp) +{ + uint16_t value_handle; + struct bt_mcs *mcs = mcp_get_mcs(mcp); + + if (!gatt_db_attribute_get_char_data(mcs->track_title, NULL, + &value_handle, NULL, NULL, NULL)) + return; + + DBG(mcp, "Track Title handle 0x%04x", value_handle); + + mcp_read_value(mcp, value_handle, read_track_title, mcp); + + mcp->track_title_id = bt_gatt_client_register_notify(mcp->client, + value_handle, mcp_track_title_register, + mcp_track_title_notify, mcp, NULL); +} + +static void bt_mcp_track_duration_attach(struct bt_mcp *mcp) +{ + uint16_t value_handle; + struct bt_mcs *mcs = mcp_get_mcs(mcp); + + if (!gatt_db_attribute_get_char_data(mcs->track_duration, NULL, + &value_handle, NULL, NULL, NULL)) + return; + + DBG(mcp, "Track Duration handle 0x%04x", value_handle); + + mcp_read_value(mcp, value_handle, read_track_duration, mcp); + + mcp->track_duration_id = bt_gatt_client_register_notify(mcp->client, + value_handle, mcp_track_duration_register, + mcp_track_duration_notify, mcp, NULL); +} + +static void bt_mcp_track_position_attach(struct bt_mcp *mcp) +{ + uint16_t value_handle; + struct bt_mcs *mcs = mcp_get_mcs(mcp); + + if (!gatt_db_attribute_get_char_data(mcs->track_position, NULL, + &value_handle, NULL, NULL, NULL)) + return; + + DBG(mcp, "Track Position handle 0x%04x", value_handle); + + mcp_read_value(mcp, value_handle, read_track_position, mcp); + + mcp->track_position_id = bt_gatt_client_register_notify(mcp->client, + value_handle, mcp_track_position_register, + mcp_track_position_notify, mcp, NULL); +} + +static void bt_mcp_media_state_attach(struct bt_mcp *mcp) +{ + uint16_t value_handle; + struct bt_mcs *mcs = mcp_get_mcs(mcp); + + if (!gatt_db_attribute_get_char_data(mcs->media_state, NULL, + &value_handle, NULL, NULL, NULL)) + return; + + DBG(mcp, "Media State handle 0x%04x", value_handle); + + mcp_read_value(mcp, value_handle, read_media_state, mcp); + + mcp->media_state_id = bt_gatt_client_register_notify(mcp->client, + value_handle, mcp_media_state_register, + mcp_media_state_notify, mcp, NULL); +} + +static void bt_mcp_media_cp_attach(struct bt_mcp *mcp) +{ + uint16_t value_handle; + struct bt_mcs *mcs = mcp_get_mcs(mcp); + + if (!gatt_db_attribute_get_char_data(mcs->media_cp, NULL, + &value_handle, NULL, NULL, NULL)) + return; + + DBG(mcp, "Media Control Point handle 0x%04x", value_handle); + + mcp->media_cp_id = bt_gatt_client_register_notify(mcp->client, + value_handle, mcp_media_cp_register, + mcp_media_cp_notify, mcp, NULL); +} + +static void bt_mcp_media_cp_op_supported_attach(struct bt_mcp *mcp) +{ + uint16_t value_handle; + struct bt_mcs *mcs = mcp_get_mcs(mcp); + + if (!gatt_db_attribute_get_char_data(mcs->media_cp_op_supportd, NULL, + &value_handle, NULL, NULL, NULL)) + return; + + DBG(mcp, "Media Control Point Opcodes Supported handle 0x%04x", + value_handle); + + mcp_read_value(mcp, value_handle, read_media_cp_op_supported, mcp); + + mcp->media_cp_op_supported_id = bt_gatt_client_register_notify( + mcp->client, value_handle, mcp_media_cp_op_supported_register, + mcp_media_cp_op_supported_notify, mcp, NULL); +} + +static void bt_mcp_content_control_id_supported_attach(struct bt_mcp *mcp) +{ + uint16_t value_handle; + struct bt_mcs *mcs = mcp_get_mcs(mcp); + + if (!gatt_db_attribute_get_char_data(mcs->content_control_id, NULL, + &value_handle, NULL, NULL, NULL)) + return; + + DBG(mcp, "Media Content Control id Supported handle 0x%04x", + value_handle); + mcp_read_value(mcp, value_handle, read_content_control_id, mcp); +} + +static void foreach_mcs_char(struct gatt_db_attribute *attr, void *user_data) +{ + struct bt_mcp *mcp = user_data; + uint16_t value_handle; + bt_uuid_t uuid, uuid_mp_name, uuid_track_changed, uuid_track_title, + uuid_track_duration, uuid_track_position, uuid_media_state, + uuid_media_cp, uuid_media_cp_op_supported, + uuid_content_control_id; + struct bt_mcs *mcs; + + if (!gatt_db_attribute_get_char_data(attr, NULL, &value_handle, + NULL, NULL, &uuid)) + return; + + bt_uuid16_create(&uuid_mp_name, MEDIA_PLAYER_NAME_CHRC_UUID); + bt_uuid16_create(&uuid_track_changed, MEDIA_TRACK_CHNGD_CHRC_UUID); + bt_uuid16_create(&uuid_track_title, MEDIA_TRACK_TITLE_CHRC_UUID); + bt_uuid16_create(&uuid_track_duration, MEDIA_TRACK_DURATION_CHRC_UUID); + bt_uuid16_create(&uuid_track_position, MEDIA_TRACK_POSTION_CHRC_UUID); + bt_uuid16_create(&uuid_media_state, MEDIA_STATE_CHRC_UUID); + bt_uuid16_create(&uuid_media_cp, MEDIA_CP_CHRC_UUID); + bt_uuid16_create(&uuid_media_cp_op_supported, + MEDIA_CP_OP_SUPPORTED_CHRC_UUID); + bt_uuid16_create(&uuid_content_control_id, + MEDIA_CONTENT_CONTROL_ID_CHRC_UUID); + + if (!bt_uuid_cmp(&uuid, &uuid_mp_name)) { + DBG(mcp, "Media Player Name found: handle 0x%04x", + value_handle); + + mcs = mcp_get_mcs(mcp); + if (!mcs || mcs->mp_name) + return; + + mcs->mp_name = attr; + bt_mcp_mp_name_attach(mcp); + } + + if (!bt_uuid_cmp(&uuid, &uuid_track_changed)) { + DBG(mcp, "Track Changed found: handle 0x%04x", value_handle); + + mcs = mcp_get_mcs(mcp); + if (!mcs || mcs->track_changed) + return; + + mcs->track_changed = attr; + bt_mcp_track_changed_attach(mcp); + } + + if (!bt_uuid_cmp(&uuid, &uuid_track_title)) { + DBG(mcp, "Track Title found: handle 0x%04x", value_handle); + + mcs = mcp_get_mcs(mcp); + if (!mcs || mcs->track_title) + return; + + mcs->track_title = attr; + bt_mcp_track_title_attach(mcp); + } + + if (!bt_uuid_cmp(&uuid, &uuid_track_duration)) { + DBG(mcp, "Track Duration found: handle 0x%04x", value_handle); + + mcs = mcp_get_mcs(mcp); + if (!mcs || mcs->track_duration) + return; + + mcs->track_duration = attr; + bt_mcp_track_duration_attach(mcp); + } + + if (!bt_uuid_cmp(&uuid, &uuid_track_position)) { + DBG(mcp, "Track Position found: handle 0x%04x", value_handle); + + + mcs = mcp_get_mcs(mcp); + if (!mcs || mcs->track_position) + return; + + mcs->track_position = attr; + bt_mcp_track_position_attach(mcp); + } + + if (!bt_uuid_cmp(&uuid, &uuid_media_state)) { + DBG(mcp, "Media State found: handle 0x%04x", value_handle); + + mcs = mcp_get_mcs(mcp); + if (!mcs || mcs->media_state) + return; + + mcs->media_state = attr; + bt_mcp_media_state_attach(mcp); + } + + if (!bt_uuid_cmp(&uuid, &uuid_media_cp)) { + DBG(mcp, "Media Control Point found: handle 0x%04x", + value_handle); + + mcs = mcp_get_mcs(mcp); + if (!mcs || mcs->media_cp) + return; + + mcs->media_cp = attr; + bt_mcp_media_cp_attach(mcp); + } + + if (!bt_uuid_cmp(&uuid, &uuid_media_cp_op_supported)) { + DBG(mcp, "Media CP Opcodes Supported found: handle 0x%04x", + value_handle); + + mcs = mcp_get_mcs(mcp); + if (!mcs || mcs->media_cp_op_supportd) + return; + + mcs->media_cp_op_supportd = attr; + bt_mcp_media_cp_op_supported_attach(mcp); + } + + if (!bt_uuid_cmp(&uuid, &uuid_content_control_id)) { + DBG(mcp, "Content Control ID found: handle 0x%04x", + value_handle); + + mcs = mcp_get_mcs(mcp); + if (!mcs || mcs->content_control_id) + return; + + mcs->content_control_id = attr; + bt_mcp_content_control_id_supported_attach(mcp); + } +} + +void bt_mcp_set_event_callbacks(struct bt_mcp *mcp, + const struct bt_mcp_event_callback *cbs, + void *user_data) +{ + struct event_callback *cb; + + if (mcp->cb) + free(mcp->cb); + + cb = new0(struct event_callback, 1); + cb->cbs = cbs; + cb->user_data = user_data; + + mcp->cb = cb; +} + +static void foreach_mcs_service(struct gatt_db_attribute *attr, + void *user_data) +{ + struct bt_mcp *mcp = user_data; + struct bt_mcs *mcs = mcp_get_mcs(mcp); + + DBG(mcp, ""); + + mcs->service = attr; + + gatt_db_service_foreach_char(attr, foreach_mcs_char, mcp); +} + +static struct bt_mcp_db *mcp_db_new(struct gatt_db *db) +{ + struct bt_mcp_db *mdb; + + if (!db) + return NULL; + + mdb = new0(struct bt_mcp_db, 1); + mdb->db = gatt_db_ref(db); + + if (!mcp_db) + mcp_db = queue_new(); + + queue_push_tail(mcp_db, mdb); + + mdb->mcs = mcs_new(db); + return mdb; +} + +static struct bt_mcp_db *mcp_get_db(struct gatt_db *db) +{ + struct bt_mcp_db *mdb; + + mdb = queue_find(mcp_db, mcp_db_match, db); + if (mdb) + return mdb; + + return mcp_db_new(db); +} + +struct bt_mcp *bt_mcp_new(struct gatt_db *ldb, struct gatt_db *rdb) +{ + struct bt_mcp *mcp; + struct bt_mcp_db *mdb; + + if (!ldb) + return NULL; + + mdb = mcp_get_db(ldb); + if (!mdb) + return NULL; + + mcp = new0(struct bt_mcp, 1); + mcp->ldb = mdb; + mcp->pending = queue_new(); + + if (!rdb) + goto done; + + mdb = new0(struct bt_mcp_db, 1); + mdb->db = gatt_db_ref(rdb); + + mcp->rdb = mdb; + +done: + bt_mcp_ref(mcp); + + return mcp; +} + +void bt_mcp_register(struct gatt_db *db) +{ + mcp_db_new(db); +} + +bool bt_mcp_attach(struct bt_mcp *mcp, struct bt_gatt_client *client) +{ + bt_uuid_t uuid; + + DBG(mcp, "mcp %p", mcp); + + mcp->client = bt_gatt_client_clone(client); + if (!mcp->client) + return false; + + if (mcp->rdb->mcs) { + bt_mcp_mp_name_attach(mcp); + bt_mcp_track_changed_attach(mcp); + bt_mcp_track_title_attach(mcp); + bt_mcp_track_duration_attach(mcp); + bt_mcp_track_position_attach(mcp); + bt_mcp_media_state_attach(mcp); + bt_mcp_media_cp_attach(mcp); + bt_mcp_media_cp_op_supported_attach(mcp); + bt_mcp_content_control_id_supported_attach(mcp); + + return true; + } + + bt_uuid16_create(&uuid, GMCS_UUID); + gatt_db_foreach_service(mcp->rdb->db, &uuid, foreach_mcs_service, mcp); + + return true; +} + +void bt_mcp_detach(struct bt_mcp *mcp) +{ + DBG(mcp, "%p", mcp); + + bt_gatt_client_unref(mcp->client); + mcp->client = NULL; +} diff -pruN 5.65-1/src/shared/mcp.h 5.66-1/src/shared/mcp.h --- 5.65-1/src/shared/mcp.h 1970-01-01 00:00:00.000000000 +0000 +++ 5.66-1/src/shared/mcp.h 2022-11-10 20:22:09.000000000 +0000 @@ -0,0 +1,61 @@ +/* SPDX-License-Identifier: LGPL-2.1-or-later */ +/* + * + * BlueZ - Bluetooth protocol stack for Linux + * + * Copyright (C) 2020 Intel Corporation. All rights reserved. + * + */ + +#include +#include + +#ifndef __packed +#define __packed __attribute__((packed)) +#endif + +struct bt_mcp; +struct bt_mcp_db; +struct bt_mcp_session_info; + +typedef void (*bt_mcp_debug_func_t)(const char *str, void *user_data); +typedef void (*bt_mcp_destroy_func_t)(void *user_data); + +struct bt_mcp_event_callback { + void (*player_name)(struct bt_mcp *mcp, const uint8_t *value, + uint16_t length); + void (*track_changed)(struct bt_mcp *mcp); + void (*track_title)(struct bt_mcp *mcp, const uint8_t *value, + uint16_t length); + void (*track_duration)(struct bt_mcp *mcp, int32_t duration); + void (*track_position)(struct bt_mcp *mcp, int32_t position); + void (*playback_speed)(struct bt_mcp *mcp, int8_t speed); + void (*seeking_speed)(struct bt_mcp *mcp, int8_t speed); + void (*play_order)(struct bt_mcp *mcp, uint8_t order); + void (*play_order_supported)(struct bt_mcp *mcp, + uint16_t order_supported); + void (*media_state)(struct bt_mcp *mcp, uint8_t state); + void (*content_control_id)(struct bt_mcp *mcp, uint8_t cc_id); +}; + +void bt_mcp_set_event_callbacks(struct bt_mcp *mcp, + const struct bt_mcp_event_callback *cbs, + void *user_data); + +bool bt_mcp_set_debug(struct bt_mcp *mcp, bt_mcp_debug_func_t cb, + void *user_data, bt_mcp_destroy_func_t destroy); + +void bt_mcp_register(struct gatt_db *db); +bool bt_mcp_attach(struct bt_mcp *mcp, struct bt_gatt_client *client); +void bt_mcp_detach(struct bt_mcp *mcp); + +struct bt_mcp *bt_mcp_new(struct gatt_db *ldb, struct gatt_db *rdb); +struct bt_mcp *bt_mcp_ref(struct bt_mcp *mcp); +void bt_mcp_unref(struct bt_mcp *mcp); + +bool bt_mcp_set_user_data(struct bt_mcp *mcp, void *user_data); +void *bt_mcp_get_user_data(struct bt_mcp *mcp); + +unsigned int bt_mcp_play(struct bt_mcp *mcp); +unsigned int bt_mcp_pause(struct bt_mcp *mcp); +unsigned int bt_mcp_stop(struct bt_mcp *mcp); diff -pruN 5.65-1/src/shared/mcs.h 5.66-1/src/shared/mcs.h --- 5.65-1/src/shared/mcs.h 1970-01-01 00:00:00.000000000 +0000 +++ 5.66-1/src/shared/mcs.h 2022-11-10 20:22:09.000000000 +0000 @@ -0,0 +1,65 @@ +/* SPDX-License-Identifier: LGPL-2.1-or-later */ +/* + * + * BlueZ - Bluetooth protocol stack for Linux + * + * Copyright (C) 2020 Intel Corporation. All rights reserved. + * + */ + +/* MCP Media State */ +#define BT_MCS_STATUS_INACTIVE 0x00 +#define BT_MCS_STATUS_PLAYING 0x01 +#define BT_MCS_STATUS_PAUSED 0x02 +#define BT_MCS_STATUS_SEEKING 0x03 + +/* MCP Control Point Opcodes */ +#define BT_MCS_CMD_PLAY 0x01 +#define BT_MCS_CMD_PAUSE 0x02 +#define BT_MCS_CMD_FAST_REWIND 0x03 +#define BT_MCS_CMD_FAST_FORWARD 0x04 +#define BT_MCS_CMD_STOP 0x05 + +#define BT_MCS_CMD_MOVE_RELATIVE 0x10 + +#define BT_MCS_CMD_PREV_SEGMENT 0x20 +#define BT_MCS_CMD_NEXT_SEGMENT 0x21 +#define BT_MCS_CMD_FIRST_SEGMENT 0x22 +#define BT_MCS_CMD_LAST_SEGMENT 0x23 +#define BT_MCS_CMD_GOTO_SEGMENT 0x24 + +#define BT_MCS_CMD_PREV_TRACK 0x30 +#define BT_MCS_CMD_NEXT_TRACK 0x31 +#define BT_MCS_CMD_FIRST_TRACK 0x32 +#define BT_MCS_CMD_LAST_TRACK 0x33 +#define BT_MCS_CMD_GOTO_TRACK 0x34 + +#define BT_MCS_CMD_PREV_GROUP 0x40 +#define BT_MCS_CMD_NEXT_GROUP 0x41 +#define BT_MCS_CMD_FIRST_GROUP 0x42 +#define BT_MCS_CMD_LAST_GROUP 0x43 +#define BT_MCS_CMD_GOTO_GROUP 0x44 + + +/* MCP Control Point Opcodes Supported */ +#define BT_MCS_CMD_PLAY_SUPPORTED 0x00000001 +#define BT_MCS_CMD_PAUSE_SUPPORTED 0x00000002 +#define BT_MCS_CMD_FAST_REWIND_SUPPORTED 0x00000004 +#define BT_MCS_CMD_FAST_FORWARD_SUPPORTED 0x00000008 +#define BT_MCS_CMD_STOP_SUPPORTED 0x00000010 +#define BT_MCS_CMD_MOVE_RELATIVE_SUPPORTED 0x00000020 +#define BT_MCS_CMD_PREV_SEGMENT_SUPPORTED 0x00000040 +#define BT_MCS_CMD_NEXT_SEGMENT_SUPPORTED 0x00000080 +#define BT_MCS_CMD_FIRST_SEGMENT_SUPPORTED 0x00000100 +#define BT_MCS_CMD_LAST_SEGMENT_SUPPORTED 0x00000200 +#define BT_MCS_CMD_GOTO_SEGMENT_SUPPORTED 0x00000400 +#define BT_MCS_CMD_PREV_TRACK_SUPPORTED 0x00000800 +#define BT_MCS_CMD_NEXT_TRACK_SUPPORTED 0x00001000 +#define BT_MCS_CMD_FIRST_TRACK_SUPPORTED 0x00002000 +#define BT_MCS_CMD_LAST_TRACK_SUPPORTED 0x00004000 +#define BT_MCS_CMD_GOTO_TRACK_SUPPORTED 0x00008000 +#define BT_MCS_CMD_PREV_GROUP_SUPPORTED 0x00010000 +#define BT_MCS_CMD_NEXT_GROUP_SUPPORTED 0x00020000 +#define BT_MCS_CMD_FIRST_GROUP_SUPPORTED 0x00040000 +#define BT_MCS_CMD_LAST_GROUP_SUPPORTED 0x00080000 +#define BT_MCS_CMD_GOTO_GROUP_SUPPORTED 0x00100000 diff -pruN 5.65-1/src/shared/shell.c 5.66-1/src/shared/shell.c --- 5.65-1/src/shared/shell.c 2022-07-24 20:59:52.000000000 +0000 +++ 5.66-1/src/shared/shell.c 2022-11-10 20:22:09.000000000 +0000 @@ -1101,6 +1101,7 @@ void bt_shell_init(int argc, char **argv struct option options[256]; char optstr[256]; size_t offset; + char *endptr = NULL; offset = sizeof(main_options) / sizeof(struct option); @@ -1132,7 +1133,11 @@ void bt_shell_init(int argc, char **argv data.mode = 1; goto done; case 't': - data.timeout = atoi(optarg); + if (optarg) + data.timeout = strtol(optarg, &endptr, 0); + + if (!endptr || *endptr != '\0') + printf("Unable to parse timeout\n"); break; case 'z': data.zsh = 1; @@ -1158,7 +1163,7 @@ void bt_shell_init(int argc, char **argv return; } - *opt->optarg[index - offset] = optarg; + *opt->optarg[index - offset] = optarg ? : ""; } index = -1; diff -pruN 5.65-1/src/shared/tester.c 5.66-1/src/shared/tester.c --- 5.65-1/src/shared/tester.c 2022-07-24 20:59:52.000000000 +0000 +++ 5.66-1/src/shared/tester.c 2022-11-10 20:22:09.000000000 +0000 @@ -34,6 +34,7 @@ #include "src/shared/mainloop.h" #include "src/shared/util.h" +#include "src/shared/io.h" #include "src/shared/tester.h" #include "src/shared/log.h" #include "src/shared/timeout.h" @@ -81,6 +82,8 @@ struct test_case { enum test_result result; enum test_stage stage; const void *test_data; + const struct iovec *iov; + size_t iovcnt; tester_data_func_t pre_setup_func; tester_data_func_t setup_func; tester_data_func_t test_func; @@ -346,14 +349,20 @@ void tester_add(const char *name, const teardown_func, NULL, 0, NULL, NULL); } -void *tester_get_data(void) +static struct test_case *tester_get_test(void) { - struct test_case *test; - if (!test_current) return NULL; - test = test_current->data; + return test_current->data; +} + +void *tester_get_data(void) +{ + struct test_case *test = tester_get_test(); + + if (!test) + return NULL; return test->user_data; } @@ -859,6 +868,142 @@ void tester_init(int *argc, char ***argv test_current = NULL; } +static struct io *ios[2]; + +static bool io_disconnected(struct io *io, void *user_data) +{ + if (io == ios[0]) { + io_destroy(ios[0]); + ios[0] = NULL; + } else if (io == ios[1]) { + io_destroy(ios[1]); + ios[1] = NULL; + } + + return false; +} + +static const struct iovec *test_get_iov(struct test_case *test) +{ + const struct iovec *iov; + + if (!test || !test->iov || !test->iovcnt) + return NULL; + + iov = test->iov; + + test->iov++; + test->iovcnt--; + + return iov; +} + +static bool test_io_send(struct io *io, void *user_data) +{ + struct test_case *test = tester_get_test(); + const struct iovec *iov = test_get_iov(test); + ssize_t len; + + if (!iov) + return false; + + len = io_send(io, iov, 1); + + tester_monitor('<', 0x0004, 0x0000, iov->iov_base, len); + + g_assert_cmpint(len, ==, iov->iov_len); + + return false; +} + +static bool test_io_recv(struct io *io, void *user_data) +{ + struct test_case *test = tester_get_test(); + const struct iovec *iov = test_get_iov(test); + unsigned char buf[512]; + int fd; + ssize_t len; + + fd = io_get_fd(io); + + len = read(fd, buf, sizeof(buf)); + + g_assert(len > 0); + + tester_monitor('>', 0x0004, 0x0000, buf, len); + + if (!iov) + return true; + + g_assert_cmpint(len, ==, iov->iov_len); + + g_assert(memcmp(buf, iov->iov_base, len) == 0); + + if (test->iovcnt) + io_set_write_handler(io, test_io_send, NULL, NULL); + + return true; +} + +static void setup_io(void) +{ + int fd[2], err; + + io_destroy(ios[0]); + io_destroy(ios[1]); + + err = socketpair(AF_UNIX, SOCK_SEQPACKET | SOCK_CLOEXEC, 0, fd); + if (err < 0) { + tester_warn("socketpair: %s (%d)", strerror(errno), errno); + return; + } + + ios[0] = io_new(fd[0]); + if (!ios[0]) { + tester_warn("io_new: %p", ios[0]); + return; + } + + io_set_close_on_destroy(ios[0], true); + io_set_disconnect_handler(ios[0], io_disconnected, NULL, NULL); + + ios[1] = io_new(fd[1]); + if (!ios[1]) { + tester_warn("io_new: %p", ios[1]); + return; + } + + io_set_close_on_destroy(ios[1], true); + io_set_disconnect_handler(ios[1], io_disconnected, NULL, NULL); + io_set_read_handler(ios[1], test_io_recv, NULL, NULL); +} + +struct io *tester_setup_io(const struct iovec *iov, int iovcnt) +{ + struct test_case *test = tester_get_test(); + + if (!ios[0] || !ios[1]) { + setup_io(); + if (!ios[0] || !ios[1]) { + tester_warn("Unable to setup IO"); + return NULL; + } + } + + test->iov = iov; + test->iovcnt = iovcnt; + + return ios[0]; +} + +void tester_io_send(void) +{ + struct test_case *test = tester_get_test(); + + if (test->iovcnt) + io_set_write_handler(ios[1], test_io_send, NULL, NULL); +} + int tester_run(void) { int ret; @@ -879,5 +1024,8 @@ int tester_run(void) if (option_monitor) bt_log_close(); + io_destroy(ios[0]); + io_destroy(ios[1]); + return ret == 0 ? EXIT_SUCCESS : EXIT_FAILURE; } diff -pruN 5.65-1/src/shared/tester.h 5.66-1/src/shared/tester.h --- 5.65-1/src/shared/tester.h 2022-07-24 20:59:52.000000000 +0000 +++ 5.66-1/src/shared/tester.h 2022-11-10 20:22:09.000000000 +0000 @@ -11,6 +11,15 @@ #include #include #include +#include + +#define data(args...) ((const unsigned char[]) { args }) + +#define IOV_DATA(args...) \ + { \ + .iov_base = (void *)data(args), \ + .iov_len = sizeof(data(args)), \ + } void tester_init(int *argc, char ***argv); int tester_run(void); @@ -66,3 +75,6 @@ typedef void (*tester_wait_func_t)(void void tester_wait(unsigned int seconds, tester_wait_func_t func, void *user_data); + +struct io *tester_setup_io(const struct iovec *iov, int iovcnt); +void tester_io_send(void); diff -pruN 5.65-1/src/shared/util.c 5.66-1/src/shared/util.c --- 5.65-1/src/shared/util.c 2022-07-24 20:59:52.000000000 +0000 +++ 5.66-1/src/shared/util.c 2022-11-10 20:22:09.000000000 +0000 @@ -65,7 +65,7 @@ void *util_memdup(const void *src, size_ void util_debug_va(util_debug_func_t function, void *user_data, const char *format, va_list va) { - char str[78]; + char str[MAX_INPUT]; if (!function || !format) return; @@ -338,7 +338,7 @@ static const struct { { 0x1849, "Generic Media Control" }, { 0x184b, "Telephony Bearer" }, { 0x184c, "Generic Telephony Bearer" }, - { 0x184c, "Microphone Control" }, + { 0x184d, "Microphone Control" }, { 0x184e, "Audio Stream Control" }, { 0x184f, "Broadcast Audio Scan" }, { 0x1850, "Published Audio Capabilities" }, diff -pruN 5.65-1/src/shared/vcp.c 5.66-1/src/shared/vcp.c --- 5.65-1/src/shared/vcp.c 1970-01-01 00:00:00.000000000 +0000 +++ 5.66-1/src/shared/vcp.c 2022-11-10 20:22:09.000000000 +0000 @@ -0,0 +1,1168 @@ +// SPDX-License-Identifier: LGPL-2.1-or-later +/* + * + * BlueZ - Bluetooth protocol stack for Linux + * + * Copyright (C) 2022 Intel Corporation. All rights reserved. + * + */ + +#define _GNU_SOURCE +#include +#include +#include +#include +#include +#include + +#include "lib/bluetooth.h" +#include "lib/uuid.h" + +#include "src/shared/queue.h" +#include "src/shared/util.h" +#include "src/shared/timeout.h" +#include "src/shared/att.h" +#include "src/shared/gatt-db.h" +#include "src/shared/gatt-server.h" +#include "src/shared/gatt-client.h" +#include "src/shared/vcp.h" + +#define DBG(_vcp, fmt, arg...) \ + vcp_debug(_vcp, "%s:%s() " fmt, __FILE__, __func__, ## arg) + +#define VCP_STEP_SIZE 1 + +/* Apllication Error Code */ +#define BT_ATT_ERROR_INVALID_CHANGE_COUNTER 0x80 +#define BT_ATT_ERROR_OPCODE_NOT_SUPPORTED 0x81 + +struct bt_vcp_db { + struct gatt_db *db; + struct bt_vcs *vcs; +}; + +typedef void (*vcp_func_t)(struct bt_vcp *vcp, bool success, uint8_t att_ecode, + const uint8_t *value, uint16_t length, + void *user_data); + +struct bt_vcp_pending { + unsigned int id; + struct bt_vcp *vcp; + vcp_func_t func; + void *user_data; +}; + +struct bt_vcs_param { + uint8_t op; + uint8_t change_counter; +} __packed; + +struct bt_vcs_ab_vol { + uint8_t change_counter; + uint8_t vol_set; +} __packed; + +struct bt_vcp_cb { + unsigned int id; + bt_vcp_func_t attached; + bt_vcp_func_t detached; + void *user_data; +}; + +typedef void (*vcp_notify_t)(struct bt_vcp *vcp, uint16_t value_handle, + const uint8_t *value, uint16_t length, + void *user_data); + +struct bt_vcp_notify { + unsigned int id; + struct bt_vcp *vcp; + vcp_notify_t func; + void *user_data; +}; + +struct bt_vcp { + int ref_count; + struct bt_vcp_db *ldb; + struct bt_vcp_db *rdb; + struct bt_gatt_client *client; + struct bt_att *att; + unsigned int vstate_id; + unsigned int vflag_id; + + struct queue *notify; + struct queue *pending; + + bt_vcp_debug_func_t debug_func; + bt_vcp_destroy_func_t debug_destroy; + void *debug_data; + void *user_data; +}; + +#define RESET_VOLUME_SETTING 0x00 +#define USERSET_VOLUME_SETTING 0x01 + +/* Contains local bt_vcp_db */ +struct vol_state { + uint8_t vol_set; + uint8_t mute; + uint8_t counter; +} __packed; + +struct bt_vcs { + struct bt_vcp_db *vdb; + struct vol_state *vstate; + uint8_t vol_flag; + struct gatt_db_attribute *service; + struct gatt_db_attribute *vs; + struct gatt_db_attribute *vs_ccc; + struct gatt_db_attribute *vol_cp; + struct gatt_db_attribute *vf; + struct gatt_db_attribute *vf_ccc; +}; + +static struct queue *vcp_db; +static struct queue *vcp_cbs; +static struct queue *sessions; + +static void *iov_pull_mem(struct iovec *iov, size_t len) +{ + void *data = iov->iov_base; + + if (iov->iov_len < len) + return NULL; + + iov->iov_base += len; + iov->iov_len -= len; + + return data; +} + +static struct bt_vcp_db *vcp_get_vdb(struct bt_vcp *vcp) +{ + if (!vcp) + return NULL; + + if (vcp->ldb) + return vcp->ldb; + + return NULL; +} + +static struct vol_state *vdb_get_vstate(struct bt_vcp_db *vdb) +{ + if (!vdb->vcs) + return NULL; + + if (vdb->vcs->vstate) + return vdb->vcs->vstate; + + return NULL; +} + +static struct bt_vcs *vcp_get_vcs(struct bt_vcp *vcp) +{ + if (!vcp) + return NULL; + + if (vcp->rdb->vcs) + return vcp->rdb->vcs; + + vcp->rdb->vcs = new0(struct bt_vcs, 1); + vcp->rdb->vcs->vdb = vcp->rdb; + + return vcp->rdb->vcs; +} + +static void vcp_detached(void *data, void *user_data) +{ + struct bt_vcp_cb *cb = data; + struct bt_vcp *vcp = user_data; + + cb->detached(vcp, cb->user_data); +} + +void bt_vcp_detach(struct bt_vcp *vcp) +{ + if (!queue_remove(sessions, vcp)) + return; + + bt_gatt_client_unref(vcp->client); + vcp->client = NULL; + + queue_foreach(vcp_cbs, vcp_detached, vcp); +} + +static void vcp_db_free(void *data) +{ + struct bt_vcp_db *vdb = data; + + if (!vdb) + return; + + gatt_db_unref(vdb->db); + + free(vdb->vcs); + free(vdb); +} + +static void vcp_free(void *data) +{ + struct bt_vcp *vcp = data; + + bt_vcp_detach(vcp); + + vcp_db_free(vcp->rdb); + + queue_destroy(vcp->pending, NULL); + + free(vcp); +} +bool bt_vcp_set_user_data(struct bt_vcp *vcp, void *user_data) +{ + if (!vcp) + return false; + + vcp->user_data = user_data; + + return true; +} + +static bool vcp_db_match(const void *data, const void *match_data) +{ + const struct bt_vcp_db *vdb = data; + const struct gatt_db *db = match_data; + + return (vdb->db == db); +} + +struct bt_att *bt_vcp_get_att(struct bt_vcp *vcp) +{ + if (!vcp) + return NULL; + + if (vcp->att) + return vcp->att; + + return bt_gatt_client_get_att(vcp->client); +} + +struct bt_vcp *bt_vcp_ref(struct bt_vcp *vcp) +{ + if (!vcp) + return NULL; + + __sync_fetch_and_add(&vcp->ref_count, 1); + + return vcp; +} + +void bt_vcp_unref(struct bt_vcp *vcp) +{ + if (!vcp) + return; + + if (__sync_sub_and_fetch(&vcp->ref_count, 1)) + return; + + vcp_free(vcp); +} + +static void vcp_debug(struct bt_vcp *vcp, const char *format, ...) +{ + va_list ap; + + if (!vcp || !format || !vcp->debug_func) + return; + + va_start(ap, format); + util_debug_va(vcp->debug_func, vcp->debug_data, format, ap); + va_end(ap); +} + +static void vcp_disconnected(int err, void *user_data) +{ + struct bt_vcp *vcp = user_data; + + DBG(vcp, "vcp %p disconnected err %d", vcp, err); + + bt_vcp_detach(vcp); +} + +static struct bt_vcp *vcp_get_session(struct bt_att *att, struct gatt_db *db) +{ + const struct queue_entry *entry; + struct bt_vcp *vcp; + + for (entry = queue_get_entries(sessions); entry; entry = entry->next) { + struct bt_vcp *vcp = entry->data; + + if (att == bt_vcp_get_att(vcp)) + return vcp; + } + + vcp = bt_vcp_new(db, NULL); + vcp->att = att; + + bt_att_register_disconnect(att, vcp_disconnected, vcp, NULL); + + bt_vcp_attach(vcp, NULL); + + return vcp; + +} + +static uint8_t vcs_rel_vol_down(struct bt_vcs *vcs, struct bt_vcp *vcp, + struct iovec *iov) +{ + struct bt_vcp_db *vdb; + struct vol_state *vstate; + uint8_t *change_counter; + + DBG(vcp, "Volume Down"); + + vdb = vcp_get_vdb(vcp); + if (!vdb) { + DBG(vcp, "error: VDB not available"); + return 0; + } + + vstate = vdb_get_vstate(vdb); + if (!vstate) { + DBG(vcp, "error: VSTATE not available"); + return 0; + } + + change_counter = iov_pull_mem(iov, sizeof(*change_counter)); + if (!change_counter) + return 0; + + if (*change_counter != vstate->counter) { + DBG(vcp, "Change Counter Mismatch Volume not decremented!"); + return BT_ATT_ERROR_INVALID_CHANGE_COUNTER; + } + + vstate->vol_set = MAX((vstate->vol_set - VCP_STEP_SIZE), 0); + vstate->counter = -~vstate->counter; /*Increment Change Counter*/ + + gatt_db_attribute_notify(vdb->vcs->vs, (void *)vstate, + sizeof(struct vol_state), + bt_vcp_get_att(vcp)); + return 0; +} + +static uint8_t vcs_rel_vol_up(struct bt_vcs *vcs, struct bt_vcp *vcp, + struct iovec *iov) +{ + struct bt_vcp_db *vdb; + struct vol_state *vstate; + uint8_t *change_counter; + + DBG(vcp, "Volume Up"); + + vdb = vcp_get_vdb(vcp); + if (!vdb) { + DBG(vcp, "error: VDB not available"); + return 0; + } + + vstate = vdb_get_vstate(vdb); + if (!vstate) { + DBG(vcp, "error: VCP database not available"); + return 0; + } + + change_counter = iov_pull_mem(iov, sizeof(*change_counter)); + if (!change_counter) + return 0; + + if (*change_counter != vstate->counter) { + DBG(vcp, "Change Counter Mismatch Volume not decremented!"); + return BT_ATT_ERROR_INVALID_CHANGE_COUNTER; + } + + vstate->vol_set = MIN((vstate->vol_set + VCP_STEP_SIZE), 255); + vstate->counter = -~vstate->counter; /*Increment Change Counter*/ + + gatt_db_attribute_notify(vdb->vcs->vs, (void *)vstate, + sizeof(struct vol_state), + bt_vcp_get_att(vcp)); + return 0; +} + +static uint8_t vcs_unmute_rel_vol_down(struct bt_vcs *vcs, struct bt_vcp *vcp, + struct iovec *iov) +{ + struct bt_vcp_db *vdb; + struct vol_state *vstate; + uint8_t *change_counter; + + DBG(vcp, "Un Mute and Volume Down"); + + vdb = vcp_get_vdb(vcp); + if (!vdb) { + DBG(vcp, "error: VDB not available"); + return 0; + } + + vstate = vdb_get_vstate(vdb); + if (!vstate) { + DBG(vcp, "error: VCP database not available"); + return 0; + } + + change_counter = iov_pull_mem(iov, sizeof(*change_counter)); + if (!change_counter) + return 0; + + if (*change_counter != vstate->counter) { + DBG(vcp, "Change Counter Mismatch Volume not decremented!"); + return BT_ATT_ERROR_INVALID_CHANGE_COUNTER; + } + + vstate->mute = 0x00; + vstate->vol_set = MAX((vstate->vol_set - VCP_STEP_SIZE), 0); + vstate->counter = -~vstate->counter; /*Increment Change Counter*/ + + gatt_db_attribute_notify(vdb->vcs->vs, (void *)vstate, + sizeof(struct vol_state), + bt_vcp_get_att(vcp)); + return 0; +} + +static uint8_t vcs_unmute_rel_vol_up(struct bt_vcs *vcs, struct bt_vcp *vcp, + struct iovec *iov) +{ + struct bt_vcp_db *vdb; + struct vol_state *vstate; + uint8_t *change_counter; + + DBG(vcp, "UN Mute and Volume Up"); + + vdb = vcp_get_vdb(vcp); + if (!vdb) { + DBG(vcp, "error: VDB not available"); + return 0; + } + + vstate = vdb_get_vstate(vdb); + if (!vstate) { + DBG(vcp, "error: VSTATE not available"); + return 0; + } + + change_counter = iov_pull_mem(iov, sizeof(*change_counter)); + if (!change_counter) + return 0; + + if (*change_counter != vstate->counter) { + DBG(vcp, "Change Counter Mismatch Volume not decremented!"); + return BT_ATT_ERROR_INVALID_CHANGE_COUNTER; + } + + vstate->mute = 0x00; + vstate->vol_set = MIN((vstate->vol_set + VCP_STEP_SIZE), 255); + vstate->counter = -~vstate->counter; /*Increment Change Counter*/ + + gatt_db_attribute_notify(vdb->vcs->vs, (void *)vstate, + sizeof(struct vol_state), + bt_vcp_get_att(vcp)); + return 0; +} + +static uint8_t vcs_set_absolute_vol(struct bt_vcs *vcs, struct bt_vcp *vcp, + struct iovec *iov) +{ + struct bt_vcp_db *vdb; + struct vol_state *vstate; + struct bt_vcs_ab_vol *req; + + DBG(vcp, "Set Absolute Volume"); + + vdb = vcp_get_vdb(vcp); + if (!vdb) { + DBG(vcp, "error: VDB not available"); + return 0; + } + + vstate = vdb_get_vstate(vdb); + if (!vstate) { + DBG(vcp, "error: VSTATE not available"); + return 0; + } + + req = iov_pull_mem(iov, sizeof(*req)); + if (!req) + return 0; + + if (req->change_counter != vstate->counter) { + DBG(vcp, "Change Counter Mismatch Volume not decremented!"); + return BT_ATT_ERROR_INVALID_CHANGE_COUNTER; + } + + vstate->vol_set = req->vol_set; + vstate->counter = -~vstate->counter; /*Increment Change Counter*/ + + gatt_db_attribute_notify(vdb->vcs->vs, (void *)vstate, + sizeof(struct vol_state), + bt_vcp_get_att(vcp)); + return 0; +} + +static uint8_t vcs_unmute(struct bt_vcs *vcs, struct bt_vcp *vcp, + struct iovec *iov) +{ + struct bt_vcp_db *vdb; + struct vol_state *vstate; + uint8_t *change_counter; + + DBG(vcp, "Un Mute"); + + vdb = vcp_get_vdb(vcp); + if (!vdb) { + DBG(vcp, "error: VDB not available"); + return 0; + } + + vstate = vdb_get_vstate(vdb); + if (!vstate) { + DBG(vcp, "error: VSTATE not available"); + return 0; + } + + change_counter = iov_pull_mem(iov, sizeof(*change_counter)); + if (!change_counter) + return 0; + + if (*change_counter != vstate->counter) { + DBG(vcp, "Change Counter Mismatch Volume not decremented!"); + return BT_ATT_ERROR_INVALID_CHANGE_COUNTER; + } + + vstate->mute = 0x00; + vstate->counter = -~vstate->counter; /*Increment Change Counter*/ + + gatt_db_attribute_notify(vdb->vcs->vs, (void *)vstate, + sizeof(struct vol_state), + bt_vcp_get_att(vcp)); + return 0; +} + +static uint8_t vcs_mute(struct bt_vcs *vcs, struct bt_vcp *vcp, + struct iovec *iov) +{ + struct bt_vcp_db *vdb; + struct vol_state *vstate; + uint8_t *change_counter; + + DBG(vcp, "MUTE"); + + vdb = vcp_get_vdb(vcp); + if (!vdb) { + DBG(vcp, "error: VDB not available"); + return 0; + } + + vstate = vdb_get_vstate(vdb); + if (!vstate) { + DBG(vcp, "error: VSTATE not available"); + return 0; + } + + change_counter = iov_pull_mem(iov, sizeof(*change_counter)); + if (!change_counter) + return 0; + + if (*change_counter != vstate->counter) { + DBG(vcp, "Change Counter Mismatch Volume not decremented!"); + return BT_ATT_ERROR_INVALID_CHANGE_COUNTER; + } + + vstate->mute = 0x01; + vstate->counter = -~vstate->counter; /*Increment Change Counter*/ + + return 0; +} + +#define BT_VCS_REL_VOL_DOWN 0x00 +#define BT_VCS_REL_VOL_UP 0x01 +#define BT_VCS_UNMUTE_REL_VOL_DOWN 0x02 +#define BT_VCS_UNMUTE_REL_VOL_UP 0x03 +#define BT_VCS_SET_ABSOLUTE_VOL 0x04 +#define BT_VCS_UNMUTE 0x05 +#define BT_VCS_MUTE 0x06 + +#define VCS_OP(_str, _op, _size, _func) \ + { \ + .str = _str, \ + .op = _op, \ + .size = _size, \ + .func = _func, \ + } + +struct vcs_op_handler { + const char *str; + uint8_t op; + size_t size; + uint8_t (*func)(struct bt_vcs *vcs, struct bt_vcp *vcp, + struct iovec *iov); +} vcp_handlers[] = { + VCS_OP("Relative Volume Down", BT_VCS_REL_VOL_DOWN, + sizeof(uint8_t), vcs_rel_vol_down), + VCS_OP("Relative Volume Up", BT_VCS_REL_VOL_UP, + sizeof(uint8_t), vcs_rel_vol_up), + VCS_OP("Unmute - Relative Volume Down", BT_VCS_UNMUTE_REL_VOL_DOWN, + sizeof(uint8_t), vcs_unmute_rel_vol_down), + VCS_OP("Unmute - Relative Volume Up", BT_VCS_UNMUTE_REL_VOL_UP, + sizeof(uint8_t), vcs_unmute_rel_vol_up), + VCS_OP("Set Absolute Volume", BT_VCS_SET_ABSOLUTE_VOL, + sizeof(struct bt_vcs_ab_vol), vcs_set_absolute_vol), + VCS_OP("UnMute", BT_VCS_UNMUTE, + sizeof(uint8_t), vcs_unmute), + VCS_OP("Mute", BT_VCS_MUTE, + sizeof(uint8_t), vcs_mute), + {} +}; + +static void vcs_cp_write(struct gatt_db_attribute *attrib, + unsigned int id, uint16_t offset, + const uint8_t *value, size_t len, + uint8_t opcode, struct bt_att *att, + void *user_data) +{ + struct bt_vcs *vcs = user_data; + struct bt_vcp *vcp = vcp_get_session(att, vcs->vdb->db); + struct iovec iov = { + .iov_base = (void *) value, + .iov_len = len, + }; + uint8_t *vcp_op; + struct vcs_op_handler *handler; + uint8_t ret = BT_ATT_ERROR_REQUEST_NOT_SUPPORTED; + + DBG(vcp, "VCP Control Point Write"); + + if (offset) { + DBG(vcp, "invalid offset %d", offset); + ret = BT_ATT_ERROR_INVALID_OFFSET; + goto respond; + } + + if (len < sizeof(*vcp_op)) { + DBG(vcp, "invalid len %ld < %ld sizeof(*param)", len, + sizeof(*vcp_op)); + ret = BT_ATT_ERROR_INVALID_ATTRIBUTE_VALUE_LEN; + goto respond; + } + + vcp_op = iov_pull_mem(&iov, sizeof(*vcp_op)); + + for (handler = vcp_handlers; handler && handler->str; handler++) { + if (handler->op != *vcp_op) + continue; + + if (iov.iov_len < handler->size) { + DBG(vcp, "invalid len %ld < %ld handler->size", len, + handler->size); + ret = BT_ATT_ERROR_OPCODE_NOT_SUPPORTED; + goto respond; + } + + break; + } + + if (handler && handler->str) { + DBG(vcp, "%s", handler->str); + + ret = handler->func(vcs, vcp, &iov); + } else { + DBG(vcp, "Unknown opcode 0x%02x", *vcp_op); + ret = BT_ATT_ERROR_OPCODE_NOT_SUPPORTED; + } + +respond: + gatt_db_attribute_write_result(attrib, id, ret); +} + +static void vcs_state_read(struct gatt_db_attribute *attrib, + unsigned int id, uint16_t offset, + uint8_t opcode, struct bt_att *att, + void *user_data) +{ + struct bt_vcs *vcs = user_data; + struct iovec iov; + + iov.iov_base = vcs->vstate; + iov.iov_len = sizeof(*vcs->vstate); + + gatt_db_attribute_read_result(attrib, id, 0, iov.iov_base, + iov.iov_len); +} + +static void vcs_flag_read(struct gatt_db_attribute *attrib, + unsigned int id, uint16_t offset, + uint8_t opcode, struct bt_att *att, + void *user_data) +{ + struct bt_vcs *vcs = user_data; + struct iovec iov; + + iov.iov_base = &vcs->vol_flag; + iov.iov_len = sizeof(vcs->vol_flag); + + gatt_db_attribute_read_result(attrib, id, 0, iov.iov_base, + iov.iov_len); +} + +static struct bt_vcs *vcs_new(struct gatt_db *db) +{ + struct bt_vcs *vcs; + struct vol_state *vstate; + bt_uuid_t uuid; + + if (!db) + return NULL; + + vcs = new0(struct bt_vcs, 1); + + vstate = new0(struct vol_state, 1); + + vcs->vstate = vstate; + vcs->vol_flag = USERSET_VOLUME_SETTING; + + /* Populate DB with VCS attributes */ + bt_uuid16_create(&uuid, VCS_UUID); + vcs->service = gatt_db_add_service(db, &uuid, true, 9); + + bt_uuid16_create(&uuid, VOL_STATE_CHRC_UUID); + vcs->vs = gatt_db_service_add_characteristic(vcs->service, + &uuid, + BT_ATT_PERM_READ, + BT_GATT_CHRC_PROP_READ | + BT_GATT_CHRC_PROP_NOTIFY, + vcs_state_read, NULL, + vcs); + + vcs->vs_ccc = gatt_db_service_add_ccc(vcs->service, + BT_ATT_PERM_READ | BT_ATT_PERM_WRITE); + + bt_uuid16_create(&uuid, VOL_CP_CHRC_UUID); + vcs->vol_cp = gatt_db_service_add_characteristic(vcs->service, + &uuid, + BT_ATT_PERM_WRITE, + BT_GATT_CHRC_PROP_WRITE, + NULL, vcs_cp_write, + vcs); + + bt_uuid16_create(&uuid, VOL_FLAG_CHRC_UUID); + vcs->vf = gatt_db_service_add_characteristic(vcs->service, + &uuid, + BT_ATT_PERM_READ, + BT_GATT_CHRC_PROP_READ | + BT_GATT_CHRC_PROP_NOTIFY, + vcs_flag_read, NULL, + vcs); + + vcs->vf_ccc = gatt_db_service_add_ccc(vcs->service, + BT_ATT_PERM_READ | BT_ATT_PERM_WRITE); + + + gatt_db_service_set_active(vcs->service, true); + + return vcs; +} + +static struct bt_vcp_db *vcp_db_new(struct gatt_db *db) +{ + struct bt_vcp_db *vdb; + + if (!db) + return NULL; + + vdb = new0(struct bt_vcp_db, 1); + vdb->db = gatt_db_ref(db); + + if (!vcp_db) + vcp_db = queue_new(); + + vdb->vcs = vcs_new(db); + vdb->vcs->vdb = vdb; + + queue_push_tail(vcp_db, vdb); + + return vdb; +} + +static struct bt_vcp_db *vcp_get_db(struct gatt_db *db) +{ + struct bt_vcp_db *vdb; + + vdb = queue_find(vcp_db, vcp_db_match, db); + if (vdb) + return vdb; + + return vcp_db_new(db); +} + +void bt_vcp_add_db(struct gatt_db *db) +{ + vcp_db_new(db); +} + +bool bt_vcp_set_debug(struct bt_vcp *vcp, bt_vcp_debug_func_t func, + void *user_data, bt_vcp_destroy_func_t destroy) +{ + if (!vcp) + return false; + + if (vcp->debug_destroy) + vcp->debug_destroy(vcp->debug_data); + + vcp->debug_func = func; + vcp->debug_destroy = destroy; + vcp->debug_data = user_data; + + return true; +} + +unsigned int bt_vcp_register(bt_vcp_func_t attached, bt_vcp_func_t detached, + void *user_data) +{ + struct bt_vcp_cb *cb; + static unsigned int id; + + if (!attached && !detached) + return 0; + + if (!vcp_cbs) + vcp_cbs = queue_new(); + + cb = new0(struct bt_vcp_cb, 1); + cb->id = ++id ? id : ++id; + cb->attached = attached; + cb->detached = detached; + cb->user_data = user_data; + + queue_push_tail(vcp_cbs, cb); + + return cb->id; +} + +static bool match_id(const void *data, const void *match_data) +{ + const struct bt_vcp_cb *cb = data; + unsigned int id = PTR_TO_UINT(match_data); + + return (cb->id == id); +} + +bool bt_vcp_unregister(unsigned int id) +{ + struct bt_vcp_cb *cb; + + cb = queue_remove_if(vcp_cbs, match_id, UINT_TO_PTR(id)); + if (!cb) + return false; + + free(cb); + + return true; +} + +struct bt_vcp *bt_vcp_new(struct gatt_db *ldb, struct gatt_db *rdb) +{ + struct bt_vcp *vcp; + struct bt_vcp_db *vdb; + + if (!ldb) + return NULL; + + vdb = vcp_get_db(ldb); + if (!vdb) + return NULL; + + vcp = new0(struct bt_vcp, 1); + vcp->ldb = vdb; + vcp->pending = queue_new(); + + if (!rdb) + goto done; + + vdb = new0(struct bt_vcp_db, 1); + vdb->db = gatt_db_ref(rdb); + + vcp->rdb = vdb; + +done: + bt_vcp_ref(vcp); + + return vcp; +} + +static void vcp_vstate_notify(struct bt_vcp *vcp, uint16_t value_handle, + const uint8_t *value, uint16_t length, + void *user_data) +{ + struct vol_state vstate; + + memcpy(&vstate, value, sizeof(struct vol_state)); + + DBG(vcp, "Vol Settings 0x%x", vstate.vol_set); + DBG(vcp, "Mute Status 0x%x", vstate.mute); + DBG(vcp, "Vol Counter 0x%x", vstate.counter); +} + +static void vcp_vflag_notify(struct bt_vcp *vcp, uint16_t value_handle, + const uint8_t *value, uint16_t length, + void *user_data) +{ + uint8_t vflag; + + memcpy(&vflag, value, sizeof(vflag)); + + DBG(vcp, "Vol Flag 0x%x", vflag); +} + +static void read_vol_flag(struct bt_vcp *vcp, bool success, uint8_t att_ecode, + const uint8_t *value, uint16_t length, + void *user_data) +{ + uint8_t *vol_flag; + struct iovec iov = { + .iov_base = (void *) value, + .iov_len = length, + }; + + if (!success) { + DBG(vcp, "Unable to read Vol Flag: error 0x%02x", att_ecode); + return; + } + + vol_flag = iov_pull_mem(&iov, sizeof(*vol_flag)); + if (!vol_flag) { + DBG(vcp, "Unable to get Vol Flag"); + return; + } + + DBG(vcp, "Vol Flag:%x", *vol_flag); +} + +static void read_vol_state(struct bt_vcp *vcp, bool success, uint8_t att_ecode, + const uint8_t *value, uint16_t length, + void *user_data) +{ + struct vol_state *vs; + struct iovec iov = { + .iov_base = (void *) value, + .iov_len = length, + }; + + if (!success) { + DBG(vcp, "Unable to read Vol State: error 0x%02x", att_ecode); + return; + } + + vs = iov_pull_mem(&iov, sizeof(*vs)); + if (!vs) { + DBG(vcp, "Unable to get Vol State"); + return; + } + + DBG(vcp, "Vol Set:%x", vs->vol_set); + DBG(vcp, "Vol Mute:%x", vs->mute); + DBG(vcp, "Vol Counter:%x", vs->counter); +} + +static void vcp_pending_destroy(void *data) +{ + struct bt_vcp_pending *pending = data; + struct bt_vcp *vcp = pending->vcp; + + if (queue_remove_if(vcp->pending, NULL, pending)) + free(pending); +} + +static void vcp_pending_complete(bool success, uint8_t att_ecode, + const uint8_t *value, uint16_t length, + void *user_data) +{ + struct bt_vcp_pending *pending = user_data; + + if (pending->func) + pending->func(pending->vcp, success, att_ecode, value, length, + pending->user_data); +} + +static void vcp_read_value(struct bt_vcp *vcp, uint16_t value_handle, + vcp_func_t func, void *user_data) +{ + struct bt_vcp_pending *pending; + + pending = new0(struct bt_vcp_pending, 1); + pending->vcp = vcp; + pending->func = func; + pending->user_data = user_data; + + pending->id = bt_gatt_client_read_value(vcp->client, value_handle, + vcp_pending_complete, pending, + vcp_pending_destroy); + if (!pending->id) { + DBG(vcp, "Unable to send Read request"); + free(pending); + return; + } + + queue_push_tail(vcp->pending, pending); +} + +static void vcp_register(uint16_t att_ecode, void *user_data) +{ + struct bt_vcp_notify *notify = user_data; + + if (att_ecode) + DBG(notify->vcp, "VCP register failed: 0x%04x", att_ecode); +} + +static void vcp_notify(uint16_t value_handle, const uint8_t *value, + uint16_t length, void *user_data) +{ + struct bt_vcp_notify *notify = user_data; + + if (notify->func) + notify->func(notify->vcp, value_handle, value, length, + notify->user_data); +} + +static void vcp_notify_destroy(void *data) +{ + struct bt_vcp_notify *notify = data; + struct bt_vcp *vcp = notify->vcp; + + if (queue_remove_if(vcp->notify, NULL, notify)) + free(notify); +} + +static unsigned int vcp_register_notify(struct bt_vcp *vcp, + uint16_t value_handle, + vcp_notify_t func, + void *user_data) +{ + struct bt_vcp_notify *notify; + + notify = new0(struct bt_vcp_notify, 1); + notify->vcp = vcp; + notify->func = func; + notify->user_data = user_data; + + notify->id = bt_gatt_client_register_notify(vcp->client, + value_handle, vcp_register, + vcp_notify, notify, + vcp_notify_destroy); + if (!notify->id) { + DBG(vcp, "Unable to register for notifications"); + free(notify); + return 0; + } + + queue_push_tail(vcp->notify, notify); + + return notify->id; +} + +static void foreach_vcs_char(struct gatt_db_attribute *attr, void *user_data) +{ + struct bt_vcp *vcp = user_data; + uint16_t value_handle; + bt_uuid_t uuid, uuid_vstate, uuid_cp, uuid_vflag; + struct bt_vcs *vcs; + + if (!gatt_db_attribute_get_char_data(attr, NULL, &value_handle, + NULL, NULL, &uuid)) + return; + + bt_uuid16_create(&uuid_vstate, VOL_STATE_CHRC_UUID); + bt_uuid16_create(&uuid_cp, VOL_CP_CHRC_UUID); + bt_uuid16_create(&uuid_vflag, VOL_FLAG_CHRC_UUID); + + if (!bt_uuid_cmp(&uuid, &uuid_vstate)) { + DBG(vcp, "VCS Vol state found: handle 0x%04x", value_handle); + + vcs = vcp_get_vcs(vcp); + if (!vcs || vcs->vs) + return; + + vcs->vs = attr; + + vcp_read_value(vcp, value_handle, read_vol_state, vcp); + + vcp->vstate_id = vcp_register_notify(vcp, value_handle, + vcp_vstate_notify, NULL); + + return; + } + + if (!bt_uuid_cmp(&uuid, &uuid_cp)) { + DBG(vcp, "VCS Volume CP found: handle 0x%04x", value_handle); + + vcs = vcp_get_vcs(vcp); + if (!vcs || vcs->vol_cp) + return; + + vcs->vol_cp = attr; + + return; + } + + if (!bt_uuid_cmp(&uuid, &uuid_vflag)) { + DBG(vcp, "VCS Vol Flag found: handle 0x%04x", value_handle); + + vcs = vcp_get_vcs(vcp); + if (!vcs || vcs->vf) + return; + + vcs->vf = attr; + + vcp_read_value(vcp, value_handle, read_vol_flag, vcp); + vcp->vflag_id = vcp_register_notify(vcp, value_handle, + vcp_vflag_notify, NULL); + + } +} + +static void foreach_vcs_service(struct gatt_db_attribute *attr, + void *user_data) +{ + struct bt_vcp *vcp = user_data; + struct bt_vcs *vcs = vcp_get_vcs(vcp); + + vcs->service = attr; + + gatt_db_service_set_claimed(attr, true); + + gatt_db_service_foreach_char(attr, foreach_vcs_char, vcp); +} + +bool bt_vcp_attach(struct bt_vcp *vcp, struct bt_gatt_client *client) +{ + bt_uuid_t uuid; + + if (!sessions) + sessions = queue_new(); + + queue_push_tail(sessions, vcp); + + if (!client) + return true; + + if (vcp->client) + return false; + + vcp->client = bt_gatt_client_clone(client); + if (!vcp->client) + return false; + + bt_uuid16_create(&uuid, VCS_UUID); + gatt_db_foreach_service(vcp->ldb->db, &uuid, foreach_vcs_service, vcp); + + return true; +} + diff -pruN 5.65-1/src/shared/vcp.h 5.66-1/src/shared/vcp.h --- 5.65-1/src/shared/vcp.h 1970-01-01 00:00:00.000000000 +0000 +++ 5.66-1/src/shared/vcp.h 2022-11-10 20:22:09.000000000 +0000 @@ -0,0 +1,59 @@ +/* SPDX-License-Identifier: LGPL-2.1-or-later */ +/* + * + * BlueZ - Bluetooth protocol stack for Linux + * + * Copyright (C) 2020 Intel Corporation. All rights reserved. + * + */ + +#include +#include + +#include "src/shared/io.h" + +#define BT_VCP_RENDERER 0x01 +#define BT_VCP_CONTROLLER 0x02 + +#define BT_VCP_RELATIVE_VOL_DOWN 0x00 +#define BT_VCP_RELATIVE_VOL_UP 0x01 +#define BT_VCP_UNMUTE_RELATIVE_VOL_DOWN 0x02 +#define BT_VCP_UNMUTE_RELATIVE_VOL_UP 0x03 +#define BT_VCP_SET_ABOSULTE_VOL 0x04 +#define BT_VCP_UNMUTE 0x05 +#define BT_VCP_MUTE 0x06 + +#ifndef MAX +#define MAX(a, b) ((a) > (b) ? (a) : (b)) +#endif + +#ifndef MIN +#define MIN(a, b) ((a) < (b) ? (a) : (b)) +#endif + +struct bt_vcp; + +typedef void (*bt_vcp_destroy_func_t)(void *user_data); +typedef void (*bt_vcp_debug_func_t)(const char *str, void *user_data); +typedef void (*bt_vcp_func_t)(struct bt_vcp *vcp, void *user_data); + +struct bt_vcp *bt_vcp_ref(struct bt_vcp *vcp); +void bt_vcp_unref(struct bt_vcp *vcp); + +void bt_vcp_add_db(struct gatt_db *db); + +bool bt_vcp_attach(struct bt_vcp *vcp, struct bt_gatt_client *client); +void bt_vcp_detach(struct bt_vcp *vcp); + +bool bt_vcp_set_debug(struct bt_vcp *vcp, bt_vcp_debug_func_t cb, + void *user_data, bt_vcp_destroy_func_t destroy); + +struct bt_att *bt_vcp_get_att(struct bt_vcp *vcp); + +bool bt_vcp_set_user_data(struct bt_vcp *vcp, void *user_data); + +/* Session related function */ +unsigned int bt_vcp_register(bt_vcp_func_t added, bt_vcp_func_t removed, + void *user_data); +bool bt_vcp_unregister(unsigned int id); +struct bt_vcp *bt_vcp_new(struct gatt_db *ldb, struct gatt_db *rdb); diff -pruN 5.65-1/test/simple-endpoint 5.66-1/test/simple-endpoint --- 5.65-1/test/simple-endpoint 2022-07-24 20:59:52.000000000 +0000 +++ 5.66-1/test/simple-endpoint 2022-11-10 20:22:09.000000000 +0000 @@ -18,6 +18,8 @@ A2DP_SINK_UUID = "0000110B-0000-1000-800 HFP_AG_UUID = "0000111F-0000-1000-8000-00805F9B34FB" HFP_HF_UUID = "0000111E-0000-1000-8000-00805F9B34FB" HSP_AG_UUID = "00001112-0000-1000-8000-00805F9B34FB" +PAC_SINK_UUID = "00008f96-0000-1000-8000-00805F9B34FB" +PAC_SOURCE_UUID = "00008f98-0000-1000-8000-00805F9B34FB" SBC_CODEC = dbus.Byte(0x00) #Channel Modes: Mono DualChannel Stereo JointStereo @@ -41,6 +43,11 @@ MP3_CAPABILITIES = dbus.Array([dbus.Byte # JointStereo 44.1Khz Layer: 3 Bit Rate: VBR Format: RFC-2250 MP3_CONFIGURATION = dbus.Array([dbus.Byte(0x21), dbus.Byte(0x02), dbus.Byte(0x00), dbus.Byte(0x80)]) +LC3_CODEC = dbus.Byte(0x06) +#Bits per sample: 16 +#Bit Rate: 96kbps +LC3_CAPABILITIES = dbus.Array([dbus.Byte(16), dbus.Byte(96)]) + PCM_CODEC = dbus.Byte(0x00) PCM_CONFIGURATION = dbus.Array([], signature="ay") @@ -131,6 +138,16 @@ if __name__ == '__main__': "Codec" : CVSD_CODEC, "Capabilities" : PCM_CONFIGURATION }) endpoint.default_configuration(dbus.Array([])) + if sys.argv[2] == "lc3sink": + properties = dbus.Dictionary({ "UUID" : PAC_SINK_UUID, + "Codec" : LC3_CODEC, + "Capabilities" : + LC3_CAPABILITIES }) + if sys.argv[2] == "lc3source": + properties = dbus.Dictionary({ "UUID" : PAC_SOURCE_UUID, + "Codec" : LC3_CODEC, + "Capabilities" : + LC3_CAPABILITIES }) print(properties) diff -pruN 5.65-1/tools/bluetooth-player.c 5.66-1/tools/bluetooth-player.c --- 5.65-1/tools/bluetooth-player.c 2022-07-24 20:59:52.000000000 +0000 +++ 5.66-1/tools/bluetooth-player.c 2022-11-10 20:22:09.000000000 +0000 @@ -26,7 +26,6 @@ #include #include "gdbus/gdbus.h" - #include "lib/bluetooth.h" #include "lib/uuid.h" diff -pruN 5.65-1/tools/btmgmt.c 5.66-1/tools/btmgmt.c --- 5.65-1/tools/btmgmt.c 2022-07-24 20:59:52.000000000 +0000 +++ 5.66-1/tools/btmgmt.c 2022-11-10 20:22:09.000000000 +0000 @@ -2183,7 +2183,7 @@ static void get_flags_rsp(uint8_t status if (status != 0) { error("Get device flags failed with status 0x%02x (%s)", status, mgmt_errstr(status)); - bt_shell_noninteractive_quit(EXIT_FAILURE); + return bt_shell_noninteractive_quit(EXIT_FAILURE); } print("Supported Flags: 0x%08x", rp->supported_flags); diff -pruN 5.65-1/tools/hciattach_bcm43xx.c 5.66-1/tools/hciattach_bcm43xx.c --- 5.65-1/tools/hciattach_bcm43xx.c 2022-07-24 20:59:52.000000000 +0000 +++ 5.66-1/tools/hciattach_bcm43xx.c 2022-11-10 20:22:09.000000000 +0000 @@ -30,10 +30,6 @@ #include "hciattach.h" -#ifndef FIRMWARE_DIR -#define FIRMWARE_DIR "/etc/firmware" -#endif - #define FW_EXT ".hcd" #define BCM43XX_CLOCK_48 1 diff -pruN 5.65-1/tools/hciattach.h 5.66-1/tools/hciattach.h --- 5.65-1/tools/hciattach.h 2022-07-24 20:59:52.000000000 +0000 +++ 5.66-1/tools/hciattach.h 2022-11-10 20:22:09.000000000 +0000 @@ -40,6 +40,10 @@ #define HCI_UART_EXT_CONFIG 4 #define HCI_UART_VND_DETECT 5 +#ifndef FIRMWARE_DIR +#define FIRMWARE_DIR "/etc/firmware" +#endif + int read_hci_event(int fd, unsigned char *buf, int size); int set_speed(int fd, struct termios *ti, int speed); int uart_speed(int speed); diff -pruN 5.65-1/tools/hciattach_qualcomm.c 5.66-1/tools/hciattach_qualcomm.c --- 5.65-1/tools/hciattach_qualcomm.c 2022-07-24 20:59:52.000000000 +0000 +++ 5.66-1/tools/hciattach_qualcomm.c 2022-11-10 20:22:09.000000000 +0000 @@ -175,10 +175,8 @@ int qualcomm_init(int fd, int speed, str } /* Read reply. */ - if (read_hci_event(fd, resp, 100) < 0) { - perror("Failed to read init response"); - return -1; - } + n = read_hci_event(fd, resp, 100); + FAILIF(n < 0, "Failed to read init response"); /* Wait for command complete event for our Opcode */ } while (resp[4] != cmd[1] && resp[5] != cmd[2]); @@ -215,23 +213,20 @@ int qualcomm_init(int fd, int speed, str } /* Read reply. */ - if ((n = read_hci_event(fd, resp, 100)) < 0) { - perror("Failed to read vendor init response"); - return -1; - } + n = read_hci_event(fd, resp, 100); + FAILIF(n < 0, "Failed to read vendor init response"); } while (resp[3] != 0 && resp[4] != 2); - snprintf(fw, sizeof(fw), "/etc/firmware/%c%c%c%c%c%c_%c%c%c%c.bin", + snprintf(fw, sizeof(fw), "%s/%c%c%c%c%c%c_%c%c%c%c.bin", + FIRMWARE_DIR, resp[18], resp[19], resp[20], resp[21], resp[22], resp[23], resp[32], resp[33], resp[34], resp[35]); /* Wait for command complete event for our Opcode */ - if (read_hci_event(fd, resp, 100) < 0) { - perror("Failed to read init response"); - return -1; - } + n = read_hci_event(fd, resp, 100); + FAILIF(n < 0, "Failed to read init response"); qualcomm_load_firmware(fd, fw, bdaddr); @@ -249,10 +244,8 @@ int qualcomm_init(int fd, int speed, str } /* Read reply. */ - if ((n = read_hci_event(fd, resp, 100)) < 0) { - perror("Failed to read reset response"); - return -1; - } + n = read_hci_event(fd, resp, 100); + FAILIF(n < 0, "Failed to read reset response"); } while (resp[4] != cmd[1] && resp[5] != cmd[2]); diff -pruN 5.65-1/tools/hciattach_tialt.c 5.66-1/tools/hciattach_tialt.c --- 5.65-1/tools/hciattach_tialt.c 2022-07-24 20:59:52.000000000 +0000 +++ 5.66-1/tools/hciattach_tialt.c 2022-11-10 20:22:09.000000000 +0000 @@ -221,7 +221,8 @@ int texasalt_init(int fd, int speed, str ((brf_chip > 7) ? "unknown" : c_brf_chip[brf_chip]), brf_chip); - sprintf(fw, "/etc/firmware/%s.bin", + sprintf(fw, "%s/%s.bin", + FIRMWARE_DIR, (brf_chip > 7) ? "unknown" : c_brf_chip[brf_chip]); texas_load_firmware(fd, fw); diff -pruN 5.65-1/tools/ioctl-tester.c 5.66-1/tools/ioctl-tester.c --- 5.65-1/tools/ioctl-tester.c 1970-01-01 00:00:00.000000000 +0000 +++ 5.66-1/tools/ioctl-tester.c 2022-11-10 20:22:09.000000000 +0000 @@ -0,0 +1,960 @@ +// SPDX-License-Identifier: LGPL-2.1-or-later +/* + * + * BlueZ - Bluetooth protocol stack for Linux + * + * Copyright (C) 2022 Intel Corporation. + * + */ + +#ifdef HAVE_CONFIG_H +#include +#endif + +#include +#include +#include +#include +#include + +#include +#include + +#include + +#include "lib/bluetooth.h" +#include "lib/mgmt.h" +#include "lib/hci.h" +#include "lib/hci_lib.h" + +#include "monitor/bt.h" +#include "emulator/bthost.h" +#include "emulator/hciemu.h" + +#include "src/shared/tester.h" +#include "src/shared/mgmt.h" +#include "src/shared/util.h" + +struct test_data { + const void *test_data; + int sock_fd; + struct hciemu *hciemu; + enum hciemu_type hciemu_type; + uint8_t client_num; + uint16_t hci_dev_id; + + struct mgmt *mgmt; + uint16_t mgmt_index; + struct mgmt *mgmt_alt; + unsigned int mgmt_alt_ev_id; + + uint16_t handle; + uint16_t acl_handle; + GIOChannel *io; + unsigned int io_id[2]; + int step; + bool reconnect; + + int unmet_conditions; +}; + +struct ioctl_data { + uint32_t cmd; + const uint32_t opt; + const void *param; + int (*cmd_param_func)(void *param, uint32_t *length); + int expected_ioctl_err; + const void *block_bdaddr; + const void *expected_data; + int (*expect_data_check_func)(const void *param, uint32_t length); +}; + +static void print_debug(const char *str, void *user_data) +{ + const char *prefix = user_data; + + tester_print("%s%s", prefix, str); +} + +static void test_add_condition(struct test_data *data) +{ + data->unmet_conditions++; + + tester_print("Test condition added, total %d", data->unmet_conditions); +} + +static void test_condition_complete(struct test_data *data) +{ + data->unmet_conditions--; + + tester_print("Test condition complete, %d left", + data->unmet_conditions); + + if (data->unmet_conditions > 0) + return; + + tester_test_passed(); +} + +static int update_hci_dev_id(struct test_data *data) +{ + struct hci_dev_list_req *dl; + struct hci_dev_req *dr; + int ret = 0; + + dl = malloc(HCI_MAX_DEV * sizeof(*dr) + sizeof(uint16_t)); + if (!dl) + return -ENOMEM; + + dl->dev_num = HCI_MAX_DEV; + dr = dl->dev_req; + + if (ioctl(data->sock_fd, HCIGETDEVLIST, (void *) dl) < 0) { + ret = -EIO; + goto exit; + } + + if (dl->dev_num != 1) { + tester_warn("dev num mismatch returned %d:expected 1", + dl->dev_num); + ret = -ENODEV; + goto exit; + } + + data->hci_dev_id = dr->dev_id; + tester_print("HCI device id: %d", data->hci_dev_id); + +exit: + free(dl); + return ret; +} + +static void read_info_callback(uint8_t status, uint16_t length, + const void *param, void *user_data) +{ + struct test_data *data = tester_get_data(); + const struct mgmt_rp_read_info *rp = param; + char addr[18]; + uint16_t manufacturer; + uint32_t supported_settings, current_settings; + + tester_print("Read Info callback"); + tester_print(" Status: 0x%02x", status); + + if (status || !param) { + tester_pre_setup_failed(); + return; + } + + ba2str(&rp->bdaddr, addr); + manufacturer = btohs(rp->manufacturer); + supported_settings = btohl(rp->supported_settings); + current_settings = btohl(rp->current_settings); + + tester_print(" Address: %s", addr); + tester_print(" Version: 0x%02x", rp->version); + tester_print(" Manufacturer: 0x%04x", manufacturer); + tester_print(" Supported settings: 0x%08x", supported_settings); + tester_print(" Current settings: 0x%08x", current_settings); + tester_print(" Class: 0x%02x%02x%02x", + rp->dev_class[2], rp->dev_class[1], rp->dev_class[0]); + tester_print(" Name: %s", rp->name); + tester_print(" Short name: %s", rp->short_name); + + if (strcmp(hciemu_get_address(data->hciemu), addr)) { + tester_pre_setup_failed(); + return; + } + + tester_pre_setup_complete(); +} + +static void index_added_callback(uint16_t index, uint16_t length, + const void *param, void *user_data) +{ + struct test_data *data = tester_get_data(); + + tester_print("Index Added callback"); + tester_print(" Index: 0x%04x", index); + + data->mgmt_index = index; + + mgmt_send(data->mgmt, MGMT_OP_READ_INFO, data->mgmt_index, 0, NULL, + read_info_callback, NULL, NULL); +} + +static void index_removed_callback(uint16_t index, uint16_t length, + const void *param, void *user_data) +{ + struct test_data *data = tester_get_data(); + + tester_print("Index Removed callback"); + tester_print(" Index: 0x%04x", index); + + if (index != data->mgmt_index) + return; + + mgmt_unregister_index(data->mgmt, data->mgmt_index); + mgmt_unregister_index(data->mgmt_alt, data->mgmt_index); + + mgmt_unref(data->mgmt); + data->mgmt = NULL; + + mgmt_unref(data->mgmt_alt); + data->mgmt_alt = NULL; + + tester_post_teardown_complete(); +} + +static void read_index_list_callback(uint8_t status, uint16_t length, + const void *param, void *user_data) +{ + struct test_data *data = tester_get_data(); + + tester_print("Read Index List callback"); + tester_print(" Status: 0x%02x", status); + + if (status || !param) { + tester_pre_setup_failed(); + return; + } + + mgmt_register(data->mgmt, MGMT_EV_INDEX_ADDED, MGMT_INDEX_NONE, + index_added_callback, NULL, NULL); + + mgmt_register(data->mgmt, MGMT_EV_INDEX_REMOVED, MGMT_INDEX_NONE, + index_removed_callback, NULL, NULL); + + data->hciemu = hciemu_new(data->hciemu_type); + if (!data->hciemu) { + tester_warn("Failed to setup HCI emulation"); + tester_pre_setup_failed(); + } + + if (tester_use_debug()) + hciemu_set_debug(data->hciemu, print_debug, "hciemu: ", NULL); + + tester_print("New hciemu instance created"); + + data->sock_fd = hci_open_dev(0); + if (data->sock_fd < 0) { + tester_warn("Failed to open socket for ioctl"); + tester_pre_setup_failed(); + return; + } + + update_hci_dev_id(data); +} + +static void test_pre_setup(const void *test_data) +{ + struct test_data *data = tester_get_data(); + + data->mgmt = mgmt_new_default(); + if (!data->mgmt) { + tester_warn("Failed to setup mgmt interface"); + tester_pre_setup_failed(); + return; + } + + data->mgmt_alt = mgmt_new_default(); + if (!data->mgmt_alt) { + tester_warn("Failed to setup alternate management interface"); + tester_pre_setup_failed(); + + mgmt_unref(data->mgmt); + data->mgmt = NULL; + return; + } + + + if (tester_use_debug()) { + mgmt_set_debug(data->mgmt, print_debug, "mgmt: ", NULL); + mgmt_set_debug(data->mgmt_alt, print_debug, "mgmt-alt: ", NULL); + } + + mgmt_send(data->mgmt, MGMT_OP_READ_INDEX_LIST, MGMT_INDEX_NONE, 0, NULL, + read_index_list_callback, NULL, NULL); +} + +static void test_post_teardown(const void *test_data) +{ + struct test_data *data = tester_get_data(); + + if (data->sock_fd >= 0) { + tester_print("Socket closed"); + hci_close_dev(data->sock_fd); + } + + hciemu_unref(data->hciemu); + data->hciemu = NULL; +} + +static void test_data_free(void *test_data) +{ + struct test_data *data = test_data; + + // TODO: free any data allocated during pre-setup + + free(data); +} + +#define test_ioctl_full(name, data, setup, func, num) \ + do { \ + struct test_data *user; \ + user = new0(struct test_data, 1); \ + if (!user) \ + break; \ + user->hciemu_type = HCIEMU_TYPE_BREDRLE; \ + user->test_data = data; \ + user->client_num = num; \ + tester_add_full(name, data, \ + test_pre_setup, setup, func, NULL, \ + test_post_teardown, 2, user, test_data_free); \ + } while (0) + +#define test_ioctl(name, data, setup, func) \ + test_ioctl_full(name, data, setup, func, 1) + +static void setup_powered_callback(uint8_t status, uint16_t length, + const void *param, void *user_data) +{ + if (status != MGMT_STATUS_SUCCESS) { + tester_setup_failed(); + return; + } + + tester_print("Controller powered on"); + + tester_setup_complete(); +} + +static void setup_powered(const void *test_data) +{ + struct test_data *data = tester_get_data(); + unsigned char param[] = { 0x01 }; + + mgmt_send(data->mgmt, MGMT_OP_SET_BONDABLE, data->mgmt_index, + sizeof(param), param, NULL, NULL, NULL); + + mgmt_send(data->mgmt, MGMT_OP_SET_CONNECTABLE, data->mgmt_index, + sizeof(param), param, NULL, NULL, NULL); + + mgmt_send(data->mgmt, MGMT_OP_SET_SSP, data->mgmt_index, + sizeof(param), param, NULL, NULL, NULL); + + mgmt_send(data->mgmt, MGMT_OP_SET_POWERED, data->mgmt_index, + sizeof(param), param, + setup_powered_callback, NULL, NULL); +} + +static void setup_add_block_bdaddr(const void *test_data) +{ + struct test_data *data = tester_get_data(); + const struct ioctl_data *ioctl_data = data->test_data; + + if (!ioctl_data->block_bdaddr) { + tester_warn("Invalid test data: block bdaddr"); + tester_setup_failed(); + return; + } + + if (ioctl(data->sock_fd, HCIBLOCKADDR, ioctl_data->block_bdaddr) < 0) { + tester_warn("Failed to add block bdaddr"); + tester_setup_failed(); + return; + } + + tester_print("Added block BDADDR"); + + tester_setup_complete(); +} + +static int conn_list_empty_check_func(const void *param, uint32_t length) +{ + struct test_data *data = tester_get_data(); + const struct ioctl_data *ioctl_data = data->test_data; + const struct hci_conn_list_req *cl_input = ioctl_data->expected_data; + const struct hci_conn_list_req *cl = param; + + if (cl->conn_num != cl_input->conn_num) + return -1; + + return 0; +} + +static int conn_info_cmd_param_func(void *param, uint32_t *length) +{ + struct test_data *data = tester_get_data(); + const struct ioctl_data *ioctl_data = data->test_data; + const struct hci_conn_info_req *cr_input = ioctl_data->param; + struct hci_conn_info_req *cr = param; + + memcpy(&cr->bdaddr, &cr_input->bdaddr, sizeof(bdaddr_t)); + cr->type = cr_input->type; + + return 0; +} + +static int auth_info_cmd_param_func(void *param, uint32_t *length) +{ + struct test_data *data = tester_get_data(); + const struct ioctl_data *ioctl_data = data->test_data; + const struct hci_auth_info_req *ar_input = ioctl_data->param; + struct hci_auth_info_req *ar = param; + + memcpy(&ar->bdaddr, &ar_input->bdaddr, sizeof(bdaddr_t)); + if (ar_input->type) + ar->type = ar_input->type; + + return 0; +} + +static const struct ioctl_data dev_down = { + .cmd = HCIDEVDOWN, +}; + +static const struct hci_dev_list_req dev_list_1 = { + .dev_num = 0x01, + .dev_req = {{ + .dev_id = 0x00, + .dev_opt = 0x04, + }}, +}; + +static const struct ioctl_data dev_list = { + .cmd = HCIGETDEVLIST, + .expected_data = (void *)&dev_list_1, +}; + +static const struct hci_dev_list_req dev_list_invalid_1_param = { + .dev_num = 0x00, +}; + +static const struct ioctl_data dev_list_invalid_1 = { + .cmd = HCIGETDEVLIST, + .param = (void *)&dev_list_invalid_1_param, + .expected_ioctl_err = EINVAL, +}; + +static const struct ioctl_data dev_info = { + .cmd = HCIGETDEVINFO, +}; + +static const struct ioctl_data reset_stat = { + .cmd = HCIDEVRESTAT, +}; + +static const struct ioctl_data set_link_mode_master = { + .cmd = HCISETLINKMODE, + .opt = HCI_LM_MASTER, +}; + +static const struct ioctl_data set_link_mode_accept = { + .cmd = HCISETLINKMODE, + .opt = HCI_LM_ACCEPT, +}; + +static const struct ioctl_data set_pkt_type_dm = { + .cmd = HCISETPTYPE, + .opt = HCI_DM1 | HCI_DM3 | HCI_DM5, +}; + +static const struct ioctl_data set_pkt_type_dh = { + .cmd = HCISETPTYPE, + .opt = HCI_DH1 | HCI_DH3 | HCI_DH5, +}; + +static const struct ioctl_data set_pkt_type_hv = { + .cmd = HCISETPTYPE, + .opt = HCI_HV1 | HCI_HV2 | HCI_HV3, +}; + +static const struct ioctl_data set_pkt_type_2dh = { + .cmd = HCISETPTYPE, + .opt = HCI_2DH1 | HCI_2DH3 | HCI_2DH5, +}; + +static const struct ioctl_data set_pkt_type_3dh = { + .cmd = HCISETPTYPE, + .opt = HCI_3DH1 | HCI_3DH3 | HCI_3DH5, +}; + +static const struct ioctl_data set_pkt_type_all = { + .cmd = HCISETPTYPE, + .opt = HCI_DM1 | HCI_DM3 | HCI_DM5 | HCI_DH1 | HCI_DH3 | HCI_DH5 | + HCI_HV1 | HCI_HV2 | HCI_HV3 | HCI_2DH1 | HCI_2DH3 | HCI_2DH5 | + HCI_3DH1 | HCI_3DH3 | HCI_3DH5, +}; + +static const struct ioctl_data set_acl_mtu_1 = { + .cmd = HCISETACLMTU, + .opt = 0x1 | (0x3FE << 16), +}; + +static const struct ioctl_data set_acl_mtu_2 = { + .cmd = HCISETACLMTU, + .opt = 0x4 | (0x63 << 16), +}; + +static const struct ioctl_data set_sco_mtu_1 = { + .cmd = HCISETSCOMTU, + .opt = 0x1 | (0x3FE << 16), +}; + +static const struct ioctl_data set_sco_mtu_2 = { + .cmd = HCISETSCOMTU, + .opt = 0x4 | (0x63 << 16), +}; + +static const uint8_t bdaddr1[] = { + 0x11, 0x22, 0x33, 0x44, 0x55, 0x66 +}; + +static const struct ioctl_data block_bdaddr_success = { + .cmd = HCIBLOCKADDR, + .param = bdaddr1, +}; + +static const struct ioctl_data block_bdaddr_fail = { + .cmd = HCIBLOCKADDR, + .param = bdaddr1, + .expected_ioctl_err = EEXIST, + .block_bdaddr = bdaddr1, +}; + +static const struct ioctl_data unblock_bdaddr_success = { + .cmd = HCIUNBLOCKADDR, + .param = bdaddr1, + .block_bdaddr = bdaddr1, +}; + +static const struct ioctl_data unblock_bdaddr_fail = { + .cmd = HCIUNBLOCKADDR, + .param = bdaddr1, + .expected_ioctl_err = ENOENT, +}; + +static const struct hci_conn_list_req conn_list_empty = { + .dev_id = 0x00, + .conn_num = 0x00, +}; + +static const struct ioctl_data conn_list_no_conn = { + .cmd = HCIGETCONNLIST, + .expected_data = (void *)&conn_list_empty, + .expect_data_check_func = conn_list_empty_check_func, +}; + +static const struct hci_conn_list_req conn_list_req_1 = { + .dev_id = 0x00, + .conn_num = 0x01, + .conn_info = {{ + .handle = 0x002a, + .bdaddr = {{ 0x00, 0x00, 0x01, 0x01, 0xaa, 0x00 }}, + .type = 0x01, + .out = 0x00, + .state = 0x0001, + .link_mode = 0x00000000, + }}, +}; + +static const struct ioctl_data conn_list = { + .cmd = HCIGETCONNLIST, + .expected_data = (void *)&conn_list_req_1, +}; + +static const struct hci_conn_info_req conn_info_req = { + .bdaddr = {{ 0x00, 0x00, 0x01, 0x01, 0xaa, 0x00 }}, + .type = ACL_LINK, + .conn_info = {{ + .handle = 0x002a, + .bdaddr = {{ 0x00, 0x00, 0x01, 0x01, 0xaa, 0x00 }}, + .type = 0x01, + .out = 0x00, + .state = 0x0001, + .link_mode = 0x00000000, + }}, +}; + +static const struct hci_conn_info_req conn_info_req_acl = { + .bdaddr = {{ 0x00, 0x00, 0x01, 0x01, 0xaa, 0x00 }}, + .type = ACL_LINK, +}; + +static const struct hci_conn_info_req conn_info_req_sco = { + .bdaddr = {{ 0x00, 0x00, 0x01, 0x01, 0xaa, 0x00 }}, + .type = SCO_LINK, +}; + +static const struct ioctl_data conn_info = { + .cmd = HCIGETCONNINFO, + .param = (void *)&conn_info_req_acl, + .cmd_param_func = conn_info_cmd_param_func, + .expected_data = (void *)&conn_info_req, +}; + +static const struct ioctl_data conn_info_no_conn = { + .cmd = HCIGETCONNINFO, + .param = (void *)&conn_info_req_acl, + .expected_ioctl_err = ENOENT, + .cmd_param_func = conn_info_cmd_param_func, +}; + +static const struct ioctl_data conn_info_wrong_type = { + .cmd = HCIGETCONNINFO, + .param = (void *)&conn_info_req_sco, + .expected_ioctl_err = ENOENT, + .cmd_param_func = conn_info_cmd_param_func, +}; + +static const struct hci_auth_info_req auth_info_req = { + .bdaddr = {{ 0x00, 0x00, 0x01, 0x01, 0xaa, 0x00 }}, +}; + +static const struct hci_auth_info_req auth_info_connected = { + .bdaddr = {{ 0x00, 0x00, 0x01, 0x01, 0xaa, 0x00 }}, + .type = 0x04, +}; + +static const struct ioctl_data auth_info_no_conn = { + .cmd = HCIGETAUTHINFO, + .param = (void *)&auth_info_req, + .expected_ioctl_err = ENOENT, + .cmd_param_func = auth_info_cmd_param_func, +}; + +static const struct ioctl_data auth_info = { + .cmd = HCIGETAUTHINFO, + .param = (void *)&auth_info_req, + .cmd_param_func = auth_info_cmd_param_func, + .expected_data = (void *)&auth_info_connected, +}; + +/* Allocate the command request parameters based on the command. + * returns the allocated request buffer and its length + */ +static int test_alloc_cmd_param(void **req, uint32_t *req_len) +{ + struct test_data *data = tester_get_data(); + const struct ioctl_data *ioctl_data = data->test_data; + struct hci_dev_req *dr = NULL; + struct hci_dev_info *di = NULL; + struct hci_dev_list_req *dl = NULL; + struct hci_conn_list_req *cl = NULL; + struct hci_conn_info *ci = NULL; + struct hci_conn_info_req *cr = NULL; + struct hci_auth_info_req *ar = NULL; + bdaddr_t *bdaddr = NULL; + uint32_t len; + + switch (ioctl_data->cmd) { + case HCISETAUTH: + case HCISETENCRYPT: + case HCISETLINKMODE: + case HCISETPTYPE: + case HCISETACLMTU: + case HCISETSCOMTU: + len = sizeof(*dr); + dr = malloc(len); + if (!dr) + return -ENOMEM; + memset(dr, 0, len); + dr->dev_id = data->hci_dev_id; + dr->dev_opt = ioctl_data->opt; + *req = dr; + *req_len = len; + break; + case HCIGETDEVINFO: + len = sizeof(*di); + di = malloc(len); + if (!di) + return -ENOMEM; + memset(di, 0, len); + di->dev_id = data->hci_dev_id; + *req = di; + *req_len = len; + break; + case HCIGETDEVLIST: + len = sizeof(*dr) + sizeof(uint16_t); + dl = malloc(len); + if (!dl) + return -ENOMEM; + memset(dl, 0, len); + dl->dev_num = 1; + *req = dl; + *req_len = len; + break; + case HCIGETCONNLIST: + len = sizeof(*cl) + sizeof(*ci); + cl = malloc(len); + if (!cl) + return -ENOMEM; + memset(cl, 0, len); + cl->dev_id = data->hci_dev_id; + cl->conn_num = 1; + *req = cl; + *req_len = len; + break; + case HCIGETCONNINFO: + len = sizeof(*cr) + sizeof(*ci); + cr = malloc(len); + if (!cr) + return -ENOMEM; + memset(cr, 0, len); + *req = cr; + *req_len = len; + break; + case HCIGETAUTHINFO: + len = sizeof(*ar); + ar = malloc(len); + if (!ar) + return -ENOMEM; + memset(ar, 0, len); + *req = ar; + *req_len = len; + break; + case HCIBLOCKADDR: + case HCIUNBLOCKADDR: + len = sizeof(bdaddr_t); + bdaddr = malloc(len); + if (!bdaddr) + return -ENOMEM; + memset(bdaddr, 0, len); + *req = bdaddr; + *req_len = len; + break; + case HCIDEVUP: + case HCIDEVDOWN: + case HCIDEVRESET: + case HCIDEVRESTAT: + /* These command uses the HCI dev id for param */ + return -ENODATA; + default: + return -EINVAL; + } + + return 0; +} + +static void test_ioctl_common(const void *test_data) +{ + struct test_data *data = tester_get_data(); + const struct ioctl_data *ioctl_data = data->test_data; + bool use_dev_id = false; + void *req = NULL; + uint32_t req_len = 0; + int ret; + + ret = test_alloc_cmd_param(&req, &req_len); + if (ret < 0) { + if (ret == -ENODATA) + use_dev_id = true; + else { + tester_warn("Failed to allocate CMD parameter"); + tester_test_failed(); + return; + } + } + + if (ioctl_data->expected_ioctl_err) + test_add_condition(data); + + if (ioctl_data->expected_data) + test_add_condition(data); + + if (!use_dev_id && ioctl_data->param) { + test_add_condition(data); + if (ioctl_data->cmd_param_func) { + ret = ioctl_data->cmd_param_func(req, &req_len); + if (ret) { + tester_warn("Failed to update cmd param"); + tester_test_failed(); + goto exit_free; + } + } else + memcpy(req, ioctl_data->param, req_len); + + tester_print("Command Parameter is updated"); + test_condition_complete(data); + } + + if (use_dev_id) + ret = ioctl(data->sock_fd, ioctl_data->cmd, data->hci_dev_id); + else + ret = ioctl(data->sock_fd, ioctl_data->cmd, req); + + if (ret < 0) { + if (ioctl_data->expected_ioctl_err) { + if (errno != ioctl_data->expected_ioctl_err) { + tester_warn("Unexpected error: %d expected: %d", + errno, ioctl_data->expected_ioctl_err); + tester_test_failed(); + goto exit_free; + } + + test_condition_complete(data); + tester_print("Received expected error: %d", errno); + goto exit_pass; + } + + tester_warn("IOCTL failed with error: %d", errno); + tester_test_failed(); + goto exit_free; + } + + if (ioctl_data->expected_data && req) { + if (ioctl_data->expect_data_check_func) + ret = ioctl_data->expect_data_check_func(req, req_len); + else + ret = memcmp(req, ioctl_data->expected_data, req_len); + + if (ret != 0) { + tester_warn("Mismatch expected data"); + util_hexdump('>', req, req_len, print_debug, ""); + util_hexdump('!', ioctl_data->expected_data, req_len, + print_debug, ""); + tester_test_failed(); + goto exit_free; + } + + test_condition_complete(data); + } + +exit_pass: + tester_test_passed(); +exit_free: + if (req) + free(req); + +} + +static void test_ioctl_connected_event(uint16_t index, uint16_t length, + const void *param, void *user_data) +{ + struct test_data *data = tester_get_data(); + + tester_print("Device Connected"); + + test_ioctl_common(data); +} + +static void test_ioctl_connection(const void *test_data) +{ + struct test_data *data = tester_get_data(); + unsigned int id; + const uint8_t *central_bdaddr; + struct bthost *bthost; + uint8_t addr_type; + + tester_print("Registering %s notification", + mgmt_evstr(MGMT_EV_DEVICE_CONNECTED)); + id = mgmt_register(data->mgmt_alt, MGMT_EV_DEVICE_CONNECTED, + data->mgmt_index, + test_ioctl_connected_event, + NULL, NULL); + data->mgmt_alt_ev_id = id; + + central_bdaddr = hciemu_get_central_bdaddr(data->hciemu); + if (!central_bdaddr) { + tester_warn("No central bdaddr"); + tester_setup_failed(); + return; + } + + addr_type = data->hciemu_type == HCIEMU_TYPE_BREDRLE ? BDADDR_BREDR : + BDADDR_LE_PUBLIC; + tester_print("ADDR TYPE: %d", addr_type); + bthost = hciemu_client_get_host(data->hciemu); + bthost_hci_connect(bthost, central_bdaddr, addr_type); +} + +int main(int argc, char *argv[]) +{ + tester_init(&argc, &argv); + + test_ioctl("HCI Down", &dev_down, NULL, test_ioctl_common); + + test_ioctl("Device List", &dev_list, + NULL, test_ioctl_common); + + test_ioctl("Device List - Invalid Param 1", &dev_list_invalid_1, + NULL, test_ioctl_common); + + test_ioctl("Device Info", &dev_info, + NULL, test_ioctl_common); + + test_ioctl("Reset Stat", &reset_stat, + setup_powered, test_ioctl_common); + + test_ioctl("Set Link Mode - ACCEPT", &set_link_mode_accept, + NULL, test_ioctl_common); + + test_ioctl("Set Link Mode - MASTER", &set_link_mode_master, + NULL, test_ioctl_common); + + test_ioctl("Set Pkt Type - DM", &set_pkt_type_dm, + NULL, test_ioctl_common); + + test_ioctl("Set Pkt Type - DH", &set_pkt_type_dh, + NULL, test_ioctl_common); + + test_ioctl("Set Pkt Type - HV", &set_pkt_type_hv, + NULL, test_ioctl_common); + + test_ioctl("Set Pkt Type - 2-DH", &set_pkt_type_2dh, + NULL, test_ioctl_common); + + test_ioctl("Set Pkt Type - 2-DH", &set_pkt_type_3dh, + NULL, test_ioctl_common); + + test_ioctl("Set Pkt Type - ALL", &set_pkt_type_all, + NULL, test_ioctl_common); + + test_ioctl("Set ACL MTU - 1", &set_acl_mtu_1, + NULL, test_ioctl_common); + + test_ioctl("Set ACL MTU - 2", &set_acl_mtu_2, + NULL, test_ioctl_common); + + test_ioctl("Set SCO MTU - 1", &set_sco_mtu_1, + NULL, test_ioctl_common); + + test_ioctl("Set SCO MTU - 2", &set_sco_mtu_2, + NULL, test_ioctl_common); + + test_ioctl("Block BDADDR - Success", &block_bdaddr_success, + NULL, test_ioctl_common); + + test_ioctl("Block BDADDR - Fail", &block_bdaddr_fail, + setup_add_block_bdaddr, test_ioctl_common); + + test_ioctl("Unblock BDADDR - Success", &unblock_bdaddr_success, + setup_add_block_bdaddr, test_ioctl_common); + + test_ioctl("Unblock BDADDR - Fail", &unblock_bdaddr_fail, + NULL, test_ioctl_common); + + test_ioctl("Connection List - No Conn", &conn_list_no_conn, + NULL, test_ioctl_common); + + test_ioctl("Connection List", &conn_list, + setup_powered, test_ioctl_connection); + + test_ioctl("Connection Info", &conn_info, + setup_powered, test_ioctl_connection); + + test_ioctl("Connection Info - No Connection", &conn_info_no_conn, + setup_powered, test_ioctl_common); + + test_ioctl("Connection Info - Wrong Type", &conn_info_wrong_type, + setup_powered, test_ioctl_common); + + test_ioctl("Authentication Info - No Connection", &auth_info_no_conn, + setup_powered, test_ioctl_common); + + test_ioctl("Authentication Info", &auth_info, + setup_powered, test_ioctl_connection); + + return tester_run(); +} diff -pruN 5.65-1/tools/iso-tester.c 5.66-1/tools/iso-tester.c --- 5.65-1/tools/iso-tester.c 2022-07-24 20:59:52.000000000 +0000 +++ 5.66-1/tools/iso-tester.c 2022-11-10 20:22:09.000000000 +0000 @@ -136,6 +136,7 @@ struct test_data { unsigned int io_id[2]; uint8_t client_num; int step; + bool reconnect; }; struct iso_client_data { @@ -146,6 +147,7 @@ struct iso_client_data { bool server; bool bcast; bool defer; + bool disconnect; }; static void mgmt_debug(const char *str, void *user_data) @@ -541,6 +543,12 @@ static const struct iovec send_16_2_1 = .iov_len = sizeof(data_16_2_1), }; +static const uint8_t data_48_2_1[100] = { [0 ... 99] = 0xff }; +static const struct iovec send_48_2_1 = { + .iov_base = (void *)data_48_2_1, + .iov_len = sizeof(data_48_2_1), +}; + static const struct iso_client_data connect_16_2_1_send = { .qos = QOS_16_2_1, .expect_err = 0, @@ -567,6 +575,13 @@ static const struct iso_client_data conn .defer = true, }; +static const struct iso_client_data connect_48_2_1_defer_send = { + .qos = QOS_48_2_1, + .expect_err = 0, + .send = &send_16_2_1, + .defer = true, +}; + static const struct iso_client_data listen_16_2_1_defer_recv = { .qos = QOS_16_2_1, .expect_err = 0, @@ -575,6 +590,14 @@ static const struct iso_client_data list .defer = true, }; +static const struct iso_client_data listen_48_2_1_defer_recv = { + .qos = QOS_48_2_1, + .expect_err = 0, + .recv = &send_48_2_1, + .server = true, + .defer = true, +}; + static const struct iso_client_data listen_16_2_1_defer_reject = { .qos = QOS_16_2_1, .expect_err = -1, @@ -590,6 +613,18 @@ static const struct iso_client_data conn .recv = &send_16_2_1, }; +static const struct iso_client_data disconnect_16_2_1 = { + .qos = QOS_16_2_1, + .expect_err = 0, + .disconnect = true, +}; + +static const struct iso_client_data reconnect_16_2_1 = { + .qos = QOS_16_2_1, + .expect_err = 0, + .disconnect = true, +}; + static const struct iso_client_data bcast_16_2_1_send = { .qos = QOS_OUT_16_2_1, .expect_err = 0, @@ -641,13 +676,42 @@ static void client_connectable_complete( } } +static void bthost_recv_data(const void *buf, uint16_t len, void *user_data) +{ + struct test_data *data = user_data; + const struct iso_client_data *isodata = data->test_data; + + tester_print("Client received %u bytes of data", len); + + if (isodata->send && (isodata->send->iov_len != len || + memcmp(isodata->send->iov_base, buf, len))) { + if (!isodata->recv->iov_base) + tester_test_failed(); + } else + tester_test_passed(); +} + +static void bthost_iso_disconnected(void *user_data) +{ + struct test_data *data = user_data; + + tester_print("ISO handle 0x%04x disconnected", data->handle); + + data->handle = 0x0000; +} + static void iso_new_conn(uint16_t handle, void *user_data) { struct test_data *data = user_data; + struct bthost *host; tester_print("New client connection with handle 0x%04x", handle); data->handle = handle; + + host = hciemu_client_get_host(data->hciemu); + bthost_add_iso_hook(host, data->handle, bthost_recv_data, data, + bthost_iso_disconnected); } static void acl_new_conn(uint16_t handle, void *user_data) @@ -687,7 +751,7 @@ static void setup_powered_callback(uint8 if (!isodata) continue; - if (isodata->send || isodata->recv) + if (isodata->send || isodata->recv || isodata->disconnect) bthost_set_iso_cb(host, iso_new_conn, data); if (isodata->bcast) { @@ -966,7 +1030,7 @@ static bool check_io_qos(const struct bt return false; } - if (io1->sdu != io2->sdu) { + if (io1->sdu && io2->sdu && io1->sdu != io2->sdu) { tester_warn("Unexpected IO SDU: %u != %u", io1->sdu, io2->sdu); return false; } @@ -1075,25 +1139,9 @@ static void iso_recv(struct test_data *d data->io_id[0] = g_io_add_watch(io, G_IO_IN, iso_recv_data, data); } -static void bthost_recv_data(const void *buf, uint16_t len, void *user_data) -{ - struct test_data *data = user_data; - const struct iso_client_data *isodata = data->test_data; - - tester_print("Client received %u bytes of data", len); - - if (isodata->send && (isodata->send->iov_len != len || - memcmp(isodata->send->iov_base, buf, len))) { - if (!isodata->recv->iov_base) - tester_test_failed(); - } else - tester_test_passed(); -} - static void iso_send(struct test_data *data, GIOChannel *io) { const struct iso_client_data *isodata = data->test_data; - struct bthost *host; ssize_t ret; int sk; @@ -1101,9 +1149,6 @@ static void iso_send(struct test_data *d tester_print("Writing %zu bytes of data", isodata->send->iov_len); - host = hciemu_client_get_host(data->hciemu); - bthost_add_iso_hook(host, data->handle, bthost_recv_data, data); - ret = writev(sk, isodata->send, 1); if (ret < 0 || isodata->send->iov_len != (size_t) ret) { tester_warn("Failed to write %zu bytes: %s (%d)", @@ -1121,6 +1166,49 @@ static void iso_send(struct test_data *d iso_recv(data, io); } +static void setup_connect(struct test_data *data, uint8_t num, GIOFunc func); +static gboolean iso_connect_cb(GIOChannel *io, GIOCondition cond, + gpointer user_data); + +static gboolean iso_disconnected(GIOChannel *io, GIOCondition cond, + gpointer user_data) +{ + struct test_data *data = user_data; + + data->io_id[0] = 0; + + if ((cond & G_IO_HUP) && !data->handle) { + tester_print("Successfully disconnected"); + + if (data->reconnect) { + data->reconnect = false; + setup_connect(data, 0, iso_connect_cb); + return FALSE; + } + + tester_test_passed(); + } else + tester_test_failed(); + + return FALSE; +} + +static void iso_shutdown(struct test_data *data, GIOChannel *io) +{ + int sk; + + sk = g_io_channel_unix_get_fd(io); + + data->io_id[0] = g_io_add_watch(io, G_IO_HUP, iso_disconnected, data); + + /* Shutdown using SHUT_WR as SHUT_RDWR cause the socket to HUP + * immediately instead of waiting for Disconnect Complete event. + */ + shutdown(sk, SHUT_WR); + + tester_print("Disconnecting..."); +} + static gboolean iso_connect(GIOChannel *io, GIOCondition cond, gpointer user_data) { @@ -1174,6 +1262,8 @@ static gboolean iso_connect(GIOChannel * iso_send(data, io); else if (isodata->recv) iso_recv(data, io); + else if (isodata->disconnect) + iso_shutdown(data, io); else tester_test_passed(); } @@ -1233,6 +1323,19 @@ static void setup_connect(struct test_da } if (isodata->defer) { + int defer; + socklen_t len; + + /* Check if socket has DEFER_SETUP set */ + len = sizeof(defer); + if (getsockopt(sk, SOL_BLUETOOTH, BT_DEFER_SETUP, &defer, + &len) < 0) { + tester_warn("getsockopt: %s (%d)", strerror(errno), + errno); + tester_test_failed(); + return; + } + memset(&pfd, 0, sizeof(pfd)); pfd.fd = sk; pfd.events = POLLOUT; @@ -1272,6 +1375,19 @@ static void test_connect(const void *tes setup_connect(data, 0, iso_connect_cb); } +static void setup_reconnect(struct test_data *data, uint8_t num, GIOFunc func) +{ + data->reconnect = true; + setup_connect(data, num, func); +} + +static void test_reconnect(const void *test_data) +{ + struct test_data *data = tester_get_data(); + + setup_reconnect(data, 0, iso_connect_cb); +} + static void test_defer(const void *test_data) { struct test_data *data = tester_get_data(); @@ -1430,7 +1546,7 @@ static void setup_listen(struct test_dat client = hciemu_get_client(data->hciemu, 0); host = hciemu_client_host(client); - bthost_set_cig_params(host, 0x01, 0x01); + bthost_set_cig_params(host, 0x01, 0x01, &isodata->qos); bthost_create_cis(host, 257, data->acl_handle); } } @@ -1674,9 +1790,17 @@ int main(int argc, char *argv[]) setup_powered, test_connect); + test_iso("ISO 48_2_1 Defer Send - Success", &connect_48_2_1_defer_send, + setup_powered, + test_connect); + test_iso("ISO Defer Receive - Success", &listen_16_2_1_defer_recv, setup_powered, test_listen); + test_iso("ISO 48_2_1 Defer Receive - Success", + &listen_48_2_1_defer_recv, + setup_powered, test_listen); + test_iso("ISO Defer Reject - Success", &listen_16_2_1_defer_reject, setup_powered, test_listen); @@ -1684,6 +1808,14 @@ int main(int argc, char *argv[]) setup_powered, test_connect); + test_iso("ISO Disconnect - Success", &disconnect_16_2_1, + setup_powered, + test_connect); + + test_iso("ISO Reconnect - Success", &reconnect_16_2_1, + setup_powered, + test_reconnect); + test_iso("ISO Broadcaster - Success", &bcast_16_2_1_send, setup_powered, test_bcast); test_iso("ISO Broadcaster BIG 0x01 - Success", &bcast_1_16_2_1_send, diff -pruN 5.65-1/tools/l2test.c 5.66-1/tools/l2test.c --- 5.65-1/tools/l2test.c 2022-07-24 20:59:52.000000000 +0000 +++ 5.66-1/tools/l2test.c 2022-11-10 20:22:09.000000000 +0000 @@ -893,8 +893,9 @@ static void recv_mode(int sk) timestamp = 0; memset(ts, 0, sizeof(ts)); } else { - sprintf(ts, "[%ld.%ld] ", - tv.tv_sec, tv.tv_usec); + sprintf(ts, "[%lld.%lld] ", + (long long)tv.tv_sec, + (long long)tv.tv_usec); } } diff -pruN 5.65-1/tools/mesh-tester.c 5.66-1/tools/mesh-tester.c --- 5.65-1/tools/mesh-tester.c 1970-01-01 00:00:00.000000000 +0000 +++ 5.66-1/tools/mesh-tester.c 2022-11-10 20:22:09.000000000 +0000 @@ -0,0 +1,1453 @@ +// SPDX-License-Identifier: GPL-2.0-or-later +/* + * + * BlueZ - Bluetooth protocol stack for Linux + * + * Copyright (C) 2012 Intel Corporation. All rights reserved. + * + * + */ + +#ifdef HAVE_CONFIG_H +#include +#endif + +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include "lib/bluetooth.h" +#include "lib/hci.h" +#include "lib/hci_lib.h" +#include "lib/mgmt.h" +#include "lib/l2cap.h" + +#include "monitor/bt.h" +#include "emulator/vhci.h" +#include "emulator/bthost.h" +#include "emulator/hciemu.h" + +#include "src/shared/util.h" +#include "src/shared/tester.h" +#include "src/shared/mgmt.h" +#include "src/shared/queue.h" + +struct test_data { + tester_data_func_t test_setup; + const void *test_data; + uint8_t expected_version; + uint16_t expected_manufacturer; + uint32_t expected_supported_settings; + uint32_t initial_settings; + struct mgmt *mgmt; + struct mgmt *mgmt_alt; + unsigned int mgmt_settings_id; + unsigned int mgmt_alt_settings_id; + unsigned int mgmt_alt_ev_id; + unsigned int mgmt_discov_ev_id; + uint8_t mgmt_version; + uint16_t mgmt_revision; + uint16_t mgmt_index; + struct hciemu *hciemu; + enum hciemu_type hciemu_type; + bool expect_hci_command_done; + struct queue *expect_hci_q; + int unmet_conditions; + int unmet_setup_conditions; + int sk; +}; + +static void print_debug(const char *str, void *user_data) +{ + const char *prefix = user_data; + + tester_print("%s%s", prefix, str); +} + +static void test_post_teardown(const void *test_data) +{ + struct test_data *data = tester_get_data(); + + if (data->sk >= 0) + close(data->sk); + + queue_destroy(data->expect_hci_q, NULL); + + hciemu_unref(data->hciemu); + data->hciemu = NULL; +} + +static void test_pre_setup_failed(void) +{ + test_post_teardown(NULL); + tester_pre_setup_failed(); +} + +static void read_version_callback(uint8_t status, uint16_t length, + const void *param, void *user_data) +{ + struct test_data *data = tester_get_data(); + const struct mgmt_rp_read_version *rp = param; + + tester_print("Read Version callback"); + tester_print(" Status: %s (0x%02x)", mgmt_errstr(status), status); + + if (status || !param) { + test_pre_setup_failed(); + return; + } + + data->mgmt_version = rp->version; + data->mgmt_revision = btohs(rp->revision); + + tester_print(" Version %u.%u", + data->mgmt_version, data->mgmt_revision); +} + +static void read_commands_callback(uint8_t status, uint16_t length, + const void *param, void *user_data) +{ + tester_print("Read Commands callback"); + tester_print(" Status: %s (0x%02x)", mgmt_errstr(status), status); + + if (status || !param) { + test_pre_setup_failed(); + return; + } +} + +static bool check_settings(uint32_t supported, uint32_t expected) +{ + int i; + + if (supported == expected) + return true; + + for (i = 0; i < 17; i++) { + if (supported & BIT(i)) + continue; + + if (expected & BIT(i)) { + tester_warn("Expected bit %u not supported", i); + return false; + } + } + + return true; +} + +static void read_info_callback(uint8_t status, uint16_t length, + const void *param, void *user_data) +{ + struct test_data *data = tester_get_data(); + const struct mgmt_rp_read_info *rp = param; + char addr[18]; + uint16_t manufacturer; + uint32_t supported_settings, current_settings; + struct bthost *bthost; + + tester_print("Read Info callback"); + tester_print(" Status: %s (0x%02x)", mgmt_errstr(status), status); + + if (status || !param) { + test_pre_setup_failed(); + return; + } + + ba2str(&rp->bdaddr, addr); + manufacturer = btohs(rp->manufacturer); + supported_settings = btohl(rp->supported_settings); + current_settings = btohl(rp->current_settings); + + tester_print(" Address: %s", addr); + tester_print(" Version: 0x%02x", rp->version); + tester_print(" Manufacturer: 0x%04x", manufacturer); + tester_print(" Supported settings: 0x%08x", supported_settings); + tester_print(" Current settings: 0x%08x", current_settings); + tester_print(" Class: 0x%02x%02x%02x", + rp->dev_class[2], rp->dev_class[1], rp->dev_class[0]); + tester_print(" Name: %s", rp->name); + tester_print(" Short name: %s", rp->short_name); + + if (strcmp(hciemu_get_address(data->hciemu), addr)) { + test_pre_setup_failed(); + return; + } + + if (rp->version != data->expected_version) { + tester_warn("Expected version: 0x%02x != 0x%02x", + rp->version, data->expected_version); + test_pre_setup_failed(); + return; + } + + if (manufacturer != data->expected_manufacturer) { + tester_warn("Expected manufacturer: 0x%04x != 0x%04x", + manufacturer, data->expected_manufacturer); + test_pre_setup_failed(); + return; + } + + if (!check_settings(supported_settings, + data->expected_supported_settings)) { + tester_warn("Expected supported settings: 0x%08x != 0x%08x", + supported_settings, + data->expected_supported_settings); + test_pre_setup_failed(); + return; + } + + if (!check_settings(current_settings, data->initial_settings)) { + tester_warn("Initial settings: 0x%08x != 0x%08x", + current_settings, data->initial_settings); + test_pre_setup_failed(); + return; + } + + if (rp->dev_class[0] != 0x00 || rp->dev_class[1] != 0x00 || + rp->dev_class[2] != 0x00) { + test_pre_setup_failed(); + return; + } + + bthost = hciemu_client_get_host(data->hciemu); + bthost_notify_ready(bthost, tester_pre_setup_complete); +} + +static const uint8_t set_exp_feat_param_mesh[] = { + 0x76, 0x6e, 0xf3, 0xe8, 0x24, 0x5f, 0x05, 0xbf, /* UUID - Mesh */ + 0x8d, 0x4d, 0x03, 0x7a, 0xd7, 0x63, 0xe4, 0x2c, + 0x01, /* Action - enable */ +}; + +static const uint8_t set_exp_feat_rsp_param_mesh[] = { + 0x76, 0x6e, 0xf3, 0xe8, 0x24, 0x5f, 0x05, 0xbf, /* UUID - Mesh */ + 0x8d, 0x4d, 0x03, 0x7a, 0xd7, 0x63, 0xe4, 0x2c, + 0x01, 0x00, 0x00, 0x00, /* Action - enable */ +}; + +static void mesh_exp_callback(uint8_t status, uint16_t length, + const void *param, void *user_data) +{ + if (status != MGMT_STATUS_SUCCESS) { + tester_print("Mesh feature could not be enabled"); + return; + } + + tester_print("Mesh feature is enabled"); +} + +static void mesh_exp_feature(struct test_data *data, uint16_t index) +{ + tester_print("Enabling Mesh feature"); + + mgmt_send(data->mgmt, MGMT_OP_SET_EXP_FEATURE, index, + sizeof(set_exp_feat_param_mesh), + set_exp_feat_param_mesh, + mesh_exp_callback, NULL, NULL); +} + +static void index_added_callback(uint16_t index, uint16_t length, + const void *param, void *user_data) +{ + struct test_data *data = tester_get_data(); + + tester_print("Index Added callback"); + tester_print(" Index: 0x%04x", index); + + data->mgmt_index = index; + + mgmt_send(data->mgmt, MGMT_OP_READ_INFO, data->mgmt_index, 0, NULL, + read_info_callback, NULL, NULL); + + tester_warn("Enable management Mesh interface"); + mesh_exp_feature(data, data->mgmt_index); + +} + +static void index_removed_callback(uint16_t index, uint16_t length, + const void *param, void *user_data) +{ + struct test_data *data = tester_get_data(); + + tester_print("Index Removed callback"); + tester_print(" Index: 0x%04x", index); + + if (index != data->mgmt_index) + return; + + mgmt_unregister_index(data->mgmt, data->mgmt_index); + mgmt_unregister_index(data->mgmt_alt, data->mgmt_index); + + mgmt_unref(data->mgmt); + data->mgmt = NULL; + + mgmt_unref(data->mgmt_alt); + data->mgmt_alt = NULL; + + tester_post_teardown_complete(); +} + +struct hci_cmd_data { + uint16_t opcode; + uint8_t len; + const void *param; +}; + +struct hci_entry { + const struct hci_cmd_data *cmd_data; +}; + +struct generic_data { + bool setup_le_states; + const uint8_t *le_states; + const uint16_t *setup_settings; + bool setup_nobredr; + bool setup_limited_discov; + const void *setup_exp_feat_param; + uint16_t setup_expect_hci_command; + const void *setup_expect_hci_param; + uint8_t setup_expect_hci_len; + uint16_t setup_send_opcode; + const void *setup_send_param; + uint16_t setup_send_len; + const struct setup_mgmt_cmd *setup_mgmt_cmd_arr; + size_t setup_mgmt_cmd_arr_size; + bool send_index_none; + const void *setup_discovery_param; + uint16_t send_opcode; + const void *send_param; + uint16_t send_len; + const void * (*send_func)(uint16_t *len); + uint8_t expect_status; + bool expect_ignore_param; + const void *expect_param; + uint16_t expect_len; + const void * (*expect_func)(uint16_t *len); + uint32_t expect_settings_set; + uint32_t expect_settings_unset; + uint16_t expect_alt_ev; + const void *expect_alt_ev_param; + bool (*verify_alt_ev_func)(const void *param, uint16_t length); + uint16_t expect_alt_ev_len; + uint16_t expect_hci_command; + const void *expect_hci_param; + int (*expect_hci_param_check_func)(const void *param, uint16_t length); + uint8_t expect_hci_len; + const void * (*expect_hci_func)(uint8_t *len); + const struct hci_cmd_data *expect_hci_list; + bool expect_pin; + uint8_t pin_len; + const void *pin; + uint8_t client_pin_len; + const void *client_pin; + bool client_enable_ssp; + uint8_t io_cap; + uint8_t client_io_cap; + uint8_t client_auth_req; + bool reject_confirm; + bool client_reject_confirm; + bool just_works; + bool client_enable_le; + bool client_enable_sc; + bool client_enable_adv; + bool expect_sc_key; + bool force_power_off; + bool addr_type_avail; + bool fail_tolerant; + uint8_t addr_type; + bool set_adv; + const uint8_t *adv_data; + uint8_t adv_data_len; +}; + +static const uint8_t set_exp_feat_param_debug[] = { + 0x1c, 0xda, 0x47, 0x1c, 0x48, 0x6c, 0x01, 0xab, /* UUID - Debug */ + 0x9f, 0x46, 0xec, 0xb9, 0x30, 0x25, 0x99, 0xd4, + 0x01, /* Action - enable */ +}; + +static void debug_exp_callback(uint8_t status, uint16_t length, + const void *param, void *user_data) +{ + if (status != MGMT_STATUS_SUCCESS) { + tester_print("Debug feature could not be enabled"); + return; + } + + tester_print("Debug feature is enabled"); +} + +static void debug_exp_feature(struct test_data *data) +{ + tester_print("Enabling Debug feature"); + + mgmt_send(data->mgmt, MGMT_OP_SET_EXP_FEATURE, MGMT_INDEX_NONE, + sizeof(set_exp_feat_param_debug), + set_exp_feat_param_debug, + debug_exp_callback, NULL, NULL); +} + +static void read_index_list_callback(uint8_t status, uint16_t length, + const void *param, void *user_data) +{ + struct test_data *data = tester_get_data(); + const struct generic_data *test = data->test_data; + + tester_print("Read Index List callback"); + tester_print(" Status: %s (0x%02x)", mgmt_errstr(status), status); + + if (status || !param) { + test_pre_setup_failed(); + return; + } + + mgmt_register(data->mgmt, MGMT_EV_INDEX_ADDED, MGMT_INDEX_NONE, + index_added_callback, NULL, NULL); + + mgmt_register(data->mgmt, MGMT_EV_INDEX_REMOVED, MGMT_INDEX_NONE, + index_removed_callback, NULL, NULL); + + data->hciemu = hciemu_new(data->hciemu_type); + if (!data->hciemu) { + tester_warn("Failed to setup HCI emulation"); + test_pre_setup_failed(); + } + + if (tester_use_debug()) + hciemu_set_debug(data->hciemu, print_debug, "hciemu: ", NULL); + + if (test && test->setup_le_states) + hciemu_set_central_le_states(data->hciemu, test->le_states); + +} + +static void test_pre_setup(const void *test_data) +{ + struct test_data *data = tester_get_data(); + + data->mgmt = mgmt_new_default(); + if (!data->mgmt) { + tester_warn("Failed to setup management interface"); + test_pre_setup_failed(); + return; + } + + data->mgmt_alt = mgmt_new_default(); + if (!data->mgmt_alt) { + tester_warn("Failed to setup alternate management interface"); + test_pre_setup_failed(); + + mgmt_unref(data->mgmt); + data->mgmt = NULL; + return; + } + + if (tester_use_debug()) { + mgmt_set_debug(data->mgmt, print_debug, "mgmt: ", NULL); + mgmt_set_debug(data->mgmt_alt, print_debug, "mgmt-alt: ", NULL); + debug_exp_feature(data); + } + + mgmt_send(data->mgmt, MGMT_OP_READ_VERSION, MGMT_INDEX_NONE, 0, NULL, + read_version_callback, NULL, NULL); + + mgmt_send(data->mgmt, MGMT_OP_READ_COMMANDS, MGMT_INDEX_NONE, 0, NULL, + read_commands_callback, NULL, NULL); + + mgmt_send(data->mgmt, MGMT_OP_READ_INDEX_LIST, MGMT_INDEX_NONE, 0, NULL, + read_index_list_callback, NULL, NULL); + + data->sk = -1; +} + +#define test_full(name, data, setup, func, timeout, type, version, \ + expected_settings, settings) \ + do { \ + struct test_data *user; \ + user = new0(struct test_data, 1); \ + user->hciemu_type = type; \ + user->test_setup = setup; \ + user->test_data = data; \ + user->expected_version = version; \ + user->expected_manufacturer = 0x05f1; \ + user->expected_supported_settings = expected_settings; \ + user->initial_settings = settings; \ + tester_add_full(name, data, \ + test_pre_setup, test_setup, func, \ + NULL, test_post_teardown, timeout, \ + user, free); \ + } while (0) + +#define test_bredrle_full(name, data, setup, func, timeout) \ + test_full(name, data, setup, func, timeout, HCIEMU_TYPE_BREDRLE, \ + 0x09, 0x0001beff, 0x00000080) + +#define test_bredrle(name, data, setup, func) \ + test_bredrle_full(name, data, setup, func, 2) + +#define test_bredr20(name, data, setup, func) \ + test_full(name, data, setup, func, 2, HCIEMU_TYPE_LEGACY, \ + 0x03, 0x000110bf, 0x00000080) + +#define test_bredr(name, data, setup, func) \ + test_full(name, data, setup, func, 2, HCIEMU_TYPE_BREDR, \ + 0x05, 0x000110ff, 0x00000080) + +#define test_le_full(name, data, setup, func, timeout) \ + test_full(name, data, setup, func, timeout, HCIEMU_TYPE_LE, \ + 0x09, 0x0001be1b, 0x00000200) + +#define test_le(name, data, setup, func) \ + test_le_full(name, data, setup, func, 2) + +#define test_bredrle50_full(name, data, setup, func, timeout) \ + test_full(name, data, setup, func, timeout, HCIEMU_TYPE_BREDRLE50, \ + 0x09, 0x0001beff, 0x00000080) + +#define test_bredrle50(name, data, setup, func) \ + test_bredrle50_full(name, data, setup, func, 2) + +#define test_hs_full(name, data, setup, func, timeout) \ + test_full(name, data, setup, func, timeout, HCIEMU_TYPE_BREDRLE, \ + 0x09, 0x0001bfff, 0x00000080) + +#define test_hs(name, data, setup, func) \ + test_hs_full(name, data, setup, func, 2) + +static void controller_setup(const void *test_data) +{ + tester_test_passed(); +} + +struct setup_mgmt_cmd { + uint8_t send_opcode; + const void *send_param; + uint16_t send_len; +}; + +static bool power_off(uint16_t index) +{ + int sk, err; + + sk = hci_open_dev(index); + if (sk < 0) + return false; + + err = ioctl(sk, HCIDEVDOWN, index); + + hci_close_dev(sk); + + if (err < 0) + return false; + + return true; +} + +static void test_condition_complete(struct test_data *data) +{ + data->unmet_conditions--; + + tester_print("Test condition complete, %d left", + data->unmet_conditions); + + if (data->unmet_conditions > 0) + return; + + tester_test_passed(); +} + +static void command_generic_callback(uint8_t status, uint16_t length, + const void *param, void *user_data) +{ + struct test_data *data = tester_get_data(); + const struct generic_data *test = data->test_data; + const void *expect_param = test->expect_param; + uint16_t expect_len = test->expect_len; + + tester_print("%s (0x%04x): %s (0x%02x)", mgmt_opstr(test->send_opcode), + test->send_opcode, mgmt_errstr(status), status); + + if (status != test->expect_status) { + if (!test->fail_tolerant || !!status != !!test->expect_status) { + tester_test_abort(); + return; + } + + tester_warn("Unexpected status got %d expected %d", + status, test->expect_status); + } + + if (!test->expect_ignore_param) { + if (test->expect_func) + expect_param = test->expect_func(&expect_len); + + if (length != expect_len) { + tester_warn("Invalid cmd response parameter size %d %d", + length, expect_len); + tester_test_failed(); + return; + } + + if (expect_param && expect_len > 0 && + memcmp(param, expect_param, length)) { + tester_warn("Unexpected cmd response parameter value"); + util_hexdump('>', param, length, print_debug, ""); + util_hexdump('!', expect_param, length, print_debug, + ""); + tester_test_failed(); + return; + } + } + + test_condition_complete(data); +} + +static void test_add_condition(struct test_data *data) +{ + data->unmet_conditions++; + + tester_print("Test condition added, total %d", data->unmet_conditions); +} + +/* Read HCI commands in the expect_hci_list and add it to the queue + */ +static void add_expect_hci_list(struct test_data *data) +{ + const struct generic_data *test = data->test_data; + const struct hci_cmd_data *hci_cmd_data; + + /* Initialize the queue */ + data->expect_hci_q = queue_new(); + + hci_cmd_data = test->expect_hci_list; + for (; hci_cmd_data->opcode; hci_cmd_data++) { + struct hci_entry *entry; + + entry = new0(struct hci_entry, 1); + entry->cmd_data = hci_cmd_data; + queue_push_tail(data->expect_hci_q, entry); + + test_add_condition(data); + } +} + +static bool match_hci_cmd_opcode(const void *data, const void *match_data) +{ + const struct hci_entry *entry = data; + uint16_t opcode = PTR_TO_UINT(match_data); + + return entry->cmd_data->opcode == opcode; +} + +static void command_hci_list_callback(uint16_t opcode, const void *param, + uint8_t length, void *user_data) +{ + struct test_data *data = user_data; + const struct hci_cmd_data *hci_cmd_data; + struct hci_entry *entry; + int ret; + + tester_print("HCI Command 0x%04x length %u", opcode, length); + + entry = queue_find(data->expect_hci_q, match_hci_cmd_opcode, + UINT_TO_PTR(opcode)); + if (!entry) + return; + + /* Save the hci cmd data before removing the queue entry */ + hci_cmd_data = entry->cmd_data; + + /* Remove the entry from the queue and free the entry */ + queue_remove(data->expect_hci_q, entry); + free(entry); + + if (length != hci_cmd_data->len) { + tester_warn("Invalid parameter size for HCI command"); + tester_test_failed(); + return; + } + + ret = memcmp(param, hci_cmd_data->param, length); + if (ret != 0) { + tester_warn("Unexpected HCI command parameter value:"); + util_hexdump('>', param, length, print_debug, ""); + util_hexdump('!', hci_cmd_data->param, length, print_debug, ""); + tester_test_failed(); + return; + } + + test_condition_complete(data); +} + +static void command_hci_callback(uint16_t opcode, const void *param, + uint8_t length, void *user_data) +{ + struct test_data *data = user_data; + const struct generic_data *test = data->test_data; + const void *expect_hci_param = test->expect_hci_param; + uint8_t expect_hci_len = test->expect_hci_len; + int ret; + + tester_print("HCI Command 0x%04x length %u", opcode, length); + + if (opcode != test->expect_hci_command || data->expect_hci_command_done) + return; + + data->expect_hci_command_done = true; + + if (test->expect_hci_func) + expect_hci_param = test->expect_hci_func(&expect_hci_len); + + if (length != expect_hci_len) { + tester_warn("Invalid parameter size for HCI command"); + tester_test_failed(); + return; + } + + if (test->expect_hci_param_check_func) + ret = test->expect_hci_param_check_func(param, length); + else + ret = memcmp(param, expect_hci_param, length); + if (ret != 0) { + tester_warn("Unexpected HCI command parameter value:"); + util_hexdump('>', param, length, print_debug, ""); + util_hexdump('!', expect_hci_param, length, print_debug, ""); + tester_test_failed(); + return; + } + + test_condition_complete(data); +} + +static bool verify_alt_ev(const void *param, uint16_t length) +{ + struct test_data *data = tester_get_data(); + const struct generic_data *test = data->test_data; + + if (length != test->expect_alt_ev_len) { + tester_warn("Invalid length %u != %u", length, + test->expect_alt_ev_len); + return false; + } + + if (test->expect_alt_ev_param && + memcmp(test->expect_alt_ev_param, param, length)) { + tester_warn("Event parameters do not match"); + util_hexdump('>', param, length, print_debug, ""); + util_hexdump('!', test->expect_alt_ev_param, length, + print_debug, ""); + return false; + } + + return true; +} + +static void command_generic_event_alt(uint16_t index, uint16_t length, + const void *param, + void *user_data) +{ + struct test_data *data = tester_get_data(); + const struct generic_data *test = data->test_data; + bool (*verify)(const void *param, uint16_t length); + + tester_print("New %s event received", mgmt_evstr(test->expect_alt_ev)); + + mgmt_unregister(data->mgmt_alt, data->mgmt_alt_ev_id); + + if (test->verify_alt_ev_func) + verify = test->verify_alt_ev_func; + else + verify = verify_alt_ev; + + if (!verify(param, length)) { + tester_warn("Incorrect %s event parameters", + mgmt_evstr(test->expect_alt_ev)); + tester_test_failed(); + return; + } + + test_condition_complete(data); +} + +static void command_generic_new_settings(uint16_t index, uint16_t length, + const void *param, void *user_data) +{ + struct test_data *data = tester_get_data(); + + tester_print("New settings event received"); + + mgmt_unregister(data->mgmt, data->mgmt_settings_id); + + tester_test_failed(); +} + +static void command_generic_new_settings_alt(uint16_t index, uint16_t length, + const void *param, void *user_data) +{ + struct test_data *data = tester_get_data(); + const struct generic_data *test = data->test_data; + uint32_t settings; + + if (length != 4) { + tester_warn("Invalid parameter size for new settings event"); + tester_test_failed(); + return; + } + + settings = get_le32(param); + + tester_print("New settings 0x%08x received", settings); + + if (test->expect_settings_unset) { + if ((settings & test->expect_settings_unset) != 0) + return; + goto done; + } + + if (!test->expect_settings_set) + return; + + if ((settings & test->expect_settings_set) != test->expect_settings_set) + return; + +done: + tester_print("Unregistering new settings notification"); + + mgmt_unregister(data->mgmt_alt, data->mgmt_alt_settings_id); + + test_condition_complete(data); +} + +static void test_command_generic(const void *test_data) +{ + struct test_data *data = tester_get_data(); + const struct generic_data *test = data->test_data; + const void *send_param = test->send_param; + uint16_t send_len = test->send_len; + unsigned int id; + uint16_t index; + + index = test->send_index_none ? MGMT_INDEX_NONE : data->mgmt_index; + + if (test->expect_settings_set || test->expect_settings_unset) { + tester_print("Registering new settings notification"); + + id = mgmt_register(data->mgmt, MGMT_EV_NEW_SETTINGS, index, + command_generic_new_settings, NULL, NULL); + data->mgmt_settings_id = id; + + id = mgmt_register(data->mgmt_alt, MGMT_EV_NEW_SETTINGS, index, + command_generic_new_settings_alt, NULL, NULL); + data->mgmt_alt_settings_id = id; + test_add_condition(data); + } + + if (test->expect_alt_ev) { + tester_print("Registering %s notification", + mgmt_evstr(test->expect_alt_ev)); + id = mgmt_register(data->mgmt_alt, test->expect_alt_ev, index, + command_generic_event_alt, NULL, NULL); + data->mgmt_alt_ev_id = id; + test_add_condition(data); + } + + if (test->expect_hci_command) { + tester_print("Registering HCI command callback"); + hciemu_add_central_post_command_hook(data->hciemu, + command_hci_callback, data); + test_add_condition(data); + } else if (test->expect_hci_list) { + /* Use this when it needs to check more than 1 hci command. + * However, it cannot be used with expect_hci_command. + */ + tester_print("Registering HCI command list callback"); + hciemu_add_central_post_command_hook(data->hciemu, + command_hci_list_callback, data); + add_expect_hci_list(data); + } + + if (test->send_opcode == 0x0000) { + tester_print("Executing no-op test"); + return; + } + + tester_print("Sending %s (0x%04x)", mgmt_opstr(test->send_opcode), + test->send_opcode); + + if (test->send_func) + send_param = test->send_func(&send_len); + + if (test->force_power_off) { + mgmt_send_nowait(data->mgmt, test->send_opcode, index, + send_len, send_param, + command_generic_callback, NULL, NULL); + power_off(data->mgmt_index); + } else { + mgmt_send(data->mgmt, test->send_opcode, index, send_len, + send_param, command_generic_callback, + NULL, NULL); + } + + test_add_condition(data); +} + +static void test_add_setup_condition(struct test_data *data) +{ + data->unmet_setup_conditions++; + + tester_print("Test setup condition added, total %d", + data->unmet_setup_conditions); +} + +static void test_setup_condition_complete(struct test_data *data) +{ + data->unmet_setup_conditions--; + + tester_print("Test setup condition complete, %d left", + data->unmet_setup_conditions); + + if (data->unmet_setup_conditions > 0) + return; + + tester_setup_complete(); +} + +static void client_cmd_complete(uint16_t opcode, uint8_t status, + const void *param, uint8_t len, + void *user_data) +{ + struct test_data *data = tester_get_data(); + const struct generic_data *test = data->test_data; + struct bthost *bthost; + + bthost = hciemu_client_get_host(data->hciemu); + + switch (opcode) { + case BT_HCI_CMD_WRITE_SCAN_ENABLE: + case BT_HCI_CMD_LE_SET_ADV_ENABLE: + case BT_HCI_CMD_LE_SET_EXT_ADV_ENABLE: + tester_print("Client set connectable: %s (0x%02x)", + mgmt_errstr(status), status); + if (!status && test->client_enable_ssp) { + bthost_write_ssp_mode(bthost, 0x01); + return; + } + break; + case BT_HCI_CMD_WRITE_SIMPLE_PAIRING_MODE: + tester_print("Client enable SSP: %s (0x%02x)", + mgmt_errstr(status), status); + break; + default: + return; + } + + if (status) + tester_setup_failed(); + else + test_setup_condition_complete(data); +} + +static void setup_bthost(void) +{ + struct test_data *data = tester_get_data(); + const struct generic_data *test = data->test_data; + struct bthost *bthost; + + bthost = hciemu_client_get_host(data->hciemu); + bthost_set_cmd_complete_cb(bthost, client_cmd_complete, data); + test_add_setup_condition(data); + + if (data->hciemu_type == HCIEMU_TYPE_LE || + test->client_enable_adv) { + if (data->hciemu_type >= HCIEMU_TYPE_BREDRLE50) { + bthost_set_ext_adv_params(bthost); + bthost_set_ext_adv_enable(bthost, 0x01); + } else + bthost_set_adv_enable(bthost, 0x01); + } else + bthost_write_scan_enable(bthost, 0x03); +} + +static void setup_complete(uint8_t status, uint16_t length, + const void *param, void *user_data) +{ + struct test_data *data = tester_get_data(); + + if (status != MGMT_STATUS_SUCCESS) { + tester_setup_failed(); + return; + } + + tester_print("Initial settings completed"); + + if (data->test_setup) + data->test_setup(data); + else + setup_bthost(); +} + +static void pin_code_request_callback(uint16_t index, uint16_t length, + const void *param, void *user_data) +{ + const struct mgmt_ev_pin_code_request *ev = param; + struct test_data *data = user_data; + const struct generic_data *test = data->test_data; + struct mgmt_cp_pin_code_reply cp; + + test_condition_complete(data); + + memset(&cp, 0, sizeof(cp)); + memcpy(&cp.addr, &ev->addr, sizeof(cp.addr)); + + if (!test->pin) { + mgmt_reply(data->mgmt, MGMT_OP_PIN_CODE_NEG_REPLY, + data->mgmt_index, sizeof(cp.addr), &cp.addr, + NULL, NULL, NULL); + return; + } + + cp.pin_len = test->pin_len; + memcpy(cp.pin_code, test->pin, test->pin_len); + + mgmt_reply(data->mgmt, MGMT_OP_PIN_CODE_REPLY, data->mgmt_index, + sizeof(cp), &cp, NULL, NULL, NULL); +} + +static void user_confirm_request_callback(uint16_t index, uint16_t length, + const void *param, + void *user_data) +{ + const struct mgmt_ev_user_confirm_request *ev = param; + struct test_data *data = user_data; + const struct generic_data *test = data->test_data; + struct mgmt_cp_user_confirm_reply cp; + uint16_t opcode; + + memset(&cp, 0, sizeof(cp)); + memcpy(&cp.addr, &ev->addr, sizeof(cp.addr)); + + if (test->reject_confirm) + opcode = MGMT_OP_USER_CONFIRM_NEG_REPLY; + else + opcode = MGMT_OP_USER_CONFIRM_REPLY; + + mgmt_reply(data->mgmt, opcode, data->mgmt_index, sizeof(cp), &cp, + NULL, NULL, NULL); +} + +static void user_passkey_request_callback(uint16_t index, uint16_t length, + const void *param, + void *user_data) +{ + const struct mgmt_ev_user_passkey_request *ev = param; + struct test_data *data = user_data; + const struct generic_data *test = data->test_data; + struct mgmt_cp_user_passkey_reply cp; + + if (test->just_works) { + tester_warn("User Passkey Request for just-works case"); + tester_test_failed(); + return; + } + + memset(&cp, 0, sizeof(cp)); + memcpy(&cp.addr, &ev->addr, sizeof(cp.addr)); + + if (test->reject_confirm) { + mgmt_reply(data->mgmt, MGMT_OP_USER_PASSKEY_NEG_REPLY, + data->mgmt_index, sizeof(cp.addr), &cp.addr, + NULL, NULL, NULL); + return; + } + + mgmt_reply(data->mgmt, MGMT_OP_USER_PASSKEY_REPLY, data->mgmt_index, + sizeof(cp), &cp, NULL, NULL, NULL); +} + +static void test_setup(const void *test_data) +{ + struct test_data *data = tester_get_data(); + const struct generic_data *test = data->test_data; + struct bthost *bthost = hciemu_client_get_host(data->hciemu); + const uint16_t *cmd; + + if (!test) + goto proceed; + + if (test->pin || test->expect_pin) { + mgmt_register(data->mgmt, MGMT_EV_PIN_CODE_REQUEST, + data->mgmt_index, pin_code_request_callback, + data, NULL); + test_add_condition(data); + } + + mgmt_register(data->mgmt, MGMT_EV_USER_CONFIRM_REQUEST, + data->mgmt_index, user_confirm_request_callback, + data, NULL); + + mgmt_register(data->mgmt, MGMT_EV_USER_PASSKEY_REQUEST, + data->mgmt_index, user_passkey_request_callback, + data, NULL); + + if (test->client_pin) + bthost_set_pin_code(bthost, test->client_pin, + test->client_pin_len); + + if (test->client_io_cap) + bthost_set_io_capability(bthost, test->client_io_cap); + + if (test->client_auth_req) + bthost_set_auth_req(bthost, test->client_auth_req); + else if (!test->just_works) + bthost_set_auth_req(bthost, 0x01); + + if (test->client_reject_confirm) + bthost_set_reject_user_confirm(bthost, true); + + if (test->client_enable_le) + bthost_write_le_host_supported(bthost, 0x01); + + if (test->client_enable_sc) + bthost_set_sc_support(bthost, 0x01); + +proceed: + if (!test || !test->setup_settings) { + if (data->test_setup) + data->test_setup(data); + else + tester_setup_complete(); + return; + } + + for (cmd = test->setup_settings; *cmd; cmd++) { + unsigned char simple_param[] = { 0x01 }; + unsigned char discov_param[] = { 0x01, 0x00, 0x00 }; + unsigned char privacy_param[] = { 0x01, + 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07, 0x08, + 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07, 0x08 }; + unsigned char set_exp_feat_param[17] = { 0x00 }; + unsigned char *param = simple_param; + size_t param_size = sizeof(simple_param); + mgmt_request_func_t func = NULL; + + /* If this is the last command (next one is 0) request + * for a callback. + */ + if (!cmd[1]) + func = setup_complete; + + if (*cmd == MGMT_OP_SET_DISCOVERABLE) { + if (test->setup_limited_discov) { + discov_param[0] = 0x02; + discov_param[1] = 0x01; + } + param_size = sizeof(discov_param); + param = discov_param; + } + + if (*cmd == MGMT_OP_SET_PRIVACY) { + param_size = sizeof(privacy_param); + param = privacy_param; + } + + if (*cmd == MGMT_OP_START_DISCOVERY) { + if (test->setup_discovery_param) + memcpy(param, test->setup_discovery_param, 1); + } + + if (*cmd == MGMT_OP_SET_EXP_FEATURE) { + if (test->setup_exp_feat_param) { + memcpy(set_exp_feat_param, + test->setup_exp_feat_param, 17); + param_size = sizeof(set_exp_feat_param); + param = set_exp_feat_param; + } + } + + if (*cmd == MGMT_OP_SET_LE && test->setup_nobredr) { + unsigned char off[] = { 0x00 }; + + tester_print("Setup sending %s (0x%04x)", + mgmt_opstr(*cmd), *cmd); + mgmt_send(data->mgmt, *cmd, data->mgmt_index, + param_size, param, NULL, NULL, NULL); + tester_print("Setup sending %s (0x%04x)", + mgmt_opstr(MGMT_OP_SET_BREDR), + MGMT_OP_SET_BREDR); + mgmt_send(data->mgmt, MGMT_OP_SET_BREDR, + data->mgmt_index, sizeof(off), off, + func, data, NULL); + } else { + tester_print("Setup sending %s (0x%04x)", + mgmt_opstr(*cmd), *cmd); + mgmt_send(data->mgmt, *cmd, data->mgmt_index, + param_size, param, func, data, NULL); + } + } +} + +static const struct generic_data enable_mesh_1 = { + .send_opcode = MGMT_OP_SET_EXP_FEATURE, + .send_param = set_exp_feat_param_mesh, + .send_len = sizeof(set_exp_feat_param_mesh), + .expect_param = set_exp_feat_rsp_param_mesh, + .expect_len = sizeof(set_exp_feat_rsp_param_mesh), + .expect_status = MGMT_STATUS_SUCCESS, +}; + +static const uint8_t set_mesh_receiver_1[] = { + 0x01, + 0x6e, 0x01, + 0xe8, 0x01, + 0x03, + 0x2a, 0x2b, 0x29 +}; + +static const struct generic_data enable_mesh_2 = { + .send_opcode = MGMT_OP_SET_MESH_RECEIVER, + .send_param = set_mesh_receiver_1, + .send_len = sizeof(set_mesh_receiver_1), + .expect_status = MGMT_STATUS_SUCCESS, +}; + +static const uint8_t read_mesh_feat_rsp_param_mesh[] = { + 0x00, 0x00, + 0x03, + 0x00 +}; + +static const uint8_t read_mesh_feat_rsp_param_mesh_disabled[] = { + 0x00, 0x00, + 0x00, + 0x00 +}; + +static const struct generic_data read_mesh_features = { + .send_opcode = MGMT_OP_MESH_READ_FEATURES, + .expect_param = read_mesh_feat_rsp_param_mesh, + .expect_len = sizeof(read_mesh_feat_rsp_param_mesh), + .expect_status = MGMT_STATUS_SUCCESS, +}; + +static const struct generic_data read_mesh_features_disabled = { + .send_opcode = MGMT_OP_MESH_READ_FEATURES, + .expect_param = read_mesh_feat_rsp_param_mesh_disabled, + .expect_len = sizeof(read_mesh_feat_rsp_param_mesh_disabled), + .expect_status = MGMT_STATUS_SUCCESS, +}; + +static const uint8_t send_mesh_1[] = { + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + BDADDR_LE_RANDOM, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, + 0x03, + 0x18, + 0x17, 0x2b, 0x01, 0x00, 0x2d, 0xda, 0x0c, 0x24, + 0x91, 0x53, 0x7a, 0xe2, 0x00, 0x00, 0x00, 0x00, + 0x9d, 0xe2, 0x12, 0x0a, 0x72, 0x50, 0x38, 0xb2 +}; + +static const uint8_t send_mesh_too_long[] = { + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + BDADDR_LE_RANDOM, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, + 0x03, + 0x28, + 0x17, 0x2b, 0x01, 0x00, 0x2d, 0xda, 0x0c, 0x24, + 0x91, 0x53, 0x7a, 0xe2, 0x00, 0x00, 0x00, 0x00, + 0x91, 0x53, 0x7a, 0xe2, 0x00, 0x00, 0x00, 0x00, + 0x91, 0x53, 0x7a, 0xe2, 0x00, 0x00, 0x00, 0x00, + 0x9d, 0xe2, 0x12, 0x0a, 0x72, 0x50, 0x38, 0xb2 +}; + +static const uint8_t mesh_send_rsp_param_mesh[] = { + 0x01 +}; + +static const struct generic_data mesh_send_mesh_1 = { + .send_opcode = MGMT_OP_MESH_SEND, + .send_param = send_mesh_1, + .send_len = sizeof(send_mesh_1), + .expect_param = mesh_send_rsp_param_mesh, + .expect_len = sizeof(mesh_send_rsp_param_mesh), + .expect_status = MGMT_STATUS_SUCCESS, + .expect_alt_ev = MGMT_EV_MESH_PACKET_CMPLT, + .expect_alt_ev_param = mesh_send_rsp_param_mesh, + .expect_alt_ev_len = sizeof(mesh_send_rsp_param_mesh), + .expect_hci_command = BT_HCI_CMD_LE_SET_ADV_ENABLE, + .expect_hci_param = mesh_send_rsp_param_mesh, + .expect_hci_len = sizeof(mesh_send_rsp_param_mesh), +}; + +static const struct generic_data mesh_send_mesh_too_short = { + .send_opcode = MGMT_OP_MESH_SEND, + .send_param = send_mesh_1, + .send_len = sizeof(send_mesh_1) - 30, + .expect_status = MGMT_STATUS_INVALID_PARAMS +}; + +static const struct generic_data mesh_send_mesh_too_long = { + .send_opcode = MGMT_OP_MESH_SEND, + .send_param = send_mesh_too_long, + .send_len = sizeof(send_mesh_too_long), + .expect_status = MGMT_STATUS_REJECTED +}; + +static void setup_powered_callback(uint8_t status, uint16_t length, + const void *param, void *user_data) +{ + if (status != MGMT_STATUS_SUCCESS) { + tester_setup_failed(); + return; + } + + tester_print("Controller powered on"); + + setup_bthost(); +} + +static const char set_le_on_param[] = { 0x01 }; + +static void setup_enable_mesh(const void *test_data) +{ + struct test_data *data = tester_get_data(); + unsigned char param[] = { 0x01 }; + + mgmt_send(data->mgmt, MGMT_OP_SET_POWERED, data->mgmt_index, + sizeof(param), param, + setup_powered_callback, NULL, NULL); + + mgmt_send(data->mgmt, MGMT_OP_SET_LE, data->mgmt_index, + sizeof(set_le_on_param), set_le_on_param, + NULL, NULL, NULL); + + mgmt_send(data->mgmt, MGMT_OP_SET_EXP_FEATURE, data->mgmt_index, + sizeof(set_exp_feat_param_mesh), + set_exp_feat_param_mesh, + mesh_exp_callback, NULL, NULL); +} + +static const uint8_t send_mesh_cancel_1[] = { + 0x01 +}; + +static const uint8_t send_mesh_cancel_2[] = { + 0x02 +}; + +static const uint8_t mesh_cancel_rsp_param_mesh[] = { + 0x00 +}; + +static const struct generic_data mesh_send_mesh_cancel_1 = { + .send_opcode = MGMT_OP_MESH_SEND_CANCEL, + .send_param = send_mesh_cancel_1, + .send_len = sizeof(send_mesh_cancel_1), + .expect_status = MGMT_STATUS_SUCCESS, + .expect_alt_ev = MGMT_EV_MESH_PACKET_CMPLT, + .expect_alt_ev_param = send_mesh_cancel_1, + .expect_alt_ev_len = sizeof(send_mesh_cancel_1), + + .expect_hci_command = BT_HCI_CMD_LE_SET_ADV_ENABLE, + .expect_hci_param = mesh_cancel_rsp_param_mesh, + .expect_hci_len = sizeof(mesh_cancel_rsp_param_mesh), +}; + +static const struct generic_data mesh_send_mesh_cancel_2 = { + .send_opcode = MGMT_OP_MESH_SEND_CANCEL, + .send_param = send_mesh_cancel_2, + .send_len = sizeof(send_mesh_cancel_2), + .expect_status = MGMT_STATUS_SUCCESS, + .expect_alt_ev = MGMT_EV_MESH_PACKET_CMPLT, + .expect_alt_ev_param = send_mesh_cancel_2, + .expect_alt_ev_len = sizeof(send_mesh_cancel_2), + + .expect_hci_command = BT_HCI_CMD_LE_SET_ADV_ENABLE, + .expect_hci_param = mesh_cancel_rsp_param_mesh, + .expect_hci_len = sizeof(mesh_cancel_rsp_param_mesh), +}; + +static void setup_multi_mesh_send(const void *test_data) +{ + struct test_data *data = tester_get_data(); + + setup_enable_mesh(test_data); + + mgmt_send(data->mgmt, MGMT_OP_MESH_SEND, data->mgmt_index, + sizeof(send_mesh_1), send_mesh_1, + NULL, NULL, NULL); + + mgmt_send(data->mgmt, MGMT_OP_MESH_SEND, data->mgmt_index, + sizeof(send_mesh_1), send_mesh_1, + NULL, NULL, NULL); +} + +int main(int argc, char *argv[]) +{ + tester_init(&argc, &argv); + + test_bredrle("Controller setup", + NULL, NULL, controller_setup); + + /* LL Mesh Enable + * Setup: None + * Run: Send Enable Experimental Feature (mesh) + * Expect: Mesh feature enable success + */ + test_bredrle("Mesh - Enable 1", + &enable_mesh_1, + NULL, + test_command_generic); + + test_bredrle("Mesh - Enable 2", + &enable_mesh_2, + setup_enable_mesh, + test_command_generic); + + test_bredrle("Mesh - Read Mesh Features", + &read_mesh_features, + setup_enable_mesh, + test_command_generic); + + test_bredrle("Mesh - Read Mesh Features - Disabled", + &read_mesh_features_disabled, + NULL, + test_command_generic); + + test_bredrle("Mesh - Send", + &mesh_send_mesh_1, + setup_enable_mesh, + test_command_generic); + + test_bredrle("Mesh - Send - too short", + &mesh_send_mesh_too_short, + setup_enable_mesh, + test_command_generic); + + test_bredrle("Mesh - Send - too long", + &mesh_send_mesh_too_long, + setup_enable_mesh, + test_command_generic); + + test_bredrle("Mesh - Send cancel - 1", + &mesh_send_mesh_cancel_1, + setup_multi_mesh_send, + test_command_generic); + + test_bredrle("Mesh - Send cancel - 2", + &mesh_send_mesh_cancel_2, + setup_multi_mesh_send, + test_command_generic); + + return tester_run(); +} diff -pruN 5.65-1/tools/mgmt-tester.c 5.66-1/tools/mgmt-tester.c --- 5.65-1/tools/mgmt-tester.c 2022-07-24 20:59:52.000000000 +0000 +++ 5.66-1/tools/mgmt-tester.c 2022-11-10 20:22:09.000000000 +0000 @@ -14,6 +14,7 @@ #include #include +#include #include #include #include @@ -33,6 +34,7 @@ #include "emulator/vhci.h" #include "emulator/bthost.h" #include "emulator/hciemu.h" +#include "emulator/btdev.h" #include "src/shared/util.h" #include "src/shared/tester.h" @@ -221,6 +223,32 @@ static void read_info_callback(uint8_t s bthost_notify_ready(bthost, tester_pre_setup_complete); } +static const uint8_t set_exp_feat_param_mesh[] = { + 0x76, 0x6e, 0xf3, 0xe8, 0x24, 0x5f, 0x05, 0xbf, /* UUID - Mesh */ + 0x8d, 0x4d, 0x03, 0x7a, 0xd7, 0x63, 0xe4, 0x2c, + 0x01, /* Action - enable */ +}; + +static void mesh_exp_callback(uint8_t status, uint16_t length, + const void *param, void *user_data) +{ + if (status != MGMT_STATUS_SUCCESS) { + tester_print("Mesh feature could not be enabled"); + return; + } + + tester_print("Mesh feature is enabled"); +} + +static void mesh_exp_feature(struct test_data *data, uint16_t index) +{ + tester_print("Enabling Mesh feature"); + + mgmt_send(data->mgmt, MGMT_OP_SET_EXP_FEATURE, index, + sizeof(set_exp_feat_param_mesh), set_exp_feat_param_mesh, + mesh_exp_callback, NULL, NULL); +} + static void index_added_callback(uint16_t index, uint16_t length, const void *param, void *user_data) { @@ -233,6 +261,10 @@ static void index_added_callback(uint16_ mgmt_send(data->mgmt, MGMT_OP_READ_INFO, data->mgmt_index, 0, NULL, read_info_callback, NULL, NULL); + + tester_warn("Enable management Mesh interface"); + mesh_exp_feature(data, data->mgmt_index); + } static void index_removed_callback(uint16_t index, uint16_t length, @@ -269,6 +301,7 @@ struct hci_entry { }; struct generic_data { + bdaddr_t *setup_bdaddr; bool setup_le_states; const uint8_t *le_states; const uint16_t *setup_settings; @@ -386,8 +419,19 @@ static void read_index_list_callback(uin if (tester_use_debug()) hciemu_set_debug(data->hciemu, print_debug, "hciemu: ", NULL); + if (test && test->setup_bdaddr) { + struct vhci *vhci = hciemu_get_vhci(data->hciemu); + struct btdev *btdev = vhci_get_btdev(vhci); + + if (!btdev_set_bdaddr(btdev, test->setup_bdaddr->b)) { + tester_warn("btdev_set_bdaddr failed"); + tester_pre_setup_failed(); + } + } + if (test && test->setup_le_states) hciemu_set_central_le_states(data->hciemu, test->le_states); + } static void test_pre_setup(const void *test_data) @@ -4024,27 +4068,41 @@ static const struct generic_data unblock static const char set_static_addr_valid_param[] = { 0x11, 0x22, 0x33, 0x44, 0x55, 0xc0 }; -static const char set_static_addr_settings[] = { 0x00, 0x82, 0x00, 0x00 }; +static const char set_static_addr_settings_param[] = { 0x01, 0x82, 0x00, 0x00 }; static const struct generic_data set_static_addr_success_test = { - .send_opcode = MGMT_OP_SET_STATIC_ADDRESS, - .send_param = set_static_addr_valid_param, - .send_len = sizeof(set_static_addr_valid_param), + .setup_bdaddr = BDADDR_ANY, + .setup_send_opcode = MGMT_OP_SET_STATIC_ADDRESS, + .setup_send_param = set_static_addr_valid_param, + .setup_send_len = sizeof(set_static_addr_valid_param), + .send_opcode = MGMT_OP_SET_POWERED, + .send_param = set_powered_on_param, + .send_len = sizeof(set_powered_on_param), .expect_status = MGMT_STATUS_SUCCESS, - .expect_param = set_static_addr_settings, - .expect_len = sizeof(set_static_addr_settings), + .expect_param = set_static_addr_settings_param, + .expect_len = sizeof(set_static_addr_settings_param), .expect_settings_set = MGMT_SETTING_STATIC_ADDRESS, + .expect_hci_command = BT_HCI_CMD_LE_SET_RANDOM_ADDRESS, + .expect_hci_param = set_static_addr_valid_param, + .expect_hci_len = sizeof(set_static_addr_valid_param), }; -static const char set_static_addr_settings_dual[] = { 0x80, 0x00, 0x00, 0x00 }; +static const char set_static_addr_settings_dual[] = { 0x81, 0x80, 0x00, 0x00 }; static const struct generic_data set_static_addr_success_test_2 = { - .send_opcode = MGMT_OP_SET_STATIC_ADDRESS, - .send_param = set_static_addr_valid_param, - .send_len = sizeof(set_static_addr_valid_param), + .setup_send_opcode = MGMT_OP_SET_STATIC_ADDRESS, + .setup_send_param = set_static_addr_valid_param, + .setup_send_len = sizeof(set_static_addr_valid_param), + .send_opcode = MGMT_OP_SET_POWERED, + .send_param = set_powered_on_param, + .send_len = sizeof(set_powered_on_param), .expect_status = MGMT_STATUS_SUCCESS, .expect_param = set_static_addr_settings_dual, .expect_len = sizeof(set_static_addr_settings_dual), + .expect_settings_set = MGMT_SETTING_STATIC_ADDRESS, + .expect_hci_command = BT_HCI_CMD_LE_SET_RANDOM_ADDRESS, + .expect_hci_param = set_static_addr_valid_param, + .expect_hci_len = sizeof(set_static_addr_valid_param), }; static const struct generic_data set_static_addr_failure_test = { @@ -7390,7 +7448,8 @@ static void command_generic_callback(uin expect_param = test->expect_func(&expect_len); if (length != expect_len) { - tester_warn("Invalid cmd response parameter size"); + tester_warn("Invalid cmd response parameter size %d %d", + length, expect_len); tester_test_failed(); return; } @@ -9858,7 +9917,7 @@ static const struct generic_data set_dev }; static const uint8_t read_exp_feat_param_success[] = { - 0x04, 0x00, /* Feature Count */ + 0x05, 0x00, /* Feature Count */ 0xd6, 0x49, 0xb0, 0xd1, 0x28, 0xeb, /* UUID - Simultaneous */ 0x27, 0x92, 0x96, 0x46, 0xc0, 0x42, /* Central Peripheral */ 0xb5, 0x10, 0x1b, 0x67, @@ -9875,6 +9934,10 @@ static const uint8_t read_exp_feat_param 0x85, 0x98, 0x6a, 0x49, 0xe0, 0x05, 0x88, 0xf1, 0xba, 0x6f, 0x00, 0x00, 0x00, 0x00, /* Flags */ + 0x76, 0x6e, 0xf3, 0xe8, 0x24, 0x5f, /* UUID - Mesh support */ + 0x05, 0xbf, 0x8d, 0x4d, 0x03, 0x7a, + 0xd7, 0x63, 0xe4, 0x2c, + 0x01, 0x00, 0x00, 0x00, /* Flags */ }; static const struct generic_data read_exp_feat_success = { @@ -11344,6 +11407,23 @@ static void test_command_generic(const v test_add_condition(data); } +static void setup_set_static_addr_success_2(const void *test_data) +{ + struct test_data *data = tester_get_data(); + struct vhci *vhci = hciemu_get_vhci(data->hciemu); + int err; + + /* Force use of static address */ + err = vhci_set_force_static_address(vhci, true); + if (err) { + tester_warn("Unable to set force_static_address: %s (%d)", + strerror(-err), -err); + tester_test_failed(); + return; + } + setup_command_generic(test_data); +} + static void check_scan(void *user_data) { struct test_data *data = tester_get_data(); @@ -13191,10 +13271,11 @@ int main(int argc, char *argv[]) test_le("Set Static Address - Success 1", &set_static_addr_success_test, - NULL, test_command_generic); + setup_command_generic, test_command_generic); test_bredrle("Set Static Address - Success 2", &set_static_addr_success_test_2, - NULL, test_command_generic); + setup_set_static_addr_success_2, + test_command_generic); test_bredrle("Set Static Address - Failure 1", &set_static_addr_failure_test, NULL, test_command_generic); diff -pruN 5.65-1/tools/mpris-proxy.c 5.66-1/tools/mpris-proxy.c --- 5.65-1/tools/mpris-proxy.c 2022-07-24 20:59:52.000000000 +0000 +++ 5.66-1/tools/mpris-proxy.c 2022-11-10 20:22:09.000000000 +0000 @@ -480,6 +480,7 @@ static void add_player(DBusConnection *c reply = dbus_connection_send_with_reply_and_block(sys, msg, -1, &err); if (!reply) { fprintf(stderr, "Can't register player\n"); + dbus_connection_unregister_object_path(sys, path); free(owner); if (dbus_error_is_set(&err)) { fprintf(stderr, "%s\n", err.message); diff -pruN 5.65-1/tools/rctest.c 5.66-1/tools/rctest.c --- 5.65-1/tools/rctest.c 2022-07-24 20:59:52.000000000 +0000 +++ 5.66-1/tools/rctest.c 2022-11-10 20:22:09.000000000 +0000 @@ -94,7 +94,7 @@ static float tv2fl(struct timeval tv) static uint8_t get_channel(const char *svr, uint16_t uuid) { sdp_session_t *sdp; - sdp_list_t *srch, *attrs, *rsp, *protos; + sdp_list_t *srch, *attrs, *rsp, *protos = NULL; uuid_t svclass; uint16_t attr; bdaddr_t dst; @@ -500,8 +500,9 @@ static void recv_mode(int sk) timestamp = 0; memset(ts, 0, sizeof(ts)); } else { - sprintf(ts, "[%ld.%ld] ", - tv.tv_sec, tv.tv_usec); + sprintf(ts, "[%lld.%lld] ", + (long long)tv.tv_sec, + (long long)tv.tv_usec); } } diff -pruN 5.65-1/tools/rfcomm-tester.c 5.66-1/tools/rfcomm-tester.c --- 5.65-1/tools/rfcomm-tester.c 2022-07-24 20:59:52.000000000 +0000 +++ 5.66-1/tools/rfcomm-tester.c 2022-11-10 20:22:09.000000000 +0000 @@ -47,6 +47,7 @@ struct test_data { struct rfcomm_client_data { uint8_t server_channel; uint8_t client_channel; + bool close; int expected_connect_err; const uint8_t *send_data; const uint8_t *read_data; @@ -297,6 +298,12 @@ const struct rfcomm_client_data connect_ .client_channel = 0x0c }; +const struct rfcomm_client_data connect_close = { + .server_channel = 0x0c, + .client_channel = 0x0c, + .close = true +}; + const uint8_t data[] = {0, 1, 2, 3, 4, 5, 6, 7, 8}; const struct rfcomm_client_data connect_send_success = { @@ -519,6 +526,8 @@ static gboolean rc_connect_cb(GIOChannel return false; } + data->io = NULL; + if (err < 0) tester_test_failed(); else @@ -527,6 +536,20 @@ static gboolean rc_connect_cb(GIOChannel return false; } +static gboolean rc_close_cb(GIOChannel *io, GIOCondition cond, + gpointer user_data) +{ + struct test_data *data = tester_get_data(); + + data->io_id = 0; + + tester_print("Closed"); + + tester_test_passed(); + + return false; +} + static void client_hook_func(const void *data, uint16_t len, void *user_data) { @@ -627,13 +650,20 @@ static void test_connect(const void *tes } io = g_io_channel_unix_new(sk); - g_io_channel_set_close_on_unref(io, TRUE); - data->io_id = g_io_add_watch(io, G_IO_OUT, rc_connect_cb, NULL); + tester_print("Connect in progress %d", sk); + + if (cli->close) { + data->io_id = g_io_add_watch(io, G_IO_NVAL, rc_close_cb, NULL); + close(sk); + tester_print("Close socket %d", sk); + } else { + g_io_channel_set_close_on_unref(io, TRUE); + data->io_id = g_io_add_watch(io, G_IO_OUT, rc_connect_cb, + NULL); + } g_io_channel_unref(io); - - tester_print("Connect in progress %d", sk); } static gboolean server_received_data(GIOChannel *io, GIOCondition cond, @@ -788,7 +818,7 @@ static void test_server(const void *test user = malloc(sizeof(struct test_data)); \ if (!user) \ break; \ - user->hciemu_type = HCIEMU_TYPE_BREDR; \ + user->hciemu_type = HCIEMU_TYPE_BREDRLE52; \ user->test_data = data; \ user->io_id = 0; \ tester_add_full(name, data, \ @@ -815,6 +845,9 @@ int main(int argc, char *argv[]) test_connect); test_rfcomm("Basic RFCOMM Socket Client - Conn Refused", &connect_nval, setup_powered_client, test_connect); + test_rfcomm("Basic RFCOMM Socket Client - Close", + &connect_close, setup_powered_client, + test_connect); test_rfcomm("Basic RFCOMM Socket Server - Success", &listen_success, setup_powered_server, test_server); test_rfcomm("Basic RFCOMM Socket Server - Write Success", diff -pruN 5.65-1/tools/test-runner.c 5.66-1/tools/test-runner.c --- 5.65-1/tools/test-runner.c 2022-07-24 20:59:52.000000000 +0000 +++ 5.66-1/tools/test-runner.c 2022-11-10 20:22:09.000000000 +0000 @@ -250,7 +250,6 @@ static void start_qemu(void) "rootfstype=9p " "rootflags=trans=virtio,version=9p2000.u " "acpi=off pci=noacpi noapic quiet ro init=%s " - "bluetooth.enable_ecred=1 " "TESTHOME=%s TESTDBUS=%u TESTDAEMON=%u " "TESTDBUSSESSION=%u XDG_RUNTIME_DIR=/run/user/0 " "TESTAUDIO=%u " @@ -614,6 +613,8 @@ static const char *test_table[] = { "rfcomm-tester", "sco-tester", "iso-tester", + "mesh-tester", + "ioctl-tester", "bnep-tester", "check-selftest", "tools/mgmt-tester", @@ -622,6 +623,8 @@ static const char *test_table[] = { "tools/rfcomm-tester", "tools/sco-tester", "tools/iso-tester", + "tools/mesh-tester", + "tools/ioctl-tester", "tools/bnep-tester", "tools/check-selftest", NULL diff -pruN 5.65-1/tools/valgrind.supp 5.66-1/tools/valgrind.supp --- 5.65-1/tools/valgrind.supp 2022-07-24 20:59:52.000000000 +0000 +++ 5.66-1/tools/valgrind.supp 2022-11-10 20:22:09.000000000 +0000 @@ -25,3 +25,10 @@ socketcall.bind(my_addr.rc_channel) fun:bind } +{ + bt_log_open + Memcheck:Param + socketcall.bind(my_addr.rc_bdaddr) + fun:bind + fun:bt_log_open +} diff -pruN 5.65-1/unit/test-tester.c 5.66-1/unit/test-tester.c --- 5.65-1/unit/test-tester.c 1970-01-01 00:00:00.000000000 +0000 +++ 5.66-1/unit/test-tester.c 2022-11-10 20:22:09.000000000 +0000 @@ -0,0 +1,93 @@ +// SPDX-License-Identifier: GPL-2.0-or-later +/* + * + * BlueZ - Bluetooth protocol stack for Linux + * + * Copyright (C) 2022 Intel Corporation. + * + * + */ + +#ifdef HAVE_CONFIG_H +#include +#endif + +#define _GNU_SOURCE +#include +#include +#include +#include + +#include + +#include "src/shared/io.h" +#include "src/shared/util.h" +#include "src/shared/tester.h" + +static void test_basic(const void *data) +{ + tester_test_passed(); +} + +static bool test_io_recv(struct io *io, void *user_data) +{ + const struct iovec *iov = user_data; + unsigned char buf[512]; + int fd; + ssize_t len; + + fd = io_get_fd(io); + + len = read(fd, buf, sizeof(buf)); + + g_assert(len > 0); + g_assert_cmpint(len, ==, iov->iov_len); + g_assert(memcmp(buf, iov->iov_base, len) == 0); + + tester_test_passed(); + + return false; +} + +static const struct iovec iov[] = { + IOV_DATA(0x01), + IOV_DATA(0x01, 0x02), +}; + +static void test_setup_io(const void *data) +{ + struct io *io; + ssize_t len; + + io = tester_setup_io(iov, ARRAY_SIZE(iov)); + g_assert(io); + + io_set_read_handler(io, test_io_recv, (void *)&iov[1], NULL); + + len = io_send(io, (void *)&iov[0], 1); + g_assert_cmpint(len, ==, iov[0].iov_len); +} + +static void test_io_send(const void *data) +{ + struct io *io; + + io = tester_setup_io(iov, ARRAY_SIZE(iov)); + g_assert(io); + + io_set_read_handler(io, test_io_recv, (void *)&iov[0], NULL); + + tester_io_send(); +} + +int main(int argc, char *argv[]) +{ + tester_init(&argc, &argv); + + tester_add("/tester/basic", NULL, NULL, test_basic, NULL); + tester_add("/tester/setup_io", NULL, NULL, test_setup_io, NULL); + tester_add("/tester/io_send", NULL, NULL, test_io_send, NULL); + + return tester_run(); +} +