550 lines
17 KiB
C
550 lines
17 KiB
C
#include <string.h>
|
|
#include <stdio.h>
|
|
#include <sys/uio.h>
|
|
#include "try.h"
|
|
#include "util.h"
|
|
#include "wire.h"
|
|
#include "log.h"
|
|
|
|
// SPEC: https://dbus.freedesktop.org/doc/dbus-specification.html#id-1.4.6
|
|
_WIRE_DEF_BYTE_TYPE(i8, int8_t)
|
|
_WIRE_DEF_BYTE_TYPE(u8, uint8_t)
|
|
_WIRE_DEF_TYPE(i16, int16_t, 2)
|
|
_WIRE_DEF_TYPE(u16, uint16_t, 2)
|
|
_WIRE_DEF_TYPE(i32, int32_t, 4)
|
|
_WIRE_DEF_TYPE(u32, uint32_t, 4)
|
|
_WIRE_DEF_TYPE(boolean, uint32_t, 4)
|
|
|
|
bool wire_write_align(WireCtx *c, uint32_t alignment)
|
|
{
|
|
// Thanks: https://github.com/c-util/c-stdaux/blob/ed5fee49a3ec13fe79f30324b23c73cd41805514/src/c-stdaux-gnuc.h#L431
|
|
uint32_t new_cursor = ((c->byte_cursor) + (alignment) - 1) & ~((alignment) - 1);
|
|
|
|
if (unlikely(new_cursor >= c->data_len || new_cursor < c->byte_cursor)) {
|
|
return false;
|
|
}
|
|
while (c->byte_cursor < new_cursor) {
|
|
c->data[c->byte_cursor++] = '\0';
|
|
}
|
|
return true;
|
|
}
|
|
|
|
bool wire_align(WireCtx *c, uint32_t alignment)
|
|
{
|
|
// Thanks: https://github.com/c-util/c-stdaux/blob/ed5fee49a3ec13fe79f30324b23c73cd41805514/src/c-stdaux-gnuc.h#L431
|
|
uint32_t new_cursor = ((c->byte_cursor) + (alignment) - 1) & ~((alignment) - 1);
|
|
|
|
if (unlikely(new_cursor >= c->data_len || new_cursor < c->byte_cursor)) {
|
|
return false;
|
|
}
|
|
c->byte_cursor = new_cursor;
|
|
return true;
|
|
}
|
|
|
|
char *wire_set_signature_single_char(WireCtx *c, const char s)
|
|
{
|
|
TRYPTR_NIL(wire_set_u8(c, 1));
|
|
|
|
uint32_t new_cursor = c->byte_cursor + 2; /* + 2 because of the extra null termination that strings have */
|
|
if (unlikely(new_cursor >= c->data_len)) {
|
|
return NULL;
|
|
}
|
|
|
|
// TODO?
|
|
char *dest = (char*)&c->data[c->byte_cursor];
|
|
dest[0] = s;
|
|
dest[1] = '\0';
|
|
|
|
c->byte_cursor = new_cursor;
|
|
|
|
return dest;
|
|
}
|
|
|
|
char *wire_get_string_impl(WireCtx *c, bool as_signature)
|
|
{
|
|
uint32_t len = 0;
|
|
if (as_signature) {
|
|
len = *(uint8_t*)TRYPTR_NIL(wire_get_u8(c));
|
|
} else {
|
|
len = *(uint32_t*)TRYPTR_NIL(wire_get_u32(c));
|
|
}
|
|
|
|
uint32_t new_cursor = c->byte_cursor + len + 1; /* + 1 because of the extra null termination that strings have */
|
|
if (unlikely(new_cursor >= c->data_len)) {
|
|
return NULL;
|
|
}
|
|
|
|
// strings always end with a null byte
|
|
if (unlikely(c->data[c->byte_cursor + len] != '\0')) {
|
|
return NULL;
|
|
}
|
|
|
|
char *str = (char *)&c->data[c->byte_cursor];
|
|
|
|
c->byte_cursor = new_cursor;
|
|
|
|
return str;
|
|
}
|
|
|
|
char *wire_set_string_fixed(WireCtx *c, const char *str, uint32_t len)
|
|
{
|
|
// len includes the null byte
|
|
|
|
TRYPTR_NIL(wire_set_u32(c, len - 1));
|
|
|
|
uint32_t new_cursor = c->byte_cursor + len;
|
|
if (unlikely(new_cursor >= c->data_len)) {
|
|
return NULL;
|
|
}
|
|
|
|
// TODO?
|
|
char *dest = (char*)&c->data[c->byte_cursor];
|
|
memcpy(dest, str, len);
|
|
|
|
c->byte_cursor = new_cursor;
|
|
|
|
return dest;
|
|
}
|
|
|
|
char *wire_set_string_impl(WireCtx *c, const char *str, bool as_signature)
|
|
{
|
|
uint32_t len = strlen(str);
|
|
if (as_signature) {
|
|
TRYPTR_NIL(wire_set_u8(c, len));
|
|
} else {
|
|
TRYPTR_NIL(wire_set_u32(c, len));
|
|
}
|
|
|
|
uint32_t new_cursor = c->byte_cursor + len + 1; /* + 1 because of the extra null termination that strings have */
|
|
if (unlikely(new_cursor >= c->data_len)) {
|
|
return NULL;
|
|
}
|
|
|
|
// TODO?
|
|
char *dest = (char*)&c->data[c->byte_cursor];
|
|
memcpy(dest, str, len + 1);
|
|
|
|
c->byte_cursor = new_cursor;
|
|
|
|
return dest;
|
|
}
|
|
|
|
char *wire_get_string_check(WireCtx *c, int min_length, int max_length) {
|
|
char *str = TRYPTR_NIL(wire_get_string_impl(c, false));
|
|
int length = strlen(str);
|
|
if (unlikely(length < min_length || length > max_length)) return NULL;
|
|
return str;
|
|
}
|
|
|
|
int wire_parse_message(WireCtx *c, WireMsg *msg)
|
|
{
|
|
// SPEC: https://dbus.freedesktop.org/doc/dbus-specification.html#message-protocol-messages
|
|
msg->endianness = *(uint8_t*)TRYPTR(wire_get_u8(c));
|
|
if (msg->endianness == 'B') {
|
|
// we only support little endian
|
|
return -1;
|
|
}
|
|
|
|
msg->type = *(uint8_t*)TRYPTR(wire_get_u8(c));
|
|
msg->flags = *(uint8_t*)TRYPTR(wire_get_u8(c));
|
|
msg->protocol_version = *(uint8_t*)TRYPTR(wire_get_u8(c));
|
|
msg->body_length = *(uint32_t*)TRYPTR(wire_get_u32(c));
|
|
msg->serial = *(uint32_t*)TRYPTR(wire_get_u32(c));
|
|
msg->header_fields_length = *(uint32_t*)TRYPTR(wire_get_u32(c));
|
|
|
|
TRYPTR(wire_write_align(c, 8));
|
|
|
|
msg->header_length = c->byte_cursor;
|
|
size_t header_fields_end = c->byte_cursor + msg->header_fields_length;
|
|
if (unlikely(header_fields_end >= c->data_len)) {
|
|
return -1;
|
|
}
|
|
|
|
VERBOSE("start parse message: type=%d, no_reply_expected=%d\n", msg->type, msg->flags & DBUS_FLAG_NO_REPLY_EXPECTED);
|
|
|
|
while (c->byte_cursor < header_fields_end) {
|
|
uint8_t field_code = *(uint8_t*)TRYPTR(wire_get_u8(c));
|
|
TRYPTR(wire_get_signature(c));
|
|
|
|
switch (field_code) {
|
|
case DBUS_HEADER_FIELD_PATH: /* through */
|
|
case DBUS_HEADER_FIELD_INTERFACE: /* through */
|
|
case DBUS_HEADER_FIELD_MEMBER: /* through */
|
|
case DBUS_HEADER_FIELD_ERROR_NAME: /* through */
|
|
case DBUS_HEADER_FIELD_DESTINATION: {
|
|
char *str = TRYPTR(wire_get_string(c));
|
|
VERBOSE("field: %s\n", str);
|
|
msg->fields[field_code].t.str = str;
|
|
} break;
|
|
case DBUS_HEADER_FIELD_SIGNATURE: {
|
|
char *str = TRYPTR(wire_get_signature(c));
|
|
VERBOSE("field: %s\n", str);
|
|
msg->fields[field_code].t.str = str;
|
|
} break;
|
|
case DBUS_HEADER_FIELD_REPLY_SERIAL: {
|
|
uint32_t u = *(uint32_t*)TRYPTR(wire_get_u32(c));
|
|
VERBOSE("field: %d\n", u);
|
|
msg->fields[field_code].t.u32 = u;
|
|
} break;
|
|
default: {
|
|
WARN("header: unknown field: %d\n", field_code);
|
|
return -1;
|
|
} break;
|
|
}
|
|
msg->fields[field_code].present = true;
|
|
|
|
// Structs are always aligned to `8`.
|
|
// SPEC: https://dbus.freedesktop.org/doc/dbus-specification.html#id-1.4.6
|
|
TRYPTR(wire_align(c, 8));
|
|
|
|
if (msg->header_fields_count++ > WIRE_MESSAGE_MAX_HEADER_FIELDS) {
|
|
return -1;
|
|
}
|
|
}
|
|
|
|
VERBOSE("end parse message\n");
|
|
|
|
/* header ends at 8 byte boundry */
|
|
TRYPTR(wire_align(c, 8));
|
|
|
|
return 0;
|
|
}
|
|
|
|
int wire_compose_signal(WireCtx *c, WireMsg *out_msg, const char *signature, const char *member, uint32_t **out_body_length)
|
|
{
|
|
out_msg->endianness = 'l';
|
|
TRYPTR(wire_set_u8(c, 'l')); /* endianness */
|
|
out_msg->type = DBUS_MESSAGE_SIGNAL;
|
|
TRYPTR(wire_set_u8(c, DBUS_MESSAGE_SIGNAL)); /* type */
|
|
out_msg->flags = 0 | DBUS_FLAG_NO_REPLY_EXPECTED;
|
|
TRYPTR(wire_set_u8(c, out_msg->flags)); /* flags */
|
|
out_msg->protocol_version = DBUS_PROTOCOL_VERSION;
|
|
TRYPTR(wire_set_u8(c, DBUS_PROTOCOL_VERSION)); /* protocol_version */
|
|
uint32_t *body_length = TRYPTR(wire_set_u32(c, 0)); /* body length */
|
|
TRYPTR(wire_set_u32(c, 1)); /* serial */
|
|
uint32_t *header_fields_length = TRYPTR(wire_set_u32(c, 0)); /* header_fields_length */
|
|
|
|
uint32_t header_fields_start = c->byte_cursor;
|
|
|
|
/* header fields */
|
|
{
|
|
/* SENDER */
|
|
{
|
|
out_msg->fields[DBUS_HEADER_FIELD_SENDER].t.str = "org.freedesktop.DBus";
|
|
out_msg->fields[DBUS_HEADER_FIELD_SENDER].present = true;
|
|
|
|
/* byte (field code) */
|
|
TRYPTR(wire_set_u8(c, DBUS_HEADER_FIELD_SENDER));
|
|
/* variant */
|
|
/* signature */
|
|
TRYPTR(wire_set_signature(c, "s"));
|
|
/* string */
|
|
TRYPTR(wire_set_string(c, "org.freedesktop.DBus"));
|
|
|
|
// need to align to 8 byte boundry after each array element?
|
|
TRYPTR(wire_write_align(c, 8));
|
|
}
|
|
|
|
/* INTERFACE */
|
|
{
|
|
out_msg->fields[DBUS_HEADER_FIELD_INTERFACE].t.str = "org.freedesktop.DBus";
|
|
out_msg->fields[DBUS_HEADER_FIELD_INTERFACE].present = true;
|
|
|
|
/* byte (field code) */
|
|
TRYPTR(wire_set_u8(c, DBUS_HEADER_FIELD_INTERFACE));
|
|
/* variant */
|
|
/* signature */
|
|
TRYPTR(wire_set_signature(c, "s"));
|
|
/* string */
|
|
TRYPTR(wire_set_string(c, "org.freedesktop.DBus"));
|
|
|
|
// need to align to 8 byte boundry after each array element?
|
|
TRYPTR(wire_write_align(c, 8));
|
|
}
|
|
|
|
/* PATH */
|
|
{
|
|
out_msg->fields[DBUS_HEADER_FIELD_PATH].t.str = "/org/freedesktop/DBus";
|
|
out_msg->fields[DBUS_HEADER_FIELD_PATH].present = true;
|
|
|
|
/* byte (field code) */
|
|
TRYPTR(wire_set_u8(c, DBUS_HEADER_FIELD_PATH));
|
|
/* variant */
|
|
/* signature */
|
|
TRYPTR(wire_set_signature(c, "o"));
|
|
/* string */
|
|
TRYPTR(wire_set_string(c, "/org/freedesktop/DBus"));
|
|
|
|
// need to align to 8 byte boundry after each array element?
|
|
TRYPTR(wire_write_align(c, 8));
|
|
}
|
|
|
|
/* MEMBER */
|
|
{
|
|
out_msg->fields[DBUS_HEADER_FIELD_MEMBER].t.str = (char*)member;
|
|
out_msg->fields[DBUS_HEADER_FIELD_MEMBER].present = true;
|
|
|
|
/* byte (field code) */
|
|
TRYPTR(wire_set_u8(c, DBUS_HEADER_FIELD_MEMBER));
|
|
/* variant */
|
|
/* signature */
|
|
TRYPTR(wire_set_signature(c, "s"));
|
|
/* string */
|
|
TRYPTR(wire_set_string(c, member));
|
|
|
|
// need to align to 8 byte boundry after each array element?
|
|
TRYPTR(wire_write_align(c, 8));
|
|
}
|
|
|
|
/* SIGNATURE */
|
|
{
|
|
out_msg->fields[DBUS_HEADER_FIELD_SIGNATURE].t.str = (char*)signature;
|
|
out_msg->fields[DBUS_HEADER_FIELD_SIGNATURE].present = true;
|
|
|
|
/* byte (field code) */
|
|
TRYPTR(wire_set_u8(c, DBUS_HEADER_FIELD_SIGNATURE));
|
|
/* variant */
|
|
/* signature */
|
|
TRYPTR(wire_set_signature(c, "g"));
|
|
/* signature */
|
|
TRYPTR(wire_set_signature(c, signature));
|
|
}
|
|
}
|
|
|
|
*header_fields_length = c->byte_cursor - header_fields_start;
|
|
|
|
// header ends on an 8 byte alignment
|
|
TRYPTR(wire_write_align(c, 8));
|
|
|
|
if (out_body_length) {
|
|
*out_body_length = body_length;
|
|
}
|
|
|
|
return 0;
|
|
}
|
|
|
|
int wire_compose_reply(WireCtx *c, WireMsg *msg, const char *signature, uint32_t **out_body_length)
|
|
{
|
|
|
|
TRYPTR(wire_set_u8(c, msg->endianness)); /* endianness */
|
|
TRYPTR(wire_set_u8(c, DBUS_MESSAGE_METHOD_RETURN)); /* type */
|
|
TRYPTR(wire_set_u8(c, 0 | DBUS_FLAG_NO_REPLY_EXPECTED)); /* flags */
|
|
TRYPTR(wire_set_u8(c, DBUS_PROTOCOL_VERSION)); /* protocol_version */
|
|
uint32_t *body_length = TRYPTR(wire_set_u32(c, 0)); /* body length */
|
|
TRYPTR(wire_set_u32(c, msg->serial+1)); /* serial */
|
|
uint32_t *header_fields_length = TRYPTR(wire_set_u32(c, 0)); /* header_fields_length */
|
|
|
|
uint32_t header_fields_start = c->byte_cursor;
|
|
|
|
/* header fields */
|
|
{
|
|
/* SENDER */
|
|
{
|
|
/* byte (field code) */
|
|
TRYPTR(wire_set_u8(c, DBUS_HEADER_FIELD_SENDER));
|
|
/* variant */
|
|
/* signature */
|
|
TRYPTR(wire_set_signature(c, "s"));
|
|
/* string */
|
|
TRYPTR(wire_set_string(c, "org.freedesktop.DBus"));
|
|
|
|
// need to align to 8 byte boundry after each array element?
|
|
TRYPTR(wire_write_align(c, 8));
|
|
}
|
|
|
|
/* SIGNATURE */
|
|
{
|
|
/* byte (field code) */
|
|
TRYPTR(wire_set_u8(c, DBUS_HEADER_FIELD_SIGNATURE));
|
|
/* variant */
|
|
/* signature */
|
|
TRYPTR(wire_set_signature(c, "g"));
|
|
/* signature */
|
|
TRYPTR(wire_set_signature(c, signature));
|
|
|
|
// need to align to 8 byte boundry after each array element?
|
|
TRYPTR(wire_write_align(c, 8));
|
|
}
|
|
|
|
/* REPLY_SERIAL */
|
|
{
|
|
/* byte (field code) */
|
|
TRYPTR(wire_set_u8(c, DBUS_HEADER_FIELD_REPLY_SERIAL));
|
|
/* variant */
|
|
/* signature */
|
|
TRYPTR(wire_set_signature(c, "u"));
|
|
/* reply serial */
|
|
TRYPTR(wire_set_u32(c, msg->serial));
|
|
}
|
|
}
|
|
|
|
*header_fields_length = c->byte_cursor - header_fields_start;
|
|
|
|
// header ends on an 8 byte alignment
|
|
TRYPTR(wire_write_align(c, 8));
|
|
|
|
if (out_body_length) {
|
|
*out_body_length = body_length;
|
|
}
|
|
|
|
return 0;
|
|
}
|
|
|
|
int wire_compose_error(WireCtx *c, WireMsg *msg, const char *error_name)
|
|
{
|
|
|
|
TRYPTR(wire_set_u8(c, msg->endianness)); /* endianness */
|
|
TRYPTR(wire_set_u8(c, DBUS_MESSAGE_ERROR)); /* type */
|
|
TRYPTR(wire_set_u8(c, 0)); /* flags */
|
|
TRYPTR(wire_set_u8(c, DBUS_PROTOCOL_VERSION)); /* protocol_version */
|
|
TRYPTR(wire_set_u32(c, 0)); /* body length */
|
|
TRYPTR(wire_set_u32(c, msg->serial+1)); /* serial */
|
|
uint32_t *header_fields_length = TRYPTR(wire_set_u32(c, 0)); /* header_fields_length */
|
|
|
|
uint32_t header_fields_start = c->byte_cursor;
|
|
|
|
/* header fields */
|
|
{
|
|
/* ERROR_NAME */
|
|
{
|
|
/* byte (field code) */
|
|
TRYPTR(wire_set_u8(c, DBUS_HEADER_FIELD_ERROR_NAME));
|
|
/* variant */
|
|
/* signature */
|
|
TRYPTR(wire_set_signature(c, "s"));
|
|
/* string */
|
|
TRYPTR(wire_set_string(c, error_name));
|
|
|
|
// need to align to 8 byte boundry after each array element?
|
|
TRYPTR(wire_write_align(c, 8));
|
|
}
|
|
|
|
/* REPLY_SERIAL */
|
|
{
|
|
/* byte (field code) */
|
|
TRYPTR(wire_set_u8(c, DBUS_HEADER_FIELD_REPLY_SERIAL));
|
|
/* variant */
|
|
/* signature */
|
|
TRYPTR(wire_set_signature(c, "u"));
|
|
/* reply serial */
|
|
TRYPTR(wire_set_u32(c, msg->serial));
|
|
}
|
|
}
|
|
|
|
*header_fields_length = c->byte_cursor - header_fields_start;
|
|
|
|
// header ends on an 8 byte alignment
|
|
TRYPTR(wire_write_align(c, 8));
|
|
|
|
return 0;
|
|
}
|
|
|
|
int wire_compose_unicast_reply(WireCtx *msg_c, WireMsg *msg, char *sender_unique_name, struct iovec bufs[2], WireCtx *header_ctx)
|
|
{
|
|
TRYPTR(wire_set_u8(header_ctx, msg->endianness)); /* endianness */
|
|
TRYPTR(wire_set_u8(header_ctx, msg->type)); /* type */
|
|
TRYPTR(wire_set_u8(header_ctx, msg->flags)); /* flags */
|
|
TRYPTR(wire_set_u8(header_ctx, DBUS_PROTOCOL_VERSION)); /* protocol_version */
|
|
TRYPTR(wire_set_u32(header_ctx, msg->body_length)); /* body length */
|
|
TRYPTR(wire_set_u32(header_ctx, msg->serial)); /* serial */
|
|
uint32_t *header_fields_length = TRYPTR(wire_set_u32(header_ctx, 0)); /* header_fields_length */
|
|
|
|
// We don't need to do this because at this point we will always end up on an 8 byte alignment.
|
|
//TRYPTR(wire_write_align(header_ctx, 8));
|
|
|
|
uint32_t header_fields_start = header_ctx->byte_cursor;
|
|
|
|
TRYPTR(wire_set_u8(header_ctx, DBUS_HEADER_FIELD_SENDER));
|
|
TRYPTR(wire_set_signature_single_char(header_ctx, 's'));
|
|
TRYPTR(wire_set_string_fixed(header_ctx, sender_unique_name, UNIQUE_NAME_LENGTH));
|
|
|
|
// We don't need to do this because the unique names are fixed-size, such that we always end up on an 8 byte alignment here.
|
|
//TRYPTR(wire_write_align(header_ctx, 8));
|
|
|
|
*header_fields_length = header_ctx->byte_cursor - header_fields_start + msg->header_fields_length;
|
|
|
|
// TODO: is this right?
|
|
if (unlikely(msg_c->byte_cursor + msg->body_length >= msg_c->data_len)) {
|
|
return -1;
|
|
}
|
|
|
|
bufs[0] = (struct iovec){ header_ctx->data, header_ctx->byte_cursor };
|
|
bufs[1] = (struct iovec){ msg_c->data + msg->header_length, msg_c->byte_cursor + msg->body_length - msg->header_length };
|
|
|
|
msg_c->byte_cursor += msg->body_length;
|
|
|
|
return 0;
|
|
}
|
|
|
|
int wire_collect_strings(WireCtx *c, WireMsg *msg, WireMsgBodyString *strings, int strings_count)
|
|
{
|
|
if (!msg->fields[DBUS_HEADER_FIELD_SIGNATURE].present) {
|
|
return -1;
|
|
}
|
|
|
|
char *sig = msg->fields[DBUS_HEADER_FIELD_SIGNATURE].t.str;
|
|
int stri = 0;
|
|
int sigi = 0;
|
|
uint32_t body_cursor = c->byte_cursor;
|
|
|
|
while (*sig) {
|
|
switch (*sig) {
|
|
case 'y': {
|
|
TRYPTR(wire_get_u8(c));
|
|
} break;
|
|
case 'b': {
|
|
TRYPTR(wire_get_u32(c));
|
|
} break;
|
|
case 'n': {
|
|
TRYPTR(wire_get_i16(c));
|
|
} break;
|
|
case 'q': {
|
|
TRYPTR(wire_get_u16(c));
|
|
} break;
|
|
case 'i': {
|
|
TRYPTR(wire_get_i32(c));
|
|
} break;
|
|
case 'u': {
|
|
TRYPTR(wire_get_u32(c));
|
|
} break;
|
|
case 'o': /* through */
|
|
case 's': {
|
|
char *s = TRYPTR(wire_get_string(c));
|
|
if (sigi >= strings_count) {
|
|
return -1;
|
|
}
|
|
strings[sigi].index = sigi;
|
|
strings[sigi].str = s;
|
|
} break;
|
|
case 'g': {
|
|
char *s = TRYPTR(wire_get_signature(c));
|
|
if (sigi >= strings_count) {
|
|
return -1;
|
|
}
|
|
strings[sigi].index = sigi;
|
|
strings[sigi].str = s;
|
|
} break;
|
|
case 'a': {
|
|
uint32_t array_size = *(uint32_t*)TRYPTR(wire_get_u32(c));
|
|
uint32_t new_cursor = c->byte_cursor + array_size;
|
|
if (new_cursor >= c->data_len) {
|
|
return -1;
|
|
}
|
|
c->byte_cursor = new_cursor;
|
|
} break;
|
|
|
|
default: {
|
|
goto end_nonfatal;
|
|
}
|
|
}
|
|
|
|
sigi++;
|
|
sig++;
|
|
}
|
|
|
|
end_nonfatal:
|
|
|
|
c->byte_cursor = body_cursor;
|
|
|
|
return stri;
|
|
}
|