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