kernel: socket: Add initial SCM + CMSG impl

Introduce initial implementation for out-of-band socket control messages
and an SCM_RIGHTS stub.

Signed-off-by: Ian Moffett <ian@osmora.org>

2 files changed, 188 insertions, 0 deletions

diff --git a/sys/include/sys/socket.h b/sys/include/sys/socket.h
index f01e2bf..de5ab61 100644
--- a/sys/include/sys/socket.h
+++ b/sys/include/sys/socket.h
@@ -31,6 +31,9 @@
 #define _SYS_SOCKET_H_
 
 #include <sys/socketvar.h>
+#include <sys/queue.h>
+#include <sys/param.h>
+#include <sys/uio.h>
 #if defined(_KERNEL)
 #include <sys/types.h>
 #include <sys/syscall.h>
@@ -51,6 +54,11 @@ typedef uint32_t socklen_t;
 #endif  /* !_SOCKLEN_T_DEFINED_ */
 
 /*
+ * Socket level number
+ */
+#define SOL_SOCKET 0xFFFF
+
+/*
  * Address family defines
  */
 #define AF_UNSPEC 0
@@ -70,20 +78,90 @@ struct sockaddr {
     char sa_data[14];
 };
 
+/*
+ * POSIX message header for recvmsg()
+ * and sendmsg() calls.
+ */
+struct msghdr {
+    void *msg_name;             /* Optional address */
+    socklen_t msg_namelen;      /* Size of address */
+    struct iovec *msg_iov;      /* Scatter/gather array */
+    int msg_iovlen;             /* Members in msg_iov */
+    void *msg_control;          /* Ancillary data, see below */
+    socklen_t msg_controllen;   /* Ancillary data buffer len */
+    int msg_flags;              /* Message flags */
+};
+
+/*
+ * POSIX control message header for
+ * ancillary data objects.
+ */
+struct cmsghdr {
+    socklen_t cmsg_len;
+    int cmsg_level;
+    int cmsg_type;
+};
+
+#define CMSG_SPACE(len) (MALIGN(sizeof(struct cmsghdr)) + MALIGN(len))
+
+/* Return pointer to cmsg data */
+#define CMSG_DATA(cmsg) PTR_OFFSET(cmsg, sizeof(struct cmsghdr))
+
+/* Return length of control message */
+#define CMSG_LEN(len) (MALIGN(sizeof(struct cmsghdr)) + MALIGN(len))
+
+/* Return pointer to next cmsghdr */
+#define CMSG_NXTHDR(mhdr, cmsg) \
+    PTR_OFFSET(cmsg, MALIGN((cmsg)>cmsg_len)) + \
+        MALIGN(sizeof(struct cmsghdr))  > \
+        PTR_OFFSET((mhdr)->msg_control, (mhdr)->msg_controllen) ? \
+        (struct cmsghdr *)NULL :    \
+        (struct cmsghdr *)PTR_OFFSET(cmsg, MALIGN((cmsg)->cmsg_len))
+
+/* Return pointer to first header */
+#define CMSG_FIRSTHDR(mhdr) \
+    ((mhdr)->msg_controllen >= sizeof(struct cmsghdr) ? \
+        (struct cmsghdr *)(mhdr)->msg_control : \
+        (struct cmsghdr *)NULL);
+
+/* Socket level control messages */
+#define SCM_RIGHTS 0x01
+
 #if defined(_KERNEL)
 
+struct cmsg {
+    union {
+        struct cmsghdr hdr;
+        uint8_t buf[CMSG_SPACE(sizeof(int))];
+    };
+
+    size_t control_len;
+    TAILQ_ENTRY(cmsg) link;
+};
+
+/*
+ * List of cmsg headers and data, queued up
+ * during sendmsg()
+ */
+struct cmsg_list {
+    TAILQ_HEAD(, cmsg) list;
+    uint8_t is_init : 1;
+};
+
 struct ksocket {
     int sockfd;
     union {
         struct sockaddr sockaddr;
         struct sockaddr_un un;
     };
+    struct cmsg_list cmsg_list;
     struct sockbuf buf;
     struct mutex *mtx;
 };
 
 scret_t sys_socket(struct syscall_args *scargs);
 scret_t sys_bind(struct syscall_args *scargs);
+
 scret_t sys_recv(struct syscall_args *scargs);
 scret_t sys_send(struct syscall_args *scargs);
 #endif  /* _KERNEL */
@@ -94,4 +172,7 @@ int bind(int sockfd, const struct sockaddr *addr, socklen_t len);
 ssize_t send(int sockfd, const void *buf, size_t size, int flags);
 ssize_t recv(int sockfd, void *buf, size_t len, int flags);
 
+ssize_t sendmsg(int socket, const struct msghdr *msg, int flags);
+ssize_t recvmsg(int socket, struct msghdr *msg, int flags);
+
 #endif  /* !_SYS_SOCKET_H_ */
diff --git a/sys/kern/kern_socket.c b/sys/kern/kern_socket.c
index a6cc8d2..99ef3b4 100644
--- a/sys/kern/kern_socket.c
+++ b/sys/kern/kern_socket.c
@@ -323,6 +323,7 @@ int
 bind(int sockfd, const struct sockaddr *addr, socklen_t len)
 {
     struct ksocket *ksock;
+    struct cmsg_list *clp;
     int error;
 
     if ((error = get_ksock(sockfd, &ksock)) < 0) {
@@ -335,6 +336,112 @@ bind(int sockfd, const struct sockaddr *addr, socklen_t len)
     if (ksock->mtx == NULL) {
         return -ENOMEM;
     }
+
+    /* Initialize the cmsg list queue */
+    clp = &ksock->cmsg_list;
+    TAILQ_INIT(&clp->list);
+    clp->is_init = 1;
+    return 0;
+}
+
+/*
+ * Send socket control message - POSIX.1-2008
+ *
+ * @socket: Socket to transmit on
+ * @msg: Further arguments
+ * @flags: Optional flags
+ *
+ * Returns zero on success, otherwise a less
+ * than zero errno.
+ */
+ssize_t
+sendmsg(int socket, const struct msghdr *msg, int flags)
+{
+    struct ksocket *ksock;
+    struct cmsg *cmsg;
+    struct sockaddr_un *un;
+    struct cmsg_list *clp;
+    size_t control_len = 0;
+    int error;
+
+    if ((error = get_ksock(socket, &ksock)) < 0) {
+        return error;
+    }
+
+    /* We cannot do sendmsg() non domain sockets */
+    un = &ksock->un;
+    if (un->sun_family != AF_UNIX) {
+        return -EBADF;
+    }
+
+    control_len = MALIGN(msg->msg_controllen);
+
+    /* Allocate a new cmsg */
+    cmsg = dynalloc(control_len + sizeof(struct cmsg));
+    if (cmsg == NULL) {
+        return -EINVAL;
+    }
+
+    memcpy(cmsg->buf, msg->msg_control, control_len);
+    clp = &ksock->cmsg_list;
+    cmsg->control_len = control_len;
+    TAILQ_INSERT_TAIL(&clp->list, cmsg, link);
+    return 0;
+}
+
+/*
+ * Receive socket control message - POSIX.1‐2017
+ *
+ * @socket: Socket to receive on
+ * @msg: Further arguments
+ * @flags: Optional flags
+ *
+ * Returns zero on success, otherwise a less
+ * than zero errno.
+ */
+ssize_t
+recvmsg(int socket, struct msghdr *msg, int flags)
+{
+    struct ksocket *ksock;
+    struct cmsg *cmsg, *tmp;
+    struct cmsghdr *cmsghdr;
+    struct cmsg_list *clp;
+    uint8_t *fds;
+    int error;
+
+    if (socket < 0) {
+        return -EINVAL;
+    }
+
+    /* Grab the socket descriptor */
+    if ((error = get_ksock(socket, &ksock)) < 0) {
+        return error;
+    }
+
+    /* Grab the control message list */
+    clp = &ksock->cmsg_list;
+    cmsg = TAILQ_FIRST(&clp->list);
+
+    while (cmsg != NULL) {
+        cmsghdr = &cmsg->hdr;
+
+        /* Check the control message type */
+        switch (cmsghdr->cmsg_type) {
+        case SCM_RIGHTS:
+            {
+                fds = (uint8_t *)CMSG_DATA(cmsghdr);
+                pr_trace("SCM_RIGHTS -> fd %d\n", fds[0]);
+                break;
+            }
+        }
+
+        tmp = cmsg;
+        cmsg = TAILQ_NEXT(cmsg, link);
+
+        TAILQ_REMOVE(&clp->list, tmp, link);
+        dynfree(tmp);
+    }
+
     return 0;
 }