diag_l2_iso14230.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 ISO14230-2 layer 2
 *
 */

#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_iso14230.h"

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

static char *cvsid = "$Id: diag_l2_iso14230.c,v 1.12 2002/07/19 19:42:04 rpalmeida Exp $";

extern int diag_l2_debug;
#define DIAG_MODULE "diag_l2_14230"


/*
 * ISO 14230 specific data
 */
struct diag_l2_14230
{
	u_int8_t type;		/* FAST/SLOW/CARB */
	
	u_int8_t srcaddr;	/* Src address used */
	u_int8_t dstaddr;	/* Dest address used (for connect) */
	u_int16_t modeflags;	/* Flags */

	u_int8_t state;

	u_int8_t first_frame;	/* First frame flag, used mainly for
					monitor mode when we need to find
					out whether we see a CARB or normal
					init */

	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 */


/*
 * Useful internal routines
 */

/*
 * Decode the message header, returning the length
 * of the message if a whole message has been received.
 * Note that this may be called with more than one message
 * but it only worries about the first message
 */
int
diag_l2_proto_14230_decode(u_int8_t *data, int len,
		 int *hdrlen, int *datalen, int *source, int *dest,
		int first_frame)
{
	int dl;

	if (diag_l2_debug & DIAG_DEBUG_PROTO)
	{
		int i;
		printf("%s: decode len %d", DIAG_MODULE, len);
		for (i = 0; i < len ; i++)
		{
			printf(" 0x%x", data[i]&0xff);
		}
		printf("\n");
	}
	
	dl = data[0] & 0x3f;
	if (dl == 0)
	{
		/* Additional length field present */
		switch (data[0] & 0xC0)
		{
		case 0x80:
		case 0xC0:
			/* Addresses supplied, additional len byte */
			if (len < 4)
			{
				if (diag_l2_debug & DIAG_DEBUG_PROTO)
				{
					printf("%s: decode len short \n",
						DIAG_MODULE);
				}
				return(DIAG_ERR_INCDATA);
			}
			*hdrlen = 4;
			*datalen = data[3];
			if (dest)
				*dest = data[1];
			if (source)
				*source = data[2];
			break;
		case 0x00:
			/* Addresses not supplied, additional len byte */
			if (first_frame)
				return(DIAG_ERR_BADDATA);
			if (len < 2)
				return(DIAG_ERR_INCDATA);
			*hdrlen = 2;
			*datalen = data[1];
			if (dest)
				*dest = 0;
			if (source)
				*source = 0;
			break;
		case 0X40:
			/* CARB MODE */
			return(DIAG_ERR_BADDATA);
		}
	}
	else
	{
		/* Additional length field not present */
		switch (data[0] & 0xC0)
		{
		case 0x80:
		case 0xC0:
			/* Addresses supplied, NO additional len byte */
			if (len < 3)
				return(DIAG_ERR_INCDATA);
			*hdrlen = 3;
			*datalen = dl;
			if (dest)
				*dest = data[1];
			if (source)
				*source = data[2];
			break;
		case 0x00:
			/* Addresses not supplied, No additional len byte */
			if (first_frame)
				return(DIAG_ERR_BADDATA);
			*hdrlen = 1;
			*datalen = dl;
			if (dest)
				*dest = 0;
			if (source)
				*source = 0;
			break;
		case 0X40:
			/* CARB MODE */
			return(DIAG_ERR_BADDATA);
		}
	}
	/*
	 * If len is silly [i.e 0] we've got this mid stream
	 */
	if (*datalen == 0)
		return(DIAG_ERR_BADDATA);

	/*
	 * And confirm data is long enough, incl cksum
	 * If not, return saying data is incomplete so far
	 */
	if (len < (*hdrlen + *datalen + 1))
		return(DIAG_ERR_INCDATA);

	if (diag_l2_debug & DIAG_DEBUG_PROTO)
	{
		printf("%s: decode hdrlen = %d, datalen = %d, cksum = 1\n",
			DIAG_MODULE, *hdrlen, *datalen);
	}
	return(*hdrlen + *datalen + 1);
}

/*
 * Internal receive function (does all the message building, but doesn't
 * do call back, returns the complete message, hasn't removed checksum
 * and header info
 *
 * Data from the first message is put into *data, and len into *datalen
 *
 * If the L1 interface is clever (DOESL2FRAME), then each read will give
 * us a complete message, and we will wait a little bit longer than the normal
 * timeout to detect "end of all responses"
 */
int
diag_l2_proto_14230_int_recv(diag_l2_conn_t *d_l2_conn, int timeout,
	u_int8_t *data, int *datalen)
{
	struct diag_l2_14230 *dp;
	int rv, l1_doesl2frame, l1flags;
	int tout;
	int state;
	diag_msg_t	*tmsg, *lastmsg;

#define ST_STATE1	1	/* Start */
#define ST_STATE2	2	/* Interbyte */
#define ST_STATE3	3	/* Inter message */

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

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

	state = ST_STATE1;

	/* Clear out last received message if not done already */
	if (d_l2_conn->diag_msg)
	{
		diag_freemsg(d_l2_conn->diag_msg);
		d_l2_conn->diag_msg = NULL;
	}

	l1flags = d_l2_conn->diag_link->diag_l2_l1flags;
	if (l1flags & (DIAG_L1_DOESL2FRAME|DIAG_L1_DOESP4WAIT))
	{
		if (timeout < 100)	/* Extend timeouts */
			timeout = 100;
	}
	if (l1flags & DIAG_L1_DOESL2FRAME)
		l1_doesl2frame = 1;
	else
		l1_doesl2frame = 0;

	while (1)
	{
		if (state == ST_STATE1)
			tout = timeout;
		else if (state == ST_STATE2)
		{
			tout = d_l2_conn->diag_l2_p2min - 2;
			if (tout < d_l2_conn->diag_l2_p1max)
				tout = d_l2_conn->diag_l2_p1max;
		}
		else if (state == ST_STATE3)
		{
			if (l1_doesl2frame)
				tout = 150;	/* Arbitrary, short, value ... */
			else
				tout = d_l2_conn->diag_l2_p2max;
		}

		/* Receive data into the buffer */
#if FULL_DEBUG
		printf("before recv, state %d timeout %d, rxoffset %d\n",
			state, tout, dp->rxoffset);
#endif

		/*
		 * In l1_doesl2frame mode, we get full frames, so we don't
		 * do the read in state2
		 */
		if ( (state == ST_STATE2) && l1_doesl2frame )
			rv = DIAG_ERR_TIMEOUT;
		else
			rv = diag_l1_recv (d_l2_conn->diag_link->diag_l2_fd, 0,
				&dp->rxbuf[dp->rxoffset],
				sizeof(dp->rxbuf) - dp->rxoffset,
				tout);
#if FULL_DEBUG
		printf("after recv, rv %d rxoffset %d\n", rv, dp->rxoffset);
#endif

		if (rv == DIAG_ERR_TIMEOUT)
		{
			/* Timeout, end of message, or end of responses */
			switch (state)
			{
			case ST_STATE1:
				/*
				 * 1st read, if we got 0 bytes, just return
				 * the timeout error
				 */
				if (dp->rxoffset == 0)
					break;
				/*
				 * Otherwise see if there are more bytes in
				 * this message,
				 */
				state = ST_STATE2;
				continue;
			case ST_STATE2:
				/*
				 * End of that message, maybe more to come
				 * Copy data into a message
				 */
				tmsg = diag_allocmsg(dp->rxoffset);
				tmsg->len = dp->rxoffset;
				memcpy(tmsg->data, dp->rxbuf, dp->rxoffset);
				(void)gettimeofday(&tmsg->rxtime, NULL);
				dp->rxoffset = 0;
				/*
				 * ADD message to list
				 */
				diag_l2_addmsg(d_l2_conn, tmsg);
				if (d_l2_conn->diag_msg == tmsg)
				{
#if FULL_DEBUG
					int i;
					printf("Copying %d bytes to data\n",
						tmsg->len);
					for (i=0; i<tmsg->len; i++)
						printf("0x%x ",tmsg->data[i]); 
					printf("\n");
#endif
					/* 1st one */
					if (data)
					{
						memcpy(data, tmsg->data,
							tmsg->len);
						*datalen = tmsg->len;
					}
				}
				state = ST_STATE3;
				continue;
			case ST_STATE3:
				/*
				 * No more messages, but we did get one
				 */
				rv = d_l2_conn->diag_msg->len;
				break;
			}
			if (state == ST_STATE3)
				break;
		}
		if (rv < 0)
		{
			/* Error */
			break;
		}

		/* Data received OK */
		dp->rxoffset += rv;

		if (dp->rxoffset && (dp->rxbuf[0] == '\0'))
		{
			/*
			 * We get this when in
			 * monitor mode and there is
			 * a fastinit, pretend it didn't exist
			 */
			dp->rxoffset--;
			if (dp->rxoffset)
				memcpy(&dp->rxbuf[0], &dp->rxbuf[1],
					dp->rxoffset);
			continue;
		}
		if ( (state == ST_STATE1) || (state == ST_STATE3) )
		{
			/*
			 * Got some data in state1/3, now we're in a message
			 */
			state = ST_STATE2;
		}
	}

	/*
	 * Now check the messages that we have checksum etc, stripping
	 * off headers etc
	 */
	if (rv >= 0)
	{
		tmsg = d_l2_conn->diag_msg;
		lastmsg = NULL;

		while (tmsg)
		{
			struct diag_l2_14230 *dp;
			int hdrlen, datalen, source, dest;

			/*
			 * We have the message with the header etc, we
			 * need to strip the header and checksum
			 */
			dp = (struct diag_l2_14230 *)d_l2_conn->diag_l2_proto_data;
			rv = diag_l2_proto_14230_decode( tmsg->data,
				tmsg->len,
				&hdrlen, &datalen, &source, &dest,
				dp->first_frame);

			if (rv < 0)		/* decode failure */
			{
				return(rv);
			}

			/*
			 * If L1 isnt doing L2 framing then it is possible
			 * we have misframed this message and it is infact
			 * more than one message, so see if we can decode it
			 */
			if ((l1_doesl2frame == 0) && (rv < tmsg->len))
			{
				/*
				 * This message contains more than one	
				 * data frame (because it arrived with
				 * odd timing), this means we have to
				 * do horrible copy about the data
				 * things ....
				 */
				diag_msg_t	*amsg;
				amsg = diag_dupsinglemsg(tmsg);
				amsg->len = rv;
				tmsg->len -=rv;
				tmsg->data += rv;

				/*  Insert new amsg before old msg */
				amsg->next = tmsg;
				if (lastmsg == NULL)
					d_l2_conn->diag_msg = amsg;
				else
					lastmsg->next = amsg;

				tmsg = amsg; /* Finish processing this one */
			}

#if FULL_DEBUG
			printf("msg %x decode/rejig done rv %d hdrlen %d datalen %d source %02x dest %02x\n",
				tmsg, rv, hdrlen, datalen, source, dest);
#endif

			if (tmsg->data[0] & 0xC0 == 0xC0)
			{
				tmsg->fmt = DIAG_FMT_ISO_FUNCADDR;
			} else {
				tmsg->fmt = 0;
			}
			tmsg->fmt |= DIAG_FMT_FRAMED | DIAG_FMT_DATAONLY ;
			tmsg->fmt |= DIAG_FMT_CKSUMMED;

			if ((l1flags & DIAG_L1_STRIPSL2CKSUM) == 0)
			{
				/* XXX check checksum */
			}

			tmsg->src = source;
			tmsg->dest = dest;
			tmsg->data += hdrlen;	/* Skip past header */
			tmsg->len -= hdrlen; /* remove header */

			/* remove checksum byte if needed */
			if ((l1flags & DIAG_L1_STRIPSL2CKSUM) == 0)
				tmsg->len--;

			dp->first_frame = 0;

			lastmsg = tmsg;
			tmsg = tmsg->next;
		}
	}
	return(rv);
}


/* External interface */

/*
 * The complex initialisation routine for ISO14230, which supports
 * 2 types of initialisation (5-BAUD, FAST) and functional
 * and physical addressing. The ISO14230 spec describes CARB initialisation
 * which is done in the ISO9141 code
 *
 * Remember, we have to wait longer on smart L1 interfaces.
 */
diag_l2_proto_14230_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_14230 *dp;
	diag_msg_t	msg;
	u_int8_t data[1024];
	int rv, wait_time;
	int hdrlen, datalen, datasrc;
	u_int8_t cbuf[1024];
	int len;
	int timeout;

	struct diag_l1_init in;

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

	d_l2_conn->diag_l2_proto_data = (void *)dp;
	dp->type = flags & DIAG_L2_TYPE_INITMASK;

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