1 #include <errno.h>
2 
3 #include "nl80211.h"
4 #include "iw.h"
5 
6 
parse_vht_chunk(const char * arg,__u8 * nss,__u16 * mcs)7 static int parse_vht_chunk(const char *arg, __u8 *nss, __u16 *mcs)
8 {
9 	int count, i;
10 	unsigned int inss, mcs_start, mcs_end, tab[10];
11 
12 	*nss = 0; *mcs = 0;
13 
14 	if (strchr(arg, '-')) {
15 		/* Format: NSS:MCS_START-MCS_END */
16 		count = sscanf(arg, "%u:%u-%u", &inss, &mcs_start, &mcs_end);
17 
18 		if (count != 3)
19 			return 0;
20 
21 		if (inss < 1 || inss > NL80211_VHT_NSS_MAX)
22 			return 0;
23 
24 		if (mcs_start > mcs_end)
25 			return 0;
26 
27 		if (mcs_start > 9 || mcs_end > 9)
28 			return 0;
29 
30 		*nss = inss;
31 		for (i = mcs_start; i <= mcs_end; i++)
32 			*mcs |= 1 << i;
33 
34 	} else {
35 		/* Format: NSS:MCSx,MCSy,... */
36 		count = sscanf(arg, "%u:%u,%u,%u,%u,%u,%u,%u,%u,%u,%u", &inss,
37 			   &tab[0], &tab[1], &tab[2], &tab[3], &tab[4], &tab[5],
38 			   &tab[6], &tab[7], &tab[8], &tab[9]);
39 
40 		if (count < 2)
41 			return 0;
42 
43 		if (inss < 1 || inss > NL80211_VHT_NSS_MAX)
44 			return 0;
45 
46 		*nss = inss;
47 		for (i = 0; i < count - 1; i++) {
48 			if (tab[i] > 9)
49 				return 0;
50 			*mcs |= 1 << tab[i];
51 		}
52 	}
53 
54 	return 1;
55 }
56 
setup_vht(struct nl80211_txrate_vht * txrate_vht,int argc,char ** argv)57 static int setup_vht(struct nl80211_txrate_vht *txrate_vht,
58 		     int argc, char **argv)
59 {
60 	__u8 nss;
61 	__u16 mcs;
62 	int i;
63 
64 	memset(txrate_vht, 0, sizeof(*txrate_vht));
65 
66 	for (i = 0; i < argc; i++) {
67 		if(!parse_vht_chunk(argv[i], &nss, &mcs))
68 			return 0;
69 
70 		nss--;
71 		txrate_vht->mcs[nss] |= mcs;
72 	}
73 
74 	return 1;
75 }
76 
77 #define VHT_ARGC_MAX	100
78 
handle_bitrates(struct nl80211_state * state,struct nl_cb * cb,struct nl_msg * msg,int argc,char ** argv,enum id_input id)79 static int handle_bitrates(struct nl80211_state *state,
80 			   struct nl_cb *cb,
81 			   struct nl_msg *msg,
82 			   int argc, char **argv,
83 			   enum id_input id)
84 {
85 	struct nlattr *nl_rates, *nl_band;
86 	int i;
87 	bool have_legacy_24 = false, have_legacy_5 = false;
88 	uint8_t legacy_24[32], legacy_5[32];
89 	int n_legacy_24 = 0, n_legacy_5 = 0;
90 	uint8_t *legacy = NULL;
91 	int *n_legacy = NULL;
92 	bool have_ht_mcs_24 = false, have_ht_mcs_5 = false;
93 	bool have_vht_mcs_24 = false, have_vht_mcs_5 = false;
94 	uint8_t ht_mcs_24[77], ht_mcs_5[77];
95 	int n_ht_mcs_24 = 0, n_ht_mcs_5 = 0;
96 	struct nl80211_txrate_vht txrate_vht_24 = {};
97 	struct nl80211_txrate_vht txrate_vht_5 = {};
98 	uint8_t *mcs = NULL;
99 	int *n_mcs = NULL;
100 	char *vht_argv_5[VHT_ARGC_MAX] = {}; char *vht_argv_24[VHT_ARGC_MAX] = {};
101 	char **vht_argv = NULL;
102 	int vht_argc_5 = 0; int vht_argc_24 = 0;
103 	int *vht_argc = NULL;
104 	int sgi_24 = 0, sgi_5 = 0, lgi_24 = 0, lgi_5 = 0;
105 
106 	enum {
107 		S_NONE,
108 		S_LEGACY,
109 		S_HT,
110 		S_VHT,
111 		S_GI,
112 	} parser_state = S_NONE;
113 
114 	for (i = 0; i < argc; i++) {
115 		char *end;
116 		double tmpd;
117 		long tmpl;
118 
119 		if (strcmp(argv[i], "legacy-2.4") == 0) {
120 			if (have_legacy_24)
121 				return 1;
122 			parser_state = S_LEGACY;
123 			legacy = legacy_24;
124 			n_legacy = &n_legacy_24;
125 			have_legacy_24 = true;
126 		} else if (strcmp(argv[i], "legacy-5") == 0) {
127 			if (have_legacy_5)
128 				return 1;
129 			parser_state = S_LEGACY;
130 			legacy = legacy_5;
131 			n_legacy = &n_legacy_5;
132 			have_legacy_5 = true;
133 		}
134 		else if (strcmp(argv[i], "ht-mcs-2.4") == 0) {
135 			if (have_ht_mcs_24)
136 				return 1;
137 			parser_state = S_HT;
138 			mcs = ht_mcs_24;
139 			n_mcs = &n_ht_mcs_24;
140 			have_ht_mcs_24 = true;
141 		} else if (strcmp(argv[i], "ht-mcs-5") == 0) {
142 			if (have_ht_mcs_5)
143 				return 1;
144 			parser_state = S_HT;
145 			mcs = ht_mcs_5;
146 			n_mcs = &n_ht_mcs_5;
147 			have_ht_mcs_5 = true;
148 		} else if (strcmp(argv[i], "vht-mcs-2.4") == 0) {
149 			if (have_vht_mcs_24)
150 				return 1;
151 			parser_state = S_VHT;
152 			vht_argv = vht_argv_24;
153 			vht_argc = &vht_argc_24;
154 			have_vht_mcs_24 = true;
155 		} else if (strcmp(argv[i], "vht-mcs-5") == 0) {
156 			if (have_vht_mcs_5)
157 				return 1;
158 			parser_state = S_VHT;
159 			vht_argv = vht_argv_5;
160 			vht_argc = &vht_argc_5;
161 			have_vht_mcs_5 = true;
162 		} else if (strcmp(argv[i], "sgi-2.4") == 0) {
163 			sgi_24 = 1;
164 			parser_state = S_GI;
165 		} else if (strcmp(argv[i], "sgi-5") == 0) {
166 			sgi_5 = 1;
167 			parser_state = S_GI;
168 		} else if (strcmp(argv[i], "lgi-2.4") == 0) {
169 			lgi_24 = 1;
170 			parser_state = S_GI;
171 		} else if (strcmp(argv[i], "lgi-5") == 0) {
172 			lgi_5 = 1;
173 			parser_state = S_GI;
174 		} else switch (parser_state) {
175 		case S_LEGACY:
176 			tmpd = strtod(argv[i], &end);
177 			if (*end != '\0')
178 				return 1;
179 			if (tmpd < 1 || tmpd > 255 * 2)
180 				return 1;
181 			legacy[(*n_legacy)++] = tmpd * 2;
182 			break;
183 		case S_HT:
184 			tmpl = strtol(argv[i], &end, 0);
185 			if (*end != '\0')
186 				return 1;
187 			if (tmpl < 0 || tmpl > 255)
188 				return 1;
189 			mcs[(*n_mcs)++] = tmpl;
190 			break;
191 		case S_VHT:
192 			if (*vht_argc >= VHT_ARGC_MAX)
193 				return 1;
194 			vht_argv[(*vht_argc)++] = argv[i];
195 			break;
196 		case S_GI:
197 			break;
198 		default:
199 			return 1;
200 		}
201 	}
202 
203 	if (have_vht_mcs_24)
204 		if(!setup_vht(&txrate_vht_24, vht_argc_24, vht_argv_24))
205 			return -EINVAL;
206 
207 	if (have_vht_mcs_5)
208 		if(!setup_vht(&txrate_vht_5, vht_argc_5, vht_argv_5))
209 			return -EINVAL;
210 
211 	if (sgi_5 && lgi_5)
212 		return 1;
213 
214 	if (sgi_24 && lgi_24)
215 		return 1;
216 
217 	nl_rates = nla_nest_start(msg, NL80211_ATTR_TX_RATES);
218 	if (!nl_rates)
219 		goto nla_put_failure;
220 
221 	if (have_legacy_24 || have_ht_mcs_24 || have_vht_mcs_24 || sgi_24 || lgi_24) {
222 		nl_band = nla_nest_start(msg, NL80211_BAND_2GHZ);
223 		if (!nl_band)
224 			goto nla_put_failure;
225 		if (have_legacy_24)
226 			nla_put(msg, NL80211_TXRATE_LEGACY, n_legacy_24, legacy_24);
227 		if (have_ht_mcs_24)
228 			nla_put(msg, NL80211_TXRATE_HT, n_ht_mcs_24, ht_mcs_24);
229 		if (have_vht_mcs_24)
230 			nla_put(msg, NL80211_TXRATE_VHT, sizeof(txrate_vht_24), &txrate_vht_24);
231 		if (sgi_24)
232 			nla_put_u8(msg, NL80211_TXRATE_GI, NL80211_TXRATE_FORCE_SGI);
233 		if (lgi_24)
234 			nla_put_u8(msg, NL80211_TXRATE_GI, NL80211_TXRATE_FORCE_LGI);
235 		nla_nest_end(msg, nl_band);
236 	}
237 
238 	if (have_legacy_5 || have_ht_mcs_5 || have_vht_mcs_5 || sgi_5 || lgi_5) {
239 		nl_band = nla_nest_start(msg, NL80211_BAND_5GHZ);
240 		if (!nl_band)
241 			goto nla_put_failure;
242 		if (have_legacy_5)
243 			nla_put(msg, NL80211_TXRATE_LEGACY, n_legacy_5, legacy_5);
244 		if (have_ht_mcs_5)
245 			nla_put(msg, NL80211_TXRATE_HT, n_ht_mcs_5, ht_mcs_5);
246 		if (have_vht_mcs_5)
247 			nla_put(msg, NL80211_TXRATE_VHT, sizeof(txrate_vht_5), &txrate_vht_5);
248 		if (sgi_5)
249 			nla_put_u8(msg, NL80211_TXRATE_GI, NL80211_TXRATE_FORCE_SGI);
250 		if (lgi_5)
251 			nla_put_u8(msg, NL80211_TXRATE_GI, NL80211_TXRATE_FORCE_LGI);
252 		nla_nest_end(msg, nl_band);
253 	}
254 
255 	nla_nest_end(msg, nl_rates);
256 
257 	return 0;
258  nla_put_failure:
259 	return -ENOBUFS;
260 }
261 
262 #define DESCR_LEGACY "[legacy-<2.4|5> <legacy rate in Mbps>*]"
263 #define DESCR DESCR_LEGACY " [ht-mcs-<2.4|5> <MCS index>*] [vht-mcs-<2.4|5> <NSS:MCSx,MCSy... | NSS:MCSx-MCSy>*] [sgi-2.4|lgi-2.4] [sgi-5|lgi-5]"
264 
265 COMMAND(set, bitrates, "[legacy-<2.4|5> <legacy rate in Mbps>*] [ht-mcs-<2.4|5> <MCS index>*] [vht-mcs-<2.4|5> <NSS:MCSx,MCSy... | NSS:MCSx-MCSy>*] [sgi-2.4|lgi-2.4] [sgi-5|lgi-5]",
266 	NL80211_CMD_SET_TX_BITRATE_MASK, 0, CIB_NETDEV, handle_bitrates,
267 	"Sets up the specified rate masks.\n"
268 	"Not passing any arguments would clear the existing mask (if any).");
269