1 /*
2 * wmediumd, wireless medium simulator for mac80211_hwsim kernel module
3 * Copyright (c) 2011 cozybit Inc.
4 * Copyright (C) 2020 Intel Corporation
5 *
6 * Author: Javier Lopez <jlopex@cozybit.com>
7 * Javier Cardona <javier@cozybit.com>
8 *
9 * This program is free software; you can redistribute it and/or
10 * modify it under the terms of the GNU General Public License
11 * as published by the Free Software Foundation; either version 2
12 * of the License, or (at your option) any later version.
13 *
14 * This program is distributed in the hope that it will be useful,
15 * but WITHOUT ANY WARRANTY; without even the implied warranty of
16 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
17 * GNU General Public License for more details.
18 *
19 * You should have received a copy of the GNU General Public License
20 * along with this program; if not, write to the Free Software
21 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
22 * 02110-1301, USA.
23 */
24
25 #include <netlink/netlink.h>
26 #include <netlink/genl/genl.h>
27 #include <netlink/genl/ctrl.h>
28 #include <netlink/genl/family.h>
29 #include <assert.h>
30 #include <stdint.h>
31 #include <getopt.h>
32 #include <signal.h>
33 #include <math.h>
34 #include <sys/syslog.h>
35 #include <sys/timerfd.h>
36 #include <errno.h>
37 #include <limits.h>
38 #include <time.h>
39 #include <unistd.h>
40 #include <stdarg.h>
41 #include <endian.h>
42 #include <sys/msg.h>
43 #include <usfstl/loop.h>
44 #include <usfstl/sched.h>
45 #include <usfstl/schedctrl.h>
46 #include <usfstl/vhost.h>
47 #include <usfstl/uds.h>
48
49 #include "wmediumd.h"
50 #include "ieee80211.h"
51 #include "config.h"
52 #include "api.h"
53 #include "pmsr.h"
54 #include "grpc.h"
55
56 USFSTL_SCHEDULER(scheduler);
57
58 static void wmediumd_deliver_frame(struct usfstl_job *job);
59
60 enum {
61 HWSIM_VQ_TX,
62 HWSIM_VQ_RX,
63 HWSIM_NUM_VQS,
64 };
65
stpcpy_safe(char * dst,const char * src)66 static char *stpcpy_safe(char *dst, const char *src) {
67 if (dst == NULL) {
68 return NULL;
69 }
70
71 if (src == NULL) {
72 *dst = '\0';
73 return dst;
74 }
75
76 return stpcpy(dst, src);
77 }
78
strlen_safe(const char * src)79 static int strlen_safe(const char *src) {
80 return src == NULL ? 0 : strlen(src);
81 }
82
div_round(int a,int b)83 static inline int div_round(int a, int b)
84 {
85 return (a + b - 1) / b;
86 }
87
sec_to_ns(time_t sec)88 static inline u64 sec_to_ns(time_t sec)
89 {
90 return sec * 1000 * 1000 * 1000;
91 }
92
ns_to_us(u64 ns)93 static inline u64 ns_to_us(u64 ns)
94 {
95 return ns / 1000L;
96 }
97
ts_to_ns(struct timespec ts)98 static inline u64 ts_to_ns(struct timespec ts)
99 {
100 return sec_to_ns(ts.tv_sec) + ts.tv_nsec;
101 }
102
distance_to_rtt(double distance)103 static inline double distance_to_rtt(double distance)
104 {
105 const long light_speed = 299792458L;
106 return distance / light_speed;
107 }
108
sec_to_ps(double sec)109 static inline double sec_to_ps(double sec)
110 {
111 return sec * 1000 * 1000 * 1000;
112 }
113
meter_to_mm(double meter)114 static inline double meter_to_mm(double meter)
115 {
116 return meter * 1000;
117 }
118
pkt_duration(int len,int rate)119 static inline int pkt_duration(int len, int rate)
120 {
121 /* preamble + signal + t_sym * n_sym, rate in 100 kbps */
122 return 16 + 4 + 4 * div_round((16 + 8 * len + 6) * 10, 4 * rate);
123 }
124
w_logf(struct wmediumd * ctx,u8 level,const char * format,...)125 int w_logf(struct wmediumd *ctx, u8 level, const char *format, ...)
126 {
127 va_list(args);
128 va_start(args, format);
129 if (ctx->log_lvl >= level) {
130 return vprintf(format, args);
131 }
132 return -1;
133 }
134
w_flogf(struct wmediumd * ctx,u8 level,FILE * stream,const char * format,...)135 int w_flogf(struct wmediumd *ctx, u8 level, FILE *stream, const char *format, ...)
136 {
137 va_list(args);
138 va_start(args, format);
139 if (ctx->log_lvl >= level) {
140 return vfprintf(stream, format, args);
141 }
142 return -1;
143 }
144
wqueue_init(struct wqueue * wqueue,int cw_min,int cw_max)145 static void wqueue_init(struct wqueue *wqueue, int cw_min, int cw_max)
146 {
147 INIT_LIST_HEAD(&wqueue->frames);
148 wqueue->cw_min = cw_min;
149 wqueue->cw_max = cw_max;
150 }
151
station_init_queues(struct station * station)152 void station_init_queues(struct station *station)
153 {
154 wqueue_init(&station->queues[IEEE80211_AC_BK], 15, 1023);
155 wqueue_init(&station->queues[IEEE80211_AC_BE], 15, 1023);
156 wqueue_init(&station->queues[IEEE80211_AC_VI], 7, 15);
157 wqueue_init(&station->queues[IEEE80211_AC_VO], 3, 7);
158 }
159
frame_has_a4(struct frame * frame)160 static inline bool frame_has_a4(struct frame *frame)
161 {
162 struct ieee80211_hdr *hdr = (void *)frame->data;
163
164 return (hdr->frame_control[1] & (FCTL_TODS | FCTL_FROMDS)) ==
165 (FCTL_TODS | FCTL_FROMDS);
166 }
167
frame_is_mgmt(struct frame * frame)168 static inline bool frame_is_mgmt(struct frame *frame)
169 {
170 struct ieee80211_hdr *hdr = (void *)frame->data;
171
172 return (hdr->frame_control[0] & FCTL_FTYPE) == FTYPE_MGMT;
173 }
174
frame_is_data(struct frame * frame)175 static inline bool frame_is_data(struct frame *frame)
176 {
177 struct ieee80211_hdr *hdr = (void *)frame->data;
178
179 return (hdr->frame_control[0] & FCTL_FTYPE) == FTYPE_DATA;
180 }
181
frame_is_data_qos(struct frame * frame)182 static inline bool frame_is_data_qos(struct frame *frame)
183 {
184 struct ieee80211_hdr *hdr = (void *)frame->data;
185
186 return (hdr->frame_control[0] & (FCTL_FTYPE | STYPE_QOS_DATA)) ==
187 (FTYPE_DATA | STYPE_QOS_DATA);
188 }
189
frame_is_probe_req(struct frame * frame)190 static inline bool frame_is_probe_req(struct frame *frame)
191 {
192 struct ieee80211_hdr *hdr = (void *)frame->data;
193
194 return (hdr->frame_control[0] & (FCTL_FTYPE | STYPE_PROBE_REQ)) ==
195 (FTYPE_MGMT | STYPE_PROBE_REQ);
196 }
197
frame_has_zero_rates(const struct frame * frame)198 static inline bool frame_has_zero_rates(const struct frame *frame)
199 {
200 for (int i = 0; i < frame->tx_rates_count; i++) {
201 if (frame->tx_rates[i].idx < 0)
202 break;
203
204 if (frame->tx_rates[i].count > 0) {
205 return false;
206 }
207 }
208
209 return true;
210 }
211
fill_tx_rates(struct frame * frame)212 static inline void fill_tx_rates(struct frame *frame)
213 {
214 if (frame->tx_rates_count <= 0) {
215 return;
216 }
217
218 int max_index = get_max_index();
219
220 /* Starting from OFDM rate (See per.c#rateset) */
221 const int basic_rate_start = 4; /* 6 mbps */
222
223 int i;
224 int rate_count = min(max_index - basic_rate_start + 1, frame->tx_rates_count);
225
226 for (i = 0; i < rate_count; i++) {
227 frame->tx_rates[i].idx = basic_rate_start + rate_count - i - 1;
228 frame->tx_rates[i].count = 4;
229 }
230
231 for (; i < frame->tx_rates_count; i++) {
232 frame->tx_rates[i].idx = -1;
233 frame->tx_rates[i].count = 0;
234 }
235 }
236
frame_get_qos_ctl(struct frame * frame)237 static inline u8 *frame_get_qos_ctl(struct frame *frame)
238 {
239 struct ieee80211_hdr *hdr = (void *)frame->data;
240
241 if (frame_has_a4(frame))
242 return (u8 *)hdr + 30;
243 else
244 return (u8 *)hdr + 24;
245 }
246
frame_select_queue_80211(struct frame * frame)247 static enum ieee80211_ac_number frame_select_queue_80211(struct frame *frame)
248 {
249 u8 *p;
250 int priority;
251
252 if (!frame_is_data(frame))
253 return IEEE80211_AC_VO;
254
255 if (!frame_is_data_qos(frame))
256 return IEEE80211_AC_BE;
257
258 p = frame_get_qos_ctl(frame);
259 priority = *p & QOS_CTL_TAG1D_MASK;
260
261 return ieee802_1d_to_ac[priority];
262 }
263
dBm_to_milliwatt(int decibel_intf)264 static double dBm_to_milliwatt(int decibel_intf)
265 {
266 #define INTF_LIMIT (31)
267 int intf_diff = NOISE_LEVEL - decibel_intf;
268
269 if (intf_diff >= INTF_LIMIT)
270 return 0.001;
271
272 if (intf_diff <= -INTF_LIMIT)
273 return 1000.0;
274
275 return pow(10.0, -intf_diff / 10.0);
276 }
277
milliwatt_to_dBm(double value)278 static double milliwatt_to_dBm(double value)
279 {
280 return 10.0 * log10(value);
281 }
282
set_interference_duration(struct wmediumd * ctx,int src_idx,int duration,int signal)283 static int set_interference_duration(struct wmediumd *ctx, int src_idx,
284 int duration, int signal)
285 {
286 int i;
287
288 if (!ctx->intf)
289 return 0;
290
291 if (signal >= CCA_THRESHOLD)
292 return 0;
293
294 for (i = 0; i < ctx->num_stas; i++) {
295 ctx->intf[ctx->num_stas * src_idx + i].duration += duration;
296 // use only latest value
297 ctx->intf[ctx->num_stas * src_idx + i].signal = signal;
298 }
299
300 return 1;
301 }
302
get_signal_offset_by_interference(struct wmediumd * ctx,int src_idx,int dst_idx)303 static int get_signal_offset_by_interference(struct wmediumd *ctx, int src_idx,
304 int dst_idx)
305 {
306 int i;
307 double intf_power;
308
309 if (!ctx->intf)
310 return 0;
311
312 intf_power = 0.0;
313 for (i = 0; i < ctx->num_stas; i++) {
314 if (i == src_idx || i == dst_idx)
315 continue;
316 if (drand48() < ctx->intf[i * ctx->num_stas + dst_idx].prob_col)
317 intf_power += dBm_to_milliwatt(
318 ctx->intf[i * ctx->num_stas + dst_idx].signal);
319 }
320
321 if (intf_power <= 1.0)
322 return 0;
323
324 return (int)(milliwatt_to_dBm(intf_power) + 0.5);
325 }
326
is_multicast_ether_addr(const u8 * addr)327 static bool is_multicast_ether_addr(const u8 *addr)
328 {
329 return 0x01 & addr[0];
330 }
331
get_station_by_addr(struct wmediumd * ctx,u8 * addr)332 static struct station *get_station_by_addr(struct wmediumd *ctx, u8 *addr)
333 {
334 struct station *station;
335
336 list_for_each_entry(station, &ctx->stations, list) {
337 if (memcmp(station->addr, addr, ETH_ALEN) == 0)
338 return station;
339 }
340 return NULL;
341 }
342
station_has_addr(struct station * station,const u8 * addr)343 static bool station_has_addr(struct station *station, const u8 *addr)
344 {
345 unsigned int i;
346
347 if (memcmp(station->addr, addr, ETH_ALEN) == 0)
348 return true;
349
350 for (i = 0; i < station->n_addrs; i++) {
351 if (memcmp(station->addrs[i].addr, addr, ETH_ALEN) == 0)
352 return true;
353 }
354
355 return false;
356 }
357
get_station_by_used_addr(struct wmediumd * ctx,u8 * addr)358 static struct station *get_station_by_used_addr(struct wmediumd *ctx, u8 *addr)
359 {
360 struct station *station;
361
362 list_for_each_entry(station, &ctx->stations, list) {
363 if (station_has_addr(station, addr))
364 return station;
365 }
366 return NULL;
367 }
368
wmediumd_wait_for_client_ack(struct wmediumd * ctx,struct client * client)369 static void wmediumd_wait_for_client_ack(struct wmediumd *ctx,
370 struct client *client)
371 {
372 client->wait_for_ack = true;
373
374 while (client->wait_for_ack)
375 usfstl_loop_wait_and_handle();
376 }
377
378 static void wmediumd_remove_client(struct wmediumd *ctx, struct client *client);
379
wmediumd_notify_frame_start(struct usfstl_job * job)380 static void wmediumd_notify_frame_start(struct usfstl_job *job)
381 {
382 struct frame *frame = container_of(job, struct frame, start_job);
383 struct wmediumd *ctx = job->data;
384 struct client *client, *tmp;
385 struct {
386 struct wmediumd_message_header hdr;
387 struct wmediumd_tx_start start;
388 } __attribute__((packed)) msg = {
389 .hdr.type = WMEDIUMD_MSG_TX_START,
390 .hdr.data_len = sizeof(msg.start),
391 .start.freq = frame->freq,
392 };
393
394 if (ctx->ctrl)
395 usfstl_sched_ctrl_sync_to(ctx->ctrl);
396
397 list_for_each_entry_safe(client, tmp, &ctx->clients, list) {
398 if (!(client->flags & WMEDIUMD_CTL_NOTIFY_TX_START))
399 continue;
400
401 if (client == frame->src)
402 msg.start.cookie = frame->cookie;
403 else
404 msg.start.cookie = 0;
405
406 /* must be API socket since flags cannot otherwise be set */
407 assert(client->type == CLIENT_API_SOCK);
408
409 if (write(client->loop.fd, &msg, sizeof(msg)) < sizeof(msg)) {
410 usfstl_loop_unregister(&client->loop);
411 wmediumd_remove_client(ctx, client);
412 continue;
413 }
414
415 wmediumd_wait_for_client_ack(ctx, client);
416 }
417 }
418
log2pcap(struct wmediumd * ctx,struct frame * frame,uint64_t ts)419 static void log2pcap(struct wmediumd *ctx, struct frame *frame, uint64_t ts)
420 {
421 struct {
422 uint8_t it_version;
423 uint8_t it_pad;
424 uint16_t it_len;
425 uint32_t it_present;
426 struct {
427 uint16_t freq, flags;
428 } channel;
429 uint8_t signal;
430 } __attribute__((packed)) radiotap_hdr = {
431 .it_len = htole16(sizeof(radiotap_hdr)),
432 .it_present = htole32(1 << 3 /* channel */ |
433 1 << 5 /* signal dBm */),
434 .channel.freq = htole16(frame->freq),
435 .signal = frame->signal,
436 };
437 struct {
438 uint32_t type, blocklen, ifidx, ts_hi, ts_lo, caplen, pktlen;
439 } __attribute__((packed)) blockhdr = {
440 .type = 6,
441 .ts_hi = ts / (1ULL << 32),
442 .ts_lo = ts,
443 .caplen = frame->data_len + sizeof(radiotap_hdr),
444 .pktlen = frame->data_len + sizeof(radiotap_hdr),
445 };
446 static const uint8_t pad[3];
447 uint32_t sz, align;
448
449 sz = blockhdr.caplen + sizeof(blockhdr) + sizeof(uint32_t);
450 blockhdr.blocklen = (sz + 3) & ~3;
451 align = blockhdr.blocklen - sz;
452
453 fwrite(&blockhdr, sizeof(blockhdr), 1, ctx->pcap_file);
454 fwrite(&radiotap_hdr, sizeof(radiotap_hdr), 1, ctx->pcap_file);
455 fwrite(frame->data, frame->data_len, 1, ctx->pcap_file);
456 fwrite(pad, align, 1, ctx->pcap_file);
457 fwrite(&blockhdr.blocklen, sizeof(blockhdr.blocklen), 1, ctx->pcap_file);
458 fflush(ctx->pcap_file);
459 }
460
queue_frame(struct wmediumd * ctx,struct station * station,struct frame * frame)461 static void queue_frame(struct wmediumd *ctx, struct station *station,
462 struct frame *frame)
463 {
464 struct ieee80211_hdr *hdr = (void *)frame->data;
465 u8 *dest = hdr->addr1;
466 uint64_t target;
467 struct wqueue *queue;
468 struct frame *tail;
469 struct station *tmpsta, *deststa;
470 int send_time;
471 int cw;
472 double error_prob;
473 bool is_acked = false;
474 bool noack = false;
475 int i, j;
476 int rate_idx;
477 int ac;
478
479 /* TODO configure phy parameters */
480 int slot_time = 9;
481 int sifs = 16;
482 int difs = 2 * slot_time + sifs;
483
484 int retries = 0;
485
486 int ack_time_usec = pkt_duration(14, index_to_rate(0, frame->freq)) +
487 sifs;
488
489 /*
490 * To determine a frame's expiration time, we compute the
491 * number of retries we might have to make due to radio conditions
492 * or contention, and add backoff time accordingly. To that, we
493 * add the expiration time of the previous frame in the queue.
494 */
495
496 ac = frame_select_queue_80211(frame);
497 queue = &station->queues[ac];
498
499 /* try to "send" this frame at each of the rates in the rateset */
500 send_time = 0;
501 cw = queue->cw_min;
502
503 int snr = SNR_DEFAULT;
504
505 if (is_multicast_ether_addr(dest)) {
506 deststa = NULL;
507 } else {
508 deststa = get_station_by_used_addr(ctx, dest);
509 if (deststa) {
510 snr = ctx->get_link_snr(ctx, station, deststa) -
511 get_signal_offset_by_interference(ctx,
512 station->index, deststa->index);
513 snr += ctx->get_fading_signal(ctx);
514 }
515 }
516 frame->signal = snr + NOISE_LEVEL;
517
518 noack = is_multicast_ether_addr(dest);
519
520 /*
521 * TODO(b/211353765) Remove this when fundamenal solution is applied
522 *
523 * Temporary workaround for relaying probe_req frame.
524 */
525 if (frame_is_probe_req(frame) && frame_has_zero_rates(frame)) {
526 fill_tx_rates(frame);
527 }
528
529 double choice = drand48();
530
531 for (i = 0; i < frame->tx_rates_count && !is_acked; i++) {
532
533 rate_idx = frame->tx_rates[i].idx;
534
535 /* no more rates in MRR */
536 if (rate_idx < 0)
537 break;
538
539 error_prob = ctx->get_error_prob(ctx, snr, rate_idx,
540 frame->freq, frame->data_len,
541 station, deststa);
542 for (j = 0; j < frame->tx_rates[i].count; j++) {
543 send_time += difs + pkt_duration(frame->data_len,
544 index_to_rate(rate_idx, frame->freq));
545
546 retries++;
547
548 /* skip ack/backoff/retries for noack frames */
549 if (noack) {
550 is_acked = true;
551 break;
552 }
553
554 /* TODO TXOPs */
555
556 /* backoff */
557 if (j > 0) {
558 send_time += (cw * slot_time) / 2;
559 cw = (cw << 1) + 1;
560 if (cw > queue->cw_max)
561 cw = queue->cw_max;
562 }
563
564 send_time += ack_time_usec;
565
566 if (choice > error_prob) {
567 is_acked = true;
568 break;
569 }
570
571 if (!use_fixed_random_value(ctx))
572 choice = drand48();
573 }
574 }
575
576 if (is_acked) {
577 frame->tx_rates[i-1].count = j + 1;
578 for (; i < frame->tx_rates_count; i++) {
579 frame->tx_rates[i].idx = -1;
580 frame->tx_rates[i].count = -1;
581 }
582 frame->flags |= HWSIM_TX_STAT_ACK;
583 }
584
585 /*
586 * delivery time starts after any equal or higher prio frame
587 * (or now, if none).
588 */
589 target = scheduler.current_time;
590 for (i = 0; i <= ac; i++) {
591 list_for_each_entry(tmpsta, &ctx->stations, list) {
592 tail = list_last_entry_or_null(&tmpsta->queues[i].frames,
593 struct frame, list);
594 if (tail && target < tail->job.start)
595 target = tail->job.start;
596 }
597 }
598
599 if (ctx->pcap_file) {
600 log2pcap(ctx, frame, target);
601
602 if (is_acked && !noack) {
603 struct {
604 struct frame frame;
605 uint16_t fc;
606 uint16_t dur;
607 uint8_t ra[6];
608 } __attribute__((packed, aligned(8))) ack = {
609 .fc = htole16(0xd4),
610 .dur = htole16(ack_time_usec),
611 };
612
613 memcpy(&ack.frame, frame, sizeof(ack.frame));
614 ack.frame.data_len = 10;
615 memcpy(ack.ra, frame->data + 10, 6);
616
617 log2pcap(ctx, &ack.frame,
618 target + send_time - ack_time_usec);
619 }
620 }
621
622 target += send_time;
623
624 frame->duration = send_time;
625 frame->src = station->client;
626
627 if (ctx->need_start_notify) {
628 frame->start_job.start = target - send_time;
629 frame->start_job.callback = wmediumd_notify_frame_start;
630 frame->start_job.data = ctx;
631 frame->start_job.name = "frame-start";
632 usfstl_sched_add_job(&scheduler, &frame->start_job);
633 }
634
635 frame->job.start = target;
636 frame->job.callback = wmediumd_deliver_frame;
637 frame->job.data = ctx;
638 frame->job.name = "frame";
639 usfstl_sched_add_job(&scheduler, &frame->job);
640 list_add_tail(&frame->list, &queue->frames);
641 }
642
wmediumd_send_to_client(struct wmediumd * ctx,struct client * client,struct nl_msg * msg)643 static void wmediumd_send_to_client(struct wmediumd *ctx,
644 struct client *client,
645 struct nl_msg *msg)
646 {
647 struct wmediumd_message_header hdr;
648 size_t len;
649 int ret;
650
651 switch (client->type) {
652 case CLIENT_NETLINK:
653 ret = nl_send_auto_complete(ctx->sock, msg);
654 if (ret < 0)
655 w_logf(ctx, LOG_ERR, "%s: nl_send_auto failed\n", __func__);
656 break;
657 case CLIENT_VHOST_USER:
658 len = nlmsg_total_size(nlmsg_datalen(nlmsg_hdr(msg)));
659 usfstl_vhost_user_dev_notify(client->dev, HWSIM_VQ_RX,
660 (void *)nlmsg_hdr(msg), len);
661 break;
662 case CLIENT_API_SOCK:
663 len = nlmsg_total_size(nlmsg_datalen(nlmsg_hdr(msg)));
664 hdr.type = WMEDIUMD_MSG_NETLINK;
665 hdr.data_len = len;
666
667 if (write(client->loop.fd, &hdr, sizeof(hdr)) < sizeof(hdr))
668 goto disconnect;
669
670 if (write(client->loop.fd, (void *)nlmsg_hdr(msg), len) < len)
671 goto disconnect;
672
673 wmediumd_wait_for_client_ack(ctx, client);
674 break;
675 }
676
677 return;
678
679 disconnect:
680 usfstl_loop_unregister(&client->loop);
681 wmediumd_remove_client(ctx, client);
682 }
683
wmediumd_remove_client(struct wmediumd * ctx,struct client * client)684 static void wmediumd_remove_client(struct wmediumd *ctx, struct client *client)
685 {
686 struct frame *frame, *tmp;
687 struct wqueue *queue;
688 struct station *station;
689 int ac;
690
691 list_for_each_entry(station, &ctx->stations, list) {
692 if (station->client == client)
693 station->client = NULL;
694 }
695
696 list_for_each_entry(station, &ctx->stations, list) {
697 for (ac = 0; ac < IEEE80211_NUM_ACS; ac++) {
698 queue = &station->queues[ac];
699 list_for_each_entry_safe(frame, tmp, &queue->frames,
700 list) {
701 if (frame->src == client) {
702 list_del(&frame->list);
703 usfstl_sched_del_job(&frame->job);
704 free(frame);
705 }
706 }
707 }
708 }
709
710 if (!list_empty(&client->list))
711 list_del(&client->list);
712 list_add(&client->list, &ctx->clients_to_free);
713
714 if (client->flags & WMEDIUMD_CTL_NOTIFY_TX_START)
715 ctx->need_start_notify--;
716
717 client->wait_for_ack = false;
718 }
719
720 /*
721 * Report transmit status to the kernel.
722 */
send_tx_info_frame_nl(struct wmediumd * ctx,struct frame * frame)723 static void send_tx_info_frame_nl(struct wmediumd *ctx, struct frame *frame)
724 {
725 struct nl_msg *msg;
726
727 msg = nlmsg_alloc();
728 if (!msg) {
729 w_logf(ctx, LOG_ERR, "Error allocating new message MSG!\n");
730 return;
731 }
732
733 if (genlmsg_put(msg, NL_AUTO_PID, NL_AUTO_SEQ, ctx->family_id,
734 0, NLM_F_REQUEST, HWSIM_CMD_TX_INFO_FRAME,
735 VERSION_NR) == NULL) {
736 w_logf(ctx, LOG_ERR, "%s: genlmsg_put failed\n", __func__);
737 goto out;
738 }
739
740 if (nla_put(msg, HWSIM_ATTR_ADDR_TRANSMITTER, ETH_ALEN,
741 frame->sender->hwaddr) ||
742 nla_put_u32(msg, HWSIM_ATTR_FLAGS, frame->flags) ||
743 nla_put_u32(msg, HWSIM_ATTR_SIGNAL, frame->signal) ||
744 nla_put(msg, HWSIM_ATTR_TX_INFO,
745 frame->tx_rates_count * sizeof(struct hwsim_tx_rate),
746 frame->tx_rates) ||
747 nla_put_u64(msg, HWSIM_ATTR_COOKIE, frame->cookie)) {
748 w_logf(ctx, LOG_ERR, "%s: Failed to fill a payload\n", __func__);
749 goto out;
750 }
751
752 if (ctx->ctrl)
753 usfstl_sched_ctrl_sync_to(ctx->ctrl);
754 wmediumd_send_to_client(ctx, frame->src, msg);
755
756 out:
757 nlmsg_free(msg);
758 }
759
760 /*
761 * Send a data frame to the kernel for reception at a specific radio.
762 */
send_cloned_frame_msg(struct wmediumd * ctx,struct client * src,struct station * dst,u8 * data,int data_len,int rate_idx,int signal,int freq,uint64_t cookie)763 static void send_cloned_frame_msg(struct wmediumd *ctx, struct client *src,
764 struct station *dst, u8 *data, int data_len,
765 int rate_idx, int signal, int freq,
766 uint64_t cookie)
767 {
768 struct client *client, *tmp;
769 struct nl_msg *msg, *cmsg = NULL;
770
771 msg = nlmsg_alloc();
772 if (!msg) {
773 w_logf(ctx, LOG_ERR, "Error allocating new message MSG!\n");
774 return;
775 }
776
777 if (genlmsg_put(msg, NL_AUTO_PID, NL_AUTO_SEQ, ctx->family_id,
778 0, NLM_F_REQUEST, HWSIM_CMD_FRAME,
779 VERSION_NR) == NULL) {
780 w_logf(ctx, LOG_ERR, "%s: genlmsg_put failed\n", __func__);
781 goto out;
782 }
783
784 if (nla_put(msg, HWSIM_ATTR_ADDR_RECEIVER, ETH_ALEN,
785 dst->hwaddr) ||
786 nla_put(msg, HWSIM_ATTR_FRAME, data_len, data) ||
787 nla_put_u32(msg, HWSIM_ATTR_RX_RATE, 1) ||
788 nla_put_u32(msg, HWSIM_ATTR_FREQ, freq) ||
789 nla_put_u32(msg, HWSIM_ATTR_SIGNAL, signal)) {
790 w_logf(ctx, LOG_ERR, "%s: Failed to fill a payload\n", __func__);
791 goto out;
792 }
793
794 w_logf(ctx, LOG_DEBUG, "cloned msg dest " MAC_FMT " (radio: " MAC_FMT ") len %d\n",
795 MAC_ARGS(dst->addr), MAC_ARGS(dst->hwaddr), data_len);
796
797 if (ctx->ctrl)
798 usfstl_sched_ctrl_sync_to(ctx->ctrl);
799
800 list_for_each_entry_safe(client, tmp, &ctx->clients, list) {
801 if (client->flags & WMEDIUMD_CTL_RX_ALL_FRAMES) {
802 if (src == client && !cmsg) {
803 struct nlmsghdr *nlh = nlmsg_hdr(msg);
804
805 cmsg = nlmsg_inherit(nlh);
806 nlmsg_append(cmsg, nlmsg_data(nlh), nlmsg_datalen(nlh), 0);
807 assert(nla_put_u64(cmsg, HWSIM_ATTR_COOKIE, cookie) == 0);
808 }
809 wmediumd_send_to_client(ctx, client,
810 src == client ? cmsg : msg);
811 } else if (!dst->client || dst->client == client) {
812 wmediumd_send_to_client(ctx, client, msg);
813 }
814 }
815
816 out:
817 nlmsg_free(msg);
818 if (cmsg)
819 nlmsg_free(cmsg);
820 }
821
wmediumd_deliver_frame(struct usfstl_job * job)822 static void wmediumd_deliver_frame(struct usfstl_job *job)
823 {
824 struct wmediumd *ctx = job->data;
825 struct frame *frame = container_of(job, struct frame, job);
826 struct ieee80211_hdr *hdr = (void *) frame->data;
827 struct station *station;
828 u8 *dest = hdr->addr1;
829 u8 *src = frame->sender->addr;
830
831 list_del(&frame->list);
832
833 if (frame->flags & HWSIM_TX_STAT_ACK) {
834 /* rx the frame on the dest interface */
835 list_for_each_entry(station, &ctx->stations, list) {
836 if (memcmp(src, station->addr, ETH_ALEN) == 0)
837 continue;
838
839 if (is_multicast_ether_addr(dest) && station->client != NULL) {
840 int snr, rate_idx, signal;
841 double error_prob;
842
843 /*
844 * we may or may not receive this based on
845 * reverse link from sender -- check for
846 * each receiver.
847 */
848 snr = ctx->get_link_snr(ctx, frame->sender,
849 station);
850 snr += ctx->get_fading_signal(ctx);
851 signal = snr + NOISE_LEVEL;
852 if (signal < CCA_THRESHOLD)
853 continue;
854
855 if (set_interference_duration(ctx,
856 frame->sender->index, frame->duration,
857 signal))
858 continue;
859
860 snr -= get_signal_offset_by_interference(ctx,
861 frame->sender->index, station->index);
862 rate_idx = frame->tx_rates[0].idx;
863 error_prob = ctx->get_error_prob(ctx,
864 (double)snr, rate_idx, frame->freq,
865 frame->data_len, frame->sender,
866 station);
867
868 if (drand48() <= error_prob) {
869 w_logf(ctx, LOG_INFO, "Dropped mcast from "
870 MAC_FMT " to " MAC_FMT " at receiver\n",
871 MAC_ARGS(src), MAC_ARGS(station->addr));
872 continue;
873 }
874
875 send_cloned_frame_msg(ctx, frame->sender->client,
876 station,
877 frame->data,
878 frame->data_len,
879 1, signal,
880 frame->freq,
881 frame->cookie);
882
883 } else if (station_has_addr(station, dest)) {
884 if (set_interference_duration(ctx,
885 frame->sender->index, frame->duration,
886 frame->signal))
887 continue;
888
889 send_cloned_frame_msg(ctx, frame->sender->client,
890 station,
891 frame->data,
892 frame->data_len,
893 1, frame->signal,
894 frame->freq,
895 frame->cookie);
896 }
897 }
898 } else
899 set_interference_duration(ctx, frame->sender->index,
900 frame->duration, frame->signal);
901
902 send_tx_info_frame_nl(ctx, frame);
903
904 free(frame);
905 }
906
wmediumd_intf_update(struct usfstl_job * job)907 static void wmediumd_intf_update(struct usfstl_job *job)
908 {
909 struct wmediumd *ctx = job->data;
910 int i, j;
911
912 for (i = 0; i < ctx->num_stas; i++)
913 for (j = 0; j < ctx->num_stas; j++) {
914 if (i == j)
915 continue;
916 // probability is used for next calc
917 ctx->intf[i * ctx->num_stas + j].prob_col =
918 ctx->intf[i * ctx->num_stas + j].duration /
919 (double)10000;
920 ctx->intf[i * ctx->num_stas + j].duration = 0;
921 }
922
923 job->start += 10000;
924 usfstl_sched_add_job(&scheduler, job);
925 }
926
927 static
nl_err_cb(struct sockaddr_nl * nla,struct nlmsgerr * nlerr,void * arg)928 int nl_err_cb(struct sockaddr_nl *nla, struct nlmsgerr *nlerr, void *arg)
929 {
930 struct genlmsghdr *gnlh = nlmsg_data(&nlerr->msg);
931 struct wmediumd *ctx = arg;
932
933 w_flogf(ctx, LOG_ERR, stderr, "nl: cmd %d, seq %d: %s\n", gnlh->cmd,
934 nlerr->msg.nlmsg_seq, strerror(abs(nlerr->error)));
935
936 return NL_SKIP;
937 }
938
send_pmsr_result_ftm(struct nl_msg * msg,struct pmsr_request_ftm * req,struct wmediumd * ctx,struct station * sender,struct station * receiver)939 static int send_pmsr_result_ftm(struct nl_msg *msg,
940 struct pmsr_request_ftm *req,
941 struct wmediumd *ctx,
942 struct station *sender,
943 struct station *receiver)
944 {
945 struct nlattr *ftm;
946 int err;
947
948 ftm = nla_nest_start(msg, NL80211_PMSR_TYPE_FTM);
949 if (!ftm)
950 return -ENOMEM;
951
952 if (!receiver) {
953 err = nla_put_u32(msg, NL80211_PMSR_FTM_RESP_ATTR_FAIL_REASON,
954 NL80211_PMSR_FTM_FAILURE_NO_RESPONSE);
955 goto out;
956 }
957
958 double distance =
959 sqrt(pow(sender->x - receiver->x, 2) + pow(sender->y - receiver->y, 2));
960 double distance_in_mm = meter_to_mm(distance);
961 double rtt_in_ps = sec_to_ps(distance_to_rtt(distance));
962 if (distance_in_mm > UINT64_MAX || rtt_in_ps > UINT64_MAX) {
963 w_logf(ctx, LOG_WARNING,
964 "%s: Devices are too far away", __func__);
965 return nla_put_u32(msg, NL80211_PMSR_FTM_RESP_ATTR_FAIL_REASON,
966 NL80211_PMSR_FTM_FAILURE_NO_RESPONSE);
967 }
968
969 int rssi = receiver->tx_power -
970 ctx->calc_path_loss(NULL, sender, receiver);
971
972 if (nla_put_u32(msg, NL80211_PMSR_FTM_RESP_ATTR_NUM_FTMR_ATTEMPTS,
973 req->ftmr_retries) ||
974 nla_put_u32(msg, NL80211_PMSR_FTM_RESP_ATTR_NUM_FTMR_SUCCESSES,
975 req->ftmr_retries) ||
976 nla_put_u8(msg, NL80211_PMSR_FTM_RESP_ATTR_NUM_BURSTS_EXP,
977 req->num_bursts_exp) ||
978 nla_put_u8(msg, NL80211_PMSR_FTM_RESP_ATTR_BURST_DURATION,
979 req->burst_duration) ||
980 nla_put_u8(msg, NL80211_PMSR_FTM_RESP_ATTR_FTMS_PER_BURST,
981 req->ftms_per_burst) ||
982 nla_put_s32(msg, NL80211_PMSR_FTM_RESP_ATTR_RSSI_AVG, rssi) ||
983 nla_put_s32(msg, NL80211_PMSR_FTM_RESP_ATTR_RSSI_SPREAD, 0) ||
984 nla_put_u64(msg, NL80211_PMSR_FTM_RESP_ATTR_RTT_AVG,
985 (uint64_t)rtt_in_ps) ||
986 nla_put_u64(msg, NL80211_PMSR_FTM_RESP_ATTR_RTT_VARIANCE, 0) ||
987 nla_put_u64(msg, NL80211_PMSR_FTM_RESP_ATTR_RTT_SPREAD, 0) ||
988 nla_put_u64(msg, NL80211_PMSR_FTM_RESP_ATTR_DIST_AVG,
989 (uint64_t)distance_in_mm) ||
990 nla_put_u64(msg, NL80211_PMSR_FTM_RESP_ATTR_DIST_VARIANCE, 0) ||
991 nla_put_u64(msg, NL80211_PMSR_FTM_RESP_ATTR_DIST_SPREAD, 0)) {
992 w_logf(ctx, LOG_ERR, "%s: Failed to fill a payload\n", __func__);
993 return -ENOMEM;
994 }
995
996 if (req->request_lci && receiver->lci) {
997 nla_put_string(msg, NL80211_PMSR_FTM_RESP_ATTR_LCI,
998 receiver->lci);
999 }
1000
1001 if (req->request_civicloc && receiver->civicloc) {
1002 nla_put_string(msg, NL80211_PMSR_FTM_RESP_ATTR_CIVICLOC,
1003 receiver->civicloc);
1004 }
1005
1006 out:
1007 if (ftm)
1008 nla_nest_end(msg, ftm);
1009
1010 return 0;
1011 }
1012
send_pmsr_result_peer(struct nl_msg * msg,struct pmsr_request_peer * req,struct wmediumd * ctx,struct station * sender)1013 static int send_pmsr_result_peer(struct nl_msg *msg,
1014 struct pmsr_request_peer *req,
1015 struct wmediumd *ctx,
1016 struct station *sender)
1017 {
1018 struct nlattr *peer, *resp, *data;
1019 struct station *receiver;
1020 int status;
1021 struct timespec ts;
1022 u64 host_time_ns;
1023 u64 ap_tsf_us;
1024 int err;
1025
1026 peer = nla_nest_start(msg, 1);
1027 if (!peer)
1028 return -ENOMEM;
1029
1030 receiver = get_station_by_addr(ctx, req->addr);
1031 if (receiver)
1032 status = NL80211_PMSR_STATUS_SUCCESS;
1033 else {
1034 w_logf(ctx, LOG_WARNING, "%s: unknown pmsr target " MAC_FMT "\n",
1035 __func__, MAC_ARGS(req->addr));
1036 status = NL80211_PMSR_STATUS_FAILURE;
1037 }
1038
1039 if (clock_gettime(CLOCK_BOOTTIME, &ts)) {
1040 w_logf(ctx, LOG_ERR, "%s: clock_gettime() failed\n", __func__);
1041 return -EINVAL;
1042 }
1043
1044 host_time_ns = ts_to_ns(ts);
1045 ap_tsf_us = ns_to_us(ts_to_ns(ts));
1046
1047 err = nla_put(msg, NL80211_PMSR_PEER_ATTR_ADDR, ETH_ALEN, req->addr);
1048 if (err)
1049 return err;
1050
1051 resp = nla_nest_start(msg, NL80211_PMSR_PEER_ATTR_RESP);
1052 if (!resp)
1053 return -ENOMEM;
1054
1055 if (nla_put_u32(msg, NL80211_PMSR_RESP_ATTR_STATUS, status) ||
1056 nla_put_u64(msg, NL80211_PMSR_RESP_ATTR_HOST_TIME, host_time_ns) ||
1057 nla_put_u64(msg, NL80211_PMSR_RESP_ATTR_AP_TSF, ap_tsf_us) ||
1058 nla_put_flag(msg, NL80211_PMSR_RESP_ATTR_FINAL)) {
1059 w_logf(ctx, LOG_ERR, "%s: Failed to fill a payload\n", __func__);
1060 return -ENOMEM;
1061 }
1062
1063 data = nla_nest_start(msg, NL80211_PMSR_RESP_ATTR_DATA);
1064 if (!data)
1065 return -ENOMEM;
1066
1067 err = send_pmsr_result_ftm(msg, &req->ftm, ctx, sender, receiver);
1068
1069 nla_nest_end(msg, data);
1070 nla_nest_end(msg, resp);
1071 nla_nest_end(msg, peer);
1072
1073 return err;
1074 }
1075
send_pmsr_result(struct pmsr_request * request,struct wmediumd * ctx,struct station * sender,struct client * client)1076 static void send_pmsr_result(struct pmsr_request* request, struct wmediumd *ctx,
1077 struct station *sender, struct client *client)
1078 {
1079 struct nl_msg *msg;
1080 struct nlattr *pmsr, *peers;
1081 struct pmsr_request_peer *peer;
1082 int cnt;
1083 int err;
1084
1085 msg = nlmsg_alloc();
1086 if (!msg) {
1087 w_logf(ctx, LOG_ERR, "%s: nlmsg_alloc failed\n", __func__);
1088 return;
1089 }
1090
1091 if (!genlmsg_put(msg, NL_AUTO_PID, NL_AUTO_SEQ, ctx->family_id,
1092 0, NLM_F_REQUEST, HWSIM_CMD_REPORT_PMSR, VERSION_NR)) {
1093 w_logf(ctx, LOG_ERR, "%s: genlmsg_put failed\n", __func__);
1094 goto out;
1095 }
1096
1097 err = nla_put(msg, HWSIM_ATTR_ADDR_TRANSMITTER,
1098 ETH_ALEN, sender->hwaddr);
1099
1100 pmsr = nla_nest_start(msg, HWSIM_ATTR_PMSR_RESULT);
1101 if (!pmsr)
1102 goto out;
1103
1104 peers = nla_nest_start(msg, NL80211_PMSR_ATTR_PEERS);
1105 if (!peers)
1106 goto out;
1107
1108 list_for_each_entry(peer, &request->peers, list) {
1109 err = send_pmsr_result_peer(msg, peer, ctx, sender);
1110 if (err) {
1111 w_logf(ctx, LOG_ERR,
1112 "%s: Failed to send pmsr result from " MAC_FMT \
1113 " to " MAC_FMT ". Stopping\n", __func__,
1114 MAC_ARGS(sender->addr),
1115 MAC_ARGS(peer->addr));
1116 break;
1117 }
1118 }
1119
1120 nla_nest_end(msg, peers);
1121 nla_nest_end(msg, pmsr);
1122
1123 wmediumd_send_to_client(ctx, client, msg);
1124
1125 out:
1126 nlmsg_free(msg);
1127 }
1128
process_start_pmsr(struct nlattr * attrs[],struct wmediumd * ctx,struct client * client)1129 static void process_start_pmsr(struct nlattr *attrs[], struct wmediumd *ctx,
1130 struct client *client)
1131 {
1132 u8 *hwaddr;
1133 struct station *sender;
1134
1135 struct pmsr_request request;
1136 struct pmsr_request_peer *peer, *tmp;
1137 int err;
1138
1139 if (!attrs[HWSIM_ATTR_ADDR_TRANSMITTER]) {
1140 w_logf(ctx, LOG_ERR, "%s: Missing sender information\n",
1141 __func__);
1142 }
1143
1144 hwaddr = (u8 *)nla_data(attrs[HWSIM_ATTR_ADDR_TRANSMITTER]);
1145 sender = get_station_by_addr(ctx, hwaddr);
1146 if (!sender) {
1147 w_logf(ctx, LOG_ERR, "%s: Unknown sender " MAC_FMT "\n",
1148 __func__, MAC_ARGS(hwaddr));
1149 return;
1150 }
1151
1152 err = parse_pmsr_request(attrs[HWSIM_ATTR_PMSR_REQUEST], &request);
1153 if (err)
1154 goto out;
1155
1156 send_pmsr_result(&request, ctx, sender, client);
1157
1158 out:
1159 list_for_each_entry_safe(peer, tmp, &request.peers, list) {
1160 list_del(&peer->list);
1161 free(peer);
1162 }
1163 }
1164
1165 /*
1166 * Handle events from the kernel. Process CMD_FRAME events and queue them
1167 * for later delivery with the scheduler.
1168 */
_process_messages(struct nl_msg * msg,struct wmediumd * ctx,struct client * client)1169 static void _process_messages(struct nl_msg *msg,
1170 struct wmediumd *ctx,
1171 struct client *client)
1172 {
1173 struct nlattr *attrs[HWSIM_ATTR_MAX+1];
1174 /* netlink header */
1175 struct nlmsghdr *nlh = nlmsg_hdr(msg);
1176 /* generic netlink header*/
1177 struct genlmsghdr *gnlh = nlmsg_data(nlh);
1178
1179 struct station *sender;
1180 struct frame *frame;
1181 struct ieee80211_hdr *hdr;
1182 u8 *src, *hwaddr, *addr;
1183 void *new_addrs;
1184 unsigned int i;
1185
1186 genlmsg_parse(nlh, 0, attrs, HWSIM_ATTR_MAX, NULL);
1187
1188 switch (gnlh->cmd) {
1189 case HWSIM_CMD_FRAME:
1190 if (attrs[HWSIM_ATTR_ADDR_TRANSMITTER]) {
1191 hwaddr = (u8 *)nla_data(attrs[HWSIM_ATTR_ADDR_TRANSMITTER]);
1192
1193 unsigned int data_len =
1194 nla_len(attrs[HWSIM_ATTR_FRAME]);
1195 char *data = (char *)nla_data(attrs[HWSIM_ATTR_FRAME]);
1196 unsigned int flags =
1197 nla_get_u32(attrs[HWSIM_ATTR_FLAGS]);
1198 unsigned int tx_rates_len =
1199 nla_len(attrs[HWSIM_ATTR_TX_INFO]);
1200 struct hwsim_tx_rate *tx_rates =
1201 (struct hwsim_tx_rate *)
1202 nla_data(attrs[HWSIM_ATTR_TX_INFO]);
1203 u64 cookie = nla_get_u64(attrs[HWSIM_ATTR_COOKIE]);
1204 u32 freq;
1205
1206 freq = attrs[HWSIM_ATTR_FREQ] ?
1207 nla_get_u32(attrs[HWSIM_ATTR_FREQ]) : 2412;
1208
1209 hdr = (struct ieee80211_hdr *)data;
1210 src = hdr->addr2;
1211
1212 if (data_len < 6 + 6 + 4)
1213 return;
1214
1215 sender = get_station_by_addr(ctx, hwaddr);
1216 if (!sender) {
1217 sender = get_station_by_used_addr(ctx, src);
1218 if (!sender) {
1219 w_flogf(ctx, LOG_ERR, stderr,
1220 "Unable to find sender station by src=" MAC_FMT " nor hwaddr=" MAC_FMT "\n",
1221 MAC_ARGS(src), MAC_ARGS(hwaddr));
1222 return;
1223 }
1224 memcpy(sender->hwaddr, hwaddr, ETH_ALEN);
1225 }
1226
1227 if (!sender->client)
1228 sender->client = client;
1229
1230 frame = calloc(1, sizeof(*frame) + data_len);
1231 if (!frame)
1232 return;
1233
1234 memcpy(frame->data, data, data_len);
1235 frame->data_len = data_len;
1236 frame->flags = flags;
1237 frame->cookie = cookie;
1238 frame->freq = freq;
1239 frame->sender = sender;
1240 frame->tx_rates_count =
1241 tx_rates_len / sizeof(struct hwsim_tx_rate);
1242 memcpy(frame->tx_rates, tx_rates,
1243 min(tx_rates_len, sizeof(frame->tx_rates)));
1244 queue_frame(ctx, sender, frame);
1245 }
1246 break;
1247 case HWSIM_CMD_ADD_MAC_ADDR:
1248 if (!attrs[HWSIM_ATTR_ADDR_TRANSMITTER] ||
1249 !attrs[HWSIM_ATTR_ADDR_RECEIVER])
1250 break;
1251 hwaddr = (u8 *)nla_data(attrs[HWSIM_ATTR_ADDR_TRANSMITTER]);
1252 addr = (u8 *)nla_data(attrs[HWSIM_ATTR_ADDR_RECEIVER]);
1253 sender = get_station_by_addr(ctx, hwaddr);
1254 if (!sender)
1255 break;
1256 for (i = 0; i < sender->n_addrs; i++) {
1257 if (memcmp(sender->addrs[i].addr, addr, ETH_ALEN) == 0) {
1258 sender->addrs[i].count += 1;
1259 return;
1260 }
1261 }
1262 new_addrs = realloc(sender->addrs, sizeof(struct addr) * (sender->n_addrs + 1));
1263 if (!new_addrs)
1264 break;
1265 sender->addrs = new_addrs;
1266 sender->addrs[sender->n_addrs].count = 1;
1267 memcpy(sender->addrs[sender->n_addrs].addr, addr, ETH_ALEN);
1268 sender->n_addrs += 1;
1269 break;
1270 case HWSIM_CMD_DEL_MAC_ADDR:
1271 if (!attrs[HWSIM_ATTR_ADDR_TRANSMITTER] ||
1272 !attrs[HWSIM_ATTR_ADDR_RECEIVER])
1273 break;
1274 hwaddr = (u8 *)nla_data(attrs[HWSIM_ATTR_ADDR_TRANSMITTER]);
1275 addr = (u8 *)nla_data(attrs[HWSIM_ATTR_ADDR_RECEIVER]);
1276 sender = get_station_by_addr(ctx, hwaddr);
1277 if (!sender)
1278 break;
1279 for (i = 0; i < sender->n_addrs; i++) {
1280 if (memcmp(sender->addrs[i].addr, addr, ETH_ALEN))
1281 continue;
1282 sender->addrs[i].count -= 1;
1283 if (sender->addrs[i].count <= 0) {
1284 sender->n_addrs -= 1;
1285 memmove(&sender->addrs[i],
1286 &sender->addrs[sender->n_addrs],
1287 sizeof(struct addr));
1288 }
1289 break;
1290 }
1291 break;
1292 case HWSIM_CMD_START_PMSR:
1293 process_start_pmsr(attrs, ctx, client);
1294 break;
1295
1296 case HWSIM_CMD_ABORT_PMSR:
1297 // Do nothing. Too late to abort any PMSR.
1298 break;
1299 }
1300 }
1301
process_messages_cb(struct nl_msg * msg,void * arg)1302 static int process_messages_cb(struct nl_msg *msg, void *arg)
1303 {
1304 struct wmediumd *ctx = arg;
1305
1306 _process_messages(msg, ctx, &ctx->nl_client);
1307 return 0;
1308 }
1309
wmediumd_vu_connected(struct usfstl_vhost_user_dev * dev)1310 static void wmediumd_vu_connected(struct usfstl_vhost_user_dev *dev)
1311 {
1312 struct wmediumd *ctx = dev->server->data;
1313 struct client *client;
1314
1315 client = calloc(1, sizeof(*client));
1316 dev->data = client;
1317 client->type = CLIENT_VHOST_USER;
1318 client->dev = dev;
1319 list_add(&client->list, &ctx->clients);
1320 }
1321
wmediumd_vu_handle(struct usfstl_vhost_user_dev * dev,struct usfstl_vhost_user_buf * buf,unsigned int vring)1322 static void wmediumd_vu_handle(struct usfstl_vhost_user_dev *dev,
1323 struct usfstl_vhost_user_buf *buf,
1324 unsigned int vring)
1325 {
1326 struct nl_msg *nlmsg;
1327 char data[4096];
1328 size_t len;
1329
1330 len = iov_read(data, sizeof(data), buf->out_sg, buf->n_out_sg);
1331
1332 if (!nlmsg_ok((const struct nlmsghdr *)data, len))
1333 return;
1334 nlmsg = nlmsg_convert((struct nlmsghdr *)data);
1335 if (!nlmsg)
1336 return;
1337
1338 _process_messages(nlmsg, dev->server->data, dev->data);
1339
1340 nlmsg_free(nlmsg);
1341 }
1342
wmediumd_vu_disconnected(struct usfstl_vhost_user_dev * dev)1343 static void wmediumd_vu_disconnected(struct usfstl_vhost_user_dev *dev)
1344 {
1345 struct client *client = dev->data;
1346
1347 dev->data = NULL;
1348 wmediumd_remove_client(dev->server->data, client);
1349 }
1350
process_set_snr_message(struct wmediumd * ctx,struct wmediumd_set_snr * set_snr)1351 static int process_set_snr_message(struct wmediumd *ctx, struct wmediumd_set_snr *set_snr) {
1352 struct station *node1 = get_station_by_addr(ctx, set_snr->node1_mac);
1353 struct station *node2 = get_station_by_addr(ctx, set_snr->node2_mac);
1354
1355 if (node1 == NULL || node2 == NULL) {
1356 return -1;
1357 }
1358
1359 ctx->snr_matrix[ctx->num_stas * node2->index + node1->index] = set_snr->snr;
1360 ctx->snr_matrix[ctx->num_stas * node1->index + node2->index] = set_snr->snr;
1361
1362 return 0;
1363 }
1364
process_load_config_message(struct wmediumd * ctx,struct wmediumd_load_config * reload_config)1365 static int process_load_config_message(struct wmediumd *ctx,
1366 struct wmediumd_load_config *reload_config) {
1367 char *config_path;
1368 int result = 0;
1369
1370 config_path = reload_config->config_path;
1371
1372 if (validate_config(config_path)) {
1373 clear_config(ctx);
1374 load_config(ctx, config_path, NULL);
1375 } else {
1376 result = -1;
1377 }
1378
1379 return result;
1380 }
1381
process_reload_current_config_message(struct wmediumd * ctx)1382 static int process_reload_current_config_message(struct wmediumd *ctx) {
1383 char *config_path;
1384 int result = 0;
1385
1386 config_path = strdup(ctx->config_path);
1387
1388 if (validate_config(config_path)) {
1389 clear_config(ctx);
1390 load_config(ctx, config_path, NULL);
1391 } else {
1392 result = -1;
1393 }
1394
1395 free(config_path);
1396
1397 return result;
1398 }
1399
process_get_stations_message(struct wmediumd * ctx,ssize_t * response_len,unsigned char ** response_data)1400 static int process_get_stations_message(struct wmediumd *ctx, ssize_t *response_len, unsigned char **response_data) {
1401 struct station *station;
1402 int station_count = 0;
1403 int extra_data_len = 0;
1404
1405 // *reponse_data contains struct wmediumd_station_infos
1406 // and then lci and civiclocs for each station follows afterwards.
1407 list_for_each_entry(station, &ctx->stations, list) {
1408 if (station->client != NULL) {
1409 ++station_count;
1410 extra_data_len += strlen_safe(station->lci) + 1;
1411 extra_data_len += strlen_safe(station->civicloc) + 1;
1412 }
1413 }
1414
1415 int station_len = sizeof(uint32_t) + sizeof(struct wmediumd_station_info) * station_count;
1416 *response_len = station_len + extra_data_len;
1417 *response_data = malloc(*response_len);
1418
1419 if (*response_data == NULL) {
1420 w_logf(ctx, LOG_ERR, "%s: failed allocate response data\n", __func__);
1421 return -1;
1422 }
1423
1424 struct wmediumd_station_infos *station_infos = (struct wmediumd_station_infos *)*response_data;
1425 station_infos->count = station_count;
1426 int station_index = 0;
1427 // Pointer to the memory after structs wmediumd_station_infos
1428 // to write lci and civicloc for each station.
1429 char *extra_data_cursor = (char *)&(station_infos->stations[station_count]);
1430
1431 list_for_each_entry(station, &ctx->stations, list) {
1432 if (station->client != NULL) {
1433 struct wmediumd_station_info *station_info = &station_infos->stations[station_index];
1434 memcpy(station_info->addr, station->addr, ETH_ALEN);
1435 memcpy(station_info->hwaddr, station->hwaddr, ETH_ALEN);
1436
1437 station_info->x = station->x;
1438 station_info->y = station->y;
1439 station_info->lci_offset = extra_data_cursor - (char *)station_info;
1440 extra_data_cursor = stpcpy_safe(extra_data_cursor, station->lci) + 1;
1441 station_info->civicloc_offset = extra_data_cursor - (char *)station_info;
1442 extra_data_cursor = stpcpy_safe(extra_data_cursor, station->civicloc) + 1;
1443 station_info->tx_power = station->tx_power;
1444 station_index++;
1445 }
1446 }
1447
1448 return 0;
1449 }
1450
process_set_position_message(struct wmediumd * ctx,struct wmediumd_set_position * set_position)1451 static int process_set_position_message(struct wmediumd *ctx, struct wmediumd_set_position *set_position) {
1452 struct station *node = get_station_by_addr(ctx, set_position->mac);
1453
1454 if (node == NULL) {
1455 return -1;
1456 }
1457
1458 node->x = set_position->x;
1459 node->y = set_position->y;
1460
1461 calc_path_loss(ctx);
1462
1463 return 0;
1464 }
1465
process_set_tx_power_message(struct wmediumd * ctx,struct wmediumd_set_tx_power * set_tx_power)1466 static int process_set_tx_power_message(struct wmediumd *ctx, struct wmediumd_set_tx_power *set_tx_power) {
1467 struct station *node = get_station_by_addr(ctx, set_tx_power->mac);
1468
1469 if (node == NULL) {
1470 return -1;
1471 }
1472
1473 node->tx_power = set_tx_power->tx_power;
1474
1475 calc_path_loss(ctx);
1476
1477 return 0;
1478 }
1479
process_set_lci_message(struct wmediumd * ctx,struct wmediumd_set_lci * set_lci,size_t data_len)1480 static int process_set_lci_message(struct wmediumd *ctx, struct wmediumd_set_lci *set_lci, size_t data_len) {
1481 struct station *node = get_station_by_addr(ctx, set_lci->mac);
1482
1483 if (node == NULL) {
1484 return -1;
1485 }
1486 int expected_len = data_len - offsetof(struct wmediumd_set_lci, lci) - 1;
1487 if (set_lci->lci[expected_len] != '\0') {
1488 return -1;
1489 }
1490
1491 if (node->lci) {
1492 free(node->lci);
1493 }
1494 node->lci = strdup(set_lci->lci);
1495
1496 return node->lci == NULL ? -1 : 0;
1497 }
1498
process_set_civicloc_message(struct wmediumd * ctx,struct wmediumd_set_civicloc * set_civicloc,size_t data_len)1499 static int process_set_civicloc_message(struct wmediumd *ctx, struct wmediumd_set_civicloc *set_civicloc, size_t data_len) {
1500 struct station *node = get_station_by_addr(ctx, set_civicloc->mac);
1501
1502 if (node == NULL) {
1503 return -1;
1504 }
1505 int expected_len = data_len - offsetof(struct wmediumd_set_civicloc, civicloc) - 1;
1506 if (set_civicloc->civicloc[expected_len] != '\0') {
1507 return -1;
1508 }
1509
1510 if (node->civicloc) {
1511 free(node->civicloc);
1512 }
1513 node->civicloc = strdup(set_civicloc->civicloc);
1514
1515 return node->civicloc == NULL ? -1 : 0;
1516 }
1517
1518 static const struct usfstl_vhost_user_ops wmediumd_vu_ops = {
1519 .connected = wmediumd_vu_connected,
1520 .handle = wmediumd_vu_handle,
1521 .disconnected = wmediumd_vu_disconnected,
1522 };
1523
close_pcapng(struct wmediumd * ctx)1524 static void close_pcapng(struct wmediumd *ctx) {
1525 if (ctx->pcap_file == NULL) {
1526 return;
1527 }
1528
1529 fflush(ctx->pcap_file);
1530 fclose(ctx->pcap_file);
1531
1532 ctx->pcap_file = NULL;
1533 }
1534
1535 static void init_pcapng(struct wmediumd *ctx, const char *filename);
1536
wmediumd_send_grpc_response(int msq_id,long msg_type_response,enum wmediumd_grpc_response_data_type data_type,ssize_t data_size,unsigned char * data_payload)1537 static void wmediumd_send_grpc_response(int msq_id, long msg_type_response, enum wmediumd_grpc_response_data_type data_type, ssize_t data_size, unsigned char *data_payload) {
1538 struct wmediumd_grpc_response_message response_message;
1539 response_message.msg_type_response = msg_type_response;
1540 response_message.data_type = data_type;
1541 response_message.data_size = data_size;
1542 if (data_size > 0) {
1543 memcpy(response_message.data_payload, data_payload, data_size);
1544 }
1545 msgsnd(msq_id, &response_message, MSG_TYPE_RESPONSE_SIZE, 0);
1546 }
1547
wmediumd_grpc_service_handler(struct usfstl_loop_entry * entry)1548 static void wmediumd_grpc_service_handler(struct usfstl_loop_entry *entry) {
1549 struct wmediumd *ctx = entry->data;
1550 ssize_t msg_len;
1551
1552 // Receive event from wmediumd_server
1553 uint64_t evt;
1554 read(entry->fd, &evt, sizeof(evt));
1555
1556 struct wmediumd_grpc_request_message request_message;
1557 msg_len = msgrcv(ctx->msq_id, &request_message, MSG_TYPE_REQUEST_SIZE, MSG_TYPE_REQUEST, IPC_NOWAIT);
1558 if (msg_len != MSG_TYPE_REQUEST_SIZE) {
1559 w_logf(ctx, LOG_ERR, "%s: failed to get request message\n", __func__);
1560 } else {
1561 switch (request_message.data_type) {
1562 case REQUEST_LIST_STATIONS: {
1563 ssize_t size = 0;
1564 unsigned char *payload = NULL;
1565 if (process_get_stations_message(ctx, &size, &payload) < 0) {
1566 w_logf(ctx, LOG_ERR, "%s: failed to execute list_stations\n", __func__);
1567 wmediumd_send_grpc_response(ctx->msq_id, request_message.msg_type_response, RESPONSE_INVALID, 0, NULL);
1568 } else {
1569 wmediumd_send_grpc_response(ctx->msq_id, request_message.msg_type_response, RESPONSE_ACK_LIST_STATIONS, size, payload);
1570 }
1571 break;
1572 }
1573 case REQUEST_LOAD_CONFIG:
1574 if (process_load_config_message(ctx, (struct wmediumd_load_config *)(request_message.data_payload)) < 0) {
1575 w_logf(ctx, LOG_ERR, "%s: failed to execute load_config\n", __func__);
1576 wmediumd_send_grpc_response(ctx->msq_id, request_message.msg_type_response, RESPONSE_INVALID, 0, NULL);
1577 break;
1578 }
1579 wmediumd_send_grpc_response(ctx->msq_id, request_message.msg_type_response, RESPONSE_ACK, 0, NULL);
1580 break;
1581 case REQUEST_RELOAD_CONFIG:
1582 if (process_reload_current_config_message(ctx) < 0) {
1583 w_logf(ctx, LOG_ERR, "%s: failed to execute reload_config\n", __func__);
1584 wmediumd_send_grpc_response(ctx->msq_id, request_message.msg_type_response, RESPONSE_INVALID, 0, NULL);
1585 break;
1586 }
1587 wmediumd_send_grpc_response(ctx->msq_id, request_message.msg_type_response, RESPONSE_ACK, 0, NULL);
1588 break;
1589 case REQUEST_SET_CIVICLOC:
1590 if (process_set_civicloc_message(ctx, (struct wmediumd_set_civicloc *)(request_message.data_payload), request_message.data_size) < 0) {
1591 w_logf(ctx, LOG_ERR, "%s: failed to execute set_civicloc\n", __func__);
1592 wmediumd_send_grpc_response(ctx->msq_id, request_message.msg_type_response, RESPONSE_INVALID, 0, NULL);
1593 break;
1594 }
1595 wmediumd_send_grpc_response(ctx->msq_id, request_message.msg_type_response, RESPONSE_ACK, 0, NULL);
1596 break;
1597 case REQUEST_SET_LCI:
1598 if (process_set_lci_message(ctx, (struct wmediumd_set_lci *)(request_message.data_payload), request_message.data_size) < 0) {
1599 w_logf(ctx, LOG_ERR, "%s: failed to execute set_lci\n", __func__);
1600 wmediumd_send_grpc_response(ctx->msq_id, request_message.msg_type_response, RESPONSE_INVALID, 0, NULL);
1601 break;
1602 }
1603 wmediumd_send_grpc_response(ctx->msq_id, request_message.msg_type_response, RESPONSE_ACK, 0, NULL);
1604 break;
1605 case REQUEST_SET_POSITION:
1606 if (process_set_position_message(ctx, (struct wmediumd_set_position *)(request_message.data_payload)) < 0) {
1607 w_logf(ctx, LOG_ERR, "%s: failed to execute set_position\n", __func__);
1608 wmediumd_send_grpc_response(ctx->msq_id, request_message.msg_type_response, RESPONSE_INVALID, 0, NULL);
1609 break;
1610 }
1611 wmediumd_send_grpc_response(ctx->msq_id, request_message.msg_type_response, RESPONSE_ACK, 0, NULL);
1612 break;
1613 case REQUEST_SET_SNR:
1614 if (process_set_snr_message(ctx, (struct wmediumd_set_snr *)(request_message.data_payload)) < 0) {
1615 w_logf(ctx, LOG_ERR, "%s: failed to execute set_snr\n", __func__);
1616 wmediumd_send_grpc_response(ctx->msq_id, request_message.msg_type_response, RESPONSE_INVALID, 0, NULL);
1617 break;
1618 }
1619 wmediumd_send_grpc_response(ctx->msq_id, request_message.msg_type_response, RESPONSE_ACK, 0, NULL);
1620 break;
1621 case REQUEST_SET_TX_POWER:
1622 if (process_set_tx_power_message(ctx, (struct wmediumd_set_tx_power *)(request_message.data_payload)) < 0) {
1623 w_logf(ctx, LOG_ERR, "%s: failed to execute set_tx_power\n", __func__);
1624 wmediumd_send_grpc_response(ctx->msq_id, request_message.msg_type_response, RESPONSE_INVALID, 0, NULL);
1625 break;
1626 }
1627 wmediumd_send_grpc_response(ctx->msq_id, request_message.msg_type_response, RESPONSE_ACK, 0, NULL);
1628 break;
1629 case REQUEST_START_PCAP:
1630 init_pcapng(ctx, ((struct wmediumd_start_pcap *)(request_message.data_payload))->pcap_path);
1631 wmediumd_send_grpc_response(ctx->msq_id, request_message.msg_type_response, RESPONSE_ACK, 0, NULL);
1632 break;
1633 case REQUEST_STOP_PCAP:
1634 close_pcapng(ctx);
1635 wmediumd_send_grpc_response(ctx->msq_id, request_message.msg_type_response, RESPONSE_ACK, 0, NULL);
1636 break;
1637 default:
1638 w_logf(ctx, LOG_ERR, "%s: unknown request type\n", __func__);
1639 wmediumd_send_grpc_response(ctx->msq_id, request_message.msg_type_response, RESPONSE_INVALID, 0, NULL);
1640 break;
1641 }
1642 }
1643 return;
1644 }
1645
1646 // TODO(273384914): Deprecate messages used in wmediumd_control after
1647 // implementing in wmediumd_grpc_service_handler to be used in the command
1648 // 'cvd env'.
wmediumd_api_handler(struct usfstl_loop_entry * entry)1649 static void wmediumd_api_handler(struct usfstl_loop_entry *entry)
1650 {
1651 struct client *client = container_of(entry, struct client, loop);
1652 struct wmediumd *ctx = entry->data;
1653 struct wmediumd_message_header hdr;
1654 enum wmediumd_message response = WMEDIUMD_MSG_ACK;
1655 struct wmediumd_message_control control = {};
1656 struct nl_msg *nlmsg;
1657 unsigned char *data;
1658 ssize_t response_len = 0;
1659 unsigned char *response_data = NULL;
1660 ssize_t len;
1661
1662 len = read(entry->fd, &hdr, sizeof(hdr));
1663 if (len != sizeof(hdr)) {
1664 if (len > 0) {
1665 // Skipping log if the fd is closed.
1666 w_logf(ctx, LOG_ERR, "%s: failed to read header\n", __func__);
1667 }
1668 goto disconnect;
1669 }
1670
1671 /* safety valve */
1672 if (hdr.data_len > 1024 * 1024) {
1673 w_logf(ctx, LOG_ERR, "%s: too large data\n", __func__);
1674 goto disconnect;
1675 }
1676
1677 data = malloc(hdr.data_len);
1678 if (!data) {
1679 w_logf(ctx, LOG_ERR, "%s: failed to malloc\n", __func__);
1680 goto disconnect;
1681 }
1682
1683 len = read(entry->fd, data, hdr.data_len);
1684 if (len != hdr.data_len) {
1685 w_logf(ctx, LOG_ERR, "%s: failed to read data\n", __func__);
1686 goto disconnect;
1687 }
1688
1689 switch (hdr.type) {
1690 case WMEDIUMD_MSG_REGISTER:
1691 if (!list_empty(&client->list)) {
1692 response = WMEDIUMD_MSG_INVALID;
1693 break;
1694 }
1695 list_add(&client->list, &ctx->clients);
1696 break;
1697 case WMEDIUMD_MSG_UNREGISTER:
1698 if (list_empty(&client->list)) {
1699 response = WMEDIUMD_MSG_INVALID;
1700 break;
1701 }
1702 list_del_init(&client->list);
1703 break;
1704 case WMEDIUMD_MSG_NETLINK:
1705 if (ctx->ctrl)
1706 usfstl_sched_ctrl_sync_from(ctx->ctrl);
1707
1708 if (!nlmsg_ok((const struct nlmsghdr *)data, len)) {
1709 response = WMEDIUMD_MSG_INVALID;
1710 break;
1711 }
1712
1713 nlmsg = nlmsg_convert((struct nlmsghdr *)data);
1714 if (!nlmsg)
1715 break;
1716
1717 _process_messages(nlmsg, ctx, client);
1718
1719 nlmsg_free(nlmsg);
1720 break;
1721 case WMEDIUMD_MSG_SET_CONTROL:
1722 /* copy what we get and understand, leave the rest zeroed */
1723 memcpy(&control, data,
1724 min(sizeof(control), hdr.data_len));
1725
1726 if (client->flags & WMEDIUMD_CTL_NOTIFY_TX_START)
1727 ctx->need_start_notify--;
1728 if (control.flags & WMEDIUMD_CTL_NOTIFY_TX_START)
1729 ctx->need_start_notify++;
1730
1731 client->flags = control.flags;
1732 break;
1733 case WMEDIUMD_MSG_GET_STATIONS:
1734 if (process_get_stations_message(ctx, &response_len, &response_data) < 0) {
1735 response = WMEDIUMD_MSG_INVALID;
1736 }
1737 response = WMEDIUMD_MSG_STATIONS_LIST;
1738 break;
1739 case WMEDIUMD_MSG_SET_SNR:
1740 if (process_set_snr_message(ctx, (struct wmediumd_set_snr *)data) < 0) {
1741 response = WMEDIUMD_MSG_INVALID;
1742 }
1743 break;
1744 case WMEDIUMD_MSG_RELOAD_CONFIG:
1745 if (process_load_config_message(ctx,
1746 (struct wmediumd_load_config *)data) < 0) {
1747 response = WMEDIUMD_MSG_INVALID;
1748 }
1749 break;
1750 case WMEDIUMD_MSG_RELOAD_CURRENT_CONFIG:
1751 if (process_reload_current_config_message(ctx) < 0) {
1752 response = WMEDIUMD_MSG_INVALID;
1753 }
1754 break;
1755 case WMEDIUMD_MSG_START_PCAP:
1756 init_pcapng(ctx, ((struct wmediumd_start_pcap *)data)->pcap_path);
1757 break;
1758 case WMEDIUMD_MSG_STOP_PCAP:
1759 close_pcapng(ctx);
1760 break;
1761 case WMEDIUMD_MSG_SET_POSITION:
1762 if (process_set_position_message(ctx, (struct wmediumd_set_position *)data) < 0) {
1763 response = WMEDIUMD_MSG_INVALID;
1764 }
1765 break;
1766 case WMEDIUMD_MSG_SET_LCI:
1767 if (process_set_lci_message(ctx,
1768 (struct wmediumd_set_lci *)data,
1769 hdr.data_len) < 0) {
1770 response = WMEDIUMD_MSG_INVALID;
1771 }
1772 break;
1773 case WMEDIUMD_MSG_SET_CIVICLOC:
1774 if (process_set_civicloc_message(ctx,
1775 (struct wmediumd_set_civicloc *)data,
1776 hdr.data_len) < 0) {
1777 response = WMEDIUMD_MSG_INVALID;
1778 }
1779 break;
1780 case WMEDIUMD_MSG_ACK:
1781 assert(client->wait_for_ack == true);
1782 assert(hdr.data_len == 0);
1783 client->wait_for_ack = false;
1784 /* don't send a response to a response, of course */
1785 return;
1786 default:
1787 w_logf(ctx, LOG_ERR, "%s: unknown message\n", __func__);
1788 response = WMEDIUMD_MSG_INVALID;
1789 break;
1790 }
1791
1792 /* return a response */
1793 hdr.type = response;
1794 hdr.data_len = response_len;
1795 len = write(entry->fd, &hdr, sizeof(hdr));
1796 if (len != sizeof(hdr)) {
1797 w_logf(ctx, LOG_ERR, "%s: failed to write response header\n", __func__);
1798 goto disconnect;
1799 }
1800
1801 if (response_data != NULL) {
1802 if (response_len != 0) {
1803 len = write(entry->fd, response_data, response_len);
1804
1805 if (len != response_len) {
1806 free(response_data);
1807 w_logf(ctx, LOG_ERR, "%s: failed to write response data\n", __func__);
1808 goto disconnect;
1809 }
1810 }
1811
1812 free(response_data);
1813 response_data = NULL;
1814 }
1815
1816 return;
1817 disconnect:
1818 usfstl_loop_unregister(&client->loop);
1819 wmediumd_remove_client(ctx, client);
1820 }
1821
wmediumd_api_connected(int fd,void * data)1822 static void wmediumd_api_connected(int fd, void *data)
1823 {
1824 struct wmediumd *ctx = data;
1825 struct client *client;
1826
1827 client = calloc(1, sizeof(*client));
1828 client->type = CLIENT_API_SOCK;
1829 client->loop.fd = fd;
1830 client->loop.data = ctx;
1831 client->loop.handler = wmediumd_api_handler;
1832 usfstl_loop_register(&client->loop);
1833 INIT_LIST_HEAD(&client->list);
1834 }
1835
1836 /*
1837 * Register with the kernel to start receiving new frames.
1838 */
send_register_msg(struct wmediumd * ctx)1839 static int send_register_msg(struct wmediumd *ctx)
1840 {
1841 struct nl_sock *sock = ctx->sock;
1842 struct nl_msg *msg;
1843 int ret;
1844
1845 msg = nlmsg_alloc();
1846 if (!msg) {
1847 w_logf(ctx, LOG_ERR, "Error allocating new message MSG!\n");
1848 return -1;
1849 }
1850
1851 if (genlmsg_put(msg, NL_AUTO_PID, NL_AUTO_SEQ, ctx->family_id,
1852 0, NLM_F_REQUEST, HWSIM_CMD_REGISTER,
1853 VERSION_NR) == NULL) {
1854 w_logf(ctx, LOG_ERR, "%s: genlmsg_put failed\n", __func__);
1855 ret = -1;
1856 goto out;
1857 }
1858
1859 ret = nl_send_auto_complete(sock, msg);
1860 if (ret < 0) {
1861 w_logf(ctx, LOG_ERR, "%s: nl_send_auto failed\n", __func__);
1862 ret = -1;
1863 goto out;
1864 }
1865 ret = 0;
1866
1867 out:
1868 nlmsg_free(msg);
1869 return ret;
1870 }
1871
sock_event_cb(struct usfstl_loop_entry * entry)1872 static void sock_event_cb(struct usfstl_loop_entry *entry)
1873 {
1874 struct wmediumd *ctx = entry->data;
1875
1876 nl_recvmsgs_default(ctx->sock);
1877 }
1878
1879 /*
1880 * Setup netlink socket and callbacks.
1881 */
init_netlink(struct wmediumd * ctx)1882 static int init_netlink(struct wmediumd *ctx)
1883 {
1884 struct nl_sock *sock;
1885 int ret;
1886
1887 ctx->cb = nl_cb_alloc(NL_CB_CUSTOM);
1888 if (!ctx->cb) {
1889 w_logf(ctx, LOG_ERR, "Error allocating netlink callbacks\n");
1890 return -1;
1891 }
1892
1893 sock = nl_socket_alloc_cb(ctx->cb);
1894 if (!sock) {
1895 w_logf(ctx, LOG_ERR, "Error allocating netlink socket\n");
1896 return -1;
1897 }
1898
1899 ctx->sock = sock;
1900
1901 ret = genl_connect(sock);
1902 if (ret < 0) {
1903 w_logf(ctx, LOG_ERR, "Error connecting netlink socket ret=%d\n", ret);
1904 return -1;
1905 }
1906
1907 ctx->family_id = genl_ctrl_resolve(sock, "MAC80211_HWSIM");
1908 if (ctx->family_id < 0) {
1909 w_logf(ctx, LOG_ERR, "Family MAC80211_HWSIM not registered\n");
1910 return -1;
1911 }
1912
1913 nl_cb_set(ctx->cb, NL_CB_MSG_IN, NL_CB_CUSTOM, process_messages_cb, ctx);
1914 nl_cb_err(ctx->cb, NL_CB_CUSTOM, nl_err_cb, ctx);
1915
1916 return 0;
1917 }
1918
1919 /*
1920 * Print the CLI help
1921 */
print_help(int exval)1922 static void print_help(int exval)
1923 {
1924 printf("wmediumd v%s - a wireless medium simulator\n", VERSION_STR);
1925 printf("wmediumd [-h] [-V] [-l LOG_LVL] [-x FILE] -c FILE \n\n");
1926
1927 printf(" -h print this help and exit\n");
1928 printf(" -V print version and exit\n\n");
1929
1930 printf(" -l LOG_LVL set the logging level\n");
1931 printf(" LOG_LVL: RFC 5424 severity, values 0 - 7\n");
1932 printf(" >= 3: errors are logged\n");
1933 printf(" >= 5: startup msgs are logged\n");
1934 printf(" >= 6: dropped packets are logged (default)\n");
1935 printf(" == 7: all packets will be logged\n");
1936 printf(" -c FILE set input config file\n");
1937 printf(" -x FILE set input PER file\n");
1938 printf(" -t socket set the time control socket\n");
1939 printf(" -u socket expose vhost-user socket, don't use netlink\n");
1940 printf(" -a socket expose wmediumd API socket\n");
1941 printf(" -n force netlink use even with vhost-user\n");
1942 printf(" -p FILE log packets to pcapng file FILE\n");
1943
1944 exit(exval);
1945 }
1946
init_pcapng(struct wmediumd * ctx,const char * filename)1947 static void init_pcapng(struct wmediumd *ctx, const char *filename)
1948 {
1949 struct {
1950 uint32_t type, blocklen, byte_order;
1951 uint16_t ver_maj, ver_min;
1952 uint64_t seclen;
1953 uint32_t blocklen2;
1954 } __attribute__((packed)) blockhdr = {
1955 .type = 0x0A0D0D0A,
1956 .blocklen = sizeof(blockhdr),
1957 .byte_order = 0x1A2B3C4D,
1958 .ver_maj = 1,
1959 .ver_min = 0,
1960 .seclen = -1,
1961 .blocklen2 = sizeof(blockhdr),
1962 };
1963 struct {
1964 uint32_t type, blocklen;
1965 uint16_t linktype, reserved;
1966 uint32_t snaplen;
1967 struct {
1968 uint16_t code, len;
1969 uint8_t val, pad[3];
1970 } opt_if_tsresol;
1971 struct {
1972 uint16_t code, len;
1973 } opt_endofopt;
1974 uint32_t blocklen2;
1975 } __attribute__((packed)) idb = {
1976 .type = 1,
1977 .blocklen = sizeof(idb),
1978 .linktype = 127, // radiotap
1979 .snaplen = -1,
1980 .opt_if_tsresol.code = 9,
1981 .opt_if_tsresol.len = 1,
1982 .opt_if_tsresol.val = 6, // usec
1983 .blocklen2 = sizeof(idb),
1984 };
1985
1986 if (ctx->pcap_file != NULL) {
1987 close_pcapng(ctx);
1988 }
1989
1990 if (!filename)
1991 return;
1992
1993 ctx->pcap_file = fopen(filename, "w+");
1994 fwrite(&blockhdr, sizeof(blockhdr), 1, ctx->pcap_file);
1995 fwrite(&idb, sizeof(idb), 1, ctx->pcap_file);
1996 }
1997
1998 #ifndef VIRTIO_F_VERSION_1
1999 #define VIRTIO_F_VERSION_1 32
2000 #endif
2001
wmediumd_main(int argc,char * argv[],int event_fd,int msq_id)2002 int wmediumd_main(int argc, char *argv[], int event_fd, int msq_id)
2003 {
2004 int opt;
2005 struct wmediumd ctx = {};
2006 char *config_file = NULL;
2007 char *per_file = NULL;
2008 const char *time_socket = NULL, *api_socket = NULL;
2009 struct usfstl_sched_ctrl ctrl = {};
2010 struct usfstl_vhost_user_server vusrv = {
2011 .ops = &wmediumd_vu_ops,
2012 .max_queues = HWSIM_NUM_VQS,
2013 .input_queues = 1 << HWSIM_VQ_TX,
2014 .features = 1ULL << VIRTIO_F_VERSION_1,
2015 .protocol_features =
2016 1ULL << VHOST_USER_PROTOCOL_F_INBAND_NOTIFICATIONS,
2017 .data = &ctx,
2018 };
2019 bool use_netlink, force_netlink = false;
2020
2021 setvbuf(stdout, NULL, _IOLBF, BUFSIZ);
2022
2023 if (argc == 1) {
2024 fprintf(stderr, "This program needs arguments....\n\n");
2025 print_help(EXIT_FAILURE);
2026 }
2027
2028 ctx.log_lvl = 6;
2029 unsigned long int parse_log_lvl;
2030 char* parse_end_token;
2031
2032 while ((opt = getopt(argc, argv, "hVc:l:x:t:u:a:np:")) != -1) {
2033 switch (opt) {
2034 case 'h':
2035 print_help(EXIT_SUCCESS);
2036 break;
2037 case 'V':
2038 printf("wmediumd v%s - a wireless medium simulator "
2039 "for mac80211_hwsim\n", VERSION_STR);
2040 exit(EXIT_SUCCESS);
2041 break;
2042 case 'c':
2043 config_file = optarg;
2044 break;
2045 case 'x':
2046 printf("Input packet error rate file: %s\n", optarg);
2047 per_file = optarg;
2048 break;
2049 case ':':
2050 printf("wmediumd: Error - Option `%c' "
2051 "needs a value\n\n", optopt);
2052 print_help(EXIT_FAILURE);
2053 break;
2054 case 'l':
2055 parse_log_lvl = strtoul(optarg, &parse_end_token, 10);
2056 if ((parse_log_lvl == ULONG_MAX && errno == ERANGE) ||
2057 optarg == parse_end_token || parse_log_lvl > 7) {
2058 printf("wmediumd: Error - Invalid RFC 5424 severity level: "
2059 "%s\n\n", optarg);
2060 print_help(EXIT_FAILURE);
2061 }
2062 ctx.log_lvl = parse_log_lvl;
2063 break;
2064 case 't':
2065 time_socket = optarg;
2066 break;
2067 case 'u':
2068 vusrv.socket = optarg;
2069 break;
2070 case 'a':
2071 api_socket = optarg;
2072 break;
2073 case 'n':
2074 force_netlink = true;
2075 break;
2076 case 'p':
2077 init_pcapng(&ctx, optarg);
2078 break;
2079 case '?':
2080 printf("wmediumd: Error - No such option: "
2081 "`%c'\n\n", optopt);
2082 print_help(EXIT_FAILURE);
2083 break;
2084 }
2085
2086 }
2087
2088 if (optind < argc)
2089 print_help(EXIT_FAILURE);
2090
2091 if (!config_file) {
2092 printf("%s: config file must be supplied\n", argv[0]);
2093 print_help(EXIT_FAILURE);
2094 }
2095
2096 w_logf(&ctx, LOG_NOTICE, "Input configuration file: %s\n", config_file);
2097
2098 INIT_LIST_HEAD(&ctx.stations);
2099 INIT_LIST_HEAD(&ctx.clients);
2100 INIT_LIST_HEAD(&ctx.clients_to_free);
2101
2102 if (load_config(&ctx, config_file, per_file))
2103 return EXIT_FAILURE;
2104
2105 use_netlink = force_netlink || !vusrv.socket;
2106
2107 /* init netlink */
2108 if (use_netlink && init_netlink(&ctx) < 0)
2109 return EXIT_FAILURE;
2110
2111 if (ctx.intf) {
2112 ctx.intf_job.start = 10000; // usec
2113 ctx.intf_job.name = "interference update";
2114 ctx.intf_job.data = &ctx;
2115 ctx.intf_job.callback = wmediumd_intf_update;
2116 usfstl_sched_add_job(&scheduler, &ctx.intf_job);
2117 }
2118
2119 if (vusrv.socket)
2120 usfstl_vhost_user_server_start(&vusrv);
2121
2122 if (use_netlink) {
2123 ctx.nl_client.type = CLIENT_NETLINK;
2124 list_add(&ctx.nl_client.list, &ctx.clients);
2125
2126 ctx.nl_loop.handler = sock_event_cb;
2127 ctx.nl_loop.data = &ctx;
2128 ctx.nl_loop.fd = nl_socket_get_fd(ctx.sock);
2129 usfstl_loop_register(&ctx.nl_loop);
2130
2131 /* register for new frames */
2132 if (send_register_msg(&ctx) == 0)
2133 w_logf(&ctx, LOG_NOTICE, "REGISTER SENT!\n");
2134 }
2135
2136 if (api_socket) {
2137 signal(SIGPIPE, SIG_IGN);
2138 usfstl_uds_create(api_socket, wmediumd_api_connected, &ctx);
2139 }
2140
2141 if (time_socket) {
2142 usfstl_sched_ctrl_start(&ctrl, time_socket,
2143 1000 /* nsec per usec */,
2144 (uint64_t)-1 /* no ID */,
2145 &scheduler);
2146 vusrv.scheduler = &scheduler;
2147 vusrv.ctrl = &ctrl;
2148 ctx.ctrl = &ctrl;
2149 } else {
2150 usfstl_sched_wallclock_init(&scheduler, 1000);
2151 }
2152
2153 // Control event_fd to communicate WmediumdService.
2154 ctx.grpc_loop.handler = wmediumd_grpc_service_handler;
2155 ctx.grpc_loop.data = &ctx;
2156 ctx.grpc_loop.fd = event_fd;
2157 usfstl_loop_register(&ctx.grpc_loop);
2158 ctx.msq_id = msq_id;
2159
2160 while (1) {
2161 if (time_socket) {
2162 usfstl_sched_next(&scheduler);
2163 } else {
2164 usfstl_sched_wallclock_wait_and_handle(&scheduler);
2165
2166 if (usfstl_sched_next_pending(&scheduler, NULL))
2167 usfstl_sched_next(&scheduler);
2168 }
2169
2170 while (!list_empty(&ctx.clients_to_free)) {
2171 struct client *client;
2172
2173 client = list_first_entry(&ctx.clients_to_free,
2174 struct client, list);
2175
2176 list_del(&client->list);
2177 free(client);
2178 }
2179 }
2180
2181 free(ctx.sock);
2182 free(ctx.cb);
2183 free(ctx.intf);
2184 free(ctx.per_matrix);
2185
2186 return EXIT_SUCCESS;
2187 }
2188