1 /*
2 * Routines to access hardware
3 *
4 * Copyright (c) 2013 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 "basic_types.h"
11 #include <stdarg.h>
12 #include "ameba_soc.h"
13 #include "rtl8721d_loguart.h"
14 #include "section_config.h"
15 #include <strproc.h>
16 #include "diag.h"
17
18 HAL_ROM_BSS_SECTION DIAG_PRINT_BUF_FUNC ConfigDebugBufferGet;
19 HAL_ROM_BSS_SECTION u32 ConfigDebugBuffer;
20 HAL_ROM_BSS_SECTION u32 ConfigDebugClose;
21
22 HAL_ROM_BSS_SECTION u32 ConfigDebug[LEVEL_NUMs];
23
24 HAL_ROM_TEXT_SECTION
DiagVSprintf(char * buf,const char * fmt,const int * dp)25 int DiagVSprintf(char *buf, const char *fmt, const int *dp)
26 {
27 char *p, *s;
28 s = buf;
29
30 for(; *fmt != '\0'; ++fmt) {
31 if(*fmt != '%') {
32 if(buf) {
33 *s++ = *fmt;
34 } else {
35 DiagPutChar(*fmt);
36 }
37
38 continue;
39 }
40
41 if(*++fmt == 's') {
42 for(p = (char *)*dp++; *p != '\0'; p++) {
43 if(buf) {
44 *s++ = *p;
45 } else {
46 DiagPutChar(*p);
47 }
48 }
49 }
50 else { /* Length of item is bounded */
51 char tmp[20], *q = tmp;
52 int alt = 0;
53 int shift = 0;// = 12;
54 const long *lpforchk = (const long *)dp;
55
56 if((*lpforchk) < 0x10) {
57 shift = 0;
58 }
59 else if(((*lpforchk) >= 0x10) && ((*lpforchk) < 0x100)) {
60 shift = 4;
61 }
62 else if(((*lpforchk) >= 0x100) && ((*lpforchk) < 0x1000)) {
63 shift = 8;
64 }
65 else if(((*lpforchk) >= 0x1000) && ((*lpforchk) < 0x10000)) {
66 shift = 12;
67 }
68 else if(((*lpforchk) >= 0x10000) && ((*lpforchk) < 0x100000)) {
69 shift = 16;
70 }
71 else if(((*lpforchk) >= 0x100000) && ((*lpforchk) < 0x1000000)) {
72 shift = 20;
73 }
74 else if(((*lpforchk) >= 0x1000000) && ((*lpforchk) < 0x10000000)) {
75 shift = 24;
76 }
77 else if((*lpforchk) >= 0x10000000) {
78 shift = 28;
79 }
80 else {
81 shift = 28;
82 }
83 #if 1 //wei patch for %02x
84
85 if((*fmt >= '0') && (*fmt <= '9'))
86 {
87 int width;
88 unsigned char fch = *fmt;
89
90 for(width=0; (fch>='0') && (fch<='9'); fch=*++fmt)
91 { width = width * 10 + fch - '0';
92 }
93
94 shift=(width-1)*4;
95 }
96
97 #endif
98
99 /*
100 * Before each format q points to tmp buffer
101 * After each format q points past end of item
102 */
103
104 if((*fmt == 'x')||(*fmt == 'X') || (*fmt == 'p') || (*fmt == 'P')) {
105 /* With x86 gcc, sizeof(long) == sizeof(int) */
106 const long *lp = (const long *)dp;
107 unsigned long h = *lp++;
108 int hex_count = 0;
109 unsigned long h_back = h;
110 int ncase = (*fmt & 0x20);
111 dp = (const int *)lp;
112
113 if((*fmt == 'p') || (*fmt == 'P'))
114 alt=1;
115
116 if(alt) {
117 *q++ = '0';
118 *q++ = 'X' | ncase;
119 }
120
121 //hback 是实际得到的数据，hex_count是统计数据的HEX字符个数
122 while(h_back) {
123 hex_count += 1;
124 h_back = h_back >> 4;
125 }
126
127 //这里修复 example： 字符有4个，但是用了%02x导致字符被截断的情况
128 if(shift < (hex_count - 1)*4)
129 shift = (hex_count - 1)*4;
130
131 //printf("(%d,%d)", hex_count, shift);
132
133 for(; shift >= 0; shift -= 4) {
134
135 *q++ = "0123456789ABCDEF"[(h >> shift) & 0xF] | ncase;
136 }
137
138 }
139 else if(*fmt == 'd') {
140 int i = *dp++;
141 char *r;
142 int digit_space = 0;
143
144 if(i < 0) {
145 *q++ = '-';
146 i = -i;
147 digit_space++;
148 }
149
150 p = q; /* save beginning of digits */
151
152 do {
153 *q++ = '0' + (i % 10);
154 i /= 10;
155 digit_space++;
156 } while(i);
157
158 //这里修复 example：用了%08d后，在数字前面没有0的情况
159 for(; shift >= 0; shift -= 4) {
160
161 if(digit_space-- > 0) {
162 ; //do nothing
163 } else {
164 *q++ = '0';
165 }
166 }
167
168 /* reverse digits, stop in middle */
169 r = q; /* don't alter q */
170
171 while(--r > p) {
172 i = *r;
173 *r = *p;
174 *p++ = i;
175 }
176 }
177 else if(*fmt == 'c')
178 *q++ = *dp++;
179 else
180 *q++ = *fmt;
181
182 /* now output the saved string */
183 for(p = tmp; p < q; ++p) {
184 if(buf) {
185 *s++ = *p;
186 } else {
187 DiagPutChar(*p);
188 }
189
190 if((*p) == '\n') {
191 DiagPutChar('\r');
192 }
193 }
194 }
195 }
196
197 if(buf)
198 *s = '\0';
199
200 return (s - buf);
201 }
202
203 HAL_ROM_TEXT_SECTION
204 _LONG_CALL_ u32
DiagPrintf(IN const char * fmt,...)205 DiagPrintf(
206 IN const char *fmt, ...
207 )
208 {
209 log_buffer_t *buf = NULL;
210 int ret = 0;
211
212 if (ConfigDebugClose == 1)
213 return 0;
214
215 if (ConfigDebugBuffer == 1 && ConfigDebugBufferGet != NULL) {
216 buf = (log_buffer_t *)ConfigDebugBufferGet(fmt);
217 }
218
219 if (buf != NULL) {
220 return DiagVSprintf(buf->buffer, fmt, ((const int *)&fmt)+1);
221 } else {
222 ret = DiagVSprintf(NULL, fmt, ((const int *)&fmt)+1);
223
224 return ret;
225 }
226 }
227
228 HAL_ROM_TEXT_SECTION
229 _LONG_CALL_ u32
DiagPrintfD(IN const char * fmt,...)230 DiagPrintfD(
231 IN const char *fmt, ...
232 )
233 {
234 //log_buffer_t *buf = NULL;
235
236 if (ConfigDebugClose == 1)
237 return 0;
238
239 return DiagVSprintf(NULL, fmt, ((const int *)&fmt)+1);
240 }
241
242 HAL_ROM_TEXT_SECTION
243 u32
DiagSPrintf(IN u8 * buf,IN const char * fmt,...)244 DiagSPrintf(
245 IN u8 *buf,
246 IN const char *fmt, ...
247 )
248 {
249 if (ConfigDebugClose == 1)
250 return 0;
251
252 return DiagVSprintf((char*)buf, fmt, ((const int *)&fmt)+1);
253 }
254
255
256
257
258 HAL_ROM_TEXT_SECTION
DiagSnPrintf(char * buf,size_t size,const char * fmt,...)259 int DiagSnPrintf(char *buf, size_t size, const char *fmt, ...)
260 {
261
262 va_list ap;
263 char *p, *s, *buf_end = NULL;
264 const int *dp = ((const int *)&fmt)+1;
265
266 if(buf == NULL)
267 return 0;
268
269
270 va_start(ap, fmt);
271 s = buf;
272 buf_end = size? (buf + size):(char*)~0;
273
274 for(; *fmt != '\0'; ++fmt) {
275
276 if(*fmt != '%') {
277 *s++ = *fmt;
278
279 if(s >= buf_end) {
280 goto Exit;
281 }
282
283 continue;
284 }
285
286 if(*++fmt == 's') {
287 for(p = (char *)*dp++; *p != '\0'; p++) {
288 *s++ = *p;
289
290 if(s >= buf_end) {
291 goto Exit;
292 }
293 }
294 }
295 else { /* Length of item is bounded */
296 char tmp[20], *q = tmp;
297 int alt = 0;
298 int shift = 0;// = 12;
299 const long *lpforchk = (const long *)dp;
300
301 if((*lpforchk) < 0x10) {
302 shift = 0;
303 }
304 else if(((*lpforchk) >= 0x10) && ((*lpforchk) < 0x100)) {
305 shift = 4;
306 }
307 else if(((*lpforchk) >= 0x100) && ((*lpforchk) < 0x1000)) {
308 shift = 8;
309 }
310 else if(((*lpforchk) >= 0x1000) && ((*lpforchk) < 0x10000)) {
311 shift = 12;
312 }
313 else if(((*lpforchk) >= 0x10000) && ((*lpforchk) < 0x100000)) {
314 shift = 16;
315 }
316 else if(((*lpforchk) >= 0x100000) && ((*lpforchk) < 0x1000000)) {
317 shift = 20;
318 }
319 else if(((*lpforchk) >= 0x1000000) && ((*lpforchk) < 0x10000000)) {
320 shift = 24;
321 }
322 else if((*lpforchk) >= 0x10000000) {
323 shift = 28;
324 }
325 else {
326 shift = 28;
327 }
328
329 if((*fmt >= '0') && (*fmt <= '9'))
330 {
331 int width;
332 unsigned char fch = *fmt;
333
334 for(width=0; (fch>='0') && (fch<='9'); fch=*++fmt)
335 { width = width * 10 + fch - '0';
336 }
337
338 shift=(width-1)*4;
339 }
340
341 /*
342 * Before each format q points to tmp buffer
343 * After each format q points past end of item
344 */
345 if((*fmt == 'x')||(*fmt == 'X') || (*fmt == 'p') || (*fmt == 'P')) {
346 /* With x86 gcc, sizeof(long) == sizeof(int) */
347 const long *lp = (const long *)dp;
348 long h = *lp++;
349 int hex_count = 0;
350 unsigned long h_back = h;
351 int ncase = (*fmt & 0x20);
352 dp = (const int *)lp;
353
354 if((*fmt == 'p') || (*fmt == 'P'))
355 alt=1;
356
357 if(alt) {
358 *q++ = '0';
359 *q++ = 'X' | ncase;
360 }
361
362 while(h_back) {
363 hex_count += (h_back & 0xF) ? 1 : 0;
364 h_back = h_back >> 4;
365 }
366
367 if(shift < (hex_count - 1)*4)
368 shift = (hex_count - 1)*4;
369
370 for(; shift >= 0; shift -= 4)
371 *q++ = "0123456789ABCDEF"[(h >> shift) & 0xF] | ncase;
372 }
373 else if(*fmt == 'd') {
374 int i = *dp++;
375 char *r;
376 int digit_space = 0;
377
378
379 if(i < 0) {
380 *q++ = '-';
381 i = -i;
382 digit_space++;
383 }
384
385 p = q; /* save beginning of digits */
386
387
388 do {
389 *q++ = '0' + (i % 10);
390 i /= 10;
391 digit_space++;
392 } while(i);
393
394
395 for(; shift >= 0; shift -= 4) {
396
397 if(digit_space-- > 0) {
398 ; //do nothing
399 } else {
400 *q++ = '0';
401 }
402 }
403
404 /* reverse digits, stop in middle */
405 r = q; /* don't alter q */
406
407 while(--r > p) {
408 i = *r;
409 *r = *p;
410 *p++ = i;
411 }
412 }
413 else if(*fmt == 'c')
414 *q++ = *dp++;
415 else
416 *q++ = *fmt;
417
418 /* now output the saved string */
419 for(p = tmp; p < q; ++p) {
420 *s++ = *p;
421
422 if(s >= buf_end) {
423 goto Exit;
424 }
425 }
426 }
427 }
428
429 Exit:
430
431 if(buf)
432 *s = '\0';
433
434 va_end(ap);
435 return(s-buf);
436
437 }
438
439