Mega2560 bootloader - various bugfixes

Patches to Mega2560 stk500v2 bootloader from Mark Sproul, to be tested.

See #181 #392 #505 #543 #544