Reduced the steps in the routine, increased performance by 20us, loop now runs
at ~10us.