Complete rewrite of firmware code. Written to be much less general. This is a consequence of the limit of the logic elements on the CPLD.