1 | // SPDX-License-Identifier: GPL-2.0-only |
2 | /* |
3 | * CAIF Framing Layer. |
4 | * |
5 | * Copyright (C) ST-Ericsson AB 2010 |
6 | * Author: Sjur Brendeland |
7 | */ |
8 | |
9 | #define pr_fmt(fmt) KBUILD_MODNAME ":%s(): " fmt, __func__ |
10 | |
11 | #include <linux/stddef.h> |
12 | #include <linux/spinlock.h> |
13 | #include <linux/slab.h> |
14 | #include <linux/crc-ccitt.h> |
15 | #include <linux/netdevice.h> |
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 */ |
25 | int __percpu *pcpu_refcnt; |
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 = kzalloc(size: sizeof(struct cffrml), GFP_ATOMIC); |
38 | if (!this) |
39 | return NULL; |
40 | this->pcpu_refcnt = alloc_percpu(int); |
41 | if (this->pcpu_refcnt == NULL) { |
42 | kfree(objp: this); |
43 | return NULL; |
44 | } |
45 | |
46 | caif_assert(offsetof(struct cffrml, layer) == 0); |
47 | |
48 | this->layer.receive = cffrml_receive; |
49 | this->layer.transmit = cffrml_transmit; |
50 | this->layer.ctrlcmd = cffrml_ctrlcmd; |
51 | snprintf(buf: this->layer.name, CAIF_LAYER_NAME_SZ, fmt: "frm%d" , phyid); |
52 | this->dofcs = use_fcs; |
53 | this->layer.id = phyid; |
54 | return (struct cflayer *) this; |
55 | } |
56 | |
57 | void cffrml_free(struct cflayer *layer) |
58 | { |
59 | struct cffrml *this = container_obj(layer); |
60 | free_percpu(pdata: this->pcpu_refcnt); |
61 | kfree(objp: layer); |
62 | } |
63 | |
64 | void cffrml_set_uplayer(struct cflayer *this, struct cflayer *up) |
65 | { |
66 | this->up = up; |
67 | } |
68 | |
69 | void cffrml_set_dnlayer(struct cflayer *this, struct cflayer *dn) |
70 | { |
71 | this->dn = dn; |
72 | } |
73 | |
74 | static u16 cffrml_checksum(u16 chks, void *buf, u16 len) |
75 | { |
76 | /* FIXME: FCS should be moved to glue in order to use OS-Specific |
77 | * solutions |
78 | */ |
79 | return crc_ccitt(crc: chks, buffer: buf, len); |
80 | } |
81 | |
82 | static int cffrml_receive(struct cflayer *layr, struct cfpkt *pkt) |
83 | { |
84 | u16 tmp; |
85 | u16 len; |
86 | u16 hdrchks; |
87 | int pktchks; |
88 | struct cffrml *this; |
89 | this = container_obj(layr); |
90 | |
91 | cfpkt_extr_head(pkt, data: &tmp, len: 2); |
92 | len = le16_to_cpu(tmp); |
93 | |
94 | /* Subtract for FCS on length if FCS is not used. */ |
95 | if (!this->dofcs) |
96 | len -= 2; |
97 | |
98 | if (cfpkt_setlen(pkt, len) < 0) { |
99 | ++cffrml_rcv_error; |
100 | pr_err("Framing length error (%d)\n" , len); |
101 | cfpkt_destroy(pkt); |
102 | return -EPROTO; |
103 | } |
104 | /* |
105 | * Don't do extract if FCS is false, rather do setlen - then we don't |
106 | * get a cache-miss. |
107 | */ |
108 | if (this->dofcs) { |
109 | cfpkt_extr_trail(pkt, data: &tmp, len: 2); |
110 | hdrchks = le16_to_cpu(tmp); |
111 | pktchks = cfpkt_iterate(pkt, iter_func: cffrml_checksum, data: 0xffff); |
112 | if (pktchks != hdrchks) { |
113 | cfpkt_add_trail(pkt, data: &tmp, len: 2); |
114 | ++cffrml_rcv_error; |
115 | ++cffrml_rcv_checsum_error; |
116 | pr_info("Frame checksum error (0x%x != 0x%x)\n" , |
117 | hdrchks, pktchks); |
118 | return -EILSEQ; |
119 | } |
120 | } |
121 | if (cfpkt_erroneous(pkt)) { |
122 | ++cffrml_rcv_error; |
123 | pr_err("Packet is erroneous!\n" ); |
124 | cfpkt_destroy(pkt); |
125 | return -EPROTO; |
126 | } |
127 | |
128 | if (layr->up == NULL) { |
129 | pr_err("Layr up is missing!\n" ); |
130 | cfpkt_destroy(pkt); |
131 | return -EINVAL; |
132 | } |
133 | |
134 | return layr->up->receive(layr->up, pkt); |
135 | } |
136 | |
137 | static int cffrml_transmit(struct cflayer *layr, struct cfpkt *pkt) |
138 | { |
139 | u16 chks; |
140 | u16 len; |
141 | __le16 data; |
142 | |
143 | struct cffrml *this = container_obj(layr); |
144 | if (this->dofcs) { |
145 | chks = cfpkt_iterate(pkt, iter_func: cffrml_checksum, data: 0xffff); |
146 | data = cpu_to_le16(chks); |
147 | cfpkt_add_trail(pkt, data: &data, len: 2); |
148 | } else { |
149 | cfpkt_pad_trail(pkt, len: 2); |
150 | } |
151 | len = cfpkt_getlen(pkt); |
152 | data = cpu_to_le16(len); |
153 | cfpkt_add_head(pkt, data: &data, len: 2); |
154 | cfpkt_info(pkt)->hdr_len += 2; |
155 | if (cfpkt_erroneous(pkt)) { |
156 | pr_err("Packet is erroneous!\n" ); |
157 | cfpkt_destroy(pkt); |
158 | return -EPROTO; |
159 | } |
160 | |
161 | if (layr->dn == NULL) { |
162 | cfpkt_destroy(pkt); |
163 | return -ENODEV; |
164 | |
165 | } |
166 | return layr->dn->transmit(layr->dn, pkt); |
167 | } |
168 | |
169 | static void cffrml_ctrlcmd(struct cflayer *layr, enum caif_ctrlcmd ctrl, |
170 | int phyid) |
171 | { |
172 | if (layr->up && layr->up->ctrlcmd) |
173 | layr->up->ctrlcmd(layr->up, ctrl, layr->id); |
174 | } |
175 | |
176 | void cffrml_put(struct cflayer *layr) |
177 | { |
178 | struct cffrml *this = container_obj(layr); |
179 | if (layr != NULL && this->pcpu_refcnt != NULL) |
180 | this_cpu_dec(*this->pcpu_refcnt); |
181 | } |
182 | |
183 | void cffrml_hold(struct cflayer *layr) |
184 | { |
185 | struct cffrml *this = container_obj(layr); |
186 | if (layr != NULL && this->pcpu_refcnt != NULL) |
187 | this_cpu_inc(*this->pcpu_refcnt); |
188 | } |
189 | |
190 | int cffrml_refcnt_read(struct cflayer *layr) |
191 | { |
192 | int i, refcnt = 0; |
193 | struct cffrml *this = container_obj(layr); |
194 | for_each_possible_cpu(i) |
195 | refcnt += *per_cpu_ptr(this->pcpu_refcnt, i); |
196 | return refcnt; |
197 | } |
198 | |