sw/app/rocon/appl_pxmc.c

   1 /*******************************************************************
   2   Motion and Robotic System (MARS) aplication components.
   3
   4   appl_pxmc.c - position controller subsystem core generic
   5                 and LP_MPW1 hardware specific support
   6
   7   Copyright (C) 2001-2013 by Pavel Pisa - originator
   8                           pisa@cmp.felk.cvut.cz
   9             (C) 2001-2013 by PiKRON Ltd. - originator
  10                     http://www.pikron.com
  11
  12   This file can be used and copied according to next
  13   license alternatives
  14    - GPL - GNU Public License
  15    - other license provided by project originators
  16
  17  *******************************************************************/
  18
  19 #include <cpu_def.h>
  20 #include <system_def.h>
  21 #include <pxmc.h>
  22 #include <pxmc_lpc_qei.h>
  23 #include <pxmc_internal.h>
  24 #include <pxmc_inp_common.h>
  25 #include <pxmc_gen_info.h>
  26 #include <hal_gpio.h>
  27 #include <hal_machperiph.h>
  28 #include <stdlib.h>
  29 #include <string.h>
  30 #include <LPC17xx.h>
  31 #include <lpcTIM.h>
  32
  33 #include "appl_defs.h"
  34 #include "appl_fpga.h"
  35
  36 int pxmc_ptofs_from_index(pxmc_state_t *mcs, unsigned long irc,
  37                            unsigned long index_irc, int diff2err);
  38
  39 #ifndef pxmc_fast_tick_time
  40 #define pxmc_fast_tick_time() (LPC_TIM0->TC)
  41 #endif
  42
  43 #define PXML_MAIN_CNT 8
  44
  45 #define PXMC_WITH_PT_ZIC 1
  46 #define PXMC_PT_ZIC_MASK 0x8000
  47
  48 #define LPCPWM1_FREQ 20000
  49
  50 #define HAL_ERR_SENSITIVITY 20
  51 #define HAL_ERR_MAX_COUNT    5
  52
  53 #define LXPWR_WITH_SIROLADC  1
  54
  55 #define LX_MASTER_DATA_OFFS  8
  56
  57 unsigned pxmc_rocon_pwm_magnitude = 2500;
  58
  59 long pxmc_rocon_irc_offset[PXML_MAIN_CNT];
  60 unsigned pxmc_rocon_mark_filt[PXML_MAIN_CNT];
  61
  62 const uint8_t onesin10bits[1024]={
  63   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,
  64   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,
  65   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,
  66   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,
  67   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,
  68   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,
  69   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,
  70   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,
  71   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,
  72   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,
  73   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,
  74   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,
  75   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,
  76   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,
  77   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,
  78   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,
  79   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,
  80   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,
  81   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,
  82   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,
  83   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,
  84   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,
  85   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,
  86   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,
  87   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,
  88   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,
  89   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,
  90   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,
  91   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,
  92   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,
  93   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,
  94   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
  95 };
  96
  97 int
  98 pxmc_inp_rocon_inp(struct pxmc_state *mcs)
  99 {
 100   int chan=mcs->pxms_inp_info;
 101   long irc;
 102   long pos;
 103
 104   /* read position from hardware */
 105   irc = fpga_irc[chan]->count;
 106   irc += pxmc_rocon_irc_offset[chan];
 107   pos = irc << PXMC_SUBDIV(mcs);
 108   mcs->pxms_as = pos - mcs->pxms_ap;
 109   mcs->pxms_ap = pos;
 110
 111   /* Running of the motor commutator */
 112   if (mcs->pxms_flg & PXMS_PTI_m)
 113     pxmc_irc_16bit_commindx(mcs, irc);
 114
 115   return 0;
 116 }
 117
 118 int
 119 pxmc_inp_rocon_is_mark(pxmc_state_t *mcs)
 120 {
 121   uint32_t irc_state;
 122   int mark;
 123   unsigned filt;
 124   int chan=mcs->pxms_inp_info;
 125
 126   irc_state = *fpga_irc_state[chan];
 127
 128   mark = ((irc_state >> (ffs(FPGA_IRC_STATE_MARK_MASK) - 1)) ^
 129          (mcs->pxms_cfg >> PXMS_CFG_HPS_b)) & 1;
 130
 131   filt = pxmc_rocon_mark_filt[chan];
 132   filt = (filt << 1) | mark;
 133   pxmc_rocon_mark_filt[chan] = filt;
 134
 135   return onesin10bits[filt & 0x03ff];
 136 }
 137
 138 int
 139 pxmc_inp_rocon_is_index_edge(pxmc_state_t *mcs)
 140 {
 141   uint32_t irc_state;
 142   int index;
 143   int chan=mcs->pxms_inp_info;
 144
 145   irc_state = *fpga_irc_state[chan];
 146   *fpga_irc_state[chan] = FPGA_IRC_STATE_INDEX_EVENT_MASK;
 147
 148   index = (irc_state >> (ffs(FPGA_IRC_STATE_INDEX_EVENT_MASK) - 1)) & 1;
 149
 150   return index;
 151 }
 152
 153 int
 154 pxmc_inp_rocon_adjust_to_irc_change(struct pxmc_state *mcs, long irc_diff)
 155 {
 156   long pos_diff = irc_diff << PXMC_SUBDIV(mcs);
 157
 158   mcs->pxms_ptofs += irc_diff;
 159
 160   mcs->pxms_ap += pos_diff;
 161   mcs->pxms_rp += pos_diff;
 162   return 0;
 163 }
 164
 165 int
 166 pxmc_inp_rocon_irc_offset_from_index(struct pxmc_state *mcs)
 167 {
 168   int chan=mcs->pxms_inp_info;
 169   long irc_offset;
 170   long irc_diff;
 171
 172   irc_offset = -fpga_irc[chan]->count_index;
 173   irc_diff = irc_offset - pxmc_rocon_irc_offset[chan];
 174   pxmc_rocon_irc_offset[chan] = irc_offset;
 175   return pxmc_inp_rocon_adjust_to_irc_change(mcs, irc_diff);
 176 }
 177
 178 int
 179 pxmc_inp_rocon_ap_zero(struct pxmc_state *mcs)
 180 {
 181   int chan=mcs->pxms_inp_info;
 182   long irc_offset;
 183   long irc_diff;
 184
 185   irc_offset = -fpga_irc[chan]->count;
 186   irc_diff = irc_offset - pxmc_rocon_irc_offset[chan];
 187   pxmc_rocon_irc_offset[chan] = irc_offset;
 188   return pxmc_inp_rocon_adjust_to_irc_change(mcs, irc_diff);
 189 }
 190
 191 int
 192 pxmc_inp_rocon_ap2hw(struct pxmc_state *mcs)
 193 {
 194   int chan=mcs->pxms_inp_info;
 195   long irc;
 196   long pos_diff;
 197
 198   irc = fpga_irc[chan]->count;
 199   pos_diff = mcs->pxms_ap - (irc << PXMC_SUBDIV(mcs));
 200
 201   irc = pos_diff >> PXMC_SUBDIV(mcs);
 202
 203   /* Adjust phase table alignemt to modified IRC readout  */
 204   mcs->pxms_ptofs += irc - pxmc_rocon_irc_offset[chan];
 205
 206   pxmc_rocon_irc_offset[chan] = irc;
 207   return 0;
 208 }
 209
 210 int
 211 pxmc_inp_rocon_ptofs_from_index_poll(struct pxmc_state *mcs, int diff2err)
 212 {
 213   int chan=mcs->pxms_inp_info;
 214   long irc;
 215   long index_irc;
 216
 217   if (!(*fpga_irc_state[chan] & FPGA_IRC_STATE_INDEX_EVENT_MASK))
 218     return 0;
 219
 220   irc = fpga_irc[chan]->count + pxmc_rocon_irc_offset[chan];
 221   index_irc = fpga_irc[chan]->count_index + pxmc_rocon_irc_offset[chan];
 222
 223   return pxmc_ptofs_from_index(mcs, irc, index_irc, diff2err);
 224 }
 225
 226 uint32_t pxmc_rocon_receiver_dummy_reg;
 227
 228 static inline volatile uint32_t *
 229 pxmc_rocon_receiver_chan2reg(unsigned chan)
 230 {
 231   volatile uint32_t *rec_reg;
 232
 233   if (chan >= 16)
 234     return &pxmc_rocon_receiver_dummy_reg;
 235
 236   rec_reg = fpga_lx_master_receiver_base;
 237
 238  #ifdef LXPWR_WITH_SIROLADC
 239   rec_reg += LX_MASTER_DATA_OFFS + 1 + (chan >> 3) * 3 + chan * 2;
 240  #else /*LXPWR_WITH_SIROLADC*/
 241   rec_reg += LX_MASTER_DATA_OFFS + chan;
 242  #endif /*LXPWR_WITH_SIROLADC*/
 243
 244   return rec_reg;
 245 }
 246
 247 inline unsigned
 248 pxmc_rocon_bldc_hal_rd(pxmc_state_t *mcs)
 249 {
 250   unsigned h = 0;
 251   volatile uint32_t *rec_reg_a, *rec_reg_b, *rec_reg_c;
 252   int chan = mcs->pxms_out_info;
 253   int hal_offs;
 254
 255  #ifdef LXPWR_WITH_SIROLADC
 256   hal_offs = 1;
 257  #else /*LXPWR_WITH_SIROLADC*/
 258   hal_offs = 0;
 259  #endif /*LXPWR_WITH_SIROLADC*/
 260
 261   rec_reg_a = pxmc_rocon_receiver_chan2reg(chan + 0);
 262   rec_reg_b = pxmc_rocon_receiver_chan2reg(chan + 1);
 263   rec_reg_c = pxmc_rocon_receiver_chan2reg(chan + 2);
 264
 265   h  = (rec_reg_a[hal_offs] >> 14) & 1;
 266   h |= (rec_reg_b[hal_offs] >> 13) & 2;
 267   h |= (rec_reg_c[hal_offs] >> 12) & 4;
 268
 269   /* return 3 bits corresponding to the HAL senzor input */
 270   return h;
 271 }
 272
 273 #if 1
 274 const unsigned char pxmc_lpc_bdc_hal_pos_table[8] =
 275 {
 276   [0] = 0xff,
 277   [7] = 0xff,
 278   [1] = 0, /*0*/
 279   [5] = 1, /*1*/
 280   [4] = 2, /*2*/
 281   [6] = 3, /*3*/
 282   [2] = 4, /*4*/
 283   [3] = 5, /*5*/
 284 };
 285 #else
 286 const unsigned char pxmc_lpc_bdc_hal_pos_table[8] =
 287 {
 288   [0] = 0xff,
 289   [7] = 0xff,
 290   [1] = 0, /*0*/
 291   [5] = 5, /*1*/
 292   [4] = 4, /*2*/
 293   [6] = 3, /*3*/
 294   [2] = 2, /*4*/
 295   [3] = 1, /*5*/
 296 };
 297 #endif
 298
 299 uint32_t pxmc_rocon_pwm_dummy_reg;
 300
 301 static inline volatile uint32_t *
 302 pxmc_rocon_pwm_chan2reg(unsigned chan)
 303 {
 304   volatile uint32_t *pwm_reg;
 305
 306   if (chan >= 16)
 307     return &pxmc_rocon_pwm_dummy_reg;
 308
 309   pwm_reg = fpga_lx_master_transmitter_base;
 310
 311  #ifdef LXPWR_WITH_SIROLADC
 312   pwm_reg += LX_MASTER_DATA_OFFS + 1 + (chan >> 3) + chan;
 313  #else /*LXPWR_WITH_SIROLADC*/
 314   pwm_reg += LX_MASTER_DATA_OFFS + chan;
 315  #endif /*LXPWR_WITH_SIROLADC*/
 316
 317   return pwm_reg;
 318 }
 319
 320 /**
 321  * pxmc_rocon_pwm3ph_wr - Output of the 3-phase PWM to the hardware
 322  * @mcs:  Motion controller state information
 323  */
 324 /*static*/ inline void
 325 pxmc_rocon_pwm3ph_wr(pxmc_state_t *mcs, short pwm1, short pwm2, short pwm3)
 326 {
 327   volatile uint32_t *pwm_reg_a, *pwm_reg_b, *pwm_reg_c;
 328   int chan = mcs->pxms_out_info;
 329
 330   pwm_reg_c = pxmc_rocon_pwm_chan2reg(chan + 0);
 331   pwm_reg_b = pxmc_rocon_pwm_chan2reg(chan + 1);
 332   pwm_reg_a = pxmc_rocon_pwm_chan2reg(chan + 2);
 333
 334   *pwm_reg_a = pwm1;
 335   *pwm_reg_b = pwm2;
 336   *pwm_reg_c = pwm3;
 337 }
 338
 339 static inline void
 340 pxmc_rocon_process_hal_error(struct pxmc_state *mcs)
 341 {
 342   if (mcs->pxms_halerc >= HAL_ERR_SENSITIVITY * HAL_ERR_MAX_COUNT)
 343   {
 344     pxmc_set_errno(mcs, PXMS_E_HAL);
 345     mcs->pxms_ene = 0;
 346     mcs->pxms_halerc--;
 347   }
 348   else
 349     mcs->pxms_halerc += HAL_ERR_SENSITIVITY;
 350 }
 351
 352 /**
 353  * pxmc_rocon_pwm3ph_out - Phase output for brush-less 3-phase motor
 354  * @mcs:  Motion controller state information
 355  */
 356 int
 357 pxmc_rocon_pwm3ph_out(pxmc_state_t *mcs)
 358 {
 359   typeof(mcs->pxms_ptvang) ptvang;
 360   int sync_mode = 0;
 361   unsigned char hal_pos;
 362   short pwm1;
 363   short pwm2;
 364   short pwm3;
 365   int indx = 0;
 366   short ene;
 367   int wind_current[4];
 368
 369   if (!(mcs->pxms_flg & PXMS_PTI_m) || !(mcs->pxms_flg & PXMS_PHA_m) ||
 370       (mcs->pxms_flg & PXMS_PRA_m))
 371   {
 372     short ptindx;
 373     short ptirc = mcs->pxms_ptirc;
 374     short divisor = mcs->pxms_ptper * 6;
 375
 376    #if 0
 377     pxmc_irc_16bit_commindx(mcs, mcs->pxms_rp >> PXMC_SUBDIV(mcs));
 378     sync_mode = 1;
 379    #elif 0
 380     {
 381       int res;
 382       res = pxmc_inp_rocon_ptofs_from_index_poll(mcs, 0);
 383       if (res < 0) {
 384         pxmc_set_errno(mcs, PXMS_E_I2PT_TOOBIG);
 385       } else if (res) {
 386         pxmc_set_flag(mcs, PXMS_PTI_b);
 387         pxmc_set_flag(mcs, PXMS_PHA_b);
 388       }
 389     }
 390    #else
 391
 392     hal_pos = pxmc_lpc_bdc_hal_pos_table[pxmc_rocon_bldc_hal_rd(mcs)];
 393
 394     if (hal_pos == 0xff)
 395     {
 396       if (mcs->pxms_ene)
 397         pxmc_rocon_process_hal_error(mcs);
 398     }
 399     else
 400     {
 401       if (mcs->pxms_halerc)
 402         mcs->pxms_halerc--;
 403
 404       ptindx = (hal_pos * ptirc + divisor / 2) / divisor;
 405
 406       if (!(mcs->pxms_flg & PXMS_PTI_m) || (mcs->pxms_flg & PXMS_PRA_m))
 407       {
 408         if (((hal_pos != mcs->pxms_hal) && (mcs->pxms_hal != 0x40)) && 1)
 409         {
 410           short ptindx_prev = (mcs->pxms_hal * ptirc + divisor / 2) / divisor;;
 411
 412           if ((ptindx > ptindx_prev + ptirc / 2) ||
 413               (ptindx_prev > ptindx + ptirc / 2))
 414           {
 415             ptindx = (ptindx_prev + ptindx - ptirc) / 2;
 416
 417             if (ptindx < 0)
 418               ptindx += ptirc;
 419           }
 420           else
 421           {
 422             ptindx = (ptindx_prev + ptindx) / 2;
 423           }
 424
 425           mcs->pxms_ptindx = ptindx;
 426
 427           mcs->pxms_ptofs = (mcs->pxms_ap >> PXMC_SUBDIV(mcs)) + mcs->pxms_ptshift - ptindx;
 428
 429           pxmc_set_flag(mcs, PXMS_PTI_b);
 430           pxmc_clear_flag(mcs, PXMS_PRA_b);
 431         }
 432         else
 433         {
 434           if (!(mcs->pxms_flg & PXMS_PTI_m))
 435             mcs->pxms_ptindx = ptindx;
 436         }
 437       } else {
 438         /* if phase table position to mask is know do fine phase table alignment */
 439         if (mcs->pxms_cfg & PXMS_CFG_I2PT_m) {
 440           int res;
 441           res = pxmc_inp_rocon_ptofs_from_index_poll(mcs, 0);
 442           if (res < 0) {
 443             pxmc_set_errno(mcs, PXMS_E_I2PT_TOOBIG);
 444           } else if (res) {
 445             pxmc_set_flag(mcs, PXMS_PTI_b);
 446             pxmc_set_flag(mcs, PXMS_PHA_b);
 447           }
 448         }
 449       }
 450       mcs->pxms_hal = hal_pos;
 451     }
 452    #endif
 453   }
 454
 455   {
 456     /*wind_current[0]=(ADC->ADDR0 & 0xFFF0)>>4;*/
 457     /* FIXME - check winding current against limit */
 458     /* pxmc_set_errno(mcs, PXMS_E_WINDCURRENT); */
 459   }
 460
 461   if (!sync_mode) {
 462     ptvang = mcs->pxms_ptvang;
 463     ene = mcs->pxms_ene;
 464   } else {
 465     ptvang = 0;
 466     ene = mcs->pxms_flg & PXMS_BSY_m? mcs->pxms_me: 0;
 467   }
 468
 469   if (ene) {
 470     indx = mcs->pxms_ptindx;
 471 #if 1
 472     /* tuning of magnetic field/voltage advance angle */
 473     indx += (mcs->pxms_s1 * mcs->pxms_as) >> (PXMC_SUBDIV(mcs) + 8);
 474 #endif
 475
 476     if (ene < 0)
 477     {
 478       /* Generating direction of stator mag. field for backward torque */
 479       ene = -ene;
 480
 481       if ((indx -= ptvang) < 0)
 482         indx += mcs->pxms_ptirc;
 483     }
 484     else
 485     {
 486       /* Generating direction of stator mag. field for forward torque */
 487       if ((indx += ptvang) >= mcs->pxms_ptirc)
 488         indx -= mcs->pxms_ptirc;
 489     }
 490
 491     if (mcs->pxms_ptscale_mult)
 492       indx = ((unsigned long)indx * mcs->pxms_ptscale_mult) >> mcs->pxms_ptscale_shift;
 493
 494     pwm1 = mcs->pxms_ptptr1[indx];
 495     pwm2 = mcs->pxms_ptptr2[indx];
 496     pwm3 = mcs->pxms_ptptr3[indx];
 497
 498 #ifdef PXMC_WITH_PT_ZIC
 499     if (labs(mcs->pxms_as) < (10 << PXMC_SUBDIV(mcs)))
 500     {
 501       pwm1 &= ~PXMC_PT_ZIC_MASK;
 502       pwm2 &= ~PXMC_PT_ZIC_MASK;
 503       pwm3 &= ~PXMC_PT_ZIC_MASK;
 504     }
 505 #endif /*PXMC_WITH_PT_ZIC*/
 506
 507     /* Default phase-table amplitude is 0x7fff, ene max is 0x7fff */
 508     /* Initialized CTM4 PWM period is 0x200 => divide by value about 2097024 */
 509     {
 510       unsigned long pwm_dc = pxmc_rocon_pwm_magnitude * (unsigned long)ene;
 511       if (pwm1 & PXMC_PT_ZIC_MASK)
 512         pwm1 = 0x8000;
 513       else
 514         pwm1 = (((unsigned long long)pwm1 * pwm_dc) >> (15 + 15)) | 0x4000;
 515       if (pwm2 & PXMC_PT_ZIC_MASK)
 516         pwm2 = 0x8000;
 517       else
 518         pwm2 = (((unsigned long long)pwm2 * pwm_dc) >> (15 + 15)) | 0x4000;
 519       if (pwm3 & PXMC_PT_ZIC_MASK)
 520         pwm3 = 0x8000;
 521       else
 522         pwm3 = (((unsigned long long)pwm3 * pwm_dc) >> (15 + 15)) | 0x4000;
 523     }
 524     pxmc_rocon_pwm3ph_wr(mcs, pwm1, pwm2, pwm3);
 525   }
 526   else
 527   {
 528     pxmc_rocon_pwm3ph_wr(mcs, 0, 0, 0);
 529   }
 530
 531   return 0;
 532 }
 533
 534 /**
 535  * pxmc_rocon_pwm2ph_wr - Output of the 2-phase stepper motor PWM to the hardware
 536  * @mcs:        Motion controller state information
 537  */
 538 /*static*/ inline void
 539 pxmc_rocon_pwm2ph_wr(pxmc_state_t *mcs, short pwm1, short pwm2)
 540 {
 541   volatile uint32_t *pwm_reg_ap, *pwm_reg_an, *pwm_reg_bp, *pwm_reg_bn;
 542   int chan = mcs->pxms_out_info;
 543
 544   pwm_reg_bn = pxmc_rocon_pwm_chan2reg(chan + 0);
 545   pwm_reg_bp = pxmc_rocon_pwm_chan2reg(chan + 1);
 546   pwm_reg_an = pxmc_rocon_pwm_chan2reg(chan + 2);
 547   pwm_reg_ap = pxmc_rocon_pwm_chan2reg(chan + 3);
 548
 549   if (pwm2 >= 0) {
 550     *pwm_reg_bp = pwm2 | 0x4000;
 551     *pwm_reg_bn = 0;
 552   } else {
 553     *pwm_reg_bp = 0;
 554     *pwm_reg_bn = -pwm2 | 0x4000;
 555   }
 556
 557   if (pwm1 >= 0) {
 558     *pwm_reg_ap = pwm1 | 0x4000;
 559     *pwm_reg_an = 0;
 560   } else {
 561     *pwm_reg_ap = 0;
 562     *pwm_reg_an = -pwm1 | 0x4000;
 563   }
 564 }
 565
 566 /**
 567  * pxmc_rocon_pwm2ph_out - Phase output of the 2-phase stepper motor PWM
 568  * @mcs:  Motion controller state information
 569  */
 570 int
 571 pxmc_rocon_pwm2ph_out(pxmc_state_t *mcs)
 572 {
 573   typeof(mcs->pxms_ptvang) ptvang;
 574   int sync_mode = 0;
 575   short pwm1;
 576   short pwm2;
 577   int indx = 0;
 578   short ene;
 579   int wind_current[4];
 580
 581   if(!(mcs->pxms_flg&PXMS_PTI_m) || !(mcs->pxms_flg&PXMS_PHA_m) ||
 582      (mcs->pxms_flg&PXMS_PRA_m)){
 583     short ptindx;
 584     short ptirc=mcs->pxms_ptirc;
 585
 586    #if 0
 587     pxmc_irc_16bit_commindx(mcs, mcs->pxms_rp >> PXMC_SUBDIV(mcs));
 588     sync_mode = 1;
 589    #elif 1
 590     {
 591       int res;
 592       res = pxmc_inp_rocon_ptofs_from_index_poll(mcs, 0);
 593       if (res < 0) {
 594         pxmc_set_errno(mcs, PXMS_E_I2PT_TOOBIG);
 595       } else if (res) {
 596         pxmc_set_flag(mcs, PXMS_PTI_b);
 597         pxmc_set_flag(mcs, PXMS_PHA_b);
 598       }
 599     }
 600    #else
 601
 602     ptindx = mcs->pxms_ptindx;
 603
 604     if(0 && (!(mcs->pxms_flg&PXMS_PTI_m) || (mcs->pxms_flg&PXMS_PRA_m))) {
 605
 606       mcs->pxms_ptindx = ptindx;
 607
 608       mcs->pxms_ptofs = (mcs->pxms_ap >> PXMC_SUBDIV(mcs)) + mcs->pxms_ptshift - ptindx;
 609
 610       pxmc_set_flag(mcs, PXMS_PTI_b);
 611       pxmc_clear_flag(mcs, PXMS_PRA_b);
 612
 613       /* if phase table position to mask is know do fine phase table alignment */
 614       //if(mcs->pxms_cfg & PXMS_CFG_I2PT_m)
 615       //  lpc_qei_setup_index_catch(&lpc_qei_state);
 616
 617     } else {
 618       if(!(mcs->pxms_flg&PXMS_PTI_m))
 619             mcs->pxms_ptindx = ptindx;
 620     }
 621    #endif
 622   }
 623
 624   {
 625     /*wind_current[0]=(ADC->ADDR0 & 0xFFF0)>>4;*/
 626     /* FIXME - check winding current against limit */
 627     /* pxmc_set_errno(mcs, PXMS_E_WINDCURRENT); */
 628   }
 629
 630   if (!sync_mode) {
 631     ptvang = mcs->pxms_ptvang;
 632     ene = mcs->pxms_ene;
 633   } else {
 634     ptvang = 0;
 635     ene = mcs->pxms_flg & PXMS_BSY_m? mcs->pxms_me: 0;
 636   }
 637
 638   if (ene) {
 639     indx = mcs->pxms_ptindx;
 640    #if 0
 641     /* tuning of magnetic field/voltage advance angle */
 642     indx += (mcs->pxms_s1 * mcs->pxms_as) >> (PXMC_SUBDIV(mcs) + 8);
 643    #endif
 644     if (ene<0){
 645       /* Generating direction of stator mag. field for backward torque */
 646       ene = -ene;
 647       if ((indx -= ptvang)<0)
 648         indx += mcs->pxms_ptirc;
 649     }else{
 650       /* Generating direction of stator mag. field for forward torque */
 651       if ((indx += ptvang) >= mcs->pxms_ptirc)
 652         indx -= mcs->pxms_ptirc;
 653     }
 654
 655     if (mcs->pxms_ptscale_mult)
 656       indx = ((unsigned long)indx * mcs->pxms_ptscale_mult) >> mcs->pxms_ptscale_shift;
 657
 658     pwm1 = mcs->pxms_ptptr1[indx];
 659     pwm2 = mcs->pxms_ptptr2[indx];
 660
 661     /* Default phase-table amplitude is 0x7fff, ene max is 0x7fff */
 662     /* Initialized PWM period is 0x200 => divide by value about 2097024 */
 663     {
 664       unsigned long pwm_dc = pxmc_rocon_pwm_magnitude * (unsigned long)ene;
 665       pwm1 = ((unsigned long long)pwm1 * pwm_dc) >> (15+15);
 666       pwm2 = ((unsigned long long)pwm2 * pwm_dc) >> (15+15);
 667     }
 668     pxmc_rocon_pwm2ph_wr(mcs, pwm1, pwm2);
 669   }else{
 670     /*pxmc_lpc_wind_current_over_cnt = 0;*/
 671     pxmc_rocon_pwm2ph_wr(mcs, 0, 0);
 672   }
 673
 674   return 0;
 675 }
 676
 677 /**
 678  * pxmc_rocon_pwm_dc_out - DC motor CW and CCW PWM output
 679  * @mcs:  Motion controller state information
 680  */
 681 int
 682 pxmc_rocon_pwm_dc_out(pxmc_state_t *mcs)
 683 {
 684   volatile uint32_t *pwm_reg_a, *pwm_reg_b;
 685   int chan = mcs->pxms_out_info;
 686   int ene = mcs->pxms_ene;
 687
 688   pwm_reg_a = pxmc_rocon_pwm_chan2reg(chan + 0);
 689   pwm_reg_b = pxmc_rocon_pwm_chan2reg(chan + 1);
 690
 691   if (ene < 0) {
 692     ene = -ene;
 693     if (ene > 0x7fff)
 694       ene = 0x7fff;
 695     ene = (ene * (pxmc_rocon_pwm_magnitude + 5)) >> 15;
 696     *pwm_reg_a = 0;
 697     *pwm_reg_b = ene | 0x4000;
 698   } else {
 699     if (ene > 0x7fff)
 700       ene = 0x7fff;
 701     ene = (ene * (pxmc_rocon_pwm_magnitude + 5)) >> 15;
 702     *pwm_reg_b = 0;
 703     *pwm_reg_a = ene | 0x4000;
 704   }
 705
 706   return 0;
 707 }
 708
 709 volatile void *pxmc_rocon_rx_data_hist_buff;
 710 volatile void *pxmc_rocon_rx_data_hist_buff_end;
 711 int pxmc_rocon_rx_data_hist_mode;
 712
 713 uint32_t pxmc_rocon_rx_last_irq;
 714 uint32_t pxmc_rocon_rx_cycle_time;
 715 uint32_t pxmc_rocon_rx_irq_latency;
 716 uint32_t pxmc_rocon_rx_irq_latency_max;
 717
 718 IRQ_HANDLER_FNC(pxmc_rocon_rx_done_isr)
 719 {
 720   uint32_t ir;
 721
 722   ir = ROCON_RX_TIM->IR & LPC_TIM_IR_ALL_m;
 723   ROCON_RX_TIM->IR = ir;
 724   if (ir & LPC_TIM_IR_CR1INT_m) {
 725     uint32_t cr0, cr1;
 726     cr0 = ROCON_RX_TIM->CR0;
 727     cr1 = ROCON_RX_TIM->CR1;
 728
 729     pxmc_rocon_rx_cycle_time = cr1 - pxmc_rocon_rx_last_irq;
 730     pxmc_rocon_rx_last_irq = cr1;
 731
 732     hal_gpio_set_value(T2MAT0_PIN, 1);
 733     hal_gpio_set_value(T2MAT1_PIN, 0);
 734     hal_gpio_set_value(T2MAT0_PIN, 0);
 735
 736     if (pxmc_rocon_rx_data_hist_buff >= pxmc_rocon_rx_data_hist_buff_end)
 737       pxmc_rocon_rx_data_hist_buff = NULL;
 738
 739     if (pxmc_rocon_rx_data_hist_buff != NULL) {
 740       if (pxmc_rocon_rx_data_hist_mode == 0) {
 741         int i;
 742         volatile uint32_t *pwm_reg = fpga_lx_master_transmitter_base + 8;
 743         volatile uint32_t *rec_reg = fpga_lx_master_receiver_base + 8;
 744         uint16_t *pbuf = (uint16_t *)pxmc_rocon_rx_data_hist_buff;
 745         for (i = 0; i < 8; i++) {
 746           *(pbuf++) = *(rec_reg++);
 747         }
 748         for (i = 0; i < 8; i++) {
 749           *(pbuf++) = *(pwm_reg++);
 750         }
 751         pxmc_rocon_rx_data_hist_buff = pbuf;
 752       } else if (pxmc_rocon_rx_data_hist_mode == 1) {
 753         int i;
 754         uint32_t *pbuf = (uint32_t *)pxmc_rocon_rx_data_hist_buff;
 755
 756         for (i = 0; i < 16; i++)
 757           *(pbuf++) = fpga_tumbl_dmem[i];
 758
 759         pxmc_rocon_rx_data_hist_buff = pbuf;
 760       }
 761     }
 762
 763     pxmc_rocon_rx_irq_latency = ROCON_RX_TIM->TC - cr1;
 764     if (pxmc_rocon_rx_irq_latency > pxmc_rocon_rx_irq_latency_max)
 765       pxmc_rocon_rx_irq_latency_max = pxmc_rocon_rx_irq_latency;
 766
 767    #ifdef PXMC_ROCON_TIMED_BY_RX_DONE
 768     pxmc_sfi_isr();
 769     do_pxmc_coordmv();
 770    #endif /*PXMC_ROCON_TIMED_BY_RX_DONE*/
 771   }
 772
 773   return IRQ_HANDLED;
 774 }
 775
 776 int
 777 pxmc_rocon_rx_done_isr_setup(irq_handler_t rx_done_isr_handler)
 778 {
 779
 780   disable_irq(ROCON_RX_IRQn);
 781
 782   hal_pin_conf_set(T2MAT0_PIN, PORT_CONF_GPIO_OUT_LO);
 783   hal_pin_conf_set(T2MAT1_PIN, PORT_CONF_GPIO_OUT_LO);
 784   hal_pin_conf(T2CAP0_PIN);
 785   hal_pin_conf(T2CAP1_PIN);
 786
 787   hal_gpio_direction_output(T2MAT0_PIN, 1);
 788   hal_gpio_direction_output(T2MAT1_PIN, 0);
 789   hal_gpio_set_value(T2MAT0_PIN, 0);
 790
 791   /* Enable CLKOUT pin function, source CCLK, divide by 1 */
 792   LPC_SC->CLKOUTCFG = 0x0100;
 793
 794   request_irq(ROCON_RX_IRQn, rx_done_isr_handler, 0, NULL,NULL);
 795
 796   ROCON_RX_TIM->TCR = 0;
 797   ROCON_RX_TIM->CTCR = 0;
 798   ROCON_RX_TIM->PR = 0; /* Divide by 1 */
 799
 800   ROCON_RX_TIM->CCR = LPC_TIM_CCR_CAP0RE_m | LPC_TIM_CCR_CAP1FE_m |
 801                    LPC_TIM_CCR_CAP1I_m;
 802
 803   ROCON_RX_TIM->EMR = __val2mfld(LPC_TIM_EMR_EMC0_m, LPC_TIM_EMR_NOP) |
 804                    __val2mfld(LPC_TIM_EMR_EMC1_m, LPC_TIM_EMR_NOP);
 805
 806   ROCON_RX_TIM->MCR = 0;                        /* No IRQ on MRx */
 807   ROCON_RX_TIM->TCR = LPC_TIM_TCR_CEN_m;        /* Enable timer counting */
 808   enable_irq(ROCON_RX_IRQn);            /* Enable interrupt */
 809
 810   return 0;
 811
 812 }
 813
 814 int
 815 pxmc_rocon_pwm_master_init(void)
 816 {
 817   int i;
 818   int grp_in = 0;
 819   int grp_out = 0;
 820   unsigned word_slot;
 821   unsigned receiver_done_div = 1;
 822  #ifdef LXPWR_WITH_SIROLADC
 823   unsigned lxpwr_header = 1;
 824   unsigned lxpwr_words = 1 + 8 * 2 + 2;
 825   unsigned lxpwr_chips = 2;
 826   unsigned lxpwr_chip_pwm_cnt = 8;
 827  #else /*LXPWR_WITH_SIROLADC*/
 828   unsigned lxpwr_header = 0;
 829   unsigned lxpwr_words = 8;
 830   unsigned lxpwr_chips = 2;
 831   unsigned lxpwr_chip_pwm_cnt = 8;
 832  #endif /*LXPWR_WITH_SIROLADC*/
 833
 834  #ifdef PXMC_ROCON_TIMED_BY_RX_DONE
 835   receiver_done_div = 5;
 836  #endif /*PXMC_ROCON_TIMED_BY_RX_DONE*/
 837
 838   *fpga_lx_master_reset = 1;
 839   *fpga_lx_master_transmitter_reg = 0;
 840   *fpga_lx_master_transmitter_cycle = 2500; /* 50 MHz -> 20 kHz */
 841   *fpga_lx_master_receiver_done_div = receiver_done_div << 8;
 842
 843   for (i = 0; i < LX_MASTER_DATA_OFFS + lxpwr_words * lxpwr_chips; i++)
 844     fpga_lx_master_receiver_base[i] = 0;
 845
 846   word_slot = LX_MASTER_DATA_OFFS;
 847   fpga_lx_master_receiver_base[grp_in++] = (word_slot << 8) | lxpwr_words;
 848   fpga_lx_master_receiver_base[grp_in++] = 0x0000;
 849
 850   for (i = 0; i < LX_MASTER_DATA_OFFS + lxpwr_words * lxpwr_chips; i++)
 851     fpga_lx_master_transmitter_base[i] = 0;
 852
 853   word_slot = LX_MASTER_DATA_OFFS + lxpwr_header + lxpwr_chip_pwm_cnt;
 854   fpga_lx_master_transmitter_base[grp_out++] = (word_slot << 8) | lxpwr_words;
 855  #ifdef LXPWR_WITH_SIROLADC
 856   fpga_lx_master_transmitter_base[word_slot] = 0xc100 | (lxpwr_words - 1);
 857  #endif /*LXPWR_WITH_SIROLADC*/
 858
 859   word_slot = LX_MASTER_DATA_OFFS + 0;
 860   fpga_lx_master_transmitter_base[grp_out++] = (word_slot << 8) | lxpwr_words;
 861  #ifdef LXPWR_WITH_SIROLADC
 862   fpga_lx_master_transmitter_base[word_slot] = 0xc100 | (lxpwr_words - 1);
 863  #endif /*LXPWR_WITH_SIROLADC*/
 864
 865   fpga_lx_master_transmitter_base[grp_out++] = 0x0000;
 866
 867   *fpga_lx_master_reset = 0;
 868   *fpga_lx_master_transmitter_cycle = 2500; /* 50 MHz -> 20 kHz */
 869   *fpga_lx_master_receiver_done_div = receiver_done_div << 8;
 870
 871   return 0;
 872 }
 873
 874 int pxmc_ptofs_from_index(pxmc_state_t *mcs, unsigned long irc,
 875                            unsigned long index_irc, int diff2err)
 876 {
 877   long ofsl;
 878   short ofs;
 879
 880   ofs = ofsl = index_irc - mcs->pxms_ptmark;
 881
 882   if (diff2err) {
 883     short diff;
 884     diff = (unsigned short)ofs - (unsigned short)mcs->pxms_ptofs;
 885     if (diff >= mcs->pxms_ptirc / 2)
 886       diff -= mcs->pxms_ptirc;
 887     if (diff <= -mcs->pxms_ptirc / 2)
 888       diff += mcs->pxms_ptirc;
 889     if (diff < 0)
 890       diff = -diff;
 891     if(diff >= mcs->pxms_ptirc / 6) {
 892       return -1;
 893     }
 894   } else {
 895     long diff;
 896     diff = (unsigned long)ofsl - irc;
 897     ofs = ofsl - (diff / mcs->pxms_ptirc) * mcs->pxms_ptirc;
 898   }
 899
 900   mcs->pxms_ptofs = ofs;
 901
 902   return 1;
 903 }
 904
 905 /**
 906  * pxmc_dummy_con - Dummy controller for synchronous BLDC/PMSM/steper drive
 907  * @mcs:        Motion controller state information
 908  */
 909 int
 910 pxmc_dummy_con(pxmc_state_t *mcs)
 911 {
 912   return 0;
 913 }
 914
 915 int pxmc_rocon_hh_gd10(pxmc_state_t *mcs);
 916 int pxmc_rocon_hh_gd20(pxmc_state_t *mcs);
 917 int pxmc_rocon_hh_gd30(pxmc_state_t *mcs);
 918 int pxmc_rocon_hh_gd40(pxmc_state_t *mcs);
 919 int pxmc_rocon_hh_gd50(pxmc_state_t *mcs);
 920 int pxmc_rocon_hh_gd90(pxmc_state_t *mcs);
 921
 922 /* initialize for hard home */
 923 int
 924 pxmc_rocon_hh_gi(pxmc_state_t *mcs)
 925 {
 926   pxmc_set_flag(mcs,PXMS_BSY_b);
 927  #if 0 /* config and speed already set in pxmc_hh */
 928   spd = mcs->pxms_ms;
 929   spd >>= mcs->pxms_cfg & PXMS_CFG_HSPD_m;
 930   if(!spd) spd = 1;
 931   if(mcs->pxms_cfg & PXMS_CFG_HDIR_m)
 932     spd = -spd;
 933   mcs->pxms_gen_hsp = spd;
 934  #endif
 935   mcs->pxms_rsfg = 0;
 936   mcs->pxms_gen_htim = 16;
 937   mcs->pxms_do_gen = pxmc_rocon_hh_gd10;
 938   return pxmc_rocon_hh_gd10(mcs);
 939 }
 940
 941 /* fill initial mark filter and then decide */
 942 int
 943 pxmc_rocon_hh_gd10(pxmc_state_t *mcs)
 944 {
 945   int mark;
 946
 947   if(mcs->pxms_flg & PXMS_ERR_m)
 948     return pxmc_spdnext_gend(mcs);
 949
 950   pxmc_spd_gacc(&(mcs->pxms_rs), 0, mcs->pxms_ma);
 951   mcs->pxms_rp += mcs->pxms_rs;
 952
 953   mark = pxmc_inp_rocon_is_mark(mcs);
 954
 955   if (mcs->pxms_gen_htim) {
 956     mcs->pxms_gen_htim--;
 957     return 0;
 958   }
 959
 960   if (!(mcs->pxms_cfg & PXMS_CFG_HLS_m)) {
 961     /* limit switch is not used */
 962     pxmc_inp_rocon_is_index_edge(mcs);
 963     mcs->pxms_do_gen = pxmc_rocon_hh_gd50;
 964     return 0;
 965   }
 966
 967   if (mark >= 6) {
 968     /* go out from switch */
 969     mcs->pxms_do_gen = pxmc_rocon_hh_gd20;
 970   } else {
 971     /* switch is off -> look for it */
 972     mcs->pxms_do_gen = pxmc_rocon_hh_gd30;
 973   }
 974   return 0;
 975 }
 976
 977 /* go out from switch */
 978 int
 979 pxmc_rocon_hh_gd20(pxmc_state_t *mcs)
 980 {
 981   int mark;
 982
 983   if(mcs->pxms_flg & PXMS_ERR_m)
 984     return pxmc_spdnext_gend(mcs);
 985
 986   mark = pxmc_inp_rocon_is_mark(mcs);
 987
 988   if (mark <= 1) {
 989     /* switch is off -> look for it again */
 990     mcs->pxms_do_gen = pxmc_rocon_hh_gd30;
 991   }
 992
 993   pxmc_spd_gacc(&(mcs->pxms_rs), -mcs->pxms_gen_hsp, mcs->pxms_ma);
 994   mcs->pxms_rp += mcs->pxms_rs;
 995
 996   return 0;
 997 }
 998
 999 /* switch is off -> look for it */
1000 int
1001 pxmc_rocon_hh_gd30(pxmc_state_t *mcs)
1002 {
1003   long spd;
1004   int mark;
1005
1006   if (mcs->pxms_flg & PXMS_ERR_m)
1007     return pxmc_spdnext_gend(mcs);
1008
1009   pxmc_spd_gacc(&(mcs->pxms_rs), mcs->pxms_gen_hsp, mcs->pxms_ma);
1010   mcs->pxms_rp += mcs->pxms_rs;
1011
1012   mark = pxmc_inp_rocon_is_mark(mcs);
1013
1014   if (mark >= 8){
1015     spd = mcs->pxms_gen_hsp >> 2; /* slow down */
1016     if (!spd)
1017       spd = 1;
1018     mcs->pxms_gen_hsp = spd;
1019     mcs->pxms_do_gen = pxmc_rocon_hh_gd40;
1020   }
1021
1022   return 0;
1023 }
1024
1025 /* switch is on -> find edge */
1026 int
1027 pxmc_rocon_hh_gd40(pxmc_state_t *mcs)
1028 {
1029   int mark;
1030
1031   if (mcs->pxms_flg & PXMS_ERR_m)
1032     return pxmc_spdnext_gend(mcs);
1033
1034   mark = pxmc_inp_rocon_is_mark(mcs);
1035
1036   if (mark <= 1) {
1037     if (mcs->pxms_cfg & PXMS_CFG_HRI_m) {
1038       pxmc_inp_rocon_is_index_edge(mcs);
1039       mcs->pxms_do_gen = pxmc_rocon_hh_gd50;
1040     } else {
1041       pxmc_inp_rocon_ap_zero(mcs);
1042       mcs->pxms_do_gen = pxmc_stop_gi;
1043     }
1044   }
1045
1046   pxmc_spd_gacc(&(mcs->pxms_rs), -mcs->pxms_gen_hsp, mcs->pxms_ma);
1047   mcs->pxms_rp += mcs->pxms_rs;
1048
1049   return 0;
1050 }
1051
1052 /* calibrate on revolution index */
1053 int
1054 pxmc_rocon_hh_gd50(pxmc_state_t *mcs)
1055 {
1056   if (mcs->pxms_flg & PXMS_ERR_m)
1057     return pxmc_spdnext_gend(mcs);
1058
1059   pxmc_spd_gacc(&(mcs->pxms_rs), mcs->pxms_gen_hsp, mcs->pxms_ma);
1060   mcs->pxms_rp += mcs->pxms_rs;
1061
1062   if (pxmc_inp_rocon_is_index_edge(mcs)){
1063     pxmc_inp_rocon_irc_offset_from_index(mcs);
1064     mcs->pxms_do_gen = pxmc_stop_gi;
1065   }
1066
1067   return 0;
1068 }
1069
1070 pxmc_call_t *pxmc_get_hh_gi_4axis(pxmc_state_t *mcs)
1071 {
1072   return pxmc_rocon_hh_gi;
1073 }
1074
1075 pxmc_state_t mcs0 =
1076 {
1077 pxms_flg:
1078   PXMS_ENI_m,
1079 pxms_do_inp:
1080   pxmc_inp_rocon_inp,
1081 pxms_do_con:
1082   pxmc_pid_con /*pxmc_dummy_con*/,
1083 pxms_do_out:
1084   pxmc_rocon_pwm_dc_out /*pxmc_rocon_pwm3ph_out*/,
1085   pxms_do_deb: 0,
1086   pxms_do_gen: 0,
1087 pxms_do_ap2hw:
1088   pxmc_inp_rocon_ap2hw,
1089   pxms_ap: 0, pxms_as: 0,
1090   pxms_rp: 55 * 256, pxms_rs: 0, pxms_subdiv: 8,
1091   pxms_md: 800 << 8, pxms_ms: 500, pxms_ma: 10,
1092   pxms_inp_info: 0,
1093   pxms_out_info: 0,
1094   pxms_ene: 0, pxms_erc: 0,
1095   pxms_p: 80, pxms_i: 10, pxms_d: 200, pxms_s1: 200, pxms_s2: 0,
1096   pxms_me: 0x7e00/*0x7fff*/,
1097 pxms_cfg:
1098   PXMS_CFG_SMTH_m | PXMS_CFG_MD2E_m | PXMS_CFG_HLS_m | PXMS_CFG_HPS_m * 0 |
1099   PXMS_CFG_HRI_m * 0 | PXMS_CFG_HDIR_m * 0 |
1100   PXMS_CFG_I2PT_m * 0 | 0x2,
1101
1102   pxms_ptper: 1,
1103   pxms_ptirc: 1000,
1104   pxms_ptmark: 1180,
1105   /*pxms_ptamp: 0x7fff,*/
1106
1107   pxms_hal: 0x40,
1108 };
1109
1110 pxmc_state_t mcs1 =
1111 {
1112 pxms_flg:
1113   PXMS_ENI_m,
1114 pxms_do_inp:
1115   pxmc_inp_rocon_inp,
1116 pxms_do_con:
1117   pxmc_pid_con,
1118 pxms_do_out:
1119   pxmc_rocon_pwm_dc_out,
1120   pxms_do_deb: 0,
1121   pxms_do_gen: 0,
1122 pxms_do_ap2hw:
1123   pxmc_inp_rocon_ap2hw,
1124   pxms_ap: 0, pxms_as: 0,
1125   pxms_rp: 55 * 256, pxms_rs: 0, pxms_subdiv: 8,
1126   pxms_md: 800 << 8, pxms_ms: 500, pxms_ma: 10,
1127   pxms_inp_info: 1,
1128   pxms_out_info: 2,
1129   pxms_ene: 0, pxms_erc: 0,
1130   pxms_p: 80, pxms_i: 10, pxms_d: 200, pxms_s1: 200, pxms_s2: 0,
1131   pxms_me: 0x7e00/*0x7fff*/,
1132 pxms_cfg:
1133   PXMS_CFG_SMTH_m | PXMS_CFG_MD2E_m | PXMS_CFG_HLS_m | PXMS_CFG_HPS_m * 0 |
1134   PXMS_CFG_HRI_m * 0 | PXMS_CFG_HDIR_m * 0 |
1135   PXMS_CFG_I2PT_m * 0 | 0x2,
1136
1137   pxms_ptper: 1,
1138   pxms_ptirc: 1000,
1139   /*pxms_ptamp: 0x7fff,*/
1140
1141   pxms_hal: 0x40,
1142 };
1143
1144 pxmc_state_t mcs2 =
1145 {
1146 pxms_flg:
1147   PXMS_ENI_m,
1148 pxms_do_inp:
1149   pxmc_inp_rocon_inp,
1150 pxms_do_con:
1151   pxmc_pid_con,
1152 pxms_do_out:
1153   pxmc_rocon_pwm_dc_out,
1154   pxms_do_deb: 0,
1155   pxms_do_gen: 0,
1156 pxms_do_ap2hw:
1157   pxmc_inp_rocon_ap2hw,
1158   pxms_ap: 0, pxms_as: 0,
1159   pxms_rp: 55 * 256, pxms_rs: 0, pxms_subdiv: 8,
1160   pxms_md: 800 << 8, pxms_ms: 500, pxms_ma: 10,
1161   pxms_inp_info: 2,
1162   pxms_out_info: 4,
1163   pxms_ene: 0, pxms_erc: 0,
1164   pxms_p: 80, pxms_i: 10, pxms_d: 200, pxms_s1: 200, pxms_s2: 0,
1165   pxms_me: 0x7e00/*0x7fff*/,
1166 pxms_cfg:
1167   PXMS_CFG_SMTH_m | PXMS_CFG_MD2E_m | PXMS_CFG_HLS_m | PXMS_CFG_HPS_m * 0 |
1168   PXMS_CFG_I2PT_m * 0 | PXMS_CFG_HRI_m |
1169   PXMS_CFG_HDIR_m | 0x2,
1170
1171   pxms_ptper: 1,
1172   pxms_ptirc: 1000,
1173   /*pxms_ptamp: 0x7fff,*/
1174
1175   pxms_hal: 0x40,
1176 };
1177
1178 pxmc_state_t mcs3 =
1179 {
1180 pxms_flg:
1181   PXMS_ENI_m,
1182 pxms_do_inp:
1183   pxmc_inp_rocon_inp,
1184 pxms_do_con:
1185   pxmc_pid_con,
1186 pxms_do_out:
1187   pxmc_rocon_pwm_dc_out,
1188   pxms_do_deb: 0,
1189   pxms_do_gen: 0,
1190 pxms_do_ap2hw:
1191   pxmc_inp_rocon_ap2hw,
1192   pxms_ap: 0, pxms_as: 0,
1193   pxms_rp: 55 * 256, pxms_rs: 0, pxms_subdiv: 8,
1194   pxms_md: 800 << 8, pxms_ms: 500, pxms_ma: 10,
1195   pxms_inp_info: 3,
1196   pxms_out_info: 6,
1197   pxms_ene: 0, pxms_erc: 0,
1198   pxms_p: 80, pxms_i: 10, pxms_d: 200, pxms_s1: 200, pxms_s2: 0,
1199   pxms_me: 0x7e00/*0x7fff*/,
1200 pxms_cfg:
1201   PXMS_CFG_SMTH_m | PXMS_CFG_MD2E_m | PXMS_CFG_HLS_m | PXMS_CFG_HPS_m * 0 |
1202   PXMS_CFG_I2PT_m * 0 | PXMS_CFG_HRI_m |
1203   PXMS_CFG_HDIR_m * 0 | 0x2,
1204
1205   pxms_ptper: 1,
1206   pxms_ptirc: 1000,
1207   /*pxms_ptamp: 0x7fff,*/
1208
1209   pxms_hal: 0x40,
1210 };
1211
1212 pxmc_state_t mcs4 =
1213 {
1214 pxms_flg:
1215   PXMS_ENI_m,
1216 pxms_do_inp:
1217   pxmc_inp_rocon_inp,
1218 pxms_do_con:
1219   pxmc_pid_con,
1220 pxms_do_out:
1221   pxmc_rocon_pwm_dc_out,
1222   pxms_do_deb: 0,
1223   pxms_do_gen: 0,
1224 pxms_do_ap2hw:
1225   pxmc_inp_rocon_ap2hw,
1226   pxms_ap: 0, pxms_as: 0,
1227   pxms_rp: 55 * 256, pxms_rs: 0, pxms_subdiv: 8,
1228   pxms_md: 800 << 8, pxms_ms: 500, pxms_ma: 10,
1229   pxms_inp_info: 4,
1230   pxms_out_info: 8,
1231   pxms_ene: 0, pxms_erc: 0,
1232   pxms_p: 80, pxms_i: 10, pxms_d: 200, pxms_s1: 200, pxms_s2: 0,
1233   pxms_me: 0x7e00/*0x7fff*/,
1234 pxms_cfg:
1235   PXMS_CFG_SMTH_m | PXMS_CFG_MD2E_m | PXMS_CFG_HLS_m | PXMS_CFG_HPS_m * 0 |
1236   PXMS_CFG_HRI_m | PXMS_CFG_I2PT_m * 0 |
1237   PXMS_CFG_HDIR_m | 0x2,
1238
1239   pxms_ptper: 1,
1240   pxms_ptirc: 1000,
1241   /*pxms_ptamp: 0x7fff,*/
1242
1243   pxms_hal: 0x40,
1244 };
1245
1246 pxmc_state_t mcs5 =
1247 {
1248 pxms_flg:
1249   PXMS_ENI_m,
1250 pxms_do_inp:
1251   pxmc_inp_rocon_inp,
1252 pxms_do_con:
1253   pxmc_pid_con,
1254 pxms_do_out:
1255   pxmc_rocon_pwm_dc_out,
1256   pxms_do_deb: 0,
1257   pxms_do_gen: 0,
1258 pxms_do_ap2hw:
1259   pxmc_inp_rocon_ap2hw,
1260   pxms_ap: 0, pxms_as: 0,
1261   pxms_rp: 55 * 256, pxms_rs: 0, pxms_subdiv: 8,
1262   pxms_md: 800 << 8, pxms_ms: 500, pxms_ma: 10,
1263   pxms_inp_info: 5,
1264   pxms_out_info: 10,
1265   pxms_ene: 0, pxms_erc: 0,
1266   pxms_p: 80, pxms_i: 10, pxms_d: 200, pxms_s1: 200, pxms_s2: 0,
1267   pxms_me: 0x7e00/*0x7fff*/,
1268 pxms_cfg:
1269   PXMS_CFG_SMTH_m | PXMS_CFG_MD2E_m | PXMS_CFG_HLS_m | PXMS_CFG_HPS_m |
1270   PXMS_CFG_HRI_m | PXMS_CFG_I2PT_m * 0 |
1271   PXMS_CFG_HDIR_m | 0x2,
1272
1273   pxms_ptper: 1,
1274   pxms_ptirc: 1000,
1275   /*pxms_ptamp: 0x7fff,*/
1276
1277   pxms_hal: 0x40,
1278 };
1279
1280 pxmc_state_t mcs6 =
1281 {
1282 pxms_flg:
1283   PXMS_ENI_m,
1284 pxms_do_inp:
1285   pxmc_inp_rocon_inp,
1286 pxms_do_con:
1287   pxmc_pid_con,
1288 pxms_do_out:
1289   pxmc_rocon_pwm_dc_out,
1290   pxms_do_deb: 0,
1291   pxms_do_gen: 0,
1292 pxms_do_ap2hw:
1293   pxmc_inp_rocon_ap2hw,
1294   pxms_ap: 0, pxms_as: 0,
1295   pxms_rp: 55 * 256, pxms_rs: 0, pxms_subdiv: 8,
1296   pxms_md: 800 << 8, pxms_ms: 500, pxms_ma: 10,
1297   pxms_inp_info: 6,
1298   pxms_out_info: 12,
1299   pxms_ene: 0, pxms_erc: 0,
1300   pxms_p: 80, pxms_i: 10, pxms_d: 200, pxms_s1: 200, pxms_s2: 0,
1301   pxms_me: 0x7e00/*0x7fff*/,
1302 pxms_cfg:
1303   PXMS_CFG_SMTH_m | PXMS_CFG_MD2E_m | PXMS_CFG_HLS_m | PXMS_CFG_I2PT_m * 0 |
1304   0x1,
1305
1306   pxms_ptper: 1,
1307   pxms_ptirc: 1000,
1308   /*pxms_ptamp: 0x7fff,*/
1309
1310   pxms_hal: 0x40,
1311 };
1312
1313 pxmc_state_t mcs7 =
1314 {
1315 pxms_flg:
1316   PXMS_ENI_m,
1317 pxms_do_inp:
1318   pxmc_inp_rocon_inp,
1319 pxms_do_con:
1320   pxmc_pid_con,
1321 pxms_do_out:
1322   pxmc_rocon_pwm_dc_out,
1323   pxms_do_deb: 0,
1324   pxms_do_gen: 0,
1325 pxms_do_ap2hw:
1326   pxmc_inp_rocon_ap2hw,
1327   pxms_ap: 0, pxms_as: 0,
1328   pxms_rp: 55 * 256, pxms_rs: 0, pxms_subdiv: 8,
1329   pxms_md: 800 << 8, pxms_ms: 500, pxms_ma: 10,
1330   pxms_inp_info: 7,
1331   pxms_out_info: 14,
1332   pxms_ene: 0, pxms_erc: 0,
1333   pxms_p: 80, pxms_i: 10, pxms_d: 200, pxms_s1: 200, pxms_s2: 0,
1334   pxms_me: 0x7e00/*0x7fff*/,
1335 pxms_cfg:
1336   PXMS_CFG_SMTH_m | PXMS_CFG_MD2E_m | PXMS_CFG_HLS_m | PXMS_CFG_I2PT_m * 0 |
1337   0x1,
1338
1339   pxms_ptper: 1,
1340   pxms_ptirc: 1000,
1341   /*pxms_ptamp: 0x7fff,*/
1342
1343   pxms_hal: 0x40,
1344 };
1345
1346
1347 pxmc_state_t *pxmc_main_arr[PXML_MAIN_CNT] =
1348 {&mcs0, &mcs1, &mcs2, &mcs3, &mcs4, &mcs5, &mcs6, &mcs7};
1349
1350
1351 pxmc_state_list_t  pxmc_main_list =
1352 {
1353 pxml_arr:
1354   pxmc_main_arr,
1355   pxml_cnt: 0
1356 };
1357
1358 void pxmc_lpc_qei_callback_index(struct lpc_qei_state_t *qst, void *context)
1359 {
1360   pxmc_state_t *mcs = (pxmc_state_t *)context;
1361   short ofs;
1362   short irc;
1363   irc = qst->index_pos;
1364
1365   if ((mcs->pxms_cfg & PXMS_CFG_I2PT_m) && (mcs->pxms_flg & PXMS_PTI_m))
1366   {
1367     short diff;
1368     ofs = irc - mcs->pxms_ptmark;
1369     diff = ofs - mcs->pxms_ptofs;
1370
1371     if (diff >= mcs->pxms_ptirc / 2)
1372       diff -= mcs->pxms_ptirc;
1373
1374     if (diff <= -mcs->pxms_ptirc / 2)
1375       diff += mcs->pxms_ptirc;
1376
1377     if (diff < 0)
1378       diff = -diff;
1379
1380     if (diff >= mcs->pxms_ptirc / 6)
1381     {
1382       pxmc_set_errno(mcs, PXMS_E_I2PT_TOOBIG);
1383     }
1384     else
1385     {
1386       mcs->pxms_ptofs = ofs;
1387       pxmc_set_flag(mcs, PXMS_PHA_b);
1388     }
1389   } /*else {
1390
1391     ofs=irc-mcs->pxms_ptofs;
1392     if((unsigned short)ofs>=(unsigned short)mcs->pxms_ptirc) {
1393       if(ofs>0) {
1394         ofs-=mcs->pxms_ptirc;
1395       } else {
1396         ofs+=mcs->pxms_ptirc;
1397       }
1398     }
1399     mcs->pxms_ptmark=ofs;
1400   }*/
1401
1402   /*lpc_qei_set_cmpos(qst, 0, qst->index_pos-4000);*/
1403 }
1404
1405 int pxmc_lpc_pthalalign_callback(pxmc_state_t *mcs)
1406 {
1407   short ptofs;
1408   short ptmark;
1409   lpc_qei_state_t *qst = &lpc_qei_state;
1410   int irc = lpc_qei_get_pos(qst);
1411   int idx_rel;
1412
1413   if (!qst->index_occ)
1414     return 0;
1415
1416   idx_rel = qst->index_pos - irc;
1417   idx_rel %= mcs->pxms_ptirc;
1418
1419   if (idx_rel < 0)
1420     idx_rel += mcs->pxms_ptirc;
1421
1422   ptofs = irc - mcs->pxms_ptofs;
1423   ptmark = ptofs + idx_rel;
1424
1425   if ((unsigned short)ptmark >= mcs->pxms_ptirc)
1426   {
1427     if (ptmark > 0)
1428       ptmark -= mcs->pxms_ptirc;
1429     else
1430       ptmark += mcs->pxms_ptirc;
1431   }
1432
1433   if ((unsigned short)ptmark < mcs->pxms_ptirc)
1434   {
1435     mcs->pxms_ptmark = ptmark;
1436   }
1437
1438   return 0;
1439 }
1440
1441 int pxmc_lpc_pthalalign(pxmc_state_t *mcs, int periods)
1442 {
1443   int res;
1444   long r2acq;
1445   long spd;
1446   pxmc_call_t *fin_fnc = pxmc_lpc_pthalalign_callback;
1447   lpc_qei_state_t *qst = &lpc_qei_state;
1448
1449   mcs->pxms_cfg &= ~PXMS_CFG_I2PT_m;
1450   lpc_qei_setup_index_catch(qst);
1451
1452   r2acq = ((long)mcs->pxms_ptirc << PXMC_SUBDIV(mcs)) * periods;
1453   spd = 1 << PXMC_SUBDIV(mcs);
1454
1455   res = pxmc_pthalalign(mcs, r2acq, spd, fin_fnc);
1456
1457   return res;
1458 }
1459
1460 int pxmc_lpc_pthalalign_run(void)
1461 {
1462   return pxmc_lpc_pthalalign(pxmc_main_list.pxml_arr[0], 20);
1463 }
1464
1465 static inline void pxmc_sfi_input(void)
1466 {
1467   int var;
1468   pxmc_state_t *mcs;
1469   pxmc_for_each_mcs(var, mcs)
1470   {
1471     /* PXMS_ENI_m - check if input (IRC) update is enabled */
1472     if (mcs->pxms_flg & PXMS_ENI_m)
1473     {
1474       pxmc_call(mcs, mcs->pxms_do_inp);
1475     }
1476   }
1477 }
1478
1479 static inline void pxmc_sfi_controller_and_output(void)
1480 {
1481   int var;
1482   pxmc_state_t *mcs;
1483   pxmc_for_each_mcs(var, mcs)
1484   {
1485     /* PXMS_ENR_m - check if controller is enabled */
1486     if (mcs->pxms_flg & PXMS_ENR_m || mcs->pxms_flg & PXMS_ENO_m)
1487     {
1488
1489       /* If output only is enabled, we skip the controller */
1490       if (mcs->pxms_flg & PXMS_ENR_m)
1491       {
1492
1493         pxmc_call(mcs, mcs->pxms_do_con);
1494
1495         /* PXMS_ERR_m - if axis in error state */
1496         if (mcs->pxms_flg & PXMS_ERR_m)
1497           mcs->pxms_ene = 0;
1498       }
1499
1500       /* for bushless motors, it is necessary to call do_out
1501         even if the controller is not enabled and PWM should be provided. */
1502       pxmc_call(mcs, mcs->pxms_do_out);
1503     }
1504   }
1505 }
1506
1507 static inline void pxmc_sfi_generator(void)
1508 {
1509   int var;
1510   pxmc_state_t *mcs;
1511   pxmc_for_each_mcs(var, mcs)
1512   {
1513     /* PXMS_ENG_m - check if requested value (position) generator is enabled */
1514     if (mcs->pxms_flg & PXMS_ENG_m)
1515     {
1516       pxmc_call(mcs, mcs->pxms_do_gen);
1517     }
1518   }
1519 }
1520
1521 static inline void pxmc_sfi_dbg(void)
1522 {
1523   int var;
1524   pxmc_state_t *mcs;
1525   pxmc_for_each_mcs(var, mcs)
1526   {
1527     if (mcs->pxms_flg & PXMS_DBG_m)
1528     {
1529       pxmc_call(mcs, mcs->pxms_do_deb);
1530     }
1531   }
1532 }
1533
1534 int pxmc_rocon_pthalalign_callback(pxmc_state_t *mcs)
1535 {
1536   short ptofs;
1537   short ptmark;
1538   int inp_chan = mcs->pxms_inp_info;
1539   int idx_rel;
1540   long irc = fpga_irc[inp_chan]->count;
1541
1542   if (!pxmc_inp_rocon_is_index_edge(mcs))
1543     return 0;
1544
1545   idx_rel = fpga_irc[inp_chan]->count_index - irc;
1546   idx_rel %= mcs->pxms_ptirc;
1547   if(idx_rel < 0)
1548     idx_rel += mcs->pxms_ptirc;
1549
1550   ptofs = irc-mcs->pxms_ptofs;
1551   ptmark = ptofs+idx_rel;
1552
1553   if((unsigned short)ptmark >= mcs->pxms_ptirc) {
1554     if(ptmark>0)
1555       ptmark -= mcs->pxms_ptirc;
1556     else
1557       ptmark += mcs->pxms_ptirc;
1558   }
1559
1560   if((unsigned short)ptmark < mcs->pxms_ptirc) {
1561     mcs->pxms_ptmark = ptmark;
1562   }
1563   return 0;
1564 }
1565
1566 int pxmc_rocon_pthalalign(pxmc_state_t *mcs, int periods)
1567 {
1568   int res;
1569   long r2acq;
1570   long spd;
1571   pxmc_call_t *fin_fnc = pxmc_rocon_pthalalign_callback;
1572
1573   mcs->pxms_cfg&=~PXMS_CFG_I2PT_m;
1574
1575   r2acq=((long)mcs->pxms_ptirc<<PXMC_SUBDIV(mcs))*periods;
1576   spd=1<<PXMC_SUBDIV(mcs);
1577
1578   /* clear bit indicating previous index */
1579   pxmc_inp_rocon_is_index_edge(mcs);
1580
1581   res=pxmc_pthalalign(mcs, r2acq, spd, fin_fnc);
1582
1583   return res;
1584 }
1585
1586 int pxmc_axis_rdmode(pxmc_state_t *mcs)
1587 {
1588   if (mcs->pxms_do_out == pxmc_rocon_pwm2ph_out)
1589     return PXMC_AXIS_MODE_STEPPER_WITH_IRC;
1590   if (mcs->pxms_do_out == pxmc_rocon_pwm_dc_out)
1591     return PXMC_AXIS_MODE_DC;
1592   if (mcs->pxms_do_out == pxmc_rocon_pwm3ph_out)
1593     return PXMC_AXIS_MODE_BLDC;
1594   return -1;
1595 }
1596
1597
1598 int
1599 pxmc_axis_pt4mode(pxmc_state_t *mcs, int mode)
1600 {
1601   static const typeof(*mcs->pxms_ptptr1) dummy0 = 0;
1602   int res = 0;
1603
1604   if (mode == PXMC_AXIS_MODE_NOCHANGE)
1605     mode = pxmc_axis_rdmode(mcs);
1606   if (mode < 0)
1607     return -1;
1608
1609   switch (mode) {
1610     /*case PXMC_AXIS_MODE_STEPPER:*/
1611     case PXMC_AXIS_MODE_STEPPER_WITH_IRC:
1612       res = pxmc_ptable_set_profile(mcs, &pxmc_ptprofile_sin, 0, 0);
1613       break;
1614     /*case PXMC_AXIS_MODE_STEPPER_WITH_PWM:*/
1615     case PXMC_AXIS_MODE_DC:
1616       /*profive some sane dummy values*/
1617       mcs->pxms_ptptr1 = (typeof(mcs->pxms_ptptr1))&dummy0;
1618       mcs->pxms_ptptr2 = (typeof(mcs->pxms_ptptr1))&dummy0;
1619       mcs->pxms_ptptr3 = (typeof(mcs->pxms_ptptr1))&dummy0;
1620
1621       mcs->pxms_ptscale_mult=1;
1622       mcs->pxms_ptscale_shift=15;
1623       break;
1624     case PXMC_AXIS_MODE_BLDC:
1625       /* res = pxmc_init_ptable(mcs, PXMC_PTPROF_SIN3FUP); */
1626      #ifndef PXMC_WITH_PT_ZIC
1627       res = pxmc_ptable_set_profile(mcs, &pxmc_ptprofile_sin3phup, 0, 0);
1628      #else /*PXMC_WITH_PT_ZIC*/
1629       res = pxmc_ptable_set_profile(mcs, &pxmc_ptprofile_sin3phup_zic, 0, 0);
1630      #endif /*PXMC_WITH_PT_ZIC*/
1631       break;
1632     default:
1633       return -1;
1634   }
1635
1636   mcs->pxms_ptvang = pxmc_ptvang_deg2irc(mcs, 90);
1637
1638   return res;
1639 }
1640
1641 /**
1642  * pxmc_axis_mode - Sets axis mode.[extern API]
1643  * @mcs:        Motion controller state information
1644  * @mode:       0 .. previous mode, 1 .. stepper motor mode,
1645  *              2 .. stepper motor with IRC feedback and PWM ,
1646  *              3 .. stepper motor with PWM control
1647  *              4 .. DC motor with IRC feedback and PWM
1648  *
1649  */
1650 int
1651 pxmc_axis_mode(pxmc_state_t *mcs, int mode)
1652 {
1653   int res;
1654
1655   pxmc_set_const_out(mcs, 0);
1656   pxmc_clear_flag(mcs, PXMS_ENI_b);
1657   pxmc_clear_flags(mcs,PXMS_ENO_m);
1658   /* Clear possible stall index flags from hardware */
1659   pxmc_inp_rocon_is_index_edge(mcs);
1660   pxmc_clear_flag(mcs, PXMS_PHA_b);
1661   pxmc_clear_flag(mcs, PXMS_PTI_b);
1662
1663
1664   if (mode == PXMC_AXIS_MODE_NOCHANGE)
1665     mode = pxmc_axis_rdmode(mcs);
1666   if (mode < 0)
1667     return -1;
1668   if (!mode)
1669     mode = PXMC_AXIS_MODE_DC;
1670
1671   res = pxmc_axis_pt4mode(mcs, mode);
1672   if (res < 0)
1673     return -1;
1674
1675   switch (mode) {
1676     /*case PXMC_AXIS_MODE_STEPPER:*/
1677     case PXMC_AXIS_MODE_STEPPER_WITH_IRC:
1678       mcs->pxms_do_out = pxmc_rocon_pwm2ph_out;
1679       break;
1680     /*case PXMC_AXIS_MODE_STEPPER_WITH_PWM:*/
1681     case PXMC_AXIS_MODE_DC:
1682       mcs->pxms_do_out = pxmc_rocon_pwm_dc_out;
1683       break;
1684     case PXMC_AXIS_MODE_BLDC:
1685       mcs->pxms_do_out = pxmc_rocon_pwm3ph_out;
1686       break;
1687     default:
1688       return -1;
1689   }
1690
1691   /* Clear possible stall index flags from hardware */
1692   pxmc_inp_rocon_is_index_edge(mcs);
1693   /* Request new phases alignment for changed parameters */
1694   pxmc_clear_flag(mcs, PXMS_PHA_b);
1695   pxmc_clear_flag(mcs, PXMS_PTI_b);
1696   pxmc_set_flag(mcs, PXMS_ENI_b);
1697   return res;
1698 }
1699
1700 void pxmc_sfi_isr(void)
1701 {
1702   unsigned long spent = pxmc_fast_tick_time();
1703
1704   pxmc_sfi_input();
1705   pxmc_sfi_controller_and_output();
1706   pxmc_sfi_generator();
1707   pxmc_sfi_dbg();
1708   /* Kick LX Master watchdog */
1709   if (pxmc_main_list.pxml_cnt != 0)
1710     *fpga_lx_master_transmitter_wdog = 1;
1711
1712   spent = pxmc_fast_tick_time() - spent;
1713
1714   if(spent > pxmc_sfi_spent_time_max)
1715     pxmc_sfi_spent_time_max = spent;
1716
1717 }
1718
1719 int pxmc_clear_power_stop(void)
1720 {
1721   return 0;
1722 }
1723
1724 int pxmc_process_state_check(unsigned long *pbusy_bits,
1725                              unsigned long *perror_bits)
1726 {
1727   unsigned short flg;
1728   unsigned short chan;
1729   unsigned long busy_bits = 0;
1730   unsigned long error_bits = 0;
1731   pxmc_state_t *mcs;
1732   flg=0;
1733   pxmc_for_each_mcs(chan, mcs) {
1734     if(mcs) {
1735       flg |= mcs->pxms_flg;
1736       if (mcs->pxms_flg & PXMS_BSY_m)
1737         busy_bits |= 1 << chan;
1738       if (mcs->pxms_flg & PXMS_ERR_m)
1739         error_bits |= 1 << chan;
1740     }
1741   }
1742   if (appl_errstop_mode) {
1743     if((flg & PXMS_ENG_m) && (flg & PXMS_ERR_m)) {
1744       pxmc_for_each_mcs(chan, mcs) {
1745         if(mcs&&(mcs->pxms_flg & PXMS_ENG_m)) {
1746           pxmc_stop(mcs, 0);
1747         }
1748       }
1749     }
1750   }
1751
1752   if (pbusy_bits != NULL)
1753     *pbusy_bits = busy_bits;
1754   if (error_bits != NULL)
1755     *perror_bits = error_bits;
1756
1757   return flg;
1758 }
1759
1760 int pxmc_done(void)
1761 {
1762   int var;
1763   pxmc_state_t *mcs;
1764
1765   if (!pxmc_main_list.pxml_cnt)
1766     return 0;
1767
1768   pxmc_for_each_mcs(var, mcs)
1769   {
1770     pxmc_set_const_out(mcs,0);
1771   }
1772
1773   pxmc_main_list.pxml_cnt = 0;
1774   __memory_barrier();
1775
1776   return 0;
1777 }
1778
1779 int pxmc_initialize(void)
1780 {
1781   int res;
1782   int i;
1783
1784   pxmc_state_t *mcs = &mcs0;
1785   lpc_qei_state_t *qst = &lpc_qei_state;
1786
1787   *fpga_irc_reset = 1;
1788
1789   for (i = 0; i < 8; i++) {
1790     fpga_irc[i]->count = 0;
1791     fpga_irc[i]->count_index = 0;
1792     *fpga_irc_state[i] = FPGA_IRC_STATE_INDEX_EVENT_MASK;
1793   }
1794
1795   /* Initialize QEI module for IRC counting */
1796   pxmc_inp_lpc_qei_init(mcs->pxms_inp_info);
1797
1798   /* Initialize QEI events processing */
1799   if (lpc_qei_setup_irq(qst) < 0)
1800     return -1;
1801
1802   *fpga_irc_reset = 0;
1803
1804   //res = pxmc_axis_pt4mode(mcs, PXMC_AXIS_MODE_BLDC);
1805
1806   /*pxmc_ctm4pwm3f_wr(mcs, 0, 0, 0);*/
1807   //pxmc_rocon_pwm3ph_wr(mcs, 0, 0, 0);
1808
1809   pxmc_rocon_pwm_master_init();
1810  #ifdef PXMC_ROCON_TIMED_BY_RX_DONE
1811   pxmc_rocon_rx_done_isr_setup(pxmc_rocon_rx_done_isr);
1812  #endif /*PXMC_ROCON_TIMED_BY_RX_DONE*/
1813
1814   pxmc_main_list.pxml_cnt = 0;
1815   pxmc_dbg_hist = NULL;
1816   __memory_barrier();
1817   pxmc_main_list.pxml_cnt = PXML_MAIN_CNT;
1818
1819   return 0;
1820 }