diff --git a/include/proto.h b/include/proto.h
index 2a3d451..b08ad74 100644
--- a/include/proto.h
+++ b/include/proto.h
@@ -93,6 +93,7 @@ struct __attribute__((__packed__)) pkt_adc_data {
     struct ll_pkt ll_pkt;
     uint64_t timestamp;
     uint32_t sampling_interval_us;
+    uint32_t total_samples;
     uint32_t sample_count;
     uint8_t samples[16][2][3];
 };
diff --git a/src/adc.c b/src/adc.c
index 09dd8b2..af71f27 100644
--- a/src/adc.c
+++ b/src/adc.c
@@ -175,8 +175,19 @@ void adc_dma_interrupt(DMA_Channel_TypeDef *channel, uint32_t flags) {
                 val |= 0xff000000;
             }
             st_adc.samples[wr_idx][1] = val;
-            st_adc.sample_rd_idx = wr_idx;
-            st_adc.sample_wr_idx = (wr_idx+1) % COUNT_OF(st_adc.samples);
+
+            wr_idx++;
+            if (wr_idx >= COUNT_OF(st_adc.samples)) {
+                wr_idx = 0;
+            }
+
+            if (wr_idx == st_adc.sample_rd_idx) {
+                st_adc.error = ERR_BUFFER_OVERFLOW;
+                st_adc.last_error = sys_time_us;
+            } else {
+                st_adc.sample_wr_idx = wr_idx;
+            }
+
             st_adc.sample_count++;
 
             if (val > ADC_FSR_POS || val < ADC_FSR_NEG) {
diff --git a/src/proto.c b/src/proto.c
index 48f7a20..7e1ce21 100644
--- a/src/proto.c
+++ b/src/proto.c
@@ -54,40 +54,35 @@ void uart_handle_user_packet(struct ll_pkt *pkt, size_t len) {
             struct pkt_adc_data *pkt_adc = (struct pkt_adc_data *)pkt;
             size_t wr_pos = st_proto.adc_sp_wr_idx[index];
             size_t rd_pos = st_proto.adc_sp_rd_idx[index];
-            size_t num_new = pkt_adc->sample_count - st_proto.adc_sample_count[index];
+            size_t num_new = pkt_adc->sample_count;
 
-            if (num_new > COUNT_OF(pkt_adc->samples)) {
-                st_proto.last_error = sys_time_us;
-                st_proto.error = ERR_BUFFER_OVERFLOW;
-                num_new = COUNT_OF(pkt_adc->samples);
-            }
-
-            for (ssize_t i=num_new-1; i>=0; i--) {
+            size_t nread = 0;
+            while (nread < num_new) {
                 for (size_t channel=0; channel<2; channel++) {
                     st_proto.adc_samples[index][channel][wr_pos] = 
-                        pkt_adc->samples[i][channel][2]    | 
-                        pkt_adc->samples[i][channel][1]<<8 |
-                        (((int32_t)(int8_t)pkt_adc->samples[i][channel][0])<<16);
+                        pkt_adc->samples[nread][channel][0]    | 
+                        (pkt_adc->samples[nread][channel][1]<<8) |
+                        (((int32_t)(int8_t)pkt_adc->samples[nread][channel][2])<<16);
                 }
 
-                size_t new_wr_pos = wr_pos+1;
-                if (new_wr_pos >= COUNT_OF(st_proto.adc_samples[0][0])) {
-                    new_wr_pos = 0;
+                nread++;
+                wr_pos++;
+                if (wr_pos >= COUNT_OF(st_proto.adc_samples[0][0])) {
+                    wr_pos = 0;
                 }
 
-                if (new_wr_pos == rd_pos) {
+                if (wr_pos == rd_pos) {
                     st_proto.last_error = sys_time_us;
                     st_proto.error = ERR_BUFFER_OVERFLOW;
                     break;
                 } else {
-                    wr_pos = new_wr_pos;
+                    st_proto.adc_sp_wr_idx[index] = wr_pos;
                 }
             }
 
-            st_proto.adc_sp_wr_idx[index] = wr_pos;
             st_proto.adc_sample_timestamp[index] = pkt_adc->timestamp;
             st_proto.adc_sampling_interval[index] = pkt_adc->sampling_interval_us;
-            st_proto.adc_sample_count[index] = pkt_adc->sample_count;
+            st_proto.adc_sample_count[index] = pkt_adc->total_samples;
 
             proto_handle_host(false);
 
@@ -184,13 +179,15 @@ void tx_adc_data() {
         },
         .timestamp = sync_time_us,
         .sampling_interval_us = st_adc.sampling_interval_us,
-        .sample_count = st_adc.sample_count,
+        .total_samples = st_adc.sample_count,
     };
 
     /* Format samples stored in native 32 bit ints into 24 bit fields */
     ssize_t rd_idx = st_adc.sample_rd_idx;
+    ssize_t wr_idx = st_adc.sample_wr_idx;
     uint8_t *p = (uint8_t *)pk.samples;
-    for (size_t i=0; i<COUNT_OF(pk.samples); i++) {
+    size_t nread = 0;
+    while (nread<COUNT_OF(pk.samples) && rd_idx != wr_idx) {
         uint32_t sp = st_adc.samples[rd_idx][0];
         *p++ = sp&0xff;
         *p++ = (sp>>8)&0xff;
@@ -201,12 +198,16 @@ void tx_adc_data() {
         *p++ = (sp>>8)&0xff;
         *p++ = (sp>>16)&0xff;
 
-        rd_idx -= 1;
-        if (rd_idx < 0) {
-            rd_idx = COUNT_OF(st_adc.samples);
+        nread++;
+        rd_idx++;
+        if (rd_idx == COUNT_OF(st_adc.samples)) {
+            rd_idx = 0;
         }
     }
 
+    st_adc.sample_rd_idx = rd_idx;
+    pk.sample_count = nread;
+
     ErrorCode rc = uart_send_packet_nonblocking(&pk.ll_pkt, sizeof(pk));
     if (rc) {
         st_proto.error = rc;
diff --git a/src/proto_usb.c b/src/proto_usb.c
index 50f02ad..40bfb0f 100644
--- a/src/proto_usb.c
+++ b/src/proto_usb.c
@@ -51,7 +51,10 @@ void handle_usb_packet(void *packet, size_t len) {
                         }
                     }
 
-                    st_proto.adc_sp_rd_idx[board] = rd_idx;
+                    if (rd_idx != wr_idx) {
+                        st_proto.adc_sp_rd_idx[board] = rd_idx;
+                    }
+
                     response.num_samples[board] = nread;
                 }
 
diff --git a/tools/usb_test.py b/tools/usb_test.py
index 32b1877..0ea06a2 100644
--- a/tools/usb_test.py
+++ b/tools/usb_test.py
@@ -150,7 +150,7 @@ def probe(port, timeout):
         time.sleep(0.01)
         packet = ser.command(PacketType.USBP_GET_MEASUREMENTS)
         for i in range(packet.num_samples[1]):
-            print(packet.measurements[1,1,i])
+            print(packet.measurements[1,1,i], packet.num_samples[1])
 
 if __name__ == '__main__':
     cli()