diag_l3_saej1979.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.
 *
 *************************************************************************
 *
 * L3 code to do SAE J1979 messaging
 *
 */
#include <stdlib.h>
#include <stdio.h>

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

#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_l3.h"
#include "diag_err.h"
#include "diag_general.h"

static char *cvsid = "$Id: diag_l3_saej1979.c,v 1.7 2002/08/23 18:54:35 rpalmeida Exp $";

#define DIAG_MODULE "diag_l3_j1979"

/*
 * Insert the L3 layer on top of the layer 2 connection
 * 
 */
int
diag_l3_j1979_start(diag_l3_conn_t *d_l3_conn)
{
	/* Nothing to do */
	return(0);
}

int
diag_l3_j1979_stop(diag_l3_conn_t *d_l3_conn)
{
	/* Nothing to do */
	return(0);
}


/*
 * Return the expected J1979 packet length for a given mode byte
 * This includes the 3 header bytes, up to 8 data bytes, 1 ERR byte
 *
 * XXX DOESN'T COPE WITH in-frame-response - will break check routine as well
 *
 * Get this wrong and all will fail, it's used to frame the incoming messages
 * properly
 */
int diag_l3_j1979_getlen(u_int8_t *data, int len)
{
	int rqst_lengths[] = { -1, 6, 7, 5, 5, 6, 6, 5, 11, 6 };
	int rv;
	u_int8_t mode;

	if (len < 5)	/* Need 3 header bytes and 1 data byte, and 1 cksum*/
		return( DIAG_ERR_INCDATA);

	mode  = data[3];

	if (mode > 0x49)
		return(DIAG_ERR_BADDATA);

	if (mode <= 9)
		return(rqst_lengths[mode]);

	/* Mode 8 messages vary in length dependent on request */
	if ((mode == 8) && ((data[4]&0x1f) == 0))
		len = 6;

	if (mode < 0x41)
		return(DIAG_ERR_BADDATA);

	switch (mode)
	{
	case 0x41:
		if (len < 5)
		{
			/* Need 1st 2 bytes of data to find length */
			rv = DIAG_ERR_INCDATA;
			break;
		}
		if ((data[4] & 0x1f) == 0)
		{	
			/* supported PID message */
			rv = 10;
			break;
		}
		if (data[4] > 0x20)
		{
			/* Sometime add J2190 support */
			rv = DIAG_ERR_BADDATA;
			break;
		}
		switch (data[4])
		{
		case 1:
			rv = 10;
			break;
		case 2:
			rv = DIAG_ERR_BADDATA;
			break;
		case 3:
			rv = 8;
			break;

		case 0x04:
		case 0x05:
		case 0x06:
		case 0x07:
		case 0x08:
		case 0x09:
		case 0x0A:
		case 0x0B:
		case 0x0C:
		case 0x0D:
		case 0x0E:
		case 0x0F:
		case 0x10:
		case 0x11:
		case 0x12:
			rv = 7;
			break;
		case 0x14:
		case 0x15:
		case 0x16:
		case 0x17:
		case 0x18:
		case 0x19:
		case 0x1A:
		case 0x1B:
			rv = 8;
			break;

		case 0x13:
		case 0x1C:
		case 0x1D:
		case 0x1E:
			rv = 7;
			break;
		case 0x1F:
			rv = 5;
			break;
		}
		break;
	case 0x42:
		if (len < 5)
		{
			rv = DIAG_ERR_INCDATA;
			break;
		}
		if ((data[4] & 0x1f) == 0)
		{
			rv = 11;
			break;
		}
		if (data[4] > 0x20)
		{
			/* Sometime add J2190 support (0x21->0x3F) */
			rv = DIAG_ERR_BADDATA;
			break;
		}
		switch (data[4])
		{
		case 2:
		case 3:
			rv = 8;
			break;

		case 0x04:
		case 0x05:
		case 0x06:
		case 0x07:
		case 0x08:
		case 0x09:
		case 0x0A:
		case 0x0B:
		case 0x0C:
		case 0x0D:
			rv = 5;
			break;

		default:
			rv = DIAG_ERR_BADDATA;
			break;
		}
		break;
	case 0x45:
		if (len < 5)
		{
			rv = DIAG_ERR_INCDATA;
			break;
		}
		if ((data[4] & 0x1f) == 0)
			rv = 11;
		else
			rv = 10;
		break;
	case 0x44:
		rv = 5;
		break;
	case 0x43:
	case 0x46:
	case 0x47:
	case 0x48:
		rv = 11;
		break;
	case 0x49:
		if (len < 5)
		{
			rv = DIAG_ERR_INCDATA;
			break;
		}
		switch (data[4])
		{
		case 1:
		case 3:
		case 5:
			rv = 7;
			break;
		case 0:
		case 2:
		case 4:
		case 6:
			rv = 11;
			break;
		default:
			rv = DIAG_ERR_BADDATA;
			break;
		}
		break;
	default:
		rv = DIAG_ERR_BADDATA;
		break;
	}
	return(rv);
}


/*
 * Send a J1979 packet - we know the length (from looking at the data)
 */
int diag_l3_j1979_send(diag_l3_conn_t *d_l3_conn, diag_msg_t *msg)
{
	int rv;
	diag_l2_conn_t *d_conn;
	u_int8_t buf[32];
	diag_msg_t newmsg;
	u_int8_t cksum;
	int i;

	/* Get l2 connection info */
	d_conn = d_l3_conn->d_l3l2_conn;

	if (diag_l3_debug & DIAG_DEBUG_WRITE)
		printf("%s: send %d bytes, l2 flags 0x%x\n",
			DIAG_MODULE, msg->len,  d_l3_conn->d_l3l2_flags);

	/* Note source address on 1st send */
	if (d_l3_conn->src == 0)
		d_l3_conn->src = msg->src;


	if (d_l3_conn->d_l3l2_flags & DIAG_L2_FLAG_DATA_ONLY)
	{
		/* L2 does framing, adds addressing and CRC, so do nothing */
		rv = diag_l2_send(d_conn, msg);
	}
	else
	{
		/* Put data in buffer */
		memcpy(&buf[3], msg->data, msg->len);

		/*
		 * Add addresses. Were using default addresses here, suitable
		 * for ISO9141 and one of the J1850 protocols. However our
		 * L2 J1850 code does framing for us, so thats no issue.
		 */
		if (msg->data[0] >= 0x40)
		{
			/* Response */
			buf[0] = 0x48;
			buf[1] = 0x6B;	/* We chose to overide msg->dest */
		}
		else
		{
			/* Request */
			buf[0] = 0x68;
			buf[1] = 0x6A;	/* We chose to overide msg->dest */
		}
		buf[2] = msg->src;

		/*
		 * We do an ISO type checksum as default. This wont be
		 * right for J1850, but that is handled by our L2 J1850 code
		 * so thats no issue.
		 */
		if ( ((d_l3_conn->d_l3l2_flags & DIAG_L2_FLAG_DOESCKSUM)==0)
			&& ((d_l3_conn->d_l3l1_flags & DIAG_L1_DOESL2CKSUM)==0))
		{
			/* No one else does checksum, so we do it */
			for (i=0, cksum = 0; i<msg->len+3; i++)
				cksum += buf[i];
			buf[msg->len+3] = cksum;

			newmsg.len = msg->len + 4; /* Old len + hdr + cksum */
		}
		else
			newmsg.len = msg->len + 3;	/* Old len + hdr */

		newmsg.data = buf;

		/* And send message */
		rv = diag_l2_send(d_conn, &newmsg);
	}
	return(rv);
}

/*
 * RX callback, called as data received from L2. If we get a full message,
 * call L3 callback routine
 */
void
diag_l3_rcv_callback(void *handle, diag_msg_t *msg)
{
	/*
	 * Got some data from L2, build it into a L3 message, if
	 * message is complete call next layer callback routine
	 */
	diag_l3_conn_t *d_l3_conn = (diag_l3_conn_t *)handle;

	if (diag_l3_debug & DIAG_DEBUG_READ)
		printf("%s: rcv_callback for %d bytes fmt 0x%x conn rxoffset %d\n",
			DIAG_MODULE, msg->len, msg->fmt, d_l3_conn->rxoffset);

	if (msg->fmt & DIAG_FMT_FRAMED)
	{
		if ( (msg->fmt & DIAG_FMT_DATAONLY) == 0)
		{
			/* Remove header etc */
			diag_msg_t *tmsg;
			/*
			 * Have to remove L3 header and checksum from each response
			 * on the message
			 *
			 * XXX checksum check needed ...
			 */
			for (tmsg = msg ; tmsg; tmsg = tmsg->next)
			{
				tmsg->fmt |= DIAG_FMT_ISO_FUNCADDR;
				tmsg->fmt |= DIAG_FMT_DATAONLY;
				tmsg->type =  tmsg->data[0];
				tmsg->dest = tmsg->data[1];
				tmsg->src =  tmsg->data[2];
				/* Length sanity check */
				if (tmsg->len >= 4)
				{
					tmsg->data += 3;
					tmsg->len -= 4;	/* Remove header and checksum */
				}
			}
		}
		else
		{
			/* XXX check checksum */

		}
		/* And send data upward if needed */
		if (d_l3_conn->callback)
			d_l3_conn->callback(d_l3_conn->handle, msg);
	}
	else
	{
		/* Add data to the receive buffer on the L3 connection */
		memcpy(&d_l3_conn->rxbuf[d_l3_conn->rxoffset],
			msg->data, msg->len);
		d_l3_conn->rxoffset += msg->len;
	}
}


/*
 * Process_data() - this is the routine that works out the framing
 * of the data , if recv() was always called at the correct time and
 * in the correct way then we would have one and one complete message
 * received by the L2 code normally - however, we cant be sure of that
 * so we employ this algorithm
 *
 * Look at the data and try and work out the length of the message based
 * on the J1979 protocol.
 * Check the checksum, if this is OK send message upward
 * If not, work thru the data working out whether the data looks correct
 * by looking for a checksum, ensuring there is only complete messages in
 * the buffer
 *
 * XXX this doesn't deal with CRCs
 *
 *
 */
void
diag_l3_j1979_process_data(diag_l3_conn_t *d_l3_conn)
{
	/* Process the received data into messages if complete */
	diag_msg_t *msg;
	int sae_msglen;
	int i;

	while (d_l3_conn->rxoffset)
	{