liuzhuoran2@huawei.com

Signed-off-by: l00645337 <liuzhuoran2@huawei.com>

BUILD.gn

@@ -0,0 +1,19 @@
+# Copyright (C) 2022 Huawei Technologies Co., Ltd.
+# Licensed under the Mulan PSL v2.
+# You can use this software according to the terms and conditions of the Mulan PSL v2.
+# You may obtain a copy of Mulan PSL v2 at:
+#     http://license.coscl.org.cn/MulanPSL2
+# THIS SOFTWARE IS PROVIDED ON AN "AS IS" BASIS, WITHOUT WARRANTIES OF ANY KIND, EITHER EXPRESS OR
+# IMPLIED, INCLUDING BUT NOT LIMITED TO NON-INFRINGEMENT, MERCHANTABILITY OR FIT FOR A PARTICULAR
+# PURPOSE.
+# See the Mulan PSL v2 for more details.
+
+import("//build/lite/config/component/lite_component.gni")
+
+copy("tee_copy") {
+    sources = [ "//device/hisilicon/hispark_taurus/tee/teehm.img" ]
+    outputs = [ "$root_out_dir/{{source_file_part}}" ]
+}
+
+group("tee_os_framework") {
+}

LICENSE

@@ -0,0 +1,127 @@
+                     木兰宽松许可证, 第2版
+
+   木兰宽松许可证， 第2版
+   2020年1月 http://license.coscl.org.cn/MulanPSL2
+
+
+   您对“软件”的复制、使用、修改及分发受木兰宽松许可证，第2版（“本许可证”）的如下条款的约束：
+
+   0. 定义
+
+      “软件”是指由“贡献”构成的许可在“本许可证”下的程序和相关文档的集合。
+
+      “贡献”是指由任一“贡献者”许可在“本许可证”下的受版权法保护的作品。
+
+      “贡献者”是指将受版权法保护的作品许可在“本许可证”下的自然人或“法人实体”。
+
+      “法人实体”是指提交贡献的机构及其“关联实体”。
+
+      “关联实体”是指，对“本许可证”下的行为方而言，控制、受控制或与其共同受控制的机构，此处的控制是指有受控方或共同受控方至少50%直接或间接的投票权、资金或其他有价证券。
+
+   1. 授予版权许可
+
+      每个“贡献者”根据“本许可证”授予您永久性的、全球性的、免费的、非独占的、不可撤销的版权许可，您可以复制、使用、修改、分发其“贡献”，不论修改与否。
+
+   2. 授予专利许可
+
+      每个“贡献者”根据“本许可证”授予您永久性的、全球性的、免费的、非独占的、不可撤销的（根据本条规定撤销除外）专利许可，供您制造、委托制造、使用、许诺销售、销售、进口其“贡献”或以其他方式转移其“贡献”。前述专利许可仅限于“贡献者”现在或将来拥有或控制的其“贡献”本身或其“贡献”与许可“贡献”时的“软件”结合而将必然会侵犯的专利权利要求，不包括对“贡献”的修改或包含“贡献”的其他结合。如果您或您的“关联实体”直接或间接地，就“软件”或其中的“贡献”对任何人发起专利侵权诉讼（包括反诉或交叉诉讼）或其他专利维权行动，指控其侵犯专利权，则“本许可证”授予您对“软件”的专利许可自您提起诉讼或发起维权行动之日终止。
+
+   3. 无商标许可
+
+      “本许可证”不提供对“贡献者”的商品名称、商标、服务标志或产品名称的商标许可，但您为满足第4条规定的声明义务而必须使用除外。
+
+   4. 分发限制
+
+      您可以在任何媒介中将“软件”以源程序形式或可执行形式重新分发，不论修改与否，但您必须向接收者提供“本许可证”的副本，并保留“软件”中的版权、商标、专利及免责声明。
+
+   5. 免责声明与责任限制
+
+      “软件”及其中的“贡献”在提供时不带任何明示或默示的担保。在任何情况下，“贡献者”或版权所有者不对任何人因使用“软件”或其中的“贡献”而引发的任何直接或间接损失承担责任，不论因何种原因导致或者基于何种法律理论，即使其曾被建议有此种损失的可能性。
+
+   6. 语言
+      “本许可证”以中英文双语表述，中英文版本具有同等法律效力。如果中英文版本存在任何冲突不一致，以中文版为准。
+
+   条款结束
+
+   如何将木兰宽松许可证，第2版，应用到您的软件
+
+   如果您希望将木兰宽松许可证，第2版，应用到您的新软件，为了方便接收者查阅，建议您完成如下三步：
+
+      1， 请您补充如下声明中的空白，包括软件名、软件的首次发表年份以及您作为版权人的名字；
+
+      2， 请您在软件包的一级目录下创建以“LICENSE”为名的文件，将整个许可证文本放入该文件中；
+
+      3， 请将如下声明文本放入每个源文件的头部注释中。
+
+   Copyright (c) [Year] [name of copyright holder]
+   [Software Name] is licensed under Mulan PSL v2.
+   You can use this software according to the terms and conditions of the Mulan PSL v2.
+   You may obtain a copy of Mulan PSL v2 at:
+            http://license.coscl.org.cn/MulanPSL2
+   THIS SOFTWARE IS PROVIDED ON AN "AS IS" BASIS, WITHOUT WARRANTIES OF ANY KIND, EITHER EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO NON-INFRINGEMENT, MERCHANTABILITY OR FIT FOR A PARTICULAR PURPOSE.
+   See the Mulan PSL v2 for more details.
+
+
+                     Mulan Permissive Software License，Version 2
+
+   Mulan Permissive Software License，Version 2 (Mulan PSL v2)
+   January 2020 http://license.coscl.org.cn/MulanPSL2
+
+   Your reproduction, use, modification and distribution of the Software shall be subject to Mulan PSL v2 (this License) with the following terms and conditions:
+
+   0. Definition
+
+      Software means the program and related documents which are licensed under this License and comprise all Contribution(s).
+
+      Contribution means the copyrightable work licensed by a particular Contributor under this License.
+
+      Contributor means the Individual or Legal Entity who licenses its copyrightable work under this License.
+
+      Legal Entity means the entity making a Contribution and all its Affiliates.
+
+      Affiliates means entities that control, are controlled by, or are under common control with the acting entity under this License, ‘control’ means direct or indirect ownership of at least fifty percent (50%) of the voting power, capital or other securities of controlled or commonly controlled entity.
+
+   1. Grant of Copyright License
+
+      Subject to the terms and conditions of this License, each Contributor hereby grants to you a perpetual, worldwide, royalty-free, non-exclusive, irrevocable copyright license to reproduce, use, modify, or distribute its Contribution, with modification or not.
+
+   2. Grant of Patent License
+
+      Subject to the terms and conditions of this License, each Contributor hereby grants to you a perpetual, worldwide, royalty-free, non-exclusive, irrevocable (except for revocation under this Section) patent license to make, have made, use, offer for sale, sell, import or otherwise transfer its Contribution, where such patent license is only limited to the patent claims owned or controlled by such Contributor now or in future which will be necessarily infringed by its Contribution alone, or by combination of the Contribution with the Software to which the Contribution was contributed. The patent license shall not apply to any modification of the Contribution, and any other combination which includes the Contribution. If you or your Affiliates directly or indirectly institute patent litigation (including a cross claim or counterclaim in a litigation) or other patent enforcement activities against any individual or entity by alleging that the Software or any Contribution in it infringes patents, then any patent license granted to you under this License for the Software shall terminate as of the date such litigation or activity is filed or taken.
+
+   3. No Trademark License
+
+      No trademark license is granted to use the trade names, trademarks, service marks, or product names of Contributor, except as required to fulfill notice requirements in Section 4.
+
+   4. Distribution Restriction
+
+      You may distribute the Software in any medium with or without modification, whether in source or executable forms, provided that you provide recipients with a copy of this License and retain copyright, patent, trademark and disclaimer statements in the Software.
+
+   5. Disclaimer of Warranty and Limitation of Liability
+
+      THE SOFTWARE AND CONTRIBUTION IN IT ARE PROVIDED WITHOUT WARRANTIES OF ANY KIND, EITHER EXPRESS OR IMPLIED. IN NO EVENT SHALL ANY CONTRIBUTOR OR COPYRIGHT HOLDER BE LIABLE TO YOU FOR ANY DAMAGES, INCLUDING, BUT NOT LIMITED TO ANY DIRECT, OR INDIRECT, SPECIAL OR CONSEQUENTIAL DAMAGES ARISING FROM YOUR USE OR INABILITY TO USE THE SOFTWARE OR THE CONTRIBUTION IN IT, NO MATTER HOW IT’S CAUSED OR BASED ON WHICH LEGAL THEORY, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGES.
+
+   6. Language
+
+      THIS LICENSE IS WRITTEN IN BOTH CHINESE AND ENGLISH, AND THE CHINESE VERSION AND ENGLISH VERSION SHALL HAVE THE SAME LEGAL EFFECT. IN THE CASE OF DIVERGENCE BETWEEN THE CHINESE AND ENGLISH VERSIONS, THE CHINESE VERSION SHALL PREVAIL.
+
+   END OF THE TERMS AND CONDITIONS
+
+   How to Apply the Mulan Permissive Software License，Version 2 (Mulan PSL v2) to Your Software
+
+      To apply the Mulan PSL v2 to your work, for easy identification by recipients, you are suggested to complete following three steps:
+
+      i Fill in the blanks in following statement, including insert your software name, the year of the first publication of your software, and your name identified as the copyright owner;
+
+      ii Create a file named “LICENSE” which contains the whole context of this License in the first directory of your software package;
+
+      iii Attach the statement to the appropriate annotated syntax at the beginning of each source file.
+
+
+   Copyright (c) [Year] [name of copyright holder]
+   [Software Name] is licensed under Mulan PSL v2.
+   You can use this software according to the terms and conditions of the Mulan PSL v2.
+   You may obtain a copy of Mulan PSL v2 at:
+               http://license.coscl.org.cn/MulanPSL2
+   THIS SOFTWARE IS PROVIDED ON AN "AS IS" BASIS, WITHOUT WARRANTIES OF ANY KIND, EITHER EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO NON-INFRINGEMENT, MERCHANTABILITY OR FIT FOR A PARTICULAR PURPOSE.
+   See the Mulan PSL v2 for more details.

README_zh.md

@@ -0,0 +1,60 @@
+# tee_tee_os_framework仓介绍<a name="ZH-CN_TOPIC_0000001148528849"></a>
+
+-   [简介](#section11660541593)
+-   [目录结构](#section161941989596)
+-   [相关仓](#section1371113476307)
+
+## 简介<a name="section11660541593"></a>
+
+tee_tee_os_framework仓二级目录结构如下：
+
+<a name="table2977131081412"></a>
+<table><thead align="left"><tr id="row7977610131417"><th class="cellrowborder" valign="top" width="50%" id="mcps1.2.3.1.1"><p id="p18792459121314"><a name="p18792459121314"></a><a name="p18792459121314"></a>二级目录</p>
+</th>
+<th class="cellrowborder" valign="top" width="50%" id="mcps1.2.3.1.2"><p id="p77921459191317"><a name="p77921459191317"></a><a name="p77921459191317"></a>描述</p>
+</th>
+
+<tr id="row6978161091412"><td class="cellrowborder" valign="top" width="50%" headers="mcps1.2.3.1.1 "><p id="p64006181102"><a name="p64006181102"></a><a name="p64006181102"></a>sample</p>
+</td>
+<td class="cellrowborder" valign="top" width="50%" headers="mcps1.2.3.1.2 "><p id="p7456843192018"><a name="p7456843192018"></a><a name="p7456843192018"></a>实例代码，包括teeloader和teed两部分</p>
+</td>
+</tr>
+
+<tr id="row6978201031415"><td class="cellrowborder" valign="top" width="50%" headers="mcps1.2.3.1.1 "><p id="p1978910485104"><a name="p1978910485104"></a><a name="p1978910485104"></a>test</p>
+</td>
+<td class="cellrowborder" valign="top" width="50%" headers="mcps1.2.3.1.2 "><p id="p1059035912204"><a name="p1059035912204"></a><a name="p1059035912204"></a>测试相关代码</p>
+</td>
+</tr>
+</tbody>
+</table>
+
+sample目录下包括teeloader和teed两部分，仅作示例代码，实际未使用。
+-  teeloader
+
+    用于加载TEE侧安全镜像到RAM（Random Access Memory）。
+
+-  teed
+
+    作为适配在ATF（Arm Trusted Firmware）中的模块，提供可信执行环境（Trusted Execution Environment，TEE）与富执行环境（Rich Execution Environment，REE）切换的服务。
+
+test目录下放置测试相关代码，用于测试TEE可信执行环境子系统基础能力。
+
+## 目录结构<a name="section161941989596"></a>
+
+```
+base/tee/tee_os_framework
+│ 
+├── sample                              # 示例代码
+│   │  
+│   ├── teeloader                       # teeloader示例代码
+│   │  
+│   └── teed                            # teed示例代码
+│ 
+└── test                                # 测试相关代码
+```
+
+## 相关仓<a name="section1371113476307"></a>
+
+**tee子系统**
+
+**tee_os_framework**

bundle.json

@@ -0,0 +1,26 @@
+{
+    "name": "@openharmony/tee_os_framework",
+    "description": "tee os_framework",
+    "version": "1.0.0",
+    "license": "Mulan PSL v2",
+    "component": {
+      "name": "tee_os_framework",
+      "subsystem": "tee",
+      "syscap": [],
+      "features": [],
+      "adapted_system_type": [ "small" ],
+      "rom":"1635KB",
+      "ram":"16M", 
+      "deps": {
+        "components": [],
+        "third_party": []
+      },
+      "build": {
+        "sub_component": [
+          "//base/tee/tee_os_framework/:tee_os_framework"
+        ],
+        "inner_kits": [],
+        "test": []
+      }
+    }
+  }

sample/teed/include/tee.h

@@ -0,0 +1,83 @@
+/*
+ * Copyright (C) 2022 Huawei Technologies Co., Ltd.
+ * Licensed under the Mulan PSL v2.
+ * You can use this software according to the terms and conditions of the Mulan PSL v2.
+ * You may obtain a copy of Mulan PSL v2 at:
+ *     http://license.coscl.org.cn/MulanPSL2
+ * THIS SOFTWARE IS PROVIDED ON AN "AS IS" BASIS, WITHOUT WARRANTIES OF ANY KIND, EITHER EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO NON-INFRINGEMENT, MERCHANTABILITY OR FIT FOR A PARTICULAR
+ * PURPOSE.
+ * See the Mulan PSL v2 for more details.
+ */
+
+#ifndef TEED_TEE_H
+#define TEED_TEE_H
+
+/*
+ * SMC function IDs that TEE uses to signal various forms of completions
+ * to the secure payload dispatcher.
+ */
+#define TEE_ENTRY_DONE            0xf2000000
+#define TEE_ON_DONE               0xf2000001
+#define TEE_OFF_DONE              0xf2000002
+#define TEE_SUSPEND_DONE          0xf2000003
+#define TEE_RESUME_DONE           0xf2000004
+#define TEE_PREEMPTED             0xf2000005
+#define TEE_ABORT_DONE            0xf2000007
+#define TEE_SYSTEM_OFF_DONE       0xf2000008
+#define TEE_SYSTEM_RESET_DONE     0xf2000009
+
+/*
+ * Function identifiers to handle S-EL1 interrupt through the synchronous
+ * handling model. If the TEE was previously interrupted then control has to
+ * be returned to the TEED after handling the interrupt else execution can
+ * remain in the TEE.
+ */
+#define TEE_HANDLED_S_EL1_INTR    0xf2000006
+
+/* SMC function ID that TEE uses to request service from secure monitor */
+#define TEE_GET_ARGS              0xf2001000
+
+/*
+ * Identifiers for various TEE services. Corresponding function IDs (whether
+ * fast or yielding) are generated by macros defined below
+ */
+#define TEE_ADD                              0x2000
+#define TEE_SUB                              0x2001
+#define TEE_MUL                              0x2002
+#define TEE_DIV                              0x2003
+#define TEE_HANDLE_SEL1_INTR_AND_RETURN      0x2004
+
+/*
+ * Identify a TEE service from function ID filtering the last 16 bits from the
+ * SMC function ID
+ */
+#define TEE_BARE_FID(fid)  ((fid) & 0xffffu)
+
+/*
+ * Generate function IDs for TEE services to be used in SMC calls, by
+ * appropriately setting bit 31 to differentiate yielding and fast SMC calls
+ */
+#define TEE_YIELD_FID(fid) ((TEE_BARE_FID(fid) | 0x72000000u))
+#define TEE_FAST_FID(fid)  ((TEE_BARE_FID(fid) | 0x72000000u) | (1u << 31))
+
+/* SMC function ID to request a previously preempted yielding smc */
+#define TEE_FID_RESUME     TEE_YIELD_FID(0x3000u)
+/*
+ * SMC function ID to request abortion of a previously preempted yielding SMC. A
+ * fast SMC is used so that the TEE abort handler does not have to be
+ * reentrant.
+ */
+#define TEE_FID_ABORT       TEE_FAST_FID(0x3001u)
+
+/*
+ * Total number of function IDs implemented for services offered to NS clients.
+ * The function IDs are defined above
+ */
+#define TEE_NUM_FID         0x5
+
+/* TEE implementation version numbers */
+#define TEE_VERSION_MAJOR   0x0 /* Major version */
+#define TEE_VERSION_MINOR   0x1 /* Minor version */
+
+#endif /* TEE_H */

sample/teed/include/teed_common.h

@@ -0,0 +1,27 @@
+/*
+ * Copyright (C) 2022 Huawei Technologies Co., Ltd.
+ * Licensed under the Mulan PSL v2.
+ * You can use this software according to the terms and conditions of the Mulan PSL v2.
+ * You may obtain a copy of Mulan PSL v2 at:
+ *     http://license.coscl.org.cn/MulanPSL2
+ * THIS SOFTWARE IS PROVIDED ON AN "AS IS" BASIS, WITHOUT WARRANTIES OF ANY KIND, EITHER EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO NON-INFRINGEMENT, MERCHANTABILITY OR FIT FOR A PARTICULAR
+ * PURPOSE.
+ * See the Mulan PSL v2 for more details.
+ */
+
+#ifndef TEED_COMMON_H
+#define TEED_COMMON_H
+
+#include <teed_private.h>
+
+void teed_init_tee_ep_state(struct entry_point_info *tee_entry_point,
+				      uint32_t rw,
+				      uintptr_t pc,
+				      tee_context_t *tee_ctx);
+
+uint64_t teed_synchronous_sp_entry(tee_context_t *tee_ctx);
+
+void __dead2 teed_synchronous_sp_exit(const tee_context_t *tee_ctx, uint64_t ret);
+
+#endif

sample/teed/include/teed_helpers.h

@@ -0,0 +1,21 @@
+/*
+ * Copyright (C) 2022 Huawei Technologies Co., Ltd.
+ * Licensed under the Mulan PSL v2.
+ * You can use this software according to the terms and conditions of the Mulan PSL v2.
+ * You may obtain a copy of Mulan PSL v2 at:
+ *     http://license.coscl.org.cn/MulanPSL2
+ * THIS SOFTWARE IS PROVIDED ON AN "AS IS" BASIS, WITHOUT WARRANTIES OF ANY KIND, EITHER EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO NON-INFRINGEMENT, MERCHANTABILITY OR FIT FOR A PARTICULAR
+ * PURPOSE.
+ * See the Mulan PSL v2 for more details.
+ */
+
+#ifndef TEED_HELPERS_H
+#define TEED_HELPERS_H
+
+#include <stdint.h>
+
+uint64_t teed_enter_sp(uint64_t *rt_context);
+void __dead2 teed_exit_sp(uint64_t rt_context, uint64_t ret);
+
+#endif

sample/teed/include/teed_private.h

@@ -0,0 +1,304 @@
+/*
+ * Copyright (C) 2022 Huawei Technologies Co., Ltd.
+ * Licensed under the Mulan PSL v2.
+ * You can use this software according to the terms and conditions of the Mulan PSL v2.
+ * You may obtain a copy of Mulan PSL v2 at:
+ *     http://license.coscl.org.cn/MulanPSL2
+ * THIS SOFTWARE IS PROVIDED ON AN "AS IS" BASIS, WITHOUT WARRANTIES OF ANY KIND, EITHER EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO NON-INFRINGEMENT, MERCHANTABILITY OR FIT FOR A PARTICULAR
+ * PURPOSE.
+ * See the Mulan PSL v2 for more details.
+ */
+
+#ifndef TEED_PRIVATE_H
+#define TEED_PRIVATE_H
+
+#include <arch.h>
+#include <context.h>
+#include <interrupt_mgmt.h>
+#include <platform_def.h>
+#include <psci.h>
+
+/*
+ * Secure Payload PM state information e.g. SP is suspended, uninitialised etc
+ * and macros to access the state information in the per-cpu 'state' flags
+ */
+#define TEE_PSTATE_OFF        0
+#define TEE_PSTATE_ON         1
+#define TEE_PSTATE_SUSPEND    2
+#define TEE_PSTATE_SHIFT      0u
+#define TEE_PSTATE_MASK       0x3u
+#define get_tee_pstate(state) (((state) >> TEE_PSTATE_SHIFT) & TEE_PSTATE_MASK)
+
+#define clr_tee_pstate(state)                        \
+	do {                                              \
+		((state) &= ~(TEE_PSTATE_MASK <<     \
+		TEE_PSTATE_SHIFT));                  \
+	} while (0)
+
+#define set_tee_pstate(st, pst)                      \
+	do {                                              \
+		clr_tee_pstate(st);                  \
+		(st) |= ((pst) & TEE_PSTATE_MASK) << \
+		TEE_PSTATE_SHIFT;                    \
+	} while (0)
+
+/* smc id added for TEE */
+#define TEE_STD_REE_SIQ                     0xb200000a
+#define TEE_STD_CRASH                       0xb200000b
+#define TEE_ENTRY_DONE_AARCH32              0xb2000000
+#define TEE_ON_DONE_AARCH32                 0xb2000001
+#define TEE_OFF_DONE_AARCH32                0xb2000002
+#define TEE_SUSPEND_DONE_AARCH32            0xb2000003
+#define TEE_RESUME_DONE_AARCH32             0xb2000004
+#define TEE_PREEMPTED_AARCH32               0xb2000005
+#define TEE_HANDLED_S_EL1_FIQ_AARCH32       0xb2000006
+
+/*
+ * Function identifiers to send smc request to SP from NS and the the repsonse
+ * to NS from SP
+ */
+#define TEE_STD_REQUEST                     0xb2000008
+#define TEE_STD_RESPONSE                    0xb2000009
+
+#define CPU_INIT_DONE                            1
+#define CPU_IDLE_STATE                           0
+#define CPU_SUSPEND_STATE                        1
+/*
+ * This flag is used by the TEED to determine if the TEE is servicing a yielding
+ * SMC request prior to programming the next entry into the TEE e.g. if TEE
+ * execution is preempted by a non-secure interrupt and handed control to the
+ * normal world. If another request which is distinct from what the TEE was
+ * previously doing arrives, then this flag will be help the TEED to either
+ * reject the new request or service it while ensuring that the previous context
+ * is not corrupted.
+ */
+#define YIELD_SMC_ACTIVE_FLAG_SHIFT             2u
+#define YIELD_SMC_ACTIVE_FLAG_MASK              1u
+#define SMC_INACTIVE                            0
+#define SMC_ACTIVE                              1
+#define get_yield_smc_active_flag(state)                                  \
+		(((state) >> YIELD_SMC_ACTIVE_FLAG_SHIFT)                 \
+		 & YIELD_SMC_ACTIVE_FLAG_MASK)
+
+#define set_yield_smc_active_flag(state)                                  \
+	do {                                                              \
+		((state) |=                                               \
+		1u << YIELD_SMC_ACTIVE_FLAG_SHIFT);                       \
+	} while (0)
+
+#define clr_yield_smc_active_flag(state)                                  \
+	do {                                                              \
+		((state) &=                                               \
+		~(YIELD_SMC_ACTIVE_FLAG_MASK                              \
+		<< YIELD_SMC_ACTIVE_FLAG_SHIFT));                         \
+	} while (0)
+/*
+ * This flag is used by the TEED to determine if the TEE has crashed in any way
+ * and it is impossible to continue. The flag will disable forwarding any calls
+ * to secure os system.
+ */
+#define INIT_CONTEXT_NOT_SAVED                0x0
+#define INIT_CONTEXT_SAVED                    0x1
+#define INVALID_PC_ADDR                       0x0
+#define INVALID_C_RT_CTX                      0x0
+#define STD_NO_CRASH_FLAG                     0x0
+#define SECURE_WORLD_FLAG                     0x0
+#define TEE_SETUP_FAIL                        0x1
+#define STD_SMC_CRASH_FLAG_SHIFT              0x3u
+#define STD_SMC_CRASH_FLAG_MASK               0x1u
+#define get_std_crash_flag(state)                                              \
+		(((state) >> STD_SMC_CRASH_FLAG_SHIFT) &                       \
+		STD_SMC_CRASH_FLAG_MASK)
+
+#define set_std_crash_flag(state)                                              \
+	do {                                                                   \
+		((state) |=                                                    \
+		1 << STD_SMC_CRASH_FLAG_SHIFT);                                \
+	} while (0)
+
+/* Secure Payload execution state information i.e. aarch32 or aarch64 */
+#define TEE_AARCH32		MODE_RW_32
+#define TEE_AARCH64		MODE_RW_64
+
+/* The SPD should know the type of Secure Payload */
+#define TEE_TYPE_UP		PSCI_TOS_NOT_UP_MIG_CAP
+#define TEE_TYPE_UPM    PSCI_TOS_UP_MIG_CAP
+#define TEE_TYPE_MP		PSCI_TOS_NOT_PRESENT_MP
+
+/*
+ * Secure Payload migrate type information as known to the SPD. We assume that
+ * the SPD is dealing with an MP Secure Payload.
+ */
+#define TEE_MIGRATE_INFO		TEE_TYPE_MP
+
+/*
+ * Number of cpus that the present on this platform. Rely on a topology
+ * tree to determine this in the future to avoid assumptions about mpidr
+ * allocation
+ */
+#define TEED_CORE_COUNT		PLATFORM_CORE_COUNT
+
+/*
+ * Constants that allow assembler code to preserve callee-saved registers of the
+ * C runtime context while performing a security state switch.
+ */
+#define TEED_C_RT_CTX_X19          0x0
+#define TEED_C_RT_CTX_X20          0x8
+#define TEED_C_RT_CTX_X21          0x10
+#define TEED_C_RT_CTX_X22          0x18
+#define TEED_C_RT_CTX_X23          0x20
+#define TEED_C_RT_CTX_X24          0x28
+#define TEED_C_RT_CTX_X25          0x30
+#define TEED_C_RT_CTX_X26          0x38
+#define TEED_C_RT_CTX_X27          0x40
+#define TEED_C_RT_CTX_X28          0x48
+#define TEED_C_RT_CTX_X29          0x50
+#define TEED_C_RT_CTX_X30          0x58
+#define TEED_C_RT_CTX_SIZE         0x60
+#define TEED_C_RT_CTX_ENTRIES      (TEED_C_RT_CTX_SIZE >> DWORD_SHIFT)
+
+/*
+ * Constants that allow assembler code to preserve caller-saved registers of the
+ * SP context while performing a TEE preemption.
+ * Note: These offsets have to match with the offsets for the corresponding
+ * registers in cpu_context as we are using memcpy to copy the values from
+ * cpu_context to sp_ctx.
+ */
+#define TEED_SP_CTX_X0             0x0
+#define TEED_SP_CTX_X1             0x8
+#define TEED_SP_CTX_X2             0x10
+#define TEED_SP_CTX_X3             0x18
+#define TEED_SP_CTX_X4             0x20
+#define TEED_SP_CTX_X5             0x28
+#define TEED_SP_CTX_X6             0x30
+#define TEED_SP_CTX_X7             0x38
+#define TEED_SP_CTX_X8             0x40
+#define TEED_SP_CTX_X9             0x48
+#define TEED_SP_CTX_X10            0x50
+#define TEED_SP_CTX_X11            0x58
+#define TEED_SP_CTX_X12            0x60
+#define TEED_SP_CTX_X13            0x68
+#define TEED_SP_CTX_X14            0x70
+#define TEED_SP_CTX_X15            0x78
+#define TEED_SP_CTX_X16            0x80
+#define TEED_SP_CTX_X17            0x88
+#define TEED_SP_CTX_SIZE           0x90
+#define TEED_SP_CTX_ENTRIES        (TEED_SP_CTX_SIZE >> DWORD_SHIFT)
+
+#ifndef __ASSEMBLY__
+
+#include <stdint.h>
+
+/*
+ * The number of arguments to save during a SMC call for TEE.
+ * Currently only x1 and x2 are used by TEE.
+ */
+#define TEE_NUM_ARGS              0x2
+
+/* AArch64 callee saved general purpose register context structure. */
+DEFINE_REG_STRUCT(c_rt_regs, TEED_C_RT_CTX_ENTRIES);
+
+/*
+ * Compile time assertion to ensure that both the compiler and linker
+ * have the same double word aligned view of the size of the C runtime
+ * register context.
+ */
+CASSERT(TEED_C_RT_CTX_SIZE == sizeof(c_rt_regs_t), \
+	assert_spd_c_rt_regs_size_mismatch);
+
+/* SEL1 Secure payload (SP) caller saved register context structure. */
+DEFINE_REG_STRUCT(sp_ctx_regs, TEED_SP_CTX_ENTRIES);
+
+/*
+ * Compile time assertion to ensure that both the compiler and linker
+ * have the same double word aligned view of the size of the C runtime
+ * register context.
+ */
+CASSERT(TEED_SP_CTX_SIZE == sizeof(sp_ctx_regs_t), \
+	assert_spd_sp_regs_size_mismatch);
+
+/*
+ * Structure which helps the SPD to maintain the per-cpu state of the SP.
+ * 'spsr_el3' - temporary copy to allow S-EL1 interrupt handling when
+ *                    the TEE has been preempted.
+ * 'elr_el3'  - temporary copy to allow S-EL1 interrupt handling when
+ *                    the TEE has been preempted.
+ * 'state'          - collection of flags to track SP state e.g. on/off
+ * 'mpidr'          - mpidr to associate a context with a cpu
+ * 'rt_context'       - stack address to restore C runtime context from after
+ *                    returning from a synchronous entry into the SP.
+ * 'cpu_context'        - space to maintain SP architectural state
+ * 'saved_tee_args' - space to store arguments for TEE arithmetic operations
+ *                    which will queried using the TEE_GET_ARGS SMC by TEE.
+ * 'sp_ctx'         - space to save the SEL1 Secure Payload(SP) caller saved
+ *                    register context after it has been preempted by an EL3
+ *                    routed NS interrupt and when a Secure Interrupt is taken
+ *                    to SP.
+ */
+typedef struct tee_context {
+	uint64_t elr_el3;
+	uint32_t spsr_el3;
+	uint32_t state;
+	uint64_t mpidr;
+	uint64_t rt_context;
+	cpu_context_t cpu_context;
+	uint64_t saved_tee_args[TEE_NUM_ARGS];
+} tee_context_t;
+
+typedef struct tee_vectors {
+	uint32_t yield_smc_entry;
+	uint32_t fast_smc_entry;
+	uint32_t cpu_on_entry;
+	uint32_t cpu_off_entry;
+	uint32_t cpu_resume_entry;
+	uint32_t cpu_suspend_entry;
+	uint32_t sel1_intr_entry;
+	uint32_t irq_return_entry;
+	uint32_t s4_resume_entry;
+	uint32_t s4_suspend_entry;
+	uint32_t system_off_entry;
+	uint32_t system_reset_entry;
+	uint32_t abort_yield_smc_entry;
+} tee_vectors_t;
+
+typedef struct smc_registers {
+	u_register_t x1;
+	u_register_t x2;
+	u_register_t x3;
+	u_register_t x4;
+} smc_registers_t;
+
+/* Helper macros to store and retrieve tee args from tee_context */
+#define store_tee_args(_tee_ctx, _x1, _x2)                           \
+	do {                                                   \
+		(_tee_ctx)->saved_tee_args[0] = _x1; \
+		(_tee_ctx)->saved_tee_args[1] = _x2; \
+	} while (0)
+
+#define get_tee_args(_tee_ctx, _x1, _x2)                             \
+	do {                                                   \
+		_x1 = (_tee_ctx)->saved_tee_args[0]; \
+		_x2 = (_tee_ctx)->saved_tee_args[1]; \
+	} while (0)
+
+/* TEED power management handlers */
+const spd_pm_ops_t *get_teed_pm(void);
+
+/* Forward declarations */
+typedef struct tee_vectors tee_vectors_t;
+tee_context_t* get_teed_sp_context(const uint32_t linear_id);
+uint64_t get_teed_sp_context_size(void);
+tee_context_t *get_teed_sp_context_ptr(void);
+tee_context_t* get_teed_sp_init_context(void);
+int64_t get_tee_init_context_saved(void);
+void set_tee_init_context_saved(int64_t tee_init_context_saved);
+uint64_t get_primary_cpu_mpidr(void);
+void set_primary_cpu_mpidr(uint64_t primary_cpu_mpidr_in);
+tee_vectors_t* get_tee_vectors_t(void);
+void set_tee_vectors_t(tee_vectors_t *const tee_vectors_tmp);
+uint64_t plat_arm_calc_core_pos(uint64_t mpidr);
+
+#endif /* __ASSEMBLY__ */
+
+#endif /* TEED_PRIVATE_H */

sample/teed/src/teed_common.c

@@ -0,0 +1,123 @@
+/*
+ * Copyright (C) 2022 Huawei Technologies Co., Ltd.
+ * Licensed under the Mulan PSL v2.
+ * You can use this software according to the terms and conditions of the Mulan PSL v2.
+ * You may obtain a copy of Mulan PSL v2 at:
+ *     http://license.coscl.org.cn/MulanPSL2
+ * THIS SOFTWARE IS PROVIDED ON AN "AS IS" BASIS, WITHOUT WARRANTIES OF ANY KIND, EITHER EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO NON-INFRINGEMENT, MERCHANTABILITY OR FIT FOR A PARTICULAR
+ * PURPOSE.
+ * See the Mulan PSL v2 for more details.
+ */
+
+#include "teed_common.h"
+
+#include <string.h>
+#include <assert.h>
+#include <arch_helpers.h>
+#include <context_mgmt.h>
+#include "teed_private.h"
+#include "teed_helpers.h"
+/*
+ * Given a secure payload entrypoint info pointer, entry point PC, register
+ * width, cpu id & pointer to a context data structure, this function will
+ * initialize tee context and entry point info for the secure payload
+ */
+void teed_init_tee_ep_state(struct entry_point_info *ep,
+				      uint32_t rw,
+				      uintptr_t pc,
+				      tee_context_t *tee_ctx)
+{
+	uint32_t ep_attr;
+	uint32_t ee;
+	uint32_t daif;
+	/* Passing a NULL context is a critical programming error */
+	assert(tee_ctx != NULL);
+	assert(ep != NULL);
+	assert(pc != INVALID_PC_ADDR);
+
+	/*
+	 * We support AArch64 TEE for now.
+	 * Associate this context with the cpu specified
+	 */
+	ee = (uint32_t)SPSR_E_LITTLE;
+	tee_ctx->mpidr = read_mpidr_el1();
+	tee_ctx->state = (uint32_t)TEE_PSTATE_OFF;
+	set_tee_pstate(tee_ctx->state, TEE_PSTATE_OFF);
+	clr_yield_smc_active_flag(tee_ctx->state);
+
+	cm_set_context(&tee_ctx->cpu_context, SECURE);
+
+	/* initialise an entrypoint to set up the CPU context */
+	ep_attr = SECURE | EP_ST_ENABLE;
+	bool ee_bit_flag = (read_sctlr_el3() & SCTLR_EE_BIT) == 0 ? false : true;
+	if (ee_bit_flag) {
+		ep_attr |= EP_EE_BIG;
+		ee = SPSR_E_BIG;
+	}
+	SET_PARAM_HEAD(ep, PARAM_EP, VERSION_1, ep_attr);
+	ep->pc = pc;
+	if (rw == TEE_AARCH64) {
+		ep->spsr = SPSR_64(MODE_EL1, MODE_SP_ELX, DISABLE_ALL_EXCEPTIONS);
+	} else {
+		daif = DAIF_ABT_BIT | DAIF_IRQ_BIT | DAIF_FIQ_BIT;
+		ep->spsr = SPSR_MODE32(MODE32_svc, pc & SPSR_T_MASK, ee, daif);
+	}
+
+	memset(&ep->args, 0, sizeof(ep->args));
+}
+
+/*
+ * This function takes an SP context pointer and:
+ * 1. Applies the S-EL1 system register context from tee_ctx->cpu_context.
+ * 2. Saves the current C runtime state (callee saved registers) on the stack
+ *    frame and saves a reference to this state.
+ * 3. Calls el3_exit() so that the EL3 system and general purpose registers
+ *    from the tee_ctx->cpu_context are used to enter the secure payload image.
+ */
+uint64_t teed_synchronous_sp_entry(tee_context_t *tee_ctx)
+{
+	uint64_t rc;
+	assert(tee_ctx != NULL);
+	assert(tee_ctx->rt_context == INVALID_C_RT_CTX);
+
+	/* Apply the Secure EL1 system register context and switch to it */
+	assert(cm_get_context(SECURE) == &tee_ctx->cpu_context);
+
+	cm_el1_sysregs_context_restore(SECURE);
+	cm_set_next_eret_context(SECURE);
+	rc = teed_enter_sp(&tee_ctx->rt_context);
+#if ENABLE_ASSERTIONS
+	tee_ctx->rt_context = INVALID_C_RT_CTX;
+#endif
+
+	return rc;
+}
+
+/*
+ * This function takes an SP context pointer and:
+ * 1. Saves the S-EL1 system register context tp tee_ctx->cpu_context.
+ * 2. Restores the current C runtime state (callee saved registers) from the
+ *    stack frame using the reference to this state saved in teed_enter_sp().
+ * 3. It does not need to save any general purpose or EL3 system register state
+ *    as the generic smc entry routine should have saved those.
+ */
+void teed_synchronous_sp_exit(const tee_context_t *tee_ctx, uint64_t ret)
+{
+	assert(tee_ctx != NULL);
+	/* Save the Secure EL1 system register context */
+	assert(cm_get_context(SECURE) == &tee_ctx->cpu_context);
+	cm_el1_sysregs_context_save(SECURE);
+	int64_t init_context_saved = get_tee_init_context_saved();
+
+	tee_context_t *tee_context_tmp = get_teed_sp_init_context();
+	assert(tee_context_tmp != NULL);
+	if (init_context_saved == INIT_CONTEXT_NOT_SAVED) {
+		memcpy(tee_context_tmp, tee_ctx, sizeof(*tee_context_tmp));
+		set_tee_init_context_saved(INIT_CONTEXT_SAVED);
+	}
+	assert(tee_ctx->rt_context != INVALID_C_RT_CTX);
+	teed_exit_sp(tee_ctx->rt_context, ret);
+	printf("sp exit: Should never reach here\n");
+	assert(0);
+}

sample/teed/src/teed_global.c

@@ -0,0 +1,78 @@
+/*
+ * Copyright (C) 2022 Huawei Technologies Co., Ltd.
+ * Licensed under the Mulan PSL v2.
+ * You can use this software according to the terms and conditions of the Mulan PSL v2.
+ * You may obtain a copy of Mulan PSL v2 at:
+ *     http://license.coscl.org.cn/MulanPSL2
+ * THIS SOFTWARE IS PROVIDED ON AN "AS IS" BASIS, WITHOUT WARRANTIES OF ANY KIND, EITHER EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO NON-INFRINGEMENT, MERCHANTABILITY OR FIT FOR A PARTICULAR
+ * PURPOSE.
+ * See the Mulan PSL v2 for more details.
+ */
+
+#include <teed_private.h>
+
+/* Array to keep track of per-cpu Secure Payload state */
+tee_context_t g_teed_sp_context[TEED_CORE_COUNT];
+tee_context_t g_teed_sp_init_context;
+int64_t g_tee_init_context_saved;
+
+#define PRIMARY_CPU_DEFAULT 0xff
+
+/* primary_cpu: which core bring up the teeos */
+uint64_t g_primary_cpu_mpidr = PRIMARY_CPU_DEFAULT;
+
+tee_vectors_t *g_tee_vectors = NULL;
+
+tee_context_t *get_teed_sp_context(const uint32_t linear_id)
+{
+	if (linear_id >= TEED_CORE_COUNT)
+		return NULL;
+
+	return &g_teed_sp_context[linear_id];
+}
+
+uint64_t get_teed_sp_context_size(void)
+{
+	return sizeof(g_teed_sp_context);
+}
+
+tee_context_t *get_teed_sp_context_ptr(void)
+{
+	return g_teed_sp_context;
+}
+
+tee_context_t *get_teed_sp_init_context(void)
+{
+	return &g_teed_sp_init_context;
+}
+
+int64_t get_tee_init_context_saved(void)
+{
+	return g_tee_init_context_saved;
+}
+
+void set_tee_init_context_saved(const int64_t tee_init_context_saved)
+{
+	g_tee_init_context_saved = tee_init_context_saved;
+}
+
+uint64_t get_primary_cpu_mpidr(void)
+{
+	return g_primary_cpu_mpidr;
+}
+
+void set_primary_cpu_mpidr(const uint64_t primary_cpu_mpidr_in)
+{
+	g_primary_cpu_mpidr = primary_cpu_mpidr_in;
+}
+
+tee_vectors_t *get_tee_vectors_t(void)
+{
+	return g_tee_vectors;
+}
+
+void set_tee_vectors_t(tee_vectors_t *const tee_vectors_tmp)
+{
+	g_tee_vectors = tee_vectors_tmp;
+}

sample/teed/src/teed_helpers.S

@@ -0,0 +1,64 @@
+/*
+ * Copyright (C) 2022 Huawei Technologies Co., Ltd.
+ * Licensed under the Mulan PSL v2.
+ * You can use this software according to the terms and conditions of the Mulan PSL v2.
+ * You may obtain a copy of Mulan PSL v2 at:
+ *     http://license.coscl.org.cn/MulanPSL2
+ * THIS SOFTWARE IS PROVIDED ON AN "AS IS" BASIS, WITHOUT WARRANTIES OF ANY KIND, EITHER EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO NON-INFRINGEMENT, MERCHANTABILITY OR FIT FOR A PARTICULAR
+ * PURPOSE.
+ * See the Mulan PSL v2 for more details.
+ */
+
+#include <asm_macros.S>
+
+#define __ASSEMBLY__
+
+#include <teed_private.h>
+
+	.global	teed_enter_sp
+func teed_enter_sp
+	/* Make space for the registers that we're going to save */
+	mov	x3, sp
+	str	x3, [x0, #0]
+	sub	sp, sp, #TEED_C_RT_CTX_SIZE
+
+	/* Save callee-saved registers on to the stack */
+	stp	x19, x20, [sp, #TEED_C_RT_CTX_X19]
+	stp	x21, x22, [sp, #TEED_C_RT_CTX_X21]
+	stp	x23, x24, [sp, #TEED_C_RT_CTX_X23]
+	stp	x25, x26, [sp, #TEED_C_RT_CTX_X25]
+	stp	x27, x28, [sp, #TEED_C_RT_CTX_X27]
+	stp	x29, x30, [sp, #TEED_C_RT_CTX_X29]
+
+	/*
+	 * el3_exit() will use the secure context to restore to the
+	 * general purpose and EL3 system registers to ERET into the secure payload
+	 */
+	b	el3_exit
+endfunc teed_enter_sp
+
+	/*
+	 * This function is called 'x0' pointing to a C
+	 * runtime context saved in teed_enter_sp()
+	 */
+	.global teed_exit_sp
+func teed_exit_sp
+	/* Restore the previous stack */
+	mov	sp, x0
+
+	/* Restore callee-saved registers on to the stack */
+	ldp	x19, x20, [x0, #(TEED_C_RT_CTX_X19 - TEED_C_RT_CTX_SIZE)]
+	ldp	x21, x22, [x0, #(TEED_C_RT_CTX_X21 - TEED_C_RT_CTX_SIZE)]
+	ldp	x23, x24, [x0, #(TEED_C_RT_CTX_X23 - TEED_C_RT_CTX_SIZE)]
+	ldp	x25, x26, [x0, #(TEED_C_RT_CTX_X25 - TEED_C_RT_CTX_SIZE)]
+	ldp	x27, x28, [x0, #(TEED_C_RT_CTX_X27 - TEED_C_RT_CTX_SIZE)]
+	ldp	x29, x30, [x0, #(TEED_C_RT_CTX_X29 - TEED_C_RT_CTX_SIZE)]
+
+	/*
+	 * This should take us back to the instruction
+	 * after the call to the last teed_enter_sp().
+	 */
+	mov	x0, x1
+	ret
+endfunc teed_exit_sp

sample/teed/src/teed_main.c

@@ -0,0 +1,550 @@
+/*
+ * Copyright (C) 2022 Huawei Technologies Co., Ltd.
+ * Licensed under the Mulan PSL v2.
+ * You can use this software according to the terms and conditions of the Mulan PSL v2.
+ * You may obtain a copy of Mulan PSL v2 at:
+ *     http://license.coscl.org.cn/MulanPSL2
+ * THIS SOFTWARE IS PROVIDED ON AN "AS IS" BASIS, WITHOUT WARRANTIES OF ANY KIND, EITHER EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO NON-INFRINGEMENT, MERCHANTABILITY OR FIT FOR A PARTICULAR
+ * PURPOSE.
+ * See the Mulan PSL v2 for more details.
+ */
+
+#include <errno.h>
+#include <assert.h>
+#include <arch_helpers.h>
+#include <bl31.h>
+#include <bl_common.h>
+#include <context.h>
+#include <context_mgmt.h>
+#include <platform.h>
+#include <runtime_svc.h>
+#include "tee.h"
+#include "teed_common.h"
+#include "teed_private.h"
+
+/*
+ * This function is the handler registered for S-EL1 interrupts by the TEED
+ * It validates the interrupt and upon success arranges entry into the TEE
+ * at 'tee_sel1_intr_entry()' for handling the interrupt.
+ */
+static uint64_t teed_sel1_interrupt_handler(uint32_t id,
+						 uint32_t flags,
+						 void *handle,
+						 void *cookie)
+{
+	uint32_t linear_id;
+	tee_context_t *tee_ctx = NULL;
+
+	/* Check the security state when the exception was generated */
+	assert(get_interrupt_src_ss(flags) == NON_SECURE);
+
+	/* Sanity check the pointer to this cpu's context */
+	assert(handle == cm_get_context(NON_SECURE));
+
+	/* Save the non-secure context before entering the TEE */
+	cm_el1_sysregs_context_save(NON_SECURE);
+	fpregs_context_save(get_fpregs_ctx(cm_get_context(NON_SECURE)));
+
+	/* Get a reference to this cpu's TEE context */
+	linear_id = plat_my_core_pos();
+	tee_ctx = get_teed_sp_context(linear_id);
+	assert(tee_ctx != NULL);
+	assert(&tee_ctx->cpu_context == cm_get_context(SECURE));
+
+	/*
+	 * Determine if the tee was previously preempted. Its last known
+	 * context has to be preserved in this case.
+	 * The tee should return control to the teeD after handling
+	 * this S-EL1 interrupt. Preserve essential EL3 context to allow entry
+	 * into the tee at the S-EL1 interrupt entry point using the
+	 * 'cpu_context' structure. There is no need to save the secure system
+	 * register context since the tee is supposed to preserve it
+	 * during S-EL1 interrupt handling.
+	 */
+	if (get_yield_smc_active_flag(tee_ctx->state) == SMC_ACTIVE) {
+		tee_ctx->spsr_el3 = (uint32_t)SMC_GET_EL3(&tee_ctx->cpu_context, CTX_SPSR_EL3);
+		tee_ctx->elr_el3 = SMC_GET_EL3(&tee_ctx->cpu_context, CTX_ELR_EL3);
+	}
+
+	cm_el1_sysregs_context_restore(SECURE);
+	fpregs_context_restore(get_fpregs_ctx(cm_get_context(SECURE)));
+	tee_vectors_t* tee_vectors_tmp = get_tee_vectors_t();
+	assert(tee_vectors_tmp != NULL);
+	cm_set_elr_spsr_el3(SECURE, (uintptr_t)&tee_vectors_tmp->sel1_intr_entry,
+			    SPSR_64(MODE_EL1, MODE_SP_ELX, DISABLE_ALL_EXCEPTIONS));
+
+	cm_set_next_eret_context(SECURE);
+	/*
+	 * Tell the TEE that it has to handle a S-EL1 interrupt
+	 * synchronously. It is safe to retrieve
+	 * this address from ELR_EL3 as the secure context will not take effect
+	 * until el3_exit().
+	 */
+	SMC_RET2((uintptr_t)&tee_ctx->cpu_context,
+		 TEE_HANDLE_SEL1_INTR_AND_RETURN, read_elr_el3());
+}
+
+/*
+ * This function passes control to the Secure Payload image (BL32) for the first
+ * time on the primary cpu after a cold boot. It assumes that a valid secure
+ * context has already been created by teed_setup() which can be directly
+ * used. It also assumes that a valid non-secure context has been initialised by
+ * PSCI so it does not need to save and restore any non-secure state. This
+ * function performs a synchronous entry into the Secure payload. The SP passes
+ * control back to this routine through a SMC.
+ */
+static int32_t teed_init(void)
+{
+	uint32_t linear_id = plat_my_core_pos();
+	tee_context_t *tee_ctx = get_teed_sp_context(linear_id);
+	entry_point_info_t *tee_entry_point = NULL;
+	uint64_t rc;
+	uint64_t mpidr = read_mpidr();
+	/* set the primary cpu */
+	set_primary_cpu_mpidr(mpidr);
+	/*
+	 * Get information about the Secure Payload (BL32) image. Its
+	 * absence is a critical failure.
+	 */
+	tee_entry_point = bl31_plat_get_next_image_ep_info(SECURE);
+	cm_init_my_context(tee_entry_point);
+
+#ifdef BOOT_BL32_FROM_OTHER_EXCEPTION
+	/* set el3 fiq bit for tee init */
+	cpu_context_t *ctx = NULL;
+	el3_state_t *state = NULL;
+	uint32_t scr_el3;
+	ctx = cm_get_context(GET_SECURITY_STATE(tee_entry_point->h.attr));
+	state = get_el3state_ctx(ctx);
+	scr_el3 = read_ctx_reg(state, CTX_SCR_EL3);
+	scr_el3 &= ~SCR_FIQ_BIT;
+	write_ctx_reg(state, CTX_SCR_EL3, scr_el3);
+
+	/* save nosecure context and disable interrupt during init */
+	cm_el1_sysregs_context_save(NON_SECURE);
+	fpregs_context_save(get_fpregs_ctx(cm_get_context(NON_SECURE)));
+#endif
+	/*
+	 * Arrange for an entry into the test secure payload. It will be
+	 * returned via TEE_ENTRY_DONE case
+	 */
+	rc = teed_synchronous_sp_entry(tee_ctx);
+	assert(rc != 0);
+	return rc;
+}
+
+/*
+ * Secure Payload Dispatcher setup. The SPD finds out the SP entrypoint and type
+ * (aarch32/aarch64) if not already known and initialises the context for entry
+ * into the SP for its initialisation.
+ */
+static int32_t teed_setup(void)
+{
+	entry_point_info_t *image_info = NULL;
+	uint32_t linear_id;
+
+	linear_id = plat_my_core_pos();
+	NOTICE("teed setup start!\n");
+	/*
+	 * Get information about the Secure Payload (BL32) image. Its
+	 * absence is a critical failure.
+	 * conditionally include the SPD service
+	 */
+	image_info = bl31_plat_get_next_image_ep_info(SECURE);
+	if (image_info == NULL) {
+		WARN("No TEE provided by BL2 boot loader, Booting device"
+			" without TEE initialization. SMC`s destined for TEE"
+			" will return SMC_UNK\n");
+		return TEE_SETUP_FAIL;
+	}
+	assert(image_info != NULL);
+	/*
+	 * If there's no valid entry point for SP, we return a non-zero value
+	 * signalling failure initializing the service. We bail out without
+	 * registering any handlers
+	 */
+	if (image_info->pc == INVALID_PC_ADDR)
+		return TEE_SETUP_FAIL;
+
+	/*
+	 * We could inspect the SP image and determine its execution
+	 * state i.e whether AArch32 or AArch64. Assuming it's AArch64
+	 * for the time being.
+	 */
+	tee_context_t *tee_ctx = get_teed_sp_context(linear_id);
+	teed_init_tee_ep_state(image_info,
+					 TEE_AARCH64,
+					 image_info->pc,
+					 tee_ctx);
+
+	/*
+	 * All TEED initialization done. Now register our init function
+	 * with BL31 for deferred invocation
+	 */
+	bl31_register_bl32_init(&teed_init);
+	return 0;
+}
+
+/*
+ * This function ID is used only by the tee to indicate that it has
+ * finished handling a S-EL1 interrupt or was preempted by a higher
+ * priority pending EL3 interrupt. Execution should resume
+ * in the normal world.
+ * TEE_HANDLED_S_EL1_INTR:
+ * TEE_HANDLED_S_EL1_FIQ_AARCH32:
+ */
+static uintptr_t smc_handle_s_el1(tee_context_t *tee_ctx,
+				  void *handle, uint32_t ns)
+{
+	if (ns != SECURE_WORLD_FLAG)
+		SMC_RET1((uintptr_t)handle, SMC_UNK);
+
+	assert(handle == cm_get_context(SECURE));
+	assert(tee_ctx != NULL);
+	cpu_context_t *ns_cpu_context = NULL;
+	/*
+	 * Restore the relevant EL3 state which saved to service
+	 * this SMC.
+	 */
+	if (get_yield_smc_active_flag(tee_ctx->state) != SMC_INACTIVE) {
+		SMC_SET_EL3(&tee_ctx->cpu_context,
+			    CTX_SPSR_EL3,
+			    tee_ctx->spsr_el3);
+		SMC_SET_EL3(&tee_ctx->cpu_context,
+			    CTX_ELR_EL3,
+			    tee_ctx->elr_el3);
+	}
+
+	/* Get a reference to the non-secure context */
+	ns_cpu_context = cm_get_context(NON_SECURE);
+	assert(ns_cpu_context != NULL);
+
+	/*
+	 * Restore non-secure state. There is no need to save the
+	 * secure system register context since the tee was supposed
+	 * to preserve it during S-EL1 interrupt handling.
+	 */
+	cm_el1_sysregs_context_restore(NON_SECURE);
+	fpregs_context_restore(get_fpregs_ctx(cm_get_context(NON_SECURE)));
+	cm_set_next_eret_context(NON_SECURE);
+	SMC_RET0((uintptr_t)ns_cpu_context);
+}
+
+/*
+ * This function ID is used only by the SP to indicate it has
+ * finished initialising itself after a cold boot
+ * TEE_ENTRY_DONE:
+ * TEE_ENTRY_DONE_AARCH32:
+ */
+static void smc_handle_entry_done(tee_context_t *tee_ctx,
+				  u_register_t x1, u_register_t *flags)
+{
+	/*
+	 * Stash the SP entry points information. This is done
+	 * only once on the primary cpu
+	 */
+	uint64_t rc;
+	tee_vectors_t* tee_vectors_tmp = (tee_vectors_t *)(uintptr_t)x1;
+	set_tee_vectors_t(tee_vectors_tmp);
+	if (tee_vectors_tmp != NULL) {
+		set_tee_pstate(tee_ctx->state, TEE_PSTATE_ON);
+		/*
+		 * tee has been successfully initialized. Register
+		 * power managemnt hooks with PSCI
+		 */
+		const spd_pm_ops_t *teed_pm_tmp = get_teed_pm();
+		psci_register_spd_pm_hook(teed_pm_tmp);
+
+		/*
+		 * Register an interrupt handler for S-EL1 interrupts
+		 * when generated during code executing in the
+		 * non-secure state.
+		 */
+		*flags = 0;
+		set_interrupt_rm_flag(*flags, NON_SECURE);
+		rc = register_interrupt_type_handler(INTR_TYPE_S_EL1,
+						     teed_sel1_interrupt_handler,
+						     *flags);
+		if (rc != 0)
+			panic();
+	}
+
+	/*
+	 * SP reports completion. The SPD must have initiated
+	 * the original request through a synchronous entry
+	 * into the SP. Jump back to the original C runtime
+	 * context.
+	 */
+	teed_synchronous_sp_exit(tee_ctx, x1);
+}
+
+/*
+ * This function ID is used only by the SP to indicate it has finished
+ * aborting a preempted Yielding SMC Call.
+ * case TEE_ABORT_DONE:
+ * *********
+ * These function IDs are used only by the SP to indicate it has
+ * finished:
+ * 1. turning itself on in response to an earlier psci
+ *    cpu_on request
+ * 2. resuming itself after an earlier psci cpu_suspend
+ *    request.
+ * TEE_ON_DONE:
+ * TEE_ON_DONE_AARCH32:
+ * TEE_RESUME_DONE:
+ * TEE_RESUME_DONE_AARCH32:
+ * ****
+ * These function IDs are used only by the SP to indicate it has
+ * finished:
+ * 1. suspending itself after an earlier psci cpu_suspend
+ *    request.
+ * 2. turning itself off in response to an earlier psci
+ *    cpu_off request.
+ * TEE_OFF_DONE:
+ * TEE_SUSPEND_DONE:
+ * TEE_SUSPEND_DONE_AARCH32:
+ * TEE_SYSTEM_OFF_DONE:
+ * TEE_SYSTEM_RESET_DONE:
+ */
+static void smc_handle_abort_on_resume(const tee_context_t *tee_ctx, u_register_t x1)
+{
+	/*
+	 * SP reports completion. The SPD must have initiated the
+	 * original request through a synchronous entry into the SP.
+	 * Jump back to the original C runtime context, and pass x1 as
+	 * return value to the caller
+	 */
+	teed_synchronous_sp_exit(tee_ctx, x1);
+}
+
+static uintptr_t smc_handle_std_request(tee_context_t *tee_ctx,
+					smc_registers_t registers_t,
+					const void *handle, uint32_t smc_fid,
+					uint32_t ns)
+{
+	if (ns == SECURE_WORLD_FLAG)
+		SMC_RET1((uintptr_t)handle, SMC_UNK);
+	assert(handle == cm_get_context(NON_SECURE));
+	cm_el1_sysregs_context_save(NON_SECURE);
+	fpregs_context_save(get_fpregs_ctx(cm_get_context(NON_SECURE)));
+	assert(&tee_ctx->cpu_context == cm_get_context(SECURE));
+	/*
+	 * Restore the correct state considering if the
+	 * OS has been active.
+	 */
+	tee_vectors_t* tee_vectors_tmp = get_tee_vectors_t();
+	assert(tee_vectors_tmp != NULL);
+	if (get_yield_smc_active_flag(tee_ctx->state) != SMC_INACTIVE) {
+		cm_set_elr_spsr_el3(SECURE,
+				    (uintptr_t)&tee_vectors_tmp->irq_return_entry,
+				    SPSR_64(MODE_EL1, MODE_SP_ELX, DISABLE_ALL_EXCEPTIONS));
+	} else {
+		cm_set_elr_spsr_el3(SECURE, (uintptr_t)&tee_vectors_tmp->yield_smc_entry,
+				    SPSR_64(MODE_EL1, MODE_SP_ELX, DISABLE_ALL_EXCEPTIONS));
+	}
+
+	cm_el1_sysregs_context_restore(SECURE);
+	fpregs_context_restore(get_fpregs_ctx(cm_get_context(SECURE)));
+	cm_set_next_eret_context(SECURE);
+
+	if (get_yield_smc_active_flag(tee_ctx->state) != SMC_INACTIVE) {
+		SMC_RET0((uintptr_t)&tee_ctx->cpu_context);
+	} else {
+		set_yield_smc_active_flag(tee_ctx->state);
+		SMC_RET5((uintptr_t)&tee_ctx->cpu_context, smc_fid, registers_t.x1,
+			 registers_t.x2, registers_t.x3, registers_t.x4);
+	}
+}
+
+static uintptr_t smc_handle_std_ree_siq(uint32_t smc_fid,
+					tee_context_t *tee_ctx,
+					u_register_t x1, void *handle,
+					uint32_t ns)
+{
+	if (ns == SECURE_WORLD_FLAG)
+		SMC_RET1((uintptr_t)handle, SMC_UNK);
+
+	assert(handle == cm_get_context(NON_SECURE));
+
+	cm_el1_sysregs_context_save(NON_SECURE);
+	fpregs_context_save(get_fpregs_ctx(cm_get_context(NON_SECURE)));
+
+	assert(&tee_ctx->cpu_context == cm_get_context(SECURE));
+
+	tee_ctx->spsr_el3 = SMC_GET_EL3(&tee_ctx->cpu_context,
+					     CTX_SPSR_EL3);
+	tee_ctx->elr_el3 = SMC_GET_EL3(&tee_ctx->cpu_context,
+					    CTX_ELR_EL3);
+	tee_vectors_t* tee_vectors_tmp = get_tee_vectors_t();
+	assert(tee_vectors_tmp != NULL);
+	cm_set_elr_spsr_el3(SECURE,
+			    (uintptr_t)&tee_vectors_tmp->fast_smc_entry,
+			    SPSR_64(MODE_EL1, MODE_SP_ELX, DISABLE_ALL_EXCEPTIONS));
+
+	write_ctx_reg(get_gpregs_ctx(&tee_ctx->cpu_context), CTX_GPREG_X1, x1);
+	(void)teed_synchronous_sp_entry(tee_ctx);
+	/* Restore non-secure state */
+	cm_el1_sysregs_context_restore(NON_SECURE);
+	fpregs_context_restore(get_fpregs_ctx(cm_get_context(NON_SECURE)));
+	cm_set_next_eret_context(NON_SECURE);
+	SMC_RET1((uintptr_t)handle, smc_fid);
+}
+
+static uintptr_t smc_handle_std_response(uint32_t smc_fid,
+					 tee_context_t *tee_ctx,
+					 smc_registers_t registers_t,
+					 const void *handle, uint32_t ns)
+{
+	if (ns != SECURE_WORLD_FLAG)
+		SMC_RET1((uintptr_t)handle, SMC_UNK);
+	/* Forward secure responses to NS */
+	cpu_context_t *ns_cpu_context = NULL;
+
+	assert(handle == cm_get_context(SECURE));
+	cm_el1_sysregs_context_save(SECURE);
+	fpregs_context_save(get_fpregs_ctx(cm_get_context(SECURE)));
+
+	/* Get a reference to the non-secure context */
+	ns_cpu_context = cm_get_context(NON_SECURE);
+	assert(ns_cpu_context != NULL);
+
+	/* Restore non-secure state */
+	cm_el1_sysregs_context_restore(NON_SECURE);
+	fpregs_context_restore(get_fpregs_ctx(cm_get_context(NON_SECURE)));
+	cm_set_next_eret_context(NON_SECURE);
+	clr_yield_smc_active_flag(tee_ctx->state);
+	SMC_RET4((uintptr_t)ns_cpu_context, smc_fid, registers_t.x1,
+		 registers_t.x2, registers_t.x3);
+}
+
+static uintptr_t smc_handle_std_crash(tee_context_t *tee_ctx,
+				      smc_registers_t registers_t, const void *handle,
+				      uint32_t ns)
+{
+	if (ns != SECURE_WORLD_FLAG)
+		SMC_RET1((uintptr_t)handle, SMC_UNK);
+	NOTICE("notify teeos has crashed\n");
+	/* Secure OS has crashed, set the flag and return to ns */
+	cpu_context_t *ns_cpu_context = NULL;
+	assert(handle == cm_get_context(SECURE));
+
+	set_std_crash_flag(tee_ctx->state);
+
+	/* Get a reference to the non-secure context */
+	ns_cpu_context = cm_get_context(NON_SECURE);
+	assert(ns_cpu_context != NULL);
+
+	/* Restore non-secure state */
+	cm_el1_sysregs_context_restore(NON_SECURE);
+	fpregs_context_restore(get_fpregs_ctx(cm_get_context(NON_SECURE)));
+	cm_set_next_eret_context(NON_SECURE);
+	SMC_RET4((uintptr_t)ns_cpu_context, TEE_STD_CRASH, registers_t.x1,
+		 registers_t.x2, registers_t.x3);
+}
+
+/*
+ * This function is responsible for handling all SMCs in the Trusted OS/App
+ * range from the non-secure state as defined in the SMC Calling Convention
+ * Document. It is also responsible for communicating with the Secure payload
+ * to delegate work and return results back to the non-secure state. Lastly it
+ * will also return any information that the secure payload needs to do the
+ * work assigned to it.
+ */
+
+static uintptr_t teed_smc_handler(uint32_t smc_fid, u_register_t x1,
+				       u_register_t x2, u_register_t x3,
+				       u_register_t x4, void *cookie,
+				       void *handle, u_register_t flags)
+{
+	uint32_t linear_id = plat_my_core_pos();
+	/* Determine which security state this SMC originated from */
+	uint32_t ns = is_caller_non_secure(flags);
+	tee_context_t *tee_ctx = get_teed_sp_context(linear_id);
+	smc_registers_t registers_t = { x1, x2, x3, x4 };
+	/*
+	 * For calls comming from non-secure side after the OS signals
+	 * it has crashed just return to NS side, no more forwarding
+	 */
+	assert(tee_ctx != NULL);
+	if (ns != SECURE_WORLD_FLAG &&
+	    get_std_crash_flag(tee_ctx->state) != STD_NO_CRASH_FLAG)
+		SMC_RET1((uintptr_t)handle, TEE_STD_CRASH);
+
+	switch (smc_fid) {
+	/*
+	 * This function ID is used only by the tee to indicate that it has
+	 * finished handling a S-EL1 interrupt or was preempted by a higher
+	 * priority pending EL3 interrupt. Execution should resume
+	 * in the normal world.
+	 */
+	case TEE_HANDLED_S_EL1_INTR:
+	case TEE_HANDLED_S_EL1_FIQ_AARCH32:
+		return smc_handle_s_el1(tee_ctx, handle, ns);
+	/*
+	 * This function ID is used only by the SP to indicate it has
+	 * finished initialising itself after a cold boot
+	 */
+	case TEE_ENTRY_DONE:
+	case TEE_ENTRY_DONE_AARCH32:
+		if (ns == SECURE_WORLD_FLAG)
+			smc_handle_entry_done(tee_ctx, x1, &flags);
+		break;
+	/*
+	 * This function ID is used only by the SP to indicate it has finished
+	 * aborting a preempted Yielding SMC Call.
+	 */
+	case TEE_ABORT_DONE:
+	case TEE_ON_DONE:
+	case TEE_ON_DONE_AARCH32:
+	case TEE_RESUME_DONE:
+	case TEE_RESUME_DONE_AARCH32:
+	/*
+	 * These function IDs are used only by the SP to indicate it has
+	 * finished:
+	 * 1. suspending itself after an earlier psci cpu_suspend
+	 *    request.
+	 * 2. turning itself off in response to an earlier psci
+	 *    cpu_off request.
+	 */
+	case TEE_SUSPEND_DONE:
+	case TEE_SUSPEND_DONE_AARCH32:
+		if (ns != SECURE_WORLD_FLAG)
+			SMC_RET1((uintptr_t)handle, SMC_UNK);
+
+		smc_handle_abort_on_resume(tee_ctx, x1);
+		break;
+	case TEE_STD_REQUEST:
+		return smc_handle_std_request(tee_ctx, registers_t, handle, smc_fid, ns);
+	case TEE_STD_REE_SIQ:
+		return smc_handle_std_ree_siq(smc_fid, tee_ctx, x1, handle, ns);
+	case TEE_STD_RESPONSE:
+		return smc_handle_std_response(smc_fid, tee_ctx, registers_t, handle, ns);
+	case TEE_STD_CRASH:
+		return smc_handle_std_crash(tee_ctx, registers_t, handle, ns);
+	default:
+		break;
+	}
+
+	SMC_RET1((uintptr_t)handle, SMC_UNK);
+}
+
+/* Define a SPD runtime service descriptor for fast SMC calls */
+DECLARE_RT_SVC(
+	teed_fast,
+	OEN_TOS_START,
+	OEN_TOS_END,
+	SMC_TYPE_FAST,
+	teed_setup,
+	teed_smc_handler
+);
+
+/* Define a SPD runtime service descriptor for Yielding SMC Calls */
+DECLARE_RT_SVC(
+	teed_std,
+	OEN_TOS_START,
+	OEN_TOS_END,
+	SMC_TYPE_YIELD,
+	NULL,
+	teed_smc_handler
+);

sample/teed/src/teed_pm.c

@@ -0,0 +1,287 @@
+/*
+ * Copyright (C) 2022 Huawei Technologies Co., Ltd.
+ * Licensed under the Mulan PSL v2.
+ * You can use this software according to the terms and conditions of the Mulan PSL v2.
+ * You may obtain a copy of Mulan PSL v2 at:
+ *     http://license.coscl.org.cn/MulanPSL2
+ * THIS SOFTWARE IS PROVIDED ON AN "AS IS" BASIS, WITHOUT WARRANTIES OF ANY KIND, EITHER EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO NON-INFRINGEMENT, MERCHANTABILITY OR FIT FOR A PARTICULAR
+ * PURPOSE.
+ * See the Mulan PSL v2 for more details.
+ */
+
+#include <assert.h>
+#include <bl_common.h>
+#include <context_mgmt.h>
+#include <arch_helpers.h>
+#include <platform.h>
+#include <runtime_svc.h>
+#include <string.h>
+#include <context.h>
+#include <spinlock.h>
+#include "teed_common.h"
+#include "teed_private.h"
+
+static uint32_t g_cpu_initialized[TEED_CORE_COUNT];
+
+/* need to be implemented */
+static int32_t is_system_suspend(void)
+{
+    return 1;
+}
+
+/*
+ * The target cpu is being turned on. Allow the TEED/TEE to perform any actions
+ * needed. Nothing at the moment.
+ */
+static void teed_cpu_on_handler(u_register_t target_cpu)
+{
+    (void)target_cpu;
+}
+
+/*
+ * This cpu is being turned off. Allow the TEED/TEE to perform
+ * any actions needed
+ */
+static int32_t teed_cpu_off_handler(u_register_t unused)
+{
+	uint32_t linear_id = plat_my_core_pos();
+	tee_context_t *tee_ctx = get_teed_sp_context(linear_id);
+	assert(tee_ctx != NULL);
+	assert(get_tee_pstate(tee_ctx->state) == TEE_PSTATE_ON);
+	/*
+	 * Abort any preempted SMC request before overwriting the SECURE
+	 * context.
+	 */
+	set_tee_pstate(tee_ctx->state, TEE_PSTATE_OFF);
+
+	return 0;
+}
+
+/*
+ * This cpu is being suspended. S-EL1 state must have been saved in the
+ * resident cpu (mpidr format) if it is a UP/UP migratable TEE.
+ */
+static void teed_cpu_suspend_handler(u_register_t max_off_pwrlvl)
+{
+	int32_t rc;
+	uint64_t power_state;
+	uint32_t linear_id = plat_my_core_pos();
+	tee_context_t *tee_ctx = get_teed_sp_context(linear_id);
+	assert(tee_ctx != NULL);
+
+	tee_vectors_t* tee_vectors_tmp = get_tee_vectors_t();
+	(void)max_off_pwrlvl;
+	assert(tee_vectors_tmp != NULL);
+	assert(get_tee_pstate(tee_ctx->state) == TEE_PSTATE_ON);
+	/*
+	 * Abort any preempted SMC request before overwriting the SECURE
+	 * context.
+	 */
+	if (is_system_suspend() != 0)
+		power_state = CPU_IDLE_STATE; /* stand for cpu idle */
+	else
+		power_state = CPU_SUSPEND_STATE; /* stand for cpu suspend */
+
+	/* Program the entry point and enter the TEE */
+	write_ctx_reg(get_gpregs_ctx(&tee_ctx->cpu_context),
+		      CTX_GPREG_X0,
+		      power_state);
+	tee_ctx->spsr_el3 = SMC_GET_EL3(&tee_ctx->cpu_context,
+					     CTX_SPSR_EL3);
+	tee_ctx->elr_el3 = SMC_GET_EL3(&tee_ctx->cpu_context,
+					    CTX_ELR_EL3);
+	cm_set_elr_spsr_el3(SECURE, (uintptr_t)&tee_vectors_tmp->cpu_suspend_entry,
+			    SPSR_64(MODE_EL1, MODE_SP_ELX, DISABLE_ALL_EXCEPTIONS));
+	rc = teed_synchronous_sp_entry(tee_ctx);
+	/*
+	 * Read the response from the TEE. A non-zero return means that
+	 * something went wrong while communicating with the TEE.
+	 */
+	if (rc != 0)
+		panic();
+	/* Update its context to reflect the state the TEE is in */
+	set_tee_pstate(tee_ctx->state, TEE_PSTATE_SUSPEND);
+}
+
+/*
+ * This cpu has been turned on. Enter the TEE to initialise S-EL1 and other bits
+ * before passing control back to the Secure Monitor. Entry in S-EL1 is done
+ * after initialising minimal architectural state that guarantees safe
+ * execution.
+ */
+static void teed_cpu_on_finish_handler(u_register_t unused)
+{
+	int32_t rc;
+	uint32_t linear_id = plat_my_core_pos();
+	tee_context_t *tee_ctx = get_teed_sp_context(linear_id);
+	assert(tee_ctx != NULL);
+	entry_point_info_t tee_on_entrypoint;
+	el3_state_t *state = NULL;
+
+	tee_vectors_t *tee_vectors_tmp = get_tee_vectors_t();
+	assert(tee_vectors_tmp != NULL);
+	assert(get_tee_pstate(tee_ctx->state) == TEE_PSTATE_OFF);
+	set_tee_pstate(tee_ctx->state, TEE_PSTATE_ON);
+
+	uint64_t mpidr = read_mpidr();
+	uint64_t primary_cpu_mpidr_tmp = get_primary_cpu_mpidr();
+	if (linear_id >= TEED_CORE_COUNT)
+		return;
+
+	bool condition = (g_cpu_initialized[linear_id] == CPU_INIT_DONE ||
+		mpidr == primary_cpu_mpidr_tmp);
+	if (!condition) {
+		tee_context_t *teed_sp_init_context = get_teed_sp_init_context();
+		assert(teed_sp_init_context != NULL);
+		(void)memcpy(&tee_ctx->cpu_context, &(teed_sp_init_context->cpu_context), sizeof(cpu_context_t));
+		(void)memset(&tee_on_entrypoint, 0, sizeof(tee_on_entrypoint));
+		/* Initialise this cpu's secure context */
+		teed_init_tee_ep_state(&tee_on_entrypoint,
+						 TEE_AARCH64,
+						 (uintptr_t)&tee_vectors_tmp->cpu_on_entry,
+						 tee_ctx);
+		state = get_el3state_ctx(&tee_ctx->cpu_context);
+		write_ctx_reg(state, CTX_ELR_EL3, tee_on_entrypoint.pc);
+		write_ctx_reg(state, CTX_SPSR_EL3, tee_on_entrypoint.spsr);
+	}
+	/* Enter the TEE */
+	rc = teed_synchronous_sp_entry(tee_ctx);
+	g_cpu_initialized[linear_id] = CPU_INIT_DONE;
+	/*
+	 * Read the response from the TEE. A non-zero return means that
+	 * something went wrong while communicating with the SP.
+	 */
+	if (rc != 0)
+		panic();
+	/* Update its context to reflect the state the SP is in */
+	set_tee_pstate(tee_ctx->state, TEE_PSTATE_ON);
+}
+
+/*
+ * This cpu has resumed from suspend. The SPD saved the TEE context when it
+ * completed the preceding suspend call. Use that context to program an entry
+ * into the TEE to allow it to do any remaining book keeping
+ */
+static void teed_cpu_suspend_finish_handler(u_register_t max_off_pwrlvl)
+{
+	int32_t rc;
+	uint64_t power_state;
+	uint32_t linear_id = plat_my_core_pos();
+	tee_context_t *tee_ctx = get_teed_sp_context(linear_id);
+	assert(tee_ctx != NULL);
+
+	(void)max_off_pwrlvl;
+	tee_vectors_t* tee_vectors_tmp = get_tee_vectors_t();
+	assert(tee_vectors_tmp != NULL);
+	assert(get_tee_pstate(tee_ctx->state) == TEE_PSTATE_SUSPEND);
+
+	if (is_system_suspend() != 0)
+		power_state = CPU_IDLE_STATE; /* stand for cpu idle */
+	else
+		power_state = CPU_SUSPEND_STATE; /* stand for cpu suspend */
+
+	/* Program the entry point, max_off_pwrlvl and enter the SP */
+	write_ctx_reg(get_gpregs_ctx(&tee_ctx->cpu_context),
+		      CTX_GPREG_X0,
+		      power_state);
+	tee_ctx->spsr_el3 = SMC_GET_EL3(&tee_ctx->cpu_context,
+					     CTX_SPSR_EL3);
+	tee_ctx->elr_el3 = SMC_GET_EL3(&tee_ctx->cpu_context,
+					    CTX_ELR_EL3);
+	cm_set_elr_spsr_el3(SECURE, (uintptr_t)&tee_vectors_tmp->cpu_resume_entry,
+			    SPSR_64(MODE_EL1, MODE_SP_ELX, DISABLE_ALL_EXCEPTIONS));
+	rc = teed_synchronous_sp_entry(tee_ctx);
+	/*
+	 * Read the response from the TEE. A non-zero return means that
+	 * something went wrong while communicating with the TEE.
+	 */
+	if (rc != 0)
+		panic();
+	/* Update its context to reflect the state the SP is in */
+	set_tee_pstate(tee_ctx->state, TEE_PSTATE_ON);
+}
+
+/*
+ * Return the type of TEE the TEED is dealing with. Report the current resident
+ * cpu (mpidr format) if it is a UP/UP migratable TEE.
+ */
+static int32_t teed_cpu_migrate_info(u_register_t *resident_cpu)
+{
+	(void)resident_cpu;
+	return TEE_MIGRATE_INFO;
+}
+
+/*
+ * System is about to be switched off. Allow the TEED/TEE to perform
+ * any actions needed.
+ */
+static void teed_system_off(void)
+{
+	uint32_t linear_id = plat_my_core_pos();
+	tee_context_t *tee_ctx = get_teed_sp_context(linear_id);
+	tee_vectors_t* tee_vectors_tmp = get_tee_vectors_t();
+
+	assert(tee_vectors_tmp != NULL);
+	assert(tee_ctx != NULL);
+	assert(get_tee_pstate(tee_ctx->state) == TEE_PSTATE_ON);
+
+	/* Program the entry point */
+	cm_set_elr_spsr_el3(SECURE, (uintptr_t)&tee_vectors_tmp->system_off_entry,
+			    SPSR_64(MODE_EL1, MODE_SP_ELX, DISABLE_ALL_EXCEPTIONS));
+	/*
+	 * Enter the TEE. We do not care about the return value because we
+	 * must continue the shutdown anyway
+	 */
+	NOTICE("teed system off\n");
+	(void)teed_synchronous_sp_entry(tee_ctx);
+}
+
+/*
+ * System is about to be reset. Allow the TEED/TEE to perform
+ * any actions needed.
+ */
+static void teed_system_reset(void)
+{
+	uint32_t linear_id = plat_my_core_pos();
+	tee_context_t *tee_ctx = get_teed_sp_context(linear_id);
+	tee_vectors_t *tee_vectors_tmp = get_tee_vectors_t();
+
+	assert(tee_vectors_tmp != NULL);
+	assert(tee_ctx != NULL);
+	assert(get_tee_pstate(tee_ctx->state) == TEE_PSTATE_ON);
+	/*
+	 * Abort any preempted SMC request before overwriting the SECURE
+	 * context.
+	 * Program the entry point
+	 */
+	cm_set_elr_spsr_el3(SECURE, (uintptr_t)&tee_vectors_tmp->system_reset_entry,
+			    SPSR_64(MODE_EL1, MODE_SP_ELX, DISABLE_ALL_EXCEPTIONS));
+	/*
+	 * Enter the TEE. We do not care about the return value because we
+	 * must continue the reset anyway
+	 */
+	NOTICE("teed system reset\n");
+	(void)teed_synchronous_sp_entry(tee_ctx);
+}
+
+/*
+ * Structure populated by the TEE Dispatcher to be given a chance to perform any
+ * TEE bookkeeping before PSCI executes a power management operation.
+ */
+static const spd_pm_ops_t g_teed_pm = {
+	.svc_on = teed_cpu_on_handler,
+	.svc_off = teed_cpu_off_handler,
+	.svc_suspend = teed_cpu_suspend_handler,
+	.svc_on_finish = teed_cpu_on_finish_handler,
+	.svc_suspend_finish = teed_cpu_suspend_finish_handler,
+	.svc_migrate = NULL,
+	.svc_migrate_info = teed_cpu_migrate_info,
+	.svc_system_off = teed_system_off,
+	.svc_system_reset = teed_system_reset
+};
+
+const spd_pm_ops_t *get_teed_pm()
+{
+	return &g_teed_pm;
+}

sample/teed/teed.mk

@@ -0,0 +1,37 @@
+# Copyright (C) 2022 Huawei Technologies Co., Ltd.
+# Licensed under the Mulan PSL v2.
+# You can use this software according to the terms and conditions of the Mulan PSL v2.
+# You may obtain a copy of Mulan PSL v2 at:
+#     http://license.coscl.org.cn/MulanPSL2
+# THIS SOFTWARE IS PROVIDED ON AN "AS IS" BASIS, WITHOUT WARRANTIES OF ANY KIND, EITHER EXPRESS OR
+# IMPLIED, INCLUDING BUT NOT LIMITED TO NON-INFRINGEMENT, MERCHANTABILITY OR FIT FOR A PARTICULAR
+# PURPOSE.
+# See the Mulan PSL v2 for more details.
+
+TEED_DIR  := services/spd/teed
+
+SPD_INCLUDES        := -Iservices/spd/teed/include \
+                       -Iinclude/bl31                   \
+                       -Iinclude/lib                    \
+                       -Iinclude/lib/el3_runtime        \
+					   -Iinclude/lib/psci               \
+					   -Iinclude/common                 \
+					   -Iinclude/plat/common            \
+					   -Iinclude/lib/el3_runtime/aarch64
+
+SPD_SOURCES := services/spd/teed/src/teed_common.c \
+               services/spd/teed/src/teed_helpers.S \
+               services/spd/teed/src/teed_main.c \
+               services/spd/teed/src/teed_pm.c \
+               services/spd/teed/src/teed_global.c
+
+CTX_INCLUDE_FPREGS := 1
+NEED_BL32       :=  yes
+
+#CFLAGS += -DBOOT_BL32_FROM_OTHER_EXCEPTION
+
+ifeq ($(EL3_EXCEPTION_HANDLING),1)
+ifeq ($(TEE_NS_INTR_ASYNC_PREEMPT),0)
+$(error When EL3_EXCEPTION_HANDLING=1, TEE_NS_INTR_ASYNC_PREEMPT must also be 1)
+endif
+endif

sample/teeloader/include/img_load.h

@@ -0,0 +1,52 @@
+/*
+ * Copyright (C) 2022 Huawei Technologies Co., Ltd.
+ * Licensed under the Mulan PSL v2.
+ * You can use this software according to the terms and conditions of the Mulan PSL v2.
+ * You may obtain a copy of Mulan PSL v2 at:
+ *     http://license.coscl.org.cn/MulanPSL2
+ * THIS SOFTWARE IS PROVIDED ON AN "AS IS" BASIS, WITHOUT WARRANTIES OF ANY KIND, EITHER EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO NON-INFRINGEMENT, MERCHANTABILITY OR FIT FOR A PARTICULAR
+ * PURPOSE.
+ * See the Mulan PSL v2 for more details.
+ */
+
+#ifndef TEE_LOADER_IMG_LOAD_H
+#define TEE_LOADER_IMG_LOAD_H
+
+#include <stdint.h>
+
+#ifndef ARRAY_SIZE
+#define ARRAY_SIZE(a) (sizeof(a) / sizeof((a)[0]))
+#endif
+
+#define KEY_INFO_MAX         8
+
+struct asym_key_t {
+    uint32_t key_magic;
+    uint32_t key_offset;
+    uint32_t key_size;
+};
+
+struct secure_img_header {
+    uint32_t header_size;
+    unsigned long long kernel_load_addr;
+    uint32_t kernel_size;
+    uint32_t task_num;
+    uint32_t task_total_size;
+    uint32_t got_size;
+    unsigned long long image_load_addr;
+    unsigned long long task_offset;
+    unsigned long long kernel_offset;
+    uint32_t sig_key_version;
+    unsigned long long sig_offset;
+    struct asym_key_t teeos_key_info[KEY_INFO_MAX];
+} __attribute__((packed));
+
+/*
+ * part_name: teeos partition name
+ * part_size: part_name buffer size
+ * maybe have other arguments
+ */
+int32_t load_teeos(const char *part_name, uint32_t part_size, ...);
+
+#endif

sample/teeloader/include/set_teeos_cfg.h

@@ -0,0 +1,80 @@
+/*
+ * Copyright (C) 2022 Huawei Technologies Co., Ltd.
+ * Licensed under the Mulan PSL v2.
+ * You can use this software according to the terms and conditions of the Mulan PSL v2.
+ * You may obtain a copy of Mulan PSL v2 at:
+ *     http://license.coscl.org.cn/MulanPSL2
+ * THIS SOFTWARE IS PROVIDED ON AN "AS IS" BASIS, WITHOUT WARRANTIES OF ANY KIND, EITHER EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO NON-INFRINGEMENT, MERCHANTABILITY OR FIT FOR A PARTICULAR
+ * PURPOSE.
+ * See the Mulan PSL v2 for more details.
+ */
+
+#ifndef TEE_LOADER_SET_TEEOS_CFG_H
+#define TEE_LOADER_SET_TEEOS_CFG_H
+
+#include <stdint.h>
+#include <stdbool.h>
+
+#define teelog(fmt, args...)
+
+typedef struct p_reg {
+    uint64_t start;
+    uint64_t end;
+} p_region_t;
+
+#ifndef  PAGE_SIZE
+#define PAGE_SIZE 4096
+#endif
+
+#define MAX_CONFIG_LENGTH 0x1000
+
+#define GIC_V3_VERSION              3
+#define GIC_V2_VERSION              2
+#define MAX_PROTECTED_REGIONS       16
+#define GICR_MAX_NUM    8
+#define PLAT_ENABLE_PAN (1ull << 60)
+#define PL011_TYPE         0x101
+#define PL011_GENERAL_TYPE 0x102
+#define PL011_V500_TYPE    0x103
+#define UART_LPC_TYPE      0x201
+#define UART_INVALID_TYPE  0xFFFF
+#define UART_TYPE_MASK     (32U)
+
+#define UART_ENABLE_FLAG   ((uint64_t)(0x54524155U) << UART_TYPE_MASK)
+#define UART_DISABLE_FLAG  ((uint64_t)(0x1234U) << UART_TYPE_MASK)
+
+struct gic_config_t {
+    char version;
+    union {
+        struct v2_t {
+            p_region_t dist;
+            p_region_t contr;
+        } v2;
+        struct v3_t {
+            p_region_t dist;
+            uint32_t redist_num;
+            uint32_t redist_stride;
+            p_region_t redist[GICR_MAX_NUM];
+        } v3;
+    };
+};
+
+int32_t set_teeos_mem(uintptr_t teeos_base_addr, uint64_t size);
+void set_teeos_uart(uint64_t uart_addr, uint64_t uart_type);
+bool set_protected_regions(p_region_t *protected_regions, uint32_t regions_num);
+void set_sharedmem_size(uint64_t shmem_size);
+bool set_random(uint64_t random_data);
+void set_gic(struct gic_config_t gic_config);
+void set_spi_num(uint32_t spi_num);
+void set_plat_features(uint64_t plat_features);
+bool copy_extend_datas(void *extend_datas, uint64_t extend_length);
+bool copy_teeos_cfg(void);
+
+uint64_t get_teeos_start(void);
+uint64_t get_teeos_code_start(void);
+uint64_t get_teeos_size(void);
+uint64_t get_sharedmem_start(void);
+uint64_t get_sharedmem_size(void);
+
+#endif

sample/teeloader/include/set_teeos_key.h

@@ -0,0 +1,23 @@
+/*
+ * Copyright (C) 2022 Huawei Technologies Co., Ltd.
+ * Licensed under the Mulan PSL v2.
+ * You can use this software according to the terms and conditions of the Mulan PSL v2.
+ * You may obtain a copy of Mulan PSL v2 at:
+ *     http://license.coscl.org.cn/MulanPSL2
+ * THIS SOFTWARE IS PROVIDED ON AN "AS IS" BASIS, WITHOUT WARRANTIES OF ANY KIND, EITHER EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO NON-INFRINGEMENT, MERCHANTABILITY OR FIT FOR A PARTICULAR
+ * PURPOSE.
+ * See the Mulan PSL v2 for more details.
+ */
+
+#ifndef TEEOS_IMG_HANDLE_H
+#define TEEOS_IMG_HANDLE_H
+
+#include <stdint.h>
+
+#define OEMKEY_SIZE 16
+
+int32_t load_teeos_key_info(void *image);
+int32_t trans_oemkey(uint8_t *oemkey, uint32_t oemkey_size);
+
+#endif

sample/teeloader/include/teeos_uuid.h

@@ -0,0 +1,40 @@
+/*
+ * Copyright (C) 2022 Huawei Technologies Co., Ltd.
+ * Licensed under the Mulan PSL v2.
+ * You can use this software according to the terms and conditions of the Mulan PSL v2.
+ * You may obtain a copy of Mulan PSL v2 at:
+ *     http://license.coscl.org.cn/MulanPSL2
+ * THIS SOFTWARE IS PROVIDED ON AN "AS IS" BASIS, WITHOUT WARRANTIES OF ANY KIND, EITHER EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO NON-INFRINGEMENT, MERCHANTABILITY OR FIT FOR A PARTICULAR
+ * PURPOSE.
+ * See the Mulan PSL v2 for more details.
+ */
+
+#ifndef TEEOS_UUID_H
+#define TEEOS_UUID_H
+
+#define TEE_SERVICE_SYSTEM                              \
+{                                                      \
+    0x00000000, 0x0000, 0x0000,                        \
+    {                                                  \
+        0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 \
+    }                                                  \
+}
+
+#define TEE_SERVICE_ALL                                \
+{                                                      \
+    0xFFFFFFFF, 0xFFFF, 0xFFFF,                        \
+    {                                                  \
+        0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF \
+    }                                                  \
+}
+
+#define NODE_LEN 8
+typedef struct tee_uuid {
+    uint32_t timeLow;
+    uint16_t timeMid;
+    uint16_t timeHiAndVersion;
+    uint8_t clockSeqAndNode[NODE_LEN];
+} TEE_UUID;
+
+#endif

sample/teeloader/include/tlv_sharedmem.h

@@ -0,0 +1,53 @@
+/*
+ * Copyright (C) 2022 Huawei Technologies Co., Ltd.
+ * Licensed under the Mulan PSL v2.
+ * You can use this software according to the terms and conditions of the Mulan PSL v2.
+ * You may obtain a copy of Mulan PSL v2 at:
+ *     http://license.coscl.org.cn/MulanPSL2
+ * THIS SOFTWARE IS PROVIDED ON AN "AS IS" BASIS, WITHOUT WARRANTIES OF ANY KIND, EITHER EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO NON-INFRINGEMENT, MERCHANTABILITY OR FIT FOR A PARTICULAR
+ * PURPOSE.
+ * See the Mulan PSL v2 for more details.
+ */
+
+
+#ifndef TEE_LOADER_TLV_SHAREDMEM_H
+#define TEE_LOADER_TLV_SHAREDMEM_H
+
+#include <stdint.h>
+
+#ifndef UINT64_MAX
+#define UINT64_MAX  0xffffffffffffffffull
+#endif
+
+#define MAGIC_START 0xfd544c56
+#define MAX_TAG_LEN 32
+#define MIN_TAG_LEN 3
+#define TLV_ITEM_DATA(item) ((void *)((char *)(item) + sizeof(struct tlv_item_tag)))
+
+struct tlv_item_tag {
+    char type[MAX_TAG_LEN];
+    uint32_t owner_len;
+    uint32_t length;
+    uint32_t magic;
+} __attribute__((__packed__));
+
+struct tlv_item_data {
+    char *type;
+    uint32_t type_size;
+    void *owner_list;
+    uint32_t owner_len;
+    void *value;
+    uint32_t value_len;
+} __attribute__((__packed__));
+
+struct tlv_tag {
+    uint32_t magic;
+    uint32_t tlv_num;
+    uint32_t total_len;
+} __attribute__((__packed__));
+
+uint32_t put_tlv_shared_mem(struct tlv_item_data tlv_item_data);
+uint32_t update_share_mem_tlv(struct tlv_item_data tlv_item_data);
+
+#endif

sample/teeloader/src/img_load.c

@@ -0,0 +1,102 @@
+/*
+ * Copyright (C) 2022 Huawei Technologies Co., Ltd.
+ * Licensed under the Mulan PSL v2.
+ * You can use this software according to the terms and conditions of the Mulan PSL v2.
+ * You may obtain a copy of Mulan PSL v2 at:
+ *     http://license.coscl.org.cn/MulanPSL2
+ * THIS SOFTWARE IS PROVIDED ON AN "AS IS" BASIS, WITHOUT WARRANTIES OF ANY KIND, EITHER EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO NON-INFRINGEMENT, MERCHANTABILITY OR FIT FOR A PARTICULAR
+ * PURPOSE.
+ * See the Mulan PSL v2 for more details.
+ */
+
+
+#include "img_load.h"
+
+#include <securec.h>
+#include "set_teeos_cfg.h"
+
+uintptr_t get_buf_addr(const char *part_name);
+int32_t storage_read(const char *part_name, uint32_t part_size, uintptr_t buf_addr);
+int32_t verify_sign(uintptr_t buf_addr);
+int32_t rsa_decrypt(uintptr_t buf_addr);
+
+static uintptr_t read_teeos(const char *part_name, uint32_t part_size)
+{
+    int32_t ret;
+    uintptr_t buf_addr = get_buf_addr(part_name);
+
+    ret = storage_read(part_name, part_size, buf_addr);
+    if (ret != 0)
+        return 0;
+
+    return buf_addr;
+}
+
+static int32_t verify_teeos(uintptr_t buf_addr)
+{
+    int32_t ret;
+    ret = verify_sign(buf_addr);
+
+    return ret;
+}
+
+static int32_t decrypt_teeos(uintptr_t buf_addr)
+{
+    int32_t ret;
+
+    ret = rsa_decrypt(buf_addr);
+
+    return ret;
+}
+
+static int32_t copy_teeos(uintptr_t buf_addr, uint64_t dst_addr)
+{
+    struct secure_img_header *img_header = (struct secure_img_header*)buf_addr;
+    void *src_ptr = (void *)(uintptr_t)((uint64_t)(uintptr_t)img_header + img_header->kernel_offset);
+    uint32_t size = img_header->kernel_size;
+
+    if (memcpy_s((void *)(uintptr_t)dst_addr, size, src_ptr, size) != EOK)
+        return -1;
+
+    return 0;
+}
+
+int32_t load_teeos(const char *part_name, uint32_t part_size, ...)
+{
+    /* 1st step: copy teeos to ram from flash */
+    uintptr_t buf_addr = read_teeos(part_name, part_size);
+    if (buf_addr == 0) {
+        teelog("read teeos failed\n");
+        return -1;
+    }
+
+    /* 2nd step: verify teeos */
+    if (verify_teeos(buf_addr) != 0) {
+        teelog("verify teeos failed\n");
+        return -1;
+    }
+
+    /* 3rd step: decrypt teeos */
+    if (decrypt_teeos(buf_addr) != 0) {
+        teelog("decrypt teeos failed\n");
+        return -1;
+    }
+
+    /* 4th step: copy teeos to destination */
+    uint64_t teeos_boot_addr = get_teeos_code_start();
+    if (teeos_boot_addr == 0) {
+        teelog("get teeos start addr error\n");
+        return -1;
+    }
+
+    if (copy_teeos(buf_addr, teeos_boot_addr) != 0) {
+        teelog("copy to teeos failed\n");
+        return -1;
+    }
+
+    (void)part_name;
+    (void)part_size;
+
+    return 0;
+}

sample/teeloader/src/set_teeos_cfg.c

@@ -0,0 +1,194 @@
+/*
+ * Copyright (C) 2022 Huawei Technologies Co., Ltd.
+ * Licensed under the Mulan PSL v2.
+ * You can use this software according to the terms and conditions of the Mulan PSL v2.
+ * You may obtain a copy of Mulan PSL v2 at:
+ *     http://license.coscl.org.cn/MulanPSL2
+ * THIS SOFTWARE IS PROVIDED ON AN "AS IS" BASIS, WITHOUT WARRANTIES OF ANY KIND, EITHER EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO NON-INFRINGEMENT, MERCHANTABILITY OR FIT FOR A PARTICULAR
+ * PURPOSE.
+ * See the Mulan PSL v2 for more details.
+ */
+
+#include <set_teeos_cfg.h>
+
+#include <securec.h>
+
+#include <teeos_uuid.h>
+#include <tlv_sharedmem.h>
+
+#define TEEOS_TEXT_OFFSET (0x8000)
+#define ALIGN_SIZE_2M    (0x200000)
+
+struct platform_info {
+    uint64_t plat_cfg_size;
+    uint64_t phys_region_size;
+    uint64_t phys_region_start;
+    uint64_t uart_addr;
+    uint64_t uart_type;
+    p_region_t protected_regions[MAX_PROTECTED_REGIONS];
+    uint64_t reserved;
+    uint64_t shmem_size;
+    uint32_t random_seed;
+    struct gic_config_t gic_config;
+    uint32_t spi_num_for_notify;
+    uint64_t plat_features;
+    struct extend_datas_t {
+        uint64_t extend_length;
+        char extend_paras[0];
+    } extend_datas;
+};
+
+static struct platform_info g_teeos_cfg;
+
+static uintptr_t g_teeos_base_addr = 0;
+
+int32_t set_teeos_mem(uintptr_t teeos_base_addr, uint64_t size)
+{
+    g_teeos_base_addr = teeos_base_addr;
+    g_teeos_cfg.phys_region_size = size;
+    if ((uint64_t)teeos_base_addr % ALIGN_SIZE_2M != 0)
+        return -1;
+
+    g_teeos_cfg.phys_region_start = (uint64_t)teeos_base_addr;
+
+    return 0;
+}
+
+void set_teeos_uart(uint64_t uart_addr, uint64_t uart_type)
+{
+    g_teeos_cfg.uart_addr = uart_addr;
+    g_teeos_cfg.uart_type = uart_type;
+}
+
+bool set_protected_regions(p_region_t *protected_regions, uint32_t regions_num)
+{
+    if (regions_num > MAX_PROTECTED_REGIONS)
+        return false;
+
+    if (protected_regions == NULL)
+        return false;
+
+    if (memcpy_s(g_teeos_cfg.protected_regions, sizeof(p_region_t) * regions_num,
+                 protected_regions, sizeof(p_region_t) * regions_num) != EOK)
+        return false;
+
+    return true;
+}
+
+void set_sharedmem_size(uint64_t shmem_size)
+{
+    g_teeos_cfg.shmem_size   = shmem_size;
+}
+
+bool set_random(uint64_t random_data)
+{
+    if (random_data == 0)
+        return false;
+
+    g_teeos_cfg.random_seed = random_data;
+
+    return true;
+}
+
+void set_gic(struct gic_config_t gic_config)
+{
+    g_teeos_cfg.gic_config = gic_config;
+}
+
+void set_spi_num(uint32_t spi_num)
+{
+    g_teeos_cfg.spi_num_for_notify = spi_num;
+}
+
+void set_plat_features(uint64_t plat_features)
+{
+    g_teeos_cfg.plat_features = plat_features;
+}
+
+bool copy_extend_datas(void *extend_datas, uint64_t extend_length)
+{
+    if (extend_datas == NULL)
+        return false;
+
+    if (sizeof(struct platform_info) + extend_length > MAX_CONFIG_LENGTH)
+        return false;
+
+    g_teeos_cfg.extend_datas.extend_length = extend_length;
+    char *dst = (char *)(uintptr_t)(g_teeos_cfg.phys_region_start + sizeof(g_teeos_cfg));
+
+    if (memcpy_s(dst, MAX_CONFIG_LENGTH - sizeof(g_teeos_cfg),
+                 extend_datas, extend_length) != EOK)
+        return false;
+
+    return true;
+}
+
+bool copy_teeos_cfg(void)
+{
+    if (g_teeos_cfg.phys_region_start == 0)
+        return false;
+
+    g_teeos_cfg.plat_cfg_size = sizeof(struct platform_info) + g_teeos_cfg.extend_datas.extend_length;
+    char *dst = (void *)(uintptr_t)g_teeos_cfg.phys_region_start;
+
+    if (memcpy_s(dst, sizeof(g_teeos_cfg),
+                 (char *)&g_teeos_cfg, g_teeos_cfg.plat_cfg_size - sizeof(uint64_t)) != EOK)
+        return false;
+
+    return true;
+}
+
+uint64_t get_teeos_start(void)
+{
+    return g_teeos_cfg.phys_region_start;
+}
+
+uint64_t get_teeos_code_start(void)
+{
+    return g_teeos_cfg.phys_region_start + TEEOS_TEXT_OFFSET;
+}
+
+uint64_t get_teeos_size(void)
+{
+    return g_teeos_cfg.phys_region_size;
+}
+
+uint64_t get_sharedmem_start(void)
+{
+    return g_teeos_cfg.phys_region_start + g_teeos_cfg.phys_region_size - g_teeos_cfg.shmem_size;
+}
+
+uint64_t get_sharedmem_size(void)
+{
+    return g_teeos_cfg.shmem_size;
+}
+
+#define CHIP_TYPE_TAG "chip_type"
+#define CHIP_TYPE_LEN_MAX 32
+int32_t set_chip_type_info(char *chip_type, uint32_t size)
+{
+    TEE_UUID all_service = TEE_SERVICE_ALL;
+    struct tlv_item_data tlv_item_data;
+    char chip_type_tmp[CHIP_TYPE_LEN_MAX] = {0};
+
+    if (memcpy_s(chip_type_tmp, CHIP_TYPE_LEN_MAX,
+                 chip_type, (size > CHIP_TYPE_LEN_MAX) ? CHIP_TYPE_LEN_MAX : size) != EOK ) {
+        teelog("copy to chip_type_tmp failed\n");
+        return -1;
+    }
+
+    tlv_item_data.type = CHIP_TYPE_TAG;
+    tlv_item_data.type_size = strlen(CHIP_TYPE_TAG);
+    tlv_item_data.owner_list = (void *)&all_service;
+    tlv_item_data.owner_len = sizeof(TEE_UUID);
+    tlv_item_data.value = chip_type;
+    tlv_item_data.value_len = size;
+
+    if (put_tlv_shared_mem(tlv_item_data) != 0) {
+        teelog("put chip_type info failed\n");
+        return -1;
+    }
+
+    return 0;
+}

sample/teeloader/src/set_teeos_key.c

@@ -0,0 +1,128 @@
+/*
+ * Copyright (C) 2022 Huawei Technologies Co., Ltd.
+ * Licensed under the Mulan PSL v2.
+ * You can use this software according to the terms and conditions of the Mulan PSL v2.
+ * You may obtain a copy of Mulan PSL v2 at:
+ *     http://license.coscl.org.cn/MulanPSL2
+ * THIS SOFTWARE IS PROVIDED ON AN "AS IS" BASIS, WITHOUT WARRANTIES OF ANY KIND, EITHER EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO NON-INFRINGEMENT, MERCHANTABILITY OR FIT FOR A PARTICULAR
+ * PURPOSE.
+ * See the Mulan PSL v2 for more details.
+ */
+
+#include "set_teeos_key.h"
+
+#include <string.h>
+#include <securec.h>
+
+#include "tlv_sharedmem.h"
+#include "teeos_uuid.h"
+#include "set_teeos_cfg.h"
+#include "img_load.h"
+
+#define KEY_INFO_MAIGIC_BASE 0x5a5aa501
+#define CA_RSA_MAGIC         0x5a5aa501
+#define TA_RSA_MAGIC         0x5a5aa502
+#define TA_CERT_MAGIC        0x5a5aa503
+#define TA_CONFIG_MAGIC      0x5a5aa504
+#define TA_ECIES_MAGIC       0x5a5aa505
+#define TA_WB_MAGIC          0x5a5aa506
+
+struct key_tag_info {
+    char tag[MAX_TAG_LEN];
+    uint32_t magic;
+};
+
+static struct key_tag_info g_teeos_key_tag[] = {
+    {"ca_rsa_pub_key",  CA_RSA_MAGIC},
+    {"ta_rsa_pub_2048release",  TA_RSA_MAGIC},
+    {"ta_root_pub_key", TA_CERT_MAGIC},
+    {"ta_config_pub_key", TA_CONFIG_MAGIC},
+    {"ta_decrypt_key_eciesv3_3072", TA_ECIES_MAGIC},
+    {"ta_decrypt_key_wbv3_3072", TA_WB_MAGIC},
+};
+
+static TEE_UUID g_key_perm_uuid[] = {
+    TEE_SERVICE_SYSTEM,
+};
+
+#define OEMKEY_MAGIC 0x55AA55AA
+#define OEMKEY_TAG   "oemkey"
+#define RES_NUM      52
+
+struct oemkey_info {
+    uint32_t  head_magic;
+    uint8_t   oemkey[OEMKEY_SIZE];
+    uint8_t   reserved[RES_NUM];
+    uint32_t  tail_magic;
+} __attribute__((__packed__));
+
+static int32_t trans_key_info_to_share_mem(struct asym_key_t *asym_key_info,
+                                           void *header, struct key_tag_info *tag_info)
+{
+    char *buffer = header;
+    int32_t ret;
+    struct tlv_item_data tlv_item_data;
+    if (asym_key_info->key_magic != tag_info->magic) {
+        teelog("asym_key_info->key_magic is %x, tag_info->magic is %x\n",
+               asym_key_info->key_magic, tag_info->magic);
+        return -1;
+    }
+
+    tlv_item_data.type = tag_info->tag;
+    tlv_item_data.type_size = strlen(tag_info->tag);
+    tlv_item_data.owner_list = g_key_perm_uuid;
+    tlv_item_data.owner_len = (uint32_t)sizeof(g_key_perm_uuid);
+    tlv_item_data.value = buffer + asym_key_info->key_offset;
+    tlv_item_data.value_len = asym_key_info->key_size;
+
+    ret = put_tlv_shared_mem(tlv_item_data);
+    return ret;
+}
+
+int32_t load_teeos_key_info(void *image)
+{
+    struct secure_img_header *img_header = image;
+    struct asym_key_t *asym_key_info = img_header->teeos_key_info;
+    uint32_t i;
+
+    for (i = 0; i < ARRAY_SIZE(g_teeos_key_tag); i++) {
+        if (trans_key_info_to_share_mem(asym_key_info, image, &g_teeos_key_tag[i]) != 0)
+            teelog("trans %s sharemem failed, id is %d\n", g_teeos_key_tag[i].tag, i);
+        asym_key_info++;
+    }
+
+    return 0;
+}
+
+int32_t trans_oemkey(uint8_t *oemkey, uint32_t oemkey_size)
+{
+    if (oemkey_size != OEMKEY_SIZE || oemkey == NULL) {
+        teelog("oemkey_size error\n");
+        return -1;
+    }
+    struct oemkey_info oemkey_info;
+    oemkey_info.head_magic = OEMKEY_MAGIC;
+    oemkey_info.tail_magic = OEMKEY_MAGIC;
+
+    if (memcpy_s(oemkey_info.oemkey, OEMKEY_SIZE, oemkey, OEMKEY_SIZE) != EOK) {
+        teelog("oemkey_size error\n");
+        return -1;
+    }
+
+    char oemkey_tag[OEMKEY_SIZE] = OEMKEY_TAG;
+    struct tlv_item_data tlv_item_data;
+    tlv_item_data.type = oemkey_tag;
+    tlv_item_data.type_size = strlen(oemkey_tag);
+    tlv_item_data.owner_list = g_key_perm_uuid;
+    tlv_item_data.owner_len = (uint32_t)sizeof(g_key_perm_uuid);
+    tlv_item_data.value = &oemkey_info;
+    tlv_item_data.value_len = sizeof(oemkey_info);
+
+    if (put_tlv_shared_mem(tlv_item_data) != 0) {
+        teelog("put oemkey tlv failed\n");
+        return -1;
+    }
+
+    return 0;
+}

sample/teeloader/src/tlv_sharedmem.c

@@ -0,0 +1,231 @@
+/*
+ * Copyright (C) 2022 Huawei Technologies Co., Ltd.
+ * Licensed under the Mulan PSL v2.
+ * You can use this software according to the terms and conditions of the Mulan PSL v2.
+ * You may obtain a copy of Mulan PSL v2 at:
+ *     http://license.coscl.org.cn/MulanPSL2
+ * THIS SOFTWARE IS PROVIDED ON AN "AS IS" BASIS, WITHOUT WARRANTIES OF ANY KIND, EITHER EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO NON-INFRINGEMENT, MERCHANTABILITY OR FIT FOR A PARTICULAR
+ * PURPOSE.
+ * See the Mulan PSL v2 for more details.
+ */
+
+#include "tlv_sharedmem.h"
+
+#include <stdlib.h>
+#include <string.h>
+#include <securec.h>
+
+#include "teeos_uuid.h"
+#include "set_teeos_cfg.h"
+
+static struct tlv_tag *g_tlv_start = NULL;
+static uint64_t g_teeos_share_mem = 0;
+
+static int32_t tlv_start_init()
+{
+    if (g_tlv_start != NULL) {
+        teelog("tlv already started\n");
+        return 0;
+    }
+
+    uint64_t sharedmem_start = get_sharedmem_start();
+    uint64_t sharedmem_size = get_sharedmem_size();
+    if (sharedmem_start == 0 || sharedmem_size == 0) {
+        teelog("sharedmem start addr or size error\n");
+        return -1;
+    }
+
+    if (sharedmem_size < sizeof(struct tlv_tag)) {
+        teelog("sharedmem size not enough\n");
+        return -1;
+    }
+
+    if (memset_s((void *)(uintptr_t)sharedmem_start, sizeof(struct tlv_tag),
+                 0, sizeof(struct tlv_tag)) != EOK) {
+        teelog("memset tlv start failed\n");
+        return -1;
+    }
+
+    g_teeos_share_mem = sharedmem_start + sizeof(*g_tlv_start);
+    struct tlv_tag *tlv = (struct tlv_tag *)(uintptr_t)sharedmem_start;
+    tlv->magic = MAGIC_START;
+    tlv->tlv_num = 0;
+    g_tlv_start = tlv;
+
+    return 0;
+}
+
+static int32_t alloc_teeos_shared_mem(uint64_t *addr, uint32_t length)
+{
+    if (addr == NULL || length == 0) {
+        teelog("invalid params\n");
+        return -1;
+    }
+
+    uint64_t teeos_mem_start = get_teeos_start();
+    uint64_t teeos_mem_size = get_teeos_size();
+
+    if (UINT64_MAX - g_teeos_share_mem < length) {
+        teelog("tlv length too large\n");
+        return -1;
+    }
+
+    if (g_teeos_share_mem + length > teeos_mem_start + teeos_mem_size) {
+        teelog("tlv length too large\n");
+        return -1;
+    }
+
+    if (memset_s((void *)(uintptr_t)g_teeos_share_mem, length, 0, length) != EOK) {
+        teelog("memset sharedmem to zero failed\n");
+        return -1;
+    }
+
+    *addr = g_teeos_share_mem;
+    g_teeos_share_mem += length;
+
+    return 0;
+}
+
+static int32_t share_mem_tlv_init_item(struct tlv_item_tag *new_item, struct tlv_item_data tlv_item_data)
+{
+    uint32_t magic = MAGIC_START + tlv_item_data.value_len + tlv_item_data.owner_len;
+    struct tlv_item_tag tmp_item;
+    if (new_item == NULL) {
+        teelog("invalide params\n");
+        return 1;
+    }
+
+    if (tlv_item_data.value_len == 0)
+        return 1;
+
+    (void)memset_s(&tmp_item, sizeof(tmp_item), 0, sizeof(tmp_item));
+    if (memcpy_s(tmp_item.type, MAX_TAG_LEN,
+                 tlv_item_data.type, tlv_item_data.type_size) != EOK) {
+        teelog("memcpy to tmp_item failed\n");
+        return -1;
+    }
+    tmp_item.length = tlv_item_data.value_len;
+    tmp_item.owner_len = tlv_item_data.owner_len;
+    tmp_item.magic = magic;
+
+    if (memcpy_s(new_item, sizeof(struct tlv_item_tag), &tmp_item, sizeof(struct tlv_item_tag)) != EOK) {
+        teelog("memcpy tag to new_item failed\n");
+        return -1;
+    }
+
+    if (memcpy_s((uint8_t *)(uintptr_t)TLV_ITEM_DATA(new_item), tlv_item_data.owner_len,
+                 (uint8_t *)tlv_item_data.owner_list, tlv_item_data.owner_len) != EOK) {
+        teelog("memcpy owner_list to new_item failed\n");
+        return -1;
+    }
+
+    if (memcpy_s((uint8_t *)(uintptr_t)(TLV_ITEM_DATA(new_item) + tlv_item_data.owner_len), tlv_item_data.value_len,
+                 (uint8_t *)tlv_item_data.value, tlv_item_data.value_len) != EOK) {
+        teelog("memcpy value to new_item failed\n");
+        return -1;
+    }
+    return 0;
+}
+
+struct tlv_item_tag* share_mem_tlv_find(uint64_t start_share_mem,
+                                        const struct tlv_tag *tlv, const char *type, uint32_t type_size)
+{
+    struct tlv_item_tag *pos = NULL;
+    uint32_t len;
+    uint64_t sharedmem_vaddr = g_teeos_share_mem + sizeof(struct tlv_tag);
+
+    pos = (struct tlv_item_tag *)(uintptr_t)(start_share_mem + sizeof(struct tlv_tag));
+    for (uint32_t i = 0; i < tlv->tlv_num; i++) {
+        len = sizeof(struct tlv_item_tag) + pos->length + pos->owner_len;
+        if (memcmp(pos->type, type, type_size) == 0 && strnlen(pos->type, MAX_TAG_LEN) <= type_size)
+            return pos;
+        if (i < tlv->tlv_num - 1)
+            pos = (struct tlv_item_tag *)(uintptr_t)(sharedmem_vaddr + len);
+    }
+
+    return NULL;
+}
+
+uint32_t update_share_mem_tlv(struct tlv_item_data tlv_item_data)
+{
+    struct tlv_item_tag *new_item = NULL;
+
+    new_item = share_mem_tlv_find(g_teeos_share_mem, g_tlv_start, tlv_item_data.type, tlv_item_data.type_size);
+    if (new_item == NULL) {
+        teelog("invalide params\n");
+        return 1;
+    }
+
+    if (new_item->length == tlv_item_data.value_len)
+        share_mem_tlv_init_item(new_item, tlv_item_data);
+    else
+        return 1;
+    return 0;
+}
+
+static uint32_t share_mem_tlv_append(struct tlv_tag *tlv, struct tlv_item_data tlv_item_data)
+{
+    uint64_t addr;
+    if (tlv == NULL) {
+        teelog("invalide params\n");
+        return 1;
+    }
+
+    if (tlv->magic != MAGIC_START) {
+        teelog("append error, tlv format error\n");
+        return 1;
+    }
+
+    /* length is the tlv length */
+    uint32_t total_len = tlv_item_data.value_len + tlv_item_data.owner_len + sizeof(struct tlv_item_tag);
+    uint32_t ret = alloc_teeos_shared_mem(&addr, total_len);
+    if (ret != 0) {
+        teelog("get share mem error\n");
+        return 1;
+    }
+
+    share_mem_tlv_init_item((struct tlv_item_tag *)(uintptr_t)addr, tlv_item_data);
+    tlv->tlv_num++;
+
+    uint64_t sharedmem_size = get_sharedmem_size();
+    if (tlv->total_len + sizeof(struct tlv_tag) >= sharedmem_size) {
+        teelog("out of share mem size\n");
+        return 1;
+    }
+
+    tlv->total_len += tlv_item_data.value_len + tlv_item_data.owner_len + sizeof(struct tlv_item_tag);
+    return 0;
+}
+
+uint32_t put_tlv_shared_mem(struct tlv_item_data tlv_item_data)
+{
+    uint32_t ret;
+
+    if (tlv_start_init() != 0) {
+        teelog("tlv init failed\n");
+        return 1;
+    }
+
+    if (tlv_item_data.type == NULL || tlv_item_data.value == NULL) {
+        teelog("tlv_item data error\n");
+        return 1;
+    }
+
+    if (tlv_item_data.owner_len % (uint32_t)(sizeof(TEE_UUID)) != 0) {
+        teelog("tlv_item owner_len error\n");
+        return 1;
+    }
+
+    if (tlv_item_data.type_size >= MAX_TAG_LEN || tlv_item_data.type_size < MIN_TAG_LEN) {
+        teelog("tlv_item type_size error\n");
+        return 1;
+    }
+
+    ret = share_mem_tlv_append(g_tlv_start, tlv_item_data);
+    if (ret != 0) {
+        teelog("add share mem error\n");
+        return ret;
+    }
+    return ret;
+}