forked from retronetworking/osmo-v5
538 lines
14 KiB
C
538 lines
14 KiB
C
|
|
#include <stdio.h>
|
|
#include <errno.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
|
|
|
|
/* 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_link *v5l;
|
|
|
|
if (subsys != SS_L_INPUT)
|
|
return 0;
|
|
|
|
/* not used by any link */
|
|
if (!isd->line->ops)
|
|
return 0;
|
|
v5l = container_of(isd->line->ops, struct v5x_link, e1_line_ops);
|
|
|
|
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 = v5l->e1_line;
|
|
|
|
/* no line assigned */
|
|
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->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->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];
|
|
if (ep->port->use_line_echo == USE_ECHO_CANCELER)
|
|
rx_can = echo_can_update(ep->ec, tx, rx);
|
|
else
|
|
rx_can = echo_sup_update(ep->es, tx, rx);
|
|
*data++ = g711_linear_to_ulaw_flipped[(uint16_t)rx_can];
|
|
} else {
|
|
rx = g711_alaw_flipped_to_linear[*data];
|
|
if (ep->port->use_line_echo == USE_ECHO_CANCELER)
|
|
rx_can = echo_can_update(ep->ec, tx, rx);
|
|
else
|
|
rx_can = echo_sup_update(ep->es, 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, (ep->port->use_line_echo == USE_ECHO_CANCELER));
|
|
if (rc > 0) {
|
|
LOGP(DV5, LOGL_NOTICE, "Detected answer tone, disable echo %s of %s port %d.\n",
|
|
(ep->port->use_line_echo == USE_ECHO_CANCELER) ? "canceler" : "suppressor",
|
|
(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_link *v5l;
|
|
|
|
/* not used by any link */
|
|
if (!ts->line->ops) {
|
|
msgb_free(msg);
|
|
return;
|
|
}
|
|
v5l = container_of(ts->line->ops, struct v5x_link, e1_line_ops);
|
|
|
|
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_link *v5l;
|
|
struct v5x_user_port *v5up;
|
|
|
|
/* not used by any link */
|
|
if (!ts->line->ops) {
|
|
msgb_free(msg);
|
|
return;
|
|
}
|
|
v5l = container_of(ts->line->ops, struct v5x_link, e1_line_ops);
|
|
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 = v5if->cc_link->e1_line;
|
|
|
|
if (!e1_line) {
|
|
msgb_free(msg);
|
|
return 0;
|
|
}
|
|
|
|
if (!v5x_l1_is_up(v5if->cc_link->l1)) {
|
|
LOGP(DLINP, LOGL_DEBUG, "Link %d is down!\n", v5if->cc_link->id);
|
|
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 = v5if->cc_link->e1_line;
|
|
|
|
if (!e1_line) {
|
|
msgb_free(msg);
|
|
return 0;
|
|
}
|
|
|
|
if (!v5x_l1_is_up(v5if->cc_link->l1)) {
|
|
LOGP(DLINP, LOGL_DEBUG, "Link %d is down!\n", v5if->cc_link->id);
|
|
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 = v5l->e1_line;
|
|
|
|
if (!e1_line) {
|
|
msgb_free(msg);
|
|
return 0;
|
|
}
|
|
|
|
if (!v5x_l1_is_up(v5l->l1)) {
|
|
LOGP(DLINP, LOGL_DEBUG, "Link %d is down!\n", v5l->id);
|
|
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 = ts->link->e1_line;
|
|
struct e1inp_ts *e1_ts;
|
|
int rc;
|
|
|
|
if (ts->b_activated)
|
|
return 0;
|
|
|
|
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 = ts->link->e1_line;
|
|
struct e1inp_ts *e1_ts;
|
|
int rc;
|
|
|
|
if (!ts->b_activated)
|
|
return 0;
|
|
|
|
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 = v5l->e1_line;
|
|
|
|
/* no line assigned */
|
|
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;
|
|
}
|
|
|
|
/* init gsmtap */
|
|
int gsmtap_init(void)
|
|
{
|
|
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);
|
|
}
|
|
|
|
return 0;
|
|
}
|
|
|
|
/* register signal to receive E1 events */
|
|
int l1_signal_init(void)
|
|
{
|
|
osmo_signal_register_handler(SS_L_INPUT, inp_sig_cb, NULL);
|
|
|
|
return 0;
|
|
}
|
|
|
|
/* init given E1 line and return e1inp_line structure pointer */
|
|
struct e1inp_line *e1_line_init(struct v5x_link *v5l, int e1_nr)
|
|
{
|
|
struct e1inp_line *e1_line;
|
|
int ts;
|
|
int rc;
|
|
|
|
e1_line = e1inp_line_find(e1_nr);
|
|
if (!e1_line)
|
|
return NULL;
|
|
|
|
/* link e1inp_line to v5l and vice versa */
|
|
/* must set ops before setting TS */
|
|
v5l->e1_line_ops.sign_link = v5le_rx_sign;
|
|
e1inp_line_bind_ops(e1_line, &v5l->e1_line_ops);
|
|
v5l->e1_line = e1_line;
|
|
|
|
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;
|
|
}
|
|
|
|
/* if config fails, remove link between e1inp_line and v5l */
|
|
rc = e1inp_line_update(e1_line);
|
|
if (rc < 0) {
|
|
e1_line_exit(v5l);
|
|
return NULL;
|
|
}
|
|
|
|
return e1_line;
|
|
}
|
|
|
|
void e1_line_exit(struct v5x_link *v5l)
|
|
{
|
|
struct e1inp_line *e1_line = v5l->e1_line;
|
|
int ts;
|
|
|
|
for (ts = 1; ts <= 31; ts++) {
|
|
struct e1inp_ts *e1_ts = &e1_line->ts[ts-1];
|
|
e1_ts->type = E1INP_TS_TYPE_NONE;
|
|
}
|
|
e1inp_line_update(e1_line);
|
|
|
|
e1inp_line_bind_ops(e1_line, NULL);
|
|
v5l->e1_line = NULL;
|
|
}
|