Refactor and simplify FIFO fill loop