diff --git a/virtio-snd.c b/virtio-snd.c index 97cdd51..90b1dbf 100644 --- a/virtio-snd.c +++ b/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;