From c0f4d14890fa3a2f70fe3e8e4af6373bb8c412d0 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Martin=20J=C3=A4ger?= Date: Sat, 2 Jan 2021 16:19:30 +0100 Subject: [PATCH 1/4] canbus: isotp: fix asserts to check rx and tx addr MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Previous implementation contained a typo so that only the rx address was checked. Signed-off-by: Martin Jäger --- subsys/canbus/isotp/isotp.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/subsys/canbus/isotp/isotp.c b/subsys/canbus/isotp/isotp.c index 05f4e084bffc9..a72de83f6a733 100644 --- a/subsys/canbus/isotp/isotp.c +++ b/subsys/canbus/isotp/isotp.c @@ -557,7 +557,7 @@ int isotp_bind(struct isotp_recv_ctx *ctx, const struct device *can_dev, __ASSERT(ctx, "ctx is NULL"); __ASSERT(can_dev, "CAN device is NULL"); - __ASSERT(rx_addr && rx_addr, "RX or TX addr is NULL"); + __ASSERT(rx_addr && tx_addr, "RX or TX addr is NULL"); __ASSERT(opts, "OPTS is NULL"); ctx->can_dev = can_dev; @@ -1106,7 +1106,7 @@ static int send(struct isotp_send_ctx *ctx, const struct device *can_dev, __ASSERT_NO_MSG(ctx); __ASSERT_NO_MSG(can_dev); - __ASSERT_NO_MSG(rx_addr && rx_addr); + __ASSERT_NO_MSG(rx_addr && tx_addr); if (complete_cb) { ctx->fin_cb.cb = complete_cb; From 4ff2a9d99a3fd2d68f01a6b35149bf264b614635 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Martin=20J=C3=A4ger?= Date: Sat, 2 Jan 2021 16:02:38 +0100 Subject: [PATCH 2/4] canbus: isotp: allow frames with padding MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit ISO 15765-2 specifies that the CAN frame length for single frames and the last consecutive frame may or may not be optimized to the actual length of the PDU payload. The ISO-TP implementation should only consider the information from the N_PCI section. The previous implementation did not allow padding. With this commit, padding is allowed by default and can even be required to be more compliant to AUTOSAR standards. Signed-off-by: Martin Jäger --- subsys/canbus/isotp/Kconfig | 16 ++++ subsys/canbus/isotp/isotp.c | 79 +++++++++++++++---- .../canbus/isotp/conformance/src/main.c | 43 +++++++--- 3 files changed, 114 insertions(+), 24 deletions(-) diff --git a/subsys/canbus/isotp/Kconfig b/subsys/canbus/isotp/Kconfig index 4ca818354f112..97dc596dc1d55 100644 --- a/subsys/canbus/isotp/Kconfig +++ b/subsys/canbus/isotp/Kconfig @@ -47,6 +47,22 @@ config ISOTP_CR_TIMEOUT Cr (receiver consecutive frame) timeout. ISO 15765-2: 1000ms +config ISOTP_REQUIRE_RX_PADDING + bool "Require padding for received messages" + help + If enabled, SFs and the last CF must always have a DLC of 8 bytes + (for classic CAN) and unused bytes must be padded by the sending + device. This setting allows to be compliant to AUTOSAR Specification + of CAN Transport Layer. + + By default, received CAN frames with or without padding are accepted. + +config ISOTP_ENABLE_TX_PADDING + bool "Enable padding for transmitted messages" + help + Add padding bytes 0xCC (as recommended by Bosch) if the PDU payload + does not fit exactly into the CAN frame. + config ISOTP_WORKQUEUE_PRIO int "Priority level of the RX and TX work queue" default 2 diff --git a/subsys/canbus/isotp/isotp.c b/subsys/canbus/isotp/isotp.c index a72de83f6a733..f5e8d5f0c3d44 100644 --- a/subsys/canbus/isotp/isotp.c +++ b/subsys/canbus/isotp/isotp.c @@ -136,6 +136,7 @@ static void receive_send_fc(struct isotp_recv_ctx *ctx, uint8_t fs) .id = ctx->tx_addr.ext_id }; uint8_t *data = frame.data; + uint8_t payload_len; int ret; __ASSERT_NO_MSG(!(fs & ISOTP_PCI_TYPE_MASK)); @@ -147,7 +148,16 @@ static void receive_send_fc(struct isotp_recv_ctx *ctx, uint8_t fs) *data++ = ISOTP_PCI_TYPE_FC | fs; *data++ = ctx->opts.bs; *data++ = ctx->opts.stmin; - frame.dlc = data - frame.data; + payload_len = data - frame.data; + +#if defined(CONFIG_ISOTP_REQUIRE_RX_PADDING) || \ + defined(CONFIG_ISOTP_ENABLE_TX_PADDING) + /* AUTOSAR requirement SWS_CanTp_00347 */ + memset(&frame.data[payload_len], 0xCC, ISOTP_CAN_DL - payload_len); + frame.dlc = ISOTP_CAN_DL; +#else + frame.dlc = payload_len; +#endif ret = can_send(ctx->can_dev, &frame, K_MSEC(ISOTP_A), receive_can_tx_isr, ctx); @@ -264,10 +274,10 @@ static void receive_state_machine(struct isotp_recv_ctx *ctx) switch (ctx->state) { case ISOTP_RX_STATE_PROCESS_SF: - ctx->buf->len = receive_get_sf_length(ctx->buf); + ctx->length = receive_get_sf_length(ctx->buf); ud_rem_len = net_buf_user_data(ctx->buf); *ud_rem_len = 0; - LOG_DBG("SM process SF of length %d", ctx->buf->len); + LOG_DBG("SM process SF of length %d", ctx->length); net_buf_put(&ctx->fifo, ctx->buf); ctx->state = ISOTP_RX_STATE_RECYCLE; receive_state_machine(ctx); @@ -380,6 +390,7 @@ static void receive_work_handler(struct k_work *item) static void process_ff_sf(struct isotp_recv_ctx *ctx, struct zcan_frame *frame) { int index = 0; + uint8_t payload_len; if (ctx->rx_addr.use_ext_addr) { if (frame->data[index++] != ctx->rx_addr.ext_addr) { @@ -391,19 +402,30 @@ static void process_ff_sf(struct isotp_recv_ctx *ctx, struct zcan_frame *frame) case ISOTP_PCI_TYPE_FF: LOG_DBG("Got FF IRQ"); if (frame->dlc != ISOTP_CAN_DL) { - LOG_INF("FF DL does not match. Ignore"); + LOG_INF("FF DLC invalid. Ignore"); return; } + payload_len = ISOTP_CAN_DL; ctx->state = ISOTP_RX_STATE_PROCESS_FF; ctx->sn_expected = 1; break; case ISOTP_PCI_TYPE_SF: LOG_DBG("Got SF IRQ"); - if ((frame->data[index] & ISOTP_PCI_FF_DL_UPPER_MASK) + - index + 1 != frame->dlc) { - LOG_INF("SF DL does not match. Ignore"); +#ifdef CONFIG_ISOTP_REQUIRE_RX_PADDING + /* AUTOSAR requirement SWS_CanTp_00345 */ + if (frame->dlc != ISOTP_CAN_DL) { + LOG_INF("SF DLC invalid. Ignore"); + return; + } +#endif + + payload_len = index + 1 + (frame->data[index] & + ISOTP_PCI_SF_DL_MASK); + + if (payload_len > frame->dlc) { + LOG_INF("SF DL does not fit. Ignore"); return; } @@ -415,7 +437,7 @@ static void process_ff_sf(struct isotp_recv_ctx *ctx, struct zcan_frame *frame) return; } - net_buf_add_mem(ctx->buf, &frame->data[index], frame->dlc - index); + net_buf_add_mem(ctx->buf, &frame->data[index], payload_len - index); } static inline void receive_add_mem(struct isotp_recv_ctx *ctx, uint8_t *data, @@ -432,7 +454,7 @@ static inline void receive_add_mem(struct isotp_recv_ctx *ctx, uint8_t *data, net_buf_add_mem(ctx->act_frag, data, tailroom); ctx->act_frag = ctx->act_frag->frags; if (!ctx->act_frag) { - LOG_ERR("No fragmet left to append data"); + LOG_ERR("No fragment left to append data"); receive_report_error(ctx, ISOTP_N_BUFFER_OVERFLW); return; } @@ -444,6 +466,7 @@ static void process_cf(struct isotp_recv_ctx *ctx, struct zcan_frame *frame) { uint32_t *ud_rem_len = (uint32_t *)net_buf_user_data(ctx->buf); int index = 0; + uint32_t data_len; if (ctx->rx_addr.use_ext_addr) { if (frame->data[index++] != ctx->rx_addr.ext_addr) { @@ -470,14 +493,20 @@ static void process_cf(struct isotp_recv_ctx *ctx, struct zcan_frame *frame) return; } - if (frame->dlc - index > ctx->length) { - LOG_ERR("The frame contains more bytes than expected"); +#ifdef CONFIG_ISOTP_REQUIRE_RX_PADDING + /* AUTOSAR requirement SWS_CanTp_00346 */ + if (frame->dlc != ISOTP_CAN_DL) { + LOG_ERR("CF DL invalid"); receive_report_error(ctx, ISOTP_N_ERROR); + return; } +#endif LOG_DBG("Got CF irq. Appending data"); - receive_add_mem(ctx, &frame->data[index], frame->dlc - index); - ctx->length -= frame->dlc - index; + data_len = (ctx->length > frame->dlc - index) ? frame->dlc - index : + ctx->length; + receive_add_mem(ctx, &frame->data[index], data_len); + ctx->length -= data_len; LOG_DBG("%d bytes remaining", ctx->length); if (ctx->length == 0) { @@ -755,6 +784,15 @@ static void send_process_fc(struct isotp_send_ctx *ctx, return; } +#ifdef CONFIG_ISOTP_ENABLE_TX_PADDING + /* AUTOSAR requirement SWS_CanTp_00349 */ + if (frame->dlc != ISOTP_CAN_DL) { + LOG_ERR("FC DL invalid. Ignore"); + send_report_error(ctx, ISOTP_N_ERROR); + return; + } +#endif + switch (*data++ & ISOTP_PCI_FS_MASK) { case ISOTP_PCI_FS_CTS: ctx->state = ISOTP_TX_SEND_CF; @@ -853,7 +891,13 @@ static inline int send_sf(struct isotp_send_ctx *ctx) __ASSERT_NO_MSG(len <= ISOTP_CAN_DL - index); memcpy(&frame.data[index], data, len); +#ifdef CONFIG_ISOTP_ENABLE_TX_PADDING + /* AUTOSAR requirement SWS_CanTp_00348 */ + memset(&frame.data[index + len], 0xCC, ISOTP_CAN_DL - len - index); + frame.dlc = ISOTP_CAN_DL; +#else frame.dlc = len + index; +#endif ctx->state = ISOTP_TX_SEND_SF; ret = can_send(ctx->can_dev, &frame, K_MSEC(ISOTP_A), @@ -926,10 +970,17 @@ static inline int send_cf(struct isotp_send_ctx *ctx) rem_len = get_ctx_data_length(ctx); len = MIN(rem_len, ISOTP_CAN_DL - index); rem_len -= len; - frame.dlc = len + index; data = get_data_ctx(ctx); memcpy(&frame.data[index], data, len); +#ifdef CONFIG_ISOTP_ENABLE_TX_PADDING + /* AUTOSAR requirement SWS_CanTp_00348 */ + memset(&frame.data[index + len], 0xCC, ISOTP_CAN_DL - len - index); + frame.dlc = ISOTP_CAN_DL; +#else + frame.dlc = len + index; +#endif + ret = can_send(ctx->can_dev, &frame, K_MSEC(ISOTP_A), send_can_tx_isr, ctx); if (ret == CAN_TX_OK) { diff --git a/tests/subsys/canbus/isotp/conformance/src/main.c b/tests/subsys/canbus/isotp/conformance/src/main.c index 248c70ec062a9..818c72009da3f 100644 --- a/tests/subsys/canbus/isotp/conformance/src/main.c +++ b/tests/subsys/canbus/isotp/conformance/src/main.c @@ -31,13 +31,19 @@ #define FC_PCI_BYTE_1(FS) ((FC_PCI_TYPE << PCI_TYPE_POS) | (FS)) #define FC_PCI_BYTE_2(BS) (BS) #define FC_PCI_BYTE_3(ST_MIN) (ST_MIN) -#define DATA_SIZE_FC 3 #define CF_PCI_TYPE 2 #define CF_PCI_BYTE_1 (CF_PCI_TYPE << PCI_TYPE_POS) #define STMIN_VAL_1 5 #define STMIN_VAL_2 50 #define STMIN_UPPER_TOLERANCE 5 +#if defined(CONFIG_ISOTP_ENABLE_TX_PADDING) || \ + defined(CONFIG_ISOTP_ENABLE_TX_PADDING) +#define DATA_SIZE_FC CAN_DL +#else +#define DATA_SIZE_FC 3 +#endif + #define CEIL(A, B) (((A) + (B) - 1) / (B)) #define BS_TIMEOUT_UPPER_MS 1100 @@ -167,6 +173,8 @@ static void get_sf(struct isotp_recv_ctx *recv_ctx, size_t data_size) zassert_equal(ret, 0, "Data differ"); } +#ifdef CONFIG_ISOTP_REQUIRE_RX_PADDING + static void get_sf_ignore(struct isotp_recv_ctx *recv_ctx) { int ret; @@ -175,6 +183,8 @@ static void get_sf_ignore(struct isotp_recv_ctx *recv_ctx) zassert_equal(ret, ISOTP_RECV_TIMEOUT, "recv returned %d", ret); } +#endif + static void send_test_data(const uint8_t *data, size_t len) { int ret; @@ -246,11 +256,18 @@ static void check_frame_series(struct frame_desired *frames, size_t length, zassert_equal(ret, 0, "Timeout waiting for msg nr %d. ret: %d", i, ret); +#if !defined(CONFIG_ISOTP_REQUIRE_RX_PADDING) && \ + !defined(CONFIG_ISOTP_ENABLE_TX_PADDING) zassert_equal(frame.dlc, desired->length, "DLC of frame nr %d differ. Desired: %d, Got: %d", i, desired->length, frame.dlc); +#endif + +#if !defined(CONFIG_ISOTP_ENABLE_TX_PADDING) ret = check_data(frame.data, desired->data, desired->length); zassert_equal(ret, 0, "Data differ"); +#endif + desired++; } ret = k_msgq_get(msgq, &frame, K_MSEC(200)); @@ -289,7 +306,9 @@ static void prepare_cf_frames(struct frame_desired *frames, size_t frames_cnt, memcpy(&des_frames[i].data[1], data_ptr, DATA_SIZE_CF); if (remaining_length < DATA_SIZE_CF) { +#ifndef CONFIG_ISOTP_ENABLE_TX_PADDING frames[i].length = remaining_length + 1; +#endif remaining_length = 0; } @@ -338,11 +357,13 @@ static void test_receive_sf(void) single_frame.data[0] = SF_PCI_BYTE_LEN_8; send_frame_series(&single_frame, 1, rx_addr.std_id); +#ifdef CONFIG_ISOTP_REQUIRE_RX_PADDING single_frame.data[0] = SF_PCI_BYTE_1; single_frame.length = 7; send_frame_series(&single_frame, 1, rx_addr.std_id); get_sf_ignore(&recv_ctx); +#endif isotp_unbind(&recv_ctx); } @@ -392,11 +413,13 @@ static void test_receive_sf_ext(void) single_frame.data[1] = SF_PCI_BYTE_1; send_frame_series(&single_frame, 1, rx_addr.std_id); +#ifdef CONFIG_ISOTP_REQUIRE_RX_PADDING single_frame.data[1] = SF_PCI_BYTE_2_EXT; single_frame.length = 7; send_frame_series(&single_frame, 1, rx_addr.std_id); get_sf_ignore(&recv_ctx); +#endif isotp_unbind(&recv_ctx); } @@ -418,7 +441,7 @@ static void test_send_data(void) fc_frame.data[0] = FC_PCI_BYTE_1(FC_PCI_CTS); fc_frame.data[1] = FC_PCI_BYTE_2(0); fc_frame.data[2] = FC_PCI_BYTE_3(0); - fc_frame.length = 3; + fc_frame.length = DATA_SIZE_FC; prepare_cf_frames(des_frames, ARRAY_SIZE(des_frames), data_ptr, remaining_length); @@ -457,7 +480,7 @@ static void test_send_data_blocks(void) fc_frame.data[0] = FC_PCI_BYTE_1(FC_PCI_CTS); fc_frame.data[1] = FC_PCI_BYTE_2(fc_opts.bs); fc_frame.data[2] = FC_PCI_BYTE_3(0); - fc_frame.length = 3; + fc_frame.length = DATA_SIZE_FC; prepare_cf_frames(des_frames, ARRAY_SIZE(des_frames), data_ptr, remaining_length); @@ -520,7 +543,7 @@ static void test_receive_data(void) fc_frame.data[0] = FC_PCI_BYTE_1(FC_PCI_CTS); fc_frame.data[1] = FC_PCI_BYTE_2(fc_opts_single.bs); fc_frame.data[2] = FC_PCI_BYTE_3(fc_opts_single.stmin); - fc_frame.length = 3; + fc_frame.length = DATA_SIZE_FC; prepare_cf_frames(des_frames, ARRAY_SIZE(des_frames), data_ptr, remaining_length); @@ -564,7 +587,7 @@ static void test_receive_data_blocks(void) fc_frame.data[0] = FC_PCI_BYTE_1(FC_PCI_CTS); fc_frame.data[1] = FC_PCI_BYTE_2(fc_opts.bs); fc_frame.data[2] = FC_PCI_BYTE_3(fc_opts.stmin); - fc_frame.length = 3; + fc_frame.length = DATA_SIZE_FC; prepare_cf_frames(des_frames, ARRAY_SIZE(des_frames), data_ptr, remaining_length); @@ -615,7 +638,7 @@ static void test_send_timeouts(void) fc_cts_frame.data[0] = FC_PCI_BYTE_1(FC_PCI_CTS); fc_cts_frame.data[1] = FC_PCI_BYTE_2(fc_opts.bs); fc_cts_frame.data[2] = FC_PCI_BYTE_3(0); - fc_cts_frame.length = 3; + fc_cts_frame.length = DATA_SIZE_FC; /* Test timeout for first FC*/ start_time = k_uptime_get_32(); @@ -716,7 +739,7 @@ static void test_stmin(void) fc_frame.data[0] = FC_PCI_BYTE_1(FC_PCI_CTS); fc_frame.data[1] = FC_PCI_BYTE_2(2); fc_frame.data[2] = FC_PCI_BYTE_3(STMIN_VAL_1); - fc_frame.length = 3; + fc_frame.length = DATA_SIZE_FC; filter_id = attach_msgq(rx_addr.std_id); zassert_true((filter_id >= 0), "Negative filter number [%d]", @@ -771,7 +794,7 @@ void test_receiver_fc_errors(void) fc_frame.data[0] = FC_PCI_BYTE_1(FC_PCI_CTS); fc_frame.data[1] = FC_PCI_BYTE_2(fc_opts.bs); fc_frame.data[2] = FC_PCI_BYTE_3(fc_opts.stmin); - fc_frame.length = 3; + fc_frame.length = DATA_SIZE_FC; filter_id = attach_msgq(tx_addr.std_id); zassert_true((filter_id >= 0), "Negative filter number [%d]", @@ -819,7 +842,7 @@ void test_sender_fc_errors(void) fc_frame.data[0] = FC_PCI_BYTE_1(3); fc_frame.data[1] = FC_PCI_BYTE_2(fc_opts.bs); fc_frame.data[2] = FC_PCI_BYTE_3(fc_opts.stmin); - fc_frame.length = 3; + fc_frame.length = DATA_SIZE_FC; k_sem_reset(&send_compl_sem); ret = isotp_send(&send_ctx, can_dev, random_data, DATA_SEND_LENGTH, @@ -841,7 +864,7 @@ void test_sender_fc_errors(void) ret = isotp_send(&send_ctx, can_dev, random_data, 5*1024, &tx_addr, &rx_addr, NULL, NULL); zassert_equal(ret, ISOTP_N_BUFFER_OVERFLW, - "Expected overflow bot got %d", ret); + "Expected overflow but got %d", ret); isotp_unbind(&recv_ctx); filter_id = attach_msgq(tx_addr.std_id); From ca9d271cb74f5002eaf9862f80d9983a26b7c706 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Martin=20J=C3=A4ger?= Date: Wed, 30 Dec 2020 15:40:03 +0100 Subject: [PATCH 3/4] canbus: isotp: add fixed addressing feature MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Fixed addressing as specified in ISO 15765-2 encodes the source and target address of a device or function inside the CAN ID according to SAE J1939. In order to allow to receive incoming requests from different nodes, the CAN filter mask has to be set such that the source address is ignored in the receive context. In addition to that, flow control frames have to be sent back to the actual source of the request, which requires adjustments to the TX CAN ID. This commit implements above features and thus allows ISO-TP to be used in a network based on SAE J1939 or NMEA-2000. Signed-off-by: Martin Jäger --- include/canbus/isotp.h | 51 ++++++++- subsys/canbus/isotp/isotp.c | 24 +++- .../canbus/isotp/conformance/src/main.c | 106 +++++++++++++++--- 3 files changed, 157 insertions(+), 24 deletions(-) diff --git a/include/canbus/isotp.h b/include/canbus/isotp.h index 60ad12edceee8..6fda2525aa4cf 100644 --- a/include/canbus/isotp.h +++ b/include/canbus/isotp.h @@ -35,7 +35,9 @@ * FC Flow Control * FF First Frame * FS Flow Status - * AE Adders Extension + * AE Address Extension + * SA Source Address + * TA Target Address */ /* @@ -90,6 +92,38 @@ /** Timeout for recv */ #define ISOTP_RECV_TIMEOUT -14 +/* + * CAN ID filtering for ISO-TP fixed addressing according to SAE J1939 + * + * Format of 29-bit CAN identifier: + * ------------------------------------------------------ + * | 28 .. 26 | 25 | 24 | 23 .. 16 | 15 .. 8 | 7 .. 0 | + * ------------------------------------------------------ + * | Priority | EDP | DP | N_TAtype | N_TA | N_SA | + * ------------------------------------------------------ + */ + +/** Position of fixed source address (SA) */ +#define ISOTP_FIXED_ADDR_SA_POS (0U) + +/** Mask to obtain fixed source address (SA) */ +#define ISOTP_FIXED_ADDR_SA_MASK (0xFF << ISOTP_FIXED_ADDR_SA_POS) + +/** Position of fixed target address (TA) */ +#define ISOTP_FIXED_ADDR_TA_POS (8U) + +/** Mask to obtain fixed target address (TA) */ +#define ISOTP_FIXED_ADDR_TA_MASK (0xFF << ISOTP_FIXED_ADDR_TA_POS) + +/** Position of priority in fixed addressing mode */ +#define ISOTP_FIXED_ADDR_PRIO_POS (26U) + +/** Mask for priority in fixed addressing mode */ +#define ISOTP_FIXED_ADDR_PRIO_MASK (0x7 << ISOTP_FIXED_ADDR_PRIO_POS) + +/* CAN filter RX mask to match any priority and source address (SA) */ +#define ISOTP_FIXED_ADDR_RX_MASK (0x03FFFF00) + #ifdef __cplusplus extern "C" { #endif @@ -100,17 +134,24 @@ extern "C" { * Used to pass addresses to the bind and send functions. */ struct isotp_msg_id { - /** Message identifier*/ + /** + * CAN identifier + * + * If ISO-TP fixed addressing is used, isotp_bind ignores SA and + * priority sections and modifies TA section in flow control frames. + */ union { uint32_t std_id : 11; uint32_t ext_id : 29; }; - /** extended address */ + /** ISO-TP extended address (if used) */ uint8_t ext_addr; - /** Indicates the identifier type (standard or extended) */ + /** Indicates the CAN identifier type (standard or extended) */ uint8_t id_type : 1; - /** Indicates if extended addressing is used */ + /** Indicates if ISO-TP extended addressing is used */ uint8_t use_ext_addr : 1; + /** Indicates if ISO-TP fixed addressing (acc. to SAE J1939) is used */ + uint8_t use_fixed_addr : 1; }; /* diff --git a/subsys/canbus/isotp/isotp.c b/subsys/canbus/isotp/isotp.c index f5e8d5f0c3d44..e01a62877b5a1 100644 --- a/subsys/canbus/isotp/isotp.c +++ b/subsys/canbus/isotp/isotp.c @@ -391,6 +391,7 @@ static void process_ff_sf(struct isotp_recv_ctx *ctx, struct zcan_frame *frame) { int index = 0; uint8_t payload_len; + uint32_t rx_sa; /* ISO-TP fixed source address (if used) */ if (ctx->rx_addr.use_ext_addr) { if (frame->data[index++] != ctx->rx_addr.ext_addr) { @@ -398,6 +399,19 @@ static void process_ff_sf(struct isotp_recv_ctx *ctx, struct zcan_frame *frame) } } + if (ctx->rx_addr.use_fixed_addr) { + /* store actual CAN ID used by the sender */ + ctx->rx_addr.ext_id = frame->id; + /* replace TX target address with RX source address */ + rx_sa = (frame->id & ISOTP_FIXED_ADDR_SA_MASK) >> + ISOTP_FIXED_ADDR_SA_POS; + ctx->tx_addr.ext_id &= ~(ISOTP_FIXED_ADDR_TA_MASK); + ctx->tx_addr.ext_id |= rx_sa << ISOTP_FIXED_ADDR_TA_POS; + /* use same priority for TX as in received message */ + ctx->tx_addr.ext_id &= ~(ISOTP_FIXED_ADDR_PRIO_MASK); + ctx->tx_addr.ext_id |= frame->id & ISOTP_FIXED_ADDR_PRIO_MASK; + } + switch (frame->data[index] & ISOTP_PCI_TYPE_MASK) { case ISOTP_PCI_TYPE_FF: LOG_DBG("Got FF IRQ"); @@ -558,12 +572,20 @@ static void receive_can_rx_isr(struct zcan_frame *frame, void *arg) static inline int attach_ff_filter(struct isotp_recv_ctx *ctx) { + uint32_t mask; + + if (ctx->rx_addr.use_fixed_addr) { + mask = ISOTP_FIXED_ADDR_RX_MASK; + } else { + mask = CAN_EXT_ID_MASK; + } + struct zcan_filter filter = { .id_type = ctx->rx_addr.id_type, .rtr = CAN_DATAFRAME, .id = ctx->rx_addr.ext_id, .rtr_mask = 1, - .id_mask = CAN_EXT_ID_MASK + .id_mask = mask }; ctx->filter_id = can_attach_isr(ctx->can_dev, receive_can_rx_isr, ctx, diff --git a/tests/subsys/canbus/isotp/conformance/src/main.c b/tests/subsys/canbus/isotp/conformance/src/main.c index 818c72009da3f..c78b5466b3c24 100644 --- a/tests/subsys/canbus/isotp/conformance/src/main.c +++ b/tests/subsys/canbus/isotp/conformance/src/main.c @@ -110,6 +110,20 @@ const struct isotp_msg_id tx_addr_ext = { .ext_addr = EXT_ADDR }; +const struct isotp_msg_id rx_addr_fixed = { + .ext_id = 0x18DA0201, + .id_type = CAN_EXTENDED_IDENTIFIER, + .use_ext_addr = 0, + .use_fixed_addr = 1 +}; + +const struct isotp_msg_id tx_addr_fixed = { + .ext_id = 0x18DA0102, + .id_type = CAN_EXTENDED_IDENTIFIER, + .use_ext_addr = 0, + .use_fixed_addr = 1 +}; + const struct device *can_dev; struct isotp_recv_ctx recv_ctx; struct isotp_send_ctx send_ctx; @@ -173,8 +187,6 @@ static void get_sf(struct isotp_recv_ctx *recv_ctx, size_t data_size) zassert_equal(ret, 0, "Data differ"); } -#ifdef CONFIG_ISOTP_REQUIRE_RX_PADDING - static void get_sf_ignore(struct isotp_recv_ctx *recv_ctx) { int ret; @@ -183,8 +195,6 @@ static void get_sf_ignore(struct isotp_recv_ctx *recv_ctx) zassert_equal(ret, ISOTP_RECV_TIMEOUT, "recv returned %d", ret); } -#endif - static void send_test_data(const uint8_t *data, size_t len) { int ret; @@ -229,7 +239,8 @@ static void send_frame_series(struct frame_desired *frames, size_t length, { int i, ret; struct zcan_frame frame = { - .id_type = CAN_STANDARD_IDENTIFIER, + .id_type = (id > 0x7FF) ? CAN_EXTENDED_IDENTIFIER : + CAN_STANDARD_IDENTIFIER, .rtr = CAN_DATAFRAME, .id = id }; @@ -274,15 +285,16 @@ static void check_frame_series(struct frame_desired *frames, size_t length, zassert_equal(ret, -EAGAIN, "Expected timeout, but received %d", ret); } -static int attach_msgq(uint32_t id) +static int attach_msgq(uint32_t id, uint32_t mask) { int filter_id; struct zcan_filter filter = { - .id_type = CAN_STANDARD_IDENTIFIER, + .id_type = (id > 0x7FF) ? CAN_EXTENDED_IDENTIFIER : + CAN_STANDARD_IDENTIFIER, .rtr = CAN_DATAFRAME, .id = id, .rtr_mask = 1, - .id_mask = CAN_STD_ID_MASK + .id_mask = mask }; filter_id = can_attach_msgq(can_dev, &frame_msgq, &filter); @@ -326,7 +338,7 @@ static void test_send_sf(void) memcpy(&des_frame.data[1], random_data, DATA_SIZE_SF); des_frame.length = DATA_SIZE_SF + 1; - filter_id = attach_msgq(rx_addr.std_id); + filter_id = attach_msgq(rx_addr.std_id, CAN_STD_ID_MASK); zassert_true((filter_id >= 0), "Negative filter number [%d]", filter_id); @@ -378,7 +390,7 @@ static void test_send_sf_ext(void) memcpy(&des_frame.data[2], random_data, DATA_SIZE_SF_EXT); des_frame.length = DATA_SIZE_SF_EXT + 2; - filter_id = attach_msgq(rx_addr_ext.std_id); + filter_id = attach_msgq(rx_addr_ext.std_id, CAN_STD_ID_MASK); zassert_true((filter_id >= 0), "Negative filter number [%d]", filter_id); @@ -424,6 +436,62 @@ static void test_receive_sf_ext(void) isotp_unbind(&recv_ctx); } +static void test_send_sf_fixed(void) +{ + int filter_id, ret; + struct frame_desired des_frame; + + des_frame.data[0] = SF_PCI_BYTE_1; + memcpy(&des_frame.data[1], random_data, DATA_SIZE_SF); + des_frame.length = DATA_SIZE_SF + 1; + + /* mask to allow any priority and source address (SA) */ + filter_id = attach_msgq(rx_addr_fixed.ext_id, 0x03FFFF00); + zassert_true((filter_id >= 0), "Negative filter number [%d]", + filter_id); + + ret = isotp_send(&send_ctx, can_dev, random_data, DATA_SIZE_SF, + &rx_addr_fixed, &tx_addr_fixed, send_complette_cb, + ISOTP_N_OK); + zassert_equal(ret, 0, "Send returned %d", ret); + + check_frame_series(&des_frame, 1, &frame_msgq); + + can_detach(can_dev, filter_id); +} + +static void test_receive_sf_fixed(void) +{ + int ret; + struct frame_desired single_frame; + + single_frame.data[0] = SF_PCI_BYTE_1; + memcpy(&single_frame.data[1], random_data, DATA_SIZE_SF); + single_frame.length = DATA_SIZE_SF + 1; + + ret = isotp_bind(&recv_ctx, can_dev, &rx_addr_fixed, &tx_addr_fixed, + &fc_opts_single, K_NO_WAIT); + zassert_equal(ret, ISOTP_N_OK, "Binding failed [%d]", ret); + + /* default source address */ + send_frame_series(&single_frame, 1, rx_addr_fixed.ext_id); + get_sf(&recv_ctx, DATA_SIZE_SF); + + /* different source address */ + send_frame_series(&single_frame, 1, rx_addr_fixed.ext_id | 0xFF); + get_sf(&recv_ctx, DATA_SIZE_SF); + + /* different priority */ + send_frame_series(&single_frame, 1, rx_addr_fixed.ext_id | (7U << 26)); + get_sf(&recv_ctx, DATA_SIZE_SF); + + /* different target address (should fail) */ + send_frame_series(&single_frame, 1, rx_addr_fixed.ext_id | 0xFF00); + get_sf_ignore(&recv_ctx); + + isotp_unbind(&recv_ctx); +} + static void test_send_data(void) { struct frame_desired fc_frame, ff_frame; @@ -446,7 +514,7 @@ static void test_send_data(void) prepare_cf_frames(des_frames, ARRAY_SIZE(des_frames), data_ptr, remaining_length); - filter_id = attach_msgq(rx_addr.std_id); + filter_id = attach_msgq(rx_addr.std_id, CAN_STD_ID_MASK); zassert_true((filter_id >= 0), "Negative filter number [%d]", filter_id); @@ -487,7 +555,7 @@ static void test_send_data_blocks(void) remaining_length = DATA_SEND_LENGTH; - filter_id = attach_msgq(rx_addr.std_id); + filter_id = attach_msgq(rx_addr.std_id, CAN_STD_ID_MASK); zassert_true((filter_id >= 0), "Negative filter number [%d]", filter_id); @@ -548,7 +616,7 @@ static void test_receive_data(void) prepare_cf_frames(des_frames, ARRAY_SIZE(des_frames), data_ptr, remaining_length); - filter_id = attach_msgq(tx_addr.std_id); + filter_id = attach_msgq(tx_addr.std_id, CAN_STD_ID_MASK); ret = isotp_bind(&recv_ctx, can_dev, &rx_addr, &tx_addr, &fc_opts_single, K_NO_WAIT); @@ -594,7 +662,7 @@ static void test_receive_data_blocks(void) remaining_frames = CEIL(remaining_length, DATA_SIZE_CF); - filter_id = attach_msgq(tx_addr.std_id); + filter_id = attach_msgq(tx_addr.std_id, CAN_STD_ID_MASK); zassert_true((filter_id >= 0), "Negative filter number [%d]", filter_id); @@ -741,7 +809,7 @@ static void test_stmin(void) fc_frame.data[2] = FC_PCI_BYTE_3(STMIN_VAL_1); fc_frame.length = DATA_SIZE_FC; - filter_id = attach_msgq(rx_addr.std_id); + filter_id = attach_msgq(rx_addr.std_id, CAN_STD_ID_MASK); zassert_true((filter_id >= 0), "Negative filter number [%d]", filter_id); @@ -796,7 +864,7 @@ void test_receiver_fc_errors(void) fc_frame.data[2] = FC_PCI_BYTE_3(fc_opts.stmin); fc_frame.length = DATA_SIZE_FC; - filter_id = attach_msgq(tx_addr.std_id); + filter_id = attach_msgq(tx_addr.std_id, CAN_STD_ID_MASK); zassert_true((filter_id >= 0), "Negative filter number [%d]", filter_id); @@ -836,7 +904,7 @@ void test_sender_fc_errors(void) memcpy(&ff_frame.data[2], random_data, DATA_SIZE_FF); ff_frame.length = DATA_SIZE_FF + 2; - filter_id = attach_msgq(tx_addr.std_id); + filter_id = attach_msgq(tx_addr.std_id, CAN_STD_ID_MASK); /* invalid flow status */ fc_frame.data[0] = FC_PCI_BYTE_1(3); @@ -866,7 +934,7 @@ void test_sender_fc_errors(void) zassert_equal(ret, ISOTP_N_BUFFER_OVERFLW, "Expected overflow but got %d", ret); isotp_unbind(&recv_ctx); - filter_id = attach_msgq(tx_addr.std_id); + filter_id = attach_msgq(tx_addr.std_id, CAN_STD_ID_MASK); k_sem_reset(&send_compl_sem); ret = isotp_send(&send_ctx, can_dev, random_data, DATA_SEND_LENGTH, @@ -918,6 +986,8 @@ void test_main(void) ztest_unit_test(test_receive_sf), ztest_unit_test(test_send_sf_ext), ztest_unit_test(test_receive_sf_ext), + ztest_unit_test(test_send_sf_fixed), + ztest_unit_test(test_receive_sf_fixed), ztest_unit_test(test_send_data), ztest_unit_test(test_send_data_blocks), ztest_unit_test(test_receive_data), From 6f8925966b1161e1a3000ded454e86c600760a89 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Martin=20J=C3=A4ger?= Date: Sun, 3 Jan 2021 10:06:44 +0100 Subject: [PATCH 4/4] canbus: isotp: prevent race-condition during debugging MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit During debugging I got "Got unexpected PDU" errors because a new CF was received before the state was set to ISOTP_TX_WAIT_FC in the send state machine. Setting the state before printing the debug information fixes the issue. Signed-off-by: Martin Jäger --- subsys/canbus/isotp/isotp.c | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/subsys/canbus/isotp/isotp.c b/subsys/canbus/isotp/isotp.c index e01a62877b5a1..e5950e7c2a8f5 100644 --- a/subsys/canbus/isotp/isotp.c +++ b/subsys/canbus/isotp/isotp.c @@ -1068,11 +1068,11 @@ static void send_state_machine(struct isotp_send_ctx *ctx) switch (ctx->state) { case ISOTP_TX_SEND_FF: - LOG_DBG("SM send FF"); send_ff(ctx); z_add_timeout(&ctx->timeout, send_timeout_handler, K_MSEC(ISOTP_BS)); ctx->state = ISOTP_TX_WAIT_FC; + LOG_DBG("SM send FF"); break; case ISOTP_TX_SEND_CF: @@ -1094,11 +1094,11 @@ static void send_state_machine(struct isotp_send_ctx *ctx) } if (ctx->opts.bs && !ctx->bs) { - LOG_DBG("BS reached. Wait for FC again"); - ctx->state = ISOTP_TX_WAIT_FC; z_add_timeout(&ctx->timeout, send_timeout_handler, K_MSEC(ISOTP_BS)); + ctx->state = ISOTP_TX_WAIT_FC; + LOG_DBG("BS reached. Wait for FC again"); break; } else if (ctx->opts.stmin) { ctx->state = ISOTP_TX_WAIT_ST; @@ -1109,10 +1109,10 @@ static void send_state_machine(struct isotp_send_ctx *ctx) break; case ISOTP_TX_WAIT_ST: - LOG_DBG("SM wait ST"); z_add_timeout(&ctx->timeout, send_timeout_handler, stmin_to_ticks(ctx->opts.stmin)); ctx->state = ISOTP_TX_SEND_CF; + LOG_DBG("SM wait ST"); break; case ISOTP_TX_ERR: