canbus: isotp: fix net_buf usage in recv function
isotp_recv and the called pull_frags functions were violating the net_buf API by interacting directly with net_buf fragments pulled from a k_fifo. This commit reworks the isotp_recv function. The currently processed net_buf is stored in an additional context variable so that reading from it can be continued in the next call to isotp_recv if not all fragments could be fit into the provided uint8_t *data buffer. Fixes #40070 Signed-off-by: Martin Jäger <martin@libre.solar>
This commit is contained in:
parent
a08e6e1d9a
commit
27fc74feff
2 changed files with 24 additions and 55 deletions
|
@ -406,6 +406,8 @@ struct isotp_recv_ctx {
|
|||
const struct device *can_dev;
|
||||
struct net_buf *buf;
|
||||
struct net_buf *act_frag;
|
||||
/* buffer currently processed in isotp_recv */
|
||||
struct net_buf *recv_buf;
|
||||
sys_snode_t alloc_node;
|
||||
uint32_t length;
|
||||
int error_nr;
|
||||
|
|
|
@ -70,17 +70,6 @@ static void receive_ff_sf_pool_free(struct net_buf *buf)
|
|||
}
|
||||
}
|
||||
|
||||
static inline int _k_fifo_wait_non_empty(struct k_fifo *fifo,
|
||||
k_timeout_t timeout)
|
||||
{
|
||||
struct k_poll_event events[] = {
|
||||
K_POLL_EVENT_INITIALIZER(K_POLL_TYPE_FIFO_DATA_AVAILABLE,
|
||||
K_POLL_MODE_NOTIFY_ONLY, fifo),
|
||||
};
|
||||
|
||||
return k_poll(events, ARRAY_SIZE(events), timeout);
|
||||
}
|
||||
|
||||
static inline void receive_report_error(struct isotp_recv_ctx *ctx, int err)
|
||||
{
|
||||
ctx->state = ISOTP_RX_STATE_ERR;
|
||||
|
@ -692,62 +681,40 @@ int isotp_recv_net(struct isotp_recv_ctx *ctx, struct net_buf **buffer,
|
|||
return *(uint32_t *)net_buf_user_data(buf);
|
||||
}
|
||||
|
||||
static inline void pull_frags(struct k_fifo *fifo, struct net_buf *buf,
|
||||
size_t len)
|
||||
{
|
||||
size_t rem_len = len;
|
||||
struct net_buf *frag = buf;
|
||||
|
||||
/* frags to be removed */
|
||||
while (frag && (frag->len <= rem_len)) {
|
||||
rem_len -= frag->len;
|
||||
frag = frag->frags;
|
||||
k_fifo_get(fifo, K_NO_WAIT);
|
||||
}
|
||||
|
||||
if (frag) {
|
||||
/* Start of frags to be preserved */
|
||||
net_buf_ref(frag);
|
||||
net_buf_pull(frag, rem_len);
|
||||
}
|
||||
|
||||
net_buf_unref(buf);
|
||||
}
|
||||
|
||||
int isotp_recv(struct isotp_recv_ctx *ctx, uint8_t *data, size_t len,
|
||||
k_timeout_t timeout)
|
||||
{
|
||||
size_t num_copied, frags_len;
|
||||
struct net_buf *buf;
|
||||
int ret;
|
||||
size_t copied, to_copy;
|
||||
int err;
|
||||
|
||||
ret = _k_fifo_wait_non_empty(&ctx->fifo, timeout);
|
||||
if (ret) {
|
||||
if (ctx->error_nr) {
|
||||
ret = ctx->error_nr;
|
||||
if (!ctx->recv_buf) {
|
||||
ctx->recv_buf = net_buf_get(&ctx->fifo, timeout);
|
||||
if (!ctx->recv_buf) {
|
||||
err = ctx->error_nr ? ctx->error_nr : ISOTP_RECV_TIMEOUT;
|
||||
ctx->error_nr = 0;
|
||||
return ret;
|
||||
}
|
||||
|
||||
if (ret == -EAGAIN) {
|
||||
return ISOTP_RECV_TIMEOUT;
|
||||
return err;
|
||||
}
|
||||
|
||||
return ISOTP_N_ERROR;
|
||||
}
|
||||
|
||||
buf = k_fifo_peek_head(&ctx->fifo);
|
||||
/* traverse fragments and delete them after copying the data */
|
||||
copied = 0;
|
||||
while (ctx->recv_buf && copied < len) {
|
||||
to_copy = MIN(len - copied, ctx->recv_buf->len);
|
||||
memcpy((uint8_t *)data + copied, ctx->recv_buf->data, to_copy);
|
||||
|
||||
if (!buf) {
|
||||
return ISOTP_N_ERROR;
|
||||
if (ctx->recv_buf->len == to_copy) {
|
||||
/* point recv_buf to next frag */
|
||||
ctx->recv_buf = net_buf_frag_del(NULL, ctx->recv_buf);
|
||||
} else {
|
||||
/* pull received data from remaining frag(s) */
|
||||
net_buf_pull(ctx->recv_buf, to_copy);
|
||||
}
|
||||
|
||||
copied += to_copy;
|
||||
}
|
||||
|
||||
frags_len = net_buf_frags_len(buf);
|
||||
num_copied = net_buf_linearize(data, len, buf, 0, len);
|
||||
|
||||
pull_frags(&ctx->fifo, buf, num_copied);
|
||||
|
||||
return num_copied;
|
||||
return copied;
|
||||
}
|
||||
|
||||
static inline void send_report_error(struct isotp_send_ctx *ctx, uint32_t err)
|
||||
|
|
Loading…
Add table
Add a link
Reference in a new issue