static void test_wqueue_limit(void)
{
struct msgb *msg;
struct osmo_wqueue wqueue;
int rc;
osmo_wqueue_init(&wqueue, 0);
OSMO_ASSERT(wqueue.max_length == 0);
OSMO_ASSERT(wqueue.current_length == 0);
OSMO_ASSERT(wqueue.read_cb == NULL);
OSMO_ASSERT(wqueue.write_cb == NULL);
OSMO_ASSERT(wqueue.except_cb == NULL);
/* try to add and fail */
msg = msgb_alloc(4096, "msg1");
rc = osmo_wqueue_enqueue(&wqueue, msg);
OSMO_ASSERT(rc < 0);
/* add one and fail on the second */
wqueue.max_length = 1;
rc = osmo_wqueue_enqueue(&wqueue, msg);
OSMO_ASSERT(rc == 0);
OSMO_ASSERT(wqueue.current_length == 1);
msg = msgb_alloc(4096, "msg2");
rc = osmo_wqueue_enqueue(&wqueue, msg);
OSMO_ASSERT(rc < 0);
/* add one more */
wqueue.max_length = 2;
rc = osmo_wqueue_enqueue(&wqueue, msg);
OSMO_ASSERT(rc == 0);
OSMO_ASSERT(wqueue.current_length == 2);
/* release everything */
osmo_wqueue_clear(&wqueue);
OSMO_ASSERT(wqueue.current_length == 0);
OSMO_ASSERT(wqueue.max_length == 2);
/* Add two, fail on the third, free it and the queue */
msg = msgb_alloc(4096, "msg3");
rc = osmo_wqueue_enqueue(&wqueue, msg);
OSMO_ASSERT(rc == 0);
OSMO_ASSERT(wqueue.current_length == 1);
msg = msgb_alloc(4096, "msg4");
rc = osmo_wqueue_enqueue(&wqueue, msg);
OSMO_ASSERT(rc == 0);
OSMO_ASSERT(wqueue.current_length == 2);
msg = msgb_alloc(4096, "msg5");
rc = osmo_wqueue_enqueue(&wqueue, msg);
OSMO_ASSERT(rc < 0);
OSMO_ASSERT(wqueue.current_length == 2);
msgb_free(msg);
osmo_wqueue_clear(&wqueue);
}
/*! \brief Copy an msgb.
*
* This function allocates a new msgb, copies the data buffer of msg,
* and adjusts the pointers (incl l1h-l4h) accordingly. The cb part
* is not copied.
* \param[in] msg The old msgb object
* \param[in] name Human-readable name to be associated with msgb
*/
struct msgb *msgb_copy(const struct msgb *msg, const char *name)
{
struct msgb *new_msg;
new_msg = msgb_alloc(msg->data_len, name);
if (!new_msg)
return NULL;
/* copy data */
memcpy(new_msg->_data, msg->_data, new_msg->data_len);
/* copy header */
new_msg->len = msg->len;
new_msg->data += msg->data - msg->_data;
new_msg->head += msg->head - msg->_data;
new_msg->tail += msg->tail - msg->_data;
if (msg->l1h)
new_msg->l1h = new_msg->_data + (msg->l1h - msg->_data);
if (msg->l2h)
new_msg->l2h = new_msg->_data + (msg->l2h - msg->_data);
if (msg->l3h)
new_msg->l3h = new_msg->_data + (msg->l3h - msg->_data);
if (msg->l4h)
new_msg->l4h = new_msg->_data + (msg->l4h - msg->_data);
return new_msg;
}
/* receive a message from L1/L2 and put it in GSMTAP */
struct msgb *gsmtap_makemsg(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;
struct gsmtap_hdr *gh;
uint8_t *dst;
msg = msgb_alloc(sizeof(*gh) + len, "gsmtap_tx");
if (!msg)
return NULL;
gh = (struct gsmtap_hdr *) msgb_put(msg, sizeof(*gh));
gh->version = GSMTAP_VERSION;
gh->hdr_len = sizeof(*gh)/4;
gh->type = GSMTAP_TYPE_UM;
gh->timeslot = ts;
gh->sub_slot = ss;
gh->arfcn = htons(arfcn);
gh->snr_db = snr;
gh->signal_dbm = signal_dbm;
gh->frame_number = htonl(fn);
gh->sub_type = chan_type;
gh->antenna_nr = 0;
dst = msgb_put(msg, len);
memcpy(dst, data, len);
return msg;
}
static int read_cb(struct osmo_rs232 *r)
{
struct msgb *msg;
LOGP(DRS232TEST, LOGL_DEBUG, "received data from rs232\n");
msg = msgb_alloc(1024, "rs232/test");
if (msg == NULL) {
LOGP(DRS232TEST, LOGL_ERROR, "cannot allocate message\n");
return 0;
}
if (osmo_rs232_read(r, msg) < 0) {
LOGP(DRS232TEST, LOGL_ERROR, "cannot receive message\n");
return 0;
}
LOGP(DRS232TEST, LOGL_DEBUG, "received %d bytes\n", msg->len);
printf("received %d bytes ", msg->len);
int i;
printf("(");
for (i=0; i<msg->len; i++)
printf("\\x%.2x", 0xff & msg->data[i]);
printf(") %s\n", msg->data);
msgb_free(msg);
return 0;
}
/* allocate a msgb containing a Litecell15_Prim_t */
struct msgb *sysp_msgb_alloc(void)
{
struct msgb *msg = msgb_alloc(sizeof(Litecell15_Prim_t), "sys_prim");
if (msg)
msg->l1h = msgb_put(msg, sizeof(Litecell15_Prim_t));
return msg;
}
/* allocate a msgb containing a GsmL1_Prim_t */
struct msgb *l1p_msgb_alloc(void)
{
struct msgb *msg = msgb_alloc(sizeof(GsmL1_Prim_t), "l1_prim");
if (msg)
msg->l1h = msgb_put(msg, sizeof(GsmL1_Prim_t));
return msg;
}
static void
loader_send_simple(uint8_t command) {
struct msgb *msg = msgb_alloc(MSGB_MAX, "loader");
msgb_put_u8(msg, command);
loader_send_request(msg);
msgb_free(msg);
osmoload.command = command;
}
static void
loader_start_jump(uint32_t address) {
struct msgb *msg = msgb_alloc(MSGB_MAX, "loader");
msgb_put_u8(msg, LOADER_JUMP);
msgb_put_u32(msg, address);
loader_send_request(msg);
msgb_free(msg);
osmoload.state = STATE_QUERY_PENDING;
osmoload.command = LOADER_JUMP;
}
static void
loader_send_flash_query(uint8_t command, uint8_t chip, uint32_t address) {
struct msgb *msg = msgb_alloc(MSGB_MAX, "loader");
msgb_put_u8(msg, command);
msgb_put_u8(msg, chip);
msgb_put_u32(msg, address);
loader_send_request(msg);
msgb_free(msg);
osmoload.command = command;
}
static void
loader_start_memget(uint8_t length, uint32_t address) {
struct msgb *msg = msgb_alloc(MSGB_MAX, "loader");
msgb_put_u8(msg, LOADER_MEM_READ);
msgb_put_u8(msg, length);
msgb_put_u32(msg, address);
loader_send_request(msg);
msgb_free(msg);
osmoload.state = STATE_QUERY_PENDING;
osmoload.command = LOADER_MEM_READ;
}
int main(void)
{
struct tcp_client *c;
struct tcp_conf conf = {
.ipproto = AF_INET,
.port = 1234,
.client = {
.inet_addr = { inet_addr("127.0.0.1") },
},
};
struct nft_sync_hdr *hdr;
struct msg_buff *msgb;
char buf[1024];
fd_set fds;
msgb = msgb_alloc(NFTS_MAX_REQUEST);
if (msgb == NULL) {
perror("msgb_alloc");
exit(EXIT_FAILURE);
}
hdr = msgb_put(msgb, sizeof(struct nft_sync_hdr) + strlen("fetch"));
hdr->len = htonl(sizeof(struct nft_sync_hdr) + strlen("fetch"));
memcpy(hdr->data, "fetch", strlen("fetch"));
c = tcp_client_create(&conf);
if (c == NULL) {
fprintf(stderr, "cannot initialize TCP client\n");
exit(EXIT_FAILURE);
}
FD_ZERO(&fds);
FD_SET(tcp_client_get_fd(c), &fds);
/* Wait for connection ... */
select(tcp_client_get_fd(c) + 1, NULL, &fds, NULL, NULL);
if (tcp_client_send(c, msgb_data(msgb), msgb_len(msgb)) < 0) {
perror("cannot send to socket");
exit(EXIT_FAILURE);
}
FD_ZERO(&fds);
FD_SET(tcp_client_get_fd(c), &fds);
/* Wait to receive data after sending request ... */
select(tcp_client_get_fd(c) + 1, &fds, NULL, NULL, NULL);
if (tcp_client_recv(c, buf, sizeof(buf)) < 0) {
perror("cannot send to socket");
exit(EXIT_FAILURE);
}
printf("[TEST OK] Received: %s\n", buf + sizeof(struct nft_sync_hdr));
tcp_client_destroy(c);
}
/* forward a RUA message to the SCCP User API to SCCP/SUA */
static int rua_to_scu(struct hnb_context *hnb, struct hnbgw_cnlink *cn,
enum osmo_scu_prim_type type,
uint32_t context_id, uint32_t cause,
const uint8_t *data, unsigned int len)
{
struct msgb *msg = msgb_alloc(1500, "rua_to_sua");
struct osmo_scu_prim *prim;
struct hnbgw_context_map *map;
int rc;
if (!cn) {
DEBUGP(DRUA, "CN=NULL, discarding message\n");
return 0;
}
prim = (struct osmo_scu_prim *) msgb_put(msg, sizeof(*prim));
osmo_prim_init(&prim->oph, SCCP_SAP_USER, type, PRIM_OP_REQUEST, msg);
map = context_map_alloc_by_hnb(hnb, context_id, cn);
/* add primitive header */
switch (type) {
case OSMO_SCU_PRIM_N_CONNECT:
prim->u.connect.called_addr;
prim->u.connect.calling_addr;
prim->u.connect.sccp_class = 2;
prim->u.connect.conn_id = map->scu_conn_id;
break;
case OSMO_SCU_PRIM_N_DATA:
prim->u.data.conn_id = map->scu_conn_id;
break;
case OSMO_SCU_PRIM_N_DISCONNECT:
prim->u.disconnect.conn_id = map->scu_conn_id;
prim->u.disconnect.cause = cause;
break;
case OSMO_SCU_PRIM_N_UNITDATA:
prim->u.unitdata.called_addr;
prim->u.unitdata.calling_addr;
break;
default:
return -EINVAL;
}
/* add optional data section, if needed */
if (data && len) {
msg->l2h = msgb_put(msg, len);
memcpy(msg->l2h, data, len);
}
rc = osmo_sua_user_link_down(cn->sua_link, &prim->oph);
return rc;
}
static int test_bearer_cap()
{
struct gsm_mncc_bearer_cap bc;
int i, rc;
for (i = 0; i < ARRAY_SIZE(bcap_tests); i++) {
struct msgb *msg = msgb_alloc(100, "test");
int lv_len;
memset(&bc, 0, sizeof(bc));
/* test decoding */
rc = gsm48_decode_bearer_cap(&bc, bcap_tests[i].lv);
if (rc < 0) {
fprintf(stderr, "Error decoding %s\n",
bcap_tests[i].name);
return rc;
}
if (memcmp(&bc, bcap_tests[i].bc, sizeof(bc))) {
fprintf(stderr, "Incorrect decoded result of %s:\n",
bcap_tests[i].name);
fprintf(stderr, " should: %s\n",
osmo_hexdump((uint8_t *) bcap_tests[i].bc, sizeof(bc)));
fprintf(stderr, " is: %s\n",
osmo_hexdump((uint8_t *) &bc, sizeof(bc)));
return -1;
}
/* also test re-encode? */
rc = gsm48_encode_bearer_cap(msg, 1, &bc);
if (rc < 0) {
fprintf(stderr, "Error encoding %s\n",
bcap_tests[i].name);
return rc;
}
lv_len = bcap_tests[i].lv[0]+1;
if (memcmp(msg->data, bcap_tests[i].lv, lv_len)) {
fprintf(stderr, "Incorrect encoded result of %s:\n",
bcap_tests[i].name);
fprintf(stderr, " should: %s\n",
osmo_hexdump(bcap_tests[i].lv, lv_len));
fprintf(stderr, " is: %s\n",
osmo_hexdump(msg->data, msg->len));
return -1;
}
printf("Test `%s' passed\n", bcap_tests[i].name);
msgb_free(msg);
}
return 0;
}
static void
loader_do_memdump(uint16_t crc, void *data, size_t length) {
int rc;
if(data && length) {
osmoload.memcrc = osmo_crc16(0, data, length);
if(osmoload.memcrc != crc) {
osmoload.memoff -= osmoload.memreq;
printf("\nbad crc %4.4x (not %4.4x) at offset 0x%8.8x", crc, osmoload.memcrc, osmoload.memoff);
} else {
putchar('.');
}
memcpy(osmoload.binbuf + osmoload.memoff, data, length);
osmoload.memoff += length;
}
uint32_t rembytes = osmoload.memlen - osmoload.memoff;
if(!rembytes) {
puts("done.");
osmoload.quit = 1;
unsigned c = osmoload.memlen;
char *p = osmoload.binbuf;
while(c) {
rc = fwrite(p, 1, c, osmoload.binfile);
if(ferror(osmoload.binfile)) {
printf("Could not read from file: %s\n", strerror(errno));
exit(1);
}
c -= rc;
p += rc;
}
fclose(osmoload.binfile);
osmoload.binfile = NULL;
free(osmoload.binbuf);
return;
}
uint8_t reqbytes = (rembytes < MEM_MSG_MAX) ? rembytes : MEM_MSG_MAX;
struct msgb *msg = msgb_alloc(MSGB_MAX, "loader");
msgb_put_u8(msg, LOADER_MEM_READ);
msgb_put_u8(msg, reqbytes);
msgb_put_u32(msg, osmoload.membase + osmoload.memoff);
loader_send_request(msg);
msgb_free(msg);
}
static int fc_in(struct bssgp_flow_control *fc, unsigned int pdu_len)
{
struct msgb *msg;
unsigned int csecs = get_centisec_diff();
csecs = round_decisec(csecs);
msg = msgb_alloc(1, "fc test");
msg->cb[0] = in_ctr++;
printf("%u: FC IN Nr %lu\n", csecs, msg->cb[0]);
bssgp_fc_in(fc, msg, pdu_len, NULL);
return 0;
}
static void
loader_start_memput(uint8_t length, uint32_t address, void *data) {
struct msgb *msg = msgb_alloc(MSGB_MAX, "loader");
msgb_put_u8(msg, LOADER_MEM_WRITE);
msgb_put_u8(msg, length);
msgb_put_u16(msg, osmo_crc16(0, data, length));
msgb_put_u32(msg, address);
memcpy(msgb_put(msg, length), data, length);
loader_send_request(msg);
msgb_free(msg);
osmoload.state = STATE_QUERY_PENDING;
osmoload.command = LOADER_MEM_WRITE;
}
void lapd_rx_cb(struct osmo_dlsap_prim *dp, uint8_t tei, uint8_t sapi,
void *rx_cbdata)
{
struct msgb *msg = dp->oph.msg;
switch (dp->oph.primitive) {
case PRIM_DL_EST:
DEBUGP(DLAPDTEST, "DL_EST: sapi(%d) tei(%d)\n", sapi, tei);
break;
case PRIM_DL_REL:
DEBUGP(DLAPDTEST, "DL_REL: sapi(%d) tei(%d)\n", sapi, tei);
break;
case PRIM_DL_DATA:
case PRIM_DL_UNIT_DATA:
if (dp->oph.operation == PRIM_OP_INDICATION) {
struct msgb *nmsg;
char *ptr;
int x;
msg->l2h = msg->l3h;
DEBUGP(DLAPDTEST, "RX: %s sapi=%d tei=%d\n",
osmo_hexdump(msgb_l2(msg), msgb_l2len(msg)),
sapi, tei);
LOGP(DLAPDTEST, LOGL_DEBUG, "forwarding message\n");
nmsg = msgb_alloc(1024, "LAPD/test");
if (nmsg == NULL) {
LOGP(DLAPDTEST, LOGL_ERROR, "cannot alloc msg\n");
return;
}
ptr = (char *)msgb_put(nmsg, sizeof(int));
x = *((int *)msg->data);
memcpy(ptr, &x, sizeof(int));
/* send the message back to client over LAPD */
lapd_transmit(lapd, tei, sapi, msg);
return;
}
break;
case PRIM_MDL_ERROR:
DEBUGP(DLMI, "MDL_EERROR: cause(%d)\n", dp->u.error_ind.cause);
break;
default:
printf("ERROR: unknown prim\n");
break;
}
}
static int process_meas_rep(struct gsm_meas_rep *mr)
{
struct msgb *msg;
struct meas_feed_meas *mfm;
struct gsm_subscriber *subscr;
/* ignore measurements as long as we don't know who it is */
if (!mr->lchan || !mr->lchan->conn || !mr->lchan->conn->subscr)
return 0;
subscr = mr->lchan->conn->subscr;
msg = msgb_alloc(sizeof(struct meas_feed_meas), "Meas. Feed");
if (!msg)
return 0;
/* fill in the header */
mfm = (struct meas_feed_meas *) msgb_put(msg, sizeof(*mfm));
mfm->hdr.msg_type = MEAS_FEED_MEAS;
mfm->hdr.version = MEAS_FEED_VERSION;
/* fill in MEAS_FEED_MEAS specific header */
strncpy(mfm->imsi, subscr->imsi, sizeof(mfm->imsi)-1);
mfm->imsi[sizeof(mfm->imsi)-1] = '\0';
strncpy(mfm->name, subscr->name, sizeof(mfm->name)-1);
mfm->name[sizeof(mfm->name)-1] = '\0';
strncpy(mfm->scenario, g_mfs.scenario, sizeof(mfm->scenario));
mfm->scenario[sizeof(mfm->scenario)-1] = '\0';
/* copy the entire measurement report */
memcpy(&mfm->mr, mr, sizeof(mfm->mr));
/* copy channel information */
/* we assume that the measurement report always belong to some timeslot */
mfm->lchan_type = (uint8_t)mr->lchan->type;
mfm->pchan_type = (uint8_t)mr->lchan->ts->pchan;
mfm->bts_nr = mr->lchan->ts->trx->bts->nr;
mfm->trx_nr = mr->lchan->ts->trx->nr;
mfm->ts_nr = mr->lchan->ts->nr;
mfm->ss_nr = mr->lchan->nr;
/* and send it to the socket */
if (osmo_wqueue_enqueue(&g_mfs.wqueue, msg) != 0)
msgb_free(msg);
return 0;
}
/* FIXME: read from a B channel TS */
static int handle_tsX_read(struct osmo_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 msgb *msg = msgb_alloc(TSX_ALLOC_SIZE, "mISDN TSx");
struct mISDNhead *hh;
int ret;
if (!msg)
return -ENOMEM;
hh = (struct mISDNhead *) msg->data;
ret = recv(bfd->fd, msg->data, TSX_ALLOC_SIZE, 0);
if (ret < 0) {
fprintf(stderr, "recvfrom error %s\n", strerror(errno));
return ret;
}
msgb_put(msg, ret);
if (hh->prim != PH_CONTROL_IND)
DEBUGP(DLMIB, "<= BCHAN len = %d, prim(0x%x) id(0x%x): %s\n",
ret, hh->prim, hh->id,
get_value_string(prim_names, hh->prim));
switch (hh->prim) {
case PH_DATA_IND:
msg->l2h = msg->data + MISDN_HEADER_LEN;
DEBUGP(DLMIB, "BCHAN RX: %s\n",
osmo_hexdump(msgb_l2(msg), ret - MISDN_HEADER_LEN));
/* the number of bytes received indicates that data to send */
handle_tsX_write(bfd, msgb_l2len(msg));
return e1inp_rx_ts(e1i_ts, msg, 0, 0);
case PH_ACTIVATE_IND:
case PH_DATA_CNF:
break;
default:
break;
}
/* FIXME: why do we free signalling msgs in the caller, and trau not? */
msgb_free(msg);
return ret;
}
static int pack_and_send(struct esme *esme, uint32_t type, void *ptr)
{
struct msgb *msg = msgb_alloc(4096, "SMPP_Tx");
int rc, rlen;
if (!msg)
return -ENOMEM;
rc = smpp34_pack(type, msg->tail, msgb_tailroom(msg), &rlen, ptr);
if (rc != 0) {
LOGP(DSMPP, LOGL_ERROR, "[%s] Error during smpp34_pack(): %s\n",
esme->system_id, smpp34_strerror);
msgb_free(msg);
return -EINVAL;
}
msgb_put(msg, rlen);
return osmo_wqueue_enqueue(&esme->wqueue, msg);
}
static void
loader_do_fprogram() {
uint32_t rembytes = osmoload.memlen - osmoload.memoff;
if(!rembytes) {
puts("done.");
osmoload.quit = 1;
return;
}
osmo_timer_schedule(&osmoload.timeout, 0, 10000000);
uint8_t reqbytes = (rembytes < MEM_MSG_MAX) ? rembytes : MEM_MSG_MAX;
osmoload.memcrc = osmo_crc16(0, (uint8_t *) osmoload.binbuf + osmoload.memoff, reqbytes);
osmoload.memreq = reqbytes;
struct msgb *msg = msgb_alloc(MSGB_MAX, "loader");
msgb_put_u8(msg, LOADER_FLASH_PROGRAM);
msgb_put_u8(msg, reqbytes);
msgb_put_u16(msg, osmoload.memcrc);
msgb_put_u8(msg, 0); // XXX: align data to 16bit
msgb_put_u8(msg, osmoload.memchip);
msgb_put_u32(msg, osmoload.membase + osmoload.memoff);
unsigned char *p = msgb_put(msg, reqbytes);
memcpy(p, osmoload.binbuf + osmoload.memoff, reqbytes);
#if 0
printf("Sending %u bytes at offset %u to address %x with crc %x\n",
reqbytes, osmoload.memoff, osmoload.membase + osmoload.memoff,
osmoload.memcrc);
#endif
loader_send_request(msg);
msgb_free(msg);
osmoload.memoff += reqbytes;
}
/**
* @brief L1CTL socket file descriptor callback function.
*
* @param ofd The osmocom file descriptor.
* @param what Indicates if the fd has a read, write or exception request. See select.h.
*
* Will be called by osmo_select_main() if data on fd is pending.
*/
static int l1ctl_sock_data_cb(struct osmo_fd *ofd, unsigned int what)
{
struct l1ctl_sock_client *lsc = ofd->data;
struct l1ctl_hdr *l1h;
struct msgb *msg;
uint16_t len;
int rc;
/* Check if request is really read request */
if (!(what & BSC_FD_READ))
return 0;
msg = msgb_alloc(L1CTL_SOCK_MSGB_SIZE, "L1CTL sock rx");
/* read length of the message first and convert to host byte order */
rc = read(ofd->fd, &len, sizeof(len));
if (rc < sizeof(len))
goto err_close;
/* convert to host byte order */
len = ntohs(len);
if (len <= 0 || len > L1CTL_SOCK_MSGB_SIZE)
goto err_close;
rc = read(ofd->fd, msgb_data(msg), len);
if (rc == len) {
msgb_put(msg, rc);
l1h = (void *) msgb_data(msg);
msg->l1h = (void *) l1h;
lsc->l1ctl_sock->recv_cb(lsc, msg);
return 0;
}
err_close:
LOGP(DL1C, LOGL_ERROR, "Failed to receive msg from l2. Connection will be closed.\n");
l1ctl_client_destroy(lsc);
return 0;
}
static int
loader_read_cb(struct osmo_fd *fd, unsigned int flags) {
struct msgb *msg;
uint16_t len;
int rc;
msg = msgb_alloc(MSGB_MAX, "loader");
if (!msg) {
fprintf(stderr, "Failed to allocate msg.\n");
return -1;
}
rc = read(fd->fd, &len, sizeof(len));
if (rc < sizeof(len)) {
fprintf(stderr, "Short read. Error.\n");
exit(2);
}
if (ntohs(len) > MSGB_MAX) {
fprintf(stderr, "Length is too big: %u\n", ntohs(len));
msgb_free(msg);
return -1;
}
/* blocking read for the poor... we can starve in here... */
msg->l2h = msgb_put(msg, ntohs(len));
rc = read(fd->fd, msg->l2h, msgb_l2len(msg));
if (rc != msgb_l2len(msg)) {
fprintf(stderr, "Can not read data: rc: %d errno: %d\n", rc, errno);
msgb_free(msg);
return -1;
}
loader_handle_reply(msg);
msgb_free(msg);
return 0;
}
int read_cb(struct osmo_dgram *conn)
{
int error;
struct msgb *msg;
LOGP(DLAPDTEST, LOGL_DEBUG, "received message from datagram\n");
msg = msgb_alloc(1200, "LAPD/test");
if (msg == NULL) {
LOGP(DLAPDTEST, LOGL_ERROR, "cannot allocate message\n");
return -1;
}
if (osmo_dgram_recv(conn, msg) < 0) {
LOGP(DLAPDTEST, LOGL_ERROR, "cannot receive message\n");
return -1;
}
if (lapd_receive(lapd, msg, &error) < 0) {
LOGP(DLAPDTEST, LOGL_ERROR, "lapd_receive returned error!\n");
return -1;
}
return 0;
}
static int pcu_sock_read(struct osmo_fd *bfd)
{
struct pcu_sock_state *state = (struct pcu_sock_state *)bfd->data;
struct gsm_pcu_if *pcu_prim;
struct msgb *msg;
int rc;
msg = msgb_alloc(sizeof(*pcu_prim), "pcu_sock_rx");
if (!msg)
return -ENOMEM;
pcu_prim = (struct gsm_pcu_if *) msg->tail;
rc = recv(bfd->fd, msg->tail, msgb_tailroom(msg), 0);
if (rc == 0)
goto close;
if (rc < 0) {
if (errno == EAGAIN)
return 0;
goto close;
}
rc = pcu_rx(pcu_prim->msg_type, pcu_prim);
/* as we always synchronously process the message in pcu_rx() and
* its callbacks, we can free the message here. */
msgb_free(msg);
return rc;
close:
msgb_free(msg);
pcu_sock_close(state, 1);
return -1;
}
/* FIXME: merge with smpp_smsc.c */
static int esme_read_cb(struct osmo_fd *ofd)
{
struct esme *esme = ofd->data;
uint32_t len;
uint8_t *lenptr = (uint8_t *) &len;
uint8_t *cur;
struct msgb *msg;
int rdlen;
int rc;
switch (esme->read_state) {
case READ_ST_IN_LEN:
rdlen = sizeof(uint32_t) - esme->read_idx;
rc = read(ofd->fd, lenptr + esme->read_idx, rdlen);
if (rc < 0) {
LOGP(DSMPP, LOGL_ERROR, "[%s] read returned %d\n",
esme->system_id, rc);
} else if (rc == 0) {
goto dead_socket;
} else
esme->read_idx += rc;
if (esme->read_idx >= sizeof(uint32_t)) {
esme->read_len = ntohl(len);
msg = msgb_alloc(esme->read_len, "SMPP Rx");
if (!msg)
return -ENOMEM;
esme->read_msg = msg;
cur = msgb_put(msg, sizeof(uint32_t));
memcpy(cur, lenptr, sizeof(uint32_t));
esme->read_state = READ_ST_IN_MSG;
esme->read_idx = sizeof(uint32_t);
}
break;
case READ_ST_IN_MSG:
msg = esme->read_msg;
rdlen = esme->read_len - esme->read_idx;
rc = read(ofd->fd, msg->tail, OSMO_MIN(rdlen, msgb_tailroom(msg)));
if (rc < 0) {
LOGP(DSMPP, LOGL_ERROR, "[%s] read returned %d\n",
esme->system_id, rc);
} else if (rc == 0) {
goto dead_socket;
} else {
esme->read_idx += rc;
msgb_put(msg, rc);
}
if (esme->read_idx >= esme->read_len) {
rc = smpp_pdu_rx(esme, esme->read_msg);
esme->read_msg = NULL;
esme->read_idx = 0;
esme->read_len = 0;
esme->read_state = READ_ST_IN_LEN;
}
break;
}
return 0;
dead_socket:
msgb_free(esme->read_msg);
osmo_fd_unregister(&esme->wqueue.bfd);
close(esme->wqueue.bfd.fd);
esme->wqueue.bfd.fd = -1;
exit(2342);
return 0;
}
/* 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;
}
static int handle_ts1_read(struct osmo_fd *bfd)
{
struct e1inp_line *line = bfd->data;
struct misdn_line *mline = line->driver_data;
unsigned int ts_nr = bfd->priv_nr;
struct e1inp_ts *e1i_ts = &line->ts[ts_nr-1];
struct e1inp_sign_link *link;
struct msgb *msg = msgb_alloc(TS1_ALLOC_SIZE, "mISDN TS1");
struct sockaddr_mISDN l2addr;
struct mISDNhead *hh;
socklen_t alen;
int ret;
if (!msg)
return -ENOMEM;
hh = (struct mISDNhead *) msg->data;
alen = sizeof(l2addr);
ret = recvfrom(bfd->fd, msg->data, 300, 0,
(struct sockaddr *) &l2addr, &alen);
if (ret < 0) {
fprintf(stderr, "recvfrom error %s\n", strerror(errno));
msgb_free(msg);
return ret;
}
if (alen != sizeof(l2addr)) {
fprintf(stderr, "%s error len\n", __func__);
msgb_free(msg);
return -EINVAL;
}
msgb_put(msg, ret);
DEBUGP(DLMI, "alen =%d, dev(%d) channel(%d) sapi(%d) tei(%d)\n",
alen, l2addr.dev, l2addr.channel, l2addr.sapi, l2addr.tei);
DEBUGP(DLMI, "<= len = %d, prim(0x%x) id(0x%x): %s\n",
ret, hh->prim, hh->id, get_value_string(prim_names, hh->prim));
switch (hh->prim) {
case DL_INFORMATION_IND:
/* mISDN tells us which channel number is allocated for this
* tuple of (SAPI, TEI). */
DEBUGP(DLMI, "DL_INFORMATION_IND: use channel(%d) sapi(%d) tei(%d) for now\n",
l2addr.channel, l2addr.sapi, l2addr.tei);
link = e1inp_lookup_sign_link(e1i_ts, l2addr.tei, l2addr.sapi);
if (!link) {
DEBUGPC(DLMI, "mISDN message for unknown sign_link\n");
msgb_free(msg);
return -EINVAL;
}
/* save the channel number in the driver private struct */
link->driver.misdn.channel = l2addr.channel;
msgb_free(msg);
break;
case DL_ESTABLISH_IND:
DEBUGP(DLMI, "DL_ESTABLISH_IND: channel(%d) sapi(%d) tei(%d)\n",
l2addr.channel, l2addr.sapi, l2addr.tei);
/* For some strange reason, sometimes the DL_INFORMATION_IND tells
* us the wrong channel, and we only get the real channel number
* during the DL_ESTABLISH_IND */
link = e1inp_lookup_sign_link(e1i_ts, l2addr.tei, l2addr.sapi);
if (!link) {
DEBUGPC(DLMI, "mISDN message for unknown sign_link\n");
msgb_free(msg);
return -EINVAL;
}
/* save the channel number in the driver private struct */
link->driver.misdn.channel = l2addr.channel;
ret = e1inp_event(e1i_ts, S_L_INP_TEI_UP, l2addr.tei, l2addr.sapi);
msgb_free(msg);
break;
case DL_RELEASE_IND:
DEBUGP(DLMI, "DL_RELEASE_IND: channel(%d) sapi(%d) tei(%d)\n",
l2addr.channel, l2addr.sapi, l2addr.tei);
ret = e1inp_event(e1i_ts, S_L_INP_TEI_DN, l2addr.tei, l2addr.sapi);
msgb_free(msg);
break;
case DL_DATA_IND:
case DL_UNITDATA_IND:
msg->l2h = msg->data + MISDN_HEADER_LEN;
DEBUGP(DLMI, "RX: %s\n", osmo_hexdump(msgb_l2(msg), ret - MISDN_HEADER_LEN));
if (mline->use_userspace_lapd) {
LOGP(DLMI, LOGL_ERROR, "DL_DATA_IND but userspace LAPD ?!?\n");
msgb_free(msg);
return -EIO;
}
ret = e1inp_rx_ts(e1i_ts, msg, l2addr.tei, l2addr.sapi);
break;
case PH_ACTIVATE_IND:
DEBUGP(DLMI, "PH_ACTIVATE_IND: channel(%d) sapi(%d) tei(%d)\n",
l2addr.channel, l2addr.sapi, l2addr.tei);
msgb_free(msg);
break;
case PH_DEACTIVATE_IND:
DEBUGP(DLMI, "PH_DEACTIVATE_IND: channel(%d) sapi(%d) tei(%d)\n",
l2addr.channel, l2addr.sapi, l2addr.tei);
msgb_free(msg);
break;
case PH_DATA_IND:
if (!mline->use_userspace_lapd) {
LOGP(DLMI, LOGL_ERROR, "PH_DATA_IND but kernel LAPD ?!?\n");
return -EIO;
}
/* remove the Misdn Header */
msgb_pull(msg, MISDN_HEADER_LEN);
/* hand into the LAPD code */
DEBUGP(DLMI, "RX: %s\n", osmo_hexdump(msg->data, msg->len));
ret = e1inp_rx_ts_lapd(e1i_ts, msg);
break;
default:
msgb_free(msg);
break;
}
return ret;
}
static void test_agch_queue(void)
{
int rc;
uint8_t out_buf[GSM_MACBLOCK_LEN];
struct gsm_time g_time;
const int num_rounds = 40;
const int num_ima_per_round = 2;
const int num_rej_per_round = 16;
int round, idx;
int count = 0;
struct msgb *msg = NULL;
int multiframes = 0;
int imm_ass_count = 0;
int imm_ass_rej_count = 0;
int imm_ass_rej_ref_count = 0;
g_time.fn = 0;
g_time.t1 = 0;
g_time.t2 = 0;
g_time.t3 = 6;
printf("Testing AGCH messages queue handling.\n");
btsb->agch_max_queue_length = 32;
btsb->agch_queue_low_level = 30;
btsb->agch_queue_high_level = 30;
btsb->agch_queue_thresh_level = 60;
for (round = 1; round <= num_rounds; round++) {
for (idx = 0; idx < num_ima_per_round; idx++) {
msg = msgb_alloc(GSM_MACBLOCK_LEN, __FUNCTION__);
put_imm_ass(msg, ++count);
bts_agch_enqueue(bts, msg);
imm_ass_count++;
}
for (idx = 0; idx < num_rej_per_round; idx++) {
msg = msgb_alloc(GSM_MACBLOCK_LEN, __FUNCTION__);
put_imm_ass_rej(msg, ++count, 10);
bts_agch_enqueue(bts, msg);
imm_ass_rej_count++;
imm_ass_rej_ref_count++;
}
}
printf("AGCH filled: count %u, imm.ass %d, imm.ass.rej %d (refs %d), "
"queue limit %u, occupied %d, "
"dropped %"PRIu64", merged %"PRIu64", rejected %"PRIu64", "
"ag-res %"PRIu64", non-res %"PRIu64"\n",
count, imm_ass_count, imm_ass_rej_count, imm_ass_rej_ref_count,
btsb->agch_max_queue_length, btsb->agch_queue_length,
btsb->agch_queue_dropped_msgs, btsb->agch_queue_merged_msgs,
btsb->agch_queue_rejected_msgs, btsb->agch_queue_agch_msgs,
btsb->agch_queue_pch_msgs);
imm_ass_count = 0;
imm_ass_rej_count = 0;
imm_ass_rej_ref_count = 0;
for (idx = 0; 1; idx++) {
struct gsm48_imm_ass *ima;
int is_agch = (idx % 3) == 0; /* 1 AG reserved, 2 PCH */
if (is_agch)
multiframes++;
rc = bts_ccch_copy_msg(bts, out_buf, &g_time, is_agch);
ima = (struct gsm48_imm_ass *)out_buf;
switch (ima->msg_type) {
case GSM48_MT_RR_IMM_ASS:
imm_ass_count++;
break;
case GSM48_MT_RR_IMM_ASS_REJ:
imm_ass_rej_count++;
imm_ass_rej_ref_count +=
count_imm_ass_rej_refs((struct gsm48_imm_ass_rej *)ima);
break;
default:
break;
}
if (is_agch && rc <= 0)
break;
}
printf("AGCH drained: multiframes %u, imm.ass %d, imm.ass.rej %d (refs %d), "
"queue limit %u, occupied %d, "
"dropped %"PRIu64", merged %"PRIu64", rejected %"PRIu64", "
"ag-res %"PRIu64", non-res %"PRIu64"\n",
multiframes, imm_ass_count, imm_ass_rej_count, imm_ass_rej_ref_count,
btsb->agch_max_queue_length, btsb->agch_queue_length,
btsb->agch_queue_dropped_msgs, btsb->agch_queue_merged_msgs,
btsb->agch_queue_rejected_msgs, btsb->agch_queue_agch_msgs,
btsb->agch_queue_pch_msgs);
}
/* decode an rtp frame and create a new buffer with payload */
static int rtp_decode(struct msgb *msg, u_int32_t callref, struct msgb **data)
{
struct msgb *new_msg;
struct gsm_data_frame *frame;
struct rtp_hdr *rtph = (struct rtp_hdr *)msg->data;
struct rtp_x_hdr *rtpxh;
u_int8_t *payload;
int payload_len;
int msg_type;
int x_len;
if (msg->len < 12) {
DEBUGPC(DMUX, "received RTP frame too short (len = %d)\n",
msg->len);
return -EINVAL;
}
if (rtph->version != RTP_VERSION) {
DEBUGPC(DMUX, "received RTP version %d not supported.\n",
rtph->version);
return -EINVAL;
}
payload = msg->data + sizeof(struct rtp_hdr) + (rtph->csrc_count << 2);
payload_len = msg->len - sizeof(struct rtp_hdr) - (rtph->csrc_count << 2);
if (payload_len < 0) {
DEBUGPC(DMUX, "received RTP frame too short (len = %d, "
"csrc count = %d)\n", msg->len, rtph->csrc_count);
return -EINVAL;
}
if (rtph->extension) {
if (payload_len < sizeof(struct rtp_x_hdr)) {
DEBUGPC(DMUX, "received RTP frame too short for "
"extension header\n");
return -EINVAL;
}
rtpxh = (struct rtp_x_hdr *)payload;
x_len = ntohs(rtpxh->length) * 4 + sizeof(struct rtp_x_hdr);
payload += x_len;
payload_len -= x_len;
if (payload_len < 0) {
DEBUGPC(DMUX, "received RTP frame too short, "
"extension header exceeds frame length\n");
return -EINVAL;
}
}
if (rtph->padding) {
if (payload_len < 0) {
DEBUGPC(DMUX, "received RTP frame too short for "
"padding length\n");
return -EINVAL;
}
payload_len -= payload[payload_len - 1];
if (payload_len < 0) {
DEBUGPC(DMUX, "received RTP frame with padding "
"greater than payload\n");
return -EINVAL;
}
}
switch (rtph->payload_type) {
case RTP_PT_GSM_FULL:
msg_type = GSM_TCHF_FRAME;
if (payload_len != 33) {
DEBUGPC(DMUX, "received RTP full rate frame with "
"payload length != 32 (len = %d)\n",
payload_len);
return -EINVAL;
}
break;
case RTP_PT_GSM_EFR:
msg_type = GSM_TCHF_FRAME_EFR;
break;
default:
DEBUGPC(DMUX, "received RTP frame with unknown payload "
"type %d\n", rtph->payload_type);
return -EINVAL;
}
new_msg = msgb_alloc(sizeof(struct gsm_data_frame) + payload_len,
"GSM-DATA");
if (!new_msg)
return -ENOMEM;
frame = (struct gsm_data_frame *)(new_msg->data);
frame->msg_type = msg_type;
frame->callref = callref;
memcpy(frame->data, payload, payload_len);
msgb_put(new_msg, sizeof(struct gsm_data_frame) + payload_len);
*data = new_msg;
return 0;
}