Skip to content
Permalink
Browse files

net: sockets: can: Add dispatcher

We need to dispatch the received CAN frame if there are multiple
sockets interested in the same CAN-IDs.

Signed-off-by: Jukka Rissanen <jukka.rissanen@linux.intel.com>
  • Loading branch information...
jukkar committed Feb 12, 2019
1 parent 67f0550 commit 6621491014741cd96e713717c8a48abb7088b426
Showing with 267 additions and 50 deletions.
  1. +7 −0 subsys/net/ip/net_context.c
  2. +8 −0 subsys/net/lib/sockets/Kconfig
  3. +252 −50 subsys/net/lib/sockets/sockets_can.c
@@ -1708,6 +1708,13 @@ int net_context_recv(struct net_context *context,
} else if (IS_ENABLED(CONFIG_NET_SOCKETS_CAN) &&
net_context_get_family(context) == AF_CAN) {
ret = recv_raw(context, cb, timeout, user_data);
if (ret == -EALREADY) {
/* This is perfectly normal for CAN sockets.
* The SocketCAN will dispatch the packet to
* correct net_context listener.
*/
ret = 0;
}
} else {
ret = -EPROTOTYPE;
}
@@ -124,6 +124,14 @@ config NET_SOCKETS_CAN
help
The value depends on your network needs.

config NET_SOCKETS_CAN_RECEIVERS
int "How many simultaneous SocketCAN receivers are allowed"
default 1
depends on NET_SOCKETS_CAN
help
The value tells how many sockets can receive data from same
Socket-CAN interface.

module = NET_SOCKETS
module-dep = NET_LOG
module-str = Log level for BSD sockets compatible API calls
@@ -22,6 +22,17 @@ LOG_MODULE_REGISTER(net_sock_can, CONFIG_NET_SOCKETS_LOG_LEVEL);

#include "sockets_internal.h"

#define MEM_ALLOC_TIMEOUT K_MSEC(50)

struct can_recv {
struct net_if *iface;
struct net_context *ctx;
canid_t can_id;
canid_t can_mask;
};

static struct can_recv receivers[CONFIG_NET_SOCKETS_CAN_RECEIVERS];

extern const struct socket_op_vtable sock_fd_op_vtable;

static const struct socket_op_vtable can_sock_fd_op_vtable;
@@ -77,32 +88,87 @@ static void zcan_received_cb(struct net_context *ctx, struct net_pkt *pkt,
union net_proto_header *proto_hdr,
int status, void *user_data)
{
NET_DBG("ctx %p pkt %p st %d ud %p", ctx, pkt, status, user_data);
/* The ctx parameter is not really relevant here. It refers to first
* net_context that was used when registering CAN socket.
* In practice there can be multiple sockets that are interested in
* same CAN id packets. That is why we need to implement the dispatcher
* which will give the packet to correct net_context(s).
*/
struct net_pkt *clone = NULL;
int i;

/* if pkt is NULL, EOF */
if (!pkt) {
struct net_pkt *last_pkt = k_fifo_peek_tail(&ctx->recv_q);
for (i = 0; i < ARRAY_SIZE(receivers); i++) {
struct zcan_frame *zframe =
(struct zcan_frame *)net_pkt_data(pkt);
struct can_frame frame;

if (receivers[i].iface != net_pkt_iface(pkt)) {
continue;
}

can_copy_zframe_to_frame(zframe, &frame);

if (!last_pkt) {
/* If there're no packets in the queue, recv() may
* be blocked waiting on it to become non-empty,
* so cancel that wait.
if ((frame.can_id & receivers[i].can_mask) !=
(receivers[i].can_id & receivers[i].can_mask)) {
continue;
}

/* If there are multiple receivers configured, we use the
* original net_pkt as a template, and just clone it to all
* recipients. This is done like this so that we avoid the
* original net_pkt being freed while we are cloning it.
*/
if (pkt != NULL && ARRAY_SIZE(receivers) > 1) {
/* There are multiple receivers, we need to clone
* the packet.
*/
sock_set_eof(ctx);
k_fifo_cancel_wait(&ctx->recv_q);
NET_DBG("Marked socket %p as peer-closed", ctx);
clone = net_pkt_clone(pkt, MEM_ALLOC_TIMEOUT);
if (!clone) {
/* Sent the packet to at least one recipient
* if there is no memory to clone the packet.
*/
clone = pkt;
}
} else {
net_pkt_set_eof(last_pkt, true);
NET_DBG("Set EOF flag on pkt %p", ctx);
clone = pkt;
}

return;
}
ctx = receivers[i].ctx;

NET_DBG("[%d] ctx %p pkt %p st %d", i, ctx, clone, status);

/* if pkt is NULL, EOF */
if (!clone) {
struct net_pkt *last_pkt =
k_fifo_peek_tail(&ctx->recv_q);

if (!last_pkt) {
/* If there're no packets in the queue,
* recv() may be blocked waiting on it to
* become non-empty, so cancel that wait.
*/
sock_set_eof(ctx);
k_fifo_cancel_wait(&ctx->recv_q);

NET_DBG("Marked socket %p as peer-closed", ctx);
} else {
net_pkt_set_eof(last_pkt, true);

NET_DBG("Set EOF flag on pkt %p", ctx);
}

return;
} else {
/* Normal packet */
net_pkt_set_eof(clone, false);

/* Normal packet */
net_pkt_set_eof(pkt, false);
k_fifo_put(&ctx->recv_q, clone);
}
}

k_fifo_put(&ctx->recv_q, pkt);
if (clone && clone != pkt) {
net_pkt_unref(pkt);
}
}

static int zcan_bind_ctx(struct net_context *ctx, const struct sockaddr *addr,
@@ -232,20 +298,18 @@ static ssize_t zcan_recvfrom_ctx(struct net_context *ctx, void *buf,
}

if (net_pkt_read(pkt, (void *)&zframe, sizeof(zframe))) {
net_pkt_unref(pkt);

errno = EIO;
return -1;
}

if (!(flags & ZSOCK_MSG_PEEK)) {
net_pkt_unref(pkt);
} else {
net_pkt_cursor_init(pkt);
}

NET_ASSERT(recv_len == sizeof(struct can_frame));

can_copy_zframe_to_frame(&zframe, (struct can_frame *)buf);

net_pkt_unref(pkt);

return recv_len;
}

@@ -361,52 +425,190 @@ static int can_sock_getsockopt_vmeth(void *obj, int level, int optname,
return zcan_getsockopt_ctx(obj, level, optname, optval, optlen);
}

static int can_register_receiver(struct net_if *iface, struct net_context *ctx,
canid_t can_id, canid_t can_mask)
{
int i;

NET_DBG("Max %lu receivers", ARRAY_SIZE(receivers));

for (i = 0; i < ARRAY_SIZE(receivers); i++) {
if (receivers[i].ctx != NULL) {
continue;
}

receivers[i].ctx = ctx;
receivers[i].iface = iface;
receivers[i].can_id = can_id;
receivers[i].can_mask = can_mask;

return i;
}

return -ENOENT;
}

static void can_unregister_receiver(struct net_if *iface,
struct net_context *ctx,
canid_t can_id, canid_t can_mask)
{
int i;

for (i = 0; i < ARRAY_SIZE(receivers); i++) {
if (receivers[i].ctx == ctx &&
receivers[i].iface == iface &&
receivers[i].can_id == can_id &&
receivers[i].can_mask == can_mask) {
receivers[i].ctx = NULL;
return;
}
}
}

static int can_register_filters(struct net_if *iface, struct net_context *ctx,
const struct can_filter *filters, int count)
{
int i, ret;

NET_DBG("Registering %d filters", count);

for (i = 0; i < count; i++) {
ret = can_register_receiver(iface, ctx, filters[i].can_id,
filters[i].can_mask);
if (ret < 0) {
goto revert;
}
}

return 0;

revert:
for (i = 0; i < count; i++) {
can_unregister_receiver(iface, ctx, filters[i].can_id,
filters[i].can_mask);
}

return ret;
}

static void can_unregister_filters(struct net_if *iface,
struct net_context *ctx,
const struct can_filter *filters,
int count)
{
int i;

NET_DBG("Unregistering %d filters", count);

for (i = 0; i < count; i++) {
can_unregister_receiver(iface, ctx, filters[i].can_id,
filters[i].can_mask);
}
}

static bool is_already_attached(struct can_filter *filter,
struct net_if *iface,
struct net_context *ctx)
{
int i;

for (i = 0; i < ARRAY_SIZE(receivers); i++) {
if (receivers[i].ctx != ctx && receivers[i].iface == iface &&
((receivers[i].can_id & receivers[i].can_mask) ==
(UNALIGNED_GET(&filter->can_id) &
UNALIGNED_GET(&filter->can_mask)))) {
return true;
}
}

return false;
}

static int can_sock_setsockopt_vmeth(void *obj, int level, int optname,
const void *optval, socklen_t optlen)
{
if (level == SOL_CAN_RAW) {
const struct canbus_api *api;
struct net_if *iface;
struct device *dev;
const struct canbus_api *api;
struct net_if *iface;
struct device *dev;
int ret;

/* The application must use can_filter and then we convert
* it to zcan_filter as the CANBUS drivers expects that.
*/
if (optname == CAN_RAW_FILTER &&
optlen != sizeof(struct can_filter)) {
errno = EINVAL;
return -1;
}
if (level != SOL_CAN_RAW) {
return zcan_setsockopt_ctx(obj, level, optname, optval, optlen);
}

if (optval == NULL) {
/* The application must use CAN_filter and then we convert
* it to zcan_filter as the CANBUS drivers expects that.
*/
if (optname == CAN_RAW_FILTER && optlen != sizeof(struct can_filter)) {
errno = EINVAL;
return -1;
}

if (optval == NULL) {
errno = EINVAL;
return -1;
}

iface = net_context_get_iface(obj);
dev = net_if_get_device(iface);
api = dev->driver_api;

if (!api || !api->setsockopt) {
errno = ENOTSUP;
return -1;
}

if (optname == CAN_RAW_FILTER) {
int count, i;

if (optlen % sizeof(struct can_filter) != 0) {
errno = EINVAL;
return -1;
}

iface = net_context_get_iface(obj);
dev = net_if_get_device(iface);
api = dev->driver_api;
count = optlen / sizeof(struct can_filter);

if (!api || !api->setsockopt) {
errno = ENOTSUP;
ret = can_register_filters(iface, obj, optval, count);
if (ret < 0) {
errno = -ret;
return -1;
}

if (optname == CAN_RAW_FILTER) {
for (i = 0; i < count; i++) {
struct can_filter *filter;
struct zcan_filter zfilter;
bool duplicate;

can_copy_filter_to_zfilter((struct can_filter *)optval,
&zfilter);
filter = &((struct can_filter *)optval)[i];

return api->setsockopt(dev, obj, level, optname,
&zfilter, sizeof(zfilter));
/* If someone has already attached the same filter to
* same interface, we do not need to do it here again.
*/
duplicate = is_already_attached(filter, iface, obj);
if (duplicate) {
continue;
}

can_copy_filter_to_zfilter(filter, &zfilter);

ret = api->setsockopt(dev, obj, level, optname,
&zfilter, sizeof(zfilter));
if (ret < 0) {
break;
}
}

if (ret < 0) {
can_unregister_filters(iface, obj, optval, count);

errno = -ret;
return -1;
}

return api->setsockopt(dev, obj, level, optname,
optval, optlen);
return 0;
}

return zcan_setsockopt_ctx(obj, level, optname, optval, optlen);
return api->setsockopt(dev, obj, level, optname, optval, optlen);
}

static const struct socket_op_vtable can_sock_fd_op_vtable = {

0 comments on commit 6621491

Please sign in to comment.
You can’t perform that action at this time.