mgmt.c

   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
  15 static int seq_handler(struct nl_msg *msg, void *arg)
  16 {
  17         return NL_OK;
  18 }
  19
  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
  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
  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
  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");