diff --git a/core/driver/socketcan/libsocketcan/can_netlink.h b/core/driver/socketcan/libsocketcan/can_netlink.h new file mode 100644 index 0000000..e3a0575 --- /dev/null +++ b/core/driver/socketcan/libsocketcan/can_netlink.h @@ -0,0 +1,134 @@ +/* + * linux/can/netlink.h + * + * Definitions for the CAN netlink interface + * + * Copyright (c) 2009 Wolfgang Grandegger + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the version 2 of the GNU General Public License + * as published by the Free Software Foundation + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + */ + +#ifndef _UAPI_CAN_NETLINK_H +#define _UAPI_CAN_NETLINK_H + +#include + +/* + * CAN bit-timing parameters + * + * For further information, please read chapter "8 BIT TIMING + * REQUIREMENTS" of the "Bosch CAN Specification version 2.0" + * at http://www.semiconductors.bosch.de/pdf/can2spec.pdf. + */ +struct can_bittiming { + __u32 bitrate; /* Bit-rate in bits/second */ + __u32 sample_point; /* Sample point in one-tenth of a percent */ + __u32 tq; /* Time quanta (TQ) in nanoseconds */ + __u32 prop_seg; /* Propagation segment in TQs */ + __u32 phase_seg1; /* Phase buffer segment 1 in TQs */ + __u32 phase_seg2; /* Phase buffer segment 2 in TQs */ + __u32 sjw; /* Synchronisation jump width in TQs */ + __u32 brp; /* Bit-rate prescaler */ +}; + +/* + * CAN harware-dependent bit-timing constant + * + * Used for calculating and checking bit-timing parameters + */ +struct can_bittiming_const { + char name[16]; /* Name of the CAN controller hardware */ + __u32 tseg1_min; /* Time segement 1 = prop_seg + phase_seg1 */ + __u32 tseg1_max; + __u32 tseg2_min; /* Time segement 2 = phase_seg2 */ + __u32 tseg2_max; + __u32 sjw_max; /* Synchronisation jump width */ + __u32 brp_min; /* Bit-rate prescaler */ + __u32 brp_max; + __u32 brp_inc; +}; + +/* + * CAN clock parameters + */ +struct can_clock { + __u32 freq; /* CAN system clock frequency in Hz */ +}; + +/* + * CAN operational and error states + */ +enum can_state { + CAN_STATE_ERROR_ACTIVE = 0, /* RX/TX error count < 96 */ + CAN_STATE_ERROR_WARNING, /* RX/TX error count < 128 */ + CAN_STATE_ERROR_PASSIVE, /* RX/TX error count < 256 */ + CAN_STATE_BUS_OFF, /* RX/TX error count >= 256 */ + CAN_STATE_STOPPED, /* Device is stopped */ + CAN_STATE_SLEEPING, /* Device is sleeping */ + CAN_STATE_MAX +}; + +/* + * CAN bus error counters + */ +struct can_berr_counter { + __u16 txerr; + __u16 rxerr; +}; + +/* + * CAN controller mode + */ +struct can_ctrlmode { + __u32 mask; + __u32 flags; +}; + +#define CAN_CTRLMODE_LOOPBACK 0x01 /* Loopback mode */ +#define CAN_CTRLMODE_LISTENONLY 0x02 /* Listen-only mode */ +#define CAN_CTRLMODE_3_SAMPLES 0x04 /* Triple sampling mode */ +#define CAN_CTRLMODE_ONE_SHOT 0x08 /* One-Shot mode */ +#define CAN_CTRLMODE_BERR_REPORTING 0x10 /* Bus-error reporting */ +#define CAN_CTRLMODE_FD 0x20 /* CAN FD mode */ +#define CAN_CTRLMODE_PRESUME_ACK 0x40 /* Ignore missing CAN ACKs */ + +/* + * CAN device statistics + */ +struct can_device_stats { + __u32 bus_error; /* Bus errors */ + __u32 error_warning; /* Changes to error warning state */ + __u32 error_passive; /* Changes to error passive state */ + __u32 bus_off; /* Changes to bus off state */ + __u32 arbitration_lost; /* Arbitration lost errors */ + __u32 restarts; /* CAN controller re-starts */ +}; + +/* + * CAN netlink interface + */ +enum { + IFLA_CAN_UNSPEC, + IFLA_CAN_BITTIMING, + IFLA_CAN_BITTIMING_CONST, + IFLA_CAN_CLOCK, + IFLA_CAN_STATE, + IFLA_CAN_CTRLMODE, + IFLA_CAN_RESTART_MS, + IFLA_CAN_RESTART, + IFLA_CAN_BERR_COUNTER, + IFLA_CAN_DATA_BITTIMING, + IFLA_CAN_DATA_BITTIMING_CONST, + __IFLA_CAN_MAX +}; + +#define IFLA_CAN_MAX (__IFLA_CAN_MAX - 1) + +#endif /* !_UAPI_CAN_NETLINK_H */ diff --git a/core/driver/socketcan/libsocketcan/libsocketcan.c b/core/driver/socketcan/libsocketcan/libsocketcan.c new file mode 100644 index 0000000..6d89767 --- /dev/null +++ b/core/driver/socketcan/libsocketcan/libsocketcan.c @@ -0,0 +1,1207 @@ +/* libsocketcan.c + * + * (C) 2009 Luotao Fu + * + * This library is free software; you can redistribute it and/or modify it under + * the terms of the GNU Lesser General Public License as published by the Free + * Software Foundation; either version 2.1 of the License, or (at your option) + * any later version. + * + * This library is distributed in the hope that it will be useful, but WITHOUT + * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS + * FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License for more + * details. + * + * You should have received a copy of the GNU Lesser General Public License + * along with this library; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ + +/** + * @file + * @brief library code + */ + +// #ifdef HAVE_CONFIG_H +// #include "libsocketcan_config.h" +// #endif + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include "libsocketcan.h" + +/* Define DISABLE_ERROR_LOG to disable printing of error messages to stderr. */ +#ifdef DISABLE_ERROR_LOG +#define perror(x) +#define fprintf(...) +#endif + +#define parse_rtattr_nested(tb, max, rta) \ + (parse_rtattr((tb), (max), RTA_DATA(rta), RTA_PAYLOAD(rta))) + +#define NLMSG_TAIL(nmsg) \ + ((struct rtattr *) (((void *) (nmsg)) + NLMSG_ALIGN((nmsg)->nlmsg_len))) + +#define IFLA_CAN_MAX (__IFLA_CAN_MAX - 1) + +#define IF_UP 1 +#define IF_DOWN 2 + +#define GET_STATE 1 +#define GET_RESTART_MS 2 +#define GET_BITTIMING 3 +#define GET_CTRLMODE 4 +#define GET_CLOCK 5 +#define GET_BITTIMING_CONST 6 +#define GET_BERR_COUNTER 7 +#define GET_XSTATS 8 +#define GET_LINK_STATS 9 + +struct get_req { + struct nlmsghdr n; + struct ifinfomsg i; +}; + +struct set_req { + struct nlmsghdr n; + struct ifinfomsg i; + char buf[1024]; +}; + +struct req_info { + __u8 restart; + __u8 disable_autorestart; + __u32 restart_ms; + struct can_ctrlmode *ctrlmode; + struct can_bittiming *bittiming; +}; + +/** + * @brief this method parse attributions of link info + * + * @param tb: point array of struct rtattr point + * @param max: index of the last element in array tb + * @param rtattr: point of link info data + * @param len: length of link info data + */ +static void +parse_rtattr(struct rtattr **tb, int max, struct rtattr *rta, int len) +{ + memset(tb, 0, sizeof(*tb) * (max + 1)); + while (RTA_OK(rta, len)) { + if (rta->rta_type <= max) { + tb[rta->rta_type] = rta; + } + + rta = RTA_NEXT(rta, len); + } +} + +static int addattr32(struct nlmsghdr *n, size_t maxlen, int type, __u32 data) +{ + int len = RTA_LENGTH(4); + struct rtattr *rta; + + if (NLMSG_ALIGN(n->nlmsg_len) + len > maxlen) { + fprintf(stderr, + "addattr32: Error! max allowed bound %zu exceeded\n", + maxlen); + return -1; + } + + rta = NLMSG_TAIL(n); + rta->rta_type = type; + rta->rta_len = len; + memcpy(RTA_DATA(rta), &data, 4); + n->nlmsg_len = NLMSG_ALIGN(n->nlmsg_len) + len; + + return 0; +} + +static int addattr_l(struct nlmsghdr *n, size_t maxlen, int type, + const void *data, int alen) +{ + int len = RTA_LENGTH(alen); + struct rtattr *rta; + + if (NLMSG_ALIGN(n->nlmsg_len) + RTA_ALIGN(len) > maxlen) { + fprintf(stderr, + "addattr_l ERROR: message exceeded bound of %zu\n", + maxlen); + return -1; + } + + rta = NLMSG_TAIL(n); + rta->rta_type = type; + rta->rta_len = len; + memcpy(RTA_DATA(rta), data, alen); + n->nlmsg_len = NLMSG_ALIGN(n->nlmsg_len) + RTA_ALIGN(len); + + return 0; +} + +/** + * @ingroup intern + * @brief send_mod_request - send a linkinfo modification request + * + * @param fd decriptor to a priorly opened netlink socket + * @param n netlink message containing the request + * + * sends a request to setup the the linkinfo to netlink layer and awaits the + * status. + * + * @return 0 if success + * @return negativ if failed + */ +static int send_mod_request(int fd, struct nlmsghdr *n) +{ + int status; + struct sockaddr_nl nladdr; + struct nlmsghdr *h; + + struct iovec iov = { + .iov_base = (void *)n, + .iov_len = n->nlmsg_len + }; + struct msghdr msg = { + .msg_name = &nladdr, + .msg_namelen = sizeof(nladdr), + .msg_iov = &iov, + .msg_iovlen = 1, + }; + char buf[16384]; + + memset(&nladdr, 0, sizeof(nladdr)); + + nladdr.nl_family = AF_NETLINK; + nladdr.nl_pid = 0; + nladdr.nl_groups = 0; + + n->nlmsg_seq = 0; + n->nlmsg_flags |= NLM_F_ACK; + + status = sendmsg(fd, &msg, 0); + + if (status < 0) { + perror("Cannot talk to rtnetlink"); + return -1; + } + + iov.iov_base = buf; + while (1) { + iov.iov_len = sizeof(buf); + status = recvmsg(fd, &msg, 0); + for (h = (struct nlmsghdr *)buf; (size_t) status >= sizeof(*h);) { + int len = h->nlmsg_len; + int l = len - sizeof(*h); + if (l < 0 || len > status) { + if (msg.msg_flags & MSG_TRUNC) { + fprintf(stderr, "Truncated message\n"); + return -1; + } + fprintf(stderr, + "!!!malformed message: len=%d\n", len); + return -1; + } + + if (h->nlmsg_type == NLMSG_ERROR) { + struct nlmsgerr *err = + (struct nlmsgerr *)NLMSG_DATA(h); + if ((size_t) l < sizeof(struct nlmsgerr)) { + fprintf(stderr, "ERROR truncated\n"); + } else { + errno = -err->error; + if (errno == 0) + return 0; + + perror("RTNETLINK answers"); + } + return -1; + } + status -= NLMSG_ALIGN(len); + h = (struct nlmsghdr *)((char *)h + NLMSG_ALIGN(len)); + } + } + + return 0; +} + +/** + * @ingroup intern + * @brief send_dump_request - send a dump linkinfo request + * + * @param fd decriptor to a priorly opened netlink socket + * @param name network interface name, null means all interfaces + * @param family rt_gen message family + * @param type netlink message header type + * + * @return 0 if success + * @return negativ if failed + */ +static int send_dump_request(int fd, const char *name, int family, int type) +{ + struct get_req req; + + memset(&req, 0, sizeof(req)); + + req.n.nlmsg_len = sizeof(req); + req.n.nlmsg_type = type; + req.n.nlmsg_flags = NLM_F_REQUEST; + req.n.nlmsg_pid = 0; + req.n.nlmsg_seq = 0; + + req.i.ifi_family = family; + /* + * If name is null, set flag to dump link information from all + * interfaces otherwise, just dump specified interface's link + * information. + */ + if (name == NULL) { + req.n.nlmsg_flags |= NLM_F_DUMP; + } else { + req.i.ifi_index = if_nametoindex(name); + if (req.i.ifi_index == 0) { + fprintf(stderr, "Cannot find device \"%s\"\n", name); + return -1; + } + } + + return send(fd, (void *)&req, sizeof(req), 0); +} + +/** + * @ingroup intern + * @brief open_nl_sock - open a netlink socket + * + * opens a netlink socket and returns the socket descriptor + * + * @return 0 if success + * @return negativ if failed + */ +static int open_nl_sock() +{ + int fd; + int sndbuf = 32768; + int rcvbuf = 32768; + unsigned int addr_len; + struct sockaddr_nl local; + + fd = socket(AF_NETLINK, SOCK_RAW, NETLINK_ROUTE); + if (fd < 0) { + perror("Cannot open netlink socket"); + return -1; + } + + setsockopt(fd, SOL_SOCKET, SO_SNDBUF, (void *)&sndbuf, sizeof(sndbuf)); + + setsockopt(fd, SOL_SOCKET, SO_RCVBUF, (void *)&rcvbuf, sizeof(rcvbuf)); + + memset(&local, 0, sizeof(local)); + local.nl_family = AF_NETLINK; + local.nl_groups = 0; + + if (bind(fd, (struct sockaddr *)&local, sizeof(local)) < 0) { + perror("Cannot bind netlink socket"); + return -1; + } + + addr_len = sizeof(local); + if (getsockname(fd, (struct sockaddr *)&local, &addr_len) < 0) { + perror("Cannot getsockname"); + return -1; + } + if (addr_len != sizeof(local)) { + fprintf(stderr, "Wrong address length %u\n", addr_len); + return -1; + } + if (local.nl_family != AF_NETLINK) { + fprintf(stderr, "Wrong address family %d\n", local.nl_family); + return -1; + } + return fd; +} + +/** + * @ingroup intern + * @brief do_get_nl_link - get linkinfo + * + * @param fd socket file descriptor to a priorly opened netlink socket + * @param acquire which parameter we want to get + * @param name name of the can device. This is the netdev name, as ifconfig -a + * shows in your system. usually it contains prefix "can" and the numer of the + * can line. e.g. "can0" + * @param res pointer to store the result + * + * This callback send a dump request into the netlink layer, collect the packet + * containing the linkinfo and fill the pointer res points to depending on the + * acquire mode set in param acquire. + * + * @return 0 if success + * @return -1 if failed + */ + +static int do_get_nl_link(int fd, __u8 acquire, const char *name, void *res) +{ + struct sockaddr_nl peer; + + char cbuf[64]; + char nlbuf[1024 * 8]; + + int ret = -1; + int done = 0; + + struct iovec iov = { + .iov_base = (void *)nlbuf, + .iov_len = sizeof(nlbuf), + }; + + struct msghdr msg = { + .msg_name = (void *)&peer, + .msg_namelen = sizeof(peer), + .msg_iov = &iov, + .msg_iovlen = 1, + .msg_control = &cbuf, + .msg_controllen = sizeof(cbuf), + .msg_flags = 0, + }; + struct nlmsghdr *nl_msg; + ssize_t msglen; + + struct rtattr *linkinfo[IFLA_INFO_MAX + 1]; + struct rtattr *can_attr[IFLA_CAN_MAX + 1]; + + if (send_dump_request(fd, name, AF_PACKET, RTM_GETLINK) < 0) { + perror("Cannot send dump request"); + return ret; + } + + while (!done && (msglen = recvmsg(fd, &msg, 0)) > 0) { + size_t u_msglen = (size_t) msglen; + /* Check to see if the buffers in msg get truncated */ + if (msg.msg_namelen != sizeof(peer) || + (msg.msg_flags & (MSG_TRUNC | MSG_CTRUNC))) { + fprintf(stderr, "Uhoh... truncated message.\n"); + return -1; + } + + for (nl_msg = (struct nlmsghdr *)nlbuf; + NLMSG_OK(nl_msg, u_msglen); + nl_msg = NLMSG_NEXT(nl_msg, u_msglen)) { + int type = nl_msg->nlmsg_type; + int len; + + if (type == NLMSG_DONE) { + done++; + continue; + } + if (type != RTM_NEWLINK) + continue; + + struct ifinfomsg *ifi = NLMSG_DATA(nl_msg); + struct rtattr *tb[IFLA_MAX + 1]; + + len = + nl_msg->nlmsg_len - NLMSG_LENGTH(sizeof(struct ifinfomsg)); + parse_rtattr(tb, IFLA_MAX, IFLA_RTA(ifi), len); + + /* Finish process if the reply message is matched */ + if (strcmp((char *)RTA_DATA(tb[IFLA_IFNAME]), name) == 0) + done++; + else + continue; + + if (acquire == GET_LINK_STATS) { + if (!tb[IFLA_STATS64]) { + fprintf(stderr, "no link statistics (64-bit) found\n"); + } else { + memcpy(res, RTA_DATA(tb[IFLA_STATS64]), sizeof(struct rtnl_link_stats64)); + ret = 0; + } + continue; + } + + if (tb[IFLA_LINKINFO]) + parse_rtattr_nested(linkinfo, + IFLA_INFO_MAX, tb[IFLA_LINKINFO]); + else + continue; + + if (acquire == GET_XSTATS) { + if (!linkinfo[IFLA_INFO_XSTATS]) + fprintf(stderr, "no can statistics found\n"); + else { + memcpy(res, RTA_DATA(linkinfo[IFLA_INFO_XSTATS]), + sizeof(struct can_device_stats)); + ret = 0; + } + continue; + } + + if (!linkinfo[IFLA_INFO_DATA]) { + fprintf(stderr, "no link data found\n"); + return ret; + } + + parse_rtattr_nested(can_attr, IFLA_CAN_MAX, + linkinfo[IFLA_INFO_DATA]); + + switch (acquire) { + case GET_STATE: + if (can_attr[IFLA_CAN_STATE]) { + *((int *)res) = *((__u32 *) + RTA_DATA(can_attr + [IFLA_CAN_STATE])); + ret = 0; + } else { + fprintf(stderr, "no state data found\n"); + } + + break; + case GET_RESTART_MS: + if (can_attr[IFLA_CAN_RESTART_MS]) { + *((__u32 *) res) = *((__u32 *) + RTA_DATA(can_attr + [IFLA_CAN_RESTART_MS])); + ret = 0; + } else + fprintf(stderr, "no restart_ms data found\n"); + + break; + case GET_BITTIMING: + if (can_attr[IFLA_CAN_BITTIMING]) { + memcpy(res, + RTA_DATA(can_attr[IFLA_CAN_BITTIMING]), + sizeof(struct can_bittiming)); + ret = 0; + } else + fprintf(stderr, "no bittiming data found\n"); + + break; + case GET_CTRLMODE: + if (can_attr[IFLA_CAN_CTRLMODE]) { + memcpy(res, + RTA_DATA(can_attr[IFLA_CAN_CTRLMODE]), + sizeof(struct can_ctrlmode)); + ret = 0; + } else + fprintf(stderr, "no ctrlmode data found\n"); + + break; + case GET_CLOCK: + if (can_attr[IFLA_CAN_CLOCK]) { + memcpy(res, + RTA_DATA(can_attr[IFLA_CAN_CLOCK]), + sizeof(struct can_clock)); + ret = 0; + } else + fprintf(stderr, + "no clock parameter data found\n"); + + break; + case GET_BITTIMING_CONST: + if (can_attr[IFLA_CAN_BITTIMING_CONST]) { + memcpy(res, + RTA_DATA(can_attr[IFLA_CAN_BITTIMING_CONST]), + sizeof(struct can_bittiming_const)); + ret = 0; + } else + fprintf(stderr, "no bittiming_const data found\n"); + + break; + case GET_BERR_COUNTER: + if (can_attr[IFLA_CAN_BERR_COUNTER]) { + memcpy(res, + RTA_DATA(can_attr[IFLA_CAN_BERR_COUNTER]), + sizeof(struct can_berr_counter)); + ret = 0; + } else + fprintf(stderr, "no berr_counter data found\n"); + + break; + + default: + fprintf(stderr, "unknown acquire mode\n"); + } + } + } + + return ret; +} + +/** + * @ingroup intern + * @brief get_link - get linkinfo + * + * @param name name of the can device. This is the netdev name, as ifconfig -a shows + * in your system. usually it contains prefix "can" and the numer of the can + * line. e.g. "can0" + * @param acquire which parameter we want to get + * @param res pointer to store the result + * + * This is a wrapper for do_get_nl_link + * + * @return 0 if success + * @return -1 if failed + */ +static int get_link(const char *name, __u8 acquire, void *res) +{ + int err, fd; + + fd = open_nl_sock(); + if (fd < 0) + return -1; + + err = do_get_nl_link(fd, acquire, name, res); + close(fd); + + return err; + +} + +/** + * @ingroup intern + * @brief do_set_nl_link - setup linkinfo + * + * @param fd socket file descriptor to a priorly opened netlink socket + * @param if_state state of the interface we want to put the device into. this + * parameter is only set if you want to use the callback to driver up/down the + * device + * @param name name of the can device. This is the netdev name, as ifconfig -a shows + * in your system. usually it contains prefix "can" and the numer of the can + * line. e.g. "can0" + * @param req_info request parameters + * + * This callback can do two different tasks: + * - bring up/down the interface + * - set up a netlink packet with request, as set up in req_info + * Which task this callback will do depends on which parameters are set. + * + * @return 0 if success + * @return -1 if failed + */ +static int do_set_nl_link(int fd, __u8 if_state, const char *name, + struct req_info *req_info) +{ + struct set_req req; + + const char *type = "can"; + + memset(&req, 0, sizeof(req)); + + req.n.nlmsg_len = NLMSG_LENGTH(sizeof(struct ifinfomsg)); + req.n.nlmsg_flags = NLM_F_REQUEST | NLM_F_ACK; + req.n.nlmsg_type = RTM_NEWLINK; + req.i.ifi_family = 0; + + req.i.ifi_index = if_nametoindex(name); + if (req.i.ifi_index == 0) { + fprintf(stderr, "Cannot find device \"%s\"\n", name); + return -1; + } + + if (if_state) { + switch (if_state) { + case IF_DOWN: + req.i.ifi_change |= IFF_UP; + req.i.ifi_flags &= ~IFF_UP; + break; + case IF_UP: + req.i.ifi_change |= IFF_UP; + req.i.ifi_flags |= IFF_UP; + break; + default: + fprintf(stderr, "unknown state\n"); + return -1; + } + } + + if (req_info != NULL) { + /* setup linkinfo section */ + struct rtattr *linkinfo = NLMSG_TAIL(&req.n); + addattr_l(&req.n, sizeof(req), IFLA_LINKINFO, NULL, 0); + addattr_l(&req.n, sizeof(req), IFLA_INFO_KIND, type, + strlen(type)); + /* setup data section */ + struct rtattr *data = NLMSG_TAIL(&req.n); + addattr_l(&req.n, sizeof(req), IFLA_INFO_DATA, NULL, 0); + + if (req_info->restart_ms > 0 || req_info->disable_autorestart) + addattr32(&req.n, 1024, IFLA_CAN_RESTART_MS, + req_info->restart_ms); + + if (req_info->restart) + addattr32(&req.n, 1024, IFLA_CAN_RESTART, 1); + + if (req_info->bittiming != NULL) { + addattr_l(&req.n, 1024, IFLA_CAN_BITTIMING, + req_info->bittiming, + sizeof(struct can_bittiming)); + } + + if (req_info->ctrlmode != NULL) { + addattr_l(&req.n, 1024, IFLA_CAN_CTRLMODE, + req_info->ctrlmode, + sizeof(struct can_ctrlmode)); + } + + /* mark end of data section */ + data->rta_len = (void *)NLMSG_TAIL(&req.n) - (void *)data; + + /* mark end of link info section */ + linkinfo->rta_len = + (void *)NLMSG_TAIL(&req.n) - (void *)linkinfo; + } + + return send_mod_request(fd, &req.n); +} + +/** + * @ingroup intern + * @brief set_link - open a netlink socket and setup linkinfo + * + * @param name name of the can device. This is the netdev name, as ifconfig -a + * shows in your system. usually it contains prefix "can" and the numer of the + * can line. e.g. "can0" + * @param if_state state of the interface we want to put the device into. this + * parameter is only set if you want to use the callback to driver up/down the + * device + * @param req_info request parameters + * + * This is a wrapper for do_set_nl_link. It opens a netlink socket and sends + * down the requests. + * + * @return 0 if success + * @return -1 if failed + */ +static int set_link(const char *name, __u8 if_state, struct req_info *req_info) +{ + int err, fd; + + fd = open_nl_sock(); + if (fd < 0) + return -1; + + err = do_set_nl_link(fd, if_state, name, req_info); + close(fd); + + return err; +} + +/** + * @ingroup extern + * can_do_start - start the can interface + * @param name name of the can device. This is the netdev name, as ifconfig -a shows + * in your system. usually it contains prefix "can" and the numer of the can + * line. e.g. "can0" + * + * This starts the can interface with the given name. It simply changes the if + * state of the interface to up. All initialisation works will be done in + * kernel. The if state can also be queried by a simple ifconfig. + * + * @return 0 if success + * @return -1 if failed + */ +int can_do_start(const char *name) +{ + return set_link(name, IF_UP, NULL); +} + +/** + * @ingroup extern + * can_do_stop - stop the can interface + * @param name name of the can device. This is the netdev name, as ifconfig -a shows + * in your system. usually it contains prefix "can" and the numer of the can + * line. e.g. "can0" + * + * This stops the can interface with the given name. It simply changes the if + * state of the interface to down. Any running communication would be stopped. + * + * @return 0 if success + * @return -1 if failed + */ +int can_do_stop(const char *name) +{ + return set_link(name, IF_DOWN, NULL); +} + +/** + * @ingroup extern + * can_do_restart - restart the can interface + * @param name name of the can device. This is the netdev name, as ifconfig -a shows + * in your system. usually it contains prefix "can" and the numer of the can + * line. e.g. "can0" + * + * This triggers the start mode of the can device. + * + * NOTE: + * - restart mode can only be triggerd if the device is in BUS_OFF and the auto + * restart not turned on (restart_ms == 0) + * + * @return 0 if success + * @return -1 if failed + */ +int can_do_restart(const char *name) +{ + int state; + __u32 restart_ms; + + /* first we check if we can restart the device at all */ + if ((can_get_state(name, &state)) < 0) { + fprintf(stderr, "cannot get bustate, " + "something is seriously wrong\n"); + return -1; + } else if (state != CAN_STATE_BUS_OFF) { + fprintf(stderr, + "Device is not in BUS_OFF," " no use to restart\n"); + return -1; + } + + if ((can_get_restart_ms(name, &restart_ms)) < 0) { + fprintf(stderr, "cannot get restart_ms, " + "something is seriously wrong\n"); + return -1; + } else if (restart_ms > 0) { + fprintf(stderr, + "auto restart with %ums interval is turned on," + " no use to restart\n", restart_ms); + return -1; + } + + struct req_info req_info = { + .restart = 1, + }; + + return set_link(name, 0, &req_info); +} + +/** + * @ingroup extern + * can_set_restart_ms - set interval of auto restart. + * + * @param name name of the can device. This is the netdev name, as ifconfig -a shows + * in your system. usually it contains prefix "can" and the numer of the can + * line. e.g. "can0" + * @param restart_ms interval of auto restart in milliseconds + * + * This sets how often the device shall automatically restart the interface in + * case that a bus_off is detected. + * + * @return 0 if success + * @return -1 if failed + */ +int can_set_restart_ms(const char *name, __u32 restart_ms) +{ + struct req_info req_info = { + .restart_ms = restart_ms, + }; + + if (restart_ms == 0) + req_info.disable_autorestart = 1; + + return set_link(name, 0, &req_info); +} + +/** + * @ingroup extern + * can_set_ctrlmode - setup the control mode. + * + * @param name name of the can device. This is the netdev name, as ifconfig -a shows + * in your system. usually it contains prefix "can" and the numer of the can + * line. e.g. "can0" + * + * @param cm pointer of a can_ctrlmode struct + * + * This sets the control mode of the can device. There are currently the + * following modes available (each mapped to its own macro): + * + * @code + * #define CAN_CTRLMODE_LOOPBACK 0x01 // Loopback mode + * #define CAN_CTRLMODE_LISTENONLY 0x02 // Listen-only mode + * #define CAN_CTRLMODE_3_SAMPLES 0x04 // Triple sampling mode + * #define CAN_CTRLMODE_ONE_SHOT 0x08 // One-Shot mode + * #define CAN_CTRLMODE_BERR_REPORTING 0x10 // Bus-error reporting + * #define CAN_CTRLMODE_FD 0x20 // CAN FD mode + * #define CAN_CTRLMODE_PRESUME_ACK 0x40 // Ignore missing CAN ACKs + * @endcode + * + * You have to define the control mode struct yourself. A can_ctrlmode struct + * is declared as: + * + * @code + * struct can_ctrlmode { + * __u32 mask; + * __u32 flags; + * } + * @endcode + * + * You can use mask to select modes you want to control and flags to determine + * if you want to turn the selected mode(s) on or off. E. g. the following + * pseudocode will turn the loopback mode on and listenonly mode off: + * + * @code + * struct can_ctrlmode cm; + * memset(&cm, 0, sizeof(cm)); + * cm.mask = CAN_CTRLMODE_LOOPBACK | CAN_CTRLMODE_LISTENONLY; + * cm.flags = CAN_CTRLMODE_LOOPBACK; + * can_set_ctrlmode(candev, &cm); + * @endcode + * + * @return 0 if success + * @return -1 if failed + */ + +int can_set_ctrlmode(const char *name, struct can_ctrlmode *cm) +{ + struct req_info req_info = { + .ctrlmode = cm, + }; + + return set_link(name, 0, &req_info); +} + +/** + * @ingroup extern + * can_set_bittiming - setup the bittiming. + * + * @param name name of the can device. This is the netdev name, as ifconfig -a shows + * in your system. usually it contains prefix "can" and the numer of the can + * line. e.g. "can0" + * @param bt pointer to a can_bittiming struct + * + * This sets the bittiming of the can device. This is for advantage usage. In + * normal cases you should use can_set_bitrate to simply define the bitrate and + * let the driver automatically calculate the bittiming. You will only need this + * function if you wish to define the bittiming in expert mode with fully + * manually defined timing values. + * You have to define the bittiming struct yourself. a can_bittiming struct + * consists of: + * + * @code + * struct can_bittiming { + * __u32 bitrate; + * __u32 sample_point; + * __u32 tq; + * __u32 prop_seg; + * __u32 phase_seg1; + * __u32 phase_seg2; + * __u32 sjw; + * __u32 brp; + * } + * @endcode + * + * to define a customized bittiming, you have to define tq, prop_seq, + * phase_seg1, phase_seg2 and sjw. See http://www.can-cia.org/index.php?id=88 + * for more information about bittiming and synchronizations on can bus. + * + * @return 0 if success + * @return -1 if failed + */ + +int can_set_bittiming(const char *name, struct can_bittiming *bt) +{ + struct req_info req_info = { + .bittiming = bt, + }; + + return set_link(name, 0, &req_info); +} + +/** + * @ingroup extern + * can_set_bitrate - setup the bitrate. + * + * @param name name of the can device. This is the netdev name, as ifconfig -a shows + * in your system. usually it contains prefix "can" and the numer of the can + * line. e.g. "can0" + * @param bitrate bitrate of the can bus + * + * This is the recommended way to setup the bus bit timing. You only have to + * give a bitrate value here. The exact bit timing will be calculated + * automatically. To use this function, make sure that CONFIG_CAN_CALC_BITTIMING + * is set to y in your kernel configuration. bitrate can be a value between + * 1000(1kbit/s) and 1000000(1000kbit/s). + * + * @return 0 if success + * @return -1 if failed + */ + +int can_set_bitrate(const char *name, __u32 bitrate) +{ + struct can_bittiming bt; + + memset(&bt, 0, sizeof(bt)); + bt.bitrate = bitrate; + + return can_set_bittiming(name, &bt); +} + +/** + * @ingroup extern + * can_set_bitrate_samplepoint - setup the bitrate. + * + * @param name name of the can device. This is the netdev name, as ifconfig -a shows + * in your system. usually it contains prefix "can" and the numer of the can + * line. e.g. "can0" + * @param bitrate bitrate of the can bus + * @param sample_point sample point value + * + * This one is similar to can_set_bitrate, only you can additionally set up the + * time point for sampling (sample point) customly instead of using the + * CIA recommended value. sample_point can be a value between 0 and 999. + * + * @return 0 if success + * @return -1 if failed + */ +int can_set_bitrate_samplepoint(const char *name, __u32 bitrate, + __u32 sample_point) +{ + struct can_bittiming bt; + + memset(&bt, 0, sizeof(bt)); + bt.bitrate = bitrate; + bt.sample_point = sample_point; + + return can_set_bittiming(name, &bt); +} + +/** + * @ingroup extern + * can_get_state - get the current state of the device + * + * @param name name of the can device. This is the netdev name, as ifconfig -a shows + * in your system. usually it contains prefix "can" and the numer of the can + * line. e.g. "can0" + * @param state pointer to store the state + * + * This one stores the current state of the can interface into the given + * pointer. Valid states are: + * - CAN_STATE_ERROR_ACTIVE + * - CAN_STATE_ERROR_WARNING + * - CAN_STATE_ERROR_PASSIVE + * - CAN_STATE_BUS_OFF + * - CAN_STATE_STOPPED + * - CAN_STATE_SLEEPING + * + * The first four states is determined by the value of RX/TX error counter. + * Please see relevant can specification for more information about this. A + * device in STATE_STOPPED is an inactive device. STATE_SLEEPING is not + * implemented on all devices. + * + * @return 0 if success + * @return -1 if failed + */ + +int can_get_state(const char *name, int *state) +{ + return get_link(name, GET_STATE, state); +} + +/** + * @ingroup extern + * can_get_restart_ms - get the current interval of auto restarting. + * + * @param name name of the can device. This is the netdev name, as ifconfig -a shows + * in your system. usually it contains prefix "can" and the numer of the can + * line. e.g. "can0" + * @param restart_ms pointer to store the value. + * + * This one stores the current interval of auto restarting into the given + * pointer. + * + * The socketcan framework can automatically restart a device if it is in + * bus_off in a given interval. This function gets this value in milliseconds. + * restart_ms == 0 means that autorestarting is turned off. + * + * @return 0 if success + * @return -1 if failed + */ + +int can_get_restart_ms(const char *name, __u32 *restart_ms) +{ + return get_link(name, GET_RESTART_MS, restart_ms); +} + +/** + * @ingroup extern + * can_get_bittiming - get the current bittimnig configuration. + * + * @param name name of the can device. This is the netdev name, as ifconfig -a shows + * in your system. usually it contains prefix "can" and the numer of the can + * line. e.g. "can0" + * @param bt pointer to the bittiming struct. + * + * This one stores the current bittiming configuration. + * + * Please see can_set_bittiming for more information about bit timing. + * + * @return 0 if success + * @return -1 if failed + */ +int can_get_bittiming(const char *name, struct can_bittiming *bt) +{ + return get_link(name, GET_BITTIMING, bt); +} + +/** + * @ingroup extern + * can_get_ctrlmode - get the current control mode. + * + * @param name name of the can device. This is the netdev name, as ifconfig -a shows + * in your system. usually it contains prefix "can" and the numer of the can + * line. e.g. "can0" + * @param cm pointer to the ctrlmode struct. + * + * This one stores the current control mode configuration. + * + * Please see can_set_ctrlmode for more information about control modes. + * + * @return 0 if success + * @return -1 if failed + */ + +int can_get_ctrlmode(const char *name, struct can_ctrlmode *cm) +{ + return get_link(name, GET_CTRLMODE, cm); +} + +/** + * @ingroup extern + * can_get_clock - get the current clock struct. + * + * @param name: name of the can device. This is the netdev name, as ifconfig -a shows + * in your system. usually it contains prefix "can" and the numer of the can + * line. e.g. "can0" + * @param clock pointer to the clock struct. + * + * This one stores the current clock configuration. At the time of writing the + * can_clock struct only contains information about the clock frequecy. This + * information is e.g. essential while setting up the bit timing. + * + * @return 0 if success + * @return -1 if failed + */ +int can_get_clock(const char *name, struct can_clock *clock) +{ + return get_link(name, GET_CLOCK, clock); +} + +/** + * @ingroup extern + * can_get_bittiming_const - get the current bittimnig constant. + * + * @param name name of the can device. This is the netdev name, as ifconfig -a shows + * in your system. usually it contains prefix "can" and the numer of the can + * line. e.g. "can0" + * @param btc pointer to the bittiming constant struct. + * + * This one stores the hardware dependent bittiming constant. The + * can_bittiming_const struct consists: + * + * @code + * struct can_bittiming_const { + * char name[16]; + * __u32 tseg1_min; + * __u32 tseg1_max; + * __u32 tseg2_min; + * __u32 tseg2_max; + * __u32 sjw_max; + * __u32 brp_min; + * __u32 brp_max; + * __u32 brp_inc; + * }; + * @endcode + * + * The information in this struct is used to calculate the bus bit timing + * automatically. + * + * @return 0 if success + * @return -1 if failed + */ +int can_get_bittiming_const(const char *name, struct can_bittiming_const *btc) +{ + return get_link(name, GET_BITTIMING_CONST, btc); +} + + +/** + * @ingroup extern + * can_get_berr_counter - get the tx/rx error counter. + * + * @param name name of the can device. This is the netdev name, as ifconfig -a shows + * in your system. usually it contains prefix "can" and the numer of the can + * line. e.g. "can0" + * @param bc pointer to the error counter struct.. + * + * This one gets the current rx/tx error counter from the hardware. + * + * @code + * struct can_berr_counter { + * __u16 txerr; + * __u16 rxerr; + * }; + * @endcode + * + * @return 0 if success + * @return -1 if failed + */ +int can_get_berr_counter(const char *name, struct can_berr_counter *bc) +{ + return get_link(name, GET_BERR_COUNTER, bc); +} + +/** + * @ingroup extern + * can_get_device_stats - get the can_device_stats. + * + * @param name name of the can device. This is the netdev name, as ifconfig -a shows + * in your system. usually it contains prefix "can" and the numer of the can + * line. e.g. "can0" + * @param bc pointer to the error counter struct.. + * + * This one gets the current can_device_stats. + * + * Please see struct can_device_stats for more information. + * + * @return 0 if success + * @return -1 if failed + */ +int can_get_device_stats(const char *name, struct can_device_stats *cds) +{ + return get_link(name, GET_XSTATS, cds); +} + +/** + * @ingroup extern + * can_get_link_statistics - get RX/TX statistics (64 bits version) + * + * @param name name of the can device. This is the netdev name, as ip link shows + * in your system. usually it contains prefix "can" and the number of the can + * line. e.g. "can0" + * @param rls pointer to the rtnl_link_stats64 struct which is from if_link.h. + * + * This one gets the current rtnl_link_stats64. Compares to the rtnl_link_stats, + * The rtnl_link_stats64 is introduced since linux kernel 2.6. After that the + * rtnl_link_stats is synchronous with struct rtnl_link_stats64 by truncating + * each member from 64 bits to 32 bits. actually, the struct rtnl_link_stats + * is kept for compatibility. + * + * Please see struct rtnl_link_stats64 (/usr/include/linux/if_link.h) for more + * information. + * + * @return 0 if success + * @return -1 if failed + */ +int can_get_link_stats(const char *name, struct rtnl_link_stats64 *rls) +{ + return get_link(name, GET_LINK_STATS, rls); +} diff --git a/core/driver/socketcan/libsocketcan/libsocketcan.h b/core/driver/socketcan/libsocketcan/libsocketcan.h new file mode 100644 index 0000000..d5b89a0 --- /dev/null +++ b/core/driver/socketcan/libsocketcan/libsocketcan.h @@ -0,0 +1,61 @@ +/* + * libsocketcan.h + * + * (C) 2009 Luotao Fu + * + * This library is free software; you can redistribute it and/or modify it under + * the terms of the GNU Lesser General Public License as published by the Free + * Software Foundation; either version 2.1 of the License, or (at your option) + * any later version. + * + * This library is distributed in the hope that it will be useful, but without + * any warranty; without even the implied warranty of merchantability or fitness + * for a particular purpose. see the gnu lesser general public license for more + * details. + * + * you should have received a copy of the gnu lesser general public license + * along with this library; if not, write to the free software foundation, inc., + * 59 temple place, suite 330, boston, ma 02111-1307 usa + */ + +#ifndef _socketcan_netlink_h +#define _socketcan_netlink_h + +/** + * @file + * @brief API overview + */ + +#include "can_netlink.h" + +struct rtnl_link_stats64; /* from */ + +#ifdef __cplusplus +extern "C" { +#endif + +int can_do_restart(const char *name); +int can_do_stop(const char *name); +int can_do_start(const char *name); + +int can_set_restart_ms(const char *name, __u32 restart_ms); +int can_set_bittiming(const char *name, struct can_bittiming *bt); +int can_set_ctrlmode(const char *name, struct can_ctrlmode *cm); +int can_set_bitrate(const char *name, __u32 bitrate); +int can_set_bitrate_samplepoint(const char *name, __u32 bitrate, __u32 sample_point); + +int can_get_restart_ms(const char *name, __u32 *restart_ms); +int can_get_bittiming(const char *name, struct can_bittiming *bt); +int can_get_ctrlmode(const char *name, struct can_ctrlmode *cm); +int can_get_state(const char *name, int *state); +int can_get_clock(const char *name, struct can_clock *clock); +int can_get_bittiming_const(const char *name, struct can_bittiming_const *btc); +int can_get_berr_counter(const char *name, struct can_berr_counter *bc); +int can_get_device_stats(const char *name, struct can_device_stats *cds); +int can_get_link_stats(const char *name, struct rtnl_link_stats64 *rls); + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/core/driver/socketcan/socket_can.cpp b/core/driver/socketcan/socket_can.cpp new file mode 100644 index 0000000..2054026 --- /dev/null +++ b/core/driver/socketcan/socket_can.cpp @@ -0,0 +1,187 @@ +#include "socket_can.hpp" +using namespace iflytop; +#define DO(exptr) \ + do { \ + if ((exptr) != 0) { \ + logger->error("do {} failed,{}", #exptr, strerror(errno)); \ + goto error; \ + } \ + } while (0) + +SocketCan::SocketCanError_t SocketCan::sendFrame(shared_ptr frame) { + if (frame == nullptr) { + logger->error("frame is null"); + return kParamError; + } + + canfd_frame_t canframe = frame->getCanFrame(); + + int ret = write(m_socketCanFd, &canframe, canframe.len + 8); + if (ret != (canframe.len + 8)) { + logger->error("write fail,{}", strerror(errno)); + return kWriteError; + } + return kSuccess; +} +SocketCan::SocketCanError_t SocketCan::sendFrame(string framestr) { + struct canfd_frame frame = {0}; + int required_mtu = 0; + + required_mtu = m_socketCanUtils.parse_canframe((char *)framestr.c_str(), &frame); + if (!required_mtu) { + logger->error("Wrong CAN-frame format!"); + return kErrorFrameFormat; + } + + if (required_mtu > (int)CAN_MTU) { + logger->error("CAN-FD frames not supported!"); + return kCANFDNotSupported; + } + + logger->info("send frame:{} {} {} {} {} {}", required_mtu, frame.can_id, frame.len, frame.flags, frame.__res0, frame.__res1); + if (write(m_socketCanFd, &frame, required_mtu) != required_mtu) { + logger->error("write failed,{}", strerror(errno)); + return kWriteError; + } + return kSuccess; +} + +void SocketCan::initialize(shared_ptr socketCanConfig) { // + ZCHECK(socketCanConfig, "socketCanConfig is null"); + m_socketCanConfig = socketCanConfig; + + socketcanInitialize(); +} + +void SocketCan::startListen() { startListenThread(); } + +void SocketCan::socketcanReInitialize() {} + +void SocketCan::startListenThread() { + if (m_thread) { + endListenThread(); + return; + } + logger->info("startListenThread"); + + m_thread.reset(new Thread("SocketCanThread", [this]() { + /** + * @brief call socketCanReadThreadLoop + */ + socketCanReadThreadLoop(); + })); +} +void SocketCan::endListenThread() { + logger->info("endListenThread"); + if (m_thread) { + m_thread->join(); + m_thread = nullptr; + } +} + +void SocketCan::socketcanInitialize() { + // setCanBitrate(canName, bitrate); + + struct ifreq ifr = {0}; + struct sockaddr_can addr = {0}; + can_filter_t zerofilter = {0}; + int recv_own_msgs = 0; /* 0 = disabled (default), 1 = enabled */ + vector filters = m_socketCanConfig->m_canfilters; + + if (!findCan(m_socketCanConfig->m_canName)) { + logger->error("can:{} not found", m_socketCanConfig->m_canName); + return; + } + + if (m_thread) endListenThread(); + if (m_socketCanFd > 0) socketcanDeInitialize(); + + logger->info("socketcanInitialize,canName:{},canBaudrate:{}", m_socketCanConfig->m_canName, m_socketCanConfig->m_canBaudrate); + + // 配置socketCan波特率,注意该方法必须放在socket之前 + setCanBitrate(m_socketCanConfig->m_canName, m_socketCanConfig->m_canBaudrate); + + // SOCKET + m_socketCanFd = socket(PF_CAN, SOCK_RAW, CAN_RAW); + if (m_socketCanFd < 0) { + logger->error("socket failed,{}", strerror(errno)); + goto error; + } + + // 设置依靠名字绑定CAN设备 + strcpy(ifr.ifr_name, m_socketCanConfig->m_canName.c_str()); + DO(ioctl(m_socketCanFd, SIOCGIFINDEX, &ifr)); + + // BIND + addr.can_family = AF_CAN; + addr.can_ifindex = ifr.ifr_ifindex; + DO(bind(m_socketCanFd, (struct sockaddr *)&addr, sizeof(addr))); + + // 设置过滤器 + if (filters.empty()) { + filters.push_back(zerofilter); + } + DO(setsockopt(m_socketCanFd, SOL_CAN_RAW, CAN_RAW_FILTER, &zerofilter, sizeof(zerofilter))); + + recv_own_msgs = 0; /* 0 = disabled (default), 1 = enabled */ + DO(setsockopt(m_socketCanFd, SOL_CAN_RAW, CAN_RAW_RECV_OWN_MSGS, &recv_own_msgs, sizeof(recv_own_msgs))); + + logger->info("socketcan init success"); + + return; +error: + logger->error("socketcan init failed"); + if (m_socketCanFd > 0) { + close(m_socketCanFd); + m_socketCanFd = -1; + } + return; +} +void SocketCan::socketcanDeInitialize() {} + +void SocketCan::canReadThread() {} + +void SocketCan::setCanBitrate(string canName, int bitrate) { + logger->info("set can:{} bitrate:{}", canName, bitrate); + dosystem(fmt::format("ip link set {} down", canName)); + dosystem(fmt::format("ip link set {} type can bitrate {}", canName, bitrate)); + dosystem(fmt::format("ip link set {} up", canName)); +} +bool SocketCan::findCan(string canName) { return access(fmt::format("/sys/class/net/{}", canName).c_str(), F_OK) == 0; } + +void SocketCan::dosystem(string cmd) { + logger->info("do cmd:{}", cmd); + system(cmd.c_str()); +} + +void SocketCan::socketCanReadThreadLoop() { + ThisThread thisThread; + + fd_set readfds, readFileDescriptors; + FD_ZERO(&readFileDescriptors); + FD_SET(m_socketCanFd, &readFileDescriptors); + + while (!thisThread.getExitFlag()) { + canfd_frame_t canframe; + + timeval waitTime; + waitTime.tv_sec = 0; + waitTime.tv_usec = 100000; // 100ms + + readfds = readFileDescriptors; + + int n = select(m_socketCanFd + 1, &readfds, 0, 0, &waitTime); + if (n <= 0) continue; + + memset(&canframe, 0x00, sizeof(canframe)); + int ret = read(m_socketCanFd, &canframe, sizeof(canframe)); + if (ret < 0) { + logger->error("read failed,{}", strerror(errno)); + continue; + } + + shared_ptr socketCanFrame = SocketCanFrame::createFrame(canframe); + logger->debug("RX:{}", socketCanFrame->toString()); + onSocketCanFrame(socketCanFrame); + } +} diff --git a/core/driver/socketcan/socket_can.hpp b/core/driver/socketcan/socket_can.hpp new file mode 100644 index 0000000..762fd27 --- /dev/null +++ b/core/driver/socketcan/socket_can.hpp @@ -0,0 +1,109 @@ +// +// Created by zwsd +// + +#pragma once +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "iflytopcpp/core/spdlogfactory/logger.hpp" +#include "iflytopcpp/core/thread/thread.hpp" +#include "socket_can_frame.hpp" +#include "socket_can_utils.hpp" + +/** + * @brief + * + * service: SocketCan + * + * 监听事件: + * 依赖状态: + * 依赖服务: + * 作用: + * + */ + +namespace iflytop { +using namespace std; +using namespace core; + +class SocketCanConfig { + public: + string m_canName; + int m_canBaudrate; + vector m_canfilters; +}; + +class SocketCanDeviceState { + public: +}; + +class SocketCan : public enable_shared_from_this { + ENABLE_LOGGER(SocketCan); + + unique_ptr m_thread; + unique_ptr m_monitorThread; + int m_socketCanFd = -1; + bool m_startListen = false; + SocketCanUtils m_socketCanUtils; + + shared_ptr m_socketCanConfig; + + public: + typedef enum { + kSuccess, + // Wrong CANFrame format! + kErrorFrameFormat, + // CAN-FD frames not supported! + kCANFDNotSupported, + kWriteError, + kParamError, + } SocketCanError_t; + + public: + nod::signal)> onSocketCanFrame; + + SocketCan(){}; + + void initialize(shared_ptr socketCanConfig); + + void startListen(); + + SocketCanError_t sendFrame(shared_ptr frame); + /** + * @brief 发送can帧 + * + * @param frame + * frame="11111111#1122334455667788" ID=0x11111111,帧类型=数据帧,帧格式=扩展帧,数据长度=8,数据=1122334455667788 + * frame="111#1122334455667788" ID=0x111, 帧类型=数据帧,帧格式=标准帧,数据长度=8,数据=1122334455667788 + * frame="111#R" ID=0x111, 帧类型=远程帧,帧格式=标准帧,数据长度=0 + * frame="11111111#R" ID=0x11111111,帧类型=远程帧,帧格式=扩展帧,数据长度=0 + * @return int + */ + SocketCanError_t sendFrame(string frame); + + private: + void socketcanInitialize(); + void socketcanDeInitialize(); + void socketcanReInitialize(); + + void startListenThread(); + void endListenThread(); + void socketCanReadThreadLoop(); + + private: + void dosystem(string cmd); + void setCanBitrate(string canName, int bitrate); + bool findCan(string canName); + + private: + void canReadThread(); +}; +} // namespace iflytop \ No newline at end of file diff --git a/core/driver/socketcan/socket_can_frame.cpp b/core/driver/socketcan/socket_can_frame.cpp new file mode 100644 index 0000000..339698a --- /dev/null +++ b/core/driver/socketcan/socket_can_frame.cpp @@ -0,0 +1,88 @@ +#include "socket_can_frame.hpp" + +#include "iflytopcpp/core/components/stringutils.hpp" +using namespace iflytop; + +shared_ptr SocketCanFrame::createExtDataFrame(uint32_t id, uint8_t *data, size_t len) { + shared_ptr canframe = make_shared(); + if (len > 8) len = 8; + canframe->m_id = id; + canframe->m_canIdentifier = kextFrame; + canframe->m_canFrameType = kdataframe; + canframe->m_dlc = len; + memcpy(canframe->m_data, data, len); + return canframe; +} + +shared_ptr SocketCanFrame::createStdDataFrame(uint32_t id, uint8_t *data, size_t len) { + shared_ptr canframe = make_shared(); + if (len > 8) len = 8; + canframe->m_id = id; + canframe->m_canIdentifier = kstdFrame; + canframe->m_canFrameType = kdataframe; + canframe->m_dlc = len; + memcpy(canframe->m_data, data, len); + return canframe; +} + +shared_ptr SocketCanFrame::createExtRemoteFrame(uint32_t id) { + shared_ptr canframe = make_shared(); + canframe->m_id = id; + canframe->m_canIdentifier = kextFrame; + canframe->m_canFrameType = kremoteframe; + canframe->m_dlc = 0; + return canframe; +} +shared_ptr SocketCanFrame::createStdRemoteFrame(uint32_t id) { + shared_ptr canframe = make_shared(); + canframe->m_id = id; + canframe->m_canIdentifier = kstdFrame; + canframe->m_canFrameType = kremoteframe; + canframe->m_dlc = 0; + return canframe; +} +shared_ptr SocketCanFrame::createFrame(canfd_frame_t frame) { + shared_ptr canframe = make_shared(); + canframe->m_id = frame.can_id & ~CAN_EFF_FLAG & ~CAN_RTR_FLAG & ~CAN_ERR_FLAG; + canframe->m_canIdentifier = frame.can_id & CAN_EFF_FLAG ? kextFrame : kstdFrame; + canframe->m_canFrameType = frame.can_id & CAN_RTR_FLAG ? kremoteframe : kdataframe; + canframe->m_dlc = frame.len; + + memcpy(canframe->m_data, frame.data, frame.len); + return canframe; +} + +void SocketCanFrame::setCanIdentifier(can_identifier_t canIdentifier) { this->m_canIdentifier = canIdentifier; } +void SocketCanFrame::setFanFrameType(can_frame_type_t fanFrameType) { this->m_canFrameType = fanFrameType; } +void SocketCanFrame::setId(uint32_t id) { this->m_id = id; } + +uint32_t SocketCanFrame::getId() { return m_id; } +can_identifier_t SocketCanFrame::getCanIdentifier() { return m_canIdentifier; } +can_frame_type_t SocketCanFrame::getFanFrameType() { return m_canFrameType; } + +string SocketCanFrame::toString() { // + return fmt::format("|{:0>8x}|{:>6}|{}|{}({})|", getId(), // + getFanFrameType() == kdataframe ? "data" : "remote", // + getCanIdentifier() == kextFrame ? "ext" : "std", // + StringUtils().bytesToString(m_data, m_dlc), m_dlc); +} + +canfd_frame_t SocketCanFrame::getCanFrame() { + canfd_frame_t frame; + memset(&frame, 0, sizeof(frame)); + + if (m_canIdentifier == kstdFrame) { + frame.can_id = (m_id & CAN_SFF_MASK); + } else { + frame.can_id = (m_id & CAN_EFF_MASK) | CAN_EFF_FLAG; + } + + if (m_canFrameType == kremoteframe) { + frame.can_id |= CAN_RTR_FLAG; + frame.len = m_dlc; + } else { + frame.len = m_dlc; + memcpy(frame.data, m_data, m_dlc); + } + return frame; +} \ No newline at end of file diff --git a/core/driver/socketcan/socket_can_frame.hpp b/core/driver/socketcan/socket_can_frame.hpp new file mode 100644 index 0000000..b475506 --- /dev/null +++ b/core/driver/socketcan/socket_can_frame.hpp @@ -0,0 +1,74 @@ +#pragma once +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "iflytopcpp/core/basic/nod/nod.hpp" +#include "iflytopcpp/core/spdlogfactory/logger.hpp" +#include "iflytopcpp/core/thread/thread.hpp" + +namespace iflytop { +using namespace std; +using namespace core; + +/******************************************************************************* + * CAN_FRAME * + *******************************************************************************/ + +typedef struct can_filter can_filter_t; +typedef struct canfd_frame canfd_frame_t; + +typedef enum { + kstdFrame, + kextFrame, +} can_identifier_t; + +typedef enum { + kdataframe, + kremoteframe, +} can_frame_type_t; + +class SocketCanFrame { + private: + uint32_t m_id = 0; + can_identifier_t m_canIdentifier = kstdFrame; + can_frame_type_t m_canFrameType = kdataframe; + uint8_t m_data[8] = {0}; + uint8_t m_dlc = 0; + + public: + static shared_ptr createExtDataFrame(uint32_t id, uint8_t *data, size_t len); + static shared_ptr createStdDataFrame(uint32_t id, uint8_t *data, size_t len); + static shared_ptr createExtRemoteFrame(uint32_t id); + static shared_ptr createStdRemoteFrame(uint32_t id); + static shared_ptr createFrame(canfd_frame_t frame); + + void setId(uint32_t id); + uint32_t getId(); + can_frame_type_t getFanFrameType(); + void setFanFrameType(can_frame_type_t fanFrameType); + can_identifier_t getCanIdentifier(); + void setCanIdentifier(can_identifier_t canIdentifier); + + canfd_frame_t getCanFrame(); + + string toString(); +}; + +} // namespace iflytop diff --git a/core/driver/socketcan/socket_can_utils.cpp b/core/driver/socketcan/socket_can_utils.cpp new file mode 100644 index 0000000..0928af2 --- /dev/null +++ b/core/driver/socketcan/socket_can_utils.cpp @@ -0,0 +1,114 @@ +#include "socket_can_utils.hpp" +using namespace iflytop; +using namespace std; + +using namespace iflytop; +#define CANID_DELIM '#' +#define CC_DLC_DELIM '_' +#define DATA_SEPERATOR '.' +#define CAN_MAX_DLC 8 +#define CAN_MAX_RAW_DLC 15 +#define CAN_MAX_DLEN 8 +static 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 */ +} + +uint16_t SocketCanUtils::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; + canid_t tmp; + + len = strlen(cs); + // printf("'%s' len %d\n", cs, len); + + 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_DLEN) { + cf->len = tmp; + + /* check for optional raw DLC value for CAN 2.0B frames */ + if ((tmp == CAN_MAX_DLEN) && (cs[idx++] == CC_DLC_DELIM)) { + tmp = asc2nibble(cs[idx]); + if ((tmp > CAN_MAX_DLEN) && (tmp <= CAN_MAX_RAW_DLC)) { + struct can_frame *ccf = (struct can_frame *)cf; + + ccf->__res1 = tmp; + } + } + } + return ret; + } + + if (cs[idx] == CANID_DELIM) { /* CAN FD frame escape char '##' */ + + maxdlen = CANFD_MAX_DLEN; + ret = CANFD_MTU; + + /* CAN FD frame ##* */ + 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; + + /* check for extra DLC when having a Classic CAN with 8 bytes payload */ + if ((maxdlen == CAN_MAX_DLEN) && (dlen == CAN_MAX_DLEN) && (cs[idx++] == CC_DLC_DELIM)) { + unsigned char dlc = asc2nibble(cs[idx]); + + if ((dlc > CAN_MAX_DLEN) && (dlc <= CAN_MAX_RAW_DLC)) { + struct can_frame *ccf = (struct can_frame *)cf; + + ccf->__res1 = dlc; + } + } + + return ret; +} \ No newline at end of file diff --git a/core/driver/socketcan/socket_can_utils.hpp b/core/driver/socketcan/socket_can_utils.hpp new file mode 100644 index 0000000..28451e5 --- /dev/null +++ b/core/driver/socketcan/socket_can_utils.hpp @@ -0,0 +1,44 @@ +#pragma once +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "iflytopcpp/core/spdlogfactory/logger.hpp" +#include "iflytopcpp/core/thread/thread.hpp" + +namespace iflytop { +using namespace std; +class SocketCanUtils { + ENABLE_LOGGER(SocketCanUtils); + + public: + /** + * @brief 解析canframe + * + * @param frame + * frame="11111111#1122334455667788" ID=0x11111111,帧类型=数据帧,帧格式=扩展帧,数据长度=8,数据=1122334455667788 + * frame="111#1122334455667788" ID=0x111, 帧类型=数据帧,帧格式=标准帧,数据长度=8,数据=1122334455667788 + * frame="111#R" ID=0x111, 帧类型=远程帧,帧格式=标准帧,数据长度=0 + * frame="11111111#R" ID=0x11111111,帧类型=远程帧,帧格式=扩展帧,数据长度=0 + * @return uint16_t 要发送的数据长度 + */ + uint16_t parse_canframe(char *cs, struct canfd_frame *cf); +}; +} // namespace iflytop \ No newline at end of file diff --git a/core/driver/socketcan/socketcan.cpp b/core/driver/socketcan/socketcan.cpp deleted file mode 100644 index e69de29..0000000 diff --git a/core/driver/socketcan/socketcan.hpp b/core/driver/socketcan/socketcan.hpp deleted file mode 100644 index e69de29..0000000 diff --git a/core/driver/socketcan/socketcan_frame.hpp b/core/driver/socketcan/socketcan_frame.hpp deleted file mode 100644 index e69de29..0000000 diff --git a/module.cmake b/module.cmake index 19e9a26..fedc23e 100644 --- a/module.cmake +++ b/module.cmake @@ -26,9 +26,15 @@ set(DEP_SRC dep/iflytopcpp/core/components/process/process_unix.cpp dep/iflytopcpp/core/components/process/process.cpp dep/iflytopcpp/core/components/timer/simple_timer.cpp - dep/iflytopcpp/core/driver/sockcanpp/CanDriver.cpp # - dep/iflytopcpp/core/components/audio/wav_recorder.cpp) + dep/iflytopcpp/core/components/audio/wav_recorder.cpp + # driver socket_can + dep/iflytopcpp/core/driver/socketcan/libsocketcan/libsocketcan.c + dep/iflytopcpp/core/driver/socketcan/socket_can_frame.cpp + dep/iflytopcpp/core/driver/socketcan/socket_can_utils.cpp + dep/iflytopcpp/core/driver/socketcan/socket_can.cpp + # +) set(DEP_DEFINE ${DEP_DEFINE}) set(PUBLIC_INCLUDE_DIRECTORIES ${PUBLIC_INCLUDE_DIRECTORIES} ./dep/iflytopcpp/core/spdlog/include/)