diff --git a/hw/bt/hci-csr.c b/hw/bt/hci-csr.c index e6b8998253..d688372ca3 100644 --- a/hw/bt/hci-csr.c +++ b/hw/bt/hci-csr.c @@ -39,9 +39,14 @@ struct csrhci_s { int out_size; uint8_t outfifo[FIFO_LEN * 2]; uint8_t inpkt[FIFO_LEN]; + enum { + CSR_HDR_LEN, + CSR_DATA_LEN, + CSR_DATA + } in_state; int in_len; int in_hdr; - int in_data; + int in_needed; QEMUTimer *out_tm; int64_t baud_delay; @@ -296,38 +301,60 @@ static int csrhci_data_len(const uint8_t *pkt) exit(-1); } +static void csrhci_ready_for_next_inpkt(struct csrhci_s *s) +{ + s->in_state = CSR_HDR_LEN; + s->in_len = 0; + s->in_needed = 2; + s->in_hdr = INT_MAX; +} + static int csrhci_write(struct CharDriverState *chr, const uint8_t *buf, int len) { struct csrhci_s *s = (struct csrhci_s *) chr->opaque; - int plen = s->in_len; + int total = 0; if (!s->enable) return 0; - s->in_len += len; - memcpy(s->inpkt + plen, buf, len); + for (;;) { + int cnt = MIN(len, s->in_needed - s->in_len); + if (cnt) { + memcpy(s->inpkt + s->in_len, buf, cnt); + s->in_len += cnt; + buf += cnt; + len -= cnt; + total += cnt; + } - while (1) { - if (s->in_len >= 2 && plen < 2) - s->in_hdr = csrhci_header_len(s->inpkt) + 1; - - if (s->in_len >= s->in_hdr && plen < s->in_hdr) - s->in_data = csrhci_data_len(s->inpkt) + s->in_hdr; - - if (s->in_len >= s->in_data) { - csrhci_in_packet(s, s->inpkt); - - memmove(s->inpkt, s->inpkt + s->in_len, s->in_len - s->in_data); - s->in_len -= s->in_data; - s->in_hdr = INT_MAX; - s->in_data = INT_MAX; - plen = 0; - } else + if (s->in_len < s->in_needed) { break; + } + + if (s->in_state == CSR_HDR_LEN) { + s->in_hdr = csrhci_header_len(s->inpkt) + 1; + assert(s->in_hdr >= s->in_needed); + s->in_needed = s->in_hdr; + s->in_state = CSR_DATA_LEN; + continue; + } + + if (s->in_state == CSR_DATA_LEN) { + s->in_needed += csrhci_data_len(s->inpkt); + /* hci_acl_hdr could specify more than 4096 bytes, so assert. */ + assert(s->in_needed <= sizeof(s->inpkt)); + s->in_state = CSR_DATA; + continue; + } + + if (s->in_state == CSR_DATA) { + csrhci_in_packet(s, s->inpkt); + csrhci_ready_for_next_inpkt(s); + } } - return len; + return total; } static void csrhci_out_hci_packet_event(void *opaque, @@ -389,11 +416,9 @@ static void csrhci_reset(struct csrhci_s *s) { s->out_len = 0; s->out_size = FIFO_LEN; - s->in_len = 0; + csrhci_ready_for_next_inpkt(s); s->baud_delay = NANOSECONDS_PER_SECOND; s->enable = 0; - s->in_hdr = INT_MAX; - s->in_data = INT_MAX; s->modem_state = 0; /* After a while... (but sooner than 10ms) */