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 int pxmcc_curadc_zero(int wait)
1295 {
1296   int chan;
1297   unsigned try = wait? 200: 0;
1298   volatile pxmcc_data_t *mcc_data = pxmc_rocon_mcc_data();
1299   volatile pxmcc_curadc_data_t *curadc;
1300
1301   for (chan = 0; chan < PXMCC_CURADC_CHANNELS; chan++)
1302     pxmc_rocon_pwm_direct_wr(chan, 0, 0);
1303
1304   do {
1305     if (mcc_data->common.fwversion == PXMCC_FWVERSION)
1306       break;
1307     if (!try--)
1308       return -1;
1309   } while(1);
1310
1311   if (wait) {
1312     if (pxmc_rocon_wait_rx_done() < 0)
1313       return -1;
1314
1315     if (pxmc_rocon_wait_rx_done() < 0)
1316       return -1;
1317   }
1318
1319   for (chan = 0; chan < PXMCC_CURADC_CHANNELS; chan++) {
1320     curadc = mcc_data->curadc + chan;
1321     curadc->siroladc_offs += curadc->cur_val;
1322   }
1323
1324   return 0;
1325 }
1326
1327 /*******************************************************************/
1328
1329 volatile void *pxmc_rocon_rx_data_hist_buff;
1330 volatile void *pxmc_rocon_rx_data_hist_buff_end;
1331 int pxmc_rocon_rx_data_hist_mode;
1332
1333 uint32_t pxmc_rocon_rx_last_irq;
1334 uint32_t pxmc_rocon_rx_cycle_time;
1335 uint32_t pxmc_rocon_rx_irq_latency;
1336 uint32_t pxmc_rocon_rx_irq_latency_max;
1337
1338 IRQ_HANDLER_FNC(pxmc_rocon_rx_done_isr)
1339 {
1340   uint32_t ir;
1341
1342   ir = ROCON_RX_TIM->IR & LPC_TIM_IR_ALL_m;
1343   ROCON_RX_TIM->IR = ir;
1344   if (ir & LPC_TIM_IR_CR1INT_m) {
1345     uint32_t cr0, cr1;
1346     cr0 = ROCON_RX_TIM->CR0;
1347     cr1 = ROCON_RX_TIM->CR1;
1348
1349     pxmc_rocon_rx_cycle_time = cr1 - pxmc_rocon_rx_last_irq;
1350     pxmc_rocon_rx_last_irq = cr1;
1351
1352     hal_gpio_set_value(T2MAT0_PIN, 1);
1353     hal_gpio_set_value(T2MAT1_PIN, 0);
1354     hal_gpio_set_value(T2MAT0_PIN, 0);
1355
1356     pxmc_rocon_rx_done_sqn_compute();
1357     pxmc_rocon_vin_compute();
1358     pxmc_rocon_rx_error_check();
1359
1360     if (pxmc_rocon_rx_data_hist_buff >= pxmc_rocon_rx_data_hist_buff_end)
1361       pxmc_rocon_rx_data_hist_buff = NULL;
1362
1363     if (pxmc_rocon_rx_data_hist_buff != NULL) {
1364       if (pxmc_rocon_rx_data_hist_mode == 0) {
1365         int i;
1366         volatile uint32_t *pwm_reg = fpga_lx_master_transmitter_base + 8;
1367         volatile uint32_t *rec_reg = fpga_lx_master_receiver_base + 8;
1368         uint16_t *pbuf = (uint16_t *)pxmc_rocon_rx_data_hist_buff;
1369         for (i = 0; i < 8; i++) {
1370           *(pbuf++) = *(rec_reg++);
1371         }
1372         for (i = 0; i < 8; i++) {
1373           *(pbuf++) = *(pwm_reg++);
1374         }
1375         pxmc_rocon_rx_data_hist_buff = pbuf;
1376       } else if (pxmc_rocon_rx_data_hist_mode == 1) {
1377         int i;
1378         uint32_t *pbuf = (uint32_t *)pxmc_rocon_rx_data_hist_buff;
1379         pxmcc_data_t *mcc_data = pxmc_rocon_mcc_data();
1380         pxmcc_axis_data_t *mcc_axis = mcc_data->axis + 0;
1381         uint32_t *ptumbl = (uint32_t *)mcc_axis;
1382
1383         for (i = 0; i < 16; i++)
1384           *(pbuf++) = *(ptumbl++);
1385
1386         pxmc_rocon_rx_data_hist_buff = pbuf;
1387       }
1388     }
1389
1390     pxmc_rocon_rx_irq_latency = ROCON_RX_TIM->TC - cr1;
1391     if (pxmc_rocon_rx_irq_latency > pxmc_rocon_rx_irq_latency_max)
1392       pxmc_rocon_rx_irq_latency_max = pxmc_rocon_rx_irq_latency;
1393
1394    #ifdef PXMC_ROCON_TIMED_BY_RX_DONE
1395     pxmc_sfi_isr();
1396     do_pxmc_coordmv();
1397    #endif /*PXMC_ROCON_TIMED_BY_RX_DONE*/
1398   }
1399
1400   return IRQ_HANDLED;
1401 }
1402
1403 int
1404 pxmc_rocon_rx_done_isr_setup(irq_handler_t rx_done_isr_handler)
1405 {
1406
1407   disable_irq(ROCON_RX_IRQn);
1408
1409   hal_pin_conf_set(T2MAT0_PIN, PORT_CONF_GPIO_OUT_LO);
1410   hal_pin_conf_set(T2MAT1_PIN, PORT_CONF_GPIO_OUT_LO);
1411   hal_pin_conf(T2CAP0_PIN);
1412   hal_pin_conf(T2CAP1_PIN);
1413
1414   hal_gpio_direction_output(T2MAT0_PIN, 1);
1415   hal_gpio_direction_output(T2MAT1_PIN, 0);
1416   hal_gpio_set_value(T2MAT0_PIN, 0);
1417
1418   /* Enable CLKOUT pin function, source CCLK, divide by 1 */
1419   LPC_SC->CLKOUTCFG = 0x0100;
1420
1421   request_irq(ROCON_RX_IRQn, rx_done_isr_handler, 0, NULL,NULL);
1422
1423   ROCON_RX_TIM->TCR = 0;
1424   ROCON_RX_TIM->CTCR = 0;
1425   ROCON_RX_TIM->PR = 0; /* Divide by 1 */
1426
1427   ROCON_RX_TIM->CCR = LPC_TIM_CCR_CAP0RE_m | LPC_TIM_CCR_CAP1FE_m |
1428                    LPC_TIM_CCR_CAP1I_m;
1429
1430   ROCON_RX_TIM->EMR = __val2mfld(LPC_TIM_EMR_EMC0_m, LPC_TIM_EMR_NOP) |
1431                    __val2mfld(LPC_TIM_EMR_EMC1_m, LPC_TIM_EMR_NOP);
1432
1433   ROCON_RX_TIM->MCR = 0;                        /* No IRQ on MRx */
1434   ROCON_RX_TIM->TCR = LPC_TIM_TCR_CEN_m;        /* Enable timer counting */
1435   enable_irq(ROCON_RX_IRQn);            /* Enable interrupt */
1436
1437   return 0;
1438
1439 }
1440
1441 int
1442 pxmc_rocon_pwm_master_setup(unsigned lxpwr_chips)
1443 {
1444   int i;
1445   int grp_in = 0;
1446   int grp_out = 0;
1447   unsigned word_slot;
1448   unsigned receiver_done_div = 1;
1449   unsigned lxpwr_chips_max = 2;
1450  #ifdef LXPWR_WITH_SIROLADC
1451   unsigned lxpwr_header = 1;
1452   unsigned lxpwr_words = 1 + 8 * 2 + 2;
1453   unsigned lxpwr_chip_pwm_cnt = 8;
1454  #else /*LXPWR_WITH_SIROLADC*/
1455   unsigned lxpwr_header = 0;
1456   unsigned lxpwr_words = 8;
1457   unsigned lxpwr_chip_pwm_cnt = 8;
1458  #endif /*LXPWR_WITH_SIROLADC*/
1459
1460  #ifdef PXMC_ROCON_TIMED_BY_RX_DONE
1461   receiver_done_div = 5;
1462  #endif /*PXMC_ROCON_TIMED_BY_RX_DONE*/
1463
1464   *fpga_lx_master_reset = 1;
1465   *fpga_lx_master_transmitter_reg = 0;
1466   *fpga_lx_master_transmitter_cycle = PXMC_LXPWR_PWM_CYCLE; /* 50 MHz -> 20 kHz */
1467   *fpga_lx_master_receiver_done_div = receiver_done_div << 8;
1468   pxmc_rocon_rx_done_sqn_inc = receiver_done_div;
1469
1470   if (lxpwr_chips > lxpwr_chips_max)
1471     return -1;
1472
1473   for (i = 0; i < LX_MASTER_DATA_OFFS + lxpwr_words * lxpwr_chips_max; i++)
1474     fpga_lx_master_receiver_base[i] = 0;
1475
1476   if (lxpwr_chips >= 2) {
1477     word_slot = LX_MASTER_DATA_OFFS + lxpwr_words;
1478     fpga_lx_master_receiver_base[grp_in++] = (word_slot << 8) | lxpwr_words;
1479   }
1480
1481   word_slot = LX_MASTER_DATA_OFFS;
1482   fpga_lx_master_receiver_base[grp_in++] = (word_slot << 8) | lxpwr_words;
1483
1484   fpga_lx_master_receiver_base[grp_in++] = 0x0000;
1485
1486   for (i = 0; i < LX_MASTER_DATA_OFFS + lxpwr_words * lxpwr_chips_max; i++)
1487     fpga_lx_master_transmitter_base[i] = 0;
1488
1489   word_slot = LX_MASTER_DATA_OFFS + lxpwr_header + lxpwr_chip_pwm_cnt;
1490   fpga_lx_master_transmitter_base[grp_out++] = (word_slot << 8) | lxpwr_words;
1491  #ifdef LXPWR_WITH_SIROLADC
1492   fpga_lx_master_transmitter_base[word_slot] = 0xc100 | (lxpwr_words - 1);
1493  #endif /*LXPWR_WITH_SIROLADC*/
1494
1495   word_slot = LX_MASTER_DATA_OFFS + 0;
1496   fpga_lx_master_transmitter_base[grp_out++] = (word_slot << 8) | lxpwr_words;
1497  #ifdef LXPWR_WITH_SIROLADC
1498   fpga_lx_master_transmitter_base[word_slot] = 0xc100 | (lxpwr_words - 1);
1499  #endif /*LXPWR_WITH_SIROLADC*/
1500
1501   fpga_lx_master_transmitter_base[grp_out++] = 0x0000;
1502
1503   *fpga_lx_master_reset = 0;
1504   *fpga_lx_master_transmitter_cycle = PXMC_LXPWR_PWM_CYCLE; /* 50 MHz -> 20 kHz */
1505   *fpga_lx_master_receiver_done_div = receiver_done_div << 8;
1506
1507   return 0;
1508 }
1509
1510 int
1511 pxmc_rocon_wait_rx_done(void)
1512 {
1513   uint32_t sqn_last;
1514   uint32_t sqn_act;
1515   uint32_t timeout = 10000;
1516
1517   sqn_last = *fpga_lx_master_receiver_done_div;
1518   sqn_last = sqn_last & 0x1f;
1519
1520   do {
1521     sqn_act = *fpga_lx_master_receiver_done_div;
1522     sqn_act = sqn_act & 0x1f;
1523     if (sqn_act != sqn_last)
1524       return 0;
1525   } while(timeout--);
1526
1527   return -1;
1528 }
1529
1530 int
1531 pxmc_rocon_pwm_master_init(void)
1532 {
1533   int res;
1534   volatile uint32_t *lxpwr_header_ptr;
1535   unsigned lxpwr_words = 1 + 8 * 2 + 2;
1536
1537   pxmc_rocon_lxpwr_chips = 0;
1538
1539   res = pxmc_rocon_pwm_master_setup(2);
1540   if (res < 0)
1541     return 0;
1542
1543   if (pxmc_rocon_wait_rx_done() < 0)
1544     return -1;
1545   if (pxmc_rocon_wait_rx_done() < 0)
1546     return -1;
1547
1548   lxpwr_header_ptr = fpga_lx_master_receiver_base;
1549   lxpwr_header_ptr += LX_MASTER_DATA_OFFS;
1550
1551   if (lxpwr_header_ptr[0] == 0xb100 + lxpwr_words - 1) {
1552     if (lxpwr_header_ptr[lxpwr_words] == 0xb100 + lxpwr_words - 1) {
1553       pxmc_rocon_lxpwr_chips = 2;
1554       return 2;
1555     }
1556     return -1;
1557   }
1558
1559   if (lxpwr_header_ptr[lxpwr_words] != 0xb100 + lxpwr_words - 1) {
1560     return -1;
1561   }
1562
1563   res = pxmc_rocon_pwm_master_setup(1);
1564   if (res < 0)
1565     return 0;
1566
1567   if (pxmc_rocon_wait_rx_done() < 0)
1568     return -1;
1569   if (pxmc_rocon_wait_rx_done() < 0)
1570     return -1;
1571
1572   if (lxpwr_header_ptr[0] != 0xb100 + lxpwr_words - 1)
1573     return -1;
1574
1575   pxmc_rocon_lxpwr_chips = 1;
1576
1577   return 1;
1578 }
1579
1580 int pxmc_ptofs_from_index(pxmc_state_t *mcs, unsigned long irc,
1581                            unsigned long index_irc, int diff2err)
1582 {
1583   long ofsl;
1584   short ofs;
1585
1586   ofs = ofsl = index_irc - mcs->pxms_ptmark;
1587
1588   if (diff2err) {
1589     short diff;
1590     diff = (unsigned short)ofs - (unsigned short)mcs->pxms_ptofs;
1591     if (diff >= mcs->pxms_ptirc / 2)
1592       diff -= mcs->pxms_ptirc;
1593     if (diff <= -mcs->pxms_ptirc / 2)
1594       diff += mcs->pxms_ptirc;
1595     if (diff < 0)
1596       diff = -diff;
1597     if(diff >= mcs->pxms_ptirc / 6) {
1598       return -1;
1599     }
1600   } else {
1601     long diff;
1602     diff = (unsigned long)ofsl - irc;
1603     ofs = ofsl - (diff / mcs->pxms_ptirc) * mcs->pxms_ptirc;
1604   }
1605
1606   mcs->pxms_ptofs = ofs;
1607
1608   return 1;
1609 }
1610
1611 /**
1612  * pxmc_dummy_con - Dummy controller for synchronous BLDC/PMSM/steper drive
1613  * @mcs:        Motion controller state information
1614  */
1615 int
1616 pxmc_dummy_con(pxmc_state_t *mcs)
1617 {
1618   return 0;
1619 }
1620
1621 int pxmc_rocon_hh_gd10(pxmc_state_t *mcs);
1622 int pxmc_rocon_hh_gd20(pxmc_state_t *mcs);
1623 int pxmc_rocon_hh_gd30(pxmc_state_t *mcs);
1624 int pxmc_rocon_hh_gd40(pxmc_state_t *mcs);
1625 int pxmc_rocon_hh_gd50(pxmc_state_t *mcs);
1626 int pxmc_rocon_hh_gd90(pxmc_state_t *mcs);
1627
1628 /* initialize for hard home */
1629 int
1630 pxmc_rocon_hh_gi(pxmc_state_t *mcs)
1631 {
1632   pxmc_set_flag(mcs,PXMS_BSY_b);
1633  #if 0 /* config and speed already set in pxmc_hh */
1634   spd = mcs->pxms_ms;
1635   spd >>= mcs->pxms_cfg & PXMS_CFG_HSPD_m;
1636   if(!spd) spd = 1;
1637   if(mcs->pxms_cfg & PXMS_CFG_HDIR_m)
1638     spd = -spd;
1639   mcs->pxms_gen_hsp = spd;
1640  #endif
1641   mcs->pxms_rsfg = 0;
1642   mcs->pxms_gen_htim = 16;
1643   mcs->pxms_do_gen = pxmc_rocon_hh_gd10;
1644   return pxmc_rocon_hh_gd10(mcs);
1645 }
1646
1647 /* fill initial mark filter and then decide */
1648 int
1649 pxmc_rocon_hh_gd10(pxmc_state_t *mcs)
1650 {
1651   int mark;
1652
1653   if(mcs->pxms_flg & PXMS_ERR_m)
1654     return pxmc_spdnext_gend(mcs);
1655
1656   pxmc_spd_gacc(&(mcs->pxms_rs), 0, mcs->pxms_ma);
1657   mcs->pxms_rp += mcs->pxms_rs;
1658
1659   mark = pxmc_inp_rocon_is_mark(mcs);
1660
1661   if (mcs->pxms_gen_htim) {
1662     mcs->pxms_gen_htim--;
1663     return 0;
1664   }
1665
1666   if (!(mcs->pxms_cfg & PXMS_CFG_HLS_m)) {
1667     /* limit switch is not used */
1668     pxmc_inp_rocon_is_index_edge(mcs);
1669     mcs->pxms_do_gen = pxmc_rocon_hh_gd50;
1670     return 0;
1671   }
1672
1673   if (mark >= 6) {
1674     /* go out from switch */
1675     mcs->pxms_do_gen = pxmc_rocon_hh_gd20;
1676   } else {
1677     /* switch is off -> look for it */
1678     mcs->pxms_do_gen = pxmc_rocon_hh_gd30;
1679   }
1680   return 0;
1681 }
1682
1683 /* go out from switch */
1684 int
1685 pxmc_rocon_hh_gd20(pxmc_state_t *mcs)
1686 {
1687   int mark;
1688
1689   if(mcs->pxms_flg & PXMS_ERR_m)
1690     return pxmc_spdnext_gend(mcs);
1691
1692   mark = pxmc_inp_rocon_is_mark(mcs);
1693
1694   if (mark <= 1) {
1695     /* switch is off -> look for it again */
1696     mcs->pxms_do_gen = pxmc_rocon_hh_gd30;
1697   }
1698
1699   pxmc_spd_gacc(&(mcs->pxms_rs), -mcs->pxms_gen_hsp, mcs->pxms_ma);
1700   mcs->pxms_rp += mcs->pxms_rs;
1701
1702   return 0;
1703 }
1704
1705 /* switch is off -> look for it */
1706 int
1707 pxmc_rocon_hh_gd30(pxmc_state_t *mcs)
1708 {
1709   long spd;
1710   int mark;
1711
1712   if (mcs->pxms_flg & PXMS_ERR_m)
1713     return pxmc_spdnext_gend(mcs);
1714
1715   pxmc_spd_gacc(&(mcs->pxms_rs), mcs->pxms_gen_hsp, mcs->pxms_ma);
1716   mcs->pxms_rp += mcs->pxms_rs;
1717
1718   mark = pxmc_inp_rocon_is_mark(mcs);
1719
1720   if (mark >= 8){
1721     spd = mcs->pxms_gen_hsp >> 2; /* slow down */
1722     if (!spd)
1723       spd = 1;
1724     mcs->pxms_gen_hsp = spd;
1725     mcs->pxms_do_gen = pxmc_rocon_hh_gd40;
1726   }
1727
1728   return 0;
1729 }
1730
1731 /* switch is on -> find edge */
1732 int
1733 pxmc_rocon_hh_gd40(pxmc_state_t *mcs)
1734 {
1735   int mark;
1736
1737   if (mcs->pxms_flg & PXMS_ERR_m)
1738     return pxmc_spdnext_gend(mcs);
1739
1740   mark = pxmc_inp_rocon_is_mark(mcs);
1741
1742   if (mark <= 1) {
1743     if (mcs->pxms_cfg & PXMS_CFG_HRI_m) {
1744       pxmc_inp_rocon_is_index_edge(mcs);
1745       mcs->pxms_do_gen = pxmc_rocon_hh_gd50;
1746     } else {
1747       pxmc_inp_rocon_ap_zero(mcs);
1748       mcs->pxms_do_gen = pxmc_stop_gi;
1749     }
1750   }
1751
1752   pxmc_spd_gacc(&(mcs->pxms_rs), -mcs->pxms_gen_hsp, mcs->pxms_ma);
1753   mcs->pxms_rp += mcs->pxms_rs;
1754
1755   return 0;
1756 }
1757
1758 /* calibrate on revolution index */
1759 int
1760 pxmc_rocon_hh_gd50(pxmc_state_t *mcs)
1761 {
1762   if (mcs->pxms_flg & PXMS_ERR_m)
1763     return pxmc_spdnext_gend(mcs);
1764
1765   pxmc_spd_gacc(&(mcs->pxms_rs), mcs->pxms_gen_hsp, mcs->pxms_ma);
1766   mcs->pxms_rp += mcs->pxms_rs;
1767
1768   if (pxmc_inp_rocon_is_index_edge(mcs)){
1769     pxmc_inp_rocon_irc_offset_from_index(mcs);
1770     mcs->pxms_do_gen = pxmc_stop_gi;
1771   }
1772
1773   return 0;
1774 }
1775
1776 pxmc_call_t *pxmc_get_hh_gi_4axis(pxmc_state_t *mcs)
1777 {
1778   return pxmc_rocon_hh_gi;
1779 }
1780
1781 pxmc_rocon_state_t mcs0 =
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 /*pxmc_dummy_con*/,
1790 pxms_do_out:
1791   pxmc_rocon_pwm_dc_out /*pxmc_rocon_pwm3ph_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: 0,
1800   pxms_out_info: 0,
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_HRI_m * 0 | PXMS_CFG_HDIR_m * 0 |
1807   PXMS_CFG_I2PT_m * 0 | 0x2,
1808
1809   pxms_ptper: 1,
1810   pxms_ptirc: 1000,
1811   pxms_ptmark: 1180,
1812   /*pxms_ptamp: 0x7fff,*/
1813
1814   pxms_hal: 0x40,
1815 },
1816   .cur_d_p = 150,
1817   .cur_d_i = 6000,
1818   .cur_q_p = 150,
1819   .cur_q_i = 6000,
1820   .cur_hold = 200,
1821 };
1822
1823 pxmc_rocon_state_t mcs1 =
1824 {
1825 .base = {
1826 pxms_flg:
1827   PXMS_ENI_m,
1828 pxms_do_inp:
1829   pxmc_inp_rocon_inp,
1830 pxms_do_con:
1831   pxmc_pid_con,
1832 pxms_do_out:
1833   pxmc_rocon_pwm_dc_out,
1834   pxms_do_deb: 0,
1835   pxms_do_gen: 0,
1836 pxms_do_ap2hw:
1837   pxmc_inp_rocon_ap2hw,
1838   pxms_ap: 0, pxms_as: 0,
1839   pxms_rp: 55 * 256, pxms_rs: 0, pxms_subdiv: 8,
1840   pxms_md: 800 << 8, pxms_ms: 500, pxms_ma: 10,
1841   pxms_inp_info: 1,
1842   pxms_out_info: 2,
1843   pxms_ene: 0, pxms_erc: 0,
1844   pxms_p: 80, pxms_i: 10, pxms_d: 200, pxms_s1: 200, pxms_s2: 0,
1845   pxms_me: 0x7e00/*0x7fff*/,
1846 pxms_cfg:
1847   PXMS_CFG_SMTH_m | PXMS_CFG_MD2E_m | PXMS_CFG_HLS_m | PXMS_CFG_HPS_m * 0 |
1848   PXMS_CFG_HRI_m * 0 | PXMS_CFG_HDIR_m * 0 |
1849   PXMS_CFG_I2PT_m * 0 | 0x2,
1850
1851   pxms_ptper: 1,
1852   pxms_ptirc: 1000,
1853   /*pxms_ptamp: 0x7fff,*/
1854
1855   pxms_hal: 0x40,
1856 }};
1857
1858 pxmc_rocon_state_t mcs2 =
1859 {
1860 .base = {
1861 pxms_flg:
1862   PXMS_ENI_m,
1863 pxms_do_inp:
1864   pxmc_inp_rocon_inp,
1865 pxms_do_con:
1866   pxmc_pid_con,
1867 pxms_do_out:
1868   pxmc_rocon_pwm_dc_out,
1869   pxms_do_deb: 0,
1870   pxms_do_gen: 0,
1871 pxms_do_ap2hw:
1872   pxmc_inp_rocon_ap2hw,
1873   pxms_ap: 0, pxms_as: 0,
1874   pxms_rp: 55 * 256, pxms_rs: 0, pxms_subdiv: 8,
1875   pxms_md: 800 << 8, pxms_ms: 500, pxms_ma: 10,
1876   pxms_inp_info: 2,
1877   pxms_out_info: 4,
1878   pxms_ene: 0, pxms_erc: 0,
1879   pxms_p: 80, pxms_i: 10, pxms_d: 200, pxms_s1: 200, pxms_s2: 0,
1880   pxms_me: 0x7e00/*0x7fff*/,
1881 pxms_cfg:
1882   PXMS_CFG_SMTH_m | PXMS_CFG_MD2E_m | PXMS_CFG_HLS_m | PXMS_CFG_HPS_m * 0 |
1883   PXMS_CFG_I2PT_m * 0 | PXMS_CFG_HRI_m |
1884   PXMS_CFG_HDIR_m | 0x2,
1885
1886   pxms_ptper: 1,
1887   pxms_ptirc: 1000,
1888   /*pxms_ptamp: 0x7fff,*/
1889
1890   pxms_hal: 0x40,
1891 }};
1892
1893 pxmc_rocon_state_t mcs3 =
1894 {
1895 .base = {
1896 pxms_flg:
1897   PXMS_ENI_m,
1898 pxms_do_inp:
1899   pxmc_inp_rocon_inp,
1900 pxms_do_con:
1901   pxmc_pid_con,
1902 pxms_do_out:
1903   pxmc_rocon_pwm_dc_out,
1904   pxms_do_deb: 0,
1905   pxms_do_gen: 0,
1906 pxms_do_ap2hw:
1907   pxmc_inp_rocon_ap2hw,
1908   pxms_ap: 0, pxms_as: 0,
1909   pxms_rp: 55 * 256, pxms_rs: 0, pxms_subdiv: 8,
1910   pxms_md: 800 << 8, pxms_ms: 500, pxms_ma: 10,
1911   pxms_inp_info: 3,
1912   pxms_out_info: 6,
1913   pxms_ene: 0, pxms_erc: 0,
1914   pxms_p: 80, pxms_i: 10, pxms_d: 200, pxms_s1: 200, pxms_s2: 0,
1915   pxms_me: 0x7e00/*0x7fff*/,
1916 pxms_cfg:
1917   PXMS_CFG_SMTH_m | PXMS_CFG_MD2E_m | PXMS_CFG_HLS_m | PXMS_CFG_HPS_m * 0 |
1918   PXMS_CFG_I2PT_m * 0 | PXMS_CFG_HRI_m |
1919   PXMS_CFG_HDIR_m * 0 | 0x2,
1920
1921   pxms_ptper: 1,
1922   pxms_ptirc: 1000,
1923   /*pxms_ptamp: 0x7fff,*/
1924
1925   pxms_hal: 0x40,
1926 }};
1927
1928 pxmc_rocon_state_t mcs4 =
1929 {
1930 .base = {
1931 pxms_flg:
1932   PXMS_ENI_m,
1933 pxms_do_inp:
1934   pxmc_inp_rocon_inp,
1935 pxms_do_con:
1936   pxmc_pid_con,
1937 pxms_do_out:
1938   pxmc_rocon_pwm_dc_out,
1939   pxms_do_deb: 0,
1940   pxms_do_gen: 0,
1941 pxms_do_ap2hw:
1942   pxmc_inp_rocon_ap2hw,
1943   pxms_ap: 0, pxms_as: 0,
1944   pxms_rp: 55 * 256, pxms_rs: 0, pxms_subdiv: 8,
1945   pxms_md: 800 << 8, pxms_ms: 500, pxms_ma: 10,
1946   pxms_inp_info: 4,
1947   pxms_out_info: 8,
1948   pxms_ene: 0, pxms_erc: 0,
1949   pxms_p: 80, pxms_i: 10, pxms_d: 200, pxms_s1: 200, pxms_s2: 0,
1950   pxms_me: 0x7e00/*0x7fff*/,
1951 pxms_cfg:
1952   PXMS_CFG_SMTH_m | PXMS_CFG_MD2E_m | PXMS_CFG_HLS_m | PXMS_CFG_HPS_m * 0 |
1953   PXMS_CFG_HRI_m | PXMS_CFG_I2PT_m * 0 |
1954   PXMS_CFG_HDIR_m | 0x2,
1955
1956   pxms_ptper: 1,
1957   pxms_ptirc: 1000,
1958   /*pxms_ptamp: 0x7fff,*/
1959
1960   pxms_hal: 0x40,
1961 }};
1962
1963 pxmc_rocon_state_t mcs5 =
1964 {
1965 .base = {
1966 pxms_flg:
1967   PXMS_ENI_m,
1968 pxms_do_inp:
1969   pxmc_inp_rocon_inp,
1970 pxms_do_con:
1971   pxmc_pid_con,
1972 pxms_do_out:
1973   pxmc_rocon_pwm_dc_out,
1974   pxms_do_deb: 0,
1975   pxms_do_gen: 0,
1976 pxms_do_ap2hw:
1977   pxmc_inp_rocon_ap2hw,
1978   pxms_ap: 0, pxms_as: 0,
1979   pxms_rp: 55 * 256, pxms_rs: 0, pxms_subdiv: 8,
1980   pxms_md: 800 << 8, pxms_ms: 500, pxms_ma: 10,
1981   pxms_inp_info: 5,
1982   pxms_out_info: 10,
1983   pxms_ene: 0, pxms_erc: 0,
1984   pxms_p: 80, pxms_i: 10, pxms_d: 200, pxms_s1: 200, pxms_s2: 0,
1985   pxms_me: 0x7e00/*0x7fff*/,
1986 pxms_cfg:
1987   PXMS_CFG_SMTH_m | PXMS_CFG_MD2E_m | PXMS_CFG_HLS_m | PXMS_CFG_HPS_m |
1988   PXMS_CFG_HRI_m | PXMS_CFG_I2PT_m * 0 |
1989   PXMS_CFG_HDIR_m | 0x2,
1990
1991   pxms_ptper: 1,
1992   pxms_ptirc: 1000,
1993   /*pxms_ptamp: 0x7fff,*/
1994
1995   pxms_hal: 0x40,
1996 }};
1997
1998 pxmc_rocon_state_t mcs6 =
1999 {
2000 .base = {
2001 pxms_flg:
2002   PXMS_ENI_m,
2003 pxms_do_inp:
2004   pxmc_inp_rocon_inp,
2005 pxms_do_con:
2006   pxmc_pid_con,
2007 pxms_do_out:
2008   pxmc_rocon_pwm_dc_out,
2009   pxms_do_deb: 0,
2010   pxms_do_gen: 0,
2011 pxms_do_ap2hw:
2012   pxmc_inp_rocon_ap2hw,
2013   pxms_ap: 0, pxms_as: 0,
2014   pxms_rp: 55 * 256, pxms_rs: 0, pxms_subdiv: 8,
2015   pxms_md: 800 << 8, pxms_ms: 500, pxms_ma: 10,
2016   pxms_inp_info: 6,
2017   pxms_out_info: 12,
2018   pxms_ene: 0, pxms_erc: 0,
2019   pxms_p: 80, pxms_i: 10, pxms_d: 200, pxms_s1: 200, pxms_s2: 0,
2020   pxms_me: 0x7e00/*0x7fff*/,
2021 pxms_cfg:
2022   PXMS_CFG_SMTH_m | PXMS_CFG_MD2E_m | PXMS_CFG_HLS_m | PXMS_CFG_I2PT_m * 0 |
2023   0x1,
2024
2025   pxms_ptper: 1,
2026   pxms_ptirc: 1000,
2027   /*pxms_ptamp: 0x7fff,*/
2028
2029   pxms_hal: 0x40,
2030 }};
2031
2032 pxmc_rocon_state_t mcs7 =
2033 {
2034 .base = {
2035 pxms_flg:
2036   PXMS_ENI_m,
2037 pxms_do_inp:
2038   pxmc_inp_rocon_inp,
2039 pxms_do_con:
2040   pxmc_pid_con,
2041 pxms_do_out:
2042   pxmc_rocon_pwm_dc_out,
2043   pxms_do_deb: 0,
2044   pxms_do_gen: 0,
2045 pxms_do_ap2hw:
2046   pxmc_inp_rocon_ap2hw,
2047   pxms_ap: 0, pxms_as: 0,
2048   pxms_rp: 55 * 256, pxms_rs: 0, pxms_subdiv: 8,
2049   pxms_md: 800 << 8, pxms_ms: 500, pxms_ma: 10,
2050   pxms_inp_info: 7,
2051   pxms_out_info: 14,
2052   pxms_ene: 0, pxms_erc: 0,
2053   pxms_p: 80, pxms_i: 10, pxms_d: 200, pxms_s1: 200, pxms_s2: 0,
2054   pxms_me: 0x7e00/*0x7fff*/,
2055 pxms_cfg:
2056   PXMS_CFG_SMTH_m | PXMS_CFG_MD2E_m | PXMS_CFG_HLS_m | PXMS_CFG_I2PT_m * 0 |
2057   0x1,
2058
2059   pxms_ptper: 1,
2060   pxms_ptirc: 1000,
2061   /*pxms_ptamp: 0x7fff,*/
2062
2063   pxms_hal: 0x40,
2064 }};
2065
2066 pxmc_state_t *pxmc_main_arr[PXML_MAIN_CNT] =
2067 {&mcs0.base, &mcs1.base, &mcs2.base, &mcs3.base,
2068  &mcs4.base, &mcs5.base, &mcs6.base, &mcs7.base};
2069
2070
2071 pxmc_state_list_t  pxmc_main_list =
2072 {
2073 pxml_arr:
2074   pxmc_main_arr,
2075   pxml_cnt: 0
2076 };
2077
2078 void pxmc_lpc_qei_callback_index(struct lpc_qei_state_t *qst, void *context)
2079 {
2080   pxmc_state_t *mcs = (pxmc_state_t *)context;
2081   short ofs;
2082   short irc;
2083   irc = qst->index_pos;
2084
2085   if ((mcs->pxms_cfg & PXMS_CFG_I2PT_m) && (mcs->pxms_flg & PXMS_PTI_m))
2086   {
2087     short diff;
2088     ofs = irc - mcs->pxms_ptmark;
2089     diff = ofs - mcs->pxms_ptofs;
2090
2091     if (diff >= mcs->pxms_ptirc / 2)
2092       diff -= mcs->pxms_ptirc;
2093
2094     if (diff <= -mcs->pxms_ptirc / 2)
2095       diff += mcs->pxms_ptirc;
2096
2097     if (diff < 0)
2098       diff = -diff;
2099
2100     if (diff >= mcs->pxms_ptirc / 6)
2101     {
2102       pxmc_set_errno(mcs, PXMS_E_I2PT_TOOBIG);
2103     }
2104     else
2105     {
2106       mcs->pxms_ptofs = ofs;
2107       pxmc_set_flag(mcs, PXMS_PHA_b);
2108     }
2109   } /*else {
2110
2111     ofs=irc-mcs->pxms_ptofs;
2112     if((unsigned short)ofs>=(unsigned short)mcs->pxms_ptirc) {
2113       if(ofs>0) {
2114         ofs-=mcs->pxms_ptirc;
2115       } else {
2116         ofs+=mcs->pxms_ptirc;
2117       }
2118     }
2119     mcs->pxms_ptmark=ofs;
2120   }*/
2121
2122   /*lpc_qei_set_cmpos(qst, 0, qst->index_pos-4000);*/
2123 }
2124
2125 int pxmc_lpc_pthalalign_callback(pxmc_state_t *mcs)
2126 {
2127   short ptofs;
2128   short ptmark;
2129   lpc_qei_state_t *qst = &lpc_qei_state;
2130   int irc = lpc_qei_get_pos(qst);
2131   int idx_rel;
2132
2133   if (!qst->index_occ)
2134     return 0;
2135
2136   idx_rel = qst->index_pos - irc;
2137   idx_rel %= mcs->pxms_ptirc;
2138
2139   if (idx_rel < 0)
2140     idx_rel += mcs->pxms_ptirc;
2141
2142   ptofs = irc - mcs->pxms_ptofs;
2143   ptmark = ptofs + idx_rel;
2144
2145   if ((unsigned short)ptmark >= mcs->pxms_ptirc)
2146   {
2147     if (ptmark > 0)
2148       ptmark -= mcs->pxms_ptirc;
2149     else
2150       ptmark += mcs->pxms_ptirc;
2151   }
2152
2153   if ((unsigned short)ptmark < mcs->pxms_ptirc)
2154   {
2155     mcs->pxms_ptmark = ptmark;
2156   }
2157
2158   return 0;
2159 }
2160
2161 int pxmc_lpc_pthalalign(pxmc_state_t *mcs, int periods)
2162 {
2163   int res;
2164   long r2acq;
2165   long spd;
2166   pxmc_call_t *fin_fnc = pxmc_lpc_pthalalign_callback;
2167   lpc_qei_state_t *qst = &lpc_qei_state;
2168
2169   mcs->pxms_cfg &= ~PXMS_CFG_I2PT_m;
2170   lpc_qei_setup_index_catch(qst);
2171
2172   r2acq = ((long)mcs->pxms_ptirc << PXMC_SUBDIV(mcs)) * periods;
2173   spd = 1 << PXMC_SUBDIV(mcs);
2174
2175   res = pxmc_pthalalign(mcs, r2acq, spd, fin_fnc);
2176
2177   return res;
2178 }
2179
2180 int pxmc_lpc_pthalalign_run(void)
2181 {
2182   return pxmc_lpc_pthalalign(pxmc_main_list.pxml_arr[0], 20);
2183 }
2184
2185 static inline void pxmc_sfi_input(void)
2186 {
2187   int var;
2188   pxmc_state_t *mcs;
2189   pxmc_for_each_mcs(var, mcs)
2190   {
2191     /* PXMS_ENI_m - check if input (IRC) update is enabled */
2192     if (mcs->pxms_flg & PXMS_ENI_m)
2193     {
2194       pxmc_call(mcs, mcs->pxms_do_inp);
2195     }
2196   }
2197 }
2198
2199 static inline void pxmc_sfi_controller_and_output(void)
2200 {
2201   int var;
2202   pxmc_state_t *mcs;
2203   pxmc_for_each_mcs(var, mcs)
2204   {
2205     /* PXMS_ENR_m - check if controller is enabled */
2206     if (mcs->pxms_flg & PXMS_ENR_m || mcs->pxms_flg & PXMS_ENO_m)
2207     {
2208
2209       /* If output only is enabled, we skip the controller */
2210       if (mcs->pxms_flg & PXMS_ENR_m)
2211       {
2212
2213         pxmc_call(mcs, mcs->pxms_do_con);
2214
2215         /* PXMS_ERR_m - if axis in error state */
2216         if (mcs->pxms_flg & PXMS_ERR_m)
2217           mcs->pxms_ene = 0;
2218       }
2219
2220       /* for bushless motors, it is necessary to call do_out
2221         even if the controller is not enabled and PWM should be provided. */
2222       pxmc_call(mcs, mcs->pxms_do_out);
2223     }
2224   }
2225 }
2226
2227 static inline void pxmc_sfi_generator(void)
2228 {
2229   int var;
2230   pxmc_state_t *mcs;
2231   pxmc_for_each_mcs(var, mcs)
2232   {
2233     /* PXMS_ENG_m - check if requested value (position) generator is enabled */
2234     if (mcs->pxms_flg & PXMS_ENG_m)
2235     {
2236       pxmc_call(mcs, mcs->pxms_do_gen);
2237     }
2238   }
2239 }
2240
2241 static inline void pxmc_sfi_dbg(void)
2242 {
2243   int var;
2244   pxmc_state_t *mcs;
2245   pxmc_for_each_mcs(var, mcs)
2246   {
2247     if (mcs->pxms_flg & PXMS_DBG_m)
2248     {
2249       pxmc_call(mcs, mcs->pxms_do_deb);
2250     }
2251   }
2252 }
2253
2254 int pxmc_rocon_pthalalign_callback(pxmc_state_t *mcs)
2255 {
2256   short ptofs;
2257   short ptmark;
2258   int inp_chan = mcs->pxms_inp_info;
2259   int idx_rel;
2260   long irc = fpga_irc[inp_chan]->count;
2261
2262   if (!pxmc_inp_rocon_is_index_edge(mcs))
2263     return 0;
2264
2265   idx_rel = fpga_irc[inp_chan]->count_index - irc;
2266   idx_rel %= mcs->pxms_ptirc;
2267   if(idx_rel < 0)
2268     idx_rel += mcs->pxms_ptirc;
2269
2270   ptofs = irc-mcs->pxms_ptofs;
2271   ptmark = ptofs+idx_rel;
2272
2273   if((unsigned short)ptmark >= mcs->pxms_ptirc) {
2274     if(ptmark>0)
2275       ptmark -= mcs->pxms_ptirc;
2276     else
2277       ptmark += mcs->pxms_ptirc;
2278   }
2279
2280   if((unsigned short)ptmark < mcs->pxms_ptirc) {
2281     mcs->pxms_ptmark = ptmark;
2282   }
2283   return 0;
2284 }
2285
2286 int pxmc_rocon_pthalalign(pxmc_state_t *mcs, int periods)
2287 {
2288   int res;
2289   long r2acq;
2290   long spd;
2291   pxmc_call_t *fin_fnc = pxmc_rocon_pthalalign_callback;
2292
2293   mcs->pxms_cfg&=~PXMS_CFG_I2PT_m;
2294
2295   r2acq=((long)mcs->pxms_ptirc<<PXMC_SUBDIV(mcs))*periods;
2296   spd=1<<PXMC_SUBDIV(mcs);
2297
2298   /* clear bit indicating previous index */
2299   pxmc_inp_rocon_is_index_edge(mcs);
2300
2301   res=pxmc_pthalalign(mcs, r2acq, spd, fin_fnc);
2302
2303   return res;
2304 }
2305
2306 int pxmc_axis_out_chans4mode(int mode)
2307 {
2308   switch (mode) {
2309     case PXMC_AXIS_MODE_DC:
2310       return 2;
2311     case PXMC_AXIS_MODE_BLDC:
2312     case PXMC_AXIS_MODE_BLDC_PXMCC:
2313       return 3;
2314     case PXMC_AXIS_MODE_STEPPER_WITH_IRC:
2315     case PXMC_AXIS_MODE_STEPPER_WITH_IRC_PXMCC:
2316     case PXMC_AXIS_MODE_STEPPER_PXMCC:
2317       return 4;
2318   }
2319   return -1;
2320 }
2321
2322 int pxmc_axis_rdmode(pxmc_state_t *mcs)
2323 {
2324   if (mcs->pxms_do_out == pxmc_rocon_pwm2ph_out)
2325     return PXMC_AXIS_MODE_STEPPER_WITH_IRC;
2326   if (mcs->pxms_do_out == pxmc_rocon_pwm_dc_out)
2327     return PXMC_AXIS_MODE_DC;
2328   if (mcs->pxms_do_out == pxmc_rocon_pwm3ph_out)
2329     return PXMC_AXIS_MODE_BLDC;
2330   if (mcs->pxms_do_out == pxmc_pxmcc_pwm3ph_out)
2331     return PXMC_AXIS_MODE_BLDC_PXMCC;
2332   if (mcs->pxms_do_out == pxmc_pxmcc_pwm2ph_out)
2333     return PXMC_AXIS_MODE_STEPPER_WITH_IRC_PXMCC;
2334   if (mcs->pxms_do_out == pxmc_pxmcc_nofb2ph_out)
2335     return PXMC_AXIS_MODE_STEPPER_PXMCC;
2336   return -1;
2337 }
2338
2339 int
2340 pxmc_axis_pt4mode(pxmc_state_t *mcs, int mode)
2341 {
2342   static const typeof(*mcs->pxms_ptptr1) dummy0 = 0;
2343   int res = 0;
2344
2345   if (mode == PXMC_AXIS_MODE_NOCHANGE)
2346     mode = pxmc_axis_rdmode(mcs);
2347   if (mode < 0)
2348     return -1;
2349
2350   switch (mode) {
2351     /* case PXMC_AXIS_MODE_STEPPER: */
2352     case PXMC_AXIS_MODE_STEPPER_PXMCC:
2353     case PXMC_AXIS_MODE_STEPPER_WITH_IRC:
2354     case PXMC_AXIS_MODE_STEPPER_WITH_IRC_PXMCC:
2355       res = pxmc_ptable_set_profile(mcs, &pxmc_ptprofile_sin, 0, 0);
2356       break;
2357     /*case PXMC_AXIS_MODE_STEPPER_WITH_PWM:*/
2358     case PXMC_AXIS_MODE_DC:
2359       /*profive some sane dummy values*/
2360       mcs->pxms_ptptr1 = (typeof(mcs->pxms_ptptr1))&dummy0;
2361       mcs->pxms_ptptr2 = (typeof(mcs->pxms_ptptr1))&dummy0;
2362       mcs->pxms_ptptr3 = (typeof(mcs->pxms_ptptr1))&dummy0;
2363
2364       mcs->pxms_ptscale_mult=1;
2365       mcs->pxms_ptscale_shift=15;
2366       break;
2367     case PXMC_AXIS_MODE_BLDC:
2368     case PXMC_AXIS_MODE_BLDC_PXMCC:
2369       /* res = pxmc_init_ptable(mcs, PXMC_PTPROF_SIN3FUP); */
2370      #ifndef PXMC_WITH_PT_ZIC
2371       res = pxmc_ptable_set_profile(mcs, &pxmc_ptprofile_sin3phup, 0, 0);
2372      #else /*PXMC_WITH_PT_ZIC*/
2373       res = pxmc_ptable_set_profile(mcs, &pxmc_ptprofile_sin3phup_zic, 0, 0);
2374      #endif /*PXMC_WITH_PT_ZIC*/
2375       break;
2376     default:
2377       return -1;
2378   }
2379
2380   mcs->pxms_ptvang = pxmc_ptvang_deg2irc(mcs, 90);
2381
2382   return res;
2383 }
2384
2385 /**
2386  * pxmc_axis_mode - Sets axis mode.[extern API]
2387  * @mcs:        Motion controller state information
2388  * @mode:       0 .. previous mode, 1 .. stepper motor mode,
2389  *              2 .. stepper motor with IRC feedback and PWM ,
2390  *              3 .. stepper motor with PWM control
2391  *              4 .. DC motor with IRC feedback and PWM
2392  *
2393  */
2394 int
2395 pxmc_axis_mode(pxmc_state_t *mcs, int mode)
2396 {
2397   int res;
2398   int prev_mode;
2399
2400   pxmc_axis_release(mcs);
2401   pxmc_clear_flag(mcs, PXMS_ENI_b);
2402   pxmc_clear_flags(mcs,PXMS_ENO_m);
2403   /* Clear possible stall index flags from hardware */
2404   pxmc_inp_rocon_is_index_edge(mcs);
2405   pxmc_clear_flag(mcs, PXMS_PHA_b);
2406   pxmc_clear_flag(mcs, PXMS_PTI_b);
2407
2408
2409   prev_mode = pxmc_axis_rdmode(mcs);
2410
2411   if (mode == PXMC_AXIS_MODE_NOCHANGE)
2412     mode = prev_mode;
2413   if (mode < 0)
2414     return -1;
2415   if (!mode)
2416     mode = PXMC_AXIS_MODE_DC;
2417
2418   if ((prev_mode == PXMC_AXIS_MODE_BLDC_PXMCC) ||
2419       (prev_mode == PXMC_AXIS_MODE_STEPPER_WITH_IRC_PXMCC) ||
2420       (prev_mode == PXMC_AXIS_MODE_STEPPER_PXMCC))
2421     pxmcc_axis_setup(mcs, PXMCC_MODE_IDLE);
2422
2423   res = pxmc_axis_pt4mode(mcs, mode);
2424   if (res < 0)
2425     return -1;
2426
2427   if (mcs->pxms_do_inp == pxmc_pxmcc_nofb2ph_inp)
2428     mcs->pxms_do_inp = pxmc_inp_rocon_inp;
2429   if (mcs->pxms_do_con == pxmc_pxmcc_nofb_con)
2430     mcs->pxms_do_con = pxmc_pid_con;
2431
2432   switch (mode) {
2433     /*case PXMC_AXIS_MODE_STEPPER:*/
2434     case PXMC_AXIS_MODE_STEPPER_WITH_IRC:
2435       mcs->pxms_do_out = pxmc_rocon_pwm2ph_out;
2436       break;
2437     /*case PXMC_AXIS_MODE_STEPPER_WITH_PWM:*/
2438     case PXMC_AXIS_MODE_DC:
2439       mcs->pxms_do_out = pxmc_rocon_pwm_dc_out;
2440       break;
2441     case PXMC_AXIS_MODE_BLDC:
2442       mcs->pxms_do_out = pxmc_rocon_pwm3ph_out;
2443       break;
2444     case PXMC_AXIS_MODE_BLDC_PXMCC:
2445       if (pxmcc_axis_setup(mcs, PXMCC_MODE_BLDC) < 0)
2446         return -1;
2447       pxmcc_axis_enable(mcs, 1);
2448       mcs->pxms_do_out = pxmc_pxmcc_pwm3ph_out;
2449       break;
2450     case PXMC_AXIS_MODE_STEPPER_WITH_IRC_PXMCC:
2451       if (pxmcc_axis_setup(mcs, PXMCC_MODE_STEPPER_WITH_IRC) < 0)
2452         return -1;
2453       pxmcc_axis_enable(mcs, 1);
2454       mcs->pxms_do_out = pxmc_pxmcc_pwm2ph_out;
2455       break;
2456     case PXMC_AXIS_MODE_STEPPER_PXMCC:
2457       if (pxmcc_axis_setup(mcs, PXMCC_MODE_STEPPER) < 0)
2458         return -1;
2459       pxmcc_axis_enable(mcs, 1);
2460       mcs->pxms_do_inp = pxmc_pxmcc_nofb2ph_inp;
2461       mcs->pxms_do_con = pxmc_pxmcc_nofb_con;
2462       mcs->pxms_do_out = pxmc_pxmcc_nofb2ph_out;
2463       break;
2464     default:
2465       return -1;
2466   }
2467
2468   /* Clear possible stall index flags from hardware */
2469   pxmc_inp_rocon_is_index_edge(mcs);
2470   /* Request new phases alignment for changed parameters */
2471   pxmc_clear_flag(mcs, PXMS_PHA_b);
2472   pxmc_clear_flag(mcs, PXMS_PTI_b);
2473   pxmc_set_flag(mcs, PXMS_ENI_b);
2474   return res;
2475 }
2476
2477 void pxmc_sfi_isr(void)
2478 {
2479   unsigned long spent = pxmc_fast_tick_time();
2480
2481   pxmc_sfi_input();
2482   pxmc_sfi_controller_and_output();
2483   pxmc_sfi_generator();
2484   pxmc_sfi_dbg();
2485   /* Kick LX Master watchdog */
2486   if (pxmc_main_list.pxml_cnt != 0)
2487     *fpga_lx_master_transmitter_wdog = 1;
2488
2489   spent = pxmc_fast_tick_time() - spent;
2490
2491   if(spent > pxmc_sfi_spent_time_max)
2492     pxmc_sfi_spent_time_max = spent;
2493
2494 }
2495
2496 int pxmc_clear_power_stop(void)
2497 {
2498   return 0;
2499 }
2500
2501 int pxmc_process_state_check(unsigned long *pbusy_bits,
2502                              unsigned long *perror_bits)
2503 {
2504   unsigned short flg;
2505   unsigned short chan;
2506   unsigned long busy_bits = 0;
2507   unsigned long error_bits = 0;
2508   pxmc_state_t *mcs;
2509   flg=0;
2510   pxmc_for_each_mcs(chan, mcs) {
2511     if(mcs) {
2512       flg |= mcs->pxms_flg;
2513       if (mcs->pxms_flg & PXMS_BSY_m)
2514         busy_bits |= 1 << chan;
2515       if (mcs->pxms_flg & PXMS_ERR_m)
2516         error_bits |= 1 << chan;
2517     }
2518   }
2519   if (appl_errstop_mode) {
2520     if((flg & PXMS_ENG_m) && (flg & PXMS_ERR_m)) {
2521       pxmc_for_each_mcs(chan, mcs) {
2522         if(mcs&&(mcs->pxms_flg & PXMS_ENG_m)) {
2523           pxmc_stop(mcs, 0);
2524         }
2525       }
2526     }
2527   }
2528
2529   if (pbusy_bits != NULL)
2530     *pbusy_bits = busy_bits;
2531   if (error_bits != NULL)
2532     *perror_bits = error_bits;
2533
2534   return flg;
2535 }
2536
2537 int pxmc_done(void)
2538 {
2539   int var;
2540   pxmc_state_t *mcs;
2541
2542   if (!pxmc_main_list.pxml_cnt)
2543     return 0;
2544
2545   pxmc_for_each_mcs(var, mcs)
2546   {
2547     pxmc_axis_release(mcs);
2548   }
2549
2550   pxmc_main_list.pxml_cnt = 0;
2551   __memory_barrier();
2552
2553   return 0;
2554 }
2555
2556 int pxmc_initialize(void)
2557 {
2558   int res;
2559   int i;
2560
2561   pxmc_main_list.pxml_cnt = 0;
2562   pxmc_dbg_hist = NULL;
2563  #ifdef PXMC_ROCON_TIMED_BY_RX_DONE
2564   disable_irq(ROCON_RX_IRQn);
2565  #endif /*PXMC_ROCON_TIMED_BY_RX_DONE*/
2566   __memory_barrier();
2567
2568   pxmc_state_t *mcs = &mcs0.base;
2569   lpc_qei_state_t *qst = &lpc_qei_state;
2570
2571   *fpga_irc_reset = 1;
2572
2573   for (i = 0; i < 8; i++) {
2574     fpga_irc[i]->count = 0;
2575     fpga_irc[i]->count_index = 0;
2576     *fpga_irc_state[i] = FPGA_IRC_STATE_INDEX_EVENT_MASK;
2577   }
2578
2579   /* Initialize QEI module for IRC counting */
2580   pxmc_inp_lpc_qei_init(mcs->pxms_inp_info);
2581
2582   /* Initialize QEI events processing */
2583   if (lpc_qei_setup_irq(qst) < 0)
2584     return -1;
2585
2586   *fpga_irc_reset = 0;
2587
2588   //res = pxmc_axis_pt4mode(mcs, PXMC_AXIS_MODE_BLDC);
2589
2590   /*pxmc_ctm4pwm3f_wr(mcs, 0, 0, 0);*/
2591   //pxmc_rocon_pwm3ph_wr(mcs, 0, 0, 0);
2592
2593   res = pxmc_rocon_pwm_master_init();
2594   if (res < 0)
2595     return -1;
2596
2597  #ifdef PXMC_ROCON_TIMED_BY_RX_DONE
2598   pxmc_rocon_rx_done_isr_setup(pxmc_rocon_rx_done_isr);
2599  #endif /*PXMC_ROCON_TIMED_BY_RX_DONE*/
2600
2601   __memory_barrier();
2602   pxmc_main_list.pxml_cnt = PXML_MAIN_CNT;
2603
2604   return 0;
2605 }