1 | // SPDX-License-Identifier: GPL-2.0-or-later |
2 | /* |
3 | * Copyright Gavin Shan, IBM Corporation 2016. |
4 | */ |
5 | |
6 | #include <linux/module.h> |
7 | #include <linux/kernel.h> |
8 | #include <linux/init.h> |
9 | #include <linux/etherdevice.h> |
10 | #include <linux/netdevice.h> |
11 | #include <linux/skbuff.h> |
12 | |
13 | #include <net/ncsi.h> |
14 | #include <net/net_namespace.h> |
15 | #include <net/sock.h> |
16 | #include <net/genetlink.h> |
17 | |
18 | #include "internal.h" |
19 | #include "ncsi-pkt.h" |
20 | |
21 | static const int padding_bytes = 26; |
22 | |
23 | u32 ncsi_calculate_checksum(unsigned char *data, int len) |
24 | { |
25 | u32 checksum = 0; |
26 | int i; |
27 | |
28 | for (i = 0; i < len; i += 2) |
29 | checksum += (((u32)data[i] << 8) | data[i + 1]); |
30 | |
31 | checksum = (~checksum + 1); |
32 | return checksum; |
33 | } |
34 | |
35 | /* This function should be called after the data area has been |
36 | * populated completely. |
37 | */ |
38 | static void (struct ncsi_pkt_hdr *h, |
39 | struct ncsi_cmd_arg *nca) |
40 | { |
41 | u32 checksum; |
42 | __be32 *pchecksum; |
43 | |
44 | h->mc_id = 0; |
45 | h->revision = NCSI_PKT_REVISION; |
46 | h->reserved = 0; |
47 | h->id = nca->id; |
48 | h->type = nca->type; |
49 | h->channel = NCSI_TO_CHANNEL(nca->package, |
50 | nca->channel); |
51 | h->length = htons(nca->payload); |
52 | h->reserved1[0] = 0; |
53 | h->reserved1[1] = 0; |
54 | |
55 | /* Fill with calculated checksum */ |
56 | checksum = ncsi_calculate_checksum(data: (unsigned char *)h, |
57 | len: sizeof(*h) + nca->payload); |
58 | pchecksum = (__be32 *)((void *)h + sizeof(struct ncsi_pkt_hdr) + |
59 | ALIGN(nca->payload, 4)); |
60 | *pchecksum = htonl(checksum); |
61 | } |
62 | |
63 | static int ncsi_cmd_handler_default(struct sk_buff *skb, |
64 | struct ncsi_cmd_arg *nca) |
65 | { |
66 | struct ncsi_cmd_pkt *cmd; |
67 | |
68 | cmd = skb_put_zero(skb, len: sizeof(*cmd)); |
69 | ncsi_cmd_build_header(h: &cmd->cmd.common, nca); |
70 | |
71 | return 0; |
72 | } |
73 | |
74 | static int ncsi_cmd_handler_sp(struct sk_buff *skb, |
75 | struct ncsi_cmd_arg *nca) |
76 | { |
77 | struct ncsi_cmd_sp_pkt *cmd; |
78 | |
79 | cmd = skb_put_zero(skb, len: sizeof(*cmd)); |
80 | cmd->hw_arbitration = nca->bytes[0]; |
81 | ncsi_cmd_build_header(h: &cmd->cmd.common, nca); |
82 | |
83 | return 0; |
84 | } |
85 | |
86 | static int ncsi_cmd_handler_dc(struct sk_buff *skb, |
87 | struct ncsi_cmd_arg *nca) |
88 | { |
89 | struct ncsi_cmd_dc_pkt *cmd; |
90 | |
91 | cmd = skb_put_zero(skb, len: sizeof(*cmd)); |
92 | cmd->ald = nca->bytes[0]; |
93 | ncsi_cmd_build_header(h: &cmd->cmd.common, nca); |
94 | |
95 | return 0; |
96 | } |
97 | |
98 | static int ncsi_cmd_handler_rc(struct sk_buff *skb, |
99 | struct ncsi_cmd_arg *nca) |
100 | { |
101 | struct ncsi_cmd_rc_pkt *cmd; |
102 | |
103 | cmd = skb_put_zero(skb, len: sizeof(*cmd)); |
104 | ncsi_cmd_build_header(h: &cmd->cmd.common, nca); |
105 | |
106 | return 0; |
107 | } |
108 | |
109 | static int ncsi_cmd_handler_ae(struct sk_buff *skb, |
110 | struct ncsi_cmd_arg *nca) |
111 | { |
112 | struct ncsi_cmd_ae_pkt *cmd; |
113 | |
114 | cmd = skb_put_zero(skb, len: sizeof(*cmd)); |
115 | cmd->mc_id = nca->bytes[0]; |
116 | cmd->mode = htonl(nca->dwords[1]); |
117 | ncsi_cmd_build_header(h: &cmd->cmd.common, nca); |
118 | |
119 | return 0; |
120 | } |
121 | |
122 | static int ncsi_cmd_handler_sl(struct sk_buff *skb, |
123 | struct ncsi_cmd_arg *nca) |
124 | { |
125 | struct ncsi_cmd_sl_pkt *cmd; |
126 | |
127 | cmd = skb_put_zero(skb, len: sizeof(*cmd)); |
128 | cmd->mode = htonl(nca->dwords[0]); |
129 | cmd->oem_mode = htonl(nca->dwords[1]); |
130 | ncsi_cmd_build_header(h: &cmd->cmd.common, nca); |
131 | |
132 | return 0; |
133 | } |
134 | |
135 | static int ncsi_cmd_handler_svf(struct sk_buff *skb, |
136 | struct ncsi_cmd_arg *nca) |
137 | { |
138 | struct ncsi_cmd_svf_pkt *cmd; |
139 | |
140 | cmd = skb_put_zero(skb, len: sizeof(*cmd)); |
141 | cmd->vlan = htons(nca->words[1]); |
142 | cmd->index = nca->bytes[6]; |
143 | cmd->enable = nca->bytes[7]; |
144 | ncsi_cmd_build_header(h: &cmd->cmd.common, nca); |
145 | |
146 | return 0; |
147 | } |
148 | |
149 | static int ncsi_cmd_handler_ev(struct sk_buff *skb, |
150 | struct ncsi_cmd_arg *nca) |
151 | { |
152 | struct ncsi_cmd_ev_pkt *cmd; |
153 | |
154 | cmd = skb_put_zero(skb, len: sizeof(*cmd)); |
155 | cmd->mode = nca->bytes[3]; |
156 | ncsi_cmd_build_header(h: &cmd->cmd.common, nca); |
157 | |
158 | return 0; |
159 | } |
160 | |
161 | static int ncsi_cmd_handler_sma(struct sk_buff *skb, |
162 | struct ncsi_cmd_arg *nca) |
163 | { |
164 | struct ncsi_cmd_sma_pkt *cmd; |
165 | int i; |
166 | |
167 | cmd = skb_put_zero(skb, len: sizeof(*cmd)); |
168 | for (i = 0; i < 6; i++) |
169 | cmd->mac[i] = nca->bytes[i]; |
170 | cmd->index = nca->bytes[6]; |
171 | cmd->at_e = nca->bytes[7]; |
172 | ncsi_cmd_build_header(h: &cmd->cmd.common, nca); |
173 | |
174 | return 0; |
175 | } |
176 | |
177 | static int ncsi_cmd_handler_ebf(struct sk_buff *skb, |
178 | struct ncsi_cmd_arg *nca) |
179 | { |
180 | struct ncsi_cmd_ebf_pkt *cmd; |
181 | |
182 | cmd = skb_put_zero(skb, len: sizeof(*cmd)); |
183 | cmd->mode = htonl(nca->dwords[0]); |
184 | ncsi_cmd_build_header(h: &cmd->cmd.common, nca); |
185 | |
186 | return 0; |
187 | } |
188 | |
189 | static int ncsi_cmd_handler_egmf(struct sk_buff *skb, |
190 | struct ncsi_cmd_arg *nca) |
191 | { |
192 | struct ncsi_cmd_egmf_pkt *cmd; |
193 | |
194 | cmd = skb_put_zero(skb, len: sizeof(*cmd)); |
195 | cmd->mode = htonl(nca->dwords[0]); |
196 | ncsi_cmd_build_header(h: &cmd->cmd.common, nca); |
197 | |
198 | return 0; |
199 | } |
200 | |
201 | static int ncsi_cmd_handler_snfc(struct sk_buff *skb, |
202 | struct ncsi_cmd_arg *nca) |
203 | { |
204 | struct ncsi_cmd_snfc_pkt *cmd; |
205 | |
206 | cmd = skb_put_zero(skb, len: sizeof(*cmd)); |
207 | cmd->mode = nca->bytes[0]; |
208 | ncsi_cmd_build_header(h: &cmd->cmd.common, nca); |
209 | |
210 | return 0; |
211 | } |
212 | |
213 | static int ncsi_cmd_handler_oem(struct sk_buff *skb, |
214 | struct ncsi_cmd_arg *nca) |
215 | { |
216 | struct ncsi_cmd_oem_pkt *cmd; |
217 | unsigned int len; |
218 | int payload; |
219 | /* NC-SI spec DSP_0222_1.2.0, section 8.2.2.2 |
220 | * requires payload to be padded with 0 to |
221 | * 32-bit boundary before the checksum field. |
222 | * Ensure the padding bytes are accounted for in |
223 | * skb allocation |
224 | */ |
225 | |
226 | payload = ALIGN(nca->payload, 4); |
227 | len = sizeof(struct ncsi_cmd_pkt_hdr) + 4; |
228 | len += max(payload, padding_bytes); |
229 | |
230 | cmd = skb_put_zero(skb, len); |
231 | unsafe_memcpy(&cmd->mfr_id, nca->data, nca->payload, |
232 | /* skb allocated with enough to load the payload */); |
233 | ncsi_cmd_build_header(h: &cmd->cmd.common, nca); |
234 | |
235 | return 0; |
236 | } |
237 | |
238 | static struct ncsi_cmd_handler { |
239 | unsigned char type; |
240 | int payload; |
241 | int (*handler)(struct sk_buff *skb, |
242 | struct ncsi_cmd_arg *nca); |
243 | } ncsi_cmd_handlers[] = { |
244 | { NCSI_PKT_CMD_CIS, 0, ncsi_cmd_handler_default }, |
245 | { NCSI_PKT_CMD_SP, 4, ncsi_cmd_handler_sp }, |
246 | { NCSI_PKT_CMD_DP, 0, ncsi_cmd_handler_default }, |
247 | { NCSI_PKT_CMD_EC, 0, ncsi_cmd_handler_default }, |
248 | { NCSI_PKT_CMD_DC, 4, ncsi_cmd_handler_dc }, |
249 | { NCSI_PKT_CMD_RC, 4, ncsi_cmd_handler_rc }, |
250 | { NCSI_PKT_CMD_ECNT, 0, ncsi_cmd_handler_default }, |
251 | { NCSI_PKT_CMD_DCNT, 0, ncsi_cmd_handler_default }, |
252 | { NCSI_PKT_CMD_AE, 8, ncsi_cmd_handler_ae }, |
253 | { NCSI_PKT_CMD_SL, 8, ncsi_cmd_handler_sl }, |
254 | { NCSI_PKT_CMD_GLS, 0, ncsi_cmd_handler_default }, |
255 | { NCSI_PKT_CMD_SVF, 8, ncsi_cmd_handler_svf }, |
256 | { NCSI_PKT_CMD_EV, 4, ncsi_cmd_handler_ev }, |
257 | { NCSI_PKT_CMD_DV, 0, ncsi_cmd_handler_default }, |
258 | { NCSI_PKT_CMD_SMA, 8, ncsi_cmd_handler_sma }, |
259 | { NCSI_PKT_CMD_EBF, 4, ncsi_cmd_handler_ebf }, |
260 | { NCSI_PKT_CMD_DBF, 0, ncsi_cmd_handler_default }, |
261 | { NCSI_PKT_CMD_EGMF, 4, ncsi_cmd_handler_egmf }, |
262 | { NCSI_PKT_CMD_DGMF, 0, ncsi_cmd_handler_default }, |
263 | { NCSI_PKT_CMD_SNFC, 4, ncsi_cmd_handler_snfc }, |
264 | { NCSI_PKT_CMD_GVI, 0, ncsi_cmd_handler_default }, |
265 | { NCSI_PKT_CMD_GC, 0, ncsi_cmd_handler_default }, |
266 | { NCSI_PKT_CMD_GP, 0, ncsi_cmd_handler_default }, |
267 | { NCSI_PKT_CMD_GCPS, 0, ncsi_cmd_handler_default }, |
268 | { NCSI_PKT_CMD_GNS, 0, ncsi_cmd_handler_default }, |
269 | { NCSI_PKT_CMD_GNPTS, 0, ncsi_cmd_handler_default }, |
270 | { NCSI_PKT_CMD_GPS, 0, ncsi_cmd_handler_default }, |
271 | { NCSI_PKT_CMD_OEM, -1, ncsi_cmd_handler_oem }, |
272 | { NCSI_PKT_CMD_PLDM, 0, NULL }, |
273 | { NCSI_PKT_CMD_GPUUID, 0, ncsi_cmd_handler_default } |
274 | }; |
275 | |
276 | static struct ncsi_request *ncsi_alloc_command(struct ncsi_cmd_arg *nca) |
277 | { |
278 | struct ncsi_dev_priv *ndp = nca->ndp; |
279 | struct ncsi_dev *nd = &ndp->ndev; |
280 | struct net_device *dev = nd->dev; |
281 | int hlen = LL_RESERVED_SPACE(dev); |
282 | int tlen = dev->needed_tailroom; |
283 | int payload; |
284 | int len = hlen + tlen; |
285 | struct sk_buff *skb; |
286 | struct ncsi_request *nr; |
287 | |
288 | nr = ncsi_alloc_request(ndp, req_flags: nca->req_flags); |
289 | if (!nr) |
290 | return NULL; |
291 | |
292 | /* NCSI command packet has 16-bytes header, payload, 4 bytes checksum. |
293 | * Payload needs padding so that the checksum field following payload is |
294 | * aligned to 32-bit boundary. |
295 | * The packet needs padding if its payload is less than 26 bytes to |
296 | * meet 64 bytes minimal ethernet frame length. |
297 | */ |
298 | len += sizeof(struct ncsi_cmd_pkt_hdr) + 4; |
299 | payload = ALIGN(nca->payload, 4); |
300 | len += max(payload, padding_bytes); |
301 | |
302 | /* Allocate skb */ |
303 | skb = alloc_skb(size: len, GFP_ATOMIC); |
304 | if (!skb) { |
305 | ncsi_free_request(nr); |
306 | return NULL; |
307 | } |
308 | |
309 | nr->cmd = skb; |
310 | skb_reserve(skb, len: hlen); |
311 | skb_reset_network_header(skb); |
312 | |
313 | skb->dev = dev; |
314 | skb->protocol = htons(ETH_P_NCSI); |
315 | |
316 | return nr; |
317 | } |
318 | |
319 | int ncsi_xmit_cmd(struct ncsi_cmd_arg *nca) |
320 | { |
321 | struct ncsi_cmd_handler *nch = NULL; |
322 | struct ncsi_request *nr; |
323 | unsigned char type; |
324 | struct ethhdr *eh; |
325 | int i, ret; |
326 | |
327 | /* Use OEM generic handler for Netlink request */ |
328 | if (nca->req_flags == NCSI_REQ_FLAG_NETLINK_DRIVEN) |
329 | type = NCSI_PKT_CMD_OEM; |
330 | else |
331 | type = nca->type; |
332 | |
333 | /* Search for the handler */ |
334 | for (i = 0; i < ARRAY_SIZE(ncsi_cmd_handlers); i++) { |
335 | if (ncsi_cmd_handlers[i].type == type) { |
336 | if (ncsi_cmd_handlers[i].handler) |
337 | nch = &ncsi_cmd_handlers[i]; |
338 | else |
339 | nch = NULL; |
340 | |
341 | break; |
342 | } |
343 | } |
344 | |
345 | if (!nch) { |
346 | netdev_err(dev: nca->ndp->ndev.dev, |
347 | format: "Cannot send packet with type 0x%02x\n" , nca->type); |
348 | return -ENOENT; |
349 | } |
350 | |
351 | /* Get packet payload length and allocate the request |
352 | * It is expected that if length set as negative in |
353 | * handler structure means caller is initializing it |
354 | * and setting length in nca before calling xmit function |
355 | */ |
356 | if (nch->payload >= 0) |
357 | nca->payload = nch->payload; |
358 | nr = ncsi_alloc_command(nca); |
359 | if (!nr) |
360 | return -ENOMEM; |
361 | |
362 | /* track netlink information */ |
363 | if (nca->req_flags == NCSI_REQ_FLAG_NETLINK_DRIVEN) { |
364 | nr->snd_seq = nca->info->snd_seq; |
365 | nr->snd_portid = nca->info->snd_portid; |
366 | nr->nlhdr = *nca->info->nlhdr; |
367 | } |
368 | |
369 | /* Prepare the packet */ |
370 | nca->id = nr->id; |
371 | ret = nch->handler(nr->cmd, nca); |
372 | if (ret) { |
373 | ncsi_free_request(nr); |
374 | return ret; |
375 | } |
376 | |
377 | /* Fill the ethernet header */ |
378 | eh = skb_push(skb: nr->cmd, len: sizeof(*eh)); |
379 | eh->h_proto = htons(ETH_P_NCSI); |
380 | eth_broadcast_addr(addr: eh->h_dest); |
381 | |
382 | /* If mac address received from device then use it for |
383 | * source address as unicast address else use broadcast |
384 | * address as source address |
385 | */ |
386 | if (nca->ndp->gma_flag == 1) |
387 | memcpy(eh->h_source, nca->ndp->ndev.dev->dev_addr, ETH_ALEN); |
388 | else |
389 | eth_broadcast_addr(addr: eh->h_source); |
390 | |
391 | /* Start the timer for the request that might not have |
392 | * corresponding response. Given NCSI is an internal |
393 | * connection a 1 second delay should be sufficient. |
394 | */ |
395 | nr->enabled = true; |
396 | mod_timer(timer: &nr->timer, expires: jiffies + 1 * HZ); |
397 | |
398 | /* Send NCSI packet */ |
399 | skb_get(skb: nr->cmd); |
400 | ret = dev_queue_xmit(skb: nr->cmd); |
401 | if (ret < 0) { |
402 | ncsi_free_request(nr); |
403 | return ret; |
404 | } |
405 | |
406 | return 0; |
407 | } |
408 | |