mirror of
https://git.kernel.org/pub/scm/network/wireless/iwd.git
synced 2024-12-22 21:22:37 +01:00
294 lines
6.5 KiB
C
294 lines
6.5 KiB
C
/*
|
|
*
|
|
* Wireless daemon for Linux
|
|
*
|
|
* Copyright (C) 2013-2014 Intel Corporation. All rights reserved.
|
|
*
|
|
* This library is free software; you can redistribute it and/or
|
|
* modify it under the terms of the GNU Lesser General Public
|
|
* License as published by the Free Software Foundation; either
|
|
* version 2.1 of the License, or (at your option) any later version.
|
|
*
|
|
* This library is distributed in the hope that it will be useful,
|
|
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
|
|
* Lesser General Public License for more details.
|
|
*
|
|
* You should have received a copy of the GNU Lesser General Public
|
|
* License along with this library; if not, write to the Free Software
|
|
* Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
|
|
*
|
|
*/
|
|
|
|
#ifdef HAVE_CONFIG_H
|
|
#include <config.h>
|
|
#endif
|
|
|
|
#include <stdio.h>
|
|
#include <stdlib.h>
|
|
#include <getopt.h>
|
|
#include <sys/socket.h>
|
|
#include <linux/genetlink.h>
|
|
#include <linux/if_arp.h>
|
|
#include <ell/ell.h>
|
|
|
|
#include "linux/nl80211.h"
|
|
#include "monitor/nlmon.h"
|
|
#include "monitor/pcap.h"
|
|
|
|
static struct nlmon *nlmon = NULL;
|
|
|
|
#define NLA_OK(nla,len) ((len) >= (int) sizeof(struct nlattr) && \
|
|
(nla)->nla_len >= sizeof(struct nlattr) && \
|
|
(nla)->nla_len <= (len))
|
|
#define NLA_NEXT(nla,attrlen) ((attrlen) -= NLA_ALIGN((nla)->nla_len), \
|
|
(struct nlattr*)(((char*)(nla)) + \
|
|
NLA_ALIGN((nla)->nla_len)))
|
|
|
|
#define NLA_LENGTH(len) (NLA_ALIGN(sizeof(struct nlattr)) + (len))
|
|
#define NLA_DATA(nla) ((void*)(((char*)(nla)) + NLA_LENGTH(0)))
|
|
#define NLA_PAYLOAD(nla) ((int)((nla)->nla_len - NLA_LENGTH(0)))
|
|
|
|
static void genl_parse(uint16_t type, const void *data, uint32_t len,
|
|
const char *ifname)
|
|
{
|
|
const struct genlmsghdr *genlmsg = data;
|
|
const struct nlattr *nla;
|
|
char name[GENL_NAMSIZ];
|
|
uint16_t id = GENL_ID_GENERATE;
|
|
|
|
if (nlmon)
|
|
return;
|
|
|
|
if (type != GENL_ID_CTRL)
|
|
return;
|
|
|
|
if (genlmsg->cmd != CTRL_CMD_NEWFAMILY)
|
|
return;
|
|
|
|
for (nla = data + GENL_HDRLEN; NLA_OK(nla, len);
|
|
nla = NLA_NEXT(nla, len)) {
|
|
switch (nla->nla_type & NLA_TYPE_MASK) {
|
|
case CTRL_ATTR_FAMILY_ID:
|
|
id = *((uint16_t *) NLA_DATA(nla));
|
|
break;
|
|
case CTRL_ATTR_FAMILY_NAME:
|
|
strncpy(name, NLA_DATA(nla), GENL_NAMSIZ);
|
|
break;
|
|
}
|
|
}
|
|
|
|
if (id == GENL_ID_GENERATE)
|
|
return;
|
|
|
|
if (!strcmp(name, NL80211_GENL_NAME))
|
|
nlmon = nlmon_open(ifname, id);
|
|
}
|
|
|
|
static void genl_notify(uint16_t type, const void *data,
|
|
uint32_t len, void *user_data)
|
|
{
|
|
const char *ifname = user_data;
|
|
|
|
genl_parse(type, data, len, ifname);
|
|
}
|
|
|
|
static void genl_callback(int error, uint16_t type, const void *data,
|
|
uint32_t len, void *user_data)
|
|
{
|
|
const char *ifname = user_data;
|
|
|
|
if (error < 0) {
|
|
fprintf(stderr, "Failed to lookup nl80211 family\n");
|
|
l_main_quit();
|
|
return;
|
|
}
|
|
|
|
genl_parse(type, data, len, ifname);
|
|
}
|
|
|
|
static struct l_netlink *genl_lookup(const char *ifname)
|
|
{
|
|
struct l_netlink *genl;
|
|
char buf[GENL_HDRLEN + NLA_HDRLEN + GENL_NAMSIZ];
|
|
struct genlmsghdr *genlmsg;
|
|
struct nlattr *nla;
|
|
|
|
genl = l_netlink_new(NETLINK_GENERIC);
|
|
|
|
l_netlink_register(genl, GENL_ID_CTRL, genl_notify, NULL, NULL);
|
|
|
|
genlmsg = (struct genlmsghdr *) buf;
|
|
genlmsg->cmd = CTRL_CMD_GETFAMILY;
|
|
genlmsg->version = 0;
|
|
genlmsg->reserved = 0;
|
|
|
|
nla = (struct nlattr *) (buf + GENL_HDRLEN);
|
|
nla->nla_len = NLA_HDRLEN + GENL_NAMSIZ;
|
|
nla->nla_type = CTRL_ATTR_FAMILY_NAME;
|
|
strncpy(buf + GENL_HDRLEN + NLA_HDRLEN,
|
|
NL80211_GENL_NAME, GENL_NAMSIZ);
|
|
|
|
l_netlink_send(genl, GENL_ID_CTRL, 0, buf, sizeof(buf),
|
|
genl_callback, (char *) ifname, NULL);
|
|
|
|
return genl;
|
|
}
|
|
|
|
static int process_pcap(struct pcap *pcap)
|
|
{
|
|
struct nlmon *nlmon = NULL;
|
|
uint8_t buf[8192];
|
|
uint32_t len;
|
|
|
|
nlmon = nlmon_create();
|
|
|
|
while (pcap_read(pcap, NULL, buf, sizeof(buf), &len)) {
|
|
uint16_t arphrd_type;
|
|
uint16_t proto_type;
|
|
|
|
if (len < 16) {
|
|
printf("Too short packet\n");
|
|
continue;
|
|
}
|
|
|
|
arphrd_type = L_GET_UNALIGNED((const uint16_t *) (buf + 2));
|
|
|
|
if (L_BE16_TO_CPU(arphrd_type) != ARPHRD_NETLINK) {
|
|
printf("Unsupported ARPHRD %u\n",
|
|
L_BE16_TO_CPU(arphrd_type));
|
|
continue;
|
|
}
|
|
|
|
proto_type = L_GET_UNALIGNED((const uint16_t *) (buf + 14));
|
|
|
|
switch (L_BE16_TO_CPU(proto_type)) {
|
|
case NETLINK_ROUTE:
|
|
printf("RTNL packet\n");
|
|
break;
|
|
case NETLINK_GENERIC:
|
|
nlmon_print(nlmon, buf + 16, len - 16);
|
|
break;
|
|
default:
|
|
printf("Other protocol %u\n",
|
|
L_BE16_TO_CPU(proto_type));
|
|
break;
|
|
}
|
|
}
|
|
|
|
nlmon_destroy(nlmon);
|
|
|
|
return EXIT_SUCCESS;
|
|
}
|
|
|
|
static void signal_handler(struct l_signal *signal, uint32_t signo,
|
|
void *user_data)
|
|
{
|
|
switch (signo) {
|
|
case SIGINT:
|
|
case SIGTERM:
|
|
l_main_quit();
|
|
break;
|
|
}
|
|
}
|
|
|
|
static void usage(void)
|
|
{
|
|
printf("iwmon - Wireless monitor\n"
|
|
"Usage:\n");
|
|
printf("\tiwmon [options]\n");
|
|
printf("options:\n"
|
|
"\t-r, --read <file> Read netlink PCAP trace files\n"
|
|
"\t-i, --interface <dev> Use specified netlink monitor\n"
|
|
"\t-h, --help Show help options\n");
|
|
}
|
|
|
|
static const struct option main_options[] = {
|
|
{ "read", required_argument, NULL, 'r' },
|
|
{ "interface", required_argument, NULL, 'i' },
|
|
{ "version", no_argument, NULL, 'v' },
|
|
{ "help", no_argument, NULL, 'h' },
|
|
{ }
|
|
};
|
|
|
|
int main(int argc, char *argv[])
|
|
{
|
|
const char *reader_path = NULL;
|
|
const char *ifname = "nlmon";
|
|
struct l_signal *signal;
|
|
struct l_netlink *genl;
|
|
sigset_t mask;
|
|
int exit_status;
|
|
|
|
for (;;) {
|
|
int opt;
|
|
|
|
opt = getopt_long(argc, argv, "r:i:vh", main_options, NULL);
|
|
if (opt < 0)
|
|
break;
|
|
|
|
switch (opt) {
|
|
case 'r':
|
|
reader_path = optarg;
|
|
break;
|
|
case 'i':
|
|
ifname = optarg;
|
|
break;
|
|
case 'v':
|
|
printf("%s\n", VERSION);
|
|
return EXIT_SUCCESS;
|
|
case 'h':
|
|
usage();
|
|
return EXIT_SUCCESS;
|
|
default:
|
|
return EXIT_FAILURE;
|
|
}
|
|
}
|
|
|
|
if (argc - optind > 0) {
|
|
fprintf(stderr, "Invalid command line parameters\n");
|
|
return EXIT_FAILURE;
|
|
}
|
|
|
|
sigemptyset(&mask);
|
|
sigaddset(&mask, SIGINT);
|
|
sigaddset(&mask, SIGTERM);
|
|
|
|
signal = l_signal_create(&mask, signal_handler, NULL, NULL);
|
|
|
|
printf("Wireless monitor ver %s\n", VERSION);
|
|
|
|
if (reader_path) {
|
|
struct pcap *pcap;
|
|
|
|
pcap = pcap_open(reader_path);
|
|
if (!pcap) {
|
|
exit_status = EXIT_FAILURE;
|
|
goto done;
|
|
}
|
|
|
|
if (pcap_get_type(pcap) != PCAP_TYPE_LINUX_SLL) {
|
|
fprintf(stderr, "Invalid packet format\n");
|
|
exit_status = EXIT_FAILURE;
|
|
} else
|
|
exit_status = process_pcap(pcap);
|
|
|
|
pcap_close(pcap);
|
|
goto done;
|
|
}
|
|
|
|
genl = genl_lookup(ifname);
|
|
|
|
l_main_run();
|
|
|
|
l_netlink_destroy(genl);
|
|
nlmon_close(nlmon);
|
|
|
|
exit_status = EXIT_SUCCESS;
|
|
|
|
done:
|
|
l_signal_remove(signal);
|
|
|
|
return exit_status;
|
|
}
|