forked from retronetworking/osmo-v5
600 lines
17 KiB
C
600 lines
17 KiB
C
/* LAPV5-DL as per Section 10.1 of ITU-T G.964 */
|
|
|
|
/* (C) 2021 by Harald Welte <laforge@gnumonks.org>
|
|
*
|
|
* All Rights Reserved
|
|
*
|
|
* SPDX-License-Identifier: GPL-2.0+
|
|
*
|
|
* This program is free software; you can redistribute it and/or modify
|
|
* it under the terms of the GNU General Public License as published by
|
|
* the Free Software Foundation; either version 2 of the License, or
|
|
* (at your option) any later version.
|
|
*
|
|
* This program is distributed in the hope that it will be useful,
|
|
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
* GNU General Public License for more details.
|
|
*
|
|
* You should have received a copy of the GNU General Public License along
|
|
* with this program; if not, write to the Free Software Foundation, Inc.,
|
|
* 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
|
|
*
|
|
*/
|
|
|
|
#include <stdio.h>
|
|
#include <string.h>
|
|
#include <errno.h>
|
|
|
|
#include <osmocom/core/linuxlist.h>
|
|
#include <osmocom/core/talloc.h>
|
|
#include <osmocom/core/msgb.h>
|
|
#include <osmocom/core/timer.h>
|
|
#include <osmocom/abis/lapd.h>
|
|
#include <osmocom/abis/lapd_pcap.h>
|
|
|
|
#include "v5x_internal.h"
|
|
#include "v5x_protocol.h"
|
|
#include "v5x_le_port_fsm.h"
|
|
#include "lapv5.h"
|
|
#include "layer1.h"
|
|
#include "logging.h"
|
|
|
|
#define LAPD_ADDR1(sapi, cr) ((((sapi) & 0x3f) << 2) | (((cr) & 0x1) << 1))
|
|
#define LAPD_ADDR2(tei) ((((tei) & 0x7f) << 1) | 0x1)
|
|
|
|
#define LAPD_ADDR_SAPI(addr) ((addr) >> 2)
|
|
#define LAPD_ADDR_CR(addr) (((addr) >> 1) & 0x1)
|
|
#define LAPD_ADDR_EA(addr) ((addr) & 0x1)
|
|
#define LAPD_ADDR_TEI(addr) ((addr) >> 1)
|
|
|
|
#define LAPD_CTRL_I4(ns) (((ns) & 0x7f) << 1)
|
|
#define LAPD_CTRL_I5(nr, p) ((((nr) & 0x7f) << 1) | ((p) & 0x1))
|
|
#define LAPD_CTRL_S4(s) ((((s) & 0x3) << 2) | 0x1)
|
|
#define LAPD_CTRL_S5(nr, p) ((((nr) & 0x7f) << 1) | ((p) & 0x1))
|
|
#define LAPD_CTRL_U4(u, p) ((((u) & 0x1c) << (5-2)) | (((p) & 0x1) << 4) | (((u) & 0x3) << 2) | 0x3)
|
|
|
|
#define LAPD_CTRL_is_I(ctrl) (((ctrl) & 0x1) == 0)
|
|
#define LAPD_CTRL_is_S(ctrl) (((ctrl) & 0x3) == 1)
|
|
#define LAPD_CTRL_is_U(ctrl) (((ctrl) & 0x3) == 3)
|
|
|
|
#define LAPD_CTRL_U_BITS(ctrl) ((((ctrl) & 0xC) >> 2) | ((ctrl) & 0xE0) >> 3)
|
|
#define LAPD_CTRL_U_PF(ctrl) (((ctrl) >> 4) & 0x1)
|
|
|
|
#define LAPD_CTRL_S_BITS(ctrl) (((ctrl) & 0xC) >> 2)
|
|
#define LAPD_CTRL_S_PF(ctrl) (ctrl & 0x1)
|
|
|
|
#define LAPD_CTRL_I_Ns(ctrl) (((ctrl) & 0xFE) >> 1)
|
|
#define LAPD_CTRL_I_P(ctrl) (ctrl & 0x1)
|
|
#define LAPD_CTRL_Nr(ctrl) (((ctrl) & 0xFE) >> 1)
|
|
|
|
#define LAPD_LEN(len) ((len << 2) | 0x1)
|
|
#define LAPD_EL 0x1
|
|
|
|
#define LAPD_SET_K(n, o) {n,o,o,o,o,o,o,o,o,o,o,o,o,o,o,o,o,o,o,o,o,o,o,o,o,o,o,o,o,o,o,o,o,o,o,o,o,o,o,o,o,o,o,o,o,o,o,o,o,o,o,o,o,o,o,o,o,o,o,o,o,o,o,o}
|
|
|
|
#define LOGLI(li, level, fmt, args ...) \
|
|
LOGP(DLLAPD, level, "(%s): " fmt, (li)->name, ## args)
|
|
|
|
#define LOGSAP(sap, level, fmt, args ...) \
|
|
LOGP(DLLAPD, level, "(%s): " fmt, (sap)->dl.name, ## args)
|
|
|
|
#define DLSAP_MSGB_SIZE 128
|
|
#define DLSAP_MSGB_HEADROOM 56
|
|
|
|
const struct lapd_profile lapd_profile_lapv5dl = {
|
|
.k = LAPD_SET_K(7,7),
|
|
.n200 = 3,
|
|
.n201 = 260,
|
|
.n202 = 3,
|
|
.t200_sec = 1, .t200_usec = 0,
|
|
.t203_sec = 10, .t203_usec = 0,
|
|
.short_address = 0
|
|
};
|
|
|
|
|
|
/* Structure representing a SAP within a TEI. It includes exactly one datalink
|
|
* instance. */
|
|
struct lapv5_sap {
|
|
struct llist_head list; /* list of SAP within instance */
|
|
struct lapv5_instance *li; /* back-pointer */
|
|
uint16_t dladdr; /* V5-Data Link Address */
|
|
|
|
struct lapd_datalink dl; /* the actual data link */
|
|
};
|
|
|
|
/* Find a SAP within a given TEI */
|
|
static struct lapv5_sap *lapv5_sap_find(struct lapv5_instance *li, uint16_t dladdr)
|
|
{
|
|
struct lapv5_sap *sap;
|
|
|
|
llist_for_each_entry(sap, &li->sap_list, list) {
|
|
if (sap->dladdr == dladdr)
|
|
return sap;
|
|
}
|
|
|
|
return NULL;
|
|
}
|
|
|
|
static int send_ph_data_req(struct lapd_msg_ctx *lctx, struct msgb *msg);
|
|
static int send_dlsap(struct osmo_dlsap_prim *dp, struct lapd_msg_ctx *lctx);
|
|
|
|
/* Allocate a new SAP within a given instance */
|
|
static struct lapv5_sap *lapv5_sap_alloc(struct lapv5_instance *li, uint16_t v5dladdr)
|
|
{
|
|
struct lapv5_sap *sap;
|
|
struct lapd_datalink *dl;
|
|
struct lapd_profile *profile;
|
|
char name[256];
|
|
int k;
|
|
|
|
snprintf(name, sizeof(name), "%s-%u", li->name, v5dladdr);
|
|
|
|
sap = talloc_zero(li, struct lapv5_sap);
|
|
if (!sap)
|
|
return NULL;
|
|
|
|
LOGP(DLLAPD, LOGL_DEBUG, "(%s): LAPV5 Allocating SAP for V5_DLADDR=%u (dl=%p, sap=%p)\n",
|
|
name, v5dladdr, &sap->dl, sap);
|
|
|
|
sap->li = li;
|
|
sap->dladdr = v5dladdr;
|
|
dl = &sap->dl;
|
|
profile = &li->profile;
|
|
|
|
k = profile->k[0];
|
|
LOGP(DLLAPD, LOGL_DEBUG, "(%s): k=%d N200=%d N201=%d T200=%d.%d T203=%d.%d\n",
|
|
name, k, profile->n200, profile->n201, profile->t200_sec,
|
|
profile->t200_usec, profile->t203_sec, profile->t203_usec);
|
|
lapd_dl_init2(dl, k, 128, profile->n201, name);
|
|
dl->use_sabme = 1; /* use SABME instead of SABM (GSM) */
|
|
dl->send_ph_data_req = send_ph_data_req;
|
|
dl->send_dlsap = send_dlsap;
|
|
dl->n200 = profile->n200;
|
|
dl->n200_est_rel = profile->n200;
|
|
dl->t200_sec = profile->t200_sec; dl->t200_usec = profile->t200_usec;
|
|
dl->t203_sec = profile->t203_sec; dl->t203_usec = profile->t203_usec;
|
|
dl->lctx.dl = &sap->dl;
|
|
dl->lctx.sapi = v5dladdr >> 7;
|
|
dl->lctx.tei = v5dladdr & 0x7f;
|
|
dl->lctx.n201 = profile->n201;
|
|
|
|
lapd_set_mode(&sap->dl, (li->network_side) ? LAPD_MODE_NETWORK : LAPD_MODE_USER);
|
|
|
|
llist_add(&sap->list, &li->sap_list);
|
|
|
|
return sap;
|
|
}
|
|
|
|
/* Free SAP instance, including the datalink */
|
|
static void lapv5_sap_free(struct lapv5_sap *sap)
|
|
{
|
|
LOGSAP(sap, LOGL_DEBUG, "LAPV5 Freeing SAP for DLADDR=%u (dl=%p, sap=%p)\n", sap->dladdr, &sap->dl, sap);
|
|
|
|
/* free datalink structures and timers */
|
|
lapd_dl_exit(&sap->dl);
|
|
|
|
llist_del(&sap->list);
|
|
talloc_free(sap);
|
|
}
|
|
|
|
/* Receive Data (PH-DATA indication) on the given LAPD Instance */
|
|
int lapv5_ph_data_ind(struct lapv5_instance *li, struct msgb *msg, int *error)
|
|
{
|
|
struct lapd_msg_ctx lctx;
|
|
struct lapv5_sap *sap;
|
|
uint16_t dladdr;
|
|
int i;
|
|
|
|
/* write to PCAP file, if enabled. */
|
|
osmo_pcap_lapd_write(li->pcap_fd, OSMO_LAPD_PCAP_INPUT, msg);
|
|
|
|
if (!li->enabled) {
|
|
LOGLI(li, LOGL_DEBUG, "LAPV5 frame ignored, because DL is disabled.\n");
|
|
msgb_free(msg);
|
|
return -EINVAL;
|
|
}
|
|
|
|
msgb_pull(msg, msg->l2h - msg->data);
|
|
|
|
LOGLI(li, LOGL_DEBUG, "RX: %s\n", osmo_hexdump(msg->data, msg->len));
|
|
if (msg->len < 2) {
|
|
LOGLI(li, LOGL_ERROR, "LAPV5 frame receive len %d < 2, ignoring\n", msg->len);
|
|
*error = LAPD_ERR_BAD_LEN;
|
|
msgb_free(msg);
|
|
return -EINVAL;
|
|
};
|
|
|
|
memset(&lctx, 0, sizeof(lctx));
|
|
|
|
i = 0;
|
|
/* adress field */
|
|
dladdr = ((msg->l2h[0] & 0xfe) << 5) | (msg->l2h[1] >> 1);
|
|
lctx.lpd = 0;
|
|
lctx.sapi = dladdr >> 7;
|
|
lctx.tei = dladdr & 0x7f;
|
|
lctx.cr = LAPD_ADDR_CR(msg->l2h[0]);
|
|
i += 2;
|
|
|
|
/* control field */
|
|
if (LAPD_CTRL_is_I(msg->l2h[i])) {
|
|
lctx.format = LAPD_FORM_I;
|
|
lctx.n_send = LAPD_CTRL_I_Ns(msg->l2h[i]);
|
|
i++;
|
|
if (msg->len < 4) {
|
|
LOGLI(li, LOGL_ERROR, "LAPV5 I frame receive len %d < 4, ignoring\n", msg->len);
|
|
*error = LAPD_ERR_BAD_LEN;
|
|
msgb_free(msg);
|
|
return -EINVAL;
|
|
}
|
|
lctx.n_recv = LAPD_CTRL_Nr(msg->l2h[i]);
|
|
lctx.p_f = LAPD_CTRL_I_P(msg->l2h[i]);
|
|
} else if (LAPD_CTRL_is_S(msg->l2h[i])) {
|
|
lctx.format = LAPD_FORM_S;
|
|
lctx.s_u = LAPD_CTRL_S_BITS(msg->l2h[i]);
|
|
i++;
|
|
if (msg->len < 4 && i == 3) {
|
|
LOGLI(li, LOGL_ERROR, "LAPV5 S frame receive len %d < 4, ignoring\n", msg->len);
|
|
*error = LAPD_ERR_BAD_LEN;
|
|
msgb_free(msg);
|
|
return -EINVAL;
|
|
}
|
|
lctx.n_recv = LAPD_CTRL_Nr(msg->l2h[i]);
|
|
lctx.p_f = LAPD_CTRL_S_PF(msg->l2h[i]);
|
|
} else if (LAPD_CTRL_is_U(msg->l2h[i])) {
|
|
lctx.format = LAPD_FORM_U;
|
|
lctx.s_u = LAPD_CTRL_U_BITS(msg->l2h[i]);
|
|
lctx.p_f = LAPD_CTRL_U_PF(msg->l2h[i]);
|
|
} else
|
|
lctx.format = LAPD_FORM_UKN;
|
|
i++;
|
|
/* length */
|
|
msg->l3h = msg->l2h + i;
|
|
msgb_pull(msg, i);
|
|
lctx.length = msg->len;
|
|
|
|
sap = lapv5_sap_find(li, dladdr);
|
|
if (!sap) {
|
|
LOGLI(li, LOGL_INFO, "LAPV5 No SAP for DLADDR=%u, allocating\n", dladdr);
|
|
sap = lapv5_sap_alloc(li, dladdr);
|
|
if (!sap) {
|
|
*error = LAPD_ERR_NO_MEM;
|
|
msgb_free(msg);
|
|
return -ENOMEM;
|
|
}
|
|
}
|
|
lctx.dl = &sap->dl;
|
|
lctx.n201 = lctx.dl->maxf;
|
|
|
|
if (msg->len > lctx.n201) {
|
|
LOGSAP(sap, LOGL_ERROR, "message len %d > N201(%d) (discarding)\n", msg->len, lctx.n201);
|
|
msgb_free(msg);
|
|
*error = LAPD_ERR_BAD_LEN;
|
|
return -EINVAL;
|
|
}
|
|
|
|
/* send to LAPD */
|
|
return lapd_ph_data_ind(msg, &lctx);
|
|
}
|
|
|
|
/* Start a (LE-side) SAP for the specified TEI/SAPI on the LAPD instance */
|
|
int lapv5_dl_est_req(struct lapv5_instance *li, uint16_t dladdr)
|
|
{
|
|
struct lapv5_sap *sap;
|
|
struct osmo_dlsap_prim dp;
|
|
struct msgb *msg;
|
|
|
|
sap = lapv5_sap_find(li, dladdr);
|
|
if (!sap) {
|
|
sap = lapv5_sap_alloc(li, dladdr);
|
|
if (!sap)
|
|
return -ENOMEM;
|
|
}
|
|
|
|
LOGSAP(sap, LOGL_DEBUG, "LAPV5 DL-ESTABLISH request DLADDR=%u\n", dladdr);
|
|
|
|
/* prepare prim */
|
|
msg = msgb_alloc_headroom(DLSAP_MSGB_SIZE, DLSAP_MSGB_HEADROOM, "DL EST");
|
|
msg->l3h = msg->data;
|
|
memset(&dp, 0, sizeof(dp));
|
|
osmo_prim_init(&dp.oph, 0, PRIM_DL_EST, PRIM_OP_REQUEST, msg);
|
|
|
|
/* send to L2 */
|
|
return lapd_recv_dlsap(&dp, &sap->dl.lctx);
|
|
}
|
|
|
|
/* Stop a (LE-side) SAP for the specified TEI/SAPI on the LAPD instance */
|
|
int lapv5_dl_rel_req(struct lapv5_instance *li, uint16_t dladdr)
|
|
{
|
|
struct lapv5_sap *sap;
|
|
struct osmo_dlsap_prim dp;
|
|
struct msgb *msg;
|
|
|
|
sap = lapv5_sap_find(li, dladdr);
|
|
if (!sap)
|
|
return -ENODEV;
|
|
|
|
LOGSAP(sap, LOGL_DEBUG, "LAPV5 DL-RELEASE request DLADDR=%u\n", dladdr);
|
|
|
|
/* prepare prim */
|
|
msg = msgb_alloc_headroom(DLSAP_MSGB_SIZE, DLSAP_MSGB_HEADROOM, "DL REL");
|
|
msg->l3h = msg->data;
|
|
memset(&dp, 0, sizeof(dp));
|
|
osmo_prim_init(&dp.oph, 0, PRIM_DL_REL, PRIM_OP_REQUEST, msg);
|
|
|
|
/* send to L2 */
|
|
return lapd_recv_dlsap(&dp, &sap->dl.lctx);
|
|
}
|
|
|
|
/* Transmit Data (DL-DATA request) on the given LAPD Instance / DLADDR */
|
|
int lapv5_dl_data_req(struct lapv5_instance *li, uint16_t dladdr, struct msgb *msg)
|
|
{
|
|
struct lapv5_sap *sap;
|
|
struct osmo_dlsap_prim dp;
|
|
|
|
sap = lapv5_sap_find(li, dladdr);
|
|
if (!sap) {
|
|
LOGLI(li, LOGL_NOTICE, "LAPV5 Tx on unknown DLADDR=%u\n", dladdr);
|
|
msgb_free(msg);
|
|
return -EINVAL;
|
|
}
|
|
|
|
/* prepare prim */
|
|
msg->l3h = msg->data;
|
|
memset(&dp, 0, sizeof(dp));
|
|
osmo_prim_init(&dp.oph, 0, PRIM_DL_DATA, PRIM_OP_REQUEST, msg);
|
|
|
|
/* send to L2 */
|
|
return lapd_recv_dlsap(&dp, &sap->dl.lctx);
|
|
};
|
|
|
|
static int send_ph_data_req(struct lapd_msg_ctx *lctx, struct msgb *msg)
|
|
{
|
|
struct lapd_datalink *dl = lctx->dl;
|
|
struct lapv5_sap *sap = container_of(dl, struct lapv5_sap, dl);
|
|
struct lapv5_instance *li = sap->li;
|
|
int format = lctx->format;
|
|
|
|
/* control field */
|
|
switch (format) {
|
|
case LAPD_FORM_I:
|
|
msg->l2h = msgb_push(msg, 2);
|
|
msg->l2h[0] = LAPD_CTRL_I4(lctx->n_send);
|
|
msg->l2h[1] = LAPD_CTRL_I5(lctx->n_recv, lctx->p_f);
|
|
break;
|
|
case LAPD_FORM_S:
|
|
msg->l2h = msgb_push(msg, 2);
|
|
msg->l2h[0] = LAPD_CTRL_S4(lctx->s_u);
|
|
msg->l2h[1] = LAPD_CTRL_S5(lctx->n_recv, lctx->p_f);
|
|
break;
|
|
case LAPD_FORM_U:
|
|
msg->l2h = msgb_push(msg, 1);
|
|
msg->l2h[0] = LAPD_CTRL_U4(lctx->s_u, lctx->p_f);
|
|
break;
|
|
default:
|
|
msgb_free(msg);
|
|
return -EINVAL;
|
|
}
|
|
/* address field */
|
|
msg->l2h = msgb_push(msg, 2);
|
|
msg->l2h[0] = LAPD_ADDR1(lctx->sapi, lctx->cr);
|
|
msg->l2h[1] = LAPD_ADDR2(lctx->tei);
|
|
|
|
/* write to PCAP file, if enabled. */
|
|
osmo_pcap_lapd_write(li->pcap_fd, OSMO_LAPD_PCAP_OUTPUT, msg);
|
|
|
|
/* forward frame to L1 */
|
|
LOGDL(dl, LOGL_DEBUG, "TX: %s\n", osmo_hexdump(msg->data, msg->len));
|
|
li->ph_data_req_cb(msg, li->ph_data_req_cbdata);
|
|
|
|
return 0;
|
|
}
|
|
|
|
/* A DL-SAP message is received from datalink instance and forwarded to L3 */
|
|
static int send_dlsap(struct osmo_dlsap_prim *dp, struct lapd_msg_ctx *lctx)
|
|
{
|
|
struct lapd_datalink *dl = lctx->dl;
|
|
struct lapv5_sap *sap = container_of(dl, struct lapv5_sap, dl);
|
|
struct lapv5_instance *li;
|
|
const char *op = (dp->oph.operation == PRIM_OP_INDICATION) ? "indication" : "confirm";
|
|
uint16_t dladdr;
|
|
|
|
li = sap->li;
|
|
|
|
dladdr = lctx->sapi << 7 | lctx->tei;
|
|
|
|
switch (dp->oph.primitive) {
|
|
case PRIM_DL_EST:
|
|
LOGDL(dl, LOGL_DEBUG, "LAPD DL-ESTABLISH %s DLADDR=%u\n", op, dladdr);
|
|
break;
|
|
case PRIM_DL_REL:
|
|
LOGDL(dl, LOGL_DEBUG, "LAPD DL-RELEASE %s DLADDR=%u\n", op, dladdr);
|
|
lapv5_sap_free(sap);
|
|
/* note: sap and dl is now gone, don't use it anymore */
|
|
break;
|
|
default:
|
|
;
|
|
}
|
|
|
|
li->dl_receive_cb(dp, dladdr, li->dl_receive_cbdata);
|
|
|
|
return 0;
|
|
}
|
|
|
|
/* Allocate a new LAPV5 instance */
|
|
struct lapv5_instance *lapv5_instance_alloc(int network_side,
|
|
int (*ph_data_req_cb)(struct msgb *msg, void *cbdata), void *ph_data_req_cbdata,
|
|
int (*dl_receive_cb)(struct osmo_dlsap_prim *odp, uint16_t dladdr, void *rx_cbdata), void *dl_receive_cbdata,
|
|
const struct lapd_profile *profile, const char *name)
|
|
{
|
|
struct lapv5_instance *li;
|
|
|
|
li = talloc_zero(NULL, struct lapv5_instance);
|
|
if (!li)
|
|
return NULL;
|
|
|
|
li->network_side = network_side;
|
|
li->ph_data_req_cb = ph_data_req_cb;
|
|
li->ph_data_req_cbdata = ph_data_req_cbdata;
|
|
li->dl_receive_cb = dl_receive_cb;
|
|
li->dl_receive_cbdata = dl_receive_cbdata;
|
|
li->pcap_fd = -1;
|
|
li->name = talloc_strdup(li, name);
|
|
memcpy(&li->profile, profile, sizeof(li->profile));
|
|
|
|
INIT_LLIST_HEAD(&li->sap_list);
|
|
|
|
return li;
|
|
}
|
|
|
|
|
|
/* Change lapd-profile on the fly (use with caution!) */
|
|
void lapv5_instance_set_profile(struct lapv5_instance *li, const struct lapd_profile *profile)
|
|
{
|
|
memcpy(&li->profile, profile, sizeof(li->profile));
|
|
}
|
|
|
|
void lapv5_instance_free(struct lapv5_instance *li)
|
|
{
|
|
struct lapv5_sap *sap, *sap2;
|
|
|
|
/* Free all SAP instances */
|
|
llist_for_each_entry_safe(sap, sap2, &li->sap_list, list) {
|
|
lapv5_sap_free(sap);
|
|
}
|
|
|
|
talloc_free(li);
|
|
}
|
|
|
|
/***********************************************************************
|
|
* LAPV5-EF (Encapsulation Function)
|
|
***********************************************************************/
|
|
|
|
/* The LAPV5-EF adds another layer around of the LAPD-style LAPV5-DL. It consists of
|
|
* a two-byte header in front of the normal data link header. Examples see Figure E.1/G.964 */
|
|
|
|
|
|
/* main entry point for receiving signaling frames from the AN. The assumption is that
|
|
* the flag octets are removed and the msg->data points to the first octet of the EFaddr,
|
|
* while the last octet (before msg->tail) points to the last FCS octet. */
|
|
int lapv5ef_rx(struct v5x_link *link, struct msgb *msg)
|
|
{
|
|
struct lapv5_instance *li = NULL;
|
|
struct v5x_user_port *v5up = NULL;
|
|
uint16_t efaddr, efaddr_enc;
|
|
bool is_isdn;
|
|
int error;
|
|
|
|
msg->l1h = msg->data;
|
|
|
|
if (msgb_length(msg) < 2) {
|
|
LOGP(DV5EF, LOGL_ERROR, "Frame too short.\n");
|
|
msgb_free(msg);
|
|
return -EINVAL;
|
|
}
|
|
|
|
efaddr_enc = msg->l2h[0] << 8 | msg->l2h[1];
|
|
msg->l2h += 2;
|
|
msgb_pull(msg, msg->l2h - msg->data);
|
|
efaddr = v5x_l3_addr_dec(efaddr_enc, &is_isdn);
|
|
|
|
|
|
if (!is_isdn) {
|
|
LOGP(DV5EF, LOGL_ERROR, "EFaddr are structured like isdn-type L3 Address.\n");
|
|
msgb_free(msg);
|
|
return -EINVAL;
|
|
}
|
|
|
|
switch (efaddr) {
|
|
case V5X_DLADDR_PSTN:
|
|
/* hand-over to LAPD-DL instance for PSTN */
|
|
li = link->interface->pstn.li;
|
|
break;
|
|
case V5X_DLADDR_CTRL:
|
|
/* hand-over to LAPD-DL instance for CTRL */
|
|
li = link->interface->control.li;
|
|
break;
|
|
case V52_DLADDR_BCC:
|
|
/* hand-over to LAPD-DL instance for BCC */
|
|
li = link->interface->bcc.li;
|
|
break;
|
|
case V52_DLADDR_PROTECTION:
|
|
/* hand-over to LAPD-DL instance for PROTECTION */
|
|
if (link == link->interface->primary_link)
|
|
li = link->interface->protection.li[0];
|
|
if (link == link->interface->secondary_link)
|
|
li = link->interface->protection.li[1];
|
|
break;
|
|
case V52_DLADDR_LCP:
|
|
/* hand-over to LAPD-DL instance for LCP */
|
|
li = link->interface->lcp.li;
|
|
break;
|
|
default:
|
|
if (efaddr >= 8176) {
|
|
/* reserved as per Section 9.2.2.2 of G.964 */
|
|
LOGP(DV5EF, LOGL_ERROR, "No LAPV5 protocol for EFaddr %d.\n", efaddr);
|
|
msgb_free(msg);
|
|
return -EINVAL;
|
|
}
|
|
/* relay function for LAPD of user ports */
|
|
v5up = v5x_user_port_find(link->interface, efaddr, true);
|
|
if (!v5up) {
|
|
LOGP(DV5EF, LOGL_ERROR, "No ISDN user port instance for EFaddr %d created.\n", efaddr);
|
|
msgb_free(msg);
|
|
return -EINVAL;
|
|
}
|
|
if (!v5x_le_port_isdn_is_operational(v5up->port_fi)) {
|
|
LOGP(DV5EF, LOGL_NOTICE, "Dropping D-channel (AN->LE) message of non-operational ISDN port for EFaddr %d.\n", efaddr);
|
|
msgb_free(msg);
|
|
return -EIO;
|
|
}
|
|
LOGP(DV5EF, LOGL_DEBUG, "Recevied frame for EFaddr %d: %s\n", efaddr, osmo_hexdump(msg->data, msg->len));
|
|
ph_socket_tx_msg(&v5up->ph_socket, 3, PH_PRIM_DATA_IND, msg->data, msg->len);
|
|
msgb_free(msg);
|
|
return 0;
|
|
}
|
|
|
|
if (!li) {
|
|
LOGP(DV5EF, LOGL_ERROR, "No LAPV5 instance for EFaddr %d created.\n", efaddr);
|
|
msgb_free(msg);
|
|
return -EINVAL;
|
|
}
|
|
|
|
lapv5_ph_data_ind(li, msg, &error);
|
|
|
|
return 0;
|
|
}
|
|
|
|
int lapv5ef_tx(struct v5x_user_port *v5up, struct msgb *msg)
|
|
{
|
|
uint16_t efaddr = v5up->nr, efaddr_enc;
|
|
|
|
if (!v5x_le_port_isdn_is_operational(v5up->port_fi)) {
|
|
LOGP(DV5EF, LOGL_NOTICE, "Dropping D-channel (LE->AN) message of non-operational ISDN port for EFaddr %d.\n", efaddr);
|
|
msgb_free(msg);
|
|
return -EIO;
|
|
}
|
|
/* relay function for LAPD of user ports */
|
|
LOGP(DV5EF, LOGL_DEBUG, "Sending frame for EFaddr %d: %s\n", efaddr, osmo_hexdump(msg->data, msg->len));
|
|
msg->l2h = msgb_push(msg, 2);
|
|
efaddr_enc = v5x_l3_addr_enc(efaddr, 1);
|
|
msg->l2h[0] = efaddr_enc >> 8;
|
|
msg->l2h[1] = efaddr_enc;
|
|
ph_data_req_hdlc(msg, v5up->interface);
|
|
|
|
return 0;
|
|
}
|
|
|
|
/* set enabled state, also free all SAP, so it will disable/enable with a cleanly */
|
|
void lapv5_set_enabled(struct lapv5_instance *li, bool enabled)
|
|
{
|
|
struct lapv5_sap *sap;
|
|
|
|
li->enabled = enabled;
|
|
|
|
/* reset DL instances */
|
|
llist_for_each_entry(sap, &li->sap_list, list) {
|
|
lapd_dl_reset(&sap->dl);
|
|
}
|
|
}
|