From 0312c42f9d9d7df342547381bf3dd07097b3e4ca Mon Sep 17 00:00:00 2001
From: Chris Pepper <p3p@p3psoft.co.uk>
Date: Sun, 17 Jun 2018 02:59:22 +0100
Subject: [PATCH] [2.0.x] LPC176x Serial cleanup (#11032)

---
 Marlin/src/HAL/HAL_LPC1768/HardwareSerial.cpp |  15 +-
 Marlin/src/HAL/HAL_LPC1768/HardwareSerial.h   | 108 +---------
 Marlin/src/HAL/HAL_LPC1768/LPC1768_Servo.cpp  |   7 +-
 Marlin/src/HAL/HAL_LPC1768/SoftwareSPI.cpp    |   4 +-
 Marlin/src/HAL/HAL_LPC1768/SoftwareSerial.cpp |   6 +-
 Marlin/src/HAL/HAL_LPC1768/SoftwareSerial.h   |   6 +-
 .../include/digipot_mcp4451_I2C_routines.c    |   3 +
 Marlin/src/HAL/HAL_LPC1768/include/serial.h   | 196 +++++++-----------
 .../HAL/HAL_LPC1768/lpc1768_flag_script.py    |   2 +-
 Marlin/src/HAL/HAL_LPC1768/main.cpp           |  76 ++++---
 Marlin/src/pins/pins_MKS_SBASE.h              |   2 +
 frameworks/CMSIS/LPC1768/lib/Print.h          |   5 +-
 frameworks/CMSIS/LPC1768/lib/Stream.h         |   6 +-
 frameworks/CMSIS/LPC1768/lib/usb/cdcuser.cpp  |   2 +-
 .../LPC1768/{Re-ARM => system}/LPC1768.ld     |   0
 .../{Re-ARM => system}/startup_LPC17xx.S      |   0
 .../{Re-ARM => system}/system_LPC17xx.c       |   4 +-
 17 files changed, 152 insertions(+), 290 deletions(-)
 rename frameworks/CMSIS/LPC1768/{Re-ARM => system}/LPC1768.ld (100%)
 rename frameworks/CMSIS/LPC1768/{Re-ARM => system}/startup_LPC17xx.S (100%)
 rename frameworks/CMSIS/LPC1768/{Re-ARM => system}/system_LPC17xx.c (99%)

diff --git a/Marlin/src/HAL/HAL_LPC1768/HardwareSerial.cpp b/Marlin/src/HAL/HAL_LPC1768/HardwareSerial.cpp
index 98ea4f8fc60..db1171be337 100644
--- a/Marlin/src/HAL/HAL_LPC1768/HardwareSerial.cpp
+++ b/Marlin/src/HAL/HAL_LPC1768/HardwareSerial.cpp
@@ -123,8 +123,8 @@ void HardwareSerial::begin(uint32_t baudrate) {
   Baudrate = baudrate;
 }
 
-int HardwareSerial::peek() {
-  int byte = -1;
+int16_t HardwareSerial::peek() {
+  int16_t byte = -1;
 
   // Temporarily lock out UART receive interrupts during this read so the UART receive
   // interrupt won't cause problems with the index values
@@ -139,8 +139,8 @@ int HardwareSerial::peek() {
   return byte;
 }
 
-int HardwareSerial::read() {
-  int byte = -1;
+int16_t HardwareSerial::read() {
+  int16_t byte = -1;
 
   // Temporarily lock out UART receive interrupts during this read so the UART receive
   // interrupt won't cause problems with the index values
@@ -201,7 +201,7 @@ size_t HardwareSerial::write(uint8_t send) {
   }
 #endif
 
-int HardwareSerial::available() {
+size_t HardwareSerial::available() {
   return (RxQueueWritePos + RX_BUFFER_SIZE - RxQueueReadPos) % RX_BUFFER_SIZE;
 }
 
@@ -210,16 +210,17 @@ void HardwareSerial::flush() {
   RxQueueReadPos = 0;
 }
 
-void HardwareSerial::printf(const char *format, ...) {
+size_t HardwareSerial::printf(const char *format, ...) {
   char RxBuffer[256];
   va_list vArgs;
   va_start(vArgs, format);
   int length = vsnprintf(RxBuffer, 256, format, vArgs);
   va_end(vArgs);
   if (length > 0 && length < 256) {
-    for (int i = 0; i < length; ++i)
+    for (size_t i = 0; i < (size_t)length; ++i)
       write(RxBuffer[i]);
   }
+  return length;
 }
 
 void HardwareSerial::IRQHandler() {
diff --git a/Marlin/src/HAL/HAL_LPC1768/HardwareSerial.h b/Marlin/src/HAL/HAL_LPC1768/HardwareSerial.h
index 0f96652410b..1f0be8958fc 100644
--- a/Marlin/src/HAL/HAL_LPC1768/HardwareSerial.h
+++ b/Marlin/src/HAL/HAL_LPC1768/HardwareSerial.h
@@ -72,120 +72,20 @@ public:
   }
 
   void begin(uint32_t baudrate);
-  int peek();
-  int read();
+  int16_t peek();
+  int16_t read();
   size_t write(uint8_t send);
   #if TX_BUFFER_SIZE > 0
     void flushTX();
   #endif
-  int available();
+  size_t available();
   void flush();
-  void printf(const char *format, ...);
+  size_t printf(const char *format, ...);
 
   operator bool() { return true; }
 
   void IRQHandler();
 
-  #define DEC 10
-  #define HEX 16
-  #define OCT 8
-  #define BIN 2
-
-  void print_bin(uint32_t value, uint8_t num_digits) {
-    uint32_t mask = 1 << (num_digits -1);
-    for (uint8_t i = 0; i < num_digits; i++) {
-      if (!(i % 4) && i)    printf(" ");
-      if (!(i % 16)  && i)  printf(" ");
-      if (value & mask)     printf("1");
-      else                  printf("0");
-      value <<= 1;
-    }
-  }
-
-  void print(const char value[]) {
-    printf("%s" , value);
-  }
-  void print(char value, int nbase = 0) {
-    if (nbase == BIN) print_bin(value,8);
-    else if (nbase == OCT) printf("%3o", value);
-    else if (nbase == HEX) printf("%2X", value);
-    else if (nbase == DEC ) printf("%d", value);
-    else printf("%c" , value);
-  }
-  void print(unsigned char value, int nbase = 0) {
-    if (nbase == BIN) print_bin(value,8);
-    else if (nbase == OCT) printf("%3o", value);
-    else if (nbase == HEX) printf("%2X", value);
-    else printf("%u" , value);
-  }
-  void print(int value, int nbase = 0) {
-    if (nbase == BIN) print_bin(value,16);
-    else if (nbase == OCT) printf("%6o", value);
-    else if (nbase == HEX) printf("%4X", value);
-    else printf("%d", value);
-  }
-  void print(unsigned int value, int nbase = 0) {
-    if (nbase == BIN) print_bin(value,16);
-    else if (nbase == OCT) printf("%6o", value);
-    else if (nbase == HEX) printf("%4X", value);
-    else printf("%u" , value);
-  }
-  void print(long value, int nbase = 0) {
-    if (nbase == BIN) print_bin(value,32);
-    else if (nbase == OCT) printf("%11o", value);
-    else if (nbase == HEX) printf("%8X", value);
-    else printf("%ld" , value);
-  }
-  void print(unsigned long value, int nbase = 0) {
-    if (nbase == BIN) print_bin(value,32);
-    else if (nbase == OCT) printf("%11o", value);
-    else if (nbase == HEX) printf("%8X", value);
-    else printf("%lu" , value);
-  }
-  void print(float value, int round = 6) {
-    printf("%f" , value);
-  }
-  void print(double value, int round = 6) {
-    printf("%f" , value );
-  }
-
-  void println(const char value[]) {
-    printf("%s\n" , value);
-  }
-  void println(char value, int nbase = 0) {
-    print(value, nbase);
-    println();
-  }
-  void println(unsigned char value, int nbase = 0) {
-    print(value, nbase);
-    println();
-  }
-  void println(int value, int nbase = 0) {
-    print(value, nbase);
-    println();
-  }
-  void println(unsigned int value, int nbase = 0) {
-    print(value, nbase);
-    println();
-  }
-  void println(long value, int nbase = 0) {
-    print(value, nbase);
-    println();
-  }
-  void println(unsigned long value, int nbase = 0) {
-    print(value, nbase);
-    println();
-  }
-  void println(float value, int round = 6) {
-    printf("%f\n" , value );
-  }
-  void println(double value, int round = 6) {
-    printf("%f\n" , value );
-  }
-  void println(void) {
-    print('\n');
-  }
-
 };
 
 #endif // MARLIN_SRC_HAL_HAL_SERIAL_H_
diff --git a/Marlin/src/HAL/HAL_LPC1768/LPC1768_Servo.cpp b/Marlin/src/HAL/HAL_LPC1768/LPC1768_Servo.cpp
index 283cd525471..4997c5e4bca 100644
--- a/Marlin/src/HAL/HAL_LPC1768/LPC1768_Servo.cpp
+++ b/Marlin/src/HAL/HAL_LPC1768/LPC1768_Servo.cpp
@@ -60,9 +60,11 @@
  * unless DEACTIVATE_SERVOS_AFTER_MOVE is enabled and a MOVE command was issued.
  */
 
+#ifdef TARGET_LPC1768
+
 #include "../../inc/MarlinConfig.h"
 
-#if HAS_SERVOS && defined(TARGET_LPC1768)
+#if HAS_SERVOS
 
   #include "LPC1768_PWM.h"
   #include "LPC1768_Servo.h"
@@ -157,4 +159,5 @@
     }
   }
 
-#endif // HAS_SERVOS && TARGET_LPC1768
+#endif // HAS_SERVOS
+#endif // TARGET_LPC1768
diff --git a/Marlin/src/HAL/HAL_LPC1768/SoftwareSPI.cpp b/Marlin/src/HAL/HAL_LPC1768/SoftwareSPI.cpp
index 87f7bf383eb..546a1d50f10 100644
--- a/Marlin/src/HAL/HAL_LPC1768/SoftwareSPI.cpp
+++ b/Marlin/src/HAL/HAL_LPC1768/SoftwareSPI.cpp
@@ -29,10 +29,10 @@
  * For TARGET_LPC1768
  */
 
-#include "../../inc/MarlinConfig.h"
-
 #ifdef TARGET_LPC1768
 
+#include "../../inc/MarlinConfig.h"
+
 // --------------------------------------------------------------------------
 // Software SPI
 // --------------------------------------------------------------------------
diff --git a/Marlin/src/HAL/HAL_LPC1768/SoftwareSerial.cpp b/Marlin/src/HAL/HAL_LPC1768/SoftwareSerial.cpp
index 9604d045ba1..60510261633 100644
--- a/Marlin/src/HAL/HAL_LPC1768/SoftwareSerial.cpp
+++ b/Marlin/src/HAL/HAL_LPC1768/SoftwareSerial.cpp
@@ -253,7 +253,7 @@ void SoftwareSerial::end() {
 
 
 // Read data from buffer
-int SoftwareSerial::read() {
+int16_t SoftwareSerial::read() {
   if (!isListening()) return -1;
 
   // Empty buffer?
@@ -265,7 +265,7 @@ int SoftwareSerial::read() {
   return d;
 }
 
-int SoftwareSerial::available() {
+size_t SoftwareSerial::available() {
   if (!isListening()) return 0;
 
   return (_receive_buffer_tail + _SS_MAX_RX_BUFF - _receive_buffer_head) % _SS_MAX_RX_BUFF;
@@ -314,7 +314,7 @@ void SoftwareSerial::flush() {
   sei();
 }
 
-int SoftwareSerial::peek() {
+int16_t SoftwareSerial::peek() {
   if (!isListening())
     return -1;
 
diff --git a/Marlin/src/HAL/HAL_LPC1768/SoftwareSerial.h b/Marlin/src/HAL/HAL_LPC1768/SoftwareSerial.h
index 93df76b53a1..f208e14b054 100644
--- a/Marlin/src/HAL/HAL_LPC1768/SoftwareSerial.h
+++ b/Marlin/src/HAL/HAL_LPC1768/SoftwareSerial.h
@@ -93,11 +93,11 @@ public:
   bool isListening() { return this == active_object; }
   bool stopListening();
   bool overflow() { bool ret = _buffer_overflow; if (ret) _buffer_overflow = false; return ret; }
-  int peek();
+  int16_t peek();
 
   virtual size_t write(uint8_t byte);
-  virtual int read();
-  virtual int available();
+  virtual int16_t read();
+  virtual size_t available();
   virtual void flush();
   operator bool() { return true; }
 
diff --git a/Marlin/src/HAL/HAL_LPC1768/include/digipot_mcp4451_I2C_routines.c b/Marlin/src/HAL/HAL_LPC1768/include/digipot_mcp4451_I2C_routines.c
index 3c2bb87c4cd..6c46c89b0c2 100644
--- a/Marlin/src/HAL/HAL_LPC1768/include/digipot_mcp4451_I2C_routines.c
+++ b/Marlin/src/HAL/HAL_LPC1768/include/digipot_mcp4451_I2C_routines.c
@@ -23,6 +23,8 @@
 // adapted from  I2C/master/master.c example
 //   https://www-users.cs.york.ac.uk/~pcc/MCP/HAPR-Course-web/CMSIS/examples/html/master_8c_source.html
 
+#ifdef TARGET_LPC1768
+
 #include "../../../inc/MarlinConfigPre.h"
 
 #if MB(MKS_SBASE)
@@ -135,3 +137,4 @@ uint8_t digipot_mcp4451_send_byte(uint8_t data) {
 #endif
 
 #endif // MB(MKS_SBASE)
+#endif // TARGET_LPC1768
diff --git a/Marlin/src/HAL/HAL_LPC1768/include/serial.h b/Marlin/src/HAL/HAL_LPC1768/include/serial.h
index fd5c58254c6..3fe8a8cb1dc 100644
--- a/Marlin/src/HAL/HAL_LPC1768/include/serial.h
+++ b/Marlin/src/HAL/HAL_LPC1768/include/serial.h
@@ -30,177 +30,131 @@
 
 #include <stdarg.h>
 #include <stdio.h>
+#include <Print.h>
 
 /**
  * Generic RingBuffer
  * T type of the buffer array
  * S size of the buffer (must be power of 2)
- *
- * todo: optimise
  */
+
 template <typename T, uint32_t S> class RingBuffer {
 public:
-  RingBuffer() { index_read = index_write = 0; }
-  uint32_t available() volatile { return buffer_mask & (index_write - index_read); }
-  uint32_t free() volatile      { return buffer_size - available(); }
-  bool empty() volatile         { return (buffer_mask & index_read) == (buffer_mask & index_write); }
-  bool full() volatile          { return index_read == buffer_mask & (index_write + 1); }
-  void clear() volatile         { index_read = index_write = 0; }
-  bool peek(T *value) volatile {
-    if (value == 0 || available() == 0)
-      return false;
-    *value = buffer[buffer_mask & index_read];
+  RingBuffer() {index_read = index_write = 0;}
+
+  uint32_t available() {return mask(index_write - index_read);}
+  uint32_t free() {return buffer_size - available();}
+  bool empty() {return index_read == index_write;}
+  bool full() {return next(index_write) == index_read;}
+  void clear() {index_read = index_write = 0;}
+
+  bool peek(T *const value) {
+    if (value == nullptr || empty()) return false;
+    *value = buffer[index_read];
     return true;
   }
-  int read() volatile {
-    if ((buffer_mask & index_read) == (buffer_mask & index_write)) return -1;
-    T val = buffer[buffer_mask & index_read];
-    ++index_read;
-    return val;
+
+  uint32_t read(T *const value) {
+    if (value == nullptr || empty()) return 0;
+    *value = buffer[index_read];
+    index_read = next(index_read);
+    return 1;
   }
-  bool write(T value) volatile {
-    uint32_t next_head = buffer_mask & (index_write + 1);
-    if (next_head != index_read) {
-      buffer[buffer_mask & index_write] = value;
-      index_write = next_head;
-      return true;
-    }
-    return false;
+
+  uint32_t write(T value) {
+    uint32_t next_head = next(index_write);
+    if (next_head == index_read) return 0;     // buffer full
+    buffer[index_write] = value;
+    index_write = next_head;
+    return 1;
   }
 
 private:
+  inline uint32_t mask(uint32_t val) {
+    return val & buffer_mask;
+  }
+
+  inline uint32_t next(uint32_t val) {
+    return mask(val + 1);
+  }
+
   static const uint32_t buffer_size = S;
   static const uint32_t buffer_mask = buffer_size - 1;
-  volatile T buffer[buffer_size];
+  T buffer[buffer_size];
   volatile uint32_t index_write;
   volatile uint32_t index_read;
 };
 
-class HalSerial {
+/**
+ *  Serial Interface Class
+ *  Data is injected directly into, and consumed from, the fifo buffers
+ */
+
+class HalSerial: public Print {
 public:
 
   #if ENABLED(EMERGENCY_PARSER)
     EmergencyParser::State emergency_state;
   #endif
 
-  HalSerial() { host_connected = false; }
+  HalSerial() : host_connected(false) { }
+  virtual ~HalSerial() { }
 
-  void begin(int32_t baud) {
-  }
+  operator bool() { return host_connected; }
 
-  int peek() {
+  void begin(int32_t baud) { }
+
+  int16_t peek() {
     uint8_t value;
     return receive_buffer.peek(&value) ? value : -1;
   }
 
-  int read() { return receive_buffer.read(); }
-
-  size_t write(char c) { return host_connected ? transmit_buffer.write((uint8_t)c) : 0; }
-
-  operator bool() { return host_connected; }
-
-  uint16_t available() {
-    return (uint16_t)receive_buffer.available();
+  int16_t read() {
+    uint8_t value;
+    return receive_buffer.read(&value) ? value : -1;
   }
 
-  void flush() { receive_buffer.clear(); }
+  size_t write(const uint8_t c) {
+    if (!host_connected) return 0;          // Do not fill buffer when host disconnected
+    while (transmit_buffer.write(c) == 0) { // Block until there is free room in buffer
+      if (!host_connected) return 0;        // Break infinite loop on host disconect
+    }
+    return 1;
+  }
 
-  uint8_t availableForWrite(void){
+  size_t available() {
+    return (size_t)receive_buffer.available();
+  }
+
+  void flush() {
+    receive_buffer.clear();
+  }
+
+  uint8_t availableForWrite(void) {
     return transmit_buffer.free() > 255 ? 255 : (uint8_t)transmit_buffer.free();
   }
 
-  void flushTX(void){
-    if (host_connected)
-      while (transmit_buffer.available()) { /* nada */ }
+  void flushTX(void) {
+    while (transmit_buffer.available() && host_connected) { /* nada */}
   }
 
-  void printf(const char *format, ...) {
+  size_t printf(const char *format, ...) {
     static char buffer[256];
     va_list vArgs;
     va_start(vArgs, format);
     int length = vsnprintf((char *) buffer, 256, (char const *) format, vArgs);
     va_end(vArgs);
+    size_t i = 0;
     if (length > 0 && length < 256) {
-      if (host_connected) {
-        for (int i = 0; i < length;) {
-          if (transmit_buffer.write(buffer[i])) {
-            ++i;
-          }
-        }
+      while (i < (size_t)length && host_connected) {
+        i += transmit_buffer.write(buffer[i]);
       }
     }
+    return i;
   }
 
-  #define DEC 10
-  #define HEX 16
-  #define OCT 8
-  #define BIN 2
-
-  void print_bin(uint32_t value, uint8_t num_digits) {
-    uint32_t mask = 1 << (num_digits -1);
-    for (uint8_t i = 0; i < num_digits; i++) {
-      if (!(i % 4) && i)    write(' ');
-      if (!(i % 16)  && i)  write(' ');
-      if (value & mask)     write('1');
-      else                  write('0');
-      value <<= 1;
-    }
-  }
-
-  void print(const char value[]) { printf("%s" , value); }
-  void print(char value, int nbase = 0) {
-    if (nbase == BIN) print_bin(value, 8);
-    else if (nbase == OCT) printf("%3o", value);
-    else if (nbase == HEX) printf("%2X", value);
-    else if (nbase == DEC ) printf("%d", value);
-    else printf("%c" , value);
-  }
-  void print(unsigned char value, int nbase = 0) {
-    if (nbase == BIN) print_bin(value, 8);
-    else if (nbase == OCT) printf("%3o", value);
-    else if (nbase == HEX) printf("%2X", value);
-    else printf("%u" , value);
-  }
-  void print(int value, int nbase = 0) {
-    if (nbase == BIN) print_bin(value, 16);
-    else if (nbase == OCT) printf("%6o", value);
-    else if (nbase == HEX) printf("%4X", value);
-    else printf("%d", value);
-  }
-  void print(unsigned int value, int nbase = 0) {
-    if (nbase == BIN) print_bin(value, 16);
-    else if (nbase == OCT) printf("%6o", value);
-    else if (nbase == HEX) printf("%4X", value);
-    else printf("%u" , value);
-  }
-  void print(long value, int nbase = 0) {
-    if (nbase == BIN) print_bin(value, 32);
-    else if (nbase == OCT) printf("%11o", value);
-    else if (nbase == HEX) printf("%8X", value);
-    else printf("%ld" , value);
-  }
-  void print(unsigned long value, int nbase = 0) {
-    if (nbase == BIN) print_bin(value, 32);
-    else if (nbase == OCT) printf("%11o", value);
-    else if (nbase == HEX) printf("%8X", value);
-    else printf("%lu" , value);
-  }
-  void print(float value, int round = 6)  { printf("%f" , value); }
-  void print(double value, int round = 6) { printf("%f" , value); }
-
-  void println(const char value[]) { printf("%s\n" , value); }
-  void println(char value, int nbase = 0) { print(value, nbase); println(); }
-  void println(unsigned char value, int nbase = 0) { print(value, nbase); println(); }
-  void println(int value, int nbase = 0) { print(value, nbase); println(); }
-  void println(unsigned int value, int nbase = 0) { print(value, nbase); println(); }
-  void println(long value, int nbase = 0) { print(value, nbase); println(); }
-  void println(unsigned long value, int nbase = 0) { print(value, nbase); println(); }
-  void println(float value, int round = 6) { printf("%f\n" , value); }
-  void println(double value, int round = 6) { printf("%f\n" , value); }
-  void println(void) { print('\n'); }
-
-  volatile RingBuffer<uint8_t, 128> receive_buffer;
-  volatile RingBuffer<uint8_t, 128> transmit_buffer;
+  RingBuffer<uint8_t, 128> receive_buffer;
+  RingBuffer<uint8_t, 128> transmit_buffer;
   volatile bool host_connected;
 };
 
diff --git a/Marlin/src/HAL/HAL_LPC1768/lpc1768_flag_script.py b/Marlin/src/HAL/HAL_LPC1768/lpc1768_flag_script.py
index 23f214ba633..44ded586fe2 100644
--- a/Marlin/src/HAL/HAL_LPC1768/lpc1768_flag_script.py
+++ b/Marlin/src/HAL/HAL_LPC1768/lpc1768_flag_script.py
@@ -49,7 +49,7 @@ else:
           "-fno-threadsafe-statics"
       ],
       LINKFLAGS=[
-          "-Wl,-Tframeworks/CMSIS/LPC1768/Re-ARM/LPC1768.ld,--gc-sections",
+          "-Wl,-Tframeworks/CMSIS/LPC1768/system/LPC1768.ld,--gc-sections",
           "-Os",
           "-mcpu=cortex-m3",
           "-mthumb",
diff --git a/Marlin/src/HAL/HAL_LPC1768/main.cpp b/Marlin/src/HAL/HAL_LPC1768/main.cpp
index 1d4dde091e2..5f4e672f385 100644
--- a/Marlin/src/HAL/HAL_LPC1768/main.cpp
+++ b/Marlin/src/HAL/HAL_LPC1768/main.cpp
@@ -35,53 +35,52 @@ extern "C" {
 #include "LPC1768_PWM.h"
 
 static __INLINE uint32_t SysTick_Config(uint32_t ticks) {
-  if (ticks > SysTick_LOAD_RELOAD_Msk)
-    return (1); /* Reload value impossible */
+  if (ticks > SysTick_LOAD_RELOAD_Msk) return 1;
 
-  SysTick->LOAD = (ticks & SysTick_LOAD_RELOAD_Msk) - 1; /* set reload register */
-  NVIC_SetPriority(SysTick_IRQn, NVIC_EncodePriority(0, 0, 0)); /* set Priority for Cortex-M3 System Interrupts */
-  SysTick->VAL = 0; /* Load the SysTick Counter Value */
+  SysTick->LOAD = (ticks & SysTick_LOAD_RELOAD_Msk) - 1;        // Set reload register
+  SysTick->VAL  = 0;                                            // Load the SysTick Counter Value
   SysTick->CTRL = SysTick_CTRL_CLKSOURCE_Msk |
-  SysTick_CTRL_TICKINT_Msk |
-  SysTick_CTRL_ENABLE_Msk; /* Enable SysTick IRQ and SysTick Timer */
-  return (0); /* Function successful */
+                  SysTick_CTRL_TICKINT_Msk   |
+                  SysTick_CTRL_ENABLE_Msk;                      // Enable SysTick IRQ and SysTick Timer
+
+  NVIC_SetPriority(SysTick_IRQn, NVIC_EncodePriority(0, 0, 0)); // Set Priority for Cortex-M3 System Interrupts
+  return 0;
 }
 
 extern "C" {
-extern void disk_timerproc(void);
-volatile uint32_t _millis;
-void SysTick_Handler(void) {
-  ++_millis;
-  disk_timerproc(); /* Disk timer process */
-}
-}
+  extern int isLPC1769();
+  extern void disk_timerproc(void);
+  volatile uint32_t _millis;
 
-// runs after clock init and before global static constructors
-extern "C" void SystemPostInit() {
-  _millis = 0;     // initialise the millisecond counter value;
-  SysTick_Config(SystemCoreClock / 1000);     // start millisecond global counter
-  GPIO_SetDir(4, 1UL << 28, 1);
-
-  for (int i = 0; i < 4; ++i) {
-    GPIO_SetValue(4, 1UL << 28);
-    delay(100);
-    GPIO_ClearValue(4, 1UL << 28);
-    delay(100);
+  void SysTick_Handler(void) {
+    ++_millis;
+    disk_timerproc();
   }
-}
 
-// detect 17x[4-8] (100MHz) or 17x9 (120MHz)
-static bool isLPC1769() {
-    #define IAP_LOCATION 0x1FFF1FF1
-    uint32_t command[1];
-    uint32_t result[5];
-    typedef void (*IAP)(uint32_t*, uint32_t*);
-    IAP iap = (IAP) IAP_LOCATION;
+  // Runs after clock init and before global static constructors
+  void SystemPostInit() {
+    _millis = 0;                            // Initialise the millisecond counter value;
+    SysTick_Config(SystemCoreClock / 1000); // Start millisecond global counter
 
-    command[0] = 54;
-    iap(command, result);
+    // Runs before setup() need to configure LED_PIN and use to indicate succsessful bootloader execution
+    #if PIN_EXISTS(LED)
+      SET_DIR_OUTPUT(LED_PIN);
+      WRITE_PIN_CLR(LED_PIN);
 
-    return ((result[1] & 0x00100000) != 0);
+      //MKS-SBASE has 3 other LEDS the bootloader uses during flashing, clear them
+      SET_DIR_OUTPUT(P1_19);
+      WRITE_PIN_CLR(P1_19);
+      SET_DIR_OUTPUT(P1_20);
+      WRITE_PIN_CLR(P1_20);
+      SET_DIR_OUTPUT(P1_21);
+      WRITE_PIN_CLR(P1_21);
+
+      for (int i = 0; i < 6; ++i) {
+        TOGGLE(LED_PIN);
+        delay(100);
+      }
+    #endif
+  }
 }
 
 extern uint32_t MSC_SD_Init(uint8_t pdrv);
@@ -96,7 +95,6 @@ int main(void) {
   const uint32_t usb_timeout = millis() + 2000;
   while (!USB_Configuration && PENDING(millis(), usb_timeout)) {
     delay(50);
-
     #if PIN_EXISTS(LED)
       TOGGLE(LED_PIN);     // Flash fast while USB initialisation completes
     #endif
@@ -107,7 +105,7 @@ int main(void) {
     #if NUM_SERIAL > 1
       MYSERIAL1.begin(BAUDRATE);
     #endif
-    SERIAL_PRINTF("\n\n%s (%dMhz) UART0 Initialised\n", isLPC1769() ? "LPC1769" : "LPC1768", SystemCoreClock / 1000000);
+    SERIAL_PRINTF("\n\necho:%s (%dMhz) Initialised\n", isLPC1769() ? "LPC1769" : "LPC1768", SystemCoreClock / 1000000);
     SERIAL_FLUSHTX();
   #endif
 
diff --git a/Marlin/src/pins/pins_MKS_SBASE.h b/Marlin/src/pins/pins_MKS_SBASE.h
index bb6c1cea387..7537d198f2a 100644
--- a/Marlin/src/pins/pins_MKS_SBASE.h
+++ b/Marlin/src/pins/pins_MKS_SBASE.h
@@ -40,6 +40,8 @@
 #define PIN_P0_28          P0_28
 */
 
+#define LED_PIN           P1_18 // LED2 P1_19, LED3 P1_20, LED4 P1_21
+
 //
 // Servo pin
 //
diff --git a/frameworks/CMSIS/LPC1768/lib/Print.h b/frameworks/CMSIS/LPC1768/lib/Print.h
index 501cce612fa..283ed6d472d 100644
--- a/frameworks/CMSIS/LPC1768/lib/Print.h
+++ b/frameworks/CMSIS/LPC1768/lib/Print.h
@@ -40,7 +40,8 @@ class Print {
     void setWriteError(const int err = 1) { write_error = err; }
   public:
     Print() : write_error(0) {}
-
+    virtual ~Print() {}
+    
     int getWriteError() { return write_error; }
     void clearWriteError() { setWriteError(0); }
 
@@ -74,7 +75,7 @@ class Print {
     size_t println(double, int = 2);
     size_t println(const Printable&);
     size_t println(void);
-    size_t printf(const char *argList, ...);
+    virtual size_t printf(const char *argList, ...);
 };
 
 #endif // _CMSIS_PRINT_H_
diff --git a/frameworks/CMSIS/LPC1768/lib/Stream.h b/frameworks/CMSIS/LPC1768/lib/Stream.h
index a0be7a9e605..680f584da84 100644
--- a/frameworks/CMSIS/LPC1768/lib/Stream.h
+++ b/frameworks/CMSIS/LPC1768/lib/Stream.h
@@ -47,9 +47,9 @@ class Stream : public Print
     int peekNextDigit(); // returns the next numeric digit in the stream or -1 if timeout
 
   public:
-    virtual int available() = 0;
-    virtual int read() = 0;
-    virtual int peek() = 0;
+    virtual size_t available() = 0;
+    virtual int16_t read() = 0; // signed int required for error (empty buffer) value
+    virtual int16_t peek() = 0;
     virtual void flush() = 0;
 
     Stream() {_timeout=1000;}
diff --git a/frameworks/CMSIS/LPC1768/lib/usb/cdcuser.cpp b/frameworks/CMSIS/LPC1768/lib/usb/cdcuser.cpp
index 8fd08f00eb5..4f6f0a337a4 100644
--- a/frameworks/CMSIS/LPC1768/lib/usb/cdcuser.cpp
+++ b/frameworks/CMSIS/LPC1768/lib/usb/cdcuser.cpp
@@ -211,7 +211,7 @@ void CDC_BulkIn(void) {
   if (numBytesAvail > 0) {
     numBytesAvail = numBytesAvail > (USB_CDC_BUFSIZE - 1) ? (USB_CDC_BUFSIZE - 1) : numBytesAvail;
     for(uint32_t i = 0; i < numBytesAvail; ++i) {
-      BulkBufIn[i] = usb_serial.transmit_buffer.read(); //todo: optimise
+      usb_serial.transmit_buffer.read(&BulkBufIn[i]);
     }
     USB_WriteEP(CDC_DEP_IN, &BulkBufIn[0], numBytesAvail);
   } else {
diff --git a/frameworks/CMSIS/LPC1768/Re-ARM/LPC1768.ld b/frameworks/CMSIS/LPC1768/system/LPC1768.ld
similarity index 100%
rename from frameworks/CMSIS/LPC1768/Re-ARM/LPC1768.ld
rename to frameworks/CMSIS/LPC1768/system/LPC1768.ld
diff --git a/frameworks/CMSIS/LPC1768/Re-ARM/startup_LPC17xx.S b/frameworks/CMSIS/LPC1768/system/startup_LPC17xx.S
similarity index 100%
rename from frameworks/CMSIS/LPC1768/Re-ARM/startup_LPC17xx.S
rename to frameworks/CMSIS/LPC1768/system/startup_LPC17xx.S
diff --git a/frameworks/CMSIS/LPC1768/Re-ARM/system_LPC17xx.c b/frameworks/CMSIS/LPC1768/system/system_LPC17xx.c
similarity index 99%
rename from frameworks/CMSIS/LPC1768/Re-ARM/system_LPC17xx.c
rename to frameworks/CMSIS/LPC1768/system/system_LPC17xx.c
index a4ab913a426..dd3163ed080 100644
--- a/frameworks/CMSIS/LPC1768/Re-ARM/system_LPC17xx.c
+++ b/frameworks/CMSIS/LPC1768/system/system_LPC17xx.c
@@ -499,7 +499,7 @@ void SystemCoreClockUpdate (void)            /* Get Core Clock Frequency      */
 }
 
 // detect 17x[4-8] (100MHz) or 17x9 (120MHz)
-static int can_120MHz() {
+int isLPC1769() {
   #define IAP_LOCATION 0x1FFF1FF1
   uint32_t command[1];
   uint32_t result[5];
@@ -558,7 +558,7 @@ void SystemInit (void)
 
   LPC_SC->CCLKCFG   = 0x00000002;       /* Setup CPU Clock Divider            */
 
-  if(can_120MHz()) {
+  if(isLPC1769()) {
     LPC_SC->PLL0CFG   = 0x0000000E;     /* configure PLL0                     */
     LPC_SC->PLL0FEED  = 0xAA;
     LPC_SC->PLL0FEED  = 0x55;