diag_l2_saej1850.c

Freediag contains various drivers (ISO9141, ISO14230, SAEJ1850-VPW, SAEJ1850-PWM) for different adap

/*
 *
 *	freediag - Vehicle Diagnostic Utility
 *
 * Copyright (C) 2001 Richard Almeida & Ibex Ltd ([e-mail])
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 *
 * This program 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 General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
 *
 *************************************************************************
 *
 * Diag
 *
 * L2 driver for SAEJ1850
 *
 *
 * INCOMPLETE, will not work, but doesnt coredump. This has been checked in
 * because scantool.c was checked in for other reasons, and needs this so it
 * doesnt coredump ...
 */

static char *cvsid = "$Id: diag_l2_saej1850.c,v 1.7 2002/05/20 20:57:39 rpalmeida Exp $";


#include "diag_os.h" /* operating specific includes */

#include <stdlib.h>
#include <stdio.h>

#include <fcntl.h>
#include <string.h>
#include <errno.h>

#include <time.h>

#include "diag.h"
#include "diag_l1.h"
#include "diag_l2.h"
#include "diag_err.h"
#include "diag_general.h"

#include "diag_l2_saej1850.h" /* prototypes for this file */

extern int diag_l2_debug;
#define DIAG_MODULE "diag_l2_j1850"

/*
 * SAEJ1850 specific data
 */
struct diag_l2_j1850
{
	u_int8_t type;		/* FAST/SLOW/CARB */
	
	u_int8_t srcaddr;	/* Src address used */
	u_int8_t dstaddr;	/* Dest address used */
	u_int16_t modeflags;	/* Flags */

	u_int8_t state;
	u_int8_t rxbuf[1024];	/* Receive buffer, for building message in */
	int rxoffset;		/* Offset to write into buffer */
};

#define STATE_CLOSED	  0	/* Established comms */
#define STATE_CONNECTING  1	/* Connecting */
#define STATE_ESTABLISHED 2	/* Established */

/* Prototypes */
u_int8_t diag_l2_proto_j1850_crc(u_int8_t *msg_buf, int nbytes);

/* External interface */

/*
 * The complex initialisation routine for SAEJ1850
 */
int
diag_l2_proto_j1850_startcomms( diag_l2_conn_t	*d_l2_conn, u_int16_t flags,
	int bitrate, u_int8_t target, u_int8_t source)
{
	struct diag_l2_j1850 *dp;
	u_int8_t cbuf[1024];

	if (diag_l2_debug & DIAG_DEBUG_OPEN)
		fprintf(stderr,
			"%s: diag_l2_j1850_startcomms conn %x\n",
				DIAG_MODULE, d_l2_conn);

	dp = calloc(1, sizeof(struct diag_l2_j1850));
	if (dp == NULL)
		return(DIAG_ERR_NOMEM);

	d_l2_conn->diag_l2_proto_data = (void *)dp;

	dp->srcaddr = source;
	dp->dstaddr = target;
	dp->modeflags = flags;

	dp->state = STATE_CONNECTING;

	/* Empty our Receive buffer and wait for idle bus */
	/* XXX is the timeout value right ? */
	(void)diag_os_read( d_l2_conn->diag_link->diag_l2_fd, cbuf,
		sizeof(cbuf), 50);

	/* Always OK */
	return(0);
}

/*
*/
int
diag_l2_proto_j1850_stopcomms(diag_l2_conn_t* d_l2_conn)
{
	struct diag_l2_14230 *dp;

	dp = (struct diag_l2_14230 *)d_l2_conn->diag_l2_proto_data;

	if (dp)
		free(dp);


	/* Always OK for now */
	return (0);
}


/* Thanks to B. Roadman's web site for this CRC code */
u_int8_t
diag_l2_proto_j1850_crc(u_int8_t *msg_buf, int nbytes)
{
	u_int8_t crc_reg=0xff,poly,i,j;
	u_int8_t *byte_point;
	u_int8_t bit_point;

	for (i=0, byte_point=msg_buf; i<nbytes; ++i, ++byte_point)
	{
		for (j=0, bit_point=0x80 ; j<8; ++j, bit_point>>=1)
		{
			if (bit_point & *byte_point)	// case for new bit = 1
			{
				if (crc_reg & 0x80)
					poly=1;	// define the polynomial
				else
					poly=0x1c;
				crc_reg= ( (crc_reg << 1) | 1) ^ poly;
			}
			else		// case for new bit = 0
			{
				poly=0;
				if (crc_reg & 0x80)
					poly=0x1d;
				crc_reg= (crc_reg << 1) ^ poly;
			}
		}
	}
	return ~crc_reg;	// Return CRC
}

/*
 * Just send the data
 *
 * We add the header and checksums here as appropriate
 */
int
diag_l2_proto_j1850_send(diag_l2_conn_t *d_l2_conn, diag_msg_t *msg)
{
	int l1flags, rv, l1protocol;
	struct diag_l2_j1850 *dp;

	u_int8_t buf[1024];
	int offset = 0;

	if (diag_l2_debug & DIAG_DEBUG_WRITE)
		fprintf(stderr,
			"%s: diag_l2_j1850_send 0x%x msg 0x%x len %d called\n",
				DIAG_MODULE, d_l2_conn, msg, msg->len);

	dp = (struct diag_l2_j1850 *)d_l2_conn->diag_l2_proto_data;
	l1flags = d_l2_conn->diag_link->diag_l2_l1flags;

	// Add the J1850 header to the data
	// XXX 0x68 is no correct for all J1850 protocols

	l1protocol = d_l2_conn->diag_link->diag_l2_l1protocol;

	if (l1protocol == DIAG_L1_J1850_PWM)
		buf[0] = 0x61;
	else
		buf[0] = 0x68;
	buf[1] = dp->dstaddr;
	buf[2] = dp->srcaddr;
	offset += 3;
	
	// Now copy in data, should check for buffer overrun really
	memcpy(&buf[offset], msg->data, msg->len);
	offset += msg->len;

	if ((l1flags & DIAG_L1_DOESL2CKSUM) == 0)
	{
		// Add in J1850 CRC
		buf[offset++] = diag_l2_proto_j1850_crc(buf, offset);
	}

	if (diag_l2_debug & DIAG_DEBUG_WRITE)
		fprintf(stderr,
			"%s: diag_l2_j1850_send sending %d bytes to L1\n",
				DIAG_MODULE, offset);

	// And send data to Layer 1
	rv = diag_l1_send (d_l2_conn->diag_link->diag_l2_fd, 0,
				buf, offset, 0);

	return(rv);
}

/*
 * Protocol receive routine
 *
 * Will sleep until a complete set of responses has been received, or fail
 * with a timeout error
 */
int
diag_l2_proto_j1850_int_recv(diag_l2_conn_t *d_l2_conn, int timeout)
{
	int rv;
	struct diag_l2_j1850 *dp;
	int tout;
	diag_msg_t	*tmsg;
	int l1flags = d_l2_conn->diag_link->diag_l2_l1flags;

	dp = (struct diag_l2_j1850 *)d_l2_conn->diag_l2_proto_data;

	if (diag_l2_debug & DIAG_DEBUG_READ)
		fprintf(stderr,
			"%s: diag_l2_j1850_int_recv offset %x\n",
				DIAG_MODULE, dp->rxoffset);

	if (l1flags & DIAG_L1_DOESL2FRAME)
	{
		tout = timeout;
		if (tout < 100)	/* Extend timeouts for clever interfaces */
			tout = 100;

		rv = diag_l1_recv (d_l2_conn->diag_link->diag_l2_fd, 0,
				&dp->rxbuf[dp->rxoffset],
				sizeof(dp->rxbuf) - dp->rxoffset,
				tout);
		if (rv < 0)
		{
			// Error
			return(rv);
		}
		dp->rxoffset += rv;
	}
	else
	{
		// No support for non framing L2 interfaces yet ...
		return(DIAG_ERR_PROTO_NOTSUPP);
	}

	// Ok, got a complete frame to send upward

	if (dp->rxoffset)
	{
		// There is data left to add to the message list ..
		tmsg = diag_allocmsg(dp->rxoffset);
		tmsg->len = dp->rxoffset;
		memcpy(tmsg->data, dp->rxbuf, dp->rxoffset);

		/*
		 * Minimum message length is 3 header bytes
		 * 1 data, 1 checksum
		 */
		if (tmsg->len >= 5)
		{
			if ((l1flags & DIAG_L1_STRIPSL2CKSUM) == 0)
			{
				/* XXX check checksum */
			}
			tmsg->dest = tmsg->data[1];
			tmsg->src = tmsg->data[2];
			tmsg->data +=3;
			tmsg->len -=3;

			/* remove checksum byte if needed */
			if ((l1flags & DIAG_L1_STRIPSL2CKSUM) == 0)
				tmsg->len--;
		}
		else
		{
			diag_freemsg(tmsg);
			return(DIAG_ERR_BADDATA);
		}

		(void)gettimeofday(&tmsg->rxtime, NULL);
		dp->rxoffset = 0;

		/*
		 * ADD message to list
		 */
		diag_l2_addmsg(d_l2_conn, tmsg);
	}

	dp->state = STATE_ESTABLISHED;
	return(0);
}


int
diag_l2_proto_j1850_recv(diag_l2_conn_t *d_l2_conn, int timeout,
	void (*callback)(void *handle, diag_msg_t *msg),
	void *handle)
{
	int rv;
	struct diag_l2_j1850 *dp;
	int tout;
	diag_msg_t	*tmsg;

	rv = diag_l2_proto_j1850_int_recv(d_l2_conn, timeout);
	if (rv < 0)	/* Failed */
		return(rv);

	/*
	 * We now have data stored on the L2 descriptor
	 */
	if (diag_l2_debug & DIAG_DEBUG_READ)
	{
		printf("%s: calling rcv callback %x handle %x msg %x\n",
			DIAG_MODULE, d_l2_conn->diag_msg,
			callback, handle);
	}

	tmsg = d_l2_conn->diag_msg;
	d_l2_conn->diag_msg = NULL;

	tmsg->fmt |= DIAG_FMT_FRAMED | DIAG_FMT_DATAONLY ;
	tmsg->fmt |= DIAG_FMT_CKSUMMED;


	/* Call used callback */
	if (callback)
		callback(handle, tmsg);

	/* message no longer needed */
	diag_freemsg(tmsg);

	if (diag_l2_debug & DIAG_DEBUG_READ)
	{
		printf("%s: rcv callback completed\n", DIAG_MODULE);
	}
	return(0);
}

/*
 * Send a request and wait for a response
 */
diag_msg_t *
diag_l2_proto_j1850_request(diag_l2_conn_t *d_l2_conn, diag_msg_t *msg,
		int *errval)
{
	int rv;
	diag_msg_t *rmsg = NULL;

	/* First send the message */
	rv = diag_l2_send(d_l2_conn, msg);
	if (rv < 0)
	{
		*errval = rv;
		return(NULL);
	}

	/* And now wait for a response */
/* XXX, whats the correct timeout for this ??? */
	rv = diag_l2_proto_j1850_int_recv(d_l2_conn, 250);
	if (rv < 0)
	{
		*errval = rv;
		return(NULL);
	}

	/* Return the message to user, who is responsible for freeing it */
	rmsg = d_l2_conn->diag_msg;
	d_l2_conn->diag_msg = NULL;
	return(rmsg);
}