improve arithmetic in C firmware
optimize assembly a bit