diff --git a/drivers/firmware/arm_ffa/driver.c b/drivers/firmware/arm_ffa/driver.c index 0f468362c288..bc2685331b27 100644 --- a/drivers/firmware/arm_ffa/driver.c +++ b/drivers/firmware/arm_ffa/driver.c @@ -32,6 +32,7 @@ #include #include #include +#include #include #include #include @@ -59,7 +60,9 @@ (FIELD_PREP(SENDER_ID_MASK, (s)) | FIELD_PREP(RECEIVER_ID_MASK, (r))) #define RXTX_MAP_MIN_BUFSZ_MASK GENMASK(1, 0) -#define RXTX_MAP_MIN_BUFSZ(x) ((x) & RXTX_MAP_MIN_BUFSZ_MASK) +#define RXTX_MAP_MAX_BUFSZ_MASK GENMASK(31, 16) +#define RXTX_MAP_MIN_BUFSZ(x) (FIELD_GET(RXTX_MAP_MIN_BUFSZ_MASK, (x))) +#define RXTX_MAP_MAX_BUFSZ(x) (FIELD_GET(RXTX_MAP_MAX_BUFSZ_MASK, (x))) #define FFA_MAX_NOTIFICATIONS 64 @@ -2101,7 +2104,7 @@ static int ffa_probe(struct platform_device *pdev) { int ret; u32 buf_sz; - size_t rxtx_bufsz = SZ_4K; + size_t rxtx_min_bufsz = SZ_4K, rxtx_max_bufsz = 0, rxtx_bufsz; if (IS_BUILTIN(CONFIG_ARM_FFA_TRANSPORT) && is_protected_kvm_enabled() && !is_pkvm_initialized()) @@ -2132,15 +2135,18 @@ static int ffa_probe(struct platform_device *pdev) ret = ffa_features(FFA_FN_NATIVE(RXTX_MAP), 0, &buf_sz, NULL); if (!ret) { if (RXTX_MAP_MIN_BUFSZ(buf_sz) == 1) - rxtx_bufsz = SZ_64K; + rxtx_min_bufsz = SZ_64K; else if (RXTX_MAP_MIN_BUFSZ(buf_sz) == 2) - rxtx_bufsz = SZ_16K; + rxtx_min_bufsz = SZ_16K; else - rxtx_bufsz = SZ_4K; + rxtx_min_bufsz = SZ_4K; + + rxtx_max_bufsz = RXTX_MAP_MAX_BUFSZ(buf_sz) * SZ_4K; + if (rxtx_max_bufsz != 0 && rxtx_max_bufsz < rxtx_min_bufsz) + rxtx_max_bufsz = rxtx_min_bufsz; } - rxtx_bufsz = PAGE_ALIGN(rxtx_bufsz); - drv_info->rxtx_bufsz = rxtx_bufsz; + rxtx_bufsz = min_not_zero(PAGE_ALIGN(rxtx_min_bufsz), rxtx_max_bufsz); drv_info->rx_buffer = alloc_pages_exact(rxtx_bufsz, GFP_KERNEL); if (!drv_info->rx_buffer) { ret = -ENOMEM; @@ -2156,10 +2162,17 @@ static int ffa_probe(struct platform_device *pdev) ret = ffa_rxtx_map(virt_to_phys(drv_info->tx_buffer), virt_to_phys(drv_info->rx_buffer), rxtx_bufsz / FFA_PAGE_SIZE); + if (ret == -EINVAL && !rxtx_max_bufsz && rxtx_min_bufsz < rxtx_bufsz) { + rxtx_bufsz = rxtx_min_bufsz; + ret = ffa_rxtx_map(virt_to_phys(drv_info->tx_buffer), + virt_to_phys(drv_info->rx_buffer), + rxtx_bufsz / FFA_PAGE_SIZE); + } if (ret) { pr_err("failed to register FFA RxTx buffers\n"); goto free_pages; } + drv_info->rxtx_bufsz = rxtx_bufsz; mutex_init(&drv_info->rx_lock); mutex_init(&drv_info->tx_lock);