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; }