sw/app/rocon/appl_pxmc.c

   1 /*******************************************************************
   2   Motion and Robotic System (MARS) aplication components.
   3
   4   appl_pxmc.c - position controller subsystem core generic
   5                 and LP_MPW1 hardware specific support
   6
   7   Copyright (C) 2001-2013 by Pavel Pisa - originator
   8                           pisa@cmp.felk.cvut.cz
   9             (C) 2001-2013 by PiKRON Ltd. - originator
  10                     http://www.pikron.com
  11
  12   This file can be used and copied according to next
  13   license alternatives
  14    - GPL - GNU Public License
  15    - other license provided by project originators
  16
  17  *******************************************************************/
  18
  19 #include <cpu_def.h>
  20 #include <system_def.h>
  21 #include <pxmc.h>
  22 #include <pxmc_lpc_qei.h>
  23 #include <pxmc_internal.h>
  24 #include <pxmc_inp_common.h>
  25 #include <pxmc_gen_info.h>
  26 #include <hal_gpio.h>
  27 #include <hal_machperiph.h>
  28 #include <stdlib.h>
  29 #include <string.h>
  30
  31 #include "appl_defs.h"
  32 #include "appl_fpga.h"
  33
  34 int pxmc_ptofs_from_index(pxmc_state_t *mcs, unsigned long irc,
  35                            unsigned long index_irc, int diff2err);
  36
  37 #ifndef pxmc_fast_tick_time
  38 #define pxmc_fast_tick_time() (LPC_TIM0->TC)
  39 #endif
  40
  41 #define PXML_MAIN_CNT 8
  42
  43 #define PXMC_WITH_PT_ZIC 1
  44 #define PXMC_PT_ZIC_MASK 0x8000
  45
  46 #define LPCPWM1_FREQ 20000
  47
  48 #define HAL_ERR_SENSITIVITY 20
  49 #define HAL_ERR_MAX_COUNT    5
  50
  51 #define LXPWR_WITH_SIROLADC  1
  52
  53 #define LX_MASTER_DATA_OFFS  8
  54
  55 unsigned pxmc_rocon_pwm_magnitude = 2500;
  56
  57 long pxmc_rocon_irc_offset[PXML_MAIN_CNT];
  58 unsigned pxmc_rocon_mark_filt[PXML_MAIN_CNT];
  59
  60 const uint8_t onesin10bits[1024]={
  61   0,1,1,2,1,2,2,3,1,2,2,3,2,3,3,4,1,2,2,3,2,3,3,4,2,3,3,4,3,4,4,5,
  62   1,2,2,3,2,3,3,4,2,3,3,4,3,4,4,5,2,3,3,4,3,4,4,5,3,4,4,5,4,5,5,6,
  63   1,2,2,3,2,3,3,4,2,3,3,4,3,4,4,5,2,3,3,4,3,4,4,5,3,4,4,5,4,5,5,6,
  64   2,3,3,4,3,4,4,5,3,4,4,5,4,5,5,6,3,4,4,5,4,5,5,6,4,5,5,6,5,6,6,7,
  65   1,2,2,3,2,3,3,4,2,3,3,4,3,4,4,5,2,3,3,4,3,4,4,5,3,4,4,5,4,5,5,6,
  66   2,3,3,4,3,4,4,5,3,4,4,5,4,5,5,6,3,4,4,5,4,5,5,6,4,5,5,6,5,6,6,7,
  67   2,3,3,4,3,4,4,5,3,4,4,5,4,5,5,6,3,4,4,5,4,5,5,6,4,5,5,6,5,6,6,7,
  68   3,4,4,5,4,5,5,6,4,5,5,6,5,6,6,7,4,5,5,6,5,6,6,7,5,6,6,7,6,7,7,8,
  69   1,2,2,3,2,3,3,4,2,3,3,4,3,4,4,5,2,3,3,4,3,4,4,5,3,4,4,5,4,5,5,6,
  70   2,3,3,4,3,4,4,5,3,4,4,5,4,5,5,6,3,4,4,5,4,5,5,6,4,5,5,6,5,6,6,7,
  71   2,3,3,4,3,4,4,5,3,4,4,5,4,5,5,6,3,4,4,5,4,5,5,6,4,5,5,6,5,6,6,7,
  72   3,4,4,5,4,5,5,6,4,5,5,6,5,6,6,7,4,5,5,6,5,6,6,7,5,6,6,7,6,7,7,8,
  73   2,3,3,4,3,4,4,5,3,4,4,5,4,5,5,6,3,4,4,5,4,5,5,6,4,5,5,6,5,6,6,7,
  74   3,4,4,5,4,5,5,6,4,5,5,6,5,6,6,7,4,5,5,6,5,6,6,7,5,6,6,7,6,7,7,8,
  75   3,4,4,5,4,5,5,6,4,5,5,6,5,6,6,7,4,5,5,6,5,6,6,7,5,6,6,7,6,7,7,8,
  76   4,5,5,6,5,6,6,7,5,6,6,7,6,7,7,8,5,6,6,7,6,7,7,8,6,7,7,8,7,8,8,9,
  77   1,2,2,3,2,3,3,4,2,3,3,4,3,4,4,5,2,3,3,4,3,4,4,5,3,4,4,5,4,5,5,6,
  78   2,3,3,4,3,4,4,5,3,4,4,5,4,5,5,6,3,4,4,5,4,5,5,6,4,5,5,6,5,6,6,7,
  79   2,3,3,4,3,4,4,5,3,4,4,5,4,5,5,6,3,4,4,5,4,5,5,6,4,5,5,6,5,6,6,7,
  80   3,4,4,5,4,5,5,6,4,5,5,6,5,6,6,7,4,5,5,6,5,6,6,7,5,6,6,7,6,7,7,8,
  81   2,3,3,4,3,4,4,5,3,4,4,5,4,5,5,6,3,4,4,5,4,5,5,6,4,5,5,6,5,6,6,7,
  82   3,4,4,5,4,5,5,6,4,5,5,6,5,6,6,7,4,5,5,6,5,6,6,7,5,6,6,7,6,7,7,8,
  83   3,4,4,5,4,5,5,6,4,5,5,6,5,6,6,7,4,5,5,6,5,6,6,7,5,6,6,7,6,7,7,8,
  84   4,5,5,6,5,6,6,7,5,6,6,7,6,7,7,8,5,6,6,7,6,7,7,8,6,7,7,8,7,8,8,9,
  85   2,3,3,4,3,4,4,5,3,4,4,5,4,5,5,6,3,4,4,5,4,5,5,6,4,5,5,6,5,6,6,7,
  86   3,4,4,5,4,5,5,6,4,5,5,6,5,6,6,7,4,5,5,6,5,6,6,7,5,6,6,7,6,7,7,8,
  87   3,4,4,5,4,5,5,6,4,5,5,6,5,6,6,7,4,5,5,6,5,6,6,7,5,6,6,7,6,7,7,8,
  88   4,5,5,6,5,6,6,7,5,6,6,7,6,7,7,8,5,6,6,7,6,7,7,8,6,7,7,8,7,8,8,9,
  89   3,4,4,5,4,5,5,6,4,5,5,6,5,6,6,7,4,5,5,6,5,6,6,7,5,6,6,7,6,7,7,8,
  90   4,5,5,6,5,6,6,7,5,6,6,7,6,7,7,8,5,6,6,7,6,7,7,8,6,7,7,8,7,8,8,9,
  91   4,5,5,6,5,6,6,7,5,6,6,7,6,7,7,8,5,6,6,7,6,7,7,8,6,7,7,8,7,8,8,9,
  92   5,6,6,7,6,7,7,8,6,7,7,8,7,8,8,9,6,7,7,8,7,8,8,9,7,8,8,9,8,9,9,10
  93 };
  94
  95 int
  96 pxmc_inp_rocon_inp(struct pxmc_state *mcs)
  97 {
  98   int chan=mcs->pxms_inp_info;
  99   long irc;
 100   long pos;
 101
 102   /* read position from hardware */
 103   irc = fpga_irc[chan]->count;
 104   irc += pxmc_rocon_irc_offset[chan];
 105   pos = irc << PXMC_SUBDIV(mcs);
 106   mcs->pxms_as = pos - mcs->pxms_ap;
 107   mcs->pxms_ap = pos;
 108
 109   /* Running of the motor commutator */
 110   if (mcs->pxms_flg & PXMS_PTI_m)
 111     pxmc_irc_16bit_commindx(mcs, irc);
 112
 113   return 0;
 114 }
 115
 116 int
 117 pxmc_inp_rocon_is_mark(pxmc_state_t *mcs)
 118 {
 119   uint32_t irc_state;
 120   int mark;
 121   unsigned filt;
 122   int chan=mcs->pxms_inp_info;
 123
 124   irc_state = *fpga_irc_state[chan];
 125
 126   mark = ((irc_state >> (ffs(FPGA_IRC_STATE_MARK_MASK) - 1)) ^
 127          (mcs->pxms_cfg >> PXMS_CFG_HPS_b)) & 1;
 128
 129   filt = pxmc_rocon_mark_filt[chan];
 130   filt = (filt << 1) | mark;
 131   pxmc_rocon_mark_filt[chan] = filt;
 132
 133   return onesin10bits[filt & 0x03ff];
 134 }
 135
 136 int
 137 pxmc_inp_rocon_is_index_edge(pxmc_state_t *mcs)
 138 {
 139   uint32_t irc_state;
 140   int index;
 141   int chan=mcs->pxms_inp_info;
 142
 143   irc_state = *fpga_irc_state[chan];
 144   *fpga_irc_state[chan] = FPGA_IRC_STATE_INDEX_EVENT_MASK;
 145
 146   index = (irc_state >> (ffs(FPGA_IRC_STATE_INDEX_EVENT_MASK) - 1)) & 1;
 147
 148   return index;
 149 }
 150
 151 int
 152 pxmc_inp_rocon_adjust_to_irc_change(struct pxmc_state *mcs, long irc_diff)
 153 {
 154   long pos_diff = irc_diff << PXMC_SUBDIV(mcs);
 155
 156   mcs->pxms_ptofs += irc_diff;
 157
 158   mcs->pxms_ap += pos_diff;
 159   mcs->pxms_rp += pos_diff;
 160   return 0;
 161 }
 162
 163 int
 164 pxmc_inp_rocon_irc_offset_from_index(struct pxmc_state *mcs)
 165 {
 166   int chan=mcs->pxms_inp_info;
 167   long irc_offset;
 168   long irc_diff;
 169
 170   irc_offset = -fpga_irc[chan]->count_index;
 171   irc_diff = irc_offset - pxmc_rocon_irc_offset[chan];
 172   pxmc_rocon_irc_offset[chan] = irc_offset;
 173   return pxmc_inp_rocon_adjust_to_irc_change(mcs, irc_diff);
 174 }
 175
 176 int
 177 pxmc_inp_rocon_ap_zero(struct pxmc_state *mcs)
 178 {
 179   int chan=mcs->pxms_inp_info;
 180   long irc_offset;
 181   long irc_diff;
 182
 183   irc_offset = -fpga_irc[chan]->count;
 184   irc_diff = irc_offset - pxmc_rocon_irc_offset[chan];
 185   pxmc_rocon_irc_offset[chan] = irc_offset;
 186   return pxmc_inp_rocon_adjust_to_irc_change(mcs, irc_diff);
 187 }
 188
 189 int
 190 pxmc_inp_rocon_ap2hw(struct pxmc_state *mcs)
 191 {
 192   int chan=mcs->pxms_inp_info;
 193   long irc;
 194   long pos_diff;
 195
 196   irc = fpga_irc[chan]->count;
 197   pos_diff = mcs->pxms_ap - (irc << PXMC_SUBDIV(mcs));
 198
 199   irc = pos_diff >> PXMC_SUBDIV(mcs);
 200
 201   /* Adjust phase table alignemt to modified IRC readout  */
 202   mcs->pxms_ptofs += irc - pxmc_rocon_irc_offset[chan];
 203
 204   pxmc_rocon_irc_offset[chan] = irc;
 205   return 0;
 206 }
 207
 208 int
 209 pxmc_inp_rocon_ptofs_from_index_poll(struct pxmc_state *mcs, int diff2err)
 210 {
 211   int chan=mcs->pxms_inp_info;
 212   long irc;
 213   long index_irc;
 214
 215   if (!(*fpga_irc_state[chan] & FPGA_IRC_STATE_INDEX_EVENT_MASK))
 216     return 0;
 217
 218   irc = fpga_irc[chan]->count + pxmc_rocon_irc_offset[chan];
 219   index_irc = fpga_irc[chan]->count_index + pxmc_rocon_irc_offset[chan];
 220
 221   return pxmc_ptofs_from_index(mcs, irc, index_irc, diff2err);
 222 }
 223
 224 uint32_t pxmc_rocon_receiver_dummy_reg;
 225
 226 static inline volatile uint32_t *
 227 pxmc_rocon_receiver_chan2reg(unsigned chan)
 228 {
 229   volatile uint32_t *rec_reg;
 230
 231   if (chan >= 16)
 232     return &pxmc_rocon_receiver_dummy_reg;
 233
 234   rec_reg = fpga_lx_master_receiver_base;
 235
 236  #ifdef LXPWR_WITH_SIROLADC
 237   rec_reg += LX_MASTER_DATA_OFFS + 1 + (chan >> 8) * 3 + chan * 2;
 238  #else /*LXPWR_WITH_SIROLADC*/
 239   rec_reg += LX_MASTER_DATA_OFFS + chan;
 240  #endif /*LXPWR_WITH_SIROLADC*/
 241
 242   return rec_reg;
 243 }
 244
 245 inline unsigned
 246 pxmc_rocon_bldc_hal_rd(pxmc_state_t *mcs)
 247 {
 248   unsigned h = 0;
 249   volatile uint32_t *rec_reg_a, *rec_reg_b, *rec_reg_c;
 250   int chan = mcs->pxms_out_info;
 251   int hal_offs;
 252
 253  #ifdef LXPWR_WITH_SIROLADC
 254   hal_offs = 1;
 255  #else /*LXPWR_WITH_SIROLADC*/
 256   hal_offs = 0;
 257  #endif /*LXPWR_WITH_SIROLADC*/
 258
 259   rec_reg_a = pxmc_rocon_receiver_chan2reg(chan + 0);
 260   rec_reg_b = pxmc_rocon_receiver_chan2reg(chan + 1);
 261   rec_reg_c = pxmc_rocon_receiver_chan2reg(chan + 2);
 262
 263   h  = (rec_reg_a[hal_offs] >> 14) & 1;
 264   h |= (rec_reg_b[hal_offs] >> 13) & 2;
 265   h |= (rec_reg_c[hal_offs] >> 12) & 4;
 266
 267   /* return 3 bits corresponding to the HAL senzor input */
 268   return h;
 269 }
 270
 271 #if 1
 272 const unsigned char pxmc_lpc_bdc_hal_pos_table[8] =
 273 {
 274   [0] = 0xff,
 275   [7] = 0xff,
 276   [1] = 0, /*0*/
 277   [5] = 1, /*1*/
 278   [4] = 2, /*2*/
 279   [6] = 3, /*3*/
 280   [2] = 4, /*4*/
 281   [3] = 5, /*5*/
 282 };
 283 #else
 284 const unsigned char pxmc_lpc_bdc_hal_pos_table[8] =
 285 {
 286   [0] = 0xff,
 287   [7] = 0xff,
 288   [1] = 0, /*0*/
 289   [5] = 5, /*1*/
 290   [4] = 4, /*2*/
 291   [6] = 3, /*3*/
 292   [2] = 2, /*4*/
 293   [3] = 1, /*5*/
 294 };
 295 #endif
 296
 297 uint32_t pxmc_rocon_pwm_dummy_reg;
 298
 299 static inline volatile uint32_t *
 300 pxmc_rocon_pwm_chan2reg(unsigned chan)
 301 {
 302   volatile uint32_t *pwm_reg;
 303
 304   if (chan >= 16)
 305     return &pxmc_rocon_pwm_dummy_reg;
 306
 307   pwm_reg = fpga_lx_master_transmitter_base;
 308
 309  #ifdef LXPWR_WITH_SIROLADC
 310   pwm_reg += LX_MASTER_DATA_OFFS + 1 + (chan >> 8) + chan;
 311  #else /*LXPWR_WITH_SIROLADC*/
 312   pwm_reg += LX_MASTER_DATA_OFFS + chan;
 313  #endif /*LXPWR_WITH_SIROLADC*/
 314
 315   return pwm_reg;
 316 }
 317
 318 /**
 319  * pxmc_rocon_pwm3ph_wr - Output of the 3-phase PWM to the hardware
 320  * @mcs:  Motion controller state information
 321  */
 322 /*static*/ inline void
 323 pxmc_rocon_pwm3ph_wr(pxmc_state_t *mcs, short pwm1, short pwm2, short pwm3)
 324 {
 325   volatile uint32_t *pwm_reg_a, *pwm_reg_b, *pwm_reg_c;
 326   int chan = mcs->pxms_out_info;
 327
 328   pwm_reg_c = pxmc_rocon_pwm_chan2reg(chan + 0);
 329   pwm_reg_b = pxmc_rocon_pwm_chan2reg(chan + 1);
 330   pwm_reg_a = pxmc_rocon_pwm_chan2reg(chan + 2);
 331
 332   *pwm_reg_a = pwm1;
 333   *pwm_reg_b = pwm2;
 334   *pwm_reg_c = pwm3;
 335 }
 336
 337 static inline void
 338 pxmc_rocon_process_hal_error(struct pxmc_state *mcs)
 339 {
 340   if (mcs->pxms_halerc >= HAL_ERR_SENSITIVITY * HAL_ERR_MAX_COUNT)
 341   {
 342     pxmc_set_errno(mcs, PXMS_E_HAL);
 343     mcs->pxms_ene = 0;
 344     mcs->pxms_halerc--;
 345   }
 346   else
 347     mcs->pxms_halerc += HAL_ERR_SENSITIVITY;
 348 }
 349
 350 /**
 351  * pxmc_rocon_pwm3ph_out - Phase output for brush-less 3-phase motor
 352  * @mcs:  Motion controller state information
 353  */
 354 int
 355 pxmc_rocon_pwm3ph_out(pxmc_state_t *mcs)
 356 {
 357   typeof(mcs->pxms_ptvang) ptvang;
 358   int sync_mode = 0;
 359   unsigned char hal_pos;
 360   short pwm1;
 361   short pwm2;
 362   short pwm3;
 363   int indx = 0;
 364   short ene;
 365   int wind_current[4];
 366
 367   if (!(mcs->pxms_flg & PXMS_PTI_m) || !(mcs->pxms_flg & PXMS_PHA_m) ||
 368       (mcs->pxms_flg & PXMS_PRA_m))
 369   {
 370     short ptindx;
 371     short ptirc = mcs->pxms_ptirc;
 372     short divisor = mcs->pxms_ptper * 6;
 373
 374    #if 0
 375     pxmc_irc_16bit_commindx(mcs, mcs->pxms_rp >> PXMC_SUBDIV(mcs));
 376     sync_mode = 1;
 377    #elif 0
 378     {
 379       int res;
 380       res = pxmc_inp_rocon_ptofs_from_index_poll(mcs, 0);
 381       if (res < 0) {
 382         pxmc_set_errno(mcs, PXMS_E_I2PT_TOOBIG);
 383       } else if (res) {
 384         pxmc_set_flag(mcs, PXMS_PTI_b);
 385         pxmc_set_flag(mcs, PXMS_PHA_b);
 386       }
 387     }
 388    #else
 389
 390     hal_pos = pxmc_lpc_bdc_hal_pos_table[pxmc_rocon_bldc_hal_rd(mcs)];
 391
 392     if (hal_pos == 0xff)
 393     {
 394       if (mcs->pxms_ene)
 395         pxmc_rocon_process_hal_error(mcs);
 396     }
 397     else
 398     {
 399       if (mcs->pxms_halerc)
 400         mcs->pxms_halerc--;
 401
 402       ptindx = (hal_pos * ptirc + divisor / 2) / divisor;
 403
 404       if (!(mcs->pxms_flg & PXMS_PTI_m) || (mcs->pxms_flg & PXMS_PRA_m))
 405       {
 406         if (((hal_pos != mcs->pxms_hal) && (mcs->pxms_hal != 0x40)) && 1)
 407         {
 408           short ptindx_prev = (mcs->pxms_hal * ptirc + divisor / 2) / divisor;;
 409
 410           if ((ptindx > ptindx_prev + ptirc / 2) ||
 411               (ptindx_prev > ptindx + ptirc / 2))
 412           {
 413             ptindx = (ptindx_prev + ptindx - ptirc) / 2;
 414
 415             if (ptindx < 0)
 416               ptindx += ptirc;
 417           }
 418           else
 419           {
 420             ptindx = (ptindx_prev + ptindx) / 2;
 421           }
 422
 423           mcs->pxms_ptindx = ptindx;
 424
 425           mcs->pxms_ptofs = (mcs->pxms_ap >> PXMC_SUBDIV(mcs)) + mcs->pxms_ptshift - ptindx;
 426
 427           pxmc_set_flag(mcs, PXMS_PTI_b);
 428           pxmc_clear_flag(mcs, PXMS_PRA_b);
 429         }
 430         else
 431         {
 432           if (!(mcs->pxms_flg & PXMS_PTI_m))
 433             mcs->pxms_ptindx = ptindx;
 434         }
 435       } else {
 436         /* if phase table position to mask is know do fine phase table alignment */
 437         if (mcs->pxms_cfg & PXMS_CFG_I2PT_m) {
 438           int res;
 439           res = pxmc_inp_rocon_ptofs_from_index_poll(mcs, 0);
 440           if (res < 0) {
 441             pxmc_set_errno(mcs, PXMS_E_I2PT_TOOBIG);
 442           } else if (res) {
 443             pxmc_set_flag(mcs, PXMS_PTI_b);
 444             pxmc_set_flag(mcs, PXMS_PHA_b);
 445           }
 446         }
 447       }
 448       mcs->pxms_hal = hal_pos;
 449     }
 450    #endif
 451   }
 452
 453   {
 454     /*wind_current[0]=(ADC->ADDR0 & 0xFFF0)>>4;*/
 455     /* FIXME - check winding current against limit */
 456     /* pxmc_set_errno(mcs, PXMS_E_WINDCURRENT); */
 457   }
 458
 459   if (!sync_mode) {
 460     ptvang = mcs->pxms_ptvang;
 461     ene = mcs->pxms_ene;
 462   } else {
 463     ptvang = 0;
 464     ene = mcs->pxms_flg & PXMS_BSY_m? mcs->pxms_me: 0;
 465   }
 466
 467   if (ene) {
 468     indx = mcs->pxms_ptindx;
 469 #if 1
 470     /* tuning of magnetic field/voltage advance angle */
 471     indx += (mcs->pxms_s1 * mcs->pxms_as) >> (PXMC_SUBDIV(mcs) + 8);
 472 #endif
 473
 474     if (ene < 0)
 475     {
 476       /* Generating direction of stator mag. field for backward torque */
 477       ene = -ene;
 478
 479       if ((indx -= ptvang) < 0)
 480         indx += mcs->pxms_ptirc;
 481     }
 482     else
 483     {
 484       /* Generating direction of stator mag. field for forward torque */
 485       if ((indx += ptvang) >= mcs->pxms_ptirc)
 486         indx -= mcs->pxms_ptirc;
 487     }
 488
 489     if (mcs->pxms_ptscale_mult)
 490       indx = ((unsigned long)indx * mcs->pxms_ptscale_mult) >> mcs->pxms_ptscale_shift;
 491
 492     pwm1 = mcs->pxms_ptptr1[indx];
 493     pwm2 = mcs->pxms_ptptr2[indx];
 494     pwm3 = mcs->pxms_ptptr3[indx];
 495
 496 #ifdef PXMC_WITH_PT_ZIC
 497     if (labs(mcs->pxms_as) < (10 << PXMC_SUBDIV(mcs)))
 498     {
 499       pwm1 &= ~PXMC_PT_ZIC_MASK;
 500       pwm2 &= ~PXMC_PT_ZIC_MASK;
 501       pwm3 &= ~PXMC_PT_ZIC_MASK;
 502     }
 503 #endif /*PXMC_WITH_PT_ZIC*/
 504
 505     /* Default phase-table amplitude is 0x7fff, ene max is 0x7fff */
 506     /* Initialized CTM4 PWM period is 0x200 => divide by value about 2097024 */
 507     {
 508       unsigned long pwm_dc = pxmc_rocon_pwm_magnitude * (unsigned long)ene;
 509       if (pwm1 & PXMC_PT_ZIC_MASK)
 510         pwm1 = 0x8000;
 511       else
 512         pwm1 = (((unsigned long long)pwm1 * pwm_dc) >> (15 + 15)) | 0x4000;
 513       if (pwm2 & PXMC_PT_ZIC_MASK)
 514         pwm2 = 0x8000;
 515       else
 516         pwm2 = (((unsigned long long)pwm2 * pwm_dc) >> (15 + 15)) | 0x4000;
 517       if (pwm3 & PXMC_PT_ZIC_MASK)
 518         pwm3 = 0x8000;
 519       else
 520         pwm3 = (((unsigned long long)pwm3 * pwm_dc) >> (15 + 15)) | 0x4000;
 521     }
 522     pxmc_rocon_pwm3ph_wr(mcs, pwm1, pwm2, pwm3);
 523   }
 524   else
 525   {
 526     pxmc_rocon_pwm3ph_wr(mcs, 0, 0, 0);
 527   }
 528
 529   return 0;
 530 }
 531
 532 /**
 533  * pxmc_rocon_pwm2ph_wr - Output of the 2-phase stepper motor PWM to the hardware
 534  * @mcs:        Motion controller state information
 535  */
 536 /*static*/ inline void
 537 pxmc_rocon_pwm2ph_wr(pxmc_state_t *mcs, short pwm1, short pwm2)
 538 {
 539   volatile uint32_t *pwm_reg_ap, *pwm_reg_an, *pwm_reg_bp, *pwm_reg_bn;
 540   int chan = mcs->pxms_out_info;
 541
 542   pwm_reg_bn = pxmc_rocon_pwm_chan2reg(chan + 0);
 543   pwm_reg_bp = pxmc_rocon_pwm_chan2reg(chan + 1);
 544   pwm_reg_an = pxmc_rocon_pwm_chan2reg(chan + 2);
 545   pwm_reg_ap = pxmc_rocon_pwm_chan2reg(chan + 3);
 546
 547   if (pwm2 >= 0) {
 548     *pwm_reg_bp = pwm2 | 0x4000;
 549     *pwm_reg_bn = 0;
 550   } else {
 551     *pwm_reg_bp = 0;
 552     *pwm_reg_bn = -pwm2 | 0x4000;
 553   }
 554
 555   if (pwm1 >= 0) {
 556     *pwm_reg_ap = pwm1 | 0x4000;
 557     *pwm_reg_an = 0;
 558   } else {
 559     *pwm_reg_ap = 0;
 560     *pwm_reg_an = -pwm1 | 0x4000;
 561   }
 562 }
 563
 564 /**
 565  * pxmc_rocon_pwm2ph_out - Phase output of the 2-phase stepper motor PWM
 566  * @mcs:  Motion controller state information
 567  */
 568 int
 569 pxmc_rocon_pwm2ph_out(pxmc_state_t *mcs)
 570 {
 571   typeof(mcs->pxms_ptvang) ptvang;
 572   int sync_mode = 0;
 573   short pwm1;
 574   short pwm2;
 575   int indx = 0;
 576   short ene;
 577   int wind_current[4];
 578
 579   if(!(mcs->pxms_flg&PXMS_PTI_m) || !(mcs->pxms_flg&PXMS_PHA_m) ||
 580      (mcs->pxms_flg&PXMS_PRA_m)){
 581     short ptindx;
 582     short ptirc=mcs->pxms_ptirc;
 583
 584    #if 0
 585     pxmc_irc_16bit_commindx(mcs, mcs->pxms_rp >> PXMC_SUBDIV(mcs));
 586     sync_mode = 1;
 587    #elif 1
 588     {
 589       int res;
 590       res = pxmc_inp_rocon_ptofs_from_index_poll(mcs, 0);
 591       if (res < 0) {
 592         pxmc_set_errno(mcs, PXMS_E_I2PT_TOOBIG);
 593       } else if (res) {
 594         pxmc_set_flag(mcs, PXMS_PTI_b);
 595         pxmc_set_flag(mcs, PXMS_PHA_b);
 596       }
 597     }
 598    #else
 599
 600     ptindx = mcs->pxms_ptindx;
 601
 602     if(0 && (!(mcs->pxms_flg&PXMS_PTI_m) || (mcs->pxms_flg&PXMS_PRA_m))) {
 603
 604       mcs->pxms_ptindx = ptindx;
 605
 606       mcs->pxms_ptofs = (mcs->pxms_ap >> PXMC_SUBDIV(mcs)) + mcs->pxms_ptshift - ptindx;
 607
 608       pxmc_set_flag(mcs, PXMS_PTI_b);
 609       pxmc_clear_flag(mcs, PXMS_PRA_b);
 610
 611       /* if phase table position to mask is know do fine phase table alignment */
 612       //if(mcs->pxms_cfg & PXMS_CFG_I2PT_m)
 613       //  lpc_qei_setup_index_catch(&lpc_qei_state);
 614
 615     } else {
 616       if(!(mcs->pxms_flg&PXMS_PTI_m))
 617             mcs->pxms_ptindx = ptindx;
 618     }
 619    #endif
 620   }
 621
 622   {
 623     /*wind_current[0]=(ADC->ADDR0 & 0xFFF0)>>4;*/
 624     /* FIXME - check winding current against limit */
 625     /* pxmc_set_errno(mcs, PXMS_E_WINDCURRENT); */
 626   }
 627
 628   if (!sync_mode) {
 629     ptvang = mcs->pxms_ptvang;
 630     ene = mcs->pxms_ene;
 631   } else {
 632     ptvang = 0;
 633     ene = mcs->pxms_flg & PXMS_BSY_m? mcs->pxms_me: 0;
 634   }
 635
 636   if (ene) {
 637     indx = mcs->pxms_ptindx;
 638    #if 0
 639     /* tuning of magnetic field/voltage advance angle */
 640     indx += (mcs->pxms_s1 * mcs->pxms_as) >> (PXMC_SUBDIV(mcs) + 8);
 641    #endif
 642     if (ene<0){
 643       /* Generating direction of stator mag. field for backward torque */
 644       ene = -ene;
 645       if ((indx -= ptvang)<0)
 646         indx += mcs->pxms_ptirc;
 647     }else{
 648       /* Generating direction of stator mag. field for forward torque */
 649       if ((indx += ptvang) >= mcs->pxms_ptirc)
 650         indx -= mcs->pxms_ptirc;
 651     }
 652
 653     if (mcs->pxms_ptscale_mult)
 654       indx = ((unsigned long)indx * mcs->pxms_ptscale_mult) >> mcs->pxms_ptscale_shift;
 655
 656     pwm1 = mcs->pxms_ptptr1[indx];
 657     pwm2 = mcs->pxms_ptptr2[indx];
 658
 659     /* Default phase-table amplitude is 0x7fff, ene max is 0x7fff */
 660     /* Initialized PWM period is 0x200 => divide by value about 2097024 */
 661     {
 662       unsigned long pwm_dc = pxmc_rocon_pwm_magnitude * (unsigned long)ene;
 663       pwm1 = ((unsigned long long)pwm1 * pwm_dc) >> (15+15);
 664       pwm2 = ((unsigned long long)pwm2 * pwm_dc) >> (15+15);
 665     }
 666     pxmc_rocon_pwm2ph_wr(mcs, pwm1, pwm2);
 667   }else{
 668     /*pxmc_lpc_wind_current_over_cnt = 0;*/
 669     pxmc_rocon_pwm2ph_wr(mcs, 0, 0);
 670   }
 671
 672   return 0;
 673 }
 674
 675 /**
 676  * pxmc_rocon_pwm_dc_out - DC motor CW and CCW PWM output
 677  * @mcs:  Motion controller state information
 678  */
 679 int
 680 pxmc_rocon_pwm_dc_out(pxmc_state_t *mcs)
 681 {
 682   volatile uint32_t *pwm_reg_a, *pwm_reg_b;
 683   int chan = mcs->pxms_out_info;
 684   int ene = mcs->pxms_ene;
 685
 686   pwm_reg_a = pxmc_rocon_pwm_chan2reg(chan + 0);
 687   pwm_reg_b = pxmc_rocon_pwm_chan2reg(chan + 1);
 688
 689   if (ene < 0) {
 690     ene = -ene;
 691     if (ene > 0x7fff)
 692       ene = 0x7fff;
 693     ene = (ene * (pxmc_rocon_pwm_magnitude + 5)) >> 15;
 694     *pwm_reg_a = 0;
 695     *pwm_reg_b = ene | 0x4000;
 696   } else {
 697     if (ene > 0x7fff)
 698       ene = 0x7fff;
 699     ene = (ene * (pxmc_rocon_pwm_magnitude + 5)) >> 15;
 700     *pwm_reg_b = 0;
 701     *pwm_reg_a = ene | 0x4000;
 702   }
 703
 704   return 0;
 705 }
 706
 707 int
 708 pxmc_rocon_pwm_master_init(void)
 709 {
 710   int i;
 711   int grp_in = 0;
 712   int grp_out = 0;
 713   unsigned word_slot;
 714  #ifdef LXPWR_WITH_SIROLADC
 715   unsigned lxpwr_header = 1;
 716   unsigned lxpwr_words = 1 + 8 * 2 + 2;
 717   unsigned lxpwr_chips = 2;
 718   unsigned lxpwr_chip_pwm_cnt = 8;
 719  #else /*LXPWR_WITH_SIROLADC*/
 720   unsigned lxpwr_header = 0;
 721   unsigned lxpwr_words = 8;
 722   unsigned lxpwr_chips = 2;
 723   unsigned lxpwr_chip_pwm_cnt = 8;
 724  #endif /*LXPWR_WITH_SIROLADC*/
 725
 726   *fpga_lx_master_reset = 1;
 727   *fpga_lx_master_transmitter_reg = 0;
 728   *fpga_lx_master_transmitter_cycle = 2500; /* 50 MHz -> 20 kHz */
 729   *fpga_lx_master_receiver_done_div = 1 << 8;
 730
 731   for (i = 0; i < LX_MASTER_DATA_OFFS + lxpwr_words * lxpwr_chips; i++)
 732     fpga_lx_master_receiver_base[i] = 0;
 733
 734   word_slot = LX_MASTER_DATA_OFFS;
 735   fpga_lx_master_receiver_base[grp_in++] = (word_slot << 8) | lxpwr_words;
 736   fpga_lx_master_receiver_base[grp_in++] = 0x0000;
 737
 738   for (i = 0; i < LX_MASTER_DATA_OFFS + lxpwr_words * lxpwr_chips; i++)
 739     fpga_lx_master_transmitter_base[i] = 0;
 740
 741   word_slot = LX_MASTER_DATA_OFFS + lxpwr_header + lxpwr_chip_pwm_cnt;
 742   fpga_lx_master_transmitter_base[grp_out++] = (word_slot << 8) | lxpwr_words;
 743  #ifdef LXPWR_WITH_SIROLADC
 744   fpga_lx_master_transmitter_base[word_slot] = 0xc100 | (lxpwr_words - 1);
 745  #endif /*LXPWR_WITH_SIROLADC*/
 746
 747   word_slot = LX_MASTER_DATA_OFFS + 0;
 748   fpga_lx_master_transmitter_base[grp_out++] = (word_slot << 8) | lxpwr_words;
 749  #ifdef LXPWR_WITH_SIROLADC
 750   fpga_lx_master_transmitter_base[word_slot] = 0xc100 | (lxpwr_words - 1);
 751  #endif /*LXPWR_WITH_SIROLADC*/
 752
 753   fpga_lx_master_transmitter_base[grp_out++] = 0x0000;
 754
 755   *fpga_lx_master_reset = 0;
 756   *fpga_lx_master_transmitter_cycle = 2500; /* 50 MHz -> 20 kHz */
 757   *fpga_lx_master_receiver_done_div = 1 << 8;
 758
 759   return 0;
 760 }
 761
 762 int pxmc_ptofs_from_index(pxmc_state_t *mcs, unsigned long irc,
 763                            unsigned long index_irc, int diff2err)
 764 {
 765   long ofsl;
 766   short ofs;
 767
 768   ofs = ofsl = index_irc - mcs->pxms_ptmark;
 769
 770   if (diff2err) {
 771     short diff;
 772     diff = (unsigned short)ofs - (unsigned short)mcs->pxms_ptofs;
 773     if (diff >= mcs->pxms_ptirc / 2)
 774       diff -= mcs->pxms_ptirc;
 775     if (diff <= -mcs->pxms_ptirc / 2)
 776       diff += mcs->pxms_ptirc;
 777     if (diff < 0)
 778       diff = -diff;
 779     if(diff >= mcs->pxms_ptirc / 6) {
 780       return -1;
 781     }
 782   } else {
 783     long diff;
 784     diff = (unsigned long)ofsl - irc;
 785     ofs = ofsl - (diff / mcs->pxms_ptirc) * mcs->pxms_ptirc;
 786   }
 787
 788   mcs->pxms_ptofs = ofs;
 789
 790   return 1;
 791 }
 792
 793 /**
 794  * pxmc_dummy_con - Dummy controller for synchronous BLDC/PMSM/steper drive
 795  * @mcs:        Motion controller state information
 796  */
 797 int
 798 pxmc_dummy_con(pxmc_state_t *mcs)
 799 {
 800   return 0;
 801 }
 802
 803 int pxmc_rocon_hh_gd10(pxmc_state_t *mcs);
 804 int pxmc_rocon_hh_gd20(pxmc_state_t *mcs);
 805 int pxmc_rocon_hh_gd30(pxmc_state_t *mcs);
 806 int pxmc_rocon_hh_gd40(pxmc_state_t *mcs);
 807 int pxmc_rocon_hh_gd50(pxmc_state_t *mcs);
 808 int pxmc_rocon_hh_gd90(pxmc_state_t *mcs);
 809
 810 /* initialize for hard home */
 811 int
 812 pxmc_rocon_hh_gi(pxmc_state_t *mcs)
 813 {
 814   pxmc_set_flag(mcs,PXMS_BSY_b);
 815  #if 0 /* config and speed already set in pxmc_hh */
 816   spd = mcs->pxms_ms;
 817   spd >>= mcs->pxms_cfg & PXMS_CFG_HSPD_m;
 818   if(!spd) spd = 1;
 819   if(mcs->pxms_cfg & PXMS_CFG_HDIR_m)
 820     spd = -spd;
 821   mcs->pxms_gen_hsp = spd;
 822  #endif
 823   mcs->pxms_rsfg = 0;
 824   mcs->pxms_gen_htim = 16;
 825   mcs->pxms_do_gen = pxmc_rocon_hh_gd10;
 826   return pxmc_rocon_hh_gd10(mcs);
 827 }
 828
 829 /* fill initial mark filter and then decide */
 830 int
 831 pxmc_rocon_hh_gd10(pxmc_state_t *mcs)
 832 {
 833   int mark;
 834
 835   if(mcs->pxms_flg & PXMS_ERR_m)
 836     return pxmc_spdnext_gend(mcs);
 837
 838   pxmc_spd_gacc(&(mcs->pxms_rs), 0, mcs->pxms_ma);
 839   mcs->pxms_rp += mcs->pxms_rs;
 840
 841   mark = pxmc_inp_rocon_is_mark(mcs);
 842
 843   if (mcs->pxms_gen_htim) {
 844     mcs->pxms_gen_htim--;
 845     return 0;
 846   }
 847
 848   if (!(mcs->pxms_cfg & PXMS_CFG_HLS_m)) {
 849     /* limit switch is not used */
 850     pxmc_inp_rocon_is_index_edge(mcs);
 851     mcs->pxms_do_gen = pxmc_rocon_hh_gd50;
 852     return 0;
 853   }
 854
 855   if (mark >= 6) {
 856     /* go out from switch */
 857     mcs->pxms_do_gen = pxmc_rocon_hh_gd20;
 858   } else {
 859     /* switch is off -> look for it */
 860     mcs->pxms_do_gen = pxmc_rocon_hh_gd30;
 861   }
 862   return 0;
 863 }
 864
 865 /* go out from switch */
 866 int
 867 pxmc_rocon_hh_gd20(pxmc_state_t *mcs)
 868 {
 869   int mark;
 870
 871   if(mcs->pxms_flg & PXMS_ERR_m)
 872     return pxmc_spdnext_gend(mcs);
 873
 874   mark = pxmc_inp_rocon_is_mark(mcs);
 875
 876   if (mark <= 1) {
 877     /* switch is off -> look for it again */
 878     mcs->pxms_do_gen = pxmc_rocon_hh_gd30;
 879   }
 880
 881   pxmc_spd_gacc(&(mcs->pxms_rs), -mcs->pxms_gen_hsp, mcs->pxms_ma);
 882   mcs->pxms_rp += mcs->pxms_rs;
 883
 884   return 0;
 885 }
 886
 887 /* switch is off -> look for it */
 888 int
 889 pxmc_rocon_hh_gd30(pxmc_state_t *mcs)
 890 {
 891   long spd;
 892   int mark;
 893
 894   if (mcs->pxms_flg & PXMS_ERR_m)
 895     return pxmc_spdnext_gend(mcs);
 896
 897   pxmc_spd_gacc(&(mcs->pxms_rs), mcs->pxms_gen_hsp, mcs->pxms_ma);
 898   mcs->pxms_rp += mcs->pxms_rs;
 899
 900   mark = pxmc_inp_rocon_is_mark(mcs);
 901
 902   if (mark >= 8){
 903     spd = mcs->pxms_gen_hsp >> 2; /* slow down */
 904     if (!spd)
 905       spd = 1;
 906     mcs->pxms_gen_hsp = spd;
 907     mcs->pxms_do_gen = pxmc_rocon_hh_gd40;
 908   }
 909
 910   return 0;
 911 }
 912
 913 /* switch is on -> find edge */
 914 int
 915 pxmc_rocon_hh_gd40(pxmc_state_t *mcs)
 916 {
 917   int mark;
 918
 919   if (mcs->pxms_flg & PXMS_ERR_m)
 920     return pxmc_spdnext_gend(mcs);
 921
 922   mark = pxmc_inp_rocon_is_mark(mcs);
 923
 924   if (mark <= 1) {
 925     if (mcs->pxms_cfg & PXMS_CFG_HRI_m) {
 926       pxmc_inp_rocon_is_index_edge(mcs);
 927       mcs->pxms_do_gen = pxmc_rocon_hh_gd50;
 928     } else {
 929       pxmc_inp_rocon_ap_zero(mcs);
 930       mcs->pxms_do_gen = pxmc_stop_gi;
 931     }
 932   }
 933
 934   pxmc_spd_gacc(&(mcs->pxms_rs), -mcs->pxms_gen_hsp, mcs->pxms_ma);
 935   mcs->pxms_rp += mcs->pxms_rs;
 936
 937   return 0;
 938 }
 939
 940 /* calibrate on revolution index */
 941 int
 942 pxmc_rocon_hh_gd50(pxmc_state_t *mcs)
 943 {
 944   if (mcs->pxms_flg & PXMS_ERR_m)
 945     return pxmc_spdnext_gend(mcs);
 946
 947   pxmc_spd_gacc(&(mcs->pxms_rs), mcs->pxms_gen_hsp, mcs->pxms_ma);
 948   mcs->pxms_rp += mcs->pxms_rs;
 949
 950   if (pxmc_inp_rocon_is_index_edge(mcs)){
 951     pxmc_inp_rocon_irc_offset_from_index(mcs);
 952     mcs->pxms_do_gen = pxmc_stop_gi;
 953   }
 954
 955   return 0;
 956 }
 957
 958 pxmc_call_t *pxmc_get_hh_gi_4axis(pxmc_state_t *mcs)
 959 {
 960   return pxmc_rocon_hh_gi;
 961 }
 962
 963 pxmc_state_t mcs0 =
 964 {
 965 pxms_flg:
 966   PXMS_ENI_m,
 967 pxms_do_inp:
 968   pxmc_inp_rocon_inp,
 969 pxms_do_con:
 970   pxmc_pid_con /*pxmc_dummy_con*/,
 971 pxms_do_out:
 972   pxmc_rocon_pwm_dc_out /*pxmc_rocon_pwm3ph_out*/,
 973   pxms_do_deb: 0,
 974   pxms_do_gen: 0,
 975 pxms_do_ap2hw:
 976   pxmc_inp_rocon_ap2hw,
 977   pxms_ap: 0, pxms_as: 0,
 978   pxms_rp: 55 * 256, pxms_rs: 0, pxms_subdiv: 8,
 979   pxms_md: 800 << 8, pxms_ms: 1000, pxms_ma: 10,
 980   pxms_inp_info: 0,
 981   pxms_out_info: 0,
 982   pxms_ene: 0, pxms_erc: 0,
 983   pxms_p: 80, pxms_i: 30, pxms_d: 200, pxms_s1: 200, pxms_s2: 0,
 984   pxms_me: 0x7e00/*0x7fff*/,
 985 pxms_cfg:
 986   PXMS_CFG_SMTH_m | PXMS_CFG_MD2E_m | PXMS_CFG_HLS_m | PXMS_CFG_HPS_m * 0 |
 987   PXMS_CFG_HRI_m * 0 | PXMS_CFG_HDIR_m * 0 |
 988   PXMS_CFG_I2PT_m * 0 | 0x2,
 989
 990   pxms_ptper: 1,
 991   pxms_ptirc: 1000,
 992   pxms_ptmark: 1180,
 993   /*pxms_ptamp: 0x7fff,*/
 994
 995   pxms_hal: 0x40,
 996 };
 997
 998 pxmc_state_t mcs1 =
 999 {
1000 pxms_flg:
1001   PXMS_ENI_m,
1002 pxms_do_inp:
1003   pxmc_inp_rocon_inp,
1004 pxms_do_con:
1005   pxmc_pid_con,
1006 pxms_do_out:
1007   pxmc_rocon_pwm_dc_out,
1008   pxms_do_deb: 0,
1009   pxms_do_gen: 0,
1010 pxms_do_ap2hw:
1011   pxmc_inp_rocon_ap2hw,
1012   pxms_ap: 0, pxms_as: 0,
1013   pxms_rp: 55 * 256, pxms_rs: 0, pxms_subdiv: 8,
1014   pxms_md: 800 << 8, pxms_ms: 1000, pxms_ma: 10,
1015   pxms_inp_info: 1,
1016   pxms_out_info: 2,
1017   pxms_ene: 0, pxms_erc: 0,
1018   pxms_p: 80, pxms_i: 30, pxms_d: 200, pxms_s1: 200, pxms_s2: 0,
1019   pxms_me: 0x7e00/*0x7fff*/,
1020 pxms_cfg:
1021   PXMS_CFG_SMTH_m | PXMS_CFG_MD2E_m | PXMS_CFG_HLS_m | PXMS_CFG_HPS_m * 0 |
1022   PXMS_CFG_HRI_m * 0 | PXMS_CFG_HDIR_m * 0 |
1023   PXMS_CFG_I2PT_m * 0 | 0x2,
1024
1025   pxms_ptper: 1,
1026   pxms_ptirc: 1000,
1027   /*pxms_ptamp: 0x7fff,*/
1028
1029   pxms_hal: 0x40,
1030 };
1031
1032 pxmc_state_t mcs2 =
1033 {
1034 pxms_flg:
1035   PXMS_ENI_m,
1036 pxms_do_inp:
1037   pxmc_inp_rocon_inp,
1038 pxms_do_con:
1039   pxmc_pid_con,
1040 pxms_do_out:
1041   pxmc_rocon_pwm_dc_out,
1042   pxms_do_deb: 0,
1043   pxms_do_gen: 0,
1044 pxms_do_ap2hw:
1045   pxmc_inp_rocon_ap2hw,
1046   pxms_ap: 0, pxms_as: 0,
1047   pxms_rp: 55 * 256, pxms_rs: 0, pxms_subdiv: 8,
1048   pxms_md: 800 << 8, pxms_ms: 1000, pxms_ma: 10,
1049   pxms_inp_info: 2,
1050   pxms_out_info: 4,
1051   pxms_ene: 0, pxms_erc: 0,
1052   pxms_p: 80, pxms_i: 30, pxms_d: 200, pxms_s1: 200, pxms_s2: 0,
1053   pxms_me: 0x7e00/*0x7fff*/,
1054 pxms_cfg:
1055   PXMS_CFG_SMTH_m | PXMS_CFG_MD2E_m | PXMS_CFG_HLS_m | PXMS_CFG_HPS_m * 0 |
1056   PXMS_CFG_I2PT_m * 0 | PXMS_CFG_HRI_m |
1057   PXMS_CFG_HDIR_m | 0x2,
1058
1059   pxms_ptper: 1,
1060   pxms_ptirc: 1000,
1061   /*pxms_ptamp: 0x7fff,*/
1062
1063   pxms_hal: 0x40,
1064 };
1065
1066 pxmc_state_t mcs3 =
1067 {
1068 pxms_flg:
1069   PXMS_ENI_m,
1070 pxms_do_inp:
1071   pxmc_inp_rocon_inp,
1072 pxms_do_con:
1073   pxmc_pid_con,
1074 pxms_do_out:
1075   pxmc_rocon_pwm_dc_out,
1076   pxms_do_deb: 0,
1077   pxms_do_gen: 0,
1078 pxms_do_ap2hw:
1079   pxmc_inp_rocon_ap2hw,
1080   pxms_ap: 0, pxms_as: 0,
1081   pxms_rp: 55 * 256, pxms_rs: 0, pxms_subdiv: 8,
1082   pxms_md: 800 << 8, pxms_ms: 1000, pxms_ma: 10,
1083   pxms_inp_info: 3,
1084   pxms_out_info: 6,
1085   pxms_ene: 0, pxms_erc: 0,
1086   pxms_p: 80, pxms_i: 30, pxms_d: 200, pxms_s1: 200, pxms_s2: 0,
1087   pxms_me: 0x7e00/*0x7fff*/,
1088 pxms_cfg:
1089   PXMS_CFG_SMTH_m | PXMS_CFG_MD2E_m | PXMS_CFG_HLS_m | PXMS_CFG_HPS_m * 0 |
1090   PXMS_CFG_I2PT_m * 0 | PXMS_CFG_HRI_m |
1091   PXMS_CFG_HDIR_m * 0 | 0x2,
1092
1093   pxms_ptper: 1,
1094   pxms_ptirc: 1000,
1095   /*pxms_ptamp: 0x7fff,*/
1096
1097   pxms_hal: 0x40,
1098 };
1099
1100 pxmc_state_t mcs4 =
1101 {
1102 pxms_flg:
1103   PXMS_ENI_m,
1104 pxms_do_inp:
1105   pxmc_inp_rocon_inp,
1106 pxms_do_con:
1107   pxmc_pid_con,
1108 pxms_do_out:
1109   pxmc_rocon_pwm_dc_out,
1110   pxms_do_deb: 0,
1111   pxms_do_gen: 0,
1112 pxms_do_ap2hw:
1113   pxmc_inp_rocon_ap2hw,
1114   pxms_ap: 0, pxms_as: 0,
1115   pxms_rp: 55 * 256, pxms_rs: 0, pxms_subdiv: 8,
1116   pxms_md: 800 << 8, pxms_ms: 1000, pxms_ma: 10,
1117   pxms_inp_info: 4,
1118   pxms_out_info: 8,
1119   pxms_ene: 0, pxms_erc: 0,
1120   pxms_p: 80, pxms_i: 30, pxms_d: 200, pxms_s1: 200, pxms_s2: 0,
1121   pxms_me: 0x7e00/*0x7fff*/,
1122 pxms_cfg:
1123   PXMS_CFG_SMTH_m | PXMS_CFG_MD2E_m | PXMS_CFG_HLS_m | PXMS_CFG_HPS_m * 0 |
1124   PXMS_CFG_HRI_m | PXMS_CFG_I2PT_m * 0 |
1125   PXMS_CFG_HDIR_m | 0x2,
1126
1127   pxms_ptper: 1,
1128   pxms_ptirc: 1000,
1129   /*pxms_ptamp: 0x7fff,*/
1130
1131   pxms_hal: 0x40,
1132 };
1133
1134 pxmc_state_t mcs5 =
1135 {
1136 pxms_flg:
1137   PXMS_ENI_m,
1138 pxms_do_inp:
1139   pxmc_inp_rocon_inp,
1140 pxms_do_con:
1141   pxmc_pid_con,
1142 pxms_do_out:
1143   pxmc_rocon_pwm_dc_out,
1144   pxms_do_deb: 0,
1145   pxms_do_gen: 0,
1146 pxms_do_ap2hw:
1147   pxmc_inp_rocon_ap2hw,
1148   pxms_ap: 0, pxms_as: 0,
1149   pxms_rp: 55 * 256, pxms_rs: 0, pxms_subdiv: 8,
1150   pxms_md: 800 << 8, pxms_ms: 1000, pxms_ma: 10,
1151   pxms_inp_info: 5,
1152   pxms_out_info: 10,
1153   pxms_ene: 0, pxms_erc: 0,
1154   pxms_p: 80, pxms_i: 30, pxms_d: 200, pxms_s1: 200, pxms_s2: 0,
1155   pxms_me: 0x7e00/*0x7fff*/,
1156 pxms_cfg:
1157   PXMS_CFG_SMTH_m | PXMS_CFG_MD2E_m | PXMS_CFG_HLS_m | PXMS_CFG_HPS_m |
1158   PXMS_CFG_HRI_m | PXMS_CFG_I2PT_m * 0 |
1159   PXMS_CFG_HDIR_m | 0x2,
1160
1161   pxms_ptper: 1,
1162   pxms_ptirc: 1000,
1163   /*pxms_ptamp: 0x7fff,*/
1164
1165   pxms_hal: 0x40,
1166 };
1167
1168 pxmc_state_t mcs6 =
1169 {
1170 pxms_flg:
1171   PXMS_ENI_m,
1172 pxms_do_inp:
1173   pxmc_inp_rocon_inp,
1174 pxms_do_con:
1175   pxmc_pid_con,
1176 pxms_do_out:
1177   pxmc_rocon_pwm_dc_out,
1178   pxms_do_deb: 0,
1179   pxms_do_gen: 0,
1180 pxms_do_ap2hw:
1181   pxmc_inp_rocon_ap2hw,
1182   pxms_ap: 0, pxms_as: 0,
1183   pxms_rp: 55 * 256, pxms_rs: 0, pxms_subdiv: 8,
1184   pxms_md: 800 << 8, pxms_ms: 1000, pxms_ma: 10,
1185   pxms_inp_info: 6,
1186   pxms_out_info: 12,
1187   pxms_ene: 0, pxms_erc: 0,
1188   pxms_p: 80, pxms_i: 30, pxms_d: 200, pxms_s1: 200, pxms_s2: 0,
1189   pxms_me: 0x7e00/*0x7fff*/,
1190 pxms_cfg:
1191   PXMS_CFG_SMTH_m | PXMS_CFG_MD2E_m | PXMS_CFG_HLS_m | PXMS_CFG_I2PT_m * 0 |
1192   0x1,
1193
1194   pxms_ptper: 1,
1195   pxms_ptirc: 1000,
1196   /*pxms_ptamp: 0x7fff,*/
1197
1198   pxms_hal: 0x40,
1199 };
1200
1201 pxmc_state_t mcs7 =
1202 {
1203 pxms_flg:
1204   PXMS_ENI_m,
1205 pxms_do_inp:
1206   pxmc_inp_rocon_inp,
1207 pxms_do_con:
1208   pxmc_pid_con,
1209 pxms_do_out:
1210   pxmc_rocon_pwm_dc_out,
1211   pxms_do_deb: 0,
1212   pxms_do_gen: 0,
1213 pxms_do_ap2hw:
1214   pxmc_inp_rocon_ap2hw,
1215   pxms_ap: 0, pxms_as: 0,
1216   pxms_rp: 55 * 256, pxms_rs: 0, pxms_subdiv: 8,
1217   pxms_md: 800 << 8, pxms_ms: 1000, pxms_ma: 10,
1218   pxms_inp_info: 7,
1219   pxms_out_info: 14,
1220   pxms_ene: 0, pxms_erc: 0,
1221   pxms_p: 80, pxms_i: 30, pxms_d: 200, pxms_s1: 200, pxms_s2: 0,
1222   pxms_me: 0x7e00/*0x7fff*/,
1223 pxms_cfg:
1224   PXMS_CFG_SMTH_m | PXMS_CFG_MD2E_m | PXMS_CFG_HLS_m | PXMS_CFG_I2PT_m * 0 |
1225   0x1,
1226
1227   pxms_ptper: 1,
1228   pxms_ptirc: 1000,
1229   /*pxms_ptamp: 0x7fff,*/
1230
1231   pxms_hal: 0x40,
1232 };
1233
1234
1235 pxmc_state_t *pxmc_main_arr[PXML_MAIN_CNT] =
1236 {&mcs0, &mcs1, &mcs2, &mcs3, &mcs4, &mcs5, &mcs6, &mcs7};
1237
1238
1239 pxmc_state_list_t  pxmc_main_list =
1240 {
1241 pxml_arr:
1242   pxmc_main_arr,
1243   pxml_cnt: 0
1244 };
1245
1246 void pxmc_lpc_qei_callback_index(struct lpc_qei_state_t *qst, void *context)
1247 {
1248   pxmc_state_t *mcs = (pxmc_state_t *)context;
1249   short ofs;
1250   short irc;
1251   irc = qst->index_pos;
1252
1253   if ((mcs->pxms_cfg & PXMS_CFG_I2PT_m) && (mcs->pxms_flg & PXMS_PTI_m))
1254   {
1255     short diff;
1256     ofs = irc - mcs->pxms_ptmark;
1257     diff = ofs - mcs->pxms_ptofs;
1258
1259     if (diff >= mcs->pxms_ptirc / 2)
1260       diff -= mcs->pxms_ptirc;
1261
1262     if (diff <= -mcs->pxms_ptirc / 2)
1263       diff += mcs->pxms_ptirc;
1264
1265     if (diff < 0)
1266       diff = -diff;
1267
1268     if (diff >= mcs->pxms_ptirc / 6)
1269     {
1270       pxmc_set_errno(mcs, PXMS_E_I2PT_TOOBIG);
1271     }
1272     else
1273     {
1274       mcs->pxms_ptofs = ofs;
1275       pxmc_set_flag(mcs, PXMS_PHA_b);
1276     }
1277   } /*else {
1278
1279     ofs=irc-mcs->pxms_ptofs;
1280     if((unsigned short)ofs>=(unsigned short)mcs->pxms_ptirc) {
1281       if(ofs>0) {
1282         ofs-=mcs->pxms_ptirc;
1283       } else {
1284         ofs+=mcs->pxms_ptirc;
1285       }
1286     }
1287     mcs->pxms_ptmark=ofs;
1288   }*/
1289
1290   /*lpc_qei_set_cmpos(qst, 0, qst->index_pos-4000);*/
1291 }
1292
1293 int pxmc_lpc_pthalalign_callback(pxmc_state_t *mcs)
1294 {
1295   short ptofs;
1296   short ptmark;
1297   lpc_qei_state_t *qst = &lpc_qei_state;
1298   int irc = lpc_qei_get_pos(qst);
1299   int idx_rel;
1300
1301   if (!qst->index_occ)
1302     return 0;
1303
1304   idx_rel = qst->index_pos - irc;
1305   idx_rel %= mcs->pxms_ptirc;
1306
1307   if (idx_rel < 0)
1308     idx_rel += mcs->pxms_ptirc;
1309
1310   ptofs = irc - mcs->pxms_ptofs;
1311   ptmark = ptofs + idx_rel;
1312
1313   if ((unsigned short)ptmark >= mcs->pxms_ptirc)
1314   {
1315     if (ptmark > 0)
1316       ptmark -= mcs->pxms_ptirc;
1317     else
1318       ptmark += mcs->pxms_ptirc;
1319   }
1320
1321   if ((unsigned short)ptmark < mcs->pxms_ptirc)
1322   {
1323     mcs->pxms_ptmark = ptmark;
1324   }
1325
1326   return 0;
1327 }
1328
1329 int pxmc_lpc_pthalalign(pxmc_state_t *mcs, int periods)
1330 {
1331   int res;
1332   long r2acq;
1333   long spd;
1334   pxmc_call_t *fin_fnc = pxmc_lpc_pthalalign_callback;
1335   lpc_qei_state_t *qst = &lpc_qei_state;
1336
1337   mcs->pxms_cfg &= ~PXMS_CFG_I2PT_m;
1338   lpc_qei_setup_index_catch(qst);
1339
1340   r2acq = ((long)mcs->pxms_ptirc << PXMC_SUBDIV(mcs)) * periods;
1341   spd = 1 << PXMC_SUBDIV(mcs);
1342
1343   res = pxmc_pthalalign(mcs, r2acq, spd, fin_fnc);
1344
1345   return res;
1346 }
1347
1348 int pxmc_lpc_pthalalign_run(void)
1349 {
1350   return pxmc_lpc_pthalalign(pxmc_main_list.pxml_arr[0], 20);
1351 }
1352
1353 static inline void pxmc_sfi_input(void)
1354 {
1355   int var;
1356   pxmc_state_t *mcs;
1357   pxmc_for_each_mcs(var, mcs)
1358   {
1359     /* PXMS_ENI_m - check if input (IRC) update is enabled */
1360     if (mcs->pxms_flg & PXMS_ENI_m)
1361     {
1362       pxmc_call(mcs, mcs->pxms_do_inp);
1363     }
1364   }
1365 }
1366
1367 static inline void pxmc_sfi_controller_and_output(void)
1368 {
1369   int var;
1370   pxmc_state_t *mcs;
1371   pxmc_for_each_mcs(var, mcs)
1372   {
1373     /* PXMS_ENR_m - check if controller is enabled */
1374     if (mcs->pxms_flg & PXMS_ENR_m || mcs->pxms_flg & PXMS_ENO_m)
1375     {
1376
1377       /* If output only is enabled, we skip the controller */
1378       if (mcs->pxms_flg & PXMS_ENR_m)
1379       {
1380
1381         pxmc_call(mcs, mcs->pxms_do_con);
1382
1383         /* PXMS_ERR_m - if axis in error state */
1384         if (mcs->pxms_flg & PXMS_ERR_m)
1385           mcs->pxms_ene = 0;
1386       }
1387
1388       /* for bushless motors, it is necessary to call do_out
1389         even if the controller is not enabled and PWM should be provided. */
1390       pxmc_call(mcs, mcs->pxms_do_out);
1391     }
1392   }
1393 }
1394
1395 static inline void pxmc_sfi_generator(void)
1396 {
1397   int var;
1398   pxmc_state_t *mcs;
1399   pxmc_for_each_mcs(var, mcs)
1400   {
1401     /* PXMS_ENG_m - check if requested value (position) generator is enabled */
1402     if (mcs->pxms_flg & PXMS_ENG_m)
1403     {
1404       pxmc_call(mcs, mcs->pxms_do_gen);
1405     }
1406   }
1407 }
1408
1409 static inline void pxmc_sfi_dbg(void)
1410 {
1411   int var;
1412   pxmc_state_t *mcs;
1413   pxmc_for_each_mcs(var, mcs)
1414   {
1415     if (mcs->pxms_flg & PXMS_DBG_m)
1416     {
1417       pxmc_call(mcs, mcs->pxms_do_deb);
1418     }
1419   }
1420 }
1421
1422 int pxmc_rocon_pthalalign_callback(pxmc_state_t *mcs)
1423 {
1424   short ptofs;
1425   short ptmark;
1426   int inp_chan = mcs->pxms_inp_info;
1427   int idx_rel;
1428   long irc = fpga_irc[inp_chan]->count;
1429
1430   if (!pxmc_inp_rocon_is_index_edge(mcs))
1431     return 0;
1432
1433   idx_rel = fpga_irc[inp_chan]->count_index - irc;
1434   idx_rel %= mcs->pxms_ptirc;
1435   if(idx_rel < 0)
1436     idx_rel += mcs->pxms_ptirc;
1437
1438   ptofs = irc-mcs->pxms_ptofs;
1439   ptmark = ptofs+idx_rel;
1440
1441   if((unsigned short)ptmark >= mcs->pxms_ptirc) {
1442     if(ptmark>0)
1443       ptmark -= mcs->pxms_ptirc;
1444     else
1445       ptmark += mcs->pxms_ptirc;
1446   }
1447
1448   if((unsigned short)ptmark < mcs->pxms_ptirc) {
1449     mcs->pxms_ptmark = ptmark;
1450   }
1451   return 0;
1452 }
1453
1454 int pxmc_rocon_pthalalign(pxmc_state_t *mcs, int periods)
1455 {
1456   int res;
1457   long r2acq;
1458   long spd;
1459   pxmc_call_t *fin_fnc = pxmc_rocon_pthalalign_callback;
1460
1461   mcs->pxms_cfg&=~PXMS_CFG_I2PT_m;
1462
1463   r2acq=((long)mcs->pxms_ptirc<<PXMC_SUBDIV(mcs))*periods;
1464   spd=1<<PXMC_SUBDIV(mcs);
1465
1466   /* clear bit indicating previous index */
1467   pxmc_inp_rocon_is_index_edge(mcs);
1468
1469   res=pxmc_pthalalign(mcs, r2acq, spd, fin_fnc);
1470
1471   return res;
1472 }
1473
1474 int pxmc_axis_rdmode(pxmc_state_t *mcs)
1475 {
1476   if (mcs->pxms_do_out == pxmc_rocon_pwm2ph_out)
1477     return PXMC_AXIS_MODE_STEPPER_WITH_IRC;
1478   if (mcs->pxms_do_out == pxmc_rocon_pwm_dc_out)
1479     return PXMC_AXIS_MODE_DC;
1480   if (mcs->pxms_do_out == pxmc_rocon_pwm3ph_out)
1481     return PXMC_AXIS_MODE_BLDC;
1482   return -1;
1483 }
1484
1485
1486 int
1487 pxmc_axis_pt4mode(pxmc_state_t *mcs, int mode)
1488 {
1489   static const typeof(*mcs->pxms_ptptr1) dummy0 = 0;
1490   int res = 0;
1491
1492   if (mode == PXMC_AXIS_MODE_NOCHANGE)
1493     mode = pxmc_axis_rdmode(mcs);
1494   if (mode < 0)
1495     return -1;
1496
1497   switch (mode) {
1498     /*case PXMC_AXIS_MODE_STEPPER:*/
1499     case PXMC_AXIS_MODE_STEPPER_WITH_IRC:
1500       res = pxmc_ptable_set_profile(mcs, &pxmc_ptprofile_sin, 0, 0);
1501       break;
1502     /*case PXMC_AXIS_MODE_STEPPER_WITH_PWM:*/
1503     case PXMC_AXIS_MODE_DC:
1504       /*profive some sane dummy values*/
1505       mcs->pxms_ptptr1 = (typeof(mcs->pxms_ptptr1))&dummy0;
1506       mcs->pxms_ptptr2 = (typeof(mcs->pxms_ptptr1))&dummy0;
1507       mcs->pxms_ptptr3 = (typeof(mcs->pxms_ptptr1))&dummy0;
1508
1509       mcs->pxms_ptscale_mult=1;
1510       mcs->pxms_ptscale_shift=15;
1511       break;
1512     case PXMC_AXIS_MODE_BLDC:
1513       /* res = pxmc_init_ptable(mcs, PXMC_PTPROF_SIN3FUP); */
1514      #ifndef PXMC_WITH_PT_ZIC
1515       res = pxmc_ptable_set_profile(mcs, &pxmc_ptprofile_sin3phup, 0, 0);
1516      #else /*PXMC_WITH_PT_ZIC*/
1517       res = pxmc_ptable_set_profile(mcs, &pxmc_ptprofile_sin3phup_zic, 0, 0);
1518      #endif /*PXMC_WITH_PT_ZIC*/
1519       break;
1520     default:
1521       return -1;
1522   }
1523
1524   mcs->pxms_ptvang = pxmc_ptvang_deg2irc(mcs, 90);
1525
1526   return res;
1527 }
1528
1529 /**
1530  * pxmc_axis_mode - Sets axis mode.[extern API]
1531  * @mcs:        Motion controller state information
1532  * @mode:       0 .. previous mode, 1 .. stepper motor mode,
1533  *              2 .. stepper motor with IRC feedback and PWM ,
1534  *              3 .. stepper motor with PWM control
1535  *              4 .. DC motor with IRC feedback and PWM
1536  *
1537  */
1538 int
1539 pxmc_axis_mode(pxmc_state_t *mcs, int mode)
1540 {
1541   int res;
1542
1543   pxmc_set_const_out(mcs, 0);
1544   pxmc_clear_flag(mcs, PXMS_ENI_b);
1545   pxmc_clear_flags(mcs,PXMS_ENO_m);
1546   /* Clear possible stall index flags from hardware */
1547   pxmc_inp_rocon_is_index_edge(mcs);
1548   pxmc_clear_flag(mcs, PXMS_PHA_b);
1549   pxmc_clear_flag(mcs, PXMS_PTI_b);
1550
1551
1552   if (mode == PXMC_AXIS_MODE_NOCHANGE)
1553     mode = pxmc_axis_rdmode(mcs);
1554   if (mode < 0)
1555     return -1;
1556   if (!mode)
1557     mode = PXMC_AXIS_MODE_DC;
1558
1559   res = pxmc_axis_pt4mode(mcs, mode);
1560   if (res < 0)
1561     return -1;
1562
1563   switch (mode) {
1564     /*case PXMC_AXIS_MODE_STEPPER:*/
1565     case PXMC_AXIS_MODE_STEPPER_WITH_IRC:
1566       mcs->pxms_do_out = pxmc_rocon_pwm2ph_out;
1567       break;
1568     /*case PXMC_AXIS_MODE_STEPPER_WITH_PWM:*/
1569     case PXMC_AXIS_MODE_DC:
1570       mcs->pxms_do_out = pxmc_rocon_pwm_dc_out;
1571       break;
1572     case PXMC_AXIS_MODE_BLDC:
1573       mcs->pxms_do_out = pxmc_rocon_pwm3ph_out;
1574       break;
1575     default:
1576       return -1;
1577   }
1578
1579   /* Clear possible stall index flags from hardware */
1580   pxmc_inp_rocon_is_index_edge(mcs);
1581   /* Request new phases alignment for changed parameters */
1582   pxmc_clear_flag(mcs, PXMS_PHA_b);
1583   pxmc_clear_flag(mcs, PXMS_PTI_b);
1584   pxmc_set_flag(mcs, PXMS_ENI_b);
1585   return res;
1586 }
1587
1588 void pxmc_sfi_isr(void)
1589 {
1590   unsigned long spent = pxmc_fast_tick_time();
1591
1592   pxmc_sfi_input();
1593   pxmc_sfi_controller_and_output();
1594   pxmc_sfi_generator();
1595   pxmc_sfi_dbg();
1596   /* Kick LX Master watchdog */
1597   if (pxmc_main_list.pxml_cnt != 0)
1598     *fpga_lx_master_transmitter_wdog = 1;
1599
1600   spent = pxmc_fast_tick_time() - spent;
1601
1602   if(spent > pxmc_sfi_spent_time_max)
1603     pxmc_sfi_spent_time_max = spent;
1604
1605 }
1606
1607 int pxmc_clear_power_stop(void)
1608 {
1609   return 0;
1610 }
1611
1612 int pxmc_process_state_check(unsigned long *pbusy_bits,
1613                              unsigned long *perror_bits)
1614 {
1615   unsigned short flg;
1616   unsigned short chan;
1617   unsigned long busy_bits = 0;
1618   unsigned long error_bits = 0;
1619   pxmc_state_t *mcs;
1620   flg=0;
1621   pxmc_for_each_mcs(chan, mcs) {
1622     if(mcs) {
1623       flg |= mcs->pxms_flg;
1624       if (mcs->pxms_flg & PXMS_BSY_m)
1625         busy_bits |= 1 << chan;
1626       if (mcs->pxms_flg & PXMS_ERR_m)
1627         error_bits |= 1 << chan;
1628     }
1629   }
1630   if (appl_errstop_mode) {
1631     if((flg & PXMS_ENG_m) && (flg & PXMS_ERR_m)) {
1632       pxmc_for_each_mcs(chan, mcs) {
1633         if(mcs&&(mcs->pxms_flg & PXMS_ENG_m)) {
1634           pxmc_stop(mcs, 0);
1635         }
1636       }
1637     }
1638   }
1639
1640   if (pbusy_bits != NULL)
1641     *pbusy_bits = busy_bits;
1642   if (error_bits != NULL)
1643     *perror_bits = error_bits;
1644
1645   return flg;
1646 }
1647
1648 int pxmc_done(void)
1649 {
1650   int var;
1651   pxmc_state_t *mcs;
1652
1653   if (!pxmc_main_list.pxml_cnt)
1654     return 0;
1655
1656   pxmc_for_each_mcs(var, mcs)
1657   {
1658     pxmc_set_const_out(mcs,0);
1659   }
1660
1661   pxmc_main_list.pxml_cnt = 0;
1662   __memory_barrier();
1663
1664   return 0;
1665 }
1666
1667 int pxmc_initialize(void)
1668 {
1669   int res;
1670   int i;
1671
1672   pxmc_state_t *mcs = &mcs0;
1673   lpc_qei_state_t *qst = &lpc_qei_state;
1674
1675   *fpga_irc_reset = 1;
1676
1677   for (i = 0; i < 8; i++) {
1678     fpga_irc[i]->count = 0;
1679     fpga_irc[i]->count_index = 0;
1680     *fpga_irc_state[i] = FPGA_IRC_STATE_INDEX_EVENT_MASK;
1681   }
1682
1683   /* Initialize QEI module for IRC counting */
1684   pxmc_inp_lpc_qei_init(mcs->pxms_inp_info);
1685
1686   /* Initialize QEI events processing */
1687   if (lpc_qei_setup_irq(qst) < 0)
1688     return -1;
1689
1690   *fpga_irc_reset = 0;
1691
1692   //res = pxmc_axis_pt4mode(mcs, PXMC_AXIS_MODE_BLDC);
1693
1694   /*pxmc_ctm4pwm3f_wr(mcs, 0, 0, 0);*/
1695   //pxmc_rocon_pwm3ph_wr(mcs, 0, 0, 0);
1696
1697   pxmc_rocon_pwm_master_init();
1698
1699   pxmc_main_list.pxml_cnt = 0;
1700   pxmc_dbg_hist = NULL;
1701   __memory_barrier();
1702   pxmc_main_list.pxml_cnt = PXML_MAIN_CNT;
1703
1704   return 0;
1705 }