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