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
;
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) {
214 bf_idx
= atoi(argv
[0]);
215 NLA_PUT_U8(srf_attrs
, NL80211_NAN_SRF_BF_IDX
, bf_idx
);
219 srf_len
= atoi(argv
[0]);
220 if (srf_len
== 0 || srf_len
> NL80211_NAN_FUNC_SRF_MAX_LEN
)
225 srf
= malloc(srf_len
);
229 memset(srf
, 0, srf_len
);
230 cur_mac
= strtok_r(argv
[0], ";", &sptr
);
232 if (mac_addr_a2n(mac_addr
, cur_mac
)) {
233 printf("mac format error %s\n", cur_mac
);
237 nan_bf(bf_idx
, srf
, srf_len
, mac_addr
, ETH_ALEN
);
238 cur_mac
= strtok_r(NULL
, ";", &sptr
);
241 NLA_PUT(srf_attrs
, NL80211_NAN_SRF_BF
, srf_len
, srf
);
244 } else if (strcmp(argv
[0], "list") == 0) {
245 struct nlattr
*nl_macs
= nla_nest_start(srf_attrs
,
246 NL80211_NAN_SRF_MAC_ADDRS
);
251 cur_mac
= strtok_r(argv
[0], ";", &sptr
);
253 if (mac_addr_a2n(mac_addr
, cur_mac
))
256 nla_put(srf_attrs
, ++i
, ETH_ALEN
, mac_addr
);
257 cur_mac
= strtok_r(NULL
, ";", &sptr
);
260 nla_nest_end(srf_attrs
, nl_macs
);
267 nla_put_nested(func_attrs
, NL80211_NAN_FUNC_SRF
, srf_attrs
);
268 return old_argc
- argc
;
273 static void parse_match_filter(char *filter
, struct nl_msg
*func_attrs
, int tx
)
275 struct nlattr
*nl_filt
;
276 char *cur_filt
, *sptr
;
280 nl_filt
= nla_nest_start(func_attrs
,
281 NL80211_NAN_FUNC_TX_MATCH_FILTER
);
283 nl_filt
= nla_nest_start(func_attrs
,
284 NL80211_NAN_FUNC_RX_MATCH_FILTER
);
286 cur_filt
= strtok_r(filter
, ":", &sptr
);
288 if (strcmp(cur_filt
, "*") != 0)
289 nla_put(func_attrs
, ++i
, strlen(cur_filt
), cur_filt
);
291 nla_put(func_attrs
, ++i
, 0, NULL
);
293 cur_filt
= strtok_r(NULL
, ":", &sptr
);
296 nla_nest_end(func_attrs
, nl_filt
);
299 static int handle_nan_add_func(struct nl80211_state
*state
,
300 struct nl_msg
*msg
, int argc
, char **argv
,
303 struct nl_msg
*func_attrs
= NULL
;
307 func_attrs
= nlmsg_alloc();
313 if (argc
> 1 && strcmp(argv
[0], "type") == 0) {
316 if (strcmp(argv
[0], "publish") == 0)
317 type
= NL80211_NAN_FUNC_PUBLISH
;
318 else if (strcmp(argv
[0], "subscribe") == 0)
319 type
= NL80211_NAN_FUNC_SUBSCRIBE
;
320 else if (strcmp(argv
[0], "followup") == 0)
321 type
= NL80211_NAN_FUNC_FOLLOW_UP
;
327 NLA_PUT_U8(func_attrs
, NL80211_NAN_FUNC_TYPE
, type
);
332 if (type
== NL80211_NAN_FUNC_SUBSCRIBE
) {
333 if (argc
> 1 && strcmp(argv
[0], "active") == 0) {
336 NLA_PUT_FLAG(func_attrs
,
337 NL80211_NAN_FUNC_SUBSCRIBE_ACTIVE
);
341 if (type
== NL80211_NAN_FUNC_PUBLISH
) {
342 __u8 publish_type
= 0;
344 if (argc
> 1 && strcmp(argv
[0], "solicited") == 0) {
347 publish_type
|= NL80211_NAN_SOLICITED_PUBLISH
;
350 if (argc
> 1 && strcmp(argv
[0], "unsolicited") == 0) {
353 publish_type
|= NL80211_NAN_UNSOLICITED_PUBLISH
;
356 NLA_PUT_U8(func_attrs
, NL80211_NAN_FUNC_PUBLISH_TYPE
,
359 /* only allow for solicited publish */
360 if (argc
> 1 && strcmp(argv
[0], "bcast") == 0) {
363 if (!(publish_type
& NL80211_NAN_SOLICITED_PUBLISH
))
366 NLA_PUT_FLAG(func_attrs
,
367 NL80211_NAN_FUNC_PUBLISH_BCAST
);
371 if (argc
> 1 && strcmp(argv
[0], "close_range") == 0) {
374 NLA_PUT_FLAG(func_attrs
, NL80211_NAN_FUNC_CLOSE_RANGE
);
377 if (argc
> 1 && strcmp(argv
[0], "name") == 0) {
378 unsigned char serv_id_c
[6] = {0};
383 compute_service_id((const unsigned char *)argv
[0],
384 strlen(argv
[0]), serv_id_c
);
385 service_id
= (__u64
)serv_id_c
[0] << 0 |
386 (__u64
)serv_id_c
[1] << 8 |
387 (__u64
)serv_id_c
[2] << 16 |
388 (__u64
)serv_id_c
[3] << 24 |
389 (__u64
)serv_id_c
[4] << 32 |
390 (__u64
)serv_id_c
[5] << 40;
392 NLA_PUT(func_attrs
, NL80211_NAN_FUNC_SERVICE_ID
, 6,
400 if (argc
> 1 && strcmp(argv
[0], "info") == 0) {
403 NLA_PUT(func_attrs
, NL80211_NAN_FUNC_SERVICE_INFO
,
404 strlen(argv
[0]), argv
[0]);
409 if (type
== NL80211_NAN_FUNC_FOLLOW_UP
) {
410 if (argc
> 1 && strcmp(argv
[0], "flw_up_id") == 0) {
413 NLA_PUT_U8(func_attrs
, NL80211_NAN_FUNC_FOLLOW_UP_ID
,
419 if (argc
> 1 && strcmp(argv
[0], "flw_up_req_id") == 0) {
422 NLA_PUT_U8(func_attrs
,
423 NL80211_NAN_FUNC_FOLLOW_UP_REQ_ID
,
429 if (argc
> 1 && strcmp(argv
[0], "flw_up_dest") == 0) {
430 unsigned char addr
[6];
434 if (mac_addr_a2n(addr
, argv
[0]))
435 goto nla_put_failure
;
436 nla_put(func_attrs
, NL80211_NAN_FUNC_FOLLOW_UP_DEST
,
443 if (type
!= NL80211_NAN_FUNC_FOLLOW_UP
&&
444 argc
> 1 && strcmp(argv
[0], "ttl") == 0) {
447 NLA_PUT_U32(func_attrs
, NL80211_NAN_FUNC_TTL
, atoi(argv
[0]));
452 if (type
!= NL80211_NAN_FUNC_FOLLOW_UP
&&
453 argc
>= 4 && strcmp(argv
[0], "srf") == 0) {
458 res
= parse_srf(argv
, argc
, func_attrs
);
466 if (type
!= NL80211_NAN_FUNC_FOLLOW_UP
&&
467 argc
> 1 && strcmp(argv
[0], "rx_filter") == 0) {
470 parse_match_filter(argv
[0], func_attrs
, 0);
476 if (type
!= NL80211_NAN_FUNC_FOLLOW_UP
&&
477 argc
> 1 && strcmp(argv
[0], "tx_filter") == 0) {
480 parse_match_filter(argv
[0], func_attrs
, 1);
489 nla_put_nested(msg
, NL80211_ATTR_NAN_FUNC
, func_attrs
);
490 register_handler(print_instance_id_handler
, NULL
);
498 COMMAND(nan
, add_func
,
499 "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...>]",
500 NL80211_CMD_ADD_NAN_FUNCTION
, 0, CIB_WDEV
,
501 handle_nan_add_func
, "");