From a9ae91d74bb8c3e64c65f18b724f4164224fed13 Mon Sep 17 00:00:00 2001 From: Anol Paisal Date: Sat, 14 Sep 2024 14:24:43 +0700 Subject: [PATCH] Added user-defined CAN filter addres and mask. --- .gitignore | 1 + include/csp/drivers/can_socketcan.h | 8 ++++++-- src/bindings/python/pycsp.c | 2 +- src/csp_yaml.c | 2 +- src/drivers/can/can_socketcan.c | 13 ++++++++----- 5 files changed, 17 insertions(+), 9 deletions(-) diff --git a/.gitignore b/.gitignore index 044dc381f..ae7123f2a 100644 --- a/.gitignore +++ b/.gitignore @@ -27,3 +27,4 @@ build-docs /.cache/ /.history/ /venv/ +/.venv/ diff --git a/include/csp/drivers/can_socketcan.h b/include/csp/drivers/can_socketcan.h index ffd867d2f..9a9d27082 100644 --- a/include/csp/drivers/can_socketcan.h +++ b/include/csp/drivers/can_socketcan.h @@ -24,10 +24,12 @@ extern "C" { * bitrate on the CAN device - this may require increased OS privileges. * @param[in] promisc if true, receive all CAN frames. If false a filter * is set on the CAN device, using device->addr + * @param[in] filter_addr filter CAN address + * @param[in] filter_mask filter CAN mask * @param[out] return_iface the added interface. * @return The added interface, or NULL in case of failure. */ -int csp_can_socketcan_open_and_add_interface(const char * device, const char * ifname, unsigned int node_id, int bitrate, bool promisc, csp_iface_t ** return_iface); +int csp_can_socketcan_open_and_add_interface(const char * device, const char * ifname, unsigned int node_id, int bitrate, bool promisc, uint16_t filter_addr, uint16_t filter_mask, csp_iface_t ** return_iface); /** * Initialize socketcan and add CSP interface. @@ -41,9 +43,11 @@ int csp_can_socketcan_open_and_add_interface(const char * device, const char * i * bitrate on the CAN device - this may require increased OS privileges. * @param[in] promisc if true, receive all CAN frames. If false a filter * is set on the CAN device, using device->addr + * @param[in] filter_addr filter CAN address + * @param[in] filter_mask filter CAN mask * @return The added interface, or NULL in case of failure. */ -csp_iface_t * csp_can_socketcan_init(const char * device, unsigned int node_id, int bitrate, bool promisc); +csp_iface_t * csp_can_socketcan_init(const char * device, unsigned int node_id, int bitrate, bool promisc, uint16_t filter_addr, uint16_t filter_mask); /** * Stop the Rx thread and free resources (testing). diff --git a/src/bindings/python/pycsp.c b/src/bindings/python/pycsp.c index 7803de25e..1f562fd54 100644 --- a/src/bindings/python/pycsp.c +++ b/src/bindings/python/pycsp.c @@ -863,7 +863,7 @@ static PyObject * pycsp_can_socketcan_init(PyObject * self, PyObject * args) { return NULL; } - int res = csp_can_socketcan_open_and_add_interface(ifc, CSP_IF_CAN_DEFAULT_NAME, addr, bitrate, promisc, NULL); + int res = csp_can_socketcan_open_and_add_interface(ifc, CSP_IF_CAN_DEFAULT_NAME, addr, bitrate, promisc, 0xFFFF, 0x0000, NULL); if (res != CSP_ERR_NONE) { return PyErr_Error("csp_can_socketcan_open_and_add_interface()", res); } diff --git a/src/csp_yaml.c b/src/csp_yaml.c index 797eeb418..4364bbb08 100644 --- a/src/csp_yaml.c +++ b/src/csp_yaml.c @@ -142,7 +142,7 @@ static void csp_yaml_end_if(struct data_s * data, unsigned int * dfl_addr) { return; } - int error = csp_can_socketcan_open_and_add_interface(data->device, data->name, addr, 1000000, true, &iface); + int error = csp_can_socketcan_open_and_add_interface(data->device, data->name, addr, 1000000, true, 0xFFFF, 0x0000, &iface); if (error != CSP_ERR_NONE) { csp_print("failed to add CAN interface [%s], error: %d", data->device, error); return; diff --git a/src/drivers/can/can_socketcan.c b/src/drivers/can/can_socketcan.c index ab662a78b..6e69aee74 100644 --- a/src/drivers/can/can_socketcan.c +++ b/src/drivers/can/can_socketcan.c @@ -120,12 +120,15 @@ static int csp_can_tx_frame(void * driver_data, uint32_t id, const uint8_t * dat } -int csp_can_socketcan_set_promisc(const bool promisc, can_context_t * ctx) { +int csp_can_socketcan_set_promisc(const bool promisc, can_context_t * ctx, uint16_t filter_addr, uint16_t filter_mask) { struct can_filter filter = { .can_id = CFP_MAKE_DST(ctx->iface.addr), .can_mask = 0x0000, /* receive anything */ }; + filter.can_id = (csp_conf.version == 1) ? CFP_MAKE_DST(filter_addr) : (unsigned int) (filter_addr << CFP2_DST_OFFSET); + filter.can_mask = (csp_conf.version == 1) ? CFP_MAKE_DST(filter_mask) : (unsigned int) (filter_mask << CFP2_DST_OFFSET); + if (ctx->socket == 0) { return CSP_ERR_INVAL; } @@ -149,7 +152,7 @@ int csp_can_socketcan_set_promisc(const bool promisc, can_context_t * ctx) { } -int csp_can_socketcan_open_and_add_interface(const char * device, const char * ifname, unsigned int node_id, int bitrate, bool promisc, csp_iface_t ** return_iface) { +int csp_can_socketcan_open_and_add_interface(const char * device, const char * ifname, unsigned int node_id, int bitrate, bool promisc, uint16_t filter_addr, uint16_t filter_mask, csp_iface_t ** return_iface) { if (ifname == NULL) { ifname = CSP_IF_CAN_DEFAULT_NAME; } @@ -208,7 +211,7 @@ int csp_can_socketcan_open_and_add_interface(const char * device, const char * i } /* Set filter mode */ - if (csp_can_socketcan_set_promisc(promisc, ctx) != CSP_ERR_NONE) { + if (csp_can_socketcan_set_promisc(promisc, ctx, filter_addr, filter_mask) != CSP_ERR_NONE) { csp_print("%s[%s]: csp_can_socketcan_set_promisc() failed, error: %s\n", __func__, ctx->name, strerror(errno)); return CSP_ERR_INVAL; } @@ -235,9 +238,9 @@ int csp_can_socketcan_open_and_add_interface(const char * device, const char * i return CSP_ERR_NONE; } -csp_iface_t * csp_can_socketcan_init(const char * device, unsigned int node_id, int bitrate, bool promisc) { +csp_iface_t * csp_can_socketcan_init(const char * device, unsigned int node_id, int bitrate, bool promisc, uint16_t filter_addr, uint16_t filter_mask) { csp_iface_t * return_iface; - int res = csp_can_socketcan_open_and_add_interface(device, CSP_IF_CAN_DEFAULT_NAME, node_id, bitrate, promisc, &return_iface); + int res = csp_can_socketcan_open_and_add_interface(device, CSP_IF_CAN_DEFAULT_NAME, node_id, bitrate, promisc, filter_addr , filter_mask, &return_iface); return (res == CSP_ERR_NONE) ? return_iface : NULL; }