lib/libposix/sys/socket: add support for sendmsg() to send multiple packets in one call

nieklinnenbank · nieklinnenbank · commit ae04f0b005cd · 2021-01-06T23:51:27.000+01:00
diff --git a/lib/libnet/NetworkClient.h b/lib/libnet/NetworkClient.h
@@ -51,7 +51,9 @@ class NetworkClient
     enum SocketAction
     {
         Connect,
-        Listen
+        Listen,
+        SendSingle,
+        SendMultiple
     };
 
     /**
@@ -64,10 +66,22 @@ class NetworkClient
     {
         IPV4::Address address;
         u16 port;
-        SocketAction action;
+        u16 action;
     }
     SocketInfo;
 
+    /**
+     * Describes a single packet.
+     *
+     * This structure is used for operations that involve multiple packets,
+     * for example: SendMultiple.
+     */
+    struct PacketInfo
+    {
+        Address address;
+        Size size;
+    };
+
     /**
      * Socket types
      */
diff --git a/lib/libnet/UDP.cpp b/lib/libnet/UDP.cpp
@@ -135,23 +135,20 @@ FileSystem::Result UDP::process(const NetworkQueue::Packet *pkt,
 }
 
 FileSystem::Result UDP::sendPacket(const NetworkClient::SocketInfo *src,
+                                   const NetworkClient::SocketInfo *dest,
                                    IOBuffer & buffer,
-                                   const Size size)
+                                   const Size size,
+                                   const Size offset)
 {
-    NetworkClient::SocketInfo dest;
     NetworkQueue::Packet *pkt;
     Header *hdr;
 
-    DEBUG("");
-
-    // Read destination
-    buffer.read(&dest, sizeof(dest));
-    DEBUG("send payload to: ipAddr = " << *IPV4::toString(dest.address) <<
-          " port = " << dest.port << " size = " << size);
+    DEBUG("address = " << *IPV4::toString(dest->address) <<
+          " port = " << dest->port << " size = " << size);
 
     // Get a fresh packet
-    const FileSystem::Result result = m_parent.getTransmitPacket(&pkt, &dest.address, sizeof(dest.address),
-                                                                 NetworkProtocol::UDP, sizeof(Header) + size - sizeof(dest));
+    const FileSystem::Result result = m_parent.getTransmitPacket(&pkt, &dest->address, sizeof(dest->address),
+                                                                 NetworkProtocol::UDP, sizeof(Header) + size);
     if (result != FileSystem::Success)
     {
         if (result != FileSystem::RetryAgain)
@@ -164,25 +161,25 @@ FileSystem::Result UDP::sendPacket(const NetworkClient::SocketInfo *src,
     // Fill UDP header
     hdr = (Header *) (pkt->data + pkt->size);
     writeBe16(&hdr->sourcePort, src->port);
-    writeBe16(&hdr->destPort, dest.port);
-    writeBe16(&hdr->length, size - sizeof(dest) + sizeof(Header));
+    writeBe16(&hdr->destPort, dest->port);
+    writeBe16(&hdr->length, size + sizeof(Header));
     writeBe16(&hdr->checksum, 0);
 
-    // Insert payload. The payload is just after the 'dest' struct in the IOBuffer.
+    // Insert payload. The payload is read from the given offset in the IOBuffer.
     // Note that the payload must not overwrite past the packet buffer
     const Size maximum = getMaximumPacketSize();
-    const Size needed = pkt->size + size - sizeof(dest);
+    const Size needed = pkt->size + size;
 
     buffer.read(pkt->data + pkt->size + sizeof(Header),
-                needed > maximum ? maximum : needed, sizeof(dest));
+                needed > maximum ? maximum : needed, offset);
 
     // Calculate final checksum
     write16(&hdr->checksum, checksum((IPV4::Header *)(pkt->data + pkt->size - sizeof(IPV4::Header)),
-                                      hdr, size - sizeof(dest)));
+                                      hdr, size));
     DEBUG("checksum = " << (uint) hdr->checksum);
 
     // Increment packet size
-    pkt->size += sizeof(Header) + size - sizeof(dest);
+    pkt->size += sizeof(Header) + size;
 
     // Transmit now
     return m_device.transmit(pkt);
diff --git a/lib/libnet/UDP.h b/lib/libnet/UDP.h
@@ -125,9 +125,11 @@ class UDP : public NetworkProtocol
      *
      * @return Result code
      */
-    FileSystem::Result sendPacket(const NetworkClient::SocketInfo *info,
+    FileSystem::Result sendPacket(const NetworkClient::SocketInfo *src,
+                                  const NetworkClient::SocketInfo *dest,
                                   IOBuffer & buffer,
-                                  const Size size);
+                                  const Size size,
+                                  const Size offset);
 
     /**
      * Calculate ICMP checksum
diff --git a/lib/libnet/UDPSocket.cpp b/lib/libnet/UDPSocket.cpp
@@ -15,6 +15,7 @@
  * along with this program.  If not, see <http://www.gnu.org/licenses/>.
  */
 
+#include <FreeNOS/System.h>
 #include <ByteOrder.h>
 #include <Randomizer.h>
 #include "Ethernet.h"
@@ -79,24 +80,74 @@ FileSystem::Result UDPSocket::write(IOBuffer & buffer,
 {
     DEBUG("");
 
-    // Receive socket information first?
-    if (!m_info.port)
-    {
-        buffer.read(&m_info, sizeof(m_info));
+    // Read socket info and action
+    NetworkClient::SocketInfo dest;
+    buffer.read(&dest, sizeof(dest));
 
-        if (m_info.port == 0)
+    // Handle the socket operation
+    switch (dest.action)
+    {
+        case NetworkClient::Listen:
         {
-            Randomizer rand;
-            m_info.port = rand.next() % 65535;
+            MemoryBlock::copy(&m_info, &dest, sizeof(m_info));
+
+            if (m_info.port == 0)
+            {
+                Randomizer rand;
+                m_info.port = rand.next() % 65535;
+            }
+
+            DEBUG("addr =" << m_info.address << " port = " << m_info.port);
+            return m_udp->bind(this, m_info.port);
         }
 
-        DEBUG("addr =" << m_info.address << " port = " << m_info.port);
+        case NetworkClient::SendSingle:
+            return m_udp->sendPacket(&m_info, &dest, buffer, size - sizeof(dest), sizeof(dest));
 
-        return m_udp->bind(this, m_info.port);
-    }
-    else
-    {
-        return m_udp->sendPacket(&m_info, buffer, size);
+        case NetworkClient::SendMultiple:
+        {
+            NetworkClient::PacketInfo packetInfo;
+            FileSystemMessage msg;
+            IOBuffer io;
+            Size packetOffset = 0;
+
+            // Read the first packet info to find the base address for all packets.
+            //
+            // Note that it is assumed here that all packet buffers
+            // originate from the same base address and that each new packet
+            // starts after NetworkQueue::PayloadBufferSize bytes.
+            buffer.read(&packetInfo, sizeof(packetInfo), sizeof(dest));
+
+            // Prepare dummy filesystem message for the I/O buffer
+            msg.from = buffer.getMessage()->from;
+            msg.action = FileSystem::WriteFile;
+            msg.buffer = (char *)packetInfo.address;
+            msg.size = NetworkQueue::MaxPackets * PAGESIZE;
+            io.setMessage(&msg);
+
+            // read the array of PacketInfo structs that describe
+            // all the packets that need to be transferred
+            for (Size i = sizeof(dest); i < size; i += sizeof(NetworkClient::PacketInfo))
+            {
+                buffer.read(&packetInfo, sizeof(packetInfo), i);
+                DEBUG("packet[" << ((i - sizeof(dest)) / sizeof(NetworkClient::PacketInfo)) <<
+                      "] size = " << packetInfo.size << " offset = " << packetOffset);
+
+                const FileSystem::Result r = m_udp->sendPacket(&m_info, &dest, io, packetInfo.size, packetOffset);
+                if (r != FileSystem::Success)
+                {
+                    ERROR("failed to send packet: result = " << (int) r);
+                    return r;
+                }
+
+                packetOffset += NetworkQueue::PayloadBufferSize;
+            }
+
+            return FileSystem::Success;
+        }
+
+        default:
+            return FileSystem::NotSupported;
     }
 }
 
diff --git a/lib/libposix/sys/socket.h b/lib/libposix/sys/socket.h
@@ -38,8 +38,31 @@ struct sockaddr
     u16 port;
 };
 
+/**
+ * Input/Output vector for multi-packet operations
+ */
+struct iovec
+{
+    void *iov_base;
+    size_t iov_len;
+};
+
 typedef Size socklen_t;
 
+/**
+ * Describes one or more datagrams
+ */
+struct msghdr
+{
+    void         *msg_name;
+    socklen_t     msg_namelen;
+    struct iovec *msg_iov;
+    size_t        msg_iovlen;
+    void         *msg_control;
+    size_t        msg_controllen;
+    int           msg_flags;
+};
+
 /**
  * Connect a socket to an address/port.
  *
@@ -81,6 +104,17 @@ extern C int recvfrom(int sockfd, void *buf, size_t len, int flags,
 extern C int sendto(int sockfd, const void *buf, size_t len, int flags,
                     const struct sockaddr *addr, socklen_t addrlen);
 
+/**
+ * Send multiple datagrams to a remote host.
+ *
+ * @param sockfd Socket file descriptor
+ * @param msg Pointer to the messages to send
+ * @param flags Optional flags for the send operation
+ *
+ * @return Number of bytes send on success and -1 on error
+ */
+extern C int sendmsg(int sockfd, const struct msghdr *msg, int flags);
+
 /**
  * @}
  * @}
diff --git a/lib/libposix/sys/socket/sendmsg.cpp b/lib/libposix/sys/socket/sendmsg.cpp
@@ -0,0 +1,52 @@
+/*
+ * Copyright (C) 2021 Niek Linnenbank
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <NetworkClient.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <sys/socket.h>
+#include <errno.h>
+
+extern C int sendmsg(int sockfd, const struct msghdr *msg, int flags)
+{
+    static u8 buf[1024];
+    NetworkClient::SocketInfo *info = (NetworkClient::SocketInfo *) buf;
+    NetworkClient::PacketInfo *pkt = (NetworkClient::PacketInfo *) (info + 1);
+    const struct sockaddr *addr = (const struct sockaddr *) msg->msg_name;
+    Size bytes = sizeof(*info);
+
+    if (msg->msg_namelen != sizeof(struct sockaddr))
+    {
+        return ERANGE;
+    }
+
+    info->address = addr->addr;
+    info->port = addr->port;
+    info->action = NetworkClient::SendMultiple;
+
+    // Prepare the array of NetworkClient::PacketInfo structs
+    for (Size i = 0; i < msg->msg_iovlen && bytes < sizeof(buf); i++)
+    {
+        pkt->address = (Address) msg->msg_iov[i].iov_base;
+        pkt->size = msg->msg_iov[i].iov_len;
+
+        bytes += sizeof(*pkt);
+        pkt++;
+    }
+
+    return ::write(sockfd, buf, bytes);
+}
diff --git a/lib/libposix/sys/socket/sendto.cpp b/lib/libposix/sys/socket/sendto.cpp
@@ -33,6 +33,7 @@ extern C int sendto(int sockfd, const void *buf, size_t len, int flags,
 
     info.address = addr->addr;
     info.port = addr->port;
+    info.action = NetworkClient::SendSingle;
 
     memcpy(packet, &info, sizeof(info));
     memcpy(packet + sizeof(info), buf, len);
diff --git a/test/server/network/loopback/LoopbackTest.cpp b/test/server/network/loopback/LoopbackTest.cpp
@@ -202,6 +202,7 @@ TestCase(LoopbackUdpPing)
     // Fill the packet contents
     info->address = IPV4::toAddress("127.0.0.1");
     info->port = 12345;
+    info->action = NetworkClient::SendSingle;
     MemoryBlock::copy(info + 1, payload, String::length(payload));
 
     // Process the message two times: first for ARP lookup, second for actual packet
