diag_l2_iso14230.c

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

	memset(data, 0, sizeof(data));

	/*
	 * If 0 has been specified, use the correct speed
	 * for ISO14230 protocol
	 */
	if (bitrate == 0)
		bitrate = 10400;
	d_l2_conn->diag_l2_speed = bitrate;

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

	dp->state = STATE_CONNECTING ;

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

	switch (dp->type)
	{

	/* Fast initialisation */
	case DIAG_L2_TYPE_FASTINIT:

		/* Build an ISO14230 StartComms message */
		if (flags & DIAG_L2_TYPE_FUNCADDR)
		{
			msg.fmt = DIAG_FMT_ISO_FUNCADDR;
			d_l2_conn->diag_l2_physaddr = 0; /* Don't know it yet */
		}
		else
		{
			msg.fmt = 0;
			d_l2_conn->diag_l2_physaddr = target;
		}
		msg.src = source;
		msg.dest = target;
		msg.len = 1 ;
		data[0]= DIAG_KW2K_SI_SCR ;	/* startCommunication rqst*/
		msg.data = data;

		/* Do fast init stuff */
		in.type = DIAG_L1_INITBUS_FAST;
		rv = diag_l2_ioctl(d_l2_conn, DIAG_IOCTL_INITBUS, &in);

		if (rv < 0)
			break;

		/* Send the prepared message */
		diag_l2_proto_14230_send(d_l2_conn, &msg);

		if (d_l2_conn->diag_link->diag_l2_l1flags & DIAG_L1_DOESL2FRAME)
			timeout = 200;
		else
			timeout = d_l2_conn->diag_l2_p2max + 20;

		/* And wait for a response, ISO14230 says will arrive in P2 */
		rv = diag_l2_proto_14230_int_recv(d_l2_conn, 
				timeout, data, &len);
		if (rv < 0)
		{
			free(dp);
			return(rv);
		}

		rv = diag_l2_proto_14230_decode( data, len,
			&hdrlen, &datalen, &datasrc, NULL, dp->first_frame);
		if (rv < 0)
		{
			free(dp);
			return(rv);
		}

		switch (data[hdrlen])
		{
		case DIAG_KW2K_RC_SCRPR:	/* StartComms +ve response */

			d_l2_conn->diag_l2_kb1 = data[hdrlen+1];
			d_l2_conn->diag_l2_kb2 = data[hdrlen+2];
			d_l2_conn->diag_l2_physaddr = datasrc;

			if (diag_l2_debug & DIAG_DEBUG_PROTO)
			{
				fprintf(stderr,
					"%s: diag_l2_14230_StartComms", 
					DIAG_MODULE);
				fprintf(stderr," Physaddr 0x%x",
					datasrc);
				fprintf(stderr," KB1 = %x, KB2 = %x\n",
					d_l2_conn->diag_l2_kb1,
					d_l2_conn->diag_l2_kb2);
			}
			dp->state = STATE_ESTABLISHED ;
			break;

		case DIAG_KW2K_RC_NR:
			if (diag_l2_debug & DIAG_DEBUG_PROTO)
			{
				fprintf(stderr,
					"%s: diag_l2_14230_StartComms", 
					DIAG_MODULE);
				fprintf(stderr, " got -ve response\n");
			}
			free(dp);
			return(DIAG_ERR_ECUSAIDNO);
		default:
			if (diag_l2_debug & DIAG_DEBUG_PROTO)
			{
				fprintf(stderr,
					"%s: diag_l2_14230_StartComms", 
					DIAG_MODULE);
				fprintf(stderr, " got unexpected response 0x%x\n",
					data[hdrlen]);
			}
			free(dp);
			return(DIAG_ERR_ECUSAIDNO);
		}
		break;


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


		/* Mode bytes are in 7-Odd-1, read as 8N1 and ignore parity */
		rv = diag_l1_recv (d_l2_conn->diag_link->diag_l2_fd, 0,
			cbuf, 1, 100);
		if (rv < 0)
			break;
		rv = diag_l1_recv (d_l2_conn->diag_link->diag_l2_fd, 0,
			&cbuf[1], 1, 100);
		if (rv < 0)
			break;

		/* ISO14230 uses KB2 of 0x8F */
		if (cbuf[1] != 0x8f)
			return(DIAG_ERR_WRONGKB);

		/* Note down the mode bytes */
		d_l2_conn->diag_l2_kb1 = cbuf[0] & 0x7f;
		d_l2_conn->diag_l2_kb2 = cbuf[1] & 0x7f;

		if ( (d_l2_conn->diag_link->diag_l2_l1flags
			& DIAG_L1_DOESSLOWINIT) == 0)
		{

			/*
			 * Now transmit KB2 inverted
			 */
			cbuf[0] = ~ d_l2_conn->diag_l2_kb2;
			rv = diag_l1_send (d_l2_conn->diag_link->diag_l2_fd, 0,
				cbuf, 1, d_l2_conn->diag_l2_p4min);

			/*
			 * And wait for the address byte inverted
			 */
			rv = diag_l1_recv (d_l2_conn->diag_link->diag_l2_fd, 0,
				cbuf, 1, 350);

			if (cbuf[0] != ~target)
				return(DIAG_ERR_WRONGKB);

		}

		dp->state = STATE_ESTABLISHED ;
		break;

	case DIAG_L2_TYPE_MONINIT:
		/* Monitor mode, don't send anything */

		dp->state = STATE_ESTABLISHED ;
		rv = 0;
		break;

	default:
		rv = DIAG_ERR_INIT_NOTSUPP;
		break;

	}

	if (rv < 0)
	{
		free(dp);
		return (rv);
	}

	/*
	 * Now, we want to remove any rubbish left
	 * in inbound buffers, and wait for the bus to be
	 * quiet for a while before we will communicate
	 * (so that the next byte received is the first byte
	 * of an iso14230 frame, not a middle byte)
	 * We use 1/2 of P2max (inter response gap) or
	 * 5 * p4max (inter byte delay), whichever is larger
	 * a correct value to use
	 */
	wait_time = d_l2_conn->diag_l2_p2max / 2 ;
	if ((d_l2_conn->diag_l2_p4max * 5) > wait_time)
		wait_time = d_l2_conn->diag_l2_p4max * 5;

	while ( diag_l1_recv (d_l2_conn->diag_link->diag_l2_fd, 0,
		  data, sizeof(data), wait_time) != DIAG_ERR_TIMEOUT) ;

	/* And we're done */
	dp->state = STATE_ESTABLISHED ;

	return(0);
}

int
diag_l2_proto_14230_stopcomms(diag_l2_conn_t* pX)
{
	/*
	 * Send a stopcomms message, and wait for the +ve response, for upto
	 * p3max - the layer 2 code that called this already turned off the
	 * idle timer
	 */
/* XXX */

	return (0);
}

/*
 * Just send the data
 *
 * We add the header and checksums here as appropriate, based on the keybytes
 *
 * We take the source and dest from the internal data NOT from the msg fields
 *
 * We also wait p3 ms 
 */
diag_l2_proto_14230_send(diag_l2_conn_t *d_l2_conn, diag_msg_t *msg)
{
	int rv, csum, i;
	int len;
	u_int8_t buf[1024];
	int offset;
	struct diag_l2_14230 *dp;

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

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

	/* Build the new message */

	if (dp->modeflags & DIAG_L2_TYPE_FUNCADDR)
		buf[0] = 0xC0;
	else
		buf[0] = 0x80;

	/* If user supplied addresses, use them, else use the originals */
	if (msg->dest)
		buf[1] = msg->dest;
	else
		buf[1] = dp->dstaddr;
	if (msg->src)
		buf[2] = msg->src;
	else
		buf[2] = dp->srcaddr;

/* XXX, check mode flag that specifies always use 4 byte hdr , 
	or mode flag showing never use extended header, and
		received keybytes */

	if (msg->len < 64)
	{
		if (msg->len < 1)
			return(DIAG_ERR_BADLEN);
		buf[0] |= msg->len;
		offset = 3;
	}
	else
	{
		/* Extended length */
		if (msg->len > 255)
			return(DIAG_ERR_BADLEN);
		buf[3] = msg->len;
		offset = 4;
	}
	memcpy(&buf[offset], msg->data, msg->len);

	len = msg->len + offset;	/* data + hdr */

	if ((d_l2_conn->diag_link->diag_l2_l1flags & DIAG_L1_DOESL2CKSUM) == 0)
	{
		/* We must add checksum, which is sum of bytes */
		for (i = 0, csum = 0; i < len; i++)
			csum += buf[i];
		buf[len] = csum;
		len++;				/* + checksum */
	}

	/* Wait p3min milliseconds, but not if doing fast/slow init */
	if (dp->state == STATE_ESTABLISHED)
		usleep(d_l2_conn->diag_l2_p3min * 1000);

	rv = diag_l1_send (d_l2_conn->diag_link->diag_l2_fd, 0,
		buf, len, 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);
}

/*
 * Protocol receive routine
 *
 * Will sleep until a complete set of responses has been received, or fail
 * with a timeout error
 *
 * The interbyte type in data from an ECU is between P1Min and P1Max
 * The intermessage time for part of one response is P2Min and P2Max
 *
 * If we are running with an intelligent L1 interface, then we will be
 * getting one message per frame, and we will wait a bit longer
 * for extra messages
 */
diag_l2_proto_14230_recv(diag_l2_conn_t *d_l2_conn, int timeout,
	void (*callback)(void *handle, diag_msg_t *msg),
	void *handle)
{
	u_int8_t data[256];
	int rv;
	int datalen;

	/* Call internal routine */
	rv = diag_l2_proto_14230_int_recv(d_l2_conn, timeout, data, &datalen);

	if (rv < 0)	/* Failure */
		return(rv);

	if (diag_l2_debug & DIAG_DEBUG_READ)
	{
		printf("%s: calling rcv callback %x handle %x\n", DIAG_MODULE,
			callback, handle);
	}

	/*
	 * Call user callback routine
	 */
	if (callback)
		callback(handle, d_l2_conn->diag_msg);

	/* No longer needed */
	diag_freemsg(d_l2_conn->diag_msg);
	d_l2_conn->diag_msg = NULL;

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

	return(0);
}

diag_msg_t *
diag_l2_proto_14230_request(diag_l2_conn_t *d_l2_conn, diag_msg_t *msg,
		int *errval)
{
	int rv;
	diag_msg_t *rmsg = NULL;

	rv = diag_l2_send(d_l2_conn, msg);
	if (rv < 0)
	{
		*errval = rv;
		return(NULL);
	}

	while (1)
	{
		rv = diag_l2_proto_14230_int_recv(d_l2_conn,
			d_l2_conn->diag_l2_p2max + 10, NULL, NULL);

		if (rv < 0)
		{
			*errval = DIAG_ERR_TIMEOUT;
			rmsg = NULL;
			break;
		}

		/*
		 * The connection now has the received message data
		 * stored, remove it and deal with it
		 */
		rmsg = d_l2_conn->diag_msg;
		d_l2_conn->diag_msg = NULL;

		/* Got a Error message */
		if (rmsg->data[0] == DIAG_KW2K_RC_NR)
		{
			if (rmsg->data[2] == DIAG_KW2K_RC_B_RR)
			{
				/*
				 * Msg is busyRepeatRequest
				 * So do a send again
				 */
				rv = diag_l2_send(d_l2_conn, msg);
				if (rv < 0)
				{
					*errval = rv;
					return(NULL);
				}
				diag_freemsg(rmsg);
				continue;
			}

			if (rmsg->data[2] == DIAG_KW2K_RC_RCR_RP)
			{
				/*
				 * Msg is a requestCorrectyRcvd-RspPending
				 * so do read again
				 */
				diag_freemsg(rmsg);
				continue;
			}
			/* Some other type of error */
		}
		else
		{
			/* Success */
			break;
		}
	}
	/* Return the message to user, who is responsible for freeing it */
	return(rmsg);
}


/*
 * AccessTimingParameters
 */
int
diag_l2_proto_14230_atp(diag_l2_conn_t* d_l2_conn, int p2min, int p2max, int p3min, int p3max, int p4min)
{
	return (0);
}

/*
 * Timer, called regularly, we don't do anything with this
 */
void
diag_l2_proto_14230_timer(int iX)
{
}

/*
 * Timeout, - if we don't send something to the ECU it will timeout
 * soon, so send it a keepalive message now.
 */
void
diag_l2_proto_14230_timeout(diag_l2_conn_t *d_l2_conn)
{
	struct diag_l2_14230 *dp;
	diag_msg_t	msg;
	u_int8_t data[256];
	int timeout;

	dp = d_l2_conn->diag_l2_proto_data;

	if (diag_l2_debug & DIAG_DEBUG_TIMER)
	{
		fprintf(stderr, "%s: timeout impending for %x type %d\n",
				DIAG_MODULE, d_l2_conn, dp->type);
	}

	msg.data = data;

	/* Prepare the "keepalive" message */
	if (dp->modeflags & DIAG_L2_IDLE_J1978)
	{
		/* Idle using J1978 spec */
		msg.len = 2;
		data[0] = 1;
		data[1] = 0;
	} else {
		/* Idle using ISO "Tester Present" message */
		msg.len = 1;
		msg.dest = 0;	/* Use default */
		msg.src = 0;	/* Use default */
		data[0] = DIAG_KW2K_SI_TP;
	}

	/*
	 * There is no point in checking for errors, or checking
	 * the received response as we can't pass an error back
	 * from here
	 */

	/* Send it, important to use l2_send as it updates the timers */
	(void)diag_l2_send(d_l2_conn, &msg);

	/*
	 * Get the response in p2max, we allow longer, and even
	 * longer on "smart" L2 interfaces
	 */
	timeout = d_l2_conn->diag_l2_p3min;
	if (d_l2_conn->diag_link->diag_l2_l1flags &
			(DIAG_L1_DOESL2FRAME|DIAG_L1_DOESP4WAIT))
	{
		if (timeout < 100)
			timeout = 100;
	}
	(void)diag_l2_recv(d_l2_conn, timeout, NULL, NULL);
}