pinebuds/platform/cmsis/patch.c

210 lines
5.1 KiB
C
Raw Permalink 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_8M_MAIN__
#include "patch.h"
#include "reg_patch.h"
#ifdef PATCH_CTRL_BASE
static struct PATCH_CTRL_T *const patch_ctrl =
(struct PATCH_CTRL_T *)PATCH_CTRL_BASE;
static struct PATCH_DATA_T *const patch_data =
(struct PATCH_DATA_T *)PATCH_DATA_BASE;
2022-08-15 04:20:27 -05:00
static uint32_t patch_map[(PATCH_ENTRY_NUM + 31) / 32];
int patch_open(uint32_t remap_addr) {
patch_ctrl->CTRL[0] |= PATCH_CTRL_GLOBAL_EN;
return 0;
2022-08-15 04:20:27 -05:00
}
int patch_code_enable_id(uint32_t id, uint32_t addr, uint32_t data) {
if (id >= PATCH_ENTRY_NUM) {
return 1;
}
2022-08-15 04:20:27 -05:00
patch_data->DATA[id] = data;
patch_ctrl->CTRL[id] = (patch_ctrl->CTRL[id] & ~PATCH_CTRL_ADDR_MASK) |
PATCH_CTRL_ADDR(addr) | PATCH_CTRL_ENTRY_EN;
return 0;
2022-08-15 04:20:27 -05:00
}
int patch_code_disable_id(uint32_t id) {
if (id >= PATCH_ENTRY_NUM) {
return 1;
}
2022-08-15 04:20:27 -05:00
patch_ctrl->CTRL[id] &= ~PATCH_CTRL_ENTRY_EN;
return 0;
2022-08-15 04:20:27 -05:00
}
int patch_data_enable_id(uint32_t id, uint32_t addr, uint32_t data) {
return patch_code_enable_id(id, addr, data);
2022-08-15 04:20:27 -05:00
}
int patch_data_disable_id(uint32_t id) { return patch_code_disable_id(id); }
2022-08-15 04:20:27 -05:00
static uint32_t func_patch_ins(uint32_t old_func, uint32_t new_func) {
2022-08-15 04:20:27 -05:00
#if defined(__ARM_ARCH_7EM__) || defined(__ARM_ARCH_8M_MAIN__)
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;
2022-08-15 04:20:27 -05:00
#else
#error "Only ARMv7-M/ARMv8-M function can be patched"
#endif
}
PATCH_ID patch_enable(enum PATCH_TYPE_T type, uint32_t addr, uint32_t data) {
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;
2022-08-15 04:20:27 -05:00
}
if (offset >= 0x01000000) {
// Branch distance too long to fit in a 32-bit branch instruction
return -2;
2022-08-15 04:20:27 -05:00
}
}
if (!(type == PATCH_TYPE_CODE || type == PATCH_TYPE_FUNC ||
type == PATCH_TYPE_DATA)) {
return -3;
}
for (id = 0; id < PATCH_ENTRY_NUM; id++) {
cnt = id / 32;
bit = (1 << (id % 32));
if ((patch_map[cnt] & bit) == 0) {
patch_map[cnt] |= bit;
break;
2022-08-15 04:20:27 -05:00
}
}
if (id >= PATCH_ENTRY_NUM) {
return -4;
}
idfunc = 0xFF;
if (type == PATCH_TYPE_FUNC && (addr & 2)) {
for (idfunc = id; idfunc < PATCH_ENTRY_NUM; idfunc++) {
cnt = idfunc / 32;
bit = (1 << (idfunc % 32));
if ((patch_map[cnt] & bit) == 0) {
patch_map[cnt] |= bit;
break;
}
2022-08-15 04:20:27 -05:00
}
if (idfunc >= PATCH_ENTRY_NUM) {
// Release previous patch
patch_map[cnt] &= ~bit;
return -5;
2022-08-15 04:20:27 -05:00
}
}
2022-08-15 04:20:27 -05:00
if (type == PATCH_TYPE_CODE) {
patch_code_enable_id(id, addr, data);
} else if (type == PATCH_TYPE_DATA) {
patch_data_enable_id(id, addr, data);
} else if (type == PATCH_TYPE_FUNC) {
uint32_t ins;
2022-08-15 04:20:27 -05:00
ins = func_patch_ins(addr, data);
2022-08-15 04:20:27 -05:00
if (addr & 2) {
uint32_t real_addr[2];
uint32_t real_data[2];
2022-08-15 04:20:27 -05:00
real_addr[0] = addr & ~3;
real_addr[1] = (addr + 4) & ~3;
2022-08-15 04:20:27 -05:00
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);
2022-08-15 04:20:27 -05:00
patch_code_enable_id(id, real_addr[0], real_data[0]);
patch_code_enable_id(idfunc, real_addr[1], real_data[1]);
} else {
patch_code_enable_id(id, addr, ins);
2022-08-15 04:20:27 -05:00
}
}
2022-08-15 04:20:27 -05:00
return ((id + 1) & 0xFF) | (((idfunc + 1) & 0xFF) << 8);
2022-08-15 04:20:27 -05:00
}
int patch_disable(PATCH_ID patch_id) {
uint8_t id;
uint8_t cnt, bit;
int i;
2022-08-15 04:20:27 -05:00
for (i = 0; i < 2; i++) {
id = (uint8_t)(patch_id >> (8 * i)) - 1;
2022-08-15 04:20:27 -05:00
if (id == 0xFF) {
return 0;
} else if (id >= PATCH_ENTRY_NUM) {
return -1;
}
2022-08-15 04:20:27 -05:00
patch_code_disable_id(id);
2022-08-15 04:20:27 -05:00
cnt = id / 32;
bit = (1 << (id % 32));
patch_map[cnt] &= ~bit;
}
2022-08-15 04:20:27 -05:00
return 0;
2022-08-15 04:20:27 -05:00
}
void patch_close(void) {
int i;
2022-08-15 04:20:27 -05:00
patch_ctrl->CTRL[0] &= ~PATCH_CTRL_GLOBAL_EN;
2022-08-15 04:20:27 -05:00
for (i = 0; i < PATCH_ENTRY_NUM; i++) {
patch_code_disable_id(i);
}
2022-08-15 04:20:27 -05:00
for (i = 0; i < ARRAY_SIZE(patch_map); i++) {
patch_map[i] = 0;
}
2022-08-15 04:20:27 -05:00
}
#endif
#endif