+ if (pxmcc->ccflg) {
+ int32_t pwm_alp;
+ int32_t pwm_bet;
+ int32_t pwm_bet_div_2_k3;
+ uint32_t phs;
+ uint32_t pwm1;
+ uint32_t pwm2;
+ uint32_t pwm3;
+
+ pwm_alp = pxmcc->pwm_d * pxmcc->ptcos + pxmcc->pwm_q * pxmcc->ptsin;
+ pwm_bet = pxmcc->pwm_d * pxmcc->ptsin - pxmcc->pwm_q * pxmcc->ptcos;
+
+ pwm_bet_div_2_k3 = RECI16_2_K3 * (pwm_bet >> 16);
+
+ if (pwm_bet > 0)
+ if (pwm_alp > 0)
+ /* pwm_bet > 2 * k3 * pwm_alp */
+ if (pwm_bet_div_2_k3 > pwm_alp)
+ phs = 1;
+ else
+ phs = 0;
+ else
+ /* -pwm_bet > 2 * k3 * pwm_alp */
+ if (pwm_bet_div_2_k3 < -pwm_alp)
+ phs = 2;
+ else
+ phs = 1;
+ else
+ if (pwm_alp > 0)
+ /* pwm_bet > -2 * k3 * pwm_alp */
+ if (pwm_bet_div_2_k3 > -pwm_alp)
+ phs = 5;
+ else
+ phs = 4;
+ else
+ /* pwm_bet > 2 * k3 * u_alp */
+ if (pwm_bet_div_2_k3 > pwm_alp)
+ phs = 3;
+ else
+ phs = 4;
+
+ if (phs <= 1) {
+ /* pwm1 = pwm_alp + 1.0/(2.0*k3) * pwm_bet */
+ pwm1 = (pwm_alp + pwm_bet_div_2_k3) >> 16;
+ /* pwm2 = 1/k3 * pwm_bet */
+ pwm2 = pwm_bet_div_2_k3 >> 15;
+ pwm3 = 0;
+ } else if (phs <= 3) {
+ pwm1 = 0;
+ /* pwm2 = 1.0/(2.0*k3) * pwm_bet - pwm_alp */
+ pwm2 = (pwm_bet_div_2_k3 - pwm_alp) >> 16;
+ /* pwm3 = -1.0/(2.0*k3) * pwm_bet - pwm_alp */
+ pwm3 = (-pwm_bet_div_2_k3 - pwm_alp) >>16;
+ } else {
+ /* pwm1 = pwm_alp - 1.0/(2.0*k3) * pwm_bet */
+ pwm1 = (pwm_alp - pwm_bet_div_2_k3) >>16;
+ pwm2 = 0;
+ /* pwm3 = -1/k3 * pwm_bet */
+ pwm3 = -pwm_bet_div_2_k3 >> 15;
+ }
+
+ pxmcc->ptphs = phs;
+
+ *FPGA_LX_MASTER_TX_PWM0 = pwm2 | 0x4000;
+ *FPGA_LX_MASTER_TX_PWM1 = pwm3 | 0x4000;
+ *FPGA_LX_MASTER_TX_PWM2 = pwm1 | 0x4000;
+ }
+