diag_l3_saej1979.c

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

		int badpacket;

		badpacket = 0;

		sae_msglen = diag_l3_j1979_getlen(d_l3_conn->rxbuf,
					d_l3_conn->rxoffset);

		if (diag_l3_debug & DIAG_DEBUG_PROTO)
		{
			printf("%s: process_data rxoffset is %d sae_msglen is %d\n",
				DIAG_MODULE, d_l3_conn->rxoffset, sae_msglen);
			printf("%s: process_data hex data is ",
				DIAG_MODULE);
			for (i=0; i < d_l3_conn->rxoffset; i++)
				printf("%02x ", d_l3_conn->rxbuf[i]);
			printf("\n");
		}

		if (sae_msglen < 0)
		{
			if (sae_msglen == DIAG_ERR_INCDATA)
			{
				/* Not enough data in this frame */
				return;
			}
			else
			{
				/* Duff data received, bad news ! */
				badpacket = 1;
			}
		}

		if (badpacket || (sae_msglen <= d_l3_conn->rxoffset ))
		{

			/* Bad packet, or full packet, need to tell user */
			u_int8_t *data;
			diag_msg_t *lmsg;

			msg = calloc(1, sizeof(diag_msg_t));
			if (msg == NULL)
			{
				/* Stuffed, no memory, cant do anything */
				return;
			}

			if (!badpacket)
				data = malloc(sae_msglen);
	
			if (badpacket || (data == NULL))
			{
				/* Failure indicated by zero len msg */
				msg->data = NULL;
				msg->len = 0;
			}
			else
			{
				msg->fmt = DIAG_FMT_ISO_FUNCADDR;
				msg->type =  d_l3_conn->rxbuf[0];
				msg->dest = d_l3_conn->rxbuf[1];
				msg->src =  d_l3_conn->rxbuf[2];
/* XXX check checksum */
				/* Copy in J1979 part of message */
				memcpy(data, &d_l3_conn->rxbuf[3], sae_msglen - 4);
				/* remove whole message from rx buf */
				memcpy(d_l3_conn->rxbuf, 
					&d_l3_conn->rxbuf[sae_msglen],
					sae_msglen);

				d_l3_conn->rxoffset -= sae_msglen;

				msg->data = data;
				msg->len = sae_msglen - 4;
			}

			gettimeofday(&msg->rxtime, NULL);

			/* Add it to the list */
			if (d_l3_conn->msg == NULL)
			{
				d_l3_conn->msg = msg;
			}
			else
			{
				lmsg = d_l3_conn->msg;
				while (lmsg->next != NULL)
				{
					lmsg = lmsg->next;
				}
				lmsg->next = msg;
			}
			if (badpacket)
			{
				/* No point in continuing */
				break;
			}
		}
		else
		{
			/* Need some more data */
			break;
		}
	}
}

/*
 * Receive a J1979 frame (building it as we get small amounts of data)
 *
 * - timeout expiry will cause return before complete packet
 *
 * Successful packet receive will call the callback routine with the message
 */
int
diag_l3_j1979_recv(diag_l3_conn_t *d_l3_conn, int timeout,
	void (* rcv_call_back)(void *handle ,diag_msg_t *) , void *handle)
{
	int rv;
	diag_msg_t *msg;
	int tout;
	int state;

#define ST_STATE1 1
#define ST_STATE2 2
#define ST_STATE3 3
#define ST_STATE4 4

/* XXX stuff do do here */

	if (d_l3_conn->d_l3l2_flags & DIAG_L2_FLAG_FRAMED)
	{
		/*
		 * L2 does framing stuff , which means we get one message
		 * with nicely formed frames
		 */
		state = ST_STATE4;
	}
	else
	{
		state = ST_STATE1;
	}

	/* Store the callback routine for use if needed */
	d_l3_conn->callback = rcv_call_back;
	d_l3_conn->handle = handle;

	/*
	 * This works by doing a read with a timeout of 0, to collect
	 * any data that was present on the FD, if no messages complete
	 * then read with the normal timeout, then read with a timeout
	 * of p4max ms until no more data is left (or timeout), then call
	 * the callback routine (if there is a message complete)
	 */
	while (1)
	{
		/* State machine for setting timeout values */
		if (state == ST_STATE1)
			tout = 0;
		else if (state == ST_STATE2)
			tout = timeout;
		else if (state == ST_STATE3)
			tout = 5; /* XXX should be p4max */
		else if (state == ST_STATE4)
			tout = timeout;

		if (diag_l3_debug & DIAG_DEBUG_PROTO)
			printf("%s: recv state %d tout %d\n",
				DIAG_MODULE, state, tout);

		/*
		 * Call L2 receive, L2 will build up the datapacket and
		 * call the callback routine if needed, we use a timeout
		 * of zero to see if data exists *now*,
		 */
		rv = diag_l2_recv(d_l3_conn->d_l3l2_conn, tout,
			diag_l3_rcv_callback, (void *)d_l3_conn);

		if (diag_l3_debug & DIAG_DEBUG_PROTO)
			printf("%s: recv returns %d\n",
				DIAG_MODULE, rv);

		if ((rv < 0) && (rv != DIAG_ERR_TIMEOUT))
			break;		/* Some nasty failure */

		if (rv == DIAG_ERR_TIMEOUT)
		{
			if ( (state == ST_STATE3) || (state == ST_STATE4) )
			{
				/* Finished */
				break;
			}
			if ( (state == ST_STATE1) && (d_l3_conn->msg == NULL) )
			{
				/*
				 * Try again, with real timeout
				 * (and thus sleep)
				 */
				state = ST_STATE2;
				tout = timeout;
				continue;
			}
		}

		if (state != ST_STATE4)
		{
			/* Process the data into messages */
			diag_l3_j1979_process_data(d_l3_conn);

			if (diag_l3_debug & DIAG_DEBUG_PROTO)
				printf("%s: recv process_data called, msg 0x%x rxoffset %d\n",
					DIAG_MODULE, d_l3_conn->msg,
					d_l3_conn->rxoffset);

			/*
			 * If there is a full message, remove it, call back
			 * the user call back routine with it, and free it
			 */
			msg = d_l3_conn->msg;
			if (msg)
			{
				d_l3_conn->msg = msg->next;

				if ( (d_l3_conn->d_l3l2_flags & DIAG_L2_FLAG_DATA_ONLY) == 0)
				{
					/* Strip hdr/checksum */
					msg->data += 3;
					msg->len -= 4;
				}
				rcv_call_back(handle, msg);
				if (msg->len)
					free (msg->data);
				free (msg);
				rv = 0;
				/* And quit while we are ahead */
				break;
			}
		}
		/* We do not have a complete message (yet) */
		if (state == ST_STATE2)
		{
			/* Part message, see if we get some more */
			state = ST_STATE3;
		}
		if (state == ST_STATE1)
		{
			/* Ok, try again with proper timeout */
			state = ST_STATE2;
		}
		if ((state == ST_STATE3) || (state == ST_STATE4))
		{
			/* Finished, we only do read once in this state */
			break;
		}
	}

	return(rv);
}

/*
 * This is called without the ADDR_ADDR_1 on it, ie it contains
 * just the SAEJ1979 data
 */
char *
diag_l3_j1979_decode(diag_l3_conn_t *d_l3_conn, diag_msg_t *msg)
{
	int i, j;
	static char buf[1024];
	char buf2[16];
/*	char *s;*/
	char area;

	if (msg->data[0] & 0x40)
		sprintf(buf, "J1979 response ");
	else
		sprintf(buf, "J1979 request ");

	switch (msg->data[0])
	{
	case 0x01:
		sprintf(buf2, "Mode 1 PID 0x%x", msg->data[1]);
		strcat(buf, buf2);
		break;
	case 0x41:
		sprintf(buf2,"Mode 1 Data: PID 0x%x ", msg->data[1]);
		strcat(buf, buf2);
		for (i=2; i < msg->len; i++)
		{
			sprintf(buf2, "0x%x ", msg->data[i]);
			strcat(buf, buf2);
		}
		break;
	case 0x02:
		sprintf(buf2, "Mode 2 PID 0x%x Frame 0x%x", msg->data[1,
			msg->data[2]]);
		break;
	case 0x42:
		sprintf(buf2,"Mode 2 FreezeFrame Data: PID 0x%x Frame 0x%x ",
			msg->data[1], msg->data[2]);
		strcat(buf, buf2);
		for (i=3; i < msg->len; i++)
		{
			sprintf(buf2, "0x%x ", msg->data[i]);
			strcat(buf, buf2);
		}
		break;
	case 0x03:
		sprintf(buf2,"Mode 2 (Powertrain DTCs)");
		strcat(buf, buf2);
		break;
	case 0x47:
		sprintf(buf2, "Non CMS ");
		strcat(buf, buf2);
		/* Fallthru */
	case 0x43:
		sprintf(buf2,"DTCs: ");
		strcat(buf, buf2);
		for (i=0, j=1; i<3; i++, j+=2)
		{
			if ((msg->data[j]==0) && (msg->data[j+1]==0))
				continue;
			
			switch ((msg->data[j] >> 6) & 0x03)
			{
			case 0:
				area = 'P';
				break;
			case 1:
				area = 'C';
				break;
			case 2:
				area = 'B';
				break;
			case 3:
				area = 'U';
				break;
			}
			sprintf(buf2, "%c%02x%02x ", area, msg->data[j] & 0x3f,
				msg->data[j+1]&0xff);
			strcat(buf, buf2);
			strcat(buf, " ");
		}
		break;
	case 0x04:
		sprintf(buf2, "Clear DTCs");
		strcat(buf, buf2);
		break;
	case 0x44:
		sprintf(buf2, "DTCs cleared");
		strcat(buf, buf2);
		break;
	case 0x05:
		sprintf(buf2, "Oxygen Sensor Test ID 0x%x Sensor 0x%x",
				buf[1], buf[2]);
		strcat(buf, buf2);
		break;
	case 0x07:
		sprintf(buf2,
			"Request Non-Continuous Monitor System Test Results");
		strcat(buf, buf2);
		break;
	/* case 0x47: */

	case 0x45:

	case 0x06:
	case 0x46:
	case 0x08:
	case 0x09:

	default:
		sprintf(buf2,"UnknownType 0x%x: Data Dump: ", msg->data[0]);
		strcat(buf, buf2);
		for (i=0; i < msg->len; i++)
		{
			sprintf(buf2, "0x%x ", msg->data[i]);
			strcat(buf, buf2);
		}
	}
	return(buf);
}


/*
 * Timer routine, called with time (in ms) since the "timer" value in
 * the L3 structure
 */
void
diag_l3_j1979_timer(diag_l3_conn_t *d_l3_conn, int ms)
{
	diag_msg_t msg;
	u_int8_t data[6];

	/* J1979 needs keepalive at least every 5 seconds, we use 3.5s */
/* XXX is this needed for all types of physical interface ? */
	if (ms < 3500)
		return;

	/* Does L2 do keepalive for us ? */
	if (d_l3_conn->d_l3l2_flags & DIAG_L2_FLAG_KEEPALIVE)
		return;

	/* OK, do keep alive on this connection */

	if (diag_l3_debug & DIAG_DEBUG_TIMER)
	{
		fprintf(stderr, "%s: timeout impending for %x %d ms\n",
				DIAG_MODULE, d_l3_conn, ms);
	}

	/*
	 * Mode 1 Pid 0 request is the SAEJ1979 idle message
	 * XXX Need to get the address bytes correct
	 */
	msg.data = data;
	msg.len = 2;
	data[0] = 1 ;		/* Mode 1 */
	data[1] = 0; 		/* Pid 0 */

	/*
	 * And set the source address, if no sends have happened, then
	 * the src address will be 0, so use the default used in J1979
	 */
	if (d_l3_conn->src)
		msg.src = d_l3_conn->src;
	else
		msg.src = 0xF1;		/* Default as used in SAE J1979 */

	/* Send it */
	(void)diag_l3_send(d_l3_conn, &msg);

	/* Get and ignore the response */
	(void)diag_l3_recv(d_l3_conn, 50, NULL, NULL);
	
	return;
}