95 lines
2.2 KiB
C
95 lines
2.2 KiB
C
#include <stdlib.h>
|
|
#include <stdint.h>
|
|
#include <stdarg.h>
|
|
#include <string.h>
|
|
#include <errno.h>
|
|
|
|
#include <skipstone/link.h>
|
|
#include <skipstone/map.h>
|
|
#include <skipstone/queue.h>
|
|
#include <skipstone/message.h>
|
|
|
|
#define case_pack(t, type_t) \
|
|
case t: { \
|
|
type_t v = va_arg(args, type_t); \
|
|
if (len < offset + sizeof(v)) { \
|
|
goto done; \
|
|
} else { \
|
|
memcpy((uint8_t *)message + offset, &v, sizeof(v)); \
|
|
offset += sizeof(v); \
|
|
} \
|
|
break; \
|
|
}
|
|
|
|
#define case_pack_int(t, type_t) \
|
|
case t: { \
|
|
type_t v = (type_t)va_arg(args, int); \
|
|
if (len < offset + sizeof(v)) { \
|
|
goto done; \
|
|
} else { \
|
|
memcpy((uint8_t *)message + offset, &v, sizeof(v)); \
|
|
offset += sizeof(v); \
|
|
} \
|
|
break; \
|
|
}
|
|
|
|
ssize_t skipstone_message_pack(void *message,
|
|
size_t len,
|
|
const char *template, ...) {
|
|
va_list args;
|
|
|
|
size_t offset = 0,
|
|
i;
|
|
|
|
va_start(args, template);
|
|
|
|
for (i=0; template[i]; i++) {
|
|
char c = template[i];
|
|
|
|
switch (c) {
|
|
case_pack_int('c', int8_t);
|
|
case_pack_int('C', uint8_t);
|
|
case_pack_int('s', int16_t);
|
|
case_pack_int('S', uint16_t);
|
|
case_pack('l', int32_t);
|
|
case_pack('L', uint32_t);
|
|
case_pack('q', int64_t);
|
|
case_pack('Q', uint64_t);
|
|
|
|
case 'z': {
|
|
char *value = va_arg(args, char *);
|
|
size_t sz = strlen(value);
|
|
|
|
if (len < offset + sz + 1) {
|
|
goto done;
|
|
} else if (sz > SKIPSTONE_MESSAGE_MAX_STRLEN) {
|
|
errno = EOVERFLOW;
|
|
|
|
goto error_toobig;
|
|
}
|
|
|
|
((uint8_t *)message)[offset++] = (uint8_t)sz;
|
|
|
|
memcpy((uint8_t *)message + offset, value, sz);
|
|
|
|
offset += sz;
|
|
|
|
break;
|
|
}
|
|
|
|
default:
|
|
errno = EINVAL;
|
|
|
|
goto error_invalid_template;
|
|
}
|
|
}
|
|
|
|
va_end(args);
|
|
|
|
done:
|
|
return offset;
|
|
|
|
error_toobig:
|
|
error_invalid_template:
|
|
return -1;
|
|
}
|