1 files changed, 211 insertions, 0 deletions

diff --git a/sys/kern/kern_socket.c b/sys/kern/kern_socket.c
index a6cc8d2..8be5031 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,119 @@ 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 sockaddr_un *un;
+    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;
+    }
+
+    /* Must be a unix domain socket */
+    un = &ksock->un;
+    if (un->sun_family != AF_UNIX) {
+        return -EBADF;
+    }
+
+    /* 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;
 }
 
@@ -440,6 +554,103 @@ sys_send(struct syscall_args *scargs)
     return send(sockfd, buf, len, flags);
 }
 
+/*
+ * recvmsg(3) syscall
+ *
+ * arg0: socket
+ * arg1: msg
+ * arg2: flags
+ */
+scret_t
+sys_recvmsg(struct syscall_args *scargs)
+{
+    struct msghdr *u_msg = (void *)scargs->arg1;
+    void *u_control;
+    size_t controllen;
+    struct iovec msg_iov;
+    struct msghdr msg;
+    ssize_t retval;
+    int socket = scargs->arg0;
+    int flags = scargs->arg2;
+    int error;
+
+    /* Read the message header */
+    error = copyin(u_msg, &msg, sizeof(msg));
+    if (error < 0) {
+        pr_error("sys_recvmsg: bad msg\n");
+        return error;
+    }
+
+    /* Grab the message I/O vector */
+    error = uio_copyin(msg.msg_iov, &msg_iov, msg.msg_iovlen);
+    if (error < 0) {
+        return error;
+    }
+
+    /* Save control fields */
+    u_control = msg.msg_control;
+    controllen = msg.msg_controllen;
+
+    /* Allocate a new control field to copy in */
+    msg.msg_control = dynalloc(controllen);
+    if (msg.msg_control == NULL) {
+        uio_copyin_clean(&msg_iov, msg.msg_iovlen);
+        return -ENOMEM;
+    }
+
+    error = copyin(u_control, msg.msg_control, controllen);
+    if (error < 0) {
+        retval = error;
+        goto done;
+    }
+
+    msg.msg_iov = &msg_iov;
+    retval = recvmsg(socket, &msg, flags);
+done:
+    uio_copyin_clean(&msg_iov, msg.msg_iovlen);
+    dynfree(msg.msg_control);
+    return retval;
+}
+
+/*
+ * sendmsg(3) syscall
+ *
+ * arg0: socket
+ * arg1: msg
+ * arg2: flags
+ */
+scret_t
+sys_sendmsg(struct syscall_args *scargs)
+{
+    struct iovec msg_iov;
+    struct msghdr *u_msg = (void *)scargs->arg1;
+    struct msghdr msg;
+    ssize_t retval;
+    int socket = scargs->arg0;
+    int flags = scargs->arg2;
+    int error;
+
+    /* Read the message header */
+    error = copyin(u_msg, &msg, sizeof(msg));
+    if (error < 0) {
+        pr_error("sys_sendmsg: bad msg\n");
+        return error;
+    }
+
+    /* Grab the message I/O vector */
+    error = uio_copyin(msg.msg_iov, &msg_iov, msg.msg_iovlen);
+    if (error < 0) {
+        return error;
+    }
+
+    msg.msg_name = NULL;
+    msg.msg_namelen = 0;
+    msg.msg_iov = &msg_iov;
+    retval = sendmsg(socket, &msg, flags);
+    uio_copyin_clean(&msg_iov, msg.msg_iovlen);
+    return retval;
+}
+
 static struct vops socket_vops = {
     .read = NULL,
     .write = NULL,