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       !(mcs->pxms_flg & (PXMS_ENO_m | PXMS_ENR_m))) {
 987     pxmc_rocon_pwm2ph_wr(mcs, 0, 0);
 988     pxmcc_axis_pwm_dq_out(mcs, 0, 0);
 989     mcc_axis->steps_inc = 0;
 990     mcsrc->cur_d_err_sum = 0;
 991     mcsrc->cur_q_err_sum = 0;
 992   } else {
 993     int pwm_d, pwm_q;
 994     int cur_d, cur_q;
 995     int cur_d_req, cur_d_err;
 996     int cur_q_req, cur_q_err;
 997     int max_pwm = mcs->pxms_me;
 998     int32_t stpinc;
 999
1000     pxmcc_axis_cur_dq_raw2filt(&cur_d, cur_d_raw);
1001     pxmcc_axis_cur_dq_raw2filt(&cur_q, cur_q_raw);
1002
1003     if (!mcs->pxms_ene)
1004       cur_d_req = 0;
1005     else
1006       cur_d_req = mcsrc->cur_hold;
1007
1008     cur_d_err = cur_d_req - cur_d;
1009
1010     pxmcc_cur_ctrl_pi(&pwm_d, &mcsrc->cur_d_err_sum, cur_d_err,
1011                       mcsrc->cur_d_p, mcsrc->cur_d_i, max_pwm);
1012
1013     /* pwm_d = (pxmc_rocon_pwm_magnitude * ene) >> 15; */
1014
1015     cur_q_req = 0;
1016
1017     cur_q_err = cur_q_req - cur_q;
1018
1019     pxmcc_cur_ctrl_pi(&pwm_q, &mcsrc->cur_q_err_sum, cur_q_err,
1020                       mcsrc->cur_q_p, mcsrc->cur_q_i, max_pwm);
1021
1022     pxmcc_axis_pwm_dq_out(mcs, pwm_d, pwm_q);
1023
1024     mcs->pxms_ap=mcs->pxms_rp;
1025     mcs->pxms_as=mcs->pxms_rs;
1026     mcc_axis->steps_lim = mcc_axis->steps_cnt + 6;
1027
1028     stpinc = mcs->pxms_rs;
1029     mcc_axis->steps_inc = stpinc;
1030
1031       /* stpinc /= (mcs->pxms_ptirc << PXMC_SUBDIV(mcs)); */
1032       /* pxms_ptscale_mult; pxms_ptscale_shift; */
1033   }
1034
1035   return 0;
1036 }
1037
1038 int pxmcc_axis_setup(pxmc_state_t *mcs, int mode)
1039 {
1040   volatile pxmcc_data_t *mcc_data = pxmc_rocon_mcc_data();
1041   volatile pxmcc_axis_data_t *mcc_axis = pxmc_rocon_mcs2pxmcc(mcs);
1042   uint32_t ptirc;
1043   uint32_t ptreci;
1044   uint32_t inp_info;
1045   uint32_t pwmtx_info;
1046   uint32_t pwmtx_info_dummy = 27;
1047   uint64_t ull;
1048   int      i;
1049   int      phcnt = 0;
1050   int      pwm_chan;
1051
1052   if (mcc_axis == NULL)
1053     return -1;
1054
1055   if (mcc_data->common.fwversion != PXMCC_FWVERSION)
1056     return -1;
1057
1058   mcc_axis->ccflg = 0;
1059   mcc_axis->mode = PXMCC_MODE_IDLE;
1060
1061   mcc_data->common.pwm_cycle = PXMC_LXPWR_PWM_CYCLE;
1062
1063   ptirc = mcs->pxms_ptirc;
1064   ull = (1ULL << 32) * mcs->pxms_ptper;
1065   ptreci = (ull + ptirc / 2) / ptirc;
1066
1067   mcc_axis->ptreci = ptreci;
1068
1069   pxmcc_pxmc_ptofs2mcc(mcs, 0);
1070
1071   inp_info = (char*)&fpga_irc[mcs->pxms_inp_info]->count - (char*)fpga_irc[0];
1072   inp_info += mcc_data->common.irc_base;
1073
1074   switch (mode) {
1075     case PXMCC_MODE_IDLE:
1076       phcnt = 0;
1077       break;
1078     case PXMCC_MODE_BLDC:
1079       phcnt = 3;
1080       break;
1081     case PXMCC_MODE_STEPPER_WITH_IRC:
1082       phcnt = 4;
1083       break;
1084     case PXMCC_MODE_STEPPER:
1085       phcnt = 4;
1086       mcc_axis->ptreci = 1;
1087       inp_info = (char*)&mcc_axis->steps_pos - (char*)mcc_data;
1088       break;
1089   }
1090
1091   mcc_axis->inp_info = inp_info;
1092   mcc_axis->out_info = mcs->pxms_out_info;
1093
1094   pwm_chan = mcs->pxms_out_info;
1095
1096   pwmtx_info = (pwmtx_info_dummy << 0) | (pwmtx_info_dummy << 8) |
1097                (pwmtx_info_dummy << 16) | (pwmtx_info_dummy << 24);
1098
1099   for (i = phcnt; --i >= 0; ) {
1100     volatile uint32_t *pwm_reg;
1101     volatile uint32_t *pwm_reg_base = fpga_lx_master_transmitter_base;
1102
1103     pwmtx_info <<= 8;
1104
1105     pwm_reg = pxmc_rocon_pwm_chan2reg(pwm_chan + i);
1106     if (pwm_reg == &pxmc_rocon_pwm_dummy_reg) {
1107       pwmtx_info |= pwmtx_info_dummy;
1108     } else {
1109       pwmtx_info |= pwm_reg - pwm_reg_base;
1110     }
1111   }
1112
1113   mcc_axis->pwmtx_info = pwmtx_info;
1114
1115   mcc_axis->mode = mode;
1116
1117   mcc_axis->ccflg = 0;
1118   mcc_axis->pwm_dq = 0;
1119   mcc_axis->steps_lim = mcc_axis->steps_cnt;
1120   mcc_axis->steps_inc = 0;
1121   mcc_axis->steps_pos = 0;
1122
1123   if (mode != PXMCC_MODE_STEPPER) {
1124     pxmcc_pxmc_ptofs2mcc(mcs, 1);
1125   }
1126   return 0;
1127 }
1128
1129 /*******************************************************************/
1130
1131 volatile void *pxmc_rocon_rx_data_hist_buff;
1132 volatile void *pxmc_rocon_rx_data_hist_buff_end;
1133 int pxmc_rocon_rx_data_hist_mode;
1134
1135 uint32_t pxmc_rocon_rx_last_irq;
1136 uint32_t pxmc_rocon_rx_cycle_time;
1137 uint32_t pxmc_rocon_rx_irq_latency;
1138 uint32_t pxmc_rocon_rx_irq_latency_max;
1139
1140 IRQ_HANDLER_FNC(pxmc_rocon_rx_done_isr)
1141 {
1142   uint32_t ir;
1143
1144   ir = ROCON_RX_TIM->IR & LPC_TIM_IR_ALL_m;
1145   ROCON_RX_TIM->IR = ir;
1146   if (ir & LPC_TIM_IR_CR1INT_m) {
1147     uint32_t cr0, cr1;
1148     cr0 = ROCON_RX_TIM->CR0;
1149     cr1 = ROCON_RX_TIM->CR1;
1150
1151     pxmc_rocon_rx_cycle_time = cr1 - pxmc_rocon_rx_last_irq;
1152     pxmc_rocon_rx_last_irq = cr1;
1153
1154     hal_gpio_set_value(T2MAT0_PIN, 1);
1155     hal_gpio_set_value(T2MAT1_PIN, 0);
1156     hal_gpio_set_value(T2MAT0_PIN, 0);
1157
1158     if (pxmc_rocon_rx_data_hist_buff >= pxmc_rocon_rx_data_hist_buff_end)
1159       pxmc_rocon_rx_data_hist_buff = NULL;
1160
1161     if (pxmc_rocon_rx_data_hist_buff != NULL) {
1162       if (pxmc_rocon_rx_data_hist_mode == 0) {
1163         int i;
1164         volatile uint32_t *pwm_reg = fpga_lx_master_transmitter_base + 8;
1165         volatile uint32_t *rec_reg = fpga_lx_master_receiver_base + 8;
1166         uint16_t *pbuf = (uint16_t *)pxmc_rocon_rx_data_hist_buff;
1167         for (i = 0; i < 8; i++) {
1168           *(pbuf++) = *(rec_reg++);
1169         }
1170         for (i = 0; i < 8; i++) {
1171           *(pbuf++) = *(pwm_reg++);
1172         }
1173         pxmc_rocon_rx_data_hist_buff = pbuf;
1174       } else if (pxmc_rocon_rx_data_hist_mode == 1) {
1175         int i;
1176         uint32_t *pbuf = (uint32_t *)pxmc_rocon_rx_data_hist_buff;
1177         pxmcc_data_t *mcc_data = pxmc_rocon_mcc_data();
1178         pxmcc_axis_data_t *mcc_axis = mcc_data->axis + 0;
1179         uint32_t *ptumbl = (uint32_t *)mcc_axis;
1180
1181         for (i = 0; i < 16; i++)
1182           *(pbuf++) = *(ptumbl++);
1183
1184         pxmc_rocon_rx_data_hist_buff = pbuf;
1185       }
1186     }
1187
1188     pxmc_rocon_rx_irq_latency = ROCON_RX_TIM->TC - cr1;
1189     if (pxmc_rocon_rx_irq_latency > pxmc_rocon_rx_irq_latency_max)
1190       pxmc_rocon_rx_irq_latency_max = pxmc_rocon_rx_irq_latency;
1191
1192    #ifdef PXMC_ROCON_TIMED_BY_RX_DONE
1193     pxmc_sfi_isr();
1194     do_pxmc_coordmv();
1195    #endif /*PXMC_ROCON_TIMED_BY_RX_DONE*/
1196   }
1197
1198   return IRQ_HANDLED;
1199 }
1200
1201 int
1202 pxmc_rocon_rx_done_isr_setup(irq_handler_t rx_done_isr_handler)
1203 {
1204
1205   disable_irq(ROCON_RX_IRQn);
1206
1207   hal_pin_conf_set(T2MAT0_PIN, PORT_CONF_GPIO_OUT_LO);
1208   hal_pin_conf_set(T2MAT1_PIN, PORT_CONF_GPIO_OUT_LO);
1209   hal_pin_conf(T2CAP0_PIN);
1210   hal_pin_conf(T2CAP1_PIN);
1211
1212   hal_gpio_direction_output(T2MAT0_PIN, 1);
1213   hal_gpio_direction_output(T2MAT1_PIN, 0);
1214   hal_gpio_set_value(T2MAT0_PIN, 0);
1215
1216   /* Enable CLKOUT pin function, source CCLK, divide by 1 */
1217   LPC_SC->CLKOUTCFG = 0x0100;
1218
1219   request_irq(ROCON_RX_IRQn, rx_done_isr_handler, 0, NULL,NULL);
1220
1221   ROCON_RX_TIM->TCR = 0;
1222   ROCON_RX_TIM->CTCR = 0;
1223   ROCON_RX_TIM->PR = 0; /* Divide by 1 */
1224
1225   ROCON_RX_TIM->CCR = LPC_TIM_CCR_CAP0RE_m | LPC_TIM_CCR_CAP1FE_m |
1226                    LPC_TIM_CCR_CAP1I_m;
1227
1228   ROCON_RX_TIM->EMR = __val2mfld(LPC_TIM_EMR_EMC0_m, LPC_TIM_EMR_NOP) |
1229                    __val2mfld(LPC_TIM_EMR_EMC1_m, LPC_TIM_EMR_NOP);
1230
1231   ROCON_RX_TIM->MCR = 0;                        /* No IRQ on MRx */
1232   ROCON_RX_TIM->TCR = LPC_TIM_TCR_CEN_m;        /* Enable timer counting */
1233   enable_irq(ROCON_RX_IRQn);            /* Enable interrupt */
1234
1235   return 0;
1236
1237 }
1238
1239 int
1240 pxmc_rocon_pwm_master_init(void)
1241 {
1242   int i;
1243   int grp_in = 0;
1244   int grp_out = 0;
1245   unsigned word_slot;
1246   unsigned receiver_done_div = 1;
1247  #ifdef LXPWR_WITH_SIROLADC
1248   unsigned lxpwr_header = 1;
1249   unsigned lxpwr_words = 1 + 8 * 2 + 2;
1250   unsigned lxpwr_chips = 2;
1251   unsigned lxpwr_chip_pwm_cnt = 8;
1252  #else /*LXPWR_WITH_SIROLADC*/
1253   unsigned lxpwr_header = 0;
1254   unsigned lxpwr_words = 8;
1255   unsigned lxpwr_chips = 2;
1256   unsigned lxpwr_chip_pwm_cnt = 8;
1257  #endif /*LXPWR_WITH_SIROLADC*/
1258
1259  #ifdef PXMC_ROCON_TIMED_BY_RX_DONE
1260   receiver_done_div = 5;
1261  #endif /*PXMC_ROCON_TIMED_BY_RX_DONE*/
1262
1263   *fpga_lx_master_reset = 1;
1264   *fpga_lx_master_transmitter_reg = 0;
1265   *fpga_lx_master_transmitter_cycle = PXMC_LXPWR_PWM_CYCLE; /* 50 MHz -> 20 kHz */
1266   *fpga_lx_master_receiver_done_div = receiver_done_div << 8;
1267
1268   for (i = 0; i < LX_MASTER_DATA_OFFS + lxpwr_words * lxpwr_chips; i++)
1269     fpga_lx_master_receiver_base[i] = 0;
1270
1271   word_slot = LX_MASTER_DATA_OFFS;
1272   fpga_lx_master_receiver_base[grp_in++] = (word_slot << 8) | lxpwr_words;
1273   fpga_lx_master_receiver_base[grp_in++] = 0x0000;
1274
1275   for (i = 0; i < LX_MASTER_DATA_OFFS + lxpwr_words * lxpwr_chips; i++)
1276     fpga_lx_master_transmitter_base[i] = 0;
1277
1278   word_slot = LX_MASTER_DATA_OFFS + lxpwr_header + lxpwr_chip_pwm_cnt;
1279   fpga_lx_master_transmitter_base[grp_out++] = (word_slot << 8) | lxpwr_words;
1280  #ifdef LXPWR_WITH_SIROLADC
1281   fpga_lx_master_transmitter_base[word_slot] = 0xc100 | (lxpwr_words - 1);
1282  #endif /*LXPWR_WITH_SIROLADC*/
1283
1284   word_slot = LX_MASTER_DATA_OFFS + 0;
1285   fpga_lx_master_transmitter_base[grp_out++] = (word_slot << 8) | lxpwr_words;
1286  #ifdef LXPWR_WITH_SIROLADC
1287   fpga_lx_master_transmitter_base[word_slot] = 0xc100 | (lxpwr_words - 1);
1288  #endif /*LXPWR_WITH_SIROLADC*/
1289
1290   fpga_lx_master_transmitter_base[grp_out++] = 0x0000;
1291
1292   *fpga_lx_master_reset = 0;
1293   *fpga_lx_master_transmitter_cycle = PXMC_LXPWR_PWM_CYCLE; /* 50 MHz -> 20 kHz */
1294   *fpga_lx_master_receiver_done_div = receiver_done_div << 8;
1295
1296   return 0;
1297 }
1298
1299 int pxmc_ptofs_from_index(pxmc_state_t *mcs, unsigned long irc,
1300                            unsigned long index_irc, int diff2err)
1301 {
1302   long ofsl;
1303   short ofs;
1304
1305   ofs = ofsl = index_irc - mcs->pxms_ptmark;
1306
1307   if (diff2err) {
1308     short diff;
1309     diff = (unsigned short)ofs - (unsigned short)mcs->pxms_ptofs;
1310     if (diff >= mcs->pxms_ptirc / 2)
1311       diff -= mcs->pxms_ptirc;
1312     if (diff <= -mcs->pxms_ptirc / 2)
1313       diff += mcs->pxms_ptirc;
1314     if (diff < 0)
1315       diff = -diff;
1316     if(diff >= mcs->pxms_ptirc / 6) {
1317       return -1;
1318     }
1319   } else {
1320     long diff;
1321     diff = (unsigned long)ofsl - irc;
1322     ofs = ofsl - (diff / mcs->pxms_ptirc) * mcs->pxms_ptirc;
1323   }
1324
1325   mcs->pxms_ptofs = ofs;
1326
1327   return 1;
1328 }
1329
1330 /**
1331  * pxmc_dummy_con - Dummy controller for synchronous BLDC/PMSM/steper drive
1332  * @mcs:        Motion controller state information
1333  */
1334 int
1335 pxmc_dummy_con(pxmc_state_t *mcs)
1336 {
1337   return 0;
1338 }
1339
1340 int pxmc_rocon_hh_gd10(pxmc_state_t *mcs);
1341 int pxmc_rocon_hh_gd20(pxmc_state_t *mcs);
1342 int pxmc_rocon_hh_gd30(pxmc_state_t *mcs);
1343 int pxmc_rocon_hh_gd40(pxmc_state_t *mcs);
1344 int pxmc_rocon_hh_gd50(pxmc_state_t *mcs);
1345 int pxmc_rocon_hh_gd90(pxmc_state_t *mcs);
1346
1347 /* initialize for hard home */
1348 int
1349 pxmc_rocon_hh_gi(pxmc_state_t *mcs)
1350 {
1351   pxmc_set_flag(mcs,PXMS_BSY_b);
1352  #if 0 /* config and speed already set in pxmc_hh */
1353   spd = mcs->pxms_ms;
1354   spd >>= mcs->pxms_cfg & PXMS_CFG_HSPD_m;
1355   if(!spd) spd = 1;
1356   if(mcs->pxms_cfg & PXMS_CFG_HDIR_m)
1357     spd = -spd;
1358   mcs->pxms_gen_hsp = spd;
1359  #endif
1360   mcs->pxms_rsfg = 0;
1361   mcs->pxms_gen_htim = 16;
1362   mcs->pxms_do_gen = pxmc_rocon_hh_gd10;
1363   return pxmc_rocon_hh_gd10(mcs);
1364 }
1365
1366 /* fill initial mark filter and then decide */
1367 int
1368 pxmc_rocon_hh_gd10(pxmc_state_t *mcs)
1369 {
1370   int mark;
1371
1372   if(mcs->pxms_flg & PXMS_ERR_m)
1373     return pxmc_spdnext_gend(mcs);
1374
1375   pxmc_spd_gacc(&(mcs->pxms_rs), 0, mcs->pxms_ma);
1376   mcs->pxms_rp += mcs->pxms_rs;
1377
1378   mark = pxmc_inp_rocon_is_mark(mcs);
1379
1380   if (mcs->pxms_gen_htim) {
1381     mcs->pxms_gen_htim--;
1382     return 0;
1383   }
1384
1385   if (!(mcs->pxms_cfg & PXMS_CFG_HLS_m)) {
1386     /* limit switch is not used */
1387     pxmc_inp_rocon_is_index_edge(mcs);
1388     mcs->pxms_do_gen = pxmc_rocon_hh_gd50;
1389     return 0;
1390   }
1391
1392   if (mark >= 6) {
1393     /* go out from switch */
1394     mcs->pxms_do_gen = pxmc_rocon_hh_gd20;
1395   } else {
1396     /* switch is off -> look for it */
1397     mcs->pxms_do_gen = pxmc_rocon_hh_gd30;
1398   }
1399   return 0;
1400 }
1401
1402 /* go out from switch */
1403 int
1404 pxmc_rocon_hh_gd20(pxmc_state_t *mcs)
1405 {
1406   int mark;
1407
1408   if(mcs->pxms_flg & PXMS_ERR_m)
1409     return pxmc_spdnext_gend(mcs);
1410
1411   mark = pxmc_inp_rocon_is_mark(mcs);
1412
1413   if (mark <= 1) {
1414     /* switch is off -> look for it again */
1415     mcs->pxms_do_gen = pxmc_rocon_hh_gd30;
1416   }
1417
1418   pxmc_spd_gacc(&(mcs->pxms_rs), -mcs->pxms_gen_hsp, mcs->pxms_ma);
1419   mcs->pxms_rp += mcs->pxms_rs;
1420
1421   return 0;
1422 }
1423
1424 /* switch is off -> look for it */
1425 int
1426 pxmc_rocon_hh_gd30(pxmc_state_t *mcs)
1427 {
1428   long spd;
1429   int mark;
1430
1431   if (mcs->pxms_flg & PXMS_ERR_m)
1432     return pxmc_spdnext_gend(mcs);
1433
1434   pxmc_spd_gacc(&(mcs->pxms_rs), mcs->pxms_gen_hsp, mcs->pxms_ma);
1435   mcs->pxms_rp += mcs->pxms_rs;
1436
1437   mark = pxmc_inp_rocon_is_mark(mcs);
1438
1439   if (mark >= 8){
1440     spd = mcs->pxms_gen_hsp >> 2; /* slow down */
1441     if (!spd)
1442       spd = 1;
1443     mcs->pxms_gen_hsp = spd;
1444     mcs->pxms_do_gen = pxmc_rocon_hh_gd40;
1445   }
1446
1447   return 0;
1448 }
1449
1450 /* switch is on -> find edge */
1451 int
1452 pxmc_rocon_hh_gd40(pxmc_state_t *mcs)
1453 {
1454   int mark;
1455
1456   if (mcs->pxms_flg & PXMS_ERR_m)
1457     return pxmc_spdnext_gend(mcs);
1458
1459   mark = pxmc_inp_rocon_is_mark(mcs);
1460
1461   if (mark <= 1) {
1462     if (mcs->pxms_cfg & PXMS_CFG_HRI_m) {
1463       pxmc_inp_rocon_is_index_edge(mcs);
1464       mcs->pxms_do_gen = pxmc_rocon_hh_gd50;
1465     } else {
1466       pxmc_inp_rocon_ap_zero(mcs);
1467       mcs->pxms_do_gen = pxmc_stop_gi;
1468     }
1469   }
1470
1471   pxmc_spd_gacc(&(mcs->pxms_rs), -mcs->pxms_gen_hsp, mcs->pxms_ma);
1472   mcs->pxms_rp += mcs->pxms_rs;
1473
1474   return 0;
1475 }
1476
1477 /* calibrate on revolution index */
1478 int
1479 pxmc_rocon_hh_gd50(pxmc_state_t *mcs)
1480 {
1481   if (mcs->pxms_flg & PXMS_ERR_m)
1482     return pxmc_spdnext_gend(mcs);
1483
1484   pxmc_spd_gacc(&(mcs->pxms_rs), mcs->pxms_gen_hsp, mcs->pxms_ma);
1485   mcs->pxms_rp += mcs->pxms_rs;
1486
1487   if (pxmc_inp_rocon_is_index_edge(mcs)){
1488     pxmc_inp_rocon_irc_offset_from_index(mcs);
1489     mcs->pxms_do_gen = pxmc_stop_gi;
1490   }
1491
1492   return 0;
1493 }
1494
1495 pxmc_call_t *pxmc_get_hh_gi_4axis(pxmc_state_t *mcs)
1496 {
1497   return pxmc_rocon_hh_gi;
1498 }
1499
1500 pxmc_rocon_state_t mcs0 =
1501 {
1502 .base = {
1503 pxms_flg:
1504   PXMS_ENI_m,
1505 pxms_do_inp:
1506   pxmc_inp_rocon_inp,
1507 pxms_do_con:
1508   pxmc_pid_con /*pxmc_dummy_con*/,
1509 pxms_do_out:
1510   pxmc_rocon_pwm_dc_out /*pxmc_rocon_pwm3ph_out*/,
1511   pxms_do_deb: 0,
1512   pxms_do_gen: 0,
1513 pxms_do_ap2hw:
1514   pxmc_inp_rocon_ap2hw,
1515   pxms_ap: 0, pxms_as: 0,
1516   pxms_rp: 55 * 256, pxms_rs: 0, pxms_subdiv: 8,
1517   pxms_md: 800 << 8, pxms_ms: 500, pxms_ma: 10,
1518   pxms_inp_info: 0,
1519   pxms_out_info: 0,
1520   pxms_ene: 0, pxms_erc: 0,
1521   pxms_p: 80, pxms_i: 10, pxms_d: 200, pxms_s1: 200, pxms_s2: 0,
1522   pxms_me: 0x7e00/*0x7fff*/,
1523 pxms_cfg:
1524   PXMS_CFG_SMTH_m | PXMS_CFG_MD2E_m | PXMS_CFG_HLS_m | PXMS_CFG_HPS_m * 0 |
1525   PXMS_CFG_HRI_m * 0 | PXMS_CFG_HDIR_m * 0 |
1526   PXMS_CFG_I2PT_m * 0 | 0x2,
1527
1528   pxms_ptper: 1,
1529   pxms_ptirc: 1000,
1530   pxms_ptmark: 1180,
1531   /*pxms_ptamp: 0x7fff,*/
1532
1533   pxms_hal: 0x40,
1534 },
1535   .cur_d_p = 150,
1536   .cur_d_i = 6000,
1537   .cur_q_p = 150,
1538   .cur_q_i = 6000,
1539   .cur_hold = 200,
1540 };
1541
1542 pxmc_rocon_state_t mcs1 =
1543 {
1544 .base = {
1545 pxms_flg:
1546   PXMS_ENI_m,
1547 pxms_do_inp:
1548   pxmc_inp_rocon_inp,
1549 pxms_do_con:
1550   pxmc_pid_con,
1551 pxms_do_out:
1552   pxmc_rocon_pwm_dc_out,
1553   pxms_do_deb: 0,
1554   pxms_do_gen: 0,
1555 pxms_do_ap2hw:
1556   pxmc_inp_rocon_ap2hw,
1557   pxms_ap: 0, pxms_as: 0,
1558   pxms_rp: 55 * 256, pxms_rs: 0, pxms_subdiv: 8,
1559   pxms_md: 800 << 8, pxms_ms: 500, pxms_ma: 10,
1560   pxms_inp_info: 1,
1561   pxms_out_info: 2,
1562   pxms_ene: 0, pxms_erc: 0,
1563   pxms_p: 80, pxms_i: 10, pxms_d: 200, pxms_s1: 200, pxms_s2: 0,
1564   pxms_me: 0x7e00/*0x7fff*/,
1565 pxms_cfg:
1566   PXMS_CFG_SMTH_m | PXMS_CFG_MD2E_m | PXMS_CFG_HLS_m | PXMS_CFG_HPS_m * 0 |
1567   PXMS_CFG_HRI_m * 0 | PXMS_CFG_HDIR_m * 0 |
1568   PXMS_CFG_I2PT_m * 0 | 0x2,
1569
1570   pxms_ptper: 1,
1571   pxms_ptirc: 1000,
1572   /*pxms_ptamp: 0x7fff,*/
1573
1574   pxms_hal: 0x40,
1575 }};
1576
1577 pxmc_rocon_state_t mcs2 =
1578 {
1579 .base = {
1580 pxms_flg:
1581   PXMS_ENI_m,
1582 pxms_do_inp:
1583   pxmc_inp_rocon_inp,
1584 pxms_do_con:
1585   pxmc_pid_con,
1586 pxms_do_out:
1587   pxmc_rocon_pwm_dc_out,
1588   pxms_do_deb: 0,
1589   pxms_do_gen: 0,
1590 pxms_do_ap2hw:
1591   pxmc_inp_rocon_ap2hw,
1592   pxms_ap: 0, pxms_as: 0,
1593   pxms_rp: 55 * 256, pxms_rs: 0, pxms_subdiv: 8,
1594   pxms_md: 800 << 8, pxms_ms: 500, pxms_ma: 10,
1595   pxms_inp_info: 2,
1596   pxms_out_info: 4,
1597   pxms_ene: 0, pxms_erc: 0,
1598   pxms_p: 80, pxms_i: 10, pxms_d: 200, pxms_s1: 200, pxms_s2: 0,
1599   pxms_me: 0x7e00/*0x7fff*/,
1600 pxms_cfg:
1601   PXMS_CFG_SMTH_m | PXMS_CFG_MD2E_m | PXMS_CFG_HLS_m | PXMS_CFG_HPS_m * 0 |
1602   PXMS_CFG_I2PT_m * 0 | PXMS_CFG_HRI_m |
1603   PXMS_CFG_HDIR_m | 0x2,
1604
1605   pxms_ptper: 1,
1606   pxms_ptirc: 1000,
1607   /*pxms_ptamp: 0x7fff,*/
1608
1609   pxms_hal: 0x40,
1610 }};
1611
1612 pxmc_rocon_state_t mcs3 =
1613 {
1614 .base = {
1615 pxms_flg:
1616   PXMS_ENI_m,
1617 pxms_do_inp:
1618   pxmc_inp_rocon_inp,
1619 pxms_do_con:
1620   pxmc_pid_con,
1621 pxms_do_out:
1622   pxmc_rocon_pwm_dc_out,
1623   pxms_do_deb: 0,
1624   pxms_do_gen: 0,
1625 pxms_do_ap2hw:
1626   pxmc_inp_rocon_ap2hw,
1627   pxms_ap: 0, pxms_as: 0,
1628   pxms_rp: 55 * 256, pxms_rs: 0, pxms_subdiv: 8,
1629   pxms_md: 800 << 8, pxms_ms: 500, pxms_ma: 10,
1630   pxms_inp_info: 3,
1631   pxms_out_info: 6,
1632   pxms_ene: 0, pxms_erc: 0,
1633   pxms_p: 80, pxms_i: 10, pxms_d: 200, pxms_s1: 200, pxms_s2: 0,
1634   pxms_me: 0x7e00/*0x7fff*/,
1635 pxms_cfg:
1636   PXMS_CFG_SMTH_m | PXMS_CFG_MD2E_m | PXMS_CFG_HLS_m | PXMS_CFG_HPS_m * 0 |
1637   PXMS_CFG_I2PT_m * 0 | PXMS_CFG_HRI_m |
1638   PXMS_CFG_HDIR_m * 0 | 0x2,
1639
1640   pxms_ptper: 1,
1641   pxms_ptirc: 1000,
1642   /*pxms_ptamp: 0x7fff,*/
1643
1644   pxms_hal: 0x40,
1645 }};
1646
1647 pxmc_rocon_state_t mcs4 =
1648 {
1649 .base = {
1650 pxms_flg:
1651   PXMS_ENI_m,
1652 pxms_do_inp:
1653   pxmc_inp_rocon_inp,
1654 pxms_do_con:
1655   pxmc_pid_con,
1656 pxms_do_out:
1657   pxmc_rocon_pwm_dc_out,
1658   pxms_do_deb: 0,
1659   pxms_do_gen: 0,
1660 pxms_do_ap2hw:
1661   pxmc_inp_rocon_ap2hw,
1662   pxms_ap: 0, pxms_as: 0,
1663   pxms_rp: 55 * 256, pxms_rs: 0, pxms_subdiv: 8,
1664   pxms_md: 800 << 8, pxms_ms: 500, pxms_ma: 10,
1665   pxms_inp_info: 4,
1666   pxms_out_info: 8,
1667   pxms_ene: 0, pxms_erc: 0,
1668   pxms_p: 80, pxms_i: 10, pxms_d: 200, pxms_s1: 200, pxms_s2: 0,
1669   pxms_me: 0x7e00/*0x7fff*/,
1670 pxms_cfg:
1671   PXMS_CFG_SMTH_m | PXMS_CFG_MD2E_m | PXMS_CFG_HLS_m | PXMS_CFG_HPS_m * 0 |
1672   PXMS_CFG_HRI_m | PXMS_CFG_I2PT_m * 0 |
1673   PXMS_CFG_HDIR_m | 0x2,
1674
1675   pxms_ptper: 1,
1676   pxms_ptirc: 1000,
1677   /*pxms_ptamp: 0x7fff,*/
1678
1679   pxms_hal: 0x40,
1680 }};
1681
1682 pxmc_rocon_state_t mcs5 =
1683 {
1684 .base = {
1685 pxms_flg:
1686   PXMS_ENI_m,
1687 pxms_do_inp:
1688   pxmc_inp_rocon_inp,
1689 pxms_do_con:
1690   pxmc_pid_con,
1691 pxms_do_out:
1692   pxmc_rocon_pwm_dc_out,
1693   pxms_do_deb: 0,
1694   pxms_do_gen: 0,
1695 pxms_do_ap2hw:
1696   pxmc_inp_rocon_ap2hw,
1697   pxms_ap: 0, pxms_as: 0,
1698   pxms_rp: 55 * 256, pxms_rs: 0, pxms_subdiv: 8,
1699   pxms_md: 800 << 8, pxms_ms: 500, pxms_ma: 10,
1700   pxms_inp_info: 5,
1701   pxms_out_info: 10,
1702   pxms_ene: 0, pxms_erc: 0,
1703   pxms_p: 80, pxms_i: 10, pxms_d: 200, pxms_s1: 200, pxms_s2: 0,
1704   pxms_me: 0x7e00/*0x7fff*/,
1705 pxms_cfg:
1706   PXMS_CFG_SMTH_m | PXMS_CFG_MD2E_m | PXMS_CFG_HLS_m | PXMS_CFG_HPS_m |
1707   PXMS_CFG_HRI_m | PXMS_CFG_I2PT_m * 0 |
1708   PXMS_CFG_HDIR_m | 0x2,
1709
1710   pxms_ptper: 1,
1711   pxms_ptirc: 1000,
1712   /*pxms_ptamp: 0x7fff,*/
1713
1714   pxms_hal: 0x40,
1715 }};
1716
1717 pxmc_rocon_state_t mcs6 =
1718 {
1719 .base = {
1720 pxms_flg:
1721   PXMS_ENI_m,
1722 pxms_do_inp:
1723   pxmc_inp_rocon_inp,
1724 pxms_do_con:
1725   pxmc_pid_con,
1726 pxms_do_out:
1727   pxmc_rocon_pwm_dc_out,
1728   pxms_do_deb: 0,
1729   pxms_do_gen: 0,
1730 pxms_do_ap2hw:
1731   pxmc_inp_rocon_ap2hw,
1732   pxms_ap: 0, pxms_as: 0,
1733   pxms_rp: 55 * 256, pxms_rs: 0, pxms_subdiv: 8,
1734   pxms_md: 800 << 8, pxms_ms: 500, pxms_ma: 10,
1735   pxms_inp_info: 6,
1736   pxms_out_info: 12,
1737   pxms_ene: 0, pxms_erc: 0,
1738   pxms_p: 80, pxms_i: 10, pxms_d: 200, pxms_s1: 200, pxms_s2: 0,
1739   pxms_me: 0x7e00/*0x7fff*/,
1740 pxms_cfg:
1741   PXMS_CFG_SMTH_m | PXMS_CFG_MD2E_m | PXMS_CFG_HLS_m | PXMS_CFG_I2PT_m * 0 |
1742   0x1,
1743
1744   pxms_ptper: 1,
1745   pxms_ptirc: 1000,
1746   /*pxms_ptamp: 0x7fff,*/
1747
1748   pxms_hal: 0x40,
1749 }};
1750
1751 pxmc_rocon_state_t mcs7 =
1752 {
1753 .base = {
1754 pxms_flg:
1755   PXMS_ENI_m,
1756 pxms_do_inp:
1757   pxmc_inp_rocon_inp,
1758 pxms_do_con:
1759   pxmc_pid_con,
1760 pxms_do_out:
1761   pxmc_rocon_pwm_dc_out,
1762   pxms_do_deb: 0,
1763   pxms_do_gen: 0,
1764 pxms_do_ap2hw:
1765   pxmc_inp_rocon_ap2hw,
1766   pxms_ap: 0, pxms_as: 0,
1767   pxms_rp: 55 * 256, pxms_rs: 0, pxms_subdiv: 8,
1768   pxms_md: 800 << 8, pxms_ms: 500, pxms_ma: 10,
1769   pxms_inp_info: 7,
1770   pxms_out_info: 14,
1771   pxms_ene: 0, pxms_erc: 0,
1772   pxms_p: 80, pxms_i: 10, pxms_d: 200, pxms_s1: 200, pxms_s2: 0,
1773   pxms_me: 0x7e00/*0x7fff*/,
1774 pxms_cfg:
1775   PXMS_CFG_SMTH_m | PXMS_CFG_MD2E_m | PXMS_CFG_HLS_m | PXMS_CFG_I2PT_m * 0 |
1776   0x1,
1777
1778   pxms_ptper: 1,
1779   pxms_ptirc: 1000,
1780   /*pxms_ptamp: 0x7fff,*/
1781
1782   pxms_hal: 0x40,
1783 }};
1784
1785 pxmc_state_t *pxmc_main_arr[PXML_MAIN_CNT] =
1786 {&mcs0.base, &mcs1.base, &mcs2.base, &mcs3.base,
1787  &mcs4.base, &mcs5.base, &mcs6.base, &mcs7.base};
1788
1789
1790 pxmc_state_list_t  pxmc_main_list =
1791 {
1792 pxml_arr:
1793   pxmc_main_arr,
1794   pxml_cnt: 0
1795 };
1796
1797 void pxmc_lpc_qei_callback_index(struct lpc_qei_state_t *qst, void *context)
1798 {
1799   pxmc_state_t *mcs = (pxmc_state_t *)context;
1800   short ofs;
1801   short irc;
1802   irc = qst->index_pos;
1803
1804   if ((mcs->pxms_cfg & PXMS_CFG_I2PT_m) && (mcs->pxms_flg & PXMS_PTI_m))
1805   {
1806     short diff;
1807     ofs = irc - mcs->pxms_ptmark;
1808     diff = ofs - mcs->pxms_ptofs;
1809
1810     if (diff >= mcs->pxms_ptirc / 2)
1811       diff -= mcs->pxms_ptirc;
1812
1813     if (diff <= -mcs->pxms_ptirc / 2)
1814       diff += mcs->pxms_ptirc;
1815
1816     if (diff < 0)
1817       diff = -diff;
1818
1819     if (diff >= mcs->pxms_ptirc / 6)
1820     {
1821       pxmc_set_errno(mcs, PXMS_E_I2PT_TOOBIG);
1822     }
1823     else
1824     {
1825       mcs->pxms_ptofs = ofs;
1826       pxmc_set_flag(mcs, PXMS_PHA_b);
1827     }
1828   } /*else {
1829
1830     ofs=irc-mcs->pxms_ptofs;
1831     if((unsigned short)ofs>=(unsigned short)mcs->pxms_ptirc) {
1832       if(ofs>0) {
1833         ofs-=mcs->pxms_ptirc;
1834       } else {
1835         ofs+=mcs->pxms_ptirc;
1836       }
1837     }
1838     mcs->pxms_ptmark=ofs;
1839   }*/
1840
1841   /*lpc_qei_set_cmpos(qst, 0, qst->index_pos-4000);*/
1842 }
1843
1844 int pxmc_lpc_pthalalign_callback(pxmc_state_t *mcs)
1845 {
1846   short ptofs;
1847   short ptmark;
1848   lpc_qei_state_t *qst = &lpc_qei_state;
1849   int irc = lpc_qei_get_pos(qst);
1850   int idx_rel;
1851
1852   if (!qst->index_occ)
1853     return 0;
1854
1855   idx_rel = qst->index_pos - irc;
1856   idx_rel %= mcs->pxms_ptirc;
1857
1858   if (idx_rel < 0)
1859     idx_rel += mcs->pxms_ptirc;
1860
1861   ptofs = irc - mcs->pxms_ptofs;
1862   ptmark = ptofs + idx_rel;
1863
1864   if ((unsigned short)ptmark >= mcs->pxms_ptirc)
1865   {
1866     if (ptmark > 0)
1867       ptmark -= mcs->pxms_ptirc;
1868     else
1869       ptmark += mcs->pxms_ptirc;
1870   }
1871
1872   if ((unsigned short)ptmark < mcs->pxms_ptirc)
1873   {
1874     mcs->pxms_ptmark = ptmark;
1875   }
1876
1877   return 0;
1878 }
1879
1880 int pxmc_lpc_pthalalign(pxmc_state_t *mcs, int periods)
1881 {
1882   int res;
1883   long r2acq;
1884   long spd;
1885   pxmc_call_t *fin_fnc = pxmc_lpc_pthalalign_callback;
1886   lpc_qei_state_t *qst = &lpc_qei_state;
1887
1888   mcs->pxms_cfg &= ~PXMS_CFG_I2PT_m;
1889   lpc_qei_setup_index_catch(qst);
1890
1891   r2acq = ((long)mcs->pxms_ptirc << PXMC_SUBDIV(mcs)) * periods;
1892   spd = 1 << PXMC_SUBDIV(mcs);
1893
1894   res = pxmc_pthalalign(mcs, r2acq, spd, fin_fnc);
1895
1896   return res;
1897 }
1898
1899 int pxmc_lpc_pthalalign_run(void)
1900 {
1901   return pxmc_lpc_pthalalign(pxmc_main_list.pxml_arr[0], 20);
1902 }
1903
1904 static inline void pxmc_sfi_input(void)
1905 {
1906   int var;
1907   pxmc_state_t *mcs;
1908   pxmc_for_each_mcs(var, mcs)
1909   {
1910     /* PXMS_ENI_m - check if input (IRC) update is enabled */
1911     if (mcs->pxms_flg & PXMS_ENI_m)
1912     {
1913       pxmc_call(mcs, mcs->pxms_do_inp);
1914     }
1915   }
1916 }
1917
1918 static inline void pxmc_sfi_controller_and_output(void)
1919 {
1920   int var;
1921   pxmc_state_t *mcs;
1922   pxmc_for_each_mcs(var, mcs)
1923   {
1924     /* PXMS_ENR_m - check if controller is enabled */
1925     if (mcs->pxms_flg & PXMS_ENR_m || mcs->pxms_flg & PXMS_ENO_m)
1926     {
1927
1928       /* If output only is enabled, we skip the controller */
1929       if (mcs->pxms_flg & PXMS_ENR_m)
1930       {
1931
1932         pxmc_call(mcs, mcs->pxms_do_con);
1933
1934         /* PXMS_ERR_m - if axis in error state */
1935         if (mcs->pxms_flg & PXMS_ERR_m)
1936           mcs->pxms_ene = 0;
1937       }
1938
1939       /* for bushless motors, it is necessary to call do_out
1940         even if the controller is not enabled and PWM should be provided. */
1941       pxmc_call(mcs, mcs->pxms_do_out);
1942     }
1943   }
1944 }
1945
1946 static inline void pxmc_sfi_generator(void)
1947 {
1948   int var;
1949   pxmc_state_t *mcs;
1950   pxmc_for_each_mcs(var, mcs)
1951   {
1952     /* PXMS_ENG_m - check if requested value (position) generator is enabled */
1953     if (mcs->pxms_flg & PXMS_ENG_m)
1954     {
1955       pxmc_call(mcs, mcs->pxms_do_gen);
1956     }
1957   }
1958 }
1959
1960 static inline void pxmc_sfi_dbg(void)
1961 {
1962   int var;
1963   pxmc_state_t *mcs;
1964   pxmc_for_each_mcs(var, mcs)
1965   {
1966     if (mcs->pxms_flg & PXMS_DBG_m)
1967     {
1968       pxmc_call(mcs, mcs->pxms_do_deb);
1969     }
1970   }
1971 }
1972
1973 int pxmc_rocon_pthalalign_callback(pxmc_state_t *mcs)
1974 {
1975   short ptofs;
1976   short ptmark;
1977   int inp_chan = mcs->pxms_inp_info;
1978   int idx_rel;
1979   long irc = fpga_irc[inp_chan]->count;
1980
1981   if (!pxmc_inp_rocon_is_index_edge(mcs))
1982     return 0;
1983
1984   idx_rel = fpga_irc[inp_chan]->count_index - irc;
1985   idx_rel %= mcs->pxms_ptirc;
1986   if(idx_rel < 0)
1987     idx_rel += mcs->pxms_ptirc;
1988
1989   ptofs = irc-mcs->pxms_ptofs;
1990   ptmark = ptofs+idx_rel;
1991
1992   if((unsigned short)ptmark >= mcs->pxms_ptirc) {
1993     if(ptmark>0)
1994       ptmark -= mcs->pxms_ptirc;
1995     else
1996       ptmark += mcs->pxms_ptirc;
1997   }
1998
1999   if((unsigned short)ptmark < mcs->pxms_ptirc) {
2000     mcs->pxms_ptmark = ptmark;
2001   }
2002   return 0;
2003 }
2004
2005 int pxmc_rocon_pthalalign(pxmc_state_t *mcs, int periods)
2006 {
2007   int res;
2008   long r2acq;
2009   long spd;
2010   pxmc_call_t *fin_fnc = pxmc_rocon_pthalalign_callback;
2011
2012   mcs->pxms_cfg&=~PXMS_CFG_I2PT_m;
2013
2014   r2acq=((long)mcs->pxms_ptirc<<PXMC_SUBDIV(mcs))*periods;
2015   spd=1<<PXMC_SUBDIV(mcs);
2016
2017   /* clear bit indicating previous index */
2018   pxmc_inp_rocon_is_index_edge(mcs);
2019
2020   res=pxmc_pthalalign(mcs, r2acq, spd, fin_fnc);
2021
2022   return res;
2023 }
2024
2025 int pxmc_axis_rdmode(pxmc_state_t *mcs)
2026 {
2027   if (mcs->pxms_do_out == pxmc_rocon_pwm2ph_out)
2028     return PXMC_AXIS_MODE_STEPPER_WITH_IRC;
2029   if (mcs->pxms_do_out == pxmc_rocon_pwm_dc_out)
2030     return PXMC_AXIS_MODE_DC;
2031   if (mcs->pxms_do_out == pxmc_rocon_pwm3ph_out)
2032     return PXMC_AXIS_MODE_BLDC;
2033   if (mcs->pxms_do_out == pxmc_pxmcc_pwm3ph_out)
2034     return PXMC_AXIS_MODE_BLDC_PXMCC;
2035   if (mcs->pxms_do_out == pxmc_pxmcc_pwm2ph_out)
2036     return PXMC_AXIS_MODE_STEPPER_WITH_IRC_PXMCC;
2037   if (mcs->pxms_do_out == pxmc_pxmcc_nofb2ph_out)
2038     return PXMC_AXIS_MODE_STEPPER_PXMCC;
2039   return -1;
2040 }
2041
2042
2043 int
2044 pxmc_axis_pt4mode(pxmc_state_t *mcs, int mode)
2045 {
2046   static const typeof(*mcs->pxms_ptptr1) dummy0 = 0;
2047   int res = 0;
2048
2049   if (mode == PXMC_AXIS_MODE_NOCHANGE)
2050     mode = pxmc_axis_rdmode(mcs);
2051   if (mode < 0)
2052     return -1;
2053
2054   switch (mode) {
2055     /* case PXMC_AXIS_MODE_STEPPER: */
2056     case PXMC_AXIS_MODE_STEPPER_PXMCC:
2057     case PXMC_AXIS_MODE_STEPPER_WITH_IRC:
2058     case PXMC_AXIS_MODE_STEPPER_WITH_IRC_PXMCC:
2059       res = pxmc_ptable_set_profile(mcs, &pxmc_ptprofile_sin, 0, 0);
2060       break;
2061     /*case PXMC_AXIS_MODE_STEPPER_WITH_PWM:*/
2062     case PXMC_AXIS_MODE_DC:
2063       /*profive some sane dummy values*/
2064       mcs->pxms_ptptr1 = (typeof(mcs->pxms_ptptr1))&dummy0;
2065       mcs->pxms_ptptr2 = (typeof(mcs->pxms_ptptr1))&dummy0;
2066       mcs->pxms_ptptr3 = (typeof(mcs->pxms_ptptr1))&dummy0;
2067
2068       mcs->pxms_ptscale_mult=1;
2069       mcs->pxms_ptscale_shift=15;
2070       break;
2071     case PXMC_AXIS_MODE_BLDC:
2072     case PXMC_AXIS_MODE_BLDC_PXMCC:
2073       /* res = pxmc_init_ptable(mcs, PXMC_PTPROF_SIN3FUP); */
2074      #ifndef PXMC_WITH_PT_ZIC
2075       res = pxmc_ptable_set_profile(mcs, &pxmc_ptprofile_sin3phup, 0, 0);
2076      #else /*PXMC_WITH_PT_ZIC*/
2077       res = pxmc_ptable_set_profile(mcs, &pxmc_ptprofile_sin3phup_zic, 0, 0);
2078      #endif /*PXMC_WITH_PT_ZIC*/
2079       break;
2080     default:
2081       return -1;
2082   }
2083
2084   mcs->pxms_ptvang = pxmc_ptvang_deg2irc(mcs, 90);
2085
2086   return res;
2087 }
2088
2089 /**
2090  * pxmc_axis_mode - Sets axis mode.[extern API]
2091  * @mcs:        Motion controller state information
2092  * @mode:       0 .. previous mode, 1 .. stepper motor mode,
2093  *              2 .. stepper motor with IRC feedback and PWM ,
2094  *              3 .. stepper motor with PWM control
2095  *              4 .. DC motor with IRC feedback and PWM
2096  *
2097  */
2098 int
2099 pxmc_axis_mode(pxmc_state_t *mcs, int mode)
2100 {
2101   int res;
2102   int prev_mode;
2103
2104   pxmc_set_const_out(mcs, 0);
2105   pxmc_clear_flag(mcs, PXMS_ENI_b);
2106   pxmc_clear_flags(mcs,PXMS_ENO_m);
2107   /* Clear possible stall index flags from hardware */
2108   pxmc_inp_rocon_is_index_edge(mcs);
2109   pxmc_clear_flag(mcs, PXMS_PHA_b);
2110   pxmc_clear_flag(mcs, PXMS_PTI_b);
2111
2112
2113   prev_mode = pxmc_axis_rdmode(mcs);
2114
2115   if (mode == PXMC_AXIS_MODE_NOCHANGE)
2116     mode = prev_mode;
2117   if (mode < 0)
2118     return -1;
2119   if (!mode)
2120     mode = PXMC_AXIS_MODE_DC;
2121
2122   if ((prev_mode == PXMC_AXIS_MODE_BLDC_PXMCC) ||
2123       (prev_mode == PXMCC_MODE_STEPPER_WITH_IRC))
2124     pxmcc_axis_setup(mcs, PXMCC_MODE_IDLE);
2125
2126   res = pxmc_axis_pt4mode(mcs, mode);
2127   if (res < 0)
2128     return -1;
2129
2130   if (mcs->pxms_do_inp == pxmc_pxmcc_nofb_inp)
2131     mcs->pxms_do_inp = pxmc_inp_rocon_inp;
2132   if (mcs->pxms_do_con == pxmc_pxmcc_nofb_con)
2133     mcs->pxms_do_con = pxmc_pid_con;
2134
2135   switch (mode) {
2136     /*case PXMC_AXIS_MODE_STEPPER:*/
2137     case PXMC_AXIS_MODE_STEPPER_WITH_IRC:
2138       mcs->pxms_do_out = pxmc_rocon_pwm2ph_out;
2139       break;
2140     /*case PXMC_AXIS_MODE_STEPPER_WITH_PWM:*/
2141     case PXMC_AXIS_MODE_DC:
2142       mcs->pxms_do_out = pxmc_rocon_pwm_dc_out;
2143       break;
2144     case PXMC_AXIS_MODE_BLDC:
2145       mcs->pxms_do_out = pxmc_rocon_pwm3ph_out;
2146       break;
2147     case PXMC_AXIS_MODE_BLDC_PXMCC:
2148       if (pxmcc_axis_setup(mcs, PXMCC_MODE_BLDC) < 0)
2149         return -1;
2150       pxmcc_axis_enable(mcs, 1);
2151       mcs->pxms_do_out = pxmc_pxmcc_pwm3ph_out;
2152       break;
2153     case PXMC_AXIS_MODE_STEPPER_WITH_IRC_PXMCC:
2154       if (pxmcc_axis_setup(mcs, PXMCC_MODE_STEPPER_WITH_IRC) < 0)
2155         return -1;
2156       pxmcc_axis_enable(mcs, 1);
2157       mcs->pxms_do_out = pxmc_pxmcc_pwm2ph_out;
2158       break;
2159     case PXMC_AXIS_MODE_STEPPER_PXMCC:
2160       if (pxmcc_axis_setup(mcs, PXMCC_MODE_STEPPER) < 0)
2161         return -1;
2162       pxmcc_axis_enable(mcs, 1);
2163       mcs->pxms_do_inp = pxmc_pxmcc_nofb_inp;
2164       mcs->pxms_do_con = pxmc_pxmcc_nofb_con;
2165       mcs->pxms_do_out = pxmc_pxmcc_nofb2ph_out;
2166       break;
2167     default:
2168       return -1;
2169   }
2170
2171   /* Clear possible stall index flags from hardware */
2172   pxmc_inp_rocon_is_index_edge(mcs);
2173   /* Request new phases alignment for changed parameters */
2174   pxmc_clear_flag(mcs, PXMS_PHA_b);
2175   pxmc_clear_flag(mcs, PXMS_PTI_b);
2176   pxmc_set_flag(mcs, PXMS_ENI_b);
2177   return res;
2178 }
2179
2180 void pxmc_sfi_isr(void)
2181 {
2182   unsigned long spent = pxmc_fast_tick_time();
2183
2184   pxmc_sfi_input();
2185   pxmc_sfi_controller_and_output();
2186   pxmc_sfi_generator();
2187   pxmc_sfi_dbg();
2188   /* Kick LX Master watchdog */
2189   if (pxmc_main_list.pxml_cnt != 0)
2190     *fpga_lx_master_transmitter_wdog = 1;
2191
2192   spent = pxmc_fast_tick_time() - spent;
2193
2194   if(spent > pxmc_sfi_spent_time_max)
2195     pxmc_sfi_spent_time_max = spent;
2196
2197 }
2198
2199 int pxmc_clear_power_stop(void)
2200 {
2201   return 0;
2202 }
2203
2204 int pxmc_process_state_check(unsigned long *pbusy_bits,
2205                              unsigned long *perror_bits)
2206 {
2207   unsigned short flg;
2208   unsigned short chan;
2209   unsigned long busy_bits = 0;
2210   unsigned long error_bits = 0;
2211   pxmc_state_t *mcs;
2212   flg=0;
2213   pxmc_for_each_mcs(chan, mcs) {
2214     if(mcs) {
2215       flg |= mcs->pxms_flg;
2216       if (mcs->pxms_flg & PXMS_BSY_m)
2217         busy_bits |= 1 << chan;
2218       if (mcs->pxms_flg & PXMS_ERR_m)
2219         error_bits |= 1 << chan;
2220     }
2221   }
2222   if (appl_errstop_mode) {
2223     if((flg & PXMS_ENG_m) && (flg & PXMS_ERR_m)) {
2224       pxmc_for_each_mcs(chan, mcs) {
2225         if(mcs&&(mcs->pxms_flg & PXMS_ENG_m)) {
2226           pxmc_stop(mcs, 0);
2227         }
2228       }
2229     }
2230   }
2231
2232   if (pbusy_bits != NULL)
2233     *pbusy_bits = busy_bits;
2234   if (error_bits != NULL)
2235     *perror_bits = error_bits;
2236
2237   return flg;
2238 }
2239
2240 int pxmc_done(void)
2241 {
2242   int var;
2243   pxmc_state_t *mcs;
2244
2245   if (!pxmc_main_list.pxml_cnt)
2246     return 0;
2247
2248   pxmc_for_each_mcs(var, mcs)
2249   {
2250     pxmc_set_const_out(mcs,0);
2251   }
2252
2253   pxmc_main_list.pxml_cnt = 0;
2254   __memory_barrier();
2255
2256   return 0;
2257 }
2258
2259 int pxmc_initialize(void)
2260 {
2261   int res;
2262   int i;
2263
2264   pxmc_state_t *mcs = &mcs0.base;
2265   lpc_qei_state_t *qst = &lpc_qei_state;
2266
2267   *fpga_irc_reset = 1;
2268
2269   for (i = 0; i < 8; i++) {
2270     fpga_irc[i]->count = 0;
2271     fpga_irc[i]->count_index = 0;
2272     *fpga_irc_state[i] = FPGA_IRC_STATE_INDEX_EVENT_MASK;
2273   }
2274
2275   /* Initialize QEI module for IRC counting */
2276   pxmc_inp_lpc_qei_init(mcs->pxms_inp_info);
2277
2278   /* Initialize QEI events processing */
2279   if (lpc_qei_setup_irq(qst) < 0)
2280     return -1;
2281
2282   *fpga_irc_reset = 0;
2283
2284   //res = pxmc_axis_pt4mode(mcs, PXMC_AXIS_MODE_BLDC);
2285
2286   /*pxmc_ctm4pwm3f_wr(mcs, 0, 0, 0);*/
2287   //pxmc_rocon_pwm3ph_wr(mcs, 0, 0, 0);
2288
2289   pxmc_rocon_pwm_master_init();
2290  #ifdef PXMC_ROCON_TIMED_BY_RX_DONE
2291   pxmc_rocon_rx_done_isr_setup(pxmc_rocon_rx_done_isr);
2292  #endif /*PXMC_ROCON_TIMED_BY_RX_DONE*/
2293
2294   pxmc_main_list.pxml_cnt = 0;
2295   pxmc_dbg_hist = NULL;
2296   __memory_barrier();
2297   pxmc_main_list.pxml_cnt = PXML_MAIN_CNT;
2298
2299   return 0;
2300 }