From e0b299acd4267845f21cbe8a07279c49e815c5c0 Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Tue, 5 Jan 2021 16:01:40 +0100 Subject: [PATCH] [shell] add a shell module with ChibiOS (#2634) * [shell] add a shell module with ChibiOS - add a set of core commands - allow to add custom commands from external modules at runtime - fix small errors in rtos_mon module * [shell] only for ap target at the moment --- conf/airframes/ENAC/fixed-wing/apogee.xml | 8 +- conf/modules/shell.xml | 32 + .../chibios/modules/core/microrl/microrl.c | 703 ++++++++++++++++++ .../chibios/modules/core/microrl/microrl.h | 110 +++ .../modules/core/microrl/microrlConfig.h | 102 +++ .../modules/core/microrl/microrlShell.c | 472 ++++++++++++ .../modules/core/microrl/microrlShell.h | 69 ++ .../arch/chibios/modules/core/rtos_mon_arch.c | 116 ++- .../arch/chibios/modules/core/shell_arch.c | 104 +++ .../arch/chibios/modules/core/shell_arch.h | 35 + sw/airborne/modules/core/shell.c | 35 + sw/airborne/modules/core/shell.h | 45 ++ 12 files changed, 1826 insertions(+), 5 deletions(-) create mode 100644 conf/modules/shell.xml create mode 100644 sw/airborne/arch/chibios/modules/core/microrl/microrl.c create mode 100644 sw/airborne/arch/chibios/modules/core/microrl/microrl.h create mode 100644 sw/airborne/arch/chibios/modules/core/microrl/microrlConfig.h create mode 100644 sw/airborne/arch/chibios/modules/core/microrl/microrlShell.c create mode 100644 sw/airborne/arch/chibios/modules/core/microrl/microrlShell.h create mode 100644 sw/airborne/arch/chibios/modules/core/shell_arch.c create mode 100644 sw/airborne/arch/chibios/modules/core/shell_arch.h create mode 100644 sw/airborne/modules/core/shell.c create mode 100644 sw/airborne/modules/core/shell.h diff --git a/conf/airframes/ENAC/fixed-wing/apogee.xml b/conf/airframes/ENAC/fixed-wing/apogee.xml index 2fa6aca37f..f6035e89b6 100644 --- a/conf/airframes/ENAC/fixed-wing/apogee.xml +++ b/conf/airframes/ENAC/fixed-wing/apogee.xml @@ -7,7 +7,7 @@ - + @@ -39,7 +39,11 @@ - + + + + + diff --git a/conf/modules/shell.xml b/conf/modules/shell.xml new file mode 100644 index 0000000000..9b074cf33c --- /dev/null +++ b/conf/modules/shell.xml @@ -0,0 +1,32 @@ + + + + Simple debug shell + + + + +
+ +
+ + + + + + + + + + + + + + + + + + + + + diff --git a/sw/airborne/arch/chibios/modules/core/microrl/microrl.c b/sw/airborne/arch/chibios/modules/core/microrl/microrl.c new file mode 100644 index 0000000000..34d4cdbb82 --- /dev/null +++ b/sw/airborne/arch/chibios/modules/core/microrl/microrl.c @@ -0,0 +1,703 @@ +/* + Author: Samoylov Eugene aka Helius (ghelius@gmail.com) + BUGS and TODO: + -- add echo_off feature + -- rewrite history for use more than 256 byte buffer +*/ + +#include +#include +#include +#include "microrl.h" +#ifdef _USE_LIBC_STDIO +#include +#endif + +//#define DEBUG_MRL +#ifdef DEBUG_MRL +#include "ch.h" +#include "printf.h" +#include "hal.h" + + +#endif // DEBUG_MRL + + +//#define DBG(...) fprintf(stderr, "\033[33m");fprintf(stderr,__VA_ARGS__);fprintf(stderr,"\033[0m"); + +char * prompt_default = _PROMPT_DEFAUTL; + +#ifdef __GNUC__ +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wstrict-overflow" +#endif + + +#ifdef _USE_HISTORY + +#ifdef _HISTORY_DEBUG +//***************************************************************************** +// print buffer content on screen +static void print_hist (ring_history_t * pThis) +{ + printf ("\n"); + for (int i = 0; i < _RING_HISTORY_LEN; i++) { + if (i == pThis->begin) + printf ("b"); + else + printf (" "); + } + printf ("\n"); + for (int i = 0; i < _RING_HISTORY_LEN; i++) { + if (isalpha(pThis->ring_buf[i])) + printf ("%c", pThis->ring_buf[i]); + else + printf ("%d", pThis->ring_buf[i]); + } + printf ("\n"); + for (int i = 0; i < _RING_HISTORY_LEN; i++) { + if (i == pThis->end) + printf ("e"); + else + printf (" "); + } + printf ("\n"); +} +#endif + +//***************************************************************************** +// remove older message from ring buffer +static void hist_erase_older (ring_history_t * pThis) +{ + int new_pos = pThis->begin + pThis->ring_buf [pThis->begin] + 1; + if (new_pos >= _RING_HISTORY_LEN) + new_pos = new_pos - _RING_HISTORY_LEN; + + pThis->begin = new_pos; +} + +//***************************************************************************** +// check space for new line, remove older while not space +static int hist_is_space_for_new (ring_history_t * pThis, int len) +{ + if (pThis->ring_buf [pThis->begin] == 0) + return true; + if (pThis->end >= pThis->begin) { + if (_RING_HISTORY_LEN - pThis->end + pThis->begin - 1 > len) + return true; + } else { + if (pThis->begin - pThis->end - 1> len) + return true; + } + return false; +} + +//***************************************************************************** +// put line to ring buffer +static void hist_save_line (ring_history_t * pThis, char * line, int len) +{ + if (len > _RING_HISTORY_LEN - 2) + return; + while (!hist_is_space_for_new (pThis, len)) { + hist_erase_older (pThis); + } + // if it's first line + if (pThis->ring_buf [pThis->begin] == 0) + pThis->ring_buf [pThis->begin] = len; + + // store line + if (len < _RING_HISTORY_LEN-pThis->end-1) + memmove (pThis->ring_buf + pThis->end + 1, line, len); + else { + int part_len = _RING_HISTORY_LEN-pThis->end-1; + memmove (pThis->ring_buf + pThis->end + 1, line, part_len); + memmove (pThis->ring_buf, line + part_len, len - part_len); + } + pThis->ring_buf [pThis->end] = len; + pThis->end = pThis->end + len + 1; + if (pThis->end >= _RING_HISTORY_LEN) + pThis->end -= _RING_HISTORY_LEN; + pThis->ring_buf [pThis->end] = 0; + pThis->cur = 0; +#ifdef _HISTORY_DEBUG + print_hist (pThis); +#endif +} + +//***************************************************************************** +// copy saved line to 'line' and return size of line + + +static int hist_restore_line (ring_history_t * pThis, char * line, int dir) +{ + int cnt = 0; + // count history record + int header = pThis->begin; + while (pThis->ring_buf [header] != 0) { + header += pThis->ring_buf [header] + 1; + if (header >= _RING_HISTORY_LEN) + header -= _RING_HISTORY_LEN; + cnt++; + } + + if (dir == _HIST_UP) { + if (cnt >= pThis->cur) { + int lheader = pThis->begin; + int j = 0; + // found record for 'pThis->cur' index + while ((pThis->ring_buf [lheader] != 0) && (cnt - j -1 != pThis->cur)) { + lheader += pThis->ring_buf [lheader] + 1; + if (lheader >= _RING_HISTORY_LEN) + lheader -= _RING_HISTORY_LEN; + j++; + } + if (pThis->ring_buf[lheader]) { + pThis->cur++; + // obtain saved line + if ((pThis->ring_buf [lheader] + lheader + 1) < _RING_HISTORY_LEN) { + // fix from coverity scan + /* + CID 60358 (#1 of 2): Out-of-bounds read (OVERRUN)30. + overrun-local: Overrunning array of 256 bytes at byte offset 256 + by dereferencing pointer &pThis->ring_buf[lheader] + 1 + */ + memmove (line, pThis->ring_buf + lheader + 1, pThis->ring_buf[lheader]); + } else { + int part0 = _RING_HISTORY_LEN - lheader - 1; + memmove (line, pThis->ring_buf + lheader + 1, part0); + memmove (line + part0, pThis->ring_buf, pThis->ring_buf[lheader] - part0); + } + return pThis->ring_buf[lheader]; + } + } + } else { + if (pThis->cur > 0) { + pThis->cur--; + int lheader = pThis->begin; + int j = 0; + + while ((pThis->ring_buf [lheader] != 0) && (cnt - j != pThis->cur)) { + lheader += pThis->ring_buf [lheader] + 1; + if (lheader >= _RING_HISTORY_LEN) + lheader -= _RING_HISTORY_LEN; + j++; + } + if ((pThis->ring_buf [lheader] + lheader +1) < _RING_HISTORY_LEN) { + memmove (line, pThis->ring_buf + lheader + 1, pThis->ring_buf[lheader]); + } else { + int part0 = _RING_HISTORY_LEN - lheader - 1; + memmove (line, pThis->ring_buf + lheader + 1, part0); + memmove (line + part0, pThis->ring_buf, pThis->ring_buf[lheader] - part0); + } + return pThis->ring_buf[lheader]; + } + } + return 0; +} + + +#endif + + +//***************************************************************************** +// split cmdline to tkn array and return nmb of token +static int split (microrl_t * pThis, int limit) +{ + int i = 0; + int ind = 0; + while (1) { + // go to the first whitespace (zerro for us) + while ((pThis->cmdline [ind] == '\0') && (ind < limit)) { + ind++; + } + if (!(ind < limit)) return i; + pThis->tkn_arr[i++] = pThis->cmdline + ind; + if (i >= _COMMAND_TOKEN_NMB) { + return -1; + } + // go to the first NOT whitespace (not zerro for us) + while ((pThis->cmdline [ind] != '\0') && (ind < limit)) { + ind++; + } + if (!(ind < limit)) return i; + } + return i; +} + + +//***************************************************************************** +inline static void print_prompt (microrl_t * pThis) +{ + pThis->print (pThis->prompt_str); +} + +//***************************************************************************** +inline static void terminal_backspace (microrl_t * pThis) +{ + pThis->print ("\033[D \033[D"); +} + +//***************************************************************************** +inline static void terminal_newline (microrl_t * pThis) +{ + pThis->print (ENDL); +} + +#ifndef _USE_LIBC_STDIO +//***************************************************************************** +// convert 16 bit value to string +// 0 value not supported!!! just make empty string +static void u16bit_to_str (unsigned int nmb, char * buf) +{ + char tmp_str [6] = {0,}; + int i = 0; + if (nmb <= 0xFFFF) { + while (nmb > 0) { + tmp_str[i++] = (nmb % 10) + '0'; + nmb /=10; + } + for (int j = 0; j < i; ++j) + *(buf++) = tmp_str [i-j-1]; + } + *buf = '\0'; +} +#endif + + +//***************************************************************************** +// set cursor at position from begin cmdline (after prompt) + offset +static void terminal_move_cursor (microrl_t * pThis, int offset) +{ + char str[16] = {0,}; +#ifdef _USE_LIBC_STDIO + if (offset > 0) { + snprintf (str, 16, "\033[%dC", offset); + } else if (offset < 0) { + snprintf (str, 16, "\033[%dD", -(offset)); + } +#else + strcpy (str, "\033["); + if (offset > 0) { + u16bit_to_str (offset, str+2); + strcat (str, "C"); + } else if (offset < 0) { + u16bit_to_str (-(offset), str+2); + strcat (str, "D"); + } else + return; +#endif + pThis->print (str); +} + +//***************************************************************************** +static void terminal_reset_cursor (microrl_t * pThis) +{ + char str[16]; +#ifdef _USE_LIBC_STDIO + snprintf (str, 16, "\033[%dD\033[%dC", \ + _COMMAND_LINE_LEN + _PROMPT_LEN + 2, _PROMPT_LEN); + +#else + strcpy (str, "\033["); + u16bit_to_str ( _COMMAND_LINE_LEN + _PROMPT_LEN + 2,str+2); + strcat (str, "D\033["); + u16bit_to_str (_PROMPT_LEN, str+strlen(str)); + strcat (str, "C"); +#endif + pThis->print (str); +} + +//***************************************************************************** +// print cmdline to screen, replace '\0' to wihitespace +static void terminal_print_line (microrl_t * pThis, int pos, int cursor) +{ + pThis->print ("\033[K"); // delete all from cursor to end + + char nch [] = {0,0}; + for (int i = pos; i < pThis->cmdlen; i++) { + nch [0] = pThis->cmdline [i]; + if (nch[0] == '\0') + nch[0] = ' '; + pThis->print (nch); + } + + terminal_reset_cursor (pThis); + terminal_move_cursor (pThis, cursor); +} + +//***************************************************************************** +void microrl_init (microrl_t * pThis, void (*print) (const char *)) +{ + memset(pThis->cmdline, 0, _COMMAND_LINE_LEN); +#ifdef _USE_HISTORY + memset(pThis->ring_hist.ring_buf, 0, _RING_HISTORY_LEN); + pThis->ring_hist.begin = 0; + pThis->ring_hist.end = 0; + pThis->ring_hist.cur = 0; +#endif + pThis->cmdlen =0; + pThis->cursor = 0; + pThis->execute = NULL; + pThis->get_completion = NULL; +#ifdef _USE_CTLR_C + pThis->sigint = NULL; +#endif + pThis->prompt_str = prompt_default; + pThis->print = print; +#ifdef _ENABLE_INIT_PROMPT + print_prompt (pThis); +#endif +} + +//***************************************************************************** +void microrl_set_complete_callback (microrl_t * pThis, + const char ** (*get_completion)(int, const char* const*)) +{ + pThis->get_completion = get_completion; +} + +//***************************************************************************** +void microrl_set_execute_callback (microrl_t * pThis, void (*execute)(int, const char* const*)) +{ + pThis->execute = execute; +} +#ifdef _USE_CTLR_C +//***************************************************************************** +void microrl_set_sigint_callback (microrl_t * pThis, void (*sigintf)(void)) +{ + pThis->sigint = sigintf; +} +#endif + +#ifdef _USE_ESC_SEQ +static void hist_search (microrl_t * pThis, int dir) +{ + int len = hist_restore_line (&pThis->ring_hist, pThis->cmdline, dir); + if (len) { + pThis->cursor = pThis->cmdlen = len; + terminal_reset_cursor (pThis); + terminal_print_line (pThis, 0, pThis->cursor); + } +} + +//***************************************************************************** +// handling escape sequences +static int escape_process (microrl_t * pThis, char ch) +{ + static int seq = 0; + + if (ch == '[') { + seq = _ESC_BRACKET; + } else if (seq == _ESC_BRACKET) { + if (ch == 'A') { +#ifdef _USE_HISTORY + hist_search (pThis, _HIST_UP); +#endif + return 1; + } else if (ch == 'B') { +#ifdef _USE_HISTORY + hist_search (pThis, _HIST_DOWN); +#endif + return 1; + } else if (ch == 'C') { + if (pThis->cursor < pThis->cmdlen) { + terminal_move_cursor (pThis, 1); + pThis->cursor++; + } + return 1; + } else if (ch == 'D') { + if (pThis->cursor > 0) { + terminal_move_cursor (pThis, -1); + pThis->cursor--; + } + return 1; + } else if (ch == '7') { + seq = _ESC_HOME; + return 0; + } else if (ch == '8') { + seq = _ESC_END; + return 0; + } + } else if (ch == '~') { + if (seq == _ESC_HOME) { + terminal_reset_cursor (pThis); + pThis->cursor = 0; + return 1; + } else if (seq == _ESC_END) { + terminal_move_cursor (pThis, pThis->cmdlen-pThis->cursor); + pThis->cursor = pThis->cmdlen; + return 1; + } + + } + return 0; +} +#endif + +//***************************************************************************** +// insert len char of text at cursor position +static int microrl_insert_text (microrl_t * pThis, const char * text, int len) +{ +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wstrict-overflow" + if (pThis->cmdlen + len < _COMMAND_LINE_LEN) { +#pragma GCC diagnostic pop + if (pThis->cmdlen != pThis->cursor) { + memmove (pThis->cmdline + pThis->cursor + len, + pThis->cmdline + pThis->cursor, + pThis->cmdlen - pThis->cursor); + } + + for (int i = 0; i < len; i++) { + pThis->cmdline [pThis->cursor + i] = text [i]; + if (pThis->cmdline [pThis->cursor + i] == ' ') { + pThis->cmdline [pThis->cursor + i] = 0; + } + } + pThis->cursor += len; + pThis->cmdlen += len; + pThis->cmdline [pThis->cmdlen] = '\0'; + return true; + } + return false; +} + +//***************************************************************************** +// remove one char at cursor +static void microrl_backspace (microrl_t * pThis) +{ + if (pThis->cursor > 0) { + terminal_backspace (pThis); + if ((pThis->cmdlen-pThis->cursor+1) != 0) { + memmove (pThis->cmdline + pThis->cursor-1, + pThis->cmdline + pThis->cursor, + pThis->cmdlen-pThis->cursor+1); + } + pThis->cursor--; + pThis->cmdline [pThis->cmdlen] = '\0'; + pThis->cmdlen--; + } +} + + +#ifdef _USE_COMPLETE + +//***************************************************************************** +static int common_len (const char ** arr) +{ + int len = 0; + int i = 1; + while (1) { + while (arr[i]!=NULL) { + if ((arr[i][len] != arr[i-1][len]) || + (arr[i][len] == '\0') || + (arr[i-1][len]=='\0')) + return len; + len++; + } + i++; + } + return 0; +} + +//***************************************************************************** +static void microrl_get_complite (microrl_t * pThis) +{ + const char ** compl_token; + + if (pThis->get_completion == NULL) // callback was not set + return; + + int status = split (pThis, pThis->cursor); + if (pThis->cmdline[pThis->cursor-1] == '\0') + pThis->tkn_arr[status++] = ""; + compl_token = pThis->get_completion (status, pThis->tkn_arr); + if (compl_token[0] != NULL) { + int i = 0; + int len; + + if (compl_token[1] == NULL) { + len = strlen (compl_token[0]); + } else { + len = common_len (compl_token); + terminal_newline (pThis); + while (compl_token [i] != NULL) { + pThis->print (compl_token[i]); + pThis->print (" "); + i++; + } + terminal_newline (pThis); + print_prompt (pThis); + } + + if (len) { + microrl_insert_text (pThis, compl_token[0] + strlen(pThis->tkn_arr[status-1]), + len - strlen(pThis->tkn_arr[status-1])); + if (compl_token[1] == NULL) + microrl_insert_text (pThis, " ", 1); + } + terminal_reset_cursor (pThis); + terminal_print_line (pThis, 0, pThis->cursor); + } +} +#endif + +//***************************************************************************** +void new_line_handler(microrl_t * pThis){ + int status; + + terminal_newline (pThis); +#ifdef _USE_HISTORY + if (pThis->cmdlen > 0) + hist_save_line (&pThis->ring_hist, pThis->cmdline, pThis->cmdlen); +#endif + status = split (pThis, pThis->cmdlen); + if (status == -1){ + // pThis->print ("ERROR: Max token amount exseed\n"); + pThis->print ("ERROR:too many tokens"); + pThis->print (ENDL); + } + if ((status > 0) && (pThis->execute != NULL)) + pThis->execute (status, pThis->tkn_arr); + print_prompt (pThis); + pThis->cmdlen = 0; + pThis->cursor = 0; + memset(pThis->cmdline, 0, _COMMAND_LINE_LEN); +#ifdef _USE_HISTORY + pThis->ring_hist.cur = 0; +#endif +} + +//***************************************************************************** +#if (defined(_ENDL_CRLF) || defined(_ENDL_LFCR)) +static int tmpch = 0; +#endif + +void microrl_insert_char (microrl_t * pThis, int ch) +{ + +#ifdef _USE_ESC_SEQ + static int escape = false; + + if (escape) { + if (escape_process(pThis, ch)) + escape = 0; + } else { +#endif + switch (ch) { + //----------------------------------------------------- +#ifdef _ENDL_CR + case KEY_CR: + new_line_handler(pThis); + break; + case KEY_LF: + break; +#elif defined(_ENDL_CRLF) + case KEY_CR: + tmpch = KEY_CR; + break; + case KEY_LF: + if (tmpch == KEY_CR) + new_line_handler(pThis); + break; +#elif defined(_ENDL_LFCR) + case KEY_LF: + tmpch = KEY_LF; + break; + case KEY_CR: + if (tmpch == KEY_LF) + new_line_handler(pThis); + break; +#else + case KEY_CR: + break; + case KEY_LF: + new_line_handler(pThis); + break; +#endif + //----------------------------------------------------- +#ifdef _USE_COMPLETE + case KEY_HT: + microrl_get_complite (pThis); + break; +#endif + //----------------------------------------------------- + case KEY_ESC: +#ifdef _USE_ESC_SEQ + escape = 1; +#endif + break; + //----------------------------------------------------- + case KEY_NAK: // ^U + while (pThis->cursor > 0) { + microrl_backspace (pThis); + } + terminal_print_line (pThis, 0, pThis->cursor); + break; + //----------------------------------------------------- + case KEY_VT: // ^K + pThis->print ("\033[K"); + pThis->cmdlen = pThis->cursor; + break; + //----------------------------------------------------- + case KEY_ENQ: // ^E + terminal_move_cursor (pThis, pThis->cmdlen-pThis->cursor); + pThis->cursor = pThis->cmdlen; + break; + //----------------------------------------------------- + case KEY_SOH: // ^A + terminal_reset_cursor (pThis); + pThis->cursor = 0; + break; + //----------------------------------------------------- + case KEY_ACK: // ^F + if (pThis->cursor < pThis->cmdlen) { + terminal_move_cursor (pThis, 1); + pThis->cursor++; + } + break; + //----------------------------------------------------- + case KEY_STX: // ^B + if (pThis->cursor) { + terminal_move_cursor (pThis, -1); + pThis->cursor--; + } + break; + //----------------------------------------------------- + case KEY_DLE: //^P +#ifdef _USE_HISTORY + hist_search (pThis, _HIST_UP); +#endif + break; + //----------------------------------------------------- + case KEY_SO: //^N +#ifdef _USE_HISTORY + hist_search (pThis, _HIST_DOWN); +#endif + break; + //----------------------------------------------------- + case KEY_DEL: // Backspace + case KEY_BS: // ^U + microrl_backspace (pThis); + terminal_print_line (pThis, pThis->cursor, pThis->cursor); + break; +#ifdef _USE_CTLR_C + case KEY_ETX: + if (pThis->sigint != NULL) + pThis->sigint(); + break; +#endif + //----------------------------------------------------- + default: + if ((ch == ' ') && (pThis->cmdlen == 0)) + break; + if (microrl_insert_text (pThis, (char*)&ch, 1)) + terminal_print_line (pThis, pThis->cursor-1, pThis->cursor); + + break; + } +#ifdef _USE_ESC_SEQ + } +#endif +} + diff --git a/sw/airborne/arch/chibios/modules/core/microrl/microrl.h b/sw/airborne/arch/chibios/modules/core/microrl/microrl.h new file mode 100644 index 0000000000..374d56970f --- /dev/null +++ b/sw/airborne/arch/chibios/modules/core/microrl/microrl.h @@ -0,0 +1,110 @@ +#ifndef _MICRORL_H_ +#define _MICRORL_H_ + +#include "microrlConfig.h" + +#define true 1 +#define false 0 + + /* define the Key codes */ +#define KEY_NUL 0 /**< ^@ Null character */ +#define KEY_SOH 1 /**< ^A Start of heading, = console interrupt */ +#define KEY_STX 2 /**< ^B Start of text, maintenance mode on HP console */ +#define KEY_ETX 3 /**< ^C End of text */ +#define KEY_EOT 4 /**< ^D End of transmission, not the same as ETB */ +#define KEY_ENQ 5 /**< ^E Enquiry, goes with ACK; old HP flow control */ +#define KEY_ACK 6 /**< ^F Acknowledge, clears ENQ logon hand */ +#define KEY_BEL 7 /**< ^G Bell, rings the bell... */ +#define KEY_BS 8 /**< ^H Backspace, works on HP terminals/computers */ +#define KEY_HT 9 /**< ^I Horizontal tab, move to next tab stop */ +#define KEY_LF 10 /**< ^J Line Feed */ +#define KEY_VT 11 /**< ^K Vertical tab */ +#define KEY_FF 12 /**< ^L Form Feed, page eject */ +#define KEY_CR 13 /**< ^M Carriage Return*/ +#define KEY_SO 14 /**< ^N Shift Out, alternate character set */ +#define KEY_SI 15 /**< ^O Shift In, resume defaultn character set */ +#define KEY_DLE 16 /**< ^P Data link escape */ +#define KEY_DC1 17 /**< ^Q XON, with XOFF to pause listings; "okay to send". */ +#define KEY_DC2 18 /**< ^R Device control 2, block-mode flow control */ +#define KEY_DC3 19 /**< ^S XOFF, with XON is TERM=18 flow control */ +#define KEY_DC4 20 /**< ^T Device control 4 */ +#define KEY_NAK 21 /**< ^U Negative acknowledge */ +#define KEY_SYN 22 /**< ^V Synchronous idle */ +#define KEY_ETB 23 /**< ^W End transmission block, not the same as EOT */ +#define KEY_CAN 24 /**< ^X Cancel line, MPE echoes !!! */ +#define KEY_EM 25 /**< ^Y End of medium, Control-Y interrupt */ +#define KEY_SUB 26 /**< ^Z Substitute */ +#define KEY_ESC 27 /**< ^[ Escape, next character is not echoed */ +#define KEY_FS 28 /**< ^\ File separator */ +#define KEY_GS 29 /**< ^] Group separator */ +#define KEY_RS 30 /**< ^^ Record separator, block-mode terminator */ +#define KEY_US 31 /**< ^_ Unit separator */ + +#define KEY_DEL 127 /**< Delete (not a real control character...) */ + +// direction of history navigation +#define _HIST_UP 0 +#define _HIST_DOWN 1 +// esc seq internal codes +#define _ESC_BRACKET 1 +#define _ESC_HOME 2 +#define _ESC_END 3 + +#ifdef _USE_HISTORY +// history struct, contain internal variable +// history store in static ring buffer for memory saving +typedef struct { + char ring_buf [_RING_HISTORY_LEN]; + int begin; + int end; + int cur; +} ring_history_t; +#endif + +// microrl struct, contain internal library data +typedef struct { +#ifdef _USE_HISTORY + ring_history_t ring_hist; // history object +#endif + char * prompt_str; // pointer to prompt string + char cmdline [_COMMAND_LINE_LEN]; // cmdline buffer + int cmdlen; // last position in command line + int cursor; // input cursor + char const * tkn_arr [_COMMAND_TOKEN_NMB]; // array of token for call 'execute' callback + void (*execute) (int argc, const char * const * argv ); // ptr to 'execute' callback + const char ** (*get_completion) (int argc, const char * const * argv ); // ptr to 'completion' callback + void (*print) (const char *); // ptr to 'print' callback +#ifdef _USE_CTLR_C + void (*sigint) (void); +#endif +} microrl_t; + +// init internal data, calls once at start up +void microrl_init (microrl_t * pThis, void (*print)(const char*)); + +// set echo mode (true/false), using for disabling echo for password input +// echo mode will enabled after user press Enter. +void microrl_set_echo (int); + +// set pointer to callback complition func, that called when user press 'Tab' +// callback func description: +// param: argc - argument count, argv - pointer array to token string +// must return NULL-terminated string, contain complite variant splitted by 'Whitespace' +// If complite token found, it's must contain only one token to be complitted +// Empty string if complite not found, and multiple string if there are some token +void microrl_set_complete_callback (microrl_t * pThis, + const char ** (*get_completion)(int, const char* const*)); + +// pointer to callback func, that called when user press 'Enter' +// execute func param: argc - argument count, argv - pointer array to token string +void microrl_set_execute_callback (microrl_t * pThis, void (*execute)(int, const char* const*)); + +// set callback for Ctrl+C terminal signal +#ifdef _USE_CTLR_C +void microrl_set_sigint_callback (microrl_t * pThis, void (*sigintf)(void)); +#endif + +// insert char to cmdline (for example call in usart RX interrupt) +void microrl_insert_char (microrl_t * pThis, int ch); + +#endif diff --git a/sw/airborne/arch/chibios/modules/core/microrl/microrlConfig.h b/sw/airborne/arch/chibios/modules/core/microrl/microrlConfig.h new file mode 100644 index 0000000000..50ba49b282 --- /dev/null +++ b/sw/airborne/arch/chibios/modules/core/microrl/microrlConfig.h @@ -0,0 +1,102 @@ +/* +Microrl library config files +Autor: Eugene Samoylov aka Helius (ghelius@gmail.com) +*/ +#ifndef _MICRORL_CONFIG_H_ +#define _MICRORL_CONFIG_H_ + +#define MICRORL_LIB_VER "1.5" + +/*********** CONFIG SECTION **************/ +/* +Command line length, define cmdline buffer size. Set max number of chars + 1, +because last byte of buffer need to contain '\0' - NULL terminator, and +not use for storing inputed char. +If user input chars more then it parametrs-1, chars not added to command line.*/ +#define _COMMAND_LINE_LEN (1+100) // for 32 chars + +/* +Command token number, define max token it command line, if number of token +typed in command line exceed this value, then prints message about it and +command line not to be parced and 'execute' callback will not calls. +Token is word separate by white space, for example 3 token line: +"IRin> set mode test" */ +#define _COMMAND_TOKEN_NMB 30 + +/* +Define you prompt string here. You can use colors escape code, for highlight you prompt, +for example this prompt will green color (if you terminal supports color)*/ +#define _PROMPT_DEFAUTL "\033[32mpprz >\033[0m " // green color + +/* +Define prompt text (without ESC sequence, only text) prompt length, it needs because if you use +ESC sequence, it's not possible detect only text length*/ +#define _PROMPT_LEN 7 + +/*Define it, if you wanna use completion functional, also set completion callback in you code, +now if user press TAB calls 'copmlitetion' callback. If you no need it, you can just set +NULL to callback ptr and do not use it, but for memory saving tune, +if you are not going to use it - disable this define.*/ +#define _USE_COMPLETE + +/*Define it, if you wanna use history. It s work's like bash history, and +set stored value to cmdline, if UP and DOWN key pressed. Using history add +memory consuming, depends from _RING_HISTORY_LEN parametr */ +#define _USE_HISTORY + +/* +History ring buffer length, define static buffer size. +For saving memory, each entered cmdline store to history in ring buffer, +so we can not say, how many line we can store, it depends from cmdline len, +but memory using more effective. We not prefer dinamic memory allocation for +small and embedded devices. Overhead is 2 char on each saved line*/ +#define _RING_HISTORY_LEN 256 + +/* +Enable Handling terminal ESC sequence. If disabling, then cursor arrow, HOME, END will not work, +use Ctrl+A(B,F,P,N,A,E,H,K,U,C) see README, but decrease code memory.*/ +#define _USE_ESC_SEQ + +/* +Use snprintf from you standard complier library, but it gives some overhead. +If not defined, use my own u16int_to_str variant, it's save about 800 byte of code size +on AVR (avr-gcc build). +Try to build with and without, and compare total code size for tune library. +*/ +//#define _USE_LIBC_STDIO + +/* +Enable 'interrupt signal' callback, if user press Ctrl+C */ +#define _USE_CTLR_C + +/* +Print prompt at 'microrl_init', if enable, prompt will print at startup, +otherwise first prompt will print after first press Enter in terminal +NOTE!: Enable it, if you call 'microrl_init' after your communication subsystem +already initialize and ready to print message */ +#undef _ENABLE_INIT_PROMPT + +/* +New line symbol */ +#define _ENDL_CR + +#if defined(_ENDL_CR) +#define ENDL "\r\n" +#elif defined(_ENDL_CRLF) +#define ENDL "\r\n" +#elif defined(_ENDL_LF) +#define ENDL "\n" +#elif defined(_ENDL_LFCR) +#define ENDL "\n\r" +#else +#error "You must define new line symbol." +#endif + +/********** END CONFIG SECTION ************/ + + +#if _RING_HISTORY_LEN > 256 +#error "This history implementation (ring buffer with 1 byte iterator) allow 256 byte buffer size maximum" +#endif + +#endif diff --git a/sw/airborne/arch/chibios/modules/core/microrl/microrlShell.c b/sw/airborne/arch/chibios/modules/core/microrl/microrlShell.c new file mode 100644 index 0000000000..b7391f8c8b --- /dev/null +++ b/sw/airborne/arch/chibios/modules/core/microrl/microrlShell.c @@ -0,0 +1,472 @@ +/** + * @file shell.c + * @brief Enhanced CLI shell code. + * + * @addtogroup SHELL + * @{ + */ + +#include +#include "microrl/microrlShell.h" +#include "microrl/microrl.h" +#include "printf.h" +//#include "stdutil.h" + + +#define printScreen(...) {chprintf (chpg, __VA_ARGS__); chprintf (chpg, "\r\n");} + +typedef struct { + void (*altFunc) (uint8_t c, uint32_t mode); + uint32_t param; +} AltCbParam; + +static void cmd_info(BaseSequentialStream *lchp, int argc, + const char * const argv[]); + + +/** + * @brief Shell termination event source. + */ + event_source_t shell_terminated; + +static MUTEX_DECL(mut); +static microrl_t rl; +static BaseSequentialStream *chpg; +static const ShellCommand *staticCommands = NULL; +static const char * complWorlds[64]; +/** + * @brief Array of the default and dynamic commands. + */ +#if SHELL_DYNAMIC_ENTRIES_NUMBER // compatibility with legacy static only behavior +static ShellCommand localCommands[SHELL_DYNAMIC_ENTRIES_NUMBER + 2U] = { +#else +static const ShellCommand localCommands[] = { +#endif + {"info", cmd_info}, + {NULL, NULL} +}; + +static AltCbParam altCbParam = {.altFunc = NULL, .param = 0}; + +void microrlPrint (const char * str) +{ + int i = 0; + + while (str[i] != 0) { + streamPut(chpg, str[i++]); + } +} + +void microrlExecute (int argc, const char * const *argv) +{ + const ShellCommand *scp = staticCommands; + const char *name = argv[0]; + + chMtxLock(&mut); + + while (scp->sc_name != NULL) { + if (strcasecmp(scp->sc_name, name) == 0) { + scp->sc_function(chpg, argc-1, &argv[1]); + goto exit; + } + scp++; + } + + scp = localCommands; + while (scp->sc_name != NULL) { + if (strcasecmp(scp->sc_name, name) == 0) { + scp->sc_function(chpg, argc-1, &argv[1]); + goto exit; + } + scp++; + } + + exit: + chMtxUnlock(&mut); +} + +const char ** microrlComplet (int argc, const char * const * argv) +{ + uint32_t j = 0; + + complWorlds [0] = NULL; + chMtxLock(&mut); + + // if there is token in cmdline + if (argc == 1) { + // get last entered token + const char *bit = argv[argc-1]; + // iterate through our available token and match it + for (const ShellCommand *scp = localCommands; + scp->sc_name != NULL; scp++) { + // if token is matched (text is part of our token starting from 0 char) + if (strstr(scp->sc_name, bit) == scp->sc_name) { + // add it to completion set + complWorlds[j++] = scp->sc_name; + } + } + for (const ShellCommand *scp = staticCommands; + scp->sc_name != NULL; scp++) { + // if token is matched (text is part of our token starting from 0 char) + if (strstr(scp->sc_name, bit) == scp->sc_name) { + // add it to completion set + complWorlds[j++] = scp->sc_name; + } + } + } else { // if there is no token in cmdline, just print all available token + for (const ShellCommand *scp = localCommands; scp->sc_name != NULL; scp++) + complWorlds[j++] = scp->sc_name; + for (const ShellCommand *scp = staticCommands; scp->sc_name != NULL; scp++) + complWorlds[j++] = scp->sc_name; + } + + // note! last ptr in array always must be NULL!!! + complWorlds[j] = NULL; + chMtxUnlock(&mut); + // return set of variants + return complWorlds; +} + + +void microrlSigint (void) +{ + chprintf (chpg, "^C catched!\n\r"); +} + + +static void usage(BaseSequentialStream *lchp, char *p) { + + chprintf(lchp, "Usage: %s\r\n", p); +} + + + +static void cmd_info(BaseSequentialStream *lchp, int argc, const char * const argv[]) { + + (void)argv; + if (argc > 0) { + usage(lchp, "info"); + return; + } + + /* + Bits 31:16 REV_ID[15:0] Revision identifier + This field indicates the revision of the device. + STM32F405xx/07xx and STM32F415xx/17xx devices: + 0x1000 = Revision A + 0x1001 = Revision Z + 0x1003 = Revision 1 + 0x1007 = Revision 2 + 0x100F= Revision Y + STM32F42xxx and STM32F43xxx devices: + 0x1000 = Revision A + 0x1003 = Revision Y + 0x1007 = Revision 1 + 0x2001= Revision 3 + Bits 15:12 Reserved, must be kept at reset value. + Bits 11:0 DEV_ID[11:0]: Device identifier (STM32F405xx/07xx and STM32F415xx/17xx) + The device ID is 0x413. + Bits 11:0 DEV_ID[11:0]: Device identifier (STM32F42xxx and STM32F43xxx) + The device ID is 0x419 + + + F7 + Bits 31:16 REV_ID[15:0] Revision identifier + This field indicates the revision of the device: + 0x1000 = Revision A + 0x1001 = Revision Z + Bits 15:12 Reserved, must be kept at reset value. + Bits 11:0 DEV_ID[11:0]: Device identifier + The device ID is 0x449. + + + L47x 49x + Bits 31:16 REV_ID[15:0] Revision identifier + This field indicates the revision of the device. + For STM32L475xx/476xx/486xx devices + 0x1000: Rev 1 + 0x1001: Rev 2 + 0x1003: Rev 3 + 0x1007: Rev 4 + For STM32L496xx/4A6xx devices + 0x1000: Rev A + 0x2000: Rev B + + Bits 11:0 DEV_ID[11:0]: Device identifier + The device ID is: + 0x461 for STM32L496xx/4A6xx devices + 0x415 for STM32L475xx/476xx/486xx devices. + */ + + + const uint16_t mcu_revid = (DBGMCU->IDCODE & DBGMCU_IDCODE_REV_ID) >> 16; + const uint16_t mcu_devid = DBGMCU->IDCODE & DBGMCU_IDCODE_DEV_ID; + char *mcu_devid_str ="not known, please fix microrlShell.c"; + char mcu_revid_chr = '?'; + + switch (mcu_devid) { + case 0x415 : mcu_devid_str = "STM32L475xx/476xx/486xx devices"; + switch (mcu_revid) { + case 0x1000 : mcu_revid_chr = '1'; break; + case 0x1001 : mcu_revid_chr = '2'; break; + case 0x1003 : mcu_revid_chr = '3'; break; + case 0x1007 : mcu_revid_chr = '4'; break; + } + break; + case 0x461 : mcu_devid_str = "STM32L496xx/4A6xx devices"; + switch (mcu_revid) { + case 0x1000 : mcu_revid_chr = 'A'; break; + case 0x2000 : mcu_revid_chr = 'B'; break; + } + break; + case 0x411 : mcu_devid_str = "STM32F2xx and *EARLY* STM32F40x and 41x"; + switch (mcu_revid) { + case 0x1000 : mcu_revid_chr = 'A'; break; + case 0x1001 : mcu_revid_chr = 'Z'; break; + case 0x2000 : mcu_revid_chr = 'B'; break; + case 0x2001 : mcu_revid_chr = 'Y'; break; + case 0x2003 : mcu_revid_chr = 'X'; break; + } + break; + case 0x413 : mcu_devid_str = "STM32F40x and 41x"; + switch (mcu_revid) { + case 0x1000 : mcu_revid_chr = 'A'; break; + case 0x1001 : mcu_revid_chr = 'Z'; break; + case 0x1003 : mcu_revid_chr = '1'; break; + case 0x1007 : mcu_revid_chr = '2'; break; + case 0x100F : mcu_revid_chr = 'Y'; break; + } + break; + case 0x419 : mcu_devid_str = "STM32F42x and F43x"; + switch (mcu_revid) { + case 0x1000 : mcu_revid_chr = 'A'; break; + case 0x1003 : mcu_revid_chr = 'Y'; break; + case 0x1007 : mcu_revid_chr = '1'; break; + case 0x2001 : mcu_revid_chr = '3'; break; + } + break; + case 0x449 : mcu_devid_str = "STM32F74x and STM32F75x"; + switch (mcu_revid) { + case 0x1000 : mcu_revid_chr = 'A'; break; + case 0x1001 : mcu_revid_chr = 'Z'; break; + } + break; + case 0x451 : mcu_devid_str = "STM32F76x and STM32F77x"; + switch (mcu_revid) { + case 0x1000 : mcu_revid_chr = 'A'; break; + case 0x1001 : mcu_revid_chr = 'Z'; break; + } + break; + case 0x435 : mcu_devid_str = "STM32L43x"; + switch (mcu_revid) { + case 0x1000 : mcu_revid_chr = 'A'; break; + case 0x1001 : mcu_revid_chr = 'Z'; break; + } + break; + case 0x446 : mcu_devid_str = "STM32F303xD/E and STM32F398xE"; + switch (mcu_revid) { + case 0x1001 : mcu_revid_chr = 'Z'; break; + case 0x1003 : mcu_revid_chr = 'Y'; break; + } + break; + } + + chprintf(lchp, "Kernel: %s\r\n", CH_KERNEL_VERSION); +#ifdef HAL_VERSION + chprintf(lchp, "Hal: %s\r\n", HAL_VERSION); +#endif + +#ifdef CH_COMPILER_NAME + chprintf(lchp, "Compiler: %s\r\n", CH_COMPILER_NAME); +#endif +#ifdef PORT_COMPILER_NAME + chprintf(lchp, "Compiler: %s\r\n", PORT_COMPILER_NAME); +#endif + +#ifdef CH_ARCHITECTURE_NAME + chprintf(lchp, "Architecture: %s\r\n", CH_ARCHITECTURE_NAME); +#endif +#ifdef PORT_ARCHITECTURE_NAME + chprintf(lchp, "Architecture: %s\r\n", PORT_ARCHITECTURE_NAME); +#endif + + +#ifdef CH_CORE_VARIANT_NAME + chprintf(lchp, "Core Variant: %s\r\n", CH_CORE_VARIANT_NAME); +#endif +#ifdef PORT_CORE_VARIANT_NAME + chprintf(lchp, "Core Variant: %s\r\n", PORT_CORE_VARIANT_NAME); +#endif +#ifdef STM32_SYSCLK +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wdouble-promotion" + chprintf(lchp, "Main STM32_SYSCLK frequency %.2f Mhz\r\n", STM32_SYSCLK/1e6f); +#pragma GCC diagnostic pop +#endif + +#ifdef CH_PORT_INFO + chprintf(lchp, "Port Info: %s\r\n", CH_PORT_INFO); +#endif +#ifdef PORT_INFO + chprintf(lchp, "Port Info: %s\r\n", PORT_INFO); +#endif + +#ifdef PLATFORM_NAME + chprintf(lchp, "Platform: %s\r\n", PLATFORM_NAME); +#endif + +#ifdef BOARD_NAME + chprintf(lchp, "Board: %s\r\n", BOARD_NAME); +#endif + + chprintf(lchp, "Chip Revision: %s REV '%c' (0x%x:0x%x)\r\n", mcu_devid_str, mcu_revid_chr, mcu_devid, mcu_revid); + +#if (!defined STM32_USE_REVISION_A_FIX) || (STM32_USE_REVISION_A_FIX == 0) + if ((mcu_devid == 0x413) && (mcu_revid_chr == 'A')) { + chprintf(lchp, "Chip Revision: %s REV '%c' PLEASE define STM32_USE_REVISION_A_FIX in mcuconf.h !!\r\n", + mcu_devid_str, mcu_revid_chr); + } +#endif + +#ifdef __DATE__ +#ifdef __TIME__ + chprintf(lchp, "Build time: %s%s%s\r\n", __DATE__, " - ", __TIME__); +#endif +#endif + + chprintf(lchp, "systime= %lu\r\n", (unsigned long)chVTGetSystemTimeX()); +} + + +/** + * @brief Shell thread function. + * + * @param[in] p pointer to a @p BaseSequentialStream object + * @return Termination reason. + * @retval MSG_OK terminated by command. + * @retval RDY_RESET terminated by reset condition on the I/O channel. + */ + +static THD_FUNCTION(shell_thread, p) { + msg_t msg = MSG_OK; + chpg = ((ShellConfig *)p)->sc_channel; + staticCommands = ((ShellConfig *)p)->sc_commands; + bool readOk=TRUE; + + + chRegSetThreadName("Enhanced_shell"); + printScreen ("ChibiOS/RT Enhanced Shell"); + while (!chThdShouldTerminateX() && readOk) { + uint8_t c; + if (streamRead(chpg, &c, 1) == 0) { + readOk=FALSE; + } else { + if (altCbParam.altFunc == NULL) { + microrl_insert_char (&rl, c); + } else { + (*altCbParam.altFunc) (c, altCbParam.param); + } + } + } + /* Atomically broadcasting the event source and terminating the thread, + there is not a chSysUnlock() because the thread terminates upon return.*/ + printScreen ("exit"); + chSysLock(); + chEvtBroadcastI(&shell_terminated); + chThdExitS(msg); +} + +/** + * @brief Shell manager initialization. + */ +void shellInit(void) { + chEvtObjectInit(&shell_terminated); + microrl_init (&rl, microrlPrint); + microrl_set_execute_callback (&rl, µrlExecute); + microrl_set_complete_callback (&rl, µrlComplet); + microrl_set_sigint_callback (&rl, µrlSigint); +} + +/** + * @brief Spawns a new shell. + * @pre @p CH_USE_MALLOC_HEAP and @p CH_USE_DYNAMIC must be enabled. + * + * @param[in] scp pointer to a @p ShellConfig object + * @param[in] size size of the shell working area to be allocated + * @param[in] prio priority level for the new shell + * @return A pointer to the shell thread. + * @retval NULL thread creation failed because memory allocation. + */ +#if CH_CFG_USE_HEAP && CH_CFG_USE_DYNAMIC +thread_t *shellCreateFromHeap(const ShellConfig *scp, size_t size, tprio_t prio) { + return chThdCreateFromHeap(NULL, size, "shell", prio, shell_thread, (void *)scp); +} +#endif + + +/** + * @brief Create statically allocated shell thread. + * + * @param[in] scp pointer to a @p ShellConfig object + * @param[in] wsp pointer to a working area dedicated to the shell thread stack + * @param[in] size size of the shell working area + * @param[in] prio priority level for the new shell + * @return A pointer to the shell thread. + */ + thread_t *shellCreateStatic(const ShellConfig *scp, void *wsp, + size_t size, tprio_t prio) { + return chThdCreateStatic(wsp, size, prio, shell_thread, (void *)scp); +} + +#if SHELL_DYNAMIC_ENTRIES_NUMBER +/** + * @brief add/change/remove dynamically new shell entries + * @pre @p SHELL_DYNAMIC_ENTRIES_NUMBER must be set + * @note if sc.sc_name already exists, entry will be overwrite + if sc.sc_name already exists and sc.sc_function is null, + entry will be removed + * @param[in] sc ShellCommand object + * @return true : OK, false : error table is full. + */ +bool shellAddEntry(const ShellCommand sc) +{ + ShellCommand* lcp = localCommands; + static const int len = (sizeof(localCommands) / sizeof(localCommands[0])) + -1U; + chMtxLock(&mut); + while ((lcp->sc_function != NULL) && + (strcmp(lcp->sc_name, sc.sc_name) != 0) && + ((lcp - localCommands) < len)) + lcp++; + + if ((lcp - localCommands) == len) { + chMtxUnlock(&mut); + return false; + } + lcp->sc_function = sc.sc_function; + lcp->sc_name = sc.sc_function == NULL ? NULL : sc.sc_name; + *(++lcp) = (ShellCommand){NULL, NULL}; + + chMtxUnlock(&mut); + return true; +} +#endif + +void modeAlternate(void (*funcp) (uint8_t c, uint32_t mode), uint32_t mode) +{ + chMtxLock(&mut); + altCbParam.altFunc = funcp; + altCbParam.param = mode; + chMtxUnlock(&mut); +} + +void modeShell(void) +{ + chMtxLock(&mut); + altCbParam.altFunc = NULL; + chMtxUnlock(&mut); + printScreen ("retour au shell"); +} + + + diff --git a/sw/airborne/arch/chibios/modules/core/microrl/microrlShell.h b/sw/airborne/arch/chibios/modules/core/microrl/microrlShell.h new file mode 100644 index 0000000000..7fb8e6668e --- /dev/null +++ b/sw/airborne/arch/chibios/modules/core/microrl/microrlShell.h @@ -0,0 +1,69 @@ +#pragma once +/** + * @file microrlShell.h + * @brief Simple CLI shell header. + * + * @addtogroup SHELL + * @{ + */ + +#include "ch.h" +#include "hal.h" + + +#ifndef SHELL_DYNAMIC_ENTRIES_NUMBER +#define SHELL_DYNAMIC_ENTRIES_NUMBER 0U +#endif + +// legacy compatibility +#define shellCreate(C, S, P) shellCreateFromHeap(C, S, P) + +/** + * @brief Command handler function type. + */ +typedef void (shellcmd_f)(BaseSequentialStream *chp, int argc, const char * const argv[]); + + +/** + * @brief Custom command entry type. + */ +typedef struct { + const char *sc_name; /**< @brief Command name. */ + shellcmd_f *sc_function; /**< @brief Command function. */ +} ShellCommand; + +/** + * @brief Shell descriptor type. + */ +typedef struct { + BaseSequentialStream *sc_channel; /**< @brief I/O channel associated + to the shell. */ + const ShellCommand *sc_commands; /**< @brief Shell extra commands + table. */ +} ShellConfig; + +#if !defined(__DOXYGEN__) +extern event_source_t shell_terminated; +#endif + +#ifdef __cplusplus +extern "C" { +#endif + void shellInit(void); +#if CH_CFG_USE_HEAP && CH_CFG_USE_DYNAMIC + thread_t *shellCreateFromHeap(const ShellConfig *scp, size_t size, tprio_t prio); +#endif + thread_t *shellCreateStatic(const ShellConfig *scp, void *wsp, + size_t size, tprio_t prio); +#if SHELL_DYNAMIC_ENTRIES_NUMBER + bool shellAddEntry(const ShellCommand sc); +#endif + bool shellGetLine(BaseSequentialStream *chp, char *line, unsigned size); + void modeAlternate (void (*) (uint8_t c, uint32_t mode), uint32_t mode); + void modeShell (void); +#ifdef __cplusplus +} +#endif + + +/** @} */ diff --git a/sw/airborne/arch/chibios/modules/core/rtos_mon_arch.c b/sw/airborne/arch/chibios/modules/core/rtos_mon_arch.c index 47b2eb27ac..101510daf3 100644 --- a/sw/airborne/arch/chibios/modules/core/rtos_mon_arch.c +++ b/sw/airborne/arch/chibios/modules/core/rtos_mon_arch.c @@ -1,5 +1,6 @@ /* * Copyright (C) 2016 Gautier Hattenberger + * 2020 Gautier Hattenberger, Alexandre Bustico * * This file is part of paparazzi * @@ -36,10 +37,119 @@ static uint32_t idle_counter, last_idle_counter; static uint16_t get_stack_free(const thread_t *tp); +#if USE_SHELL +#include "modules/core/shell.h" +#include "printf.h" +#include "string.h" +#define MAX_CPU_INFO_ENTRIES 20 + +typedef struct _ThreadCpuInfo { + float ticks[MAX_CPU_INFO_ENTRIES]; + float cpu[MAX_CPU_INFO_ENTRIES]; + float totalTicks; +} ThreadCpuInfo ; + + +static void stampThreadCpuInfo (ThreadCpuInfo *ti) +{ + const thread_t *tp = chRegFirstThread(); + uint32_t idx=0; + float totalTicks =0; + do { + totalTicks+= (float) tp->time; + ti->cpu[idx] = (float) tp->time - ti->ticks[idx];; + ti->ticks[idx] = (float) tp->time; + tp = chRegNextThread ((thread_t *)tp); + idx++; + } while ((tp != NULL) && (idx < MAX_CPU_INFO_ENTRIES)); + + const float diffTotal = totalTicks- ti->totalTicks; + ti->totalTicks = totalTicks; + tp = chRegFirstThread(); + idx=0; + do { + ti->cpu[idx] = (ti->cpu[idx]*100.f)/diffTotal; + tp = chRegNextThread ((thread_t *)tp); + idx++; + } while ((tp != NULL) && (idx < MAX_CPU_INFO_ENTRIES)); +} + +static float stampThreadGetCpuPercent (const ThreadCpuInfo *ti, const uint32_t idx) +{ + if (idx >= MAX_CPU_INFO_ENTRIES) + return -1.f; + return ti->cpu[idx]; +} + +static void cmd_threads(shell_stream_t *lchp, int argc,const char* const argv[]) { + static const char *states[] = {CH_STATE_NAMES}; + thread_t *tp = chRegFirstThread(); + (void)argv; + (void)argc; + float totalTicks=0; + float idleTicks=0; + + static ThreadCpuInfo threadCpuInfo = { + .ticks = {[0 ... MAX_CPU_INFO_ENTRIES-1] = 0.f}, + .cpu = {[0 ... MAX_CPU_INFO_ENTRIES-1] =-1.f}, + .totalTicks = 0.f + }; + stampThreadCpuInfo (&threadCpuInfo); + + chprintf (lchp, " addr stack frestk prio refs state time \t percent name\r\n"); + uint32_t idx=0; + do { + chprintf (lchp, "%.8lx %.8lx %6lu %4lu %4lu %9s %9lu %.1f \t%s\r\n", + (uint32_t)tp, (uint32_t)tp->ctx.sp, + get_stack_free (tp), + (uint32_t)tp->prio, (uint32_t)(tp->refs - 1), + states[tp->state], (uint32_t)tp->time, + stampThreadGetCpuPercent (&threadCpuInfo, idx), + chRegGetThreadNameX(tp)); + totalTicks+= (float) tp->time; + if (strcmp (chRegGetThreadNameX(tp), "idle") == 0) + idleTicks = (float) tp->time; + tp = chRegNextThread ((thread_t *)tp); + idx++; + } while (tp != NULL); + const float idlePercent = (idleTicks*100.f)/totalTicks; + const float cpuPercent = 100.f - idlePercent; + chprintf (lchp, "\r\ncpu load = %.2f%%\r\n", cpuPercent); +} + +static void cmd_rtos_mon(shell_stream_t *sh, int argc, const char * const argv[]) +{ + (void) argv; + if (argc > 0) { + chprintf(sh, "Usage: rtos_mon\r\n"); + return; + } + + chprintf(sh, "Data reported in the RTOS_MON message:\r\n"); + chprintf(sh, " core free mem: %u\r\n", rtos_mon.core_free_memory); + chprintf(sh, " heap free mem: %u\r\n", rtos_mon.heap_free_memory); + chprintf(sh, " heap fragments: %u\r\n", rtos_mon.heap_fragments); + chprintf(sh, " heap largest: %u\r\n", rtos_mon.heap_largest); + chprintf(sh, " CPU load: %d \%\r\n", rtos_mon.cpu_load); + chprintf(sh, " number of threads: %d\r\n", rtos_mon.thread_counter); + chprintf(sh, " thread names: %s\r\n", rtos_mon.thread_names); + for (int i = 0; i < rtos_mon.thread_counter; i++) { + chprintf(sh, " thread %d load: %0.1f, free stack: %d\r\n", i, + (float)rtos_mon.thread_load[i] / 10.f, rtos_mon.thread_free_stack[i]); + } + chprintf(sh, " CPU time: %.2f\r\n", rtos_mon.cpu_time); +} +#endif + void rtos_mon_init_arch(void) { idle_counter = 0; last_idle_counter = 0; + +#if USE_SHELL + shell_add_entry("rtos_mon", cmd_rtos_mon); + shell_add_entry("threads", cmd_threads); +#endif } // Fill data structure @@ -85,15 +195,15 @@ void rtos_mon_periodic_arch(void) rtos_mon.thread_counter++; } while (tp != NULL && rtos_mon.thread_counter < RTOS_MON_MAX_THREADS); - // store individual thread load (as centi-percent integer) + // store individual thread load (as centi-percent integer, i.e. (th_time/sum)*10*100) for (i = 0; i < rtos_mon.thread_counter; i ++) { - rtos_mon.thread_load[i] = (uint16_t)(10000.f * (float)thread_p_time[i] / sum); + rtos_mon.thread_load[i] = (uint16_t)(1000.f * (float)thread_p_time[i] / sum); } // assume we call the counter once a second // so the difference in seconds is always one // NOTE: not perfectly precise, +-5% on average so take it into consideration - rtos_mon.cpu_load = (1 - (float)(idle_counter - last_idle_counter) / CH_CFG_ST_FREQUENCY) * 100; + rtos_mon.cpu_load = (uint8_t)((1.f - ((float)idle_counter / sum)) * 100.f); last_idle_counter = idle_counter; } diff --git a/sw/airborne/arch/chibios/modules/core/shell_arch.c b/sw/airborne/arch/chibios/modules/core/shell_arch.c new file mode 100644 index 0000000000..1fb315ba39 --- /dev/null +++ b/sw/airborne/arch/chibios/modules/core/shell_arch.c @@ -0,0 +1,104 @@ +/* + * Copyright (C) Alexandre Bustico + * + * This file is part of paparazzi + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, see + * . + */ + +/** @file "modules/core/shell_arch.c" + * @author Alexandre Bustico + * Simple debug shell + */ + +#include "modules/core/shell.h" +#include "modules/core/microrl/microrlShell.h" +#include "mcu_periph/uart.h" +#include "printf.h" +#include "subsystems/abi.h" + +/************************* + * Basic static commands * + *************************/ + +static void cmd_mem(BaseSequentialStream *lchp, int argc, const char * const argv[]) +{ + (void)argv; + if (argc > 0) { + chprintf(lchp, "Usage: mem\r\n"); + return; + } + + chprintf (lchp, "core free memory : %u bytes\r\n", chCoreGetStatusX()); + //chprintf (lchp, "heap free memory : %u bytes\r\n", getHeapFree()); +} + +static void cmd_abi(BaseSequentialStream *lchp, int argc, const char * const argv[]) +{ + (void)argv; + if (argc > 0) { + chprintf(lchp, "Usage: abi\r\n"); + return; + } + + chprintf(lchp, "ABI message bindings\r\n"); + for (int i = 0; i < ABI_MESSAGE_NB; i++) { + chprintf(lchp, " msg %d: ", i); + if (abi_queues[i] == NULL) { + chprintf(lchp, "no bindings\r\n"); + } else { + abi_event *e; + ABI_FOREACH(abi_queues[i], e) { + chprintf(lchp, "(cb 0x%lx, id %d), ", e->cb, e->id); + } + chprintf(lchp, "\r\n"); + } + } +} + +static const ShellCommand commands[] = { + {"mem", cmd_mem}, + {"abi", cmd_abi}, + {NULL, NULL} +}; + +static ShellConfig shell_cfg = { + NULL, + commands +}; + +/** + * Add dynamic entry + */ +void shell_add_entry(char *cmd_name, shell_cmd_t *cmd) +{ + shellAddEntry((ShellCommand) {cmd_name, cmd}); +} + + +/** Arch init + */ +void shell_init_arch(void) +{ + // This should be called after mcu periph init + shell_cfg.sc_channel = (BaseSequentialStream *) (SHELL_DEV.reg_addr); + + shellInit(); + thread_t * shelltp = shellCreateFromHeap(&shell_cfg, 2048U, NORMALPRIO); + if (shelltp == NULL) { + chSysHalt("fail starting shell"); + } +} + diff --git a/sw/airborne/arch/chibios/modules/core/shell_arch.h b/sw/airborne/arch/chibios/modules/core/shell_arch.h new file mode 100644 index 0000000000..38163ea185 --- /dev/null +++ b/sw/airborne/arch/chibios/modules/core/shell_arch.h @@ -0,0 +1,35 @@ +/* + * Copyright (C) Alexandre Bustico + * + * This file is part of paparazzi + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, see + * . + */ + +/** @file "modules/core/shell_arch.h" + * @author Alexandre Bustico + * Simple debug shell + */ + +#ifndef SHELL_ARCH_H +#define SHELL_ARCH_H + +#include + +typedef BaseSequentialStream shell_stream_t; + +extern void shell_init_arch(void); + +#endif // SHELL_ARCH_H diff --git a/sw/airborne/modules/core/shell.c b/sw/airborne/modules/core/shell.c new file mode 100644 index 0000000000..708867deb0 --- /dev/null +++ b/sw/airborne/modules/core/shell.c @@ -0,0 +1,35 @@ +/* + * Copyright (C) Alexandre Bustico + * + * This file is part of paparazzi + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, see + * . + */ + +/** @file "modules/core/shell.c" + * @author Alexandre Bustico + * Simple debug shell + */ + +#include "modules/core/shell.h" +#include "modules/core/shell_arch.h" + +void shell_init(void) +{ + shell_init_arch(); +} + +// add entry function implemented in arch part directly + diff --git a/sw/airborne/modules/core/shell.h b/sw/airborne/modules/core/shell.h new file mode 100644 index 0000000000..253bb8de22 --- /dev/null +++ b/sw/airborne/modules/core/shell.h @@ -0,0 +1,45 @@ +/* + * Copyright (C) Alexandre Bustico + * + * This file is part of paparazzi + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, see + * . + */ + +/** @file "modules/core/shell.h" + * @author Alexandre Bustico + * Simple debug shell + */ + +#ifndef SHELL_H +#define SHELL_H + +#include "modules/core/shell_arch.h" + +/** Init shell + */ +extern void shell_init(void); + +/** Command handler + */ +typedef void (shell_cmd_t)(shell_stream_t *stream, int argc, const char * const argv[]); + +/** Add new shell entry + * + * maximum number of entry might be limited, see specific implementations + */ +extern void shell_add_entry(char *cmd_name, shell_cmd_t *cmd); + +#endif // SHELL_H