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         if (msg.host_info) {
 121                 char *data = blobmsg_format_json(msg.host_info, true);
 122                 fprintf(stderr, "\tHost info: %s\n", data);
 123                 free(data);
 124         }
 125
 126         blob_for_each_attr(cur, msg.nodes, rem)
 127                 decode_node(cur);
 128 }
 129
 130 static void
 131 recv_packet(unsigned char *user, const struct pcap_pkthdr *hdr,
 132             const unsigned char *packet)
 133 {
 134         char addr[INET_ADDRSTRLEN];
 135         struct ip_header *ip;
 136         struct udp_header *uh;
 137         struct blob_attr *data;
 138         int len = hdr->caplen;
 139         int hdrlen;
 140
 141         len -= pkt_offset;
 142         packet += pkt_offset;
 143         ip = (void *) packet;
 144
 145         hdrlen = IP_HL(ip) * 4;
 146         if (hdrlen < 20 || hdrlen >= len)
 147                 return;
 148
 149         len -= hdrlen;
 150         packet += hdrlen;
 151
 152         inet_ntop(AF_INET, &ip->ip_src, addr, sizeof(addr));
 153
 154         hdrlen = sizeof(*uh);
 155         if (len <= hdrlen)
 156                 return;
 157
 158         uh = (void *) packet;
 159         packet += hdrlen;
 160         len -= hdrlen;
 161
 162         if (uh->uh_dport != htons(APMGR_PORT))
 163                 return;
 164
 165         data = (void *) packet;
 166
 167         fprintf(stderr, "[%s]: len=%d ", addr, len);
 168
 169         if (len != blob_pad_len(data)) {
 170                 fprintf(stderr, "invalid data\n");
 171                 return;
 172         }
 173
 174         decode_packet(data);
 175 }
 176
 177 int main(int argc, char **argv)
 178 {
 179         static char errbuf[PCAP_ERRBUF_SIZE];
 180         struct bpf_program fp;
 181
 182         if (argc != 2) {
 183                 fprintf(stderr, "Usage: %s <interface>\n", argv[0]);
 184                 return 1;
 185         }
 186
 187         pcap = pcap_open_live(argv[1], APMGR_BUFLEN, 1, 1000, errbuf);
 188         if (!pcap) {
 189                         fprintf(stderr, "Failed to open interface %s: %s\n", argv[1], errbuf);
 190                         return 1;
 191         }
 192
 193         pcap_compile(pcap, &fp, "port "APMGR_PORT_STR, 1, PCAP_NETMASK_UNKNOWN);
 194         pcap_setfilter(pcap, &fp);
 195
 196         switch (pcap_datalink(pcap)) {
 197         case DLT_EN10MB:
 198                 pkt_offset = 14;
 199                 break;
 200         case DLT_RAW:
 201                 pkt_offset = 0;
 202                 break;
 203         default:
 204                 fprintf(stderr, "Invalid link type\n");
 205                 return -1;
 206         }
 207
 208         pcap_loop(pcap, 0, recv_packet, NULL);
 209         pcap_close(pcap);
 210
 211         return 0;
 212 }