ARM v8 SMMU Stage 2 translation fails

Hi,

Working on a SoC with MMU_600. There is a baremetal code which runs in EL3. SMMU is configured for stage 1 and stage 2 translation, VA to PA is one to one mapping.

Configuring Stage 1 only works fine for 4K granule, but when stage 1 and stage 2 are enabled stage 2 fails with a translation fault.

Does EL3 support stage 2 translation ? or any suggestions on configurations. 

Thank you,

code snippet Stage 2 config:

S2VMID = sid;
S2T0SZ = TTBCR_T0SZ(40); // (64 - 40)
S2SL0 = STE_S2SL0_4KBTG0_L1_16_64KBTG0_L2; // 1
S2IR0 = STE_S2IR0_WBC_RA_WA; // 1
S2OR0 = STE_S2OR0_WBC_RA_WA; // 1
S2SH0 = STE_S2SH0_IS; // 3
S2TG = granul_size; // 0 == 4K
S2PS = STE_S2PS_40b; // 2
S2AA64 = STE_S2AA64_AARCH64; // 1
S2ENDI = STE_S2ENDI_LE; 0
S2AFFD = STE_S2AFFD_NOFLT; // 1
S2R = 1;