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
print_vendor_response(struct nl_msg * msg,void * arg)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
read_file(FILE * file,char * buf,size_t size)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
read_hex(unsigned int argc,char ** argv,char * buf,size_t size)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
handle_vendor(struct nl80211_state * state,struct nl_msg * msg,int argc,char ** argv,enum id_input id)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 if (file != stdin)
111 fclose(file);
112 } else
113 count = read_hex(argc - 2, &argv[2], buf, sizeof(buf));
114
115 if (count < 0)
116 return -EINVAL;
117
118 if (count > 0)
119 NLA_PUT(msg, NL80211_ATTR_VENDOR_DATA, count, buf);
120
121 return 0;
122
123 nla_put_failure:
124 if (file && file != stdin)
125 fclose(file);
126 return -ENOBUFS;
127 }
128
handle_vendor_recv(struct nl80211_state * state,struct nl_msg * msg,int argc,char ** argv,enum id_input id)129 static int handle_vendor_recv(struct nl80211_state *state,
130 struct nl_msg *msg, int argc,
131 char **argv, enum id_input id)
132 {
133 register_handler(print_vendor_response, (void *) true);
134 return handle_vendor(state, msg, argc, argv, id);
135 }
136
handle_vendor_recv_bin(struct nl80211_state * state,struct nl_msg * msg,int argc,char ** argv,enum id_input id)137 static int handle_vendor_recv_bin(struct nl80211_state *state,
138 struct nl_msg *msg, int argc,
139 char **argv, enum id_input id)
140 {
141 register_handler(print_vendor_response, (void *) false);
142 return handle_vendor(state, msg, argc, argv, id);
143 }
144
145 COMMAND(vendor, send, "<oui> <subcmd> <filename|-|hex data>", NL80211_CMD_VENDOR, 0, CIB_NETDEV, handle_vendor, "");
146 COMMAND(vendor, recv, "<oui> <subcmd> <filename|-|hex data>", NL80211_CMD_VENDOR, 0, CIB_NETDEV, handle_vendor_recv, "");
147 COMMAND(vendor, recvbin, "<oui> <subcmd> <filename|-|hex data>", NL80211_CMD_VENDOR, 0, CIB_NETDEV, handle_vendor_recv_bin, "");
148