developer | 8fb759f | 2022-02-21 16:39:38 +0800 | [diff] [blame] | 1 | /* Copyright (C) 2021 Mediatek Inc. */ |
| 2 | #define _GNU_SOURCE |
| 3 | |
| 4 | #include "mtk_vendor_nl80211.h" |
| 5 | #include "mt76-vendor.h" |
| 6 | #include "mwctl.h" |
| 7 | |
| 8 | struct csi_data *csi; |
| 9 | int csi_idx; |
| 10 | |
| 11 | static struct nla_policy csi_ctrl_policy[NUM_MTK_VENDOR_ATTRS_CSI_CTRL] = { |
| 12 | [MTK_VENDOR_ATTR_CSI_CTRL_CFG] = { .type = NLA_NESTED }, |
| 13 | [MTK_VENDOR_ATTR_CSI_CTRL_CFG_MODE] = { .type = NLA_U8 }, |
| 14 | [MTK_VENDOR_ATTR_CSI_CTRL_CFG_TYPE] = { .type = NLA_U8 }, |
| 15 | [MTK_VENDOR_ATTR_CSI_CTRL_CFG_VAL1] = { .type = NLA_U8 }, |
| 16 | [MTK_VENDOR_ATTR_CSI_CTRL_CFG_VAL2] = { .type = NLA_U8 }, |
| 17 | [MTK_VENDOR_ATTR_CSI_CTRL_MAC_ADDR] = { .type = NLA_NESTED }, |
| 18 | [MTK_VENDOR_ATTR_CSI_CTRL_INTERVAL] = { .type = NLA_U32 }, |
| 19 | [MTK_VENDOR_ATTR_CSI_CTRL_DUMP_NUM] = { .type = NLA_U16 }, |
| 20 | [MTK_VENDOR_ATTR_CSI_CTRL_DATA] = { .type = NLA_NESTED }, |
| 21 | }; |
| 22 | |
| 23 | static struct nla_policy csi_data_policy[NUM_MTK_VENDOR_ATTRS_CSI_DATA] = { |
| 24 | [MTK_VENDOR_ATTR_CSI_DATA_VER] = { .type = NLA_U8 }, |
| 25 | [MTK_VENDOR_ATTR_CSI_DATA_TS] = { .type = NLA_U32 }, |
| 26 | [MTK_VENDOR_ATTR_CSI_DATA_RSSI] = { .type = NLA_U8 }, |
| 27 | [MTK_VENDOR_ATTR_CSI_DATA_SNR] = { .type = NLA_U8 }, |
| 28 | [MTK_VENDOR_ATTR_CSI_DATA_BW] = { .type = NLA_U8 }, |
| 29 | [MTK_VENDOR_ATTR_CSI_DATA_CH_IDX] = { .type = NLA_U8 }, |
| 30 | [MTK_VENDOR_ATTR_CSI_DATA_TA] = { .type = NLA_NESTED }, |
| 31 | [MTK_VENDOR_ATTR_CSI_DATA_I] = { .type = NLA_NESTED }, |
| 32 | [MTK_VENDOR_ATTR_CSI_DATA_Q] = { .type = NLA_NESTED }, |
| 33 | [MTK_VENDOR_ATTR_CSI_DATA_INFO] = { .type = NLA_U32 }, |
| 34 | [MTK_VENDOR_ATTR_CSI_DATA_TX_ANT] = { .type = NLA_U8 }, |
| 35 | [MTK_VENDOR_ATTR_CSI_DATA_RX_ANT] = { .type = NLA_U8 }, |
| 36 | [MTK_VENDOR_ATTR_CSI_DATA_MODE] = { .type = NLA_U8 }, |
| 37 | [MTK_VENDOR_ATTR_CSI_DATA_H_IDX] = { .type = NLA_U32 }, |
| 38 | }; |
| 39 | |
| 40 | static int mt76_csi_dump_cb(struct nl_msg *msg, void *arg) |
| 41 | { |
| 42 | struct nlattr *tb[NUM_MTK_VENDOR_ATTRS_CSI_CTRL]; |
| 43 | struct nlattr *tb_data[NUM_MTK_VENDOR_ATTRS_CSI_DATA]; |
| 44 | struct nlattr *attr; |
| 45 | struct nlattr *cur; |
| 46 | size_t idx; |
| 47 | int rem; |
| 48 | struct csi_data *c = &csi[csi_idx]; |
| 49 | |
| 50 | attr = unl_find_attr(&unl, msg, NL80211_ATTR_VENDOR_DATA); |
| 51 | if (!attr) { |
| 52 | fprintf(stderr, "Testdata attribute not found\n"); |
| 53 | return NL_SKIP; |
| 54 | } |
| 55 | |
| 56 | nla_parse_nested(tb, MTK_VENDOR_ATTR_CSI_CTRL_MAX, |
| 57 | attr, csi_ctrl_policy); |
| 58 | |
| 59 | if (!tb[MTK_VENDOR_ATTR_CSI_CTRL_DATA]) |
| 60 | return NL_SKIP; |
| 61 | |
| 62 | nla_parse_nested(tb_data, MTK_VENDOR_ATTR_CSI_DATA_MAX, |
| 63 | tb[MTK_VENDOR_ATTR_CSI_CTRL_DATA], csi_data_policy); |
| 64 | |
| 65 | if (!(tb_data[MTK_VENDOR_ATTR_CSI_DATA_VER] && |
| 66 | tb_data[MTK_VENDOR_ATTR_CSI_DATA_TS] && |
| 67 | tb_data[MTK_VENDOR_ATTR_CSI_DATA_RSSI] && |
| 68 | tb_data[MTK_VENDOR_ATTR_CSI_DATA_SNR] && |
| 69 | tb_data[MTK_VENDOR_ATTR_CSI_DATA_BW] && |
| 70 | tb_data[MTK_VENDOR_ATTR_CSI_DATA_CH_IDX] && |
| 71 | tb_data[MTK_VENDOR_ATTR_CSI_DATA_TA] && |
| 72 | tb_data[MTK_VENDOR_ATTR_CSI_DATA_I] && |
| 73 | tb_data[MTK_VENDOR_ATTR_CSI_DATA_Q] && |
| 74 | tb_data[MTK_VENDOR_ATTR_CSI_DATA_INFO] && |
| 75 | tb_data[MTK_VENDOR_ATTR_CSI_DATA_MODE] && |
| 76 | tb_data[MTK_VENDOR_ATTR_CSI_DATA_H_IDX])) { |
| 77 | fprintf(stderr, "Attributes error for CSI data\n"); |
| 78 | return NL_SKIP; |
| 79 | } |
| 80 | |
| 81 | c->rssi = nla_get_u8(tb_data[MTK_VENDOR_ATTR_CSI_DATA_RSSI]); |
| 82 | c->snr = nla_get_u8(tb_data[MTK_VENDOR_ATTR_CSI_DATA_SNR]); |
| 83 | c->data_bw = nla_get_u8(tb_data[MTK_VENDOR_ATTR_CSI_DATA_BW]); |
| 84 | c->pri_ch_idx = nla_get_u8(tb_data[MTK_VENDOR_ATTR_CSI_DATA_CH_IDX]); |
| 85 | c->rx_mode = nla_get_u8(tb_data[MTK_VENDOR_ATTR_CSI_DATA_MODE]); |
| 86 | |
| 87 | c->tx_idx = nla_get_u16(tb_data[MTK_VENDOR_ATTR_CSI_DATA_TX_ANT]); |
| 88 | c->rx_idx = nla_get_u16(tb_data[MTK_VENDOR_ATTR_CSI_DATA_RX_ANT]); |
| 89 | |
| 90 | c->info = nla_get_u32(tb_data[MTK_VENDOR_ATTR_CSI_DATA_INFO]); |
| 91 | c->h_idx = nla_get_u32(tb_data[MTK_VENDOR_ATTR_CSI_DATA_H_IDX]); |
| 92 | |
| 93 | c->ts = nla_get_u32(tb_data[MTK_VENDOR_ATTR_CSI_DATA_TS]); |
| 94 | |
| 95 | idx = 0; |
| 96 | nla_for_each_nested(cur, tb_data[MTK_VENDOR_ATTR_CSI_DATA_TA], rem) { |
| 97 | if (idx < ETH_ALEN) |
| 98 | c->ta[idx++] = nla_get_u8(cur); |
| 99 | } |
| 100 | |
| 101 | idx = 0; |
| 102 | nla_for_each_nested(cur, tb_data[MTK_VENDOR_ATTR_CSI_DATA_I], rem) { |
| 103 | if (idx < CSI_MAX_COUNT) |
| 104 | c->data_i[idx++] = nla_get_u16(cur); |
| 105 | } |
| 106 | |
| 107 | idx = 0; |
| 108 | nla_for_each_nested(cur, tb_data[MTK_VENDOR_ATTR_CSI_DATA_Q], rem) { |
| 109 | if (idx < CSI_MAX_COUNT) |
| 110 | c->data_q[idx++] = nla_get_u16(cur); |
| 111 | } |
| 112 | |
| 113 | csi_idx++; |
| 114 | |
| 115 | return NL_SKIP; |
| 116 | } |
| 117 | |
| 118 | static int mt76_csi_to_json(const char *name) |
| 119 | { |
| 120 | #define MAX_BUF_SIZE 6000 |
| 121 | FILE *f; |
| 122 | int i, ret = -ENOMEM; |
| 123 | |
| 124 | f = fopen(name, "a+"); |
| 125 | if (!f) { |
| 126 | printf("open failure"); |
| 127 | return 1; |
| 128 | } |
| 129 | |
| 130 | if (fwrite("[", 1, 1, f) != 1) { |
| 131 | perror("fwrite"); |
| 132 | goto out; |
| 133 | } |
| 134 | |
| 135 | for (i = 0; i < csi_idx; i++) { |
| 136 | struct csi_data *c = &csi[i]; |
| 137 | char *pos, *buf; |
| 138 | int j; |
| 139 | |
| 140 | buf = malloc(MAX_BUF_SIZE); |
| 141 | if (!buf) |
| 142 | goto out; |
| 143 | |
| 144 | pos = buf; |
| 145 | pos += snprintf(pos, MAX_BUF_SIZE, "%c", '['); |
| 146 | |
| 147 | pos += snprintf(pos, MAX_BUF_SIZE, "%d,", c->ts); |
| 148 | pos += snprintf(pos, MAX_BUF_SIZE, "\"%02x%02x%02x%02x%02x%02x\",", c->ta[0], c->ta[1], c->ta[2], c->ta[3], c->ta[4], c->ta[5]); |
| 149 | |
| 150 | pos += snprintf(pos, MAX_BUF_SIZE, "%d,", c->rssi); |
| 151 | pos += snprintf(pos, MAX_BUF_SIZE, "%u,", c->snr); |
| 152 | pos += snprintf(pos, MAX_BUF_SIZE, "%u,", c->data_bw); |
| 153 | pos += snprintf(pos, MAX_BUF_SIZE, "%u,", c->pri_ch_idx); |
| 154 | pos += snprintf(pos, MAX_BUF_SIZE, "%u,", c->rx_mode); |
| 155 | pos += snprintf(pos, MAX_BUF_SIZE, "%d,", c->tx_idx); |
| 156 | pos += snprintf(pos, MAX_BUF_SIZE, "%d,", c->rx_idx); |
| 157 | pos += snprintf(pos, MAX_BUF_SIZE, "%d,", c->h_idx); |
| 158 | pos += snprintf(pos, MAX_BUF_SIZE, "%d,", c->info); |
| 159 | |
| 160 | pos += snprintf(pos, MAX_BUF_SIZE, "%c", '['); |
| 161 | for (j = 0; j < 256; j++) { |
| 162 | pos += snprintf(pos, MAX_BUF_SIZE, "%d", c->data_i[j]); |
| 163 | if (j != 255) |
| 164 | pos += snprintf(pos, MAX_BUF_SIZE, ","); |
| 165 | } |
| 166 | pos += snprintf(pos, MAX_BUF_SIZE, "%c,", ']'); |
| 167 | |
| 168 | pos += snprintf(pos, MAX_BUF_SIZE, "%c", '['); |
| 169 | for (j = 0; j < 256; j++) { |
| 170 | pos += snprintf(pos, MAX_BUF_SIZE, "%d", c->data_q[j]); |
| 171 | if (j != 255) |
| 172 | pos += snprintf(pos, MAX_BUF_SIZE, ","); |
| 173 | } |
| 174 | pos += snprintf(pos, MAX_BUF_SIZE, "%c", ']'); |
| 175 | |
| 176 | pos += snprintf(pos, MAX_BUF_SIZE, "%c", ']'); |
| 177 | if (i != csi_idx - 1) |
| 178 | pos += snprintf(pos, MAX_BUF_SIZE, ","); |
| 179 | |
| 180 | if (fwrite(buf, 1, pos - buf, f) != (pos - buf)) { |
| 181 | perror("fwrite"); |
| 182 | free(buf); |
| 183 | goto out; |
| 184 | } |
| 185 | |
| 186 | free(buf); |
| 187 | } |
| 188 | |
| 189 | if (fwrite("]", 1, 1, f) != 1) { |
| 190 | perror("fwrite"); |
| 191 | goto out; |
| 192 | } |
| 193 | |
| 194 | ret = 0; |
| 195 | out: |
| 196 | if (fclose(f)) |
| 197 | perror("fclose"); |
| 198 | |
| 199 | return ret; |
| 200 | } |
| 201 | |
| 202 | struct unl unl_mt76; |
| 203 | |
| 204 | int mt76_csi_dump(struct nl_msg *nlmsg, int argc, |
| 205 | char **argv, void *ctx) |
| 206 | { |
| 207 | int pkt_num, ret = 0, i; |
| 208 | struct nl_msg *msg; |
| 209 | void *data; |
| 210 | int if_idx = 0; |
| 211 | |
| 212 | if (argc < 2) |
| 213 | return 1; |
| 214 | |
| 215 | pkt_num = strtol(argv[0], NULL, 10); |
| 216 | if (pkt_num < 0 || pkt_num > 30000) |
| 217 | return -EINVAL; |
| 218 | |
| 219 | #define CSI_DUMP_PER_NUM 3 |
| 220 | csi_idx = 0; |
| 221 | csi = (struct csi_data *)calloc(pkt_num, sizeof(*csi)); |
| 222 | if_idx = *((int*)ctx); |
| 223 | |
| 224 | for (i = 0; i < pkt_num / CSI_DUMP_PER_NUM; i++) { |
| 225 | if (unl_genl_init(&unl_mt76, "nl80211") < 0) { |
| 226 | fprintf(stderr, "Failed to connect to nl80211\n"); |
| 227 | return 2; |
| 228 | } |
| 229 | |
| 230 | msg = unl_genl_msg(&unl_mt76, NL80211_CMD_VENDOR, true); |
| 231 | |
| 232 | if (nla_put_u32(msg, NL80211_ATTR_IFINDEX, if_idx) || |
| 233 | nla_put_u32(msg, NL80211_ATTR_VENDOR_ID, MTK_NL80211_VENDOR_ID) || |
| 234 | nla_put_u32(msg, NL80211_ATTR_VENDOR_SUBCMD, MTK_NL80211_VENDOR_SUBCMD_CSI_CTRL)) |
| 235 | return false; |
| 236 | |
| 237 | data = nla_nest_start(msg, NL80211_ATTR_VENDOR_DATA | NLA_F_NESTED); |
| 238 | if (!data) |
| 239 | return -ENOMEM; |
| 240 | |
| 241 | if (nla_put_u16(msg, MTK_VENDOR_ATTR_CSI_CTRL_DUMP_NUM, CSI_DUMP_PER_NUM)) |
| 242 | return false; |
| 243 | |
| 244 | nla_nest_end(msg, data); |
| 245 | |
| 246 | ret = unl_genl_request(&unl_mt76, msg, mt76_csi_dump_cb, NULL); |
| 247 | if (ret) |
| 248 | fprintf(stderr, "nl80211 call failed: %s\n", strerror(-ret)); |
| 249 | |
| 250 | unl_free(&unl_mt76); |
| 251 | } |
| 252 | |
| 253 | mt76_csi_to_json(argv[1]); |
| 254 | free(csi); |
| 255 | |
| 256 | return ret; |
| 257 | } |
| 258 | |
| 259 | static int mt76_csi_set_attr(struct nl_msg *msg, int argc, char **argv) |
| 260 | { |
| 261 | int idx = MTK_VENDOR_ATTR_CSI_CTRL_CFG_MODE; |
| 262 | char *val, *s1, *s2, *cur; |
| 263 | void *data; |
| 264 | |
| 265 | val = strchr(argv[0], '='); |
| 266 | if (!val) |
| 267 | return -EINVAL; |
| 268 | |
| 269 | *(val++) = 0; |
| 270 | |
| 271 | if (!strncmp(argv[0], "ctrl", 4)) { |
| 272 | data = nla_nest_start(msg, MTK_VENDOR_ATTR_CSI_CTRL_CFG | NLA_F_NESTED); |
| 273 | if (!data) |
| 274 | return -ENOMEM; |
| 275 | |
| 276 | s1 = s2 = strdup(val); |
| 277 | |
| 278 | while ((cur = strsep(&s1, ",")) != NULL) { |
| 279 | u8 param = strtoul(cur, NULL, 0); |
| 280 | |
| 281 | nla_put_u8(msg, idx++, param); |
| 282 | } |
| 283 | |
| 284 | nla_nest_end(msg, data); |
| 285 | |
| 286 | free(s2); |
| 287 | |
| 288 | if (argc == 2 && |
| 289 | !strncmp(argv[1], "mac_addr", strlen("mac_addr"))) { |
| 290 | u8 a[ETH_ALEN]; |
| 291 | int matches, i; |
| 292 | |
| 293 | val = strchr(argv[1], '='); |
| 294 | if (!val) |
| 295 | return -EINVAL; |
| 296 | |
| 297 | *(val++) = 0; |
| 298 | matches = sscanf(val, "%hhx:%hhx:%hhx:%hhx:%hhx:%hhx", |
| 299 | a, a+1, a+2, a+3, a+4, a+5); |
| 300 | |
| 301 | if (matches != ETH_ALEN) |
| 302 | return -EINVAL; |
| 303 | |
| 304 | data = nla_nest_start(msg, MTK_VENDOR_ATTR_CSI_CTRL_MAC_ADDR | NLA_F_NESTED); |
| 305 | if (!data) |
| 306 | return -ENOMEM; |
| 307 | |
| 308 | for (i = 0; i < ETH_ALEN; i++) |
| 309 | nla_put_u8(msg, i, a[i]); |
| 310 | |
| 311 | nla_nest_end(msg, data); |
| 312 | } |
| 313 | } else if (!strncmp(argv[0], "interval", 8)) { |
| 314 | u32 interval = strtoul(val, NULL, 0); |
| 315 | |
| 316 | nla_put_u32(msg, MTK_VENDOR_ATTR_CSI_CTRL_INTERVAL, interval); |
| 317 | } |
| 318 | |
| 319 | return 0; |
| 320 | } |
| 321 | |
| 322 | int mt76_csi_set(struct nl_msg *msg, int argc, |
| 323 | char **argv, void *ctx) |
| 324 | { |
| 325 | void *data; |
| 326 | |
| 327 | if (argc < 1) |
| 328 | return 1; |
| 329 | |
| 330 | data = nla_nest_start(msg, NL80211_ATTR_VENDOR_DATA | NLA_F_NESTED); |
| 331 | if (!data) |
| 332 | return -ENOMEM; |
| 333 | |
| 334 | mt76_csi_set_attr(msg, argc, argv); |
| 335 | |
| 336 | nla_nest_end(msg, data); |
| 337 | |
| 338 | return 0; |
| 339 | } |
| 340 | |
| 341 | DECLARE_SECTION(dump); |
| 342 | |
| 343 | COMMAND(dump, csi, "", |
| 344 | MTK_NL80211_VENDOR_SUBCMD_CSI_CTRL, 0, CIB_NETDEV, mt76_csi_dump, |
| 345 | "dump csi <packet num> <filename>"); |
| 346 | |
| 347 | DECLARE_SECTION(set); |
| 348 | |
| 349 | COMMAND(set, csi, "ctrl=<opt1>,<opt2>,<opt3>,<opt4> (macaddr=<macaddr>)", |
| 350 | MTK_NL80211_VENDOR_SUBCMD_CSI_CTRL, 0, CIB_NETDEV, mt76_amnt_dump, |
| 351 | "set csi ctrl=<opt1>,<opt2>,<opt3>,<opt4> (macaddr=<macaddr>)\n" |
| 352 | "set csi interval=<interval (us)>"); |
| 353 | |