1 files changed, 21 insertions, 47 deletions

diff --git a/hw/bt/hci-csr.c b/hw/bt/hci-csr.c
index d688372ca..2e970b656 100644
--- a/hw/bt/hci-csr.c
+++ b/hw/bt/hci-csr.c
@@ -22,7 +22,6 @@
 #include "qemu-common.h"
 #include "sysemu/char.h"
 #include "qemu/timer.h"
-#include "qemu/bswap.h"
 #include "hw/irq.h"
 #include "sysemu/bt.h"
 #include "hw/bt.h"
@@ -39,14 +38,9 @@ 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_needed;
+    int in_data;
     QEMUTimer *out_tm;
     int64_t baud_delay;
 
@@ -301,60 +295,38 @@ 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 total = 0;
+    int plen = s->in_len;
 
     if (!s->enable)
         return 0;
 
-    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;
-        }
-
-        if (s->in_len < s->in_needed) {
-            break;
-        }
+    s->in_len += len;
+    memcpy(s->inpkt + plen, buf, len);
 
-        if (s->in_state == CSR_HDR_LEN) {
+    while (1) {
+        if (s->in_len >= 2 && plen < 2)
             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_len >= s->in_hdr && plen < s->in_hdr)
+            s->in_data = csrhci_data_len(s->inpkt) + s->in_hdr;
 
-        if (s->in_state == CSR_DATA) {
+        if (s->in_len >= s->in_data) {
             csrhci_in_packet(s, s->inpkt);
-            csrhci_ready_for_next_inpkt(s);
-        }
+
+            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
+            break;
     }
 
-    return total;
+    return len;
 }
 
 static void csrhci_out_hci_packet_event(void *opaque,
@@ -416,9 +388,11 @@ static void csrhci_reset(struct csrhci_s *s)
 {
     s->out_len = 0;
     s->out_size = FIFO_LEN;
-    csrhci_ready_for_next_inpkt(s);
+    s->in_len = 0;
     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) */