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