mprintf.c 3.4 KB
Newer Older
1 2 3 4 5
#include <stdio.h>
#include <stdarg.h>
#include <stdlib.h>

#include "uart.h"
6
#include "util.h"
7

8
int vprintf(char const *format, va_list ap)
9
{
10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29
	unsigned char scratch[16];
	unsigned char format_flag;
	unsigned int u_val = 0;
	unsigned char base;
	unsigned char *ptr;
	unsigned char width = 0;
	unsigned char fill;

	while (1) {

		width = 0;
		fill = ' ';
		while ((format_flag = *format++) != '%') {
			if (!format_flag) {
				va_end(ap);
				return (0);
			}

			uart_write_byte(format_flag);
		}
30

31 32 33 34 35 36 37 38 39 40 41 42 43 44
		// check for zero pad
		format_flag = *format - '0';
		if (format_flag == 0)	// zero pad
		{
			fill = '0';
			format++;
		}
		// check for width spec
		format_flag = *format - '0';
		if (format_flag > 0 && format_flag <= 9)	// width set
		{
			width = format_flag;
			format++;
		}
45

46
		switch (format_flag = *format++) {
47
		case 'c':
48
			format_flag = va_arg(ap, int);
49

50
			//fall through
51

52
		default:
53
			uart_write_byte(format_flag);
54

55
			continue;
56

57 58
		case 'S':
		case 's':
59 60 61 62
			ptr = (unsigned char *)va_arg(ap, char *);
			while (*ptr)
				uart_write_byte(*ptr++);
			continue;
63 64 65

		case 'd':

66 67
			base = 10;
			goto CONVERSION_LOOP;
68 69

		case 'u':
70 71
			base = 10;
			goto CONVERSION_LOOP;
72

73
		case 'x':
74
			base = 16;
75 76 77

CONVERSION_LOOP:

78 79 80 81 82
			u_val = va_arg(ap, unsigned int);
			if ((format_flag == 'd') && (u_val & 0x80000000)) {
				uart_write_byte('-');
				u_val = -u_val;
			}
83

84
			ptr = scratch + 16;
85

86
			*--ptr = 0;
87

88 89 90 91
			do {
				char ch = (u_val % base) + '0';
				if (ch > '9')
					ch += 'a' - '9' - 1;
92

93
				*--ptr = ch;
94

95
				u_val /= base;
96

97 98
				if (width)
					width--;
99

100
			} while (u_val > 0);
101

102 103
			while (width--)
				*--ptr = fill;
104

105 106
			while (*ptr)
				uart_write_byte(*ptr++);
107

108 109 110
		}
	}
	return 0;
111 112
}

113
static int _p_vsprintf(char const *format, va_list ap, char *dst)
114
{
115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135
	unsigned char scratch[16];
	unsigned char format_flag;
	unsigned int u_val = 0;
	unsigned char base;
	unsigned char *ptr;
	unsigned char width = 0;
	unsigned char fill;

	while (1) {

		width = 0;
		fill = ' ';
		while ((format_flag = *format++) != '%') {
			if (!format_flag) {
				va_end(ap);
				*dst++ = 0;
				return (0);
			}

			*dst++ = format_flag;
		}
136

137 138 139 140 141 142 143 144 145 146 147 148 149 150
		// check for zero pad
		format_flag = *format - '0';
		if (format_flag == 0)	// zero pad
		{
			fill = '0';
			format++;
		}
		// check for width spec
		format_flag = *format - '0';
		if (format_flag > 0 && format_flag <= 9)	// width set
		{
			width = format_flag;
			format++;
		}
151

152
		switch (format_flag = *format++) {
153
		case 'c':
154
			format_flag = va_arg(ap, int);
155

156
			//fall through
157

158
		default:
159
			*dst++ = format_flag;
160

161
			continue;
162

163 164
		case 'S':
		case 's':
165 166 167 168
			ptr = (unsigned char *)va_arg(ap, char *);
			while (*ptr)
				*dst++ = *ptr++;
			continue;
169 170 171

		case 'd':
		case 'u':
172 173
			base = 10;
			goto CONVERSION_LOOP;
174

175
		case 'x':
176
			base = 16;
177 178 179

CONVERSION_LOOP:

180
			u_val = va_arg(ap, unsigned int);
181

182
			ptr = scratch + 16;
183

184
			*--ptr = 0;
185

186 187 188 189
			do {
				char ch = (u_val % base) + '0';
				if (ch > '9')
					ch += 'a' - '9' - 1;
190

191
				*--ptr = ch;
192

193
				u_val /= base;
194

195 196
				if (width)
					width--;
197

198
			} while (u_val > 0);
199

200 201
//        while (width--)
//                      *--ptr = fill;
202

203 204
			while (*ptr)
				*dst++ = *ptr++;
205

206 207 208 209
		}
	}
	*dst++ = 0;
	return 0;
210 211 212 213 214
}

int mprintf(char const *format, ...)
{
	int rval;
215 216 217 218 219
	va_list ap;
	va_start(ap, format);
	rval = vprintf(format, ap);
	va_end(ap);
	return rval;
220

221 222 223 224
}

int sprintf(char *dst, char const *format, ...)
{
225 226 227 228
	va_list ap;
	va_start(ap, format);
	int r = _p_vsprintf(format, ap, dst);
	return r;
229

230
}