/* Copyright 2022 Sony Corporation */

#include <command.h>
#include <linux/stddef.h>
#include <linux/sizes.h>
#include <memalign.h>
#include <utility.h>
#include <errno.h>
#include <blk.h>
#include <mmc.h>
#include <cli.h>
#include <debug_impl.h>


#define BOOT_ENABLE_OFFSET		22528 // emmc offset = 0x5800
#define BOOT_ENABLE_OFFSET_OTA_NO_OVERWRITE	64 // emmc offset = 0x40
#define BASE64_DECODE_SIZE		0x400


static const unsigned char map[256] = {
	255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255,
	255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255,
	255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255,
	255, 255, 255, 255, 255, 255, 255,  62, 255, 255, 255,  63,
	52,  53,  54,  55,  56,  57,  58,  59,  60,  61, 255, 255,
	255, 254, 255, 255, 255,   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, 255, 255, 255, 255, 255,
	255,  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, 255, 255, 255, 255, 255, 255, 255, 255, 255,
	255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255,
	255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255,
	255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255,
	255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255,
	255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255,
	255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255,
	255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255,
	255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255,
	255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255,
	255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255,
	255, 255, 255, 255
};

static inline void __swap(uint8_t *o1, uint8_t *o2, int size)
{
	int k;

	for (k = 0; k < size; k++) {
		uint8_t tmp = o1[k];

		o1[k] = o2[k];
		o2[k] = tmp;
	}
}

static inline void reverse(void *objects, size_t size, size_t length)
{
	uint8_t *ptr = objects;
	int i, j;

	for (i = 0, j = length - 1; i < j; i++, j--)
		__swap(ptr + size * i, ptr + size * j, size);
}

static int base64_decode(const unsigned char *in,  unsigned long inlen,
		unsigned char *out, unsigned long *outlen)
{
	unsigned long t, x, y, z;
	unsigned char c;
	int           g = 3;

	for (x = y = z = t = 0; x < inlen; x++) {
		c = map[in[x] & 0xFF];
		if (c == 255)
			continue;
		if (c == 254) {
			c = 0;
			if (--g < 0)
				return -2;
		} else if (g != 3) {
			return -2;
		}

		t = (t << 6) | c;

		if (++y == 4) {
			if (z + g > *outlen)
				return -1;
			out[z++] = (unsigned char)((t >> 16) & 255);
			if (g > 1)
				out[z++] = (unsigned char)((t >> 8) & 255);
			if (g > 2)
				out[z++] = (unsigned char)(t & 255);
			y = t = 0;
		}
	}
	if (y != 0)
		return -2;

	*outlen = z;
	return 0;
}

static inline struct mmc *__init_mmc_dev(int dev)
{
	struct mmc *mmc;

	mmc = find_mmc_device(dev);
	if (!mmc) {
		UBOOT_ERROR("no mmc device at slot %x\n", dev);
		return NULL;
	}

	if (mmc_init(mmc))
		return NULL;

	return mmc;
}

static struct mmc *_get_mmc_dev(void)
{
	if (get_mmc_num() <= 0) {
		UBOOT_ERROR("no emmc available\n");
		return NULL;
	}

	return __init_mmc_dev(0);
}

static int storage_write(void *buf, size_t buf_len, uint32_t blk_addr)
{
	unsigned long n;
	lbaint_t blk_cnt, blk_start;
	struct mmc *mmc;
	struct blk_desc *desc;

	mmc = _get_mmc_dev();
	if (!mmc)
		return -ENODEV;

	desc = mmc_get_blk_desc(mmc);
	if (!desc) {
		UBOOT_ERROR("mmc_get_blk_desc failed\n");
		return -ENODEV;
	}

	blk_start = ALIGN(blk_addr, mmc->write_bl_len) / mmc->write_bl_len;
	blk_cnt = ALIGN(buf_len, mmc->write_bl_len) / mmc->write_bl_len;
	n = blk_dwrite(desc, blk_start, blk_cnt, buf);
	if (n != blk_cnt) {
		UBOOT_ERROR("blk_dwrite failed\n");
		return -EINVAL;
	}

	return 0;
}


int do_boot_enable(cmd_tbl_t *cmdtp, int flag, int argc, char * const argv[])
{
	unsigned char in[BASE64_DECODE_SIZE] = {0};
	unsigned char buffer[BASE64_DECODE_SIZE] = {0};
	unsigned char boot_enable_bin[0x400];
	unsigned long boot_enable_bin_size = 0x100;

	memset(boot_enable_bin, 0, 0x400);

	cli_readline_into_buffer("input base64 encoded boot_enable.bin: ", (char *)in, 0);

	if (!strncmp((const char *)in, "quit", strlen("quit")) || !strncmp((const char *)in, "exit", strlen("exit"))) {
		printf("terminate...\n");
		return CMD_RET_SUCCESS;
	}

	/* 256 bytes boot enable bin is encoded into 344 byte string */
	if (strlen((const char *)in) > 344) {
		UBOOT_ERROR("input length=%zu is larger than 344\n", strlen((const char *)in));
		return CMD_RET_FAILURE;
	}

	if (base64_decode(in, strlen((const char *)in), buffer, &boot_enable_bin_size)) {
		UBOOT_ERROR("base64 decode failed, format error!\n");
		return CMD_RET_FAILURE;
	}

	if (boot_enable_bin_size != 0x100) {
		UBOOT_ERROR("base64 decode got invaild boot_enable_bin_size %ld\n",
				boot_enable_bin_size);
		return CMD_RET_FAILURE;
	}

	memcpy(boot_enable_bin, buffer, boot_enable_bin_size);

	if (storage_write(boot_enable_bin, 0x400, BOOT_ENABLE_OFFSET * 512)) {
		UBOOT_ERROR("failed to write boot_enable.bin into emmc!\n ");
		return CMD_RET_FAILURE;
	}

	if (storage_write(boot_enable_bin, 0x400, BOOT_ENABLE_OFFSET_OTA_NO_OVERWRITE * 512)) {
		UBOOT_ERROR("failed to write boot_enable.bin into emmc!\n ");
		return CMD_RET_FAILURE;
	}

	printf("successfully written to emmc !\n");

	return CMD_RET_SUCCESS;
}

U_BOOT_CMD(
	boot_enable, 1, 0, do_boot_enable,
	"Write b64encode boot enable bin to eMMC",
	NULL
);
