kservice.c 32 KB

1234567891011121314151617181920212223242526272829303132333435363738394041424344454647484950515253545556575859606162636465666768697071727374757677787980818283848586878889909192939495969798991001011021031041051061071081091101111121131141151161171181191201211221231241251261271281291301311321331341351361371381391401411421431441451461471481491501511521531541551561571581591601611621631641651661671681691701711721731741751761771781791801811821831841851861871881891901911921931941951961971981992002012022032042052062072082092102112122132142152162172182192202212222232242252262272282292302312322332342352362372382392402412422432442452462472482492502512522532542552562572582592602612622632642652662672682692702712722732742752762772782792802812822832842852862872882892902912922932942952962972982993003013023033043053063073083093103113123133143153163173183193203213223233243253263273283293303313323333343353363373383393403413423433443453463473483493503513523533543553563573583593603613623633643653663673683693703713723733743753763773783793803813823833843853863873883893903913923933943953963973983994004014024034044054064074084094104114124134144154164174184194204214224234244254264274284294304314324334344354364374384394404414424434444454464474484494504514524534544554564574584594604614624634644654664674684694704714724734744754764774784794804814824834844854864874884894904914924934944954964974984995005015025035045055065075085095105115125135145155165175185195205215225235245255265275285295305315325335345355365375385395405415425435445455465475485495505515525535545555565575585595605615625635645655665675685695705715725735745755765775785795805815825835845855865875885895905915925935945955965975985996006016026036046056066076086096106116126136146156166176186196206216226236246256266276286296306316326336346356366376386396406416426436446456466476486496506516526536546556566576586596606616626636646656666676686696706716726736746756766776786796806816826836846856866876886896906916926936946956966976986997007017027037047057067077087097107117127137147157167177187197207217227237247257267277287297307317327337347357367377387397407417427437447457467477487497507517527537547557567577587597607617627637647657667677687697707717727737747757767777787797807817827837847857867877887897907917927937947957967977987998008018028038048058068078088098108118128138148158168178188198208218228238248258268278288298308318328338348358368378388398408418428438448458468478488498508518528538548558568578588598608618628638648658668678688698708718728738748758768778788798808818828838848858868878888898908918928938948958968978988999009019029039049059069079089099109119129139149159169179189199209219229239249259269279289299309319329339349359369379389399409419429439449459469479489499509519529539549559569579589599609619629639649659669679689699709719729739749759769779789799809819829839849859869879889899909919929939949959969979989991000100110021003100410051006100710081009101010111012101310141015101610171018101910201021102210231024102510261027102810291030103110321033103410351036103710381039104010411042104310441045104610471048104910501051105210531054105510561057105810591060106110621063106410651066106710681069107010711072107310741075107610771078107910801081108210831084108510861087108810891090109110921093109410951096109710981099110011011102110311041105110611071108110911101111111211131114111511161117111811191120112111221123112411251126112711281129113011311132113311341135113611371138113911401141114211431144114511461147114811491150115111521153115411551156115711581159116011611162116311641165116611671168116911701171117211731174117511761177117811791180118111821183118411851186118711881189119011911192119311941195119611971198119912001201120212031204120512061207120812091210121112121213121412151216121712181219122012211222122312241225122612271228122912301231123212331234123512361237123812391240124112421243124412451246124712481249125012511252125312541255125612571258125912601261126212631264126512661267126812691270127112721273127412751276127712781279128012811282128312841285128612871288128912901291129212931294129512961297129812991300130113021303130413051306130713081309131013111312131313141315131613171318131913201321132213231324132513261327132813291330133113321333133413351336133713381339134013411342134313441345134613471348134913501351135213531354135513561357135813591360136113621363
  1. /*
  2. * Copyright (c) 2006-2018, RT-Thread Development Team
  3. *
  4. * SPDX-License-Identifier: Apache-2.0
  5. *
  6. * Change Logs:
  7. * Date Author Notes
  8. * 2006-03-16 Bernard the first version
  9. * 2006-05-25 Bernard rewrite vsprintf
  10. * 2006-08-10 Bernard add rt_show_version
  11. * 2010-03-17 Bernard remove rt_strlcpy function
  12. * fix gcc compiling issue.
  13. * 2010-04-15 Bernard remove weak definition on ICCM16C compiler
  14. * 2012-07-18 Arda add the alignment display for signed integer
  15. * 2012-11-23 Bernard fix IAR compiler error.
  16. * 2012-12-22 Bernard fix rt_kprintf issue, which found by Grissiom.
  17. * 2013-06-24 Bernard remove rt_kprintf if RT_USING_CONSOLE is not defined.
  18. * 2013-09-24 aozima make sure the device is in STREAM mode when used by rt_kprintf.
  19. * 2015-07-06 Bernard Add rt_assert_handler routine.
  20. */
  21. #include <rtthread.h>
  22. #include <rthw.h>
  23. #ifdef RT_USING_MODULE
  24. #include <dlmodule.h>
  25. #endif
  26. /* use precision */
  27. #define RT_PRINTF_PRECISION
  28. /**
  29. * @addtogroup KernelService
  30. */
  31. /**@{*/
  32. /* global errno in RT-Thread */
  33. static volatile int __rt_errno;
  34. #if defined(RT_USING_DEVICE) && defined(RT_USING_CONSOLE)
  35. static rt_device_t _console_device = RT_NULL;
  36. #endif
  37. /*
  38. * This function will get errno
  39. *
  40. * @return errno
  41. */
  42. rt_err_t rt_get_errno(void)
  43. {
  44. rt_thread_t tid;
  45. if (rt_interrupt_get_nest() != 0)
  46. {
  47. /* it's in interrupt context */
  48. return __rt_errno;
  49. }
  50. tid = rt_thread_self();
  51. if (tid == RT_NULL)
  52. return __rt_errno;
  53. return tid->error;
  54. }
  55. RTM_EXPORT(rt_get_errno);
  56. /*
  57. * This function will set errno
  58. *
  59. * @param error the errno shall be set
  60. */
  61. void rt_set_errno(rt_err_t error)
  62. {
  63. rt_thread_t tid;
  64. if (rt_interrupt_get_nest() != 0)
  65. {
  66. /* it's in interrupt context */
  67. __rt_errno = error;
  68. return;
  69. }
  70. tid = rt_thread_self();
  71. if (tid == RT_NULL)
  72. {
  73. __rt_errno = error;
  74. return;
  75. }
  76. tid->error = error;
  77. }
  78. RTM_EXPORT(rt_set_errno);
  79. /**
  80. * This function returns errno.
  81. *
  82. * @return the errno in the system
  83. */
  84. int *_rt_errno(void)
  85. {
  86. rt_thread_t tid;
  87. if (rt_interrupt_get_nest() != 0)
  88. return (int *)&__rt_errno;
  89. tid = rt_thread_self();
  90. if (tid != RT_NULL)
  91. return (int *) & (tid->error);
  92. return (int *)&__rt_errno;
  93. }
  94. RTM_EXPORT(_rt_errno);
  95. /**
  96. * This function will set the content of memory to specified value
  97. *
  98. * @param s the address of source memory
  99. * @param c the value shall be set in content
  100. * @param count the copied length
  101. *
  102. * @return the address of source memory
  103. */
  104. void *rt_memset(void *s, int c, rt_ubase_t count)
  105. {
  106. #ifdef RT_USING_TINY_SIZE
  107. char *xs = (char *)s;
  108. while (count--)
  109. *xs++ = c;
  110. return s;
  111. #else
  112. #define LBLOCKSIZE (sizeof(long))
  113. #define UNALIGNED(X) ((long)X & (LBLOCKSIZE - 1))
  114. #define TOO_SMALL(LEN) ((LEN) < LBLOCKSIZE)
  115. unsigned int i;
  116. char *m = (char *)s;
  117. unsigned long buffer;
  118. unsigned long *aligned_addr;
  119. unsigned int d = c & 0xff; /* To avoid sign extension, copy C to an
  120. unsigned variable. */
  121. if (!TOO_SMALL(count) && !UNALIGNED(s))
  122. {
  123. /* If we get this far, we know that n is large and m is word-aligned. */
  124. aligned_addr = (unsigned long *)s;
  125. /* Store D into each char sized location in BUFFER so that
  126. * we can set large blocks quickly.
  127. */
  128. if (LBLOCKSIZE == 4)
  129. {
  130. buffer = (d << 8) | d;
  131. buffer |= (buffer << 16);
  132. }
  133. else
  134. {
  135. buffer = 0;
  136. for (i = 0; i < LBLOCKSIZE; i ++)
  137. buffer = (buffer << 8) | d;
  138. }
  139. while (count >= LBLOCKSIZE * 4)
  140. {
  141. *aligned_addr++ = buffer;
  142. *aligned_addr++ = buffer;
  143. *aligned_addr++ = buffer;
  144. *aligned_addr++ = buffer;
  145. count -= 4 * LBLOCKSIZE;
  146. }
  147. while (count >= LBLOCKSIZE)
  148. {
  149. *aligned_addr++ = buffer;
  150. count -= LBLOCKSIZE;
  151. }
  152. /* Pick up the remainder with a bytewise loop. */
  153. m = (char *)aligned_addr;
  154. }
  155. while (count--)
  156. {
  157. *m++ = (char)d;
  158. }
  159. return s;
  160. #undef LBLOCKSIZE
  161. #undef UNALIGNED
  162. #undef TOO_SMALL
  163. #endif
  164. }
  165. RTM_EXPORT(rt_memset);
  166. /**
  167. * This function will copy memory content from source address to destination
  168. * address.
  169. *
  170. * @param dst the address of destination memory
  171. * @param src the address of source memory
  172. * @param count the copied length
  173. *
  174. * @return the address of destination memory
  175. */
  176. void *rt_memcpy(void *dst, const void *src, rt_ubase_t count)
  177. {
  178. #ifdef RT_USING_TINY_SIZE
  179. char *tmp = (char *)dst, *s = (char *)src;
  180. rt_ubase_t len;
  181. if (tmp <= s || tmp > (s + count))
  182. {
  183. while (count--)
  184. *tmp ++ = *s ++;
  185. }
  186. else
  187. {
  188. for (len = count; len > 0; len --)
  189. tmp[len - 1] = s[len - 1];
  190. }
  191. return dst;
  192. #else
  193. #define UNALIGNED(X, Y) \
  194. (((long)X & (sizeof (long) - 1)) | ((long)Y & (sizeof (long) - 1)))
  195. #define BIGBLOCKSIZE (sizeof (long) << 2)
  196. #define LITTLEBLOCKSIZE (sizeof (long))
  197. #define TOO_SMALL(LEN) ((LEN) < BIGBLOCKSIZE)
  198. char *dst_ptr = (char *)dst;
  199. char *src_ptr = (char *)src;
  200. long *aligned_dst;
  201. long *aligned_src;
  202. int len = count;
  203. /* If the size is small, or either SRC or DST is unaligned,
  204. then punt into the byte copy loop. This should be rare. */
  205. if (!TOO_SMALL(len) && !UNALIGNED(src_ptr, dst_ptr))
  206. {
  207. aligned_dst = (long *)dst_ptr;
  208. aligned_src = (long *)src_ptr;
  209. /* Copy 4X long words at a time if possible. */
  210. while (len >= BIGBLOCKSIZE)
  211. {
  212. *aligned_dst++ = *aligned_src++;
  213. *aligned_dst++ = *aligned_src++;
  214. *aligned_dst++ = *aligned_src++;
  215. *aligned_dst++ = *aligned_src++;
  216. len -= BIGBLOCKSIZE;
  217. }
  218. /* Copy one long word at a time if possible. */
  219. while (len >= LITTLEBLOCKSIZE)
  220. {
  221. *aligned_dst++ = *aligned_src++;
  222. len -= LITTLEBLOCKSIZE;
  223. }
  224. /* Pick up any residual with a byte copier. */
  225. dst_ptr = (char *)aligned_dst;
  226. src_ptr = (char *)aligned_src;
  227. }
  228. while (len--)
  229. *dst_ptr++ = *src_ptr++;
  230. return dst;
  231. #undef UNALIGNED
  232. #undef BIGBLOCKSIZE
  233. #undef LITTLEBLOCKSIZE
  234. #undef TOO_SMALL
  235. #endif
  236. }
  237. RTM_EXPORT(rt_memcpy);
  238. /**
  239. * This function will move memory content from source address to destination
  240. * address.
  241. *
  242. * @param dest the address of destination memory
  243. * @param src the address of source memory
  244. * @param n the copied length
  245. *
  246. * @return the address of destination memory
  247. */
  248. void *rt_memmove(void *dest, const void *src, rt_ubase_t n)
  249. {
  250. char *tmp = (char *)dest, *s = (char *)src;
  251. if (s < tmp && tmp < s + n)
  252. {
  253. tmp += n;
  254. s += n;
  255. while (n--)
  256. *(--tmp) = *(--s);
  257. }
  258. else
  259. {
  260. while (n--)
  261. *tmp++ = *s++;
  262. }
  263. return dest;
  264. }
  265. RTM_EXPORT(rt_memmove);
  266. /**
  267. * This function will compare two areas of memory
  268. *
  269. * @param cs one area of memory
  270. * @param ct znother area of memory
  271. * @param count the size of the area
  272. *
  273. * @return the result
  274. */
  275. rt_int32_t rt_memcmp(const void *cs, const void *ct, rt_ubase_t count)
  276. {
  277. const unsigned char *su1, *su2;
  278. int res = 0;
  279. for (su1 = cs, su2 = ct; 0 < count; ++su1, ++su2, count--)
  280. if ((res = *su1 - *su2) != 0)
  281. break;
  282. return res;
  283. }
  284. RTM_EXPORT(rt_memcmp);
  285. /**
  286. * This function will return the first occurrence of a string.
  287. *
  288. * @param s1 the source string
  289. * @param s2 the find string
  290. *
  291. * @return the first occurrence of a s2 in s1, or RT_NULL if no found.
  292. */
  293. char *rt_strstr(const char *s1, const char *s2)
  294. {
  295. int l1, l2;
  296. l2 = rt_strlen(s2);
  297. if (!l2)
  298. return (char *)s1;
  299. l1 = rt_strlen(s1);
  300. while (l1 >= l2)
  301. {
  302. l1 --;
  303. if (!rt_memcmp(s1, s2, l2))
  304. return (char *)s1;
  305. s1 ++;
  306. }
  307. return RT_NULL;
  308. }
  309. RTM_EXPORT(rt_strstr);
  310. /**
  311. * This function will compare two strings while ignoring differences in case
  312. *
  313. * @param a the string to be compared
  314. * @param b the string to be compared
  315. *
  316. * @return the result
  317. */
  318. rt_uint32_t rt_strcasecmp(const char *a, const char *b)
  319. {
  320. int ca, cb;
  321. do
  322. {
  323. ca = *a++ & 0xff;
  324. cb = *b++ & 0xff;
  325. if (ca >= 'A' && ca <= 'Z')
  326. ca += 'a' - 'A';
  327. if (cb >= 'A' && cb <= 'Z')
  328. cb += 'a' - 'A';
  329. }
  330. while (ca == cb && ca != '\0');
  331. return ca - cb;
  332. }
  333. RTM_EXPORT(rt_strcasecmp);
  334. /**
  335. * This function will copy string no more than n bytes.
  336. *
  337. * @param dst the string to copy
  338. * @param src the string to be copied
  339. * @param n the maximum copied length
  340. *
  341. * @return the result
  342. */
  343. char *rt_strncpy(char *dst, const char *src, rt_ubase_t n)
  344. {
  345. if (n != 0)
  346. {
  347. char *d = dst;
  348. const char *s = src;
  349. do
  350. {
  351. if ((*d++ = *s++) == 0)
  352. {
  353. /* NUL pad the remaining n-1 bytes */
  354. while (--n != 0)
  355. *d++ = 0;
  356. break;
  357. }
  358. } while (--n != 0);
  359. }
  360. return (dst);
  361. }
  362. RTM_EXPORT(rt_strncpy);
  363. /**
  364. * This function will compare two strings with specified maximum length
  365. *
  366. * @param cs the string to be compared
  367. * @param ct the string to be compared
  368. * @param count the maximum compare length
  369. *
  370. * @return the result
  371. */
  372. rt_int32_t rt_strncmp(const char *cs, const char *ct, rt_ubase_t count)
  373. {
  374. register signed char __res = 0;
  375. while (count)
  376. {
  377. if ((__res = *cs - *ct++) != 0 || !*cs++)
  378. break;
  379. count --;
  380. }
  381. return __res;
  382. }
  383. RTM_EXPORT(rt_strncmp);
  384. /**
  385. * This function will compare two strings without specified length
  386. *
  387. * @param cs the string to be compared
  388. * @param ct the string to be compared
  389. *
  390. * @return the result
  391. */
  392. rt_int32_t rt_strcmp(const char *cs, const char *ct)
  393. {
  394. while (*cs && *cs == *ct)
  395. cs++, ct++;
  396. return (*cs - *ct);
  397. }
  398. RTM_EXPORT(rt_strcmp);
  399. /**
  400. * The strnlen() function returns the number of characters in the
  401. * string pointed to by s, excluding the terminating null byte ('\0'),
  402. * but at most maxlen. In doing this, strnlen() looks only at the
  403. * first maxlen characters in the string pointed to by s and never
  404. * beyond s+maxlen.
  405. *
  406. * @param s the string
  407. * @param maxlen the max size
  408. * @return the length of string
  409. */
  410. rt_size_t rt_strnlen(const char *s, rt_ubase_t maxlen)
  411. {
  412. const char *sc;
  413. for (sc = s; *sc != '\0' && sc - s < maxlen; ++sc) /* nothing */
  414. ;
  415. return sc - s;
  416. }
  417. /**
  418. * This function will return the length of a string, which terminate will
  419. * null character.
  420. *
  421. * @param s the string
  422. *
  423. * @return the length of string
  424. */
  425. rt_size_t rt_strlen(const char *s)
  426. {
  427. const char *sc;
  428. for (sc = s; *sc != '\0'; ++sc) /* nothing */
  429. ;
  430. return sc - s;
  431. }
  432. RTM_EXPORT(rt_strlen);
  433. #ifdef RT_USING_HEAP
  434. /**
  435. * This function will duplicate a string.
  436. *
  437. * @param s the string to be duplicated
  438. *
  439. * @return the duplicated string pointer
  440. */
  441. char *rt_strdup(const char *s)
  442. {
  443. rt_size_t len = rt_strlen(s) + 1;
  444. char *tmp = (char *)rt_malloc(len);
  445. if (!tmp)
  446. return RT_NULL;
  447. rt_memcpy(tmp, s, len);
  448. return tmp;
  449. }
  450. RTM_EXPORT(rt_strdup);
  451. #if defined(__CC_ARM) || defined(__CLANG_ARM)
  452. char *strdup(const char *s) __attribute__((alias("rt_strdup")));
  453. #endif
  454. #endif
  455. /**
  456. * This function will show the version of rt-thread rtos
  457. */
  458. void rt_show_version(void)
  459. {
  460. rt_kprintf("\n \\ | /\n");
  461. rt_kprintf("- RT - Thread Operating System\n");
  462. rt_kprintf(" / | \\ %d.%d.%d build %s\n",
  463. RT_VERSION, RT_SUBVERSION, RT_REVISION, __DATE__);
  464. rt_kprintf(" 2006 - 2018 Copyright by rt-thread team\n");
  465. }
  466. RTM_EXPORT(rt_show_version);
  467. /* private function */
  468. #define isdigit(c) ((unsigned)((c) - '0') < 10)
  469. rt_inline int divide(long *n, int base)
  470. {
  471. int res;
  472. /* optimized for processor which does not support divide instructions. */
  473. if (base == 10)
  474. {
  475. res = (int)(((unsigned long)*n) % 10U);
  476. *n = (long)(((unsigned long)*n) / 10U);
  477. }
  478. else
  479. {
  480. res = (int)(((unsigned long)*n) % 16U);
  481. *n = (long)(((unsigned long)*n) / 16U);
  482. }
  483. return res;
  484. }
  485. rt_inline int skip_atoi(const char **s)
  486. {
  487. register int i = 0;
  488. while (isdigit(**s))
  489. i = i * 10 + *((*s)++) - '0';
  490. return i;
  491. }
  492. #define ZEROPAD (1 << 0) /* pad with zero */
  493. #define SIGN (1 << 1) /* unsigned/signed long */
  494. #define PLUS (1 << 2) /* show plus */
  495. #define SPACE (1 << 3) /* space if plus */
  496. #define LEFT (1 << 4) /* left justified */
  497. #define SPECIAL (1 << 5) /* 0x */
  498. #define LARGE (1 << 6) /* use 'ABCDEF' instead of 'abcdef' */
  499. #ifdef RT_PRINTF_PRECISION
  500. static char *print_number(char *buf,
  501. char *end,
  502. long num,
  503. int base,
  504. int s,
  505. int precision,
  506. int type)
  507. #else
  508. static char *print_number(char *buf,
  509. char *end,
  510. long num,
  511. int base,
  512. int s,
  513. int type)
  514. #endif
  515. {
  516. char c, sign;
  517. #ifdef RT_PRINTF_LONGLONG
  518. char tmp[32];
  519. #else
  520. char tmp[16];
  521. #endif
  522. const char *digits;
  523. static const char small_digits[] = "0123456789abcdef";
  524. static const char large_digits[] = "0123456789ABCDEF";
  525. register int i;
  526. register int size;
  527. size = s;
  528. digits = (type & LARGE) ? large_digits : small_digits;
  529. if (type & LEFT)
  530. type &= ~ZEROPAD;
  531. c = (type & ZEROPAD) ? '0' : ' ';
  532. /* get sign */
  533. sign = 0;
  534. if (type & SIGN)
  535. {
  536. if (num < 0)
  537. {
  538. sign = '-';
  539. num = -num;
  540. }
  541. else if (type & PLUS)
  542. sign = '+';
  543. else if (type & SPACE)
  544. sign = ' ';
  545. }
  546. #ifdef RT_PRINTF_SPECIAL
  547. if (type & SPECIAL)
  548. {
  549. if (base == 16)
  550. size -= 2;
  551. else if (base == 8)
  552. size--;
  553. }
  554. #endif
  555. i = 0;
  556. if (num == 0)
  557. tmp[i++] = '0';
  558. else
  559. {
  560. while (num != 0)
  561. tmp[i++] = digits[divide(&num, base)];
  562. }
  563. #ifdef RT_PRINTF_PRECISION
  564. if (i > precision)
  565. precision = i;
  566. size -= precision;
  567. #else
  568. size -= i;
  569. #endif
  570. if (!(type & (ZEROPAD | LEFT)))
  571. {
  572. if ((sign) && (size > 0))
  573. size--;
  574. while (size-- > 0)
  575. {
  576. if (buf <= end)
  577. *buf = ' ';
  578. ++ buf;
  579. }
  580. }
  581. if (sign)
  582. {
  583. if (buf <= end)
  584. {
  585. *buf = sign;
  586. -- size;
  587. }
  588. ++ buf;
  589. }
  590. #ifdef RT_PRINTF_SPECIAL
  591. if (type & SPECIAL)
  592. {
  593. if (base == 8)
  594. {
  595. if (buf <= end)
  596. *buf = '0';
  597. ++ buf;
  598. }
  599. else if (base == 16)
  600. {
  601. if (buf <= end)
  602. *buf = '0';
  603. ++ buf;
  604. if (buf <= end)
  605. {
  606. *buf = type & LARGE ? 'X' : 'x';
  607. }
  608. ++ buf;
  609. }
  610. }
  611. #endif
  612. /* no align to the left */
  613. if (!(type & LEFT))
  614. {
  615. while (size-- > 0)
  616. {
  617. if (buf <= end)
  618. *buf = c;
  619. ++ buf;
  620. }
  621. }
  622. #ifdef RT_PRINTF_PRECISION
  623. while (i < precision--)
  624. {
  625. if (buf <= end)
  626. *buf = '0';
  627. ++ buf;
  628. }
  629. #endif
  630. /* put number in the temporary buffer */
  631. while (i-- > 0)
  632. {
  633. if (buf <= end)
  634. *buf = tmp[i];
  635. ++ buf;
  636. }
  637. while (size-- > 0)
  638. {
  639. if (buf <= end)
  640. *buf = ' ';
  641. ++ buf;
  642. }
  643. return buf;
  644. }
  645. rt_int32_t rt_vsnprintf(char *buf,
  646. rt_size_t size,
  647. const char *fmt,
  648. va_list args)
  649. {
  650. #ifdef RT_PRINTF_LONGLONG
  651. unsigned long long num;
  652. #else
  653. rt_uint32_t num;
  654. #endif
  655. int i, len;
  656. char *str, *end, c;
  657. const char *s;
  658. rt_uint8_t base; /* the base of number */
  659. rt_uint8_t flags; /* flags to print number */
  660. rt_uint8_t qualifier; /* 'h', 'l', or 'L' for integer fields */
  661. rt_int32_t field_width; /* width of output field */
  662. #ifdef RT_PRINTF_PRECISION
  663. int precision; /* min. # of digits for integers and max for a string */
  664. #endif
  665. str = buf;
  666. end = buf + size - 1;
  667. /* Make sure end is always >= buf */
  668. if (end < buf)
  669. {
  670. end = ((char *) - 1);
  671. size = end - buf;
  672. }
  673. for (; *fmt ; ++fmt)
  674. {
  675. if (*fmt != '%')
  676. {
  677. if (str <= end)
  678. *str = *fmt;
  679. ++ str;
  680. continue;
  681. }
  682. /* process flags */
  683. flags = 0;
  684. while (1)
  685. {
  686. /* skips the first '%' also */
  687. ++ fmt;
  688. if (*fmt == '-') flags |= LEFT;
  689. else if (*fmt == '+') flags |= PLUS;
  690. else if (*fmt == ' ') flags |= SPACE;
  691. else if (*fmt == '#') flags |= SPECIAL;
  692. else if (*fmt == '0') flags |= ZEROPAD;
  693. else break;
  694. }
  695. /* get field width */
  696. field_width = -1;
  697. if (isdigit(*fmt)) field_width = skip_atoi(&fmt);
  698. else if (*fmt == '*')
  699. {
  700. ++ fmt;
  701. /* it's the next argument */
  702. field_width = va_arg(args, int);
  703. if (field_width < 0)
  704. {
  705. field_width = -field_width;
  706. flags |= LEFT;
  707. }
  708. }
  709. #ifdef RT_PRINTF_PRECISION
  710. /* get the precision */
  711. precision = -1;
  712. if (*fmt == '.')
  713. {
  714. ++ fmt;
  715. if (isdigit(*fmt)) precision = skip_atoi(&fmt);
  716. else if (*fmt == '*')
  717. {
  718. ++ fmt;
  719. /* it's the next argument */
  720. precision = va_arg(args, int);
  721. }
  722. if (precision < 0) precision = 0;
  723. }
  724. #endif
  725. /* get the conversion qualifier */
  726. qualifier = 0;
  727. #ifdef RT_PRINTF_LONGLONG
  728. if (*fmt == 'h' || *fmt == 'l' || *fmt == 'L')
  729. #else
  730. if (*fmt == 'h' || *fmt == 'l')
  731. #endif
  732. {
  733. qualifier = *fmt;
  734. ++ fmt;
  735. #ifdef RT_PRINTF_LONGLONG
  736. if (qualifier == 'l' && *fmt == 'l')
  737. {
  738. qualifier = 'L';
  739. ++ fmt;
  740. }
  741. #endif
  742. }
  743. /* the default base */
  744. base = 10;
  745. switch (*fmt)
  746. {
  747. case 'c':
  748. if (!(flags & LEFT))
  749. {
  750. while (--field_width > 0)
  751. {
  752. if (str <= end) *str = ' ';
  753. ++ str;
  754. }
  755. }
  756. /* get character */
  757. c = (rt_uint8_t)va_arg(args, int);
  758. if (str <= end) *str = c;
  759. ++ str;
  760. /* put width */
  761. while (--field_width > 0)
  762. {
  763. if (str <= end) *str = ' ';
  764. ++ str;
  765. }
  766. continue;
  767. case 's':
  768. s = va_arg(args, char *);
  769. if (!s) s = "(NULL)";
  770. len = rt_strlen(s);
  771. #ifdef RT_PRINTF_PRECISION
  772. if (precision > 0 && len > precision) len = precision;
  773. #endif
  774. if (!(flags & LEFT))
  775. {
  776. while (len < field_width--)
  777. {
  778. if (str <= end) *str = ' ';
  779. ++ str;
  780. }
  781. }
  782. for (i = 0; i < len; ++i)
  783. {
  784. if (str <= end) *str = *s;
  785. ++ str;
  786. ++ s;
  787. }
  788. while (len < field_width--)
  789. {
  790. if (str <= end) *str = ' ';
  791. ++ str;
  792. }
  793. continue;
  794. case 'p':
  795. if (field_width == -1)
  796. {
  797. field_width = sizeof(void *) << 1;
  798. flags |= ZEROPAD;
  799. }
  800. #ifdef RT_PRINTF_PRECISION
  801. str = print_number(str, end,
  802. (long)va_arg(args, void *),
  803. 16, field_width, precision, flags);
  804. #else
  805. str = print_number(str, end,
  806. (long)va_arg(args, void *),
  807. 16, field_width, flags);
  808. #endif
  809. continue;
  810. case '%':
  811. if (str <= end) *str = '%';
  812. ++ str;
  813. continue;
  814. /* integer number formats - set up the flags and "break" */
  815. case 'o':
  816. base = 8;
  817. break;
  818. case 'X':
  819. flags |= LARGE;
  820. case 'x':
  821. base = 16;
  822. break;
  823. case 'd':
  824. case 'i':
  825. flags |= SIGN;
  826. case 'u':
  827. break;
  828. default:
  829. if (str <= end) *str = '%';
  830. ++ str;
  831. if (*fmt)
  832. {
  833. if (str <= end) *str = *fmt;
  834. ++ str;
  835. }
  836. else
  837. {
  838. -- fmt;
  839. }
  840. continue;
  841. }
  842. #ifdef RT_PRINTF_LONGLONG
  843. if (qualifier == 'L') num = va_arg(args, long long);
  844. else if (qualifier == 'l')
  845. #else
  846. if (qualifier == 'l')
  847. #endif
  848. {
  849. num = va_arg(args, rt_uint32_t);
  850. if (flags & SIGN) num = (rt_int32_t)num;
  851. }
  852. else if (qualifier == 'h')
  853. {
  854. num = (rt_uint16_t)va_arg(args, rt_int32_t);
  855. if (flags & SIGN) num = (rt_int16_t)num;
  856. }
  857. else
  858. {
  859. num = va_arg(args, rt_uint32_t);
  860. if (flags & SIGN) num = (rt_int32_t)num;
  861. }
  862. #ifdef RT_PRINTF_PRECISION
  863. str = print_number(str, end, num, base, field_width, precision, flags);
  864. #else
  865. str = print_number(str, end, num, base, field_width, flags);
  866. #endif
  867. }
  868. if (str <= end) *str = '\0';
  869. else *end = '\0';
  870. /* the trailing null byte doesn't count towards the total
  871. * ++str;
  872. */
  873. return str - buf;
  874. }
  875. RTM_EXPORT(rt_vsnprintf);
  876. /**
  877. * This function will fill a formatted string to buffer
  878. *
  879. * @param buf the buffer to save formatted string
  880. * @param size the size of buffer
  881. * @param fmt the format
  882. */
  883. rt_int32_t rt_snprintf(char *buf, rt_size_t size, const char *fmt, ...)
  884. {
  885. rt_int32_t n;
  886. va_list args;
  887. va_start(args, fmt);
  888. n = rt_vsnprintf(buf, size, fmt, args);
  889. va_end(args);
  890. return n;
  891. }
  892. RTM_EXPORT(rt_snprintf);
  893. /**
  894. * This function will fill a formatted string to buffer
  895. *
  896. * @param buf the buffer to save formatted string
  897. * @param arg_ptr the arg_ptr
  898. * @param format the format
  899. */
  900. rt_int32_t rt_vsprintf(char *buf, const char *format, va_list arg_ptr)
  901. {
  902. return rt_vsnprintf(buf, (rt_size_t) - 1, format, arg_ptr);
  903. }
  904. RTM_EXPORT(rt_vsprintf);
  905. /**
  906. * This function will fill a formatted string to buffer
  907. *
  908. * @param buf the buffer to save formatted string
  909. * @param format the format
  910. */
  911. rt_int32_t rt_sprintf(char *buf, const char *format, ...)
  912. {
  913. rt_int32_t n;
  914. va_list arg_ptr;
  915. va_start(arg_ptr, format);
  916. n = rt_vsprintf(buf, format, arg_ptr);
  917. va_end(arg_ptr);
  918. return n;
  919. }
  920. RTM_EXPORT(rt_sprintf);
  921. #ifdef RT_USING_CONSOLE
  922. #ifdef RT_USING_DEVICE
  923. /**
  924. * This function returns the device using in console.
  925. *
  926. * @return the device using in console or RT_NULL
  927. */
  928. rt_device_t rt_console_get_device(void)
  929. {
  930. return _console_device;
  931. }
  932. RTM_EXPORT(rt_console_get_device);
  933. /**
  934. * This function will set a device as console device.
  935. * After set a device to console, all output of rt_kprintf will be
  936. * redirected to this new device.
  937. *
  938. * @param name the name of new console device
  939. *
  940. * @return the old console device handler
  941. */
  942. rt_device_t rt_console_set_device(const char *name)
  943. {
  944. rt_device_t new, old;
  945. /* save old device */
  946. old = _console_device;
  947. /* find new console device */
  948. new = rt_device_find(name);
  949. if (new != RT_NULL)
  950. {
  951. if (_console_device != RT_NULL)
  952. {
  953. /* close old console device */
  954. rt_device_close(_console_device);
  955. }
  956. /* set new console device */
  957. rt_device_open(new, RT_DEVICE_OFLAG_RDWR | RT_DEVICE_FLAG_STREAM);
  958. _console_device = new;
  959. }
  960. return old;
  961. }
  962. RTM_EXPORT(rt_console_set_device);
  963. #endif
  964. RT_WEAK void rt_hw_console_output(const char *str)
  965. {
  966. /* empty console output */
  967. }
  968. RTM_EXPORT(rt_hw_console_output);
  969. /**
  970. * This function will put string to the console.
  971. *
  972. * @param str the string output to the console.
  973. */
  974. void rt_kputs(const char *str)
  975. {
  976. if (!str) return;
  977. #ifdef RT_USING_DEVICE
  978. if (_console_device == RT_NULL)
  979. {
  980. rt_hw_console_output(str);
  981. }
  982. else
  983. {
  984. rt_uint16_t old_flag = _console_device->open_flag;
  985. _console_device->open_flag |= RT_DEVICE_FLAG_STREAM;
  986. rt_device_write(_console_device, 0, str, rt_strlen(str));
  987. _console_device->open_flag = old_flag;
  988. }
  989. #else
  990. rt_hw_console_output(str);
  991. #endif
  992. }
  993. /**
  994. * This function will print a formatted string on system console
  995. *
  996. * @param fmt the format
  997. */
  998. void rt_kprintf(const char *fmt, ...)
  999. {
  1000. va_list args;
  1001. rt_size_t length;
  1002. static char rt_log_buf[RT_CONSOLEBUF_SIZE];
  1003. va_start(args, fmt);
  1004. /* the return value of vsnprintf is the number of bytes that would be
  1005. * written to buffer had if the size of the buffer been sufficiently
  1006. * large excluding the terminating null byte. If the output string
  1007. * would be larger than the rt_log_buf, we have to adjust the output
  1008. * length. */
  1009. length = rt_vsnprintf(rt_log_buf, sizeof(rt_log_buf) - 1, fmt, args);
  1010. if (length > RT_CONSOLEBUF_SIZE - 1)
  1011. length = RT_CONSOLEBUF_SIZE - 1;
  1012. #ifdef RT_USING_DEVICE
  1013. if (_console_device == RT_NULL)
  1014. {
  1015. rt_hw_console_output(rt_log_buf);
  1016. }
  1017. else
  1018. {
  1019. rt_uint16_t old_flag = _console_device->open_flag;
  1020. _console_device->open_flag |= RT_DEVICE_FLAG_STREAM;
  1021. rt_device_write(_console_device, 0, rt_log_buf, length);
  1022. _console_device->open_flag = old_flag;
  1023. }
  1024. #else
  1025. rt_hw_console_output(rt_log_buf);
  1026. #endif
  1027. va_end(args);
  1028. }
  1029. RTM_EXPORT(rt_kprintf);
  1030. #endif
  1031. #ifdef RT_USING_HEAP
  1032. /**
  1033. * This function allocates a memory block, which address is aligned to the
  1034. * specified alignment size.
  1035. *
  1036. * @param size the allocated memory block size
  1037. * @param align the alignment size
  1038. *
  1039. * @return the allocated memory block on successful, otherwise returns RT_NULL
  1040. */
  1041. void *rt_malloc_align(rt_size_t size, rt_size_t align)
  1042. {
  1043. void *ptr;
  1044. void *align_ptr;
  1045. int uintptr_size;
  1046. rt_size_t align_size;
  1047. /* sizeof pointer */
  1048. uintptr_size = sizeof(void*);
  1049. uintptr_size -= 1;
  1050. /* align the alignment size to uintptr size byte */
  1051. align = ((align + uintptr_size) & ~uintptr_size);
  1052. /* get total aligned size */
  1053. align_size = ((size + uintptr_size) & ~uintptr_size) + align;
  1054. /* allocate memory block from heap */
  1055. ptr = rt_malloc(align_size);
  1056. if (ptr != RT_NULL)
  1057. {
  1058. /* the allocated memory block is aligned */
  1059. if (((rt_ubase_t)ptr & (align - 1)) == 0)
  1060. {
  1061. align_ptr = (void *)((rt_ubase_t)ptr + align);
  1062. }
  1063. else
  1064. {
  1065. align_ptr = (void *)(((rt_ubase_t)ptr + (align - 1)) & ~(align - 1));
  1066. }
  1067. /* set the pointer before alignment pointer to the real pointer */
  1068. *((rt_ubase_t *)((rt_ubase_t)align_ptr - sizeof(void *))) = (rt_ubase_t)ptr;
  1069. ptr = align_ptr;
  1070. }
  1071. return ptr;
  1072. }
  1073. RTM_EXPORT(rt_malloc_align);
  1074. /**
  1075. * This function release the memory block, which is allocated by
  1076. * rt_malloc_align function and address is aligned.
  1077. *
  1078. * @param ptr the memory block pointer
  1079. */
  1080. void rt_free_align(void *ptr)
  1081. {
  1082. void *real_ptr;
  1083. real_ptr = (void *) * (rt_ubase_t *)((rt_ubase_t)ptr - sizeof(void *));
  1084. rt_free(real_ptr);
  1085. }
  1086. RTM_EXPORT(rt_free_align);
  1087. #endif
  1088. #ifndef RT_USING_CPU_FFS
  1089. const rt_uint8_t __lowest_bit_bitmap[] =
  1090. {
  1091. /* 00 */ 0, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0,
  1092. /* 10 */ 4, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0,
  1093. /* 20 */ 5, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0,
  1094. /* 30 */ 4, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0,
  1095. /* 40 */ 6, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0,
  1096. /* 50 */ 4, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0,
  1097. /* 60 */ 5, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0,
  1098. /* 70 */ 4, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0,
  1099. /* 80 */ 7, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0,
  1100. /* 90 */ 4, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0,
  1101. /* A0 */ 5, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0,
  1102. /* B0 */ 4, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0,
  1103. /* C0 */ 6, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0,
  1104. /* D0 */ 4, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0,
  1105. /* E0 */ 5, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0,
  1106. /* F0 */ 4, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0
  1107. };
  1108. /**
  1109. * This function finds the first bit set (beginning with the least significant bit)
  1110. * in value and return the index of that bit.
  1111. *
  1112. * Bits are numbered starting at 1 (the least significant bit). A return value of
  1113. * zero from any of these functions means that the argument was zero.
  1114. *
  1115. * @return return the index of the first bit set. If value is 0, then this function
  1116. * shall return 0.
  1117. */
  1118. int __rt_ffs(int value)
  1119. {
  1120. if (value == 0) return 0;
  1121. if (value & 0xff)
  1122. return __lowest_bit_bitmap[value & 0xff] + 1;
  1123. if (value & 0xff00)
  1124. return __lowest_bit_bitmap[(value & 0xff00) >> 8] + 9;
  1125. if (value & 0xff0000)
  1126. return __lowest_bit_bitmap[(value & 0xff0000) >> 16] + 17;
  1127. return __lowest_bit_bitmap[(value & 0xff000000) >> 24] + 25;
  1128. }
  1129. #endif
  1130. #ifdef RT_DEBUG
  1131. /* RT_ASSERT(EX)'s hook */
  1132. void (*rt_assert_hook)(const char *ex, const char *func, rt_size_t line);
  1133. /**
  1134. * This function will set a hook function to RT_ASSERT(EX). It will run when the expression is false.
  1135. *
  1136. * @param hook the hook function
  1137. */
  1138. void rt_assert_set_hook(void (*hook)(const char *ex, const char *func, rt_size_t line))
  1139. {
  1140. rt_assert_hook = hook;
  1141. }
  1142. /**
  1143. * The RT_ASSERT function.
  1144. *
  1145. * @param ex the assertion condition string
  1146. * @param func the function name when assertion.
  1147. * @param line the file line number when assertion.
  1148. */
  1149. void rt_assert_handler(const char *ex_string, const char *func, rt_size_t line)
  1150. {
  1151. volatile char dummy = 0;
  1152. if (rt_assert_hook == RT_NULL)
  1153. {
  1154. #ifdef RT_USING_MODULE
  1155. if (dlmodule_self())
  1156. {
  1157. /* close assertion module */
  1158. dlmodule_exit(-1);
  1159. }
  1160. else
  1161. #endif
  1162. {
  1163. rt_kprintf("(%s) assertion failed at function:%s, line number:%d \n", ex_string, func, line);
  1164. while (dummy == 0);
  1165. }
  1166. }
  1167. else
  1168. {
  1169. rt_assert_hook(ex_string, func, line);
  1170. }
  1171. }
  1172. RTM_EXPORT(rt_assert_handler);
  1173. #endif /* RT_DEBUG */
  1174. #if !defined (RT_USING_NEWLIB) && defined (RT_USING_MINILIBC) && defined (__GNUC__)
  1175. #include <sys/types.h>
  1176. void *memcpy(void *dest, const void *src, size_t n) __attribute__((weak, alias("rt_memcpy")));
  1177. void *memset(void *s, int c, size_t n) __attribute__((weak, alias("rt_memset")));
  1178. void *memmove(void *dest, const void *src, size_t n) __attribute__((weak, alias("rt_memmove")));
  1179. int memcmp(const void *s1, const void *s2, size_t n) __attribute__((weak, alias("rt_memcmp")));
  1180. size_t strlen(const char *s) __attribute__((weak, alias("rt_strlen")));
  1181. char *strstr(const char *s1, const char *s2) __attribute__((weak, alias("rt_strstr")));
  1182. int strcasecmp(const char *a, const char *b) __attribute__((weak, alias("rt_strcasecmp")));
  1183. char *strncpy(char *dest, const char *src, size_t n) __attribute__((weak, alias("rt_strncpy")));
  1184. int strncmp(const char *cs, const char *ct, size_t count) __attribute__((weak, alias("rt_strncmp")));
  1185. #ifdef RT_USING_HEAP
  1186. char *strdup(const char *s) __attribute__((weak, alias("rt_strdup")));
  1187. #endif
  1188. int sprintf(char *buf, const char *format, ...) __attribute__((weak, alias("rt_sprintf")));
  1189. int snprintf(char *buf, rt_size_t size, const char *fmt, ...) __attribute__((weak, alias("rt_snprintf")));
  1190. int vsprintf(char *buf, const char *format, va_list arg_ptr) __attribute__((weak, alias("rt_vsprintf")));
  1191. #endif
  1192. /**@}*/