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