Pileus Git - ~andy/linux/blob - drivers/uwb/wlp/messages.c

   1 /*
   2  * WiMedia Logical Link Control Protocol (WLP)
   3  * Message construction and parsing
   4  *
   5  * Copyright (C) 2007 Intel Corporation
   6  * Reinette Chatre <reinette.chatre@intel.com>
   7  *
   8  * This program is free software; you can redistribute it and/or
   9  * modify it under the terms of the GNU General Public License version
  10  * 2 as published by the Free Software Foundation.
  11  *
  12  * This program is distributed in the hope that it will be useful,
  13  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  14  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  15  * GNU General Public License for more details.
  16  *
  17  * You should have received a copy of the GNU General Public License
  18  * along with this program; if not, write to the Free Software
  19  * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
  20  * 02110-1301, USA.
  21  *
  22  *
  23  * FIXME: docs
  24  */
  25
  26 #include <linux/wlp.h>
  27
  28 #include "wlp-internal.h"
  29
  30 static
  31 const char *__wlp_assoc_frame[] = {
  32         [WLP_ASSOC_D1] = "WLP_ASSOC_D1",
  33         [WLP_ASSOC_D2] = "WLP_ASSOC_D2",
  34         [WLP_ASSOC_M1] = "WLP_ASSOC_M1",
  35         [WLP_ASSOC_M2] = "WLP_ASSOC_M2",
  36         [WLP_ASSOC_M3] = "WLP_ASSOC_M3",
  37         [WLP_ASSOC_M4] = "WLP_ASSOC_M4",
  38         [WLP_ASSOC_M5] = "WLP_ASSOC_M5",
  39         [WLP_ASSOC_M6] = "WLP_ASSOC_M6",
  40         [WLP_ASSOC_M7] = "WLP_ASSOC_M7",
  41         [WLP_ASSOC_M8] = "WLP_ASSOC_M8",
  42         [WLP_ASSOC_F0] = "WLP_ASSOC_F0",
  43         [WLP_ASSOC_E1] = "WLP_ASSOC_E1",
  44         [WLP_ASSOC_E2] = "WLP_ASSOC_E2",
  45         [WLP_ASSOC_C1] = "WLP_ASSOC_C1",
  46         [WLP_ASSOC_C2] = "WLP_ASSOC_C2",
  47         [WLP_ASSOC_C3] = "WLP_ASSOC_C3",
  48         [WLP_ASSOC_C4] = "WLP_ASSOC_C4",
  49 };
  50
  51 static const char *wlp_assoc_frame_str(unsigned id)
  52 {
  53         if (id >= ARRAY_SIZE(__wlp_assoc_frame))
  54                 return "unknown association frame";
  55         return __wlp_assoc_frame[id];
  56 }
  57
  58 static const char *__wlp_assc_error[] = {
  59         "none",
  60         "Authenticator Failure",
  61         "Rogue activity suspected",
  62         "Device busy",
  63         "Setup Locked",
  64         "Registrar not ready",
  65         "Invalid WSS selection",
  66         "Message timeout",
  67         "Enrollment session timeout",
  68         "Device password invalid",
  69         "Unsupported version",
  70         "Internal error",
  71         "Undefined error",
  72         "Numeric comparison failure",
  73         "Waiting for user input",
  74 };
  75
  76 static const char *wlp_assc_error_str(unsigned id)
  77 {
  78         if (id >= ARRAY_SIZE(__wlp_assc_error))
  79                 return "unknown WLP association error";
  80         return __wlp_assc_error[id];
  81 }
  82
  83 static inline void wlp_set_attr_hdr(struct wlp_attr_hdr *hdr, unsigned type,
  84                                     size_t len)
  85 {
  86         hdr->type = cpu_to_le16(type);
  87         hdr->length = cpu_to_le16(len);
  88 }
  89
  90 /*
  91  * Populate fields of a constant sized attribute
  92  *
  93  * @returns: total size of attribute including size of new value
  94  *
  95  * We have two instances of this function (wlp_pset and wlp_set): one takes
  96  * the value as a parameter, the other takes a pointer to the value as
  97  * parameter. They thus only differ in how the value is assigned to the
  98  * attribute.
  99  *
 100  * We use sizeof(*attr) - sizeof(struct wlp_attr_hdr) instead of
 101  * sizeof(type) to be able to use this same code for the structures that
 102  * contain 8bit enum values and be able to deal with pointer types.
 103  */
 104 #define wlp_set(type, type_code, name)                                  \
 105 static size_t wlp_set_##name(struct wlp_attr_##name *attr, type value)  \
 106 {                                                                       \
 107         wlp_set_attr_hdr(&attr->hdr, type_code,                         \
 108                          sizeof(*attr) - sizeof(struct wlp_attr_hdr));  \
 109         attr->name = value;                                             \
 110         return sizeof(*attr);                                           \
 111 }
 112
 113 #define wlp_pset(type, type_code, name)                                 \
 114 static size_t wlp_set_##name(struct wlp_attr_##name *attr, type value)  \
 115 {                                                                       \
 116         wlp_set_attr_hdr(&attr->hdr, type_code,                         \
 117                          sizeof(*attr) - sizeof(struct wlp_attr_hdr));  \
 118         attr->name = *value;                                            \
 119         return sizeof(*attr);                                           \
 120 }
 121
 122 /**
 123  * Populate fields of a variable attribute
 124  *
 125  * @returns: total size of attribute including size of new value
 126  *
 127  * Provided with a pointer to the memory area reserved for the
 128  * attribute structure, the field is populated with the value. The
 129  * reserved memory has to contain enough space for the value.
 130  */
 131 #define wlp_vset(type, type_code, name)                                 \
 132 static size_t wlp_set_##name(struct wlp_attr_##name *attr, type value,  \
 133                                 size_t len)                             \
 134 {                                                                       \
 135         wlp_set_attr_hdr(&attr->hdr, type_code, len);                   \
 136         memcpy(attr->name, value, len);                                 \
 137         return sizeof(*attr) + len;                                     \
 138 }
 139
 140 wlp_vset(char *, WLP_ATTR_DEV_NAME, dev_name)
 141 wlp_vset(char *, WLP_ATTR_MANUF, manufacturer)
 142 wlp_set(enum wlp_assoc_type, WLP_ATTR_MSG_TYPE, msg_type)
 143 wlp_vset(char *, WLP_ATTR_MODEL_NAME, model_name)
 144 wlp_vset(char *, WLP_ATTR_MODEL_NR, model_nr)
 145 wlp_vset(char *, WLP_ATTR_SERIAL, serial)
 146 wlp_vset(char *, WLP_ATTR_WSS_NAME, wss_name)
 147 wlp_pset(struct wlp_uuid *, WLP_ATTR_UUID_E, uuid_e)
 148 wlp_pset(struct wlp_uuid *, WLP_ATTR_UUID_R, uuid_r)
 149 wlp_pset(struct wlp_uuid *, WLP_ATTR_WSSID, wssid)
 150 wlp_pset(struct wlp_dev_type *, WLP_ATTR_PRI_DEV_TYPE, prim_dev_type)
 151 /*wlp_pset(struct wlp_dev_type *, WLP_ATTR_SEC_DEV_TYPE, sec_dev_type)*/
 152 wlp_set(u8, WLP_ATTR_WLP_VER, version)
 153 wlp_set(enum wlp_assc_error, WLP_ATTR_WLP_ASSC_ERR, wlp_assc_err)
 154 wlp_set(enum wlp_wss_sel_mthd, WLP_ATTR_WSS_SEL_MTHD, wss_sel_mthd)
 155 wlp_set(u8, WLP_ATTR_ACC_ENRL, accept_enrl)
 156 wlp_set(u8, WLP_ATTR_WSS_SEC_STAT, wss_sec_status)
 157 wlp_pset(struct uwb_mac_addr *, WLP_ATTR_WSS_BCAST, wss_bcast)
 158 wlp_pset(struct wlp_nonce *, WLP_ATTR_ENRL_NONCE, enonce)
 159 wlp_pset(struct wlp_nonce *, WLP_ATTR_REG_NONCE, rnonce)
 160 wlp_set(u8, WLP_ATTR_WSS_TAG, wss_tag)
 161 wlp_pset(struct uwb_mac_addr *, WLP_ATTR_WSS_VIRT, wss_virt)
 162
 163 /**
 164  * Fill in the WSS information attributes
 165  *
 166  * We currently only support one WSS, and this is assumed in this function
 167  * that can populate only one WSS information attribute.
 168  */
 169 static size_t wlp_set_wss_info(struct wlp_attr_wss_info *attr,
 170                                struct wlp_wss *wss)
 171 {
 172         size_t datalen;
 173         void *ptr = attr->wss_info;
 174         size_t used = sizeof(*attr);
 175
 176         datalen = sizeof(struct wlp_wss_info) + strlen(wss->name);
 177         wlp_set_attr_hdr(&attr->hdr, WLP_ATTR_WSS_INFO, datalen);
 178         used = wlp_set_wssid(ptr, &wss->wssid);
 179         used += wlp_set_wss_name(ptr + used, wss->name, strlen(wss->name));
 180         used += wlp_set_accept_enrl(ptr + used, wss->accept_enroll);
 181         used += wlp_set_wss_sec_status(ptr + used, wss->secure_status);
 182         used += wlp_set_wss_bcast(ptr + used, &wss->bcast);
 183         return sizeof(*attr) + used;
 184 }
 185
 186 /**
 187  * Verify attribute header
 188  *
 189  * @hdr:     Pointer to attribute header that will be verified.
 190  * @type:    Expected attribute type.
 191  * @len:     Expected length of attribute value (excluding header).
 192  *
 193  * Most attribute values have a known length even when they do have a
 194  * length field. This knowledge can be used via this function to verify
 195  * that the length field matches the expected value.
 196  */
 197 static int wlp_check_attr_hdr(struct wlp *wlp, struct wlp_attr_hdr *hdr,
 198                        enum wlp_attr_type type, unsigned len)
 199 {
 200         struct device *dev = &wlp->rc->uwb_dev.dev;
 201
 202         if (le16_to_cpu(hdr->type) != type) {
 203                 dev_err(dev, "WLP: unexpected header type. Expected "
 204                         "%u, got %u.\n", type, le16_to_cpu(hdr->type));
 205                 return -EINVAL;
 206         }
 207         if (le16_to_cpu(hdr->length) != len) {
 208                 dev_err(dev, "WLP: unexpected length in header. Expected "
 209                         "%u, got %u.\n", len, le16_to_cpu(hdr->length));
 210                 return -EINVAL;
 211         }
 212         return 0;
 213 }
 214
 215 /**
 216  * Check if header of WSS information attribute valid
 217  *
 218  * @returns: length of WSS attributes (value of length attribute field) if
 219  *             valid WSS information attribute found
 220  *           -ENODATA if no WSS information attribute found
 221  *           -EIO other error occured
 222  *
 223  * The WSS information attribute is optional. The function will be provided
 224  * with a pointer to data that could _potentially_ be a WSS information
 225  * attribute. If a valid WSS information attribute is found it will return
 226  * 0, if no WSS information attribute is found it will return -ENODATA, and
 227  * another error will be returned if it is a WSS information attribute, but
 228  * some parsing failure occured.
 229  */
 230 static int wlp_check_wss_info_attr_hdr(struct wlp *wlp,
 231                                        struct wlp_attr_hdr *hdr, size_t buflen)
 232 {
 233         struct device *dev = &wlp->rc->uwb_dev.dev;
 234         size_t len;
 235         int result = 0;
 236
 237         if (buflen < sizeof(*hdr)) {
 238                 dev_err(dev, "WLP: Not enough space in buffer to parse"
 239                         " WSS information attribute header.\n");
 240                 result = -EIO;
 241                 goto out;
 242         }
 243         if (le16_to_cpu(hdr->type) != WLP_ATTR_WSS_INFO) {
 244                 /* WSS information is optional */
 245                 result = -ENODATA;
 246                 goto out;
 247         }
 248         len = le16_to_cpu(hdr->length);
 249         if (buflen < sizeof(*hdr) + len) {
 250                 dev_err(dev, "WLP: Not enough space in buffer to parse "
 251                         "variable data. Got %d, expected %d.\n",
 252                         (int)buflen, (int)(sizeof(*hdr) + len));
 253                 result = -EIO;
 254                 goto out;
 255         }
 256         result = len;
 257 out:
 258         return result;
 259 }
 260
 261
 262 /**
 263  * Get value of attribute from fixed size attribute field.
 264  *
 265  * @attr:    Pointer to attribute field.
 266  * @value:   Pointer to variable in which attribute value will be placed.
 267  * @buflen:  Size of buffer in which attribute field (including header)
 268  *           can be found.
 269  * @returns: Amount of given buffer consumed by parsing for this attribute.
 270  *
 271  * The size and type of the value is known by the type of the attribute.
 272  */
 273 #define wlp_get(type, type_code, name)                                  \
 274 ssize_t wlp_get_##name(struct wlp *wlp, struct wlp_attr_##name *attr,   \
 275                       type *value, ssize_t buflen)                      \
 276 {                                                                       \
 277         struct device *dev = &wlp->rc->uwb_dev.dev;                     \
 278         if (buflen < 0)                                                 \
 279                 return -EINVAL;                                         \
 280         if (buflen < sizeof(*attr)) {                                   \
 281                 dev_err(dev, "WLP: Not enough space in buffer to parse" \
 282                         " attribute field. Need %d, received %zu\n",    \
 283                         (int)sizeof(*attr), buflen);                    \
 284                 return -EIO;                                            \
 285         }                                                               \
 286         if (wlp_check_attr_hdr(wlp, &attr->hdr, type_code,              \
 287                                sizeof(attr->name)) < 0) {               \
 288                 dev_err(dev, "WLP: Header verification failed. \n");    \
 289                 return -EINVAL;                                         \
 290         }                                                               \
 291         *value = attr->name;                                            \
 292         return sizeof(*attr);                                           \
 293 }
 294
 295 #define wlp_get_sparse(type, type_code, name) \
 296         static wlp_get(type, type_code, name)
 297
 298 /**
 299  * Get value of attribute from variable sized attribute field.
 300  *
 301  * @max:     The maximum size of this attribute. This value is dictated by
 302  *           the maximum value from the WLP specification.
 303  *
 304  * @attr:    Pointer to attribute field.
 305  * @value:   Pointer to variable that will contain the value. The memory
 306  *           must already have been allocated for this value.
 307  * @buflen:  Size of buffer in which attribute field (including header)
 308  *           can be found.
 309  * @returns: Amount of given bufferconsumed by parsing for this attribute.
 310  */
 311 #define wlp_vget(type_val, type_code, name, max)                        \
 312 static ssize_t wlp_get_##name(struct wlp *wlp,                          \
 313                               struct wlp_attr_##name *attr,             \
 314                               type_val *value, ssize_t buflen)          \
 315 {                                                                       \
 316         struct device *dev = &wlp->rc->uwb_dev.dev;                     \
 317         size_t len;                                                     \
 318         if (buflen < 0)                                                 \
 319                 return -EINVAL;                                         \
 320         if (buflen < sizeof(*attr)) {                                   \
 321                 dev_err(dev, "WLP: Not enough space in buffer to parse" \
 322                         " header.\n");                                  \
 323                 return -EIO;                                            \
 324         }                                                               \
 325         if (le16_to_cpu(attr->hdr.type) != type_code) {                 \
 326                 dev_err(dev, "WLP: Unexpected attribute type. Got %u, " \
 327                         "expected %u.\n", le16_to_cpu(attr->hdr.type),  \
 328                         type_code);                                     \
 329                 return -EINVAL;                                         \
 330         }                                                               \
 331         len = le16_to_cpu(attr->hdr.length);                            \
 332         if (len > max) {                                                \
 333                 dev_err(dev, "WLP: Attribute larger than maximum "      \
 334                         "allowed. Received %zu, max is %d.\n", len,     \
 335                         (int)max);                                      \
 336                 return -EFBIG;                                          \
 337         }                                                               \
 338         if (buflen < sizeof(*attr) + len) {                             \
 339                 dev_err(dev, "WLP: Not enough space in buffer to parse "\
 340                         "variable data.\n");                            \
 341                 return -EIO;                                            \
 342         }                                                               \
 343         memcpy(value, (void *) attr + sizeof(*attr), len);              \
 344         return sizeof(*attr) + len;                                     \
 345 }
 346
 347 wlp_get(u8, WLP_ATTR_WLP_VER, version)
 348 wlp_get_sparse(enum wlp_wss_sel_mthd, WLP_ATTR_WSS_SEL_MTHD, wss_sel_mthd)
 349 wlp_get_sparse(struct wlp_dev_type, WLP_ATTR_PRI_DEV_TYPE, prim_dev_type)
 350 wlp_get_sparse(enum wlp_assc_error, WLP_ATTR_WLP_ASSC_ERR, wlp_assc_err)
 351 wlp_get_sparse(struct wlp_uuid, WLP_ATTR_UUID_E, uuid_e)
 352 wlp_get_sparse(struct wlp_uuid, WLP_ATTR_UUID_R, uuid_r)
 353 wlp_get(struct wlp_uuid, WLP_ATTR_WSSID, wssid)
 354 wlp_get_sparse(u8, WLP_ATTR_ACC_ENRL, accept_enrl)
 355 wlp_get_sparse(u8, WLP_ATTR_WSS_SEC_STAT, wss_sec_status)
 356 wlp_get_sparse(struct uwb_mac_addr, WLP_ATTR_WSS_BCAST, wss_bcast)
 357 wlp_get_sparse(u8, WLP_ATTR_WSS_TAG, wss_tag)
 358 wlp_get_sparse(struct uwb_mac_addr, WLP_ATTR_WSS_VIRT, wss_virt)
 359 wlp_get_sparse(struct wlp_nonce, WLP_ATTR_ENRL_NONCE, enonce)
 360 wlp_get_sparse(struct wlp_nonce, WLP_ATTR_REG_NONCE, rnonce)
 361
 362 /* The buffers for the device info attributes can be found in the
 363  * wlp_device_info struct. These buffers contain one byte more than the
 364  * max allowed by the spec - this is done to be able to add the
 365  * terminating \0 for user display. This terminating byte is not required
 366  * in the actual attribute field (because it has a length field) so the
 367  * maximum allowed for this value is one less than its size in the
 368  * structure.
 369  */
 370 wlp_vget(char, WLP_ATTR_WSS_NAME, wss_name,
 371          FIELD_SIZEOF(struct wlp_wss, name) - 1)
 372 wlp_vget(char, WLP_ATTR_DEV_NAME, dev_name,
 373          FIELD_SIZEOF(struct wlp_device_info, name) - 1)
 374 wlp_vget(char, WLP_ATTR_MANUF, manufacturer,
 375          FIELD_SIZEOF(struct wlp_device_info, manufacturer) - 1)
 376 wlp_vget(char, WLP_ATTR_MODEL_NAME, model_name,
 377          FIELD_SIZEOF(struct wlp_device_info, model_name) - 1)
 378 wlp_vget(char, WLP_ATTR_MODEL_NR, model_nr,
 379          FIELD_SIZEOF(struct wlp_device_info, model_nr) - 1)
 380 wlp_vget(char, WLP_ATTR_SERIAL, serial,
 381          FIELD_SIZEOF(struct wlp_device_info, serial) - 1)
 382
 383 /**
 384  * Retrieve WSS Name, Accept enroll, Secure status, Broadcast from WSS info
 385  *
 386  * @attr: pointer to WSS name attribute in WSS information attribute field
 387  * @info: structure that will be populated with data from WSS information
 388  *        field (WSS name, Accept enroll, secure status, broadcast address)
 389  * @buflen: size of buffer
 390  *
 391  * Although the WSSID attribute forms part of the WSS info attribute it is
 392  * retrieved separately and stored in a different location.
 393  */
 394 static ssize_t wlp_get_wss_info_attrs(struct wlp *wlp,
 395                                       struct wlp_attr_hdr *attr,
 396                                       struct wlp_wss_tmp_info *info,
 397                                       ssize_t buflen)
 398 {
 399         struct device *dev = &wlp->rc->uwb_dev.dev;
 400         void *ptr = attr;
 401         size_t used = 0;
 402         ssize_t result = -EINVAL;
 403
 404         result = wlp_get_wss_name(wlp, ptr, info->name, buflen);
 405         if (result < 0) {
 406                 dev_err(dev, "WLP: unable to obtain WSS name from "
 407                         "WSS info in D2 message.\n");
 408                 goto error_parse;
 409         }
 410         used += result;
 411
 412         result = wlp_get_accept_enrl(wlp, ptr + used, &info->accept_enroll,
 413                                      buflen - used);
 414         if (result < 0) {
 415                 dev_err(dev, "WLP: unable to obtain accepting "
 416                         "enrollment from WSS info in D2 message.\n");
 417                 goto error_parse;
 418         }
 419         if (info->accept_enroll != 0 && info->accept_enroll != 1) {
 420                 dev_err(dev, "WLP: invalid value for accepting "
 421                         "enrollment in D2 message.\n");
 422                 result = -EINVAL;
 423                 goto error_parse;
 424         }
 425         used += result;
 426
 427         result = wlp_get_wss_sec_status(wlp, ptr + used, &info->sec_status,
 428                                         buflen - used);
 429         if (result < 0) {
 430                 dev_err(dev, "WLP: unable to obtain secure "
 431                         "status from WSS info in D2 message.\n");
 432                 goto error_parse;
 433         }
 434         if (info->sec_status != 0 && info->sec_status != 1) {
 435                 dev_err(dev, "WLP: invalid value for secure "
 436                         "status in D2 message.\n");
 437                 result = -EINVAL;
 438                 goto error_parse;
 439         }
 440         used += result;
 441
 442         result = wlp_get_wss_bcast(wlp, ptr + used, &info->bcast,
 443                                    buflen - used);
 444         if (result < 0) {
 445                 dev_err(dev, "WLP: unable to obtain broadcast "
 446                         "address from WSS info in D2 message.\n");
 447                 goto error_parse;
 448         }
 449         used += result;
 450         result = used;
 451 error_parse:
 452         return result;
 453 }
 454
 455 /**
 456  * Create a new WSSID entry for the neighbor, allocate temporary storage
 457  *
 458  * Each neighbor can have many WSS active. We maintain a list of WSSIDs
 459  * advertised by neighbor. During discovery we also cache information about
 460  * these WSS in temporary storage.
 461  *
 462  * The temporary storage will be removed after it has been used (eg.
 463  * displayed to user), the wssid element will be removed from the list when
 464  * the neighbor is rediscovered or when it disappears.
 465  */
 466 static struct wlp_wssid_e *wlp_create_wssid_e(struct wlp *wlp,
 467                                               struct wlp_neighbor_e *neighbor)
 468 {
 469         struct device *dev = &wlp->rc->uwb_dev.dev;
 470         struct wlp_wssid_e *wssid_e;
 471
 472         wssid_e = kzalloc(sizeof(*wssid_e), GFP_KERNEL);
 473         if (wssid_e == NULL) {
 474                 dev_err(dev, "WLP: unable to allocate memory "
 475                         "for WSS information.\n");
 476                 goto error_alloc;
 477         }
 478         wssid_e->info = kzalloc(sizeof(struct wlp_wss_tmp_info), GFP_KERNEL);
 479         if (wssid_e->info == NULL) {
 480                 dev_err(dev, "WLP: unable to allocate memory "
 481                         "for temporary WSS information.\n");
 482                 kfree(wssid_e);
 483                 wssid_e = NULL;
 484                 goto error_alloc;
 485         }
 486         list_add(&wssid_e->node, &neighbor->wssid);
 487 error_alloc:
 488         return wssid_e;
 489 }
 490
 491 /**
 492  * Parse WSS information attribute
 493  *
 494  * @attr: pointer to WSS information attribute header
 495  * @buflen: size of buffer in which WSS information attribute appears
 496  * @wssid: will place wssid from WSS info attribute in this location
 497  * @wss_info: will place other information from WSS information attribute
 498  * in this location
 499  *
 500  * memory for @wssid and @wss_info must be allocated when calling this
 501  */
 502 static ssize_t wlp_get_wss_info(struct wlp *wlp, struct wlp_attr_wss_info *attr,
 503                                 size_t buflen, struct wlp_uuid *wssid,
 504                                 struct wlp_wss_tmp_info *wss_info)
 505 {
 506         struct device *dev = &wlp->rc->uwb_dev.dev;
 507         ssize_t result;
 508         size_t len;
 509         size_t used = 0;
 510         void *ptr;
 511
 512         result = wlp_check_wss_info_attr_hdr(wlp, (struct wlp_attr_hdr *)attr,
 513                                              buflen);
 514         if (result < 0)
 515                 goto out;
 516         len = result;
 517         used = sizeof(*attr);
 518         ptr = attr;
 519
 520         result = wlp_get_wssid(wlp, ptr + used, wssid, buflen - used);
 521         if (result < 0) {
 522                 dev_err(dev, "WLP: unable to obtain WSSID from WSS info.\n");
 523                 goto out;
 524         }
 525         used += result;
 526         result = wlp_get_wss_info_attrs(wlp, ptr + used, wss_info,
 527                                         buflen - used);
 528         if (result < 0) {
 529                 dev_err(dev, "WLP: unable to obtain WSS information "
 530                         "from WSS information attributes. \n");
 531                 goto out;
 532         }
 533         used += result;
 534         if (len + sizeof(*attr) != used) {
 535                 dev_err(dev, "WLP: Amount of data parsed does not "
 536                         "match length field. Parsed %zu, length "
 537                         "field %zu. \n", used, len);
 538                 result = -EINVAL;
 539                 goto out;
 540         }
 541         result = used;
 542 out:
 543         return result;
 544 }
 545
 546 /**
 547  * Retrieve WSS info from association frame
 548  *
 549  * @attr:     pointer to WSS information attribute
 550  * @neighbor: ptr to neighbor being discovered, NULL if enrollment in
 551  *            progress
 552  * @wss:      ptr to WSS being enrolled in, NULL if discovery in progress
 553  * @buflen:   size of buffer in which WSS information appears
 554  *
 555  * The WSS information attribute appears in the D2 association message.
 556  * This message is used in two ways: to discover all neighbors or to enroll
 557  * into a WSS activated by a neighbor. During discovery we only want to
 558  * store the WSS info in a cache, to be deleted right after it has been
 559  * used (eg. displayed to the user). During enrollment we store the WSS
 560  * information for the lifetime of enrollment.
 561  *
 562  * During discovery we are interested in all WSS information, during
 563  * enrollment we are only interested in the WSS being enrolled in. Even so,
 564  * when in enrollment we keep parsing the message after finding the WSS of
 565  * interest, this simplifies the calling routine in that it can be sure
 566  * that all WSS information attributes have been parsed out of the message.
 567  *
 568  * Association frame is process with nbmutex held. The list access is safe.
 569  */
 570 static ssize_t wlp_get_all_wss_info(struct wlp *wlp,
 571                                     struct wlp_attr_wss_info *attr,
 572                                     struct wlp_neighbor_e *neighbor,
 573                                     struct wlp_wss *wss, ssize_t buflen)
 574 {
 575         struct device *dev = &wlp->rc->uwb_dev.dev;
 576         size_t used = 0;
 577         ssize_t result = -EINVAL;
 578         struct wlp_attr_wss_info *cur;
 579         struct wlp_uuid wssid;
 580         struct wlp_wss_tmp_info wss_info;
 581         unsigned enroll; /* 0 - discovery to cache, 1 - enrollment */
 582         struct wlp_wssid_e *wssid_e;
 583         char buf[WLP_WSS_UUID_STRSIZE];
 584
 585         if (buflen < 0)
 586                 goto out;
 587
 588         if (neighbor != NULL && wss == NULL)
 589                 enroll = 0; /* discovery */
 590         else if (wss != NULL && neighbor == NULL)
 591                 enroll = 1; /* enrollment */
 592         else
 593                 goto out;
 594
 595         cur = attr;
 596         while (buflen - used > 0) {
 597                 memset(&wss_info, 0, sizeof(wss_info));
 598                 cur = (void *)cur + used;
 599                 result = wlp_get_wss_info(wlp, cur, buflen - used, &wssid,
 600                                           &wss_info);
 601                 if (result == -ENODATA) {
 602                         result = used;
 603                         goto out;
 604                 } else if (result < 0) {
 605                         dev_err(dev, "WLP: Unable to parse WSS information "
 606                                 "from WSS information attribute. \n");
 607                         result = -EINVAL;
 608                         goto error_parse;
 609                 }
 610                 if (enroll && !memcmp(&wssid, &wss->wssid, sizeof(wssid))) {
 611                         if (wss_info.accept_enroll != 1) {
 612                                 dev_err(dev, "WLP: Requested WSS does "
 613                                         "not accept enrollment.\n");
 614                                 result = -EINVAL;
 615                                 goto out;
 616                         }
 617                         memcpy(wss->name, wss_info.name, sizeof(wss->name));
 618                         wss->bcast = wss_info.bcast;
 619                         wss->secure_status = wss_info.sec_status;
 620                         wss->accept_enroll = wss_info.accept_enroll;
 621                         wss->state = WLP_WSS_STATE_PART_ENROLLED;
 622                         wlp_wss_uuid_print(buf, sizeof(buf), &wssid);
 623                         dev_dbg(dev, "WLP: Found WSS %s. Enrolling.\n", buf);
 624                 } else {
 625                         wssid_e = wlp_create_wssid_e(wlp, neighbor);
 626                         if (wssid_e == NULL) {
 627                                 dev_err(dev, "WLP: Cannot create new WSSID "
 628                                         "entry for neighbor %02x:%02x.\n",
 629                                         neighbor->uwb_dev->dev_addr.data[1],
 630                                         neighbor->uwb_dev->dev_addr.data[0]);
 631                                 result = -ENOMEM;
 632                                 goto out;
 633                         }
 634                         wssid_e->wssid = wssid;
 635                         *wssid_e->info = wss_info;
 636                 }
 637                 used += result;
 638         }
 639         result = used;
 640 error_parse:
 641         if (result < 0 && !enroll) /* this was a discovery */
 642                 wlp_remove_neighbor_tmp_info(neighbor);
 643 out:
 644         return result;
 645
 646 }
 647
 648 /**
 649  * Parse WSS information attributes into cache for discovery
 650  *
 651  * @attr: the first WSS information attribute in message
 652  * @neighbor: the neighbor whose cache will be populated
 653  * @buflen: size of the input buffer
 654  */
 655 static ssize_t wlp_get_wss_info_to_cache(struct wlp *wlp,
 656                                          struct wlp_attr_wss_info *attr,
 657                                          struct wlp_neighbor_e *neighbor,
 658                                          ssize_t buflen)
 659 {
 660         return wlp_get_all_wss_info(wlp, attr, neighbor, NULL, buflen);
 661 }
 662
 663 /**
 664  * Parse WSS information attributes into WSS struct for enrollment
 665  *
 666  * @attr: the first WSS information attribute in message
 667  * @wss: the WSS that will be enrolled
 668  * @buflen: size of the input buffer
 669  */
 670 static ssize_t wlp_get_wss_info_to_enroll(struct wlp *wlp,
 671                                           struct wlp_attr_wss_info *attr,
 672                                           struct wlp_wss *wss, ssize_t buflen)
 673 {
 674         return wlp_get_all_wss_info(wlp, attr, NULL, wss, buflen);
 675 }
 676
 677 /**
 678  * Construct a D1 association frame
 679  *
 680  * We use the radio control functions to determine the values of the device
 681  * properties. These are of variable length and the total space needed is
 682  * tallied first before we start constructing the message. The radio
 683  * control functions return strings that are terminated with \0. This
 684  * character should not be included in the message (there is a length field
 685  * accompanying it in the attribute).
 686  */
 687 static int wlp_build_assoc_d1(struct wlp *wlp, struct wlp_wss *wss,
 688                               struct sk_buff **skb)
 689 {
 690
 691         struct device *dev = &wlp->rc->uwb_dev.dev;
 692         int result = 0;
 693         struct wlp_device_info *info;
 694         size_t used = 0;
 695         struct wlp_frame_assoc *_d1;
 696         struct sk_buff *_skb;
 697         void *d1_itr;
 698
 699         if (wlp->dev_info == NULL) {
 700                 result = __wlp_setup_device_info(wlp);
 701                 if (result < 0) {
 702                         dev_err(dev, "WLP: Unable to setup device "
 703                                 "information for D1 message.\n");
 704                         goto error;
 705                 }
 706         }
 707         info = wlp->dev_info;
 708         _skb = dev_alloc_skb(sizeof(*_d1)
 709                       + sizeof(struct wlp_attr_uuid_e)
 710                       + sizeof(struct wlp_attr_wss_sel_mthd)
 711                       + sizeof(struct wlp_attr_dev_name)
 712                       + strlen(info->name)
 713                       + sizeof(struct wlp_attr_manufacturer)
 714                       + strlen(info->manufacturer)
 715                       + sizeof(struct wlp_attr_model_name)
 716                       + strlen(info->model_name)
 717                       + sizeof(struct wlp_attr_model_nr)
 718                       + strlen(info->model_nr)
 719                       + sizeof(struct wlp_attr_serial)
 720                       + strlen(info->serial)
 721                       + sizeof(struct wlp_attr_prim_dev_type)
 722                       + sizeof(struct wlp_attr_wlp_assc_err));
 723         if (_skb == NULL) {
 724                 dev_err(dev, "WLP: Cannot allocate memory for association "
 725                         "message.\n");
 726                 result = -ENOMEM;
 727                 goto error;
 728         }
 729         _d1 = (void *) _skb->data;
 730         _d1->hdr.mux_hdr = cpu_to_le16(WLP_PROTOCOL_ID);
 731         _d1->hdr.type = WLP_FRAME_ASSOCIATION;
 732         _d1->type = WLP_ASSOC_D1;
 733
 734         wlp_set_version(&_d1->version, WLP_VERSION);
 735         wlp_set_msg_type(&_d1->msg_type, WLP_ASSOC_D1);
 736         d1_itr = _d1->attr;
 737         used = wlp_set_uuid_e(d1_itr, &wlp->uuid);
 738         used += wlp_set_wss_sel_mthd(d1_itr + used, WLP_WSS_REG_SELECT);
 739         used += wlp_set_dev_name(d1_itr + used, info->name,
 740                                  strlen(info->name));
 741         used += wlp_set_manufacturer(d1_itr + used, info->manufacturer,
 742                                      strlen(info->manufacturer));
 743         used += wlp_set_model_name(d1_itr + used, info->model_name,
 744                                    strlen(info->model_name));
 745         used += wlp_set_model_nr(d1_itr + used, info->model_nr,
 746                                  strlen(info->model_nr));
 747         used += wlp_set_serial(d1_itr + used, info->serial,
 748                                strlen(info->serial));
 749         used += wlp_set_prim_dev_type(d1_itr + used, &info->prim_dev_type);
 750         used += wlp_set_wlp_assc_err(d1_itr + used, WLP_ASSOC_ERROR_NONE);
 751         skb_put(_skb, sizeof(*_d1) + used);
 752         *skb = _skb;
 753 error:
 754         return result;
 755 }
 756
 757 /**
 758  * Construct a D2 association frame
 759  *
 760  * We use the radio control functions to determine the values of the device
 761  * properties. These are of variable length and the total space needed is
 762  * tallied first before we start constructing the message. The radio
 763  * control functions return strings that are terminated with \0. This
 764  * character should not be included in the message (there is a length field
 765  * accompanying it in the attribute).
 766  */
 767 static
 768 int wlp_build_assoc_d2(struct wlp *wlp, struct wlp_wss *wss,
 769                        struct sk_buff **skb, struct wlp_uuid *uuid_e)
 770 {
 771
 772         struct device *dev = &wlp->rc->uwb_dev.dev;
 773         int result = 0;
 774         struct wlp_device_info *info;
 775         size_t used = 0;
 776         struct wlp_frame_assoc *_d2;
 777         struct sk_buff *_skb;
 778         void *d2_itr;
 779         size_t mem_needed;
 780
 781         if (wlp->dev_info == NULL) {
 782                 result = __wlp_setup_device_info(wlp);
 783                 if (result < 0) {
 784                         dev_err(dev, "WLP: Unable to setup device "
 785                                 "information for D2 message.\n");
 786                         goto error;
 787                 }
 788         }
 789         info = wlp->dev_info;
 790         mem_needed = sizeof(*_d2)
 791                       + sizeof(struct wlp_attr_uuid_e)
 792                       + sizeof(struct wlp_attr_uuid_r)
 793                       + sizeof(struct wlp_attr_dev_name)
 794                       + strlen(info->name)
 795                       + sizeof(struct wlp_attr_manufacturer)
 796                       + strlen(info->manufacturer)
 797                       + sizeof(struct wlp_attr_model_name)
 798                       + strlen(info->model_name)
 799                       + sizeof(struct wlp_attr_model_nr)
 800                       + strlen(info->model_nr)
 801                       + sizeof(struct wlp_attr_serial)
 802                       + strlen(info->serial)
 803                       + sizeof(struct wlp_attr_prim_dev_type)
 804                       + sizeof(struct wlp_attr_wlp_assc_err);
 805         if (wlp->wss.state >= WLP_WSS_STATE_ACTIVE)
 806                 mem_needed += sizeof(struct wlp_attr_wss_info)
 807                               + sizeof(struct wlp_wss_info)
 808                               + strlen(wlp->wss.name);
 809         _skb = dev_alloc_skb(mem_needed);
 810         if (_skb == NULL) {
 811                 dev_err(dev, "WLP: Cannot allocate memory for association "
 812                         "message.\n");
 813                 result = -ENOMEM;
 814                 goto error;
 815         }
 816         _d2 = (void *) _skb->data;
 817         _d2->hdr.mux_hdr = cpu_to_le16(WLP_PROTOCOL_ID);
 818         _d2->hdr.type = WLP_FRAME_ASSOCIATION;
 819         _d2->type = WLP_ASSOC_D2;
 820
 821         wlp_set_version(&_d2->version, WLP_VERSION);
 822         wlp_set_msg_type(&_d2->msg_type, WLP_ASSOC_D2);
 823         d2_itr = _d2->attr;
 824         used = wlp_set_uuid_e(d2_itr, uuid_e);
 825         used += wlp_set_uuid_r(d2_itr + used, &wlp->uuid);
 826         if (wlp->wss.state >= WLP_WSS_STATE_ACTIVE)
 827                 used += wlp_set_wss_info(d2_itr + used, &wlp->wss);
 828         used += wlp_set_dev_name(d2_itr + used, info->name,
 829                                  strlen(info->name));
 830         used += wlp_set_manufacturer(d2_itr + used, info->manufacturer,
 831                                      strlen(info->manufacturer));
 832         used += wlp_set_model_name(d2_itr + used, info->model_name,
 833                                    strlen(info->model_name));
 834         used += wlp_set_model_nr(d2_itr + used, info->model_nr,
 835                                  strlen(info->model_nr));
 836         used += wlp_set_serial(d2_itr + used, info->serial,
 837                                strlen(info->serial));
 838         used += wlp_set_prim_dev_type(d2_itr + used, &info->prim_dev_type);
 839         used += wlp_set_wlp_assc_err(d2_itr + used, WLP_ASSOC_ERROR_NONE);
 840         skb_put(_skb, sizeof(*_d2) + used);
 841         *skb = _skb;
 842 error:
 843         return result;
 844 }
 845
 846 /**
 847  * Allocate memory for and populate fields of F0 association frame
 848  *
 849  * Currently (while focusing on unsecure enrollment) we ignore the
 850  * nonce's that could be placed in the message. Only the error field is
 851  * populated by the value provided by the caller.
 852  */
 853 static
 854 int wlp_build_assoc_f0(struct wlp *wlp, struct sk_buff **skb,
 855                        enum wlp_assc_error error)
 856 {
 857         struct device *dev = &wlp->rc->uwb_dev.dev;
 858         int result = -ENOMEM;
 859         struct {
 860                 struct wlp_frame_assoc f0_hdr;
 861                 struct wlp_attr_enonce enonce;
 862                 struct wlp_attr_rnonce rnonce;
 863                 struct wlp_attr_wlp_assc_err assc_err;
 864         } *f0;
 865         struct sk_buff *_skb;
 866         struct wlp_nonce tmp;
 867
 868         _skb = dev_alloc_skb(sizeof(*f0));
 869         if (_skb == NULL) {
 870                 dev_err(dev, "WLP: Unable to allocate memory for F0 "
 871                         "association frame. \n");
 872                 goto error_alloc;
 873         }
 874         f0 = (void *) _skb->data;
 875         f0->f0_hdr.hdr.mux_hdr = cpu_to_le16(WLP_PROTOCOL_ID);
 876         f0->f0_hdr.hdr.type = WLP_FRAME_ASSOCIATION;
 877         f0->f0_hdr.type = WLP_ASSOC_F0;
 878         wlp_set_version(&f0->f0_hdr.version, WLP_VERSION);
 879         wlp_set_msg_type(&f0->f0_hdr.msg_type, WLP_ASSOC_F0);
 880         memset(&tmp, 0, sizeof(tmp));
 881         wlp_set_enonce(&f0->enonce, &tmp);
 882         wlp_set_rnonce(&f0->rnonce, &tmp);
 883         wlp_set_wlp_assc_err(&f0->assc_err, error);
 884         skb_put(_skb, sizeof(*f0));
 885         *skb = _skb;
 886         result = 0;
 887 error_alloc:
 888         return result;
 889 }
 890
 891 /**
 892  * Parse F0 frame
 893  *
 894  * We just retrieve the values and print it as an error to the user.
 895  * Calling function already knows an error occured (F0 indicates error), so
 896  * we just parse the content as debug for higher layers.
 897  */
 898 int wlp_parse_f0(struct wlp *wlp, struct sk_buff *skb)
 899 {
 900         struct device *dev = &wlp->rc->uwb_dev.dev;
 901         struct wlp_frame_assoc *f0 = (void *) skb->data;
 902         void *ptr = skb->data;
 903         size_t len = skb->len;
 904         size_t used;
 905         ssize_t result;
 906         struct wlp_nonce enonce, rnonce;
 907         enum wlp_assc_error assc_err;
 908         char enonce_buf[WLP_WSS_NONCE_STRSIZE];
 909         char rnonce_buf[WLP_WSS_NONCE_STRSIZE];
 910
 911         used = sizeof(*f0);
 912         result = wlp_get_enonce(wlp, ptr + used, &enonce, len - used);
 913         if (result < 0) {
 914                 dev_err(dev, "WLP: unable to obtain Enrollee nonce "
 915                         "attribute from F0 message.\n");
 916                 goto error_parse;
 917         }
 918         used += result;
 919         result = wlp_get_rnonce(wlp, ptr + used, &rnonce, len - used);
 920         if (result < 0) {
 921                 dev_err(dev, "WLP: unable to obtain Registrar nonce "
 922                         "attribute from F0 message.\n");
 923                 goto error_parse;
 924         }
 925         used += result;
 926         result = wlp_get_wlp_assc_err(wlp, ptr + used, &assc_err, len - used);
 927         if (result < 0) {
 928                 dev_err(dev, "WLP: unable to obtain WLP Association error "
 929                         "attribute from F0 message.\n");
 930                 goto error_parse;
 931         }
 932         wlp_wss_nonce_print(enonce_buf, sizeof(enonce_buf), &enonce);
 933         wlp_wss_nonce_print(rnonce_buf, sizeof(rnonce_buf), &rnonce);
 934         dev_err(dev, "WLP: Received F0 error frame from neighbor. Enrollee "
 935                 "nonce: %s, Registrar nonce: %s, WLP Association error: %s.\n",
 936                 enonce_buf, rnonce_buf, wlp_assc_error_str(assc_err));
 937         result = 0;
 938 error_parse:
 939         return result;
 940 }
 941
 942 /**
 943  * Retrieve variable device information from association message
 944  *
 945  * The device information parsed is not required in any message. This
 946  * routine will thus not fail if an attribute is not present.
 947  * The attributes are expected in a certain order, even if all are not
 948  * present. The "attribute type" value is used to ensure the attributes
 949  * are parsed in the correct order.
 950  *
 951  * If an error is encountered during parsing the function will return an
 952  * error code, when this happens the given device_info structure may be
 953  * partially filled.
 954  */
 955 static
 956 int wlp_get_variable_info(struct wlp *wlp, void *data,
 957                           struct wlp_device_info *dev_info, ssize_t len)
 958 {
 959         struct device *dev = &wlp->rc->uwb_dev.dev;
 960         size_t used = 0;
 961         struct wlp_attr_hdr *hdr;
 962         ssize_t result = 0;
 963         unsigned last = 0;
 964
 965         while (len - used > 0) {
 966                 if (len - used < sizeof(*hdr)) {
 967                         dev_err(dev, "WLP: Partial data in frame, cannot "
 968                                 "parse. \n");
 969                         goto error_parse;
 970                 }
 971                 hdr = data + used;
 972                 switch (le16_to_cpu(hdr->type)) {
 973                 case WLP_ATTR_MANUF:
 974                         if (last >= WLP_ATTR_MANUF) {
 975                                 dev_err(dev, "WLP: Incorrect order of "
 976                                         "attribute values in D1 msg.\n");
 977                                 goto error_parse;
 978                         }
 979                         result = wlp_get_manufacturer(wlp, data + used,
 980                                                       dev_info->manufacturer,
 981                                                       len - used);
 982                         if (result < 0) {
 983                                 dev_err(dev, "WLP: Unable to obtain "
 984                                         "Manufacturer attribute from D1 "
 985                                         "message.\n");
 986                                 goto error_parse;
 987                         }
 988                         last = WLP_ATTR_MANUF;
 989                         used += result;
 990                         break;
 991                 case WLP_ATTR_MODEL_NAME:
 992                         if (last >= WLP_ATTR_MODEL_NAME) {
 993                                 dev_err(dev, "WLP: Incorrect order of "
 994                                         "attribute values in D1 msg.\n");
 995                                 goto error_parse;
 996                         }
 997                         result = wlp_get_model_name(wlp, data + used,
 998                                                     dev_info->model_name,
 999                                                     len - used);
1000                         if (result < 0) {
1001                                 dev_err(dev, "WLP: Unable to obtain Model "
1002                                         "name attribute from D1 message.\n");
1003                                 goto error_parse;
1004                         }
1005                         last = WLP_ATTR_MODEL_NAME;
1006                         used += result;
1007                         break;
1008                 case WLP_ATTR_MODEL_NR:
1009                         if (last >= WLP_ATTR_MODEL_NR) {
1010                                 dev_err(dev, "WLP: Incorrect order of "
1011                                         "attribute values in D1 msg.\n");
1012                                 goto error_parse;
1013                         }
1014                         result = wlp_get_model_nr(wlp, data + used,
1015                                                   dev_info->model_nr,
1016                                                   len - used);
1017                         if (result < 0) {
1018                                 dev_err(dev, "WLP: Unable to obtain Model "
1019                                         "number attribute from D1 message.\n");
1020                                 goto error_parse;
1021                         }
1022                         last = WLP_ATTR_MODEL_NR;
1023                         used += result;
1024                         break;
1025                 case WLP_ATTR_SERIAL:
1026                         if (last >= WLP_ATTR_SERIAL) {
1027                                 dev_err(dev, "WLP: Incorrect order of "
1028                                         "attribute values in D1 msg.\n");
1029                                 goto error_parse;
1030                         }
1031                         result = wlp_get_serial(wlp, data + used,
1032                                                 dev_info->serial, len - used);
1033                         if (result < 0) {
1034                                 dev_err(dev, "WLP: Unable to obtain Serial "
1035                                         "number attribute from D1 message.\n");
1036                                 goto error_parse;
1037                         }
1038                         last = WLP_ATTR_SERIAL;
1039                         used += result;
1040                         break;
1041                 case WLP_ATTR_PRI_DEV_TYPE:
1042                         if (last >= WLP_ATTR_PRI_DEV_TYPE) {
1043                                 dev_err(dev, "WLP: Incorrect order of "
1044                                         "attribute values in D1 msg.\n");
1045                                 goto error_parse;
1046                         }
1047                         result = wlp_get_prim_dev_type(wlp, data + used,
1048                                                        &dev_info->prim_dev_type,
1049                                                        len - used);
1050                         if (result < 0) {
1051                                 dev_err(dev, "WLP: Unable to obtain Primary "
1052                                         "device type attribute from D1 "
1053                                         "message.\n");
1054                                 goto error_parse;
1055                         }
1056                         dev_info->prim_dev_type.category =
1057                                 le16_to_cpu(dev_info->prim_dev_type.category);
1058                         dev_info->prim_dev_type.subID =
1059                                 le16_to_cpu(dev_info->prim_dev_type.subID);
1060                         last = WLP_ATTR_PRI_DEV_TYPE;
1061                         used += result;
1062                         break;
1063                 default:
1064                         /* This is not variable device information. */
1065                         goto out;
1066                         break;
1067                 }
1068         }
1069 out:
1070         return used;
1071 error_parse:
1072         return -EINVAL;
1073 }
1074
1075 /**
1076  * Parse incoming D1 frame, populate attribute values
1077  *
1078  * Caller provides pointers to memory already allocated for attributes
1079  * expected in the D1 frame. These variables will be populated.
1080  */
1081 static
1082 int wlp_parse_d1_frame(struct wlp *wlp, struct sk_buff *skb,
1083                        struct wlp_uuid *uuid_e,
1084                        enum wlp_wss_sel_mthd *sel_mthd,
1085                        struct wlp_device_info *dev_info,
1086                        enum wlp_assc_error *assc_err)
1087 {
1088         struct device *dev = &wlp->rc->uwb_dev.dev;
1089         struct wlp_frame_assoc *d1 = (void *) skb->data;
1090         void *ptr = skb->data;
1091         size_t len = skb->len;
1092         size_t used;
1093         ssize_t result;
1094
1095         used = sizeof(*d1);
1096         result = wlp_get_uuid_e(wlp, ptr + used, uuid_e, len - used);
1097         if (result < 0) {
1098                 dev_err(dev, "WLP: unable to obtain UUID-E attribute from D1 "
1099                         "message.\n");
1100                 goto error_parse;
1101         }
1102         used += result;
1103         result = wlp_get_wss_sel_mthd(wlp, ptr + used, sel_mthd, len - used);
1104         if (result < 0) {
1105                 dev_err(dev, "WLP: unable to obtain WSS selection method "
1106                         "from D1 message.\n");
1107                 goto error_parse;
1108         }
1109         used += result;
1110         result = wlp_get_dev_name(wlp, ptr + used, dev_info->name,
1111                                      len - used);
1112         if (result < 0) {
1113                 dev_err(dev, "WLP: unable to obtain Device Name from D1 "
1114                         "message.\n");
1115                 goto error_parse;
1116         }
1117         used += result;
1118         result = wlp_get_variable_info(wlp, ptr + used, dev_info, len - used);
1119         if (result < 0) {
1120                 dev_err(dev, "WLP: unable to obtain Device Information from "
1121                         "D1 message.\n");
1122                 goto error_parse;
1123         }
1124         used += result;
1125         result = wlp_get_wlp_assc_err(wlp, ptr + used, assc_err, len - used);
1126         if (result < 0) {
1127                 dev_err(dev, "WLP: unable to obtain WLP Association Error "
1128                         "Information from D1 message.\n");
1129                 goto error_parse;
1130         }
1131         result = 0;
1132 error_parse:
1133         return result;
1134 }
1135 /**
1136  * Handle incoming D1 frame
1137  *
1138  * The frame has already been verified to contain an Association header with
1139  * the correct version number. Parse the incoming frame, construct and send
1140  * a D2 frame in response.
1141  *
1142  * It is not clear what to do with most fields in the incoming D1 frame. We
1143  * retrieve and discard the information here for now.
1144  */
1145 void wlp_handle_d1_frame(struct work_struct *ws)
1146 {
1147         struct wlp_assoc_frame_ctx *frame_ctx = container_of(ws,
1148                                                   struct wlp_assoc_frame_ctx,
1149                                                   ws);
1150         struct wlp *wlp = frame_ctx->wlp;
1151         struct wlp_wss *wss = &wlp->wss;
1152         struct sk_buff *skb = frame_ctx->skb;
1153         struct uwb_dev_addr *src = &frame_ctx->src;
1154         int result;
1155         struct device *dev = &wlp->rc->uwb_dev.dev;
1156         struct wlp_uuid uuid_e;
1157         enum wlp_wss_sel_mthd sel_mthd = 0;
1158         struct wlp_device_info dev_info;
1159         enum wlp_assc_error assc_err;
1160         struct sk_buff *resp = NULL;
1161
1162         /* Parse D1 frame */
1163         mutex_lock(&wss->mutex);
1164         mutex_lock(&wlp->mutex); /* to access wlp->uuid */
1165         memset(&dev_info, 0, sizeof(dev_info));
1166         result = wlp_parse_d1_frame(wlp, skb, &uuid_e, &sel_mthd, &dev_info,
1167                                     &assc_err);
1168         if (result < 0) {
1169                 dev_err(dev, "WLP: Unable to parse incoming D1 frame.\n");
1170                 kfree_skb(skb);
1171                 goto out;
1172         }
1173
1174         kfree_skb(skb);
1175         if (!wlp_uuid_is_set(&wlp->uuid)) {
1176                 dev_err(dev, "WLP: UUID is not set. Set via sysfs to "
1177                         "proceed. Respong to D1 message with error F0.\n");
1178                 result = wlp_build_assoc_f0(wlp, &resp,
1179                                             WLP_ASSOC_ERROR_NOT_READY);
1180                 if (result < 0) {
1181                         dev_err(dev, "WLP: Unable to construct F0 message.\n");
1182                         goto out;
1183                 }
1184         } else {
1185                 /* Construct D2 frame */
1186                 result = wlp_build_assoc_d2(wlp, wss, &resp, &uuid_e);
1187                 if (result < 0) {
1188                         dev_err(dev, "WLP: Unable to construct D2 message.\n");
1189                         goto out;
1190                 }
1191         }
1192         /* Send D2 frame */
1193         BUG_ON(wlp->xmit_frame == NULL);
1194         result = wlp->xmit_frame(wlp, resp, src);
1195         if (result < 0) {
1196                 dev_err(dev, "WLP: Unable to transmit D2 association "
1197                         "message: %d\n", result);
1198                 if (result == -ENXIO)
1199                         dev_err(dev, "WLP: Is network interface up? \n");
1200                 /* We could try again ... */
1201                 dev_kfree_skb_any(resp); /* we need to free if tx fails */
1202         }
1203 out:
1204         kfree(frame_ctx);
1205         mutex_unlock(&wlp->mutex);
1206         mutex_unlock(&wss->mutex);
1207 }
1208
1209 /**
1210  * Parse incoming D2 frame, create and populate temporary cache
1211  *
1212  * @skb: socket buffer in which D2 frame can be found
1213  * @neighbor: the neighbor that sent the D2 frame
1214  *
1215  * Will allocate memory for temporary storage of information learned during
1216  * discovery.
1217  */
1218 int wlp_parse_d2_frame_to_cache(struct wlp *wlp, struct sk_buff *skb,
1219                                 struct wlp_neighbor_e *neighbor)
1220 {
1221         struct device *dev = &wlp->rc->uwb_dev.dev;
1222         struct wlp_frame_assoc *d2 = (void *) skb->data;
1223         void *ptr = skb->data;
1224         size_t len = skb->len;
1225         size_t used;
1226         ssize_t result;
1227         struct wlp_uuid uuid_e;
1228         struct wlp_device_info *nb_info;
1229         enum wlp_assc_error assc_err;
1230
1231         used = sizeof(*d2);
1232         result = wlp_get_uuid_e(wlp, ptr + used, &uuid_e, len - used);
1233         if (result < 0) {
1234                 dev_err(dev, "WLP: unable to obtain UUID-E attribute from D2 "
1235                         "message.\n");
1236                 goto error_parse;
1237         }
1238         if (memcmp(&uuid_e, &wlp->uuid, sizeof(uuid_e))) {
1239                 dev_err(dev, "WLP: UUID-E in incoming D2 does not match "
1240                         "local UUID sent in D1. \n");
1241                 goto error_parse;
1242         }
1243         used += result;
1244         result = wlp_get_uuid_r(wlp, ptr + used, &neighbor->uuid, len - used);
1245         if (result < 0) {
1246                 dev_err(dev, "WLP: unable to obtain UUID-R attribute from D2 "
1247                         "message.\n");
1248                 goto error_parse;
1249         }
1250         used += result;
1251         result = wlp_get_wss_info_to_cache(wlp, ptr + used, neighbor,
1252                                            len - used);
1253         if (result < 0) {
1254                 dev_err(dev, "WLP: unable to obtain WSS information "
1255                         "from D2 message.\n");
1256                 goto error_parse;
1257         }
1258         used += result;
1259         neighbor->info = kzalloc(sizeof(struct wlp_device_info), GFP_KERNEL);
1260         if (neighbor->info == NULL) {
1261                 dev_err(dev, "WLP: cannot allocate memory to store device "
1262                         "info.\n");
1263                 result = -ENOMEM;
1264                 goto error_parse;
1265         }
1266         nb_info = neighbor->info;
1267         result = wlp_get_dev_name(wlp, ptr + used, nb_info->name,
1268                                   len - used);
1269         if (result < 0) {
1270                 dev_err(dev, "WLP: unable to obtain Device Name from D2 "
1271                         "message.\n");
1272                 goto error_parse;
1273         }
1274         used += result;
1275         result = wlp_get_variable_info(wlp, ptr + used, nb_info, len - used);
1276         if (result < 0) {
1277                 dev_err(dev, "WLP: unable to obtain Device Information from "
1278                         "D2 message.\n");
1279                 goto error_parse;
1280         }
1281         used += result;
1282         result = wlp_get_wlp_assc_err(wlp, ptr + used, &assc_err, len - used);
1283         if (result < 0) {
1284                 dev_err(dev, "WLP: unable to obtain WLP Association Error "
1285                         "Information from D2 message.\n");
1286                 goto error_parse;
1287         }
1288         if (assc_err != WLP_ASSOC_ERROR_NONE) {
1289                 dev_err(dev, "WLP: neighbor device returned association "
1290                         "error %d\n", assc_err);
1291                 result = -EINVAL;
1292                 goto error_parse;
1293         }
1294         result = 0;
1295 error_parse:
1296         if (result < 0)
1297                 wlp_remove_neighbor_tmp_info(neighbor);
1298         return result;
1299 }
1300
1301 /**
1302  * Parse incoming D2 frame, populate attribute values of WSS bein enrolled in
1303  *
1304  * @wss: our WSS that will be enrolled
1305  * @skb: socket buffer in which D2 frame can be found
1306  * @neighbor: the neighbor that sent the D2 frame
1307  * @wssid: the wssid of the WSS in which we want to enroll
1308  *
1309  * Forms part of enrollment sequence. We are trying to enroll in WSS with
1310  * @wssid by using @neighbor as registrar. A D1 message was sent to
1311  * @neighbor and now we need to parse the D2 response. The neighbor's
1312  * response is searched for the requested WSS and if found (and it accepts
1313  * enrollment), we store the information.
1314  */
1315 int wlp_parse_d2_frame_to_enroll(struct wlp_wss *wss, struct sk_buff *skb,
1316                                  struct wlp_neighbor_e *neighbor,
1317                                  struct wlp_uuid *wssid)
1318 {
1319         struct wlp *wlp = container_of(wss, struct wlp, wss);
1320         struct device *dev = &wlp->rc->uwb_dev.dev;
1321         void *ptr = skb->data;
1322         size_t len = skb->len;
1323         size_t used;
1324         ssize_t result;
1325         struct wlp_uuid uuid_e;
1326         struct wlp_uuid uuid_r;
1327         struct wlp_device_info nb_info;
1328         enum wlp_assc_error assc_err;
1329         char uuid_bufA[WLP_WSS_UUID_STRSIZE];
1330         char uuid_bufB[WLP_WSS_UUID_STRSIZE];
1331
1332         used = sizeof(struct wlp_frame_assoc);
1333         result = wlp_get_uuid_e(wlp, ptr + used, &uuid_e, len - used);
1334         if (result < 0) {
1335                 dev_err(dev, "WLP: unable to obtain UUID-E attribute from D2 "
1336                         "message.\n");
1337                 goto error_parse;
1338         }
1339         if (memcmp(&uuid_e, &wlp->uuid, sizeof(uuid_e))) {
1340                 dev_err(dev, "WLP: UUID-E in incoming D2 does not match "
1341                         "local UUID sent in D1. \n");
1342                 goto error_parse;
1343         }
1344         used += result;
1345         result = wlp_get_uuid_r(wlp, ptr + used, &uuid_r, len - used);
1346         if (result < 0) {
1347                 dev_err(dev, "WLP: unable to obtain UUID-R attribute from D2 "
1348                         "message.\n");
1349                 goto error_parse;
1350         }
1351         if (memcmp(&uuid_r, &neighbor->uuid, sizeof(uuid_r))) {
1352                 wlp_wss_uuid_print(uuid_bufA, sizeof(uuid_bufA),
1353                                    &neighbor->uuid);
1354                 wlp_wss_uuid_print(uuid_bufB, sizeof(uuid_bufB), &uuid_r);
1355                 dev_err(dev, "WLP: UUID of neighbor does not match UUID "
1356                         "learned during discovery. Originally discovered: %s, "
1357                         "now from D2 message: %s\n", uuid_bufA, uuid_bufB);
1358                 result = -EINVAL;
1359                 goto error_parse;
1360         }
1361         used += result;
1362         wss->wssid = *wssid;
1363         result = wlp_get_wss_info_to_enroll(wlp, ptr + used, wss, len - used);
1364         if (result < 0) {
1365                 dev_err(dev, "WLP: unable to obtain WSS information "
1366                         "from D2 message.\n");
1367                 goto error_parse;
1368         }
1369         if (wss->state != WLP_WSS_STATE_PART_ENROLLED) {
1370                 dev_err(dev, "WLP: D2 message did not contain information "
1371                         "for successful enrollment. \n");
1372                 result = -EINVAL;
1373                 goto error_parse;
1374         }
1375         used += result;
1376         /* Place device information on stack to continue parsing of message */
1377         result = wlp_get_dev_name(wlp, ptr + used, nb_info.name,
1378                                   len - used);
1379         if (result < 0) {
1380                 dev_err(dev, "WLP: unable to obtain Device Name from D2 "
1381                         "message.\n");
1382                 goto error_parse;
1383         }
1384         used += result;
1385         result = wlp_get_variable_info(wlp, ptr + used, &nb_info, len - used);
1386         if (result < 0) {
1387                 dev_err(dev, "WLP: unable to obtain Device Information from "
1388                         "D2 message.\n");
1389                 goto error_parse;
1390         }
1391         used += result;
1392         result = wlp_get_wlp_assc_err(wlp, ptr + used, &assc_err, len - used);
1393         if (result < 0) {
1394                 dev_err(dev, "WLP: unable to obtain WLP Association Error "
1395                         "Information from D2 message.\n");
1396                 goto error_parse;
1397         }
1398         if (assc_err != WLP_ASSOC_ERROR_NONE) {
1399                 dev_err(dev, "WLP: neighbor device returned association "
1400                         "error %d\n", assc_err);
1401                 if (wss->state == WLP_WSS_STATE_PART_ENROLLED) {
1402                         dev_err(dev, "WLP: Enrolled in WSS (should not "
1403                                 "happen according to spec). Undoing. \n");
1404                         wlp_wss_reset(wss);
1405                 }
1406                 result = -EINVAL;
1407                 goto error_parse;
1408         }
1409         result = 0;
1410 error_parse:
1411         return result;
1412 }
1413
1414 /**
1415  * Parse C3/C4 frame into provided variables
1416  *
1417  * @wssid: will point to copy of wssid retrieved from C3/C4 frame
1418  * @tag:   will point to copy of tag retrieved from C3/C4 frame
1419  * @virt_addr: will point to copy of virtual address retrieved from C3/C4
1420  * frame.
1421  *
1422  * Calling function has to allocate memory for these values.
1423  *
1424  * skb contains a valid C3/C4 frame, return the individual fields of this
1425  * frame in the provided variables.
1426  */
1427 int wlp_parse_c3c4_frame(struct wlp *wlp, struct sk_buff *skb,
1428                        struct wlp_uuid *wssid, u8 *tag,
1429                        struct uwb_mac_addr *virt_addr)
1430 {
1431         struct device *dev = &wlp->rc->uwb_dev.dev;
1432         int result;
1433         void *ptr = skb->data;
1434         size_t len = skb->len;
1435         size_t used;
1436         struct wlp_frame_assoc *assoc = ptr;
1437
1438         used = sizeof(*assoc);
1439         result = wlp_get_wssid(wlp, ptr + used, wssid, len - used);
1440         if (result < 0) {
1441                 dev_err(dev, "WLP: unable to obtain WSSID attribute from "
1442                         "%s message.\n", wlp_assoc_frame_str(assoc->type));
1443                 goto error_parse;
1444         }
1445         used += result;
1446         result = wlp_get_wss_tag(wlp, ptr + used, tag, len - used);
1447         if (result < 0) {
1448                 dev_err(dev, "WLP: unable to obtain WSS tag attribute from "
1449                         "%s message.\n", wlp_assoc_frame_str(assoc->type));
1450                 goto error_parse;
1451         }
1452         used += result;
1453         result = wlp_get_wss_virt(wlp, ptr + used, virt_addr, len - used);
1454         if (result < 0) {
1455                 dev_err(dev, "WLP: unable to obtain WSS virtual address "
1456                         "attribute from %s message.\n",
1457                         wlp_assoc_frame_str(assoc->type));
1458                 goto error_parse;
1459         }
1460 error_parse:
1461         return result;
1462 }
1463
1464 /**
1465  * Allocate memory for and populate fields of C1 or C2 association frame
1466  *
1467  * The C1 and C2 association frames appear identical - except for the type.
1468  */
1469 static
1470 int wlp_build_assoc_c1c2(struct wlp *wlp, struct wlp_wss *wss,
1471                          struct sk_buff **skb, enum wlp_assoc_type type)
1472 {
1473         struct device *dev = &wlp->rc->uwb_dev.dev;
1474         int result  = -ENOMEM;
1475         struct {
1476                 struct wlp_frame_assoc c_hdr;
1477                 struct wlp_attr_wssid wssid;
1478         } *c;
1479         struct sk_buff *_skb;
1480
1481         _skb = dev_alloc_skb(sizeof(*c));
1482         if (_skb == NULL) {
1483                 dev_err(dev, "WLP: Unable to allocate memory for C1/C2 "
1484                         "association frame. \n");
1485                 goto error_alloc;
1486         }
1487         c = (void *) _skb->data;
1488         c->c_hdr.hdr.mux_hdr = cpu_to_le16(WLP_PROTOCOL_ID);
1489         c->c_hdr.hdr.type = WLP_FRAME_ASSOCIATION;
1490         c->c_hdr.type = type;
1491         wlp_set_version(&c->c_hdr.version, WLP_VERSION);
1492         wlp_set_msg_type(&c->c_hdr.msg_type, type);
1493         wlp_set_wssid(&c->wssid, &wss->wssid);
1494         skb_put(_skb, sizeof(*c));
1495         *skb = _skb;
1496         result = 0;
1497 error_alloc:
1498         return result;
1499 }
1500
1501
1502 static
1503 int wlp_build_assoc_c1(struct wlp *wlp, struct wlp_wss *wss,
1504                        struct sk_buff **skb)
1505 {
1506         return wlp_build_assoc_c1c2(wlp, wss, skb, WLP_ASSOC_C1);
1507 }
1508
1509 static
1510 int wlp_build_assoc_c2(struct wlp *wlp, struct wlp_wss *wss,
1511                        struct sk_buff **skb)
1512 {
1513         return wlp_build_assoc_c1c2(wlp, wss, skb, WLP_ASSOC_C2);
1514 }
1515
1516
1517 /**
1518  * Allocate memory for and populate fields of C3 or C4 association frame
1519  *
1520  * The C3 and C4 association frames appear identical - except for the type.
1521  */
1522 static
1523 int wlp_build_assoc_c3c4(struct wlp *wlp, struct wlp_wss *wss,
1524                          struct sk_buff **skb, enum wlp_assoc_type type)
1525 {
1526         struct device *dev = &wlp->rc->uwb_dev.dev;
1527         int result  = -ENOMEM;
1528         struct {
1529                 struct wlp_frame_assoc c_hdr;
1530                 struct wlp_attr_wssid wssid;
1531                 struct wlp_attr_wss_tag wss_tag;
1532                 struct wlp_attr_wss_virt wss_virt;
1533         } *c;
1534         struct sk_buff *_skb;
1535
1536         _skb = dev_alloc_skb(sizeof(*c));
1537         if (_skb == NULL) {
1538                 dev_err(dev, "WLP: Unable to allocate memory for C3/C4 "
1539                         "association frame. \n");
1540                 goto error_alloc;
1541         }
1542         c = (void *) _skb->data;
1543         c->c_hdr.hdr.mux_hdr = cpu_to_le16(WLP_PROTOCOL_ID);
1544         c->c_hdr.hdr.type = WLP_FRAME_ASSOCIATION;
1545         c->c_hdr.type = type;
1546         wlp_set_version(&c->c_hdr.version, WLP_VERSION);
1547         wlp_set_msg_type(&c->c_hdr.msg_type, type);
1548         wlp_set_wssid(&c->wssid, &wss->wssid);
1549         wlp_set_wss_tag(&c->wss_tag, wss->tag);
1550         wlp_set_wss_virt(&c->wss_virt, &wss->virtual_addr);
1551         skb_put(_skb, sizeof(*c));
1552         *skb = _skb;
1553         result = 0;
1554 error_alloc:
1555         return result;
1556 }
1557
1558 static
1559 int wlp_build_assoc_c3(struct wlp *wlp, struct wlp_wss *wss,
1560                        struct sk_buff **skb)
1561 {
1562         return wlp_build_assoc_c3c4(wlp, wss, skb, WLP_ASSOC_C3);
1563 }
1564
1565 static
1566 int wlp_build_assoc_c4(struct wlp *wlp, struct wlp_wss *wss,
1567                        struct sk_buff **skb)
1568 {
1569         return wlp_build_assoc_c3c4(wlp, wss, skb, WLP_ASSOC_C4);
1570 }
1571
1572
1573 #define wlp_send_assoc(type, id)                                        \
1574 static int wlp_send_assoc_##type(struct wlp *wlp, struct wlp_wss *wss,  \
1575                                  struct uwb_dev_addr *dev_addr)         \
1576 {                                                                       \
1577         struct device *dev = &wlp->rc->uwb_dev.dev;                     \
1578         int result;                                                     \
1579         struct sk_buff *skb = NULL;                                     \
1580                                                                         \
1581         /* Build the frame */                                           \
1582         result = wlp_build_assoc_##type(wlp, wss, &skb);                \
1583         if (result < 0) {                                               \
1584                 dev_err(dev, "WLP: Unable to construct %s association " \
1585                         "frame: %d\n", wlp_assoc_frame_str(id), result);\
1586                 goto error_build_assoc;                                 \
1587         }                                                               \
1588         /* Send the frame */                                            \
1589         BUG_ON(wlp->xmit_frame == NULL);                                \
1590         result = wlp->xmit_frame(wlp, skb, dev_addr);                   \
1591         if (result < 0) {                                               \
1592                 dev_err(dev, "WLP: Unable to transmit %s association "  \
1593                         "message: %d\n", wlp_assoc_frame_str(id),       \
1594                         result);                                        \
1595                 if (result == -ENXIO)                                   \
1596                         dev_err(dev, "WLP: Is network interface "       \
1597                                 "up? \n");                              \
1598                 goto error_xmit;                                        \
1599         }                                                               \
1600         return 0;                                                       \
1601 error_xmit:                                                             \
1602         /* We could try again ... */                                    \
1603         dev_kfree_skb_any(skb);/*we need to free if tx fails*/          \
1604 error_build_assoc:                                                      \
1605         return result;                                                  \
1606 }
1607
1608 wlp_send_assoc(d1, WLP_ASSOC_D1)
1609 wlp_send_assoc(c1, WLP_ASSOC_C1)
1610 wlp_send_assoc(c3, WLP_ASSOC_C3)
1611
1612 int wlp_send_assoc_frame(struct wlp *wlp, struct wlp_wss *wss,
1613                          struct uwb_dev_addr *dev_addr,
1614                          enum wlp_assoc_type type)
1615 {
1616         int result = 0;
1617         struct device *dev = &wlp->rc->uwb_dev.dev;
1618         switch (type) {
1619         case WLP_ASSOC_D1:
1620                 result = wlp_send_assoc_d1(wlp, wss, dev_addr);
1621                 break;
1622         case WLP_ASSOC_C1:
1623                 result = wlp_send_assoc_c1(wlp, wss, dev_addr);
1624                 break;
1625         case WLP_ASSOC_C3:
1626                 result = wlp_send_assoc_c3(wlp, wss, dev_addr);
1627                 break;
1628         default:
1629                 dev_err(dev, "WLP: Received request to send unknown "
1630                         "association message.\n");
1631                 result = -EINVAL;
1632                 break;
1633         }
1634         return result;
1635 }
1636
1637 /**
1638  * Handle incoming C1 frame
1639  *
1640  * The frame has already been verified to contain an Association header with
1641  * the correct version number. Parse the incoming frame, construct and send
1642  * a C2 frame in response.
1643  */
1644 void wlp_handle_c1_frame(struct work_struct *ws)
1645 {
1646         struct wlp_assoc_frame_ctx *frame_ctx = container_of(ws,
1647                                                   struct wlp_assoc_frame_ctx,
1648                                                   ws);
1649         struct wlp *wlp = frame_ctx->wlp;
1650         struct wlp_wss *wss = &wlp->wss;
1651         struct device *dev = &wlp->rc->uwb_dev.dev;
1652         struct wlp_frame_assoc *c1 = (void *) frame_ctx->skb->data;
1653         unsigned int len = frame_ctx->skb->len;
1654         struct uwb_dev_addr *src = &frame_ctx->src;
1655         int result;
1656         struct wlp_uuid wssid;
1657         struct sk_buff *resp = NULL;
1658
1659         /* Parse C1 frame */
1660         mutex_lock(&wss->mutex);
1661         result = wlp_get_wssid(wlp, (void *)c1 + sizeof(*c1), &wssid,
1662                                len - sizeof(*c1));
1663         if (result < 0) {
1664                 dev_err(dev, "WLP: unable to obtain WSSID from C1 frame.\n");
1665                 goto out;
1666         }
1667         if (!memcmp(&wssid, &wss->wssid, sizeof(wssid))
1668             && wss->state == WLP_WSS_STATE_ACTIVE) {
1669                 /* Construct C2 frame */
1670                 result = wlp_build_assoc_c2(wlp, wss, &resp);
1671                 if (result < 0) {
1672                         dev_err(dev, "WLP: Unable to construct C2 message.\n");
1673                         goto out;
1674                 }
1675         } else {
1676                 /* Construct F0 frame */
1677                 result = wlp_build_assoc_f0(wlp, &resp, WLP_ASSOC_ERROR_INV);
1678                 if (result < 0) {
1679                         dev_err(dev, "WLP: Unable to construct F0 message.\n");
1680                         goto out;
1681                 }
1682         }
1683         /* Send C2 frame */
1684         BUG_ON(wlp->xmit_frame == NULL);
1685         result = wlp->xmit_frame(wlp, resp, src);
1686         if (result < 0) {
1687                 dev_err(dev, "WLP: Unable to transmit response association "
1688                         "message: %d\n", result);
1689                 if (result == -ENXIO)
1690                         dev_err(dev, "WLP: Is network interface up? \n");
1691                 /* We could try again ... */
1692                 dev_kfree_skb_any(resp); /* we need to free if tx fails */
1693         }
1694 out:
1695         kfree_skb(frame_ctx->skb);
1696         kfree(frame_ctx);
1697         mutex_unlock(&wss->mutex);
1698 }
1699
1700 /**
1701  * Handle incoming C3 frame
1702  *
1703  * The frame has already been verified to contain an Association header with
1704  * the correct version number. Parse the incoming frame, construct and send
1705  * a C4 frame in response. If the C3 frame identifies a WSS that is locally
1706  * active then we connect to this neighbor (add it to our EDA cache).
1707  */
1708 void wlp_handle_c3_frame(struct work_struct *ws)
1709 {
1710         struct wlp_assoc_frame_ctx *frame_ctx = container_of(ws,
1711                                                   struct wlp_assoc_frame_ctx,
1712                                                   ws);
1713         struct wlp *wlp = frame_ctx->wlp;
1714         struct wlp_wss *wss = &wlp->wss;
1715         struct device *dev = &wlp->rc->uwb_dev.dev;
1716         struct sk_buff *skb = frame_ctx->skb;
1717         struct uwb_dev_addr *src = &frame_ctx->src;
1718         int result;
1719         struct sk_buff *resp = NULL;
1720         struct wlp_uuid wssid;
1721         u8 tag;
1722         struct uwb_mac_addr virt_addr;
1723
1724         /* Parse C3 frame */
1725         mutex_lock(&wss->mutex);
1726         result = wlp_parse_c3c4_frame(wlp, skb, &wssid, &tag, &virt_addr);
1727         if (result < 0) {
1728                 dev_err(dev, "WLP: unable to obtain values from C3 frame.\n");
1729                 goto out;
1730         }
1731         if (!memcmp(&wssid, &wss->wssid, sizeof(wssid))
1732             && wss->state >= WLP_WSS_STATE_ACTIVE) {
1733                 result = wlp_eda_update_node(&wlp->eda, src, wss,
1734                                              (void *) virt_addr.data, tag,
1735                                              WLP_WSS_CONNECTED);
1736                 if (result < 0) {
1737                         dev_err(dev, "WLP: Unable to update EDA cache "
1738                                 "with new connected neighbor information.\n");
1739                         result = wlp_build_assoc_f0(wlp, &resp,
1740                                                     WLP_ASSOC_ERROR_INT);
1741                         if (result < 0) {
1742                                 dev_err(dev, "WLP: Unable to construct F0 "
1743                                         "message.\n");
1744                                 goto out;
1745                         }
1746                 } else {
1747                         wss->state = WLP_WSS_STATE_CONNECTED;
1748                         /* Construct C4 frame */
1749                         result = wlp_build_assoc_c4(wlp, wss, &resp);
1750                         if (result < 0) {
1751                                 dev_err(dev, "WLP: Unable to construct C4 "
1752                                         "message.\n");
1753                                 goto out;
1754                         }
1755                 }
1756         } else {
1757                 /* Construct F0 frame */
1758                 result = wlp_build_assoc_f0(wlp, &resp, WLP_ASSOC_ERROR_INV);
1759                 if (result < 0) {
1760                         dev_err(dev, "WLP: Unable to construct F0 message.\n");
1761                         goto out;
1762                 }
1763         }
1764         /* Send C4 frame */
1765         BUG_ON(wlp->xmit_frame == NULL);
1766         result = wlp->xmit_frame(wlp, resp, src);
1767         if (result < 0) {
1768                 dev_err(dev, "WLP: Unable to transmit response association "
1769                         "message: %d\n", result);
1770                 if (result == -ENXIO)
1771                         dev_err(dev, "WLP: Is network interface up? \n");
1772                 /* We could try again ... */
1773                 dev_kfree_skb_any(resp); /* we need to free if tx fails */
1774         }
1775 out:
1776         kfree_skb(frame_ctx->skb);
1777         kfree(frame_ctx);
1778         mutex_unlock(&wss->mutex);
1779 }
1780
1781