Restructure the code to handle mode-switching better
Implement ST:TOS mode