Set thread priority

To do this we need to delay driver initialization until
MsdImgDevice::Init, since setting thread priority depends having a
PlatformDevice available.

Test: "k thread list", since the current thread priority isn't visible
outside the kernel
Bug: 35229

Change-Id: I02655f29a209633b9a37d05862a8c89be671930a
diff --git a/fuchsia/msd_img_device.cc b/fuchsia/msd_img_device.cc
index 65f3089..09f2959 100644
--- a/fuchsia/msd_img_device.cc
+++ b/fuchsia/msd_img_device.cc
@@ -25,8 +25,11 @@
 {
 static uint64_t registers_physical_address_s = 0u;
 static void* registers_cpu_address_s = nullptr;
+
+static bool g_driver_initialized = false;
 } // namespace
 
+MsdImgDevice* MsdImgDevice::global_device_s_;
 
 
 MsdImgDevice::MsdImgDevice(std::unique_ptr<magma::PlatformDevice> platform_device,
@@ -36,10 +39,14 @@
 {
 	magic_ = kMagic;
 	sys_info_ = std::make_unique<FuchsiaSysInfo>();
+	DASSERT(!global_device_s_);
+	global_device_s_ = this;
 }
 
 MsdImgDevice::~MsdImgDevice()
 {
+	DASSERT(global_device_s_ == this);
+	global_device_s_ = nullptr;
 	registers_physical_address_s = 0u;
 	registers_cpu_address_s = nullptr;
 #if !defined(NO_HARDWARE)
@@ -52,11 +59,47 @@
 	{
 		PVRSRVDeviceDestroy(device_node_);
 	}
+	if (g_driver_initialized)
+	{
+		PVRSRVDriverDeInit();
+#if defined(PVRSRV_ENABLE_PROCESS_STATS)
+		PVRSRVStatsDestroy();
+#endif
+		g_driver_initialized = false;
+	}
+}
+
+bool
+MsdImgDevice::InitDriver()
+{
+	DASSERT(!g_driver_initialized);
+	PVRSRV_ERROR pvrerr;
+#if defined(PVRSRV_ENABLE_PROCESS_STATS)
+	pvrerr = PVRSRVStatsInitialise();
+	if (pvrerr != PVRSRV_OK)
+	{
+		return DRETF(false, "PVRSRVStatsInitialize failed: %d\n", pvrerr);
+	}
+#endif
+
+	pvrerr = PVRSRVDriverInit();
+	if (pvrerr != PVRSRV_OK)
+	{
+		return DRETF(false, "PVRSRVDriverInit failed: %d\n", pvrerr);
+	}
+	g_driver_initialized = true;
+	return true;
 }
 
 bool
 MsdImgDevice::Init()
 {
+	// Initialize here because it needs |global_device_s_| to be initialized to
+	// set priorities on threads.
+	if (!InitDriver())
+	{
+		return DRETF(false, "Failed to initialize driver");
+	}
 #if defined(NO_HARDWARE)
 	bus_mapper_ = std::make_unique<MockBusMapper>();
 	registers_physical_address_s = PAGE_SIZE;
diff --git a/fuchsia/msd_img_device.h b/fuchsia/msd_img_device.h
index 4d8be9f..de6612c 100644
--- a/fuchsia/msd_img_device.h
+++ b/fuchsia/msd_img_device.h
@@ -45,6 +45,8 @@
 		return msd_device;
 	}
 
+	static MsdImgDevice* GetGlobalDevice() { return global_device_s_; }
+
 	magma::PlatformDevice* platform_device() { return platform_device_.get(); }
 	magma::PlatformBusMapper* bus_mapper() { return bus_mapper_.get(); }
 
@@ -69,8 +71,10 @@
 private:
 	MsdImgDevice(std::unique_ptr<magma::PlatformDevice>,
 		     ImgSysDevice* sys_device);
+	bool InitDriver();
 
 	static const uint32_t kMagic = 'idev';
+	static MsdImgDevice* global_device_s_;
 
 	ImgSysDevice* sys_device_;
 
diff --git a/fuchsia/msd_img_driver.cc b/fuchsia/msd_img_driver.cc
index 9e82bae..e00afd3 100644
--- a/fuchsia/msd_img_driver.cc
+++ b/fuchsia/msd_img_driver.cc
@@ -13,43 +13,17 @@
 }
 
 
-static bool g_driver_initialized;
 
 MsdImgDriver::MsdImgDriver() { magic_ = kMagic; }
 
 MsdImgDriver::~MsdImgDriver()
 {
-	if (g_driver_initialized)
-	{
-		PVRSRVDriverDeInit();
-#if defined(PVRSRV_ENABLE_PROCESS_STATS)
-		PVRSRVStatsDestroy();
-#endif
-		g_driver_initialized = false;
-	}
 }
 
 // static
 std::unique_ptr<MsdImgDriver>
 MsdImgDriver::Create()
 {
-	DASSERT(!g_driver_initialized);
-	PVRSRV_ERROR pvrerr;
-#if defined(PVRSRV_ENABLE_PROCESS_STATS)
-	pvrerr = PVRSRVStatsInitialise();
-	if (pvrerr != PVRSRV_OK)
-	{
-		return DRETP(nullptr, "PVRSRVStatsInitialize failed: %d\n", pvrerr);
-	}
-#endif
-
-	pvrerr = PVRSRVDriverInit();
-	if (pvrerr != PVRSRV_OK)
-	{
-		return DRETP(nullptr, "PVRSRVDriverInit failed: %d\n", pvrerr);
-		return 0;
-	}
-	g_driver_initialized = true;
 	return std::unique_ptr<MsdImgDriver>(new MsdImgDriver);
 }
 
diff --git a/services/server/env/fuchsia/osfunc.cc b/services/server/env/fuchsia/osfunc.cc
index bfd3c71..e262b23 100644
--- a/services/server/env/fuchsia/osfunc.cc
+++ b/services/server/env/fuchsia/osfunc.cc
@@ -126,7 +126,19 @@
 {
 	static constexpr uint32_t kMagic = 'hThr';
 	uint32_t magic = kMagic;
-	std::thread thread;
+	~Thread()
+	{
+		if (running)
+		{
+			thrd_join(thread, 0);
+		}
+	}
+	// Use C11 threads so we can get the thread handle to set the priority
+	thrd_t thread = 0;
+	bool running = false;
+	std::string name;
+	void *hData = nullptr;
+	PFN_THREAD pfnThread = nullptr;
 
 	static Thread *cast(IMG_HANDLE thr)
 	{
@@ -546,6 +558,13 @@
 	misr_handle->thread = std::thread(
 		[](PFN_MISR pfnMISR, void *hData, msd_img::MisrHandle *misr_handle) {
 			magma::PlatformThreadHelper::SetCurrentThreadName("MISR Thread");
+#if !defined(NO_HARDWARE)
+			std::unique_ptr<magma::PlatformHandle> profile =
+				MsdImgDevice::GetGlobalDevice()->platform_device()->GetSchedulerProfile(
+					magma::PlatformDevice::kPriorityHighest, "img-priority");
+
+			magma::PlatformThreadHelper::SetProfile(profile.get());
+#endif // defined(NO_HARDWARE)
 			while (true)
 			{
 				zx_signals_t observed;
@@ -606,17 +625,22 @@
 	       IMG_BOOL bIsSupportingThread,
 	       void *hData)
 {
-	auto handle = std::make_unique<msd_img::Thread>();
-	handle->thread = std::thread(
-		[](PFN_THREAD thread, void *data, const std::string &thread_name) {
-			magma::PlatformThreadHelper::SetCurrentThreadName(thread_name);
-			thread(data);
-		},
-		pfnThread, hData, std::string(pszThreadName));
-	*phThread = handle.release();
-	return PVRSRV_OK;
+	return OSThreadCreatePriority(phThread, pszThreadName, pfnThread, pfnDebugDumpCB, bIsSupportingThread, hData,
+				      OS_THREAD_NOSET_PRIORITY);
 }
 
+namespace
+{
+int
+thread_func(void *data)
+{
+	msd_img::Thread *handle = msd_img::Thread::cast(data);
+	magma::PlatformThreadHelper::SetCurrentThreadName(handle->name);
+	handle->pfnThread(handle->hData);
+	return 0;
+}
+} // namespace
+
 PVRSRV_ERROR
 OSThreadCreatePriority(IMG_HANDLE *phThread,
 		       IMG_CHAR *pszThreadName,
@@ -626,16 +650,28 @@
 		       void *hData,
 		       OS_THREAD_LEVEL eThreadPriority)
 {
-	// The thread is created, but priority is ignored.
-	// TODO(MA-579): Implement.
-	return OSThreadCreate(phThread, pszThreadName, pfnThread, pfnDebugDumpCB, bIsSupportingThread, hData);
+	auto handle = std::make_unique<msd_img::Thread>();
+	handle->name = std::string(pszThreadName);
+	handle->hData = hData;
+	handle->pfnThread = pfnThread;
+	int res = thrd_create(&handle->thread, thread_func, handle.get());
+	if (res != thrd_success)
+	{
+		return PVRSRV_ERROR_INIT_FAILURE;
+	}
+	handle->running = true;
+	*phThread = handle.release();
+	if (eThreadPriority != OS_THREAD_NOSET_PRIORITY)
+	{
+		return OSSetThreadPriority(*phThread, eThreadPriority, 0);
+	}
+	return PVRSRV_OK;
 }
 
 PVRSRV_ERROR
 OSThreadDestroy(IMG_HANDLE hThread)
 {
 	msd_img::Thread *handle = msd_img::Thread::cast(hThread);
-	handle->thread.join();
 	delete handle;
 	return PVRSRV_OK;
 }
@@ -646,12 +682,43 @@
 	NOT_IMPLEMENTED();
 }
 
+magma::PlatformDevice::Priority
+OSThreadToMagmaPriority(IMG_UINT32 nThreadPriority)
+{
+	switch (nThreadPriority)
+	{
+		case OS_THREAD_HIGHEST_PRIORITY:
+		case OS_THREAD_HIGH_PRIORITY:
+			// TODO - Reduce priority of high.
+			return magma::PlatformDevice::kPriorityHighest;
+		case OS_THREAD_LOWEST_PRIORITY: return magma::PlatformDevice::kPriorityLowest;
+		case OS_THREAD_LOW_PRIORITY: return magma::PlatformDevice::kPriorityLow;
+
+		case OS_THREAD_NOSET_PRIORITY:
+		case OS_THREAD_NORMAL_PRIORITY:
+		default: return magma::PlatformDevice::kPriorityDefault;
+	}
+}
+
 PVRSRV_ERROR
 OSSetThreadPriority(IMG_HANDLE hThread, IMG_UINT32 nThreadPriority, IMG_UINT32 nThreadWeight)
 {
-	// TODO(MA-579): Implement.
-	NOT_IMPLEMENTED();
+	msd_img::Thread *handle = msd_img::Thread::cast(hThread);
+
+#if defined(NO_HARDWARE)
+	// With no hardware there's no platform device to read the profile
+	// from.
 	return PVRSRV_OK;
+#endif
+	std::unique_ptr<magma::PlatformHandle> profile =
+		MsdImgDevice::GetGlobalDevice()->platform_device()->GetSchedulerProfile(
+			OSThreadToMagmaPriority(nThreadPriority), "img-priority");
+	if (!profile)
+	{
+		return PVRSRV_ERROR_INIT_FAILURE;
+	}
+	return magma::PlatformThreadHelper::SetThreadProfile(handle->thread, profile.get()) ? PVRSRV_OK
+											    : PVRSRV_ERROR_INIT_FAILURE;
 }
 
 void *
diff --git a/services/system/fuchsia/sysconfig.cc b/services/system/fuchsia/sysconfig.cc
index d90c4c4..8e82438 100644
--- a/services/system/fuchsia/sysconfig.cc
+++ b/services/system/fuchsia/sysconfig.cc
@@ -71,6 +71,13 @@
 private:
 	void ProcessInterrupts()
 	{
+#if !defined(NO_HARDWARE)
+		std::unique_ptr<magma::PlatformHandle> profile =
+			MsdImgDevice::GetGlobalDevice()->platform_device()->GetSchedulerProfile(
+				magma::PlatformDevice::kPriorityHighest, "img-priority");
+
+		magma::PlatformThreadHelper::SetProfile(profile.get());
+#endif // defined(NO_HARDWARE)
 		while (true)
 		{
 			if (!interrupt_->Wait())