#include <stdio.h>
#include <unistd.h>
#include <sys/mman.h>  /* mlockall() */
#include <sched.h>  /* sched_setscheduler() */
#include <sys/io.h>  /* ioperm(), inb() outb(), must compile with -O2 */
#include <signal.h> /* signal() */
#include <linux/fdreg.h>   /* FD_DOR, floppy register address */
#include <stdlib.h>  // exit()

/*

gcc motorb.c -Wall -O2 -o motorb
Tested on Linux 2.2.20, gcc 2.95.3

Bugs:
1.
Several minutes of continuous floppy disk activity will block motorb
2.
busy() and set()/unset() are race conditions, it is possible for these to clash with the kernel driver.... leading to 
3.
motorb should be a kernel module

*/

int drive;

int status()
 {
   unsigned char val=inb( FD_DOR );
   if( val & (0x10<<drive) )
      return 1;
   return 0;    
 }

int busy()  /* Check for busyness of device */
 {
   unsigned char val=inb( FD_DOR );
   //printf("0x%02x\n",val);
   if( val & 0x10 )
      return -1;
   return 0;
 }

int unset()  /* clear (activate) FD1 motor, ie. start of wdt trigger */
 {
   unsigned char val=inb( FD_DOR );
   outb( val & (0xFF^(0x10<<drive)), FD_DOR );

   return 0;
 }

int set() /* set (disable) FD1 motor, ie. triggler wdt to reset timer */
 {
   unsigned char val=inb( FD_DOR );
   outb( val | (0x10<<drive), FD_DOR );
  
   return 0;
 }


void shutdown(int status) /* handle all signals cleanly */
 {
   unset(drive);
   exit(0);
 }


int setrealtime() /* Give us reasonable chance of running uninterupted */
 {
   struct sched_param sp;
   sp.sched_priority=10;
   
   return sched_setscheduler(0, SCHED_RR, &sp);
 }

int main(int argc, char **argv)
 {
   long n=0;
   if( argc!=2 || !( (n=strtol(argv[1],NULL,10))==0 || n==1 || n==-1) )
   {
	   printf("motorb <0|1>\n");
	   printf("sets or clears the _MOTOR_B bit, note active low,\nso setting here means a low voltage.\n");
	   exit(1);
   }
   
   /* Get io permisions, lock process onto memory-never swap, and set realtime.
      Very important we run when needed, even if swap is exhausted. */
   if( ioperm( FD_DOR, 1, -1) || mlockall(MCL_FUTURE) || setrealtime() )
    {
      perror(argv[0]);
      exit(0);
    }

   /* Catch all signals to clear FD_DOR's Motor 1 bit when shutdown */
   signal( SIGINT, shutdown );
   signal( SIGHUP, shutdown );
   signal( SIGTERM, shutdown );

   drive=1;

   if( n==-1 )
	   printf("%d\n",status() );
   else if( n==0 )
	   unset();
   else if( n==1 )
	   set();

   exit(0);
   
   /* set then clear the floppy driver motor 1 bit */
   /* Note the usleep() negate the setrealtime(), but its the best I know of */
   printf("waiting\n");
   while( busy() )  /* Make sure register is clear */
      usleep(1000000);
   printf("setting\n");
   set();
   printf("waiting\n");
   while( busy() )  /* Make sure register is clear */
      usleep(1000000);
   sleep(5);
   printf("clearing\n");
   unset();

   exit(0);
 }

/* End */