#include "canJ1939.h"

void printTxCanFrame(const char* can_name, struct can_frame *frame)
{
	int i;
	int len=0;
	char buff[128];

	len += sprintf(&buff[len], "%s.send.%08x, len:%d, ", can_name, frame->can_id, frame->can_dlc);
	for (i=0;i<frame->can_dlc;i++) { len += sprintf(&buff[len], "%02x ", frame->data[i]); }
	len += sprintf(&buff[len],"\n");
	printv(buff);
}

void printRxCanFrame(const char* can_name, struct can_frame *frame)
{
	int i;
	int len=0;
	char buff[128];
	
	len += sprintf(&buff[len], "%s.recv.%08x, len:%d, ", can_name, frame->can_id, frame->can_dlc);
	for (i=0;i<frame->can_dlc;i++) { len += sprintf(&buff[len], "%02x ", frame->data[i]); }
	len += sprintf(&buff[len],"\n");
	printv(buff);
}

int can_J1939_init(can_j1939_t* dev, const char* can_name, int bitrate)
{
	int ret;
	{
		char cmdline[128] = { '\0' };
		sprintf(cmdline, "ip link set %s down\nip link set %s up type can bitrate %d\n", can_name, can_name, bitrate);
		int ret = system(cmdline);
		if (ret < 0) { printe("ret=%d\n",ret); return WEXITSTATUS(ret);}
		sleep(1);
	}
	if (dev)
	{
		can_J1939_init_TPCM(&dev->rxtpcm, 0xFF, 0x00, 0x00, NULL, 0);
		can_J1939_init_TPCM(&dev->txtpcm, 0xFF, 0x00, 0x00, NULL, 0);
		dev->rxtp = &dev->rxtpcm;
		dev->txtp = &dev->txtpcm;

		dev->can_name = can_name;
		dev->fd = socket(PF_CAN, SOCK_RAW, CAN_RAW);
		if (dev->fd > 0) {
			struct ifreq ifr;
			memset(&ifr, 0, sizeof(ifr));
			strcpy(ifr.ifr_name, can_name);
			/* Determine the interface index */
			ioctl(dev->fd, SIOCGIFINDEX, &ifr);

			struct sockaddr_can can_addr;
			can_addr.can_family = AF_CAN;
			can_addr.can_ifindex = ifr.ifr_ifindex;
			/* bin the socket to a CAN interface */
			ret = bind(dev->fd, (struct sockaddr *)&can_addr, sizeof(can_addr));
			if (ret!=0) printe("ret=%d\n",ret);

			int flags = fcntl(dev->fd,F_GETFL,0);
			ret = fcntl(dev->fd,F_SETFL, O_NONBLOCK|flags);
			if (ret!=0) printe("ret=%d\n",ret);

			/* Set the lookback rules */
			int loopback = 0;
			ret = setsockopt(dev->fd, SOL_CAN_RAW, CAN_RAW_LOOPBACK, &loopback, sizeof(loopback));
			if (ret!=0) printe("ret=%d\n",ret);

			/* Set the filter rules */  
			// ret = setsockopt(dev->fd, SOL_CAN_RAW, CAN_RAW_FILTER, NULL, 0);
			// if (ret!=0) printe("ret=%d\n",ret);

			return dev->fd;
		}
		printe("socket(ret=%d)\n", dev->fd);
		return -2;
	}
	return -1;
}

void can_J1939_exit(can_j1939_t* dev)
{
	return;
	if (dev)
	{
		char cmdline[128] = { '\0' };
		sprintf(cmdline, "ip link set %s down", dev->can_name);
		int ret = system(cmdline);
		if (ret < 0) { printe("ret=%d\n",ret); }
		sleep(3);
	}
}


int	can_J1939_read(can_j1939_t* dev, struct can_frame *frame, int timeout)
{
	if(timeout < 0) timeout = 0;
	if (dev && dev->fd > 0)
	{
		fd_set readfds;
		FD_ZERO(&readfds);
		FD_SET(dev->fd, &readfds);
		struct timeval tv;
		tv.tv_sec = timeout/1000;
		tv.tv_usec = (timeout%1000)*1000;
		if ((timeout = select(dev->fd+1, &readfds, NULL, NULL, &tv)) >= 0)
		{
			if (timeout == 0)
			{
				return timeout;
			}
			if(FD_ISSET(dev->fd, &readfds))
			{
				int ret = read(dev->fd, frame, sizeof(struct can_frame));
				// if (ret) { printRxCanFrame(dev->can_name, frame); }
				return ret;
			}
			printe("FD_ISSET(fd=%d) timeout.\n", dev->fd);
			return -3;
		}
		printe("select(fd=%d) error.\n", dev->fd);
		return -2;
	}
	return -1;
}

int	can_J1939_send(can_j1939_t* dev, struct can_frame *frame)
{
	if (dev && dev->fd > 0)
	{
		// printTxCanFrame(dev->can_name, frame);
		dev->txframe_ms = systime_ms();
		int ret = write(dev->fd, frame, sizeof(struct can_frame));
		if (ret == sizeof(struct can_frame))
		{
			return 0;
		}
	}
	return -1;
}

void can_J1939_init_TPCM(tpcm_t* tp, uint32_t PGN, uint8_t DA, uint8_t SA, uint8_t* data, int len)
{
	if (tp)
	{
		tp->DA = DA;
		tp->SA = SA;
		tp->PGN = PGN;
		tp->buff = data;
		tp->bytes = len;
		tp->tpkgs = (len + 6)/7;
		tp->rpkgs = (len + 6)/7;
		tp->index = 0;
	}
}

int can_J1939_send_TPCM_RTS(can_j1939_t* dev, tpcm_t* tp)
{
	struct can_frame frame;
	memset(&frame, 0, sizeof(struct can_frame));
	frame.can_id = (0x7<<26)|(0xE8<<16)|(tp->DA<<8)|tp->SA;
	frame.can_id |= CAN_EFF_FLAG;
	frame.data[0] = 0x10;
	frame.data[1] = (tp->bytes>>0)&0xFF;
	frame.data[2] = (tp->bytes>>8)&0xFF;
	frame.data[3] = tp->tpkgs;
	frame.data[4] = 0xFF;
	frame.data[5] = (tp->PGN>>0)&0xFF;
	frame.data[6] = (tp->PGN>>8)&0xFF;
	frame.data[7] = (tp->PGN>>16)&0xFF;
	frame.can_dlc = 8;
	
	return can_J1939_send(dev, &frame);
}

int can_J1939_send_TPCM_CTS(can_j1939_t* dev, tpcm_t* tp, uint8_t pkgsn)
{
	struct can_frame frame;
	memset(&frame, 0, sizeof(struct can_frame));
	frame.can_id = (0x7<<26)|(0xEC<<16)|(tp->DA<<8)|tp->SA;
	frame.can_id |= CAN_EFF_FLAG;
	frame.data[0] = 0x11;
	frame.data[1] = tp->rpkgs;
	frame.data[2] = pkgsn;
	frame.data[3] = 0xFF;
	frame.data[4] = 0xFF;
	frame.data[5] = (tp->PGN>>0)&0xFF;
	frame.data[6] = (tp->PGN>>8)&0xFF;
	frame.data[7] = (tp->PGN>>16)&0xFF;
	frame.can_dlc = 8;
	
	return can_J1939_send(dev, &frame);
}

int can_J1939_send_TPCM_EOMACK(can_j1939_t* dev, tpcm_t* tp)
{
	struct can_frame frame;
	memset(&frame, 0, sizeof(struct can_frame));
	frame.can_id = (0x7<<26)|(0xEC<<16)|(tp->DA<<8)|tp->SA;
	frame.can_id |= CAN_EFF_FLAG;
	frame.data[0] = 0x13;
	frame.data[1] = (tp->bytes>>0)&0xFF;
	frame.data[2] = (tp->bytes>>8)&0xFF;
	frame.data[3] = tp->rpkgs;
	frame.data[4] = 0xFF;
	frame.data[5] = (tp->PGN>>0)&0xFF;
	frame.data[6] = (tp->PGN>>8)&0xFF;
	frame.data[7] = (tp->PGN>>16)&0xFF;
	frame.can_dlc = 8;
	
	return can_J1939_send(dev, &frame);
}

int can_J1939_send_TPCM_ABORT(can_j1939_t* dev, tpcm_t* tp)
{
	struct can_frame frame;
	memset(&frame, 0, sizeof(struct can_frame));
	frame.can_id = (0x7<<26)|(0xE8<<16)|(tp->DA<<8)|tp->SA;
	frame.data[0] = 0xFF;
	frame.data[1] = 0xFF;
	frame.data[2] = 0xFF;
	frame.data[3] = 0xFF;
	frame.data[4] = 0xFF;
	frame.data[5] = (tp->PGN>>0)&0xFF;
	frame.data[6] = (tp->PGN>>8)&0xFF;
	frame.data[7] = (tp->PGN>>16)&0xFF;
	frame.can_dlc = 8;
	frame.can_id |= CAN_EFF_FLAG;
	return can_J1939_send(dev, &frame);
}

int can_J1939_send_TPCM_BAM(can_j1939_t* dev, tpcm_t* tp)
{
	struct can_frame frame;
	memset(&frame, 0, sizeof(struct can_frame));
	frame.can_id = (0x7<<26)|(0xE8<<16)|(tp->DA<<8)|tp->SA;
	frame.can_id |= CAN_EFF_FLAG;
	frame.data[0] = 0x20;
	frame.data[1] = (tp->bytes>>0)&0xFF;
	frame.data[2] = (tp->bytes>>8)&0xFF;
	frame.data[3] = tp->tpkgs;
	frame.data[4] = 0xFF;
	frame.data[5] = (tp->PGN>>0)&0xFF;
	frame.data[6] = (tp->PGN>>8)&0xFF;
	frame.data[7] = (tp->PGN>>16)&0xFF;
	frame.can_dlc = 8;
	
	return can_J1939_send(dev, &frame);
}

int can_J1939_send_TPDT(can_j1939_t* dev, tpcm_t* tp, uint8_t pkgsn)
{
	struct can_frame frame;
	memset(&frame, 0, sizeof(struct can_frame));
	frame.can_id = (0x7<<26)|(0xEB<<16)|(tp->DA<<8)|tp->SA;
	frame.can_id |= CAN_EFF_FLAG;
	frame.data[0] = pkgsn;
	tp->index = (pkgsn-1)*7;
	int i;
	for (i = 0; tp->index<tp->bytes && i<7; i++)
	{
		frame.data[i] = tp->buff[tp->index++]; 
	}
	frame.can_dlc = i+1;
	
	return can_J1939_send(dev, &frame);
}
