allow specifying gsmtap dest ip

This allows distinguishing multiple modems by specifying another
loopback ip i.e. -i 127.0.0.4
Linux has a default local route for 127.0.0.0/8 so it "just works".

README

@@ -9,3 +9,40 @@ the necessary utility.
 In general all Qualcomm devices that export DIAG should be
 supported. We have tested it with the Quectel UC20, EC20 and
 EC25 devices.
+
+The convenient way to create a pcap dump of the messages is to run
+./capture.sh -s /dev/ttyUSB0 -f outfilename.pcap
+This requires tcpdump, as well as the required permissions
+to capture, i.e. on ubuntu this can be conveniently achieved by
+sudo setcap cap_net_raw,cap_net_admin=ep /usr/sbin/tcpdump
+
+In order to distinguish multiple different modems in the capture file
+passing a GSMTAP destination ip is possible with the -i parameter, i.e.
+./capture.sh -s /dev/ttyUSB0 -f outfilename.pcap -i 127.0.0.4
+This will "just work" on linux, since there is a default 127.0.0.0/8 route.
+
+Additionally wireshark needs Edit->Preferences->Protocols->NAS-EPS
+"Force dissect as plain EPS" set to true, since capturing encrypted NAS
+messages is fairly useless (although possible), so unencrypted
+unprotected NAS messages are emitted, but wireshark will by default
+refuse to dissect plain messages because according to the spec most
+messages need to be at least integrity protected.
+
+Ubuntus ModemManager will interfere with the modem, so it needs to be
+told to leave the modem alone. This can be accomplished by adding the
+following line to 
+/etc/udev/rules.d/69-block-mm.rules :
+ATTRS{idVendor}=="2c7c" ATTRS{idProduct}=="0125", ENV{ID_MM_DEVICE_IGNORE}="1
+
+This example is for quectel ec25-e, adjust the USB VID:PID according to
+what lsusb tells you about your particular modem. The rules then need to
+be reloaded and ModemManager restarted:
+sudo udevadm control --reload-rules
+sudo udevadm trigger
+sudo systemctl restart ModemManager
+
+There appears to be a bug on Ubuntu 18.04 and the ModemManager will still
+interfere, see 
+https://bugs.launchpad.net/ubuntu/+source/modemmanager/+bug/1827328
+
+

capture.sh

@@ -0,0 +1,26 @@
+#!/bin/bash
+
+while [[ "$#" -gt 0 ]]; do
+    case $1 in
+        -s|--serialpath) serialpath="$2"; shift ;;
+        -i|--ip) gsmtapip="$2"; shift ;;
+        -f|--filename) filename="$2"; shift ;;
+        -Q|--qcdebug) qcdebug=1 ;;
+        *) echo "unknown parameter: $1"; exit 1 ;;
+    esac
+    shift
+done
+
+#killall tcpdump
+
+if [ -n "$gsmtapip" ]; then
+tcpdump -i any udp port 4729 and dst $gsmtapip -w $filename &
+tcpdump_pid=$!
+./src/osmo-qcdiag-log -s $serialpath -G -i $gsmtapip
+else
+tcpdump -i any udp port 4729 -w $filename &
+tcpdump_pid=$!
+./src/osmo-qcdiag-log -s $serialpath -G
+fi
+
+kill -9 $tcpdump_pid

src/Makefile

@@ -2,7 +2,7 @@ CPPFLAGS ?= -g -O0 -Wall `pkg-config --cflags libosmocore` `pkg-config --cflags
 LIBS ?= `pkg-config --libs libosmocore` `pkg-config --libs qmi-glib`
 all: osmo-qcdiag-log
 
-MODS_LOG = gprs_l1.c gprs_rlc.o gprs_mac.o diag_gsm.o diag_log.o diag_log_gsm.o diag_log_gprs.o diag_log_wcdma.o diag_log_umts.o diag_log_qmi.o diag_dpl.o diag_log_simcard.o diag_event.o
+MODS_LOG = gprs_l1.c gprs_rlc.o gprs_mac.o diag_gsm.o diag_log.o diag_log_gsm.o diag_log_gprs.o diag_log_wcdma.o diag_log_umts.o diag_log_lte.o diag_log_qmi.o diag_dpl.o diag_log_simcard.o diag_event.o
 
 osmo-qcdiag-log: diagchar_hdlc.o diag_io.o osmo-qcdiag-log.o diag_msg.o protocol.o diag_cmd.o $(MODS_LOG)
 	$(CC) $(CPPFLAGS) -o $@ $^ $(LIBS)

src/diag_cmd.c

@@ -51,8 +51,7 @@ int diag_process_msg(struct diag_instance *di, struct msgb *msg)
 	uint8_t cmd = msg->l2h[0];
 
 	if (di->gsmtap) {
-		gsmtap_send_ex(di->gsmtap, GSMTAP_TYPE_QC_DIAG, 0, 0, 0,
-				0, 0, 0, 0, msgb_l2(msg), msgb_l2len(msg));
+//		process(di, msgb_l2(msg), msgb_l2len(msg));
 	}
 
 	switch (cmd) {
@@ -75,6 +74,8 @@ int diag_process_msg(struct diag_instance *di, struct msgb *msg)
 			msgb_free(msg);
 			return 1;
 		} else {
+				/* swallow config messages */
+				if(msg->l2h[0] != DIAG_LOG_CONFIG_F && msg->l2h[0] != DIAG_EXT_MSG_CONFIG_F)
 			printf("Got %d bytes data of unknown payload type 0x%02x: %s\n",
 				msgb_length(msg), msg->l2h[0],
 				osmo_hexdump(msgb_data(msg), msgb_length(msg)));

src/diag_dpl.c

@@ -120,7 +120,7 @@ int diag_dpl_set_if_log(struct diag_instance *di, uint8_t iface_id,
 /* LOG_DATA_PROTOCOL_LOGGING_C must be enabled */
 /* dpli_log_full_packet() used to log packet; maximum of 256 bytes per
  * diag message; DPLI_BUILD_LOG_PACKET */
-static void handle_pcap_msg(struct log_hdr *lh, struct msgb *msg)
+static void handle_pcap_msg(struct diag_instance *di, struct log_hdr *lh, struct msgb *msg)
 {
 	struct dpl_hdr *dh = (struct dpl_hdr *) msgb_data(msg);
 	printf("PCAP(fl=0x%02x, ifn=0x%02x, prot=0x%02x, inst=%u, seq=%u, seg=%u): %s\n",
@@ -133,7 +133,9 @@ static const struct diag_log_dispatch_tbl log_tbl[] = {
 	{ L1X(LOG_DATA_PROTOCOL_LOGGING_C), handle_pcap_msg },
 };
 
+#if 0
 static __attribute__((constructor)) void on_dso_load_gsm(void)
 {
 	diag_log_reg_dispatch(log_tbl, ARRAY_SIZE(log_tbl));
 }
+#endif

src/diag_event.c

@@ -152,8 +152,9 @@ static void diag_rx_event_report_f(struct diag_instance *di, struct msgb *msg)
 static const struct diag_cmd_dispatch_tbl cmd_tbl[] = {
 	{ DIAG_EVENT_REPORT_F, diag_rx_event_report_f },
 };
-
+#if 0
 static __attribute__((constructor)) void on_dso_load_event(void)
 {
 	diag_cmd_reg_dispatch(cmd_tbl, ARRAY_SIZE(cmd_tbl));
 }
+#endif

src/diag_io.c

@@ -94,18 +94,29 @@ int diag_transmit_buf(struct diag_instance *di, const uint8_t *data, size_t data
 
 struct msgb *diag_read_msg(struct diag_instance *di)
 {
-	uint8_t buf[DIAG_MAX_HDLC_BUF_SIZE];
+	static uint8_t buf[DIAG_MAX_HDLC_BUF_SIZE];
 	struct diag_hdlc_decode_type hdlc_decode;
 	struct msgb *msg;
-	int rc;
+	int rc = 0;
+	static int buf_items_left = 0;
 
+//	printf("--\nRx dump pre: %s\n", osmo_hexdump(buf, buf_items_left));
 	/* read raw data into buffer */
-	rc = read(di->fd, buf, sizeof(buf));
-	if (rc <= 0) {
-		fprintf(stderr, "Short read!\n");
-		exit(1);
+
+	if(!buf_items_left) {
+		rc = read(di->fd, buf+buf_items_left, sizeof(buf)-buf_items_left);
+		if (rc <= 0) {
+			fprintf(stderr, "Short read! %d -> exiting...\n", rc);
+			exit(1);
+		}
 	}
 
+	unsigned int read_total = rc + buf_items_left;
+	unsigned int read_current = rc;
+
+//	printf("1 Rx dump total: %s\n", osmo_hexdump(buf, read_total));
+	fflush(stdout);
+
 	if (!di->rx.msg) {
 		di->rx.msg = msgb_alloc(DIAG_MAX_REQ_SIZE, "DIAG Rx");
 		di->rx.msg->l2h = di->rx.msg->tail;
@@ -116,12 +127,27 @@ struct msgb *diag_read_msg(struct diag_instance *di)
 	hdlc_decode.dest_ptr = msg->tail;
 	hdlc_decode.dest_size = msgb_tailroom(msg);
 	hdlc_decode.src_ptr = buf;
-	hdlc_decode.src_size = rc;
+	hdlc_decode.src_size = read_total;
 	hdlc_decode.src_idx = 0;
 	hdlc_decode.dest_idx = 0;
 
+
 	rc = diag_hdlc_decode(&hdlc_decode);
 
+
+	int consumed = hdlc_decode.dest_idx;
+	int current_remaining_in_buf = read_total-consumed;
+//	printf("rx %d->%d, rem %d %u \n", read_total, consumed, current_remaining_in_buf, hdlc_decode.src_idx-hdlc_decode.dest_idx);
+
+	if(hdlc_decode.dest_idx < read_total) {
+		memmove(buf, buf+consumed, current_remaining_in_buf);
+		buf_items_left = current_remaining_in_buf;
+	} else
+		buf_items_left = 0;
+
+//	printf("2 Rx dump total: %s\n", osmo_hexdump(buf, buf_items_left));
+
+
 	if (msgb_length(msg) + hdlc_decode.dest_idx > DIAG_MAX_REQ_SIZE) {
 		fprintf(stderr, "Dropping packet. pkt_size: %d, max: %d\n",
 			msgb_length(msg) + hdlc_decode.dest_idx,
@@ -142,8 +168,9 @@ struct msgb *diag_read_msg(struct diag_instance *di)
 		rc = crc_check(msgb_data(msg), msgb_length(msg));
 		if (rc) {
 			fprintf(stderr, "Bad CRC, dropping packet\n");
-			//msgb_free(msg);
-			//return NULL;
+			printf("Rx broken crc: %s\n", msgb_hexdump(msg));
+			msgb_free(msg);
+			return NULL;
 		}
 		msgb_get(msg, HDLC_FOOTER_LEN);
 

src/diag_io.h

@@ -6,6 +6,12 @@
 
 #define DIAG_INST_F_HEXDUMP	0x00000001
 #define DIAG_INST_F_GSMTAP_DIAG	0x00000002
+#define DIAG_INST_F_GSMTAP_DECODED	0x00000004
+
+
+#define DIAG_INST_RAT_2G	0x00000001
+#define DIAG_INST_RAT_3G	0x00000002
+#define DIAG_INST_RAT_4G	0x00000004
 
 struct diag_instance {
 	int fd;
@@ -16,6 +22,12 @@ struct diag_instance {
 	} tx;
 	struct gsmtap_inst *gsmtap;
 	uint32_t flags;
+	uint32_t gsm_arfcn;
+	uint32_t umts_arfcn_ul;
+	uint32_t umts_arfcn_dl;
+	uint32_t lte_arfcn_ul;
+	uint32_t lte_arfcn_dl;
+	uint32_t rat_type; /* tracking for 2g NAS */
 };
 
 struct msgb *msgb_alloc_diag(void);

src/diag_log.c

@@ -43,6 +43,12 @@ struct diag_log_config_req_hdr {
 	uint32_t	operation;
 } __attribute((packed));
 
+struct diag_log_config_get_ranges_rsp {
+	struct diag_log_config_req_hdr hdr;
+	uint32_t	status;
+	uint32_t	last_valid_num[16];
+} __attribute((packed));
+
 struct diag_log_config_set_mask {
 	struct diag_log_config_req_hdr hdr;
 	uint32_t	equip_id;
@@ -50,6 +56,8 @@ struct diag_log_config_set_mask {
 	uint8_t		data[0];
 } __attribute((packed));
 
+static struct diag_log_config_get_ranges_rsp msg_ranges;
+
 struct msgb *gen_log_config_set_mask(uint32_t equip_id, uint32_t last_item)
 {
 	struct msgb *msg = msgb_alloc(DIAG_MAX_REQ_SIZE, "Diag Tx");
@@ -66,7 +74,20 @@ struct msgb *gen_log_config_set_mask(uint32_t equip_id, uint32_t last_item)
 	return msg;
 }
 
-int log_config_set_mask_bit(struct msgb *msg, uint32_t bit_in)
+struct msgb *gen_log_config_get_ranges()
+{
+	struct msgb *msg = msgb_alloc(DIAG_MAX_REQ_SIZE, "Diag Tx");
+	struct diag_log_config_req_hdr *dlcsm;
+
+	msg->l2h = msgb_put(msg, sizeof(*dlcsm));
+	dlcsm = (struct diag_log_config_req_hdr *) msg->l2h;
+	dlcsm->msg_type = DIAG_LOG_CONFIG_F;
+	dlcsm->operation = LOG_CONFIG_RETRIEVE_ID_RANGES_OP;
+
+	return msg;
+}
+
+int log_config_set_mask_bit(struct msgb *msg, uint32_t bit_in, uint8_t value)
 {
 	struct diag_log_config_set_mask *dlcsm;
 	dlcsm = (struct diag_log_config_set_mask *) msg->l2h;
@@ -79,7 +100,7 @@ int log_config_set_mask_bit(struct msgb *msg, uint32_t bit_in)
 		return -1;
 	}
 
-	mask[byte] |= (1 << bit);
+	mask[byte] |= value ? (1 << bit) : 0;
 
 	return 0;
 }
@@ -102,40 +123,48 @@ void diag_log_reg_dispatch(const struct diag_log_dispatch_tbl *tbl, unsigned int
 	}
 }
 
-void diag_log_enable_all_supported_family(struct diag_instance *di, uint8_t family)
+void diag_log_set_all_supported_family(struct diag_instance *di, uint8_t family, uint8_t bitvalue)
 {
 	struct msgb *msg, *rx;
 	unsigned int i, size;
 	unsigned int family_base = (family & 0xf) << 12;
-	unsigned int max = 0;
+//	unsigned int max = 0;
+	family = family & 0xF;
 
-	for (i = family_base; i < family_base + 0x1000; i++) {
-		if (log_handlers[i]) {
-			if (max < i)
-				max = i;
-		}
-	}
+//	for (i = family_base; i < family_base + 0x1000; i++) {
+//		if (log_handlers[i]) {
+//			if (max < i)
+//				max = i;
+//		}
+//	}
+
+//	if (family == 1)
+//		max = 0x1586;
+
+//	if (!max)
+//		return;
 
-	if (family == 1)
-		max = 0x1586;
+//	size = max - family_base;
 
-	if (!max)
+	size = msg_ranges.last_valid_num[family];
+	if(!size) {
+		printf("family %u: size 0, skipping...\n", family);
 		return;
+	}
 
-	size = max - family_base;
 	printf("family %u: allocating log mask of size %u\n", family, size);
 	msg = gen_log_config_set_mask(family, size);
 	for (i = family_base; i < family_base + 0x1000; i++) {
 		if (log_handlers[i])
-			log_config_set_mask_bit(msg, i-family_base);
+			log_config_set_mask_bit(msg, i-family_base, bitvalue);
 	}
 
-	if (family == 1) {
-		for (i = 0x572; i < 0x585; i++) {
-			printf("Setting log 0x%04x\n", i);
-			log_config_set_mask_bit(msg, i);
-		}
-	}
+//	if (family == 1) {
+//		for (i = 0x572; i < 0x585; i++) {
+//			printf("Setting log 0x%04x\n", i);
+//			log_config_set_mask_bit(msg, i);
+//		}
+//	}
 
 	rx = diag_transceive_msg(di, msg);
 	if (rx->l2h[0] != DIAG_LOG_CONFIG_F)
@@ -145,11 +174,35 @@ void diag_log_enable_all_supported_family(struct diag_instance *di, uint8_t fami
 }
 
 void diag_log_enable_all_supported(struct diag_instance *di)
+{
+	unsigned int i;
+	struct msgb *msg, *rx;
+	struct diag_log_config_get_ranges_rsp *res;
+
+	/* max bit num not yet received */
+	if(msg_ranges.hdr.msg_type != DIAG_LOG_CONFIG_F) {
+		msg = gen_log_config_get_ranges();
+		rx = diag_transceive_msg(di, msg);
+		res = (struct diag_log_config_get_ranges_rsp *) (msgb_data(rx));
+		if(sizeof(struct diag_log_config_get_ranges_rsp) == msgb_length(rx))
+			msg_ranges = *res;
+	}
+	for (i = 0; i < 0xF; i++) {
+			printf("max size for family %x: %u\n", i, res->last_valid_num[i]);
+	}
+
+	for (i = 0; i < 0xF; i++) {
+		diag_log_set_all_supported_family(di, i, 1);
+	}
+}
+
+
+void diag_log_disable_all_supported(struct diag_instance *di)
 {
 	unsigned int i;
 
 	for (i = 0; i < 0xF; i++) {
-		diag_log_enable_all_supported_family(di, i);
+		diag_log_set_all_supported_family(di, i, 0);
 	}
 }
 
@@ -159,23 +212,43 @@ void diag_log_handle(struct diag_instance *di, struct msgb *msg)
 	struct log_hdr *lh;
 
 	dlh = (struct diag_log_hdr *) msg->data;
-	/* FIXME: verify length */
-	msg->l3h = msgb_pull(msg, sizeof(*dlh));
 
-	lh = (struct log_hdr *) msg->l3h;
-	/* FIXME: verify length */
-	msgb_pull(msg, sizeof(*lh));
+	if (sizeof(struct diag_log_hdr) < msgb_length(msg))
+		msg->l3h = msgb_pull(msg, sizeof(*dlh));
+	else {
+		printf("size mismatch! %d\n", __LINE__);
+		return;
+	}
+
+	if (dlh->len != msgb_length(msg)) {
+		printf("size mismatch! %d\n", __LINE__);
+		return;
+	}
+
+	lh = (struct log_hdr *)msg->l3h;
+
+	if (sizeof(struct log_hdr) < msgb_length(msg))
+		msgb_pull(msg, sizeof(*lh));
+	else {
+		printf("size mismatch! %d\n", __LINE__);
+		return;
+	}
+
+	if (lh->len != (msgb_length(msg) + sizeof(struct log_hdr))) {
+		printf("size mismatch! %d %d:%d\n", __LINE__, lh->len, msgb_length(msg));
+		return;
+	}
 
-	printf("LOG(0x%04x|%u|%u): ", lh->code,
-		diag_ts_to_epoch(lh->ts), diag_ts_to_fn(lh->ts));
+//	printf("LOG(0x%04x|%u|%u): ", lh->code,
+//		diag_ts_to_epoch(lh->ts), diag_ts_to_fn(lh->ts));
 
 	if (log_handlers[lh->code])
-		log_handlers[lh->code](lh, msg);
+		log_handlers[lh->code](di, lh, msg);
 	else
 		printf("%s\n", osmo_hexdump(lh->data, lh->len));
 }
 
-void diag_log_hdl_default(struct log_hdr *lh, struct msgb *msg)
+void diag_log_hdl_default(struct diag_instance *di, struct log_hdr *lh, struct msgb *msg)
 {
 	printf("%s\n", osmo_hexdump(lh->data, lh->len));
 }

src/diag_log.h

@@ -3,7 +3,7 @@
 #include "diag_io.h"
 #include "protocol/protocol.h"
 
-typedef void diag_log_handler(struct log_hdr *lh, struct msgb *msg);
+typedef void diag_log_handler(struct diag_instance *di, struct log_hdr *lh, struct msgb *msg);
 
 struct diag_log_dispatch_tbl {
 	uint16_t code;
@@ -16,11 +16,12 @@ void diag_log_handle(struct diag_instance *di, struct msgb *msg);
 /* called by individual modules to register their own decoders */
 void diag_log_reg_dispatch(const struct diag_log_dispatch_tbl *tbl, unsigned int size);
 
-void diag_log_enable_all_supported_family(struct diag_instance *di, uint8_t family);
+void diag_log_set_all_supported_family(struct diag_instance *di, uint8_t family, uint8_t bitvalue);
 void diag_log_enable_all_supported(struct diag_instance *di);
+void diag_log_disable_all_supported(struct diag_instance *di);
 
 /* functions for log configuration */
 struct msgb *gen_log_config_set_mask(uint32_t equip_id, uint32_t last_item);
-int log_config_set_mask_bit(struct msgb *msg, uint32_t bit_in);
+int log_config_set_mask_bit(struct msgb *msg, uint32_t bit_in, uint8_t enabled);
 
-void diag_log_hdl_default(struct log_hdr *lh, struct msgb *msg);
+void diag_log_hdl_default(struct diag_instance *di, struct log_hdr *lh, struct msgb *msg);

src/diag_log_gprs.c

@@ -18,6 +18,10 @@
 
 #include <stdio.h>
 
+#include <osmocom/core/gsmtap.h>
+#include <osmocom/core/gsmtap_util.h>
+#include <osmocom/core/utils.h>
+
 #include "diag_log.h"
 #include "protocol/diag_log_gsm.h"
 #include "protocol/diag_log_gprs_rlc.h"
@@ -25,7 +29,7 @@
 #include "protocol/diag_log_gprs_l1.h"
 
 
-static void handle_grr_state_msg(struct log_hdr *lh, struct msgb *msg)
+static void handle_grr_state_msg(struct diag_instance *di, struct log_hdr *lh, struct msgb *msg)
 {
 	struct diag_gprs_grr_state *rrs = (struct diag_gprs_grr_state *) msgb_data(msg);
 
@@ -33,7 +37,7 @@ static void handle_grr_state_msg(struct log_hdr *lh, struct msgb *msg)
 		get_value_string(diag_gprs_grr_st_vals, rrs->grr_state));
 }
 
-static void handle_rlc_ul_abnrml_rls(struct log_hdr *lh, struct msgb *msg)
+static void handle_rlc_ul_abnrml_rls(struct diag_instance *di, struct log_hdr *lh, struct msgb *msg)
 {
 	struct gprs_rlc_ul_abnrml_rls_counts *arc;
 	arc = (struct gprs_rlc_ul_abnrml_rls_counts *) msgb_data(msg);
@@ -42,17 +46,57 @@ static void handle_rlc_ul_abnrml_rls(struct log_hdr *lh, struct msgb *msg)
 		arc->access_reject_cnt, arc->arc_retry_cnt, arc->arc_wo_retry_cnt, arc->arc_sys_info_cnt);
 }
 
-static void handle_mac_sign_msg(struct log_hdr *lh, struct msgb *msg)
+static void handle_mac_sign_msg(struct diag_instance *di, struct log_hdr *lh, struct msgb *msg)
 {
+	uint32_t ctype = 0;
+	uint16_t arfcn = di->gsm_arfcn;
+	uint8_t *data;
+	unsigned int len;
 	struct gprs_mac_signalling_msg *msm;
 	msm = (struct gprs_mac_signalling_msg *) msgb_data(msg);
-	printf("MAC-SIGN-MSG { chan_type=%s, msg_type=%s, msg=%s }\n",
-		get_value_string(gprs_mac_chan_type_vals, msm->chan_type),
-		get_value_string(gprs_mac_msg_type_vals, msm->msg_type),
-			osmo_hexdump(msm->msg, msm->msg_len));
+	di->rat_type = DIAG_INST_RAT_2G;
+//	printf("MAC-SIGN-MSG { chan_type=%s, msg_type=%s, msg=%s }\n",
+//		get_value_string(gprs_mac_chan_type_vals, msm->chan_type),
+//		get_value_string(gprs_mac_msg_type_vals, msm->msg_type),
+//			osmo_hexdump(msm->msg, msm->msg_len));
+
+	if(msm->msg_type == PACKET_CHANNEL_REQUEST)
+		return;
+	if(msm->chan_type == 255)
+		return;
+
+	switch(msm->chan_type & 0x7f){
+		case PACCH_RRBP_CHANNEL:
+		case UL_PACCH_CHANNEL:
+		case DL_PACCH_CHANNEL:
+			ctype = GSMTAP_CHANNEL_PACCH;
+			break;
+		default:
+			printf("Unhandled MAC-SIGN-MSG: %s  %u: %s\n",
+				get_value_string(gprs_mac_chan_type_vals, msm->chan_type & 0x7f),
+				msm->msg_len,
+				osmo_hexdump(msgb_data(msg), msm->msg_len));
+			return;
+	}
+
+	arfcn = msm->chan_type & 0x80 ? arfcn: arfcn | GSMTAP_ARFCN_F_UPLINK;
+
+	data = msm->msg;
+	len = msm->msg_len;
+
+	/* prepend flag */
+	data--;
+	*data = 1 << 6; /* PAYLOAD_TYPE_CTRL_NO_OPT_OCTET */
+	len++;
+
+
+	/* abis due to no lapdm header */
+	if (di->gsmtap && di->flags & DIAG_INST_F_GSMTAP_DECODED) {
+		gsmtap_send_ex(di->gsmtap, GSMTAP_TYPE_UM, arfcn, 0, ctype, 0, 0, 0, 0, data, len);
+	}
 }
 
-static void handle_llc_me_info(struct log_hdr *lh, struct msgb *msg)
+static void handle_llc_me_info(struct diag_instance *di, struct log_hdr *lh, struct msgb *msg)
 {
 	struct diag_gprs_llme_info *gli;
 	gli = (struct diag_gprs_llme_info *) msgb_data(msg);
@@ -62,7 +106,7 @@ static void handle_llc_me_info(struct log_hdr *lh, struct msgb *msg)
 		osmo_hexdump_nospc(gli->enc_key, sizeof(gli->enc_key)));
 }
 
-static void handle_llc_xid_info(struct log_hdr *lh, struct msgb *msg)
+static void handle_llc_xid_info(struct diag_instance *di, struct log_hdr *lh, struct msgb *msg)
 {
 	struct diag_gprs_llc_xid_info *glxi;
 
@@ -82,7 +126,7 @@ static void handle_llc_xid_info(struct log_hdr *lh, struct msgb *msg)
 }
 
 
-static void handle_llc_pdu_stats(struct log_hdr *lh, struct msgb *msg)
+static void handle_llc_pdu_stats(struct diag_instance *di, struct log_hdr *lh, struct msgb *msg)
 {
 	struct diag_gprs_llc_stats *gls;
 	gls = (struct diag_gprs_llc_stats *) msgb_data(msg);
@@ -94,7 +138,7 @@ static void handle_llc_pdu_stats(struct log_hdr *lh, struct msgb *msg)
 		gls->llpdu_tx, gls->llpdu_rx, gls->llpdu_fcs_err, gls->llpdu_frm_rej, gls->llpdu_tlli_err, gls->llpdu_addr_err, gls->llpdu_short_err);
 }
 
-static void handle_mac_state(struct log_hdr *lh, struct msgb *msg)
+static void handle_mac_state(struct diag_instance *di, struct log_hdr *lh, struct msgb *msg)
 {
 	struct gprs_mac_state_change *msc;
 	msc = (struct gprs_mac_state_change *) msgb_data(msg);
@@ -116,7 +160,7 @@ static void handle_mac_state(struct log_hdr *lh, struct msgb *msg)
 		get_value_string(gprs_mac_mode_vals, msc->mac_mode), name);
 }
 
-static void handle_mac_dl_tbf_est(struct log_hdr *lh, struct msgb *msg)
+static void handle_mac_dl_tbf_est(struct diag_instance *di, struct log_hdr *lh, struct msgb *msg)
 {
 	struct gprs_mac_dl_tbf_est *dte;
 	dte = (struct gprs_mac_dl_tbf_est *) msgb_data(msg);
@@ -125,7 +169,7 @@ static void handle_mac_dl_tbf_est(struct log_hdr *lh, struct msgb *msg)
 		dte->dl_tfi, dte->rlc_mode, dte->dl_ts_bmap, dte->is_egprs_tbf, dte->egprs_win_size, dte->egprs_link_qual_mode, dte->bep_period2);
 }
 
-static void handle_mac_ul_tbf_est(struct log_hdr *lh, struct msgb *msg)
+static void handle_mac_ul_tbf_est(struct diag_instance *di, struct log_hdr *lh, struct msgb *msg)
 {
 	struct gprs_mac_ul_tbf_est *ute;
 	ute = (struct gprs_mac_ul_tbf_est *) msgb_data(msg);
@@ -134,7 +178,7 @@ static void handle_mac_ul_tbf_est(struct log_hdr *lh, struct msgb *msg)
 		ute->tbf_req_cause, ute->acc_granted, ute->radio_prio, ute->peak_tput, ute->ul_tfi, ute->rlc_mode, ute->ul_ts_bmap, ute->is_egprs_tbf, ute->egprs_win_size, ute->resegment, ute->bep_period2);
 }
 
-static void handle_mac_dl_tbf_rel(struct log_hdr *lh, struct msgb *msg)
+static void handle_mac_dl_tbf_rel(struct diag_instance *di, struct log_hdr *lh, struct msgb *msg)
 {
 	struct gprs_mac_tbf_release *tr;
 	tr = (struct gprs_mac_tbf_release *) msgb_data(msg);
@@ -142,7 +186,7 @@ static void handle_mac_dl_tbf_rel(struct log_hdr *lh, struct msgb *msg)
 	printf("MAC-DL-TBF-REL { tfi=%u, fail_cause=%u }\n", tr->tfi, tr->fail_cause);
 }
 
-static void handle_mac_ul_tbf_rel(struct log_hdr *lh, struct msgb *msg)
+static void handle_mac_ul_tbf_rel(struct diag_instance *di, struct log_hdr *lh, struct msgb *msg)
 {
 	struct gprs_mac_tbf_release *tr;
 	tr = (struct gprs_mac_tbf_release *) msgb_data(msg);
@@ -150,7 +194,7 @@ static void handle_mac_ul_tbf_rel(struct log_hdr *lh, struct msgb *msg)
 	printf("MAC-DL-TBF-REL { tfi=%u, fail_cause=%u }\n", tr->tfi, tr->fail_cause);
 }
 
-static void handle_mac_dl_acknack(struct log_hdr *lh, struct msgb *msg)
+static void handle_mac_dl_acknack(struct diag_instance *di, struct log_hdr *lh, struct msgb *msg)
 {
 	struct gprs_mac_dl_acknack *da;
 	da = (struct gprs_mac_dl_acknack *) msgb_data(msg);
@@ -159,7 +203,7 @@ static void handle_mac_dl_acknack(struct log_hdr *lh, struct msgb *msg)
 		osmo_hexdump(msgb_data(msg)+1, msgb_length(msg)-1));
 }
 
-static void handle_mac_ul_acknack(struct log_hdr *lh, struct msgb *msg)
+static void handle_mac_ul_acknack(struct diag_instance *di, struct log_hdr *lh, struct msgb *msg)
 {
 	//struct gprs_mac_ul_acknack *ua;
 	//ua = (struct gprs_mac_ul_acknack *) msgb_data(msg);
@@ -168,7 +212,7 @@ static void handle_mac_ul_acknack(struct log_hdr *lh, struct msgb *msg)
 		osmo_hexdump(msgb_data(msg), msgb_length(msg)));
 }
 
-static void handle_rlc_ul_evt_cnt(struct log_hdr *lh, struct msgb *msg)
+static void handle_rlc_ul_evt_cnt(struct diag_instance *di, struct log_hdr *lh, struct msgb *msg)
 {
 	struct gprs_rlc_ul_event_counts *uec;
 	uec = (struct gprs_rlc_ul_event_counts *) msgb_data(msg);
@@ -177,7 +221,7 @@ static void handle_rlc_ul_evt_cnt(struct log_hdr *lh, struct msgb *msg)
 		uec->llc_event_cnt, uec->mac_event_cnt, uec->pl1_event_cnt, uec->tmr_event_cnt);
 }
 
-static void handle_rlc_ul_stats(struct log_hdr *lh, struct msgb *msg)
+static void handle_rlc_ul_stats(struct diag_instance *di, struct log_hdr *lh, struct msgb *msg)
 {
 	struct gprs_rlc_ul_stats *uls;
 	uls = (struct gprs_rlc_ul_stats *) msgb_data(msg);
@@ -187,7 +231,7 @@ static void handle_rlc_ul_stats(struct log_hdr *lh, struct msgb *msg)
 		get_value_string(gprs_rlc_ul_substate_vals, uls->rlc_ul_substate));
 }
 
-static void handle_rlc_dl_stats(struct log_hdr *lh, struct msgb *msg)
+static void handle_rlc_dl_stats(struct diag_instance *di, struct log_hdr *lh, struct msgb *msg)
 {
 	struct gprs_rlc_dl_stats *dls;
 	dls = (struct gprs_rlc_dl_stats *) msgb_data(msg);
@@ -196,7 +240,7 @@ static void handle_rlc_dl_stats(struct log_hdr *lh, struct msgb *msg)
 		get_value_string(gprs_rlc_dl_state_vals, dls->rlc_dl_state));
 }
 
-static void handle_rlc_ul_header(struct log_hdr *lh, struct msgb *msg)
+static void handle_rlc_ul_header(struct diag_instance *di, struct log_hdr *lh, struct msgb *msg)
 {
 	struct gprs_rlc_ul_header *ulh;
 	ulh = (struct gprs_rlc_ul_header *) msgb_data(msg);
@@ -206,7 +250,7 @@ static void handle_rlc_ul_header(struct log_hdr *lh, struct msgb *msg)
 		osmo_hexdump(ulh->ul_hdr, sizeof(ulh->ul_hdr)));
 }
 
-static void handle_rlc_rel(struct log_hdr *lh, struct msgb *msg)
+static void handle_rlc_rel(struct diag_instance *di, struct log_hdr *lh, struct msgb *msg)
 {
 	struct gprs_rlc_release_ind *rli;
 	rli = (struct gprs_rlc_release_ind *) msgb_data(msg);
@@ -218,12 +262,26 @@ static void handle_rlc_rel(struct log_hdr *lh, struct msgb *msg)
 	printf("RLC-%cL-RELEASE { tfi=%u, cause=%u }\n", ud, rli->tfi, rli->cause);
 }
 
-static void handle_gmm_ota_msg(struct log_hdr *lh, struct msgb *msg)
-{
-	printf("GMM-OTA-MESSAGE { FIXME }\n");
+static void handle_gmm_ota_msg(struct diag_instance *di, struct log_hdr *lh, struct msgb *msg)
+{	uint32_t ctype = 0;
+	uint16_t arfcn = di->gsm_arfcn;
+	uint8_t *data;
+	unsigned int len;
+	struct gprs_sm_gmm_ota_msg *msm;
+	msm = (struct gprs_sm_gmm_ota_msg *) msgb_data(msg);
+	di->rat_type = DIAG_INST_RAT_2G;
+
+	arfcn = msm->chan_type ? arfcn: arfcn | GSMTAP_ARFCN_F_UPLINK;
+
+	data = msm->msg;
+	len = msm->msg_len;
+
+	if (di->gsmtap && di->flags & DIAG_INST_F_GSMTAP_DECODED) {
+		gsmtap_send_ex(di->gsmtap, GSMTAP_TYPE_ABIS, arfcn, 0, ctype, 0, 0, 0, 0, data, len);
+	}
 }
 
-static void handle_ul_acknack_v2(struct log_hdr *lh, struct msgb *msg)
+static void handle_ul_acknack_v2(struct diag_instance *di, struct log_hdr *lh, struct msgb *msg)
 {
 	struct gprs_rlc_ul_acknack_params_v2 *ula;
 	ula = (struct gprs_rlc_ul_acknack_params_v2 *) msgb_data(msg);
@@ -233,7 +291,7 @@ static void handle_ul_acknack_v2(struct log_hdr *lh, struct msgb *msg)
 		ula->countdown_val, ula->va, ula->vs, ula->stall_ind, ula->rrb_high32, ula->rrb_low32);
 }
 
-static void handle_dl_acknack_v2(struct log_hdr *lh, struct msgb *msg)
+static void handle_dl_acknack_v2(struct diag_instance *di, struct log_hdr *lh, struct msgb *msg)
 {
 	struct gprs_rlc_dl_acknack_params_v2 *dla;
 	dla = (struct gprs_rlc_dl_acknack_params_v2 *) msgb_data(msg);
@@ -243,7 +301,7 @@ static void handle_dl_acknack_v2(struct log_hdr *lh, struct msgb *msg)
 		get_value_string(gprs_coding_schemes, dla->coding_scheme), dla->rrb_high32, dla->rrb_low32);
 }
 
-static void handle_tx_sched_res(struct log_hdr *lh, struct msgb *msg)
+static void handle_tx_sched_res(struct diag_instance *di, struct log_hdr *lh, struct msgb *msg)
 {
 	struct gprs_tx_sched_res *tsr;
 	tsr = (struct gprs_tx_sched_res *) msgb_data(msg);
@@ -264,7 +322,7 @@ static void handle_tx_sched_res(struct log_hdr *lh, struct msgb *msg)
 	printf(" ] }\n");
 }
 
-static void handle_gprs_power_control(struct log_hdr *lh, struct msgb *msg)
+static void handle_gprs_power_control(struct diag_instance *di, struct log_hdr *lh, struct msgb *msg)
 {
 	struct gprs_power_control *gpc;
 	gpc = (struct gprs_power_control *) msgb_data(msg);
@@ -275,7 +333,7 @@ static void handle_gprs_power_control(struct log_hdr *lh, struct msgb *msg)
 		gpc->alpha, gpc->derived_c, gpc->pmax);
 }
 
-static void handle_gprs_xfer_sum(struct log_hdr *lh, struct msgb *msg)
+static void handle_gprs_xfer_sum(struct diag_instance *di, struct log_hdr *lh, struct msgb *msg)
 {
 	struct gprs_xfer_sum *gxs;
 	gxs = (struct gprs_xfer_sum *) msgb_data(msg);
@@ -285,7 +343,7 @@ static void handle_gprs_xfer_sum(struct log_hdr *lh, struct msgb *msg)
 		gxs->dl_ptcch_ts, gxs->ta, gxs->usf_granularity, gxs->ul_bitmap_tn, gxs->dl_bitmap_tn);
 }
 
-static void handle_gprs_aif_sum(struct log_hdr *lh, struct msgb *msg)
+static void handle_gprs_aif_sum(struct diag_instance *di, struct log_hdr *lh, struct msgb *msg)
 {
 	struct gprs_air_if_summary *gaifs;
 	gaifs = (struct gprs_air_if_summary *) msgb_data(msg);
@@ -294,7 +352,7 @@ static void handle_gprs_aif_sum(struct log_hdr *lh, struct msgb *msg)
 		gaifs->fn, gaifs->band_ind, gaifs->dl_ts, gaifs->ul_ts, gaifs->rx_power);
 }
 
-static void handle_gprs_rx_msg_metrics_a_v2(struct log_hdr *lh, struct msgb *msg)
+static void handle_gprs_rx_msg_metrics_a_v2(struct diag_instance *di, struct log_hdr *lh, struct msgb *msg)
 {
 	struct gprs_rx_msg_metr_a_v2 *metr;
 	metr = (struct gprs_rx_msg_metr_a_v2 *) msgb_data(msg);
@@ -315,7 +373,7 @@ static inline uint32_t round_next_octet(uint32_t num_bits)
 	return num_bytes;
 }
 
-static void handle_egprs_rlc_epdan(struct log_hdr *lh, struct msgb *msg)
+static void handle_egprs_rlc_epdan(struct diag_instance *di, struct log_hdr *lh, struct msgb *msg)
 {
 	struct diag_egprs_rlc_epdan *epd = (struct diag_egprs_rlc_epdan *) msgb_data(msg);
 
@@ -331,6 +389,7 @@ static void handle_egprs_rlc_epdan(struct log_hdr *lh, struct msgb *msg)
 }
 
 static const struct diag_log_dispatch_tbl log_tbl[] = {
+#if 0
 	/* LLC */
 	{ GSM(LOG_GPRS_LLC_ME_INFO_C), handle_llc_me_info },		/* requested? */
 	{ GSM(LOG_GPRS_LLC_PDU_STATS_C), handle_llc_pdu_stats },	/* requested? */
@@ -351,15 +410,17 @@ static const struct diag_log_dispatch_tbl log_tbl[] = {
 	{ 0x5206, diag_log_hdl_default },
 	/* MAC */
 	{ GSM(LOG_GPRS_MAC_STATE_C), handle_mac_state },
+	#endif
 	{ GSM(LOG_GPRS_MAC_SIGNALLING_MESSAGE_C), handle_mac_sign_msg },
+	{ GSM(LOG_GPRS_SM_GMM_OTA_MESSAGE_C), handle_gmm_ota_msg },
+	#if 0
 	{ GSM(LOG_GPRS_MAC_DL_TBF_ESTABLISH_C), handle_mac_dl_tbf_est },
 	{ GSM(LOG_GPRS_MAC_UL_TBF_ESTABLISH_C), handle_mac_ul_tbf_est },
 	{ GSM(LOG_EGPRS_MAC_DL_ACKNACK_C), handle_mac_dl_acknack },
 	{ GSM(LOG_EGPRS_MAC_UL_ACKNACK_C), handle_mac_ul_acknack },
 	{ GSM(LOG_GPRS_MAC_DL_TBF_RELEASE_C), handle_mac_dl_tbf_rel },
 	{ GSM(LOG_GPRS_MAC_UL_TBF_RELEASE_C), handle_mac_ul_tbf_rel },
-	/* SM/GMM */
-	{ GSM(LOG_GPRS_SM_GMM_OTA_MESSAGE_C), handle_gmm_ota_msg },
+
 
 	/* Layer 1 */
 	{ 0x5230, diag_log_hdl_default },
@@ -383,6 +444,7 @@ static const struct diag_log_dispatch_tbl log_tbl[] = {
 	{ 0x508f, diag_log_hdl_default },
 
 	{ 0x5209, diag_log_hdl_default },
+	#endif
 };
 
 static __attribute__((constructor)) void on_dso_load_gprs(void)

src/diag_log_gsm.c

@@ -18,20 +18,66 @@
 
 #include <stdio.h>
 
+#include <osmocom/core/gsmtap.h>
+#include <osmocom/core/gsmtap_util.h>
+#include <osmocom/core/utils.h>
+
 #include "diag_log.h"
 #include "protocol/diag_log_gsm.h"
 
+struct dirty_fcch {
+	uint16_t arfcn_band;
+	/* don't care about anyhting else */
+};
 
-static void handle_rr_sig_msg(struct log_hdr *lh, struct msgb *msg)
+static void handle_rr_sig_msg(struct diag_instance *di, struct log_hdr *lh, struct msgb *msg)
 {
+	uint32_t ctype = 0;
+	uint16_t arfcn = di->gsm_arfcn;
+	uint8_t *data;
+	unsigned int len;
 	struct diag_gsm_rr_msg *rm = (struct diag_gsm_rr_msg *) msgb_data(msg);
 
-	printf("RR: %s %02x %u: %s\n",
-		get_value_string(diag_gsm_l2_chantype_vals, rm->chan_type & 0x7f),
-		rm->msg_type, rm->length, osmo_hexdump(msgb_data(msg), rm->length));
+	di->rat_type = DIAG_INST_RAT_2G;
+//	printf("RR: %s %02x %u: %s\n",
+//		get_value_string(diag_gsm_l2_chantype_vals, rm->chan_type & 0x7f),
+//		rm->msg_type, rm->length, osmo_hexdump(msgb_data(msg), rm->length));
+
+	switch(rm->chan_type & 0x7f){
+		case DIAG_GSM_L2_CHAN_TYPE_DCCH: ctype =GSMTAP_CHANNEL_SDCCH; break;
+		case DIAG_GSM_L2_CHAN_TYPE_BCCH: ctype =GSMTAP_CHANNEL_BCCH; break;
+		case DIAG_GSM_L2_CHAN_TYPE_RACH: ctype =GSMTAP_CHANNEL_RACH; break;
+		case DIAG_GSM_L2_CHAN_TYPE_CCCH: ctype =GSMTAP_CHANNEL_CCCH; break;
+		case DIAG_GSM_L2_CHAN_TYPE_SACCH: ctype =GSMTAP_CHANNEL_SDCCH | GSMTAP_CHANNEL_ACCH; break;
+		case DIAG_GSM_L2_CHAN_TYPE_SDCCH: ctype =GSMTAP_CHANNEL_SDCCH; break;
+		case DIAG_GSM_L2_CHAN_TYPE_FACCH_F: ctype =GSMTAP_CHANNEL_TCH_F | GSMTAP_CHANNEL_ACCH; break;
+		case DIAG_GSM_L2_CHAN_TYPE_FACCH_H: ctype =GSMTAP_CHANNEL_TCH_H | GSMTAP_CHANNEL_ACCH; break;
+		default:
+			printf("Unhandled RR: %s  %u: %s\n",
+				get_value_string(diag_gsm_l2_chantype_vals, rm->chan_type & 0x7f),
+				rm->length,
+				osmo_hexdump(msgb_data(msg), rm->length));
+			return;
+	}
+
+	arfcn = rm->chan_type & 0x80 ? arfcn : arfcn | GSMTAP_ARFCN_F_UPLINK;
+
+	data = rm->data;
+	len = rm->length;
+
+	/* skip l2 pseudo len */
+	if (ctype ==  GSMTAP_CHANNEL_BCCH || ctype ==  GSMTAP_CHANNEL_CCCH){
+			data++;
+			len--;
+		}
+
+	/* abis due to no lapdm header */
+	if (di->gsmtap && di->flags & DIAG_INST_F_GSMTAP_DECODED) {
+		gsmtap_send_ex(di->gsmtap, GSMTAP_TYPE_ABIS, arfcn, 0, ctype, 0, 0, 0, 0, data, len);
+	}
 }
 
-static void handle_rr_state_msg(struct log_hdr *lh, struct msgb *msg)
+static void handle_rr_state_msg(struct diag_instance *di, struct log_hdr *lh, struct msgb *msg)
 {
 	struct diag_gsm_rr_state *rrs = (struct diag_gsm_rr_state *) msgb_data(msg);
 	printf("RR-STATE { state=%s, substate=%u, status=%u, mode=%u }\n",
@@ -40,7 +86,7 @@ static void handle_rr_state_msg(struct log_hdr *lh, struct msgb *msg)
 
 }
 
-static void handle_mdsp_cmd(struct log_hdr *lh, struct msgb *msg)
+static void handle_mdsp_cmd(struct diag_instance *di, struct log_hdr *lh, struct msgb *msg)
 {
 	struct diag_mdsp_log_cmds *dmlcs = (struct diag_mdsp_log_cmds *) msgb_data(msg);
 	int i;
@@ -60,7 +106,7 @@ static void handle_mdsp_cmd(struct log_hdr *lh, struct msgb *msg)
 	printf(" ] }\n");
 }
 
-static void handle_l2_state(struct log_hdr *lh, struct msgb *msg)
+static void handle_l2_state(struct diag_instance *di, struct log_hdr *lh, struct msgb *msg)
 {
 	struct diag_gsm_l2_state *l2s = (struct diag_gsm_l2_state *) msgb_data(msg);
 
@@ -81,7 +127,7 @@ static void handle_l2_state(struct log_hdr *lh, struct msgb *msg)
 		get_value_string(diag_gsm_l2_event_vals, l2s->l2_event));
 }
 
-static void handle_l2_transm_status(struct log_hdr *lh, struct msgb *msg)
+static void handle_l2_transm_status(struct diag_instance *di, struct log_hdr *lh, struct msgb *msg)
 {
 	struct diag_gsm_l2_transm_status *lts = (struct diag_gsm_l2_transm_status *) msgb_data(msg);
 
@@ -92,12 +138,23 @@ static void handle_l2_transm_status(struct log_hdr *lh, struct msgb *msg)
 		lts->seq_err, lts->frame_type, lts->msg_entries, lts->seg_entries);
 }
 
+static void handle_arfcn(struct diag_instance *di, struct log_hdr *lh, struct msgb *msg)
+{
+	struct dirty_fcch *rm = (struct dirty_fcch *) msgb_data(msg);
+	di->gsm_arfcn = rm->arfcn_band & 0x0FFF;
+}
+
 static const struct diag_log_dispatch_tbl log_tbl[] = {
 	{ GSM(LOG_GSM_RR_SIGNALING_MESSAGE_C), handle_rr_sig_msg },
+	{ GSM(LOG_GSM_RR_CELL_INFO_C), handle_arfcn },
+	{ GSM(LOG_GSM_FCCH_DECODE_C), handle_arfcn },
+	{ GSM(LOG_GSM_SCH_DECODE_C), handle_arfcn },
+#if 0
 	{ GSM(LOG_GSM_RR_STATE_C), handle_rr_state_msg },
 	{ GSM(LOG_GSM_MDSP_CMD_C), handle_mdsp_cmd },
 	{ GSM(LOG_GSM_L2_STATE_C), handle_l2_state },
 	{ GSM(LOG_GSM_L2_TRANSMISSION_STATUS_C), handle_l2_transm_status},
+#endif
 };
 
 static __attribute__((constructor)) void on_dso_load_gsm(void)

src/diag_log_lte.c

@@ -0,0 +1,254 @@
+#include <stdio.h>
+
+#include <osmocom/core/gsmtap.h>
+#include <osmocom/core/gsmtap_util.h>
+#include <osmocom/core/utils.h>
+
+#include "diag_log.h"
+#include "protocol/diag_log_lte.h"
+
+struct sub_chan_map {
+    uint8_t is_ul;
+    uint8_t qc_id;
+    uint8_t gsmtap_lte_rrc_msg;
+};
+
+
+static struct sub_chan_map rrc_subtype_map_lt9_13_22[] = {
+    {0, 1, GSMTAP_LTE_RRC_SUB_BCCH_BCH_Message   },
+    {0, 2, GSMTAP_LTE_RRC_SUB_BCCH_DL_SCH_Message},
+    {0, 3, GSMTAP_LTE_RRC_SUB_MCCH_Message   },
+    {0, 4, GSMTAP_LTE_RRC_SUB_PCCH_Message   },
+    {0, 5, GSMTAP_LTE_RRC_SUB_DL_CCCH_Message     },
+    {0, 6, GSMTAP_LTE_RRC_SUB_DL_DCCH_Message     },
+    {1, 7, GSMTAP_LTE_RRC_SUB_UL_CCCH_Message     },
+    {1, 8, GSMTAP_LTE_RRC_SUB_UL_DCCH_Message     },
+};
+
+
+
+static struct sub_chan_map rrc_subtype_map_9to12[] = {
+	{0,8, GSMTAP_LTE_RRC_SUB_BCCH_BCH_Message      },
+	{0,9, GSMTAP_LTE_RRC_SUB_BCCH_DL_SCH_Message   },
+	{0,10, GSMTAP_LTE_RRC_SUB_MCCH_Message	      },
+	{0,11, GSMTAP_LTE_RRC_SUB_PCCH_Message	      },
+	{0,12, GSMTAP_LTE_RRC_SUB_DL_CCCH_Message      },
+	{0,13, GSMTAP_LTE_RRC_SUB_DL_DCCH_Message      },
+	{1,14, GSMTAP_LTE_RRC_SUB_UL_CCCH_Message      },
+	{1,15, GSMTAP_LTE_RRC_SUB_UL_DCCH_Message      },
+    };
+
+static struct sub_chan_map rrc_subtype_map_14_15_16[] = {
+	{0,1, GSMTAP_LTE_RRC_SUB_BCCH_BCH_Message       },
+	{0,2, GSMTAP_LTE_RRC_SUB_BCCH_DL_SCH_Message    },
+	{0,4, GSMTAP_LTE_RRC_SUB_MCCH_Message	       },
+	{0,5, GSMTAP_LTE_RRC_SUB_PCCH_Message	       },
+	{0,6, GSMTAP_LTE_RRC_SUB_DL_CCCH_Message       },
+	{0,7, GSMTAP_LTE_RRC_SUB_DL_DCCH_Message       },
+	{1,8, GSMTAP_LTE_RRC_SUB_UL_CCCH_Message       },
+	{1,9, GSMTAP_LTE_RRC_SUB_UL_DCCH_Message       },
+    };
+
+
+static struct sub_chan_map rrc_subtype_map_19_26[] = {
+	{0,1, GSMTAP_LTE_RRC_SUB_BCCH_BCH_Message	},
+	{0,3, GSMTAP_LTE_RRC_SUB_BCCH_DL_SCH_Message	},
+	{0,6, GSMTAP_LTE_RRC_SUB_MCCH_Message		},
+	{0,7, GSMTAP_LTE_RRC_SUB_PCCH_Message		},
+	{0,8, GSMTAP_LTE_RRC_SUB_DL_CCCH_Message	},
+	{0,9, GSMTAP_LTE_RRC_SUB_DL_DCCH_Message	},
+	{1,10,GSMTAP_LTE_RRC_SUB_UL_CCCH_Message	},
+	{1,11,GSMTAP_LTE_RRC_SUB_UL_DCCH_Message	},
+	{0,45, GSMTAP_LTE_RRC_SUB_BCCH_BCH_Message_NB	},
+	{0,46, GSMTAP_LTE_RRC_SUB_BCCH_BCH_Message_NB	},
+	{0,47, GSMTAP_LTE_RRC_SUB_PCCH_Message_NB	},
+	{0,48, GSMTAP_LTE_RRC_SUB_DL_CCCH_Message_NB	},
+	{0,49, GSMTAP_LTE_RRC_SUB_DL_DCCH_Message_NB	},
+	{1,50, GSMTAP_LTE_RRC_SUB_UL_CCCH_Message_NB	},
+	{1,52, GSMTAP_LTE_RRC_SUB_UL_DCCH_Message_NB	},
+    };
+
+static struct sub_chan_map rrc_subtype_map_20[] = {
+	{0,1, GSMTAP_LTE_RRC_SUB_BCCH_BCH_Message ,	   },
+	{0,2, GSMTAP_LTE_RRC_SUB_BCCH_DL_SCH_Message,	   },
+	{0,4, GSMTAP_LTE_RRC_SUB_MCCH_Message,		   },
+	{0,5, GSMTAP_LTE_RRC_SUB_PCCH_Message,		   },
+	{0,6, GSMTAP_LTE_RRC_SUB_DL_CCCH_Message,	   },
+	{0,7, GSMTAP_LTE_RRC_SUB_DL_DCCH_Message,	   },
+	{1,8, GSMTAP_LTE_RRC_SUB_UL_CCCH_Message,	   },
+	{1,9, GSMTAP_LTE_RRC_SUB_UL_DCCH_Message	   },
+	{0,54, GSMTAP_LTE_RRC_SUB_BCCH_BCH_Message_NB,	   },
+	{0,55, GSMTAP_LTE_RRC_SUB_BCCH_BCH_Message_NB,	   },
+	{0,56, GSMTAP_LTE_RRC_SUB_PCCH_Message_NB,	   },
+	{0,57, GSMTAP_LTE_RRC_SUB_DL_CCCH_Message_NB,	   },
+	{0,58, GSMTAP_LTE_RRC_SUB_DL_DCCH_Message_NB,	   },
+	{1,59, GSMTAP_LTE_RRC_SUB_UL_CCCH_Message_NB,	   },
+	{1,61, GSMTAP_LTE_RRC_SUB_UL_DCCH_Message_NB	   },
+    };
+
+struct chan_map_result {
+    uint8_t is_ul;
+    uint8_t chan;
+};
+
+struct lte_cell_info_dirty_012 {
+    uint16_t band;
+    uint16_t dl_arfcn;
+    uint16_t ul_arfcn;
+    /* don't care about anything else */
+};
+
+struct lte_cell_info_dirty_2plus {
+    uint16_t band;
+    uint32_t dl_arfcn;
+    uint32_t ul_arfcn;
+    /* don't care about anything else */
+};
+
+#define MAPC(xx)	\
+for(i=0; i < ARRAY_SIZE(xx); i++) {\
+if(inout->chan == xx[i].qc_id){ \
+	inout->chan = xx[i].gsmtap_lte_rrc_msg; \
+	inout->is_ul = xx[i].is_ul; \
+	return 0; \
+    } \
+    }
+
+
+static int find_map_in_ver(int version, struct chan_map_result* inout) {
+    unsigned int i = 0;
+    if(version < 9 || version == 13 || version == 22)
+	 MAPC(rrc_subtype_map_lt9_13_22)
+    else if(version >= 9 && version <= 12)
+	 MAPC(rrc_subtype_map_9to12)
+    else if(version >= 14 && version <= 16)
+	 MAPC(rrc_subtype_map_14_15_16)
+    else if(version == 19 || version == 26)
+	 MAPC(rrc_subtype_map_19_26)
+    else if(version  == 20)
+	 MAPC(rrc_subtype_map_20)
+    else
+	 return -1;
+
+    return 0;
+
+}
+
+static void handle_lte_rrc_ota_msg(struct diag_instance *di, struct log_hdr *lh, struct msgb *msg)
+{
+	uint16_t arfcn = 0;
+	uint8_t* data;
+	struct chan_map_result mc;
+	struct diag_lte_rrc_ota_base_8plus_msg *r8p;
+	struct diag_lte_rrc_ota_base_msg *rrm = (struct diag_lte_rrc_ota_base_msg *) msgb_data(msg);
+
+	di->rat_type = DIAG_INST_RAT_4G;
+
+	if (rrm->ext_ver < 8 || rrm->ext_ver > 24) {
+		printf("Unhandled LTE OTA rel: %u\n", rrm->ext_ver);
+		return;
+	}
+	r8p = (struct diag_lte_rrc_ota_base_8plus_msg *)msgb_data(msg);
+	data = msgb_data(msg) + sizeof(struct diag_lte_rrc_ota_base_8plus_msg);
+
+	if (msgb_length(msg) - sizeof(struct diag_lte_rrc_ota_base_8plus_msg) != r8p->len) {
+		printf("Unhandled LTE OTA rel len: %u %u\n", rrm->ext_ver, r8p->len);
+		return;
+	}
+
+	switch (r8p->channel_type) {
+	case 254:
+	case 255:
+		return;
+	}
+
+	mc.chan = r8p->channel_type;
+	if (find_map_in_ver(r8p->hdr.ext_ver, &mc) < 0)
+	    printf("Unhandled LTE OTA rel len: %u %u\n", rrm->ext_ver, r8p->len);
+
+	arfcn = mc.is_ul ? di->lte_arfcn_ul : di->lte_arfcn_dl;
+	arfcn = mc.is_ul ? arfcn | GSMTAP_ARFCN_F_UPLINK: arfcn;
+
+//	printf("lte m: %s\n", osmo_hexdump(data, 1));
+
+
+	if (di->gsmtap && di->flags & DIAG_INST_F_GSMTAP_DECODED) {
+		gsmtap_send_ex(di->gsmtap, GSMTAP_TYPE_LTE_RRC, arfcn, 0, mc.chan, 0, 0, 0, 0, data ,r8p->len);
+	}
+}
+
+static void handle_lte_rrc_cell_info(struct diag_instance *di, struct log_hdr *lh, struct msgb *msg)
+{
+
+	uint8_t* data;
+
+	struct diag_lte_rrc_ota_base_8plus_msg *r8p;
+	struct diag_lte_rrc_ota_base_msg *rrm = (struct diag_lte_rrc_ota_base_msg *) msgb_data(msg);
+
+	uint8_t ver = *(uint8_t *) msgb_data(msg);
+	if(ver < 3) {
+		struct lte_cell_info_dirty_012* i = (struct lte_cell_info_dirty_012 *) msgb_data(msg);
+		di->lte_arfcn_dl = i->dl_arfcn;
+		di->lte_arfcn_ul = i->ul_arfcn;
+	} else {
+		struct lte_cell_info_dirty_2plus* i = (struct lte_cell_info_dirty_2plus *) msgb_data(msg);
+		di->lte_arfcn_dl = i->dl_arfcn;
+		di->lte_arfcn_ul = i->ul_arfcn;
+	}
+
+}
+
+static void handle_lte_nas_msg(struct diag_instance *di, struct log_hdr *lh, struct msgb *msg)
+{
+	uint16_t arfcn = 0;
+	uint8_t* data;
+	uint32_t len;
+	bool sec_hdr = false;
+	bool is_ul = false;
+
+	di->rat_type = DIAG_INST_RAT_4G;
+
+	data = msgb_data(msg) + sizeof(struct diag_lte_rrc_nas_msg);
+	len = msgb_length(msg) - sizeof(struct diag_lte_rrc_nas_msg);
+
+	switch(lh->code) {
+		case LTE(LOG_LTE_NAS_ESM_OTA_IN_SEC_MSG_LOG_C):
+		case LTE(LOG_LTE_NAS_ESM_OTA_OUT_SEC_MSG_LOG_C):
+		case LTE(LOG_LTE_NAS_EMM_OTA_IN_SEC_MSG_LOG_C):
+		case LTE(LOG_LTE_NAS_EMM_OTA_OUT_SEC_MSG_LOG_C):
+			sec_hdr = true;
+			break;
+	}
+
+	switch(lh->code) {
+		case LTE(LOG_LTE_NAS_ESM_OTA_IN_MSG_LOG_C):
+		case LTE(LOG_LTE_NAS_EMM_OTA_IN_MSG_LOG_C):
+		case LTE(LOG_LTE_NAS_ESM_OTA_IN_SEC_MSG_LOG_C):
+		case LTE(LOG_LTE_NAS_EMM_OTA_IN_SEC_MSG_LOG_C):
+			is_ul = true;
+			break;
+		}
+
+	arfcn = is_ul ? di->lte_arfcn_ul : di->lte_arfcn_dl;
+	if (di->gsmtap && di->flags & DIAG_INST_F_GSMTAP_DECODED) {
+		gsmtap_send_ex(di->gsmtap, GSMTAP_TYPE_LTE_NAS, is_ul ? arfcn | GSMTAP_ARFCN_F_UPLINK : arfcn, 0, sec_hdr ? GSMTAP_LTE_NAS_SEC_HEADER : GSMTAP_LTE_NAS_PLAIN, 0, 0, 0, 0, data , len);
+	}
+}
+
+static const struct diag_log_dispatch_tbl log_tbl[] = {
+	{ LTE(LOG_LTE_RRC_OTA_MSG_LOG_C), handle_lte_rrc_ota_msg },
+	{ LTE(LOG_LTE_NAS_ESM_OTA_IN_MSG_LOG_C), handle_lte_nas_msg },
+	{ LTE(LOG_LTE_NAS_ESM_OTA_OUT_MSG_LOG_C), handle_lte_nas_msg },
+	{ LTE(LOG_LTE_NAS_EMM_OTA_IN_MSG_LOG_C), handle_lte_nas_msg },
+	{ LTE(LOG_LTE_NAS_EMM_OTA_OUT_MSG_LOG_C), handle_lte_nas_msg },
+//	{ LTE(LOG_LTE_NAS_ESM_OTA_IN_SEC_MSG_LOG_C), handle_lte_nas_msg },
+//	{ LTE(LOG_LTE_NAS_ESM_OTA_OUT_SEC_MSG_LOG_C), handle_lte_nas_msg },
+//	{ LTE(LOG_LTE_NAS_EMM_OTA_IN_SEC_MSG_LOG_C), handle_lte_nas_msg },
+//	{ LTE(LOG_LTE_NAS_EMM_OTA_OUT_SEC_MSG_LOG_C), handle_lte_nas_msg },
+	{ LTE(LOG_LTE_RRC_SERV_CELL_INFO_LOG_C), handle_lte_rrc_cell_info },
+};
+
+static __attribute__((constructor)) void on_dso_load_lte(void)
+{
+	diag_log_reg_dispatch(log_tbl, ARRAY_SIZE(log_tbl));
+}

src/diag_log_qmi.c

@@ -26,7 +26,7 @@
 
 /* A small wrapper around libqmi-glib to give us a human-readable string
  * representation of QMI messages that we receive from DIAG */
-static int dump_qmi_msg(const uint8_t *data, unsigned int len)
+static int dump_qmi_msg(struct diag_instance *di, const uint8_t *data, unsigned int len)
 {
 	GByteArray *buffer;
 	GError *error = NULL;
@@ -49,9 +49,9 @@ static int dump_qmi_msg(const uint8_t *data, unsigned int len)
 	return 0;
 }
 
-static void handle_qmi_msg(struct log_hdr *lh, struct msgb *msg)
+static void handle_qmi_msg(struct diag_instance *di, struct log_hdr *lh, struct msgb *msg)
 {
-	dump_qmi_msg(lh->data, lh->len);
+	dump_qmi_msg(di, lh->data, lh->len);
 }
 
 #define CORE(x)	(0x1000 + x)