Add frame dequeue function

virtio-snd.c

@@ -578,6 +578,42 @@ static void virtio_snd_read_pcm_release(const virtio_snd_pcm_hdr_t *query,
 double omega = 0.0;
 int totalframesp = 0;
 int totalframesr = 0;
+static void __virtio_snd_frame_dequeue(void **out,
+                                       uint32_t n,
+                                       uint32_t stream_id)
+{
+    virtio_snd_prop_t *props = &vsnd_props[stream_id];
+    uint32_t cons_head, prod_tail;
+    uint32_t cons_next, entries;
+    uint32_t mask = props->pp.buffer_bytes;
+
+    cons_head = props->ring.cons.head;
+    prod_tail = props->ring.prod.tail;
+    /* The subtraction is done between two unsigned 32bits value
+     * (the result is always modulo 32 bits even if we have
+     * cons_head > prod_tail). So 'entries' is always between 0
+     * and size(ring)-1. */
+    entries = prod_tail - cons_head;
+
+    if (n > entries)
+        fprintf(stderr, "copy length is larget than entries\n");
+
+    cons_next = cons_head + n;
+    props->ring.cons.head = cons_next;
+
+    /* Copy the frame to output stream */
+    uint32_t size = props->pp.buffer_bytes;
+    uint32_t idx = cons_head & mask;
+    if (idx + n < size) {
+        memcpy(*out, props->ring.buffer + idx, n);
+    } else {
+        memcpy(*out, props->ring.buffer + idx, size - idx);
+        memcpy(*out + (size - idx), props->ring.buffer, n - (size - idx));
+    }
+    asm("" ::: "memory");
+
+    props->ring.cons.tail = cons_next;
+}
 static void virtio_snd_cb(struct CNFADriver *dev,
                           short *out,
                           short *in,
@@ -610,7 +646,7 @@ static void virtio_snd_cb(struct CNFADriver *dev,
     }*/
 
     /* TODO: add single consumer */
-    memcpy(out, vsnd_props[id].ring.buffer, out_buf_sz);
+    __virtio_snd_frame_dequeue(&out, out_buf_sz, id);
 #if 0
     for (int i = 0; i < framesp; i++) {
         // Shift phase, so we run at 440 Hz (A4)
@@ -726,9 +762,7 @@ static void __virtio_snd_frame_enqueue(void *payload,
 {
     uint32_t prod_head, cons_tail;
     uint32_t prod_next, free_entries;
-    virtio_snd_prop_t *props =
-        &vsnd_props[stream_id]; /* XXX: access the address for the ease of data
-                                  manipulation */
+    virtio_snd_prop_t *props = &vsnd_props[stream_id];
     uint32_t mask = props->pp.buffer_bytes;
 
     prod_head = props->ring.prod.head;
@@ -738,10 +772,16 @@ static void __virtio_snd_frame_enqueue(void *payload,
      * prod_head > cons_tail). So 'free_entries' is always between 0
      * and size(ring)-1. */
     free_entries = mask + cons_tail - prod_head;
+    /*fprintf(stderr,
+            "mask %" PRIu32 " cons_tail %" PRIu32 " prod_head %" PRIu32 "\n",
+            mask, cons_tail, prod_head);*/
 
     /* Move prod_head. */
     if (n > free_entries)
-        fprintf(stderr, "payload length %" PRIu32 " larger than free_entries %" PRIu32 "\n", n, free_entries);
+        fprintf(stderr,
+                "payload length %" PRIu32 " larger than free_entries %" PRIu32
+                "\n",
+                n, free_entries);
     prod_next = prod_head + n;
     props->ring.prod.head = prod_next;