add support for more pwm output channels

3 files changed, 39 insertions, 7 deletions

diff --git a/src/drivers/boards/trifle/devconfig.c b/src/drivers/boards/trifle/devconfig.c
index 47b69a27e..1fec4acd6 100644
--- a/src/drivers/boards/trifle/devconfig.c
+++ b/src/drivers/boards/trifle/devconfig.c
@@ -21,7 +21,32 @@ __EXPORT const struct  pwm_servo_timer pwm_servo_timers[PWM_SERVO_MAX_TIMERS] =
 __EXPORT const struct pwm_servo_channel pwm_servo_channels[PWM_SERVO_MAX_CHANNELS] = {
   {
     .timer = &pwm_servo_timers[0],
+    .ftm_channel = 1,
+    .pinconfig = PIN_PORTC | PIN2 | PIN_ALT4_OUTPUT | PIN_ALT4_HIGHDRIVE
+  },
+  {
+    .timer = &pwm_servo_timers[0],
+    .ftm_channel = 0,
+    .pinconfig = PIN_PORTC | PIN1 | PIN_ALT4_OUTPUT | PIN_ALT4_HIGHDRIVE
+  },
+  {
+    .timer = &pwm_servo_timers[0],
+    .ftm_channel = 6,
+    .pinconfig = PIN_PORTD | PIN6 | PIN_ALT4_OUTPUT | PIN_ALT4_HIGHDRIVE
+  },
+  {
+    .timer = &pwm_servo_timers[0],
     .ftm_channel = 5,
-    .pinconfig = PIN_PORTD | PIN5 |  PIN_ALT4_OUTPUT | PIN_ALT4_HIGHDRIVE
+    .pinconfig = PIN_PORTD | PIN5 | PIN_ALT4_OUTPUT | PIN_ALT4_HIGHDRIVE
+  },
+    {
+    .timer = &pwm_servo_timers[0],
+    .ftm_channel = 2,
+    .pinconfig = PIN_PORTC | PIN3 | PIN_ALT4_OUTPUT | PIN_ALT4_HIGHDRIVE
+  },
+  {
+    .timer = &pwm_servo_timers[0],
+    .ftm_channel = 3,
+    .pinconfig = PIN_PORTC | PIN4 | PIN_ALT4_OUTPUT | PIN_ALT4_HIGHDRIVE
   }
 };
diff --git a/src/drivers/kinetis/drv_pwm_servo.c b/src/drivers/kinetis/drv_pwm_servo.c
index 7e7756983..e64a82764 100644
--- a/src/drivers/kinetis/drv_pwm_servo.c
+++ b/src/drivers/kinetis/drv_pwm_servo.c
@@ -81,7 +81,6 @@
        // configure pin
        kinetis_pinconfig(channel->pinconfig); 
      }
-
       
    }
 
@@ -167,6 +166,10 @@ up_pwm_servo_get_rate_group(unsigned group)
 int
 up_pwm_servo_set_rate_group_update(unsigned group, unsigned rate)
 {
+
+  if (group > PWM_SERVO_MAX_TIMERS) {
+    return -ERANGE;
+  }
   
   /*
    * ftm rate = bus frequency / prescaler
@@ -184,10 +187,6 @@ up_pwm_servo_set_rate_group_update(unsigned group, unsigned rate)
   }
     
   const struct pwm_servo_timer* timer = &pwm_servo_timers[group];
-
-  //  uint32_t regval = getreg32(timer->ftm_base + KINETIS_FTM_MODE_OFFSET);
-  //putreg32(regval | FTM_MODE_WPDIS, timer->ftm_base + KINETIS_FTM_MODE_OFFSET);
-
   
   uint32_t regval = getreg32(timer->ftm_base + KINETIS_FTM_SC_OFFSET);
   regval = regval & ~FTM_SC_PS_MASK;
@@ -211,6 +210,10 @@ up_pwm_servo_set_rate_group_update(unsigned group, unsigned rate)
 int
 up_pwm_servo_set(unsigned channel, servo_position_t value)
 {
+  if (channel > PWM_SERVO_MAX_CHANNELS) {
+    return -ERANGE;
+  }
+  
   const struct pwm_servo_channel* ch = &pwm_servo_channels[channel];
 
   putreg32(
@@ -229,6 +232,10 @@ up_pwm_servo_set(unsigned channel, servo_position_t value)
 servo_position_t
 up_pwm_servo_get(unsigned channel)
 {
+  if (channel > PWM_SERVO_MAX_CHANNELS) {
+    return -ERANGE;
+  }
+  
   const struct pwm_servo_channel* ch = &pwm_servo_channels[channel];
   uint32_t reg = ch->timer->ftm_base + KINETIS_FTM_CV_OFFSET(ch->ftm_channel);
   uint32_t mod = getreg32(reg);
diff --git a/src/drivers/kinetis/drv_pwm_servo.h b/src/drivers/kinetis/drv_pwm_servo.h
index 160b7aad7..d5b4afc3d 100644
--- a/src/drivers/kinetis/drv_pwm_servo.h
+++ b/src/drivers/kinetis/drv_pwm_servo.h
@@ -40,7 +40,7 @@
 #define KINETIS_PWM_SERVO_H
 
 #define PWM_SERVO_MAX_TIMERS 1
-#define PWM_SERVO_MAX_CHANNELS 1
+#define PWM_SERVO_MAX_CHANNELS 6
 
 #include <stdbool.h>
 #include <drivers/drv_pwm_output.h>