Tone fix for #59 and optimizations

This fixes issue #59 and optimize the way the prescaler is found.
Furthermore non needed instructions are not repeated.

cores/arduino/Tone.cpp

@@ -29,11 +29,13 @@ volatile uint32_t *portClearRegister;
 volatile uint32_t portBitMask;
 volatile int64_t toggleCount;
 volatile bool toneIsActive = false;
+volatile bool firstTimeRunning = false;
 
 #define TONE_TC         TC5
 #define TONE_TC_IRQn    TC5_IRQn
 #define TONE_TC_TOP     0xFFFF
 #define TONE_TC_CHANNEL 0
+
 void TC5_Handler (void) __attribute__ ((weak, alias("Tone_Handler")));
 
 static inline void resetTC (Tc* TCx)
@@ -55,6 +57,21 @@ void toneAccurateClock (uint32_t accurateSystemCoreClockFrequency)
 
 void tone (uint32_t outputPin, uint32_t frequency, uint32_t duration)
 {
+  // Configure interrupt request
+  NVIC_DisableIRQ(TONE_TC_IRQn);
+  NVIC_ClearPendingIRQ(TONE_TC_IRQn);
+  
+  if(!firstTimeRunning)
+  {
+    firstTimeRunning = true;
+    
+    NVIC_SetPriority(TONE_TC_IRQn, 0);
+      
+    // Enable GCLK for TC4 and TC5 (timer counter input clock)
+    GCLK->CLKCTRL.reg = (uint16_t) (GCLK_CLKCTRL_CLKEN | GCLK_CLKCTRL_GEN_GCLK0 | GCLK_CLKCTRL_ID(GCM_TC4_TC5));
+    while (GCLK->STATUS.bit.SYNCBUSY);
+  }
+  
   if (toneIsActive && (outputPin != lastOutputPin))
     noTone(lastOutputPin);
 
@@ -67,89 +84,70 @@ void tone (uint32_t outputPin, uint32_t frequency, uint32_t duration)
 
   ccValue = toneMaxFrequency / frequency - 1;
   prescalerConfigBits = TC_CTRLA_PRESCALER_DIV1;
-
-  if (ccValue > TONE_TC_TOP)
+  
+  uint8_t i = 0;
+  
+  while(ccValue > TONE_TC_TOP)
+  {
+    ccValue = toneMaxFrequency / frequency / (2<<i) - 1;
+    i++;
+    if(i == 4 || i == 6 || i == 8) //DIV32 DIV128 and DIV512 are not available
+     i++;
+  }
+  
+  switch(i-1)
   {
-    ccValue = toneMaxFrequency / frequency / 2 - 1;
-    prescalerConfigBits = TC_CTRLA_PRESCALER_DIV2;
-
-    if (ccValue > TONE_TC_TOP)
-    {
-      ccValue = toneMaxFrequency / frequency / 4 - 1;
-      prescalerConfigBits = TC_CTRLA_PRESCALER_DIV4;
-
-      if (ccValue > TONE_TC_TOP)
-      {
-        ccValue = toneMaxFrequency / frequency / 8 - 1;
-        prescalerConfigBits = TC_CTRLA_PRESCALER_DIV8;
-
-        if (ccValue > TONE_TC_TOP)
-        {
-          ccValue = toneMaxFrequency / frequency / 16 - 1;
-          prescalerConfigBits = TC_CTRLA_PRESCALER_DIV16;
-
-          if (ccValue > TONE_TC_TOP)
-          {
-            ccValue = toneMaxFrequency / frequency / 64 - 1;
-            prescalerConfigBits = TC_CTRLA_PRESCALER_DIV64;
-
-            if (ccValue > TONE_TC_TOP)
-            {
-              ccValue = toneMaxFrequency / frequency / 256 - 1;
-              prescalerConfigBits = TC_CTRLA_PRESCALER_DIV256;
-
-              if (ccValue > TONE_TC_TOP)
-              {
-                ccValue = toneMaxFrequency / frequency / 1024 - 1;
-                prescalerConfigBits = TC_CTRLA_PRESCALER_DIV1024;
-              }
-            }
-          }
-        }
-      }
-    }
+    case 0: prescalerConfigBits = TC_CTRLA_PRESCALER_DIV2; break;
+    
+    case 1: prescalerConfigBits = TC_CTRLA_PRESCALER_DIV4; break;
+    
+    case 2: prescalerConfigBits = TC_CTRLA_PRESCALER_DIV8; break;
+    
+    case 3: prescalerConfigBits = TC_CTRLA_PRESCALER_DIV16; break;
+    
+    case 5: prescalerConfigBits = TC_CTRLA_PRESCALER_DIV64; break;
+      
+    case 7: prescalerConfigBits = TC_CTRLA_PRESCALER_DIV256; break;
+    
+    case 9: prescalerConfigBits = TC_CTRLA_PRESCALER_DIV1024; break;
+    
+    default: break;
   }
 
   toggleCount = (duration > 0 ? frequency * duration * 2 / 1000UL : -1);
 
-  // Enable GCLK for TC4 and TC5 (timer counter input clock)
-  GCLK->CLKCTRL.reg = (uint16_t) (GCLK_CLKCTRL_CLKEN | GCLK_CLKCTRL_GEN_GCLK0 | GCLK_CLKCTRL_ID(GCM_TC4_TC5));
-  while (GCLK->STATUS.bit.SYNCBUSY);
-
   resetTC(TONE_TC);
 
-  // Set Timer counter Mode to 16 bits
-  TONE_TC->COUNT16.CTRLA.reg |= TC_CTRLA_MODE_COUNT16;
-
-  // Set TONE_TC mode as match frequency
-  TONE_TC->COUNT16.CTRLA.reg |= TC_CTRLA_WAVEGEN_MFRQ;
-
-  TONE_TC->COUNT16.CTRLA.reg |= prescalerConfigBits;
+  uint16_t tmpReg = 0;
+  tmpReg |= TC_CTRLA_MODE_COUNT16;  // Set Timer counter Mode to 16 bits
+  tmpReg |= TC_CTRLA_WAVEGEN_MFRQ;  // Set TONE_TC mode as match frequency
+  tmpReg |= prescalerConfigBits;
+  TONE_TC->COUNT16.CTRLA.reg |= tmpReg;
+  WAIT_TC16_REGS_SYNC(TONE_TC)
 
   TONE_TC->COUNT16.CC[TONE_TC_CHANNEL].reg = (uint16_t) ccValue;
   WAIT_TC16_REGS_SYNC(TONE_TC)
 
-  // Configure interrupt request
-  NVIC_DisableIRQ(TONE_TC_IRQn);
-  NVIC_ClearPendingIRQ(TONE_TC_IRQn);
-  NVIC_SetPriority(TONE_TC_IRQn, 0);
-  NVIC_EnableIRQ(TONE_TC_IRQn);
-
   portToggleRegister = &(PORT->Group[g_APinDescription[outputPin].ulPort].OUTTGL.reg);
   portClearRegister = &(PORT->Group[g_APinDescription[outputPin].ulPort].OUTCLR.reg);
   portBitMask = (1ul << g_APinDescription[outputPin].ulPin);
 
   // Enable the TONE_TC interrupt request
   TONE_TC->COUNT16.INTENSET.bit.MC0 = 1;
-
-  lastOutputPin = outputPin;
-  digitalWrite(outputPin, LOW);
-  pinMode(outputPin, OUTPUT);
-  toneIsActive = true;
+  
+  if (outputPin != lastOutputPin)
+  {
+    lastOutputPin = outputPin;
+    digitalWrite(outputPin, LOW);
+    pinMode(outputPin, OUTPUT);
+    toneIsActive = true;
+  }
 
   // Enable TONE_TC
   TONE_TC->COUNT16.CTRLA.reg |= TC_CTRLA_ENABLE;
   WAIT_TC16_REGS_SYNC(TONE_TC)
+  
+  NVIC_EnableIRQ(TONE_TC_IRQn);
 }
 
 void noTone (uint32_t outputPin)