diff options
Diffstat (limited to 'cethcan/ether.c')
| -rw-r--r-- | cethcan/ether.c | 157 |
1 files changed, 157 insertions, 0 deletions
diff --git a/cethcan/ether.c b/cethcan/ether.c new file mode 100644 index 0000000..5b935ed --- /dev/null +++ b/cethcan/ether.c @@ -0,0 +1,157 @@ +#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; +}; + +struct frame { + uint8_t daddr[ETH_ALEN]; + uint8_t saddr[ETH_ALEN]; + uint16_t proto; + uint8_t oui[3]; + uint16_t protoid; + uint8_t protover; + uint32_t ts1, ts2; + uint32_t can_daddr; + uint8_t can_dlc; + uint8_t can_data[8]; +} __attribute__((packed)); + + +static void ether_can_handler(void *arg, struct can_message *msg) +{ + struct ether *e = arg; + lprintf("%s: TX not implemented", e->u->name); +} + +static void ether_rx_dataframe(struct ether *e, struct frame *f) +{ + struct can_message msg; + + msg.daddr = ntohl(f->can_daddr); + msg.dlc = f->can_dlc; + if (msg.dlc > 8) + msg.dlc = 8; + memcpy(msg.bytes, f->can_data, msg.dlc); + + can_broadcast(e->u, &msg); +} + +static void ether_sock_handler(int sock, short event, void *arg) +{ + struct ether *e = arg; + union { + struct sockaddr_ll ll; + struct sockaddr_storage ss; + struct sockaddr su; + } lladdr; + socklen_t addrlen = sizeof(lladdr); + union { + struct frame frame; + uint8_t raw[1536]; + } buf; + + if (event & EV_READ) { + ssize_t rlen = recvfrom(e->sock, &buf, sizeof(buf), + MSG_TRUNC, &lladdr.su, &addrlen); + if (rlen > 0) { + if (0) { + lprintf("got %zd bytes", rlen); + char pbuffer[16 * 3 + 1]; + for (size_t i = 0; i < (size_t)rlen; i++) { + size_t j = i % 16; + sprintf(pbuffer + (3 * j), " %02x", buf.raw[i]); + if (j == 15) + lprintf(">>%s", pbuffer); + } + if ((rlen % 16) != 15) + lprintf(">>%s", pbuffer); + } + + if (ntohs(buf.frame.proto) == ETHER_PROTO + && buf.frame.oui[0] == 0x00 + && buf.frame.oui[1] == 0x80 + && buf.frame.oui[2] == 0x41 + && ntohs(buf.frame.protoid) == 0xaaaa) { + switch (buf.frame.protover) { + case 3: + ether_rx_dataframe(e, &buf.frame); + break; + default: + lprintf("%s: unsupported CAN protocol version %d", + e->u->name, buf.frame.protover); + } + } else { + lprintf("%s: non-CAN frame (%zd bytes)", + e->u->name, rlen); + } + } + } +} + +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; +} |