pinebuds/platform/cmsis/patch_armv7m.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 "cmsis.h"
#include "patch.h"
#include "plat_addr_map.h"
#include "reg_patch_armv7m.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