xref: /freebsd/contrib/wpa/src/drivers/driver_macsec_linux.c (revision 6966ac055c3b7a39266fb982493330df7a097997)
1 /*
2  * Driver interaction with Linux MACsec kernel module
3  * Copyright (c) 2016, Sabrina Dubroca <sd@queasysnail.net> and Red Hat, Inc.
4  * Copyright (c) 2019, The Linux Foundation
5  *
6  * This software may be distributed under the terms of the BSD license.
7  * See README for more details.
8  */
9 
10 #include "includes.h"
11 #include <sys/ioctl.h>
12 #include <net/if.h>
13 #include <netpacket/packet.h>
14 #include <net/if_arp.h>
15 #include <net/if.h>
16 #include <netlink/netlink.h>
17 #include <netlink/genl/genl.h>
18 #include <netlink/genl/ctrl.h>
19 #include <netlink/route/link.h>
20 #include <netlink/route/link/macsec.h>
21 #include <linux/if_macsec.h>
22 #include <inttypes.h>
23 
24 #include "utils/common.h"
25 #include "utils/eloop.h"
26 #include "common/eapol_common.h"
27 #include "pae/ieee802_1x_kay.h"
28 #include "driver.h"
29 #include "driver_wired_common.h"
30 
31 #define DRV_PREFIX "macsec_linux: "
32 
33 #define UNUSED_SCI 0xffffffffffffffff
34 
35 struct cb_arg {
36 	struct macsec_drv_data *drv;
37 	u32 *pn;
38 	int ifindex;
39 	u8 txsa;
40 	u8 rxsa;
41 	u64 rxsci;
42 };
43 
44 struct macsec_genl_ctx {
45 	struct nl_sock *sk;
46 	int macsec_genl_id;
47 	struct cb_arg cb_arg;
48 };
49 
50 struct macsec_drv_data {
51 	struct driver_wired_common_data common;
52 	struct rtnl_link *link;
53 	struct nl_cache *link_cache;
54 	struct nl_sock *sk;
55 	struct macsec_genl_ctx ctx;
56 
57 	struct netlink_data *netlink;
58 	struct nl_handle *nl;
59 	char ifname[IFNAMSIZ + 1];
60 	int ifi;
61 	int parent_ifi;
62 	int use_pae_group_addr;
63 
64 	Boolean created_link;
65 
66 	Boolean controlled_port_enabled;
67 	Boolean controlled_port_enabled_set;
68 
69 	Boolean protect_frames;
70 	Boolean protect_frames_set;
71 
72 	Boolean encrypt;
73 	Boolean encrypt_set;
74 
75 	Boolean replay_protect;
76 	Boolean replay_protect_set;
77 
78 	u32 replay_window;
79 
80 	u8 encoding_sa;
81 	Boolean encoding_sa_set;
82 };
83 
84 
85 static int dump_callback(struct nl_msg *msg, void *argp);
86 
87 
88 static struct nl_msg * msg_prepare(enum macsec_nl_commands cmd,
89 				   const struct macsec_genl_ctx *ctx,
90 				   unsigned int ifindex)
91 {
92 	struct nl_msg *msg;
93 
94 	msg = nlmsg_alloc();
95 	if (!msg) {
96 		wpa_printf(MSG_ERROR, DRV_PREFIX "failed to alloc message");
97 		return NULL;
98 	}
99 
100 	if (!genlmsg_put(msg, 0, 0, ctx->macsec_genl_id, 0, 0, cmd, 0)) {
101 		wpa_printf(MSG_ERROR, DRV_PREFIX "failed to put header");
102 		goto nla_put_failure;
103 	}
104 
105 	NLA_PUT_U32(msg, MACSEC_ATTR_IFINDEX, ifindex);
106 
107 	return msg;
108 
109 nla_put_failure:
110 	nlmsg_free(msg);
111 	return NULL;
112 }
113 
114 
115 static int nla_put_rxsc_config(struct nl_msg *msg, u64 sci)
116 {
117 	struct nlattr *nest = nla_nest_start(msg, MACSEC_ATTR_RXSC_CONFIG);
118 
119 	if (!nest)
120 		return -1;
121 
122 	NLA_PUT_U64(msg, MACSEC_RXSC_ATTR_SCI, sci);
123 
124 	nla_nest_end(msg, nest);
125 
126 	return 0;
127 
128 nla_put_failure:
129 	return -1;
130 }
131 
132 
133 static int init_genl_ctx(struct macsec_drv_data *drv)
134 {
135 	struct macsec_genl_ctx *ctx = &drv->ctx;
136 
137 	ctx->sk = nl_socket_alloc();
138 	if (!ctx->sk) {
139 		wpa_printf(MSG_ERROR, DRV_PREFIX "failed to alloc genl socket");
140 		return -1;
141 	}
142 
143 	if (genl_connect(ctx->sk) < 0) {
144 		wpa_printf(MSG_ERROR,
145 			   DRV_PREFIX "connection to genl socket failed");
146 		goto out_free;
147 	}
148 
149 	ctx->macsec_genl_id = genl_ctrl_resolve(ctx->sk, "macsec");
150 	if (ctx->macsec_genl_id < 0) {
151 		wpa_printf(MSG_ERROR, DRV_PREFIX "genl resolve failed");
152 		goto out_free;
153 	}
154 
155 	memset(&ctx->cb_arg, 0, sizeof(ctx->cb_arg));
156 	ctx->cb_arg.drv = drv;
157 
158 	nl_socket_modify_cb(ctx->sk, NL_CB_VALID, NL_CB_CUSTOM, dump_callback,
159 			    &ctx->cb_arg);
160 
161 	return 0;
162 
163 out_free:
164 	nl_socket_free(ctx->sk);
165 	ctx->sk = NULL;
166 	return -1;
167 }
168 
169 
170 static int try_commit(struct macsec_drv_data *drv)
171 {
172 	int err;
173 
174 	if (!drv->sk)
175 		return 0;
176 
177 	if (!drv->link)
178 		return 0;
179 
180 	if (drv->controlled_port_enabled_set) {
181 		struct rtnl_link *change = rtnl_link_alloc();
182 
183 		wpa_printf(MSG_DEBUG, DRV_PREFIX
184 			   "%s: try_commit controlled_port_enabled=%d",
185 			   drv->ifname, drv->controlled_port_enabled);
186 		if (!change)
187 			return -1;
188 
189 		rtnl_link_set_name(change, drv->ifname);
190 
191 		if (drv->controlled_port_enabled)
192 			rtnl_link_set_flags(change, IFF_UP);
193 		else
194 			rtnl_link_unset_flags(change, IFF_UP);
195 
196 		err = rtnl_link_change(drv->sk, change, change, 0);
197 		if (err < 0)
198 			return err;
199 
200 		rtnl_link_put(change);
201 
202 		drv->controlled_port_enabled_set = FALSE;
203 	}
204 
205 	if (drv->protect_frames_set) {
206 		wpa_printf(MSG_DEBUG, DRV_PREFIX
207 			   "%s: try_commit protect_frames=%d",
208 			   drv->ifname, drv->protect_frames);
209 		rtnl_link_macsec_set_protect(drv->link, drv->protect_frames);
210 	}
211 
212 	if (drv->encrypt_set) {
213 		wpa_printf(MSG_DEBUG, DRV_PREFIX "%s: try_commit encrypt=%d",
214 			   drv->ifname, drv->encrypt);
215 		rtnl_link_macsec_set_encrypt(drv->link, drv->encrypt);
216 	}
217 
218 	if (drv->replay_protect_set) {
219 		wpa_printf(MSG_DEBUG, DRV_PREFIX
220 			   "%s: try_commit replay_protect=%d replay_window=%d",
221 			   drv->ifname, drv->replay_protect,
222 			   drv->replay_window);
223 		rtnl_link_macsec_set_replay_protect(drv->link,
224 						    drv->replay_protect);
225 		if (drv->replay_protect)
226 			rtnl_link_macsec_set_window(drv->link,
227 						    drv->replay_window);
228 	}
229 
230 	if (drv->encoding_sa_set) {
231 		wpa_printf(MSG_DEBUG, DRV_PREFIX
232 			   "%s: try_commit encoding_sa=%d",
233 			   drv->ifname, drv->encoding_sa);
234 		rtnl_link_macsec_set_encoding_sa(drv->link, drv->encoding_sa);
235 	}
236 
237 	err = rtnl_link_add(drv->sk, drv->link, 0);
238 	if (err < 0)
239 		return err;
240 
241 	drv->protect_frames_set = FALSE;
242 	drv->encrypt_set = FALSE;
243 	drv->replay_protect_set = FALSE;
244 
245 	return 0;
246 }
247 
248 
249 static void macsec_drv_wpa_deinit(void *priv)
250 {
251 	struct macsec_drv_data *drv = priv;
252 
253 	driver_wired_deinit_common(&drv->common);
254 	os_free(drv);
255 }
256 
257 
258 static int macsec_check_macsec(void)
259 {
260 	struct nl_sock *sk;
261 	int err = -1;
262 
263 	sk = nl_socket_alloc();
264 	if (!sk) {
265 		wpa_printf(MSG_ERROR, DRV_PREFIX "failed to alloc genl socket");
266 		return -1;
267 	}
268 
269 	if (genl_connect(sk) < 0) {
270 		wpa_printf(MSG_ERROR,
271 			   DRV_PREFIX "connection to genl socket failed");
272 		goto out_free;
273 	}
274 
275 	if (genl_ctrl_resolve(sk, "macsec") < 0) {
276 		wpa_printf(MSG_ERROR,
277 			   DRV_PREFIX "genl resolve failed - macsec kernel module not present?");
278 		goto out_free;
279 	}
280 
281 	err = 0;
282 
283 out_free:
284 	nl_socket_free(sk);
285 	return err;
286 }
287 
288 
289 static void * macsec_drv_wpa_init(void *ctx, const char *ifname)
290 {
291 	struct macsec_drv_data *drv;
292 
293 	if (macsec_check_macsec() < 0)
294 		return NULL;
295 
296 	drv = os_zalloc(sizeof(*drv));
297 	if (!drv)
298 		return NULL;
299 
300 	if (driver_wired_init_common(&drv->common, ifname, ctx) < 0) {
301 		os_free(drv);
302 		return NULL;
303 	}
304 
305 	return drv;
306 }
307 
308 
309 static int macsec_drv_macsec_init(void *priv, struct macsec_init_params *params)
310 {
311 	struct macsec_drv_data *drv = priv;
312 	int err;
313 
314 	wpa_printf(MSG_DEBUG, "%s", __func__);
315 
316 	drv->sk = nl_socket_alloc();
317 	if (!drv->sk)
318 		return -1;
319 
320 	err = nl_connect(drv->sk, NETLINK_ROUTE);
321 	if (err < 0) {
322 		wpa_printf(MSG_ERROR, DRV_PREFIX
323 			   "Unable to connect NETLINK_ROUTE socket: %s",
324 			   strerror(errno));
325 		goto sock;
326 	}
327 
328 	err = rtnl_link_alloc_cache(drv->sk, AF_UNSPEC, &drv->link_cache);
329 	if (err < 0) {
330 		wpa_printf(MSG_ERROR, DRV_PREFIX "Unable to get link cache: %s",
331 			   strerror(errno));
332 		goto sock;
333 	}
334 
335 	drv->parent_ifi = rtnl_link_name2i(drv->link_cache, drv->common.ifname);
336 	if (drv->parent_ifi == 0) {
337 		wpa_printf(MSG_ERROR, DRV_PREFIX
338 			   "couldn't find ifindex for interface %s",
339 			   drv->common.ifname);
340 		goto cache;
341 	}
342 	wpa_printf(MSG_DEBUG, DRV_PREFIX "ifname=%s parent_ifi=%d",
343 		   drv->common.ifname, drv->parent_ifi);
344 
345 	err = init_genl_ctx(drv);
346 	if (err < 0)
347 		goto cache;
348 
349 	return 0;
350 
351 cache:
352 	nl_cache_free(drv->link_cache);
353 	drv->link_cache = NULL;
354 sock:
355 	nl_socket_free(drv->sk);
356 	drv->sk = NULL;
357 	return -1;
358 }
359 
360 
361 static int macsec_drv_macsec_deinit(void *priv)
362 {
363 	struct macsec_drv_data *drv = priv;
364 
365 	wpa_printf(MSG_DEBUG, "%s", __func__);
366 
367 	if (drv->sk)
368 		nl_socket_free(drv->sk);
369 	drv->sk = NULL;
370 
371 	if (drv->link_cache)
372 		nl_cache_free(drv->link_cache);
373 	drv->link_cache = NULL;
374 
375 	if (drv->ctx.sk)
376 		nl_socket_free(drv->ctx.sk);
377 
378 	return 0;
379 }
380 
381 
382 static int macsec_drv_get_capability(void *priv, enum macsec_cap *cap)
383 {
384 	wpa_printf(MSG_DEBUG, "%s", __func__);
385 
386 	*cap = MACSEC_CAP_INTEG_AND_CONF;
387 
388 	return 0;
389 }
390 
391 
392 /**
393  * macsec_drv_enable_protect_frames - Set protect frames status
394  * @priv: Private driver interface data
395  * @enabled: TRUE = protect frames enabled
396  *           FALSE = protect frames disabled
397  * Returns: 0 on success, -1 on failure (or if not supported)
398  */
399 static int macsec_drv_enable_protect_frames(void *priv, Boolean enabled)
400 {
401 	struct macsec_drv_data *drv = priv;
402 
403 	wpa_printf(MSG_DEBUG, "%s -> %s", __func__, enabled ? "TRUE" : "FALSE");
404 
405 	drv->protect_frames_set = TRUE;
406 	drv->protect_frames = enabled;
407 
408 	return try_commit(drv);
409 }
410 
411 
412 /**
413  * macsec_drv_enable_encrypt - Set protect frames status
414  * @priv: Private driver interface data
415  * @enabled: TRUE = protect frames enabled
416  *           FALSE = protect frames disabled
417  * Returns: 0 on success, -1 on failure (or if not supported)
418  */
419 static int macsec_drv_enable_encrypt(void *priv, Boolean enabled)
420 {
421 	struct macsec_drv_data *drv = priv;
422 
423 	wpa_printf(MSG_DEBUG, "%s -> %s", __func__, enabled ? "TRUE" : "FALSE");
424 
425 	drv->encrypt_set = TRUE;
426 	drv->encrypt = enabled;
427 
428 	return try_commit(drv);
429 }
430 
431 
432 /**
433  * macsec_drv_set_replay_protect - Set replay protect status and window size
434  * @priv: Private driver interface data
435  * @enabled: TRUE = replay protect enabled
436  *           FALSE = replay protect disabled
437  * @window: replay window size, valid only when replay protect enabled
438  * Returns: 0 on success, -1 on failure (or if not supported)
439  */
440 static int macsec_drv_set_replay_protect(void *priv, Boolean enabled,
441 					 u32 window)
442 {
443 	struct macsec_drv_data *drv = priv;
444 
445 	wpa_printf(MSG_DEBUG, "%s -> %s, %u", __func__,
446 		   enabled ? "TRUE" : "FALSE", window);
447 
448 	drv->replay_protect_set = TRUE;
449 	drv->replay_protect = enabled;
450 	if (enabled)
451 		drv->replay_window = window;
452 
453 	return try_commit(drv);
454 }
455 
456 
457 /**
458  * macsec_drv_set_current_cipher_suite - Set current cipher suite
459  * @priv: Private driver interface data
460  * @cs: EUI64 identifier
461  * Returns: 0 on success, -1 on failure (or if not supported)
462  */
463 static int macsec_drv_set_current_cipher_suite(void *priv, u64 cs)
464 {
465 	wpa_printf(MSG_DEBUG, "%s -> %016" PRIx64, __func__, cs);
466 	return 0;
467 }
468 
469 
470 /**
471  * macsec_drv_enable_controlled_port - Set controlled port status
472  * @priv: Private driver interface data
473  * @enabled: TRUE = controlled port enabled
474  *           FALSE = controlled port disabled
475  * Returns: 0 on success, -1 on failure (or if not supported)
476  */
477 static int macsec_drv_enable_controlled_port(void *priv, Boolean enabled)
478 {
479 	struct macsec_drv_data *drv = priv;
480 
481 	wpa_printf(MSG_DEBUG, "%s -> %s", __func__, enabled ? "TRUE" : "FALSE");
482 
483 	drv->controlled_port_enabled = enabled;
484 	drv->controlled_port_enabled_set = TRUE;
485 
486 	return try_commit(drv);
487 }
488 
489 
490 static struct nla_policy sa_policy[MACSEC_SA_ATTR_MAX + 1] = {
491 	[MACSEC_SA_ATTR_AN] = { .type = NLA_U8 },
492 	[MACSEC_SA_ATTR_ACTIVE] = { .type = NLA_U8 },
493 	[MACSEC_SA_ATTR_PN] = { .type = NLA_U32 },
494 	[MACSEC_SA_ATTR_KEYID] = { .type = NLA_BINARY },
495 };
496 
497 static struct nla_policy sc_policy[MACSEC_RXSC_ATTR_MAX + 1] = {
498 	[MACSEC_RXSC_ATTR_SCI] = { .type = NLA_U64 },
499 	[MACSEC_RXSC_ATTR_ACTIVE] = { .type = NLA_U8 },
500 	[MACSEC_RXSC_ATTR_SA_LIST] = { .type = NLA_NESTED },
501 };
502 
503 static struct nla_policy main_policy[MACSEC_ATTR_MAX + 1] = {
504 	[MACSEC_ATTR_IFINDEX] = { .type = NLA_U32 },
505 	[MACSEC_ATTR_SECY] = { .type = NLA_NESTED },
506 	[MACSEC_ATTR_TXSA_LIST] = { .type = NLA_NESTED },
507 	[MACSEC_ATTR_RXSC_LIST] = { .type = NLA_NESTED },
508 };
509 
510 static int dump_callback(struct nl_msg *msg, void *argp)
511 {
512 	struct nlmsghdr *ret_hdr = nlmsg_hdr(msg);
513 	struct nlattr *tb_msg[MACSEC_ATTR_MAX + 1];
514 	struct cb_arg *arg = (struct cb_arg *) argp;
515 	struct genlmsghdr *gnlh = (struct genlmsghdr *) nlmsg_data(ret_hdr);
516 	int err;
517 
518 	if (ret_hdr->nlmsg_type != arg->drv->ctx.macsec_genl_id)
519 		return 0;
520 
521 	err = nla_parse(tb_msg, MACSEC_ATTR_MAX, genlmsg_attrdata(gnlh, 0),
522 			genlmsg_attrlen(gnlh, 0), main_policy);
523 	if (err < 0)
524 		return 0;
525 
526 	if (!tb_msg[MACSEC_ATTR_IFINDEX])
527 		return 0;
528 
529 	if (nla_get_u32(tb_msg[MACSEC_ATTR_IFINDEX]) != (u32) arg->ifindex)
530 		return 0;
531 
532 	if (arg->txsa < 4 && !tb_msg[MACSEC_ATTR_TXSA_LIST]) {
533 		return 0;
534 	} else if (arg->txsa < 4) {
535 		struct nlattr *nla;
536 		int rem;
537 
538 		nla_for_each_nested(nla, tb_msg[MACSEC_ATTR_TXSA_LIST], rem) {
539 			struct nlattr *tb[MACSEC_SA_ATTR_MAX + 1];
540 
541 			err = nla_parse_nested(tb, MACSEC_SA_ATTR_MAX, nla,
542 					       sa_policy);
543 			if (err < 0)
544 				continue;
545 			if (!tb[MACSEC_SA_ATTR_AN])
546 				continue;
547 			if (nla_get_u8(tb[MACSEC_SA_ATTR_AN]) != arg->txsa)
548 				continue;
549 			if (!tb[MACSEC_SA_ATTR_PN])
550 				return 0;
551 			*arg->pn = nla_get_u32(tb[MACSEC_SA_ATTR_PN]);
552 			return 0;
553 		}
554 
555 		return 0;
556 	}
557 
558 	if (arg->rxsci == UNUSED_SCI)
559 		return 0;
560 
561 	if (tb_msg[MACSEC_ATTR_RXSC_LIST]) {
562 		struct nlattr *nla;
563 		int rem;
564 
565 		nla_for_each_nested(nla, tb_msg[MACSEC_ATTR_RXSC_LIST], rem) {
566 			struct nlattr *tb[MACSEC_RXSC_ATTR_MAX + 1];
567 
568 			err = nla_parse_nested(tb, MACSEC_RXSC_ATTR_MAX, nla,
569 					       sc_policy);
570 			if (err < 0)
571 				return 0;
572 			if (!tb[MACSEC_RXSC_ATTR_SCI])
573 				continue;
574 			if (nla_get_u64(tb[MACSEC_RXSC_ATTR_SCI]) != arg->rxsci)
575 				continue;
576 			if (!tb[MACSEC_RXSC_ATTR_SA_LIST])
577 				return 0;
578 
579 			nla_for_each_nested(nla, tb[MACSEC_RXSC_ATTR_SA_LIST],
580 					    rem) {
581 				struct nlattr *tb_sa[MACSEC_SA_ATTR_MAX + 1];
582 
583 				err = nla_parse_nested(tb_sa,
584 						       MACSEC_SA_ATTR_MAX, nla,
585 						       sa_policy);
586 				if (err < 0)
587 					continue;
588 				if (!tb_sa[MACSEC_SA_ATTR_AN])
589 					continue;
590 				if (nla_get_u8(tb_sa[MACSEC_SA_ATTR_AN]) !=
591 				    arg->rxsa)
592 					continue;
593 				if (!tb_sa[MACSEC_SA_ATTR_PN])
594 					return 0;
595 				*arg->pn =
596 					nla_get_u32(tb_sa[MACSEC_SA_ATTR_PN]);
597 
598 				return 0;
599 			}
600 
601 			return 0;
602 		}
603 
604 		return 0;
605 	}
606 
607 	return 0;
608 }
609 
610 
611 static int nl_send_recv(struct nl_sock *sk, struct nl_msg *msg)
612 {
613 	int ret;
614 
615 	ret = nl_send_auto_complete(sk, msg);
616 	if (ret < 0) {
617 		wpa_printf(MSG_ERROR, DRV_PREFIX "%s: failed to send: %d (%s)",
618 			   __func__, ret, nl_geterror(-ret));
619 		return ret;
620 	}
621 
622 	ret = nl_recvmsgs_default(sk);
623 	if (ret < 0) {
624 		wpa_printf(MSG_ERROR, DRV_PREFIX "%s: failed to recv: %d (%s)",
625 			   __func__, ret, nl_geterror(-ret));
626 	}
627 
628 	return ret;
629 }
630 
631 
632 static int do_dump(struct macsec_drv_data *drv, u8 txsa, u64 rxsci, u8 rxsa,
633 		   u32 *pn)
634 {
635 	struct macsec_genl_ctx *ctx = &drv->ctx;
636 	struct nl_msg *msg;
637 	int ret = 1;
638 
639 	ctx->cb_arg.ifindex = drv->ifi;
640 	ctx->cb_arg.rxsci = rxsci;
641 	ctx->cb_arg.rxsa = rxsa;
642 	ctx->cb_arg.txsa = txsa;
643 	ctx->cb_arg.pn = pn;
644 
645 	msg = nlmsg_alloc();
646 	if (!msg) {
647 		wpa_printf(MSG_ERROR, DRV_PREFIX "%s: failed to alloc message",
648 			   __func__);
649 		return 1;
650 	}
651 
652 	if (!genlmsg_put(msg, NL_AUTO_PORT, NL_AUTO_SEQ, ctx->macsec_genl_id, 0,
653 			 NLM_F_DUMP, MACSEC_CMD_GET_TXSC, 0)) {
654 		wpa_printf(MSG_ERROR, DRV_PREFIX "%s: failed to put header",
655 			   __func__);
656 		goto out_free_msg;
657 	}
658 
659 	ret = nl_send_recv(ctx->sk, msg);
660 	if (ret < 0)
661 		wpa_printf(MSG_ERROR,
662 			   DRV_PREFIX "failed to communicate: %d (%s)",
663 			   ret, nl_geterror(-ret));
664 
665 	ctx->cb_arg.pn = NULL;
666 
667 out_free_msg:
668 	nlmsg_free(msg);
669 	return ret;
670 }
671 
672 
673 /**
674  * macsec_drv_get_receive_lowest_pn - Get receive lowest PN
675  * @priv: Private driver interface data
676  * @sa: secure association
677  * Returns: 0 on success, -1 on failure (or if not supported)
678  */
679 static int macsec_drv_get_receive_lowest_pn(void *priv, struct receive_sa *sa)
680 {
681 	struct macsec_drv_data *drv = priv;
682 	int err;
683 
684 	wpa_printf(MSG_DEBUG, DRV_PREFIX "%s", __func__);
685 
686 	err = do_dump(drv, 0xff, mka_sci_u64(&sa->sc->sci), sa->an,
687 		      &sa->lowest_pn);
688 	wpa_printf(MSG_DEBUG, DRV_PREFIX "%s: result %d", __func__,
689 		   sa->lowest_pn);
690 
691 	return err;
692 }
693 
694 
695 /**
696  * macsec_drv_set_receive_lowest_pn - Set receive lowest PN
697  * @priv: Private driver interface data
698  * @sa: secure association
699  * Returns: 0 on success, -1 on failure (or if not supported)
700  */
701 static int macsec_drv_set_receive_lowest_pn(void *priv, struct receive_sa *sa)
702 {
703 	struct macsec_drv_data *drv = priv;
704 	struct macsec_genl_ctx *ctx = &drv->ctx;
705 	struct nl_msg *msg;
706 	struct nlattr *nest;
707 	int ret = -1;
708 
709 	wpa_printf(MSG_DEBUG,
710 		   DRV_PREFIX "%s: set_receive_lowest_pn -> %d: %d",
711 		   drv->ifname, sa->an, sa->next_pn);
712 
713 	msg = msg_prepare(MACSEC_CMD_UPD_RXSA, ctx, drv->ifi);
714 	if (!msg)
715 		return ret;
716 
717 	nest = nla_nest_start(msg, MACSEC_ATTR_SA_CONFIG);
718 	if (!nest)
719 		goto nla_put_failure;
720 
721 	NLA_PUT_U8(msg, MACSEC_SA_ATTR_AN, sa->an);
722 	NLA_PUT_U32(msg, MACSEC_SA_ATTR_PN, sa->next_pn);
723 
724 	nla_nest_end(msg, nest);
725 
726 	ret = nl_send_recv(ctx->sk, msg);
727 	if (ret < 0) {
728 		wpa_printf(MSG_ERROR,
729 			   DRV_PREFIX "failed to communicate: %d (%s)",
730 			   ret, nl_geterror(-ret));
731 	}
732 
733 nla_put_failure:
734 	nlmsg_free(msg);
735 	return ret;
736 }
737 
738 
739 /**
740  * macsec_drv_get_transmit_next_pn - Get transmit next PN
741  * @priv: Private driver interface data
742  * @sa: secure association
743  * Returns: 0 on success, -1 on failure (or if not supported)
744  */
745 static int macsec_drv_get_transmit_next_pn(void *priv, struct transmit_sa *sa)
746 {
747 	struct macsec_drv_data *drv = priv;
748 	int err;
749 
750 	wpa_printf(MSG_DEBUG, "%s", __func__);
751 
752 	err = do_dump(drv, sa->an, UNUSED_SCI, 0xff, &sa->next_pn);
753 	wpa_printf(MSG_DEBUG, DRV_PREFIX "%s: err %d result %d", __func__, err,
754 		   sa->next_pn);
755 	return err;
756 }
757 
758 
759 /**
760  * macsec_drv_set_transmit_next_pn - Set transmit next pn
761  * @priv: Private driver interface data
762  * @sa: secure association
763  * Returns: 0 on success, -1 on failure (or if not supported)
764  */
765 static int macsec_drv_set_transmit_next_pn(void *priv, struct transmit_sa *sa)
766 {
767 	struct macsec_drv_data *drv = priv;
768 	struct macsec_genl_ctx *ctx = &drv->ctx;
769 	struct nl_msg *msg;
770 	struct nlattr *nest;
771 	int ret = -1;
772 
773 	wpa_printf(MSG_DEBUG, "%s -> %d: %d", __func__, sa->an, sa->next_pn);
774 
775 	msg = msg_prepare(MACSEC_CMD_UPD_TXSA, ctx, drv->ifi);
776 	if (!msg)
777 		return ret;
778 
779 	nest = nla_nest_start(msg, MACSEC_ATTR_SA_CONFIG);
780 	if (!nest)
781 		goto nla_put_failure;
782 
783 	NLA_PUT_U8(msg, MACSEC_SA_ATTR_AN, sa->an);
784 	NLA_PUT_U32(msg, MACSEC_SA_ATTR_PN, sa->next_pn);
785 
786 	nla_nest_end(msg, nest);
787 
788 	ret = nl_send_recv(ctx->sk, msg);
789 	if (ret < 0) {
790 		wpa_printf(MSG_ERROR,
791 			   DRV_PREFIX "failed to communicate: %d (%s)",
792 			   ret, nl_geterror(-ret));
793 	}
794 
795 nla_put_failure:
796 	nlmsg_free(msg);
797 	return ret;
798 }
799 
800 
801 #define SCISTR MACSTR "::%hx"
802 #define SCI2STR(addr, port) MAC2STR(addr), htons(port)
803 
804 /**
805  * macsec_drv_create_receive_sc - Create secure channel for receiving
806  * @priv: Private driver interface data
807  * @sc: secure channel
808  * @sci_addr: secure channel identifier - address
809  * @sci_port: secure channel identifier - port
810  * @conf_offset: confidentiality offset (0, 30, or 50)
811  * @validation: frame validation policy (0 = Disabled, 1 = Checked,
812  *	2 = Strict)
813  * Returns: 0 on success, -1 on failure (or if not supported)
814  */
815 static int macsec_drv_create_receive_sc(void *priv, struct receive_sc *sc,
816 					unsigned int conf_offset,
817 					int validation)
818 {
819 	struct macsec_drv_data *drv = priv;
820 	struct macsec_genl_ctx *ctx = &drv->ctx;
821 	struct nl_msg *msg;
822 	int ret = -1;
823 
824 	wpa_printf(MSG_DEBUG, DRV_PREFIX "%s: create_receive_sc -> " SCISTR
825 		   " (conf_offset=%u validation=%d)",
826 		   drv->ifname, SCI2STR(sc->sci.addr, sc->sci.port),
827 		   conf_offset, validation);
828 
829 	msg = msg_prepare(MACSEC_CMD_ADD_RXSC, ctx, drv->ifi);
830 	if (!msg)
831 		return ret;
832 
833 	if (nla_put_rxsc_config(msg, mka_sci_u64(&sc->sci)))
834 		goto nla_put_failure;
835 
836 	ret = nl_send_recv(ctx->sk, msg);
837 	if (ret < 0) {
838 		wpa_printf(MSG_ERROR,
839 			   DRV_PREFIX "%s: failed to communicate: %d (%s)",
840 			   __func__, ret, nl_geterror(-ret));
841 	}
842 
843 nla_put_failure:
844 	nlmsg_free(msg);
845 	return ret;
846 }
847 
848 
849 /**
850  * macsec_drv_delete_receive_sc - Delete secure connection for receiving
851  * @priv: private driver interface data from init()
852  * @sc: secure channel
853  * Returns: 0 on success, -1 on failure
854  */
855 static int macsec_drv_delete_receive_sc(void *priv, struct receive_sc *sc)
856 {
857 	struct macsec_drv_data *drv = priv;
858 	struct macsec_genl_ctx *ctx = &drv->ctx;
859 	struct nl_msg *msg;
860 	int ret = -1;
861 
862 	wpa_printf(MSG_DEBUG, DRV_PREFIX "%s: delete_receive_sc -> " SCISTR,
863 		   drv->ifname, SCI2STR(sc->sci.addr, sc->sci.port));
864 
865 	msg = msg_prepare(MACSEC_CMD_DEL_RXSC, ctx, drv->ifi);
866 	if (!msg)
867 		return ret;
868 
869 	if (nla_put_rxsc_config(msg, mka_sci_u64(&sc->sci)))
870 		goto nla_put_failure;
871 
872 	ret = nl_send_recv(ctx->sk, msg);
873 	if (ret < 0) {
874 		wpa_printf(MSG_ERROR,
875 			   DRV_PREFIX "%s: failed to communicate: %d (%s)",
876 			   __func__, ret, nl_geterror(-ret));
877 	}
878 
879 nla_put_failure:
880 	nlmsg_free(msg);
881 	return ret;
882 }
883 
884 
885 /**
886  * macsec_drv_create_receive_sa - Create secure association for receive
887  * @priv: private driver interface data from init()
888  * @sa: secure association
889  * Returns: 0 on success, -1 on failure
890  */
891 static int macsec_drv_create_receive_sa(void *priv, struct receive_sa *sa)
892 {
893 	struct macsec_drv_data *drv = priv;
894 	struct macsec_genl_ctx *ctx = &drv->ctx;
895 	struct nl_msg *msg;
896 	struct nlattr *nest;
897 	int ret = -1;
898 
899 	wpa_printf(MSG_DEBUG,
900 		   DRV_PREFIX "%s: create_receive_sa -> %d on " SCISTR
901 		   " (enable_receive=%d next_pn=%u)",
902 		   drv->ifname, sa->an,
903 		   SCI2STR(sa->sc->sci.addr, sa->sc->sci.port),
904 		   sa->enable_receive, sa->next_pn);
905 	wpa_hexdump(MSG_DEBUG, DRV_PREFIX "SA keyid",
906 		    &sa->pkey->key_identifier,
907 		    sizeof(sa->pkey->key_identifier));
908 	wpa_hexdump_key(MSG_DEBUG, DRV_PREFIX "SA key",
909 			sa->pkey->key, sa->pkey->key_len);
910 
911 	msg = msg_prepare(MACSEC_CMD_ADD_RXSA, ctx, drv->ifi);
912 	if (!msg)
913 		return ret;
914 
915 	if (nla_put_rxsc_config(msg, mka_sci_u64(&sa->sc->sci)))
916 		goto nla_put_failure;
917 
918 	nest = nla_nest_start(msg, MACSEC_ATTR_SA_CONFIG);
919 	if (!nest)
920 		goto nla_put_failure;
921 
922 	NLA_PUT_U8(msg, MACSEC_SA_ATTR_AN, sa->an);
923 	NLA_PUT_U8(msg, MACSEC_SA_ATTR_ACTIVE, sa->enable_receive);
924 	NLA_PUT_U32(msg, MACSEC_SA_ATTR_PN, sa->next_pn);
925 	NLA_PUT(msg, MACSEC_SA_ATTR_KEYID, sizeof(sa->pkey->key_identifier),
926 		&sa->pkey->key_identifier);
927 	NLA_PUT(msg, MACSEC_SA_ATTR_KEY, sa->pkey->key_len, sa->pkey->key);
928 
929 	nla_nest_end(msg, nest);
930 
931 	ret = nl_send_recv(ctx->sk, msg);
932 	if (ret < 0) {
933 		wpa_printf(MSG_ERROR,
934 			   DRV_PREFIX "%s: failed to communicate: %d (%s)",
935 			   __func__, ret, nl_geterror(-ret));
936 	}
937 
938 nla_put_failure:
939 	nlmsg_free(msg);
940 	return ret;
941 }
942 
943 
944 /**
945  * macsec_drv_delete_receive_sa - Delete secure association for receive
946  * @priv: private driver interface data from init()
947  * @sa: secure association
948  * Returns: 0 on success, -1 on failure
949  */
950 static int macsec_drv_delete_receive_sa(void *priv, struct receive_sa *sa)
951 {
952 	struct macsec_drv_data *drv = priv;
953 	struct macsec_genl_ctx *ctx = &drv->ctx;
954 	struct nl_msg *msg;
955 	struct nlattr *nest;
956 	int ret = -1;
957 
958 	wpa_printf(MSG_DEBUG, DRV_PREFIX "%s: delete_receive_sa -> %d on "
959 		   SCISTR, drv->ifname, sa->an,
960 		   SCI2STR(sa->sc->sci.addr, sa->sc->sci.port));
961 
962 	msg = msg_prepare(MACSEC_CMD_DEL_RXSA, ctx, drv->ifi);
963 	if (!msg)
964 		return ret;
965 
966 	if (nla_put_rxsc_config(msg, mka_sci_u64(&sa->sc->sci)))
967 		goto nla_put_failure;
968 
969 	nest = nla_nest_start(msg, MACSEC_ATTR_SA_CONFIG);
970 	if (!nest)
971 		goto nla_put_failure;
972 
973 	NLA_PUT_U8(msg, MACSEC_SA_ATTR_AN, sa->an);
974 
975 	nla_nest_end(msg, nest);
976 
977 	ret = nl_send_recv(ctx->sk, msg);
978 	if (ret < 0) {
979 		wpa_printf(MSG_ERROR,
980 			   DRV_PREFIX "%s: failed to communicate: %d (%s)",
981 			   __func__, ret, nl_geterror(-ret));
982 	}
983 
984 nla_put_failure:
985 	nlmsg_free(msg);
986 	return ret;
987 }
988 
989 
990 static int set_active_rx_sa(const struct macsec_genl_ctx *ctx, int ifindex,
991 			    u64 sci, unsigned char an, Boolean state)
992 {
993 	struct nl_msg *msg;
994 	struct nlattr *nest;
995 	int ret = -1;
996 
997 	msg = msg_prepare(MACSEC_CMD_UPD_RXSA, ctx, ifindex);
998 	if (!msg)
999 		return ret;
1000 
1001 	if (nla_put_rxsc_config(msg, sci))
1002 		goto nla_put_failure;
1003 
1004 	nest = nla_nest_start(msg, MACSEC_ATTR_SA_CONFIG);
1005 	if (!nest)
1006 		goto nla_put_failure;
1007 
1008 	NLA_PUT_U8(msg, MACSEC_SA_ATTR_AN, an);
1009 	NLA_PUT_U8(msg, MACSEC_SA_ATTR_ACTIVE, !!state);
1010 
1011 	nla_nest_end(msg, nest);
1012 
1013 	ret = nl_send_recv(ctx->sk, msg);
1014 	if (ret < 0)
1015 		wpa_printf(MSG_ERROR,
1016 			   DRV_PREFIX "%s: failed to communicate: %d (%s)",
1017 			   __func__, ret, nl_geterror(-ret));
1018 
1019 nla_put_failure:
1020 	nlmsg_free(msg);
1021 	return ret;
1022 }
1023 
1024 
1025 /**
1026  * macsec_drv_enable_receive_sa - Enable the SA for receive
1027  * @priv: private driver interface data from init()
1028  * @sa: secure association
1029  * Returns: 0 on success, -1 on failure
1030  */
1031 static int macsec_drv_enable_receive_sa(void *priv, struct receive_sa *sa)
1032 {
1033 	struct macsec_drv_data *drv = priv;
1034 	struct macsec_genl_ctx *ctx = &drv->ctx;
1035 
1036 	wpa_printf(MSG_DEBUG, DRV_PREFIX "%s: enable_receive_sa -> %d on "
1037 		   SCISTR, drv->ifname, sa->an,
1038 		   SCI2STR(sa->sc->sci.addr, sa->sc->sci.port));
1039 
1040 	return set_active_rx_sa(ctx, drv->ifi, mka_sci_u64(&sa->sc->sci),
1041 				sa->an, TRUE);
1042 }
1043 
1044 
1045 /**
1046  * macsec_drv_disable_receive_sa - Disable SA for receive
1047  * @priv: private driver interface data from init()
1048  * @sa: secure association
1049  * Returns: 0 on success, -1 on failure
1050  */
1051 static int macsec_drv_disable_receive_sa(void *priv, struct receive_sa *sa)
1052 {
1053 	struct macsec_drv_data *drv = priv;
1054 	struct macsec_genl_ctx *ctx = &drv->ctx;
1055 
1056 	wpa_printf(MSG_DEBUG, DRV_PREFIX "%s: disable_receive_sa -> %d on "
1057 		   SCISTR, drv->ifname, sa->an,
1058 		   SCI2STR(sa->sc->sci.addr, sa->sc->sci.port));
1059 
1060 	return set_active_rx_sa(ctx, drv->ifi, mka_sci_u64(&sa->sc->sci),
1061 				sa->an, FALSE);
1062 }
1063 
1064 
1065 static struct rtnl_link * lookup_sc(struct nl_cache *cache, int parent, u64 sci)
1066 {
1067 	struct rtnl_link *needle;
1068 	void *match;
1069 
1070 	needle = rtnl_link_macsec_alloc();
1071 	if (!needle)
1072 		return NULL;
1073 
1074 	rtnl_link_set_link(needle, parent);
1075 	rtnl_link_macsec_set_sci(needle, sci);
1076 
1077 	match = nl_cache_find(cache, (struct nl_object *) needle);
1078 	rtnl_link_put(needle);
1079 
1080 	return (struct rtnl_link *) match;
1081 }
1082 
1083 
1084 /**
1085  * macsec_drv_create_transmit_sc - Create secure connection for transmit
1086  * @priv: private driver interface data from init()
1087  * @sc: secure channel
1088  * @conf_offset: confidentiality offset
1089  * Returns: 0 on success, -1 on failure
1090  */
1091 static int macsec_drv_create_transmit_sc(
1092 	void *priv, struct transmit_sc *sc,
1093 	unsigned int conf_offset)
1094 {
1095 	struct macsec_drv_data *drv = priv;
1096 	struct rtnl_link *link;
1097 	char *ifname;
1098 	u64 sci;
1099 	int err;
1100 
1101 	wpa_printf(MSG_DEBUG, DRV_PREFIX
1102 		   "%s: create_transmit_sc -> " SCISTR " (conf_offset=%d)",
1103 		   drv->common.ifname, SCI2STR(sc->sci.addr, sc->sci.port),
1104 		   conf_offset);
1105 
1106 	if (!drv->sk) {
1107 		wpa_printf(MSG_ERROR, DRV_PREFIX "NULL rtnl socket");
1108 		return -1;
1109 	}
1110 
1111 	link = rtnl_link_macsec_alloc();
1112 	if (!link) {
1113 		wpa_printf(MSG_ERROR, DRV_PREFIX "couldn't allocate link");
1114 		return -1;
1115 	}
1116 
1117 	rtnl_link_set_link(link, drv->parent_ifi);
1118 
1119 	sci = mka_sci_u64(&sc->sci);
1120 	rtnl_link_macsec_set_sci(link, sci);
1121 
1122 	drv->created_link = TRUE;
1123 
1124 	err = rtnl_link_add(drv->sk, link, NLM_F_CREATE);
1125 	if (err == -NLE_BUSY) {
1126 		wpa_printf(MSG_INFO,
1127 			   DRV_PREFIX "link already exists, using it");
1128 		drv->created_link = FALSE;
1129 	} else if (err < 0) {
1130 		rtnl_link_put(link);
1131 		wpa_printf(MSG_ERROR, DRV_PREFIX "couldn't create link: err %d",
1132 			   err);
1133 		return err;
1134 	}
1135 
1136 	rtnl_link_put(link);
1137 
1138 	nl_cache_refill(drv->sk, drv->link_cache);
1139 	link = lookup_sc(drv->link_cache, drv->parent_ifi, sci);
1140 	if (!link) {
1141 		wpa_printf(MSG_ERROR, DRV_PREFIX "couldn't find link");
1142 		return -1;
1143 	}
1144 
1145 	drv->ifi = rtnl_link_get_ifindex(link);
1146 	ifname = rtnl_link_get_name(link);
1147 	wpa_printf(MSG_DEBUG,
1148 		   DRV_PREFIX "%s: create_transmit_sc: ifi=%d ifname=%s",
1149 		   drv->common.ifname, drv->ifi, ifname);
1150 	os_strlcpy(drv->ifname, ifname, sizeof(drv->ifname));
1151 	rtnl_link_put(link);
1152 
1153 	drv->link = rtnl_link_macsec_alloc();
1154 	if (!drv->link) {
1155 		wpa_printf(MSG_ERROR, DRV_PREFIX "couldn't allocate link");
1156 		return -1;
1157 	}
1158 
1159 	rtnl_link_set_name(drv->link, drv->ifname);
1160 
1161 	/* In case some settings have already been done but we couldn't apply
1162 	 * them. */
1163 	return try_commit(drv);
1164 }
1165 
1166 
1167 /**
1168  * macsec_drv_delete_transmit_sc - Delete secure connection for transmit
1169  * @priv: private driver interface data from init()
1170  * @sc: secure channel
1171  * Returns: 0 on success, -1 on failure
1172  */
1173 static int macsec_drv_delete_transmit_sc(void *priv, struct transmit_sc *sc)
1174 {
1175 	struct macsec_drv_data *drv = priv;
1176 	int err;
1177 
1178 	wpa_printf(MSG_DEBUG, DRV_PREFIX "%s: delete_transmit_sc -> " SCISTR,
1179 		   drv->ifname, SCI2STR(sc->sci.addr, sc->sci.port));
1180 
1181 	if (!drv->sk)
1182 		return 0;
1183 
1184 	if (!drv->created_link) {
1185 		rtnl_link_put(drv->link);
1186 		drv->link = NULL;
1187 		wpa_printf(MSG_DEBUG, DRV_PREFIX
1188 			   "we didn't create the link, leave it alone");
1189 		return 0;
1190 	}
1191 
1192 	err = rtnl_link_delete(drv->sk, drv->link);
1193 	if (err < 0)
1194 		wpa_printf(MSG_ERROR, DRV_PREFIX "couldn't delete link");
1195 	rtnl_link_put(drv->link);
1196 	drv->link = NULL;
1197 
1198 	return err;
1199 }
1200 
1201 
1202 /**
1203  * macsec_drv_create_transmit_sa - Create secure association for transmit
1204  * @priv: private driver interface data from init()
1205  * @sa: secure association
1206  * Returns: 0 on success, -1 on failure
1207  */
1208 static int macsec_drv_create_transmit_sa(void *priv, struct transmit_sa *sa)
1209 {
1210 	struct macsec_drv_data *drv = priv;
1211 	struct macsec_genl_ctx *ctx = &drv->ctx;
1212 	struct nl_msg *msg;
1213 	struct nlattr *nest;
1214 	int ret = -1;
1215 
1216 	wpa_printf(MSG_DEBUG, DRV_PREFIX "%s: create_transmit_sa -> %d on "
1217 		   SCISTR " (enable_transmit=%d next_pn=%u)",
1218 		   drv->ifname, sa->an,
1219 		   SCI2STR(sa->sc->sci.addr, sa->sc->sci.port),
1220 		   sa->enable_transmit, sa->next_pn);
1221 	wpa_hexdump(MSG_DEBUG, DRV_PREFIX "SA keyid",
1222 		    &sa->pkey->key_identifier,
1223 		    sizeof(sa->pkey->key_identifier));
1224 	wpa_hexdump_key(MSG_DEBUG, DRV_PREFIX "SA key",
1225 			sa->pkey->key, sa->pkey->key_len);
1226 
1227 	msg = msg_prepare(MACSEC_CMD_ADD_TXSA, ctx, drv->ifi);
1228 	if (!msg)
1229 		return ret;
1230 
1231 	nest = nla_nest_start(msg, MACSEC_ATTR_SA_CONFIG);
1232 	if (!nest)
1233 		goto nla_put_failure;
1234 
1235 	NLA_PUT_U8(msg, MACSEC_SA_ATTR_AN, sa->an);
1236 	NLA_PUT_U32(msg, MACSEC_SA_ATTR_PN, sa->next_pn);
1237 	NLA_PUT(msg, MACSEC_SA_ATTR_KEYID, sizeof(sa->pkey->key_identifier),
1238 		&sa->pkey->key_identifier);
1239 	NLA_PUT(msg, MACSEC_SA_ATTR_KEY, sa->pkey->key_len, sa->pkey->key);
1240 	NLA_PUT_U8(msg, MACSEC_SA_ATTR_ACTIVE, sa->enable_transmit);
1241 
1242 	nla_nest_end(msg, nest);
1243 
1244 	ret = nl_send_recv(ctx->sk, msg);
1245 	if (ret < 0) {
1246 		wpa_printf(MSG_ERROR,
1247 			   DRV_PREFIX "%s: failed to communicate: %d (%s)",
1248 			   __func__, ret, nl_geterror(-ret));
1249 	}
1250 
1251 nla_put_failure:
1252 	nlmsg_free(msg);
1253 	return ret;
1254 }
1255 
1256 
1257 /**
1258  * macsec_drv_delete_transmit_sa - Delete secure association for transmit
1259  * @priv: private driver interface data from init()
1260  * @sa: secure association
1261  * Returns: 0 on success, -1 on failure
1262  */
1263 static int macsec_drv_delete_transmit_sa(void *priv, struct transmit_sa *sa)
1264 {
1265 	struct macsec_drv_data *drv = priv;
1266 	struct macsec_genl_ctx *ctx = &drv->ctx;
1267 	struct nl_msg *msg;
1268 	struct nlattr *nest;
1269 	int ret = -1;
1270 
1271 	wpa_printf(MSG_DEBUG, DRV_PREFIX "%s: delete_transmit_sa -> %d on "
1272 		   SCISTR, drv->ifname, sa->an,
1273 		   SCI2STR(sa->sc->sci.addr, sa->sc->sci.port));
1274 
1275 	msg = msg_prepare(MACSEC_CMD_DEL_TXSA, ctx, drv->ifi);
1276 	if (!msg)
1277 		return ret;
1278 
1279 	nest = nla_nest_start(msg, MACSEC_ATTR_SA_CONFIG);
1280 	if (!nest)
1281 		goto nla_put_failure;
1282 
1283 	NLA_PUT_U8(msg, MACSEC_SA_ATTR_AN, sa->an);
1284 
1285 	nla_nest_end(msg, nest);
1286 
1287 	ret = nl_send_recv(ctx->sk, msg);
1288 	if (ret < 0) {
1289 		wpa_printf(MSG_ERROR,
1290 			   DRV_PREFIX "%s: failed to communicate: %d (%s)",
1291 			   __func__, ret, nl_geterror(-ret));
1292 	}
1293 
1294 nla_put_failure:
1295 	nlmsg_free(msg);
1296 	return ret;
1297 }
1298 
1299 
1300 static int set_active_tx_sa(const struct macsec_genl_ctx *ctx, int ifindex,
1301 			    unsigned char an, Boolean state)
1302 {
1303 	struct nl_msg *msg;
1304 	struct nlattr *nest;
1305 	int ret = -1;
1306 
1307 	msg = msg_prepare(MACSEC_CMD_UPD_TXSA, ctx, ifindex);
1308 	if (!msg)
1309 		return ret;
1310 
1311 	nest = nla_nest_start(msg, MACSEC_ATTR_SA_CONFIG);
1312 	if (!nest)
1313 		goto nla_put_failure;
1314 
1315 	NLA_PUT_U8(msg, MACSEC_SA_ATTR_AN, an);
1316 	NLA_PUT_U8(msg, MACSEC_SA_ATTR_ACTIVE, !!state);
1317 
1318 	nla_nest_end(msg, nest);
1319 
1320 	ret = nl_send_recv(ctx->sk, msg);
1321 	if (ret < 0) {
1322 		wpa_printf(MSG_ERROR,
1323 			   DRV_PREFIX "%s: failed to communicate: %d (%s)",
1324 			   __func__, ret, nl_geterror(-ret));
1325 	}
1326 
1327 nla_put_failure:
1328 	nlmsg_free(msg);
1329 	return ret;
1330 }
1331 
1332 
1333 /**
1334  * macsec_drv_enable_transmit_sa - Enable SA for transmit
1335  * @priv: private driver interface data from init()
1336  * @sa: secure association
1337  * Returns: 0 on success, -1 on failure
1338  */
1339 static int macsec_drv_enable_transmit_sa(void *priv, struct transmit_sa *sa)
1340 {
1341 	struct macsec_drv_data *drv = priv;
1342 	struct macsec_genl_ctx *ctx = &drv->ctx;
1343 	int ret;
1344 
1345 	wpa_printf(MSG_DEBUG, DRV_PREFIX "%s: enable_transmit_sa -> %d on "
1346 		   SCISTR, drv->ifname, sa->an,
1347 		   SCI2STR(sa->sc->sci.addr, sa->sc->sci.port));
1348 
1349 	ret = set_active_tx_sa(ctx, drv->ifi, sa->an, TRUE);
1350 	if (ret < 0) {
1351 		wpa_printf(MSG_ERROR, DRV_PREFIX "failed to enable txsa");
1352 		return ret;
1353 	}
1354 
1355 	drv->encoding_sa_set = TRUE;
1356 	drv->encoding_sa = sa->an;
1357 
1358 	return try_commit(drv);
1359 }
1360 
1361 
1362 /**
1363  * macsec_drv_disable_transmit_sa - Disable SA for transmit
1364  * @priv: private driver interface data from init()
1365  * @sa: secure association
1366  * Returns: 0 on success, -1 on failure
1367  */
1368 static int macsec_drv_disable_transmit_sa(void *priv, struct transmit_sa *sa)
1369 {
1370 	struct macsec_drv_data *drv = priv;
1371 	struct macsec_genl_ctx *ctx = &drv->ctx;
1372 
1373 	wpa_printf(MSG_DEBUG, DRV_PREFIX "%s: disable_transmit_sa -> %d on "
1374 		   SCISTR, drv->ifname, sa->an,
1375 		   SCI2STR(sa->sc->sci.addr, sa->sc->sci.port));
1376 
1377 	return set_active_tx_sa(ctx, drv->ifi, sa->an, FALSE);
1378 }
1379 
1380 
1381 static int macsec_drv_status(void *priv, char *buf, size_t buflen)
1382 {
1383 	struct macsec_drv_data *drv = priv;
1384 	int res;
1385 	char *pos, *end;
1386 
1387 	pos = buf;
1388 	end = buf + buflen;
1389 
1390 	res = os_snprintf(pos, end - pos,
1391 			  "ifname=%s\n"
1392 			  "ifi=%d\n"
1393 			  "parent_ifname=%s\n"
1394 			  "parent_ifi=%d\n",
1395 			  drv->common.ifname, drv->ifi,
1396 			  drv->ifname, drv->parent_ifi);
1397 	if (os_snprintf_error(end - pos, res))
1398 		return pos - buf;
1399 	pos += res;
1400 
1401 	return pos - buf;
1402 }
1403 
1404 
1405 #ifdef __linux__
1406 
1407 static void macsec_drv_handle_data(void *ctx, unsigned char *buf, size_t len)
1408 {
1409 #ifdef HOSTAPD
1410 	struct ieee8023_hdr *hdr;
1411 	u8 *pos, *sa;
1412 	size_t left;
1413 	union wpa_event_data event;
1414 
1415 	/* must contain at least ieee8023_hdr 6 byte source, 6 byte dest,
1416 	 * 2 byte ethertype */
1417 	if (len < 14) {
1418 		wpa_printf(MSG_MSGDUMP, "%s: too short (%lu)",
1419 			   __func__, (unsigned long) len);
1420 		return;
1421 	}
1422 
1423 	hdr = (struct ieee8023_hdr *) buf;
1424 
1425 	switch (ntohs(hdr->ethertype)) {
1426 	case ETH_P_PAE:
1427 		wpa_printf(MSG_MSGDUMP, "Received EAPOL packet");
1428 		sa = hdr->src;
1429 		os_memset(&event, 0, sizeof(event));
1430 		event.new_sta.addr = sa;
1431 		wpa_supplicant_event(ctx, EVENT_NEW_STA, &event);
1432 
1433 		pos = (u8 *) (hdr + 1);
1434 		left = len - sizeof(*hdr);
1435 		drv_event_eapol_rx(ctx, sa, pos, left);
1436 		break;
1437 
1438 	default:
1439 		wpa_printf(MSG_DEBUG, "Unknown ethertype 0x%04x in data frame",
1440 			   ntohs(hdr->ethertype));
1441 		break;
1442 	}
1443 #endif /* HOSTAPD */
1444 }
1445 
1446 
1447 static void macsec_drv_handle_read(int sock, void *eloop_ctx, void *sock_ctx)
1448 {
1449 	int len;
1450 	unsigned char buf[3000];
1451 
1452 	len = recv(sock, buf, sizeof(buf), 0);
1453 	if (len < 0) {
1454 		wpa_printf(MSG_ERROR, "macsec_linux: recv: %s",
1455 			   strerror(errno));
1456 		return;
1457 	}
1458 
1459 	macsec_drv_handle_data(eloop_ctx, buf, len);
1460 }
1461 
1462 #endif /* __linux__ */
1463 
1464 
1465 static int macsec_drv_init_sockets(struct macsec_drv_data *drv, u8 *own_addr)
1466 {
1467 #ifdef __linux__
1468 	struct ifreq ifr;
1469 	struct sockaddr_ll addr;
1470 
1471 	drv->common.sock = socket(PF_PACKET, SOCK_RAW, htons(ETH_P_PAE));
1472 	if (drv->common.sock < 0) {
1473 		wpa_printf(MSG_ERROR, "socket[PF_PACKET,SOCK_RAW]: %s",
1474 			   strerror(errno));
1475 		return -1;
1476 	}
1477 
1478 	if (eloop_register_read_sock(drv->common.sock, macsec_drv_handle_read,
1479 				     drv->common.ctx, NULL)) {
1480 		wpa_printf(MSG_INFO, "Could not register read socket");
1481 		return -1;
1482 	}
1483 
1484 	os_memset(&ifr, 0, sizeof(ifr));
1485 	os_strlcpy(ifr.ifr_name, drv->common.ifname, sizeof(ifr.ifr_name));
1486 	if (ioctl(drv->common.sock, SIOCGIFINDEX, &ifr) != 0) {
1487 		wpa_printf(MSG_ERROR, "ioctl(SIOCGIFINDEX): %s",
1488 			   strerror(errno));
1489 		return -1;
1490 	}
1491 
1492 	os_memset(&addr, 0, sizeof(addr));
1493 	addr.sll_family = AF_PACKET;
1494 	addr.sll_ifindex = ifr.ifr_ifindex;
1495 	wpa_printf(MSG_DEBUG, "Opening raw packet socket for ifindex %d",
1496 		   addr.sll_ifindex);
1497 
1498 	if (bind(drv->common.sock, (struct sockaddr *) &addr, sizeof(addr)) < 0)
1499 	{
1500 		wpa_printf(MSG_ERROR, "bind: %s", strerror(errno));
1501 		return -1;
1502 	}
1503 
1504 	/* filter multicast address */
1505 	if (wired_multicast_membership(drv->common.sock, ifr.ifr_ifindex,
1506 				       pae_group_addr, 1) < 0) {
1507 		wpa_printf(MSG_ERROR, "wired: Failed to add multicast group "
1508 			   "membership");
1509 		return -1;
1510 	}
1511 
1512 	os_memset(&ifr, 0, sizeof(ifr));
1513 	os_strlcpy(ifr.ifr_name, drv->common.ifname, sizeof(ifr.ifr_name));
1514 	if (ioctl(drv->common.sock, SIOCGIFHWADDR, &ifr) != 0) {
1515 		wpa_printf(MSG_ERROR, "ioctl(SIOCGIFHWADDR): %s",
1516 			   strerror(errno));
1517 		return -1;
1518 	}
1519 
1520 	if (ifr.ifr_hwaddr.sa_family != ARPHRD_ETHER) {
1521 		wpa_printf(MSG_INFO, "Invalid HW-addr family 0x%04x",
1522 			   ifr.ifr_hwaddr.sa_family);
1523 		return -1;
1524 	}
1525 	os_memcpy(own_addr, ifr.ifr_hwaddr.sa_data, ETH_ALEN);
1526 
1527 	return 0;
1528 #else /* __linux__ */
1529 	return -1;
1530 #endif /* __linux__ */
1531 }
1532 
1533 
1534 static void * macsec_drv_hapd_init(struct hostapd_data *hapd,
1535 				   struct wpa_init_params *params)
1536 {
1537 	struct macsec_drv_data *drv;
1538 
1539 	drv = os_zalloc(sizeof(struct macsec_drv_data));
1540 	if (drv == NULL) {
1541 		wpa_printf(MSG_INFO,
1542 			   "Could not allocate memory for wired driver data");
1543 		return NULL;
1544 	}
1545 
1546 	drv->common.ctx = hapd;
1547 	os_strlcpy(drv->common.ifname, params->ifname,
1548 		   sizeof(drv->common.ifname));
1549 	drv->use_pae_group_addr = params->use_pae_group_addr;
1550 
1551 	if (macsec_drv_init_sockets(drv, params->own_addr)) {
1552 		os_free(drv);
1553 		return NULL;
1554 	}
1555 
1556 	return drv;
1557 }
1558 
1559 
1560 static void macsec_drv_hapd_deinit(void *priv)
1561 {
1562 	struct macsec_drv_data *drv = priv;
1563 
1564 	if (drv->common.sock >= 0) {
1565 		eloop_unregister_read_sock(drv->common.sock);
1566 		close(drv->common.sock);
1567 	}
1568 
1569 	os_free(drv);
1570 }
1571 
1572 
1573 static int macsec_drv_send_eapol(void *priv, const u8 *addr,
1574 				 const u8 *data, size_t data_len, int encrypt,
1575 				 const u8 *own_addr, u32 flags)
1576 {
1577 	struct macsec_drv_data *drv = priv;
1578 	struct ieee8023_hdr *hdr;
1579 	size_t len;
1580 	u8 *pos;
1581 	int res;
1582 
1583 	len = sizeof(*hdr) + data_len;
1584 	hdr = os_zalloc(len);
1585 	if (hdr == NULL) {
1586 		wpa_printf(MSG_INFO,
1587 			   "%s: malloc() failed (len=%lu)",
1588 			   __func__, (unsigned long) len);
1589 		return -1;
1590 	}
1591 
1592 	os_memcpy(hdr->dest, drv->use_pae_group_addr ? pae_group_addr : addr,
1593 		  ETH_ALEN);
1594 	os_memcpy(hdr->src, own_addr, ETH_ALEN);
1595 	hdr->ethertype = htons(ETH_P_PAE);
1596 
1597 	pos = (u8 *) (hdr + 1);
1598 	os_memcpy(pos, data, data_len);
1599 
1600 	res = send(drv->common.sock, (u8 *) hdr, len, 0);
1601 	os_free(hdr);
1602 
1603 	if (res < 0) {
1604 		wpa_printf(MSG_ERROR,
1605 			   "%s: packet len: %lu - failed: send: %s",
1606 			   __func__, (unsigned long) len, strerror(errno));
1607 	}
1608 
1609 	return res;
1610 }
1611 
1612 
1613 const struct wpa_driver_ops wpa_driver_macsec_linux_ops = {
1614 	.name = "macsec_linux",
1615 	.desc = "MACsec Ethernet driver for Linux",
1616 	.get_ssid = driver_wired_get_ssid,
1617 	.get_bssid = driver_wired_get_bssid,
1618 	.get_capa = driver_wired_get_capa,
1619 	.init = macsec_drv_wpa_init,
1620 	.deinit = macsec_drv_wpa_deinit,
1621 	.hapd_init = macsec_drv_hapd_init,
1622 	.hapd_deinit = macsec_drv_hapd_deinit,
1623 	.hapd_send_eapol = macsec_drv_send_eapol,
1624 
1625 	.macsec_init = macsec_drv_macsec_init,
1626 	.macsec_deinit = macsec_drv_macsec_deinit,
1627 	.macsec_get_capability = macsec_drv_get_capability,
1628 	.enable_protect_frames = macsec_drv_enable_protect_frames,
1629 	.enable_encrypt = macsec_drv_enable_encrypt,
1630 	.set_replay_protect = macsec_drv_set_replay_protect,
1631 	.set_current_cipher_suite = macsec_drv_set_current_cipher_suite,
1632 	.enable_controlled_port = macsec_drv_enable_controlled_port,
1633 	.get_receive_lowest_pn = macsec_drv_get_receive_lowest_pn,
1634 	.set_receive_lowest_pn = macsec_drv_set_receive_lowest_pn,
1635 	.get_transmit_next_pn = macsec_drv_get_transmit_next_pn,
1636 	.set_transmit_next_pn = macsec_drv_set_transmit_next_pn,
1637 	.create_receive_sc = macsec_drv_create_receive_sc,
1638 	.delete_receive_sc = macsec_drv_delete_receive_sc,
1639 	.create_receive_sa = macsec_drv_create_receive_sa,
1640 	.delete_receive_sa = macsec_drv_delete_receive_sa,
1641 	.enable_receive_sa = macsec_drv_enable_receive_sa,
1642 	.disable_receive_sa = macsec_drv_disable_receive_sa,
1643 	.create_transmit_sc = macsec_drv_create_transmit_sc,
1644 	.delete_transmit_sc = macsec_drv_delete_transmit_sc,
1645 	.create_transmit_sa = macsec_drv_create_transmit_sa,
1646 	.delete_transmit_sa = macsec_drv_delete_transmit_sa,
1647 	.enable_transmit_sa = macsec_drv_enable_transmit_sa,
1648 	.disable_transmit_sa = macsec_drv_disable_transmit_sa,
1649 
1650 	.status = macsec_drv_status,
1651 };
1652