sw/app/rocon/appl_pxmc.c

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