-
Notifications
You must be signed in to change notification settings - Fork 0
/
ml_print.c
128 lines (118 loc) · 2.1 KB
/
ml_print.c
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
#include "stdarg.h"
#include "ml_print.h"
#include "lcd.h"
void ml_printf_impl(char *str, va_list ap);
static char buf[12];
void ml_printNatural(uint16_t num, uint16_t base)
{
/* Add the prefix for octal and hex; exit if not supported base */
switch(base)
{
case 2:
case 10:
break;
case 8:
display_char('0');
break;
case 16:
display_char('0');
display_char('x');
break;
default:
return;
}
char const digits[] = "0123456789ABCDEF";
/* We're using a 16b int, so we know that num < 65536 */
uint32_t div_max = base;
while(div_max * base < 65536)
{
div_max = div_max * base;
}
uint16_t i = (uint16_t) div_max;
/* Remove leading 0s */
while(i != 1 && (num / i) % base == 0)
{
i = i / base;
}
/* Write the number */
while(i != 1)
{
display_char(digits[(num / i) % base]);
i = i / base;
}
display_char(digits[num % base]);
}
void ml_printf(char *str, ...)
{
va_list ap;
va_start(ap,str);
ml_printf_impl(str, ap);
va_end(ap);
}
void ml_printf_at(char *str, uint16_t x, uint16_t y, ...)
{
set_lcd_position(x, y);
va_list ap;
va_start(ap,y);
ml_printf_impl(str, ap);
va_end(ap);
}
void ml_printf_impl(char *str, va_list ap)
{
char c;
while((c = *(str++)))
{
if(c != '%')
display_char(c);
else
{
uint16_t num;
c = *(str++);
char sc;
char *s = buf;
switch(c)
{
case 0:
break;
case 'u':
case 'd':
num = va_arg(ap, uint16_t);
if(c == 'd' && (int16_t)num < 0)
{
num = -(int16_t)num;
display_char('-');
}
ml_printNatural(num, 10);
break;
case 'x':
num = va_arg(ap, uint16_t);
ml_printNatural(num, 16);
break;
case 'b':
num = va_arg(ap, uint16_t);
ml_printNatural(num, 2);
break;
case 'o':
num = va_arg(ap, uint16_t);
ml_printNatural(num, 8);
break;
case 'c':
display_char((char) va_arg(ap, int));
break;
case 's':
s = va_arg(ap, char*);
while((sc = *(s++)))
{
display_char(sc);
}
break;
case '%':
display_char('%');
break;
default:
break;
}
}
}
display_char('\n');
}