firmware/src/ : Fix bugs