path: root/src/can_encoder.c

/*
 * Copyright (c) 2017-2018 TOYOTA MOTOR CORPORATION
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *      http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#define _GNU_SOURCE
#include <stdio.h>
#include <string.h>
#include <unistd.h>
#include <stdlib.h>
#include <errno.h>
#include <linux/can.h>
#include <linux/can/error.h>
#include <pthread.h>
#include <search.h>

#include "can_encoder.h"
#include "debug_msg.h"
#include "wheel-service.h"


#define CANID_DELIM '#'
#define DATA_SEPERATOR '.'


static struct can_data_t *phead = NULL, *ptail = NULL;
static pthread_mutex_t lock;
static char buf[MAX_CANDATA_SIZE+1] = {0};
static char str[MAX_LENGTH+1] = {0};
static void *canmsg_root = NULL;


/* CAN DLC to real data length conversion helpers */
static const unsigned char dlc2len[] = {0, 1, 2, 3, 4, 5, 6, 7,
					8, 12, 16, 20, 24, 32, 48, 64};

/*
 * get data length from can_dlc with sanitized can_dlc
 */
unsigned char can_dlc2len(unsigned char can_dlc)
{
	return dlc2len[can_dlc & 0x0F];
}

static const unsigned char len2dlc[] = {0, 1, 2, 3, 4, 5, 6, 7, 8,		/* 0 - 8 */
					9, 9, 9, 9,											/* 9 - 12 */
					10, 10, 10, 10,										/* 13 - 16 */
					11, 11, 11, 11,										/* 17 - 20 */
					12, 12, 12, 12,										/* 21 - 24 */
					13, 13, 13, 13, 13, 13, 13, 13,						/* 25 - 32 */
					14, 14, 14, 14, 14, 14, 14, 14,						/* 33 - 40 */
					14, 14, 14, 14, 14, 14, 14, 14,						/* 41 - 48 */
					15, 15, 15, 15, 15, 15, 15, 15,						/* 49 - 56 */
					15, 15, 15, 15, 15, 15, 15, 15};					/* 57 - 64 */

/*
 * map the sanitized data length to an appropriate data length code
 */
unsigned char can_len2dlc(unsigned char len)
{
	if (len > 64)
		return 0xF;

	return len2dlc[len];
}

/*
 * asc to nibble
 */
unsigned char asc2nibble(char c) {

	if ((c >= '0') && (c <= '9'))
		return c - '0';

	if ((c >= 'A') && (c <= 'F'))
		return c - 'A' + 10;

	if ((c >= 'a') && (c <= 'f'))
		return c - 'a' + 10;

	return 16; /* error */
}

static int canmsg_compare(const void *pa, const void *pb)
{
	struct canmsg_info_t *a =(struct canmsg_info_t *)pa;
	struct canmsg_info_t *b =(struct canmsg_info_t *)pb;
	return strcmp(a->canid,b->canid);
}

/*
 * init
 */
void init_can_encoder(void)
{
	pthread_mutex_init(&lock, NULL);
	phead = NULL;
	ptail = NULL;
}

/*
 * push can msg to queue
 */
int push(char *dat)
{
	if (dat == NULL)
	{
		DBG_ERROR("push data is NULL");
		return -1;
	}

	struct can_data_t *p = malloc(sizeof(struct can_data_t));
	strncpy(p->dat, dat, MAX_CANDATA_SIZE);
	p->dat[MAX_CANDATA_SIZE] = '\0';
	p->next = NULL;

	pthread_mutex_lock(&lock);
	if (ptail == NULL)
	{
		ptail = p;
		phead = p;
	}
	else
	{
		ptail->next = p;
		ptail = p;
	}
	pthread_mutex_unlock(&lock);
	return 0;
}

/*
 * pop can msg from queue
 */
struct can_data_t* pop(void)
{
	struct can_data_t *p = NULL;
	pthread_mutex_lock(&lock);
	if (phead != NULL)
	{
		p = phead;
		if (phead->next != NULL)
		{
			phead = p->next;
		}
		else
		{
			phead = NULL;
			ptail = NULL;
		}
	}
	pthread_mutex_unlock(&lock);
	return p;
}

/*
 * clear transmission msg queue
 */
void clear(void)
{
	struct can_data_t *p = NULL;
	pthread_mutex_lock(&lock);
	while (phead != NULL)
	{
		p = phead;
		phead = phead->next;
		free(p);
	}
	ptail = NULL;
	pthread_mutex_unlock(&lock);
}

/*
 * make "0" string
 */
static char *makeZeroString(uint8_t dlc)
{
	int len = dlc * 2;

	if (len > MAX_LENGTH)
	{
		DBG_ERROR("makeZeroString input dlc error; dlc=%d",dlc);
		return NULL;
	}

	for (int i = 0; i < len; i++) {
		str[i] = '0';
	}

	str[len + 1] = '\0';

	return str;
}

static struct canmsg_info_t * getCanMsg_dict(const char * can_id)
{
	struct canmsg_info_t info;
	strncpy(info.canid, can_id, sizeof(info.canid));
	info.value = 0;

	void *v;
	v = tfind((void *)&info, &canmsg_root, canmsg_compare);
	if (v == NULL)
	{
		/* new msg, add node */
		struct canmsg_info_t * p = (struct canmsg_info_t *)malloc(sizeof(struct canmsg_info_t));
		strncpy(p->canid, can_id, sizeof(p->canid));
		p->value = 0;
		v = tsearch((void *)p, &canmsg_root, canmsg_compare);
		if (v == NULL)
		{
			DBG_ERROR("add canmsg failed: not enogh memory?");
		}
	}

	return (*(struct canmsg_info_t **)v);
}

/*
 * make can frame data
 */
char * makeCanData(struct prop_info_t *property_info)
{
	char tmp[MAX_LENGTH+1] = {0};
	u_int64_t val = 0, mask = 0;
	struct canmsg_info_t * p = getCanMsg_dict(property_info->can_id);
	if (p == NULL)
	{
		return NULL;
	}

	memset(buf, 0, sizeof(buf));
	sprintf(buf, "%s\#%s", property_info->can_id, makeZeroString(property_info->dlc));
	mask = (1 << (property_info->bit_size)) - 1;
	val = mask & property_info->curValue.uint32_val;
	val = val << ((property_info->dlc * 8) - property_info->bit_size - property_info->bit_pos);
	mask = mask << ((property_info->dlc * 8) - property_info->bit_size - property_info->bit_pos);
	mask = ~mask;
	p->value = p->value & mask;
	p->value = p->value | val;

	sprintf(tmp, "%lx", p->value);
	strncpy((buf + 4 + property_info->dlc * 2 -strlen(tmp)), tmp ,strlen(tmp));

//	DBG_ERROR("makeCanData buf is [%s]", buf);

	return buf;
}

/*
 * convert to canframe format
 */
int parse_canframe(char *cs, struct canfd_frame *cf) {
	/* documentation see lib.h */

	int i, idx, dlen, len;
	int maxdlen = CAN_MAX_DLEN;
	int ret = CAN_MTU;
	unsigned char tmp;

	len = strlen(cs);

	memset(cf, 0, sizeof(*cf)); /* init CAN FD frame, e.g. LEN = 0 */

	if (len < 4)
		return 0;

	if (cs[3] == CANID_DELIM) { /* 3 digits */

		idx = 4;
		for (i=0; i<3; i++){
			if ((tmp = asc2nibble(cs[i])) > 0x0F)
				return 0;
			cf->can_id |= (tmp << (2-i)*4);
		}

	} else if (cs[8] == CANID_DELIM) { /* 8 digits */

		idx = 9;
		for (i=0; i<8; i++){
			if ((tmp = asc2nibble(cs[i])) > 0x0F)
				return 0;
			cf->can_id |= (tmp << (7-i)*4);
		}
		if (!(cf->can_id & CAN_ERR_FLAG)) /* 8 digits but no errorframe?  */
			cf->can_id |= CAN_EFF_FLAG;   /* then it is an extended frame */

	} else
		return 0;

	if((cs[idx] == 'R') || (cs[idx] == 'r')){ /* RTR frame */
		cf->can_id |= CAN_RTR_FLAG;

		/* check for optional DLC value for CAN 2.0B frames */
		if(cs[++idx] && (tmp = asc2nibble(cs[idx])) <= CAN_MAX_DLC)
			cf->len = tmp;

		return ret;
	}

	if (cs[idx] == CANID_DELIM) { /* CAN FD frame escape char '##' */

		maxdlen = CANFD_MAX_DLEN;
		ret = CANFD_MTU;

		/* CAN FD frame <canid>##<flags><data>* */
		if ((tmp = asc2nibble(cs[idx+1])) > 0x0F)
			return 0;

		cf->flags = tmp;
		idx += 2;
	}

	for (i=0, dlen=0; i < maxdlen; i++){

		if(cs[idx] == DATA_SEPERATOR) /* skip (optional) separator */
			idx++;

		if(idx >= len) /* end of string => end of data */
			break;

		if ((tmp = asc2nibble(cs[idx++])) > 0x0F)
			return 0;
		cf->data[i] = (tmp << 4);
		if ((tmp = asc2nibble(cs[idx++])) > 0x0F)
			return 0;
		cf->data[i] |= tmp;
		dlen++;
	}
	cf->len = dlen;

	return ret;
}