static int modem_pipe_read(struct m_pipe *pipe, struct modem_io *io)
{
unsigned data_size = io->size;
char hdr[M_PIPE_MAX_HDR];
int ret;
if (fifo_read(pipe->rx, hdr, pipe->header_size) == 0)
return -EAGAIN;
ret = pipe->pull_header(io, hdr);
if (ret)
return ret;
if (data_size < io->size) {
pr_info("modem_pipe_read: discarding packet (%d)\n", io->size);
if (fifo_skip(pipe->rx, io->size + 1) != (io->size + 1))
return -EIO;
return -EAGAIN;
} else {
if (fifo_read_user(pipe->rx, io->data, io->size) != io->size)
return -EIO;
if (fifo_skip(pipe->rx, 1) != 1)
return -EIO;
}
return 0;
}
static void handle_raw_rx(struct modemctl *mc)
{
struct raw_hdr raw;
struct sk_buff *skb = NULL;
int recvdata = 0;
/* process inbound packets */
while (fifo_read(&mc->raw_rx, &raw, sizeof(raw)) == sizeof(raw)) {
struct net_device *dev = mc->ndev;
unsigned sz = raw.len - (sizeof(raw) - 1);
if (unlikely(raw.channel != RAW_CH_VNET0)) {
MODEM_COUNT(mc, rx_unknown);
pr_err("[VNET] unknown channel %d\n", raw.channel);
if (fifo_skip(&mc->raw_rx, sz + 1) != (sz + 1))
goto purge_raw_fifo;
continue;
}
skb = dev_alloc_skb(sz + NET_IP_ALIGN);
if (skb == NULL) {
MODEM_COUNT(mc, rx_dropped);
/* TODO: consider timer + retry instead of drop? */
pr_err("[VNET] cannot alloc %d byte packet\n", sz);
if (fifo_skip(&mc->raw_rx, sz + 1) != (sz + 1))
goto purge_raw_fifo;
continue;
}
skb->dev = dev;
skb_reserve(skb, NET_IP_ALIGN);
if (fifo_read(&mc->raw_rx, skb_put(skb, sz), sz) != sz)
goto purge_raw_fifo;
if (fifo_skip(&mc->raw_rx, 1) != 1)
goto purge_raw_fifo;
skb->protocol = __constant_htons(ETH_P_IP);
dev->stats.rx_packets++;
dev->stats.rx_bytes += skb->len;
netif_rx(skb);
recvdata = 1;
MODEM_COUNT(mc, rx_received);
}
if (recvdata)
wake_lock_timeout(&mc->ip_rx_wakelock, HZ * 2);
return;
purge_raw_fifo:
if (skb)
dev_kfree_skb_irq(skb);
pr_err("[VNET] purging raw rx fifo!\n");
fifo_purge(&mc->raw_tx);
MODEM_COUNT(mc, rx_purged);
}
static void process_rx_fifo(void *arg) {
if(!parsed_header) {
// <sync byte (0xC0)><version (0x00)><length of ALP command (1 byte)><ALP command> // TODO CRC
if(fifo_get_size(&rx_fifo) > SERIAL_ALP_FRAME_HEADER_SIZE) {
uint8_t header[SERIAL_ALP_FRAME_HEADER_SIZE];
fifo_peek(&rx_fifo, header, 0, SERIAL_ALP_FRAME_HEADER_SIZE);
DPRINT_DATA(header, 3); // TODO tmp
if(header[0] != SERIAL_ALP_FRAME_SYNC_BYTE || header[1] != SERIAL_ALP_FRAME_VERSION) {
fifo_skip(&rx_fifo, 1);
DPRINT("skip");
parsed_header = false;
payload_len = 0;
if(fifo_get_size(&rx_fifo) > SERIAL_ALP_FRAME_HEADER_SIZE)
sched_post_task(&process_rx_fifo);
return;
}
parsed_header = true;
fifo_skip(&rx_fifo, SERIAL_ALP_FRAME_HEADER_SIZE);
payload_len = header[2];
DPRINT("found header, payload size = %i", payload_len);
sched_post_task(&process_rx_fifo);
}
} else {
if(fifo_get_size(&rx_fifo) < payload_len) {
DPRINT("payload not complete yet");
return;
}
// payload complete, start parsing
// rx_fifo can be bigger than the current serial packet, init a subview fifo
// which is restricted to payload_len so we can't parse past this packet.
fifo_t payload_fifo;
fifo_init_subview(&payload_fifo, &rx_fifo, 0, payload_len);
process_serial_frame(&payload_fifo);
// pop parsed bytes from original fifo
fifo_skip(&rx_fifo, payload_len - fifo_get_size(&payload_fifo));
parsed_header = false;
}
}
uint8_t alp_get_expected_response_length(uint8_t* alp_command, uint8_t alp_command_length) {
uint8_t expected_response_length = 0;
fifo_t fifo;
fifo_init_filled(&fifo, alp_command, alp_command_length, alp_command_length + 1);
while(fifo_get_size(&fifo) > 0) {
alp_control_t control;
fifo_pop(&fifo, (uint8_t*)&control.raw, 1);
switch(control.operation) {
case ALP_OP_READ_FILE_DATA:
fifo_skip(&fifo, 1); // skip file ID
alp_parse_length_operand(&fifo); // offset
expected_response_length += alp_parse_length_operand(&fifo);;
break;
case ALP_OP_REQUEST_TAG:
fifo_skip(&fifo, 1); // skip tag ID operand
break;
case ALP_OP_RETURN_FILE_DATA:
case ALP_OP_WRITE_FILE_DATA:
fifo_skip(&fifo, 1); // skip file ID
alp_parse_length_operand(&fifo); // offset
fifo_skip(&fifo, alp_parse_length_operand(&fifo));
break;
case ALP_OP_FORWARD: ;
uint8_t itf_id;
fifo_pop(&fifo, &itf_id, 1);
if(itf_id == ALP_ITF_ID_D7ASP) {
fifo_skip(&fifo, 1); // skip QoS, dormant timeout
d7ap_addressee_ctrl_t addressee_ctrl;
fifo_pop(&fifo, (uint8_t*)&addressee_ctrl.raw, 1);
fifo_skip(&fifo, 2 + alp_addressee_id_length(addressee_ctrl.id_type)); // skip addressee ctrl, access class
// TODO refactor to reuse same logic for parsing and response length counting
}
// other ITFs have no configuration
break;
case ALP_OP_WRITE_FILE_PROPERTIES:
fifo_skip(&fifo, 1 + sizeof(fs_file_header_t)); // skip file ID & header
break;
// TODO other operations
default:
DPRINT("op %i not implemented", control.operation);
assert(false);
}
}
DPRINT("Expected ALP response length=%i", expected_response_length);
return expected_response_length;
}