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