From 42c7ab0baecaa56739d6e1ba73506d4d3d8a72ed Mon Sep 17 00:00:00 2001 From: Andrew Zaborowski Date: Tue, 9 Jul 2019 03:24:36 +0200 Subject: [PATCH] p2putil: Add P2P attribute parsers Define structs and types for most P2P attributes and p2p_parse_attrs similar to wsc_parse_attrs -- a generic parser for attributes in a P2P IE payload. This parser may write into the provided buffer even on error but it's private to p2putil.c. The local callers will take care of keeping the user-provided buffers untouched on error. --- src/p2putil.c | 684 ++++++++++++++++++++++++++++++++++++++++++++++++++ src/p2putil.h | 104 ++++++++ 2 files changed, 788 insertions(+) diff --git a/src/p2putil.c b/src/p2putil.c index 014907d5..5481cb98 100644 --- a/src/p2putil.c +++ b/src/p2putil.c @@ -25,10 +25,14 @@ #endif #include +#include +#include +#include #include #include "src/p2putil.h" +#include "src/ie.h" void p2p_attr_iter_init(struct p2p_attr_iter *iter, const uint8_t *pdu, size_t len) @@ -39,6 +43,7 @@ void p2p_attr_iter_init(struct p2p_attr_iter *iter, const uint8_t *pdu, iter->type = -1; } +/* Wi-Fi P2P Technical Specification v1.7 Section 4.1.1 */ bool p2p_attr_iter_next(struct p2p_attr_iter *iter) { if (iter->type != (enum p2p_attr) -1) @@ -52,3 +57,682 @@ bool p2p_attr_iter_next(struct p2p_attr_iter *iter) iter->len = l_get_le16(iter->pos + 1); return true; } + +enum attr_flag { + ATTR_FLAG_REQUIRED = 0x1, /* Always required */ +}; + +typedef bool (*attr_handler)(const uint8_t *, size_t, void *); + +static bool extract_p2p_byte(const uint8_t *attr, size_t len, + void *data) +{ + uint8_t *out = data; + + if (len != 1) + return false; + + *out = attr[0]; + return true; +} + +/* Section 4.1.2 */ +static bool extract_p2p_status(const uint8_t *attr, size_t len, + void *data) +{ + enum p2p_attr_status_code *out = data; + + if (len != 1) + return false; + + *out = attr[0]; + return true; +} + +/* Section 4.1.4 */ +static bool extract_p2p_capability(const uint8_t *attr, size_t len, + void *data) +{ + struct p2p_capability_attr *out = data; + + if (len != 2) + return false; + + out->device_caps = attr[0]; + out->group_caps = attr[1]; + return true; +} + +/* Section 4.1.5, 4.1.9, 4.1.11, ... */ +static bool extract_p2p_addr(const uint8_t *attr, size_t len, + void *data) +{ + uint8_t *out = data; + + if (len != 6) + return false; + + memcpy(out, attr, 6); + return true; +} + +struct p2p_go_intent_attr { + uint8_t intent; + bool tie_breaker; +}; + +/* Section 4.1.6 */ +static bool extract_p2p_go_intent(const uint8_t *attr, size_t len, + void *data) +{ + struct p2p_go_intent_attr *out = data; + uint8_t intent; + + if (len != 1) + return false; + + intent = attr[0] >> 1; + + if (intent & ~15) + return false; + + out->intent = intent; + out->tie_breaker = attr[0] & 1; + + return true; +} + +/* Section 4.1.7 */ +static bool extract_p2p_config_timeout(const uint8_t *attr, size_t len, + void *data) +{ + struct p2p_config_timeout_attr *out = data; + + if (len != 2) + return false; + + out->go_config_timeout = attr[0]; + out->client_config_timeout = attr[1]; + return true; +} + +/* Section 4.1.8, 4.1.19, ... */ +static bool extract_p2p_channel(const uint8_t *attr, size_t len, + void *data) +{ + struct p2p_channel_attr *out = data; + + if (len != 5) + return false; + + out->country[0] = attr[0]; + out->country[1] = attr[1]; + out->country[2] = attr[2]; + out->oper_class = attr[3]; + out->channel_num = attr[4]; + return true; +} + +/* Section 4.1.10 */ +static bool extract_p2p_listen_timing(const uint8_t *attr, size_t len, + void *data) +{ + struct p2p_extended_listen_timing_attr *out = data; + + if (len != 4) + return false; + + out->avail_period_ms = l_get_le16(attr + 0); + out->avail_interval_ms = l_get_le16(attr + 2); + return true; +} + +/* Section 4.1.13 */ +static bool extract_p2p_channel_list(const uint8_t *attr, size_t len, + void *data) +{ + struct p2p_channel_list_attr *out = data; + + if (len < 6) + return false; + + out->country[0] = *attr++; + out->country[1] = *attr++; + out->country[2] = *attr++; + len -= 3; + + out->channel_entries = l_queue_new(); + + while (len) { + struct p2p_channel_entries *entries; + + if (len < 2 || len < (size_t) 2 + attr[1]) { + l_queue_destroy(out->channel_entries, l_free); + out->channel_entries = NULL; + return false; + } + + entries = l_malloc(sizeof(struct p2p_channel_entries) + attr[1]); + entries->oper_class = *attr++; + entries->n_channels = *attr++; + memcpy(entries->channels, attr, entries->n_channels); + l_queue_push_tail(out->channel_entries, entries); + + attr += entries->n_channels; + len -= entries->n_channels; + } + + return true; +} + +/* Section 4.1.14 */ +static bool extract_p2p_notice_of_absence(const uint8_t *attr, size_t len, + void *data) +{ + struct p2p_notice_of_absence_attr *out = data; + uint8_t index; + uint8_t ct_window; + bool opp_ps; + + if (len % 13 != 2) + return false; + + index = *attr++; + ct_window = *attr & 127; + opp_ps = *attr++ >> 7; + len -= 2; + + if (opp_ps && !ct_window) + return false; + + out->index = index; + out->opp_ps = opp_ps; + out->ct_window = ct_window; + out->descriptors = l_queue_new(); + + while (len) { + struct p2p_notice_of_absence_desc *desc; + + desc = l_new(struct p2p_notice_of_absence_desc, 1); + desc->count_type = attr[0]; + desc->duration = l_get_le32(attr + 1); + desc->interval = l_get_le32(attr + 5); + desc->start_time = l_get_le32(attr + 9); + l_queue_push_tail(out->descriptors, desc); + + attr += 13; + len -= 13; + } + + return true; +} + +/* Section 4.1.15 */ +static bool extract_p2p_device_info(const uint8_t *attr, size_t len, + void *data) +{ + struct p2p_device_info_attr *out = data; + int r; + int name_len; + int i; + int types_num; + + if (len < 21) + return false; + + memcpy(out->device_addr, attr + 0, 6); + out->wsc_config_methods = l_get_be16(attr + 6); + + r = wsc_parse_primary_device_type(attr + 8, 8, + &out->primary_device_type); + if (r < 0) + return false; + + types_num = attr[16]; + if (len < 17u + types_num * 8 + 4) + return false; + + if (l_get_be16(attr + 17 + types_num * 8) != WSC_ATTR_DEVICE_NAME) + return false; + + name_len = l_get_be16(attr + 17 + types_num * 8 + 2); + if (len < 17u + types_num * 8 + 4 + name_len || name_len > 32) + return false; + + out->secondary_device_types = l_queue_new(); + + for (i = 0; i < types_num; i++) { + struct wsc_primary_device_type *device_type = + l_new(struct wsc_primary_device_type, 1); + + l_queue_push_tail(out->secondary_device_types, device_type); + + r = wsc_parse_primary_device_type(attr + 17 + i * 8, 8, + device_type); + if (r < 0) { + l_queue_destroy(out->secondary_device_types, l_free); + out->secondary_device_types = NULL; + return false; + } + } + + memcpy(out->device_name, attr + 17 + types_num * 8 + 4, name_len); + + return true; +} + +static void p2p_free_client_info_descriptor(void *data) +{ + struct p2p_client_info_descriptor *desc = data; + + l_queue_destroy(desc->secondary_device_types, l_free); + l_free(desc); +} + +/* Section 4.1.16 */ +static bool extract_p2p_group_info(const uint8_t *attr, size_t len, + void *data) +{ + struct l_queue **out = data; + + while (len) { + uint8_t desc_len = *attr++; + struct p2p_client_info_descriptor *desc; + int r, name_len, i, types_num; + + if (len < 1u + desc_len) + goto error; + + if (!*out) + *out = l_queue_new(); + + desc = l_new(struct p2p_client_info_descriptor, 1); + l_queue_push_tail(*out, desc); + + memcpy(desc->device_addr, attr + 0, 6); + memcpy(desc->interface_addr, attr + 6, 6); + desc->device_caps = attr[12]; + desc->wsc_config_methods = l_get_be16(attr + 13); + + r = wsc_parse_primary_device_type(attr + 15, 8, + &desc->primary_device_type); + if (r < 0) + goto error; + + types_num = attr[23]; + if (desc_len < 24 + types_num * 8 + 4) + goto error; + + if (l_get_be16(attr + 24 + types_num * 8) != + WSC_ATTR_DEVICE_NAME) + goto error; + + name_len = l_get_be16(attr + 24 + types_num * 8 + 2); + if (desc_len < 24 + types_num * 8 + 4 + name_len || + name_len > 32) + goto error; + + desc->secondary_device_types = l_queue_new(); + + for (i = 0; i < types_num; i++) { + struct wsc_primary_device_type *device_type = + l_new(struct wsc_primary_device_type, 1); + + l_queue_push_tail(desc->secondary_device_types, + device_type); + + r = wsc_parse_primary_device_type(attr + 24 + i * 8, 8, + device_type); + if (r < 0) + goto error; + } + + memcpy(desc->device_name, attr + 24 + types_num * 8 + 4, + name_len); + + attr += 24 + types_num * 8 + 4 + name_len; + len -= 1 + desc_len; + } + + return true; + +error: + l_queue_destroy(*out, p2p_free_client_info_descriptor); + *out = NULL; + + return false; +} + +/* Section 4.1.17, 4.1.29, ... */ +static bool extract_p2p_group_id(const uint8_t *attr, size_t len, + void *data) +{ + struct p2p_group_id_attr *out = data; + + if (len < 6 || len > 38) + return false; + + memcpy(out->device_addr, attr + 0, 6); + memcpy(out->ssid, attr + 6, len - 6); + return true; +} + +/* Section 4.1.18 */ +static bool extract_p2p_interface(const uint8_t *attr, size_t len, + void *data) +{ + struct p2p_interface_attr *out = data; + int addr_count; + + if (len < 7) + return false; + + addr_count = attr[6]; + + if (len < 7u + addr_count * 6) + return false; + + memcpy(out->device_addr, attr + 0, 6); + out->interface_addrs = l_queue_new(); + attr += 7; + + while (addr_count--) { + l_queue_push_tail(out->interface_addrs, l_memdup(attr, 6)); + attr += 6; + } + + return true; +} + +/* Section 4.1.20 */ +static bool extract_p2p_invitation_flags(const uint8_t *attr, size_t len, + void *data) +{ + bool *out = data; + + if (len != 1) + return false; + + *out = attr[0] & 1; /* Invitation Type flag */ + return true; +} + +/* Section 4.1.22 */ +static bool extract_p2p_service_hashes(const uint8_t *attr, size_t len, + void *data) +{ + struct l_queue **out = data; + + if (len % 6 != 0) + return false; + + *out = l_queue_new(); + + while (len) { + l_queue_push_tail(*out, l_memdup(attr, 6)); + attr += 6; + len -= 6; + } + + return true; +} + +/* Section 4.1.23 */ +static bool extract_p2p_session_info(const uint8_t *attr, size_t len, + void *data) +{ + struct p2p_session_info_data_attr *out = data; + + out->data_len = len; + memcpy(out->data, data, len); + return true; +} + +/* Section 4.1.25 */ +static bool extract_p2p_advertisement_id(const uint8_t *attr, size_t len, + void *data) +{ + struct p2p_advertisement_id_info_attr *out = data; + + if (len != 10) + return false; + + out->advertisement_id = l_get_le32(attr + 0); + memcpy(out->service_mac_addr, attr + 4, 6); + return true; +} + +static void p2p_free_advertised_service_descriptor(void *data) +{ + struct p2p_advertised_service_descriptor *desc = data; + + l_free(desc->service_name); + l_free(desc); +} + +/* Section 4.1.26 */ +static bool extract_p2p_advertised_service_info(const uint8_t *attr, size_t len, + void *data) +{ + struct l_queue **out = data; + + while (len) { + struct p2p_advertised_service_descriptor *desc; + int name_len; + + if (len < 7) + goto error; + + name_len = attr[6]; + if (len < 7u + name_len) + goto error; + + if (!l_utf8_validate((const char *) attr + 7, name_len, NULL)) + goto error; + + if (!*out) + *out = l_queue_new(); + + desc = l_new(struct p2p_advertised_service_descriptor, 1); + l_queue_push_tail(*out, desc); + + desc->advertisement_id = l_get_le32(attr + 0); + desc->wsc_config_methods = l_get_be16(attr + 4); + desc->service_name = l_strndup((const char *) attr + 7, + name_len); + + attr += 7 + name_len; + len -= 7 + name_len; + } + + return true; + +error: + l_queue_destroy(*out, p2p_free_advertised_service_descriptor); + return false; +} + +/* Section 4.1.27 */ +static bool extract_p2p_session_id(const uint8_t *attr, size_t len, void *data) +{ + struct p2p_session_id_info_attr *out = data; + + if (len != 10) + return false; + + out->session_id = l_get_le32(attr + 0); + memcpy(out->session_mac_addr, attr + 4, 6); + return true; +} + +/* Section 4.1.28 */ +static bool extract_p2p_feature_capability(const uint8_t *attr, size_t len, + void *data) +{ + enum p2p_asp_coordination_transport_protocol *out = data; + + if (len != 2) + return false; + + if (attr[0] == 0x01) + *out = P2P_ASP_TRANSPORT_UDP; + else + *out = P2P_ASP_TRANSPORT_UNKNOWN; + + return true; +} + +static attr_handler handler_for_type(enum p2p_attr type) +{ + switch (type) { + case P2P_ATTR_STATUS: + return extract_p2p_status; + case P2P_ATTR_MINOR_REASON_CODE: + return extract_p2p_byte; + case P2P_ATTR_P2P_CAPABILITY: + return extract_p2p_capability; + case P2P_ATTR_P2P_DEVICE_ID: + case P2P_ATTR_P2P_GROUP_BSSID: + case P2P_ATTR_INTENDED_P2P_INTERFACE_ADDR: + return extract_p2p_addr; + case P2P_ATTR_GO_INTENT: + return extract_p2p_go_intent; + case P2P_ATTR_CONFIGURATION_TIMEOUT: + return extract_p2p_config_timeout; + case P2P_ATTR_LISTEN_CHANNEL: + case P2P_ATTR_OPERATING_CHANNEL: + return extract_p2p_channel; + case P2P_ATTR_EXTENDED_LISTEN_TIMING: + return extract_p2p_listen_timing; + case P2P_ATTR_P2P_MANAGEABILITY: + break; + case P2P_ATTR_CHANNEL_LIST: + return extract_p2p_channel_list; + case P2P_ATTR_NOTICE_OF_ABSENCE: + return extract_p2p_notice_of_absence; + case P2P_ATTR_P2P_DEVICE_INFO: + return extract_p2p_device_info; + case P2P_ATTR_P2P_GROUP_INFO: + return extract_p2p_group_info; + case P2P_ATTR_P2P_GROUP_ID: + case P2P_ATTR_PERSISTENT_GROUP_INFO: + return extract_p2p_group_id; + case P2P_ATTR_P2P_INTERFACE: + return extract_p2p_interface; + case P2P_ATTR_INVITATION_FLAGS: + return extract_p2p_invitation_flags; + case P2P_ATTR_OOB_GO_NEGOTIATION_CHANNEL: + break; + case P2P_ATTR_SVC_HASH: + return extract_p2p_service_hashes; + case P2P_ATTR_SESSION_INFO_DATA_INFO: + return extract_p2p_session_info; + case P2P_ATTR_CONNECTION_CAPABILITY_INFO: + return extract_p2p_byte; + case P2P_ATTR_ADVERTISEMENT_ID_INFO: + return extract_p2p_advertisement_id; + case P2P_ATTR_ADVERTISED_SVC_INFO: + return extract_p2p_advertised_service_info; + case P2P_ATTR_SESSION_ID_INFO: + return extract_p2p_session_id; + case P2P_ATTR_FEATURE_CAPABILITY: + return extract_p2p_feature_capability; + case P2P_ATTR_VENDOR_SPECIFIC_ATTR: + break; + } + + return NULL; +} + +struct attr_handler_entry { + enum p2p_attr type; + unsigned int flags; + void *data; + bool present; +}; + +/* + * This function may find an error after having parsed part of the message + * and may have allocated memory so the output needs to be deallocated + * properly even on error return values. + */ +static int p2p_parse_attrs(const uint8_t *pdu, size_t len, int type, ...) +{ + struct p2p_attr_iter iter; + uint8_t *p2p_data; + ssize_t p2p_len; + struct l_queue *entries; + va_list args; + bool have_required = true; + bool parse_error = false; + struct attr_handler_entry *entry; + const struct l_queue_entry *e; + + p2p_data = ie_tlv_extract_p2p_payload(pdu, len, &p2p_len); + if (!p2p_data) + return p2p_len; + + p2p_attr_iter_init(&iter, p2p_data, p2p_len); + + va_start(args, type); + + entries = l_queue_new(); + + while (type != -1) { + entry = l_new(struct attr_handler_entry, 1); + + entry->type = type; + entry->flags = va_arg(args, unsigned int); + entry->data = va_arg(args, void *); + + type = va_arg(args, enum p2p_attr); + l_queue_push_tail(entries, entry); + } + + va_end(args); + + while (p2p_attr_iter_next(&iter)) { + attr_handler handler; + + for (e = l_queue_get_entries(entries); e; e = e->next) { + entry = e->data; + + if (p2p_attr_iter_get_type(&iter) == entry->type) + break; + } + + if (!e || entry->present) { + parse_error = true; + goto done; + } + + entry->present = true; + handler = handler_for_type(entry->type); + + if (!handler(p2p_attr_iter_get_data(&iter), + p2p_attr_iter_get_length(&iter), entry->data)) { + parse_error = true; + goto done; + } + } + + for (e = l_queue_get_entries(entries); e; e = e->next) { + entry = e->data; + + if (!entry->present && (entry->flags & ATTR_FLAG_REQUIRED)) { + parse_error = true; + goto done; + } + } + +done: + l_free(p2p_data); + l_queue_destroy(entries, l_free); + + if (!have_required) + return -EINVAL; + if (parse_error) + return -EBADMSG; + + return 0; +} diff --git a/src/p2putil.h b/src/p2putil.h index 04cfa571..247bb5e4 100644 --- a/src/p2putil.h +++ b/src/p2putil.h @@ -22,6 +22,10 @@ #include +#include "src/wscutil.h" + +struct l_queue; + /* Wi-Fi P2P Technical Specification v1.7, Section 4.1.1, Table 6 */ enum p2p_attr { P2P_ATTR_STATUS = 0, @@ -147,3 +151,103 @@ static inline const uint8_t *p2p_attr_iter_get_data(struct p2p_attr_iter *iter) { return iter->pos + 3; } + +struct p2p_capability_attr { + uint8_t device_caps; + uint8_t group_caps; +}; + +struct p2p_config_timeout_attr { + uint8_t go_config_timeout; + uint8_t client_config_timeout; +}; + +struct p2p_channel_attr { + char country[3]; + uint8_t oper_class; + uint8_t channel_num; +}; + +struct p2p_extended_listen_timing_attr { + uint16_t avail_period_ms; + uint16_t avail_interval_ms; +}; + +struct p2p_channel_entries { + uint8_t oper_class; + int n_channels; + uint8_t channels[]; +}; + +struct p2p_channel_list_attr { + char country[3]; + struct l_queue *channel_entries; +}; + +struct p2p_notice_of_absence_desc { + uint8_t count_type; + uint32_t duration; + uint32_t interval; + uint32_t start_time; +}; + +struct p2p_notice_of_absence_attr { + uint8_t index; + bool opp_ps; + uint8_t ct_window; + struct l_queue *descriptors; +}; + +struct p2p_device_info_attr { + uint8_t device_addr[6]; + uint16_t wsc_config_methods; + struct wsc_primary_device_type primary_device_type; + struct l_queue *secondary_device_types; + char device_name[33]; +}; + +struct p2p_client_info_descriptor { + uint8_t device_addr[6]; + uint8_t interface_addr[6]; + uint8_t device_caps; + uint16_t wsc_config_methods; + struct wsc_primary_device_type primary_device_type; + struct l_queue *secondary_device_types; + char device_name[33]; +}; + +struct p2p_group_id_attr { + uint8_t device_addr[6]; + char ssid[33]; +}; + +struct p2p_interface_attr { + uint8_t device_addr[6]; + struct l_queue *interface_addrs; +}; + +struct p2p_session_info_data_attr { + size_t data_len; + uint8_t data[144]; +}; + +struct p2p_advertisement_id_info_attr { + uint32_t advertisement_id; + uint8_t service_mac_addr[6]; +}; + +struct p2p_advertised_service_descriptor { + uint32_t advertisement_id; + uint16_t wsc_config_methods; + char *service_name; +}; + +struct p2p_session_id_info_attr { + uint32_t session_id; + uint8_t session_mac_addr[6]; +}; + +enum p2p_asp_coordination_transport_protocol { + P2P_ASP_TRANSPORT_UNKNOWN = 0, + P2P_ASP_TRANSPORT_UDP, +};