2023-11-24 05:43:28 +00:00
# include "cmd_can.h"
2023-11-28 11:40:41 +00:00
# include "can.h"
2023-11-28 12:02:00 +00:00
# include "esp_err.h"
2023-11-28 05:21:04 +00:00
# include "esp_log.h"
2023-11-28 15:21:07 +00:00
# include "hal/twai_types.h"
2023-11-25 08:01:19 +00:00
# include "inttypes.h"
2023-11-29 09:01:20 +00:00
# include "list.h"
2023-11-29 06:30:15 +00:00
# include "sdkconfig.h"
2023-11-24 09:02:17 +00:00
# include "freertos/projdefs.h"
# include "string.h"
# include "esp_console.h"
2023-11-24 10:13:14 +00:00
# include "argtable3/argtable3.h"
2023-11-24 09:02:17 +00:00
# include "xvprintf.h"
2023-11-24 15:11:06 +00:00
# include <stddef.h>
# include <stdio.h>
# include <ctype.h>
2023-11-24 13:22:22 +00:00
2023-11-28 15:59:40 +00:00
twai_filter_config_t my_filters = TWAI_FILTER_CONFIG_ACCEPT_ALL ( ) ;
2023-11-28 12:26:25 +00:00
static void register_cansend ( void ) ;
2023-11-25 07:25:12 +00:00
static void register_canup ( void ) ;
static void register_candown ( void ) ;
static void register_canstats ( void ) ;
static void register_canstart ( void ) ;
static void register_canrecover ( void ) ;
2023-11-29 06:30:15 +00:00
static void register_canfilter ( void ) ;
static void register_cansmartfilter ( void ) ;
2023-11-24 05:43:28 +00:00
void register_can_commands ( void ) {
2023-11-28 12:26:25 +00:00
register_cansend ( ) ;
2023-11-26 05:34:12 +00:00
register_canup ( ) ;
register_candown ( ) ;
register_canstats ( ) ;
register_canstart ( ) ;
register_canrecover ( ) ;
2023-11-29 06:30:15 +00:00
register_canfilter ( ) ;
register_cansmartfilter ( ) ;
2023-11-24 05:43:28 +00:00
}
2023-11-24 09:02:17 +00:00
2023-11-24 10:13:14 +00:00
static struct {
struct arg_str * message ;
struct arg_end * end ;
} cansend_args ;
2023-11-24 09:02:17 +00:00
static int send_can_frame ( int argc , char * * argv ) {
2023-11-29 08:27:23 +00:00
twai_message_t msg = { 0 } ;
2023-11-28 10:37:17 +00:00
char printf_str [ 70 ] ;
2023-11-24 10:13:14 +00:00
int nerrors = arg_parse ( argc , argv , ( void * * ) & cansend_args ) ;
if ( nerrors ! = 0 ) {
arg_print_errors ( stderr , cansend_args . end , argv [ 0 ] ) ;
return 1 ;
}
2023-11-24 13:22:22 +00:00
const char * can_msg_ptr = cansend_args . message - > sval [ 0 ] ;
2023-11-24 15:11:06 +00:00
char * can_msg_str_buf = strdup ( can_msg_ptr ) ;
char * id_substr = strtok ( can_msg_str_buf , " # " ) ;
char * data_substr = strtok ( NULL , " # " ) ;
if ( ( id_substr = = NULL ) | | ( strtok ( NULL , " # " ) ! = NULL ) ) goto invalid_args ;
int id_l = strlen ( id_substr ) ;
int dt_l = data_substr = = NULL ? 0 : strlen ( data_substr ) ;
2023-11-29 08:27:23 +00:00
if ( ( id_l > 8 ) | | ( dt_l > 16 ) | | ( dt_l % 2 ) ) goto invalid_args ;
2023-11-24 15:11:06 +00:00
for ( int i = 0 ; i < id_l ; i + + ) if ( ! isxdigit ( ( int ) id_substr [ i ] ) ) goto invalid_args ;
for ( int i = 0 ; i < dt_l ; i + + ) if ( ! isxdigit ( ( int ) data_substr [ i ] ) ) goto invalid_args ;
int msg_id ;
if ( sscanf ( id_substr , " %X " , & msg_id ) < 1 ) goto invalid_args ;
for ( int i = 0 ; i < ( dt_l / 2 ) ; i + + ) {
char * byte_to_parse = malloc ( 3 ) ;
strncpy ( byte_to_parse , data_substr + i * 2 , 2 ) ;
int num ;
int res = sscanf ( byte_to_parse , " %X " , & num ) ;
free ( byte_to_parse ) ;
if ( res < 1 ) goto invalid_args ;
msg . data [ i ] = num ;
}
msg . data_length_code = dt_l / 2 ;
msg . identifier = msg_id ;
2023-11-29 08:27:23 +00:00
msg . extd = ( id_l > 3 ) ;
2023-11-28 11:45:51 +00:00
esp_err_t res = twai_transmit ( & msg , pdMS_TO_TICKS ( 1000 ) ) ;
switch ( res ) {
case ESP_OK :
can_msg_to_str ( & msg , " sent " , printf_str ) ;
print_w_clr_time ( printf_str , NULL , true ) ;
break ;
case ESP_ERR_TIMEOUT :
2023-11-28 15:21:07 +00:00
print_w_clr_time ( " Timeout! " , LOG_COLOR_RED , true ) ;
2023-11-28 11:45:51 +00:00
break ;
case ESP_ERR_NOT_SUPPORTED :
2023-11-28 15:21:07 +00:00
print_w_clr_time ( " Can't sent in Listen-Only mode! " , LOG_COLOR_RED , true ) ;
2023-11-28 11:45:51 +00:00
break ;
default :
2023-11-28 15:21:07 +00:00
print_w_clr_time ( " Invalid state! " , LOG_COLOR_RED , true ) ;
2023-11-28 11:45:51 +00:00
break ;
}
2023-11-28 10:37:17 +00:00
free ( can_msg_str_buf ) ;
2023-11-24 09:02:17 +00:00
return 0 ;
2023-11-24 15:11:06 +00:00
invalid_args :
2023-11-29 08:27:23 +00:00
print_w_clr_time ( " Invalid arguments! " , LOG_COLOR_RED , true ) ;
2023-11-24 15:11:06 +00:00
free ( can_msg_str_buf ) ;
return 1 ;
2023-11-24 09:02:17 +00:00
}
2023-11-25 07:25:12 +00:00
static int canrecover ( int argc , char * * argv ) {
2023-11-28 11:40:41 +00:00
esp_err_t res = twai_initiate_recovery ( ) ;
2023-11-28 15:21:07 +00:00
if ( res = = ESP_OK ) print_w_clr_time ( " Started CAN recovery. " , LOG_COLOR_GREEN , true ) ;
else if ( curr_can_state . state = = CAN_NOT_INSTALLED ) print_w_clr_time ( " CAN driver is not installed! " , LOG_COLOR_RED , true ) ;
else print_w_clr_time ( " Can't start recovery - not in bus-off state! " , LOG_COLOR_RED , true ) ;
2023-11-25 07:25:12 +00:00
return 0 ;
}
static const char * can_states_str [ ] = {
" not installed " ,
" stopped " ,
" error active " ,
" error passive " ,
" bus off " ,
" recovering "
} ;
static int canstats ( int argc , char * * argv ) {
2023-11-26 10:55:32 +00:00
if ( curr_can_state . state = = CAN_NOT_INSTALLED ) {
2023-11-29 08:27:23 +00:00
print_w_clr_time ( " CAN driver is not installed! " , LOG_COLOR_RED , true ) ;
2023-11-25 07:25:12 +00:00
return 0 ;
} else {
2023-11-26 10:55:32 +00:00
const char * state_str = can_states_str [ curr_can_state . state ] ;
2023-11-25 07:25:12 +00:00
printf ( " status: %s \n " , state_str ) ;
2023-11-26 10:55:32 +00:00
printf ( " TX Err Counter: % " PRIu32 " \n " , curr_can_state . tx_error_counter ) ;
printf ( " RX Err Counter: % " PRIu32 " \n " , curr_can_state . rx_error_counter ) ;
printf ( " Failed transmit: % " PRIu32 " \n " , curr_can_state . tx_failed_count ) ;
printf ( " Arbitration lost times: % " PRIu32 " \n " , curr_can_state . arb_lost_count ) ;
printf ( " Bus-off count: % " PRIu32 " \n " , curr_can_state . bus_error_count ) ;
2023-11-25 07:25:12 +00:00
}
return 0 ;
}
2023-11-28 15:21:07 +00:00
static const char * can_modes [ ] = {
" normal " ,
" no_ack " ,
" listen_only " ,
} ;
2023-11-28 15:59:40 +00:00
static struct {
struct arg_int * speed ;
struct arg_lit * filters ;
struct arg_lit * autorecover ;
struct arg_str * mode ;
struct arg_end * end ;
} canup_args ;
2023-11-25 07:25:12 +00:00
static int canup ( int argc , char * * argv ) {
2023-11-26 05:34:12 +00:00
esp_err_t res ;
2023-11-28 15:21:07 +00:00
static twai_timing_config_t t_config ;
twai_general_config_t gen_cfg = default_g_config ;
// TODO: add CAN filtering
2023-11-28 15:59:40 +00:00
twai_filter_config_t f_config ;
2023-11-29 06:30:15 +00:00
int nerrors = arg_parse ( argc , argv , ( void * * ) & canup_args ) ;
if ( nerrors ! = 0 ) {
arg_print_errors ( stderr , canup_args . end , argv [ 0 ] ) ;
return 1 ;
}
2023-11-28 15:59:40 +00:00
if ( canup_args . filters - > count ) {
f_config = my_filters ;
2023-11-29 08:27:23 +00:00
printf ( " Using %s filters. \n " , adv_filters . enabled ? " smart " : " basic hw " ) ;
2023-11-28 15:59:40 +00:00
} else {
f_config = ( twai_filter_config_t ) TWAI_FILTER_CONFIG_ACCEPT_ALL ( ) ;
2023-11-29 08:27:23 +00:00
printf ( " Using accept all filters. \n " ) ;
2023-11-28 15:59:40 +00:00
}
2023-11-28 05:21:04 +00:00
esp_log_level_t prev_gpio_lvl = esp_log_level_get ( " gpio " ) ;
2023-11-28 15:21:07 +00:00
int mode = 0 ;
if ( canup_args . mode - > count ) {
const char * mode_str = canup_args . mode - > sval [ 0 ] ;
while ( mode < 4 ) {
if ( mode = = 3 ) {
print_w_clr_time ( " Unsupported mode! " , LOG_COLOR_RED , true ) ;
return 1 ;
}
if ( memcmp ( mode_str , can_modes [ mode ] , strlen ( mode_str ) ) = = 0 ) break ;
mode + + ;
}
}
switch ( mode ) {
case 1 :
gen_cfg . mode = TWAI_MODE_NO_ACK ;
print_w_clr_time ( " Starting CAN in No Ack Mode... " , LOG_COLOR_BLUE , true ) ;
break ;
case 2 :
gen_cfg . mode = TWAI_MODE_LISTEN_ONLY ;
print_w_clr_time ( " Starting CAN in Listen Only Mode... " , LOG_COLOR_BLUE , true ) ;
break ;
default : //0
print_w_clr_time ( " Starting CAN in Normal Mode... " , LOG_COLOR_BLUE , true ) ;
break ;
}
switch ( canup_args . speed - > ival [ 0 ] ) {
case 1000 :
t_config = ( twai_timing_config_t ) TWAI_TIMING_CONFIG_1KBITS ( ) ;
break ;
case 5000 :
t_config = ( twai_timing_config_t ) TWAI_TIMING_CONFIG_5KBITS ( ) ;
break ;
case 10000 :
t_config = ( twai_timing_config_t ) TWAI_TIMING_CONFIG_10KBITS ( ) ;
break ;
case 12500 :
t_config = ( twai_timing_config_t ) TWAI_TIMING_CONFIG_12_5KBITS ( ) ;
break ;
case 16000 :
t_config = ( twai_timing_config_t ) TWAI_TIMING_CONFIG_16KBITS ( ) ;
break ;
case 20000 :
t_config = ( twai_timing_config_t ) TWAI_TIMING_CONFIG_20KBITS ( ) ;
break ;
case 25000 :
t_config = ( twai_timing_config_t ) TWAI_TIMING_CONFIG_25KBITS ( ) ;
break ;
case 50000 :
t_config = ( twai_timing_config_t ) TWAI_TIMING_CONFIG_50KBITS ( ) ;
break ;
case 100000 :
t_config = ( twai_timing_config_t ) TWAI_TIMING_CONFIG_100KBITS ( ) ;
break ;
case 125000 :
t_config = ( twai_timing_config_t ) TWAI_TIMING_CONFIG_125KBITS ( ) ;
break ;
case 250000 :
t_config = ( twai_timing_config_t ) TWAI_TIMING_CONFIG_250KBITS ( ) ;
break ;
case 500000 :
t_config = ( twai_timing_config_t ) TWAI_TIMING_CONFIG_500KBITS ( ) ;
break ;
case 800000 :
t_config = ( twai_timing_config_t ) TWAI_TIMING_CONFIG_800KBITS ( ) ;
break ;
case 1000000 :
t_config = ( twai_timing_config_t ) TWAI_TIMING_CONFIG_1MBITS ( ) ;
break ;
default :
print_w_clr_time ( " Unsupported speed! " , LOG_COLOR_RED , true ) ;
return 1 ;
}
2023-11-26 05:34:12 +00:00
xSemaphoreTake ( can_mutex , portMAX_DELAY ) ;
2023-11-28 15:21:07 +00:00
esp_log_level_set ( " gpio " , ESP_LOG_ERROR ) ;
res = twai_driver_install ( & gen_cfg , & t_config , & f_config ) ;
if ( res = = ESP_OK ) {
print_w_clr_time ( " CAN driver installed " , LOG_COLOR_BLUE , true ) ;
if ( canup_args . autorecover - > count ) {
print_w_clr_time ( " Auto recovery is enabled! " , LOG_COLOR_PURPLE , true ) ;
auto_recovery = true ;
} else auto_recovery = false ;
} else if ( res = = ESP_ERR_INVALID_STATE ) {
print_w_clr_time ( " Driver is already installed! " , LOG_COLOR_BROWN , true ) ;
goto free_exit ;
} else {
print_w_clr_time ( " Couldn't install CAN driver! Rebooting... " , LOG_COLOR_RED , true ) ;
2023-11-25 07:25:12 +00:00
esp_restart ( ) ;
}
2023-11-28 15:21:07 +00:00
ESP_ERROR_CHECK ( twai_start ( ) ) ;
2023-11-28 15:26:16 +00:00
is_error_passive = false ;
2023-11-28 15:21:07 +00:00
print_w_clr_time ( " CAN driver started " , LOG_COLOR_BLUE , true ) ;
free_exit :
2023-11-26 05:34:12 +00:00
xSemaphoreGive ( can_mutex ) ;
2023-11-28 05:21:04 +00:00
esp_log_level_set ( " gpio " , prev_gpio_lvl ) ;
2023-11-25 07:25:12 +00:00
return 0 ;
}
static int canstart ( int argc , char * * argv ) {
2023-11-26 05:34:12 +00:00
xSemaphoreTake ( can_mutex , portMAX_DELAY ) ;
2023-11-28 12:05:24 +00:00
esp_err_t res = twai_start ( ) ;
2023-11-28 15:26:16 +00:00
if ( res = = ESP_OK ) {
print_w_clr_time ( " CAN driver started " , LOG_COLOR_GREEN , true ) ;
is_error_passive = false ;
} else print_w_clr_time ( " Driver is not in stopped state, or is not installed. " , LOG_COLOR_RED , true ) ;
2023-11-26 05:34:12 +00:00
xSemaphoreGive ( can_mutex ) ;
2023-11-25 07:25:12 +00:00
return 0 ;
}
static int candown ( int argc , char * * argv ) {
2023-11-26 05:34:12 +00:00
xSemaphoreTake ( can_mutex , portMAX_DELAY ) ;
2023-11-28 12:02:00 +00:00
if ( curr_can_state . state ! = CAN_BUF_OFF ) {
esp_err_t res = twai_stop ( ) ;
2023-11-28 15:21:07 +00:00
if ( res = = ESP_OK ) print_w_clr_time ( " CAN was stopped. " , LOG_COLOR_GREEN , true ) ;
2023-11-28 12:02:00 +00:00
else {
2023-11-28 15:21:07 +00:00
print_w_clr_time ( " Driver is not in running state, or is not installed. " , LOG_COLOR_RED , true ) ;
2023-11-28 12:02:00 +00:00
xSemaphoreGive ( can_mutex ) ;
return 1 ;
}
}
2023-11-25 07:25:12 +00:00
ESP_ERROR_CHECK ( twai_driver_uninstall ( ) ) ;
2023-11-26 05:34:12 +00:00
xSemaphoreGive ( can_mutex ) ;
2023-11-25 07:25:12 +00:00
return 0 ;
}
2023-11-28 12:26:25 +00:00
static void register_cansend ( void ) {
2023-11-24 10:13:14 +00:00
cansend_args . message = arg_str1 ( NULL , NULL , " ID#data " , " Message to send, ID and data bytes, all in hex. # is the delimiter. " ) ;
2023-11-28 14:09:17 +00:00
cansend_args . end = arg_end ( 2 ) ;
2023-11-24 10:13:14 +00:00
2023-11-24 09:02:17 +00:00
const esp_console_cmd_t cmd = {
. command = " cansend " ,
2023-11-24 10:13:14 +00:00
. help = " Send a can message to the bus, example: cansend 00008C03#02 " ,
2023-11-24 09:02:17 +00:00
. hint = NULL ,
. func = & send_can_frame ,
2023-11-24 10:13:14 +00:00
. argtable = & cansend_args ,
2023-11-24 09:02:17 +00:00
} ;
ESP_ERROR_CHECK ( esp_console_cmd_register ( & cmd ) ) ;
}
2023-11-25 07:25:12 +00:00
static void register_canup ( void ) {
2023-11-28 12:26:25 +00:00
2023-11-28 14:09:17 +00:00
canup_args . speed = arg_int1 ( NULL , NULL , " <speed> " , " CAN bus speed, in bps. See help for supported speeds. " ) ;
2023-11-28 12:49:01 +00:00
canup_args . mode = arg_str0 ( " m " , " mode " , " <normal|no_ack|listen_only> " , " Set CAN mode. Normal (default), No Ack (for self-testing) or Listen Only (to prevent transmitting, for monitoring). " ) ;
2023-11-28 15:59:40 +00:00
canup_args . filters = arg_lit0 ( " f " , NULL , " Use predefined CAN filters. " ) ;
2023-11-28 14:09:17 +00:00
canup_args . autorecover = arg_lit0 ( " r " , " auto-recovery " , " Set to enable auto-recovery of CAN bus if case of bus-off event " ) ;
2023-11-28 12:26:25 +00:00
canup_args . end = arg_end ( 4 ) ;
2023-11-25 07:25:12 +00:00
const esp_console_cmd_t cmd = {
. command = " canup " ,
2023-11-28 12:02:00 +00:00
. help = " Install can drivers and start can interface. Used right after board start or during runtime for changing CAN configuration. Supported speeds: 1mbits, 800kbits, 500kbits, 250kbits, 125kbits, 100kbits, 50kbits, 25kbits, 20kbits, 16kbits, 12.5kbits, 10kbits, 5kbits, 1kbits. " ,
2023-11-25 07:25:12 +00:00
. hint = NULL ,
. func = & canup ,
. argtable = & canup_args ,
} ;
ESP_ERROR_CHECK ( esp_console_cmd_register ( & cmd ) ) ;
}
static void register_candown ( void ) {
const esp_console_cmd_t cmd = {
. command = " candown " ,
. help = " Stop CAN interface and uninstall CAN driver, for example, to install and start with different parameters/filters. " ,
. hint = NULL ,
. func = & candown ,
} ;
ESP_ERROR_CHECK ( esp_console_cmd_register ( & cmd ) ) ;
}
static void register_canstats ( void ) {
const esp_console_cmd_t cmd = {
. command = " canstats " ,
. help = " Print CAN statistics. " ,
. hint = NULL ,
. func = & canstats ,
} ;
ESP_ERROR_CHECK ( esp_console_cmd_register ( & cmd ) ) ;
}
static void register_canstart ( void ) {
const esp_console_cmd_t cmd = {
. command = " canstart " ,
2023-11-28 12:02:00 +00:00
. help = " Start CAN interface, used after bus recovery, otherwise see canup command. " ,
2023-11-25 07:25:12 +00:00
. hint = NULL ,
. func = & canstart ,
} ;
ESP_ERROR_CHECK ( esp_console_cmd_register ( & cmd ) ) ;
}
static void register_canrecover ( void ) {
const esp_console_cmd_t cmd = {
. command = " canrecover " ,
. help = " Recover CAN after buf-off. Used when auto-recovery is turned off. " ,
. hint = NULL ,
. func = & canrecover ,
} ;
ESP_ERROR_CHECK ( esp_console_cmd_register ( & cmd ) ) ;
}
2023-11-29 06:30:15 +00:00
static struct {
2023-11-29 08:27:23 +00:00
struct arg_lit * dual_arg ;
struct arg_str * code_arg ;
struct arg_str * mask_arg ;
2023-11-29 06:30:15 +00:00
struct arg_end * end ;
} canfilter_args ;
static int canfilter ( int argc , char * * argv ) {
int nerrors = arg_parse ( argc , argv , ( void * * ) & canfilter_args ) ;
if ( nerrors ! = 0 ) {
arg_print_errors ( stderr , canfilter_args . end , argv [ 0 ] ) ;
return 1 ;
}
2023-11-29 08:27:23 +00:00
const char * mask_s = canfilter_args . mask_arg - > sval [ 0 ] ;
const char * code_s = canfilter_args . code_arg - > sval [ 0 ] ;
int m_l = strlen ( mask_s ) ;
int c_l = strlen ( code_s ) ;
if ( m_l ! = 8 | | c_l ! = 8 ) goto invalid_args ;
for ( int i = 0 ; i < m_l ; i + + ) if ( ! isxdigit ( ( int ) mask_s [ i ] ) ) goto invalid_args ;
for ( int i = 0 ; i < c_l ; i + + ) if ( ! isxdigit ( ( int ) code_s [ i ] ) ) goto invalid_args ;
uint32_t mask = 0 ;
uint32_t code = 0 ;
if ( sscanf ( mask_s , " % " PRIX32 , & mask ) < 1 ) goto invalid_args ;
if ( sscanf ( code_s , " % " PRIX32 , & code ) < 1 ) goto invalid_args ;
if ( canfilter_args . dual_arg - > count ) {
2023-11-29 06:30:15 +00:00
my_filters . single_filter = false ;
2023-11-29 08:27:23 +00:00
print_w_clr_time ( " Setting hw filters in dual mode. " , LOG_COLOR_GREEN , true ) ;
2023-11-29 06:30:15 +00:00
} else {
my_filters . single_filter = true ;
2023-11-29 08:27:23 +00:00
print_w_clr_time ( " Setting hw filters in single mode. " , LOG_COLOR_GREEN , true ) ;
2023-11-29 06:30:15 +00:00
}
2023-11-29 08:27:23 +00:00
printf ( " mask: % " PRIX32 " , code: % " PRIX32 " \n " , mask , code ) ;
my_filters . acceptance_code = code ;
my_filters . acceptance_mask = mask ;
adv_filters . enabled = false ;
adv_filters . sw_filtering = false ;
2023-11-29 06:30:15 +00:00
return 0 ;
2023-11-29 08:27:23 +00:00
invalid_args :
print_w_clr_time ( " Invalid arguments! " , LOG_COLOR_RED , true ) ;
return 1 ;
2023-11-29 06:30:15 +00:00
}
static void register_canfilter ( void ) {
2023-11-29 08:27:23 +00:00
canfilter_args . mask_arg = arg_str1 ( " m " , " mask " , " <mask> " , " Acceptance mask (as in esp-idf docs), uint32_t in hex form, 8 symbols. " ) ;
canfilter_args . code_arg = arg_str1 ( " c " , " code " , " <code> " , " Acceptance code (as in esp-idf docs), uint32_t in hex form, 8 symbols. " ) ;
canfilter_args . dual_arg = arg_lit0 ( " d " , NULL , " Use Dual Filter Mode. " ) ;
2023-11-29 06:30:15 +00:00
const esp_console_cmd_t cmd = {
. command = " canfilter " ,
. help = " Manually setup basic hardware filtering. " ,
. hint = NULL ,
. func = & canfilter ,
. argtable = & canfilter_args ,
} ;
ESP_ERROR_CHECK ( esp_console_cmd_register ( & cmd ) ) ;
}
static struct {
struct arg_str * filters ;
struct arg_end * end ;
} cansmart_args ;
2023-11-29 11:18:54 +00:00
void smartfilters_destroy ( List * * head ) {
while ( * head ! = NULL ) {
List * tmp_cursor = * head ;
* head = ( * head ) - > next ;
free ( ( smart_filt_element_t * ) tmp_cursor - > data ) ;
free ( tmp_cursor ) ;
}
}
2023-11-29 06:30:15 +00:00
static int cansmartfilter ( int argc , char * * argv ) {
2023-11-29 11:18:54 +00:00
char * filter_str_buf = NULL ;
smart_filt_element_t * filt_element = NULL ;
2023-11-29 06:30:15 +00:00
int nerrors = arg_parse ( argc , argv , ( void * * ) & cansmart_args ) ;
if ( nerrors ! = 0 ) {
arg_print_errors ( stderr , cansmart_args . end , argv [ 0 ] ) ;
return 1 ;
}
2023-11-29 11:18:54 +00:00
smartfilters_destroy ( & adv_filters . filters ) ;
adv_filters . sw_filtering = false ;
adv_filters . enabled = false ;
bool tmp_sw = false ;
uint32_t hwfilt_code = 0 ;
uint32_t hwfilt_mask = 0 ;
2023-11-29 09:01:20 +00:00
for ( int i = 0 ; i < cansmart_args . filters - > count ; i + + ) {
2023-11-29 11:18:54 +00:00
filt_element = malloc ( sizeof ( smart_filt_element_t ) ) ;
const char * filter_str_ptr = cansmart_args . filters - > sval [ i ] ;
filter_str_buf = strdup ( filter_str_ptr ) ;
char * code_substr = strtok ( filter_str_buf , " # " ) ;
char * mask_substr = strtok ( NULL , " # " ) ;
if ( code_substr = = NULL | | mask_substr = = NULL | | strtok ( NULL , " # " ) ! = NULL ) goto invalid_args ;
int m_l = strlen ( mask_substr ) ;
int c_l = strlen ( code_substr ) ;
if ( m_l > 8 | | c_l > 8 ) goto invalid_args ;
for ( int i = 0 ; i < m_l ; i + + ) if ( ! isxdigit ( ( int ) mask_substr [ i ] ) ) goto invalid_args ;
for ( int i = 0 ; i < c_l ; i + + ) if ( ! isxdigit ( ( int ) code_substr [ i ] ) ) goto invalid_args ;
if ( sscanf ( code_substr , " % " PRIX32 , & filt_element - > filt ) < 1 ) goto invalid_args ;
if ( sscanf ( mask_substr , " % " PRIX32 , & filt_element - > mask ) < 1 ) goto invalid_args ;
list_push ( & adv_filters . filters , ( void * ) filt_element ) ;
if ( i = = 0 ) {
hwfilt_mask = filt_element - > mask ;
hwfilt_code = filt_element - > filt ;
} else {
uint32_t common_bits = filt_element - > mask & hwfilt_mask ;
uint32_t new_bits = filt_element - > mask - common_bits ;
uint32_t missing_bits = hwfilt_mask - common_bits ;
hwfilt_mask & = filt_element - > mask ;
uint32_t bit_to_delete = ( hwfilt_code ^ filt_element - > filt ) & hwfilt_mask ;
hwfilt_mask - = bit_to_delete ;
if ( new_bits | | missing_bits | | bit_to_delete ) tmp_sw = true ;
}
filt_element = NULL ;
2023-11-29 09:01:20 +00:00
}
2023-11-29 11:18:54 +00:00
my_filters . single_filter = true ;
my_filters . acceptance_mask = ~ ( hwfilt_mask < < 3 ) ;
my_filters . acceptance_code = hwfilt_code < < 3 ;
adv_filters . sw_filtering = tmp_sw ;
adv_filters . enabled = true ;
print_w_clr_time ( " Smart filters were set. " , LOG_COLOR_GREEN , true ) ;
printf ( " Num of smart filters: %d \n " , list_sizeof ( adv_filters . filters ) ) ;
2023-11-29 06:30:15 +00:00
return 0 ;
2023-11-29 11:18:54 +00:00
invalid_args :
free ( filter_str_buf ) ;
free ( filt_element ) ;
print_w_clr_time ( " Invalid arguments! " , LOG_COLOR_RED , true ) ;
smartfilters_destroy ( & adv_filters . filters ) ;
return 1 ;
2023-11-29 06:30:15 +00:00
}
static void register_cansmartfilter ( void ) {
cansmart_args . filters = arg_strn ( NULL , NULL , " <filter1> <filter2> ... " , 1 , CONFIG_CAN_MAX_SMARTFILTERS_NUM , " Filters, in hex format. Each one contains mask and code in format code#mask. Both mask and code are uint32_t numbers in hex format. Example: 0000FF00#0000FFFF " ) ;
const esp_console_cmd_t cmd = {
. command = " cansmartfilter " ,
2023-11-29 11:18:54 +00:00
. help = " Setup smart mixed filters (hardware + software). Num of filters can be up to the value in config. Supportd only ID filtering of extended frames, standart frames aren't supported for now. " ,
2023-11-29 06:30:15 +00:00
. hint = NULL ,
. func = & cansmartfilter ,
. argtable = & cansmart_args ,
} ;
ESP_ERROR_CHECK ( esp_console_cmd_register ( & cmd ) ) ;
}