From d08793a456caa327a250c19a9f5d2bbce70e75ea Mon Sep 17 00:00:00 2001
From: Janne Grunau <j@jannau.net>
Date: Tue, 29 Mar 2022 13:10:32 +0200
Subject: [PATCH] dcp: Free iovad_dcp including dart L2 tables on shutdown

Signed-off-by: Janne Grunau <j@jannau.net>
---
 src/dart.c  | 25 +++++++++++++++++++++++++
 src/dart.h  |  1 +
 src/dcp.c   |  3 ++-
 src/iova.c  | 15 ++++++++++++++-
 src/iova.h  |  3 ++-
 src/types.h |  1 +
 6 files changed, 45 insertions(+), 3 deletions(-)

diff --git a/src/dart.c b/src/dart.c
index e2816e08..8fa1c304 100644
--- a/src/dart.c
+++ b/src/dart.c
@@ -271,6 +271,31 @@ void dart_unmap(dart_dev_t *dart, uintptr_t iova, size_t len)
     dart_tlb_invalidate(dart);
 }
 
+void dart_free_l2(dart_dev_t *dart, uintptr_t iova)
+{
+    if (iova & ((1 << 25) - 1)) {
+        printf("dart: %08lx is not at the start of L2 table\n", iova);
+        return;
+    }
+
+    u32 ttbr = (iova >> 36) & 0x3;
+    u32 l1_index = (iova >> 25) & 0x7ff;
+
+    if (!(dart->l1[ttbr][l1_index] & DART_PTE_VALID))
+        return;
+
+    u64 *l2 = dart_get_l2(dart, l1_index);
+
+    for (u32 idx = 0; idx < 2048; idx++) {
+        if (l2[idx] & DART_PTE_VALID) {
+            printf("dart: %08lx is still mapped\n", iova + (idx << 14));
+            return;
+        }
+    }
+    dart->l1[ttbr][l1_index] = 0;
+    free(l2);
+}
+
 static void *dart_translate_internal(dart_dev_t *dart, uintptr_t iova, int silent)
 {
     u32 ttbr = (iova >> 36) & 0x3;
diff --git a/src/dart.h b/src/dart.h
index ab00f671..9ed49249 100644
--- a/src/dart.h
+++ b/src/dart.h
@@ -11,6 +11,7 @@ dart_dev_t *dart_init(uintptr_t base, u8 device, bool keep_pts);
 dart_dev_t *dart_init_adt(const char *path, int instance, int device, bool keep_pts);
 int dart_map(dart_dev_t *dart, uintptr_t iova, void *bfr, size_t len);
 void dart_unmap(dart_dev_t *dart, uintptr_t iova, size_t len);
+void dart_free_l2(dart_dev_t *dart, uintptr_t iova);
 void *dart_translate(dart_dev_t *dart, uintptr_t iova);
 u64 dart_search(dart_dev_t *dart, void *paddr);
 s64 dart_find_iova(dart_dev_t *dart, s64 start, size_t len);
diff --git a/src/dcp.c b/src/dcp.c
index 547e44dd..08f15e46 100644
--- a/src/dcp.c
+++ b/src/dcp.c
@@ -48,7 +48,7 @@ dcp_dev_t *dcp_init(const char *dcp_path, const char *dcp_dart_path, const char
     rtkit_hibernate(dcp->rtkit);
     rtkit_free(dcp->rtkit);
 out_iovad:
-    iovad_shutdown(dcp->iovad_dcp);
+    iovad_shutdown(dcp->iovad_dcp, dcp->dart_dcp);
     dart_shutdown(dcp->dart_disp);
 out_dart_dcp:
     dart_shutdown(dcp->dart_dcp);
@@ -62,6 +62,7 @@ int dcp_shutdown(dcp_dev_t *dcp)
     rtkit_hibernate(dcp->rtkit);
     rtkit_free(dcp->rtkit);
     dart_shutdown(dcp->dart_disp);
+    iovad_shutdown(dcp->iovad_dcp, dcp->dart_dcp);
     dart_shutdown(dcp->dart_dcp);
     free(dcp);
 
diff --git a/src/iova.c b/src/iova.c
index d29d0b75..c3146cd9 100644
--- a/src/iova.c
+++ b/src/iova.c
@@ -12,11 +12,18 @@ struct iova_block {
 };
 
 struct iova_domain {
+    u64 base;
+    u64 limit;
     struct iova_block *free_list;
 };
 
 iova_domain_t *iovad_init(u64 base, u64 limit)
 {
+    if (base != ALIGN_UP(base, SZ_32M)) {
+        printf("iovad_init: base it not is not aligned to SZ_32M\n");
+        return NULL;
+    }
+
     iova_domain_t *iovad = malloc(sizeof(*iovad));
     if (!iovad)
         return NULL;
@@ -33,12 +40,14 @@ iova_domain_t *iovad_init(u64 base, u64 limit)
     blk->iova = base;
     blk->sz = limit - SZ_16K;
     blk->next = NULL;
+    iovad->base = base;
+    iovad->limit = limit;
     iovad->free_list = blk;
 
     return iovad;
 }
 
-void iovad_shutdown(iova_domain_t *iovad)
+void iovad_shutdown(iova_domain_t *iovad, dart_dev_t *dart)
 {
     struct iova_block *blk = iovad->free_list;
 
@@ -49,6 +58,10 @@ void iovad_shutdown(iova_domain_t *iovad)
         free(blk_free);
     }
 
+    if (dart)
+        for (u64 addr = iovad->base; addr < iovad->limit; addr += SZ_32M)
+            dart_free_l2(dart, addr);
+
     free(iovad);
 }
 
diff --git a/src/iova.h b/src/iova.h
index 5d533d9d..1637be43 100644
--- a/src/iova.h
+++ b/src/iova.h
@@ -3,12 +3,13 @@
 #ifndef IOVA_H
 #define IOVA_H
 
+#include "dart.h"
 #include "types.h"
 
 typedef struct iova_domain iova_domain_t;
 
 iova_domain_t *iovad_init(u64 base, u64 limit);
-void iovad_shutdown(iova_domain_t *iovad);
+void iovad_shutdown(iova_domain_t *iovad, dart_dev_t *dart);
 
 bool iova_reserve(iova_domain_t *iovad, u64 iova, size_t sz);
 u64 iova_alloc(iova_domain_t *iovad, size_t sz);
diff --git a/src/types.h b/src/types.h
index 899f17fc..6fd07894 100644
--- a/src/types.h
+++ b/src/types.h
@@ -47,6 +47,7 @@ typedef s64 ssize_t;
 #define SZ_4K  (1 << 12)
 #define SZ_16K (1 << 14)
 #define SZ_1M  (1 << 20)
+#define SZ_32M (1 << 25)
 
 #ifdef __ASSEMBLER__