Add bus controller testbench
Fix bus controller bugs revealed in testing. Tests look OK.