fix TCB and TLS

Signed-off-by: Anhad Singh <andypython@protonmail.com>

src/header/dl-tls/mod.rs

@@ -1,5 +1,9 @@
 //! dl-tls implementation for Redox
 
+use core::alloc::Layout;
+
+use alloc::alloc::alloc_zeroed;
+
 use crate::{ld_so::tcb::Tcb, platform::types::*};
 
 #[repr(C)]
@@ -10,53 +14,58 @@ pub struct dl_tls_index {
 
 #[no_mangle]
 pub unsafe extern "C" fn __tls_get_addr(ti: *mut dl_tls_index) -> *mut c_void {
+    let tcb = Tcb::current().unwrap();
+
     trace!(
-        "__tls_get_addr({:p}: {:#x}, {:#x})",
+        "__tls_get_addr({:p}: {:#x}, {:#x}, masters_len={}, dtv_len={})",
         ti,
         (*ti).ti_module,
-        (*ti).ti_offset
+        (*ti).ti_offset,
+        tcb.masters().unwrap().len(),
+        tcb.dtv_mut().len()
     );
-    let mod_id = (*ti).ti_module as usize;
-    let offset = if cfg!(target_arch = "riscv64") {
-        ((*ti).ti_offset as usize).wrapping_add(0x800) // dynamic offsets are 0x800-based on risc-v
-    } else {
-        (*ti).ti_offset as usize
-    };
-    if mod_id > 0 {
-        if let Some(tcb) = Tcb::current() {
-            if let Some(masters) = tcb.masters() {
-                if let Some(master) = masters.get(mod_id - 1) {
-                    // module id is 1-based
-                    let addr = if cfg!(any(target_arch = "x86", target_arch = "x86_64")) {
-                        tcb.tls_end.sub(master.offset).add(offset)
-                    } else {
-                        // FIXME aarch64/risc-v only support static master
-                        if mod_id == 1 && offset < tcb.tls_len {
-                            tcb.tls_end.sub(tcb.tls_len).add(offset)
-                        } else {
-                            0 as *mut u8
-                        }
-                    };
-                    if !addr.is_null() {
-                        trace!(
-                            "__tls_get_addr({:p}: {:#x}, {:#x}) = {:p}",
-                            ti,
-                            (*ti).ti_module,
-                            (*ti).ti_offset,
-                            addr
-                        );
-                        return addr as *mut c_void;
-                    }
-                }
-            }
-        }
+
+    if tcb.dtv_mut().unwrap_or_default().len() < tcb.masters().unwrap().len() {
+        // Reallocate DTV.
+        tcb.setup_dtv(tcb.masters().unwrap().len());
     }
-    panic!(
-        "__tls_get_addr({:p}: {:#x}, {:#x}) failed",
-        ti,
-        (*ti).ti_module,
-        (*ti).ti_offset
-    );
+
+    let ti = &*ti;
+    let dtv_index = ti.ti_module as usize - 1;
+
+    if tcb.dtv_mut().unwrap()[dtv_index].is_null() {
+        // Allocate TLS for module.
+        let master = &tcb.masters().unwrap()[ti.ti_module as usize - 1];
+
+        // FIXME(andypython): master.align
+        let layout = unsafe {
+            Layout::from_size_align_unchecked(master.offset /* aligned ph.p_memsz */, 16)
+        };
+
+        let module_tls = alloc_zeroed(layout);
+
+        core::ptr::copy_nonoverlapping(master.ptr, module_tls, master.len);
+
+        // Set the DTV entry.
+        tcb.dtv_mut().unwrap()[dtv_index] = module_tls;
+    }
+
+    let mut ptr = tcb.dtv_mut().unwrap()[dtv_index];
+
+    if cfg!(target_arch = "riscv64") {
+        ptr = ptr.add(0x800 + ti.ti_offset as usize); // dynamic offsets are 0x800-based on risc-v
+    } else {
+        ptr = ptr.add(ti.ti_offset as usize);
+    }
+
+    if ptr.is_null() {
+        panic!(
+            "__tls_get_addr({ti:p}: {:#x}, {:#x})",
+            ti.ti_module, ti.ti_offset
+        );
+    }
+
+    ptr.cast::<c_void>()
 }
 
 // x86 can define a version that does not require stack alignment