Several fixes for lapv5.c
This commit is contained in:
Andreas Eversberg
parent
e4ddaea00d
commit
7498809d2d
76
lapv5.c
76
lapv5.c
|
|
@ -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,7 +327,7 @@ 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 */
|
||||
|
|
@ -336,7 +335,7 @@ void lapv5_dl_data_req(struct lapv5_instance *li, uint8_t dladdr, struct msgb *m
|
|||
osmo_prim_init(&dp.oph, 0, PRIM_DL_DATA, PRIM_OP_REQUEST, msg);
|
||||
|
||||
/* send to L2 */
|
||||
lapd_recv_dlsap(&dp, &sap->dl.lctx);
|
||||
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;
|
||||
}
|
||||
|
|
|
|||
14
lapv5.h
14
lapv5.h
|
|
@ -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…
Reference in New Issue