looks fine to me...

you hame some unnecessary register moves when adding, but it should still function.

In your main program, do you look at R2 or A ???