src/haptic/linux/SDL_syshaptic.c

/*
    SDL - Simple DirectMedia Layer
    Copyright (C) 2008 Edgar Simo

    This library is free software; you can redistribute it and/or
    modify it under the terms of the GNU Lesser General Public
    License as published by the Free Software Foundation; either
    version 2.1 of the License, or (at your option) any later version.

    This library is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
    Lesser General Public License for more details.

    You should have received a copy of the GNU Lesser General Public
    License along with this library; if not, write to the Free Software
    Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA

    Sam Lantinga
    slouken@libsdl.org
*/
#include "SDL_config.h"

#ifdef SDL_HAPTIC_LINUX

#include "SDL_haptic.h"
#include "../SDL_syshaptic.h"
#include "SDL_joystick.h"
#include "../../joystick/SDL_sysjoystick.h" /* For the real SDL_Joystick */
#include "../../joystick/linux/SDL_sysjoystick_c.h" /* For joystick hwdata */ 

#include <unistd.h> /* close */
#include <linux/input.h>
#include <sys/ioctl.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <linux/limits.h>
#include <string.h>
#include <errno.h>
#include <math.h>

#ifndef M_PI
#  define M_PI     3.14159265358979323846
#endif


#define MAX_HAPTICS  32


/*
 * List of available haptic devices.
 */
static struct
{
   char *fname;
   SDL_Haptic *haptic;
} SDL_hapticlist[MAX_HAPTICS];


/*
 * Haptic system hardware data.
 */
struct haptic_hwdata
{
   int fd;
};


/*
 * Haptic system effect data.
 */
struct haptic_hweffect
{
   struct ff_effect effect; /* The linux kernel effect structure. */
};


#define test_bit(nr, addr) \
   (((1UL << ((nr) & 31)) & (((const unsigned int *) addr)[(nr) >> 5])) != 0)
#define EV_TEST(ev,f) \
   if (test_bit((ev), features)) ret |= (f);
/*
 * Test whether a device has haptic properties.
 * Returns available properties or 0 if there are none.
 */
static int
EV_IsHaptic(int fd)
{
   unsigned int ret;
   unsigned long features[1 + FF_MAX/sizeof(unsigned long)];

   /* Ask device for what it has. */
   ret = 0;
   if (ioctl(fd, EVIOCGBIT(EV_FF, sizeof(features)), features) < 0) {
      SDL_SetError("Unable to get device's haptic abilities: %s", strerror(errno));
      return -1;
   }

   /* Convert supported features to SDL_HAPTIC platform-neutral features. */
   EV_TEST(FF_CONSTANT,   SDL_HAPTIC_CONSTANT);
   EV_TEST(FF_PERIODIC,   SDL_HAPTIC_SINE |
                          SDL_HAPTIC_SQUARE |
                          SDL_HAPTIC_TRIANGLE |
                          SDL_HAPTIC_SAWTOOTHUP |
                          SDL_HAPTIC_SAWTOOTHDOWN);
   EV_TEST(FF_RAMP,       SDL_HAPTIC_RAMP);
   EV_TEST(FF_SPRING,     SDL_HAPTIC_SPRING);
   EV_TEST(FF_FRICTION,   SDL_HAPTIC_FRICTION);
   EV_TEST(FF_DAMPER,     SDL_HAPTIC_DAMPER);
   EV_TEST(FF_INERTIA,    SDL_HAPTIC_INERTIA);
   EV_TEST(FF_CUSTOM,     SDL_HAPTIC_CUSTOM);
   EV_TEST(FF_GAIN,       SDL_HAPTIC_GAIN);
   EV_TEST(FF_AUTOCENTER, SDL_HAPTIC_AUTOCENTER);

   /* Return what it supports. */
   return ret;
}


/*
 * Tests whether a device is a mouse or not.
 */
static int
EV_IsMouse(int fd)
{
   unsigned long argp[40];

   /* Ask for supported features. */
   if (ioctl(fd, EVIOCGBIT(EV_KEY, sizeof(argp)), argp) < 0) {
      return -1;
   }

   /* Currently we only test for BTN_MOUSE which can give fake positives. */
   if (test_bit(BTN_MOUSE,argp) != 0) {
      return 1;
   }

   return 0;
}

/*
 * Initializes the haptic subsystem by finding available devices.
 */
int
SDL_SYS_HapticInit(void)
{
   const char joydev_pattern[] = "/dev/input/event%d";
   dev_t dev_nums[MAX_HAPTICS];
   char path[PATH_MAX];
   struct stat sb;
   int fd;
   int i, j, k;
   int duplicate;
   int numhaptics;

   numhaptics = 0;

   /* 
    * Limit amount of checks to MAX_HAPTICS since we may or may not have
    * permission to some or all devices.
    */
   i = 0;
   for (j = 0; j < MAX_HAPTICS; ++j) {

      snprintf(path, PATH_MAX, joydev_pattern, i++);

      /* check to see if file exists */
      if (stat(path,&sb) != 0)
         break;

      /* check for duplicates */
      duplicate = 0;
      for (k = 0; (k < numhaptics) && !duplicate; ++k) {
         if (sb.st_rdev == dev_nums[k]) {
            duplicate = 1;
         }                                                     
      }                                                         
      if (duplicate) {
         continue;
      }

      /* try to open */
      fd = open(path, O_RDWR, 0);
      if (fd < 0) continue;

#ifdef DEBUG_INPUT_EVENTS
      printf("Checking %s\n", path);
#endif          

      /* see if it works */
      if (EV_IsHaptic(fd) > 0) {
         SDL_hapticlist[numhaptics].fname = SDL_strdup(path);
         SDL_hapticlist[numhaptics].haptic = NULL;
         dev_nums[numhaptics] = sb.st_rdev;
         ++numhaptics;
      }
      close(fd);
   }

   return numhaptics;
}


/*
 * Return the name of a haptic device, does not need to be opened.
 */
const char *
SDL_SYS_HapticName(int index)
{
   int fd;
   static char namebuf[128];
   char *name;

   /* Open the haptic device. */
   name = NULL;
   fd = open(SDL_hapticlist[index].fname, O_RDONLY, 0);

   if (fd >= 0) {

      /* Check for name ioctl. */
      if (ioctl(fd, EVIOCGNAME(sizeof(namebuf)), namebuf) <= 0) {

         /* No name found, return device character device */
         name = SDL_hapticlist[index].fname;
      }
      /* Name found, return name. */
      else {
         name = namebuf;
      }
   }
   close(fd);

   return name;
}


/*
 * Opens the haptic device from the file descriptor.
 */
static int
SDL_SYS_HapticOpenFromFD(SDL_Haptic * haptic, int fd)
{
   /* Allocate the hwdata */
   haptic->hwdata = (struct haptic_hwdata *)
         SDL_malloc(sizeof(*haptic->hwdata));
   if (haptic->hwdata == NULL) {
      SDL_OutOfMemory();
      goto open_err;
   }
   SDL_memset(haptic->hwdata, 0, sizeof(*haptic->hwdata));

   /* Set the data */
   haptic->hwdata->fd = fd;
   haptic->supported = EV_IsHaptic(fd);

   /* Set the effects */
   if (ioctl(fd, EVIOCGEFFECTS, &haptic->neffects) < 0) {
      SDL_SetError("Unable to query haptic device memory: %s", strerror(errno));
      goto open_err;
   }
   haptic->effects = (struct haptic_effect *)
         SDL_malloc(sizeof(struct haptic_effect) * haptic->neffects);
   if (haptic->effects == NULL) {
      SDL_OutOfMemory();
      goto open_err;
   }
   /* Clear the memory */
   SDL_memset(haptic->effects, 0,
         sizeof(struct haptic_effect) * haptic->neffects);

   return 0;

   /* Error handling */
open_err:
   close(fd);
   if (haptic->hwdata != NULL) {
      free(haptic->hwdata);
      haptic->hwdata = NULL;
   }
   return -1;
}


/*
 * Opens a haptic device for usage.
 */
int
SDL_SYS_HapticOpen(SDL_Haptic * haptic)
{
   int fd;

   /* Open the character device */
   fd = open(SDL_hapticlist[haptic->index].fname, O_RDWR, 0);
   if (fd < 0) {
      SDL_SetError("Unable to open %s: %s",
            SDL_hapticlist[haptic->index], strerror(errno));
      return -1;
   }
   
   return SDL_SYS_HapticOpenFromFD(haptic,fd);
}


/*
 * Opens a haptic device from first mouse it finds for usage.
 */
int
SDL_SYS_HapticMouse(void)
{
   int fd;
   int i;

   for (i=0; i<SDL_numhaptics; i++) {

      /* Open the device. */
      fd = open(SDL_hapticlist[i].fname, O_RDWR, 0);
      if (fd < 0) {
         SDL_SetError("Unable to open %s: %s",
               SDL_hapticlist[i], strerror(errno));
         return -1;
      }

      if (EV_IsMouse(fd)) {
         close(fd);
         return i;
      }

      close(fd);
   }
   
   return -1;
}


/*
 * Checks to see if a joystick has haptic features.
 */
int
SDL_SYS_JoystickIsHaptic(SDL_Joystick * joystick)
{
   return EV_IsHaptic(joystick->hwdata->fd);
}


/*
 * Checks to see if the haptic device and joystick and in reality the same.
 */
int
SDL_SYS_JoystickSameHaptic(SDL_Haptic * haptic, SDL_Joystick * joystick)
{
   if (SDL_strcmp(joystick->name,haptic->name)==0) {
      return 1;
   }
   return 0;
}


/*
 * Opens a SDL_Haptic from a SDL_Joystick.
 */
int
SDL_SYS_HapticOpenFromJoystick(SDL_Haptic * haptic, SDL_Joystick * joystick)
{
   int fd;
   fd = open(joystick->hwdata->fname, O_RDWR, 0);
   return SDL_SYS_HapticOpenFromFD(haptic,fd);
}


/*
 * Closes the haptic device.
 */
void
SDL_SYS_HapticClose(SDL_Haptic * haptic)
{
   if (haptic->hwdata) {

      /* Clean up */
      close(haptic->hwdata->fd);

      /* Free */
      SDL_free(haptic->hwdata);
      haptic->hwdata = NULL;
      SDL_free(haptic->effects);
      haptic->neffects = 0;
   }
}


/* 
 * Clean up after system specific haptic stuff
 */
void
SDL_SYS_HapticQuit(void)
{
   int i;

   for (i=0; SDL_hapticlist[i].fname != NULL; i++) {
      SDL_free(SDL_hapticlist[i].fname);
   }
   SDL_hapticlist[0].fname = NULL;
}

/*
 * Returns the ff_effect usable direction from a SDL_HapticDirection.
 */
static Uint16
SDL_SYS_ToDirection( SDL_HapticDirection * dir )
{
   Uint32 tmp;
   float f; /* Ideally we'd use fixed point math instead of floats... */

   switch (dir->type) {
      case SDL_HAPTIC_POLAR:
         /* Linux directions are inverted. */
         tmp = (((18000 + dir->dir[0]) % 36000) * 0xFFFF) / 36000;
         return (Uint16) tmp;
      case SDL_HAPTIC_CARTESIAN:
         /* We must invert "x" and "y" to go clockwise. */
         f = atan2(dir->dir[0], dir->dir[1]);
         tmp = (int)(f*18000./M_PI) % 36000;
         tmp = (tmp * 0xFFFF) / 36000;
         return (Uint16) tmp;

      default:
         return -1;
   }

   return 0;
}

#define  CLAMP(x)    (((x) > 32767) ? 32767 : x)
/*
 * Initializes the linux effect struct from a haptic_effect.
 * Values above 32767 (for unsigned) are unspecified so we must clamp.
 */
static int
SDL_SYS_ToFFEffect( struct ff_effect * dest, SDL_HapticEffect * src )
{
   int i;
   SDL_HapticConstant *constant;
   SDL_HapticPeriodic *periodic;
   SDL_HapticCondition *condition;
   SDL_HapticRamp *ramp;

   /* Clear up */
   SDL_memset(dest, 0, sizeof(struct ff_effect));

   switch (src->type) {
      case SDL_HAPTIC_CONSTANT:
         constant = &src->constant;

         /* Header */
         dest->type = FF_CONSTANT;
         dest->direction = SDL_SYS_ToDirection(&constant->direction);

         /* Replay */
         dest->replay.length = CLAMP(constant->length);
         dest->replay.delay = CLAMP(constant->delay);

         /* Trigger */
         dest->trigger.button = CLAMP(constant->button);
         dest->trigger.interval = CLAMP(constant->interval);

         /* Constant */
         dest->u.constant.level = constant->level;

         /* Envelope */
         dest->u.constant.envelope.attack_length = CLAMP(constant->attack_length);
         dest->u.constant.envelope.attack_level = CLAMP(constant->attack_level);
         dest->u.constant.envelope.fade_length = CLAMP(constant->fade_length);
         dest->u.constant.envelope.fade_level = CLAMP(constant->fade_level);

         break;

      case SDL_HAPTIC_SINE:
      case SDL_HAPTIC_SQUARE:
      case SDL_HAPTIC_TRIANGLE:
      case SDL_HAPTIC_SAWTOOTHUP:
      case SDL_HAPTIC_SAWTOOTHDOWN:
         periodic = &src->periodic;

         /* Header */
         dest->type = FF_PERIODIC;
         dest->direction = SDL_SYS_ToDirection(&periodic->direction);
         
         /* Replay */
         dest->replay.length = CLAMP(periodic->length);
         dest->replay.delay = CLAMP(periodic->delay);
         
         /* Trigger */
         dest->trigger.button = CLAMP(periodic->button);
         dest->trigger.interval = CLAMP(periodic->interval);
         
         /* Periodic */
         if (periodic->type == SDL_HAPTIC_SINE)
            dest->u.periodic.waveform = FF_SINE;
         else if (periodic->type == SDL_HAPTIC_SQUARE)
            dest->u.periodic.waveform = FF_SQUARE;
         else if (periodic->type == SDL_HAPTIC_TRIANGLE)       
            dest->u.periodic.waveform = FF_TRIANGLE;
         else if (periodic->type == SDL_HAPTIC_SAWTOOTHUP)       
            dest->u.periodic.waveform = FF_SAW_UP;
         else if (periodic->type == SDL_HAPTIC_SAWTOOTHDOWN)       
            dest->u.periodic.waveform = FF_SAW_DOWN;
         dest->u.periodic.period = CLAMP(periodic->period);
         dest->u.periodic.magnitude = periodic->magnitude;
         dest->u.periodic.offset = periodic->offset;
         dest->u.periodic.phase = CLAMP(periodic->phase);
         
         /* Envelope */
         dest->u.periodic.envelope.attack_length = CLAMP(periodic->attack_length);
         dest->u.periodic.envelope.attack_level = CLAMP(periodic->attack_level);
         dest->u.periodic.envelope.fade_length = CLAMP(periodic->fade_length);
         dest->u.periodic.envelope.fade_level = CLAMP(periodic->fade_level);

         break;

      case SDL_HAPTIC_SPRING:
      case SDL_HAPTIC_DAMPER:
      case SDL_HAPTIC_INERTIA:
      case SDL_HAPTIC_FRICTION:
         condition = &src->condition;

         /* Header */
         if (condition->type == SDL_HAPTIC_SPRING)
            dest->type = FF_SPRING;
         else if (condition->type == SDL_HAPTIC_DAMPER)
            dest->type = FF_DAMPER;
         else if (condition->type == SDL_HAPTIC_INERTIA)
            dest->type = FF_INERTIA;
         else if (condition->type == SDL_HAPTIC_FRICTION)
            dest->type = FF_FRICTION;
         dest->direction = 0; /* Handled by the condition-specifics. */

         /* Replay */
         dest->replay.length = CLAMP(condition->length);
         dest->replay.delay = CLAMP(condition->delay);

         /* Trigger */
         dest->trigger.button = CLAMP(condition->button);
         dest->trigger.interval = CLAMP(condition->interval);

         /* Condition */
         /* X axis */
         dest->u.condition[0].right_saturation = CLAMP(condition->right_sat[0]);
         dest->u.condition[0].left_saturation = CLAMP(condition->left_sat[0]);
         dest->u.condition[0].right_coeff = condition->right_coeff[0];
         dest->u.condition[0].left_coeff = condition->left_coeff[0];
         dest->u.condition[0].deadband = CLAMP(condition->deadband[0]);
         dest->u.condition[0].center = condition->center[0];
         /* Y axis */
         dest->u.condition[1].right_saturation = CLAMP(condition->right_sat[1]);
         dest->u.condition[1].left_saturation = CLAMP(condition->left_sat[1]);
         dest->u.condition[1].right_coeff = condition->right_coeff[1];
         dest->u.condition[1].left_coeff = condition->left_coeff[1];  
         dest->u.condition[1].deadband = CLAMP(condition->deadband[1]);
         dest->u.condition[1].center = condition->center[1];

         break;

      case SDL_HAPTIC_RAMP:
         ramp = &src->ramp;

         /* Header */
         dest->type = FF_RAMP;
         dest->direction = SDL_SYS_ToDirection(&ramp->direction);

         /* Replay */
         dest->replay.length = CLAMP(ramp->length);
         dest->replay.delay = CLAMP(ramp->delay);

         /* Trigger */
         dest->trigger.button = CLAMP(ramp->button);
         dest->trigger.interval = CLAMP(ramp->interval);

         /* Ramp */
         dest->u.ramp.start_level = ramp->start;
         dest->u.ramp.end_level = ramp->end;

         /* Envelope */
         dest->u.ramp.envelope.attack_length = CLAMP(ramp->attack_length);
         dest->u.ramp.envelope.attack_level = CLAMP(ramp->attack_level);
         dest->u.ramp.envelope.fade_length = CLAMP(ramp->fade_length);
         dest->u.ramp.envelope.fade_level = CLAMP(ramp->fade_level);

         break;


      default:
         SDL_SetError("Unknown haptic effect type");
         return -1;
   }

   return 0;
}


/*
 * Creates a new haptic effect.
 */
int
SDL_SYS_HapticNewEffect(SDL_Haptic * haptic, struct haptic_effect * effect,
      SDL_HapticEffect * base)
{
   struct ff_effect * linux_effect;

   /* Allocate the hardware effect */
   effect->hweffect = (struct haptic_hweffect *) 
         SDL_malloc(sizeof(struct haptic_hweffect));
   if (effect->hweffect == NULL) {
      SDL_OutOfMemory();
      return -1;
   }

   /* Prepare the ff_effect */
   linux_effect = &effect->hweffect->effect;
   if (SDL_SYS_ToFFEffect( linux_effect, base ) != 0) {
      goto new_effect_err;
   }
   linux_effect->id = -1; /* Have the kernel give it an id */

   /* Upload the effect */
   if (ioctl(haptic->hwdata->fd, EVIOCSFF, linux_effect) < 0) {
      SDL_SetError("Error uploading effect to the haptic device: %s", strerror(errno));
      goto new_effect_err;
   }

   return 0;

new_effect_err:
   free(effect->hweffect);
   effect->hweffect = NULL;
   return -1;
}


/*
 * Updates an effect.
 *
 * Note: Dynamically updating the direction can in some cases force
 * the effect to restart and run once.
 */
int SDL_SYS_HapticUpdateEffect(SDL_Haptic * haptic,
      struct haptic_effect * effect, SDL_HapticEffect * data)
{
   struct ff_effect linux_effect;

   /* Create the new effect */
   if (SDL_SYS_ToFFEffect( &linux_effect, data ) != 0) {
      return -1;
   }
   linux_effect.id = effect->hweffect->effect.id;

   /* See if it can be uploaded. */
   if (ioctl(haptic->hwdata->fd, EVIOCSFF, &linux_effect) < 0) {
      SDL_SetError("Error updating the haptic effect: %s", strerror(errno));
      return -1;
   }

   /* Copy the new effect into memory. */
   SDL_memcpy( &effect->hweffect->effect, &linux_effect, sizeof(struct ff_effect) );

   return effect->hweffect->effect.id;
}


/*
 * Runs an effect.
 */
int
SDL_SYS_HapticRunEffect(SDL_Haptic * haptic, struct haptic_effect * effect)
{
   struct input_event run;

   /* Prepare to run the effect */
   run.type = EV_FF;
   run.code = effect->hweffect->effect.id;
   run.value = 1;

   if (write(haptic->hwdata->fd, (const void*) &run, sizeof(run)) < 0) {
      SDL_SetError("Unable to run the haptic effect: %s", strerror(errno));
      return -1;
   }

   return 0;
}


/*
 * Stops an effect.
 */
int
SDL_SYS_HapticStopEffect(SDL_Haptic * haptic, struct haptic_effect * effect)
{
   struct input_event stop;

   stop.type = EV_FF;
   stop.code = effect->hweffect->effect.id;
   stop.value = 0;

   if (write(haptic->hwdata->fd, (const void*) &stop, sizeof(stop)) < 0) {
      SDL_SetError("Unable to stop the haptic effect: %s", strerror(errno));
      return -1;
   }

   return 0;
}


/*
 * Frees the effect.
 */
void
SDL_SYS_HapticDestroyEffect(SDL_Haptic * haptic, struct haptic_effect * effect)
{
   if (ioctl(haptic->hwdata->fd, EVIOCRMFF, effect->hweffect->effect.id) < 0) {
      SDL_SetError("Error removing the effect from the haptic device: %s",
            strerror(errno));
   }
   SDL_free(effect->hweffect);
   effect->hweffect = NULL;
}


/*
 * Gets the status of a haptic effect.
 */
int
SDL_SYS_HapticGetEffectStatus(SDL_Haptic * haptic, struct haptic_effect * effect)
{
#if 0  /* Not supported atm. */
   struct input_event ie;

   ie.type = EV_FF;
   ie.type = EV_FF_STATUS;
   ie.code = effect->hweffect->effect.id;

   if (write(haptic->hwdata->fd, &ie, sizeof(ie)) < 0) {
      SDL_SetError("Error getting haptic device status.");
      return -1;
   }

   return 0;
#endif

   return -1;
}


/*
 * Sets the gain.
 */
int
SDL_SYS_HapticSetGain(SDL_Haptic * haptic, int gain)
{
   struct input_event ie;

   ie.type = EV_FF;
   ie.code = FF_GAIN;
   ie.value = (0xFFFFUL * gain) / 100;

   if (write(haptic->hwdata->fd, &ie, sizeof(ie)) < 0) {
      SDL_SetError("Error setting gain: %s", strerror(errno));
      return -1;
   }

   return 0;
}


/*
 * Sets the autocentering.
 */
int
SDL_SYS_HapticSetAutocenter(SDL_Haptic * haptic, int autocenter)
{
   struct input_event ie;

   ie.type = EV_FF;
   ie.code = FF_AUTOCENTER;
   ie.value = (0xFFFFUL * autocenter) / 100;

   if (write(haptic->hwdata->fd, &ie, sizeof(ie)) < 0) {
      SDL_SetError("Error setting autocenter: %s", strerror(errno));
      return -1;
   }

   return 0;
}


#endif /* SDL_HAPTIC_LINUX */