From 73178c81534012d1b08919dfd7cea13ebcbfdf72 Mon Sep 17 00:00:00 2001
From: Florian Kaltenberger <florian.kaltenberger@eurecom.fr>
Date: Wed, 9 Jan 2019 20:49:15 +0100
Subject: [PATCH] Nr cpu affinity2

---
 cmake_targets/build_oai                       |  4 +++
 .../SYRTEM_IQ_QuickStart.readme               | 12 +++++--
 targets/COMMON/threads_t.h                    | 27 ++++++++--------
 targets/RT/USER/lte-softmodem.c               |  8 +++--
 targets/RT/USER/lte-softmodem.h               |  3 +-
 targets/RT/USER/lte-ue.c                      |  8 ++---
 targets/RT/USER/lte-uesoftmodem.c             |  9 ++++--
 targets/RT/USER/nr-ru.c                       |  5 +--
 targets/RT/USER/nr-softmodem.c                | 11 ++++---
 targets/RT/USER/nr-softmodem.h                |  6 ----
 targets/RT/USER/nr-ue.c                       | 16 +++++-----
 targets/RT/USER/nr-uesoftmodem.c              | 31 ++++++++++++++++---
 targets/RT/USER/nr-uesoftmodem.h              | 17 +++++-----
 targets/RT/USER/rt_wrapper.c                  | 28 +++++++++++------
 14 files changed, 117 insertions(+), 68 deletions(-)

diff --git a/cmake_targets/build_oai b/cmake_targets/build_oai
index f4b64026e1..f018e8c422 100755
--- a/cmake_targets/build_oai
+++ b/cmake_targets/build_oai
@@ -339,6 +339,10 @@ function main() {
             FORCE_DEADLINE_SCHEDULER_FLAG_USER="True"
             echo_info "Enabling the usage of deadline scheduler"
             shift 1;;
+       --enable-cpu-affinity)
+            CPU_AFFINITY_FLAG_USER="True"
+            echo_info "Enabling CPU Affinity (only valid when not using deadline scheduler)"
+            shift 1;;
        --disable-cpu-affinity)
             CPU_AFFINITY_FLAG_USER="False"
             echo_info "Disabling CPU Affinity (only valid when not using deadline scheduler)"
diff --git a/targets/ARCH/ADRV9371_ZC706/SYRTEM_IQ_QuickStart.readme b/targets/ARCH/ADRV9371_ZC706/SYRTEM_IQ_QuickStart.readme
index d7f6585a75..cd74fdb397 100755
--- a/targets/ARCH/ADRV9371_ZC706/SYRTEM_IQ_QuickStart.readme
+++ b/targets/ARCH/ADRV9371_ZC706/SYRTEM_IQ_QuickStart.readme
@@ -115,8 +115,16 @@ sudo cset shield --reset
 cd eur_oai_develop-nr/openairinterface5g/
 source oaienv
 
-sudo -E ./cmake_targets/build_oai -c --gNB -w ADRV9371_ZC706 -I
-sudo -E ./cmake_targets/build_oai -c --gNB -w ADRV9371_ZC706
+sudo -E ./cmake_targets/build_oai -c --gNB -w ADRV9371_ZC706 -I --enable-cpu-affinity
+
+/* CPU available shall be mapped on CPU2 to get_nprocs() number with hyperthreading disabled */
+/* because : */
+/* - cset shield leave CPU0 for system */
+/* - CPU1 is reserved for HwRxTx thread for I/Q acquisition ! */
+/* Note: "/usr/local/etc/syriq/cpu-irq.sh" shall be changed if more than 8 cores are used */
+/*   ie. "cset shield --force --kthread on -c 1-7" where "-c 1-7" reserves CPU1 to CPU7 for application (CPU0 is for system) */
+
+sudo -E ./cmake_targets/build_oai -c --gNB -w ADRV9371_ZC706 --enable-cpu-affinity
 
 
 
diff --git a/targets/COMMON/threads_t.h b/targets/COMMON/threads_t.h
index 99fb0d8cf4..0671f40988 100644
--- a/targets/COMMON/threads_t.h
+++ b/targets/COMMON/threads_t.h
@@ -2,19 +2,20 @@
 #define _THREADS_T_H_
 
 typedef struct threads_s {
-    int iq;
-    int one;
-    int two;
-    int three;
-    int slot1_proc_one;
-    int slot1_proc_two;
-    int slot1_proc_three;
-    int dlsch_td_one;
-	int dlsch_td_two;
-	int dlsch_td_three;
-	int dlsch_td1_one;
-	int dlsch_td1_two;
-	int dlsch_td1_three;
+  int main;
+  int sync;
+  int one;
+  int two;
+  int three;
+  int slot1_proc_one;
+  int slot1_proc_two;
+  int slot1_proc_three;
+  //int dlsch_td_one;
+  //int dlsch_td_two;
+  //int dlsch_td_three;
+  //int dlsch_td1_one;
+  //int dlsch_td1_two;
+  //int dlsch_td1_three;
 } threads_t;
 
 #endif /* _THREADS_T_H_ */
diff --git a/targets/RT/USER/lte-softmodem.c b/targets/RT/USER/lte-softmodem.c
index 03a461454f..8dbd1d51c9 100644
--- a/targets/RT/USER/lte-softmodem.c
+++ b/targets/RT/USER/lte-softmodem.c
@@ -253,7 +253,7 @@ extern char uecap_xer[1024];
 char uecap_xer_in=0;
 
 int oaisim_flag=0;
-threads_t threads= {-1,-1,-1,-1,-1,-1,-1};
+threads_t threads= {-1,-1,-1,-1,-1,-1,-1,-1};
 
 /* see file openair2/LAYER2/MAC/main.c for why abstraction_flag is needed
  * this is very hackish - find a proper solution
@@ -906,14 +906,16 @@ int main( int argc, char **argv )
   char cpu_affinity[1024];
   CPU_ZERO(&cpuset);
 #ifdef CPU_AFFINITY
+  int j;
   if (get_nprocs() > 2) {
-    CPU_SET(0, &cpuset);
+    for (j = 2; j < get_nprocs(); j++)
+      CPU_SET(j, &cpuset);
     s = pthread_setaffinity_np(pthread_self(), sizeof(cpu_set_t), &cpuset);
     if (s != 0) {
       perror( "pthread_setaffinity_np");
       exit_fun("Error setting processor affinity");
     }
-    LOG_I(HW, "Setting the affinity of main function to CPU 0, for device library to use CPU 0 only!\n");
+    LOG_I(HW, "Setting the affinity of main function to all CPUs, for device library to use CPU 0 only!\n");
   }
 #endif
   
diff --git a/targets/RT/USER/lte-softmodem.h b/targets/RT/USER/lte-softmodem.h
index 96ab799c3d..0870405fac 100644
--- a/targets/RT/USER/lte-softmodem.h
+++ b/targets/RT/USER/lte-softmodem.h
@@ -169,7 +169,8 @@
 {"clock",                   CONFIG_HLP_CLK,         0,                      uptr:&clock_source,                 defintval:0,                    TYPE_UINT,      0},                     \
 {"wait-for-sync",           NULL,                   PARAMFLAG_BOOL,         iptr:&wait_for_sync,                defintval:0,                    TYPE_INT,       0},                     \
 {"single-thread-enable",    CONFIG_HLP_NOSNGLT,     PARAMFLAG_BOOL,         iptr:&single_thread_flag,           defintval:0,                    TYPE_INT,       0},                     \
-{"threadIQ",                NULL,                   0,                      iptr:&(threads.iq),                 defintval:1,                    TYPE_INT,       0},                     \
+{"threadMain",                NULL,                   0,                      iptr:&(threads.main),                 defintval:1,                    TYPE_INT,       0},                     \
+{"threadSync",                NULL,                   0,                      iptr:&(threads.sync),                 defintval:1,                    TYPE_INT,       0},                     \
 {"threadOneSubframe",       NULL,                   0,                      iptr:&(threads.one),                defintval:1,                    TYPE_INT,       0},                     \
 {"threadTwoSubframe",       NULL,                   0,                      iptr:&(threads.two),                defintval:1,                    TYPE_INT,       0},                     \
 {"threadThreeSubframe",     NULL,                   0,                      iptr:&(threads.three),              defintval:1,                    TYPE_INT,       0},                     \
diff --git a/targets/RT/USER/lte-ue.c b/targets/RT/USER/lte-ue.c
index 965203f47e..a9a880dde7 100644
--- a/targets/RT/USER/lte-ue.c
+++ b/targets/RT/USER/lte-ue.c
@@ -456,8 +456,8 @@ static void *UE_thread_synch(void *arg)
 
   cpu_set_t cpuset;
   CPU_ZERO(&cpuset);
-  if ( threads.iq != -1 )
-    CPU_SET(threads.iq, &cpuset);
+  if ( threads.sync != -1 )
+    CPU_SET(threads.sync, &cpuset);
   // this thread priority must be lower that the main acquisition thread
   sprintf(threadname, "sync UE %d\n", UE->Mod_id);
   init_thread(100000, 500000, FIFO_PRIORITY-1, &cpuset, threadname);
@@ -1435,8 +1435,8 @@ void *UE_thread(void *arg) {
 
   cpu_set_t cpuset;
   CPU_ZERO(&cpuset);
-  if ( threads.iq != -1 )
-    CPU_SET(threads.iq, &cpuset);
+  if ( threads.main != -1 )
+    CPU_SET(threads.main, &cpuset);
   init_thread(100000, 500000, FIFO_PRIORITY, &cpuset,
 	      "UHD Threads");
 
diff --git a/targets/RT/USER/lte-uesoftmodem.c b/targets/RT/USER/lte-uesoftmodem.c
index 3d2995e5d1..617756e722 100644
--- a/targets/RT/USER/lte-uesoftmodem.c
+++ b/targets/RT/USER/lte-uesoftmodem.c
@@ -260,7 +260,7 @@ extern char uecap_xer[1024];
 char uecap_xer_in=0;
 
 int oaisim_flag=0;
-threads_t threads= {-1,-1,-1,-1,-1,-1,-1};
+threads_t threads = {-1,-1,-1,-1,-1,-1,-1};
 
 /* see file openair2/LAYER2/MAC/main.c for why abstraction_flag is needed
  * this is very hackish - find a proper solution
@@ -1014,14 +1014,17 @@ printf("~~~~~~~~~~~~~~~~~~~~successfully get the parallel config[%d], worker con
   char cpu_affinity[1024];
   CPU_ZERO(&cpuset);
 #ifdef CPU_AFFINITY
+  int j;
   if (get_nprocs() > 2) {
-    CPU_SET(0, &cpuset);
+    for (j = 2; j < get_nprocs(); j++)
+      CPU_SET(j, &cpuset);
+    
     s = pthread_setaffinity_np(pthread_self(), sizeof(cpu_set_t), &cpuset);
     if (s != 0) {
       perror( "pthread_setaffinity_np");
       exit_fun("Error setting processor affinity");
     }
-    LOG_I(HW, "Setting the affinity of main function to CPU 0, for device library to use CPU 0 only!\n");
+    LOG_I(HW, "Setting the affinity of main function to all CPUs, for device library to use CPU 0 only!\n");
   }
 #endif
   
diff --git a/targets/RT/USER/nr-ru.c b/targets/RT/USER/nr-ru.c
index 31698232b4..0e91074691 100644
--- a/targets/RT/USER/nr-ru.c
+++ b/targets/RT/USER/nr-ru.c
@@ -1499,7 +1499,7 @@ static void* ru_thread( void* param ) {
   int                ret;
   int                subframe =9;
   int                frame    =1023; 
-  char               filename[40];
+  char               filename[40],threadname[40];
   int                print_frame = 2;
   int                i = 0;
 
@@ -1508,7 +1508,8 @@ static void* ru_thread( void* param ) {
 
 
   // set default return value
-  thread_top_init("ru_thread",0,870000,1000000,1000000);
+  sprintf(threadname,"ru_thread %d",ru->idx);
+  thread_top_init(threadname,0,870000,1000000,1000000);
 
   LOG_I(PHY,"Starting RU %d (%s,%s),\n",ru->idx,NB_functions[ru->function],NB_timing[ru->if_timing]);
 
diff --git a/targets/RT/USER/nr-softmodem.c b/targets/RT/USER/nr-softmodem.c
index 67d226327c..f6e61884fe 100644
--- a/targets/RT/USER/nr-softmodem.c
+++ b/targets/RT/USER/nr-softmodem.c
@@ -247,8 +247,6 @@ double cpuf;
 extern char uecap_xer[1024];
 char uecap_xer_in=0;
 
-threads_t threads= {-1,-1,-1,-1,-1,-1,-1};
-
 /* see file openair2/LAYER2/MAC/main.c for why abstraction_flag is needed
  * this is very hackish - find a proper solution
  */
@@ -1036,14 +1034,19 @@ int main( int argc, char **argv )
   char cpu_affinity[1024];
   CPU_ZERO(&cpuset);
 #ifdef CPU_AFFINITY
+  int j;
   if (get_nprocs() > 2) {
-    CPU_SET(0, &cpuset);
+    // CPU_SET(1, &cpuset);
+    for (j = 2; j < get_nprocs(); j++)
+    {
+      CPU_SET(j, &cpuset);
+    }
     s = pthread_setaffinity_np(pthread_self(), sizeof(cpu_set_t), &cpuset);
     if (s != 0) {
       perror( "pthread_setaffinity_np");
       exit_fun("Error setting processor affinity");
     }
-    LOG_I(HW, "Setting the affinity of main function to CPU 0, for device library to use CPU 0 only!\n");
+    LOG_I(HW, "Setting the affinity of main function to all CPUs, for device library to use CPU 0 only!\n");
   }
 #endif
   
diff --git a/targets/RT/USER/nr-softmodem.h b/targets/RT/USER/nr-softmodem.h
index 9dce5f34bd..5ee2880f3d 100644
--- a/targets/RT/USER/nr-softmodem.h
+++ b/targets/RT/USER/nr-softmodem.h
@@ -152,12 +152,6 @@
 {"external-clock",        	 CONFIG_HLP_EXCCLK,	PARAMFLAG_BOOL,   uptr:&clock_source,			defintval:0,			   TYPE_INT,	  0},			   \
 {"wait-for-sync",         	 NULL,  		PARAMFLAG_BOOL,   iptr:&wait_for_sync,  		defintval:0,			   TYPE_INT,	  0},			   \
 {"single-thread-disable", 	 CONFIG_HLP_NOSNGLT,	PARAMFLAG_BOOL,   iptr:&single_thread_flag,		defintval:1,			   TYPE_INT,	  0},			   \
-{"threadIQ",              	 NULL,  		0,		  iptr:&(threads.iq),			defintval:1,			   TYPE_INT,	  0},			   \
-{"threadOneSubframe",     	 NULL,  		0,		  iptr:&(threads.one),  		defintval:1,			   TYPE_INT,	  0},			   \
-{"threadTwoSubframe",    	 NULL,  		0,		  iptr:&(threads.two),  		defintval:1,			   TYPE_INT,	  0},			   \
-{"threadThreeSubframe",    	 NULL,  		0,		  iptr:&(threads.three),  		defintval:1,			   TYPE_INT,	  0},			   \
-{"threadSlot1ProcOne",     	 NULL,  		0,		  iptr:&(threads.slot1_proc_one),      	defintval:1,			   TYPE_INT,	  0},			   \
-{"threadSlot1ProcTwo",    	 NULL,  		0,		  iptr:&(threads.slot1_proc_two),      	defintval:1,			   TYPE_INT,	  0},			   \
 {"A" ,  		  	 CONFIG_HLP_TADV,	0,		  uptr:&timing_advance, 		defintval:0,			   TYPE_UINT,	  0},			   \
 {"C" ,  		  	 CONFIG_HLP_DLF,	0,		  uptr:&(downlink_frequency[0][0]),	defuintval:DEFAULT_DLF,  	   TYPE_UINT,	  0},			   \
 {"a" ,  		  	 CONFIG_HLP_CHOFF,	0,		  iptr:&chain_offset,			defintval:0,			   TYPE_INT,	  0},			   \
diff --git a/targets/RT/USER/nr-ue.c b/targets/RT/USER/nr-ue.c
index 22ada266bf..ed55e15d3c 100644
--- a/targets/RT/USER/nr-ue.c
+++ b/targets/RT/USER/nr-ue.c
@@ -341,8 +341,8 @@ static void *UE_thread_synch(void *arg) {
 
     cpu_set_t cpuset;
     CPU_ZERO(&cpuset);
-    if ( threads.iq != -1 )
-        CPU_SET(threads.iq, &cpuset);
+    if ( threads.sync != -1 )
+        CPU_SET(threads.sync, &cpuset);
     // this thread priority must be lower that the main acquisition thread
     sprintf(threadname, "sync UE %d", UE->Mod_id);
     init_thread(100000, 500000, FIFO_PRIORITY-1, &cpuset, threadname);
@@ -833,15 +833,15 @@ void *UE_thread(void *arg) {
 
     cpu_set_t cpuset;
     CPU_ZERO(&cpuset);
-    if ( threads.iq != -1 )
-        CPU_SET(threads.iq, &cpuset);
-    init_thread(100000, 500000, FIFO_PRIORITY, &cpuset,
-                "UHD Threads");
+    if ( threads.main != -1 )
+        CPU_SET(threads.main, &cpuset);
+    sprintf(threadname, "Main UE %d", UE->Mod_id);
+    init_thread(100000, 500000, FIFO_PRIORITY, &cpuset,threadname);
+    
     if ((oaisim_flag == 0) && (UE->mode !=loop_through_memory))
         AssertFatal(0== openair0_device_load(&(UE->rfdevice), &openair0_cfg[0]), "");
     UE->rfdevice.host_type = RAU_HOST;
-    sprintf(threadname, "Main UE %d", UE->Mod_id);
-    pthread_setname_np(pthread_self(), threadname);
+
     init_UE_threads(UE);
 
 #ifdef NAS_UE
diff --git a/targets/RT/USER/nr-uesoftmodem.c b/targets/RT/USER/nr-uesoftmodem.c
index 68f0254762..beebdfd561 100644
--- a/targets/RT/USER/nr-uesoftmodem.c
+++ b/targets/RT/USER/nr-uesoftmodem.c
@@ -268,7 +268,8 @@ char uecap_xer[1024],uecap_xer_in=0;
 int oaisim_flag=0;
 int emulate_rf = 0;
 
-threads_t threads= {-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1};
+threads_t threads = {-1,-1,-1,-1,-1,-1,-1,-1};
+int threads_offset = 0;
 
 char* usrp_args=NULL;
 char* usrp_clksrc=NULL;
@@ -1029,8 +1030,25 @@ int main( int argc, char **argv ) {
   //dump_frame_parms(frame_parms[0]);
   
   init_openair0();
-  
 
+  // default threads.main = 2
+  // if there are enough processors, all others on subsequent CPUs
+  if (threads_offset>0) {
+    if (threads_offset+8<=get_nprocs()) {
+      if (threads.main<0) threads.main=threads_offset+1;
+      if (threads.sync<0) threads.sync=threads_offset+2;
+      if (threads.one<0) threads.one=threads_offset+3;
+      if (threads.two<0) threads.two=threads_offset+4;
+      if (threads.three<0) threads.three=threads_offset+5;
+      if (threads.slot1_proc_one<0) threads.slot1_proc_one=threads_offset+6;
+      if (threads.slot1_proc_two<0) threads.slot1_proc_two=threads_offset+7;
+      if (threads.slot1_proc_three<0) threads.slot1_proc_three=threads_offset+8;
+    }
+    else {
+      LOG_E(HW,"Not enough CPUs available (nprocs (=%d) >= threadmain (=%d) + 10)\n",get_nprocs(),threads_offset);
+      exit_fun("Error setting thread affinity\n");
+    }
+  }
 
 #ifndef DEADLINE_SCHEDULER
 
@@ -1041,14 +1059,17 @@ int main( int argc, char **argv ) {
     char cpu_affinity[1024];
     CPU_ZERO(&cpuset);
 #ifdef CPU_AFFINITY
-    if (get_nprocs() > 2) {
-        CPU_SET(0, &cpuset);
+    int j;
+    if (get_nprocs()>=2) {
+      for (j = 2; j < get_nprocs(); j++)
+	CPU_SET(j, &cpuset);
+
         s = pthread_setaffinity_np(pthread_self(), sizeof(cpu_set_t), &cpuset);
         if (s != 0) {
             perror( "pthread_setaffinity_np");
             exit_fun("Error setting processor affinity");
         }
-        LOG_I(HW, "Setting the affinity of main function to CPU 0, for device library to use CPU 0 only!\n");
+        LOG_I(HW, "Setting the affinity of main function to all CPUs, for device library to use CPU 0 only!\n");
     }
 #endif
 
diff --git a/targets/RT/USER/nr-uesoftmodem.h b/targets/RT/USER/nr-uesoftmodem.h
index 99fb64f175..816e753086 100644
--- a/targets/RT/USER/nr-uesoftmodem.h
+++ b/targets/RT/USER/nr-uesoftmodem.h
@@ -157,13 +157,16 @@
 {"external-clock",          CONFIG_HLP_EXCCLK,      PARAMFLAG_BOOL,         uptr:&clock_source,                 defintval:0,                    TYPE_INT,       0},                     \
 {"wait-for-sync",           NULL,                   PARAMFLAG_BOOL,         iptr:&wait_for_sync,                defintval:0,                    TYPE_INT,       0},                     \
 {"single-thread-disable",   CONFIG_HLP_NOSNGLT,     PARAMFLAG_BOOL,         iptr:&single_thread_flag,           defintval:1,                    TYPE_INT,       0},                     \
-{"threadIQ",                NULL,                   0,                      iptr:&(threads.iq),                 defintval:1,                    TYPE_INT,       0},                     \
-{"threadOneSubframe",       NULL,                   0,                      iptr:&(threads.one),                defintval:1,                    TYPE_INT,       0},                     \
-{"threadTwoSubframe",       NULL,                   0,                      iptr:&(threads.two),                defintval:1,                    TYPE_INT,       0},                     \
-{"threadThreeSubframe",     NULL,                   0,                      iptr:&(threads.three),              defintval:1,                    TYPE_INT,       0},                     \
-{"threadSlot1ProcOne",      NULL,                   0,                      iptr:&(threads.slot1_proc_one),     defintval:1,                    TYPE_INT,       0},                     \
-{"threadSlot1ProcTwo",      NULL,                   0,                      iptr:&(threads.slot1_proc_two),     defintval:1,                    TYPE_INT,       0},                     \
-{"nr_dlsch-demod-shift",    CONFIG_HLP_DLSHIFT,     0,                      iptr:(int32_t *)&nr_dlsch_demod_shift, defintval:0,                    TYPE_INT,       0},                     \
+{"threadoffset",            NULL,                   0,                      iptr:&(threads_offset),             defintval:0,                    TYPE_INT,       0},                     \
+{"threadMain",              NULL,                   0,                      iptr:&(threads.main),               defintval:-1,                    TYPE_INT,       0},                     \
+{"threadSync",              NULL,                   0,                      iptr:&(threads.sync),               defintval:-1,                    TYPE_INT,       0},                     \
+{"threadOneSubframe",       NULL,                   0,                      iptr:&(threads.one),                defintval:-1,                    TYPE_INT,       0},                     \
+{"threadTwoSubframe",       NULL,                   0,                      iptr:&(threads.two),                defintval:-1,                    TYPE_INT,       0},                     \
+{"threadThreeSubframe",     NULL,                   0,                      iptr:&(threads.three),              defintval:-1,                    TYPE_INT,       0},                     \
+{"threadSlot1ProcOne",      NULL,                   0,                      iptr:&(threads.slot1_proc_one),     defintval:-1,                    TYPE_INT,       0},                     \
+{"threadSlot1ProcTwo",      NULL,                   0,                      iptr:&(threads.slot1_proc_two),     defintval:-1,                    TYPE_INT,       0},                     \
+{"threadSlot1ProcThree",    NULL,                   0,                      iptr:&(threads.slot1_proc_three),   defintval:-1,                    TYPE_INT,       0},                     \
+{"nr-dlsch-demod-shift",    CONFIG_HLP_DLSHIFT,     0,                      iptr:(int32_t *)&nr_dlsch_demod_shift, defintval:0,                     TYPE_INT,       0},                     \
 {"A" ,                      CONFIG_HLP_TADV,        0,                      uptr:&timing_advance,               defintval:0,                    TYPE_UINT,      0},                     \
 {"C" ,                      CONFIG_HLP_DLF,         0,                      uptr:&(downlink_frequency[0][0]),   defuintval:2680000000,          TYPE_UINT,      0},                     \
 {"a" ,                      CONFIG_HLP_CHOFF,       0,                      iptr:&chain_offset,                 defintval:0,                    TYPE_INT,       0},                     \
diff --git a/targets/RT/USER/rt_wrapper.c b/targets/RT/USER/rt_wrapper.c
index 443d183068..e4dc54f78f 100644
--- a/targets/RT/USER/rt_wrapper.c
+++ b/targets/RT/USER/rt_wrapper.c
@@ -294,20 +294,20 @@ void thread_top_init(char *thread_name,
   char cpu_affinity[1024];
   cpu_set_t cpuset;
 
-  /* Set affinity mask to include CPUs 1 to MAX_CPUS */
+  /* Set affinity mask to include CPUs 2 to MAX_CPUS */
   /* CPU 0 is reserved for UHD threads */
   /* CPU 1 is reserved for all RX_TX threads */
-  /* Enable CPU Affinity only if number of CPUs >2 */
+  /* Enable CPU Affinity only if number of CPUs > 2 */
   CPU_ZERO(&cpuset);
 
 #ifdef CPU_AFFINITY
-  if (get_nprocs() > 2)
+  if (affinity == 0) {
+    LOG_W(HW,"thread_top_init() called with affinity==0, but overruled by #ifdef CPU_AFFINITY\n");
+  }
+  else if (get_nprocs() > 2)
   {
-    if (affinity == 0)
-      CPU_SET(0,&cpuset);
-    else
-      for (j = 1; j < get_nprocs(); j++)
-        CPU_SET(j, &cpuset);
+    for (j = 2; j < get_nprocs(); j++)
+      CPU_SET(j, &cpuset);
     s = pthread_setaffinity_np(pthread_self(), sizeof(cpu_set_t), &cpuset);
     if (s != 0)
     {
@@ -315,21 +315,29 @@ void thread_top_init(char *thread_name,
       exit_fun("Error setting processor affinity");
     }
   }
+#else //CPU_AFFINITY
+  if (affinity) {
+    LOG_W(HW,"thread_top_init() called with affinity>0, but overruled by #ifndef CPU_AFFINITY.\n");
+  }
 #endif //CPU_AFFINITY
 
   /* Check the actual affinity mask assigned to the thread */
   s = pthread_getaffinity_np(pthread_self(), sizeof(cpu_set_t), &cpuset);
-  if (s != 0) {
+  if (s != 0)
+  {
     perror( "pthread_getaffinity_np");
     exit_fun("Error getting processor affinity ");
   }
   memset(cpu_affinity,0,sizeof(cpu_affinity));
   for (j = 0; j < 1024; j++)
-    if (CPU_ISSET(j, &cpuset)) {  
+  {
+    if (CPU_ISSET(j, &cpuset))
+    {  
       char temp[1024];
       sprintf (temp, " CPU_%d", j);
       strcat(cpu_affinity, temp);
     }
+  }
 
   memset(&sparam, 0, sizeof(sparam));
   sparam.sched_priority = sched_get_priority_max(SCHED_FIFO);
-- 
2.26.2