pinebuds/platform/cmsis/patch_armv7m.c

271 lines
6.5 KiB
C
Raw Normal View History

2022-08-15 04:20:27 -05:00
/***************************************************************************
*
* 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