ATF实现原理_arm atf

背景：

    这周本来应该发“Linux进程地址空间管理”的，但是由于准备驾考等原因耽误了。插入一篇几个月前写的文章“ATF实现原理”。这篇文章主要讲解ATF的开机流程和安全空间与非安全空间的切换原理。文章是基于MTK平台写的，也以此纪念下在MTK工作的日子——那是我职业生涯中最美好的日子。

目录

一、    ARM体系架构基础    2

1-1、AArch64 Exception LEVE    2

1-2、Secure world 与 Non Secure world    2

1-3、异常    5

二、    ATF实现原理    7

2-1、MTK平台Android开机流程以及ATF的内存布局    7

2-2、ATF的启动流程    8

2-2-1、从Preloader到ATF    8

2-2-2、ATF的开机初始化    10

2-2-3、从ATF到LK    13

2-3、Non-Secure world与 Secure world之间的切换    16

2-3-1、t-base开机初始化    16

2-3-2、Non-Secure world到 Secure world的切换    17

2-3-3、从 Secure world返回到Non-Secure world    20

一、    ARM体系架构基础

1-1、AArch64 Exception LEVE

SPSR与CPSR寄存器中每个bit的含义相同，在发生异常的时候CPSR会保存在SPSR中。CPSR/SPSR是最重要的寄存器之一，它保存了指令运行状态，异常类型、全局中断开关、 Exception LEVE 等信息。 其中SPSR的M[3:0]保存了程序所运行的Exception LEVE。M[3:0]具体定义如下面截图所示。

M[3:2]用于设置EL0~EL3，M[1]保留，M[0]表明使用SP_EL0 还是使用SP_ELX，这里的X指当前所处的Exception LEVE编号。

 

1-2、Secure world 与 Non Secure world

在Android系统中应用程序运行在Non-secure EL0，linux kernel运行在Non-secure EL1 ，TEE比如trustonic运行在Secure EL0和Secure EL1。Android和TEE的切换都必须经过EL3的monitor，在mtk平台上ATF就是一个运行在EL3上面的monitor。

 

Non Secure world是如何切换到Secure world的呢？

首先Non Secure world运行的程序需要切换到EL3，在EL3中monitor会通过设置寄存器SCR( Secure Configuration Register)的NS bit来切换到Secure world。

Non-secure world与Secure world是如何隔离开来的？

查看"arm 架构参考手册"有下面这段描述：处于Secure state的时候PE可以访问 Secure物理地址空间也可以访问Non Secure物理地址空间；处于Non-secure state的时候PE只可以访问Non Secure物理地址空间，不可以访问Secure物理地址空间。我们知道PE是通过地址来访问所有的硬件资源的。只要我们把一些硬件资源的地址区间映射到Non-secure world，另一些硬件资源（比如spi、部分dram）的地址区间映射到Secure world，就可以实现Non-secure world与Secure world的隔离了。

 

怎么实现将地址映射到Non-secure world或者Secure world？

操作系统（android 和TEE）使用的地址都是虚拟地址，虚拟地址都是需要页表转换之后才能访问到实际的物理地址。转换的过程是由MMU来完成，TTBR是页表基地址。页表中包含多个enrty，每一个entry都是按照一定规则来填写的。每一个entry中包含下一级（页表或则目标page）的物理地址和描述符（table descriptor/Block descriptor/ Page descriptor）。虚拟地址转换为物理地址的过程就是MMU解析页表找到目标物理页的过程。

关于页表的详细信息可以到arm官网下载arm架构参考手册"DDI0487A_i_armv8_arm.pdf"，这里只看与secure 相关的部分。下面是一个Page descriptor各个位域的含义，可以看到其中有一个bit是NS，在内存访问的时候MMU会检查这个bit。如果NS为0，而且访问请求来至Non-secure world，这个访问请求会被忽略，MMU会触发一个Permission fault。

 

1-3、异常

如果触发异常，程序会跳转到异常向量表去执行。Arrch64的异常类型和异常在异常向量表中的偏移见下表。整个异常向量表的大小为0x800字节，每一个entry都是0x80对齐。异常总共有4种：synchronous、IRQ、FIQ、SError。根据异常发生的exception level又分为4类：1）发生异常的exception level栈指针使用SP_EL0；2）发生异常的exception level栈指针使用SP_ELx；3）低level触发异常进入高level，而且低level使用的是64位架构；4）低level触发异常进入高level，而且低level使用的是32位架构。以linux kernel为例，kernel中收到中断将进入第2类异常入口；32位应用程序系统调用进入第3类异常入口；64位应用程序系统调用进入第4类异常入口。

与32位架构不同，64位架构的异常向量表地址不固定，在启动的时候由软件将异常向量表的地址设置到专用寄存器VBAR（Vector Base Address Register）中。除了上面的异常之外还有一种特殊的异常"复位"，复位是一种最高优先级的异常，复位异常向量表存放在寄存器RVBAR_ELx 中。从异常向量表中可知不同Exception level之间是可以切换的。为此arm定义了几条用于不同EL跳转的指令。其中HVC、SMC、SVC、可以分别从低level进入EL2、EL3、EL1。这几条指令的执行将会触发同步异常（synchronous）根据具体场景进入到异常向量表种对应的entry。

前面的几条指令都是由低level进入高level，如果要返回原来的level该怎么办？比如应用程序运行在EL0，发起系统调用进入到EL1，执行完成之后如何返回EL0？

指令ERET就是为这种场景准备的，在发生异常的时候，异常的下一条指令地址将被保存在ELR寄存器中，异常返回的时候ERET将自动把ELR中的地址保存到PC中继续异常之前的程序执行。

 

二、    ATF实现原理

前一章讲了arm体系架构对Secure的支持。这一章将从软件的角度来看arm Non-secure state和Secure state切换的具体实现。前面1-2节了解到Non-secure与Secure之间的切换需要经过EL3（secure monitor），本章要讲的ATF（Arm Trust Firmware）就是运行在EL3上的一个monitor。

2-1、MTK平台Android开机流程以及ATF的内存布局

 

开机过程中preloader把atf加载到sram中，ATF在sram中的内存布局如下：

2-2、ATF的启动流程

2-2-1、从Preloader到ATF

从"开机流程"中可以看到，在进入ATF之前系统处于preloader阶段。ATF的运行需要依赖preloader提供的参数。其中有三个重要的参数：⑴ LK入口地址；⑵ TEE 入口地址；

⑶ ATF入口地址。下面就来分析这三个地址如何获取的。

vendor/mediatek/proprietary/bootable/bootloader/preloader/platform/mt6797/src/core/main.c
void main(u32 *arg)
{
	............
    if (0 != bldr_load_images(&jump_addr)) {
        print("%s Second Bootloader Load Failed\n", MOD);
        goto error;
    }
	............
    trustzone_post_init();
	............
    bldr_jump64(jump_addr, jump_arg, sizeof(boot_arg_t));
	............
}

这是preloader的main函数，我们只关注它做的三件事：加载lk、atf、tee等到内存；设置atf参数；跳转到atf。下面分别讲解这三个函数的具体实现。

static int bldr_load_images(u32 *jump_addr)
{
	............
    addr = CFG_UBOOT_MEMADDR;
    ret = bldr_load_part_lk(bootdev, &addr, &size);
    *jump_addr = addr;
	............
    addr = CFG_ATF_ROM_MEMADDR;
    ret = bldr_load_tee_part("tee1", bootdev, &addr, 0, &size);
	............
}

函数bldr_load_images加载后续各个image，这里我们只关注lk、atf、tee。函数bldr_load_part_lk加载lk到地址CFG_UBOOT_MEMADDR。这里找到了前面提到的三个重要参数的第一个"LK入口地址"。函数bldr_load_tee_part加载atf到CFG_ATF_ROM_MEMADDR为起始的内存区域，然后加载tee到指定地址，这里的指定地址包含在tee头中。

int bldr_load_tee_part(char *name, blkdev_t *bdev, u32 *addr, u32 offset, u32 *size)
{
    ............
    ret = part_load(bdev, part, addr, offset, size);  //加载ATF
 
    u32 tee_addr = 0;
    u32 next_offset = sizeof(part_hdr_t) + *size;
 
    ret = part_load(bdev, part, &tee_addr, next_offset, size); //加载TEE到指定地址
	............
    tee_set_entry(tee_addr); //设置tee加载地址为tee入口地址
	............
}
 
void tee_set_entry(u32 addr)
{
    tee_entry_addr = addr;
	............
}

前面函数tee_set_entry设置了tee入口地址，下面将进一步将tee入口地址设置到atf的参数中。

void trustzone_post_init(void)
{
    atf_arg_t_ptr teearg = (atf_arg_t_ptr)trustzone_get_atf_boot_param_addr();
    teearg->atf_magic = ATF_BOOTCFG_MAGIC;
    teearg->tee_entry = tee_entry_addr;
    teearg->tee_boot_arg_addr = TEE_BOOT_ARG_ADDR;
	............

Atf参数字段teearg->tee_entry保存了tee入口地址，后面atf中将用到。这也是前面提到的三个重要参数中的第二个"TEE 入口地址"。参数准备好了接下来该跳转到ATF中运行了。

void trustzone_jump(u32 addr, u32 arg1, u32 arg2)
{ 
	............
    jumparch64(addr, arg1, arg2, trustzone_get_atf_boot_param_addr());
}

前面Main函数中bldr_jump64会调用到函数trustzone_jump，接着进一步调用jumparch64实现从preloader到ATF的跳转。这里需要注意的是函数jumparch64传递的参数。尤其是第一个LK 入口地址，第三个ATF参数地址。函数jumparch64是汇编代码实现，参数传递方式是r0 ~ r3分别保存第一个到第四个参数。

jumparch64:
    MOV r4, r1   /* r4 argument */
    MOV r5, r2   /* r5 argument */
    MOV r6, r0   /* keep LK jump addr */
 
    MOV r7, r3   /* r3 = TEE boot entry, relocate to r7 */
 
    /* setup the reset vector base address after warm reset to Aarch64 */
    LDR r0, =bl31_base_addr
    LDR r0,[r0]
 
    LDR r1, =rst_vector_base_addr
    LDR r1,[r1]
str r0,[r1]
	............
/* do warm reset:reset request */
MRC p15,0,r0,c12,c0,2
orr r0, r0, #2
MCR p15,0,r0,c12,c0,2
DSB 
ISB
    MOV r0, #0xC0000000
.globl WFI_LOOP
WFI_LOOP:
    WFI
B WFI_LOOP

函数jumparch64开始将r1、r2、r0、r3 分别保存在了r4、r5、r6、r7。这里需要记住的是LK的入口地址保存在了r6中。接下来将bl31_base_addr设置了到了寄存器rst_vector_base_addr中。rst_vector_base_addr就是"异常"那一节中讲到的RVBAR。我们知道RVBAR中保存的是复位向量表的基地址。这里将bl31_base_addr设置到了RVBAR中，下面看看bl31_base_addr是怎么来的：

u32 bl31_base_addr = BL31_BASE;

#define BL31_BASE (CFG_ATF_ROM_MEMADDR + BL31_VECTOR_SIZE + ATF_HEADER_SIG_SIZE)

bl31_base_addr = ATF加载地址 + 异常向量表大小 + ATF 参数头，这个值计算后是：0x00101000。

这就是前面提到的三个重要参数中的第三个"ATF入口地址"。

设置了复位向量表之后就发起复位请求，复位之后程序就从Preloader进入到了ATF。

 

2-2-2、ATF的开机初始化

前一节中preloader通过复位进入ATF。复位之后程序首先跳转到复位向量表。从前面"MTK平台Android开机流程"一节可知，复位向量表就放在bl31_entrypoint.o的开头。

vendor/mediatek/proprietary/trustzone/atf/v1.0/bl31/aarch64/bl31_entrypoint.S

.globl  bl31_entrypoint
.globl  bl31_on_entrypoint
 
func bl31_entrypoint
    ldr     x8, =BOOT_ARGUMENT_LOCATION
    str     w4, [x8]
    ldr     x8, =BOOT_ARGUMENT_SIZE
    str     w5, [x8]
    ldr     x8, =BL33_START_ADDRESS
    str     w6, [x8]
    ldr     x8, =TEE_BOOT_INFO_ADDR
    str     w7, [x8]
	............
 
    adr x1, runtime_exceptions
    msr vbar_el3, x1
    isb
	............
    bl  bl31_early_platform_setup
	............
    bl  bl31_main
b   el3_exit

在复位处理函数中做了这么几件重要的事情；保存preloader传递过来的参数，比较重要的是LK的入口地址保存到了BL33_START_ADDRESS中；设置异常限量表的基地址到VBAR中；早期平台初始化，这里面完成了Non-secure world与Secure world上下文的初始化；进入ATF的main函数中进行环境和服务的初始化；el3_exit退出ATF进入LK。具体细节后面一一详解。

void bl31_early_platform_setup(bl31_params_t *from_bl2, void *plat_params_from_bl2)
{
    ............
    memcpy((void *)>eearg, (void *)(uintptr_t)TEE_BOOT_INFO_ADDR, sizeof(atf_arg_t));
    atf_arg_t_ptr teearg = >eearg;
    ............
    SET_PARAM_HEAD(&bl32_image_ep_info,
                PARAM_EP,
                VERSION_1,
                0);
    SET_SECURITY_STATE(bl32_image_ep_info.h.attr, SECURE);
    bl32_image_ep_info.pc = teearg->tee_entry; //设置TEE的入口地址
    bl32_image_ep_info.spsr = plat_get_spsr_for_bl32_entry();
 
    SET_PARAM_HEAD(&bl33_image_ep_info,
                PARAM_EP,
                VERSION_1,
                0);
    bl33_image_ep_info.pc = plat_get_ns_image_entrypoint(); //设置LK的入口地址
    bl33_image_ep_info.spsr = plat_get_spsr_for_bl33_entry();
    ............

这个函数做的比较重要的两件事就是：设置TEE的入口地址到bl32_image_ep_info.pc；设置LK的入口地址到bl33_image_ep_info.pc。

unsigned long plat_get_ns_image_entrypoint(void)
{   
    return BL33_START_ADDRESS;
}   

BL33_START_ADDRESS;就是preloader传递进来的LK入口地址。

void bl31_main(void)
{       
    ............
 
    runtime_svc_init();
    ............
    bl31_prepare_next_image_entry();
    ............
}

bl31_main中我们比较关注的是两件事：runtime service的初始化；进入下一个image（LK）前的准备。下面先来看runtime service的初始化。

ATF中实现了多个runtime service，每一个runtime service都是用结构体rt_svc_desc来描述。服务通过start_oen、end_oen、call_type编码后来唯一识别。ATF中总共支持128个runtime service，其中call_type可以区分请求的服务是高64个还是低64个，start_oen和end_oen指定了编码宽度。Init字段保存了服务的初始化函数。Handle保存了服务处理函数。

typedef struct rt_svc_desc {
    uint8_t start_oen;
    uint8_t end_oen;
    uint8_t call_type;
    const char *name; 
    rt_svc_init_t init;
    rt_svc_handle_t handle;
} rt_svc_desc_t;

ATF编译之后这些服务都放在rt_svc_descs这个section中。

 

void runtime_svc_init(void)
{
    ............
    rt_svc_descs_num = RT_SVC_DESCS_END - RT_SVC_DESCS_START;
    rt_svc_descs_num /= sizeof(rt_svc_desc_t);
    ............
    rt_svc_descs = (rt_svc_desc_t *) RT_SVC_DESCS_START;
    for (index = 0; index < rt_svc_descs_num; index++) {
   	 	............
        if (rt_svc_descs[index].init) {
            rc = rt_svc_descs[index].init();
            if (rc) {
                ERROR("Error initializing runtime service %s\n",
                        rt_svc_descs[index].name);
                continue;
            }
        }
 
        start_idx = get_unique_oen(rt_svc_descs[index].start_oen,
                rt_svc_descs[index].call_type);
        end_idx = get_unique_oen(rt_svc_descs[index].end_oen,
                rt_svc_descs[index].call_type);
 
        for (; start_idx <= end_idx; start_idx++)
            rt_svc_descs_indices[start_idx] = index;
  		  ............
}

函数runtime_svc_init 的主要工作就是初始化数组rt_svc_descs_indices。将runtime service在rt_svc_descs中的索引写入数组rt_svc_descs_indices中对应位置。

 

2-2-3、从ATF到LK

完成runtime service的初始化之后就为跳转到下一个image（LK）做准备。

void bl31_prepare_next_image_entry(void)
{
    ............
    image_type = bl31_get_next_image_type();
    next_image_info = bl31_plat_get_next_image_ep_info(image_type);
    ............
    cm_init_context(read_mpidr_el1(), next_image_info);
    cm_prepare_el3_exit(image_type);
}

前面函数首先获取image_type，接下来我们程序将跳转到LK中，LK运行在Non-secure环境中，所以这里的image_type等于NON_SECURE，接下来设置Non-secure上下文，最后在cm_prepare_el3_exit中将栈指针SP指向上下文内存。

entry_point_info_t *bl31_plat_get_next_image_ep_info(uint32_t type)
{   
    if (type == NON_SECURE) 
        return &bl33_image_ep_info;
    else
        return &bl32_image_ep_info;
    ............
}

由前面分析可知函数bl31_plat_get_next_image_ep_info 将返回bl33_image_ep_info，并作为参数传递到cm_init_context中进一步初始化Non-secure上下文。

在继续讲解之前先认识一个结构体cpu_context_t，这就是前面提到的执行上下文，他包含了系统通用寄存器，和其他系统专用寄存器。部分寄存器内容如下描述。

 

void cm_init_context(uint64_t mpidr, const entry_point_info_t *ep)
{
    ctx = cm_get_context_by_mpidr(mpidr, security_state);
	............
    if (security_state != SECURE)
        scr_el3 |= SCR_NS_BIT; //设置为Non-secure
    ............
    /* Populate EL3 state so that we've the right context before doing ERET */
    state = get_el3state_ctx(ctx);
    write_ctx_reg(state, CTX_SCR_EL3, scr_el3);
    write_ctx_reg(state, CTX_ELR_EL3, ep->pc);
    write_ctx_reg(state, CTX_SPSR_EL3, ep->spsr);
	............

cm_init_context中初始化Non-secure上下文，最重要的两个地方是配置SCR寄存器为Non-secure；和设置下一步跳转的程序入口地址ep->pc设置到ctx-> el3state_ctxde 的CTX_ELR_EL3中。在前面函数bl31_early_platform_setup中有讲到ep->pc中保存的是LK的入口地址。上下文设置完成接下来就需要将这些值设置到栈中，以便最终加载到对应寄存器。

void cm_prepare_el3_exit(uint32_t security_state)
{
    cpu_context_t *ctx = cm_get_context(security_state);
    ............
    cm_set_next_context(ctx);
}
 
static inline void cm_set_next_context(void *context)
{
    ............
 
    __asm__ volatile("msr   spsel, #1\n"
				"mov   sp, %0\n"
				"msr   spsel, #0\n"
            	 : : "r" (context));
}

上面两个函数做的事就是将上下文设置到栈中，让sp指向ctx。走到这里ATF的启动就完成了，下一个image的运行上下文也准备好了，最后要做的是就是跳转到下一个image中去执行。

el3_exit: ; .type el3_exit, %function
 
    mov x17, sp
    msr spsel, #1
    str x17, [sp, #CTX_EL3STATE_OFFSET + CTX_RUNTIME_SP]
 
    ldr x18, [sp, #CTX_EL3STATE_OFFSET + CTX_SCR_EL3]
    ldp x16, x17, [sp, #CTX_EL3STATE_OFFSET + CTX_SPSR_EL3] //将spsr和elr分别保存到x16和x17中
    msr scr_el3, x18
    msr spsr_el3, x16
    msr elr_el3, x17 //elr 也就是前面保存的LK入口地址加载到elr中。
 
    b   restore_gp_registers_eret
 
func restore_gp_registers_eret
    ............
    ldp x30, x17, [sp, #CTX_GPREGS_OFFSET + CTX_GPREG_LR]
    msr sp_el0, x17
    ldp x16, x17, [sp, #CTX_GPREGS_OFFSET + CTX_GPREG_X16]
    eret

函数el3_exit中先将LK的入口地址保存到elr中然后调用异常返回指令eret返回。前面讲过eret这条指令的调用将会导致elr加载到pc中，程序就会到elr指向的地址继续执行。这就完成ATF到LK的跳转。

2-3、Non-Secure world与 Secure world之间的切换

2-3-1、t-base开机初始化

下面以tbase为例来讲解Non-Secure world与 Secure world之间的切换过程。文件tbase_fastcall中定义了名为tbase_fastcall的runtime service。

vendor/mediatek/proprietary/trustzone/atf/v1.0/services/spd/tbase/tbase_fastcall.c

DECLARE_RT_SVC(
  tbase_fastcall,
  OEN_TOS_START,
  OEN_TOS_END,
  SMC_TYPE_FAST,
  tbase_fastcall_setup,
  tbase_fastcall_handler
);

宏DECLARE_RT_SVC根据传入的参数设置结构体rt_svc_desc的各个成员，并且保证连接到rt_svc_descs这个section。tbase_fastcall_setup是这个service的初始化函数，它在函数runtime_svc_init中调用，主要工作是设置Secure上下文，如下：

void runtime_svc_init(void)
{
    ............
        if (rt_svc_descs[index].init) {
            rc = rt_svc_descs[index].init();
             ............
        }
 
int32_t tbase_fastcall_setup(void)
{
  entry_point_info_t *image_info;
  image_info = bl31_plat_get_next_image_ep_info(SECURE);
  ............
  tbase_init_eret(image_info->pc,TBASE_AARCH32);
}

这里函数bl31_plat_get_next_image_ep_info将返回 bl32_image_ep_info。image_info->pc作为参数传递到tbase_init_eret中，前面函数bl31_early_platform_setup中有讲到image_info->pc保存的是TEE的入口地址。其中函数tbase_init_eret设置了Secure上下文。

static void tbase_init_eret( uint64_t entrypoint, uint32_t rw ) {
    ............
  scr &= ~SCR_NS_BIT;
    ............
    ctx = cm_get_context(SECURE);
    ............
    state = get_el3state_ctx(ctx);
    write_ctx_reg(state, CTX_SPSR_EL3, spsr);
    write_ctx_reg(state, CTX_ELR_EL3, entrypoint);
    write_ctx_reg(state, CTX_SCR_EL3, scr);
    ............
}

这个函数中两个重要的地方是配置SCR寄存器为Secure；和设置下一步跳转的程序入口地址entrypoint，设置到ctx-> el3state_ctxde 的CTX_ELR_EL3中。这里的entrypoint就是前面传入的TEE入口地址。

2-3-2、Non-Secure world到 Secure world的切换

Linux kernel运行在Non-Secure EL1，如果要进入TEE，首先需要调用汇编指令smc进入EL3，由monitor（ATF）来完成Non-Secure world到 Secure world的切换。在mtk平台上函数mt_secure_call是进入EL3的入口函数，它调用smc指令并通过x0~x3传入四个参数。其中x0中是多个位域的一个编码，根据它可以找到哪个service以及service中的哪一项服务。

static noinline int mt_secure_call(u64 function_id, u64 arg0, u64 arg1, u64 arg2)
{   
    register u64 reg0 __asm__("x0") = function_id;
    register u64 reg1 __asm__("x1") = arg0;
    register u64 reg2 __asm__("x2") = arg1;
    register u64 reg3 __asm__("x3") = arg2;
    int ret = 0;
 
    asm volatile ("smc    #0\n" : "+r" (reg0) :
"r"(reg1), "r"(reg2), "r"(reg3));
 
    ret = (int)reg0;
    return ret;
}

前面运行指令smc触发一个同步异常，进入EL3异常向量表对应同步异常入口。

smc_handler64:
    ............
 
    mov x6, sp
    ............
    ubfx    x16, x0, #FUNCID_OEN_SHIFT, #FUNCID_OEN_WIDTH
    ubfx    x15, x0, #FUNCID_TYPE_SHIFT, #FUNCID_TYPE_WIDTH
    orr x16, x16, x15, lsl #FUNCID_OEN_WIDTH
 
    adr x11, (__RT_SVC_DESCS_START__ + RT_SVC_DESC_HANDLE)
 
    /* Load descriptor index from array of indices */
    adr x14, rt_svc_descs_indices
    ldrb    w15, [x14, x16]
    ............
    lsl w10, w15, #RT_SVC_SIZE_LOG2
    ldr x15, [x11, w10, uxtw]
    ............
    mrs x16, spsr_el3
    mrs x17, elr_el3
    mrs x18, scr_el3
    stp x16, x17, [x6, #CTX_EL3STATE_OFFSET + CTX_SPSR_EL3]
    str x18, [x6, #CTX_EL3STATE_OFFSET + CTX_SCR_EL3]
    ............
    blr x15

函数smc_handler64主要做了三件事：

⑴ 根据function_id找到对应的runtime service,查找方法：

Index = (function_id >> 24 & 0x3f) | ((function_id >> 31) << 6)；

Handler = __RT_SVC_DESCS_START__ + RT_SVC_DESC_HANDLE + rt_svc_descs_indices[Index] << 5

__RT_SVC_DESCS_START__为rt_svc_descs的起始地址，RT_SVC_DESC_HANDLE为服务处理函数handle在结构体rt_svc_desc中的偏移，左移5，是因为结构体rt_svc_desc大小为32字节。

⑵ 保存Non-Secure world中的spsr_el3 、elr_el3、scr_el3到栈中。

⑶ 跳转到runtime service的处理函数handle中执行

前面提到了runtime service，tbase_fastcall，这个service的服务处理函数是tbase_fastcall_handler其具体实现如下：

static uint64_t tbase_fastcall_handler(uint32_t smc_fid,
        uint64_t x1,
        uint64_t x2,
        uint64_t x3,
        uint64_t x4,
        void *cookie,
        void *handle,
        uint64_t flags)
{
  uint64_t mpidr = read_mpidr();
  uint32_t linear_id = platform_get_core_pos(mpidr);
  tbase_context *tbase_ctx = &secure_context[linear_id];
  int caller_security_state = flags&1;
 
  if (caller_security_state==SECURE) {
    switch(maskSWdRegister(smc_fid)) {
      case TBASE_SMC_FASTCALL_RETURN: {
        DBG_PRINTF( "tbase_fastcall_handler TBASE_SMC_FASTCALL_RETURN\n\r");
        tbase_synchronous_sp_exit(tbase_ctx, 0, 1);
      }
      ............
  else
  {
    gp_regs_t *ns_gpregs = get_gpregs_ctx((cpu_context_t *)handle);
    write_ctx_reg(ns_gpregs, CTX_GPREG_X0, smc_fid ); // These are not saved yet
    write_ctx_reg(ns_gpregs, CTX_GPREG_X1, x1 );
    write_ctx_reg(ns_gpregs, CTX_GPREG_X2, x2 );
    write_ctx_reg(ns_gpregs, CTX_GPREG_X3, x3 );
    cm_el1_sysregs_context_save(NON_SECURE);
    ............
    tbase_synchronous_sp_entry(tbase_ctx);
    cm_el1_sysregs_context_restore(NON_SECURE);
    cm_set_next_eret_context(NON_SECURE);
    return 0; 
  }

Linux kernel运行在Non-Secure中，所以这里程序会走下一个分支。这里主要做了三件重要的事：⑴保存异常前的寄存器值到Non-secure上下文中⑵调用函数tbase_synchronous_sp_entry进入TEE环境 ⑶准备Non-secure上下文，异常返回，返回到linux kernel中。下面分别讲解。

uint64_t tbase_synchronous_sp_entry(tbase_context *tbase_ctx)
{
    ............
    cm_set_next_eret_context(SECURE);
 
    rc = tbase_enter_sp(&tbase_ctx->c_rt_ctx);
    ............
}
 
void cm_set_next_eret_context(uint32_t security_state)
{
    cpu_context_t *ctx;
 
    ctx = cm_get_context(security_state);
    assert(ctx);
 
    cm_set_next_context(ctx);
}

函数cm_get_context设置secure上下文到栈中，然后调用函数cm_set_next_context进入TEE程序。

func tbase_enter_sp
    mov x3, sp
    str x3, [x0, #0]  //保存sp到tbase_ctx->c_rt_ctx中
    sub sp, sp, #TSPD_C_RT_CTX_SIZE
 
    /* Save callee-saved registers on to the stack */
    stp x19, x20, [sp, #TSPD_C_RT_CTX_X19]
    stp x21, x22, [sp, #TSPD_C_RT_CTX_X21]
    stp x23, x24, [sp, #TSPD_C_RT_CTX_X23]
    stp x25, x26, [sp, #TSPD_C_RT_CTX_X25]
    stp x27, x28, [sp, #TSPD_C_RT_CTX_X27]
    stp x29, x30, [sp, #TSPD_C_RT_CTX_X29] //保存lr（x30）到栈中
 
    b   el3_exit

func表明tbase_enter_sp是一个函数，调用这个函数会将函数返回地址保存到lr（x30）中，这一点很重要，后面还会用到。然后跳转到el3_exit 做异常返回，这个代码块前面讲过。在函数tbase_fastcall_setup中有将secure上下文中的elr设置为了TEE入口地址所以这里异常返回，将会返回到TEE中。

2-3-3、从 Secure world返回到Non-Secure world

程序进入TEE中完成指定的任务之后最终还会返回到linux kernel中。在返回到linux kernel之前会先返回到ATF中。下面接着再看tbase_fastcall_handler这个函数。这个函数有两个分支，前面讲了后一个分支。TEE是在Secure world运行的，在TEE返回的时候程序就会走到前一个分支，下面看程序具体如何运行：

static uint64_t tbase_fastcall_handler(uint32_t smc_fid,
        uint64_t x1,
        uint64_t x2,
        uint64_t x3,
        uint64_t x4,
        void *cookie,
        void *handle,
        uint64_t flags)
{
  uint64_t mpidr = read_mpidr();
  uint32_t linear_id = platform_get_core_pos(mpidr);
  tbase_context *tbase_ctx = &secure_context[linear_id];
  int caller_security_state = flags&1;
 
  if (caller_security_state==SECURE) {
    switch(maskSWdRegister(smc_fid)) {
      case TBASE_SMC_FASTCALL_RETURN: {
        DBG_PRINTF( "tbase_fastcall_handler TBASE_SMC_FASTCALL_RETURN\n\r");
        tbase_synchronous_sp_exit(tbase_ctx, 0, 1);
      }
    ............
  else
  {
    ............
  }

函数先会调用tbase_synchronous_sp_exit来做实际的返回操作。

func tbase_exit_sp
    mov sp, x0  //前面x0保存了栈指针
 
    ldp x19, x20, [x0, #(TSPD_C_RT_CTX_X19 - TSPD_C_RT_CTX_SIZE)]
    ldp x21, x22, [x0, #(TSPD_C_RT_CTX_X21 - TSPD_C_RT_CTX_SIZE)]
    ldp x23, x24, [x0, #(TSPD_C_RT_CTX_X23 - TSPD_C_RT_CTX_SIZE)]
    ldp x25, x26, [x0, #(TSPD_C_RT_CTX_X25 - TSPD_C_RT_CTX_SIZE)]
    ldp x27, x28, [x0, #(TSPD_C_RT_CTX_X27 - TSPD_C_RT_CTX_SIZE)]
    ldp x29, x30, [x0, #(TSPD_C_RT_CTX_X29 - TSPD_C_RT_CTX_SIZE)] //将lr值从栈中加载到lr（x30）中
    mov x0, x1
    ret

汇编指令ret做程序返回，这条指令会导致lr自动加载到pc中，然后程序跳转到lr指定的地址运行。这个与eret指令类似，不过那里保存返回地址的是elr。这里的函数返回又会返回到函数tbase_fastcall_handler中执行，如下：

static uint64_t tbase_fastcall_handler(uint32_t smc_fid,
        uint64_t x1,
        uint64_t x2,
        uint64_t x3,
        uint64_t x4,
        void *cookie,
        void *handle,
        uint64_t flags)
{
      ............
  if (caller_security_state==SECURE) {
      ............
  else
  {
    ............
    tbase_synchronous_sp_entry(tbase_ctx); //这里进入TEE
    cm_el1_sysregs_context_restore(NON_SECURE); // TEE返回后会返回到这里执行
    cm_set_next_eret_context(NON_SECURE);
    return 0; 
  }

cm_el1_sysregs_context_restore(NON_SECURE);将SECURE环境的寄存器保存到NON_SECURE的上下文中，这其中就包含TEE环境的返回值。然后通过函数cm_set_next_eret_context(NON_SECURE);设置NON_SECURE的上下文到栈中，最后程序返回异常处理函数smc_handler64通过el3_exit返回NON_SECURE world中的linux kernel

void cm_set_next_eret_context(uint32_t security_state)
{ 
    cpu_context_t *ctx;
 
    ctx = cm_get_context(security_state);
    assert(ctx);
    cm_set_next_context(ctx);
}   

函数cm_set_next_eret_context执行完成之后，函数tbase_fastcall_handler就返回了，返回到异常处理函数smc_handler64中，具体如下：

smc_handler64:
    ............
    mov x6, sp
    ............
    blr x15  //根据function id进入对应的service handle
el3_exit: ; .type el3_exit, %function   //service handle函数返回后继续完成退出的工作
    ............
    msr scr_el3, x18
    msr spsr_el3, x16
    msr elr_el3, x17
    b   restore_gp_registers_eret
 
func restore_gp_registers_eret
    ............
    msr sp_el0, x17
    ldp x16, x17, [sp, #CTX_GPREGS_OFFSET + CTX_GPREG_X16]
    eret  //实现异常返回

函数tbase_fastcall_handler中Non-secure分支会调用函数cm_el1_sysregs_context_save(NON_SECURE); 保存异常前的寄存器值到Non-secure上下文中，这其中就包含elr，由于是从linux kernel中调用smc下来的，所以这里elr保存的就是linux kernel中的地址，所以这里异常返回就会返回到linux中。