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
get_beacon_period(u8 * data)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
get_beacon_timestamp_lo(u8 * data)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
get_beacon_timestamp_hi(u8 * data)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
get_sub_type(u8 * header)89 static inline enum sub_frame_type get_sub_type(u8 *header)
90 {
91 return ((enum sub_frame_type)(header[0] & 0xFC));
92 }
93
get_to_ds(u8 * header)94 static inline u8 get_to_ds(u8 *header)
95 {
96 return (header[1] & 0x01);
97 }
98
get_from_ds(u8 * header)99 static inline u8 get_from_ds(u8 *header)
100 {
101 return ((header[1] & 0x02) >> 1);
102 }
103
get_address1(u8 * msa,u8 * addr)104 static inline void get_address1(u8 *msa, u8 *addr)
105 {
106 memcpy(addr, msa + 4, 6);
107 }
108
get_address2(u8 * msa,u8 * addr)109 static inline void get_address2(u8 *msa, u8 *addr)
110 {
111 memcpy(addr, msa + 10, 6);
112 }
113
get_address3(u8 * msa,u8 * addr)114 static inline void get_address3(u8 *msa, u8 *addr)
115 {
116 memcpy(addr, msa + 16, 6);
117 }
118
get_BSSID(u8 * data,u8 * bssid)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
get_ssid(u8 * data,u8 * ssid,u8 * p_ssid_len)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
get_cap_info(u8 * data)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
get_asoc_status(u8 * data)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
get_tim_elm(u8 * msa,u16 rx_len,u16 tag_param_offset)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
get_current_channel_802_11n(u8 * msa,u16 rx_len)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
wilc_parse_network_info(u8 * msg_buffer,struct network_info ** ret_network_info)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
wilc_parse_assoc_resp_info(u8 * buffer,u32 buffer_len,struct connect_info * ret_conn_info)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 }
288
