From a64a694f6d1427e28bc55b0aa90ffbf3bccecc69 Mon Sep 17 00:00:00 2001
From: WereCatf <werecatf@runbox.com>
Date: Sat, 16 Apr 2016 20:02:06 +0300
Subject: [PATCH] Speed up SPI.writePattern()

---
 libraries/SPI/SPI.cpp | 59 +++++++++++++++++++++++--------------------
 libraries/SPI/SPI.h   |  1 -
 2 files changed, 32 insertions(+), 28 deletions(-)

diff --git a/libraries/SPI/SPI.cpp b/libraries/SPI/SPI.cpp
index e9c6cf35d..97457ef01 100644
--- a/libraries/SPI/SPI.cpp
+++ b/libraries/SPI/SPI.cpp
@@ -350,11 +350,7 @@ void SPIClass::writeBytes_(uint8_t * data, uint8_t size) {
     while(SPI1CMD & SPIBUSY) {}
 }
 
-
 /**
- * Note:
- *  data need to be aligned to 32Bit
- *  or you get an Fatal exception (9)
  * @param data uint8_t *
  * @param size uint8_t  max for size is 64Byte
  * @param repeat uint32_t
@@ -362,37 +358,46 @@ void SPIClass::writeBytes_(uint8_t * data, uint8_t size) {
 void SPIClass::writePattern(uint8_t * data, uint8_t size, uint32_t repeat) {
     if(size > 64) return; //max Hardware FIFO
 
-    uint32_t byte = (size * repeat);
-    uint8_t r = (64 / size);
+    while(SPI1CMD & SPIBUSY) {}
 
-    while(byte) {
-        if(byte > 64) {
-            writePattern_(data, size, r);
-            byte -= 64;
-        } else {
-            writePattern_(data, size, (byte / size));
-            byte = 0;
-        }
-    }
-}
+    uint32_t buffer[16];
+    uint8_t *bufferPtr=(uint8_t *)&buffer;
+    uint8_t *dataPtr = data;
+    volatile uint32_t * fifoPtr = &SPI1W0;
+    uint8_t r = 64 / size;
+    uint32_t repeatRem = repeat % r;
+    repeat = repeat / r;
 
-void SPIClass::writePattern_(uint8_t * data, uint8_t size, uint8_t repeat) {
-    uint8_t bytes = (size * repeat);
-    uint8_t buffer[64];
-    uint8_t * bufferPtr = &buffer[0];
-    uint8_t * dataPtr;
-    uint8_t dataSize = bytes;
-    for(uint8_t i = 0; i < repeat; i++) {
-        dataSize = size;
+    while(r--){
         dataPtr = data;
-        while(dataSize--) {
+        for(uint8_t i=0; i<size; i++){
             *bufferPtr = *dataPtr;
-            dataPtr++;
             bufferPtr++;
+            dataPtr++;
         }
     }
 
-    writeBytes(&buffer[0], bytes);
+    fifoPtr = &SPI1W0;
+    for(uint8_t i=0; i<16; i++){
+        *fifoPtr = buffer[i];
+        fifoPtr++;
+    }
+
+    r = 64 / size;
+
+    SPI1U = SPIUMOSI | SPIUSSE;
+    //Fill fifo with data
+    setDataBits(r * size * 8);
+    while(repeat--){
+        SPI1CMD |= SPIBUSY;
+        while(SPI1CMD & SPIBUSY) {}
+    }
+
+    setDataBits(repeatRem * size * 8);
+    SPI1CMD |= SPIBUSY;
+    while(SPI1CMD & SPIBUSY) {}
+
+    SPI1U = SPIUMOSI | SPIUDUPLEX | SPIUSSE;
 }
 
 /**
diff --git a/libraries/SPI/SPI.h b/libraries/SPI/SPI.h
index 141f30ef4..4a9273a61 100644
--- a/libraries/SPI/SPI.h
+++ b/libraries/SPI/SPI.h
@@ -75,7 +75,6 @@ public:
 private:
   bool useHwCs;
   void writeBytes_(uint8_t * data, uint8_t size);
-  void writePattern_(uint8_t * data, uint8_t size, uint8_t repeat);
   void transferBytes_(uint8_t * out, uint8_t * in, uint8_t size);
   inline void setDataBits(uint16_t bits);
 };