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