Freego_MC_BOOT/Core/can_tp/i15765.c

#include "MCU_CAN.h"
#include "i15765.h"
#include "bits.h"

/* FC types */	//Flow Control
#define I15765_FS_CTS    0
#define I15765_FS_WT     1
#define I15765_FS_OVFLW  2
/* PDU types */
#define I15765_PDU_SF    0		//Single Frame
#define I15765_PDU_FF    1		//First Frame
#define I15765_PDU_CF    2		//Consecutive Frame
#define I15765_PDU_FC    3		//Flow Control  N_PCI

/* source address */
#define I15765CFG_SA                           0xf1

/* multi-frame TX buffer size */
#define I15765CFG_MF_TX_BUF_NUM                  1
#define I15765CFG_MF_TX_BUF_SIZE               1024

/* multi-frame RX buffer size */
#define I15765CFG_MF_RX_BUF_NUM                  1
#define I15765CFG_MF_RX_BUF_SIZE               2048

extern uint8_t can_rx(uint8_t p, can_t *frame);

extern void CAN_SendData(uint8_t canCom, uint32_t mailbox, uint32_t messageId, uint8_t * data, uint32_t len);

extern void i15765app_process(i15765_t *msg);

/* misc */
#define MIN(X, Y)       ((X) < (Y) ? (X) : (Y))
#define ADDR_FUNC(tat)  (tat == I15765_TAT_NF11 || tat == I15765_TAT_NF29)


enum
{
    STATE_IDLE = 0,
    STATE_TX_FF,
    STATE_TX_FC,
    STATE_TX_CF,
    STATE_WT_FC,
    STATE_WT_CF,
    STATE_TX_CTS,
    STATE_TX_OVFLW
};


/* multiframe rx type */
typedef struct
{
	uint8_t sn;		//Sequence Number
	i15765_t msg;
	uint8_t state;
	uint16_t timeout;
	uint8_t buf[I15765CFG_MF_RX_BUF_SIZE];

} i15765_mfr_t;


/* multiframe tx type */
typedef struct
{
	uint8_t bs;
	uint8_t bs_cnt;
	uint16_t st;
	uint16_t st_cnt;
	uint8_t sn;
	uint8_t wt_cnt;
	i15765_t msg;
	uint8_t state;
	uint8_t *status;
	uint16_t timeout;
	uint8_t buf[I15765CFG_MF_TX_BUF_SIZE];

} i15765_mft_t;


/* multiframe buffers */
i15765_mfr_t i15765_mfrb[I15765CFG_MF_RX_BUF_NUM];
i15765_mft_t i15765_mftb[I15765CFG_MF_TX_BUF_NUM];

/* used instead of NULL pointers */
uint8_t i15765_tmp;

/* source address */
uint8_t i15765_sa = I15765CFG_SA;




/*
** Compare address information of two messages.
**
** INPUT: msg1 - Pointer to first message
**        msg2 - Pointer to second message
**
** RETURN: 1: if the ai's match
**         0: otherwise
*/
uint8_t i15765_ai_cmp(i15765_t *msg1, i15765_t *msg2)
{
	if((msg1->sa != msg2->sa)
	        || (msg1->ta != msg2->ta)
	        || (msg1->tat != msg2->tat))
		return 0;

	return 1;
}
/*
** Initialize the receive and transmit buffers.
*/
void i15765_init(void)
{

	uint8_t i;

	/* clear the rx buffers */
	for(i = 0; i < I15765CFG_MF_RX_BUF_NUM; i++)
		i15765_mfrb[i].state = STATE_IDLE;

	/* clear the tx buffers */
	for(i = 0; i < I15765CFG_MF_TX_BUF_NUM; i++)
	{

		i15765_mftb[i].state = STATE_IDLE;
		i15765_mftb[i].status = 0;//&i15765_tmp;
	}

//	i15765app_init();
}



/*
** Sets the pointer to the first available index.
**
** INPUT: mfrb - pointer to available index
**
** RETURN: 0: success, index found
**         1: failure, buf is full
*/
uint8_t i15765_mfrb_get(i15765_mfr_t **mfrb)
{

	uint8_t i;

	/* find first available index */
	for(i = 0; i < I15765CFG_MF_RX_BUF_NUM; i++)
	{

		if(i15765_mfrb[i].state == STATE_IDLE)
		{

			*mfrb = &i15765_mfrb[i];
			return 0;
		}
	}

	return 1;
}

/*
** Sets the pointer to the first available index.
**
** INPUT: mftb - pointer to available index
**
** RETURN: 0: success, index found
**         1: failure, buf is full
*/
uint8_t i15765_mftb_get(i15765_mft_t **mftb)
{

	uint8_t i;

	/* find first available index */
	for(i = 0; i < I15765CFG_MF_TX_BUF_NUM; i++)
	{

		if(i15765_mftb[i].state == STATE_IDLE)
		{

			*mftb = &i15765_mftb[i];
			return 0;
		}
	}

	return 1;
}



/*
** Delete all receive mf buffers with matching AE info
*/
void i15765_mfrb_del(i15765_t *msg)
{
	uint8_t i;

	for(i = 0; i < I15765CFG_MF_RX_BUF_NUM; i++)
	{
		if(i15765_ai_cmp(&i15765_mfrb[i].msg, msg))
		{
			i15765_mfrb[i].state = STATE_IDLE;
		}
	}
	return;
}



/*
** Delete all transmit mf buffers with matching AE info
*/
void i15765_mftb_del(i15765_t *msg)
{

	uint8_t i;

	for(i = 0; i < I15765CFG_MF_TX_BUF_NUM; i++)
	{

		if(i15765_ai_cmp(&i15765_mftb[i].msg, msg)
		        && (i15765_mftb[i].state != STATE_IDLE))
		{

			i15765_mftb[i].state = STATE_IDLE;
			*i15765_mftb[i].status = I15765_FAILED;
			i15765_mftb[i].status = &i15765_tmp;
		}
	}

	return;
}



/*
** Sets the ptr to point to the first index in buf whose
** address information matches the current message.
**
** INPUT: msg - the address information to match
**        ptr - double pointer to matching index
**
** RETURN: 0 - match found
**         1 - no match
*/
uint8_t i15765_mftb_seek(i15765_t *msg, i15765_mft_t **ptr)
{

	uint8_t i;

	for(i = 0; i < I15765CFG_MF_TX_BUF_NUM; i++)
	{

		if(i15765_mftb[i].state != STATE_IDLE)
		{
			*ptr = &i15765_mftb[i];
			return 0;
		}
	}

	return 1;
}
/*
** Sets the ptr to point to the first index in buf whose
** address information matches the current message.
**
** INPUT: msg - the address information to match
**        ptr - double pointer to matching index
**
** RETURN: 0 - match found
**         1 - no match
*/
uint8_t i15765_mfrb_seek(i15765_t *msg, i15765_mfr_t **ptr)
{

	uint8_t i;

	for(i = 0; i < I15765CFG_MF_RX_BUF_NUM; i++)
	{
		if(i15765_mfrb[i].state != STATE_IDLE)
		{
			*ptr = &i15765_mfrb[i];
			return 0;
		}
	}

	return 1;
}
/*
** Convert an i15765 message to a CAN frame and transmit it
**
** INPUT:  msg - the message to transmit
**
** RETURN: 0 - success
**         1 - failure
**
*/
uint8_t i15765_tx(i15765_t *msg)
{

	can_t can;
	uint8_t i = 0;

	// 11bit  ��׼֡  ֱ�Ӹ�ֵID��
	/* physical response */
	can.id = UDS_TX_ID;
//	can.id = msg->ID;
	/* pack the data, garbage will be packed when we overflow so it is the
	   receiver's responsibility to only read as much as was valid */
	for(i = 0; i < 8; i++)
		can.buf[i] = (i < msg->buf_len) ? msg->buf[i] : BUFFER_DATA_LAST;

	/* fix all packets to 8 bytes per 15765-4 */
	can.buf_len = 8;

	/* Send message */
//	return can_tx(0, &can);

	CAN_SendData(UDS_CAN_COM, UDS_TX_MAILBOX, can.id, can.buf, can.buf_len);

	return 0;
}

/*
** Initialize and attempt transmission of a single frame message.
** INPUT:  frame - pointer to the i15765 frame to be transmitted
** RETURN: 0 - success
**         1 - failure
*/
uint8_t i15765_tx_sf(i15765_t *msg)
{

	uint8_t i;
	uint8_t cnt;
	i15765_t sf;
	uint8_t buf[8];

	/* copy over the old message */
	sf = *msg;

	/* start writing at the beginning */
	i = 0;

	/* construct SF N_PDU and pack it into a CAN frame */
	buf[i++] = (I15765_PDU_SF << 4) | (uint8_t)msg->buf_len;

	for(cnt = 0; cnt < msg->buf_len; cnt++)
		buf[i++] = msg->buf[cnt];

	sf.buf = buf;
	sf.buf_len = i;

	/* attempt to buffer the CAN frame */
	return i15765_tx(&sf);
}



/*
** Transmit a first frame.
** INPUT: frame - pointer to the i15765 multiframe buffer
*/
void i15765_tx_ff(i15765_mft_t *mftb)
{

	uint8_t i = 0;
	uint8_t *ptr;
	uint8_t buf[8];
	i15765_t frame;

	/* copy over message */
	frame = mftb->msg;
	frame.buf = buf;
	ptr = mftb->buf;

	/* construct FF N_PDU and pack it into a CAN frame */
	frame.buf[i++] = (I15765_PDU_FF << 4) | (mftb->msg.buf_len >> 8);
	frame.buf[i++] = (uint8_t)mftb->msg.buf_len;

	/* copy over the data */
	while(i < 8)
		frame.buf[i++] = *ptr++;

	/* FF's are always 8 bytes long */
	frame.buf_len = 8;

	/* transmit the CAN frame, testing to see if it was sent correctly */
	if(i15765_tx(&frame) == 0)
	{

		mftb->state = STATE_WT_FC;
		mftb->timeout = TIMEOUT_FC_S;
		mftb->msg.buf = ptr;
	}

	return;
}



/*
** Transmit a consecutive frame.
** INPUT: mftb - pointer to the i15765 multiframe buffer
*/
void i15765_tx_cf(i15765_mft_t *mftb)
{

	uint8_t min;
	uint8_t i = 0;
	uint8_t *ptr;
	uint16_t rem;
	uint8_t buf[8];
	i15765_t frame;

	/* copy over message */
	frame = mftb->msg;
	frame.buf = buf;
	ptr = mftb->msg.buf;

	/* update separation time */
	if(mftb->st_cnt)
	{

		/* delay */
		mftb->st_cnt--;
		return;

	}
	else
	{

		/* transmit */
		mftb->st_cnt = mftb->st;
	}

	/* load PDU type */
	frame.buf[i++] = (I15765_PDU_CF << 4) | (mftb->sn++ & 0xf);

	/* how much data can we (or need we) copy over? */
	rem = mftb->msg.buf_len - (mftb->msg.buf - mftb->buf);
	min = (uint8_t)MIN(rem, 8 - i);

	/* add in the PDU field (and maybe AE) */
	min += i;

	/* copy  over the data */
	while(i < min)
		frame.buf[i++] = *ptr++;

	/* FF's are always 8 bytes long */
	frame.buf_len = min;

	/* transmit the CAN frame, testing to see if it was sent correctly */
	if(i15765_tx(&frame) == 0)
	{

		mftb->timeout = TIMEOUT_CF_S;
		mftb->msg.buf = ptr;

		/* if bs is zero, then transmit away, else check how many we have left */
		mftb->bs_cnt--;
		if(mftb->bs && (mftb->bs_cnt == 0))
		{
			//			mftb->state = STATE_WT_FC;//qiaoxu  Ϊ���ó����ķ��ʹ���8֡
		}
	}

	rem = mftb->msg.buf_len - (mftb->msg.buf - mftb->buf);
	if(rem == 0)
	{

		*mftb->status = I15765_SENT;
		mftb->status = &i15765_tmp;
		mftb->state = STATE_IDLE;
	}

	return;
}



/*
** Initialize and attempt transmit a multi-frame message.
** INPUT:  msg - pointer to the i15765 message to be transmitted
**         status - pointer to user RAM location for status feedback
**
*/
void i15765_tx_mf(i15765_t *msg, uint8_t *status)
{

	uint16_t i;
	i15765_mft_t *mftb;

	/* delete any mf buffers with matching address information */
//	i15765_mftb_del(msg);

	/* assume failure */
	*status = I15765_FAILED;

	/* find an available spot for this message */
	if(i15765_mftb_get(&mftb))
		return;

	/* there was room, so update status */
	*status = I15765_SENDING;

	/* copy over the data */
	for(i = 0; i < msg->buf_len; i++)
		mftb->buf[i] = msg->buf[i];

	/* copy over message */
	mftb->msg = *msg;
	mftb->msg.buf = mftb->buf;

	/* set state to wait for a flow control PDU */
	mftb->state = STATE_TX_FF;
	mftb->sn = 1;

	mftb->status = status;

	/* transmit the CAN frame, testing to see if it was sent correctly */
	mftb->timeout = TIMEOUT_TX_S;
	i15765_tx_ff(mftb);
}



/*
** Transmit a flow control frame and updates the status of mfrb. Reception
** will be paused until the FC has been transmitted.
** INPUT: mfrb - pointer multiframe receive buffer
*/
void i15765_tx_fc(i15765_mfr_t *mfrb)
{

	uint8_t fs;
	i15765_t msg;
	uint8_t buf[8];

	/* get the FF_DL and check for potential buffer overflow */
	if(mfrb->msg.buf_len > I15765CFG_MF_RX_BUF_SIZE)
	{

		fs = I15765_FS_OVFLW;
		mfrb->state = STATE_TX_OVFLW;

	}
	else
	{

		fs = I15765_FS_CTS;		//
//		fs = I15765_FS_WT;		//wait
		mfrb->state = STATE_TX_CTS;
	}

	/* pack the pci */
	msg.buf = buf;
	msg.buf[0] = (I15765_PDU_FC << 4) | fs;
	msg.buf[1] = BS;	//qiaoxu modify
	msg.buf[2] = STmin; //qiaoxu modify
	msg.buf_len = 3;

	msg.sa = i15765_sa;
	msg.ta = mfrb->msg.sa;
	msg.tat = mfrb->msg.tat;

	/* set pri same as received FF */
	msg.pri = mfrb->msg.pri;

	/* send the message */
	if(i15765_tx(&msg) == 0)
	{
		/* transmission successful */
		mfrb->timeout = TIMEOUT_CF_R;
		mfrb->state = (mfrb->state == STATE_TX_CTS) ? STATE_WT_CF : STATE_IDLE;
	}
}

/*
** Handles a recieved single frame
** INPUT: msg - pointer to the received message
*/
void i15765_rx_sf(i15765_t *msg)
{

	/* ignore message if errors are present */
	if((msg->buf[0] == 0) || (msg->buf[0] > (msg->buf_len - 1)))
	{
		return;
	}

	/* message length */
	msg->buf_len = msg->buf[0];

	/* skip PCI field */
	msg->buf++;

	i15765app_process(msg);
}

/*
** Handles a recieved first frame.
** INPUT: msg - pointer to the received message segment
*/
void i15765_rx_ff(i15765_t *msg)
{

	uint8_t i;
	uint16_t ff_dl;
	i15765_mfr_t *mfrb;

	//qiaoxu   ɾ��
	/* delete any mf buffers with matching address information */
	i15765_mfrb_del(msg);

	/* find an available spot for this message */
	if(i15765_mfrb_get(&mfrb))
		return;

	/* first frame data length */
	ff_dl = ((msg->buf[0] & 0xf) << 8) | msg->buf[1];

	/* if the total size could fit in an SF PDU, ignore */
	if(ff_dl <= 7)
		return;

	/* skip PCI field */
	msg->buf += 2;
	msg->buf_len -= 2;

	/* buffer the necessary info */
	mfrb->sn = 0;
	mfrb->msg = *msg;
	mfrb->msg.buf_len = ff_dl;

	/* reset buf pointer and store data that was recieved with the FF */
	mfrb->msg.buf = mfrb->buf;
	for(i = 0; i < msg->buf_len; i++)
		*mfrb->msg.buf++ = msg->buf[i];

	/* transmit a flow control frame (the status will be updated based on FS) */
	mfrb->timeout = TIMEOUT_TX_R;
	i15765_tx_fc(mfrb);
}



/*
** Handles a recieved consecutive frame
** INPUT: msg - pointer to the received message
*/
void i15765_rx_cf(i15765_t *msg)
{
	uint8_t i;
	uint8_t min;
	uint16_t rem;
	i15765_mfr_t *mfrb;

	/* find the matching index */	//qiaoxu
	if(i15765_mfrb_seek(msg, &mfrb))
		return;

	/* if the CF frame is not expected, ignore it */
	if(mfrb->state != STATE_WT_CF)
		return;

	/* increment the sequence number and compare */
	if((++mfrb->sn & 0xf) == (msg->buf[0] & 0xf))
	{
		/* skip PCI field */
		msg->buf++;
		msg->buf_len--;

		/* if we are at the end of the message, stop early */
		rem = (mfrb->buf + mfrb->msg.buf_len) - mfrb->msg.buf;

		min = (uint8_t) MIN(msg->buf_len, rem);

		/* add the data to the buffer */
		for(i = 0; i < min; i++)
			*mfrb->msg.buf++ = msg->buf[i];

		rem = (mfrb->buf + mfrb->msg.buf_len) - mfrb->msg.buf;

		/* if we stopped early, the reception is complete */
		if(rem == 0)
		{

			mfrb->msg.buf = mfrb->buf;
			i15765app_process(&mfrb->msg);
			mfrb->state = STATE_IDLE;

		}
		else
		{
			mfrb->timeout = TIMEOUT_CF_R;

		}
	}
	else
	{
		/* abort the reception */
		mfrb->state = STATE_IDLE;

	}
}


/*
** Handle a recieved flow control frame
** INPUT: msg - pointer to the received message
*/
void i15765_rx_fc(i15765_t *msg)
{

	i15765_mft_t *mftb;

	/* find matching multiframe transmit buffer */
	if(i15765_mftb_seek(msg, &mftb))//qiaoxu
		return;

	/* if the FC frame is not expected, ignore it */
	if(mftb->state != STATE_WT_FC)
		return;

	//qiaoxu add
	if(msg->buf_len<3)
	{
		return;
	}
	//////////////////////////////////////////////////////////////

	/* determine what type of FC this is and act appropriately */
	switch(msg->buf[0] & 0xf)
	{

		/* continue to send */
	case 0:
		mftb->state = STATE_TX_CF;
		mftb->bs = msg->buf[1];
		mftb->bs_cnt = mftb->bs;

		if(msg->buf[2] <= 0x7f)
			mftb->st = msg->buf[2];
		else if((msg->buf[2] >= 0xf1) && (msg->buf[2] <= 0xf9))
			mftb->st = 0x01;
		else
			mftb->st = 0x7f;

		/* convert to units of 100 microseconds (1ms) */
		mftb->st *= ((uint16_t)1);

		/* convert to ticks */
		mftb->st /= ((uint8_t)I15765CFG_TICK_PERIOD);

		mftb->st_cnt = mftb->st;
		mftb->timeout = TIMEOUT_CF_S;
		mftb->wt_cnt = 0;
		break;

		/* wait (NOTE - must be above "default" case) */
	case 1:
		mftb->state = STATE_WT_FC;
		mftb->timeout = TIMEOUT_FC_S;

		/* if too many waits, bail */
		if(++mftb->wt_cnt < 5)
			break;

		/* all others */
	default:
		mftb->state = STATE_IDLE;
		*mftb->status = I15765_FAILED;
		mftb->status = &i15765_tmp;
		break;
	}
}



/*
** Translates the CAN frame into an i15765 message
** INPUT: can - pointer to the received CAN frame
*/
void i15765_rx_post(can_t *can)
{
	i15765_t msg;
	uint8_t pdu_type;

	if(!(can->id & B31))
	{
		msg.buf = &can->buf[0];
		msg.buf_len = can->buf_len;
		msg.pri = 0;

		if(can->id == 0x7DF)
		{

			/* support incoming functional requests */
			msg.tat = I15765_TAT_NF11;
			//msg.sa = 0xf1;
			msg.sa = I15765CFG_SA;

		}
		else if(can->id == UDS_RX_ID)//
		{
			/* support incoming requests (0xf1 is external test equipment) */
			msg.tat = I15765_TAT_NP11;
			//msg.sa = 0xf1;
			msg.sa = I15765CFG_SA;	//
			msg.ta = (can->id - 0x7e0) + 1;
		}

	}

	/* SF, FF, CF, or FC? */
	pdu_type = msg.buf[0] >> 4;

	/* if this message is not for us or not functional, discard it */
	if((ADDR_FUNC(msg.tat) && pdu_type != I15765_PDU_SF))
	{
		return;
	}

	if(pdu_type <= 3)
	{
		switch(pdu_type)
		{
		case I15765_PDU_SF:
			i15765_rx_sf(&msg);
			break;
		case I15765_PDU_FF:
			i15765_rx_ff(&msg);
			break;
		case I15765_PDU_CF:
			i15765_rx_cf(&msg);
			break;
		case I15765_PDU_FC:
			i15765_rx_fc(&msg);
			break;
		}
	}
}

/*
** Requests a message to be transmitted, either single or multi-frame
** INPUT: msg - the message to transmit
**        status - pointer to user RAM location for status feedback
*/
void i15765_tx_app(i15765_t *msg, uint8_t *status)
{

	/* status always has to point somewhere */
	if(status == 0)
		status = &i15765_tmp;

	/* load in source address into out going message */
	msg->sa = i15765_sa;

	/* if its small enough to send in a SF, pack and transmit */
	if(msg->buf_len <= 7)
	{

		*status = (i15765_tx_sf(msg) == 0) ? I15765_SENT : I15765_FAILED;

	}
	else if(msg->buf_len <= I15765CFG_MF_TX_BUF_SIZE)
	{

		/* multiframe - only support physical addressing */
		i15765_tx_mf(msg, status);

	}
	else
	{

		/* too big or no available buffers */
		*status = I15765_FAILED;
	}
}



/*
** Cycle through our active messages and attempt to continue
** transmitting.  Also, check for timeouts as we go.
*/
void i15765_tx_update(void)
{

	uint8_t i;

	/* for each currently transmitting message, continue */
	for(i = 0; i < I15765CFG_MF_TX_BUF_NUM; i++)
	{

		/* if we are waiting on a transmission to complete, try it again */
		switch(i15765_mftb[i].state)
		{

		case STATE_TX_FF:
			i15765_tx_ff(&i15765_mftb[i]);
			break;
		case STATE_TX_CF:
			i15765_tx_cf(&i15765_mftb[i]);
			break;
		}

		/* update timeout */
		if(i15765_mftb[i].timeout)
		{

			i15765_mftb[i].timeout--;

		}
		else
		{

			i15765_mftb[i].state = STATE_IDLE;
			*i15765_mftb[i].status = I15765_FAILED;
			i15765_mftb[i].status = &i15765_tmp;
//			i15765_mftb[i].status = 0;

		}
	}
}

/*
** Cycle through our active messages and attempt to continue
** reception checking for timeouts as we go.
*/
void i15765_rx_update(void)
{

	can_t can;
	uint8_t i;

	/* read all the CAN frames and pass them up */
	while(can_rx(0, &can) == 0)
		i15765_rx_post(&can);

	/* for each active message, check for expiration and cancel if needed */
	for(i = 0; i < I15765CFG_MF_RX_BUF_NUM; i++)
	{

		/* if we are waiting to send an FC frame, try it again */
		if((i15765_mfrb[i].state == STATE_TX_CTS)
		        || (i15765_mfrb[i].state == STATE_TX_OVFLW))
			i15765_tx_fc(&i15765_mfrb[i]);

		/* update timeout */
		if(i15765_mfrb[i].timeout)
			i15765_mfrb[i].timeout--;
		else
			i15765_mfrb[i].state = STATE_IDLE;
	}
}

/*
** This function is the time base for the entire i15765 module.
*/
void i15765_update(void)
{
	i15765_rx_update();
	i15765_tx_update();
}