Commit | Line | Data |
---|---|---|
b482cd20 SB |
1 | /* |
2 | * CAIF Framing Layer. | |
3 | * | |
4 | * Copyright (C) ST-Ericsson AB 2010 | |
5 | * Author: Sjur Brendeland/sjur.brandeland@stericsson.com | |
6 | * License terms: GNU General Public License (GPL) version 2 | |
7 | */ | |
8 | ||
b31fa5ba JP |
9 | #define pr_fmt(fmt) KBUILD_MODNAME ":%s(): " fmt, __func__ |
10 | ||
b482cd20 SB |
11 | #include <linux/stddef.h> |
12 | #include <linux/spinlock.h> | |
13 | #include <linux/slab.h> | |
14 | #include <linux/crc-ccitt.h> | |
15 | #include <net/caif/caif_layer.h> | |
16 | #include <net/caif/cfpkt.h> | |
17 | #include <net/caif/cffrml.h> | |
18 | ||
19 | #define container_obj(layr) container_of(layr, struct cffrml, layer) | |
20 | ||
21 | struct cffrml { | |
22 | struct cflayer layer; | |
23 | bool dofcs; /* !< FCS active */ | |
24 | }; | |
25 | ||
26 | static int cffrml_receive(struct cflayer *layr, struct cfpkt *pkt); | |
27 | static int cffrml_transmit(struct cflayer *layr, struct cfpkt *pkt); | |
28 | static void cffrml_ctrlcmd(struct cflayer *layr, enum caif_ctrlcmd ctrl, | |
29 | int phyid); | |
30 | ||
31 | static u32 cffrml_rcv_error; | |
32 | static u32 cffrml_rcv_checsum_error; | |
33 | struct cflayer *cffrml_create(u16 phyid, bool use_fcs) | |
34 | { | |
35 | struct cffrml *this = kmalloc(sizeof(struct cffrml), GFP_ATOMIC); | |
36 | if (!this) { | |
b31fa5ba | 37 | pr_warn("Out of memory\n"); |
b482cd20 SB |
38 | return NULL; |
39 | } | |
40 | caif_assert(offsetof(struct cffrml, layer) == 0); | |
41 | ||
42 | memset(this, 0, sizeof(struct cflayer)); | |
43 | this->layer.receive = cffrml_receive; | |
44 | this->layer.transmit = cffrml_transmit; | |
45 | this->layer.ctrlcmd = cffrml_ctrlcmd; | |
46 | snprintf(this->layer.name, CAIF_LAYER_NAME_SZ, "frm%d", phyid); | |
47 | this->dofcs = use_fcs; | |
48 | this->layer.id = phyid; | |
49 | return (struct cflayer *) this; | |
50 | } | |
51 | ||
52 | void cffrml_set_uplayer(struct cflayer *this, struct cflayer *up) | |
53 | { | |
54 | this->up = up; | |
55 | } | |
56 | ||
57 | void cffrml_set_dnlayer(struct cflayer *this, struct cflayer *dn) | |
58 | { | |
59 | this->dn = dn; | |
60 | } | |
61 | ||
62 | static u16 cffrml_checksum(u16 chks, void *buf, u16 len) | |
63 | { | |
64 | /* FIXME: FCS should be moved to glue in order to use OS-Specific | |
65 | * solutions | |
66 | */ | |
67 | return crc_ccitt(chks, buf, len); | |
68 | } | |
69 | ||
70 | static int cffrml_receive(struct cflayer *layr, struct cfpkt *pkt) | |
71 | { | |
72 | u16 tmp; | |
73 | u16 len; | |
74 | u16 hdrchks; | |
75 | u16 pktchks; | |
76 | struct cffrml *this; | |
77 | this = container_obj(layr); | |
78 | ||
79 | cfpkt_extr_head(pkt, &tmp, 2); | |
80 | len = le16_to_cpu(tmp); | |
81 | ||
82 | /* Subtract for FCS on length if FCS is not used. */ | |
83 | if (!this->dofcs) | |
84 | len -= 2; | |
85 | ||
86 | if (cfpkt_setlen(pkt, len) < 0) { | |
87 | ++cffrml_rcv_error; | |
b31fa5ba | 88 | pr_err("Framing length error (%d)\n", len); |
b482cd20 SB |
89 | cfpkt_destroy(pkt); |
90 | return -EPROTO; | |
91 | } | |
92 | /* | |
93 | * Don't do extract if FCS is false, rather do setlen - then we don't | |
94 | * get a cache-miss. | |
95 | */ | |
96 | if (this->dofcs) { | |
97 | cfpkt_extr_trail(pkt, &tmp, 2); | |
98 | hdrchks = le16_to_cpu(tmp); | |
99 | pktchks = cfpkt_iterate(pkt, cffrml_checksum, 0xffff); | |
100 | if (pktchks != hdrchks) { | |
101 | cfpkt_add_trail(pkt, &tmp, 2); | |
102 | ++cffrml_rcv_error; | |
103 | ++cffrml_rcv_checsum_error; | |
b31fa5ba JP |
104 | pr_info("Frame checksum error (0x%x != 0x%x)\n", |
105 | hdrchks, pktchks); | |
b482cd20 SB |
106 | return -EILSEQ; |
107 | } | |
108 | } | |
109 | if (cfpkt_erroneous(pkt)) { | |
110 | ++cffrml_rcv_error; | |
b31fa5ba | 111 | pr_err("Packet is erroneous!\n"); |
b482cd20 SB |
112 | cfpkt_destroy(pkt); |
113 | return -EPROTO; | |
114 | } | |
115 | return layr->up->receive(layr->up, pkt); | |
116 | } | |
117 | ||
118 | static int cffrml_transmit(struct cflayer *layr, struct cfpkt *pkt) | |
119 | { | |
120 | int tmp; | |
121 | u16 chks; | |
122 | u16 len; | |
b482cd20 SB |
123 | struct cffrml *this = container_obj(layr); |
124 | if (this->dofcs) { | |
125 | chks = cfpkt_iterate(pkt, cffrml_checksum, 0xffff); | |
126 | tmp = cpu_to_le16(chks); | |
127 | cfpkt_add_trail(pkt, &tmp, 2); | |
128 | } else { | |
129 | cfpkt_pad_trail(pkt, 2); | |
130 | } | |
131 | len = cfpkt_getlen(pkt); | |
132 | tmp = cpu_to_le16(len); | |
133 | cfpkt_add_head(pkt, &tmp, 2); | |
134 | cfpkt_info(pkt)->hdr_len += 2; | |
135 | if (cfpkt_erroneous(pkt)) { | |
b31fa5ba | 136 | pr_err("Packet is erroneous!\n"); |
b482cd20 SB |
137 | return -EPROTO; |
138 | } | |
4dd820c0 | 139 | return layr->dn->transmit(layr->dn, pkt); |
b482cd20 SB |
140 | } |
141 | ||
142 | static void cffrml_ctrlcmd(struct cflayer *layr, enum caif_ctrlcmd ctrl, | |
143 | int phyid) | |
144 | { | |
145 | if (layr->up->ctrlcmd) | |
146 | layr->up->ctrlcmd(layr->up, ctrl, layr->id); | |
147 | } |