- /* then get the rest (if any) from the beginning of the buffer */
- memcpy(buffer + l, fifo->buffer, len - l);
+ /*
+ * first get the data from fifo->out until the end of the buffer
+ */
+ l = min(len, fifo->size - (fifo->out & (fifo->size - 1)));
+ memcpy(buf, fifo->buffer + (fifo->out & (fifo->size - 1)), l);
+
+ /*
+ * then get the rest (if any) from the beginning of the buffer
+ */
+ memcpy(buf + l, fifo->buffer, len - l);
+ }