// util/base32.c, version 1.0.4
// Base32 source file from libutil
// Copyright (C) 2021, Jakob Wakeling
// MIT Licence

#include "base32.h"

#include <stdint.h>
#include <stdlib.h>
#include <string.h>

static const uint8_t B32E[];
static const uint8_t B32D[];

/* Encode Base32 */
size_t b32encode(uint8_t *dst, uint8_t *src, size_t len) {
	register uint8_t *i = src, *o = dst;
	
	for (; len >= 5; len -= 5) {
		*o++ = B32E[i[0] >> 3];
		*o++ = B32E[((i[0] & 0x07) << 2) | (i[1] >> 6)];
		*o++ = B32E[(i[1] >> 1) & 0x1F];
		*o++ = B32E[((i[1] & 0x01) << 4) | (i[2] >> 4)];
		*o++ = B32E[((i[2] & 0x0F) << 1) | (i[3] >> 7)];
		*o++ = B32E[(i[3] >> 2) & 0x1F];
		*o++ = B32E[((i[3] & 0x03) << 3) | (i[4] >> 5)];
		*o++ = B32E[i[4] & 0x1F]; i += 5;
	}
	
	switch (len) {
	case 1: {
		*o++ = B32E[i[0] >> 3];
		*o++ = B32E[(i[0] & 0x07) << 2];
		memset(o, '=', 6); o += 6; break;
	}
	case 2: {
		*o++ = B32E[i[0] >> 3];
		*o++ = B32E[((i[0] & 0x07) << 2) | (i[1] >> 6)];
		*o++ = B32E[(i[1] >> 1) & 0x1F];
		*o++ = B32E[(i[1] & 0x01) << 4];
		memset(o, '=', 4); o += 4; break;
	}
	case 3: {
		*o++ = B32E[i[0] >> 3];
		*o++ = B32E[((i[0] & 0x07) << 2) | (i[1] >> 6)];
		*o++ = B32E[(i[1] >> 1) & 0x1F];
		*o++ = B32E[((i[1] & 0x01) << 4) | (i[2] >> 4)];
		*o++ = B32E[(i[2] & 0x0F) << 1];
		memset(o, '=', 3); o += 3; break;
	}
	case 4: {
		*o++ = B32E[i[0] >> 3];
		*o++ = B32E[((i[0] & 0x07) << 2) | (i[1] >> 6)];
		*o++ = B32E[(i[1] >> 1) & 0x1F];
		*o++ = B32E[((i[1] & 0x01) << 4) | (i[2] >> 4)];
		*o++ = B32E[((i[2] & 0x0F) << 1) | (i[3] >> 7)];
		*o++ = B32E[(i[3] >> 2) & 0x1F];
		*o++ = B32E[(i[3] & 0x03) << 3];
		*o++ = '='; break;
	}
	}
	
	return o - dst;
}

/* Decode Base32 */
size_t b32decode(uint8_t *dst, uint8_t *src, size_t len) {
	register uint8_t *i = src, *o = dst;
	
	for (; src[len - 1] == '='; --len);
	
	for (; len >= 8; len -= 8) {
		*o++ = (B32D[i[0]] << 3) | (B32D[i[1]] >> 2);
		*o++ = (B32D[i[1]] << 6) | (B32D[i[2]] << 1) | (B32D[i[3]] >> 4);
		*o++ = (B32D[i[3]] << 4) | (B32D[i[4]] >> 1);
		*o++ = (B32D[i[4]] << 7) | (B32D[i[5]] << 2) | (B32D[i[6]] >> 3);
		*o++ = (B32D[i[6]] << 5) | (B32D[i[7]]); i += 8;
	}
	
	switch (len) {
	case 2: {
		*o++ = (B32D[i[0]] << 3) | (B32D[i[1]] >> 2);
		*o++ = (B32D[i[1]] << 6); break;
	}
	case 4: {
		*o++ = (B32D[i[0]] << 3) | (B32D[i[1]] >> 2);
		*o++ = (B32D[i[1]] << 6) | (B32D[i[2]] << 1) | (B32D[i[3]] >> 4);
		*o++ = (B32D[i[3]] << 4); break;
	}
	case 5: {
		*o++ = (B32D[i[0]] << 3) | (B32D[i[1]] >> 2);
		*o++ = (B32D[i[1]] << 6) | (B32D[i[2]] << 1) | (B32D[i[3]] >> 4);
		*o++ = (B32D[i[3]] << 4) | (B32D[i[4]] >> 1);
		*o++ = (B32D[i[4]] << 7); break;
	}
	case 7: {
		*o++ = (B32D[i[0]] << 3) | (B32D[i[1]] >> 2);
		*o++ = (B32D[i[1]] << 6) | (B32D[i[2]] << 1) | (B32D[i[3]] >> 4);
		*o++ = (B32D[i[3]] << 4) | (B32D[i[4]] >> 1);
		*o++ = (B32D[i[4]] << 7) | (B32D[i[5]] << 2) | (B32D[i[6]] >> 3);
		*o++ = (B32D[i[6]] << 5); break;
	}
	}
	
	return o - dst;
}

static const uint8_t B32E[] = {
	'A', 'B', 'C', 'D', 'E', 'F', 'G', 'H', 'I', 'J', 'K', 'L', 'M',
	'N', 'O', 'P', 'Q', 'R', 'S', 'T', 'U', 'V', 'W', 'X', 'Y', 'Z',
	'2', '3', '4', '5', '6', '7'
};

static const uint8_t B32D[] = {
	 0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,
	 0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,
	 0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,
	 0,  0, 26, 27, 28, 29, 30, 31,  0,  0,  0,  0,  0,  0,  0,  0,
	 0,  0,  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
};