lib: sbi_hsm: Don't try to restore state on failed change

When a state change fails there's no need to restore the original
state as it remains the same.

Signed-off-by: Andrew Jones <ajones@ventanamicro.com>
Reviewed-by: Anup Patel <anup@brainfault.org>