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