If you intend to publish this software later you need to improve the code commenting. Having a comment per line is fine but on its own is not enough. At the head of each subroutine you should have a section describing what it does, what input and output registers it uses and any other registers it destroys.

You get off to a good start at the head of the code but then forget to say what the code as a whole is doing.

Ian