hw/lx-rocon_firmware/firmware.c

   1 /*******************************************************************
   2   Components for embedded applications builded for
   3   laboratory and medical instruments firmware
   4
   5   firmware.c - multi axis motion controller coprocessor
   6                firmware for FPGA tumble CPU of lx-rocon system
   7
   8   (C) 2001-2014 by Pavel Pisa pisa@cmp.felk.cvut.cz
   9   (C) 2002-2014 by PiKRON Ltd. http://www.pikron.com
  10
  11   This file can be used and copied according to next
  12   license alternatives
  13    - GPL - GNU Public License
  14    - other license provided by project originators
  15
  16  *******************************************************************/
  17
  18 #define SUPPRESS_CONDITIONALS 1
  19 #undef  COMPUTE_PHASE_SECTOR
  20
  21 #include <stdint.h>
  22 #include "pxmcc_types.h"
  23 #include "tumbl_addr.h"
  24
  25 /* k3 = math.sqrt(3) / 2 */
  26
  27 #define CONST16_K3   56756
  28
  29 /* 1 / k3 * 65536 */
  30
  31 #define RECI16_K3    75674
  32
  33 /* 1 / (2 * k3) * 65536 */
  34 #define RECI16_2_K3  37837
  35
  36 #define offsbychar(_ptr, _offs) \
  37    ((typeof(&(_ptr)[0]))((char*)(_ptr) + (_offs)))
  38
  39 pxmcc_data_t pxmcc_data;
  40
  41 void init_defvals(void)
  42 {
  43 }
  44
  45 #if 0
  46
  47 uint32_t sin_lat[7];
  48
  49 void find_sin_lat(void)
  50 {
  51   int i;
  52   register uint32_t a0, a1, a2, a3, a4, a5;
  53
  54   *FPGA_FNCAPPROX_SIN = 0;
  55
  56   for (i = 0; i < 20; i++)
  57     asm volatile("": : : "memory");
  58
  59   *FPGA_FNCAPPROX_SIN = 0x40000000;
  60   a0 = *FPGA_FNCAPPROX_SIN;
  61   a1 = *FPGA_FNCAPPROX_SIN;
  62   a2 = *FPGA_FNCAPPROX_SIN;
  63   a3 = *FPGA_FNCAPPROX_SIN;
  64   a4 = *FPGA_FNCAPPROX_SIN;
  65   a5 = *FPGA_FNCAPPROX_SIN;
  66   asm volatile("": : : "memory");
  67
  68   sin_lat[0] = a0;
  69   sin_lat[1] = a1;
  70   sin_lat[2] = a2;
  71   sin_lat[3] = a3;
  72   sin_lat[4] = a4;
  73   sin_lat[5] = a5;
  74   sin_lat[6] = 0x4321;
  75   sin_lat[0] = 0x1234;
  76 }
  77
  78 #endif
  79
  80 void main(void)
  81 {
  82   uint32_t last_rx_done_sqn = 0;
  83   pxmcc_axis_data_t *pxmcc;
  84
  85   pxmcc_data.common.pwm_cycle = 2500;
  86   pxmcc_data.common.min_idle = 0x7fff;
  87   pxmcc_data.common.irc_base = (uint32_t)FPGA_IRC0;
  88   pxmcc_data.curadc[0].siroladc_offs = 0x0c17;
  89   pxmcc_data.curadc[1].siroladc_offs = 0x0c66;
  90   pxmcc_data.curadc[2].siroladc_offs = 0x0c66;
  91   pxmcc_data.curadc[3].siroladc_offs = 0x0c66;
  92
  93   pxmcc = pxmcc_data.axis;
  94   do {
  95     pxmcc->ccflg  = 0;
  96     pxmcc->mode   = PXMCC_MODE_IDLE;
  97     pxmcc->ptirc  = 1000;
  98     pxmcc->ptreci = 4294967; /* (1LL<<32)*ptper/ptirc */
  99     pxmcc->pwm_dq = 0;
 100     pxmcc++;
 101   } while(pxmcc != pxmcc_data.axis + PXMCC_AXIS_COUNT);
 102
 103   asm volatile("": : : "memory");
 104   pxmcc_data.common.fwversion = PXMCC_FWVERSION;
 105   asm volatile("": : : "memory");
 106
 107  #if 0
 108   find_sin_lat();
 109  #endif
 110
 111   while (1) {
 112     pxmcc = pxmcc_data.axis;
 113     do {
 114       uint32_t irc;
 115       uint32_t ofs = pxmcc->ptofs;
 116       uint32_t per = pxmcc->ptirc;
 117       int32_t  pti;
 118       uint32_t pta;
 119       uint32_t pwmtx_info;
 120       volatile uint32_t *uptr;
 121
 122       irc = *(uint32_t*)pxmcc->inp_info;
 123
 124       pti = irc - ofs;
 125       /*
 126        * the '>=' is appropriate to keep pti in <0;per-1> range,
 127        * but sine and cosine computations can work over multiple
 128        * periods and value of per=0xffffffff allows to control/stop
 129        * updates if only '>' is used.
 130        */
 131       if ((uint32_t)pti > per) {
 132         if (pti < 0) {
 133           ofs -= per;
 134         } else {
 135           ofs += per;
 136         }
 137         pti = irc - ofs;
 138         pxmcc->ptofs = ofs;
 139       }
 140       pxmcc->ptindx = pti;
 141
 142       pta = pti * pxmcc->ptreci;
 143
 144       *FPGA_FNCAPPROX_SIN = pta;
 145       asm volatile("nop\n");
 146       asm volatile("nop\n");
 147       asm volatile("nop\n");
 148       pxmcc->ptsin = *FPGA_FNCAPPROX_SIN;
 149       asm volatile("nop\n");
 150       pxmcc->ptcos = *FPGA_FNCAPPROX_COS;
 151
 152       if (pxmcc->ccflg) {
 153         int32_t pwm_alp;
 154         int32_t pwm_bet;
 155         int32_t pwm_bet_div_2_k3;
 156         uint32_t pwm1;
 157         uint32_t pwm2;
 158         uint32_t pwm3;
 159         uint32_t pwm4;
 160         int32_t pwm_d;
 161         int32_t pwm_q;
 162        #if defined(COMPUTE_PHASE_SECTOR) || !defined(SUPPRESS_CONDITIONALS)
 163         uint32_t phs;
 164        #endif /*COMPUTE_PHASE_SECTOR*/
 165
 166         pwm_d = (volatile uint32_t)pxmcc->pwm_dq;
 167         pwm_q = (pwm_d << 16) >> 16;
 168         pwm_d >>= 16;
 169
 170         pwm_alp = pwm_d * pxmcc->ptcos - pwm_q * pxmcc->ptsin;
 171         pwm_bet = pwm_d * pxmcc->ptsin + pwm_q * pxmcc->ptcos;
 172
 173         if (pxmcc->mode == PXMCC_MODE_BLDC) {
 174           pwm_bet_div_2_k3 = RECI16_2_K3 * (pwm_bet >> 16);
 175
 176          #ifndef SUPPRESS_CONDITIONALS
 177           if (pwm_bet > 0)
 178             if (pwm_alp > 0)
 179             /* pwm_bet > 2 * k3 * pwm_alp */
 180               if (pwm_bet_div_2_k3 > pwm_alp)
 181                 phs = 1;
 182               else
 183                 phs = 0;
 184             else
 185               /* -pwm_bet > 2 * k3 * pwm_alp */
 186               if (pwm_bet_div_2_k3 < -pwm_alp)
 187                 phs = 2;
 188               else
 189                 phs = 1;
 190           else
 191             if (pwm_alp > 0)
 192               /* pwm_bet > -2 * k3 * pwm_alp */
 193               if (pwm_bet_div_2_k3 > -pwm_alp)
 194                 phs = 5;
 195               else
 196                 phs = 4;
 197             else
 198               /* pwm_bet > 2 * k3 * u_alp */
 199               if (pwm_bet_div_2_k3 > pwm_alp)
 200                 phs = 3;
 201               else
 202                 phs = 4;
 203
 204           if (phs <= 1) {
 205             /* pwm1 = pwm_alp + 1.0/(2.0*k3) * pwm_bet */
 206             pwm1 = (pwm_alp + pwm_bet_div_2_k3) >> 16;
 207             /* pwm2 = 1/k3 * pwm_bet */
 208             pwm2 = pwm_bet_div_2_k3 >> 15;
 209             pwm3 = 0;
 210           } else if (phs <= 3) {
 211             pwm1 = 0;
 212             /* pwm2 = 1.0/(2.0*k3) * pwm_bet - pwm_alp */
 213             pwm2 = (pwm_bet_div_2_k3 - pwm_alp) >> 16;
 214             /* pwm3 = -1.0/(2.0*k3) * pwm_bet - pwm_alp */
 215             pwm3 = (-pwm_bet_div_2_k3 - pwm_alp) >>16;
 216           } else {
 217             /* pwm1 = pwm_alp - 1.0/(2.0*k3) * pwm_bet */
 218             pwm1 = (pwm_alp - pwm_bet_div_2_k3) >>16;
 219             pwm2 = 0;
 220             /* pwm3 = -1/k3 * pwm_bet */
 221             pwm3 = -pwm_bet_div_2_k3 >> 15;
 222           }
 223          #else /*SUPPRESS_CONDITIONALS*/
 224           {
 225             int32_t alp_m_bet_d2k3;
 226             uint32_t state23_msk;
 227             uint32_t pwm23_shift;
 228             uint32_t bet_sgn = pwm_bet >> 31;
 229             uint32_t alp_sgn = pwm_alp >> 31;
 230            #ifdef COMPUTE_PHASE_SECTOR
 231             uint32_t bet_sgn_cpl = ~bet_sgn;
 232            #endif /*COMPUTE_PHASE_SECTOR*/
 233
 234             alp_m_bet_d2k3 = (alp_sgn ^ pwm_alp) - (bet_sgn ^ (pwm_bet_div_2_k3 + bet_sgn));
 235             alp_m_bet_d2k3 = alp_m_bet_d2k3 >> 31;
 236
 237             state23_msk = alp_sgn & ~alp_m_bet_d2k3;
 238
 239             /*
 240              *   bet alp amb s23
 241              *    0   0   0   0 -> 0 (000)
 242              *    0   0  -1   0 -> 1 (001)
 243              *   -1   0  -1   0 -> 1 (001)
 244              *   -1   0   0  -1 -> 2 (010)
 245              *   -1  -1   0  -1 -> 3 (011)
 246              *   -1  -1  -1   0 -> 4 (100)
 247              *    0  -1  -1   0 -> 4 (100)
 248              *    0  -1   0   0 -> 5 (101)
 249              */
 250
 251            #ifdef COMPUTE_PHASE_SECTOR
 252             phs = (bet_sgn & 5) + (bet_sgn_cpl ^ ((alp_m_bet_d2k3 + 2 * state23_msk) + bet_sgn_cpl));
 253            #endif /*COMPUTE_PHASE_SECTOR*/
 254
 255             pwm1 = pwm_alp & state23_msk;
 256             pwm2 = pwm_bet_div_2_k3 - pwm1;
 257             pwm3 = -pwm_bet_div_2_k3 - pwm1;
 258             pwm2 &= (~bet_sgn | state23_msk);
 259             pwm3 &= (bet_sgn | state23_msk);
 260             pwm1 = pwm_alp + pwm2 + pwm3;
 261             pwm1 &= ~state23_msk;
 262             pwm1 >>= 16;
 263             pwm23_shift = 15 - state23_msk;
 264             pwm2 >>= pwm23_shift;
 265             pwm3 >>= pwm23_shift;
 266           }
 267          #endif /*SUPPRESS_CONDITIONALS*/
 268
 269          #ifdef COMPUTE_PHASE_SECTOR
 270           pxmcc->ptphs = phs;
 271          #endif /*COMPUTE_PHASE_SECTOR*/
 272
 273          #if 0
 274           *FPGA_LX_MASTER_TX_PWM0 = pwm2 | 0x4000;
 275           *FPGA_LX_MASTER_TX_PWM1 = pwm3 | 0x4000;
 276           *FPGA_LX_MASTER_TX_PWM2 = pwm1 | 0x4000;
 277          #else
 278           pwmtx_info = pxmcc->pwmtx_info;
 279           uptr = FPGA_LX_MASTER_TX + ((pwmtx_info >>  0) & 0xff);
 280           pxmcc->pwm_prew[1] = *uptr & 0x3fff;
 281           *uptr = pwm2 | 0x4000;
 282           uptr = FPGA_LX_MASTER_TX + ((pwmtx_info >>  8) & 0xff);
 283           pxmcc->pwm_prew[2] = *uptr & 0x3fff;
 284           *uptr = pwm3 | 0x4000;
 285           uptr = FPGA_LX_MASTER_TX + ((pwmtx_info >> 16) & 0xff);
 286           pxmcc->pwm_prew[0] = *uptr & 0x3fff;
 287           *uptr = pwm1 | 0x4000;
 288          #endif
 289         } else {
 290           uint32_t alp_sgn = pwm_alp >> 31;
 291           uint32_t bet_sgn = pwm_bet >> 31;
 292           pwm_alp >>= 16;
 293           pwm_bet >>= 16;
 294           pwm1 = -pwm_alp & alp_sgn;
 295           pwm2 = pwm_alp & ~alp_sgn;
 296           pwm3 = -pwm_bet & bet_sgn;
 297           pwm4 = pwm_bet & ~bet_sgn;
 298
 299           pwmtx_info = pxmcc->pwmtx_info;
 300           uptr = FPGA_LX_MASTER_TX + ((pwmtx_info >>  0) & 0xff);
 301           pxmcc->pwm_prew[0] = *uptr & 0x3fff;
 302           *uptr = pwm1 | 0x4000;
 303           uptr = FPGA_LX_MASTER_TX + ((pwmtx_info >>  8) & 0xff);
 304           pxmcc->pwm_prew[1] = *uptr & 0x3fff;
 305           *uptr = pwm2 | 0x4000;
 306           uptr = FPGA_LX_MASTER_TX + ((pwmtx_info >> 16) & 0xff);
 307           pxmcc->pwm_prew[2] = *uptr & 0x3fff;
 308           *uptr = pwm3 | 0x4000;
 309           uptr = FPGA_LX_MASTER_TX + ((pwmtx_info >> 24) & 0xff);
 310           pxmcc->pwm_prew[3] = *uptr & 0x3fff;
 311           *uptr = pwm4 | 0x4000;
 312           if (pxmcc->mode == PXMCC_MODE_STEPPER) {
 313             if (pxmcc->steps_cnt != pxmcc->steps_lim) {
 314               pxmcc->steps_cnt++;
 315               pxmcc->steps_pos += pxmcc->steps_inc;
 316             }
 317           }
 318         }
 319       } else {
 320         if (pxmcc->mode == PXMCC_MODE_BLDC) {
 321           pwmtx_info = pxmcc->pwmtx_info;
 322           uptr = FPGA_LX_MASTER_TX + ((pwmtx_info >>  0) & 0xff);
 323           pxmcc->pwm_prew[1] = *uptr & 0x3fff;
 324           uptr = FPGA_LX_MASTER_TX + ((pwmtx_info >>  8) & 0xff);
 325           pxmcc->pwm_prew[2] = *uptr & 0x3fff;
 326           uptr = FPGA_LX_MASTER_TX + ((pwmtx_info >> 16) & 0xff);
 327           pxmcc->pwm_prew[0] = *uptr & 0x3fff;
 328         } else {
 329           pwmtx_info = pxmcc->pwmtx_info;
 330           uptr = FPGA_LX_MASTER_TX + ((pwmtx_info >>  0) & 0xff);
 331           pxmcc->pwm_prew[0] = *uptr & 0x3fff;
 332           uptr = FPGA_LX_MASTER_TX + ((pwmtx_info >>  8) & 0xff);
 333           pxmcc->pwm_prew[1] = *uptr & 0x3fff;
 334           uptr = FPGA_LX_MASTER_TX + ((pwmtx_info >> 16) & 0xff);
 335           pxmcc->pwm_prew[2] = *uptr & 0x3fff;
 336           uptr = FPGA_LX_MASTER_TX + ((pwmtx_info >> 24) & 0xff);
 337           pxmcc->pwm_prew[3] = *uptr & 0x3fff;
 338         }
 339       }
 340       pxmcc++;
 341
 342     } while(pxmcc != pxmcc_data.axis + PXMCC_AXIS_COUNT);
 343
 344     asm volatile("": : : "memory");
 345
 346     {
 347       uint32_t idlecnt = 0;
 348       uint32_t sqn;
 349       do {
 350         sqn = *FPGA_LX_MASTER_RX_DDIV;
 351         idlecnt++;
 352       } while (sqn == last_rx_done_sqn);
 353       pxmcc_data.common.act_idle = idlecnt;
 354       if ((idlecnt < pxmcc_data.common.min_idle) &&
 355           last_rx_done_sqn) {
 356         pxmcc_data.common.min_idle = idlecnt;
 357       }
 358       last_rx_done_sqn = sqn;
 359       pxmcc_data.common.rx_done_sqn = last_rx_done_sqn;
 360       asm volatile("": : : "memory");
 361     }
 362
 363     {
 364       int i;
 365       pxmcc_curadc_data_t *curadc = pxmcc_data.curadc;
 366       volatile uint32_t *siroladc = FPGA_LX_MASTER_RX_ADC0;
 367       uint32_t val;
 368
 369       for (i = 0; i < PXMCC_CURADC_CHANNELS; ) {
 370         val = *siroladc;
 371
 372         curadc->cur_val = (uint16_t)(val - curadc->siroladc_last) -
 373                           curadc->siroladc_offs;
 374         curadc->siroladc_last = val;
 375
 376         i++;
 377         curadc += 1;
 378         siroladc += 2;
 379         /* additional 3 required for 7 -> 8 change */
 380         if (!(i & 7))
 381           siroladc += 3;
 382       }
 383     }
 384
 385     pxmcc = pxmcc_data.axis;
 386     do {
 387       int32_t  cur_alp;
 388       int32_t  cur_bet;
 389       int32_t  cur_d;
 390       int32_t  cur_q;
 391       uint32_t pwm1;
 392       uint32_t pwm2;
 393       uint32_t pwm3;
 394       int32_t  cur1;
 395       int32_t  cur2;
 396       int32_t  cur3;
 397       int32_t  *pcurmult;
 398       uint32_t curmult_idx;
 399       uint32_t pwm_reci;
 400       uint32_t out_info;
 401      #if defined(COMPUTE_PHASE_SECTOR) || !defined(SUPPRESS_CONDITIONALS)
 402       uint32_t phs;
 403      #ifdef COMPUTE_PHASE_SECTOR
 404       phs = pxmcc->ptphs;
 405      #endif /*COMPUTE_PHASE_SECTOR*/
 406      #endif /*COMPUTE_PHASE_SECTOR*/
 407
 408       out_info = pxmcc->out_info;
 409       if (pxmcc->mode == PXMCC_MODE_BLDC) {
 410
 411         pwm1 = pxmcc->pwm_prew[0];
 412         pwm2 = pxmcc->pwm_prew[1];
 413         pwm3 = pxmcc->pwm_prew[2];
 414
 415        #ifndef SUPPRESS_CONDITIONALS
 416         #ifndef COMPUTE_PHASE_SECTOR
 417         if (pwm1 > pwm2)
 418           if (pwm2 > pwm3)
 419             phs = 0;
 420           else if (pwm1 > pwm3)
 421             phs = 5;
 422           else
 423             phs = 4;
 424         else
 425           if (pwm2 < pwm3)
 426             phs = 3;
 427           else if (pwm1 < pwm3)
 428             phs = 2;
 429           else
 430             phs = 1;
 431         #endif /*COMPUTE_PHASE_SECTOR*/
 432
 433         curmult_idx = (0x00201201 >> (4 * phs)) & 3;
 434         pwm_reci = pxmcc_data.common.pwm_cycle - pxmcc->pwm_prew[curmult_idx];
 435         pwm_reci = (pxmcc_data.common.pwm_cycle << 16) / pwm_reci;
 436
 437         /*
 438          * Translate index from pwm1, pwm2, pwm3 order to
 439          * to order of current sources 0->2 1->0 2->1
 440          *
 441          * This solution modifies directly value in pxmcc_curadc_data_t
 442          * so it is destructive and has not to be applied twice,
 443          * but it is much better optimized
 444          */
 445         curmult_idx = (0x102 >> (curmult_idx * 4)) & 3;
 446         pcurmult = &pxmcc_data.curadc[out_info + curmult_idx].cur_val;
 447         *pcurmult = (int32_t)(pwm_reci * (*pcurmult)) >> 16;
 448
 449         cur2 = pxmcc_data.curadc[out_info + 0].cur_val;
 450         cur3 = pxmcc_data.curadc[out_info + 1].cur_val;
 451         cur1 = pxmcc_data.curadc[out_info + 2].cur_val;
 452
 453         if ((phs == 5) || (phs == 0))
 454           cur_alp = -(cur2 + cur3);
 455         else
 456           cur_alp = cur1;
 457
 458         if ((phs == 5) || (phs == 0))
 459           cur_bet = cur2 - cur3;
 460         else if ((phs == 3) || (phs == 4))
 461           cur_bet = 2 * cur2 + cur1;
 462         else /* 1 2 */
 463           cur_bet = -(2 * cur3 + cur1);
 464        #else /*SUPPRESS_CONDITIONALS*/
 465         {
 466           /*
 467            *   u1>u2 u2>u3 u1>u3               cm
 468            *     0     1     1 ->  1 (1, 0, 2) 0
 469            *     0     1     0 ->  2 (1, 2, 0) 2
 470            *     0     0     0 ->  3 (2, 1, 0) 1
 471            *     1     0     0 ->  4 (2, 0, 1) 0
 472            *     1     0     1 ->  5 (0, 2, 1) 2
 473            *     1     1     1 ->  0 (0, 1, 2) 1
 474            */
 475
 476           uint32_t u1gtu2 = (int32_t)(pwm2 - pwm1) >> 31;
 477           uint32_t u1gtu3 = (int32_t)(pwm3 - pwm1) >> 31;
 478           uint32_t u2gtu3 = (int32_t)(pwm3 - pwm2) >> 31;
 479           uint32_t state50_msk = u1gtu2 & u1gtu3;
 480           uint32_t pwm_reci_bits;
 481           uint32_t sz4idx = sizeof(*pxmcc->pwm_prew);
 482           uint32_t sz4cur = sizeof(*pxmcc_data.curadc);
 483           uint32_t curmult_idx2curadc;
 484
 485          #if 0
 486           curmult_idx = (((u1gtu3 ^ u1gtu2) | 1) ^ u2gtu3 ^ u1gtu2) & 3;
 487           pwm_reci = pxmcc_data.common.pwm_cycle - pxmcc->pwm_prew[curmult_idx];
 488          #else
 489           /* Variant where curmult_idx is directly computed as byte offset to uint32_t array */
 490           curmult_idx = (((u1gtu3 ^ u1gtu2) | (1 * sz4idx)) ^ u2gtu3 ^ u1gtu2) & (3 * sz4idx);
 491           pwm_reci = pxmcc_data.common.pwm_cycle - *offsbychar(pxmcc->pwm_prew, curmult_idx);
 492          #endif
 493
 494          #if 0
 495           pwm_reci_bits = __builtin_clzl(pwm_reci);
 496          #else
 497           asm("clz %0,%1\n":"=r"(pwm_reci_bits):"r"(pwm_reci));
 498          #endif
 499           pwm_reci <<= pwm_reci_bits;
 500           *FPGA_FNCAPPROX_RECI = pwm_reci;
 501           asm volatile("nop\n");
 502           asm volatile("nop\n");
 503           asm volatile("nop\n");
 504           pwm_reci = *FPGA_FNCAPPROX_RECI;
 505           pwm_reci >>= 16;
 506           pwm_reci *= pxmcc_data.common.pwm_cycle;
 507           pwm_reci >>= 30 - pwm_reci_bits;
 508
 509           /*
 510            * Translate index from pwm1, pwm2, pwm3 order to
 511            * to order of current sources 0->2 1->0 2->1
 512            *
 513            * This solution modifies directly value in pxmcc_curadc_data_t
 514            * so it is destructive and has not to be applied twice,
 515            * but it is much better optimized
 516            */
 517          #if 0
 518           curmult_idx = (0x102 >> (curmult_idx * 4)) & 3;
 519           pcurmult = &pxmcc_data.curadc[out_info + curmult_idx].cur_val;
 520          #else
 521           curmult_idx2curadc = ((2 * sz4cur) << (0 * sz4idx)) |
 522                                ((0 * sz4cur) << (1 * sz4idx)) |
 523                                ((1 * sz4cur) << (2 * sz4idx));
 524           curmult_idx = (curmult_idx2curadc >> curmult_idx) & (sz4cur | (sz4cur << 1));
 525           pcurmult = &pxmcc_data.curadc[out_info].cur_val;
 526           pcurmult = offsbychar(pcurmult, curmult_idx);
 527          #endif
 528           *pcurmult = (int32_t)(pwm_reci * (*pcurmult)) >> 16;
 529
 530           cur2 = pxmcc_data.curadc[out_info + 0].cur_val;
 531           cur3 = pxmcc_data.curadc[out_info + 1].cur_val;
 532           cur1 = pxmcc_data.curadc[out_info + 2].cur_val;
 533
 534           cur_alp = -(cur2 + cur3);                /* 5 0 */
 535           cur_alp &= state50_msk;                  /* 1 2 3 4 */
 536           cur_alp |= cur1 & ~state50_msk;
 537
 538           cur_bet = (-2 * cur3 - cur1) & u2gtu3;   /* 1 2 */
 539           cur_bet |= (2 * cur2 + cur1) & ~u2gtu3;  /* 3 4 */
 540           cur_bet &= ~state50_msk;
 541           cur_bet |= (cur2 - cur3) & state50_msk;  /* 5 0 */
 542         }
 543        #endif /*SUPPRESS_CONDITIONALS*/
 544
 545         cur_alp *= 3;
 546         cur_alp >>= 1;
 547
 548         cur_bet *= CONST16_K3;
 549         cur_bet >>= 16;
 550       } else {
 551         int32_t bet_pwm = pxmcc->pwm_prew[2];
 552         uint32_t bet_sgn = (bet_pwm - 1) >> 31;
 553         int32_t alp_pwm = pxmcc->pwm_prew[0];
 554         uint32_t alp_sgn = (alp_pwm - 1) >> 31;
 555         cur_bet = (pxmcc_data.curadc[out_info + 3].cur_val & ~bet_sgn) -
 556                   (pxmcc_data.curadc[out_info + 2].cur_val & bet_sgn);
 557         cur_alp = (pxmcc_data.curadc[out_info + 1].cur_val & ~alp_sgn) -
 558                   (pxmcc_data.curadc[out_info + 0].cur_val & alp_sgn);
 559       }
 560
 561       cur_d =  cur_alp * pxmcc->ptcos + cur_bet * pxmcc->ptsin;
 562       cur_q = -cur_alp * pxmcc->ptsin + cur_bet * pxmcc->ptcos;
 563
 564       pxmcc->cur_dq = (cur_d & 0xffff0000) | ((cur_q >> 16) & 0xffff);
 565
 566       pxmcc->cur_d_cum = ((pxmcc->cur_d_cum + (cur_d >> 4)) & ~0x3f) |
 567                          (last_rx_done_sqn & 0x1f);
 568       pxmcc->cur_q_cum = ((pxmcc->cur_q_cum + (cur_q >> 4)) & ~0x3f) |
 569                          (last_rx_done_sqn & 0x1f);
 570
 571       pxmcc++;
 572     } while(pxmcc != pxmcc_data.axis + PXMCC_AXIS_COUNT);
 573   }
 574 }