Several fixes for lapv5.c

jolly-gsmtap
Andreas Eversberg 2 months ago
parent e4ddaea00d
commit 7498809d2d
  1. 80
      lapv5.c
  2. 14
      lapv5.h

@ -24,6 +24,7 @@
#include "v5x_internal.h"
#include "v5x_protocol.h"
#include "logging.h"
#include "lapv5.h"
#include <stdio.h>
@ -31,15 +32,14 @@
#include <errno.h>
#include <osmocom/core/linuxlist.h>
#include <osmocom/core/logging.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>
#define LAPD_ADDR2(sapi, cr) ((((sapi) & 0x3f) << 2) | (((cr) & 0x1) << 1))
#define LAPD_ADDR3(tei) ((((tei) & 0x7f) << 1) | 0x1)
#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)
@ -135,6 +135,7 @@ static struct lapv5_sap *lapv5_sap_alloc(struct lapv5_instance *li, uint16_t v5d
LOGP(DLLAPD, LOGL_NOTICE, "(%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;
@ -152,8 +153,8 @@ static struct lapv5_sap *lapv5_sap_alloc(struct lapv5_instance *li, uint16_t v5d
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 & 0xff;
dl->lctx.tei = v5dladdr >> 8;
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);
@ -181,33 +182,29 @@ 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;
bool is_isdn;
int i;
/* write to PCAP file, if enabled. */
osmo_pcap_lapd_write(li->pcap_fd, OSMO_LAPD_PCAP_INPUT, msg);
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;
};
msg->l2h = msg->data;
memset(&lctx, 0, sizeof(lctx));
i = 0;
/* adress field */
dladdr = v51_l3_addr_dec(msg->l2h[0] << 8 | msg->l2h[1], &is_isdn);
if (!is_isdn) {
LOGLI(li, LOGL_ERROR, "LAPV5 frame with single-octet addr not permitted\n");
*error = LAPD_ERR_BAD_LEN;
return -EINVAL;
};
dladdr = ((msg->l2h[0] & 0xfe) << 5) | (msg->l2h[1] >> 1);
lctx.lpd = 0;
lctx.tei = dladdr >> 8;
lctx.sapi = dladdr & 0xff;
lctx.sapi = dladdr >> 7;
lctx.tei = dladdr & 0x7f;
lctx.cr = LAPD_ADDR_CR(msg->l2h[0]);
i += 2;
@ -219,6 +216,7 @@ int lapv5_ph_data_ind(struct lapv5_instance *li, struct msgb *msg, int *error)
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]);
@ -230,6 +228,7 @@ int lapv5_ph_data_ind(struct lapv5_instance *li, struct msgb *msg, int *error)
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]);
@ -319,7 +318,7 @@ int lapv5_dl_rel_req(struct lapv5_instance *li, uint16_t dladdr)
}
/* Transmit Data (DL-DATA request) on the given LAPD Instance / DLADDR */
void lapv5_dl_data_req(struct lapv5_instance *li, uint8_t dladdr, struct msgb *msg)
int lapv5_dl_data_req(struct lapv5_instance *li, uint16_t dladdr, struct msgb *msg)
{
struct lapv5_sap *sap;
struct osmo_dlsap_prim dp;
@ -328,15 +327,15 @@ void lapv5_dl_data_req(struct lapv5_instance *li, uint8_t dladdr, struct msgb *m
if (!sap) {
LOGLI(li, LOGL_INFO, "LAPV5 Tx on unknown DLADDR=%u\n", dladdr);
msgb_free(msg);
return;
return -EINVAL;
}
/* prepare prim */
msg->l3h = msg->data;
osmo_prim_init(&dp.oph, 0, PRIM_DL_DATA, PRIM_OP_REQUEST, msg);
osmo_prim_init(&dp.oph, 0, PRIM_DL_DATA, PRIM_OP_REQUEST, msg);
/* send to L2 */
lapd_recv_dlsap(&dp, &sap->dl.lctx);
/* 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)
@ -368,8 +367,8 @@ static int send_ph_data_req(struct lapd_msg_ctx *lctx, struct msgb *msg)
}
/* address field */
msg->l2h = msgb_push(msg, 2);
msg->l2h[0] = LAPD_ADDR2(lctx->sapi, lctx->cr);
msg->l2h[1] = LAPD_ADDR3(lctx->tei);
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);
@ -392,7 +391,7 @@ static int send_dlsap(struct osmo_dlsap_prim *dp, struct lapd_msg_ctx *lctx)
li = sap->li;
dladdr = lctx->tei << 8 | lctx->sapi;
dladdr = lctx->sapi << 7 | lctx->tei;
switch (dp->oph.primitive) {
case PRIM_DL_EST:
@ -414,8 +413,8 @@ static int send_dlsap(struct osmo_dlsap_prim *dp, struct lapd_msg_ctx *lctx)
/* Allocate a new LAPV5 instance */
struct lapv5_instance *lapv5_instance_alloc(int network_side,
void (*ph_data_req_cb)(struct msgb *msg, void *cbdata), void *ph_data_req_cbdata,
void (*dl_receive_cb)(struct osmo_dlsap_prim *odp, uint16_t dladdr, void *rx_cbdata), void *dl_receive_cbdata,
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;
@ -470,23 +469,27 @@ void lapv5_instance_free(struct lapv5_instance *li)
* while the last octet (before msg->tail) points to the last FCS octet. */
int lapv5ef_rx(struct v5x_link *link, struct msgb *msg)
{
struct lapd_datalink *dl;
struct lapv5_instance *li;
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;
}
msg->l2h = msg->l1h + 2;
efaddr_enc = msg->l1h[0] << 8 | msg->l1h[1];
efaddr_enc = msg->l2h[0] << 8 | msg->l2h[1];
msg->l2h += 2;
msgb_pull(msg, msg->l2h - msg->data);
efaddr = v51_l3_addr_dec(efaddr_enc, &is_isdn);
if (!is_isdn) {
/* EFaddr are structured like isdn-type L3 Address */
LOGP(DV5EF, LOGL_ERROR, "EFaddr are structured like isdn-type L3 Address.\n");
msgb_free(msg);
return -EINVAL;
}
@ -494,32 +497,33 @@ int lapv5ef_rx(struct v5x_link *link, struct msgb *msg)
switch (efaddr) {
case V51_DLADDR_PSTN:
/* hand-over to LAPD-DL instance for PSTN */
dl = &link->interface->pstn.dl;
li = link->interface->pstn.li;
/* TODO */
break;
case V51_DLADDR_CTRL:
/* hand-over to LAPD-DL instance for CTRL */
/* TODO */
dl = &link->interface->control.dl;
li = link->interface->control.li;
break;
case V52_DLADDR_BCC:
dl = &link->interface->bcc.dl;
li = link->interface->bcc.li;
/* TOOD: implement V5.2 */
msgb_free(msg);
break;
case V52_DLADDR_PROTECTION:
dl = &link->interface->protection[0].dl;
li = link->interface->protection[0].li;
/* TOOD: implement V5.2 */
msgb_free(msg);
break;
case V52_DLADDR_LCP:
dl = &link->interface->lcp.dl;
li = link->interface->lcp.li;
/* TOOD: implement V5.2 */
msgb_free(msg);
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;
}
@ -527,5 +531,13 @@ int lapv5ef_rx(struct v5x_link *link, struct msgb *msg)
/* TODO */
}
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;
}

@ -6,9 +6,9 @@ struct lapv5_instance {
struct llist_head list; /* list of LAPV5 instances */
bool network_side;
void (*ph_data_req_cb)(struct msgb *msg, void *cbdata);
int (*ph_data_req_cb)(struct msgb *msg, void *cbdata);
void *ph_data_req_cbdata;
void (*dl_receive_cb)(struct osmo_dlsap_prim *odp, uint16_t dladdr, void *rx_cbdata);
int (*dl_receive_cb)(struct osmo_dlsap_prim *odp, uint16_t dladdr, void *rx_cbdata);
void *dl_receive_cbdata;
struct lapd_profile profile; /* must be a copy */
@ -18,15 +18,19 @@ struct lapv5_instance {
char *name; /* human-readable name */
};
extern const struct lapd_profile lapd_profile_lapv5dl;
int lapv5ef_rx(struct v5x_link *link, struct msgb *msg);
int lapv5_dl_est_req(struct lapv5_instance *li, uint16_t dladdr);
int lapv5_dl_rel_req(struct lapv5_instance *li, uint16_t dladdr);
void lapv5_dl_data_req(struct lapv5_instance *li, uint8_t dladdr, struct msgb *msg);
int lapv5_dl_data_req(struct lapv5_instance *li, uint16_t dladdr, struct msgb *msg);
int lapv5_ph_data_ind(struct lapv5_instance *li, struct msgb *msg, int *error);
struct lapv5_instance *lapv5_instance_alloc(int network_side,
void (*ph_data_req_cb)(struct msgb *msg, void *cbdata), void *ph_data_req_cbdata,
void (*dl_receive_cb)(struct osmo_dlsap_prim *odp, uint16_t dladdr, void *rx_cbdata), void *dl_receive_cbdata,
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);
int lapv5ef_rx(struct v5x_link *link, struct msgb *msg);

Loading…
Cancel
Save