return addr;
 }
 
+static void pnv_pci_ioda2_table_do_free_pages(__be64 *addr,
+               unsigned long size, unsigned int levels);
+
 static __be64 *pnv_tce(struct iommu_table *tbl, bool user, long idx, bool alloc)
 {
        __be64 *tmp = user ? tbl->it_userspace : (__be64 *) tbl->it_base;
 
        while (level) {
                int n = (idx & mask) >> (level * shift);
-               unsigned long tce;
+               unsigned long oldtce, tce = be64_to_cpu(READ_ONCE(tmp[n]));
 
-               if (tmp[n] == 0) {
+               if (!tce) {
                        __be64 *tmp2;
 
                        if (!alloc)
                        if (!tmp2)
                                return NULL;
 
-                       tmp[n] = cpu_to_be64(__pa(tmp2) |
-                                       TCE_PCI_READ | TCE_PCI_WRITE);
+                       tce = __pa(tmp2) | TCE_PCI_READ | TCE_PCI_WRITE;
+                       oldtce = be64_to_cpu(cmpxchg(&tmp[n], 0,
+                                       cpu_to_be64(tce)));
+                       if (oldtce) {
+                               pnv_pci_ioda2_table_do_free_pages(tmp2,
+                                       ilog2(tbl->it_level_size) + 3, 1);
+                               tce = oldtce;
+                       }
                }
-               tce = be64_to_cpu(tmp[n]);
 
                tmp = __va(tce & ~(TCE_PCI_READ | TCE_PCI_WRITE));
                idx &= ~mask;