lib: os: cbprintf: Add alignment offset to packaging

Added parameter to CBPRINTF_STATIC_PACKAGE which indicates buffer
alignment offset compared to CBPRINTF_PACKAGE_ALIGNMENT. When offset
is set to 0, macro assumes that input buffer is aligned to
CBPRINTF_PACKAGE_ALIGNMENT. When offset is positive, macro assumes
that buffer address is shifted by given number of bytes to
CBPRINTF_PACKAGE_ALIGNMENT alignment.

Extended cbprintf_package to use len argument as alignment offset
indicator when calculating length only (package pointer is null).

Features are not available for xtensa platform which seems to
require 16 byte alignment from the package. It is only an assumption
due to lack of the documentation and may be fixed in the future.

Feature allows to avoid unnecessary padding when package is part of
a message and preceeded by a header of a known size. For example,
message header on 32 bit architecture has 12 bytes, long doubles are
not used so cbprintf requires 8 byte alignment. Without alignment
offset indicator, package containing just a string with one argument
would need 4 byte padding after the header and 4 byte padding after
the package. Message would be 32 bytes long. With alignment offset
indication both paddings are not needed and message is only 24 bytes
long.

Signed-off-by: Krzysztof Chruscinski <krzysztof.chruscinski@nordicsemi.no>

include/sys/cbprintf.h

@@ -45,9 +45,13 @@ extern "C" {
  */
 
 /** @brief Required alignment of the buffer used for packaging. */
+#ifdef __xtensa__
+#define CBPRINTF_PACKAGE_ALIGNMENT 16
+#else
 #define CBPRINTF_PACKAGE_ALIGNMENT \
 	(IS_ENABLED(CONFIG_CBPRINTF_PACKAGE_LONGDOUBLE) ? \
 		sizeof(long double) : MAX(sizeof(double), sizeof(long long)))
+#endif
 
 /** @brief Signature for a cbprintf callback function.
  *
@@ -107,10 +111,17 @@ typedef int (*cbprintf_cb)(/* int c, void *ctx */);
  * store the packed information. If input buffer was too small it is set to
  * -ENOSPC.
  *
+ * @param align_offset input buffer alignment offset in bytes. Where offset 0
+ * means that buffer is aligned to CBPRINTF_PACKAGE_ALIGNMENT. Xtensa requires
+ * that @p packaged is aligned to CBPRINTF_PACKAGE_ALIGNMENT so it must be
+ * multiply of CBPRINTF_PACKAGE_ALIGNMENT or 0.
+ *
  * @param ... formatted string with arguments. Format string must be constant.
  */
-#define CBPRINTF_STATIC_PACKAGE(packaged, inlen, outlen, ... /* fmt, ... */) \
-		Z_CBPRINTF_STATIC_PACKAGE(packaged, inlen, outlen, __VA_ARGS__)
+#define CBPRINTF_STATIC_PACKAGE(packaged, inlen, outlen, align_offset, \
+				... /* fmt, ... */) \
+	Z_CBPRINTF_STATIC_PACKAGE(packaged, inlen, outlen, \
+				  align_offset, __VA_ARGS__)
 
 /** @brief Capture state required to output formatted data later.
  *
@@ -126,11 +137,16 @@ typedef int (*cbprintf_cb)(/* int c, void *ctx */);
  * @param packaged pointer to where the packaged data can be stored.  Pass a
  * null pointer to store nothing but still calculate the total space required.
  * The data stored here is relocatable, that is it can be moved to another
- * contiguous block of memory. The pointer must be aligned to a multiple of
- * the largest element in the argument list.
+ * contiguous block of memory. However, under condition that alignment is
+ * maintained. It must be aligned to at least the size of a pointer.
  *
- * @param len this must be set to the number of bytes available at @p packaged.
- * Ignored if @p packaged is NULL.
+ * @param len this must be set to the number of bytes available at @p packaged
+ * if it is not null. If @p packaged is null then it indicates hypothetical
+ * buffer alignment offset in bytes compared to CBPRINTF_PACKAGE_ALIGNMENT
+ * alignment. Buffer alignment offset impacts returned size of the package.
+ * Xtensa requires that buffer is always aligned to CBPRINTF_PACKAGE_ALIGNMENT
+ * so it must be multiply of CBPRINTF_PACKAGE_ALIGNMENT or 0 when @p packaged is
+ * null.
  *
  * @param format a standard ISO C format string with characters and conversion
  * specifications.

include/sys/cbprintf_internal.h

@@ -193,23 +193,27 @@ static inline void cbprintf_wcpy(int *dst, int *src, uint32_t len)
  *
  * @param _idx index. Index is postincremented.
  *
+ * @param _align_offset Current index with alignment offset.
+ *
  * @param _max maximum index (buffer capacity).
  *
  * @param _arg argument.
  */
-#define Z_CBPRINTF_PACK_ARG2(_buf, _idx, _max, _arg) do { \
+#define Z_CBPRINTF_PACK_ARG2(_buf, _idx, _align_offset, _max, _arg) do { \
 	BUILD_ASSERT(!((sizeof(double) < VA_STACK_ALIGN(long double)) && \
 			Z_CBPRINTF_IS_LONGDOUBLE(_arg) && \
 			!IS_ENABLED(CONFIG_CBPRINTF_PACKAGE_LONGDOUBLE)),\
 			"Packaging of long double not enabled in Kconfig."); \
-	while (_idx % Z_CBPRINTF_ALIGNMENT(_arg)) { \
+	while (_align_offset % Z_CBPRINTF_ALIGNMENT(_arg)) { \
 		_idx += sizeof(int); \
+		_align_offset += sizeof(int); \
 	} \
 	uint32_t _arg_size = Z_CBPRINTF_ARG_SIZE(_arg); \
 	if (_buf && _idx < _max) { \
 		Z_CBPRINTF_STORE_ARG(&_buf[_idx], _arg); \
 	} \
 	_idx += _arg_size; \
+	_align_offset += _arg_size; \
 } while (0)
 
 /** @brief Package single argument.
@@ -219,7 +223,7 @@ static inline void cbprintf_wcpy(int *dst, int *src, uint32_t len)
  * @param arg argument.
  */
 #define Z_CBPRINTF_PACK_ARG(arg) \
-	Z_CBPRINTF_PACK_ARG2(_pbuf, _pkg_len, _pmax, arg)
+	Z_CBPRINTF_PACK_ARG2(_pbuf, _pkg_len, _pkg_offset, _pmax, arg)
 
 /** @brief Package descriptor.
  *
@@ -246,35 +250,41 @@ union z_cbprintf_hdr {
  *
  * @param _outlen number of bytes required to store the package.
  *
+ * @param _align_offset Input buffer alignment offset in words. Where offset 0
+ * means that buffer is aligned to CBPRINTF_PACKAGE_ALIGNMENT.
+ *
  * @param ... String with variable list of arguments.
  */
-#define Z_CBPRINTF_STATIC_PACKAGE_GENERIC(buf, _inlen, _outlen, \
+#define Z_CBPRINTF_STATIC_PACKAGE_GENERIC(buf, _inlen, _outlen, _align_offset, \
 					  ... /* fmt, ... */) \
 do { \
 	_Pragma("GCC diagnostic push") \
 	_Pragma("GCC diagnostic ignored \"-Wpointer-arith\"") \
+	BUILD_ASSERT(!IS_ENABLED(CONFIG_XTENSA) || \
+		     (IS_ENABLED(CONFIG_XTENSA) && \
+		      !(_align_offset % CBPRINTF_PACKAGE_ALIGNMENT)), \
+			"Xtensa requires aligned package."); \
+	BUILD_ASSERT((_align_offset % sizeof(int)) == 0, \
+			"Alignment offset must be multiply of a word."); \
 	if (IS_ENABLED(CONFIG_CBPRINTF_STATIC_PACKAGE_CHECK_ALIGNMENT)) { \
 		__ASSERT(!((uintptr_t)buf & (CBPRINTF_PACKAGE_ALIGNMENT - 1)), \
 			"Buffer must be aligned."); \
 	} \
 	uint8_t *_pbuf = buf; \
-	size_t _pmax = (buf != NULL) ? _inlen : SIZE_MAX; \
-	size_t _pkg_len = 0; \
+	int _pmax = (buf != NULL) ? _inlen : INT32_MAX; \
+	int _pkg_len = 0; \
+	int _pkg_offset = _align_offset; \
 	union z_cbprintf_hdr *_len_loc; \
 	/* package starts with string address and field with length */ \
-	if (_pmax < sizeof(char *) + 2 * sizeof(uint16_t)) { \
+	if (_pmax < sizeof(union z_cbprintf_hdr)) { \
+		_outlen = -ENOSPC; \
 		break; \
 	} \
 	_len_loc = (union z_cbprintf_hdr *)_pbuf; \
 	_pkg_len += sizeof(union z_cbprintf_hdr); \
-	if (_pbuf) { \
-		*(char **)&_pbuf[_pkg_len] = GET_ARG_N(1, __VA_ARGS__); \
-	} \
-	_pkg_len += sizeof(char *); \
+	_pkg_offset += sizeof(union z_cbprintf_hdr); \
 	/* Pack remaining arguments */\
-	COND_CODE_0(NUM_VA_ARGS_LESS_1(__VA_ARGS__), (), ( \
-	    FOR_EACH(Z_CBPRINTF_PACK_ARG, (;), GET_ARGS_LESS_N(1, __VA_ARGS__));\
-	)) \
+	FOR_EACH(Z_CBPRINTF_PACK_ARG, (;), __VA_ARGS__);\
 	/* Store length */ \
 	_outlen = (_pkg_len > _pmax) ? -ENOSPC : _pkg_len; \
 	/* Store length in the header, set number of dumped strings to 0 */ \
@@ -286,16 +296,19 @@ do { \
 } while (0)
 
 #if Z_C_GENERIC
-#define Z_CBPRINTF_STATIC_PACKAGE(packaged, inlen, outlen, ... /* fmt, ... */) \
-	Z_CBPRINTF_STATIC_PACKAGE_GENERIC(packaged, inlen, outlen, __VA_ARGS__)
+#define Z_CBPRINTF_STATIC_PACKAGE(packaged, inlen, outlen, align_offset, \
+				  ... /* fmt, ... */) \
+	Z_CBPRINTF_STATIC_PACKAGE_GENERIC(packaged, inlen, outlen, \
+					  align_offset, __VA_ARGS__)
 #else
-#define Z_CBPRINTF_STATIC_PACKAGE(packaged, inlen, outlen, ... /* fmt, ... */) \
+#define Z_CBPRINTF_STATIC_PACKAGE(packaged, inlen, outlen, align_offset, \
+				  ... /* fmt, ... */) \
 do { \
 	/* Small trick needed to avoid warning on always true */ \
 	if (((uintptr_t)packaged + 1) != 1) { \
 		outlen = cbprintf_package(packaged, inlen, __VA_ARGS__); \
 	} else { \
-		outlen = cbprintf_package(NULL, 0, __VA_ARGS__); \
+		outlen = cbprintf_package(NULL, align_offset, __VA_ARGS__); \
 	} \
 } while (0)
 #endif /* Z_C_GENERIC */

lib/os/cbprintf_packaged.c

@@ -205,6 +205,18 @@ int cbvprintf_package(void *packaged, size_t len,
 	const char *s;
 	bool parsing = false;
 
+	/* Buffer must be aligned at least to size of a pointer. */
+	if ((uintptr_t)packaged & (sizeof(void *) - 1)) {
+		return -EFAULT;
+	}
+
+#if defined(__xtensa__)
+	/* Xtensa requires package to be 16 bytes aligned. */
+	if ((uintptr_t)packaged & (CBPRINTF_PACKAGE_ALIGNMENT - 1)) {
+		return -EFAULT;
+	}
+#endif
+
 	/*
 	 * Make room to store the arg list size and the number of
 	 * appended strings. They both occupy 1 byte each.
@@ -218,9 +230,17 @@ int cbvprintf_package(void *packaged, size_t len,
 	/*
 	 * When buf0 is NULL we don't store anything.
 	 * Instead we count the needed space to store the data.
+	 * In this case, incoming len argument indicates the anticipated
+	 * buffer "misalignment" offset.
 	 */
 	if (!buf0) {
-		len = 0;
+#if defined(__xtensa__)
+		if (len % CBPRINTF_PACKAGE_ALIGNMENT) {
+			return -EFAULT;
+		}
+#endif
+		buf += len % CBPRINTF_PACKAGE_ALIGNMENT;
+		len = -(len % CBPRINTF_PACKAGE_ALIGNMENT);
 	}
 
 	/*
@@ -374,11 +394,6 @@ int cbvprintf_package(void *packaged, size_t len,
 		/* align destination buffer location */
 		buf = (void *) ROUND_UP(buf, align);
 
-		/* Check if buffer is properly aligned. */
-		if ((uintptr_t)buf0 & (align - 1)) {
-			return -EFAULT;
-		}
-
 		/* make sure the data fits */
 		if (buf0 && buf - buf0 + size > len) {
 			return -ENOSPC;