path: root/src/wheel-service.c

/*
 * Copyright (c) 2017-2019 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 <netdb.h>
#include <fcntl.h>
#include <math.h>
#include <sys/time.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <json-c/json.h>
#include <sys/stat.h>
#include <pthread.h>
#include <systemd/sd-event.h>
#include <net/if.h>
#include <sys/ioctl.h>
#include <sys/socket.h>
#include <linux/can.h>
#include <linux/can/raw.h>


#include "wheel-service.h"
#include "steering_wheel_json.h"
#include "prop_search.h"
#include "js_raw.h"
#include "can_encoder.h"

#define ENABLE_EVENT_DROP
#define STEERING_WHEEL_JSON	"/etc/steering_wheel.json"
#define BUS_MAP_CONF "/etc/dev-mapping.conf"

struct wheel_conf
{
	char *devname;
};

struct transmission_bus_conf
{
	char *hs;
	char *ls;
};
static struct transmission_bus_conf trans_conf;

/*
 * notify function
 */
int notify_property_changed(struct prop_info_t *property_info)
{
	DBG_NOTICE("notify_property_changed name=%s,value=%d", property_info->name, property_info->curValue);

	int rc = push(makeCanData(property_info));
	if (rc < 0)
	{
		DBG_ERROR("push failed");
		return -1;
	}

	return 0;
}

/*
 * transmission loop
 */
static void *transmission_event_loop(void *args)
{
	int s; /* can raw socket */
	int required_mtu;
	int mtu;
	int enable_canfd = 1;
	struct sockaddr_can addr;
	struct canfd_frame frame;
	struct ifreq ifr;
//	int retry = 0;

	/* open socket */
	if ((s = socket(PF_CAN, SOCK_RAW, CAN_RAW)) < 0) {
		perror("open socket failed");
		return 0;
	}

	addr.can_family = AF_CAN;
	strcpy(ifr.ifr_name, trans_conf.hs);
	/* wait until hs device start */
	while(1) {
		if (ioctl(s, SIOCGIFINDEX, &ifr) < 0) {
			clear();		/* clear transmission msg queue */
			perror("SIOCGIFINDEX");
			sleep(2);
		}
		else
		{
			break;
		}
	}

	addr.can_ifindex = ifr.ifr_ifindex;

	/* disable default receive filter on this RAW socket */
	/* This is obsolete as we do not read from the socket at all, but for */
	/* this reason we can remove the receive list in the Kernel to save a */
	/* little (really a very little!) CPU usage.                          */
	setsockopt(s, SOL_CAN_RAW, CAN_RAW_FILTER, NULL, 0);

	if (bind(s, (struct sockaddr *)&addr, sizeof(addr)) < 0) {
		perror("bind");
		return 0;
	}

	while(1)
	{
		struct can_data_t* p = pop();
		if(p == NULL)
		{
			/* sleep 150ms */
			usleep(150000);
			continue;
		}

		/* parse CAN frame */
		required_mtu = parse_canframe(p->dat, &frame);
		free(p);
		if (!required_mtu){
			DBG_ERROR("\nWrong CAN-frame format! Try:\n\n");
			DBG_ERROR("    <can_id>#{R|data}          for CAN 2.0 frames\n");
			DBG_ERROR("    <can_id>##<flags>{data}    for CAN FD frames\n\n");
			DBG_ERROR("<can_id> can have 3 (SFF) or 8 (EFF) hex chars\n");
			DBG_ERROR("{data} has 0..8 (0..64 CAN FD) ASCII hex-values (optionally");
			DBG_ERROR(" separated by '.')\n");
			DBG_ERROR("<flags> a single ASCII Hex value (0 .. F) which defines");
			DBG_ERROR(" canfd_frame.flags\n\n");
			DBG_ERROR("e.g. 5A1#11.2233.44556677.88 / 123#DEADBEEF / 5AA# / ");
			DBG_ERROR("123##1 / 213##311\n     1F334455#1122334455667788 / 123#R ");
			DBG_ERROR("for remote transmission request.\n\n");
			continue;
		}

		if (required_mtu > CAN_MTU) {

			/* check if the frame fits into the CAN netdevice */
			if (ioctl(s, SIOCGIFMTU, &ifr) < 0) {
				perror("SIOCGIFMTU");
				continue;
			}
			mtu = ifr.ifr_mtu;

			if (mtu != CANFD_MTU) {
				DBG_ERROR("CAN interface ist not CAN FD capable - sorry.\n");
				continue;
			}

			/* interface is ok - try to switch the socket into CAN FD mode */
			if (setsockopt(s, SOL_CAN_RAW, CAN_RAW_FD_FRAMES,
				       &enable_canfd, sizeof(enable_canfd))){
				DBG_ERROR("error when enabling CAN FD support\n");
				continue;
			}

			/* ensure discrete CAN FD length values 0..8, 12, 16, 20, 24, 32, 64 */
			frame.len = can_dlc2len(can_len2dlc(frame.len));
		}

		/* send frame */
		if (write(s, &frame, required_mtu) != required_mtu) {
			perror("write");
		}
	}
}
/*
 * transmission thread main function
 */
static pthread_t runTransmissionLoop(void)
{
	pthread_t thread_id = 0;
    int ret = pthread_create(&thread_id, NULL, transmission_event_loop, NULL);
	if(ret != 0) {
		DBG_ERROR( "Cannot run eventloop due to error:%d", errno);
		return -1;
	}

	return thread_id;
}

/*
 * connect to js device
 */
static int connection(struct wheel_conf *conf)
{
	int js;
	int ret;
	int rc;
	sd_event_source *source;
	sd_event *loop = NULL;

	js = js_open(conf->devname);
	if (js < 0)
	{
		js_close(js);
		DBG_ERROR("can't connect to joy stick, the event loop failed");
		return 0;
	}

	if(init_timer() < 0)
	{
		js_close(js);
		DBG_ERROR("can't start update timer");
		return 0;
	}

	/* get the default event loop */
	rc = sd_event_default(&loop);
	if (rc < 0) {
		DBG_ERROR("connection to default event loop failed: %s\n", strerror(-rc));
		loop = NULL;
		return 0;
	}

	ret = sd_event_add_io(
				loop,
				&source,
				js,
				EPOLLIN,
				on_event, NULL);
	if (ret < 0)
	{
		js_close(js);
		DBG_ERROR("can't add event, the event loop failed");
		return 0;
	}

	DBG_NOTICE("connect to JS(%s), event loop started",conf->devname);

	return 0;
}

/*
 * read /etc/dev-mapping.conf file
 */
static int readTransBus(void)
{
	char *line = NULL;
	char bus_name[4], bus_val[10];
	size_t len = 0;
	ssize_t read;

	FILE *fp = fopen(BUS_MAP_CONF,"r");
	if (fp == NULL)
	{
		DBG_ERROR("cannot read /etc/dev-mapping.conf");
		return -1;
	}

	while((	read = getline(&line, &len, fp)) != -1) {
		if(line == NULL || line[0] == '[')
			continue;

		memset(bus_name, 0, sizeof(bus_name));
		memset(bus_val, 0, sizeof(bus_val));

		sscanf(line, "%2s=\"%s", bus_name, bus_val);
		bus_val[strlen(bus_val)-1] = '\0';
		if (strcmp(bus_name, "hs") == 0)
		{
			trans_conf.hs = strdup(bus_val);
		}
		else if (strcmp(bus_name, "ls") == 0)
		{
			trans_conf.ls = strdup(bus_val);
		}
	}

	if(line != NULL)
		free(line);

	fclose(fp);
//	DBG_ERROR("readTransBus end, hs:%s,ls:%s", trans_conf.hs, trans_conf.ls);
	return 0;
}

/*
 * parse configuration file (steering_wheel.json)
 */
static int init_conf(int fd_conf, struct wheel_conf *conf)
{

	char *filebuf = NULL;
	json_object *jobj = NULL;
	struct stat stbuf;

	FILE *fp = fdopen(fd_conf,"r");
	if (fp == NULL)
	{
		DBG_ERROR("cannot read configuration file(steering_wheel.json)");
		return -1;
	}

	if (fstat(fd_conf, &stbuf) == -1)
	{
		DBG_ERROR("can't get file state");
		return -1;
	}

//	fseek(fp, 0, SEEK_SET);
	filebuf = (char *)malloc(stbuf.st_size);
	fread(filebuf, 1, stbuf.st_size, fp);
	fclose(fp);

	jobj = json_tokener_parse(filebuf);
	if (jobj == NULL)
//	if (is_error(jobj))
	{
		DBG_ERROR("json: Invalid steering_wheel.json format");
		return -1;
	}
	json_object_object_foreach(jobj, key, val)
	{
		if (strcmp(key,"dev_name") == 0)
		{
			conf->devname = strdup(json_object_get_string(val));
		}
		else if (strcmp(key,"wheel_map") == 0)
		{
			wheel_define_init(json_object_get_string(val));
		}
		else if (strcmp(key,"gear_para") == 0)
		{
			wheel_gear_para_init(json_object_get_string(val));
		}
	}
	json_object_put(jobj);
	free(filebuf);

	return 0;
}

/*
 * init js device
 */
static int init()
{
	DBG_NOTICE("init");

	int fd_conf;
	static struct wheel_conf conf;

	init_can_encoder();

	if (readTransBus())
	{
		DBG_ERROR("read file  (/etc/dev-mapping.conf) failed");
		return -1;
	}

	fd_conf = open(STEERING_WHEEL_JSON, O_RDONLY);
	if (fd_conf < 0)
	{
		DBG_ERROR("wheel configuration (steering_wheel.json) is not access");
		return -1;
	}
	if (init_conf(fd_conf, &conf))
	{
		DBG_ERROR("wheel json file  (steering_wheel_map.json) is not access");
		return -1;
	}

	return connection(&conf);
}

/*
 * entry function
 */
int main(void)
{
	int rc;
	pthread_t thread_id;
	sd_event *loop = NULL;

	/* device init  */
	rc = init();
	if (rc < 0) {
		DBG_ERROR("js device init failed: %s\n", strerror(-rc));
		return 1;
	}

	/* start post thread */
	thread_id = runTransmissionLoop();
	if (thread_id < 0) {
		DBG_ERROR("run post thread failed: %s\n", strerror(-thread_id));
	}

	/* get the default event loop */
	rc = sd_event_default(&loop);
	if (rc < 0) {
		DBG_ERROR("connection to default event loop failed: %s\n", strerror(-rc));
		return 1;
	}

	/* loop until end */
	for(;;)
		sd_event_run(loop, 30000000);
	return 0;
}