pinebuds/platform/cmsis/patch.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_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;

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;
}

int patch_code_enable_id(uint32_t id, uint32_t addr, uint32_t data)
{
    if (id >= PATCH_ENTRY_NUM) {
        return 1;
    }

    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;
}

int patch_code_disable_id(uint32_t id)
{
    if (id >= PATCH_ENTRY_NUM) {
        return 1;
    }

    patch_ctrl->CTRL[id] &= ~PATCH_CTRL_ENTRY_EN;
    return 0;
}

int patch_data_enable_id(uint32_t id, uint32_t addr, uint32_t data)
{
    return patch_code_enable_id(id, addr, data);
}

int patch_data_disable_id(uint32_t id)
{
    return patch_code_disable_id(id);
}

static uint32_t func_patch_ins(uint32_t old_func, uint32_t new_func)
{
#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;
#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;
        }
        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 || 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;
        }
    }
    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;
            }
        }
        if (idfunc >= PATCH_ENTRY_NUM) {
            // Release previous patch
            patch_map[cnt] &= ~bit;
            return -5;
        }
    }

    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;

        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, 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);
        }
    }

    return ((id + 1) & 0xFF) | (((idfunc + 1) & 0xFF) << 8);
}

int patch_disable(PATCH_ID patch_id)
{
    uint8_t id;
    uint8_t cnt, bit;
    int i;

    for (i = 0; i < 2; i++) {
        id = (uint8_t)(patch_id >> (8 * i)) - 1;

        if (id == 0xFF) {
            return 0;
        } else if (id >= PATCH_ENTRY_NUM) {
            return -1;
        }

        patch_code_disable_id(id);

        cnt = id / 32;
        bit = (1 << (id % 32));
        patch_map[cnt] &= ~bit;
    }

    return 0;
}

void patch_close(void)
{
    int i;

    patch_ctrl->CTRL[0] &= ~PATCH_CTRL_GLOBAL_EN;

    for (i = 0; i < PATCH_ENTRY_NUM; i++) {
        patch_code_disable_id(i);
    }

    for (i = 0; i < ARRAY_SIZE(patch_map); i++) {
        patch_map[i] = 0;
    }
}
#endif
#endif