/* drvCompuSm.c */
/* base/src/drv $Id$ */
/*
 * subroutines and tasks that are used to interface to the Compumotor 1830
 * stepper motor drivers
 *
 * 	Author:	Bob Dalesio
 * 	Date:	01-03-90
 *
 *	Experimental Physics and Industrial Control System (EPICS)
 *
 *	Copyright 1991, the Regents of the University of California,
 *	and the University of Chicago Board of Governors.
 *
 *	This software was produced under  U.S. Government contracts:
 *	(W-7405-ENG-36) at the Los Alamos National Laboratory,
 *	and (W-31-109-ENG-38) at Argonne National Laboratory.
 *
 *	Initial development by:
 *		The Controls and Automation Group (AT-8)
 *		Ground Test Accelerator
 *		Accelerator Technology Division
 *		Los Alamos National Laboratory
 *
 *	Co-developed with
 *		The Controls and Computing Group
 *		Accelerator Systems Division
 *		Advanced Photon Source
 *		Argonne National Laboratory
 *
 * Modification Log:
 * -----------------
 * .01	02-07-90	lrd	add command to read the motor status
 * .02	04-11-90	lrd	made velocity mode motor go active
 * .03	04-12-90	lrd	only allow one connection to a motor
 * .04	04-13-90	lrd	add externally initiated motor motion monitoring
 * .05	09-05-91	joh	updated for v5 vxWorks
 * .06	10-09-91	lrd	monitor for external motion once every 2 seconds
 *				not at 30 Hz (.04 was not implemented correctly)
 * .06	11-31-91        bg	added compu_sm_io_report. Added sysBusToLocalAdrs()
 *                              for addressing.
 * .07	03-02-92        bg	added level and ability to print raw values to 
 *                              compu_sm_io_report for level > 0.
 * .08	05-04-92        bg	added compu_sm_reset and rebootHookAdd so ioc can be
 *                              rebooted with control X. 
 * .09	06-25-92        bg	Combined drvCompuSm.c and compu_sm_driver.c
 * .10	06-26-92        bg	Added level to compu_sm_io_report in drvCompuSm 
 *                              structure   
 * .11	06-29-92	joh	took file ptr arg out of io report
 * .12	08-06-92	joh	merged compu sm include file
 * .13	08-27-92	joh	silenced ANSI C function proto warning	
 * .14	08-27-92	joh	fixed no epics init
 * .15	08-02-93	mrk	Added call to taskwdInsert
 * .16	10-29-93	jba	Fixed max number of cards to use module_types.c
 *                      Fixed error in calculating card addresses
 */
#include <vxWorks.h>
#include <vme.h>
#include <sysLib.h>
#include <semLib.h>		/* library for semaphore support */
#include <wdLib.h>
#include <rngLib.h>		/* library for ring buffer support */

/* drvCompuSm.c -  Driver Support Routines for CompuSm */

#include	<dbDefs.h>
#include	<drvSup.h>
#include	<module_types.h>
#include	 <steppermotor.h>
#include	 <task_params.h>
#include	 <taskwd.h>


long compu_sm_io_report();
long compu_driver_init();

struct {
	long	number;
	DRVSUPFUN	report;
	DRVSUPFUN	init;
} drvCompuSm={
	2,
	compu_sm_io_report,
	compu_driver_init};

/* compumotor vme interface information */
#define MAX_COMPU_MOTORS		8

#define RESP_SZ		16		/* card returns 16 chars - cmd & resp */
#define RESPBUF_SZ	(RESP_SZ+1)	/* intr routine also passes motor no. */

/*	Control Byte bit definitions for the Compumotor 1830	*/
					/* bits 0 and 1 are not used */
#define	CBEND	0x04			/* end of command string */
#define CBLMR	0x08			/* last message byte read */
#define CBCR	0x10			/* command ready in cm_idb */
#define CBMA	0x20			/* message accepted from odb */
#define CBEI	0x40			/* enable interrupts */
#define CBBR	0x80			/* board reset */
#define	SND_MORE	CBCR | CBEI		/* more chars to send */
#define SND_LAST	CBEND | SND_MORE	/* last char being sent */
#define	RD_MORE		CBMA | CBEI		/* more chars to read */
#define RD_LAST		CBLMR | RD_MORE		/* last byte we need to read */

/*	Status Byte bit definitions	*/
#define SBIRDY	0x10			/* idb is ready */

/*	Structure used for communication with a Compumotor 1830
**	Motor Controller. The data buffer is repeated 64 times
**	on even word addresses (0xXXXXXX1,0xXXXXX5...) and the
**	control location is repeated 64 times on odd word 
**	addresses (0xXXXXX3, 0xXXXXX7...). The registers must
**	be read and written as bytes.
*/
struct compumotor {
	char	cm_d1;		/* not accessable */
	char	cm_idb;		/* input data buffer */
	char	cm_d2;		/* not accessable */
	char	cm_cb;		/* control byte */
	char	cm_d3[0x100-4];	/* fill to next standard address */
};


/* This file includes defines for all compumotor 1830 controller 
** commands. */

#define SM_NULL		0x0	/* Null command */
#define	SM_INT		0x1	/* Interrupt X */
#define SM_WRT_STAT	0x8	/* Write X to the user defined status bits */
#define	SM_SET_STAT	0x9	/* Set user defined status bit number X */ 
#define	SM_CLR_STAT	0xa	/* Clear user defined status bit number X */
#define	SM_SET_PROG	0xb	/* Set programmable output bit X */
#define	SM_CLR_PROG	0xc	/* Clear programmable output bit X */
#define	SM_WRT_PROG	0xd	/* Write X to the programmable output bits */
#define	SM_DEF_X_TO_Y	0xf	/* Define bit X to indicate state Y */
#define	SM_TOG_JOG	0x10	/* Disable/enable the JOG inputs */
#define	SM_DEF_JOG	0x11	/* Define JOG input functions */
#define SM_TOG_REM_PWR	0x12	/* Turn off/on remote power shutdown */
#define	SM_TOG_REM_SHUT	0x13	/* Disable/enable the "remote shutdown" bit */
#define	SM_SET_CW_MOTN	0x14	/* Set CW motion equal to +- */
#define	SM_TOG_POS_MTN	0x18	/* Turn off/on post-move position maintenance */
#define SM_TOG_STOP_STL	0x19	/* Turn off/on termination on stall detect */
#define SM_REP_X_Y	0x20	/* Repeat the following X commands Y times */
#define	SM_REP_TIL_CONT	0x21	/* Repeat the following X commands
				   until a CONTINUE is received */
#define SM_WAIT_CONT	0x28	/* Wait for a CONTINUE */
#define SM_WAIT_MILLI	0x29	/* Wait X milliseconds */
#define SM_WAIT_SECOND	0x2a	/* Wait X seconds */
#define SM_WAIT_MINUTE	0x2b	/* Wait X minutes */
#define SM_WAIT_TRIGGER	0x2c	/* Wait for trigger X to go active */
#define SM_DEF_A_OP_POS	0x2e	/* Define the abs open-loop position as X */
#define SM_DEF_A_CL_POS	0x2f	/* Define absolute closed-loop position */
#define	SM_DEF_ABS_ZERO	0x30	/* Define the present position as the 
				   absolute zero position */
#define	SM_DEF_VEL_ACC	0x31	/* Define default velocity and acceleration */
#define	SM_MOV_DEFAULT	0x32	/* Perform the default move (trapezoidal
				   continuous) */
#define SM_MOV_REL_POS	0x33	/* Go to relative position X at default
				   velocity and acceleration */
#define SM_MOV_ABS_POS	0x34	/* Go to absolute position X at default
				   velocity and acceleration */
#define	SM_MOV_REL_ENC	0x35	/* Go to relative encoder position X */
#define SM_MOV_ABS_ENC	0x36	/* Go to absolute encoder position X */
#define SM_DEF_OP_HOME	0x38	/* Define HOME location (open loop */
#define SM_DEF_CL_HOME	0x38	/* Define HOME location (closed loop) */
#define	SM_MOV_HOME_POS	0x39	/* Go HOME at the default velocity and
				   acceleration */
#define SM_MOV_HOME_ENC	0x3a	/* Go to encoder HOME at the default
				   velocity and acceleration */
#define	SM_DO_MOV_X	0x40	/* Perform move number X */
#define	SM_DO_SEQ_X	0x41	/* Perform sequence buffer X */
#define	SM_DO_VEL_STR	0x42	/* Perform the velocity streaming buffer */
#define	SM_CONT		0x48	/* CONTINUE (perform next command) */
#define	SM_OP_LOOP_MODE	0x50	/* Enter open loop indexer mode */
#define	SM_VEL_DIS_MODE	0x51	/* Enter velocity-distance streaming mode */
#define SM_VEL_TIM_MODE	0x52	/* Enter velocity-time streaming mode */
#define	SM_STOP		0x70	/* STOP motion */
#define SM_DSC_SEQ	0x71	/* Discontinue the sequence buffer */
#define	SM_SSP_SEQ	0x72	/* Suspend the sequence buffer;
				   wait for a CONTINUE to resume */
#define	SM_DSC_SNGL	0x73	/* Discontinue any singular command 
				   currently being performed */
#define	SM_STOP_ON_TRG	0x74	/* STOP motion when trigger X goes active */
#define	SM_DSC_SEQ_TRG	0x75	/* Discontinue the sequence buffer when
				   trigger X goes active */
#define SM_SSP_SEQ_TRG	0x76	/* Suspend sequence buffer when trigger
				   X goes active */
#define	SM_DSC_SNGL_TRG	0x77	/* Discontinue any singular command when
				   trigger X goes active */
#define	SM_KILL		0x78	/* Kill motion */
#define	SM_KILL_SEQ	0x79	/* Kill the sequence buffer */
#define	SM_KILL_SEQ_SNGL 0x7a	/* Kill current sequence singular command;
				   wait for CONTINUE */	
#define	SM_KILL_VEL_STR	0x7b	/* Kill the velocity streaming buffer */
#define	SM_KILL_ON_TRG	0x7c	/* Kill motion when trigger X goes active */
#define	SM_KILL_SEQ_TRG	0x7d	/* Kill the sequence buffer when trigger
				   X goes active */
#define	SM_KL_SQSNG_TRG	0x7e	/* Kill current sequence singular command
				   when trigger X goes active; wait for
				   a continue */
#define	SM_KL_VLSTR_TRG	0x7f	/* Kill the velocity streaming buffer
				   when trigger X goes active */
#define	SM_GET_B_REL_POS 0x80	/* Request position relative to the
				   beginning of the current move */
#define	SM_GET_E_REL_POS 0x81	/* Request position relative to the
				   end of the current move */
#define	SM_GET_H_REL_POS 0x82	/* Request position relative to the home
				   limit switch */
#define	SM_GET_Z_REL_POS 0x83	/* Request position relative to the 
				   absolute zero position */
#define	SM_GET_CUR_DIR	0x84	/* Request current direction */
#define	SM_GET_VEL	0x85	/* Request current velocity */
#define	SM_GET_ACC	0x86	/* Request current acceleration */
#define	SM_GET_MOV_STAT 0x88	/* Request current move status */
#define SM_GET_LIM_STAT	0x89	/* Request state of the limit switches */
#define	SM_GET_HOME_STAT 0x8a	/* Request state of the HOME switch */
#define SM_GET_TRV_DIR	0x8b	/* Request direction of travel */
#define	SM_GET_MOT_MOV	0x8c	/* Request whether motor is moving or not */
#define	SM_GET_MOT_CONST 0x8d	/* Request whether motor is at constant,
			  	   nonzero velocity or not */
#define	SM_GET_MOT_ACC	0x8e	/* Request whether motor is or is not
				   accelerating */
#define	SM_GET_MOT_DEC	0x8f	/* Request whether motor is or is not
				   decelerating */
#define	SM_GET_MODE	0x90	/* Request present mode */
#define	SM_GET_MV_PARM	0x91	/* Request move parameters for move number X */
#define	SM_GET_SEQ_CMMD	0x92	/* request commands stored in the
				   sequence buffer */
#define	SM_GET_MVDEF_STAT 0x93	/* Request state of the move definitions */
#define	SM_GET_TRG_STAT	0x94	/* Request state of trigger inputs */
#define	SM_GET_JOG_STAT	0x95	/* Request state of JOG inputs */
#define	SM_GET_Z_STAT	0x96	/* Request state of the Channel Z home input */
#define	SM_GET_OUT_STAT	0x97	/* Request the state of the programmable
				   output bits */
#define	SM_GET_REL_ENC	0x98	/* Request relative encoder count */
#define	SM_GET_REL_ERR	0x99	/* Request relative error from desired
				   closed loop position */
#define	SM_GET_ABS_ENC	0x9a	/* Request absolute encoder count */
#define	SM_GET_SLIP_STAT 0x9b	/* Request slip detect status */
#define	SM_GET_RATIO	0x9c	/* Request motor pulse to encoder pulse ratio */
#define	SM_GET_RESOLTN	0x9d	/* Request motor resolution */
#define	SM_GET_BACK_SIG	0x9e	/* Request backlash sigma (motor steps)*/
#define	SM_GET_ALG	0x9f	/* Request position maintenance alg.
				   const, and max velocity */
#define	SM_INT_NXT_MOV	0xa0	/* Interrupt at start of next move */
#define	SM_INT_ALL_MOV	0xa1	/* Interrupt at the start of every move */
#define	SM_INT_NXT_NZ	0xa2	/* Interrupt at constant nonzero velocity
				   of next move */
#define SM_INT_ALL_NZ	0xa3	/* Interrupt at constant nonzero velocity
				   of every move */
#define	SM_INT_NXT_END	0xa4	/* Interrupt at next end of motion */
#define	SM_INT_ALL_END	0xa5	/* Interrupt at every end of motion */
#define	SM_INT_NXT_STL	0xa6	/* Interrupt on next stall detect */
#define	SM_INT_ALL_STL	0xa7	/* Interrupt on every stall detect */
#define	SM_INT_NXT_PLIM	0xa8	/* Interrupt the next time the motor 
				   hits the positive limit */
#define	SM_INT_ALL_PLIM	0xa9	/* Interrupt on every positive limit */
#define	SM_INT_NXT_NLIM	0xaa	/* Interrupt the next time the motor
				   hits the negative limit */
#define	SM_INT_ALL_NLIM	0xab	/* Interrupt on every negative limit */
#define	SM_INT_TRG	0xac	/* Interrupt on trigger X active */
#define	SM_INT_INHBT	0xaf	/* Inhibit all interrupts */
#define	SM_DEF_RATIO	0xb0	/* Define motor pulse to encoder pulse ratio */
#define SM_DEF_RESOLTN	0xb1	/* Define motor resolution */
#define	SM_DEF_BACK_SIG	0xb2	/* Define backlash sigma (motor steps)*/
#define	SM_DEF_ALG	0xb3	/* Define position maintenance algorithm
				   const, and max velocity */
#define	SM_DEF_TEETH	0xb4	/* Define  the number of rotor teeth */
#define	SM_DEF_DEADBAND	0xb5	/* Def the deadband region in encoder pulses */
#define	SM_DEF_REL_TRP	0xc8	/* Define move X as a relative, 
				   trapezoidal move */
#define	SM_DEF_ABS_TRP	0xcb	/* Define move X as an absolute
				   trapezoidal move */
#define	SM_DEF_CONT	0xce	/* Define move X as a continuous move */
#define SM_DEF_REL_CL	0xd4	/* Define move X, define it as relative,
				   closed-loop move */
#define	SM_DEF_ABS_CL	0xd5	/* Def move X as an abs, closed- loop move */
#define	SM_DEF_STSTP_VEL 0xd6	/* Define the start/stop velocity */
#define	SM_DEL_MOV_X	0xd7	/* Delete move X */
#define	SM_END_SEQ_DEF	0xd8	/* End definition of sequence buffer */
#define	SM_BEG_SEQ_DEF	0xd9	/* Begin definition of sequence buffer */
#define	SM_DEL_SEQ_X	0xda	/* Delete sequence buffer X */
#define	SM_LD_VD_DATA	0xe0	/* Place data into the velocity-distance buffer */
#define	SM_LD_VT_DATA	0xe1	/* Place data into the velocity-time buffer */
#define	SM_GET_FREE_BYT	0xe2	/* Request number of free bytes in vel-
				   streaming/sequence buffer */
#define	SM_DEF_VS_CMMD	0xee	/* Define command to be executed during
				   the velocity streaming buffer */
#define	SM_GET_NUM_REV	0xfd	/* Request software part number and revision */
#define	SM_TEST_SWITCH	0xff	/* Perform the test switch function */


#define VELOCITY_MODE	0
#define	MAX_COMMANDS	256
#define COMPU_INT_LEVEL   5

/* array of pointers to stepper motor driver cards present in system */
struct compumotor	*pcompu_motors[MAX_COMPU_MOTORS];

LOCAL SEM_ID compu_wakeup;	/* compumotor data request task semaphore */

/* response variables */
LOCAL SEM_ID smRespSem;		/* task semaphore */
LOCAL RING_ID smRespQ;			/* ring buffer */
int smRespId;				/* task id */
#define RESP_Q_SZ (RESPBUF_SZ * 50)	/* response ring buffer size */

/* interrupt buffers */
unsigned char	sm_responses[MAX_COMPU_MOTORS][RESPBUF_SZ];
unsigned short	counts[MAX_COMPU_MOTORS];
/* counts for debugging */
long dbicounts; /* counts number of times through interrupt routine */
long dbrcounts; /* counts number of times through response routine */
long dbscounts; /* counts number of times through send routine */

/* VME memory Short Address Space is set up in gta_init */
static int *compu_addr;

/* motor information */
struct compu_motor{
short	active;		/* flag to tell the oms_task if the motor is moving */
int	callback;	/* routine in database library to call with status  */
int	callback_arg;	/* argument to callback routine  */
short	update_count;
short	mode;
short	motor_resolution;
};
struct compu_motor	compu_motor_array[MAX_COMPU_MOTORS];

/* Forward reference. */
VOID compu_sm_reset();
VOID compu_sm_stat();

/* motor status - returned to the database library routines */
struct motor_data	compu_motor_data_array[MAX_COMPU_MOTORS];

/* moving status bit descriptions */
#define	CW_LIMIT	0x01	/* clockwise???? limit       */
#define	CCW_LIMIT	0x02	/* counter-clockwise???? limit       */
#define DIRECTION	0x08	/* direction bit        */
#define MOVING		0x10	/* moving status bit    */
#define	CONSTANT_VEL	0x20	/* constant velocity    */

/* directions in driver card-ese */
#define	CLKW	0	/* clockwise direction  */
#define	CCLKW	1	/* counter clockwise direction */

/*
 * Code Portions:
 *
 * smCmdTask	Task which writes commands to the hardware
 * smRespTask	Task which places reponses from the hardware into resp buffers
 * sm_intr	Interrupt Handler - collects response data from the hardware
 * sm_drv_init	Initializes all motors, semaphores, ring buffers and interrupts
 * sm_driver	Subroutine for outside world to issue commands to motors
 * motor_select	Subroutine to setting callback arg and verifying no other user
 * motor_deselect Subroutine to free the motor for other users
 *
 * Interaction Chart:
 *            --------------                          -------------------
 *          /               \                        /                   \ 
 *         |    smRespTask   |                      |      smCmdTask      |
 *          \               /                        \                   /
 *           ---------------                          ------------------- 
 *              ^        ^                                      |
 *        TAKES |        | GETS                                 |
 *              |        |                                      |
 *   --------------  ---------------                            |
 *   Resp Semaphore   Response Queue                            |
 *   --------------  ---------------                            |
 *              ^        ^                                      |
 *        GIVES |        | PUTS                                 |
 *              |        |                                      |
 *           ---------------                                    |
 *          /                \                                  |
 *         |   sm_intr        |                                 |
 *          \                /                                  |
 *           ---------------                                    |
 *                   ^ reads responses          writes commands |
 *                   |  from hardware               to hardware V 
 */

/*
 * COMPU_RESP_TASK
 *
 * converts readback from the compumotor 1830 cards into a structure that
 * is returned to the database library layer every .1 second while a motor
 * is moving
 */
compu_resp_task()
{
    unsigned char		resp[RESPBUF_SZ];
    register short		i;
    register struct motor_data	*pmotor_data;

    FOREVER {
        /* wait for somebody to wake us up */
       	semTake (smRespSem, WAIT_FOREVER);
		(dbrcounts < 0xFFFF ? dbrcounts++: 0);
 	/* the response buffer contains: 	 	 	*/
	/* 0 -	motor number				 	*/
	/* 1 -	the command which solicited this response	*/
	/* 2 -	the first byte of the response		 	*/

	/* process requests in the command ring buffer */
        while (rngBufGet(smRespQ,(char *)resp,RESPBUF_SZ) == RESPBUF_SZ){
		pmotor_data = &compu_motor_data_array[resp[0]];

		/* convert argument */
		switch(resp[1]){

		case (SM_GET_VEL):
		{
			register long	*pvelocity = (long *)(&resp[3]);
			pmotor_data->velocity = *pvelocity; 

			break;
		}
		case (SM_GET_MOV_STAT):
		{
			register struct compu_motor	*pcompu_motor;
			register int 			(*psmcb_routine)();

			pcompu_motor = &compu_motor_array[resp[0]];
			pmotor_data->moving = (resp[2] & MOVING)?1:0;
			pmotor_data->constant_velocity = (resp[2] & CONSTANT_VEL)?1:0;
			pmotor_data->cw_limit = (resp[2] & CW_LIMIT)?1:0;
			pmotor_data->ccw_limit = (resp[2] & CCW_LIMIT)?1:0;
			pmotor_data->direction = (resp[2] & DIRECTION)?1:0;

			/* post every .1 second or not moving */
			if ((pcompu_motor->update_count-- <= 0) 
			  || (pmotor_data->moving == 0)){
				if (pcompu_motor->callback != 0){
					(int)psmcb_routine = pcompu_motor->callback;
					(*psmcb_routine)(pmotor_data,pcompu_motor->callback_arg);
				}
				if (pmotor_data->moving){
					/* motors are reported at 10 Hz */
					pcompu_motor->update_count = 3;
				}else{
                                        pcompu_motor->active = FALSE;
					pcompu_motor->update_count = 0;
				}
			}
			break;
		}
		case (SM_GET_ABS_ENC):
		{
			register long		*pencoder = (long *)(&resp[2]);
			pmotor_data->encoder_position = *pencoder;
			break;
		}
		case (SM_GET_Z_REL_POS):
		{
			register long	*pmotor = (long *)(&resp[4]);
			pmotor_data->motor_position = *pmotor;
			break;
		}
		case (SM_GET_CUR_DIR):
			pmotor_data->direction = (resp[2] == 0xff)?CLKW:CCLKW;
			break;
		}
        }
    }
}

/* Data request commands for the positional and velocity mode motors */
char compu_velo_reqs[] = { SM_GET_VEL,     SM_GET_MOV_STAT };
#define NUM_VEL_REQS	2
char compu_pos_reqs[] =  { SM_GET_ABS_ENC, SM_GET_Z_REL_POS, SM_GET_MOV_STAT };
#define NUM_POS_REQS	3
/*
 * COMPU_TASK
 *
 * task to solicit currnet status from the compumotor 1830 cards while they
 * are active
 */
compu_task()
{
    register short		inactive_count;
    register short		card;
    register short		i;
    register struct compumotor	*pmotor;
    register char		*preqs;

    /* inactive motors get monitored once every 2 seconds in case they are */
    /* being moved manually						   */
    inactive_count = 60;
    while(1){
	/* This task is run 30 times a second */
	taskDelay(2);
	for (card = 0; card < sm_num_cards[CM57_83E]; card++){
	    pmotor = pcompu_motors[card];
	    if (pmotor == 0) continue;
	    if ((compu_motor_array[card].active)
	      || (inactive_count <=0)){
		if (compu_motor_array[card].mode == VELOCITY_MODE){
			preqs = &compu_velo_reqs[0];
			/* request status data */
			for (i = 0; i < NUM_VEL_REQS; i++,preqs++)
				compu_send_msg(pmotor,preqs,1);
		}else{
			preqs = &compu_pos_reqs[0];
			/* request status data */
			for (i = 0; i < NUM_POS_REQS; i++,preqs++)
				compu_send_msg(pmotor,preqs,1);
		}
	    }
	}
	if (--inactive_count < 0) inactive_count = 60;
    }
}

/*
 * COMPU_INTR
 *
 * interrupt vector for the compumotor 1830 card
 */
compu_intr(mdnum)
register int mdnum;
{
    register struct compumotor	*pmtr;	/* memory port to motor card */
    register int	key;

    key = intLock();

    /* pointer to the compumotor card interface */
    pmtr = pcompu_motors[mdnum];
	if (pmtr == 0)
	{
		intUnlock(key);
		return(0);
	};
	(dbicounts < 0xFFFF ? dbicounts++: 0);
    /* place the response byte into the appropriate response buffer */
    sm_responses[mdnum][counts[mdnum]] = pmtr->cm_idb;
    counts[mdnum]++;

    /* when the buffer is full pass it onto the repsonse task */
    if (counts[mdnum] == RESPBUF_SZ){
        if (rngBufPut(smRespQ,(char *)sm_responses[mdnum],RESPBUF_SZ) != RESPBUF_SZ)
            logMsg("smRespQ %d - Full\n",mdnum);
	else
	    semGive (smRespSem);

	/* the zero-th byte is the motor number */
        counts[mdnum] = 1;        /* start with command */

        /* inform the hardware that the response is complete */
        pmtr->cm_cb = RD_LAST;
    }else{
	/* inform the hardware there is more to send */
        pmtr->cm_cb = RD_MORE;
    }

    intUnlock(key);
	return(0);
}

/*
 * COMPU_DRIVER_INIT
 *
 * initialization for the compumotor 1830 card
 */
long
compu_driver_init(){
    register short i;
    int status;
    struct compumotor    *pmtr;    /* memory port to motor card */
    int    cok = CBBR;			/*to reset board */
    short	none_found;		/* flags a steppermotor is present */
    int		taskId;
    struct compumotor *pmtrb;

   /* intialize each driver which is present */
    none_found = TRUE;
    rebootHookAdd((FUNCPTR)compu_sm_reset);
    status = sysBusToLocalAdrs(
		VME_AM_SUP_SHORT_IO,
		sm_addrs[CM57_83E], 
		(int **)&compu_addr);
    if (status != OK){ 
      printf("%s: failed to map A16 base\n", __FILE__); 
      return ERROR;
    }
   
    pmtrb = (struct compumotor *)compu_addr;  
    for (i = 0; i < sm_num_cards[CM57_83E]; i++) {
       pmtr =  (struct compumotor *)((int)pmtrb + (i<<8)); 

	/* initialize when card is present */

        if (vxMemProbe(&pmtr->cm_cb,WRITE,1,&cok) != ERROR){
	    none_found = FALSE;
            pcompu_motors[i] = pmtr;		/* ptr to interface */
            intConnect((MD_INT_BASE+i)*4,compu_intr,i);	/* interrupt enable */
            sysIntEnable(COMPU_INT_LEVEL); 

            /* init interrupt receive buffers */
            sm_responses[i][0] = i;	/* motor number */
            counts[i] = 1;		/* buffer index */
        }else{
            pcompu_motors[i] = 0;	/* flags no board is present */
        }
    }
    if (none_found) return(0);

    /* initialize the response task ring buffer */
    if ((smRespQ = rngCreate(RESP_Q_SZ)) == (RING_ID)NULL)
        panic ("sm_init: cmRespQ\n");

    /* intialize the semaphores which awakens the sleeping           *
     * stepper motor command task and the stepper motor response task */
    if(!(smRespSem=semBCreate(SEM_Q_FIFO,SEM_EMPTY)))
	errMessage(0,"semBcreate failed in compu_driver_init");
    if(!(compu_wakeup=semBCreate(SEM_Q_FIFO,SEM_EMPTY)))
	errMessage(0,"semBcreate failed in compu_driver_init");

    /* spawn the sleeping motor driver command and response tasks */
    smRespId = 
	taskSpawn("compu_resp_task",SMRESP_PRI,SMRESP_OPT,SMRESP_STACK,compu_resp_task);
    taskwdInsert(smRespId,NULL,NULL);
    taskId = taskSpawn("compu_task",SMRESP_PRI,SMRESP_OPT,2000,compu_task);
    taskwdInsert(taskId,NULL,NULL);
    return(0);
}

short trigger = 0;
/*
 * COMPU_DRIVER
 *
 * driver interface to the database library layer
 */
compu_driver(card, channel, value_flag,arg1,arg2)
register short	card;
short	channel;
short		value_flag;
register int	arg1;
register int	arg2;
{
        register int	*pint;
        register short	*pshort;
	short		i;
	char		compu_msg[20];
	i = 0;
	/* verify the stepper motor driver card is present */
	if ((card < 0) || (card > sm_num_cards[CM57_83E]) || (!pcompu_motors[card]))
		return (-1);

	switch (value_flag){
	case (SM_MODE):
		/* set the motor mode */
		compu_motor_array[card].mode = arg1;
		break;

	case (SM_VELOCITY):
		compu_motor_data_array[card].velocity = arg1;
		compu_motor_data_array[card].accel = arg2;

		/* set the velocity */
		compu_msg[0] = SM_DEF_VEL_ACC;
		compu_msg[1] = 0;		/* time is in seconds */
		compu_msg[2] = 0;
		pint = (int *)&compu_msg[3];	/* velocity */
		*pint = arg1;
		pint++;				/* acceleration */
		*pint = arg2;
		compu_send_msg(pcompu_motors[card],compu_msg,11);

		break;

	case (SM_MOVE):
		if (compu_motor_array[card].mode == VELOCITY_MODE)
			return(0);

		/* move the motor */
		i = 0;
		compu_msg[i++] = SM_MOV_REL_POS;
		pint = (int *)&compu_msg[i];
		*pint = arg1;
		i += 4;

		compu_send_msg(pcompu_motors[card],compu_msg,i);

		/* set the motor to active */
		compu_motor_array[card].active = TRUE;

		/* wakeup the compu task */
		semGive(compu_wakeup);

		break;

	case (SM_MOTION):
		if (arg1 == 0){
			compu_msg[0] = SM_STOP;
			compu_send_msg(pcompu_motors[card],compu_msg,1);
		}else if (compu_motor_array[card].mode == VELOCITY_MODE){
			compu_msg[0] = SM_MOV_DEFAULT;
			compu_msg[1] = arg2;		/* direction */
			compu_send_msg(pcompu_motors[card],compu_msg,2);
			compu_motor_array[card].active = TRUE;
		}

		/* wakeup the compu task */
		semGive(compu_wakeup);
		break;

	case (SM_CALLBACK):
		/* put the callback routine and argument into the data array */
		i = 0;
		if (compu_motor_array[card].callback != 0) return(-1);
		compu_motor_array[card].callback = arg1;
		compu_motor_array[card].callback_arg = arg2;
		break;

	case (SM_SET_HOME):
		if (compu_motor_array[card].mode == VELOCITY_MODE)
			return(OK);

		/* set the motor and encoder position to zero */
		compu_msg[0] = SM_DEF_ABS_ZERO;
		compu_send_msg(pcompu_motors[card],compu_msg,1);

		break;
		
	case (SM_ENCODER_RATIO):
		compu_motor_array[card].motor_resolution = arg1;

		/* set the encoder ratio */
		compu_msg[0] = SM_DEF_RATIO;
		pshort = (short *)&compu_msg[1];
		*pshort = arg1;			/* motor resolution */
		pshort++;
		*pshort = arg2;			/* encoder resolution */
		compu_send_msg(pcompu_motors[card],compu_msg,5);

		/* set the motor resolution */
		compu_msg[0] = SM_DEF_RESOLTN;
		pshort = (short *)&compu_msg[1];
		*pshort = 0;
		pshort++;
		*pshort = arg1;			/* motor resolution */
		compu_send_msg(pcompu_motors[card],compu_msg,5);

		break;
	case (SM_READ):
		/* set the motor to active */
		compu_motor_array[card].active = TRUE;

		/* wakeup the compu task */
		semGive(compu_wakeup);

		break;

	}
	return (OK);
}

/*
 * COMPU_SEND_MSG
 *
 * send a message to the compumotor 1830
 */
int	wait_count;
compu_send_msg(pmotor,pmsg,count)
register struct	compumotor	*pmotor;
register char			*pmsg;
register short			count;
{
	(dbscounts < 0xFFFF ? dbscounts++: 0);
        /* write out this command one byte at a time */
	while (count){

                /* wait for the driver to be ready */
                while ((pmotor->cm_cb & SBIRDY) == 0){
			taskDelay(0);
                	wait_count++;
		}

                /* next byte in the input data buffer of compumotor */
                pmotor->cm_idb = *pmsg;
    		pmsg++;
		count--;

                /* tell compumotor more or complete */
                if (count == 0){
			pmotor->cm_cb = SND_LAST;
		}else{
			pmotor->cm_cb = SND_MORE;
		}
        }
}


/*
 * COMPU_SM_IO_REPORT
 *
 * send a message to the compumotor 1830
 */

long compu_sm_io_report(level)
  short int level;
 {
   register int i;

	for (i = 0; i < sm_num_cards[CM57_83E]; i++){
		if (pcompu_motors[i]){	
                       
                    printf("SM: CM1830:     card %d\n",i);
                    if (level > 0)
                        compu_sm_stat(i); 
                }
        }

	return OK;
 }

VOID compu_sm_stat(compu_num)
 short int compu_num;
 {
 struct motor_data  *pmotor_data;
  printf("\tCW limit = %d\t,CCW limit = %d\tMoving = %d\tDirection = %d\n",
         compu_motor_data_array[compu_num].cw_limit,
         compu_motor_data_array[compu_num].ccw_limit,
         compu_motor_data_array[compu_num].moving,
         compu_motor_data_array[compu_num].direction);

  printf("\tConstant Velocity = %d\t, Velocity = %d\t \n",
         compu_motor_data_array[compu_num].constant_velocity,
         compu_motor_data_array[compu_num].velocity);

  printf("\tAcceleration = %d\tEncoder Position = %d\tMotor Position = %d\n", 
         compu_motor_data_array[compu_num].accel,
         compu_motor_data_array[compu_num].encoder_position,
         compu_motor_data_array[compu_num].motor_position);
 }

/*
 *
 * Subroutine to be called during a CTL X reboot. Inhibits interrupts. 
 *
 */

VOID compu_sm_reset()
 {
   short int i;
   char	compu_msg[20];

	for (i = 0; i < sm_num_cards[CM57_83E]; i++){
		if (pcompu_motors[i]){	
                    compu_msg[0] = SM_INT_INHBT;  
                    compu_send_msg(pcompu_motors[i],compu_msg,1);
                }
        }
 }