c

#include <freertos/FreeRTOS.h>
#include <freertos/queue.h>
#include <freertos/event_groups.h>
#include <driver/can.h>
#include <nvs_flash.h>
#include <esp_task_wdt.h>
#include <stdio.h>
#include <string.h>
#include <sys/time.h>
#include "can.h"
#include "hci.h"

const int can_tx_pin = GPIO_NUM_0;
const int can_rx_pin = GPIO_NUM_2;

struct
{
	can_timing_config_t timing;
	can_filter_config_t filter;
	can_message_t periodic_messages[8];
	uint8_t flags; /* initialized, rx */
} can_config;

can_general_config_t config =
{
	.mode = CAN_MODE_NORMAL,
	.tx_io = can_tx_pin,
	.rx_io = can_rx_pin,
	.clkout_io = -1,
	.bus_off_io = -1,
	.tx_queue_len = 10,
	.rx_queue_len = 10,
	.alerts_enabled = CAN_ALERT_NONE,
	.clkout_divider = 0
};

const uint8_t CAN_FLAG_INIT = 1 << 0;
const uint8_t CAN_FLAG_RX_ON = 1 << 1;
static QueueHandle_t can_rx_queue;
static EventBits_t can_reinstall_done = 1 << 0;
static EventGroupHandle_t can_event_group;
struct can_rx_event
{
	uint8_t event;
};

enum
{
	EVENT_CAN_RX_OFF = 0,
	EVENT_CAN_RX_ON = 1,
	EVENT_CAN_REINSTALL = 2
};


int can_init()
{
	esp_err_t err;
	/* Setup default timing */
	can_config.timing.brp = 8;
	can_config.timing.tseg_1 = 5;
	can_config.timing.tseg_2 = 4;
	can_config.timing.sjw = 1;
	/* Setup default filter */
	can_config.filter.acceptance_code = 0;
	can_config.filter.acceptance_mask = 0xffffffff;
	can_config.filter.single_filter = 1;
	err = can_driver_install(&config, &can_config.timing, &can_config.filter);

	if(err != ESP_OK)
	{
		printf("ERR CAN init failed!\n");
		return -1;
	}

	err = can_start();
	if(err != ESP_OK)
	{
		printf("ERR CAN init failed!\n");
		return -1;
	}

	can_event_group = xEventGroupCreate();
	can_config.flags |= CAN_FLAG_INIT;
	return 0;
}

int can_reinstall()
{
	esp_err_t err;
	can_stop();
	can_driver_uninstall();
	/* Setup default timing */
	err = can_driver_install(&config, &can_config.timing, &can_config.filter);
	if(err != ESP_OK)
		return -1;
	err = can_start();
	if(err != ESP_OK)
		return -1;
	return 0;
}

int parse_message_format(can_message_t *msg, const char *fmt)
{
	uint32_t id;
	char remote;
	memset(msg, 0, sizeof(*msg));
	sscanf(fmt, "%x#%c", &id, &remote);
	msg->flags |= CAN_MSG_FLAG_SS;
	if(remote == 'R')
	{
		msg->flags |= CAN_MSG_FLAG_RTR;
		msg->identifier = id;
		msg->data_length_code = 0;
	}
	else
	{
		int data[8];
		int ret = sscanf(fmt, "%x#%2x%2x%2x%2x%2x%2x%2x%2x",
			&id, &data[0], &data[1], &data[2], &data[3],
			&data[4], &data[5], &data[6], &data[7]);
		if(ret < 1)
			return -1;
		int len = ret - 1;
		msg->identifier = id;
		msg->data_length_code = len;
		for(int i = 0; i < len; i++)
			msg->data[i] = data[i];
	}

	return 0;
}

void can_command()
{
	char *cmd = strtok(NULL, " ");
	esp_err_t err;
	/* Make sure we have a command */
	if(!cmd)
		goto einval;
	/*
	 * Compare commande against known commands and handle them, otherwise tell
	 * the user it was invalid.
	 */
	if(strcmp(cmd, "help") == 0)
	{
		printf("OK\n");
		printf(
			"Available commands:\n"
			"Source clock: 80 MHz\n"
			"CAN bitrate = 80 000 000 / brp / (1 + tseg_1 + tseg_2)\n"
			"\n"
			"can help - write this text\n"
			"can rx on/off - enable or disable RX\n"
			"can send <id>#<data in hex> - e.g. send 13f#02e8\n"
			"can config brp [value] - get or set current can brp (2-128, even)\n"
			"can config tseg_1 [value] - get or set current can tseg_1 (1-16)\n"
			"can config tseg_2 [value] - get or set current can tseg_2 (1-8)\n"
			"can config sjw [value] - get or set current can sjw (1-4)\n"
			"\n");
	}
	else if(strcmp(cmd, "rx") == 0)
	{
		const char *arg = strtok(NULL, " ");

		if(strcmp(arg, "on") == 0)
			can_rx_on();
		else if(strcmp(arg, "off") == 0)
			can_rx_off();
		else
			goto einval;
	}
	else if(strcmp(cmd, "send") == 0)
	{
		can_message_t msg;
		const char *msg_str = strtok(NULL, " ");

		memset(&msg, 0, sizeof(msg));
		if(parse_message_format(&msg, msg_str) < 0)
			goto einval;
		struct timeval current_time;
		gettimeofday(&current_time, NULL);
		/* printf("seconds : %ld\nmicro seconds : %ld", current_time.tv_sec, current_time.tv_usec); */
		/* unsigned long currentMillis = xTaskGetTickCount(); */
		unsigned long time_in_micros = 1000000 * current_time.tv_sec + current_time.tv_usec;
		printf("T: %ld SENT %x#", time_in_micros, msg.identifier);
		for(int i = 0; i < msg.data_length_code; i++)
			printf("%02x", msg.data[i]);
		printf("\n");
		err = can_transmit(&msg, 0);
		if(err != ESP_OK)
		{
			printf("ERR Transmit failed!\n");
			/*
			 * If we get to many transmission errors due to non-acked frames
			 * then the bus will go into an error state, restart it by
			 * reinstalling it.
			 */
			can_send_reinstall();
			return;
		}

	}
		else if(strcmp(cmd, "sendex") == 0)
	{
		can_message_t msg;
		const char *msg_str = strtok(NULL, " ");

		memset(&msg, 0, sizeof(msg));
		if(parse_message_format(&msg, msg_str) < 0)
			goto einval;
		msg.extd = 1;
		struct timeval current_time;
		gettimeofday(&current_time, NULL);
		/* printf("seconds : %ld\nmicro seconds : %ld", current_time.tv_sec, current_time.tv_usec); */
		/* unsigned long currentMillis = xTaskGetTickCount(); */
		unsigned long time_in_micros = 1000000 * current_time.tv_sec + current_time.tv_usec;
		printf("T: %ld SENT %x#", time_in_micros, msg.identifier);
		for(int i = 0; i < msg.data_length_code; i++)
			printf("%02x", msg.data[i]);
		printf("\n");
		err = can_transmit(&msg, 0);
		if(err != ESP_OK)
		{
			printf("ERR Transmit failed!\n");

			/*
			 * If we get to many transmission errors due to non-acked frames
			 * then the bus will go into an error state, restart it by
			 * reinstalling it.
			 */
			can_send_reinstall();

			return;
		}

	}
	else if(strcmp(cmd, "config") == 0)
	{
		/* Read value argument */
		const char *arg = strtok(NULL, " ");
		if(!arg)
			goto einval;
		if(strcmp(arg, "brp") == 0)
		{
			const char *value_str = strtok(NULL, " ");
			if(!value_str)
				printf("OK %d\n", can_config.timing.brp);

			else
			{
				int value = atoi(value_str);

				if(value < 2 || value > 127 || value % 2)
					goto einval;
				can_config.timing.brp = value;
				can_send_reinstall();
			}
		}
		else if(strcmp(arg, "tseg_1") == 0)
		{
			const char *value_str = strtok(NULL, " ");
			if(!value_str)
				printf("OK %d\n", can_config.timing.tseg_1);
			else
			{
				int value = atoi(value_str);
				if(value < 1 || value > 16 )
					goto einval;
				can_config.timing.tseg_1 = value;
				can_send_reinstall();
			}
		}
		else if(strcmp(arg, "tseg_2") == 0)
		{
			const char *value_str = strtok(NULL, " ");
			if(!value_str)
				printf("OK %d\n", can_config.timing.tseg_2);
			else
			{
				int value = atoi(value_str);
				if(value < 1 || value > 8 )
					goto einval;
				can_config.timing.tseg_2 = value;
				can_send_reinstall();
			}
		}
		else if(strcmp(arg, "sjw") == 0)
		{
			const char *value_str = strtok(NULL, " ");
			if(!value_str)
				printf("OK %d\n", can_config.timing.sjw);
			else
			{
				int value = atoi(value_str);
				if(value < 1 || value > 4 )
					goto einval;
				can_config.timing.sjw = value;
				can_send_reinstall();
			}
		}
		else
			goto einval;
	}
	else if(strcmp(cmd, "status") == 0)
	{
	}
	else
		goto einval;
	return;

einval:
	printf("INVALID COMMAND");
	return;
}

void can_rx_off()
{
	struct can_rx_event event =
	{
		.event = EVENT_CAN_RX_OFF
	};
	xQueueSendToBack(can_rx_queue, &event, 0);
}

void can_rx_on()
{
	struct can_rx_event event =
	{
		.event = EVENT_CAN_RX_ON
	};
	xQueueSendToBack(can_rx_queue, &event, 0);
}

void can_send_reinstall()
{
	struct can_rx_event event =
	{
		.event = EVENT_CAN_REINSTALL
	};
	xQueueSendToBack(can_rx_queue, &event, 0);
	/* Wait for reinstallation done */
	xEventGroupWaitBits(can_event_group, can_reinstall_done, 1, 1, 1000 * portTICK_PERIOD_MS);
}

void can_rx_thread(void *parameters)
{
	/* Check that CAN initialized correctly */
	while(!(can_config.flags & CAN_FLAG_INIT))
		vTaskDelay(100 * portTICK_PERIOD_MS);
	esp_task_wdt_delete(xTaskGetCurrentTaskHandle());
	can_rx_queue = xQueueCreate(10, sizeof(struct can_rx_event));
	int rx_running = 0;

	while(1)
	{
		can_message_t msg;
		while(can_receive(&msg, 10) == ESP_OK)
		{
			/* AUTOPING */
			/* can_transmit(&msg, 0); */
			/* AUTOPING */
			if(rx_running)
			{

				struct timeval tvr;
				gettimeofday(&tvr, NULL);
				/* unsigned long currentMillis = xTaskGetTickCount(); */
				unsigned long time_in_micros = 1000000 * tvr.tv_sec + tvr.tv_usec;
				printf("T: %ld CAN RX: %x#", time_in_micros, msg.identifier);
				if(msg.flags & CAN_MSG_FLAG_RTR)
					printf("R");
				else
					for(int i = 0; i < msg.data_length_code; i++)
						printf("%02x", msg.data[i]);
				/*
				if (msg.extd)
    				printf("Message is in Extended Format\n");
				else
   					printf("Message is in Standard Format\n");
				*/

				printf("\n");
			}
		}

		struct can_rx_event event;
		if(xQueueReceive(can_rx_queue, &event, 0))
		{
			if(event.event == EVENT_CAN_RX_OFF)
			{
				rx_running = 0;
				printf("OK\n");
			}

			else if(event.event == EVENT_CAN_RX_ON)
			{
				rx_running = 1;
				printf("OK\n");
			}

			else if(event.event == EVENT_CAN_REINSTALL)
			{
				if(can_reinstall() < 0)
					printf("ERR Unknown error\n");
				else
					printf("OK\n");
				/* Also perform a full recovery which helps on bus-off recovery */
				can_initiate_recovery();

				while(1)
				{
					twai_status_info_t info;
					twai_get_status_info(&info);
					if(info.state != CAN_STATE_RECOVERING)
						break;
				}

				xEventGroupSetBits(can_event_group, can_reinstall_done);
			}
		}
	}
}