1 /* dpauxmon.c
2 * dpauxmon is an extcap tool used to monitor DisplayPort AUX channel traffic
3 * coming in from the kernel via generic netlink
4 * Copyright 2018, Dirk Eibach, Guntermann & Drunck GmbH <dirk.eibach@gdsys.cc>
5 *
6 * Wireshark - Network traffic analyzer
7 * By Gerald Combs <gerald@wireshark.org>
8 * Copyright 1998 Gerald Combs
9 *
10 * SPDX-License-Identifier: GPL-2.0-or-later
11 */
12
13 #include "config.h"
14 #define WS_LOG_DOMAIN "dpauxmon"
15
16 #include "extcap-base.h"
17
18 #include <wsutil/strtoi.h>
19 #include <wsutil/filesystem.h>
20 #include <wsutil/netlink.h>
21 #include <wsutil/privileges.h>
22 #include <wsutil/wslog.h>
23 #include <writecap/pcapio.h>
24
25 #include <netlink/netlink.h>
26 #include <netlink/genl/genl.h>
27 #include <netlink/genl/ctrl.h>
28 #include <netlink/genl/mngt.h>
29
30 #include <signal.h>
31 #include <errno.h>
32
33 #include <linux/genetlink.h>
34
35 #include "dpauxmon_user.h"
36
37 #define PCAP_SNAPLEN 128
38
39 #define DPAUXMON_EXTCAP_INTERFACE "dpauxmon"
40 #define DPAUXMON_VERSION_MAJOR "0"
41 #define DPAUXMON_VERSION_MINOR "1"
42 #define DPAUXMON_VERSION_RELEASE "0"
43
44 static gboolean run_loop = TRUE;
45 FILE* pcap_fp = NULL;
46
47 enum {
48 EXTCAP_BASE_OPTIONS_ENUM,
49 OPT_HELP,
50 OPT_VERSION,
51 OPT_INTERFACE_ID,
52 };
53
54 static struct ws_option longopts[] = {
55 EXTCAP_BASE_OPTIONS,
56 /* Generic application options */
57 { "help", ws_no_argument, NULL, OPT_HELP},
58 { "version", ws_no_argument, NULL, OPT_VERSION},
59 /* Interfaces options */
60 { "interface_id", ws_required_argument, NULL, OPT_INTERFACE_ID},
61 { 0, 0, 0, 0 }
62 };
63
64 static struct nla_policy dpauxmon_attr_policy[DPAUXMON_ATTR_MAX + 1] = {
65 [DPAUXMON_ATTR_IFINDEX] = { .type = NLA_U32 },
66 [DPAUXMON_ATTR_FROM_SOURCE] = { .type = NLA_FLAG },
67 [DPAUXMON_ATTR_TIMESTAMP] = { .type = NLA_MSECS },
68 };
69
70 struct family_handler_args {
71 const char *group;
72 int id;
73 };
74
list_config(char * interface)75 static int list_config(char *interface)
76 {
77 unsigned inc = 0;
78
79 if (!interface) {
80 ws_warning("No interface specified.");
81 return EXIT_FAILURE;
82 }
83
84 if (g_strcmp0(interface, DPAUXMON_EXTCAP_INTERFACE)) {
85 ws_warning("interface must be %s", DPAUXMON_EXTCAP_INTERFACE);
86 return EXIT_FAILURE;
87 }
88
89 printf("arg {number=%u}{call=--interface_id}{display=Interface index}"
90 "{type=unsigned}{range=1,65535}{default=%u}{tooltip=The dpauxmon interface index}\n",
91 inc++, 0);
92
93 extcap_config_debug(&inc);
94
95 return EXIT_SUCCESS;
96 }
97
exit_from_loop(int signo _U_)98 static void exit_from_loop(int signo _U_)
99 {
100 run_loop = FALSE;
101 }
102
setup_dumpfile(const char * fifo,FILE ** fp)103 static int setup_dumpfile(const char* fifo, FILE** fp)
104 {
105 guint64 bytes_written = 0;
106 int err;
107
108 if (!g_strcmp0(fifo, "-")) {
109 *fp = stdout;
110 return EXIT_SUCCESS;
111 }
112
113 *fp = fopen(fifo, "wb");
114 if (!(*fp)) {
115 ws_warning("Error creating output file: %s", g_strerror(errno));
116 return EXIT_FAILURE;
117 }
118
119 if (!libpcap_write_file_header(*fp, 275, PCAP_SNAPLEN, FALSE, &bytes_written, &err)) {
120 ws_warning("Can't write pcap file header");
121 return EXIT_FAILURE;
122 }
123
124 return EXIT_SUCCESS;
125 }
126
dump_packet(FILE * fp,const char * buf,const guint32 buflen,guint64 ts_usecs)127 static int dump_packet(FILE* fp, const char* buf, const guint32 buflen, guint64 ts_usecs)
128 {
129 guint64 bytes_written = 0;
130 int err;
131 int ret = EXIT_SUCCESS;
132
133 if (!libpcap_write_packet(fp, ts_usecs / 1000000, ts_usecs % 1000000, buflen, buflen, buf, &bytes_written, &err)) {
134 ws_warning("Can't write packet");
135 ret = EXIT_FAILURE;
136 }
137
138 fflush(fp);
139
140 return ret;
141 }
142
error_handler(struct sockaddr_nl * nla _U_,struct nlmsgerr * err,void * arg)143 static int error_handler(struct sockaddr_nl *nla _U_, struct nlmsgerr *err,
144 void *arg)
145 {
146 int *ret = (int*)arg;
147 *ret = err->error;
148 return NL_STOP;
149 }
150
ack_handler(struct nl_msg * msg _U_,void * arg)151 static int ack_handler(struct nl_msg *msg _U_, void *arg)
152 {
153 int *ret = (int*)arg;
154 *ret = 0;
155 return NL_STOP;
156 }
157
family_handler(struct nl_msg * msg,void * arg)158 static int family_handler(struct nl_msg *msg, void *arg)
159 {
160 struct family_handler_args *grp = (struct family_handler_args *)arg;
161 struct nlattr *tb[CTRL_ATTR_MAX + 1];
162 struct genlmsghdr *gnlh = (struct genlmsghdr *)nlmsg_data(nlmsg_hdr(msg));
163 struct nlattr *mcgrp;
164 int rem_mcgrp;
165
166 nla_parse(tb, CTRL_ATTR_MAX, genlmsg_attrdata(gnlh, 0),
167 genlmsg_attrlen(gnlh, 0), NULL);
168
169 if (!tb[CTRL_ATTR_MCAST_GROUPS])
170 return NL_SKIP;
171
172 nla_for_each_nested(mcgrp, tb[CTRL_ATTR_MCAST_GROUPS], rem_mcgrp) {
173 struct nlattr *tb_mcgrp[CTRL_ATTR_MCAST_GRP_MAX + 1];
174
175 nla_parse(tb_mcgrp, CTRL_ATTR_MCAST_GRP_MAX,
176 (struct nlattr *)nla_data(mcgrp), nla_len(mcgrp), NULL);
177
178 if (!tb_mcgrp[CTRL_ATTR_MCAST_GRP_NAME] ||
179 !tb_mcgrp[CTRL_ATTR_MCAST_GRP_ID])
180 continue;
181
182 if (strncmp((const char*)nla_data(tb_mcgrp[CTRL_ATTR_MCAST_GRP_NAME]),
183 grp->group,
184 nla_len(tb_mcgrp[CTRL_ATTR_MCAST_GRP_NAME])))
185 continue;
186
187 grp->id = nla_get_u32(tb_mcgrp[CTRL_ATTR_MCAST_GRP_ID]);
188
189 break;
190 }
191
192 return NL_SKIP;
193 }
194
nl_get_multicast_id(struct nl_sock * sock,int family,const char * group)195 static int nl_get_multicast_id(struct nl_sock *sock, int family,
196 const char *group)
197 {
198 struct nl_msg *msg;
199 struct nl_cb *cb;
200 int ret, ctrlid;
201 struct family_handler_args grp = {
202 .group = group,
203 .id = -ENOENT,
204 };
205
206 msg = nlmsg_alloc();
207 if (!msg)
208 return -ENOMEM;
209
210 cb = nl_cb_alloc(NL_CB_DEFAULT);
211 if (!cb) {
212 ret = -ENOMEM;
213 goto out_fail_cb;
214 }
215
216 ctrlid = genl_ctrl_resolve(sock, "nlctrl");
217
218 genlmsg_put(msg, 0, 0, ctrlid, 0, 0, CTRL_CMD_GETFAMILY, 0);
219
220 ret = -ENOBUFS;
221 NLA_PUT_U16(msg, CTRL_ATTR_FAMILY_ID, family);
222
223 ret = nl_send_auto_complete(sock, msg);
224 if (ret < 0)
225 goto nla_put_failure;
226
227 ret = 1;
228
229 nl_cb_err(cb, NL_CB_CUSTOM, error_handler, &ret);
230 nl_cb_set(cb, NL_CB_ACK, NL_CB_CUSTOM, ack_handler, &ret);
231 nl_cb_set(cb, NL_CB_VALID, NL_CB_CUSTOM, family_handler, &grp);
232
233 while (ret > 0)
234 nl_recvmsgs(sock, cb);
235
236 if (ret == 0)
237 ret = grp.id;
238 nla_put_failure:
239 nl_cb_put(cb);
240 out_fail_cb:
241 nlmsg_free(msg);
242 return ret;
243 }
244
245 /*
246 * netlink callback handlers
247 */
248
nl_receive_timeout(struct nl_sock * sk,struct sockaddr_nl * nla,unsigned char ** buf,struct ucred ** creds)249 static int nl_receive_timeout(struct nl_sock* sk, struct sockaddr_nl* nla, unsigned char** buf, struct ucred** creds)
250 {
251 struct pollfd fds = {nl_socket_get_fd(sk), POLLIN, 0};
252 int poll_res = poll(&fds, 1, 500);
253
254 if (poll_res < 0) {
255 ws_debug("poll() failed in nl_receive_timeout");
256 g_usleep(500000);
257 return -nl_syserr2nlerr(errno);
258 }
259
260 return poll_res ? nl_recv(sk, nla, buf, creds) : 0;
261 }
262
send_start(struct nl_sock * sock,int family,unsigned int interface_id)263 static int send_start(struct nl_sock *sock, int family, unsigned int interface_id)
264 {
265 struct nl_msg *msg;
266 void *hdr;
267 int err;
268 int res = 0;
269
270 msg = nlmsg_alloc();
271 if (msg == NULL) {
272 ws_critical("Unable to allocate netlink message");
273 return -ENOMEM;
274 }
275
276 hdr = genlmsg_put(msg, NL_AUTO_PID, NL_AUTO_SEQ, family, 0, 0,
277 DPAUXMON_CMD_START, 1);
278 if (hdr == NULL) {
279 ws_critical("Unable to write genl header");
280 res = -ENOMEM;
281 goto out_free;
282 }
283
284 if ((err = nla_put_u32(msg, DPAUXMON_ATTR_IFINDEX, interface_id)) < 0) {
285 ws_critical("Unable to add attribute: %s", nl_geterror(err));
286 res = -EIO;
287 goto out_free;
288 }
289
290 if ((err = nl_send_auto_complete(sock, msg)) < 0)
291 ws_debug("Starting monitor failed, already running? :%s", nl_geterror(err));
292
293 out_free:
294 nlmsg_free(msg);
295 return res;
296 }
297
send_stop(struct nl_sock * sock,int family,unsigned int interface_id)298 static void send_stop(struct nl_sock *sock, int family, unsigned int interface_id)
299 {
300 struct nl_msg *msg;
301 void *hdr;
302 int err;
303
304 msg = nlmsg_alloc();
305 if (msg == NULL) {
306 ws_critical("Unable to allocate netlink message");
307 return;
308 }
309
310 hdr = genlmsg_put(msg, NL_AUTO_PID, NL_AUTO_SEQ, family, 0, 0,
311 DPAUXMON_CMD_STOP, 1);
312 if (hdr == NULL) {
313 ws_critical("Unable to write genl header");
314 goto out_free;
315 }
316
317 if ((err = nla_put_u32(msg, DPAUXMON_ATTR_IFINDEX, interface_id)) < 0) {
318 ws_critical("Unable to add attribute: %s", nl_geterror(err));
319 goto out_free;
320 }
321
322 if ((err = nl_send_auto_complete(sock, msg)) < 0) {
323 ws_critical("Unable to send message: %s", nl_geterror(err));
324 goto out_free;
325 }
326
327 out_free:
328 nlmsg_free(msg);
329 }
330
handle_data(struct nl_cache_ops * unused _U_,struct genl_cmd * cmd _U_,struct genl_info * info,void * arg _U_)331 static int handle_data(struct nl_cache_ops *unused _U_, struct genl_cmd *cmd _U_,
332 struct genl_info *info, void *arg _U_)
333 {
334 unsigned char *data;
335 guint32 data_size;
336 guint64 ts = 0;
337 guint8 packet[21] = { 0x00 };
338
339 if (!info->attrs[DPAUXMON_ATTR_DATA])
340 return NL_SKIP;
341
342 data = (unsigned char*)nla_data(info->attrs[DPAUXMON_ATTR_DATA]);
343 data_size = nla_len(info->attrs[DPAUXMON_ATTR_DATA]);
344
345 if (data_size > 19) {
346 ws_debug("Invalid packet size %u", data_size);
347 return NL_SKIP;
348 }
349
350 if (info->attrs[DPAUXMON_ATTR_TIMESTAMP])
351 ts = nla_get_msecs(info->attrs[DPAUXMON_ATTR_TIMESTAMP]);
352
353 packet[1] = info->attrs[DPAUXMON_ATTR_FROM_SOURCE] ? 0x01 : 0x00;
354
355 memcpy(&packet[2], data, data_size);
356
357 if (dump_packet(pcap_fp, packet, data_size + 2, ts) == EXIT_FAILURE)
358 run_loop = FALSE;
359
360 return NL_OK;
361 }
362
parse_cb(struct nl_msg * msg,void * arg _U_)363 static int parse_cb(struct nl_msg *msg, void *arg _U_)
364 {
365 return genl_handle_msg(msg, NULL);
366 }
367
368 static struct genl_cmd cmds[] = {
369 #if 0
370 {
371 .c_id = DPAUXMON_CMD_START,
372 .c_name = "dpauxmon start",
373 .c_maxattr = DPAUXMON_ATTR_MAX,
374 .c_attr_policy = dpauxmon_attr_policy,
375 .c_msg_parser = &handle_start,
376 },
377 {
378 .c_id = DPAUXMON_CMD_STOP,
379 .c_name = "dpauxmon stop",
380 .c_maxattr = DPAUXMON_ATTR_MAX,
381 .c_attr_policy = dpauxmon_attr_policy,
382 .c_msg_parser = &handle_stop,
383 },
384 #endif
385 {
386 .c_id = DPAUXMON_CMD_DATA,
387 .c_name = "dpauxmon data",
388 .c_maxattr = DPAUXMON_ATTR_MAX,
389 .c_attr_policy = dpauxmon_attr_policy,
390 .c_msg_parser = &handle_data,
391 },
392 };
393
394 #define ARRAY_SIZE(X) (sizeof(X) / sizeof((X)[0]))
395
396 static struct genl_ops ops = {
397 .o_name = "dpauxmon",
398 .o_cmds = cmds,
399 .o_ncmds = ARRAY_SIZE(cmds),
400 };
401
402 struct nl_sock *sock;
403
run_listener(const char * fifo,unsigned int interface_id)404 static void run_listener(const char* fifo, unsigned int interface_id)
405 {
406 int err;
407 int grp;
408 struct sigaction int_handler = { .sa_handler = exit_from_loop };
409 struct nl_cb *socket_cb;
410
411 if (sigaction(SIGINT, &int_handler, 0)) {
412 ws_warning("Can't set signal handler");
413 return;
414 }
415
416 if (setup_dumpfile(fifo, &pcap_fp) == EXIT_FAILURE) {
417 if (pcap_fp)
418 goto close_out;
419 }
420
421 if (!(sock = nl_socket_alloc())) {
422 ws_critical("Unable to allocate netlink socket");
423 goto close_out;
424 }
425
426 if ((err = nl_connect(sock, NETLINK_GENERIC)) < 0) {
427 ws_critical("Unable to connect netlink socket: %s",
428 nl_geterror(err));
429 goto free_out;
430 }
431
432 if ((err = genl_register_family(&ops)) < 0) {
433 ws_critical("Unable to register Generic Netlink family: %s",
434 nl_geterror(err));
435 goto err_out;
436 }
437
438 if ((err = genl_ops_resolve(sock, &ops)) < 0) {
439 ws_critical("Unable to resolve family name: %s",
440 nl_geterror(err));
441 goto err_out;
442 }
443
444 /* register notification handler callback */
445 if ((err = nl_socket_modify_cb(sock, NL_CB_VALID, NL_CB_CUSTOM,
446 parse_cb, NULL)) < 0) {
447 ws_critical("Unable to modify valid message callback %s",
448 nl_geterror(err));
449 goto err_out;
450 }
451
452 grp = nl_get_multicast_id(sock, ops.o_id, "notify");
453 nl_socket_add_membership(sock, grp);
454
455 if (!(socket_cb = nl_socket_get_cb(sock))) {
456 ws_warning("Can't overwrite recv callback");
457 } else {
458 nl_cb_overwrite_recv(socket_cb, nl_receive_timeout);
459 nl_cb_put(socket_cb);
460 }
461
462 err = send_start(sock, ops.o_id, interface_id);
463 if (err)
464 goto err_out;
465
466 nl_socket_disable_seq_check(sock);
467
468 ws_debug("DisplayPort AUX monitor running on interface %u", interface_id);
469
470 while(run_loop == TRUE) {
471 if ((err = nl_recvmsgs_default(sock)) < 0)
472 ws_warning("Unable to receive message: %s", nl_geterror(err));
473 }
474
475 send_stop(sock, ops.o_id, interface_id);
476
477 err_out:
478 nl_close(sock);
479 free_out:
480 nl_socket_free(sock);
481 close_out:
482 fclose(pcap_fp);
483 }
484
main(int argc,char * argv[])485 int main(int argc, char *argv[])
486 {
487 char* init_progfile_dir_error;
488 int option_idx = 0;
489 int result;
490 unsigned int interface_id = 0;
491 int ret = EXIT_FAILURE;
492 extcap_parameters* extcap_conf = g_new0(extcap_parameters, 1);
493 char* help_header = NULL;
494
495 /* Initialize log handler early so we can have proper logging during startup. */
496 extcap_log_init("dpauxmon");
497
498 /*
499 * Get credential information for later use.
500 */
501 init_process_policies();
502
503 /*
504 * Attempt to get the pathname of the directory containing the
505 * executable file.
506 */
507 init_progfile_dir_error = init_progfile_dir(argv[0]);
508 if (init_progfile_dir_error != NULL) {
509 ws_warning("Can't get pathname of directory containing the extcap program: %s.",
510 init_progfile_dir_error);
511 g_free(init_progfile_dir_error);
512 }
513
514 extcap_base_set_util_info(extcap_conf, argv[0], DPAUXMON_VERSION_MAJOR, DPAUXMON_VERSION_MINOR, DPAUXMON_VERSION_RELEASE,
515 NULL);
516 extcap_base_register_interface(extcap_conf, DPAUXMON_EXTCAP_INTERFACE, "DisplayPort AUX channel monitor capture", 275, "DisplayPort AUX channel monitor");
517
518 help_header = g_strdup_printf(
519 " %s --extcap-interfaces\n"
520 " %s --extcap-interface=%s --extcap-dlts\n"
521 " %s --extcap-interface=%s --extcap-config\n"
522 " %s --extcap-interface=%s --interface_id 0 --fifo myfifo --capture",
523 argv[0], argv[0], DPAUXMON_EXTCAP_INTERFACE, argv[0], DPAUXMON_EXTCAP_INTERFACE, argv[0], DPAUXMON_EXTCAP_INTERFACE);
524 extcap_help_add_header(extcap_conf, help_header);
525 g_free(help_header);
526 extcap_help_add_option(extcap_conf, "--help", "print this help");
527 extcap_help_add_option(extcap_conf, "--version", "print the version");
528 extcap_help_add_option(extcap_conf, "--port <port> ", "the dpauxmon interface index");
529
530 ws_opterr = 0;
531 ws_optind = 0;
532
533 if (argc == 1) {
534 extcap_help_print(extcap_conf);
535 goto end;
536 }
537
538 while ((result = ws_getopt_long(argc, argv, ":", longopts, &option_idx)) != -1) {
539 switch (result) {
540
541 case OPT_HELP:
542 extcap_help_print(extcap_conf);
543 ret = EXIT_SUCCESS;
544 goto end;
545
546 case OPT_VERSION:
547 extcap_version_print(extcap_conf);
548 goto end;
549
550 case OPT_INTERFACE_ID:
551 if (!ws_strtou32(ws_optarg, NULL, &interface_id)) {
552 ws_warning("Invalid interface id: %s", ws_optarg);
553 goto end;
554 }
555 break;
556
557 case ':':
558 /* missing option argument */
559 ws_warning("Option '%s' requires an argument", argv[ws_optind - 1]);
560 break;
561
562 default:
563 if (!extcap_base_parse_options(extcap_conf, result - EXTCAP_OPT_LIST_INTERFACES, ws_optarg)) {
564 ws_warning("Invalid option: %s", argv[ws_optind - 1]);
565 goto end;
566 }
567 }
568 }
569
570 extcap_cmdline_debug(argv, argc);
571
572 if (ws_optind != argc) {
573 ws_warning("Unexpected extra option: %s", argv[ws_optind]);
574 goto end;
575 }
576
577 if (extcap_base_handle_interface(extcap_conf)) {
578 ret = EXIT_SUCCESS;
579 goto end;
580 }
581
582 if (extcap_conf->show_config) {
583 ret = list_config(extcap_conf->interface);
584 goto end;
585 }
586
587 if (extcap_conf->capture)
588 run_listener(extcap_conf->fifo, interface_id);
589
590 end:
591 /* clean up stuff */
592 extcap_base_cleanup(&extcap_conf);
593 return ret;
594 }
595