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         int err = 0;
  51
  52         struct {
  53                 struct nlmsghdr n;
  54                 struct rtcanmsg r;
  55                 char buf[1000];
  56
  57         } req;
  58
  59         struct can_filter filter;
  60         struct sockaddr_nl nladdr;
  61
  62         struct cgw_frame_mod modmsg;
  63
  64         u_int32_t src = if_nametoindex("vcan2");
  65         u_int32_t dst = if_nametoindex("vcan3");
  66
  67         s = socket(PF_NETLINK, SOCK_RAW, NETLINK_ROUTE);
  68
  69         memset(&req, 0, sizeof(req));
  70
  71         req.n.nlmsg_len   = NLMSG_LENGTH(sizeof(struct rtcanmsg));
  72         req.n.nlmsg_flags = NLM_F_REQUEST | NLM_F_CREATE | NLM_F_EXCL;
  73         req.n.nlmsg_type  = RTM_NEWROUTE;
  74         req.n.nlmsg_seq   = 0;
  75
  76         req.r.can_family  = AF_CAN;
  77         req.r.gwtype = CGW_TYPE_CAN_CAN;
  78         req.r.flags = CGW_FLAGS_CAN_ECHO;
  79
  80         addattr_l(&req.n, sizeof(req), CGW_SRC_IF, &src, sizeof(src));
  81         addattr_l(&req.n, sizeof(req), CGW_DST_IF, &dst, sizeof(dst));
  82
  83         /* add new attributes here */
  84
  85         filter.can_id = 0x400;
  86         filter.can_mask = 0x700;
  87
  88         addattr_l(&req.n, sizeof(req), CGW_FILTER, &filter, sizeof(filter));
  89
  90         if (sizeof(modmsg) != CGW_MODATTR_LEN) {
  91                 printf("Problem with packed msg. Use linear copy instead.\n");
  92                 return 1;
  93         }
  94
  95         modmsg.cf.can_id  = 0x555;
  96         modmsg.cf.can_dlc = 5;
  97         *(unsigned long long *)modmsg.cf.data = 0x5555555555555555ULL;
  98
  99         modmsg.modtype = CGW_MOD_ID;
 100         addattr_l(&req.n, sizeof(req), CGW_MOD_SET, &modmsg, CGW_MODATTR_LEN);
 101
 102         modmsg.modtype = CGW_MOD_DLC;
 103         addattr_l(&req.n, sizeof(req), CGW_MOD_AND, &modmsg, CGW_MODATTR_LEN);
 104
 105         modmsg.modtype = CGW_MOD_DATA;
 106         addattr_l(&req.n, sizeof(req), CGW_MOD_XOR, &modmsg, CGW_MODATTR_LEN);
 107
 108         memset(&nladdr, 0, sizeof(nladdr));
 109         nladdr.nl_family = AF_NETLINK;
 110         nladdr.nl_pid    = 0;
 111         nladdr.nl_groups = 0;
 112
 113         err = sendto(s, &req, req.n.nlmsg_len, 0,
 114                      (struct sockaddr*)&nladdr, sizeof(nladdr));
 115
 116         perror("netlink says ");
 117         close(s);
 118
 119         return 0;
 120 }
 121