osmo-v5/src/layer1.c

#include <stdio.h>
#include <errno.h>
#include <osmocom/abis/e1_input.h>
#include <osmocom/core/gsmtap.h>
#include <osmocom/core/gsmtap_util.h>
#include <osmocom/core/signal.h>

#include "v5x_internal.h"
#include "v5x_protocol.h"
#include "lapv5.h"
#include "layer1.h"
#include "v5x_l1_fsm.h"
#include "v5x_le_management.h"
#include "logging.h"
#include "libg711/g711.h"

extern int ulaw;
extern int test_sa7;
extern const char *gsmtap_ip;

extern struct v5x_instance *v5i;

static struct gsmtap_inst *g_gti = NULL;

/* only temporarily until this is in libosmocore gsmtap.h */
#ifndef GSMTAP_E1T1_V5EF
#define GSMTAP_E1T1_V5EF	0x06
#endif

static inline struct e1inp_line *e1_line_from_link(struct v5x_link *v5l)
{
	struct e1inp_line *e1_line;

	if (v5l->e1_line < 0)
		return NULL;
	e1_line = e1inp_line_find(v5l->e1_line);
	if (!e1_line)
		return NULL;

	return e1_line;
}

/* Callback function to receive L1 signal from E1 port */
static int inp_sig_cb(unsigned int subsys, unsigned int signal, void __attribute__((unused)) *handler_data,
	void *signal_data)
{
	struct input_signal_data *isd = signal_data;
	struct v5x_interface *v5if;
	struct v5x_link *v5l;

	if (subsys != SS_L_INPUT)
		return 0;

	/* no interface */
	if (llist_empty(&v5i->interfaces))
		return 0;
	v5if = llist_first_entry(&v5i->interfaces, struct v5x_interface, list);

	/* not used by any link */
	v5l = v5x_link_find_e1_line(v5if, isd->line->num);
	if (!v5l)
		return 0;

	switch (signal) {
	case S_L_INP_LINE_LOS:
		v5x_l1_signal_rcv(v5l, L1_SIGNAL_LOS);
		break;
	case S_L_INP_LINE_NOLOS:
		v5x_l1_signal_rcv(v5l, L1_SIGNAL_NO_LOS);
		break;
	case S_L_INP_LINE_RAI:
		v5x_l1_signal_rcv(v5l, L1_SIGNAL_RAI);
		break;
	case S_L_INP_LINE_NORAI:
		v5x_l1_signal_rcv(v5l, L1_SIGNAL_NO_RAI);
		break;
	case S_L_INP_LINE_AIS:
		v5x_l1_signal_rcv(v5l, L1_SIGNAL_AIS);
		break;
	case S_L_INP_LINE_NOAIS:
		v5x_l1_signal_rcv(v5l, L1_SIGNAL_NO_AIS);
		break;
	case S_L_INP_LINE_SA_BITS:
		if ((isd->sa_bits & 0x40)) {
			v5x_l1_signal_rcv(v5l, L1_SIGNAL_SA7_1);
			if (test_sa7) {
				printf("Currently Sa 7 is set to 1, changing it to 0. (Link ID %d)\n", v5l->id);
				v5x_l1_signal_snd(v5l, L1_SIGNAL_SA7_0);
			}
		} else {
			v5x_l1_signal_rcv(v5l, L1_SIGNAL_SA7_0);
			if (test_sa7) {
				printf("Currently Sa 7 is set to 0, changing it to 1. (Link ID %d)\n", v5l->id);
				v5x_l1_signal_snd(v5l, L1_SIGNAL_SA7_1);
			}
		}
		break;
	case S_L_INP_LINE_SLIP_RX:
		printf("RX slip detected on link %d.\n", v5l->id);
		break;
	case S_L_INP_LINE_SLIP_TX:
		printf("TX slip detected on link %d.\n", v5l->id);
		break;
	default:
		;
	}

	return 0;
}

/* Send L1 signal to E1 port */
int v5x_l1_signal_snd(struct v5x_link *v5l, enum l1_signal_prim prim)
{
	struct e1inp_line *e1_line;

	/* no line assigned */
	if (v5l->e1_line < 0)
		return 0;
	e1_line = e1inp_line_find(v5l->e1_line);
	if (!e1_line)
		return 0;

	switch (prim) {
	case L1_SIGNAL_SA7_0:
		e1inp_ts_set_sa_bits(e1_line, 0xbf);
		break;
	case L1_SIGNAL_SA7_1:
		e1inp_ts_set_sa_bits(e1_line, 0xff);
		break;
	default:
		;
	}

	return 0;
}

/* store TX to buffer */
static void echo_tx(struct v5x_echo_proc *ep, uint8_t *data, int len)
{
	int in;
	int16_t tx;

	if (!ep->ec || !ep->enabled)
		return;

	while (len--) {
		if (ulaw)
			tx = g711_ulaw_flipped_to_linear[*data++];
		else
			tx = g711_alaw_flipped_to_linear[*data++];
		ep->tx_buffer[ep->tx_buffer_in] = tx;
		in = (ep->tx_buffer_in + 1) % EC_BUFFER;
		/* buffer overflow condition, should not happen, if application syncs to RX */
		if (in == ep->tx_buffer_out)
			break;
		ep->tx_buffer_in = in;
	}
}

/* remove echo from RX using TX from buffer */
static void echo_rx(struct v5x_echo_proc *ep, uint8_t *data, int len)
{
	struct v5x_user_port *v5up = ep->port;
	int16_t tx, rx, rx_can;
	int16_t answer_buffer[len];
	int i;
	int rc;

	if (!ep->ec || !ep->enabled)
		return;

	for (i = 0; i < len; i++) {
		/* buffer underrun condition, may happen before buffer was filled with first frame */
		if (ep->tx_buffer_out == ep->tx_buffer_in)
			tx = 0;
		else {
			tx = ep->tx_buffer[ep->tx_buffer_out];
			ep->tx_buffer_out = (ep->tx_buffer_out + 1) % EC_BUFFER;
		}
		if (ulaw) {
			rx = g711_ulaw_flipped_to_linear[*data];
			rx_can = echo_can_update(ep->ec, tx, rx);
			*data++ = g711_linear_to_ulaw_flipped[(uint16_t)rx_can];
		} else {
			rx = g711_alaw_flipped_to_linear[*data];
			rx_can = echo_can_update(ep->ec, tx, rx);
			*data++ = g711_linear_to_alaw_flipped[(uint16_t)rx_can];
		}
		answer_buffer[i] = rx + tx; /* yes, may overflow, but then it is no valid tone anyway */
	}

	rc = answertone_process(&ep->at, answer_buffer, len);
	if (rc > 0) {
		LOGP(DV5, LOGL_NOTICE, "Detected answer tone, disable echo chanceler of %s port %d.\n",
			(v5up->type == V5X_USER_TYPE_PSTN) ? "PSTN" : "ISDN", v5up->nr);
		ep->enabled = 0;
	}
}

/* data L1 -> L2 from E1 interface */
static void hdlc_rx_cb(struct e1inp_ts *ts, struct msgb *msg)
{
	struct v5x_interface *v5if;
	struct v5x_link *v5l;

	/* no interface */
	if (llist_empty(&v5i->interfaces)) {
		msgb_free(msg);
		return;
	}
	v5if = llist_first_entry(&v5i->interfaces, struct v5x_interface, list);

	/* not used by any link */
	v5l = v5x_link_find_e1_line(v5if, ts->line->num);
	if (!v5l) {
		msgb_free(msg);
		return;
	}

	LOGP(DLINP, LOGL_DEBUG, "Link %d L1->L2: %s\n", v5l->id, msgb_hexdump(msg));

	/* send V5 data via gsmtap so wireshark can receive + decode it */
	if (g_gti) {
		gsmtap_send_ex(g_gti, GSMTAP_TYPE_E1T1, v5l->id, ts->num, GSMTAP_E1T1_V5EF,
				0, 0, 0, 0, msgb_data(msg), msgb_length(msg));
	}

	lapv5ef_rx(v5l, msg);
}

/* data B-channel data from E1 interface */
static void raw_rx_cb(struct e1inp_ts *ts, struct msgb *msg)
{
	struct v5x_interface *v5if;
	struct v5x_link *v5l;
	struct v5x_user_port *v5up;

	/* no interface */
	if (llist_empty(&v5i->interfaces)) {
		msgb_free(msg);
		return;
	}
	v5if = llist_first_entry(&v5i->interfaces, struct v5x_interface, list);

	/* not used by any link */
	v5l = v5x_link_find_e1_line(v5if, ts->line->num);
	if (!v5l) {
		msgb_free(msg);
		return;
	}
	v5up = v5l->ts[ts->num].v5up;

	/* not used by any user port */
	if (!v5up) {
		msgb_free(msg);
		return;
	}

	/* we want B-channel data flipped */
	osmo_revbytebits_buf(msg->data, msg->len);
				
	/* if assigned and active, send B-channel data to socket interface */
	if (v5up->ts[0] && v5up->ts[0]->nr == ts->num && v5up->ts[0]->b_activated) {
		echo_rx(&v5up->ep[0], msg->data, msg->len);
		ph_socket_tx_msg(&v5up->ph_socket, 1, PH_PRIM_DATA_IND, msg->data, msg->len);
	}
	if (v5up->ts[1] && v5up->ts[1]->nr == ts->num && v5up->ts[1]->b_activated) {
		echo_rx(&v5up->ep[1], msg->data, msg->len);
		ph_socket_tx_msg(&v5up->ph_socket, 2, PH_PRIM_DATA_IND, msg->data, msg->len);
	}
	msgb_free(msg);
}

/* send HDLC frame to signaling channel (from ISDN) */
int ph_data_req_hdlc(struct msgb *msg, struct v5x_interface *v5if)
{
	struct e1inp_line *e1_line;
	e1_line = e1_line_from_link(v5if->cc_link);
	if (!e1_line) {
		msgb_free(msg);
		return 0;
	}

	LOGP(DLINP, LOGL_DEBUG, "Link %d L2->L1: %s\n", v5if->cc_link->id, msgb_hexdump(msg));

	struct e1inp_ts *ts = &e1_line->ts[v5if->cc_link->c_channel[0].ts->nr - 1];

	/* send V5 data via gsmtap so wireshark can receive + decode it */
	if (g_gti) {
		gsmtap_send_ex(g_gti, GSMTAP_TYPE_E1T1, v5if->cc_link->id | GSMTAP_ARFCN_F_UPLINK, ts->num,
				GSMTAP_E1T1_V5EF, 0, 0, 0, 0, msgb_data(msg), msgb_length(msg));
	}

	return e1inp_ts_send_hdlc(ts, msg);
}

/* send HDLC frame to signaling channel (from DL) */
int ph_data_req_dl_cc(struct msgb *msg, void *cbdata)
{
	struct v5x_interface *v5if = (struct v5x_interface *)cbdata;
	struct e1inp_line *e1_line;

	e1_line = e1_line_from_link(v5if->cc_link);
	if (!e1_line) {
		msgb_free(msg);
		return 0;
	}

	struct e1inp_ts *ts = &e1_line->ts[v5if->cc_link->c_channel[0].ts->nr - 1];

	/* add frame relay header */
	msg->l2h = msgb_push(msg, 2);
	msg->l2h[0] = msg->l2h[2] & 0xfd;
	msg->l2h[1] = msg->l2h[3];

	LOGP(DLINP, LOGL_DEBUG, "Link %d L2->L1: %s\n", v5if->cc_link->id, msgb_hexdump(msg));

	/* send V5 data via gsmtap so wireshark can receive + decode it */
	if (g_gti) {
		gsmtap_send_ex(g_gti, GSMTAP_TYPE_E1T1, v5if->cc_link->id | GSMTAP_ARFCN_F_UPLINK, ts->num,
				GSMTAP_E1T1_V5EF, 0, 0, 0, 0, msgb_data(msg), msgb_length(msg));
	}

	return e1inp_ts_send_hdlc(ts, msg);
}

/* send HDLC frame to protection link (from DL) */
int ph_data_req_dl_prot(struct msgb *msg, void *cbdata)
{
	struct v5x_link *v5l = (struct v5x_link *)cbdata;
	struct e1inp_line *e1_line;

	e1_line = e1_line_from_link(v5l);
	if (!e1_line) {
		msgb_free(msg);
		return 0;
	}

	struct e1inp_ts *ts = &e1_line->ts[v5l->c_channel[0].ts->nr - 1];

	/* add frame relay header */
	msg->l2h = msgb_push(msg, 2);
	msg->l2h[0] = msg->l2h[2] & 0xfd;
	msg->l2h[1] = msg->l2h[3];

	LOGP(DLINP, LOGL_DEBUG, "Link %d L2->L1: %s\n", v5l->id, msgb_hexdump(msg));

	/* send V5 data via gsmtap so wireshark can receive + decode it */
	if (g_gti) {
		gsmtap_send_ex(g_gti, GSMTAP_TYPE_E1T1, v5l->id | GSMTAP_ARFCN_F_UPLINK, ts->num, GSMTAP_E1T1_V5EF,
				0, 0, 0, 0, msgb_data(msg), msgb_length(msg));
	}

	return e1inp_ts_send_hdlc(ts, msg);
}

int ph_activate_req(struct v5x_timeslot *ts)
{
	struct e1inp_line *e1_line;
	struct e1inp_ts *e1_ts;
	int rc;

	if (ts->b_activated)
		return 0;

	e1_line = e1_line_from_link(ts->link);
	if (!e1_line)
		return -EINVAL;
	e1_ts = &e1_line->ts[ts->nr - 1];

	e1inp_ts_config_raw(e1_ts, e1_line, raw_rx_cb);
	rc = e1inp_line_update(e1_line);

	ts->b_activated = 1;

	return rc;
}

int ph_deactivate_req(struct v5x_timeslot *ts)
{
	struct e1inp_line *e1_line;
	struct e1inp_ts *e1_ts;
	int rc;

	if (!ts->b_activated)
		return 0;

	e1_line = e1_line_from_link(ts->link);
	if (!e1_line)
		return -EINVAL;
	e1_ts = &e1_line->ts[ts->nr - 1];

	e1_ts->type = E1INP_TS_TYPE_NONE;
	rc = e1inp_line_update(e1_line);

	ts->b_activated = 0;

	return rc;
}

/* send raw (B-channel) data to E1 interface */
static int ph_data_req_raw(struct v5x_link *v5l, struct msgb *msg, int ts_nr)
{
	struct e1inp_line *e1_line;

	/* no line assigned */
	e1_line = e1_line_from_link(v5l);
	if (!e1_line) {
		msgb_free(msg);
		return 0;
	}

	/* we want B-channel data flipped */
	osmo_revbytebits_buf(msg->data, msg->len);
				
	struct e1inp_ts *ts = &e1_line->ts[ts_nr-1];

	return e1inp_ts_send_raw(ts, msg);
}

/* receive message from PH-socket */
void ph_socket_rx_cb(ph_socket_t *s, int channel, uint8_t prim, uint8_t *data, int length)
{
	struct v5x_user_port *v5up = s->priv;

	switch (prim) {
	case PH_PRIM_CTRL_REQ:
		/* deactivate channels, if active */
		if ((channel == 0 || channel == 3) && length && *data == PH_CTRL_BLOCK) {
			v5x_le_channel_unassign(v5up, 1);
			v5x_le_channel_unassign(v5up, 2);
		}
		v5x_le_nat_ph_rcv(v5up, prim, data, length);
		break;
	case PH_PRIM_ACT_REQ:
		if (channel == 1 || channel == 2) {
			v5x_le_channel_assign(v5up, channel);
			ph_socket_tx_msg(s, channel, PH_PRIM_ACT_IND, NULL, 0);
			break;
		}
		v5x_le_nat_ph_rcv(v5up, prim, data, length);
		break;
	case PH_PRIM_DACT_REQ:
		if (channel == 1 || channel == 2) {
			v5x_le_channel_unassign(v5up, channel);
			ph_socket_tx_msg(s, channel, PH_PRIM_DACT_IND, NULL, 0);
			break;
		}
		v5x_le_nat_ph_rcv(v5up, prim, data, length);
		break;
	case PH_PRIM_DATA_REQ:
		if (v5up->type == V5X_USER_TYPE_PSTN && channel == 0) {
			struct msgb *msg = msgb_alloc_headroom(length + 32, 32, "V5 PSTN MSG");
			memcpy(msgb_put(msg, length), data, length);
			v5x_le_nat_fe_rcv(v5up, msg);
		} else if (v5up->type == V5X_USER_TYPE_ISDN && channel == 3) {
			struct msgb *msg = msgb_alloc_headroom(length + 32, 32, "V5 EF MSG");
			memcpy(msgb_put(msg, length), data, length);
			lapv5ef_tx(v5up, msg);
		} else if ((channel == 1 || channel == 2) && v5up->ts[channel - 1] && v5up->ts[channel - 1]->b_activated) {
			echo_tx(&v5up->ep[channel - 1], data, length);
			struct msgb *msg = msgb_alloc_headroom(length + 32, 32, "B MSG");
			memcpy(msgb_put(msg, length), data, length);
			ph_data_req_raw(v5up->ts[channel - 1]->link, msg, v5up->ts[channel - 1]->nr);
		}
		/* always confirm */
		ph_socket_tx_msg(s, channel, PH_PRIM_DATA_CNF, NULL, 0);
		break;
	}
}

//FIXME: we use HDLC
static int v5le_rx_sign(struct msgb *msg)
{
	LOGP(DLMI, LOGL_NOTICE, "Rx: %s\n", msgb_hexdump(msg));

	msgb_free(msg);

	return 0;
}

static const struct e1inp_line_ops v5le_e1_line_ops = {
	.sign_link_up = NULL,
	.sign_link_down = NULL,
	.sign_link = v5le_rx_sign,
};

/* init all E1 lines and register input signals */
int e1_init(void)
{
	struct e1inp_line *e1_line;
	int i, ts;

	if (gsmtap_ip) {
		g_gti = gsmtap_source_init(gsmtap_ip, GSMTAP_UDP_PORT, 0);
		if (!g_gti) {
			fprintf(stderr, "Failed to use '%s' as IP for GSMTAP\n", gsmtap_ip);
			return -EINVAL;
		}
		gsmtap_source_add_sink(g_gti);
	}

	for (i = 0; i < 256; i++) {
		e1_line = e1inp_line_find(i);
		if (!e1_line)
			continue;

		e1inp_line_bind_ops(e1_line, &v5le_e1_line_ops);

		for (ts = 1; ts <= 31; ts++) {
			struct e1inp_ts *e1_ts = &e1_line->ts[ts-1];
			if (ts == 16) { // FIXME: make this depending on c_channel
				//e1inp_ts_config_sign(e1_ts, e1_line);
				//e1inp_sign_link_create(e1_ts, E1INP_SIGN_NONE, NULL, 115/*TEI*/, 0/*SAPI*/);
				e1inp_ts_config_hdlc(e1_ts, e1_line, hdlc_rx_cb);
			} else
				e1_ts->type = E1INP_TS_TYPE_NONE;
		}

		e1inp_line_update(e1_line);
	}

	osmo_signal_register_handler(SS_L_INPUT, inp_sig_cb, NULL);

	return 0;
}