support two serial lines at the same time

Firmware/MarlinSerial.cpp

@@ -67,17 +67,17 @@ FORCE_INLINE void store_char(unsigned char c)
       }
   }
 #ifndef SNMM
-  SIGNAL(USART2_RX_vect)
+  SIGNAL(USART1_RX_vect)
   {
       if (selectedSerialPort == 1) {
         // Test for a framing error.
-        if (UCSR2A & (1<<FE2)) {
+        if (UCSR1A & (1<<FE1)) {
             // Characters received with the framing errors will be ignored.
             // Dummy register read (discard)
-            (void)(*(char *)UDR2);
+            (void)(*(char *)UDR1);
         } else {
             // Read the input register.
-            unsigned char c = UDR2;
+            unsigned char c = UDR1;
             store_char(c);
             
         }
@@ -129,20 +129,20 @@ void MarlinSerial::begin(long baud)
 
   if (selectedSerialPort == 1) { //set up also the second serial port 
 	  if (useU2X) {
-		  UCSR2A = 1 << U2X2;
+		  UCSR1A = 1 << U2X1;
 		  baud_setting = (F_CPU / 4 / baud - 1) / 2;
 	  } else {
-		  UCSR2A = 0;
+		  UCSR1A = 0;
 		  baud_setting = (F_CPU / 8 / baud - 1) / 2;
 	  }
 
 	  // assign the baud_setting, a.k.a. ubbr (USART Baud Rate Register)
-	  UBRR2H = baud_setting >> 8;
-	  UBRR2L = baud_setting;
+	  UBRR1H = baud_setting >> 8;
+	  UBRR1L = baud_setting;
 	  
-	  sbi(UCSR2B, RXEN2);
-	  sbi(UCSR2B, TXEN2);
-	  sbi(UCSR2B, RXCIE2);	  
+	  sbi(UCSR1B, RXEN1);
+	  sbi(UCSR1B, TXEN1);
+	  sbi(UCSR1B, RXCIE1);	  
   }
 #endif
 }
@@ -154,9 +154,9 @@ void MarlinSerial::end()
   cbi(M_UCSRxB, M_RXCIEx);
 
 #ifndef SNMM
-  cbi(UCSR2B, RXEN2);
-  cbi(UCSR2B, TXEN2);
-  cbi(UCSR2B, RXCIE2);
+  cbi(UCSR1B, RXEN1);
+  cbi(UCSR1B, TXEN1);
+  cbi(UCSR1B, RXCIE1);
 #endif
 }
 

Firmware/MarlinSerial.h

@@ -101,7 +101,7 @@ class MarlinSerial //: public Stream
     {
       return (unsigned int)(RX_BUFFER_SIZE + rx_buffer.head - rx_buffer.tail) % RX_BUFFER_SIZE;
     }
-    
+    /*
     FORCE_INLINE void write(uint8_t c)
     {
       while (!((M_UCSRxA) & (1 << M_UDREx)))
@@ -109,7 +109,20 @@ class MarlinSerial //: public Stream
 
       M_UDRx = c;
     }
-    
+    */
+	void write(uint8_t c)
+		{
+		if (selectedSerialPort == 0) {
+			while (!((M_UCSRxA) & (1 << M_UDREx)))
+				;
+				M_UDRx = c;
+		}
+		else if (selectedSerialPort == 1) {
+			while (!((UCSR1A) & (1 << UDRE1)))
+				;
+				UDR1 = c;
+		}
+	}
     
     void checkRx(void)
     {
@@ -135,14 +148,14 @@ class MarlinSerial //: public Stream
                 }
             }
         } else if(selectedSerialPort == 1) {
-            if((UCSR2A & (1<<RXC2)) != 0) {
+            if((UCSR1A & (1<<RXC1)) != 0) {
                 // Test for a framing error.
-                if (UCSR2A & (1<<FE2)) {
+                if (UCSR1A & (1<<FE1)) {
                     // Characters received with the framing errors will be ignored.
                     // The temporary variable "c" was made volatile, so the compiler does not optimize this out.
-                    volatile unsigned char c = UDR2;
+                    volatile unsigned char c = UDR1;
                 } else {
-                    unsigned char c  =  UDR2;
+                    unsigned char c  =  UDR1;
                     int i = (unsigned int)(rx_buffer.head + 1) % RX_BUFFER_SIZE;
                     // if we should be storing the received character into the location
                     // just before the tail (meaning that the head would advance to the