mirror of https://gerrit.osmocom.org/libosmocore
390 lines
12 KiB
C
390 lines
12 KiB
C
/*! \file i460_mux.c
|
|
* ITU-T I.460 sub-channel multiplexer + demultiplexer */
|
|
/*
|
|
* (C) 2020 by Harald Welte <laforge@gnumonks.org>
|
|
*
|
|
* 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.
|
|
*/
|
|
|
|
#include <errno.h>
|
|
|
|
#include <osmocom/core/bits.h>
|
|
#include <osmocom/core/utils.h>
|
|
#include <osmocom/core/msgb.h>
|
|
#include <osmocom/isdn/i460_mux.h>
|
|
|
|
/*! count the number of sub-channels in this I.460 slot.
|
|
* \param[in] ts timeslot that holds the I.460 subchannels.
|
|
* \return number of subchannels. */
|
|
int osmo_i460_subchan_count(struct osmo_i460_timeslot *ts)
|
|
{
|
|
int i, num_used = 0;
|
|
|
|
for (i = 0; i < ARRAY_SIZE(ts->schan); i++) {
|
|
if (ts->schan[i].rate != OSMO_I460_RATE_NONE)
|
|
num_used++;
|
|
}
|
|
|
|
return num_used;
|
|
}
|
|
|
|
/* does this channel have no sub-streams (single 64k subchannel)? */
|
|
static bool osmo_i460_has_single_64k_schan(struct osmo_i460_timeslot *ts)
|
|
{
|
|
if (osmo_i460_subchan_count(ts) != 1)
|
|
return false;
|
|
|
|
if (ts->schan[0].rate != OSMO_I460_RATE_64k)
|
|
return false;
|
|
|
|
return true;
|
|
}
|
|
|
|
/***********************************************************************
|
|
* Demultiplexer
|
|
***********************************************************************/
|
|
|
|
/* append a single bit to a sub-channel */
|
|
static void demux_subchan_append_bit(struct osmo_i460_subchan *schan, uint8_t bit)
|
|
{
|
|
struct osmo_i460_subchan_demux *demux = &schan->demux;
|
|
|
|
OSMO_ASSERT(demux->out_bitbuf);
|
|
OSMO_ASSERT(demux->out_idx < demux->out_bitbuf_size);
|
|
|
|
demux->out_bitbuf[demux->out_idx++] = bit ? 1 : 0;
|
|
|
|
if (demux->out_idx >= demux->out_bitbuf_size) {
|
|
if (demux->out_cb_bits)
|
|
demux->out_cb_bits(schan, demux->user_data, demux->out_bitbuf, demux->out_idx);
|
|
else {
|
|
/* pack bits into bytes */
|
|
OSMO_ASSERT((demux->out_idx % 8) == 0);
|
|
unsigned int num_bytes = demux->out_idx / 8;
|
|
uint8_t bytes[num_bytes];
|
|
osmo_ubit2pbit(bytes, demux->out_bitbuf, demux->out_idx);
|
|
demux->out_cb_bytes(schan, demux->user_data, bytes, num_bytes);
|
|
}
|
|
demux->out_idx = 0;
|
|
}
|
|
}
|
|
|
|
/* extract those bits relevant to this schan of each byte in 'data' */
|
|
static void demux_subchan_extract_bits(struct osmo_i460_subchan *schan, const uint8_t *data, size_t data_len)
|
|
{
|
|
int i;
|
|
|
|
for (i = 0; i < data_len; i++) {
|
|
uint8_t inbyte = data[i];
|
|
/* I.460 defines sub-channel 0 is using bit positions 1+2 (the two
|
|
* most significant bits, hence we extract msb-first */
|
|
uint8_t inbits = inbyte << schan->bit_offset;
|
|
|
|
/* extract the bits relevant to the given schan */
|
|
switch (schan->rate) {
|
|
case OSMO_I460_RATE_8k:
|
|
demux_subchan_append_bit(schan, inbits & 0x80);
|
|
break;
|
|
case OSMO_I460_RATE_16k:
|
|
demux_subchan_append_bit(schan, inbits & 0x80);
|
|
demux_subchan_append_bit(schan, inbits & 0x40);
|
|
break;
|
|
case OSMO_I460_RATE_32k:
|
|
demux_subchan_append_bit(schan, inbits & 0x80);
|
|
demux_subchan_append_bit(schan, inbits & 0x40);
|
|
demux_subchan_append_bit(schan, inbits & 0x20);
|
|
demux_subchan_append_bit(schan, inbits & 0x10);
|
|
break;
|
|
case OSMO_I460_RATE_64k:
|
|
demux_subchan_append_bit(schan, inbits & 0x80);
|
|
demux_subchan_append_bit(schan, inbits & 0x40);
|
|
demux_subchan_append_bit(schan, inbits & 0x20);
|
|
demux_subchan_append_bit(schan, inbits & 0x10);
|
|
demux_subchan_append_bit(schan, inbits & 0x08);
|
|
demux_subchan_append_bit(schan, inbits & 0x04);
|
|
demux_subchan_append_bit(schan, inbits & 0x02);
|
|
demux_subchan_append_bit(schan, inbits & 0x01);
|
|
break;
|
|
default:
|
|
OSMO_ASSERT(0);
|
|
}
|
|
}
|
|
}
|
|
|
|
/*! Feed multiplexed data (from an E1 timeslot) into de-multiplexer.
|
|
* \param[in] ts timeslot state.
|
|
* \param[in] data input data bytes as received from E1/T1.
|
|
* \param[in] data_len length of data in bytes. */
|
|
void osmo_i460_demux_in(struct osmo_i460_timeslot *ts, const uint8_t *data, size_t data_len)
|
|
{
|
|
struct osmo_i460_subchan *schan;
|
|
struct osmo_i460_subchan_demux *demux;
|
|
int i;
|
|
|
|
/* fast path if entire 64k slot is used */
|
|
if (osmo_i460_has_single_64k_schan(ts)) {
|
|
schan = &ts->schan[0];
|
|
demux = &schan->demux;
|
|
if (demux->out_cb_bytes)
|
|
demux->out_cb_bytes(schan, demux->user_data, data, data_len);
|
|
else {
|
|
ubit_t bits[data_len*8];
|
|
osmo_pbit2ubit(bits, data, data_len*8);
|
|
demux->out_cb_bits(schan, demux->user_data, bits, data_len*8);
|
|
}
|
|
return;
|
|
}
|
|
|
|
/* Slow path iterating over all lchans */
|
|
for (i = 0; i < ARRAY_SIZE(ts->schan); i++) {
|
|
schan = &ts->schan[i];
|
|
if (schan->rate == OSMO_I460_RATE_NONE)
|
|
continue;
|
|
demux_subchan_extract_bits(schan, data, data_len);
|
|
}
|
|
}
|
|
|
|
|
|
/***********************************************************************
|
|
* Multiplexer
|
|
***********************************************************************/
|
|
|
|
/*! enqueue a to-be-transmitted message buffer containing unpacked bits */
|
|
void osmo_i460_mux_enqueue(struct osmo_i460_subchan *schan, struct msgb *msg)
|
|
{
|
|
OSMO_ASSERT(msgb_length(msg) > 0);
|
|
msgb_enqueue(&schan->mux.tx_queue, msg);
|
|
}
|
|
|
|
/* mux: pull the next bit out of the given sub-channel */
|
|
static ubit_t mux_schan_provide_bit(struct osmo_i460_subchan *schan)
|
|
{
|
|
struct osmo_i460_subchan_mux *mux = &schan->mux;
|
|
struct msgb *msg;
|
|
ubit_t bit;
|
|
|
|
/* if we don't have anything to transmit, return '1' bits */
|
|
if (llist_empty(&mux->tx_queue)) {
|
|
/* User code now has a last chance to put something into the queue. */
|
|
if (mux->in_cb_queue_empty)
|
|
mux->in_cb_queue_empty(schan, mux->user_data);
|
|
|
|
/* If the queue is still empty, return idle bits */
|
|
if (llist_empty(&mux->tx_queue))
|
|
return 0x01;
|
|
}
|
|
msg = llist_entry(mux->tx_queue.next, struct msgb, list);
|
|
bit = msgb_pull_u8(msg);
|
|
|
|
/* free msgb if we have pulled the last bit */
|
|
if (msgb_length(msg) <= 0) {
|
|
llist_del(&msg->list);
|
|
talloc_free(msg);
|
|
}
|
|
|
|
return bit;
|
|
}
|
|
|
|
/*! provide one byte with the subchan-specific bits of given sub-channel.
|
|
* \param[in] schan sub-channel that is to provide bits
|
|
* \param[out] mask bitmask of those bits filled in
|
|
* \returns bits of given sub-channel */
|
|
static uint8_t mux_subchan_provide_bits(struct osmo_i460_subchan *schan, uint8_t *mask)
|
|
{
|
|
uint8_t outbits = 0;
|
|
uint8_t outmask;
|
|
|
|
/* I.460 defines sub-channel 0 is using bit positions 1+2 (the two
|
|
* most significant bits, hence we provide msb-first */
|
|
|
|
switch (schan->rate) {
|
|
case OSMO_I460_RATE_8k:
|
|
outbits = mux_schan_provide_bit(schan) << 7;
|
|
outmask = 0x80;
|
|
break;
|
|
case OSMO_I460_RATE_16k:
|
|
outbits |= mux_schan_provide_bit(schan) << 7;
|
|
outbits |= mux_schan_provide_bit(schan) << 6;
|
|
outmask = 0xC0;
|
|
break;
|
|
case OSMO_I460_RATE_32k:
|
|
outbits |= mux_schan_provide_bit(schan) << 7;
|
|
outbits |= mux_schan_provide_bit(schan) << 6;
|
|
outbits |= mux_schan_provide_bit(schan) << 5;
|
|
outbits |= mux_schan_provide_bit(schan) << 4;
|
|
outmask = 0xF0;
|
|
break;
|
|
case OSMO_I460_RATE_64k:
|
|
outbits |= mux_schan_provide_bit(schan) << 7;
|
|
outbits |= mux_schan_provide_bit(schan) << 6;
|
|
outbits |= mux_schan_provide_bit(schan) << 5;
|
|
outbits |= mux_schan_provide_bit(schan) << 4;
|
|
outbits |= mux_schan_provide_bit(schan) << 3;
|
|
outbits |= mux_schan_provide_bit(schan) << 2;
|
|
outbits |= mux_schan_provide_bit(schan) << 1;
|
|
outbits |= mux_schan_provide_bit(schan) << 0;
|
|
outmask = 0xFF;
|
|
break;
|
|
default:
|
|
OSMO_ASSERT(0);
|
|
}
|
|
*mask = outmask >> schan->bit_offset;
|
|
return outbits >> schan->bit_offset;
|
|
}
|
|
|
|
/* provide one byte of multiplexed I.460 bits */
|
|
static uint8_t mux_timeslot_provide_bits(struct osmo_i460_timeslot *ts)
|
|
{
|
|
int i, count = 0;
|
|
uint8_t ret = 0xff; /* unused bits must be '1' as per I.460 */
|
|
|
|
for (i = 0; i < ARRAY_SIZE(ts->schan); i++) {
|
|
struct osmo_i460_subchan *schan = &ts->schan[i];
|
|
uint8_t bits, mask;
|
|
|
|
if (schan->rate == OSMO_I460_RATE_NONE)
|
|
continue;
|
|
count++;
|
|
bits = mux_subchan_provide_bits(schan, &mask);
|
|
ret &= ~mask;
|
|
ret |= bits;
|
|
}
|
|
|
|
return ret;
|
|
}
|
|
|
|
|
|
/*! Get multiplexed data from de-multiplexer (for feeding it into an E1 timeslot).
|
|
* \param[in] ts timeslot state.
|
|
* \param[out] out caller-provided buffer where to store generated output bytes.
|
|
* \param[in] out_len number of bytes to be stored at out. */
|
|
int osmo_i460_mux_out(struct osmo_i460_timeslot *ts, uint8_t *out, size_t out_len)
|
|
{
|
|
int i;
|
|
|
|
/* fast path if entire 64k slot is used */
|
|
//if (osmo_i460_has_single_64k_schan(ts)) { }
|
|
|
|
for (i = 0; i < out_len; i++)
|
|
out[i] = mux_timeslot_provide_bits(ts);
|
|
|
|
return out_len;
|
|
}
|
|
|
|
|
|
/***********************************************************************
|
|
* Initialization / Control
|
|
***********************************************************************/
|
|
|
|
|
|
static int alloc_bitbuf(void *ctx, struct osmo_i460_subchan *schan, size_t num_bits)
|
|
{
|
|
struct osmo_i460_subchan_demux *demux = &schan->demux;
|
|
|
|
talloc_free(demux->out_bitbuf);
|
|
demux->out_bitbuf = talloc_zero_size(ctx, num_bits);
|
|
if (!demux->out_bitbuf)
|
|
return -ENOMEM;
|
|
demux->out_bitbuf_size = num_bits;
|
|
|
|
return 0;
|
|
}
|
|
|
|
|
|
static int find_unused_subchan_idx(const struct osmo_i460_timeslot *ts)
|
|
{
|
|
int i;
|
|
|
|
for (i = 0; i < ARRAY_SIZE(ts->schan); i++) {
|
|
const struct osmo_i460_subchan *schan = &ts->schan[i];
|
|
if (schan->rate == OSMO_I460_RATE_NONE)
|
|
return i;
|
|
}
|
|
return -1;
|
|
}
|
|
|
|
/* reset subchannel struct into a defined state */
|
|
static void subchan_reset(struct osmo_i460_subchan *schan, bool first_time)
|
|
{
|
|
/* Before we zero out the subchannel struct, we must be sure that the
|
|
* tx_queue is cleared and all dynamically allocated memory is freed.
|
|
* However, on an uninitalized subchannel struct we can not be sure
|
|
* that the pointers are valid. If the subchannel is reset the first
|
|
* time the caller must set first_time to true. */
|
|
if (!first_time) {
|
|
if (schan->demux.out_bitbuf)
|
|
talloc_free(schan->demux.out_bitbuf);
|
|
msgb_queue_free(&schan->mux.tx_queue);
|
|
}
|
|
|
|
/* Reset subchannel to a defined state */
|
|
memset(schan, 0, sizeof(*schan));
|
|
schan->rate = OSMO_I460_RATE_NONE;
|
|
INIT_LLIST_HEAD(&schan->mux.tx_queue);
|
|
}
|
|
|
|
/*! initialize an I.460 timeslot */
|
|
void osmo_i460_ts_init(struct osmo_i460_timeslot *ts)
|
|
{
|
|
int i;
|
|
|
|
for (i = 0; i < ARRAY_SIZE(ts->schan); i++) {
|
|
struct osmo_i460_subchan *schan = &ts->schan[i];
|
|
schan->ts = ts;
|
|
subchan_reset(schan, true);
|
|
}
|
|
}
|
|
|
|
/*! add a new sub-channel to the given timeslot
|
|
* \param[in] ctx talloc context from where to allocate the internal buffer
|
|
* \param[in] ts timeslot to which to add a sub-channel
|
|
* \param[in] chd description of the sub-channel to be added
|
|
* \return pointer to sub-channel on success, NULL on error */
|
|
struct osmo_i460_subchan *
|
|
osmo_i460_subchan_add(void *ctx, struct osmo_i460_timeslot *ts, const struct osmo_i460_schan_desc *chd)
|
|
{
|
|
struct osmo_i460_subchan *schan;
|
|
int idx, rc;
|
|
|
|
idx = find_unused_subchan_idx(ts);
|
|
if (idx < 0)
|
|
return NULL;
|
|
|
|
schan = &ts->schan[idx];
|
|
|
|
schan->rate = chd->rate;
|
|
schan->bit_offset = chd->bit_offset;
|
|
|
|
schan->demux.out_cb_bits = chd->demux.out_cb_bits;
|
|
schan->demux.out_cb_bytes = chd->demux.out_cb_bytes;
|
|
schan->demux.user_data = chd->demux.user_data;
|
|
schan->mux.in_cb_queue_empty = chd->mux.in_cb_queue_empty;
|
|
schan->mux.user_data = chd->mux.user_data;
|
|
rc = alloc_bitbuf(ctx, schan, chd->demux.num_bits);
|
|
if (rc < 0) {
|
|
subchan_reset(schan, false);
|
|
return NULL;
|
|
}
|
|
|
|
/* return number of schan in use */
|
|
return schan;
|
|
}
|
|
|
|
/* remove a su-channel from the multiplex */
|
|
void osmo_i460_subchan_del(struct osmo_i460_subchan *schan)
|
|
{
|
|
subchan_reset(schan, false);
|
|
}
|
|
|
|
/*! @} */
|