summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
authorDavid Lamparter <equinox@diac24.net>2013-03-25 22:41:56 +0100
committerDavid Lamparter <equinox@diac24.net>2013-03-25 22:41:56 +0100
commit2de09bbd1dc380734946b2389f1177f48ae0b4e5 (patch)
treece091be56d2c22b818f767da5bb4da7cfe365e87
parentf5c47e42e40e980c51fa561ca8bb0beb421ae0c0 (diff)
cethcan: more code
-rw-r--r--cethcan/can.c3
-rw-r--r--cethcan/ether.c77
2 files changed, 79 insertions, 1 deletions
diff --git a/cethcan/can.c b/cethcan/can.c
index 4a61756..a36fd6c 100644
--- a/cethcan/can.c
+++ b/cethcan/can.c
@@ -42,7 +42,8 @@ void can_broadcast(struct can_user *origin, struct can_message *msg)
}
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);
+ lprintf("%s: %08x (%zu)%s", origin->name,
+ (unsigned)msg->daddr, msg->dlc, buf);
for (u = users; u; u = u->next)
if (u != origin)
diff --git a/cethcan/ether.c b/cethcan/ether.c
index 5daac2e..5b935ed 100644
--- a/cethcan/ether.c
+++ b/cethcan/ether.c
@@ -17,12 +17,89 @@ struct ether {
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)