osmo-isdntap/src/isdntap.c

#include <errno.h>
#include <fcntl.h>
#include <unistd.h>

#include <osmocom/core/isdnhdlc.h>
#include <osmocom/core/utils.h>
#include <osmocom/core/gsmtap_util.h>
#include <osmocom/core/rate_ctr.h>
#include <osmocom/core/gsmtap.h>
#include <osmocom/core/hashtable.h>

#include <osmocom/gsm/tlv.h>

#include "isdntap.h"
#include "q931.h"
#include "log.h"

/***********************************************************************
 * data structures
 ***********************************************************************/

struct isdntap_line *isdntap_line_find(struct isdntap_instance *itd, const char *name)
{
	struct isdntap_line *line;

	llist_for_each_entry(line, &itd->lines, list) {
		if (!strcmp(name, line->name))
			return line;
	}
	return NULL;
}

struct isdntap_line *isdntap_line_new(struct isdntap_instance *itd, const char *name)
{
	struct isdntap_line *line = talloc_zero(itd, struct isdntap_line);

	if (!line)
		return NULL;

	line->name = talloc_strdup(line, name);

	hash_init(line->q931.call_state);
	//line->ctrs = rate_ctr_group_alloc(line, &line_ctrg_desc, ctr_idx);

	/* initialize timeslots */
	for (unsigned int i = 0; i < ARRAY_SIZE(line->ts); i++) {
		struct isdntap_ts *ts = &line->ts[i];
		ts->line = line;
		ts->num = i;
		ts->mode = E1_TS_TRACE_MODE_NONE;
		ts->gsmtap_subtype = GSMTAP_E1T1_LAPD;
		osmo_isdnhdlc_rcv_init(&ts->hdlc.state[0].vars, 0);
		osmo_isdnhdlc_rcv_init(&ts->hdlc.state[1].vars, 0);
		ts->driver.dahdi.rx.fd = -1;
		ts->driver.dahdi.tx.fd = -1;
		ts->driver.dahdi.channo = -1;
	}

	llist_add_tail(&line->list, &itd->lines);

	return line;
}


/***********************************************************************
 * call state hash table
 ***********************************************************************/

/* one call / entry in the state table */
struct q931_call_state {
	struct hlist_node node;

	uint32_t call_ref;		/* decoded call reference */
	bool net2user;			/* call established in Net->User direction? */
	time_t create_time;		/* time at which call was created */
	struct q931_channel_id chan_id;	/* channel identification of associated user plane */
	struct q931_party_number calling_party;
	struct q931_party_number called_party;
	bool sending_complete;		/* called party number is complete */
};

static struct q931_call_state *
calltbl_find_callref(struct isdntap_line *line, uint32_t callref)
{
	struct q931_call_state *st;

	hash_for_each_possible(line->q931.call_state, st, node, callref) {
		if (st->call_ref == callref)
			return st;
	}
	return NULL;
}

static struct q931_call_state *
calltbl_create_callref(struct isdntap_line *line, uint32_t callref, bool net2user)
{
	struct q931_call_state *st = talloc_zero(line, struct q931_call_state);
	if (!st)
		return NULL;

	st->call_ref = callref;
	st->net2user = net2user;
	st->create_time = time(NULL);
	hash_add(line->q931.call_state, &st->node, callref);
	return st;
}

static struct q931_call_state *
calltbl_find_or_create_callref(struct isdntap_line *line, uint32_t callref, bool net2user)
{
	struct q931_call_state *cstate;
	cstate = calltbl_find_callref(line, callref);
	if (!cstate)
		cstate = calltbl_create_callref(line, callref, false);
	return cstate;
}

static void calltbl_delete_callref(struct q931_call_state *cstate)
{
	hash_del(&cstate->node);
	talloc_free(cstate);
}

static void calltbl_delete_all_callref(struct isdntap_line *line)
{
	struct q931_call_state *cstate;
	struct hlist_node *n;
	int i;

	hash_for_each_safe(line->q931.call_state, i, n, cstate, node) {
		calltbl_delete_callref(cstate);
	}
}

/* delete any call_state we have on chan_id */
static void calltbl_delete_callref_on_chan_id(struct isdntap_line *line, const struct q931_channel_id *chan_id)
{
	struct q931_call_state *cstate;
	struct hlist_node *n;
	int i;

	hash_for_each_safe(line->q931.call_state, i, n, cstate, node) {
		if (cstate->chan_id.info_chan_type == chan_id->info_chan_type &&
		    cstate->chan_id.d_channel == chan_id->d_channel &&
		    cstate->chan_id.b_channel == chan_id->b_channel) {
			calltbl_delete_callref(cstate);
		}
	}
}

/***********************************************************************
 * Q.931 processing
 ***********************************************************************/

/* append additional digits received (overlap dialling) to called party number */
static void append_called_digits(struct q931_party_number *out, const uint8_t *buf, size_t len)
{
	unsigned int idx = strlen(out->digits);
	size_t i;

	for (i = 0; i < len; i++) {
		if (idx >= ARRAY_SIZE(out->digits))
			return;
		out->digits[idx] = buf[i];
		idx++;
	}
}

static int isdntap_q931_rx_global(struct isdntap_line *line, const struct q931_msg_parsed *parsed)
{
	int rc;

	switch (parsed->msg_type) {
	case Q931_MSGT_RESTART:
		if (TLVP_PRES_LEN(&parsed->ies, Q931_IEI_RESTART_IND, 1)) {
			struct q931_channel_id chan_id;
			uint8_t rest_ind = *TLVP_VAL(&parsed->ies, Q931_IEI_RESTART_IND) & 0x7;
			switch (rest_ind) {
			case 0: /* clear indicated channel[s] */
				if (!TLVP_PRESENT(&parsed->ies, Q931_IEI_CHANNEL_ID))
					return -EINVAL;
				rc = q931_decode_channel_id(&chan_id,
							    TLVP_VAL(&parsed->ies, Q931_IEI_CHANNEL_ID),
							    TLVP_LEN(&parsed->ies, Q931_IEI_CHANNEL_ID));
				if (rc < 0)
					return rc;
				/* clear any calls on selected channel */
				calltbl_delete_callref_on_chan_id(line, &chan_id);
				break;
			case 6:
			case 7: /* clear all channels */
				calltbl_delete_all_callref(line);
				break;
			}
		}
		break;
	}
	return 0;
}

/* receive one Q.931 message for signaling analysis */
static int isdntap_q931_rx(struct isdntap_line *line, bool net2user, const uint8_t *buf, size_t len)
{
	struct q931_msg_parsed parsed;
	struct q931_call_state *cstate;
	int rc;

	rc = q931_msg_parse(&parsed, buf, len);
	if (rc < 0)
		return rc;

	/* process special case of global call reference elsewhere */
	if (parsed.call_ref == 0) {
		return isdntap_q931_rx_global(line, &parsed);
	}

	/* Step 1: look-up (or create in case of SETUP) the call state record for call reference */
	switch (parsed.msg_type) {
	case Q931_MSGT_SETUP:
		cstate = calltbl_find_or_create_callref(line, parsed.call_ref, false);
		if (!cstate)
			return -ENOMEM;
		break;
	default:
		cstate = calltbl_find_callref(line, parsed.call_ref);
		if (!cstate)
			return -ENODEV;
		break;
	}

	/* Step 2: Additional information gathering */
	switch (parsed.msg_type) {
	case Q931_MSGT_SETUP:
		/* decode called/calling party (if any) for context */
		if (TLVP_PRESENT(&parsed.ies, Q931_IEI_CALLED_PARTY_NUM)) {
			q931_decode_called_party(&cstate->called_party,
						  TLVP_VAL(&parsed.ies, Q931_IEI_CALLED_PARTY_NUM),
						  TLVP_LEN(&parsed.ies, Q931_IEI_CALLED_PARTY_NUM));
		}
		if (TLVP_PRESENT(&parsed.ies, Q931_IEI_CALLING_PARTY_NUM)) {
			q931_decode_calling_party(&cstate->calling_party,
						  TLVP_VAL(&parsed.ies, Q931_IEI_CALLING_PARTY_NUM),
						  TLVP_LEN(&parsed.ies, Q931_IEI_CALLING_PARTY_NUM));
		}
		if (TLVP_PRESENT(&parsed.ies, Q931_IEI_SENDING_COMPLETE))
			cstate->sending_complete = true;
		break;
	case Q931_MSGT_INFORMATION:
		/* append any additional digits to called_party */
		if (TLVP_PRESENT(&parsed.ies, Q931_IEI_KEYPAD_FACILITY) &&
		    !cstate->sending_complete) {
			append_called_digits(&cstate->called_party,
					     TLVP_VAL(&parsed.ies, Q931_IEI_KEYPAD_FACILITY),
					     TLVP_LEN(&parsed.ies, Q931_IEI_KEYPAD_FACILITY));
		}
		/* mark as complete, if we finally are */
		if (TLVP_PRESENT(&parsed.ies, Q931_IEI_SENDING_COMPLETE))
			cstate->sending_complete = true;
		break;
	default:
		break;
	}

	/* Step 3: dispatch by message type; look in those that contain a ChannelIndicator */
	switch (parsed.msg_type) {
	case Q931_MSGT_SETUP:
	case Q931_MSGT_CALL_PROCEEDING:
	case Q931_MSGT_SETUP_ACK:
#if 0
	case Q931_MSGT_ALERTING:
	case Q931_MSGT_CONNECT:
	case Q931_MSGT_RESUME_ACK:
#endif
		if (TLVP_PRESENT(&parsed.ies, Q931_IEI_CHANNEL_ID)) {
			rc = q931_decode_channel_id(&cstate->chan_id,
						    TLVP_VAL(&parsed.ies, Q931_IEI_CHANNEL_ID),
						    TLVP_LEN(&parsed.ies, Q931_IEI_CHANNEL_ID));
			if (rc < 0)
				return rc;
			if (cstate->chan_id.exclusive && !cstate->chan_id.d_channel) {
				/* now we know the exact B channel used */
				/* TODO: start capturing */
			}
		}
		break;
	case Q931_MSGT_RELEASE:
	case Q931_MSGT_RELEASE_COMPLETE:
		/* forget about the call */
		calltbl_delete_callref(cstate);
		cstate = NULL;
		break;
	default:
		break;
	}

	return 0;
}

/* trace one Q.921 / LAPD frame for signaling analysis */
static void isdntap_q921_rx(struct isdntap_line *line, bool net2user, const uint8_t *buf, size_t len)
{
	uint8_t sapi, tei;
	/* Parse LAPD header; Ignore anything != I frames */
	/* Q.921 header: 2 bytes address; 1-2 byte control; [optional] information */

	if (len < 2)
		return;

	/* Address field: Figure 5 / Q.921 */
	sapi = buf[0] >> 2;
	tei = buf[1] >> 1;

	/* SAPI value for circuit-switched call control (Table 2/Q.921) */
	if (sapi != 0) {
		/* skip unknown SAPI */
		return;
	}

	if (len < 3)
		return;

	/* Control field: Table 4/Q.921 */
	if (buf[2] & 0x01) {
		/* skip frames != I-frame (which have 0 as lsb of 1st octet) */
		return;
	}

	if (len < 4)
		return;

	/* skip N(R) / N(S) and go directly to Q.931 payload */
	isdntap_q931_rx(line, net2user, buf + 4, len - 4);
}

/* main entry point for received D-channel data on a given TS */
int isdntap_ts_rx_dchan(struct isdntap_ts *ts, const uint8_t *buf, size_t len, bool net2user)
{
	isdntap_q921_rx(ts->line, net2user, buf, len);
	return 0;
}

/* main entry point for received B-channel data on a given TS */
int isdntap_ts_rx_bchan(struct isdntap_ts *ts, const uint8_t *buf, size_t len, bool is_rx)
{
	struct msgb *msg = msgb_alloc_c(ts->line, len, "Bchan");
	uint8_t *cur;
	int rc;

	if (!msg)
		return -ENOMEM;

	cur = msgb_put(msg, len);
	memcpy(cur, buf, len);

	if (is_rx)
		rc = osmo_wqueue_enqueue(&ts->output.file.rx, msg);
	else
		rc = osmo_wqueue_enqueue(&ts->output.file.tx, msg);
	if (rc)
		msgb_free(msg);

	return 0;
}

/* helper function for opening an output file */
static int isdntap_open_file(struct isdntap_ts *ts, const char *call_label, const char *suffix)
{
	char buf[PATH_MAX];
	int rc;

	snprintf(buf, sizeof(buf), "%s/isdntap-%s-%u-%s.raw", ts->line->inst->cfg.output_path,
		 call_label, ts->num, suffix);

	rc = open(buf, O_CREAT|O_WRONLY|O_TRUNC|O_NONBLOCK);
	if (rc < 0) {
		LOGP(DITAP, LOGL_ERROR, "Error opening file %s: %s\n", buf, strerror(errno));
		return rc;
	}

	LOGP(DITAP, LOGL_INFO, "Opened file %s\n", buf);
	return rc;
}

int isdntap_ts_start_bchan(struct isdntap_ts *ts, const char *call_label)
{
	int fd_rx, fd_tx;

	osmo_wqueue_init(&ts->output.file.tx, 100);
	fd_tx = isdntap_open_file(ts, call_label, "tx");
	if (fd_tx < 0)
		return fd_tx;
	ts->output.file.tx.bfd.fd = fd_tx;
	osmo_fd_register(&ts->output.file.tx.bfd);

	osmo_wqueue_init(&ts->output.file.rx, 100);
	fd_rx = isdntap_open_file(ts, call_label, "rx");
	if (!fd_rx)
		goto out_cleanup_tx;
	ts->output.file.rx.bfd.fd = fd_rx;
	osmo_fd_register(&ts->output.file.rx.bfd);

	return 0;

out_cleanup_tx:
	osmo_fd_unregister(&ts->output.file.tx.bfd);
	ts->output.file.tx.bfd.fd = -1;
	close(fd_tx);

	return -1;
}

void isdntap_ts_stop_bchan(struct isdntap_ts *ts)
{
	/* FIXME: ideally we'd make sure that the write queues are both fully flushed first */

	if (ts->output.file.tx.bfd.fd >= 0) {
		osmo_wqueue_clear(&ts->output.file.tx);
		osmo_fd_unregister(&ts->output.file.tx.bfd);
		close(ts->output.file.tx.bfd.fd);
		ts->output.file.tx.bfd.fd = -1;
	}

	if (ts->output.file.rx.bfd.fd >= 0) {
		osmo_wqueue_clear(&ts->output.file.rx);
		osmo_fd_unregister(&ts->output.file.rx.bfd);
		close(ts->output.file.rx.bfd.fd);
		ts->output.file.rx.bfd.fd = -1;
	}
}


/***********************************************************************
 * e1d integration
 ***********************************************************************/

# if 0

/* trace one timeslot of a line */
void e1tap_trace_ts(struct e1tap_ts *ts, const uint8_t *tsbuf, size_t frame_count, uint8_t hdlc_idx)
{
	struct e1tap_line *line = ts->line;
	int oi = 0;
	int rc;

	OSMO_ASSERT(hdlc_idx <= 0);

	switch (ts->mode) {
	case E1_TS_TRACE_MODE_HDLC:
	case E1_TS_TRACE_MODE_ISDN_D:
		while (oi < frame_count) {
			int num_consumed;

			/* feed the new bytes into the HDLC decoder */
			rc = osmo_isdnhdlc_decode(&ts->hdlc.state[hdlc_idx].vars,
						  &tsbuf[oi], frame_count-oi,
						  &num_consumed, ts->hdlc.state[hdlc_idx].out,
						  sizeof(ts->hdlc.state[hdlc_idx].out));
			if (rc > 0) {
				/* if HDLC decoder produced output, send it via GSMTAP */
				gsmtap_send_ex(line->gti, GSMTAP_TYPE_E1T1, flags,
						ts->num, ts->gsmtap_subtype, 0, 0, 0, 0,
						ts->hdlc.state[hdlc_idx].out, rc);
				/* feed D-channel into higher level analysis */
				if (ts->mode == E1_TS_TRACE_MODE_ISDN_D)
					isdntap_q921_rx(line, hdlc_idx, ts->hdlc.state[hdlc_idx].out, rc);
			} else if (rc < 0) {
				/* FIXME: log error */
			}
			oi += num_consumed;
		}
		break;
	default:
		break;
	}
}

/* trace an entire line (we pass in full 32byte frames */
void e1tap_trace_line(struct e1tap_line *line, bool mux_out, const uint8_t *buf, int frame_count)
{
	uint8_t tsbuf[frame_count];
	uint8_t hdlc_idx;

	if (!line)
		return;

	if (mux_out)
		hdlc_idx = 1;
	else
		hdlc_idx = 0;

	for (unsigned int tn = 1; tn < ARRAY_SIZE(line->ts); tn++) {
		struct e1tap_ts *ts = line->ts[i];
		/* fast path */
		if (ts->mode == E1_TS_TRACE_MODE_NONE)
			continue;
		/* demultiplex the bytes of the given TS */
		for (unsigned int f = 0; f < frame_count; f++)
			tsbuf[f] = buf[32*f + tn];
		e1tap_trace_ts(ts, tsbuf, frame_count, hdlc_idx);
	}
}


/* USB/trunkdev <- application/OCTOI */
void
e1_trace_mux_out(struct e1_line *line, const uint8_t *buf, int frame_count)
{
	return _e1_trace(line, true, buf, frame_count);
}

/* USB/trunkdev -> application/OCTOI */
void
e1_trace_mux_in(struct e1_line *line, const uint8_t *buf, int frame_count)
{
	return _e1_trace(line, false, buf, frame_count);
}

#endif