can-j1939: add libj1939

libj1939 provides a parser for struct sockaddr_can with j1939 info

Signed-off-by: Kurt Van Dijck <kurt.van.dijck@eia.be>
Signed-off-by: Marc Kleine-Budde <mkl@pengutronix.de>

GNUmakefile.am

@@ -12,6 +12,7 @@ LDADD = \
 noinst_HEADERS = \
 	canframelen.h \
 	lib.h \
+	libj1939.h \
 	terminal.h \
 	include/linux/can/bcm.h \
 	include/linux/can/error.h \
@@ -25,12 +26,15 @@ noinst_HEADERS = \
 
 
 noinst_LTLIBRARIES = \
-	libcan.la
+	libcan.la \
+	libj1939.la
 
 libcan_la_SOURCES = \
 	lib.c \
 	canframelen.c
 
+libj1939_la_SOURCES = \
+	libj1939.c
 
 bin_PROGRAMS = \
 	asc2log \

Makefile

@@ -73,7 +73,7 @@ install:
 	cp -f $(PROGRAMS) $(DESTDIR)$(PREFIX)/bin
 
 distclean:
-	rm -f $(PROGRAMS) *.o *~
+	rm -f $(PROGRAMS) $(LIBRARIES) *.o *~
 
 cansend.o:	lib.h
 cangen.o:	lib.h

libj1939.c

@@ -0,0 +1,176 @@
+/*
+ * 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 <string.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <errno.h>
+#include <inttypes.h>
+
+#include <error.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <net/if.h>
+#include <sys/ioctl.h>
+
+#include <net/if.h>
+
+#include "libj1939.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)
+			error(1, errno, "if_nameindex()");
+	}
+}
+
+/* retrieve name */
+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 == 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);
+	} else
+		return NULL;
+}
+
+/* retrieve index */
+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);
+	} else
+		return 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 == 0x0ee00)
+			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 <= 0x3ffff)
+		str += sprintf(str, ",%05x", can->can_addr.j1939.pgn);
+
+	return buf;
+}
+

libj1939.h

@@ -0,0 +1,19 @@
+#include <sys/socket.h>
+#include <linux/can.h>
+#include <linux/can/j1939.h>
+
+#ifndef J1939_LIB_H
+#define J1939_LIB_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+extern int libj1939_str2addr(const char *str, char **endp, struct sockaddr_can *can);
+extern const char *libj1939_addr2str(const struct sockaddr_can *can);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif