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