Code clean up and handling FIFO errors

Revision:
69:4305ac772a66
Parent:
31:8ab5d9a29a9f