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
  15 #include "extcap-base.h"
  16
  17 #include <wsutil/strtoi.h>
  18 #include <wsutil/filesystem.h>
  19 #include <writecap/pcapio.h>
  20
  21 #include <netlink/netlink.h>
  22 #include <netlink/genl/genl.h>
  23 #include <netlink/genl/ctrl.h>
  24 #include <netlink/genl/mngt.h>
  25
  26 #include <signal.h>
  27 #include <errno.h>
  28
  29 #include <linux/genetlink.h>
  30
  31 #include "dpauxmon_user.h"
  32
  33 #define PCAP_SNAPLEN 128
  34
  35 #define DPAUXMON_EXTCAP_INTERFACE "dpauxmon"
  36 #define DPAUXMON_VERSION_MAJOR "0"
  37 #define DPAUXMON_VERSION_MINOR "1"
  38 #define DPAUXMON_VERSION_RELEASE "0"
  39
  40 static gboolean run_loop = TRUE;
  41 FILE* pcap_fp = NULL;
  42
  43 enum {
  44         EXTCAP_BASE_OPTIONS_ENUM,
  45         OPT_HELP,
  46         OPT_VERSION,
  47         OPT_INTERFACE_ID,
  48 };
  49
  50 static struct option longopts[] = {
  51         EXTCAP_BASE_OPTIONS,
  52         /* Generic application options */
  53         { "help", no_argument, NULL, OPT_HELP},
  54         { "version", no_argument, NULL, OPT_VERSION},
  55         /* Interfaces options */
  56         { "interface_id", required_argument, NULL, OPT_INTERFACE_ID},
  57         { 0, 0, 0, 0 }
  58 };
  59
  60 static struct nla_policy dpauxmon_attr_policy[DPAUXMON_ATTR_MAX + 1] = {
  61         [DPAUXMON_ATTR_IFINDEX] = { .type = NLA_U32 },
  62         [DPAUXMON_ATTR_FROM_SOURCE] = { .type = NLA_FLAG },
  63         [DPAUXMON_ATTR_TIMESTAMP] = { .type = NLA_MSECS },
  64 };
  65
  66 struct family_handler_args {
  67         const char *group;
  68         int id;
  69 };
  70
  71 static int list_config(char *interface)
  72 {
  73         unsigned inc = 0;
  74
  75         if (!interface) {
  76                 g_warning("No interface specified.");
  77                 return EXIT_FAILURE;
  78         }
  79
  80         if (g_strcmp0(interface, DPAUXMON_EXTCAP_INTERFACE)) {
  81                 g_warning("interface must be %s", DPAUXMON_EXTCAP_INTERFACE);
  82                 return EXIT_FAILURE;
  83         }
  84
  85         printf("arg {number=%u}{call=--interface_id}{display=Interface index}"
  86                 "{type=unsigned}{range=1,65535}{default=%u}{tooltip=The dpauxmon interface index}\n",
  87                 inc++, 0);
  88
  89         extcap_config_debug(&inc);
  90
  91         return EXIT_SUCCESS;
  92 }
  93
  94 static void exit_from_loop(int signo _U_)
  95 {
  96         run_loop = FALSE;
  97 }
  98
  99 static int setup_dumpfile(const char* fifo, FILE** fp)
 100 {
 101         guint64 bytes_written = 0;
 102         int err;
 103
 104         if (!g_strcmp0(fifo, "-")) {
 105                 *fp = stdout;
 106                 return EXIT_SUCCESS;
 107         }
 108
 109         *fp = fopen(fifo, "w");
 110         if (!(*fp)) {
 111                 g_warning("Error creating output file: %s", g_strerror(errno));
 112                 return EXIT_FAILURE;
 113         }
 114
 115         if (!libpcap_write_file_header(*fp, 275, PCAP_SNAPLEN, FALSE, &bytes_written, &err)) {
 116                 g_warning("Can't write pcap file header");
 117                 return EXIT_FAILURE;
 118         }
 119
 120         return EXIT_SUCCESS;
 121 }
 122
 123 static int dump_packet(FILE* fp, const char* buf, const ssize_t buflen, guint64 ts_usecs)
 124 {
 125         guint64 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                 g_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 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                 g_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                 g_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                 g_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                 g_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                 g_debug("Starting monitor failed, already running?");
 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                 g_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                 g_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                 g_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                 g_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         int data_size;
 332         guint64 ts = 0;
 333         guint8 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                 g_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                 run_loop = FALSE;
 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 sigaction int_handler = { .sa_handler = exit_from_loop };
 405         struct nl_cb *socket_cb;
 406
 407         if (sigaction(SIGINT, &int_handler, 0)) {
 408                 g_warning("Can't set signal handler");
 409                 return;
 410         }
 411
 412         if (setup_dumpfile(fifo, &pcap_fp) == EXIT_FAILURE) {
 413                 if (pcap_fp)
 414                         goto close_out;
 415         }
 416
 417         if (!(sock = nl_socket_alloc())) {
 418                 g_critical("Unable to allocate netlink socket");
 419                 goto close_out;
 420         }
 421
 422         if ((err = nl_connect(sock, NETLINK_GENERIC)) < 0) {
 423                 g_critical("Unable to connect netlink socket: %s",
 424                            nl_geterror(err));
 425                 goto free_out;
 426         }
 427
 428         if ((err = genl_register_family(&ops)) < 0) {
 429                 g_critical("Unable to register Generic Netlink family");
 430                 goto err_out;
 431         }
 432
 433         if ((err = genl_ops_resolve(sock, &ops)) < 0) {
 434                 g_critical("Unable to resolve family name");
 435                 goto err_out;
 436         }
 437
 438         /* register notification handler callback */
 439         if ((err = nl_socket_modify_cb(sock, NL_CB_VALID, NL_CB_CUSTOM,
 440                         parse_cb, NULL)) < 0) {
 441                 g_critical("Unable to modify valid message callback");
 442                 goto err_out;
 443         }
 444
 445         grp = nl_get_multicast_id(sock, ops.o_id, "notify");
 446         nl_socket_add_membership(sock, grp);
 447
 448         if (!(socket_cb = nl_socket_get_cb(sock))) {
 449                 g_warning("Can't overwrite recv callback");
 450         } else {
 451                 nl_cb_overwrite_recv(socket_cb, nl_receive_timeout);
 452                 nl_cb_put(socket_cb);
 453         }
 454
 455         err = send_start(sock, ops.o_id, interface_id);
 456         if (err)
 457                 goto err_out;
 458
 459         nl_socket_disable_seq_check(sock);
 460
 461         g_debug("DisplayPort AUX monitor running on interface %u", interface_id);
 462
 463         while(run_loop == TRUE) {
 464                 if ((err = nl_recvmsgs_default(sock)) < 0)
 465                         g_warning("Unable to receive message: %s", nl_geterror(err));
 466         }
 467
 468         send_stop(sock, ops.o_id, interface_id);
 469
 470 err_out:
 471         nl_close(sock);
 472 free_out:
 473         nl_socket_free(sock);
 474 close_out:
 475         fclose(pcap_fp);
 476 }
 477
 478 int main(int argc, char *argv[])
 479 {
 480         int option_idx = 0;
 481         int result;
 482         unsigned int interface_id = 0;
 483         int ret = EXIT_FAILURE;
 484         extcap_parameters* extcap_conf = g_new0(extcap_parameters, 1);
 485         char* help_header = NULL;
 486
 487         extcap_base_set_util_info(extcap_conf, argv[0], DPAUXMON_VERSION_MAJOR, DPAUXMON_VERSION_MINOR, DPAUXMON_VERSION_RELEASE,
 488                 NULL);
 489         extcap_base_register_interface(extcap_conf, DPAUXMON_EXTCAP_INTERFACE, "DisplayPort AUX channel monitor capture", 275, "DisplayPort AUX channel monitor");
 490
 491         help_header = g_strdup_printf(
 492                 " %s --extcap-interfaces\n"
 493                 " %s --extcap-interface=%s --extcap-dlts\n"
 494                 " %s --extcap-interface=%s --extcap-config\n"
 495                 " %s --extcap-interface=%s --interface_id 0 --fifo myfifo --capture",
 496                 argv[0], argv[0], DPAUXMON_EXTCAP_INTERFACE, argv[0], DPAUXMON_EXTCAP_INTERFACE, argv[0], DPAUXMON_EXTCAP_INTERFACE);
 497         extcap_help_add_header(extcap_conf, help_header);
 498         g_free(help_header);
 499         extcap_help_add_option(extcap_conf, "--help", "print this help");
 500         extcap_help_add_option(extcap_conf, "--version", "print the version");
 501         extcap_help_add_option(extcap_conf, "--port <port> ", "the dpauxmon interface index");
 502
 503         opterr = 0;
 504         optind = 0;
 505
 506         if (argc == 1) {
 507                 extcap_help_print(extcap_conf);
 508                 goto end;
 509         }
 510
 511         while ((result = getopt_long(argc, argv, ":", longopts, &option_idx)) != -1) {
 512                 switch (result) {
 513
 514                 case OPT_HELP:
 515                         extcap_help_print(extcap_conf);
 516                         ret = EXIT_SUCCESS;
 517                         goto end;
 518
 519                 case OPT_VERSION:
 520                         printf("%s\n", extcap_conf->version);
 521                         goto end;
 522
 523                 case OPT_INTERFACE_ID:
 524                         if (!ws_strtou32(optarg, NULL, &interface_id)) {
 525                                 g_warning("Invalid interface id: %s", optarg);
 526                                 goto end;
 527                         }
 528                         break;
 529
 530                 case ':':
 531                         /* missing option argument */
 532                         g_warning("Option '%s' requires an argument", argv[optind - 1]);
 533                         break;
 534
 535                 default:
 536                         if (!extcap_base_parse_options(extcap_conf, result - EXTCAP_OPT_LIST_INTERFACES, optarg)) {
 537                                 g_warning("Invalid option: %s", argv[optind - 1]);
 538                                 goto end;
 539                         }
 540                 }
 541         }
 542
 543         extcap_cmdline_debug(argv, argc);
 544
 545         if (optind != argc) {
 546                 g_warning("Unexpected extra option: %s", argv[optind]);
 547                 goto end;
 548         }
 549
 550         if (extcap_base_handle_interface(extcap_conf)) {
 551                 ret = EXIT_SUCCESS;
 552                 goto end;
 553         }
 554
 555         if (extcap_conf->show_config) {
 556                 ret = list_config(extcap_conf->interface);
 557                 goto end;
 558         }
 559
 560         if (extcap_conf->capture)
 561                 run_listener(extcap_conf->fifo, interface_id);
 562
 563 end:
 564         /* clean up stuff */
 565         extcap_base_cleanup(&extcap_conf);
 566         return ret;
 567 }