1 #include <string.h>
2 #include <errno.h>
3
4 #include <netlink/genl/genl.h>
5 #include <netlink/genl/family.h>
6 #include <netlink/genl/ctrl.h>
7 #include <netlink/msg.h>
8 #include <netlink/attr.h>
9
10 #include "nl80211.h"
11 #include "iw.h"
12
13 SECTION(mgmt);
14
seq_handler(struct nl_msg * msg,void * arg)15 static int seq_handler(struct nl_msg *msg, void *arg)
16 {
17 return NL_OK;
18 }
19
dump_mgmt_frame(struct nl_msg * msg,void * arg)20 static int dump_mgmt_frame(struct nl_msg *msg, void *arg)
21 {
22 struct genlmsghdr *gnlh = nlmsg_data(nlmsg_hdr(msg));
23 struct nlattr *tb_msg[NL80211_ATTR_MAX + 1];
24
25 nla_parse(tb_msg, NL80211_ATTR_MAX, genlmsg_attrdata(gnlh, 0),
26 genlmsg_attrlen(gnlh, 0), NULL);
27
28 if (tb_msg[NL80211_ATTR_WIPHY_FREQ]) {
29 uint32_t freq = nla_get_u32(tb_msg[NL80211_ATTR_WIPHY_FREQ]);
30 printf("freq %u MHz\n", freq);
31 }
32
33 if (tb_msg[NL80211_ATTR_RX_SIGNAL_DBM]) {
34 /* nl80211_send_mgmt sends signed dBm value as u32 */
35 int dbm = nla_get_u32(tb_msg[NL80211_ATTR_RX_SIGNAL_DBM]);
36 printf("rssi %d dBm\n", dbm);
37 }
38
39 if (tb_msg[NL80211_ATTR_FRAME]) {
40 int len = nla_len(tb_msg[NL80211_ATTR_FRAME]);
41 uint8_t *data = nla_data(tb_msg[NL80211_ATTR_FRAME]);
42 iw_hexdump("mgmt", data, len);
43 }
44
45 return 0;
46 }
47
register_mgmt_frame(struct nl80211_state * state,struct nl_msg * msg,int argc,char ** argv,enum id_input id)48 static int register_mgmt_frame(struct nl80211_state *state,
49 struct nl_msg *msg, int argc, char **argv,
50 enum id_input id)
51 {
52 unsigned int type;
53 unsigned char *match;
54 size_t match_len;
55 int ret;
56
57 if (argc < 2)
58 return HANDLER_RET_USAGE;
59
60 ret = sscanf(argv[0], "%x", &type);
61 if (ret != 1) {
62 printf("invalid frame type: %s\n", argv[0]);
63 return 2;
64 }
65
66 match = parse_hex(argv[1], &match_len);
67 if (!match) {
68 printf("invalid frame pattern: %s\n", argv[1]);
69 return 2;
70 }
71
72 NLA_PUT_U16(msg, NL80211_ATTR_FRAME_TYPE, type);
73 NLA_PUT(msg, NL80211_ATTR_FRAME_MATCH, match_len, match);
74
75 free(match);
76 return 0;
77
78 nla_put_failure:
79 free(match);
80 return -ENOBUFS;
81 }
82
handle_mgmt_reg(struct nl80211_state * state,struct nl_msg * msg,int argc,char ** argv,enum id_input id)83 static int handle_mgmt_reg(struct nl80211_state *state,
84 struct nl_msg *msg, int argc,
85 char **argv, enum id_input id)
86 {
87 return register_mgmt_frame(state, msg, argc, argv, id);
88 }
89
90 HIDDEN(mgmt, reg, "", NL80211_CMD_REGISTER_FRAME, 0, CIB_NETDEV, handle_mgmt_reg);
91
handle_mgmt_dump(struct nl80211_state * state,struct nl_msg * msg,int argc,char ** argv,enum id_input id)92 static int handle_mgmt_dump(struct nl80211_state *state,
93 struct nl_msg *msg, int argc,
94 char **argv, enum id_input id)
95 {
96 struct nl_cb *mgmt_cb;
97 char *ndev = argv[0];
98 int mgmt_argc = 5;
99 char **mgmt_argv;
100 unsigned int count = 0;
101 int err = 0;
102
103 mgmt_argv = calloc(mgmt_argc, sizeof(char*));
104 if (!mgmt_argv)
105 return -ENOMEM;
106
107 mgmt_argv[0] = ndev;
108 mgmt_argv[1] = "mgmt";
109 mgmt_argv[2] = "reg";
110
111 if (argc < 6) {
112 err = HANDLER_RET_USAGE;
113 goto out;
114 }
115
116 argc -= 3;
117 argv += 3;
118 while (argc >= 3) {
119 if (strcmp(argv[0], "frame") != 0) {
120 err = HANDLER_RET_USAGE;
121 goto out;
122 }
123
124 mgmt_argv[3] = argv[1];
125 mgmt_argv[4] = argv[2];
126
127 argc -= 3;
128 argv += 3;
129
130 err = handle_cmd(state, II_NETDEV, mgmt_argc, mgmt_argv);
131 if (err)
132 goto out;
133 }
134
135 if (argc == 2 && strcmp(argv[0], "count") == 0) {
136 count = 1 + atoi(argv[1]);
137 if (count < 1)
138 count = 1;
139
140 argc -= 2;
141 argv += 2;
142 } else if (argc) {
143 err = HANDLER_RET_USAGE;
144 goto out;
145 }
146
147 mgmt_cb = nl_cb_alloc(iw_debug ? NL_CB_DEBUG : NL_CB_DEFAULT);
148 if (!mgmt_cb) {
149 err = 1;
150 goto out;
151 }
152
153 /* need to turn off sequence number checking */
154 nl_cb_set(mgmt_cb, NL_CB_SEQ_CHECK, NL_CB_CUSTOM, seq_handler, NULL);
155 nl_cb_set(mgmt_cb, NL_CB_VALID, NL_CB_CUSTOM, dump_mgmt_frame, NULL);
156
157 while (--count)
158 nl_recvmsgs(state->nl_sock, mgmt_cb);
159
160 nl_cb_put(mgmt_cb);
161 out:
162 free(mgmt_argv);
163 return err;
164 }
165
166 COMMAND(mgmt, dump, "frame <type as hex ab> <pattern as hex ab:cd:..> [frame <type> <pattern>]* [count <frames>]",
167 0, 0, CIB_NETDEV, handle_mgmt_dump,
168 "Register for receiving certain mgmt frames and print them.\n"
169 "Frames are selected by their type and pattern containing\n"
170 "the first several bytes of the frame that should match.\n\n"
171 "Example: iw dev wlan0 mgmt dump frame 40 00 frame 40 01:02 count 10\n");
172