1
2typedef unsigned long va_list;
3
4#define ACC 4
5#define __read(source) \
6({ va_list __res; \
7 __asm__ __volatile__( \
8 "move\t%0, " #source "\n\t" \
9 : "=r" (__res)); \
10 __res; \
11})
12
13enum format_type {
14 FORMAT_TYPE_NONE,
15 FORMAT_TYPE_HEX,
16 FORMAT_TYPE_ULONG,
17 FORMAT_TYPE_FLOAT
18};
19
20struct printf_spec {
21 char type;
22};
23
24static int format_decode(char *fmt, struct printf_spec *spec)
25{
26 char *start = fmt;
27
28 for (; *fmt ; ++fmt) {
29 if (*fmt == '%') {
30 break;
31 }
32 }
33
34 switch (*++fmt) {
35 case 'x':
36 spec->type = FORMAT_TYPE_HEX;
37 break;
38
39 case 'd':
40 spec->type = FORMAT_TYPE_ULONG;
41 break;
42
43 case 'f':
44 spec->type = FORMAT_TYPE_FLOAT;
45 break;
46
47 default:
48 spec->type = FORMAT_TYPE_NONE;
49 }
50
51 return ++fmt - start;
52}
53
54void *memcpy(void *dest, void *src, int n)
55{
56 int i;
57 char *s = src;
58 char *d = dest;
59
60 for (i = 0; i < n; i++) {
61 d[i] = s[i];
62 }
63 return dest;
64}
65
66char *number(char *buf, va_list num)
67{
68 int i;
69 char *str = buf;
70 static char digits[16] = "0123456789abcdef";
71 str = str + sizeof(num) * 2;
72
73 for (i = 0; i < sizeof(num) * 2; i++) {
74 *--str = digits[num & 15];
75 num >>= 4;
76 }
77
78 return buf + sizeof(num) * 2;
79}
80
81char *__number(char *buf, va_list num)
82{
83 int i;
84 va_list mm = num;
85 char *str = buf;
86
87 if (!num) {
88 *str++ = '0';
89 return str;
90 }
91
92 for (i = 0; mm; mm = mm/10, i++) {
93
94 }
95
96 str = str + i;
97
98 while (num) {
99 *--str = num % 10 + 48;
100 num = num / 10;
101 }
102
103 return str + i;
104}
105
106va_list modf(va_list args, va_list *integer, va_list *num)
107{
108 int i;
109 double dot_v = 0;
110 va_list E, DOT, DOT_V;
111
112 if (!args) {
113 return 0;
114 }
115
116 for (i = 0, args = args << 1 >> 1; i < 52; i++) {
117 if ((args >> i) & 0x1) {
118 break;
119 }
120 }
121
122 *integer = 0;
123
124 if ((args >> 56 != 0x3f) || (args >> 52 == 0x3ff)) {
125 E = (args >> 52) - 1023;
126 DOT = 52 - E - i;
127 DOT_V = args << (12 + E) >> (12 + E) >> i;
128 *integer = ((args << 12 >> 12) >> (i + DOT)) | (1 << E);
129 } else {
130 E = ~((args >> 52) - 1023) + 1;
131 DOT_V = args << 12 >> 12;
132
133 dot_v += 1.0 / (1 << E);
134
135 for (i = 1; i <= 16; i++) {
136 if ((DOT_V >> (52 - i)) & 0x1) {
137 dot_v += 1.0 / (1 << E + i);
138 }
139 }
140
141 for (i = 1, E = 0; i <= ACC; i++) {
142 dot_v *= 10;
143 if (!(va_list)dot_v) {
144 E++;
145 }
146 }
147
148 *num = E;
149
150 return dot_v;
151 }
152
153 if (args & 0xf) {
154 for (i = 1; i <= 16; i++) {
155 if ((DOT_V >> (DOT - i)) & 0x1) {
156 dot_v += 1.0 / (1 << i);
157 }
158 }
159
160 for (i = 1, E = 0; i <= ACC; i++) {
161 dot_v *= 10;
162 if (!(va_list)dot_v) {
163 E++;
164 }
165 }
166
167 *num = E;
168
169 return dot_v;
170 } else if (DOT) {
171 for (i = 1; i <= DOT; i++) {
172 if ((DOT_V >> (DOT - i)) & 0x1) {
173 dot_v += 1.0 / (1 << i);
174 }
175 }
176
177 for (i = 1; i <= ACC; i++) {
178 dot_v = dot_v * 10;
179 }
180
181 return dot_v;
182 }
183
184 return 0;
185}
186
187int vsnprintf(char *buf, int size, char *fmt, va_list args)
188{
189 char *str, *mm;
190 struct printf_spec spec = {0};
191
192 str = mm = buf;
193
194 while (*fmt) {
195 char *old_fmt = fmt;
196 int read = format_decode(fmt, &spec);
197
198 fmt += read;
199
200 switch (spec.type) {
201 case FORMAT_TYPE_NONE: {
202 memcpy(str, old_fmt, read);
203 str += read;
204 break;
205 }
206 case FORMAT_TYPE_HEX: {
207 memcpy(str, old_fmt, read);
208 str = number(str + read, args);
209 for (; *mm ; ++mm) {
210 if (*mm == '%') {
211 *mm = '0';
212 break;
213 }
214 }
215 break;
216 }
217 case FORMAT_TYPE_ULONG: {
218 memcpy(str, old_fmt, read - 2);
219 str = __number(str + read - 2, args);
220 break;
221 }
222 case FORMAT_TYPE_FLOAT: {
223 va_list integer, dot_v, num;
224 dot_v = modf(args, &integer, &num);
225 memcpy(str, old_fmt, read - 2);
226 str += read - 2;
227 if ((args >> 63 & 0x1)) {
228 *str++ = '-';
229 }
230 str = __number(str, integer);
231 if (dot_v) {
232 *str++ = '.';
233 while (num--) {
234 *str++ = '0';
235 }
236 str = __number(str, dot_v);
237 }
238 break;
239 }
240 }
241 }
242 *str = '\0';
243
244 return str - buf;
245}
246
247static void serial_out(char *str)
248{
249 while (*str) {
250 *(char *)0xffffffffb80003f8 = *str++;
251 }
252}
253
254int vprintf(char *fmt, va_list args)
255{
256 int printed_len = 0;
257 static char printf_buf[512];
258 printed_len = vsnprintf(printf_buf, sizeof(printf_buf), fmt, args);
259 serial_out(printf_buf);
260 return printed_len;
261}
262
263int printf(char *fmt, ...)
264{
265 return vprintf(fmt, __read($5));
266}
267
