Logo Search packages:      
Sourcecode: allegro4.2 version File versions  Download package

swpp.c

/*         ______   ___    ___
 *        /\  _  \ /\_ \  /\_ \
 *        \ \ \L\ \\//\ \ \//\ \      __     __   _ __   ___
 *         \ \  __ \ \ \ \  \ \ \   /'__`\ /'_ `\/\`'__\/ __`\
 *          \ \ \/\ \ \_\ \_ \_\ \_/\  __//\ \L\ \ \ \//\ \L\ \
 *           \ \_\ \_\/\____\/\____\ \____\ \____ \ \_\\ \____/
 *            \/_/\/_/\/____/\/____/\/____/\/___L\ \/_/ \/___/
 *                                           /\____/
 *                                           \_/__/
 *
 *      Preliminary driver for Microsoft Sidewinder 3D/Precision/
 *      Force Feedback Pro Joysticks.
 *
 *      By Acho A. Tang.
 *
 *      Notes on the 3D Pro:
 *        - digital initialization is not 100% reliable,
 *        - toggling the TM/CH switch will put the joystick back
 *          in analogue mode,
 *        - the stick must remain in a near-neutral position during
 *          detection or the process will fail.
 *
 *      Notes on the Force Feedback Pro:
 *        - the driver has not been tested on a real FF Pro and it
 *          lacks force feedback support.
 *
 *      See readme.txt for copyright information.
 */


#include <stdio.h>
#include <string.h>
#include "allegro.h"
#include "allegro/internal/aintern.h"
#include "allegro/platform/aintdos.h"

#ifndef ALLEGRO_DOS
   #error something is wrong with the makefile
#endif


#define SWPP_DEBUG      0 /* Compiler switch for a debug-only build */
#define SWPP_VERBOSE    0 /* Compiler switch for debug messages */

#define SWPP_NPORTS     2 /* Number of game ports to scan */
#define SWPP_NSCANS     8 /* Number of scan attempts on each port */
#define SWPP_NSUCCESS   4 /* Number of successful attempts to pass detection */
#define SWPP_WAITID   800 /* Max wait for ID packet triplets [800 us](arbitrary) */
#define SWPP_START    400 /* Max wait for the first data triplet [400 us](arbitrary) */
#define SWPP_STROBE    50 /* Max wait for each successive data triplet [50 us](theoretical minimum [11 us]) */
#define SWPP_CMDELAY   80 /* Interrupt command delay [80 us](must be within X timer's period [354.2 us]) */
#define SWPP_MAXBITS  256 /* Max number of triplets */

#define NTSC_CLOCK 14318180.0
#define SW3D_POLLOUT 3000 /* Max analogue polling timeout [3 ms] */
#define SW3D_FID       32
#define SW3D_TSUB1     (115*2+(305-115)*2/SW3D_FID) /* SW3D A->D time constant 1 */
#define SW3D_TSUB2     (SW3D_TSUB1+697+775)         /* SW3D A->D time constant 2 */
#define SW3D_TSUB3     (SW3D_TSUB1+288+312)         /* SW3D A->D time constant 3 */

#define SWPP_NID       57 /* No. of triplets in a Precision Pro full packet */
#define SWFF_NID       30 /* No. of triplets in a Force Feedback Pro full packet *** UNCONFIRMED *** */
#define SW3D_NID      226 /* No. of triplets in a 3D Pro full packet */

#define SWPP_NDATA     16 /* No. of triplets in a Precision Pro data packet */
#define SWFF_NDATA     16 /* No. of triplets in a Force Feedback Pro data packet */
#define SW3D_NDATA     22 /* No. of triplets in a 3D Pro data packet */
#define SW3D_NSTREAM   (SW3D_NDATA*3)

enum { SWXX=0, SWPP, SWFF, SW3D };


/* driver functions */
static int swpp_init(void);
static void swpp_exit(void);
static int swpp_poll(void);

#if !SWPP_DEBUG
JOYSTICK_DRIVER joystick_sw_pp =
{
      JOY_TYPE_SIDEWINDER_PP,
      empty_string,
      empty_string,
      "Sidewinder Precision Pro",
      swpp_init,
      swpp_exit,
      swpp_poll,
      NULL,  /* AL_METHOD(int, save_data, (void)); */
      NULL,  /* AL_METHOD(int, load_data, (void)); */
      NULL,  /* AL_METHOD(AL_CONST char *, calibrate_name, (int n)); */
      NULL   /* AL_METHOD(int, calibrate, (int n)); */
};
#endif


/* global context */
00097 static struct SWINF_TYPE
{
      unsigned int port, speed, mode, nid, ndata;
      unsigned int buttons;
      int hatx, haty;
      int x, y;
      int throttle, twist;
} swinf;

static int swpp_initialized = 0;


/* utility macros */
#define US2COUNT(u, s) ((u * s) >> 10)
#define WAITSWPP(p, c) do inportb(p); while (--c > 0)


/* estimates game port speed in queries/ms (WARNING: timer 2 will be reprogrammed) */
extern void _swpp_get_portspeed(unsigned int port, unsigned int npasses, unsigned int *nqueries, unsigned int *elapsed);

static unsigned int get_portspeed(unsigned int port)
{
#define NQUERIES 1024

      unsigned int elapsed[5], nqueries[5]={1,NQUERIES+1,NQUERIES+1,NQUERIES+1,1};
      unsigned int emin=-1, emax=0, e, imin=0, imax=0, i;
      double e64, r64;
      unsigned char ah, al;

      ah = al = inportb(0x61); al &= ~2; al |= 1; outportb(0x61, al); /* enable timer channel 2 */
      outportb(0x43, 0xb4); /* wake timer */


      _enter_critical();

      _swpp_get_portspeed(port, 5, nqueries, elapsed);

      _exit_critical();


      outportb(0x61, ah);

      for (i=1; i<4; i++)
      {
            e = elapsed[i];

            if (e <  emin) { emin = e; imin = i; }
            if (e >= emax) { emax = e; imax = i; }
      }

      e = elapsed[6-imin-imax] - elapsed[4];

      if ((int)e > 0)
      {
            r64 = (double)e * 1000000;
            e64 = 1193182.0 * NQUERIES * 1024;
            e64 /= r64;

            e = (unsigned int)e64;
      }
      else e = 0;

#if SWPP_VERBOSE
      printf("SWPP get_portspeed(%xh): %u queries/ms\n", port, e);
#endif

      return(e);

#undef NQUERIES
}


/* attempts to switch the Sidewinder 3D Pro into digital mode
 *
 * WARNING: No time-out during polling. Computer may freeze if timer gets
 *          stuck although it rarely happens. A better solution is to use
 *          timer 0 interrupt provided that it's not being used by Allegro.
 */
extern void _swpp_init_digital(unsigned int port, unsigned int ncmds, unsigned char *cmdiv, unsigned int *timeout);

static int init_digital(unsigned int port, unsigned int speed)
{
      unsigned char cmdiv[6];
      unsigned char ah, al;
      unsigned int i;

      ah = al = inportb(0x61); al &= ~2; al |= 1; outportb(0x61, al); /* enable timer channel 2 */
      outportb(0x43, 0xb0); /* wake timer */

      i = NTSC_CLOCK * SW3D_TSUB1 / 24000000; /* clock divisor for command 1 delay */
      cmdiv[0] = i & 0xff;
      cmdiv[1] = i>>8;

      i = NTSC_CLOCK * SW3D_TSUB2 / 24000000; /* clock divisor for command 2 delay */
      cmdiv[2] = i & 0xff;
      cmdiv[3] = i>>8;

      i = NTSC_CLOCK * SW3D_TSUB3 / 24000000; /* clock divisor for command 3 delay */
      cmdiv[4] = i & 0xff;
      cmdiv[5] = i>>8;

      i = US2COUNT(SW3D_POLLOUT * 3, speed); /* axis 0 timeout */


      _enter_critical();

      _swpp_init_digital(port, 3, cmdiv, &i);

      _exit_critical();


      outportb(0x61, ah);

#if SWPP_VERBOSE
      printf("SWPP init_digital(%xh): D1=%x%02x D2=%x%02x D3=%x%02x\n",
                  port, cmdiv[1], cmdiv[0], cmdiv[3], cmdiv[2], cmdiv[5], cmdiv[4]);
#endif

      return(i);
}


/* standard 4-axis polling */
extern void _swpp_read_analogue(unsigned int port, unsigned int *outbuf, unsigned int pollout);

static int read_analogue(unsigned int port, unsigned int speed, unsigned char *buf)
{
      unsigned int outbuf[6];
      unsigned int pollout, i;

      if (inportb(port))
      {
            pollout = US2COUNT(SW3D_POLLOUT, speed);


            _enter_critical();

            _swpp_read_analogue(port, outbuf, pollout);

            _exit_critical();


            if (!outbuf[5])
                  for (i=1; i<5; i++) { if (outbuf[i] >= pollout) outbuf[i] = -1; }
      }
      else
      {
            for (i=0; i<6; i++) outbuf[i] = -1;
            pollout = -1;
      }

#if SWPP_VERBOSE
      i = outbuf[0];

      printf("SWPP read_analogue(%xh): limit=%d A0=%d A1=%d A2=%d A3=%d B=%04x\n", port, pollout,
                  outbuf[1], outbuf[2], outbuf[3], outbuf[4], (i&1)|((i&2)<<3)|((i&4)<<6)|((i&8)<<9));
#endif

      for (i=0; i<6; i++) buf[i] = (unsigned char)outbuf[i];

      return(~outbuf[0] & 0x10);
}


/* thanks to Vojtech (vojtech@suse.cz) for the technical infos */
static unsigned int read_packet(unsigned int port, unsigned int speed, unsigned char *buf, int length, int id)
{
      int timeout, start, strobe, cmdelay, cmout=0, count=0, i;
      unsigned char ah, al=0;

      if (id)
      {
            timeout = start = US2COUNT(SWPP_WAITID, speed);
            cmdelay = US2COUNT(SWPP_CMDELAY, speed);

            _enter_critical();

            outportb(port, 0);
            al = inportb(port);
            do
            {
                  ah = al;
                  al = inportb(port);

                  /* rising edge on button 0 */
                  if (al & ~ah & 0x10)
                  {
                        buf[count++] = al >> 5;
                        if (count >= length) break;
                        timeout = start;
                  }

                  /* falling edge on axis 0 */
                  if (cmdelay && ~al & ah & 0x01) cmout = 1;
                  if (cmout && !(--cmdelay))
                  {
                        cmout = 0;
                        outportb(port, 0);
                  }
            }
            while (--timeout);

            _exit_critical();

      }
      else
      {
            timeout = start = US2COUNT(SWPP_START, speed);
            strobe = US2COUNT(SWPP_STROBE, speed);

            _enter_critical();

            outportb(port, 0);
            al = inportb(port);
            do
            {
                  ah = al;
                  al = inportb(port);

                  /* rising edge on button 0 */
                  if (al & ~ah & 0x10)
                  {
                        buf[count++] = al >> 5;
                        if (count >= length) break;
                        timeout = strobe;
                  }
            }
            while (--timeout);

            _exit_critical();
      }

#if SWPP_VERBOSE
      printf("SWPP read_packet(%xh): %d triplets [", port, count);
      for (i=0; i<count; i++) printf("%1x", buf[i]); printf("]\n");
#endif

      return(count);
}


/* compares packet parity against the parity bit */
static unsigned int check_parity(unsigned char *buf, unsigned int length)
{
      unsigned char xbits[8]={0,1,1,0,1,0,0,1};
      unsigned char soft_parity, hard_parity, i, j;

      j = (unsigned char)length;
      length--;

      soft_parity = 1;
      hard_parity = buf[length] >> 2;
      buf[length] &= 3;

      for (i=0; i<j; i++) soft_parity ^= xbits[buf[i]];

      return(soft_parity == hard_parity);
}


/* verifies sync parity, compares checksum, and bit-packs good SW3D triplets */
static unsigned int pack_sw3d(unsigned char *buf, unsigned int length)
{
      unsigned char *workbuf;
      unsigned long pack64[2];
      unsigned long temp32;
      int sum, shift, n, i;

      if ((i = length - SW3D_NSTREAM) < 0) return(0);
      workbuf = buf + i + SW3D_NSTREAM + SW3D_NDATA;

      for (n=-SW3D_NSTREAM; n; n+=SW3D_NDATA)
      {
            pack64[0] = pack64[1] = 0;
            sum = shift = 0;

            for (i=-SW3D_NDATA; i; i++)
            {
                  temp32 = (unsigned long)workbuf[n+i];

                  if (shift > 29 && shift < 32)
                  {
                        pack64[0] |= temp32<<shift;
                        pack64[1] |= temp32>>(32-shift);
                  }
                  else
                        pack64[shift>>5 & 1] |= temp32<<(shift & 0x1f);

                  shift += 3;
            }

            if ((pack64[0]^0x80) & 0x80808080 || pack64[1] & 0x80808080) continue;

            temp32 = pack64[0];
            do sum += temp32 & 0x0f; while (temp32 >>= 4);

            temp32 = pack64[1];
            do sum += temp32 & 0x0f; while (temp32 >>= 4);

            if (!(sum & 0x0f))
            {
                  ((unsigned long *)buf)[0] = pack64[0];
                  ((unsigned long *)buf)[1] = pack64[1];
                  return(1);
            }
      }

      return(0);
}


/* parses SWPP triplets into meaningful values */
static void decode_swpp(struct SWINF_TYPE *inf, unsigned char *buf)
{
      int data, pos;

      data = buf[0] | buf[1]<<3 | buf[2]<<6;
      inf->buttons = ~data & 0x1ff;

      data = buf[3] | buf[4]<<3 | buf[5]<<6 | (buf[6]&1)<<9;
      inf->x = (data >> 2) - 128;

      data = (buf[6]&6)>>1 | buf[7]<<2 | buf[8]<<5 | (buf[9]&3)<<8;
      inf->y = (data >> 2) - 128;

      data = (buf[9]&4)>>2 | buf[10]<<1 | buf[11]<<4;
      data = ~data & 0x7f;
      inf->throttle = (data << 1) + (data >> 6) - 128;

      data = buf[12] | buf[13]<<3;
      inf->twist = (data << 2) + (data >> 4) - 128;

      data = buf[14] | (buf[15]&1)<<3;
      data = (1 << (data - 1)) & 0xff;

      if (data & 0xe0) pos = -128; else
      if (data & 0x0e) pos = 127; else
      pos = 0;
      inf->hatx = pos;

      if (data & 0x83) pos = -128; else
      if (data & 0x38) pos = 127; else
      pos = 0;
      inf->haty = pos;

#if SWPP_VERBOSE
      printf("SWPP decode_swpp(): x=%d y=%d tw=%d th=%d hx=%d hy=%d B=%03x\n",
                  inf->x, inf->y, inf->twist, inf->throttle, inf->hatx, inf->haty, inf->buttons);
#endif
}


/* parse packed SW3D triplets into meaningful values */
static void decode_sw3d(struct SWINF_TYPE *inf, unsigned char *buf)
{
      int data, pos;

      data = (buf[1]&0x7f) | (buf[4]&0x40)<<1;
      inf->buttons = (~data & 0xff) | (buf[4]&0x20)<<3;

      data = (buf[0]&0x38)<<4 | (buf[2]&0x7f);
      inf->x = (data >> 2) - 128;

      data = (buf[0]&0x07)<<7 | (buf[3]&0x7f);
      inf->y = (data >> 2) - 128;

      data = (buf[4]&0x18)<<4 | (buf[5]&0x7f);
      inf->twist = (data >> 1) - 128;

      data = (buf[4]&0x07)<<7 | (buf[6]&0x7f);
      data = ~data & 0x3ff;
      inf->throttle = (data >> 2) -128;

      data = (buf[0]&0x40)>>3 | (buf[7]&0x70)>>4;
      data = (1 << (data - 1)) & 0xff;

      if (data & 0xe0) pos = -128; else
      if (data & 0x0e) pos = 127; else
      pos = 0;
      inf->hatx = pos;

      if (data & 0x83) pos = -128; else
      if (data & 0x38) pos = 127; else
      pos = 0;
      inf->haty = pos;

#if SWPP_VERBOSE
      printf("SWPP decode_sw3d(): x=%d y=%d tw=%d th=%d hx=%d hy=%d B=%03x\n",
                  inf->x,inf->y,inf->twist,inf->throttle,inf->hatx,inf->haty,inf->buttons);
#endif
}


/* initializes the driver */
static int swpp_init(void)
{
      unsigned char buf[SWPP_MAXBITS];
      unsigned int portlist[SWPP_NPORTS]={0x201,0x200};
      unsigned int port=-1, speed=-1, mode=-1, ndata=-1, nid=-1, avgspeed=0;
      unsigned int tries=SWPP_NSCANS, lockport=0, success=0, counter, i;

#if !SWPP_DEBUG
      /* fill joystick info */
      num_joysticks = 1;

      joy[0].flags = JOYFLAG_ANALOGUE | JOYFLAG_SIGNED;
      joy[0].num_sticks = 4;
      joy[0].num_buttons = 9;

      joy[0].stick[0].flags = JOYFLAG_ANALOGUE | JOYFLAG_SIGNED;
      joy[0].stick[0].num_axis = 2;
      joy[0].stick[0].name = get_config_text("Stick");
      joy[0].stick[0].axis[0].name = get_config_text("X");
      joy[0].stick[0].axis[1].name = get_config_text("Y");

      joy[0].stick[1].flags = JOYFLAG_ANALOGUE | JOYFLAG_SIGNED;
      joy[0].stick[1].num_axis = 1;
      joy[0].stick[1].name = get_config_text("Twist");
      joy[0].stick[1].axis[0].name = get_config_text("");

      joy[0].stick[2].flags = JOYFLAG_ANALOGUE | JOYFLAG_SIGNED;
      joy[0].stick[2].num_axis = 1;
      joy[0].stick[2].name = get_config_text("Throttle");
      joy[0].stick[2].axis[0].name = get_config_text("");

      joy[0].stick[3].flags = JOYFLAG_DIGITAL | JOYFLAG_SIGNED;
      joy[0].stick[3].num_axis = 2;
      joy[0].stick[3].name = get_config_text("Hat");
      joy[0].stick[3].axis[0].name = get_config_text("X");
      joy[0].stick[3].axis[1].name = get_config_text("Y");

      joy[0].button[0].name = get_config_text("B1");
      joy[0].button[1].name = get_config_text("B2");
      joy[0].button[2].name = get_config_text("B3");
      joy[0].button[3].name = get_config_text("B4");
      joy[0].button[4].name = get_config_text("B5");
      joy[0].button[5].name = get_config_text("B6");
      joy[0].button[6].name = get_config_text("B7");
      joy[0].button[7].name = get_config_text("B8");
      joy[0].button[8].name = get_config_text("B9");
#endif

      if (swpp_initialized)
      {
            read_packet(swinf.port, swinf.speed, buf, SWPP_MAXBITS, 1);
            counter = US2COUNT(50000, swinf.speed); WAITSWPP(swinf.port, counter);

            return(0);
      }

      /* do a simple detection run on each port */
      do
      {
            i = 0;
            do
            {
                  if (!lockport) port = portlist[i];

                  /* check port speed */
                  if (!(speed = get_portspeed(port))) continue;
                  counter = US2COUNT(50000, speed); WAITSWPP(port, counter);

                  /* check X timer */
                  if (!read_analogue(port, speed, buf)) continue;
                  counter = US2COUNT(50000, speed); WAITSWPP(port, counter);

                  /* check ID */
                  if (!(nid = read_packet(port, speed, buf, SWPP_MAXBITS, 1)))
                  {
                        /* initiate digital mode and recheck ID */
                        if (!init_digital(port, speed)) continue;
                        counter = US2COUNT(300000, speed); WAITSWPP(port, counter);

                        if (!(nid = read_packet(port, speed, buf, SWPP_MAXBITS, 1))) continue;
                  }
                  counter = US2COUNT(50000, speed); WAITSWPP(port, counter);
                  lockport = 1;

                  /* check packet length and parity */
                  ndata = read_packet(port, speed, buf, SWPP_MAXBITS, 0);
                  counter = US2COUNT(50000, speed); WAITSWPP(port, counter);

                  mode = SWXX;

                  if (nid == SWPP_NID)
                  {
                        if (ndata != SWPP_NDATA || !check_parity(buf, ndata)) continue;
                        mode = SWPP;
                  }
                  else if (nid >= SW3D_NID)
                  {
                        if (ndata < SW3D_NSTREAM || !pack_sw3d(buf, ndata)) continue;
                        mode = SW3D; nid = SW3D_NID; ndata = SW3D_NSTREAM;
                  }
                  else
                  {
                        if (ndata != SWFF_NDATA || !check_parity(buf, ndata)) continue;
                        mode = SWFF;
                  }

                  avgspeed += speed;
                  success++;
            }
            while (!lockport && ++i < SWPP_NPORTS);
      }
      while (success < SWPP_NSUCCESS && --tries + success >= SWPP_NSUCCESS);

      if (success < SWPP_NSUCCESS) { swinf.mode = mode; return(-1); }

      avgspeed /= SWPP_NSUCCESS;

      memset(&swinf, 0, sizeof(struct SWINF_TYPE));
      swinf.port = port;
      swinf.speed = avgspeed;
      swinf.mode = mode;
      swinf.nid = nid;
      swinf.ndata = ndata;

      swpp_initialized = 1;

#if SWPP_VERBOSE
      printf("SWPP swpp_init(): port=%xh speed=%d mode=%d nid=%d ndata=%d\n",port, avgspeed, mode, nid, ndata);
#endif

      return(0);
}


/* polls the joystick state */
static int swpp_poll(void)
{
      unsigned char buf[SWPP_MAXBITS];
      unsigned int i;
      int d;

      switch (swinf.mode)
      {
            case SWPP:
            case SWFF:
                  if (read_packet(swinf.port, swinf.speed, buf, swinf.ndata, 0) == swinf.ndata)
                        if (check_parity(buf, swinf.ndata)) decode_swpp(&swinf, buf);
            break;

            case SW3D:
                  if ((i = read_packet(swinf.port, swinf.speed, buf, swinf.ndata+1, 0)) >= swinf.ndata)
                        if (pack_sw3d(buf, i)) decode_sw3d(&swinf, buf);
            break;
      }

#if !SWPP_DEBUG
      d = swinf.x;
      joy[0].stick[0].axis[0].pos = d;
      joy[0].stick[0].axis[0].d1  = d <= -64;
      joy[0].stick[0].axis[0].d2  = d >= 64;

      d = swinf.y;
      joy[0].stick[0].axis[1].pos = d;
      joy[0].stick[0].axis[1].d1  = d <= -64;
      joy[0].stick[0].axis[1].d2  = d >= 64;

      d = swinf.twist;
      joy[0].stick[1].axis[0].pos = d;
      joy[0].stick[1].axis[0].d1  = d <= -64;
      joy[0].stick[1].axis[0].d2  = d >= 64;

      d = swinf.throttle;
      joy[0].stick[2].axis[0].pos = d;
      joy[0].stick[2].axis[0].d1  = d <= -64;
      joy[0].stick[2].axis[0].d2  = d >= 64;

      d = swinf.hatx;
      joy[0].stick[3].axis[0].pos = d;
      joy[0].stick[3].axis[0].d1 = d <= -64;
      joy[0].stick[3].axis[0].d2 = d >= 64;

      d = swinf.haty;
      joy[0].stick[3].axis[1].pos = d;
      joy[0].stick[3].axis[1].d1 = d <= -64;
      joy[0].stick[3].axis[1].d2 = d >= 64;

      d = swinf.buttons;
      for (i=0; i<9; i++) joy[0].button[i].b = d & (1 << i);
#endif

      return(0);
}


/* shuts down the driver */
static void swpp_exit(void) {}


#if SWPP_DEBUG
int main(void)
{
      swpp_init();

      if (swinf.mode != -1)
      {
            printf("\nFound ");
            switch (swinf.mode)
            {
                  default:
                  case SWXX: printf("unsupported Sidewinder input device\n"); break;
                  case SWPP: printf("Sidewinder Precision Pro joystick\n"); break;
                  case SWFF: printf("Sidewinder Force Feedback Pro joystick\n"); break;
                  case SW3D: printf("Sidewinder 3D Pro joystick\n");
            }
      }
      else printf("\nNo Sidewinder joystick found\n");

      return(0);
}
#endif

Generated by  Doxygen 1.6.0   Back to index