diff --git a/src/sm_io/modules/fmc_active_clk/sm_io_fmc_active_clk_codes.h b/src/sm_io/modules/fmc_active_clk/sm_io_fmc_active_clk_codes.h
index bbb428aa4993f947a7f91a013b842b97d88d32eb..f28fdaf3bf805b3fe61d8cebccfb9fe8d7c11a10 100644
--- a/src/sm_io/modules/fmc_active_clk/sm_io_fmc_active_clk_codes.h
+++ b/src/sm_io/modules/fmc_active_clk/sm_io_fmc_active_clk_codes.h
@@ -40,8 +40,8 @@
#define FMC_ACTIVE_CLK_NAME_AD9510_OUTPUTS "fmc_active_clk_ad9510_outputs"
#define FMC_ACTIVE_CLK_OPCODE_AD9510_PLL_CLK_SEL 13
#define FMC_ACTIVE_CLK_NAME_AD9510_PLL_CLK_SEL "fmc_active_clk_ad9510_pll_clk_sel"
-#define FMC_ACTIVE_CLK_OPCODE_SI571_SET_FREQ 14
-#define FMC_ACTIVE_CLK_NAME_SI571_SET_FREQ "fmc_active_clk_si571_set_freq"
+#define FMC_ACTIVE_CLK_OPCODE_SI571_FREQ 14
+#define FMC_ACTIVE_CLK_NAME_SI571_FREQ "fmc_active_clk_si571_freq"
#define FMC_ACTIVE_CLK_OPCODE_SI571_GET_DEFAULTS 15
#define FMC_ACTIVE_CLK_NAME_SI571_GET_DEFAULTS "fmc_active_clk_si571_get_defaults"
#define FMC_ACTIVE_CLK_OPCODE_END 16
diff --git a/src/sm_io/modules/fmc_active_clk/sm_io_fmc_active_clk_exp.c b/src/sm_io/modules/fmc_active_clk/sm_io_fmc_active_clk_exp.c
index b22b11bc4bbbd2f853d444fa273294d444e8f720..906c34912bf0796c7f3b58ef3504a5cb1ead8ab7 100644
--- a/src/sm_io/modules/fmc_active_clk/sm_io_fmc_active_clk_exp.c
+++ b/src/sm_io/modules/fmc_active_clk/sm_io_fmc_active_clk_exp.c
@@ -230,7 +230,7 @@ FMC_ACTIVE_CLK_AD9510_FUNC_NAME_HEADER(pll_clk_sel)
}
/* Macros to avoid repetition of the function body Si57X */
-typedef smch_err_e (*smch_si57x_func_fp) (smch_si57x_t *self, double param);
+typedef smch_err_e (*smch_si57x_func_fp) (smch_si57x_t *self, double *param);
#define FMC_ACTIVE_CLK_SI571_FUNC_NAME(func_name) \
_fmc_active_clk_si571_ ## func_name
@@ -238,7 +238,8 @@ typedef smch_err_e (*smch_si57x_func_fp) (smch_si57x_t *self, double param);
#define FMC_ACTIVE_CLK_SI571_FUNC_NAME_HEADER(func_name) \
static int FMC_ACTIVE_CLK_SI571_FUNC_NAME(func_name) (void *owner, void *args, void *ret)
-#define FMC_ACTIVE_CLK_SI571_FUNC_BODY(owner, args, ret, func, error_msg) \
+#define FMC_ACTIVE_CLK_SI571_FUNC_BODY(owner, args, ret, read_func, write_func, \
+ error_msg) \
do { \
(void) ret; \
assert (owner); \
@@ -251,30 +252,72 @@ typedef smch_err_e (*smch_si57x_func_fp) (smch_si57x_t *self, double param);
err_get_fmcaclk_handler, -FMC_ACTIVE_CLK_ERR); \
smch_si57x_t *smch_si57x = SMIO_SI57X_HANDLER(fmcaclk); \
uint32_t rw = *(uint32_t *) EXP_MSG_ZMQ_FIRST_ARG(args); \
- (void) rw; /* Ignored for now */ \
double param = *(double *) EXP_MSG_ZMQ_NEXT_ARG(args); \
\
+ smch_err_e serr = SMCH_SUCCESS; \
/* Call specific function */ \
- smch_err_e serr = ((smch_si57x_func_fp) func) (smch_si57x, param); \
- ASSERT_TEST(serr == SMCH_SUCCESS, error_msg, \
- err_smpr_write, -FMC_ACTIVE_CLK_ERR); \
+ if (rw) { \
+ WHEN(ISEMPTY(read_func))( \
+ (void) ret; \
+ DBE_DEBUG (DBG_SM_IO | DBG_LVL_TRACE, "[sm_io:fmc_active_clk_exp] " \
+ "SI57x read function not implemented\n"); \
+ err = -FMC_ACTIVE_CLK_UNINPL; \
+ return err; \
+ ) \
+ WHENNOT(ISEMPTY(read_func))( \
+ double value = 0; \
+ serr = ((smch_si57x_func_fp) read_func) (smch_si57x, \
+ &value); \
+ if (serr != SMCH_SUCCESS) { \
+ err = -FMC_ACTIVE_CLK_ERR; \
+ } \
+ else { \
+ *((double *) ret) = value; \
+ err = sizeof (value); \
+ DBE_DEBUG (DBG_SM_IO | DBG_LVL_TRACE, "[sm_io:fmc_active_clk_exp] " \
+ "SI57x function read value = 0x%f\n", value); \
+ } \
+ ) \
+ } \
+ else { \
+ WHEN(ISEMPTY(write_func))( \
+ DBE_DEBUG (DBG_SM_IO | DBG_LVL_TRACE, "[sm_io:fmc_active_clk_exp] " \
+ "SI57x write function not implemented\n"); \
+ err = -FMC_ACTIVE_CLK_UNINPL; \
+ return err; \
+ ) \
+ WHENNOT(ISEMPTY(write_func))( \
+ serr = ((smch_si57x_func_fp) write_func) (smch_si57x, \
+ ¶m); \
+ if (serr != SMCH_SUCCESS) { \
+ err = -FMC_ACTIVE_CLK_ERR; \
+ } \
+ else { \
+ err = -FMC_ACTIVE_CLK_OK; \
+ } \
+ ) \
+ } \
\
-err_smpr_write: \
err_get_fmcaclk_handler: \
return err; \
\
} while(0)
-FMC_ACTIVE_CLK_SI571_FUNC_NAME_HEADER(set_freq)
+FMC_ACTIVE_CLK_SI571_FUNC_NAME_HEADER(freq)
+{
+ FMC_ACTIVE_CLK_SI571_FUNC_BODY(owner, args, ret, smch_si57x_get_freq,
+ smch_si57x_set_freq, "Could not set SI571 frequency");
+}
+
+static smch_err_e smch_si57x_get_defaults_compat (smch_si57x_t *self, double *param)
{
- FMC_ACTIVE_CLK_SI571_FUNC_BODY(owner, args, ret, smch_si57x_set_freq,
- "Could not set SI571 frequency");
+ return smch_si57x_get_defaults (self, *param);
}
FMC_ACTIVE_CLK_SI571_FUNC_NAME_HEADER(get_defaults)
{
- FMC_ACTIVE_CLK_SI571_FUNC_BODY(owner, args, ret, smch_si57x_get_defaults,
- "Could not restart SI571 to its defaults");
+ FMC_ACTIVE_CLK_SI571_FUNC_BODY(owner, args, ret, /* No read func*/,
+ smch_si57x_get_defaults_compat, "Could not restart SI571 to its defaults");
}
/* Exported function pointers */
@@ -293,7 +336,7 @@ const disp_table_func_fp fmc_active_clk_exp_fp [] = {
FMC_ACTIVE_CLK_AD9510_FUNC_NAME(cp_current),
FMC_ACTIVE_CLK_AD9510_FUNC_NAME(outputs),
FMC_ACTIVE_CLK_AD9510_FUNC_NAME(pll_clk_sel),
- FMC_ACTIVE_CLK_SI571_FUNC_NAME(set_freq),
+ FMC_ACTIVE_CLK_SI571_FUNC_NAME(freq),
FMC_ACTIVE_CLK_SI571_FUNC_NAME(get_defaults),
NULL
};
diff --git a/src/sm_io/modules/fmc_active_clk/sm_io_fmc_active_clk_exports.c b/src/sm_io/modules/fmc_active_clk/sm_io_fmc_active_clk_exports.c
index 0b413346671b0a43417e2d2111697e67778d5ab2..41a8b595b6a86601526fa84c8c04ecba3063b61d 100644
--- a/src/sm_io/modules/fmc_active_clk/sm_io_fmc_active_clk_exports.c
+++ b/src/sm_io/modules/fmc_active_clk/sm_io_fmc_active_clk_exports.c
@@ -178,9 +178,9 @@ disp_op_t fmc_active_clk_ad9510_pll_clk_sel_exp = {
}
};
-disp_op_t fmc_active_clk_si571_set_freq_exp = {
- .name = FMC_ACTIVE_CLK_NAME_SI571_SET_FREQ,
- .opcode = FMC_ACTIVE_CLK_OPCODE_SI571_SET_FREQ,
+disp_op_t fmc_active_clk_si571_freq_exp = {
+ .name = FMC_ACTIVE_CLK_NAME_SI571_FREQ,
+ .opcode = FMC_ACTIVE_CLK_OPCODE_SI571_FREQ,
.retval = DISP_ARG_END,
.retval_owner = DISP_OWNER_OTHER,
.args = {
@@ -218,7 +218,7 @@ const disp_op_t *fmc_active_clk_exp_ops [] = {
&fmc_active_clk_ad9510_cp_current_exp,
&fmc_active_clk_ad9510_outputs_exp,
&fmc_active_clk_ad9510_pll_clk_sel_exp,
- &fmc_active_clk_si571_set_freq_exp,
+ &fmc_active_clk_si571_freq_exp,
&fmc_active_clk_si571_get_defaults_exp,
NULL
};
diff --git a/src/sm_io/modules/fmc_active_clk/sm_io_fmc_active_clk_exports.h b/src/sm_io/modules/fmc_active_clk/sm_io_fmc_active_clk_exports.h
index 4a97ffcc2dd4b96058b0401f4f279ba0dde36dd9..3a3ec16ef29368bd08a2625fce0946553dc12603 100644
--- a/src/sm_io/modules/fmc_active_clk/sm_io_fmc_active_clk_exports.h
+++ b/src/sm_io/modules/fmc_active_clk/sm_io_fmc_active_clk_exports.h
@@ -24,7 +24,7 @@ extern disp_op_t fmc_active_clk_ad9510_mux_status_exp;
extern disp_op_t fmc_active_clk_ad9510_cp_current_exp;
extern disp_op_t fmc_active_clk_ad9510_outputs_exp;
extern disp_op_t fmc_active_clk_ad9510_pll_clk_sel_exp;
-extern disp_op_t fmc_active_clk_si571_set_freq_exp;
+extern disp_op_t fmc_active_clk_si571_freq_exp;
extern disp_op_t fmc_active_clk_si571_get_defaults_exp;
extern const disp_op_t *fmc_active_clk_exp_ops [];