Update for SMP

nasa · Jul 23, 2023 · 3675ce0 · 3675ce0
1 parent be5ee80
commit 3675ce0
Showing 1 changed file with 30 additions and 26 deletions.
diff --git a/fsw/modules/rtems_sysmon/rtems_sysmon.c b/fsw/modules/rtems_sysmon/rtems_sysmon.c
@@ -67,9 +67,10 @@ typedef struct rtems_sysmon_cpuload_state
     volatile bool is_running;
     volatile bool should_run;
 
-    rtems_id task_id;
+    rtems_id   task_id;
     rtems_name task_name;
 
+    uint8_t    num_cpus;
     rtems_sysmon_cpuload_core_t per_core[RTEMS_SYSMON_MAX_CPUS];
 
 } rtems_sysmon_cpuload_state_t;
@@ -122,10 +123,11 @@ void rtems_sysmon_Init(uint32_t local_module_id)
 
 static bool rtems_cpu_usage_vistor(Thread_Control *the_thread, void *arg)
 {
-    rtems_sysmon_cpuload_core_t *core_p = (rtems_sysmon_cpuload_core_t *) arg;
+    rtems_sysmon_cpuload_state_t *state = (rtems_sysmon_cpuload_state_t *)arg;
+    rtems_sysmon_cpuload_core_t* core_p = &state->per_core[state->num_cpus];
 
-    Timestamp_Control uptime_at_last_calc = core_p[0].last_run_time;
-    Timestamp_Control idle_uptime_at_last_calc = core_p[0].idle_last_uptime;
+    Timestamp_Control uptime_at_last_calc = core_p->last_run_time;
+    Timestamp_Control idle_uptime_at_last_calc = core_p->idle_last_uptime;
     Timestamp_Control current_uptime;
     Timestamp_Control idle_task_uptime;
     Timestamp_Control idle_uptime_elapsed;
@@ -151,33 +153,33 @@ static bool rtems_cpu_usage_vistor(Thread_Control *the_thread, void *arg)
         _Timestamp_Divide(&idle_uptime_elapsed, &total_elapsed, &ival, &fval);   /* ival - points to the integer portion */
                                                                                  /* fval - points to the thousandths of percentage */
 
-        core_p[0].last_run_time = current_uptime;
-        core_p[0].idle_last_uptime = idle_task_uptime;
+        core_p->last_run_time = current_uptime;
+        core_p->idle_last_uptime = idle_task_uptime;
 
         if(ival >= 100)
         {
-            core_p[0].avg_load = 0xFFFFFF; /* max */
+            core_p->avg_load = 0xFFFFFF; /* max */
         }
         else if(total_elapsed == 0)
         {
-            core_p[0].avg_load = 0;
+            core_p->avg_load = 0;
         }
         else
         {
             while (fval > 999) { fval /= 10; } /* Keep 3 most significant digits. Should not occur. */
-            core_p[0].avg_load = (RTEMS_SYSMON_MAX_SCALE - ((ival * 1000) + fval)); /* Get percentages as integer */
+            core_p->avg_load = (RTEMS_SYSMON_MAX_SCALE - ((ival * 1000) + fval)); /* Get percentages as integer */
 
             /* 
             ** Mimic ADC so that "analogio" API can be used with out modification. API assumes 24 bits.
             ** First calculate out of 0x1000 and then duplicate it to expand to 24 bits. Doing this prevents 
             ** an overflow. avg_load has a "real" resolution of 12 bits.
             */
-            core_p[0].avg_load = (0x1000 * core_p[0].avg_load) / RTEMS_SYSMON_MAX_SCALE; 
-            core_p[0].avg_load |= (core_p[0].avg_load << 12);
+            core_p->avg_load = (0x1000 * core_p->avg_load) / RTEMS_SYSMON_MAX_SCALE; 
+            core_p->avg_load |= (core_p->avg_load << 12);
         }
 
         #ifdef DEBUG_BUILD
-        //rtems_cpu_usage_report();
+        rtems_cpu_usage_report();
 
         uint32_t microsec = _Timestamp_Get_nanoseconds( &idle_uptime_elapsed ) / TOD_NANOSECONDS_PER_MICROSECOND;
         uint32_t sec = _Timestamp_Get_seconds( &idle_uptime_elapsed );
@@ -187,10 +189,15 @@ static bool rtems_cpu_usage_vistor(Thread_Control *the_thread, void *arg)
         microsec = _Timestamp_Get_nanoseconds( &total_elapsed ) / TOD_NANOSECONDS_PER_MICROSECOND;
         sec = _Timestamp_Get_seconds( &total_elapsed );
         RTEMS_SYSMON_DEBUG("CFE_PSP(rtems_sysmon): Total elapsed CPU time = %7u.%06u, CPU Load =%08X\n",
-                           sec, microsec, core_p[0].avg_load);
-
+                           sec, microsec, core_p->avg_load);
         #endif
-        status = true;
+
+        state->num_cpus++;
+        if(state->num_cpus >= RTEMS_SYSMON_MAX_CPUS)
+        {
+            /* stop checking when all idle tasks has been found */
+            status = true;
+        }
     }
 
     /* return true to exit iterating tasks */
@@ -199,7 +206,8 @@ static bool rtems_cpu_usage_vistor(Thread_Control *the_thread, void *arg)
 
 void rtems_sysmon_update_stat(rtems_sysmon_cpuload_state_t *state)
 {
-    rtems_task_iterate( rtems_cpu_usage_vistor, &state->per_core );
+    state->num_cpus = 0;
+    rtems_task_iterate( rtems_cpu_usage_vistor, state);
 }
 
 rtems_task rtems_sysmon_Task(rtems_task_argument arg)
@@ -209,12 +217,16 @@ rtems_task rtems_sysmon_Task(rtems_task_argument arg)
     OS_time_t     curr_sample;
     OS_time_t     next_sample;
     int           msec_diff;
+    int           i;
 
     /* Initialize */
     rtems_cpu_usage_reset();
 
     memset(state->per_core, 0, sizeof(rtems_sysmon_cpuload_core_t));
-    state->per_core[0].last_run_time = CPU_usage_Uptime_at_last_reset;
+    for (i = 0; i < RTEMS_SYSMON_MAX_CPUS; i++)
+    {
+        state->per_core[i].last_run_time = CPU_usage_Uptime_at_last_reset;
+    }
 
     while (state->should_run)
     {
@@ -333,15 +345,7 @@ int32_t rtems_sysmon_aggregate_dispatch(uint32_t CommandCode, uint16_t Subchanne
         {
             if (Arg.U32)
             {
-                if(RTEMS_SYSMON_MAX_CPUS > 1)
-                {
-                    OS_printf("CFE_PSP(RTEMS_SysMon): SMP Not Supported");
-                    StatusCode = CFE_PSP_ERROR;
-                }
-                else
-                {
-                    StatusCode = rtems_sysmon_Start(state);
-                }
+                StatusCode = rtems_sysmon_Start(state);
             }
             else
             {