diff options
| -rw-r--r-- | cethcan/Makefile | 2 | ||||
| -rw-r--r-- | cethcan/can.c | 10 | ||||
| -rw-r--r-- | cethcan/cethcan.h | 12 | ||||
| -rw-r--r-- | cethcan/ether.c | 80 | ||||
| -rw-r--r-- | cethcan/main.c | 9 |
5 files changed, 110 insertions, 3 deletions
diff --git a/cethcan/Makefile b/cethcan/Makefile index 33b2069..a81124c 100644 --- a/cethcan/Makefile +++ b/cethcan/Makefile @@ -3,7 +3,7 @@ love: cethcan PKGS="libevent jansson" -cethcan: main.o can.o +cethcan: main.o can.o ether.o gcc -g -o $@ `pkg-config --libs $(PKGS)` $^ clean: diff --git a/cethcan/can.c b/cethcan/can.c index be8c635..4a61756 100644 --- a/cethcan/can.c +++ b/cethcan/can.c @@ -32,8 +32,18 @@ struct can_user *can_register_alloc(void *arg, can_handler handler, void can_broadcast(struct can_user *origin, struct can_message *msg) { struct can_user *u; + char buf[3*8+1]; msg->origin = origin; + + if (msg->dlc > 8) { + lprintf("invalid CAN message (DLC = %zu)", msg->dlc); + return; + } + for (size_t i = 0; i < msg->dlc; i++) + sprintf(buf + 3 * i, " %02x", msg->bytes[i]); + lprintf("%08x (%zu)%s", (unsigned)msg->daddr, msg->dlc, buf); + for (u = users; u; u = u->next) if (u != origin) u->handler(u->arg, msg); diff --git a/cethcan/cethcan.h b/cethcan/cethcan.h index 1ac898f..477221e 100644 --- a/cethcan/cethcan.h +++ b/cethcan/cethcan.h @@ -23,8 +23,16 @@ #include <event2/event.h> #include <jansson.h> +#define lprintf(...) do { \ + struct timeval tv; struct tm tm; char tvbuf[64]; \ + gettimeofday(&tv, NULL); localtime_r(&tv.tv_sec, &tm); \ + strftime(tvbuf, sizeof(tvbuf), "%Y-%m-%d %H:%M:%S", &tm); \ + fprintf(stderr, "%s.%03d ", tvbuf, tv.tv_usec / 1000); \ + fprintf(stderr, __VA_ARGS__); \ + fprintf(stderr, "\n"); \ + } while (0) + extern struct event_base *ev_base; -extern json_t *config; struct can_user; @@ -57,4 +65,6 @@ extern struct can_user *can_register_alloc(void *arg, can_handler handler, extern void can_broadcast(struct can_user *origin, struct can_message *msg); extern void can_init(void); +extern int ether_init(json_t *config); + #endif /* _CETHCAN_H */ diff --git a/cethcan/ether.c b/cethcan/ether.c new file mode 100644 index 0000000..5daac2e --- /dev/null +++ b/cethcan/ether.c @@ -0,0 +1,80 @@ +#include "cethcan.h" + +#include <net/if.h> +#include <net/ethernet.h> +#include <netpacket/packet.h> + +#define ETHER_PROTO 0x88b7 +#define ETHER_MCADDR 0xff, 0x3a, 0xf6, 'C', 'A', 'N' + +struct ether { + struct can_user *u; + struct event *ev; + + char *ifname; + int ifindex; + + int sock; +}; + +static void ether_can_handler(void *arg, struct can_message *msg) +{ +} + +static void ether_sock_handler(int sock, short event, void *arg) +{ +} + +int ether_init(json_t *config) +{ + struct ether *e; + const char *iface; + int ifindex; + struct packet_mreq pmr = { + .mr_type = PACKET_MR_MULTICAST, + .mr_alen = ETH_ALEN, + .mr_address = { ETHER_MCADDR }, + }; + + if (!json_is_object(config)) { + lprintf("ethernet config must be an object/dictionary"); + return 1; + } + if (!json_is_string(json_object_get(config, "interface"))) { + lprintf("ethernet config must have an 'interface' key"); + return 1; + } + iface = json_string_value(json_object_get(config, "interface")); + ifindex = if_nametoindex(iface); + + if (ifindex == 0) { + lprintf("ethernet interface '%s' error: %s", + iface, strerror(errno)); + return 1; + } + + e = calloc(sizeof(*e), 1); + e->ifname = strdup(iface); + e->ifindex = ifindex; + + e->sock = socket(AF_PACKET, SOCK_RAW, htons(ETHER_PROTO)); + if (e->sock == -1) { + lprintf("ethernet interface '%s' socket() error: %s", + iface, strerror(errno)); + return 1; + } + + pmr.mr_ifindex = ifindex; + if (setsockopt(e->sock, SOL_PACKET, PACKET_ADD_MEMBERSHIP, + &pmr, sizeof(pmr))) { + lprintf("ethernet interface '%s' multicast join error: %s", + iface, strerror(errno)); + return 1; + } + + e->u = can_register_alloc(e, ether_can_handler, "ether[%s]", iface); + e->ev = event_new(ev_base, e->sock, EV_READ | EV_PERSIST, ether_sock_handler, e); + event_add(e->ev, NULL); + + return 0; +} diff --git a/cethcan/main.c b/cethcan/main.c index 938721e..6d79a9f 100644 --- a/cethcan/main.c +++ b/cethcan/main.c @@ -1,13 +1,13 @@ #include "cethcan.h" struct event_base *ev_base; -json_t *config; int main(int argc, char **argv) { int optch = 0; const char *cfgfile = "cethcan.json"; json_error_t je; + json_t *config, *ethercfg; do { optch = getopt(argc, argv, "c:"); @@ -40,6 +40,13 @@ int main(int argc, char **argv) can_init(); + ethercfg = json_object_get(config, "ethernet"); + for (size_t i = 0; i < json_array_size(ethercfg); i++) { + json_t *c = json_array_get(ethercfg, i); + if (ether_init(c)) + return 1; + } + event_base_loop(ev_base, 0); return 0; } |