Merge pull request #1233 from masaohamanaka/master

RZ_A1H - Modify PWM driver for period improvement

libraries/mbed/targets/hal/TARGET_RENESAS/TARGET_RZ_A1H/PeripheralNames.h

@@ -55,7 +55,26 @@ typedef enum {
 } PWMType;
 
 typedef enum {
-    PWM0_PIN,
+    TIOC0A  = 0,
+    TIOC0B,
+    TIOC0C,
+    TIOC0D,
+    TIOC1A = 0x10,
+    TIOC1B,
+    TIOC2A = 0x20,
+    TIOC2B,
+    TIOC3A = 0x30,
+    TIOC3B,
+    TIOC3C,
+    TIOC3D,
+    TIOC4A = 0x40,
+    TIOC4B,
+    TIOC4C,
+    TIOC4D,
+} MTU2_PWMType;
+
+typedef enum {
+    PWM0_PIN  = 0,
     PWM1_PIN,
     PWM2_PIN,
     PWM3_PIN,
@@ -69,6 +88,28 @@ typedef enum {
     PWM11_PIN,
     PWM12_PIN,
     PWM13_PIN,
+    MTU2_PWM0_PIN = 0x20,
+    MTU2_PWM1_PIN,
+    MTU2_PWM2_PIN,
+    MTU2_PWM3_PIN,
+    MTU2_PWM4_PIN,
+    MTU2_PWM5_PIN,
+    MTU2_PWM6_PIN,
+    MTU2_PWM7_PIN,
+    MTU2_PWM8_PIN,
+    MTU2_PWM9_PIN,
+    MTU2_PWM10_PIN,
+    MTU2_PWM11_PIN,
+    MTU2_PWM12_PIN,
+    MTU2_PWM13_PIN,
+    MTU2_PWM14_PIN,
+    MTU2_PWM15_PIN,
+    MTU2_PWM16_PIN,
+    MTU2_PWM17_PIN,
+    MTU2_PWM18_PIN,
+    MTU2_PWM19_PIN,
+    MTU2_PWM20_PIN,
+    MTU2_PWM21_PIN,
 } PWMName;
 
 typedef enum {

libraries/mbed/targets/hal/TARGET_RENESAS/TARGET_RZ_A1H/pwmout_api.c

@@ -21,26 +21,52 @@
 #include "cpg_iodefine.h"
 #include "pwm_iodefine.h"
 
+#define MTU2_PWM_NUM            22
+#define MTU2_PWM_SIGNAL         2
+#define MTU2_PWM_OFFSET         0x20
+
 //  PORT ID, PWM ID, Pin function
 static const PinMap PinMap_PWM[] = {
-    {P4_4     , PWM0_PIN , 4},
+    {P2_1     , MTU2_PWM0_PIN  , 6},
-    {P3_2     , PWM1_PIN , 7},
+    {P2_11    , MTU2_PWM1_PIN  , 5},
-    {P4_6     , PWM2_PIN , 4},
+    {P3_8     , MTU2_PWM2_PIN  , 6},
-    {P4_7     , PWM3_PIN , 4},
+    {P3_10    , MTU2_PWM3_PIN  , 6},
-    {P8_14    , PWM4_PIN , 6},
+    {P4_0     , MTU2_PWM4_PIN  , 2},
-    {P8_15    , PWM5_PIN , 6},
+    {P4_4     , MTU2_PWM5_PIN  , 3},
-    {P8_13    , PWM6_PIN , 6},
+    {P4_6     , MTU2_PWM6_PIN  , 3},
-    {P8_11    , PWM7_PIN , 6},
+    {P5_0     , MTU2_PWM7_PIN  , 6},
-    {P8_8     , PWM8_PIN , 6},
+    {P5_3     , MTU2_PWM8_PIN  , 6},
-    {P10_0    , PWM9_PIN , 3},
+    {P5_5     , MTU2_PWM9_PIN  , 6},
-    {P8_12    , PWM10_PIN, 6},
+    {P7_2     , MTU2_PWM10_PIN , 7},
-    {P8_9     , PWM11_PIN, 6},
+    {P7_4     , MTU2_PWM11_PIN , 7},
-    {P8_10    , PWM12_PIN, 6},
+    {P7_6     , MTU2_PWM12_PIN , 7},
-    {P4_5     , PWM13_PIN, 4},
+    {P7_10    , MTU2_PWM13_PIN , 7},
-    {NC, NC, 0}
+    {P7_12    , MTU2_PWM14_PIN , 7},
+    {P7_14    , MTU2_PWM15_PIN , 7},
+    {P8_8     , MTU2_PWM16_PIN , 5},
+    {P8_10    , MTU2_PWM17_PIN , 4},
+    {P8_12    , MTU2_PWM18_PIN , 4},
+    {P8_14    , MTU2_PWM19_PIN , 4},
+    {P11_0    , MTU2_PWM20_PIN , 2},
+    {P11_2    , MTU2_PWM21_PIN , 2},
+    {P4_4     , PWM0_PIN       , 4},
+    {P3_2     , PWM1_PIN       , 7},
+    {P4_6     , PWM2_PIN       , 4},
+    {P4_7     , PWM3_PIN       , 4},
+    {P8_14    , PWM4_PIN       , 6},
+    {P8_15    , PWM5_PIN       , 6},
+    {P8_13    , PWM6_PIN       , 6},
+    {P8_11    , PWM7_PIN       , 6},
+    {P8_8     , PWM8_PIN       , 6},
+    {P10_0    , PWM9_PIN       , 3},
+    {P8_12    , PWM10_PIN      , 6},
+    {P8_9     , PWM11_PIN      , 6},
+    {P8_10    , PWM12_PIN      , 6},
+    {P4_5     , PWM13_PIN      , 4},
+    {NC       , NC             , 0}
 };
 
-static PWMType PORT[] = {
+static const PWMType PORT[] = {
      PWM2E,          // PWM0_PIN
      PWM2C,          // PWM1_PIN
      PWM2G,          // PWM2_PIN
@@ -57,6 +83,31 @@ static PWMType PORT[] = {
      PWM2F,          // PWM13_PIN
 };
 
+static const MTU2_PWMType MTU2_PORT[] = {
+     TIOC2A,         // MTU2_PWM0_PIN
+     TIOC1A,         // MTU2_PWM1_PIN
+     TIOC4A,         // MTU2_PWM2_PIN
+     TIOC4C,         // MTU2_PWM3_PIN
+     TIOC0A,         // MTU2_PWM4_PIN
+     TIOC4A,         // MTU2_PWM5_PIN
+     TIOC4C,         // MTU2_PWM6_PIN
+     TIOC0A,         // MTU2_PWM7_PIN
+     TIOC3C,         // MTU2_PWM8_PIN
+     TIOC0C,         // MTU2_PWM9_PIN
+     TIOC0C,         // MTU2_PWM10_PIN
+     TIOC1A,         // MTU2_PWM11_PIN
+     TIOC2A,         // MTU2_PWM12_PIN
+     TIOC3C,         // MTU2_PWM13_PIN
+     TIOC4A,         // MTU2_PWM14_PIN
+     TIOC4C,         // MTU2_PWM15_PIN
+     TIOC1A,         // MTU2_PWM16_PIN
+     TIOC3A,         // MTU2_PWM17_PIN
+     TIOC3C,         // MTU2_PWM18_PIN
+     TIOC2A,         // MTU2_PWM19_PIN
+     TIOC4A,         // MTU2_PWM20_PIN
+     TIOC4C,         // MTU2_PWM21_PIN
+};
+
 static __IO uint16_t *PWM_MATCH[] = {
     &PWMPWBFR_2E,    // PWM0_PIN
     &PWMPWBFR_2C,    // PWM1_PIN
@@ -74,40 +125,170 @@ static __IO uint16_t *PWM_MATCH[] = {
     &PWMPWBFR_2E,    // PWM13_PIN
 };
 
+static __IO uint16_t *MTU2_PWM_MATCH[MTU2_PWM_NUM][MTU2_PWM_SIGNAL] = {
+    { &MTU2TGRA_2, &MTU2TGRB_2 },       // MTU2_PWM0_PIN
+    { &MTU2TGRA_1, &MTU2TGRB_1 },       // MTU2_PWM1_PIN
+    { &MTU2TGRA_4, &MTU2TGRB_4 },       // MTU2_PWM2_PIN
+    { &MTU2TGRC_4, &MTU2TGRD_4 },       // MTU2_PWM3_PIN
+    { &MTU2TGRA_0, &MTU2TGRB_0 },       // MTU2_PWM4_PIN
+    { &MTU2TGRA_4, &MTU2TGRB_4 },       // MTU2_PWM5_PIN
+    { &MTU2TGRC_4, &MTU2TGRD_4 },       // MTU2_PWM6_PIN
+    { &MTU2TGRA_0, &MTU2TGRB_0 },       // MTU2_PWM7_PIN
+    { &MTU2TGRC_3, &MTU2TGRD_3 },       // MTU2_PWM8_PIN
+    { &MTU2TGRC_0, &MTU2TGRD_0 },       // MTU2_PWM9_PIN
+    { &MTU2TGRC_0, &MTU2TGRD_0 },       // MTU2_PWM10_PIN
+    { &MTU2TGRA_1, &MTU2TGRB_1 },       // MTU2_PWM11_PIN
+    { &MTU2TGRA_2, &MTU2TGRB_2 },       // MTU2_PWM12_PIN
+    { &MTU2TGRC_3, &MTU2TGRD_3 },       // MTU2_PWM13_PIN
+    { &MTU2TGRA_4, &MTU2TGRB_4 },       // MTU2_PWM14_PIN
+    { &MTU2TGRC_4, &MTU2TGRD_4 },       // MTU2_PWM15_PIN
+    { &MTU2TGRA_1, &MTU2TGRB_1 },       // MTU2_PWM16_PIN
+    { &MTU2TGRA_3, &MTU2TGRB_3 },       // MTU2_PWM17_PIN
+    { &MTU2TGRC_3, &MTU2TGRD_3 },       // MTU2_PWM18_PIN
+    { &MTU2TGRA_2, &MTU2TGRB_2 },       // MTU2_PWM19_PIN
+    { &MTU2TGRA_4, &MTU2TGRB_4 },       // MTU2_PWM20_PIN
+    { &MTU2TGRC_4, &MTU2TGRD_4 }        // MTU2_PWM21_PIN
+};
+
+static __IO uint8_t *TCR_MATCH[] = {
+    &MTU2TCR_0,
+    &MTU2TCR_1,
+    &MTU2TCR_2,
+    &MTU2TCR_3,
+    &MTU2TCR_4,
+};
+
+static __IO uint8_t *TIORH_MATCH[] = {
+    &MTU2TIORH_0,
+    &MTU2TIOR_1,
+    &MTU2TIOR_2,
+    &MTU2TIORH_3,
+    &MTU2TIORH_4,
+};
+
+static __IO uint8_t *TIORL_MATCH[] = {
+    &MTU2TIORL_0,
+    NULL,
+    NULL,
+    &MTU2TIORL_3,
+    &MTU2TIORL_4,
+};
+
+static __IO uint16_t *TGRA_MATCH[] = {
+    &MTU2TGRA_0,
+    &MTU2TGRA_1,
+    &MTU2TGRA_2,
+    &MTU2TGRA_3,
+    &MTU2TGRA_4,
+};
+
+static __IO uint16_t *TGRC_MATCH[] = {
+    &MTU2TGRC_0,
+    NULL,
+    NULL,
+    &MTU2TGRC_3,
+    &MTU2TGRC_4,
+};
+
+static __IO uint8_t *TMDR_MATCH[] = {
+    &MTU2TMDR_0,
+    &MTU2TMDR_1,
+    &MTU2TMDR_2,
+    &MTU2TMDR_3,
+    &MTU2TMDR_4,
+};
+
+static int MAX_PERIOD[] = {
+    125000,
+    503000,
+    2000000,
+    2000000,
+    2000000,
+};
+
+typedef enum {
+    MODE_PWM = 0,
+    MODE_MTU2
+} PWMmode;
+
+typedef enum {
+    MTU2_PULSE = 0,
+    MTU2_PERIOD
+} MTU2Signal;
+
+static int pwm_mode = MODE_PWM;
 static uint16_t init_period_ch1 = 0;
 static uint16_t init_period_ch2 = 0;
+static uint16_t init_mtu2_period_ch[5] = {0};
 static int32_t  period_ch1 = 1;
 static int32_t  period_ch2 = 1;
+static int32_t  mtu2_period_ch[5] = {1, 1, 1, 1, 1};
 
 void pwmout_init(pwmout_t* obj, PinName pin) {
     // determine the channel
     PWMName pwm = (PWMName)pinmap_peripheral(pin, PinMap_PWM);
     MBED_ASSERT(pwm != (PWMName)NC);
 
-    // power on
+    if (pwm >= MTU2_PWM_OFFSET) {
-    CPGSTBCR3 &= ~(1<<0);
+        /* PWM by MTU2 */
-
+        int tmp_pwm;
-    obj->pwm = pwm;
+        
-    if (((uint32_t)PORT[obj->pwm] & 0x00000010) != 0) {
+        pwm_mode = MODE_MTU2;
-        obj->ch  = 2;
+        // power on
-        PWMPWPR_2_BYTE_L = 0x00;
+        CPGSTBCR3 &= ~(CPG_STBCR3_BIT_MSTP33);
+        
+        obj->pwm = pwm;
+        tmp_pwm = (int)(obj->pwm - MTU2_PWM_OFFSET);
+        if (((uint32_t)MTU2_PORT[tmp_pwm] & 0x00000040) == 0x00000040) {
+            obj->ch  = 4;
+            MTU2TOER |= 0x36;
+        } else if (((uint32_t)MTU2_PORT[tmp_pwm] & 0x00000030) == 0x00000030) {
+            obj->ch  = 3;
+            MTU2TOER |= 0x09;
+        } else if (((uint32_t)MTU2_PORT[tmp_pwm] & 0x00000020) == 0x00000020) {
+            obj->ch  = 2;
+        } else if (((uint32_t)MTU2_PORT[tmp_pwm] & 0x00000010) == 0x00000010) {
+            obj->ch  = 1;
+        } else {
+            obj->ch  = 0;
+        }
+        // Wire pinout
+        pinmap_pinout(pin, PinMap_PWM);
+        // default duty 0.0f
+        pwmout_write(obj, 0);
+        if (init_mtu2_period_ch[obj->ch] == 0) {
+            // default period 1ms
+            pwmout_period_us(obj, 1000);
+            init_mtu2_period_ch[obj->ch] = 1;
+        }
     } else {
-        obj->ch  = 1;
+        /* PWM */
-        PWMPWPR_1_BYTE_L = 0x00;
+        pwm_mode = MODE_PWM;
-    }
+        // power on
+        CPGSTBCR3 &= ~(CPG_STBCR3_BIT_MSTP30);
 
-    // Wire pinout
+        obj->pwm = pwm;
-    pinmap_pinout(pin, PinMap_PWM);
+        if (((uint32_t)PORT[obj->pwm] & 0x00000010) == 0x00000010) {
+            obj->ch  = 2;
+            PWMPWPR_2_BYTE_L = 0x00;
+        } else {
+            obj->ch  = 1;
+            PWMPWPR_1_BYTE_L = 0x00;
+        }
 
-    // default to 491us: standard for servos, and fine for e.g. brightness control
+        // Wire pinout
-    pwmout_write(obj, 0);
+        pinmap_pinout(pin, PinMap_PWM);
-    if ((obj->ch == 2) && (init_period_ch2 == 0)) {
+
-        pwmout_period_us(obj, 491);
+        // default to 491us: standard for servos, and fine for e.g. brightness control
-        init_period_ch2 = 1;
+        pwmout_write(obj, 0);
-    }
+        if ((obj->ch == 2) && (init_period_ch2 == 0)) {
-    if ((obj->ch == 1) && (init_period_ch1 == 0)) {
+            pwmout_period_us(obj, 491);
-        pwmout_period_us(obj, 491);
+            init_period_ch2 = 1;
-        init_period_ch1 = 1;
+        }
+        if ((obj->ch == 1) && (init_period_ch1 == 0)) {
+            pwmout_period_us(obj, 491);
+            init_period_ch1 = 1;
+        }
     }
 }
 
@@ -119,35 +300,65 @@ void pwmout_write(pwmout_t* obj, float value) {
     uint32_t wk_cycle;
     uint16_t v;
 
-    if (value < 0.0f) {
+    if (pwm_mode == MODE_MTU2) {
-        value = 0.0f;
+        /* PWM by MTU2 */
-    } else if (value > 1.0f) {
+        int tmp_pwm;
-        value = 1.0f;
+        
+        if (value < 0.0f) {
+            value = 0.0f;
+        } else if (value > 1.0f) {
+            value = 1.0f;
+        } else {
+            // Do Nothing
+        }
+        tmp_pwm = (int)(obj->pwm - MTU2_PWM_OFFSET);
+        wk_cycle = *MTU2_PWM_MATCH[tmp_pwm][MTU2_PERIOD] & 0xffff;
+        // set channel match to percentage
+        *MTU2_PWM_MATCH[tmp_pwm][MTU2_PULSE] = (uint16_t)((float)wk_cycle * value);
     } else {
-        // Do Nothing
+        /* PWM */
-    }
+        if (value < 0.0f) {
+            value = 0.0f;
+        } else if (value > 1.0f) {
+            value = 1.0f;
+        } else {
+            // Do Nothing
+        }
 
-    if (obj->ch == 2) {
+        if (obj->ch == 2) {
-        wk_cycle = PWMPWCYR_2 & 0x03ff;
+            wk_cycle = PWMPWCYR_2 & 0x03ff;
-    } else {
+        } else {
-        wk_cycle = PWMPWCYR_1 & 0x03ff;
+            wk_cycle = PWMPWCYR_1 & 0x03ff;
-    }
+        }
 
-    // set channel match to percentage
+        // set channel match to percentage
-    v = (uint16_t)((float)wk_cycle * value);
+        v = (uint16_t)((float)wk_cycle * value);
-    *PWM_MATCH[obj->pwm] = (v | ((PORT[obj->pwm] & 1) << 12));
+        *PWM_MATCH[obj->pwm] = (v | ((PORT[obj->pwm] & 1) << 12));
+    }
 }
 
 float pwmout_read(pwmout_t* obj) {
     uint32_t wk_cycle;
     float value;
 
-    if (obj->ch == 2) {
+    if (pwm_mode == MODE_MTU2) {
-        wk_cycle = PWMPWCYR_2 & 0x03ff;
+        /* PWM by MTU2 */
+        uint32_t wk_pulse;
+        int tmp_pwm;
+        
+        tmp_pwm = (int)(obj->pwm - MTU2_PWM_OFFSET);
+        wk_cycle = *MTU2_PWM_MATCH[tmp_pwm][MTU2_PERIOD] & 0xffff;
+        wk_pulse = *MTU2_PWM_MATCH[tmp_pwm][MTU2_PULSE] & 0xffff;
+        value = ((float)wk_pulse / (float)wk_cycle);
     } else {
-        wk_cycle = PWMPWCYR_1 & 0x03ff;
+        /* PWM */
+        if (obj->ch == 2) {
+            wk_cycle = PWMPWCYR_2 & 0x03ff;
+        } else {
+            wk_cycle = PWMPWCYR_1 & 0x03ff;
+        }
+        value = ((float)(*PWM_MATCH[obj->pwm] & 0x03ff) / (float)wk_cycle);
     }
-    value = ((float)(*PWM_MATCH[obj->pwm] & 0x03ff) / (float)wk_cycle);
 
     return (value > 1.0f) ? (1.0f) : (value);
 }
@@ -171,66 +382,154 @@ static void set_duty_again(__IO uint16_t *p_pwmpbfr, uint16_t last_cycle, uint16
     *p_pwmpbfr = (v | (wk_pwmpbfr & 0x1000));
 }
 
+static void set_mtu2_duty_again(__IO uint16_t *p_pwmpbfr, uint16_t last_cycle, uint16_t new_cycle){
+    uint16_t wk_pwmpbfr;
+    float    value;
+
+    wk_pwmpbfr = *p_pwmpbfr;
+    value      = ((float)(wk_pwmpbfr & 0xffff) / (float)last_cycle);
+    *p_pwmpbfr = (uint16_t)((float)new_cycle * value);
+}
+
 // Set the PWM period, keeping the duty cycle the same.
 void pwmout_period_us(pwmout_t* obj, int us) {
+    uint64_t wk_cycle_mtu2;
     uint32_t pclk_base;
     uint32_t wk_cycle;
-    uint16_t wk_last_cycle;
     uint32_t wk_cks = 0;
+    uint16_t wk_last_cycle;
+    int      max_us = 0;
 
-    if (us > 491) {
+    if (pwm_mode == MODE_MTU2) {
-        us = 491;
+        /* PWM by MTU2 */
-    } else if (us < 1) {
+        int tmp_pwm;
-        us = 1;
+        uint16_t tmp_tgra;
-    } else {
+        uint16_t tmp_tgrc;
-        // Do Nothing
+        uint8_t tmp_tcr_up;
-    }
+        uint8_t tmp_tstr_sp;
+        uint8_t tmp_tstr_st;
+        
+        max_us = MAX_PERIOD[obj->ch];
+        if (us > max_us) {
+            us = max_us;
+        } else if (us < 1) {
+            us = 1;
+        } else {
+            // Do Nothing
+        }
 
-    if (RZ_A1_IsClockMode0() == false) {
+        if (RZ_A1_IsClockMode0() == false) {
-        pclk_base = (uint32_t)CM1_RENESAS_RZ_A1_P0_CLK / 10000;
+            pclk_base = (uint32_t)CM1_RENESAS_RZ_A1_P0_CLK;
-    } else {
+        } else {
-        pclk_base = (uint32_t)CM0_RENESAS_RZ_A1_P0_CLK / 10000;
+            pclk_base = (uint32_t)CM0_RENESAS_RZ_A1_P0_CLK;
-    }
+        }
 
-    wk_cycle = pclk_base * us;
+        wk_cycle_mtu2 = (uint64_t)pclk_base * us;
-    while (wk_cycle >= 102350) {
+        while (wk_cycle_mtu2 >= 65535000000) {
-        wk_cycle >>= 1;
+            if ((obj->ch == 1) && (wk_cks == 3)) {
-        wk_cks++;
+                wk_cks+=2;
-    }
+            } else if ((obj->ch == 2) && (wk_cks == 3)) {
-    wk_cycle = (wk_cycle + 50) / 100;
+                wk_cycle_mtu2 >>= 2;
+                wk_cks+=3;
+            }
+            wk_cycle_mtu2 >>= 2;
+            wk_cks++;
+        }
+        wk_cycle = (uint32_t)(wk_cycle_mtu2 / 1000000);
+        
+        tmp_pwm = (int)(obj->pwm - MTU2_PWM_OFFSET);
+        if (((uint8_t)MTU2_PORT[tmp_pwm] & 0x02) == 0x02) {
+            tmp_tcr_up = 0xC0;
+        } else {
+            tmp_tcr_up = 0x40;
+        }
+        if ((obj->ch == 4) || (obj->ch == 3)) {
+            tmp_tstr_sp = ~(0x38 | (1 << (obj->ch + 3)));
+            tmp_tstr_st = (1 << (obj->ch + 3));
+        } else {
+            tmp_tstr_sp = ~(0x38 | (1 << obj->ch));
+            tmp_tstr_st = (1 << obj->ch);
+        }
+        // Counter Stop
+        MTU2TSTR &= tmp_tstr_sp;
+        wk_last_cycle = *MTU2_PWM_MATCH[tmp_pwm][MTU2_PERIOD] & 0xffff;
+        *TCR_MATCH[obj->ch] = tmp_tcr_up | wk_cks;
+        *TIORH_MATCH[obj->ch] = 0x21;
+        if ((obj->ch == 0) || (obj->ch == 3) || (obj->ch == 4)) {
+            *TIORL_MATCH[obj->ch] = 0x21;
+        }
+        *MTU2_PWM_MATCH[tmp_pwm][MTU2_PERIOD] = (uint16_t)wk_cycle;     // Set period
 
-    if (obj->ch == 2) {
+        // Set duty again(TGRA)
-        wk_last_cycle    = PWMPWCYR_2 & 0x03ff;
+        tmp_tgra = *TGRA_MATCH[obj->ch];
-        PWMPWCR_2_BYTE_L = 0xc0 | wk_cks;
+        set_mtu2_duty_again(&tmp_tgra, wk_last_cycle, wk_cycle);
-        PWMPWCYR_2       = (uint16_t)wk_cycle;
+        if ((obj->ch == 0) || (obj->ch == 3) || (obj->ch == 4)) {
-
+            // Set duty again(TGRC)
-        // Set duty again
+            tmp_tgrc = *TGRC_MATCH[obj->ch];
-        set_duty_again(&PWMPWBFR_2A, wk_last_cycle, wk_cycle);
+            set_mtu2_duty_again(&tmp_tgrc, wk_last_cycle, wk_cycle);
-        set_duty_again(&PWMPWBFR_2C, wk_last_cycle, wk_cycle);
+        }
-        set_duty_again(&PWMPWBFR_2E, wk_last_cycle, wk_cycle);
+        *TMDR_MATCH[obj->ch] = 0x02;                                    // PWM mode 1
-        set_duty_again(&PWMPWBFR_2G, wk_last_cycle, wk_cycle);
 
         // Counter Start
-        PWMPWCR_2_BYTE_L |= 0x08;
+        MTU2TSTR |= tmp_tstr_st;
-
         // Save for future use
-        period_ch2 = us;
+        mtu2_period_ch[obj->ch] = us;
     } else {
-        wk_last_cycle    = PWMPWCYR_1 & 0x03ff;
+        /* PWM */
-        PWMPWCR_1_BYTE_L = 0xc0 | wk_cks;
+        if (us > 491) {
-        PWMPWCYR_1       = (uint16_t)wk_cycle;
+            us = 491;
+        } else if (us < 1) {
+            us = 1;
+        } else {
+            // Do Nothing
+        }
 
-        // Set duty again
+        if (RZ_A1_IsClockMode0() == false) {
-        set_duty_again(&PWMPWBFR_1A, wk_last_cycle, wk_cycle);
+            pclk_base = (uint32_t)CM1_RENESAS_RZ_A1_P0_CLK / 10000;
-        set_duty_again(&PWMPWBFR_1C, wk_last_cycle, wk_cycle);
+        } else {
-        set_duty_again(&PWMPWBFR_1E, wk_last_cycle, wk_cycle);
+            pclk_base = (uint32_t)CM0_RENESAS_RZ_A1_P0_CLK / 10000;
-        set_duty_again(&PWMPWBFR_1G, wk_last_cycle, wk_cycle);
+        }
 
-        // Counter Start
+        wk_cycle = pclk_base * us;
-        PWMPWCR_1_BYTE_L |= 0x08;
+        while (wk_cycle >= 102350) {
+            wk_cycle >>= 1;
+            wk_cks++;
+        }
+        wk_cycle = (wk_cycle + 50) / 100;
 
-        // Save for future use
+        if (obj->ch == 2) {
-        period_ch1 = us;
+            wk_last_cycle    = PWMPWCYR_2 & 0x03ff;
+            PWMPWCR_2_BYTE_L = 0xc0 | wk_cks;
+            PWMPWCYR_2       = (uint16_t)wk_cycle;
+
+            // Set duty again
+            set_duty_again(&PWMPWBFR_2A, wk_last_cycle, wk_cycle);
+            set_duty_again(&PWMPWBFR_2C, wk_last_cycle, wk_cycle);
+            set_duty_again(&PWMPWBFR_2E, wk_last_cycle, wk_cycle);
+            set_duty_again(&PWMPWBFR_2G, wk_last_cycle, wk_cycle);
+
+            // Counter Start
+            PWMPWCR_2_BYTE_L |= 0x08;
+
+            // Save for future use
+            period_ch2 = us;
+        } else {
+            wk_last_cycle    = PWMPWCYR_1 & 0x03ff;
+            PWMPWCR_1_BYTE_L = 0xc0 | wk_cks;
+            PWMPWCYR_1       = (uint16_t)wk_cycle;
+
+            // Set duty again
+            set_duty_again(&PWMPWBFR_1A, wk_last_cycle, wk_cycle);
+            set_duty_again(&PWMPWBFR_1C, wk_last_cycle, wk_cycle);
+            set_duty_again(&PWMPWBFR_1E, wk_last_cycle, wk_cycle);
+            set_duty_again(&PWMPWBFR_1G, wk_last_cycle, wk_cycle);
+
+            // Counter Start
+            PWMPWCR_1_BYTE_L |= 0x08;
+
+            // Save for future use
+            period_ch1 = us;
+        }
     }
 }
 
@@ -245,15 +544,23 @@ void pwmout_pulsewidth_ms(pwmout_t* obj, int ms) {
 void pwmout_pulsewidth_us(pwmout_t* obj, int us) {
     float value = 0;
 
-    if (obj->ch == 2) {
+    if (pwm_mode == MODE_MTU2) {
-        if (period_ch2 != 0) {
+        /* PWM by MTU2 */
-            value = (float)us / (float)period_ch2;
+        if (mtu2_period_ch[obj->ch] != 0) {
+            value = (float)us / (float)mtu2_period_ch[obj->ch];
         }
     } else {
-        if (period_ch1 != 0) {
+        /* PWM */
-            value = (float)us / (float)period_ch1;
+        if (obj->ch == 2) {
+            if (period_ch2 != 0) {
+                value = (float)us / (float)period_ch2;
+            }
+        } else {
+            if (period_ch1 != 0) {
+                value = (float)us / (float)period_ch1;
+            }
         }
-    }
 
-    pwmout_write(obj, value);
+        pwmout_write(obj, value);
+    }
 }