diff --git a/lapv5.c b/lapv5.c index b142544..80a2533 100644 --- a/lapv5.c +++ b/lapv5.c @@ -24,6 +24,7 @@ #include "v5x_internal.h" #include "v5x_protocol.h" +#include "logging.h" #include "lapv5.h" #include @@ -31,15 +32,14 @@ #include #include -#include #include #include #include #include #include -#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; } diff --git a/lapv5.h b/lapv5.h index da5c057..612e0a4 100644 --- a/lapv5.h +++ b/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);