diff --git a/include/skipstone/message.h b/include/skipstone/message.h index 63b56e2..e529eff 100644 --- a/include/skipstone/message.h +++ b/include/skipstone/message.h @@ -14,4 +14,8 @@ ssize_t skipstone_message_pack(void *message, size_t len, const char *template, ...); +ssize_t skipstone_message_unpack(void *message, + size_t len, + const char *template, ...); + #endif /* _SKIPSTONE_MESSAGE_H */ diff --git a/src/message.c b/src/message.c index a7592f6..5693f31 100644 --- a/src/message.c +++ b/src/message.c @@ -95,3 +95,77 @@ error_invalid_template: return -1; } + +#define case_unpack(t, bytes) \ + case t: \ + if (len < offset + bytes) { \ + goto done; \ + } \ + memcpy(va_arg(args, void *), (uint8_t *)message + offset, bytes); \ + offset += bytes; \ + break; + +ssize_t skipstone_message_unpack(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_unpack('c', sizeof(int8_t)); + case_unpack('C', sizeof(uint8_t)); + case_unpack('s', sizeof(int16_t)); + case_unpack('S', sizeof(uint16_t)); + case_unpack('l', sizeof(int32_t)); + case_unpack('L', sizeof(uint32_t)); + case_unpack('q', sizeof(int64_t)); + case_unpack('Q', sizeof(uint64_t)); + + case 'z': { + uint8_t sz; + void *dest; + + if (len < offset + 1) { + goto done; + } + + sz = ((uint8_t *)message)[offset++]; + + if (len < offset + sz + 1) { + goto done; + } + + dest = va_arg(args, void *); + + memcpy(dest, (uint8_t *)message + offset, sz); + + offset += sz; + + ((uint8_t *)message)[offset++] = '\0'; + + break; + } + + default: + errno = EINVAL; + + goto error_invalid; + } + } + + va_end(args); + +done: + return offset; + +error_invalid: + va_end(args); + + return -1; +}