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