Optimized by Martin Storsjö <martin@martin.st>.

The speedup vs C code is around 3.2-4.4x.
Signed-off-by: Martin Storsjö <martin@martin.st>