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         ret = sscanf(argv[0], "%x", &type);
  58         if (ret != 1) {
  59                 printf("invalid frame type: %s\n", argv[0]);
  60                 return 2;
  61         }
  62
  63         match = parse_hex(argv[1], &match_len);
  64         if (!match) {
  65                 printf("invalid frame pattern: %s\n", argv[1]);
  66                 return 2;
  67         }
  68
  69         NLA_PUT_U16(msg, NL80211_ATTR_FRAME_TYPE, type);
  70         NLA_PUT(msg, NL80211_ATTR_FRAME_MATCH, match_len, match);
  71
  72         free(match);
  73         return 0;
  74
  75 nla_put_failure:
  76         free(match);
  77         return -ENOBUFS;
  78 }
  79
  80 static int handle_mgmt_reg(struct nl80211_state *state,
  81                                     struct nl_msg *msg, int argc,
  82                                     char **argv, enum id_input id)
  83 {
  84         return register_mgmt_frame(state, msg, argc, argv, id);
  85 }
  86
  87 HIDDEN(mgmt, reg, "", NL80211_CMD_REGISTER_FRAME, 0, CIB_NETDEV, handle_mgmt_reg);
  88
  89 static int handle_mgmt_dump(struct nl80211_state *state,
  90                                struct nl_msg *msg, int argc,
  91                                char **argv, enum id_input id)
  92 {
  93         struct nl_cb *mgmt_cb;
  94         char *ndev = argv[0];
  95         int mgmt_argc = 5;
  96         char **mgmt_argv;
  97         unsigned int count = 0;
  98         int err = 0;
  99         int i;
 100
 101         mgmt_argv = calloc(mgmt_argc, sizeof(char*));
 102         if (!mgmt_argv)
 103                 return -ENOMEM;
 104
 105         mgmt_argv[0] = ndev;
 106         mgmt_argv[1] = "mgmt";
 107         mgmt_argv[2] = "reg";
 108
 109         for (i = 3; i < argc; i += 3) {
 110                 if (strcmp(argv[i], "count") == 0) {
 111                         count = 1 + atoi(argv[i + 1]);
 112                         if (count < 1)
 113                                 count = 1;
 114                         break;
 115                 }
 116
 117                 if (strcmp(argv[i], "frame") != 0) {
 118                         err = 1;
 119                         goto out;
 120                 }
 121
 122                 mgmt_argv[3] = argv[i + 1];
 123                 mgmt_argv[4] = argv[i + 2];
 124
 125                 err = handle_cmd(state, II_NETDEV, mgmt_argc, mgmt_argv);
 126                 if (err)
 127                         goto out;
 128         }
 129
 130         mgmt_cb = nl_cb_alloc(iw_debug ? NL_CB_DEBUG : NL_CB_DEFAULT);
 131         if (!mgmt_cb) {
 132                 err = 1;
 133                 goto out;
 134         }
 135
 136         /* need to turn off sequence number checking */
 137         nl_cb_set(mgmt_cb, NL_CB_SEQ_CHECK, NL_CB_CUSTOM, seq_handler, NULL);
 138         nl_cb_set(mgmt_cb, NL_CB_VALID, NL_CB_CUSTOM, dump_mgmt_frame, NULL);
 139
 140         while (--count)
 141                 nl_recvmsgs(state->nl_sock, mgmt_cb);
 142
 143         nl_cb_put(mgmt_cb);
 144 out:
 145         free(mgmt_argv);
 146         return err;
 147 }
 148
 149 COMMAND(mgmt, dump, "frame <type as hex ab> <pattern as hex ab:cd:..> [frame <type> <pattern>]* [count <frames>]",
 150         0, 0, CIB_NETDEV, handle_mgmt_dump,
 151         "Register for receiving certain mgmt frames and print them.\n"
 152         "Frames are selected by their type and pattern containing\n"
 153         "the first several bytes of the frame that should match.\n\n"
 154         "Example: iw dev wlan0 mgmt dump frame 40 00 frame 40 01:02 count 10\n");