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