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