diff options
Diffstat (limited to 'src/wheel-service.c')
| -rw-r--r-- | src/wheel-service.c | 420 |
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; +} + |