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