posix: Add pthread_setschedprio()

Close #2734.

3 files changed, 85 insertions, 0 deletions

diff --git a/cpukit/posix/Makefile.am b/cpukit/posix/Makefile.am
index 9f73501cb0..461e6ebbc5 100644
--- a/cpukit/posix/Makefile.am
+++ b/cpukit/posix/Makefile.am
@@ -133,6 +133,7 @@ libposix_a_SOURCES += src/pthreadatfork.c src/pthreadattrdestroy.c \
     src/pthreadself.c \
     src/pthreadsetschedparam.c src/pthreadsigmask.c \
     src/psxpriorityisvalid.c src/psxtransschedparam.c
+libposix_a_SOURCES += src/pthreadsetschedprio.c
 
 ## RTEMS specific support methods
 libposix_a_SOURCES += src/pthreadattrcompare.c
diff --git a/cpukit/posix/src/pthreadsetschedprio.c b/cpukit/posix/src/pthreadsetschedprio.c
new file mode 100644
index 0000000000..856e49df04
--- /dev/null
+++ b/cpukit/posix/src/pthreadsetschedprio.c
@@ -0,0 +1,54 @@
+/*
+ * Copyright (c) 2016 embedded brains GmbH
+ *
+ * The license and distribution terms for this file may be
+ * found in the file LICENSE in this distribution or at
+ * http://www.rtems.org/license/LICENSE.
+ */
+
+#if HAVE_CONFIG_H
+#include "config.h"
+#endif
+
+#include <pthread.h>
+#include <errno.h>
+
+#include <rtems/posix/priorityimpl.h>
+#include <rtems/posix/threadsup.h>
+#include <rtems/score/threadimpl.h>
+
+int pthread_setschedprio( pthread_t thread, int prio )
+{
+  Thread_Control    *the_thread;
+  Per_CPU_Control   *cpu_self;
+  POSIX_API_Control *api;
+  Priority_Control   unused;
+  ISR_lock_Context   lock_context;
+  Priority_Control   new_priority;
+
+  if ( !_POSIX_Priority_Is_valid( prio ) ) {
+    return EINVAL;
+  }
+
+  new_priority = _POSIX_Priority_To_core( prio );
+
+  the_thread = _Thread_Get( thread, &lock_context );
+
+  if ( the_thread == NULL ) {
+    return ESRCH;
+  }
+
+  api = the_thread->API_Extensions[ THREAD_API_POSIX ];
+
+  cpu_self = _Thread_Dispatch_disable_critical( &lock_context );
+
+  _Thread_State_acquire_critical( the_thread, &lock_context );
+  api->schedparam.sched_priority = prio;
+  api->Attributes.schedparam.sched_priority = prio;
+  _Thread_State_release( the_thread, &lock_context );
+
+  _Thread_Set_priority( the_thread, new_priority, &unused, true );
+
+  _Thread_Dispatch_enable( cpu_self );
+  return 0;
+}
diff --git a/testsuites/psxtests/psx05/init.c b/testsuites/psxtests/psx05/init.c
index 6e7229e42f..bbc863afae 100644
--- a/testsuites/psxtests/psx05/init.c
+++ b/testsuites/psxtests/psx05/init.c
@@ -200,6 +200,16 @@ static void test_set_priority( void )
 
   rtems_test_assert( counter == 1 );
 
+  status = pthread_setschedprio( pthread_self(), param.sched_priority + 1 );
+  rtems_test_assert( status == 0 );
+
+  rtems_test_assert( counter == 1 );
+
+  status = pthread_setschedprio( pthread_self(), param.sched_priority );
+  rtems_test_assert( status == 0 );
+
+  rtems_test_assert( counter == 1 );
+
   counter = -1;
   sched_yield();
 
@@ -209,6 +219,25 @@ static void test_set_priority( void )
   rtems_test_assert( counter == -1 );
 }
 
+static void test_errors_pthread_setschedprio( void )
+{
+  int                status;
+  int                policy;
+  struct sched_param param;
+
+  status = pthread_getschedparam( pthread_self(), &policy, &param );
+  rtems_test_assert( status == 0 );
+
+  status = pthread_setschedprio( pthread_self(), INT_MAX );
+  rtems_test_assert( status == EINVAL );
+
+  status = pthread_setschedprio( 0xdeadbeef, param.sched_priority );
+  rtems_test_assert( status == ESRCH );
+
+  status = pthread_setschedprio( pthread_self(), param.sched_priority );
+  rtems_test_assert( status == 0 );
+}
+
 void *POSIX_Init(
   void *argument
 )
@@ -232,6 +261,7 @@ void *POSIX_Init(
 
   test_get_priority();
   test_set_priority();
+  test_errors_pthread_setschedprio();
 
   /* set the time of day, and print our buffer in multiple ways */