Fix bugs for new firmware