static size_t bundle_decode_block(struct mmem* const bundlemem, const uint8_t* const buffer, const size_t max_len)
{
uint8_t type;
int block_offs = 0;
size_t offs = 0;
uint32_t flags, size;
struct bundle_t *bundle;
struct bundle_block_t *block;
int n;
type = buffer[offs];
offs++;
/* Flags */
offs += sdnv_decode(&buffer[offs], max_len-offs, &flags);
/* Payload Size */
offs += sdnv_decode(&buffer[offs], max_len-offs, &size);
if (size > max_len-offs) {
LOG(LOGD_DTN, LOG_BUNDLE, LOGL_ERR, "Bundle payload length too big: %lu > %lu", size, max_len-offs);
return 0;
}
block_offs = bundlemem->size;
if( type == BUNDLE_BLOCK_TYPE_AEB ) {
// TODO remove const cast
return offs + bundle_ageing_parse_age_extension_block(bundlemem, type, flags, (uint8_t*)&buffer[offs], size);
}
n = mmem_realloc(bundlemem, bundlemem->size + sizeof(struct bundle_block_t) + size);
if( !n ) {
LOG(LOGD_DTN, LOG_BUNDLE, LOGL_ERR, "Bundle payload length too big for MMEM.");
return 0;
}
bundle = (struct bundle_t *) MMEM_PTR(bundlemem);
bundle->num_blocks++;
/* Add the block to the end of the bundle */
block = (struct bundle_block_t *)((uint8_t *)bundle + block_offs);
block->type = type;
block->flags = flags;
block->block_size = size;
/* Copy the actual payload over */
memcpy(block->payload, &buffer[offs], block->block_size);
return offs + block->block_size;
}
/**
* \brief Function that could parse the IPND service block
* \param buffer Pointer to the block
* \param length Length of the block
* \return the number of decoded bytes
*/
uint8_t discovery_ipnd_parse_service_block(uint32_t eid, const uint8_t* buffer, const uint8_t length, ipnd_msg_attrs_t* const attrs) {
uint32_t offset, num_services, i, sdnv_len, tag_len, data_len;
uint8_t *tag_buf;
int h;
// decode the number of services
sdnv_len = sdnv_decode(buffer, length, &num_services);
offset = sdnv_len;
buffer += sdnv_len;
// iterate through all services
for (i = 0; num_services > i; ++i) {
// decode service tag length
sdnv_len = sdnv_decode(buffer, length - offset, &tag_len);
offset += sdnv_len;
buffer += sdnv_len;
// decode service tag string
tag_buf = (uint8_t*)buffer;
offset += tag_len;
buffer += tag_len;
// decode service content length
sdnv_len = sdnv_decode(buffer, length - offset, &data_len);
offset += sdnv_len;
buffer += sdnv_len;
/* parse UDP-CL service data, if available */
const size_t udpcl_len = STATIC_STRLEN(DISCOVERY_IPND_SERVICE_UDP);
if (tag_len == udpcl_len && memcmp(tag_buf, DISCOVERY_IPND_SERVICE_UDP, udpcl_len) == 0) {
// TODO warn if the port will be overwritten
discovery_ipnd_parse_service_param(buffer, data_len, attrs);
}
// Allow all registered DTN APPs to parse the IPND service block
for(h=0; dtn_apps[h] != NULL; h++) {
if( dtn_apps[h]->parse_ipnd_service_block == NULL ) {
continue;
}
dtn_apps[h]->parse_ipnd_service_block(eid, tag_buf, tag_len, (uint8_t*)buffer, data_len);
}
offset += data_len;
buffer += data_len;
}
return offset;
}
/**
* \brief Parses the age extension block
* \param bundlemem Bundle MMEM Pointer
* \param type Block type
* \param flags Block Flags
* \param buffer Block Payload Pointer
* \param length Block Payload Length
* \return Length of parsed block payload
*/
uint8_t bundle_ageing_parse_age_extension_block(struct mmem *bundlemem, uint8_t type, uint32_t flags, uint8_t * buffer, int length) {
uint8_t offset = 0;
struct bundle_t *bundle;
/* Check for the proper block type */
if( type != BUNDLE_BLOCK_TYPE_AEB ) {
return 0;
}
if( bundlemem == NULL ) {
return 0;
}
bundle = (struct bundle_t *) MMEM_PTR(bundlemem);
if( bundle == NULL ) {
return 0;
}
#if UDTN_SUPPORT_LONG_AEB
/* Decode the age block value */
if( sdnv_len(buffer) > 4 ) {
// 64 bit operations are expensive - avoid them where possible
uint64_t age = 0;
offset = sdnv_decode_long(buffer, length, &age);
// Convert Age to milliseconds
bundle->aeb_value_ms = (uint32_t) (age / 1000);
} else {
uint32_t age = 0;
offset = sdnv_decode(buffer, length, &age);
// Convert Age to milliseconds
bundle->aeb_value_ms = age / 1000;
}
#else
uint32_t age = 0;
offset = sdnv_decode(buffer, length, &age);
// Convert Age to milliseconds
bundle->aeb_value_ms = age / 1000;
#endif
return offset;
}
int main(int argc, char* argv[])
{
size_t i;
memset(buf, 0, sizeof(buf));
progname = strrchr(argv[0], '/');
if (progname == 0) {
progname = argv[0];
} else {
progname++;
}
if (!strcmp(progname, "num2sdnv")) {
mode = ENCODE;
} else if (!strcmp(progname, "sdnv2num")) {
mode = DECODE;
}
argv++;
argc--;
while (argc > 1) {
char* arg = argv[0];
argv++;
argc--;
if (!strcmp(arg, "-e")) {
mode = ENCODE;
} else if (!strcmp(arg, "-d")) {
mode = DECODE;
} else {
fprintf(stderr, "unknown argument %s\n", arg);
exit(1);
}
}
if (argc != 1 || mode == 0) {
fprintf(stderr, "usage: %s [-de] <num>\n"
" -e encode number to sdnv\n"
" -d decode sdnv to number\n",
progname);
exit(1);
}
numstr = argv[0];
if (mode == ENCODE) {
if (numstr[0] == '0' && numstr[1] == 'x') {
val = strtoull(numstr + 2, &end, 16);
} else {
val = strtoull(numstr, &end, 10);
}
if (*end != '\0') {
fprintf(stderr, "invalid number %s\n", numstr);
exit(1);
}
len = sdnv_encode(val, buf, sizeof(buf));
} else {
if (numstr[0] == '0' && numstr[1] == 'x') {
numstr += 2;
}
if ((strlen(numstr) % 2) != 0) {
fprintf(stderr, "number string %s must contain full bytes\n",
numstr);
exit(1);
}
for (i = 0; i < strlen(numstr) / 2; ++i) {
buf[i] = (HEXTONUM(numstr[2*i]) << 4) +
HEXTONUM(numstr[2*i + 1]);
}
len = sdnv_decode(buf, strlen(numstr)/2, &val);
}
printf("val: %llu (0x%llx)\n", (unsigned long long)val, (unsigned long long)val);
printf("len: %d\n", len);
printf("sdnv: ");
if (len > 0) {
for (i = 0; i < (size_t)len; ++i) {
printf("%c%c", hex[(buf[i] >> 4) & 0xf], hex[buf[i] & 0xf]);
}
}
printf("\n");
return 0;
}
struct mmem *bundle_recover_bundle(const uint8_t* const buffer, const size_t size)
{
uint32_t primary_size, value;
size_t offs = 0;
struct mmem *bundlemem;
struct bundle_t *bundle;
int ret = 0;
bundlemem = bundle_create_bundle();
if (!bundlemem)
return NULL;
bundle = (struct bundle_t *) MMEM_PTR(bundlemem);
LOG(LOGD_DTN, LOG_BUNDLE, LOGL_DBG, "rec bptr: %p blptr:%p",bundle,buffer);
/* Version 0x06 is the one described and supported in RFC5050 */
if (buffer[0] != 0x06) {
LOG(LOGD_DTN, LOG_BUNDLE, LOGL_ERR, "Version 0x%02x not supported", buffer[0]);
goto err;
}
offs++;
/* Flags */
offs += sdnv_decode(&buffer[offs], size-offs, &bundle->flags);
/* Block Length - Number of bytes in this block following this
* field */
offs += sdnv_decode(&buffer[offs], size-offs, &primary_size);
primary_size += offs;
/*
* Use temp variable, otherwise raises hard fault exception.
* Variable is not aligned for correct offset,
* because of packed attribute.
* For example address has to be a multiply of 8.
* But packed attribute is needed,
* because of memory allocation for the payload block
*/
uint64_t sdnv_temp = 0;
/* Destination node + SSP */
offs += sdnv_decode(&buffer[offs], size-offs, &bundle->dst_node);
offs += sdnv_decode_long(&buffer[offs], size-offs, &sdnv_temp);
bundle->dst_srv = sdnv_temp;
/* Source node + SSP */
offs += sdnv_decode(&buffer[offs], size-offs, &bundle->src_node);
offs += sdnv_decode_long(&buffer[offs], size-offs, &sdnv_temp);
bundle->src_srv = sdnv_temp;
/* Report-to node + SSP */
offs += sdnv_decode(&buffer[offs], size-offs, &bundle->rep_node);
offs += sdnv_decode(&buffer[offs], size-offs, &bundle->rep_srv);
/* Custodian node + SSP */
offs += sdnv_decode(&buffer[offs], size-offs, &bundle->cust_node);
offs += sdnv_decode(&buffer[offs], size-offs, &bundle->cust_srv);
/* Creation Timestamp */
offs += sdnv_decode_long(&buffer[offs], size-offs, &sdnv_temp);
bundle->tstamp = sdnv_temp;
/* Creation Timestamp Sequence Number */
offs += sdnv_decode(&buffer[offs], size-offs, &bundle->tstamp_seq);
/* Lifetime */
offs += sdnv_decode(&buffer[offs], size-offs, &bundle->lifetime);
/* Directory Length */
offs += sdnv_decode(&buffer[offs], size-offs, &value);
if (value != 0) {
LOG(LOGD_DTN, LOG_BUNDLE, LOGL_ERR, "Bundle does not use CBHE.");
goto err;
}
if (bundle->flags & BUNDLE_FLAG_FRAGMENT) {
LOG(LOGD_DTN, LOG_BUNDLE, LOGL_INF, "Bundle is a fragment");
/* Fragment Offset */
offs += sdnv_decode(&buffer[offs], size-offs, &bundle->frag_offs);
/* Total App Data Unit Length */
offs += sdnv_decode(&buffer[offs], size-offs, &bundle->app_len);
}
if (offs != primary_size) {
LOG(LOGD_DTN, LOG_BUNDLE, LOGL_ERR, "Problem decoding the primary bundle block.");
goto err;
}
/* FIXME: Loop around and decode all blocks - does this work? */
while (size-offs > 1) {
ret = bundle_decode_block(bundlemem, &buffer[offs], size-offs);
/* If block decode failed, we are out of memory and have to abort */
if( ret < 1 ) {
goto err;
}
offs += ret;
}
return bundlemem;
err:
bundle_delete_bundle(bundlemem);
return NULL;
}
