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