Fix for Atmega 1281/2561.
Thanks to Matt for providing this patch!