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;
}
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 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;
}
static void loader_send_init(uint8_t dlci)
{
struct msgb *msg = sercomm_alloc_msgb(9);
msgb_put_u8(msg, LOADER_INIT);
msgb_put_u32(msg, 0);
msgb_put_u32(msg, (uint32_t)&_start);
sercomm_sendmsg(dlci, 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_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;
}
struct msgb *bsc_msc_id_get_resp(int fixed, const char *token, const uint8_t *res, int len)
{
struct msgb *msg;
if (!token) {
LOGP(DMSC, LOGL_ERROR, "No token specified.\n");
return NULL;
}
msg = msgb_alloc_headroom(4096, 128, "id resp");
if (!msg) {
LOGP(DMSC, LOGL_ERROR, "Failed to create the message.\n");
return NULL;
}
/*
* The situation is bizarre. The encoding doesn't follow the
* TLV structure. It is more like a LV and old versions had
* it wrong but we want new versions to old servers so we
* introduce the quirk here.
*/
msg->l2h = msgb_v_put(msg, IPAC_MSGT_ID_RESP);
if (fixed) {
msgb_put_u8(msg, 0);
msgb_put_u8(msg, strlen(token) + 2);
msgb_tv_fixed_put(msg, IPAC_IDTAG_UNITNAME, strlen(token) + 1, (uint8_t *) token);
if (len > 0) {
msgb_put_u8(msg, 0);
msgb_put_u8(msg, len + 1);
msgb_tv_fixed_put(msg, 0x24, len, res);
}
} else {
msgb_l16tv_put(msg, strlen(token) + 1,
IPAC_IDTAG_UNITNAME, (uint8_t *) token);
}
return msg;
}
static struct msgb *l1_to_rtppayload_amr(uint8_t *l1_payload, uint8_t payload_len,
struct gsm_lchan *lchan)
{
#ifndef USE_L1_RTP_MODE
struct amr_multirate_conf *amr_mrc = &lchan->tch.amr_mr;
#endif
struct msgb *msg;
uint8_t amr_if2_len = payload_len - 2;
uint8_t *cur;
msg = msgb_alloc_headroom(1024, 128, "L1C-to-RTP");
if (!msg)
return NULL;
#ifdef USE_L1_RTP_MODE
cur = msgb_put(msg, amr_if2_len);
memcpy(cur, l1_payload+2, amr_if2_len);
/*
* Audiocode's MGW doesn't like receiving CMRs that are not
* the same as the previous one. This means we need to patch
* the content here.
*/
if ((cur[0] & 0xF0) == 0xF0)
cur[0]= lchan->tch.last_cmr << 4;
else
lchan->tch.last_cmr = cur[0] >> 4;
#else
u_int8_t cmr;
uint8_t ft = l1_payload[2] & 0xF;
uint8_t cmr_idx = l1_payload[1];
/* CMR == Unset means CMR was not transmitted at this TDMA */
if (cmr_idx == GsmL1_AmrCodecMode_Unset)
cmr = lchan->tch.last_cmr;
else if (cmr_idx >= amr_mrc->num_modes ||
cmr_idx > GsmL1_AmrCodecMode_Unset) {
/* Make sure the CMR of the phone is in the active codec set */
LOGP(DL1C, LOGL_NOTICE, "L1->RTP: overriding CMR IDX %u\n", cmr_idx);
cmr = AMR_CMR_NONE;
} else {
cmr = amr_mrc->bts_mode[cmr_idx].mode;
lchan->tch.last_cmr = cmr;
}
/* RFC 3267 4.4.1 Payload Header */
msgb_put_u8(msg, (cmr << 4));
/* RFC 3267 AMR TOC */
msgb_put_u8(msg, AMR_TOC_QBIT | (ft << 3));
cur = msgb_put(msg, amr_if2_len-1);
/* step1: reverse the bit-order within every byte */
osmo_revbytebits_buf(l1_payload+2, amr_if2_len);
/* step2: shift everything left by one nibble */
osmo_nibble_shift_left_unal(cur, l1_payload+2, amr_if2_len*2 -1);
#endif /* USE_L1_RTP_MODE */
return msg;
}
/*
* We don't look at the content of the request yet and lie
* about most of the responses.
*/
static void respond_to(struct sockaddr_in *src, struct osmo_fd *fd,
uint8_t *data, size_t len)
{
static int fetched_info = 0;
static char mac_str[20] = {0, };
static char model_name[64] = {0, };
static char ser_str[20] = {0, };
struct sockaddr_in loc_addr;
int rc;
char loc_ip[INET_ADDRSTRLEN];
struct msgb *msg = msgb_alloc_headroom(512, 128, "ipa get response");
if (!msg) {
LOGP(DFIND, LOGL_ERROR, "Failed to allocate msgb\n");
return;
}
if (!fetched_info) {
int fd_eth;
int serno;
/* fetch the MAC */
fd_eth = open(ETH0_ADDR_SYSFS, O_RDONLY);
if (fd_eth >= 0) {
read(fd_eth, mac_str, sizeof(mac_str)-1);
mac_str[sizeof(mac_str)-1] = '\0';
close(fd_eth);
}
/* fetch the serial number */
lc15bts_par_get_int(tall_mgr_ctx, LC15BTS_PAR_SERNR, &serno);
snprintf(ser_str, sizeof(ser_str), "%d", serno);
strncpy(model_name, get_hwversion_desc(), sizeof(model_name)-1);
fetched_info = 1;
}
if (source_for_dest(&src->sin_addr, &loc_addr.sin_addr) != 0) {
LOGP(DFIND, LOGL_ERROR, "Failed to determine local source\n");
return;
}
msgb_put_u8(msg, IPAC_MSGT_ID_RESP);
/* append MAC addr */
quirk_l16tv_put(msg, strlen(mac_str) + 1, IPAC_IDTAG_MACADDR, (uint8_t *) mac_str);
/* append ip address */
inet_ntop(AF_INET, &loc_addr.sin_addr, loc_ip, sizeof(loc_ip));
quirk_l16tv_put(msg, strlen(loc_ip) + 1, IPAC_IDTAG_IPADDR, (uint8_t *) loc_ip);
/* append the serial number */
quirk_l16tv_put(msg, strlen(ser_str) + 1, IPAC_IDTAG_SERNR, (uint8_t *) ser_str);
/* abuse some flags */
quirk_l16tv_put(msg, strlen(model_name) + 1, IPAC_IDTAG_UNIT, (uint8_t *) model_name);
/* ip.access nanoBTS would reply to port==3006 */
ipaccess_prepend_header_quirk(msg, IPAC_PROTO_IPACCESS);
rc = sendto(fd->fd, msg->data, msg->len, 0, (struct sockaddr *)src, sizeof(*src));
if (rc != msg->len)
LOGP(DFIND, LOGL_ERROR,
"Failed to send with rc(%d) errno(%d)\n", rc, errno);
}
static void cmd_handler(uint8_t dlci, struct msgb *msg)
{
if (msg->data_len < 1) {
return;
}
uint8_t command = msgb_pull_u8(msg);
int res = 0;
flash_lock_t lock;
void *data;
uint8_t chip;
uint8_t nbytes;
uint16_t crc, mycrc;
uint32_t address;
struct msgb *reply = sercomm_alloc_msgb(256); // XXX
if (!reply) {
printf("Failed to allocate reply buffer!\n");
goto out;
}
switch (command) {
case LOADER_PING:
loader_send_simple(reply, dlci, LOADER_PING);
break;
case LOADER_RESET:
loader_send_simple(reply, dlci, LOADER_RESET);
device_reset();
break;
case LOADER_POWEROFF:
loader_send_simple(reply, dlci, LOADER_POWEROFF);
device_poweroff();
break;
case LOADER_ENTER_ROM_LOADER:
loader_send_simple(reply, dlci, LOADER_ENTER_ROM_LOADER);
device_enter_loader(1);
break;
case LOADER_ENTER_FLASH_LOADER:
loader_send_simple(reply, dlci, LOADER_ENTER_FLASH_LOADER);
device_enter_loader(0);
break;
case LOADER_MEM_READ:
nbytes = msgb_pull_u8(msg);
address = msgb_pull_u32(msg);
crc = osmo_crc16(0, (void *)address, nbytes);
msgb_put_u8(reply, LOADER_MEM_READ);
msgb_put_u8(reply, nbytes);
msgb_put_u16(reply, crc);
msgb_put_u32(reply, address);
memcpy(msgb_put(reply, nbytes), (void *)address, nbytes);
sercomm_sendmsg(dlci, reply);
break;
case LOADER_MEM_WRITE:
nbytes = msgb_pull_u8(msg);
crc = msgb_pull_u16(msg);
address = msgb_pull_u32(msg);
data = msgb_pull(msg, nbytes) - nbytes;
mycrc = osmo_crc16(0, data, nbytes);
if (mycrc == crc) {
memcpy((void *)address, data, nbytes);
}
msgb_put_u8(reply, LOADER_MEM_WRITE);
msgb_put_u8(reply, nbytes);
msgb_put_u16(reply, mycrc);
msgb_put_u32(reply, address);
sercomm_sendmsg(dlci, reply);
break;
case LOADER_JUMP:
address = msgb_pull_u32(msg);
msgb_put_u8(reply, LOADER_JUMP);
msgb_put_u32(reply, address);
sercomm_sendmsg(dlci, reply);
device_jump((void *)address);
break;
case LOADER_FLASH_INFO:
msgb_put_u8(reply, LOADER_FLASH_INFO);
msgb_put_u8(reply, 1); // nchips
// chip 1
msgb_put_u32(reply, (uint32_t)the_flash.f_base);
msgb_put_u32(reply, the_flash.f_size);
msgb_put_u8(reply, the_flash.f_nregions);
unsigned i;
for (i = 0; i < the_flash.f_nregions; i++) {
msgb_put_u32(reply, the_flash.f_regions[i].fr_bnum);
msgb_put_u32(reply, the_flash.f_regions[i].fr_bsize);
}
sercomm_sendmsg(dlci, reply);
break;
case LOADER_FLASH_ERASE:
case LOADER_FLASH_UNLOCK:
case LOADER_FLASH_LOCK:
case LOADER_FLASH_LOCKDOWN:
chip = msgb_pull_u8(msg);
address = msgb_pull_u32(msg);
if (command == LOADER_FLASH_ERASE) {
res = flash_block_erase(&the_flash, address);
}
if (command == LOADER_FLASH_UNLOCK) {
res = flash_block_unlock(&the_flash, address);
}
if (command == LOADER_FLASH_LOCK) {
res = flash_block_lock(&the_flash, address);
}
if (command == LOADER_FLASH_LOCKDOWN) {
res = flash_block_lockdown(&the_flash, address);
}
msgb_put_u8(reply, command);
msgb_put_u8(reply, chip);
msgb_put_u32(reply, address);
msgb_put_u32(reply, (res != 0));
sercomm_sendmsg(dlci, reply);
break;
case LOADER_FLASH_GETLOCK:
chip = msgb_pull_u8(msg);
address = msgb_pull_u32(msg);
lock = flash_block_getlock(&the_flash, address);
msgb_put_u8(reply, command);
msgb_put_u8(reply, chip);
msgb_put_u32(reply, address);
switch (lock) {
case FLASH_UNLOCKED:
msgb_put_u32(reply, LOADER_FLASH_UNLOCKED);
break;
case FLASH_LOCKED:
msgb_put_u32(reply, LOADER_FLASH_LOCKED);
break;
case FLASH_LOCKED_DOWN:
msgb_put_u32(reply, LOADER_FLASH_LOCKED_DOWN);
break;
default:
msgb_put_u32(reply, 0xFFFFFFFF);
break;
}
sercomm_sendmsg(dlci, reply);
break;
case LOADER_FLASH_PROGRAM:
nbytes = msgb_pull_u8(msg);
crc = msgb_pull_u16(msg);
msgb_pull_u8(msg); // XXX align
chip = msgb_pull_u8(msg);
address = msgb_pull_u32(msg);
data = msgb_pull(msg, nbytes) - nbytes;
mycrc = osmo_crc16(0, data, nbytes);
if (mycrc == crc) {
res = flash_program(&the_flash, address, data, nbytes);
}
msgb_put_u8(reply, LOADER_FLASH_PROGRAM);
msgb_put_u8(reply, nbytes);
msgb_put_u16(reply, mycrc);
msgb_put_u8(reply, 0); // XXX align
msgb_put_u8(reply, chip);
msgb_put_u32(reply, address);
msgb_put_u32(reply, (uint32_t) res); // XXX
sercomm_sendmsg(dlci, reply);
break;
default:
printf("unknown command %d\n", command);
msgb_free(reply);
break;
}
out:
msgb_free(msg);
}
static void loader_send_simple(struct msgb *msg, uint8_t dlci, uint8_t command)
{
msgb_put_u8(msg, command);
sercomm_sendmsg(dlci, msg);
}
static int osmo_stats_reporter_statsd_send(struct osmo_stats_reporter *srep,
const char *name1, unsigned int index1, const char *name2, int64_t value,
const char *unit)
{
char *buf;
int buf_size;
int nchars, rc = 0;
char *fmt = NULL;
char *prefix = srep->name_prefix;
int old_len = msgb_length(srep->buffer);
if (prefix) {
if (name1) {
if (index1 != 0)
fmt = "%1$s.%2$s.%6$u.%3$s:%4$d|%5$s";
else
fmt = "%1$s.%2$s.%3$s:%4$d|%5$s";
} else {
fmt = "%1$s.%2$0.0s%3$s:%4$d|%5$s";
}
} else {
prefix = "";
if (name1) {
if (index1 != 0)
fmt = "%1$s%2$s.%6$u.%3$s:%4$d|%5$s";
else
fmt = "%1$s%2$s.%3$s:%4$d|%5$s";
} else {
fmt = "%1$s%2$0.0s%3$s:%4$d|%5$s";
}
}
if (srep->agg_enabled) {
if (msgb_length(srep->buffer) > 0 &&
msgb_tailroom(srep->buffer) > 0)
{
msgb_put_u8(srep->buffer, '\n');
}
}
buf = (char *)msgb_put(srep->buffer, 0);
buf_size = msgb_tailroom(srep->buffer);
nchars = snprintf(buf, buf_size, fmt,
prefix, name1, name2,
value, unit, index1);
if (nchars >= buf_size) {
/* Truncated */
/* Restore original buffer (without trailing LF) */
msgb_trim(srep->buffer, old_len);
/* Send it */
rc = osmo_stats_reporter_send_buffer(srep);
/* Try again */
buf = (char *)msgb_put(srep->buffer, 0);
buf_size = msgb_tailroom(srep->buffer);
nchars = snprintf(buf, buf_size, fmt,
prefix, name1, name2,
value, unit, index1);
if (nchars >= buf_size)
return -EMSGSIZE;
}
if (nchars > 0)
msgb_trim(srep->buffer, msgb_length(srep->buffer) + nchars);
if (!srep->agg_enabled)
rc = osmo_stats_reporter_send_buffer(srep);
return rc;
}
