1 /*
2  * Copyright (c) 2020-2023, Arm Limited. All rights reserved.
3  *
4  * SPDX-License-Identifier: BSD-3-Clause
5  *
6  */
7 
8 #include <stdarg.h>
9 #include <stddef.h>
10 #include <stdint.h>
11 #include "tfm_hal_defs.h"
12 #include "tfm_hal_sp_logdev.h"
13 
14 #define PRINT_BUFF_SIZE 32
15 #define NUM_BUFF_SIZE 12
16 
17 struct formatted_buffer_t {
18     size_t pos;
19     uint8_t buf[PRINT_BUFF_SIZE];
20 };
21 
22 static const char hex_digits_lo[] = {'0', '1', '2', '3', '4', '5', '6', '7',
23                                      '8', '9', 'a', 'b', 'c', 'd', 'e', 'f'};
24 static const char hex_digits_up[] = {'0', '1', '2', '3', '4', '5', '6', '7',
25                                      '8', '9', 'A', 'B', 'C', 'D', 'E', 'F'};
26 
_tfm_flush_formatted_buffer(struct formatted_buffer_t * pb,uint8_t data)27 static void _tfm_flush_formatted_buffer(struct formatted_buffer_t *pb,
28                                         uint8_t data)
29 {
30     pb->buf[pb->pos++] = data;
31     if (pb->pos >= PRINT_BUFF_SIZE) {
32         pb->pos = 0;
33         /* uart flush and print here. */
34         tfm_hal_output_sp_log(pb->buf, PRINT_BUFF_SIZE);
35     }
36 }
37 
_tfm_string_output(struct formatted_buffer_t * pb,const char * str)38 static int _tfm_string_output(struct formatted_buffer_t *pb,
39                               const char *str)
40 {
41     int count = 0;
42 
43     while (*str) {
44         _tfm_flush_formatted_buffer(pb, *str++);
45         count++;
46     }
47 
48     return count;
49 }
50 
_tfm_dec_num_output(struct formatted_buffer_t * pb,int32_t num,uint8_t sign)51 static int _tfm_dec_num_output(struct formatted_buffer_t *pb,
52                                int32_t num, uint8_t sign)
53 {
54     int count = 0;
55     uint8_t num_buff[NUM_BUFF_SIZE] = {0};
56     uint32_t number = (uint32_t)num;
57     uint32_t k = 0;
58 
59     if (sign == 'd' && num < 0) {
60         _tfm_flush_formatted_buffer(pb, '-');
61         count++;
62         number = -num;
63     }
64 
65     do {
66         num_buff[k++] = '0' + number % 10;
67         number /= 10;
68     } while (number);
69 
70     while (k) {
71         _tfm_flush_formatted_buffer(pb, num_buff[--k]);
72         count++;
73     }
74 
75     return count;
76 }
77 
_tfm_hex_num_output(struct formatted_buffer_t * pb,uint32_t num,const char * hex_digits)78 static int _tfm_hex_num_output(struct formatted_buffer_t *pb, uint32_t num,
79                                const char *hex_digits)
80 {
81     int count = 0;
82     uint8_t num_buff[NUM_BUFF_SIZE] = {0};
83     uint32_t k = 0;
84 
85     do {
86         num_buff[k++] = hex_digits[num & 0x0f];
87         num >>= 4;
88     } while (num);
89 
90     while (k) {
91         _tfm_flush_formatted_buffer(pb, num_buff[--k]);
92         count++;
93     }
94 
95     return count;
96 }
97 
vprintf(const char * fmt,va_list ap)98 int vprintf(const char *fmt, va_list ap)
99 {
100     int count = 0;
101     struct formatted_buffer_t outputbuf;
102 
103     outputbuf.pos = 0;
104 
105     if (fmt == NULL) {
106         return TFM_HAL_ERROR_INVALID_INPUT;
107     }
108 
109     while (*fmt) {
110         if (*fmt == '%') {
111             switch (*(++fmt)) {
112             case 'd':
113             case 'i':
114                 count += _tfm_dec_num_output(&outputbuf,
115                                              va_arg(ap, int32_t), 'd');
116                 break;
117             case 'u':
118                 count += _tfm_dec_num_output(&outputbuf,
119                                              va_arg(ap, int32_t), 'u');
120                 break;
121             case 'x':
122                 count += _tfm_hex_num_output(&outputbuf, va_arg(ap, uint32_t),
123                                              hex_digits_lo);
124                 break;
125             case 'X':
126                 count += _tfm_hex_num_output(&outputbuf, va_arg(ap, uint32_t),
127                                              hex_digits_up);
128                 break;
129             case 'p':
130                 count += _tfm_string_output(&outputbuf, "0x");
131                 count += _tfm_hex_num_output(&outputbuf, va_arg(ap, uint32_t),
132                                              hex_digits_lo);
133                 break;
134             case 's':
135                 count += _tfm_string_output(&outputbuf, va_arg(ap, char*));
136                 break;
137             case 'c':
138                 _tfm_flush_formatted_buffer(&outputbuf,
139                                             (uint8_t)va_arg(ap, int32_t));
140                 count++;
141                 break;
142             case '%':
143                 _tfm_flush_formatted_buffer(&outputbuf, '%');
144                 count++;
145                 break;
146             default:
147                 count += _tfm_string_output(&outputbuf, "[Unsupported Tag]");
148                 continue;
149             }
150             fmt++;
151         } else {
152             _tfm_flush_formatted_buffer(&outputbuf, *fmt++);
153             count++;
154         }
155     }
156 
157     /* End of printf, flush buf */
158     if (outputbuf.pos) {
159         count += tfm_hal_output_sp_log(outputbuf.buf, outputbuf.pos);
160     }
161 
162     return count;
163 }
164 
printf(const char * fmt,...)165 int printf(const char *fmt, ...)
166 {
167     int count = 0;
168     va_list ap;
169 
170     va_start(ap, fmt);
171     count = vprintf(fmt, ap);
172     va_end(ap);
173 
174     return count;
175 }
176