firmware: removed debugging output, clean up logic