1 /*
2 * Routines to access rom software library.
3 *
4 * Copyright (c) 2019 Realtek Semiconductor Corp.
5 *
6 * This module is a confidential and proprietary property of RealTek and
7 * possession or use of this module requires written permission of RealTek.
8 */
9
10 #include "diag.h"
11 #include "strproc.h"
12 #include <stdarg.h>
13
14 typedef union _va_int_ {
15 va_list ap;
16 const int *dp;
17 } va_int,*pva_int;
18
_rtl_vsprintf(char * buf,size_t size,const char * fmt,const int * dp)19 int _rtl_vsprintf(char *buf, size_t size, const char *fmt, const int *dp)
20 {
21 char *p, *s, *buf_end = NULL;
22
23 if(buf == NULL)
24 return 0;
25
26 s = buf;
27 buf_end = size? (buf + size):(char*)~0;
28
29 for(; *fmt != '\0'; ++fmt) {
30 if(*fmt != '%') {
31 *s++ = *fmt;
32
33 if(s >= buf_end) {
34 goto Exit;
35 }
36
37 continue;
38 }
39
40 if(*++fmt == 's') {
41 for(p = (char *)*dp++; *p != '\0'; p++) {
42 *s++ = *p;
43
44 if(s >= buf_end) {
45 goto Exit;
46 }
47 }
48 }
49 else { /* Length of item is bounded */
50 char tmp[20], *q = tmp;
51 int alt = 0;
52 int shift = 0;// = 12;
53 const long *lpforchk = (const long *)dp;
54
55 if((*lpforchk) < 0x10) {
56 shift = 0;
57 }
58 else if(((*lpforchk) >= 0x10) && ((*lpforchk) < 0x100)) {
59 shift = 4;
60 }
61 else if(((*lpforchk) >= 0x100) && ((*lpforchk) < 0x1000)) {
62 shift = 8;
63 }
64 else if(((*lpforchk) >= 0x1000) && ((*lpforchk) < 0x10000)) {
65 shift = 12;
66 }
67 else if(((*lpforchk) >= 0x10000) && ((*lpforchk) < 0x100000)) {
68 shift = 16;
69 }
70 else if(((*lpforchk) >= 0x100000) && ((*lpforchk) < 0x1000000)) {
71 shift = 20;
72 }
73 else if(((*lpforchk) >= 0x1000000) && ((*lpforchk) < 0x10000000)) {
74 shift = 24;
75 }
76 else if((*lpforchk) >= 0x10000000) {
77 shift = 28;
78 }
79 else {
80 shift = 28;
81 }
82
83 if((*fmt >= '0') && (*fmt <= '9'))
84 {
85 int width;
86 unsigned char fch = *fmt;
87
88 for(width=0; (fch>='0') && (fch<='9'); fch=*++fmt)
89 { width = width * 10 + fch - '0';
90 }
91
92 shift=(width-1)*4;
93 }
94
95
96 /*
97 * Before each format q points to tmp buffer
98 * After each format q points past end of item
99 */
100
101 if((*fmt == 'x')||(*fmt == 'X') || (*fmt == 'p') || (*fmt == 'P')) {
102 /* With x86 gcc, sizeof(long) == sizeof(int) */
103 const long *lp = (const long *)dp;
104 long h = *lp++;
105 int hex_count = 0;
106 unsigned long h_back = h;
107 int ncase = (*fmt & 0x20);
108 dp = (const int *)lp;
109
110 if((*fmt == 'p') || (*fmt == 'P'))
111 alt=1;
112
113 if(alt) {
114 *q++ = '0';
115 *q++ = 'X' | ncase;
116 }
117
118 while(h_back) {
119 hex_count += (h_back & 0xF) ? 1 : 0;
120 h_back = h_back >> 4;
121 }
122
123 if(shift < (hex_count - 1)*4)
124 shift = (hex_count - 1)*4;
125
126 for(; shift >= 0; shift -= 4)
127 *q++ = "0123456789ABCDEF"[(h >> shift) & 0xF] | ncase;
128 }
129 else if(*fmt == 'd') {
130 int i = *dp++;
131 char *r;
132 int digit_space = 0;
133
134 if(i < 0) {
135 *q++ = '-';
136 i = -i;
137 digit_space++;
138 }
139
140 p = q; /* save beginning of digits */
141
142 do {
143 *q++ = '0' + (i % 10);
144 i /= 10;
145 digit_space++;
146 } while(i);
147
148
149 for(; shift >= 0; shift -= 4) {
150
151 if(digit_space-- > 0) {
152 ; //do nothing
153 } else {
154 *q++ = '0';
155 }
156 }
157
158 /* reverse digits, stop in middle */
159 r = q; /* don't alter q */
160
161 while(--r > p) {
162 i = *r;
163 *r = *p;
164 *p++ = i;
165 }
166 }
167 else if(*fmt == 'c')
168 *q++ = *dp++;
169 else {
170 if(*fmt != '%')
171 DiagPrintf("%s: format not support!\n", __func__);
172 *q++ = *fmt;
173 }
174
175 /* now output the saved string */
176 for(p = tmp; p < q; ++p) {
177 *s++ = *p;
178
179 if(s >= buf_end) {
180 goto Exit;
181 }
182 }
183 }
184 }
185
186 Exit:
187
188 if(buf)
189 *s = '\0';
190
191 return (s - buf);
192 }
193
_rtl_vsnprintf(char * buf,size_t size,const char * fmt,va_list ap)194 int _rtl_vsnprintf(char *buf, size_t size, const char *fmt, va_list ap)
195 {
196 int ret;
197 va_int m;
198 m.ap = ap;
199
200 ret = _rtl_vsprintf(buf, size, fmt, m.dp);
201 return ret;
202 }
203
_rtl_snprintf(char * str,size_t size,const char * fmt,...)204 int _rtl_snprintf(char* str, size_t size, const char* fmt, ...)
205 {
206 int ret;
207 ret = _rtl_vsprintf(str, size, fmt, ((const int *)&fmt)+1);
208 return ret;
209 }
210
_rtl_sprintf(char * str,const char * fmt,...)211 int _rtl_sprintf(char* str, const char* fmt, ...)
212 {
213 const char* fmt1;
214
215 fmt1 = fmt;
216
217 for(; *fmt1 != '\0'; ++fmt1) {
218 if(*fmt1 != '%')
219 continue;
220 else
221 fmt1 ++;
222
223 while(isdigit(*fmt1)){
224 fmt1 ++;
225 }
226
227 if((*fmt1 == 's') || (*fmt1 == 'x') || (*fmt1 == 'X') || (*fmt1 == 'p') || (*fmt1 == 'P') || (*fmt1 == 'd') || (*fmt1 == 'c') || (*fmt1 == '%'))
228 continue;
229 else {
230 DiagPrintf("%s: format not support!\n", __func__);
231 break;
232 }
233 }
234
235 if (ConfigDebugClose == 1)
236 return 0;
237
238 return DiagVSprintf((char*)str, fmt, ((const int *)&fmt)+1);
239 }
240
_rtl_printf(const char * fmt,...)241 int _rtl_printf(const char* fmt, ...)
242 {
243 u32 ret;
244 const char* fmt1;
245 log_buffer_t *buf = NULL;
246
247 fmt1 = fmt;
248
249 for(; *fmt1 != '\0'; ++fmt1) {
250 if(*fmt1 == '"') {
251 do {
252 fmt1 ++;
253 } while(*fmt1 != '"');
254 fmt1 ++;
255 }
256
257 if(*fmt1 != '%')
258 continue;
259 else
260 fmt1 ++;
261
262 while(isdigit(*fmt1)){
263 fmt1 ++;
264 }
265
266 if((*fmt1 == 's') || (*fmt1 == 'x') || (*fmt1 == 'X') || (*fmt1 == 'p') || (*fmt1 == 'P') || (*fmt1 == 'd') || (*fmt1 == 'c') || (*fmt1 == '%'))
267 continue;
268 else {
269 DiagPrintf("%s: format not support!\n", __func__);
270 break;
271 }
272 }
273
274 if (ConfigDebugClose == 1)
275 return 0;
276
277 if (ConfigDebugBuffer == 1 && ConfigDebugBufferGet != NULL) {
278 buf = (log_buffer_t *)ConfigDebugBufferGet(fmt);
279 }
280
281 if (buf != NULL) {
282 return DiagVSprintf(buf->buffer, fmt, ((const int *)&fmt)+1);
283 } else {
284 ret = DiagVSprintf(NULL, fmt, ((const int *)&fmt)+1);
285
286 return ret;
287 }
288 }
289
290 /*
291 * Fast implementation of tolower() for internal usage. Do not use in your
292 * code.
293 */
_tolower(const char c)294 static inline char _tolower(const char c)
295 {
296 return c | 0x20;
297 }
298
_rtl_sscanf(const char * buf,const char * fmt,...)299 int _rtl_sscanf(const char *buf, const char *fmt, ...)
300 {
301 int ret;
302 char nxt_fmt = -1;
303 va_list args;
304 const char* fmt1;
305
306 fmt1 = fmt;
307
308 for(; *fmt1 != '\0'; ++fmt1) {
309 if(*fmt1 != '%')
310 continue;
311 else
312 fmt1 ++;
313
314 if(*fmt1 == '*')
315 continue;
316
317 while(isdigit(*fmt1)){
318 fmt1 ++;
319 }
320
321 if(*fmt1 == 'h' || _tolower(*fmt1) == 'l' || _tolower(*fmt1) == 'z') {
322 nxt_fmt = *fmt1++;
323 if(nxt_fmt == *fmt1) {
324 if((nxt_fmt == 'h') || (nxt_fmt == 'l'))
325 fmt1++;
326 }
327 }
328
329 if(!*fmt1) {
330 break;
331 }
332
333 if(*fmt1 == 'n')
334 continue;
335
336 if((*fmt1 == 'c') || (*fmt1 == 's') || (*fmt1 == 'o') || (*fmt1 == 'x') || (*fmt1 == 'X') || (*fmt1 == 'i') || (*fmt1 == 'd') || (*fmt1 == 'u') || (*fmt1 == '%'))
337 continue;
338 else {
339 DiagPrintf("%s: format not support!\n", __func__);
340 break;
341 }
342 }
343
344 va_start (args, fmt);
345 ret = _vsscanf(buf, fmt, args);
346 va_end (args);
347 return ret;
348 }
349