Remember the serial characters during Optiboot initialization

Firmware/Marlin_main.cpp

@@ -1014,6 +1014,12 @@ void setup()
 	lcd_splash();
     Sound_Init();                                // also guarantee "SET_OUTPUT(BEEPER)"
 
+	selectedSerialPort = eeprom_read_byte((uint8_t*)EEPROM_SECOND_SERIAL_ACTIVE);
+	if (selectedSerialPort == 0xFF) selectedSerialPort = 0;
+	MYSERIAL.begin(BAUDRATE);
+	fdev_setup_stream(uartout, uart_putchar, NULL, _FDEV_SETUP_WRITE); //setup uart out stream
+	stdout = uartout;
+
 #ifdef W25X20CL
     bool w25x20cl_success = w25x20cl_init();
 	uint8_t optiboot_status = 1;
@@ -1041,15 +1047,13 @@ void setup()
 	if ((farm_mode == 0xFF && farm_no == 0) || ((uint16_t)farm_no == 0xFFFF)) 
 		farm_mode = false; //if farm_mode has not been stored to eeprom yet and farm number is set to zero or EEPROM is fresh, deactivate farm mode
 	if ((uint16_t)farm_no == 0xFFFF) farm_no = 0;
-	
-	selectedSerialPort = eeprom_read_byte((uint8_t*)EEPROM_SECOND_SERIAL_ACTIVE);
-	if (selectedSerialPort == 0xFF) selectedSerialPort = 0;
 	if (farm_mode)
 	{
 		no_response = true; //we need confirmation by recieving PRUSA thx
 		important_status = 8;
 		prusa_statistics(8);
 		selectedSerialPort = 1;
+		MYSERIAL.begin(BAUDRATE);
 #ifdef TMC2130
 		//increased extruder current (PFW363)
 		tmc2130_current_h[E_AXIS] = 36;
@@ -1063,15 +1067,12 @@ void setup()
           if(!(eeprom_read_byte((uint8_t*)EEPROM_FAN_CHECK_ENABLED)))
                eeprom_update_byte((unsigned char *)EEPROM_FAN_CHECK_ENABLED,true);
 	}
-	MYSERIAL.begin(BAUDRATE);
-	fdev_setup_stream(uartout, uart_putchar, NULL, _FDEV_SETUP_WRITE); //setup uart out stream
 #ifndef W25X20CL
 	SERIAL_PROTOCOLLNPGM("start");
 #else
 	if (optiboot_status == 1)
 		SERIAL_PROTOCOLLNPGM("start");
 #endif
-	stdout = uartout;
 	SERIAL_ECHO_START;
 	printf_P(PSTR(" " FW_VERSION_FULL "\n"));
 

Firmware/optiboot_w25x20cl.cpp

@@ -122,38 +122,38 @@ 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;
-    // Initialize the serial line.
-    UCSR0A |= (1 << U2X0);
-    UBRR0L = (((float)(F_CPU))/(((float)(115200))*8.0)-1.0+0.5);
-    UCSR0B = (1 << RXEN0) | (1 << TXEN0);
     // Flush the serial line.
     while (RECV_READY) {
       watchdogReset();
       // Dummy register read (discard)
       (void)(*(char *)UDR0);
     }
+    MYSERIAL.flush(); //clear RX buffer
+    int SerialHead = rx_buffer.head;
     // Send the initial magic string.
     while (ptr != end)
       putch(pgm_read_byte(ptr ++));
     watchdogReset();
-    // Wait for one second until a magic string (constant entry_magic) is received
+    // Wait for two seconds until a magic string (constant entry_magic) is received
     // from the serial line.
     ptr = entry_magic_receive;
     end = strlen_P(entry_magic_receive) + ptr;
     while (ptr != end) {
-      while (! RECV_READY) {
+      while (rx_buffer.head == SerialHead) {
         watchdogReset();
         delayMicroseconds(1);
         if (++ boot_timer > boot_timeout)
           // Timeout expired, continue with the application.
           return 0;
       }
-      ch = UDR0;
+      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
           return 0;
       watchdogReset();
     }
+    cbi(UCSR0B, RXCIE0); //disable the MarlinSerial0 interrupt
     // Send the cfm magic string.
     ptr = entry_magic_cfm;
     while (ptr != end)