--- /dev/null
+#include <corax/types.h>
+#include <corax/errno.h>
+#include <sys/types.h>
+#include <string.h>
+
+#define STATE_NORMAL 0
+#define STATE_FLAG 1
+#define STATE_WIDTH 2
+#define STATE_PREC 3
+#define STATE_LENGTH 4
+#define STATE_CONV 5
+#define STATE_ERR 6
+
+#define FLAG_POUND (1 << 0)
+#define FLAG_ZERO (1 << 1)
+#define FLAG_MINUS (1 << 2)
+#define FLAG_SPACE (1 << 3)
+#define FLAG_PLUS (1 << 4)
+#define FLAG_APOS (1 << 5)
+#define FLAG_LCHEX (1 << 6)
+#define FLAG_USIGN (1 << 7)
+
+static int _convert_oct(char *dst, int flags, int precision, int width, int padlen, void *data)
+{
+ union {
+ u64_t v64;
+ u32_t v32[2];
+ u16_t v16[4];
+ u8_t v8[8];
+ } val;
+
+ char str[22];
+ int ret_val;
+ int olen;
+ char pad;
+
+ ret_val = 0;
+ pad = flags & FLAG_ZERO ? '0' : ' ';
+ val.v64 = 0LL;
+ olen = 0;
+
+ switch(width) {
+ case 8:
+ val.v64 = *(u64_t*)data;
+ break;
+
+ default:
+ case 4:
+ val.v32[0] = *(u32_t*)data;
+ break;
+
+ case 2:
+ val.v16[0] = *(u16_t*)data;
+ break;
+
+ case 1:
+ val.v8[0] = *(u8_t*)data;
+ break;
+ }
+
+ memset(str, '0', sizeof(str));
+
+ while(val.v64) {
+ olen++;
+ str[sizeof(str) - olen] = (char)(val.v64 & 0x7) + '0';
+ val.v64 >>= 3;
+ }
+
+ /* prepend the unit symbol, if # was found in the conversion specification */
+ if(flags & FLAG_POUND) {
+ str[sizeof(str) - 1 - olen] = '0';
+ olen++;
+ }
+
+ /* padlen - olen bytes need to be padded */
+ padlen -= olen;
+
+ while(padlen > 0 && precision > 0) {
+ dst[ret_val++] = pad;
+ padlen--;
+ precision--;
+ }
+
+ memcpy(dst + ret_val, str + sizeof(str) - olen, olen);
+ ret_val += olen;
+
+ return(ret_val);
+}
+
+static int _convert_hex(char *dst, int flags, int precision, int width, int padlen, void *data)
+{
+ static const char hex_lc[] = "0123456789abcdef";
+ static const char hex_uc[] = "0123456789ABCDEF";
+ static const char hexp_lc[] = "0x";
+ static const char hexp_uc[] = "0X";
+
+ union {
+ u64_t v64;
+ u32_t v32[2];
+ u16_t v16[4];
+ u8_t v8[8];
+ } val;
+
+ const char *charset;
+ char str[16];
+ char pad;
+ int ret_val;
+ int len;
+ int i;
+
+ pad = flags & FLAG_ZERO ? '0' : ' ';
+ charset = flags & FLAG_LCHEX ? hex_lc : hex_uc;
+ ret_val = 0;
+ val.v64 = 0LL;
+
+ switch(width) {
+ case 8:
+ val.v64 = *(u64_t*)data;
+ break;
+
+ default:
+ case 4:
+ val.v32[0] = *(u32_t*)data;
+ break;
+
+ case 2:
+ val.v16[0] = *(u16_t*)data;
+ break;
+
+ case 1:
+ val.v8[0] = *(u8_t*)data;
+ break;
+ }
+
+ for(i = 0; i < 8; i++) {
+ str[(i * 2) + 0] = charset[(val.v8[7 - i] >> 4) & 0xf];
+ str[(i * 2) + 1] = charset[(val.v8[7 - i] >> 0) & 0xf];
+ }
+
+ if(flags & FLAG_POUND) {
+ /* we're supposed to print the unit prefix */
+
+ const char *prefix;
+
+ prefix = flags & FLAG_LCHEX ? hexp_lc : hexp_uc;
+
+ for(i = 0; i < strlen(prefix) && precision > 0; i++, precision--) {
+ dst[ret_val++] = prefix[i];
+ padlen--;
+ }
+ }
+
+ /* count number of leading zeroes */
+ for(i = 0; i < 16 && str[i] == '0'; i++);
+ len = 16 - i;
+
+ /* we're supposed to print (padlen - len) bytes of padding */
+ for(padlen -= len; padlen > 0 && precision > 0; padlen--, precision--) {
+ dst[ret_val++] = pad;
+ }
+
+ if(len > precision) {
+ len = precision;
+ }
+
+ memcpy(dst + ret_val, str + i, len);
+ ret_val += len;
+
+ return(ret_val);
+}
+
+static int _convert_int(char *dst, int flags, int precision, int width, int padlen, void *data)
+{
+ char rev[24];
+ i64_t val;
+ u64_t uval;
+ int ret_val;
+ char sign;
+ int rlen;
+ char pad;
+
+ ret_val = 0;
+ sign = 0;
+ rlen = 0;
+ pad = flags & FLAG_SPACE ? ' ' : '0';
+
+ if(flags & FLAG_USIGN) {
+ switch(width) {
+ case 8:
+ uval = *(u64_t*)data;
+ break;
+
+ default:
+ case 4:
+ uval = *(u32_t*)data;
+ break;
+
+ case 2:
+ uval = *(u16_t*)data;
+ break;
+
+ case 1:
+ uval = *(u8_t*)data;
+ break;
+ }
+
+ val = 0;
+ } else {
+ switch(width) {
+ case 8:
+ val = *(i64_t*)data;
+ break;
+
+ default:
+ case 4:
+ val = *(i32_t*)data;
+ break;
+
+ case 2:
+ val = *(i16_t*)data;
+ break;
+
+ case 1:
+ val = *(i8_t*)data;
+ break;
+ }
+
+ uval = val;
+
+ /* print sign only for signed integer conversion */
+ if(val < 0) {
+ uval = -val;
+ sign = '-';
+ } else if(flags & FLAG_PLUS) {
+ sign = flags & FLAG_SPACE ? ' ' : '+';
+ }
+
+ if(sign && precision > 0) {
+ dst[ret_val++] = sign;
+ precision--;
+ padlen--;
+ }
+ }
+
+ do {
+ rev[rlen++] = '0' + (uval % 10);
+ uval /= 10;
+ } while(uval > 0);
+
+ padlen -= rlen;
+
+ while(padlen-- > 0 && precision-- > 0) {
+ dst[ret_val++] = pad;
+ }
+
+ while(rlen > 0 && precision > 0) {
+ dst[ret_val++] = rev[--rlen];
+ precision--;
+ }
+
+ return(ret_val);
+}
+
+static int _convert_bin(char *dst, int flags, int precision, int width, int length, void *data)
+{
+ char str[64];
+ union {
+ u64_t v64;
+ u32_t v32[2];
+ u16_t v16[4];
+ u8_t v8[8];
+ } val;
+ u64_t mask;
+ int ret_val;
+ int i;
+ int blen;
+ char pad;
+
+ ret_val = 0;
+ val.v64 = 0LL;
+ pad = flags & FLAG_ZERO ? '0' : ' ';
+
+ switch(width) {
+ case 8:
+ val.v64 = *(u64_t*)data;
+ break;
+
+ default:
+ case 4:
+ val.v32[0] = *(u32_t*)data;
+ break;
+
+ case 2:
+ val.v16[0] = *(u16_t*)data;
+ break;
+
+ case 1:
+ val.v8[0] = *(u8_t*)data;
+ break;
+ }
+
+ for(mask = 0x8000000000000000LL, i = 0; mask; mask >>= 1, i++) {
+ str[i] = val.v64 & mask ? '1' : '0';
+ }
+
+ for(i = 0; i < 64 && str[i] == '0'; i++);
+ blen = 64 - i;
+
+ /* print the prefix, if desired */
+ if(flags & FLAG_POUND && precision > 0) {
+ dst[ret_val++] = 'b';
+ width--;
+ }
+
+ /* width - blen is the amount of padding we still need */
+ width -= blen;
+
+ while(width > 0 && precision > 0) {
+ dst[ret_val++] = pad;
+ width--;
+ precision--;
+ }
+
+ memcpy(dst + ret_val, str + i, blen);
+ ret_val += blen;
+
+ return(ret_val);
+}
+
+static int _convert_str(char *dst, int precision, int padlen, const char *src)
+{
+ static const char nilstr[] = "(null)";
+ int slen;
+ int ret_val;
+
+ ret_val = 0;
+
+ if(!src) {
+ src = nilstr;
+ }
+
+ /*
+ * We're supposed to write at least padlen bytes, and at most precision bytes.
+ * That means, `padlen - strlen(src)' is the number of spaces we have to insert
+ * in the front.
+ */
+
+ slen = strlen(src);
+ padlen -= slen;
+
+ while(precision-- > 0 && padlen-- > 0) {
+ dst[ret_val++] = ' ';
+ }
+
+ /* precision may still be larger than slen */
+ if(slen < precision) {
+ precision = slen;
+ }
+
+ /* copy the actual string, if there is space left */
+ if(precision > 0) {
+ memcpy(dst + ret_val, src, precision);
+ ret_val += precision;
+ }
+
+ return(ret_val);
+}
+
+int snprintf(char *str, size_t size, const char *format, ...)
+{
+ int offset;
+ int state;
+ int flags;
+ int width;
+ int precision;
+ int length;
+ const char *fmt;
+ void *arg;
+ char c;
+
+#define RESET() do { \
+ flags = 0; \
+ width = 0; \
+ precision = 0; \
+ length = 0; \
+ } while(0)
+#define OUTPUT(_c) do { \
+ if(offset < size) { \
+ str[offset++] = (_c); \
+ } else { \
+ goto gtfo; \
+ } \
+ } while(0)
+#define NEXT() fmt++
+#define NEXTARG() (arg + (length == 8 ? 8 : sizeof(arg)))
+#define ADD_LENGTH(_l,_c) do { \
+ switch(_c) { \
+ case 'l': \
+ (_l) = !(_l) ? 4 : ((_l) << 1); \
+ break; \
+ case 'h': \
+ (_l) = !(_l) ? 2 : ((_l) >> 1); \
+ break; \
+ default: \
+ break; \
+ } \
+ } while(0)
+#define ADD_WIDTH(_l,_c) (_l) = (_l) * 10 + ((_c) - '0')
+#define ADD_PRECISION ADD_WIDTH
+#define ADD_FLAG(_f,_c) do { \
+ switch(_c) { \
+ case '#': \
+ (_f) |= FLAG_POUND; \
+ break; \
+ case '0': \
+ (_f) |= FLAG_ZERO; \
+ break; \
+ case '-': \
+ (_f) |= FLAG_MINUS; \
+ break; \
+ case ' ': \
+ (_f) |= FLAG_SPACE | FLAG_PLUS; \
+ break; \
+ case '+': \
+ (_f) |= FLAG_PLUS; \
+ break; \
+ case '\'': \
+ (_f) |= FLAG_APOS; \
+ break; \
+ default: \
+ break; \
+ } \
+ } while(0)
+#define SET_STATE(_s) (state = (_s))
+
+ offset = 0;
+ state = STATE_NORMAL;
+ fmt = format;
+ arg = (void*)&format + sizeof(format);
+
+ while((c = *fmt)) {
+ switch(state) {
+ case STATE_NORMAL:
+ switch(c) {
+ case '%':
+ SET_STATE(STATE_FLAG);
+ RESET();
+ break;
+
+ default:
+ OUTPUT(c);
+ break;
+ }
+
+ NEXT();
+ break;
+
+ case STATE_FLAG:
+ switch(c) {
+ case '#':
+ case '0':
+ case '-':
+ case ' ':
+ case '+':
+ case '\'':
+ ADD_FLAG(flags, c);
+ NEXT();
+ break;
+
+ case '1':
+ case '2':
+ case '3':
+ case '4':
+ case '5':
+ case '6':
+ case '7':
+ case '8':
+ case '9':
+ SET_STATE(STATE_WIDTH);
+ /* parse again in STATE_WIDTH */
+ break;
+
+ case '.':
+ SET_STATE(STATE_PREC);
+ NEXT();
+ break;
+
+ case 'h':
+ case 'l':
+ SET_STATE(STATE_LENGTH);
+ /* parse again in STATE_LENGTH */
+ break;
+
+ case 'd':
+ case 'i':
+ case 'u':
+ case 'o':
+ case 'p':
+ case 'x':
+ case 'X':
+ case 'c':
+ case 's':
+ case 'b':
+ case '%':
+ /* parse again in STATE_CONV */
+ SET_STATE(STATE_CONV);
+ break;
+
+ default:
+ SET_STATE(STATE_NORMAL);
+ break;
+ }
+ break;
+
+ case STATE_WIDTH:
+ switch(c) {
+ case '0':
+ case '1':
+ case '2':
+ case '3':
+ case '4':
+ case '5':
+ case '6':
+ case '7':
+ case '8':
+ case '9':
+ ADD_WIDTH(width, c);
+ NEXT();
+ break;
+
+ case '.':
+ SET_STATE(STATE_PREC);
+ NEXT();
+ break;
+
+ case 'l':
+ case 'h':
+ SET_STATE(STATE_LENGTH);
+ break;
+
+ case 'd':
+ case 'i':
+ case 'u':
+ case 'o':
+ case 'p':
+ case 'x':
+ case 'X':
+ case 'c':
+ case 's':
+ case 'b':
+ case '%':
+ SET_STATE(STATE_CONV);
+ break;
+
+ default:
+ SET_STATE(STATE_ERR);
+ break;
+ }
+ break;
+
+ case STATE_PREC:
+ switch(c) {
+ case '0':
+ case '1':
+ case '2':
+ case '3':
+ case '4':
+ case '5':
+ case '6':
+ case '7':
+ case '8':
+ case '9':
+ ADD_PRECISION(precision, c);
+ NEXT();
+ break;
+
+ case 'h':
+ case 'l':
+ SET_STATE(STATE_LENGTH);
+ break;
+
+ case 'd':
+ case 'i':
+ case 'u':
+ case 'o':
+ case 'p':
+ case 'x':
+ case 'X':
+ case 'c':
+ case 's':
+ case 'b':
+ case '%':
+ SET_STATE(STATE_CONV);
+ break;
+
+ default:
+ SET_STATE(STATE_ERR);
+ break;
+ }
+ break;
+
+ case STATE_LENGTH:
+ switch(c) {
+ case 'h':
+ case 'l':
+ ADD_LENGTH(length, c);
+ NEXT();
+ break;
+
+ case 'd':
+ case 'i':
+ case 'u':
+ case 'o':
+ case 'p':
+ case 'x':
+ case 'X':
+ case 'c':
+ case 's':
+ case 'b':
+ case '%':
+ SET_STATE(STATE_CONV);
+ break;
+
+ default:
+ SET_STATE(STATE_ERR);
+ break;
+ }
+ break;
+
+ case STATE_CONV:
+ switch(c) {
+ case 'u':
+ flags |= FLAG_USIGN;
+ /* fall-through */
+ case 'd':
+ case 'i':
+ if(!precision || (size - offset) < precision) {
+ precision = size - offset;
+ }
+
+ offset += _convert_int(str + offset, flags, precision,
+ length, width, arg);
+ arg = NEXTARG();
+ break;
+
+ case 'o':
+ if(!precision || (size - offset) < precision) {
+ precision = size - offset;
+ }
+
+ offset += _convert_oct(str + offset, flags, precision,
+ length, width, arg);
+ arg = NEXTARG();
+ break;
+
+ case 'x':
+ flags |= FLAG_LCHEX;
+ /* fall through */
+ case 'X':
+ if(!precision || (size - offset) < precision) {
+ precision = size - offset;
+ }
+
+ offset += _convert_hex(str + offset, flags, precision,
+ length, width, arg);
+ arg = NEXTARG();
+ break;
+
+ case 'p':
+ offset += _convert_hex(str + offset, FLAG_POUND | FLAG_LCHEX,
+ size - offset, sizeof(void*), 0, arg);
+ arg = NEXTARG();
+ break;
+
+ case 'c':
+ OUTPUT(*(char*)arg);
+ arg = NEXTARG();
+ break;
+
+ case 's':
+ if(!precision || (size - offset) < precision) {
+ precision = size - offset;
+ }
+
+ offset += _convert_str(str + offset, precision, width, *(const char**)arg);
+ arg = NEXTARG();
+ break;
+
+ case 'b':
+ if(!precision || (size - offset) < precision) {
+ precision = size - offset;
+ }
+
+ offset += _convert_bin(str + offset, flags, precision, width, length, arg);
+ arg = NEXTARG();
+ break;
+
+ case '%':
+ OUTPUT('%');
+ break;
+
+ default:
+ break;
+ }
+
+ SET_STATE(STATE_NORMAL);
+ NEXT();
+ break;
+
+ case STATE_ERR:
+ SET_STATE(STATE_NORMAL);
+ OUTPUT(c);
+ NEXT();
+ break;
+
+ default:
+ SET_STATE(STATE_ERR);
+ break;
+ }
+ }
+
+ /* add trailing '\0', if there is space left for it */
+ if(offset < size) {
+ str[offset] = 0;
+ }
+
+gtfo:
+ return(offset);
+
+#undef RESET
+#undef OUTPUT
+#undef NEXT
+#undef NEXTARG
+#undef ADD_LENGTH
+#undef ADD_WIDTH
+#undef ADD_PRECISION
+#undef ADD_FLAG
+#undef SET_STATE
+}
+
+int printf(const char *fmt, ...)
+{
+ /*
+ * FIXME: Implement printf() properly once all of the
+ * necessary ingredients are in place
+ */
+
+ return(-ENOSYS);
+}
projects / corax / commitdiff