1 files changed, 97 insertions, 2 deletions

diff --git a/packages/libogcfpc/src/ogc/lwp_mutex.inc b/packages/libogcfpc/src/ogc/lwp_mutex.inc
index be01108f58..00277c38e3 100644
--- a/packages/libogcfpc/src/ogc/lwp_mutex.inc
+++ b/packages/libogcfpc/src/ogc/lwp_mutex.inc
@@ -49,8 +49,7 @@ procedure __lwp_mutex_seize_irq_blocking(mutex: Plwp_mutex; timeout: cuint64); c
 
 procedure __lwp_mutex_flush(mutex: Plwp_mutex; status: cuint32); cdecl; external;
 
-
-function __lwp_mutex_seize_irq_trylock(mutex: plwp_mutex; isr_level: pcuint32): cuint32; cdecl; external;
+function __lwp_mutex_seize_irq_trylock(mutex: plwp_mutex; isr_level: pcuint32): cuint32; inline;
 
 procedure __lwp_mutex_seize(_mutex_t: plwp_mutex; _id: cuint32; _wait: cuint8; _timeout, _level: cuint32); inline;
 
@@ -58,6 +57,32 @@ procedure __lwp_mutex_seize(_mutex_t: plwp_mutex; _id: cuint32; _wait: cuint8; _
 
 
 {$IFDEF OGC_IMPLEMENTATION}
+
+function __lwp_mutex_locked(mutex: plwp_mutex): boolean;
+begin
+	result := (mutex^.lock = LWP_MUTEX_LOCKED);
+end;
+
+function __lwp_mutex_isinheritprio(attrs: plwp_mutex_attr): boolean;
+begin
+	result := (attrs^.mode = LWP_MUTEX_INHERITPRIO);
+end;
+
+function __lwp_mutex_isprioceiling(attrs: plwp_mutex_attr): boolean;
+begin
+	result := (attrs^.mode = LWP_MUTEX_PRIORITYCEIL);
+end;
+
+function __lwp_mutex_ispriority(attrs: plwp_mutex_attr): boolean;
+begin
+	result := (attrs^.mode = LWP_MUTEX_PRIORITY);
+end;
+
+function __lwp_mutex_isfifo(attrs: plwp_mutex_attr): boolean;
+begin
+	result := (attrs^.mode = LWP_MUTEX_FIFO);
+end;
+
 procedure __lwp_mutex_seize(_mutex_t: plwp_mutex; _id: cuint32; _wait: cuint8; _timeout, _level: cuint32); inline;
 begin
   repeat
@@ -79,6 +104,76 @@ begin
 		end;
 	until false;
 end;
+
+function __lwp_mutex_seize_irq_trylock(mutex: plwp_mutex; isr_level: pcuint32): cuint32; inline;
+var
+	exec: plwp_cntrl;
+	level: cuint32;
+	prioceiling, priocurr: cuint32;
+begin
+	level := isr_level^;
+	exec := _thr_executing;
+	exec^.wait.ret_code := LWP_MUTEX_SUCCESSFUL;
+	if not __lwp_mutex_locked(mutex) then 
+	begin
+		mutex^.lock := LWP_MUTEX_LOCKED;
+		mutex^.holder := exec;
+		mutex^.nest_cnt := 1;
+		if __lwp_mutex_isinheritprio(@(mutex^.atrrs)) or __lwp_mutex_isprioceiling(@(mutex^.atrrs)) then
+			inc(exec^.res_cnt);
+		if not __lwp_mutex_isprioceiling(@(mutex^.atrrs)) then
+		begin
+			_CPU_ISR_Restore(level);
+			result := 0;
+		end;
+		begin
+			prioceiling := mutex^.atrrs.prioceil;
+			priocurr := exec^.cur_prio;
+			if (priocurr = prioceiling) then
+			begin
+				_CPU_ISR_Restore(level);
+				result := 0;
+			end;
+			if (priocurr > prioceiling) then
+			begin
+				__lwp_thread_dispatchdisable();
+				_CPU_ISR_Restore(level);
+				__lwp_thread_changepriority(mutex^.holder, mutex^.atrrs.prioceil, 0);
+				__lwp_thread_dispatchenable();
+				result := 0;
+			end;
+			exec^.wait.ret_code := LWP_MUTEX_CEILINGVIOL;
+			mutex^.nest_cnt := 0;
+			dec(exec^.res_cnt);
+			_CPU_ISR_Restore(level);
+			result := 0;
+		end;
+		result := 0;
+	end;
+
+	if (__lwp_thread_isexec(mutex^.holder)) <> 0 then
+	begin
+		case mutex^.atrrs.nest_behavior of
+			LWP_MUTEX_NEST_ACQUIRE:
+			begin
+				inc(mutex^.nest_cnt);
+				_CPU_ISR_Restore(level);
+				result := 0;
+			end;
+			LWP_MUTEX_NEST_ERROR:
+			begin
+				exec^.wait.ret_code := LWP_MUTEX_NEST_NOTALLOWED;
+				_CPU_ISR_Restore(level);
+				result := 0;
+			end;
+			LWP_MUTEX_NEST_BLOCK:
+			begin
+			end;
+		end;
+	end;
+	result := 1;
+end;
+
 //{$ifdef LIBOGC_INTERNAL}
 //{$include libogc/lwp_mutex.inl}
 //{$endif}