bus: put together messages with memfd payload correctly

4 files changed, 95 insertions, 17 deletions

diff --git a/src/libsystemd-bus/bus-kernel.c b/src/libsystemd-bus/bus-kernel.c
index 54a5e1691c..a0e892a49f 100644
--- a/src/libsystemd-bus/bus-kernel.c
+++ b/src/libsystemd-bus/bus-kernel.c
@@ -65,19 +65,32 @@ static int parse_unique_name(const char *s, uint64_t *id) {
 
 static void append_payload_vec(struct kdbus_item **d, const void *p, size_t sz) {
         assert(d);
-        assert(p);
         assert(sz > 0);
 
         *d = ALIGN8_PTR(*d);
 
         (*d)->size = offsetof(struct kdbus_item, vec) + sizeof(struct kdbus_vec);
         (*d)->type = KDBUS_MSG_PAYLOAD_VEC;
-        (*d)->vec.address = (uint64_t) p;
+        (*d)->vec.address = PTR_TO_UINT64(p);
         (*d)->vec.size = sz;
 
         *d = (struct kdbus_item *) ((uint8_t*) *d + (*d)->size);
 }
 
+static void append_payload_memfd(struct kdbus_item **d, int memfd, size_t sz) {
+        assert(d);
+        assert(memfd >= 0);
+        assert(sz > 0);
+
+        *d = ALIGN8_PTR(*d);
+        (*d)->size = offsetof(struct kdbus_item, memfd) + sizeof(struct kdbus_memfd);
+        (*d)->type = KDBUS_MSG_PAYLOAD_MEMFD;
+        (*d)->memfd.fd = memfd;
+        (*d)->memfd.size = sz;
+
+        *d = (struct kdbus_item *) ((uint8_t*) *d + (*d)->size);
+}
+
 static void append_destination(struct kdbus_item **d, const char *s, size_t length) {
         assert(d);
         assert(s);
@@ -210,6 +223,9 @@ static int bus_message_setup_kmsg(sd_bus *b, sd_bus_message *m) {
 
         sz = offsetof(struct kdbus_msg, items);
 
+        assert_cc(ALIGN8(offsetof(struct kdbus_item, vec) + sizeof(struct kdbus_vec)) ==
+                  ALIGN8(offsetof(struct kdbus_item, memfd) + sizeof(struct kdbus_memfd)));
+
         /* Add in fixed header, fields header and payload */
         sz += (1 + m->n_body_parts) *
                 ALIGN8(offsetof(struct kdbus_item, vec) + sizeof(struct kdbus_vec));
@@ -250,19 +266,38 @@ static int bus_message_setup_kmsg(sd_bus *b, sd_bus_message *m) {
                 append_destination(&d, m->destination, dl);
 
         append_payload_vec(&d, m->header, BUS_MESSAGE_BODY_BEGIN(m));
-        MESSAGE_FOREACH_PART(part, i, m)
+
+        MESSAGE_FOREACH_PART(part, i, m) {
+                if (part->is_zero) {
+                        append_payload_vec(&d, NULL, part->size);
+                        continue;
+                }
+
+                if (part->memfd >= 0 && part->sealed) {
+                        bus_body_part_unmap(part);
+
+                        if (!part->data) {
+                                append_payload_memfd(&d, part->memfd, part->size);
+                                continue;
+                        }
+                }
+
+                if (part->memfd >= 0) {
+                        r = bus_body_part_map(part);
+                        if (r < 0)
+                                goto fail;
+                }
+
                 append_payload_vec(&d, part->data, part->size);
+        }
 
         if (m->kdbus->dst_id == KDBUS_DST_ID_BROADCAST) {
                 void *p;
 
                 p = append_bloom(&d, BLOOM_SIZE);
                 r = bus_message_setup_bloom(m, p);
-                if (r < 0) {
-                        free(m->kdbus);
-                        m->kdbus = NULL;
-                        return -r;
-                }
+                if (r < 0)
+                        goto fail;
         }
 
         if (m->n_fds > 0)
@@ -274,6 +309,11 @@ static int bus_message_setup_kmsg(sd_bus *b, sd_bus_message *m) {
         m->free_kdbus = true;
 
         return 0;
+
+fail:
+        free(m->kdbus);
+        m->kdbus = NULL;
+        return r;
 }
 
 int bus_kernel_take_fd(sd_bus *b) {
diff --git a/src/libsystemd-bus/bus-message.c b/src/libsystemd-bus/bus-message.c
index 747b44ac94..b5a311530b 100644
--- a/src/libsystemd-bus/bus-message.c
+++ b/src/libsystemd-bus/bus-message.c
@@ -2184,7 +2184,7 @@ int sd_bus_message_append_array_memfd(sd_bus_message *m, char type, sd_memfd *me
         return sd_bus_message_close_container(m);
 }
 
-static int body_part_map_for_read(struct bus_body_part *part) {
+int bus_body_part_map(struct bus_body_part *part) {
         void *p;
         size_t psz;
 
@@ -2210,9 +2210,36 @@ static int body_part_map_for_read(struct bus_body_part *part) {
 
         part->mapped = psz;
         part->data = p;
+        part->munmap_this = true;
+
         return 0;
 }
 
+void bus_body_part_unmap(struct bus_body_part *part) {
+
+        assert_se(part);
+
+        if (part->memfd < 0)
+                return;
+
+        if (!part->sealed)
+                return;
+
+        if (!part->data)
+                return;
+
+        if (!part->munmap_this)
+                return;
+
+        assert_se(munmap(part->data, part->mapped) == 0);
+
+        part->data = NULL;
+        part->mapped = 0;
+        part->munmap_this = false;
+
+        return;
+}
+
 static int buffer_peek(const void *p, uint32_t sz, size_t *rindex, size_t align, size_t nbytes, void **r) {
         size_t k, start, end;
 
@@ -2271,7 +2298,7 @@ static struct bus_body_part* find_part(sd_bus_message *m, size_t index, size_t s
 
                 if (index + sz <= begin + part->size) {
 
-                        r = body_part_map_for_read(part);
+                        r = bus_body_part_map(part);
                         if (r < 0)
                                 return NULL;
 
@@ -3709,8 +3736,10 @@ int bus_message_seal(sd_bus_message *m, uint64_t serial) {
 
         MESSAGE_FOREACH_PART(part, i, m)
                 if (part->memfd >= 0 && !part->sealed) {
-                        ioctl(part->memfd, KDBUS_CMD_MEMFD_SEAL_SET, 1);
-                        part->sealed = true;
+                        bus_body_part_unmap(part);
+
+                        if (ioctl(part->memfd, KDBUS_CMD_MEMFD_SEAL_SET, 1) >= 0)
+                                part->sealed = true;
                 }
 
         m->header->serial = serial;
diff --git a/src/libsystemd-bus/bus-message.h b/src/libsystemd-bus/bus-message.h
index f9c8fc9c08..44390c6b50 100644
--- a/src/libsystemd-bus/bus-message.h
+++ b/src/libsystemd-bus/bus-message.h
@@ -232,3 +232,6 @@ struct bus_body_part *message_append_part(sd_bus_message *m);
 
 #define MESSAGE_FOREACH_PART(part, i, m) \
         for ((i) = 0, (part) = &(m)->body; (i) < (m)->n_body_parts; (i)++, (part) = (part)->next)
+
+int bus_body_part_map(struct bus_body_part *part);
+void bus_body_part_unmap(struct bus_body_part *part);
diff --git a/src/libsystemd-bus/test-bus-zero-copy.c b/src/libsystemd-bus/test-bus-zero-copy.c
index 024a0bfddf..0d8435ec1e 100644
--- a/src/libsystemd-bus/test-bus-zero-copy.c
+++ b/src/libsystemd-bus/test-bus-zero-copy.c
@@ -38,6 +38,7 @@ int main(int argc, char *argv[]) {
         int r, bus_ref;
         sd_bus_message *m;
         sd_memfd *f;
+        uint64_t sz;
 
         log_set_max_level(LOG_DEBUG);
 
@@ -79,21 +80,27 @@ int main(int argc, char *argv[]) {
 
         memset(p, 'L', 32);
 
-        r = sd_memfd_new_and_map(&f, 32, &p);
+        r = sd_memfd_new_and_map(&f, 17, &p);
         assert_se(r >= 0);
 
-        memset(p, 'P', 32);
-        munmap(p, 32);
+        memset(p, 'P', 17);
+        munmap(p, 17);
 
-        r = sd_memfd_set_size(f, 32);
+        r = sd_memfd_get_size(f, &sz);
         assert_se(r >= 0);
+        assert_se(sz == 17);
 
         r = sd_bus_message_append_array_memfd(m, 'y', f);
         assert_se(r >= 0);
 
+        sd_memfd_free(f);
+
         r = sd_bus_message_close_container(m);
         assert_se(r >= 0);
 
+        r = sd_bus_message_append(m, "u", 4711);
+        assert_se(r >= 0);
+
         r = bus_message_seal(m, 55);
         assert_se(r >= 0);
 
@@ -106,7 +113,6 @@ int main(int argc, char *argv[]) {
 
         sd_bus_unref(a);
         sd_bus_unref(b);
-        sd_memfd_free(f);
 
         return 0;
 }