// SPDX-License-Identifier: GPL-2.0
#include <linux/libfdt_env.h>
#include <asm/setup.h>
#include <libfdt.h>

#if defined(CONFIG_ARM_ATAG_DTB_COMPAT_CMDLINE_EXTEND)
#define do_extend_cmdline 1
#else
#define do_extend_cmdline 0
#endif

#define NR_BANKS 16

static int node_offset(void *fdt, const char *node_path)
{
        int offset = fdt_path_offset(fdt, node_path);
        if (offset == -FDT_ERR_NOTFOUND)
                /* Add the node to root if not found, dropping the leading '/' */
                offset = fdt_add_subnode(fdt, 0, node_path + 1);
        return offset;
}

static int setprop(void *fdt, const char *node_path, const char *property,
                   void *val_array, int size)
{
        int offset = node_offset(fdt, node_path);
        if (offset < 0)
                return offset;
        return fdt_setprop(fdt, offset, property, val_array, size);
}

static int setprop_string(void *fdt, const char *node_path,
                          const char *property, const char *string)
{
        int offset = node_offset(fdt, node_path);
        if (offset < 0)
                return offset;
        return fdt_setprop_string(fdt, offset, property, string);
}

static int setprop_cell(void *fdt, const char *node_path,
                        const char *property, uint32_t val)
{
        int offset = node_offset(fdt, node_path);
        if (offset < 0)
                return offset;
        return fdt_setprop_cell(fdt, offset, property, val);
}

static const void *getprop(const void *fdt, const char *node_path,
                           const char *property, int *len)
{
        int offset = fdt_path_offset(fdt, node_path);

        if (offset == -FDT_ERR_NOTFOUND)
                return NULL;

        return fdt_getprop(fdt, offset, property, len);
}

static uint32_t get_cell_size(const void *fdt)
{
        int len;
        uint32_t cell_size = 1;
        const __be32 *size_len =  getprop(fdt, "/", "#size-cells", &len);

        if (size_len)
                cell_size = fdt32_to_cpu(*size_len);
        return cell_size;
}

static void merge_fdt_bootargs(void *fdt, const char *fdt_cmdline)
{
        char cmdline[COMMAND_LINE_SIZE];
        const char *fdt_bootargs;
        char *ptr = cmdline;
        int len = 0;

        /* copy the fdt command line into the buffer */
        fdt_bootargs = getprop(fdt, "/chosen", "bootargs", &len);
        if (fdt_bootargs)
                if (len < COMMAND_LINE_SIZE) {
                        memcpy(ptr, fdt_bootargs, len);
                        /* len is the length of the string
                         * including the NULL terminator */
                        ptr += len - 1;
                }

        /* and append the ATAG_CMDLINE */
        if (fdt_cmdline) {
                len = strlen(fdt_cmdline);
                if (ptr - cmdline + len + 2 < COMMAND_LINE_SIZE) {
                        *ptr++ = ' ';
                        memcpy(ptr, fdt_cmdline, len);
                        ptr += len;
                }
        }
        *ptr = '\0';

        setprop_string(fdt, "/chosen", "bootargs", cmdline);
}

static void hex_str(char *out, uint32_t value)
{
        uint32_t digit;
        int idx;

        for (idx = 7; idx >= 0; idx--) {
                digit = value >> 28;
                value <<= 4;
                digit &= 0xf;
                if (digit < 10)
                        digit += '0';
                else
                        digit += 'A'-10;
                *out++ = digit;
        }
        *out = '\0';
}

/*
 * Convert and fold provided ATAGs into the provided FDT.
 *
 * Return values:
 *    = 0 -> pretend success
 *    = 1 -> bad ATAG (may retry with another possible ATAG pointer)
 *    < 0 -> error from libfdt
 */
int atags_to_fdt(void *atag_list, void *fdt, int total_space)
{
        struct tag *atag = atag_list;
        /* In the case of 64 bits memory size, need to reserve 2 cells for
         * address and size for each bank */
        __be32 mem_reg_property[2 * 2 * NR_BANKS];
        int memcount = 0;
        int ret, memsize;

        /* make sure we've got an aligned pointer */
        if ((u32)atag_list & 0x3)
                return 1;

        /* if we get a DTB here we're done already */
        if (*(__be32 *)atag_list == cpu_to_fdt32(FDT_MAGIC))
               return 0;

        /* validate the ATAG */
        if (atag->hdr.tag != ATAG_CORE ||
            (atag->hdr.size != tag_size(tag_core) &&
             atag->hdr.size != 2))
                return 1;

        /* let's give it all the room it could need */
        ret = fdt_open_into(fdt, fdt, total_space);
        if (ret < 0)
                return ret;

        for_each_tag(atag, atag_list) {
                if (atag->hdr.tag == ATAG_CMDLINE) {
                        /* Append the ATAGS command line to the device tree
                         * command line.
                         * NB: This means that if the same parameter is set in
                         * the device tree and in the tags, the one from the
                         * tags will be chosen.
                         */
                        if (do_extend_cmdline)
                                merge_fdt_bootargs(fdt,
                                                   atag->u.cmdline.cmdline);
                        else
                                setprop_string(fdt, "/chosen", "bootargs",
                                               atag->u.cmdline.cmdline);
                } else if (atag->hdr.tag == ATAG_MEM) {
                        if (memcount >= sizeof(mem_reg_property)/4)
                                continue;
                        if (!atag->u.mem.size)
                                continue;
                        memsize = get_cell_size(fdt);

                        if (memsize == 2) {
                                /* if memsize is 2, that means that
                                 * each data needs 2 cells of 32 bits,
                                 * so the data are 64 bits */
                                __be64 *mem_reg_prop64 =
                                        (__be64 *)mem_reg_property;
                                mem_reg_prop64[memcount++] =
                                        cpu_to_fdt64(atag->u.mem.start);
                                mem_reg_prop64[memcount++] =
                                        cpu_to_fdt64(atag->u.mem.size);
                        } else {
                                mem_reg_property[memcount++] =
                                        cpu_to_fdt32(atag->u.mem.start);
                                mem_reg_property[memcount++] =
                                        cpu_to_fdt32(atag->u.mem.size);
                        }

                } else if (atag->hdr.tag == ATAG_INITRD2) {
                        uint32_t initrd_start, initrd_size;
                        initrd_start = atag->u.initrd.start;
                        initrd_size = atag->u.initrd.size;
                        setprop_cell(fdt, "/chosen", "linux,initrd-start",
                                        initrd_start);
                        setprop_cell(fdt, "/chosen", "linux,initrd-end",
                                        initrd_start + initrd_size);
                } else if (atag->hdr.tag == ATAG_SERIAL) {
                        char serno[16+2];
                        hex_str(serno, atag->u.serialnr.high);
                        hex_str(serno+8, atag->u.serialnr.low);
                        setprop_string(fdt, "/", "serial-number", serno);
                }
        }

        if (memcount) {
                setprop(fdt, "/memory", "reg", mem_reg_property,
                        4 * memcount * memsize);
        }

        return fdt_pack(fdt);
}