firmware: code clean up, consolidated wait loops to save 4 bytes