/* FDC Laser scanner system driver

July 2, 2008  David Lawrence

This file comprises a library of functions used to drive the VS4 VME
motor controller from VxWorks. There are generic routines and ones
specific to the FDC laser system.


 */


#include <stdio.h>
#include <string.h>
#include <signal.h>
#include <time.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <unistd.h>

#include <sigLib.h>
#include <vme.h>

#include "vs4.h"

/* Address of the VS4 board */
#define VS4_DEFAULT_ADDR		0xFF80

/* The following is used to insert a memory barrier to ensure the */
/* PPC chip doesn't re-order the read and write commands that are */
/* accessing different memory locations. */
#define EIEIO __asm__ volatile ("eieio")


volatile struct VS4  *vs4=NULL;



/*------------------
/* VS4Init
/*------------------*/
int VS4Init(unsigned short addr)
{
	int err;
	
	if(addr==0){
		addr=VS4_DEFAULT_ADDR;
		printf("Address of 0 passed to VS4Init. Using default address 0x%4x instead.\n", VS4_DEFAULT_ADDR);
	}
	
	err = sysBusToLocalAdrs(VME_AM_USR_SHORT_IO, addr, (long **)&vs4);
	if(err!=0){
		fprintf(stderr,"Error initializing VS4 at 0x%x (err=%d)\n", addr, err);
		return -1;
	}
	
	printf("Set Local address to 0x%x\n", (unsigned long)vs4);
	
	printf("Testing writing/reading from interrupt vector register\n");
	vs4->interrupt = 0x0007;
	printf("Interrupt register: 0x%x\n", vs4->interrupt);
	
	printf("Reading VS4 version string and serial number:\n");
	VS4Command("WY");
	
	printf("Setting initial velocities to 1000 steps/sec and acceleration to 2000 steps/sec^2\n");
	VS4Command("AX VL600 AC2000 AY VL1500 AC2000 AZ VL1000 AC2000 AT VL1000 AC2000");

	printf("Turning off soft limits\n");
	VS4Command("AX SF AY SF AZ SF AT SF");

	return 0;
}

/*------------------
/* VS4Registers
/*------------------*/
int VS4Registers(void)
{
	if(!vs4)VS4Init(0);

	printf("\n");
	printf("VS4 Registers:\n");
	printf("==========================\n");
	printf("     data : 0x%2x\n", vs4->data & 0xFF);
	printf("     done : 0x%2x\n", vs4->done & 0xFF);
	printf("  control : 0x%2x\n", vs4->control & 0xFF);
	printf("   status : 0x%2x\n", vs4->status & 0xFF);
	printf("interrupt : 0x%2x\n", vs4->interrupt & 0xFF);
	printf("\n");
	
	return 0;
}

/*------------------
/* VS4reg
/*------------------*/
int VS4reg(struct VS4  *my_vs4)
{
	if(!vs4)VS4Init(0);
	
	*my_vs4 = *vs4;
	
	return 0;
}

/*------------------
/* VS4Command
/*------------------*/
int VS4Command(const char *mess)
{
	/* This is just a higher level wrapper for VS4cmd that automatically */
	/* prints any return message to the screen. */
	int err;
	char ret_mess[256];
	
	printf("Writing command to VS4 : \"%s\" (%d bytes)\n", mess, strlen(mess));
	err=VS4cmd(mess, ret_mess, 256, 1);
	
	printf("return message: \"%s\"\n", ret_mess);
	
	return err;
}


/*------------------
/* VS4cmd
/*------------------*/
int VS4cmd(const char *mess, char *ret_mess, int ret_mess_size, int chomp_ret_mess)
{
	/*
	Submit an ASCII command string to the VS4. Any return message is
	read automatically and copied into the ret_mess buffer. The parameters are:
	
		mess					Command string for the VS4.
		
		ret_mess				Buffer to hold return string (if any).
								If no return message, this will contain
								an empty string. A value of NULL can
								be passed in here if no return message is
								required. Note that the message will still be
								read out to ensure the buffer is empty before
								completeing this call.
								
		ret_mess_size		Size of ret_mess buffer.
		
		chomp_ret_mess		Flag indicating whether any leading and/or 
								trailing <LF> and <CR> characters should
								be stripped from the return message. A
								value of 0 means they should not be stripped
								and any other value indicates that they should.
	*/

	int n;
	int i;
	int warned = 0;
	
	/* Write command string to VS4 */
	for(i=0; i<strlen(mess); i++){
		/* Make sure VS4 is ready to take this byte */
		int j;
		for(j=0; j<1000; j++)if(vs4->status & TBE_S)break;
		if(j>=1000){
			fprintf(stderr, "ERROR: The transmit buffer is full! Cannot write command \"%s\"!\n", mess);
			return -1;
		}
		EIEIO;
		vs4->data = (unsigned short)mess[i];
		EIEIO;
	}
	
	/* Read back any response */
	/* Do a large loop in case we read back faster than the VS4 can transmit */
	n=0;
	for(i=0; i<10000; i++){
		EIEIO;
		if(vs4->status & IBF_S){
			char c;
			EIEIO;
			c = (char)(vs4->data&0xFF);
			if(chomp_ret_mess && (c=='\r' || c=='\n')){
				/* ignore this <LF> or <CR> character */
			}else{
				if(n<ret_mess_size-1 && ret_mess!=NULL)ret_mess[n++] = c;
			}
		}
		if(n>=ret_mess_size-1 && ret_mess!=NULL){
			if(!warned)fprintf(stderr, "WARNING: return message buffer overflow. Return message truncated.\n");
			warned = 1;
		}
	}
	ret_mess[n] = 0;
	
	return 0;
}