231 lines
7.5 KiB
C
231 lines
7.5 KiB
C
|
#include <string.h>
|
||
|
|
#include <stdio.h>
|
||
|
|
#include "try.h"
|
||
|
|
#include "wire.h"
|
||
|
|
|
||
|
|
bool wire_align(wire_context_t *c, uint32_t alignment)
|
||
|
|
{
|
||
|
|
if ((c->byte_cursor % alignment) == 0) {
|
||
|
|
return true;
|
||
|
|
}
|
||
|
|
uint32_t new_cursor = c->byte_cursor + alignment - (c->byte_cursor % alignment);
|
||
|
|
if (new_cursor >= c->data_len || new_cursor < c->byte_cursor) {
|
||
|
|
return false;
|
||
|
|
}
|
||
|
|
c->byte_cursor = new_cursor;
|
||
|
|
return true;
|
||
|
|
}
|
||
|
|
|
||
|
|
bool wire_write_align(wire_context_t *c, uint32_t alignment)
|
||
|
|
{
|
||
|
|
if ((c->byte_cursor % alignment) == 0) {
|
||
|
|
return true;
|
||
|
|
}
|
||
|
|
uint32_t new_cursor = c->byte_cursor + alignment - (c->byte_cursor % alignment);
|
||
|
|
if (new_cursor >= c->data_len || new_cursor < c->byte_cursor) {
|
||
|
|
return false;
|
||
|
|
}
|
||
|
|
while (c->byte_cursor < new_cursor) {
|
||
|
|
c->data[c->byte_cursor] = '\0';
|
||
|
|
c->byte_cursor++;
|
||
|
|
}
|
||
|
|
return true;
|
||
|
|
}
|
||
|
|
|
||
|
|
// 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);
|
||
|
|
|
||
|
|
char *wire_get_string(wire_context_t *c, uint32_t *out_len)
|
||
|
|
{
|
||
|
|
uint32_t len = *TRY_NONNULL(uint32_t*, wire_get_u32(c), NULL);
|
||
|
|
if (out_len) {
|
||
|
|
*out_len = len;
|
||
|
|
}
|
||
|
|
|
||
|
|
uint32_t new_cursor = c->byte_cursor + len + 1; /* + 1 because of the extra null termination that strings have */
|
||
|
|
if (new_cursor >= c->data_len) {
|
||
|
|
return NULL;
|
||
|
|
}
|
||
|
|
|
||
|
|
char *str = (char *)&c->data[c->byte_cursor];
|
||
|
|
|
||
|
|
c->byte_cursor = new_cursor;
|
||
|
|
|
||
|
|
return str;
|
||
|
|
}
|
||
|
|
|
||
|
|
char *wire_set_string(wire_context_t *c, const char *str)
|
||
|
|
{
|
||
|
|
uint32_t len = strlen(str);
|
||
|
|
TRY_NONNULL(uint32_t*, wire_set_u32(c, len), NULL);
|
||
|
|
|
||
|
|
uint32_t new_cursor = c->byte_cursor + len + 1; /* + 1 because of the extra null termination that strings have */
|
||
|
|
if (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_signature(wire_context_t *c, uint8_t *out_len)
|
||
|
|
{
|
||
|
|
uint8_t len = *TRY_NONNULL(uint8_t*, wire_get_u8(c), NULL);
|
||
|
|
if (out_len) {
|
||
|
|
*out_len = len;
|
||
|
|
}
|
||
|
|
|
||
|
|
uint32_t new_cursor = c->byte_cursor + len + 1; /* + 1 because of the extra null termination that strings have */
|
||
|
|
if (new_cursor >= c->data_len) {
|
||
|
|
return NULL;
|
||
|
|
}
|
||
|
|
|
||
|
|
char *str = (char *)&c->data[c->byte_cursor];
|
||
|
|
|
||
|
|
c->byte_cursor = new_cursor;
|
||
|
|
|
||
|
|
return str;
|
||
|
|
}
|
||
|
|
|
||
|
|
char *wire_set_signature(wire_context_t *c, const char *str)
|
||
|
|
{
|
||
|
|
uint8_t len = strlen(str);
|
||
|
|
TRY_NONNULL(uint8_t*, wire_set_u8(c, len), NULL);
|
||
|
|
|
||
|
|
uint32_t new_cursor = c->byte_cursor + len + 1; /* + 1 because of the extra null termination that strings have */
|
||
|
|
if (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;
|
||
|
|
}
|
||
|
|
|
||
|
|
int wire_parse_message(wire_context_t *c, wire_message_t *msg)
|
||
|
|
{
|
||
|
|
// SPEC: https://dbus.freedesktop.org/doc/dbus-specification.html#message-protocol-messages
|
||
|
|
msg->endianness = *TRY_NONNULL(uint8_t*, wire_get_u8(c), -1);
|
||
|
|
msg->type = *TRY_NONNULL(uint8_t*, wire_get_u8(c), -1);
|
||
|
|
msg->flags = *TRY_NONNULL(uint8_t*, wire_get_u8(c), -1);
|
||
|
|
msg->protocol_version = *TRY_NONNULL(uint8_t*, wire_get_u8(c), -1);
|
||
|
|
msg->body_length = *TRY_NONNULL(uint32_t*, wire_get_u32(c), -1);
|
||
|
|
msg->serial = *TRY_NONNULL(uint32_t*, wire_get_u32(c), -1);
|
||
|
|
msg->header_fields_length = *TRY_NONNULL(uint32_t*, wire_get_u32(c), -1);
|
||
|
|
|
||
|
|
size_t header_fields_end = c->byte_cursor + msg->header_fields_length;
|
||
|
|
|
||
|
|
while (c->byte_cursor < header_fields_end) {
|
||
|
|
uint8_t field_code = *TRY_NONNULL(uint8_t*, wire_get_u8(c), -1);
|
||
|
|
TRY_NONNULL(char*, wire_get_signature(c, NULL), -1);
|
||
|
|
|
||
|
|
msg->fields[msg->header_fields_count].type = field_code;
|
||
|
|
|
||
|
|
switch (field_code) {
|
||
|
|
case DBUS_HEADER_FIELD_PATH: {
|
||
|
|
char *str = TRY_NONNULL(char*, wire_get_string(c, NULL), -1);
|
||
|
|
msg->fields[msg->header_fields_count].t.str = str;
|
||
|
|
printf("DBUS_HEADER_FIELD_PATH: %s\n", str);
|
||
|
|
} break;
|
||
|
|
case DBUS_HEADER_FIELD_INTERFACE: {
|
||
|
|
char *str = TRY_NONNULL(char*, wire_get_string(c, NULL), -1);
|
||
|
|
msg->fields[msg->header_fields_count].t.str = str;
|
||
|
|
printf("DBUS_HEADER_FIELD_INTERFACE: %s\n", str);
|
||
|
|
} break;
|
||
|
|
case DBUS_HEADER_FIELD_MEMBER: {
|
||
|
|
char *str = TRY_NONNULL(char*, wire_get_string(c, NULL), -1);
|
||
|
|
msg->fields[msg->header_fields_count].t.str = str;
|
||
|
|
printf("DBUS_HEADER_FIELD_MEMBER: %s\n", str);
|
||
|
|
} break;
|
||
|
|
case DBUS_HEADER_FIELD_DESTINATION: {
|
||
|
|
char *str = TRY_NONNULL(char*, wire_get_string(c, NULL), -1);
|
||
|
|
msg->fields[msg->header_fields_count].t.str = str;
|
||
|
|
printf("DBUS_HEADER_FIELD_DESTINATION: %s\n", str);
|
||
|
|
} break;
|
||
|
|
|
||
|
|
default: {
|
||
|
|
printf("header: unknown field: %d\n", field_code);
|
||
|
|
return -1;
|
||
|
|
} break;
|
||
|
|
}
|
||
|
|
|
||
|
|
// Structs are always aligned to `8`.
|
||
|
|
// SPEC: https://dbus.freedesktop.org/doc/dbus-specification.html#id-1.4.6
|
||
|
|
TRY_NONNULL(bool, wire_align(c, 8), -1);
|
||
|
|
|
||
|
|
if (msg->header_fields_count++ > WIRE_MESSAGE_MAX_HEADER_FIELDS) {
|
||
|
|
return -1;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
return 0;
|
||
|
|
}
|
||
|
|
|
||
|
|
int wire_compose_reply(wire_context_t *c, wire_message_t *msg, const char *signature, uint32_t **out_body_length)
|
||
|
|
{
|
||
|
|
|
||
|
|
TRY_NONNULL(uint8_t*, wire_set_u8(c, msg->endianness), -1); /* endianness */
|
||
|
|
TRY_NONNULL(uint8_t*, wire_set_u8(c, DBUS_MESSAGE_METHOD_RETURN), -1); /* type */
|
||
|
|
TRY_NONNULL(uint8_t*, wire_set_u8(c, 0), -1); /* flags */
|
||
|
|
TRY_NONNULL(uint8_t*, wire_set_u8(c, DBUS_PROTOCOL_VERSION), -1); /* protocol_version */
|
||
|
|
uint32_t *body_length = TRY_NONNULL(uint32_t*, wire_set_u32(c, 0), -1); /* body length */
|
||
|
|
TRY_NONNULL(uint32_t*, wire_set_u32(c, msg->serial+1), -1); /* serial */
|
||
|
|
uint32_t *header_fields_length = TRY_NONNULL(uint32_t*, wire_set_u32(c, 0), -1); /* header_fields_length */
|
||
|
|
|
||
|
|
uint32_t header_fields_start = c->byte_cursor;
|
||
|
|
|
||
|
|
/* header fields */
|
||
|
|
{
|
||
|
|
/* SIGNATURE */
|
||
|
|
{
|
||
|
|
/* byte (field code) */
|
||
|
|
TRY_NONNULL(uint8_t*, wire_set_u8(c, DBUS_HEADER_FIELD_SIGNATURE), -1);
|
||
|
|
/* variant */
|
||
|
|
/* signature */
|
||
|
|
TRY_NONNULL(char*, wire_set_signature(c, "g"), -1);
|
||
|
|
/* signature */
|
||
|
|
TRY_NONNULL(char*, wire_set_signature(c, signature), -1);
|
||
|
|
|
||
|
|
// need to align to 8 byte boundry after each array element?
|
||
|
|
TRY_NONNULL(bool, wire_write_align(c, 8), -1);
|
||
|
|
}
|
||
|
|
|
||
|
|
/* REPLY_SERIAL */
|
||
|
|
{
|
||
|
|
/* byte (field code) */
|
||
|
|
TRY_NONNULL(uint8_t*, wire_set_u8(c, DBUS_HEADER_FIELD_REPLY_SERIAL), -1);
|
||
|
|
/* variant */
|
||
|
|
/* signature */
|
||
|
|
TRY_NONNULL(char*, wire_set_signature(c, "u"), -1);
|
||
|
|
/* reply serial */
|
||
|
|
TRY_NONNULL(uint32_t*, wire_set_u32(c, msg->serial), -1);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
*header_fields_length = c->byte_cursor - header_fields_start;
|
||
|
|
|
||
|
|
// header ends on an 8 byte alignment
|
||
|
|
TRY_NONNULL(bool, wire_write_align(c, 8), -1);
|
||
|
|
|
||
|
|
if (out_body_length) {
|
||
|
|
*out_body_length = body_length;
|
||
|
|
}
|
||
|
|
|
||
|
|
return 0;
|
||
|
|
}
|