[Feature]add MT2731_MP2_MR2_SVN388 baseline version
Change-Id: Ief04314834b31e27effab435d3ca8ba33b499059
diff --git a/src/bsp/lk/app/accelerometer/accelerometer.c b/src/bsp/lk/app/accelerometer/accelerometer.c
new file mode 100644
index 0000000..8ae3f73
--- /dev/null
+++ b/src/bsp/lk/app/accelerometer/accelerometer.c
@@ -0,0 +1,54 @@
+/*
+ * Copyright (c) 2008 Travis Geiselbrecht
+ *
+ * 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 <app.h>
+#include <debug.h>
+#include <stdio.h>
+#include <dev/accelerometer.h>
+#include <compiler.h>
+
+
+#if defined(WITH_LIB_CONSOLE)
+#include <lib/console.h>
+
+void read_xyz(void);
+
+STATIC_COMMAND_START
+STATIC_COMMAND("read_xyz", "read xyz vectors", (console_cmd)&read_xyz)
+STATIC_COMMAND_END(accelerometer);
+
+#endif
+
+void read_xyz(void)
+{
+ position_vector_t pos_vector;
+ acc_read_xyz(&pos_vector);
+ printf("X value = %f\n",pos_vector.x);
+ printf("Y value = %f\n",pos_vector.y);
+ printf("Z value = %f\n",pos_vector.z);
+
+}
+
+APP_START(accelerometer)
+ .flags = 0,
+APP_END
+
diff --git a/src/bsp/lk/app/accelerometer/rules.mk b/src/bsp/lk/app/accelerometer/rules.mk
new file mode 100644
index 0000000..2a717b6
--- /dev/null
+++ b/src/bsp/lk/app/accelerometer/rules.mk
@@ -0,0 +1,10 @@
+LOCAL_DIR := $(GET_LOCAL_DIR)
+
+MODULE := $(LOCAL_DIR)
+
+MODULE_SRCS += \
+ $(LOCAL_DIR)/accelerometer.c \
+
+MODULE_ARM_OVERRIDE_SRCS := \
+
+include make/module.mk
diff --git a/src/bsp/lk/app/app.c b/src/bsp/lk/app/app.c
new file mode 100644
index 0000000..259eb2e
--- /dev/null
+++ b/src/bsp/lk/app/app.c
@@ -0,0 +1,69 @@
+/*
+ * Copyright (c) 2009 Travis Geiselbrecht
+ *
+ * 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 <stdio.h>
+#include <app.h>
+#include <kernel/thread.h>
+
+extern const struct app_descriptor __apps_start[];
+extern const struct app_descriptor __apps_end[];
+
+static void start_app(const struct app_descriptor *app);
+
+/* one time setup */
+void apps_init(void)
+{
+ const struct app_descriptor *app;
+
+ /* call all the init routines */
+ for (app = __apps_start; app != __apps_end; app++) {
+ if (app->init)
+ app->init(app);
+ }
+
+ /* start any that want to start on boot */
+ for (app = __apps_start; app != __apps_end; app++) {
+ if (app->entry && (app->flags & APP_FLAG_DONT_START_ON_BOOT) == 0) {
+ start_app(app);
+ }
+ }
+}
+
+static int app_thread_entry(void *arg)
+{
+ const struct app_descriptor *app = (const struct app_descriptor *)arg;
+
+ app->entry(app, NULL);
+
+ return 0;
+}
+
+static void start_app(const struct app_descriptor *app)
+{
+ uint32_t stack_size = (app->flags & APP_FLAG_CUSTOM_STACK_SIZE) ? app->stack_size : DEFAULT_STACK_SIZE;
+
+ printf("starting app %s\n", app->name);
+ thread_t *t = thread_create(app->name, &app_thread_entry, (void *)app, DEFAULT_PRIORITY, stack_size);
+ thread_detach(t);
+ thread_resume(t);
+}
+
diff --git a/src/bsp/lk/app/app.ld b/src/bsp/lk/app/app.ld
new file mode 100644
index 0000000..9f8b32d
--- /dev/null
+++ b/src/bsp/lk/app/app.ld
@@ -0,0 +1,8 @@
+SECTIONS {
+ .apps : {
+ __apps_start = .;
+ KEEP (*(.apps))
+ __apps_end = .;
+ }
+}
+INSERT AFTER .rodata;
diff --git a/src/bsp/lk/app/avbboot/avb.c b/src/bsp/lk/app/avbboot/avb.c
new file mode 100644
index 0000000..50af835
--- /dev/null
+++ b/src/bsp/lk/app/avbboot/avb.c
@@ -0,0 +1,398 @@
+/*
+ * Copyright (c) 2019 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 <lib/bio.h>
+#include <lib/dl_commands.h>
+/* VERIFIED BOOT 2 */
+#include <libavb/libavb.h>
+#include <libavb_ab/libavb_ab.h>
+
+#include <platform/mmc_rpmb.h>
+#include <sys/types.h>
+#include <string.h>
+
+static AvbIOResult mt_read_from_partition(AvbOps* ops,
+ const char* partition,
+ int64_t offset,
+ size_t num_bytes,
+ void* buffer,
+ size_t* out_num_read)
+{
+ bdev_t *bdev;
+ off_t part_size;
+ size_t read_bytes;
+
+ bdev = bio_open_by_label(partition) ? : bio_open(partition);
+ if (!bdev) {
+ dprintf(CRITICAL, "Partition [%s] is not exist.\n", partition);
+ return AVB_IO_RESULT_ERROR_NO_SUCH_PARTITION;
+ }
+
+ part_size = bdev->total_size;
+
+ if (offset < 0) {
+ if (-offset > part_size)
+ {
+ bio_close(bdev);
+ return AVB_IO_RESULT_ERROR_RANGE_OUTSIDE_PARTITION;
+ }
+
+ offset += part_size;
+ }
+
+ if (offset+num_bytes > (uint64_t)part_size)
+ {
+ bio_close(bdev);
+ return AVB_IO_RESULT_ERROR_RANGE_OUTSIDE_PARTITION;
+ }
+
+ read_bytes = bio_read(bdev, buffer, offset, num_bytes);
+
+ if (out_num_read != NULL)
+ *out_num_read = read_bytes;
+
+ bio_close(bdev);
+ return AVB_IO_RESULT_OK;
+}
+
+static AvbIOResult mt_write_to_partition(AvbOps* ops,
+ const char* partition,
+ int64_t offset,
+ size_t num_bytes,
+ const void* buffer)
+{
+ bdev_t *bdev;
+ off_t part_size;
+ size_t write_bytes;
+
+ bdev = bio_open_by_label(partition) ? : bio_open(partition);
+ if (!bdev) {
+ dprintf(CRITICAL, "Partition [%s] is not exist.\n", partition);
+ return AVB_IO_RESULT_ERROR_NO_SUCH_PARTITION;
+ }
+
+ part_size = bdev->total_size;
+
+ if (offset < 0) {
+ if (-offset > part_size)
+ {
+ bio_close(bdev);
+ return AVB_IO_RESULT_ERROR_RANGE_OUTSIDE_PARTITION;
+ }
+
+ offset += part_size;
+ }
+
+ if (offset+num_bytes > (uint64_t)part_size)
+ {
+ bio_close(bdev);
+ return AVB_IO_RESULT_ERROR_RANGE_OUTSIDE_PARTITION;
+ }
+
+ write_bytes = bio_write(bdev, buffer,offset, num_bytes);
+
+ if (write_bytes != num_bytes)
+ {
+ bio_close(bdev);
+ return AVB_IO_RESULT_ERROR_IO;
+ }
+
+ bio_close(bdev);
+ return AVB_IO_RESULT_OK;
+}
+
+#ifdef AVB_ENABLE_ANTIROLLBACK
+typedef struct {
+ uint64_t rollback_indexes[AVB_MAX_NUMBER_OF_ROLLBACK_INDEX_LOCATIONS];
+} rollback_indexes_t;
+#define VERIDX_IN_PRMB 0
+/*antirollback index is stored in rpmb region block 1*/
+static AvbIOResult mt_read_rollback_index(AvbOps* ops,
+ size_t rollback_index_location,
+ uint64_t* out_rollback_index)
+{
+ int ret=0;
+ unsigned char blk[256] = {0};
+ ret = mmc_rpmb_block_read(VERIDX_IN_PRMB,blk);
+ if(ret != 0 )
+ {
+ *out_rollback_index = 0;
+ return AVB_IO_RESULT_OK;
+ }
+ rollback_indexes_t* indexes = (rollback_indexes_t*)blk;
+ *out_rollback_index=indexes->rollback_indexes[rollback_index_location];
+#if 0
+ dprintf(CRITICAL, "indexes read: %08llx %08llx %08llx %08llx\n",
+ indexes->rollback_indexes[0], indexes->rollback_indexes[1],
+ indexes->rollback_indexes[2], indexes->rollback_indexes[3]);
+#endif
+ return AVB_IO_RESULT_OK;
+}
+
+static AvbIOResult mt_write_rollback_index(AvbOps* ops,
+ size_t rollback_index_location,
+ uint64_t rollback_index)
+{
+ int ret=0;
+ unsigned char blk[256] = {0};
+ ret = mmc_rpmb_block_read(VERIDX_IN_PRMB,blk);
+ if(ret != 0 )
+ {
+ return AVB_IO_RESULT_ERROR_IO;
+ }
+ rollback_indexes_t* indexes = (rollback_indexes_t*)blk;
+ indexes->rollback_indexes[rollback_index_location] = rollback_index;
+#if 0
+ dprintf(CRITICAL, "indexes write: %08llx %08llx %08llx %08llx\n",
+ indexes->rollback_indexes[0], indexes->rollback_indexes[1],
+ indexes->rollback_indexes[2], indexes->rollback_indexes[3]);
+#endif
+ ret = mmc_rpmb_block_write(VERIDX_IN_PRMB,(unsigned char*)indexes);
+ if(ret != 0 )
+ {
+ return AVB_IO_RESULT_ERROR_IO;
+ }
+ return AVB_IO_RESULT_OK;
+}
+#else
+static AvbIOResult mt_read_rollback_index(AvbOps* ops,
+ size_t rollback_index_location,
+ uint64_t* out_rollback_index)
+{
+ *out_rollback_index = 0;
+ return AVB_IO_RESULT_OK;
+}
+
+static AvbIOResult mt_write_rollback_index(AvbOps* ops,
+ size_t rollback_index_location,
+ uint64_t rollback_index)
+{
+ return AVB_IO_RESULT_OK;
+}
+#endif
+
+#define INDEX_RPMB_UNLOCK 1
+#define AVB_DEVICE_UNLOCK 0x5a
+static AvbIOResult mt_read_is_device_unlocked(AvbOps* ops, bool* out_is_unlocked)
+{
+ if (out_is_unlocked != NULL) {
+ *out_is_unlocked = false;
+ }
+
+#if defined(AVB_ENABLE_ANTIROLLBACK) || defined(AVB_ENABLE_DEVICE_STATE_CHANGE)
+ unsigned char blk[256]={0};
+ int ret = -1;
+
+ ret=mmc_rpmb_block_read(INDEX_RPMB_UNLOCK,&blk[0]);
+ if(ret != 0){
+ dprintf(CRITICAL, "mmc_rpmb_block_read fail %d.\n", ret);
+ return AVB_IO_RESULT_ERROR_IO;
+ }
+
+ if(blk[0] == AVB_DEVICE_UNLOCK)
+ *out_is_unlocked = true;
+#endif
+
+ dprintf(CRITICAL, "mt_read_is_device_unlocked return: %d.\n", *out_is_unlocked);
+
+ return AVB_IO_RESULT_OK;
+}
+
+static AvbIOResult mt_get_unique_guid_for_partition(AvbOps* ops,
+ const char* partition,
+ char* guid_buf,
+ size_t guid_buf_size)
+{
+ bdev_t *bdev;
+
+ bdev = bio_open_by_label(partition) ? : bio_open(partition);
+ if (!bdev) {
+ dprintf(CRITICAL, "Partition [%s] is not exist.\n", partition);
+ return AVB_IO_RESULT_ERROR_NO_SUCH_PARTITION;
+ }
+
+ strlcpy(guid_buf, bdev->unique_uuid, guid_buf_size);
+
+ bio_close(bdev);
+ return AVB_IO_RESULT_OK;
+}
+
+#include <libfdt.h>
+#include <image.h>
+extern const unsigned char blob[];
+static const unsigned char* get_pubkey_in_blob(void)
+{
+ int sig_node;
+ int noffset;
+ const void *sig_blob=&blob[0];
+ sig_node = fdt_subnode_offset(sig_blob, 0, FDT_SIG_NODE);
+ for (noffset = fdt_first_subnode(sig_blob, sig_node);
+ noffset >= 0;
+ noffset = fdt_next_subnode(sig_blob, noffset))
+ {
+ return (const unsigned char*)fdt_getprop(sig_blob, noffset, BLOB_MOD_NODE, NULL);
+ }
+ return NULL;
+}
+
+static AvbIOResult mt_validate_vbmeta_public_key(AvbOps* ops,
+ const uint8_t* public_key_data,
+ size_t public_key_length,
+ const uint8_t* public_key_metadata,
+ size_t public_key_metadata_length,
+ bool* out_is_trusted)
+{
+ unsigned char* pubkey_in_blob;
+ unsigned char* pubkey_in_footer;
+ AvbRSAPublicKeyHeader* header;
+
+ header = (AvbRSAPublicKeyHeader*)public_key_data;
+ pubkey_in_blob = (unsigned char*)get_pubkey_in_blob();
+ pubkey_in_footer = (unsigned char*)(public_key_data+sizeof(AvbRSAPublicKeyHeader));
+ if(memcmp(pubkey_in_blob,pubkey_in_footer,avb_be32toh(header->key_num_bits)/32)==0)
+ {
+ *out_is_trusted = true;
+ }
+ else
+ {
+ *out_is_trusted = false;
+ }
+
+ return AVB_IO_RESULT_OK;
+}
+
+static AvbOps avbops;
+
+static AvbABOps avbabops = {
+ .ops = &avbops,
+
+ .read_ab_metadata = avb_ab_data_read,
+ .write_ab_metadata = avb_ab_data_write,
+};
+
+
+static AvbOps avbops = {
+ .ab_ops = &avbabops,
+
+ .read_from_partition = mt_read_from_partition,
+ .write_to_partition = mt_write_to_partition,
+ .validate_vbmeta_public_key = mt_validate_vbmeta_public_key,
+ .read_rollback_index = mt_read_rollback_index,
+ .write_rollback_index = mt_write_rollback_index,
+ .read_is_device_unlocked = mt_read_is_device_unlocked,
+ .get_unique_guid_for_partition = mt_get_unique_guid_for_partition,
+};
+
+bool is_device_unlocked(void)
+{
+ AvbIOResult io_ret;
+ bool is_device_unlocked;
+ io_ret = avbops.read_is_device_unlocked(&avbops, &is_device_unlocked);
+ if (io_ret != AVB_IO_RESULT_OK) {
+ //any error treat as locked
+ return false;
+ }
+ return is_device_unlocked;
+}
+
+void* get_partition_data(char* part_name,AvbSlotVerifyData* verifyData)
+{
+ size_t i=0;
+
+ for(;i<verifyData->num_loaded_partitions;i++)
+ {
+ if(avb_strcmp(part_name,verifyData->loaded_partitions[i].partition_name)==0)
+ {
+ return verifyData->loaded_partitions[i].data;
+ }
+ }
+ return NULL;
+}
+
+AvbSlotVerifyResult avb_update_rollback_indexes(AvbOps* ops,AvbSlotVerifyData* verifyData)
+{
+ size_t n = 0;
+ AvbIOResult io_ret;
+ AvbSlotVerifyResult ret = AVB_SLOT_VERIFY_RESULT_OK;
+ /* Update stored rollback index such that the stored rollback index
+ * is the largest value supporting all currently bootable slots. Do
+ * this for every rollback index location.
+ */
+ for (n = 0; n < AVB_MAX_NUMBER_OF_ROLLBACK_INDEX_LOCATIONS; n++) {
+ uint64_t rollback_index_value = 0;
+
+ rollback_index_value = verifyData->rollback_indexes[n];
+ if (rollback_index_value != 0) {
+ uint64_t current_rollback_index_value;
+ io_ret = ops->read_rollback_index(ops, n, ¤t_rollback_index_value);
+ if (io_ret == AVB_IO_RESULT_ERROR_OOM) {
+ ret = AVB_SLOT_VERIFY_RESULT_ERROR_OOM;
+ goto out;
+ } else if (io_ret != AVB_IO_RESULT_OK) {
+ avb_error("Error getting rollback index for slot.\n");
+ ret = AVB_SLOT_VERIFY_RESULT_ERROR_OOM;
+ goto out;
+ }
+ if (current_rollback_index_value != rollback_index_value) {
+ io_ret = ops->write_rollback_index(ops, n, rollback_index_value);
+ if (io_ret == AVB_IO_RESULT_ERROR_OOM) {
+ ret = AVB_SLOT_VERIFY_RESULT_ERROR_OOM;
+ goto out;
+ } else if (io_ret != AVB_IO_RESULT_OK) {
+ avb_error("Error setting stored rollback index.\n");
+ ret = AVB_SLOT_VERIFY_RESULT_ERROR_OOM;
+ goto out;
+ }
+ }
+ }
+ }
+out:
+ return ret;
+}
+
+#ifndef AB_OTA_UPDATER
+AvbSlotVerifyResult android_verified_boot_2_0(AvbSlotVerifyData** verifyData)
+{
+ AvbSlotVerifyResult verify_result;
+ const char* requested_partitions[] = {BOOT_PART_NAME,NULL};
+ verify_result = avb_slot_verify(&avbops,requested_partitions,"",is_device_unlocked(),verifyData);
+ dprintf(CRITICAL, "avb boot verification result is %s\n",avb_slot_verify_result_to_string(verify_result));
+ if(verify_result == AVB_SLOT_VERIFY_RESULT_OK)
+ {
+ verify_result = avb_update_rollback_indexes(&avbops,*verifyData);
+ dprintf(CRITICAL, "avb boot rollback indexes result is %s\n",avb_slot_verify_result_to_string(verify_result));
+ }
+ return verify_result;
+}
+#else
+//static AvbSlotVerifyData *slot_data;
+AvbSlotVerifyResult android_verified_boot_2_0(AvbSlotVerifyData** verifyData)
+{
+ AvbABFlowResult ab_result;
+ const char* requested_partitions[] = {"bootimg", NULL};
+ /* ab flow */
+ ab_result = avb_ab_flow(&avbabops, requested_partitions, false, verifyData);
+ dprintf(CRITICAL, "ab_result: %s\n", avb_ab_flow_result_to_string(ab_result));
+
+ return ab_result;
+}
+#endif
diff --git a/src/bsp/lk/app/avbboot/avb.h b/src/bsp/lk/app/avbboot/avb.h
new file mode 100644
index 0000000..7111f09
--- /dev/null
+++ b/src/bsp/lk/app/avbboot/avb.h
@@ -0,0 +1,34 @@
+/*
+ * Copyright (c) 2016 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.
+ */
+#pragma once
+
+#include <libavb/libavb.h>
+#include <libavb_ab/libavb_ab.h>
+#include <sys/types.h>
+
+
+bool is_device_unlocked(void);
+AvbSlotVerifyResult android_verified_boot_2_0(AvbSlotVerifyData** verifyData);
+void* get_partition_data(char* part_name,AvbSlotVerifyData* verifyData);
+
+
diff --git a/src/bsp/lk/app/avbboot/avbboot.c b/src/bsp/lk/app/avbboot/avbboot.c
new file mode 100644
index 0000000..1256866
--- /dev/null
+++ b/src/bsp/lk/app/avbboot/avbboot.c
@@ -0,0 +1,427 @@
+/*
+ * Copyright (c) 2016 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 <app.h>
+#include <assert.h>
+#include <boot_mode.h>
+#include <errno.h>
+#include <libfdt.h>
+#include <kernel/event.h>
+#include <kernel/thread.h>
+#include <kernel/vm.h>
+#include <lib/dl_commands.h>
+#include <lib/mempool.h>
+#include <platform.h>
+#include <platform/mtk_key.h>
+#include <platform/mtk_wdt.h>
+#include <rpmb/include/rpmb_mac.h>
+
+#include "fit.h"
+#include "avb.h"
+
+/* BL33 load and entry point address */
+#define CFG_BL33_LOAD_EP_ADDR (BL33_ADDR)
+#define ERR_ADDR (0xffffffff)
+
+typedef void (*jump32_func_type)(uint32_t addr, uint32_t arg1, uint32_t arg2) __NO_RETURN;
+typedef void (*jump64_func_type)(uint64_t bl31_addr, uint64_t bl33_addr, uint64_t arg1) __NO_RETURN;
+
+struct fit_load_data {
+ char *part_name;
+ char *recovery_part_name;
+ void *buf;
+ u32 boot_mode;
+ ulong kernel_entry;
+ ulong dtb_load;
+ ulong trustedos_entry;
+};
+
+/* global variables, also used in dl_commands.c */
+void *kernel_buf;
+void *tz_buf;
+void *bl33_buf;
+
+#if ARCH_ARM64
+void mtk_sip(uint32_t smc_fid, uint64_t bl31_addr, uint64_t bl33_addr)
+{
+ jump64_func_type jump64_func = (jump64_func_type)bl31_addr;
+ (*jump64_func)(bl31_addr, bl33_addr, 0UL);
+}
+#endif
+
+void prepare_bl2_exit(ulong smc_fid, ulong bl31_addr, ulong bl33_addr, ulong arg1)
+{
+#if ARCH_ARM64
+ /* switch to el3 via smc, and will jump to mtk_sip from smc handler */
+ __asm__ volatile("smc #0\n\t");
+#else
+ jump32_func_type jump32_func = (jump32_func_type)bl31_addr;
+ (*jump32_func)(bl33_addr, 0, 0);
+#endif
+}
+
+static void setup_bl33(uint *bl33, ulong fdt, ulong kernel_ep)
+{
+ bl33[12] = (ulong)fdt;
+ bl33[14] = (unsigned)0;
+ bl33[16] = (unsigned)0;
+ bl33[18] = (unsigned)0;
+ bl33[20] = (ulong)kernel_ep;
+ bl33[21] = (ulong)0;
+ bl33[22] = (unsigned)MACH_TYPE;
+}
+
+static int extract_fdt(void *fdt, int size)
+{
+ int ret = 0;
+
+ /* DTB maximum size is 2MB */
+ ret = fdt_open_into(fdt, fdt, size);
+ if (ret) {
+ dprintf(CRITICAL, "open fdt failed\n");
+ return ret;
+ }
+ ret = fdt_check_header(fdt);
+ if (ret) {
+ dprintf(CRITICAL, "check fdt failed\n");
+ return ret;
+ }
+
+ return ret;
+}
+
+static bool check_uart_enter(void)
+{
+ char c;
+ platform_dgetc(&c, false);
+ return (c == 13);
+}
+
+static bool download_check(void)
+{
+ if (check_fastboot_mode()) {
+ set_clr_fastboot_mode(false);
+ return true;
+ } else {
+ return (check_uart_enter() || check_download_key());
+ }
+}
+
+static bool recovery_check(void)
+{
+ if (check_recovery_mode()) {
+ set_clr_recovery_mode(false);
+ return true;
+ } else
+ return false;
+}
+
+static int fit_load_images(void *fit, struct fit_load_data *fit_data,bool need_verified)
+{
+ int ret;
+
+ /* TODO: decide verify policy with config. */
+ dprintf(CRITICAL, "verify fit conf sig: %s\n", fit_data->part_name);
+ if (need_verified) {
+ ret = fit_conf_verify_sig(NULL, fit);
+ if (ret < 0)
+ return ret;
+ }
+
+ ret = fit_load_image(NULL, "kernel", fit, NULL, NULL,
+ (paddr_t *)&fit_data->kernel_entry, need_verified);
+ if (ret && (ret != -ENOENT)) {
+ dprintf(CRITICAL, "%s load kernel failed\n", fit_data->part_name);
+ return ret;
+ }
+
+ ret = fit_load_image(NULL, "tee", fit, NULL, NULL,
+ (paddr_t *)&fit_data->trustedos_entry, need_verified);
+ if (ret && (ret != -ENOENT)) {
+ dprintf(CRITICAL, "%s load trustedos failed\n", fit_data->part_name);
+ return ret;
+ }
+
+ ret = fit_load_image(NULL, "ramdisk", fit, NULL, NULL, NULL, need_verified);
+ if (ret && (ret != -ENOENT)) {
+ dprintf(CRITICAL, "%s load ramdisk failed\n", fit_data->part_name);
+ return ret;
+ }
+
+ ret = fit_load_image(NULL, "fdt", fit, (addr_t *)&fit_data->dtb_load, NULL,
+ NULL, need_verified);
+ if (ret && (ret != -ENOENT)) {
+ fit_data->dtb_load = ERR_ADDR;
+ dprintf(CRITICAL, "%s load fdt failed\n", fit_data->part_name);
+ return ret;
+ }
+
+ return 0;
+}
+
+
+static int fit_load_thread(void *arg,bool need_veriried)
+{
+ int err;
+ struct fit_load_data *fit_data = (struct fit_load_data *)arg;
+
+ if (fit_data->boot_mode == FASTBOOT_BOOT) {
+ err = fit_load_images(fit_data->buf, fit_data, need_veriried);
+ return err;
+ }
+
+ while (fit_data->boot_mode == NORMAL_BOOT) {
+ err = fit_get_image(fit_data->part_name, &fit_data->buf);
+ if (err)
+ break;
+
+ err = fit_load_images(fit_data->buf, fit_data, need_veriried);
+ if (err)
+ break;
+
+ return 0;
+ }
+
+ dprintf(CRITICAL, "%s try recovery mode !!\n", fit_data->recovery_part_name);
+ // RECOVERY_BOOT
+ err = fit_get_image(fit_data->recovery_part_name, &fit_data->buf);
+ if (err)
+ return err;
+
+ err = fit_load_images(fit_data->buf, fit_data, need_veriried);
+
+ return err;
+}
+
+#define MAX_CMDLINE_LENGTH 2048
+static int cmdlineoverlay(void *boot_dtb, char *cmdline, int len, char *ab_suffix)
+{
+ int chosen_node_offset = 0;
+ int ret = -1;
+ char *new_cmd;
+
+ new_cmd = mempool_alloc(MAX_CMDLINE_LENGTH,MEMPOOL_ANY);
+ memset(new_cmd,0,MAX_CMDLINE_LENGTH);
+
+ ret = extract_fdt(boot_dtb, MAX_DTB_SIZE);
+ if (ret != 0) {
+ dprintf(CRITICAL, "extract_fdt error.\n");
+ return -1;
+ }
+
+ chosen_node_offset = fdt_path_offset(boot_dtb, "/chosen");
+
+ const char *cmdline_read;
+ int length;
+ cmdline_read = fdt_getprop(boot_dtb, chosen_node_offset, "bootargs", &length);
+
+ memcpy(new_cmd,cmdline_read,strlen(cmdline_read));
+ new_cmd[strlen(cmdline_read)] = ' ';
+ memcpy(new_cmd+strlen(cmdline_read)+1,cmdline,strlen(cmdline));
+
+#ifdef AB_OTA_UPDATER
+ new_cmd[strlen(new_cmd)] = ' ';
+ memcpy(new_cmd+strlen(new_cmd),"androidboot.slot=",strlen("androidboot.slot="));
+ memcpy(new_cmd+strlen(new_cmd),ab_suffix+1,1);
+#else
+ //unused ab_suffix;
+#endif
+
+ dprintf(INFO, "new cmdline: %s ,length:%lu\n", new_cmd, strlen(new_cmd));
+
+ ret = fdt_setprop(boot_dtb, chosen_node_offset, "bootargs", new_cmd, strlen(new_cmd) + 1);
+
+ if (ret != 0) {
+ dprintf(CRITICAL, "fdt_setprop error.\n");
+ return -1;
+ }
+
+ ret = fdt_pack(boot_dtb);
+ if (ret != 0) {
+ dprintf(CRITICAL, "fdt_pack error.\n");
+ return -1;
+ }
+
+ return 0;
+}
+
+extern void ext_boot(void);
+static void avbboot_task(const struct app_descriptor *app, void *args)
+{
+ void *fit, *dtbo_buf;
+ struct fit_load_data tz,bootimg;
+ int ret_tz;
+ int ret;
+ u32 boot_mode = NORMAL_BOOT;
+ AvbSlotVerifyResult verify_result;
+ AvbSlotVerifyData *verify_data;
+
+ uint bl33[] = { 0xea000005, /* b BL33_32_ENTRY | ands x5, x0, x0 */
+ 0x58000160, /* .word 0x58000160 | ldr x0, _X0 */
+ 0x58000181, /* .word 0x58000181 | ldr x1, _X1 */
+ 0x580001a2, /* .word 0x580001a2 | ldr x2, _X2 */
+ 0x580001c3, /* .word 0x580001c3 | ldr x3, _X3 */
+ 0x580001e4, /* .word 0x580001e4 | ldr x4, _X4 */
+ 0xd61f0080, /* .word 0xd61f0080 | br x4 */
+ /* BL33_32_ENTRY: | */
+ 0xe59f0030, /* ldr r0, _R0 | .word 0xe59f0030 */
+ 0xe59f1030, /* ldr r1, _R1 | .word 0xe59f1030 */
+ 0xe59f2004, /* ldr r2, _X0 | .word 0xe59f2004 */
+ 0xe59ff020, /* ldr pc, _X4 | .word 0xe59ff020 */
+ 0x00000000, /* .word 0x00000000 */
+ 0x00000000, /* _X0: .word 0x00000000 */
+ 0x00000000, /* .word 0x00000000 */
+ 0x00000000, /* _X1: .word 0x00000000 */
+ 0x00000000, /* .word 0x00000000 */
+ 0x00000000, /* _X2: .word 0x00000000 */
+ 0x00000000, /* .word 0x00000000 */
+ 0x00000000, /* _X3: .word 0x00000000 */
+ 0x00000000, /* .word 0x00000000 */
+ 0x00000000, /* _X4: .word 0x00000000 */
+ 0x00000000, /* _R0: .word 0x00000000 */
+ 0x00000000, /* _R1: .word 0x00000000 */
+ 0x00000000 /* .word 0x00000000 */
+ };
+
+#if (defined AVB_ENABLE_ANTIROLLBACK) || (defined AVB_ENABLE_DEVICE_STATE_CHANGE)
+ rpmb_init();
+#endif
+
+ /* recovery */
+ if (recovery_check()) {
+ boot_mode = RECOVERY_BOOT;
+ }
+
+ /* fastboot */
+ if (download_check()) {
+ ext_boot();
+ boot_mode = FASTBOOT_BOOT;
+ }
+
+ tz_buf = mempool_alloc(MAX_TEE_DRAM_SIZE, MEMPOOL_ANY);
+ if (!tz_buf) {
+ dprintf(CRITICAL, "alloc buf fail, kernel %p, tz %p\n",
+ kernel_buf, tz_buf);
+ return;
+ }
+
+ tz.part_name = (char *)TZ_PART_NAME;
+ tz.recovery_part_name = (char *)RECOVERY_TZ_PART_NAME;
+ tz.boot_mode = boot_mode;
+ tz.buf = tz_buf;
+
+ ret_tz = fit_load_thread(&tz, true);
+
+ if (ret_tz) {
+ dprintf(CRITICAL, "load tz image failed\n");
+ return;
+ }
+
+ verify_result = android_verified_boot_2_0(&verify_data);
+ if (verify_result == AVB_SLOT_VERIFY_RESULT_OK || is_device_unlocked()) {
+ bootimg.part_name = (char *)BOOT_PART_NAME;
+ fit = get_partition_data(bootimg.part_name, verify_data);
+ ret = fit_load_images(fit, &bootimg, false);
+ if (ret) {
+ dprintf(CRITICAL, "load boot image failed\n");
+ return;
+ }
+
+ if (cmdlineoverlay((void *)bootimg.dtb_load,verify_data->cmdline,strlen(verify_data->cmdline),verify_data->ab_suffix)) {
+ dprintf(CRITICAL, "cmdline overlay fail\n");
+ return;
+ }
+ } else {
+ bootimg.recovery_part_name = (char *)RECOVERY_BOOT_PART_NAME;
+ bootimg.boot_mode = RECOVERY_BOOT;
+ bootimg.buf = mempool_alloc(MAX_KERNEL_SIZE, MEMPOOL_ANY);
+ ret = fit_load_thread(&bootimg, true);
+ if (ret) {
+ dprintf(CRITICAL, "load recovery image failed\n");
+ return;
+ }
+ }
+
+ dtbo_buf = mempool_alloc(MAX_DTBO_SIZE, MEMPOOL_ANY);
+ if (!dtbo_buf) {
+ dprintf(CRITICAL, "alloc dtbo buf fail\n");
+ return;
+ }
+
+ /* check if dtbo is existed */
+ ret = fit_get_image(DTBO_PART_NAME, &dtbo_buf);
+ if (ret == 0) {
+ void *fdt_dtbo;
+ void *fdt_dtb;
+
+ if (bootimg.dtb_load == ERR_ADDR) {
+ dprintf(CRITICAL, "dtbo failed, no dtb\n");
+ return;
+ }
+ fdt_dtb = (void *)bootimg.dtb_load;
+
+ /* extract fdt */
+ ret = extract_fdt(fdt_dtb, MAX_DTB_SIZE);
+ if (ret) {
+ dprintf(CRITICAL, "extract fdt failed\n");
+ return;
+ }
+
+ dprintf(ALWAYS, "[fitboot] do overlay\n");
+ fdt_dtbo = (void *)dtbo_buf;
+ ret = fdt_overlay_apply(fdt_dtb, fdt_dtbo);
+ if (ret) {
+ dprintf(CRITICAL, "fdt merge failed, ret %d\n", ret);
+ return;
+ }
+
+ /* pack fdt */
+ ret = fdt_pack(fdt_dtb);
+ if (ret) {
+ dprintf(CRITICAL, "ft pack failed\n");
+ return;
+ }
+ }
+
+ /* load bl33 for tz to jump*/
+ extern __WEAK paddr_t kvaddr_to_paddr(void *ptr);
+ addr_t fdt_pa = kvaddr_to_paddr?kvaddr_to_paddr((void *)bootimg.dtb_load):bootimg.dtb_load;
+ setup_bl33(bl33, fdt_pa, (uint)(bootimg.kernel_entry));
+ memmove((void *)CFG_BL33_LOAD_EP_ADDR, bl33, sizeof(bl33));
+
+ ulong bl33_pa = CFG_BL33_LOAD_EP_ADDR;
+ ulong smc_fid = 0xc2000000UL; /* only used in ARCH_ARM64 */
+
+#if ARCH_ARM64 && WITH_KERNEL_VM
+ /* 64-bit LK use non identity mapping VA, VA to PA translation needed */
+ bl33_pa = (ulong)kvaddr_to_paddr((void *)CFG_BL33_LOAD_EP_ADDR);
+#endif
+ dprintf(ALWAYS, "LK run time: %lld (us)\n", current_time_hires());
+ dprintf(ALWAYS, "jump to tz %p\n", (void *)tz.kernel_entry);
+ arch_chain_load((void *)prepare_bl2_exit, smc_fid, tz.kernel_entry, bl33_pa, 0UL);
+}
+
+APP_START(avbboot)
+.entry = avbboot_task,
+ .flags = 0,
+ APP_END
diff --git a/src/bsp/lk/app/avbboot/rules.mk b/src/bsp/lk/app/avbboot/rules.mk
new file mode 100644
index 0000000..cd9c842
--- /dev/null
+++ b/src/bsp/lk/app/avbboot/rules.mk
@@ -0,0 +1,47 @@
+LOCAL_DIR := $(GET_LOCAL_DIR)
+
+MODULE := $(LOCAL_DIR)
+MODULE_BUILDDIR := $(call TOBUILDDIR,$(MODULE))
+MODULE_INCLUDES += $(MODULE_BUILDDIR)/../../../include lib
+
+
+SCRATCH_SIZE ?= 0x04000000 # 64MB
+MAX_TEE_DRAM_SIZE ?= 0x04000000 # 64MB
+MAX_KERNEL_SIZE ?= 0x02000000 # 32MB
+MAX_DTB_SIZE ?= 0x00200000 # 2MB
+MAX_DTBO_SIZE ?= 0x00200000 # 2MB
+MAX_BL33_SIZE ?= 0x00100000 # 1MB
+MAX_LZ4_BUF_SIZE ?= 0x00100000 # 1MB
+
+GLOBAL_DEFINES += SCRATCH_SIZE=$(SCRATCH_SIZE) \
+ MAX_TEE_DRAM_SIZE=$(MAX_TEE_DRAM_SIZE) \
+ MAX_KERNEL_SIZE=$(MAX_KERNEL_SIZE) \
+ MAX_DTB_SIZE=$(MAX_DTB_SIZE) \
+ MAX_DTBO_SIZE=$(MAX_DTBO_SIZE) \
+ MAX_BL33_SIZE=$(MAX_BL33_SIZE) \
+ MAX_LZ4_BUF_SIZE=$(MAX_LZ4_BUF_SIZE)
+
+ifeq ($(strip $(AVB_ENABLE_ANTIROLLBACK)),yes)
+GLOBAL_COMPILEFLAGS += -DAVB_ENABLE_ANTIROLLBACK
+endif
+
+ifeq ($(strip $(AB_OTA_UPDATER)),yes)
+GLOBAL_COMPILEFLAGS += -DAB_OTA_UPDATER
+endif
+
+MODULE_DEPS += \
+ lib/bio \
+ lib/mempool \
+ lib/fdt \
+ lib/fit \
+ lib/fastboot \
+ lib/libavb \
+ lib/libavb_ab \
+
+MODULE_SRCS += \
+ $(LOCAL_DIR)/avbboot.c \
+ $(LOCAL_DIR)/avb.c \
+
+GLOBAL_COMPILEFLAGS += -Os
+
+include make/module.mk
diff --git a/src/bsp/lk/app/blxboot/avb.c b/src/bsp/lk/app/blxboot/avb.c
new file mode 100644
index 0000000..fbeea72
--- /dev/null
+++ b/src/bsp/lk/app/blxboot/avb.c
@@ -0,0 +1,406 @@
+/*
+ * Copyright (c) 2019 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 <lib/bio.h>
+#include <lib/dl_commands.h>
+/* VERIFIED BOOT 2 */
+#include <libavb/libavb.h>
+#include <libavb_ab/libavb_ab.h>
+
+#include <platform/mmc_rpmb.h>
+#include <sys/types.h>
+#include <string.h>
+
+#include "blxboot_ab.h"
+
+static AvbIOResult mt_read_from_partition(AvbOps *ops,
+ const char *partition,
+ int64_t offset,
+ size_t num_bytes,
+ void *buffer,
+ size_t *out_num_read)
+{
+ bdev_t *bdev;
+ off_t part_size;
+ size_t read_bytes;
+
+ bdev = bio_open_by_label(partition) ? : bio_open(partition);
+ if (!bdev) {
+ dprintf(CRITICAL, "Partition [%s] is not exist.\n", partition);
+ return AVB_IO_RESULT_ERROR_NO_SUCH_PARTITION;
+ }
+
+ part_size = bdev->total_size;
+
+ if (offset < 0) {
+ if (-offset > part_size) {
+ bio_close(bdev);
+ return AVB_IO_RESULT_ERROR_RANGE_OUTSIDE_PARTITION;
+ }
+
+ offset += part_size;
+ }
+
+ if (offset+num_bytes > (uint64_t)part_size) {
+ bio_close(bdev);
+ return AVB_IO_RESULT_ERROR_RANGE_OUTSIDE_PARTITION;
+ }
+
+ read_bytes = bio_read(bdev, buffer, offset, num_bytes);
+
+ if (out_num_read != NULL)
+ *out_num_read = read_bytes;
+
+ bio_close(bdev);
+ return AVB_IO_RESULT_OK;
+}
+
+static AvbIOResult mt_write_to_partition(AvbOps *ops,
+ const char *partition,
+ int64_t offset,
+ size_t num_bytes,
+ const void *buffer)
+{
+ bdev_t *bdev;
+ off_t part_size;
+ size_t write_bytes;
+
+ bdev = bio_open_by_label(partition) ? : bio_open(partition);
+ if (!bdev) {
+ dprintf(CRITICAL, "Partition [%s] is not exist.\n", partition);
+ return AVB_IO_RESULT_ERROR_NO_SUCH_PARTITION;
+ }
+
+ part_size = bdev->total_size;
+
+ if (offset < 0) {
+ if (-offset > part_size) {
+ bio_close(bdev);
+ return AVB_IO_RESULT_ERROR_RANGE_OUTSIDE_PARTITION;
+ }
+
+ offset += part_size;
+ }
+
+ if (offset+num_bytes > (uint64_t)part_size) {
+ bio_close(bdev);
+ return AVB_IO_RESULT_ERROR_RANGE_OUTSIDE_PARTITION;
+ }
+
+ write_bytes = bio_write(bdev, buffer,offset, num_bytes);
+
+ if (write_bytes != num_bytes) {
+ bio_close(bdev);
+ return AVB_IO_RESULT_ERROR_IO;
+ }
+
+ bio_close(bdev);
+ return AVB_IO_RESULT_OK;
+}
+
+#ifdef AVB_ENABLE_ANTIROLLBACK
+typedef struct {
+ uint64_t rollback_indexes[AVB_MAX_NUMBER_OF_ROLLBACK_INDEX_LOCATIONS];
+} rollback_indexes_t;
+#define VERIDX_IN_PRMB 0
+/*antirollback index is stored in rpmb region block 1*/
+static AvbIOResult mt_read_rollback_index(AvbOps *ops,
+ size_t rollback_index_location,
+ uint64_t *out_rollback_index)
+{
+ int ret=0;
+ unsigned char blk[256] = {0};
+ ret = mmc_rpmb_block_read(VERIDX_IN_PRMB,blk);
+ if (ret != 0 ) {
+ *out_rollback_index = 0;
+ return AVB_IO_RESULT_OK;
+ }
+ rollback_indexes_t *indexes = (rollback_indexes_t *)blk;
+ *out_rollback_index=indexes->rollback_indexes[rollback_index_location];
+#if 0
+ dprintf(CRITICAL, "indexes read: %08llx %08llx %08llx %08llx\n",
+ indexes->rollback_indexes[0], indexes->rollback_indexes[1],
+ indexes->rollback_indexes[2], indexes->rollback_indexes[3]);
+#endif
+ return AVB_IO_RESULT_OK;
+}
+
+static AvbIOResult mt_write_rollback_index(AvbOps *ops,
+ size_t rollback_index_location,
+ uint64_t rollback_index)
+{
+ int ret=0;
+ unsigned char blk[256] = {0};
+ ret = mmc_rpmb_block_read(VERIDX_IN_PRMB,blk);
+ if (ret != 0 ) {
+ return AVB_IO_RESULT_ERROR_IO;
+ }
+ rollback_indexes_t *indexes = (rollback_indexes_t *)blk;
+ indexes->rollback_indexes[rollback_index_location] = rollback_index;
+#if 0
+ dprintf(CRITICAL, "indexes write: %08llx %08llx %08llx %08llx\n",
+ indexes->rollback_indexes[0], indexes->rollback_indexes[1],
+ indexes->rollback_indexes[2], indexes->rollback_indexes[3]);
+#endif
+ ret = mmc_rpmb_block_write(VERIDX_IN_PRMB,(unsigned char *)indexes);
+ if (ret != 0 ) {
+ return AVB_IO_RESULT_ERROR_IO;
+ }
+ return AVB_IO_RESULT_OK;
+}
+#else
+static AvbIOResult mt_read_rollback_index(AvbOps *ops,
+ size_t rollback_index_location,
+ uint64_t *out_rollback_index)
+{
+ *out_rollback_index = 0;
+ return AVB_IO_RESULT_OK;
+}
+
+static AvbIOResult mt_write_rollback_index(AvbOps *ops,
+ size_t rollback_index_location,
+ uint64_t rollback_index)
+{
+ return AVB_IO_RESULT_OK;
+}
+#endif
+
+#define INDEX_RPMB_UNLOCK 1
+#define AVB_DEVICE_UNLOCK 0x5a
+static AvbIOResult mt_read_is_device_unlocked(AvbOps *ops, bool *out_is_unlocked)
+{
+ if (out_is_unlocked != NULL) {
+ *out_is_unlocked = false;
+ }
+
+#if defined(AVB_ENABLE_ANTIROLLBACK) || defined(AVB_ENABLE_DEVICE_STATE_CHANGE)
+ unsigned char blk[256]= {0};
+ int ret = -1;
+
+ ret=mmc_rpmb_block_read(INDEX_RPMB_UNLOCK,&blk[0]);
+ if (ret != 0) {
+ dprintf(CRITICAL, "mmc_rpmb_block_read fail %d.\n", ret);
+ return AVB_IO_RESULT_ERROR_IO;
+ }
+
+ if (blk[0] == AVB_DEVICE_UNLOCK)
+ *out_is_unlocked = true;
+#endif
+
+ dprintf(CRITICAL, "mt_read_is_device_unlocked return: %d.\n", *out_is_unlocked);
+
+ return AVB_IO_RESULT_OK;
+}
+
+static AvbIOResult mt_get_unique_node_for_partition(AvbOps *ops,
+ const char *partition,
+ char *guid_buf,
+ size_t guid_buf_size)
+{
+
+ strlcpy(guid_buf, "/dev/ubiblock0_0", guid_buf_size);
+ return AVB_IO_RESULT_OK;
+
+}
+static AvbIOResult mt_get_unique_guid_for_partition(AvbOps *ops,
+ const char *partition,
+ char *guid_buf,
+ size_t guid_buf_size)
+{
+ bdev_t *bdev;
+
+ bdev = bio_open_by_label(partition) ? : bio_open(partition);
+ if (!bdev) {
+ dprintf(CRITICAL, "Partition [%s] is not exist.\n", partition);
+ return AVB_IO_RESULT_ERROR_NO_SUCH_PARTITION;
+ }
+
+ strlcpy(guid_buf, bdev->unique_uuid, guid_buf_size);
+ bio_close(bdev);
+ return AVB_IO_RESULT_OK;
+}
+
+#include <libfdt.h>
+#include <image.h>
+extern const unsigned char blob[];
+static const unsigned char *get_pubkey_in_blob(void)
+{
+ int sig_node;
+ int noffset;
+ const void *sig_blob=&blob[0];
+ sig_node = fdt_subnode_offset(sig_blob, 0, FDT_SIG_NODE);
+ for (noffset = fdt_first_subnode(sig_blob, sig_node);
+ noffset >= 0;
+ noffset = fdt_next_subnode(sig_blob, noffset)) {
+ return (const unsigned char *)fdt_getprop(sig_blob, noffset, BLOB_MOD_NODE, NULL);
+ }
+ return NULL;
+}
+
+static AvbIOResult mt_validate_vbmeta_public_key(AvbOps *ops,
+ const uint8_t *public_key_data,
+ size_t public_key_length,
+ const uint8_t *public_key_metadata,
+ size_t public_key_metadata_length,
+ bool *out_is_trusted)
+{
+ unsigned char *pubkey_in_blob;
+ unsigned char *pubkey_in_footer;
+ AvbRSAPublicKeyHeader *header;
+
+ header = (AvbRSAPublicKeyHeader *)public_key_data;
+ pubkey_in_blob = (unsigned char *)get_pubkey_in_blob();
+ if (!pubkey_in_blob) {
+ avb_error("Public key is NULL in blob\n");
+ return AVB_IO_RESULT_ERROR_NO_SUCH_VALUE;
+ }
+ pubkey_in_footer = (unsigned char *)(public_key_data+sizeof(AvbRSAPublicKeyHeader));
+ if (memcmp(pubkey_in_blob,pubkey_in_footer,avb_be32toh(header->key_num_bits)/32)==0) {
+ *out_is_trusted = true;
+ } else {
+ *out_is_trusted = false;
+ }
+
+ return AVB_IO_RESULT_OK;
+}
+
+static AvbOps avbops;
+
+static AvbABOps avbabops = {
+ .ops = &avbops,
+
+ .read_ab_metadata = avb_ab_data_read,
+ .write_ab_metadata = avb_ab_data_write,
+};
+
+static AvbIOResult mt_get_size_of_partition(AvbOps *ops,
+ const char *partition,
+ uint64_t *out_size_num_bytes)
+{
+ bdev_t *bdev;
+
+ bdev = bio_open_by_label(partition) ? : bio_open(partition);
+ *out_size_num_bytes = (uint64_t)bdev->total_size;
+ return AVB_IO_RESULT_OK;
+}
+
+static AvbOps avbops = {
+ .ab_ops = &avbabops,
+
+ .read_from_partition = mt_read_from_partition,
+ .write_to_partition = mt_write_to_partition,
+ .validate_vbmeta_public_key = mt_validate_vbmeta_public_key,
+ .read_rollback_index = mt_read_rollback_index,
+ .write_rollback_index = mt_write_rollback_index,
+ .read_is_device_unlocked = mt_read_is_device_unlocked,
+ .get_size_of_partition = mt_get_size_of_partition,
+#ifdef BOOT_DEV_NAND
+ .get_unique_guid_for_partition = mt_get_unique_node_for_partition,
+#else
+ .get_unique_guid_for_partition = mt_get_unique_guid_for_partition,
+#endif
+};
+
+bool is_device_unlocked(void)
+{
+ AvbIOResult io_ret;
+ bool is_device_unlocked;
+ io_ret = avbops.read_is_device_unlocked(&avbops, &is_device_unlocked);
+ if (io_ret != AVB_IO_RESULT_OK) {
+ //any error treat as locked
+ return false;
+ }
+ return is_device_unlocked;
+}
+
+void *get_partition_data(const char *part_name, AvbSlotVerifyData *verifyData)
+{
+ size_t i=0;
+
+ if ((part_name == NULL) || (verifyData == NULL))
+ return NULL;
+
+ for (; i<verifyData->num_loaded_partitions; i++) {
+ if (avb_strcmp(part_name,verifyData->loaded_partitions[i].partition_name)==0) {
+ return verifyData->loaded_partitions[i].data;
+ }
+ }
+ return NULL;
+}
+
+AvbSlotVerifyResult avb_update_rollback_indexes(AvbOps *ops,AvbSlotVerifyData *verifyData)
+{
+ size_t n = 0;
+ AvbIOResult io_ret;
+ AvbSlotVerifyResult ret = AVB_SLOT_VERIFY_RESULT_OK;
+ /* Update stored rollback index such that the stored rollback index
+ * is the largest value supporting all currently bootable slots. Do
+ * this for every rollback index location.
+ */
+ for (n = 0; n < AVB_MAX_NUMBER_OF_ROLLBACK_INDEX_LOCATIONS; n++) {
+ uint64_t rollback_index_value = 0;
+
+ rollback_index_value = verifyData->rollback_indexes[n];
+ if (rollback_index_value != 0) {
+ uint64_t current_rollback_index_value;
+ io_ret = ops->read_rollback_index(ops, n, ¤t_rollback_index_value);
+ if (io_ret == AVB_IO_RESULT_ERROR_OOM) {
+ ret = AVB_SLOT_VERIFY_RESULT_ERROR_OOM;
+ goto out;
+ } else if (io_ret != AVB_IO_RESULT_OK) {
+ avb_error("Error getting rollback index for slot.\n");
+ ret = AVB_SLOT_VERIFY_RESULT_ERROR_OOM;
+ goto out;
+ }
+ if (current_rollback_index_value != rollback_index_value) {
+ io_ret = ops->write_rollback_index(ops, n, rollback_index_value);
+ if (io_ret == AVB_IO_RESULT_ERROR_OOM) {
+ ret = AVB_SLOT_VERIFY_RESULT_ERROR_OOM;
+ goto out;
+ } else if (io_ret != AVB_IO_RESULT_OK) {
+ avb_error("Error setting stored rollback index.\n");
+ ret = AVB_SLOT_VERIFY_RESULT_ERROR_OOM;
+ goto out;
+ }
+ }
+ }
+ }
+out:
+ return ret;
+}
+
+AvbSlotVerifyResult android_verified_boot_2_0(const char *part_name,
+ AvbSlotVerifyData **verifyData)
+{
+ AvbSlotVerifyResult verify_result;
+ const char *requested_partitions[] = { part_name, NULL };
+ const char *ab_suffix = get_suffix() ? : "";
+
+ verify_result = avb_slot_verify(&avbops,requested_partitions,ab_suffix,is_device_unlocked(),AVB_HASHTREE_ERROR_MODE_RESTART,verifyData);
+ dprintf(CRITICAL, "avb boot verification result is %s\n",avb_slot_verify_result_to_string(verify_result));
+ if (verify_result == AVB_SLOT_VERIFY_RESULT_OK) {
+ verify_result = avb_update_rollback_indexes(&avbops,*verifyData);
+ dprintf(CRITICAL, "avb boot rollback indexes result is %s\n",avb_slot_verify_result_to_string(verify_result));
+ }
+ return verify_result;
+}
+
diff --git a/src/bsp/lk/app/blxboot/avb.h b/src/bsp/lk/app/blxboot/avb.h
new file mode 100644
index 0000000..18b3fd7
--- /dev/null
+++ b/src/bsp/lk/app/blxboot/avb.h
@@ -0,0 +1,34 @@
+/*
+ * Copyright (c) 2016 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.
+ */
+#pragma once
+
+#include <libavb/libavb.h>
+#include <libavb_ab/libavb_ab.h>
+#include <sys/types.h>
+
+bool is_device_unlocked(void);
+AvbSlotVerifyResult android_verified_boot_2_0(const char *part_name,
+ AvbSlotVerifyData **verifyData);
+void *get_partition_data(const char *part_name,AvbSlotVerifyData *verifyData);
+
+
diff --git a/src/bsp/lk/app/blxboot/bl2boot.c b/src/bsp/lk/app/blxboot/bl2boot.c
new file mode 100644
index 0000000..0c2a8f6
--- /dev/null
+++ b/src/bsp/lk/app/blxboot/bl2boot.c
@@ -0,0 +1,295 @@
+/*
+ * 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 <boot_mode.h>
+#include <errno.h>
+#include <kernel/vm.h>
+#include <lib/mempool.h>
+#include <libfdt.h>
+#include <platform.h>
+#include <platform/platform_blx.h>
+#include <stdbool.h>
+#include <string.h>
+#include <trace.h>
+
+#include "blx_common.h"
+#include "blxboot_ab.h"
+#include "blxboot_plat.h"
+
+#define LOCAL_TRACE 0
+
+struct bl2boot {
+ struct blxboot commonboot;
+
+ /* keep image data pointer for quick reference */
+ struct image_load_data *bl33imgdata;
+ struct image_load_data *bootimgdata;
+ struct image_load_data *dtboimgdata;
+ struct image_load_data *tzimgdata;
+ struct image_load_data *vpdimgdata;
+};
+
+enum {
+ ARM32_TO_ARM32,
+ ARM32_TO_ARM64,
+ ARM64_TO_ARM32,
+ ARM64_TO_ARM64,
+};
+
+static bool is_arch_arm64(void)
+{
+#if ARCH_ARM64
+ return true;
+#else
+ return false;
+#endif
+}
+
+static bool is_enabled_builtin_bl33(void)
+{
+#if ENABLE_BUILTIN_BL33
+ return true;
+#else
+ return false;
+#endif
+}
+
+static void update_builtin_bl33(struct blxboot *obj)
+{
+ struct bl2boot *bl2obj = (struct bl2boot *)obj;
+ struct image_load_data *bl33imgdata;
+ uint32_t *bl33;
+
+ if (bl2obj == NULL)
+ return;
+
+ bl33imgdata = bl2obj->bl33imgdata;
+ bl33 = (uint32_t *)paddr_to_kvaddr(bl33imgdata->kernel_entry);
+
+ bl33[12] = (ulong)(bl2obj->bootimgdata->dtb_load);
+ bl33[14] = (unsigned)0;
+ bl33[16] = (unsigned)0;
+ bl33[18] = (unsigned)0;
+ bl33[20] = (ulong)(bl2obj->bootimgdata->kernel_entry);
+ bl33[21] = (ulong)0;
+ bl33[22] = (unsigned)MACH_TYPE;
+}
+
+static int arm_exec_mode_transition(void)
+{
+#if ARCH_ARM64
+ /*Assume BL31 is aarch64 in case BL2 is aarch64. No handling BL2 of aarch64
+ and BL33 of aarch32. */
+ return ARM64_TO_ARM64;
+#else
+#if BL2_BOOT_NEXT_64BITS
+ return ARM32_TO_ARM64;
+#else
+ return ARM32_TO_ARM32;
+#endif
+#endif
+}
+
+void mtk_sip(ulong smc_fid, ulong tz_ep, ulong bl33_addr, ulong boot_reason)
+{
+ jump_func_type jump_func = (jump_func_type)tz_ep;
+ (*jump_func)(bl33_addr, boot_reason, 0UL, 0UL);
+}
+
+static void exitfn_smc64_to_next64(ulong smc_fid, ulong tz_ep,
+ ulong bl33_addr, ulong unused0)
+{
+ __asm__ volatile("smc #0\n\t");
+}
+
+static void exitfn_jump32(ulong unused0, ulong tz_ep,
+ ulong bl33_addr, ulong boot_arg)
+{
+ jump_func_type jump_fn = (jump_func_type)tz_ep;
+ (*jump_fn)(bl33_addr, boot_arg, 0, 0);
+}
+
+__WEAK void platform_set_aarch64_reset_vector(ulong vector)
+{
+ /* implementation in platform, or panic. */
+ PANIC_UNIMPLEMENTED;
+}
+
+static void exitfn_warmreset32_to_next64(ulong unused0, ulong tz_ep,
+ ulong bl33_addr, ulong boot_arg)
+{
+ /* set aarch64 reset vector */
+ platform_set_aarch64_reset_vector(tz_ep);
+
+ /* warm reset to aarch64 */
+#if ARCH_ARM
+ uint32_t tmpr;
+
+ __asm__ volatile(
+ "mrc p15, 0, %0, c12, c0, 2\n\t"
+ "orr %0, %0, #1\n\t"
+ "mcr p15, 0, %0, c12, c0, 2\n\t"
+ "mrc p15, 0, %0, c12, c0, 2\n\t"
+ "orr %0, %0, #2\n\t"
+ "mcr p15, 0, %0, c12, c0, 2\n\t"
+ "mov r0, %2\n\t"
+ "mov r1, %3\n\t"
+ "dsb sy\n\t"
+ "isb sy\n\t"
+ "wfi\n\t"
+ : "=r" (tmpr)
+ : "0" (tmpr), "r" (bl33_addr), "r" (boot_arg)
+ );
+#endif
+}
+
+static void notify(struct blxboot *obj, struct imageinfo_t *img)
+{
+ struct image_load_data *imgdata = img->imgdata;
+ struct bl2boot *bl2obj = (struct bl2boot *)obj;
+
+ LTRACEF("%s: %s\n", __func__, imgdata->part_name);
+ switch (img->type) {
+ case IMGTYPE_TZ:
+ bl2obj->tzimgdata = imgdata;
+ break;
+ case IMGTYPE_BL33:
+ case IMGTYPE_BUILTIN_BL33:
+ bl2obj->bl33imgdata = imgdata;
+ break;
+ case IMGTYPE_KERNEL:
+ bl2obj->bootimgdata = imgdata;
+ break;
+ case IMGTYPE_DTBO:
+ bl2obj->dtboimgdata = imgdata;
+ break;
+ case IMGTYPE_VPD:
+ bl2obj->vpdimgdata = imgdata;
+ break;
+ case IMGTYPE_SCPSYS:
+ plat_start_scpsys();
+ LTRACEF("start scpsys@%llx\n", current_time_hires());
+ break;
+ default:
+ break;
+ }
+}
+
+static void fixup_image(struct blxboot *obj, void *fdt_dtb)
+{
+ if (is_enabled_builtin_bl33())
+ update_builtin_bl33(obj);
+}
+
+static void setup_boot_param(struct blxboot *obj, struct boot_param *prm)
+{
+ struct bl2boot *bl2obj = (struct bl2boot *)obj;
+ uint32_t boot_mode;
+
+ prm->arg0 = MTK_SIP_KERNEL_BOOT_AARCH64; /* only for 64bits BL2 */
+ prm->arg1 = bl2obj->tzimgdata->kernel_entry; /* next entry point */
+ prm->arg2 = bl2obj->bl33imgdata->kernel_entry; /* next boot param */
+ /* pass bootargs from bl2 to bl33 */
+ extern void *bl2_set_boot_args(uint32_t boot_mode) __attribute__((weak));
+ boot_mode = obj->bootcfg.boot_mode;
+ prm->arg3 = bl2_set_boot_args ?
+ (ulong)kvaddr_to_paddr(bl2_set_boot_args(boot_mode)) : 0;
+}
+
+static int init(void)
+{
+ int rc;
+
+ rc = check_ab_boot();
+ if ((rc == 0) || (rc == -ENOTSUP))
+ return 0;
+
+ return rc;
+}
+
+static void get_overlay_image(struct blxboot *obj, void **fdt_dtb,
+ void **dtbo, void **vpd)
+{
+ struct bl2boot *bl2obj = (struct bl2boot *)obj;
+
+ assert(obj);
+
+ *fdt_dtb = NULL;
+ *dtbo = NULL;
+ *vpd = NULL;
+ if (!is_enabled_builtin_bl33())
+ return;
+
+ if (bl2obj->bootimgdata)
+ *fdt_dtb = (void *)bl2obj->bootimgdata->dtb_load;
+
+ if (bl2obj->dtboimgdata)
+ *dtbo = (void *)bl2obj->dtboimgdata->dtb_load;
+
+ if (bl2obj->vpdimgdata)
+ *vpd = (void *)bl2obj->vpdimgdata->dtb_load;
+}
+
+struct blxboot *blxboot_create(void)
+{
+ struct bl2boot *bl2obj;
+ struct blxboot *blxobj;
+ struct blxOps *ops;
+ struct blxCfg *cfg;
+
+ bl2obj = mempool_alloc(sizeof(struct bl2boot), MEMPOOL_ANY);
+ if (!bl2obj)
+ return NULL;
+
+ memset(bl2obj, 0, sizeof(struct bl2boot));
+
+ blxobj = (struct blxboot *)bl2obj;
+
+ cfg = &blxobj->bootcfg;
+ cfg->boot_mode = NORMAL_BOOT;
+ cfg->ab_suffix = NULL;
+
+ ops = &blxobj->ops;
+ ops->init = init;
+ ops->notify = notify;
+ ops->fixup_image = fixup_image;
+ ops->setup_boot_param = setup_boot_param;
+ ops->get_overlay_image = get_overlay_image;
+
+ switch (arm_exec_mode_transition()) {
+ case ARM32_TO_ARM32:
+ ops->exit = exitfn_jump32;
+ break;
+ case ARM32_TO_ARM64:
+ ops->exit = exitfn_warmreset32_to_next64;
+ break;
+ case ARM64_TO_ARM32:
+ ops->exit = NULL; /* assert to stop here? */
+ break;
+ case ARM64_TO_ARM64:
+ ops->exit = exitfn_smc64_to_next64;
+ break;
+ }
+
+ return (struct blxboot *)bl2obj;
+}
diff --git a/src/bsp/lk/app/blxboot/bl33boot.c b/src/bsp/lk/app/blxboot/bl33boot.c
new file mode 100644
index 0000000..bfe3815
--- /dev/null
+++ b/src/bsp/lk/app/blxboot/bl33boot.c
@@ -0,0 +1,264 @@
+/*
+ * 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 <boot_mode.h>
+#include <compiler.h>
+#include <debug.h>
+#include <kernel/vm.h>
+#include <lib/mempool.h>
+#include <libfdt.h>
+#include <platform/psci.h>
+#include <stdbool.h>
+#include <string.h>
+#include <trace.h>
+
+#include "blx_common.h"
+#include "blxboot_plat.h"
+
+#define LOCAL_TRACE 0
+
+#ifndef SLAVE_CPU
+#define SLAVE_CPU 3
+#endif
+#define SLAVE_CPU_COLDBOOT_TAG 0x10000001
+
+struct bl33boot {
+ struct blxboot commonboot;
+
+ /* keep image data pointer for quick reference */
+ struct image_load_data *bootimgdata;
+ struct image_load_data *dtboimgdata;
+ struct image_load_data *vpdimgdata;
+};
+
+enum {
+ ARM32_TO_ARM32,
+ ARM32_TO_ARM64,
+ ARM64_TO_ARM32,
+ ARM64_TO_ARM64,
+};
+
+static bool is_next_boot_64bits(void)
+{
+#if BL33_BOOT_NEXT_64BITS
+ return true;
+#else
+ return false;
+#endif
+}
+
+static int arm_exec_mode_transition(void)
+{
+#if ARCH_ARM64
+#if BL33_BOOT_NEXT_64BITS
+ return ARM64_TO_ARM64;
+#else
+ return ARM64_TO_ARM32;
+#endif
+#else
+#if BL33_BOOT_NEXT_64BITS
+ return ARM32_TO_ARM64;
+#else
+ return ARM32_TO_ARM32;
+#endif
+#endif
+}
+
+/* various bl33 exit functions */
+/* 32bits bl33 to 32bits kernel */
+static void exitfn_jump32(ulong unused0, ulong kernel_ep, ulong machtype,
+ ulong fdt_addr)
+{
+ jump_func_type jump_fn = (jump_func_type)kernel_ep;
+ (*jump_fn)(0, machtype, fdt_addr, 0);
+}
+
+/* 64bits bl33 to 32bits kernel */
+static void exitfn_smc64_to_next32(ulong smc_fid, ulong kernel_ep,
+ ulong machtype, ulong fdt_addr)
+{
+ /* x0 ~ x3 already in place, set x4 */
+ __asm__ volatile("mov x4, %0\n\t"
+ "smc #0\n\t"
+ : : "I" (BOOT_AARCH32));
+}
+
+/* 32bits bl33 to 64bits kernel */
+static void exitfn_smc32_to_next64(ulong smc_fid, ulong kernel_ep,
+ ulong fdt_addr, ulong unused)
+{
+ /* w0 ~ w3 already in place, set w4 */
+ __asm__ volatile("mov w4, %0\n\t"
+ "smc #0\n\t"
+ : : "I" (BOOT_AARCH64));
+}
+
+void mtk_sip(ulong smc_fid, ulong kernel_hyp_ep, ulong fdt_addr)
+{
+ jump_func_type jump_fn = (jump_func_type)kernel_hyp_ep;
+ (*jump_fn)(fdt_addr, 0, 0, 0);
+}
+
+/* 64bits bl33 to 64bits kernel or hypervisor */
+static void exitfn_hvc64_to_next64(ulong smc_fid, ulong kernel_hyp_ep,
+ ulong fdt_addr, ulong unused0)
+{
+ __asm__ volatile("hvc #0\n\t");
+}
+
+static void notify(struct blxboot *obj, struct imageinfo_t *img)
+{
+ extern void platform_next_entry(addr_t entry) __attribute__((weak));
+ int ret __UNUSED;
+ paddr_t entry_addr __UNUSED;
+ struct image_load_data *imgdata = img->imgdata;
+ struct bl33boot *bl33obj = (struct bl33boot *)obj;
+
+ LTRACEF("%s: %s\n", __func__, imgdata->part_name);
+ switch (img->type) {
+ case IMGTYPE_KERNEL:
+ bl33obj->bootimgdata = imgdata;
+ break;
+ case IMGTYPE_DTBO:
+ bl33obj->dtboimgdata = imgdata;
+ break;
+ case IMGTYPE_VPD:
+ bl33obj->vpdimgdata = imgdata;
+ break;
+#if ENABLE_SLAVE_CPU_LOAD
+ case IMGTYPE_SLAVE_CPU:
+ if (platform_next_entry) {
+ platform_next_entry(imgdata->kernel_entry);
+ /* entry point for slave CPU */
+ entry_addr = kvaddr_to_paddr ?
+ kvaddr_to_paddr((void *)&platform_next_entry) :
+ ((void *)&platform_next_entry);
+ } else {
+ entry_addr = (paddr_t)imgdata->kernel_entry;
+ }
+
+ ret = psci_cpu_on(SLAVE_CPU, (ulong)entry_addr,
+ SLAVE_CPU_COLDBOOT_TAG);
+ if (ret != E_PSCI_SUCCESS) {
+ /* error handling for adsp cpu on failure */
+ LTRACEF("psci_cpu_on cpu%d fail, err=%d\n", SLAVE_CPU, ret);
+ }
+ break;
+#endif
+ default:
+ break;
+ }
+}
+
+static void setup_kernel32_boot_param(struct blxboot *obj,
+ struct boot_param *prm)
+{
+ struct bl33boot *bl33obj = (struct bl33boot *)obj;
+
+ prm->arg0 = MTK_SIP_KERNEL_BOOT_AARCH32; /* used in smc call */
+ prm->arg1 = bl33obj->bootimgdata->kernel_entry;
+ prm->arg2 = MACH_TYPE;
+ prm->arg3 = kvaddr_to_paddr ?
+ kvaddr_to_paddr((void *)bl33obj->bootimgdata->dtb_load) :
+ bl33obj->bootimgdata->dtb_load;
+}
+
+static void setup_kernel64_boot_param(struct blxboot *obj,
+ struct boot_param *prm)
+{
+ struct bl33boot *bl33obj = (struct bl33boot *)obj;
+
+ prm->arg0 = MTK_SIP_KERNEL_BOOT_AARCH64;
+ prm->arg1 = bl33obj->bootimgdata->kernel_entry;
+ prm->arg2 = kvaddr_to_paddr ?
+ kvaddr_to_paddr((void *)bl33obj->bootimgdata->dtb_load) :
+ bl33obj->bootimgdata->dtb_load;
+ prm->arg3 = 0; /* unused */
+}
+
+static void get_overlay_image(struct blxboot *obj, void **fdt_dtb,
+ void **dtbo, void **vpd)
+{
+ struct bl33boot *bl33obj = (struct bl33boot *)obj;
+
+ assert(obj);
+
+ *fdt_dtb = NULL;
+ *dtbo = NULL;
+ *vpd = NULL;
+
+ if (bl33obj->bootimgdata)
+ *fdt_dtb = (void *)bl33obj->bootimgdata->dtb_load;
+
+ if (bl33obj->dtboimgdata)
+ *dtbo = (void *)bl33obj->dtboimgdata->dtb_load;
+
+ if (bl33obj->vpdimgdata)
+ *vpd = (void *)bl33obj->vpdimgdata->dtb_load;
+}
+
+struct blxboot *blxboot_create(void)
+{
+ struct bl33boot *bl33obj;
+ struct blxboot *blxobj;
+ struct blxOps *ops;
+ struct blxCfg *cfg;
+
+ bl33obj = mempool_alloc(sizeof(struct bl33boot), MEMPOOL_ANY);
+ if (!bl33obj)
+ return NULL;
+
+ memset(bl33obj, 0, sizeof(struct bl33boot));
+
+ blxobj = (struct blxboot *)bl33obj;
+
+ cfg = &blxobj->bootcfg;
+ cfg->boot_mode = NORMAL_BOOT;
+ cfg->ab_suffix = NULL;
+
+ ops = &blxobj->ops;
+ ops->init = NULL;
+ ops->notify = notify;
+ ops->fixup_image = NULL;
+ ops->setup_boot_param = is_next_boot_64bits() ?
+ setup_kernel64_boot_param : setup_kernel32_boot_param;
+ ops->get_overlay_image = get_overlay_image;
+
+ switch (arm_exec_mode_transition()) {
+ case ARM32_TO_ARM32:
+ ops->exit = exitfn_jump32;
+ break;
+ case ARM32_TO_ARM64:
+ ops->exit = exitfn_smc32_to_next64;
+ break;
+ case ARM64_TO_ARM32:
+ ops->exit = exitfn_smc64_to_next32;
+ break;
+ case ARM64_TO_ARM64:
+ ops->exit = exitfn_hvc64_to_next64;
+ break;
+ }
+
+ return (struct blxboot *)bl33obj;
+}
diff --git a/src/bsp/lk/app/blxboot/blx_common.h b/src/bsp/lk/app/blxboot/blx_common.h
new file mode 100644
index 0000000..9921da3
--- /dev/null
+++ b/src/bsp/lk/app/blxboot/blx_common.h
@@ -0,0 +1,70 @@
+/*
+ * 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.
+ */
+
+#pragma once
+
+#include "imageinfo.h"
+
+/* smc function id from ATF MTK SiP service */
+#define MTK_SIP_KERNEL_BOOT_AARCH32 0x82000200
+#define MTK_SIP_KERNEL_BOOT_AARCH64 0xc2000200
+
+#define BOOT_AARCH32 0
+#define BOOT_AARCH64 1
+
+extern __WEAK paddr_t kvaddr_to_paddr(void *ptr);
+
+typedef void (*jump_func_type)(ulong arg0, ulong arg1, ulong arg2,
+ ulong arg3) __NO_RETURN;
+
+struct boot_param {
+ ulong arg0;
+ ulong arg1;
+ ulong arg2;
+ ulong arg3;
+};
+
+struct blxboot;
+
+struct blxOps {
+ int (*init)(void);
+ void (*notify)(struct blxboot *obj, struct imageinfo_t *img);
+ void (*fixup_image)(struct blxboot *obj, void *fdt_dtb);
+ void (*setup_boot_param)(struct blxboot *obj, struct boot_param *prm);
+ void (*exit)(ulong arg0, ulong arg1, ulong arg2, ulong arg3);
+ void (*get_overlay_image)(struct blxboot *obj, void **fdt_dtb,
+ void **dtbo, void **vpd);
+};
+
+struct blxCfg {
+ uint32_t boot_mode;
+ const char *ab_suffix;
+};
+
+struct blxboot {
+ struct blxOps ops;
+ struct blxCfg bootcfg;
+};
+
+struct blxboot *blxboot_create(void);
+
diff --git a/src/bsp/lk/app/blxboot/blxboot.c b/src/bsp/lk/app/blxboot/blxboot.c
new file mode 100644
index 0000000..5b154ef
--- /dev/null
+++ b/src/bsp/lk/app/blxboot/blxboot.c
@@ -0,0 +1,248 @@
+/*
+ * 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 <app.h>
+#include <assert.h>
+#include <boot_mode.h>
+#include <err.h>
+#include <errno.h>
+#include <fit.h>
+#include <kernel/event.h>
+#include <kernel/thread.h>
+#include <kernel/vm.h>
+#include <libfdt.h>
+#include <lib/dl_commands.h> /* [TODO] separate rpmb define from fastboot */
+#include <lib/mempool.h>
+#include <platform.h>
+#include <rpmb/include/rpmb_mac.h> /* [TODO] #include <lib/rpmb_mac.h> */
+#include <trace.h>
+
+#include "blx_common.h"
+#include "blxboot_ab.h"
+#include "blxboot_plat.h"
+#include "dto.h"
+#include "imageinfo.h"
+
+#define LOCAL_TRACE 0
+
+extern void ext_boot(void);
+extern struct imageinfo_t imagelist[];
+
+static int load_all_images(struct blxboot *obj)
+{
+ int rc;
+ int load_idx;
+ struct imageinfo_t *img;
+ const char *part_name[3];
+ uint32_t *boot_mode = &(obj->bootcfg.boot_mode);
+
+ img = &imagelist[0];
+ while (img->type != IMGTYPE_NONE) {
+ LTRACEF("image load: %s\n", img->imgdata->part_name);
+
+ ASSERT((img->load != NULL) && (img->imgdata != NULL));
+
+ /* setup the load retry sequence: fastboot -> normal -> recovery */
+ if (*boot_mode == NORMAL_BOOT)
+ load_idx = 1;
+ else if (*boot_mode == RECOVERY_BOOT)
+ load_idx = 0;
+ else /* FASTBOOT_BOOT */
+ load_idx = 2;
+
+ part_name[2] = img->imgdata->membdev_name;
+ part_name[1] = img->imgdata->part_name;
+ part_name[0] = img->imgdata->recovery_part_name;
+
+ /* for non recovery boot partition, no need to try recovery partition
+ * if its name is the same with normal boot partition */
+ if (*boot_mode != RECOVERY_BOOT) {
+ if ((part_name[0] != NULL) && (part_name[1] != NULL) &&
+ (!strcmp(part_name[0], part_name[1])))
+ part_name[0] = NULL;
+ }
+
+ rc = -ENODEV;
+ while (load_idx >= 0) {
+ if (part_name[load_idx] != NULL) {
+ rc = img->load(part_name[load_idx], img);
+ if (rc == 0)
+ break;
+ }
+
+ load_idx--;
+ }
+
+ if (rc == 0) {
+ if (obj->ops.notify)
+ obj->ops.notify(obj, img);
+ } else {
+ LTRACEF("load part: %s, type=%d failed\n",
+ img->imgdata->part_name, img->type);
+ if ((img->type != IMGTYPE_DTBO) && (img->type != IMGTYPE_VPD))
+ return -1;
+
+ /* dtbo/vpd image: allow partition not exist or no valid data.
+ * Other cases, boot fail */
+ if ((rc != -ENODEV) && (rc != -ENOTSUP))
+ return -1;
+ }
+ img++;
+ }
+
+ return 0;
+}
+
+static void append_suffix(const char **name, const char *suffix, int suffix_len)
+{
+ char *new_name;
+ int len;
+
+ len = strlen(*name) + suffix_len + 1;
+ new_name = mempool_alloc(len, MEMPOOL_ANY);
+ ASSERT(new_name != NULL);
+ sprintf(new_name, "%s%s", *name, suffix);
+ *name = new_name;
+}
+
+static void update_image_partition_name(const char *suffix)
+{
+ struct imageinfo_t *img;
+ struct image_load_data *imgdata;
+ int suffix_len;
+
+ suffix_len = strlen(suffix);
+ for (img = &imagelist[0]; img->type != IMGTYPE_NONE; img++) {
+ ASSERT((img != NULL) && (img->imgdata != NULL));
+ imgdata = img->imgdata;
+ if (!imgdata->has_slot)
+ continue;
+
+ append_suffix(&imgdata->part_name, suffix, suffix_len);
+ if (imgdata->recovery_part_name)
+ append_suffix(&imgdata->recovery_part_name, suffix, suffix_len);
+ }
+}
+
+static void blxboot_task(const struct app_descriptor *app, void *args)
+{
+ struct blxboot *blxobj;
+ struct boot_param bprm;
+ bool fastboot_on_fail;
+ uint32_t *boot_mode;
+ void *fdt_dtb, *dtbo, *vpd;
+ int rc;
+
+#if (defined AVB_ENABLE_ANTIROLLBACK) || (defined AVB_ENABLE_DEVICE_STATE_CHANGE)
+ rpmb_init();
+#endif
+
+ if (plat_fixup_init() != NO_ERROR)
+ return;
+
+ blxobj = blxboot_create();
+ if (!blxobj)
+ return;
+
+ boot_mode = &(blxobj->bootcfg.boot_mode);
+ *boot_mode = get_boot_mode();
+ assert(*boot_mode <= FASTBOOT_BOOT);
+
+ if (blxobj->ops.init) {
+ rc = blxobj->ops.init();
+ /*
+ * if init fails, and boot mode is
+ * - fastboot mode: enter fastboot mode
+ * - non fastboot mode:
+ * . AB OTA updater enabled: reboot,
+ * . non AB OTA updater: enter fastboot mode
+ */
+ if (rc && (*boot_mode != FASTBOOT_BOOT)) {
+ if (is_enabled_ab_ota_updater())
+ platform_halt(HALT_ACTION_REBOOT, HALT_REASON_SW_RESET);
+ else
+ *boot_mode = FASTBOOT_BOOT;
+ }
+ }
+
+ blxobj->bootcfg.ab_suffix = get_suffix();
+ if (blxobj->bootcfg.ab_suffix != NULL)
+ update_image_partition_name(blxobj->bootcfg.ab_suffix);
+
+ if (is_enabled_ab_ota_updater())
+ fastboot_on_fail = false;
+ else
+ fastboot_on_fail = true; /* enter fastboot mode if load fail */
+
+ do {
+ if (*boot_mode == FASTBOOT_BOOT) {
+ ext_boot();
+ fastboot_on_fail = false;
+ }
+
+ if ((rc = load_all_images(blxobj)) == 0)
+ break;
+
+ if (fastboot_on_fail)
+ *boot_mode = FASTBOOT_BOOT;
+ } while (fastboot_on_fail);
+
+ if (rc != 0) {
+ if (is_enabled_ab_ota_updater())
+ platform_halt(HALT_ACTION_REBOOT, HALT_REASON_SW_RESET);
+
+ return;
+ }
+
+ /* dtbo may contains kernel bootargs, do overlay_fdt before fixup_image */
+ blxobj->ops.get_overlay_image(blxobj, &fdt_dtb, &dtbo, &vpd);
+ if (fdt_dtb && (dtbo || vpd)) {
+ rc = overlay_fdt(fdt_dtb, dtbo, vpd);
+ if (rc) {
+ LTRACEF_LEVEL(CRITICAL, "dtbo overlay failed\n");
+ return;
+ }
+ }
+
+ if (blxobj->ops.fixup_image)
+ blxobj->ops.fixup_image(blxobj, fdt_dtb);
+
+ plat_fixup_hook(*boot_mode, blxobj->bootcfg.ab_suffix,
+ fdt_dtb, MAX_DTB_SIZE);
+
+ if (blxobj->ops.setup_boot_param) {
+ blxobj->ops.setup_boot_param(blxobj, &bprm);
+ }
+
+ dprintf(ALWAYS, "LK run time: %lld (us)\n", current_time_hires());
+ dprintf(ALWAYS, "jump to next ep %p, arg (0x%lx)\n",
+ (void *)bprm.arg1, bprm.arg2);
+
+ arch_chain_load((void *)blxobj->ops.exit,
+ bprm.arg0, bprm.arg1, bprm.arg2, bprm.arg3);
+}
+
+APP_START(blxboot)
+.entry = blxboot_task,
+ .flags = 0,
+ APP_END
diff --git a/src/bsp/lk/app/blxboot/blxboot_ab.c b/src/bsp/lk/app/blxboot/blxboot_ab.c
new file mode 100644
index 0000000..c6a3687
--- /dev/null
+++ b/src/bsp/lk/app/blxboot/blxboot_ab.c
@@ -0,0 +1,48 @@
+/*
+ * Copyright (c) 2019 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 <compiler.h>
+#include <errno.h>
+#include <stdbool.h>
+#include <stddef.h>
+
+/* ab related */
+__WEAK int check_ab_boot(void)
+{
+ return -ENOTSUP;
+}
+
+__WEAK const char *get_suffix(void)
+{
+ return NULL;
+}
+
+bool is_enabled_ab_ota_updater(void)
+{
+ /* [todo] remove #if defined(MT2712_ANDROID) when yocto is ready */
+#if defined(MT2712_ANDROID) && defined(AB_OTA_UPDATER)
+ return true;
+#else
+ return false;
+#endif
+}
diff --git a/src/bsp/lk/app/blxboot/blxboot_ab.h b/src/bsp/lk/app/blxboot/blxboot_ab.h
new file mode 100644
index 0000000..41a82e9
--- /dev/null
+++ b/src/bsp/lk/app/blxboot/blxboot_ab.h
@@ -0,0 +1,37 @@
+/*
+ * Copyright (c) 2019 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 <stdbool.h>
+
+/* check_ab_boot() - check ab boot status and do slot switch if necessary
+ * blxboot_ab.c has only weak imp., return -ENOTSUP */
+int check_ab_boot(void);
+
+/* get_suffix() - get current slot suffix, blxboot_ab.c has only weak imp.,
+ * return NULL */
+const char *get_suffix(void);
+
+/*
+ * is_enabled_ab_ota_updater() - check whether AB_OTA_UPDATER feature is enabled
+ */
+bool is_enabled_ab_ota_updater(void);
diff --git a/src/bsp/lk/app/blxboot/blxboot_plat.c b/src/bsp/lk/app/blxboot/blxboot_plat.c
new file mode 100644
index 0000000..623e022
--- /dev/null
+++ b/src/bsp/lk/app/blxboot/blxboot_plat.c
@@ -0,0 +1,62 @@
+/*
+ * Copyright (c) 2019 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 <compiler.h>
+#include <err.h>
+#include <stddef.h>
+
+#include "blxboot_plat.h"
+
+/* scpsys related weak impl. */
+__WEAK void plat_start_scpsys(void)
+{
+}
+
+/* platform fixup related weak impl. */
+__WEAK int plat_fixup_init(void)
+{
+ return NO_ERROR;
+}
+
+__WEAK void plat_fixup_append(char *append_str)
+{
+}
+
+__WEAK void plat_fixup_hook(uint32_t boot_mode, const char *ab_suffix,
+ void *dtb, size_t dtb_size)
+{
+}
+
+/* dtbo hardware information comprison function */
+__WEAK int plat_compare_dtbo_hwinfo(uint32_t dtbo_version,
+ struct dt_table_entry *entry)
+{
+ /* [TODO] For boot time consideration (decompression speed), compressed
+ * overlays is not supported for now. */
+ if ((dtbo_version > ANDROID_DTBO_V0) &&
+ android_dtbo_tbl_entry_compress_algo(entry) != NO_COMPRESSION) {
+ return -1;
+ }
+
+ return 0;
+}
diff --git a/src/bsp/lk/app/blxboot/blxboot_plat.h b/src/bsp/lk/app/blxboot/blxboot_plat.h
new file mode 100644
index 0000000..0a0704b
--- /dev/null
+++ b/src/bsp/lk/app/blxboot/blxboot_plat.h
@@ -0,0 +1,86 @@
+/*
+ * Copyright (c) 2019 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.
+ */
+
+#pragma once
+
+#include <lib/android_dtbo.h>
+#include <stddef.h>
+
+/*
+ * plat_start_scpsys() - start scpsys processor
+ *
+ * blxboot has only weak imp., override this function if necessary.
+ */
+void plat_start_scpsys(void);
+
+/*
+ * plat_fixup_init() - init platform fixup stuff
+ *
+ * blxboot has only weak imp. override this function if necessary.
+ *
+ * return:
+ * NO_ERROR, on success
+ * otherwise, on failure
+ */
+int plat_fixup_init(void);
+
+/*
+ * plat_fixup_append() - append string for platform fixup
+ *
+ * blxboot has only weak imp. override this function if necessary.
+ *
+ * @append_str: the string to be appended to kernel command line
+ */
+void plat_fixup_append(char *append_str);
+
+/*
+ * plat_fixup_hook() - platform fixup hook function
+ *
+ * blxboot has only weak imp. override this function if necessary.
+ *
+ * @boot_mode: current boot mode, one of NORMAL_BOOT, RECOVERY_BOOT and
+ * FASTBOOT_BOOT
+ * @ab_suffix: pointer to ab boot suffix string, could be NULL if ab is
+ * not enabled
+ * @dtb: pointer to the device tree blob
+ * @dtb_size: the maximum size of dtb
+ */
+void plat_fixup_hook(uint32_t boot_mode, const char *ab_suffix,
+ void *dtb, size_t dtb_size);
+
+/*
+ * plat_compare_dtbo_hwinfo() - platform defined dtbo hardware information
+ * comparison function
+ *
+ * blxboot has only weak imp., override this function if necessary.
+ *
+ * @dtbo_version: android dtbo format version
+ * @entry: pointer to the table entry which contains the id, rev, and custom
+ * information for matching the dtbo to current hardware configuration
+ *
+ * return:
+ * 0, on success (entry selected)
+ * -1, on failure
+ */
+int plat_compare_dtbo_hwinfo(uint32_t dtbo_version,
+ struct dt_table_entry *entry);
diff --git a/src/bsp/lk/app/blxboot/dto.c b/src/bsp/lk/app/blxboot/dto.c
new file mode 100644
index 0000000..7a6b2b0
--- /dev/null
+++ b/src/bsp/lk/app/blxboot/dto.c
@@ -0,0 +1,197 @@
+/*
+ * Copyright (c) 2019 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 <debug.h>
+#include <lib/android_dtbo.h>
+#include <lib/mempool.h>
+#include <libfdt.h>
+#include <platform.h>
+#include <trace.h>
+
+#include "blxboot_plat.h"
+#include "imageinfo.h"
+
+#define LOCAL_TRACE 0
+
+struct bootargs_overlay {
+ int noffset;
+ int len;
+ const void *path;
+ const char *prop;
+};
+
+int extract_fdt(void *fdt, int size)
+{
+ int ret = 0;
+
+ ret = fdt_open_into(fdt, fdt, size);
+ if (ret) {
+ dprintf(CRITICAL, "open fdt failed\n");
+ return ret;
+ }
+ ret = fdt_check_header(fdt);
+ if (ret) {
+ dprintf(CRITICAL, "check fdt failed\n");
+ return ret;
+ }
+
+ return ret;
+}
+
+static int dtbo_overlay(void *fdt_dtb, void *dtbo_entry)
+{
+ struct bootargs_overlay bootargs;
+ int noffset;
+
+ if (fdt_check_header(dtbo_entry)) {
+ LTRACEF("%s failed.\n", "dtbo check header");
+ return -1;
+ }
+
+ do {
+ noffset = fdt_path_offset(dtbo_entry, "/__symbols__");
+ if (noffset < 0)
+ break;
+
+ bootargs.path = fdt_getprop(dtbo_entry, noffset, "chosen", NULL);
+ if (bootargs.path == NULL)
+ break;
+
+ bootargs.noffset = fdt_path_offset(dtbo_entry, bootargs.path);
+ if (bootargs.noffset < 0)
+ break;
+
+ bootargs.prop = fdt_getprop(dtbo_entry, bootargs.noffset,
+ "bootargs_ext", &bootargs.len);
+ if (bootargs.prop == NULL)
+ break;
+
+ plat_fixup_append((char *)bootargs.prop);
+ } while (0);
+
+ if (fdt_overlay_apply(fdt_dtb, dtbo_entry)) {
+ LTRACEF("%s failed.\n", "fdt_overlay_apply");
+ return -1;
+ }
+
+ return 0;
+}
+
+static int android_dtbo_overlay(void *fdt_dtb, void *dtbo, bool update_cmd)
+{
+/* assume the maxium dtbo entries in dtbo.img */
+#define MAX_DTBO_ENTRIES_COUNT 100
+
+ static const char android_dtbo_param[] = "androidboot.dtbo_idx=";
+ uint32_t i;
+ uint32_t dtbo_entry_count;
+ uint32_t dtbo_version;
+ char *dtbo_idx_str;
+ char *dtbo_idx_str_end;
+ void *dtbo_entry;
+ struct dt_table_entry *tbl_entry;
+
+ if (android_dtbo_check_header(dtbo) != DTBO_RET_OK) {
+ LTRACEF("%s failed.\n", "android dtbo check header");
+ return -1;
+ }
+
+ dtbo_entry_count = android_dtbo_dt_entry_count(dtbo);
+ if (dtbo_entry_count >= MAX_DTBO_ENTRIES_COUNT) {
+ LTRACEF("Too many dtbo entries.\n");
+ return -1;
+ }
+
+ dtbo_idx_str_end = NULL;
+ if (update_cmd) {
+ dtbo_idx_str = mempool_alloc((dtbo_entry_count * 3) +
+ strlen(android_dtbo_param) + 1, MEMPOOL_ANY);
+ if (dtbo_idx_str == NULL) {
+ LTRACEF("mempool_alloc for dtboidx_str failed.\n");
+ return -1;
+ }
+ dtbo_idx_str_end = dtbo_idx_str;
+ sprintf(dtbo_idx_str_end, "%s", android_dtbo_param);
+ dtbo_idx_str_end += strlen(android_dtbo_param);
+ }
+
+ dtbo_version = android_dtbo_version(dtbo);
+ for (i = 0; i < dtbo_entry_count; i++) {
+ if (android_dtbo_get_dt_table_entry(dtbo, i, &tbl_entry) != DTBO_RET_OK)
+ break;
+
+ if (plat_compare_dtbo_hwinfo(dtbo_version, tbl_entry) != 0)
+ continue;
+
+ dtbo_entry = android_dtbo_get_dt_dtbo_entry(dtbo, i);
+ if (dtbo_entry == NULL)
+ break;
+
+ if (dtbo_overlay(fdt_dtb, dtbo_entry)) {
+ LTRACEF("fdt_overlay_apply failed: index=%u\n", i);
+ continue;
+ }
+
+ if (update_cmd) {
+ sprintf(dtbo_idx_str_end, "%d,", i);
+ dtbo_idx_str_end += i < 10 ? 2 : 3;
+ }
+ }
+
+ if (update_cmd) {
+ if (dtbo_idx_str_end != (dtbo_idx_str + strlen(android_dtbo_param))) {
+ *(dtbo_idx_str + strlen(dtbo_idx_str) - 1) = '\0';
+ plat_fixup_append(dtbo_idx_str);
+ }
+ mempool_free(dtbo_idx_str);
+ }
+
+ return 0;
+}
+
+int overlay_fdt(void *fdt_dtb, void *dtbo, void *vpd)
+{
+ /* [TODO] clarify: can we remove ERR_ADDR, just to check it against NULL */
+ if (fdt_dtb == (void *)ERR_ADDR) {
+ LTRACEF("no valid dtb.\n");
+ return -1;
+ }
+
+ if (extract_fdt(fdt_dtb, MAX_DTB_SIZE)) {
+ LTRACEF("%s failed.\n", "extract_fdt");
+ return -1;
+ }
+
+ if (vpd && (android_dtbo_overlay(fdt_dtb, vpd, false) != 0) &&
+ (dtbo_overlay(fdt_dtb, vpd) != 0))
+ return -1;
+
+ if (dtbo && (android_dtbo_overlay(fdt_dtb, dtbo, true) != 0) &&
+ (dtbo_overlay(fdt_dtb, dtbo) != 0))
+ return -1;
+
+ if (fdt_pack(fdt_dtb))
+ return -1;
+
+ return 0;
+}
diff --git a/src/bsp/lk/app/blxboot/dto.h b/src/bsp/lk/app/blxboot/dto.h
new file mode 100644
index 0000000..59a1cd8
--- /dev/null
+++ b/src/bsp/lk/app/blxboot/dto.h
@@ -0,0 +1,37 @@
+/*
+ * Copyright (c) 2019 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.
+ */
+
+#pragma once
+
+/**
+ * overlay_fdt() - overlay dtbo image with kernel dtb
+ *
+ * @fdt_dtb: kernel dtb start address
+ * @dtbo: feature dtbo start address
+ * @vpd: vital project data dtbo start address
+ *
+ * returns:
+ * 0, on success
+ * -1, on failure
+ */
+int overlay_fdt(void *fdt_dtb, void *dtbo, void *vpd);
diff --git a/src/bsp/lk/app/blxboot/imageinfo.h b/src/bsp/lk/app/blxboot/imageinfo.h
new file mode 100644
index 0000000..e9fbbfb
--- /dev/null
+++ b/src/bsp/lk/app/blxboot/imageinfo.h
@@ -0,0 +1,66 @@
+/*
+ * 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.
+ */
+
+#pragma once
+
+#include <stdbool.h>
+#include <sys/types.h>
+
+#define ERR_ADDR (0xffffffff)
+
+enum {
+ IMGTYPE_NONE,
+ IMGTYPE_TZ,
+ IMGTYPE_BL33,
+ IMGTYPE_BUILTIN_BL33,
+ IMGTYPE_KERNEL,
+ IMGTYPE_CM4,
+ IMGTYPE_SLAVE_CPU,
+ IMGTYPE_SPM,
+ IMGTYPE_HYPERVISOR,
+ IMGTYPE_DTBO,
+ IMGTYPE_VPD,
+ IMGTYPE_SCPSYS,
+ IMGTYPE_SCPDATA,
+ IMGTYPE_SCPCFG,
+ IMGTYPE_MODEM,
+ IMGTYPE_HSM_OS,
+};
+
+struct image_load_data {
+ const char *part_name;
+ const char *recovery_part_name;
+ const char *membdev_name; /* for fastboot flash download:xx command */
+ void *buf;
+ bool has_slot;
+
+ ulong kernel_entry;
+ ulong dtb_load;
+ ulong trustedos_entry;
+};
+
+struct imageinfo_t {
+ uint32_t type;
+ struct image_load_data *imgdata;
+ int (*load)(const char *part_name, struct imageinfo_t *img);
+};
diff --git a/src/bsp/lk/app/blxboot/imagelist.c b/src/bsp/lk/app/blxboot/imagelist.c
new file mode 100644
index 0000000..d320600
--- /dev/null
+++ b/src/bsp/lk/app/blxboot/imagelist.c
@@ -0,0 +1,806 @@
+/*
+ * 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 <debug.h>
+#include <errno.h>
+#include <fit.h>
+#include <kernel/vm.h>
+#include <lib/bio.h>
+#include <lib/mempool.h>
+#include <libfdt.h>
+#include <platform.h>
+#include <string.h>
+#include <sys/types.h>
+#include <trace.h>
+
+#include "avb.h"
+#include "blxboot_ab.h"
+#include "blxboot_plat.h"
+#include "imageinfo.h"
+
+#if ENABLE_ANDROID_BOOTIMG_SUPPORT
+#include <lib/android_bootimg.h>
+#include <lib/boot_info.h>
+#endif
+
+#define LOCAL_TRACE 0
+
+/* BL33 load and entry point address */
+#define CFG_BL33_LOAD_EP_ADDR (BL33_ADDR)
+
+/* ramdisk args to be appended to kernel command line */
+#define MAX_RAMDISK_ARG_LEN (45)
+#define RAMDISK_ARG_STR_FMT "initrd=0x%lx,0x%x"
+
+extern __WEAK paddr_t kvaddr_to_paddr(void *ptr);
+
+/* forward declaration */
+static int load_avb_image(const char *part_name,
+ struct imageinfo_t *img);
+static int load_builtin_bl33_image(const char *part_name,
+ struct imageinfo_t *img);
+static int load_fit_image(const char *part_name,
+ struct imageinfo_t *img);
+static int load_android_image(const char *part_name,
+ struct imageinfo_t *img);
+static int load_dtbo_image(const char *part_name,
+ struct imageinfo_t *img);
+static int load_scpsys_image(const char *part_name,
+ struct imageinfo_t *img);
+static int load_scpdata_image(const char *part_name,
+ struct imageinfo_t *img);
+static int load_scpcfg_image(const char *part_name,
+ struct imageinfo_t *img);
+#if ENABLE_MODEM_LOAD
+static int load_md_image(const char *part_name,
+ struct imageinfo_t *img);
+#endif
+#if ENABLE_HSM_OS_LOAD
+static int load_hsm_os_image(const char *part_name,
+ struct imageinfo_t *img);
+#endif
+
+struct image_load_data tzimg = {
+ .part_name = TZ_PART_NAME,
+ .recovery_part_name = RECOVERY_TZ_PART_NAME,
+ .membdev_name = "download:tz",
+ .has_slot = (TZ_HAS_SLOT != 0),
+};
+
+struct image_load_data bl33img = {
+ .part_name = BL33_PART_NAME,
+ .recovery_part_name = RECOVERY_BL33_PART_NAME,
+ .membdev_name = "download:bl33",
+ .has_slot = (BL33_HAS_SLOT != 0),
+};
+
+#if ENABLE_SLAVE_CPU_LOAD
+struct image_load_data scpuimg = {
+ .part_name = SLAVE_CPU_PART_NAME,
+ .recovery_part_name = RECOVERY_SLAVE_CPU_PART_NAME,
+ .membdev_name = "download:scpu",
+ .has_slot = (SLAVE_CPU_HAS_SLOT != 0),
+};
+#endif
+
+struct image_load_data bootimg = {
+ .part_name = BOOT_PART_NAME,
+ .recovery_part_name = RECOVERY_BOOT_PART_NAME,
+ .membdev_name = "download:boot",
+ .has_slot = (BOOT_HAS_SLOT != 0),
+};
+
+struct image_load_data dtboimg = {
+ .part_name = DTBO_PART_NAME,
+ .has_slot = (DTBO_HAS_SLOT != 0),
+};
+
+struct image_load_data vpdimg = {
+#if defined(VPD_PART_NAME)
+ .part_name = VPD_PART_NAME,
+#endif
+#if defined(RECOVERY_VPD_PART_NAME)
+ .recovery_part_name = RECOVERY_VPD_PART_NAME,
+#endif
+ .membdev_name = "download:vpd",
+ .has_slot = (VPD_HAS_SLOT != 0),
+};
+
+#if ENABLE_SCP_LOAD
+struct image_load_data scpsysimg = {
+ .part_name = SCPSYS_PART_NAME,
+ .recovery_part_name = RECOVERY_SCPSYS_PART_NAME,
+ .membdev_name = "download:scpsys",
+ .has_slot = (SCPSYS_HAS_SLOT != 0),
+};
+#endif
+
+#if ENABLE_SCP_AUX_LOAD
+struct image_load_data scpdataimg = {
+ .part_name = SCPDATA_PART_NAME,
+ .recovery_part_name = RECOVERY_SCPDATA_PART_NAME,
+ .membdev_name = "download:scpdata",
+ .has_slot = (SCPDATA_HAS_SLOT != 0),
+};
+
+struct image_load_data scpcfgimg = {
+ .part_name = SCPCFG_PART_NAME,
+ .recovery_part_name = RECOVERY_SCPCFG_PART_NAME,
+ .buf = (void *)SCP_CFG_VA,
+ .has_slot = (SCPCFG_HAS_SLOT != 0),
+};
+#endif
+
+#if ENABLE_SPM_FW_LOAD
+struct image_load_data spmfwimg = {
+ .part_name = SPM_PART_NAME,
+ .recovery_part_name = SPM_PART_NAME,
+ .membdev_name = "download:spmfw",
+ .has_slot = (SPMFW_HAS_SLOT != 0),
+};
+#endif
+
+#if ENABLE_MODEM_LOAD
+struct image_load_data mdimg = {
+ .part_name = MD_PART_NAME,
+ .membdev_name = "download:md",
+ .buf = (void *)MD_ADDR,
+ .has_slot = (MD_HAS_SLOT != 0),
+};
+#endif
+
+#if ENABLE_HSM_OS_LOAD
+struct image_load_data hsm_os_img = {
+ .part_name = HSM_OS_PART_NAME,
+ .membdev_name = "download:hsmos",
+ .buf = (void *)HSM_OS_ADDR,
+ .has_slot = (HSM_OS_HAS_SLOT != 0),
+};
+#endif
+
+struct imageinfo_t imagelist[] = {
+#if ENABLE_SCP_AUX_LOAD
+ {
+ .type = IMGTYPE_SCPDATA,
+ .load = load_scpdata_image,
+ .imgdata = &scpdataimg,
+ },
+ {
+ .type = IMGTYPE_SCPCFG,
+ .load = load_scpcfg_image,
+ .imgdata = &scpcfgimg,
+ },
+#endif
+
+#if ENABLE_SCP_LOAD
+ {
+ .type = IMGTYPE_SCPSYS,
+ .load = load_scpsys_image,
+ .imgdata = &scpsysimg,
+ },
+#endif
+
+#if ENABLE_TZ_LOAD
+ {
+ .type = IMGTYPE_TZ,
+ .load = load_fit_image,
+ .imgdata = &tzimg,
+ },
+#endif /* ENABLE_TZ_LOAD */
+
+#if ENABLE_BL33_LOAD
+ {
+#if ENABLE_BUILTIN_BL33
+ .type = IMGTYPE_BUILTIN_BL33,
+ .load = load_builtin_bl33_image,
+#else
+ .type = IMGTYPE_BL33,
+ .load = load_fit_image,
+#endif
+ .imgdata = &bl33img,
+ },
+#endif /* ENABLE_BL33_LOAD */
+
+#if ENABLE_SLAVE_CPU_LOAD
+ {
+ .type = IMGTYPE_SLAVE_CPU,
+ .load = load_fit_image,
+ .imgdata = &scpuimg,
+ },
+#endif /* ENABLE_DSP_LOAD */
+
+#if ENABLE_KERNEL_LOAD
+ {
+ .type = IMGTYPE_KERNEL,
+#if defined(AVB_VERIFY_KERNEL)
+ .load = load_avb_image,
+#else
+#if ENABLE_ANDROID_BOOTIMG_SUPPORT
+ .load = load_android_image,
+#else
+ .load = load_fit_image,
+#endif
+#endif
+ .imgdata = &bootimg,
+ },
+ {
+ .type = IMGTYPE_VPD,
+ .load = load_dtbo_image,
+ .imgdata = &vpdimg,
+ },
+ {
+ .type = IMGTYPE_DTBO,
+ .load = load_dtbo_image,
+ .imgdata = &dtboimg,
+ },
+#endif /* ENABLE_KERNEL_LOAD */
+
+#if ENABLE_SPM_FW_LOAD
+ {
+ .type = IMGTYPE_SPM,
+ .load = load_fit_image,
+ .imgdata = &spmfwimg,
+ },
+#endif /* ENABLE_SPM_FW_LOAD */
+
+#if ENABLE_MODEM_LOAD
+ {
+ .type = IMGTYPE_MODEM,
+ .load = load_md_image,
+ .imgdata = &mdimg,
+ },
+#endif
+
+#if ENABLE_HSM_OS_LOAD
+ {
+ .type = IMGTYPE_HSM_OS,
+ .load = load_hsm_os_image,
+ .imgdata = &hsm_os_img,
+ },
+#endif
+
+ { .type = IMGTYPE_NONE } /* imagelist end marker */
+};
+
+static void append_cmdline_ramdisk_arg(addr_t ramdisk_addr, uint32_t ramdisk_sz)
+{
+ char ramdisk_arg[MAX_RAMDISK_ARG_LEN];
+
+ memset(ramdisk_arg, 0x0, sizeof(ramdisk_arg));
+ snprintf(ramdisk_arg, MAX_RAMDISK_ARG_LEN, RAMDISK_ARG_STR_FMT,
+ ramdisk_addr, ramdisk_sz);
+ plat_fixup_append(ramdisk_arg);
+}
+
+static int fit_load_images(void *fit, struct image_load_data *fit_data,
+ bool need_verified)
+{
+ addr_t load;
+ size_t load_size;
+ int ret;
+
+ /* TODO: decide verify policy with config. */
+ if (need_verified) {
+ dprintf(ALWAYS, "verify fit conf sig: %s\n", fit_data->part_name);
+ ret = fit_conf_verify_sig(NULL, fit);
+ if (ret < 0)
+ return ret;
+ }
+
+ ret = fit_load_image(NULL, "kernel", fit, NULL, NULL,
+ (paddr_t *)&fit_data->kernel_entry, need_verified);
+ if (ret && (ret != -ENOENT)) {
+ dprintf(CRITICAL, "%s load kernel failed\n", fit_data->part_name);
+ return ret;
+ }
+
+ ret = fit_load_image(NULL, "ramdisk", fit, &load, &load_size, NULL,
+ need_verified);
+ if (ret) {
+ if (ret != -ENOENT) {
+ dprintf(CRITICAL, "%s load ramdisk failed\n", fit_data->part_name);
+ return ret;
+ }
+ } else {
+#if WITH_KERNEL_VM
+ load = kvaddr_to_paddr((void *)load);
+#endif
+ append_cmdline_ramdisk_arg(load, load_size);
+ }
+
+ ret = fit_load_image(NULL, "fdt", fit, (addr_t *)&fit_data->dtb_load, NULL,
+ NULL, need_verified);
+ if (ret && (ret != -ENOENT)) {
+ fit_data->dtb_load = ERR_ADDR; /* [TODO] remove or assign it to NULL */
+ dprintf(CRITICAL, "%s load fdt failed\n", fit_data->part_name);
+ return ret;
+ }
+
+ ret = fit_load_image(NULL, "tee", fit, NULL, NULL,
+ (paddr_t *)&fit_data->trustedos_entry, need_verified);
+ if (ret && (ret != -ENOENT)) {
+ dprintf(CRITICAL, "%s load trustedos failed\n", fit_data->part_name);
+ return ret;
+ }
+
+ return 0;
+}
+
+#if ENABLE_ANDROID_BOOTIMG_SUPPORT
+static int load_android_hdr_images(const void *part_data,
+ struct image_load_data *android_data)
+{
+ int rc;
+ uint32_t ramdisk_target_addr, kernel_target_addr;
+ uint32_t ramdisk_sz, zimage_sz, dtb_sz;
+ uint64_t dtb_target_addr;
+ vaddr_t ramdisk_addr, kernel_addr, dtb_addr;
+ char android_dtb_param[] = "androidboot.dtb_idx=0";
+
+ load_bootinfo_bootimg_hdr((struct bootimg_hdr *)part_data);
+ set_bootimg_loaded((vaddr_t)part_data);
+
+ ramdisk_target_addr = get_ramdisk_target_addr();
+ ramdisk_addr = get_ramdisk_addr();
+ ramdisk_sz = get_ramdisk_real_sz();
+
+ kernel_target_addr = get_kernel_target_addr();
+ kernel_addr = get_kernel_addr();
+ zimage_sz = get_kernel_real_sz();
+
+ dtb_target_addr = get_dtb_target_addr();
+ dtb_addr = get_dtb_addr();
+ dtb_sz = get_dtb_size();
+
+ assert(ramdisk_target_addr != 0);
+ assert(ramdisk_addr != 0);
+ assert(kernel_target_addr != 0);
+ assert(kernel_addr != 0);
+ assert(dtb_target_addr != 0);
+ assert(dtb_addr != 0);
+
+ if(get_header_version() < BOOT_HEADER_VERSION_TWO) {
+ LTRACEF_LEVEL(CRITICAL,
+ "Android Image Header version is lower than v2\n");
+ return -EINVAL;
+ }
+
+ rc = android_processing_data("ramdisk", ramdisk_addr,
+ ramdisk_target_addr, ramdisk_sz);
+ if (rc != 0) {
+ LTRACEF_LEVEL(CRITICAL, "load %s failed\n", "ramdisk");
+ return -EINVAL;
+ }
+ append_cmdline_ramdisk_arg((addr_t)ramdisk_target_addr, ramdisk_sz);
+
+ rc = android_processing_data("kernel", kernel_addr,
+ kernel_target_addr, zimage_sz);
+ if (rc != 0) {
+ LTRACEF_LEVEL(CRITICAL, "load %s failed\n", "kernel");
+ return -EINVAL;
+ }
+ android_data->kernel_entry = kernel_target_addr;
+
+ rc = android_processing_data("dtb", dtb_addr,
+ dtb_target_addr, dtb_sz);
+ if (rc != 0) {
+ LTRACEF_LEVEL(CRITICAL, "load %s failed\n", "dtb");
+ return -EINVAL;
+ }
+#if WITH_KERNEL_VM
+ dtb_target_addr = (vaddr_t)paddr_to_kvaddr(dtb_target_addr);
+#endif
+ android_data->dtb_load = dtb_target_addr;
+
+ plat_fixup_append(android_dtb_param);
+
+ return 0;
+}
+#endif
+
+static int load_avb_image(const char *part_name, struct imageinfo_t *img)
+{
+ int ret;
+ size_t part_name_len;
+ struct image_load_data *imgdata;
+ char *raw_part_name;
+ const char *suffix;
+ AvbSlotVerifyResult verify_result;
+ AvbSlotVerifyData *verify_data;
+
+ if ((img == NULL) || (img->imgdata == NULL))
+ return -1;
+
+ imgdata = img->imgdata;
+
+ raw_part_name = strdup(part_name);
+ if (raw_part_name == NULL)
+ return -ENOMEM;
+
+ if (imgdata->has_slot) {
+ /* prepare partition name without ab suffix */
+ part_name_len = strlen(part_name);
+ suffix = get_suffix();
+ if (suffix && (part_name_len > strlen(suffix))) {
+ part_name_len -= strlen(suffix);
+ *(raw_part_name + part_name_len) = '\0';
+ }
+ }
+
+ ret = 0;
+ verify_result = android_verified_boot_2_0(raw_part_name, &verify_data);
+ if ((verify_result != AVB_SLOT_VERIFY_RESULT_OK) && !is_device_unlocked()) {
+ ret = -10;
+ goto exit;
+ }
+
+ void *part_data = get_partition_data(raw_part_name, verify_data);
+ if (part_data == NULL) {
+ ret = -11;
+ goto exit;
+ }
+
+#if ENABLE_ANDROID_BOOTIMG_SUPPORT
+ if (img->type == IMGTYPE_KERNEL) {
+ ret = load_android_hdr_images(part_data, imgdata);
+ } else {
+ if (android_dtbo_check_header((const void *)part_data) == DTBO_RET_OK) {
+ /* android dtbo image without fit header */
+ imgdata->dtb_load = (ulong)part_data;
+ } else {
+ /* android dtbo image with fit header */
+ ret = fit_load_images(part_data, imgdata, false);
+ }
+ }
+#else
+ ret = fit_load_images(part_data, imgdata, false);
+#endif
+
+ if (ret) {
+ LTRACEF_LEVEL(CRITICAL, "load %s image failed\n", part_name);
+ ret = -21;
+ goto exit;
+ }
+
+ if (img->type == IMGTYPE_KERNEL)
+ plat_fixup_append(verify_data->cmdline);
+
+exit:
+ if (raw_part_name)
+ free(raw_part_name);
+
+ return ret;
+}
+
+static int load_builtin_bl33_image(const char *part_name,
+ struct imageinfo_t *img)
+{
+ uint32_t bl33[] = { 0xea000005, /* b BL33_32_ENTRY | ands x5, x0, x0 */
+ 0x58000160, /* .word 0x58000160 | ldr x0, _X0 */
+ 0x58000181, /* .word 0x58000181 | ldr x1, _X1 */
+ 0x580001a2, /* .word 0x580001a2 | ldr x2, _X2 */
+ 0x580001c3, /* .word 0x580001c3 | ldr x3, _X3 */
+ 0x580001e4, /* .word 0x580001e4 | ldr x4, _X4 */
+ 0xd61f0080, /* .word 0xd61f0080 | br x4 */
+ /* BL33_32_ENTRY: | */
+ 0xe59f0030, /* ldr r0, _R0 | .word 0xe59f0030 */
+ 0xe59f1030, /* ldr r1, _R1 | .word 0xe59f1030 */
+ 0xe59f2004, /* ldr r2, _X0 | .word 0xe59f2004 */
+ 0xe59ff020, /* ldr pc, _X4 | .word 0xe59ff020 */
+ 0x00000000, /* .word 0x00000000 */
+ 0x00000000, /* _X0: .word 0x00000000 */
+ 0x00000000, /* .word 0x00000000 */
+ 0x00000000, /* _X1: .word 0x00000000 */
+ 0x00000000, /* .word 0x00000000 */
+ 0x00000000, /* _X2: .word 0x00000000 */
+ 0x00000000, /* .word 0x00000000 */
+ 0x00000000, /* _X3: .word 0x00000000 */
+ 0x00000000, /* .word 0x00000000 */
+ 0x00000000, /* _X4: .word 0x00000000 */
+ 0x00000000, /* _R0: .word 0x00000000 */
+ 0x00000000, /* _R1: .word 0x00000000 */
+ 0x00000000 /* .word 0x00000000 */
+ };
+
+ memcpy((void *)CFG_BL33_LOAD_EP_ADDR, bl33, sizeof(bl33));
+ img->imgdata->kernel_entry = kvaddr_to_paddr ?
+ kvaddr_to_paddr((void *)CFG_BL33_LOAD_EP_ADDR) : CFG_BL33_LOAD_EP_ADDR;
+
+ return 0;
+}
+
+static int load_fit_image(const char *part_name, struct imageinfo_t *img)
+{
+ int err;
+
+ if ((err = fit_get_image(part_name, &img->imgdata->buf)) == 0) {
+ err = fit_load_images(img->imgdata->buf, img->imgdata, true);
+ }
+
+ return err;
+}
+
+#if ENABLE_ANDROID_BOOTIMG_SUPPORT
+static int load_android_image(const char *part_name, struct imageinfo_t *img)
+{
+ int err;
+
+ if ((err = android_get_image(part_name, &img->imgdata->buf)) == 0) {
+ err = load_android_hdr_images(img->imgdata->buf, img->imgdata);
+ }
+
+ return err;
+}
+#endif
+
+static int load_android_dtbo_image(const char *part_name,
+ struct imageinfo_t *img, uint32_t size)
+{
+ int ret;
+
+ /* android dtbo format */
+ LTRACEF("part_name(%s) in pure android dtbo format.\n", part_name);
+#if defined(AVB_VERIFY_KERNEL)
+ ret = load_avb_image(part_name, img);
+#else
+ bdev_t *bdev;
+ void *buf;
+ ssize_t read_bytes;
+
+ bdev = bio_open_by_label(part_name) ? : bio_open(part_name);
+ if (!bdev)
+ return -ENODEV;
+
+ ret = 0;
+ buf = mempool_alloc(size, MEMPOOL_ANY);
+ if (!buf) {
+ ret = -ENOMEM;
+ goto _err;
+ }
+
+ read_bytes = bio_read(bdev, buf, 0, size);
+ if (read_bytes < (ssize_t)size) {
+ LTRACEF("Read android dtbo image fail: read(0x%x), got(0x%zx)\n",
+ size, read_bytes);
+ mempool_free(buf);
+ ret = -EIO;
+ goto _err;
+ }
+
+ img->imgdata->dtb_load = (ulong)buf;
+
+_err:
+ if (bdev)
+ bio_close(bdev);
+#endif
+
+ return ret;
+}
+
+static int load_fdt_dtbo_image(const char *part_name, struct imageinfo_t *img)
+{
+ int ret;
+ int noffset;
+
+ /* legacy fdt format dtbo or android dtbo format with FIT header */
+ ret = fit_get_image(part_name, &img->imgdata->buf);
+ if (ret != 0)
+ return ret;
+
+ /* check configuration node to know it's legacy dtbo or not */
+ noffset = fdt_path_offset(img->imgdata->buf, "/configurations");
+ if (noffset < 0) {
+ LTRACEF("part_name(%s) in legacy dtbo format\n", part_name);
+ img->imgdata->dtb_load = (ulong)img->imgdata->buf;
+ return 0;
+ }
+
+ LTRACEF("part_name(%s) in fit android dtbo format.\n", part_name);
+ /* we verifies android dtbo image in the same method with kernel */
+#if defined(AVB_VERIFY_KERNEL)
+ if (img->type == IMGTYPE_DTBO) {
+ mempool_free(img->imgdata->buf);
+ img->imgdata->buf = NULL;
+
+ /* [TODO] load_avb_image() will waste time to load dtbo image again */
+ ret = load_avb_image(part_name, img);
+ } else {
+ ret = fit_load_images(img->imgdata->buf, img->imgdata, true);
+ }
+#else
+ /* get android dtbo */
+ ret = fit_load_images(img->imgdata->buf, img->imgdata, true);
+#endif
+
+ return ret;
+}
+
+static int load_dtbo_image(const char *part_name, struct imageinfo_t *img)
+{
+ int ret;
+ uint32_t total_size;
+ ssize_t read_bytes;
+ void *buf;
+ bdev_t *bdev;
+
+ bdev = bio_open_by_label(part_name) ? : bio_open(part_name);
+ if (!bdev) {
+ LTRACEF("Partition [%s] is not exist.\n", part_name);
+ return -ENODEV;
+ }
+
+ ret = 0;
+ /* read 1 block to determine the dtbo image format */
+ buf = malloc(bdev->block_size);
+ if (!buf) {
+ ret = -ENOMEM;
+ goto _err;
+ }
+
+ memset(buf, 0, bdev->block_size);
+ read_bytes = bio_read(bdev, buf, 0, bdev->block_size);
+ if (read_bytes < (ssize_t)bdev->block_size) {
+ LTRACEF("bio_read offset: 0, size: %ld, got: %ld\n",
+ bdev->block_size, read_bytes);
+ ret = -EIO;
+ goto _err;
+ }
+
+ if (android_dtbo_check_header((const void *)buf) == DTBO_RET_OK) {
+ total_size = android_dtbo_total_size((const void *)buf);
+ if (load_android_dtbo_image(part_name, img, total_size) < 0)
+ ret = -EINVAL;
+ } else if (fdt_check_header((const void *)buf) == 0) {
+ if (load_fdt_dtbo_image(part_name, img) < 0)
+ ret = -EINVAL;
+ } else {
+ LTRACEF("Not valid dtbo image.\n");
+ ret = -ENOTSUP;
+ }
+
+_err:
+ if (buf)
+ free(buf);
+ bio_close(bdev);
+
+ return ret;
+}
+
+static int load_scpsys_image(const char *part_name, struct imageinfo_t *img)
+{
+ int err;
+ void *fit;
+
+ err = fit_get_image(part_name, &img->imgdata->buf);
+ if (err) {
+ LTRACEF("fit_get_image %s fail, err=%d\n", part_name, err);
+ return err;
+ }
+ fit = img->imgdata->buf;
+
+ dprintf(ALWAYS, "verify fit conf sig: %s\n", part_name);
+ err = fit_conf_verify_sig(NULL, fit);
+ if (err < 0)
+ return err;
+
+ err = fit_load_image(NULL, "kernel", fit, NULL, NULL, NULL, true);
+ if (err) {
+ LTRACEF("fit_load_image %s: sram fw fail, err=%d\n", part_name, err);
+ return err;
+ }
+
+ /* the dram part of scpsys image is an optional image, could be absent */
+ err = fit_load_image(NULL, "firmware", fit, NULL, NULL, NULL, true);
+ if (err && err != -ENOENT) {
+ LTRACEF("fit_load_image %s: dram fw fail, err=%d\n", part_name, err);
+ return err;
+ }
+
+ return 0;
+}
+
+static int load_scpdata_image(const char *part_name, struct imageinfo_t *img)
+{
+#define FIT_FASTLOGO_IMG_NODE_NAME "logo"
+#define FIT_GUIDELINE_IMG_NODE_NAME "guideline"
+#define FIT_WARNMSG_IMG_NODE_NAME "warnmsg"
+#define FIT_CVBSIMG_IMG_NODE_NAME "cvbsimg"
+
+ int err;
+ void *fit;
+
+ err = fit_get_image(part_name, &img->imgdata->buf);
+ if (err) {
+ LTRACEF("fit_get_image %s fail, err=%d\n", part_name, err);
+ return err;
+ }
+ fit = img->imgdata->buf;
+
+ err = fit_load_loadable_image(fit, FIT_FASTLOGO_IMG_NODE_NAME, NULL);
+ if (err) {
+ LTRACEF("fit_load_loadable_image %s fail, err=%d\n",
+ FIT_FASTLOGO_IMG_NODE_NAME, err);
+ return err;
+ }
+
+ err = fit_load_loadable_image(fit, FIT_GUIDELINE_IMG_NODE_NAME, NULL);
+ if (err) {
+ LTRACEF("fit_load_loadable_image %s fail, err=%d\n",
+ FIT_GUIDELINE_IMG_NODE_NAME, err);
+ return err;
+ }
+
+ err = fit_load_loadable_image(fit, FIT_WARNMSG_IMG_NODE_NAME, NULL);
+ if (err) {
+ LTRACEF("fit_load_loadable_image %s fail, err=%d\n",
+ FIT_WARNMSG_IMG_NODE_NAME, err);
+ return err;
+ }
+
+ err = fit_load_loadable_image(fit, FIT_CVBSIMG_IMG_NODE_NAME, NULL);
+ if (err) {
+ LTRACEF("fit_load_loadable_image %s fail, err=%d\n",
+ FIT_CVBSIMG_IMG_NODE_NAME, err);
+ return err;
+ }
+
+ return 0;
+}
+
+/* [TODO] mvoe load_scpcfg_image to mt2712 platform layer to keep SCP_CFG_VA
+ * invisble to others */
+static int load_scpcfg_image(const char *part_name, struct imageinfo_t *img)
+{
+ int err;
+ ssize_t bytes_read;
+ bdev_t *bdev;
+
+ bdev = bio_open_by_label(part_name) ? : bio_open(part_name);
+ if (!bdev)
+ return -ENODEV;
+
+ err = 0;
+ bytes_read = bio_read(bdev, img->imgdata->buf, 0, bdev->total_size);
+ if (bytes_read < bdev->total_size)
+ err = -EIO;
+
+ bio_close(bdev);
+
+ return err;
+}
+
+#if ENABLE_MODEM_LOAD
+extern int load_modem_image(const char *part_name);
+static int load_md_image(const char *part_name, struct imageinfo_t *img)
+{
+ return load_modem_image(part_name);
+}
+#endif
+
+#if ENABLE_HSM_OS_LOAD
+extern int load_hsm_os(const char *part_name);
+static int load_hsm_os_image(const char *part_name, struct imageinfo_t *img)
+{
+ return load_hsm_os(part_name);
+}
+#endif
diff --git a/src/bsp/lk/app/blxboot/images_ab_slot_def.mk b/src/bsp/lk/app/blxboot/images_ab_slot_def.mk
new file mode 100644
index 0000000..b305db3
--- /dev/null
+++ b/src/bsp/lk/app/blxboot/images_ab_slot_def.mk
@@ -0,0 +1,30 @@
+# default ab slot configuration
+
+TZ_HAS_SLOT ?= 0
+DTBO_HAS_SLOT ?= 0
+VPD_HAS_SLOT ?= 0
+BL33_HAS_SLOT ?= 0
+BOOT_HAS_SLOT ?= 0
+ROOTFS_HAS_SLOT ?= 0
+SLAVE_CPU_HAS_SLOT ?= 0
+SCPSYS_HAS_SLOT ?= 0
+SCPDATA_HAS_SLOT ?= 0
+SCPCFG_HAS_SLOT ?= 0
+SPMFW_HAS_SLOT ?= 0
+MD_HAS_SLOT ?= 0
+HSM_OS_HAS_SLOT ?= 0
+
+GLOBAL_DEFINES += TZ_HAS_SLOT=$(TZ_HAS_SLOT) \
+ DTBO_HAS_SLOT=$(DTBO_HAS_SLOT) \
+ VPD_HAS_SLOT=$(VPD_HAS_SLOT) \
+ BL33_HAS_SLOT=$(BL33_HAS_SLOT) \
+ BOOT_HAS_SLOT=$(BOOT_HAS_SLOT) \
+ ROOTFS_HAS_SLOT=$(ROOTFS_HAS_SLOT) \
+ SLAVE_CPU_HAS_SLOT=$(SLAVE_CPU_HAS_SLOT) \
+ SCPSYS_HAS_SLOT=$(SCPSYS_HAS_SLOT) \
+ SCPDATA_HAS_SLOT=$(SCPDATA_HAS_SLOT) \
+ SCPCFG_HAS_SLOT=$(SCPCFG_HAS_SLOT) \
+ SPMFW_HAS_SLOT=$(SPMFW_HAS_SLOT) \
+ MD_HAS_SLOT=$(MD_HAS_SLOT) \
+ HSM_OS_HAS_SLOT=$(HSM_OS_HAS_SLOT) \
+
diff --git a/src/bsp/lk/app/blxboot/rules.mk b/src/bsp/lk/app/blxboot/rules.mk
new file mode 100644
index 0000000..6c69041
--- /dev/null
+++ b/src/bsp/lk/app/blxboot/rules.mk
@@ -0,0 +1,82 @@
+LOCAL_DIR := $(GET_LOCAL_DIR)
+
+MODULE := $(LOCAL_DIR)
+MTK_COMMON_PLAT_DIR := $(LOCAL_DIR)/../../platform/mediatek/common
+# [TODO] follow normal ways to remove lib from MODULE_INCLUDES
+MODULE_INCLUDES += $(MODULE_BUILDDIR)/../../../include lib \
+ $(MTK_COMMON_PLAT_DIR)/include
+
+SCRATCH_SIZE ?= 0x04000000 # 64MB
+MAX_DTB_SIZE ?= 0x00200000 # 2MB
+
+GLOBAL_DEFINES += SCRATCH_SIZE=$(SCRATCH_SIZE) \
+ MAX_DTB_SIZE=$(MAX_DTB_SIZE)
+
+# secure boot options
+ifeq ($(strip $(SECURE_BOOT_ENABLE)),yes)
+ifeq ($(strip $(SECURE_BOOT_TYPE)),avb)
+GLOBAL_COMPILEFLAGS += -DAVB_VERIFY_KERNEL
+
+ifeq ($(strip $(AVB_ENABLE_ANTIROLLBACK)),yes)
+GLOBAL_COMPILEFLAGS += -DAVB_ENABLE_ANTIROLLBACK
+endif
+endif # SECURE_BOOT_TYPE
+endif # SECURE_BOOT_ENABLE
+
+ifeq ($(strip $(AB_OTA_UPDATER)),yes)
+GLOBAL_COMPILEFLAGS += -DAB_OTA_UPDATER
+endif
+
+# loading kernel format config,
+# 0: fit image header 1: android image header
+ENABLE_ANDROID_BOOTIMG_SUPPORT ?= 0
+GLOBAL_DEFINES += \
+ ENABLE_ANDROID_BOOTIMG_SUPPORT=$(ENABLE_ANDROID_BOOTIMG_SUPPORT)
+
+ifeq ($(strip $(ENABLE_ANDROID_BOOTIMG_SUPPORT)),1)
+MODULE_DEPS += lib/android_bootimg
+endif
+
+# Todo: remove this option after cmdlineoverlay integration done
+ifeq ($(strip $(ANDROID_2712)),yes)
+MODULE_COMPILEFLAGS += -DMT2712_ANDROID
+endif
+
+MODULE_DEPS += \
+ lib/bio \
+ lib/mempool \
+ lib/fdt \
+ lib/fit \
+ lib/fastboot \
+ lib/libavb \
+ lib/libavb_ab \
+ lib/android_dtbo
+
+MODULE_SRCS += \
+ $(LOCAL_DIR)/blxboot.c \
+ $(LOCAL_DIR)/blxboot_ab.c \
+ $(LOCAL_DIR)/blxboot_plat.c \
+ $(LOCAL_DIR)/dto.c \
+ $(LOCAL_DIR)/imagelist.c \
+ $(LOCAL_DIR)/avb.c \
+ $(MTK_COMMON_PLAT_DIR)/drivers/smc/smc.c \
+ $(MTK_COMMON_PLAT_DIR)/drivers/smc/psci.c
+
+ifeq ($(strip $(AB_OTA_UPDATER)),yes)
+MODULE_DEPS += lib/bootctrl
+endif
+
+ifeq ($(strip $(AB_UPGRADE_APP)),yes)
+MODULE_DEPS += lib/upgrade_app_ctrl
+endif
+
+ifeq ($(LK_AS_BL33),0)
+MODULE_SRCS += $(LOCAL_DIR)/bl2boot.c
+else
+MODULE_SRCS += $(LOCAL_DIR)/bl33boot.c
+endif
+
+GLOBAL_COMPILEFLAGS += -Os
+
+include $(LOCAL_DIR)/images_ab_slot_def.mk
+include make/module.mk
diff --git a/src/bsp/lk/app/fitboot/fitboot.c b/src/bsp/lk/app/fitboot/fitboot.c
new file mode 100644
index 0000000..dd2c288
--- /dev/null
+++ b/src/bsp/lk/app/fitboot/fitboot.c
@@ -0,0 +1,706 @@
+/*
+ * Copyright (c) 2016 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 <app.h>
+#include <assert.h>
+#include <boot_mode.h>
+#include <errno.h>
+#include <libfdt.h>
+#include <kernel/event.h>
+#include <kernel/thread.h>
+#include <kernel/vm.h>
+#include <lib/mempool.h>
+#ifdef AB_UPGRADE_APP
+#include <lib/bio.h>
+#endif
+#include <platform.h>
+#include <platform/mtk_key.h>
+#include <platform/mtk_wdt.h>
+#include <trace.h>
+
+#include "fit.h"
+#ifdef RTC_CHECK_FASTBOOT
+#include "rtc.h"
+#endif
+
+#ifdef SET_FDT_EMI_INFO
+#include "platform/emi_info_v1.h"
+#endif
+
+#define LOCAL_TRACE 0
+
+/* BL33 load and entry point address */
+#define CFG_BL33_LOAD_EP_ADDR (BL33_ADDR)
+#define ERR_ADDR (0xffffffff)
+
+#ifdef AB_UPGRADE_APP
+#define UPG_SUCCEED 2
+#endif
+
+typedef void (*jump32_func_type)(uint32_t addr, uint32_t arg1, uint32_t arg2) __NO_RETURN;
+typedef void (*jump64_func_type)(uint64_t bl31_addr, uint64_t bl33_addr, uint64_t arg1) __NO_RETURN;
+
+struct fit_load_data {
+ char *part_name;
+ char *recovery_part_name;
+ void *buf;
+ u32 boot_mode;
+ ulong kernel_entry;
+ ulong dtb_load;
+ ulong trustedos_entry;
+};
+
+#ifdef AB_UPGRADE_APP
+typedef struct boot_flag boot_flag;
+struct boot_flag {
+ int lastboot;
+ int usea;
+ int useb;
+ int current;
+};
+#endif
+
+/* global variables, also used in dl_commands.c */
+void *kernel_buf;
+void *tz_buf;
+void *bl33_buf;
+
+__WEAK bool plat_fixup_hook(void* bootimg_dtb_load, ...);
+
+#if ARCH_ARM64
+void mtk_sip(uint32_t smc_fid, uint64_t bl31_addr, uint64_t bl33_addr)
+{
+ jump64_func_type jump64_func = (jump64_func_type)bl31_addr;
+ (*jump64_func)(bl31_addr, bl33_addr, 0UL);
+}
+#endif
+
+void prepare_bl2_exit(ulong smc_fid, ulong bl31_addr, ulong bl33_addr, ulong arg1)
+{
+#if ARCH_ARM64
+ /* switch to el3 via smc, and will jump to mtk_sip from smc handler */
+ __asm__ volatile("smc #0\n\t");
+#else
+ jump32_func_type jump32_func = (jump32_func_type)bl31_addr;
+ (*jump32_func)(bl33_addr, 0, 0);
+#endif
+}
+
+#pragma GCC push_options
+#pragma GCC optimize("O1")
+static void setup_bl33(uint *bl33, ulong fdt, ulong kernel_ep)
+{
+ bl33[12] = (ulong)fdt;
+ bl33[14] = (unsigned)0;
+ bl33[16] = (unsigned)0;
+ bl33[18] = (unsigned)0;
+ bl33[20] = (ulong)kernel_ep;
+ bl33[21] = (ulong)0;
+ bl33[22] = (unsigned)MACH_TYPE;
+}
+#pragma GCC pop_options
+
+static int extract_fdt(void *fdt, int size)
+{
+ int ret = 0;
+
+ /* DTB maximum size is 2MB */
+ ret = fdt_open_into(fdt, fdt, size);
+ if (ret) {
+ dprintf(CRITICAL, "open fdt failed\n");
+ return ret;
+ }
+ ret = fdt_check_header(fdt);
+ if (ret) {
+ dprintf(CRITICAL, "check fdt failed\n");
+ return ret;
+ }
+
+ return ret;
+}
+
+static bool check_uart_enter(void)
+{
+ char c;
+
+ if (platform_dgetc(&c, false) != 0)
+ return false;
+ return (c == 13);
+}
+
+#ifdef AB_UPGRADE_APP
+int set_currently_boot_flag(int last_flag, int current_flag, const char *part_name)
+{
+ int ret;
+ long len = 0;
+ u32 writesize = 2048;
+ int index = -1;
+ unsigned long long ptn = 0;
+ unsigned long long size = 0;
+ char *buf;
+ boot_flag set_flag;
+
+ /* read partition */
+ struct bdev *nand_MISC = bio_open_by_label("misc");
+ if (!nand_MISC) {
+ LTRACEF("open misc partition failed.\n");
+ return 1;
+ }
+ buf = malloc(writesize);
+ if (buf == NULL) {
+ LTRACEF("malloc for writesize failed.\n");
+ return 1;
+ }
+ memset(buf, 0, writesize);
+
+ len = bio_read(nand_MISC, buf, 0, sizeof(boot_flag));
+ if (len < 0) {
+ dprintf(CRITICAL, "%s read error. LINE: %d\n", part_name, __LINE__);
+ free(buf);
+ buf = NULL;
+ return -1;
+ }
+ /* dump flag for debug */
+ dprintf(CRITICAL, "current boot flag is %d\n", current_flag);
+ /* set currently flag to buf */
+ set_flag.lastboot = last_flag;
+ set_flag.current = current_flag;
+ set_flag.usea = (int)-1;
+ dprintf(CRITICAL, "last_flag boot flag is %d\n", last_flag);
+ set_flag.useb = (int)-1;
+ memset(buf, 0, writesize);
+ memcpy(buf, (void *)&set_flag, sizeof(boot_flag));
+ /* write buf to offset 0, which size is 2048 */
+ len = bio_write(nand_MISC, (char *)buf, 0, (u32)writesize);
+ if (len <= 0) {
+ dprintf(CRITICAL, "nand write fail, return : %d, error: %s\n",len, strerror(errno));
+ dprintf(CRITICAL, "buf: %s\n", buf);
+ ret = -1;
+ }
+ else {
+ dprintf(CRITICAL, "set flag: lastboot = %d, use A = %d, use B = %d, current = %d\n", set_flag.lastboot, set_flag.usea, set_flag.useb, set_flag.current);
+ ret = 0;
+ }
+
+ if (buf) {
+ free(buf);
+ buf = NULL;
+ }
+ return ret;
+}
+
+int check_boot_partition(const char *part_name)
+{
+ int ret = 0;
+ boot_flag flag;
+ int boot = 0;
+
+ struct bdev *nand_MISC = bio_open_by_label("misc");
+ int len = -1;
+ char *buf;
+
+ if (!nand_MISC) {
+ printf("failed to open MISC\n");
+ return 0;
+ }
+ printf("open MISC successfully\n");
+
+ /* read partition */
+ buf = malloc(sizeof(boot_flag));
+ if (buf == NULL) {
+ LTRACEF("malloc for boot_flag failed.\n");
+ return 2;
+ }
+
+ len = bio_read(nand_MISC, buf, 0, sizeof(boot_flag));
+ if (len < 0) {
+ dprintf(CRITICAL, "read %s: boot flag read error. LINE: %d\n", part_name, __LINE__);
+ free(buf);
+ buf = NULL;
+ return -1;
+ }
+ memcpy(&flag, (void *)buf, sizeof(boot_flag));
+
+ /* dump flag for debug */
+ dprintf(CRITICAL, "lastboot = %d, use A = %d, use B = %d, current = %d\n", flag.lastboot, flag.usea, flag.useb, flag.current);
+
+ /* make dicision */
+ if (flag.lastboot == 0) {
+ if (flag.useb == UPG_SUCCEED) {
+ boot = 1;
+ dprintf(CRITICAL,"***last succeed boot from A system,upgrade B succeed***\n");
+ dprintf(CRITICAL,"***now boot from system B***\n");
+ } else {
+ boot = 0;
+ dprintf(CRITICAL,"***last succeed boot from A system,upgrade B failed or no upgrade B***\n");
+ dprintf(CRITICAL,"***now boot from system A***\n");
+ }
+ } else if (flag.lastboot == 1) {
+ if (flag.usea == UPG_SUCCEED) {
+ boot = 0;
+ dprintf(CRITICAL,"***last succeed boot from B system,upgrade A succeed***\n");
+ dprintf(CRITICAL,"***now boot from system A***\n");
+ } else {
+ boot = 1;
+ dprintf(CRITICAL,"***last succeed boot from B system,upgrade A failed or no upgrade A***\n");
+ dprintf(CRITICAL,"***now boot from system B***\n");
+ }
+ } else {
+ dprintf(CRITICAL, "boot flag is not match, use default boot partition\n");
+ boot = 0;
+ }
+
+ if ((flag.current != boot) || (flag.usea == UPG_SUCCEED) || (flag.useb == UPG_SUCCEED)) {
+ ret = bio_erase(nand_MISC, 0, nand_MISC->total_size); //erase total_size
+ printf("bio erase ret %d\n", ret);
+ ret = set_currently_boot_flag(flag.lastboot, boot, part_name);
+ if (ret!=0)
+ dprintf(CRITICAL, "set flags fail. LINE: %d\n", __LINE__);
+ }
+ if (buf) {
+ free(buf);
+ buf = NULL;
+ }
+ return boot;
+}
+
+static int cmdlineoverlay(void *boot_dtb, char *cmdline, int len)
+{
+ int chosen_node_offset = 0;
+ int ret = -1;
+ char separator[2] = {0};
+ char str_nand[32] = "ubi.mtd=";
+ char str_emmc[32] = "root=/dev/mmcblk0p";
+ char str_real[32] = {0};
+ int header_length = 0;
+ ret = extract_fdt(boot_dtb, MAX_DTB_SIZE);
+ if (ret != 0) {
+ dprintf(CRITICAL, "extract_fdt error.\n");
+ return -1;
+ }
+
+ chosen_node_offset = fdt_path_offset(boot_dtb, "/chosen");
+ char *cmdline_read;
+ int length;
+ cmdline_read = fdt_getprop(boot_dtb, chosen_node_offset, "bootargs", &length);
+ dprintf(CRITICAL, "dtsi cmdline: %s ,lenth:%zu\n", cmdline_read, strlen(cmdline_read));
+ char *pos1;
+ char *pos2;
+
+ if ((pos1 = strstr(cmdline_read,str_nand)))
+ {
+ separator[0] = ',';
+ header_length = strlen(str_nand);
+ strncpy(str_real, str_nand, header_length);
+ }
+ else if ((pos1 = strstr(cmdline_read,str_emmc)))
+ {
+ separator[0] = ' ';
+ header_length = strlen(str_emmc);
+ strncpy(str_real, str_emmc, header_length);
+ }
+ else
+ {
+ dprintf(CRITICAL, "no ubi.mtd= or root=/dev/mmcblk0p in cmdline, error!\n");
+ return -1;
+ }
+
+ pos2 = strstr(pos1, separator);
+ if (pos2 == NULL) {
+ dprintf(CRITICAL, "can not find separator in cmdline, error!\n");
+ return -1;
+ }
+ if ((pos2 - pos1 - header_length) <= 0) {
+ dprintf(CRITICAL, "no part number in cmdline, error!\n");
+ return -1;
+ }
+
+ char mtdnum_str[3];
+ char mtdnum_str_new[3];
+ strncpy(mtdnum_str, pos1 + header_length, (pos2 - pos1 - header_length));
+ mtdnum_str[pos2 - pos1 - header_length] = '\0';
+ int mtdnum = atoi(mtdnum_str);
+ mtdnum ++;
+ sprintf(mtdnum_str_new, "%d", mtdnum);
+ if (mtdnum >= 10) {
+ char half_before[1024] = {'\0'};
+ char half_behind[1024] = {'\0'};
+ strncpy(half_before, cmdline_read, pos2 - cmdline_read);
+ char *pos3 = strstr(half_before, str_real);
+ strncpy(pos3 + header_length, mtdnum_str_new, 2);
+ strncpy(half_behind, pos2, strlen(pos2) + 1);
+ cmdline_read = strncat(half_before, half_behind, strlen(half_behind));
+
+ } else {
+ strncpy(pos1 + header_length, mtdnum_str_new, 1);
+ }
+ printf("cmdline new: %s , length: %zu", cmdline_read, strlen(cmdline_read));
+ ret = fdt_setprop(boot_dtb, chosen_node_offset, "bootargs", cmdline_read, strlen(cmdline_read) + 1);
+ if (ret != 0) {
+ dprintf(CRITICAL, "fdt_setprop error.\n");
+ return -1;
+ }
+ ret = fdt_pack(boot_dtb);
+ if (ret != 0) {
+ dprintf(CRITICAL, "fdt_pack error.\n");
+ return -1;
+ }
+
+ return 0;
+}
+#endif
+
+static bool download_check(void)
+{
+ if (check_fastboot_mode()) {
+ set_clr_fastboot_mode(false);
+ dprintf(CRITICAL, "download_check: check_fastboot_mode\n");
+ return true;
+#ifdef RTC_CHECK_FASTBOOT
+ } else if (rtc_bootloader_check()) {
+ rtc_bootloader_set_clr(false);
+ printf("download_check done");
+ return true;
+#endif
+ } else {
+ dprintf(CRITICAL, "download_check: check_uart_enter:%d, check_download_key:%d\n",
+ check_uart_enter(), check_download_key());
+ return (check_uart_enter() || check_download_key());
+ }
+}
+
+static bool recovery_check(void)
+{
+ if (check_recovery_mode()) {
+ set_clr_recovery_mode(false);
+ return true;
+ } else
+ return false;
+}
+
+static int fit_load_images(void *fit, struct fit_load_data *fit_data)
+{
+ int ret;
+
+ /* TODO: decide verify policy with config. */
+ dprintf(CRITICAL, "verify fit conf sig: %s\n", fit_data->part_name);
+ ret = fit_conf_verify_sig(NULL, fit);
+ if (ret < 0)
+ return ret;
+
+ ret = fit_load_image(NULL, "kernel", fit, NULL, NULL,
+ (paddr_t *)&fit_data->kernel_entry, true);
+ if (ret && (ret != -ENOENT)) {
+ dprintf(CRITICAL, "%s load kernel failed\n", fit_data->part_name);
+ return ret;
+ }
+
+ ret = fit_load_image(NULL, "tee", fit, NULL, NULL,
+ (paddr_t *)&fit_data->trustedos_entry, true);
+ if (ret && (ret != -ENOENT)) {
+ dprintf(CRITICAL, "%s load trustedos failed\n", fit_data->part_name);
+ return ret;
+ }
+
+ ret = fit_load_image(NULL, "ramdisk", fit, NULL, NULL, NULL, true);
+ if (ret && (ret != -ENOENT)) {
+ dprintf(CRITICAL, "%s load ramdisk failed\n", fit_data->part_name);
+ return ret;
+ }
+
+ ret = fit_load_image(NULL, "fdt", fit, (addr_t *)&fit_data->dtb_load, NULL,
+ NULL, true);
+ if (ret && (ret != -ENOENT)) {
+ fit_data->dtb_load = ERR_ADDR;
+ dprintf(CRITICAL, "%s load fdt failed\n", fit_data->part_name);
+ return ret;
+ }
+
+ return 0;
+}
+
+
+static int fit_load_thread(void *arg)
+{
+ int err;
+ struct fit_load_data *fit_data = (struct fit_load_data *)arg;
+
+ if (fit_data->boot_mode == FASTBOOT_BOOT) {
+ err = fit_load_images(fit_data->buf, fit_data);
+ return err;
+ }
+
+ while (fit_data->boot_mode == NORMAL_BOOT) {
+ err = fit_get_image(fit_data->part_name, &fit_data->buf);
+ if (err)
+ break;
+
+ err = fit_load_images(fit_data->buf, fit_data);
+ if (err)
+ break;
+
+ return 0;
+ }
+
+#ifdef AB_UPGRADE_APP
+ /* For ab upgrade system, there is no recovery mode */
+#else
+ dprintf(CRITICAL, "%s try recovery mode !!\n", fit_data->part_name);
+ // RECOVERY_BOOT
+ err = fit_get_image(fit_data->recovery_part_name, &fit_data->buf);
+ if (err)
+ return err;
+
+ err = fit_load_images(fit_data->buf, fit_data);
+#endif
+
+ return err;
+}
+
+extern void ext_boot(void);
+static void fitboot_task(const struct app_descriptor *app, void *args)
+{
+ void *fit, *dtbo_buf;
+ struct fit_load_data tz, bootimg;
+ thread_t *tz_t, *bootimg_t;
+ int ret_tz, ret_bootimg;
+
+ int ret;
+ u32 boot_mode = NORMAL_BOOT;
+
+ uint bl33[] = { 0xea000005, /* b BL33_32_ENTRY | ands x5, x0, x0 */
+ 0x58000160, /* .word 0x58000160 | ldr x0, _X0 */
+ 0x58000181, /* .word 0x58000181 | ldr x1, _X1 */
+ 0x580001a2, /* .word 0x580001a2 | ldr x2, _X2 */
+ 0x580001c3, /* .word 0x580001c3 | ldr x3, _X3 */
+ 0x580001e4, /* .word 0x580001e4 | ldr x4, _X4 */
+ 0xd61f0080, /* .word 0xd61f0080 | br x4 */
+ /* BL33_32_ENTRY: | */
+ 0xe59f0030, /* ldr r0, _R0 | .word 0xe59f0030 */
+ 0xe59f1030, /* ldr r1, _R1 | .word 0xe59f1030 */
+ 0xe59f2004, /* ldr r2, _X0 | .word 0xe59f2004 */
+ 0xe59ff020, /* ldr pc, _X4 | .word 0xe59ff020 */
+ 0x00000000, /* .word 0x00000000 */
+ 0x00000000, /* _X0: .word 0x00000000 */
+ 0x00000000, /* .word 0x00000000 */
+ 0x00000000, /* _X1: .word 0x00000000 */
+ 0x00000000, /* .word 0x00000000 */
+ 0x00000000, /* _X2: .word 0x00000000 */
+ 0x00000000, /* .word 0x00000000 */
+ 0x00000000, /* _X3: .word 0x00000000 */
+ 0x00000000, /* .word 0x00000000 */
+ 0x00000000, /* _X4: .word 0x00000000 */
+ 0x00000000, /* _R0: .word 0x00000000 */
+ 0x00000000, /* _R1: .word 0x00000000 */
+ 0x00000000 /* .word 0x00000000 */
+ };
+
+ /* alloc kernel and tz buffer from mempool */
+ kernel_buf = mempool_alloc(MAX_KERNEL_SIZE, MEMPOOL_ANY);
+ tz_buf = mempool_alloc(MAX_TEE_DRAM_SIZE, MEMPOOL_ANY);
+ if (!kernel_buf || !tz_buf) {
+ dprintf(CRITICAL, "alloc buf fail, kernel %p, tz %p\n",
+ kernel_buf, tz_buf);
+ return;
+ }
+#ifdef AB_UPGRADE_APP
+ /* For ab upgrade system, there is no recovery mode */
+#else
+ /* recovery */
+ if (recovery_check()) {
+ boot_mode = RECOVERY_BOOT;
+ }
+#endif
+
+ /* fastboot */
+ if (download_check()) {
+FASTBOOT:
+ ext_boot();
+ boot_mode = FASTBOOT_BOOT;
+ }
+
+ bootimg.part_name = (char *)BOOT_PART_NAME;
+ tz.part_name = (char *)TZ_PART_NAME;
+
+#ifdef AB_UPGRADE_APP
+ /* disable wdt */
+ /*1.choose A/B boot & tz img.*/
+ int boot_part = 0;
+ boot_part = check_boot_partition("misc");
+ if (boot_part == 0) {
+ dprintf(CRITICAL, "choose first boot partition:%s , tee choose: %s\n",(char *)BOOT_PART_NAME, (char *)TZ_PART_NAME);
+ bootimg.part_name = (char *)BOOT_PART_NAME;
+ tz.part_name = (char *)TZ_PART_NAME;
+ //cmdlineoverlay(bootimg.dtb_load, NULL, 0); from b partition,need to set
+
+ } else if (boot_part == 1) {
+ dprintf(CRITICAL, "choose second boot partition: %s , tee choose: %s\n", (char *)RECOVERY_BOOT_PART_NAME, (char *)RECOVERY_TZ_PART_NAME);
+ bootimg.part_name = (char *)RECOVERY_BOOT_PART_NAME;
+ tz.part_name = (char *)RECOVERY_TZ_PART_NAME;
+
+ } else {
+ dprintf(CRITICAL, "unknow boot_part (%d), using first boot partition\n", boot_part);
+ bootimg.part_name = (char *)BOOT_PART_NAME;
+ tz.part_name = (char *)TZ_PART_NAME;
+ }
+#endif
+
+ bootimg.recovery_part_name = (char *)RECOVERY_BOOT_PART_NAME;
+ bootimg.boot_mode = boot_mode;
+ bootimg.buf = kernel_buf;
+ bootimg_t = thread_create("bootimg_ctl", fit_load_thread, &bootimg,
+ DEFAULT_PRIORITY, DEFAULT_STACK_SIZE);
+
+ /* create a tz thread to load tz */;
+ tz.recovery_part_name = (char *)RECOVERY_TZ_PART_NAME;
+ tz.boot_mode = boot_mode;
+ tz.buf = tz_buf;
+ tz_t = thread_create("tz_ctl", fit_load_thread, &tz,
+ DEFAULT_PRIORITY, DEFAULT_STACK_SIZE);
+ if (!bootimg_t || !tz_t) {
+ dprintf(CRITICAL, "create load threads failed\n");
+ return;
+ }
+
+ thread_resume(bootimg_t);
+ thread_resume(tz_t);
+
+ thread_join(bootimg_t, &ret_bootimg, INFINITE_TIME);
+ thread_join(tz_t, &ret_tz, INFINITE_TIME);
+
+ if (ret_bootimg) {
+ dprintf(CRITICAL, "load boot image failed\n");
+ goto FASTBOOT;
+ }
+
+ if (ret_tz) {
+ dprintf(CRITICAL, "load tz image failed\n");
+ goto FASTBOOT;
+ }
+
+ plat_fixup_hook((void *)bootimg.dtb_load);
+
+ dtbo_buf = mempool_alloc(MAX_DTBO_SIZE, MEMPOOL_ANY);
+ if (!dtbo_buf) {
+ dprintf(CRITICAL, "alloc dtbo buf fail\n");
+ return;
+ }
+
+#ifdef AB_UPGRADE_APP
+ /*2.overlay cmdline to choose A/B rootfs*/
+ if (boot_part == 1) {
+ dprintf(CRITICAL, "load second partitions, need to overlay cmdline\n");
+ cmdlineoverlay((void *)bootimg.dtb_load, NULL, 0);
+ }
+#endif
+
+#ifdef SET_FDT_EMI_INFO
+ /* set fdt emi info*/
+ ret = extract_fdt((void *)bootimg.dtb_load, MAX_DTB_SIZE);
+ if (ret) {
+ dprintf(CRITICAL, "extract fdt failed\n");
+ return;
+ }
+
+ ret = set_fdt_emi_info((void *)bootimg.dtb_load);
+ if (ret < 0) {
+ dprintf(CRITICAL, "failed to set fdt emi info\n");
+ }
+
+ ret = fdt_pack((void *)bootimg.dtb_load);
+ if (ret) {
+ dprintf(CRITICAL, "ft pack failed\n");
+ return;
+ }
+#endif
+
+ /* check if dtbo is existed */
+ ret = fit_get_image(DTBO_PART_NAME, &dtbo_buf);
+ if (ret == 0) {
+ void *fdt_dtbo;
+ void *fdt_dtb;
+
+ if (bootimg.dtb_load == ERR_ADDR) {
+ dprintf(CRITICAL, "dtbo failed, no dtb\n");
+ return;
+ }
+ fdt_dtb = (void *)bootimg.dtb_load;
+
+ /* extract fdt */
+ ret = extract_fdt(fdt_dtb, MAX_DTB_SIZE);
+ if (ret) {
+ dprintf(CRITICAL, "extract fdt failed\n");
+ return;
+ }
+
+ dprintf(ALWAYS, "[fitboot] do overlay\n");
+ fdt_dtbo = (void *)dtbo_buf;
+ ret = fdt_overlay_apply(fdt_dtb, fdt_dtbo);
+ if (ret) {
+ dprintf(CRITICAL, "fdt merge failed, ret %d\n", ret);
+ return;
+ }
+
+ /* pack fdt */
+ ret = fdt_pack(fdt_dtb);
+ if (ret) {
+ dprintf(CRITICAL, "ft pack failed\n");
+ return;
+ }
+ }
+
+ /* load sspm*/
+ extern void fit_load_sspm_image() __attribute__((weak));
+ if (fit_load_sspm_image)
+ fit_load_sspm_image();
+
+ /*load spmfw*/
+ extern void fit_load_spmfw_image() __attribute__((weak));
+ if (fit_load_spmfw_image)
+ fit_load_spmfw_image();
+
+ /* load bl33 for tz to jump*/
+#if WITH_KERNEL_VM
+ addr_t fdt_pa = kvaddr_to_paddr((void *)bootimg.dtb_load);
+#else
+ addr_t fdt_pa = bootimg.dtb_load;
+#endif
+ setup_bl33(bl33, fdt_pa, (uint)(bootimg.kernel_entry));
+ memmove((void *)CFG_BL33_LOAD_EP_ADDR, bl33, sizeof(bl33));
+
+ ulong bl33_pa = CFG_BL33_LOAD_EP_ADDR;
+ ulong smc_fid = 0xc2000000UL; /* only used in ARCH_ARM64 */
+
+#if ARCH_ARM64 && WITH_KERNEL_VM
+ /* 64-bit LK use non identity mapping VA, VA to PA translation needed */
+ bl33_pa = (ulong)kvaddr_to_paddr((void *)CFG_BL33_LOAD_EP_ADDR);
+#endif
+ dprintf(ALWAYS, "LK run time: %lld (us)\n", current_time_hires());
+ dprintf(ALWAYS, "jump to tz %p\n", (void *)tz.kernel_entry);
+ arch_chain_load((void *)prepare_bl2_exit, smc_fid, tz.kernel_entry, bl33_pa, 0UL);
+}
+
+APP_START(fitboot)
+.entry = fitboot_task,
+ .flags = 0,
+ APP_END
diff --git a/src/bsp/lk/app/fitboot/rules.mk b/src/bsp/lk/app/fitboot/rules.mk
new file mode 100644
index 0000000..bdc3099
--- /dev/null
+++ b/src/bsp/lk/app/fitboot/rules.mk
@@ -0,0 +1,35 @@
+LOCAL_DIR := $(GET_LOCAL_DIR)
+
+MODULE := $(LOCAL_DIR)
+MODULE_BUILDDIR := $(call TOBUILDDIR,$(MODULE))
+MODULE_INCLUDES += $(MODULE_BUILDDIR)/../../../include
+GLOBAL_INCLUDES += $(LOCAL_DIR)/../../platform/$(PLATFORM)/$(MTK_PLATFORM)/include/platform
+
+
+
+SCRATCH_SIZE ?= 0x04000000 # 64MB
+MAX_TEE_DRAM_SIZE ?= 0x04000000 # 64MB
+MAX_KERNEL_SIZE ?= 0x02000000 # 32MB
+MAX_DTB_SIZE ?= 0x00200000 # 2MB
+MAX_DTBO_SIZE ?= 0x00200000 # 2MB
+MAX_BL33_SIZE ?= 0x00100000 # 1MB
+MAX_LZ4_BUF_SIZE ?= 0x00100000 # 1MB
+
+GLOBAL_DEFINES += SCRATCH_SIZE=$(SCRATCH_SIZE) \
+ MAX_TEE_DRAM_SIZE=$(MAX_TEE_DRAM_SIZE) \
+ MAX_KERNEL_SIZE=$(MAX_KERNEL_SIZE) \
+ MAX_DTB_SIZE=$(MAX_DTB_SIZE) \
+ MAX_DTBO_SIZE=$(MAX_DTBO_SIZE) \
+ MAX_BL33_SIZE=$(MAX_BL33_SIZE) \
+ MAX_LZ4_BUF_SIZE=$(MAX_LZ4_BUF_SIZE)
+
+MODULE_DEPS += \
+ lib/bio \
+ lib/mempool \
+ lib/fdt \
+ lib/fit \
+ lib/fastboot \
+
+MODULE_SRCS += \
+ $(LOCAL_DIR)/fitboot.c \
+include make/module.mk
diff --git a/src/bsp/lk/app/inetsrv/inetsrv.c b/src/bsp/lk/app/inetsrv/inetsrv.c
new file mode 100644
index 0000000..11d412b
--- /dev/null
+++ b/src/bsp/lk/app/inetsrv/inetsrv.c
@@ -0,0 +1,238 @@
+/*
+ * Copyright (c) 2014 Travis Geiselbrecht
+ *
+ * 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 <app.h>
+#include <err.h>
+#include <debug.h>
+#include <trace.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <compiler.h>
+#include <kernel/thread.h>
+#include <lib/minip.h>
+#include <lib/tftp.h>
+#include <lib/cksum.h>
+#include <platform.h>
+
+#include "inetsrv.h"
+
+static int chargen_worker(void *socket)
+{
+ uint64_t count = 0;
+ tcp_socket_t *s = socket;
+
+/* enough buffer to hold an entire defacto chargen sequences */
+#define CHARGEN_BUFSIZE (0x5f * 0x5f) // 9025 bytes
+
+ uint8_t *buf = malloc(CHARGEN_BUFSIZE);
+ if (!buf)
+ return ERR_NO_MEMORY;
+
+ /* generate the sequence */
+ uint8_t c = '!';
+ for (size_t i = 0; i < CHARGEN_BUFSIZE; i++) {
+ buf[i] = c++;
+ if (c == 0x7f)
+ c = ' ';
+ }
+
+ lk_time_t t = current_time();
+ for (;;) {
+ ssize_t ret = tcp_write(s, buf, CHARGEN_BUFSIZE);
+ //TRACEF("tcp_write returns %d\n", ret);
+ if (ret < 0)
+ break;
+
+ count += ret;
+ }
+ t = current_time() - t;
+
+ TRACEF("chargen worker exiting, wrote %llu bytes in %u msecs (%llu bytes/sec)\n",
+ count, (uint32_t)t, count * 1000 / t);
+ free(buf);
+ tcp_close(s);
+
+ return 0;
+}
+
+static int chargen_server(void *arg)
+{
+ status_t err;
+ tcp_socket_t *listen_socket;
+
+ err = tcp_open_listen(&listen_socket, 19);
+ if (err < 0) {
+ TRACEF("error opening chargen listen socket\n");
+ return -1;
+ }
+
+ for (;;) {
+ tcp_socket_t *accept_socket;
+
+ err = tcp_accept(listen_socket, &accept_socket);
+ TRACEF("tcp_accept returns returns %d, handle %p\n", err, accept_socket);
+ if (err < 0) {
+ TRACEF("error accepting socket, retrying\n");
+ continue;
+ }
+
+ TRACEF("starting chargen worker\n");
+ thread_detach_and_resume(thread_create("chargen_worker", &chargen_worker, accept_socket, DEFAULT_PRIORITY, DEFAULT_STACK_SIZE));
+ }
+}
+
+static int discard_worker(void *socket)
+{
+ uint64_t count = 0;
+ uint32_t crc = 0;
+ tcp_socket_t *s = socket;
+
+#define DISCARD_BUFSIZE 1024
+
+ uint8_t *buf = malloc(DISCARD_BUFSIZE);
+ if (!buf) {
+ TRACEF("error allocating buffer\n");
+ }
+
+ lk_time_t t = current_time();
+ for (;;) {
+ ssize_t ret = tcp_read(s, buf, DISCARD_BUFSIZE);
+ if (ret <= 0)
+ break;
+
+ crc = crc32(crc, buf, ret);
+
+ count += ret;
+ }
+ t = current_time() - t;
+
+ TRACEF("discard worker exiting, read %llu bytes in %u msecs (%llu bytes/sec), crc32 0x%x\n",
+ count, (uint32_t)t, count * 1000 / t, crc);
+ tcp_close(s);
+
+ free(buf);
+
+ return 0;
+}
+
+static int discard_server(void *arg)
+{
+ status_t err;
+ tcp_socket_t *listen_socket;
+
+ err = tcp_open_listen(&listen_socket, 9);
+ if (err < 0) {
+ TRACEF("error opening discard listen socket\n");
+ return -1;
+ }
+
+ for (;;) {
+ tcp_socket_t *accept_socket;
+
+ err = tcp_accept(listen_socket, &accept_socket);
+ TRACEF("tcp_accept returns returns %d, handle %p\n", err, accept_socket);
+ if (err < 0) {
+ TRACEF("error accepting socket, retrying\n");
+ continue;
+ }
+
+ TRACEF("starting discard worker\n");
+ thread_detach_and_resume(thread_create("discard_worker", &discard_worker, accept_socket, DEFAULT_PRIORITY, DEFAULT_STACK_SIZE));
+ }
+}
+
+static int echo_worker(void *socket)
+{
+ tcp_socket_t *s = socket;
+
+#define ECHO_BUFSIZE 1024
+
+ uint8_t *buf = malloc(ECHO_BUFSIZE);
+ if (!buf) {
+ TRACEF("error allocating buffer\n");
+ return ERR_NO_MEMORY;
+ }
+
+ for (;;) {
+ ssize_t ret = tcp_read(s, buf, sizeof(buf));
+ if (ret <= 0)
+ break;
+
+ tcp_write(s, buf, ret);
+ if (ret <= 0)
+ break;
+ }
+
+ TRACEF("echo worker exiting\n");
+ tcp_close(s);
+ free(buf);
+
+ return 0;
+}
+
+static int echo_server(void *arg)
+{
+ status_t err;
+ tcp_socket_t *listen_socket;
+
+ err = tcp_open_listen(&listen_socket, 7);
+ if (err < 0) {
+ TRACEF("error opening echo listen socket\n");
+ return -1;
+ }
+
+ for (;;) {
+ tcp_socket_t *accept_socket;
+
+ err = tcp_accept(listen_socket, &accept_socket);
+ TRACEF("tcp_accept returns returns %d, handle %p\n", err, accept_socket);
+ if (err < 0) {
+ TRACEF("error accepting socket, retrying\n");
+ continue;
+ }
+
+ TRACEF("starting echo worker\n");
+ thread_detach_and_resume(thread_create("echo_worker", &echo_worker, accept_socket, DEFAULT_PRIORITY, DEFAULT_STACK_SIZE));
+ }
+}
+
+static void inetsrv_init(const struct app_descriptor *app)
+{
+}
+
+static void inetsrv_entry(const struct app_descriptor *app, void *args)
+{
+ /* XXX wait for the stack to initialize */
+
+ printf("starting internet servers\n");
+
+ thread_detach_and_resume(thread_create("chargen", &chargen_server, NULL, DEFAULT_PRIORITY, DEFAULT_STACK_SIZE));
+ thread_detach_and_resume(thread_create("discard", &discard_server, NULL, DEFAULT_PRIORITY, DEFAULT_STACK_SIZE));
+ thread_detach_and_resume(thread_create("echo", &echo_server, NULL, DEFAULT_PRIORITY, DEFAULT_STACK_SIZE));
+ tftp_server_init(NULL);
+}
+
+APP_START(inetsrv)
+ .init = inetsrv_init,
+ .entry = inetsrv_entry,
+ .flags = 0,
+APP_END
diff --git a/src/bsp/lk/app/inetsrv/inetsrv.h b/src/bsp/lk/app/inetsrv/inetsrv.h
new file mode 100644
index 0000000..451cd6f
--- /dev/null
+++ b/src/bsp/lk/app/inetsrv/inetsrv.h
@@ -0,0 +1,24 @@
+/*
+ * Copyright (c) 2014 Travis Geiselbrecht
+ *
+ * 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.
+ */
+#pragma once
+
diff --git a/src/bsp/lk/app/inetsrv/rules.mk b/src/bsp/lk/app/inetsrv/rules.mk
new file mode 100644
index 0000000..05bd83b
--- /dev/null
+++ b/src/bsp/lk/app/inetsrv/rules.mk
@@ -0,0 +1,14 @@
+LOCAL_DIR := $(GET_LOCAL_DIR)
+
+MODULE := $(LOCAL_DIR)
+
+MODULE_SRCS += \
+ $(LOCAL_DIR)/inetsrv.c \
+
+
+MODULE_DEPS := \
+ lib/cksum \
+ lib/minip \
+ lib/tftp \
+
+include make/module.mk
diff --git a/src/bsp/lk/app/lkboot/commands.c b/src/bsp/lk/app/lkboot/commands.c
new file mode 100644
index 0000000..c6c45f3
--- /dev/null
+++ b/src/bsp/lk/app/lkboot/commands.c
@@ -0,0 +1,413 @@
+/*
+ * Copyright (c) 2014 Brian Swetland
+ *
+ * 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 <platform.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <debug.h>
+#include <string.h>
+#include <endian.h>
+#include <malloc.h>
+#include <arch.h>
+#include <err.h>
+#include <trace.h>
+#include <pow2.h>
+
+#include <kernel/thread.h>
+#include <kernel/vm.h>
+
+#include <lib/bio.h>
+#include <lib/bootargs.h>
+#include <lib/bootimage.h>
+#include <lib/ptable.h>
+#include <lib/sysparam.h>
+
+#include <app/lkboot.h>
+
+#if PLATFORM_ZYNQ
+#include <platform/fpga.h>
+#include <platform/zynq.h>
+#endif
+
+#define bootdevice "spi0"
+
+#define LOCAL_TRACE 0
+
+struct lkb_command {
+ struct lkb_command *next;
+ const char *name;
+ lkb_handler_t handler;
+ void *cookie;
+};
+
+struct lkb_command *lkb_cmd_list = NULL;
+
+void lkb_register(const char *name, lkb_handler_t handler, void *cookie) {
+ struct lkb_command *cmd = malloc(sizeof(struct lkb_command));
+ if (cmd != NULL) {
+ cmd->next = lkb_cmd_list;
+ cmd->name = name;
+ cmd->handler = handler;
+ cmd->cookie = cookie;
+ lkb_cmd_list = cmd;
+ }
+}
+
+static int do_reboot(void *arg) {
+ thread_sleep(250);
+ platform_halt(HALT_ACTION_REBOOT, HALT_REASON_SW_RESET);
+ return 0;
+}
+
+struct chainload_args {
+ void *func;
+ ulong args[4];
+};
+
+static int chainload_thread(void *arg)
+{
+ struct chainload_args *args = (struct chainload_args *)arg;
+
+ thread_sleep(250);
+
+ TRACEF("chain loading address %p, args 0x%lx 0x%lx 0x%lx 0x%lx\n",
+ args->func, args->args[0], args->args[1], args->args[2], args->args[3]);
+ arch_chain_load((void *)args->func, args->args[0], args->args[1], args->args[2], args->args[3]);
+
+ for (;;);
+}
+
+static int do_boot(lkb_t *lkb, size_t len, const char **result)
+{
+ LTRACEF("lkb %p, len %zu, result %p\n", lkb, len, result);
+
+ void *buf;
+ paddr_t buf_phys;
+
+ if (vmm_alloc_contiguous(vmm_get_kernel_aspace(), "lkboot_iobuf",
+ len, &buf, log2_uint(1024*1024), 0, ARCH_MMU_FLAG_UNCACHED) < 0) {
+ *result = "not enough memory";
+ return -1;
+ }
+ arch_mmu_query((vaddr_t)buf, &buf_phys, NULL);
+ LTRACEF("iobuffer %p (phys 0x%lx)\n", buf, buf_phys);
+
+ if (lkb_read(lkb, buf, len)) {
+ *result = "io error";
+ // XXX free buffer here
+ return -1;
+ }
+
+ /* construct a boot argument list */
+ const size_t bootargs_size = PAGE_SIZE;
+#if 0
+ void *args = (void *)((uintptr_t)lkb_iobuffer + lkb_iobuffer_size - bootargs_size);
+ paddr_t args_phys = lkb_iobuffer_phys + lkb_iobuffer_size - bootargs_size;
+#elif PLATFORM_ZYNQ
+ /* grab the top page of sram */
+ /* XXX do this better */
+ paddr_t args_phys = SRAM_BASE + SRAM_SIZE - bootargs_size;
+ void *args = paddr_to_kvaddr(args_phys);
+#else
+#error need better way
+#endif
+ LTRACEF("boot args %p, phys 0x%lx, len %zu\n", args, args_phys, bootargs_size);
+
+ bootargs_start(args, bootargs_size);
+ bootargs_add_command_line(args, bootargs_size, "what what");
+ arch_clean_cache_range((vaddr_t)args, bootargs_size);
+
+ ulong lk_args[4];
+ bootargs_generate_lk_arg_values(args_phys, lk_args);
+
+ const void *ptr;
+
+ /* sniff it to see if it's a bootimage or a raw image */
+ bootimage_t *bi;
+ if (bootimage_open(buf, len, &bi) >= 0) {
+ size_t len;
+
+ /* it's a bootimage */
+ TRACEF("detected bootimage\n");
+
+ /* find the lk image */
+ if (bootimage_get_file_section(bi, TYPE_LK, &ptr, &len) >= 0) {
+ TRACEF("found lk section at %p\n", ptr);
+
+ /* add the boot image to the argument list */
+ size_t bootimage_size;
+ bootimage_get_range(bi, NULL, &bootimage_size);
+
+ bootargs_add_bootimage_pointer(args, bootargs_size, "pmem", buf_phys, bootimage_size);
+ }
+ } else {
+ /* raw image, just chain load it directly */
+ TRACEF("raw image, chainloading\n");
+
+ ptr = buf;
+ }
+
+ /* start a boot thread to complete the startup */
+ static struct chainload_args cl_args;
+
+ cl_args.func = (void *)ptr;
+ cl_args.args[0] = lk_args[0];
+ cl_args.args[1] = lk_args[1];
+ cl_args.args[2] = lk_args[2];
+ cl_args.args[3] = lk_args[3];
+
+ thread_resume(thread_create("boot", &chainload_thread, &cl_args,
+ DEFAULT_PRIORITY, DEFAULT_STACK_SIZE));
+
+ return 0;
+}
+
+/* try to boot the system from a flash partition */
+status_t do_flash_boot(void)
+{
+ status_t err;
+
+ LTRACE_ENTRY;
+
+ /* construct a boot argument list */
+ const size_t bootargs_size = PAGE_SIZE;
+#if 0
+ /* old code */
+ void *args = (void *)((uintptr_t)lkb_iobuffer + lkb_iobuffer_size - bootargs_size);
+ paddr_t args_phys = lkb_iobuffer_phys + lkb_iobuffer_size - bootargs_size;
+#elif PLATFORM_ZYNQ
+ /* grab the top page of sram */
+ paddr_t args_phys = SRAM_BASE + SRAM_SIZE - bootargs_size;
+ void *args = paddr_to_kvaddr(args_phys);
+#else
+#error need better way
+#endif
+ LTRACEF("boot args %p, phys 0x%lx, len %zu\n", args, args_phys, bootargs_size);
+
+ bootargs_start(args, bootargs_size);
+ bootargs_add_command_line(args, bootargs_size, "what what");
+ arch_clean_cache_range((vaddr_t)args, bootargs_size);
+
+ ulong lk_args[4];
+ bootargs_generate_lk_arg_values(args_phys, lk_args);
+
+ const void *ptr;
+
+ if (!ptable_found_valid()) {
+ TRACEF("ptable not found\n");
+ return ERR_NOT_FOUND;
+ }
+
+ /* find the system partition */
+ struct ptable_entry entry;
+ err = ptable_find("system", &entry);
+ if (err < 0) {
+ TRACEF("cannot find system partition\n");
+ return ERR_NOT_FOUND;
+ }
+
+ /* get a direct pointer to the device */
+ bdev_t *bdev = ptable_get_device();
+ if (!bdev) {
+ TRACEF("error opening boot device\n");
+ return ERR_NOT_FOUND;
+ }
+
+ /* convert the bdev to a memory pointer */
+ err = bio_ioctl(bdev, BIO_IOCTL_GET_MEM_MAP, (void *)&ptr);
+ TRACEF("err %d, ptr %p\n", err, ptr);
+ if (err < 0) {
+ TRACEF("error getting direct pointer to block device\n");
+ return ERR_NOT_FOUND;
+ }
+
+ /* sniff it to see if it's a bootimage or a raw image */
+ bootimage_t *bi;
+ if (bootimage_open((char *)ptr + entry.offset, entry.length, &bi) >= 0) {
+ size_t len;
+
+ /* it's a bootimage */
+ TRACEF("detected bootimage\n");
+
+ /* find the lk image */
+ if (bootimage_get_file_section(bi, TYPE_LK, &ptr, &len) >= 0) {
+ TRACEF("found lk section at %p\n", ptr);
+
+ /* add the boot image to the argument list */
+ size_t bootimage_size;
+ bootimage_get_range(bi, NULL, &bootimage_size);
+
+ bootargs_add_bootimage_pointer(args, bootargs_size, bdev->name, entry.offset, bootimage_size);
+ }
+ } else {
+ /* did not find a bootimage, abort */
+ bio_ioctl(bdev, BIO_IOCTL_PUT_MEM_MAP, NULL);
+ return ERR_NOT_FOUND;
+ }
+
+ TRACEF("chain loading binary at %p\n", ptr);
+ arch_chain_load((void *)ptr, lk_args[0], lk_args[1], lk_args[2], lk_args[3]);
+
+ /* put the block device back into block mode (though we never get here) */
+ bio_ioctl(bdev, BIO_IOCTL_PUT_MEM_MAP, NULL);
+
+ return NO_ERROR;
+}
+
+// return NULL for success, error string for failure
+int lkb_handle_command(lkb_t *lkb, const char *cmd, const char *arg, size_t len, const char **result)
+{
+ *result = NULL;
+
+ struct lkb_command *lcmd;
+ for (lcmd = lkb_cmd_list; lcmd; lcmd = lcmd->next) {
+ if (!strcmp(lcmd->name, cmd)) {
+ *result = lcmd->handler(lkb, arg, len, lcmd->cookie);
+ return 0;
+ }
+ }
+
+ if (!strcmp(cmd, "flash") || !strcmp(cmd, "erase")) {
+ struct ptable_entry entry;
+ bdev_t *bdev;
+
+ if (ptable_find(arg, &entry) < 0) {
+ size_t plen = len;
+ /* doesn't exist, make one */
+ if (ptable_add(arg, plen, 0) < 0) {
+ *result = "error creating partition";
+ return -1;
+ }
+
+ if (ptable_find(arg, &entry) < 0) {
+ *result = "couldn't find partition after creating it";
+ return -1;
+ }
+ }
+ if (len > entry.length) {
+ *result = "partition too small";
+ return -1;
+ }
+
+ if (!(bdev = ptable_get_device())) {
+ *result = "ptable_get_device failed";
+ return -1;
+ }
+
+ printf("lkboot: erasing partition of size %llu\n", entry.length);
+ if (bio_erase(bdev, entry.offset, entry.length) != (ssize_t)entry.length) {
+ *result = "bio_erase failed";
+ return -1;
+ }
+
+ if (!strcmp(cmd, "flash")) {
+ printf("lkboot: writing to partition\n");
+
+ void *buf = malloc(bdev->block_size);
+ if (!buf) {
+ *result = "memory allocation failed";
+ return -1;
+ }
+
+ size_t pos = 0;
+ while (pos < len) {
+ size_t toread = MIN(len - pos, bdev->block_size);
+
+ LTRACEF("offset %zu, toread %zu\n", pos, toread);
+
+ if (lkb_read(lkb, buf, toread)) {
+ *result = "io error";
+ free(buf);
+ return -1;
+ }
+
+ if (bio_write(bdev, buf, entry.offset + pos, toread) != (ssize_t)toread) {
+ *result = "bio_write failed";
+ free(buf);
+ return -1;
+ }
+
+ pos += toread;
+ }
+
+ free(buf);
+ }
+ } else if (!strcmp(cmd, "remove")) {
+ if (ptable_remove(arg) < 0) {
+ *result = "remove failed";
+ return -1;
+ }
+ } else if (!strcmp(cmd, "fpga")) {
+#if PLATFORM_ZYNQ
+ void *buf = malloc(len);
+ if (!buf) {
+ *result = "error allocating buffer";
+ return -1;
+ }
+
+ /* translate to physical address */
+ paddr_t pa = kvaddr_to_paddr(buf);
+ if (pa == 0) {
+ *result = "error allocating buffer";
+ free(buf);
+ return -1;
+
+ }
+
+ if (lkb_read(lkb, buf, len)) {
+ *result = "io error";
+ free(buf);
+ return -1;
+ }
+
+ /* make sure the cache is flushed for this buffer for DMA coherency purposes */
+ arch_clean_cache_range((vaddr_t)buf, len);
+
+ /* program the fpga */
+ zynq_reset_fpga();
+ zynq_program_fpga(pa, len);
+
+ free(buf);
+#else
+ *result = "no fpga";
+ return -1;
+#endif
+ } else if (!strcmp(cmd, "boot")) {
+ return do_boot(lkb, len, result);
+ } else if (!strcmp(cmd, "getsysparam")) {
+ const void *ptr;
+ size_t len;
+ if (sysparam_get_ptr(arg, &ptr, &len) == 0) {
+ lkb_write(lkb, ptr, len);
+ }
+ } else if (!strcmp(cmd, "reboot")) {
+ thread_resume(thread_create("reboot", &do_reboot, NULL,
+ DEFAULT_PRIORITY, DEFAULT_STACK_SIZE));
+ } else {
+ *result = "unknown command";
+ return -1;
+ }
+
+ return 0;
+}
diff --git a/src/bsp/lk/app/lkboot/dcc.c b/src/bsp/lk/app/lkboot/dcc.c
new file mode 100644
index 0000000..e12b99b
--- /dev/null
+++ b/src/bsp/lk/app/lkboot/dcc.c
@@ -0,0 +1,259 @@
+/*
+ * Copyright (c) 2015 Travis Geiselbrecht
+ *
+ * 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 "lkboot.h"
+
+#include <stdio.h>
+#include <debug.h>
+#include <string.h>
+#include <compiler.h>
+#include <err.h>
+#include <assert.h>
+#include <trace.h>
+#include <stdlib.h>
+#include <lib/cbuf.h>
+#include <app/lkboot.h>
+#include <arch/arm/dcc.h>
+#include <arch/mmu.h>
+#include <kernel/mutex.h>
+
+#include "pdcc.h"
+
+#define LOCAL_TRACE 0
+
+static struct pdcc_buffer_descriptor buffer_desc __ALIGNED(256);
+static paddr_t buffer_desc_phys;
+
+#define DCC_BUFLEN 256
+
+static uint8_t htod_buffer[DCC_BUFLEN] __ALIGNED(CACHE_LINE);
+static uint8_t dtoh_buffer[DCC_BUFLEN] __ALIGNED(CACHE_LINE);
+
+static uint htod_index;
+static uint htod_pos;
+static bool dtoh_filled;
+
+static void send_pdcc_command(uint32_t opcode, uint32_t data)
+{
+ uint32_t word;
+
+ word = PDCC_VALID |
+ ((opcode & 0x7f) << PDCC_OPCODE_SHIFT) |
+ (data & 0x00ffffff);
+
+ // XXX may block forever
+ LTRACEF("sending 0x%x\n", word);
+ arm_dcc_write(&word, 1, INFINITE_TIME);
+}
+
+static void send_buffer_header(void)
+{
+ send_pdcc_command(PDCC_OP_BUF_HEADER, buffer_desc_phys / 256);
+}
+
+static void send_reset(void)
+{
+ send_pdcc_command(PDCC_OP_RESET, 0);
+}
+
+static void send_out_index_update(uint32_t index)
+{
+ send_pdcc_command(PDCC_OP_UPDATE_OUT_INDEX, index);
+}
+
+static void send_buffer_consumed(void)
+{
+ send_pdcc_command(PDCC_OP_CONSUMED_IN, 0);
+}
+
+#define DCC_PROCESS_RESET 1
+static int dcc_process_opcode(uint32_t word)
+{
+ int ret = 0;
+
+ if (word & PDCC_VALID) {
+ uint32_t opcode = PDCC_OPCODE(word);
+ uint32_t data = PDCC_DATA(word);
+ LTRACEF("word 0x%x, opcode 0x%x, data 0x%x\n", word, opcode, data);
+ switch (opcode) {
+ case PDCC_OP_RESET:
+ htod_index = 0;
+ htod_pos = 0;
+ dtoh_filled = false;
+
+ // try to send the buffer header
+ send_buffer_header();
+ ret = DCC_PROCESS_RESET;
+ break;
+ case PDCC_OP_BUF_HEADER:
+ // we shouldn't get this
+ break;
+
+ case PDCC_OP_UPDATE_OUT_INDEX:
+ if (data > DCC_BUFLEN) {
+ // out of range
+ send_reset();
+ } else {
+ htod_index = data;
+ htod_pos = 0;
+ arch_invalidate_cache_range((vaddr_t)htod_buffer, DCC_BUFLEN);
+ }
+ break;
+
+ case PDCC_OP_CONSUMED_IN:
+ arch_invalidate_cache_range((vaddr_t)dtoh_buffer, DCC_BUFLEN);
+ dtoh_filled = false;
+ break;
+ default:
+ TRACEF("bad opcode from host 0x%x\n", opcode);
+ send_reset();
+ }
+ }
+
+ return ret;
+}
+
+static ssize_t dcc_read(void *unused, void *_data, size_t len)
+{
+ unsigned char *data = _data;
+ size_t pos = 0;
+ uint32_t dcc;
+
+ LTRACEF("buf %p, len %zu, htod_pos %u, htod_index %u\n", _data, len, htod_pos, htod_index);
+
+ lk_time_t timeout = 0; // first dcc command should be with no timeout
+ while (pos < len) {
+ // process a dcc command
+ ssize_t err = arm_dcc_read(&dcc, 1, timeout);
+ if (err > 0) {
+ err = dcc_process_opcode(dcc);
+ if (err == DCC_PROCESS_RESET) {
+ return ERR_IO;
+ }
+ }
+
+ // see if there is any data in the incoming buffer
+ if (htod_index > 0) {
+ size_t tocopy = MIN(htod_index - htod_pos, len - pos);
+
+ memcpy(&data[pos], &htod_buffer[htod_pos], tocopy);
+ pos += tocopy;
+ htod_pos += tocopy;
+
+ // if we consumed everything, tell the host we're done with the buffer
+ if (htod_pos == htod_index) {
+ send_buffer_consumed();
+ htod_index = 0;
+ htod_pos = 0;
+ arch_invalidate_cache_range((vaddr_t)htod_buffer, DCC_BUFLEN);
+ }
+ }
+
+ timeout = 1000;
+ }
+
+ return 0;
+}
+
+static ssize_t dcc_write(void *unused, const void *_data, size_t len)
+{
+ const unsigned char *data = _data;
+ size_t pos = 0;
+
+ LTRACEF("buf %p, len %zu\n", _data, len);
+
+ while (pos < len) {
+ LTRACEF("pos %zu, len %zu, dtoh_filled %d\n", pos, len, dtoh_filled);
+ if (!dtoh_filled) {
+ // put as much data as we can in the outgoing buffer
+ size_t tocopy = MIN(len, DCC_BUFLEN);
+
+ LTRACEF("tocopy %zu\n", tocopy);
+ memcpy(dtoh_buffer, data, tocopy);
+ arch_clean_cache_range((vaddr_t)dtoh_buffer, DCC_BUFLEN);
+ send_out_index_update(tocopy);
+ dtoh_filled = true;
+
+ pos += tocopy;
+ }
+
+ // process a dcc command
+ uint32_t dcc;
+ ssize_t err = arm_dcc_read(&dcc, 1, 1000);
+ if (err > 0) {
+ err = dcc_process_opcode(dcc);
+ if (err == DCC_PROCESS_RESET) {
+ return ERR_IO;
+ }
+ }
+ }
+
+ return pos;
+}
+
+lkb_t *lkboot_check_dcc_open(void)
+{
+ lkb_t *lkb = NULL;
+
+ // read a dcc op and process it
+ {
+ uint32_t dcc;
+ ssize_t err = arm_dcc_read(&dcc, 1, 0);
+ if (err > 0) {
+ err = dcc_process_opcode(dcc);
+ }
+ }
+
+ if (htod_index > 0) {
+ // we have data, construct a lkb and return it
+ LTRACEF("we have data on dcc, starting command handler\n");
+ lkb = lkboot_create_lkb(NULL, dcc_read, dcc_write);
+ }
+
+ return lkb;
+}
+
+void lkboot_dcc_init(void)
+{
+ paddr_t pa;
+ __UNUSED status_t err;
+
+ buffer_desc.version = PDCC_VERSION;
+
+ err = arch_mmu_query((vaddr_t)htod_buffer, &pa, NULL);
+ DEBUG_ASSERT(err == NO_ERROR);
+
+ buffer_desc.htod_buffer_phys = pa;
+ buffer_desc.htod_buffer_len = DCC_BUFLEN;
+
+ err = arch_mmu_query((vaddr_t)dtoh_buffer, &pa, NULL);
+ DEBUG_ASSERT(err == NO_ERROR);
+
+ buffer_desc.dtoh_buffer_phys = pa;
+ buffer_desc.dtoh_buffer_len = DCC_BUFLEN;
+
+ err = arch_mmu_query((vaddr_t)&buffer_desc, &buffer_desc_phys, NULL);
+ DEBUG_ASSERT(err == NO_ERROR);
+
+ arch_clean_cache_range((vaddr_t)&buffer_desc, sizeof(buffer_desc));
+}
+
diff --git a/src/bsp/lk/app/lkboot/include/app/lkboot.h b/src/bsp/lk/app/lkboot/include/app/lkboot.h
new file mode 100644
index 0000000..03f81a1
--- /dev/null
+++ b/src/bsp/lk/app/lkboot/include/app/lkboot.h
@@ -0,0 +1,43 @@
+/*
+ * Copyright (c) 2014 Brian Swetland
+ *
+ * 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.
+ */
+
+#pragma once
+
+typedef struct LKB lkb_t;
+
+// lkb_read/write may *only* be called from within a lkb_handler()
+// len of 0 is invalid for both
+
+// returns 0 on success, -1 on failure (io error, etc)
+int lkb_read(lkb_t *lkb, void *data, size_t len);
+int lkb_write(lkb_t *lkb, const void *data, size_t len);
+
+// len is the number of bytes the host has declared that it will send
+// use lkb_read() to read some or all of this data
+// return NULL on success, or an asciiz string (message) for error
+typedef const char* (*lkb_handler_t)(lkb_t *lkb,
+ const char *arg, unsigned len, void *cookie);
+
+// cmd must be a string constant
+void lkb_register(const char *cmd, lkb_handler_t handler, void *cookie);
+
diff --git a/src/bsp/lk/app/lkboot/inet.c b/src/bsp/lk/app/lkboot/inet.c
new file mode 100644
index 0000000..defac38
--- /dev/null
+++ b/src/bsp/lk/app/lkboot/inet.c
@@ -0,0 +1,66 @@
+/*
+ * Copyright (c) 2015 Travis Geiselbrecht
+ *
+ * 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.
+ */
+
+#if WITH_LIB_MINIP
+#include <app.h>
+
+#include <platform.h>
+#include <stdio.h>
+#include <debug.h>
+#include <string.h>
+#include <pow2.h>
+#include <err.h>
+#include <assert.h>
+#include <trace.h>
+
+#include <app/lkboot.h>
+
+#include "lkboot.h"
+
+#include <lib/minip.h>
+
+#define LOCAL_TRACE 0
+
+static ssize_t tcp_readx(void *s, void *_data, size_t len) {
+ char *data = _data;
+ while (len > 0) {
+ int r = tcp_read(s, data, len);
+ if (r <= 0) return -1;
+ data += r;
+ len -= r;
+ }
+ return 0;
+}
+
+lkb_t *lkboot_tcp_opened(void *s)
+{
+ lkb_t *lkb;
+
+ lkb = lkboot_create_lkb(s, tcp_readx, (void *)tcp_write);
+ if (!lkb)
+ return NULL;
+
+ return lkb;
+}
+
+#endif
diff --git a/src/bsp/lk/app/lkboot/lkboot.c b/src/bsp/lk/app/lkboot/lkboot.c
new file mode 100644
index 0000000..a5b432e
--- /dev/null
+++ b/src/bsp/lk/app/lkboot/lkboot.c
@@ -0,0 +1,354 @@
+/*
+ * Copyright (c) 2014 Brian Swetland
+ *
+ * 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 "lkboot.h"
+
+#include <app.h>
+
+#include <platform.h>
+#include <stdio.h>
+#include <debug.h>
+#include <string.h>
+#include <pow2.h>
+#include <err.h>
+#include <assert.h>
+#include <trace.h>
+
+#include <lib/sysparam.h>
+
+#include <kernel/thread.h>
+#include <kernel/mutex.h>
+
+#include <kernel/vm.h>
+#include <app/lkboot.h>
+
+#if WITH_LIB_MINIP
+#include <lib/minip.h>
+#endif
+
+#ifndef LKBOOT_WITH_SERVER
+#define LKBOOT_WITH_SERVER 1
+#endif
+#ifndef LKBOOT_AUTOBOOT
+#define LKBOOT_AUTOBOOT 1
+#endif
+#ifndef LKBOOT_AUTOBOOT_TIMEOUT
+#define LKBOOT_AUTOBOOT_TIMEOUT 5000
+#endif
+
+#define LOCAL_TRACE 0
+
+#define STATE_OPEN 0
+#define STATE_DATA 1
+#define STATE_RESP 2
+#define STATE_DONE 3
+#define STATE_ERROR 4
+
+typedef struct LKB {
+ lkb_read_hook *read;
+ lkb_write_hook *write;
+
+ void *cookie;
+
+ int state;
+ size_t avail;
+} lkb_t;
+
+lkb_t *lkboot_create_lkb(void *cookie, lkb_read_hook *read, lkb_write_hook *write) {
+ lkb_t *lkb = malloc(sizeof(lkb_t));
+ if (!lkb)
+ return NULL;
+
+ lkb->cookie = cookie;
+ lkb->state = STATE_OPEN;
+ lkb->avail = 0;
+ lkb->read = read;
+ lkb->write = write;
+
+ return lkb;
+}
+
+static int lkb_send(lkb_t *lkb, u8 opcode, const void *data, size_t len) {
+ msg_hdr_t hdr;
+
+ // once we sent our OKAY or FAIL or errored out, no more writes
+ if (lkb->state >= STATE_DONE) return -1;
+
+ switch (opcode) {
+ case MSG_OKAY:
+ case MSG_FAIL:
+ lkb->state = STATE_DONE;
+ if (len > 0xFFFF) return -1;
+ break;
+ case MSG_LOG:
+ if (len > 0xFFFF) return -1;
+ break;
+ case MSG_SEND_DATA:
+ if (len > 0x10000) return -1;
+ break;
+ case MSG_GO_AHEAD:
+ if (lkb->state == STATE_OPEN) {
+ lkb->state = STATE_DATA;
+ break;
+ }
+ len = 0;
+ default:
+ lkb->state = STATE_ERROR;
+ opcode = MSG_FAIL;
+ data = "internal error";
+ len = 14;
+ break;
+ }
+
+ hdr.opcode = opcode;
+ hdr.extra = 0;
+ hdr.length = (opcode == MSG_SEND_DATA) ? (len - 1) : len;
+ if (lkb->write(lkb->cookie, &hdr, sizeof(hdr)) != sizeof(&hdr)) {
+ printf("xmit hdr fail\n");
+ lkb->state = STATE_ERROR;
+ return -1;
+ }
+ if (len && (lkb->write(lkb->cookie, data, len) != (ssize_t)len)) {
+ printf("xmit data fail\n");
+ lkb->state = STATE_ERROR;
+ return -1;
+ }
+ return 0;
+}
+
+#define lkb_okay(lkb) lkb_send(lkb, MSG_OKAY, NULL, 0)
+#define lkb_fail(lkb, msg) lkb_send(lkb, MSG_FAIL, msg, strlen(msg))
+
+int lkb_write(lkb_t *lkb, const void *_data, size_t len) {
+ const char *data = _data;
+ while (len > 0) {
+ size_t xfer = (len > 65536) ? 65536 : len;
+ if (lkb_send(lkb, MSG_SEND_DATA, data, xfer)) return -1;
+ len -= xfer;
+ data += xfer;
+ }
+ return 0;
+}
+
+int lkb_read(lkb_t *lkb, void *_data, size_t len) {
+ char *data = _data;
+
+ if (lkb->state == STATE_RESP) {
+ return 0;
+ }
+ if (lkb->state == STATE_OPEN) {
+ if (lkb_send(lkb, MSG_GO_AHEAD, NULL, 0)) return -1;
+ }
+ while (len > 0) {
+ if (lkb->avail == 0) {
+ msg_hdr_t hdr;
+ if (lkb->read(lkb->cookie, &hdr, sizeof(hdr))) goto fail;
+ if (hdr.opcode == MSG_END_DATA) {
+ lkb->state = STATE_RESP;
+ return -1;
+ }
+ if (hdr.opcode != MSG_SEND_DATA) goto fail;
+ lkb->avail = ((size_t) hdr.length) + 1;
+ }
+ if (lkb->avail >= len) {
+ if (lkb->read(lkb->cookie, data, len)) goto fail;
+ lkb->avail -= len;
+ return 0;
+ }
+ if (lkb->read(lkb->cookie, data, lkb->avail)) {
+ lkb->state = STATE_ERROR;
+ return -1;
+ }
+ data += lkb->avail;
+ len -= lkb->avail;
+ lkb->avail = 0;
+ }
+ return 0;
+
+fail:
+ lkb->state = STATE_ERROR;
+ return -1;
+}
+
+status_t lkboot_process_command(lkb_t *lkb)
+{
+ msg_hdr_t hdr;
+ char cmd[128];
+ char *arg;
+ int err;
+ const char *result;
+ unsigned len;
+
+ if (lkb->read(lkb->cookie, &hdr, sizeof(hdr))) goto fail;
+ if (hdr.opcode != MSG_CMD) goto fail;
+ if (hdr.length > 127) goto fail;
+ if (lkb->read(lkb->cookie, cmd, hdr.length)) goto fail;
+ cmd[hdr.length] = 0;
+
+ TRACEF("recv '%s'\n", cmd);
+
+ if (!(arg = strchr(cmd, ':'))) goto fail;
+ *arg++ = 0;
+ len = atoul(arg);
+ if (!(arg = strchr(arg, ':'))) goto fail;
+ arg++;
+
+ err = lkb_handle_command(lkb, cmd, arg, len, &result);
+ if (err >= 0) {
+ lkb_okay(lkb);
+ } else {
+ lkb_fail(lkb, result);
+ }
+
+ TRACEF("command handled with success\n");
+ return NO_ERROR;
+
+fail:
+ TRACEF("command failed\n");
+ return ERR_IO;
+}
+
+static status_t lkboot_server(lk_time_t timeout)
+{
+ lkboot_dcc_init();
+
+#if WITH_LIB_MINIP
+ /* open the server's socket */
+ tcp_socket_t *listen_socket = NULL;
+ if (tcp_open_listen(&listen_socket, 1023) < 0) {
+ printf("lkboot: error opening listen socket\n");
+ return ERR_NO_MEMORY;
+ }
+#endif
+
+ /* run the main lkserver loop */
+ printf("lkboot: starting server\n");
+ lk_time_t t = current_time(); /* remember when we started */
+ for (;;) {
+ bool handled_command = false;
+
+ lkb_t *lkb;
+
+#if WITH_LIB_MINIP
+ /* wait for a new connection */
+ lk_time_t sock_timeout = 100;
+ tcp_socket_t *s;
+ if (tcp_accept_timeout(listen_socket, &s, sock_timeout) >= 0) {
+ DEBUG_ASSERT(s);
+
+ /* handle the command and close it */
+ lkb = lkboot_tcp_opened(s);
+ lkboot_process_command(lkb);
+ free(lkb);
+ tcp_close(s);
+ handled_command = true;
+ }
+#endif
+
+ /* check if anything is coming in on dcc */
+ lkb = lkboot_check_dcc_open();
+ if (lkb) {
+ lkboot_process_command(lkb);
+ free(lkb);
+ handled_command = true;
+ }
+
+ /* after the first command, stay in the server loop forever */
+ if (handled_command && timeout != INFINITE_TIME) {
+ timeout = INFINITE_TIME;
+ printf("lkboot: handled command, staying in server loop\n");
+ }
+
+ /* see if we need to drop out and try to direct boot */
+ if (timeout != INFINITE_TIME && (current_time() - t >= timeout)) {
+ break;
+ }
+ }
+
+#if WITH_LIB_MINIP
+ tcp_close(listen_socket);
+#endif
+
+ printf("lkboot: server timed out\n");
+
+ return ERR_TIMED_OUT;
+}
+
+/* platform code can override this to conditionally abort autobooting from flash */
+__WEAK bool platform_abort_autoboot(void)
+{
+ return false;
+}
+
+static void lkboot_task(const struct app_descriptor *app, void *args)
+{
+ /* read a few sysparams to decide if we're going to autoboot */
+ uint8_t autoboot = 1;
+ sysparam_read("lkboot.autoboot", &autoboot, sizeof(autoboot));
+
+ /* let platform code have a shot at disabling the autoboot behavior */
+ if (platform_abort_autoboot())
+ autoboot = 0;
+
+#if !LKBOOT_AUTOBOOT
+ autoboot = 0;
+#endif
+
+ /* if we're going to autoobot, read the timeout value */
+ lk_time_t autoboot_timeout;
+ if (!autoboot) {
+ autoboot_timeout = INFINITE_TIME;
+ } else {
+ autoboot_timeout = LKBOOT_AUTOBOOT_TIMEOUT;
+ sysparam_read("lkboot.autoboot_timeout", &autoboot_timeout, sizeof(autoboot_timeout));
+ }
+
+ TRACEF("autoboot %u autoboot_timeout %u\n", autoboot, (uint)autoboot_timeout);
+
+#if LKBOOT_WITH_SERVER
+ lkboot_server(autoboot_timeout);
+#else
+ if (autoboot_timeout != INFINITE_TIME) {
+ TRACEF("waiting for %u milliseconds before autobooting\n", (uint)autoboot_timeout);
+ thread_sleep(autoboot_timeout);
+ }
+#endif
+
+ if (autoboot_timeout != INFINITE_TIME) {
+ TRACEF("trying to boot from flash...\n");
+ status_t err = do_flash_boot();
+ TRACEF("do_flash_boot returns %d\n", err);
+ }
+
+#if LKBOOT_WITH_SERVER
+ TRACEF("restarting server\n");
+ lkboot_server(INFINITE_TIME);
+#endif
+
+ TRACEF("nothing to do, exiting\n");
+}
+
+APP_START(lkboot)
+ .entry = lkboot_task,
+ .flags = 0,
+APP_END
diff --git a/src/bsp/lk/app/lkboot/lkboot.h b/src/bsp/lk/app/lkboot/lkboot.h
new file mode 100644
index 0000000..4d3bb1c
--- /dev/null
+++ b/src/bsp/lk/app/lkboot/lkboot.h
@@ -0,0 +1,47 @@
+/*
+ * Copyright (c) 2015 Travis Geiselbrecht
+ *
+ * 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.
+ */
+#pragma once
+
+#include <sys/types.h>
+#include <app/lkboot.h>
+#include "lkboot_protocol.h"
+
+/* private to lkboot app */
+
+int lkb_handle_command(lkb_t *lkb, const char *cmd, const char *arg, size_t len, const char **result);
+
+status_t do_flash_boot(void);
+
+typedef ssize_t lkb_read_hook(void *s, void *data, size_t len);
+typedef ssize_t lkb_write_hook(void *s, const void *data, size_t len);
+
+lkb_t *lkboot_create_lkb(void *cookie, lkb_read_hook *read, lkb_write_hook *write);
+status_t lkboot_process_command(lkb_t *);
+
+/* inet server */
+lkb_t *lkboot_tcp_opened(void *s);
+
+/* dcc based server */
+void lkboot_dcc_init(void);
+lkb_t *lkboot_check_dcc_open(void);
+
diff --git a/src/bsp/lk/app/lkboot/lkboot_protocol.h b/src/bsp/lk/app/lkboot/lkboot_protocol.h
new file mode 100644
index 0000000..bc34b09
--- /dev/null
+++ b/src/bsp/lk/app/lkboot/lkboot_protocol.h
@@ -0,0 +1,80 @@
+/*
+ * Copyright (c) 2014 Brian Swetland
+ *
+ * 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.
+ */
+#pragma once
+
+typedef struct {
+ unsigned char opcode;
+ unsigned char extra;
+ unsigned short length;
+} msg_hdr_t;
+
+// unless otherwise specified, extra is always zero.
+
+#define MSG_OKAY 0x00
+// length must be zero.
+// server indicates command was successful.
+
+#define MSG_FAIL 0xFF
+// length may be nonzero, if so data is a human readable error message
+// extra may be nonzero, if so it is a more specific error code
+
+#define MSG_LOG 0xFE
+// data contains human readable log message from server
+// server may issue these at any time
+
+#define MSG_GO_AHEAD 0x01
+// length must be zero
+// server indicates that command was valid and it is ready for data
+// client should send MSG_SEND_DATA messages to transfer data
+
+#define MSG_CMD 0x40
+// length must be greater than zero
+// data will contain an ascii command
+// server may reject excessively large commands
+
+#define MSG_SEND_DATA 0x41
+// client sends data to server
+// length is datalen -1 (to allow for full 64k chunks)
+
+#define MSG_END_DATA 0x42
+// client ends data stream
+// server will then respond with MSG_OKAY or MSG_FAIL
+
+// command strings are in the form of
+// <command> ':' <decimal-datalen> ':' <optional-arguments>
+
+// example:
+// C: MSG_CMD "flash:32768:bootloader"
+// S: MSG_GO_AHEAD
+// C: MSG_SEND_DATA 16384 ...
+// C: MSG_SEND_DATA 16384 ...
+// C: MSG_END_DATA
+// S: MSG_LOG "erasing sectors"
+// S: MSG_LOG "writing sectors"
+// S: MSG_OKAY
+//
+// C: MSG_CMD "eraese:0:bootloader"
+// S: MSG_FAIL "unknown command 'eraese'"
+//
+// C: MSG_CMD "reboot:0:"
+// S: MSG_OKAY
diff --git a/src/bsp/lk/app/lkboot/pdcc.h b/src/bsp/lk/app/lkboot/pdcc.h
new file mode 100644
index 0000000..068b878
--- /dev/null
+++ b/src/bsp/lk/app/lkboot/pdcc.h
@@ -0,0 +1,53 @@
+/*
+ * Copyright (c) 2015 Travis Geiselbrecht
+ *
+ * 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.
+ */
+#pragma once
+
+#include <stdint.h>
+
+/* in memory and DCC descriptors for the PDCC protocol */
+
+/* shared outside of lk repository, be careful of modifications */
+#define PDCC_VERSION 1
+
+struct pdcc_buffer_descriptor {
+ uint32_t version;
+
+ uint32_t htod_buffer_phys;
+ uint32_t htod_buffer_len;
+
+ uint32_t dtoh_buffer_phys;
+ uint32_t dtoh_buffer_len;
+};
+
+#define PDCC_VALID (1<<31)
+#define PDCC_OPCODE_SHIFT (24)
+#define PDCC_OPCODE(x) (((x) >> PDCC_OPCODE_SHIFT) & 0x7f)
+#define PDCC_DATA(x) ((x) & 0x00ffffff);
+
+enum {
+ PDCC_OP_RESET = 0,
+ PDCC_OP_BUF_HEADER,
+ PDCC_OP_UPDATE_OUT_INDEX,
+ PDCC_OP_CONSUMED_IN,
+};
+
diff --git a/src/bsp/lk/app/lkboot/rules.mk b/src/bsp/lk/app/lkboot/rules.mk
new file mode 100644
index 0000000..1ee1376
--- /dev/null
+++ b/src/bsp/lk/app/lkboot/rules.mk
@@ -0,0 +1,19 @@
+LOCAL_DIR := $(GET_LOCAL_DIR)
+
+MODULE := $(LOCAL_DIR)
+
+MODULE_DEPS += \
+ lib/bio \
+ lib/bootargs \
+ lib/bootimage \
+ lib/cbuf \
+ lib/ptable \
+ lib/sysparam
+
+MODULE_SRCS += \
+ $(LOCAL_DIR)/commands.c \
+ $(LOCAL_DIR)/dcc.c \
+ $(LOCAL_DIR)/inet.c \
+ $(LOCAL_DIR)/lkboot.c \
+
+include make/module.mk
diff --git a/src/bsp/lk/app/loader/loader.c b/src/bsp/lk/app/loader/loader.c
new file mode 100644
index 0000000..c363ee1
--- /dev/null
+++ b/src/bsp/lk/app/loader/loader.c
@@ -0,0 +1,202 @@
+/*
+ * Copyright (c) 2015 Carlos Pizano-Uribe <cpu@chromium.org>
+ *
+ * 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 <stdio.h>
+#include <string.h>
+#include <stdint.h>
+#include <stdlib.h>
+
+#include <lib/tftp.h>
+#include <lib/cksum.h>
+#include <lib/elf.h>
+
+#include <kernel/thread.h>
+
+#if defined(WITH_LIB_CONSOLE)
+#include <lib/console.h>
+#else
+#error "loader app needs a console"
+#endif
+
+#if defined(SDRAM_BASE)
+#define DOWNLOAD_BASE ((void*)SDRAM_BASE)
+#else
+#define DOWNLOAD_BASE ((void*)0)
+#endif
+
+#define FNAME_SIZE 64
+#define DOWNLOAD_SLOT_SIZE (512 * 1024)
+
+typedef enum {
+ DOWNLOAD_ANY,
+ DOWNLOAD_ELF,
+} download_type;
+
+typedef struct {
+ unsigned char* start;
+ unsigned char* end;
+ unsigned char* max;
+ char name[FNAME_SIZE];
+ download_type type;
+} download_t;
+
+static download_t* make_download(const char* name)
+{
+ download_t* d = malloc(sizeof(download_t));
+ memset(d, 0, sizeof(download_t));
+ strncpy(d->name, name, FNAME_SIZE);
+ return d;
+}
+
+static void set_ram_zone(download_t* d, int slot) {
+ d->start = DOWNLOAD_BASE + (DOWNLOAD_SLOT_SIZE * slot);
+ d->end = d->start;
+ d->max = d->end + DOWNLOAD_SLOT_SIZE;
+ memset(d->start, 0, DOWNLOAD_SLOT_SIZE);
+}
+
+static size_t output_result(const download_t* download)
+{
+ size_t len = download->end - download->start;
+ unsigned long crc = crc32(0, download->start, len);
+ printf("[%s] done, start at: %p - %zu bytes, crc32 = %lu\n",
+ download->name, download->start, len, crc);
+ return len;
+}
+
+static int run_elf(void* entry_point)
+{
+ void (*elf_start)(void) = (void*)entry_point;
+ printf("elf (%p) running ...\n", entry_point);
+ thread_sleep(10);
+ elf_start();
+ printf("elf (%p) finished\n", entry_point);
+ return 0;
+}
+
+static void process_elf_blob(const void* start, size_t len)
+{
+ void* entrypt;
+ elf_handle_t elf;
+
+ status_t st = elf_open_handle_memory(&elf, start, len);
+ if (st < 0) {
+ printf("unable to open elf handle\n");
+ return;
+ }
+
+ st = elf_load(&elf);
+ if (st < 0) {
+ printf("elf processing failed, status : %d\n", st);
+ goto exit;
+ }
+
+ entrypt = (void*)elf.entry;
+ if (entrypt < start || entrypt >= (void*)((char*)start + len)) {
+ printf("out of bounds entrypoint for elf : %p\n", entrypt);
+ goto exit;
+ }
+
+ printf("elf looks good\n");
+ thread_resume(thread_create("elf_runner", &run_elf, entrypt,
+ DEFAULT_PRIORITY, DEFAULT_STACK_SIZE));
+exit:
+ elf_close_handle(&elf);
+}
+
+int tftp_callback(void* data, size_t len, void* arg)
+{
+ download_t* download = arg;
+ size_t final_len;
+
+ if (!data) {
+ final_len = output_result(download);
+ if (download->type == DOWNLOAD_ELF) {
+ process_elf_blob(download->start, final_len);
+ }
+
+ download->end = download->start;
+ return 0;
+ }
+
+ if ((download->end + len) > download->max) {
+ printf("transfer too big, aborting\n");
+ return -1;
+ }
+ if (len) {
+ memcpy(download->end, data, len);
+ download->end += len;
+ }
+ return 0;
+}
+
+static int loader(int argc, const cmd_args *argv)
+{
+ static int any_slot = 0;
+ static int elf_slot = 1;
+
+ download_t* download;
+ int slot;
+
+ if (!DOWNLOAD_BASE) {
+ printf("loader not available. it needs sdram\n");
+ return 0;
+ }
+
+ if (argc < 3) {
+usage:
+ printf("load any [filename] <slot>\n"
+ "load elf [filename] <slot>\n"
+ "protocol is tftp and <slot> is optional\n");
+ return 0;
+ }
+
+ download = make_download(argv[2].str);
+
+ if (strcmp(argv[1].str, "any") == 0) {
+ download->type = DOWNLOAD_ANY;
+ slot = any_slot;
+ any_slot += 2;
+ } else if (strcmp(argv[1].str, "elf") == 0) {
+ download->type = DOWNLOAD_ELF;
+ slot = elf_slot;
+ elf_slot += 2;
+ } else {
+ goto usage;
+ }
+
+ if (argc == 4) {
+ slot = argv[3].i;
+ }
+
+ set_ram_zone(download, slot);
+ tftp_set_write_client(download->name, &tftp_callback, download);
+ printf("ready for %s over tftp (at %p)\n", argv[2].str, download->start);
+ return 0;
+}
+
+STATIC_COMMAND_START
+STATIC_COMMAND("load", "download and run via tftp", &loader)
+STATIC_COMMAND_END(loader);
+
diff --git a/src/bsp/lk/app/loader/rules.mk b/src/bsp/lk/app/loader/rules.mk
new file mode 100644
index 0000000..e1a1f11
--- /dev/null
+++ b/src/bsp/lk/app/loader/rules.mk
@@ -0,0 +1,14 @@
+LOCAL_DIR := $(GET_LOCAL_DIR)
+
+MODULE := $(LOCAL_DIR)
+
+MODULE_SRCS += \
+ $(LOCAL_DIR)/loader.c \
+
+
+MODULE_DEPS := \
+ lib/cksum \
+ lib/tftp \
+ lib/elf
+
+include make/module.mk
diff --git a/src/bsp/lk/app/lpcboot/lpc43xx-spifi.c b/src/bsp/lk/app/lpcboot/lpc43xx-spifi.c
new file mode 100644
index 0000000..73968ed
--- /dev/null
+++ b/src/bsp/lk/app/lpcboot/lpc43xx-spifi.c
@@ -0,0 +1,107 @@
+/*
+ * Copyright (c) 2015 Brian Swetland
+ *
+ * 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 <debug.h>
+#include <string.h>
+#include <stdlib.h>
+#include <printf.h>
+#include <reg.h>
+
+#include <platform/lpc43xx-spifi.h>
+
+#define CMD_PAGE_PROGRAM 0x02
+#define CMD_READ_DATA 0x03
+#define CMD_READ_STATUS 0x05
+#define CMD_WRITE_ENABLE 0x06
+#define CMD_SECTOR_ERASE 0x20
+
+static void spifi_write_enable(void) {
+ writel(CMD_FF_SERIAL | CMD_FR_OP | CMD_OPCODE(CMD_WRITE_ENABLE),
+ SPIFI_CMD);
+ while (readl(SPIFI_STAT) & STAT_CMD) ;
+}
+
+static void spifi_wait_busy(void) {
+ while (readl(SPIFI_STAT) & STAT_CMD) ;
+ writel(CMD_POLLBIT(0) | CMD_POLLCLR | CMD_POLL |
+ CMD_FF_SERIAL | CMD_FR_OP | CMD_OPCODE(CMD_READ_STATUS),
+ SPIFI_CMD);
+ while (readl(SPIFI_STAT) & STAT_CMD) ;
+ // discard matching status byte from fifo
+ readb(SPIFI_DATA);
+}
+
+void spifi_page_program(u32 addr, u32 *ptr, u32 count) {
+ spifi_write_enable();
+ writel(addr, SPIFI_ADDR);
+ writel(CMD_DATALEN(count * 4) | CMD_FF_SERIAL | CMD_FR_OP_3B |
+ CMD_DOUT | CMD_OPCODE(CMD_PAGE_PROGRAM), SPIFI_CMD);
+ while (count-- > 0) {
+ writel(*ptr++, SPIFI_DATA);
+ }
+ spifi_wait_busy();
+}
+
+void spifi_sector_erase(u32 addr) {
+ spifi_write_enable();
+ writel(addr, SPIFI_ADDR);
+ writel(CMD_FF_SERIAL | CMD_FR_OP_3B | CMD_OPCODE(CMD_SECTOR_ERASE),
+ SPIFI_CMD);
+ spifi_wait_busy();
+}
+
+int spifi_verify_erased(u32 addr, u32 count) {
+ int err = 0;
+ writel(addr, SPIFI_ADDR);
+ writel(CMD_DATALEN(count * 4) | CMD_FF_SERIAL | CMD_FR_OP_3B |
+ CMD_OPCODE(CMD_READ_DATA), SPIFI_CMD);
+ while (count-- > 0) {
+ if (readl(SPIFI_DATA) != 0xFFFFFFFF) err = -1;
+ }
+ while (readl(SPIFI_STAT) & STAT_CMD) ;
+ return err;
+}
+
+int spifi_verify_page(u32 addr, u32 *ptr) {
+ int count = 256 / 4;
+ int err = 0;
+ writel(addr, SPIFI_ADDR);
+ writel(CMD_DATALEN(count * 4) | CMD_FF_SERIAL | CMD_FR_OP_3B |
+ CMD_OPCODE(CMD_READ_DATA), SPIFI_CMD);
+ while (count-- > 0) {
+ if (readl(SPIFI_DATA) != *ptr++) err = -1;
+ }
+ while (readl(SPIFI_STAT) & STAT_CMD) ;
+ return err;
+}
+
+// at reset-stop, all clocks are running from 12MHz internal osc
+// todo: run SPIFI_CLK at a much higher rate
+// todo: use 4bit modes
+void spifi_init(void) {
+ // reset spifi controller
+ writel(STAT_RESET, SPIFI_STAT);
+ while (readl(SPIFI_STAT) & STAT_RESET) ;
+ writel(0xFFFFF, SPIFI_CTRL);
+}
+
diff --git a/src/bsp/lk/app/lpcboot/lpcboot.c b/src/bsp/lk/app/lpcboot/lpcboot.c
new file mode 100644
index 0000000..fcd0593
--- /dev/null
+++ b/src/bsp/lk/app/lpcboot/lpcboot.c
@@ -0,0 +1,325 @@
+/*
+ * Copyright (c) 2015 Brian Swetland
+ *
+ * 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 <app.h>
+#include <err.h>
+#include <debug.h>
+#include <string.h>
+#include <stdlib.h>
+#include <printf.h>
+#include <dev/udc.h>
+
+#include <platform.h>
+#include <arch/arm.h>
+#include <kernel/thread.h>
+#include <kernel/event.h>
+#include <kernel/timer.h>
+
+#include <platform/lpc43xx-gpio.h>
+
+#define PIN_LED PIN(1,1)
+#define GPIO_LED GPIO(0,8)
+
+void spifi_init(void);
+void spifi_page_program(u32 addr, u32 *ptr, u32 count);
+void spifi_sector_erase(u32 addr);
+int spifi_verify_erased(u32 addr, u32 count);
+int spifi_verify_page(u32 addr, u32 *ptr);
+
+static event_t txevt = EVENT_INITIAL_VALUE(txevt, 0, 0);
+static event_t rxevt = EVENT_INITIAL_VALUE(rxevt, 0, 0);
+
+static udc_request_t *txreq;
+static udc_request_t *rxreq;
+static udc_endpoint_t *txept;
+static udc_endpoint_t *rxept;
+
+static volatile int online;
+static volatile int txstatus;
+static volatile int rxstatus;
+static volatile unsigned rxactual;
+
+static void lpcboot_notify(udc_gadget_t *gadget, unsigned event) {
+ if (event == UDC_EVENT_ONLINE) {
+ online = 1;
+ } else {
+ online = 0;
+ }
+}
+
+static void rx_complete(udc_request_t *req, unsigned actual, int status) {
+ rxactual = actual;
+ rxstatus = status;
+ event_signal(&rxevt, 0);
+}
+
+static void tx_complete(udc_request_t *req, unsigned actual, int status) {
+ txstatus = status;
+ event_signal(&txevt, 0);
+}
+
+void usb_xmit(void *data, unsigned len) {
+ event_unsignal(&txevt);
+ txreq->buffer = data;
+ txreq->length = len;
+ txstatus = 1;
+ udc_request_queue(txept, txreq);
+ event_wait(&txevt);
+}
+
+int usb_recv(void *data, unsigned len, lk_time_t timeout) {
+ event_unsignal(&rxevt);
+ rxreq->buffer = data;
+ rxreq->length = len;
+ rxstatus = 1;
+ udc_request_queue(rxept, rxreq);
+ if (event_wait_timeout(&rxevt, timeout)) {
+ return ERR_TIMED_OUT;
+ }
+ return rxactual;
+}
+
+static udc_device_t lpcboot_device = {
+ .vendor_id = 0x1209,
+ .product_id = 0x5039,
+ .version_id = 0x0100,
+};
+
+static udc_endpoint_t *lpcboot_endpoints[2];
+
+static udc_gadget_t lpcboot_gadget = {
+ .notify = lpcboot_notify,
+ .ifc_class = 0xFF,
+ .ifc_subclass = 0xFF,
+ .ifc_protocol = 0xFF,
+ .ifc_endpoints = 2,
+ .ept = lpcboot_endpoints,
+};
+
+static void lpcboot_init(const struct app_descriptor *app)
+{
+ udc_init(&lpcboot_device);
+ lpcboot_endpoints[0] = txept = udc_endpoint_alloc(UDC_BULK_IN, 512);
+ lpcboot_endpoints[1] = rxept = udc_endpoint_alloc(UDC_BULK_OUT, 512);
+ txreq = udc_request_alloc();
+ rxreq = udc_request_alloc();
+ rxreq->complete = rx_complete;
+ txreq->complete = tx_complete;
+ udc_register_gadget(&lpcboot_gadget);
+}
+
+#define RAM_BASE 0x10000000
+#define RAM_SIZE (128 * 1024)
+
+#define BOOT_BASE 0
+#define BOOT_SIZE (32 * 1024)
+
+#define ROM_BASE (32 * 1024)
+#define ROM_SIZE (128 * 1024)
+
+struct device_info {
+ u8 part[16];
+ u8 board[16];
+ u32 version;
+ u32 ram_base;
+ u32 ram_size;
+ u32 rom_base;
+ u32 rom_size;
+ u32 unused0;
+ u32 unused1;
+ u32 unused2;
+};
+
+struct device_info DEVICE = {
+ .part = "LPC43xx",
+ .board = TARGET,
+ .version = 0x0001000,
+ .ram_base = RAM_BASE,
+ .ram_size = RAM_SIZE,
+ .rom_base = ROM_BASE,
+ .rom_size = ROM_SIZE,
+};
+
+
+#define MAGIC1 0xAA113377
+#define MAGIC2 0xAA773311
+#define MAGIC1_ADDR 0x20003FF8
+#define MAGIC2_ADDR 0x20003FFC
+
+void boot_app(void) {
+ writel(MAGIC1, MAGIC1_ADDR);
+ writel(MAGIC2, MAGIC2_ADDR);
+}
+
+int erase_page(u32 addr) {
+ spifi_sector_erase(addr);
+ return spifi_verify_erased(addr, 0x1000/4);
+}
+
+int write_page(u32 addr, void *ptr) {
+ unsigned n;
+ u32 *x = ptr;
+ for (n = 0; n < 16; n++) {
+ spifi_page_program(addr, x, 256 / 4);
+ if (spifi_verify_page(addr, x)) return -1;
+ addr += 256;
+ x += (256 / 4);
+ }
+ return 0;
+}
+
+static uint32_t ram[4096/4];
+
+void handle(u32 magic, u32 cmd, u32 arg) {
+ u32 reply[2];
+ u32 addr, xfer;
+ int err = 0;
+
+ if (magic != 0xDB00A5A5)
+ return;
+
+ reply[0] = magic;
+ reply[1] = -1;
+
+ switch (cmd) {
+ case 'E':
+ reply[1] = erase_page(ROM_BASE);
+ break;
+ case 'W':
+ case 'w':
+ if (cmd == 'W') {
+ if (arg > ROM_SIZE)
+ break;
+ addr = ROM_BASE;
+ } else {
+ if (arg > BOOT_SIZE)
+ break;
+ addr = BOOT_BASE;
+ }
+ reply[1] = 0;
+ usb_xmit(reply, 8);
+ while (arg > 0) {
+ xfer = (arg > 4096) ? 4096 : arg;
+ usb_recv(ram, xfer, INFINITE_TIME);
+ if (!err) err = erase_page(addr);
+ if (!err) err = write_page(addr, ram);
+ addr += 4096;
+ arg -= xfer;
+ }
+ printf("flash %s\n", err ? "ERROR" : "OK");
+ reply[1] = err;
+ break;
+#if WITH_BOOT_TO_RAM
+ case 'X':
+ if (arg > RAM_SIZE)
+ break;
+ reply[1] = 0;
+ usb_xmit(reply, 8);
+ usb_recv(ram, arg);
+ usb_xmit(reply, 8);
+
+ /* let last txn clear */
+ usb_recv_timeout(buf, 64, 10);
+
+ boot_image(ram);
+ break;
+#endif
+ case 'Q':
+ reply[1] = 0;
+ usb_xmit(reply, 8);
+ usb_xmit(&DEVICE, sizeof(DEVICE));
+ return;
+ case 'A':
+ boot_app();
+ case 'R':
+ /* reboot "normally" */
+ reply[1] = 0;
+ usb_xmit(reply, 8);
+ udc_stop();
+ platform_halt(HALT_ACTION_REBOOT, HALT_REASON_SW_RESET);
+ default:
+ break;
+ }
+ usb_xmit(reply, 8);
+}
+
+static short led_idx = 0;
+static short led_delay[] = { 500, 100, 100, 100, };
+static short led_state[] = { 1, 0, 1, 0, };
+static timer_t led_timer = TIMER_INITIAL_VALUE(led_timer);
+
+static enum handler_return led_timer_cb(timer_t *timer, lk_time_t now, void *arg) {
+ gpio_set(GPIO_LED, led_state[led_idx]);
+ timer_set_oneshot(timer, led_delay[led_idx], led_timer_cb, NULL);
+ led_idx++;
+ if (led_idx == (sizeof(led_state)/sizeof(led_state[0]))) {
+ led_idx = 0;
+ }
+ return 0;
+}
+
+static void lpcboot_entry(const struct app_descriptor *app, void *args)
+{
+ lk_time_t timeout;
+ int r;
+ u32 buf[64/4];
+
+#if 0
+ timeout = INFINITE_TIME;
+#else
+ if (readl(32768) != 0) {
+ timeout = 3000;
+ } else {
+ timeout = INFINITE_TIME;
+ }
+#endif
+
+ pin_config(PIN_LED, PIN_MODE(0) | PIN_PLAIN);
+ gpio_config(GPIO_LED, GPIO_OUTPUT);
+ led_timer_cb(&led_timer, 0, NULL);
+
+ udc_start();
+ spifi_init();
+ for (;;) {
+ if (!online) {
+ thread_yield();
+ continue;
+ }
+ r = usb_recv(buf, 64, timeout);
+ if (r == ERR_TIMED_OUT) {
+ boot_app();
+ platform_halt(HALT_ACTION_REBOOT, HALT_REASON_SW_RESET);
+ }
+ if (r == 12) {
+ handle(buf[0], buf[1], buf[2]);
+ timeout = INFINITE_TIME;
+ }
+ }
+}
+
+APP_START(usbtest)
+ .init = lpcboot_init,
+ .entry = lpcboot_entry,
+APP_END
+
+
diff --git a/src/bsp/lk/app/lpcboot/miniloader.S b/src/bsp/lk/app/lpcboot/miniloader.S
new file mode 100644
index 0000000..1bba9fc
--- /dev/null
+++ b/src/bsp/lk/app/lpcboot/miniloader.S
@@ -0,0 +1,58 @@
+
+.syntax unified
+
+miniloader_vectors:
+ .word 0x20003FF0
+ .word miniloader_reset + 1
+ .word miniloader_fault + 1
+ .word miniloader_fault + 1
+ .word miniloader_fault + 1
+ .word miniloader_fault + 1
+ .word miniloader_fault + 1
+ .word miniloader_fault + 1
+ .word miniloader_fault + 1
+ .word miniloader_fault + 1
+ .word miniloader_fault + 1
+ .word miniloader_fault + 1
+ .word miniloader_fault + 1
+ .word miniloader_fault + 1
+ .word miniloader_fault + 1
+ .word miniloader_fault + 1
+
+// miniloader_boot(unsigned src, unsigned count)
+miniloader_boot:
+ mov r2, #0x10000000
+miniloader_boot_loop:
+ ldr r3, [r0], #4
+ str r3, [r2], #4
+ subs r1, #1
+ bne miniloader_boot_loop
+ mov r0, #0x10000000
+ ldr sp, [r0]
+ ldr r0, [r0, #4]
+ bx r0
+
+miniloader_reset:
+ ldr r0, =0x20003FF8
+ ldr r1, =0xAA113377
+ ldr r2, =0xAA773311
+ ldr r3, [r0]
+ ldr r4, [r0, #4]
+ mov r5, #0
+ str r5, [r0]
+ str r5, [r0, #4]
+ cmp r1, r3
+ bne start_bootloader
+ cmp r2, r4
+ bne start_bootloader
+start_app:
+ ldr r0, =0x8000
+ ldr r1, =(131072/4)
+ b miniloader_boot
+start_bootloader:
+ ldr r0, =0x00001000
+ ldr r1, =(32768/4)
+ b miniloader_boot
+
+miniloader_fault:
+ b .
diff --git a/src/bsp/lk/app/lpcboot/rules.mk b/src/bsp/lk/app/lpcboot/rules.mk
new file mode 100644
index 0000000..0e1c692
--- /dev/null
+++ b/src/bsp/lk/app/lpcboot/rules.mk
@@ -0,0 +1,10 @@
+LOCAL_DIR := $(GET_LOCAL_DIR)
+
+MODULE := $(LOCAL_DIR)
+
+MODULE_SRCS += \
+ $(LOCAL_DIR)/lpcboot.c \
+ $(LOCAL_DIR)/lpc43xx-spifi.c
+
+include make/module.mk
+
diff --git a/src/bsp/lk/app/mdebug/fw-m0sub.S b/src/bsp/lk/app/mdebug/fw-m0sub.S
new file mode 100644
index 0000000..b7185b5
--- /dev/null
+++ b/src/bsp/lk/app/mdebug/fw-m0sub.S
@@ -0,0 +1,519 @@
+/* fw-m0sub.S
+ *
+ * Copyright 2015 Brian Swetland <swetland@frotz.net>
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+.syntax unified
+
+m0_vectors:
+ .word 0x18003FF0
+ .word m0_reset + 1
+ .word m0_fault + 1
+ .word m0_fault + 1
+ .word m0_fault + 1
+ .word m0_fault + 1
+ .word m0_fault + 1
+ .word m0_fault + 1
+ .word m0_fault + 1
+ .word m0_fault + 1
+ .word m0_fault + 1
+ .word m0_fault + 1
+ .word m0_fault + 1
+ .word m0_fault + 1
+ .word m0_fault + 1
+ .word m0_fault + 1
+// external IRQs
+ .word m0_fault + 1
+ .word m0_irq + 1
+
+m0_fault:
+ ldr r0, =0x18000000
+ ldr r1, =0xeeee0000
+ mrs r2, xpsr
+ movs r3, #0xFF
+ ands r2, r2, r3
+ orrs r1, r1, r2
+ str r1, [r0]
+ b .
+
+.ltorg
+
+#define REPORT_DELAY 0
+
+#define COMM_BASE 0x18004000
+
+#define COMM_CMD 0
+#define COMM_ARG0 4
+#define COMM_ARG1 8
+#define COMM_RESP 12
+#define COMM_RETRY 16
+
+
+#define M4_TXEV 0x40043130 // write 0 to clear
+
+#define SGPIO_BASE (0x40101210)
+#define OFF_IN 0
+#define OFF_OUT 4
+#define OFF_OEN 8
+#define SGPIO_IN (0x40101210)
+#define SGPIO_OUT (0x40101214)
+#define SGPIO_OEN (0x40101218)
+
+#define CLK_BIT 11
+#define DIO_BIT 14
+#define TEN_BIT 15
+#define CLK_MSK (1 << CLK_BIT)
+#define DIO_MSK (1 << DIO_BIT)
+#define TEN_MSK (1 << TEN_BIT)
+
+#define CLK1_OUT (CLK_MSK | TEN_MSK)
+#define CLK0_OUT (TEN_MSK)
+#define CLK1_IN (CLK_MSK)
+#define CLK0_IN (0)
+
+#define OEN_IN ((1 << CLK_BIT) | (1 << TEN_BIT))
+#define OEN_OUT ((1 << CLK_BIT) | (1 << DIO_BIT) | (1 << TEN_BIT))
+
+#define NOP4 nop ; nop ; nop ; nop
+#define NOP8 NOP4 ; NOP4
+#define NOP16 NOP8 ; NOP8
+
+//#define DELAY nop ; nop
+//#define DELAY NOP8
+
+// r11 CLK1_OUT const
+// r10 CLK0_OUT const
+// r9 delay subroutine
+// r8 comm_base addr
+// r7 SGPIO_BASE addr
+// r6 DIO_MSK const
+// r5 CLK1_IN const
+// r4 CLK0_IN const
+// r3 outbits data
+
+snooze_2m:
+ nop ; nop ; nop ; nop
+ nop ; nop ; nop ; nop
+ nop ; nop ; nop ; nop
+ nop ; nop ; nop ; nop
+snooze_3m:
+ nop ; nop ; nop ; nop
+ nop ; nop ; nop ; nop
+snooze_4m:
+ nop ; nop ; nop ; nop
+ nop ; nop ; nop ; nop
+snooze_6m:
+ nop ; nop ; nop ; nop
+snooze_8m:
+ bx lr
+
+// delay 0 nops 16MHz
+// delay 2 nops 12MHz
+// delay 4 nops 9.6MHz
+#define DELAY blx r9
+
+// 12 cycles + DELAY x 2
+.macro ONE_BIT_OUT
+ lsls r2, r3, #DIO_BIT // shift bit 1 to posn
+ ands r2, r2, r6 // isolate bit 1
+ movs r1, r2 // save bit 1
+ add r2, r2, r10 // combine with CLK1
+ DELAY
+ str r2, [r7, #OFF_OUT] // commit negative egde
+ lsrs r3, r3, #1 // advance to next bit
+ add r1, r1, r11 // combine with CLK1
+ nop
+ nop
+ DELAY
+ str r1, [r7, #OFF_OUT] // commit positive edge
+.endm
+
+.macro ONE_BIT_IN
+ ands r0, r0, r6 // isolate input bit
+ lsls r0, r0, #(31-DIO_BIT) // move to posn 31
+ lsrs r3, r3, #1 // make room
+ orrs r3, r3, r0 // add bit
+ DELAY
+ str r4, [r7, #OFF_OUT] // commit negative edge
+ ldr r0, [r7, #OFF_IN] // sample input
+ nop
+ nop
+ DELAY
+ str r5, [r7, #OFF_OUT] // commit positive edge
+.endm
+
+// used for the final parity and turn bits on input so this
+// actually only reads one bit
+read_2:
+ push {lr}
+ nop
+ nop
+ nop
+ nop
+ DELAY
+ str r4, [r7, #OFF_OUT]
+ ldr r0, [r7, #OFF_IN]
+ nop
+ nop
+ DELAY
+ str r5, [r7, #OFF_OUT]
+ ands r0, r0, r6 // isolate bit
+ lsrs r0, r0, #DIO_BIT // shift to bit0
+ nop
+ nop
+ DELAY
+ str r4, [r7, #OFF_OUT]
+ nop
+ nop
+ nop
+ nop
+ DELAY
+ str r5, [r7, #OFF_OUT]
+ pop {pc}
+
+// w0: <15> <parity:1> <cmd:16>
+// w1: <data:32>
+
+
+write_16:
+ push {lr}
+ b _write_16
+write_32:
+ push {lr}
+ ONE_BIT_OUT
+ ONE_BIT_OUT
+ ONE_BIT_OUT
+ ONE_BIT_OUT
+ ONE_BIT_OUT
+ ONE_BIT_OUT
+ ONE_BIT_OUT
+ ONE_BIT_OUT
+ ONE_BIT_OUT
+ ONE_BIT_OUT
+ ONE_BIT_OUT
+ ONE_BIT_OUT
+ ONE_BIT_OUT
+ ONE_BIT_OUT
+ ONE_BIT_OUT
+ ONE_BIT_OUT
+_write_16:
+ ONE_BIT_OUT
+ ONE_BIT_OUT
+ ONE_BIT_OUT
+ ONE_BIT_OUT
+ ONE_BIT_OUT
+ ONE_BIT_OUT
+ ONE_BIT_OUT
+ ONE_BIT_OUT
+ ONE_BIT_OUT
+ ONE_BIT_OUT
+ ONE_BIT_OUT
+ ONE_BIT_OUT
+ ONE_BIT_OUT
+ ONE_BIT_OUT
+ ONE_BIT_OUT
+ ONE_BIT_OUT
+ pop {pc}
+write_1:
+ push {lr}
+ ONE_BIT_OUT
+ pop {pc}
+
+read_4:
+ push {lr}
+ b _read_4
+read_32:
+ push {lr}
+ ONE_BIT_IN
+ ONE_BIT_IN
+ ONE_BIT_IN
+ ONE_BIT_IN
+ ONE_BIT_IN
+ ONE_BIT_IN
+ ONE_BIT_IN
+ ONE_BIT_IN
+ ONE_BIT_IN
+ ONE_BIT_IN
+ ONE_BIT_IN
+ ONE_BIT_IN
+ ONE_BIT_IN
+ ONE_BIT_IN
+ ONE_BIT_IN
+ ONE_BIT_IN
+ ONE_BIT_IN
+ ONE_BIT_IN
+ ONE_BIT_IN
+ ONE_BIT_IN
+ ONE_BIT_IN
+ ONE_BIT_IN
+ ONE_BIT_IN
+ ONE_BIT_IN
+ ONE_BIT_IN
+ ONE_BIT_IN
+ ONE_BIT_IN
+ ONE_BIT_IN
+_read_4:
+ ONE_BIT_IN
+ ONE_BIT_IN
+ ONE_BIT_IN
+ ONE_BIT_IN
+ ands r0, r0, r6 // isolate input bit
+ lsls r0, r0, #(31-DIO_BIT) // move to posn 31
+ lsrs r3, r3, #1 // make room
+ orrs r3, r3, r0 // add bit
+ pop {pc}
+
+init:
+ ldr r0, =CLK1_OUT
+ mov r11, r0
+ ldr r0, =CLK0_OUT
+ mov r10, r0
+ ldr r0, =(snooze_4m + 1)
+ mov r9, r0
+ ldr r0, =COMM_BASE
+ mov r8, r0
+ ldr r7, =SGPIO_BASE
+ ldr r6, =DIO_MSK
+ ldr r5, =CLK1_IN
+ ldr r4, =CLK0_IN
+ bx lr
+
+#define MAX_RETRY 8192
+
+err_fail:
+ movs r0, #3
+ mov r3, r8
+ str r0, [r3, #COMM_RESP];
+ pop {pc}
+
+err_timeout:
+ movs r0, #2
+ mov r3, r8
+ str r0, [r3, #COMM_RESP];
+ pop {pc}
+
+cmd_read_txn:
+ push {lr}
+
+ ldr r0, =MAX_RETRY
+ //movs r0, #MAX_RETRY
+ mov r12, r0
+
+rd_retry:
+ ldr r3, [r3, #COMM_ARG0]
+ bl write_16
+
+ ldr r3, =OEN_IN
+ str r3, [r7, #OFF_OEN]
+ bl read_4
+
+ lsrs r3, r3, #29
+ cmp r3, #1 // OK
+ beq rd_okay
+
+ ldr r1, =OEN_OUT
+ str r1, [r7, #OFF_OEN]
+
+ cmp r3, #2 // WAIT
+ bne err_fail
+
+ mov r0, r12
+ subs r0, r0, #1
+ mov r12, r0
+ beq err_timeout
+ mov r3, r8
+ b rd_retry
+
+rd_okay:
+ bl read_32
+ bl read_2
+ ldr r1, =OEN_OUT
+ str r1, [r7, #OFF_OEN]
+ mov r1, r11
+ orrs r1, r1, r6
+ str r1, [r7, #OFF_OUT]
+
+ mov r1, r8 // get COMM_BASE
+ str r3, [r1, #COMM_ARG0]
+ str r0, [r1, #COMM_ARG1]
+ movs r0, #0
+ str r0, [r1, #COMM_RESP]
+#if REPORT_DELAY
+ mov r0, r12
+ str r0, [r1, #COMM_RETRY]
+#endif
+ pop {pc}
+
+
+cmd_write_txn:
+ push {lr}
+
+ ldr r0, =MAX_RETRY
+ mov r12, r0
+
+wr_retry:
+ ldr r3, [r3, #COMM_ARG0]
+ bl write_16
+ push {r3} // stash parity bit
+
+ ldr r3, =OEN_IN
+ str r3, [r7, #OFF_OEN]
+ bl read_4
+
+ lsrs r3, r3, #29
+ cmp r3, #1 // OK
+ beq wr_okay
+
+ pop {r0} // discard saved parity bit
+
+ ldr r1, =OEN_OUT
+ str r1, [r7, #OFF_OEN]
+
+ cmp r3, #2 // WAIT
+ bne err_fail
+
+ mov r0, r12
+ subs r0, r0, #1
+ mov r12, r0
+ beq err_timeout
+
+ mov r3, r8
+ b wr_retry
+
+wr_okay:
+ ldr r3, =OEN_OUT
+ str r3, [r7, #OFF_OEN]
+ bl write_1
+
+ mov r3, r8
+ ldr r3, [r3, #COMM_ARG1]
+ bl write_32
+
+ pop {r3} // recover parity bit
+ bl write_1
+
+ mov r3, r8 // get COMM_BASE
+ movs r0, #0
+ str r0, [r3, #COMM_RESP]
+#if REPORT_DELAY
+ mov r0, r12
+ str r0, [r3, #COMM_RETRY]
+#endif
+ pop {pc}
+
+cmd_reset:
+ push {lr}
+ ldr r3, =0xffffffff
+ mov r12, r3
+ bl write_32
+ mov r3, r12
+ bl write_32
+
+ ldr r3, =0b1110011110011110
+ bl write_16
+
+ mov r3, r12
+ bl write_32
+ mov r3, r12
+ bl write_32
+
+ mov r3, r8
+ movs r0, #0
+ str r0, [r3, #COMM_RESP]
+ pop {pc}
+
+
+m0_irq:
+ push {lr}
+
+ // clear event from m4
+ ldr r0, =M4_TXEV
+ movs r1, #0
+ str r1, [r0]
+
+ mov r3, r8 // get COMM_BASE
+ ldr r0, [r3, #COMM_CMD]
+ cmp r0, #5
+ bls good_cmd
+ movs r0, #0
+good_cmd:
+ lsls r0, r0, #2
+ adr r1, cmd_table
+ ldr r2, [r1, r0]
+ blx r2
+
+ pop {pc}
+
+.align 2
+cmd_table:
+ .word cmd_invalid + 1
+ .word cmd_nop + 1
+ .word cmd_read_txn + 1
+ .word cmd_write_txn + 1
+ .word cmd_reset + 1
+ .word cmd_setclock + 1
+
+cmd_invalid:
+ movs r0, #9
+ str r0, [r3, #COMM_RESP]
+ bx lr
+
+cmd_nop:
+ movs r0, #0
+ str r0, [r3, #COMM_RESP]
+ bx lr
+
+cmd_setclock:
+ ldr r0, [r3, #COMM_ARG0]
+ cmp r0, #8
+ bls good_clock
+ movs r0, #0
+good_clock:
+ lsls r0, r0, #2
+ adr r1, snooze_table
+ ldr r1, [r1, r0]
+ mov r9, r1
+
+ movs r0, #0
+ str r0, [r3, #COMM_RESP]
+ bx lr
+
+.align 2
+snooze_table:
+ .word snooze_2m + 1
+ .word snooze_2m + 1
+ .word snooze_2m + 1
+ .word snooze_3m + 1
+ .word snooze_4m + 1
+ .word snooze_4m + 1
+ .word snooze_6m + 1
+ .word snooze_6m + 1
+ .word snooze_8m + 1
+
+m0_reset:
+ ldr r0, =0x18000000
+ ldr r1, =0xaaaa0000
+ str r1, [r0]
+
+ bl init
+
+ // enable IRQ1 (Event From M4)
+ ldr r0, =0xE000E100
+ movs r1, #2
+ str r1, [r0]
+
+m0_idle:
+ wfi
+ b m0_idle
diff --git a/src/bsp/lk/app/mdebug/fw-m0sub.h b/src/bsp/lk/app/mdebug/fw-m0sub.h
new file mode 100644
index 0000000..173b358
--- /dev/null
+++ b/src/bsp/lk/app/mdebug/fw-m0sub.h
@@ -0,0 +1,186 @@
+unsigned char zero_bin[] = {
+ 0xf0, 0x3f, 0x00, 0x18, 0x41, 0x08, 0x00, 0x18, 0x49, 0x00, 0x00, 0x18,
+ 0x49, 0x00, 0x00, 0x18, 0x49, 0x00, 0x00, 0x18, 0x49, 0x00, 0x00, 0x18,
+ 0x49, 0x00, 0x00, 0x18, 0x49, 0x00, 0x00, 0x18, 0x49, 0x00, 0x00, 0x18,
+ 0x49, 0x00, 0x00, 0x18, 0x49, 0x00, 0x00, 0x18, 0x49, 0x00, 0x00, 0x18,
+ 0x49, 0x00, 0x00, 0x18, 0x49, 0x00, 0x00, 0x18, 0x49, 0x00, 0x00, 0x18,
+ 0x49, 0x00, 0x00, 0x18, 0x49, 0x00, 0x00, 0x18, 0xc5, 0x07, 0x00, 0x18,
+ 0x04, 0x48, 0x05, 0x49, 0xef, 0xf3, 0x03, 0x82, 0xff, 0x23, 0x1a, 0x40,
+ 0x11, 0x43, 0x01, 0x60, 0xfe, 0xe7, 0x00, 0x00, 0x00, 0x00, 0x00, 0x18,
+ 0x00, 0x00, 0xee, 0xee, 0xc0, 0x46, 0xc0, 0x46, 0xc0, 0x46, 0xc0, 0x46,
+ 0xc0, 0x46, 0xc0, 0x46, 0xc0, 0x46, 0xc0, 0x46, 0xc0, 0x46, 0xc0, 0x46,
+ 0xc0, 0x46, 0xc0, 0x46, 0xc0, 0x46, 0xc0, 0x46, 0xc0, 0x46, 0xc0, 0x46,
+ 0xc0, 0x46, 0xc0, 0x46, 0xc0, 0x46, 0xc0, 0x46, 0xc0, 0x46, 0xc0, 0x46,
+ 0xc0, 0x46, 0xc0, 0x46, 0xc0, 0x46, 0xc0, 0x46, 0xc0, 0x46, 0xc0, 0x46,
+ 0xc0, 0x46, 0xc0, 0x46, 0xc0, 0x46, 0xc0, 0x46, 0xc0, 0x46, 0xc0, 0x46,
+ 0xc0, 0x46, 0xc0, 0x46, 0x70, 0x47, 0x00, 0xb5, 0xc0, 0x46, 0xc0, 0x46,
+ 0xc0, 0x46, 0xc0, 0x46, 0xc8, 0x47, 0x7c, 0x60, 0x38, 0x68, 0xc0, 0x46,
+ 0xc0, 0x46, 0xc8, 0x47, 0x7d, 0x60, 0x30, 0x40, 0x80, 0x0b, 0xc0, 0x46,
+ 0xc0, 0x46, 0xc8, 0x47, 0x7c, 0x60, 0xc0, 0x46, 0xc0, 0x46, 0xc0, 0x46,
+ 0xc0, 0x46, 0xc8, 0x47, 0x7d, 0x60, 0x00, 0xbd, 0x00, 0xb5, 0xc0, 0xe0,
+ 0x00, 0xb5, 0x9a, 0x03, 0x32, 0x40, 0x11, 0x00, 0x52, 0x44, 0xc8, 0x47,
+ 0x7a, 0x60, 0x5b, 0x08, 0x59, 0x44, 0xc0, 0x46, 0xc0, 0x46, 0xc8, 0x47,
+ 0x79, 0x60, 0x9a, 0x03, 0x32, 0x40, 0x11, 0x00, 0x52, 0x44, 0xc8, 0x47,
+ 0x7a, 0x60, 0x5b, 0x08, 0x59, 0x44, 0xc0, 0x46, 0xc0, 0x46, 0xc8, 0x47,
+ 0x79, 0x60, 0x9a, 0x03, 0x32, 0x40, 0x11, 0x00, 0x52, 0x44, 0xc8, 0x47,
+ 0x7a, 0x60, 0x5b, 0x08, 0x59, 0x44, 0xc0, 0x46, 0xc0, 0x46, 0xc8, 0x47,
+ 0x79, 0x60, 0x9a, 0x03, 0x32, 0x40, 0x11, 0x00, 0x52, 0x44, 0xc8, 0x47,
+ 0x7a, 0x60, 0x5b, 0x08, 0x59, 0x44, 0xc0, 0x46, 0xc0, 0x46, 0xc8, 0x47,
+ 0x79, 0x60, 0x9a, 0x03, 0x32, 0x40, 0x11, 0x00, 0x52, 0x44, 0xc8, 0x47,
+ 0x7a, 0x60, 0x5b, 0x08, 0x59, 0x44, 0xc0, 0x46, 0xc0, 0x46, 0xc8, 0x47,
+ 0x79, 0x60, 0x9a, 0x03, 0x32, 0x40, 0x11, 0x00, 0x52, 0x44, 0xc8, 0x47,
+ 0x7a, 0x60, 0x5b, 0x08, 0x59, 0x44, 0xc0, 0x46, 0xc0, 0x46, 0xc8, 0x47,
+ 0x79, 0x60, 0x9a, 0x03, 0x32, 0x40, 0x11, 0x00, 0x52, 0x44, 0xc8, 0x47,
+ 0x7a, 0x60, 0x5b, 0x08, 0x59, 0x44, 0xc0, 0x46, 0xc0, 0x46, 0xc8, 0x47,
+ 0x79, 0x60, 0x9a, 0x03, 0x32, 0x40, 0x11, 0x00, 0x52, 0x44, 0xc8, 0x47,
+ 0x7a, 0x60, 0x5b, 0x08, 0x59, 0x44, 0xc0, 0x46, 0xc0, 0x46, 0xc8, 0x47,
+ 0x79, 0x60, 0x9a, 0x03, 0x32, 0x40, 0x11, 0x00, 0x52, 0x44, 0xc8, 0x47,
+ 0x7a, 0x60, 0x5b, 0x08, 0x59, 0x44, 0xc0, 0x46, 0xc0, 0x46, 0xc8, 0x47,
+ 0x79, 0x60, 0x9a, 0x03, 0x32, 0x40, 0x11, 0x00, 0x52, 0x44, 0xc8, 0x47,
+ 0x7a, 0x60, 0x5b, 0x08, 0x59, 0x44, 0xc0, 0x46, 0xc0, 0x46, 0xc8, 0x47,
+ 0x79, 0x60, 0x9a, 0x03, 0x32, 0x40, 0x11, 0x00, 0x52, 0x44, 0xc8, 0x47,
+ 0x7a, 0x60, 0x5b, 0x08, 0x59, 0x44, 0xc0, 0x46, 0xc0, 0x46, 0xc8, 0x47,
+ 0x79, 0x60, 0x9a, 0x03, 0x32, 0x40, 0x11, 0x00, 0x52, 0x44, 0xc8, 0x47,
+ 0x7a, 0x60, 0x5b, 0x08, 0x59, 0x44, 0xc0, 0x46, 0xc0, 0x46, 0xc8, 0x47,
+ 0x79, 0x60, 0x9a, 0x03, 0x32, 0x40, 0x11, 0x00, 0x52, 0x44, 0xc8, 0x47,
+ 0x7a, 0x60, 0x5b, 0x08, 0x59, 0x44, 0xc0, 0x46, 0xc0, 0x46, 0xc8, 0x47,
+ 0x79, 0x60, 0x9a, 0x03, 0x32, 0x40, 0x11, 0x00, 0x52, 0x44, 0xc8, 0x47,
+ 0x7a, 0x60, 0x5b, 0x08, 0x59, 0x44, 0xc0, 0x46, 0xc0, 0x46, 0xc8, 0x47,
+ 0x79, 0x60, 0x9a, 0x03, 0x32, 0x40, 0x11, 0x00, 0x52, 0x44, 0xc8, 0x47,
+ 0x7a, 0x60, 0x5b, 0x08, 0x59, 0x44, 0xc0, 0x46, 0xc0, 0x46, 0xc8, 0x47,
+ 0x79, 0x60, 0x9a, 0x03, 0x32, 0x40, 0x11, 0x00, 0x52, 0x44, 0xc8, 0x47,
+ 0x7a, 0x60, 0x5b, 0x08, 0x59, 0x44, 0xc0, 0x46, 0xc0, 0x46, 0xc8, 0x47,
+ 0x79, 0x60, 0x9a, 0x03, 0x32, 0x40, 0x11, 0x00, 0x52, 0x44, 0xc8, 0x47,
+ 0x7a, 0x60, 0x5b, 0x08, 0x59, 0x44, 0xc0, 0x46, 0xc0, 0x46, 0xc8, 0x47,
+ 0x79, 0x60, 0x9a, 0x03, 0x32, 0x40, 0x11, 0x00, 0x52, 0x44, 0xc8, 0x47,
+ 0x7a, 0x60, 0x5b, 0x08, 0x59, 0x44, 0xc0, 0x46, 0xc0, 0x46, 0xc8, 0x47,
+ 0x79, 0x60, 0x9a, 0x03, 0x32, 0x40, 0x11, 0x00, 0x52, 0x44, 0xc8, 0x47,
+ 0x7a, 0x60, 0x5b, 0x08, 0x59, 0x44, 0xc0, 0x46, 0xc0, 0x46, 0xc8, 0x47,
+ 0x79, 0x60, 0x9a, 0x03, 0x32, 0x40, 0x11, 0x00, 0x52, 0x44, 0xc8, 0x47,
+ 0x7a, 0x60, 0x5b, 0x08, 0x59, 0x44, 0xc0, 0x46, 0xc0, 0x46, 0xc8, 0x47,
+ 0x79, 0x60, 0x9a, 0x03, 0x32, 0x40, 0x11, 0x00, 0x52, 0x44, 0xc8, 0x47,
+ 0x7a, 0x60, 0x5b, 0x08, 0x59, 0x44, 0xc0, 0x46, 0xc0, 0x46, 0xc8, 0x47,
+ 0x79, 0x60, 0x9a, 0x03, 0x32, 0x40, 0x11, 0x00, 0x52, 0x44, 0xc8, 0x47,
+ 0x7a, 0x60, 0x5b, 0x08, 0x59, 0x44, 0xc0, 0x46, 0xc0, 0x46, 0xc8, 0x47,
+ 0x79, 0x60, 0x9a, 0x03, 0x32, 0x40, 0x11, 0x00, 0x52, 0x44, 0xc8, 0x47,
+ 0x7a, 0x60, 0x5b, 0x08, 0x59, 0x44, 0xc0, 0x46, 0xc0, 0x46, 0xc8, 0x47,
+ 0x79, 0x60, 0x9a, 0x03, 0x32, 0x40, 0x11, 0x00, 0x52, 0x44, 0xc8, 0x47,
+ 0x7a, 0x60, 0x5b, 0x08, 0x59, 0x44, 0xc0, 0x46, 0xc0, 0x46, 0xc8, 0x47,
+ 0x79, 0x60, 0x9a, 0x03, 0x32, 0x40, 0x11, 0x00, 0x52, 0x44, 0xc8, 0x47,
+ 0x7a, 0x60, 0x5b, 0x08, 0x59, 0x44, 0xc0, 0x46, 0xc0, 0x46, 0xc8, 0x47,
+ 0x79, 0x60, 0x9a, 0x03, 0x32, 0x40, 0x11, 0x00, 0x52, 0x44, 0xc8, 0x47,
+ 0x7a, 0x60, 0x5b, 0x08, 0x59, 0x44, 0xc0, 0x46, 0xc0, 0x46, 0xc8, 0x47,
+ 0x79, 0x60, 0x9a, 0x03, 0x32, 0x40, 0x11, 0x00, 0x52, 0x44, 0xc8, 0x47,
+ 0x7a, 0x60, 0x5b, 0x08, 0x59, 0x44, 0xc0, 0x46, 0xc0, 0x46, 0xc8, 0x47,
+ 0x79, 0x60, 0x9a, 0x03, 0x32, 0x40, 0x11, 0x00, 0x52, 0x44, 0xc8, 0x47,
+ 0x7a, 0x60, 0x5b, 0x08, 0x59, 0x44, 0xc0, 0x46, 0xc0, 0x46, 0xc8, 0x47,
+ 0x79, 0x60, 0x9a, 0x03, 0x32, 0x40, 0x11, 0x00, 0x52, 0x44, 0xc8, 0x47,
+ 0x7a, 0x60, 0x5b, 0x08, 0x59, 0x44, 0xc0, 0x46, 0xc0, 0x46, 0xc8, 0x47,
+ 0x79, 0x60, 0x9a, 0x03, 0x32, 0x40, 0x11, 0x00, 0x52, 0x44, 0xc8, 0x47,
+ 0x7a, 0x60, 0x5b, 0x08, 0x59, 0x44, 0xc0, 0x46, 0xc0, 0x46, 0xc8, 0x47,
+ 0x79, 0x60, 0x9a, 0x03, 0x32, 0x40, 0x11, 0x00, 0x52, 0x44, 0xc8, 0x47,
+ 0x7a, 0x60, 0x5b, 0x08, 0x59, 0x44, 0xc0, 0x46, 0xc0, 0x46, 0xc8, 0x47,
+ 0x79, 0x60, 0x9a, 0x03, 0x32, 0x40, 0x11, 0x00, 0x52, 0x44, 0xc8, 0x47,
+ 0x7a, 0x60, 0x5b, 0x08, 0x59, 0x44, 0xc0, 0x46, 0xc0, 0x46, 0xc8, 0x47,
+ 0x79, 0x60, 0x00, 0xbd, 0x00, 0xb5, 0x9a, 0x03, 0x32, 0x40, 0x11, 0x00,
+ 0x52, 0x44, 0xc8, 0x47, 0x7a, 0x60, 0x5b, 0x08, 0x59, 0x44, 0xc0, 0x46,
+ 0xc0, 0x46, 0xc8, 0x47, 0x79, 0x60, 0x00, 0xbd, 0x00, 0xb5, 0x34, 0xe1,
+ 0x00, 0xb5, 0x30, 0x40, 0x40, 0x04, 0x5b, 0x08, 0x03, 0x43, 0xc8, 0x47,
+ 0x7c, 0x60, 0x38, 0x68, 0xc0, 0x46, 0xc0, 0x46, 0xc8, 0x47, 0x7d, 0x60,
+ 0x30, 0x40, 0x40, 0x04, 0x5b, 0x08, 0x03, 0x43, 0xc8, 0x47, 0x7c, 0x60,
+ 0x38, 0x68, 0xc0, 0x46, 0xc0, 0x46, 0xc8, 0x47, 0x7d, 0x60, 0x30, 0x40,
+ 0x40, 0x04, 0x5b, 0x08, 0x03, 0x43, 0xc8, 0x47, 0x7c, 0x60, 0x38, 0x68,
+ 0xc0, 0x46, 0xc0, 0x46, 0xc8, 0x47, 0x7d, 0x60, 0x30, 0x40, 0x40, 0x04,
+ 0x5b, 0x08, 0x03, 0x43, 0xc8, 0x47, 0x7c, 0x60, 0x38, 0x68, 0xc0, 0x46,
+ 0xc0, 0x46, 0xc8, 0x47, 0x7d, 0x60, 0x30, 0x40, 0x40, 0x04, 0x5b, 0x08,
+ 0x03, 0x43, 0xc8, 0x47, 0x7c, 0x60, 0x38, 0x68, 0xc0, 0x46, 0xc0, 0x46,
+ 0xc8, 0x47, 0x7d, 0x60, 0x30, 0x40, 0x40, 0x04, 0x5b, 0x08, 0x03, 0x43,
+ 0xc8, 0x47, 0x7c, 0x60, 0x38, 0x68, 0xc0, 0x46, 0xc0, 0x46, 0xc8, 0x47,
+ 0x7d, 0x60, 0x30, 0x40, 0x40, 0x04, 0x5b, 0x08, 0x03, 0x43, 0xc8, 0x47,
+ 0x7c, 0x60, 0x38, 0x68, 0xc0, 0x46, 0xc0, 0x46, 0xc8, 0x47, 0x7d, 0x60,
+ 0x30, 0x40, 0x40, 0x04, 0x5b, 0x08, 0x03, 0x43, 0xc8, 0x47, 0x7c, 0x60,
+ 0x38, 0x68, 0xc0, 0x46, 0xc0, 0x46, 0xc8, 0x47, 0x7d, 0x60, 0x30, 0x40,
+ 0x40, 0x04, 0x5b, 0x08, 0x03, 0x43, 0xc8, 0x47, 0x7c, 0x60, 0x38, 0x68,
+ 0xc0, 0x46, 0xc0, 0x46, 0xc8, 0x47, 0x7d, 0x60, 0x30, 0x40, 0x40, 0x04,
+ 0x5b, 0x08, 0x03, 0x43, 0xc8, 0x47, 0x7c, 0x60, 0x38, 0x68, 0xc0, 0x46,
+ 0xc0, 0x46, 0xc8, 0x47, 0x7d, 0x60, 0x30, 0x40, 0x40, 0x04, 0x5b, 0x08,
+ 0x03, 0x43, 0xc8, 0x47, 0x7c, 0x60, 0x38, 0x68, 0xc0, 0x46, 0xc0, 0x46,
+ 0xc8, 0x47, 0x7d, 0x60, 0x30, 0x40, 0x40, 0x04, 0x5b, 0x08, 0x03, 0x43,
+ 0xc8, 0x47, 0x7c, 0x60, 0x38, 0x68, 0xc0, 0x46, 0xc0, 0x46, 0xc8, 0x47,
+ 0x7d, 0x60, 0x30, 0x40, 0x40, 0x04, 0x5b, 0x08, 0x03, 0x43, 0xc8, 0x47,
+ 0x7c, 0x60, 0x38, 0x68, 0xc0, 0x46, 0xc0, 0x46, 0xc8, 0x47, 0x7d, 0x60,
+ 0x30, 0x40, 0x40, 0x04, 0x5b, 0x08, 0x03, 0x43, 0xc8, 0x47, 0x7c, 0x60,
+ 0x38, 0x68, 0xc0, 0x46, 0xc0, 0x46, 0xc8, 0x47, 0x7d, 0x60, 0x30, 0x40,
+ 0x40, 0x04, 0x5b, 0x08, 0x03, 0x43, 0xc8, 0x47, 0x7c, 0x60, 0x38, 0x68,
+ 0xc0, 0x46, 0xc0, 0x46, 0xc8, 0x47, 0x7d, 0x60, 0x30, 0x40, 0x40, 0x04,
+ 0x5b, 0x08, 0x03, 0x43, 0xc8, 0x47, 0x7c, 0x60, 0x38, 0x68, 0xc0, 0x46,
+ 0xc0, 0x46, 0xc8, 0x47, 0x7d, 0x60, 0x30, 0x40, 0x40, 0x04, 0x5b, 0x08,
+ 0x03, 0x43, 0xc8, 0x47, 0x7c, 0x60, 0x38, 0x68, 0xc0, 0x46, 0xc0, 0x46,
+ 0xc8, 0x47, 0x7d, 0x60, 0x30, 0x40, 0x40, 0x04, 0x5b, 0x08, 0x03, 0x43,
+ 0xc8, 0x47, 0x7c, 0x60, 0x38, 0x68, 0xc0, 0x46, 0xc0, 0x46, 0xc8, 0x47,
+ 0x7d, 0x60, 0x30, 0x40, 0x40, 0x04, 0x5b, 0x08, 0x03, 0x43, 0xc8, 0x47,
+ 0x7c, 0x60, 0x38, 0x68, 0xc0, 0x46, 0xc0, 0x46, 0xc8, 0x47, 0x7d, 0x60,
+ 0x30, 0x40, 0x40, 0x04, 0x5b, 0x08, 0x03, 0x43, 0xc8, 0x47, 0x7c, 0x60,
+ 0x38, 0x68, 0xc0, 0x46, 0xc0, 0x46, 0xc8, 0x47, 0x7d, 0x60, 0x30, 0x40,
+ 0x40, 0x04, 0x5b, 0x08, 0x03, 0x43, 0xc8, 0x47, 0x7c, 0x60, 0x38, 0x68,
+ 0xc0, 0x46, 0xc0, 0x46, 0xc8, 0x47, 0x7d, 0x60, 0x30, 0x40, 0x40, 0x04,
+ 0x5b, 0x08, 0x03, 0x43, 0xc8, 0x47, 0x7c, 0x60, 0x38, 0x68, 0xc0, 0x46,
+ 0xc0, 0x46, 0xc8, 0x47, 0x7d, 0x60, 0x30, 0x40, 0x40, 0x04, 0x5b, 0x08,
+ 0x03, 0x43, 0xc8, 0x47, 0x7c, 0x60, 0x38, 0x68, 0xc0, 0x46, 0xc0, 0x46,
+ 0xc8, 0x47, 0x7d, 0x60, 0x30, 0x40, 0x40, 0x04, 0x5b, 0x08, 0x03, 0x43,
+ 0xc8, 0x47, 0x7c, 0x60, 0x38, 0x68, 0xc0, 0x46, 0xc0, 0x46, 0xc8, 0x47,
+ 0x7d, 0x60, 0x30, 0x40, 0x40, 0x04, 0x5b, 0x08, 0x03, 0x43, 0xc8, 0x47,
+ 0x7c, 0x60, 0x38, 0x68, 0xc0, 0x46, 0xc0, 0x46, 0xc8, 0x47, 0x7d, 0x60,
+ 0x30, 0x40, 0x40, 0x04, 0x5b, 0x08, 0x03, 0x43, 0xc8, 0x47, 0x7c, 0x60,
+ 0x38, 0x68, 0xc0, 0x46, 0xc0, 0x46, 0xc8, 0x47, 0x7d, 0x60, 0x30, 0x40,
+ 0x40, 0x04, 0x5b, 0x08, 0x03, 0x43, 0xc8, 0x47, 0x7c, 0x60, 0x38, 0x68,
+ 0xc0, 0x46, 0xc0, 0x46, 0xc8, 0x47, 0x7d, 0x60, 0x30, 0x40, 0x40, 0x04,
+ 0x5b, 0x08, 0x03, 0x43, 0xc8, 0x47, 0x7c, 0x60, 0x38, 0x68, 0xc0, 0x46,
+ 0xc0, 0x46, 0xc8, 0x47, 0x7d, 0x60, 0x30, 0x40, 0x40, 0x04, 0x5b, 0x08,
+ 0x03, 0x43, 0xc8, 0x47, 0x7c, 0x60, 0x38, 0x68, 0xc0, 0x46, 0xc0, 0x46,
+ 0xc8, 0x47, 0x7d, 0x60, 0x30, 0x40, 0x40, 0x04, 0x5b, 0x08, 0x03, 0x43,
+ 0xc8, 0x47, 0x7c, 0x60, 0x38, 0x68, 0xc0, 0x46, 0xc0, 0x46, 0xc8, 0x47,
+ 0x7d, 0x60, 0x30, 0x40, 0x40, 0x04, 0x5b, 0x08, 0x03, 0x43, 0xc8, 0x47,
+ 0x7c, 0x60, 0x38, 0x68, 0xc0, 0x46, 0xc0, 0x46, 0xc8, 0x47, 0x7d, 0x60,
+ 0x30, 0x40, 0x40, 0x04, 0x5b, 0x08, 0x03, 0x43, 0xc8, 0x47, 0x7c, 0x60,
+ 0x38, 0x68, 0xc0, 0x46, 0xc0, 0x46, 0xc8, 0x47, 0x7d, 0x60, 0x30, 0x40,
+ 0x40, 0x04, 0x5b, 0x08, 0x03, 0x43, 0x00, 0xbd, 0x5f, 0x48, 0x83, 0x46,
+ 0x5f, 0x48, 0x82, 0x46, 0x5f, 0x48, 0x81, 0x46, 0x5f, 0x48, 0x80, 0x46,
+ 0x5f, 0x4f, 0x60, 0x4e, 0x60, 0x4d, 0x61, 0x4c, 0x70, 0x47, 0x03, 0x20,
+ 0x43, 0x46, 0xd8, 0x60, 0x00, 0xbd, 0x02, 0x20, 0x43, 0x46, 0xd8, 0x60,
+ 0x00, 0xbd, 0x00, 0xb5, 0x5c, 0x48, 0x84, 0x46, 0x5b, 0x68, 0xff, 0xf7,
+ 0xeb, 0xfc, 0x52, 0x4b, 0xbb, 0x60, 0xff, 0xf7, 0x79, 0xfe, 0x5b, 0x0f,
+ 0x01, 0x2b, 0x09, 0xd0, 0x57, 0x49, 0xb9, 0x60, 0x02, 0x2b, 0xe6, 0xd1,
+ 0x60, 0x46, 0x01, 0x38, 0x84, 0x46, 0xe6, 0xd0, 0x43, 0x46, 0xeb, 0xe7,
+ 0xff, 0xf7, 0x6c, 0xfe, 0xff, 0xf7, 0xbd, 0xfc, 0x50, 0x49, 0xb9, 0x60,
+ 0x59, 0x46, 0x31, 0x43, 0x79, 0x60, 0x41, 0x46, 0x4b, 0x60, 0x88, 0x60,
+ 0x00, 0x20, 0xc8, 0x60, 0x00, 0xbd, 0x00, 0xb5, 0x49, 0x48, 0x84, 0x46,
+ 0x5b, 0x68, 0xff, 0xf7, 0xc5, 0xfc, 0x08, 0xb4, 0x3e, 0x4b, 0xbb, 0x60,
+ 0xff, 0xf7, 0x52, 0xfe, 0x5b, 0x0f, 0x01, 0x2b, 0x0a, 0xd0, 0x01, 0xbc,
+ 0x43, 0x49, 0xb9, 0x60, 0x02, 0x2b, 0xbe, 0xd1, 0x60, 0x46, 0x01, 0x38,
+ 0x84, 0x46, 0xbe, 0xd0, 0x43, 0x46, 0xe9, 0xe7, 0x3e, 0x4b, 0xbb, 0x60,
+ 0xff, 0xf7, 0x32, 0xfe, 0x43, 0x46, 0x9b, 0x68, 0xff, 0xf7, 0xac, 0xfc,
+ 0x08, 0xbc, 0xff, 0xf7, 0x2b, 0xfe, 0x43, 0x46, 0x00, 0x20, 0xd8, 0x60,
+ 0x00, 0xbd, 0x00, 0xb5, 0x37, 0x4b, 0x9c, 0x46, 0xff, 0xf7, 0xa0, 0xfc,
+ 0x63, 0x46, 0xff, 0xf7, 0x9d, 0xfc, 0x35, 0x4b, 0xff, 0xf7, 0x98, 0xfc,
+ 0x63, 0x46, 0xff, 0xf7, 0x97, 0xfc, 0x63, 0x46, 0xff, 0xf7, 0x94, 0xfc,
+ 0x43, 0x46, 0x00, 0x20, 0xd8, 0x60, 0x00, 0xbd, 0x00, 0xb5, 0x2f, 0x48,
+ 0x00, 0x21, 0x01, 0x60, 0x43, 0x46, 0x18, 0x68, 0x05, 0x28, 0x00, 0xd9,
+ 0x00, 0x20, 0x80, 0x00, 0x01, 0xa1, 0x0a, 0x58, 0x90, 0x47, 0x00, 0xbd,
+ 0xf9, 0x07, 0x00, 0x18, 0xff, 0x07, 0x00, 0x18, 0xff, 0x06, 0x00, 0x18,
+ 0x4b, 0x07, 0x00, 0x18, 0x9b, 0x07, 0x00, 0x18, 0x05, 0x08, 0x00, 0x18,
+ 0x09, 0x20, 0xd8, 0x60, 0x70, 0x47, 0x00, 0x20, 0xd8, 0x60, 0x70, 0x47,
+ 0x58, 0x68, 0x08, 0x28, 0x00, 0xd9, 0x00, 0x20, 0x80, 0x00, 0x03, 0xa1,
+ 0x09, 0x58, 0x89, 0x46, 0x00, 0x20, 0xd8, 0x60, 0x70, 0x47, 0xc0, 0x46,
+ 0x65, 0x00, 0x00, 0x18, 0x65, 0x00, 0x00, 0x18, 0x65, 0x00, 0x00, 0x18,
+ 0x85, 0x00, 0x00, 0x18, 0x95, 0x00, 0x00, 0x18, 0x95, 0x00, 0x00, 0x18,
+ 0xa5, 0x00, 0x00, 0x18, 0xa5, 0x00, 0x00, 0x18, 0xad, 0x00, 0x00, 0x18,
+ 0x11, 0x48, 0x12, 0x49, 0x01, 0x60, 0xff, 0xf7, 0x45, 0xff, 0x11, 0x48,
+ 0x02, 0x21, 0x01, 0x60, 0x30, 0xbf, 0xfd, 0xe7, 0x00, 0x88, 0x00, 0x00,
+ 0x00, 0x80, 0x00, 0x00, 0x95, 0x00, 0x00, 0x18, 0x00, 0x40, 0x00, 0x18,
+ 0x10, 0x12, 0x10, 0x40, 0x00, 0x40, 0x00, 0x00, 0x00, 0x08, 0x00, 0x00,
+ 0x00, 0x00, 0x00, 0x00, 0x00, 0x20, 0x00, 0x00, 0x00, 0xc8, 0x00, 0x00,
+ 0xff, 0xff, 0xff, 0xff, 0x9e, 0xe7, 0x00, 0x00, 0x30, 0x31, 0x04, 0x40,
+ 0x00, 0x00, 0x00, 0x18, 0x00, 0x00, 0xaa, 0xaa, 0x00, 0xe1, 0x00, 0xe0
+};
+unsigned int zero_bin_len = 2196;
diff --git a/src/bsp/lk/app/mdebug/jtag.c b/src/bsp/lk/app/mdebug/jtag.c
new file mode 100644
index 0000000..517ab5c
--- /dev/null
+++ b/src/bsp/lk/app/mdebug/jtag.c
@@ -0,0 +1,116 @@
+/* swdp-m0sub.c
+ *
+ * Copyright 2015 Brian Swetland <swetland@frotz.net>
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include <app.h>
+#include <debug.h>
+#include <string.h>
+#include <stdlib.h>
+#include <printf.h>
+
+#include <platform.h>
+#include <arch/arm.h>
+#include <kernel/thread.h>
+
+#include <platform/lpc43xx-gpio.h>
+#include <platform/lpc43xx-sgpio.h>
+#include <platform/lpc43xx-clocks.h>
+
+#include "rswdp.h"
+
+#include "lpclink2.h"
+
+static void gpio_init(void) {
+ pin_config(PIN_LED, PIN_MODE(0) | PIN_PLAIN);
+ pin_config(PIN_RESET, PIN_MODE(4) | PIN_PLAIN);
+ pin_config(PIN_RESET_TXEN, PIN_MODE(4) | PIN_PLAIN);
+ pin_config(PIN_TMS_TXEN, PIN_MODE(0) | PIN_PLAIN);
+
+ pin_config(PIN_TDO, PIN_MODE(6) | PIN_PLAIN | PIN_INPUT | PIN_FAST);
+ pin_config(PIN_TCK, PIN_MODE(6) | PIN_PLAIN | PIN_FAST);
+ pin_config(PIN_TDI, PIN_MODE(6) | PIN_PLAIN | PIN_FAST);
+ pin_config(PIN_TMS, PIN_MODE(6) | PIN_PLAIN | PIN_FAST);
+
+ gpio_set(GPIO_LED, 0);
+ gpio_set(GPIO_RESET, 1);
+ gpio_set(GPIO_RESET_TXEN, 0);
+ gpio_set(GPIO_TMS_TXEN, 1);
+
+ gpio_config(GPIO_LED, GPIO_OUTPUT);
+ gpio_config(GPIO_RESET, GPIO_OUTPUT);
+ gpio_config(GPIO_RESET_TXEN, GPIO_OUTPUT);
+ gpio_config(GPIO_TMS_TXEN, GPIO_OUTPUT);
+}
+
+#define POS_TDO 10
+#define POS_TCK 11
+#define POS_TDI 12
+#define POS_TMS 14
+
+#define BIT_TDO (1 << POS_TDO)
+#define BIT_TCK (1 << POS_TCK)
+#define BIT_TDI (1 << POS_TDI)
+#define BIT_TMS (1 << POS_TMS)
+
+void jtag_init(void) {
+ gpio_init();
+
+ writel(BASE_CLK_SEL(CLK_PLL1), BASE_PERIPH_CLK);
+ spin(1000);
+
+ // configure for SGPIO_OUT/OEN control
+ writel(CFG_OUT_GPIO | CFG_OE_GPIO, SGPIO_OUT_CFG(POS_TDO));
+ writel(CFG_OUT_GPIO | CFG_OE_GPIO, SGPIO_OUT_CFG(POS_TCK));
+ writel(CFG_OUT_GPIO | CFG_OE_GPIO, SGPIO_OUT_CFG(POS_TDI));
+ writel(CFG_OUT_GPIO | CFG_OE_GPIO, SGPIO_OUT_CFG(POS_TMS));
+
+ // TCK=0 TDI=0 TMS=0 TDO=input
+ writel(0, SGPIO_OUT);
+ writel(BIT_TCK | BIT_TDI | BIT_TMS, SGPIO_OEN);
+}
+
+static unsigned jtag_tick(unsigned tms, unsigned tdi) {
+ unsigned x = (tms << POS_TMS) | (tdi << POS_TDI);
+ unsigned v;
+ writel(x, SGPIO_OUT);
+ writel(x, SGPIO_OUT);
+ writel(x, SGPIO_OUT);
+ x |= BIT_TCK;
+ v = readl(SGPIO_IN);
+ writel(x, SGPIO_OUT);
+ writel(x, SGPIO_OUT);
+ writel(x, SGPIO_OUT);
+ writel(x, SGPIO_OUT);
+ writel(x, SGPIO_OUT);
+ writel(x, SGPIO_OUT);
+ x ^= BIT_TCK;
+ writel(x, SGPIO_OUT);
+ return (v >> POS_TDO) & 1;
+}
+
+int jtag_io(unsigned count, unsigned tms, unsigned tdi, unsigned *tdo) {
+ unsigned n = 0;
+ unsigned bit = 0;
+ while (count > 0) {
+ n |= (jtag_tick(tms & 1, tdi & 1) << bit);
+ bit++;
+ count--;
+ tms >>= 1;
+ tdi >>= 1;
+ }
+ *tdo = n;
+ return 0;
+}
diff --git a/src/bsp/lk/app/mdebug/lpclink2.h b/src/bsp/lk/app/mdebug/lpclink2.h
new file mode 100644
index 0000000..ab29a8d
--- /dev/null
+++ b/src/bsp/lk/app/mdebug/lpclink2.h
@@ -0,0 +1,30 @@
+
+// gpio configuration for JTAG
+#define PIN_LED PIN(1,1)
+#define PIN_RESET PIN(2,5)
+#define PIN_RESET_TXEN PIN(2,6)
+#define PIN_TMS_TXEN PIN(1,5) // SGPIO15=6
+#define PIN_TMS PIN(1,6) // SGPIO14=6
+#define PIN_TCK PIN(1,17) // SGPIO11=6
+#define PIN_TDI PIN(1,18) // SGPIO12=6
+#define PIN_TDO PIN(1,14) // U1_RXD=1, SGPIO10=6
+
+#define GPIO_LED GPIO(0,8)
+#define GPIO_RESET GPIO(5,5)
+#define GPIO_RESET_TXEN GPIO(5,6)
+#define GPIO_TMS_TXEN GPIO(1,8)
+#define GPIO_TMS GPIO(1,9)
+#define GPIO_TCK GPIO(0,12)
+#define GPIO_TDI GPIO(0,13)
+#define GPIO_TDO GPIO(1,7)
+
+// alternate names for SWD
+#define PIN_SWDIO_TXEN PIN_TMS_TXEN
+#define PIN_SWDIO PIN_TMS
+#define PIN_SWCLK PIN_TCK
+#define PIN_SWO PIN_TDO
+
+#define GPIO_SWDIO_TXEN GPIO_TMS_TXEN
+#define GPIO_SWDIO GPIO_TMS
+#define GPIO_SWCLK GPIO_TCK
+
diff --git a/src/bsp/lk/app/mdebug/mdebug.c b/src/bsp/lk/app/mdebug/mdebug.c
new file mode 100644
index 0000000..f186f39
--- /dev/null
+++ b/src/bsp/lk/app/mdebug/mdebug.c
@@ -0,0 +1,160 @@
+/* mdebug.c
+ *
+ * Copyright 2015 Brian Swetland <swetland@frotz.net>
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include <app.h>
+#include <debug.h>
+#include <string.h>
+#include <stdlib.h>
+#include <printf.h>
+#include <dev/udc.h>
+
+#include <platform.h>
+#include <arch/arm.h>
+#include <kernel/thread.h>
+#include <kernel/event.h>
+
+#include <platform/lpc43xx-gpio.h>
+
+#include "swd.h"
+
+#define TX_AHEAD 1
+
+static event_t txevt = EVENT_INITIAL_VALUE(txevt, TX_AHEAD, 0);
+static event_t rxevt = EVENT_INITIAL_VALUE(rxevt, 0, 0);
+
+static udc_request_t *txreq;
+static udc_request_t *rxreq;
+static udc_endpoint_t *txept;
+static udc_endpoint_t *rxept;
+
+static volatile int online;
+static volatile int txstatus;
+static volatile int rxstatus;
+static volatile int rxactual;
+
+static void mdebug_notify(udc_gadget_t *gadget, unsigned event) {
+ if (event == UDC_EVENT_ONLINE) {
+ online = 1;
+ } else {
+ online = 0;
+ }
+}
+
+static void rx_complete(udc_request_t *req, unsigned actual, int status) {
+ rxactual = actual;
+ rxstatus = status;
+ event_signal(&rxevt, 0);
+}
+
+static void tx_complete(udc_request_t *req, unsigned actual, int status) {
+ txstatus = status;
+ event_signal(&txevt, 0);
+}
+
+#if TX_AHEAD
+void usb_xmit(void *data, unsigned len) {
+ event_wait(&txevt);
+ event_unsignal(&txevt);
+ txreq->buffer = data;
+ txreq->length = len;
+ txstatus = 1;
+ if (udc_request_queue(txept, txreq)) {
+ printf("txqf\n");
+ event_signal(&txevt, 0);
+ }
+}
+#else
+void usb_xmit(void *data, unsigned len) {
+ event_unsignal(&txevt);
+ txreq->buffer = data;
+ txreq->length = len;
+ txstatus = 1;
+ if (udc_request_queue(txept, txreq) == 0) {
+ event_wait(&txevt);
+ }
+}
+#endif
+
+int usb_recv(void *data, unsigned len) {
+ event_unsignal(&rxevt);
+ rxreq->buffer = data;
+ rxreq->length = len;
+ rxstatus = 1;
+ if (udc_request_queue(rxept, rxreq)) {
+ printf("rxqf\n");
+ return -1;
+ }
+ event_wait(&rxevt);
+ return rxstatus ? rxstatus : rxactual;
+}
+
+static udc_device_t mdebug_device = {
+ .vendor_id = 0x1209,
+ .product_id = 0x5038,
+ .version_id = 0x0100,
+};
+
+static udc_endpoint_t *mdebug_endpoints[2];
+
+static udc_gadget_t mdebug_gadget = {
+ .notify = mdebug_notify,
+ .ifc_class = 0xFF,
+ .ifc_subclass = 0xFF,
+ .ifc_protocol = 0xFF,
+ .ifc_endpoints = 2,
+ .ept = mdebug_endpoints,
+};
+
+static void mdebug_init(const struct app_descriptor *app)
+{
+ swd_init();
+
+ udc_init(&mdebug_device);
+ mdebug_endpoints[0] = txept = udc_endpoint_alloc(UDC_BULK_IN, 512);
+ mdebug_endpoints[1] = rxept = udc_endpoint_alloc(UDC_BULK_OUT, 512);
+ txreq = udc_request_alloc();
+ rxreq = udc_request_alloc();
+ rxreq->complete = rx_complete;
+ txreq->complete = tx_complete;
+ udc_register_gadget(&mdebug_gadget);
+}
+
+void handle_rswd(void);
+void swo_init(udc_endpoint_t *ept);
+void swo_config(unsigned mhz);
+
+static void mdebug_entry(const struct app_descriptor *app, void *args)
+{
+ udc_start();
+ swo_init(txept);
+ swo_config(6000000);
+
+ for (;;) {
+ if (!online) {
+ thread_yield();
+ continue;
+ }
+ handle_rswd();
+ }
+}
+
+APP_START(usbtest)
+ .init = mdebug_init,
+ .entry = mdebug_entry,
+APP_END
+
+
diff --git a/src/bsp/lk/app/mdebug/rswd.c b/src/bsp/lk/app/mdebug/rswd.c
new file mode 100644
index 0000000..7daa8b7
--- /dev/null
+++ b/src/bsp/lk/app/mdebug/rswd.c
@@ -0,0 +1,319 @@
+/* rswd.c
+ *
+ * Copyright 2011-2015 Brian Swetland <swetland@frotz.net>
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include <reg.h>
+#include <debug.h>
+#include <string.h>
+#include <stdlib.h>
+#include <printf.h>
+
+#include <platform.h>
+
+#include "swd.h"
+#include "rswdp.h"
+
+void usb_xmit(void *data, unsigned len);
+int usb_recv(void *data, unsigned len);
+
+unsigned swdp_trace = 0;
+
+// indicates host knows about v1.0 protocol features
+unsigned host_version = 0;
+
+static u8 optable[16] = {
+ [OP_RD | OP_DP | OP_X0] = RD_IDCODE,
+ [OP_RD | OP_DP | OP_X4] = RD_DPCTRL,
+ [OP_RD | OP_DP | OP_X8] = RD_RESEND,
+ [OP_RD | OP_DP | OP_XC] = RD_BUFFER,
+ [OP_WR | OP_DP | OP_X0] = WR_ABORT,
+ [OP_WR | OP_DP | OP_X4] = WR_DPCTRL,
+ [OP_WR | OP_DP | OP_X8] = WR_SELECT,
+ [OP_WR | OP_DP | OP_XC] = WR_BUFFER,
+ [OP_RD | OP_AP | OP_X0] = RD_AP0,
+ [OP_RD | OP_AP | OP_X4] = RD_AP1,
+ [OP_RD | OP_AP | OP_X8] = RD_AP2,
+ [OP_RD | OP_AP | OP_XC] = RD_AP3,
+ [OP_WR | OP_AP | OP_X0] = WR_AP0,
+ [OP_WR | OP_AP | OP_X4] = WR_AP1,
+ [OP_WR | OP_AP | OP_X8] = WR_AP2,
+ [OP_WR | OP_AP | OP_XC] = WR_AP3,
+};
+
+static const char *board_str = TARGET;
+static const char *build_str = "fw v0.91 (" __DATE__ ", " __TIME__ ")";
+
+static void _reboot(void) {
+ platform_halt(HALT_ACTION_REBOOT, HALT_REASON_SW_RESET);
+}
+
+#define MODE_SWD 0
+#define MODE_JTAG 1
+static unsigned mode = MODE_SWD;
+
+/* TODO bounds checking -- we trust the host far too much */
+void process_txn(u32 txnid, u32 *rx, int rxc, u32 *tx) {
+ unsigned msg, op, n;
+ unsigned txc = 1;
+ unsigned count = 0;
+ unsigned status = 0;
+ void (*func)(void) = 0;
+
+ tx[0] = txnid;
+
+ while (rxc-- > 0) {
+ count++;
+ msg = *rx++;
+ op = RSWD_MSG_OP(msg);
+ n = RSWD_MSG_ARG(msg);
+#if CONFIG_MDEBUG_TRACE
+ printf("> %02x %02x %04x <\n", RSWD_MSG_CMD(msg), op, n);
+#endif
+ switch (RSWD_MSG_CMD(msg)) {
+ case CMD_NULL:
+ continue;
+ case CMD_SWD_WRITE:
+ while (n-- > 0) {
+ rxc--;
+ status = swd_write(optable[op], *rx++);
+ if (status) {
+ goto done;
+ }
+ }
+ continue;
+ case CMD_SWD_READ:
+ tx[txc++] = RSWD_MSG(CMD_SWD_DATA, 0, n);
+ while (n-- > 0) {
+ status = swd_read(optable[op], tx + txc);
+ if (status) {
+ txc++;
+ while (n-- > 0)
+ tx[txc++] = 0xfefefefe;
+ goto done;
+ }
+ txc++;
+ }
+ continue;
+ case CMD_SWD_DISCARD:
+ while (n-- > 0) {
+ u32 tmp;
+ status = swd_read(optable[op], &tmp);
+ if (status) {
+ goto done;
+ }
+ }
+ continue;
+ case CMD_ATTACH:
+ if (mode != MODE_SWD) {
+ mode = MODE_SWD;
+ swd_init();
+ }
+ swd_reset();
+ continue;
+ case CMD_JTAG_IO:
+ if (mode != MODE_JTAG) {
+ mode = MODE_JTAG;
+ jtag_init();
+ }
+ tx[txc++] = RSWD_MSG(CMD_JTAG_DATA, 0, n);
+ while (n > 0) {
+ unsigned xfer = (n > 32) ? 32 : n;
+ jtag_io(xfer, rx[0], rx[1], tx + txc);
+ rx += 2;
+ rxc -= 2;
+ txc += 1;
+ n -= xfer;
+ }
+ continue;
+ case CMD_JTAG_VRFY:
+ if (mode != MODE_JTAG) {
+ mode = MODE_JTAG;
+ jtag_init();
+ }
+ // (n/32) x 4 words: TMS, TDI, DATA, MASK
+ while (n > 0) {
+ unsigned xfer = (n > 32) ? 32 : n;
+ jtag_io(xfer, rx[0], rx[1], tx + txc);
+ if ((tx[txc] & rx[3]) != rx[2]) {
+ status = ERR_BAD_MATCH;
+ goto done;
+ }
+ rx += 4;
+ rxc -= 4;
+ n -= xfer;
+ }
+ continue;
+ case CMD_JTAG_TX: {
+ unsigned tms = (op & 1) ? 0xFFFFFFFF : 0;
+ if (mode != MODE_JTAG) {
+ mode = MODE_JTAG;
+ jtag_init();
+ }
+ while (n > 0) {
+ unsigned xfer = (n > 32) ? 32 : n;
+ jtag_io(xfer, tms, rx[0], rx);
+ rx++;
+ rxc--;
+ n -= xfer;
+ }
+ continue;
+ }
+ case CMD_JTAG_RX: {
+ unsigned tms = (op & 1) ? 0xFFFFFFFF : 0;
+ unsigned tdi = (op & 2) ? 0xFFFFFFFF : 0;
+ if (mode != MODE_JTAG) {
+ mode = MODE_JTAG;
+ jtag_init();
+ }
+ tx[txc++] = RSWD_MSG(CMD_JTAG_DATA, 0, n);
+ while (n > 0) {
+ unsigned xfer = (n > 32) ? 32 : n;
+ jtag_io(xfer, tms, tdi, tx + txc);
+ txc++;
+ n -= xfer;
+ }
+ continue;
+ }
+ case CMD_RESET:
+ swd_hw_reset(n);
+ continue;
+ case CMD_DOWNLOAD: {
+ //u32 *addr = (void*) *rx++;
+ rxc--;
+ while (n) {
+ //*addr++ = *rx++;
+ rx++;
+ rxc--;
+ }
+ continue;
+ }
+ case CMD_EXECUTE:
+ //func = (void*) *rx++;
+ rxc--;
+ continue;
+ case CMD_TRACE:
+ swdp_trace = op;
+ continue;
+ case CMD_BOOTLOADER:
+ func = _reboot;
+ continue;
+ case CMD_SET_CLOCK:
+ n = swd_set_clock(n);
+ printf("swdp clock is now %d KHz\n", n);
+ if (host_version >= RSWD_VERSION_1_0) {
+ tx[txc++] = RSWD_MSG(CMD_CLOCK_KHZ, 0, n);
+ }
+ continue;
+ case CMD_SWO_CLOCK:
+ n = swo_set_clock(n);
+ printf("swo clock is now %d KHz\n", n);
+ continue;
+ case CMD_VERSION:
+ host_version = n;
+ tx[txc++] = RSWD_MSG(CMD_VERSION, 0, RSWD_VERSION);
+
+ n = strlen(board_str);
+ memcpy(tx + txc + 1, board_str, n + 1);
+ n = (n + 4) / 4;
+ tx[txc++] = RSWD_MSG(CMD_BOARD_STR, 0, n);
+ txc += n;
+
+ n = strlen(build_str);
+ memcpy(tx + txc + 1, build_str, n + 1);
+ n = (n + 4) / 4;
+ tx[txc++] = RSWD_MSG(CMD_BUILD_STR, 0, n);
+ txc += n;
+
+ tx[txc++] = RSWD_MSG(CMD_RX_MAXDATA, 0, 8192);
+ txc += n;
+ continue;
+ default:
+ printf("unknown command %02x\n", RSWD_MSG_CMD(msg));
+ status = 1;
+ goto done;
+ }
+ }
+
+done:
+ tx[txc++] = RSWD_MSG(CMD_STATUS, status, count);
+
+ /* if we're about to send an even multiple of the packet size
+ * (64), add a NULL op on the end to create a short packet at
+ * the end.
+ */
+ if ((txc & 0xf) == 0)
+ tx[txc++] = RSWD_MSG(CMD_NULL, 0, 0);
+
+#if CONFIG_MDEBUG_TRACE
+ printf("[ send %d words ]\n", txc);
+ for (n = 0; n < txc; n+=4) {
+ printx("%08x %08x %08x %08x\n",
+ tx[n], tx[n+1], tx[n+2], tx[n+3]);
+ }
+#endif
+ usb_xmit(tx, txc * 4);
+
+ if (func) {
+ for (n = 0; n < 1000000; n++) asm("nop");
+ func();
+ for (;;) ;
+ }
+}
+
+// io buffers in AHB SRAM
+static u32 *rxbuffer = (void*) 0x20001000;
+static u32 *txbuffer[2] = {(void*) 0x20003000, (void*) 0x20005000 };
+
+#include <kernel/thread.h>
+
+void handle_rswd(void) {
+ int rxc;
+ int toggle = 0;
+
+#if CONFIG_MDEBUG_TRACE
+ printf("[ rswdp agent v0.9 ]\n");
+ printf("[ built " __DATE__ " " __TIME__ " ]\n");
+#endif
+
+ for (;;) {
+ rxc = usb_recv(rxbuffer, 8192);
+
+#if CONFIG_MDEBUG_TRACE
+ int n;
+ printx("[ recv %d words ]\n", rxc/4);
+ for (n = 0; n < (rxc/4); n+=4) {
+ printx("%08x %08x %08x %08x\n",
+ rxbuffer[n], rxbuffer[n+1],
+ rxbuffer[n+2], rxbuffer[n+3]);
+ }
+#endif
+
+ if ((rxc < 4) || (rxc & 3)) {
+ printf("error, runt frame, or strange frame... %d\n", rxc);
+ continue;
+ }
+
+ rxc = rxc / 4;
+
+ if ((rxbuffer[0] & 0xFFFF0000) != 0xAA770000) {
+ printf("invalid frame %x\n", rxbuffer[0]);
+ continue;
+ }
+
+ process_txn(rxbuffer[0], rxbuffer + 1, rxc - 1, txbuffer[toggle]);
+ toggle ^= 1;
+ }
+}
diff --git a/src/bsp/lk/app/mdebug/rswdp.h b/src/bsp/lk/app/mdebug/rswdp.h
new file mode 100644
index 0000000..0de0aea
--- /dev/null
+++ b/src/bsp/lk/app/mdebug/rswdp.h
@@ -0,0 +1,127 @@
+/* rswdp.h - remote serial wire debug protocol
+ *
+ * Copyright 2011 Brian Swetland <swetland@frotz.net>
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+/* Remote Serial Wire Debug Protocol */
+
+#ifndef _RSWDP_PROTOCOL_H_
+#define _RSWDP_PROTOCOL_H_
+
+/* Basic framing:
+ * - host and device exchange "transactions" consisting of
+ * some number of "messages".
+ * - each "message" has a 32bit header and may have 0 or more
+ * 32bit words of payload
+ * - a transaction may not exceed 4K (1024 words)
+ * - a transaction is sent in a series of USB BULK packets
+ * - the final packet must be a short packet unless the
+ * transaction is exactly 4K in length
+ * - packets must be a multiple of 4 bytes
+ * - the first message in a transaction must be
+ * CMD_TXN_START or CMD_TXN_ASYNC
+ */
+
+#define RSWD_MSG(cmd,op,n) ((((cmd)&0xFF) << 24) | (((op) & 0xFF)<<16) | ((n) & 0xFFFF))
+#define RSWD_MSG_CMD(n) (((n) >> 24) & 0xFF)
+#define RSWD_MSG_OP(n) (((n) >> 16) & 0xFF)
+#define RSWD_MSG_ARG(n) ((n) & 0xFFFF)
+
+#define RSWD_TXN_START(seq) (0xAA770000 | ((seq) & 0xFFFF))
+#define RSWD_TXN_ASYNC (0xAA001111)
+
+/* valid: either */
+#define CMD_NULL 0x00 /* used for padding */
+
+/* valid: host to target */
+#define CMD_SWD_WRITE 0x01 /* op=addr arg=count payload: data x count */
+#define CMD_SWD_READ 0x02 /* op=addr arg=count payload: data x count */
+#define CMD_SWD_DISCARD 0x03 /* op=addr arg=count payload: none (discards) */
+#define CMD_ATTACH 0x04 /* do swdp reset/connect handshake */
+#define CMD_RESET 0x05 /* arg=1 -> assert RESETn, otherwise deassert */
+#define CMD_DOWNLOAD 0x06 /* arg=wordcount, payload: addr x 1, data x n */
+#define CMD_EXECUTE 0x07 /* payload: addr x 1 */
+#define CMD_TRACE 0x08 /* op=tracebits n=0 */
+#define CMD_BOOTLOADER 0x09 /* return to bootloader for reflashing */
+#define CMD_SET_CLOCK 0x0A /* set SWCLK rate to n khz */
+#define CMD_SWO_CLOCK 0x0B /* set SWOCLK rate to n khz, 0 = disable SWO */
+#define CMD_JTAG_IO 0x0C /* op=0, arg=bitcount, data x (count/32) * 2 */
+ /* tms, tdi word pairs per 32bits */
+#define CMD_JTAG_TX 0x0D /* tms=op.0, arg=bitcount, data x (count/32) words of tdi */
+#define CMD_JTAG_RX 0x0E /* tms=op.0, tdi=op.1, arg=bitcount, return (count/32) words */
+#define CMD_JTAG_VRFY 0x0F /* arg=bitcount, tms/tdi data/mask, error if tdo&mask != data */
+
+/* valid: target to host */
+#define CMD_STATUS 0x10 /* op=errorcode, arg=commands since last TXN_START */
+#define CMD_SWD_DATA 0x11 /* op=0 arg=count, payload: data x count */
+#define CMD_SWO_DATA 0x12 /* op=0 arg=count, payload: ((count+3)/4) words */
+#define CMD_JTAG_DATA 0x14 /* op=0 arg=bitcount, payload: (arg/32) words */
+
+/* valid: target to host async */
+#define CMD_DEBUG_PRINT 0x20 /* arg*4 bytes of ascii debug output */
+
+/* valid: bidirectional query/config messages */
+#define CMD_VERSION 0x30 /* arg=bcdversion (0x0100 etc) */
+#define CMD_BUILD_STR 0x31 /* arg=wordcount, payload = asciiz */
+#define CMD_BOARD_STR 0x32 /* arg=wordcount, payload = asciiz */
+#define CMD_RX_MAXDATA 0x33 /* arg=bytes, declares senders rx buffer size */
+#define CMD_CLOCK_KHZ 0x34 /* arg=khz, reports active clock rate */
+
+/* CMD_STATUS error codes */
+#define ERR_NONE 0
+#define ERR_INTERNAL 1
+#define ERR_TIMEOUT 2
+#define ERR_IO 3
+#define ERR_PARITY 4
+#define ERR_BAD_MATCH 5
+
+#define RSWD_VERSION 0x0102
+
+#define RSWD_VERSION_1_0 0x0100
+#define RSWD_VERSION_1_1 0x0101
+#define RSWD_VERSION_1_2 0x0102
+
+// Pre-1.0
+// - max packet size fixed at 2048 bytes
+//
+// Version 1.0
+// - CMD_VERSION, CMD_BUILD_STR, CMD_BOARD_STR, CMD_RX_MAXDATA,
+// CMD_CLOCK_KHZ added
+//
+// Version 1.1
+// - CMD_SWO_DATA arg is now byte count, not word count
+//
+// Version 1.2
+// - CMD_JTAG_IO, CMD_JTAG_RX, CMD_JTAG_TX, CMD_JTAG_VRFY, CMD_JTAG_DATA added
+
+/* CMD_SWD_OP operations - combine for direct AP/DP io */
+#define OP_RD 0x00
+#define OP_WR 0x01
+#define OP_DP 0x00
+#define OP_AP 0x02
+#define OP_X0 0x00
+#define OP_X4 0x04
+#define OP_X8 0x08
+#define OP_XC 0x0C
+
+/* DP registers */
+#define DP_IDCODE (OP_DP|OP_X0)
+#define DP_ABORT (OP_DP|OP_X0)
+#define DP_DPCTRL (OP_DP|OP_X4)
+#define DP_RESEND (OP_DP|OP_X8)
+#define DP_SELECT (OP_DP|OP_X8)
+#define DP_BUFFER (OP_DP|OP_XC)
+
+#endif
diff --git a/src/bsp/lk/app/mdebug/rules.mk b/src/bsp/lk/app/mdebug/rules.mk
new file mode 100644
index 0000000..168d3aa
--- /dev/null
+++ b/src/bsp/lk/app/mdebug/rules.mk
@@ -0,0 +1,13 @@
+LOCAL_DIR := $(GET_LOCAL_DIR)
+
+MODULE := $(LOCAL_DIR)
+
+MODULE_SRCS += \
+ $(LOCAL_DIR)/mdebug.c \
+ $(LOCAL_DIR)/rswd.c \
+ $(LOCAL_DIR)/jtag.c \
+ $(LOCAL_DIR)/swd-m0sub.c \
+ $(LOCAL_DIR)/swo-uart1.c
+
+include make/module.mk
+
diff --git a/src/bsp/lk/app/mdebug/swd-m0sub.c b/src/bsp/lk/app/mdebug/swd-m0sub.c
new file mode 100644
index 0000000..63ce42a
--- /dev/null
+++ b/src/bsp/lk/app/mdebug/swd-m0sub.c
@@ -0,0 +1,184 @@
+/* swdp-m0sub.c
+ *
+ * Copyright 2015 Brian Swetland <swetland@frotz.net>
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include <app.h>
+#include <debug.h>
+#include <string.h>
+#include <stdlib.h>
+#include <printf.h>
+
+#include <platform.h>
+#include <arch/arm.h>
+#include <kernel/thread.h>
+
+#include <platform/lpc43xx-gpio.h>
+#include <platform/lpc43xx-sgpio.h>
+#include <platform/lpc43xx-clocks.h>
+
+#include "rswdp.h"
+
+#include "lpclink2.h"
+
+static void gpio_init(void) {
+ pin_config(PIN_LED, PIN_MODE(0) | PIN_PLAIN);
+ pin_config(PIN_RESET, PIN_MODE(4) | PIN_PLAIN);
+ pin_config(PIN_RESET_TXEN, PIN_MODE(4) | PIN_PLAIN);
+
+ pin_config(PIN_SWDIO_TXEN, PIN_MODE(6) | PIN_PLAIN | PIN_FAST);
+ pin_config(PIN_SWDIO, PIN_MODE(6) | PIN_PLAIN | PIN_INPUT | PIN_FAST);
+ pin_config(PIN_SWCLK, PIN_MODE(6) | PIN_PLAIN | PIN_FAST);
+
+ pin_config(PIN_SWO, PIN_MODE(1) | PIN_PLAIN | PIN_INPUT | PIN_FAST);
+
+ gpio_set(GPIO_LED, 0);
+ gpio_set(GPIO_RESET, 1);
+ gpio_set(GPIO_RESET_TXEN, 0);
+
+ gpio_config(GPIO_LED, GPIO_OUTPUT);
+ gpio_config(GPIO_RESET, GPIO_OUTPUT);
+ gpio_config(GPIO_RESET_TXEN, GPIO_OUTPUT);
+}
+
+
+/* returns 1 if the number of bits set in n is odd */
+static unsigned parity(unsigned n) {
+ n = (n & 0x55555555) + ((n & 0xaaaaaaaa) >> 1);
+ n = (n & 0x33333333) + ((n & 0xcccccccc) >> 2);
+ n = (n & 0x0f0f0f0f) + ((n & 0xf0f0f0f0) >> 4);
+ n = (n & 0x00ff00ff) + ((n & 0xff00ff00) >> 8);
+ n = (n & 0x0000ffff) + ((n & 0xffff0000) >> 16);
+ return n & 1;
+}
+
+#include "fw-m0sub.h"
+
+#define M0SUB_ZEROMAP 0x40043308
+#define M0SUB_TXEV 0x40043314 // write 0 to clear
+#define M4_TXEV 0x40043130 // write 0 to clear
+
+#define RESET_CTRL0 0x40053100
+#define M0_SUB_RST (1 << 12)
+
+#define COMM_CMD 0x18004000
+#define COMM_ARG1 0x18004004
+#define COMM_ARG2 0x18004008
+#define COMM_RESP 0x1800400C
+
+#define M0_CMD_ERR 0
+#define M0_CMD_NOP 1
+#define M0_CMD_READ 2
+#define M0_CMD_WRITE 3
+#define M0_CMD_RESET 4
+#define M0_CMD_SETCLOCK 5
+
+#define RSP_BUSY 0xFFFFFFFF
+
+void swd_init(void) {
+ gpio_init();
+
+ writel(BASE_CLK_SEL(CLK_PLL1), BASE_PERIPH_CLK);
+ spin(1000);
+
+ // SGPIO15 SWDIO_TXEN
+ writel(CFG_OUT_GPIO | CFG_OE_GPIO, SGPIO_OUT_CFG(15));
+ // SGPIO14 SWDIO
+ writel(CFG_OUT_GPIO | CFG_OE_GPIO, SGPIO_OUT_CFG(14));
+ // SGPIO11 SWCLK
+ writel(CFG_OUT_GPIO | CFG_OE_GPIO, SGPIO_OUT_CFG(11));
+
+ // all outputs enabled and high
+ writel((1 << 11) | (1 << 14) | (1 << 15), SGPIO_OUT);
+ writel((1 << 11) | (1 << 14) | (1 << 15), SGPIO_OEN);
+
+ writel(0, M4_TXEV);
+ writel(M0_SUB_RST, RESET_CTRL0);
+ writel(0x18000000, M0SUB_ZEROMAP);
+ writel(0xffffffff, 0x18004000);
+ memcpy((void*) 0x18000000, zero_bin, sizeof(zero_bin));
+ DSB;
+ writel(0, RESET_CTRL0);
+}
+
+int swd_write(unsigned hdr, unsigned data) {
+ unsigned n;
+ unsigned p = parity(data);
+ writel(M0_CMD_WRITE, COMM_CMD);
+ writel((hdr << 8) | (p << 16), COMM_ARG1);
+ writel(data, COMM_ARG2);
+ writel(RSP_BUSY, COMM_RESP);
+ DSB;
+ asm("sev");
+ while ((n = readl(COMM_RESP)) == RSP_BUSY) ;
+ //printf("wr s=%d\n", n);
+ return n;
+}
+
+int swd_read(unsigned hdr, unsigned *val) {
+ unsigned n, data, p;
+ writel(M0_CMD_READ, COMM_CMD);
+ writel(hdr << 8, COMM_ARG1);
+ writel(RSP_BUSY, COMM_RESP);
+ DSB;
+ asm("sev");
+ while ((n = readl(COMM_RESP)) == RSP_BUSY) ;
+ if (n) {
+ return n;
+ }
+ data = readl(COMM_ARG1);
+ p = readl(COMM_ARG2);
+ if (p != parity(data)) {
+ return ERR_PARITY;
+ }
+ //printf("rd s=%d p=%d d=%08x\n", n, p, data);
+ *val = data;
+ return 0;
+}
+
+void swd_reset(void) {
+ unsigned n;
+ writel(M0_CMD_RESET, COMM_CMD);
+ writel(RSP_BUSY, COMM_RESP);
+ DSB;
+ asm("sev");
+ while ((n = readl(COMM_RESP)) == RSP_BUSY) ;
+}
+
+unsigned swd_set_clock(unsigned khz) {
+ unsigned n;
+ if (khz > 8000) {
+ khz = 8000;
+ }
+ writel(M0_CMD_SETCLOCK, COMM_CMD);
+ writel(khz/1000, COMM_ARG1);
+ writel(RSP_BUSY, COMM_RESP);
+ DSB;
+ asm("sev");
+ while ((n = readl(COMM_RESP)) == RSP_BUSY) ;
+
+ // todo: accurate value
+ return khz;
+}
+
+void swd_hw_reset(int assert) {
+ if (assert) {
+ gpio_set(GPIO_RESET, 0);
+ gpio_set(GPIO_RESET_TXEN, 1);
+ } else {
+ gpio_set(GPIO_RESET, 1);
+ gpio_set(GPIO_RESET_TXEN, 0);
+ }
+}
diff --git a/src/bsp/lk/app/mdebug/swd-sgpio.c b/src/bsp/lk/app/mdebug/swd-sgpio.c
new file mode 100644
index 0000000..5221387
--- /dev/null
+++ b/src/bsp/lk/app/mdebug/swd-sgpio.c
@@ -0,0 +1,366 @@
+/* swdp-sgpio.c
+ *
+ * Copyright 2015 Brian Swetland <swetland@frotz.net>
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include <app.h>
+#include <debug.h>
+#include <string.h>
+#include <stdlib.h>
+#include <printf.h>
+#include <dev/udc.h>
+
+#include <platform.h>
+#include <arch/arm.h>
+#include <kernel/thread.h>
+
+#include <platform/lpc43xx-gpio.h>
+#include <platform/lpc43xx-sgpio.h>
+#include <platform/lpc43xx-clocks.h>
+
+#define PIN_LED PIN(1,1)
+#define PIN_RESET PIN(2,5)
+#define PIN_RESET_TXEN PIN(2,6)
+#define PIN_SWDIO_TXEN PIN(1,5) // SGPIO15=6
+#define PIN_SWDIO PIN(1,6) // SGPIO14=6
+#define PIN_SWO PIN(1,14) // U1_RXD=1
+#define PIN_SWCLK PIN(1,17) // SGPIO11=6
+
+#define GPIO_LED GPIO(0,8)
+#define GPIO_RESET GPIO(5,5)
+#define GPIO_RESET_TXEN GPIO(5,6)
+#define GPIO_SWDIO_TXEN GPIO(1,8)
+#define GPIO_SWDIO GPIO(1,9)
+#define GPIO_SWCLK GPIO(0,12)
+
+static unsigned sgpio_div = 31; // 6MHz
+
+static void gpio_init(void) {
+ pin_config(PIN_LED, PIN_MODE(0) | PIN_PLAIN);
+ pin_config(PIN_RESET, PIN_MODE(4) | PIN_PLAIN);
+ pin_config(PIN_RESET_TXEN, PIN_MODE(4) | PIN_PLAIN);
+ pin_config(PIN_SWDIO_TXEN, PIN_MODE(0) | PIN_PLAIN);
+ pin_config(PIN_SWDIO, PIN_MODE(0) | PIN_PLAIN | PIN_INPUT | PIN_FAST);
+ pin_config(PIN_SWCLK, PIN_MODE(0) | PIN_PLAIN | PIN_FAST);
+ pin_config(PIN_SWO, PIN_MODE(1) | PIN_PLAIN | PIN_INPUT | PIN_FAST);
+
+ gpio_set(GPIO_LED, 0);
+ gpio_set(GPIO_RESET, 1);
+ gpio_set(GPIO_RESET_TXEN, 0);
+ gpio_set(GPIO_SWDIO, 0);
+ gpio_set(GPIO_SWDIO_TXEN, 1);
+ gpio_set(GPIO_SWCLK, 0);
+
+ gpio_config(GPIO_LED, GPIO_OUTPUT);
+ gpio_config(GPIO_RESET, GPIO_OUTPUT);
+ gpio_config(GPIO_RESET_TXEN, GPIO_OUTPUT);
+ gpio_config(GPIO_SWDIO, GPIO_OUTPUT);
+ gpio_config(GPIO_SWDIO_TXEN, GPIO_OUTPUT);
+ gpio_config(GPIO_SWCLK, GPIO_OUTPUT);
+}
+
+/* returns 1 if the number of bits set in n is odd */
+static unsigned parity(unsigned n) {
+ n = (n & 0x55555555) + ((n & 0xaaaaaaaa) >> 1);
+ n = (n & 0x33333333) + ((n & 0xcccccccc) >> 2);
+ n = (n & 0x0f0f0f0f) + ((n & 0xf0f0f0f0) >> 4);
+ n = (n & 0x00ff00ff) + ((n & 0xff00ff00) >> 8);
+ n = (n & 0x0000ffff) + ((n & 0xffff0000) >> 16);
+ return n & 1;
+}
+
+static void sgpio_txn(unsigned slices) {
+ // clear any previous status bits
+ writel(0xFFFF, SLICE_XHG_STS_CLR);
+ // kick the txn
+ writel(slices, SLICE_CTRL_ENABLE);
+ writel(slices, SLICE_CTRL_DISABLE);
+ // wait for all slices to complete
+ while ((readl(SLICE_XHG_STS) & slices) != slices) ;
+ // shut down clocks
+ writel(0, SLICE_CTRL_ENABLE);
+ writel(0, SLICE_CTRL_DISABLE);
+}
+
+// SWDIO SLICE_H SLICE_P SLICE_D SLICE_O SWDIO
+// SGPIO14 -> [31....0] -> [31....0] [31....0] -> [31....0] -> SGPIO14
+//
+// SLICE_M SWDIO_TXEN
+// [31....7] -> SGPIO15
+//
+// SLICE_F SWDIO_OE
+// [31....0] -> SGPIO14_OE
+
+
+// configures all slices, muxes, etc
+// ensures that outputs are enabled and SWDIO and SWCLK are high
+static void sgpio_init(void) {
+ writel(BASE_CLK_SEL(CLK_PLL1), BASE_PERIPH_CLK);
+
+ // make sure everything's shut down
+ writel(0, SLICE_CTRL_ENABLE);
+ writel(0, SLICE_CTRL_DISABLE);
+ writel(0xFFFF, SLICE_XHG_STS_CLR);
+
+ // SWDIO_TXEN (SGPIO15)
+ // M[31..7] -> OUT
+ writel(CFG_OUT_M8B | CFG_OE_GPIO, SGPIO_OUT_CFG(15));
+ writel(CLK_USE_SLICE | QUAL_ENABLE | CONCAT_SLICE, SLICE_CFG1(SLC_M));
+ writel(CLK_GEN_INTERNAL | SHIFT_1BPC, SLICE_CFG2(SLC_M));
+
+ // SWDIO (SGPIO14)
+ // IN -> H[31..0] -> P[31..0]
+ // D[31..0] -> O[31..0] -> OUT
+ // F[31..0] -> OE
+ writel(CFG_OUT_M2C | CFG_OE_M1, SGPIO_OUT_CFG(14));
+ writel(CLK_USE_SLICE | QUAL_ENABLE | CONCAT_PIN, SLICE_CFG1(SLC_H));
+ writel(CLK_GEN_INTERNAL | SHIFT_1BPC, SLICE_CFG2(SLC_H));
+ writel(CLK_USE_SLICE | QUAL_ENABLE | CONCAT_SLICE | CONCAT_2_SLICE, SLICE_CFG1(SLC_P));
+ writel(CLK_GEN_INTERNAL | SHIFT_1BPC, SLICE_CFG2(SLC_P));
+ writel(CLK_USE_SLICE | QUAL_ENABLE | CONCAT_SLICE, SLICE_CFG1(SLC_D));
+ writel(CLK_GEN_INTERNAL | SHIFT_1BPC, SLICE_CFG2(SLC_D));
+ writel(CLK_USE_SLICE | QUAL_ENABLE | CONCAT_SLICE | CONCAT_2_SLICE, SLICE_CFG1(SLC_O));
+ writel(CLK_GEN_INTERNAL | SHIFT_1BPC, SLICE_CFG2(SLC_O));
+ writel(CLK_USE_SLICE | QUAL_ENABLE | CONCAT_SLICE, SLICE_CFG1(SLC_F));
+ writel(CLK_GEN_INTERNAL | SHIFT_1BPC, SLICE_CFG2(SLC_F));
+
+ // SWDCLK (SGPIO11)
+ // SLICE_N CLK -> OUT
+ writel(CFG_OUT_CLK | CFG_OE_GPIO, SGPIO_OUT_CFG(11));
+ writel(CLK_USE_SLICE | QUAL_ENABLE, SLICE_CFG1(SLC_N));
+ writel(CLK_GEN_INTERNAL | SHIFT_1BPC | INV_CLK_OUT, SLICE_CFG2(SLC_N));
+
+ // ensure output and enables idle high
+ writel(1, SLICE_REG(SLC_F));
+ writel(1, SLICE_REG(SLC_O));
+ writel(1 << 7, SLICE_REG(SLC_M));
+
+ // enable all outputs
+ writel((1 << 11) | (1 << 14) | (1 << 15), SGPIO_OEN);
+
+ // select SGPIOs via pin mux
+ pin_config(PIN_SWDIO_TXEN, PIN_MODE(6) | PIN_PLAIN | PIN_FAST);
+ pin_config(PIN_SWDIO, PIN_MODE(6) | PIN_PLAIN | PIN_INPUT | PIN_FAST);
+ pin_config(PIN_SWCLK, PIN_MODE(6) | PIN_PLAIN | PIN_FAST);
+}
+
+static void sgpio_swd_clock_setup(unsigned div) {
+ writel(div, SLICE_PRESET(SLC_D));
+ writel(div, SLICE_PRESET(SLC_F));
+ writel(div, SLICE_PRESET(SLC_H));
+ writel(div, SLICE_PRESET(SLC_M));
+ writel(div, SLICE_PRESET(SLC_N));
+ writel(div, SLICE_PRESET(SLC_O));
+ writel(div, SLICE_PRESET(SLC_P));
+}
+
+static void sgpio_swd_reset(unsigned div) {
+ // shift out 64 clocks while DATA is 1
+ writel(0, SLICE_COUNT(SLC_N));
+ writel(POS_POS(63) | POS_RESET(63), SLICE_POS(SLC_N));
+ sgpio_txn(1 << SLC_N);
+
+ // shift out 16bit jtag->swd escape pattern
+ writel(0, SLICE_COUNT(SLC_N));
+ writel(POS_POS(15) | POS_RESET(15), SLICE_POS(SLC_N));
+
+ writel(0b1110011110011110, SLICE_REG(SLC_O));
+ writel(1, SLICE_SHADOW(SLC_O));
+ writel(div, SLICE_COUNT(SLC_O));
+ writel(POS_POS(15) | POS_RESET(15), SLICE_POS(SLC_O));
+ sgpio_txn((1 << SLC_N) | (1 << SLC_O));
+
+ // shift out 64 clocks while DATA is 1
+ writel(0, SLICE_COUNT(SLC_N));
+ writel(POS_POS(63) | POS_RESET(63), SLICE_POS(SLC_N));
+ sgpio_txn(1 << SLC_N);
+};
+
+// shift out 8 0s then the 8bit header, then disable outputs
+// and shift in the turnaround bit (ignored) and 3 bit ack
+// leaves output enables low
+//
+// todo: make leader optional/adjustable
+static int sgpio_swd_header(unsigned div, uint32_t hdr) {
+ unsigned timeout = 16;
+ unsigned ack;
+
+ for (;;) {
+ // 16 bits tx_en, then stop, disabling tx_en
+ writel(0xFFFF << 7, SLICE_REG(SLC_M));
+ writel(0, SLICE_SHADOW(SLC_M));
+ writel(div, SLICE_COUNT(SLC_M));
+ writel(POS_POS(15) | POS_RESET(15), SLICE_POS(SLC_M));
+
+ // 16 bits output, then stop, disabling OE
+ writel(0xFFFF, SLICE_REG(SLC_F));
+ writel(0, SLICE_SHADOW(SLC_F));
+ writel(div, SLICE_COUNT(SLC_F));
+ writel(POS_POS(15) | POS_RESET(15), SLICE_POS(SLC_F));
+
+ // 16 bits data out
+ writel(hdr << 8, SLICE_REG(SLC_O));
+ writel(1, SLICE_SHADOW(SLC_O));
+ writel(div, SLICE_COUNT(SLC_O));
+ writel(POS_POS(15) | POS_RESET(15), SLICE_POS(SLC_O));
+
+ // 20 bits data in
+ writel(0, SLICE_COUNT(SLC_H));
+ writel(POS_POS(19) | POS_RESET(19), SLICE_POS(SLC_H));
+ writel(0, SLICE_REG(SLC_H));
+
+ // 20 bits clock
+ writel(0, SLICE_COUNT(SLC_N));
+ writel(POS_POS(19) | POS_RESET(19), SLICE_POS(SLC_N));
+
+ sgpio_txn((1<<SLC_M)|(1<<SLC_F)|(1<<SLC_O)|(1<<SLC_H)|(1<<SLC_N));
+
+ if ((ack = readl(SLICE_SHADOW(SLC_H)) >> 29) == 1) {
+ // OKAY
+ if (timeout < 16) printf("[%d]\n",16-timeout);
+ return 0;
+ }
+
+ // re-enable oe, tx_en, and make data high
+ writel(1, SLICE_REG(SLC_O));
+ writel(1 << 7, SLICE_REG(SLC_M));
+ writel(1, SLICE_REG(SLC_F));
+
+ // technically we should do a Turn cycle here,
+ // but we rely on the fact that we prefix all ops
+ // with some leader 0s and can be lazy
+
+ if (ack == 2) {
+ // WAIT
+ if (timeout == 0) {
+ return -1;
+ }
+ timeout--;
+ } else {
+ printf("ERR %d\n", ack);
+ // FAULT or invalid response
+ return -1;
+ }
+ }
+}
+
+static int sgpio_swd_read(unsigned div, uint32_t hdr, uint32_t *_data) {
+ uint32_t data, p;
+
+ //printf("rd(%d,%02x)\n", div, hdr);
+ if (sgpio_swd_header(div, hdr)) {
+ return -1;
+ }
+
+ // 34 bits in -> H -> P
+ writel(0, SLICE_COUNT(SLC_H));
+ writel(POS_POS(33) | POS_RESET(33), SLICE_POS(SLC_H));
+ writel(0, SLICE_REG(SLC_H));
+ writel(0, SLICE_COUNT(SLC_P));
+ writel(POS_POS(33) | POS_RESET(33), SLICE_POS(SLC_P));
+ writel(0, SLICE_REG(SLC_P));
+
+ writel(0, SLICE_COUNT(SLC_N));
+ writel(POS_POS(33) | POS_RESET(33), SLICE_POS(SLC_N));
+ sgpio_txn((1<<SLC_H)|(1<<SLC_P)|(1<<SLC_N));
+
+ // re-enable oe, tx_en, and make data high
+ writel(1, SLICE_REG(SLC_O));
+ writel(1 << 7, SLICE_REG(SLC_M));
+ writel(1, SLICE_REG(SLC_F));
+
+ p = readl(SLICE_SHADOW(SLC_H));
+ data = (p << 2) | (readl(SLICE_SHADOW(SLC_P)) >> 30);
+ p = (p >> 30) & 1;
+
+ //printf("RD %08x %d\n", data, p);
+ if (parity(data) != p) {
+ printf("parity error\n");
+ return -1;
+ }
+ *_data = data;
+ return 0;
+}
+
+static int sgpio_swd_write(unsigned div, uint32_t hdr, uint32_t data) {
+ uint32_t p = parity(data);
+
+ //printf("wr(%d,%02x,%08x) p=%d\n", div, hdr, data, p);
+ if (sgpio_swd_header(div, hdr)) {
+ return -1;
+ }
+
+ // 34 bits D -> O -> out
+ writel(div, SLICE_COUNT(SLC_D));
+ writel(POS_POS(33) | POS_RESET(33), SLICE_POS(SLC_D));
+ writel((p << 1) | (data >> 31), SLICE_REG(SLC_D));
+ writel(div, SLICE_COUNT(SLC_O));
+ writel(POS_POS(33) | POS_RESET(33), SLICE_POS(SLC_O));
+ writel((data << 1) | 1, SLICE_REG(SLC_O));
+
+ writel(0, SLICE_COUNT(SLC_N));
+ writel(POS_POS(33) | POS_RESET(33), SLICE_POS(SLC_N));
+
+ // re-enable oe, tx_en
+ writel(1 << 7, SLICE_REG(SLC_M));
+ writel(1, SLICE_REG(SLC_F));
+
+ sgpio_txn((1<<SLC_D)|(1<<SLC_O)|(1<<SLC_N));
+
+ return 0;
+}
+
+void swd_init(void) {
+ gpio_init();
+ sgpio_init();
+}
+
+void swd_reset(void) {
+ unsigned div = sgpio_div;
+ sgpio_swd_clock_setup(div);
+ sgpio_swd_reset(div);
+}
+
+int swd_read(unsigned reg, unsigned *val) {
+ unsigned div = sgpio_div;
+ sgpio_swd_clock_setup(div);
+ return sgpio_swd_read(div, reg, val);
+}
+
+int swd_write(unsigned reg, unsigned val) {
+ unsigned div = sgpio_div;
+ sgpio_swd_clock_setup(div);
+ return sgpio_swd_write(div, reg, val);
+}
+
+unsigned swd_set_clock(unsigned khz) {
+ unsigned div;
+ if (khz < 2000) khz = 2000;
+ if (khz > 48000) khz = 48000;
+ div = 192000 / khz;
+ sgpio_div = div - 1;
+ return 192000 / div;
+}
+
+void swd_hw_reset(int assert) {
+ if (assert) {
+ gpio_set(GPIO_RESET, 0);
+ gpio_set(GPIO_RESET_TXEN, 1);
+ } else {
+ gpio_set(GPIO_RESET, 1);
+ gpio_set(GPIO_RESET_TXEN, 0);
+ }
+}
+
diff --git a/src/bsp/lk/app/mdebug/swd.h b/src/bsp/lk/app/mdebug/swd.h
new file mode 100644
index 0000000..2822406
--- /dev/null
+++ b/src/bsp/lk/app/mdebug/swd.h
@@ -0,0 +1,58 @@
+/* swd.h
+ *
+ * Copyright 2011 Brian Swetland <swetland@frotz.net>
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef _SWDP_H_
+#define _SWDP_H_
+
+void swd_init(void);
+void swd_reset(void);
+int swd_write(unsigned reg, unsigned val);
+int swd_read(unsigned reg, unsigned *val);
+
+unsigned swd_set_clock(unsigned khz);
+unsigned swo_set_clock(unsigned khz);
+void swd_hw_reset(int assert);
+
+void jtag_init(void);
+int jtag_io(unsigned count, unsigned tms, unsigned tdi, unsigned *tdo);
+
+// swdp_read/write() register codes
+
+// Park Stop Parity Addr3 Addr2 RnW APnDP Start
+
+#define RD_IDCODE 0b10100101
+#define RD_DPCTRL 0b10001101
+#define RD_RESEND 0b10010101
+#define RD_BUFFER 0b10111101
+
+#define WR_ABORT 0b10000001
+#define WR_DPCTRL 0b10101001
+#define WR_SELECT 0b10110001
+#define WR_BUFFER 0b10011001
+
+#define RD_AP0 0b10000111
+#define RD_AP1 0b10101111
+#define RD_AP2 0b10110111
+#define RD_AP3 0b10011111
+
+#define WR_AP0 0b10100011
+#define WR_AP1 0b10001011
+#define WR_AP2 0b10010011
+#define WR_AP3 0b10111011
+
+#endif
+
diff --git a/src/bsp/lk/app/mdebug/swo-uart1.c b/src/bsp/lk/app/mdebug/swo-uart1.c
new file mode 100644
index 0000000..7803a1d
--- /dev/null
+++ b/src/bsp/lk/app/mdebug/swo-uart1.c
@@ -0,0 +1,151 @@
+/* swo-uart1.c
+ *
+ * Copyright 2015 Brian Swetland <swetland@frotz.net>
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include <string.h>
+#include <debug.h>
+#include <reg.h>
+#include <kernel/thread.h>
+
+#include <dev/udc.h>
+#include <arch/arm/cm.h>
+
+#include <platform/lpc43xx-uart.h>
+#include <platform/lpc43xx-gpdma.h>
+#include <platform/lpc43xx-clocks.h>
+
+#include "rswdp.h"
+
+#define UART_BASE UART1_BASE
+#define BASE_UART_CLK BASE_UART1_CLK
+
+extern uint8_t __lpc43xx_main_clock_sel;
+extern uint32_t __lpc43xx_main_clock_mhz;
+
+#define TXNSIZE 128
+#define TXNCOUNT 4
+
+typedef struct swo_txn {
+ unsigned buf[TXNSIZE/4 + 2];
+ struct swo_txn *next;
+ udc_request_t *req;
+ unsigned busy;
+ unsigned num;
+} txn_t;
+
+static txn_t TXN[TXNCOUNT];
+static txn_t *txwr = TXN;
+static udc_endpoint_t *txept;
+
+void swo_start_dma(void *ptr);
+
+static void tx_done(udc_request_t *req, unsigned actual, int status) {
+ txn_t *txn = req->context;
+ txn->busy = 0;
+ if (txwr == txn) {
+ // writer wants to write here, and is waiting...
+ swo_start_dma(txn->buf + 2);
+ }
+}
+
+void lpc43xx_DMA_IRQ(void) {
+ txn_t *txn = txwr;
+ arm_cm_irq_entry();
+ writel(0xFF, DMA_INTTCCLR);
+ writel(0xFF, DMA_INTERRCLR);
+ if (udc_request_queue(txept, txn->req)) {
+ // failed, usb probably offline, just re-use the buffer
+ } else {
+ txn->busy = 1;
+ txwr = txn = txn->next;
+ }
+ if (!txn->busy) {
+ // if busy, when the usb txn completes, it will start dma then
+ swo_start_dma(txn->buf + 2);
+ }
+ arm_cm_irq_exit(0);
+}
+
+void swo_init(udc_endpoint_t *_txept) {
+ int n;
+ txept = _txept;
+ for (n = 0; n < TXNCOUNT; n++) {
+ TXN[n].req = udc_request_alloc();
+ TXN[n].req->context = TXN + n;
+ TXN[n].req->buffer = TXN[n].buf;
+ TXN[n].req->length = TXNSIZE + 8;
+ TXN[n].req->complete = tx_done;
+ TXN[n].num = n;
+ TXN[n].busy = 0;
+ TXN[n].next = TXN + (n + 1);
+ TXN[n].buf[0] = RSWD_TXN_ASYNC;
+ TXN[n].buf[1] = RSWD_MSG(CMD_SWO_DATA, 0, TXNSIZE);
+ }
+ TXN[n-1].next = TXN;
+
+ // configure peripheral 4 as uart1_rx
+ writel((readl(DMAMUX_REG) & DMAMUX_M(4)) | DMAMUX_P(4, P4_UART1_RX), DMAMUX_REG);
+ writel(DMA_CONFIG_EN, DMA_CONFIG);
+ NVIC_EnableIRQ(DMA_IRQn);
+
+ // kick off the process with an initial DMA
+ swo_start_dma(txwr->buf + 2);
+}
+
+void swo_start_dma(void *ptr) {
+ writel(UART1_BASE + REG_RBR, DMA_SRC(0));
+ writel((u32) ptr, DMA_DST(0));
+ writel(0, DMA_LLI(0));
+ writel(DMA_XFER_SIZE(TXNSIZE) |
+ DMA_SRC_BURST(BURST_1) | DMA_DST_BURST(BURST_4) |
+ DMA_SRC_BYTE | DMA_DST_WORD | DMA_SRC_MASTER1 | DMA_DST_MASTER0 |
+ DMA_DST_INCR | DMA_PROT1 | DMA_TC_IE,
+ DMA_CTL(0));
+ writel(DMA_ENABLE | DMA_SRC_PERIPH(4) | DMA_FLOW_P2M_DMAc | DMA_TC_IRQ_EN,
+ DMA_CFG(0));
+}
+void swo_config(unsigned mhz) {
+ if (mhz > 0) {
+ uint32_t div = __lpc43xx_main_clock_mhz / 16 / mhz;
+ writel(BASE_CLK_SEL(__lpc43xx_main_clock_sel), BASE_UART_CLK);
+ writel(LCR_DLAB, UART_BASE + REG_LCR);
+ writel(div & 0xFF, UART_BASE + REG_DLL);
+ writel((div >> 8) & 0xFF, UART_BASE + REG_DLM);
+ writel(LCR_WLS_8 | LCR_SBS_1, UART_BASE + REG_LCR);
+ writel(FCR_FIFOEN | FCR_RX_TRIG_1 | FCR_DMAMODE, UART_BASE + REG_FCR);
+ }
+}
+
+unsigned swo_set_clock(unsigned khz) {
+ if (khz >= 12000) {
+ khz = 12000;
+ } else if (khz >= 8000) {
+ khz = 8000;
+ } else if (khz >= 6000) {
+ khz = 6000;
+ } else if (khz >= 4000) {
+ khz = 4000;
+ } else if (khz >= 3000) {
+ khz = 3000;
+ } else if (khz >= 2000) {
+ khz = 2000;
+ } else {
+ khz = 1000;
+ }
+ swo_config(khz * 1000);
+ return khz;
+}
+
diff --git a/src/bsp/lk/app/pcitests/pci_tests.c b/src/bsp/lk/app/pcitests/pci_tests.c
new file mode 100644
index 0000000..9b33043
--- /dev/null
+++ b/src/bsp/lk/app/pcitests/pci_tests.c
@@ -0,0 +1,260 @@
+/*
+ * Copyright (c) 2009 Corey Tabaka
+ *
+ * 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 <app.h>
+#include <debug.h>
+#include <stdint.h>
+#include <string.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <compiler.h>
+#include <platform.h>
+#include <dev/pci.h>
+
+#if defined(WITH_LIB_CONSOLE)
+#include <lib/console.h>
+
+/*
+ * enumerates pci devices
+ */
+static void pci_list(void)
+{
+ pci_location_t state;
+ uint16_t device_id, vendor_id;
+ uint8_t header_type;
+ uint8_t base_class, sub_class, interface;
+ int busses = 0, devices = 0, lines = 0, devfn, ret;
+ int c;
+
+ printf("Scanning...\n");
+
+ for (state.bus = 0; state.bus <= pci_get_last_bus(); state.bus++) {
+ busses++;
+
+ for (devfn = 0; devfn < 256; devfn++) {
+ state.dev_fn = devfn;
+
+ ret = pci_read_config_half(&state, PCI_CONFIG_VENDOR_ID, &vendor_id);
+ if (ret != _PCI_SUCCESSFUL) goto error;
+
+ ret = pci_read_config_half(&state, PCI_CONFIG_DEVICE_ID, &device_id);
+ if (ret != _PCI_SUCCESSFUL) goto error;
+
+ ret = pci_read_config_byte(&state, PCI_CONFIG_HEADER_TYPE, &header_type);
+ if (ret != _PCI_SUCCESSFUL) goto error;
+
+ ret = pci_read_config_byte(&state, PCI_CONFIG_CLASS_CODE_BASE, &base_class);
+ if (ret != _PCI_SUCCESSFUL) goto error;
+
+ ret = pci_read_config_byte(&state, PCI_CONFIG_CLASS_CODE_SUB, &sub_class);
+ if (ret != _PCI_SUCCESSFUL) goto error;
+
+ ret = pci_read_config_byte(&state, PCI_CONFIG_CLASS_CODE_INTR, &interface);
+ if (ret != _PCI_SUCCESSFUL) goto error;
+
+ if (vendor_id != 0xffff) {
+ printf("%02x:%02x vendor_id=%04x device_id=%04x, header_type=%02x "
+ "base_class=%02x, sub_class=%02x, interface=%02x\n", state.bus, state.dev_fn,
+ vendor_id, device_id, header_type, base_class, sub_class, interface);
+ devices++;
+ lines++;
+ }
+
+ if (~header_type & PCI_HEADER_TYPE_MULTI_FN) {
+ // this is not a multi-function device, so advance to the next device
+ devfn |= 7;
+ }
+
+ if (lines == 23) {
+ printf("... press any key to continue, q to quit ...");
+ while ((c = getchar()) < 0);
+ printf("\n");
+ lines = 0;
+
+ if (c == 'q' || c == 'Q') goto quit;
+ }
+ }
+ }
+
+ printf("... done. Scanned %d busses, %d device/functions\n", busses, devices);
+quit:
+ return;
+
+error:
+ printf("Error while reading PCI config space: %02x\n", ret);
+}
+
+/*
+ * a somewhat fugly pci config space examine/modify command. this should probably
+ * be broken up a bit.
+ */
+static int pci_config(int argc, const cmd_args *argv)
+{
+ pci_location_t loc;
+ pci_config_t config;
+ uint32_t offset;
+ unsigned int i;
+ int ret;
+
+ if (argc < 5) {
+ return -1;
+ }
+
+ if (!strcmp(argv[2].str, "dump")) {
+ loc.bus = atoui(argv[3].str);
+ loc.dev_fn = atoui(argv[4].str);
+
+ for (i=0; i < sizeof(pci_config_t); i++) {
+ ret = pci_read_config_byte(&loc, i, (uint8_t *) &config + i);
+ if (ret != _PCI_SUCCESSFUL) goto error;
+ }
+
+ printf("Device at %02x:%02x vendor id=%04x device id=%04x\n", loc.bus,
+ loc.dev_fn, config.vendor_id, config.device_id);
+ printf("command=%04x status=%04x pi=%02x sub cls=%02x base cls=%02x\n",
+ config.command, config.status, config.program_interface,
+ config.sub_class, config.base_class);
+
+ for (i=0; i < 6; i+=2) {
+ printf("bar%d=%08x bar%d=%08x\n", i, config.base_addresses[i],
+ i+1, config.base_addresses[i+1]);
+ }
+ } else if (!strcmp(argv[2].str, "rb") || !strcmp(argv[2].str, "rh") || !strcmp(argv[2].str, "rw")) {
+ if (argc != 6) {
+ return -1;
+ }
+
+ loc.bus = atoui(argv[3].str);
+ loc.dev_fn = atoui(argv[4].str);
+ offset = atoui(argv[5].str);
+
+ switch (argv[2].str[1]) {
+ case 'b': {
+ uint8_t value;
+ ret = pci_read_config_byte(&loc, offset, &value);
+ if (ret != _PCI_SUCCESSFUL) goto error;
+
+ printf("byte at device %02x:%02x config offset %04x: %02x\n", loc.bus, loc.dev_fn, offset, value);
+ }
+ break;
+
+ case 'h': {
+ uint16_t value;
+ ret = pci_read_config_half(&loc, offset, &value);
+ if (ret != _PCI_SUCCESSFUL) goto error;
+
+ printf("half at device %02x:%02x config offset %04x: %04x\n", loc.bus, loc.dev_fn, offset, value);
+ }
+ break;
+
+ case 'w': {
+ uint32_t value;
+ ret = pci_read_config_word(&loc, offset, &value);
+ if (ret != _PCI_SUCCESSFUL) goto error;
+
+ printf("word at device %02x:%02x config offset %04x: %08x\n", loc.bus, loc.dev_fn, offset, value);
+ }
+ break;
+ }
+ } else if (!strcmp(argv[2].str, "mb") || !strcmp(argv[2].str, "mh") || !strcmp(argv[2].str, "mw")) {
+ if (argc != 7) {
+ return -1;
+ }
+
+ loc.bus = atoui(argv[3].str);
+ loc.dev_fn = atoui(argv[4].str);
+ offset = atoui(argv[5].str);
+
+ switch (argv[2].str[1]) {
+ case 'b': {
+ uint8_t value = atoui(argv[6].str);
+ ret = pci_write_config_byte(&loc, offset, value);
+ if (ret != _PCI_SUCCESSFUL) goto error;
+
+ printf("byte to device %02x:%02x config offset %04x: %02x\n", loc.bus, loc.dev_fn, offset, value);
+ }
+ break;
+
+ case 'h': {
+ uint16_t value = atoui(argv[6].str);
+ ret = pci_write_config_half(&loc, offset, value);
+ if (ret != _PCI_SUCCESSFUL) goto error;
+
+ printf("half to device %02x:%02x config offset %04x: %04x\n", loc.bus, loc.dev_fn, offset, value);
+ }
+ break;
+
+ case 'w': {
+ uint32_t value = atoui(argv[6].str);
+ ret = pci_write_config_word(&loc, offset, value);
+ if (ret != _PCI_SUCCESSFUL) goto error;
+
+ printf("word to device %02x:%02x config offset %04x: %08x\n", loc.bus, loc.dev_fn, offset, value);
+ }
+ break;
+ }
+ } else {
+ return -1;
+ }
+
+ return 0;
+
+error:
+ printf("Error while reading PCI config space: %02x\n", ret);
+ return -2;
+}
+
+static int pci_cmd(int argc, const cmd_args *argv)
+{
+ if (argc < 2) {
+ printf("pci commands:\n");
+usage:
+ printf("%s list\n", argv[0].str);
+ printf("%s config dump <bus> <devfn>\n", argv[0].str);
+ printf("%s config <rb|rh|rw> <bus> <devfn> <offset>\n", argv[0].str);
+ printf("%s config <mb|mh|mw> <bus> <devfn> <offset> <value>\n", argv[0].str);
+ goto out;
+ }
+
+ if (!strcmp(argv[1].str, "list")) {
+ pci_list();
+ } else if (!strcmp(argv[1].str, "config")) {
+ if (pci_config(argc, argv)) {
+ goto usage;
+ }
+ } else {
+ goto usage;
+ }
+
+out:
+ return 0;
+}
+
+STATIC_COMMAND_START
+STATIC_COMMAND("pci", "pci toolbox", &pci_cmd)
+STATIC_COMMAND_END(pcitests);
+
+#endif
+
+APP_START(pcitests)
+APP_END
+
diff --git a/src/bsp/lk/app/pcitests/rules.mk b/src/bsp/lk/app/pcitests/rules.mk
new file mode 100644
index 0000000..5d27fcf
--- /dev/null
+++ b/src/bsp/lk/app/pcitests/rules.mk
@@ -0,0 +1,8 @@
+LOCAL_DIR := $(GET_LOCAL_DIR)
+
+MODULE := $(LOCAL_DIR)
+
+MODULE_SRCS += \
+ $(LOCAL_DIR)/pci_tests.c
+
+include make/module.mk
diff --git a/src/bsp/lk/app/rules.mk b/src/bsp/lk/app/rules.mk
new file mode 100644
index 0000000..8e24edf
--- /dev/null
+++ b/src/bsp/lk/app/rules.mk
@@ -0,0 +1,10 @@
+LOCAL_DIR := $(GET_LOCAL_DIR)
+
+MODULE := $(LOCAL_DIR)
+
+MODULE_SRCS += \
+ $(LOCAL_DIR)/app.c
+
+EXTRA_LINKER_SCRIPTS += $(LOCAL_DIR)/app.ld
+
+include make/module.mk
diff --git a/src/bsp/lk/app/shell/rules.mk b/src/bsp/lk/app/shell/rules.mk
new file mode 100644
index 0000000..5ae267f
--- /dev/null
+++ b/src/bsp/lk/app/shell/rules.mk
@@ -0,0 +1,11 @@
+LOCAL_DIR := $(GET_LOCAL_DIR)
+
+MODULE := $(LOCAL_DIR)
+
+MODULE_DEPS += \
+ lib/console
+
+MODULE_SRCS += \
+ $(LOCAL_DIR)/shell.c
+
+include make/module.mk
diff --git a/src/bsp/lk/app/shell/shell.c b/src/bsp/lk/app/shell/shell.c
new file mode 100644
index 0000000..bdf67c5
--- /dev/null
+++ b/src/bsp/lk/app/shell/shell.c
@@ -0,0 +1,41 @@
+/*
+ * Copyright (c) 2009 Travis Geiselbrecht
+ *
+ * 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 <app.h>
+#include <debug.h>
+#include <lib/console.h>
+
+static void shell_init(const struct app_descriptor *app)
+{
+ console_init();
+}
+
+static void shell_entry(const struct app_descriptor *app, void *args)
+{
+ console_start();
+}
+
+APP_START(shell)
+ .init = shell_init,
+ .entry = shell_entry,
+APP_END
+
diff --git a/src/bsp/lk/app/stringtests/rules.mk b/src/bsp/lk/app/stringtests/rules.mk
new file mode 100644
index 0000000..9497813
--- /dev/null
+++ b/src/bsp/lk/app/stringtests/rules.mk
@@ -0,0 +1,10 @@
+LOCAL_DIR := $(GET_LOCAL_DIR)
+
+MODULE := $(LOCAL_DIR)
+
+MODULE_SRCS += \
+ $(LOCAL_DIR)/string_tests.c \
+
+# put arch local .S files here if developing memcpy/memmove
+
+include make/module.mk
diff --git a/src/bsp/lk/app/stringtests/string_tests.c b/src/bsp/lk/app/stringtests/string_tests.c
new file mode 100644
index 0000000..5aeb8ca
--- /dev/null
+++ b/src/bsp/lk/app/stringtests/string_tests.c
@@ -0,0 +1,360 @@
+/*
+ * Copyright (c) 2008-2014 Travis Geiselbrecht
+ *
+ * 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 <stdio.h>
+#include <string.h>
+#include <malloc.h>
+#include <app.h>
+#include <platform.h>
+#include <kernel/thread.h>
+
+static uint8_t *src;
+static uint8_t *dst;
+
+static uint8_t *src2;
+static uint8_t *dst2;
+
+#define BUFFER_SIZE (2*1024*1024)
+#define ITERATIONS (256*1024*1024 / BUFFER_SIZE) // enough iterations to have to copy/set 256MB of memory
+
+#if 1
+static inline void *mymemcpy(void *dst, const void *src, size_t len) { return memcpy(dst, src, len); }
+static inline void *mymemset(void *dst, int c, size_t len) { return memset(dst, c, len); }
+#else
+// if we're testing our own memcpy, use this
+extern void *mymemcpy(void *dst, const void *src, size_t len);
+extern void *mymemset(void *dst, int c, size_t len);
+#endif
+
+/* reference implementations of memmove/memcpy */
+typedef long word;
+
+#define lsize sizeof(word)
+#define lmask (lsize - 1)
+
+static void *c_memmove(void *dest, void const *src, size_t count)
+{
+ char *d = (char *)dest;
+ const char *s = (const char *)src;
+ int len;
+
+ if (count == 0 || dest == src)
+ return dest;
+
+ if ((long)d < (long)s) {
+ if (((long)d | (long)s) & lmask) {
+ // src and/or dest do not align on word boundary
+ if ((((long)d ^ (long)s) & lmask) || (count < lsize))
+ len = count; // copy the rest of the buffer with the byte mover
+ else
+ len = lsize - ((long)d & lmask); // move the ptrs up to a word boundary
+
+ count -= len;
+ for (; len > 0; len--)
+ *d++ = *s++;
+ }
+ for (len = count / lsize; len > 0; len--) {
+ *(word *)d = *(word *)s;
+ d += lsize;
+ s += lsize;
+ }
+ for (len = count & lmask; len > 0; len--)
+ *d++ = *s++;
+ } else {
+ d += count;
+ s += count;
+ if (((long)d | (long)s) & lmask) {
+ // src and/or dest do not align on word boundary
+ if ((((long)d ^ (long)s) & lmask) || (count <= lsize))
+ len = count;
+ else
+ len = ((long)d & lmask);
+
+ count -= len;
+ for (; len > 0; len--)
+ *--d = *--s;
+ }
+ for (len = count / lsize; len > 0; len--) {
+ d -= lsize;
+ s -= lsize;
+ *(word *)d = *(word *)s;
+ }
+ for (len = count & lmask; len > 0; len--)
+ *--d = *--s;
+ }
+
+ return dest;
+}
+
+static void *c_memset(void *s, int c, size_t count)
+{
+ char *xs = (char *) s;
+ size_t len = (-(size_t)s) & lmask;
+ word cc = c & 0xff;
+
+ if ( count > len ) {
+ count -= len;
+ cc |= cc << 8;
+ cc |= cc << 16;
+ if (sizeof(word) == 8)
+ cc |= (uint64_t)cc << 32; // should be optimized out on 32 bit machines
+
+ // write to non-aligned memory byte-wise
+ for ( ; len > 0; len-- )
+ *xs++ = c;
+
+ // write to aligned memory dword-wise
+ for ( len = count / lsize; len > 0; len-- ) {
+ *((word *)xs) = (word)cc;
+ xs += lsize;
+ }
+
+ count &= lmask;
+ }
+
+ // write remaining bytes
+ for ( ; count > 0; count-- )
+ *xs++ = c;
+
+ return s;
+}
+
+static void *null_memcpy(void *dst, const void *src, size_t len)
+{
+ return dst;
+}
+
+static lk_time_t bench_memcpy_routine(void *memcpy_routine(void *, const void *, size_t), size_t srcalign, size_t dstalign)
+{
+ int i;
+ lk_time_t t0;
+
+ t0 = current_time();
+ for (i=0; i < ITERATIONS; i++) {
+ memcpy_routine(dst + dstalign, src + srcalign, BUFFER_SIZE);
+ }
+ return current_time() - t0;
+}
+
+static void bench_memcpy(void)
+{
+ lk_time_t null, c, libc, mine;
+ size_t srcalign, dstalign;
+
+ printf("memcpy speed test\n");
+ thread_sleep(200); // let the debug string clear the serial port
+
+ for (srcalign = 0; srcalign < 64; ) {
+ for (dstalign = 0; dstalign < 64; ) {
+
+ null = bench_memcpy_routine(&null_memcpy, srcalign, dstalign);
+ c = bench_memcpy_routine(&c_memmove, srcalign, dstalign);
+ libc = bench_memcpy_routine(&memcpy, srcalign, dstalign);
+ mine = bench_memcpy_routine(&mymemcpy, srcalign, dstalign);
+
+ printf("srcalign %zu, dstalign %zu: ", srcalign, dstalign);
+ printf(" null memcpy %u msecs\n", null);
+ printf("c memcpy %u msecs, %llu bytes/sec; ", c, (uint64_t)BUFFER_SIZE * ITERATIONS * 1000ULL / c);
+ printf("libc memcpy %u msecs, %llu bytes/sec; ", libc, (uint64_t)BUFFER_SIZE * ITERATIONS * 1000ULL / libc);
+ printf("my memcpy %u msecs, %llu bytes/sec; ", mine, (uint64_t)BUFFER_SIZE * ITERATIONS * 1000ULL / mine);
+ printf("\n");
+
+ if (dstalign < 8)
+ dstalign++;
+ else
+ dstalign <<= 1;
+ }
+ if (srcalign < 8)
+ srcalign++;
+ else
+ srcalign <<= 1;
+ }
+}
+
+static void fillbuf(void *ptr, size_t len, uint32_t seed)
+{
+ size_t i;
+
+ for (i = 0; i < len; i++) {
+ ((char *)ptr)[i] = seed;
+ seed *= 0x1234567;
+ }
+}
+
+static void validate_memcpy(void)
+{
+ size_t srcalign, dstalign, size;
+ const size_t maxsize = 256;
+
+ printf("testing memcpy for correctness\n");
+
+ /*
+ * do the simple tests to make sure that memcpy doesn't color outside
+ * the lines for all alignment cases
+ */
+ for (srcalign = 0; srcalign < 64; srcalign++) {
+ printf("srcalign %zu\n", srcalign);
+ for (dstalign = 0; dstalign < 64; dstalign++) {
+ //printf("\tdstalign %zu\n", dstalign);
+ for (size = 0; size < maxsize; size++) {
+
+ //printf("srcalign %zu, dstalign %zu, size %zu\n", srcalign, dstalign, size);
+
+ fillbuf(src, maxsize * 2, 567);
+ fillbuf(src2, maxsize * 2, 567);
+ fillbuf(dst, maxsize * 2, 123514);
+ fillbuf(dst2, maxsize * 2, 123514);
+
+ c_memmove(dst + dstalign, src + srcalign, size);
+ memcpy(dst2 + dstalign, src2 + srcalign, size);
+
+ int comp = memcmp(dst, dst2, maxsize * 2);
+ if (comp != 0) {
+ printf("error! srcalign %zu, dstalign %zu, size %zu\n", srcalign, dstalign, size);
+ }
+ }
+ }
+ }
+}
+
+static lk_time_t bench_memset_routine(void *memset_routine(void *, int, size_t), size_t dstalign, size_t len)
+{
+ int i;
+ lk_time_t t0;
+
+ t0 = current_time();
+ for (i=0; i < ITERATIONS; i++) {
+ memset_routine(dst + dstalign, 0, len);
+ }
+ return current_time() - t0;
+}
+
+static void bench_memset(void)
+{
+ lk_time_t c, libc, mine;
+ size_t dstalign;
+
+ printf("memset speed test\n");
+ thread_sleep(200); // let the debug string clear the serial port
+
+ for (dstalign = 0; dstalign < 64; dstalign++) {
+
+ c = bench_memset_routine(&c_memset, dstalign, BUFFER_SIZE);
+ libc = bench_memset_routine(&memset, dstalign, BUFFER_SIZE);
+ mine = bench_memset_routine(&mymemset, dstalign, BUFFER_SIZE);
+
+ printf("dstalign %zu: ", dstalign);
+ printf("c memset %u msecs, %llu bytes/sec; ", c, (uint64_t)BUFFER_SIZE * ITERATIONS * 1000ULL / c);
+ printf("libc memset %u msecs, %llu bytes/sec; ", libc, (uint64_t)BUFFER_SIZE * ITERATIONS * 1000ULL / libc);
+ printf("my memset %u msecs, %llu bytes/sec; ", mine, (uint64_t)BUFFER_SIZE * ITERATIONS * 1000ULL / mine);
+ printf("\n");
+ }
+}
+
+static void validate_memset(void)
+{
+ size_t dstalign, size;
+ int c;
+ const size_t maxsize = 256;
+
+ printf("testing memset for correctness\n");
+
+ for (dstalign = 0; dstalign < 64; dstalign++) {
+ printf("align %zd\n", dstalign);
+ for (size = 0; size < maxsize; size++) {
+ for (c = -1; c < 257; c++) {
+
+ fillbuf(dst, maxsize * 2, 123514);
+ fillbuf(dst2, maxsize * 2, 123514);
+
+ c_memset(dst + dstalign, c, size);
+ memset(dst2 + dstalign, c, size);
+
+ int comp = memcmp(dst, dst2, maxsize * 2);
+ if (comp != 0) {
+ printf("error! align %zu, c 0x%hhx, size %zu\n", dstalign, c, size);
+ }
+ }
+ }
+ }
+}
+
+#if defined(WITH_LIB_CONSOLE)
+#include <lib/console.h>
+
+static int string_tests(int argc, const cmd_args *argv)
+{
+ src = memalign(64, BUFFER_SIZE + 256);
+ dst = memalign(64, BUFFER_SIZE + 256);
+ src2 = memalign(64, BUFFER_SIZE + 256);
+ dst2 = memalign(64, BUFFER_SIZE + 256);
+
+ printf("src %p, dst %p\n", src, dst);
+ printf("src2 %p, dst2 %p\n", src2, dst2);
+
+ if (!src || !dst || !src2 || !dst2) {
+ printf("failed to allocate all the buffers\n");
+ goto out;
+ }
+
+ if (argc < 3) {
+ printf("not enough arguments:\n");
+usage:
+ printf("%s validate <routine>\n", argv[0].str);
+ printf("%s bench <routine>\n", argv[0].str);
+ goto out;
+ }
+
+ if (!strcmp(argv[1].str, "validate")) {
+ if (!strcmp(argv[2].str, "memcpy")) {
+ validate_memcpy();
+ } else if (!strcmp(argv[2].str, "memset")) {
+ validate_memset();
+ }
+ } else if (!strcmp(argv[1].str, "bench")) {
+ if (!strcmp(argv[2].str, "memcpy")) {
+ bench_memcpy();
+ } else if (!strcmp(argv[2].str, "memset")) {
+ bench_memset();
+ }
+ } else {
+ goto usage;
+ }
+
+out:
+ free(src);
+ free(dst);
+ free(src2);
+ free(dst2);
+
+ return 0;
+}
+
+STATIC_COMMAND_START
+STATIC_COMMAND("string", "memcpy tests", &string_tests)
+STATIC_COMMAND_END(stringtests);
+
+#endif
+
+APP_START(stringtests)
+APP_END
+
diff --git a/src/bsp/lk/app/tests/benchmarks.c b/src/bsp/lk/app/tests/benchmarks.c
new file mode 100644
index 0000000..30de163
--- /dev/null
+++ b/src/bsp/lk/app/tests/benchmarks.c
@@ -0,0 +1,251 @@
+/*
+ * Copyright (c) 2008-2012 Travis Geiselbrecht
+ *
+ * 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 <sys/types.h>
+#include <stdio.h>
+#include <rand.h>
+#include <err.h>
+#include <stdlib.h>
+#include <string.h>
+#include <app/tests.h>
+#include <kernel/thread.h>
+#include <kernel/mutex.h>
+#include <kernel/semaphore.h>
+#include <kernel/event.h>
+#include <platform.h>
+
+const size_t BUFSIZE = (1024*1024);
+const uint ITER = 1024;
+
+__NO_INLINE static void bench_set_overhead(void)
+{
+ uint32_t *buf = malloc(BUFSIZE);
+
+ uint count = arch_cycle_count();
+ for (uint i = 0; i < ITER; i++) {
+ __asm__ volatile("");
+ }
+ count = arch_cycle_count() - count;
+
+ printf("took %u cycles overhead to loop %u times\n",
+ count, ITER);
+
+ free(buf);
+}
+
+__NO_INLINE static void bench_memset(void)
+{
+ void *buf = malloc(BUFSIZE);
+
+ uint count = arch_cycle_count();
+ for (uint i = 0; i < ITER; i++) {
+ memset(buf, 0, BUFSIZE);
+ }
+ count = arch_cycle_count() - count;
+
+ printf("took %u cycles to memset a buffer of size %u %d times (%u bytes), %f bytes/cycle\n",
+ count, BUFSIZE, ITER, BUFSIZE * ITER, (BUFSIZE * ITER) / (float)count);
+
+ free(buf);
+}
+
+#define bench_cset(type) \
+__NO_INLINE static void bench_cset_##type(void) \
+{ \
+ type *buf = malloc(BUFSIZE); \
+ \
+ uint count = arch_cycle_count(); \
+ for (uint i = 0; i < ITER; i++) { \
+ for (uint j = 0; j < BUFSIZE / sizeof(*buf); j++) { \
+ buf[j] = 0; \
+ } \
+ } \
+ count = arch_cycle_count() - count; \
+ \
+ printf("took %u cycles to manually clear a buffer using wordsize %d of size %u %d times (%u bytes), %f bytes/cycle\n", \
+ count, sizeof(*buf), BUFSIZE, ITER, BUFSIZE * ITER, (BUFSIZE * ITER) / (float)count); \
+ \
+ free(buf); \
+}
+
+bench_cset(uint8_t)
+bench_cset(uint16_t)
+bench_cset(uint32_t)
+bench_cset(uint64_t)
+
+__NO_INLINE static void bench_cset_wide(void)
+{
+ uint32_t *buf = malloc(BUFSIZE);
+
+ uint count = arch_cycle_count();
+ for (uint i = 0; i < ITER; i++) {
+ for (uint j = 0; j < BUFSIZE / sizeof(*buf) / 8; j++) {
+ buf[j*8] = 0;
+ buf[j*8+1] = 0;
+ buf[j*8+2] = 0;
+ buf[j*8+3] = 0;
+ buf[j*8+4] = 0;
+ buf[j*8+5] = 0;
+ buf[j*8+6] = 0;
+ buf[j*8+7] = 0;
+ }
+ }
+ count = arch_cycle_count() - count;
+
+ printf("took %u cycles to manually clear a buffer of size %u %d times 8 words at a time (%u bytes), %f bytes/cycle\n",
+ count, BUFSIZE, ITER, BUFSIZE * ITER, (BUFSIZE * ITER) / (float)count);
+
+ free(buf);
+}
+
+__NO_INLINE static void bench_memcpy(void)
+{
+ uint8_t *buf = calloc(1, BUFSIZE);
+
+ uint count = arch_cycle_count();
+ for (uint i = 0; i < ITER; i++) {
+ memcpy(buf, buf + BUFSIZE / 2, BUFSIZE / 2);
+ }
+ count = arch_cycle_count() - count;
+
+ printf("took %u cycles to memcpy a buffer of size %u %d times (%u source bytes), %f source bytes/cycle\n",
+ count, BUFSIZE / 2, ITER, BUFSIZE / 2 * ITER, (BUFSIZE / 2 * ITER) / (float)count);
+
+ free(buf);
+}
+
+#if ARCH_ARM
+__NO_INLINE static void arm_bench_cset_stm(void)
+{
+ uint32_t *buf = malloc(BUFSIZE);
+
+ uint count = arch_cycle_count();
+ for (uint i = 0; i < ITER; i++) {
+ for (uint j = 0; j < BUFSIZE / sizeof(*buf) / 8; j++) {
+ __asm__ volatile(
+ "stm %0, {r0-r7};"
+ :: "r" (&buf[j*8])
+ );
+ }
+ }
+ count = arch_cycle_count() - count;
+
+ printf("took %u cycles to manually clear a buffer of size %u %d times 8 words at a time using stm (%u bytes), %f bytes/cycle\n",
+ count, BUFSIZE, ITER, BUFSIZE * ITER, (BUFSIZE * ITER) / (float)count);
+
+ free(buf);
+}
+
+#if (__CORTEX_M >= 0x03)
+__NO_INLINE static void arm_bench_multi_issue(void)
+{
+ uint32_t cycles;
+ uint32_t a = 0, b = 0, c = 0, d = 0, e = 0, f = 0, g = 0, h = 0;
+#define ITER 1000000
+ uint count = ITER;
+ cycles = arch_cycle_count();
+ while (count--) {
+ asm volatile ("");
+ asm volatile ("add %0, %0, %0" : "=r" (a) : "r" (a));
+ asm volatile ("add %0, %0, %0" : "=r" (b) : "r" (b));
+ asm volatile ("and %0, %0, %0" : "=r" (c) : "r" (c));
+ asm volatile ("mov %0, %0" : "=r" (d) : "r" (d));
+ asm volatile ("orr %0, %0, %0" : "=r" (e) : "r" (e));
+ asm volatile ("add %0, %0, %0" : "=r" (f) : "r" (f));
+ asm volatile ("and %0, %0, %0" : "=r" (g) : "r" (g));
+ asm volatile ("mov %0, %0" : "=r" (h) : "r" (h));
+ }
+ cycles = arch_cycle_count() - cycles;
+
+ printf("took %u cycles to issue 8 integer ops (%f cycles/iteration)\n", cycles, (float)cycles / ITER);
+#undef ITER
+}
+#endif // __CORTEX_M
+#endif // ARCH_ARM
+
+#if WITH_LIB_LIBM
+#include <math.h>
+
+__NO_INLINE static void bench_sincos(void)
+{
+ printf("touching the floating point unit\n");
+ __UNUSED volatile double _hole = sin(0);
+
+ uint count = arch_cycle_count();
+ __UNUSED double a = sin(2.0);
+ count = arch_cycle_count() - count;
+ printf("took %u cycles for sin()\n", count);
+
+ count = arch_cycle_count();
+ a = cos(2.0);
+ count = arch_cycle_count() - count;
+ printf("took %u cycles for cos()\n", count);
+
+ count = arch_cycle_count();
+ a = sinf(2.0);
+ count = arch_cycle_count() - count;
+ printf("took %u cycles for sinf()\n", count);
+
+ count = arch_cycle_count();
+ a = cosf(2.0);
+ count = arch_cycle_count() - count;
+ printf("took %u cycles for cosf()\n", count);
+
+ count = arch_cycle_count();
+ a = sqrt(1234567.0);
+ count = arch_cycle_count() - count;
+ printf("took %u cycles for sqrt()\n", count);
+
+ count = arch_cycle_count();
+ a = sqrtf(1234567.0f);
+ count = arch_cycle_count() - count;
+ printf("took %u cycles for sqrtf()\n", count);
+}
+
+#endif // WITH_LIB_LIBM
+
+int benchmarks(int argc, const cmd_args *argv)
+{
+ bench_set_overhead();
+ bench_memset();
+ bench_memcpy();
+
+ bench_cset_uint8_t();
+ bench_cset_uint16_t();
+ bench_cset_uint32_t();
+ bench_cset_uint64_t();
+ bench_cset_wide();
+
+#if ARCH_ARM
+ arm_bench_cset_stm();
+
+#if (__CORTEX_M >= 0x03)
+ arm_bench_multi_issue();
+#endif
+#endif
+#if WITH_LIB_LIBM
+ bench_sincos();
+#endif
+
+ return NO_ERROR;
+}
+
diff --git a/src/bsp/lk/app/tests/cache_tests.c b/src/bsp/lk/app/tests/cache_tests.c
new file mode 100644
index 0000000..46c8a93
--- /dev/null
+++ b/src/bsp/lk/app/tests/cache_tests.c
@@ -0,0 +1,88 @@
+/*
+ * Copyright (c) 2014 Travis Geiselbrecht
+ *
+ * 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.
+ */
+#if ARM_WITH_CACHE
+
+#include <stdbool.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <arch.h>
+#include <arch/ops.h>
+#include <lib/console.h>
+#include <platform.h>
+
+static void bench_cache(size_t bufsize, uint8_t* buf)
+{
+ lk_bigtime_t t;
+ bool do_free;
+
+ if (buf == 0) {
+ buf = memalign(PAGE_SIZE, bufsize);
+ do_free = true;
+ } else {
+ do_free = false;
+ }
+
+ printf("buf %p, size %zu\n", buf, bufsize);
+
+ if (!buf)
+ return;
+
+ t = current_time_hires();
+ arch_clean_cache_range((addr_t)buf, bufsize);
+ t = current_time_hires() - t;
+
+ printf("took %llu usecs to clean %d bytes (cold)\n", t, bufsize);
+
+ memset(buf, 0x99, bufsize);
+
+ t = current_time_hires();
+ arch_clean_cache_range((addr_t)buf, bufsize);
+ t = current_time_hires() - t;
+
+ if (do_free)
+ free(buf);
+
+ printf("took %llu usecs to clean %d bytes (hot)\n", t, bufsize);
+}
+
+static int cache_tests(int argc, const cmd_args *argv)
+{
+ uint8_t* buf;
+ buf = (uint8_t *)((argc > 1) ? argv[1].u : 0UL);
+
+ printf("testing cache\n");
+
+ bench_cache(2*1024, buf);
+ bench_cache(64*1024, buf);
+ bench_cache(256*1024, buf);
+ bench_cache(1*1024*1024, buf);
+ bench_cache(8*1024*1024, buf);
+ return 0;
+}
+
+STATIC_COMMAND_START
+STATIC_COMMAND("cache_tests", "test/bench the cpu cache", &cache_tests)
+STATIC_COMMAND_END(cache_tests);
+
+#endif
diff --git a/src/bsp/lk/app/tests/clock_tests.c b/src/bsp/lk/app/tests/clock_tests.c
new file mode 100644
index 0000000..b5e7f74
--- /dev/null
+++ b/src/bsp/lk/app/tests/clock_tests.c
@@ -0,0 +1,120 @@
+/*
+ * Copyright (c) 2012 Travis Geiselbrecht
+ *
+ * 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 <stdio.h>
+#include <rand.h>
+#include <err.h>
+#include <app/tests.h>
+#include <kernel/thread.h>
+#include <kernel/mutex.h>
+#include <kernel/semaphore.h>
+#include <kernel/event.h>
+#include <platform.h>
+
+int clock_tests(int argc, const cmd_args *argv)
+{
+ uint32_t c;
+ lk_time_t t;
+ lk_bigtime_t t2;
+
+ thread_sleep(100);
+ c = arch_cycle_count();
+ t = current_time();
+ c = arch_cycle_count() - c;
+ printf("%u cycles per current_time()\n", c);
+
+ thread_sleep(100);
+ c = arch_cycle_count();
+ t2 = current_time_hires();
+ c = arch_cycle_count() - c;
+ printf("%u cycles per current_time_hires()\n", c);
+
+ printf("making sure time never goes backwards\n");
+ {
+ printf("testing current_time()\n");
+ lk_time_t start = current_time();
+ lk_time_t last = start;
+ for (;;) {
+ t = current_time();
+ //printf("%lu %lu\n", last, t);
+ if (TIME_LT(t, last)) {
+ printf("WARNING: time ran backwards: %lu < %lu\n", t, last);
+ last = t;
+ continue;
+ }
+ last = t;
+ if (last - start > 5000)
+ break;
+ }
+ }
+ {
+ printf("testing current_time_hires()\n");
+ lk_bigtime_t start = current_time_hires();
+ lk_bigtime_t last = start;
+ for (;;) {
+ t2 = current_time_hires();
+ //printf("%llu %llu\n", last, t2);
+ if (t2 < last) {
+ printf("WARNING: time ran backwards: %llu < %llu\n", t2, last);
+ last = t2;
+ continue;
+ }
+ last = t2;
+ if (last - start > 5000000)
+ break;
+ }
+ }
+
+ printf("making sure current_time() and current_time_hires() are always the same base\n");
+ {
+ lk_time_t start = current_time();
+ for (;;) {
+ t = current_time();
+ t2 = current_time_hires();
+ if (t > ((t2 + 500) / 1000)) {
+ printf("WARNING: current_time() ahead of current_time_hires() %lu %llu\n", t, t2);
+ }
+ if (t - start > 5000)
+ break;
+ }
+ }
+
+ printf("counting to 5, in one second intervals\n");
+ for (int i = 0; i < 5; i++) {
+ thread_sleep(1000);
+ printf("%d\n", i + 1);
+ }
+
+ printf("measuring cpu clock against current_time_hires()\n");
+ for (int i = 0; i < 5; i++) {
+ uint cycles = arch_cycle_count();
+ lk_bigtime_t start = current_time_hires();
+ while ((current_time_hires() - start) < 1000000)
+ ;
+ cycles = arch_cycle_count() - cycles;
+ printf("%u cycles per second\n", cycles);
+ }
+
+ return NO_ERROR;
+}
+
+// vim: set noexpandtab:
diff --git a/src/bsp/lk/app/tests/fibo.c b/src/bsp/lk/app/tests/fibo.c
new file mode 100644
index 0000000..3c3320f
--- /dev/null
+++ b/src/bsp/lk/app/tests/fibo.c
@@ -0,0 +1,96 @@
+/*
+ * Copyright (c) 2012 Travis Geiselbrecht
+ *
+ * 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 <stdio.h>
+#include <rand.h>
+#include <err.h>
+#include <app/tests.h>
+#include <kernel/thread.h>
+#include <kernel/mutex.h>
+#include <kernel/semaphore.h>
+#include <kernel/event.h>
+#include <platform.h>
+
+static int fibo_thread(void *argv)
+{
+ long fibo = (intptr_t)argv;
+
+ thread_t *t[2];
+
+ if (fibo == 0)
+ return 0;
+ if (fibo == 1)
+ return 1;
+
+ char name[32];
+ snprintf(name, sizeof(name), "fibo %lu", fibo - 1);
+ t[0] = thread_create(name, &fibo_thread, (void *)(fibo - 1), DEFAULT_PRIORITY, DEFAULT_STACK_SIZE);
+ if (!t[0]) {
+ printf("error creating thread for fibo %d\n", fibo-1);
+ return 0;
+ }
+ snprintf(name, sizeof(name), "fibo %lu", fibo - 2);
+ t[1] = thread_create(name, &fibo_thread, (void *)(fibo - 2), DEFAULT_PRIORITY, DEFAULT_STACK_SIZE);
+ if (!t[1]) {
+ printf("error creating thread for fibo %d\n", fibo-2);
+ thread_resume(t[0]);
+ thread_join(t[0], NULL, INFINITE_TIME);
+ return 0;
+ }
+
+ thread_resume(t[0]);
+ thread_resume(t[1]);
+
+ int retcode0, retcode1;
+
+ thread_join(t[0], &retcode0, INFINITE_TIME);
+ thread_join(t[1], &retcode1, INFINITE_TIME);
+
+ return retcode0 + retcode1;
+}
+
+int fibo(int argc, const cmd_args *argv)
+{
+
+ if (argc < 2) {
+ printf("not enough args\n");
+ return -1;
+ }
+
+ lk_time_t tim = current_time();
+
+ thread_t *t = thread_create("fibo", &fibo_thread, (void *)(uintptr_t)argv[1].u, DEFAULT_PRIORITY, DEFAULT_STACK_SIZE);
+ thread_resume(t);
+
+ int retcode;
+ thread_join(t, &retcode, INFINITE_TIME);
+
+ tim = current_time() - tim;
+
+ printf("fibo %d\n", retcode);
+ printf("took %u msecs to calculate\n", tim);
+
+ return NO_ERROR;
+}
+
+// vim: set noexpandtab:
+
diff --git a/src/bsp/lk/app/tests/float.c b/src/bsp/lk/app/tests/float.c
new file mode 100755
index 0000000..5d3d8c6
--- /dev/null
+++ b/src/bsp/lk/app/tests/float.c
@@ -0,0 +1,116 @@
+/*
+ * Copyright (c) 2013-2015 Travis Geiselbrecht
+ *
+ * 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.
+ */
+#if ARM_WITH_VFP || ARCH_ARM64 || X86_WITH_FPU
+
+#include <stdio.h>
+#include <rand.h>
+#include <err.h>
+#include <lib/console.h>
+#include <app/tests.h>
+#include <kernel/thread.h>
+#include <kernel/mutex.h>
+#include <kernel/semaphore.h>
+#include <kernel/event.h>
+#include <platform.h>
+
+extern void float_vfp_arm_instruction_test(void);
+extern void float_vfp_thumb_instruction_test(void);
+extern void float_neon_arm_instruction_test(void);
+extern void float_neon_thumb_instruction_test(void);
+
+/* optimize this function to cause it to try to use a lot of registers */
+__OPTIMIZE("O3")
+static int float_thread(void *arg)
+{
+ double *val = arg;
+ uint i, j;
+
+ double a[16];
+
+ /* do a bunch of work with floating point to test context switching */
+ a[0] = *val;
+ for (i = 1; i < countof(a); i++) {
+ a[i] = a[i-1] * 1.01;
+ }
+
+ for (i = 0; i < 1000000; i++) {
+ a[0] += i;
+ for (j = 1; j < countof(a); j++) {
+ a[j] += a[j-1] * 0.00001;
+ }
+ }
+
+ *val = a[countof(a) - 1];
+
+ return 1;
+}
+
+#if ARCH_ARM
+static void arm_float_instruction_trap_test(void)
+{
+ printf("testing fpu trap\n");
+
+#if !ARM_ONLY_THUMB
+ float_vfp_arm_instruction_test();
+ float_neon_arm_instruction_test();
+#endif
+ float_vfp_thumb_instruction_test();
+ float_neon_thumb_instruction_test();
+
+ printf("if we got here, we probably decoded everything properly\n");
+}
+#endif
+
+static void float_tests(void)
+{
+ printf("floating point test:\n");
+
+ /* test lazy fpu load on separate thread */
+ thread_t *t[8];
+ double val[countof(t)];
+
+ printf("creating %u floating point threads\n", countof(t));
+ for (uint i = 0; i < countof(t); i++) {
+ val[i] = i;
+ t[i] = thread_create("float", &float_thread, &val[i], LOW_PRIORITY, DEFAULT_STACK_SIZE);
+ thread_resume(t[i]);
+ }
+
+ int res;
+ for (uint i = 0; i < countof(t); i++) {
+ thread_join(t[i], &res, INFINITE_TIME);
+ printf("float thread %u returns %d, val %f\n", i, res, val[i]);
+ }
+ printf("the above values should be close\n");
+
+#if ARCH_ARM
+ /* test all the instruction traps */
+ arm_float_instruction_trap_test();
+#endif
+}
+
+STATIC_COMMAND_START
+STATIC_COMMAND("float_tests", "floating point test", (console_cmd)&float_tests)
+STATIC_COMMAND_END(float_tests);
+
+#endif // ARM_WITH_VFP || ARCH_ARM64
diff --git a/src/bsp/lk/app/tests/float_instructions.S b/src/bsp/lk/app/tests/float_instructions.S
new file mode 100644
index 0000000..297fcbd
--- /dev/null
+++ b/src/bsp/lk/app/tests/float_instructions.S
@@ -0,0 +1,87 @@
+/*
+ * Copyright (c) 2014 Travis Geiselbrecht
+ *
+ * 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 <asm.h>
+
+#if ARM_WITH_VFP
+
+.fpu neon
+.syntax unified
+
+.macro disable, scratchreg
+ vmrs \scratchreg, fpexc
+ bic \scratchreg, #(1<<30)
+ vmsr fpexc, \scratchreg
+.endm
+
+.macro vfp_instructions
+ disable r12
+ vadd.f32 s0, s0, s0
+
+ disable r12
+ vadd.f64 d0, d0, d0
+
+ disable r12
+ ldr r0, =float_test_scratch
+ vldr s0, [r0]
+.endm
+
+.macro neon_instructions
+ disable r12
+ vadd.f32 q0, q0, q0
+
+ disable r12
+ ldr r0, =float_test_scratch
+ vld1.f32 { q0 }, [r0]
+
+ disable r12
+ vmov s0, r0
+.endm
+
+#if !ARM_ONLY_THUMB
+.arm
+
+FUNCTION(float_vfp_arm_instruction_test)
+ vfp_instructions
+ bx lr
+
+FUNCTION(float_neon_arm_instruction_test)
+ neon_instructions
+ bx lr
+#endif
+
+.thumb
+
+FUNCTION(float_vfp_thumb_instruction_test)
+ vfp_instructions
+ bx lr
+
+FUNCTION(float_neon_thumb_instruction_test)
+ neon_instructions
+ bx lr
+
+.data
+LOCAL_DATA(float_test_scratch)
+ .word 0
+ .word 0
+
+#endif // ARM_WITH_VFP
diff --git a/src/bsp/lk/app/tests/float_print_host.c b/src/bsp/lk/app/tests/float_print_host.c
new file mode 100644
index 0000000..1176876
--- /dev/null
+++ b/src/bsp/lk/app/tests/float_print_host.c
@@ -0,0 +1,17 @@
+#include <stdio.h>
+
+#define countof(a) (sizeof(a) / sizeof((a)[0]))
+
+#include "float_test_vec.c"
+
+int main(void)
+{
+ printf("floating point printf tests\n");
+
+ for (size_t i = 0; i < float_test_vec_size; i++) {
+ PRINT_FLOAT;
+ }
+
+ return 0;
+}
+
diff --git a/src/bsp/lk/app/tests/float_test_vec.c b/src/bsp/lk/app/tests/float_test_vec.c
new file mode 100644
index 0000000..d7d77c1
--- /dev/null
+++ b/src/bsp/lk/app/tests/float_test_vec.c
@@ -0,0 +1,81 @@
+/*
+ * Copyright (c) 2014 Travis Geiselbrecht
+ *
+ * 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 <stdint.h>
+
+union double_int {
+ double d;
+ uint64_t i;
+};
+
+static const union double_int float_test_vec[] = {
+ { .d = -2.0 },
+ { .d = -1.0 },
+ { .d = -0.5 },
+ { .d = -0.0 },
+ { .d = 0.0 },
+ { .d = 0.01 },
+ { .d = 0.1 },
+ { .d = 0.2 },
+ { .d = 0.25 },
+ { .d = 0.5 },
+ { .d = 0.75 },
+ { .d = 1.0 },
+ { .d = 2.0 },
+ { .d = 3.0 },
+ { .d = 10.0 },
+ { .d = 100.0 },
+ { .d = 123456.0 },
+ { .d = -123456.0 },
+ { .d = 546.5645644531f },
+ { .d = -546.5645644531f },
+ { .d = 0.12345 },
+ { .d = 0.0000012345 },
+ { .d = 0.0000019999 },
+ { .d = 0.0000015 },
+ { .i = 0x4005bf0a8b145649ULL }, // e
+ { .i = 0x400921fb54442d18ULL }, // pi
+ { .i = 0x43f0000000000000ULL }, // 2^64
+ { .i = 0x7fefffffffffffffULL }, // largest normalized
+ { .i = 0x0010000000000000ULL }, // least positive normalized
+ { .i = 0x0000000000000001ULL }, // smallest possible denorm
+ { .i = 0x000fffffffffffffULL }, // largest possible denorm
+ { .i = 0x7ff0000000000001ULL }, // smallest SNAn
+ { .i = 0x7ff7ffffffffffffULL }, // largest SNAn
+ { .i = 0x7ff8000000000000ULL }, // smallest QNAn
+ { .i = 0x7fffffffffffffffULL }, // largest QNAn
+ { .i = 0xfff0000000000000ULL }, // -infinity
+ { .i = 0x7ff0000000000000ULL }, // +infinity
+};
+
+#define countof(a) (sizeof(a) / sizeof((a)[0]))
+static const unsigned int float_test_vec_size = countof(float_test_vec);
+
+#define PRINT_FLOAT \
+ printf("0x%016llx %f %F %a %A\n", \
+ float_test_vec[i], \
+ *(const double *)&float_test_vec[i], \
+ *(const double *)&float_test_vec[i], \
+ *(const double *)&float_test_vec[i], \
+ *(const double *)&float_test_vec[i])
+
diff --git a/src/bsp/lk/app/tests/include/app/tests.h b/src/bsp/lk/app/tests/include/app/tests.h
new file mode 100644
index 0000000..ea65bf8
--- /dev/null
+++ b/src/bsp/lk/app/tests/include/app/tests.h
@@ -0,0 +1,38 @@
+/*
+ * Copyright (c) 2008-2014 Travis Geiselbrecht
+ *
+ * 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.
+ */
+#ifndef __APP_TESTS_H
+#define __APP_TESTS_H
+
+#include <lib/console.h>
+
+int port_tests(int argc, const cmd_args *argv);
+int fibo(int argc, const cmd_args *argv);
+int spinner(int argc, const cmd_args *argv);
+int thread_tests(int argc, const cmd_args *argv);
+int benchmarks(int argc, const cmd_args *argv);
+int clock_tests(int argc, const cmd_args *argv);
+int printf_tests(int argc, const cmd_args *argv);
+int printf_tests_float(int argc, const cmd_args *argv);
+
+#endif
+
diff --git a/src/bsp/lk/app/tests/mem_tests.c b/src/bsp/lk/app/tests/mem_tests.c
new file mode 100644
index 0000000..0b76e64
--- /dev/null
+++ b/src/bsp/lk/app/tests/mem_tests.c
@@ -0,0 +1,242 @@
+/*
+ * Copyright (c) 2014 Travis Geiselbrecht
+ *
+ * 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 <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <err.h>
+#include <arch.h>
+#include <arch/ops.h>
+#include <lib/console.h>
+#include <platform.h>
+#include <debug.h>
+
+#if WITH_KERNEL_VM
+#include <kernel/vm.h>
+#endif
+
+static void mem_test_fail(void *ptr, uint32_t should, uint32_t is)
+{
+ printf("ERROR at %p: should be 0x%x, is 0x%x\n", ptr, should, is);
+
+ ptr = (void *)ROUNDDOWN((uintptr_t)ptr, 64);
+ hexdump(ptr, 128);
+}
+
+static status_t do_pattern_test(void *ptr, size_t len, uint32_t pat)
+{
+ volatile uint32_t *vbuf32 = ptr;
+ size_t i;
+
+ printf("\tpattern 0x%08x\n", pat);
+ for (i = 0; i < len / 4; i++) {
+ vbuf32[i] = pat;
+ }
+
+ for (i = 0; i < len / 4; i++) {
+ if (vbuf32[i] != pat) {
+ mem_test_fail((void *)&vbuf32[i], pat, vbuf32[i]);
+ return ERR_GENERIC;
+ }
+ }
+
+ return NO_ERROR;
+}
+
+static status_t do_moving_inversion_test(void *ptr, size_t len, uint32_t pat)
+{
+ volatile uint32_t *vbuf32 = ptr;
+ size_t i;
+
+ printf("\tpattern 0x%08x\n", pat);
+
+ /* fill memory */
+ for (i = 0; i < len / 4; i++) {
+ vbuf32[i] = pat;
+ }
+
+ /* from the bottom, walk through each cell, inverting the value */
+ //printf("\t\tbottom up invert\n");
+ for (i = 0; i < len / 4; i++) {
+ if (vbuf32[i] != pat) {
+ mem_test_fail((void *)&vbuf32[i], pat, vbuf32[i]);
+ return ERR_GENERIC;
+ }
+
+ vbuf32[i] = ~pat;
+ }
+
+ /* repeat, walking from top down */
+ //printf("\t\ttop down invert\n");
+ for (i = len / 4; i > 0; i--) {
+ if (vbuf32[i-1] != ~pat) {
+ mem_test_fail((void *)&vbuf32[i-1], ~pat, vbuf32[i-1]);
+ return ERR_GENERIC;
+ }
+
+ vbuf32[i-1] = pat;
+ }
+
+ /* verify that we have the original pattern */
+ //printf("\t\tfinal test\n");
+ for (i = 0; i < len / 4; i++) {
+ if (vbuf32[i] != pat) {
+ mem_test_fail((void *)&vbuf32[i], pat, vbuf32[i]);
+ return ERR_GENERIC;
+ }
+ }
+
+ return NO_ERROR;
+}
+
+static void do_mem_tests(void *ptr, size_t len)
+{
+ size_t i;
+
+ /* test 1: simple write address to memory, read back */
+ printf("test 1: simple address write, read back\n");
+ volatile uint32_t *vbuf32 = ptr;
+ for (i = 0; i < len / 4; i++) {
+ vbuf32[i] = i;
+ }
+
+ for (i = 0; i < len / 4; i++) {
+ if (vbuf32[i] != i) {
+ mem_test_fail((void *)&vbuf32[i], i, vbuf32[i]);
+ goto out;
+ }
+ }
+
+ /* test 2: write various patterns, read back */
+ printf("test 2: write patterns, read back\n");
+
+ static const uint32_t pat[] = {
+ 0x0, 0xffffffff,
+ 0xaaaaaaaa, 0x55555555,
+ };
+
+ for (size_t p = 0; p < countof(pat); p++) {
+ if (do_pattern_test(ptr, len, pat[p]) < 0)
+ goto out;
+ }
+ // shift bits through 32bit word
+ for (uint32_t p = 1; p != 0; p <<= 1) {
+ if (do_pattern_test(ptr, len, p) < 0)
+ goto out;
+ }
+ // shift bits through 16bit word, invert top of 32bit
+ for (uint16_t p = 1; p != 0; p <<= 1) {
+ if (do_pattern_test(ptr, len, ((~p) << 16) | p) < 0)
+ goto out;
+ }
+
+ /* test 3: moving inversion, patterns */
+ printf("test 3: moving inversions with patterns\n");
+ for (size_t p = 0; p < countof(pat); p++) {
+ if (do_moving_inversion_test(ptr, len, pat[p]) < 0)
+ goto out;
+
+ }
+ // shift bits through 32bit word
+ for (uint32_t p = 1; p != 0; p <<= 1) {
+ if (do_moving_inversion_test(ptr, len, p) < 0)
+ goto out;
+ }
+ // shift bits through 16bit word, invert top of 32bit
+ for (uint16_t p = 1; p != 0; p <<= 1) {
+ if (do_moving_inversion_test(ptr, len, ((~p) << 16) | p) < 0)
+ goto out;
+ }
+
+out:
+ printf("done with tests\n");
+}
+
+static int mem_test(int argc, const cmd_args *argv)
+{
+ if (argc < 2) {
+ printf("not enough arguments\n");
+usage:
+ printf("usage: %s <length>\n", argv[0].str);
+ printf("usage: %s <base> <length>\n", argv[0].str);
+ return -1;
+ }
+
+ if (argc == 2) {
+ void *ptr;
+ size_t len = argv[1].u;
+
+#if WITH_KERNEL_VM
+ /* rounding up len to the next page */
+ len = PAGE_ALIGN(len);
+ if (len == 0) {
+ printf("invalid length\n");
+ return -1;
+ }
+
+ /* allocate a region to test in */
+ status_t err = vmm_alloc_contiguous(vmm_get_kernel_aspace(), "memtest", len, &ptr, 0, 0, ARCH_MMU_FLAG_UNCACHED);
+ if (err < 0) {
+ printf("error %d allocating test region\n", err);
+ return -1;
+ }
+
+ paddr_t pa;
+ arch_mmu_query((vaddr_t)ptr, &pa, 0);
+ printf("physical address 0x%lx\n", pa);
+#else
+ /* allocate from the heap */
+ ptr = malloc(len);
+ if (!ptr ) {
+ printf("error allocating test area from heap\n");
+ return -1;
+ }
+
+#endif
+
+ printf("got buffer at %p of length 0x%lx\n", ptr, len);
+
+ /* run the tests */
+ do_mem_tests(ptr, len);
+
+#if WITH_KERNEL_VM
+ // XXX free memory region here
+ printf("NOTE: leaked memory\n");
+#else
+ free(ptr);
+#endif
+ } else if (argc == 3) {
+ void *ptr = argv[1].p;
+ size_t len = argv[2].u;
+
+ /* run the tests */
+ do_mem_tests(ptr, len);
+ } else {
+ goto usage;
+ }
+
+ return 0;
+}
+
+STATIC_COMMAND_START
+STATIC_COMMAND("mem_test", "test memory", &mem_test)
+STATIC_COMMAND_END(mem_tests);
diff --git a/src/bsp/lk/app/tests/port_tests.c b/src/bsp/lk/app/tests/port_tests.c
new file mode 100644
index 0000000..591096c
--- /dev/null
+++ b/src/bsp/lk/app/tests/port_tests.c
@@ -0,0 +1,605 @@
+/*
+ * Copyright (c) 2015 Carlos Pizano-Uribe cpu@chromium.org
+ *
+ * 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 <debug.h>
+#include <err.h>
+#include <rand.h>
+#include <string.h>
+#include <trace.h>
+
+#include <kernel/port.h>
+#include <kernel/thread.h>
+
+#include <platform.h>
+
+#define LOCAL_TRACE 0
+
+void* context1 = (void*) 0x53;
+
+static void dump_port_result(const port_result_t* result)
+{
+ const port_packet_t* p = &result->packet;
+ LTRACEF("[%02x %02x %02x %02x %02x %02x %02x %02x]\n",
+ p->value[0], p->value[1], p->value[2], p->value[3],
+ p->value[4], p->value[5], p->value[6], p->value[7]);
+}
+
+static int single_thread_basic(void)
+{
+ port_t w_port;
+ status_t st = port_create("sh_prt1", PORT_MODE_UNICAST, &w_port);
+ if (st < 0) {
+ printf("could not create port, status = %d\n", st);
+ return __LINE__;
+ }
+
+ port_t r_port;
+ st = port_open("sh_prt0", context1, &r_port);
+ if (st != ERR_NOT_FOUND) {
+ printf("expected not to find port, status = %d\n", st);
+ return __LINE__;
+ }
+
+ st = port_open("sh_prt1", context1, &r_port);
+ if (st < 0) {
+ printf("could not open port, status = %d\n", st);
+ return __LINE__;
+ }
+
+ port_packet_t packet[3] = {
+ {{0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07, 0x08}},
+ {{0x0a, 0x0b, 0x0c, 0x0d, 0x0e, 0x0f, 0x10, 0x11}},
+ {{0x33, 0x66, 0x99, 0xcc, 0x33, 0x66, 0x99, 0xcc}},
+ };
+
+ st = port_write(w_port, &packet[0], 1);
+ if (st < 0) {
+ printf("could not write port, status = %d\n", st);
+ return __LINE__;
+ }
+
+ printf("reading from port:\n");
+
+ port_result_t res = {0};
+
+ st = port_read(r_port, 0, &res);
+ if (st < 0) {
+ printf("could not read port, status = %d\n", st);
+ return __LINE__;
+ }
+ if (res.ctx != context1) {
+ printf("bad context! = %p\n", res.ctx);
+ return __LINE__;
+ }
+
+ st = port_read(r_port, 0, &res);
+ if (st != ERR_TIMED_OUT) {
+ printf("expected timeout, status = %d\n", st);
+ return __LINE__;
+ }
+
+ st = port_write(w_port, &packet[1], 1);
+ if (st < 0) {
+ printf("could not write port, status = %d\n", st);
+ return __LINE__;
+ }
+
+ st = port_write(w_port, &packet[0], 1);
+ if (st < 0) {
+ printf("could not write port, status = %d\n", st);
+ return __LINE__;
+ }
+
+ st = port_write(w_port, &packet[2], 1);
+ if (st < 0) {
+ printf("could not write port, status = %d\n", st);
+ return __LINE__;
+ }
+
+ int expected_count = 3;
+ while (true) {
+ st = port_read(r_port, 0, &res);
+ if (st < 0)
+ break;
+ dump_port_result(&res);
+ --expected_count;
+ }
+
+ if (expected_count != 0) {
+ printf("invalid read count = %d\n", expected_count);
+ return __LINE__;
+ }
+
+ printf("\n");
+
+ // port should be empty. should be able to write 8 packets.
+ expected_count = 8;
+ while (true) {
+ st = port_write(w_port, &packet[1], 1);
+ if (st < 0)
+ break;
+ --expected_count;
+ st = port_write(w_port, &packet[2], 1);
+ if (st < 0)
+ break;
+ --expected_count;
+ }
+
+ if (expected_count != 0) {
+ printf("invalid write count = %d\n", expected_count);
+ return __LINE__;
+ }
+
+ // tod(cpu) fix this possibly wrong error.
+ if (st != ERR_PARTIAL_WRITE) {
+ printf("expected buffer error, status =%d\n", st);
+ return __LINE__;
+ }
+
+ // read 3 packets.
+ for (int ix = 0; ix != 3; ++ix) {
+ st = port_read(r_port, 0, &res);
+ if (st < 0) {
+ printf("could not read port, status = %d\n", st);
+ return __LINE__;
+ }
+ }
+
+ // there are 5 packets, now we add another 3.
+ st = port_write(w_port, packet, 3);
+ if (st < 0) {
+ printf("could not write port, status = %d\n", st);
+ return __LINE__;
+ }
+
+ expected_count = 8;
+ while (true) {
+ st = port_read(r_port, 0, &res);
+ if (st < 0)
+ break;
+ dump_port_result(&res);
+ --expected_count;
+ }
+
+ if (expected_count != 0) {
+ printf("invalid read count = %d\n", expected_count);
+ return __LINE__;
+ }
+
+ // attempt to use the wrong port.
+ st = port_write(r_port, &packet[1], 1);
+ if (st != ERR_BAD_HANDLE) {
+ printf("expected bad handle error, status = %d\n", st);
+ return __LINE__;
+ }
+
+ st = port_read(w_port, 0, &res);
+ if (st != ERR_BAD_HANDLE) {
+ printf("expected bad handle error, status = %d\n", st);
+ return __LINE__;
+ }
+
+ st = port_close(r_port);
+ if (st < 0) {
+ printf("could not close read port, status = %d\n", st);
+ return __LINE__;
+ }
+
+ st = port_close(w_port);
+ if (st < 0) {
+ printf("could not close write port, status = %d\n", st);
+ return __LINE__;
+ }
+
+ st = port_close(r_port);
+ if (st != ERR_BAD_HANDLE) {
+ printf("expected bad handle error, status = %d\n", st);
+ return __LINE__;
+ }
+
+ st = port_close(w_port);
+ if (st != ERR_BAD_HANDLE) {
+ printf("expected bad handle error, status = %d\n", st);
+ return __LINE__;
+ }
+
+ st = port_destroy(w_port);
+ if (st < 0) {
+ printf("could not destroy port, status = %d\n", st);
+ return __LINE__;
+ }
+
+ printf("single_thread_basic : ok\n");
+ return 0;
+}
+
+static int ping_pong_thread(void *arg)
+{
+ port_t r_port;
+ status_t st = port_open("ping_port", NULL, &r_port);
+ if (st < 0) {
+ printf("thread: could not open port, status = %d\n", st);
+ return __LINE__;
+ }
+
+ bool should_dispose_pong_port = true;
+ port_t w_port;
+ st = port_create("pong_port", PORT_MODE_UNICAST, &w_port);
+ if (st == ERR_ALREADY_EXISTS) {
+ // won the race to create the port.
+ should_dispose_pong_port = false;
+ } else if (st < 0) {
+ printf("thread: could not open port, status = %d\n", st);
+ return __LINE__;
+ }
+
+ port_result_t pr;
+
+ // the loop is read-mutate-write until the write port
+ // is closed by the master thread.
+ while (true) {
+ st = port_read(r_port, INFINITE_TIME, &pr);
+
+ if (st == ERR_CANCELLED) {
+ break;
+ } else if (st < 0) {
+ printf("thread: could not read port, status = %d\n", st);
+ return __LINE__;
+ }
+
+ pr.packet.value[0]++;
+ pr.packet.value[5]--;
+
+ st = port_write(w_port, &pr.packet, 1);
+ if (st < 0) {
+ printf("thread: could not write port, status = %d\n", st);
+ return __LINE__;
+ }
+ }
+
+ port_close(r_port);
+
+ if (should_dispose_pong_port) {
+ port_close(w_port);
+ port_destroy(w_port);
+ }
+
+ return 0;
+
+bail:
+ return __LINE__;
+}
+
+
+int two_threads_basic(void)
+{
+ port_t w_port;
+ status_t st = port_create("ping_port", PORT_MODE_BROADCAST, &w_port);
+ if (st < 0) {
+ printf("could not create port, status = %d\n", st);
+ return __LINE__;
+ }
+
+ thread_t* t1 = thread_create(
+ "worker1", &ping_pong_thread, NULL, DEFAULT_PRIORITY, DEFAULT_STACK_SIZE);
+ thread_t* t2 = thread_create(
+ "worker2", &ping_pong_thread, NULL, DEFAULT_PRIORITY, DEFAULT_STACK_SIZE);
+ thread_resume(t1);
+ thread_resume(t2);
+
+ // wait for the pong port to be created, the two threads race to do it.
+ port_t r_port;
+ while (true) {
+ status_t st = port_open("pong_port", NULL, &r_port);
+ if (st == NO_ERROR) {
+ break;
+ } else if (st == ERR_NOT_FOUND) {
+ thread_sleep(100);
+ } else {
+ printf("could not open port, status = %d\n", st);
+ return __LINE__;
+ }
+ }
+
+ // We have two threads listening to the ping port. Which both reply
+ // on the pong port, so we get two packets in per packet out.
+ const int passes = 256;
+ printf("two_threads_basic test, %d passes\n", passes);
+
+ port_packet_t packet_out = {{0xaf, 0x77, 0x00, 0x01, 0x02, 0x03, 0x04, 0x05}};
+
+ port_result_t pr;
+ for (int ix = 0; ix != passes; ++ix) {
+ const size_t count = 1 + ((unsigned int)rand() % 3);
+
+ for (size_t jx = 0; jx != count; ++jx) {
+ st = port_write(w_port, &packet_out, 1);
+ if (st < 0) {
+ printf("could not write port, status = %d\n", st);
+ return __LINE__;
+ }
+ }
+
+ packet_out.value[0]++;
+ packet_out.value[5]--;
+
+ for (size_t jx = 0; jx != count * 2; ++jx) {
+ st = port_read(r_port, INFINITE_TIME, &pr);
+ if (st < 0) {
+ printf("could not read port, status = %d\n", st);
+ return __LINE__;
+ }
+
+ if ((pr.packet.value[0] != packet_out.value[0]) ||
+ (pr.packet.value[5] != packet_out.value[5])) {
+ printf("unexpected data in packet, loop %d", ix);
+ return __LINE__;
+ }
+ }
+ }
+
+ thread_sleep(100);
+
+ // there should be no more packets to read.
+ st = port_read(r_port, 0, &pr);
+ if (st != ERR_TIMED_OUT) {
+ printf("unexpected packet, status = %d\n", st);
+ return __LINE__;
+ }
+
+ printf("two_threads_basic master shutdown\n");
+
+ st = port_close(r_port);
+ if (st < 0) {
+ printf("could not close port, status = %d\n", st);
+ return __LINE__;
+ }
+
+ st = port_close(w_port);
+ if (st < 0) {
+ printf("could not close port, status = %d\n", st);
+ return __LINE__;
+ }
+
+ st = port_destroy(w_port);
+ if (st < 0) {
+ printf("could not destroy port, status = %d\n", st);
+ return __LINE__;
+ }
+
+ int retcode = -1;
+ thread_join(t1, &retcode, INFINITE_TIME);
+ if (retcode)
+ goto fail;
+
+ thread_join(t2, &retcode, INFINITE_TIME);
+ if (retcode)
+ goto fail;
+
+ return 0;
+
+fail:
+ printf("child thread exited with %d\n", retcode);
+ return __LINE__;
+}
+
+#define CMD_PORT_CTX ((void*) 0x77)
+#define TS1_PORT_CTX ((void*) 0x11)
+#define TS2_PORT_CTX ((void*) 0x12)
+
+typedef enum {
+ ADD_PORT,
+ QUIT
+} action_t;
+
+typedef struct {
+ action_t what;
+ port_t port;
+} watcher_cmd;
+
+status_t send_watcher_cmd(port_t cmd_port, action_t action, port_t port)
+{
+ watcher_cmd cmd = {action, port};
+ return port_write(cmd_port, ((port_packet_t*) &cmd), 1);;
+}
+
+static int group_watcher_thread(void *arg)
+{
+ port_t watched[8] = {0};
+ status_t st = port_open("grp_ctrl", CMD_PORT_CTX, &watched[0]);
+ if (st < 0) {
+ printf("could not open port, status = %d\n", st);
+ return __LINE__;
+ }
+
+ size_t count = 1;
+ port_t group;
+ int ctx_count = -1;
+
+ while (true) {
+ st = port_group(watched, count, &group);
+ if (st < 0) {
+ printf("could not make group, status = %d\n", st);
+ return __LINE__;
+ }
+
+ port_result_t pr;
+ while (true) {
+ st = port_read(group, INFINITE_TIME, &pr);
+ if (st < 0) {
+ printf("could not read port, status = %d\n", st);
+ return __LINE__;
+ }
+
+ if (pr.ctx == CMD_PORT_CTX) {
+ break;
+ } else if (pr.ctx == TS1_PORT_CTX) {
+ ctx_count += 1;
+ } else if (pr.ctx == TS2_PORT_CTX) {
+ ctx_count += 2;
+ } else {
+ printf("unknown context %p\n", pr.ctx);
+ return __LINE__;
+ }
+ }
+
+ // Either adding a port or exiting; either way close the
+ // existing group port and create a new one if needed
+ // at the top of the loop.
+
+ port_close(group);
+ watcher_cmd* wc = (watcher_cmd*) &pr.packet;
+
+ if (wc->what == ADD_PORT) {
+ watched[count++] = wc->port;
+ } else if (wc->what == QUIT) {
+ break;
+ } else {
+ printf("unknown command %d\n", wc->what);
+ return __LINE__;
+ }
+ }
+
+ if (ctx_count != 2) {
+ printf("unexpected context count %d", ctx_count);
+ return __LINE__;
+ }
+
+ printf("group watcher shutdown\n");
+
+ for (size_t ix = 0; ix != count; ++ix) {
+ st = port_close(watched[ix]);
+ if (st < 0) {
+ printf("failed to close read port, status = %d\n", st);
+ return __LINE__;
+ }
+ }
+
+ return 0;
+}
+
+static status_t make_port_pair(const char* name, void* ctx, port_t* write, port_t* read)
+{
+ status_t st = port_create(name, PORT_MODE_UNICAST, write);
+ if (st < 0)
+ return st;
+ return port_open(name,ctx, read);
+}
+
+int group_basic(void)
+{
+ // we spin a thread that connects to a well known port, then we
+ // send two ports that it will add to a group port.
+ port_t cmd_port;
+ status_t st = port_create("grp_ctrl", PORT_MODE_UNICAST, &cmd_port);
+ if (st < 0 ) {
+ printf("could not create port, status = %d\n", st);
+ return __LINE__;
+ }
+
+ thread_t* wt = thread_create(
+ "g_watcher", &group_watcher_thread, NULL, DEFAULT_PRIORITY, DEFAULT_STACK_SIZE);
+ thread_resume(wt);
+
+ port_t w_test_port1, r_test_port1;
+ st = make_port_pair("tst_port1", TS1_PORT_CTX, &w_test_port1, &r_test_port1);
+ if (st < 0)
+ return __LINE__;
+
+ port_t w_test_port2, r_test_port2;
+ st = make_port_pair("tst_port2", TS2_PORT_CTX, &w_test_port2, &r_test_port2);
+ if (st < 0)
+ return __LINE__;
+
+ st = send_watcher_cmd(cmd_port, ADD_PORT, r_test_port1);
+ if (st < 0)
+ return __LINE__;
+
+ st = send_watcher_cmd(cmd_port, ADD_PORT, r_test_port2);
+ if (st < 0)
+ return __LINE__;
+
+ thread_sleep(50);
+
+ port_packet_t pp = {{0}};
+ st = port_write(w_test_port1, &pp, 1);
+ if (st < 0)
+ return __LINE__;
+
+ st = port_write(w_test_port2, &pp, 1);
+ if (st < 0)
+ return __LINE__;
+
+ st = send_watcher_cmd(cmd_port, QUIT, 0);
+ if (st < 0)
+ return __LINE__;
+
+ int retcode = -1;
+ thread_join(wt, &retcode, INFINITE_TIME);
+ if (retcode) {
+ printf("child thread exited with %d\n", retcode);
+ return __LINE__;
+ }
+
+ st = port_close(w_test_port1);
+ if (st < 0)
+ return __LINE__;
+ st = port_close(w_test_port2);
+ if (st < 0)
+ return __LINE__;
+ st = port_close(cmd_port);
+ if (st < 0)
+ return __LINE__;
+ st = port_destroy(w_test_port1);
+ if (st < 0)
+ return __LINE__;
+ st = port_destroy(w_test_port2);
+ if (st < 0)
+ return __LINE__;
+ st = port_destroy(cmd_port);
+ if (st < 0)
+ return __LINE__;
+
+ return 0;
+}
+
+#define RUN_TEST(t) result = t(); if (result) goto fail
+
+int port_tests(void)
+{
+ int result;
+ int count = 3;
+ while (count--) {
+ RUN_TEST(single_thread_basic);
+ RUN_TEST(two_threads_basic);
+ RUN_TEST(group_basic);
+ }
+
+ printf("all tests passed\n");
+ return 0;
+fail:
+ printf("test failed at line %d\n", result);
+ return 1;
+}
+
+#undef RUN_TEST
diff --git a/src/bsp/lk/app/tests/printf_tests.c b/src/bsp/lk/app/tests/printf_tests.c
new file mode 100644
index 0000000..3a9747f
--- /dev/null
+++ b/src/bsp/lk/app/tests/printf_tests.c
@@ -0,0 +1,137 @@
+/*
+ * Copyright (c) 2008-2014 Travis Geiselbrecht
+ *
+ * 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 <app/tests.h>
+#include <err.h>
+#include <stdio.h>
+#include <string.h>
+#include <debug.h>
+
+int printf_tests(int argc, const cmd_args *argv)
+{
+ printf("printf tests\n");
+
+ printf("numbers:\n");
+ printf("int8: %hhd %hhd %hhd\n", -12, 0, 254);
+ printf("uint8: %hhu %hhu %hhu\n", -12, 0, 254);
+ printf("int16: %hd %hd %hd\n", -1234, 0, 1234);
+ printf("uint16:%hu %hu %hu\n", -1234, 0, 1234);
+ printf("int: %d %d %d\n", -12345678, 0, 12345678);
+ printf("uint: %u %u %u\n", -12345678, 0, 12345678);
+ printf("long: %ld %ld %ld\n", -12345678L, 0L, 12345678L);
+ printf("ulong: %lu %lu %lu\n", -12345678UL, 0UL, 12345678UL);
+
+ printf("longlong: %lli %lli %lli\n", -12345678LL, 0LL, 12345678LL);
+ printf("ulonglong: %llu %llu %llu\n", -12345678LL, 0LL, 12345678LL);
+ printf("ssize_t: %zd %zd %zd\n", (ssize_t)-12345678, (ssize_t)0, (ssize_t)12345678);
+ printf("usize_t: %zu %zu %zu\n", (size_t)-12345678, (size_t)0, (size_t)12345678);
+ printf("intmax_t: %jd %jd %jd\n", (intmax_t)-12345678, (intmax_t)0, (intmax_t)12345678);
+ printf("uintmax_t: %ju %ju %ju\n", (uintmax_t)-12345678, (uintmax_t)0, (uintmax_t)12345678);
+ printf("ptrdiff_t: %td %td %td\n", (ptrdiff_t)-12345678, (ptrdiff_t)0, (ptrdiff_t)12345678);
+ printf("ptrdiff_t (u): %tu %tu %tu\n", (ptrdiff_t)-12345678, (ptrdiff_t)0, (ptrdiff_t)12345678);
+
+ printf("hex:\n");
+ printf("uint8: %hhx %hhx %hhx\n", -12, 0, 254);
+ printf("uint16:%hx %hx %hx\n", -1234, 0, 1234);
+ printf("uint: %x %x %x\n", -12345678, 0, 12345678);
+ printf("ulong: %lx %lx %lx\n", -12345678UL, 0UL, 12345678UL);
+ printf("ulong: %X %X %X\n", -12345678, 0, 12345678);
+ printf("ulonglong: %llx %llx %llx\n", -12345678LL, 0LL, 12345678LL);
+ printf("usize_t: %zx %zx %zx\n", (size_t)-12345678, (size_t)0, (size_t)12345678);
+
+ printf("alt/sign:\n");
+ printf("uint: %#x %#X\n", 0xabcdef, 0xabcdef);
+ printf("int: %+d %+d\n", 12345678, -12345678);
+ printf("int: % d %+d\n", 12345678, 12345678);
+
+ printf("formatting\n");
+ printf("int: a%8da\n", 12345678);
+ printf("int: a%9da\n", 12345678);
+ printf("int: a%-9da\n", 12345678);
+ printf("int: a%10da\n", 12345678);
+ printf("int: a%-10da\n", 12345678);
+ printf("int: a%09da\n", 12345678);
+ printf("int: a%010da\n", 12345678);
+ printf("int: a%6da\n", 12345678);
+
+ printf("a%1sa\n", "b");
+ printf("a%9sa\n", "b");
+ printf("a%-9sa\n", "b");
+ printf("a%5sa\n", "thisisatest");
+
+ printf("%03d\n", -2); /* '-02' */
+ printf("%0+3d\n", -2); /* '-02' */
+ printf("%0+3d\n", 2); /* '+02' */
+ printf("%+3d\n", 2); /* ' +2' */
+ printf("% 3d\n", -2000); /* '-2000' */
+ printf("% 3d\n", 2000); /* ' 2000' */
+ printf("%+3d\n", 2000); /* '+2000' */
+ printf("%10s\n", "test"); /* ' test' */
+ printf("%010s\n", "test"); /* ' test' */
+ printf("%-10s\n", "test"); /* 'test ' */
+ printf("%-010s\n", "test"); /* 'test ' */
+
+ int err;
+
+ err = printf("a");
+ printf(" returned %d\n", err);
+ err = printf("ab");
+ printf(" returned %d\n", err);
+ err = printf("abc");
+ printf(" returned %d\n", err);
+ err = printf("abcd");
+ printf(" returned %d\n", err);
+ err = printf("abcde");
+ printf(" returned %d\n", err);
+ err = printf("abcdef");
+ printf(" returned %d\n", err);
+
+ /* make sure snprintf terminates at the right spot */
+ char buf[32];
+
+ memset(buf, 0, sizeof(buf));
+ err = sprintf(buf, "0123456789abcdef012345678");
+ printf("sprintf returns %d\n", err);
+ hexdump8(buf, sizeof(buf));
+
+ memset(buf, 0, sizeof(buf));
+ err = snprintf(buf, 15, "0123456789abcdef012345678");
+ printf("snprintf returns %d\n", err);
+ hexdump8(buf, sizeof(buf));
+
+ return NO_ERROR;
+}
+
+#include "float_test_vec.c"
+
+int printf_tests_float(int argc, const cmd_args *argv)
+{
+ printf("floating point printf tests\n");
+
+ for (size_t i = 0; i < float_test_vec_size; i++) {
+ PRINT_FLOAT;
+ }
+
+ return NO_ERROR;
+}
+
+
diff --git a/src/bsp/lk/app/tests/rules.mk b/src/bsp/lk/app/tests/rules.mk
new file mode 100644
index 0000000..aba98c7
--- /dev/null
+++ b/src/bsp/lk/app/tests/rules.mk
@@ -0,0 +1,23 @@
+LOCAL_DIR := $(GET_LOCAL_DIR)
+
+MODULE := $(LOCAL_DIR)
+
+MODULE_SRCS += \
+ $(LOCAL_DIR)/benchmarks.c \
+ $(LOCAL_DIR)/cache_tests.c \
+ $(LOCAL_DIR)/clock_tests.c \
+ $(LOCAL_DIR)/fibo.c \
+ $(LOCAL_DIR)/float.c \
+ $(LOCAL_DIR)/float_instructions.S \
+ $(LOCAL_DIR)/float_test_vec.c \
+ $(LOCAL_DIR)/mem_tests.c \
+ $(LOCAL_DIR)/printf_tests.c \
+ $(LOCAL_DIR)/tests.c \
+ $(LOCAL_DIR)/thread_tests.c \
+ $(LOCAL_DIR)/port_tests.c \
+
+MODULE_ARM_OVERRIDE_SRCS := \
+
+MODULE_COMPILEFLAGS += -Wno-format
+
+include make/module.mk
diff --git a/src/bsp/lk/app/tests/tests.c b/src/bsp/lk/app/tests/tests.c
new file mode 100644
index 0000000..4d5dcda
--- /dev/null
+++ b/src/bsp/lk/app/tests/tests.c
@@ -0,0 +1,52 @@
+/*
+ * Copyright (c) 2008 Travis Geiselbrecht
+ *
+ * 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 <app.h>
+#include <debug.h>
+#include <app/tests.h>
+#include <compiler.h>
+
+#if defined(WITH_LIB_CONSOLE)
+#include <lib/console.h>
+
+STATIC_COMMAND_START
+STATIC_COMMAND("printf_tests", "test printf", &printf_tests)
+STATIC_COMMAND("printf_tests_float", "test printf with floating point", &printf_tests_float)
+STATIC_COMMAND("thread_tests", "test the scheduler", &thread_tests)
+STATIC_COMMAND("port_tests", "test the ports", &port_tests)
+STATIC_COMMAND("clock_tests", "test clocks", &clock_tests)
+STATIC_COMMAND("bench", "miscellaneous benchmarks", &benchmarks)
+STATIC_COMMAND("fibo", "threaded fibonacci", &fibo)
+STATIC_COMMAND("spinner", "create a spinning thread", &spinner)
+STATIC_COMMAND_END(tests);
+
+#endif
+
+static void tests_init(const struct app_descriptor *app)
+{
+}
+
+APP_START(tests)
+.init = tests_init,
+.flags = 0,
+APP_END
+
diff --git a/src/bsp/lk/app/tests/thread_tests.c b/src/bsp/lk/app/tests/thread_tests.c
new file mode 100644
index 0000000..543bd52
--- /dev/null
+++ b/src/bsp/lk/app/tests/thread_tests.c
@@ -0,0 +1,666 @@
+/*
+ * Copyright (c) 2008-2015 Travis Geiselbrecht
+ *
+ * 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 <debug.h>
+#include <trace.h>
+#include <rand.h>
+#include <err.h>
+#include <assert.h>
+#include <string.h>
+#include <app/tests.h>
+#include <kernel/thread.h>
+#include <kernel/mutex.h>
+#include <kernel/semaphore.h>
+#include <kernel/event.h>
+#include <platform.h>
+
+static int sleep_thread(void *arg)
+{
+ for (;;) {
+ printf("sleeper %p\n", get_current_thread());
+ thread_sleep(rand() % 500);
+ }
+ return 0;
+}
+
+int sleep_test(void)
+{
+ int i;
+ for (i=0; i < 16; i++)
+ thread_detach_and_resume(thread_create("sleeper", &sleep_thread, NULL, DEFAULT_PRIORITY, DEFAULT_STACK_SIZE));
+ return 0;
+}
+
+static semaphore_t sem;
+static const int sem_total_its = 10000;
+static const int sem_thread_max_its = 1000;
+static const int sem_start_value = 10;
+static int sem_remaining_its = 0;
+static int sem_threads = 0;
+static mutex_t sem_test_mutex;
+
+static int semaphore_producer(void *unused)
+{
+ printf("semaphore producer %p starting up, running for %d iterations\n", get_current_thread(), sem_total_its);
+
+ for (int x = 0; x < sem_total_its; x++) {
+ sem_post(&sem, true);
+ }
+
+ return 0;
+}
+
+static int semaphore_consumer(void *unused)
+{
+ unsigned int iterations = 0;
+
+ mutex_acquire(&sem_test_mutex);
+ if (sem_remaining_its >= sem_thread_max_its) {
+ iterations = rand();
+ iterations %= sem_thread_max_its;
+ } else {
+ iterations = sem_remaining_its;
+ }
+ sem_remaining_its -= iterations;
+ mutex_release(&sem_test_mutex);
+
+ printf("semaphore consumer %p starting up, running for %u iterations\n", get_current_thread(), iterations);
+ for (unsigned int x = 0; x < iterations; x++)
+ sem_wait(&sem);
+ printf("semaphore consumer %p done\n", get_current_thread());
+ atomic_add(&sem_threads, -1);
+ return 0;
+}
+
+static int semaphore_test(void)
+{
+ static semaphore_t isem = SEMAPHORE_INITIAL_VALUE(isem, 99);
+ printf("preinitialized sempahore:\n");
+ hexdump(&isem, sizeof(isem));
+
+ sem_init(&sem, sem_start_value);
+ mutex_init(&sem_test_mutex);
+
+ sem_remaining_its = sem_total_its;
+ while (1) {
+ mutex_acquire(&sem_test_mutex);
+ if (sem_remaining_its) {
+ thread_detach_and_resume(thread_create("semaphore consumer", &semaphore_consumer, NULL, DEFAULT_PRIORITY, DEFAULT_STACK_SIZE));
+ atomic_add(&sem_threads, 1);
+ } else {
+ mutex_release(&sem_test_mutex);
+ break;
+ }
+ mutex_release(&sem_test_mutex);
+ }
+
+ thread_detach_and_resume(thread_create("semaphore producer", &semaphore_producer, NULL, DEFAULT_PRIORITY, DEFAULT_STACK_SIZE));
+
+ while (sem_threads)
+ thread_yield();
+
+ if (sem.count == sem_start_value)
+ printf("semaphore tests successfully complete\n");
+ else
+ printf("semaphore tests failed: %d != %d\n", sem.count, sem_start_value);
+
+ sem_destroy(&sem);
+ mutex_destroy(&sem_test_mutex);
+
+ return 0;
+}
+
+static int mutex_thread(void *arg)
+{
+ int i;
+ const int iterations = 1000000;
+
+ static volatile int shared = 0;
+
+ mutex_t *m = (mutex_t *)arg;
+
+ printf("mutex tester thread %p starting up, will go for %d iterations\n", get_current_thread(), iterations);
+
+ for (i = 0; i < iterations; i++) {
+ mutex_acquire(m);
+
+ if (shared != 0)
+ panic("someone else has messed with the shared data\n");
+
+ shared = (intptr_t)get_current_thread();
+ thread_yield();
+ shared = 0;
+
+ mutex_release(m);
+ thread_yield();
+ }
+
+ return 0;
+}
+
+static int mutex_timeout_thread(void *arg)
+{
+ mutex_t *timeout_mutex = (mutex_t *)arg;
+ status_t err;
+
+ printf("mutex_timeout_thread acquiring mutex %p with 1 second timeout\n", timeout_mutex);
+ err = mutex_acquire_timeout(timeout_mutex, 1000);
+ if (err == ERR_TIMED_OUT)
+ printf("mutex_acquire_timeout returns with TIMEOUT\n");
+ else
+ printf("mutex_acquire_timeout returns %d\n", err);
+
+ return err;
+}
+
+static int mutex_zerotimeout_thread(void *arg)
+{
+ mutex_t *timeout_mutex = (mutex_t *)arg;
+ status_t err;
+
+ printf("mutex_zerotimeout_thread acquiring mutex %p with zero second timeout\n", timeout_mutex);
+ err = mutex_acquire_timeout(timeout_mutex, 0);
+ if (err == ERR_TIMED_OUT)
+ printf("mutex_acquire_timeout returns with TIMEOUT\n");
+ else
+ printf("mutex_acquire_timeout returns %d\n", err);
+
+ return err;
+}
+
+int mutex_test(void)
+{
+ static mutex_t imutex = MUTEX_INITIAL_VALUE(imutex);
+ printf("preinitialized mutex:\n");
+ hexdump(&imutex, sizeof(imutex));
+
+ mutex_t m;
+ mutex_init(&m);
+
+ thread_t *threads[5];
+
+ for (uint i=0; i < countof(threads); i++) {
+ threads[i] = thread_create("mutex tester", &mutex_thread, &m, DEFAULT_PRIORITY, DEFAULT_STACK_SIZE);
+ thread_resume(threads[i]);
+ }
+
+ for (uint i=0; i < countof(threads); i++) {
+ thread_join(threads[i], NULL, INFINITE_TIME);
+ }
+
+ printf("done with simple mutex tests\n");
+
+ printf("testing mutex timeout\n");
+
+ mutex_t timeout_mutex;
+
+ mutex_init(&timeout_mutex);
+ mutex_acquire(&timeout_mutex);
+
+ for (uint i=0; i < 2; i++) {
+ threads[i] = thread_create("mutex timeout tester", &mutex_timeout_thread, (void *)&timeout_mutex, DEFAULT_PRIORITY, DEFAULT_STACK_SIZE);
+ thread_resume(threads[i]);
+ }
+
+ for (uint i=2; i < 4; i++) {
+ threads[i] = thread_create("mutex timeout tester", &mutex_zerotimeout_thread, (void *)&timeout_mutex, DEFAULT_PRIORITY, DEFAULT_STACK_SIZE);
+ thread_resume(threads[i]);
+ }
+
+ thread_sleep(5000);
+ mutex_release(&timeout_mutex);
+
+ for (uint i=0; i < 4; i++) {
+ thread_join(threads[i], NULL, INFINITE_TIME);
+ }
+
+ printf("done with mutex tests\n");
+
+ mutex_destroy(&timeout_mutex);
+
+ return 0;
+}
+
+static event_t e;
+
+static int event_signaller(void *arg)
+{
+ printf("event signaller pausing\n");
+ thread_sleep(1000);
+
+// for (;;) {
+ printf("signalling event\n");
+ event_signal(&e, true);
+ printf("done signalling event\n");
+ thread_yield();
+// }
+
+ return 0;
+}
+
+static int event_waiter(void *arg)
+{
+ int count = (intptr_t)arg;
+
+ printf("event waiter starting\n");
+
+ while (count > 0) {
+ printf("%p: waiting on event...\n", get_current_thread());
+ if (event_wait(&e) < 0) {
+ printf("%p: event_wait() returned error\n", get_current_thread());
+ return -1;
+ }
+ printf("%p: done waiting on event...\n", get_current_thread());
+ thread_yield();
+ count--;
+ }
+
+ return 0;
+}
+
+void event_test(void)
+{
+ thread_t *threads[5];
+
+ static event_t ievent = EVENT_INITIAL_VALUE(ievent, true, 0x1234);
+ printf("preinitialized event:\n");
+ hexdump(&ievent, sizeof(ievent));
+
+ printf("event tests starting\n");
+
+ /* make sure signalling the event wakes up all the threads */
+ event_init(&e, false, 0);
+ threads[0] = thread_create("event signaller", &event_signaller, NULL, DEFAULT_PRIORITY, DEFAULT_STACK_SIZE);
+ threads[1] = thread_create("event waiter 0", &event_waiter, (void *)2, DEFAULT_PRIORITY, DEFAULT_STACK_SIZE);
+ threads[2] = thread_create("event waiter 1", &event_waiter, (void *)2, DEFAULT_PRIORITY, DEFAULT_STACK_SIZE);
+ threads[3] = thread_create("event waiter 2", &event_waiter, (void *)2, DEFAULT_PRIORITY, DEFAULT_STACK_SIZE);
+ threads[4] = thread_create("event waiter 3", &event_waiter, (void *)2, DEFAULT_PRIORITY, DEFAULT_STACK_SIZE);
+
+ for (uint i = 0; i < countof(threads); i++)
+ thread_resume(threads[i]);
+
+ thread_sleep(2000);
+ printf("destroying event\n");
+ event_destroy(&e);
+
+ for (uint i = 0; i < countof(threads); i++)
+ thread_join(threads[i], NULL, INFINITE_TIME);
+
+ /* make sure signalling the event wakes up precisely one thread */
+ event_init(&e, false, EVENT_FLAG_AUTOUNSIGNAL);
+ threads[0] = thread_create("event signaller", &event_signaller, NULL, DEFAULT_PRIORITY, DEFAULT_STACK_SIZE);
+ threads[1] = thread_create("event waiter 0", &event_waiter, (void *)99, DEFAULT_PRIORITY, DEFAULT_STACK_SIZE);
+ threads[2] = thread_create("event waiter 1", &event_waiter, (void *)99, DEFAULT_PRIORITY, DEFAULT_STACK_SIZE);
+ threads[3] = thread_create("event waiter 2", &event_waiter, (void *)99, DEFAULT_PRIORITY, DEFAULT_STACK_SIZE);
+ threads[4] = thread_create("event waiter 3", &event_waiter, (void *)99, DEFAULT_PRIORITY, DEFAULT_STACK_SIZE);
+
+ for (uint i = 0; i < countof(threads); i++)
+ thread_resume(threads[i]);
+
+ thread_sleep(2000);
+ event_destroy(&e);
+
+ for (uint i = 0; i < countof(threads); i++)
+ thread_join(threads[i], NULL, INFINITE_TIME);
+
+ printf("event tests done\n");
+}
+
+static int quantum_tester(void *arg)
+{
+ for (;;) {
+ printf("%p: in this thread. rq %d\n", get_current_thread(), get_current_thread()->remaining_quantum);
+ }
+ return 0;
+}
+
+void quantum_test(void)
+{
+ thread_detach_and_resume(thread_create("quantum tester 0", &quantum_tester, NULL, DEFAULT_PRIORITY, DEFAULT_STACK_SIZE));
+ thread_detach_and_resume(thread_create("quantum tester 1", &quantum_tester, NULL, DEFAULT_PRIORITY, DEFAULT_STACK_SIZE));
+ thread_detach_and_resume(thread_create("quantum tester 2", &quantum_tester, NULL, DEFAULT_PRIORITY, DEFAULT_STACK_SIZE));
+ thread_detach_and_resume(thread_create("quantum tester 3", &quantum_tester, NULL, DEFAULT_PRIORITY, DEFAULT_STACK_SIZE));
+}
+
+static event_t context_switch_event;
+static event_t context_switch_done_event;
+
+static int context_switch_tester(void *arg)
+{
+ int i;
+ uint total_count = 0;
+ const int iter = 100000;
+ int thread_count = (intptr_t)arg;
+
+ event_wait(&context_switch_event);
+
+ uint count = arch_cycle_count();
+ for (i = 0; i < iter; i++) {
+ thread_yield();
+ }
+ total_count += arch_cycle_count() - count;
+ thread_sleep(1000);
+ printf("took %u cycles to yield %d times, %u per yield, %u per yield per thread\n",
+ total_count, iter, total_count / iter, total_count / iter / thread_count);
+
+ event_signal(&context_switch_done_event, true);
+
+ return 0;
+}
+
+void context_switch_test(void)
+{
+ event_init(&context_switch_event, false, 0);
+ event_init(&context_switch_done_event, false, 0);
+
+ thread_detach_and_resume(thread_create("context switch idle", &context_switch_tester, (void *)1, DEFAULT_PRIORITY, DEFAULT_STACK_SIZE));
+ thread_sleep(100);
+ event_signal(&context_switch_event, true);
+ event_wait(&context_switch_done_event);
+ thread_sleep(100);
+
+ event_unsignal(&context_switch_event);
+ event_unsignal(&context_switch_done_event);
+ thread_detach_and_resume(thread_create("context switch 2a", &context_switch_tester, (void *)2, DEFAULT_PRIORITY, DEFAULT_STACK_SIZE));
+ thread_detach_and_resume(thread_create("context switch 2b", &context_switch_tester, (void *)2, DEFAULT_PRIORITY, DEFAULT_STACK_SIZE));
+ thread_sleep(100);
+ event_signal(&context_switch_event, true);
+ event_wait(&context_switch_done_event);
+ thread_sleep(100);
+
+ event_unsignal(&context_switch_event);
+ event_unsignal(&context_switch_done_event);
+ thread_detach_and_resume(thread_create("context switch 4a", &context_switch_tester, (void *)4, DEFAULT_PRIORITY, DEFAULT_STACK_SIZE));
+ thread_detach_and_resume(thread_create("context switch 4b", &context_switch_tester, (void *)4, DEFAULT_PRIORITY, DEFAULT_STACK_SIZE));
+ thread_detach_and_resume(thread_create("context switch 4c", &context_switch_tester, (void *)4, DEFAULT_PRIORITY, DEFAULT_STACK_SIZE));
+ thread_detach_and_resume(thread_create("context switch 4d", &context_switch_tester, (void *)4, DEFAULT_PRIORITY, DEFAULT_STACK_SIZE));
+ thread_sleep(100);
+ event_signal(&context_switch_event, true);
+ event_wait(&context_switch_done_event);
+ thread_sleep(100);
+}
+
+static volatile int atomic;
+static volatile int atomic_count;
+
+static int atomic_tester(void *arg)
+{
+ int add = (intptr_t)arg;
+ int i;
+
+ const int iter = 10000000;
+
+ TRACEF("add %d, %d iterations\n", add, iter);
+
+ for (i=0; i < iter; i++) {
+ atomic_add(&atomic, add);
+ }
+
+ int old = atomic_add(&atomic_count, -1);
+ TRACEF("exiting, old count %d\n", old);
+
+ return 0;
+}
+
+static void atomic_test(void)
+{
+ atomic = 0;
+ atomic_count = 8;
+
+ printf("testing atomic routines\n");
+
+ thread_t *threads[8];
+ threads[0] = thread_create("atomic tester 1", &atomic_tester, (void *)1, LOW_PRIORITY, DEFAULT_STACK_SIZE);
+ threads[1] = thread_create("atomic tester 1", &atomic_tester, (void *)1, LOW_PRIORITY, DEFAULT_STACK_SIZE);
+ threads[2] = thread_create("atomic tester 1", &atomic_tester, (void *)1, LOW_PRIORITY, DEFAULT_STACK_SIZE);
+ threads[3] = thread_create("atomic tester 1", &atomic_tester, (void *)1, LOW_PRIORITY, DEFAULT_STACK_SIZE);
+ threads[4] = thread_create("atomic tester 2", &atomic_tester, (void *)-1, LOW_PRIORITY, DEFAULT_STACK_SIZE);
+ threads[5] = thread_create("atomic tester 2", &atomic_tester, (void *)-1, LOW_PRIORITY, DEFAULT_STACK_SIZE);
+ threads[6] = thread_create("atomic tester 2", &atomic_tester, (void *)-1, LOW_PRIORITY, DEFAULT_STACK_SIZE);
+ threads[7] = thread_create("atomic tester 2", &atomic_tester, (void *)-1, LOW_PRIORITY, DEFAULT_STACK_SIZE);
+
+ /* start all the threads */
+ for (uint i = 0; i < countof(threads); i++)
+ thread_resume(threads[i]);
+
+ /* wait for them to all stop */
+ for (uint i = 0; i < countof(threads); i++) {
+ thread_join(threads[i], NULL, INFINITE_TIME);
+ }
+
+ printf("atomic count == %d (should be zero)\n", atomic);
+}
+
+static volatile int preempt_count;
+
+static int preempt_tester(void *arg)
+{
+ spin(1000000);
+
+ printf("exiting ts %lld\n", current_time_hires());
+
+ atomic_add(&preempt_count, -1);
+#undef COUNT
+
+ return 0;
+}
+
+static void preempt_test(void)
+{
+ /* create 5 threads, let them run. If the system is properly timer preempting,
+ * the threads should interleave each other at a fine enough granularity so
+ * that they complete at roughly the same time. */
+ printf("testing preemption\n");
+
+ preempt_count = 5;
+
+ for (int i = 0; i < preempt_count; i++)
+ thread_detach_and_resume(thread_create("preempt tester", &preempt_tester, NULL, LOW_PRIORITY, DEFAULT_STACK_SIZE));
+
+ while (preempt_count > 0) {
+ thread_sleep(1000);
+ }
+
+ printf("done with preempt test, above time stamps should be very close\n");
+
+ /* do the same as above, but mark the threads as real time, which should
+ * effectively disable timer based preemption for them. They should
+ * complete in order, about a second apart. */
+ printf("testing real time preemption\n");
+
+ preempt_count = 5;
+
+ for (int i = 0; i < preempt_count; i++) {
+ thread_t *t = thread_create("preempt tester", &preempt_tester, NULL, LOW_PRIORITY, DEFAULT_STACK_SIZE);
+ thread_set_real_time(t);
+ thread_detach_and_resume(t);
+ }
+
+ while (preempt_count > 0) {
+ thread_sleep(1000);
+ }
+
+ printf("done with real-time preempt test, above time stamps should be 1 second apart\n");
+}
+
+static int join_tester(void *arg)
+{
+ long val = (long)arg;
+
+ printf("\t\tjoin tester starting\n");
+ thread_sleep(500);
+ printf("\t\tjoin tester exiting with result %ld\n", val);
+
+ return val;
+}
+
+static int join_tester_server(void *arg)
+{
+ int ret;
+ status_t err;
+ thread_t *t;
+
+ printf("\ttesting thread_join/thread_detach\n");
+
+ printf("\tcreating and waiting on thread to exit with thread_join\n");
+ t = thread_create("join tester", &join_tester, (void *)1, DEFAULT_PRIORITY, DEFAULT_STACK_SIZE);
+ thread_resume(t);
+ ret = 99;
+ printf("\tthread magic is 0x%x (should be 0x%x)\n", t->magic, THREAD_MAGIC);
+ err = thread_join(t, &ret, INFINITE_TIME);
+ printf("\tthread_join returns err %d, retval %d\n", err, ret);
+ printf("\tthread magic is 0x%x (should be 0)\n", t->magic);
+
+ printf("\tcreating and waiting on thread to exit with thread_join, after thread has exited\n");
+ t = thread_create("join tester", &join_tester, (void *)2, DEFAULT_PRIORITY, DEFAULT_STACK_SIZE);
+ thread_resume(t);
+ thread_sleep(1000); // wait until thread is already dead
+ ret = 99;
+ printf("\tthread magic is 0x%x (should be 0x%x)\n", t->magic, THREAD_MAGIC);
+ err = thread_join(t, &ret, INFINITE_TIME);
+ printf("\tthread_join returns err %d, retval %d\n", err, ret);
+ printf("\tthread magic is 0x%x (should be 0)\n", t->magic);
+
+ printf("\tcreating a thread, detaching it, let it exit on its own\n");
+ t = thread_create("join tester", &join_tester, (void *)3, DEFAULT_PRIORITY, DEFAULT_STACK_SIZE);
+ thread_detach(t);
+ thread_resume(t);
+ thread_sleep(1000); // wait until the thread should be dead
+ printf("\tthread magic is 0x%x (should be 0)\n", t->magic);
+
+ printf("\tcreating a thread, detaching it after it should be dead\n");
+ t = thread_create("join tester", &join_tester, (void *)4, DEFAULT_PRIORITY, DEFAULT_STACK_SIZE);
+ thread_resume(t);
+ thread_sleep(1000); // wait until thread is already dead
+ printf("\tthread magic is 0x%x (should be 0x%x)\n", t->magic, THREAD_MAGIC);
+ thread_detach(t);
+ printf("\tthread magic is 0x%x\n", t->magic);
+
+ printf("\texiting join tester server\n");
+
+ return 55;
+}
+
+static void join_test(void)
+{
+ int ret;
+ status_t err;
+ thread_t *t;
+
+ printf("testing thread_join/thread_detach\n");
+
+ printf("creating thread join server thread\n");
+ t = thread_create("join tester server", &join_tester_server, (void *)1, DEFAULT_PRIORITY, DEFAULT_STACK_SIZE);
+ thread_resume(t);
+ ret = 99;
+ err = thread_join(t, &ret, INFINITE_TIME);
+ printf("thread_join returns err %d, retval %d (should be 0 and 55)\n", err, ret);
+}
+
+static void spinlock_test(void)
+{
+ spin_lock_saved_state_t state;
+ spin_lock_t lock;
+
+ spin_lock_init(&lock);
+
+ // verify basic functionality (single core)
+ printf("testing spinlock:\n");
+ ASSERT(!spin_lock_held(&lock));
+ ASSERT(!arch_ints_disabled());
+ spin_lock_irqsave(&lock, state);
+ ASSERT(arch_ints_disabled());
+ ASSERT(spin_lock_held(&lock));
+ spin_unlock_irqrestore(&lock, state);
+ ASSERT(!spin_lock_held(&lock));
+ ASSERT(!arch_ints_disabled());
+ printf("seems to work\n");
+
+#define COUNT (1024*1024)
+ uint32_t c = arch_cycle_count();
+ for (uint i = 0; i < COUNT; i++) {
+ spin_lock(&lock);
+ spin_unlock(&lock);
+ }
+ c = arch_cycle_count() - c;
+
+ printf("%u cycles to acquire/release lock %u times (%u cycles per)\n", c, COUNT, c / COUNT);
+
+ c = arch_cycle_count();
+ for (uint i = 0; i < COUNT; i++) {
+ spin_lock_irqsave(&lock, state);
+ spin_unlock_irqrestore(&lock, state);
+ }
+ c = arch_cycle_count() - c;
+
+ printf("%u cycles to acquire/release lock w/irqsave %u times (%u cycles per)\n", c, COUNT, c / COUNT);
+#undef COUNT
+}
+
+int thread_tests(int argc, const cmd_args *argv)
+{
+ mutex_test();
+ semaphore_test();
+ event_test();
+
+ spinlock_test();
+ atomic_test();
+
+ thread_sleep(200);
+ context_switch_test();
+
+ preempt_test();
+
+ join_test();
+
+ return 0;
+}
+
+static int spinner_thread(void *arg)
+{
+ for (;;)
+ ;
+
+ return 0;
+}
+
+int spinner(int argc, const cmd_args *argv)
+{
+ if (argc < 2) {
+ printf("not enough args\n");
+ printf("usage: %s <priority> <rt>\n", argv[0].str);
+ return -1;
+ }
+
+ thread_t *t = thread_create("spinner", spinner_thread, NULL, argv[1].u, DEFAULT_STACK_SIZE);
+ if (!t)
+ return ERR_NO_MEMORY;
+
+ if (argc >= 3 && !strcmp(argv[2].str, "rt")) {
+ thread_set_real_time(t);
+ }
+ thread_resume(t);
+
+ return 0;
+}
+
+/* vim: set ts=4 sw=4 noexpandtab: */
diff --git a/src/bsp/lk/app/udctest/rules.mk b/src/bsp/lk/app/udctest/rules.mk
new file mode 100644
index 0000000..542785a
--- /dev/null
+++ b/src/bsp/lk/app/udctest/rules.mk
@@ -0,0 +1,9 @@
+LOCAL_DIR := $(GET_LOCAL_DIR)
+
+MODULE := $(LOCAL_DIR)
+
+MODULE_SRCS += \
+ $(LOCAL_DIR)/udctest.c
+
+include make/module.mk
+
diff --git a/src/bsp/lk/app/udctest/udctest.c b/src/bsp/lk/app/udctest/udctest.c
new file mode 100644
index 0000000..4ccf997
--- /dev/null
+++ b/src/bsp/lk/app/udctest/udctest.c
@@ -0,0 +1,110 @@
+/*
+ * Copyright (c) 2015 Brian Swetland
+ *
+ * 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 <app.h>
+#include <debug.h>
+#include <string.h>
+#include <stdlib.h>
+#include <printf.h>
+#include <dev/udc.h>
+
+udc_request_t *txreq;
+udc_request_t *rxreq;
+udc_endpoint_t *txept;
+udc_endpoint_t *rxept;
+
+static char rxbuf[4096];
+
+static void rx_complete(udc_request_t *req, unsigned actual, int status) {
+ //printf("rx done %d %d\n", actual, status);
+ if (status == 0) {
+ udc_request_queue(rxept, rxreq);
+ }
+}
+
+static void tx_complete(udc_request_t *req, unsigned actual, int status) {
+ //printf("tx done %d %d\n", actual, status);
+ if (status == 0) {
+ udc_request_queue(txept, txreq);
+ }
+}
+
+static void udctest_notify(udc_gadget_t *gadget, unsigned event) {
+ printf("event %d\n", event);
+ if (event == UDC_EVENT_ONLINE) {
+ udc_request_queue(rxept, rxreq);
+ udc_request_queue(txept, txreq);
+ }
+}
+
+static udc_device_t udctest_device = {
+ .vendor_id = 0x18d1,
+ .product_id = 0xdb01,
+ .version_id = 0x0100,
+ .manufacturer = "Frobozz Magic USB Device Company",
+ .product = "Frobozzco USB Device",
+ .serialno = "00000005",
+};
+
+static udc_endpoint_t *udctest_endpoints[2];
+
+static udc_gadget_t udctest_gadget = {
+ .notify = udctest_notify,
+ .ifc_class = 0xFF,
+ .ifc_subclass = 0x42,
+ .ifc_protocol = 0x01,
+ .ifc_endpoints = 2,
+ .ifc_string = "string",
+ .ept = udctest_endpoints,
+};
+
+static void udctest_init(const struct app_descriptor *app)
+{
+ printf("usbtest_init()\n");
+ udc_init(&udctest_device);
+ udctest_endpoints[0] = txept = udc_endpoint_alloc(UDC_BULK_IN, 512);
+ udctest_endpoints[1] = rxept = udc_endpoint_alloc(UDC_BULK_OUT, 512);
+ txreq = udc_request_alloc();
+ rxreq = udc_request_alloc();
+ rxreq->buffer = rxbuf;
+ rxreq->length = sizeof(rxbuf);
+ rxreq->complete = rx_complete;
+ txreq->buffer = rxbuf;
+ txreq->length = sizeof(rxbuf);
+ txreq->complete = tx_complete;
+ udc_register_gadget(&udctest_gadget);
+}
+
+static void udctest_entry(const struct app_descriptor *app, void *args)
+{
+ printf("udctest_entry()\n");
+ udc_start();
+}
+
+APP_START(usbtest)
+ .init = udctest_init,
+ .entry = udctest_entry,
+APP_END
+
+
diff --git a/src/bsp/lk/app/usbtest/descriptor.c b/src/bsp/lk/app/usbtest/descriptor.c
new file mode 100644
index 0000000..97ebab7
--- /dev/null
+++ b/src/bsp/lk/app/usbtest/descriptor.c
@@ -0,0 +1,110 @@
+#include <err.h>
+#include <debug.h>
+#include <stdio.h>
+#include <target.h>
+#include <compiler.h>
+#include <dev/usb.h>
+#include <dev/usbc.h>
+#include <hw/usb.h>
+
+#define W(w) (w & 0xff), (w >> 8)
+#define W3(w) (w & 0xff), ((w >> 8) & 0xff), ((w >> 16) & 0xff)
+
+/* top level device descriptor */
+static const uint8_t dev_descr[] = {
+ 0x12, /* descriptor length */
+ DEVICE, /* Device Descriptor type */
+ W(0x0200), /* USB Version */
+ 0xff, /* class */
+ 0xff, /* subclass */
+ 0xff, /* protocol */
+ 64, /* max packet size, ept0 */
+ W(0x9999), /* vendor */
+ W(0x9999), /* product */
+ W(0x9999), /* release */
+ 0x0, /* manufacturer string */
+ 0x0, /* product string */
+ 0x0, /* serialno string */
+ 0x1, /* num configs */
+};
+
+/* high/low speed device qualifier */
+static const uint8_t devqual_descr[] = {
+ 0x0a, /* len */
+ DEVICE_QUALIFIER, /* Device Qualifier type */
+ W(0x0200), /* USB version */
+ 0x00, /* class */
+ 0x00, /* subclass */
+ 0x00, /* protocol */
+ 64, /* max packet size, ept0 */
+ 0x01, /* num configs */
+ 0x00 /* reserved */
+};
+
+static const uint8_t cfg_descr[] = {
+ 0x09, /* Length of Cfg Descr */
+ CONFIGURATION, /* Type of Cfg Descr */
+ W(0x09), /* Total Length (incl ifc, ept) */
+ 0x00, /* # Interfaces */
+ 0x01, /* Cfg Value */
+ 0x00, /* Cfg String */
+ 0xc0, /* Attributes -- self powered */
+ 250, /* Power Consumption - 500mA */
+};
+
+static const uchar langid[] = { 0x04, 0x03, 0x09, 0x04 };
+
+static const uint8_t if_descriptor_lowspeed[] = {
+ 0x09, /* length */
+ INTERFACE, /* type */
+ 0x01, /* interface num */
+ 0x00, /* alternates */
+ 0x02, /* endpoint count */
+ 0xff, /* interface class */
+ 0xff, /* interface subclass */
+ 0x00, /* interface protocol */
+ 0x00, /* string index */
+
+ /* endpoint 1 IN */
+ 0x07, /* length */
+ ENDPOINT, /* type */
+ 0x81, /* address: 1 IN */
+ 0x02, /* type: bulk */
+ W(64), /* max packet size: 64 */
+ 00, /* interval */
+
+ /* endpoint 1 OUT */
+ 0x07, /* length */
+ ENDPOINT, /* type */
+ 0x01, /* address: 1 OUT */
+ 0x02, /* type: bulk */
+ W(64), /* max packet size: 64 */
+ 00, /* interval */
+};
+
+static usb_config config = {
+ .lowspeed = {
+ .device = USB_DESC_STATIC(dev_descr),
+ .device_qual = USB_DESC_STATIC(devqual_descr),
+ .config = USB_DESC_STATIC(cfg_descr),
+ },
+ .highspeed = {
+ .device = USB_DESC_STATIC(dev_descr),
+ .device_qual = USB_DESC_STATIC(devqual_descr),
+ .config = USB_DESC_STATIC(cfg_descr),
+ },
+
+ .langid = USB_DESC_STATIC(langid),
+};
+
+void usbtest_usb_setup(void)
+{
+ usb_setup(&config);
+ printf("appending interfaces\n");
+ usb_append_interface_lowspeed(if_descriptor_lowspeed, sizeof(if_descriptor_lowspeed));
+ usb_append_interface_highspeed(if_descriptor_lowspeed, sizeof(if_descriptor_lowspeed));
+ usbc_setup_endpoint(1, USB_OUT, 64);
+ usbc_setup_endpoint(1, USB_IN, 64);
+}
+
+// vim: set ts=4 sw=4 expandtab:
diff --git a/src/bsp/lk/app/usbtest/rules.mk b/src/bsp/lk/app/usbtest/rules.mk
new file mode 100644
index 0000000..67c7717
--- /dev/null
+++ b/src/bsp/lk/app/usbtest/rules.mk
@@ -0,0 +1,13 @@
+LOCAL_DIR := $(GET_LOCAL_DIR)
+
+MODULE := $(LOCAL_DIR)
+
+MODULE_DEPS += \
+ dev/usb
+
+MODULE_SRCS += \
+ $(LOCAL_DIR)/usbtest.c \
+ $(LOCAL_DIR)/descriptor.c \
+
+include make/module.mk
+
diff --git a/src/bsp/lk/app/usbtest/usbtest.c b/src/bsp/lk/app/usbtest/usbtest.c
new file mode 100644
index 0000000..db2b218
--- /dev/null
+++ b/src/bsp/lk/app/usbtest/usbtest.c
@@ -0,0 +1,129 @@
+#include <app.h>
+#include <debug.h>
+#include <err.h>
+#include <string.h>
+#include <stdlib.h>
+#include <dev/usb.h>
+#include <dev/usbc.h>
+#include <kernel/debug.h>
+#include <kernel/thread.h>
+#include <kernel/event.h>
+
+#define LOCAL_TRACE 1
+
+extern void usbtest_usb_setup(void);
+
+static status_t rx_callback(ep_t endpoint, struct usbc_transfer *transfer);
+static usbc_transfer_t rx;
+static uint8_t rxbuf[4096];
+static volatile bool rxqueued;
+
+static status_t tx_callback(ep_t endpoint, struct usbc_transfer *transfer);
+static usbc_transfer_t tx;
+static uint8_t txbuf[4095];
+static volatile bool txqueued;
+
+static event_t testevent;
+
+/* RX */
+static void queue_rx_transfer(void)
+{
+ rx.callback = rx_callback;
+ rx.result = 0;
+ rx.buf = rxbuf;
+ rx.buflen = sizeof(rxbuf);
+ rx.bufpos = 0;
+ rx.extra = NULL;
+
+ memset(rxbuf, 0x99, sizeof(rxbuf));
+
+ rxqueued = true;
+ usbc_queue_rx(1, &rx);
+}
+
+static status_t rx_callback(ep_t endpoint, struct usbc_transfer *transfer)
+{
+ LTRACEF("ep %u, transfer %p\n", endpoint, transfer);
+
+ rxqueued = false;
+ event_signal(&testevent, false);
+
+ return NO_ERROR;
+}
+
+/* TX */
+static void queue_tx_transfer(void)
+{
+ tx.callback = tx_callback;
+ tx.result = 0;
+ tx.buf = txbuf;
+ tx.buflen = sizeof(txbuf);
+ tx.bufpos = 0;
+ tx.extra = NULL;
+
+ for (uint i = 0; i < sizeof(txbuf); i++)
+ txbuf[i] = i * 3;
+
+ txqueued = true;
+ usbc_queue_tx(1, &tx);
+}
+
+static status_t tx_callback(ep_t endpoint, struct usbc_transfer *transfer)
+{
+ LTRACEF("ep %u, transfer %p\n", endpoint, transfer);
+
+ txqueued = false;
+ event_signal(&testevent, false);
+
+ return NO_ERROR;
+}
+
+static void usbtest_init(const struct app_descriptor *app)
+{
+ LTRACE_ENTRY;
+ event_init(&testevent, false, EVENT_FLAG_AUTOUNSIGNAL);
+ usbtest_usb_setup();
+ LTRACE_EXIT;
+}
+
+static void usbtest_entry(const struct app_descriptor *app, void *args)
+{
+ LTRACE_ENTRY;
+
+ TRACEF("starting usb stack\n");
+ usb_start();
+
+ // XXX get callback from stack
+ thread_sleep(2000);
+
+ TRACEF("queuing transfers\n");
+ queue_rx_transfer();
+ queue_tx_transfer();
+
+ while (event_wait(&testevent) == NO_ERROR) {
+ if (!rxqueued) {
+ /* dump the state of the transfer */
+ LTRACEF("rx transfer completed\n");
+ usbc_dump_transfer(&rx);
+ hexdump8(rx.buf, MIN(128, rx.bufpos));
+
+ queue_rx_transfer();
+ }
+ if (!txqueued) {
+ /* dump the state of the transfer */
+ LTRACEF("tx transfer completed\n");
+ usbc_dump_transfer(&tx);
+
+ queue_tx_transfer();
+ }
+ }
+
+ LTRACE_EXIT;
+}
+
+APP_START(usbtest)
+ .init = usbtest_init,
+ .entry = usbtest_entry,
+APP_END
+
+
diff --git a/src/bsp/lk/app/zynq-common/init.c b/src/bsp/lk/app/zynq-common/init.c
new file mode 100644
index 0000000..bec6ad8
--- /dev/null
+++ b/src/bsp/lk/app/zynq-common/init.c
@@ -0,0 +1,215 @@
+/*
+ * Copyright (c) 2014-2015 Travis Geiselbrecht
+ *
+ * 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 <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <debug.h>
+#include <trace.h>
+#include <lk/init.h>
+#include <lib/bootargs.h>
+#include <lib/bootimage.h>
+#include <lib/ptable.h>
+#include <lib/sysparam.h>
+#include <lib/watchdog.h>
+#include <dev/spiflash.h>
+#include <kernel/vm.h>
+#include <kernel/thread.h>
+
+#include <platform/gem.h>
+#include <platform/fpga.h>
+
+#if WITH_LIB_MINIP
+#include <lib/minip.h>
+#endif
+
+#define BLOCK_DEVICE_NAME "spi0"
+
+static void zynq_common_target_init(uint level)
+{
+ status_t err;
+
+ /* zybo has a spiflash on qspi */
+ spiflash_detect();
+
+ bdev_t *spi = bio_open(BLOCK_DEVICE_NAME);
+ if (spi) {
+ /* find or create a partition table at the start of flash */
+ if (ptable_scan(BLOCK_DEVICE_NAME, 0) < 0) {
+ ptable_create_default(BLOCK_DEVICE_NAME, 0);
+ }
+
+ struct ptable_entry entry = { 0 };
+
+ /* find and recover sysparams */
+ if (ptable_find("sysparam", &entry) < 0) {
+ /* didn't find sysparam partition, create it */
+ ptable_add("sysparam", 0x1000, 0);
+ ptable_find("sysparam", &entry);
+ }
+
+ if (entry.length > 0) {
+ sysparam_scan(spi, entry.offset, entry.length);
+
+#if SYSPARAM_ALLOW_WRITE
+ /* for testing purposes, put at least one sysparam value in */
+ if (sysparam_add("dummy", "value", sizeof("value")) >= 0) {
+ sysparam_write();
+ }
+#endif
+
+#if LK_DEBUGLEVEL > 1
+ sysparam_dump(true);
+#endif
+ }
+
+ /* create bootloader partition if it does not exist */
+ ptable_add("bootloader", 0x40000, 0);
+
+#if LK_DEBUGLEVEL > 1
+ printf("flash partition table:\n");
+ ptable_dump();
+#endif
+ }
+
+ /* recover boot arguments */
+ const char *cmdline = bootargs_get_command_line();
+ if (cmdline) {
+ printf("lk command line: '%s'\n", cmdline);
+ }
+
+ /* see if we came from a bootimage */
+ const char *device;
+ uint64_t bootimage_phys;
+ size_t bootimage_size;
+ if (bootargs_get_bootimage_pointer(&bootimage_phys, &bootimage_size, &device) >= 0) {
+ bootimage_t *bi = NULL;
+ bool put_bio_memmap = false;
+
+ printf("our bootimage is at device '%s', phys 0x%llx, size %zx\n", device, bootimage_phys, bootimage_size);
+
+ /* if the bootimage we came from is in physical memory, find it */
+ if (!strcmp(device, "pmem")) {
+ void *ptr = paddr_to_kvaddr(bootimage_phys);
+ if (ptr) {
+ bootimage_open(ptr, bootimage_size, &bi);
+ }
+ } else if (!strcmp(device, BLOCK_DEVICE_NAME)) {
+ /* we were loaded from spi flash, go look at it to see if we can find it */
+ if (spi) {
+ void *ptr = 0;
+ int err = bio_ioctl(spi, BIO_IOCTL_GET_MEM_MAP, (void *)&ptr);
+ if (err >= 0) {
+ put_bio_memmap = true;
+ ptr = (uint8_t *)ptr + bootimage_phys;
+ bootimage_open(ptr, bootimage_size, &bi);
+ }
+ }
+ }
+
+ /* did we find the bootimage? */
+ if (bi) {
+ /* we have a valid bootimage, find the fpga section */
+ const void *fpga_ptr;
+ size_t fpga_len;
+
+ if (bootimage_get_file_section(bi, TYPE_FPGA_IMAGE, &fpga_ptr, &fpga_len) >= 0) {
+ /* we have a fpga image */
+
+ /* lookup the physical address of the bitfile */
+ paddr_t pa = kvaddr_to_paddr((void *)fpga_ptr);
+ if (pa != 0) {
+ /* program the fpga with it*/
+ printf("loading fpga image at %p (phys 0x%lx), len %zx\n", fpga_ptr, pa, fpga_len);
+ zynq_reset_fpga();
+ err = zynq_program_fpga(pa, fpga_len);
+ if (err < 0) {
+ printf("error %d loading fpga\n", err);
+ }
+ printf("fpga image loaded\n");
+ }
+ }
+ }
+
+ /* if we memory mapped it, put the block device back to block mode */
+ if (put_bio_memmap) {
+ /* HACK: for completely non-obvious reasons, need to sleep here for a little bit.
+ * Experimentally it was found that if fetching the fpga image out of qspi flash,
+ * for a period of time after the transfer is complete, there appears to be stray
+ * AXI bus transactions that cause the system to hang if immediately after
+ * programming the qspi memory aperture is destroyed.
+ */
+ thread_sleep(10);
+
+ bio_ioctl(spi, BIO_IOCTL_PUT_MEM_MAP, NULL);
+ }
+ }
+
+#if WITH_LIB_MINIP
+ /* pull some network stack related params out of the sysparam block */
+ uint8_t mac_addr[6];
+ uint32_t ip_addr = IPV4_NONE;
+ uint32_t ip_mask = IPV4_NONE;
+ uint32_t ip_gateway = IPV4_NONE;
+
+ if (sysparam_read("net0.mac_addr", mac_addr, sizeof(mac_addr)) < (ssize_t)sizeof(mac_addr)) {
+ /* couldn't find eth address, make up a random one */
+ for (size_t i = 0; i < sizeof(mac_addr); i++) {
+ mac_addr[i] = rand() & 0xff;
+ }
+
+ /* unicast and locally administered */
+ mac_addr[0] &= ~(1<<0);
+ mac_addr[0] |= (1<<1);
+ }
+
+ uint8_t use_dhcp = 0;
+ sysparam_read("net0.use_dhcp", &use_dhcp, sizeof(use_dhcp));
+ sysparam_read("net0.ip_addr", &ip_addr, sizeof(ip_addr));
+ sysparam_read("net0.ip_mask", &ip_mask, sizeof(ip_mask));
+ sysparam_read("net0.ip_gateway", &ip_gateway, sizeof(ip_gateway));
+
+ minip_set_macaddr(mac_addr);
+ gem_set_macaddr(mac_addr);
+
+ if (!use_dhcp && ip_addr != IPV4_NONE) {
+ minip_init(gem_send_raw_pkt, NULL, ip_addr, ip_mask, ip_gateway);
+ } else {
+ /* Configure IP stack and hook to the driver */
+ minip_init_dhcp(gem_send_raw_pkt, NULL);
+ }
+ gem_set_callback(minip_rx_driver_callback);
+#endif
+}
+
+/* init after target_init() */
+LK_INIT_HOOK(app_zynq_common, &zynq_common_target_init, LK_INIT_LEVEL_TARGET);
+
+/* watchdog setup, as early as possible */
+static void zynq_watchdog_init(uint level)
+{
+ /* start the watchdog timer */
+ watchdog_hw_set_enabled(true);
+}
+
+LK_INIT_HOOK(app_zynq_common_watchdog, &zynq_watchdog_init, LK_INIT_LEVEL_KERNEL);
+
diff --git a/src/bsp/lk/app/zynq-common/rules.mk b/src/bsp/lk/app/zynq-common/rules.mk
new file mode 100644
index 0000000..f2f1b17
--- /dev/null
+++ b/src/bsp/lk/app/zynq-common/rules.mk
@@ -0,0 +1,17 @@
+LOCAL_DIR := $(GET_LOCAL_DIR)
+
+MODULE := $(LOCAL_DIR)
+
+MODULE_DEPS += \
+ lib/bootargs \
+ lib/bootimage \
+ lib/sysparam \
+ lib/ptable
+
+GLOBAL_DEFINES += \
+ SYSPARAM_ALLOW_WRITE=1
+
+MODULE_SRCS += \
+ $(LOCAL_DIR)/init.c
+
+include make/module.mk