LCOV - code coverage report
Current view: top level - lib/os - cbprintf_packaged.c (source / functions) Hit Total Coverage
Test: coverage.info Lines: 0 314 0.0 %
Date: 2022-08-18 11:36:24 Functions: 0 9 0.0 %
Legend: Lines: hit not hit | Branches: + taken - not taken # not executed Branches: 0 194 0.0 %

           Branch data     Line data    Source code
       1                 :            : /*
       2                 :            :  * Copyright (c) 2021 BayLibre, SAS
       3                 :            :  *
       4                 :            :  * SPDX-License-Identifier: Apache-2.0
       5                 :            :  */
       6                 :            : 
       7                 :            : #include <errno.h>
       8                 :            : #include <stdarg.h>
       9                 :            : #include <stdint.h>
      10                 :            : #include <string.h>
      11                 :            : #include <linker/utils.h>
      12                 :            : #include <sys/cbprintf.h>
      13                 :            : #include <sys/types.h>
      14                 :            : #include <sys/util.h>
      15                 :            : #include <sys/__assert.h>
      16                 :            : 
      17                 :            : 
      18                 :            : /**
      19                 :            :  * @brief Check if address is in read only section.
      20                 :            :  *
      21                 :            :  * @param addr Address.
      22                 :            :  *
      23                 :            :  * @return True if address identified within read only section.
      24                 :            :  */
      25                 :          0 : static inline bool ptr_in_rodata(const char *addr)
      26                 :            : {
      27                 :            : #if defined(CBPRINTF_VIA_UNIT_TEST)
      28                 :            :         /* Unit test is X86 (or other host) but not using Zephyr
      29                 :            :          * linker scripts.
      30                 :            :          */
      31                 :            :         return false;
      32                 :            : #else
      33                 :          0 :         return linker_is_in_rodata(addr);
      34                 :            : #endif
      35                 :            : }
      36                 :            : 
      37                 :            : /*
      38                 :            :  * va_list creation
      39                 :            :  */
      40                 :            : 
      41                 :            : #if defined(__aarch64__)
      42                 :            : /*
      43                 :            :  * Reference:
      44                 :            :  *
      45                 :            :  * Procedure Call Standard for the ARM 64-bit Architecture
      46                 :            :  */
      47                 :            : 
      48                 :            : struct __va_list {
      49                 :            :         void    *__stack;
      50                 :            :         void    *__gr_top;
      51                 :            :         void    *__vr_top;
      52                 :            :         int     __gr_offs;
      53                 :            :         int     __vr_offs;
      54                 :            : };
      55                 :            : 
      56                 :            : BUILD_ASSERT(sizeof(va_list) == sizeof(struct __va_list),
      57                 :            :              "architecture specific support is wrong");
      58                 :            : 
      59                 :            : static int cbprintf_via_va_list(cbprintf_cb out,
      60                 :            :                                 cbvprintf_exteral_formatter_func formatter,
      61                 :            :                                 void *ctx,
      62                 :            :                                 const char *fmt, void *buf)
      63                 :            : {
      64                 :            :         union {
      65                 :            :                 va_list ap;
      66                 :            :                 struct __va_list __ap;
      67                 :            :         } u;
      68                 :            : 
      69                 :            :         /* create a valid va_list with our buffer */
      70                 :            :         u.__ap.__stack = buf;
      71                 :            :         u.__ap.__gr_top = NULL;
      72                 :            :         u.__ap.__vr_top = NULL;
      73                 :            :         u.__ap.__gr_offs = 0;
      74                 :            :         u.__ap.__vr_offs = 0;
      75                 :            : 
      76                 :            :         return formatter(out, ctx, fmt, u.ap);
      77                 :            : }
      78                 :            : 
      79                 :            : #elif defined(__x86_64__)
      80                 :            : /*
      81                 :            :  * Reference:
      82                 :            :  *
      83                 :            :  * System V Application Binary Interface
      84                 :            :  * AMD64 Architecture Processor Supplement
      85                 :            :  */
      86                 :            : 
      87                 :            : struct __va_list {
      88                 :            :         unsigned int gp_offset;
      89                 :            :         unsigned int fp_offset;
      90                 :            :         void *overflow_arg_area;
      91                 :            :         void *reg_save_area;
      92                 :            : };
      93                 :            : 
      94                 :            : BUILD_ASSERT(sizeof(va_list) == sizeof(struct __va_list),
      95                 :            :              "architecture specific support is wrong");
      96                 :            : 
      97                 :            : static int cbprintf_via_va_list(cbprintf_cb out,
      98                 :            :                                 cbvprintf_exteral_formatter_func formatter,
      99                 :            :                                 void *ctx,
     100                 :            :                                 const char *fmt, void *buf)
     101                 :            : {
     102                 :            :         union {
     103                 :            :                 va_list ap;
     104                 :            :                 struct __va_list __ap;
     105                 :            :         } u;
     106                 :            : 
     107                 :            :         /* create a valid va_list with our buffer */
     108                 :            :         u.__ap.overflow_arg_area = buf;
     109                 :            :         u.__ap.reg_save_area = NULL;
     110                 :            :         u.__ap.gp_offset = (6 * 8);
     111                 :            :         u.__ap.fp_offset = (6 * 8 + 16 * 16);
     112                 :            : 
     113                 :            :         return formatter(out, ctx, fmt, u.ap);
     114                 :            : }
     115                 :            : 
     116                 :            : #elif defined(__xtensa__)
     117                 :            : /*
     118                 :            :  * Reference:
     119                 :            :  *
     120                 :            :  * gcc source code (gcc/config/xtensa/xtensa.c)
     121                 :            :  * xtensa_build_builtin_va_list(), xtensa_va_start(),
     122                 :            :  * xtensa_gimplify_va_arg_expr()
     123                 :            :  */
     124                 :            : 
     125                 :            : struct __va_list {
     126                 :            :         void *__va_stk;
     127                 :            :         void *__va_reg;
     128                 :            :         int __va_ndx;
     129                 :            : };
     130                 :            : 
     131                 :            : BUILD_ASSERT(sizeof(va_list) == sizeof(struct __va_list),
     132                 :            :              "architecture specific support is wrong");
     133                 :            : 
     134                 :            : static int cbprintf_via_va_list(cbprintf_cb out,
     135                 :            :                                 cbvprintf_exteral_formatter_func formatter,
     136                 :            :                                 void *ctx,
     137                 :            :                                 const char *fmt, void *buf)
     138                 :            : {
     139                 :            :         union {
     140                 :            :                 va_list ap;
     141                 :            :                 struct __va_list __ap;
     142                 :            :         } u;
     143                 :            : 
     144                 :            :         /* create a valid va_list with our buffer */
     145                 :            :         u.__ap.__va_stk = (char *)buf - 32;
     146                 :            :         u.__ap.__va_reg = NULL;
     147                 :            :         u.__ap.__va_ndx = (6 + 2) * 4;
     148                 :            : 
     149                 :            :         return formatter(out, ctx, fmt, u.ap);
     150                 :            : }
     151                 :            : 
     152                 :            : #else
     153                 :            : /*
     154                 :            :  * Default implementation shared by many architectures like
     155                 :            :  * 32-bit ARM and Intel.
     156                 :            :  *
     157                 :            :  * We assume va_list is a simple pointer.
     158                 :            :  */
     159                 :            : 
     160                 :            : BUILD_ASSERT(sizeof(va_list) == sizeof(void *),
     161                 :            :              "architecture specific support is needed");
     162                 :            : 
     163                 :          0 : static int cbprintf_via_va_list(cbprintf_cb out,
     164                 :            :                                 cbvprintf_exteral_formatter_func formatter,
     165                 :            :                                 void *ctx,
     166                 :            :                                 const char *fmt, void *buf)
     167                 :            : {
     168                 :            :         union {
     169                 :            :                 va_list ap;
     170                 :            :                 void *ptr;
     171                 :            :         } u;
     172                 :            : 
     173                 :          0 :         u.ptr = buf;
     174                 :            : 
     175                 :          0 :         return formatter(out, ctx, fmt, u.ap);
     176                 :            : }
     177                 :            : 
     178                 :            : #endif
     179                 :            : 
     180                 :          0 : static int z_strncpy(char *dst, const char *src, size_t num)
     181                 :            : {
     182         [ #  # ]:          0 :         for (size_t i = 0; i < num; i++) {
     183                 :          0 :                 dst[i] = src[i];
     184         [ #  # ]:          0 :                 if (src[i] == '\0') {
     185                 :          0 :                         return i + 1;
     186                 :            :                 }
     187                 :            :         }
     188                 :            : 
     189                 :          0 :         return -ENOSPC;
     190                 :            : }
     191                 :            : 
     192                 :          0 : static size_t get_package_len(void *packaged)
     193                 :            : {
     194         [ #  # ]:          0 :         __ASSERT_NO_MSG(packaged != NULL);
     195                 :            : 
     196                 :          0 :         uint8_t *buf = packaged;
     197                 :          0 :         uint8_t *start = buf;
     198                 :            :         unsigned int args_size, s_nbr, ros_nbr;
     199                 :            : 
     200                 :          0 :         args_size = buf[0] * sizeof(int);
     201                 :          0 :         s_nbr     = buf[1];
     202                 :          0 :         ros_nbr   = buf[2];
     203                 :            : 
     204                 :            :         /* Move beyond args. */
     205                 :          0 :         buf += args_size;
     206                 :            : 
     207                 :            :         /* Move beyond read-only string indexes array. */
     208                 :          0 :         buf += ros_nbr;
     209                 :            : 
     210                 :            :         /* Move beyond strings appended to the package. */
     211         [ #  # ]:          0 :         for (int i = 0; i < s_nbr; i++) {
     212                 :          0 :                 buf++;
     213                 :          0 :                 buf += strlen((const char *)buf) + 1;
     214                 :            :         }
     215                 :            : 
     216                 :          0 :         return (size_t)(uintptr_t)(buf - start);
     217                 :            : }
     218                 :            : 
     219                 :          0 : static int append_string(void *dst, size_t max, const char *str, uint16_t strl)
     220                 :            : {
     221                 :          0 :         char *buf = dst;
     222                 :            : 
     223         [ #  # ]:          0 :         if (dst == NULL) {
     224                 :          0 :                 return 1 + strlen(str);
     225                 :            :         }
     226                 :            : 
     227         [ #  # ]:          0 :         if (strl) {
     228                 :          0 :                 memcpy(dst, str, strl);
     229                 :            : 
     230                 :          0 :                 return strl;
     231                 :            :         }
     232                 :            : 
     233                 :          0 :         return z_strncpy(buf, str, max);
     234                 :            : }
     235                 :            : 
     236                 :          0 : int cbvprintf_package(void *packaged, size_t len, uint32_t flags,
     237                 :            :                       const char *fmt, va_list ap)
     238                 :            : {
     239                 :            : /*
     240                 :            :  * Internally, a byte is used to store location of a string argument within a
     241                 :            :  * package. MSB bit is set if string is read-only so effectively 7 bits are
     242                 :            :  * used for index, which should be enough.
     243                 :            :  */
     244                 :            : #define STR_POS_RO_FLAG BIT(7)
     245                 :            : #define STR_POS_MASK BIT_MASK(7)
     246                 :            : 
     247                 :            : /* Buffer offset abstraction for better code clarity. */
     248                 :            : #define BUF_OFFSET ((uintptr_t)buf - (uintptr_t)buf0)
     249                 :            : 
     250                 :          0 :         uint8_t *buf0 = packaged;  /* buffer start (may be NULL) */
     251                 :          0 :         uint8_t *buf = buf0;       /* current buffer position */
     252                 :            :         unsigned int size;         /* current argument's size */
     253                 :            :         unsigned int align;        /* current argument's required alignment */
     254                 :            :         uint8_t str_ptr_pos[16];   /* string pointer positions */
     255                 :          0 :         unsigned int s_idx = 0;    /* index into str_ptr_pos[] */
     256                 :          0 :         unsigned int s_rw_cnt = 0; /* number of rw strings */
     257                 :          0 :         unsigned int s_ro_cnt = 0; /* number of ro strings */
     258                 :            :         unsigned int i;
     259                 :            :         const char *s;
     260                 :          0 :         bool parsing = false;
     261                 :            :         /* Flag indicates that rw strings are stored as array with positions,
     262                 :            :          * instead of appending them to the package.
     263                 :            :          */
     264                 :          0 :         bool rws_pos_en = !!(flags & CBPRINTF_PACKAGE_ADD_RW_STR_POS);
     265                 :            :         /* Get number of first read only strings present in the string.
     266                 :            :          * There is always at least 1 (fmt) but flags can indicate more, e.g
     267                 :            :          * fixed prefix appended to all strings.
     268                 :            :          */
     269                 :          0 :         int fros_cnt = 1 + Z_CBPRINTF_PACKAGE_FIRST_RO_STR_CNT_GET(flags);
     270                 :            : 
     271                 :            :         /* Buffer must be aligned at least to size of a pointer. */
     272         [ #  # ]:          0 :         if ((uintptr_t)packaged % sizeof(void *)) {
     273                 :          0 :                 return -EFAULT;
     274                 :            :         }
     275                 :            : 
     276                 :            : #if defined(__xtensa__)
     277                 :            :         /* Xtensa requires package to be 16 bytes aligned. */
     278                 :            :         if ((uintptr_t)packaged % CBPRINTF_PACKAGE_ALIGNMENT) {
     279                 :            :                 return -EFAULT;
     280                 :            :         }
     281                 :            : #endif
     282                 :            : 
     283                 :            :         /*
     284                 :            :          * Make room to store the arg list size and the number of
     285                 :            :          * appended strings. They both occupy 1 byte each.
     286                 :            :          *
     287                 :            :          * Given the next value to store is the format string pointer
     288                 :            :          * which is guaranteed to be at least 4 bytes, we just reserve
     289                 :            :          * a pointer size for the above to preserve alignment.
     290                 :            :          */
     291                 :          0 :         buf += sizeof(char *);
     292                 :            : 
     293                 :            :         /*
     294                 :            :          * When buf0 is NULL we don't store anything.
     295                 :            :          * Instead we count the needed space to store the data.
     296                 :            :          * In this case, incoming len argument indicates the anticipated
     297                 :            :          * buffer "misalignment" offset.
     298                 :            :          */
     299         [ #  # ]:          0 :         if (buf0 == NULL) {
     300                 :          0 :                 buf += len % CBPRINTF_PACKAGE_ALIGNMENT;
     301                 :            :                 /*
     302                 :            :                  * The space to store the data is represented by both the
     303                 :            :                  * buffer offset as well as the extra string data to be
     304                 :            :                  * appended. When only figuring out the needed space, we
     305                 :            :                  * don't append anything. Instead, we reuse the len variable
     306                 :            :                  * to sum the size of that data.
     307                 :            :                  *
     308                 :            :                  * Also, we subtract any initial misalignment offset from
     309                 :            :                  * the total as this won't be part of the buffer. To avoid
     310                 :            :                  * going negative with an unsigned variable, we add an offset
     311                 :            :                  * (CBPRINTF_PACKAGE_ALIGNMENT) that will be removed before
     312                 :            :                  * returning.
     313                 :            :                  */
     314                 :          0 :                 len = CBPRINTF_PACKAGE_ALIGNMENT - (len % CBPRINTF_PACKAGE_ALIGNMENT);
     315                 :            :         }
     316                 :            : 
     317                 :            :         /*
     318                 :            :          * Otherwise we must ensure we can store at least
     319                 :            :          * the pointer to the format string itself.
     320                 :            :          */
     321   [ #  #  #  # ]:          0 :         if (buf0 != NULL && BUF_OFFSET + sizeof(char *) > len) {
     322                 :          0 :                 return -ENOSPC;
     323                 :            :         }
     324                 :            : 
     325                 :            :         /*
     326                 :            :          * Then process the format string itself.
     327                 :            :          * Here we branch directly into the code processing strings
     328                 :            :          * which is in the middle of the following while() loop. That's the
     329                 :            :          * reason for the post-decrement on fmt as it will be incremented
     330                 :            :          * prior to the next (actually first) round of that loop.
     331                 :            :          */
     332                 :          0 :         s = fmt--;
     333                 :          0 :         align = VA_STACK_ALIGN(char *);
     334                 :          0 :         size = sizeof(char *);
     335                 :          0 :         goto process_string;
     336                 :            : 
     337                 :            :         /* Scan the format string */
     338         [ #  # ]:          0 :         while (*++fmt != '\0') {
     339         [ #  # ]:          0 :                 if (!parsing) {
     340         [ #  # ]:          0 :                         if (*fmt == '%') {
     341                 :          0 :                                 parsing = true;
     342                 :          0 :                                 align = VA_STACK_ALIGN(int);
     343                 :          0 :                                 size = sizeof(int);
     344                 :            :                         }
     345                 :          0 :                         continue;
     346                 :            :                 }
     347   [ #  #  #  #  :          0 :                 switch (*fmt) {
          #  #  #  #  #  
                      # ]
     348                 :          0 :                 case '%':
     349                 :          0 :                         parsing = false;
     350                 :          0 :                         continue;
     351                 :            : 
     352                 :          0 :                 case '#':
     353                 :            :                 case '-':
     354                 :            :                 case '+':
     355                 :            :                 case ' ':
     356                 :            :                 case '0':
     357                 :            :                 case '1':
     358                 :            :                 case '2':
     359                 :            :                 case '3':
     360                 :            :                 case '4':
     361                 :            :                 case '5':
     362                 :            :                 case '6':
     363                 :            :                 case '7':
     364                 :            :                 case '8':
     365                 :            :                 case '9':
     366                 :            :                 case '.':
     367                 :            :                 case 'h':
     368                 :            :                 case 'l':
     369                 :            :                 case 'L':
     370                 :          0 :                         continue;
     371                 :            : 
     372                 :          0 :                 case '*':
     373                 :          0 :                         break;
     374                 :            : 
     375                 :          0 :                 case 'j':
     376                 :          0 :                         align = VA_STACK_ALIGN(intmax_t);
     377                 :          0 :                         size = sizeof(intmax_t);
     378                 :          0 :                         continue;
     379                 :            : 
     380                 :          0 :                 case 'z':
     381                 :          0 :                         align = VA_STACK_ALIGN(size_t);
     382                 :          0 :                         size = sizeof(size_t);
     383                 :          0 :                         continue;
     384                 :            : 
     385                 :          0 :                 case 't':
     386                 :          0 :                         align = VA_STACK_ALIGN(ptrdiff_t);
     387                 :          0 :                         size = sizeof(ptrdiff_t);
     388                 :          0 :                         continue;
     389                 :            : 
     390                 :          0 :                 case 'c':
     391                 :            :                 case 'd':
     392                 :            :                 case 'i':
     393                 :            :                 case 'o':
     394                 :            :                 case 'u':
     395                 :            :                 case 'x':
     396                 :            :                 case 'X':
     397         [ #  # ]:          0 :                         if (fmt[-1] == 'l') {
     398         [ #  # ]:          0 :                                 if (fmt[-2] == 'l') {
     399                 :          0 :                                         align = VA_STACK_ALIGN(long long);
     400                 :          0 :                                         size = sizeof(long long);
     401                 :            :                                 } else {
     402                 :          0 :                                         align = VA_STACK_ALIGN(long);
     403                 :          0 :                                         size = sizeof(long);
     404                 :            :                                 }
     405                 :            :                         }
     406                 :          0 :                         parsing = false;
     407                 :          0 :                         break;
     408                 :            : 
     409                 :          0 :                 case 's':
     410                 :            :                 case 'p':
     411                 :            :                 case 'n':
     412                 :          0 :                         align = VA_STACK_ALIGN(void *);
     413                 :          0 :                         size = sizeof(void *);
     414                 :          0 :                         parsing = false;
     415                 :          0 :                         break;
     416                 :            : 
     417                 :          0 :                 case 'a':
     418                 :            :                 case 'A':
     419                 :            :                 case 'e':
     420                 :            :                 case 'E':
     421                 :            :                 case 'f':
     422                 :            :                 case 'F':
     423                 :            :                 case 'g':
     424                 :            :                 case 'G': {
     425                 :            :                         /*
     426                 :            :                          * Handle floats separately as they may be
     427                 :            :                          * held in a different register set.
     428                 :            :                          */
     429                 :            :                         union { double d; long double ld; } v;
     430                 :            : 
     431         [ #  # ]:          0 :                         if (fmt[-1] == 'L') {
     432                 :          0 :                                 v.ld = va_arg(ap, long double);
     433                 :          0 :                                 align = VA_STACK_ALIGN(long double);
     434                 :          0 :                                 size = sizeof(long double);
     435                 :            :                         } else {
     436                 :          0 :                                 v.d = va_arg(ap, double);
     437                 :          0 :                                 align = VA_STACK_ALIGN(double);
     438                 :          0 :                                 size = sizeof(double);
     439                 :            :                         }
     440                 :            :                         /* align destination buffer location */
     441                 :          0 :                         buf = (void *) ROUND_UP(buf, align);
     442         [ #  # ]:          0 :                         if (buf0 != NULL) {
     443                 :            :                                 /* make sure it fits */
     444         [ #  # ]:          0 :                                 if (BUF_OFFSET + size > len) {
     445                 :          0 :                                         return -ENOSPC;
     446                 :            :                                 }
     447                 :            :                                 if (Z_CBPRINTF_VA_STACK_LL_DBL_MEMCPY) {
     448                 :            :                                         memcpy(buf, &v, size);
     449         [ #  # ]:          0 :                                 } else if (fmt[-1] == 'L') {
     450                 :          0 :                                         *(long double *)buf = v.ld;
     451                 :            :                                 } else {
     452                 :          0 :                                         *(double *)buf = v.d;
     453                 :            :                                 }
     454                 :            :                         }
     455                 :          0 :                         buf += size;
     456                 :          0 :                         parsing = false;
     457                 :          0 :                         continue;
     458                 :            :                 }
     459                 :            : 
     460                 :          0 :                 default:
     461                 :          0 :                         parsing = false;
     462                 :          0 :                         continue;
     463                 :            :                 }
     464                 :            : 
     465                 :            :                 /* align destination buffer location */
     466                 :          0 :                 buf = (void *) ROUND_UP(buf, align);
     467                 :            : 
     468                 :            :                 /* make sure the data fits */
     469   [ #  #  #  # ]:          0 :                 if (buf0 != NULL && BUF_OFFSET + size > len) {
     470                 :          0 :                         return -ENOSPC;
     471                 :            :                 }
     472                 :            : 
     473                 :            :                 /* copy va_list data over to our buffer */
     474         [ #  # ]:          0 :                 if (*fmt == 's') {
     475                 :          0 :                         s = va_arg(ap, char *);
     476                 :          0 : process_string:
     477         [ #  # ]:          0 :                         if (buf0 != NULL) {
     478                 :          0 :                                 *(const char **)buf = s;
     479                 :            :                         }
     480                 :            : 
     481   [ #  #  #  # ]:          0 :                         bool is_ro = (fros_cnt-- > 0) ? true : ptr_in_rodata(s);
     482                 :          0 :                         bool do_ro = !!(flags & CBPRINTF_PACKAGE_ADD_RO_STR_POS);
     483                 :            : 
     484   [ #  #  #  # ]:          0 :                         if (is_ro && !do_ro) {
     485                 :            :                                 /* nothing to do */
     486                 :            :                         } else {
     487                 :          0 :                                 uint32_t s_ptr_idx = BUF_OFFSET / sizeof(int);
     488                 :            : 
     489                 :            :                                 /*
     490                 :            :                                  * In the do_ro case we must consider
     491                 :            :                                  * room for possible STR_POS_RO_FLAG.
     492                 :            :                                  * Otherwise the index range is 8 bits
     493                 :            :                                  * and any overflow is caught later.
     494                 :            :                                  */
     495   [ #  #  #  # ]:          0 :                                 if (do_ro && s_ptr_idx > STR_POS_MASK) {
     496                 :          0 :                                         __ASSERT(false, "String with too many arguments");
     497                 :          0 :                                         return -EINVAL;
     498                 :            :                                 }
     499                 :            : 
     500         [ #  # ]:          0 :                                 if (s_idx >= ARRAY_SIZE(str_ptr_pos)) {
     501                 :          0 :                                         __ASSERT(false, "str_ptr_pos[] too small");
     502                 :          0 :                                         return -EINVAL;
     503                 :            :                                 }
     504                 :            : 
     505         [ #  # ]:          0 :                                 if (buf0 != NULL) {
     506                 :            :                                         /*
     507                 :            :                                          * Remember string pointer location.
     508                 :            :                                          * We will append non-ro strings later.
     509                 :            :                                          */
     510                 :          0 :                                         str_ptr_pos[s_idx] = s_ptr_idx;
     511         [ #  # ]:          0 :                                         if (is_ro) {
     512                 :            :                                                 /* flag read-only string. */
     513                 :          0 :                                                 str_ptr_pos[s_idx] |= STR_POS_RO_FLAG;
     514                 :          0 :                                                 s_ro_cnt++;
     515                 :            :                                         } else {
     516                 :          0 :                                                 s_rw_cnt++;
     517                 :            :                                         }
     518   [ #  #  #  # ]:          0 :                                 } else if (is_ro || rws_pos_en) {
     519                 :            :                                         /*
     520                 :            :                                          * Add only pointer position prefix
     521                 :            :                                          * when counting strings.
     522                 :            :                                          */
     523                 :          0 :                                         len += 1;
     524                 :            :                                 } else {
     525                 :            :                                         /*
     526                 :            :                                          * Add the string length, the final '\0'
     527                 :            :                                          * and size of the pointer position prefix.
     528                 :            :                                          */
     529                 :          0 :                                         len += strlen(s) + 1 + 1;
     530                 :            :                                 }
     531                 :            : 
     532                 :          0 :                                 s_idx++;
     533                 :            :                         }
     534                 :          0 :                         buf += sizeof(char *);
     535         [ #  # ]:          0 :                 } else if (size == sizeof(int)) {
     536                 :          0 :                         int v = va_arg(ap, int);
     537                 :            : 
     538         [ #  # ]:          0 :                         if (buf0 != NULL) {
     539                 :          0 :                                 *(int *)buf = v;
     540                 :            :                         }
     541                 :          0 :                         buf += sizeof(int);
     542         [ #  # ]:          0 :                 } else if (size == sizeof(long)) {
     543                 :          0 :                         long v = va_arg(ap, long);
     544                 :            : 
     545         [ #  # ]:          0 :                         if (buf0 != NULL) {
     546                 :          0 :                                 *(long *)buf = v;
     547                 :            :                         }
     548                 :          0 :                         buf += sizeof(long);
     549         [ #  # ]:          0 :                 } else if (size == sizeof(long long)) {
     550                 :          0 :                         long long v = va_arg(ap, long long);
     551                 :            : 
     552         [ #  # ]:          0 :                         if (buf0 != NULL) {
     553                 :            :                                 if (Z_CBPRINTF_VA_STACK_LL_DBL_MEMCPY) {
     554                 :            :                                         memcpy(buf, &v, sizeof(long long));
     555                 :            :                                 } else {
     556                 :          0 :                                         *(long long *)buf = v;
     557                 :            :                                 }
     558                 :            :                         }
     559                 :          0 :                         buf += sizeof(long long);
     560                 :            :                 } else {
     561                 :          0 :                         __ASSERT(false, "unexpected size %u", size);
     562                 :          0 :                         return -EINVAL;
     563                 :            :                 }
     564                 :            :         }
     565                 :            : 
     566                 :            :         /*
     567                 :            :          * We remember the size of the argument list as a multiple of
     568                 :            :          * sizeof(int) and limit it to a 8-bit field. That means 1020 bytes
     569                 :            :          * worth of va_list, or about 127 arguments on a 64-bit system
     570                 :            :          * (twice that on 32-bit systems). That ought to be good enough.
     571                 :            :          */
     572         [ #  # ]:          0 :         if (BUF_OFFSET / sizeof(int) > 255) {
     573                 :          0 :                 __ASSERT(false, "too many format args");
     574                 :          0 :                 return -EINVAL;
     575                 :            :         }
     576                 :            : 
     577                 :            :         /*
     578                 :            :          * If all we wanted was to count required buffer size
     579                 :            :          * then we have it now.
     580                 :            :          */
     581         [ #  # ]:          0 :         if (buf0 == NULL) {
     582                 :          0 :                 return BUF_OFFSET + len - CBPRINTF_PACKAGE_ALIGNMENT;
     583                 :            :         }
     584                 :            : 
     585                 :            :         /* Clear our buffer header. We made room for it initially. */
     586                 :          0 :         *(char **)buf0 = NULL;
     587                 :            : 
     588                 :            :         /* Record end of argument list. */
     589                 :          0 :         buf0[0] = BUF_OFFSET / sizeof(int);
     590                 :            : 
     591         [ #  # ]:          0 :         if (rws_pos_en) {
     592                 :            :                 /* Strings are appended, update location counter. */
     593                 :          0 :                 buf0[1] = 0;
     594                 :          0 :                 buf0[3] = s_rw_cnt;
     595                 :            :         } else {
     596                 :            :                 /* Strings are appended, update append counter. */
     597                 :          0 :                 buf0[1] = s_rw_cnt;
     598                 :          0 :                 buf0[3] = 0;
     599                 :            :         }
     600                 :            : 
     601                 :          0 :         buf0[2] = s_ro_cnt;
     602                 :            : 
     603                 :            :         /* Store strings pointer locations of read only strings. */
     604         [ #  # ]:          0 :         if (s_ro_cnt) {
     605         [ #  # ]:          0 :                 for (i = 0; i < s_idx; i++) {
     606         [ #  # ]:          0 :                         if (!(str_ptr_pos[i] & STR_POS_RO_FLAG)) {
     607                 :          0 :                                 continue;
     608                 :            :                         }
     609                 :            : 
     610                 :          0 :                         uint8_t pos = str_ptr_pos[i] & STR_POS_MASK;
     611                 :            : 
     612                 :            :                         /* make sure it fits */
     613         [ #  # ]:          0 :                         if (BUF_OFFSET + 1 > len) {
     614                 :          0 :                                 return -ENOSPC;
     615                 :            :                         }
     616                 :            :                         /* store the pointer position prefix */
     617                 :          0 :                         *buf++ = pos;
     618                 :            :                 }
     619                 :            :         }
     620                 :            : 
     621                 :            :         /* Store strings prefixed by their pointer location. */
     622         [ #  # ]:          0 :         for (i = 0; i < s_idx; i++) {
     623                 :            :                 /* Process only RW strings. */
     624   [ #  #  #  # ]:          0 :                 if (s_ro_cnt && str_ptr_pos[i] & STR_POS_RO_FLAG) {
     625                 :          0 :                         continue;
     626                 :            :                 }
     627                 :            : 
     628         [ #  # ]:          0 :                 if (rws_pos_en) {
     629                 :          0 :                         size = 0;
     630                 :            :                 } else {
     631                 :            :                         /* retrieve the string pointer */
     632                 :          0 :                         s = *(char **)(buf0 + str_ptr_pos[i] * sizeof(int));
     633                 :            :                         /* clear the in-buffer pointer (less entropy if compressed) */
     634                 :          0 :                         *(char **)(buf0 + str_ptr_pos[i] * sizeof(int)) = NULL;
     635                 :            :                         /* find the string length including terminating '\0' */
     636                 :          0 :                         size = strlen(s) + 1;
     637                 :            :                 }
     638                 :            : 
     639                 :            :                 /* make sure it fits */
     640         [ #  # ]:          0 :                 if (BUF_OFFSET + 1 + size > len) {
     641                 :          0 :                         return -ENOSPC;
     642                 :            :                 }
     643                 :            :                 /* store the pointer position prefix */
     644                 :          0 :                 *buf++ = str_ptr_pos[i];
     645                 :            :                 /* copy the string with its terminating '\0' */
     646                 :          0 :                 memcpy(buf, s, size);
     647                 :          0 :                 buf += size;
     648                 :            :         }
     649                 :            : 
     650                 :            :         /*
     651                 :            :          * TODO: remove pointers for appended strings since they're useless.
     652                 :            :          * TODO: explore leveraging same mechanism to remove alignment padding
     653                 :            :          */
     654                 :            : 
     655                 :          0 :         return BUF_OFFSET;
     656                 :            : 
     657                 :            : #undef BUF_OFFSET
     658                 :            : #undef STR_POS_RO_FLAG
     659                 :            : #undef STR_POS_MASK
     660                 :            : }
     661                 :            : 
     662                 :          0 : int cbprintf_package(void *packaged, size_t len, uint32_t flags,
     663                 :            :                      const char *format, ...)
     664                 :            : {
     665                 :            :         va_list ap;
     666                 :            :         int ret;
     667                 :            : 
     668                 :          0 :         va_start(ap, format);
     669                 :          0 :         ret = cbvprintf_package(packaged, len, flags, format, ap);
     670                 :          0 :         va_end(ap);
     671                 :          0 :         return ret;
     672                 :            : }
     673                 :            : 
     674                 :          0 : int cbpprintf_external(cbprintf_cb out,
     675                 :            :                        cbvprintf_exteral_formatter_func formatter,
     676                 :            :                        void *ctx, void *packaged)
     677                 :            : {
     678                 :          0 :         uint8_t *buf = packaged;
     679                 :            :         char *fmt, *s, **ps;
     680                 :            :         unsigned int i, args_size, s_nbr, ros_nbr, rws_nbr, s_idx;
     681                 :            : 
     682         [ #  # ]:          0 :         if (buf == NULL) {
     683                 :          0 :                 return -EINVAL;
     684                 :            :         }
     685                 :            : 
     686                 :            :         /* Retrieve the size of the arg list and number of strings. */
     687                 :          0 :         args_size = buf[0] * sizeof(int);
     688                 :          0 :         s_nbr     = buf[1];
     689                 :          0 :         ros_nbr   = buf[2];
     690                 :          0 :         rws_nbr   = buf[3];
     691                 :            : 
     692                 :            :         /* Locate the string table */
     693                 :          0 :         s = (char *)(buf + args_size + ros_nbr + rws_nbr);
     694                 :            : 
     695                 :            :         /*
     696                 :            :          * Patch in string pointers.
     697                 :            :          */
     698         [ #  # ]:          0 :         for (i = 0; i < s_nbr; i++) {
     699                 :            :                 /* Locate pointer location for this string */
     700                 :          0 :                 s_idx = *(uint8_t *)s++;
     701                 :          0 :                 ps = (char **)(buf + s_idx * sizeof(int));
     702                 :            :                 /* update the pointer with current string location */
     703                 :          0 :                 *ps = s;
     704                 :            :                 /* move to next string */
     705                 :          0 :                 s += strlen(s) + 1;
     706                 :            :         }
     707                 :            : 
     708                 :            :         /* Retrieve format string */
     709                 :          0 :         fmt = ((char **)buf)[1];
     710                 :            : 
     711                 :            :         /* skip past format string pointer */
     712                 :          0 :         buf += sizeof(char *) * 2;
     713                 :            : 
     714                 :            :         /* Turn this into a va_list and  print it */
     715                 :          0 :         return cbprintf_via_va_list(out, formatter, ctx, fmt, buf);
     716                 :            : }
     717                 :            : 
     718                 :          0 : int cbprintf_package_copy(void *in_packaged,
     719                 :            :                           size_t in_len,
     720                 :            :                           void *packaged,
     721                 :            :                           size_t len,
     722                 :            :                           uint32_t flags,
     723                 :            :                           uint16_t *strl,
     724                 :            :                           size_t strl_len)
     725                 :            : {
     726         [ #  # ]:          0 :         __ASSERT_NO_MSG(in_packaged != NULL);
     727                 :            : 
     728                 :          0 :         uint8_t *buf = in_packaged;
     729                 :          0 :         uint32_t *buf32 = in_packaged;
     730                 :            :         unsigned int args_size, ros_nbr, rws_nbr;
     731                 :            :         bool rw_cpy;
     732                 :            :         bool ro_cpy;
     733                 :            : 
     734         [ #  # ]:          0 :         in_len != 0 ? in_len : get_package_len(in_packaged);
     735                 :            : 
     736                 :            :         /* Get number of RO string indexes in the package and check if copying
     737                 :            :          * includes appending those strings.
     738                 :            :          */
     739                 :          0 :         ros_nbr = buf[2];
     740         [ #  # ]:          0 :         ro_cpy = ros_nbr &&
     741         [ #  # ]:          0 :                 (flags & CBPRINTF_PACKAGE_COPY_RO_STR) == CBPRINTF_PACKAGE_COPY_RO_STR;
     742                 :            : 
     743                 :            :         /* Get number of RW string indexes in the package and check if copying
     744                 :            :          * includes appending those strings.
     745                 :            :          */
     746                 :          0 :         rws_nbr = buf[3];
     747         [ #  # ]:          0 :         rw_cpy = rws_nbr > 0 &&
     748         [ #  # ]:          0 :                  (flags & CBPRINTF_PACKAGE_COPY_RW_STR) == CBPRINTF_PACKAGE_COPY_RW_STR;
     749                 :            : 
     750                 :            : 
     751                 :            :         /* If flags are not set or appending request without rw string indexes
     752                 :            :          * present is chosen, just do a simple copy (or length calculation).
     753                 :            :          * Assuming that it is the most common case.
     754                 :            :          */
     755   [ #  #  #  # ]:          0 :         if (!rw_cpy && !ro_cpy) {
     756         [ #  # ]:          0 :                 if (packaged) {
     757                 :          0 :                         memcpy(packaged, in_packaged, in_len);
     758                 :            :                 }
     759                 :            : 
     760                 :          0 :                 return in_len;
     761                 :            :         }
     762                 :            : 
     763                 :            :         /* If we got here, it means that coping will be more complex and will be
     764                 :            :          * done with strings appending.
     765                 :            :          * Retrieve the size of the arg list.
     766                 :            :          */
     767                 :          0 :         args_size = buf[0] * sizeof(int);
     768                 :            : 
     769                 :          0 :         size_t out_len = in_len;
     770                 :            : 
     771                 :            :         /* Pointer to array with string locations. Array starts with read-only
     772                 :            :          * string locations.
     773                 :            :          */
     774                 :          0 :         uint8_t *str_pos = &buf[args_size];
     775                 :          0 :         size_t strl_cnt = 0;
     776                 :            : 
     777                 :            :         /* If null destination, just calculate output length. */
     778         [ #  # ]:          0 :         if (packaged == NULL) {
     779         [ #  # ]:          0 :                 if (ro_cpy) {
     780         [ #  # ]:          0 :                         for (int i = 0; i < ros_nbr; i++) {
     781                 :          0 :                                 const char *str = *(const char **)&buf32[*str_pos];
     782                 :          0 :                                 int len = append_string(NULL, 0, str, 0);
     783                 :            : 
     784                 :            :                                 /* If possible store calculated string length. */
     785   [ #  #  #  # ]:          0 :                                 if (strl && strl_cnt < strl_len) {
     786                 :          0 :                                         strl[strl_cnt++] = (uint16_t)len;
     787                 :            :                                 }
     788                 :          0 :                                 out_len += len;
     789                 :          0 :                                 str_pos++;
     790                 :            :                         }
     791                 :            :                 } else {
     792   [ #  #  #  # ]:          0 :                         if (ros_nbr && flags & CBPRINTF_PACKAGE_COPY_KEEP_RO_STR) {
     793                 :          0 :                                 str_pos += ros_nbr;
     794                 :            :                         }
     795                 :            :                 }
     796                 :            : 
     797                 :          0 :                 bool drop_ro_str_pos = !(flags &
     798                 :            :                                         (CBPRINTF_PACKAGE_COPY_KEEP_RO_STR |
     799                 :            :                                          CBPRINTF_PACKAGE_COPY_RO_STR));
     800                 :            : 
     801                 :            :                 /* Handle RW strings. */
     802         [ #  # ]:          0 :                 for (int i = 0; i < rws_nbr; i++) {
     803                 :          0 :                         const char *str = *(const char **)&buf32[*str_pos];
     804                 :          0 :                         bool is_ro = ptr_in_rodata(str);
     805                 :            : 
     806   [ #  #  #  # ]:          0 :                         if ((is_ro && flags & CBPRINTF_PACKAGE_COPY_RO_STR) ||
     807   [ #  #  #  # ]:          0 :                             (!is_ro && flags & CBPRINTF_PACKAGE_COPY_RW_STR)) {
     808                 :          0 :                                 int len = append_string(NULL, 0, str, 0);
     809                 :            : 
     810                 :            :                                 /* If possible store calculated string length. */
     811   [ #  #  #  # ]:          0 :                                 if (strl && strl_cnt < strl_len) {
     812                 :          0 :                                         strl[strl_cnt++] = (uint16_t)len;
     813                 :            :                                 }
     814                 :          0 :                                 out_len += len;
     815                 :            :                         }
     816                 :            : 
     817   [ #  #  #  # ]:          0 :                         if (is_ro && drop_ro_str_pos) {
     818                 :            :                                 /* If read-only string location is dropped decreased
     819                 :            :                                  * length.
     820                 :            :                                  */
     821                 :          0 :                                 out_len--;
     822                 :            :                         }
     823                 :            : 
     824                 :          0 :                         str_pos++;
     825                 :            :                 }
     826                 :            : 
     827                 :          0 :                 return out_len;
     828                 :            :         }
     829                 :            : 
     830                 :            :         uint8_t cpy_str_pos[16];
     831                 :            :         uint8_t scpy_cnt;
     832                 :          0 :         uint8_t *dst = packaged;
     833                 :          0 :         uint8_t *dst_hdr = packaged;
     834                 :            : 
     835                 :          0 :         memcpy(dst, in_packaged, args_size);
     836                 :          0 :         dst += args_size;
     837                 :            : 
     838                 :            :         /* Pointer to the beginning of string locations in the destination package. */
     839                 :          0 :         uint8_t *dst_str_loc = dst;
     840                 :            : 
     841                 :            :         /* If read-only strings shall be appended to the output package copy
     842                 :            :          * their indexes to the local array, otherwise indicate that indexes
     843                 :            :          * shall remain in the output package.
     844                 :            :          */
     845         [ #  # ]:          0 :         if (ro_cpy) {
     846                 :          0 :                 memcpy(cpy_str_pos, str_pos, ros_nbr);
     847                 :          0 :                 scpy_cnt = ros_nbr;
     848                 :            :                 /* Read only string indexes removed from package. */
     849                 :          0 :                 dst_hdr[2] = 0;
     850                 :          0 :                 str_pos += ros_nbr;
     851                 :            :         } else {
     852                 :          0 :                 scpy_cnt = 0;
     853   [ #  #  #  # ]:          0 :                 if (ros_nbr && flags & CBPRINTF_PACKAGE_COPY_KEEP_RO_STR) {
     854                 :          0 :                         memcpy(dst, str_pos, ros_nbr);
     855                 :          0 :                         dst += ros_nbr;
     856                 :          0 :                         str_pos += ros_nbr;
     857                 :            :                 } else {
     858                 :          0 :                         dst_hdr[2] = 0;
     859                 :            :                 }
     860                 :            :         }
     861                 :            : 
     862                 :            :         /* Go through read-write strings and identify which shall be appended.
     863                 :            :          * Note that there may be read-only strings there. Use address evaluation
     864                 :            :          * to determine if strings is read-only.
     865                 :            :          */
     866         [ #  # ]:          0 :         for (int i = 0; i < rws_nbr; i++) {
     867                 :          0 :                 const char *str = *(const char **)&buf32[*str_pos];
     868                 :          0 :                 bool is_ro = ptr_in_rodata(str);
     869                 :            : 
     870         [ #  # ]:          0 :                 if (is_ro) {
     871         [ #  # ]:          0 :                         if (flags & CBPRINTF_PACKAGE_COPY_RO_STR) {
     872                 :          0 :                                 cpy_str_pos[scpy_cnt++] = *str_pos;
     873         [ #  # ]:          0 :                         } else if (flags & CBPRINTF_PACKAGE_COPY_KEEP_RO_STR) {
     874                 :          0 :                                 *dst++ = *str_pos;
     875                 :            :                                 /* Increment amount of ro locations. */
     876                 :          0 :                                 dst_hdr[2]++;
     877                 :            :                         } else {
     878                 :            :                                 /* Drop information about ro_str location. */
     879                 :            :                         }
     880                 :            :                 } else {
     881         [ #  # ]:          0 :                         if (flags & CBPRINTF_PACKAGE_COPY_RW_STR) {
     882                 :          0 :                                 cpy_str_pos[scpy_cnt++] = *str_pos;
     883                 :            :                         } else {
     884                 :          0 :                                 *dst++ = *str_pos;
     885                 :            :                         }
     886                 :            :                 }
     887                 :          0 :                 str_pos++;
     888                 :            :         }
     889                 :            : 
     890                 :            :         /* Increment amount of strings appended to the package. */
     891                 :          0 :         dst_hdr[1] += scpy_cnt;
     892                 :            :         /* Update number of rw string locations in the package. */
     893                 :          0 :         dst_hdr[3] = (uint8_t)(uintptr_t)(dst - dst_str_loc) - dst_hdr[2];
     894                 :            : 
     895                 :            :         /* Copy appended strings from source package to destination. */
     896                 :          0 :         size_t strs_len = in_len - (args_size + ros_nbr + rws_nbr);
     897                 :            : 
     898                 :          0 :         memcpy(dst, str_pos, strs_len);
     899                 :            : 
     900                 :          0 :         dst += strs_len;
     901                 :            : 
     902         [ #  # ]:          0 :         if (scpy_cnt == 0) {
     903                 :          0 :                 return dst - dst_hdr;
     904                 :            :         }
     905                 :            : 
     906                 :            :         /* Calculate remaining space in the buffer. */
     907                 :          0 :         size_t rem = len - ((size_t)(uintptr_t)(dst - dst_hdr));
     908                 :            : 
     909         [ #  # ]:          0 :         if (rem <= scpy_cnt) {
     910                 :          0 :                 return -ENOSPC;
     911                 :            :         }
     912                 :            : 
     913                 :            :         /* Append strings */
     914         [ #  # ]:          0 :         for (int i = 0; i < scpy_cnt; i++) {
     915                 :          0 :                 uint8_t loc = cpy_str_pos[i];
     916                 :          0 :                 const char *str = *(const char **)&buf32[loc];
     917                 :            :                 int cpy_len;
     918         [ #  # ]:          0 :                 uint16_t str_len = strl ? strl[i] : 0;
     919                 :            : 
     920                 :          0 :                 *dst = loc;
     921                 :          0 :                 rem--;
     922                 :          0 :                 dst++;
     923                 :          0 :                 cpy_len = append_string(dst, rem, str, str_len);
     924                 :            : 
     925         [ #  # ]:          0 :                 if (cpy_len < 0) {
     926                 :          0 :                         return -ENOSPC;
     927                 :            :                 }
     928                 :            : 
     929                 :          0 :                 rem -= cpy_len;
     930                 :          0 :                 dst += cpy_len;
     931                 :            :         }
     932                 :            : 
     933                 :          0 :         return len - rem;
     934                 :            : }

Generated by: LCOV version 1.14