Add safe register functins, for thumb mode