summaryrefslogtreecommitdiffstats
path: root/src/wheel-service.c
diff options
context:
space:
mode:
Diffstat (limited to 'src/wheel-service.c')
-rw-r--r--src/wheel-service.c420
1 files changed, 420 insertions, 0 deletions
diff --git a/src/wheel-service.c b/src/wheel-service.c
new file mode 100644
index 0000000..38303e8
--- /dev/null
+++ b/src/wheel-service.c
@@ -0,0 +1,420 @@
+/*
+ * 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;
+}
+