Some k20 cleanup

2 files changed, 22 insertions, 11 deletions

diff --git a/chipsets.h b/chipsets.h
index 7f5d69ff..e1396d72 100644
--- a/chipsets.h
+++ b/chipsets.h
@@ -271,7 +271,7 @@ public:
 
 // We want to force all avr's to use the Trinket controller when running at 8Mhz, because even the 328's at 8Mhz
 // need the more tightly defined timeframes.
-#if (F_CPU == 8000000 || F_CPU == 16000000 || F_CPU == 24000000 || F_CPU == 48000000) // 125ns/clock
+#if (F_CPU == 8000000 || F_CPU == 16000000 || F_CPU == 24000000 || F_CPU == 48000000 || F_CPU == 96000000) // 125ns/clock
 #define FMUL (F_CPU/8000000)
 // WS2811@8Mhz 2 clocks, 5 clocks, 3 clocks
 template <uint8_t DATA_PIN, EOrder RGB_ORDER = RGB>
diff --git a/clockless_arm_k20.h b/clockless_arm_k20.h
index 59905d45..319dd471 100644
--- a/clockless_arm_k20.h
+++ b/clockless_arm_k20.h
@@ -66,8 +66,8 @@ public:
 	}
 #endif
 
-	template<int BITS> __attribute__ ((always_inline)) inline static void writeBits(register uint32_t & next_mark, register data_ptr_t port, register data_t hi, register data_t lo, register uint8_t & b)  {
-		for(register uint32_t i = BITS; i > 0; i--) { 
+	template<int BITS, int PX> __attribute__ ((always_inline)) inline static void writeBits(register uint32_t & next_mark, register data_ptr_t port, register data_t hi, register data_t lo, PixelController<RGB_ORDER> & pixels, register uint8_t & b)  {
+		for(register uint32_t i = BITS-1; i > 0; i--) { 
 			while(ARM_DWT_CYCCNT < next_mark);
 			next_mark = ARM_DWT_CYCCNT + (T1+T2+T3);
 			FastPin<DATA_PIN>::fastset(port, hi);
@@ -76,6 +76,20 @@ public:
 			while(ARM_DWT_CYCCNT < flip_mark);
 			FastPin<DATA_PIN>::fastset(port, lo);
 		}
+
+		while(ARM_DWT_CYCCNT < next_mark);
+		next_mark = ARM_DWT_CYCCNT + (T1+T2+T3);
+		FastPin<DATA_PIN>::fastset(port, hi);
+		uint32_t flip_mark = next_mark - ((b&0x80) ? (T3) : (T2+T3));
+
+		// load the next byte in the gap where we would've bit shifted in the past
+		switch(PX) { 
+			case 0: b = pixels.advanceAndLoadAndScale0(); break;
+			case 1: b = pixels.loadAndScale1(); break;
+			case 2: b = pixels.loadAndScale2(); break;
+		}
+		while(ARM_DWT_CYCCNT < flip_mark);
+		FastPin<DATA_PIN>::fastset(port, lo);
 	}
 
 	// This method is made static to force making register Y available to use for data on AVR - if the method is non-static, then 
@@ -89,7 +103,7 @@ public:
 		// Setup the pixel controller and load/scale the first byte 
 		pixels.preStepFirstByteDithering();
 		register uint8_t b = pixels.loadAndScale0();
-		
+
 	    // Get access to the clock 
 		ARM_DEMCR    |= ARM_DEMCR_TRCENA;
 		ARM_DWT_CTRL |= ARM_DWT_CTRL_CYCCNTENA;
@@ -100,16 +114,13 @@ public:
 			pixels.stepDithering();
 
 			// Write first byte, read next byte
-			writeBits<8+XTRA0>(next_mark, port, hi, lo, b);
-			b = pixels.loadAndScale1();
+			writeBits<8+XTRA0,1>(next_mark, port, hi, lo, pixels, b);
 
 			// Write second byte, read 3rd byte
-			writeBits<8+XTRA0>(next_mark, port, hi, lo, b);
-			b = pixels.loadAndScale2();
+			writeBits<8+XTRA0,2>(next_mark, port, hi, lo, pixels, b);
 
-			// Write third byte
-			writeBits<8+XTRA0>(next_mark, port, hi, lo, b);
-			b = pixels.advanceAndLoadAndScale0();
+			// Write third byte, read 1st byte of next pixel
+			writeBits<8+XTRA0,0>(next_mark, port, hi, lo, pixels, b);
 		};
 	}
 };