270 lines
6.5 KiB
C
270 lines
6.5 KiB
C
/***************************************************************************
|
|
*
|
|
* Copyright 2015-2019 BES.
|
|
* All rights reserved. All unpublished rights reserved.
|
|
*
|
|
* No part of this work may be used or reproduced in any form or by any
|
|
* means, or stored in a database or retrieval system, without prior written
|
|
* permission of BES.
|
|
*
|
|
* Use of this work is governed by a license granted by BES.
|
|
* This work contains confidential and proprietary information of
|
|
* BES. which is protected by copyright, trade secret,
|
|
* trademark and other intellectual property rights.
|
|
*
|
|
****************************************************************************/
|
|
#ifdef __ARM_ARCH_7EM__
|
|
|
|
#include "patch.h"
|
|
#include "reg_patch_armv7m.h"
|
|
#include "plat_addr_map.h"
|
|
#include "cmsis.h"
|
|
|
|
static struct PATCH_ARMV7M_T * const patch = (struct PATCH_ARMV7M_T *)0xE0002000;
|
|
|
|
static uint32_t patch_map[(__NUM_CODE_PATCH + __NUM_LIT_PATCH + 31) / 32];
|
|
|
|
static uint32_t get_num_code(void)
|
|
{
|
|
return __NUM_CODE_PATCH;
|
|
}
|
|
|
|
static uint32_t get_num_lit(void)
|
|
{
|
|
return __NUM_LIT_PATCH;
|
|
}
|
|
|
|
static uint32_t get_remap_addr(void)
|
|
{
|
|
return GET_BITFIELD(patch->FP_REMAP, REMAP_ADDR) | (RAM_BASE & 0xE0000000);
|
|
}
|
|
|
|
int patch_open(uint32_t remap_addr)
|
|
{
|
|
patch->FP_REMAP = REMAP_ADDR(remap_addr);
|
|
patch->FP_CTRL = FP_CTRL_KEY | FP_CTRL_ENABLE;
|
|
return 0;
|
|
}
|
|
|
|
int patch_code_enable_id(uint32_t id, uint32_t addr, uint32_t data)
|
|
{
|
|
uint32_t *remap_addr;
|
|
|
|
if (id >= get_num_code()) {
|
|
return 1;
|
|
}
|
|
|
|
remap_addr = (uint32_t *)get_remap_addr();
|
|
remap_addr[id] = data;
|
|
patch->FP_COMP[id] = COMP_REPLACE(0) | COMP_ADDR(addr) | COMP_ENABLE;
|
|
return 0;
|
|
}
|
|
|
|
int patch_code_disable_id(uint32_t id)
|
|
{
|
|
if (id >= get_num_code()) {
|
|
return 1;
|
|
}
|
|
|
|
patch->FP_COMP[id] = 0;
|
|
return 0;
|
|
}
|
|
|
|
int patch_data_enable_id(uint32_t id, uint32_t addr, uint32_t data)
|
|
{
|
|
uint32_t *remap_addr;
|
|
|
|
if (id >= get_num_lit()) {
|
|
return 1;
|
|
}
|
|
id += get_num_code();
|
|
|
|
remap_addr = (uint32_t *)get_remap_addr();
|
|
remap_addr[id] = data;
|
|
patch->FP_COMP[id] = COMP_REPLACE(0) | COMP_ADDR(addr) | COMP_ENABLE;
|
|
return 0;
|
|
}
|
|
|
|
int patch_data_disable_id(uint32_t id)
|
|
{
|
|
if (id >= get_num_lit()) {
|
|
return 1;
|
|
}
|
|
id += get_num_code();
|
|
|
|
patch->FP_COMP[id] = 0;
|
|
return 0;
|
|
}
|
|
|
|
static uint32_t func_patch_ins(uint32_t old_func, uint32_t new_func)
|
|
{
|
|
#ifdef __ARM_ARCH_7EM__
|
|
union {
|
|
uint32_t d32;
|
|
uint16_t d16[2];
|
|
} ins;
|
|
uint32_t immd;
|
|
uint8_t j1, j2, s;
|
|
|
|
ins.d32 = 0x9000F000;
|
|
immd = (new_func & ~1) - ((old_func + 4) & ~1);
|
|
s = immd >> 31;
|
|
j1 = s ^ !((immd >> 23) & 1);
|
|
j2 = s ^ !((immd >> 22) & 1);
|
|
ins.d16[0] |= (s << 10) | ((immd >> 12) & 0x3FF);
|
|
ins.d16[1] |= (j1 << 13) | (j2 << 11) | ((immd >> 1) & 0x7FF);
|
|
|
|
return ins.d32;
|
|
#else
|
|
#error "Only ARMv7-M function can be patched"
|
|
#endif
|
|
}
|
|
|
|
PATCH_ID patch_enable(enum PATCH_TYPE_T type, uint32_t addr, uint32_t data)
|
|
{
|
|
uint8_t start, end;
|
|
uint8_t id, idfunc;
|
|
uint8_t cnt;
|
|
uint32_t bit;
|
|
uint32_t offset;
|
|
|
|
if (addr < ROM_BASE || addr >= (ROM_BASE + 0x20000000)) {
|
|
// Not in code region
|
|
return -1;
|
|
}
|
|
if (type == PATCH_TYPE_FUNC) {
|
|
if (data >= addr) {
|
|
offset = data - addr;
|
|
} else {
|
|
offset = addr - data;
|
|
}
|
|
if (offset >= 0x01000000) {
|
|
// Branch distance too long to fit in a 32-bit branch instruction
|
|
return -2;
|
|
}
|
|
}
|
|
|
|
if (type == PATCH_TYPE_CODE || type == PATCH_TYPE_FUNC) {
|
|
start = 0;
|
|
end = start + get_num_code();
|
|
} else if (type == PATCH_TYPE_DATA) {
|
|
start = get_num_code();
|
|
end = start + get_num_lit();
|
|
} else {
|
|
return -3;
|
|
}
|
|
|
|
for (id = start; id < end; id++) {
|
|
cnt = id / 32;
|
|
bit = (1 << (id % 32));
|
|
if ((patch_map[cnt] & bit) == 0) {
|
|
patch_map[cnt] |= bit;
|
|
break;
|
|
}
|
|
}
|
|
if (id >= end) {
|
|
return -4;
|
|
}
|
|
|
|
idfunc = 0xFF;
|
|
|
|
if (type == PATCH_TYPE_FUNC && (addr & 2)) {
|
|
for (idfunc = id; idfunc < end; idfunc++) {
|
|
cnt = idfunc / 32;
|
|
bit = (1 << (idfunc % 32));
|
|
if ((patch_map[cnt] & bit) == 0) {
|
|
patch_map[cnt] |= bit;
|
|
break;
|
|
}
|
|
}
|
|
if (idfunc >= end) {
|
|
// Release previous patch
|
|
patch_map[cnt] &= ~bit;
|
|
return -5;
|
|
}
|
|
}
|
|
|
|
if (type == PATCH_TYPE_CODE) {
|
|
patch_code_enable_id(id - start, addr, data);
|
|
} else if (type == PATCH_TYPE_DATA) {
|
|
patch_data_enable_id(id - start, addr, data);
|
|
} else if (type == PATCH_TYPE_FUNC) {
|
|
uint32_t ins;
|
|
|
|
ins = func_patch_ins(addr, data);
|
|
|
|
if (addr & 2) {
|
|
uint32_t real_addr[2];
|
|
uint32_t real_data[2];
|
|
|
|
real_addr[0] = addr & ~3;
|
|
real_addr[1] = (addr + 4) & ~3;
|
|
|
|
real_data[0] = *(volatile uint32_t *)real_addr[0];
|
|
real_data[1] = *(volatile uint32_t *)real_addr[1];
|
|
real_data[0] = (real_data[0] & 0x0000FFFF) | ((ins & 0xFFFF) << 16);
|
|
real_data[1] = (real_data[1] & 0xFFFF0000) | ((ins >> 16) & 0xFFFF);
|
|
|
|
patch_code_enable_id(id - start, real_addr[0], real_data[0]);
|
|
patch_code_enable_id(idfunc - start, real_addr[1], real_data[1]);
|
|
} else {
|
|
patch_code_enable_id(id - start, addr, ins);
|
|
}
|
|
}
|
|
|
|
return ((id + 1) & 0xFF) | (((idfunc + 1) & 0xFF) << 8);
|
|
}
|
|
|
|
int patch_disable(PATCH_ID patch_id)
|
|
{
|
|
uint8_t id;
|
|
uint8_t code_start, code_end, data_start, data_end;
|
|
uint8_t cnt, bit;
|
|
int i;
|
|
|
|
code_start = 0;
|
|
code_end = code_start + get_num_code();
|
|
data_start = code_end;
|
|
data_end = data_start + get_num_lit();
|
|
|
|
for (i = 0; i < 2; i++) {
|
|
id = (uint8_t)(patch_id >> (8 * i)) - 1;
|
|
|
|
if (id == 0xFF) {
|
|
return 0;
|
|
} else if (id >= code_start && id < code_end) {
|
|
patch_code_disable_id(id);
|
|
} else if (id >= data_start && id < data_end) {
|
|
patch_data_disable_id(id - data_start);
|
|
} else {
|
|
return -1;
|
|
}
|
|
|
|
cnt = id / 32;
|
|
bit = (1 << (id % 32));
|
|
patch_map[cnt] &= ~bit;
|
|
}
|
|
|
|
return 0;
|
|
}
|
|
|
|
void patch_close(void)
|
|
{
|
|
int i;
|
|
|
|
patch->FP_CTRL = FP_CTRL_KEY;
|
|
|
|
for (i = 0; i < get_num_code(); i++) {
|
|
patch_code_disable_id(i);
|
|
}
|
|
|
|
for (i = get_num_code(); i < (get_num_code() + get_num_lit()); i++) {
|
|
patch_data_disable_id(i);
|
|
}
|
|
|
|
for (i = 0; i < ARRAY_SIZE(patch_map); i++) {
|
|
patch_map[i] = 0;
|
|
}
|
|
}
|
|
|
|
#endif
|