hybrid PWM system

Uses PWM1 to directly control pins 4, 6 & 11 (servo 0, 1 & 3) and PWM1
generated interrupts to control other pins.

Interupt control of the servo pins had too much jitter so switched all
that we could to PWM1 direct control.  The PWM1 direct control pins have
less than 1 microsecond pulse width jitter while the interrupt
controlled ones can have 20+ microseconds of jitter.

Also added insurance to the servo code in the "disable servo after move"
section.

Marlin/src/HAL/HAL_LPC1768/LPC1768_PWM.h

@@ -23,17 +23,26 @@
 /**
  * The class Servo uses the PWM class to implement it's functions
  *
- * The PWM1 module is only used to generate interrups at specified times. It
- * is NOT used to directly toggle pins. The ISR writes to the pin assigned to
- * that interrupt
- *
  * All PWMs use the same repetition rate - 20mS because that's the normal servo rate
+*/
+
+/**
+ * This is a hybrid system.  
+ *
+ * The PWM1 module is used to directly control the Servo 0, 1 & 3 pins.  This keeps
+ * the pulse width jitter to under a microsecond.
  *
+ * For all other pins the PWM1 module is used to generate interrupts.  The ISR 
+ * routine does the actual setting/clearing of pins.  The upside is that any pin can
+ * have a PWM channel assigned to it.  The downside is that there is more pulse width
+ * jitter. The jitter depends on what else is happening in the system and what ISRs
+ * prempt the PWM ISR.  Writing to the SD card can add 20 microseconds to the pulse
+ * width.
  */
-
+ 
 /**
  * The data structures are setup to minimize the computation done by the ISR which
- * minimizes ISR execution time.  Execution times are 1.7 to 1.9 microseconds.
+ * minimizes ISR execution time.  Execution times are 2.2 - 3.7 microseconds.
  *
  * Two tables are used.  active_table is used by the ISR.  Changes to the table are
  * are done by copying the active_table into the work_table, updating the work_table
@@ -47,34 +56,39 @@
  *
  * The ISR's priority is set to the maximum otherwise other ISRs can cause considerable
  * jitter in the PWM high time.
+ *
+ * See the end of this file for details on the hardware/firmware interaction
  */
 
 
 #ifdef TARGET_LPC1768
 #include <lpc17xx_pinsel.h>
-//#include "../HAL.h"
-//#include "../../../macros.h"
-#include "serial.h"
-
-typedef struct {       // holds all data needed to control the 6 PWM channels
-    uint8_t sequence;  // 0: available slot, 1 - 6: PWM channel assigned to that slot
-    uint8_t logical_pin;
-    uint16_t PWM_mask;
+
+#define NUM_PWMS 6
+
+typedef struct {            // holds all data needed to control/init one of the PWM channels
+    uint8_t  sequence;       // 0: available slot, 1 - 6: PWM channel assigned to that slot
+    uint8_t  logical_pin;
+    uint16_t PWM_mask;       // MASK TO CHECK/WRITE THE IR REGISTER
     volatile uint32_t* set_register;
     volatile uint32_t* clr_register;
-    uint32_t write_mask;
-    uint32_t microseconds;
-    uint32_t min;
-    uint32_t max;
-    bool    PWM_flag;     //
-    uint8_t servo_index;  // 0 - MAX_SERVO -1 : servo index,  0xFF : PWM channel
-    bool    active_flag;
+    uint32_t write_mask;     // USED BY SET/CLEAR COMMANDS 
+    uint32_t microseconds;   // value written to MR register
+    uint32_t min;            // lower value limit checked by WRITE routine before writing to the MR register
+    uint32_t max;            // upper value limit checked by WRITE routine before writing to the MR register
+    bool     PWM_flag;       // 0 - USED BY sERVO, 1 - USED BY ANALOGWRITE
+    uint8_t  servo_index;    // 0 - MAX_SERVO -1 : servo index,  0xFF : PWM channel
+    bool     active_flag;    // THIS TABLE ENTRY IS ACTIVELY TOGGLING A PIN
+    uint8_t  assigned_MR;    // Which MR (1-6) is used by this logical channel
+    uint32_t PCR_bit;        // PCR register bit to enable PWM1 control of this pin
+    uint32_t PINSEL3_bits;   // PINSEL3 register bits to set pin mode to PWM1 control
 
 } PWM_map;
 
+
 #define MICRO_MAX 0xffffffff
 
-#define PWM_MAP_INIT_ROW {0, 0xff, 0, 0, 0, 0, MICRO_MAX, 0, 0, 0, 0, 0}
+#define PWM_MAP_INIT_ROW {0, 0xff, 0, 0, 0, 0, MICRO_MAX, 0, 0, 0, 0, 0, 0, 0, 0}
 #define PWM_MAP_INIT {PWM_MAP_INIT_ROW,\
                       PWM_MAP_INIT_ROW,\
                       PWM_MAP_INIT_ROW,\
@@ -83,18 +97,14 @@ typedef struct {       // holds all data needed to control the 6 PWM channels
                       PWM_MAP_INIT_ROW,\
                      };
 
-PWM_map PWM1_map_A[6] = PWM_MAP_INIT;
-PWM_map PWM1_map_B[6] = PWM_MAP_INIT;
+PWM_map PWM1_map_A[NUM_PWMS] = PWM_MAP_INIT;
+PWM_map PWM1_map_B[NUM_PWMS] = PWM_MAP_INIT;
 
 PWM_map *active_table = PWM1_map_A;
 PWM_map *work_table = PWM1_map_B;
 PWM_map *ISR_table;
 
 
-#define NUM_PWMS 6
-
-volatile uint8_t PWM1_ISR_index = 0;
-
 #define IR_BIT(p) (p >= 0 && p <= 3 ? p : p + 4 )
 #define COPY_ACTIVE_TABLE    for (uint8_t i = 0; i < 6 ; i++) work_table[i] = active_table[i]
 #define PIN_IS_INVERTED(p) 0  // place holder in case inverting PWM output is offered
@@ -169,11 +179,12 @@ void LPC1768_PWM_init(void) {
 }
 
 
-bool PWM_table_swap;  // flag to tell the ISR that the tables have been swapped
-
+bool PWM_table_swap = false;  // flag to tell the ISR that the tables have been swapped
+bool PWM_MR0_wait = false;  // flag to ensure don't delay MR0 interrupt 
 
 
 bool LPC1768_PWM_attach_pin(uint8_t pin, uint32_t min = 1, uint32_t max = (LPC_PWM1_MR0 - MR0_MARGIN), uint8_t servo_index = 0xff) {
+  while (PWM_table_swap) delay(5);  // don't do anything until the previous change has been implemented by the ISR
   COPY_ACTIVE_TABLE;  // copy active table into work table
   uint8_t slot = 0;
   for (uint8_t i = 0; i < NUM_PWMS ; i++)         // see if already in table
@@ -196,6 +207,9 @@ bool LPC1768_PWM_attach_pin(uint8_t pin, uint32_t min = 1, uint32_t max = (LPC_P
   work_table[slot].active_flag = false;
 
   //swap tables
+  PWM_MR0_wait = true;
+  while (PWM_MR0_wait) delay(5);  //wait until MR0 interrupt has happend so don't delay it.
+  
   NVIC_DisableIRQ(PWM1_IRQn);
   PWM_map *pointer_swap = active_table;
   active_table = work_table;
@@ -206,18 +220,36 @@ bool LPC1768_PWM_attach_pin(uint8_t pin, uint32_t min = 1, uint32_t max = (LPC_P
   return 1;
 }
 
-
-
-bool LPC1768_PWM_write(uint8_t pin, uint32_t value) {
-  COPY_ACTIVE_TABLE;  // copy active table into work table
-  uint8_t slot = 0xFF;
-  for (uint8_t i = 0; i < NUM_PWMS; i++)         // find slot
-    if (work_table[i].logical_pin == pin) slot = i;
-  if (slot == 0xFF) return false;    // return error if pin not found
-  digitalWrite(pin, 0);  // set pin to output & set it low
-  work_table[slot].microseconds = MAX(MIN(value, work_table[slot].max), work_table[slot].min);
-  work_table[slot].active_flag = true;
-
+#define pin_11_PWM_channel 2
+#define pin_6_PWM_channel  3
+#define pin_4_PWM_channel  1
+
+// used to keep track of which Match Registers have been used and if they will be used by the
+// PWM1 module to directly control the pin or will be used to generate an interrupt
+typedef struct {                        // status of PWM1 channel
+    uint8_t map_used;                   // 0 - this MR register not used/assigned
+    uint8_t map_PWM_INT;                // 0 - available for interrupts, 1 - in use by PWM
+    uint8_t map_PWM_PIN;                // logical pin number for this PwM1 controlled pin / port
+    volatile uint32_t* MR_register;     // address of the MR register for this PWM1 channel
+    uint32_t PCR_bit;                   // PCR register bit to enable PWM1 control of this pin
+    uint32_t PINSEL3_bits;              // PINSEL3 register bits to set pin mode to PWM1 control
+} MR_map;
+
+MR_map map_MR[NUM_PWMS];  
+ 
+void LPC1768_PWM_update_map_MR(void) {
+  map_MR[0] = {0, (uint8_t) (LPC_PWM1->PCR & _BV(8 + pin_4_PWM_channel)  ? 1 : 0),  4, &LPC_PWM1->MR1, 0, 0};
+  map_MR[1] = {0, (uint8_t) (LPC_PWM1->PCR & _BV(8 + pin_11_PWM_channel) ? 1 : 0), 11, &LPC_PWM1->MR2, 0, 0};
+  map_MR[2] = {0, (uint8_t) (LPC_PWM1->PCR & _BV(8 + pin_6_PWM_channel)  ? 1 : 0),  6, &LPC_PWM1->MR3, 0, 0};
+  map_MR[3] = {0, 0,  0, &LPC_PWM1->MR4, 0, 0};
+  map_MR[4] = {0, 0,  0, &LPC_PWM1->MR5, 0, 0};
+  map_MR[5] = {0, 0,  0, &LPC_PWM1->MR6, 0, 0};
+}  
+
+
+uint32_t LPC1768_PWM_interrupt_mask = 1;
+
+void LPC1768_PWM_update(void) {
   for (uint8_t i = NUM_PWMS; --i;) {  // (bubble) sort table by microseconds
     bool didSwap = false;
     PWM_map temp;
@@ -232,122 +264,137 @@ bool LPC1768_PWM_write(uint8_t pin, uint32_t value) {
     if (!didSwap) break;
   }
 
-  for (uint8_t i = 0; i < NUM_PWMS; i++)                             // set the index & PWM_mask
+  LPC1768_PWM_interrupt_mask = 0;                          // set match registers to new values, build IRQ mask
+  for (uint8_t i = 0; i < NUM_PWMS; i++) { 
     if (work_table[i].active_flag == true) {
       work_table[i].sequence = i + 1;
-      work_table[i].PWM_mask = _BV(IR_BIT(i + 1));
+      
+      // first see if there is a PWM1 controlled pin for this entry
+      bool found = false;
+      for (uint8_t j = 0; (j < NUM_PWMS) && !found; j++) {      
+        if ( (map_MR[j].map_PWM_PIN == work_table[i].logical_pin) && map_MR[j].map_PWM_INT ) {
+          *map_MR[j].MR_register = work_table[i].microseconds;  // found one of the PWM pins
+          work_table[i].PWM_mask = 0;
+          work_table[i].PCR_bit = map_MR[j].PCR_bit;            // PCR register bit to enable PWM1 control of this pin
+          work_table[i].PINSEL3_bits = map_MR[j].PINSEL3_bits;  // PINSEL3 register bits to set pin mode to PWM1 control} MR_map;
+          map_MR[j].map_used = 2;
+          work_table[i].assigned_MR = j +1;                    // only used to help in debugging                                                                  
+          found = true;
+        }
+      } 
+      
+      // didn't find a PWM1 pin so get an interrupt
+      for (uint8_t k = 0; (k < NUM_PWMS) && !found; k++) {     
+        if ( !(map_MR[k].map_PWM_INT || map_MR[k].map_used)) {
+          *map_MR[k].MR_register = work_table[i].microseconds;  // found one for an interrupt pin
+          map_MR[k].map_used = 1;
+          LPC1768_PWM_interrupt_mask |= _BV(3 * (k + 1));  // set bit in the MCR to enable this MR to generate an interrupt
+          work_table[i].PWM_mask = _BV(IR_BIT(k + 1));  // bit in the IR that will go active when this MR generates an interrupt
+          work_table[i].assigned_MR = k +1;                // only used to help in debugging 
+          found = true;
+        }  
+      }
     }
-    else work_table[i].sequence = 0;
-
-  uint32_t interrupt_mask = 0;                          // set match registers to new values, build IRQ mask
-  if (work_table[0].active_flag == true) {
-    LPC_PWM1->MR1 = work_table[0].microseconds;
-    interrupt_mask |= _BV(3);
-  }
-  if (work_table[1].active_flag == true) {
-    LPC_PWM1->MR2 = work_table[1].microseconds;
-    interrupt_mask |= _BV(6);
-  }
-  if (work_table[2].active_flag == true) {
-    LPC_PWM1->MR3 = work_table[2].microseconds;
-    interrupt_mask |= _BV(9);
-  }
-  if (work_table[3].active_flag == true) {
-    LPC_PWM1->MR4 = work_table[3].microseconds;
-    interrupt_mask |= _BV(12);
-  }
-  if (work_table[4].active_flag == true) {
-    LPC_PWM1->MR5 = work_table[4].microseconds;
-    interrupt_mask |= _BV(15);
-  }
-  if (work_table[5].active_flag == true) {
-    LPC_PWM1->MR6 = work_table[5].microseconds;
-    interrupt_mask |= _BV(18);
-  }
-  interrupt_mask |= _BV(0);  // add in MR0 interrupt
-
-  // swap tables
+    else
+      work_table[i].sequence = 0;
+  }      
+  LPC1768_PWM_interrupt_mask |= (uint32_t) _BV(0);  // add in MR0 interrupt
+
+   // swap tables
+   
+  PWM_MR0_wait = true;
+  while (PWM_MR0_wait) delay(5);  //wait until MR0 interrupt has happend so don't delay it.
+  
   NVIC_DisableIRQ(PWM1_IRQn);
   LPC_PWM1->LER = 0x07E;  // Set the latch Enable Bits to load the new Match Values for MR1 - MR6
   PWM_map *pointer_swap = active_table;
   active_table = work_table;
   work_table = pointer_swap;
   PWM_table_swap = true;  // tell the ISR that the tables have been swapped
-  LPC_PWM1->MCR = interrupt_mask;          // enable new PWM individual channel interrupts
   NVIC_EnableIRQ(PWM1_IRQn);  // re-enable PWM interrupts
-
-  return 1;
 }
 
 
-
-bool LPC1768_PWM_detach_pin(uint8_t pin) {
+bool LPC1768_PWM_write(uint8_t pin, uint32_t value) {
+  while (PWM_table_swap) delay(5);  // don't do anything until the previous change has been implemented by the ISR
   COPY_ACTIVE_TABLE;  // copy active table into work table
   uint8_t slot = 0xFF;
   for (uint8_t i = 0; i < NUM_PWMS; i++)         // find slot
     if (work_table[i].logical_pin == pin) slot = i;
   if (slot == 0xFF) return false;    // return error if pin not found
-  pinMode(pin, INPUT_PULLUP);  // set pin to input with pullup
-  work_table[slot] = PWM_MAP_INIT_ROW;
 
-  for (uint8_t i = NUM_PWMS; --i;) {  // (bubble) sort table by microseconds
-    bool didSwap = false;
-    PWM_map temp;
-    for (uint16_t j = 0; j < i; ++j) {
-      if (work_table[j].microseconds > work_table[j + 1].microseconds) {
-        temp = work_table[j + 1];
-        work_table[j + 1] = work_table[j];
-        work_table[j] = temp;
-        didSwap = true;
-      }
-    }
-    if (!didSwap) break;
-  }
+  LPC1768_PWM_update_map_MR();
+  
+  switch(pin) {
+    case 11:                        // Servo 0, PWM1 channel 2 (Pin 11  P1.20 PWM1.2)
+      map_MR[pin_11_PWM_channel - 1].PCR_bit = _BV(8 + pin_11_PWM_channel);  // enable PWM1 module control of this pin
+      map_MR[pin_11_PWM_channel - 1].map_PWM_INT = 1;               // 0 - available for interrupts, 1 - in use by PWM
+      map_MR[pin_11_PWM_channel - 1].PINSEL3_bits = 0x2 <<  8;      // ISR must do this AFTER setting PCR
+      break;
+    case  6:                        // Servo 1, PWM1 channel 3 (Pin 6  P1.21 PWM1.3)
+      map_MR[pin_6_PWM_channel - 1].PCR_bit = _BV(8 + pin_6_PWM_channel);                  // enable PWM1 module control of this pin
+      map_MR[pin_6_PWM_channel - 1].map_PWM_INT = 1;                // 0 - available for interrupts, 1 - in use by PWM
+      map_MR[pin_6_PWM_channel - 1].PINSEL3_bits = 0x2 << 10;       // ISR must do this AFTER setting PCR
+      break;
+    case  4:                        // Servo 3, PWM1 channel 1 (Pin 4  P1.18 PWM1.1)
+      map_MR[pin_4_PWM_channel - 1].PCR_bit = _BV(8 + pin_4_PWM_channel);                  // enable PWM1 module control of this pin 
+      map_MR[pin_4_PWM_channel - 1].map_PWM_INT = 1;                // 0 - available for interrupts, 1 - in use by PWM
+      map_MR[pin_4_PWM_channel - 1].PINSEL3_bits =  0x2 <<  4;       // ISR must do this AFTER setting PCR
+      break;
+    default:                                                        // ISR pins 
+      pinMode(pin, OUTPUT);  // set pin to output but don't write anything in case it's already in use
+      break;
+  }      
+    
+  work_table[slot].microseconds = MAX(MIN(value, work_table[slot].max), work_table[slot].min);
+  work_table[slot].active_flag = true;
 
-  for (uint8_t i = 0; i < NUM_PWMS; i++)                             // set the index & PWM_mask
-    if (work_table[i].active_flag == true) {
-      work_table[i].sequence = i + 1;
-      work_table[i].PWM_mask = _BV(IR_BIT(i + 1));
-    }
-    else work_table[i].sequence = 0;
+  LPC1768_PWM_update();
 
-  uint32_t interrupt_mask = 0;                        // set match registers to new values, build IRQ mask
-  if (work_table[0].active_flag == true) {
-    LPC_PWM1->MR1 = work_table[0].microseconds;
-    interrupt_mask |= _BV(3);
-  }
-  if (work_table[1].active_flag == true) {
-    LPC_PWM1->MR2 = work_table[1].microseconds;
-    interrupt_mask |= _BV(6);
-  }
-  if (work_table[2].active_flag == true) {
-    LPC_PWM1->MR3 = work_table[2].microseconds;
-    interrupt_mask |= _BV(9);
-  }
-  if (work_table[3].active_flag == true) {
-    LPC_PWM1->MR4 = work_table[3].microseconds;
-    interrupt_mask |= _BV(12);
-  }
-  if (work_table[4].active_flag == true) {
-    LPC_PWM1->MR5 = work_table[4].microseconds;
-    interrupt_mask |= _BV(15);
-  }
-  if (work_table[5].active_flag == true) {
-    LPC_PWM1->MR6 = work_table[5].microseconds;
-    interrupt_mask |= _BV(18);
-  }
+  return 1;
+}  
 
-  interrupt_mask |= _BV(0);  // add in MR0 interrupt
 
-  // swap tables
-  NVIC_DisableIRQ(PWM1_IRQn);
-  LPC_PWM1->LER = 0x07E;  // Set the latch Enable Bits to load the new Match Values for MR1 - MR6
-  PWM_map *pointer_swap = active_table;
-  active_table = work_table;
-  work_table = pointer_swap;
-  PWM_table_swap = true;  // tell the ISR that the tables have been swapped
-  LPC_PWM1->MCR = interrupt_mask;          // enable remaining PWM individual channel interrupts
-  NVIC_EnableIRQ(PWM1_IRQn);  // re-enable PWM interrupts
+bool LPC1768_PWM_detach_pin(uint8_t pin) {
+  while (PWM_table_swap) delay(5);  // don't do anything until the previous change has been implemented by the ISR
+  COPY_ACTIVE_TABLE;  // copy active table into work table
+  uint8_t slot = 0xFF;
+  for (uint8_t i = 0; i < NUM_PWMS; i++)         // find slot
+    if (work_table[i].logical_pin == pin) slot = i;
+  if (slot == 0xFF) return false;    // return error if pin not found
+
+  LPC1768_PWM_update_map_MR();
+
+  // OK to make these changes before the MR0 interrupt
+  switch(pin) {
+    case 11:                        // Servo 0, PWM1 channel 2  (Pin 11  P1.20 PWM1.2)
+      LPC_PWM1->PCR &= ~(_BV(8 + pin_11_PWM_channel));                 // disable PWM1 module control of this pin
+      map_MR[pin_11_PWM_channel - 1].PCR_bit = 0;
+      LPC_PINCON->PINSEL3 &= ~(0x3 <<  8);    // return pin to general purpose I/O
+      map_MR[pin_11_PWM_channel - 1].PINSEL3_bits = 0;
+      map_MR[pin_11_PWM_channel - 1].map_PWM_INT = 0;               // 0 - available for interrupts, 1 - in use by PWM
+      break;
+    case  6:                        // Servo 1, PWM1 channel 3  (Pin 6  P1.21 PWM1.3)
+      LPC_PWM1->PCR &= ~(_BV(8 + pin_6_PWM_channel));                  // disable PWM1 module control of this pin
+      map_MR[pin_6_PWM_channel - 1].PCR_bit = 0;
+      LPC_PINCON->PINSEL3 &= ~(0x3 << 10);  // return pin to general purpose I/O
+      map_MR[pin_6_PWM_channel - 1].PINSEL3_bits = 0;
+      map_MR[pin_6_PWM_channel - 1].map_PWM_INT = 0;                // 0 - available for interrupts, 1 - in use by PWM
+      break;
+    case  4:                        // Servo 3, PWM1 channel 1 (Pin 4  P1.18 PWM1.1)
+      LPC_PWM1->PCR &= ~(_BV(8 + pin_4_PWM_channel));                  // disable PWM1 module control of this pin 
+      map_MR[pin_4_PWM_channel - 1].PCR_bit =  0;
+      LPC_PINCON->PINSEL3 &= ~(0x3 <<  4);  // return pin to general purpose I/O
+      map_MR[pin_4_PWM_channel - 1].PINSEL3_bits =  0;
+      map_MR[pin_4_PWM_channel - 1].map_PWM_INT = 0;                // 0 - available for interrupts, 1 - in use by PWM
+      break;
+  } 
+  
+  pinMode(pin, INPUT);
+  
+  work_table[slot] = PWM_MAP_INIT_ROW;
+
+  LPC1768_PWM_update();
 
   return 1;
 }
@@ -356,27 +403,101 @@ bool LPC1768_PWM_detach_pin(uint8_t pin) {
 
 #define HAL_PWM_LPC1768_ISR  extern "C" void PWM1_IRQHandler(void)
 
+
+// Both loops could be terminated when the last active channel is found but that would
+// result in variations ISR run time which results in variations in pulse width
+
+/**
+ * Changes to PINSEL3, PCR and MCR are only done during the MR0 interrupt otherwise
+ * the wrong pin may be toggled or even have the system hang.
+ */
+  
+  
 HAL_PWM_LPC1768_ISR {
   if (PWM_table_swap) ISR_table = work_table;   // use old table if a swap was just done
   else ISR_table = active_table;
 
   if (LPC_PWM1->IR & 0x1) {                                      // MR0 interrupt
-    PWM_table_swap = false;                                         // MR0 means new values could have been
-    ISR_table = active_table;                                       // loaded so set everything to normal operation
-    for (uint8_t i = 0; (i < NUM_PWMS) && ISR_table[i].active_flag ; i++)
-      *ISR_table[i].set_register = ISR_table[i].write_mask;     // set all enabled channels active
+    ISR_table = active_table;                    // MR0 means new values could have been loaded so set everything
+    if (PWM_table_swap) LPC_PWM1->MCR = LPC1768_PWM_interrupt_mask; // enable new PWM individual channel interrupts
+
+    for (uint8_t i = 0; (i < NUM_PWMS) ; i++) {
+      if(ISR_table[i].active_flag && !((ISR_table[i].logical_pin ==  11) || 
+                                       (ISR_table[i].logical_pin ==  4) || 
+                                       (ISR_table[i].logical_pin ==  6))) 
+        *ISR_table[i].set_register = ISR_table[i].write_mask;       // set pins for all enabled interrupt channels active
+      if (PWM_table_swap && ISR_table[i].PCR_bit) {
+        LPC_PWM1->PCR |= ISR_table[i].PCR_bit;              // enable PWM1 module control of this pin
+        LPC_PINCON->PINSEL3 |= ISR_table[i].PINSEL3_bits;   // set pin mode to PWM1 control - must be done after PCR 
+      }
+    }
+    PWM_table_swap = false;
+    PWM_MR0_wait = false;
     LPC_PWM1->IR = 0x01;                                             // clear the MR0 interrupt flag bit
-    PWM1_ISR_index = 0;
   }
   else {
-    if (ISR_table[PWM1_ISR_index].active_flag && (LPC_PWM1->IR & ISR_table[PWM1_ISR_index].PWM_mask)) {
-      LPC_PWM1->IR = ISR_table[PWM1_ISR_index].PWM_mask;       // clear the interrupt flag bit
-      *ISR_table[PWM1_ISR_index].clr_register = ISR_table[PWM1_ISR_index].write_mask;   // set channel to inactive
-    }
-    PWM1_ISR_index++;                                           // should be the index for the next interrupt
+    for (uint8_t i = 0; i < NUM_PWMS ; i++)
+      if (ISR_table[i].active_flag && (LPC_PWM1->IR & ISR_table[i].PWM_mask) ){
+        LPC_PWM1->IR = ISR_table[i].PWM_mask;       // clear the interrupt flag bits for expected interrupts
+        *ISR_table[i].clr_register = ISR_table[i].write_mask;   // set channel to inactive
+      }
   }
+ 
+  LPC_PWM1->IR = 0x70F;  // guarantees all interrupt flags are cleared which, if there is an unexpected
+                           // PWM interrupt, will keep the ISR from hanging which will crash the controller
 
 return;
 }
-
 #endif
+
+/////////////////////////////////////////////////////////////////
+/////////////////  HARDWARE FIRMWARE INTERACTION ////////////////
+/////////////////////////////////////////////////////////////////
+
+/**
+ *  Almost all changes to the hardware registers must be coordinated with the Match Register 0 (MR0)
+ *  interrupt.  The only exception is detaching pins.  It doesn't matter when they go
+ *  tristate.  
+ *
+ *  The LPC1768_PWM_init routine kicks off the MR0 interrupt.  This interrupt is never disabled or 
+ *  delayed. 
+ *
+ *  The PWM_table_swap flag is set when the firmware has swapped in an updated table.  It is
+ *  cleared by the ISR during the MR0 interrupt as it completes the swap and accompanying updates.
+ *  It serves two purposes:
+ *    1) Tells the ISR that the tables have been swapped
+ *    2) Keeps the firmware from starting a new update until the previous one has been completed. 
+ *
+ *  The PWM_MR0_wait flag is set when the firmware is ready to swap in an updated table and cleared by 
+ *  the ISR during the MR0 interrupt.  It is used to avoid delaying the MR0 interrupt when swapping in
+ *  an updated table.  This avoids glitches in pulse width and/or repetition rate. 
+ *
+ *  The sequence of events during a write to a PWM channel is:
+ *    1) Waits until PWM_table_swap flag is false before starting
+ *    2) Copies the active table into the work table
+ *    3) Updates the work table
+ *         NOTES - MR1-MR6 are updated at this time.  The updates aren't put into use until the first
+ *                 MR0 after the LER register has been written.  The LER register is written during the
+ *                 table swap process.
+ *               - The MCR mask is created at this time.  It is not used until the ISR writes the MCR
+ *                 during the MR0 interrupt in the table swap process.
+ *    4) Sets the PWM_MR0_wait flag
+ *    5) ISR clears the PWM_MR0_wait flag during the next MR0 interrupt
+ *    6) Once the PWM_MR0_wait flag is cleared then the firmware:
+ *          disables the ISR interrupt
+ *          swaps the pointers to the tables
+ *          writes to the LER register
+ *          sets the PWM_table_swap flag active
+ *          re-enables the ISR
+ *     7) On the next interrupt the ISR changes it's pointer to the work table which is now the old, 
+ *        unmodified, active table.
+ *     8) On the next MR0 interrupt the ISR:
+ *          switches over to the active table
+ *          clears the PWM_table_swap and PWM_MR0_wait flags
+ *          updates the MCR register with the possibly new interrupt sources/assignments
+ *          writes to the PCR register to enable the direct control of the Servo 0, 1 & 3 pins by the PWM1 module
+ *          sets the PINSEL3 register to function/mode 0x2 for the Servo 0, 1 & 3 pins
+ *             NOTE - PCR must be set before PINSEL
+ *          sets the pins controlled by the ISR to their active states
+ */
+ 

Marlin/src/HAL/HAL_LPC1768/LPC1768_Servo.cpp

@@ -159,6 +159,7 @@
       #if ENABLED(DEACTIVATE_SERVOS_AFTER_MOVE)
         this->detach();
         LPC1768_PWM_detach_pin(servo_info[this->servoIndex].Pin.nbr);  // shut down the PWM signal
+        LPC1768_PWM_attach_pin(servo_info[this->servoIndex].Pin.nbr, MIN_PULSE_WIDTH, MAX_PULSE_WIDTH, this->servoIndex);  // make sure no one else steals the slot
       #endif
     }
   }