gprs: Add a custom GPRS filter

Allow to inspect UDP messages and check for GPRS, NS, BSSGP
and then filter LLC frames. Parsing the vL datastructure with
the libpcap syntax is a pain. It could be done using BPF but
we do not want to use bpf asm to specify the entire ruleset.

I looked into using libepan/libwireshark but this has memory
issues and is painful too. So let's parse UDP, NS, BSSGP using
the info we already have. I tried a bit of editcap to generate
a bit of broken data. The length check might still be bad.

I used my crash_20100602.pcap file to count the LLC frames we
detect and compare that to wireshark it ended with the right
number.

  pcap add-filter gprs

can be used to enable the new filtering option after the OS
has received the packet.

Fixes: ONW#1314

5 files changed, 145 insertions, 1 deletions

diff --git a/configure.ac b/configure.ac
index d0738f8..5efca3a 100644
--- a/configure.ac
+++ b/configure.ac
@@ -45,6 +45,7 @@ AC_SUBST([PCAP_CFLAGS])
 
 dnl checks for libraries
 PKG_CHECK_MODULES(LIBOSMOCORE, libosmocore >= 0.3.2)
+PKG_CHECK_MODULES(LIBOSMOGSM, libosmogsm >= 0.3.2)
 PKG_CHECK_MODULES(LIBOSMOVTY, libosmovty >= 0.3.0)
 
 
diff --git a/include/osmo-pcap/osmo_pcap_client.h b/include/osmo-pcap/osmo_pcap_client.h
index 7aae7ca..30b55ee 100644
--- a/include/osmo-pcap/osmo_pcap_client.h
+++ b/include/osmo-pcap/osmo_pcap_client.h
@@ -36,6 +36,7 @@ struct osmo_pcap_client {
 	struct bpf_program bpf;
 	char   *filter_string;
 	int filter_itself;
+	int gprs_filtering;
 	struct osmo_fd fd;
 
 	char *srv_ip;
diff --git a/src/Makefile.am b/src/Makefile.am
index 95a9d6b..cc3f478 100644
--- a/src/Makefile.am
+++ b/src/Makefile.am
@@ -6,7 +6,7 @@ bin_PROGRAMS = osmo_pcap_client osmo_pcap_server
 osmo_pcap_client_SOURCES = osmo_client_main.c osmo_common.c \
 			   osmo_client_core.c osmo_client_vty.c \
 			   osmo_client_network.c
-osmo_pcap_client_LDADD = $(PCAP_LIBS) $(LIBOSMOCORE_LIBS) $(LIBOSMOVTY_LIBS)
+osmo_pcap_client_LDADD = $(PCAP_LIBS) $(LIBOSMOCORE_LIBS) $(LIBOSMOVTY_LIBS) $(LIBOSMOGSM_LIBS)
 
 osmo_pcap_server_SOURCES = osmo_server_main.c osmo_common.c \
 			   osmo_server_vty.c osmo_server_network.c
diff --git a/src/osmo_client_core.c b/src/osmo_client_core.c
index c8bd8bd..d116b1b 100644
--- a/src/osmo_client_core.c
+++ b/src/osmo_client_core.c
@@ -20,17 +20,133 @@
  *
  */
 
+#define _BSD_SOURCE
 #include <osmo-pcap/osmo_pcap_client.h>
 #include <osmo-pcap/common.h>
 
+#include <osmocom/gprs/gprs_bssgp.h>
+#include <osmocom/gprs/protocol/gsm_08_16.h>
+#include <osmocom/gprs/protocol/gsm_08_18.h>
+
 #include <osmocom/core/talloc.h>
 
+#include <netinet/ip.h>
+#include <netinet/udp.h>
+
 #include <limits.h>
 
 #ifndef PCAP_NETMASK_UNKNOWN
 #define PCAP_NETMASK_UNKNOWN 0xffffffff
 #endif
 
+#define IP_LEN		sizeof(struct ip)
+#define UDP_LEN		sizeof(struct udphdr)
+#define NS_LEN		1
+
+static int saw_llc = 0;
+static int failed_to_parse = 0;
+
+static int check_gprs(const u_char *data, bpf_u_int32 len)
+{
+	struct tlv_parsed tp;
+	struct gprs_ns_hdr *hdr = (struct gprs_ns_hdr *) data;
+	struct bssgp_ud_hdr *bssgp_hdr;
+	uint8_t llc_sapi;
+
+	switch (hdr->pdu_type) {
+	case NS_PDUT_UNITDATA:
+		break;
+	default:
+		return 1;
+	}
+
+	len -= sizeof(*hdr);
+
+	/* NS_PDUT_UNITDATA from here.. */
+	/* skip NS SDU control bits and BVCI */
+	if (len < 3)
+		return 1;
+	len -= 3;
+
+	/* Check if the BSSGP UD hdr fits */
+	if (len < sizeof(*bssgp_hdr))
+		return 1;
+	bssgp_hdr = (struct bssgp_ud_hdr *) &hdr->data[3];
+
+	/* We only need to check UL/DL messages for the sapi */
+	if (bssgp_hdr->pdu_type != BSSGP_PDUT_DL_UNITDATA
+		&& bssgp_hdr->pdu_type != BSSGP_PDUT_UL_UNITDATA)
+		return 1;
+	len -= sizeof(*bssgp_hdr);
+
+	/* now parse the rest of the IEs */
+	memset(&tp, 0, sizeof(tp));
+	if (bssgp_tlv_parse(&tp, &bssgp_hdr->data[0], len) < 0)
+		return 1;
+
+	if (!TLVP_PRESENT(&tp, BSSGP_IE_LLC_PDU))
+		return 1;
+	if (TLVP_LEN(&tp, BSSGP_IE_LLC_PDU) < 1)
+		return 1;
+
+	llc_sapi = TLVP_VAL(&tp, BSSGP_IE_LLC_PDU)[0] & 0x0f;
+	/* Skip user data 3, 5, 9, 11 */
+	if (llc_sapi == 3 || llc_sapi == 5 || llc_sapi == 9 || llc_sapi == 11)
+		return 0;
+	return 1;
+}
+
+static int forward_packet(
+			struct osmo_pcap_client *client,
+			struct pcap_pkthdr *hdr,
+			const u_char *data)
+{
+	int ll_type;
+	int offset;
+	struct ip *ip_hdr;
+	const u_char *ip_data;
+	const u_char *udp_data;
+	const u_char *payload_data;
+	bpf_u_int32 payload_len;
+
+	if (!client->gprs_filtering)
+		return 1;
+
+	ll_type = pcap_datalink(client->handle);
+	switch (ll_type) {
+	case DLT_EN10MB:
+		offset = 14;
+		break;
+	case DLT_LINUX_SLL:
+		offset = 16;
+		break;
+	default:
+		LOGP(DCLIENT, LOGL_ERROR, "LL type %d/%s not handled.\n",
+			ll_type, pcap_datalink_val_to_name(ll_type));
+		return 1;
+	}
+
+	/* Check if this can be a full UDP frame with NS */
+	if (offset + IP_LEN + UDP_LEN + NS_LEN > hdr->caplen)
+		return 1;
+
+	ip_data = data + offset;
+	ip_hdr = (struct ip *) ip_data;
+
+	/* Only handle IPv4 */
+	if (ip_hdr->ip_v != 4)
+		return 1;
+	/* Only handle UDP */
+	if (ip_hdr->ip_p != 17)
+		return 1;
+
+	udp_data = ip_data + IP_LEN;
+	payload_data = udp_data + UDP_LEN;
+	payload_len = hdr->caplen - offset - IP_LEN - UDP_LEN;
+
+	return check_gprs(payload_data, payload_len);
+}
+
 
 static int pcap_read_cb(struct osmo_fd *fd, unsigned int what)
 {
@@ -42,6 +158,9 @@ static int pcap_read_cb(struct osmo_fd *fd, unsigned int what)
 	if (!data)
 		return -1;
 
+	if (!forward_packet(client, &hdr, data))
+		return 0;
+
 	osmo_client_send_data(client, &hdr, data);
 	return 0;
 }
diff --git a/src/osmo_client_vty.c b/src/osmo_client_vty.c
index 0b30eb7..a8739b1 100644
--- a/src/osmo_client_vty.c
+++ b/src/osmo_client_vty.c
@@ -59,6 +59,8 @@ static int config_write_client(struct vty *vty)
 			pcap_client->filter_string, VTY_NEWLINE);
 	vty_out(vty, " pcap detect-loop %d%s",
 		pcap_client->filter_itself, VTY_NEWLINE);
+	if (pcap_client->gprs_filtering)
+		vty_out(vty, " pcap add-filter gprs%s", VTY_NEWLINE);
 
 	if (pcap_client->srv_ip)
 		vty_out(vty, " server ip %s%s",
@@ -80,6 +82,24 @@ DEFUN(cfg_client_device,
 	return CMD_SUCCESS;
 }
 
+DEFUN(cfg_client_add_gprs,
+      cfg_client_add_gprs_cmd,
+      "pcap add-filter gprs",
+      PCAP_STRING "Add-filter\n" "Custom filtering for GPRS\n")
+{
+	pcap_client->gprs_filtering = 1;
+	return CMD_SUCCESS;
+}
+
+DEFUN(cfg_client_del_gprs,
+      cfg_client_del_gprs_cmd,
+      "no pcap add-filter gprs",
+      NO_STR PCAP_STRING "Add-filter\n" "Custom filter for GPRS\n")
+{
+	pcap_client->gprs_filtering = 0;
+	return CMD_SUCCESS;
+}
+
 DEFUN(cfg_client_filter,
       cfg_client_filter_cmd,
       "pcap filter .NAME",
@@ -144,5 +164,8 @@ int vty_client_init(struct osmo_pcap_client *pcap)
 	install_element(CLIENT_NODE, &cfg_server_ip_cmd);
 	install_element(CLIENT_NODE, &cfg_server_port_cmd);
 
+	install_element(CLIENT_NODE, &cfg_client_add_gprs_cmd);
+	install_element(CLIENT_NODE, &cfg_client_del_gprs_cmd);
+
 	return 0;
 }