openpilot is an open source driver assistance system. openpilot performs the functions of Automated Lane Centering and Adaptive Cruise Control for over 200 supported car makes and models.

128 lines
2.9 KiB

#include <stdio.h>
#include <stdlib.h>
#include <stdbool.h>
#include <string.h>
#include <unistd.h>
#include <assert.h>
#include <errno.h>
#include <sys/mman.h>
#include <sys/socket.h>
#include <sys/un.h>
#include "ipc.h"
int ipc_connect(const char* socket_path) {
int err;
#ifdef __APPLE__
int sock = socket(AF_UNIX, SOCK_STREAM, 0);
#else
int sock = socket(AF_UNIX, SOCK_SEQPACKET, 0);
#endif
if (sock < 0) return -1;
struct sockaddr_un addr = {
.sun_family = AF_UNIX,
};
snprintf(addr.sun_path, sizeof(addr.sun_path), "%s", socket_path);
err = connect(sock, (struct sockaddr*)&addr, sizeof(addr));
if (err != 0) {
close(sock);
return -1;
}
return sock;
}
int ipc_bind(const char* socket_path) {
int err;
unlink(socket_path);
#ifdef __APPLE__
int sock = socket(AF_UNIX, SOCK_STREAM, 0);
#else
int sock = socket(AF_UNIX, SOCK_SEQPACKET, 0);
#endif
struct sockaddr_un addr = {
.sun_family = AF_UNIX,
};
snprintf(addr.sun_path, sizeof(addr.sun_path), "%s", socket_path);
err = bind(sock, (struct sockaddr *)&addr, sizeof(addr));
assert(err == 0);
err = listen(sock, 3);
assert(err == 0);
return sock;
}
int ipc_sendrecv_with_fds(bool send, int fd, void *buf, size_t buf_size, int* fds, int num_fds,
int *out_num_fds) {
int err;
char control_buf[CMSG_SPACE(sizeof(int) * num_fds)];
memset(control_buf, 0, CMSG_SPACE(sizeof(int) * num_fds));
struct iovec iov = {
.iov_base = buf,
.iov_len = buf_size,
};
struct msghdr msg = {
.msg_iov = &iov,
.msg_iovlen = 1,
};
if (num_fds > 0) {
assert(fds);
msg.msg_control = control_buf;
msg.msg_controllen = CMSG_SPACE(sizeof(int) * num_fds);
}
if (send) {
if (num_fds) {
struct cmsghdr *cmsg = CMSG_FIRSTHDR(&msg);
assert(cmsg);
cmsg->cmsg_level = SOL_SOCKET;
cmsg->cmsg_type = SCM_RIGHTS;
cmsg->cmsg_len = CMSG_LEN(sizeof(int) * num_fds);
memcpy(CMSG_DATA(cmsg), fds, sizeof(int) * num_fds);
// printf("send clen %d -> %d\n", num_fds, cmsg->cmsg_len);
}
return sendmsg(fd, &msg, 0);
} else {
int r = recvmsg(fd, &msg, 0);
if (r < 0) return r;
int recv_fds = 0;
if (msg.msg_controllen > 0) {
struct cmsghdr *cmsg = CMSG_FIRSTHDR(&msg);
assert(cmsg);
assert(cmsg->cmsg_level == SOL_SOCKET && cmsg->cmsg_type == SCM_RIGHTS);
recv_fds = (cmsg->cmsg_len - CMSG_LEN(0));
assert(recv_fds > 0 && (recv_fds % sizeof(int)) == 0);
recv_fds /= sizeof(int);
// printf("recv clen %d -> %d\n", cmsg->cmsg_len, recv_fds);
// assert(cmsg->cmsg_len == CMSG_LEN(sizeof(int) * num_fds));
assert(fds && recv_fds <= num_fds);
memcpy(fds, CMSG_DATA(cmsg), sizeof(int) * recv_fds);
}
if (msg.msg_flags) {
for (int i=0; i<recv_fds; i++) {
close(fds[i]);
}
return -1;
}
if (fds) {
assert(out_num_fds);
*out_num_fds = recv_fds;
}
return r;
}
}