1 #include <net/if.h>
2 #include <errno.h>
3 #include <string.h>
4 
5 #include <netlink/genl/genl.h>
6 #include <netlink/genl/family.h>
7 #include <netlink/genl/ctrl.h>
8 #include <netlink/msg.h>
9 #include <netlink/attr.h>
10 
11 #include "nl80211.h"
12 #include "iw.h"
13 
14 /* These enums need to be kept in sync with the kernel */
15 enum hwsim_testmode_attr {
16 	__HWSIM_TM_ATTR_INVALID	= 0,
17 	HWSIM_TM_ATTR_CMD	= 1,
18 	HWSIM_TM_ATTR_PS	= 2,
19 
20 	/* keep last */
21 	__HWSIM_TM_ATTR_AFTER_LAST,
22 	HWSIM_TM_ATTR_MAX	= __HWSIM_TM_ATTR_AFTER_LAST - 1
23 };
24 
25 enum hwsim_testmode_cmd {
26 	HWSIM_TM_CMD_SET_PS		= 0,
27 	HWSIM_TM_CMD_GET_PS		= 1,
28 	HWSIM_TM_CMD_STOP_QUEUES	= 2,
29 	HWSIM_TM_CMD_WAKE_QUEUES	= 3,
30 };
31 
32 
33 SECTION(hwsim);
34 
print_hwsim_ps_handler(struct nl_msg * msg,void * arg)35 static int print_hwsim_ps_handler(struct nl_msg *msg, void *arg)
36 {
37 	struct nlattr *attrs[NL80211_ATTR_MAX + 1];
38 	struct nlattr *tb[HWSIM_TM_ATTR_MAX + 1];
39 	struct genlmsghdr *gnlh = nlmsg_data(nlmsg_hdr(msg));
40 
41 	nla_parse(attrs, NL80211_ATTR_MAX, genlmsg_attrdata(gnlh, 0),
42 		  genlmsg_attrlen(gnlh, 0), NULL);
43 
44 	if (!attrs[NL80211_ATTR_TESTDATA])
45 		return NL_SKIP;
46 
47 	nla_parse(tb, HWSIM_TM_ATTR_MAX, nla_data(attrs[NL80211_ATTR_TESTDATA]),
48 		  nla_len(attrs[NL80211_ATTR_TESTDATA]), NULL);
49 
50 	printf("HWSIM PS: %d\n", nla_get_u32(tb[HWSIM_TM_ATTR_PS]));
51 
52 	return NL_SKIP;
53 }
54 
handle_hwsim_getps(struct nl80211_state * state,struct nl_cb * cb,struct nl_msg * msg,int argc,char ** argv,enum id_input id)55 static int handle_hwsim_getps(struct nl80211_state *state, struct nl_cb *cb,
56 			      struct nl_msg *msg, int argc, char **argv,
57 			      enum id_input id)
58 {
59 	struct nlattr *tmdata;
60 
61 	tmdata = nla_nest_start(msg, NL80211_ATTR_TESTDATA);
62 	if (!tmdata)
63 		goto nla_put_failure;
64 
65 	NLA_PUT_U32(msg, HWSIM_TM_ATTR_CMD, HWSIM_TM_CMD_GET_PS);
66 
67 	nla_nest_end(msg, tmdata);
68 
69 	nl_cb_set(cb, NL_CB_VALID, NL_CB_CUSTOM,
70 		  print_hwsim_ps_handler, NULL);
71 	return 0;
72  nla_put_failure:
73 	return -ENOBUFS;
74 }
75 COMMAND(hwsim, getps, "", NL80211_CMD_TESTMODE, 0, CIB_PHY, handle_hwsim_getps, "");
76 
handle_hwsim_setps(struct nl80211_state * state,struct nl_cb * cb,struct nl_msg * msg,int argc,char ** argv,enum id_input id)77 static int handle_hwsim_setps(struct nl80211_state *state, struct nl_cb *cb,
78 			      struct nl_msg *msg, int argc, char **argv,
79 			      enum id_input id)
80 {
81 	struct nlattr *tmdata;
82 	__u32 ps;
83 	char *end;
84 
85 	if (argc != 1)
86 		return 1;
87 
88 	ps = strtoul(argv[0], &end, 0);
89 	if (*end)
90 		return 1;
91 
92 	tmdata = nla_nest_start(msg, NL80211_ATTR_TESTDATA);
93 	if (!tmdata)
94 		goto nla_put_failure;
95 
96 	NLA_PUT_U32(msg, HWSIM_TM_ATTR_CMD, HWSIM_TM_CMD_SET_PS);
97 	NLA_PUT_U32(msg, HWSIM_TM_ATTR_PS, ps);
98 
99 	nla_nest_end(msg, tmdata);
100 
101 	nl_cb_set(cb, NL_CB_VALID, NL_CB_CUSTOM,
102 		  print_hwsim_ps_handler, NULL);
103 	return 0;
104  nla_put_failure:
105 	return -ENOBUFS;
106 }
107 COMMAND(hwsim, setps, "<value>", NL80211_CMD_TESTMODE, 0, CIB_PHY, handle_hwsim_setps, "");
108 
handle_hwsim_stop_queues(struct nl80211_state * state,struct nl_cb * cb,struct nl_msg * msg,int argc,char ** argv,enum id_input id)109 static int handle_hwsim_stop_queues(struct nl80211_state *state, struct nl_cb *cb,
110 				    struct nl_msg *msg, int argc, char **argv,
111 				    enum id_input id)
112 {
113 	struct nlattr *tmdata;
114 
115 	if (argc != 0)
116 		return 1;
117 
118 	tmdata = nla_nest_start(msg, NL80211_ATTR_TESTDATA);
119 	if (!tmdata)
120 		goto nla_put_failure;
121 
122 	NLA_PUT_U32(msg, HWSIM_TM_ATTR_CMD, HWSIM_TM_CMD_STOP_QUEUES);
123 
124 	nla_nest_end(msg, tmdata);
125 	return 0;
126  nla_put_failure:
127 	return -ENOBUFS;
128 }
129 COMMAND(hwsim, stopqueues, "", NL80211_CMD_TESTMODE, 0, CIB_PHY, handle_hwsim_stop_queues, "");
130 
handle_hwsim_wake_queues(struct nl80211_state * state,struct nl_cb * cb,struct nl_msg * msg,int argc,char ** argv,enum id_input id)131 static int handle_hwsim_wake_queues(struct nl80211_state *state, struct nl_cb *cb,
132 				    struct nl_msg *msg, int argc, char **argv,
133 				    enum id_input id)
134 {
135 	struct nlattr *tmdata;
136 
137 	if (argc != 0)
138 		return 1;
139 
140 	tmdata = nla_nest_start(msg, NL80211_ATTR_TESTDATA);
141 	if (!tmdata)
142 		goto nla_put_failure;
143 
144 	NLA_PUT_U32(msg, HWSIM_TM_ATTR_CMD, HWSIM_TM_CMD_WAKE_QUEUES);
145 
146 	nla_nest_end(msg, tmdata);
147 	return 0;
148  nla_put_failure:
149 	return -ENOBUFS;
150 }
151 COMMAND(hwsim, wakequeues, "", NL80211_CMD_TESTMODE, 0, CIB_PHY, handle_hwsim_wake_queues, "");
152