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> | |
cb3cb423 | 15 | #include <linux/netdevice.h> |
b482cd20 SB |
16 | #include <net/caif/caif_layer.h> |
17 | #include <net/caif/cfpkt.h> | |
18 | #include <net/caif/cffrml.h> | |
19 | ||
20 | #define container_obj(layr) container_of(layr, struct cffrml, layer) | |
21 | ||
22 | struct cffrml { | |
23 | struct cflayer layer; | |
24 | bool dofcs; /* !< FCS active */ | |
cb3cb423 | 25 | int __percpu *pcpu_refcnt; |
b482cd20 SB |
26 | }; |
27 | ||
28 | static int cffrml_receive(struct cflayer *layr, struct cfpkt *pkt); | |
29 | static int cffrml_transmit(struct cflayer *layr, struct cfpkt *pkt); | |
30 | static void cffrml_ctrlcmd(struct cflayer *layr, enum caif_ctrlcmd ctrl, | |
31 | int phyid); | |
32 | ||
33 | static u32 cffrml_rcv_error; | |
34 | static u32 cffrml_rcv_checsum_error; | |
35 | struct cflayer *cffrml_create(u16 phyid, bool use_fcs) | |
36 | { | |
37 | struct cffrml *this = kmalloc(sizeof(struct cffrml), GFP_ATOMIC); | |
38 | if (!this) { | |
b31fa5ba | 39 | pr_warn("Out of memory\n"); |
b482cd20 SB |
40 | return NULL; |
41 | } | |
cb3cb423 | 42 | this->pcpu_refcnt = alloc_percpu(int); |
43 | if (this->pcpu_refcnt == NULL) { | |
44 | kfree(this); | |
45 | return NULL; | |
46 | } | |
47 | ||
b482cd20 SB |
48 | caif_assert(offsetof(struct cffrml, layer) == 0); |
49 | ||
50 | memset(this, 0, sizeof(struct cflayer)); | |
51 | this->layer.receive = cffrml_receive; | |
52 | this->layer.transmit = cffrml_transmit; | |
53 | this->layer.ctrlcmd = cffrml_ctrlcmd; | |
54 | snprintf(this->layer.name, CAIF_LAYER_NAME_SZ, "frm%d", phyid); | |
55 | this->dofcs = use_fcs; | |
56 | this->layer.id = phyid; | |
57 | return (struct cflayer *) this; | |
58 | } | |
59 | ||
cb3cb423 | 60 | void cffrml_free(struct cflayer *layer) |
61 | { | |
62 | struct cffrml *this = container_obj(layer); | |
63 | free_percpu(this->pcpu_refcnt); | |
64 | kfree(layer); | |
65 | } | |
66 | ||
b482cd20 SB |
67 | void cffrml_set_uplayer(struct cflayer *this, struct cflayer *up) |
68 | { | |
69 | this->up = up; | |
70 | } | |
71 | ||
72 | void cffrml_set_dnlayer(struct cflayer *this, struct cflayer *dn) | |
73 | { | |
74 | this->dn = dn; | |
75 | } | |
76 | ||
77 | static u16 cffrml_checksum(u16 chks, void *buf, u16 len) | |
78 | { | |
79 | /* FIXME: FCS should be moved to glue in order to use OS-Specific | |
80 | * solutions | |
81 | */ | |
82 | return crc_ccitt(chks, buf, len); | |
83 | } | |
84 | ||
85 | static int cffrml_receive(struct cflayer *layr, struct cfpkt *pkt) | |
86 | { | |
87 | u16 tmp; | |
88 | u16 len; | |
89 | u16 hdrchks; | |
90 | u16 pktchks; | |
91 | struct cffrml *this; | |
92 | this = container_obj(layr); | |
93 | ||
94 | cfpkt_extr_head(pkt, &tmp, 2); | |
95 | len = le16_to_cpu(tmp); | |
96 | ||
97 | /* Subtract for FCS on length if FCS is not used. */ | |
98 | if (!this->dofcs) | |
99 | len -= 2; | |
100 | ||
101 | if (cfpkt_setlen(pkt, len) < 0) { | |
102 | ++cffrml_rcv_error; | |
b31fa5ba | 103 | pr_err("Framing length error (%d)\n", len); |
b482cd20 SB |
104 | cfpkt_destroy(pkt); |
105 | return -EPROTO; | |
106 | } | |
107 | /* | |
108 | * Don't do extract if FCS is false, rather do setlen - then we don't | |
109 | * get a cache-miss. | |
110 | */ | |
111 | if (this->dofcs) { | |
112 | cfpkt_extr_trail(pkt, &tmp, 2); | |
113 | hdrchks = le16_to_cpu(tmp); | |
114 | pktchks = cfpkt_iterate(pkt, cffrml_checksum, 0xffff); | |
115 | if (pktchks != hdrchks) { | |
116 | cfpkt_add_trail(pkt, &tmp, 2); | |
117 | ++cffrml_rcv_error; | |
118 | ++cffrml_rcv_checsum_error; | |
b31fa5ba JP |
119 | pr_info("Frame checksum error (0x%x != 0x%x)\n", |
120 | hdrchks, pktchks); | |
b482cd20 SB |
121 | return -EILSEQ; |
122 | } | |
123 | } | |
124 | if (cfpkt_erroneous(pkt)) { | |
125 | ++cffrml_rcv_error; | |
b31fa5ba | 126 | pr_err("Packet is erroneous!\n"); |
b482cd20 SB |
127 | cfpkt_destroy(pkt); |
128 | return -EPROTO; | |
129 | } | |
c85c2951 | 130 | |
131 | if (layr->up == NULL) { | |
132 | pr_err("Layr up is missing!\n"); | |
133 | cfpkt_destroy(pkt); | |
134 | return -EINVAL; | |
135 | } | |
136 | ||
b482cd20 SB |
137 | return layr->up->receive(layr->up, pkt); |
138 | } | |
139 | ||
140 | static int cffrml_transmit(struct cflayer *layr, struct cfpkt *pkt) | |
141 | { | |
142 | int tmp; | |
143 | u16 chks; | |
144 | u16 len; | |
b482cd20 SB |
145 | struct cffrml *this = container_obj(layr); |
146 | if (this->dofcs) { | |
147 | chks = cfpkt_iterate(pkt, cffrml_checksum, 0xffff); | |
148 | tmp = cpu_to_le16(chks); | |
149 | cfpkt_add_trail(pkt, &tmp, 2); | |
150 | } else { | |
151 | cfpkt_pad_trail(pkt, 2); | |
152 | } | |
153 | len = cfpkt_getlen(pkt); | |
154 | tmp = cpu_to_le16(len); | |
155 | cfpkt_add_head(pkt, &tmp, 2); | |
156 | cfpkt_info(pkt)->hdr_len += 2; | |
157 | if (cfpkt_erroneous(pkt)) { | |
b31fa5ba | 158 | pr_err("Packet is erroneous!\n"); |
c85c2951 | 159 | cfpkt_destroy(pkt); |
b482cd20 SB |
160 | return -EPROTO; |
161 | } | |
c85c2951 | 162 | |
163 | if (layr->dn == NULL) { | |
164 | cfpkt_destroy(pkt); | |
165 | return -ENODEV; | |
166 | ||
167 | } | |
4dd820c0 | 168 | return layr->dn->transmit(layr->dn, pkt); |
b482cd20 SB |
169 | } |
170 | ||
171 | static void cffrml_ctrlcmd(struct cflayer *layr, enum caif_ctrlcmd ctrl, | |
172 | int phyid) | |
173 | { | |
c85c2951 | 174 | if (layr->up && layr->up->ctrlcmd) |
b482cd20 SB |
175 | layr->up->ctrlcmd(layr->up, ctrl, layr->id); |
176 | } | |
0b1e9738 | 177 | |
178 | void cffrml_put(struct cflayer *layr) | |
179 | { | |
cb3cb423 | 180 | struct cffrml *this = container_obj(layr); |
181 | if (layr != NULL && this->pcpu_refcnt != NULL) | |
182 | irqsafe_cpu_dec(*this->pcpu_refcnt); | |
0b1e9738 | 183 | } |
184 | ||
185 | void cffrml_hold(struct cflayer *layr) | |
186 | { | |
cb3cb423 | 187 | struct cffrml *this = container_obj(layr); |
188 | if (layr != NULL && this->pcpu_refcnt != NULL) | |
189 | irqsafe_cpu_inc(*this->pcpu_refcnt); | |
190 | } | |
191 | ||
192 | int cffrml_refcnt_read(struct cflayer *layr) | |
193 | { | |
194 | int i, refcnt = 0; | |
195 | struct cffrml *this = container_obj(layr); | |
196 | for_each_possible_cpu(i) | |
197 | refcnt += *per_cpu_ptr(this->pcpu_refcnt, i); | |
198 | return refcnt; | |
0b1e9738 | 199 | } |