#include "com_CAN.h"

int com_CAN_init(com_can_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)
	{
		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);
			/* Set the lookback rules */

			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);

			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 com_CAN_exit(com_can_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 com_CAN_filter(com_can_t* dev, uint32_t canMask, int canID)
{
	if (dev)
	{
		struct can_filter rfilter[1];
		rfilter[0].can_id = canID; 
		rfilter[0].can_mask = canMask;//CAN_SFF_MASK;
		return setsockopt(dev->fd, SOL_CAN_RAW, CAN_RAW_FILTER, &rfilter, sizeof(rfilter)); 
	}
	return -1;
}

int	com_CAN_read(com_can_t* dev, struct can_frame *frame, int timeout)
{
	if(timeout < 0) timeout = 0;
	if (dev)
	{
		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 (select(dev->fd+1, &readfds, NULL, NULL, &tv) >= 0)
		{
			if(FD_ISSET(dev->fd, &readfds))
			{
				int ret = read(dev->fd, frame, sizeof(struct can_frame));
				printv("%s.recv.%08x, len:%d, %02x %02x %02x %02x %02x %02x %02x %02x\n", 
					dev->can_name, frame->can_id, frame->can_dlc, 
					frame->data[0], frame->data[1], frame->data[2], frame->data[3], 
					frame->data[4], frame->data[5], frame->data[6], frame->data[7]);
				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	com_CAN_send(com_can_t* dev, struct can_frame *frame)
{
	if (dev)
	{
		printv("%s.send.%08x, len:%d, %02x %02x %02x %02x %02x %02x %02x %02x\n", 
			dev->can_name, frame->can_id, frame->can_dlc, 
			frame->data[0], frame->data[1], frame->data[2], frame->data[3], 
			frame->data[4], frame->data[5], frame->data[6], frame->data[7]);
		return write(dev->fd, frame, sizeof(struct can_frame));
	}
	return -1;
}

int com_CAN_send_PDU1PGN(com_can_t* dev, uint32_t PGN, uint8_t SA, uint8_t* data, int len)
{
	int i;
	if (len>8) len=8;
	struct can_frame frame;
	memset(&frame, 0, sizeof(struct can_frame));
	frame.can_id = ((PGN & 0x0000FF00)<<8)|SA;
	frame.can_dlc = len;
	for (i=0; i<len; i++)
	{
		frame.data[i] = data[i];
	}
	return com_CAN_send(dev, &frame);
}

int com_CAN_send_PDU1(com_can_t* dev, uint8_t RDP, uint8_t PF, uint8_t PS, uint8_t SA, uint8_t* data, int len)
{
	int i;
	if (len>8) len=8;
	struct can_frame frame;
	memset(&frame, 0, sizeof(struct can_frame));
	frame.can_id = ((RDP & 0x3)<<24)|(PF<<16)|(PS<<8)|SA;
	frame.can_dlc = len;
	frame.can_id |= CAN_EFF_FLAG;
	for (i=0; i<len; i++)
	{
		frame.data[i] = data[i];
	}
	return com_CAN_send(dev, &frame);
}



