| #include <string.h> |
| #include <errno.h> |
| |
| #include <netlink/genl/genl.h> |
| #include <netlink/genl/family.h> |
| #include <netlink/genl/ctrl.h> |
| #include <netlink/msg.h> |
| #include <netlink/attr.h> |
| |
| #include "nl80211.h" |
| #include "iw.h" |
| |
| SECTION(mgmt); |
| |
| static int seq_handler(struct nl_msg *msg, void *arg) |
| { |
| return NL_OK; |
| } |
| |
| static int dump_mgmt_frame(struct nl_msg *msg, void *arg) |
| { |
| struct genlmsghdr *gnlh = nlmsg_data(nlmsg_hdr(msg)); |
| struct nlattr *tb_msg[NL80211_ATTR_MAX + 1]; |
| |
| nla_parse(tb_msg, NL80211_ATTR_MAX, genlmsg_attrdata(gnlh, 0), |
| genlmsg_attrlen(gnlh, 0), NULL); |
| |
| if (tb_msg[NL80211_ATTR_WIPHY_FREQ]) { |
| uint32_t freq = nla_get_u32(tb_msg[NL80211_ATTR_WIPHY_FREQ]); |
| printf("freq %u MHz\n", freq); |
| } |
| |
| if (tb_msg[NL80211_ATTR_RX_SIGNAL_DBM]) { |
| /* nl80211_send_mgmt sends signed dBm value as u32 */ |
| int dbm = nla_get_u32(tb_msg[NL80211_ATTR_RX_SIGNAL_DBM]); |
| printf("rssi %d dBm\n", dbm); |
| } |
| |
| if (tb_msg[NL80211_ATTR_FRAME]) { |
| int len = nla_len(tb_msg[NL80211_ATTR_FRAME]); |
| uint8_t *data = nla_data(tb_msg[NL80211_ATTR_FRAME]); |
| iw_hexdump("mgmt", data, len); |
| } |
| |
| return 0; |
| } |
| |
| static int register_mgmt_frame(struct nl80211_state *state, |
| struct nl_msg *msg, int argc, char **argv, |
| enum id_input id) |
| { |
| unsigned int type; |
| unsigned char *match; |
| size_t match_len; |
| int ret; |
| |
| if (argc < 2) |
| return HANDLER_RET_USAGE; |
| |
| ret = sscanf(argv[0], "%x", &type); |
| if (ret != 1) { |
| printf("invalid frame type: %s\n", argv[0]); |
| return 2; |
| } |
| |
| match = parse_hex(argv[1], &match_len); |
| if (!match) { |
| printf("invalid frame pattern: %s\n", argv[1]); |
| return 2; |
| } |
| |
| NLA_PUT_U16(msg, NL80211_ATTR_FRAME_TYPE, type); |
| NLA_PUT(msg, NL80211_ATTR_FRAME_MATCH, match_len, match); |
| |
| free(match); |
| return 0; |
| |
| nla_put_failure: |
| free(match); |
| return -ENOBUFS; |
| } |
| |
| static int handle_mgmt_reg(struct nl80211_state *state, |
| struct nl_msg *msg, int argc, |
| char **argv, enum id_input id) |
| { |
| return register_mgmt_frame(state, msg, argc, argv, id); |
| } |
| |
| HIDDEN(mgmt, reg, "", NL80211_CMD_REGISTER_FRAME, 0, CIB_NETDEV, handle_mgmt_reg); |
| |
| static int handle_mgmt_dump(struct nl80211_state *state, |
| struct nl_msg *msg, int argc, |
| char **argv, enum id_input id) |
| { |
| struct nl_cb *mgmt_cb; |
| char *ndev = argv[0]; |
| int mgmt_argc = 5; |
| char **mgmt_argv; |
| unsigned int count = 0; |
| int err = 0; |
| |
| mgmt_argv = calloc(mgmt_argc, sizeof(char*)); |
| if (!mgmt_argv) |
| return -ENOMEM; |
| |
| mgmt_argv[0] = ndev; |
| mgmt_argv[1] = "mgmt"; |
| mgmt_argv[2] = "reg"; |
| |
| if (argc < 6) { |
| err = HANDLER_RET_USAGE; |
| goto out; |
| } |
| |
| argc -= 3; |
| argv += 3; |
| while (argc >= 3) { |
| if (strcmp(argv[0], "frame") != 0) { |
| err = HANDLER_RET_USAGE; |
| goto out; |
| } |
| |
| mgmt_argv[3] = argv[1]; |
| mgmt_argv[4] = argv[2]; |
| |
| argc -= 3; |
| argv += 3; |
| |
| err = handle_cmd(state, II_NETDEV, mgmt_argc, mgmt_argv); |
| if (err) |
| goto out; |
| } |
| |
| if (argc == 2 && strcmp(argv[0], "count") == 0) { |
| count = 1 + atoi(argv[1]); |
| if (count < 1) |
| count = 1; |
| |
| argc -= 2; |
| argv += 2; |
| } else if (argc) { |
| err = HANDLER_RET_USAGE; |
| goto out; |
| } |
| |
| mgmt_cb = nl_cb_alloc(iw_debug ? NL_CB_DEBUG : NL_CB_DEFAULT); |
| if (!mgmt_cb) { |
| err = 1; |
| goto out; |
| } |
| |
| /* need to turn off sequence number checking */ |
| nl_cb_set(mgmt_cb, NL_CB_SEQ_CHECK, NL_CB_CUSTOM, seq_handler, NULL); |
| nl_cb_set(mgmt_cb, NL_CB_VALID, NL_CB_CUSTOM, dump_mgmt_frame, NULL); |
| |
| while (--count) |
| nl_recvmsgs(state->nl_sock, mgmt_cb); |
| |
| nl_cb_put(mgmt_cb); |
| out: |
| free(mgmt_argv); |
| return err; |
| } |
| |
| COMMAND(mgmt, dump, "frame <type as hex ab> <pattern as hex ab:cd:..> [frame <type> <pattern>]* [count <frames>]", |
| 0, 0, CIB_NETDEV, handle_mgmt_dump, |
| "Register for receiving certain mgmt frames and print them.\n" |
| "Frames are selected by their type and pattern containing\n" |
| "the first several bytes of the frame that should match.\n\n" |
| "Example: iw dev wlan0 mgmt dump frame 40 00 frame 40 01:02 count 10\n"); |