/* LAPV5-DL as per Section 10.1 of ITU-T G.964 */ /* (C) 2021-2022 by Harald Welte * (C) 2022-2023 by Andreas Eversberg * * 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 #include #include #include #include #include #include #include #include #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; LOGLI(li, 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]; LOGLI(li, 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); 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; }; dladdr = ((msg->l2h[0] & 0xfe) << 5) | (msg->l2h[1] >> 1); if (!li->enabled) { LOGLI(li, LOGL_NOTICE, "LAPV5 frame ignored, because DL is disabled.\n"); if (dladdr == V5X_DLADDR_PSTN) LOGLI(li, LOGL_NOTICE, "PLEASE ENABLE EARLY PSTN DATALINK ENABLE AT INTERFACE CONFIG!\n"); msgb_free(msg); return -EINVAL; } memset(&lctx, 0, sizeof(lctx)); i = 0; /* adress field */ 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) { LOGV5L(link, 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) { LOGV5L(link, 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 */ LOGV5L(link, 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) { LOGV5L(link, 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)) { LOGV5UP(v5up, DV5EF, LOGL_NOTICE, "Dropping D-channel (AN->LE) message of non-operational ISDN port for EFaddr %d.\n", efaddr); msgb_free(msg); return -EIO; } LOGV5UP(v5up, 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) { LOGV5UP(v5up, 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)) { LOGV5UP(v5up, 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 */ LOGV5UP(v5up, 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; if (li->enabled == enabled) return; li->enabled = enabled; /* reset DL instances */ llist_for_each_entry(sap, &li->sap_list, list) { lapd_dl_reset(&sap->dl); } }