monitor.c

   1 /*
   2  *   This program is free software; you can redistribute it and/or modify
   3  *   it under the terms of the GNU General Public License as published by
   4  *   the Free Software Foundation; either version 2 of the License.
   5  *
   6  *   This program is distributed in the hope that it will be useful,
   7  *   but WITHOUT ANY WARRANTY; without even the implied warranty of
   8  *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
   9  *   GNU General Public License for more details.
  10  *
  11  *   You should have received a copy of the GNU General Public License
  12  *   along with this program; if not, write to the Free Software
  13  *   Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307, USA.
  14  *
  15  *   Copyright (C) 2020 embedd.ch
  16  *   Copyright (C) 2020 Felix Fietkau <nbd@nbd.name>
  17  *   Copyright (C) 2020 John Crispin <john@phrozen.org>
  18  */
  19
  20 #include <netinet/udp.h>
  21 #include <arpa/inet.h>
  22 #include <stdio.h>
  23
  24 #include <pcap/pcap.h>
  25
  26 #include <libubox/blobmsg_json.h>
  27
  28 #include "usteer.h"
  29 #include "remote.h"
  30
  31 static pcap_t *pcap;
  32 static int pkt_offset;
  33
  34 /* IP header */
  35 struct ip_header {
  36         uint8_t ip_vhl;         /* version << 4 | header length >> 2 */
  37         uint8_t ip_tos;         /* type of service */
  38         uint16_t ip_len;                /* total length */
  39         uint16_t ip_id;         /* identification */
  40         uint16_t ip_off;                /* fragment offset field */
  41 #define IP_RF 0x8000            /* reserved fragment flag */
  42 #define IP_DF 0x4000            /* dont fragment flag */
  43 #define IP_MF 0x2000            /* more fragments flag */
  44 #define IP_OFFMASK 0x1fff       /* mask for fragmenting bits */
  45         uint8_t ip_ttl;         /* time to live */
  46         uint8_t ip_p;           /* protocol */
  47         uint16_t ip_sum;                /* checksum */
  48         struct in_addr ip_src, ip_dst; /* source and dest address */
  49 };
  50 #define IP_HL(ip)               (((ip)->ip_vhl) & 0x0f)
  51 #define IP_V(ip)                (((ip)->ip_vhl) >> 4)
  52
  53 struct udp_header {
  54         uint16_t uh_sport;       /* source port */
  55         uint16_t uh_dport;       /* destination port */
  56         uint16_t uh_ulen;        /* udp length */
  57         uint16_t uh_sum;     /* udp checksum */
  58 };
  59
  60
  61 static void
  62 decode_sta(struct blob_attr *data)
  63 {
  64         struct apmsg_sta msg;
  65
  66         if (!parse_apmsg_sta(&msg, data))
  67                 return;
  68
  69         fprintf(stderr, "\t\tSta "MAC_ADDR_FMT" signal=%d connected=%d timeout=%d\n",
  70                 MAC_ADDR_DATA(msg.addr), msg.signal, msg.connected, msg.timeout);
  71 }
  72
  73 static void
  74 decode_node(struct blob_attr *data)
  75 {
  76         struct apmsg_node msg;
  77         struct blob_attr *cur;
  78         int rem;
  79
  80         if (!parse_apmsg_node(&msg, data))
  81                 return;
  82
  83         fprintf(stderr, "\tNode %s, freq=%d, n_assoc=%d, noise=%d load=%d max_assoc=%d\n",
  84                 msg.name, msg.freq, msg.n_assoc, msg.noise, msg.load, msg.max_assoc);
  85         if (msg.rrm_nr) {
  86                 fprintf(stderr, "\t\tRRM:");
  87                 blobmsg_for_each_attr(cur, msg.rrm_nr, rem) {
  88                         if (!blobmsg_check_attr(cur, false))
  89                                 continue;
  90                         if (blobmsg_type(cur) != BLOBMSG_TYPE_STRING)
  91                                 continue;
  92                         fprintf(stderr, " %s", blobmsg_get_string(cur));
  93                 }
  94                 fprintf(stderr, "\n");
  95         }
  96
  97         if (msg.node_info) {
  98                 char *data = blobmsg_format_json(msg.node_info, true);
  99                 fprintf(stderr, "\t\tNode info: %s\n", data);
 100                 free(data);
 101         }
 102
 103         blob_for_each_attr(cur, msg.stations, rem)
 104                 decode_sta(cur);
 105 }
 106
 107 static void
 108 decode_packet(struct blob_attr *data)
 109 {
 110         struct apmsg msg;
 111         struct blob_attr *cur;
 112         int rem;
 113
 114         if (!parse_apmsg(&msg, data)) {
 115                 fprintf(stderr, "missing fields\n");
 116                 return;
 117         }
 118
 119         fprintf(stderr, "id=%08x, seq=%d\n", msg.id, msg.seq);
 120
 121         blob_for_each_attr(cur, msg.nodes, rem)
 122                 decode_node(cur);
 123 }
 124
 125 static void
 126 recv_packet(unsigned char *user, const struct pcap_pkthdr *hdr,
 127             const unsigned char *packet)
 128 {
 129         char addr[INET_ADDRSTRLEN];
 130         struct ip_header *ip;
 131         struct udp_header *uh;
 132         struct blob_attr *data;
 133         int len = hdr->caplen;
 134         int hdrlen;
 135
 136         len -= pkt_offset;
 137         packet += pkt_offset;
 138         ip = (void *) packet;
 139
 140         hdrlen = IP_HL(ip) * 4;
 141         if (hdrlen < 20 || hdrlen >= len)
 142                 return;
 143
 144         len -= hdrlen;
 145         packet += hdrlen;
 146
 147         inet_ntop(AF_INET, &ip->ip_src, addr, sizeof(addr));
 148
 149         hdrlen = sizeof(*uh);
 150         if (len <= hdrlen)
 151                 return;
 152
 153         uh = (void *) packet;
 154         packet += hdrlen;
 155         len -= hdrlen;
 156
 157         if (uh->uh_dport != htons(APMGR_PORT))
 158                 return;
 159
 160         data = (void *) packet;
 161
 162         fprintf(stderr, "[%s]: len=%d ", addr, len);
 163
 164         if (len != blob_pad_len(data)) {
 165                 fprintf(stderr, "invalid data\n");
 166                 return;
 167         }
 168
 169         decode_packet(data);
 170 }
 171
 172 int main(int argc, char **argv)
 173 {
 174         static char errbuf[PCAP_ERRBUF_SIZE];
 175         struct bpf_program fp;
 176
 177         if (argc != 2) {
 178                 fprintf(stderr, "Usage: %s <interface>\n", argv[0]);
 179                 return 1;
 180         }
 181
 182         pcap = pcap_open_live(argv[1], APMGR_BUFLEN, 1, 1000, errbuf);
 183         if (!pcap) {
 184                         fprintf(stderr, "Failed to open interface %s: %s\n", argv[1], errbuf);
 185                         return 1;
 186         }
 187
 188         pcap_compile(pcap, &fp, "port "APMGR_PORT_STR, 1, PCAP_NETMASK_UNKNOWN);
 189         pcap_setfilter(pcap, &fp);
 190
 191         switch (pcap_datalink(pcap)) {
 192         case DLT_EN10MB:
 193                 pkt_offset = 14;
 194                 break;
 195         case DLT_RAW:
 196                 pkt_offset = 0;
 197                 break;
 198         default:
 199                 fprintf(stderr, "Invalid link type\n");
 200                 return -1;
 201         }
 202
 203         pcap_loop(pcap, 0, recv_packet, NULL);
 204         pcap_close(pcap);
 205
 206         return 0;
 207 }