diag_l0_br.c

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

		break;
	case DIAG_L1_INITBUS_FAST:
		if ((dev->dev_features & BR_FEATURE_FASTINIT) == 0)
		{
			/* Fast init Not supported */
			rv = -1;
		}
		else
		{
			/* Fastinit done on 1st TX */
			dev->dev_state =  BR_STATE_KWP_FASTINIT;
			rv = 0;
		}
		break;
	default:
		rv = -1;
		break;
	}
	return(rv);
}

/*
 * Set speed/parity etc
 *
 * If called by the user then we ignore what he says and use 19200
 * 8 none 1
 *
 * The internal routine still exists because it makes the code
 * look similar to the other L0 interfaces
 */
diag_l0_br_setspeed(int fd, int speed, int bits,
		int stop_bits, int parflag )
{
	struct diag_l0_br_device *dev;
	dev = diag_l0_br_finddev(fd);	/* Find dev for this fd */

	return (diag_os_tty_setup(fd, &dev->dev_ttystate, 19200, 8, 1,
				DIAG_L1_PAR_N) );
}

/*
 * Routine to read a whole BR1 message
 * length of which depends on the first value received.
 * This also handles "error" messages (top bit of first value set)
 *
 * Returns length of received message, or TIMEOUT error, or BUSERROR
 * if the BR interface tells us theres a congested bus
 */
diag_l0_br_getmsg(int fd, u_int8_t *dp, int timeout)
{
	int cnt, offset, ret;
	u_int8_t firstbyte;
	int readlen;

	if ( (diag_l0_debug & (DIAG_DEBUG_READ|DIAG_DEBUG_DATA)) ==
			(DIAG_DEBUG_READ|DIAG_DEBUG_DATA) )
	{
		fprintf(stderr, "%s: fd %d getmsg timeout %d\n",
			DIAG_MODULE, fd, timeout);
	}

	/*
	 * First read the 1st byte, using the supplied timeout
	 */
	ret = diag_os_read(fd, &firstbyte, 1, timeout);
	if (ret < 0)
	{
		if ( (diag_l0_debug & (DIAG_DEBUG_READ|DIAG_DEBUG_DATA)) ==
			(DIAG_DEBUG_READ|DIAG_DEBUG_DATA) )
		{
			fprintf(stderr, "%s: fd %d getmsg 1st byte timed out\n",
				DIAG_MODULE, fd);
		}
		return(ret);
	}

	/*
	 * Now read data
	 */
	offset = 0;
	readlen = firstbyte & 0x0f;
	while (offset != readlen)
	{
		/*
		 * Reasonable timeout here as the interface told us how
		 * much data to expect, so it should arrive
		 */
		cnt = diag_os_read(fd, &dp[offset], readlen - offset, 100);
		if (cnt < 0)
		{
			if ( (diag_l0_debug & (DIAG_DEBUG_READ|DIAG_DEBUG_DATA))
				== (DIAG_DEBUG_READ|DIAG_DEBUG_DATA) )
			{
				fprintf(stderr, "%s: fd %d getmsg byte %d of %d timed out\n",
				DIAG_MODULE, fd, offset, readlen );
			}
			return(DIAG_ERR_TIMEOUT);
		}
		offset += cnt;
	}

	if ( (diag_l0_debug & (DIAG_DEBUG_READ|DIAG_DEBUG_DATA)) ==
		(DIAG_DEBUG_READ|DIAG_DEBUG_DATA) )
	{
		int i;
		fprintf(stderr, "%s: fd %d getmsg read ctl 0x%x data:",
			DIAG_MODULE, fd, firstbyte & 0xff);
		for (i = 0; i < readlen; i++)
			printf(" 0x%x", dp[i] & 0xff);
		printf("\n");
	}

	/*
	 * Message read complete, check error flag
	 * Top bit set means error, Bit 6 = VPW/PWM bus
	 * congestion (i.e retry).
	 */
	if (firstbyte & 0x80)	/* Error indicator */
		return(DIAG_ERR_TIMEOUT);

	if (firstbyte & 0x40)	/* VPW/PWM bus conflict, need to retry */
		return(DIAG_ERR_BUSERROR);

	if (readlen == 0)	/* Should never happen */
		return(DIAG_ERR_TIMEOUT);

	return(readlen);
}

int
diag_l0_br_write(int fd, u_int8_t *dp, int txlen)
{
	int cnt;

	while ((cnt = write(fd, dp, txlen)) != txlen)
	{
		/* Partial write */
		if (cnt <  0)
		{
			/* error */
			if (errno != EINTR)
			{
				perror("write");
				fprintf(stderr, "%s: write returned error %d !!\n", DIAG_MODULE, errno);
				return(-1);
			}
			/* errno - EINTR is OK */
		}
		/* Successfully wrote cnt bytes (or 0 && EINTR), so inc pointers and continue */
		txlen -= cnt;
		dp += cnt;
	}
	return(0);
}


/* 
 * Write Message routine. Adds the length byte to the data before sending,
 * and the frame number for VPW/PWM. The type is used to set the top bits
 * of the control byte
 *
 * Returns 0 on success, <0 on error
 */
diag_l0_br_writemsg(struct diag_l0_br_device *dev, u_int8_t type,
		 char *dp, int txlen)
{
	int cnt, i, rv, j1850mode;
	u_int8_t outb;

	if ( (diag_l0_debug & (DIAG_DEBUG_WRITE|DIAG_DEBUG_DATA)) ==
			(DIAG_DEBUG_WRITE|DIAG_DEBUG_DATA) )
	{
		fprintf(stderr, "%s: device 0x%x fd %d sending to BR1\n",
			DIAG_MODULE, dev, dev->dev_fd);
	}

	if (txlen > 15)
	{
		return(DIAG_ERR_BADLEN);
	}

	if ((dev->dev_protocol == DIAG_L1_J1850_VPW) ||
		(dev->dev_protocol == DIAG_L1_J1850_PWM))
	{
		j1850mode = 1;
		outb = txlen + 1; /* We also send a frame number */
	}
	else
	{
		j1850mode = 0;
		outb = txlen;
	}

	outb |= type;	/* Set the type bits on the control byte */

	/* Send the length byte */
	rv = diag_l0_br_write(dev->dev_fd, &outb, 1);
	if (rv < 0)
		return(rv);

	/* And now the data */
	if ( (diag_l0_debug & (DIAG_DEBUG_WRITE|DIAG_DEBUG_DATA)) ==
			(DIAG_DEBUG_WRITE|DIAG_DEBUG_DATA) )
	{
		fprintf(stderr, "%s: device 0x%x writing data: ",
			DIAG_MODULE, dev);
		fprintf(stderr,"0x%x ", (int)outb);	/* Length byte */
		for (i=0; i<txlen; i++)
				fprintf(stderr, "0x%x ", dp[i] & 0xff); 
		fprintf(stderr, "\n");
	}	
	rv = diag_l0_br_write(dev->dev_fd, dp, txlen);
	if (rv < 0)
		return(rv);

	/*
	 * ISO mode is raw pass through. In J1850 we need to send
	 * frame numbers, and keep track of whether we are sending/receiving
	 * in order to receive multiple frames.
	 */
	if (j1850mode)
	{
		if ( (diag_l0_debug & (DIAG_DEBUG_WRITE|DIAG_DEBUG_DATA)) ==
			(DIAG_DEBUG_WRITE|DIAG_DEBUG_DATA) )
		{
			fprintf(stderr,
				"%s: device 0x%x writing data: 0x%x\n",
				DIAG_MODULE, dev, dev->dev_framenr & 0xff);
		}	
		rv = diag_l0_br_write(dev->dev_fd, &dev->dev_framenr, 1);
		if (rv < 0)
			return(rv);
	}
	return(0);
}


/*
 * Send a load of data
 *
 * Returns 0 on success, -1 on failure
 *
 * This routine will do a fastinit if needed, but all 5 baud inits
 * will have been done by the slowinit() code
 */
diag_l0_br_send(int fd, int subinterface, char *data, int len)
{
	int rv;

	struct diag_l0_br_device *dev;

	dev = diag_l0_br_finddev(fd);	/* Find dev info */

	if (diag_l0_debug & DIAG_DEBUG_WRITE)
	{
		int i;
		fprintf(stderr, "%s: device fd %d send %d bytes protocol %d state %d: ",
			DIAG_MODULE, fd, len,
			dev->dev_protocol, dev->dev_state);
		if (diag_l0_debug & DIAG_DEBUG_DATA)
		{
			int i;
			for (i=0; i<len; i++)
				fprintf(stderr, "0x%x ", data[i] & 0xff); 
			fprintf(stderr, "\n");
		}
		else
			fprintf(stderr,"\n");
	}

	/*
	 * Special handling for fastinit, we need to collect up the
	 * bytes of the StartComms message sent by the upper layer
	 * when we have a whole message we can then send the request
	 * as part of a special initialisation type
	 */
	if (dev->dev_state == BR_STATE_KWP_FASTINIT)
	{
		char outbuf[6];
		if (dev->dev_txlen < 5)
		{
			memcpy(&dev->dev_txbuf[dev->dev_txlen], data, len);
			dev->dev_txlen += len;
			rv = 0;
		}

		if (dev->dev_txlen >= 5)
		{
			/*
			 * Startcomms request is 5 bytes long - we have
			 * 5 bytes, so now we should send the initialisation
			 */
			outbuf[0] = 0x03;
			memcpy(&outbuf[1], dev->dev_txbuf, 5);
			rv = diag_l0_br_writemsg(dev, BR_WRTYPE_INIT,
				outbuf, 6);
			/* Stays in FASTINIT state until first read */
		}
	}
	else
	{
		/*
		 * Now, keep a copy of the data, and set the framenr to 1
		 * This means the receive code will resend the request if it
		 * wants to get a frame number 2 or 3 or whatever
		 */
		memcpy(dev->dev_rxbuf, data, len);
		dev->dev_txlen = len;
		dev->dev_framenr = 1;

		/* And now encapsulate and send the data */
		rv = diag_l0_br_writemsg(dev, BR_WRTYPE_DATA, data, len);
	}

	return(rv);
}

/*
 * Get data (blocking), returns number of chars read, between 1 and len
 * If timeout is set to 0, this becomes non-blocking
 *
 * This attempts to read whole message, so if we receive any data, timeout
 * is restarted
 *
 * Messages received from the BR1 are of format
 * <control_byte><data ..>
 * If control byte is < 16, its a length byte, else its a error descriptor
 */
diag_l0_br_recv(int fd , int subinterface, char *data, int len, int timeout)
{
	int cksum, cnt, rv, retrycnt;

	struct diag_l0_br_device *dev;
	dev = diag_l0_br_finddev(fd);	/* Find dev info */

	if (diag_l0_debug & DIAG_DEBUG_READ)
		fprintf(stderr,
			"%s: fd %d recv upto %d bytes timeout %d, rxlen %d offset %d framenr %d protocol %d state %d\n",
			DIAG_MODULE, fd, len, timeout, dev->dev_rxlen,
			dev->dev_rdoffset, dev->dev_framenr, dev->dev_protocol,
			dev->dev_state);

	switch (dev->dev_state)
	{
	case BR_STATE_KWP_FASTINIT:
		/* Extend timeouts */
		timeout = 300;
		dev->dev_state = BR_STATE_OPEN;
		break;
	case BR_STATE_KWP_SENDKB1:
		if (len >= 2)
		{
			data[0] = dev->dev_kb1;
			data[1] = dev->dev_kb2;
			dev->dev_state =  BR_STATE_OPEN;
			return (2);
		}
		else if (len == 1)
		{
			*data = dev->dev_kb1;
			dev->dev_state = BR_STATE_KWP_SENDKB2;
			return (1);
		}
		return(0);	/* Strange, user asked for 0 bytes */
		

	case BR_STATE_KWP_SENDKB2:
		if (len >= 1)
		{
			*data = dev->dev_kb2;
			dev->dev_state =  BR_STATE_OPEN;
			return (1);
		}
		return(0);	/* Strange, user asked for 0 bytes */
	}

	switch (dev->dev_protocol)
	{
	case DIAG_L1_ISO9141:
	case DIAG_L1_ISO14230:
		/* Raw mode */
		cnt = diag_os_read(dev->dev_fd, data, len, timeout);
		break;
	default:
		/*
		 * PWM/VPW Modes
		 *
		 * If theres stuff on the dev-descriptor, give it back
		 * to the user.
		 * We extend timeouts here because in PWM/VPW
		 * modes the interface tells us if there is a timeout, and
		 * we get out of sync if we dont wait for it.
		 */
		if (timeout < 500)
			timeout = 500;

		if (dev->dev_rxlen == 0)
		{
			/*
			 * No message available, try getting one
			 *
			 * If this is the 2nd read after a send, then
			 * we need to resend the request with the next
			 * frame number to see if any more data is ready
			 */
			if (dev->dev_framenr > 1)
			{
				rv = diag_l0_br_writemsg(dev,
					BR_WRTYPE_DATA,
					dev->dev_txbuf, dev->dev_txlen);
			}
			dev->dev_framenr++;

			retrycnt = 0;
			while (1)
			{
				dev->dev_rdoffset = 0;
				rv = diag_l0_br_getmsg(dev->dev_fd,
					dev->dev_rxbuf, timeout);
				if (rv >= 0)
				{
					dev->dev_rxlen = rv;
					break;
				}
				if ((rv != DIAG_ERR_BUSERROR) ||
					(retrycnt >= 30))
				{
					dev->dev_rxlen = 0;
					return(rv);
				}
				/* Need to resend and try again */
				rv = diag_l0_br_writemsg(dev,
					BR_WRTYPE_DATA, dev->dev_txbuf,
					dev->dev_txlen);
				if (rv < 0)
					return(rv);
				retrycnt++;
			}
		}
		if (dev->dev_rxlen)
		{
			int bufbytes = dev->dev_rxlen - dev->dev_rdoffset;

			if (bufbytes <= len)
			{
				memcpy(data, &dev->dev_rxbuf[dev->dev_rdoffset], bufbytes);
				dev->dev_rxlen = dev->dev_rdoffset = 0;
				return(bufbytes);
			}
			else
			{
				memcpy(data, &dev->dev_rxbuf[dev->dev_rdoffset], len);
				dev->dev_rdoffset += len;
				return(len);
			}
		}
		cnt = 0;
	}

	/* OK, got whole message */
	if (diag_l0_debug & DIAG_DEBUG_READ)
	{
		int i;
		fprintf(stderr,
			"%s: fd %d received from BR1: ", DIAG_MODULE, fd);
		for (i=0; i < cnt; i++)
				fprintf(stderr, "0x%x ", data[i]&0xff);
		fprintf(stderr, "\n");
	}

	return(cnt);
}


int
diag_l0_br_getflags(int fd)
{
	/*
	 * ISO14230/J1850 protocol does L2 framing, ISO9141 is just
	 * raw, once initialised
	 */
	struct diag_l0_br_device *dev;
	int flags;

	dev = diag_l0_br_finddev(fd);	/* Find dev info */

	flags = 0;
	switch (dev->dev_protocol)
	{
	case DIAG_L1_J1850_VPW:
	case DIAG_L1_J1850_PWM:
			flags = DIAG_L1_DOESL2FRAME;
			break;
	case DIAG_L1_ISO9141:
			flags = DIAG_L1_SLOW ;
			flags |= DIAG_L1_DOESP4WAIT;
			break;
 	case DIAG_L1_ISO14230:
			flags = DIAG_L1_SLOW | DIAG_L1_FAST | DIAG_L1_PREFFAST;
			flags |= DIAG_L1_DOESP4WAIT;
			break;
	}

	if (diag_l0_debug & DIAG_DEBUG_PROTO)
		fprintf(stderr,
			"%s: getflags fd %d proto %d flags 0x%x\n",
			DIAG_MODULE, fd, dev->dev_protocol, flags);

	return( flags );
}