git commit -m "first commit for v2"
This commit is contained in:
431
Devices/Libraries/Systems/can-utils/libj1939.c
Executable file
431
Devices/Libraries/Systems/can-utils/libj1939.c
Executable file
@@ -0,0 +1,431 @@
|
||||
/* SPDX-License-Identifier: GPL-2.0-only */
|
||||
/*
|
||||
* Copyright (c) 2011 EIA Electronics
|
||||
*
|
||||
* Authors:
|
||||
* Kurt Van Dijck <kurt.van.dijck@eia.be>
|
||||
*
|
||||
* 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
|
||||
*/
|
||||
|
||||
#include <err.h>
|
||||
#include <errno.h>
|
||||
#include <fcntl.h>
|
||||
#include <inttypes.h>
|
||||
#include <limits.h>
|
||||
#include <linux/kernel.h>
|
||||
#include <net/if.h>
|
||||
#include <net/if.h>
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include <sys/epoll.h>
|
||||
#include <sys/ioctl.h>
|
||||
#include <sys/socket.h>
|
||||
#include <time.h>
|
||||
#include <unistd.h>
|
||||
|
||||
#include "libj1939.h"
|
||||
#include "lib.h"
|
||||
|
||||
/* static data */
|
||||
static struct if_nameindex *saved;
|
||||
|
||||
__attribute__((destructor))
|
||||
static void libj1939_cleanup(void)
|
||||
{
|
||||
if (saved)
|
||||
if_freenameindex(saved);
|
||||
saved = 0;
|
||||
}
|
||||
|
||||
static inline void fetch_names(void)
|
||||
{
|
||||
if (!saved) {
|
||||
saved = if_nameindex();
|
||||
if (!saved)
|
||||
err(1, "if_nameindex()");
|
||||
}
|
||||
}
|
||||
|
||||
/* retrieve name */
|
||||
static const char *libj1939_ifnam(int ifindex)
|
||||
{
|
||||
const struct if_nameindex *lp, *cached = saved;
|
||||
|
||||
fetch_names();
|
||||
|
||||
for (lp = saved; lp->if_index; ++lp) {
|
||||
if (lp->if_index == (unsigned int)ifindex)
|
||||
return lp->if_name;
|
||||
}
|
||||
if (cached) {
|
||||
/*
|
||||
* the list was not recent
|
||||
* iterate twice, but force a refresh now
|
||||
* recursion stops since the 'saved' pointer is cleaned
|
||||
*/
|
||||
libj1939_cleanup();
|
||||
return libj1939_ifnam(ifindex);
|
||||
}
|
||||
return NULL;
|
||||
}
|
||||
|
||||
/* retrieve index */
|
||||
static int libj1939_ifindex(const char *str)
|
||||
{
|
||||
const struct if_nameindex *lp, *cached = saved;
|
||||
char *endp;
|
||||
int ret;
|
||||
|
||||
ret = strtol(str, &endp, 0);
|
||||
if (!*endp)
|
||||
/* did some good parse */
|
||||
return ret;
|
||||
|
||||
fetch_names();
|
||||
for (lp = saved; lp->if_index; ++lp) {
|
||||
if (!strcmp(lp->if_name, str))
|
||||
return lp->if_index;
|
||||
}
|
||||
if (cached) {
|
||||
libj1939_cleanup();
|
||||
return libj1939_ifindex(str);
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
void libj1939_parse_canaddr(char *spec, struct sockaddr_can *paddr)
|
||||
{
|
||||
char *str;
|
||||
|
||||
str = strsep(&spec, ":");
|
||||
if (strlen(str))
|
||||
paddr->can_ifindex = if_nametoindex(str);
|
||||
|
||||
str = strsep(&spec, ",");
|
||||
if (str && strlen(str))
|
||||
paddr->can_addr.j1939.addr = strtoul(str, NULL, 0);
|
||||
|
||||
str = strsep(&spec, ",");
|
||||
if (str && strlen(str))
|
||||
paddr->can_addr.j1939.pgn = strtoul(str, NULL, 0);
|
||||
|
||||
str = strsep(&spec, ",");
|
||||
if (str && strlen(str))
|
||||
paddr->can_addr.j1939.name = strtoull(str, NULL, 0);
|
||||
}
|
||||
|
||||
int libj1939_str2addr(const char *str, char **endp, struct sockaddr_can *can)
|
||||
{
|
||||
char *p;
|
||||
const char *pstr;
|
||||
uint64_t tmp64;
|
||||
unsigned long tmp;
|
||||
|
||||
if (!endp)
|
||||
endp = &p;
|
||||
memset(can, 0, sizeof(*can));
|
||||
can->can_family = AF_CAN;
|
||||
can->can_addr.j1939.name = J1939_NO_NAME;
|
||||
can->can_addr.j1939.addr = J1939_NO_ADDR;
|
||||
can->can_addr.j1939.pgn = J1939_NO_PGN;
|
||||
|
||||
pstr = strchr(str, ':');
|
||||
if (pstr) {
|
||||
char tmp[IFNAMSIZ];
|
||||
if ((pstr - str) >= IFNAMSIZ)
|
||||
return -1;
|
||||
strncpy(tmp, str, pstr - str);
|
||||
tmp[pstr - str] = 0;
|
||||
can->can_ifindex = libj1939_ifindex(tmp);
|
||||
} else {
|
||||
can->can_ifindex = libj1939_ifindex(str);
|
||||
if (can->can_ifindex) {
|
||||
if (endp)
|
||||
*endp = (char *)&str[strlen(str)];
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
if (pstr)
|
||||
++pstr;
|
||||
else
|
||||
pstr = str;
|
||||
|
||||
|
||||
tmp64 = strtoull(pstr, endp, 16);
|
||||
if (*endp <= pstr)
|
||||
return 0;
|
||||
if ((*endp - pstr) == 2)
|
||||
can->can_addr.j1939.addr = tmp64;
|
||||
else
|
||||
can->can_addr.j1939.name = tmp64;
|
||||
if (!**endp)
|
||||
return 0;
|
||||
|
||||
str = *endp + 1;
|
||||
tmp = strtoul(str, endp, 16);
|
||||
if (*endp > str)
|
||||
can->can_addr.j1939.pgn = tmp;
|
||||
return 0;
|
||||
}
|
||||
|
||||
const char *libj1939_addr2str(const struct sockaddr_can *can)
|
||||
{
|
||||
char *str;
|
||||
static char buf[128];
|
||||
|
||||
str = buf;
|
||||
if (can->can_ifindex) {
|
||||
const char *ifname;
|
||||
ifname = libj1939_ifnam(can->can_ifindex);
|
||||
if (!ifname)
|
||||
str += sprintf(str, "#%i:", can->can_ifindex);
|
||||
else
|
||||
str += sprintf(str, "%s:", ifname);
|
||||
}
|
||||
if (can->can_addr.j1939.name) {
|
||||
str += sprintf(str, "%016llx", (unsigned long long)can->can_addr.j1939.name);
|
||||
if (can->can_addr.j1939.pgn == J1939_PGN_ADDRESS_CLAIMED)
|
||||
str += sprintf(str, ".%02x", can->can_addr.j1939.addr);
|
||||
} else if (can->can_addr.j1939.addr <= 0xfe)
|
||||
str += sprintf(str, "%02x", can->can_addr.j1939.addr);
|
||||
else
|
||||
str += sprintf(str, "-");
|
||||
if (can->can_addr.j1939.pgn <= J1939_PGN_MAX)
|
||||
str += sprintf(str, ",%05x", can->can_addr.j1939.pgn);
|
||||
|
||||
return buf;
|
||||
}
|
||||
|
||||
void libj1939_init_sockaddr_can(struct sockaddr_can *sac, uint32_t pgn)
|
||||
{
|
||||
sac->can_family = AF_CAN;
|
||||
sac->can_addr.j1939.addr = J1939_NO_ADDR;
|
||||
sac->can_addr.j1939.name = J1939_NO_NAME;
|
||||
sac->can_addr.j1939.pgn = pgn;
|
||||
}
|
||||
|
||||
/**
|
||||
* libj1939_open_socket - Open a new J1939 socket
|
||||
*
|
||||
* This function opens a new J1939 socket.
|
||||
*
|
||||
* Return: The file descriptor of the new socket, or a negative error code.
|
||||
*/
|
||||
int libj1939_open_socket(void)
|
||||
{
|
||||
int ret;
|
||||
|
||||
/* Create a new CAN J1939 socket */
|
||||
ret = socket(PF_CAN, SOCK_DGRAM, CAN_J1939);
|
||||
if (ret < 0) {
|
||||
/* Get the error code and print an error message */
|
||||
ret = -errno;
|
||||
pr_err("socket(j1939): %d (%s)", ret, strerror(ret));
|
||||
return ret;
|
||||
}
|
||||
return ret;
|
||||
}
|
||||
|
||||
/**
|
||||
* libj1939_bind_socket - Bind a J1939 socket to a specific address
|
||||
* @sock: The file descriptor of the socket
|
||||
* @addr: The address to bind to
|
||||
*
|
||||
* This function binds a J1939 socket to a specific address.
|
||||
*
|
||||
* Return: 0 on success, or a negative error code.
|
||||
*/
|
||||
int libj1939_bind_socket(int sock, struct sockaddr_can *addr)
|
||||
{
|
||||
int ret;
|
||||
|
||||
ret = bind(sock, (void *)addr, sizeof(*addr));
|
||||
if (ret < 0) {
|
||||
ret = -errno;
|
||||
pr_err("failed to bind: %d (%s)", ret, strerror(ret));
|
||||
return ret;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* libj1939_socket_prio - Set the priority of a J1939 socket
|
||||
* @sock: The file descriptor of the socket
|
||||
* @prio: The priority to set
|
||||
*
|
||||
* This function sets the priority of a J1939 socket.
|
||||
*
|
||||
* Return: 0 on success, or a negative error code.
|
||||
*/
|
||||
int libj1939_socket_prio(int sock, int prio)
|
||||
{
|
||||
int ret;
|
||||
|
||||
ret = setsockopt(sock, SOL_CAN_J1939, SO_J1939_SEND_PRIO,
|
||||
&prio, sizeof(prio));
|
||||
if (ret < 0) {
|
||||
ret = -errno;
|
||||
pr_warn("Failed to set priority %i. Error %i (%s)", prio, ret,
|
||||
strerror(ret));
|
||||
return ret;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* libj1939_set_broadcast - Enable broadcast on a J1939 socket
|
||||
* @sock: The file descriptor of the socket
|
||||
*
|
||||
* This function enables broadcast on a J1939 socket.
|
||||
*
|
||||
* Return: 0 on success, or a negative error code.
|
||||
*/
|
||||
int libj1939_set_broadcast(int sock)
|
||||
{
|
||||
int broadcast = true;
|
||||
int ret;
|
||||
|
||||
ret = setsockopt(sock, SOL_SOCKET, SO_BROADCAST, &broadcast,
|
||||
sizeof(broadcast));
|
||||
if (ret < 0) {
|
||||
ret = -errno;
|
||||
pr_err("setsockopt(SO_BROADCAST): %d (%s)", ret, strerror(ret));
|
||||
return ret;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* libj1939_add_socket_to_epoll - Add a socket to an epoll instance
|
||||
* @epoll_fd: The file descriptor of the epoll instance
|
||||
* @sock: The file descriptor of the socket
|
||||
* @events: The events to monitor
|
||||
*
|
||||
* This function adds a socket to an epoll instance.
|
||||
*
|
||||
* Return: 0 on success, or a negative error code.
|
||||
*/
|
||||
int libj1939_add_socket_to_epoll(int epoll_fd, int sock, uint32_t events)
|
||||
{
|
||||
struct epoll_event ev = {0};
|
||||
int ret;
|
||||
|
||||
ev.events = events;
|
||||
ev.data.fd = sock;
|
||||
|
||||
ret = epoll_ctl(epoll_fd, EPOLL_CTL_ADD, sock, &ev);
|
||||
if (ret < 0) {
|
||||
ret = errno;
|
||||
pr_err("epoll_ctl(EPOLL_CTL_ADD): %d (%s)", ret, strerror(ret));
|
||||
return ret;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* libj1939_create_epoll - Create a new epoll instance
|
||||
*
|
||||
* This function creates a new epoll instance.
|
||||
*
|
||||
* Return: The file descriptor of the new epoll instance, or a negative error
|
||||
* code.
|
||||
*/
|
||||
int libj1939_create_epoll(void)
|
||||
{
|
||||
int ret, epoll_fd;
|
||||
|
||||
epoll_fd = epoll_create1(0);
|
||||
if (epoll_fd < 0) {
|
||||
ret = -errno;
|
||||
pr_err("epoll_create1: %d (%s)", ret, strerror(ret));
|
||||
return ret;
|
||||
}
|
||||
|
||||
return epoll_fd;
|
||||
}
|
||||
|
||||
/**
|
||||
* libj1939_get_timeout_ms - Get the timeout in milliseconds until a specific
|
||||
* time
|
||||
* @ts: The time to wait for
|
||||
* @return: The timeout in milliseconds until the specified time
|
||||
*
|
||||
* This function calculates the timeout in milliseconds until a specific time.
|
||||
*
|
||||
* Return: The timeout in milliseconds until the specified time.
|
||||
*/
|
||||
static int libj1939_get_timeout_ms(struct timespec *ts)
|
||||
{
|
||||
struct timespec curr_time;
|
||||
int64_t time_diff;
|
||||
int timeout_ms;
|
||||
|
||||
clock_gettime(CLOCK_MONOTONIC, &curr_time);
|
||||
time_diff = timespec_diff_ms(ts, &curr_time);
|
||||
if (time_diff < 0) {
|
||||
/* Too late to send next message. Send it now */
|
||||
timeout_ms = 0;
|
||||
} else {
|
||||
if (time_diff > INT_MAX) {
|
||||
pr_warn("timeout too long: %" PRId64 " ms", time_diff);
|
||||
time_diff = INT_MAX;
|
||||
}
|
||||
|
||||
timeout_ms = time_diff;
|
||||
}
|
||||
|
||||
return timeout_ms;
|
||||
}
|
||||
|
||||
/**
|
||||
* libj1939_prepare_for_events - Prepare and wait for events on an epoll
|
||||
* @cmn: The common J1939 instance data
|
||||
* @nfds: The number of file descriptors that are ready
|
||||
* @dont_wait: Don't wait for events, just check if there are any
|
||||
*
|
||||
* This function calculates the timeout until the next message should be sent
|
||||
* or any other event should be handled, prepares the epoll instance for events
|
||||
* by waiting for the specified timeout or until an event occurs, and waits for
|
||||
* events on the epoll instance.
|
||||
*
|
||||
* Return: 0 on success, or a negative error code.
|
||||
*/
|
||||
int libj1939_prepare_for_events(struct libj1939_cmn *cmn, int *nfds,
|
||||
bool dont_wait)
|
||||
{
|
||||
int ret, timeout_ms;
|
||||
|
||||
if (dont_wait)
|
||||
timeout_ms = 0;
|
||||
else
|
||||
timeout_ms = libj1939_get_timeout_ms(&cmn->next_send_time);
|
||||
|
||||
ret = epoll_wait(cmn->epoll_fd, cmn->epoll_events,
|
||||
cmn->epoll_events_size, timeout_ms);
|
||||
if (ret < 0) {
|
||||
ret = -errno;
|
||||
if (ret != -EINTR) {
|
||||
*nfds = 0;
|
||||
return ret;
|
||||
}
|
||||
}
|
||||
|
||||
*nfds = ret;
|
||||
|
||||
ret = clock_gettime(CLOCK_MONOTONIC, &cmn->last_time);
|
||||
if (ret < 0) {
|
||||
ret = -errno;
|
||||
pr_err("failed to get time: %i (%s)", ret, strerror(ret));
|
||||
return ret;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
Reference in New Issue
Block a user