vendor.c

   1 #include <errno.h>
   2 #include <string.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(vendor);
  14
  15 static int print_vendor_response(struct nl_msg *msg, void *arg)
  16 {
  17         struct nlattr *attr;
  18         struct genlmsghdr *gnlh = nlmsg_data(nlmsg_hdr(msg));
  19         bool print_ascii = (bool) arg;
  20         uint8_t *data;
  21         int len;
  22
  23         attr = nla_find(genlmsg_attrdata(gnlh, 0),
  24                         genlmsg_attrlen(gnlh, 0),
  25                         NL80211_ATTR_VENDOR_DATA);
  26         if (!attr) {
  27                 fprintf(stderr, "vendor data attribute missing!\n");
  28                 return NL_SKIP;
  29         }
  30
  31         data = (uint8_t *) nla_data(attr);
  32         len = nla_len(attr);
  33
  34         if (print_ascii)
  35                 iw_hexdump("vendor response", data, len);
  36         else
  37                 fwrite(data, 1, len, stdout);
  38
  39         return NL_OK;
  40 }
  41
  42 static int read_file(FILE *file, char *buf, size_t size)
  43 {
  44         size_t count = 0;
  45         int data;
  46
  47         while ((data = fgetc(file)) != EOF) {
  48                 if (count >= size)
  49                         return -EINVAL;
  50                 buf[count] = data;
  51                 count++;
  52         }
  53
  54         return count;
  55 }
  56
  57 static int read_hex(unsigned int argc, char **argv, char *buf, size_t size)
  58 {
  59         unsigned int i, data;
  60         int res;
  61
  62         if (argc > size)
  63                 return -EINVAL;
  64
  65         for (i = 0; i < argc; i++) {
  66                 res = sscanf(argv[i], "0x%x", &data);
  67                 if (res != 1 || data > 0xff)
  68                         return -EINVAL;
  69                 buf[i] = data;
  70         }
  71
  72         return argc;
  73 }
  74
  75 static int handle_vendor(struct nl80211_state *state,
  76                          struct nl_msg *msg, int argc, char **argv,
  77                          enum id_input id)
  78 {
  79         unsigned int oui;
  80         unsigned int subcmd;
  81         char buf[2048] = {};
  82         int res, count = 0;
  83         FILE *file = NULL;
  84
  85         if (argc < 3)
  86                 return 1;
  87
  88         res = sscanf(argv[0], "0x%x", &oui);
  89         if (res != 1) {
  90                 printf("Vendor command must start with 0x\n");
  91                 return 2;
  92         }
  93
  94         res = sscanf(argv[1], "0x%x", &subcmd);
  95         if (res != 1) {
  96                 printf("Sub command must start with 0x\n");
  97                 return 2;
  98         }
  99
 100         if (!strcmp(argv[2], "-"))
 101                 file = stdin;
 102         else
 103                 file = fopen(argv[2], "r");
 104
 105         NLA_PUT_U32(msg, NL80211_ATTR_VENDOR_ID, oui);
 106         NLA_PUT_U32(msg, NL80211_ATTR_VENDOR_SUBCMD, subcmd);
 107
 108         if (file) {
 109                 count = read_file(file, buf, sizeof(buf));
 110                 fclose(file);
 111         } else
 112                 count = read_hex(argc - 2, &argv[2], buf, sizeof(buf));
 113
 114         if (count < 0)
 115                 return -EINVAL;
 116
 117         if (count > 0)
 118                 NLA_PUT(msg, NL80211_ATTR_VENDOR_DATA, count, buf);
 119
 120         return 0;
 121
 122 nla_put_failure:
 123         return -ENOBUFS;
 124 }
 125
 126 static int handle_vendor_recv(struct nl80211_state *state,
 127                               struct nl_msg *msg, int argc,
 128                               char **argv, enum id_input id)
 129 {
 130         register_handler(print_vendor_response, (void *) true);
 131         return handle_vendor(state, msg, argc, argv, id);
 132 }
 133
 134 static int handle_vendor_recv_bin(struct nl80211_state *state,
 135                                   struct nl_msg *msg, int argc,
 136                                   char **argv, enum id_input id)
 137 {
 138         register_handler(print_vendor_response, (void *) false);
 139         return handle_vendor(state, msg, argc, argv, id);
 140 }
 141
 142 COMMAND(vendor, send, "<oui> <subcmd> <filename|-|hex data>", NL80211_CMD_VENDOR, 0, CIB_NETDEV, handle_vendor, "");
 143 COMMAND(vendor, recv, "<oui> <subcmd> <filename|-|hex data>", NL80211_CMD_VENDOR, 0, CIB_NETDEV, handle_vendor_recv, "");
 144 COMMAND(vendor, recvbin, "<oui> <subcmd> <filename|-|hex data>", NL80211_CMD_VENDOR, 0, CIB_NETDEV, handle_vendor_recv_bin, "");