← Back to branch summary

~ubuntu-branches/ubuntu/precise/linux-ti-omap4/precise-security

« back to all changes in this revision

Viewing changes to net/can/gw.c

  • Committer: Package Import Robot
  • Author(s): Paolo Pisati, Paolo Pisati
  • Date: 2011-12-06 15:56:07 UTC
  • Revision ID: package-import@ubuntu.com-20111206155607-pcf44kv5fmhk564f
Tags: 3.2.0-1401.1
[ Paolo Pisati ]

* Rebased on top of Ubuntu-3.2.0-3.8
* Tilt-tracking @ ef2487af4bb15bdd0689631774b5a5e3a59f74e2
* Delete debian.ti-omap4/control, it shoudln't be tracked
* Fix architecture spelling (s/armel/armhf/)
* [Config] Update configs following 3.2 import
* [Config] Fix compilation: disable CODA and ARCH_OMAP3
* [Config] Fix compilation: disable Ethernet Faraday
* Update series to precise

Show diffs side-by-side

added added

removed removed

Lines of Context:
net/can/gw.c
 
1
/*
 
2
 * gw.c - CAN frame Gateway/Router/Bridge with netlink interface
 
3
 *
 
4
 * Copyright (c) 2011 Volkswagen Group Electronic Research
 
5
 * All rights reserved.
 
6
 *
 
7
 * Redistribution and use in source and binary forms, with or without
 
8
 * modification, are permitted provided that the following conditions
 
9
 * are met:
 
10
 * 1. Redistributions of source code must retain the above copyright
 
11
 *    notice, this list of conditions and the following disclaimer.
 
12
 * 2. Redistributions in binary form must reproduce the above copyright
 
13
 *    notice, this list of conditions and the following disclaimer in the
 
14
 *    documentation and/or other materials provided with the distribution.
 
15
 * 3. Neither the name of Volkswagen nor the names of its contributors
 
16
 *    may be used to endorse or promote products derived from this software
 
17
 *    without specific prior written permission.
 
18
 *
 
19
 * Alternatively, provided that this notice is retained in full, this
 
20
 * software may be distributed under the terms of the GNU General
 
21
 * Public License ("GPL") version 2, in which case the provisions of the
 
22
 * GPL apply INSTEAD OF those given above.
 
23
 *
 
24
 * The provided data structures and external interfaces from this code
 
25
 * are not restricted to be used by modules with a GPL compatible license.
 
26
 *
 
27
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 
28
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 
29
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
 
30
 * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
 
31
 * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
 
32
 * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
 
33
 * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
 
34
 * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
 
35
 * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 
36
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 
37
 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
 
38
 * DAMAGE.
 
39
 *
 
40
 */
 
41
 
 
42
#include <linux/module.h>
 
43
#include <linux/init.h>
 
44
#include <linux/types.h>
 
45
#include <linux/list.h>
 
46
#include <linux/spinlock.h>
 
47
#include <linux/rcupdate.h>
 
48
#include <linux/rculist.h>
 
49
#include <linux/net.h>
 
50
#include <linux/netdevice.h>
 
51
#include <linux/if_arp.h>
 
52
#include <linux/skbuff.h>
 
53
#include <linux/can.h>
 
54
#include <linux/can/core.h>
 
55
#include <linux/can/gw.h>
 
56
#include <net/rtnetlink.h>
 
57
#include <net/net_namespace.h>
 
58
#include <net/sock.h>
 
59
 
 
60
#define CAN_GW_VERSION "20101209"
 
61
static __initdata const char banner[] =
 
62
        KERN_INFO "can: netlink gateway (rev " CAN_GW_VERSION ")\n";
 
63
 
 
64
MODULE_DESCRIPTION("PF_CAN netlink gateway");
 
65
MODULE_LICENSE("Dual BSD/GPL");
 
66
MODULE_AUTHOR("Oliver Hartkopp <oliver.hartkopp@volkswagen.de>");
 
67
MODULE_ALIAS("can-gw");
 
68
 
 
69
HLIST_HEAD(cgw_list);
 
70
static struct notifier_block notifier;
 
71
 
 
72
static struct kmem_cache *cgw_cache __read_mostly;
 
73
 
 
74
/* structure that contains the (on-the-fly) CAN frame modifications */
 
75
struct cf_mod {
 
76
        struct {
 
77
                struct can_frame and;
 
78
                struct can_frame or;
 
79
                struct can_frame xor;
 
80
                struct can_frame set;
 
81
        } modframe;
 
82
        struct {
 
83
                u8 and;
 
84
                u8 or;
 
85
                u8 xor;
 
86
                u8 set;
 
87
        } modtype;
 
88
        void (*modfunc[MAX_MODFUNCTIONS])(struct can_frame *cf,
 
89
                                          struct cf_mod *mod);
 
90
 
 
91
        /* CAN frame checksum calculation after CAN frame modifications */
 
92
        struct {
 
93
                struct cgw_csum_xor xor;
 
94
                struct cgw_csum_crc8 crc8;
 
95
        } csum;
 
96
        struct {
 
97
                void (*xor)(struct can_frame *cf, struct cgw_csum_xor *xor);
 
98
                void (*crc8)(struct can_frame *cf, struct cgw_csum_crc8 *crc8);
 
99
        } csumfunc;
 
100
};
 
101
 
 
102
 
 
103
/*
 
104
 * So far we just support CAN -> CAN routing and frame modifications.
 
105
 *
 
106
 * The internal can_can_gw structure contains data and attributes for
 
107
 * a CAN -> CAN gateway job.
 
108
 */
 
109
struct can_can_gw {
 
110
        struct can_filter filter;
 
111
        int src_idx;
 
112
        int dst_idx;
 
113
};
 
114
 
 
115
/* list entry for CAN gateways jobs */
 
116
struct cgw_job {
 
117
        struct hlist_node list;
 
118
        struct rcu_head rcu;
 
119
        u32 handled_frames;
 
120
        u32 dropped_frames;
 
121
        struct cf_mod mod;
 
122
        union {
 
123
                /* CAN frame data source */
 
124
                struct net_device *dev;
 
125
        } src;
 
126
        union {
 
127
                /* CAN frame data destination */
 
128
                struct net_device *dev;
 
129
        } dst;
 
130
        union {
 
131
                struct can_can_gw ccgw;
 
132
                /* tbc */
 
133
        };
 
134
        u8 gwtype;
 
135
        u16 flags;
 
136
};
 
137
 
 
138
/* modification functions that are invoked in the hot path in can_can_gw_rcv */
 
139
 
 
140
#define MODFUNC(func, op) static void func(struct can_frame *cf, \
 
141
                                           struct cf_mod *mod) { op ; }
 
142
 
 
143
MODFUNC(mod_and_id, cf->can_id &= mod->modframe.and.can_id)
 
144
MODFUNC(mod_and_dlc, cf->can_dlc &= mod->modframe.and.can_dlc)
 
145
MODFUNC(mod_and_data, *(u64 *)cf->data &= *(u64 *)mod->modframe.and.data)
 
146
MODFUNC(mod_or_id, cf->can_id |= mod->modframe.or.can_id)
 
147
MODFUNC(mod_or_dlc, cf->can_dlc |= mod->modframe.or.can_dlc)
 
148
MODFUNC(mod_or_data, *(u64 *)cf->data |= *(u64 *)mod->modframe.or.data)
 
149
MODFUNC(mod_xor_id, cf->can_id ^= mod->modframe.xor.can_id)
 
150
MODFUNC(mod_xor_dlc, cf->can_dlc ^= mod->modframe.xor.can_dlc)
 
151
MODFUNC(mod_xor_data, *(u64 *)cf->data ^= *(u64 *)mod->modframe.xor.data)
 
152
MODFUNC(mod_set_id, cf->can_id = mod->modframe.set.can_id)
 
153
MODFUNC(mod_set_dlc, cf->can_dlc = mod->modframe.set.can_dlc)
 
154
MODFUNC(mod_set_data, *(u64 *)cf->data = *(u64 *)mod->modframe.set.data)
 
155
 
 
156
static inline void canframecpy(struct can_frame *dst, struct can_frame *src)
 
157
{
 
158
        /*
 
159
         * Copy the struct members separately to ensure that no uninitialized
 
160
         * data are copied in the 3 bytes hole of the struct. This is needed
 
161
         * to make easy compares of the data in the struct cf_mod.
 
162
         */
 
163
 
 
164
        dst->can_id = src->can_id;
 
165
        dst->can_dlc = src->can_dlc;
 
166
        *(u64 *)dst->data = *(u64 *)src->data;
 
167
}
 
168
 
 
169
static int cgw_chk_csum_parms(s8 fr, s8 to, s8 re)
 
170
{
 
171
        /*
 
172
         * absolute dlc values 0 .. 7 => 0 .. 7, e.g. data [0]
 
173
         * relative to received dlc -1 .. -8 :
 
174
         * e.g. for received dlc = 8
 
175
         * -1 => index = 7 (data[7])
 
176
         * -3 => index = 5 (data[5])
 
177
         * -8 => index = 0 (data[0])
 
178
         */
 
179
 
 
180
        if (fr > -9 && fr < 8 &&
 
181
            to > -9 && to < 8 &&
 
182
            re > -9 && re < 8)
 
183
                return 0;
 
184
        else
 
185
                return -EINVAL;
 
186
}
 
187
 
 
188
static inline int calc_idx(int idx, int rx_dlc)
 
189
{
 
190
        if (idx < 0)
 
191
                return rx_dlc + idx;
 
192
        else
 
193
                return idx;
 
194
}
 
195
 
 
196
static void cgw_csum_xor_rel(struct can_frame *cf, struct cgw_csum_xor *xor)
 
197
{
 
198
        int from = calc_idx(xor->from_idx, cf->can_dlc);
 
199
        int to = calc_idx(xor->to_idx, cf->can_dlc);
 
200
        int res = calc_idx(xor->result_idx, cf->can_dlc);
 
201
        u8 val = xor->init_xor_val;
 
202
        int i;
 
203
 
 
204
        if (from < 0 || to < 0 || res < 0)
 
205
                return;
 
206
 
 
207
        if (from <= to) {
 
208
                for (i = from; i <= to; i++)
 
209
                        val ^= cf->data[i];
 
210
        } else {
 
211
                for (i = from; i >= to; i--)
 
212
                        val ^= cf->data[i];
 
213
        }
 
214
 
 
215
        cf->data[res] = val;
 
216
}
 
217
 
 
218
static void cgw_csum_xor_pos(struct can_frame *cf, struct cgw_csum_xor *xor)
 
219
{
 
220
        u8 val = xor->init_xor_val;
 
221
        int i;
 
222
 
 
223
        for (i = xor->from_idx; i <= xor->to_idx; i++)
 
224
                val ^= cf->data[i];
 
225
 
 
226
        cf->data[xor->result_idx] = val;
 
227
}
 
228
 
 
229
static void cgw_csum_xor_neg(struct can_frame *cf, struct cgw_csum_xor *xor)
 
230
{
 
231
        u8 val = xor->init_xor_val;
 
232
        int i;
 
233
 
 
234
        for (i = xor->from_idx; i >= xor->to_idx; i--)
 
235
                val ^= cf->data[i];
 
236
 
 
237
        cf->data[xor->result_idx] = val;
 
238
}
 
239
 
 
240
static void cgw_csum_crc8_rel(struct can_frame *cf, struct cgw_csum_crc8 *crc8)
 
241
{
 
242
        int from = calc_idx(crc8->from_idx, cf->can_dlc);
 
243
        int to = calc_idx(crc8->to_idx, cf->can_dlc);
 
244
        int res = calc_idx(crc8->result_idx, cf->can_dlc);
 
245
        u8 crc = crc8->init_crc_val;
 
246
        int i;
 
247
 
 
248
        if (from < 0 || to < 0 || res < 0)
 
249
                return;
 
250
 
 
251
        if (from <= to) {
 
252
                for (i = crc8->from_idx; i <= crc8->to_idx; i++)
 
253
                        crc = crc8->crctab[crc^cf->data[i]];
 
254
        } else {
 
255
                for (i = crc8->from_idx; i >= crc8->to_idx; i--)
 
256
                        crc = crc8->crctab[crc^cf->data[i]];
 
257
        }
 
258
 
 
259
        switch (crc8->profile) {
 
260
 
 
261
        case CGW_CRC8PRF_1U8:
 
262
                crc = crc8->crctab[crc^crc8->profile_data[0]];
 
263
                break;
 
264
 
 
265
        case  CGW_CRC8PRF_16U8:
 
266
                crc = crc8->crctab[crc^crc8->profile_data[cf->data[1] & 0xF]];
 
267
                break;
 
268
 
 
269
        case CGW_CRC8PRF_SFFID_XOR:
 
270
                crc = crc8->crctab[crc^(cf->can_id & 0xFF)^
 
271
                                   (cf->can_id >> 8 & 0xFF)];
 
272
                break;
 
273
 
 
274
        }
 
275
 
 
276
        cf->data[crc8->result_idx] = crc^crc8->final_xor_val;
 
277
}
 
278
 
 
279
static void cgw_csum_crc8_pos(struct can_frame *cf, struct cgw_csum_crc8 *crc8)
 
280
{
 
281
        u8 crc = crc8->init_crc_val;
 
282
        int i;
 
283
 
 
284
        for (i = crc8->from_idx; i <= crc8->to_idx; i++)
 
285
                crc = crc8->crctab[crc^cf->data[i]];
 
286
 
 
287
        switch (crc8->profile) {
 
288
 
 
289
        case CGW_CRC8PRF_1U8:
 
290
                crc = crc8->crctab[crc^crc8->profile_data[0]];
 
291
                break;
 
292
 
 
293
        case  CGW_CRC8PRF_16U8:
 
294
                crc = crc8->crctab[crc^crc8->profile_data[cf->data[1] & 0xF]];
 
295
                break;
 
296
 
 
297
        case CGW_CRC8PRF_SFFID_XOR:
 
298
                crc = crc8->crctab[crc^(cf->can_id & 0xFF)^
 
299
                                   (cf->can_id >> 8 & 0xFF)];
 
300
                break;
 
301
        }
 
302
 
 
303
        cf->data[crc8->result_idx] = crc^crc8->final_xor_val;
 
304
}
 
305
 
 
306
static void cgw_csum_crc8_neg(struct can_frame *cf, struct cgw_csum_crc8 *crc8)
 
307
{
 
308
        u8 crc = crc8->init_crc_val;
 
309
        int i;
 
310
 
 
311
        for (i = crc8->from_idx; i >= crc8->to_idx; i--)
 
312
                crc = crc8->crctab[crc^cf->data[i]];
 
313
 
 
314
        switch (crc8->profile) {
 
315
 
 
316
        case CGW_CRC8PRF_1U8:
 
317
                crc = crc8->crctab[crc^crc8->profile_data[0]];
 
318
                break;
 
319
 
 
320
        case  CGW_CRC8PRF_16U8:
 
321
                crc = crc8->crctab[crc^crc8->profile_data[cf->data[1] & 0xF]];
 
322
                break;
 
323
 
 
324
        case CGW_CRC8PRF_SFFID_XOR:
 
325
                crc = crc8->crctab[crc^(cf->can_id & 0xFF)^
 
326
                                   (cf->can_id >> 8 & 0xFF)];
 
327
                break;
 
328
        }
 
329
 
 
330
        cf->data[crc8->result_idx] = crc^crc8->final_xor_val;
 
331
}
 
332
 
 
333
/* the receive & process & send function */
 
334
static void can_can_gw_rcv(struct sk_buff *skb, void *data)
 
335
{
 
336
        struct cgw_job *gwj = (struct cgw_job *)data;
 
337
        struct can_frame *cf;
 
338
        struct sk_buff *nskb;
 
339
        int modidx = 0;
 
340
 
 
341
        /* do not handle already routed frames - see comment below */
 
342
        if (skb_mac_header_was_set(skb))
 
343
                return;
 
344
 
 
345
        if (!(gwj->dst.dev->flags & IFF_UP)) {
 
346
                gwj->dropped_frames++;
 
347
                return;
 
348
        }
 
349
 
 
350
        /*
 
351
         * clone the given skb, which has not been done in can_rcv()
 
352
         *
 
353
         * When there is at least one modification function activated,
 
354
         * we need to copy the skb as we want to modify skb->data.
 
355
         */
 
356
        if (gwj->mod.modfunc[0])
 
357
                nskb = skb_copy(skb, GFP_ATOMIC);
 
358
        else
 
359
                nskb = skb_clone(skb, GFP_ATOMIC);
 
360
 
 
361
        if (!nskb) {
 
362
                gwj->dropped_frames++;
 
363
                return;
 
364
        }
 
365
 
 
366
        /*
 
367
         * Mark routed frames by setting some mac header length which is
 
368
         * not relevant for the CAN frames located in the skb->data section.
 
369
         *
 
370
         * As dev->header_ops is not set in CAN netdevices no one is ever
 
371
         * accessing the various header offsets in the CAN skbuffs anyway.
 
372
         * E.g. using the packet socket to read CAN frames is still working.
 
373
         */
 
374
        skb_set_mac_header(nskb, 8);
 
375
        nskb->dev = gwj->dst.dev;
 
376
 
 
377
        /* pointer to modifiable CAN frame */
 
378
        cf = (struct can_frame *)nskb->data;
 
379
 
 
380
        /* perform preprocessed modification functions if there are any */
 
381
        while (modidx < MAX_MODFUNCTIONS && gwj->mod.modfunc[modidx])
 
382
                (*gwj->mod.modfunc[modidx++])(cf, &gwj->mod);
 
383
 
 
384
        /* check for checksum updates when the CAN frame has been modified */
 
385
        if (modidx) {
 
386
                if (gwj->mod.csumfunc.crc8)
 
387
                        (*gwj->mod.csumfunc.crc8)(cf, &gwj->mod.csum.crc8);
 
388
 
 
389
                if (gwj->mod.csumfunc.xor)
 
390
                        (*gwj->mod.csumfunc.xor)(cf, &gwj->mod.csum.xor);
 
391
        }
 
392
 
 
393
        /* clear the skb timestamp if not configured the other way */
 
394
        if (!(gwj->flags & CGW_FLAGS_CAN_SRC_TSTAMP))
 
395
                nskb->tstamp.tv64 = 0;
 
396
 
 
397
        /* send to netdevice */
 
398
        if (can_send(nskb, gwj->flags & CGW_FLAGS_CAN_ECHO))
 
399
                gwj->dropped_frames++;
 
400
        else
 
401
                gwj->handled_frames++;
 
402
}
 
403
 
 
404
static inline int cgw_register_filter(struct cgw_job *gwj)
 
405
{
 
406
        return can_rx_register(gwj->src.dev, gwj->ccgw.filter.can_id,
 
407
                               gwj->ccgw.filter.can_mask, can_can_gw_rcv,
 
408
                               gwj, "gw");
 
409
}
 
410
 
 
411
static inline void cgw_unregister_filter(struct cgw_job *gwj)
 
412
{
 
413
        can_rx_unregister(gwj->src.dev, gwj->ccgw.filter.can_id,
 
414
                          gwj->ccgw.filter.can_mask, can_can_gw_rcv, gwj);
 
415
}
 
416
 
 
417
static int cgw_notifier(struct notifier_block *nb,
 
418
                        unsigned long msg, void *data)
 
419
{
 
420
        struct net_device *dev = (struct net_device *)data;
 
421
 
 
422
        if (!net_eq(dev_net(dev), &init_net))
 
423
                return NOTIFY_DONE;
 
424
        if (dev->type != ARPHRD_CAN)
 
425
                return NOTIFY_DONE;
 
426
 
 
427
        if (msg == NETDEV_UNREGISTER) {
 
428
 
 
429
                struct cgw_job *gwj = NULL;
 
430
                struct hlist_node *n, *nx;
 
431
 
 
432
                ASSERT_RTNL();
 
433
 
 
434
                hlist_for_each_entry_safe(gwj, n, nx, &cgw_list, list) {
 
435
 
 
436
                        if (gwj->src.dev == dev || gwj->dst.dev == dev) {
 
437
                                hlist_del(&gwj->list);
 
438
                                cgw_unregister_filter(gwj);
 
439
                                kfree(gwj);
 
440
                        }
 
441
                }
 
442
        }
 
443
 
 
444
        return NOTIFY_DONE;
 
445
}
 
446
 
 
447
static int cgw_put_job(struct sk_buff *skb, struct cgw_job *gwj)
 
448
{
 
449
        struct cgw_frame_mod mb;
 
450
        struct rtcanmsg *rtcan;
 
451
        struct nlmsghdr *nlh = nlmsg_put(skb, 0, 0, 0, sizeof(*rtcan), 0);
 
452
        if (!nlh)
 
453
                return -EMSGSIZE;
 
454
 
 
455
        rtcan = nlmsg_data(nlh);
 
456
        rtcan->can_family = AF_CAN;
 
457
        rtcan->gwtype = gwj->gwtype;
 
458
        rtcan->flags = gwj->flags;
 
459
 
 
460
        /* add statistics if available */
 
461
 
 
462
        if (gwj->handled_frames) {
 
463
                if (nla_put_u32(skb, CGW_HANDLED, gwj->handled_frames) < 0)
 
464
                        goto cancel;
 
465
                else
 
466
                        nlh->nlmsg_len += NLA_HDRLEN + NLA_ALIGN(sizeof(u32));
 
467
        }
 
468
 
 
469
        if (gwj->dropped_frames) {
 
470
                if (nla_put_u32(skb, CGW_DROPPED, gwj->dropped_frames) < 0)
 
471
                        goto cancel;
 
472
                else
 
473
                        nlh->nlmsg_len += NLA_HDRLEN + NLA_ALIGN(sizeof(u32));
 
474
        }
 
475
 
 
476
        /* check non default settings of attributes */
 
477
 
 
478
        if (gwj->mod.modtype.and) {
 
479
                memcpy(&mb.cf, &gwj->mod.modframe.and, sizeof(mb.cf));
 
480
                mb.modtype = gwj->mod.modtype.and;
 
481
                if (nla_put(skb, CGW_MOD_AND, sizeof(mb), &mb) < 0)
 
482
                        goto cancel;
 
483
                else
 
484
                        nlh->nlmsg_len += NLA_HDRLEN + NLA_ALIGN(sizeof(mb));
 
485
        }
 
486
 
 
487
        if (gwj->mod.modtype.or) {
 
488
                memcpy(&mb.cf, &gwj->mod.modframe.or, sizeof(mb.cf));
 
489
                mb.modtype = gwj->mod.modtype.or;
 
490
                if (nla_put(skb, CGW_MOD_OR, sizeof(mb), &mb) < 0)
 
491
                        goto cancel;
 
492
                else
 
493
                        nlh->nlmsg_len += NLA_HDRLEN + NLA_ALIGN(sizeof(mb));
 
494
        }
 
495
 
 
496
        if (gwj->mod.modtype.xor) {
 
497
                memcpy(&mb.cf, &gwj->mod.modframe.xor, sizeof(mb.cf));
 
498
                mb.modtype = gwj->mod.modtype.xor;
 
499
                if (nla_put(skb, CGW_MOD_XOR, sizeof(mb), &mb) < 0)
 
500
                        goto cancel;
 
501
                else
 
502
                        nlh->nlmsg_len += NLA_HDRLEN + NLA_ALIGN(sizeof(mb));
 
503
        }
 
504
 
 
505
        if (gwj->mod.modtype.set) {
 
506
                memcpy(&mb.cf, &gwj->mod.modframe.set, sizeof(mb.cf));
 
507
                mb.modtype = gwj->mod.modtype.set;
 
508
                if (nla_put(skb, CGW_MOD_SET, sizeof(mb), &mb) < 0)
 
509
                        goto cancel;
 
510
                else
 
511
                        nlh->nlmsg_len += NLA_HDRLEN + NLA_ALIGN(sizeof(mb));
 
512
        }
 
513
 
 
514
        if (gwj->mod.csumfunc.crc8) {
 
515
                if (nla_put(skb, CGW_CS_CRC8, CGW_CS_CRC8_LEN,
 
516
                            &gwj->mod.csum.crc8) < 0)
 
517
                        goto cancel;
 
518
                else
 
519
                        nlh->nlmsg_len += NLA_HDRLEN + \
 
520
                                NLA_ALIGN(CGW_CS_CRC8_LEN);
 
521
        }
 
522
 
 
523
        if (gwj->mod.csumfunc.xor) {
 
524
                if (nla_put(skb, CGW_CS_XOR, CGW_CS_XOR_LEN,
 
525
                            &gwj->mod.csum.xor) < 0)
 
526
                        goto cancel;
 
527
                else
 
528
                        nlh->nlmsg_len += NLA_HDRLEN + \
 
529
                                NLA_ALIGN(CGW_CS_XOR_LEN);
 
530
        }
 
531
 
 
532
        if (gwj->gwtype == CGW_TYPE_CAN_CAN) {
 
533
 
 
534
                if (gwj->ccgw.filter.can_id || gwj->ccgw.filter.can_mask) {
 
535
                        if (nla_put(skb, CGW_FILTER, sizeof(struct can_filter),
 
536
                                    &gwj->ccgw.filter) < 0)
 
537
                                goto cancel;
 
538
                        else
 
539
                                nlh->nlmsg_len += NLA_HDRLEN +
 
540
                                        NLA_ALIGN(sizeof(struct can_filter));
 
541
                }
 
542
 
 
543
                if (nla_put_u32(skb, CGW_SRC_IF, gwj->ccgw.src_idx) < 0)
 
544
                        goto cancel;
 
545
                else
 
546
                        nlh->nlmsg_len += NLA_HDRLEN + NLA_ALIGN(sizeof(u32));
 
547
 
 
548
                if (nla_put_u32(skb, CGW_DST_IF, gwj->ccgw.dst_idx) < 0)
 
549
                        goto cancel;
 
550
                else
 
551
                        nlh->nlmsg_len += NLA_HDRLEN + NLA_ALIGN(sizeof(u32));
 
552
        }
 
553
 
 
554
        return skb->len;
 
555
 
 
556
cancel:
 
557
        nlmsg_cancel(skb, nlh);
 
558
        return -EMSGSIZE;
 
559
}
 
560
 
 
561
/* Dump information about all CAN gateway jobs, in response to RTM_GETROUTE */
 
562
static int cgw_dump_jobs(struct sk_buff *skb, struct netlink_callback *cb)
 
563
{
 
564
        struct cgw_job *gwj = NULL;
 
565
        struct hlist_node *n;
 
566
        int idx = 0;
 
567
        int s_idx = cb->args[0];
 
568
 
 
569
        rcu_read_lock();
 
570
        hlist_for_each_entry_rcu(gwj, n, &cgw_list, list) {
 
571
                if (idx < s_idx)
 
572
                        goto cont;
 
573
 
 
574
                if (cgw_put_job(skb, gwj) < 0)
 
575
                        break;
 
576
cont:
 
577
                idx++;
 
578
        }
 
579
        rcu_read_unlock();
 
580
 
 
581
        cb->args[0] = idx;
 
582
 
 
583
        return skb->len;
 
584
}
 
585
 
 
586
/* check for common and gwtype specific attributes */
 
587
static int cgw_parse_attr(struct nlmsghdr *nlh, struct cf_mod *mod,
 
588
                          u8 gwtype, void *gwtypeattr)
 
589
{
 
590
        struct nlattr *tb[CGW_MAX+1];
 
591
        struct cgw_frame_mod mb;
 
592
        int modidx = 0;
 
593
        int err = 0;
 
594
 
 
595
        /* initialize modification & checksum data space */
 
596
        memset(mod, 0, sizeof(*mod));
 
597
 
 
598
        err = nlmsg_parse(nlh, sizeof(struct rtcanmsg), tb, CGW_MAX, NULL);
 
599
        if (err < 0)
 
600
                return err;
 
601
 
 
602
        /* check for AND/OR/XOR/SET modifications */
 
603
 
 
604
        if (tb[CGW_MOD_AND] &&
 
605
            nla_len(tb[CGW_MOD_AND]) == CGW_MODATTR_LEN) {
 
606
                nla_memcpy(&mb, tb[CGW_MOD_AND], CGW_MODATTR_LEN);
 
607
 
 
608
                canframecpy(&mod->modframe.and, &mb.cf);
 
609
                mod->modtype.and = mb.modtype;
 
610
 
 
611
                if (mb.modtype & CGW_MOD_ID)
 
612
                        mod->modfunc[modidx++] = mod_and_id;
 
613
 
 
614
                if (mb.modtype & CGW_MOD_DLC)
 
615
                        mod->modfunc[modidx++] = mod_and_dlc;
 
616
 
 
617
                if (mb.modtype & CGW_MOD_DATA)
 
618
                        mod->modfunc[modidx++] = mod_and_data;
 
619
        }
 
620
 
 
621
        if (tb[CGW_MOD_OR] &&
 
622
            nla_len(tb[CGW_MOD_OR]) == CGW_MODATTR_LEN) {
 
623
                nla_memcpy(&mb, tb[CGW_MOD_OR], CGW_MODATTR_LEN);
 
624
 
 
625
                canframecpy(&mod->modframe.or, &mb.cf);
 
626
                mod->modtype.or = mb.modtype;
 
627
 
 
628
                if (mb.modtype & CGW_MOD_ID)
 
629
                        mod->modfunc[modidx++] = mod_or_id;
 
630
 
 
631
                if (mb.modtype & CGW_MOD_DLC)
 
632
                        mod->modfunc[modidx++] = mod_or_dlc;
 
633
 
 
634
                if (mb.modtype & CGW_MOD_DATA)
 
635
                        mod->modfunc[modidx++] = mod_or_data;
 
636
        }
 
637
 
 
638
        if (tb[CGW_MOD_XOR] &&
 
639
            nla_len(tb[CGW_MOD_XOR]) == CGW_MODATTR_LEN) {
 
640
                nla_memcpy(&mb, tb[CGW_MOD_XOR], CGW_MODATTR_LEN);
 
641
 
 
642
                canframecpy(&mod->modframe.xor, &mb.cf);
 
643
                mod->modtype.xor = mb.modtype;
 
644
 
 
645
                if (mb.modtype & CGW_MOD_ID)
 
646
                        mod->modfunc[modidx++] = mod_xor_id;
 
647
 
 
648
                if (mb.modtype & CGW_MOD_DLC)
 
649
                        mod->modfunc[modidx++] = mod_xor_dlc;
 
650
 
 
651
                if (mb.modtype & CGW_MOD_DATA)
 
652
                        mod->modfunc[modidx++] = mod_xor_data;
 
653
        }
 
654
 
 
655
        if (tb[CGW_MOD_SET] &&
 
656
            nla_len(tb[CGW_MOD_SET]) == CGW_MODATTR_LEN) {
 
657
                nla_memcpy(&mb, tb[CGW_MOD_SET], CGW_MODATTR_LEN);
 
658
 
 
659
                canframecpy(&mod->modframe.set, &mb.cf);
 
660
                mod->modtype.set = mb.modtype;
 
661
 
 
662
                if (mb.modtype & CGW_MOD_ID)
 
663
                        mod->modfunc[modidx++] = mod_set_id;
 
664
 
 
665
                if (mb.modtype & CGW_MOD_DLC)
 
666
                        mod->modfunc[modidx++] = mod_set_dlc;
 
667
 
 
668
                if (mb.modtype & CGW_MOD_DATA)
 
669
                        mod->modfunc[modidx++] = mod_set_data;
 
670
        }
 
671
 
 
672
        /* check for checksum operations after CAN frame modifications */
 
673
        if (modidx) {
 
674
 
 
675
                if (tb[CGW_CS_CRC8] &&
 
676
                    nla_len(tb[CGW_CS_CRC8]) == CGW_CS_CRC8_LEN) {
 
677
 
 
678
                        struct cgw_csum_crc8 *c = (struct cgw_csum_crc8 *)\
 
679
                                nla_data(tb[CGW_CS_CRC8]);
 
680
 
 
681
                        err = cgw_chk_csum_parms(c->from_idx, c->to_idx,
 
682
                                                 c->result_idx);
 
683
                        if (err)
 
684
                                return err;
 
685
 
 
686
                        nla_memcpy(&mod->csum.crc8, tb[CGW_CS_CRC8],
 
687
                                   CGW_CS_CRC8_LEN);
 
688
 
 
689
                        /*
 
690
                         * select dedicated processing function to reduce
 
691
                         * runtime operations in receive hot path.
 
692
                         */
 
693
                        if (c->from_idx < 0 || c->to_idx < 0 ||
 
694
                            c->result_idx < 0)
 
695
                                mod->csumfunc.crc8 = cgw_csum_crc8_rel;
 
696
                        else if (c->from_idx <= c->to_idx)
 
697
                                mod->csumfunc.crc8 = cgw_csum_crc8_pos;
 
698
                        else
 
699
                                mod->csumfunc.crc8 = cgw_csum_crc8_neg;
 
700
                }
 
701
 
 
702
                if (tb[CGW_CS_XOR] &&
 
703
                    nla_len(tb[CGW_CS_XOR]) == CGW_CS_XOR_LEN) {
 
704
 
 
705
                        struct cgw_csum_xor *c = (struct cgw_csum_xor *)\
 
706
                                nla_data(tb[CGW_CS_XOR]);
 
707
 
 
708
                        err = cgw_chk_csum_parms(c->from_idx, c->to_idx,
 
709
                                                 c->result_idx);
 
710
                        if (err)
 
711
                                return err;
 
712
 
 
713
                        nla_memcpy(&mod->csum.xor, tb[CGW_CS_XOR],
 
714
                                   CGW_CS_XOR_LEN);
 
715
 
 
716
                        /*
 
717
                         * select dedicated processing function to reduce
 
718
                         * runtime operations in receive hot path.
 
719
                         */
 
720
                        if (c->from_idx < 0 || c->to_idx < 0 ||
 
721
                            c->result_idx < 0)
 
722
                                mod->csumfunc.xor = cgw_csum_xor_rel;
 
723
                        else if (c->from_idx <= c->to_idx)
 
724
                                mod->csumfunc.xor = cgw_csum_xor_pos;
 
725
                        else
 
726
                                mod->csumfunc.xor = cgw_csum_xor_neg;
 
727
                }
 
728
        }
 
729
 
 
730
        if (gwtype == CGW_TYPE_CAN_CAN) {
 
731
 
 
732
                /* check CGW_TYPE_CAN_CAN specific attributes */
 
733
 
 
734
                struct can_can_gw *ccgw = (struct can_can_gw *)gwtypeattr;
 
735
                memset(ccgw, 0, sizeof(*ccgw));
 
736
 
 
737
                /* check for can_filter in attributes */
 
738
                if (tb[CGW_FILTER] &&
 
739
                    nla_len(tb[CGW_FILTER]) == sizeof(struct can_filter))
 
740
                        nla_memcpy(&ccgw->filter, tb[CGW_FILTER],
 
741
                                   sizeof(struct can_filter));
 
742
 
 
743
                err = -ENODEV;
 
744
 
 
745
                /* specifying two interfaces is mandatory */
 
746
                if (!tb[CGW_SRC_IF] || !tb[CGW_DST_IF])
 
747
                        return err;
 
748
 
 
749
                if (nla_len(tb[CGW_SRC_IF]) == sizeof(u32))
 
750
                        nla_memcpy(&ccgw->src_idx, tb[CGW_SRC_IF],
 
751
                                   sizeof(u32));
 
752
 
 
753
                if (nla_len(tb[CGW_DST_IF]) == sizeof(u32))
 
754
                        nla_memcpy(&ccgw->dst_idx, tb[CGW_DST_IF],
 
755
                                   sizeof(u32));
 
756
 
 
757
                /* both indices set to 0 for flushing all routing entries */
 
758
                if (!ccgw->src_idx && !ccgw->dst_idx)
 
759
                        return 0;
 
760
 
 
761
                /* only one index set to 0 is an error */
 
762
                if (!ccgw->src_idx || !ccgw->dst_idx)
 
763
                        return err;
 
764
        }
 
765
 
 
766
        /* add the checks for other gwtypes here */
 
767
 
 
768
        return 0;
 
769
}
 
770
 
 
771
static int cgw_create_job(struct sk_buff *skb,  struct nlmsghdr *nlh,
 
772
                          void *arg)
 
773
{
 
774
        struct rtcanmsg *r;
 
775
        struct cgw_job *gwj;
 
776
        int err = 0;
 
777
 
 
778
        if (nlmsg_len(nlh) < sizeof(*r))
 
779
                return -EINVAL;
 
780
 
 
781
        r = nlmsg_data(nlh);
 
782
        if (r->can_family != AF_CAN)
 
783
                return -EPFNOSUPPORT;
 
784
 
 
785
        /* so far we only support CAN -> CAN routings */
 
786
        if (r->gwtype != CGW_TYPE_CAN_CAN)
 
787
                return -EINVAL;
 
788
 
 
789
        gwj = kmem_cache_alloc(cgw_cache, GFP_KERNEL);
 
790
        if (!gwj)
 
791
                return -ENOMEM;
 
792
 
 
793
        gwj->handled_frames = 0;
 
794
        gwj->dropped_frames = 0;
 
795
        gwj->flags = r->flags;
 
796
        gwj->gwtype = r->gwtype;
 
797
 
 
798
        err = cgw_parse_attr(nlh, &gwj->mod, CGW_TYPE_CAN_CAN, &gwj->ccgw);
 
799
        if (err < 0)
 
800
                goto out;
 
801
 
 
802
        err = -ENODEV;
 
803
 
 
804
        /* ifindex == 0 is not allowed for job creation */
 
805
        if (!gwj->ccgw.src_idx || !gwj->ccgw.dst_idx)
 
806
                goto out;
 
807
 
 
808
        gwj->src.dev = dev_get_by_index(&init_net, gwj->ccgw.src_idx);
 
809
 
 
810
        if (!gwj->src.dev)
 
811
                goto out;
 
812
 
 
813
        /* check for CAN netdev not using header_ops - see gw_rcv() */
 
814
        if (gwj->src.dev->type != ARPHRD_CAN || gwj->src.dev->header_ops)
 
815
                goto put_src_out;
 
816
 
 
817
        gwj->dst.dev = dev_get_by_index(&init_net, gwj->ccgw.dst_idx);
 
818
 
 
819
        if (!gwj->dst.dev)
 
820
                goto put_src_out;
 
821
 
 
822
        /* check for CAN netdev not using header_ops - see gw_rcv() */
 
823
        if (gwj->dst.dev->type != ARPHRD_CAN || gwj->dst.dev->header_ops)
 
824
                goto put_src_dst_out;
 
825
 
 
826
        ASSERT_RTNL();
 
827
 
 
828
        err = cgw_register_filter(gwj);
 
829
        if (!err)
 
830
                hlist_add_head_rcu(&gwj->list, &cgw_list);
 
831
 
 
832
put_src_dst_out:
 
833
        dev_put(gwj->dst.dev);
 
834
put_src_out:
 
835
        dev_put(gwj->src.dev);
 
836
out:
 
837
        if (err)
 
838
                kmem_cache_free(cgw_cache, gwj);
 
839
 
 
840
        return err;
 
841
}
 
842
 
 
843
static void cgw_remove_all_jobs(void)
 
844
{
 
845
        struct cgw_job *gwj = NULL;
 
846
        struct hlist_node *n, *nx;
 
847
 
 
848
        ASSERT_RTNL();
 
849
 
 
850
        hlist_for_each_entry_safe(gwj, n, nx, &cgw_list, list) {
 
851
                hlist_del(&gwj->list);
 
852
                cgw_unregister_filter(gwj);
 
853
                kfree(gwj);
 
854
        }
 
855
}
 
856
 
 
857
static int cgw_remove_job(struct sk_buff *skb,  struct nlmsghdr *nlh, void *arg)
 
858
{
 
859
        struct cgw_job *gwj = NULL;
 
860
        struct hlist_node *n, *nx;
 
861
        struct rtcanmsg *r;
 
862
        struct cf_mod mod;
 
863
        struct can_can_gw ccgw;
 
864
        int err = 0;
 
865
 
 
866
        if (nlmsg_len(nlh) < sizeof(*r))
 
867
                return -EINVAL;
 
868
 
 
869
        r = nlmsg_data(nlh);
 
870
        if (r->can_family != AF_CAN)
 
871
                return -EPFNOSUPPORT;
 
872
 
 
873
        /* so far we only support CAN -> CAN routings */
 
874
        if (r->gwtype != CGW_TYPE_CAN_CAN)
 
875
                return -EINVAL;
 
876
 
 
877
        err = cgw_parse_attr(nlh, &mod, CGW_TYPE_CAN_CAN, &ccgw);
 
878
        if (err < 0)
 
879
                return err;
 
880
 
 
881
        /* two interface indices both set to 0 => remove all entries */
 
882
        if (!ccgw.src_idx && !ccgw.dst_idx) {
 
883
                cgw_remove_all_jobs();
 
884
                return 0;
 
885
        }
 
886
 
 
887
        err = -EINVAL;
 
888
 
 
889
        ASSERT_RTNL();
 
890
 
 
891
        /* remove only the first matching entry */
 
892
        hlist_for_each_entry_safe(gwj, n, nx, &cgw_list, list) {
 
893
 
 
894
                if (gwj->flags != r->flags)
 
895
                        continue;
 
896
 
 
897
                if (memcmp(&gwj->mod, &mod, sizeof(mod)))
 
898
                        continue;
 
899
 
 
900
                /* if (r->gwtype == CGW_TYPE_CAN_CAN) - is made sure here */
 
901
                if (memcmp(&gwj->ccgw, &ccgw, sizeof(ccgw)))
 
902
                        continue;
 
903
 
 
904
                hlist_del(&gwj->list);
 
905
                cgw_unregister_filter(gwj);
 
906
                kfree(gwj);
 
907
                err = 0;
 
908
                break;
 
909
        }
 
910
 
 
911
        return err;
 
912
}
 
913
 
 
914
static __init int cgw_module_init(void)
 
915
{
 
916
        printk(banner);
 
917
 
 
918
        cgw_cache = kmem_cache_create("can_gw", sizeof(struct cgw_job),
 
919
                                      0, 0, NULL);
 
920
 
 
921
        if (!cgw_cache)
 
922
                return -ENOMEM;
 
923
 
 
924
        /* set notifier */
 
925
        notifier.notifier_call = cgw_notifier;
 
926
        register_netdevice_notifier(&notifier);
 
927
 
 
928
        if (__rtnl_register(PF_CAN, RTM_GETROUTE, NULL, cgw_dump_jobs, NULL)) {
 
929
                unregister_netdevice_notifier(&notifier);
 
930
                kmem_cache_destroy(cgw_cache);
 
931
                return -ENOBUFS;
 
932
        }
 
933
 
 
934
        /* Only the first call to __rtnl_register can fail */
 
935
        __rtnl_register(PF_CAN, RTM_NEWROUTE, cgw_create_job, NULL, NULL);
 
936
        __rtnl_register(PF_CAN, RTM_DELROUTE, cgw_remove_job, NULL, NULL);
 
937
 
 
938
        return 0;
 
939
}
 
940
 
 
941
static __exit void cgw_module_exit(void)
 
942
{
 
943
        rtnl_unregister_all(PF_CAN);
 
944
 
 
945
        unregister_netdevice_notifier(&notifier);
 
946
 
 
947
        rtnl_lock();
 
948
        cgw_remove_all_jobs();
 
949
        rtnl_unlock();
 
950
 
 
951
        rcu_barrier(); /* Wait for completion of call_rcu()'s */
 
952
 
 
953
        kmem_cache_destroy(cgw_cache);
 
954
}
 
955
 
 
956
module_init(cgw_module_init);
 
957
module_exit(cgw_module_exit);