sw/app/rocon/appl_pxmc.c

   1 /*******************************************************************
   2   Motion and Robotic System (MARS) aplication components.
   3
   4   appl_pxmc.c - position controller RoCoN hardware support
   5
   6   Copyright (C) 2001-2013 by Pavel Pisa - originator
   7                           pisa@cmp.felk.cvut.cz
   8             (C) 2001-2013 by PiKRON Ltd. - originator
   9                     http://www.pikron.com
  10
  11   This file can be used and copied according to next
  12   license alternatives
  13    - GPL - GNU Public License
  14    - other license provided by project originators
  15
  16  *******************************************************************/
  17
  18 #include <cpu_def.h>
  19 #include <system_def.h>
  20 #include <pxmc.h>
  21 #include <pxmc_lpc_qei.h>
  22 #include <pxmc_internal.h>
  23 #include <pxmc_inp_common.h>
  24 #include <pxmc_gen_info.h>
  25 #include <hal_gpio.h>
  26 #include <hal_machperiph.h>
  27 #include <stdlib.h>
  28 #include <string.h>
  29 #include <LPC17xx.h>
  30 #include <lpcTIM.h>
  31
  32 #include "appl_defs.h"
  33 #include "appl_fpga.h"
  34 #include "appl_pxmc.h"
  35 #include "pxmcc_types.h"
  36 #include "pxmcc_interface.h"
  37
  38 #define PXMC_AXIS_MODE_BLDC_PXMCC             (PXMC_AXIS_MODE_BLDC + 1)
  39 #define PXMC_AXIS_MODE_STEPPER_WITH_IRC_PXMCC (PXMC_AXIS_MODE_BLDC + 2)
  40 #define PXMC_AXIS_MODE_STEPPER_PXMCC          (PXMC_AXIS_MODE_BLDC + 3)
  41
  42 int pxmc_ptofs_from_index(pxmc_state_t *mcs, unsigned long irc,
  43                            unsigned long index_irc, int diff2err);
  44
  45 #ifndef pxmc_fast_tick_time
  46 #define pxmc_fast_tick_time() (LPC_TIM0->TC)
  47 #endif
  48
  49 #define PXML_MAIN_CNT 8
  50
  51 #define PXMC_WITH_PT_ZIC 1
  52 #define PXMC_PT_ZIC_MASK 0x8000
  53
  54 #define LPCPWM1_FREQ 20000
  55
  56 #define HAL_ERR_SENSITIVITY 20
  57 #define HAL_ERR_MAX_COUNT    5
  58
  59 #define LXPWR_WITH_SIROLADC  1
  60
  61 #define LX_MASTER_DATA_OFFS  8
  62
  63 #define PXMC_LXPWR_PWM_CYCLE 2500
  64
  65 unsigned pxmc_rocon_pwm_magnitude = PXMC_LXPWR_PWM_CYCLE;
  66
  67 long pxmc_rocon_irc_offset[PXML_MAIN_CNT];
  68 unsigned pxmc_rocon_mark_filt[PXML_MAIN_CNT];
  69
  70 static inline
  71 pxmc_rocon_state_t *pxmc_state2rocon_state(pxmc_state_t *mcs)
  72 {
  73   pxmc_rocon_state_t *mcsrc;
  74  #ifdef UL_CONTAINEROF
  75   mcsrc = UL_CONTAINEROF(mcs, pxmc_rocon_state_t, base);
  76  #else /*UL_CONTAINEROF*/
  77   mcsrc = (pxmc_rocon_state_t*)((char*)mcs - __builtin_offsetof(pxmc_rocon_state_t, base));
  78  #endif /*UL_CONTAINEROF*/
  79   return mcsrc;
  80 }
  81
  82 const uint8_t onesin10bits[1024]={
  83   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,
  84   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,
  85   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,
  86   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,
  87   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,
  88   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,
  89   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,
  90   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,
  91   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,
  92   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,
  93   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,
  94   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,
  95   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,
  96   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,
  97   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,
  98   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,
  99   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,
 100   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,
 101   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,
 102   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,
 103   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,
 104   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,
 105   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,
 106   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,
 107   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,
 108   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,
 109   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,
 110   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,
 111   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,
 112   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,
 113   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,
 114   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
 115 };
 116
 117 int
 118 pxmc_inp_rocon_inp(struct pxmc_state *mcs)
 119 {
 120   int chan=mcs->pxms_inp_info;
 121   long irc;
 122   long pos;
 123
 124   /* read position from hardware */
 125   irc = fpga_irc[chan]->count;
 126   irc += pxmc_rocon_irc_offset[chan];
 127   pos = irc << PXMC_SUBDIV(mcs);
 128   mcs->pxms_as = pos - mcs->pxms_ap;
 129   mcs->pxms_ap = pos;
 130
 131   /* Running of the motor commutator */
 132   if (mcs->pxms_flg & PXMS_PTI_m)
 133     pxmc_irc_16bit_commindx(mcs, irc);
 134
 135   return 0;
 136 }
 137
 138 int
 139 pxmc_inp_rocon_is_mark(pxmc_state_t *mcs)
 140 {
 141   uint32_t irc_state;
 142   int mark;
 143   unsigned filt;
 144   int chan=mcs->pxms_inp_info;
 145
 146   irc_state = *fpga_irc_state[chan];
 147
 148   mark = ((irc_state >> (ffs(FPGA_IRC_STATE_MARK_MASK) - 1)) ^
 149          (mcs->pxms_cfg >> PXMS_CFG_HPS_b)) & 1;
 150
 151   filt = pxmc_rocon_mark_filt[chan];
 152   filt = (filt << 1) | mark;
 153   pxmc_rocon_mark_filt[chan] = filt;
 154
 155   return onesin10bits[filt & 0x03ff];
 156 }
 157
 158 int
 159 pxmc_inp_rocon_is_index_edge(pxmc_state_t *mcs)
 160 {
 161   uint32_t irc_state;
 162   int index;
 163   int chan=mcs->pxms_inp_info;
 164
 165   irc_state = *fpga_irc_state[chan];
 166   *fpga_irc_state[chan] = FPGA_IRC_STATE_INDEX_EVENT_MASK;
 167
 168   index = (irc_state >> (ffs(FPGA_IRC_STATE_INDEX_EVENT_MASK) - 1)) & 1;
 169
 170   return index;
 171 }
 172
 173 int
 174 pxmc_inp_rocon_adjust_to_irc_change(struct pxmc_state *mcs, long irc_diff)
 175 {
 176   long pos_diff = irc_diff << PXMC_SUBDIV(mcs);
 177
 178   mcs->pxms_ptofs += irc_diff;
 179
 180   mcs->pxms_ap += pos_diff;
 181   mcs->pxms_rp += pos_diff;
 182   return 0;
 183 }
 184
 185 int
 186 pxmc_inp_rocon_irc_offset_from_index(struct pxmc_state *mcs)
 187 {
 188   int chan=mcs->pxms_inp_info;
 189   long irc_offset;
 190   long irc_diff;
 191
 192   irc_offset = -fpga_irc[chan]->count_index;
 193   irc_diff = irc_offset - pxmc_rocon_irc_offset[chan];
 194   pxmc_rocon_irc_offset[chan] = irc_offset;
 195   return pxmc_inp_rocon_adjust_to_irc_change(mcs, irc_diff);
 196 }
 197
 198 int
 199 pxmc_inp_rocon_ap_zero(struct pxmc_state *mcs)
 200 {
 201   int chan=mcs->pxms_inp_info;
 202   long irc_offset;
 203   long irc_diff;
 204
 205   irc_offset = -fpga_irc[chan]->count;
 206   irc_diff = irc_offset - pxmc_rocon_irc_offset[chan];
 207   pxmc_rocon_irc_offset[chan] = irc_offset;
 208   return pxmc_inp_rocon_adjust_to_irc_change(mcs, irc_diff);
 209 }
 210
 211 int
 212 pxmc_inp_rocon_ap2hw(struct pxmc_state *mcs)
 213 {
 214   int chan=mcs->pxms_inp_info;
 215   long irc;
 216   long pos_diff;
 217
 218   irc = fpga_irc[chan]->count;
 219   pos_diff = mcs->pxms_ap - (irc << PXMC_SUBDIV(mcs));
 220
 221   irc = pos_diff >> PXMC_SUBDIV(mcs);
 222
 223   /* Adjust phase table alignemt to modified IRC readout  */
 224   mcs->pxms_ptofs += irc - pxmc_rocon_irc_offset[chan];
 225
 226   pxmc_rocon_irc_offset[chan] = irc;
 227   return 0;
 228 }
 229
 230 int
 231 pxmc_inp_rocon_ptofs_from_index_poll(struct pxmc_state *mcs, int diff2err)
 232 {
 233   int chan=mcs->pxms_inp_info;
 234   long irc;
 235   long index_irc;
 236
 237   if (!(*fpga_irc_state[chan] & FPGA_IRC_STATE_INDEX_EVENT_MASK))
 238     return 0;
 239
 240   irc = fpga_irc[chan]->count + pxmc_rocon_irc_offset[chan];
 241   index_irc = fpga_irc[chan]->count_index + pxmc_rocon_irc_offset[chan];
 242
 243   return pxmc_ptofs_from_index(mcs, irc, index_irc, diff2err);
 244 }
 245
 246 uint32_t pxmc_rocon_receiver_dummy_reg;
 247
 248 static inline volatile uint32_t *
 249 pxmc_rocon_receiver_chan2reg(unsigned chan)
 250 {
 251   volatile uint32_t *rec_reg;
 252
 253   if (chan >= 16)
 254     return &pxmc_rocon_receiver_dummy_reg;
 255
 256   rec_reg = fpga_lx_master_receiver_base;
 257
 258  #ifdef LXPWR_WITH_SIROLADC
 259   rec_reg += LX_MASTER_DATA_OFFS + 1 + (chan >> 3) * 3 + chan * 2;
 260  #else /*LXPWR_WITH_SIROLADC*/
 261   rec_reg += LX_MASTER_DATA_OFFS + chan;
 262  #endif /*LXPWR_WITH_SIROLADC*/
 263
 264   return rec_reg;
 265 }
 266
 267 inline unsigned
 268 pxmc_rocon_bldc_hal_rd(pxmc_state_t *mcs)
 269 {
 270   unsigned h = 0;
 271   volatile uint32_t *rec_reg_a, *rec_reg_b, *rec_reg_c;
 272   int chan = mcs->pxms_out_info;
 273   int hal_offs;
 274
 275  #ifdef LXPWR_WITH_SIROLADC
 276   hal_offs = 1;
 277  #else /*LXPWR_WITH_SIROLADC*/
 278   hal_offs = 0;
 279  #endif /*LXPWR_WITH_SIROLADC*/
 280
 281   rec_reg_a = pxmc_rocon_receiver_chan2reg(chan + 0);
 282   rec_reg_b = pxmc_rocon_receiver_chan2reg(chan + 1);
 283   rec_reg_c = pxmc_rocon_receiver_chan2reg(chan + 2);
 284
 285   h  = (rec_reg_a[hal_offs] >> 14) & 1;
 286   h |= (rec_reg_b[hal_offs] >> 13) & 2;
 287   h |= (rec_reg_c[hal_offs] >> 12) & 4;
 288
 289   /* return 3 bits corresponding to the HAL senzor input */
 290   return h;
 291 }
 292
 293 #if 1
 294 const unsigned char pxmc_lpc_bdc_hal_pos_table[8] =
 295 {
 296   [0] = 0xff,
 297   [7] = 0xff,
 298   [1] = 0, /*0*/
 299   [5] = 1, /*1*/
 300   [4] = 2, /*2*/
 301   [6] = 3, /*3*/
 302   [2] = 4, /*4*/
 303   [3] = 5, /*5*/
 304 };
 305 #else
 306 const unsigned char pxmc_lpc_bdc_hal_pos_table[8] =
 307 {
 308   [0] = 0xff,
 309   [7] = 0xff,
 310   [1] = 0, /*0*/
 311   [5] = 5, /*1*/
 312   [4] = 4, /*2*/
 313   [6] = 3, /*3*/
 314   [2] = 2, /*4*/
 315   [3] = 1, /*5*/
 316 };
 317 #endif
 318
 319 uint32_t pxmc_rocon_pwm_dummy_reg;
 320
 321 static inline volatile uint32_t *
 322 pxmc_rocon_pwm_chan2reg(unsigned chan)
 323 {
 324   volatile uint32_t *pwm_reg;
 325
 326   if (chan >= 16)
 327     return &pxmc_rocon_pwm_dummy_reg;
 328
 329   pwm_reg = fpga_lx_master_transmitter_base;
 330
 331  #ifdef LXPWR_WITH_SIROLADC
 332   pwm_reg += LX_MASTER_DATA_OFFS + 1 + (chan >> 3) + chan;
 333  #else /*LXPWR_WITH_SIROLADC*/
 334   pwm_reg += LX_MASTER_DATA_OFFS + chan;
 335  #endif /*LXPWR_WITH_SIROLADC*/
 336
 337   return pwm_reg;
 338 }
 339
 340 /**
 341  * pxmc_rocon_pwm3ph_wr - Output of the 3-phase PWM to the hardware
 342  * @mcs:  Motion controller state information
 343  */
 344 /*static*/ inline void
 345 pxmc_rocon_pwm3ph_wr(pxmc_state_t *mcs, short pwm1, short pwm2, short pwm3)
 346 {
 347   volatile uint32_t *pwm_reg_a, *pwm_reg_b, *pwm_reg_c;
 348   int chan = mcs->pxms_out_info;
 349
 350   pwm_reg_c = pxmc_rocon_pwm_chan2reg(chan + 0);
 351   pwm_reg_b = pxmc_rocon_pwm_chan2reg(chan + 1);
 352   pwm_reg_a = pxmc_rocon_pwm_chan2reg(chan + 2);
 353
 354   *pwm_reg_a = pwm1;
 355   *pwm_reg_b = pwm2;
 356   *pwm_reg_c = pwm3;
 357 }
 358
 359 static inline void
 360 pxmc_rocon_process_hal_error(struct pxmc_state *mcs)
 361 {
 362   if (mcs->pxms_halerc >= HAL_ERR_SENSITIVITY * HAL_ERR_MAX_COUNT)
 363   {
 364     pxmc_set_errno(mcs, PXMS_E_HAL);
 365     mcs->pxms_ene = 0;
 366     mcs->pxms_halerc--;
 367   }
 368   else
 369     mcs->pxms_halerc += HAL_ERR_SENSITIVITY;
 370 }
 371
 372 /**
 373  * pxmc_rocon_pwm3ph_out - Phase output for brush-less 3-phase motor
 374  * @mcs:  Motion controller state information
 375  */
 376 int
 377 pxmc_rocon_pwm3ph_out(pxmc_state_t *mcs)
 378 {
 379   typeof(mcs->pxms_ptvang) ptvang;
 380   int sync_mode = 0;
 381   unsigned char hal_pos;
 382   short pwm1;
 383   short pwm2;
 384   short pwm3;
 385   int indx = 0;
 386   short ene;
 387   int wind_current[4];
 388
 389   if (!(mcs->pxms_flg & PXMS_PTI_m) || !(mcs->pxms_flg & PXMS_PHA_m) ||
 390       (mcs->pxms_flg & PXMS_PRA_m))
 391   {
 392     short ptindx;
 393     short ptirc = mcs->pxms_ptirc;
 394     short divisor = mcs->pxms_ptper * 6;
 395
 396    #if 0
 397     pxmc_irc_16bit_commindx(mcs, mcs->pxms_rp >> PXMC_SUBDIV(mcs));
 398     sync_mode = 1;
 399    #elif 0
 400     {
 401       int res;
 402       res = pxmc_inp_rocon_ptofs_from_index_poll(mcs, 0);
 403       if (res < 0) {
 404         pxmc_set_errno(mcs, PXMS_E_I2PT_TOOBIG);
 405       } else if (res) {
 406         pxmc_set_flag(mcs, PXMS_PTI_b);
 407         pxmc_set_flag(mcs, PXMS_PHA_b);
 408       }
 409     }
 410    #else
 411
 412     hal_pos = pxmc_lpc_bdc_hal_pos_table[pxmc_rocon_bldc_hal_rd(mcs)];
 413
 414     if (hal_pos == 0xff)
 415     {
 416       if (mcs->pxms_ene)
 417         pxmc_rocon_process_hal_error(mcs);
 418     }
 419     else
 420     {
 421       if (mcs->pxms_halerc)
 422         mcs->pxms_halerc--;
 423
 424       ptindx = (hal_pos * ptirc + divisor / 2) / divisor;
 425
 426       if (!(mcs->pxms_flg & PXMS_PTI_m) || (mcs->pxms_flg & PXMS_PRA_m))
 427       {
 428         if (((hal_pos != mcs->pxms_hal) && (mcs->pxms_hal != 0x40)) && 1)
 429         {
 430           short ptindx_prev = (mcs->pxms_hal * ptirc + divisor / 2) / divisor;;
 431
 432           if ((ptindx > ptindx_prev + ptirc / 2) ||
 433               (ptindx_prev > ptindx + ptirc / 2))
 434           {
 435             ptindx = (ptindx_prev + ptindx - ptirc) / 2;
 436
 437             if (ptindx < 0)
 438               ptindx += ptirc;
 439           }
 440           else
 441           {
 442             ptindx = (ptindx_prev + ptindx) / 2;
 443           }
 444
 445           mcs->pxms_ptindx = ptindx;
 446
 447           mcs->pxms_ptofs = (mcs->pxms_ap >> PXMC_SUBDIV(mcs)) + mcs->pxms_ptshift - ptindx;
 448
 449           pxmc_set_flag(mcs, PXMS_PTI_b);
 450           pxmc_clear_flag(mcs, PXMS_PRA_b);
 451         }
 452         else
 453         {
 454           if (!(mcs->pxms_flg & PXMS_PTI_m))
 455             mcs->pxms_ptindx = ptindx;
 456         }
 457       } else {
 458         /* if phase table position to mask is know do fine phase table alignment */
 459         if (mcs->pxms_cfg & PXMS_CFG_I2PT_m) {
 460           int res;
 461           res = pxmc_inp_rocon_ptofs_from_index_poll(mcs, 0);
 462           if (res < 0) {
 463             pxmc_set_errno(mcs, PXMS_E_I2PT_TOOBIG);
 464           } else if (res) {
 465             pxmc_set_flag(mcs, PXMS_PTI_b);
 466             pxmc_set_flag(mcs, PXMS_PHA_b);
 467           }
 468         }
 469       }
 470       mcs->pxms_hal = hal_pos;
 471     }
 472    #endif
 473   }
 474
 475   {
 476     /*wind_current[0]=(ADC->ADDR0 & 0xFFF0)>>4;*/
 477     /* FIXME - check winding current against limit */
 478     /* pxmc_set_errno(mcs, PXMS_E_WINDCURRENT); */
 479   }
 480
 481   if (!sync_mode) {
 482     ptvang = mcs->pxms_ptvang;
 483     ene = mcs->pxms_ene;
 484   } else {
 485     ptvang = 0;
 486     ene = mcs->pxms_flg & PXMS_BSY_m? mcs->pxms_me: 0;
 487   }
 488
 489   if (ene) {
 490     indx = mcs->pxms_ptindx;
 491 #if 1
 492     /* tuning of magnetic field/voltage advance angle */
 493     indx += (mcs->pxms_s1 * mcs->pxms_as) >> (PXMC_SUBDIV(mcs) + 8);
 494 #endif
 495
 496     if (ene < 0)
 497     {
 498       /* Generating direction of stator mag. field for backward torque */
 499       ene = -ene;
 500
 501       if ((indx -= ptvang) < 0)
 502         indx += mcs->pxms_ptirc;
 503     }
 504     else
 505     {
 506       /* Generating direction of stator mag. field for forward torque */
 507       if ((indx += ptvang) >= mcs->pxms_ptirc)
 508         indx -= mcs->pxms_ptirc;
 509     }
 510
 511     if (mcs->pxms_ptscale_mult)
 512       indx = ((unsigned long)indx * mcs->pxms_ptscale_mult) >> mcs->pxms_ptscale_shift;
 513
 514     pwm1 = mcs->pxms_ptptr1[indx];
 515     pwm2 = mcs->pxms_ptptr2[indx];
 516     pwm3 = mcs->pxms_ptptr3[indx];
 517
 518 #ifdef PXMC_WITH_PT_ZIC
 519     if (labs(mcs->pxms_as) < (10 << PXMC_SUBDIV(mcs)))
 520     {
 521       pwm1 &= ~PXMC_PT_ZIC_MASK;
 522       pwm2 &= ~PXMC_PT_ZIC_MASK;
 523       pwm3 &= ~PXMC_PT_ZIC_MASK;
 524     }
 525 #endif /*PXMC_WITH_PT_ZIC*/
 526
 527     /* Default phase-table amplitude is 0x7fff, ene max is 0x7fff */
 528     /* Initialized CTM4 PWM period is 0x200 => divide by value about 2097024 */
 529     {
 530       unsigned long pwm_dc = pxmc_rocon_pwm_magnitude * (unsigned long)ene;
 531       if (pwm1 & PXMC_PT_ZIC_MASK)
 532         pwm1 = 0x8000;
 533       else
 534         pwm1 = (((unsigned long long)pwm1 * pwm_dc) >> (15 + 15)) | 0x4000;
 535       if (pwm2 & PXMC_PT_ZIC_MASK)
 536         pwm2 = 0x8000;
 537       else
 538         pwm2 = (((unsigned long long)pwm2 * pwm_dc) >> (15 + 15)) | 0x4000;
 539       if (pwm3 & PXMC_PT_ZIC_MASK)
 540         pwm3 = 0x8000;
 541       else
 542         pwm3 = (((unsigned long long)pwm3 * pwm_dc) >> (15 + 15)) | 0x4000;
 543     }
 544     pxmc_rocon_pwm3ph_wr(mcs, pwm1, pwm2, pwm3);
 545   }
 546   else
 547   {
 548     pxmc_rocon_pwm3ph_wr(mcs, 0, 0, 0);
 549   }
 550
 551   return 0;
 552 }
 553
 554 /**
 555  * pxmc_rocon_pwm2ph_wr - Output of the 2-phase stepper motor PWM to the hardware
 556  * @mcs:        Motion controller state information
 557  */
 558 /*static*/ inline void
 559 pxmc_rocon_pwm2ph_wr(pxmc_state_t *mcs, short pwm1, short pwm2)
 560 {
 561   volatile uint32_t *pwm_reg_ap, *pwm_reg_an, *pwm_reg_bp, *pwm_reg_bn;
 562   int chan = mcs->pxms_out_info;
 563
 564   pwm_reg_bn = pxmc_rocon_pwm_chan2reg(chan + 0);
 565   pwm_reg_bp = pxmc_rocon_pwm_chan2reg(chan + 1);
 566   pwm_reg_an = pxmc_rocon_pwm_chan2reg(chan + 2);
 567   pwm_reg_ap = pxmc_rocon_pwm_chan2reg(chan + 3);
 568
 569   if (pwm2 >= 0) {
 570     *pwm_reg_bp = pwm2 | 0x4000;
 571     *pwm_reg_bn = 0;
 572   } else {
 573     *pwm_reg_bp = 0;
 574     *pwm_reg_bn = -pwm2 | 0x4000;
 575   }
 576
 577   if (pwm1 >= 0) {
 578     *pwm_reg_ap = pwm1 | 0x4000;
 579     *pwm_reg_an = 0;
 580   } else {
 581     *pwm_reg_ap = 0;
 582     *pwm_reg_an = -pwm1 | 0x4000;
 583   }
 584 }
 585
 586 /**
 587  * pxmc_rocon_pwm2ph_out - Phase output of the 2-phase stepper motor PWM
 588  * @mcs:  Motion controller state information
 589  */
 590 int
 591 pxmc_rocon_pwm2ph_out(pxmc_state_t *mcs)
 592 {
 593   typeof(mcs->pxms_ptvang) ptvang;
 594   int sync_mode = 0;
 595   short pwm1;
 596   short pwm2;
 597   int indx = 0;
 598   short ene;
 599   int wind_current[4];
 600
 601   if(!(mcs->pxms_flg&PXMS_PTI_m) || !(mcs->pxms_flg&PXMS_PHA_m) ||
 602      (mcs->pxms_flg&PXMS_PRA_m)){
 603     short ptindx;
 604     short ptirc=mcs->pxms_ptirc;
 605
 606    #if 0
 607     pxmc_irc_16bit_commindx(mcs, mcs->pxms_rp >> PXMC_SUBDIV(mcs));
 608     sync_mode = 1;
 609    #elif 1
 610     {
 611       int res;
 612       res = pxmc_inp_rocon_ptofs_from_index_poll(mcs, 0);
 613       if (res < 0) {
 614         pxmc_set_errno(mcs, PXMS_E_I2PT_TOOBIG);
 615       } else if (res) {
 616         pxmc_set_flag(mcs, PXMS_PTI_b);
 617         pxmc_set_flag(mcs, PXMS_PHA_b);
 618       }
 619     }
 620    #else
 621
 622     ptindx = mcs->pxms_ptindx;
 623
 624     if(0 && (!(mcs->pxms_flg&PXMS_PTI_m) || (mcs->pxms_flg&PXMS_PRA_m))) {
 625
 626       mcs->pxms_ptindx = ptindx;
 627
 628       mcs->pxms_ptofs = (mcs->pxms_ap >> PXMC_SUBDIV(mcs)) + mcs->pxms_ptshift - ptindx;
 629
 630       pxmc_set_flag(mcs, PXMS_PTI_b);
 631       pxmc_clear_flag(mcs, PXMS_PRA_b);
 632
 633       /* if phase table position to mask is know do fine phase table alignment */
 634       //if(mcs->pxms_cfg & PXMS_CFG_I2PT_m)
 635       //  lpc_qei_setup_index_catch(&lpc_qei_state);
 636
 637     } else {
 638       if(!(mcs->pxms_flg&PXMS_PTI_m))
 639             mcs->pxms_ptindx = ptindx;
 640     }
 641    #endif
 642   }
 643
 644   {
 645     /*wind_current[0]=(ADC->ADDR0 & 0xFFF0)>>4;*/
 646     /* FIXME - check winding current against limit */
 647     /* pxmc_set_errno(mcs, PXMS_E_WINDCURRENT); */
 648   }
 649
 650   if (!sync_mode) {
 651     ptvang = mcs->pxms_ptvang;
 652     ene = mcs->pxms_ene;
 653   } else {
 654     ptvang = 0;
 655     ene = mcs->pxms_flg & PXMS_BSY_m? mcs->pxms_me: 0;
 656   }
 657
 658   if (ene) {
 659     indx = mcs->pxms_ptindx;
 660    #if 0
 661     /* tuning of magnetic field/voltage advance angle */
 662     indx += (mcs->pxms_s1 * mcs->pxms_as) >> (PXMC_SUBDIV(mcs) + 8);
 663    #endif
 664     if (ene<0){
 665       /* Generating direction of stator mag. field for backward torque */
 666       ene = -ene;
 667       if ((indx -= ptvang)<0)
 668         indx += mcs->pxms_ptirc;
 669     }else{
 670       /* Generating direction of stator mag. field for forward torque */
 671       if ((indx += ptvang) >= mcs->pxms_ptirc)
 672         indx -= mcs->pxms_ptirc;
 673     }
 674
 675     if (mcs->pxms_ptscale_mult)
 676       indx = ((unsigned long)indx * mcs->pxms_ptscale_mult) >> mcs->pxms_ptscale_shift;
 677
 678     pwm1 = mcs->pxms_ptptr1[indx];
 679     pwm2 = mcs->pxms_ptptr2[indx];
 680
 681     /* Default phase-table amplitude is 0x7fff, ene max is 0x7fff */
 682     /* Initialized PWM period is 0x200 => divide by value about 2097024 */
 683     {
 684       unsigned long pwm_dc = pxmc_rocon_pwm_magnitude * (unsigned long)ene;
 685       pwm1 = ((unsigned long long)pwm1 * pwm_dc) >> (15+15);
 686       pwm2 = ((unsigned long long)pwm2 * pwm_dc) >> (15+15);
 687     }
 688     pxmc_rocon_pwm2ph_wr(mcs, pwm1, pwm2);
 689   }else{
 690     /*pxmc_lpc_wind_current_over_cnt = 0;*/
 691     pxmc_rocon_pwm2ph_wr(mcs, 0, 0);
 692   }
 693
 694   return 0;
 695 }
 696
 697 /**
 698  * pxmc_rocon_pwm_dc_out - DC motor CW and CCW PWM output
 699  * @mcs:  Motion controller state information
 700  */
 701 int
 702 pxmc_rocon_pwm_dc_out(pxmc_state_t *mcs)
 703 {
 704   volatile uint32_t *pwm_reg_a, *pwm_reg_b;
 705   int chan = mcs->pxms_out_info;
 706   int ene = mcs->pxms_ene;
 707
 708   pwm_reg_a = pxmc_rocon_pwm_chan2reg(chan + 0);
 709   pwm_reg_b = pxmc_rocon_pwm_chan2reg(chan + 1);
 710
 711   if (ene < 0) {
 712     ene = -ene;
 713     if (ene > 0x7fff)
 714       ene = 0x7fff;
 715     ene = (ene * (pxmc_rocon_pwm_magnitude + 5)) >> 15;
 716     *pwm_reg_a = 0;
 717     *pwm_reg_b = ene | 0x4000;
 718   } else {
 719     if (ene > 0x7fff)
 720       ene = 0x7fff;
 721     ene = (ene * (pxmc_rocon_pwm_magnitude + 5)) >> 15;
 722     *pwm_reg_b = 0;
 723     *pwm_reg_a = ene | 0x4000;
 724   }
 725
 726   return 0;
 727 }
 728
 729 /*******************************************************************/
 730 /* PXMCC - PXMC coprocessor support and communication */
 731
 732 void pxmcc_pxmc_ptofs2mcc(pxmc_state_t *mcs, int enable_update)
 733 {
 734   volatile pxmcc_axis_data_t *mcc_axis = pxmc_rocon_mcs2pxmcc(mcs);
 735   int inp_chan=mcs->pxms_inp_info;
 736   uint32_t ptofs;
 737   uint32_t irc;
 738
 739   if (mcc_axis != NULL) {
 740     if (enable_update >= 0)
 741       mcc_axis->ptirc = 0xffffffff;
 742     ptofs = mcs->pxms_ptofs - pxmc_rocon_irc_offset[inp_chan];
 743     irc = fpga_irc[inp_chan]->count;
 744     ptofs = (int16_t)(ptofs - irc) + irc;
 745     mcc_axis->ptofs = ptofs;
 746     if (enable_update > 0) {
 747       mcc_axis->ptirc = mcs->pxms_ptirc;
 748     }
 749   }
 750 }
 751
 752 static inline
 753 void pxmcc_axis_get_cur_dq_filt_raw(pxmc_state_t *mcs,
 754                                     int32_t *p_cur_d_raw, int32_t *p_cur_q_raw)
 755 {
 756   volatile pxmcc_axis_data_t *mcc_axis = pxmc_rocon_mcs2pxmcc(mcs);
 757   pxmc_rocon_state_t *mcsrc = pxmc_state2rocon_state(mcs);
 758   uint32_t cur_d_cum = mcc_axis->cur_d_cum;
 759   uint32_t cur_q_cum = mcc_axis->cur_q_cum;
 760   int32_t cur_d;
 761   int32_t cur_q;
 762
 763   cur_d = cur_d_cum - mcsrc->cur_d_cum_prev;
 764   mcsrc->cur_d_cum_prev = cur_d_cum;
 765   cur_q = cur_q_cum - mcsrc->cur_q_cum_prev;
 766   mcsrc->cur_q_cum_prev = cur_q_cum;
 767
 768   *p_cur_d_raw = cur_d;
 769   *p_cur_q_raw = cur_q;
 770 }
 771
 772 static inline
 773 void pxmcc_axis_cur_dq_raw2filt(int *p_cur, int32_t cur_raw)
 774 {
 775   int cur_div;
 776   int32_t cur;
 777   cur_div = cur_raw & 0x1f;
 778   cur = cur_raw / cur_div;
 779   cur += (1 << 11) | 0x20;
 780   *p_cur = cur >> 12;
 781 }
 782
 783
 784 void pxmcc_axis_get_cur_dq_filt(pxmc_state_t *mcs, int *p_cur_d, int *p_cur_q)
 785 {
 786   int32_t cur_d_raw;
 787   int32_t cur_q_raw;
 788
 789   pxmcc_axis_get_cur_dq_filt_raw(mcs, &cur_d_raw, &cur_q_raw);
 790
 791   pxmcc_axis_cur_dq_raw2filt(p_cur_d, cur_d_raw);
 792   pxmcc_axis_cur_dq_raw2filt(p_cur_q, cur_q_raw);
 793 }
 794
 795 static inline
 796 void pxmcc_cur_ctrl_pi(int *p_pwm, int32_t *p_cur_err_sum,
 797                int cur_err, short cur_ctrl_p, short cur_ctrl_i, int max_pwm)
 798 {
 799   int pwm;
 800   int32_t cur_err_sum = *p_cur_err_sum;
 801
 802   pwm = (cur_err * cur_ctrl_p) >> 8;
 803
 804   if (cur_ctrl_i)
 805     cur_err_sum += cur_err * cur_ctrl_i;
 806   else
 807     cur_err_sum = 0;
 808
 809   pwm += cur_err_sum >> 16;
 810
 811   if (pwm > max_pwm) {
 812     cur_err_sum -= (pwm - max_pwm) << 16;
 813     pwm = max_pwm;
 814   } else if (-pwm > max_pwm) {
 815     cur_err_sum -= (pwm + max_pwm) << 16;
 816     pwm = 0 - max_pwm;
 817   }
 818   *p_cur_err_sum = cur_err_sum;
 819   *p_pwm = pwm;
 820 }
 821
 822 int
 823 pxmc_pxmcc_pwm3ph_out(pxmc_state_t *mcs)
 824 {
 825   if (!(mcs->pxms_flg & PXMS_PTI_m) || !(mcs->pxms_flg & PXMS_PHA_m) ||
 826       (mcs->pxms_flg & PXMS_PRA_m)) {
 827     short ptindx;
 828     short ptirc = mcs->pxms_ptirc;
 829     short divisor = mcs->pxms_ptper * 6;
 830     unsigned char hal_pos;
 831
 832     hal_pos = pxmc_lpc_bdc_hal_pos_table[pxmc_rocon_bldc_hal_rd(mcs)];
 833
 834     if (hal_pos == 0xff) {
 835       if (mcs->pxms_ene)
 836         pxmc_rocon_process_hal_error(mcs);
 837     } else {
 838       if (mcs->pxms_halerc)
 839         mcs->pxms_halerc--;
 840
 841       ptindx = (hal_pos * ptirc + divisor / 2) / divisor;
 842
 843       if (!(mcs->pxms_flg & PXMS_PTI_m) || (mcs->pxms_flg & PXMS_PRA_m)) {
 844         int set_ptofs_fl = 0;
 845         int ptofs_enable_update = 0;
 846
 847         if (((hal_pos != mcs->pxms_hal) && (mcs->pxms_hal != 0x40)) && 1) {
 848           short ptindx_prev = (mcs->pxms_hal * ptirc + divisor / 2) / divisor;;
 849
 850           if ((ptindx > ptindx_prev + ptirc / 2) ||
 851               (ptindx_prev > ptindx + ptirc / 2)) {
 852             ptindx = (ptindx_prev + ptindx - ptirc) / 2;
 853
 854             if (ptindx < 0)
 855               ptindx += ptirc;
 856           } else {
 857             ptindx = (ptindx_prev + ptindx) / 2;
 858           }
 859
 860           set_ptofs_fl = 1;
 861           ptofs_enable_update = 1;
 862
 863           pxmc_set_flag(mcs, PXMS_PTI_b);
 864           pxmc_clear_flag(mcs, PXMS_PRA_b);
 865         } else {
 866           if (!(mcs->pxms_flg & PXMS_PTI_m))
 867             set_ptofs_fl = 1;
 868         }
 869         if (set_ptofs_fl) {
 870           mcs->pxms_ptindx = ptindx;
 871           mcs->pxms_ptofs = (mcs->pxms_ap >> PXMC_SUBDIV(mcs)) + mcs->pxms_ptshift - ptindx;
 872
 873           pxmcc_pxmc_ptofs2mcc(mcs, ptofs_enable_update);
 874         }
 875       } else {
 876         /* if phase table position to mask is know do fine phase table alignment */
 877         if (mcs->pxms_cfg & PXMS_CFG_I2PT_m) {
 878           int res;
 879
 880           res = pxmc_inp_rocon_ptofs_from_index_poll(mcs, 0);
 881           if (res < 0) {
 882             pxmc_set_errno(mcs, PXMS_E_I2PT_TOOBIG);
 883           } else if (res) {
 884             pxmcc_pxmc_ptofs2mcc(mcs, 1);
 885             pxmc_set_flag(mcs, PXMS_PTI_b);
 886             pxmc_set_flag(mcs, PXMS_PHA_b);
 887           }
 888         }
 889       }
 890       mcs->pxms_hal = hal_pos;
 891     }
 892   }
 893
 894   {
 895     /*wind_current[0]=(ADC->ADDR0 & 0xFFF0)>>4;*/
 896     /* FIXME - check winding current against limit */
 897     /* pxmc_set_errno(mcs, PXMS_E_WINDCURRENT); */
 898   }
 899
 900   {
 901     int ene, pwm_d, pwm_q;
 902
 903     ene = mcs->pxms_ene;
 904     pwm_d = 0;
 905     pwm_q = (pxmc_rocon_pwm_magnitude * ene) >> 15;
 906
 907     pxmcc_axis_pwm_dq_out(mcs, pwm_d, pwm_q);
 908
 909     if (mcs->pxms_flg & PXMS_ERR_m)
 910       pxmc_rocon_pwm3ph_wr(mcs, 0, 0, 0);
 911   }
 912
 913   return 0;
 914 }
 915
 916 int
 917 pxmc_pxmcc_pwm2ph_out(pxmc_state_t *mcs)
 918 {
 919   if(!(mcs->pxms_flg&PXMS_PTI_m) || !(mcs->pxms_flg&PXMS_PHA_m) ||
 920      (mcs->pxms_flg&PXMS_PRA_m)) {
 921
 922     {
 923       /* Wait for index mark to align phases */
 924       int res;
 925       res = pxmc_inp_rocon_ptofs_from_index_poll(mcs, 0);
 926       if (res < 0) {
 927         pxmc_set_errno(mcs, PXMS_E_I2PT_TOOBIG);
 928       } else if (res) {
 929         pxmcc_pxmc_ptofs2mcc(mcs, 1);
 930         pxmc_set_flag(mcs, PXMS_PTI_b);
 931         pxmc_set_flag(mcs, PXMS_PHA_b);
 932       } else {
 933         pxmcc_pxmc_ptofs2mcc(mcs, 0);
 934       }
 935     }
 936   }
 937
 938   {
 939     int ene, pwm_d, pwm_q;
 940
 941     ene = mcs->pxms_ene;
 942     pwm_d = 0;
 943     pwm_q = (pxmc_rocon_pwm_magnitude * ene) >> 15;
 944
 945     pxmcc_axis_pwm_dq_out(mcs, pwm_d, pwm_q);
 946
 947     if (mcs->pxms_flg & PXMS_ERR_m)
 948       pxmc_rocon_pwm2ph_wr(mcs, 0, 0);
 949   }
 950
 951   return 0;
 952 }
 953
 954 /**
 955  * pxmc_pxmcc_nofb_inp - Dummy input for direct stepper motor control
 956  * @mcs:        Motion controller state information
 957  */
 958 int
 959 pxmc_pxmcc_nofb_inp(pxmc_state_t *mcs)
 960 {
 961   return 0;
 962 }
 963
 964 /**
 965  * pxmc_pxmcc_nofb_con - Empty controller for direct stepper motor control
 966  * @mcs:        Motion controller state information
 967  */
 968 int
 969 pxmc_pxmcc_nofb_con(pxmc_state_t *mcs)
 970 {
 971   mcs->pxms_ene=mcs->pxms_me;
 972   return 0;
 973 }
 974
 975 int
 976 pxmc_pxmcc_nofb2ph_out(pxmc_state_t *mcs)
 977 {
 978   pxmc_rocon_state_t *mcsrc = pxmc_state2rocon_state(mcs);
 979   volatile pxmcc_axis_data_t *mcc_axis = pxmc_rocon_mcs2pxmcc(mcs);
 980   int32_t cur_d_raw;
 981   int32_t cur_q_raw;
 982
 983   pxmcc_axis_get_cur_dq_filt_raw(mcs, &cur_d_raw, &cur_q_raw);
 984
 985   if (mcs->pxms_flg & PXMS_ERR_m) {
 986     pxmc_rocon_pwm2ph_wr(mcs, 0, 0);
 987     pxmcc_axis_pwm_dq_out(mcs, 0, 0);
 988     mcc_axis->steps_inc = 0;
 989     mcsrc->cur_d_err_sum = 0;
 990     mcsrc->cur_q_err_sum = 0;
 991   } else {
 992     int pwm_d, pwm_q;
 993     int cur_d, cur_q;
 994     int cur_d_req, cur_d_err;
 995     int cur_q_req, cur_q_err;
 996     int max_pwm = mcs->pxms_me;
 997     int32_t stpinc;
 998
 999     pxmcc_axis_cur_dq_raw2filt(&cur_d, cur_d_raw);
1000     pxmcc_axis_cur_dq_raw2filt(&cur_q, cur_q_raw);
1001
1002     if (!mcs->pxms_ene)
1003       cur_d_req = 0;
1004     else
1005       cur_d_req = mcsrc->cur_hold;
1006
1007     cur_d_err = cur_d_req - cur_d;
1008
1009     pxmcc_cur_ctrl_pi(&pwm_d, &mcsrc->cur_d_err_sum, cur_d_err,
1010                       mcsrc->cur_d_p, mcsrc->cur_d_i, max_pwm);
1011
1012     /* pwm_d = (pxmc_rocon_pwm_magnitude * ene) >> 15; */
1013
1014     cur_q_req = 0;
1015
1016     cur_q_err = cur_q_req - cur_q;
1017
1018     pxmcc_cur_ctrl_pi(&pwm_q, &mcsrc->cur_q_err_sum, cur_q_err,
1019                       mcsrc->cur_q_p, mcsrc->cur_q_i, max_pwm);
1020
1021     pxmcc_axis_pwm_dq_out(mcs, pwm_d, pwm_q);
1022
1023     mcs->pxms_ap=mcs->pxms_rp;
1024     mcs->pxms_as=mcs->pxms_rs;
1025     mcc_axis->steps_lim = mcc_axis->steps_cnt + 6;
1026
1027     stpinc = mcs->pxms_rs;
1028     mcc_axis->steps_inc = stpinc;
1029
1030       /* stpinc /= (mcs->pxms_ptirc << PXMC_SUBDIV(mcs)); */
1031       /* pxms_ptscale_mult; pxms_ptscale_shift; */
1032   }
1033
1034   return 0;
1035 }
1036
1037 int pxmcc_axis_setup(pxmc_state_t *mcs, int mode)
1038 {
1039   volatile pxmcc_data_t *mcc_data = pxmc_rocon_mcc_data();
1040   volatile pxmcc_axis_data_t *mcc_axis = pxmc_rocon_mcs2pxmcc(mcs);
1041   uint32_t ptirc;
1042   uint32_t ptreci;
1043   uint32_t inp_info;
1044   uint32_t pwmtx_info;
1045   uint32_t pwmtx_info_dummy = 27;
1046   uint64_t ull;
1047   int      i;
1048   int      phcnt = 0;
1049   int      pwm_chan;
1050
1051   if (mcc_axis == NULL)
1052     return -1;
1053
1054   if (mcc_data->common.fwversion != PXMCC_FWVERSION)
1055     return -1;
1056
1057   mcc_axis->ccflg = 0;
1058   mcc_axis->mode = PXMCC_MODE_IDLE;
1059
1060   mcc_data->common.pwm_cycle = PXMC_LXPWR_PWM_CYCLE;
1061
1062   ptirc = mcs->pxms_ptirc;
1063   ull = (1ULL << 32) * mcs->pxms_ptper;
1064   ptreci = (ull + ptirc / 2) / ptirc;
1065
1066   mcc_axis->ptreci = ptreci;
1067
1068   pxmcc_pxmc_ptofs2mcc(mcs, 0);
1069
1070   inp_info = (char*)&fpga_irc[mcs->pxms_inp_info]->count - (char*)fpga_irc[0];
1071   inp_info += mcc_data->common.irc_base;
1072
1073   switch (mode) {
1074     case PXMCC_MODE_IDLE:
1075       phcnt = 0;
1076       break;
1077     case PXMCC_MODE_BLDC:
1078       phcnt = 3;
1079       break;
1080     case PXMCC_MODE_STEPPER_WITH_IRC:
1081       phcnt = 4;
1082       break;
1083     case PXMCC_MODE_STEPPER:
1084       phcnt = 4;
1085       mcc_axis->ptreci = 1;
1086       inp_info = (char*)&mcc_axis->steps_pos - (char*)mcc_data;
1087       break;
1088   }
1089
1090   mcc_axis->inp_info = inp_info;
1091   mcc_axis->out_info = mcs->pxms_out_info;
1092
1093   pwm_chan = mcs->pxms_out_info;
1094
1095   pwmtx_info = (pwmtx_info_dummy << 0) | (pwmtx_info_dummy << 8) |
1096                (pwmtx_info_dummy << 16) | (pwmtx_info_dummy << 24);
1097
1098   for (i = phcnt; --i >= 0; ) {
1099     volatile uint32_t *pwm_reg;
1100     volatile uint32_t *pwm_reg_base = fpga_lx_master_transmitter_base;
1101
1102     pwmtx_info <<= 8;
1103
1104     pwm_reg = pxmc_rocon_pwm_chan2reg(pwm_chan + i);
1105     if (pwm_reg == &pxmc_rocon_pwm_dummy_reg) {
1106       pwmtx_info |= pwmtx_info_dummy;
1107     } else {
1108       pwmtx_info |= pwm_reg - pwm_reg_base;
1109     }
1110   }
1111
1112   mcc_axis->pwmtx_info = pwmtx_info;
1113
1114   mcc_axis->mode = mode;
1115
1116   mcc_axis->ccflg = 0;
1117   mcc_axis->pwm_dq = 0;
1118   mcc_axis->steps_lim = mcc_axis->steps_cnt;
1119   mcc_axis->steps_inc = 0;
1120   mcc_axis->steps_pos = 0;
1121
1122   if (mode != PXMCC_MODE_STEPPER) {
1123     pxmcc_pxmc_ptofs2mcc(mcs, 1);
1124   }
1125   return 0;
1126 }
1127
1128 /*******************************************************************/
1129
1130 volatile void *pxmc_rocon_rx_data_hist_buff;
1131 volatile void *pxmc_rocon_rx_data_hist_buff_end;
1132 int pxmc_rocon_rx_data_hist_mode;
1133
1134 uint32_t pxmc_rocon_rx_last_irq;
1135 uint32_t pxmc_rocon_rx_cycle_time;
1136 uint32_t pxmc_rocon_rx_irq_latency;
1137 uint32_t pxmc_rocon_rx_irq_latency_max;
1138
1139 IRQ_HANDLER_FNC(pxmc_rocon_rx_done_isr)
1140 {
1141   uint32_t ir;
1142
1143   ir = ROCON_RX_TIM->IR & LPC_TIM_IR_ALL_m;
1144   ROCON_RX_TIM->IR = ir;
1145   if (ir & LPC_TIM_IR_CR1INT_m) {
1146     uint32_t cr0, cr1;
1147     cr0 = ROCON_RX_TIM->CR0;
1148     cr1 = ROCON_RX_TIM->CR1;
1149
1150     pxmc_rocon_rx_cycle_time = cr1 - pxmc_rocon_rx_last_irq;
1151     pxmc_rocon_rx_last_irq = cr1;
1152
1153     hal_gpio_set_value(T2MAT0_PIN, 1);
1154     hal_gpio_set_value(T2MAT1_PIN, 0);
1155     hal_gpio_set_value(T2MAT0_PIN, 0);
1156
1157     if (pxmc_rocon_rx_data_hist_buff >= pxmc_rocon_rx_data_hist_buff_end)
1158       pxmc_rocon_rx_data_hist_buff = NULL;
1159
1160     if (pxmc_rocon_rx_data_hist_buff != NULL) {
1161       if (pxmc_rocon_rx_data_hist_mode == 0) {
1162         int i;
1163         volatile uint32_t *pwm_reg = fpga_lx_master_transmitter_base + 8;
1164         volatile uint32_t *rec_reg = fpga_lx_master_receiver_base + 8;
1165         uint16_t *pbuf = (uint16_t *)pxmc_rocon_rx_data_hist_buff;
1166         for (i = 0; i < 8; i++) {
1167           *(pbuf++) = *(rec_reg++);
1168         }
1169         for (i = 0; i < 8; i++) {
1170           *(pbuf++) = *(pwm_reg++);
1171         }
1172         pxmc_rocon_rx_data_hist_buff = pbuf;
1173       } else if (pxmc_rocon_rx_data_hist_mode == 1) {
1174         int i;
1175         uint32_t *pbuf = (uint32_t *)pxmc_rocon_rx_data_hist_buff;
1176         pxmcc_data_t *mcc_data = pxmc_rocon_mcc_data();
1177         pxmcc_axis_data_t *mcc_axis = mcc_data->axis + 0;
1178         uint32_t *ptumbl = (uint32_t *)mcc_axis;
1179
1180         for (i = 0; i < 16; i++)
1181           *(pbuf++) = *(ptumbl++);
1182
1183         pxmc_rocon_rx_data_hist_buff = pbuf;
1184       }
1185     }
1186
1187     pxmc_rocon_rx_irq_latency = ROCON_RX_TIM->TC - cr1;
1188     if (pxmc_rocon_rx_irq_latency > pxmc_rocon_rx_irq_latency_max)
1189       pxmc_rocon_rx_irq_latency_max = pxmc_rocon_rx_irq_latency;
1190
1191    #ifdef PXMC_ROCON_TIMED_BY_RX_DONE
1192     pxmc_sfi_isr();
1193     do_pxmc_coordmv();
1194    #endif /*PXMC_ROCON_TIMED_BY_RX_DONE*/
1195   }
1196
1197   return IRQ_HANDLED;
1198 }
1199
1200 int
1201 pxmc_rocon_rx_done_isr_setup(irq_handler_t rx_done_isr_handler)
1202 {
1203
1204   disable_irq(ROCON_RX_IRQn);
1205
1206   hal_pin_conf_set(T2MAT0_PIN, PORT_CONF_GPIO_OUT_LO);
1207   hal_pin_conf_set(T2MAT1_PIN, PORT_CONF_GPIO_OUT_LO);
1208   hal_pin_conf(T2CAP0_PIN);
1209   hal_pin_conf(T2CAP1_PIN);
1210
1211   hal_gpio_direction_output(T2MAT0_PIN, 1);
1212   hal_gpio_direction_output(T2MAT1_PIN, 0);
1213   hal_gpio_set_value(T2MAT0_PIN, 0);
1214
1215   /* Enable CLKOUT pin function, source CCLK, divide by 1 */
1216   LPC_SC->CLKOUTCFG = 0x0100;
1217
1218   request_irq(ROCON_RX_IRQn, rx_done_isr_handler, 0, NULL,NULL);
1219
1220   ROCON_RX_TIM->TCR = 0;
1221   ROCON_RX_TIM->CTCR = 0;
1222   ROCON_RX_TIM->PR = 0; /* Divide by 1 */
1223
1224   ROCON_RX_TIM->CCR = LPC_TIM_CCR_CAP0RE_m | LPC_TIM_CCR_CAP1FE_m |
1225                    LPC_TIM_CCR_CAP1I_m;
1226
1227   ROCON_RX_TIM->EMR = __val2mfld(LPC_TIM_EMR_EMC0_m, LPC_TIM_EMR_NOP) |
1228                    __val2mfld(LPC_TIM_EMR_EMC1_m, LPC_TIM_EMR_NOP);
1229
1230   ROCON_RX_TIM->MCR = 0;                        /* No IRQ on MRx */
1231   ROCON_RX_TIM->TCR = LPC_TIM_TCR_CEN_m;        /* Enable timer counting */
1232   enable_irq(ROCON_RX_IRQn);            /* Enable interrupt */
1233
1234   return 0;
1235
1236 }
1237
1238 int
1239 pxmc_rocon_pwm_master_init(void)
1240 {
1241   int i;
1242   int grp_in = 0;
1243   int grp_out = 0;
1244   unsigned word_slot;
1245   unsigned receiver_done_div = 1;
1246  #ifdef LXPWR_WITH_SIROLADC
1247   unsigned lxpwr_header = 1;
1248   unsigned lxpwr_words = 1 + 8 * 2 + 2;
1249   unsigned lxpwr_chips = 2;
1250   unsigned lxpwr_chip_pwm_cnt = 8;
1251  #else /*LXPWR_WITH_SIROLADC*/
1252   unsigned lxpwr_header = 0;
1253   unsigned lxpwr_words = 8;
1254   unsigned lxpwr_chips = 2;
1255   unsigned lxpwr_chip_pwm_cnt = 8;
1256  #endif /*LXPWR_WITH_SIROLADC*/
1257
1258  #ifdef PXMC_ROCON_TIMED_BY_RX_DONE
1259   receiver_done_div = 5;
1260  #endif /*PXMC_ROCON_TIMED_BY_RX_DONE*/
1261
1262   *fpga_lx_master_reset = 1;
1263   *fpga_lx_master_transmitter_reg = 0;
1264   *fpga_lx_master_transmitter_cycle = PXMC_LXPWR_PWM_CYCLE; /* 50 MHz -> 20 kHz */
1265   *fpga_lx_master_receiver_done_div = receiver_done_div << 8;
1266
1267   for (i = 0; i < LX_MASTER_DATA_OFFS + lxpwr_words * lxpwr_chips; i++)
1268     fpga_lx_master_receiver_base[i] = 0;
1269
1270   word_slot = LX_MASTER_DATA_OFFS;
1271   fpga_lx_master_receiver_base[grp_in++] = (word_slot << 8) | lxpwr_words;
1272   fpga_lx_master_receiver_base[grp_in++] = 0x0000;
1273
1274   for (i = 0; i < LX_MASTER_DATA_OFFS + lxpwr_words * lxpwr_chips; i++)
1275     fpga_lx_master_transmitter_base[i] = 0;
1276
1277   word_slot = LX_MASTER_DATA_OFFS + lxpwr_header + lxpwr_chip_pwm_cnt;
1278   fpga_lx_master_transmitter_base[grp_out++] = (word_slot << 8) | lxpwr_words;
1279  #ifdef LXPWR_WITH_SIROLADC
1280   fpga_lx_master_transmitter_base[word_slot] = 0xc100 | (lxpwr_words - 1);
1281  #endif /*LXPWR_WITH_SIROLADC*/
1282
1283   word_slot = LX_MASTER_DATA_OFFS + 0;
1284   fpga_lx_master_transmitter_base[grp_out++] = (word_slot << 8) | lxpwr_words;
1285  #ifdef LXPWR_WITH_SIROLADC
1286   fpga_lx_master_transmitter_base[word_slot] = 0xc100 | (lxpwr_words - 1);
1287  #endif /*LXPWR_WITH_SIROLADC*/
1288
1289   fpga_lx_master_transmitter_base[grp_out++] = 0x0000;
1290
1291   *fpga_lx_master_reset = 0;
1292   *fpga_lx_master_transmitter_cycle = PXMC_LXPWR_PWM_CYCLE; /* 50 MHz -> 20 kHz */
1293   *fpga_lx_master_receiver_done_div = receiver_done_div << 8;
1294
1295   return 0;
1296 }
1297
1298 int pxmc_ptofs_from_index(pxmc_state_t *mcs, unsigned long irc,
1299                            unsigned long index_irc, int diff2err)
1300 {
1301   long ofsl;
1302   short ofs;
1303
1304   ofs = ofsl = index_irc - mcs->pxms_ptmark;
1305
1306   if (diff2err) {
1307     short diff;
1308     diff = (unsigned short)ofs - (unsigned short)mcs->pxms_ptofs;
1309     if (diff >= mcs->pxms_ptirc / 2)
1310       diff -= mcs->pxms_ptirc;
1311     if (diff <= -mcs->pxms_ptirc / 2)
1312       diff += mcs->pxms_ptirc;
1313     if (diff < 0)
1314       diff = -diff;
1315     if(diff >= mcs->pxms_ptirc / 6) {
1316       return -1;
1317     }
1318   } else {
1319     long diff;
1320     diff = (unsigned long)ofsl - irc;
1321     ofs = ofsl - (diff / mcs->pxms_ptirc) * mcs->pxms_ptirc;
1322   }
1323
1324   mcs->pxms_ptofs = ofs;
1325
1326   return 1;
1327 }
1328
1329 /**
1330  * pxmc_dummy_con - Dummy controller for synchronous BLDC/PMSM/steper drive
1331  * @mcs:        Motion controller state information
1332  */
1333 int
1334 pxmc_dummy_con(pxmc_state_t *mcs)
1335 {
1336   return 0;
1337 }
1338
1339 int pxmc_rocon_hh_gd10(pxmc_state_t *mcs);
1340 int pxmc_rocon_hh_gd20(pxmc_state_t *mcs);
1341 int pxmc_rocon_hh_gd30(pxmc_state_t *mcs);
1342 int pxmc_rocon_hh_gd40(pxmc_state_t *mcs);
1343 int pxmc_rocon_hh_gd50(pxmc_state_t *mcs);
1344 int pxmc_rocon_hh_gd90(pxmc_state_t *mcs);
1345
1346 /* initialize for hard home */
1347 int
1348 pxmc_rocon_hh_gi(pxmc_state_t *mcs)
1349 {
1350   pxmc_set_flag(mcs,PXMS_BSY_b);
1351  #if 0 /* config and speed already set in pxmc_hh */
1352   spd = mcs->pxms_ms;
1353   spd >>= mcs->pxms_cfg & PXMS_CFG_HSPD_m;
1354   if(!spd) spd = 1;
1355   if(mcs->pxms_cfg & PXMS_CFG_HDIR_m)
1356     spd = -spd;
1357   mcs->pxms_gen_hsp = spd;
1358  #endif
1359   mcs->pxms_rsfg = 0;
1360   mcs->pxms_gen_htim = 16;
1361   mcs->pxms_do_gen = pxmc_rocon_hh_gd10;
1362   return pxmc_rocon_hh_gd10(mcs);
1363 }
1364
1365 /* fill initial mark filter and then decide */
1366 int
1367 pxmc_rocon_hh_gd10(pxmc_state_t *mcs)
1368 {
1369   int mark;
1370
1371   if(mcs->pxms_flg & PXMS_ERR_m)
1372     return pxmc_spdnext_gend(mcs);
1373
1374   pxmc_spd_gacc(&(mcs->pxms_rs), 0, mcs->pxms_ma);
1375   mcs->pxms_rp += mcs->pxms_rs;
1376
1377   mark = pxmc_inp_rocon_is_mark(mcs);
1378
1379   if (mcs->pxms_gen_htim) {
1380     mcs->pxms_gen_htim--;
1381     return 0;
1382   }
1383
1384   if (!(mcs->pxms_cfg & PXMS_CFG_HLS_m)) {
1385     /* limit switch is not used */
1386     pxmc_inp_rocon_is_index_edge(mcs);
1387     mcs->pxms_do_gen = pxmc_rocon_hh_gd50;
1388     return 0;
1389   }
1390
1391   if (mark >= 6) {
1392     /* go out from switch */
1393     mcs->pxms_do_gen = pxmc_rocon_hh_gd20;
1394   } else {
1395     /* switch is off -> look for it */
1396     mcs->pxms_do_gen = pxmc_rocon_hh_gd30;
1397   }
1398   return 0;
1399 }
1400
1401 /* go out from switch */
1402 int
1403 pxmc_rocon_hh_gd20(pxmc_state_t *mcs)
1404 {
1405   int mark;
1406
1407   if(mcs->pxms_flg & PXMS_ERR_m)
1408     return pxmc_spdnext_gend(mcs);
1409
1410   mark = pxmc_inp_rocon_is_mark(mcs);
1411
1412   if (mark <= 1) {
1413     /* switch is off -> look for it again */
1414     mcs->pxms_do_gen = pxmc_rocon_hh_gd30;
1415   }
1416
1417   pxmc_spd_gacc(&(mcs->pxms_rs), -mcs->pxms_gen_hsp, mcs->pxms_ma);
1418   mcs->pxms_rp += mcs->pxms_rs;
1419
1420   return 0;
1421 }
1422
1423 /* switch is off -> look for it */
1424 int
1425 pxmc_rocon_hh_gd30(pxmc_state_t *mcs)
1426 {
1427   long spd;
1428   int mark;
1429
1430   if (mcs->pxms_flg & PXMS_ERR_m)
1431     return pxmc_spdnext_gend(mcs);
1432
1433   pxmc_spd_gacc(&(mcs->pxms_rs), mcs->pxms_gen_hsp, mcs->pxms_ma);
1434   mcs->pxms_rp += mcs->pxms_rs;
1435
1436   mark = pxmc_inp_rocon_is_mark(mcs);
1437
1438   if (mark >= 8){
1439     spd = mcs->pxms_gen_hsp >> 2; /* slow down */
1440     if (!spd)
1441       spd = 1;
1442     mcs->pxms_gen_hsp = spd;
1443     mcs->pxms_do_gen = pxmc_rocon_hh_gd40;
1444   }
1445
1446   return 0;
1447 }
1448
1449 /* switch is on -> find edge */
1450 int
1451 pxmc_rocon_hh_gd40(pxmc_state_t *mcs)
1452 {
1453   int mark;
1454
1455   if (mcs->pxms_flg & PXMS_ERR_m)
1456     return pxmc_spdnext_gend(mcs);
1457
1458   mark = pxmc_inp_rocon_is_mark(mcs);
1459
1460   if (mark <= 1) {
1461     if (mcs->pxms_cfg & PXMS_CFG_HRI_m) {
1462       pxmc_inp_rocon_is_index_edge(mcs);
1463       mcs->pxms_do_gen = pxmc_rocon_hh_gd50;
1464     } else {
1465       pxmc_inp_rocon_ap_zero(mcs);
1466       mcs->pxms_do_gen = pxmc_stop_gi;
1467     }
1468   }
1469
1470   pxmc_spd_gacc(&(mcs->pxms_rs), -mcs->pxms_gen_hsp, mcs->pxms_ma);
1471   mcs->pxms_rp += mcs->pxms_rs;
1472
1473   return 0;
1474 }
1475
1476 /* calibrate on revolution index */
1477 int
1478 pxmc_rocon_hh_gd50(pxmc_state_t *mcs)
1479 {
1480   if (mcs->pxms_flg & PXMS_ERR_m)
1481     return pxmc_spdnext_gend(mcs);
1482
1483   pxmc_spd_gacc(&(mcs->pxms_rs), mcs->pxms_gen_hsp, mcs->pxms_ma);
1484   mcs->pxms_rp += mcs->pxms_rs;
1485
1486   if (pxmc_inp_rocon_is_index_edge(mcs)){
1487     pxmc_inp_rocon_irc_offset_from_index(mcs);
1488     mcs->pxms_do_gen = pxmc_stop_gi;
1489   }
1490
1491   return 0;
1492 }
1493
1494 pxmc_call_t *pxmc_get_hh_gi_4axis(pxmc_state_t *mcs)
1495 {
1496   return pxmc_rocon_hh_gi;
1497 }
1498
1499 pxmc_rocon_state_t mcs0 =
1500 {
1501 .base = {
1502 pxms_flg:
1503   PXMS_ENI_m,
1504 pxms_do_inp:
1505   pxmc_inp_rocon_inp,
1506 pxms_do_con:
1507   pxmc_pid_con /*pxmc_dummy_con*/,
1508 pxms_do_out:
1509   pxmc_rocon_pwm_dc_out /*pxmc_rocon_pwm3ph_out*/,
1510   pxms_do_deb: 0,
1511   pxms_do_gen: 0,
1512 pxms_do_ap2hw:
1513   pxmc_inp_rocon_ap2hw,
1514   pxms_ap: 0, pxms_as: 0,
1515   pxms_rp: 55 * 256, pxms_rs: 0, pxms_subdiv: 8,
1516   pxms_md: 800 << 8, pxms_ms: 500, pxms_ma: 10,
1517   pxms_inp_info: 0,
1518   pxms_out_info: 0,
1519   pxms_ene: 0, pxms_erc: 0,
1520   pxms_p: 80, pxms_i: 10, pxms_d: 200, pxms_s1: 200, pxms_s2: 0,
1521   pxms_me: 0x7e00/*0x7fff*/,
1522 pxms_cfg:
1523   PXMS_CFG_SMTH_m | PXMS_CFG_MD2E_m | PXMS_CFG_HLS_m | PXMS_CFG_HPS_m * 0 |
1524   PXMS_CFG_HRI_m * 0 | PXMS_CFG_HDIR_m * 0 |
1525   PXMS_CFG_I2PT_m * 0 | 0x2,
1526
1527   pxms_ptper: 1,
1528   pxms_ptirc: 1000,
1529   pxms_ptmark: 1180,
1530   /*pxms_ptamp: 0x7fff,*/
1531
1532   pxms_hal: 0x40,
1533 },
1534   .cur_d_p = 150,
1535   .cur_d_i = 6000,
1536   .cur_q_p = 150,
1537   .cur_q_i = 6000,
1538   .cur_hold = 200,
1539 };
1540
1541 pxmc_rocon_state_t mcs1 =
1542 {
1543 .base = {
1544 pxms_flg:
1545   PXMS_ENI_m,
1546 pxms_do_inp:
1547   pxmc_inp_rocon_inp,
1548 pxms_do_con:
1549   pxmc_pid_con,
1550 pxms_do_out:
1551   pxmc_rocon_pwm_dc_out,
1552   pxms_do_deb: 0,
1553   pxms_do_gen: 0,
1554 pxms_do_ap2hw:
1555   pxmc_inp_rocon_ap2hw,
1556   pxms_ap: 0, pxms_as: 0,
1557   pxms_rp: 55 * 256, pxms_rs: 0, pxms_subdiv: 8,
1558   pxms_md: 800 << 8, pxms_ms: 500, pxms_ma: 10,
1559   pxms_inp_info: 1,
1560   pxms_out_info: 2,
1561   pxms_ene: 0, pxms_erc: 0,
1562   pxms_p: 80, pxms_i: 10, pxms_d: 200, pxms_s1: 200, pxms_s2: 0,
1563   pxms_me: 0x7e00/*0x7fff*/,
1564 pxms_cfg:
1565   PXMS_CFG_SMTH_m | PXMS_CFG_MD2E_m | PXMS_CFG_HLS_m | PXMS_CFG_HPS_m * 0 |
1566   PXMS_CFG_HRI_m * 0 | PXMS_CFG_HDIR_m * 0 |
1567   PXMS_CFG_I2PT_m * 0 | 0x2,
1568
1569   pxms_ptper: 1,
1570   pxms_ptirc: 1000,
1571   /*pxms_ptamp: 0x7fff,*/
1572
1573   pxms_hal: 0x40,
1574 }};
1575
1576 pxmc_rocon_state_t mcs2 =
1577 {
1578 .base = {
1579 pxms_flg:
1580   PXMS_ENI_m,
1581 pxms_do_inp:
1582   pxmc_inp_rocon_inp,
1583 pxms_do_con:
1584   pxmc_pid_con,
1585 pxms_do_out:
1586   pxmc_rocon_pwm_dc_out,
1587   pxms_do_deb: 0,
1588   pxms_do_gen: 0,
1589 pxms_do_ap2hw:
1590   pxmc_inp_rocon_ap2hw,
1591   pxms_ap: 0, pxms_as: 0,
1592   pxms_rp: 55 * 256, pxms_rs: 0, pxms_subdiv: 8,
1593   pxms_md: 800 << 8, pxms_ms: 500, pxms_ma: 10,
1594   pxms_inp_info: 2,
1595   pxms_out_info: 4,
1596   pxms_ene: 0, pxms_erc: 0,
1597   pxms_p: 80, pxms_i: 10, pxms_d: 200, pxms_s1: 200, pxms_s2: 0,
1598   pxms_me: 0x7e00/*0x7fff*/,
1599 pxms_cfg:
1600   PXMS_CFG_SMTH_m | PXMS_CFG_MD2E_m | PXMS_CFG_HLS_m | PXMS_CFG_HPS_m * 0 |
1601   PXMS_CFG_I2PT_m * 0 | PXMS_CFG_HRI_m |
1602   PXMS_CFG_HDIR_m | 0x2,
1603
1604   pxms_ptper: 1,
1605   pxms_ptirc: 1000,
1606   /*pxms_ptamp: 0x7fff,*/
1607
1608   pxms_hal: 0x40,
1609 }};
1610
1611 pxmc_rocon_state_t mcs3 =
1612 {
1613 .base = {
1614 pxms_flg:
1615   PXMS_ENI_m,
1616 pxms_do_inp:
1617   pxmc_inp_rocon_inp,
1618 pxms_do_con:
1619   pxmc_pid_con,
1620 pxms_do_out:
1621   pxmc_rocon_pwm_dc_out,
1622   pxms_do_deb: 0,
1623   pxms_do_gen: 0,
1624 pxms_do_ap2hw:
1625   pxmc_inp_rocon_ap2hw,
1626   pxms_ap: 0, pxms_as: 0,
1627   pxms_rp: 55 * 256, pxms_rs: 0, pxms_subdiv: 8,
1628   pxms_md: 800 << 8, pxms_ms: 500, pxms_ma: 10,
1629   pxms_inp_info: 3,
1630   pxms_out_info: 6,
1631   pxms_ene: 0, pxms_erc: 0,
1632   pxms_p: 80, pxms_i: 10, pxms_d: 200, pxms_s1: 200, pxms_s2: 0,
1633   pxms_me: 0x7e00/*0x7fff*/,
1634 pxms_cfg:
1635   PXMS_CFG_SMTH_m | PXMS_CFG_MD2E_m | PXMS_CFG_HLS_m | PXMS_CFG_HPS_m * 0 |
1636   PXMS_CFG_I2PT_m * 0 | PXMS_CFG_HRI_m |
1637   PXMS_CFG_HDIR_m * 0 | 0x2,
1638
1639   pxms_ptper: 1,
1640   pxms_ptirc: 1000,
1641   /*pxms_ptamp: 0x7fff,*/
1642
1643   pxms_hal: 0x40,
1644 }};
1645
1646 pxmc_rocon_state_t mcs4 =
1647 {
1648 .base = {
1649 pxms_flg:
1650   PXMS_ENI_m,
1651 pxms_do_inp:
1652   pxmc_inp_rocon_inp,
1653 pxms_do_con:
1654   pxmc_pid_con,
1655 pxms_do_out:
1656   pxmc_rocon_pwm_dc_out,
1657   pxms_do_deb: 0,
1658   pxms_do_gen: 0,
1659 pxms_do_ap2hw:
1660   pxmc_inp_rocon_ap2hw,
1661   pxms_ap: 0, pxms_as: 0,
1662   pxms_rp: 55 * 256, pxms_rs: 0, pxms_subdiv: 8,
1663   pxms_md: 800 << 8, pxms_ms: 500, pxms_ma: 10,
1664   pxms_inp_info: 4,
1665   pxms_out_info: 8,
1666   pxms_ene: 0, pxms_erc: 0,
1667   pxms_p: 80, pxms_i: 10, pxms_d: 200, pxms_s1: 200, pxms_s2: 0,
1668   pxms_me: 0x7e00/*0x7fff*/,
1669 pxms_cfg:
1670   PXMS_CFG_SMTH_m | PXMS_CFG_MD2E_m | PXMS_CFG_HLS_m | PXMS_CFG_HPS_m * 0 |
1671   PXMS_CFG_HRI_m | PXMS_CFG_I2PT_m * 0 |
1672   PXMS_CFG_HDIR_m | 0x2,
1673
1674   pxms_ptper: 1,
1675   pxms_ptirc: 1000,
1676   /*pxms_ptamp: 0x7fff,*/
1677
1678   pxms_hal: 0x40,
1679 }};
1680
1681 pxmc_rocon_state_t mcs5 =
1682 {
1683 .base = {
1684 pxms_flg:
1685   PXMS_ENI_m,
1686 pxms_do_inp:
1687   pxmc_inp_rocon_inp,
1688 pxms_do_con:
1689   pxmc_pid_con,
1690 pxms_do_out:
1691   pxmc_rocon_pwm_dc_out,
1692   pxms_do_deb: 0,
1693   pxms_do_gen: 0,
1694 pxms_do_ap2hw:
1695   pxmc_inp_rocon_ap2hw,
1696   pxms_ap: 0, pxms_as: 0,
1697   pxms_rp: 55 * 256, pxms_rs: 0, pxms_subdiv: 8,
1698   pxms_md: 800 << 8, pxms_ms: 500, pxms_ma: 10,
1699   pxms_inp_info: 5,
1700   pxms_out_info: 10,
1701   pxms_ene: 0, pxms_erc: 0,
1702   pxms_p: 80, pxms_i: 10, pxms_d: 200, pxms_s1: 200, pxms_s2: 0,
1703   pxms_me: 0x7e00/*0x7fff*/,
1704 pxms_cfg:
1705   PXMS_CFG_SMTH_m | PXMS_CFG_MD2E_m | PXMS_CFG_HLS_m | PXMS_CFG_HPS_m |
1706   PXMS_CFG_HRI_m | PXMS_CFG_I2PT_m * 0 |
1707   PXMS_CFG_HDIR_m | 0x2,
1708
1709   pxms_ptper: 1,
1710   pxms_ptirc: 1000,
1711   /*pxms_ptamp: 0x7fff,*/
1712
1713   pxms_hal: 0x40,
1714 }};
1715
1716 pxmc_rocon_state_t mcs6 =
1717 {
1718 .base = {
1719 pxms_flg:
1720   PXMS_ENI_m,
1721 pxms_do_inp:
1722   pxmc_inp_rocon_inp,
1723 pxms_do_con:
1724   pxmc_pid_con,
1725 pxms_do_out:
1726   pxmc_rocon_pwm_dc_out,
1727   pxms_do_deb: 0,
1728   pxms_do_gen: 0,
1729 pxms_do_ap2hw:
1730   pxmc_inp_rocon_ap2hw,
1731   pxms_ap: 0, pxms_as: 0,
1732   pxms_rp: 55 * 256, pxms_rs: 0, pxms_subdiv: 8,
1733   pxms_md: 800 << 8, pxms_ms: 500, pxms_ma: 10,
1734   pxms_inp_info: 6,
1735   pxms_out_info: 12,
1736   pxms_ene: 0, pxms_erc: 0,
1737   pxms_p: 80, pxms_i: 10, pxms_d: 200, pxms_s1: 200, pxms_s2: 0,
1738   pxms_me: 0x7e00/*0x7fff*/,
1739 pxms_cfg:
1740   PXMS_CFG_SMTH_m | PXMS_CFG_MD2E_m | PXMS_CFG_HLS_m | PXMS_CFG_I2PT_m * 0 |
1741   0x1,
1742
1743   pxms_ptper: 1,
1744   pxms_ptirc: 1000,
1745   /*pxms_ptamp: 0x7fff,*/
1746
1747   pxms_hal: 0x40,
1748 }};
1749
1750 pxmc_rocon_state_t mcs7 =
1751 {
1752 .base = {
1753 pxms_flg:
1754   PXMS_ENI_m,
1755 pxms_do_inp:
1756   pxmc_inp_rocon_inp,
1757 pxms_do_con:
1758   pxmc_pid_con,
1759 pxms_do_out:
1760   pxmc_rocon_pwm_dc_out,
1761   pxms_do_deb: 0,
1762   pxms_do_gen: 0,
1763 pxms_do_ap2hw:
1764   pxmc_inp_rocon_ap2hw,
1765   pxms_ap: 0, pxms_as: 0,
1766   pxms_rp: 55 * 256, pxms_rs: 0, pxms_subdiv: 8,
1767   pxms_md: 800 << 8, pxms_ms: 500, pxms_ma: 10,
1768   pxms_inp_info: 7,
1769   pxms_out_info: 14,
1770   pxms_ene: 0, pxms_erc: 0,
1771   pxms_p: 80, pxms_i: 10, pxms_d: 200, pxms_s1: 200, pxms_s2: 0,
1772   pxms_me: 0x7e00/*0x7fff*/,
1773 pxms_cfg:
1774   PXMS_CFG_SMTH_m | PXMS_CFG_MD2E_m | PXMS_CFG_HLS_m | PXMS_CFG_I2PT_m * 0 |
1775   0x1,
1776
1777   pxms_ptper: 1,
1778   pxms_ptirc: 1000,
1779   /*pxms_ptamp: 0x7fff,*/
1780
1781   pxms_hal: 0x40,
1782 }};
1783
1784 pxmc_state_t *pxmc_main_arr[PXML_MAIN_CNT] =
1785 {&mcs0.base, &mcs1.base, &mcs2.base, &mcs3.base,
1786  &mcs4.base, &mcs5.base, &mcs6.base, &mcs7.base};
1787
1788
1789 pxmc_state_list_t  pxmc_main_list =
1790 {
1791 pxml_arr:
1792   pxmc_main_arr,
1793   pxml_cnt: 0
1794 };
1795
1796 void pxmc_lpc_qei_callback_index(struct lpc_qei_state_t *qst, void *context)
1797 {
1798   pxmc_state_t *mcs = (pxmc_state_t *)context;
1799   short ofs;
1800   short irc;
1801   irc = qst->index_pos;
1802
1803   if ((mcs->pxms_cfg & PXMS_CFG_I2PT_m) && (mcs->pxms_flg & PXMS_PTI_m))
1804   {
1805     short diff;
1806     ofs = irc - mcs->pxms_ptmark;
1807     diff = ofs - mcs->pxms_ptofs;
1808
1809     if (diff >= mcs->pxms_ptirc / 2)
1810       diff -= mcs->pxms_ptirc;
1811
1812     if (diff <= -mcs->pxms_ptirc / 2)
1813       diff += mcs->pxms_ptirc;
1814
1815     if (diff < 0)
1816       diff = -diff;
1817
1818     if (diff >= mcs->pxms_ptirc / 6)
1819     {
1820       pxmc_set_errno(mcs, PXMS_E_I2PT_TOOBIG);
1821     }
1822     else
1823     {
1824       mcs->pxms_ptofs = ofs;
1825       pxmc_set_flag(mcs, PXMS_PHA_b);
1826     }
1827   } /*else {
1828
1829     ofs=irc-mcs->pxms_ptofs;
1830     if((unsigned short)ofs>=(unsigned short)mcs->pxms_ptirc) {
1831       if(ofs>0) {
1832         ofs-=mcs->pxms_ptirc;
1833       } else {
1834         ofs+=mcs->pxms_ptirc;
1835       }
1836     }
1837     mcs->pxms_ptmark=ofs;
1838   }*/
1839
1840   /*lpc_qei_set_cmpos(qst, 0, qst->index_pos-4000);*/
1841 }
1842
1843 int pxmc_lpc_pthalalign_callback(pxmc_state_t *mcs)
1844 {
1845   short ptofs;
1846   short ptmark;
1847   lpc_qei_state_t *qst = &lpc_qei_state;
1848   int irc = lpc_qei_get_pos(qst);
1849   int idx_rel;
1850
1851   if (!qst->index_occ)
1852     return 0;
1853
1854   idx_rel = qst->index_pos - irc;
1855   idx_rel %= mcs->pxms_ptirc;
1856
1857   if (idx_rel < 0)
1858     idx_rel += mcs->pxms_ptirc;
1859
1860   ptofs = irc - mcs->pxms_ptofs;
1861   ptmark = ptofs + idx_rel;
1862
1863   if ((unsigned short)ptmark >= mcs->pxms_ptirc)
1864   {
1865     if (ptmark > 0)
1866       ptmark -= mcs->pxms_ptirc;
1867     else
1868       ptmark += mcs->pxms_ptirc;
1869   }
1870
1871   if ((unsigned short)ptmark < mcs->pxms_ptirc)
1872   {
1873     mcs->pxms_ptmark = ptmark;
1874   }
1875
1876   return 0;
1877 }
1878
1879 int pxmc_lpc_pthalalign(pxmc_state_t *mcs, int periods)
1880 {
1881   int res;
1882   long r2acq;
1883   long spd;
1884   pxmc_call_t *fin_fnc = pxmc_lpc_pthalalign_callback;
1885   lpc_qei_state_t *qst = &lpc_qei_state;
1886
1887   mcs->pxms_cfg &= ~PXMS_CFG_I2PT_m;
1888   lpc_qei_setup_index_catch(qst);
1889
1890   r2acq = ((long)mcs->pxms_ptirc << PXMC_SUBDIV(mcs)) * periods;
1891   spd = 1 << PXMC_SUBDIV(mcs);
1892
1893   res = pxmc_pthalalign(mcs, r2acq, spd, fin_fnc);
1894
1895   return res;
1896 }
1897
1898 int pxmc_lpc_pthalalign_run(void)
1899 {
1900   return pxmc_lpc_pthalalign(pxmc_main_list.pxml_arr[0], 20);
1901 }
1902
1903 static inline void pxmc_sfi_input(void)
1904 {
1905   int var;
1906   pxmc_state_t *mcs;
1907   pxmc_for_each_mcs(var, mcs)
1908   {
1909     /* PXMS_ENI_m - check if input (IRC) update is enabled */
1910     if (mcs->pxms_flg & PXMS_ENI_m)
1911     {
1912       pxmc_call(mcs, mcs->pxms_do_inp);
1913     }
1914   }
1915 }
1916
1917 static inline void pxmc_sfi_controller_and_output(void)
1918 {
1919   int var;
1920   pxmc_state_t *mcs;
1921   pxmc_for_each_mcs(var, mcs)
1922   {
1923     /* PXMS_ENR_m - check if controller is enabled */
1924     if (mcs->pxms_flg & PXMS_ENR_m || mcs->pxms_flg & PXMS_ENO_m)
1925     {
1926
1927       /* If output only is enabled, we skip the controller */
1928       if (mcs->pxms_flg & PXMS_ENR_m)
1929       {
1930
1931         pxmc_call(mcs, mcs->pxms_do_con);
1932
1933         /* PXMS_ERR_m - if axis in error state */
1934         if (mcs->pxms_flg & PXMS_ERR_m)
1935           mcs->pxms_ene = 0;
1936       }
1937
1938       /* for bushless motors, it is necessary to call do_out
1939         even if the controller is not enabled and PWM should be provided. */
1940       pxmc_call(mcs, mcs->pxms_do_out);
1941     }
1942   }
1943 }
1944
1945 static inline void pxmc_sfi_generator(void)
1946 {
1947   int var;
1948   pxmc_state_t *mcs;
1949   pxmc_for_each_mcs(var, mcs)
1950   {
1951     /* PXMS_ENG_m - check if requested value (position) generator is enabled */
1952     if (mcs->pxms_flg & PXMS_ENG_m)
1953     {
1954       pxmc_call(mcs, mcs->pxms_do_gen);
1955     }
1956   }
1957 }
1958
1959 static inline void pxmc_sfi_dbg(void)
1960 {
1961   int var;
1962   pxmc_state_t *mcs;
1963   pxmc_for_each_mcs(var, mcs)
1964   {
1965     if (mcs->pxms_flg & PXMS_DBG_m)
1966     {
1967       pxmc_call(mcs, mcs->pxms_do_deb);
1968     }
1969   }
1970 }
1971
1972 int pxmc_rocon_pthalalign_callback(pxmc_state_t *mcs)
1973 {
1974   short ptofs;
1975   short ptmark;
1976   int inp_chan = mcs->pxms_inp_info;
1977   int idx_rel;
1978   long irc = fpga_irc[inp_chan]->count;
1979
1980   if (!pxmc_inp_rocon_is_index_edge(mcs))
1981     return 0;
1982
1983   idx_rel = fpga_irc[inp_chan]->count_index - irc;
1984   idx_rel %= mcs->pxms_ptirc;
1985   if(idx_rel < 0)
1986     idx_rel += mcs->pxms_ptirc;
1987
1988   ptofs = irc-mcs->pxms_ptofs;
1989   ptmark = ptofs+idx_rel;
1990
1991   if((unsigned short)ptmark >= mcs->pxms_ptirc) {
1992     if(ptmark>0)
1993       ptmark -= mcs->pxms_ptirc;
1994     else
1995       ptmark += mcs->pxms_ptirc;
1996   }
1997
1998   if((unsigned short)ptmark < mcs->pxms_ptirc) {
1999     mcs->pxms_ptmark = ptmark;
2000   }
2001   return 0;
2002 }
2003
2004 int pxmc_rocon_pthalalign(pxmc_state_t *mcs, int periods)
2005 {
2006   int res;
2007   long r2acq;
2008   long spd;
2009   pxmc_call_t *fin_fnc = pxmc_rocon_pthalalign_callback;
2010
2011   mcs->pxms_cfg&=~PXMS_CFG_I2PT_m;
2012
2013   r2acq=((long)mcs->pxms_ptirc<<PXMC_SUBDIV(mcs))*periods;
2014   spd=1<<PXMC_SUBDIV(mcs);
2015
2016   /* clear bit indicating previous index */
2017   pxmc_inp_rocon_is_index_edge(mcs);
2018
2019   res=pxmc_pthalalign(mcs, r2acq, spd, fin_fnc);
2020
2021   return res;
2022 }
2023
2024 int pxmc_axis_rdmode(pxmc_state_t *mcs)
2025 {
2026   if (mcs->pxms_do_out == pxmc_rocon_pwm2ph_out)
2027     return PXMC_AXIS_MODE_STEPPER_WITH_IRC;
2028   if (mcs->pxms_do_out == pxmc_rocon_pwm_dc_out)
2029     return PXMC_AXIS_MODE_DC;
2030   if (mcs->pxms_do_out == pxmc_rocon_pwm3ph_out)
2031     return PXMC_AXIS_MODE_BLDC;
2032   if (mcs->pxms_do_out == pxmc_pxmcc_pwm3ph_out)
2033     return PXMC_AXIS_MODE_BLDC_PXMCC;
2034   if (mcs->pxms_do_out == pxmc_pxmcc_pwm2ph_out)
2035     return PXMC_AXIS_MODE_STEPPER_WITH_IRC_PXMCC;
2036   if (mcs->pxms_do_out == pxmc_pxmcc_nofb2ph_out)
2037     return PXMC_AXIS_MODE_STEPPER_PXMCC;
2038   return -1;
2039 }
2040
2041
2042 int
2043 pxmc_axis_pt4mode(pxmc_state_t *mcs, int mode)
2044 {
2045   static const typeof(*mcs->pxms_ptptr1) dummy0 = 0;
2046   int res = 0;
2047
2048   if (mode == PXMC_AXIS_MODE_NOCHANGE)
2049     mode = pxmc_axis_rdmode(mcs);
2050   if (mode < 0)
2051     return -1;
2052
2053   switch (mode) {
2054     /* case PXMC_AXIS_MODE_STEPPER: */
2055     case PXMC_AXIS_MODE_STEPPER_PXMCC:
2056     case PXMC_AXIS_MODE_STEPPER_WITH_IRC:
2057     case PXMC_AXIS_MODE_STEPPER_WITH_IRC_PXMCC:
2058       res = pxmc_ptable_set_profile(mcs, &pxmc_ptprofile_sin, 0, 0);
2059       break;
2060     /*case PXMC_AXIS_MODE_STEPPER_WITH_PWM:*/
2061     case PXMC_AXIS_MODE_DC:
2062       /*profive some sane dummy values*/
2063       mcs->pxms_ptptr1 = (typeof(mcs->pxms_ptptr1))&dummy0;
2064       mcs->pxms_ptptr2 = (typeof(mcs->pxms_ptptr1))&dummy0;
2065       mcs->pxms_ptptr3 = (typeof(mcs->pxms_ptptr1))&dummy0;
2066
2067       mcs->pxms_ptscale_mult=1;
2068       mcs->pxms_ptscale_shift=15;
2069       break;
2070     case PXMC_AXIS_MODE_BLDC:
2071     case PXMC_AXIS_MODE_BLDC_PXMCC:
2072       /* res = pxmc_init_ptable(mcs, PXMC_PTPROF_SIN3FUP); */
2073      #ifndef PXMC_WITH_PT_ZIC
2074       res = pxmc_ptable_set_profile(mcs, &pxmc_ptprofile_sin3phup, 0, 0);
2075      #else /*PXMC_WITH_PT_ZIC*/
2076       res = pxmc_ptable_set_profile(mcs, &pxmc_ptprofile_sin3phup_zic, 0, 0);
2077      #endif /*PXMC_WITH_PT_ZIC*/
2078       break;
2079     default:
2080       return -1;
2081   }
2082
2083   mcs->pxms_ptvang = pxmc_ptvang_deg2irc(mcs, 90);
2084
2085   return res;
2086 }
2087
2088 /**
2089  * pxmc_axis_mode - Sets axis mode.[extern API]
2090  * @mcs:        Motion controller state information
2091  * @mode:       0 .. previous mode, 1 .. stepper motor mode,
2092  *              2 .. stepper motor with IRC feedback and PWM ,
2093  *              3 .. stepper motor with PWM control
2094  *              4 .. DC motor with IRC feedback and PWM
2095  *
2096  */
2097 int
2098 pxmc_axis_mode(pxmc_state_t *mcs, int mode)
2099 {
2100   int res;
2101   int prev_mode;
2102
2103   pxmc_set_const_out(mcs, 0);
2104   pxmc_clear_flag(mcs, PXMS_ENI_b);
2105   pxmc_clear_flags(mcs,PXMS_ENO_m);
2106   /* Clear possible stall index flags from hardware */
2107   pxmc_inp_rocon_is_index_edge(mcs);
2108   pxmc_clear_flag(mcs, PXMS_PHA_b);
2109   pxmc_clear_flag(mcs, PXMS_PTI_b);
2110
2111
2112   prev_mode = pxmc_axis_rdmode(mcs);
2113
2114   if (mode == PXMC_AXIS_MODE_NOCHANGE)
2115     mode = prev_mode;
2116   if (mode < 0)
2117     return -1;
2118   if (!mode)
2119     mode = PXMC_AXIS_MODE_DC;
2120
2121   if ((prev_mode == PXMC_AXIS_MODE_BLDC_PXMCC) ||
2122       (prev_mode == PXMCC_MODE_STEPPER_WITH_IRC))
2123     pxmcc_axis_setup(mcs, PXMCC_MODE_IDLE);
2124
2125   res = pxmc_axis_pt4mode(mcs, mode);
2126   if (res < 0)
2127     return -1;
2128
2129   if (mcs->pxms_do_inp == pxmc_pxmcc_nofb_inp)
2130     mcs->pxms_do_inp = pxmc_inp_rocon_inp;
2131   if (mcs->pxms_do_con == pxmc_pxmcc_nofb_con)
2132     mcs->pxms_do_con = pxmc_pid_con;
2133
2134   switch (mode) {
2135     /*case PXMC_AXIS_MODE_STEPPER:*/
2136     case PXMC_AXIS_MODE_STEPPER_WITH_IRC:
2137       mcs->pxms_do_out = pxmc_rocon_pwm2ph_out;
2138       break;
2139     /*case PXMC_AXIS_MODE_STEPPER_WITH_PWM:*/
2140     case PXMC_AXIS_MODE_DC:
2141       mcs->pxms_do_out = pxmc_rocon_pwm_dc_out;
2142       break;
2143     case PXMC_AXIS_MODE_BLDC:
2144       mcs->pxms_do_out = pxmc_rocon_pwm3ph_out;
2145       break;
2146     case PXMC_AXIS_MODE_BLDC_PXMCC:
2147       if (pxmcc_axis_setup(mcs, PXMCC_MODE_BLDC) < 0)
2148         return -1;
2149       pxmcc_axis_enable(mcs, 1);
2150       mcs->pxms_do_out = pxmc_pxmcc_pwm3ph_out;
2151       break;
2152     case PXMC_AXIS_MODE_STEPPER_WITH_IRC_PXMCC:
2153       if (pxmcc_axis_setup(mcs, PXMCC_MODE_STEPPER_WITH_IRC) < 0)
2154         return -1;
2155       pxmcc_axis_enable(mcs, 1);
2156       mcs->pxms_do_out = pxmc_pxmcc_pwm2ph_out;
2157       break;
2158     case PXMC_AXIS_MODE_STEPPER_PXMCC:
2159       if (pxmcc_axis_setup(mcs, PXMCC_MODE_STEPPER) < 0)
2160         return -1;
2161       pxmcc_axis_enable(mcs, 1);
2162       mcs->pxms_do_inp = pxmc_pxmcc_nofb_inp;
2163       mcs->pxms_do_con = pxmc_pxmcc_nofb_con;
2164       mcs->pxms_do_out = pxmc_pxmcc_nofb2ph_out;
2165       break;
2166     default:
2167       return -1;
2168   }
2169
2170   /* Clear possible stall index flags from hardware */
2171   pxmc_inp_rocon_is_index_edge(mcs);
2172   /* Request new phases alignment for changed parameters */
2173   pxmc_clear_flag(mcs, PXMS_PHA_b);
2174   pxmc_clear_flag(mcs, PXMS_PTI_b);
2175   pxmc_set_flag(mcs, PXMS_ENI_b);
2176   return res;
2177 }
2178
2179 void pxmc_sfi_isr(void)
2180 {
2181   unsigned long spent = pxmc_fast_tick_time();
2182
2183   pxmc_sfi_input();
2184   pxmc_sfi_controller_and_output();
2185   pxmc_sfi_generator();
2186   pxmc_sfi_dbg();
2187   /* Kick LX Master watchdog */
2188   if (pxmc_main_list.pxml_cnt != 0)
2189     *fpga_lx_master_transmitter_wdog = 1;
2190
2191   spent = pxmc_fast_tick_time() - spent;
2192
2193   if(spent > pxmc_sfi_spent_time_max)
2194     pxmc_sfi_spent_time_max = spent;
2195
2196 }
2197
2198 int pxmc_clear_power_stop(void)
2199 {
2200   return 0;
2201 }
2202
2203 int pxmc_process_state_check(unsigned long *pbusy_bits,
2204                              unsigned long *perror_bits)
2205 {
2206   unsigned short flg;
2207   unsigned short chan;
2208   unsigned long busy_bits = 0;
2209   unsigned long error_bits = 0;
2210   pxmc_state_t *mcs;
2211   flg=0;
2212   pxmc_for_each_mcs(chan, mcs) {
2213     if(mcs) {
2214       flg |= mcs->pxms_flg;
2215       if (mcs->pxms_flg & PXMS_BSY_m)
2216         busy_bits |= 1 << chan;
2217       if (mcs->pxms_flg & PXMS_ERR_m)
2218         error_bits |= 1 << chan;
2219     }
2220   }
2221   if (appl_errstop_mode) {
2222     if((flg & PXMS_ENG_m) && (flg & PXMS_ERR_m)) {
2223       pxmc_for_each_mcs(chan, mcs) {
2224         if(mcs&&(mcs->pxms_flg & PXMS_ENG_m)) {
2225           pxmc_stop(mcs, 0);
2226         }
2227       }
2228     }
2229   }
2230
2231   if (pbusy_bits != NULL)
2232     *pbusy_bits = busy_bits;
2233   if (error_bits != NULL)
2234     *perror_bits = error_bits;
2235
2236   return flg;
2237 }
2238
2239 int pxmc_done(void)
2240 {
2241   int var;
2242   pxmc_state_t *mcs;
2243
2244   if (!pxmc_main_list.pxml_cnt)
2245     return 0;
2246
2247   pxmc_for_each_mcs(var, mcs)
2248   {
2249     pxmc_set_const_out(mcs,0);
2250   }
2251
2252   pxmc_main_list.pxml_cnt = 0;
2253   __memory_barrier();
2254
2255   return 0;
2256 }
2257
2258 int pxmc_initialize(void)
2259 {
2260   int res;
2261   int i;
2262
2263   pxmc_state_t *mcs = &mcs0.base;
2264   lpc_qei_state_t *qst = &lpc_qei_state;
2265
2266   *fpga_irc_reset = 1;
2267
2268   for (i = 0; i < 8; i++) {
2269     fpga_irc[i]->count = 0;
2270     fpga_irc[i]->count_index = 0;
2271     *fpga_irc_state[i] = FPGA_IRC_STATE_INDEX_EVENT_MASK;
2272   }
2273
2274   /* Initialize QEI module for IRC counting */
2275   pxmc_inp_lpc_qei_init(mcs->pxms_inp_info);
2276
2277   /* Initialize QEI events processing */
2278   if (lpc_qei_setup_irq(qst) < 0)
2279     return -1;
2280
2281   *fpga_irc_reset = 0;
2282
2283   //res = pxmc_axis_pt4mode(mcs, PXMC_AXIS_MODE_BLDC);
2284
2285   /*pxmc_ctm4pwm3f_wr(mcs, 0, 0, 0);*/
2286   //pxmc_rocon_pwm3ph_wr(mcs, 0, 0, 0);
2287
2288   pxmc_rocon_pwm_master_init();
2289  #ifdef PXMC_ROCON_TIMED_BY_RX_DONE
2290   pxmc_rocon_rx_done_isr_setup(pxmc_rocon_rx_done_isr);
2291  #endif /*PXMC_ROCON_TIMED_BY_RX_DONE*/
2292
2293   pxmc_main_list.pxml_cnt = 0;
2294   pxmc_dbg_hist = NULL;
2295   __memory_barrier();
2296   pxmc_main_list.pxml_cnt = PXML_MAIN_CNT;
2297
2298   return 0;
2299 }