[Feature]add MT2731_MP2_MR2_SVN388 baseline version
Change-Id: Ief04314834b31e27effab435d3ca8ba33b499059
diff --git a/src/bsp/lk/lib/kcmdline/kcmdline.c b/src/bsp/lk/lib/kcmdline/kcmdline.c
new file mode 100644
index 0000000..3ef13c6
--- /dev/null
+++ b/src/bsp/lk/lib/kcmdline/kcmdline.c
@@ -0,0 +1,310 @@
+/*
+ * Copyright (c) 2018 MediaTek Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining
+ * a copy of this software and associated documentation files
+ * (the "Software"), to deal in the Software without restriction,
+ * including without limitation the rights to use, copy, modify, merge,
+ * publish, distribute, sublicense, and/or sell copies of the Software,
+ * and to permit persons to whom the Software is furnished to do so,
+ * subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be
+ * included in all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
+ * IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY
+ * CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,
+ * TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
+ * SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+ */
+
+#include <assert.h>
+#include <debug.h>
+#include <err.h>
+#include <kernel/mutex.h>
+#include <kernel/thread.h>
+#include <lib/kcmdline.h>
+#include <libfdt.h>
+#include <list.h>
+#include <malloc.h>
+#include <stddef.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <trace.h>
+
+#if LK_DEBUGLEVEL > 0
+#define LOCAL_TRACE 1
+#else
+#define LOCAL_TRACE 0
+#endif
+
+#define CMDLINE_OVERFLOW_STR "[ERROR] CMDLINE overflow"
+
+struct subst_entry {
+ struct list_node node;
+ char *old_arg;
+ char *new_arg;
+};
+
+/* variable for keeping substition arg */
+struct list_node subst_list = LIST_INITIAL_VALUE(subst_list);
+
+/* variable for keeping append arg */
+static char *cmdline_buf;
+static char *cmdline_tail;
+static char *cmdline_end;
+
+static mutex_t lock = MUTEX_INITIAL_VALUE(lock);
+
+static inline void validate_cmdline_boundary(const char *tail, const char *end)
+{
+ if (tail >= end) {
+ dprintf(CRITICAL, CMDLINE_OVERFLOW_STR"\n");
+ panic(CMDLINE_OVERFLOW_STR);
+ }
+}
+
+static void dump_cmdline(void *fdt)
+{
+ int len;
+ int chosen_node_offset;
+ const char *cmdline;
+
+ chosen_node_offset = fdt_path_offset(fdt, "/chosen");
+ if (chosen_node_offset < 0) {
+ LTRACEF("can't find chosen node.\n");
+ return;
+ }
+
+ cmdline = fdt_getprop(fdt, chosen_node_offset, "bootargs", &len);
+ if (!cmdline) {
+ LTRACEF("fdt_getprop bootargs failed.\n");
+ return;
+ }
+
+ LTRACEF("cmdline len=%zd, str=\"%s\"\n", strlen(cmdline), cmdline);
+}
+
+int kcmdline_init(void)
+{
+ if (cmdline_buf)
+ return ERR_ALREADY_EXISTS;
+
+ cmdline_buf = (char *)malloc(CMDLINE_LEN);
+ if (!cmdline_buf)
+ return ERR_NO_MEMORY;
+
+ memset(cmdline_buf, 0, CMDLINE_LEN);
+ cmdline_tail = cmdline_buf;
+ cmdline_end = cmdline_buf + CMDLINE_LEN;
+
+ return NO_ERROR;
+}
+
+int kcmdline_finalized(void *fdt, size_t size)
+{
+ int ret;
+ int len;
+ int offset;
+ const void *fdt_bootargs;
+ char *temp_ptr = NULL;
+ size_t append_arg_len = 0;
+ struct subst_entry *entry;
+ struct subst_entry *temp;
+
+ if (!cmdline_buf)
+ return ERR_NOT_READY;
+
+ if (!fdt || !size) {
+ LTRACEF("Invalid args: fdt(%p), size(%zd)\n", fdt, size);
+ return ERR_INVALID_ARGS;
+ }
+
+ ret = NO_ERROR;
+ mutex_acquire(&lock);
+
+ if ((*cmdline_buf == 0x0) && list_is_empty(&subst_list))
+ goto exit;
+
+ /* cmdline_buf is filled with bootargs before kcmdline_finalize is called */
+ if (*cmdline_buf != 0x0) {
+ append_arg_len = strlen(cmdline_buf);
+ temp_ptr = (char *)malloc(append_arg_len + 1);
+ if (!temp_ptr) {
+ ret = ERR_NO_MEMORY;
+ goto exit;
+ }
+ snprintf(temp_ptr, (append_arg_len + 1), "%s", cmdline_buf);
+ LTRACEF("append_arg_len:%zu cmdline_buf:%s\n",
+ append_arg_len, cmdline_buf);
+ }
+
+ ret = fdt_open_into(fdt, fdt, size);
+ if (ret) {
+ LTRACEF("fdt_open_into failed\n");
+ goto exit;
+ }
+ ret = fdt_check_header(fdt);
+ if (ret) {
+ LTRACEF("fdt_check_header failed\n");
+ goto exit;
+ }
+
+ /* Reset cmdline_tail */
+ cmdline_tail = cmdline_buf;
+ offset = fdt_path_offset(fdt, "/chosen");
+ fdt_bootargs = fdt_getprop(fdt, offset, "bootargs", &len);
+ if (!fdt_bootargs) {
+ ret = ERR_NOT_FOUND;
+ goto exit;
+ }
+
+ /* add appended string to final string */
+ validate_cmdline_boundary(cmdline_tail + append_arg_len +
+ strlen(fdt_bootargs) + 2, cmdline_end);
+ cmdline_tail += snprintf(cmdline_tail, CMDLINE_LEN, "%s",
+ (char *)fdt_bootargs);
+ if (temp_ptr) {
+ cmdline_tail += snprintf(cmdline_tail, cmdline_end - cmdline_tail,
+ " %s", temp_ptr);
+ }
+
+ /* subst string in final string */
+ list_for_every_entry_safe(&subst_list, entry, temp,
+ struct subst_entry, node) {
+ char *pos = strstr(cmdline_buf, entry->old_arg);
+ size_t old_len = strlen(entry->old_arg);
+ if (pos && ((*(pos + old_len) == ' ') || (*(pos + old_len) == '\0'))) {
+ size_t new_len = strlen(entry->new_arg);
+ char *p;
+
+ /* erase old arg with space */
+ memset(pos, ' ', old_len);
+ if (old_len >= new_len) {
+ p = pos; /* replace old arg with new arg */
+ } else {
+ /* append new arg in the end of cmdline */
+ validate_cmdline_boundary(cmdline_tail + new_len + 2,
+ cmdline_end);
+ p = cmdline_tail;
+ cmdline_tail += (new_len + 1);
+ *p++ = ' ';
+ }
+ memcpy(p, entry->new_arg, new_len);
+ }
+
+ /* free memory and delete node */
+ free(entry->old_arg);
+ free(entry->new_arg);
+ list_delete(&entry->node);
+ free(entry);
+ entry = NULL;
+ }
+
+ ret = fdt_setprop(fdt, offset, "bootargs", cmdline_buf,
+ strlen(cmdline_buf) + 1);
+ if (ret != 0) {
+ dprintf(CRITICAL, "fdt_setprop error.\n");
+ ret = ERR_GENERIC;
+ goto exit;
+ }
+
+ ret = fdt_pack(fdt);
+ if (ret != 0) {
+ dprintf(CRITICAL, "fdt_pack error.\n");
+ ret = ERR_GENERIC;
+ goto exit;
+ }
+
+ free(cmdline_buf);
+ cmdline_buf = NULL;
+
+#if LOCAL_TRACE
+ dump_cmdline(fdt);
+#endif
+
+exit:
+ free(temp_ptr);
+ mutex_release(&lock);
+
+ return ret;
+}
+
+void kcmdline_print(void)
+{
+ struct subst_entry *entry;
+
+ if (!cmdline_buf)
+ return;
+
+ mutex_acquire(&lock);
+ LTRACEF("append cmdline: %s\n", cmdline_buf);
+ LTRACEF("append cmdline size: %zd\n", strlen(cmdline_buf));
+ LTRACEF("subst list:\n");
+
+ /* traverse list to show subst_list */
+ list_for_every_entry(&subst_list, entry, struct subst_entry, node) {
+ LTRACEF("old_arg: %s, new_arg: %s\n", entry->old_arg, entry->new_arg);
+ }
+ mutex_release(&lock);
+}
+
+int kcmdline_append(const char *append_arg)
+{
+ size_t append_arg_len;
+
+ if (!cmdline_buf)
+ return ERR_NOT_READY;
+
+ if (!append_arg)
+ return ERR_INVALID_ARGS;
+
+ mutex_acquire(&lock);
+ append_arg_len = strlen(append_arg);
+ validate_cmdline_boundary(cmdline_tail + append_arg_len + 1, cmdline_end);
+ cmdline_tail += snprintf(cmdline_tail, cmdline_end - cmdline_tail, " %s",
+ append_arg);
+ mutex_release(&lock);
+
+ return NO_ERROR;
+}
+
+int kcmdline_subst(const char *old_arg, const char *new_arg)
+{
+ struct subst_entry *entry;
+
+ if (!cmdline_buf)
+ return ERR_NOT_READY;
+
+ if (!old_arg || !new_arg) {
+ LTRACEF("Invalid args: old_arg(%p), new_arg(%p)\n", old_arg, new_arg);
+ return ERR_INVALID_ARGS;
+ }
+
+ entry = (struct subst_entry *)malloc(sizeof(struct subst_entry));
+ if (!entry)
+ return ERR_NO_MEMORY;
+
+ memset(entry, 0, sizeof(struct subst_entry));
+ entry->old_arg = strdup(old_arg);
+ entry->new_arg = strdup(new_arg);
+
+ if (!entry->old_arg || !entry->new_arg) {
+ free(entry->old_arg);
+ free(entry->new_arg);
+ free(entry);
+
+ return ERR_NO_MEMORY;
+ }
+
+ mutex_acquire(&lock);
+ list_add_tail(&subst_list, &entry->node);
+ mutex_release(&lock);
+
+ return NO_ERROR;
+}
+