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 <stddef.h> /* Compiler defns such as size_t, NULL etc. */
13 #include "strproc.h"
14 #include "section_config.h"
15 #include "diag.h"
16 #include "ameba_soc.h"
17
18 #define USHRT_MAX ((u16)(~0U))
19 #define SHRT_MAX ((s16)(USHRT_MAX>>1))
20 #define ULLONG_MAX (~0ULL)
21 #define STR_STORE_MAX_LEN 24
22 #define KSTRTOX_OVERFLOW (1U << 31)
23
24
25 /**
26 * div_u64_rem - unsigned 64bit divide with 32bit divisor with remainder
27 *
28 * This is commonly provided by 32bit archs to provide an optimized 64bit
29 * divide.
30 */
31 //static inline
32 LIBC_ROM_TEXT_SECTION
div_u64_rem(u64 dividend,u32 divisor,u32 * remainder)33 _LONG_CALL_ static u64 div_u64_rem(u64 dividend, u32 divisor, u32 *remainder)
34 {
35 *remainder = (u32)dividend % divisor;
36 return (u32)dividend / divisor;
37 }
38
39 /**
40 * div_s64_rem - signed 64bit divide with 32bit divisor with remainder
41 */
42 //static inline
43 LIBC_ROM_TEXT_SECTION
div_s64_rem(s64 dividend,s32 divisor,s32 * remainder)44 _LONG_CALL_ static s64 div_s64_rem(s64 dividend, s32 divisor, s32 *remainder)
45 {
46 *remainder = (s32)dividend % divisor;
47 return (s32)dividend / divisor;
48 }
49
50 /**
51 * div_u64 - unsigned 64bit divide with 32bit divisor
52 *
53 * This is the most common 64bit divide and should be used if possible,
54 * as many 32bit archs can optimize this variant better than a full 64bit
55 * divide.
56 */
57 #ifndef div_u64
58 //static inline
59 LIBC_ROM_TEXT_SECTION
div_u64(u64 dividend,u32 divisor)60 _LONG_CALL_ static u64 div_u64(u64 dividend, u32 divisor)
61 {
62 u32 remainder;
63 return div_u64_rem(dividend, divisor, &remainder);
64 }
65 #endif
66
67 /**
68 * div_s64 - signed 64bit divide with 32bit divisor
69 */
70 #ifndef div_s64
71 //static inline
72 LIBC_ROM_TEXT_SECTION
div_s64(s64 dividend,s32 divisor)73 _LONG_CALL_ static s64 div_s64(s64 dividend, s32 divisor)
74 {
75 s32 remainder;
76 return div_s64_rem(dividend, divisor, &remainder);
77 }
78 #endif
79
80 LIBC_ROM_TEXT_SECTION
_parse_integer_fixup_radix(const char * s,unsigned int * base)81 _LONG_CALL_ static const char *_parse_integer_fixup_radix(const char *s, unsigned int *base)
82 {
83 if (*base == 0) {
84 if (s[0] == '0') {
85 if (_tolower(s[1]) == 'x' && isxdigit(s[2]))
86 *base = 16;
87 else
88 *base = 8;
89 } else
90 *base = 10;
91 }
92 if (*base == 16 && s[0] == '0' && _tolower(s[1]) == 'x')
93 s += 2;
94 return s;
95 }
96
97 LIBC_ROM_TEXT_SECTION
skip_spaces(const char * str)98 _LONG_CALL_ static char *skip_spaces(const char *str)
99 {
100 while (isspace(*str))
101 ++str;
102 return (char *)str;
103 }
104
105 LIBC_ROM_TEXT_SECTION
skip_atoi(const char ** s)106 _LONG_CALL_ static int skip_atoi(const char **s)
107 {
108 int i = 0;
109
110 while (isdigit(**s))
111 i = i*10 + *((*s)++) - '0';
112
113 return i;
114 }
115
116 LIBC_ROM_TEXT_SECTION
judge_digit_width(const char * str,unsigned int base)117 static int judge_digit_width(const char *str, unsigned int base)
118 {
119
120 int width = 0;
121
122 if ((base == 16 || base == 0) && str[0] == '0' && _tolower(str[1]) == 'x') {
123 str += 2;
124 width += 2;
125
126 while(isxdigit(*str)) {
127 width++;
128 str++;
129 }
130 }else if(base == 16){
131 while(isxdigit(*str)) {
132 width++;
133 str++;
134 }
135 }else if(base == 8 ||(base == 0 && str[0] == '0' )) {
136 while(isdigit(*str) && (*str)<'8'){
137 width++;
138 str++;
139 }
140 }else{
141 while(isdigit(*str)) {
142 width++;
143 str++;
144 }
145 }
146
147 return width;
148 }
149
150
151 LIBC_ROM_TEXT_SECTION
_vsscanf(const char * buf,const char * fmt,va_list args)152 _LONG_CALL_ int _vsscanf(const char *buf, const char *fmt, va_list args)
153 {
154 const char *str = buf;
155 char *next;
156 char digit;
157 int num = 0;
158 int i =0;
159 u8 qualifier;
160 unsigned int base;
161 union {
162 long long s;
163 unsigned long long u;
164 } val;
165 s16 field_width;
166 bool is_sign;
167
168 char str_store[STR_STORE_MAX_LEN];
169 for(i = 0; i<STR_STORE_MAX_LEN ; i++)
170 str_store[i] = 0;
171
172
173 while(*fmt) {
174 /* skip any white space in format */
175 /* white space in format matchs any amount of
176 * white space, including none, in the input.
177 */
178 if(isspace(*fmt)) {
179 fmt = skip_spaces(++fmt);
180 str = skip_spaces(str);
181 }
182
183 /* anything that is not a conversion must match exactly */
184 if(*fmt != '%' && *fmt) {
185 if(*fmt++ != *str++) {
186 break;
187 }
188
189 continue;
190 }
191
192 if(!*fmt) {
193 break;
194 }
195
196 ++fmt;
197
198 /* skip this conversion.
199 * advance both strings to next white space
200 */
201 if(*fmt == '*') {
202 if(!*str) {
203 break;
204 }
205
206 while(!isspace(*fmt) && *fmt != '%' && *fmt)
207 fmt++;
208
209 while(!isspace(*str) && *str)
210 str++;
211
212 continue;
213 }
214
215 /* get field width */
216 field_width = -1;
217
218 if(isdigit(*fmt)) {
219
220 field_width = skip_atoi(&fmt);
221
222
223
224 if(field_width <= 0) {
225
226 break;
227 }
228 }
229
230 /* get conversion qualifier */
231 qualifier = -1;
232
233 if(*fmt == 'h' || _tolower(*fmt) == 'l' ||
234 _tolower(*fmt) == 'z') {
235 qualifier = *fmt++;
236
237 if(qualifier == *fmt) {
238 if(qualifier == 'h') {
239 qualifier = 'H';
240 fmt++;
241 } else if(qualifier == 'l') {
242 qualifier = 'L';
243 fmt++;
244 }
245 }
246 }
247
248 if(!*fmt) {
249 break;
250 }
251
252 if(*fmt == 'n') {
253 /* return number of characters read so far */
254 *va_arg(args, int *) = str - buf;
255 ++fmt;
256 continue;
257 }
258
259 if(!*str) {
260 break;
261 }
262
263 base = 10;
264 is_sign = 0;
265
266 switch(*fmt++) {
267 case 'c': {
268 char *s = (char *)va_arg(args, char*);
269
270 if(field_width == -1)
271 field_width = 1;
272
273 do {
274 *s++ = *str++;
275 } while(--field_width > 0 && *str);
276
277 num++;
278 }
279
280 continue;
281
282 case 's': {
283 char *s = (char *)va_arg(args, char *);
284
285 if(field_width == -1)
286 field_width = SHRT_MAX;
287
288 /* first, skip leading white space in buffer */
289 str = skip_spaces(str);
290
291 /* now copy until next white space */
292 while(*str && !isspace(*str) && field_width--) {
293 *s++ = *str++;
294 }
295
296 *s = '\0';
297 num++;
298 }
299
300 continue;
301
302 case 'o':
303 base = 8;
304 break;
305
306 case 'x':
307 case 'X':
308 base = 16;
309 break;
310
311 case 'i':
312 base = 0;
313
314 case 'd':
315 is_sign = 1;
316
317 case 'u':
318 break;
319
320 case '%':
321
322 /* looking for '%' in str */
323 if(*str++ != '%') {
324 return num;
325 }
326
327 continue;
328
329 default:
330 /* invalid format; stop here */
331 return num;
332 }
333
334 /* have some sort of integer conversion.
335 * first, skip white space in buffer.
336 */
337 str = skip_spaces(str);
338
339 digit = *str;
340
341 if(is_sign && digit == '-')
342 {
343 if(field_width==-1)
344 {
345 field_width = judge_digit_width(str+1, base);
346 field_width++;
347 }else{
348 field_width = (judge_digit_width(str+1,base)+1)<field_width ?
349 (judge_digit_width(str+1,base)+1) :
350 field_width;
351 }
352 digit = *(str + 1);
353 }else if(field_width==-1){
354 field_width = judge_digit_width(str, base);
355 }else{
356 field_width = judge_digit_width(str,base)<field_width ?
357 judge_digit_width(str,base) :
358 field_width;
359 }
360
361 if(!digit
362 || (base == 16 && !isxdigit(digit))
363 || (base == 10 && !isdigit(digit))
364 || (base == 8 && (!isdigit(digit) || digit > '7'))
365 || (base == 0 && !isdigit(digit))) {
366 break;
367 }
368
369 //here problem *******************************************
370
371
372
373 //troy add ,fix support %2d, but not support %d
374 #if 0
375 if(field_width <= 0) {
376
377 field_width = judge_digit_width(str);
378 }
379 #else
380 //field_width = judge_digit_width(str, base);
381 #endif
382
383
384 /////troy add, fix str passed inwidth wrong
385 assert_param (field_width <= STR_STORE_MAX_LEN);
386 for(i = 0; i<field_width ; i++)
387 str_store[i] = str[i];
388
389 #if 0//ndef CONFIG_STDLIB_STRING
390 /* simple strtoul need this */
391 next = (char*)str + field_width;
392 #endif
393
394 if(is_sign) {
395 val.s = qualifier != 'L' ?
396 _strtol(str_store, &next, base) :
397 _strtoll(str_store, &next, base);
398 } else {
399 val.u = qualifier != 'L' ?
400 _strtoul(str_store, &next, base) :
401 _strtoull(str_store, &next, base);
402 }
403
404 #if 1//def CONFIG_STDLIB_STRING
405 /* standard strtoul need this */
406 next = (char*)str + field_width;
407 #endif
408 ////troy add
409 for(i = 0; i<STR_STORE_MAX_LEN ; i++)
410 str_store[i] = 0;
411
412
413 //判断转换的字符串的宽度是否大于 %2d
414 if(field_width > 0 && next - str > field_width) {
415 if(base == 0)
416 _parse_integer_fixup_radix(str, &base);
417
418 while(next - str > field_width) {
419 if(is_sign) {
420 val.s = div_s64(val.s, base);
421 } else {
422 val.u = div_u64(val.u, base);
423 }
424
425 --next;
426 }
427 }
428
429 switch(qualifier) {
430 case 'H': /* that's 'hh' in format */
431 if(is_sign)
432 *va_arg(args, signed char *) = val.s;
433 else
434 *va_arg(args, unsigned char *) = val.u;
435
436 break;
437
438 case 'h':
439 if(is_sign)
440 *va_arg(args, short *) = val.s;
441 else
442 *va_arg(args, unsigned short *) = val.u;
443
444 break;
445
446 case 'l':
447 if(is_sign)
448 *va_arg(args, long *) = val.s;
449 else
450 *va_arg(args, unsigned long *) = val.u;
451
452 break;
453
454 case 'L':
455 if(is_sign)
456 *va_arg(args, long long *) = val.s;
457 else
458 *va_arg(args, unsigned long long *) = val.u;
459
460 break;
461
462 case 'Z':
463 case 'z':
464 *va_arg(args, size_t *) = val.u;
465 break;
466
467 default:
468 if(is_sign)
469 *va_arg(args, int *) = val.s;
470 else
471 *va_arg(args, unsigned int *) = val.u;
472
473 break;
474 }
475
476 num++;
477
478 if(!next) {
479 break;
480 }
481
482 str = next;
483 }
484
485 return num;
486 }
487
488 /**
489 * sscanf - Unformat a buffer into a list of arguments
490 * @buf: input buffer
491 * @fmt: formatting of buffer
492 * @...: resulting arguments
493 */
494 LIBC_ROM_TEXT_SECTION
_sscanf(const char * buf,const char * fmt,...)495 _LONG_CALL_ int _sscanf(const char *buf, const char *fmt, ...)
496 {
497 va_list args;
498 int i;
499
500 va_start(args, fmt);
501 i = _vsscanf(buf, fmt, args);
502 va_end(args);
503
504 return i;
505 }