/* 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);
}
/* safely enable a message into the L1S TX queue */
void l1a_txq_msgb_enq(struct llist_head *queue, struct msgb *msg)
{
unsigned long flags;
local_firq_save(flags);
msgb_enqueue(queue, msg);
local_irq_restore(flags);
}
/*! \brief Enqueue a new \ref msgb into a write queue
* \param[in] queue Write queue to be used
* \param[in] data to-be-enqueued message buffer
*/
int osmo_wqueue_enqueue(struct osmo_wqueue *queue, struct msgb *data)
{
// if (queue->current_length + 1 >= queue->max_length)
// LOGP(DMSC, LOGL_ERROR, "The queue is full. Dropping not yet implemented.\n");
++queue->current_length;
msgb_enqueue(&queue->msg_queue, data);
queue->bfd.when |= BSC_FD_WRITE;
return 0;
}
/*! \brief Enqueue a new \ref msgb into a write queue
* \param[in] queue Write queue to be used
* \param[in] data to-be-enqueued message buffer
* \returns 0 on success; negative on error
*/
int osmo_wqueue_enqueue(struct osmo_wqueue *queue, struct msgb *data)
{
if (queue->current_length >= queue->max_length) {
LOGP(DLGLOBAL, LOGL_ERROR,
"wqueue(%p) is full. Rejecting msgb\n", queue);
return -ENOSPC;
}
++queue->current_length;
msgb_enqueue(&queue->msg_queue, data);
queue->bfd.when |= BSC_FD_WRITE;
return 0;
}
/* receive a message from L1/L2 and put it in GSMTAP */
int gsmtap_sendmsg(uint16_t arfcn, uint8_t ts, uint8_t chan_type, uint8_t ss,
uint32_t fn, int8_t signal_dbm, uint8_t snr,
const uint8_t *data, unsigned int len)
{
struct msgb *msg;
/* gsmtap was never initialized, so don't try to send anything */
if (gsmtap_bfd.fd == -1)
return 0;
msg = gsmtap_makemsg(arfcn, ts, chan_type, ss, fn, signal_dbm,
snr, data, len);
if (!msg)
return -ENOMEM;
msgb_enqueue(&gsmtap_txqueue, msg);
gsmtap_bfd.when |= BSC_FD_WRITE;
return 0;
}
int pcu_sock_send(struct msgb *msg)
{
struct pcu_sock_state *state = pcu_sock_state;
struct osmo_fd *conn_bfd;
if (!state) {
LOGP(DL1IF, LOGL_NOTICE, "PCU socket not created, dropping "
"message\n");
return -EINVAL;
}
conn_bfd = &state->conn_bfd;
if (conn_bfd->fd <= 0) {
LOGP(DL1IF, LOGL_NOTICE, "PCU socket not connected, dropping "
"message\n");
return -EIO;
}
msgb_enqueue(&state->upqueue, msg);
conn_bfd->when |= BSC_FD_WRITE;
return 0;
}
static void l1ctl_rx_data_tbf_req(struct l1_model_ms *ms, struct msgb *msg)
{
struct l1ctl_hdr *l1h = (struct l1ctl_hdr *) msg->data;
struct l1ctl_info_ul_tbf *udt = (struct l1ctl_info_ul_tbf *) l1h->data;
enum osmo_gprs_cs osmo_cs;
int block_size;
msg->l2h = udt->payload;
LOGPMS(DL1P, LOGL_ERROR, ms, "Rx L1CTL_DATA_TBF_REQ (tbf_id=%d, data=%s)\n",
udt->tbf_nr, osmo_hexdump(msg->l2h, msgb_l2len(msg)));
if (udt->tbf_nr != 0) {
LOGPMS(DL1C, LOGL_ERROR, ms, "TBF_NR != 0 not supported yet!\n");
return;
}
if (ms->state.state != MS_STATE_TBF) {
LOGPMS(DL1P, LOGL_ERROR, ms, "DATA_TBF_REQ in state != TBF\n");
return;
}
osmo_cs = get_l1ctl_cs_by_osmo(udt->coding_scheme);
if (osmo_cs < 0) {
LOGPMS(DL1P, LOGL_ERROR, ms, "DATA_RBF_REQ with invalid CS\n");
return;
}
block_size = osmo_gprs_ul_block_size_bytes(osmo_cs);
if (msgb_l2len(msg) < block_size) {
int pad_len = block_size - msgb_l2len(msg);
uint8_t *pad = msgb_put(msg, pad_len);
memset(pad, GSM_MACBLOCK_PADDING, pad_len);
}
msgb_enqueue(&ms->state.tbf.ul.tx_queue, msg);
}
/* read from incoming RTP/RTCP socket */
static int rtp_socket_read(struct rtp_socket *rs, struct rtp_sub_socket *rss)
{
int rc;
struct msgb *msg = msgb_alloc(RTP_ALLOC_SIZE, "RTP/RTCP");
struct msgb *new_msg;
struct rtp_sub_socket *other_rss;
if (!msg)
return -ENOMEM;
rc = read(rss->bfd.fd, msg->data, RTP_ALLOC_SIZE);
if (rc <= 0) {
rss->bfd.when &= ~BSC_FD_READ;
return rc;
}
msgb_put(msg, rc);
switch (rs->rx_action) {
case RTP_PROXY:
if (!rs->proxy.other_sock) {
rc = -EIO;
goto out_free;
}
if (rss->bfd.priv_nr == RTP_PRIV_RTP)
other_rss = &rs->proxy.other_sock->rtp;
else if (rss->bfd.priv_nr == RTP_PRIV_RTCP) {
other_rss = &rs->proxy.other_sock->rtcp;
/* modify RTCP SDES CNAME */
rc = rtcp_mangle(msg, rs);
if (rc < 0)
goto out_free;
} else {
rc = -EINVAL;
goto out_free;
}
msgb_enqueue(&other_rss->tx_queue, msg);
other_rss->bfd.when |= BSC_FD_WRITE;
break;
case RTP_RECV_UPSTREAM:
if (!rs->receive.callref || !rs->receive.net) {
rc = -EIO;
goto out_free;
}
if (rss->bfd.priv_nr == RTP_PRIV_RTCP) {
if (!mangle_rtcp_cname) {
msgb_free(msg);
break;
}
/* modify RTCP SDES CNAME */
rc = rtcp_mangle(msg, rs);
if (rc < 0)
goto out_free;
msgb_enqueue(&rss->tx_queue, msg);
rss->bfd.when |= BSC_FD_WRITE;
break;
}
if (rss->bfd.priv_nr != RTP_PRIV_RTP) {
rc = -EINVAL;
goto out_free;
}
rc = rtp_decode(msg, rs->receive.callref, &new_msg);
if (rc < 0)
goto out_free;
msgb_free(msg);
msgb_enqueue(&rs->receive.net->upqueue, new_msg);
break;
case RTP_NONE: /* if socket exists, but disabled by app */
msgb_free(msg);
break;
}
return 0;
out_free:
msgb_free(msg);
return rc;
}
/* encode and send a rtp frame */
int rtp_send_frame(struct rtp_socket *rs, struct gsm_data_frame *frame)
{
struct rtp_sub_socket *rss = &rs->rtp;
struct msgb *msg;
struct rtp_hdr *rtph;
int payload_type;
int payload_len;
int duration; /* in samples */
if (rs->tx_action != RTP_SEND_DOWNSTREAM) {
/* initialize sequences */
rs->tx_action = RTP_SEND_DOWNSTREAM;
rs->transmit.ssrc = rand();
rs->transmit.sequence = random();
rs->transmit.timestamp = random();
}
switch (frame->msg_type) {
case GSM_TCHF_FRAME:
payload_type = RTP_PT_GSM_FULL;
payload_len = 33;
duration = 160;
break;
case GSM_TCHF_FRAME_EFR:
payload_type = RTP_PT_GSM_EFR;
payload_len = 31;
duration = 160;
break;
default:
DEBUGPC(DMUX, "unsupported message type %d\n",
frame->msg_type);
return -EINVAL;
}
{
struct timeval tv, tv_diff;
long int usec_diff, frame_diff;
gettimeofday(&tv, NULL);
tv_difference(&tv_diff, &rs->transmit.last_tv, &tv);
rs->transmit.last_tv = tv;
usec_diff = tv_diff.tv_sec * 1000000 + tv_diff.tv_usec;
frame_diff = (usec_diff / 20000);
if (abs(frame_diff) > 1) {
long int frame_diff_excess = frame_diff - 1;
LOGP(DMUX, LOGL_NOTICE,
"Correcting frame difference of %ld frames\n", frame_diff_excess);
rs->transmit.sequence += frame_diff_excess;
rs->transmit.timestamp += frame_diff_excess * duration;
}
}
msg = msgb_alloc(sizeof(struct rtp_hdr) + payload_len, "RTP-GSM-FULL");
if (!msg)
return -ENOMEM;
rtph = (struct rtp_hdr *)msg->data;
rtph->version = RTP_VERSION;
rtph->padding = 0;
rtph->extension = 0;
rtph->csrc_count = 0;
rtph->marker = 0;
rtph->payload_type = payload_type;
rtph->sequence = htons(rs->transmit.sequence++);
rtph->timestamp = htonl(rs->transmit.timestamp);
rs->transmit.timestamp += duration;
rtph->ssrc = htonl(rs->transmit.ssrc);
memcpy(msg->data + sizeof(struct rtp_hdr), frame->data, payload_len);
msgb_put(msg, sizeof(struct rtp_hdr) + payload_len);
msgb_enqueue(&rss->tx_queue, msg);
rss->bfd.when |= BSC_FD_WRITE;
return 0;
}
void ipa_server_conn_send(struct ipa_server_conn *conn, struct msgb *msg)
{
msgb_enqueue(&conn->tx_queue, msg);
conn->ofd.when |= BSC_FD_WRITE;
}
void ipa_client_conn_send(struct ipa_client_conn *link, struct msgb *msg)
{
msgb_enqueue(&link->tx_queue, msg);
link->ofd->when |= BSC_FD_WRITE;
}
