1 #ifdef WL_EXT_GENL
2 #include <bcmendian.h>
3 #include <wl_android.h>
4 #include <dhd_config.h>
5 #include <net/genetlink.h>
6
7 #define AGENL_ERROR(name, arg1, args...) \
8 do { \
9 if (android_msg_level & ANDROID_ERROR_LEVEL) { \
10 printf("[%s] AGENL-ERROR) %s : " arg1, name, __func__, ## args); \
11 } \
12 } while (0)
13 #define AGENL_TRACE(name, arg1, args...) \
14 do { \
15 if (android_msg_level & ANDROID_TRACE_LEVEL) { \
16 printf("[%s] AGENL-TRACE) %s : " arg1, name, __func__, ## args); \
17 } \
18 } while (0)
19 #define AGENL_INFO(name, arg1, args...) \
20 do { \
21 if (android_msg_level & ANDROID_INFO_LEVEL) { \
22 printf("[%s] AGENL-INFO) %s : " arg1, name, __func__, ## args); \
23 } \
24 } while (0)
25
26 #define htod32(i) i
27 #define htod16(i) i
28 #define dtoh32(i) i
29 #define dtoh16(i) i
30
31 #ifdef SENDPROB
32 #define MGMT_PROBE_REQ 0x40
33 #define MGMT_PROBE_RES 0x50
34 #endif
35
36 enum {
37 __GENL_CUSTOM_ATTR_INVALID,
38 GENL_CUSTOM_ATTR_MSG, /* message */
39 __GENL_CUSTOM_ATTR_MAX,
40 };
41
42 enum {
43 __GENLL_CUSTOM_COMMAND_INVALID,
44 GENL_CUSTOM_COMMAND_BIND, /* bind */
45 GENL_CUSTOM_COMMAND_SEND, /* user -> kernel */
46 GENL_CUSTOM_COMMAND_RECV, /* kernel -> user */
47 __GENL_CUSTOM_COMMAND_MAX,
48 };
49
50 #if defined(ALIBABA_ZEROCONFIG)
51 #define GENL_FAMILY_NAME "WIFI_NL_CUSTOM"
52 #define PROBE_RSP_DST_MAC_OFFSET 4
53 #define PROBE_RSP_VNDR_ID_OFFSET 55
54 #else
55 #define GENL_FAMILY_NAME "WLAN_NL_CUSTOM"
56 #define PROBE_RSP_DST_MAC_OFFSET 4
57 #define PROBE_RSP_VNDR_ID_OFFSET DOT11_MGMT_HDR_LEN
58 #endif
59 #define PROBE_RSP_VNDR_LEN_OFFSET (PROBE_RSP_VNDR_ID_OFFSET+1)
60 #define PROBE_RSP_VNDR_OUI_OFFSET (PROBE_RSP_VNDR_ID_OFFSET+2)
61 #define MAX_CUSTOM_PKT_LENGTH 2048
62 #define GENL_CUSTOM_ATTR_MAX (__GENL_CUSTOM_ATTR_MAX - 1)
63 #define GENLMSG_UNICAST_RETRY_LIMIT 5
64
65 typedef struct genl_params {
66 struct net_device *dev;
67 bool bind;
68 int pm;
69 int bind_pid;
70 int send_retry_cnt;
71 } genl_params_t;
72
73 struct genl_params *g_zconf = NULL;
74
75 static int wl_ext_genl_bind(struct sk_buff *skb, struct genl_info *info);
76 static int wl_ext_genl_recv(struct sk_buff *skb, struct genl_info *info);
77 static int wl_ext_genl_send(struct genl_params *zconf, struct net_device *dev,
78 char* buf, int buf_len);
79
80 static struct nla_policy wl_ext_genl_policy[GENL_CUSTOM_ATTR_MAX + 1] = {
81 [GENL_CUSTOM_ATTR_MSG] = {.type = NLA_NUL_STRING},
82 };
83
84 static struct genl_ops wl_ext_genl_ops[] = {
85 {
86 .cmd = GENL_CUSTOM_COMMAND_BIND,
87 .flags = 0,
88 .policy = wl_ext_genl_policy,
89 .doit = wl_ext_genl_bind,
90 .dumpit = NULL,
91 },
92 {
93 .cmd = GENL_CUSTOM_COMMAND_SEND,
94 .flags = 0,
95 .policy = wl_ext_genl_policy,
96 .doit = wl_ext_genl_recv,
97 .dumpit = NULL,
98 },
99 };
100
101 static struct genl_family wl_ext_genl_family = {
102 .id = GENL_ID_GENERATE,
103 .hdrsize = 0,
104 .name = GENL_FAMILY_NAME,
105 .version = 1,
106 .maxattr = GENL_CUSTOM_ATTR_MAX,
107 };
108
109 #ifdef SENDPROB
110 static int
wl_ext_add_del_ie_hex(struct net_device * dev,uint pktflag,char * ie_data,int ie_len,const char * add_del_cmd)111 wl_ext_add_del_ie_hex(struct net_device *dev, uint pktflag,
112 char *ie_data, int ie_len, const char* add_del_cmd)
113 {
114 vndr_ie_setbuf_t *vndr_ie = NULL;
115 char iovar_buf[WLC_IOCTL_SMLEN]="\0";
116 int tot_len = 0, iecount;
117 int err = -1;
118
119 if (!ie_len) {
120 AGENL_ERROR(dev->name, "wrong ie_len %d\n", ie_len);
121 goto exit;
122 }
123
124 tot_len = (int)(sizeof(vndr_ie_setbuf_t) + (ie_len));
125 vndr_ie = (vndr_ie_setbuf_t *) kzalloc(tot_len, GFP_KERNEL);
126 if (!vndr_ie) {
127 AGENL_ERROR(dev->name, "IE memory alloc failed\n");
128 err = -ENOMEM;
129 goto exit;
130 }
131
132 /* Copy the vndr_ie SET command ("add"/"del") to the buffer */
133 strncpy(vndr_ie->cmd, add_del_cmd, VNDR_IE_CMD_LEN - 1);
134 vndr_ie->cmd[VNDR_IE_CMD_LEN - 1] = '\0';
135
136 /* Set the IE count - the buffer contains only 1 IE */
137 iecount = htod32(1);
138 memcpy((void *)&vndr_ie->vndr_ie_buffer.iecount, &iecount, sizeof(s32));
139
140 /* Set packet flag to indicate that BEACON's will contain this IE */
141 pktflag = htod32(pktflag);
142 memcpy((void *)&vndr_ie->vndr_ie_buffer.vndr_ie_list[0].pktflag, &pktflag,
143 sizeof(u32));
144
145 /* Set the IE ID */
146 vndr_ie->vndr_ie_buffer.vndr_ie_list[0].vndr_ie_data.id = (uchar)DOT11_MNG_VS_ID;
147
148 /* Set the IE LEN */
149 vndr_ie->vndr_ie_buffer.vndr_ie_list[0].vndr_ie_data.len = ie_len;
150
151 /* Set the IE OUI and DATA */
152 memcpy((char *)vndr_ie->vndr_ie_buffer.vndr_ie_list[0].vndr_ie_data.oui, ie_data, ie_len);
153
154 err = wldev_iovar_setbuf(dev, "vndr_ie", vndr_ie, tot_len,
155 iovar_buf, sizeof(iovar_buf), NULL);
156 if (err != 0)
157 AGENL_ERROR(dev->name, "vndr_ie, ret=%d\n", err);
158
159 exit:
160 if (vndr_ie) {
161 kfree(vndr_ie);
162 }
163 return err;
164 }
165
166 static int
wl_ext_send_probersp(struct net_device * dev,char * buf,int buf_len)167 wl_ext_send_probersp(struct net_device *dev, char* buf, int buf_len)
168 {
169 char addr[ETHER_ADDR_LEN], *pVndrOUI;
170 char iovar_buf[WLC_IOCTL_SMLEN]="\0";
171 int err = -1, ie_len;
172
173 if (buf == NULL || buf_len <= 0){
174 AGENL_ERROR(dev->name, "buf is NULL or buf_len <= 0\n");
175 return -1;
176 }
177
178 AGENL_TRACE(dev->name, "Enter\n");
179
180 memcpy(addr, (buf+PROBE_RSP_DST_MAC_OFFSET), ETHER_ADDR_LEN);
181 pVndrOUI = (buf+PROBE_RSP_VNDR_OUI_OFFSET);
182 ie_len = *(buf+PROBE_RSP_VNDR_LEN_OFFSET);
183
184 if (ie_len > (buf_len-PROBE_RSP_VNDR_OUI_OFFSET)) {
185 AGENL_ERROR(dev->name, "wrong vendor ie len %d\n", ie_len);
186 return -1;
187 }
188
189 err = wl_ext_add_del_ie_hex(dev, VNDR_IE_PRBRSP_FLAG, pVndrOUI, ie_len, "add");
190 if (err)
191 goto exit;
192
193 err = wldev_iovar_setbuf(dev, "send_probresp", addr, ETHER_ADDR_LEN,
194 iovar_buf, sizeof(iovar_buf), NULL);
195 if (err != 0)
196 AGENL_ERROR(dev->name, "vndr_ie, ret=%d\n", err);
197
198 OSL_SLEEP(100);
199 wl_ext_add_del_ie_hex(dev, VNDR_IE_PRBRSP_FLAG, pVndrOUI, ie_len, "del");
200
201 exit:
202 return err;
203 }
204
205 static int
wl_ext_set_probreq(struct net_device * dev,bool set)206 wl_ext_set_probreq(struct net_device *dev, bool set)
207 {
208 int bytes_written = 0;
209 char recv_probreq[32];
210
211 AGENL_TRACE(dev->name, "Enter\n");
212
213 if (set) {
214 sprintf(recv_probreq, "wl recv_probreq 1");
215 wl_android_ext_priv_cmd(dev, recv_probreq, 0, &bytes_written);
216 } else {
217 sprintf(recv_probreq, "wl recv_probreq 0");
218 wl_android_ext_priv_cmd(dev, recv_probreq, 0, &bytes_written);
219 }
220
221 return 0;
222 }
223
224 void
wl_ext_probreq_event(struct net_device * dev,void * argu,const wl_event_msg_t * e,void * data)225 wl_ext_probreq_event(struct net_device *dev, void *argu,
226 const wl_event_msg_t *e, void *data)
227 {
228 struct genl_params *zconf = (struct genl_params *)argu;
229 int i, ret = 0, num_ie = 0, totlen;
230 uint32 event_len = 0;
231 char *buf, *pbuf;
232 uint rem_len, buflen = MAX_CUSTOM_PKT_LENGTH;
233 uint32 event_id[] = {DOT11_MNG_VS_ID};
234 uint32 datalen = ntoh32(e->datalen);
235 bcm_tlv_t *ie;
236
237 AGENL_TRACE(dev->name, "Enter\n");
238
239 rem_len = buflen;
240 buf = kzalloc(MAX_CUSTOM_PKT_LENGTH, GFP_KERNEL);
241 if (unlikely(!buf)) {
242 AGENL_ERROR(dev->name, "Could not allocate buf\n");
243 return;
244 }
245
246 // copy mgmt header
247 pbuf = buf;
248 memcpy(pbuf, data, DOT11_MGMT_HDR_LEN);
249 rem_len -= (DOT11_MGMT_HDR_LEN+1);
250 datalen -= DOT11_MGMT_HDR_LEN;
251 data += DOT11_MGMT_HDR_LEN;
252
253 // copy IEs
254 pbuf = buf + DOT11_MGMT_HDR_LEN;
255 #if 1 // non-sort by id
256 ie = (bcm_tlv_t*)data;
257 totlen = datalen;
258 while (ie && totlen >= TLV_HDR_LEN) {
259 int ie_id = -1;
260 int ie_len = ie->len + TLV_HDR_LEN;
261 for (i=0; i<sizeof(event_id)/sizeof(event_id[0]); i++) {
262 if (ie->id == event_id[i]) {
263 ie_id = ie->id;
264 break;
265 }
266 }
267 if ((ie->id == ie_id) && (totlen >= ie_len) && (rem_len >= ie_len)) {
268 memcpy(pbuf, ie, ie_len);
269 pbuf += ie_len;
270 rem_len -= ie_len;
271 num_ie++;
272 }
273 ie = (bcm_tlv_t*)((uint8*)ie + ie_len);
274 totlen -= ie_len;
275 }
276 #else // sort by id
277 for (i = 0; i < sizeof(event_id)/sizeof(event_id[0]); i++) {
278 void *pdata = data;
279 int data_len = datalen;
280 while (rem_len > 0) {
281 ie = bcm_parse_tlvs(pdata, data_len, event_id[i]);
282 if (!ie)
283 break;
284 if (rem_len < (ie->len+TLV_HDR_LEN)) {
285 ANDROID_TRACE(("%s: buffer is not enough\n", __FUNCTION__));
286 break;
287 }
288 memcpy(pbuf, ie, min(ie->len+TLV_HDR_LEN, rem_len));
289 pbuf += (ie->len+TLV_HDR_LEN);
290 rem_len -= (ie->len+TLV_HDR_LEN);
291 data_len -= (((void *)ie-pdata) + (ie->len+TLV_HDR_LEN));
292 pdata = (char *)ie + (ie->len+TLV_HDR_LEN);
293 num_ie++;
294 }
295 }
296 #endif
297 if (num_ie) {
298 event_len = buflen - rem_len;
299 AGENL_INFO(dev->name, "num_ie=%d\n", num_ie);
300 if (android_msg_level & ANDROID_INFO_LEVEL)
301 prhex("buf", buf, event_len);
302 ret = wl_ext_genl_send(zconf, dev, buf, event_len);
303 }
304
305 if(buf)
306 kfree(buf);
307 return;
308 }
309 #endif
310
311 static int
wl_ext_genl_recv(struct sk_buff * skb,struct genl_info * info)312 wl_ext_genl_recv(struct sk_buff *skb, struct genl_info *info)
313 {
314 struct genl_params *zconf = g_zconf;
315 struct net_device *dev;
316 struct nlattr *na;
317 char* pData = NULL;
318 int DataLen = 0;
319
320 if (info == NULL) {
321 AGENL_ERROR(dev->name, "genl_info is NULL\n");
322 return -1;
323 }
324
325 if (zconf == NULL) {
326 AGENL_ERROR("wlan", "g_zconf is NULL\n");
327 return -1;
328 }
329 dev = zconf->dev;
330
331 #if LINUX_VERSION_CODE >= KERNEL_VERSION(3, 7, 0)
332 AGENL_TRACE(dev->name, "Enter snd_portid=%d\n", info->snd_portid);
333 #else
334 AGENL_TRACE(dev->name, "Enter\n");
335 #endif
336 na = info->attrs[GENL_CUSTOM_ATTR_MSG];
337
338 if (na) {
339 pData = (char*) nla_data(na);
340 DataLen = nla_len(na);
341 AGENL_INFO(dev->name, "nla_len(na) : %d\n", DataLen);
342 if (android_msg_level & ANDROID_INFO_LEVEL)
343 prhex("nla_data(na)", pData, DataLen);
344 }
345
346 #ifdef SENDPROB
347 if(*pData == MGMT_PROBE_RES) {
348 wl_ext_send_probersp(dev, pData, DataLen);
349 } else if(*pData == MGMT_PROBE_REQ) {
350 AGENL_ERROR(dev->name, "probe req\n");
351 } else {
352 AGENL_ERROR(dev->name, "Unexpected pkt %d\n", *pData);
353 if (android_msg_level & ANDROID_INFO_LEVEL)
354 prhex("nla_data(na)", pData, DataLen);
355 }
356 #endif
357
358 return 0;
359 }
360
361 static int
wl_ext_genl_send(struct genl_params * zconf,struct net_device * dev,char * buf,int buf_len)362 wl_ext_genl_send(struct genl_params *zconf, struct net_device *dev,
363 char* buf, int buf_len)
364 {
365 struct sk_buff *skb = NULL;
366 char* msg_head = NULL;
367 int ret = -1;
368 int bytes_written = 0;
369 char recv_probreq[32];
370
371 if (zconf->bind_pid == -1) {
372 AGENL_ERROR(dev->name, "There is no binded process\n");
373 return -1;
374 }
375
376 if(buf == NULL || buf_len <= 0) {
377 AGENL_ERROR(dev->name, "buf is NULL or buf_len : %d\n", buf_len);
378 return -1;
379 }
380
381 skb = genlmsg_new(MAX_CUSTOM_PKT_LENGTH, GFP_KERNEL);
382
383 if (skb) {
384 msg_head = genlmsg_put(skb, 0, 0, &wl_ext_genl_family, 0, GENL_CUSTOM_COMMAND_RECV);
385 if (msg_head == NULL) {
386 nlmsg_free(skb);
387 AGENL_ERROR(dev->name, "genlmsg_put fail\n");
388 return -1;
389 }
390
391 ret = nla_put(skb, GENL_CUSTOM_ATTR_MSG, buf_len, buf);
392 if (ret != 0) {
393 nlmsg_free(skb);
394 AGENL_ERROR(dev->name, "nla_put fail : %d\n", ret);
395 return ret;
396 }
397
398 genlmsg_end(skb, msg_head);
399
400 /* sending message */
401 AGENL_TRACE(dev->name, "send to process %d\n", zconf->bind_pid);
402 ret = genlmsg_unicast(&init_net, skb, zconf->bind_pid);
403 if (ret != 0) {
404 AGENL_ERROR(dev->name, "genlmsg_unicast fail : %d\n", ret);
405 zconf->send_retry_cnt++;
406 if(zconf->send_retry_cnt >= GENLMSG_UNICAST_RETRY_LIMIT) {
407 AGENL_ERROR(dev->name, "Exceeding retry cnt %d, Unbind pid : %d\n",
408 zconf->send_retry_cnt, zconf->bind_pid);
409 zconf->bind_pid = -1;
410 sprintf(recv_probreq, "wl recv_probreq 0");
411 wl_android_ext_priv_cmd(dev, recv_probreq, 0, &bytes_written);
412 }
413 return ret;
414 }
415 } else {
416 AGENL_ERROR(dev->name, "genlmsg_new fail\n");
417 return -1;
418 }
419
420 zconf->send_retry_cnt = 0;
421
422 return 0;
423 }
424
425 static int
wl_ext_genl_bind(struct sk_buff * skb,struct genl_info * info)426 wl_ext_genl_bind(struct sk_buff *skb, struct genl_info *info)
427 {
428 struct genl_params *zconf = g_zconf;
429 struct net_device *dev;
430 struct dhd_pub *dhd;
431 struct nlattr *na;
432 bool bind;
433 char* pData = NULL;
434 int DataLen = 0;
435
436 if (info == NULL) {
437 AGENL_ERROR("wlan", "genl_info is NULL\n");
438 return -1;
439 }
440
441 if (zconf == NULL) {
442 AGENL_ERROR("wlan", "zconf is NULL\n");
443 return -1;
444 }
445 dev = zconf->dev;
446 dhd = dhd_get_pub(dev);
447
448 AGENL_TRACE(dev->name, "Enter\n");
449
450 na = info->attrs[GENL_CUSTOM_ATTR_MSG];
451 if (na) {
452 pData = (char*) nla_data(na);
453 DataLen = nla_len(na);
454 AGENL_INFO(dev->name, "nla_len(na) : %d\n", DataLen);
455 if (android_msg_level & ANDROID_INFO_LEVEL)
456 prhex("nla_data(na)", pData, DataLen);
457 }
458
459 if (strcmp(pData, "BIND") == 0) {
460 bind = TRUE;
461 } else if (strcmp(pData, "UNBIND") == 0) {
462 bind = FALSE;
463 } else {
464 AGENL_ERROR(dev->name, "Unknown cmd %s\n", pData);
465 return -1;
466 }
467
468 if (bind == zconf->bind) {
469 AGENL_TRACE(dev->name, "Already %s\n", bind?"BIND":"UNBIND");
470 return 0;
471 }
472
473 if (bind) {
474 #if LINUX_VERSION_CODE >= KERNEL_VERSION(3, 7, 0)
475 zconf->bind_pid = info->snd_portid;
476 #endif
477 AGENL_TRACE(dev->name, "BIND pid = %d\n", zconf->bind_pid);
478 #ifdef SENDPROB
479 wl_ext_set_probreq(dev, TRUE);
480 #endif
481 zconf->bind = TRUE;
482 zconf->pm = dhd->conf->pm;
483 dhd->conf->pm = PM_OFF;
484 } else {
485 #if LINUX_VERSION_CODE >= KERNEL_VERSION(3, 7, 0)
486 AGENL_TRACE(dev->name, "UNBIND snd_portid = %d\n", info->snd_portid);
487 #else
488 AGENL_TRACE(dev->name, "UNBIND pid = %d\n", zconf->bind_pid);
489 #endif
490 zconf->bind_pid = -1;
491 #ifdef SENDPROB
492 wl_ext_set_probreq(dev, FALSE);
493 #endif
494 dhd->conf->pm = zconf->pm;
495 zconf->bind = FALSE;
496 }
497
498 return 0;
499 }
500
501 int
wl_ext_genl_init(struct net_device * net)502 wl_ext_genl_init(struct net_device *net)
503 {
504 struct dhd_pub *dhd = dhd_get_pub(net);
505 struct genl_params *zconf = dhd->zconf;
506 int ret = 0;
507
508 AGENL_TRACE(net->name, "Enter falimy name: \"%s\"\n", wl_ext_genl_family.name);
509
510 zconf = kzalloc(sizeof(struct genl_params), GFP_KERNEL);
511 if (unlikely(!zconf)) {
512 AGENL_ERROR(net->name, "Could not allocate zconf\n");
513 return -ENOMEM;
514 }
515 dhd->zconf = (void *)zconf;
516
517 #if LINUX_VERSION_CODE >= KERNEL_VERSION(4, 10, 0)
518 ret = genl_register_family(&wl_ext_genl_family);
519 //fix me: how to attach wl_ext_genl_ops
520 ret = -1;
521 #elif LINUX_VERSION_CODE >= KERNEL_VERSION(3, 13, 0)
522 ret = genl_register_family_with_ops(&wl_ext_genl_family, wl_ext_genl_ops);
523 #else
524 ret = genl_register_family_with_ops(&wl_ext_genl_family, wl_ext_genl_ops,
525 ARRAY_SIZE(wl_ext_genl_ops));
526 #endif
527 if (ret != 0) {
528 AGENL_ERROR(net->name, "GE_NELINK family registration fail\n");
529 goto err;
530 }
531 zconf->bind_pid = -1;
532 #ifdef SENDPROB
533 ret = wl_ext_event_register(net, dhd, WLC_E_PROBREQ_MSG, wl_ext_probreq_event,
534 zconf, PRIO_EVENT_IAPSTA);
535 if (ret)
536 goto err;
537 #endif
538 zconf->dev = net;
539 g_zconf = zconf;
540
541 return ret;
542 err:
543 if(zconf)
544 kfree(zconf);
545 return ret;
546 }
547
548 void
wl_ext_genl_deinit(struct net_device * net)549 wl_ext_genl_deinit(struct net_device *net)
550 {
551 struct dhd_pub *dhd = dhd_get_pub(net);
552 struct genl_params *zconf = dhd->zconf;
553
554 AGENL_TRACE(net->name, "Enter\n");
555
556 #ifdef SENDPROB
557 wl_ext_event_deregister(net, dhd, WLC_E_PROBREQ_MSG, wl_ext_probreq_event);
558 #endif
559
560 genl_unregister_family(&wl_ext_genl_family);
561 if(zconf != NULL) {
562 kfree(dhd->zconf);
563 dhd->zconf = NULL;
564 }
565 g_zconf = NULL;
566
567 }
568 #endif
569