v811_spc009/vendor/huanglong/uapi/flash/source/cmdline_parts.c

/*
 * Copyright (c) Hisilicon Technologies Co., Ltd. 2020-2020. All rights reserved.
 * Description: parse the cmdline parameter
 * Author: Hisilicon
 * Create: 2020-10-15
 */

#include "cmdline_parts.h"
#include "soc_log.h"

#include "uapi_flash.h"
#include "flash_ext.h"
#include "nand.h"
#include "securec.h"

#define BDEVNAME_SIZE   32  /* Largest string for a blockdev identifier */
#undef LOG_MODULE_ID
#define LOG_MODULE_ID SOC_ID_FLASH

struct cmdline_subpart {
    td_char name[BDEVNAME_SIZE]; /* partition name, such as 'rootfs' */
    td_u64 from;
    td_u64 size;
    struct cmdline_subpart *next_subpart;
};

struct cmdline_parts_info {
    td_char part_name[BDEVNAME_SIZE];
    td_u64 start_addr;
    td_u64 part_size;
};

struct cmdline_parts {
    td_char name[BDEVNAME_SIZE]; /* block device, such as 'mmcblk0' */
    td_u64 end_addr;
    struct cmdline_subpart *subpart;
    struct cmdline_parts *next_parts;
};

static struct cmdline_parts_info g_cmd_partition[MAX_PARTS];
static struct cmdline_parts *g_cmd_parts_head = NULL;

static td_u32 min(td_u32 a, td_u32 b)
{
    return a <= b ? a : b;
}

static td_u64 memparse(const td_char *ptr, td_char **retptr)
{
    td_char *endptr = NULL;   /* local pointer to end of parsed string */
    td_u64 ret = strtoull(ptr, &endptr, 0);

    switch (*endptr) {
        case 'G':
        case 'g':
            ret <<= 10; /* 10 - left shift bits */
        /* fall-through */
        case 'M':
        case 'm':
            ret <<= 10; /* 10 - left shift bits */
        /* fall-through */
        case 'K':
        case 'k':
            ret <<= 10; /* 10 - left shift bits */
            endptr++;
        /* fall-through */
        default:
            break;
    }

    if (retptr != NULL) {
        *retptr = endptr;
    }

    return ret;
}

static td_s32 parse_cmdline_ascii_2d(td_char **parse_cmdline,
                                     struct cmdline_subpart *new_subpart, td_s32 part_len)
{
    td_char *cmdline = *parse_cmdline;

    if (part_len <= 0) {
        return -1;
    }

    if (*cmdline == '-') {
        new_subpart->size = (td_u64)(~0ULL);
        cmdline++;
    } else {
        new_subpart->size = memparse(cmdline, &cmdline);
        if (new_subpart->size == 0) {
            soc_print("cmdline partition size is invalid.\n");
            return -1;
        }
    }

    *parse_cmdline = cmdline;
    return 0;
}

static td_void parse_cmdline_ascii_40(td_char **parse_cmdline,
                                      struct cmdline_subpart *new_subpart, td_s32 part_len, struct cmdline_parts *parts)
{
    td_char *cmdline = *parse_cmdline;

    if (part_len <= 0) {
        return;
    }

    if (*cmdline == '@') {
        cmdline++;
        new_subpart->from = memparse(cmdline, &cmdline);
        parts->end_addr   = new_subpart->from + new_subpart->size;
    } else {
        new_subpart->from = parts->end_addr;
        parts->end_addr  += new_subpart->size;
    }

    *parse_cmdline = cmdline;
}

static td_s32 parse_cmdline_ascii_28(td_char **parse_cmdline,
                                     struct cmdline_subpart *new_subpart, td_s32 part_len)
{
    td_char *cmdline = *parse_cmdline;
    td_u32 length;

    if (part_len <= 0) {
        return -1;
    }

    if (*cmdline == '(') {
        td_char *next = strchr(++cmdline, ')');
        if (next == NULL) {
            soc_print("cmdline partition format is invalid");
            return -1;
        }

        length = min((td_u32)(next - cmdline), sizeof(new_subpart->name) - 1);
        if (strncpy_s(new_subpart->name, sizeof(new_subpart->name), cmdline, length) != EOK) {
            soc_print("Failed to snprintf_s.\n");
            return -1;
        }
        new_subpart->name[length] = '\0';

        cmdline = ++next;
    } else {
        new_subpart->name[0] = '\0';
    }

    *parse_cmdline = cmdline;
    return 0;
}

static td_s32 parse_subpart(struct cmdline_parts *parts,
                            struct cmdline_subpart **subpart, td_char *cmdline, td_s32 cmdline_len)
{
    struct cmdline_subpart *new_subpart = NULL;

    if (cmdline_len <= 0) {
        return -EINVAL;
    }

    *subpart = NULL;

    new_subpart = malloc(sizeof(struct cmdline_subpart));
    if (new_subpart == NULL) {
        return -ENOMEM;
    }
    memset_s(new_subpart, sizeof(struct cmdline_subpart), 0, sizeof(struct cmdline_subpart));

    if (parse_cmdline_ascii_2d(&cmdline, new_subpart, sizeof(struct cmdline_subpart)) < 0) {
        free(new_subpart);
        return -EINVAL;
    }

    parse_cmdline_ascii_40(&cmdline, new_subpart, sizeof(struct cmdline_subpart), parts);

    if (parse_cmdline_ascii_28(&cmdline, new_subpart, sizeof(struct cmdline_subpart)) < 0) {
        free(new_subpart);
        return -EINVAL;
    }

    *subpart = new_subpart;
    return 0;
}

static td_void free_subpart(struct cmdline_parts *parts)
{
    struct cmdline_subpart *subpart = NULL;

    while (parts->subpart != NULL) {
        subpart = parts->subpart;
        parts->subpart = subpart->next_subpart;
        free(subpart);
    }
}

static td_void free_parts(struct cmdline_parts **parts)
{
    struct cmdline_parts *next_parts = NULL;

    while (*parts) {
        next_parts = (*parts)->next_parts;
        free_subpart(*parts);
        free(*parts);
        *parts = next_parts;
    }
}

static td_s32 parse_cmdline_parts(struct cmdline_parts *newparts, td_char *cmdline)
{
    td_u32 length;
    td_char buf[BDEVNAME_SIZE + 32 + 4]; /* 32 -, 4 - */
    td_char *next = NULL;
    td_char *cmd_str = cmdline;
    struct cmdline_subpart **next_subpart = NULL;

    next = strchr(cmd_str, ':');
    if (next == NULL) {
        soc_print("cmdline partition has not block device");
        return TD_FAILURE;
    }

    length = min((td_u32)(next - cmd_str), sizeof(newparts->name) - 1);
    if (strncpy_s(newparts->name, sizeof(newparts->name), cmd_str, length) != EOK) {
        return TD_FAILURE;
    }
    newparts->name[length] = '\0';
    newparts->end_addr = 0;
    next_subpart = &newparts->subpart;

    while (next != NULL) {
        ++next;
        if (*next == 0) {
            break;
        }
        cmd_str = next;
        next = strchr(cmd_str, ',');

        length = (!next) ? (sizeof(buf) - 1) : min((td_u32)(next - cmd_str), sizeof(buf) - 1);
        if (strncpy_s(buf, sizeof(buf), cmd_str, length) != EOK) {
            return TD_FAILURE;
        }

        buf[length] = '\0';

        if (parse_subpart(newparts, next_subpart, buf, sizeof(buf)) != 0) {
            return TD_FAILURE;
        }

        next_subpart = &(*next_subpart)->next_subpart;
    }

    if (newparts->subpart == NULL) {
        soc_print("cmdline partition has not valid partition");
        return TD_FAILURE;
    }

    return TD_SUCCESS;
}

static td_s32 parse_parts(struct cmdline_parts **parts, td_char *cmdline)
{
    struct cmdline_parts *newparts = NULL;

    if (cmdline == NULL) {
        soc_print("cmdline partition invalid");
        return -EINVAL;
    }

    newparts = malloc(sizeof(struct cmdline_parts));
    if (newparts == NULL) {
        return -ENOMEM;
    }
    memset_s(newparts, sizeof(struct cmdline_parts), 0, sizeof(struct cmdline_parts));

    if (parse_cmdline_parts(newparts, cmdline) != TD_SUCCESS) {
        free_subpart(newparts);
        free(newparts);
        return -EINVAL;
    }

    *parts = newparts;

    return 0;
}

static td_s32 parse_cmdline(struct cmdline_parts **parts, const td_char *cmdline)
{
    td_s32 ret;
    td_char *buf = NULL;
    td_char *pbuf = NULL;
    td_char *next = NULL;
    struct cmdline_parts **next_parts = NULL;

    *parts = NULL;

    if (strlen(cmdline) == SIZE_MAX) {
        next = pbuf = buf = strdup(cmdline);
    } else {
        next = pbuf = buf = strndup(cmdline, strlen(cmdline));
    }

    if (buf == NULL) {
        return -ENOMEM;
    }

    next_parts = parts;

    while ((next != NULL) && (*pbuf != 0)) {
        next = strchr(pbuf, ';');
        if (next != NULL) {
            *next = '\0';
        }

        ret = parse_parts(next_parts, pbuf);
        if (ret != 0) {
            goto fail;
        }

        if (next != NULL) {
            pbuf = ++next;
        }

        next_parts = &(*next_parts)->next_parts;
    }

    if (!*parts) {
        soc_print("cmdline partition has not valid partition.\n");
        ret = -EINVAL;
        goto fail;
    }

    ret = 0;
    free(buf);
    return ret;

fail:
    free_parts(parts);
    free(buf);
    return ret;
}


static struct cmdline_parts *get_cmdline_parts(const td_char *cmdline_string, td_u32 cmdline_len)
{
    td_char *parts_string = NULL;
    struct cmdline_parts *tmp_cmd_parts = NULL;

    if (cmdline_string == NULL) {
        return NULL;
    }

    if (cmdline_len <= 0) {
        return NULL;
    }

    parts_string = strstr(cmdline_string, "mmcblk0:");
    if (parts_string == NULL) {
        parts_string = strstr(cmdline_string, "sdd:");
        if (parts_string == NULL) {
            return NULL;
        }
    }

    if (parse_cmdline(&tmp_cmd_parts, parts_string) != 0) {
        if (tmp_cmd_parts != NULL) {
            free_parts(&tmp_cmd_parts);
        }
        return NULL;
    }

    return tmp_cmd_parts;
}

td_s32 cmdline_parts_init(td_char *bootargs)
{
    td_char *cmdline_string = NULL;
    td_char *pcmdline_buf = NULL;
    td_char *pend = NULL;

    if (bootargs == NULL) {
        return -1;
    }

    if (g_cmd_parts_head != NULL) {
        return 0;
    }

    cmdline_string = strstr(bootargs, "blkdevparts=");
    if (cmdline_string == NULL) {
        return -EINVAL;
    }
    cmdline_string += sizeof("blkdevparts=") - 1;

    pcmdline_buf = strdup(cmdline_string);
    if (pcmdline_buf == NULL) {
        return -ENOMEM;
    }

    pend = strchr(pcmdline_buf, ' ');
    if ((pend != NULL) && ((td_u32)(pend - pcmdline_buf) <= strlen(pcmdline_buf))) {
        *pend = '\0';
    }

    g_cmd_parts_head = get_cmdline_parts(pcmdline_buf, (td_u32)strlen(pcmdline_buf));
    if (g_cmd_parts_head == NULL) {
        soc_print("Fail to get cmdline parts from: %s\n", pcmdline_buf);
        free(pcmdline_buf);
        pcmdline_buf = NULL;
        return -ENODEV;
    }

    free(pcmdline_buf);
    pcmdline_buf = NULL;
    return 0;
}

static td_s32 find_cmdline_parts(struct cmdline_parts **cmdline_parts, const td_char *media_name)
{
    td_s32 got;
    size_t len;
    struct cmdline_parts *tmp_cmdline_parts = *cmdline_parts;

    got = 0;
    while ((tmp_cmdline_parts != NULL) && (got == 0)) {
        len = (strlen(media_name) < sizeof(tmp_cmdline_parts->name)) ?
              (strlen(media_name) + 1) : sizeof(tmp_cmdline_parts->name);
        if (strncmp(tmp_cmdline_parts->name, media_name, len) == 0) {
            got = 1;
            break;
        }
        tmp_cmdline_parts = tmp_cmdline_parts->next_parts;
    }

    if ((got == 0) || (tmp_cmdline_parts == NULL)) {
        return 0;
    }
    *cmdline_parts = tmp_cmdline_parts;
    return 1;
}

static td_s32 find_cmdline_subpart(struct cmdline_subpart **subpart, const td_char *ptn_name)
{
    td_s32 got;
    size_t len;
    struct cmdline_subpart *tmp_subpart = *subpart;

    got = 0;
    while ((tmp_subpart != NULL) && (got == 0)) {
        len = (strlen(ptn_name) < sizeof(tmp_subpart->name)) ?
              (strlen(ptn_name) + 1) : sizeof(tmp_subpart->name);
        if (strncmp(tmp_subpart->name, ptn_name, len) == 0) {
            got = 1;
            break;
        }
        tmp_subpart = tmp_subpart->next_subpart;
    }

    if ((got == 0) || (tmp_subpart == NULL)) {
        return 0;
    }

    *subpart = tmp_subpart;
    return 1;
}

/* 1 - find, 0 - no find */
td_s32 find_flash_part(const td_char *cmdline_string,
                       const td_char *media_name, const td_char *ptn_name, td_u64 *start, td_u64 *length)
{
    struct cmdline_parts *tmp_cmdline_parts = NULL;
    struct cmdline_subpart *tmp_subpart     = NULL;

    if ((media_name == NULL) || (ptn_name == NULL) || (start == NULL) || (length == NULL)) {
        return 0;
    }

    if (g_cmd_parts_head == NULL) {
        return 0;
    }
    tmp_cmdline_parts = g_cmd_parts_head;
    if (find_cmdline_parts(&tmp_cmdline_parts, media_name) == 0) {
        soc_print("%s not found from: %s\n", media_name, cmdline_string);
        return 0;
    }

    tmp_subpart = tmp_cmdline_parts->subpart;
    if (find_cmdline_subpart(&tmp_subpart, ptn_name) == 0) {
        soc_print("%s not found from: %s\n", ptn_name, cmdline_string);
        return 0;
    }

    if (tmp_subpart == NULL) {
        return 0;
    }

    *start  = (td_u64)(tmp_subpart->from);
    *length = (td_u64)(tmp_subpart->size);
    soc_log_dbg("Got partition %s: start=0x%llX,size=%llu\n", ptn_name, *start, *length);

    return 1;
}

static td_s32 get_cmdline_subpart(td_u32 *part_index)
{
    td_u32 partindex = 0;
    struct cmdline_subpart *part = NULL;

    if ((g_cmd_parts_head == NULL) || (g_cmd_parts_head->subpart == NULL)) {
        soc_print("cmdline parts not initialized.\n");
        return -1;
    }

    part = g_cmd_parts_head->subpart;
    while ((part != NULL) && (partindex < MAX_PARTS)) {
        if (strncpy_s(g_cmd_partition[partindex].part_name, BDEVNAME_SIZE, part->name, BDEVNAME_SIZE - 1) != 0) {
            return -1;
        }
        g_cmd_partition[partindex].part_name[sizeof(g_cmd_partition[partindex].part_name) - 1] = '\0';
        g_cmd_partition[partindex].start_addr = (part->from);
        g_cmd_partition[partindex].part_size = (part->size);

        partindex++;
        part = part->next_subpart;
    }
    *part_index = partindex;
    return 0;
}

/* 0 - success, -1 - fail */
td_s32 get_part_info(td_u8 partnum, td_u64 *start, td_u64 *size)
{
    td_u32 partindex = 0;

    if (start == NULL || size == NULL) {
        return -1;
    }

    if (get_cmdline_subpart(&partindex) != 0) {
        return -1;
    }

    if (partnum == 0) {
        soc_print("partnum(%u) is invalid\n", (td_u32)partnum);
        return -1;
    }
    if (partnum > partindex) {
        soc_print("partnum is to large, max partnum is %u.\n", partindex);
        return -1;
    }

    *start = (td_u64)g_cmd_partition[partnum - 1].start_addr;
    *size = (td_u64)g_cmd_partition[partnum - 1].part_size;

    return 0;
}