diff --git a/include/thingset/can.h b/include/thingset/can.h index 31b8794..82d2fd0 100644 --- a/include/thingset/can.h +++ b/include/thingset/can.h @@ -86,18 +86,27 @@ extern "C" { #define THINGSET_CAN_DATA_ID_GET(id) \ (((uint32_t)id & THINGSET_CAN_DATA_ID_MASK) >> THINGSET_CAN_DATA_ID_POS) -/* bus ID for request/response messages */ -#define THINGSET_CAN_BUS_ID_POS (16U) -#define THINGSET_CAN_BUS_ID_MASK (0xFF << THINGSET_CAN_BUS_ID_POS) -#define THINGSET_CAN_BUS_ID_SET(id) \ - (((uint32_t)id << THINGSET_CAN_BUS_ID_POS) & THINGSET_CAN_BUS_ID_MASK) -#define THINGSET_CAN_BUS_ID_GET(id) \ - (((uint32_t)id & THINGSET_CAN_BUS_ID_MASK) >> THINGSET_CAN_BUS_ID_POS) -#define THINGSET_CAN_BUS_ID_DEFAULT (0xDA) // 218, N_TAtype = physical +/* bus numbers for request/response messages */ +#define THINGSET_CAN_SOURCE_BUS_POS (16U) +#define THINGSET_CAN_SOURCE_BUS_MASK (0xF << THINGSET_CAN_SOURCE_BUS_POS) +#define THINGSET_CAN_SOURCE_BUS_SET(id) \ + (((uint32_t)id << THINGSET_CAN_SOURCE_BUS_POS) & THINGSET_CAN_SOURCE_BUS_MASK) +#define THINGSET_CAN_SOURCE_BUS_GET(id) \ + (((uint32_t)id & THINGSET_CAN_SOURCE_BUS_MASK) >> THINGSET_CAN_SOURCE_BUS_POS) +#define THINGSET_CAN_SOURCE_BUS_DEFAULT (0x0) +#define THINGSET_CAN_TARGET_BUS_POS (20U) +#define THINGSET_CAN_TARGET_BUS_MASK (0xF << THINGSET_CAN_TARGET_BUS_POS) +#define THINGSET_CAN_TARGET_BUS_SET(id) \ + (((uint32_t)id << THINGSET_CAN_TARGET_BUS_POS) & THINGSET_CAN_TARGET_BUS_MASK) +#define THINGSET_CAN_TARGET_BUS_GET(id) \ + (((uint32_t)id & THINGSET_CAN_TARGET_BUS_MASK) >> THINGSET_CAN_TARGET_BUS_POS) +#define THINGSET_CAN_TARGET_BUS_DEFAULT (0x0) /* random number for address discovery messages */ -#define THINGSET_CAN_RAND_SET THINGSET_CAN_BUS_ID_SET -#define THINGSET_CAN_RAND_GET THINGSET_CAN_BUS_ID_GET +#define THINGSET_CAN_RAND_POS (16U) +#define THINGSET_CAN_RAND_MASK (0xFF << THINGSET_CAN_RAND_POS) +#define THINGSET_CAN_RAND_SET(id) (((uint32_t)id << THINGSET_CAN_RAND_POS) & THINGSET_CAN_RAND_MASK) +#define THINGSET_CAN_RAND_GET(id) (((uint32_t)id & THINGSET_CAN_RAND_MASK) >> THINGSET_CAN_RAND_POS) /* message types */ #define THINGSET_CAN_TYPE_POS (24U) @@ -180,6 +189,7 @@ struct thingset_can thingset_can_report_rx_callback_t report_rx_cb; int64_t next_pub_time; uint8_t node_addr; + uint8_t bus_number; }; #ifdef CONFIG_THINGSET_CAN_MULTIPLE_INSTANCES @@ -196,7 +206,7 @@ struct thingset_can * @returns length of message or negative errno in case of error */ int thingset_can_receive_inst(struct thingset_can *ts_can, uint8_t *rx_buf, size_t rx_buf_size, - uint8_t *source_addr, k_timeout_t timeout); + uint8_t *source_addr, uint8_t *source_bus, k_timeout_t timeout); #ifdef CONFIG_ISOTP_FAST /** @@ -214,8 +224,9 @@ int thingset_can_receive_inst(struct thingset_can *ts_can, uint8_t *rx_buf, size * @returns 0 for success or negative errno in case of error */ int thingset_can_send_inst(struct thingset_can *ts_can, uint8_t *tx_buf, size_t tx_len, - uint8_t target_addr, thingset_can_response_callback_t rsp_callback, - void *callback_arg, k_timeout_t timeout); + uint8_t target_addr, uint8_t target_bus, + thingset_can_response_callback_t rsp_callback, void *callback_arg, + k_timeout_t timeout); #else /** * Send ThingSet message to other node @@ -228,7 +239,7 @@ int thingset_can_send_inst(struct thingset_can *ts_can, uint8_t *tx_buf, size_t * @returns 0 for success or negative errno in case of error */ int thingset_can_send_inst(struct thingset_can *ts_can, uint8_t *tx_buf, size_t tx_len, - uint8_t target_addr); + uint8_t target_addr, uint8_t target_bus); /** * Process incoming ThingSet requests @@ -271,7 +282,8 @@ int thingset_can_set_report_rx_callback_inst(struct thingset_can *ts_can, * * @returns 0 for success or negative errno in case of error */ -int thingset_can_init_inst(struct thingset_can *ts_can, const struct device *can_dev); +int thingset_can_init_inst(struct thingset_can *ts_can, const struct device *can_dev, + uint8_t bus_number); #else /* !CONFIG_THINGSET_CAN_MULTIPLE_INSTANCES */ @@ -285,7 +297,7 @@ int thingset_can_init_inst(struct thingset_can *ts_can, const struct device *can * * @returns 0 for success or negative errno in case of error */ -int thingset_can_send(uint8_t *tx_buf, size_t tx_len, uint8_t target_addr, +int thingset_can_send(uint8_t *tx_buf, size_t tx_len, uint8_t target_addr, uint8_t target_bus, thingset_can_response_callback_t rsp_callback, void *callback_arg, k_timeout_t timeout); #else @@ -298,7 +310,7 @@ int thingset_can_send(uint8_t *tx_buf, size_t tx_len, uint8_t target_addr, * * @returns 0 for success or negative errno in case of error */ -int thingset_can_send(uint8_t *tx_buf, size_t tx_len, uint8_t target_addr); +int thingset_can_send(uint8_t *tx_buf, size_t tx_len, uint8_t target_addr, uint8_t target_bus); #endif /* CONFIG_ISOTP_FAST */ /** diff --git a/include/thingset/isotp_fast.h b/include/thingset/isotp_fast.h index 9a5e87c..78d3eb4 100644 --- a/include/thingset/isotp_fast.h +++ b/include/thingset/isotp_fast.h @@ -144,4 +144,4 @@ int isotp_fast_recv(struct isotp_fast_ctx *ctx, struct can_filter sender, uint8_ * @returns 0 on success. */ int isotp_fast_send(struct isotp_fast_ctx *ctx, const uint8_t *data, size_t len, - const isotp_fast_node_id target_addr, void *sent_cb_arg); + const isotp_fast_node_id target_addr, uint8_t target_bus, void *sent_cb_arg); diff --git a/src/Kconfig.can b/src/Kconfig.can index b66d350..ed93267 100644 --- a/src/Kconfig.can +++ b/src/Kconfig.can @@ -74,6 +74,12 @@ config THINGSET_CAN_MULTIPLE_INSTANCES for a dedicated CAN device and and messages have to be processed manually in application threads. +config THINGSET_CAN_BUS_NUMBER + int "ThingSet CAN bus number" + depends on !THINGSET_CAN_MULTIPLE_INSTANCES + range 0 15 + default 0 + config THINGSET_CAN_RESPONSE_DELAY int "ThingSet CAN response delay" range 0 100 diff --git a/src/Kconfig.isotp_fast b/src/Kconfig.isotp_fast index ed15de3..8cfd83e 100644 --- a/src/Kconfig.isotp_fast +++ b/src/Kconfig.isotp_fast @@ -38,4 +38,23 @@ config ISOTP_FAST_BLOCKING_RECEIVE Whether to make blocking receive functionality available to ease migration from the old API. +config ISOTP_FAST_BUS_SIZE + int "CAN filter mask to match any priority and source address (SA)" + default 0 + range 0 4 + help + Size of the bus field in the CAN ID. Use 0 to disable. + +config ISOTP_FAST_TARGET_BUS_POS + int "Position of the target bus" + default 20 + help + Target bus position in bits. + +config ISOTP_FAST_SOURCE_BUS_POS + int "Position of the source bus" + default 16 + help + Source bus position in bits. + endif diff --git a/src/can.c b/src/can.c index 0f6e1b1..1152e8e 100644 --- a/src/can.c +++ b/src/can.c @@ -20,7 +20,7 @@ #include "packetizer.h" -LOG_MODULE_REGISTER(thingset_can, CONFIG_THINGSET_SDK_LOG_LEVEL); +LOG_MODULE_REGISTER(thingset_can, 4); extern uint8_t eui64[8]; @@ -133,6 +133,13 @@ static void thingset_can_addr_claim_tx_cb(const struct device *dev, int error, v } } +static void thingset_can_discovery_tx_cb(const struct device *dev, int error, void *user_data) +{ + if (error != 0) { + LOG_ERR("Discovery failed with %d", error); + } +} + static void thingset_can_addr_claim_tx_handler(struct k_work *work) { struct k_work_delayable *dwork = k_work_delayable_from_work(work); @@ -345,7 +352,7 @@ void thingset_can_request_response_timeout_handler(struct k_timer *timer) } #else int thingset_can_receive_inst(struct thingset_can *ts_can, uint8_t *rx_buffer, size_t rx_buf_size, - uint8_t *source_addr, k_timeout_t timeout) + uint8_t *source_addr, uint8_t *source_bus, k_timeout_t timeout) { int ret, rem_len, rx_len; struct net_buf *netbuf; @@ -390,7 +397,9 @@ int thingset_can_receive_inst(struct thingset_can *ts_can, uint8_t *rx_buffer, s } else if (rx_len > 0 && rem_len == 0) { *source_addr = THINGSET_CAN_SOURCE_GET(ts_can->recv_ctx.rx_addr.ext_id); - LOG_DBG("ISO-TP received %d bytes from addr %d", rx_len, *source_addr); + *source_bus = THINGSET_CAN_SOURCE_BUS_GET(ts_can->recv_ctx.rx_addr.ext_id); + LOG_DBG("ISO-TP received %d bytes from addr 0x%X on bus 0x%X", rx_len, *source_addr, + *source_bus); return rx_len; } else if (rem_len == ISOTP_RECV_TIMEOUT) { @@ -404,8 +413,9 @@ int thingset_can_receive_inst(struct thingset_can *ts_can, uint8_t *rx_buffer, s #ifdef CONFIG_ISOTP_FAST int thingset_can_send_inst(struct thingset_can *ts_can, uint8_t *tx_buf, size_t tx_len, - uint8_t target_addr, thingset_can_response_callback_t rsp_callback, - void *callback_arg, k_timeout_t timeout) + uint8_t target_addr, uint8_t target_bus, + thingset_can_response_callback_t rsp_callback, void *callback_arg, + k_timeout_t timeout) { if (!device_is_ready(ts_can->dev)) { return -ENODEV; @@ -422,16 +432,18 @@ int thingset_can_send_inst(struct thingset_can *ts_can, uint8_t *tx_buf, size_t NULL); k_timer_start(&ts_can->request_response.timer, timeout, timeout); ts_can->request_response.can_id = THINGSET_CAN_TYPE_CHANNEL | THINGSET_CAN_PRIO_CHANNEL + | THINGSET_CAN_TARGET_BUS_SET(ts_can->bus_number) + | THINGSET_CAN_SOURCE_BUS_SET(target_bus) | THINGSET_CAN_TARGET_SET(ts_can->node_addr) | THINGSET_CAN_SOURCE_SET(target_addr); } - int ret = isotp_fast_send(&ts_can->ctx, tx_buf, tx_len, target_addr, ts_can); + int ret = isotp_fast_send(&ts_can->ctx, tx_buf, tx_len, target_addr, target_bus, ts_can); if (ret == ISOTP_N_OK) { return 0; } else { - LOG_ERR("Error sending data to addr %d: %d", target_addr, ret); + LOG_ERR("Error sending data to addr 0x%X: %d", target_addr, ret); return -EIO; } } @@ -460,9 +472,11 @@ void isotp_fast_recv_callback(struct net_buf *buffer, int rem_len, isotp_fast_ca int tx_len = thingset_process_message(&ts, ts_can->rx_buffer, len, sbuf->data, sbuf->size); if (tx_len > 0) { - isotp_fast_node_id target_id = (uint8_t)(can_id & 0xFF); - int err = thingset_can_send_inst(ts_can, sbuf->data, tx_len, target_id, NULL, NULL, - K_NO_WAIT); + isotp_fast_node_id target_addr = THINGSET_CAN_SOURCE_GET(can_id); + uint8_t target_bus = THINGSET_CAN_SOURCE_BUS_GET(can_id); + LOG_WRN("CAN ID: 0x%X, target bus: %d", can_id, target_bus); + int err = thingset_can_send_inst(ts_can, sbuf->data, tx_len, target_addr, + target_bus, NULL, NULL, K_NO_WAIT); if (err != 0) { k_sem_give(&sbuf->lock); } @@ -493,19 +507,21 @@ void isotp_fast_sent_callback(int result, void *arg) } #else int thingset_can_send_inst(struct thingset_can *ts_can, uint8_t *tx_buf, size_t tx_len, - uint8_t target_addr) + uint8_t target_addr, uint8_t target_bus) { if (!device_is_ready(ts_can->dev)) { return -ENODEV; } - ts_can->tx_addr.ext_id = THINGSET_CAN_TYPE_CHANNEL | THINGSET_CAN_PRIO_CHANNEL - | THINGSET_CAN_TARGET_SET(target_addr) - | THINGSET_CAN_SOURCE_SET(ts_can->node_addr); + ts_can->tx_addr.ext_id = + THINGSET_CAN_TYPE_CHANNEL | THINGSET_CAN_PRIO_CHANNEL + | THINGSET_CAN_TARGET_BUS_SET(target_bus) | THINGSET_CAN_SOURCE_BUS_SET(ts_can->bus_number) + | THINGSET_CAN_TARGET_SET(target_addr) | THINGSET_CAN_SOURCE_SET(ts_can->node_addr); - ts_can->rx_addr.ext_id = THINGSET_CAN_TYPE_CHANNEL | THINGSET_CAN_PRIO_CHANNEL - | THINGSET_CAN_TARGET_SET(ts_can->node_addr) - | THINGSET_CAN_SOURCE_SET(target_addr); + ts_can->rx_addr.ext_id = + THINGSET_CAN_TYPE_CHANNEL | THINGSET_CAN_PRIO_CHANNEL + | THINGSET_CAN_TARGET_BUS_SET(ts_can->bus_number) | THINGSET_CAN_SOURCE_BUS_SET(target_bus) + | THINGSET_CAN_TARGET_SET(ts_can->node_addr) | THINGSET_CAN_SOURCE_SET(target_addr); int ret = isotp_send(&ts_can->send_ctx, ts_can->dev, tx_buf, tx_len, &ts_can->tx_addr, &ts_can->rx_addr, NULL, NULL); @@ -568,7 +584,8 @@ int thingset_can_process_inst(struct thingset_can *ts_can, k_timeout_t timeout) } #endif /* CONFIG_ISOTP_FAST */ -int thingset_can_init_inst(struct thingset_can *ts_can, const struct device *can_dev) +int thingset_can_init_inst(struct thingset_can *ts_can, const struct device *can_dev, + uint8_t bus_number) { struct can_frame tx_frame = { .flags = CAN_FRAME_IDE, @@ -593,18 +610,21 @@ int thingset_can_init_inst(struct thingset_can *ts_can, const struct device *can k_work_init_delayable(&ts_can->addr_claim_work, thingset_can_addr_claim_tx_handler); ts_can->dev = can_dev; + ts_can->bus_number = bus_number; /* set initial address (will be changed if already used on the bus) */ if (ts_can->node_addr < 1 || ts_can->node_addr > THINGSET_CAN_ADDR_MAX) { ts_can->node_addr = 1; } + LOG_WRN("event init"); k_event_init(&ts_can->events); #ifdef CONFIG_CAN_FD_MODE can_set_mode(ts_can->dev, CAN_MODE_FD); #endif + LOG_WRN("can_start"); can_start(ts_can->dev); filter_id = @@ -623,12 +643,17 @@ int thingset_can_init_inst(struct thingset_can *ts_can, const struct device *can | THINGSET_CAN_RAND_SET(rand) | THINGSET_CAN_TARGET_SET(ts_can->node_addr) | THINGSET_CAN_SOURCE_SET(THINGSET_CAN_ADDR_ANONYMOUS); tx_frame.dlc = 0; - err = can_send(ts_can->dev, &tx_frame, K_MSEC(10), NULL, NULL); + LOG_WRN("send discovery"); + err = can_send(ts_can->dev, &tx_frame, K_MSEC(100), /*thingset_can_discovery_tx_cb*/ NULL, + NULL); + LOG_WRN("send discovery finished"); if (err != 0) { + LOG_DBG("Failed to send discovery frame"); k_sleep(K_MSEC(100)); continue; } + LOG_WRN("wait address claiming"); /* wait 500 ms for address claim message from other node */ uint32_t event = k_event_wait(&ts_can->events, EVENT_ADDRESS_ALREADY_USED, false, K_MSEC(500)); @@ -643,6 +668,7 @@ int thingset_can_init_inst(struct thingset_can *ts_can, const struct device *can thingset_sdk_reschedule_work(&ts_can->addr_claim_work, K_NO_WAIT); + LOG_WRN("wait for address claiming"); event = k_event_wait(&ts_can->events, EVENT_ADDRESS_CLAIM_MSG_SENT, false, K_MSEC(100)); if (!(event & EVENT_ADDRESS_CLAIM_MSG_SENT)) { k_sleep(K_MSEC(100)); @@ -665,6 +691,7 @@ int thingset_can_init_inst(struct thingset_can *ts_can, const struct device *can } } + LOG_WRN("storage_save_queued"); #if CONFIG_THINGSET_STORAGE /* save node address as init value for next boot-up */ thingset_storage_save_queued(); @@ -692,12 +719,14 @@ int thingset_can_init_inst(struct thingset_can *ts_can, const struct device *can #ifdef CONFIG_ISOTP_FAST isotp_fast_can_id rx_can_id = THINGSET_CAN_TYPE_CHANNEL | THINGSET_CAN_PRIO_CHANNEL | THINGSET_CAN_TARGET_SET(ts_can->node_addr); + LOG_WRN("isotp_fast_bind"); isotp_fast_bind(&ts_can->ctx, can_dev, rx_can_id, &fc_opts, isotp_fast_recv_callback, ts_can, isotp_fast_recv_error_callback, isotp_fast_sent_callback); #endif thingset_sdk_reschedule_work(&ts_can->reporting_work, K_NO_WAIT); + LOG_WRN("done"); return 0; } @@ -752,17 +781,17 @@ THINGSET_ADD_ITEM_UINT8(TS_ID_NET, TS_ID_NET_CAN_NODE_ADDR, "pCANNodeAddr", &ts_can_single.node_addr, THINGSET_ANY_RW, TS_SUBSET_NVM); #ifdef CONFIG_ISOTP_FAST -int thingset_can_send(uint8_t *tx_buf, size_t tx_len, uint8_t target_addr, +int thingset_can_send(uint8_t *tx_buf, size_t tx_len, uint8_t target_addr, uint8_t target_bus, thingset_can_response_callback_t rsp_callback, void *callback_arg, k_timeout_t timeout) { - return thingset_can_send_inst(&ts_can_single, tx_buf, tx_len, target_addr, rsp_callback, - callback_arg, timeout); + return thingset_can_send_inst(&ts_can_single, tx_buf, tx_len, target_addr, target_bus, + rsp_callback, callback_arg, timeout); } #else -int thingset_can_send(uint8_t *tx_buf, size_t tx_len, uint8_t target_addr) +int thingset_can_send(uint8_t *tx_buf, size_t tx_len, uint8_t target_addr, uint8_t target_bus) { - return thingset_can_send_inst(&ts_can_single, tx_buf, tx_len, target_addr); + return thingset_can_send_inst(&ts_can_single, tx_buf, tx_len, target_addr, target_bus); } #endif /* CONFIG_ISOTP_FAST */ @@ -780,7 +809,7 @@ static void thingset_can_thread() { int err; - err = thingset_can_init_inst(&ts_can_single, can_dev); + err = thingset_can_init_inst(&ts_can_single, can_dev, CONFIG_THINGSET_CAN_BUS_NUMBER); if (err != 0) { LOG_ERR("Failed to init ThingSet CAN: %d", err); return; diff --git a/src/isotp_fast.c b/src/isotp_fast.c index 60e0abd..79e4e10 100644 --- a/src/isotp_fast.c +++ b/src/isotp_fast.c @@ -217,11 +217,13 @@ static void receive_can_tx(const struct device *dev, int error, void *arg) static void receive_send_fc(struct isotp_fast_recv_ctx *rctx, uint8_t fs) { + /* swap bus and address for FC frame */ struct can_frame frame = { .flags = CAN_FRAME_IDE | ((rctx->ctx->opts->flags & ISOTP_MSG_FDF) != 0 ? CAN_FRAME_FDF : 0), - .id = (rctx->rx_can_id & 0xFFFF0000) | ((rctx->rx_can_id & 0xFF00) >> 8) - | ((rctx->rx_can_id & 0xFF) << 8) + .id = (rctx->rx_can_id & 0xFF000000) | ((rctx->rx_can_id & 0x0000FF00) >> 8) + | ((rctx->rx_can_id & 0x000000FF) << 8) | ((rctx->rx_can_id & 0x000F0000) << 4) + | ((rctx->rx_can_id & 0x00F00000) >> 4) }; uint8_t *data = frame.data; uint8_t payload_len; @@ -821,7 +823,7 @@ static inline void prepare_filter(struct can_filter *filter, isotp_fast_can_id r const struct isotp_fast_opts *opts) { filter->id = rx_can_id; - filter->mask = ISOTP_FIXED_ADDR_RX_MASK; + filter->mask = 0x03F0FF00; /* fixed target bus and target address of any priority */ filter->flags = CAN_FILTER_DATA | CAN_FILTER_IDE | ((opts->flags & ISOTP_MSG_FDF) != 0 ? CAN_FILTER_FDF : 0); } @@ -990,11 +992,11 @@ int isotp_fast_recv(struct isotp_fast_ctx *ctx, struct can_filter sender, uint8_ #endif /* CONFIG_ISOTP_FAST_BLOCKING_RECEIVE */ int isotp_fast_send(struct isotp_fast_ctx *ctx, const uint8_t *data, size_t len, - const isotp_fast_node_id target_addr, void *cb_arg) + const isotp_fast_node_id target_addr, uint8_t target_bus, void *cb_arg) { - const isotp_fast_can_id rx_can_id = (ctx->rx_can_id & 0xFFFF0000) - | (isotp_fast_get_target_addr(ctx->rx_can_id)) - | (target_addr << ISOTP_FIXED_ADDR_TA_POS); + const isotp_fast_can_id rx_can_id = + (ctx->rx_can_id & 0xFFFF0000) | (target_bus << CONFIG_ISOTP_FAST_TARGET_BUS_POS) + | (isotp_fast_get_target_addr(ctx->rx_can_id)) | (target_addr << ISOTP_FIXED_ADDR_TA_POS); if (len <= (CAN_MAX_DLEN - ISOTP_FAST_SF_LEN_BYTE)) { struct can_frame frame; prepare_frame(&frame, ctx, rx_can_id);