Optimization on basis of gcov

Code blocks to more common first order. Also couple checks got to
be refactored next to section where update happens, that will
reduce unnecessary checking.

Signed-off-by: Sami Kerola <kerolasa@iki.fi>