/*! \brief dequeue a msg that's pending transmission via L1 and wrap it into
* a osmo_phsap_prim */
int lapdm_phsap_dequeue_prim(struct lapdm_entity *le, struct osmo_phsap_prim *pp)
{
struct msgb *msg;
uint8_t pad;
msg = tx_dequeue_msgb(le);
if (!msg)
return -ENODEV;
/* if we have a message, send PH-DATA.req */
osmo_prim_init(&pp->oph, SAP_GSM_PH, PRIM_PH_DATA,
PRIM_OP_REQUEST, msg);
/* Pull chan_nr and link_id */
pp->u.data.chan_nr = *msg->data;
msgb_pull(msg, 1);
pp->u.data.link_id = *msg->data;
msgb_pull(msg, 1);
pad = *msg->data;
msgb_pull(msg, 1);
/* Pad the frame, we can transmit now */
lapdm_pad_msgb(msg, pad);
return 0;
}
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
loader_handle_reply(struct msgb *msg) {
if(osmoload.print_replies) {
printf("Received %d bytes:\n", msg->len);
osmoload_osmo_hexdump(msg->data, msg->len);
}
uint8_t cmd = msgb_pull_u8(msg);
uint8_t chip;
uint8_t length;
uint16_t crc;
uint32_t address;
uint32_t entrypoint;
uint32_t status;
void *data;
switch(cmd) {
case LOADER_INIT:
address = msgb_pull_u32(msg);
entrypoint = msgb_pull_u32(msg);
printf("Loader at entry %x has been started, requesting load to %x\n", entrypoint, address);
break;
case LOADER_PING:
case LOADER_RESET:
case LOADER_POWEROFF:
case LOADER_ENTER_ROM_LOADER:
case LOADER_ENTER_FLASH_LOADER:
break;
case LOADER_MEM_READ:
length = msgb_pull_u8(msg);
crc = msgb_pull_u16(msg);
address = msgb_pull_u32(msg);
data = msgb_pull(msg, length) - length;
break;
case LOADER_MEM_WRITE:
length = msgb_pull_u8(msg);
crc = msgb_pull_u16(msg);
address = msgb_pull_u32(msg);
break;
case LOADER_JUMP:
address = msgb_pull_u32(msg);
break;
case LOADER_FLASH_INFO:
break;
case LOADER_FLASH_GETLOCK:
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);
status = msgb_pull_u32(msg);
break;
case LOADER_FLASH_PROGRAM:
length = 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);
status = msgb_pull_u32(msg);
break;
default:
printf("Received unknown reply %d:\n", cmd);
osmoload_osmo_hexdump(msg->data, msg->len);
osmoload.quit = 1;
return;
}
switch(osmoload.state) {
case STATE_QUERY_PENDING:
case STATE_DUMPING:
switch(cmd) {
case LOADER_PING:
printf("Received pong.\n");
break;
case LOADER_RESET:
printf("Reset confirmed.\n");
break;
case LOADER_POWEROFF:
printf("Poweroff confirmed.\n");
break;
case LOADER_ENTER_ROM_LOADER:
printf("Jump to ROM loader confirmed.\n");
break;
case LOADER_ENTER_FLASH_LOADER:
printf("Jump to flash loader confirmed.\n");
break;
case LOADER_MEM_READ:
printf("Received memory dump of %d bytes at 0x%x:\n", length, address);
osmoload_osmo_hexdump(data, length);
break;
case LOADER_MEM_WRITE:
printf("Confirmed memory write of %d bytes at 0x%x.\n", length, address);
break;
case LOADER_JUMP:
printf("Confirmed jump to 0x%x.\n", address);
break;
case LOADER_FLASH_ERASE:
printf("Confirmed flash erase of chip %d address 0x%8.8x, status %s\n",
chip, address, status ? "FAILED" : "ok");
break;
case LOADER_FLASH_GETLOCK:
printf("Lock state of chip %d address 0x%8.8x is %s\n",
chip, address, (status == LOADER_FLASH_LOCKED ? "locked"
: (status == LOADER_FLASH_LOCKED_DOWN ? "locked down"
: (status == LOADER_FLASH_UNLOCKED ? "unlocked"
: "UNKNOWN"))));
break;
case LOADER_FLASH_UNLOCK:
printf("Confirmed flash unlock of chip %d address 0x%8.8x, status %s\n",
chip, address, status ? "FAILED" : "ok");
break;
case LOADER_FLASH_LOCK:
printf("Confirmed flash lock of chip %d address 0x%8.8x, status %s\n",
chip, address, status ? "FAILED" : "ok");
break;
case LOADER_FLASH_LOCKDOWN:
printf("Confirmed flash lockdown of chip %d address 0x%8.8x, status %s\n",
chip, address, status ? "FAILED" : "ok");
break;
case LOADER_FLASH_INFO:
loader_parse_flash_info(msg);
break;
default:
break;
}
if(osmoload.state == STATE_QUERY_PENDING) {
if(osmoload.command == cmd) {
osmoload.quit = 1;
}
}
break;
case STATE_DUMP_IN_PROGRESS:
if(cmd == LOADER_MEM_READ) {
loader_do_memdump(crc, data, length);
}
break;
case STATE_LOAD_IN_PROGRESS:
if(cmd == LOADER_MEM_WRITE) {
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('.');
}
loader_do_memload();
}
break;
case STATE_PROGRAM_GET_INFO:
case STATE_PROGRAM_IN_PROGRESS:
if(cmd == LOADER_FLASH_PROGRAM) {
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('.');
}
if(((int)status) != 0) {
printf("\nstatus %d, aborting\n", status);
exit(1);
}
loader_do_fprogram();
}
break;
case STATE_FLASHRANGE_GET_INFO:
case STATE_FLASHRANGE_IN_PROGRESS:
loader_do_flashrange(cmd, msg, chip, address, status);
break;
default:
break;
}
fflush(stdout);
}
/* remove the L2 header from a MSGB */
static inline unsigned char *msgb_pull_l2h(struct msgb *msg)
{
unsigned char *ret = msgb_pull(msg, msg->l3h - msg->l2h);
msg->l2h = NULL;
return ret;
}
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);
}
/* handling sim events */
void sim_handler(void)
{
static struct msgb *msg;
struct l1ctl_hdr *l1h;
static uint8_t mode;
static uint8_t *response;
static uint16_t length;
switch (sim_state) {
case SIM_STATE_IDLE:
if (!sim_len)
break; /* wait for SIM command */
/* check if instructions expects a response */
if (/* GET RESPONSE needs SIM_APDU_GET */
(sim_len == 5 && sim_data[0] == SIM_CLASS &&
sim_data[1] == SIM_GET_RESPONSE && sim_data[2] == 0x00 &&
sim_data[3] == 0x00) ||
/* READ BINARY/RECORD needs SIM_APDU_GET */
(sim_len >= 5 && sim_data[0] == SIM_CLASS &&
(sim_data[1] == SIM_READ_BINARY ||
sim_data[1] == SIM_READ_RECORD)))
mode = SIM_APDU_GET;
else
mode = SIM_APDU_PUT;
length = sim_data[4];
/* allocate space for expected response */
msg = msgb_alloc_headroom(256, L3_MSG_HEAD
+ sizeof(struct l1ctl_hdr), "l1ctl1");
response = msgb_put(msg, length + 2 + 1);
sim_state = SIM_STATE_TX_HEADER;
/* send APDU header */
calypso_sim_transmit(sim_data, 5);
break;
case SIM_STATE_TX_HEADER:
if (!txDoneFlag)
break; /* wait until header is transmitted */
/* Disable all interrupt driven functions */
writew(0xFF, REG_SIM_MASKIT);
/* Case 1: No input, No Output */
if (length == 0) {
sim_state = SIM_STATE_RX_STATUS;
calypso_sim_receive(response + 1, 2);
break;
}
/* Case 2: No input / Output of known length */
if (mode == SIM_APDU_PUT) {
sim_state = SIM_STATE_RX_ACK;
calypso_sim_receive(response, 1);
break;
/* Case 4: Input / No output */
} else {
sim_state = SIM_STATE_RX_ACK_DATA;
calypso_sim_receive(response, length + 1 + 2);
}
break;
case SIM_STATE_RX_STATUS:
if (!rxDoneFlag)
break; /* wait until data is received */
/* Disable all interrupt driven functions */
writew(0xFF, REG_SIM_MASKIT);
/* disable special ignore case */
sim_ignore_waiting_char = 0;
/* wrong number of bytes received */
if (sim_rx_character_count != 2) {
puts("SIM: Failed to read status\n");
goto error;
}
msgb_pull(msg, length + 1); /* pull up to status info */
goto queue;
case SIM_STATE_RX_ACK:
if (!rxDoneFlag)
break; /* wait until data is received */
/* Disable all interrupt driven functions */
writew(0xFF, REG_SIM_MASKIT);
/* error received */
if (sim_rx_character_count == 2) {
puts("SIM: command failed\n");
msgb_pull(msg, msg->len - 2);
msg->data[0] = response[0];
msg->data[1] = response[1];
goto queue;
}
/* wrong number of bytes received */
if (sim_rx_character_count != 1) {
puts("SIM: ACK read failed\n");
goto error;
}
if (response[0] != sim_data[1]) {
puts("SIM: ACK does not match request\n");
goto error;
}
sim_state = SIM_STATE_TX_DATA;
calypso_sim_transmit(sim_data + 5, length);
break;
case SIM_STATE_TX_DATA:
if (!txDoneFlag)
break; /* wait until data is transmitted */
/* Disable all interrupt driven functions */
writew(0xFF, REG_SIM_MASKIT);
/* Ignore waiting char for RUN GSM ALGORITHM */
/* TODO: implement proper handling of the "Procedure Bytes"
than this is no longer needed */
if(sim_data[1] == 0x88)
sim_ignore_waiting_char = 1;
sim_state = SIM_STATE_RX_STATUS;
calypso_sim_receive(response + length + 1, 2);
break;
case SIM_STATE_RX_ACK_DATA:
if (!rxDoneFlag)
break; /* wait until data is received */
/* Disable all interrupt driven functions */
writew(0xFF, REG_SIM_MASKIT);
/* error received */
if (sim_rx_character_count == 2) {
puts("SIM: command failed\n");
msgb_pull(msg, msg->len - 2);
msg->data[0] = response[0];
msg->data[1] = response[1];
goto queue;
}
/* wrong number of bytes received */
if (sim_rx_character_count != length + 1 + 2) {
puts("SIM: Failed to read data\n");
goto error;
}
msgb_pull(msg, 1); /* pull ACK byte */
goto queue;
}
return;
error:
msgb_pull(msg, msg->len - 2);
msg->data[0] = 0;
msg->data[1] = 0;
queue:
printf("SIM Response (%d): %s\n", msg->len,
osmo_hexdump(msg->data, msg->len));
l1h = (struct l1ctl_hdr *) msgb_push(msg, sizeof(*l1h));
l1h->msg_type = L1CTL_SIM_CONF;
l1h->flags = 0;
msg->l1h = (uint8_t *)l1h;
l1_queue_for_l2(msg);
/* go IDLE */
sim_state = SIM_STATE_IDLE;
sim_len = 0;
return;
}
