[Feature]add MT2731_MP2_MR2_SVN388 baseline version
Change-Id: Ief04314834b31e27effab435d3ca8ba33b499059
diff --git a/src/bsp/lk/lib/android_bootimg/android_bootimg.c b/src/bsp/lk/lib/android_bootimg/android_bootimg.c
new file mode 100644
index 0000000..c7dc25c
--- /dev/null
+++ b/src/bsp/lk/lib/android_bootimg/android_bootimg.c
@@ -0,0 +1,145 @@
+/*
+ * Copyright (c) 2020 MediaTek Inc.
+ *
+ * Use of this source code is governed by a MIT-style
+ * license that can be found in the LICENSE file or at
+ * https://opensource.org/licenses/MIT
+ */
+
+#include <arch/ops.h>
+#include <assert.h>
+#include <errno.h>
+#include <lib/android_bootimg.h>
+#include <lib/bio.h>
+#include <lib/boot_info.h>
+#include <lib/decompress.h>
+#include <lib/mempool.h>
+#include <kernel/vm.h>
+#include <string.h>
+#include <trace.h>
+
+#define LOCAL_TRACE 0
+
+static int get_decompress_algo(unsigned char *src)
+{
+ int ret = NO_DECOMPRESS;
+ uint16_t gzip_magic;
+ uint32_t lz4_magic;
+
+ /* lz4 magic stores to 32-bits little-endian */
+ lz4_magic = src[0];
+ lz4_magic |= (src[1] << 8);
+ lz4_magic |= (src[2] << 16);
+ lz4_magic |= (src[3] << 24);
+
+ /* gzip magic stores to 16-bits big-endian */
+ gzip_magic = (src[0] << 8);
+ gzip_magic |= src[1];
+
+ if (lz4_magic == LZ4_LEGACY_MAGIC)
+ ret = LZ4_DECOMPRESS;
+ else if (gzip_magic == GZIP_MAGIC)
+ ret = GZIP_DECOMPRESS;
+
+ return ret;
+}
+
+int android_get_image(const char *label, void **load_buf)
+{
+ bdev_t *bdev;
+ struct bootimg_hdr bootimg;
+ size_t totalsize;
+ int hdr_len, ret = 0;
+ void *buf = NULL;
+
+ bdev = bio_open_by_label(label) ? : bio_open(label);
+ if (!bdev) {
+ LTRACEF_LEVEL(CRITICAL, "Partition [%s] is not exist.\n", label);
+ return -ENODEV;
+ }
+
+ hdr_len = sizeof(struct bootimg_hdr);
+ if (bio_read(bdev, &bootimg, 0, hdr_len) < hdr_len ) {
+ ret = -EIO;
+ goto closebdev;
+ }
+
+ ret = load_bootinfo_bootimg_hdr(&bootimg);
+ if (ret)
+ goto closebdev;
+
+ totalsize = get_page_sz() + get_ramdisk_sz() + get_dtb_size()
+ + get_kernel_sz();
+ buf = mempool_alloc(totalsize, MEMPOOL_ANY);
+ if (!buf) {
+ ret = -ENOMEM;
+ goto closebdev;
+ }
+
+ if ((size_t)bio_read(bdev, buf, 0, totalsize) < totalsize) {
+ ret = -EIO;
+ goto closebdev;
+ }
+
+ *load_buf = buf;
+
+closebdev:
+ bio_close(bdev);
+ if (ret && buf)
+ mempool_free(buf);
+
+ return ret;
+}
+
+int android_processing_data(const char *image_name, vaddr_t load_data,
+ addr_t target_addr, size_t size)
+{
+ int ret;
+ int algo;
+ vaddr_t load_addr;
+
+ if (size == 0) {
+ LTRACEF_LEVEL(CRITICAL, "%s is NULL\n", image_name);
+ return -EINVAL;
+ }
+
+#if WITH_KERNEL_VM
+ load_addr = (vaddr_t)paddr_to_kvaddr(target_addr);
+#endif
+
+ algo = get_decompress_algo((unsigned char *)load_data);
+ switch(algo) {
+ case LZ4_DECOMPRESS:
+ ret = unlz4((void *)load_data, size - 4, (void *)load_addr);
+ if (ret != LZ4_OK) {
+ LTRACEF_LEVEL(ALWAYS, "[%s] lz4 decompress failure\n",
+ image_name);
+ return -LZ4_FAIL;
+ }
+ /* In lz4 kernel image, the last 4 bytes are the uncompressed
+ * kernel image size.
+ */
+ size = *(uint32_t *)(load_data + size - 4);
+ break;
+ case GZIP_DECOMPRESS:
+ if (strcmp(image_name, "ramdisk")) {
+ LTRACEF_LEVEL(ALWAYS, "[%s] gzip is not suppport\n",
+ image_name);
+ return -EINVAL;
+ }
+ default:
+ memmove((void *)load_addr, (void *)load_data, size);
+ break;
+ }
+
+#if WITH_KERNEL_VM
+ /* always flush cache to PoC */
+ arch_clean_cache_range(load_addr, size);
+#endif
+
+ LTRACEF("[%s] load_addr = 0x%lx\n", image_name, load_addr);
+ LTRACEF("[%s] data = 0x%lx\n", image_name, load_data);
+ LTRACEF("[%s] size = %zu\n", image_name, size);
+
+ return 0;
+}