/* write a frame into the tx queue */
static int tx_ph_data_enqueue(struct lapdm_datalink *dl, struct msgb *msg,
uint8_t chan_nr, uint8_t link_id, uint8_t pad)
{
struct lapdm_entity *le = dl->entity;
struct osmo_phsap_prim pp;
/* if there is a pending message, queue it */
if (le->tx_pending || le->flags & LAPDM_ENT_F_POLLING_ONLY) {
*msgb_push(msg, 1) = pad;
*msgb_push(msg, 1) = link_id;
*msgb_push(msg, 1) = chan_nr;
msgb_enqueue(&dl->dl.tx_queue, msg);
return -EBUSY;
}
osmo_prim_init(&pp.oph, SAP_GSM_PH, PRIM_PH_DATA,
PRIM_OP_REQUEST, msg);
pp.u.data.chan_nr = chan_nr;
pp.u.data.link_id = link_id;
/* send the frame now */
le->tx_pending = 0; /* disabled flow control */
lapdm_pad_msgb(msg, pad);
return le->l1_prim_cb(&pp.oph, le->l1_ctx);
}
void ipa_prepend_header_ext(struct msgb *msg, int proto)
{
struct ipaccess_head_ext *hh_ext;
/* prepend the osmo ip.access header extension */
hh_ext = (struct ipaccess_head_ext *) msgb_push(msg, sizeof(*hh_ext));
hh_ext->proto = proto;
}
static void ipaccess_prepend_header_quirk(struct msgb *msg, int proto)
{
struct ipaccess_head *hh;
/* prepend the ip.access header */
hh = (struct ipaccess_head *) msgb_push(msg, sizeof(*hh) + 1);
hh->len = htons(msg->len - sizeof(*hh) - 1);
hh->proto = proto;
}
void ipa_msg_push_header(struct msgb *msg, uint8_t proto)
{
struct ipaccess_head *hh;
msg->l2h = msg->data;
hh = (struct ipaccess_head *) msgb_push(msg, sizeof(*hh));
hh->proto = proto;
hh->len = htons(msgb_l2len(msg));
}
/* send a data frame to layer 1 */
static int lapdm_send_ph_data_req(struct lapd_msg_ctx *lctx, struct msgb *msg)
{
uint8_t l3_len = msg->tail - msg->data;
struct lapd_datalink *dl = lctx->dl;
struct lapdm_datalink *mdl =
container_of(dl, struct lapdm_datalink, dl);
struct lapdm_msg_ctx *mctx = &mdl->mctx;
int format = lctx->format;
/* prepend l2 header */
msg->l2h = msgb_push(msg, 3);
msg->l2h[0] = LAPDm_ADDR(lctx->lpd, lctx->sapi, lctx->cr);
/* EA is set here too */
switch (format) {
case LAPD_FORM_I:
msg->l2h[1] = LAPDm_CTRL_I(lctx->n_recv, lctx->n_send,
lctx->p_f);
break;
case LAPD_FORM_S:
msg->l2h[1] = LAPDm_CTRL_S(lctx->n_recv, lctx->s_u, lctx->p_f);
break;
case LAPD_FORM_U:
msg->l2h[1] = LAPDm_CTRL_U(lctx->s_u, lctx->p_f);
break;
default:
msgb_free(msg);
return -EINVAL;
}
msg->l2h[2] = LAPDm_LEN(l3_len); /* EL is set here too */
if (lctx->more)
msg->l2h[2] |= LAPDm_MORE;
/* add ACCH header with last indicated tx-power and TA */
if ((mctx->link_id & 0x40)) {
struct lapdm_entity *le = mdl->entity;
msg->l2h = msgb_push(msg, 2);
msg->l2h[0] = le->tx_power;
msg->l2h[1] = le->ta;
}
return tx_ph_data_enqueue(mctx->dl, msg, mctx->chan_nr, mctx->link_id,
23);
}
/* push a control header (3 dwords) to the start of a msgb. This format
* is used for command and response packets */
void octvocnet_push_ctl_hdr(struct msgb *msg, uint32_t dest_fifo_id,
uint32_t src_fifo_id, uint32_t socket_id)
{
tOCTVOCNET_PKT_CTL_HEADER *ch;
ch = (tOCTVOCNET_PKT_CTL_HEADER *) msgb_push(msg, sizeof(*ch));
ch->ulDestFifoId = htonl(dest_fifo_id);
ch->ulSourceFifoId = htonl(src_fifo_id);
ch->ulSocketId = htonl(socket_id);
}
/* iterate over all chunks in one RTCP message, look for CNAME IEs and
* replace all of those with 'new_cname' */
static int rtcp_sdes_cname_mangle(struct msgb *msg, struct rtcp_hdr *rh,
u_int16_t *rtcp_len, const char *new_cname)
{
u_int8_t *rtcp_end;
u_int8_t *cur = (u_int8_t *) rh;
u_int8_t tag, len = 0;
rtcp_end = cur + *rtcp_len;
/* move cur to end of RTP header */
cur += sizeof(*rh);
/* iterate over Chunks */
while (cur+4 < rtcp_end) {
/* skip four bytes SSRC/CSRC */
cur += 4;
/* iterate over IE's inside the chunk */
while (cur+1 < rtcp_end) {
tag = *cur++;
if (tag == 0) {
/* end of chunk, skip additional zero */
while (*cur++ == 0) { }
break;
}
len = *cur++;
if (tag == RTCP_IE_CNAME) {
/* we've found the CNAME, lets mangle it */
if (len < strlen(new_cname)) {
/* we need to make more space */
int increase = strlen(new_cname) - len;
msgb_push(msg, increase);
memmove(cur+len+increase, cur+len,
rtcp_end - (cur+len));
/* FIXME: we have to respect RTCP
* padding/alignment rules! */
len += increase;
*(cur-1) += increase;
rtcp_end += increase;
*rtcp_len += increase;
}
/* copy new CNAME into message */
memcpy(cur, new_cname, strlen(new_cname));
/* FIXME: zero the padding in case new CNAME
* is smaller than old one !!! */
}
cur += len;
}
}
return 0;
}
static int handle_ts1_write(struct bsc_fd *bfd)
{
struct e1inp_line *line = bfd->data;
unsigned int ts_nr = bfd->priv_nr;
struct e1inp_ts *e1i_ts = &line->ts[ts_nr-1];
struct e1inp_sign_link *sign_link;
struct sockaddr_mISDN sa;
struct msgb *msg;
struct mISDNhead *hh;
u_int8_t *l2_data;
int ret;
bfd->when &= ~BSC_FD_WRITE;
/* get the next msg for this timeslot */
msg = e1inp_tx_ts(e1i_ts, &sign_link);
if (!msg) {
/* no message after tx delay timer */
return 0;
}
l2_data = msg->data;
/* prepend the mISDNhead */
hh = (struct mISDNhead *) msgb_push(msg, sizeof(*hh));
hh->prim = DL_DATA_REQ;
DEBUGP(DMI, "TX channel(%d) TEI(%d) SAPI(%d): %s\n",
sign_link->driver.misdn.channel, sign_link->tei,
sign_link->sapi, hexdump(l2_data, msg->len - MISDN_HEADER_LEN));
/* construct the sockaddr */
sa.family = AF_ISDN;
sa.sapi = sign_link->sapi;
sa.dev = sign_link->tei;
sa.channel = sign_link->driver.misdn.channel;
ret = sendto(bfd->fd, msg->data, msg->len, 0,
(struct sockaddr *)&sa, sizeof(sa));
if (ret < 0)
fprintf(stderr, "%s sendto failed %d\n", __func__, ret);
msgb_free(msg);
/* set tx delay timer for next event */
e1i_ts->sign.tx_timer.cb = timeout_ts1_write;
e1i_ts->sign.tx_timer.data = e1i_ts;
bsc_schedule_timer(&e1i_ts->sign.tx_timer, 0, 50000);
return ret;
}
/* Prefix msg with a RP-DATA header and send as CP-DATA */
static int gsm411_rp_sendmsg(struct gsm411_smr_inst *inst, struct msgb *msg,
uint8_t rp_msg_type, uint8_t rp_msg_ref,
int mnsms_msg_type)
{
struct gsm411_rp_hdr *rp;
uint8_t len = msg->len;
/* GSM 04.11 RP-DATA header */
rp = (struct gsm411_rp_hdr *)msgb_push(msg, sizeof(*rp));
rp->len = len + 2;
rp->msg_type = rp_msg_type;
rp->msg_ref = rp_msg_ref;
return inst->mn_send(inst, mnsms_msg_type, msg);
}
static int gsm_recv_voice(struct osmocom_ms *ms, struct msgb *msg)
{
struct gsm_data_frame *mncc;
/* distribute and then free */
if (ms->mncc_entity.mncc_recv && ms->mncc_entity.ref) {
/* push mncc header in front of data */
mncc = (struct gsm_data_frame *)
msgb_push(msg, sizeof(struct gsm_data_frame));
mncc->msg_type = GSM_TCHF_FRAME;
mncc->callref = ms->mncc_entity.ref;
ms->mncc_entity.mncc_recv(ms, mncc->msg_type, mncc);
}
msgb_free(msg);
return 0;
}
/* push a common header (1 dword) to the start of a msgb */
void octpkt_push_common_hdr(struct msgb *msg, uint8_t format,
uint8_t trace, uint32_t ptype)
{
uint32_t ch;
uint32_t *chptr;
uint32_t tot_len = msgb_length(msg) + sizeof(ch);
ch = ((format & cOCTPKT_HDR_FORMAT_PROTO_TYPE_LEN_MASK_FORMAT_BIT_MASK)
<< cOCTPKT_HDR_FORMAT_PROTO_TYPE_LEN_MASK_FORMAT_BIT_OFFSET) |
((trace & cOCTPKT_HDR_FORMAT_PROTO_TYPE_LEN_MASK_TRACE_BIT_MASK)
<< cOCTPKT_HDR_FORMAT_PROTO_TYPE_LEN_MASK_TRACE_BIT_OFFSET) |
((ptype & cOCTPKT_HDR_FORMAT_PROTO_TYPE_LEN_MASK_CONTROL_PROTOCOL_TYPE_BIT_MASK)
<< cOCTPKT_HDR_FORMAT_PROTO_TYPE_LEN_MASK_CONTROL_PROTOCOL_TYPE_BIT_OFFSET) |
(tot_len & cOCTPKT_HDR_FORMAT_PROTO_TYPE_LEN_MASK_LENGTH_BIT_MASK);
chptr = (uint32_t *) msgb_push(msg, sizeof(ch));
*chptr = htonl(ch);
}
/* Take a B4 format message from L1 and create RSLms UNIT DATA IND */
static int send_rslms_rll_l3_ui(struct lapdm_msg_ctx *mctx, struct msgb *msg)
{
uint8_t l3_len = msg->tail - (uint8_t *)msgb_l3(msg);
struct abis_rsl_rll_hdr *rllh;
/* Add the RSL + RLL header */
msgb_tv16_push(msg, RSL_IE_L3_INFO, l3_len);
msgb_push(msg, 2 + 2);
rsl_rll_push_hdr(msg, RSL_MT_UNIT_DATA_IND, mctx->chan_nr,
mctx->link_id, 1);
rllh = (struct abis_rsl_rll_hdr *)msgb_l2(msg);
rllh->data[0] = RSL_IE_TIMING_ADVANCE;
rllh->data[1] = mctx->ta_ind;
rllh->data[2] = RSL_IE_MS_POWER;
rllh->data[3] = mctx->tx_power_ind;
return rslms_sendmsg(msg, mctx->dl->entity);
}
/*! \brief call-back from LAPD code, called when it wants to Tx data */
static void misdn_write_msg(struct msgb *msg, void *cbdata)
{
struct osmo_fd *bfd = cbdata;
// struct e1inp_line *line = bfd->data;
// unsigned int ts_nr = bfd->priv_nr;
// struct e1inp_ts *e1i_ts = &line->ts[ts_nr-1];
struct mISDNhead *hh;
int ret;
DEBUGP(DLMI, "PH_DATA_REQ: len=%d %s\n", msg->len,
osmo_hexdump(msg->data, msg->len));
hh = (struct mISDNhead *) msgb_push(msg, MISDN_HEADER_LEN);
hh->prim = PH_DATA_REQ;
hh->id = 0;
ret = write(bfd->fd, msg->data, msg->len);
if (ret < 0)
LOGP(DLMI, LOGL_NOTICE, "write failed %d\n", ret);
msgb_free(msg);
}
/*! \brief RL-UL-UNITDATA.req (Chapter 10.2.2)
* \param[in] bctx BVC Context
* \param[in] tlli TLLI to identify MS
* \param[in] qos_profile Pointer to three octests of QoS profile
* \param[in] llc_pdu msgb pointer containing UL Unitdata IE payload
*/
int bssgp_tx_ul_ud(struct bssgp_bvc_ctx *bctx, uint32_t tlli,
const uint8_t *qos_profile, struct msgb *llc_pdu)
{
struct msgb *msg = llc_pdu;
uint8_t bssgp_cid[8];
uint8_t bssgp_align[3] = {0, 0, 0};
struct bssgp_ud_hdr *budh;
int align = sizeof(*budh);
/* FIXME: Optional LSA Identifier List, PFI */
/* Cell Identifier */
bssgp_create_cell_id(bssgp_cid, &bctx->ra_id, bctx->cell_id);
align += 2; /* add T+L */
align += sizeof(bssgp_cid);
/* First push alignment IE */
align += 2; /* add T+L */
align = (4 - align) & 3; /* how many octest are required to align? */
msgb_tvlv_push(msg, BSSGP_IE_ALIGNMENT, align, bssgp_align);
/* Push other IEs */
msgb_tvlv_push(msg, BSSGP_IE_CELL_ID, sizeof(bssgp_cid), bssgp_cid);
/* User Data Header */
budh = (struct bssgp_ud_hdr *) msgb_push(msg, sizeof(*budh));
budh->tlli = htonl(tlli);
memcpy(budh->qos_profile, qos_profile, 3);
budh->pdu_type = BSSGP_PDUT_UL_UNITDATA;
/* set NSEI and BVCI in msgb cb */
msgb_nsei(msg) = bctx->nsei;
msgb_bvci(msg) = bctx->bvci;
rate_ctr_inc(&bctx->ctrg->ctr[BSSGP_CTR_PKTS_OUT]);
rate_ctr_add(&bctx->ctrg->ctr[BSSGP_CTR_BYTES_OUT], msg->len);
return gprs_ns_sendmsg(bssgp_nsi, msg);
}
int ga_csr_uplink_dtap_sms(int sock, char *dest, char *text, uint8_t tpdu_mr, uint8_t rpdu_mr){
struct uma_msg_s *uma_msg;
u_int8_t *titi, *tata;
int j, i, len;
struct gsm48_hdr *ngh;
struct gsm411_rp_hdr *rph;
struct msgb *nmsg;
struct gsm_sms *sms = malloc(sizeof(struct gsm_sms));
uint8_t *data;
uint8_t oa[12];
uint8_t oa_len;
uint8_t *rp_ud_len;
if(strlen(dest) >= sizeof(sms->dest_addr)){
fprintf(stderr, "Broken destination number\n");
return -2;
}
memset(sms, 0, sizeof(struct gsm_sms));
sms->validity_period = 0xff;
sms->status_rep_req = 0;
sms->ud_hdr_ind = 0;
sms->protocol_id = 0;
sms->data_coding_scheme = 0;
sms->reject_duplicates = 0;
sms->msg_ref = tpdu_mr;
sms->user_data_len = gsm_7bit_encode(sms->user_data, text);
/* workaround that libosmocore is currently returning the number of
encoded octets and not septets
*/
sms->user_data_len = (sms->user_data_len*8)/7;
strncpy(sms->dest_addr, dest, sizeof(sms->dest_addr) - 1);
nmsg = gsm411_msgb_alloc();
if (!nmsg){
return -1;
}
rph = (struct gsm411_rp_hdr *)msgb_push(nmsg, sizeof(*rph));
rph->msg_type = GSM411_MT_RP_DATA_MO;
rph->msg_ref = rpdu_mr;
data = (u_int8_t *)msgb_put(nmsg, 1);
data[0] = 0;
/* hardcode smsc address for now */
data = (u_int8_t *)msgb_put(nmsg, 8);
data[0] = 0x07;
data[1] = 0x91;
data[2] = 0x33;
data[3] = 0x06;
data[4] = 0x09;
data[5] = 0x10;
data[6] = 0x93;
data[7] = 0xf0;
ngh = (struct gsm48_hdr *) msgb_push(nmsg, sizeof(*ngh));
ngh->proto_discr = GSM48_PDISC_SMS;
ngh->msg_type = GSM411_MT_CP_DATA;
data = msgb_put(nmsg, 1);
rp_ud_len = data;
*rp_ud_len = gsm340_tpdu_encode(nmsg, sms);
/* substract rpdu length, type and ref from total length */
rph->len = nmsg->len - 3;
uma_msg = uma_create_msg(GA_CSR_UPLINK_DIRECT_TRANSFER, 0, GA_CSR);
uma_msg->tlv[uma_msg->ntlv++] = create_IEI_L3_Message(nmsg->data, nmsg->len);
j = uma_create_buffer(&titi, uma_msg);
printf("Sending GA-CSR UPLINK DIRECT TRANSFER CP-DATA RP-DATA...");
write(sock, titi, j);
printf("passed to proxy\n");
// there seems to be a memory corruption in libosmocore/libuma, hence not freeing even though i should :/
//uma_delete_msg(uma_msg);
//msgb_free(nmsg);
return 0;
}
/* handling sim events */
void sim_handler(void)
{
static struct msgb *msg;
struct l1ctl_hdr *l1h;
static uint8_t mode;
static uint8_t *response;
static uint16_t length;
switch (sim_state) {
case SIM_STATE_IDLE:
if (!sim_len)
break; /* wait for SIM command */
/* check if instructions expects a response */
if (/* GET RESPONSE needs SIM_APDU_GET */
(sim_len == 5 && sim_data[0] == SIM_CLASS &&
sim_data[1] == SIM_GET_RESPONSE && sim_data[2] == 0x00 &&
sim_data[3] == 0x00) ||
/* READ BINARY/RECORD needs SIM_APDU_GET */
(sim_len >= 5 && sim_data[0] == SIM_CLASS &&
(sim_data[1] == SIM_READ_BINARY ||
sim_data[1] == SIM_READ_RECORD)))
mode = SIM_APDU_GET;
else
mode = SIM_APDU_PUT;
length = sim_data[4];
/* allocate space for expected response */
msg = msgb_alloc_headroom(256, L3_MSG_HEAD
+ sizeof(struct l1ctl_hdr), "l1ctl1");
response = msgb_put(msg, length + 2 + 1);
sim_state = SIM_STATE_TX_HEADER;
/* send APDU header */
calypso_sim_transmit(sim_data, 5);
break;
case SIM_STATE_TX_HEADER:
if (!txDoneFlag)
break; /* wait until header is transmitted */
/* Disable all interrupt driven functions */
writew(0xFF, REG_SIM_MASKIT);
/* Case 1: No input, No Output */
if (length == 0) {
sim_state = SIM_STATE_RX_STATUS;
calypso_sim_receive(response + 1, 2);
break;
}
/* Case 2: No input / Output of known length */
if (mode == SIM_APDU_PUT) {
sim_state = SIM_STATE_RX_ACK;
calypso_sim_receive(response, 1);
break;
/* Case 4: Input / No output */
} else {
sim_state = SIM_STATE_RX_ACK_DATA;
calypso_sim_receive(response, length + 1 + 2);
}
break;
case SIM_STATE_RX_STATUS:
if (!rxDoneFlag)
break; /* wait until data is received */
/* Disable all interrupt driven functions */
writew(0xFF, REG_SIM_MASKIT);
/* disable special ignore case */
sim_ignore_waiting_char = 0;
/* wrong number of bytes received */
if (sim_rx_character_count != 2) {
puts("SIM: Failed to read status\n");
goto error;
}
msgb_pull(msg, length + 1); /* pull up to status info */
goto queue;
case SIM_STATE_RX_ACK:
if (!rxDoneFlag)
break; /* wait until data is received */
/* Disable all interrupt driven functions */
writew(0xFF, REG_SIM_MASKIT);
/* error received */
if (sim_rx_character_count == 2) {
puts("SIM: command failed\n");
msgb_pull(msg, msg->len - 2);
msg->data[0] = response[0];
msg->data[1] = response[1];
goto queue;
}
/* wrong number of bytes received */
if (sim_rx_character_count != 1) {
puts("SIM: ACK read failed\n");
goto error;
}
if (response[0] != sim_data[1]) {
puts("SIM: ACK does not match request\n");
goto error;
}
sim_state = SIM_STATE_TX_DATA;
calypso_sim_transmit(sim_data + 5, length);
break;
case SIM_STATE_TX_DATA:
if (!txDoneFlag)
break; /* wait until data is transmitted */
/* Disable all interrupt driven functions */
writew(0xFF, REG_SIM_MASKIT);
/* Ignore waiting char for RUN GSM ALGORITHM */
/* TODO: implement proper handling of the "Procedure Bytes"
than this is no longer needed */
if(sim_data[1] == 0x88)
sim_ignore_waiting_char = 1;
sim_state = SIM_STATE_RX_STATUS;
calypso_sim_receive(response + length + 1, 2);
break;
case SIM_STATE_RX_ACK_DATA:
if (!rxDoneFlag)
break; /* wait until data is received */
/* Disable all interrupt driven functions */
writew(0xFF, REG_SIM_MASKIT);
/* error received */
if (sim_rx_character_count == 2) {
puts("SIM: command failed\n");
msgb_pull(msg, msg->len - 2);
msg->data[0] = response[0];
msg->data[1] = response[1];
goto queue;
}
/* wrong number of bytes received */
if (sim_rx_character_count != length + 1 + 2) {
puts("SIM: Failed to read data\n");
goto error;
}
msgb_pull(msg, 1); /* pull ACK byte */
goto queue;
}
return;
error:
msgb_pull(msg, msg->len - 2);
msg->data[0] = 0;
msg->data[1] = 0;
queue:
printf("SIM Response (%d): %s\n", msg->len,
osmo_hexdump(msg->data, msg->len));
l1h = (struct l1ctl_hdr *) msgb_push(msg, sizeof(*l1h));
l1h->msg_type = L1CTL_SIM_CONF;
l1h->flags = 0;
msg->l1h = (uint8_t *)l1h;
l1_queue_for_l2(msg);
/* go IDLE */
sim_state = SIM_STATE_IDLE;
sim_len = 0;
return;
}
