diff --git a/site_ansto/fsm.c b/site_ansto/fsm.c index 1a85760c..83a7ce80 100644 --- a/site_ansto/fsm.c +++ b/site_ansto/fsm.c @@ -1,5 +1,27 @@ #include "fsm.h" +char* fsm_textify(const char* pInp, int inp_len, char* pOut, int out_len) { + int iInp = 0; + int iOut = 0; + for (iInp = 0; iInp < inp_len; ++iInp) { + if ((pInp[iInp] < 32 || pInp[iInp] > 126)) { + if (iOut < out_len - 4) { + const char hex[] = "0123456789abcdef"; + pOut[iOut++] = '\\'; + pOut[iOut++] = hex[(pInp[iInp] >> 4) & 0xf]; + pOut[iOut++] = hex[pInp[iInp] & 0xf]; + } + else if (iOut < out_len - 1) + pOut[iOut++] = '.'; + } + else if (iOut < out_len - 1) + pOut[iOut++] = pInp[iInp]; + } + if (iOut < out_len) + pOut[iOut++] = '\0'; + return pOut; +} + static void report_event(pStateMachine self, pEvtEvent event) { char line[132]; char text[132]; @@ -25,7 +47,9 @@ int fsm_msg_callback(pAsyncTxn pCmd) if (pCmd->txn_status == ATX_TIMEOUT) { if (self->debug) { - SICSLogWrite(pCmd->out_buf, eStatus); + char line[1024]; + fsm_textify(pCmd->out_buf, pCmd->out_len, line, sizeof(line)); + SICSLogWrite(line, eStatus); SICSLogWrite("", eStatus); } event.event_type = eTimeoutEvent; @@ -33,8 +57,11 @@ int fsm_msg_callback(pAsyncTxn pCmd) } else { if (self->debug) { - SICSLogWrite(pCmd->out_buf, eStatus); - SICSLogWrite(pCmd->inp_buf, eStatus); + char line[1024]; + fsm_textify(pCmd->out_buf, pCmd->out_len, line, sizeof(line)); + SICSLogWrite(line, eStatus); + fsm_textify(pCmd->inp_buf, pCmd->inp_idx, line, sizeof(line)); + SICSLogWrite(line, eStatus); } event.event_type = eMessageEvent; event.event.msg.cmd = pCmd; @@ -68,8 +95,7 @@ void fsm_change_state(pStateMachine self, StateFunc func) { self->state_name ? self->state_name(self->myState) : "", self->mySubState, self->state_name ? self->state_name(func) : ""); - if (self->debug) - SICSLogWrite(line, eStatus); + SICSLogWrite(line, eStatus); } self->myPrevState = self->myState; self->myState = func; diff --git a/site_ansto/fsm.h b/site_ansto/fsm.h index e4f9c070..45449dfa 100644 --- a/site_ansto/fsm.h +++ b/site_ansto/fsm.h @@ -53,6 +53,8 @@ struct statemachine_s { const char* (*event_name)(pEvtEvent event, char* text, int length); }; +char* fsm_textify(const char* pInp, int inp_len, char* pOut, int out_len); + int fsm_msg_callback(pAsyncTxn pCmd); int fsm_tmr_callback(void* ctx, int mode); int fsm_cmd_execute(pStateMachine self, const void* cmd);