Speedup print() on Teensy-LC

teensy3/Print.cpp

 	return vdprintf((int)this, (const char *)format, ap);
 }
 
+#ifdef __MKL26Z64__
+
+// optimized code inspired by Stimmer's optimization
+// obviously a dit different, adapted to 32 bit Cortex-M0+
+// http://forum.arduino.cc/index.php?topic=167414.msg1293679#msg1293679
+// http://forum.arduino.cc/index.php?topic=167414.msg1309482#msg1309482
+//  equivelant code:
+//    mod = div % 10;
+//    div = div / 10;
+//    tmp1 = {random};
+//    tmp2 = 10;
+#if 1
+#define divmod10_asm(div, mod, tmp1, tmp2, const3333) \
+asm (                              \
+  " lsr   %2, %0, #16"     "\n\t"  \
+  " mul   %2, %4"          "\n\t"  \
+  " uxth  %1, %0"          "\n\t"  \
+  " mul   %1, %4"          "\n\t"  \
+  " add   %1, #1"          "\n\t"  \
+  " lsr   %0, %2, #16"     "\n\t"  \
+  " lsl   %2, %2, #16"     "\n\t"  \
+  " add   %1, %2"          "\n\t"  \
+  " mov   %3, #0"          "\n\t"  \
+  " adc   %0, %3"          "\n\t"  \
+  " lsl   %0, %0, #15"     "\n\t"  \
+  " lsr   %2, %1, #17"     "\n\t"  \
+  " orr   %0, %2"          "\n\t"  \
+  " lsl   %1, %1, #15"     "\n\t"  \
+  " lsr   %2, %1, #16"     "\n\t"  \
+  " lsl   %3, %0, #16"     "\n\t"  \
+  " orr   %2, %3"          "\n\t"  \
+  " lsr   %3, %0, #16"     "\n\t"  \
+  " add   %1, %0"          "\n\t"  \
+  " adc   %0, %1"          "\n\t"  \
+  " sub   %0, %1"          "\n\t"  \
+  " add   %1, %2"          "\n\t"  \
+  " adc   %0, %3"          "\n\t"  \
+  " lsr   %1, %1, #4"      "\n\t"  \
+  " mov   %3, #10"         "\n\t"  \
+  " mul   %1, %3"          "\n\t"  \
+  " lsr   %1, %1, #28"     "\n\t"  \
+  : "+l" (div),                    \
+    "=&l" (mod),                   \
+    "=&l" (tmp1),                  \
+    "=&l" (tmp2)                   \
+  : "l" (const3333)                \
+  :                                \
+)
+#else
+#define divmod10_asm(_div, _mod, _tmp1, _tmp2, _const3333)    \
+  ({ _tmp1 = _div; _div = _div / 10; _mod = _tmp1 - _div * 10; })
+  // ({_mod = _div % 10, _div = _div / 10; })
+#endif
+
+size_t Print::printNumberDec(unsigned long n, uint8_t sign)
+{
+        uint8_t buf[11], *p;
+        uint32_t digit, t1, t2, c3333=0x3333;
+
+        p = buf + (sizeof(buf));
+        do {
+                divmod10_asm(n, digit, t1, t2, c3333);
+                *--p = digit + '0';
+        } while (n);
+        if (sign) *--p = '-';
+        return write(p, sizeof(buf) - (p - buf));
+}
+
+size_t Print::printNumberHex(unsigned long n)
+{
+        uint8_t digit, buf[8], *p;
+
+        p = buf + (sizeof(buf));
+        do {
+                digit = n & 15;
+                *--p = (digit < 10) ? '0' + digit : 'A' + digit - 10;
+                n >>= 4;
+        } while (n);
+        return write(p, sizeof(buf) - (p - buf));
+}
+
+size_t Print::printNumberBin(unsigned long n)
+{
+        uint8_t buf[32], *p;
+
+        p = buf + (sizeof(buf));
+        do {
+                *--p = '0' + ((uint8_t)n & 1);
+                n >>= 1;
+        } while (n);
+        return write(p, sizeof(buf) - (p - buf));
+}
+
+size_t Print::printNumberAny(unsigned long n, uint8_t base)
+{
+        uint8_t digit, buf[21], *p;
+        uint32_t tmp;
+
+        p = buf + sizeof(buf);
+        do {
+                tmp = n;
+                n = n / base;
+                digit = tmp - n * base;
+                *--p = (digit < 10) ? '0' + digit : 'A' + digit - 10;
+        } while (n);
+        return write(p, sizeof(buf) - (p - buf));
+}
+
+#else
 
 size_t Print::printNumber(unsigned long n, uint8_t base, uint8_t sign)
 {
 	return write(buf + i, sizeof(buf) - i);
 }
 
+#endif
 
 size_t Print::printFloat(double number, uint8_t digits) 
 {

teensy3/Print.h

   private:
 	char write_error;
 	size_t printFloat(double n, uint8_t digits);
+#ifdef __MKL26Z64__
+	size_t printNumberDec(unsigned long n, uint8_t sign);
+	size_t printNumberHex(unsigned long n);
+	size_t printNumberBin(unsigned long n);
+	size_t printNumberAny(unsigned long n, uint8_t base);
+	inline size_t printNumber(unsigned long n, uint8_t base, uint8_t sign) __attribute__((always_inline)) {
+		// when "base" is a constant (pretty much always), the
+		// compiler optimizes this to a single function call.
+		if (base == 0) return write((uint8_t)n);
+		if (base == 10 || base < 2) return printNumberDec(n, sign);
+		if (base == 16) return printNumberHex(n);
+		if (base == 2) return printNumberBin(n);
+		return printNumberAny(n, base);
+	}
+#else
 	size_t printNumber(unsigned long n, uint8_t base, uint8_t sign);
+#endif
 };