extcap/dpauxmon.c

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