---

stg.c

Go to the documentation of this file.

/*
  stg.c

  Servo To Go board functions

  Modification history:

  2-Aug-2001  FMP added stgAdcStart,Wait,Read functions
  30-Jul-2001  FMP fixed bug in poll loop in RawADC so that it stops polling
  when EOC is 1, per Scott Shamblin's bug fix.
  19-Jun-2001  FMP changed typo #ifdef STG_8_AXIS to the proper STG_8_AXES,
  in three places where the limit and home switches were read, as reported
  by Scott Shamblin at U Fl. Nothing like having a real machine to work with.
  15-Jun-2000 WPS changed include asm/io.h to sys/io.h for rtlinux_2_2.
  13-Mar-2000 WPS eliminate compiler warning MODULE redefined.
  28-Oct-1999  FMP added EncoderLatch() function and call to it in
  stgEncoderReadAll()
  22-Oct-1999  FMP added MODULE_PARM() macro for exporting STG_BASE_ADDRESS
  16-Aug-1999  FMP didn't error out if axis was outside range-- just
  zeroed if appropriate, or ignored
  31-Mar-1998  FMP added analog and digital IO stuff
  26-Mar-1998  FMP added STG_BASE_ADDRESS decl
  8-Jan-1998  FMP took out amp disable on init, quit since we don't know
  the polarity
  15-Nov-1997 FMP removed debounce here. Solution to debounce problem
  is to use 5V pullups on STG digital IO. I'll add generic debounce code
  later.
  5-Nov-1997  FMP added stgAmpFault()
  10-Oct-1997  FMP added 0-ing, disabling of DACs, amps on extInit,quit
  8-Oct-1997  FMP added debounce to switch reading code
  25-Sep-1997  FMP added digital IO stuff
  19-Sep-1997  FMP used #defines for base address, number of axes
  18-Sep-1997  FMP removed piezo hack; changed loops in EncoderInit() and
  EncReadAll() to run 4 axis checks instead of doing all 8 always
  19-Aug-1997  PJM Added offset to volts in extDACwrite()
  01-Aug-1997  FMP changed encoder counts from int to double
  31-Jul-1997  FMP added StartADC, RawADC; changed sign of lCounts in
  RawDAC since the polarity was reversed
  28-Jul-1997  FMP added extLimitSwitchRead()
  1-Jul-1997  FMP added wait to extInit() to fix problem with
  extEncoderReadAll() returning garbage immediately after an init
  20-Jun-1997  FMP added exintf.h generic defs
  10-Jun-1997  FMP created
  */

/* ident tag */
#ifndef __GNUC__
#ifndef __attribute__
#define __attribute__(x)
#endif
#endif

static char __attribute__((unused)) ident[] = "$Id: stg.c,v 1.13 2001/10/30 23:31:03 paul_c Exp $";

#include "stg.h"                /* these decls */
#include "extintf.h"            /* EXT_ENCODER_INDEX_MODEL */

/* base address-- override default at compile time in stg.h,
   or at module load time with STG_BASE_ADDRESS=value */
unsigned short STG_BASE_ADDRESS = DEFAULT_STG_BASE_ADDRESS;
int FIND_STG_BASE_ADDRESS=0;

#ifdef STG_8_AXES
#define STG_MAX_AXIS 8
#else
#define STG_MAX_AXIS 4
#endif

#if defined(rtlinux) || defined(rtai)
#ifndef MODULE
#define MODULE
#endif
#define DO_IT
#endif

#ifdef LINUX
#define DO_IT
/*
  Compiling this for LINUX (not RTLINUX) means linking this into a Linux
  process, running non-real-time in user space. This is useful for debugging.
  If this is done, then the following stuff needs to be done to access the
  IO space.
  */

#ifdef DEFINE_EXTERN_BEFORE_IO
/*
  Because of a limitation in gcc (present at least in 2.7.2.1 and below), you
  _have to_ compile any source code that uses these routines with optimisation
  turned on (gcc -O1 or higher), or alternatively #define extern to be
  empty before #including <asm/io.h>.

  Otherwise, you'll get errors at link time like:

  stg.c:475: undefined reference to `__inbc'
 */
#define extern
#endif

/*
  Need to access ports in range 0x200-0x21F and 0x600-0x61F, for default
  STG base address. ioperm() only enables up to 0x3FF, so we need to use
  iopl() to grant full access to IO space.
  */
#include <sys/types.h>
#include <unistd.h>             /* iopl() */

#endif /* LINUX  */

#ifdef DO_IT

#if defined(__KERNEL__) && defined(linux)
#include <linux/kernel.h>
#if !defined(rtai)
/* P.C.  These two headers cause a bunch of compiler errors on
   a RH7.1 system using 2.4.9-rtai
   */
#include <linux/types.h>
#include <linux/fs.h>
#endif
#endif

#if defined(linux) || defined(rtlinux) || defined(rtai)
#include <sys/io.h>             /* ioperm() */
#endif


/* Linux or RT-Linux use io ports */
#ifndef LINUX_VERSION_CODE
#include <linux/version.h>
#endif

#ifndef __GLIBC__
#include <features.h>           /* __GLIBC__  */
#endif

// Pre 2.2 kernels don't seem to have the KERNEL_VERSION macro.
#ifndef KERNEL_VERSION
#define KERNEL_VERSION(a,b,c) (((a) << 16) + ((b) << 8) + (c))
#endif

/* Only include this for systems with an old version of glibc or an
 old version of the linux kernel.  Othewise it is not needed and results
 in a bunch of redefined symbol warnings/errors.
*/
#if LINUX_VERSION_CODE < KERNEL_VERSION(2,2,0) || !defined(__GLIBC__) || __GLIBC__ < 2
#include <asm/io.h>             /* outw, inw */
#endif

#else

/* otherwise do nothing */

#include <stdio.h>

static int iopl(int level)
{
  return 0;
}

static unsigned char inb(unsigned int port)
{
  if (port >= STG_BASE_ADDRESS &&
      port <= STG_BASE_ADDRESS + 0x1f)
    {
      return 0;
    }

  if (port >= STG_BASE_ADDRESS + 0x400 &&
      port <= STG_BASE_ADDRESS + 0x400 + 0x1f)
    {
      return 0;
    }

  printf("inb: address out of bounds: %X\n", port);
  return 0;
}

static void outb(unsigned char byte, unsigned int port)
{
  /* allow writes to 0x80 for 1 msec delay */
  if (port == 0x80)
    {
      return;
    }

  if (port >= STG_BASE_ADDRESS &&
      port <= STG_BASE_ADDRESS + 0x1f)
    {
      return;
    }

  if (port >= STG_BASE_ADDRESS + 0x400 &&
      port <= STG_BASE_ADDRESS + 0x400 + 0x1f)
    {
      return;
    }

  printf("outb: address out of bounds: %X\n", port);
}

static unsigned short inw(unsigned int port)
{
  if (port % 2)
    {
      printf("inw: alignment error: %X\n", port);
      return 0;
    }

  if (port >= STG_BASE_ADDRESS &&
      port <= STG_BASE_ADDRESS + 0x1e)
    {
      return 0;
    }

  if (port >= STG_BASE_ADDRESS + 0x400 &&
      port <= STG_BASE_ADDRESS + 0x400 + 0x1e)
    {
      return 0;
    }

  printf("inw: address out of bounds: %X\n", port);
  return 0;
}

static void outw(unsigned short word, unsigned int port)
{
  if (port % 2)
    {
      printf("outw: alignment error: %X\n", port);
      return;
    }

  if (port >= STG_BASE_ADDRESS &&
      port <= STG_BASE_ADDRESS + 0x1e)
    {
      return;
    }

  if (port >= STG_BASE_ADDRESS + 0x400 &&
      port <= STG_BASE_ADDRESS + 0x400 + 0x1e)
    {
      return;
    }

  printf("outw: address out of bounds: %X\n", port);
  return;
}

#endif /* don't DO_IT */

/* map DOS funcs to Linux */
#define _outp(port,val) outb(val,port)
#define outp(port,val) outb(val,port)
#define _inp(port) inb(port)
#define inp(port) inb(port)
#define _inpw(port) inw(port)
#define _outpw(port,val) outw(val,port)

static short int nIrq;

void SetIrq(short nRequestedIrq)
{
  unsigned char byIntReg;

  nIrq = nRequestedIrq;  /* assume it's OK for now, check later */

  byIntReg = 0x80;       /* initial value for the high bits in the register */
  /* sets auto zero on the ADC.  Maybe this should */
  /* be a parameter if we want to do something different */

  /* now put low bits into byIntReg to select irq */

  switch (nRequestedIrq)
    {
    case 3: break;      /* add zero */

    case 5: byIntReg |= 4;
      break;

    case 7: byIntReg |= 2;
      break;

    case 9: byIntReg |= 6;
      break;

    case 10: byIntReg |= 5;
      break;

    case 11: byIntReg |= 7;
      break;

    case 12: byIntReg |= 3;
      break;

    case 15: byIntReg |= 1;
      break;

    default: nIrq = 5;      /* ERROR, requested irq not valid, use 5 */
      byIntReg |= 4; /* There is no safe value, leaving zero */
      /* here would select IRQ 3 which is worse */
      /* than 5 because IRQ 3 is usually for COM 2 */
      break;
    }

  _outp(STG_BASE_ADDRESS + INTC, byIntReg);  /* set irq */
}

unsigned short BaseFind()
{
  short i, j, k, l, ofs;
  unsigned short io_add;

  for (i = 15; i >= 0; i--)     /* search all possible addresses */
    {
      io_add = i * 0x20 + 0x200;
      if ((_inp(io_add + BRDTST) & 0x0f) == i) /* does jumper = i? */
        {
          k = 0;
          for (j = 7; j >= 0; j--)
            {
              ofs = (_inp(io_add + BRDTST) & 0xf0) / 16;
              l = 0;
              if (ofs > 7)                     /* is SER set?  */
                l = 1;
              ofs = ofs & 7;                   /* mask for Q2,Q1,Q0 */
              if (l == 1)
                k += (1 << ofs);             /* shift bit into position */
            }
          if (k == 0x75) return io_add;        /* SER sequence is 01110101 */
        }
    }
  return(0);
}

short int Initialize(short int nRequestedIrq)
{
  /*
   * Initialize the interrupt controller
   */
  _outp(STG_BASE_ADDRESS + MIO_2, 0x92);  /* initialize INTC as output reg. */
  /* sets port D to input since we have */
  /* to set it to something. */
  SetIrq(nRequestedIrq);        /* selects the IRQ in INTC. Now, if a stray */
  /* interrupt is issued (see below) it will */
  /* go to an interrupt that isn't enabled on */
  /* the motherboard yet (if your system is */
  /* set up correctly). */
  _outp( STG_BASE_ADDRESS + ICW1, 0x1a ); /* initialize 82C59 as single chip, */
  /* level triggered */
  _outp( STG_BASE_ADDRESS + ICW2, 0x00 ); /* icw2 - not used, must write */
  /* could issue stray interrupt here - danger! */
  _outp( STG_BASE_ADDRESS + OCW1, 0xff);  /* mask off all interrupt sources (the */
  /* interrupt on the motherboard isn't */
  /* enabled yet, you do that when you install */
  /* your interrupt handler.). */
  return 0;
}

int StartADC(unsigned short wAxis)
{
  if (wAxis > 7) {
    return -1;
  }

  /* do a dummy read from the ADC, just to set the input multiplexer to
     the right channel */
  _inpw(STG_BASE_ADDRESS + ADC_0 + (wAxis << 1));

  /* wait 4 uS for settling time on the multiplexer and ADC. You probably
     shouldn't really have a delay in a driver */
  _outp(0x80, 0);
  _outp(0x80, 0);
  _outp(0x80, 0);
  _outp(0x80, 0);

  /* now start conversion */
  _outpw(STG_BASE_ADDRESS + ADC_0 + (wAxis << 1), 0);

  return 0;
};

short RawADC(unsigned short wAxis)
{
  short ret;
  short j;

  if (wAxis > 7)
    return -1;

  /*
    there must have been a delay between StartADC() and RawADC(),
    of 19 usec if autozeroing (we are), 4 uses otherwise. In code
    that calls this, make sure you split these calls up with some
    intervening code
    */

  /* make sure conversion is done, assume polling delay is done.
     EOC (End Of Conversion) is bit 0x08 in IIR (Interrupt Request
     Register) of Interrupt Controller.  Don't wait forever though
     bail out eventually. */

  for (j = 0; !(_inp(STG_BASE_ADDRESS + IRR) & 0x08) && (j < 1000); j++);

  ret = _inpw(STG_BASE_ADDRESS + ADC_0 + (wAxis << 1));

  if (ret & 0x1000)       /* is sign bit negative? */
    ret |= 0xf000;      /* sign extend */
  else
    ret &= 0xfff;       /* make sure high order bits are zero. */

  return ret;
};

void EncoderInit()
{
  unsigned int wAdd;
  unsigned int wAddTop;

#ifdef STG_8_AXES
  wAddTop = STG_BASE_ADDRESS + CNT6_C;
#else
  wAddTop = STG_BASE_ADDRESS + CNT2_C;
#endif

  for (wAdd = STG_BASE_ADDRESS + CNT0_C; wAdd <= wAddTop; wAdd +=4)
    {
      /* Set Counter Command Register - Master Control, Master Reset (MRST), */
      /* and Reset address pointer (RADR). */

      _outpw(wAdd, 0x2323);

      /* Set Counter Command Register - Input Control, OL Load (P3), */
      /* and Enable Inputs A and B (INA/B). */

      _outpw(wAdd, 0x6868);

      /* Set Counter Command Register - Output Control */

      _outpw(wAdd, 0x8080);

      /* Set Counter Command Register - Quadrature */

      _outpw(wAdd, 0xc3c3);
    }
}

void SelectInterruptPeriod(long lPeriodSelect)
{
  _outp(STG_BASE_ADDRESS + TMRCMD, 0x56);   /* timer 1, read/load LSB (MSB is 0) */
  /* mode 3 (square wave) */
  _outp(STG_BASE_ADDRESS + TIMER_1, 0xb4);  /* 0xb4 = 180 -> 25 uSec period */

  switch (lPeriodSelect)
    {
    case _500_MICROSECONDS:
      _outp(STG_BASE_ADDRESS + TMRCMD, 0x34);    /* timer 0, read/load LSB followed by */
      /* MSB, mode 2 (real-time interrupt) */
      _outp(STG_BASE_ADDRESS + TIMER_0, 0x14);   /* 0x14 = 20 = .5 mS */
      _outp(STG_BASE_ADDRESS + TIMER_0, 0x00);
      break;
    case _1_MILLISECOND:
      _outp(STG_BASE_ADDRESS + TMRCMD, 0x34);    /* timer 0, read/load LSB followed by */
      /* MSB, mode 2 (real-time interrupt) */
      _outp(STG_BASE_ADDRESS + TIMER_0, 0x28);   /* 0x28 = 40 = 1 mS */
      _outp(STG_BASE_ADDRESS + TIMER_0, 0x00);
      break;
    case _2_MILLISECONDS:
      _outp(STG_BASE_ADDRESS + TMRCMD, 0x34);    /* timer 0, read/load LSB followed by */
      /* MSB, mode 2 (real-time interrupt) */
      _outp(STG_BASE_ADDRESS + TIMER_0, 0x50);   /* 0x50 = 80 = 2 mS */
      _outp(STG_BASE_ADDRESS + TIMER_0, 0x00);
      break;
    case _3_MILLISECONDS:
      _outp(STG_BASE_ADDRESS + TMRCMD, 0x34);    /* timer 0, read/load LSB followed by */
      /* MSB, mode 2 (real-time interrupt) */
      _outp(STG_BASE_ADDRESS + TIMER_0, 0x78);   /* 0x78 = 120 = 3 mS */
      _outp(STG_BASE_ADDRESS + TIMER_0, 0x00);
      break;
    case _4_MILLISECONDS:
      _outp(STG_BASE_ADDRESS + TMRCMD, 0x34);    /* timer 0, read/load LSB followed by */
      /* MSB, mode 2 (real-time interrupt) */
      _outp(STG_BASE_ADDRESS + TIMER_0, 0xA0);   /* 0xA0 = 160 = 4 mS */
      _outp(STG_BASE_ADDRESS + TIMER_0, 0x00);
      break;
    case _5_MILLISECONDS:
      _outp(STG_BASE_ADDRESS + TMRCMD, 0x34);    /* timer 0, read/load LSB followed by */
      /* MSB, mode 2 (real-time interrupt) */
      _outp(STG_BASE_ADDRESS + TIMER_0, 0xC8);   /* 0xC8 = 200 = 5 mS */
      _outp(STG_BASE_ADDRESS + TIMER_0, 0x00);
      break;
    case _10_MILLISECONDS:
      _outp(STG_BASE_ADDRESS + TMRCMD, 0x34);    /* timer 0, read/load LSB followed by */
      /* MSB, mode 2 (real-time interrupt) */
      _outp(STG_BASE_ADDRESS + TIMER_0, 0x90);   /* 0x0190 = 400 = 10 mS */
      _outp(STG_BASE_ADDRESS + TIMER_0, 0x01);
      break;
    case _100_MILLISECONDS:
      _outp(STG_BASE_ADDRESS + TMRCMD, 0x34);    /* timer 0, read/load LSB followed by */
      /* MSB, mode 2 (real-time interrupt) */
      _outp(STG_BASE_ADDRESS + TIMER_0, 0xA0);   /* 0x0FA0 = 4000 = 100 mS */
      _outp(STG_BASE_ADDRESS + TIMER_0, 0x0F);
      break;
    case _1_SECOND:
      _outp(STG_BASE_ADDRESS + TMRCMD, 0x34);    /* timer 0, read/load LSB followed by */
      /* MSB, mode 2 (real-time interrupt) */
      _outp(STG_BASE_ADDRESS + TIMER_0, 0x40);   /* 0x9C40 = 40000 = 1 S */
      _outp(STG_BASE_ADDRESS + TIMER_0, 0x9c);
      break;
    default:
      /* wrong input? then don't change it */
      break;
    }
};

/*
  output to DAC word works like:

  0x0000 -> +10 V
  0x1000 ->   0 V
  0x1FFF -> -10 V
  */
int RawDAC(short nAxis, double volts)
{
  short nCounts;

  if ( (nAxis > 7) || (nAxis < 0) )        /* is axis within range? */
    return -1;

  /* convert -10.0 -> 10.0 to 0x1FFF -> 0x0000 */
  nCounts = (short) ((10.0 - volts) / 20.0 * 0x1FFF);

  if (nCounts > 0x1FFF)
    {
      nCounts = 0x1FFF;
    }
  if (nCounts < 0)
    {
      nCounts = 0;
    }

  _outpw(STG_BASE_ADDRESS + DAC_0 + (nAxis << 1), nCounts);

  return 0;
};

int EncoderLatch()
{
  unsigned short wAdd;
  unsigned short wAddTop;

#ifdef STG_8_AXES
  wAddTop = STG_BASE_ADDRESS + CNT6_C;
#else
  wAddTop = STG_BASE_ADDRESS + CNT2_C;
#endif

  for (wAdd = STG_BASE_ADDRESS + CNT0_C; wAdd <= wAddTop; wAdd +=4)
    {
      _outpw(wAdd, 0x0303);
    }

  return 0;
}

int EncReadAll(LONGBYTE * lbEnc)
{
  WORDBYTE wbTransfer;
  static unsigned char byOldByte2[STG_MAX_AXIS];
  static unsigned char byEncHighByte[STG_MAX_AXIS];
  short i;
  unsigned int wAdd;
  unsigned int wAddTop;

  /* Disable interrupts here?  No, the timer will latch new data in the */
  /* hardware anyway.  Maybe we should stop the timer?  In an interrupt */
  /* service routine, you're synchronized with the timer; so the readings */
  /* will never change while you're reading them.  If you're polling, you */
  /* would first latch the encoder counts with the EncoderLatch() function. */
  /* But, the timer could latch the counts again, in the middle of the read. */

#ifdef STG_8_AXES
  wAddTop = STG_BASE_ADDRESS + CNT6_C;
#else
  wAddTop = STG_BASE_ADDRESS + CNT2_C;
#endif

  /* reset counter internal addr ptr to point to first byte */
  for (wAdd = STG_BASE_ADDRESS + CNT0_C; wAdd <= wAddTop; wAdd +=4)
    {
      _outpw(wAdd, 0x0101);
    }

  for (i = 0; i < 3; i++)            /* 24 bits means get 3 bytes each */
    {
      wbTransfer.Word = _inpw(STG_BASE_ADDRESS + CNT0_D);

      lbEnc[0].Byte[i] = wbTransfer.Byte.high;
      lbEnc[1].Byte[i] = wbTransfer.Byte.low;

      wbTransfer.Word = _inpw(STG_BASE_ADDRESS + CNT2_D);

      lbEnc[2].Byte[i] = wbTransfer.Byte.high;
      lbEnc[3].Byte[i] = wbTransfer.Byte.low;

#ifdef STG_8_AXES
      wbTransfer.Word = _inpw(STG_BASE_ADDRESS + CNT4_D);

      lbEnc[4].Byte[i] = wbTransfer.Byte.high;
      lbEnc[5].Byte[i] = wbTransfer.Byte.low;

      wbTransfer.Word = _inpw(STG_BASE_ADDRESS + CNT6_D);

      lbEnc[6].Byte[i] = wbTransfer.Byte.high;
      lbEnc[7].Byte[i] = wbTransfer.Byte.low;
#endif
    }

  /* code to sign extend the 24 bit value */
  /* but we won't do this. */
  /*for ( i = 0; i < 8; i++) */
  /*  lbEnc[i].Byte[3] = lbEnc[i].Byte[2] & 0x80 ? 0xff : 0; */

  /* maintain the high byte, to extend the counter to 32 bits */
  /* */
  /* base decisions to increment or decrement the high byte */
  /* on the highest 2 bits of the 24 bit value.  To get the */
  /* highest 2 bits, use 0xc0 as a mask on byte [2] (the third */
  /* byte). */

  for (i = 0; i < STG_MAX_AXIS; i++)
    {
      /* check for -1 to 0 transition */

      if (    ( (byOldByte2[i]    & 0xc0) == 0xc0 ) /* 11xxxxxx */
              && ( (lbEnc[i].Byte[2] & 0xc0) == 0 ) /* 00xxxxxx */
              )
        byEncHighByte[i]++;

      /* check for 0 to -1 transition */

      if (    ( (byOldByte2[i]    & 0xc0) == 0 ) /* 00xxxxxx */
              && ( (lbEnc[i].Byte[2] & 0xc0) == 0xc0 ) /* 11xxxxxx */
              )
        byEncHighByte[i]--;

      lbEnc[i].Byte[3] = byEncHighByte[i];
      byOldByte2[i] = lbEnc[i].Byte[2]; /* current byte 2 becomes old one */
    }

  return 0;
}

void ResetIndexLatch()
{
  _inp(STG_BASE_ADDRESS + ODDRST);        /* reset index pulse latch for ODD axis */
  _inp(STG_BASE_ADDRESS + BRDTST);        /* reset index pulse latch for EVEN axis */
}

static unsigned char byIndexPollAxis = 0;
static unsigned char byIndexPulsePolarity = 1;

void SelectIndexAxis(unsigned char byAxis, unsigned char byPol)
{
  /* */
  /* initialize stuff to poll index pulse */
  /* */
  unsigned char byIntc;

  byIndexPollAxis = byAxis;           /* save axis to check later */
  byIndexPulsePolarity = byPol;       /* save polarity as new default */
  byAxis &= 0x6;                      /* ignore low bit, we check 2 axes at a time */
  byAxis <<= 3;                       /* shift into position for IXS1, IXS0 */
  byIntc = _inp(STG_BASE_ADDRESS + INTC); /* get a copy of INTC, we'll change */
  /* some bits in it, not all */
  byIntc &= ~(IXLVL | IXS1 | IXS0);   /* zero bits for axis and polarity */
  byIntc |= byAxis;                   /* put axes address in INTC */
  if (byPol != 0)                     /* is index pulse active high? */
    byIntc |= IXLVL;
  _outp(STG_BASE_ADDRESS + INTC, byIntc);
  ResetIndexLatch();

  /* The latched index pulse should be low now.  If it's not, either something's */
  /* wrong, or we happened to initialize it while the index pulse was active. */
}

unsigned char CurrentIRR()
{
  outp(STG_BASE_ADDRESS + OCW3, 0x0a);           /* IRR on next read */
  return inp(STG_BASE_ADDRESS + IRR);
}

int IndexPulseLatch()
{
  /* poll the latched index pulse of the axis that was previously set up */

  unsigned char byIRR, byAxisMask;

  byIRR = CurrentIRR();
  byAxisMask = (byIndexPollAxis & 1) ? LIXODD : LIXEVN;  /* even or odd axis? */
  if (byIRR & byAxisMask)       /* check latched index pulse */
    return 1;
  return 0;
}

long RawDI()
{
  IO32 xInBits;

  xInBits.port.A = _inp(STG_BASE_ADDRESS + DIO_A);
  xInBits.port.B = _inp(STG_BASE_ADDRESS + DIO_B);
  xInBits.port.C = _inp(STG_BASE_ADDRESS + DIO_C);
  xInBits.port.D = _inp(STG_BASE_ADDRESS + DIO_D);

  return xInBits.all;
};

void RawDO(unsigned char byBitNumber, unsigned char by0or1, unsigned short nPort)
{
  unsigned nOffset;
  unsigned char byData;

  switch (nPort)
    {
    case 0:
      nOffset = DIO_A;
      break;
    case 1:
      nOffset = DIO_B;
      break;
    case 2:
      nOffset = DIO_C;
      break;
    case 3:
      nOffset = DIO_D;
      break;
    default:
      return;
    }
  byData = _inp(STG_BASE_ADDRESS + nOffset);
  if (by0or1 == 1)
    byData |= 1 << byBitNumber;
  else
    byData &= ~(1 << byBitNumber);
  _outp(STG_BASE_ADDRESS + nOffset, byData);
};

void RawDOAll(IO32 xOutBits)
{
  _outp(STG_BASE_ADDRESS + DIO_A, xOutBits.port.A);
  _outp(STG_BASE_ADDRESS + DIO_B, xOutBits.port.B);
  _outp(STG_BASE_ADDRESS + DIO_C, xOutBits.port.C);
  _outp(STG_BASE_ADDRESS + DIO_D, xOutBits.port.D);
};

/*
  nSwDir is bitmask for directions, ports A-D
  Bit = 1 means output, bit = 0 means input

  Looks like:

  MSB                               LSB
  v---------------v-------------------v
  |   |   |   | D | CHI | CLO | B | A |
  ^---------------^-------------------^

  */
void DIOdirection(short nSwDir)
{
  unsigned char byHwDir;        /* direction bits for hardware */
  unsigned char bySaveIntc, bySaveIMR;

  byHwDir = 0x9b;               /* initially all ports input */

  if (nSwDir & 0x01)            /* check the bit for A out */
    byHwDir &= ~A_OUT;          /* if output, set bit to 0 */
  if (nSwDir & 0x02)
    byHwDir &= ~B_OUT;
  if (nSwDir & 0x04)
    byHwDir &= ~C_LOW_OUT;
  if (nSwDir & 0x08)
    byHwDir &= ~C_HI_OUT;
  _outp(STG_BASE_ADDRESS + MIO_1, byHwDir); /* set direction for A, B and C */

  bySaveIntc = _inp(STG_BASE_ADDRESS + INTC); /* INTC needs to be saved, because */
  /* MIO_2 reinitializes the 8255 which */
  /* implements the INTC register. */
  byHwDir = 0x92;               /* initialize port D input */
  if (nSwDir & 0x10)            /* is port D output? */
    byHwDir &= ~D_OUT;          /* if yes, set bit to 0,  */
  bySaveIMR = _inp(STG_BASE_ADDRESS + IMR); /* get the current interrupt mask */
  _outp(STG_BASE_ADDRESS + OCW1, 0xff); /* mask off all interrupts */
  _outp(STG_BASE_ADDRESS + MIO_2, byHwDir); /* set direction for port D */
  _outp(STG_BASE_ADDRESS + INTC, bySaveIntc); /* restore interrupt control reg. */
  _outp(STG_BASE_ADDRESS + OCW1, bySaveIMR); /* restore interrupt mask */
};

/* Implementations of external functions */

/* saved outputs, for stgAioCheck(), since we can't really read outputs */
static double checkedOutputs[STG_MAX_AXIS];

#define STG_INIT_WAIT 1000      /* usecs to wait after stgMotInit() */

int stgMotInit(const char * stuff)
{
  int t;

#ifdef LINUX
  /* enable port programming */
  if (-1 == iopl(3))
    {
      return -1;
    }
#endif

  Initialize(5);
  EncoderInit();

  /* output 0's to amps */
  for (t = 0; t < STG_MAX_AXIS; t++)
    {
      /* write 0 to DACs */
      stgDacWrite(t, 0.0);
    }

  /* init digital IO directions */
  stgDioInit(0);

  /* need to force a wait here-- calls to stgEncoderReadAll()
     immediately after this return garbage. Is there status to
     poll on for 'ready'? */
  for (t = 0; t < STG_INIT_WAIT; t++)
    {
      /* do a dummy 1 usec write */
      _outp(0x80, 0);
    }

  return 0;
}

int stgMotQuit()
{
  int t;

  for (t = 0; t < STG_MAX_AXIS; t++)
    {
      /* write 0's to DACs */
      stgDacWrite(t, 0.0);
    }

  return 0;
}

int stgAdcNum(void)
{
  return STG_MAX_AXIS;
}

int stgAdcStart(int adc)
{
  if (adc < 0 ||
      adc >= STG_MAX_AXIS) {
    return 0;                 /* don't care */
  }

  StartADC((unsigned short) adc);

  return 0;
}

#define STG_ADC_WAIT_USEC 20    /* microsecs to wait for conversion */

void stgAdcWait(void)
{
  int t;

  for (t = 0; t < STG_ADC_WAIT_USEC; t++) {
    _outp(0x80, 0);             /* 0x80 is historical dummy, 1 usec write */
  }
}

int stgAdcRead(int adc, double * volts)
{
  if (adc < 0 ||
      adc >= STG_MAX_AXIS) {
    return 0;                 /* don't care */
  }

  *volts = (double) RawADC((unsigned short) adc);

  return 0;
}

int stgDacNum()
{
  return STG_MAX_AXIS;
}

int stgDacWrite(int dac, double volts)
{
  if (dac < 0 ||
      dac >= STG_MAX_AXIS) {
    return 0;                 /* don't care */
  }

  checkedOutputs[dac] = volts;

  return RawDAC(dac, volts);
}

int stgDacWriteAll(int max, double * volts)
{
  int t;
  int smax;

  /* clip smax to max supported-- if they want more, ignore */
  if (max > STG_MAX_AXIS) {
    smax = STG_MAX_AXIS;
  }
  else {
    smax = max;
  }

  for (t = 0; t < smax; t++) {
    if (0 != stgDacWrite(t, volts[t])){
      return -1;
    }
  }

  return 0;
}

unsigned int stgEncoderIndexModel(void)
{
  return EXT_ENCODER_INDEX_MODEL_MANUAL;
}

int stgEncoderSetIndexModel(unsigned int model)
{
  if (model != EXT_ENCODER_INDEX_MODEL_MANUAL)
    {
      return -1;
    }

  return 0;
}

int stgEncoderNum()
{
  return STG_MAX_AXIS;
}

int stgEncoderRead(int encoder, double * counts)
{
  double allCounts[STG_MAX_AXIS];

  if (encoder < 0 ||
      encoder >= STG_MAX_AXIS) {
    *counts = 0.0;
    return 0;
  }

  stgEncoderReadAll(encoder + 1, allCounts);

  *counts = allCounts[encoder];

  return 0;
}

int stgEncoderReadAll(int max, double * counts)
{
  LONGBYTE lbEnc[STG_MAX_AXIS];
  int t;
  int smax;                     /* how many we actually have */

  /* clip smax to max supported-- if they want more, give
     them zeros */
  if (max > STG_MAX_AXIS) {
    smax = STG_MAX_AXIS;
  }
  else {
    smax = max;
  }

  EncoderLatch();
  EncReadAll(lbEnc);

  /* fill ours with the actual values */
  for (t = 0; t < smax; t++) {
    counts[t] = (double) lbEnc[t].Long;
  }
  /* and fill the rest with zeros */
  for (t = smax; t < max; t++) {
    counts[t] = 0.0;
  }

  return 0;
}

int stgEncoderResetIndex(int encoder)
{
  if (encoder < 0 ||
      encoder >= STG_MAX_AXIS)
    {
      return 0;
    }

  SelectIndexAxis(encoder, 1);

  return 0;
}

int stgEncoderReadLatch(int encoder, int * flag)
{
  *flag = IndexPulseLatch();

  return 0;
}

/* basically same code as IndexPulseLatch() except mask is 2 bits higher */
int stgEncoderReadLevel(int encoder, int * flag)
{
  unsigned char byIRR, byAxisMask;

  byIRR = CurrentIRR();
  byAxisMask = (byIndexPollAxis & 1) ? IXODD : IXEVN;

  if (byIRR & byAxisMask)
    {
      *flag = 1;
    }
  else
    {
      *flag = 0;
    }

  return 0;
}

/* Note: board only supports 6 axes max of digital in, out for limit and
   home switches, amp enables  */

/* digital input bit masks, 32-bit word */
/* maps to ports A and B */
#define HOME_0    0x00000001
#define MIN_LIM_0 0x00000002
#define MAX_LIM_0 0x00000004
#define FAULT_0   0x00000008
#define HOME_1    0x00000010
#define MIN_LIM_1 0x00000020
#define MAX_LIM_1 0x00000040
#define FAULT_1   0x00000080
#define HOME_2    0x00000100
#define MIN_LIM_2 0x00000200
#define MAX_LIM_2 0x00000400
#define FAULT_2   0x00000800
#define HOME_3    0x00001000
#define MIN_LIM_3 0x00002000
#define MAX_LIM_3 0x00004000
#define FAULT_3   0x00008000

#ifdef STG_8_AXES
/* skip C, maps to port D */
#define HOME_4    0x01000000
#define MIN_LIM_4 0x02000000
#define MAX_LIM_4 0x04000000
#define FAULT_4   0x08000000
#define HOME_5    0x10000000
#define MIN_LIM_5 0x20000000
#define MAX_LIM_5 0x40000000
#define FAULT_5   0x80000000

#endif

/* no space for axes 7 and 8, so these won't go through */
int stgMaxLimitSwitchRead(int axis, int * flag)
{
  long bits;
  int retval = 0;

  bits = RawDI();
  *flag = 0;

  switch (axis)
    {
    case 0:
      if (bits & MAX_LIM_0)
        {
          *flag = 1;
        }
      break;

    case 1:
      if (bits & MAX_LIM_1)
        {
          *flag = 1;
        }
      break;

    case 2:
      if (bits & MAX_LIM_2)
        {
          *flag = 1;
        }
      break;

    case 3:
      if (bits & MAX_LIM_3)
        {
          *flag = 1;
        }
      break;

#ifdef STG_8_AXES
    case 4:
      if (bits & MAX_LIM_4)
        {
          *flag = 1;
        }
      break;

    case 5:
      if (bits & MAX_LIM_5)
        {
          *flag = 1;
        }
      break;

#endif

    default:
      retval = -1;
      break;
    }

  return retval;
}


/* no space for axes 7 and 8, so these won't go through */
int stgMinLimitSwitchRead(int axis, int * flag)
{
  long bits;
  int retval = 0;

  bits = RawDI();
  *flag = 0;

  switch (axis)
    {
    case 0:
      if (bits & MIN_LIM_0)
        {
          *flag = 1;
        }
      break;

    case 1:
      if (bits & MIN_LIM_1)
        {
          *flag = 1;
        }
      break;

    case 2:
      if (bits & MIN_LIM_2)
        {
          *flag = 1;
        }
      break;

    case 3:
      if (bits & MIN_LIM_3)
        {
          *flag = 1;
        }
      break;

#ifdef STG_8_AXES
    case 4:
      if (bits & MIN_LIM_4)
        {
          *flag = 1;
        }
      break;

    case 5:
      if (bits & MIN_LIM_5)
        {
          *flag = 1;
        }
      break;

#endif

    default:
      retval = -1;
      break;
    }

  return retval;
}

/* no space for axes 7 and 8, so these won't go through */
int stgHomeSwitchRead(int axis, int *flag)
{
  long bits;
  int retval = 0;

  bits = RawDI();
  *flag = 0;

  switch (axis)
    {
    case 0:
      if (bits & HOME_0)
        {
          *flag = 1;
        }
      break;

    case 1:
      if (bits & HOME_1)
        {
          *flag = 1;
        }
      break;

    case 2:
      if (bits & HOME_2)
        {
          *flag = 1;
        }
      break;

    case 3:
      if (bits & HOME_3)
        {
          *flag = 1;
        }
      break;

#ifdef STG_8_AXES
    case 4:
      if (bits & HOME_4)
        {
          *flag = 1;
        }
      break;

    case 5:
      if (bits & HOME_5)
        {
          *flag = 1;
        }
      break;

#endif

    default:
      retval = -1;
      break;
    }

  return retval;
}

/* there is space for axes 7 and 8, so these will go through */
int stgAmpEnable(int axis, int enable)
{
  if (axis < 0 || axis >= STG_MAX_AXIS)
    {
      return 0;
    }

  RawDO(axis, (unsigned char) enable, 2); /* 2 is port C */

  return 0;
}

/* no space for axes 7 and 8, so these won't go through */
int stgAmpFault(int axis, int * flag)
{
  long bits;
  int retval = 0;

  bits = RawDI();
  *flag = 0;

  switch (axis)
    {
    case 0:
      if (bits & FAULT_0)
        {
          *flag = 1;
        }
      break;

    case 1:
      if (bits & FAULT_1)
        {
          *flag = 1;
        }
      break;

    case 2:
      if (bits & FAULT_2)
        {
          *flag = 1;
        }
      break;

    case 3:
      if (bits & FAULT_3)
        {
          *flag = 1;
        }
      break;

#ifdef STG_8_AXES
    case 4:
      if (bits & FAULT_4)
        {
          *flag = 1;
        }
      break;

    case 5:
      if (bits & FAULT_5)
        {
          *flag = 1;
        }
      break;

#endif

    default:
      retval = -1;
      break;
    }

  return retval;
}

/*
  Analog and Digital IO

  Analog and digital IO are mapped directly onto the board with no care
  that they don't collide with axes functions above. This is good in the
  sense that you can effect motion, bad in the same sense. This means that
  you should leave indices 0..axis-1 alone and use the higher indices for
  general-purpose IO. For example, if you have a 4-axis board and only
  3 real axes, you can use digital input at indices 12..15 for anything.

  Analog input is an option for the board. There are 8 inputs, which
  don't collide with any motion IO so you can use indices 0..7
  regardless of how many motion axes you have.

  If you have an 8 axis board, only 6 axis can be fully used for motion
  since there is not enough digital input for all the home and limit switches.

  For a 4-axis board, there are:
  24 digital inputs (includes limit switches and amp faults at 0..15)
  8 digital outputs (includes amp enables at 0..3)
  8 analog inputs (with option)
  4 analog outputs (includes amp outputs at 0..3)

  For an 8-axis board (6 motion axes max), there are:
  24 digital inputs (includes limit switches and amp faults at 0..23)
  8 digital outputs (includes amp enables at 0..5)
  8 analog inputs (with option)
  8 analog outputs (includes amp outputs at 0..5)

  Here's the actual map. If you aren't using some axes then their
  slots are up for grabs.

  DIGITAL INPUT
  -------------
  Index   Function      Connector/Pin
  -----   --------      -------------
  00      X home sw     P1/47
  01      X +lim sw     P1/45
  02      X -lim sw     P1/43
  03      X amp fault   P1/41
  04      Y home sw     P1/39
  05      Y +lim sw     P1/37
  06      Y -lim sw     P1/35
  07      Y amp fault   P1/33
  08      Z home sw     P1/31
  09      Z +lim sw     P1/29
  10      Z -lim sw     P1/27
  11      Z amp fault   P1/25
  12      U home sw     P1/23
  13      U +lim sw     P1/21
  14      U -lim sw     P1/19
  15      U amp fault   P1/17
  16      V home sw     P2/31
  17      V +lim sw     P2/29
  18      V -lim sw     P2/27
  19      V amp fault   P2/25
  20      W home sw     P2/23
  21      W +lim sw     P2/21
  22      W -lim sw     P2/19
  23      W amp fault   P2/17

  DIGITAL OUTPUT
  -------------
  Index   Function      Connector/Pin
  -----   --------      -------------
  00      X amp enable  P1/15
  01      Y amp enable  P1/13
  02      Z amp enable  P1/11
  03      U amp enable  P1/9
  04      V amp enable  P1/7
  05      W amp enable  P1/5
  06      (none)        P1/3
  07      (none)        P1/1

  ANALOG INPUT
  -------------
  Index   Function      Connector/Pin
  -----   --------      -------------
  00*     (none)        P2/1
  01*     (none)        P2/3
  02*     (none)        P2/5
  03*     (none)        P2/7
  04*     (none)        P2/9
  05*     (none)        P2/11
  06*     (none)        P2/13
  07*     (none)        P2/15

  ANALOG OUTPUT
  -------------
  Index   Function      Connector/Pin
  -----   --------      -------------
  00      X amp ref     P3/2
  01      Y amp ref     P3/8
  02      Z amp ref     P3/5
  03      U amp ref     P3/11
  04**    V amp ref     P4/2
  05**    W amp ref     P4/8
  06**    (none)        P4/5
  07**    (none)        P4/11

  * = only available as option
  ** = only available for 8-axis boards, with STG_8_AXES defined

  */

int stgDioInit(const char * stuff)
{
  /* set digital IO bits for A,B input, C output */
  DIOdirection(0x0C);

  return 0;
}

int stgDioQuit()
{
  return 0;
}

int stgDioMaxInputs()
{
  return 24;
}

int stgDioMaxOutputs()
{
  return 24;
}

int stgDioRead(int index, int *value)
{
  unsigned char byte;
  unsigned char mask;

  mask = 1 << (index % 8);

  /* try A */
  if (index >= 0 && index < 8)
    {
      byte = _inp(STG_BASE_ADDRESS + DIO_A);
      *value = (byte & mask ? 1 : 0);
      return 0;
    }

  /* try B */
  if (index >= 8 && index < 16)
    {
      byte = _inp(STG_BASE_ADDRESS + DIO_B);
      *value = (byte & mask ? 1 : 0);
      return 0;
    }

  /* try D */
  if (index >= 16 && index < 24)
    {
      byte = _inp(STG_BASE_ADDRESS + DIO_D);
      *value = (byte & mask ? 1 : 0);
      return 0;
    }

  /* bad index */
  *value = 0;
  return -1;
}

/* writes go to the C register */
int stgDioWrite(int index, int value)
{
  if (index < 0 || index >= 8)
    {
      return -1;
    }

  RawDO(index, (unsigned char) value, 2); /* 2 is port C */

  return 0;
}

/* we can really read the outputs in the C register */
int stgDioCheck(int index, int *value)
{
  unsigned char byte;
  unsigned char mask;

  if (index < 0 || index >= 8)
    {
      *value = 0;
      return -1;
    }

  byte = _inp(STG_BASE_ADDRESS + DIO_C);
  mask = 1 << index;

  *value = (byte & mask ? 1 : 0);

  return 0;
}

/* can read bytes 0..2 */
int stgDioByteRead(int index, unsigned char *byte)
{
  if (index == 0)
    {
      *byte = _inp(STG_BASE_ADDRESS + DIO_A);
      return 0;
    }

  if (index == 1)
    {
      *byte = _inp(STG_BASE_ADDRESS + DIO_B);
      return 0;
    }

  if (index == 2)
    {
      *byte = _inp(STG_BASE_ADDRESS + DIO_D);
      return 0;
    }

  *byte = 0;
  return -1;
}

/* can read shorts 0..1, 0 is BA, 1 is 0D */
int stgDioShortRead(int index, unsigned short *sh)
{
  unsigned char lo, hi;

  if (index == 0)
    {
      lo = _inp(STG_BASE_ADDRESS + DIO_A);
      hi = _inp(STG_BASE_ADDRESS + DIO_B);

      *sh = (hi << 8) + lo;
      return 0;
    }

  if (index == 1)
    {
      lo = _inp(STG_BASE_ADDRESS + DIO_D);

      *sh = lo;
      return 0;
    }

  *sh = 0;
  return -1;
}

/* can only read 1 word, and this is 0DBA */
int stgDioWordRead(int index, unsigned int *word)
{
  if (index != 0)
    {
      *word = 0;
      return -1;
    }

  *word = _inp(STG_BASE_ADDRESS + DIO_D) << 16;
  *word += _inp(STG_BASE_ADDRESS + DIO_B) << 8;
  *word += _inp(STG_BASE_ADDRESS + DIO_A);

  return 0;
}

/* index is 0 only for port C */
int stgDioByteWrite(int index, unsigned char byte)
{
  if (index == 0)
    {
      _outp(STG_BASE_ADDRESS + DIO_C, byte);
      return 0;
    }

  return -1;
}

/* can't do a short write-- only 1 byte of digital out */
int stgDioShortWrite(int index, unsigned short sh)
{
  return -1;
}

/* can't do a word write-- only 1 byte of digital out */
int stgDioWordWrite(int index, unsigned int word)
{
  return -1;
}

/* can do a byte check on output C register */
int stgDioByteCheck(int index, unsigned char *byte)
{
  if (index == 0)
    {
      *byte = _inp(STG_BASE_ADDRESS + DIO_C);
      return 0;
    }

  *byte = 0;
  return -1;
}

/* can't read a short-- only 8 bits of digital out */
int stgDioShortCheck(int index, unsigned short *sh)
{
  *sh = 0;
  return -1;
}

/* cant' read a word-- only 8 bits of digital out */
int stgDioWordCheck(int index, unsigned int *word)
{
  *word = 0;
  return -1;
}

int stgAioInit(const char * stuff)
{
  /* nothing need be done */
  return 0;
}

int stgAioQuit()
{
  return 0;
}

int stgAioMaxInputs()
{
  return 8;                     /* make sure you have this option */
}

int stgAioMaxOutputs()
{
  return STG_MAX_AXIS;
}

int stgAioStart(int index)
{
  return stgAdcStart(index);
}

void stgAioWait(void)
{
  stgAdcWait();
}

int stgAioRead(int index, double *volts)
{
  return stgAdcRead(index, volts);
}

int stgAioWrite(int index, double volts)
{
  return stgDacWrite(index, volts);
}

int stgAioCheck(int index, double *volts)
{
  if (index < 0 || index >= STG_MAX_AXIS) {
    *volts = 0.0;
    return 0;
  }

  /* can't read outputs directly, so return stored value */
  *volts = checkedOutputs[index];
  return 0;
}

int stgModel()
{
  return 1;
}

---