Merge pull request #792 from CnRJay/feat/libkernel-sendmsg-recvmsg

feat(libkernel): implement sendmsg and recvmsg

core/libs/prx/libkernel/Socket/src/Udp.cpp

@@ -20,6 +20,9 @@
 #include <memory>
 #include <mutex>
 #include <cstdarg>
+#include <cstddef>
+#include <stdexcept>
+#include <vector>
 
 extern "C" int* APS5_VABI __error_nid_postfix();
 namespace {
@@ -388,4 +391,94 @@ std::int64_t APS5_VABI recvfrom_nid_postfix(int descriptor, void* buffer, std::u
     if (address) GuestAddress(native, address, addressLength);
     return result;
 }
+struct GuestIovec {
+    void* base;
+    std::uint64_t length;
+};
+struct GuestMsghdr {
+    void* name;
+    std::uint32_t nameLength;
+    GuestIovec* iov;
+    int iovLength;
+    void* control;
+    std::uint32_t controlLength;
+    int flags;
+};
+static_assert(sizeof(GuestMsghdr) == 48 && offsetof(GuestMsghdr, iov) == 16 && offsetof(GuestMsghdr, control) == 32 && offsetof(GuestMsghdr, flags) == 44);
+static std::int64_t MessageLength(const GuestMsghdr* message) {
+    if (!message) return Fail(14);
+    if (message->iovLength < 0 || message->iovLength > 1024) return Fail(40);
+    if (message->iovLength && !message->iov) return Fail(14);
+    std::uint64_t total = 0;
+    for (int i = 0; i < message->iovLength; ++i) {
+        const auto& entry = message->iov[i];
+        if (!entry.base && entry.length) return Fail(14);
+        if (entry.length > INT_MAX - total) return Fail(22);
+        total += entry.length;
+    }
+    return static_cast<std::int64_t>(total);
+}
+std::int64_t APS5_VABI sendmsg_nid_postfix(int descriptor, const GuestMsghdr* message, int flags) {
+    const auto total = MessageLength(message);
+    if (total < 0) return -1;
+    if (message->control && message->controlLength) throw std::runtime_error("sendmsg: control data is not supported");
+    std::vector<char> buffer;
+    try {
+        buffer.resize(static_cast<std::size_t>(total));
+    } catch (const std::bad_alloc&) {
+        return Fail(55);
+    }
+    std::size_t offset = 0;
+    for (int i = 0; i < message->iovLength; ++i) {
+        if (message->iov[i].length) std::memcpy(buffer.data() + offset, message->iov[i].base, message->iov[i].length);
+        offset += message->iov[i].length;
+    }
+    return sendto_nid_postfix(descriptor, buffer.data(), buffer.size(), flags, message->name, message->name ? message->nameLength : 0u);
+}
+std::int64_t APS5_VABI recvmsg_nid_postfix(int descriptor, GuestMsghdr* message, int flags) {
+    const auto total = MessageLength(message);
+    if (total < 0) return -1;
+    const auto socket = Lookup(descriptor);
+    if (!socket) return -1;
+    if ((flags & ~2) != 0) return Fail(45);
+    const bool datagram = socket->type == 2;
+    std::vector<char> buffer;
+    try {
+        buffer.resize(static_cast<std::size_t>(total) + (datagram ? 1u : 0u));
+    } catch (const std::bad_alloc&) {
+        return Fail(55);
+    }
+    sockaddr_storage native{};
+    socklen_t size = sizeof(native);
+    std::int64_t received = ::recvfrom(socket->value, buffer.data(), static_cast<int>(buffer.size()),
+        flags & 2 ? MSG_PEEK : 0, reinterpret_cast<sockaddr*>(&native), &size);
+    if (received < 0) {
+#ifdef _WIN32
+        if (!datagram || WSAGetLastError() != WSAEMSGSIZE) return Fail(NativeError());
+        received = static_cast<std::int64_t>(buffer.size());
+#else
+        return Fail(NativeError());
+#endif
+    }
+    message->flags = 0;
+    if (received > total) {
+        received = total;
+        message->flags = 0x10;
+    }
+    std::size_t offset = 0;
+    for (int i = 0; i < message->iovLength && offset < static_cast<std::size_t>(received); ++i) {
+        const auto count = std::min<std::size_t>(message->iov[i].length, static_cast<std::size_t>(received) - offset);
+        std::memcpy(message->iov[i].base, buffer.data() + offset, count);
+        offset += count;
+    }
+    if (message->name) {
+        if (size > 0 && (native.ss_family == AF_INET || native.ss_family == AF_INET6)) {
+            GuestAddress(native, message->name, &message->nameLength);
+        } else {
+            message->nameLength = 0;
+        }
+    }
+    message->controlLength = 0;
+    return received;
+}
 }

core/libs/prx/libkernel/Unimplemented.cpp

@@ -27,13 +27,4 @@ int APS5_VABI pthread_cancel_nid_postfix(void) {
     NotImplemented_nid_no_patch("0D4-FVvEikw");
     return 0;
 }
-int APS5_VABI recvmsg_nid_postfix(void) {
-    NotImplemented_nid_no_patch(__func__);
-    return 0;
-}
-
-int APS5_VABI sendmsg_nid_postfix(void) {
-    NotImplemented_nid_no_patch(__func__);
-    return 0;
-}
 }

core/libs/tests/GuestUdp.cpp

@@ -20,6 +20,23 @@ const char* APS5_VABI inet_ntop_nid_postfix(int, const void*, char*, std::uint32
 int APS5_VABI inet_pton_nid_postfix(int, const char*, void*);
 int* APS5_VABI __error_nid_postfix();
 }
+struct Iovec {
+    void* base;
+    std::uint64_t length;
+};
+struct Msghdr {
+    void* name;
+    std::uint32_t nameLength;
+    Iovec* iov;
+    int iovLength;
+    void* control;
+    std::uint32_t controlLength;
+    int flags;
+};
+extern "C" {
+std::int64_t APS5_VABI sendmsg_nid_postfix(int, const Msghdr*, int);
+std::int64_t APS5_VABI recvmsg_nid_postfix(int, Msghdr*, int);
+}
 static void Require(bool value) { if (!value) std::abort(); }
 int main() {
     const int receiver = socket_nid_postfix(2, 2, 0);
@@ -63,6 +80,39 @@ int main() {
     Require(std::strcmp(buffer, message) == 0 && source[1] == 2 && source[4] == 127);
     Require(ioctl_nid_postfix(receiver, 0x4004667f, &queued) == 0 && queued == 0);
     Require(ioctl_nid_postfix(receiver, 123, &queued) == -1 && *__error_nid_postfix() == 25);
+    char head[] = "scatter ";
+    char tail[] = "gather";
+    Iovec parts[2]{{head, 8}, {tail, 6}};
+    Msghdr outgoing{destination.data(), static_cast<std::uint32_t>(destination.size()), parts, 2, nullptr, 0, 0};
+    Require(sendmsg_nid_postfix(sender, &outgoing, 0) == 14);
+    const auto wait = [&] {
+        queued = 0;
+        for (int i = 0; i < 100 && queued == 0; ++i) {
+            Require(ioctl_nid_postfix(receiver, 0x4004667f, &queued) == 0);
+            if (!queued) std::this_thread::sleep_for(std::chrono::milliseconds(1));
+        }
+        Require(queued != 0);
+    };
+    wait();
+    char first[4]{}, second[16]{};
+    Iovec pieces[2]{{first, sizeof(first)}, {second, sizeof(second)}};
+    source.fill(0);
+    unsigned char control[8]{};
+    Msghdr incoming{source.data(), static_cast<std::uint32_t>(source.size()), pieces, 2, control, sizeof(control), -1};
+    Require(recvmsg_nid_postfix(receiver, &incoming, 0) == 14);
+    Require(std::memcmp(first, "scat", 4) == 0 && std::memcmp(second, "ter gather", 10) == 0);
+    Require(incoming.flags == 0 && incoming.controlLength == 0 && incoming.nameLength == 16 && source[1] == 2 && source[4] == 127);
+    Require(sendmsg_nid_postfix(sender, &outgoing, 0) == 14);
+    wait();
+    char shortBuffer[5]{};
+    Iovec single{shortBuffer, sizeof(shortBuffer)};
+    Msghdr truncated{nullptr, 0, &single, 1, nullptr, 0, 0};
+    Require(recvmsg_nid_postfix(receiver, &truncated, 0) == 5);
+    Require(std::memcmp(shortBuffer, "scatt", 5) == 0 && truncated.flags == 0x10);
+    Require(recvmsg_nid_postfix(receiver, &truncated, 0) == -1 && *__error_nid_postfix() == 35);
+    Msghdr oversized{nullptr, 0, parts, 1025, nullptr, 0, 0};
+    Require(sendmsg_nid_postfix(sender, &oversized, 0) == -1 && *__error_nid_postfix() == 40);
+    Require(recvmsg_nid_postfix(receiver, nullptr, 0) == -1 && *__error_nid_postfix() == 14);
     Require(close_nid_postfix(receiver) == 0);
     Require(close_nid_postfix(receiver) == -1 && *__error_nid_postfix() == 9);
     Require(ioctl_nid_postfix(receiver, 0x4004667f, &queued) == -1);