MC100/Applications/prot/can_message.c

#include "bsp/bsp.h"
#include "os/co_task.h"
#include "libs/logger.h"
#include "libs/utils.h"
#include "can_message.h"
#include "wait_queue.h"

#define MAX_CAN_MESSAGE 4
static can_message_t messages[MAX_CAN_MESSAGE];
static msg_list_t msg_list[10];
static wait_queue_t wait_queue;


static node_list_t _pending_list = LIST_HEAD_INIT(_pending_list);
static node_list_t _idle_list = LIST_HEAD_INIT(_idle_list);

static void can_process_message(can_message_t *message);
static void free_can_message(can_message_t *message);
static void _can_poll_task(void*);
static void handle_can_frame(void);

void can_message_init(void){
	for(int i = 0; i < ARRAY_SIZE(msg_list); i++) {
		init_list_node(&msg_list[i].list);
		list_add_tail(&_idle_list, &msg_list[i].list);
	}
	shark_can0_init();
	co_task_create(_can_poll_task, NULL, 256);
}

void put_raw_message(can_id_t id, uint8_t *data, int len) {
	if (list_empty(&_idle_list)){
		return;
	}
	msg_list_t *raw = (msg_list_t *)_idle_list.next;
	list_del(&raw->list);
	raw->msg.id = id;
	raw->msg.len = len;
	memcpy(raw->msg.data, data, len);
	list_add_tail(&_pending_list, &raw->list);
}

static msg_list_t *_get_raw_message(void) {
	if (list_empty(&_pending_list)){
		return NULL;
	}
	msg_list_t *raw = (msg_list_t *)_pending_list.next;
	list_del(&raw->list);
	return raw;
}
static void free_raw_message(msg_list_t *raw) {
	list_add_tail(&_idle_list, &raw->list);
}

static void _can_poll_task(void *args) {
	while(1) {
		handle_can_frame();
		co_task_yield();
	}
}


static can_message_t *get_message_by_id(can_id_t id){
	can_message_t *idle_msg = NULL;
	can_message_t *src_msg = NULL;
	for (int i = 0; i < MAX_CAN_MESSAGE; i++){
		/*first found the same src&dest */
		if ((messages[i].src == id.src) && (messages[i].dest == id.dest)){
			return (messages + i);
		}
		if (messages[i].src == id.src){
			src_msg = messages + i;
		}else if (messages[i].data == NULL){
			idle_msg = messages + i;
		}
	}
	return ((src_msg!=NULL)?src_msg:idle_msg);
}


s32 can_send_message(uint32_t can_id, u8 *data, int len, s32 timeout){
	can_id_t id;
	id.id = can_id;
	if (id.type != ptype_request || id.retry <= 1) {
		return shark_can0_send_message(can_id, data, len) == 0?CAN_SEND_SUCCESS:CAN_SEND_ERROR;
	}
	s32 s_ret = CAN_SEND_NO_WAIT_QUEUE;
	s32 index = wait_queue_add(&wait_queue, decoder_can_key(data));
	if (index >= 0){
		s_ret = CAN_SEND_TIMEOUT;
		s32 retry = id.retry;
		while(retry -- > 0) {
			s_ret = shark_can0_send_message(can_id, data, len) == 0?CAN_SEND_SUCCESS:CAN_SEND_ERROR;
			if (s_ret != CAN_SEND_SUCCESS){
				continue;
			}
			s_ret = wait_queue_wait_key(&wait_queue, index, timeout)?CAN_SEND_SUCCESS:CAN_SEND_TIMEOUT;
			if (s_ret == CAN_SEND_SUCCESS){
				break;
			}
		}
		wait_queue_delete_key(&wait_queue, index);
	}
	return s_ret;

}

static void handle_can_frame(void){
	msg_list_t *raw = _get_raw_message();
	if (raw == NULL) {
		return;
	}
	can_id_t id = raw->msg.id;
	uint8_t *data = raw->msg.data;
	int len = raw->msg.len;
	
	can_message_t *message = get_message_by_id(id);
	if (message == NULL) {
		return ;
	}
	uint16_t key = 0;

	int total = id.total?id.total:32;
	int idx = id.idx?id.idx:32;

	if((total <= CAN_MESSAGE_MAX_FRAMES) && (idx <= CAN_MESSAGE_MAX_FRAMES)
			&& (idx <= total)){
		if ((idx == 1) && (len >= 2)) { //first frame for message
			key = decoder_can_key(data);

			message->key = key;
			message->dest = id.dest;
			message->src = id.src;
			message->len = 0;
			message->idx = idx;
			message->type= id.type;
			message->total_frame = total;
			len = len - 2; //skip key
			data = data + 2;
		}
		if (message->data){
			if ((message->idx == idx) && (message->total_frame == total)){
				memcpy(message->data + message->len, data, len);
				message->len += len;
				if (idx == total) { //last frame
					if (id.type == ptype_response) {
						wait_queue_key_acked(&wait_queue, message->key);
					}
					can_process_message(message);
				}
				message->idx = (idx + 1);
			}else {
				free_can_message(message);
			}
		}
	}
	free_raw_message(raw);
}


static void can_process_message(can_message_t *message){
	if (message->key & 0xFF >= 0xF0) {
		//do iap update
	}else if (message->src == 0x42){ //只处理后控的指令

	}
	free_can_message(message);
}
static void free_can_message(can_message_t *message){
	message->src = 0;
	message->len = 0;
	message->idx = 0xFF;
	message->total_frame = 0xFF;
}


void can_send_response(uint8_t can_add, uint8_t *data, int len){
	can_send_message(get_reponse_can_id(can_add), data, len, 50);
}

void can_send_message_ack(can_message_t *msg, uint8_t success){
	u8 response[3];
	encoder_can_key(response, msg->key);
	response[2] = success;
	can_send_message(get_reponse_can_id(msg->src), response, 3, 50);
}


void can_send_indicator(uint8_t can_add, uint8_t *data, int len){
	can_send_message(get_indicator_can_id(can_add), data, len, 50);
}

void can_send_request(uint8_t can_add, uint8_t *data, int len){
	can_send_message(get_request_can_id(can_add,3), data, len, 300);
}