Initial effort at porting tlc_servos.h (compiles but fails to run)

Tlc5940.h

@@ -50,8 +50,8 @@
 #define disable_XLAT_pulses()   TCCR1A = _BV(COM1B1)
 
 #elif defined(__arm__) && defined(TEENSYDUINO)
-#define set_XLAT_interrupt()    { uint32_t tmp __attribute__((unused)) = FTM1_SC; FTM1_SC = FTM_SC_CLKS(1) | FTM_SC_CPWMS | FTM_SC_TOIE; }
+#define set_XLAT_interrupt()    FTM1_SC = FTM_SC_CLKS(1) | FTM_SC_CPWMS | FTM_SC_TOIE | (FTM1_SC & FTM_SC_PS(7))
-#define clear_XLAT_interrupt()  FTM1_SC = FTM_SC_CLKS(1) | FTM_SC_CPWMS
+#define clear_XLAT_interrupt()  FTM1_SC = FTM_SC_CLKS(1) | FTM_SC_CPWMS | (FTM1_SC & FTM_SC_PS(7))
 #define enable_XLAT_pulses()    CORE_PIN3_CONFIG = PORT_PCR_MUX(3)|PORT_PCR_DSE|PORT_PCR_SRE
 #define disable_XLAT_pulses()   CORE_PIN3_CONFIG = PORT_PCR_MUX(1)|PORT_PCR_DSE|PORT_PCR_SRE
 

pinouts/Teensy_KinetisK20.h

@@ -39,38 +39,73 @@
   #define TLC_TIMER_TEENSY3_NORMAL_CGL1		11
   #define TLC_TIMER_TEENSY3_NORMAL_MOD		30720
   #define TLC_TIMER_TEENSY3_NORMAL_CV		4
+  #define TLC_TIMER_TEENSY3_SERVO_CGH1		58
+  #define TLC_TIMER_TEENSY3_SERVO_CGL1		235
+  #define TLC_TIMER_TEENSY3_SERVO_PS		4
+  #define TLC_TIMER_TEENSY3_SERVO_MOD		37500
+  #define TLC_TIMER_TEENSY3_SERVO_CV		1
 #elif F_BUS == 56000000
   #define TLC_TIMER_TEENSY3_NORMAL_CGH1		3
   #define TLC_TIMER_TEENSY3_NORMAL_CGL1		11
   #define TLC_TIMER_TEENSY3_NORMAL_MOD		28672
   #define TLC_TIMER_TEENSY3_NORMAL_CV		3
+  #define TLC_TIMER_TEENSY3_SERVO_CGH1		54
+  #define TLC_TIMER_TEENSY3_SERVO_CGL1		219
+  #define TLC_TIMER_TEENSY3_SERVO_PS		4
+  #define TLC_TIMER_TEENSY3_SERVO_MOD		35000
+  #define TLC_TIMER_TEENSY3_SERVO_CV		1
 #elif F_BUS == 48000000
   #define TLC_TIMER_TEENSY3_NORMAL_CGH1		3
   #define TLC_TIMER_TEENSY3_NORMAL_CGL1		9
   #define TLC_TIMER_TEENSY3_NORMAL_MOD		24576
   #define TLC_TIMER_TEENSY3_NORMAL_CV		3
+  #define TLC_TIMER_TEENSY3_SERVO_CGH1		47
+  #define TLC_TIMER_TEENSY3_SERVO_CGL1		187
+  #define TLC_TIMER_TEENSY3_SERVO_PS		3
+  #define TLC_TIMER_TEENSY3_SERVO_MOD		60000
+  #define TLC_TIMER_TEENSY3_SERVO_CV		1
 #elif F_BUS == 36000000
   #define TLC_TIMER_TEENSY3_NORMAL_CGH1		2
   #define TLC_TIMER_TEENSY3_NORMAL_CGL1		7
   #define TLC_TIMER_TEENSY3_NORMAL_MOD		18432
   #define TLC_TIMER_TEENSY3_NORMAL_CV		2
+  #define TLC_TIMER_TEENSY3_SERVO_CGH1		35
+  #define TLC_TIMER_TEENSY3_SERVO_CGL1		141
+  #define TLC_TIMER_TEENSY3_SERVO_PS		3
+  #define TLC_TIMER_TEENSY3_SERVO_MOD		45000
+  #define TLC_TIMER_TEENSY3_SERVO_CV		1
 #elif F_BUS == 24000000
   #define TLC_TIMER_TEENSY3_NORMAL_CGH1		2
   #define TLC_TIMER_TEENSY3_NORMAL_CGL1		4
   #define TLC_TIMER_TEENSY3_NORMAL_MOD		12288
   #define TLC_TIMER_TEENSY3_NORMAL_CV		2
+  #define TLC_TIMER_TEENSY3_SERVO_CGH1		23
+  #define TLC_TIMER_TEENSY3_SERVO_CGL1		94
+  #define TLC_TIMER_TEENSY3_SERVO_PS		2
+  #define TLC_TIMER_TEENSY3_SERVO_MOD		60000
+  #define TLC_TIMER_TEENSY3_SERVO_CV		1
 #elif F_BUS == 16000000
   #define TLC_TIMER_TEENSY3_NORMAL_CGH1		1
   #define TLC_TIMER_TEENSY3_NORMAL_CGL1		3
   #define TLC_TIMER_TEENSY3_NORMAL_MOD		8192
   #define TLC_TIMER_TEENSY3_NORMAL_CV		1
-#elif F_BUS <= 8000000
+  #define TLC_TIMER_TEENSY3_SERVO_CGH1		16
+  #define TLC_TIMER_TEENSY3_SERVO_CGL1		62
+  #define TLC_TIMER_TEENSY3_SERVO_PS		2
+  #define TLC_TIMER_TEENSY3_SERVO_MOD		40000
+  #define TLC_TIMER_TEENSY3_SERVO_CV		1
+#elif F_BUS == 8000000
   #define TLC_TIMER_TEENSY3_NORMAL_CGH1		1
   #define TLC_TIMER_TEENSY3_NORMAL_CGL1		1
   #define TLC_TIMER_TEENSY3_NORMAL_MOD		4096
   #define TLC_TIMER_TEENSY3_NORMAL_CV		1
+  #define TLC_TIMER_TEENSY3_SERVO_CGH1		8
+  #define TLC_TIMER_TEENSY3_SERVO_CGL1		31
+  #define TLC_TIMER_TEENSY3_SERVO_PS		1
+  #define TLC_TIMER_TEENSY3_SERVO_MOD		40000
+  #define TLC_TIMER_TEENSY3_SERVO_CV		1
 #else
-#error "F_BUS must be 60, 56, 48, 36, 24, 16, 8, 4, or 2 MHz"
+#error "F_BUS must be 60, 56, 48, 36, 24, 16, 8 MHz"
 #endif
 
 #endif

tlc_servos.h

@@ -22,7 +22,6 @@
 /** \file
     TLC servo functions. */
 
-#include <avr/io.h>
 #include "Tlc5940.h"
 
 #ifndef SERVO_MAX_ANGLE
@@ -74,6 +73,7 @@ uint8_t tlc_valToAngle(uint16_t value);
 void tlc_initServos(uint8_t initAngle)
 {
     Tlc.init(tlc_angleToVal(initAngle));
+#if defined(__AVR__)
     TCCR1B &= ~(_BV(CS12) | _BV(CS11) | _BV(CS10)); // stop timer1
     ICR1 = SERVO_TIMER1_TOP;
     TCNT1 = 0;
@@ -83,6 +83,9 @@ void tlc_initServos(uint8_t initAngle)
     TCNT2 = 0;
     OCR2 = SERVO_TIMER2_TOP / 2;
     TCCR2 = oldTCCR2;
+#elif defined(TLC_TIMER3_GSCLK)
+    // TODO: timer3 implementation needed...
+    // pull requests would be most welcome! ;-)
 #else
     uint8_t oldTCCR2B = TCCR2B;
     TCCR2B = 0;
@@ -91,6 +94,23 @@ void tlc_initServos(uint8_t initAngle)
     TCCR2B = oldTCCR2B;
 #endif
     TCCR1B |= _BV(CS11); // start timer1 with div 8 prescale
+
+#elif defined(__arm__) && defined(TEENSYDUINO)
+    //clear_XLAT_interrupt();
+    uint32_t sc = FTM1_SC;
+    FTM1_SC = 0;  // stop timer
+    CMT_MSC = 0;
+    CMT_CGH1 = TLC_TIMER_TEENSY3_SERVO_CGH1;
+    CMT_CGL1 = TLC_TIMER_TEENSY3_SERVO_CGL1;
+    CMT_MSC = 0x01; // GSCLK target is 204800 Hz
+    // TODO: this reconfiguration of FTM1 crashes... why?
+    // pull requests would be most welcome! ;-)
+    FTM1_CNT = 0;
+    FTM1_MOD = TLC_TIMER_TEENSY3_SERVO_MOD;
+    FTM1_C0V = TLC_TIMER_TEENSY3_SERVO_MOD - TLC_TIMER_TEENSY3_SERVO_CV;
+    FTM1_C1V = TLC_TIMER_TEENSY3_SERVO_MOD - TLC_TIMER_TEENSY3_SERVO_CV - 1;
+    FTM1_SC = FTM_SC_CLKS(1) | FTM_SC_CPWMS | TLC_TIMER_TEENSY3_SERVO_PS;
+#endif
 }
 
 /** Sets a servo on channel to angle.