drivers/staging/wilc1000/coreconfigurator.c

   1 // SPDX-License-Identifier: GPL-2.0
   2 /*
   3  * Copyright (c) 2012 - 2018 Microchip Technology Inc., and its subsidiaries.
   4  * All rights reserved.
   5  */
   6
   7 #include <linux/ieee80211.h>
   8
   9 #include "coreconfigurator.h"
  10
  11 #define TAG_PARAM_OFFSET        (MAC_HDR_LEN + TIME_STAMP_LEN + \
  12                                  BEACON_INTERVAL_LEN + CAP_INFO_LEN)
  13
  14 enum sub_frame_type {
  15         ASSOC_REQ             = 0x00,
  16         ASSOC_RSP             = 0x10,
  17         REASSOC_REQ           = 0x20,
  18         REASSOC_RSP           = 0x30,
  19         PROBE_REQ             = 0x40,
  20         PROBE_RSP             = 0x50,
  21         BEACON                = 0x80,
  22         ATIM                  = 0x90,
  23         DISASOC               = 0xA0,
  24         AUTH                  = 0xB0,
  25         DEAUTH                = 0xC0,
  26         ACTION                = 0xD0,
  27         PS_POLL               = 0xA4,
  28         RTS                   = 0xB4,
  29         CTS                   = 0xC4,
  30         ACK                   = 0xD4,
  31         CFEND                 = 0xE4,
  32         CFEND_ACK             = 0xF4,
  33         DATA                  = 0x08,
  34         DATA_ACK              = 0x18,
  35         DATA_POLL             = 0x28,
  36         DATA_POLL_ACK         = 0x38,
  37         NULL_FRAME            = 0x48,
  38         CFACK                 = 0x58,
  39         CFPOLL                = 0x68,
  40         CFPOLL_ACK            = 0x78,
  41         QOS_DATA              = 0x88,
  42         QOS_DATA_ACK          = 0x98,
  43         QOS_DATA_POLL         = 0xA8,
  44         QOS_DATA_POLL_ACK     = 0xB8,
  45         QOS_NULL_FRAME        = 0xC8,
  46         QOS_CFPOLL            = 0xE8,
  47         QOS_CFPOLL_ACK        = 0xF8,
  48         BLOCKACK_REQ          = 0x84,
  49         BLOCKACK              = 0x94,
  50         FRAME_SUBTYPE_FORCE_32BIT  = 0xFFFFFFFF
  51 };
  52
  53 static inline u16 get_beacon_period(u8 *data)
  54 {
  55         u16 bcn_per;
  56
  57         bcn_per  = data[0];
  58         bcn_per |= (data[1] << 8);
  59
  60         return bcn_per;
  61 }
  62
  63 static inline u32 get_beacon_timestamp_lo(u8 *data)
  64 {
  65         u32 time_stamp = 0;
  66         u32 index    = MAC_HDR_LEN;
  67
  68         time_stamp |= data[index++];
  69         time_stamp |= (data[index++] << 8);
  70         time_stamp |= (data[index++] << 16);
  71         time_stamp |= (data[index]   << 24);
  72
  73         return time_stamp;
  74 }
  75
  76 static inline u32 get_beacon_timestamp_hi(u8 *data)
  77 {
  78         u32 time_stamp = 0;
  79         u32 index    = (MAC_HDR_LEN + 4);
  80
  81         time_stamp |= data[index++];
  82         time_stamp |= (data[index++] << 8);
  83         time_stamp |= (data[index++] << 16);
  84         time_stamp |= (data[index]   << 24);
  85
  86         return time_stamp;
  87 }
  88
  89 static inline enum sub_frame_type get_sub_type(u8 *header)
  90 {
  91         return ((enum sub_frame_type)(header[0] & 0xFC));
  92 }
  93
  94 static inline u8 get_to_ds(u8 *header)
  95 {
  96         return (header[1] & 0x01);
  97 }
  98
  99 static inline u8 get_from_ds(u8 *header)
 100 {
 101         return ((header[1] & 0x02) >> 1);
 102 }
 103
 104 static inline void get_address1(u8 *msa, u8 *addr)
 105 {
 106         memcpy(addr, msa + 4, 6);
 107 }
 108
 109 static inline void get_address2(u8 *msa, u8 *addr)
 110 {
 111         memcpy(addr, msa + 10, 6);
 112 }
 113
 114 static inline void get_address3(u8 *msa, u8 *addr)
 115 {
 116         memcpy(addr, msa + 16, 6);
 117 }
 118
 119 static inline void get_bssid(u8 *data, u8 *bssid)
 120 {
 121         if (get_from_ds(data) == 1)
 122                 get_address2(data, bssid);
 123         else if (get_to_ds(data) == 1)
 124                 get_address1(data, bssid);
 125         else
 126                 get_address3(data, bssid);
 127 }
 128
 129 static inline void get_ssid(u8 *data, u8 *ssid, u8 *p_ssid_len)
 130 {
 131         u8 i, j, len;
 132
 133         len = data[TAG_PARAM_OFFSET + 1];
 134         j   = TAG_PARAM_OFFSET + 2;
 135
 136         if (len >= MAX_SSID_LEN)
 137                 len = 0;
 138
 139         for (i = 0; i < len; i++, j++)
 140                 ssid[i] = data[j];
 141
 142         ssid[len] = '\0';
 143
 144         *p_ssid_len = len;
 145 }
 146
 147 static inline u16 get_cap_info(u8 *data)
 148 {
 149         u16 cap_info = 0;
 150         u16 index    = MAC_HDR_LEN;
 151         enum sub_frame_type st;
 152
 153         st = get_sub_type(data);
 154
 155         if (st == BEACON || st == PROBE_RSP)
 156                 index += TIME_STAMP_LEN + BEACON_INTERVAL_LEN;
 157
 158         cap_info  = data[index];
 159         cap_info |= (data[index + 1] << 8);
 160
 161         return cap_info;
 162 }
 163
 164 static inline u16 get_asoc_status(u8 *data)
 165 {
 166         u16 asoc_status;
 167
 168         asoc_status = data[3];
 169         return (asoc_status << 8) | data[2];
 170 }
 171
 172 static u8 *get_tim_elm(u8 *msa, u16 rx_len, u16 tag_param_offset)
 173 {
 174         u16 index;
 175
 176         index = tag_param_offset;
 177
 178         while (index < (rx_len - FCS_LEN)) {
 179                 if (msa[index] == WLAN_EID_TIM)
 180                         return &msa[index];
 181                 index += (IE_HDR_LEN + msa[index + 1]);
 182         }
 183
 184         return NULL;
 185 }
 186
 187 static u8 get_current_channel_802_11n(u8 *msa, u16 rx_len)
 188 {
 189         u16 index;
 190
 191         index = TAG_PARAM_OFFSET;
 192         while (index < (rx_len - FCS_LEN)) {
 193                 if (msa[index] == WLAN_EID_DS_PARAMS)
 194                         return msa[index + 2];
 195                 index += msa[index + 1] + IE_HDR_LEN;
 196         }
 197
 198         return 0;
 199 }
 200
 201 s32 wilc_parse_network_info(u8 *msg_buffer,
 202                             struct network_info **ret_network_info)
 203 {
 204         struct network_info *network_info;
 205         u8 *wid_val, *msa, *tim_elm, *ies;
 206         u32 tsf_lo, tsf_hi;
 207         u16 wid_len, rx_len, ies_len;
 208         u8 msg_type, index;
 209
 210         msg_type = msg_buffer[0];
 211
 212         if ('N' != msg_type)
 213                 return -EFAULT;
 214
 215         wid_len = MAKE_WORD16(msg_buffer[6], msg_buffer[7]);
 216         wid_val = &msg_buffer[8];
 217
 218         network_info = kzalloc(sizeof(*network_info), GFP_KERNEL);
 219         if (!network_info)
 220                 return -ENOMEM;
 221
 222         network_info->rssi = wid_val[0];
 223
 224         msa = &wid_val[1];
 225
 226         rx_len = wid_len - 1;
 227         network_info->cap_info = get_cap_info(msa);
 228         network_info->tsf_lo = get_beacon_timestamp_lo(msa);
 229
 230         tsf_lo = get_beacon_timestamp_lo(msa);
 231         tsf_hi = get_beacon_timestamp_hi(msa);
 232
 233         network_info->tsf_hi = tsf_lo | ((u64)tsf_hi << 32);
 234
 235         get_ssid(msa, network_info->ssid, &network_info->ssid_len);
 236         get_bssid(msa, network_info->bssid);
 237
 238         network_info->ch = get_current_channel_802_11n(msa, rx_len
 239                                                        + FCS_LEN);
 240
 241         index = MAC_HDR_LEN + TIME_STAMP_LEN;
 242
 243         network_info->beacon_period = get_beacon_period(msa + index);
 244
 245         index += BEACON_INTERVAL_LEN + CAP_INFO_LEN;
 246
 247         tim_elm = get_tim_elm(msa, rx_len + FCS_LEN, index);
 248         if (tim_elm)
 249                 network_info->dtim_period = tim_elm[3];
 250         ies = &msa[TAG_PARAM_OFFSET];
 251         ies_len = rx_len - TAG_PARAM_OFFSET;
 252
 253         if (ies_len > 0) {
 254                 network_info->ies = kmemdup(ies, ies_len, GFP_KERNEL);
 255                 if (!network_info->ies) {
 256                         kfree(network_info);
 257                         return -ENOMEM;
 258                 }
 259         }
 260         network_info->ies_len = ies_len;
 261
 262         *ret_network_info = network_info;
 263
 264         return 0;
 265 }
 266
 267 s32 wilc_parse_assoc_resp_info(u8 *buffer, u32 buffer_len,
 268                                struct connect_info *ret_conn_info)
 269 {
 270         u8 *ies;
 271         u16 ies_len;
 272
 273         ret_conn_info->status = get_asoc_status(buffer);
 274         if (ret_conn_info->status == WLAN_STATUS_SUCCESS) {
 275                 ies = &buffer[CAP_INFO_LEN + STATUS_CODE_LEN + AID_LEN];
 276                 ies_len = buffer_len - (CAP_INFO_LEN + STATUS_CODE_LEN +
 277                                         AID_LEN);
 278
 279                 ret_conn_info->resp_ies = kmemdup(ies, ies_len, GFP_KERNEL);
 280                 if (!ret_conn_info->resp_ies)
 281                         return -ENOMEM;
 282
 283                 ret_conn_info->resp_ies_len = ies_len;
 284         }
 285
 286         return 0;
 287 }