Ported old statemachine code to use hsmpy

hsmpy synced to b042750