5 #include <netlink/genl/genl.h>
6 #include <netlink/genl/family.h>
7 #include <netlink/genl/ctrl.h>
8 #include <netlink/msg.h>
9 #include <netlink/attr.h>
19 static int parse_bands(int argc
, char **argv
)
23 for (i
= 0; i
< argc
; i
++) {
24 if (!strcasecmp("2ghz", argv
[i
]))
25 bands
|= BIT(NL80211_BAND_2GHZ
);
26 else if (!strcasecmp("5ghz", argv
[i
]))
27 bands
|= BIT(NL80211_BAND_5GHZ
);
35 static int handle_nan_start(struct nl80211_state
*state
,
36 struct nl_msg
*msg
, int argc
, char **argv
,
44 if (strcmp(argv
[0], "pref") == 0) {
47 NLA_PUT_U8(msg
, NL80211_ATTR_NAN_MASTER_PREF
, atoi(argv
[0]));
51 /* Master preference is mandatory */
55 if (argc
> 1 && !strcmp(argv
[0], "bands")) {
59 bands
= parse_bands(argc
, argv
);
63 NLA_PUT_U32(msg
, NL80211_ATTR_BANDS
, bands
);
71 COMMAND(nan
, start
, "pref <pref> [bands [2GHz] [5GHz]]",
72 NL80211_CMD_START_NAN
, 0, CIB_WDEV
, handle_nan_start
, "");
74 static int handle_nan_stop(struct nl80211_state
*state
,
75 struct nl_msg
*msg
, int argc
, char **argv
,
80 COMMAND(nan
, stop
, "", NL80211_CMD_STOP_NAN
, 0, CIB_WDEV
, handle_nan_stop
, "");
82 static int handle_nan_config(struct nl80211_state
*state
,
83 struct nl_msg
*msg
, int argc
, char **argv
,
91 if (strcmp(argv
[0], "pref") == 0) {
94 NLA_PUT_U8(msg
, NL80211_ATTR_NAN_MASTER_PREF
, atoi(argv
[0]));
99 if (argc
> 1 && !strcmp(argv
[0], "bands")) {
103 bands
= parse_bands(argc
, argv
);
107 NLA_PUT_U32(msg
, NL80211_ATTR_BANDS
, bands
);
110 } else if (argc
!= 0)
117 COMMAND(nan
, config
, "[pref <pref>] [bands [2GHz] [5GHz]]",
118 NL80211_CMD_CHANGE_NAN_CONFIG
, 0, CIB_WDEV
, handle_nan_config
, "");
120 static int handle_nan_rm_func(struct nl80211_state
*state
,
121 struct nl_msg
*msg
, int argc
, char **argv
,
127 if (strcmp(argv
[0], "cookie") == 0) {
130 NLA_PUT_U64(msg
, NL80211_ATTR_COOKIE
, atoi(argv
[0]));
142 COMMAND(nan
, rm_func
, "cookie <cookie>", NL80211_CMD_DEL_NAN_FUNCTION
, 0,
143 CIB_WDEV
, handle_nan_rm_func
, "");
145 static int compute_service_id(const unsigned char *serv_name
,
146 unsigned int len
, unsigned char *res
)
149 unsigned char md_value
[32];
150 int retcode
= sha256(serv_name
, size
, md_value
);
154 memcpy(res
, md_value
, 6);
159 static int print_instance_id_handler(struct nl_msg
*msg
, void *arg
)
161 struct nlattr
*tb
[NL80211_ATTR_MAX
+ 1];
162 struct nlattr
*func
[NL80211_NAN_FUNC_ATTR_MAX
+ 1];
163 struct genlmsghdr
*gnlh
= nlmsg_data(nlmsg_hdr(msg
));
165 nla_parse(tb
, NL80211_ATTR_MAX
, genlmsg_attrdata(gnlh
, 0),
166 genlmsg_attrlen(gnlh
, 0), NULL
);
168 if (!tb
[NL80211_ATTR_COOKIE
]) {
169 fprintf(stderr
, "cookie is missing!\n");
173 nla_parse_nested(func
, NL80211_NAN_FUNC_ATTR_MAX
,
174 tb
[NL80211_ATTR_NAN_FUNC
],
176 if (!func
[NL80211_NAN_FUNC_INSTANCE_ID
]) {
177 fprintf(stderr
, "instance id is missing!\n");
181 printf("instance_id: %d, cookie: %" PRIu64
"\n",
182 nla_get_u8(func
[NL80211_NAN_FUNC_INSTANCE_ID
]),
183 nla_get_u64(tb
[NL80211_ATTR_COOKIE
]));
188 static int parse_srf(char **argv
, int argc
, struct nl_msg
*func_attrs
)
190 struct nl_msg
*srf_attrs
;
192 unsigned char mac_addr
[ETH_ALEN
];
193 char *cur_mac
, *sptr
= NULL
;
195 srf_attrs
= nlmsg_alloc();
196 if (strcmp(argv
[0], "include") == 0)
197 NLA_PUT_FLAG(srf_attrs
, NL80211_NAN_SRF_INCLUDE
);
198 else if (strcmp(argv
[0], "exclude") != 0)
203 if (strcmp(argv
[0], "bf") == 0) {
215 bf_idx
= atoi(argv
[0]);
216 NLA_PUT_U8(srf_attrs
, NL80211_NAN_SRF_BF_IDX
, bf_idx
);
220 srf_len
= atoi(argv
[0]);
221 if (srf_len
== 0 || srf_len
> NL80211_NAN_FUNC_SRF_MAX_LEN
)
226 srf
= malloc(srf_len
);
230 memset(srf
, 0, srf_len
);
231 cur_mac
= strtok_r(argv
[0], ";", &sptr
);
233 if (mac_addr_a2n(mac_addr
, cur_mac
)) {
234 printf("mac format error %s\n", cur_mac
);
239 nan_bf(bf_idx
, srf
, srf_len
, mac_addr
, ETH_ALEN
);
240 cur_mac
= strtok_r(NULL
, ";", &sptr
);
243 err
= nla_put(srf_attrs
, NL80211_NAN_SRF_BF
, srf_len
, srf
);
246 goto nla_put_failure
;
249 } else if (strcmp(argv
[0], "list") == 0) {
250 struct nlattr
*nl_macs
= nla_nest_start(srf_attrs
,
251 NL80211_NAN_SRF_MAC_ADDRS
);
256 cur_mac
= strtok_r(argv
[0], ";", &sptr
);
258 if (mac_addr_a2n(mac_addr
, cur_mac
))
261 nla_put(srf_attrs
, ++i
, ETH_ALEN
, mac_addr
);
262 cur_mac
= strtok_r(NULL
, ";", &sptr
);
265 nla_nest_end(srf_attrs
, nl_macs
);
272 nla_put_nested(func_attrs
, NL80211_NAN_FUNC_SRF
, srf_attrs
);
273 return old_argc
- argc
;
278 static void parse_match_filter(char *filter
, struct nl_msg
*func_attrs
, int tx
)
280 struct nlattr
*nl_filt
;
281 char *cur_filt
, *sptr
= NULL
;
285 nl_filt
= nla_nest_start(func_attrs
,
286 NL80211_NAN_FUNC_TX_MATCH_FILTER
);
288 nl_filt
= nla_nest_start(func_attrs
,
289 NL80211_NAN_FUNC_RX_MATCH_FILTER
);
291 cur_filt
= strtok_r(filter
, ":", &sptr
);
293 if (strcmp(cur_filt
, "*") != 0)
294 nla_put(func_attrs
, ++i
, strlen(cur_filt
), cur_filt
);
296 nla_put(func_attrs
, ++i
, 0, NULL
);
298 cur_filt
= strtok_r(NULL
, ":", &sptr
);
301 nla_nest_end(func_attrs
, nl_filt
);
304 static int handle_nan_add_func(struct nl80211_state
*state
,
305 struct nl_msg
*msg
, int argc
, char **argv
,
308 struct nl_msg
*func_attrs
= NULL
;
312 func_attrs
= nlmsg_alloc();
318 if (argc
> 1 && strcmp(argv
[0], "type") == 0) {
321 if (strcmp(argv
[0], "publish") == 0)
322 type
= NL80211_NAN_FUNC_PUBLISH
;
323 else if (strcmp(argv
[0], "subscribe") == 0)
324 type
= NL80211_NAN_FUNC_SUBSCRIBE
;
325 else if (strcmp(argv
[0], "followup") == 0)
326 type
= NL80211_NAN_FUNC_FOLLOW_UP
;
332 NLA_PUT_U8(func_attrs
, NL80211_NAN_FUNC_TYPE
, type
);
337 if (type
== NL80211_NAN_FUNC_SUBSCRIBE
) {
338 if (argc
> 1 && strcmp(argv
[0], "active") == 0) {
341 NLA_PUT_FLAG(func_attrs
,
342 NL80211_NAN_FUNC_SUBSCRIBE_ACTIVE
);
346 if (type
== NL80211_NAN_FUNC_PUBLISH
) {
347 __u8 publish_type
= 0;
349 if (argc
> 1 && strcmp(argv
[0], "solicited") == 0) {
352 publish_type
|= NL80211_NAN_SOLICITED_PUBLISH
;
355 if (argc
> 1 && strcmp(argv
[0], "unsolicited") == 0) {
358 publish_type
|= NL80211_NAN_UNSOLICITED_PUBLISH
;
361 NLA_PUT_U8(func_attrs
, NL80211_NAN_FUNC_PUBLISH_TYPE
,
364 /* only allow for solicited publish */
365 if (argc
> 1 && strcmp(argv
[0], "bcast") == 0) {
368 if (!(publish_type
& NL80211_NAN_SOLICITED_PUBLISH
))
371 NLA_PUT_FLAG(func_attrs
,
372 NL80211_NAN_FUNC_PUBLISH_BCAST
);
376 if (argc
> 1 && strcmp(argv
[0], "close_range") == 0) {
379 NLA_PUT_FLAG(func_attrs
, NL80211_NAN_FUNC_CLOSE_RANGE
);
382 if (argc
> 1 && strcmp(argv
[0], "name") == 0) {
383 unsigned char serv_id_c
[6] = {0};
388 compute_service_id((const unsigned char *)argv
[0],
389 strlen(argv
[0]), serv_id_c
);
390 service_id
= (__u64
)serv_id_c
[0] << 0 |
391 (__u64
)serv_id_c
[1] << 8 |
392 (__u64
)serv_id_c
[2] << 16 |
393 (__u64
)serv_id_c
[3] << 24 |
394 (__u64
)serv_id_c
[4] << 32 |
395 (__u64
)serv_id_c
[5] << 40;
397 NLA_PUT(func_attrs
, NL80211_NAN_FUNC_SERVICE_ID
, 6,
405 if (argc
> 1 && strcmp(argv
[0], "info") == 0) {
408 NLA_PUT(func_attrs
, NL80211_NAN_FUNC_SERVICE_INFO
,
409 strlen(argv
[0]), argv
[0]);
414 if (type
== NL80211_NAN_FUNC_FOLLOW_UP
) {
415 if (argc
> 1 && strcmp(argv
[0], "flw_up_id") == 0) {
418 NLA_PUT_U8(func_attrs
, NL80211_NAN_FUNC_FOLLOW_UP_ID
,
424 if (argc
> 1 && strcmp(argv
[0], "flw_up_req_id") == 0) {
427 NLA_PUT_U8(func_attrs
,
428 NL80211_NAN_FUNC_FOLLOW_UP_REQ_ID
,
434 if (argc
> 1 && strcmp(argv
[0], "flw_up_dest") == 0) {
435 unsigned char addr
[6];
439 if (mac_addr_a2n(addr
, argv
[0]))
440 goto nla_put_failure
;
441 nla_put(func_attrs
, NL80211_NAN_FUNC_FOLLOW_UP_DEST
,
448 if (type
!= NL80211_NAN_FUNC_FOLLOW_UP
&&
449 argc
> 1 && strcmp(argv
[0], "ttl") == 0) {
452 NLA_PUT_U32(func_attrs
, NL80211_NAN_FUNC_TTL
, atoi(argv
[0]));
457 if (type
!= NL80211_NAN_FUNC_FOLLOW_UP
&&
458 argc
>= 4 && strcmp(argv
[0], "srf") == 0) {
463 res
= parse_srf(argv
, argc
, func_attrs
);
471 if (type
!= NL80211_NAN_FUNC_FOLLOW_UP
&&
472 argc
> 1 && strcmp(argv
[0], "rx_filter") == 0) {
475 parse_match_filter(argv
[0], func_attrs
, 0);
481 if (type
!= NL80211_NAN_FUNC_FOLLOW_UP
&&
482 argc
> 1 && strcmp(argv
[0], "tx_filter") == 0) {
485 parse_match_filter(argv
[0], func_attrs
, 1);
494 nla_put_nested(msg
, NL80211_ATTR_NAN_FUNC
, func_attrs
);
495 register_handler(print_instance_id_handler
, NULL
);
503 COMMAND(nan
, add_func
,
504 "type <publish|subscribe|followup> [active] [solicited] [unsolicited] [bcast] [close_range] name <name> [info <info>] [flw_up_id <id> flw_up_req_id <id> flw_up_dest <mac>] [ttl <ttl>] [srf <include|exclude> <bf|list> [bf_idx] [bf_len] <mac1;mac2...>] [rx_filter <str1:str2...>] [tx_filter <str1:str2...>]",
505 NL80211_CMD_ADD_NAN_FUNCTION
, 0, CIB_WDEV
,
506 handle_nan_add_func
, "");