Small refactoring: reduce code size (this will help to fit into ATMega32 FLASH)