test/gwtest.c

   1 /*
   2  * A real quick'n'dirty hack to add a netlink CAN gateway entry.
   3  *
   4  * Parts of this code were taken from the iproute source and the original
   5  * vcan.c from Urs Thuermann.
   6  *
   7  * Oliver Hartkopp 2010-02-18
   8  */
   9
  10
  11 #include <stdio.h>
  12 #include <stdlib.h>
  13 #include <string.h>
  14 #include <unistd.h>
  15 #include <sys/socket.h>
  16 #include <net/if.h>
  17
  18 #include <asm/types.h>
  19 #include <linux/netlink.h>
  20 #include <linux/rtnetlink.h>
  21 #include <socketcan/can/gw.h>
  22
  23 #include <linux/if_link.h>
  24
  25 #define NLMSG_TAIL(nmsg) \
  26         ((struct rtattr *)(((void *) (nmsg)) + NLMSG_ALIGN((nmsg)->nlmsg_len)))
  27
  28 int addattr_l(struct nlmsghdr *n, int maxlen, int type, const void *data,
  29               int alen)
  30 {
  31         int len = RTA_LENGTH(alen);
  32         struct rtattr *rta;
  33
  34         if (NLMSG_ALIGN(n->nlmsg_len) + RTA_ALIGN(len) > maxlen) {
  35                 fprintf(stderr, "addattr_l: message exceeded bound of %d\n",
  36                         maxlen);
  37                 return -1;
  38         }
  39         rta = NLMSG_TAIL(n);
  40         rta->rta_type = type;
  41         rta->rta_len = len;
  42         memcpy(RTA_DATA(rta), data, alen);
  43         n->nlmsg_len = NLMSG_ALIGN(n->nlmsg_len) + RTA_ALIGN(len);
  44         return 0;
  45 }
  46
  47 int main(int argc, char **argv)
  48 {
  49         int s;
  50
  51         struct {
  52                 struct nlmsghdr n;
  53                 struct rtcanmsg r;
  54                 char buf[1000];
  55
  56         } req;
  57
  58         struct can_filter filter;
  59         struct sockaddr_nl nladdr;
  60
  61         struct cgw_frame_mod modmsg;
  62
  63         u_int32_t src = if_nametoindex("vcan2");
  64         u_int32_t dst = if_nametoindex("vcan3");
  65
  66         s = socket(PF_NETLINK, SOCK_RAW, NETLINK_ROUTE);
  67
  68         memset(&req, 0, sizeof(req));
  69
  70         req.n.nlmsg_len   = NLMSG_LENGTH(sizeof(struct rtcanmsg));
  71         req.n.nlmsg_flags = NLM_F_REQUEST | NLM_F_CREATE | NLM_F_EXCL;
  72         req.n.nlmsg_type  = RTM_NEWROUTE;
  73         req.n.nlmsg_seq   = 0;
  74
  75         req.r.can_family  = AF_CAN;
  76         req.r.gwtype = CGW_TYPE_CAN_CAN;
  77         req.r.flags = CGW_FLAGS_CAN_ECHO;
  78
  79         addattr_l(&req.n, sizeof(req), CGW_SRC_IF, &src, sizeof(src));
  80         addattr_l(&req.n, sizeof(req), CGW_DST_IF, &dst, sizeof(dst));
  81
  82         /* add new attributes here */
  83
  84         filter.can_id = 0x400;
  85         filter.can_mask = 0x700;
  86
  87         addattr_l(&req.n, sizeof(req), CGW_FILTER, &filter, sizeof(filter));
  88
  89         if (sizeof(modmsg) != CGW_MODATTR_LEN) {
  90                 printf("Problem with packed msg. Use linear copy instead.\n");
  91                 return 1;
  92         }
  93
  94         modmsg.cf.can_id  = 0x555;
  95         modmsg.cf.can_dlc = 5;
  96         *(unsigned long long *)modmsg.cf.data = 0x5555555555555555ULL;
  97
  98         modmsg.modtype = CGW_MOD_ID;
  99         addattr_l(&req.n, sizeof(req), CGW_MOD_SET, &modmsg, CGW_MODATTR_LEN);
 100
 101         modmsg.modtype = CGW_MOD_DLC;
 102         addattr_l(&req.n, sizeof(req), CGW_MOD_AND, &modmsg, CGW_MODATTR_LEN);
 103
 104         modmsg.modtype = CGW_MOD_DATA;
 105         addattr_l(&req.n, sizeof(req), CGW_MOD_XOR, &modmsg, CGW_MODATTR_LEN);
 106
 107         memset(&nladdr, 0, sizeof(nladdr));
 108         nladdr.nl_family = AF_NETLINK;
 109         nladdr.nl_pid    = 0;
 110         nladdr.nl_groups = 0;
 111
 112         sendto(s, &req, req.n.nlmsg_len, 0,
 113                (struct sockaddr*)&nladdr, sizeof(nladdr));
 114
 115         perror("netlink says ");
 116         close(s);
 117
 118         return 0;
 119 }
 120