int msm_iommu_pagetable_map(struct msm_iommu_pt *pt, unsigned long va,
phys_addr_t pa, size_t len, int prot)
{
u64 *fl_pte;
u32 fl_offset;
u32 sl_offset;
u64 *sl_table;
u64 *sl_pte;
u64 upper_attr;
u64 lower_attr;
s32 ret;
u32 redirect = pt->redirect;
ret = common_error_check(len, pt->fl_table);
if (ret)
goto fail;
if (!pt->fl_table) {
pr_err("Null page table\n");
ret = -EINVAL;
goto fail;
}
__get_attr(prot, &upper_attr, &lower_attr);
fl_offset = FL_OFFSET(va);
fl_pte = pt->fl_table + fl_offset;
ret = handle_1st_lvl(fl_pte, pa, upper_attr, lower_attr, len, redirect);
if (ret)
goto fail;
sl_table = FOLLOW_TO_NEXT_TABLE(fl_pte);
sl_offset = SL_OFFSET(va);
sl_pte = sl_table + sl_offset;
if (len == SZ_32M)
ret = sl_32m_map(sl_pte, pa, upper_attr, lower_attr, redirect);
else if (len == SZ_2M)
ret = sl_2m_map(sl_pte, pa, upper_attr, lower_attr, redirect);
else if (len == SZ_64K || len == SZ_4K)
ret = handle_3rd_lvl(sl_pte, va, pa, upper_attr, lower_attr,
len, redirect);
fail:
return ret;
}
struct con_pair * __get_pair( char ** cp )
{
char *keyword, *value;
struct con_pair * con_p;
__get_attr( cp, &keyword, &value );
if ( keyword )
{
con_p = malloc( sizeof( *con_p ));
con_p -> keyword = keyword;
con_p -> attribute = value;
return con_p;
}
else
{
return NULL;
}
}
s32 msm_iommu_pagetable_map_range(struct msm_iommu_pt *pt, u32 va,
struct scatterlist *sg, u32 len, s32 prot)
{
phys_addr_t pa;
u32 offset = 0;
u64 *fl_pte;
u64 *sl_pte;
u32 fl_offset;
u32 sl_offset;
u64 *sl_table = NULL;
u32 chunk_size, chunk_offset = 0;
s32 ret = 0;
u64 up_at;
u64 lo_at;
u32 redirect = pt->redirect;
unsigned int start_va = va;
BUG_ON(len & (SZ_4K - 1));
if (!pt->fl_table) {
pr_err("Null page table\n");
ret = -EINVAL;
goto fail;
}
__get_attr(prot, &up_at, &lo_at);
pa = get_phys_addr(sg);
while (offset < len) {
u32 chunk_left = sg->length - chunk_offset;
fl_offset = FL_OFFSET(va);
fl_pte = pt->fl_table + fl_offset;
chunk_size = SZ_4K;
if (is_fully_aligned(va, pa, chunk_left, SZ_1G))
chunk_size = SZ_1G;
else if (is_fully_aligned(va, pa, chunk_left, SZ_32M))
chunk_size = SZ_32M;
else if (is_fully_aligned(va, pa, chunk_left, SZ_2M))
chunk_size = SZ_2M;
else if (is_fully_aligned(va, pa, chunk_left, SZ_64K))
chunk_size = SZ_64K;
trace_iommu_map_range(va, pa, sg->length, chunk_size);
ret = handle_1st_lvl(fl_pte, pa, up_at, lo_at,
chunk_size, redirect);
if (ret)
goto fail;
sl_table = FOLLOW_TO_NEXT_TABLE(fl_pte);
sl_offset = SL_OFFSET(va);
sl_pte = sl_table + sl_offset;
if (chunk_size == SZ_32M)
ret = sl_32m_map(sl_pte, pa, up_at, lo_at, redirect);
else if (chunk_size == SZ_2M)
ret = sl_2m_map(sl_pte, pa, up_at, lo_at, redirect);
else if (chunk_size == SZ_64K || chunk_size == SZ_4K)
ret = handle_3rd_lvl(sl_pte, va, pa, up_at, lo_at,
chunk_size, redirect);
if (ret)
goto fail;
offset += chunk_size;
chunk_offset += chunk_size;
va += chunk_size;
pa += chunk_size;
if (chunk_offset >= sg->length && offset < len) {
chunk_offset = 0;
sg = sg_next(sg);
pa = get_phys_addr(sg);
}
}
fail:
if (ret && offset > 0)
__msm_iommu_pagetable_unmap_range(pt, start_va, offset, 1);
return ret;
}
