예제 #1
0
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;
}
예제 #2
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;
  }
}
예제 #4
0
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;
}