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 #define USE_PACKED_STRUCT
  48
  49 int main(int argc, char **argv)
  50 {
  51         int s;
  52         int err = 0;
  53
  54         struct {
  55                 struct nlmsghdr n;
  56                 struct rtcanmsg r;
  57                 char buf[1000];
  58
  59         } req;
  60
  61         struct can_filter filter;
  62         struct sockaddr_nl nladdr;
  63
  64 #ifdef USE_PACKED_STRUCT
  65         struct modattr {
  66                 struct can_frame cf;
  67                 __u8 modtype;
  68         } __attribute__((packed));
  69
  70         struct modattr modmsg;
  71 #else
  72         static struct can_frame modframe;
  73         char modbuf[CGW_MODATTR_LEN];
  74 #endif
  75
  76         s = socket(PF_NETLINK, SOCK_RAW, NETLINK_ROUTE);
  77
  78         memset(&req, 0, sizeof(req));
  79
  80         req.n.nlmsg_len   = NLMSG_LENGTH(sizeof(struct rtcanmsg));
  81         req.n.nlmsg_flags = NLM_F_REQUEST | NLM_F_CREATE | NLM_F_EXCL;
  82         req.n.nlmsg_type  = RTM_NEWROUTE;
  83         req.n.nlmsg_seq   = 0;
  84
  85         req.r.can_family  = AF_CAN;
  86         req.r.src_ifindex = if_nametoindex("vcan2");
  87         req.r.dst_ifindex = if_nametoindex("vcan3");
  88         req.r.can_txflags = CAN_GW_TXFLAGS_ECHO;
  89
  90         /* add new attributes here */
  91
  92         filter.can_id = 0x400;
  93         filter.can_mask = 0x700;
  94
  95         addattr_l(&req.n, sizeof(req), CGW_FILTER, &filter, sizeof(filter));
  96
  97 #ifdef USE_PACKED_STRUCT
  98
  99         if (sizeof(modmsg) != CGW_MODATTR_LEN) {
 100                 printf("Problem with packed msg. Use linear copy instead.\n");
 101                 return 1;
 102         }
 103
 104         modmsg.cf.can_id  = 0x555;
 105         modmsg.cf.can_dlc = 5;
 106         *(unsigned long long *)modmsg.cf.data = 0x5555555555555555ULL;
 107
 108         modmsg.modtype = CGW_MOD_ID;
 109         addattr_l(&req.n, sizeof(req), CGW_MOD_SET, &modmsg, CGW_MODATTR_LEN);
 110
 111         modmsg.modtype = CGW_MOD_DLC;
 112         addattr_l(&req.n, sizeof(req), CGW_MOD_AND, &modmsg, CGW_MODATTR_LEN);
 113
 114         modmsg.modtype = CGW_MOD_DATA;
 115         addattr_l(&req.n, sizeof(req), CGW_MOD_XOR, &modmsg, CGW_MODATTR_LEN);
 116
 117 #else
 118
 119         modframe.can_id  = 0x555;
 120         modframe.can_dlc = 5;
 121         *(unsigned long long *)modframe.data = 0x5555555555555555ULL;
 122
 123         memcpy(modbuf, &modframe, sizeof(struct can_frame));
 124
 125         modbuf[sizeof(struct can_frame)] = CGW_MOD_ID;
 126         addattr_l(&req.n, sizeof(req), CGW_MOD_SET, modbuf, CGW_MODATTR_LEN);
 127
 128         modbuf[sizeof(struct can_frame)] = CGW_MOD_DLC;
 129         addattr_l(&req.n, sizeof(req), CGW_MOD_AND, modbuf, CGW_MODATTR_LEN);
 130
 131         modbuf[sizeof(struct can_frame)] = CGW_MOD_DATA;
 132         addattr_l(&req.n, sizeof(req), CGW_MOD_XOR, modbuf, CGW_MODATTR_LEN);
 133
 134 #endif
 135
 136         memset(&nladdr, 0, sizeof(nladdr));
 137         nladdr.nl_family = AF_NETLINK;
 138         nladdr.nl_pid    = 0;
 139         nladdr.nl_groups = 0;
 140
 141         err = sendto(s, &req, req.n.nlmsg_len, 0,
 142                      (struct sockaddr*)&nladdr, sizeof(nladdr));
 143
 144         perror("netlink says ");
 145         close(s);
 146
 147         return 0;
 148 }
 149