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
 729   for (i = 0; i < LX_MASTER_DATA_OFFS + lxpwr_words * lxpwr_chips; i++)
 730     fpga_lx_master_receiver_base[i] = 0;
 731
 732   word_slot = LX_MASTER_DATA_OFFS;
 733   fpga_lx_master_receiver_base[grp_in++] = (word_slot << 8) | lxpwr_words;
 734   fpga_lx_master_receiver_base[grp_in++] = 0x0000;
 735
 736   for (i = 0; i < LX_MASTER_DATA_OFFS + lxpwr_words * lxpwr_chips; i++)
 737     fpga_lx_master_transmitter_base[i] = 0;
 738
 739   word_slot = LX_MASTER_DATA_OFFS + lxpwr_header + lxpwr_chip_pwm_cnt;
 740   fpga_lx_master_transmitter_base[grp_out++] = (word_slot << 8) | lxpwr_words;
 741  #ifdef LXPWR_WITH_SIROLADC
 742   fpga_lx_master_transmitter_base[word_slot] = 0xc100 | (lxpwr_words - 1);
 743  #endif /*LXPWR_WITH_SIROLADC*/
 744
 745   word_slot = LX_MASTER_DATA_OFFS + 0;
 746   fpga_lx_master_transmitter_base[grp_out++] = (word_slot << 8) | lxpwr_words;
 747  #ifdef LXPWR_WITH_SIROLADC
 748   fpga_lx_master_transmitter_base[word_slot] = 0xc100 | (lxpwr_words - 1);
 749  #endif /*LXPWR_WITH_SIROLADC*/
 750
 751   fpga_lx_master_transmitter_base[grp_out++] = 0x0000;
 752
 753   *fpga_lx_master_reset = 0;
 754
 755   return 0;
 756 }
 757
 758 int pxmc_ptofs_from_index(pxmc_state_t *mcs, unsigned long irc,
 759                            unsigned long index_irc, int diff2err)
 760 {
 761   long ofsl;
 762   short ofs;
 763
 764   ofs = ofsl = index_irc - mcs->pxms_ptmark;
 765
 766   if (diff2err) {
 767     short diff;
 768     diff = (unsigned short)ofs - (unsigned short)mcs->pxms_ptofs;
 769     if (diff >= mcs->pxms_ptirc / 2)
 770       diff -= mcs->pxms_ptirc;
 771     if (diff <= -mcs->pxms_ptirc / 2)
 772       diff += mcs->pxms_ptirc;
 773     if (diff < 0)
 774       diff = -diff;
 775     if(diff >= mcs->pxms_ptirc / 6) {
 776       return -1;
 777     }
 778   } else {
 779     long diff;
 780     diff = (unsigned long)ofsl - irc;
 781     ofs = ofsl - (diff / mcs->pxms_ptirc) * mcs->pxms_ptirc;
 782   }
 783
 784   mcs->pxms_ptofs = ofs;
 785
 786   return 1;
 787 }
 788
 789 /**
 790  * pxmc_dummy_con - Dummy controller for synchronous BLDC/PMSM/steper drive
 791  * @mcs:        Motion controller state information
 792  */
 793 int
 794 pxmc_dummy_con(pxmc_state_t *mcs)
 795 {
 796   return 0;
 797 }
 798
 799 int pxmc_rocon_hh_gd10(pxmc_state_t *mcs);
 800 int pxmc_rocon_hh_gd20(pxmc_state_t *mcs);
 801 int pxmc_rocon_hh_gd30(pxmc_state_t *mcs);
 802 int pxmc_rocon_hh_gd40(pxmc_state_t *mcs);
 803 int pxmc_rocon_hh_gd50(pxmc_state_t *mcs);
 804 int pxmc_rocon_hh_gd90(pxmc_state_t *mcs);
 805
 806 /* initialize for hard home */
 807 int
 808 pxmc_rocon_hh_gi(pxmc_state_t *mcs)
 809 {
 810   pxmc_set_flag(mcs,PXMS_BSY_b);
 811  #if 0 /* config and speed already set in pxmc_hh */
 812   spd = mcs->pxms_ms;
 813   spd >>= mcs->pxms_cfg & PXMS_CFG_HSPD_m;
 814   if(!spd) spd = 1;
 815   if(mcs->pxms_cfg & PXMS_CFG_HDIR_m)
 816     spd = -spd;
 817   mcs->pxms_gen_hsp = spd;
 818  #endif
 819   mcs->pxms_rsfg = 0;
 820   mcs->pxms_gen_htim = 16;
 821   mcs->pxms_do_gen = pxmc_rocon_hh_gd10;
 822   return pxmc_rocon_hh_gd10(mcs);
 823 }
 824
 825 /* fill initial mark filter and then decide */
 826 int
 827 pxmc_rocon_hh_gd10(pxmc_state_t *mcs)
 828 {
 829   int mark;
 830
 831   if(mcs->pxms_flg & PXMS_ERR_m)
 832     return pxmc_spdnext_gend(mcs);
 833
 834   pxmc_spd_gacc(&(mcs->pxms_rs), 0, mcs->pxms_ma);
 835   mcs->pxms_rp += mcs->pxms_rs;
 836
 837   mark = pxmc_inp_rocon_is_mark(mcs);
 838
 839   if (mcs->pxms_gen_htim) {
 840     mcs->pxms_gen_htim--;
 841     return 0;
 842   }
 843
 844   if (!(mcs->pxms_cfg & PXMS_CFG_HLS_m)) {
 845     /* limit switch is not used */
 846     pxmc_inp_rocon_is_index_edge(mcs);
 847     mcs->pxms_do_gen = pxmc_rocon_hh_gd50;
 848     return 0;
 849   }
 850
 851   if (mark >= 6) {
 852     /* go out from switch */
 853     mcs->pxms_do_gen = pxmc_rocon_hh_gd20;
 854   } else {
 855     /* switch is off -> look for it */
 856     mcs->pxms_do_gen = pxmc_rocon_hh_gd30;
 857   }
 858   return 0;
 859 }
 860
 861 /* go out from switch */
 862 int
 863 pxmc_rocon_hh_gd20(pxmc_state_t *mcs)
 864 {
 865   int mark;
 866
 867   if(mcs->pxms_flg & PXMS_ERR_m)
 868     return pxmc_spdnext_gend(mcs);
 869
 870   mark = pxmc_inp_rocon_is_mark(mcs);
 871
 872   if (mark <= 1) {
 873     /* switch is off -> look for it again */
 874     mcs->pxms_do_gen = pxmc_rocon_hh_gd30;
 875   }
 876
 877   pxmc_spd_gacc(&(mcs->pxms_rs), -mcs->pxms_gen_hsp, mcs->pxms_ma);
 878   mcs->pxms_rp += mcs->pxms_rs;
 879
 880   return 0;
 881 }
 882
 883 /* switch is off -> look for it */
 884 int
 885 pxmc_rocon_hh_gd30(pxmc_state_t *mcs)
 886 {
 887   long spd;
 888   int mark;
 889
 890   if (mcs->pxms_flg & PXMS_ERR_m)
 891     return pxmc_spdnext_gend(mcs);
 892
 893   pxmc_spd_gacc(&(mcs->pxms_rs), mcs->pxms_gen_hsp, mcs->pxms_ma);
 894   mcs->pxms_rp += mcs->pxms_rs;
 895
 896   mark = pxmc_inp_rocon_is_mark(mcs);
 897
 898   if (mark >= 8){
 899     spd = mcs->pxms_gen_hsp >> 2; /* slow down */
 900     if (!spd)
 901       spd = 1;
 902     mcs->pxms_gen_hsp = spd;
 903     mcs->pxms_do_gen = pxmc_rocon_hh_gd40;
 904   }
 905
 906   return 0;
 907 }
 908
 909 /* switch is on -> find edge */
 910 int
 911 pxmc_rocon_hh_gd40(pxmc_state_t *mcs)
 912 {
 913   int mark;
 914
 915   if (mcs->pxms_flg & PXMS_ERR_m)
 916     return pxmc_spdnext_gend(mcs);
 917
 918   mark = pxmc_inp_rocon_is_mark(mcs);
 919
 920   if (mark <= 1) {
 921     if (mcs->pxms_cfg & PXMS_CFG_HRI_m) {
 922       pxmc_inp_rocon_is_index_edge(mcs);
 923       mcs->pxms_do_gen = pxmc_rocon_hh_gd50;
 924     } else {
 925       pxmc_inp_rocon_ap_zero(mcs);
 926       mcs->pxms_do_gen = pxmc_stop_gi;
 927     }
 928   }
 929
 930   pxmc_spd_gacc(&(mcs->pxms_rs), -mcs->pxms_gen_hsp, mcs->pxms_ma);
 931   mcs->pxms_rp += mcs->pxms_rs;
 932
 933   return 0;
 934 }
 935
 936 /* calibrate on revolution index */
 937 int
 938 pxmc_rocon_hh_gd50(pxmc_state_t *mcs)
 939 {
 940   if (mcs->pxms_flg & PXMS_ERR_m)
 941     return pxmc_spdnext_gend(mcs);
 942
 943   pxmc_spd_gacc(&(mcs->pxms_rs), mcs->pxms_gen_hsp, mcs->pxms_ma);
 944   mcs->pxms_rp += mcs->pxms_rs;
 945
 946   if (pxmc_inp_rocon_is_index_edge(mcs)){
 947     pxmc_inp_rocon_irc_offset_from_index(mcs);
 948     mcs->pxms_do_gen = pxmc_stop_gi;
 949   }
 950
 951   return 0;
 952 }
 953
 954 pxmc_call_t *pxmc_get_hh_gi_4axis(pxmc_state_t *mcs)
 955 {
 956   return pxmc_rocon_hh_gi;
 957 }
 958
 959 pxmc_state_t mcs0 =
 960 {
 961 pxms_flg:
 962   PXMS_ENI_m,
 963 pxms_do_inp:
 964   pxmc_inp_rocon_inp,
 965 pxms_do_con:
 966   pxmc_pid_con /*pxmc_dummy_con*/,
 967 pxms_do_out:
 968   pxmc_rocon_pwm_dc_out /*pxmc_rocon_pwm3ph_out*/,
 969   pxms_do_deb: 0,
 970   pxms_do_gen: 0,
 971 pxms_do_ap2hw:
 972   pxmc_inp_rocon_ap2hw,
 973   pxms_ap: 0, pxms_as: 0,
 974   pxms_rp: 55 * 256, pxms_rs: 0, pxms_subdiv: 8,
 975   pxms_md: 800 << 8, pxms_ms: 1000, pxms_ma: 10,
 976   pxms_inp_info: 0,
 977   pxms_out_info: 0,
 978   pxms_ene: 0, pxms_erc: 0,
 979   pxms_p: 80, pxms_i: 30, pxms_d: 200, pxms_s1: 200, pxms_s2: 0,
 980   pxms_me: 0x7e00/*0x7fff*/,
 981 pxms_cfg:
 982   PXMS_CFG_SMTH_m | PXMS_CFG_MD2E_m | PXMS_CFG_HLS_m | PXMS_CFG_HPS_m * 0 |
 983   PXMS_CFG_HRI_m * 0 | PXMS_CFG_HDIR_m * 0 |
 984   PXMS_CFG_I2PT_m * 0 | 0x2,
 985
 986   pxms_ptper: 1,
 987   pxms_ptirc: 1000,
 988   pxms_ptmark: 1180,
 989   /*pxms_ptamp: 0x7fff,*/
 990
 991   pxms_hal: 0x40,
 992 };
 993
 994 pxmc_state_t mcs1 =
 995 {
 996 pxms_flg:
 997   PXMS_ENI_m,
 998 pxms_do_inp:
 999   pxmc_inp_rocon_inp,
1000 pxms_do_con:
1001   pxmc_pid_con,
1002 pxms_do_out:
1003   pxmc_rocon_pwm_dc_out,
1004   pxms_do_deb: 0,
1005   pxms_do_gen: 0,
1006 pxms_do_ap2hw:
1007   pxmc_inp_rocon_ap2hw,
1008   pxms_ap: 0, pxms_as: 0,
1009   pxms_rp: 55 * 256, pxms_rs: 0, pxms_subdiv: 8,
1010   pxms_md: 800 << 8, pxms_ms: 1000, pxms_ma: 10,
1011   pxms_inp_info: 1,
1012   pxms_out_info: 2,
1013   pxms_ene: 0, pxms_erc: 0,
1014   pxms_p: 80, pxms_i: 30, pxms_d: 200, pxms_s1: 200, pxms_s2: 0,
1015   pxms_me: 0x7e00/*0x7fff*/,
1016 pxms_cfg:
1017   PXMS_CFG_SMTH_m | PXMS_CFG_MD2E_m | PXMS_CFG_HLS_m | PXMS_CFG_HPS_m * 0 |
1018   PXMS_CFG_HRI_m * 0 | PXMS_CFG_HDIR_m * 0 |
1019   PXMS_CFG_I2PT_m * 0 | 0x2,
1020
1021   pxms_ptper: 1,
1022   pxms_ptirc: 1000,
1023   /*pxms_ptamp: 0x7fff,*/
1024
1025   pxms_hal: 0x40,
1026 };
1027
1028 pxmc_state_t mcs2 =
1029 {
1030 pxms_flg:
1031   PXMS_ENI_m,
1032 pxms_do_inp:
1033   pxmc_inp_rocon_inp,
1034 pxms_do_con:
1035   pxmc_pid_con,
1036 pxms_do_out:
1037   pxmc_rocon_pwm_dc_out,
1038   pxms_do_deb: 0,
1039   pxms_do_gen: 0,
1040 pxms_do_ap2hw:
1041   pxmc_inp_rocon_ap2hw,
1042   pxms_ap: 0, pxms_as: 0,
1043   pxms_rp: 55 * 256, pxms_rs: 0, pxms_subdiv: 8,
1044   pxms_md: 800 << 8, pxms_ms: 1000, pxms_ma: 10,
1045   pxms_inp_info: 2,
1046   pxms_out_info: 4,
1047   pxms_ene: 0, pxms_erc: 0,
1048   pxms_p: 80, pxms_i: 30, pxms_d: 200, pxms_s1: 200, pxms_s2: 0,
1049   pxms_me: 0x7e00/*0x7fff*/,
1050 pxms_cfg:
1051   PXMS_CFG_SMTH_m | PXMS_CFG_MD2E_m | PXMS_CFG_HLS_m | PXMS_CFG_HPS_m * 0 |
1052   PXMS_CFG_I2PT_m * 0 | PXMS_CFG_HRI_m |
1053   PXMS_CFG_HDIR_m | 0x2,
1054
1055   pxms_ptper: 1,
1056   pxms_ptirc: 1000,
1057   /*pxms_ptamp: 0x7fff,*/
1058
1059   pxms_hal: 0x40,
1060 };
1061
1062 pxmc_state_t mcs3 =
1063 {
1064 pxms_flg:
1065   PXMS_ENI_m,
1066 pxms_do_inp:
1067   pxmc_inp_rocon_inp,
1068 pxms_do_con:
1069   pxmc_pid_con,
1070 pxms_do_out:
1071   pxmc_rocon_pwm_dc_out,
1072   pxms_do_deb: 0,
1073   pxms_do_gen: 0,
1074 pxms_do_ap2hw:
1075   pxmc_inp_rocon_ap2hw,
1076   pxms_ap: 0, pxms_as: 0,
1077   pxms_rp: 55 * 256, pxms_rs: 0, pxms_subdiv: 8,
1078   pxms_md: 800 << 8, pxms_ms: 1000, pxms_ma: 10,
1079   pxms_inp_info: 3,
1080   pxms_out_info: 6,
1081   pxms_ene: 0, pxms_erc: 0,
1082   pxms_p: 80, pxms_i: 30, pxms_d: 200, pxms_s1: 200, pxms_s2: 0,
1083   pxms_me: 0x7e00/*0x7fff*/,
1084 pxms_cfg:
1085   PXMS_CFG_SMTH_m | PXMS_CFG_MD2E_m | PXMS_CFG_HLS_m | PXMS_CFG_HPS_m * 0 |
1086   PXMS_CFG_I2PT_m * 0 | PXMS_CFG_HRI_m |
1087   PXMS_CFG_HDIR_m * 0 | 0x2,
1088
1089   pxms_ptper: 1,
1090   pxms_ptirc: 1000,
1091   /*pxms_ptamp: 0x7fff,*/
1092
1093   pxms_hal: 0x40,
1094 };
1095
1096 pxmc_state_t mcs4 =
1097 {
1098 pxms_flg:
1099   PXMS_ENI_m,
1100 pxms_do_inp:
1101   pxmc_inp_rocon_inp,
1102 pxms_do_con:
1103   pxmc_pid_con,
1104 pxms_do_out:
1105   pxmc_rocon_pwm_dc_out,
1106   pxms_do_deb: 0,
1107   pxms_do_gen: 0,
1108 pxms_do_ap2hw:
1109   pxmc_inp_rocon_ap2hw,
1110   pxms_ap: 0, pxms_as: 0,
1111   pxms_rp: 55 * 256, pxms_rs: 0, pxms_subdiv: 8,
1112   pxms_md: 800 << 8, pxms_ms: 1000, pxms_ma: 10,
1113   pxms_inp_info: 4,
1114   pxms_out_info: 8,
1115   pxms_ene: 0, pxms_erc: 0,
1116   pxms_p: 80, pxms_i: 30, pxms_d: 200, pxms_s1: 200, pxms_s2: 0,
1117   pxms_me: 0x7e00/*0x7fff*/,
1118 pxms_cfg:
1119   PXMS_CFG_SMTH_m | PXMS_CFG_MD2E_m | PXMS_CFG_HLS_m | PXMS_CFG_HPS_m * 0 |
1120   PXMS_CFG_HRI_m | PXMS_CFG_I2PT_m * 0 |
1121   PXMS_CFG_HDIR_m | 0x2,
1122
1123   pxms_ptper: 1,
1124   pxms_ptirc: 1000,
1125   /*pxms_ptamp: 0x7fff,*/
1126
1127   pxms_hal: 0x40,
1128 };
1129
1130 pxmc_state_t mcs5 =
1131 {
1132 pxms_flg:
1133   PXMS_ENI_m,
1134 pxms_do_inp:
1135   pxmc_inp_rocon_inp,
1136 pxms_do_con:
1137   pxmc_pid_con,
1138 pxms_do_out:
1139   pxmc_rocon_pwm_dc_out,
1140   pxms_do_deb: 0,
1141   pxms_do_gen: 0,
1142 pxms_do_ap2hw:
1143   pxmc_inp_rocon_ap2hw,
1144   pxms_ap: 0, pxms_as: 0,
1145   pxms_rp: 55 * 256, pxms_rs: 0, pxms_subdiv: 8,
1146   pxms_md: 800 << 8, pxms_ms: 1000, pxms_ma: 10,
1147   pxms_inp_info: 5,
1148   pxms_out_info: 10,
1149   pxms_ene: 0, pxms_erc: 0,
1150   pxms_p: 80, pxms_i: 30, pxms_d: 200, pxms_s1: 200, pxms_s2: 0,
1151   pxms_me: 0x7e00/*0x7fff*/,
1152 pxms_cfg:
1153   PXMS_CFG_SMTH_m | PXMS_CFG_MD2E_m | PXMS_CFG_HLS_m | PXMS_CFG_HPS_m |
1154   PXMS_CFG_HRI_m | PXMS_CFG_I2PT_m * 0 |
1155   PXMS_CFG_HDIR_m | 0x2,
1156
1157   pxms_ptper: 1,
1158   pxms_ptirc: 1000,
1159   /*pxms_ptamp: 0x7fff,*/
1160
1161   pxms_hal: 0x40,
1162 };
1163
1164 pxmc_state_t mcs6 =
1165 {
1166 pxms_flg:
1167   PXMS_ENI_m,
1168 pxms_do_inp:
1169   pxmc_inp_rocon_inp,
1170 pxms_do_con:
1171   pxmc_pid_con,
1172 pxms_do_out:
1173   pxmc_rocon_pwm_dc_out,
1174   pxms_do_deb: 0,
1175   pxms_do_gen: 0,
1176 pxms_do_ap2hw:
1177   pxmc_inp_rocon_ap2hw,
1178   pxms_ap: 0, pxms_as: 0,
1179   pxms_rp: 55 * 256, pxms_rs: 0, pxms_subdiv: 8,
1180   pxms_md: 800 << 8, pxms_ms: 1000, pxms_ma: 10,
1181   pxms_inp_info: 6,
1182   pxms_out_info: 12,
1183   pxms_ene: 0, pxms_erc: 0,
1184   pxms_p: 80, pxms_i: 30, pxms_d: 200, pxms_s1: 200, pxms_s2: 0,
1185   pxms_me: 0x7e00/*0x7fff*/,
1186 pxms_cfg:
1187   PXMS_CFG_SMTH_m | PXMS_CFG_MD2E_m | PXMS_CFG_HLS_m | PXMS_CFG_I2PT_m * 0 |
1188   0x1,
1189
1190   pxms_ptper: 1,
1191   pxms_ptirc: 1000,
1192   /*pxms_ptamp: 0x7fff,*/
1193
1194   pxms_hal: 0x40,
1195 };
1196
1197 pxmc_state_t mcs7 =
1198 {
1199 pxms_flg:
1200   PXMS_ENI_m,
1201 pxms_do_inp:
1202   pxmc_inp_rocon_inp,
1203 pxms_do_con:
1204   pxmc_pid_con,
1205 pxms_do_out:
1206   pxmc_rocon_pwm_dc_out,
1207   pxms_do_deb: 0,
1208   pxms_do_gen: 0,
1209 pxms_do_ap2hw:
1210   pxmc_inp_rocon_ap2hw,
1211   pxms_ap: 0, pxms_as: 0,
1212   pxms_rp: 55 * 256, pxms_rs: 0, pxms_subdiv: 8,
1213   pxms_md: 800 << 8, pxms_ms: 1000, pxms_ma: 10,
1214   pxms_inp_info: 7,
1215   pxms_out_info: 14,
1216   pxms_ene: 0, pxms_erc: 0,
1217   pxms_p: 80, pxms_i: 30, pxms_d: 200, pxms_s1: 200, pxms_s2: 0,
1218   pxms_me: 0x7e00/*0x7fff*/,
1219 pxms_cfg:
1220   PXMS_CFG_SMTH_m | PXMS_CFG_MD2E_m | PXMS_CFG_HLS_m | PXMS_CFG_I2PT_m * 0 |
1221   0x1,
1222
1223   pxms_ptper: 1,
1224   pxms_ptirc: 1000,
1225   /*pxms_ptamp: 0x7fff,*/
1226
1227   pxms_hal: 0x40,
1228 };
1229
1230
1231 pxmc_state_t *pxmc_main_arr[PXML_MAIN_CNT] =
1232 {&mcs0, &mcs1, &mcs2, &mcs3, &mcs4, &mcs5, &mcs6, &mcs7};
1233
1234
1235 pxmc_state_list_t  pxmc_main_list =
1236 {
1237 pxml_arr:
1238   pxmc_main_arr,
1239   pxml_cnt: 0
1240 };
1241
1242 void pxmc_lpc_qei_callback_index(struct lpc_qei_state_t *qst, void *context)
1243 {
1244   pxmc_state_t *mcs = (pxmc_state_t *)context;
1245   short ofs;
1246   short irc;
1247   irc = qst->index_pos;
1248
1249   if ((mcs->pxms_cfg & PXMS_CFG_I2PT_m) && (mcs->pxms_flg & PXMS_PTI_m))
1250   {
1251     short diff;
1252     ofs = irc - mcs->pxms_ptmark;
1253     diff = ofs - mcs->pxms_ptofs;
1254
1255     if (diff >= mcs->pxms_ptirc / 2)
1256       diff -= mcs->pxms_ptirc;
1257
1258     if (diff <= -mcs->pxms_ptirc / 2)
1259       diff += mcs->pxms_ptirc;
1260
1261     if (diff < 0)
1262       diff = -diff;
1263
1264     if (diff >= mcs->pxms_ptirc / 6)
1265     {
1266       pxmc_set_errno(mcs, PXMS_E_I2PT_TOOBIG);
1267     }
1268     else
1269     {
1270       mcs->pxms_ptofs = ofs;
1271       pxmc_set_flag(mcs, PXMS_PHA_b);
1272     }
1273   } /*else {
1274
1275     ofs=irc-mcs->pxms_ptofs;
1276     if((unsigned short)ofs>=(unsigned short)mcs->pxms_ptirc) {
1277       if(ofs>0) {
1278         ofs-=mcs->pxms_ptirc;
1279       } else {
1280         ofs+=mcs->pxms_ptirc;
1281       }
1282     }
1283     mcs->pxms_ptmark=ofs;
1284   }*/
1285
1286   /*lpc_qei_set_cmpos(qst, 0, qst->index_pos-4000);*/
1287 }
1288
1289 int pxmc_lpc_pthalalign_callback(pxmc_state_t *mcs)
1290 {
1291   short ptofs;
1292   short ptmark;
1293   lpc_qei_state_t *qst = &lpc_qei_state;
1294   int irc = lpc_qei_get_pos(qst);
1295   int idx_rel;
1296
1297   if (!qst->index_occ)
1298     return 0;
1299
1300   idx_rel = qst->index_pos - irc;
1301   idx_rel %= mcs->pxms_ptirc;
1302
1303   if (idx_rel < 0)
1304     idx_rel += mcs->pxms_ptirc;
1305
1306   ptofs = irc - mcs->pxms_ptofs;
1307   ptmark = ptofs + idx_rel;
1308
1309   if ((unsigned short)ptmark >= mcs->pxms_ptirc)
1310   {
1311     if (ptmark > 0)
1312       ptmark -= mcs->pxms_ptirc;
1313     else
1314       ptmark += mcs->pxms_ptirc;
1315   }
1316
1317   if ((unsigned short)ptmark < mcs->pxms_ptirc)
1318   {
1319     mcs->pxms_ptmark = ptmark;
1320   }
1321
1322   return 0;
1323 }
1324
1325 int pxmc_lpc_pthalalign(pxmc_state_t *mcs, int periods)
1326 {
1327   int res;
1328   long r2acq;
1329   long spd;
1330   pxmc_call_t *fin_fnc = pxmc_lpc_pthalalign_callback;
1331   lpc_qei_state_t *qst = &lpc_qei_state;
1332
1333   mcs->pxms_cfg &= ~PXMS_CFG_I2PT_m;
1334   lpc_qei_setup_index_catch(qst);
1335
1336   r2acq = ((long)mcs->pxms_ptirc << PXMC_SUBDIV(mcs)) * periods;
1337   spd = 1 << PXMC_SUBDIV(mcs);
1338
1339   res = pxmc_pthalalign(mcs, r2acq, spd, fin_fnc);
1340
1341   return res;
1342 }
1343
1344 int pxmc_lpc_pthalalign_run(void)
1345 {
1346   return pxmc_lpc_pthalalign(pxmc_main_list.pxml_arr[0], 20);
1347 }
1348
1349 static inline void pxmc_sfi_input(void)
1350 {
1351   int var;
1352   pxmc_state_t *mcs;
1353   pxmc_for_each_mcs(var, mcs)
1354   {
1355     /* PXMS_ENI_m - check if input (IRC) update is enabled */
1356     if (mcs->pxms_flg & PXMS_ENI_m)
1357     {
1358       pxmc_call(mcs, mcs->pxms_do_inp);
1359     }
1360   }
1361 }
1362
1363 static inline void pxmc_sfi_controller_and_output(void)
1364 {
1365   int var;
1366   pxmc_state_t *mcs;
1367   pxmc_for_each_mcs(var, mcs)
1368   {
1369     /* PXMS_ENR_m - check if controller is enabled */
1370     if (mcs->pxms_flg & PXMS_ENR_m || mcs->pxms_flg & PXMS_ENO_m)
1371     {
1372
1373       /* If output only is enabled, we skip the controller */
1374       if (mcs->pxms_flg & PXMS_ENR_m)
1375       {
1376
1377         pxmc_call(mcs, mcs->pxms_do_con);
1378
1379         /* PXMS_ERR_m - if axis in error state */
1380         if (mcs->pxms_flg & PXMS_ERR_m)
1381           mcs->pxms_ene = 0;
1382       }
1383
1384       /* for bushless motors, it is necessary to call do_out
1385         even if the controller is not enabled and PWM should be provided. */
1386       pxmc_call(mcs, mcs->pxms_do_out);
1387     }
1388   }
1389 }
1390
1391 static inline void pxmc_sfi_generator(void)
1392 {
1393   int var;
1394   pxmc_state_t *mcs;
1395   pxmc_for_each_mcs(var, mcs)
1396   {
1397     /* PXMS_ENG_m - check if requested value (position) generator is enabled */
1398     if (mcs->pxms_flg & PXMS_ENG_m)
1399     {
1400       pxmc_call(mcs, mcs->pxms_do_gen);
1401     }
1402   }
1403 }
1404
1405 static inline void pxmc_sfi_dbg(void)
1406 {
1407   int var;
1408   pxmc_state_t *mcs;
1409   pxmc_for_each_mcs(var, mcs)
1410   {
1411     if (mcs->pxms_flg & PXMS_DBG_m)
1412     {
1413       pxmc_call(mcs, mcs->pxms_do_deb);
1414     }
1415   }
1416 }
1417
1418 int pxmc_rocon_pthalalign_callback(pxmc_state_t *mcs)
1419 {
1420   short ptofs;
1421   short ptmark;
1422   int inp_chan = mcs->pxms_inp_info;
1423   int idx_rel;
1424   long irc = fpga_irc[inp_chan]->count;
1425
1426   if (!pxmc_inp_rocon_is_index_edge(mcs))
1427     return 0;
1428
1429   idx_rel = fpga_irc[inp_chan]->count_index - irc;
1430   idx_rel %= mcs->pxms_ptirc;
1431   if(idx_rel < 0)
1432     idx_rel += mcs->pxms_ptirc;
1433
1434   ptofs = irc-mcs->pxms_ptofs;
1435   ptmark = ptofs+idx_rel;
1436
1437   if((unsigned short)ptmark >= mcs->pxms_ptirc) {
1438     if(ptmark>0)
1439       ptmark -= mcs->pxms_ptirc;
1440     else
1441       ptmark += mcs->pxms_ptirc;
1442   }
1443
1444   if((unsigned short)ptmark < mcs->pxms_ptirc) {
1445     mcs->pxms_ptmark = ptmark;
1446   }
1447   return 0;
1448 }
1449
1450 int pxmc_rocon_pthalalign(pxmc_state_t *mcs, int periods)
1451 {
1452   int res;
1453   long r2acq;
1454   long spd;
1455   pxmc_call_t *fin_fnc = pxmc_rocon_pthalalign_callback;
1456
1457   mcs->pxms_cfg&=~PXMS_CFG_I2PT_m;
1458
1459   r2acq=((long)mcs->pxms_ptirc<<PXMC_SUBDIV(mcs))*periods;
1460   spd=1<<PXMC_SUBDIV(mcs);
1461
1462   /* clear bit indicating previous index */
1463   pxmc_inp_rocon_is_index_edge(mcs);
1464
1465   res=pxmc_pthalalign(mcs, r2acq, spd, fin_fnc);
1466
1467   return res;
1468 }
1469
1470 int pxmc_axis_rdmode(pxmc_state_t *mcs)
1471 {
1472   if (mcs->pxms_do_out == pxmc_rocon_pwm2ph_out)
1473     return PXMC_AXIS_MODE_STEPPER_WITH_IRC;
1474   if (mcs->pxms_do_out == pxmc_rocon_pwm_dc_out)
1475     return PXMC_AXIS_MODE_DC;
1476   if (mcs->pxms_do_out == pxmc_rocon_pwm3ph_out)
1477     return PXMC_AXIS_MODE_BLDC;
1478   return -1;
1479 }
1480
1481
1482 int
1483 pxmc_axis_pt4mode(pxmc_state_t *mcs, int mode)
1484 {
1485   static const typeof(*mcs->pxms_ptptr1) dummy0 = 0;
1486   int res = 0;
1487
1488   if (mode == PXMC_AXIS_MODE_NOCHANGE)
1489     mode = pxmc_axis_rdmode(mcs);
1490   if (mode < 0)
1491     return -1;
1492
1493   switch (mode) {
1494     /*case PXMC_AXIS_MODE_STEPPER:*/
1495     case PXMC_AXIS_MODE_STEPPER_WITH_IRC:
1496       res = pxmc_ptable_set_profile(mcs, &pxmc_ptprofile_sin, 0, 0);
1497       break;
1498     /*case PXMC_AXIS_MODE_STEPPER_WITH_PWM:*/
1499     case PXMC_AXIS_MODE_DC:
1500       /*profive some sane dummy values*/
1501       mcs->pxms_ptptr1 = (typeof(mcs->pxms_ptptr1))&dummy0;
1502       mcs->pxms_ptptr2 = (typeof(mcs->pxms_ptptr1))&dummy0;
1503       mcs->pxms_ptptr3 = (typeof(mcs->pxms_ptptr1))&dummy0;
1504
1505       mcs->pxms_ptscale_mult=1;
1506       mcs->pxms_ptscale_shift=15;
1507       break;
1508     case PXMC_AXIS_MODE_BLDC:
1509       /* res = pxmc_init_ptable(mcs, PXMC_PTPROF_SIN3FUP); */
1510      #ifndef PXMC_WITH_PT_ZIC
1511       res = pxmc_ptable_set_profile(mcs, &pxmc_ptprofile_sin3phup, 0, 0);
1512      #else /*PXMC_WITH_PT_ZIC*/
1513       res = pxmc_ptable_set_profile(mcs, &pxmc_ptprofile_sin3phup_zic, 0, 0);
1514      #endif /*PXMC_WITH_PT_ZIC*/
1515       break;
1516     default:
1517       return -1;
1518   }
1519
1520   mcs->pxms_ptvang = pxmc_ptvang_deg2irc(mcs, 90);
1521
1522   return res;
1523 }
1524
1525 /**
1526  * pxmc_axis_mode - Sets axis mode.[extern API]
1527  * @mcs:        Motion controller state information
1528  * @mode:       0 .. previous mode, 1 .. stepper motor mode,
1529  *              2 .. stepper motor with IRC feedback and PWM ,
1530  *              3 .. stepper motor with PWM control
1531  *              4 .. DC motor with IRC feedback and PWM
1532  *
1533  */
1534 int
1535 pxmc_axis_mode(pxmc_state_t *mcs, int mode)
1536 {
1537   int res;
1538
1539   pxmc_set_const_out(mcs, 0);
1540   pxmc_clear_flag(mcs, PXMS_ENI_b);
1541   pxmc_clear_flags(mcs,PXMS_ENO_m);
1542   /* Clear possible stall index flags from hardware */
1543   pxmc_inp_rocon_is_index_edge(mcs);
1544   pxmc_clear_flag(mcs, PXMS_PHA_b);
1545   pxmc_clear_flag(mcs, PXMS_PTI_b);
1546
1547
1548   if (mode == PXMC_AXIS_MODE_NOCHANGE)
1549     mode = pxmc_axis_rdmode(mcs);
1550   if (mode < 0)
1551     return -1;
1552   if (!mode)
1553     mode = PXMC_AXIS_MODE_DC;
1554
1555   res = pxmc_axis_pt4mode(mcs, mode);
1556   if (res < 0)
1557     return -1;
1558
1559   switch (mode) {
1560     /*case PXMC_AXIS_MODE_STEPPER:*/
1561     case PXMC_AXIS_MODE_STEPPER_WITH_IRC:
1562       mcs->pxms_do_out = pxmc_rocon_pwm2ph_out;
1563       break;
1564     /*case PXMC_AXIS_MODE_STEPPER_WITH_PWM:*/
1565     case PXMC_AXIS_MODE_DC:
1566       mcs->pxms_do_out = pxmc_rocon_pwm_dc_out;
1567       break;
1568     case PXMC_AXIS_MODE_BLDC:
1569       mcs->pxms_do_out = pxmc_rocon_pwm3ph_out;
1570       break;
1571     default:
1572       return -1;
1573   }
1574
1575   /* Clear possible stall index flags from hardware */
1576   pxmc_inp_rocon_is_index_edge(mcs);
1577   /* Request new phases alignment for changed parameters */
1578   pxmc_clear_flag(mcs, PXMS_PHA_b);
1579   pxmc_clear_flag(mcs, PXMS_PTI_b);
1580   pxmc_set_flag(mcs, PXMS_ENI_b);
1581   return res;
1582 }
1583
1584 void pxmc_sfi_isr(void)
1585 {
1586   unsigned long spent = pxmc_fast_tick_time();
1587
1588   pxmc_sfi_input();
1589   pxmc_sfi_controller_and_output();
1590   pxmc_sfi_generator();
1591   pxmc_sfi_dbg();
1592   /* Kick LX Master watchdog */
1593   if (pxmc_main_list.pxml_cnt != 0)
1594     *fpga_lx_master_transmitter_wdog = 1;
1595
1596   spent = pxmc_fast_tick_time() - spent;
1597
1598   if(spent > pxmc_sfi_spent_time_max)
1599     pxmc_sfi_spent_time_max = spent;
1600
1601 }
1602
1603 int pxmc_clear_power_stop(void)
1604 {
1605   return 0;
1606 }
1607
1608 int pxmc_process_state_check(unsigned long *pbusy_bits,
1609                              unsigned long *perror_bits)
1610 {
1611   unsigned short flg;
1612   unsigned short chan;
1613   unsigned long busy_bits = 0;
1614   unsigned long error_bits = 0;
1615   pxmc_state_t *mcs;
1616   flg=0;
1617   pxmc_for_each_mcs(chan, mcs) {
1618     if(mcs) {
1619       flg |= mcs->pxms_flg;
1620       if (mcs->pxms_flg & PXMS_BSY_m)
1621         busy_bits |= 1 << chan;
1622       if (mcs->pxms_flg & PXMS_ERR_m)
1623         error_bits |= 1 << chan;
1624     }
1625   }
1626   if (appl_errstop_mode) {
1627     if((flg & PXMS_ENG_m) && (flg & PXMS_ERR_m)) {
1628       pxmc_for_each_mcs(chan, mcs) {
1629         if(mcs&&(mcs->pxms_flg & PXMS_ENG_m)) {
1630           pxmc_stop(mcs, 0);
1631         }
1632       }
1633     }
1634   }
1635
1636   if (pbusy_bits != NULL)
1637     *pbusy_bits = busy_bits;
1638   if (error_bits != NULL)
1639     *perror_bits = error_bits;
1640
1641   return flg;
1642 }
1643
1644 int pxmc_done(void)
1645 {
1646   int var;
1647   pxmc_state_t *mcs;
1648
1649   if (!pxmc_main_list.pxml_cnt)
1650     return 0;
1651
1652   pxmc_for_each_mcs(var, mcs)
1653   {
1654     pxmc_set_const_out(mcs,0);
1655   }
1656
1657   pxmc_main_list.pxml_cnt = 0;
1658   __memory_barrier();
1659
1660   return 0;
1661 }
1662
1663 int pxmc_initialize(void)
1664 {
1665   int res;
1666   int i;
1667
1668   pxmc_state_t *mcs = &mcs0;
1669   lpc_qei_state_t *qst = &lpc_qei_state;
1670
1671   *fpga_irc_reset = 1;
1672
1673   for (i = 0; i < 8; i++) {
1674     fpga_irc[i]->count = 0;
1675     fpga_irc[i]->count_index = 0;
1676     *fpga_irc_state[i] = FPGA_IRC_STATE_INDEX_EVENT_MASK;
1677   }
1678
1679   /* Initialize QEI module for IRC counting */
1680   pxmc_inp_lpc_qei_init(mcs->pxms_inp_info);
1681
1682   /* Initialize QEI events processing */
1683   if (lpc_qei_setup_irq(qst) < 0)
1684     return -1;
1685
1686   *fpga_irc_reset = 0;
1687
1688   //res = pxmc_axis_pt4mode(mcs, PXMC_AXIS_MODE_BLDC);
1689
1690   /*pxmc_ctm4pwm3f_wr(mcs, 0, 0, 0);*/
1691   //pxmc_rocon_pwm3ph_wr(mcs, 0, 0, 0);
1692
1693   pxmc_rocon_pwm_master_init();
1694
1695   pxmc_main_list.pxml_cnt = 0;
1696   pxmc_dbg_hist = NULL;
1697   __memory_barrier();
1698   pxmc_main_list.pxml_cnt = PXML_MAIN_CNT;
1699
1700   return 0;
1701 }