util.c (4710B)
1 #include <stdint.h> 2 #include <string.h> 3 #include <stdarg.h> 4 #include <stdio.h> 5 #include <arpa/inet.h> 6 7 #include "util.h" 8 9 struct dummy { 10 int dummy : (TOTP_EMAX == 0) * 2 - 1; 11 }; 12 13 void xormem(void *a, const void *b, size_t len) 14 { 15 uint8_t *wa = a; 16 const uint8_t *rb = b; 17 while (len--) 18 *wa++ ^= *rb++; 19 } 20 21 const char *totp_errstr[] = { 22 "Database ended unexpectedly", 23 "Malformed database or invalid passphrase", 24 }; 25 26 void hmac(const void *key, size_t keylen, 27 const void *data, size_t datalen, 28 digest_init init, 29 digest_update update, 30 digest_finish finish, 31 size_t ctxsz, 32 size_t blocksz, 33 size_t digestsz, 34 ptrdiff_t buf_off, 35 ptrdiff_t digest_off, 36 void *h) 37 { 38 char s[ctxsz]; 39 char s2[ctxsz]; 40 41 init(s); 42 memset(s + buf_off, 0x36, blocksz); 43 if (keylen > blocksz) { 44 init(s2); 45 update(s2, key, keylen); 46 finish(s2); 47 xormem(s + buf_off, s2 + digest_off, digestsz); 48 } else { 49 xormem(s + buf_off, key, keylen); 50 } 51 52 update(s, s + buf_off, blocksz); 53 update(s, data, datalen); 54 finish(s); 55 56 memset(s2 + buf_off, 0x5c, blocksz); 57 if (keylen > blocksz) 58 xormem(s2 + buf_off, s2 + digest_off, digestsz); 59 else 60 xormem(s2 + buf_off, key, keylen); 61 init(s2); 62 update(s2, s2 + buf_off, blocksz); 63 update(s2, s + digest_off, digestsz); 64 finish(s2); 65 memcpy(h, s2 + digest_off, digestsz); 66 } 67 68 uint32_t hotp(const void *key, size_t keylen, 69 const void *counter, size_t counterlen, 70 void (*hmac_f)(const void *key, size_t keylen, 71 const void *data, size_t datalen, 72 void *h), size_t hshsz) 73 { 74 uint8_t h[hshsz]; 75 uint8_t *pos; 76 77 hmac_f(key, keylen, counter, counterlen, h); 78 pos = &h[h[hshsz - 1] & 0xf]; 79 80 return (uint32_t)(pos[0] & INT8_MAX) << 24 | 81 (uint32_t)pos[1] << 16 | 82 (uint32_t)pos[2] << 8 | 83 pos[3]; 84 } 85 86 uint32_t totp(const void *key, size_t keylen, 87 time_t t1, uint8_t period, 88 time_t t0, 89 void (*hmac_f)(const void *key, size_t keylen, 90 const void *data, size_t datalen, 91 void *h), size_t hshsz) 92 { 93 uint8_t tb[sizeof(uint64_t)]; 94 writebeu64(tb, (t1 - t0) / period); 95 96 return hotp(key, keylen, &tb, sizeof(tb), hmac_f, hshsz); 97 } 98 99 size_t strncspn(const char *s, size_t l, const char *c) 100 { 101 size_t disallowed[256 / 8 / sizeof(size_t)] = { 0 }; 102 const unsigned char *u = (const unsigned char *)c; 103 104 if (!*c) 105 return 0; 106 for (u = (const unsigned char *)c; 107 *u && (disallowed[*u / (8 * sizeof(*disallowed))] |= 1ULL << (*u % (8 * sizeof(*disallowed)))); 108 u++); 109 disallowed[0] |= 1; 110 for (u = (const unsigned char *)s; 111 l-- && !(disallowed[*u / (8 * sizeof(*disallowed))] & 1ULL << (*u % (8 * sizeof(*disallowed)))); 112 u++); 113 114 return (const char *)u - s; 115 } 116 117 size_t strnspn(const char *s, size_t l, const char *c) 118 { 119 size_t allowed[256 / 8 / sizeof(size_t)] = { 0 }; 120 const unsigned char *u = (const unsigned char *)c; 121 122 if (!*c) 123 return 0; 124 for (u = (const unsigned char *)c; 125 *u && (allowed[*u / (8 * sizeof(*allowed))] |= 1ULL << (*u % (8 * sizeof(*allowed)))); 126 u++); 127 for (u = (const unsigned char *)s; 128 l-- && (allowed[*u / (8 * sizeof(*allowed))] & 1ULL << (*u % (8 * sizeof(*allowed)))); 129 u++); 130 131 return (const char *)u - s; 132 } 133 134 const char *totp_strerror(int err) 135 { 136 if (err < TOTP_EMIN) 137 return ""; 138 if (err >= TOTP_EMAX) 139 return strerror(err); 140 return totp_errstr[err - TOTP_EMIN]; 141 } 142 143 void croak(const char *fmt, ...) 144 { 145 va_list args; 146 va_start(args, fmt); 147 vfprintf(stderr, fmt, args); 148 va_end(args); 149 fputs("\n", stderr); 150 151 exit(1); 152 } 153 154 struct bytes debase32(struct bytes data) 155 { 156 uint8_t *wp = data.data; 157 const uint8_t *rp; 158 uint16_t v = 0; 159 size_t b = 0; 160 161 static const uint8_t val[256] = { 162 ['A'] = 1, ['B'] = 2, ['C'] = 3, ['D'] = 4, ['E'] = 5, 163 ['F'] = 6, ['G'] = 7, ['H'] = 8, ['I'] = 9, ['J'] = 10, 164 ['K'] = 11, ['L'] = 12, ['M'] = 13, ['N'] = 14, ['O'] = 15, 165 ['P'] = 16, ['Q'] = 17, ['R'] = 18, ['S'] = 19, ['T'] = 20, 166 ['U'] = 21, ['V'] = 22, ['W'] = 23, ['X'] = 24, ['Y'] = 25, 167 ['Z'] = 26, ['a'] = 1, ['b'] = 2, ['c'] = 3, ['d'] = 4, 168 ['e'] = 5, ['f'] = 6, ['g'] = 7, ['h'] = 8, ['i'] = 9, 169 ['j'] = 10, ['k'] = 11, ['l'] = 12, ['m'] = 13, ['n'] = 14, 170 ['o'] = 15, ['p'] = 16, ['q'] = 17, ['r'] = 18, ['s'] = 19, 171 ['t'] = 20, ['u'] = 21, ['v'] = 22, ['w'] = 23, ['x'] = 24, 172 ['y'] = 25, ['z'] = 26, ['2'] = 27, ['3'] = 28, ['4'] = 29, 173 ['5'] = 30, ['6'] = 31, ['7'] = 32 174 }; 175 176 for (rp = data.data; rp < data.end && *rp && *rp != '='; rp++) { 177 uint8_t c = val[*rp]; 178 if (!c) 179 return (struct bytes){ 0 }; 180 v = v << 5 | (c - 1); 181 b += 5; 182 if (b >= 8) { 183 *wp++ = (v >> (b & 7)) & 255; 184 b -= 8; 185 } 186 } 187 188 return (struct bytes){ data.data, wp }; 189 } 190 191 void randmem(void *mem, size_t n) 192 { 193 uint8_t *wp = mem; 194 uint8_t *end = wp + n; 195 196 while (wp < end) 197 *wp++ = rand(); 198 }