Code cleanup & abililty to use Serial/Serial1/Serial2 for firmware update