From 68d53f540aa5d433b395b4f4f583de8035639ebd Mon Sep 17 00:00:00 2001 From: hzy Date: Tue, 14 Mar 2023 18:28:06 +0000 Subject: [PATCH 3/4] soc: qcom: mdt_loader: Add necessary function to load userpd Spilt from <1678164097-13247-9-git-send-email-quic_mmanikan@quicinc.com> Signed-off-by: hzy --- drivers/soc/qcom/mdt_loader.c | 314 ++++++++++++++++++++++++++++ include/linux/soc/qcom/mdt_loader.h | 9 + 2 files changed, 323 insertions(+) diff --git a/drivers/soc/qcom/mdt_loader.c b/drivers/soc/qcom/mdt_loader.c index 6034cd8992b0..3e3146add4ae 100644 --- a/drivers/soc/qcom/mdt_loader.c +++ b/drivers/soc/qcom/mdt_loader.c @@ -16,6 +16,40 @@ #include #include #include +#include + +#include "../../remoteproc/qcom_common.h" +#define PDSEG_PAS_ID 0xD + +/** + * struct region - structure passed to TrustZone + * @addr: address of dma region, where dma blocks/chunks address resides + * @blk_size: size of each block + */ +struct region { + u64 addr; + unsigned blk_size; +}; + +/** + * struct pdseg_dma_mem_info + * @tz_addr: reference to structure passed to trustzone + * @blocks: no of blocks + * @tz_dma: dma handle of tz_addr + * @dma_blk_arr_addr_phys: dma handle of dma_blk_arr_addr + * @dma_blk_arr_addr: VA of dma array, where each index points to + * dma block PA + * @pt: stores VA of each block + */ + +struct pdseg_dma_mem_info { + struct region *tz_addr; + int blocks; + dma_addr_t tz_dma; + dma_addr_t dma_blk_arr_addr_phys; + u64 *dma_blk_arr_addr; + void **pt; +}; static bool mdt_phdr_valid(const struct elf32_phdr *phdr) { @@ -286,6 +320,259 @@ static int __qcom_mdt_load(struct device *dev, const struct firmware *fw, return ret; } +static int allocate_dma_mem(struct device *dev, + struct pdseg_dma_mem_info *pd_dma, + int max_size) +{ + dma_addr_t dma_tmp = 0; + int i; + + pd_dma->blocks = DIV_ROUND_UP(max_size, PAGE_SIZE); + + /* Allocate dma memory for structure passed to trust zone */ + pd_dma->tz_addr = dma_alloc_coherent(dev, sizeof(struct region), + &pd_dma->tz_dma, GFP_DMA); + if (!pd_dma->tz_addr) { + pr_err("Error in dma alloc\n"); + return -ENOMEM; + } + + /* Allocate dma memory to store array of blocks PA */ + pd_dma->dma_blk_arr_addr = + dma_alloc_coherent(dev, (pd_dma->blocks * sizeof(u64)), + &pd_dma->dma_blk_arr_addr_phys, GFP_DMA); + if (!pd_dma->dma_blk_arr_addr) { + pr_err("Error in dma alloc\n"); + goto free_tz_dma_alloc; + } + + /* Assign dma block array PA to trustzone structure addr variable */ + memcpy(&pd_dma->tz_addr->addr, &pd_dma->dma_blk_arr_addr_phys, + sizeof(dma_addr_t)); + + /* Allocate memory to store array of blocks VA */ + pd_dma->pt = kzalloc(pd_dma->blocks * sizeof(void *), GFP_KERNEL); + if (!pd_dma->pt) { + pr_err("Error in memory alloc\n"); + goto free_dma_blk_arr_alloc; + } + + for (i = 0; i < pd_dma->blocks; i++) { + /* Allocate dma memory for blocks with PAGE_SIZE each */ + pd_dma->pt[i] = dma_alloc_coherent(dev, PAGE_SIZE, + &dma_tmp, GFP_DMA); + if (!pd_dma->pt[i]) { + pr_err("Error in dma alloc i:%d - blocks:%d\n", i, + pd_dma->blocks); + goto free_mem_alloc; + } + + /* Assign dma block PA to dma_blk_arr_addr */ + memcpy(&pd_dma->dma_blk_arr_addr[i], &dma_tmp, + sizeof(dma_addr_t)); + } + pd_dma->tz_addr->blk_size = PAGE_SIZE; + return 0; + +free_mem_alloc: + i = 0; + while (i < pd_dma->blocks && pd_dma->pt[i]) { + memcpy(&dma_tmp, &pd_dma->dma_blk_arr_addr[i], + sizeof(dma_addr_t)); + dma_free_coherent(dev, PAGE_SIZE, pd_dma->pt[i], dma_tmp); + i++; + } + kfree(pd_dma->pt); +free_dma_blk_arr_alloc: + dma_free_coherent(dev, (pd_dma->blocks * sizeof(u64)), + pd_dma->dma_blk_arr_addr, + pd_dma->dma_blk_arr_addr_phys); +free_tz_dma_alloc: + dma_free_coherent(dev, sizeof(struct region), pd_dma->tz_addr, + pd_dma->tz_dma); + + return -ENOMEM; +} + +static void free_dma_mem(struct device *dev, struct pdseg_dma_mem_info *pd_dma) +{ + int i; + dma_addr_t dma_tmp = 0; + + for (i = 0; i < pd_dma->blocks; i++) { + memcpy(&dma_tmp, &pd_dma->dma_blk_arr_addr[i], + sizeof(dma_addr_t)); + dma_free_coherent(dev, PAGE_SIZE, pd_dma->pt[i], + dma_tmp); + } + + dma_free_coherent(dev, (pd_dma->blocks * sizeof(u64)), + pd_dma->dma_blk_arr_addr, + pd_dma->dma_blk_arr_addr_phys); + + dma_free_coherent(dev, sizeof(struct region), pd_dma->tz_addr, + pd_dma->tz_dma); + kfree(pd_dma->pt); +} + +static int memcpy_pdseg_to_dma_blk(const char *fw_name, struct device *dev, + int ph_no, struct pdseg_dma_mem_info *pd_dma) +{ + const struct firmware *seg_fw; + int ret, offset_tmp = 0, tmp = 0; + size_t size = 0; + + ret = request_firmware(&seg_fw, fw_name, dev); + if (ret) { + dev_err(dev, "failed to load %s\n", fw_name); + return ret; + } + size = seg_fw->size < PAGE_SIZE ? + seg_fw->size : PAGE_SIZE; + while (tmp < pd_dma->blocks && size) { + memset_io(pd_dma->pt[tmp], 0, PAGE_SIZE); + memcpy_toio(pd_dma->pt[tmp], seg_fw->data + offset_tmp, size); + tmp++; + offset_tmp += size; + if ((seg_fw->size - offset_tmp) < PAGE_SIZE) + size = seg_fw->size - offset_tmp; + } + release_firmware(seg_fw); + ret = qcom_scm_pas_load_seg(PDSEG_PAS_ID, ph_no, pd_dma->tz_dma, + tmp); + if (ret) { + dev_err(dev, "pd seg memcpy scm failed\n"); + return ret; + } + return ret; +} + +static int __qcom_mdt_load_pd_seg(struct device *dev, const struct firmware *fw, + const char *fw_name, int pas_id, void *mem_region, + phys_addr_t mem_phys, size_t mem_size, + phys_addr_t *reloc_base, bool pas_init) +{ + const struct elf32_phdr *phdrs; + const struct elf32_phdr *phdr; + const struct elf32_hdr *ehdr; + phys_addr_t mem_reloc; + phys_addr_t min_addr = PHYS_ADDR_MAX; + ssize_t offset; + bool relocate = false; + int ret = 0; + int i; + u8 pd_asid; + int max_size = 0; + struct pdseg_dma_mem_info pd_dma = {0}; + char *firmware_name; + size_t fw_name_len = strlen(fw_name); + + if (!fw || !mem_region || !mem_phys || !mem_size) + return -EINVAL; + + firmware_name = kstrdup(fw_name, GFP_KERNEL); + if (!firmware_name) + return -ENOMEM; + + pd_asid = qcom_get_pd_asid(dev->of_node); + + ehdr = (struct elf32_hdr *)fw->data; + phdrs = (struct elf32_phdr *)(ehdr + 1); + + for (i = 0; i < ehdr->e_phnum; i++) { + phdr = &phdrs[i]; + + if (!mdt_phdr_valid(phdr)) + continue; + /* + * While doing PD specific reloading, load only that PD + * specific writeable entries. Skip others + */ + if ((QCOM_MDT_PF_ASID(phdr->p_flags) != pd_asid) || + ((phdr->p_flags & PF_W) == 0)) + continue; + + if (phdr->p_flags & QCOM_MDT_RELOCATABLE) + relocate = true; + + if (phdr->p_paddr < min_addr) + min_addr = phdr->p_paddr; + + if (max_size < phdr->p_memsz) + max_size = phdr->p_memsz; + } + + /** + * During userpd PIL segments reloading, Q6 is live. Due to + * this we can't access memory region of PIL segments. So + * create DMA chunks/blocks to store PIL segments data. + */ + ret = allocate_dma_mem(dev, &pd_dma, max_size); + if (ret) + goto out; + + if (relocate) { + /* + * The image is relocatable, so offset each segment based on + * the lowest segment address. + */ + mem_reloc = min_addr; + } else { + /* + * Image is not relocatable, so offset each segment based on + * the allocated physical chunk of memory. + */ + mem_reloc = mem_phys; + } + + for (i = 0; i < ehdr->e_phnum; i++) { + phdr = &phdrs[i]; + + if (!mdt_phdr_valid(phdr)) + continue; + + /* + * While doing PD specific reloading, load only that PD + * specific writeable entries. Skip others + */ + if ((QCOM_MDT_PF_ASID(phdr->p_flags) != pd_asid) || + ((phdr->p_flags & PF_W) == 0)) + continue; + + offset = phdr->p_paddr - mem_reloc; + if (offset < 0 || offset + phdr->p_memsz > mem_size) { + dev_err(dev, "segment outside memory range\n"); + ret = -EINVAL; + break; + } + + if (phdr->p_filesz > phdr->p_memsz) { + dev_err(dev, + "refusing to load segment %d with p_filesz > p_memsz\n", + i); + ret = -EINVAL; + break; + } + + if (phdr->p_filesz) { + snprintf(firmware_name + fw_name_len - 3, 4, "b%02d", i); + + /* copy PIL segments data to dma blocks */ + ret = memcpy_pdseg_to_dma_blk(firmware_name, dev, i, &pd_dma); + if (ret) + goto free_dma; + } + } +free_dma: + free_dma_mem(dev, &pd_dma); + +out: + if (reloc_base) + *reloc_base = mem_reloc; + + return ret; +} + /** * qcom_mdt_load() - load the firmware which header is loaded as fw * @dev: device handle to associate resources with @@ -332,5 +619,32 @@ int qcom_mdt_load_no_init(struct device *dev, const struct firmware *fw, } EXPORT_SYMBOL_GPL(qcom_mdt_load_no_init); +/** + * qcom_mdt_load_pd_seg() - load userpd specific PIL segements + * @dev: device handle to associate resources with + * @fw: firmware object for the mdt file + * @firmware: name of the firmware, for construction of segment file names + * @pas_id: PAS identifier + * @mem_region: allocated memory region to load firmware into + * @mem_phys: physical address of allocated memory region + * @mem_size: size of the allocated memory region + * @reloc_base: adjusted physical address after relocation + * + * Here userpd PIL segements are stitched with rootpd firmware. + * This function reloads userpd specific PIL segments during SSR + * of userpd. + * + * Returns 0 on success, negative errno otherwise. + */ +int qcom_mdt_load_pd_seg(struct device *dev, const struct firmware *fw, + const char *firmware, int pas_id, void *mem_region, + phys_addr_t mem_phys, size_t mem_size, + phys_addr_t *reloc_base) +{ + return __qcom_mdt_load_pd_seg(dev, fw, firmware, pas_id, mem_region, mem_phys, + mem_size, reloc_base, true); +} +EXPORT_SYMBOL_GPL(qcom_mdt_load_pd_seg); + MODULE_DESCRIPTION("Firmware parser for Qualcomm MDT format"); MODULE_LICENSE("GPL v2"); diff --git a/include/linux/soc/qcom/mdt_loader.h b/include/linux/soc/qcom/mdt_loader.h index e600baec6825..1c63b0922ee4 100644 --- a/include/linux/soc/qcom/mdt_loader.h +++ b/include/linux/soc/qcom/mdt_loader.h @@ -7,6 +7,11 @@ #define QCOM_MDT_TYPE_MASK (7 << 24) #define QCOM_MDT_TYPE_HASH (2 << 24) #define QCOM_MDT_RELOCATABLE BIT(27) +#define QCOM_MDT_ASID_MASK 0xfu +#define QCOM_MDT_PF_ASID_SHIFT 16 +#define QCOM_MDT_PF_ASID_MASK (QCOM_MDT_ASID_MASK << QCOM_MDT_PF_ASID_SHIFT) +#define QCOM_MDT_PF_ASID(x) \ + (((x) >> QCOM_MDT_PF_ASID_SHIFT) & QCOM_MDT_ASID_MASK) struct device; struct firmware; @@ -21,6 +26,10 @@ int qcom_mdt_load_no_init(struct device *dev, const struct firmware *fw, const char *fw_name, int pas_id, void *mem_region, phys_addr_t mem_phys, size_t mem_size, phys_addr_t *reloc_base); +int qcom_mdt_load_pd_seg(struct device *dev, const struct firmware *fw, + const char *firmware, int pas_id, void *mem_region, + phys_addr_t mem_phys, size_t mem_size, + phys_addr_t *reloc_base); void *qcom_mdt_read_metadata(const struct firmware *fw, size_t *data_len); #endif -- 2.25.1