sw/app/rocon/appl_pxmc.c

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