diag_l2_mb1.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.
 *
 *************************************************************************
 *
 * L2 driver for Mercedes Benz protocol used on things like the
 * EGS (auto gearbox controller) on 1999/2000/2001 cars
 * I have called this Mercedes Benz protocol 1, since all other control
 * units I have played with use ISO14230
 *
 */

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

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

static char *cvsid = "$Id: diag_l2_mb1.c,v 1.3 2002/04/03 04:56:40 bjorn_helgaas Exp $";

extern int diag_l2_debug;
#define DIAG_MODULE "diag_l2_mb1"


int
diag_l2_proto_mb1_startcomms( diag_l2_conn_t	*d_l2_conn, u_int16_t flags,
	int bitrate, u_int8_t target, u_int8_t source)
{
	struct diag_l1_init in;
	u_int8_t cbuf[2];
	int rv;
	int baud;
	unsigned char rxbuf[1024];

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

	/*
	 * If 0 has been specified, use a suitable default
	 */
	if (bitrate == 0)
		baud = 9600;
	else
		baud = bitrate;
	d_l2_conn->diag_l2_speed = baud;

	/* Set the speed as shown */
	rv = diag_l1_setspeed( d_l2_conn->diag_link->diag_l2_fd,
		baud, 8, 1, DIAG_L1_PAR_N);
	if (rv < 0)
		return (rv);

	/* Empty our Receive buffer and wait for idle bus */
	(void)diag_os_read( d_l2_conn->diag_link->diag_l2_fd, cbuf, 1024,
		200);

	/*
	 * Ensure bus is idle for 300ms (This may not be needed)
	 */
	usleep(300000);

	/*
	 * Do 5Baud init
	 */
	in.type = DIAG_L1_INITBUS_5BAUD;
	in.addr = target;
	rv = diag_l2_ioctl(d_l2_conn, DIAG_IOCTL_INITBUS, &in);
	if (rv < 0)
		return(rv);

	/*
	 * L0 code should have set correct baud rate now etc
	 * Now read keybytes, ignoring parity
	 */
	rv = diag_l1_recv (d_l2_conn->diag_link->diag_l2_fd, 0,
		cbuf, 1, 100);
	if (rv < 0)
		return(rv);
	rv = diag_l1_recv (d_l2_conn->diag_link->diag_l2_fd, 0,
		&cbuf[1], 1, 100);
	if (rv < 0)
	{
		return(rv);
	}

	if (diag_l2_debug & DIAG_DEBUG_INIT)
		printf("%s: startcomms conn %x got kb 0x%x 0x%x\n",
				DIAG_MODULE, d_l2_conn, cbuf[0], cbuf[1]);

	/*
	 * Check the received keybytes
	 */
	d_l2_conn->diag_l2_kb1 = cbuf[0];
	d_l2_conn->diag_l2_kb2 = cbuf[1];

	if (cbuf[0] != 0xC2)
		return(DIAG_ERR_WRONGKB);
	if (cbuf[1] != 0xCD)
		return(DIAG_ERR_WRONGKB);

	/*
	 * Set the P3max (idle timer) to 1 second
	 */
	d_l2_conn->diag_l2_p3max = 1000;

	/*
	 * Now we probably get a message back that we don't want
	 * particularly that tells us the ecu part num, h/w and s/w versions
	 */
	(void) diag_l2_proto_mb1_int_recv(d_l2_conn, 1000, rxbuf, sizeof(rxbuf));

	return(rv);
}

/*
 * Decode the message header, and check the message is complete
 * and that the checksum is correct.
 *
 * Once we know the actual msglen, *msglen is filled in, else it is set to 0
 *
 * Data/len is received data/len
 */
int
diag_l2_proto_mb1_decode(u_int8_t *data, int len, int *msglen)
{
	u_int16_t cksum;
	int i;

	if (diag_l2_debug & DIAG_DEBUG_READ)
	{
		int i;
		printf("%s: decode len %d", DIAG_MODULE, len);
		for (i = 0; i < len ; i++)
		{
			printf(" 0x%x", data[i]&0xff);
		}
		printf("\n");
	}
	*msglen = 0;

	if (len < 3)
		return(DIAG_ERR_INCDATA);

	if (data[2] > len)
		return(DIAG_ERR_INCDATA);

	*msglen = data[3];

	for (i=0, cksum=0; i < len-2; i++)
	{
		cksum += data[i];
	}
	if (data[len-2] != (cksum &0xff))
	{
		if (diag_l2_debug & DIAG_DEBUG_READ)
		{
			printf("%s: recv cksum 0x02%x 0x%02x, wanted 0x%x\n",
				DIAG_MODULE, data[len-1] & 0xff,
				data[len-2] &0xff, cksum & 0xffff);
		}
		return(DIAG_ERR_BADCSUM);
	}
	if (data[len-1] != ((cksum>>8) & 0xff))
	{
		if (diag_l2_debug & DIAG_DEBUG_READ)
		{
			printf("%s: recv cksum 0x02%x 0x%02x, wanted 0x%x\n",
				DIAG_MODULE, data[len-1] & 0xff,
				data[len-2] &0xff, cksum & 0xffff);
		}
		return(DIAG_ERR_BADCSUM);
	}
	return(0);
}

/*
 * Internal receive, reads a whole message from the ECU -
 * returns the data length of the packet received
 */
int
diag_l2_proto_mb1_int_recv(diag_l2_conn_t *d_l2_conn, int timeout,
	u_int8_t *data, int len)
{
	int rxoffset;
	int tout, rv;
	int msglen;
	int readlen;

	rxoffset = 0;
	tout = timeout;
	msglen = 0;
	readlen = 3;
	while ( (rv = diag_l1_recv (d_l2_conn->diag_link->diag_l2_fd, 0,
			&data[rxoffset], readlen, tout)) > 0)
	{
		rxoffset += rv;
		tout = 100;

		/* Got some data */
		
		rv = diag_l2_proto_mb1_decode(data, rxoffset, &msglen);

		if (rv >= 0)
		{
			/* Full packet ! */
			break;
		}
		else if (rv != DIAG_ERR_INCDATA)
		{
			/* Bad things happened */
			rxoffset = rv;
			break;
		}

		/* Not full, read some more */

		if (msglen)
			readlen = msglen - rxoffset;
		else if (rxoffset < 3)
			readlen = 3;
		else
			readlen = len - rxoffset;
	}
	return(rxoffset);
}


/*
 * Read data, attempts to get complete set of responses
 *
 */
diag_l2_proto_mb1_recv(diag_l2_conn_t *d_l2_conn, int timeout,
	void (*callback)(void *handle, diag_msg_t *msg), void *handle)
{
	u_int8_t	rxbuf[1024];
	int rv;
	diag_msg_t *msg;

	rv = diag_l2_proto_mb1_int_recv(d_l2_conn, timeout, rxbuf,
		sizeof(rxbuf));

	if (rv < 0)
		return(rv);

	if (diag_l2_debug & DIAG_DEBUG_READ)
		fprintf(stderr,
			"%s: recv conn 0x%x got %d byte message\n", 
				DIAG_MODULE, d_l2_conn, rv);
	if (rv < 5)
	{
		/* Bad, minimum message is 5 bytes */
		return(DIAG_ERR_BADDATA);
	}

	/*
	 * Ok, alloc a message
	 */
	msg = diag_allocmsg(rv - 4);
	msg->data[0] = rxbuf[1];		/* Command */
	memcpy(&msg->data[1], &rxbuf[3], rv - 3);	/* Data */
	(void)gettimeofday(&msg->rxtime, NULL);
	msg->len = rv - 4;
	msg->fmt = DIAG_FMT_FRAMED | DIAG_FMT_DATAONLY;

	/*
	 * Call user callback routine
	 */
	if (callback)
		callback(handle, msg);

	/* No longer needed */
	diag_freemsg(msg);

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

/*
 * Send the data, using MB1 protocol
 */
diag_l2_proto_mb1_send(diag_l2_conn_t *d_l2_conn, diag_msg_t *msg)
{
	int rv;
	int sleeptime;
	u_int8_t txbuf[1024];
	u_int16_t cksum;
	int i;

	if (diag_l2_debug & DIAG_DEBUG_WRITE)
		fprintf(stderr,
			"%s: diag_l2_send 0x%x 0x%x called\n",
				DIAG_MODULE, d_l2_conn, msg);

	/*
	 * Make sure enough time between last receive and this send
	 * In fact, because of the timeout on recv(), this is pretty small, but
	 * we take the safe road and wait the whole of p3min plus whatever
	 * delay happened before
	 */
	sleeptime = d_l2_conn->diag_l2_p3min;
	if (sleeptime > 0)
		diag_os_millisleep(sleeptime);

	txbuf[0] = d_l2_conn->diag_l2_destaddr;
	txbuf[1] = msg->data[0]; 	/* Command */
	txbuf[2] = msg->len + 4;	/* Data + Hdr + 2 byte checksum */
	memcpy(&txbuf[3], &msg->data[1], msg->len-1);

	/* Checksum is 16 bit addition, in LSB order on packet */
	for (i = 0, cksum = 0; i < (msg->len+2) ; i++)
		cksum += txbuf[i];

	txbuf[msg->len+2] = cksum & 0xff;
	txbuf[msg->len+3] = (cksum>>8) & 0xff;

	if ( (diag_l2_debug & DIAG_DEBUG_WRITE) &&
			(diag_l2_debug & DIAG_DEBUG_DATA))
	{
		int i;
		printf("About to send %d bytes: ", txbuf[2]);
		for (i=0; i<txbuf[2]; i++)
			printf("0x%02x ", txbuf[i]);
		printf("\n");
	}

	rv = diag_l1_send (d_l2_conn->diag_link->diag_l2_fd, 0,
		txbuf, txbuf[2], d_l2_conn->diag_l2_p4min);

	if (diag_l2_debug & DIAG_DEBUG_WRITE)
		fprintf(stderr, "%s: send() about to return %d\n",
				DIAG_MODULE, rv);

	return(rv);
}

diag_msg_t *
diag_l2_proto_mb1_request(diag_l2_conn_t *d_l2_conn, diag_msg_t *msg,
		int *errval)
{
	int rv;
	diag_msg_t *rmsg = NULL;
	u_int8_t	rxbuf[1024];

	rv = diag_l2_send(d_l2_conn, msg);
	if (rv < 0)
	{
		*errval = rv;
		return(NULL);
	}
	/* And wait for response for 1 second */

	rv = diag_l2_proto_mb1_int_recv(d_l2_conn, 1000, rxbuf,
		sizeof(rxbuf));

	if (diag_l2_debug & DIAG_DEBUG_READ)
		fprintf(stderr,
			"%s: msg receive conn 0x%x got %d byte message\n", 
				DIAG_MODULE, d_l2_conn, rv);
	if (rv < 5)
	{
		/* Bad, minimum message is 5 bytes, or error happened */
		if (rv < 0)
			*errval = rv;
		else
			*errval = DIAG_ERR_BADDATA;
	}
	else
	{
		/*
		 * Ok, alloc a message
		 */
		rmsg = diag_allocmsg(rv - 4);
		rmsg->data[0] = rxbuf[1];		/* Command */
		memcpy(&rmsg->data[1], &rxbuf[3], rv - 3);	/* Data */
		(void)gettimeofday(&rmsg->rxtime, NULL);
		rmsg->len = rv - 4;
		rmsg->fmt = DIAG_FMT_FRAMED | DIAG_FMT_DATAONLY;
	}
	return(rmsg);
}


/*
 * Timeout, called to send idle packet to keep link to ECU alive
 */
void
diag_l2_proto_mb1_timeout(diag_l2_conn_t *d_l2_conn)
{
	diag_msg_t msg;
	u_int8_t txbuf[8];
	u_int8_t rxbuf[1000];
	int rv;

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

	txbuf[0] = 0x50;	/* Idle command */
	txbuf[1] = 0x01;	
	msg.data = txbuf;
	msg.len = 2;

	/* Use l2_send as it updates timeout timers */
	rv =  diag_l2_send(d_l2_conn, &msg);

	/* And receive/ignore the response */
	if (rv >= 0)
		(void) diag_l2_proto_mb1_int_recv(d_l2_conn, 1000, rxbuf, sizeof(rxbuf));
	return;
}