From 46ef64b4c099277d10680eb8ed903c6d38bf0029 Mon Sep 17 00:00:00 2001
From: "ezekiel.dohmen" <ezekiel.dohmen@ligo.org>
Date: Wed, 10 Apr 2024 09:34:38 -0700
Subject: [PATCH] Adding init to RT model
---
src/epics/util/lib/Statespace.pm | 13 +++++++
src/fe/moduleLoad.c | 18 +++++++++
src/fe/statespace/stateSpaceCtrl_cdev.c | 6 ++-
src/fe/statespace/stateSpacePart.c | 39 ++++++++++++-------
.../part_headers/statespace/stateSpacePart.h | 4 +-
5 files changed, 64 insertions(+), 16 deletions(-)
diff --git a/src/epics/util/lib/Statespace.pm b/src/epics/util/lib/Statespace.pm
index 6cb27383d..c29febc76 100644
--- a/src/epics/util/lib/Statespace.pm
+++ b/src/epics/util/lib/Statespace.pm
@@ -50,6 +50,19 @@ sub printEpics {
}
+# return an array of EpicsVariable objects
+# this function should be idempotent: calling it over and over should give the same
+# result and have no side effects.
+sub getEpicsVariables {
+ my ($i) = @_;
+ @epics_vars = ();
+
+ # Add EPICS input to reset the state of the statespace part
+ push @epics_vars, CDS::EpicsVariable::new("$::xpartName[$i]\_RESET", "$::xpartName[$i]\_RESET", "int", "ai", 1, 0, {"PREC" => 0});
+
+ return @epics_vars;
+}
+
# Print variable declarations int front-end file
# Current part number is passed as first argument
sub printFrontEndVars {
diff --git a/src/fe/moduleLoad.c b/src/fe/moduleLoad.c
index 3fd048a27..bb85a45d9 100644
--- a/src/fe/moduleLoad.c
+++ b/src/fe/moduleLoad.c
@@ -16,6 +16,9 @@
#include "../fe/verify_slots.h"
#include "../fe/mapApp.h" //initmap()
#include "../fe/dolphin.h"
+#include "part_headers/statespace/stateSpacePart.h"
+#include "part_headers/statespace/stateSpaceCtrl_cdev.h"
+
#include <linux/uaccess.h>
@@ -94,6 +97,17 @@ static int __init rt_fe_init( void )
demodulation_init();
+ //Statespace Setup
+ if ( globStateSpaceInit( STATESPACE_NUM_PARTS ) != 0 ) { //STATESPACE_NUM_PARTS defined in FE_HEADER
+ RTSLOG_ERROR( "rt_fe_init() - Failed to initialize statespace parts, num: %d\n", STATESPACE_NUM_PARTS );
+ return -5;
+ }
+
+ if( initStateSpaceCtrl_cdev(SYSTEM_NAME_STRING_LOWER) != 0 ) {
+ RTSLOG_ERROR( "rt_fe_init() - Failed to initialize the statespace cdev: /dev/%s\n", SYSTEM_NAME_STRING_LOWER );
+ return -5;
+ }
+
#ifdef DOLPHIN_TEST
{
@@ -320,6 +334,10 @@ static void rt_fe_cleanup( void )
finish_dolphin( );
#endif
+ //Clean up Statespace global state
+ cleanupStateSpaceCtrl_cdev();
+ globStateSpaceFree();
+
// Print out any error messages from FE code on exit
print_exit_messages( fe_status_return, fe_status_return_subcode, SYSTEM_NAME_STRING_LOWER );
detach_shared_memory( );
diff --git a/src/fe/statespace/stateSpaceCtrl_cdev.c b/src/fe/statespace/stateSpaceCtrl_cdev.c
index 3db78d057..1f1ade016 100644
--- a/src/fe/statespace/stateSpaceCtrl_cdev.c
+++ b/src/fe/statespace/stateSpaceCtrl_cdev.c
@@ -258,7 +258,11 @@ static long stateSpaceCtrl_ioctl(struct file *file, unsigned int cmd, unsign
if (whole_cfg_ptr == NULL) return -ENOMEM;
((struct stateSpace_part_config_t *)whole_cfg_ptr)->ioctl_version = STATE_SPACE_IOCTL_V0;
- getConfigAndDataOfPartStateSpace(req_part_index, data_block_sz, (void*)whole_cfg_ptr);
+ if ( getConfigAndDataOfPartStateSpace(req_part_index, data_block_sz, (void*)whole_cfg_ptr) != 0 ) {
+ RTSLOG_ERROR("stateSpaceCtrl_cdev - getConfigAndDataOfPartStateSpace(%d) failed, has the part been configured?\n", req_part_index);
+ vfree(whole_cfg_ptr);
+ return -EINVAL;
+ }
if( copy_to_user(config_req_usr_ptr, whole_cfg_ptr, sizeof(struct stateSpace_part_config_t) + data_block_sz ) != 0)
{
RTSLOG_ERROR("stateSpaceCtrl_cdev - copy_to_user() failed\n");
diff --git a/src/fe/statespace/stateSpacePart.c b/src/fe/statespace/stateSpacePart.c
index 79c6f75e0..673be4bff 100644
--- a/src/fe/statespace/stateSpacePart.c
+++ b/src/fe/statespace/stateSpacePart.c
@@ -18,22 +18,24 @@ int globStateSpaceInit( unsigned num_parts )
{
g_model_config.num_parts = num_parts;
+ if ( num_parts > 0 ) { //Don't try a 0 byte allocation
- g_model_config.ss_parts_ptr = vmalloc(sizeof(*g_model_config.ss_parts_ptr) * num_parts);
- if ( g_model_config.ss_parts_ptr == NULL ) {
- RTSLOG_ERROR("stateSpace - Failed to allocate memory for state space parts.\n");
- return -1;
- }
-
- for(int i=0; i < num_parts; ++i)
- {
- g_model_config.ss_parts_ptr[i].context = (StateSpaceCtx_t*) vmalloc(sizeof(StateSpaceCtx_t));
- if( g_model_config.ss_parts_ptr[i].context == NULL) {
- RTSLOG_ERROR("Failed to vmalloc() conbtext for the StateSpace part.");
+ g_model_config.ss_parts_ptr = vmalloc(sizeof(*g_model_config.ss_parts_ptr) * num_parts);
+ if ( g_model_config.ss_parts_ptr == NULL ) {
+ RTSLOG_ERROR("stateSpace - Failed to allocate memory for state space parts.\n");
return -1;
}
- //Zero out the ctx
- memset((void*)g_model_config.ss_parts_ptr[i].context, 0, sizeof(StateSpaceCtx_t));
+
+ for(int i=0; i < num_parts; ++i)
+ {
+ g_model_config.ss_parts_ptr[i].context = (StateSpaceCtx_t*) vmalloc(sizeof(StateSpaceCtx_t));
+ if( g_model_config.ss_parts_ptr[i].context == NULL) {
+ RTSLOG_ERROR("Failed to vmalloc() context for the StateSpace part.");
+ return -1;
+ }
+ //Zero out the ctx
+ memset((void*)g_model_config.ss_parts_ptr[i].context, 0, sizeof(StateSpaceCtx_t));
+ }
}
return 0;
@@ -268,8 +270,15 @@ void getConfigOfPartStateSpace(unsigned part_index, struct stateSpace_part_confi
fill_me->state_vec_len = g_model_config.ss_parts_ptr[part_index].context->state_len;
}
-void getConfigAndDataOfPartStateSpace(unsigned part_index, unsigned data_sz, struct stateSpace_part_config_t * fill_me)
+int getConfigAndDataOfPartStateSpace(unsigned part_index, unsigned data_sz, struct stateSpace_part_config_t * fill_me)
{
+ if ( g_model_config.ss_parts_ptr[part_index].context == NULL ||
+ g_model_config.ss_parts_ptr[part_index].context->mem_block_ptr == NULL) {
+ //mem_block_ptr will be NULL after the model starts until matrixInitStateSpace()
+ //through the IOCTL interface is called.
+ return -1;
+
+ }
fill_me->part_index = part_index;
fill_me->input_vec_len = g_model_config.ss_parts_ptr[part_index].model_inputs;
fill_me->output_vec_len = g_model_config.ss_parts_ptr[part_index].model_outputs;
@@ -278,6 +287,8 @@ void getConfigAndDataOfPartStateSpace(unsigned part_index, unsigned data_sz, str
memcpy( &fill_me->data_block[fill_me->state_vec_len],
g_model_config.ss_parts_ptr[part_index].context->state_matrix,
data_sz - (fill_me->state_vec_len * 8));
+
+ return 0;
}
diff --git a/src/include/part_headers/statespace/stateSpacePart.h b/src/include/part_headers/statespace/stateSpacePart.h
index e4d2a413b..84140f1d0 100644
--- a/src/include/part_headers/statespace/stateSpacePart.h
+++ b/src/include/part_headers/statespace/stateSpacePart.h
@@ -45,7 +45,9 @@ void setStateVecStateSpace(unsigned part_index, double * state_vec);
unsigned getNumPartsStateSpace( void );
void getPartNamePtrStateSpace( unsigned part_index, char ** name_ptr );
void getConfigOfPartStateSpace(unsigned part_index, struct stateSpace_part_config_t * );
-void getConfigAndDataOfPartStateSpace(unsigned part_index, unsigned data_sz, struct stateSpace_part_config_t * fill_me);
+
+// @return -1 if data for part is not available
+int getConfigAndDataOfPartStateSpace(unsigned part_index, unsigned data_sz, struct stateSpace_part_config_t * fill_me);
//Test Methods
int testBufferOverrun(unsigned part_index);
--
GitLab