libeap/src/p2p/p2p_parse.c

   1 /*
   2  * P2P - IE parser
   3  * Copyright (c) 2009-2010, Atheros Communications
   4  *
   5  * This software may be distributed under the terms of the BSD license.
   6  * See README for more details.
   7  */
   8
   9 #include "includes.h"
  10
  11 #include "common.h"
  12 #include "common/ieee802_11_defs.h"
  13 #include "common/ieee802_11_common.h"
  14 #include "wps/wps_i.h"
  15 #include "p2p_i.h"
  16
  17
  18 void p2p_copy_filter_devname(char *dst, size_t dst_len,
  19                              const void *src, size_t src_len)
  20 {
  21         size_t i;
  22
  23         if (src_len >= dst_len)
  24                 src_len = dst_len - 1;
  25         os_memcpy(dst, src, src_len);
  26         dst[src_len] = '\0';
  27         for (i = 0; i < src_len; i++) {
  28                 if (dst[i] == '\0')
  29                         break;
  30                 if (is_ctrl_char(dst[i]))
  31                         dst[i] = '_';
  32         }
  33 }
  34
  35
  36 static int p2p_parse_attribute(u8 id, const u8 *data, u16 len,
  37                                struct p2p_message *msg)
  38 {
  39         const u8 *pos;
  40         u16 nlen;
  41         char devtype[WPS_DEV_TYPE_BUFSIZE];
  42
  43         switch (id) {
  44         case P2P_ATTR_CAPABILITY:
  45                 if (len < 2) {
  46                         wpa_printf(MSG_DEBUG, "P2P: Too short Capability "
  47                                    "attribute (length %d)", len);
  48                         return -1;
  49                 }
  50                 msg->capability = data;
  51                 wpa_printf(MSG_DEBUG, "P2P: * Device Capability %02x "
  52                            "Group Capability %02x",
  53                            data[0], data[1]);
  54                 break;
  55         case P2P_ATTR_DEVICE_ID:
  56                 if (len < ETH_ALEN) {
  57                         wpa_printf(MSG_DEBUG, "P2P: Too short Device ID "
  58                                    "attribute (length %d)", len);
  59                         return -1;
  60                 }
  61                 msg->device_id = data;
  62                 wpa_printf(MSG_DEBUG, "P2P: * Device ID " MACSTR,
  63                            MAC2STR(msg->device_id));
  64                 break;
  65         case P2P_ATTR_GROUP_OWNER_INTENT:
  66                 if (len < 1) {
  67                         wpa_printf(MSG_DEBUG, "P2P: Too short GO Intent "
  68                                    "attribute (length %d)", len);
  69                         return -1;
  70                 }
  71                 msg->go_intent = data;
  72                 wpa_printf(MSG_DEBUG, "P2P: * GO Intent: Intent %u "
  73                            "Tie breaker %u", data[0] >> 1, data[0] & 0x01);
  74                 break;
  75         case P2P_ATTR_STATUS:
  76                 if (len < 1) {
  77                         wpa_printf(MSG_DEBUG, "P2P: Too short Status "
  78                                    "attribute (length %d)", len);
  79                         return -1;
  80                 }
  81                 msg->status = data;
  82                 wpa_printf(MSG_DEBUG, "P2P: * Status: %d", data[0]);
  83                 break;
  84         case P2P_ATTR_LISTEN_CHANNEL:
  85                 if (len == 0) {
  86                         wpa_printf(MSG_DEBUG, "P2P: * Listen Channel: Ignore "
  87                                    "null channel");
  88                         break;
  89                 }
  90                 if (len < 5) {
  91                         wpa_printf(MSG_DEBUG, "P2P: Too short Listen Channel "
  92                                    "attribute (length %d)", len);
  93                         return -1;
  94                 }
  95                 msg->listen_channel = data;
  96                 wpa_printf(MSG_DEBUG, "P2P: * Listen Channel: "
  97                            "Country %c%c(0x%02x) Regulatory "
  98                            "Class %d Channel Number %d", data[0], data[1],
  99                            data[2], data[3], data[4]);
 100                 break;
 101         case P2P_ATTR_OPERATING_CHANNEL:
 102                 if (len == 0) {
 103                         wpa_printf(MSG_DEBUG, "P2P: * Operating Channel: "
 104                                    "Ignore null channel");
 105                         break;
 106                 }
 107                 if (len < 5) {
 108                         wpa_printf(MSG_DEBUG, "P2P: Too short Operating "
 109                                    "Channel attribute (length %d)", len);
 110                         return -1;
 111                 }
 112                 msg->operating_channel = data;
 113                 wpa_printf(MSG_DEBUG, "P2P: * Operating Channel: "
 114                            "Country %c%c(0x%02x) Regulatory "
 115                            "Class %d Channel Number %d", data[0], data[1],
 116                            data[2], data[3], data[4]);
 117                 break;
 118         case P2P_ATTR_CHANNEL_LIST:
 119                 if (len < 3) {
 120                         wpa_printf(MSG_DEBUG, "P2P: Too short Channel List "
 121                                    "attribute (length %d)", len);
 122                         return -1;
 123                 }
 124                 msg->channel_list = data;
 125                 msg->channel_list_len = len;
 126                 wpa_printf(MSG_DEBUG, "P2P: * Channel List: Country String "
 127                            "'%c%c(0x%02x)'", data[0], data[1], data[2]);
 128                 wpa_hexdump(MSG_MSGDUMP, "P2P: Channel List",
 129                             msg->channel_list, msg->channel_list_len);
 130                 break;
 131         case P2P_ATTR_GROUP_INFO:
 132                 msg->group_info = data;
 133                 msg->group_info_len = len;
 134                 wpa_printf(MSG_DEBUG, "P2P: * Group Info");
 135                 break;
 136         case P2P_ATTR_DEVICE_INFO:
 137                 if (len < ETH_ALEN + 2 + 8 + 1) {
 138                         wpa_printf(MSG_DEBUG, "P2P: Too short Device Info "
 139                                    "attribute (length %d)", len);
 140                         return -1;
 141                 }
 142                 msg->p2p_device_info = data;
 143                 msg->p2p_device_info_len = len;
 144                 pos = data;
 145                 msg->p2p_device_addr = pos;
 146                 pos += ETH_ALEN;
 147                 msg->config_methods = WPA_GET_BE16(pos);
 148                 pos += 2;
 149                 msg->pri_dev_type = pos;
 150                 pos += 8;
 151                 msg->num_sec_dev_types = *pos++;
 152                 if (msg->num_sec_dev_types * 8 > data + len - pos) {
 153                         wpa_printf(MSG_DEBUG, "P2P: Device Info underflow");
 154                         return -1;
 155                 }
 156                 pos += msg->num_sec_dev_types * 8;
 157                 if (data + len - pos < 4) {
 158                         wpa_printf(MSG_DEBUG, "P2P: Invalid Device Name "
 159                                    "length %d", (int) (data + len - pos));
 160                         return -1;
 161                 }
 162                 if (WPA_GET_BE16(pos) != ATTR_DEV_NAME) {
 163                         wpa_hexdump(MSG_DEBUG, "P2P: Unexpected Device Name "
 164                                     "header", pos, 4);
 165                         return -1;
 166                 }
 167                 pos += 2;
 168                 nlen = WPA_GET_BE16(pos);
 169                 pos += 2;
 170                 if (nlen > data + len - pos || nlen > WPS_DEV_NAME_MAX_LEN) {
 171                         wpa_printf(MSG_DEBUG, "P2P: Invalid Device Name "
 172                                    "length %u (buf len %d)", nlen,
 173                                    (int) (data + len - pos));
 174                         return -1;
 175                 }
 176                 p2p_copy_filter_devname(msg->device_name,
 177                                         sizeof(msg->device_name), pos, nlen);
 178                 wpa_printf(MSG_DEBUG, "P2P: * Device Info: addr " MACSTR
 179                            " primary device type %s device name '%s' "
 180                            "config methods 0x%x",
 181                            MAC2STR(msg->p2p_device_addr),
 182                            wps_dev_type_bin2str(msg->pri_dev_type, devtype,
 183                                                 sizeof(devtype)),
 184                            msg->device_name, msg->config_methods);
 185                 break;
 186         case P2P_ATTR_CONFIGURATION_TIMEOUT:
 187                 if (len < 2) {
 188                         wpa_printf(MSG_DEBUG, "P2P: Too short Configuration "
 189                                    "Timeout attribute (length %d)", len);
 190                         return -1;
 191                 }
 192                 msg->config_timeout = data;
 193                 wpa_printf(MSG_DEBUG, "P2P: * Configuration Timeout");
 194                 break;
 195         case P2P_ATTR_INTENDED_INTERFACE_ADDR:
 196                 if (len < ETH_ALEN) {
 197                         wpa_printf(MSG_DEBUG, "P2P: Too short Intended P2P "
 198                                    "Interface Address attribute (length %d)",
 199                                    len);
 200                         return -1;
 201                 }
 202                 msg->intended_addr = data;
 203                 wpa_printf(MSG_DEBUG, "P2P: * Intended P2P Interface Address: "
 204                            MACSTR, MAC2STR(msg->intended_addr));
 205                 break;
 206         case P2P_ATTR_GROUP_BSSID:
 207                 if (len < ETH_ALEN) {
 208                         wpa_printf(MSG_DEBUG, "P2P: Too short P2P Group BSSID "
 209                                    "attribute (length %d)", len);
 210                         return -1;
 211                 }
 212                 msg->group_bssid = data;
 213                 wpa_printf(MSG_DEBUG, "P2P: * P2P Group BSSID: " MACSTR,
 214                            MAC2STR(msg->group_bssid));
 215                 break;
 216         case P2P_ATTR_GROUP_ID:
 217                 if (len < ETH_ALEN || len > ETH_ALEN + SSID_MAX_LEN) {
 218                         wpa_printf(MSG_DEBUG, "P2P: Invalid P2P Group ID "
 219                                    "attribute length %d", len);
 220                         return -1;
 221                 }
 222                 msg->group_id = data;
 223                 msg->group_id_len = len;
 224                 wpa_printf(MSG_DEBUG, "P2P: * P2P Group ID: Device Address "
 225                            MACSTR, MAC2STR(msg->group_id));
 226                 wpa_hexdump_ascii(MSG_DEBUG, "P2P: * P2P Group ID: SSID",
 227                                   msg->group_id + ETH_ALEN,
 228                                   msg->group_id_len - ETH_ALEN);
 229                 break;
 230         case P2P_ATTR_INVITATION_FLAGS:
 231                 if (len < 1) {
 232                         wpa_printf(MSG_DEBUG, "P2P: Too short Invitation "
 233                                    "Flag attribute (length %d)", len);
 234                         return -1;
 235                 }
 236                 msg->invitation_flags = data;
 237                 wpa_printf(MSG_DEBUG, "P2P: * Invitation Flags: bitmap 0x%x",
 238                            data[0]);
 239                 break;
 240         case P2P_ATTR_MANAGEABILITY:
 241                 if (len < 1) {
 242                         wpa_printf(MSG_DEBUG, "P2P: Too short Manageability "
 243                                    "attribute (length %d)", len);
 244                         return -1;
 245                 }
 246                 msg->manageability = data;
 247                 wpa_printf(MSG_DEBUG, "P2P: * Manageability: bitmap 0x%x",
 248                            data[0]);
 249                 break;
 250         case P2P_ATTR_NOTICE_OF_ABSENCE:
 251                 if (len < 2) {
 252                         wpa_printf(MSG_DEBUG, "P2P: Too short Notice of "
 253                                    "Absence attribute (length %d)", len);
 254                         return -1;
 255                 }
 256                 msg->noa = data;
 257                 msg->noa_len = len;
 258                 wpa_printf(MSG_DEBUG, "P2P: * Notice of Absence");
 259                 break;
 260         case P2P_ATTR_EXT_LISTEN_TIMING:
 261                 if (len < 4) {
 262                         wpa_printf(MSG_DEBUG, "P2P: Too short Extended Listen "
 263                                    "Timing attribute (length %d)", len);
 264                         return -1;
 265                 }
 266                 msg->ext_listen_timing = data;
 267                 wpa_printf(MSG_DEBUG, "P2P: * Extended Listen Timing "
 268                            "(period %u msec  interval %u msec)",
 269                            WPA_GET_LE16(msg->ext_listen_timing),
 270                            WPA_GET_LE16(msg->ext_listen_timing + 2));
 271                 break;
 272         case P2P_ATTR_MINOR_REASON_CODE:
 273                 if (len < 1) {
 274                         wpa_printf(MSG_DEBUG, "P2P: Too short Minor Reason "
 275                                    "Code attribute (length %d)", len);
 276                         return -1;
 277                 }
 278                 msg->minor_reason_code = data;
 279                 wpa_printf(MSG_DEBUG, "P2P: * Minor Reason Code: %u",
 280                            *msg->minor_reason_code);
 281                 break;
 282         case P2P_ATTR_OOB_GO_NEG_CHANNEL:
 283                 if (len < 6) {
 284                         wpa_printf(MSG_DEBUG, "P2P: Too short OOB GO Neg "
 285                                    "Channel attribute (length %d)", len);
 286                         return -1;
 287                 }
 288                 msg->oob_go_neg_channel = data;
 289                 wpa_printf(MSG_DEBUG, "P2P: * OOB GO Neg Channel: "
 290                            "Country %c%c(0x%02x) Operating Class %d "
 291                            "Channel Number %d Role %d",
 292                            data[0], data[1], data[2], data[3], data[4],
 293                            data[5]);
 294                 break;
 295         case P2P_ATTR_SERVICE_HASH:
 296                 if (len < P2PS_HASH_LEN) {
 297                         wpa_printf(MSG_DEBUG,
 298                                    "P2P: Too short Service Hash (length %u)",
 299                                    len);
 300                         return -1;
 301                 }
 302                 msg->service_hash_count = len / P2PS_HASH_LEN;
 303                 msg->service_hash = data;
 304                 wpa_hexdump(MSG_DEBUG, "P2P: * Service Hash(s)", data, len);
 305                 break;
 306         case P2P_ATTR_SESSION_INFORMATION_DATA:
 307                 msg->session_info = data;
 308                 msg->session_info_len = len;
 309                 wpa_printf(MSG_DEBUG, "P2P: * Service Instance: %u bytes - %p",
 310                            len, data);
 311                 break;
 312         case P2P_ATTR_CONNECTION_CAPABILITY:
 313                 if (len < 1) {
 314                         wpa_printf(MSG_DEBUG,
 315                                    "P2P: Too short Connection Capability (length %u)",
 316                                    len);
 317                         return -1;
 318                 }
 319                 msg->conn_cap = data;
 320                 wpa_printf(MSG_DEBUG, "P2P: * Connection Capability: 0x%x",
 321                            *msg->conn_cap);
 322                 break;
 323         case P2P_ATTR_ADVERTISEMENT_ID:
 324                 if (len < 10) {
 325                         wpa_printf(MSG_DEBUG,
 326                                    "P2P: Too short Advertisement ID (length %u)",
 327                                    len);
 328                         return -1;
 329                 }
 330                 msg->adv_id = data;
 331                 msg->adv_mac = &data[sizeof(u32)];
 332                 wpa_printf(MSG_DEBUG, "P2P: * Advertisement ID %x",
 333                            WPA_GET_LE32(data));
 334                 break;
 335         case P2P_ATTR_ADVERTISED_SERVICE:
 336                 if (len < 8) {
 337                         wpa_printf(MSG_DEBUG,
 338                                    "P2P: Too short Service Instance (length %u)",
 339                                    len);
 340                         return -1;
 341                 }
 342                 msg->adv_service_instance = data;
 343                 msg->adv_service_instance_len = len;
 344                 if (len <= 255 + 8) {
 345                         char str[256];
 346                         u8 namelen;
 347
 348                         namelen = data[6];
 349                         if (namelen > len - 7)
 350                                 break;
 351                         os_memcpy(str, &data[7], namelen);
 352                         str[namelen] = '\0';
 353                         wpa_printf(MSG_DEBUG, "P2P: * Service Instance: %x-%s",
 354                                    WPA_GET_LE32(data), str);
 355                 } else {
 356                         wpa_printf(MSG_DEBUG, "P2P: * Service Instance: %p",
 357                                    data);
 358                 }
 359                 break;
 360         case P2P_ATTR_SESSION_ID:
 361                 if (len < sizeof(u32) + ETH_ALEN) {
 362                         wpa_printf(MSG_DEBUG,
 363                                    "P2P: Too short Session ID Info (length %u)",
 364                                    len);
 365                         return -1;
 366                 }
 367                 msg->session_id = data;
 368                 msg->session_mac = &data[sizeof(u32)];
 369                 wpa_printf(MSG_DEBUG, "P2P: * Session ID: %x " MACSTR,
 370                            WPA_GET_LE32(data), MAC2STR(msg->session_mac));
 371                 break;
 372         case P2P_ATTR_FEATURE_CAPABILITY:
 373                 if (!len) {
 374                         wpa_printf(MSG_DEBUG,
 375                                    "P2P: Too short Feature Capability (length %u)",
 376                                    len);
 377                         return -1;
 378                 }
 379                 msg->feature_cap = data;
 380                 msg->feature_cap_len = len;
 381                 wpa_printf(MSG_DEBUG, "P2P: * Feature Cap (length=%u)", len);
 382                 break;
 383         case P2P_ATTR_PERSISTENT_GROUP:
 384         {
 385                 if (len < ETH_ALEN || len > ETH_ALEN + SSID_MAX_LEN) {
 386                         wpa_printf(MSG_DEBUG,
 387                                    "P2P: Invalid Persistent Group Info (length %u)",
 388                                    len);
 389                         return -1;
 390                 }
 391
 392                 msg->persistent_dev = data;
 393                 msg->persistent_ssid_len = len - ETH_ALEN;
 394                 msg->persistent_ssid = &data[ETH_ALEN];
 395                 wpa_printf(MSG_DEBUG, "P2P: * Persistent Group: " MACSTR " %s",
 396                            MAC2STR(msg->persistent_dev),
 397                            wpa_ssid_txt(msg->persistent_ssid,
 398                                         msg->persistent_ssid_len));
 399                 break;
 400         }
 401         default:
 402                 wpa_printf(MSG_DEBUG, "P2P: Skipped unknown attribute %d "
 403                            "(length %d)", id, len);
 404                 break;
 405         }
 406
 407         return 0;
 408 }
 409
 410
 411 /**
 412  * p2p_parse_p2p_ie - Parse P2P IE
 413  * @buf: Concatenated P2P IE(s) payload
 414  * @msg: Buffer for returning parsed attributes
 415  * Returns: 0 on success, -1 on failure
 416  *
 417  * Note: Caller is responsible for clearing the msg data structure before
 418  * calling this function.
 419  */
 420 int p2p_parse_p2p_ie(const struct wpabuf *buf, struct p2p_message *msg)
 421 {
 422         const u8 *pos = wpabuf_head_u8(buf);
 423         const u8 *end = pos + wpabuf_len(buf);
 424
 425         wpa_printf(MSG_DEBUG, "P2P: Parsing P2P IE");
 426
 427         while (pos < end) {
 428                 u16 attr_len;
 429                 u8 id;
 430
 431                 if (end - pos < 3) {
 432                         wpa_printf(MSG_DEBUG, "P2P: Invalid P2P attribute");
 433                         return -1;
 434                 }
 435                 id = *pos++;
 436                 attr_len = WPA_GET_LE16(pos);
 437                 pos += 2;
 438                 wpa_printf(MSG_DEBUG, "P2P: Attribute %d length %u",
 439                            id, attr_len);
 440                 if (attr_len > end - pos) {
 441                         wpa_printf(MSG_DEBUG, "P2P: Attribute underflow "
 442                                    "(len=%u left=%d)",
 443                                    attr_len, (int) (end - pos));
 444                         wpa_hexdump(MSG_MSGDUMP, "P2P: Data", pos, end - pos);
 445                         return -1;
 446                 }
 447                 if (p2p_parse_attribute(id, pos, attr_len, msg))
 448                         return -1;
 449                 pos += attr_len;
 450         }
 451
 452         return 0;
 453 }
 454
 455
 456 static int p2p_parse_wps_ie(const struct wpabuf *buf, struct p2p_message *msg)
 457 {
 458         struct wps_parse_attr attr;
 459         int i;
 460
 461         wpa_printf(MSG_DEBUG, "P2P: Parsing WPS IE");
 462         if (wps_parse_msg(buf, &attr))
 463                 return -1;
 464         if (attr.dev_name && attr.dev_name_len < sizeof(msg->device_name) &&
 465             !msg->device_name[0])
 466                 os_memcpy(msg->device_name, attr.dev_name, attr.dev_name_len);
 467         if (attr.config_methods) {
 468                 msg->wps_config_methods =
 469                         WPA_GET_BE16(attr.config_methods);
 470                 wpa_printf(MSG_DEBUG, "P2P: Config Methods (WPS): 0x%x",
 471                            msg->wps_config_methods);
 472         }
 473         if (attr.dev_password_id) {
 474                 msg->dev_password_id = WPA_GET_BE16(attr.dev_password_id);
 475                 wpa_printf(MSG_DEBUG, "P2P: Device Password ID: %d",
 476                            msg->dev_password_id);
 477                 msg->dev_password_id_present = 1;
 478         }
 479         if (attr.primary_dev_type) {
 480                 char devtype[WPS_DEV_TYPE_BUFSIZE];
 481                 msg->wps_pri_dev_type = attr.primary_dev_type;
 482                 wpa_printf(MSG_DEBUG, "P2P: Primary Device Type (WPS): %s",
 483                            wps_dev_type_bin2str(msg->wps_pri_dev_type, devtype,
 484                                                 sizeof(devtype)));
 485         }
 486         if (attr.sec_dev_type_list) {
 487                 msg->wps_sec_dev_type_list = attr.sec_dev_type_list;
 488                 msg->wps_sec_dev_type_list_len = attr.sec_dev_type_list_len;
 489         }
 490
 491         for (i = 0; i < P2P_MAX_WPS_VENDOR_EXT; i++) {
 492                 msg->wps_vendor_ext[i] = attr.vendor_ext[i];
 493                 msg->wps_vendor_ext_len[i] = attr.vendor_ext_len[i];
 494         }
 495
 496         msg->manufacturer = attr.manufacturer;
 497         msg->manufacturer_len = attr.manufacturer_len;
 498         msg->model_name = attr.model_name;
 499         msg->model_name_len = attr.model_name_len;
 500         msg->model_number = attr.model_number;
 501         msg->model_number_len = attr.model_number_len;
 502         msg->serial_number = attr.serial_number;
 503         msg->serial_number_len = attr.serial_number_len;
 504
 505         msg->oob_dev_password = attr.oob_dev_password;
 506         msg->oob_dev_password_len = attr.oob_dev_password_len;
 507
 508         return 0;
 509 }
 510
 511
 512 /**
 513  * p2p_parse_ies - Parse P2P message IEs (both WPS and P2P IE)
 514  * @data: IEs from the message
 515  * @len: Length of data buffer in octets
 516  * @msg: Buffer for returning parsed attributes
 517  * Returns: 0 on success, -1 on failure
 518  *
 519  * Note: Caller is responsible for clearing the msg data structure before
 520  * calling this function.
 521  *
 522  * Note: Caller must free temporary memory allocations by calling
 523  * p2p_parse_free() when the parsed data is not needed anymore.
 524  */
 525 int p2p_parse_ies(const u8 *data, size_t len, struct p2p_message *msg)
 526 {
 527         struct ieee802_11_elems elems;
 528
 529         ieee802_11_parse_elems(data, len, &elems, 0);
 530         if (elems.ds_params)
 531                 msg->ds_params = elems.ds_params;
 532         if (elems.ssid)
 533                 msg->ssid = elems.ssid - 2;
 534
 535         msg->wps_attributes = ieee802_11_vendor_ie_concat(data, len,
 536                                                           WPS_DEV_OUI_WFA);
 537         if (msg->wps_attributes &&
 538             p2p_parse_wps_ie(msg->wps_attributes, msg)) {
 539                 p2p_parse_free(msg);
 540                 return -1;
 541         }
 542
 543         msg->p2p_attributes = ieee802_11_vendor_ie_concat(data, len,
 544                                                           P2P_IE_VENDOR_TYPE);
 545         if (msg->p2p_attributes &&
 546             p2p_parse_p2p_ie(msg->p2p_attributes, msg)) {
 547                 wpa_printf(MSG_DEBUG, "P2P: Failed to parse P2P IE data");
 548                 if (msg->p2p_attributes)
 549                         wpa_hexdump_buf(MSG_MSGDUMP, "P2P: P2P IE data",
 550                                         msg->p2p_attributes);
 551                 p2p_parse_free(msg);
 552                 return -1;
 553         }
 554
 555 #ifdef CONFIG_WIFI_DISPLAY
 556         if (elems.wfd) {
 557                 msg->wfd_subelems = ieee802_11_vendor_ie_concat(
 558                         data, len, WFD_IE_VENDOR_TYPE);
 559         }
 560 #endif /* CONFIG_WIFI_DISPLAY */
 561
 562         msg->pref_freq_list = elems.pref_freq_list;
 563         msg->pref_freq_list_len = elems.pref_freq_list_len;
 564
 565         return 0;
 566 }
 567
 568
 569 /**
 570  * p2p_parse - Parse a P2P Action frame contents
 571  * @data: Action frame payload after Category and Code fields
 572  * @len: Length of data buffer in octets
 573  * @msg: Buffer for returning parsed attributes
 574  * Returns: 0 on success, -1 on failure
 575  *
 576  * Note: Caller must free temporary memory allocations by calling
 577  * p2p_parse_free() when the parsed data is not needed anymore.
 578  */
 579 int p2p_parse(const u8 *data, size_t len, struct p2p_message *msg)
 580 {
 581         os_memset(msg, 0, sizeof(*msg));
 582         wpa_printf(MSG_DEBUG, "P2P: Parsing the received message");
 583         if (len < 1) {
 584                 wpa_printf(MSG_DEBUG, "P2P: No Dialog Token in the message");
 585                 return -1;
 586         }
 587         msg->dialog_token = data[0];
 588         wpa_printf(MSG_DEBUG, "P2P: * Dialog Token: %d", msg->dialog_token);
 589
 590         return p2p_parse_ies(data + 1, len - 1, msg);
 591 }
 592
 593
 594 int p2p_parse_ies_separate(const u8 *wsc, size_t wsc_len, const u8 *p2p,
 595                            size_t p2p_len, struct p2p_message *msg)
 596 {
 597         os_memset(msg, 0, sizeof(*msg));
 598
 599         msg->wps_attributes = wpabuf_alloc_copy(wsc, wsc_len);
 600         if (msg->wps_attributes &&
 601             p2p_parse_wps_ie(msg->wps_attributes, msg)) {
 602                 p2p_parse_free(msg);
 603                 return -1;
 604         }
 605
 606         msg->p2p_attributes = wpabuf_alloc_copy(p2p, p2p_len);
 607         if (msg->p2p_attributes &&
 608             p2p_parse_p2p_ie(msg->p2p_attributes, msg)) {
 609                 wpa_printf(MSG_DEBUG, "P2P: Failed to parse P2P IE data");
 610                 if (msg->p2p_attributes)
 611                         wpa_hexdump_buf(MSG_MSGDUMP, "P2P: P2P IE data",
 612                                         msg->p2p_attributes);
 613                 p2p_parse_free(msg);
 614                 return -1;
 615         }
 616
 617         return 0;
 618 }
 619
 620
 621 /**
 622  * p2p_parse_free - Free temporary data from P2P parsing
 623  * @msg: Parsed attributes
 624  */
 625 void p2p_parse_free(struct p2p_message *msg)
 626 {
 627         wpabuf_free(msg->p2p_attributes);
 628         msg->p2p_attributes = NULL;
 629         wpabuf_free(msg->wps_attributes);
 630         msg->wps_attributes = NULL;
 631 #ifdef CONFIG_WIFI_DISPLAY
 632         wpabuf_free(msg->wfd_subelems);
 633         msg->wfd_subelems = NULL;
 634 #endif /* CONFIG_WIFI_DISPLAY */
 635 }
 636
 637
 638 int p2p_group_info_parse(const u8 *gi, size_t gi_len,
 639                          struct p2p_group_info *info)
 640 {
 641         const u8 *g, *gend;
 642
 643         os_memset(info, 0, sizeof(*info));
 644         if (gi == NULL)
 645                 return 0;
 646
 647         g = gi;
 648         gend = gi + gi_len;
 649         while (g < gend) {
 650                 struct p2p_client_info *cli;
 651                 const u8 *cend;
 652                 u16 count;
 653                 u8 len;
 654
 655                 cli = &info->client[info->num_clients];
 656                 len = *g++;
 657                 if (len > gend - g || len < 2 * ETH_ALEN + 1 + 2 + 8 + 1)
 658                         return -1; /* invalid data */
 659                 cend = g + len;
 660                 /* g at start of P2P Client Info Descriptor */
 661                 cli->p2p_device_addr = g;
 662                 g += ETH_ALEN;
 663                 cli->p2p_interface_addr = g;
 664                 g += ETH_ALEN;
 665                 cli->dev_capab = *g++;
 666
 667                 cli->config_methods = WPA_GET_BE16(g);
 668                 g += 2;
 669                 cli->pri_dev_type = g;
 670                 g += 8;
 671
 672                 /* g at Number of Secondary Device Types */
 673                 len = *g++;
 674                 if (8 * len > cend - g)
 675                         return -1; /* invalid data */
 676                 cli->num_sec_dev_types = len;
 677                 cli->sec_dev_types = g;
 678                 g += 8 * len;
 679
 680                 /* g at Device Name in WPS TLV format */
 681                 if (cend - g < 2 + 2)
 682                         return -1; /* invalid data */
 683                 if (WPA_GET_BE16(g) != ATTR_DEV_NAME)
 684                         return -1; /* invalid Device Name TLV */
 685                 g += 2;
 686                 count = WPA_GET_BE16(g);
 687                 g += 2;
 688                 if (count > cend - g)
 689                         return -1; /* invalid Device Name TLV */
 690                 if (count >= WPS_DEV_NAME_MAX_LEN)
 691                         count = WPS_DEV_NAME_MAX_LEN;
 692                 cli->dev_name = (const char *) g;
 693                 cli->dev_name_len = count;
 694
 695                 g = cend;
 696
 697                 info->num_clients++;
 698                 if (info->num_clients == P2P_MAX_GROUP_ENTRIES)
 699                         return -1;
 700         }
 701
 702         return 0;
 703 }
 704
 705
 706 static int p2p_group_info_text(const u8 *gi, size_t gi_len, char *buf,
 707                                char *end)
 708 {
 709         char *pos = buf;
 710         int ret;
 711         struct p2p_group_info info;
 712         unsigned int i;
 713
 714         if (p2p_group_info_parse(gi, gi_len, &info) < 0)
 715                 return 0;
 716
 717         for (i = 0; i < info.num_clients; i++) {
 718                 struct p2p_client_info *cli;
 719                 char name[WPS_DEV_NAME_MAX_LEN + 1];
 720                 char devtype[WPS_DEV_TYPE_BUFSIZE];
 721                 u8 s;
 722                 int count;
 723
 724                 cli = &info.client[i];
 725                 ret = os_snprintf(pos, end - pos, "p2p_group_client: "
 726                                   "dev=" MACSTR " iface=" MACSTR,
 727                                   MAC2STR(cli->p2p_device_addr),
 728                                   MAC2STR(cli->p2p_interface_addr));
 729                 if (os_snprintf_error(end - pos, ret))
 730                         return pos - buf;
 731                 pos += ret;
 732
 733                 ret = os_snprintf(pos, end - pos,
 734                                   " dev_capab=0x%x config_methods=0x%x "
 735                                   "dev_type=%s",
 736                                   cli->dev_capab, cli->config_methods,
 737                                   wps_dev_type_bin2str(cli->pri_dev_type,
 738                                                        devtype,
 739                                                        sizeof(devtype)));
 740                 if (os_snprintf_error(end - pos, ret))
 741                         return pos - buf;
 742                 pos += ret;
 743
 744                 for (s = 0; s < cli->num_sec_dev_types; s++) {
 745                         ret = os_snprintf(pos, end - pos, " dev_type=%s",
 746                                           wps_dev_type_bin2str(
 747                                                   &cli->sec_dev_types[s * 8],
 748                                                   devtype, sizeof(devtype)));
 749                         if (os_snprintf_error(end - pos, ret))
 750                                 return pos - buf;
 751                         pos += ret;
 752                 }
 753
 754                 os_memcpy(name, cli->dev_name, cli->dev_name_len);
 755                 name[cli->dev_name_len] = '\0';
 756                 count = (int) cli->dev_name_len - 1;
 757                 while (count >= 0) {
 758                         if (is_ctrl_char(name[count]))
 759                                 name[count] = '_';
 760                         count--;
 761                 }
 762
 763                 ret = os_snprintf(pos, end - pos, " dev_name='%s'\n", name);
 764                 if (os_snprintf_error(end - pos, ret))
 765                         return pos - buf;
 766                 pos += ret;
 767         }
 768
 769         return pos - buf;
 770 }
 771
 772
 773 /**
 774  * p2p_attr_text - Build text format description of P2P IE attributes
 775  * @data: P2P IE contents
 776  * @buf: Buffer for returning text
 777  * @end: Pointer to the end of the buf area
 778  * Returns: Number of octets written to the buffer or -1 on faikure
 779  *
 780  * This function can be used to parse P2P IE contents into text format
 781  * field=value lines.
 782  */
 783 int p2p_attr_text(struct wpabuf *data, char *buf, char *end)
 784 {
 785         struct p2p_message msg;
 786         char *pos = buf;
 787         int ret;
 788
 789         os_memset(&msg, 0, sizeof(msg));
 790         if (p2p_parse_p2p_ie(data, &msg))
 791                 return -1;
 792
 793         if (msg.capability) {
 794                 ret = os_snprintf(pos, end - pos,
 795                                   "p2p_dev_capab=0x%x\n"
 796                                   "p2p_group_capab=0x%x\n",
 797                                   msg.capability[0], msg.capability[1]);
 798                 if (os_snprintf_error(end - pos, ret))
 799                         return pos - buf;
 800                 pos += ret;
 801         }
 802
 803         if (msg.pri_dev_type) {
 804                 char devtype[WPS_DEV_TYPE_BUFSIZE];
 805                 ret = os_snprintf(pos, end - pos,
 806                                   "p2p_primary_device_type=%s\n",
 807                                   wps_dev_type_bin2str(msg.pri_dev_type,
 808                                                        devtype,
 809                                                        sizeof(devtype)));
 810                 if (os_snprintf_error(end - pos, ret))
 811                         return pos - buf;
 812                 pos += ret;
 813         }
 814
 815         ret = os_snprintf(pos, end - pos, "p2p_device_name=%s\n",
 816                           msg.device_name);
 817         if (os_snprintf_error(end - pos, ret))
 818                 return pos - buf;
 819         pos += ret;
 820
 821         if (msg.p2p_device_addr) {
 822                 ret = os_snprintf(pos, end - pos, "p2p_device_addr=" MACSTR
 823                                   "\n",
 824                                   MAC2STR(msg.p2p_device_addr));
 825                 if (os_snprintf_error(end - pos, ret))
 826                         return pos - buf;
 827                 pos += ret;
 828         }
 829
 830         ret = os_snprintf(pos, end - pos, "p2p_config_methods=0x%x\n",
 831                           msg.config_methods);
 832         if (os_snprintf_error(end - pos, ret))
 833                 return pos - buf;
 834         pos += ret;
 835
 836         ret = p2p_group_info_text(msg.group_info, msg.group_info_len,
 837                                   pos, end);
 838         if (ret < 0)
 839                 return pos - buf;
 840         pos += ret;
 841
 842         return pos - buf;
 843 }
 844
 845
 846 int p2p_get_cross_connect_disallowed(const struct wpabuf *p2p_ie)
 847 {
 848         struct p2p_message msg;
 849
 850         os_memset(&msg, 0, sizeof(msg));
 851         if (p2p_parse_p2p_ie(p2p_ie, &msg))
 852                 return 0;
 853
 854         if (!msg.manageability)
 855                 return 0;
 856
 857         return !(msg.manageability[0] & P2P_MAN_CROSS_CONNECTION_PERMITTED);
 858 }
 859
 860
 861 u8 p2p_get_group_capab(const struct wpabuf *p2p_ie)
 862 {
 863         struct p2p_message msg;
 864
 865         os_memset(&msg, 0, sizeof(msg));
 866         if (p2p_parse_p2p_ie(p2p_ie, &msg))
 867                 return 0;
 868
 869         if (!msg.capability)
 870                 return 0;
 871
 872         return msg.capability[1];
 873 }
 874
 875
 876 const u8 * p2p_get_go_dev_addr(const struct wpabuf *p2p_ie)
 877 {
 878         struct p2p_message msg;
 879
 880         os_memset(&msg, 0, sizeof(msg));
 881         if (p2p_parse_p2p_ie(p2p_ie, &msg))
 882                 return NULL;
 883
 884         if (msg.p2p_device_addr)
 885                 return msg.p2p_device_addr;
 886         if (msg.device_id)
 887                 return msg.device_id;
 888
 889         return NULL;
 890 }