rcl_yaml_param_parser: add support for binary tag to load byte array parameter

rcl_yaml_param_parser/CMakeLists.txt

@@ -8,6 +8,7 @@ find_package(rcutils REQUIRED)
 find_package(rmw REQUIRED)
 find_package(libyaml_vendor REQUIRED)
 find_package(yaml REQUIRED)
+find_package(OpenSSL COMPONENTS Crypto REQUIRED)
 
 # Default to C++17
 if(NOT CMAKE_CXX_STANDARD)
@@ -40,6 +41,7 @@ target_link_libraries(${PROJECT_NAME} PUBLIC
 target_link_libraries(${PROJECT_NAME} PRIVATE
   rmw::rmw
   yaml
+  OpenSSL::Crypto
 )
 
 # Set the visibility to hidden by default if possible

rcl_yaml_param_parser/src/add_to_arrays.c

@@ -56,6 +56,22 @@ rcutils_ret_t add_val_to_bool_arr(
   ADD_VALUE_TO_SIMPLE_ARRAY(val_array, value, bool, allocator);
 }
 
+///
+/// Add a value to an byte array. Create the array if it does not exist
+///
+rcutils_ret_t add_val_to_byte_arr(
+  rcl_byte_array_t * const val_array,
+  uint8_t * value,
+  const rcutils_allocator_t allocator)
+{
+  RCUTILS_CHECK_ARGUMENT_FOR_NULL(val_array, RCUTILS_RET_INVALID_ARGUMENT);
+  RCUTILS_CHECK_ARGUMENT_FOR_NULL(value, RCUTILS_RET_INVALID_ARGUMENT);
+  RCUTILS_CHECK_ALLOCATOR_WITH_MSG(
+    &allocator, "invalid allocator", return RCUTILS_RET_INVALID_ARGUMENT);
+
+  ADD_VALUE_TO_SIMPLE_ARRAY(val_array, value, uint8_t, allocator);
+}
+
 ///
 /// Add a value to an integer array. Create the array if it does not exist
 ///

rcl_yaml_param_parser/src/impl/parse.h

@@ -37,7 +37,8 @@ void * get_value(
   yaml_scalar_style_t style,
   const yaml_char_t * const tag,
   data_types_t * val_type,
-  const rcutils_allocator_t allocator);
+  const rcutils_allocator_t allocator,
+  size_t * out_len);
 
 RCL_YAML_PARAM_PARSER_PUBLIC
 RCUTILS_WARN_UNUSED

rcl_yaml_param_parser/src/impl/types.h

@@ -45,7 +45,8 @@ typedef enum data_types_e
   DATA_TYPE_BOOL = 1U,
   DATA_TYPE_INT64 = 2U,
   DATA_TYPE_DOUBLE = 3U,
-  DATA_TYPE_STRING = 4U
+  DATA_TYPE_STRING = 4U,
+  DATA_TYPE_BYTE = 5U
 } data_types_t;
 
 typedef enum namespace_type_e

rcl_yaml_param_parser/src/parse.c

@@ -19,6 +19,9 @@
 
 #include <yaml.h>
 
+#include <openssl/bio.h>
+#include <openssl/evp.h>
+
 #include "rcutils/allocator.h"
 #include "rcutils/error_handling.h"
 #include "rcutils/format_string.h"
@@ -37,6 +40,12 @@
 #include "rcl_yaml_param_parser/parser.h"
 #include "rcl_yaml_param_parser/visibility_control.h"
 
+/// The tag @c !!binary for binary values. Not defined in libyaml
+#define BINARY_TAG          "tag:yaml.org,2002:binary"
+
+/// Global variable temporary used when using libcrypto
+const rcutils_allocator_t * g_alloc = NULL;
+
 ///
 /// Check a name space whether it is valid
 ///
@@ -72,6 +81,106 @@ RCL_YAML_PARAM_PARSER_LOCAL
 rcutils_ret_t
 _validate_name(const char * name, rcutils_allocator_t allocator);
 
+///
+/// malloc function using the allocator, to be used when decoding base64
+///
+void * _crypto_malloc(size_t num, const char *file, int line)
+{
+  (void)file;
+  (void)line;
+  return g_alloc->allocate(num, g_alloc->state);
+}
+
+///
+/// realloc function using the allocator, to be used when decoding base64
+///
+void * _crypto_realloc(void *addr, size_t num, const char *file, int line)
+{
+  (void)file;
+  (void)line;
+  return g_alloc->reallocate(addr, num, g_alloc->state);
+}
+
+///
+/// free function using the allocator, to be used when decoding base64
+///
+void _crypto_free(void *addr, const char *file, int line)
+{
+  (void)file;
+  (void)line;
+  g_alloc->deallocate(addr, g_alloc->state);
+}
+
+///
+/// Decode base64 string into uint8_t[] using OpenSSL library
+///
+rcutils_ret_t base64_decode(
+  const char * const input,
+  uint8_t ** output,
+  size_t * output_len,
+  const rcutils_allocator_t allocator
+)
+{
+  BIO *bio, *b64;
+  *output = NULL;
+  *output_len = 0;
+
+  RCUTILS_CHECK_ARGUMENT_FOR_NULL(input, -1);
+  RCUTILS_CHECK_ALLOCATOR_WITH_MSG(
+    &allocator, "allocator is invalid", return -1);
+
+  /// Tell libcrypto to use our allocator
+  g_alloc = &allocator;
+  CRYPTO_set_mem_functions(_crypto_malloc, _crypto_realloc, _crypto_free);
+
+  /// Create a memory BIO to hold the base64 data
+  bio = BIO_new_mem_buf(input, -1);
+  if (!bio) {
+    RCUTILS_SAFE_FWRITE_TO_STDERR("Failed to create BIO\n");
+    return RCUTILS_RET_ERROR;
+  }
+
+  // Create a base64 filter BIO
+  b64 = BIO_new(BIO_f_base64());
+  if (!b64) {
+    RCUTILS_SAFE_FWRITE_TO_STDERR("Failed to create base64 BIO\n");
+    BIO_free(bio);
+    return RCUTILS_RET_ERROR;
+  }
+
+  // Disable newlines (base64 decoder expects no newlines)
+  BIO_set_flags(b64, BIO_FLAGS_BASE64_NO_NL);
+
+  // Chain the BIOs
+  bio = BIO_push(b64, bio);
+
+  // Allocate a buffer for the decoded data (max possible size)
+  *output = allocator.zero_allocate(strlen(input), sizeof(char), allocator.state);
+  if (!*output) {
+    RCUTILS_SAFE_FWRITE_TO_STDERR("Failed to allocate memory\n");
+    BIO_free_all(bio);
+    return RCUTILS_RET_BAD_ALLOC;
+  }
+
+  // Decode the base64 data
+  *output_len = (size_t)BIO_read(bio, *output, (int)strlen(input));
+  if (*output_len <= 0) {
+    RCUTILS_SAFE_FWRITE_TO_STDERR("Failed to decode base64\n");
+    allocator.deallocate(*output, allocator.state);
+    *output = NULL;
+    BIO_free_all(bio);
+    return RCUTILS_RET_ERROR;
+  }
+
+  BIO_free_all(bio);
+
+  // Reset libcrypto memory functions
+  CRYPTO_set_mem_functions(NULL, NULL, NULL);
+  g_alloc = NULL;
+
+  return RCUTILS_RET_OK;
+}
+
 ///
 /// Determine the type of the value and return the converted value
 /// NOTE: Only canonical forms supported as of now
@@ -81,7 +190,8 @@ void * get_value(
   yaml_scalar_style_t style,
   const yaml_char_t * const tag,
   data_types_t * val_type,
-  const rcutils_allocator_t allocator)
+  const rcutils_allocator_t allocator,
+  size_t * out_len)
 {
   void * ret_val;
   int64_t ival;
@@ -90,6 +200,7 @@ void * get_value(
 
   RCUTILS_CHECK_ARGUMENT_FOR_NULL(value, NULL);
   RCUTILS_CHECK_ARGUMENT_FOR_NULL(val_type, NULL);
+  RCUTILS_CHECK_ARGUMENT_FOR_NULL(out_len, NULL);
   RCUTILS_CHECK_ALLOCATOR_WITH_MSG(
     &allocator, "allocator is invalid", return NULL);
 
@@ -99,6 +210,16 @@ void * get_value(
     return rcutils_strdup(value, allocator);
   }
 
+  /// Check for yaml binary tag
+  if (tag != NULL && strcmp(BINARY_TAG, (char *)tag) == 0) {
+    *val_type = DATA_TYPE_BYTE;
+
+    if (RCUTILS_RET_OK != base64_decode(value, (uint8_t **)&ret_val, out_len, allocator)) {
+      return NULL;
+    }
+    return ret_val;
+  }
+
   /// Check if it is bool
   if (style != YAML_SINGLE_QUOTED_SCALAR_STYLE &&
     style != YAML_DOUBLE_QUOTED_SCALAR_STYLE)
@@ -267,7 +388,8 @@ rcutils_ret_t parse_value(
   rcl_variant_t * param_value = &(params_st->params[node_idx].parameter_values[parameter_idx]);
 
   data_types_t val_type;
-  void * ret_val = get_value(value, style, tag, &val_type, allocator);
+  size_t out_len = 0;
+  void * ret_val = get_value(value, style, tag, &val_type, allocator, &out_len);
   if (NULL == ret_val) {
     RCUTILS_SET_ERROR_MSG_WITH_FORMAT_STRING(
       "Error parsing value %s at line %d", value, line_num);
@@ -448,6 +570,27 @@ rcutils_ret_t parse_value(
         }
       }
       break;
+    case DATA_TYPE_BYTE:
+      if (NULL != param_value->byte_array_value) {
+        // Overwriting, deallocate original
+        if (NULL != param_value->byte_array_value->values) {
+          allocator.deallocate(param_value->byte_array_value->values, allocator.state);
+        }
+        allocator.deallocate(param_value->byte_array_value, allocator.state);
+        param_value->byte_array_value = NULL;
+      }
+      *seq_data_type = val_type;
+      param_value->byte_array_value =
+        allocator.zero_allocate(1UL, sizeof(rcl_byte_array_t), allocator.state);
+      if (NULL == param_value->byte_array_value) {
+        allocator.deallocate(ret_val, allocator.state);
+        RCUTILS_SAFE_FWRITE_TO_STDERR("Error allocating mem\n");
+        ret = RCUTILS_RET_BAD_ALLOC;
+        break;
+      }
+      param_value->byte_array_value->values = ret_val;
+      param_value->byte_array_value->size = out_len;
+      break;
     default:
       RCUTILS_SET_ERROR_MSG_WITH_FORMAT_STRING(
         "Unknown data type of value %s at line %d", value, line_num);

rcl_yaml_param_parser/src/parser.c

@@ -407,6 +407,16 @@ void rcl_yaml_node_struct_print(
               printf(": %lf\n", *(param_var->double_value));
             } else if (NULL != param_var->string_value) {
               printf(": %s\n", param_var->string_value);
+            } else if (NULL != param_var->byte_array_value) {
+              printf(": ");
+              for (size_t i = 0; i < param_var->byte_array_value->size; i++) {
+                if (param_var->byte_array_value->values) {
+                  printf(
+                    "0x%02x, ",
+                    param_var->byte_array_value->values[i]);
+                }
+              }
+              printf("\n");
             } else if (NULL != param_var->bool_array_value) {
               printf(": ");
               for (size_t i = 0; i < param_var->bool_array_value->size; i++) {

rcl_yaml_param_parser/src/yaml_variant.c

@@ -80,6 +80,12 @@ void rcl_yaml_variant_fini(
   } else if (NULL != param_var->string_value) {
     allocator.deallocate(param_var->string_value, allocator.state);
     param_var->string_value = NULL;
+  } else if (NULL != param_var->byte_array_value) {
+    if (NULL != param_var->byte_array_value->values) {
+      allocator.deallocate(param_var->byte_array_value->values, allocator.state);
+    }
+    allocator.deallocate(param_var->byte_array_value, allocator.state);
+    param_var->byte_array_value = NULL;
   } else if (NULL != param_var->bool_array_value) {
     if (NULL != param_var->bool_array_value->values) {
       allocator.deallocate(param_var->bool_array_value->values, allocator.state);
@@ -132,6 +138,10 @@ bool rcl_yaml_variant_copy(
       RCUTILS_SAFE_FWRITE_TO_STDERR("Error allocating variant mem when copying string_value\n");
       return false;
     }
+  } else if (NULL != param_var->byte_array_value) {
+    RCL_YAML_VARIANT_COPY_ARRAY_VALUE(
+      out_param_var->byte_array_value, param_var->byte_array_value, allocator,
+      rcl_byte_array_t, uint8_t);
   } else if (NULL != param_var->bool_array_value) {
     RCL_YAML_VARIANT_COPY_ARRAY_VALUE(
       out_param_var->bool_array_value, param_var->bool_array_value, allocator,

rcl_yaml_param_parser/test/benchmark/benchmark_variant.cpp

@@ -121,6 +121,36 @@ BENCHMARK_F(PerformanceTest, string_copy_variant)(benchmark::State & st)
   }
 }
 
+BENCHMARK_F(PerformanceTest, array_byte_copy_variant)(benchmark::State & st)
+{
+  rcl_variant_t src_variant{};
+  rcl_variant_t dest_variant{};
+  rcutils_allocator_t allocator = rcutils_get_default_allocator();
+  using ArrayT = std::remove_pointer<decltype(src_variant.byte_array_value)>::type;
+  src_variant.byte_array_value =
+    static_cast<ArrayT *>(allocator.allocate(sizeof(ArrayT), allocator.state));
+  using ValueT = std::remove_pointer<decltype(src_variant.byte_array_value->values)>::type;
+  src_variant.byte_array_value->values =
+    static_cast<ValueT *>(
+    allocator.zero_allocate(kSize, sizeof(ValueT), allocator.state));
+  OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT(
+  {
+    rcl_yaml_variant_fini(&src_variant, allocator);
+    rcl_yaml_variant_fini(&dest_variant, allocator);
+  });
+  src_variant.byte_array_value->size = kSize;
+
+  reset_heap_counters();
+
+  for (auto _ : st) {
+    RCUTILS_UNUSED(_);
+    if (!rcl_yaml_variant_copy(&dest_variant, &src_variant, allocator)) {
+      st.SkipWithError(rcutils_get_error_string().str);
+    }
+    rcl_yaml_variant_fini(&dest_variant, allocator);
+  }
+}
+
 BENCHMARK_F(PerformanceTest, array_bool_copy_variant)(benchmark::State & st)
 {
   rcl_variant_t src_variant{};

rcl_yaml_param_parser/test/correct_config.yaml

@@ -54,3 +54,6 @@ string_tag:
     string_bool: !!str yes
     string_int: !!str 1234
     string_double: !!str 12.34
+binary_tag:
+  ros__parameters:
+    byte_array: !!binary AQID

rcl_yaml_param_parser/test/test_parse.cpp

@@ -140,6 +140,35 @@ TEST(TestParse, parse_value_sequence) {
   ASSERT_EQ(RCUTILS_RET_OK, node_params_init(&params_st->params[0], allocator));
   params_st->num_nodes = 1u;
 
+  // byte array value
+  yaml_char_t byte_value[] = "AQ==";
+  const size_t byte_value_length = sizeof(byte_value) / sizeof(byte_value[0]);
+  event.data.scalar.value = byte_value;
+  event.data.scalar.length = byte_value_length;
+  // Set tag, needed to parse base64 encoded binary
+  yaml_char_t tag_value[] = "tag:yaml.org,2002:binary";
+  event.data.scalar.tag = tag_value;
+
+  // Check proper sequence type for byte
+  seq_data_type = DATA_TYPE_UNKNOWN;
+  EXPECT_EQ(
+    RCUTILS_RET_OK,
+    parse_value(event, is_seq, node_idx, parameter_idx, &seq_data_type, params_st)) <<
+    rcutils_get_error_string().str;
+  ASSERT_NE(
+    nullptr, params_st->params[node_idx].parameter_values[parameter_idx].byte_array_value);
+  EXPECT_EQ(
+    params_st->params[node_idx].parameter_values[parameter_idx].byte_array_value->values[0], 1);
+  allocator.deallocate(
+    params_st->params[node_idx].parameter_values[parameter_idx].byte_array_value->values,
+    allocator.state);
+  allocator.deallocate(
+    params_st->params[node_idx].parameter_values[parameter_idx].byte_array_value, allocator.state);
+  params_st->params[node_idx].parameter_values[parameter_idx].byte_array_value = nullptr;
+
+  // Erase tag
+  event.data.scalar.tag = NULL;
+
   // bool array value
   yaml_char_t bool_value[] = "true";
   const size_t bool_value_length = sizeof(bool_value) / sizeof(bool_value[0]);
@@ -154,7 +183,7 @@ TEST(TestParse, parse_value_sequence) {
     rcutils_get_error_string().str;
   EXPECT_EQ(
     nullptr,
-    params_st->params[node_idx].parameter_values[parameter_idx].integer_array_value);
+    params_st->params[node_idx].parameter_values[parameter_idx].bool_array_value);
   rcutils_reset_error();
 
   // Check proper sequence type