Improved queue and buffering

Utilities.h

@@ -620,10 +620,10 @@ static inline size_t fifo16_pop_locked(FIFOBuffer16 *f) {
 
 inline void fifo16_init(FIFOBuffer16 *f, size_t *buffer, size_t size) {
   f->head = f->tail = f->begin = buffer;
-  f->end = buffer + (size/sizeof(size_t)) - 2;
+  f->end = buffer + (size/sizeof(size_t)) - 1;
 }
 
 inline size_t fifo16_len(FIFOBuffer16 *f) {
-  return ((f->end - f->begin))/sizeof(size_t);
+  return ((f->end - f->begin));
 }