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