Handle second serial port correctly

Firmware/Marlin_main.cpp

@@ -1014,8 +1014,9 @@ void setup()
 	lcd_splash();
     Sound_Init();                                // also guarantee "SET_OUTPUT(BEEPER)"
 
-	selectedSerialPort = eeprom_read_byte((uint8_t*)EEPROM_SECOND_SERIAL_ACTIVE);
+	selectedSerialPort = eeprom_read_byte((uint8_t *)EEPROM_SECOND_SERIAL_ACTIVE);
 	if (selectedSerialPort == 0xFF) selectedSerialPort = 0;
+	eeprom_update_byte((uint8_t *)EEPROM_SECOND_SERIAL_ACTIVE, selectedSerialPort);
 	MYSERIAL.begin(BAUDRATE);
 	fdev_setup_stream(uartout, uart_putchar, NULL, _FDEV_SETUP_WRITE); //setup uart out stream
 	stdout = uartout;
@@ -1070,7 +1071,7 @@ void setup()
 #ifndef W25X20CL
 	SERIAL_PROTOCOLLNPGM("start");
 #else
-	if (optiboot_status == 1)
+	if ((optiboot_status != 0) || (selectedSerialPort != 0))
 		SERIAL_PROTOCOLLNPGM("start");
 #endif
 	SERIAL_ECHO_START;

Firmware/optiboot_w25x20cl.cpp

@@ -122,12 +122,14 @@ uint8_t optiboot_w25x20cl_enter()
     unsigned long  boot_timer = 0;
     const char    *ptr = entry_magic_send;
     const char    *end = strlen_P(entry_magic_send) + ptr;
+    const uint8_t selectedSerialPort_bak = selectedSerialPort;
     // Flush the serial line.
     while (RECV_READY) {
       watchdogReset();
       // Dummy register read (discard)
       (void)(*(char *)UDR0);
     }
+    selectedSerialPort = 0; //switch to Serial0
     MYSERIAL.flush(); //clear RX buffer
     int SerialHead = rx_buffer.head;
     // Send the initial magic string.
@@ -143,14 +145,20 @@ uint8_t optiboot_w25x20cl_enter()
         watchdogReset();
         delayMicroseconds(1);
         if (++ boot_timer > boot_timeout)
+        {
           // Timeout expired, continue with the application.
+          selectedSerialPort = selectedSerialPort_bak; //revert Serial setting
           return 0;
+        }
       }
       ch = rx_buffer.buffer[SerialHead];
       SerialHead = (unsigned int)(SerialHead + 1) % RX_BUFFER_SIZE;
       if (pgm_read_byte(ptr ++) != ch)
+      {
           // Magic was not received correctly, continue with the application
+          selectedSerialPort = selectedSerialPort_bak; //revert Serial setting
           return 0;
+      }
       watchdogReset();
     }
     cbi(UCSR0B, RXCIE0); //disable the MarlinSerial0 interrupt