gcc 5.1.1 wrongly warns about uninitialized variable @r2 when compiling with "-Og". Refactor the code to avoid the warnings. Fixes: 564cabd4