Firmware: adapted to v3.1 rev C hardware