commit f8974fb33ababeb84572e86b23262025923cfb47 Author: zhaoxc0502 Date: Thu Jun 16 17:14:33 2022 +0800 linux_net Change-Id: I11a28ee31b48319f8b8dfe854585ddb8e6fc9f1b diff --git a/net/Kconfig b/net/Kconfig index d6567162c..4c8d5ebe1 100644 --- a/net/Kconfig +++ b/net/Kconfig @@ -232,6 +232,7 @@ source "net/ieee802154/Kconfig" source "net/mac802154/Kconfig" source "net/sched/Kconfig" source "net/dcb/Kconfig" +source "net/tsn/Kconfig" source "net/dns_resolver/Kconfig" source "net/batman-adv/Kconfig" source "net/openvswitch/Kconfig" diff --git a/net/Makefile b/net/Makefile index 5744bf199..64866482e 100644 --- a/net/Makefile +++ b/net/Makefile @@ -59,6 +59,9 @@ obj-$(CONFIG_CAIF) += caif/ ifneq ($(CONFIG_DCB),) obj-y += dcb/ endif +ifneq ($(CONFIG_TSN),) +obj-y += tsn/ +endif obj-$(CONFIG_6LOWPAN) += 6lowpan/ obj-$(CONFIG_IEEE802154) += ieee802154/ obj-$(CONFIG_MAC802154) += mac802154/ diff --git a/net/bluetooth/smp.c b/net/bluetooth/smp.c index 2b7879afc..8454bdeee 100644 --- a/net/bluetooth/smp.c +++ b/net/bluetooth/smp.c @@ -3362,31 +3362,8 @@ static void smp_del_chan(struct l2cap_chan *chan) l2cap_chan_put(chan); } -static ssize_t force_bredr_smp_read(struct file *file, - char __user *user_buf, - size_t count, loff_t *ppos) +int smp_force_bredr(struct hci_dev *hdev, bool enable) { - struct hci_dev *hdev = file->private_data; - char buf[3]; - - buf[0] = hci_dev_test_flag(hdev, HCI_FORCE_BREDR_SMP) ? 'Y': 'N'; - buf[1] = '\n'; - buf[2] = '\0'; - return simple_read_from_buffer(user_buf, count, ppos, buf, 2); -} - -static ssize_t force_bredr_smp_write(struct file *file, - const char __user *user_buf, - size_t count, loff_t *ppos) -{ - struct hci_dev *hdev = file->private_data; - bool enable; - int err; - - err = kstrtobool_from_user(user_buf, count, &enable); - if (err) - return err; - if (enable == hci_dev_test_flag(hdev, HCI_FORCE_BREDR_SMP)) return -EALREADY; @@ -3408,16 +3385,9 @@ static ssize_t force_bredr_smp_write(struct file *file, hci_dev_change_flag(hdev, HCI_FORCE_BREDR_SMP); - return count; + return 0; } -static const struct file_operations force_bredr_smp_fops = { - .open = simple_open, - .read = force_bredr_smp_read, - .write = force_bredr_smp_write, - .llseek = default_llseek, -}; - int smp_register(struct hci_dev *hdev) { struct l2cap_chan *chan; @@ -3442,17 +3412,7 @@ int smp_register(struct hci_dev *hdev) hdev->smp_data = chan; - /* If the controller does not support BR/EDR Secure Connections - * feature, then the BR/EDR SMP channel shall not be present. - * - * To test this with Bluetooth 4.0 controllers, create a debugfs - * switch that allows forcing BR/EDR SMP support and accepting - * cross-transport pairing on non-AES encrypted connections. - */ if (!lmp_sc_capable(hdev)) { - debugfs_create_file("force_bredr_smp", 0644, hdev->debugfs, - hdev, &force_bredr_smp_fops); - /* Flag can be already set here (due to power toggle) */ if (!hci_dev_test_flag(hdev, HCI_FORCE_BREDR_SMP)) return 0; diff --git a/net/bluetooth/smp.h b/net/bluetooth/smp.h index 121edadd5..fc35a8bf3 100644 --- a/net/bluetooth/smp.h +++ b/net/bluetooth/smp.h @@ -193,6 +193,8 @@ bool smp_irk_matches(struct hci_dev *hdev, const u8 irk[16], int smp_generate_rpa(struct hci_dev *hdev, const u8 irk[16], bdaddr_t *rpa); int smp_generate_oob(struct hci_dev *hdev, u8 hash[16], u8 rand[16]); +int smp_force_bredr(struct hci_dev *hdev, bool enable); + int smp_register(struct hci_dev *hdev); void smp_unregister(struct hci_dev *hdev); diff --git a/net/core/dev.c b/net/core/dev.c index b0c3dd268..552dd50d1 100644 --- a/net/core/dev.c +++ b/net/core/dev.c @@ -3769,10 +3769,33 @@ static inline int __dev_xmit_skb(struct sk_buff *skb, struct Qdisc *q, qdisc_calculate_pkt_len(skb, q); if (q->flags & TCQ_F_NOLOCK) { + if (q->flags & TCQ_F_CAN_BYPASS && nolock_qdisc_is_empty(q) && + qdisc_run_begin(q)) { + /* Retest nolock_qdisc_is_empty() within the protection + * of q->seqlock to protect from racing with requeuing. + */ + if (unlikely(!nolock_qdisc_is_empty(q))) { + rc = q->enqueue(skb, q, &to_free) & + NET_XMIT_MASK; + __qdisc_run(q); + qdisc_run_end(q); + + goto no_lock_out; + } + + qdisc_bstats_cpu_update(q, skb); + if (sch_direct_xmit(skb, q, dev, txq, NULL, true) && + !nolock_qdisc_is_empty(q)) + __qdisc_run(q); + + qdisc_run_end(q); + return NET_XMIT_SUCCESS; + } + rc = q->enqueue(skb, q, &to_free) & NET_XMIT_MASK; - if (likely(!netif_xmit_frozen_or_stopped(txq))) - qdisc_run(q); + qdisc_run(q); +no_lock_out: if (unlikely(to_free)) kfree_skb_list(to_free); return rc; @@ -10633,6 +10656,17 @@ struct net_device *alloc_netdev_mqs(int sizeof_priv, const char *name, nf_hook_ingress_init(dev); + static char num=0; + if (0 == strcmp(name, "eth%d")) { + if (1 == num) { + printk("ZXC========Save net of wlan dev-Name:%s", name); + extern void set_krn_netdev(struct net_device *dev); + set_krn_netdev(dev); + } else { + num++; + } + } + return dev; free_all: diff --git a/net/core/skbuff.c b/net/core/skbuff.c index 825e6b988..54600f062 100644 --- a/net/core/skbuff.c +++ b/net/core/skbuff.c @@ -928,6 +928,32 @@ void napi_consume_skb(struct sk_buff *skb, int budget) } EXPORT_SYMBOL(napi_consume_skb); +/** + * skb_recycle - clean up an skb for reuse + * @skb: buffer + * + * Recycles the skb to be reused as a receive buffer. This + * function does any necessary reference count dropping, and + * cleans up the skbuff as if it just came from __alloc_skb(). + */ +void skb_recycle(struct sk_buff *skb) +{ + struct skb_shared_info *shinfo; + u8 head_frag = skb->head_frag; + + skb_release_head_state(skb); + + shinfo = skb_shinfo(skb); + memset(shinfo, 0, offsetof(struct skb_shared_info, dataref)); + atomic_set(&shinfo->dataref, 1); + + memset(skb, 0, offsetof(struct sk_buff, tail)); + skb->data = skb->head + NET_SKB_PAD; + skb->head_frag = head_frag; + skb_reset_tail_pointer(skb); +} +EXPORT_SYMBOL(skb_recycle); + /* Make sure a field is enclosed inside headers_start/headers_end section */ #define CHECK_SKB_FIELD(field) \ BUILD_BUG_ON(offsetof(struct sk_buff, field) < \ diff --git a/net/ethernet/eth.c b/net/ethernet/eth.c index dac65180c..f8484fa7d 100644 --- a/net/ethernet/eth.c +++ b/net/ethernet/eth.c @@ -534,8 +534,10 @@ EXPORT_SYMBOL(eth_platform_get_mac_address); int nvmem_get_mac_address(struct device *dev, void *addrbuf) { struct nvmem_cell *cell; - const void *mac; + const unsigned char *mac; + unsigned char macaddr[ETH_ALEN]; size_t len; + int i = 0; cell = nvmem_cell_get(dev, "mac-address"); if (IS_ERR(cell)) @@ -547,14 +549,27 @@ int nvmem_get_mac_address(struct device *dev, void *addrbuf) if (IS_ERR(mac)) return PTR_ERR(mac); - if (len != ETH_ALEN || !is_valid_ether_addr(mac)) { - kfree(mac); - return -EINVAL; + if (len != ETH_ALEN) + goto invalid_addr; + + if (dev->of_node && + of_property_read_bool(dev->of_node, "nvmem_macaddr_swap")) { + for (i = 0; i < ETH_ALEN; i++) + macaddr[i] = mac[ETH_ALEN - i - 1]; + } else { + ether_addr_copy(macaddr, mac); } - ether_addr_copy(addrbuf, mac); + if (!is_valid_ether_addr(macaddr)) + goto invalid_addr; + + ether_addr_copy(addrbuf, macaddr); kfree(mac); return 0; + +invalid_addr: + kfree(mac); + return -EINVAL; } EXPORT_SYMBOL(nvmem_get_mac_address); diff --git a/net/llc/af_llc.c b/net/llc/af_llc.c index 99a37c411..ac5cadd02 100644 --- a/net/llc/af_llc.c +++ b/net/llc/af_llc.c @@ -276,7 +276,6 @@ static int llc_ui_autobind(struct socket *sock, struct sockaddr_llc *addr) { struct sock *sk = sock->sk; struct llc_sock *llc = llc_sk(sk); - struct net_device *dev = NULL; struct llc_sap *sap; int rc = -EINVAL; @@ -288,14 +287,14 @@ static int llc_ui_autobind(struct socket *sock, struct sockaddr_llc *addr) goto out; rc = -ENODEV; if (sk->sk_bound_dev_if) { - dev = dev_get_by_index(&init_net, sk->sk_bound_dev_if); - if (dev && addr->sllc_arphrd != dev->type) { - dev_put(dev); - dev = NULL; + llc->dev = dev_get_by_index(&init_net, sk->sk_bound_dev_if); + if (llc->dev && addr->sllc_arphrd != llc->dev->type) { + dev_put(llc->dev); + llc->dev = NULL; } } else - dev = dev_getfirstbyhwtype(&init_net, addr->sllc_arphrd); - if (!dev) + llc->dev = dev_getfirstbyhwtype(&init_net, addr->sllc_arphrd); + if (!llc->dev) goto out; rc = -EUSERS; llc->laddr.lsap = llc_ui_autoport(); @@ -305,11 +304,6 @@ static int llc_ui_autobind(struct socket *sock, struct sockaddr_llc *addr) sap = llc_sap_open(llc->laddr.lsap, NULL); if (!sap) goto out; - - /* Note: We do not expect errors from this point. */ - llc->dev = dev; - dev = NULL; - memcpy(llc->laddr.mac, llc->dev->dev_addr, IFHWADDRLEN); memcpy(&llc->addr, addr, sizeof(llc->addr)); /* assign new connection to its SAP */ @@ -317,7 +311,6 @@ static int llc_ui_autobind(struct socket *sock, struct sockaddr_llc *addr) sock_reset_flag(sk, SOCK_ZAPPED); rc = 0; out: - dev_put(dev); return rc; } @@ -340,7 +333,6 @@ static int llc_ui_bind(struct socket *sock, struct sockaddr *uaddr, int addrlen) struct sockaddr_llc *addr = (struct sockaddr_llc *)uaddr; struct sock *sk = sock->sk; struct llc_sock *llc = llc_sk(sk); - struct net_device *dev = NULL; struct llc_sap *sap; int rc = -EINVAL; @@ -356,26 +348,25 @@ static int llc_ui_bind(struct socket *sock, struct sockaddr *uaddr, int addrlen) rc = -ENODEV; rcu_read_lock(); if (sk->sk_bound_dev_if) { - dev = dev_get_by_index_rcu(&init_net, sk->sk_bound_dev_if); - if (dev) { + llc->dev = dev_get_by_index_rcu(&init_net, sk->sk_bound_dev_if); + if (llc->dev) { if (is_zero_ether_addr(addr->sllc_mac)) - memcpy(addr->sllc_mac, dev->dev_addr, + memcpy(addr->sllc_mac, llc->dev->dev_addr, IFHWADDRLEN); - if (addr->sllc_arphrd != dev->type || + if (addr->sllc_arphrd != llc->dev->type || !ether_addr_equal(addr->sllc_mac, - dev->dev_addr)) { + llc->dev->dev_addr)) { rc = -EINVAL; - dev = NULL; + llc->dev = NULL; } } - } else { - dev = dev_getbyhwaddr_rcu(&init_net, addr->sllc_arphrd, + } else + llc->dev = dev_getbyhwaddr_rcu(&init_net, addr->sllc_arphrd, addr->sllc_mac); - } - if (dev) - dev_hold(dev); + if (llc->dev) + dev_hold(llc->dev); rcu_read_unlock(); - if (!dev) + if (!llc->dev) goto out; if (!addr->sllc_sap) { rc = -EUSERS; @@ -408,11 +399,6 @@ static int llc_ui_bind(struct socket *sock, struct sockaddr *uaddr, int addrlen) goto out_put; } } - - /* Note: We do not expect errors from this point. */ - llc->dev = dev; - dev = NULL; - llc->laddr.lsap = addr->sllc_sap; memcpy(llc->laddr.mac, addr->sllc_mac, IFHWADDRLEN); memcpy(&llc->addr, addr, sizeof(llc->addr)); @@ -423,7 +409,6 @@ static int llc_ui_bind(struct socket *sock, struct sockaddr *uaddr, int addrlen) out_put: llc_sap_put(sap); out: - dev_put(dev); release_sock(sk); return rc; } diff --git a/net/sched/sch_generic.c b/net/sched/sch_generic.c index 05aa2571a..ad86e551e 100644 --- a/net/sched/sch_generic.c +++ b/net/sched/sch_generic.c @@ -52,6 +52,8 @@ static void qdisc_maybe_clear_missed(struct Qdisc *q, */ if (!netif_xmit_frozen_or_stopped(txq)) set_bit(__QDISC_STATE_MISSED, &q->state); + else + set_bit(__QDISC_STATE_DRAINING, &q->state); } /* Main transmission queue. */ @@ -164,9 +166,13 @@ static inline void dev_requeue_skb(struct sk_buff *skb, struct Qdisc *q) skb = next; } - if (lock) + + if (lock) { spin_unlock(lock); - __netif_schedule(q); + set_bit(__QDISC_STATE_MISSED, &q->state); + } else { + __netif_schedule(q); + } } static void try_bulk_dequeue_skb(struct Qdisc *q, @@ -409,7 +415,11 @@ void __qdisc_run(struct Qdisc *q) while (qdisc_restart(q, &packets)) { quota -= packets; if (quota <= 0) { - __netif_schedule(q); + if (q->flags & TCQ_F_NOLOCK) + set_bit(__QDISC_STATE_MISSED, &q->state); + else + __netif_schedule(q); + break; } } @@ -460,6 +470,14 @@ static void dev_watchdog(struct timer_list *t) txq->trans_timeout++; break; } + + /* Devices with HW_ACCEL_MQ have multiple txqs + * but update only the first one's transmission + * timestamp so avoid checking the rest. + */ + if (dev->features & NETIF_F_HW_ACCEL_MQ) + break; + } if (some_queue_timedout) { @@ -680,13 +698,14 @@ static struct sk_buff *pfifo_fast_dequeue(struct Qdisc *qdisc) if (likely(skb)) { qdisc_update_stats_at_dequeue(qdisc, skb); } else if (need_retry && - test_bit(__QDISC_STATE_MISSED, &qdisc->state)) { + READ_ONCE(qdisc->state) & QDISC_STATE_NON_EMPTY) { /* Delay clearing the STATE_MISSED here to reduce * the overhead of the second spin_trylock() in * qdisc_run_begin() and __netif_schedule() calling * in qdisc_run_end(). */ clear_bit(__QDISC_STATE_MISSED, &qdisc->state); + clear_bit(__QDISC_STATE_DRAINING, &qdisc->state); /* Make sure dequeuing happens after clearing * STATE_MISSED. @@ -696,8 +715,6 @@ static struct sk_buff *pfifo_fast_dequeue(struct Qdisc *qdisc) need_retry = false; goto retry; - } else { - WRITE_ONCE(qdisc->empty, true); } return skb; @@ -898,7 +915,6 @@ struct Qdisc *qdisc_alloc(struct netdev_queue *dev_queue, sch->enqueue = ops->enqueue; sch->dequeue = ops->dequeue; sch->dev_queue = dev_queue; - sch->empty = true; dev_hold(dev); refcount_set(&sch->refcnt, 1); @@ -1204,6 +1220,7 @@ static void dev_reset_queue(struct net_device *dev, spin_unlock_bh(qdisc_lock(qdisc)); if (nolock) { clear_bit(__QDISC_STATE_MISSED, &qdisc->state); + clear_bit(__QDISC_STATE_DRAINING, &qdisc->state); spin_unlock_bh(&qdisc->seqlock); } } diff --git a/net/sunrpc/xprt.c b/net/sunrpc/xprt.c index e19f44918..8201531ce 100644 --- a/net/sunrpc/xprt.c +++ b/net/sunrpc/xprt.c @@ -881,7 +881,12 @@ void xprt_connect(struct rpc_task *task) if (!xprt_lock_write(xprt, task)) return; - if (!xprt_connected(xprt) && !test_bit(XPRT_CLOSE_WAIT, &xprt->state)) { + if (test_and_clear_bit(XPRT_CLOSE_WAIT, &xprt->state)) { + trace_xprt_disconnect_cleanup(xprt); + xprt->ops->close(xprt); + } + + if (!xprt_connected(xprt)) { task->tk_rqstp->rq_connect_cookie = xprt->connect_cookie; rpc_sleep_on_timeout(&xprt->pending, task, NULL, xprt_request_timeout(task->tk_rqstp)); diff --git a/net/sunrpc/xprtsock.c b/net/sunrpc/xprtsock.c index b7c262f6d..16c7758e7 100644 --- a/net/sunrpc/xprtsock.c +++ b/net/sunrpc/xprtsock.c @@ -847,7 +847,7 @@ static int xs_local_send_request(struct rpc_rqst *req) /* Close the stream if the previous transmission was incomplete */ if (xs_send_request_was_aborted(transport, req)) { - xprt_force_disconnect(xprt); + xs_close(xprt); return -ENOTCONN; } @@ -885,7 +885,7 @@ static int xs_local_send_request(struct rpc_rqst *req) -status); fallthrough; case -EPIPE: - xprt_force_disconnect(xprt); + xs_close(xprt); status = -ENOTCONN; } @@ -1167,16 +1167,6 @@ static void xs_reset_transport(struct sock_xprt *transport) if (sk == NULL) return; - /* - * Make sure we're calling this in a context from which it is safe - * to call __fput_sync(). In practice that means rpciod and the - * system workqueue. - */ - if (!(current->flags & PF_WQ_WORKER)) { - WARN_ON_ONCE(1); - set_bit(XPRT_CLOSE_WAIT, &xprt->state); - return; - } if (atomic_read(&transport->xprt.swapper)) sk_clear_memalloc(sk); @@ -1200,7 +1190,7 @@ static void xs_reset_transport(struct sock_xprt *transport) mutex_unlock(&transport->recv_mutex); trace_rpc_socket_close(xprt, sock); - __fput_sync(filp); + fput(filp); xprt_disconnect_done(xprt); } diff --git a/net/tsn/Kconfig b/net/tsn/Kconfig new file mode 100644 index 000000000..9f22807a7 --- /dev/null +++ b/net/tsn/Kconfig @@ -0,0 +1,15 @@ +config TSN + bool "802.1 Time-Sensitive Networking support" + default n + depends on VLAN_8021Q && PTP_1588_CLOCK + help + This enables support for TSN(time sensitive networking) + TSN features include: + 802.1Qav: + 802.1Qbv: + 802.1Qci: + 802.1Qbu: + 802.1AS: + 802.1CB: + + If unsure, say N. diff --git a/net/tsn/Makefile b/net/tsn/Makefile new file mode 100644 index 000000000..ed46381bf --- /dev/null +++ b/net/tsn/Makefile @@ -0,0 +1 @@ +obj-$(CONFIG_TSN) += genl_tsn.o diff --git a/net/tsn/genl_tsn.c b/net/tsn/genl_tsn.c new file mode 100644 index 000000000..d0dc368de --- /dev/null +++ b/net/tsn/genl_tsn.c @@ -0,0 +1,3730 @@ +// SPDX-License-Identifier: (GPL-2.0+ OR BSD-3-Clause) +/* Copyright 2017-2019 NXP */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#define NLA_PARSE_NESTED(a, b, c, d) \ + nla_parse_nested_deprecated(a, b, c, d, NULL) +#define NLA_PUT_U64(a, b, c) nla_put_u64_64bit(a, b, c, NLA_U64) + +static struct genl_family tsn_family; + +LIST_HEAD(port_list); + +static const struct nla_policy tsn_cmd_policy[TSN_CMD_ATTR_MAX + 1] = { + [TSN_CMD_ATTR_MESG] = { .type = NLA_STRING }, + [TSN_CMD_ATTR_DATA] = { .type = NLA_S32 }, + [TSN_ATTR_IFNAME] = { .type = NLA_STRING }, + [TSN_ATTR_PORT_NUMBER] = { .type = NLA_U8 }, + [TSN_ATTR_CAP] = { .type = NLA_NESTED }, + [TSN_ATTR_QBV] = { .type = NLA_NESTED }, + [TSN_ATTR_STREAM_IDENTIFY] = { .type = NLA_NESTED }, + [TSN_ATTR_QCI_SP] = { .type = NLA_NESTED }, + [TSN_ATTR_QCI_SFI] = { .type = NLA_NESTED }, + [TSN_ATTR_QCI_SGI] = { .type = NLA_NESTED }, + [TSN_ATTR_QCI_FMI] = { .type = NLA_NESTED }, + [TSN_ATTR_CBS] = { .type = NLA_NESTED }, + [TSN_ATTR_TSD] = { .type = NLA_NESTED }, + [TSN_ATTR_QBU] = { .type = NLA_NESTED }, + [TSN_ATTR_CT] = { .type = NLA_NESTED }, + [TSN_ATTR_CBGEN] = { .type = NLA_NESTED }, + [TSN_ATTR_CBREC] = { .type = NLA_NESTED }, + [TSN_ATTR_CBSTAT] = { .type = NLA_NESTED }, + [TSN_ATTR_DSCP] = { .type = NLA_NESTED }, +}; + +static const struct nla_policy tsn_cap_policy[TSN_CAP_ATTR_MAX + 1] = { + [TSN_CAP_ATTR_QBV] = { .type = NLA_FLAG }, + [TSN_CAP_ATTR_QCI] = { .type = NLA_FLAG }, + [TSN_CAP_ATTR_QBU] = { .type = NLA_FLAG }, + [TSN_CAP_ATTR_CBS] = { .type = NLA_FLAG }, + [TSN_CAP_ATTR_CB] = { .type = NLA_FLAG }, + [TSN_CAP_ATTR_TBS] = { .type = NLA_FLAG }, + [TSN_CAP_ATTR_CTH] = { .type = NLA_FLAG }, +}; + +static const struct nla_policy qci_cap_policy[TSN_QCI_STREAM_ATTR_MAX + 1] = { + [TSN_QCI_STREAM_ATTR_MAX_SFI] = { .type = NLA_U32 }, + [TSN_QCI_STREAM_ATTR_MAX_SGI] = { .type = NLA_U32 }, + [TSN_QCI_STREAM_ATTR_MAX_FMI] = { .type = NLA_U32 }, + [TSN_QCI_STREAM_ATTR_SLM] = { .type = NLA_U32 }, +}; + +static const struct nla_policy ct_policy[TSN_CT_ATTR_MAX + 1] = { + [TSN_CT_ATTR_QUEUE_STATE] = { .type = NLA_U8 } +}; + +static const struct nla_policy cbgen_policy[TSN_CBGEN_ATTR_MAX + 1] = { + [TSN_CBGEN_ATTR_INDEX] = { .type = NLA_U32 }, + [TSN_CBGEN_ATTR_PORT_MASK] = { .type = NLA_U8 }, + [TSN_CBGEN_ATTR_SPLIT_MASK] = { .type = NLA_U8 }, + [TSN_CBGEN_ATTR_SEQ_LEN] = { .type = NLA_U8 }, + [TSN_CBGEN_ATTR_SEQ_NUM] = { .type = NLA_U32 }, +}; + +static const struct nla_policy cbrec_policy[TSN_CBREC_ATTR_MAX + 1] = { + [TSN_CBREC_ATTR_INDEX] = { .type = NLA_U32 }, + [TSN_CBREC_ATTR_SEQ_LEN] = { .type = NLA_U8 }, + [TSN_CBREC_ATTR_HIS_LEN] = { .type = NLA_U8 }, + [TSN_CBREC_ATTR_TAG_POP_EN] = { .type = NLA_FLAG }, +}; + +static const struct nla_policy cbstat_policy[TSN_CBSTAT_ATTR_MAX + 1] = { + [TSN_CBSTAT_ATTR_INDEX] = { .type = NLA_U32 }, + [TSN_CBSTAT_ATTR_GEN_REC] = { .type = NLA_U8 }, + [TSN_CBSTAT_ATTR_ERR] = { .type = NLA_U8 }, + [TSN_CBSTAT_ATTR_SEQ_NUM] = { .type = NLA_U32 }, + [TSN_CBSTAT_ATTR_SEQ_LEN] = { .type = NLA_U8 }, + [TSN_CBSTAT_ATTR_SPLIT_MASK] = { .type = NLA_U8 }, + [TSN_CBSTAT_ATTR_PORT_MASK] = { .type = NLA_U8 }, + [TSN_CBSTAT_ATTR_HIS_LEN] = { .type = NLA_U8 }, + [TSN_CBSTAT_ATTR_SEQ_HIS] = { .type = NLA_U32 }, +}; + +static const struct nla_policy qbu_policy[TSN_QBU_ATTR_MAX + 1] = { + [TSN_QBU_ATTR_ADMIN_STATE] = { .type = NLA_U8 }, + [TSN_QBU_ATTR_HOLD_ADVANCE] = { .type = NLA_U32}, + [TSN_QBU_ATTR_RELEASE_ADVANCE] = { .type = NLA_U32}, + [TSN_QBU_ATTR_ACTIVE] = { .type = NLA_FLAG}, + [TSN_QBU_ATTR_HOLD_REQUEST] = { .type = NLA_U8}, +}; + +static const struct nla_policy cbs_policy[TSN_CBS_ATTR_MAX + 1] = { + [TSN_CBS_ATTR_TC_INDEX] = { .type = NLA_U8}, + [TSN_CBS_ATTR_BW] = { .type = NLA_U8}, +}; + +static const struct nla_policy tsd_policy[TSN_TSD_ATTR_MAX + 1] = { + [TSN_TSD_ATTR_ENABLE] = { .type = NLA_FLAG}, + [TSN_TSD_ATTR_DISABLE] = { .type = NLA_FLAG}, + [TSN_TSD_ATTR_PERIOD] = { .type = NLA_U32}, + [TSN_TSD_ATTR_MAX_FRM_NUM] = { .type = NLA_U32}, + [TSN_TSD_ATTR_CYCLE_NUM] = { .type = NLA_U32}, + [TSN_TSD_ATTR_LOSS_STEPS] = { .type = NLA_U32}, + [TSN_TSD_ATTR_SYN_IMME] = { .type = NLA_FLAG}, +}; + +static const struct nla_policy qbv_policy[TSN_QBV_ATTR_MAX + 1] = { + [TSN_QBV_ATTR_ADMINENTRY] = { .type = NLA_NESTED}, + [TSN_QBV_ATTR_OPERENTRY] = { .type = NLA_NESTED}, + [TSN_QBV_ATTR_ENABLE] = { .type = NLA_FLAG}, + [TSN_QBV_ATTR_DISABLE] = { .type = NLA_FLAG}, + [TSN_QBV_ATTR_CONFIGCHANGE] = { .type = NLA_FLAG}, + [TSN_QBV_ATTR_CONFIGCHANGETIME] = { .type = NLA_U64}, + [TSN_QBV_ATTR_MAXSDU] = { .type = NLA_U32}, + [TSN_QBV_ATTR_GRANULARITY] = { .type = NLA_U32}, + [TSN_QBV_ATTR_CURRENTTIME] = { .type = NLA_U64}, + [TSN_QBV_ATTR_CONFIGPENDING] = {.type = NLA_FLAG}, + [TSN_QBV_ATTR_CONFIGCHANGEERROR] = { .type = NLA_U64}, + [TSN_QBV_ATTR_LISTMAX] = { .type = NLA_U32}, +}; + +static const struct nla_policy qbv_ctrl_policy[TSN_QBV_ATTR_CTRL_MAX + 1] = { + [TSN_QBV_ATTR_CTRL_LISTCOUNT] = { .type = NLA_U32}, + [TSN_QBV_ATTR_CTRL_GATESTATE] = { .type = NLA_U8}, + [TSN_QBV_ATTR_CTRL_CYCLETIME] = { .type = NLA_U32}, + [TSN_QBV_ATTR_CTRL_CYCLETIMEEXT] = { .type = NLA_U32}, + [TSN_QBV_ATTR_CTRL_BASETIME] = { .type = NLA_U64}, + [TSN_QBV_ATTR_CTRL_LISTENTRY] = { .type = NLA_NESTED}, +}; + +static const struct nla_policy qbv_entry_policy[TSN_QBV_ATTR_ENTRY_MAX + 1] = { + [TSN_QBV_ATTR_ENTRY_ID] = { .type = NLA_U32}, + [TSN_QBV_ATTR_ENTRY_GC] = { .type = NLA_U8}, + [TSN_QBV_ATTR_ENTRY_TM] = { .type = NLA_U32}, +}; + +static const struct nla_policy cb_streamid_policy[TSN_STREAMID_ATTR_MAX + 1] = { + [TSN_STREAMID_ATTR_INDEX] = { .type = NLA_U32}, + [TSN_STREAMID_ATTR_ENABLE] = { .type = NLA_FLAG}, + [TSN_STREAMID_ATTR_DISABLE] = { .type = NLA_FLAG}, + [TSN_STREAMID_ATTR_STREAM_HANDLE] = { .type = NLA_S32}, + [TSN_STREAMID_ATTR_IFOP] = { .type = NLA_U32}, + [TSN_STREAMID_ATTR_OFOP] = { .type = NLA_U32}, + [TSN_STREAMID_ATTR_IFIP] = { .type = NLA_U32}, + [TSN_STREAMID_ATTR_OFIP] = { .type = NLA_U32}, + [TSN_STREAMID_ATTR_TYPE] = { .type = NLA_U8}, + [TSN_STREAMID_ATTR_NDMAC] = { .type = NLA_U64}, + [TSN_STREAMID_ATTR_NTAGGED] = { .type = NLA_U8}, + [TSN_STREAMID_ATTR_NVID] = { .type = NLA_U16}, + [TSN_STREAMID_ATTR_SMAC] = { .type = NLA_U64}, + [TSN_STREAMID_ATTR_STAGGED] = { .type = NLA_U8}, + [TSN_STREAMID_ATTR_SVID] = { .type = NLA_U16}, + [TSN_STREAMID_ATTR_COUNTERS_PSI] = { .type = NLA_U64}, + [TSN_STREAMID_ATTR_COUNTERS_PSO] = { .type = NLA_U64}, + [TSN_STREAMID_ATTR_COUNTERS_PSPPI] = { .type = NLA_U64}, + [TSN_STREAMID_ATTR_COUNTERS_PSPPO] = { .type = NLA_U64}, +}; + +static const struct nla_policy qci_sfi_policy[TSN_QCI_SFI_ATTR_MAX + 1] = { + [TSN_QCI_SFI_ATTR_INDEX] = { .type = NLA_U32}, + [TSN_QCI_SFI_ATTR_ENABLE] = { .type = NLA_FLAG}, + [TSN_QCI_SFI_ATTR_DISABLE] = { .type = NLA_FLAG}, + [TSN_QCI_SFI_ATTR_STREAM_HANDLE] = { .type = NLA_S32}, + [TSN_QCI_SFI_ATTR_PRIO_SPEC] = { .type = NLA_S8}, + [TSN_QCI_SFI_ATTR_GATE_ID] = { .type = NLA_U32}, + [TSN_QCI_SFI_ATTR_FILTER_TYPE] = { .type = NLA_U8}, + [TSN_QCI_SFI_ATTR_FLOW_ID] = { .type = NLA_S32}, + [TSN_QCI_SFI_ATTR_MAXSDU] = { .type = NLA_U16}, + [TSN_QCI_SFI_ATTR_COUNTERS] = { + .len = sizeof(struct tsn_qci_psfp_sfi_counters)}, + [TSN_QCI_SFI_ATTR_OVERSIZE_ENABLE] = { .type = NLA_FLAG}, + [TSN_QCI_SFI_ATTR_OVERSIZE] = { .type = NLA_FLAG}, +}; + +static const struct nla_policy qci_sgi_policy[] = { + [TSN_QCI_SGI_ATTR_INDEX] = { .type = NLA_U32}, + [TSN_QCI_SGI_ATTR_ENABLE] = { .type = NLA_FLAG}, + [TSN_QCI_SGI_ATTR_DISABLE] = { .type = NLA_FLAG}, + [TSN_QCI_SGI_ATTR_CONFCHANGE] = { .type = NLA_FLAG}, + [TSN_QCI_SGI_ATTR_IRXEN] = { .type = NLA_FLAG}, + [TSN_QCI_SGI_ATTR_IRX] = { .type = NLA_FLAG}, + [TSN_QCI_SGI_ATTR_OEXEN] = { .type = NLA_FLAG}, + [TSN_QCI_SGI_ATTR_OEX] = { .type = NLA_FLAG}, + [TSN_QCI_SGI_ATTR_ADMINENTRY] = { .type = NLA_NESTED}, + [TSN_QCI_SGI_ATTR_OPERENTRY] = { .type = NLA_NESTED}, + [TSN_QCI_SGI_ATTR_CCTIME] = { .type = NLA_U64}, + [TSN_QCI_SGI_ATTR_TICKG] = { .type = NLA_U32}, + [TSN_QCI_SGI_ATTR_CUTIME] = { .type = NLA_U64}, + [TSN_QCI_SGI_ATTR_CPENDING] = { .type = NLA_FLAG}, + [TSN_QCI_SGI_ATTR_CCERROR] = { .type = NLA_U64}, +}; + +static const struct nla_policy qci_sgi_ctrl_policy[] = { + [TSN_SGI_ATTR_CTRL_INITSTATE] = { .type = NLA_FLAG}, + [TSN_SGI_ATTR_CTRL_LEN] = { .type = NLA_U8}, + [TSN_SGI_ATTR_CTRL_CYTIME] = { .type = NLA_U32}, + [TSN_SGI_ATTR_CTRL_CYTIMEEX] = { .type = NLA_U32}, + [TSN_SGI_ATTR_CTRL_BTIME] = { .type = NLA_U64}, + [TSN_SGI_ATTR_CTRL_INITIPV] = { .type = NLA_S8}, + [TSN_SGI_ATTR_CTRL_GCLENTRY] = { .type = NLA_NESTED}, +}; + +static const struct nla_policy qci_sgi_gcl_policy[] = { + [TSN_SGI_ATTR_GCL_GATESTATE] = { .type = NLA_FLAG}, + [TSN_SGI_ATTR_GCL_IPV] = { .type = NLA_S8}, + [TSN_SGI_ATTR_GCL_INTERVAL] = { .type = NLA_U32}, + [TSN_SGI_ATTR_GCL_OCTMAX] = { .type = NLA_U32}, +}; + +static const struct nla_policy qci_fmi_policy[] = { + [TSN_QCI_FMI_ATTR_INDEX] = { .type = NLA_U32}, + [TSN_QCI_FMI_ATTR_ENABLE] = { .type = NLA_FLAG}, + [TSN_QCI_FMI_ATTR_DISABLE] = { .type = NLA_FLAG}, + [TSN_QCI_FMI_ATTR_CIR] = { .type = NLA_U32}, + [TSN_QCI_FMI_ATTR_CBS] = { .type = NLA_U32}, + [TSN_QCI_FMI_ATTR_EIR] = { .type = NLA_U32}, + [TSN_QCI_FMI_ATTR_EBS] = { .type = NLA_U32}, + [TSN_QCI_FMI_ATTR_CF] = { .type = NLA_FLAG}, + [TSN_QCI_FMI_ATTR_CM] = { .type = NLA_FLAG}, + [TSN_QCI_FMI_ATTR_DROPYL] = { .type = NLA_FLAG}, + [TSN_QCI_FMI_ATTR_MAREDEN] = { .type = NLA_FLAG}, + [TSN_QCI_FMI_ATTR_MARED] = { .type = NLA_FLAG}, + [TSN_QCI_FMI_ATTR_COUNTERS] = { + .len = sizeof(struct tsn_qci_psfp_fmi_counters)}, +}; + +static const struct nla_policy dscp_policy[] = { + [TSN_DSCP_ATTR_INDEX] = { .type = NLA_U32}, + [TSN_DSCP_ATTR_DISABLE] = { .type = NLA_FLAG}, + [TSN_DSCP_ATTR_COS] = { .type = NLA_U8}, + [TSN_DSCP_ATTR_DPL] = { .type = NLA_U8}, +}; + +static ATOMIC_NOTIFIER_HEAD(tsn_notif_chain); + +/** + * register_tsn_notifier - Register notifier + * @nb: notifier_block + * + * Register switch device notifier. + */ +int register_tsn_notifier(struct notifier_block *nb) +{ + return atomic_notifier_chain_register(&tsn_notif_chain, nb); +} +EXPORT_SYMBOL_GPL(register_tsn_notifier); + +/** + * unregister_tsn_notifier - Unregister notifier + * @nb: notifier_block + * + * Unregister switch device notifier. + */ +int unregister_tsn_notifier(struct notifier_block *nb) +{ + return atomic_notifier_chain_unregister(&tsn_notif_chain, nb); +} +EXPORT_SYMBOL_GPL(unregister_tsn_notifier); + +/** + * call_tsn_notifiers - Call notifiers + * @val: value passed unmodified to notifier function + * @dev: port device + * @info: notifier information data + * + * Call all network notifier blocks. + */ +int call_tsn_notifiers(unsigned long val, struct net_device *dev, + struct tsn_notifier_info *info) +{ + info->dev = dev; + return atomic_notifier_call_chain(&tsn_notif_chain, val, info); +} +EXPORT_SYMBOL_GPL(call_tsn_notifiers); + +struct tsn_port *tsn_get_port(struct net_device *ndev) +{ + struct tsn_port *port; + bool tsn_found = false; + + list_for_each_entry(port, &port_list, list) { + if (port->netdev == ndev) { + tsn_found = true; + break; + } + } + + if (!tsn_found) + return NULL; + + return port; +} +EXPORT_SYMBOL_GPL(tsn_get_port); + +static int tsn_prepare_reply(struct genl_info *info, u8 cmd, + struct sk_buff **skbp, size_t size) +{ + struct sk_buff *skb; + void *reply; + + /* If new attributes are added, please revisit this allocation + */ + skb = genlmsg_new(size, GFP_KERNEL); + if (!skb) + return -ENOMEM; + + if (!info) { + nlmsg_free(skb); + return -EINVAL; + } + + reply = genlmsg_put_reply(skb, info, &tsn_family, 0, cmd); + if (!reply) { + nlmsg_free(skb); + return -EINVAL; + } + + *skbp = skb; + return 0; +} + +static int tsn_mk_reply(struct sk_buff *skb, int aggr, void *data, int len) +{ + /* add a netlink attribute to a socket buffer */ + return nla_put(skb, aggr, len, data); +} + +static int tsn_send_reply(struct sk_buff *skb, struct genl_info *info) +{ + struct genlmsghdr *genlhdr = nlmsg_data(nlmsg_hdr(skb)); + void *reply = genlmsg_data(genlhdr); + + genlmsg_end(skb, reply); + + return genlmsg_reply(skb, info); +} + +static int cmd_attr_echo_message(struct genl_info *info) +{ + struct nlattr *na; + char *msg; + struct sk_buff *rep_skb; + size_t size; + int ret; + + na = info->attrs[TSN_CMD_ATTR_MESG]; + if (!na) + return -EINVAL; + + msg = (char *)nla_data(na); + pr_info("tsn generic netlink receive echo mesg %s\n", msg); + + size = nla_total_size(strlen(msg) + 1); + + ret = tsn_prepare_reply(info, TSN_CMD_REPLY, &rep_skb, + size + NLMSG_ALIGN(MAX_USER_SIZE)); + if (ret < 0) + return ret; + + ret = tsn_mk_reply(rep_skb, TSN_CMD_ATTR_MESG, msg, size); + if (ret < 0) + goto err; + + return tsn_send_reply(rep_skb, info); + +err: + nlmsg_free(rep_skb); + return ret; +} + +static int cmd_attr_echo_data(struct genl_info *info) +{ + struct nlattr *na; + s32 data; + struct sk_buff *rep_skb; + size_t size; + int ret; + + /*read data */ + na = info->attrs[TSN_CMD_ATTR_DATA]; + if (!na) + return -EINVAL; + + data = nla_get_s32(info->attrs[TSN_CMD_ATTR_DATA]); + pr_info("tsn generic netlink receive echo data %d\n", data); + + /* send back */ + size = nla_total_size(sizeof(s32)); + + ret = tsn_prepare_reply(info, TSN_CMD_REPLY, &rep_skb, + size + NLMSG_ALIGN(MAX_USER_SIZE)); + if (ret < 0) + return ret; + + /* netlink lib func */ + ret = nla_put_s32(rep_skb, TSN_CMD_ATTR_DATA, data); + if (ret < 0) + goto err; + + return tsn_send_reply(rep_skb, info); + +err: + nlmsg_free(rep_skb); + return ret; +} + +static int tsn_echo_cmd(struct sk_buff *skb, struct genl_info *info) +{ + if (info->attrs[TSN_CMD_ATTR_MESG]) + return cmd_attr_echo_message(info); + else if (info->attrs[TSN_CMD_ATTR_DATA]) + return cmd_attr_echo_data(info); + + return -EINVAL; +} + +static int tsn_simple_reply(struct genl_info *info, u32 cmd, + char *portname, s32 retvalue) +{ + struct sk_buff *rep_skb; + size_t size; + int ret; + + /* send back */ + size = nla_total_size(strlen(portname) + 1); + size += nla_total_size(sizeof(s32)); + + ret = tsn_prepare_reply(info, cmd, + &rep_skb, size + NLMSG_ALIGN(MAX_USER_SIZE)); + if (ret < 0) + return ret; + + /* netlink lib func */ + ret = nla_put_string(rep_skb, TSN_ATTR_IFNAME, portname); + if (ret < 0) + goto err; + + ret = nla_put_s32(rep_skb, TSN_CMD_ATTR_DATA, retvalue); + if (ret < 0) + goto err; + + return tsn_send_reply(rep_skb, info); + +err: + nlmsg_free(rep_skb); + return ret; +} + +struct tsn_port *tsn_init_check(struct genl_info *info, + struct net_device **ndev) +{ + struct nlattr *na; + char *portname; + struct net_device *netdev; + struct tsn_port *port; + bool tsn_found = false; + + if (!info->attrs[TSN_ATTR_IFNAME]) { + tsn_simple_reply(info, TSN_CMD_REPLY, + "no portname", -EINVAL); + return NULL; + } + + na = info->attrs[TSN_ATTR_IFNAME]; + + portname = (char *)nla_data(na); + + netdev = __dev_get_by_name(genl_info_net(info), portname); + if (!netdev) { + tsn_simple_reply(info, TSN_CMD_REPLY, + "error device", -ENODEV); + return NULL; + } + + list_for_each_entry(port, &port_list, list) { + if (port->netdev == netdev) { + tsn_found = true; + break; + } + } + + if (!tsn_found) { + tsn_simple_reply(info, TSN_CMD_REPLY, + netdev->name, -ENODEV); + return NULL; + } + + *ndev = netdev; + + return port; +} + +static int tsn_cap_get(struct sk_buff *skb, struct genl_info *info) +{ + struct sk_buff *rep_skb; + struct nlattr *tsn_cap_attr; + int ret; + u32 cap = 0; + struct net_device *netdev; + struct genlmsghdr *genlhdr; + const struct tsn_ops *tsnops; + struct tsn_port *port; + + port = tsn_init_check(info, &netdev); + if (!port) { + ret = -ENODEV; + goto out; + } + + tsnops = port->tsnops; + genlhdr = info->genlhdr; + if (!tsnops->get_capability) { + ret = -EOPNOTSUPP; + goto out; + } + + cap = tsnops->get_capability(netdev); + if (!cap) { + ret = -EOPNOTSUPP; + goto out; + } + + /* Pad netlink reply data */ + ret = tsn_prepare_reply(info, genlhdr->cmd, + &rep_skb, NLMSG_ALIGN(MAX_ATTR_SIZE)); + if (ret < 0) + goto out; + + if (nla_put_string(rep_skb, TSN_ATTR_IFNAME, netdev->name)) { + ret = -EMSGSIZE; + goto err; + } + + tsn_cap_attr = nla_nest_start_noflag(rep_skb, TSN_ATTR_CAP); + if (!tsn_cap_attr) { + ret = -EMSGSIZE; + goto err; + } + + if (cap & TSN_CAP_QBV) { + if (nla_put_flag(rep_skb, TSN_CAP_ATTR_QBV)) + goto err; + } + + if (cap & TSN_CAP_QCI) { + if (nla_put_flag(rep_skb, TSN_CAP_ATTR_QCI)) + goto err; + } + + if (cap & TSN_CAP_QBU) { + if (nla_put_flag(rep_skb, TSN_CAP_ATTR_QBU)) + goto err; + } + + if (cap & TSN_CAP_CBS) { + if (nla_put_flag(rep_skb, TSN_CAP_ATTR_CBS)) + goto err; + } + + if (cap & TSN_CAP_CB) { + if (nla_put_flag(rep_skb, TSN_CAP_ATTR_CB)) + goto err; + } + + if (cap & TSN_CAP_TBS) { + if (nla_put_flag(rep_skb, TSN_CAP_ATTR_TBS)) + goto err; + } + + if (cap & TSN_CAP_CTH) { + if (nla_put_flag(rep_skb, TSN_CAP_ATTR_CTH)) + goto err; + } + + nla_nest_end(rep_skb, tsn_cap_attr); + + tsn_send_reply(rep_skb, info); + return 0; +err: + nlmsg_free(rep_skb); +out: + if (ret < 0) + tsn_simple_reply(info, TSN_CMD_REPLY, netdev->name, ret); + return ret; +} + +static int cmd_cb_streamid_set(struct genl_info *info) +{ + struct nlattr *na, *sid[TSN_STREAMID_ATTR_MAX + 1]; + u32 sid_index; + u8 iden_type = 1; + bool enable; + int ret; + struct net_device *netdev; + struct tsn_cb_streamid sidconf; + const struct tsn_ops *tsnops; + struct tsn_port *port; + + port = tsn_init_check(info, &netdev); + if (!port) + return -ENODEV; + + tsnops = port->tsnops; + + memset(&sidconf, 0, sizeof(struct tsn_cb_streamid)); + + if (!info->attrs[TSN_ATTR_STREAM_IDENTIFY]) + return -EINVAL; + + na = info->attrs[TSN_ATTR_STREAM_IDENTIFY]; + + ret = NLA_PARSE_NESTED(sid, TSN_STREAMID_ATTR_MAX, + na, cb_streamid_policy); + if (ret) + return -EINVAL; + + if (!sid[TSN_STREAMID_ATTR_INDEX]) + return -EINVAL; + + sid_index = nla_get_u32(sid[TSN_STREAMID_ATTR_INDEX]); + + if (sid[TSN_STREAMID_ATTR_ENABLE]) + enable = true; + else if (sid[TSN_STREAMID_ATTR_DISABLE]) + enable = false; + else + return -EINVAL; + + if (!enable) + goto loaddev; + + if (sid[TSN_STREAMID_ATTR_TYPE]) + iden_type = nla_get_u8(sid[TSN_STREAMID_ATTR_TYPE]); + else + return -EINVAL; + + sidconf.type = iden_type; + switch (iden_type) { + case STREAMID_NULL: + if (!sid[TSN_STREAMID_ATTR_NDMAC] || + !sid[TSN_STREAMID_ATTR_NTAGGED] || + !sid[TSN_STREAMID_ATTR_NVID]) { + return -EINVAL; + } + + sidconf.para.nid.dmac = + nla_get_u64(sid[TSN_STREAMID_ATTR_NDMAC]); + sidconf.para.nid.tagged = + nla_get_u8(sid[TSN_STREAMID_ATTR_NTAGGED]); + sidconf.para.nid.vid = + nla_get_u16(sid[TSN_STREAMID_ATTR_NVID]); + break; + case STREAMID_SMAC_VLAN: + /* TODO: not supportted yet */ + if (!sid[TSN_STREAMID_ATTR_SMAC] || + !sid[TSN_STREAMID_ATTR_STAGGED] || + !sid[TSN_STREAMID_ATTR_SVID]) { + return -EINVAL; + } + + sidconf.para.sid.smac = + nla_get_u64(sid[TSN_STREAMID_ATTR_SMAC]); + sidconf.para.sid.tagged = + nla_get_u8(sid[TSN_STREAMID_ATTR_STAGGED]); + sidconf.para.sid.vid = + nla_get_u16(sid[TSN_STREAMID_ATTR_SVID]); + break; + case STREAMID_DMAC_VLAN: + + case STREAMID_IP: + + default: + tsn_simple_reply(info, TSN_CMD_REPLY, + netdev->name, -EINVAL); + return -EINVAL; + } + + if (sid[TSN_STREAMID_ATTR_STREAM_HANDLE]) + sidconf.handle = + nla_get_s32(sid[TSN_STREAMID_ATTR_STREAM_HANDLE]); + + if (sid[TSN_STREAMID_ATTR_IFOP]) + sidconf.ifac_oport = nla_get_u32(sid[TSN_STREAMID_ATTR_IFOP]); + if (sid[TSN_STREAMID_ATTR_OFOP]) + sidconf.ofac_oport = nla_get_u32(sid[TSN_STREAMID_ATTR_OFOP]); + if (sid[TSN_STREAMID_ATTR_IFIP]) + sidconf.ifac_iport = nla_get_u32(sid[TSN_STREAMID_ATTR_IFIP]); + if (sid[TSN_STREAMID_ATTR_OFIP]) + sidconf.ofac_iport = nla_get_u32(sid[TSN_STREAMID_ATTR_OFIP]); + +loaddev: + if (!tsnops->cb_streamid_set) { + tsn_simple_reply(info, TSN_CMD_REPLY, + netdev->name, -EPERM); + return -EOPNOTSUPP; + } + + ret = tsnops->cb_streamid_set(netdev, sid_index, enable, &sidconf); + if (ret < 0) { + tsn_simple_reply(info, TSN_CMD_REPLY, netdev->name, ret); + return ret; + } + + /* simple reply here. To be continue */ + if (tsn_simple_reply(info, TSN_CMD_REPLY, netdev->name, 0)) + return -1; + + return 0; +} + +static int tsn_cb_streamid_set(struct sk_buff *skb, struct genl_info *info) +{ + if (info->attrs[TSN_ATTR_IFNAME]) { + cmd_cb_streamid_set(info); + return 0; + } + + return -1; +} + +static int cmd_cb_streamid_get(struct genl_info *info) +{ + struct nlattr *na, *sidattr, *sid[TSN_STREAMID_ATTR_MAX + 1]; + u32 sid_index; + struct genlmsghdr *genlhdr; + struct sk_buff *rep_skb; + int ret, i; + int valid; + struct net_device *netdev; + struct tsn_cb_streamid sidconf; + struct tsn_cb_streamid_counters sidcounts; + const struct tsn_ops *tsnops; + struct tsn_port *port; + + port = tsn_init_check(info, &netdev); + if (!port) + return -ENODEV; + + tsnops = port->tsnops; + + memset(&sidconf, 0, sizeof(struct tsn_cb_streamid)); + memset(&sidcounts, 0, sizeof(struct tsn_cb_streamid_counters)); + + if (!info->attrs[TSN_ATTR_STREAM_IDENTIFY]) + return -EINVAL; + + na = info->attrs[TSN_ATTR_STREAM_IDENTIFY]; + + ret = NLA_PARSE_NESTED(sid, TSN_STREAMID_ATTR_MAX, + na, cb_streamid_policy); + if (ret) + return -EINVAL; + + if (!sid[TSN_STREAMID_ATTR_INDEX]) + return -EINVAL; + + sid_index = nla_get_u32(sid[TSN_STREAMID_ATTR_INDEX]); + + if (!tsnops->cb_streamid_get) { + tsn_simple_reply(info, TSN_CMD_REPLY, + netdev->name, -EPERM); + ret = -EINVAL; + goto exit; + } else { + valid = tsnops->cb_streamid_get(netdev, sid_index, &sidconf); + if (valid < 0) { + tsn_simple_reply(info, TSN_CMD_REPLY, + netdev->name, valid); + return valid; + } + } + + /* send back */ + genlhdr = info->genlhdr; + ret = tsn_prepare_reply(info, genlhdr->cmd, &rep_skb, + NLMSG_ALIGN(MAX_ATTR_SIZE)); + if (ret < 0) + return ret; + + /* input netlink the parameters */ + sidattr = nla_nest_start_noflag(rep_skb, TSN_ATTR_STREAM_IDENTIFY); + if (!sidattr) { + ret = -EINVAL; + goto err; + } + + if (nla_put_u32(rep_skb, TSN_STREAMID_ATTR_INDEX, sid_index)) + goto err; + + if (valid == 1) { + if (nla_put_flag(rep_skb, TSN_STREAMID_ATTR_ENABLE)) + goto err; + } else if (valid == 0) { + if (nla_put_flag(rep_skb, TSN_STREAMID_ATTR_DISABLE)) + goto err; + } else { + tsn_simple_reply(info, TSN_CMD_REPLY, + netdev->name, -EINVAL); + goto err; + } + + if (nla_put_s32(rep_skb, + TSN_STREAMID_ATTR_STREAM_HANDLE, sidconf.handle) || + nla_put_u32(rep_skb, TSN_STREAMID_ATTR_IFOP, sidconf.ifac_oport) || + nla_put_u32(rep_skb, TSN_STREAMID_ATTR_OFOP, sidconf.ofac_oport) || + nla_put_u32(rep_skb, TSN_STREAMID_ATTR_IFIP, sidconf.ifac_iport) || + nla_put_u32(rep_skb, TSN_STREAMID_ATTR_OFIP, sidconf.ofac_iport) || + nla_put_u8(rep_skb, TSN_STREAMID_ATTR_TYPE, sidconf.type)) + goto err; + + switch (sidconf.type) { + case STREAMID_NULL: + if (NLA_PUT_U64(rep_skb, TSN_STREAMID_ATTR_NDMAC, + sidconf.para.nid.dmac) || + nla_put_u16(rep_skb, TSN_STREAMID_ATTR_NVID, + sidconf.para.nid.vid) || + nla_put_u8(rep_skb, TSN_STREAMID_ATTR_NTAGGED, + sidconf.para.nid.tagged)) + goto err; + break; + case STREAMID_SMAC_VLAN: + if (NLA_PUT_U64(rep_skb, TSN_STREAMID_ATTR_SMAC, + sidconf.para.sid.smac) || + nla_put_u16(rep_skb, TSN_STREAMID_ATTR_SVID, + sidconf.para.sid.vid) || + nla_put_u8(rep_skb, TSN_STREAMID_ATTR_STAGGED, + sidconf.para.sid.tagged)) + goto err; + break; + case STREAMID_DMAC_VLAN: + case STREAMID_IP: + default: + tsn_simple_reply(info, TSN_CMD_REPLY, + netdev->name, -EINVAL); + goto err; + } + + if (!tsnops->cb_streamid_counters_get) { + tsn_simple_reply(info, TSN_CMD_REPLY, + netdev->name, -EPERM); + goto err; + } else { + ret = tsnops->cb_streamid_counters_get(netdev, + sid_index, + &sidcounts); + if (ret < 0) { + tsn_simple_reply(info, TSN_CMD_REPLY, + netdev->name, ret); + goto err; + } + } + + if (NLA_PUT_U64(rep_skb, TSN_STREAMID_ATTR_COUNTERS_PSI, + sidcounts.per_stream.input) || + NLA_PUT_U64(rep_skb, TSN_STREAMID_ATTR_COUNTERS_PSO, + sidcounts.per_stream.output)) + goto err; + + for (i = 0; i < 32; i++) { + if (NLA_PUT_U64(rep_skb, TSN_STREAMID_ATTR_COUNTERS_PSPPI, + sidcounts.per_streamport[i].input) || + NLA_PUT_U64(rep_skb, TSN_STREAMID_ATTR_COUNTERS_PSPPO, + sidcounts.per_streamport[i].output)) + goto err; + } + + nla_nest_end(rep_skb, sidattr); + /* end netlink input the parameters */ + + /* netlink lib func */ + ret = nla_put_string(rep_skb, TSN_ATTR_IFNAME, netdev->name); + if (ret < 0) + goto err; + + ret = nla_put_s32(rep_skb, TSN_CMD_ATTR_DATA, 0); + if (ret < 0) + goto err; + + return tsn_send_reply(rep_skb, info); + +err: + nlmsg_free(rep_skb); +exit: + return ret; +} + +static int tsn_cb_streamid_get(struct sk_buff *skb, struct genl_info *info) +{ + if (info->attrs[TSN_ATTR_IFNAME]) { + cmd_cb_streamid_get(info); + return 0; + } + + return -1; +} + +static int cmb_cb_streamid_counters_get(struct genl_info *info) +{ + return 0; +} + +static int tsn_cb_streamid_counters_get(struct sk_buff *skb, + struct genl_info *info) +{ + if (info->attrs[TSN_ATTR_IFNAME]) { + cmb_cb_streamid_counters_get(info); + return 0; + } + + return -1; +} + +static int tsn_qci_cap_get(struct sk_buff *skb, struct genl_info *info) +{ + struct nlattr *qci_cap; + struct sk_buff *rep_skb; + int ret; + struct net_device *netdev; + struct genlmsghdr *genlhdr; + struct tsn_qci_psfp_stream_param qci_cap_status; + const struct tsn_ops *tsnops; + struct tsn_port *port; + + port = tsn_init_check(info, &netdev); + if (!port) { + ret = -EINVAL; + goto out; + } + + tsnops = port->tsnops; + + genlhdr = info->genlhdr; + + memset(&qci_cap_status, 0, sizeof(qci_cap_status)); + + if (!tsnops->qci_get_maxcap) { + ret = -EOPNOTSUPP; + goto out; + } + + ret = tsnops->qci_get_maxcap(netdev, &qci_cap_status); + if (ret < 0) + goto out; + + /* Pad netlink reply data */ + ret = tsn_prepare_reply(info, genlhdr->cmd, + &rep_skb, NLMSG_ALIGN(MAX_ATTR_SIZE)); + if (ret < 0) + goto out; + + if (nla_put_string(rep_skb, TSN_ATTR_IFNAME, netdev->name)) { + ret = -EMSGSIZE; + goto err; + } + + qci_cap = nla_nest_start_noflag(rep_skb, TSN_ATTR_QCI_SP); + if (!qci_cap) { + ret = -EMSGSIZE; + goto err; + } + + if (nla_put_u32(rep_skb, TSN_QCI_STREAM_ATTR_MAX_SFI, + qci_cap_status.max_sf_instance) || + nla_put_u32(rep_skb, TSN_QCI_STREAM_ATTR_MAX_SGI, + qci_cap_status.max_sg_instance) || + nla_put_u32(rep_skb, TSN_QCI_STREAM_ATTR_MAX_FMI, + qci_cap_status.max_fm_instance) || + nla_put_u32(rep_skb, TSN_QCI_STREAM_ATTR_SLM, + qci_cap_status.supported_list_max)) { + ret = -EMSGSIZE; + goto err; + } + + nla_nest_end(rep_skb, qci_cap); + + tsn_send_reply(rep_skb, info); + + return 0; +err: + nlmsg_free(rep_skb); +out: + if (ret < 0) + tsn_simple_reply(info, TSN_CMD_REPLY, netdev->name, ret); + + return ret; +} + +static int cmd_qci_sfi_set(struct genl_info *info) +{ + struct nlattr *na, *sfi[TSN_QCI_SFI_ATTR_MAX + 1]; + u32 sfi_handle; + bool enable; + int ret; + struct net_device *netdev; + struct tsn_qci_psfp_sfi_conf sficonf; + const struct tsn_ops *tsnops; + struct tsn_port *port; + + port = tsn_init_check(info, &netdev); + if (!port) + return -ENODEV; + + tsnops = port->tsnops; + + memset(&sficonf, 0, sizeof(struct tsn_qci_psfp_sfi_conf)); + + if (!info->attrs[TSN_ATTR_QCI_SFI]) + return -EINVAL; + + na = info->attrs[TSN_ATTR_QCI_SFI]; + + ret = NLA_PARSE_NESTED(sfi, TSN_QCI_SFI_ATTR_MAX, na, qci_sfi_policy); + if (ret) { + pr_info("tsn: parse value TSN_QCI_SFI_ATTR_MAX error."); + return -EINVAL; + } + + if (!sfi[TSN_QCI_SFI_ATTR_INDEX]) + return -EINVAL; + + sfi_handle = nla_get_u32(sfi[TSN_QCI_SFI_ATTR_INDEX]); + + if (sfi[TSN_QCI_SFI_ATTR_ENABLE]) { + enable = true; + } else if (sfi[TSN_QCI_SFI_ATTR_DISABLE]) { + enable = false; + goto loaddrive; + } else { + pr_err("tsn: must provde ENABLE or DISABLE attribute.\n"); + tsn_simple_reply(info, TSN_CMD_REPLY, + netdev->name, -EINVAL); + return -EINVAL; + } + + if (!sfi[TSN_QCI_SFI_ATTR_GATE_ID]) { + pr_err("tsn: must provide stream gate index\n"); + tsn_simple_reply(info, TSN_CMD_REPLY, + netdev->name, -EINVAL); + return -EINVAL; + } + + if (!sfi[TSN_QCI_SFI_ATTR_STREAM_HANDLE]) + sficonf.stream_handle_spec = -1; + else + sficonf.stream_handle_spec = + nla_get_s32(sfi[TSN_QCI_SFI_ATTR_STREAM_HANDLE]); + + if (!sfi[TSN_QCI_SFI_ATTR_PRIO_SPEC]) + sficonf.priority_spec = -1; + else + sficonf.priority_spec = + nla_get_s8(sfi[TSN_QCI_SFI_ATTR_PRIO_SPEC]); + + sficonf.stream_gate_instance_id = + nla_get_u32(sfi[TSN_QCI_SFI_ATTR_GATE_ID]); + + if (sfi[TSN_QCI_SFI_ATTR_MAXSDU]) + sficonf.stream_filter.maximum_sdu_size = + nla_get_u16(sfi[TSN_QCI_SFI_ATTR_MAXSDU]); + else + sficonf.stream_filter.maximum_sdu_size = 0; + + if (sfi[TSN_QCI_SFI_ATTR_FLOW_ID]) + sficonf.stream_filter.flow_meter_instance_id = + nla_get_s32(sfi[TSN_QCI_SFI_ATTR_FLOW_ID]); + else + sficonf.stream_filter.flow_meter_instance_id = -1; + + if (sfi[TSN_QCI_SFI_ATTR_OVERSIZE_ENABLE]) + sficonf.block_oversize_enable = true; + + if (sfi[TSN_QCI_SFI_ATTR_OVERSIZE]) + sficonf.block_oversize = true; + +loaddrive: + if (!tsnops->qci_sfi_set) { + tsn_simple_reply(info, TSN_CMD_REPLY, + netdev->name, -EPERM); + return -EINVAL; + } + + ret = tsnops->qci_sfi_set(netdev, sfi_handle, enable, &sficonf); + if (ret < 0) { + tsn_simple_reply(info, TSN_CMD_REPLY, netdev->name, ret); + return ret; + } + + ret = tsn_simple_reply(info, TSN_CMD_REPLY, netdev->name, 0); + + if (ret) + return ret; + return 0; +} + +static int tsn_qci_sfi_set(struct sk_buff *skb, struct genl_info *info) +{ + if (info->attrs[TSN_ATTR_IFNAME]) { + cmd_qci_sfi_set(info); + return 0; + } + + return -1; +} + +static int cmd_qci_sfi_get(struct genl_info *info) +{ + struct nlattr *na, *sfiattr; + struct nlattr *sfi[TSN_QCI_SFI_ATTR_MAX + 1]; + u32 sfi_handle; + struct sk_buff *rep_skb; + int ret, valid = 0; + struct net_device *netdev; + struct genlmsghdr *genlhdr; + struct tsn_qci_psfp_sfi_conf sficonf; + struct tsn_qci_psfp_sfi_counters sficount; + const struct tsn_ops *tsnops; + struct tsn_port *port; + + port = tsn_init_check(info, &netdev); + if (!port) + return -ENODEV; + + tsnops = port->tsnops; + + genlhdr = info->genlhdr; + + if (!info->attrs[TSN_ATTR_QCI_SFI]) + return -EINVAL; + + na = info->attrs[TSN_ATTR_QCI_SFI]; + + ret = NLA_PARSE_NESTED(sfi, TSN_QCI_SFI_ATTR_MAX, + na, qci_sfi_policy); + if (ret) + return -EINVAL; + + if (!sfi[TSN_QCI_SFI_ATTR_INDEX]) + return -EINVAL; + + sfi_handle = nla_get_u32(sfi[TSN_QCI_SFI_ATTR_INDEX]); + + memset(&sficonf, 0, sizeof(struct tsn_qci_psfp_sfi_conf)); + memset(&sficount, 0, sizeof(struct tsn_qci_psfp_sfi_counters)); + + if (!tsnops->qci_sfi_get || !tsnops->qci_sfi_counters_get) { + tsn_simple_reply(info, TSN_CMD_REPLY, + netdev->name, -EPERM); + ret = -EINVAL; + goto exit; + } else { + valid = tsnops->qci_sfi_get(netdev, sfi_handle, &sficonf); + if (valid < 0) { + tsn_simple_reply(info, TSN_CMD_REPLY, + netdev->name, valid); + return valid; + } + + valid = tsnops->qci_sfi_counters_get(netdev, sfi_handle, + &sficount); + if (valid < 0) { + tsn_simple_reply(info, TSN_CMD_REPLY, + netdev->name, valid); + return valid; + } + } + + ret = tsn_prepare_reply(info, genlhdr->cmd, + &rep_skb, NLMSG_ALIGN(MAX_ATTR_SIZE)); + if (ret < 0) + return ret; + + if (nla_put_string(rep_skb, TSN_ATTR_IFNAME, netdev->name)) + goto err; + + sfiattr = nla_nest_start_noflag(rep_skb, TSN_ATTR_QCI_SFI); + if (!sfiattr) { + tsn_simple_reply(info, TSN_CMD_REPLY, + netdev->name, -EINVAL); + ret = -EINVAL; + goto err; + } + + if (nla_put_u32(rep_skb, TSN_QCI_SFI_ATTR_INDEX, sfi_handle)) + goto err; + + if (valid) { + if (nla_put_flag(rep_skb, TSN_QCI_SFI_ATTR_ENABLE)) + goto err; + } else { + if (nla_put_flag(rep_skb, TSN_QCI_SFI_ATTR_DISABLE)) + goto err; + } + + if (nla_put_s32(rep_skb, TSN_QCI_SFI_ATTR_STREAM_HANDLE, + sficonf.stream_handle_spec) || + nla_put_s8(rep_skb, TSN_QCI_SFI_ATTR_PRIO_SPEC, + sficonf.priority_spec) || + nla_put_u32(rep_skb, TSN_QCI_SFI_ATTR_GATE_ID, + sficonf.stream_gate_instance_id)) + goto err; + + if (sficonf.stream_filter.maximum_sdu_size) + if (nla_put_u16(rep_skb, TSN_QCI_SFI_ATTR_MAXSDU, + sficonf.stream_filter.maximum_sdu_size)) + goto err; + + if (sficonf.stream_filter.flow_meter_instance_id >= 0) + if (nla_put_s32(rep_skb, TSN_QCI_SFI_ATTR_FLOW_ID, + sficonf.stream_filter.flow_meter_instance_id)) + goto err; + + if (sficonf.block_oversize_enable) + if (nla_put_flag(rep_skb, TSN_QCI_SFI_ATTR_OVERSIZE_ENABLE)) + goto err; + if (sficonf.block_oversize) + if (nla_put_flag(rep_skb, TSN_QCI_SFI_ATTR_OVERSIZE)) + goto err; + + if (nla_put(rep_skb, TSN_QCI_SFI_ATTR_COUNTERS, + sizeof(struct tsn_qci_psfp_sfi_counters), &sficount)) + goto err; + + nla_nest_end(rep_skb, sfiattr); + + return tsn_send_reply(rep_skb, info); +err: + nlmsg_free(rep_skb); + tsn_simple_reply(info, TSN_CMD_REPLY, + netdev->name, -EINVAL); +exit: + return ret; +} + +static int tsn_qci_sfi_get(struct sk_buff *skb, struct genl_info *info) +{ + if (info->attrs[TSN_ATTR_IFNAME]) { + cmd_qci_sfi_get(info); + return 0; + } + + return -1; +} + +static int cmd_qci_sfi_counters_get(struct genl_info *info) +{ + struct nlattr *na, *sfiattr; + struct nlattr *sfi[TSN_QCI_SFI_ATTR_MAX + 1]; + u32 sfi_handle; + struct sk_buff *rep_skb; + int ret; + struct net_device *netdev; + struct genlmsghdr *genlhdr; + struct tsn_qci_psfp_sfi_counters sficount; + const struct tsn_ops *tsnops; + struct tsn_port *port; + + port = tsn_init_check(info, &netdev); + if (!port) + return -ENODEV; + + tsnops = port->tsnops; + + genlhdr = info->genlhdr; + + if (!info->attrs[TSN_ATTR_QCI_SFI]) + return -EINVAL; + + na = info->attrs[TSN_ATTR_QCI_SFI]; + + ret = NLA_PARSE_NESTED(sfi, TSN_QCI_SFI_ATTR_MAX, + na, qci_sfi_policy); + if (ret) + return -EINVAL; + + if (!sfi[TSN_QCI_SFI_ATTR_INDEX]) + return -EINVAL; + + sfi_handle = nla_get_u32(sfi[TSN_QCI_SFI_ATTR_INDEX]); + + memset(&sficount, 0, sizeof(struct tsn_qci_psfp_sfi_counters)); + if (!tsnops->qci_sfi_counters_get) { + tsn_simple_reply(info, TSN_CMD_REPLY, + netdev->name, -EPERM); + return -1; + } + + ret = tsnops->qci_sfi_counters_get(netdev, sfi_handle, &sficount); + if (ret < 0) { + tsn_simple_reply(info, TSN_CMD_REPLY, + netdev->name, ret); + return ret; + } + + ret = tsn_prepare_reply(info, genlhdr->cmd, &rep_skb, + NLMSG_ALIGN(MAX_ATTR_SIZE)); + if (ret < 0) + return ret; + + if (nla_put_string(rep_skb, TSN_ATTR_IFNAME, netdev->name)) + goto err; + + sfiattr = nla_nest_start_noflag(rep_skb, TSN_ATTR_QCI_SFI); + if (!sfiattr) { + ret = -EINVAL; + goto err; + } + + if (nla_put_u32(rep_skb, TSN_QCI_SFI_ATTR_INDEX, sfi_handle)) + goto err; + + ret = tsnops->qci_sfi_counters_get(netdev, sfi_handle, &sficount); + if (ret < 0) + goto err; + + if (nla_put(rep_skb, TSN_QCI_SFI_ATTR_COUNTERS, + sizeof(struct tsn_qci_psfp_sfi_counters), &sficount)) + goto err; + + nla_nest_end(rep_skb, sfiattr); + + return tsn_send_reply(rep_skb, info); +err: + nlmsg_free(rep_skb); + tsn_simple_reply(info, TSN_CMD_REPLY, netdev->name, -EINVAL); + return ret; +} + +static int tsn_qci_sfi_counters_get(struct sk_buff *skb, struct genl_info *info) +{ + if (info->attrs[TSN_ATTR_IFNAME]) { + cmd_qci_sfi_counters_get(info); + return 0; + } + + return -1; +} + +static int cmd_qci_sgi_set(struct genl_info *info) +{ + struct nlattr *na; + struct nlattr *sgia[TSN_QCI_SGI_ATTR_MAX + 1]; + struct nlattr *admin[TSN_SGI_ATTR_CTRL_MAX + 1]; + int ret = 0; + struct net_device *netdev; + const struct tsn_ops *tsnops; + struct tsn_qci_psfp_sgi_conf sgi; + struct tsn_qci_psfp_gcl *gcl = NULL; + u16 sgi_handle = 0; + u16 listcount = 0; + struct tsn_port *port; + + port = tsn_init_check(info, &netdev); + if (!port) + return -ENODEV; + + tsnops = port->tsnops; + + memset(&sgi, 0, sizeof(struct tsn_qci_psfp_sgi_conf)); + + if (!info->attrs[TSN_ATTR_QCI_SGI]) { + tsn_simple_reply(info, TSN_CMD_REPLY, + netdev->name, -EINVAL); + return -EINVAL; + } + + na = info->attrs[TSN_ATTR_QCI_SGI]; + + ret = NLA_PARSE_NESTED(sgia, TSN_QCI_SGI_ATTR_MAX, + na, qci_sgi_policy); + if (ret) { + tsn_simple_reply(info, TSN_CMD_REPLY, + netdev->name, -EINVAL); + return -EINVAL; + } + + if (sgia[TSN_QCI_SGI_ATTR_ENABLE] && sgia[TSN_QCI_SGI_ATTR_DISABLE]) { + pr_err("tsn: enable or disable?\n"); + tsn_simple_reply(info, TSN_CMD_REPLY, + netdev->name, -EINVAL); + return -1; + } + + if (sgia[TSN_QCI_SGI_ATTR_INDEX]) + sgi_handle = nla_get_u32(sgia[TSN_QCI_SGI_ATTR_INDEX]); + + if (sgia[TSN_QCI_SGI_ATTR_DISABLE]) { + sgi.gate_enabled = 0; + goto loaddev; + } else { + /* set default to be enable*/ + sgi.gate_enabled = 1; + } + + if (sgia[TSN_QCI_SGI_ATTR_CONFCHANGE]) + sgi.config_change = 1; + + if (sgia[TSN_QCI_SGI_ATTR_IRXEN]) + sgi.block_invalid_rx_enable = 1; + + if (sgia[TSN_QCI_SGI_ATTR_IRX]) + sgi.block_invalid_rx = 1; + + if (sgia[TSN_QCI_SGI_ATTR_OEXEN]) + sgi.block_octets_exceeded_enable = 1; + + if (sgia[TSN_QCI_SGI_ATTR_OEX]) + sgi.block_octets_exceeded = 1; + + if (sgia[TSN_QCI_SGI_ATTR_ADMINENTRY]) { + struct nlattr *entry; + int rem; + int count = 0; + + na = sgia[TSN_QCI_SGI_ATTR_ADMINENTRY]; + ret = NLA_PARSE_NESTED(admin, TSN_SGI_ATTR_CTRL_MAX, + na, qci_sgi_ctrl_policy); + + /* Other parameters in admin control */ + if (admin[TSN_SGI_ATTR_CTRL_INITSTATE]) + sgi.admin.gate_states = 1; + + if (admin[TSN_SGI_ATTR_CTRL_CYTIME]) + sgi.admin.cycle_time = + nla_get_u32(admin[TSN_SGI_ATTR_CTRL_CYTIME]); + + if (admin[TSN_SGI_ATTR_CTRL_CYTIMEEX]) + sgi.admin.cycle_time_extension = + nla_get_u32(admin[TSN_SGI_ATTR_CTRL_CYTIMEEX]); + + if (admin[TSN_SGI_ATTR_CTRL_BTIME]) + sgi.admin.base_time = + nla_get_u64(admin[TSN_SGI_ATTR_CTRL_BTIME]); + + if (admin[TSN_SGI_ATTR_CTRL_INITIPV]) + sgi.admin.init_ipv = + nla_get_s8(admin[TSN_SGI_ATTR_CTRL_INITIPV]); + else + sgi.admin.init_ipv = -1; + + if (admin[TSN_SGI_ATTR_CTRL_LEN]) { + sgi.admin.control_list_length = + nla_get_u8(admin[TSN_SGI_ATTR_CTRL_LEN]); + listcount = sgi.admin.control_list_length; + } + + if (!listcount) + goto loaddev; + + gcl = kmalloc_array(listcount, sizeof(*gcl), GFP_KERNEL); + + memset(gcl, 0, listcount * sizeof(struct tsn_qci_psfp_gcl)); + + /* Check the whole admin attrs, + * checkout the TSN_SGI_ATTR_CTRL_GCLENTRY attributes + */ + nla_for_each_nested(entry, na, rem) { + struct nlattr *gcl_entry[TSN_SGI_ATTR_GCL_MAX + 1]; + struct nlattr *ti, *om; + + if (nla_type(entry) != TSN_SGI_ATTR_CTRL_GCLENTRY) + continue; + + /* parse each TSN_SGI_ATTR_CTRL_GCLENTRY */ + ret = NLA_PARSE_NESTED(gcl_entry, TSN_SGI_ATTR_GCL_MAX, + entry, qci_sgi_gcl_policy); + /* Parse gate control list */ + if (gcl_entry[TSN_SGI_ATTR_GCL_GATESTATE]) + (gcl + count)->gate_state = 1; + + if (gcl_entry[TSN_SGI_ATTR_GCL_IPV]) + (gcl + count)->ipv = + nla_get_s8(gcl_entry[TSN_SGI_ATTR_GCL_IPV]); + + if (gcl_entry[TSN_SGI_ATTR_GCL_INTERVAL]) { + ti = gcl_entry[TSN_SGI_ATTR_GCL_INTERVAL]; + (gcl + count)->time_interval = nla_get_u32(ti); + } + + if (gcl_entry[TSN_SGI_ATTR_GCL_OCTMAX]) { + om = gcl_entry[TSN_SGI_ATTR_GCL_OCTMAX]; + (gcl + count)->octet_max = nla_get_u32(om); + } + + count++; + + if (count >= listcount) + break; + } + + if (count < listcount) { + tsn_simple_reply(info, TSN_CMD_REPLY, + netdev->name, -EINVAL); + pr_err("tsn: count less than TSN_SGI_ATTR_CTRL_LEN\n"); + kfree(gcl); + return -EINVAL; + } + + } else { + pr_info("tsn: no admin list parameters setting\n"); + } + +loaddev: + if (!tsnops->qci_sgi_set) { + tsn_simple_reply(info, TSN_CMD_REPLY, + netdev->name, -EPERM); + kfree(gcl); + return -EINVAL; + } + + sgi.admin.gcl = gcl; + + ret = tsnops->qci_sgi_set(netdev, sgi_handle, &sgi); + kfree(gcl); + if (!ret) + return tsn_simple_reply(info, TSN_CMD_REPLY, + netdev->name, 0); + + tsn_simple_reply(info, TSN_CMD_REPLY, + netdev->name, ret); + return ret; +} + +static int tsn_qci_sgi_set(struct sk_buff *skb, struct genl_info *info) +{ + if (info->attrs[TSN_ATTR_IFNAME]) { + cmd_qci_sgi_set(info); + return 0; + } + + return -1; +} + +static int cmd_qci_sgi_get(struct genl_info *info) +{ + struct nlattr *na, *sgiattr, *adminattr, *sglattr; + struct nlattr *sgi[TSN_QCI_SGI_ATTR_MAX + 1]; + struct sk_buff *rep_skb; + int ret; + struct net_device *netdev; + struct genlmsghdr *genlhdr; + struct tsn_qci_psfp_sgi_conf sgiadmin; + struct tsn_qci_psfp_gcl *gcl = NULL; + const struct tsn_ops *tsnops; + u16 sgi_handle; + u8 listcount, i; + struct tsn_port *port; + + port = tsn_init_check(info, &netdev); + if (!port) + return -ENODEV; + + tsnops = port->tsnops; + + if (!info->attrs[TSN_ATTR_QCI_SGI]) { + tsn_simple_reply(info, TSN_CMD_REPLY, + netdev->name, -EINVAL); + pr_err("tsn: no sgi handle input\n"); + return -EINVAL; + } + + na = info->attrs[TSN_ATTR_QCI_SGI]; + + ret = NLA_PARSE_NESTED(sgi, TSN_QCI_SGI_ATTR_MAX, + na, qci_sgi_policy); + if (ret) + return -EINVAL; + + if (!sgi[TSN_QCI_SGI_ATTR_INDEX]) { + tsn_simple_reply(info, TSN_CMD_REPLY, + netdev->name, -EINVAL); + pr_err("tsn: no sgi handle input\n"); + return -EINVAL; + } + + sgi_handle = nla_get_u32(sgi[TSN_QCI_SGI_ATTR_INDEX]); + + /* Get config data from device */ + genlhdr = info->genlhdr; + + memset(&sgiadmin, 0, sizeof(struct tsn_qci_psfp_sgi_conf)); + + if (!tsnops->qci_sgi_get) { + tsn_simple_reply(info, TSN_CMD_REPLY, + netdev->name, -EPERM); + return -1; + } + + ret = tsnops->qci_sgi_get(netdev, sgi_handle, &sgiadmin); + if (ret < 0) { + tsn_simple_reply(info, TSN_CMD_REPLY, + netdev->name, ret); + return ret; + } + + /* Form netlink reply data */ + ret = tsn_prepare_reply(info, genlhdr->cmd, + &rep_skb, NLMSG_ALIGN(MAX_ATTR_SIZE)); + if (ret < 0) + return ret; + + if (nla_put_string(rep_skb, TSN_ATTR_IFNAME, netdev->name)) + goto err; + + sgiattr = nla_nest_start_noflag(rep_skb, TSN_ATTR_QCI_SGI); + if (!sgiattr) + goto err; + + if (nla_put_u32(rep_skb, TSN_QCI_SGI_ATTR_INDEX, sgi_handle)) + goto err; + + /* Gate enable? sgiadmin.gate_enabled */ + if (sgiadmin.gate_enabled) { + if (nla_put_flag(rep_skb, TSN_QCI_SGI_ATTR_ENABLE)) + goto err; + } else { + if (nla_put_flag(rep_skb, TSN_QCI_SGI_ATTR_DISABLE)) + goto err; + } + + if (sgiadmin.config_change) + if (nla_put_flag(rep_skb, TSN_QCI_SGI_ATTR_CONFCHANGE)) + goto err; + + if (sgiadmin.block_invalid_rx_enable) + if (nla_put_flag(rep_skb, TSN_QCI_SGI_ATTR_IRXEN)) + goto err; + + if (sgiadmin.block_invalid_rx) + if (nla_put_flag(rep_skb, TSN_QCI_SGI_ATTR_IRX)) + goto err; + + if (sgiadmin.block_octets_exceeded_enable) + if (nla_put_flag(rep_skb, TSN_QCI_SGI_ATTR_OEXEN)) + goto err; + + if (sgiadmin.block_octets_exceeded) + if (nla_put_flag(rep_skb, TSN_QCI_SGI_ATTR_OEX)) + goto err; + + /* Administration */ + adminattr = nla_nest_start_noflag(rep_skb, TSN_QCI_SGI_ATTR_ADMINENTRY); + if (!adminattr) + goto err; + + if (sgiadmin.admin.gate_states) + if (nla_put_flag(rep_skb, TSN_SGI_ATTR_CTRL_INITSTATE)) + goto err; + + if (nla_put_u32(rep_skb, TSN_SGI_ATTR_CTRL_CYTIME, + sgiadmin.admin.cycle_time) || + nla_put_u32(rep_skb, TSN_SGI_ATTR_CTRL_CYTIMEEX, + sgiadmin.admin.cycle_time_extension) || + NLA_PUT_U64(rep_skb, TSN_SGI_ATTR_CTRL_BTIME, + sgiadmin.admin.base_time) || + nla_put_u8(rep_skb, TSN_SGI_ATTR_CTRL_INITIPV, + sgiadmin.admin.init_ipv)) + goto err; + + listcount = sgiadmin.admin.control_list_length; + if (!listcount) + goto out1; + + if (!sgiadmin.admin.gcl) { + pr_err("error: no gate control list\n"); + ret = -EINVAL; + goto err; + } + + gcl = sgiadmin.admin.gcl; + + /* loop list */ + for (i = 0; i < listcount; i++) { + s8 ipv; + u32 ti, omax; + + if (!(gcl + i)) { + pr_err("error: list count too big\n"); + ret = -EINVAL; + kfree(sgiadmin.admin.gcl); + goto err; + } + + /* Adminastration entry */ + sglattr = nla_nest_start_noflag(rep_skb, + TSN_SGI_ATTR_CTRL_GCLENTRY); + if (!sglattr) + goto err; + ipv = (gcl + i)->ipv; + ti = (gcl + i)->time_interval; + omax = (gcl + i)->octet_max; + + if ((gcl + i)->gate_state) + if (nla_put_flag(rep_skb, TSN_SGI_ATTR_GCL_GATESTATE)) + goto err; + + if (nla_put_s8(rep_skb, TSN_SGI_ATTR_GCL_IPV, ipv) || + nla_put_u32(rep_skb, TSN_SGI_ATTR_GCL_INTERVAL, ti) || + nla_put_u32(rep_skb, TSN_SGI_ATTR_GCL_OCTMAX, omax)) + goto err; + + /* End administration entry */ + nla_nest_end(rep_skb, sglattr); + } + + kfree(sgiadmin.admin.gcl); + if (nla_put_u8(rep_skb, TSN_SGI_ATTR_CTRL_LEN, listcount)) + goto err; + +out1: + /* End adminastration */ + nla_nest_end(rep_skb, adminattr); + + nla_nest_end(rep_skb, sgiattr); + + return tsn_send_reply(rep_skb, info); +err: + nlmsg_free(rep_skb); + tsn_simple_reply(info, TSN_CMD_REPLY, netdev->name, ret); + return ret; +} + +static int tsn_qci_sgi_get(struct sk_buff *skb, struct genl_info *info) +{ + if (info->attrs[TSN_ATTR_IFNAME]) { + cmd_qci_sgi_get(info); + return 0; + } + + return -1; +} + +static int cmd_qci_sgi_status_get(struct genl_info *info) +{ + struct nlattr *na, *sgiattr, *operattr, *sglattr; + struct nlattr *sgi[TSN_QCI_SGI_ATTR_MAX + 1]; + struct sk_buff *rep_skb; + int ret; + struct net_device *netdev; + struct genlmsghdr *genlhdr; + struct tsn_psfp_sgi_status sgistat; + struct tsn_qci_psfp_gcl *gcl = NULL; + const struct tsn_ops *tsnops; + u16 sgi_handle; + u8 listcount; + int valid, i; + struct tsn_port *port; + + port = tsn_init_check(info, &netdev); + if (!port) + return -ENODEV; + + tsnops = port->tsnops; + + if (!info->attrs[TSN_ATTR_QCI_SGI]) { + tsn_simple_reply(info, TSN_CMD_REPLY, + netdev->name, -EINVAL); + pr_err("tsn: no sgi handle input\n"); + return -EINVAL; + } + + na = info->attrs[TSN_ATTR_QCI_SGI]; + + ret = NLA_PARSE_NESTED(sgi, TSN_QCI_SGI_ATTR_MAX, + na, qci_sgi_policy); + if (ret) + return -EINVAL; + + if (!sgi[TSN_QCI_SGI_ATTR_INDEX]) { + tsn_simple_reply(info, TSN_CMD_REPLY, + netdev->name, -EINVAL); + pr_err("tsn: no sgi handle input\n"); + return -EINVAL; + } + + sgi_handle = nla_get_u32(sgi[TSN_QCI_SGI_ATTR_INDEX]); + + /* Get status data from device */ + genlhdr = info->genlhdr; + + memset(&sgistat, 0, sizeof(struct tsn_psfp_sgi_status)); + + if (!tsnops->qci_sgi_status_get) { + tsn_simple_reply(info, TSN_CMD_REPLY, + netdev->name, -EPERM); + return -1; + } + + valid = tsnops->qci_sgi_status_get(netdev, sgi_handle, &sgistat); + if (valid < 0) { + tsn_simple_reply(info, TSN_CMD_REPLY, + netdev->name, valid); + return valid; + } + + /* Form netlink reply data */ + ret = tsn_prepare_reply(info, genlhdr->cmd, + &rep_skb, NLMSG_ALIGN(MAX_ATTR_SIZE)); + if (ret < 0) + return ret; + + if (nla_put_string(rep_skb, TSN_ATTR_IFNAME, netdev->name)) + goto err; + + /* Down one netlink attribute level */ + sgiattr = nla_nest_start_noflag(rep_skb, TSN_ATTR_QCI_SGI); + if (!sgiattr) + goto err; + + if (nla_put_u32(rep_skb, TSN_QCI_SGI_ATTR_INDEX, sgi_handle)) + goto err; + + /* Gate enable */ + if (valid == 1) { + if (nla_put_flag(rep_skb, TSN_QCI_SGI_ATTR_ENABLE)) + goto err; + } else { + if (nla_put_flag(rep_skb, TSN_QCI_SGI_ATTR_DISABLE)) + goto err; + } + + if (nla_put_u32(rep_skb, TSN_QCI_SGI_ATTR_TICKG, + sgistat.tick_granularity) || + NLA_PUT_U64(rep_skb, TSN_QCI_SGI_ATTR_CCTIME, + sgistat.config_change_time) || + NLA_PUT_U64(rep_skb, TSN_QCI_SGI_ATTR_CUTIME, + sgistat.current_time) || + NLA_PUT_U64(rep_skb, TSN_QCI_SGI_ATTR_CCERROR, + sgistat.config_change_error)) + goto err; + + if (sgistat.config_pending) + if (nla_put_flag(rep_skb, TSN_QCI_SGI_ATTR_CPENDING)) + goto err; + + /* operation data */ + operattr = nla_nest_start_noflag(rep_skb, TSN_QCI_SGI_ATTR_OPERENTRY); + if (!operattr) + goto err; + + if (sgistat.oper.gate_states) + if (nla_put_flag(rep_skb, TSN_SGI_ATTR_CTRL_INITSTATE)) + goto err; + + if (nla_put_u32(rep_skb, TSN_SGI_ATTR_CTRL_CYTIME, + sgistat.oper.cycle_time) || + nla_put_u32(rep_skb, TSN_SGI_ATTR_CTRL_CYTIMEEX, + sgistat.oper.cycle_time_extension) || + NLA_PUT_U64(rep_skb, TSN_SGI_ATTR_CTRL_BTIME, + sgistat.oper.base_time) || + nla_put_u8(rep_skb, TSN_SGI_ATTR_CTRL_INITIPV, + sgistat.oper.init_ipv)) + goto err; + + /* Loop list */ + listcount = sgistat.oper.control_list_length; + if (!listcount) + goto out1; + + if (!sgistat.oper.gcl) { + pr_err("error: list lenghth is not zero!\n"); + ret = -EINVAL; + goto err; + } + + gcl = sgistat.oper.gcl; + + /* loop list */ + for (i = 0; i < listcount; i++) { + s8 ipv; + u32 ti, omax; + + if (!(gcl + i)) { + pr_err("error: list count too big\n"); + ret = -EINVAL; + kfree(sgistat.oper.gcl); + goto err; + } + + /* Operation entry */ + sglattr = nla_nest_start_noflag(rep_skb, + TSN_SGI_ATTR_CTRL_GCLENTRY); + if (!sglattr) + goto err; + ipv = (gcl + i)->ipv; + ti = (gcl + i)->time_interval; + omax = (gcl + i)->octet_max; + + if ((gcl + i)->gate_state) + if (nla_put_flag(rep_skb, TSN_SGI_ATTR_GCL_GATESTATE)) + goto err; + + if (nla_put_s8(rep_skb, TSN_SGI_ATTR_GCL_IPV, ipv) || + nla_put_u32(rep_skb, TSN_SGI_ATTR_GCL_INTERVAL, ti) || + nla_put_u32(rep_skb, TSN_SGI_ATTR_GCL_OCTMAX, omax)) + goto err; + + /* End operation entry */ + nla_nest_end(rep_skb, sglattr); + } + + kfree(sgistat.oper.gcl); + if (nla_put_u8(rep_skb, TSN_SGI_ATTR_CTRL_LEN, listcount)) + goto err; +out1: + /* End operation */ + nla_nest_end(rep_skb, operattr); + + nla_nest_end(rep_skb, sgiattr); + + return tsn_send_reply(rep_skb, info); +err: + nlmsg_free(rep_skb); + tsn_simple_reply(info, TSN_CMD_REPLY, netdev->name, ret); + return ret; +} + +static int tsn_qci_sgi_status_get(struct sk_buff *skb, struct genl_info *info) +{ + if (info->attrs[TSN_ATTR_IFNAME]) { + cmd_qci_sgi_status_get(info); + return 0; + } + + return -1; +} + +static int cmd_qci_fmi_set(struct genl_info *info) +{ + struct nlattr *na, *fmi[TSN_QCI_FMI_ATTR_MAX + 1]; + u32 index; + int ret; + struct net_device *netdev; + struct tsn_qci_psfp_fmi fmiconf; + const struct tsn_ops *tsnops; + bool enable = 0; + struct tsn_port *port; + + port = tsn_init_check(info, &netdev); + if (!port) + return -ENODEV; + + tsnops = port->tsnops; + + memset(&fmiconf, 0, sizeof(struct tsn_qci_psfp_fmi)); + + if (!info->attrs[TSN_ATTR_QCI_FMI]) + return -EINVAL; + + na = info->attrs[TSN_ATTR_QCI_FMI]; + + ret = NLA_PARSE_NESTED(fmi, TSN_QCI_FMI_ATTR_MAX, na, qci_fmi_policy); + if (ret) { + pr_info("tsn: parse value TSN_QCI_FMI_ATTR_MAX error."); + return -EINVAL; + } + + if (!fmi[TSN_QCI_FMI_ATTR_INDEX]) + return -EINVAL; + + index = nla_get_u32(fmi[TSN_QCI_FMI_ATTR_INDEX]); + + if (fmi[TSN_QCI_FMI_ATTR_DISABLE]) + goto loaddev; + + enable = 1; + + if (fmi[TSN_QCI_FMI_ATTR_CIR]) + fmiconf.cir = nla_get_u32(fmi[TSN_QCI_FMI_ATTR_CIR]); + + if (fmi[TSN_QCI_FMI_ATTR_CBS]) + fmiconf.cbs = nla_get_u32(fmi[TSN_QCI_FMI_ATTR_CBS]); + + if (fmi[TSN_QCI_FMI_ATTR_EIR]) + fmiconf.eir = nla_get_u32(fmi[TSN_QCI_FMI_ATTR_EIR]); + + if (fmi[TSN_QCI_FMI_ATTR_EBS]) + fmiconf.ebs = nla_get_u32(fmi[TSN_QCI_FMI_ATTR_EBS]); + + if (fmi[TSN_QCI_FMI_ATTR_CF]) + fmiconf.cf = 1; + + if (fmi[TSN_QCI_FMI_ATTR_CM]) + fmiconf.cm = 1; + + if (fmi[TSN_QCI_FMI_ATTR_DROPYL]) + fmiconf.drop_on_yellow = 1; + + if (fmi[TSN_QCI_FMI_ATTR_MAREDEN]) + fmiconf.mark_red_enable = 1; + + if (fmi[TSN_QCI_FMI_ATTR_MARED]) + fmiconf.mark_red = 1; + +loaddev: + + if (!tsnops->qci_fmi_set) { + tsn_simple_reply(info, TSN_CMD_REPLY, + netdev->name, -EPERM); + return -EINVAL; + } + + ret = tsnops->qci_fmi_set(netdev, index, enable, &fmiconf); + if (ret < 0) { + tsn_simple_reply(info, TSN_CMD_REPLY, netdev->name, ret); + return ret; + } + + ret = tsn_simple_reply(info, TSN_CMD_REPLY, netdev->name, 0); + + if (ret) + return ret; + return 0; +} + +static int tsn_qci_fmi_set(struct sk_buff *skb, struct genl_info *info) +{ + if (info->attrs[TSN_ATTR_IFNAME]) { + cmd_qci_fmi_set(info); + return 0; + } + + return -1; +} + +static int cmd_qci_fmi_get(struct genl_info *info) +{ + struct nlattr *na, *fmi[TSN_QCI_FMI_ATTR_MAX + 1], *fmiattr; + u32 index; + struct sk_buff *rep_skb; + int ret; + struct net_device *netdev; + struct tsn_qci_psfp_fmi fmiconf; + struct tsn_qci_psfp_fmi_counters counters; + const struct tsn_ops *tsnops; + struct genlmsghdr *genlhdr; + struct tsn_port *port; + + port = tsn_init_check(info, &netdev); + if (!port) + return -ENODEV; + + tsnops = port->tsnops; + + if (!info->attrs[TSN_ATTR_QCI_FMI]) + return -EINVAL; + + na = info->attrs[TSN_ATTR_QCI_FMI]; + + ret = NLA_PARSE_NESTED(fmi, TSN_QCI_FMI_ATTR_MAX, + na, qci_fmi_policy); + if (ret) { + pr_info("tsn: parse value TSN_QCI_FMI_ATTR_MAX error."); + return -EINVAL; + } + + if (!fmi[TSN_QCI_FMI_ATTR_INDEX]) + return -EINVAL; + + index = nla_get_u32(fmi[TSN_QCI_FMI_ATTR_INDEX]); + + /* Get data from device */ + memset(&fmiconf, 0, sizeof(struct tsn_qci_psfp_fmi)); + memset(&counters, 0, sizeof(struct tsn_qci_psfp_fmi_counters)); + + if (!tsnops->qci_fmi_get) { + tsn_simple_reply(info, TSN_CMD_REPLY, + netdev->name, -EPERM); + return -EINVAL; + } + + ret = tsnops->qci_fmi_get(netdev, index, &fmiconf, &counters); + if (ret < 0) { + tsn_simple_reply(info, TSN_CMD_REPLY, netdev->name, ret); + return ret; + } + + genlhdr = info->genlhdr; + + /* Form netlink reply data */ + ret = tsn_prepare_reply(info, genlhdr->cmd, + &rep_skb, NLMSG_ALIGN(MAX_ATTR_SIZE)); + if (ret < 0) + return ret; + + if (nla_put_string(rep_skb, TSN_ATTR_IFNAME, netdev->name)) + goto err; + + fmiattr = nla_nest_start_noflag(rep_skb, TSN_ATTR_QCI_FMI); + if (!fmiattr) + goto err; + + if (nla_put_u32(rep_skb, TSN_QCI_FMI_ATTR_INDEX, index) || + nla_put_u32(rep_skb, TSN_QCI_FMI_ATTR_CIR, fmiconf.cir) || + nla_put_u32(rep_skb, TSN_QCI_FMI_ATTR_CBS, fmiconf.cbs) || + nla_put_u32(rep_skb, TSN_QCI_FMI_ATTR_EIR, fmiconf.eir) || + nla_put_u32(rep_skb, TSN_QCI_FMI_ATTR_EBS, fmiconf.ebs)) + goto err; + + if (fmiconf.cf) + if (nla_put_flag(rep_skb, TSN_QCI_FMI_ATTR_CF)) + goto err; + + if (fmiconf.cm) + if (nla_put_flag(rep_skb, TSN_QCI_FMI_ATTR_CM)) + goto err; + + if (fmiconf.drop_on_yellow) + if (nla_put_flag(rep_skb, TSN_QCI_FMI_ATTR_DROPYL)) + goto err; + + if (fmiconf.mark_red_enable) + if (nla_put_flag(rep_skb, TSN_QCI_FMI_ATTR_MAREDEN)) + goto err; + + if (fmiconf.mark_red) + if (nla_put_flag(rep_skb, TSN_QCI_FMI_ATTR_MAREDEN)) + goto err; + + if (nla_put(rep_skb, TSN_QCI_FMI_ATTR_COUNTERS, + sizeof(struct tsn_qci_psfp_fmi_counters), &counters)) + goto err; + + nla_nest_end(rep_skb, fmiattr); + + tsn_send_reply(rep_skb, info); + + return 0; +err: + nlmsg_free(rep_skb); + tsn_simple_reply(info, TSN_CMD_REPLY, netdev->name, ret); + return ret; +} + +static int tsn_qci_fmi_get(struct sk_buff *skb, struct genl_info *info) +{ + if (info->attrs[TSN_ATTR_IFNAME]) { + cmd_qci_fmi_get(info); + return 0; + } + + return -1; +} + +static int cmd_qbv_set(struct genl_info *info) +{ + struct nlattr *na, *na1; + struct nlattr *qbv_table; + struct nlattr *qbv[TSN_QBV_ATTR_MAX + 1]; + struct nlattr *qbvctrl[TSN_QBV_ATTR_CTRL_MAX + 1]; + int rem; + int ret = 0; + struct net_device *netdev; + struct tsn_qbv_conf qbvconfig; + const struct tsn_ops *tsnops; + struct tsn_qbv_entry *gatelist = NULL; + int count = 0; + struct tsn_port *port; + + port = tsn_init_check(info, &netdev); + if (!port) + return -ENODEV; + + tsnops = port->tsnops; + + memset(&qbvconfig, 0, sizeof(struct tsn_qbv_conf)); + + if (!info->attrs[TSN_ATTR_QBV]) + return -EINVAL; + + na = info->attrs[TSN_ATTR_QBV]; + + ret = NLA_PARSE_NESTED(qbv, TSN_QBV_ATTR_MAX, na, qbv_policy); + if (ret) + return -EINVAL; + + if (qbv[TSN_QBV_ATTR_ENABLE]) + qbvconfig.gate_enabled = 1; + else + goto setdrive; + + if (qbv[TSN_QBV_ATTR_CONFIGCHANGE]) + qbvconfig.config_change = 1; + + if (!qbv[TSN_QBV_ATTR_ADMINENTRY]) { + tsn_simple_reply(info, TSN_CMD_REPLY, + netdev->name, -EINVAL); + return -1; + } + + na1 = qbv[TSN_QBV_ATTR_ADMINENTRY]; + ret = NLA_PARSE_NESTED(qbvctrl, TSN_QBV_ATTR_CTRL_MAX, + na1, qbv_ctrl_policy); + if (ret) + return -EINVAL; + + if (qbvctrl[TSN_QBV_ATTR_CTRL_CYCLETIME]) { + qbvconfig.admin.cycle_time = + nla_get_u32(qbvctrl[TSN_QBV_ATTR_CTRL_CYCLETIME]); + } + + if (qbvctrl[TSN_QBV_ATTR_CTRL_CYCLETIMEEXT]) { + qbvconfig.admin.cycle_time_extension = + nla_get_u32(qbvctrl[TSN_QBV_ATTR_CTRL_CYCLETIMEEXT]); + } + + if (qbvctrl[TSN_QBV_ATTR_CTRL_BASETIME]) { + qbvconfig.admin.base_time = + nla_get_u64(qbvctrl[TSN_QBV_ATTR_CTRL_BASETIME]); + } + + if (qbvctrl[TSN_QBV_ATTR_CTRL_GATESTATE]) { + qbvconfig.admin.gate_states = + nla_get_u8(qbvctrl[TSN_QBV_ATTR_CTRL_GATESTATE]); + } + + if (qbvctrl[TSN_QBV_ATTR_CTRL_LISTCOUNT]) { + int listcount; + + listcount = nla_get_u32(qbvctrl[TSN_QBV_ATTR_CTRL_LISTCOUNT]); + + qbvconfig.admin.control_list_length = listcount; + + gatelist = kmalloc_array(listcount, + sizeof(*gatelist), + GFP_KERNEL); + + nla_for_each_nested(qbv_table, na1, rem) { + struct nlattr *qbv_entry[TSN_QBV_ATTR_ENTRY_MAX + 1]; + + if (nla_type(qbv_table) != TSN_QBV_ATTR_CTRL_LISTENTRY) + continue; + + ret = NLA_PARSE_NESTED(qbv_entry, + TSN_QBV_ATTR_ENTRY_MAX, + qbv_table, qbv_entry_policy); + if (ret) + return -EINVAL; + + (gatelist + count)->gate_state = + nla_get_u8(qbv_entry[TSN_QBV_ATTR_ENTRY_GC]); + (gatelist + count)->time_interval = + nla_get_u32(qbv_entry[TSN_QBV_ATTR_ENTRY_TM]); + count++; + if (count > listcount) + break; + } + } + + if (gatelist) + qbvconfig.admin.control_list = gatelist; + +setdrive: + if (!tsnops->qbv_set) { + tsn_simple_reply(info, TSN_CMD_REPLY, + netdev->name, -EPERM); + goto err; + } + + ret = tsnops->qbv_set(netdev, &qbvconfig); + + /* send back */ + if (ret < 0) + tsn_simple_reply(info, TSN_CMD_REPLY, + netdev->name, ret); + else + tsn_simple_reply(info, TSN_CMD_REPLY, + netdev->name, 0); + +err: + kfree(gatelist); + return ret; +} + +static int tsn_qbv_set(struct sk_buff *skb, struct genl_info *info) +{ + if (info->attrs[TSN_ATTR_IFNAME]) { + cmd_qbv_set(info); + return 0; + } + + return -1; +} + +static int cmd_qbv_get(struct genl_info *info) +{ + struct nlattr *qbv, *qbvadminattr; + struct sk_buff *rep_skb; + int ret; + int len = 0, i = 0; + struct net_device *netdev; + struct genlmsghdr *genlhdr; + struct tsn_qbv_conf qbvconf; + const struct tsn_ops *tsnops; + struct tsn_port *port; + + port = tsn_init_check(info, &netdev); + if (!port) + return -ENODEV; + + tsnops = port->tsnops; + + genlhdr = info->genlhdr; + + memset(&qbvconf, 0, sizeof(struct tsn_qbv_conf)); + + if (!tsnops->qbv_get) { + tsn_simple_reply(info, TSN_CMD_REPLY, + netdev->name, -EPERM); + return -1; + } + + ret = tsnops->qbv_get(netdev, &qbvconf); + if (ret < 0) { + tsn_simple_reply(info, TSN_CMD_REPLY, + netdev->name, ret); + return ret; + } + + ret = tsn_prepare_reply(info, genlhdr->cmd, + &rep_skb, NLMSG_ALIGN(MAX_ATTR_SIZE)); + if (ret < 0) + return ret; + + if (nla_put_string(rep_skb, TSN_ATTR_IFNAME, netdev->name)) + goto err; + + qbv = nla_nest_start_noflag(rep_skb, TSN_ATTR_QBV); + if (!qbv) + goto err; + + qbvadminattr = nla_nest_start_noflag(rep_skb, TSN_QBV_ATTR_ADMINENTRY); + if (!qbvadminattr) + goto err; + + if (qbvconf.admin.control_list) { + len = qbvconf.admin.control_list_length; + if (nla_put_u32(rep_skb, TSN_QBV_ATTR_CTRL_LISTCOUNT, len)) + goto err; + + for (i = 0; i < len; i++) { + struct nlattr *qbv_table; + u8 gs; + u32 tp; + int glisttype = TSN_QBV_ATTR_CTRL_LISTENTRY; + + gs = (qbvconf.admin.control_list + i)->gate_state; + tp = (qbvconf.admin.control_list + i)->time_interval; + + qbv_table = + nla_nest_start_noflag(rep_skb, glisttype); + if (!qbv_table) + goto err; + + if (nla_put_u32(rep_skb, TSN_QBV_ATTR_ENTRY_ID, i) || + nla_put_u8(rep_skb, TSN_QBV_ATTR_ENTRY_GC, gs) || + nla_put_u32(rep_skb, TSN_QBV_ATTR_ENTRY_TM, tp)) + goto err; + nla_nest_end(rep_skb, qbv_table); + } + + if (qbvconf.admin.gate_states) + if (nla_put_u8(rep_skb, TSN_QBV_ATTR_CTRL_GATESTATE, + qbvconf.admin.gate_states)) + goto err; + + if (qbvconf.admin.cycle_time) + if (nla_put_u32(rep_skb, TSN_QBV_ATTR_CTRL_CYCLETIME, + qbvconf.admin.cycle_time)) + goto err; + + if (qbvconf.admin.cycle_time_extension) + if (nla_put_u32(rep_skb, TSN_QBV_ATTR_CTRL_CYCLETIMEEXT, + qbvconf.admin.cycle_time_extension)) + goto err; + + if (qbvconf.admin.base_time) + if (NLA_PUT_U64(rep_skb, TSN_QBV_ATTR_CTRL_BASETIME, + qbvconf.admin.base_time)) + goto err; + + kfree(qbvconf.admin.control_list); + + } else { + pr_info("tsn: error get administrator data."); + } + + nla_nest_end(rep_skb, qbvadminattr); + + if (qbvconf.gate_enabled) { + if (nla_put_flag(rep_skb, TSN_QBV_ATTR_ENABLE)) + goto err; + } else { + if (nla_put_flag(rep_skb, TSN_QBV_ATTR_DISABLE)) + goto err; + } + + if (qbvconf.maxsdu) + if (nla_put_u32(rep_skb, TSN_QBV_ATTR_MAXSDU, qbvconf.maxsdu)) + goto err; + + if (qbvconf.config_change) + if (nla_put_flag(rep_skb, TSN_QBV_ATTR_CONFIGCHANGE)) + goto err; + + nla_nest_end(rep_skb, qbv); + + tsn_send_reply(rep_skb, info); + + return ret; +err: + nlmsg_free(rep_skb); + tsn_simple_reply(info, TSN_CMD_REPLY, netdev->name, ret); + return ret; +} + +static int cmd_qbv_status_get(struct genl_info *info) +{ + struct nlattr *qbv, *qbvoperattr; + struct sk_buff *rep_skb; + int ret; + int len = 0, i = 0; + struct net_device *netdev; + struct genlmsghdr *genlhdr; + struct tsn_qbv_status qbvstatus; + const struct tsn_ops *tsnops; + struct tsn_port *port; + + port = tsn_init_check(info, &netdev); + if (!port) + return -ENODEV; + + tsnops = port->tsnops; + + genlhdr = info->genlhdr; + + memset(&qbvstatus, 0, sizeof(struct tsn_qbv_status)); + + if (!tsnops->qbv_get_status) { + tsn_simple_reply(info, TSN_CMD_REPLY, + netdev->name, -EPERM); + return -1; + } + + ret = tsnops->qbv_get_status(netdev, &qbvstatus); + if (ret < 0) { + tsn_simple_reply(info, TSN_CMD_REPLY, + netdev->name, ret); + return ret; + } + + ret = tsn_prepare_reply(info, genlhdr->cmd, + &rep_skb, NLMSG_ALIGN(MAX_ATTR_SIZE)); + if (ret < 0) + return ret; + + if (nla_put_string(rep_skb, TSN_ATTR_IFNAME, netdev->name)) + goto err; + + qbv = nla_nest_start_noflag(rep_skb, TSN_ATTR_QBV); + if (!qbv) + goto err; + + qbvoperattr = nla_nest_start_noflag(rep_skb, TSN_QBV_ATTR_OPERENTRY); + if (!qbvoperattr) + goto err; + + if (qbvstatus.oper.control_list) { + len = qbvstatus.oper.control_list_length; + if (nla_put_u32(rep_skb, TSN_QBV_ATTR_CTRL_LISTCOUNT, len)) { + nla_nest_cancel(rep_skb, qbvoperattr); + goto err; + } + + for (i = 0; i < len; i++) { + struct nlattr *qbv_table; + u8 gs; + u32 tp; + int glisttype = TSN_QBV_ATTR_CTRL_LISTENTRY; + + gs = (qbvstatus.oper.control_list + i)->gate_state; + tp = (qbvstatus.oper.control_list + i)->time_interval; + + qbv_table = nla_nest_start_noflag(rep_skb, glisttype); + if (!qbv_table) + goto err; + + if (nla_put_u32(rep_skb, TSN_QBV_ATTR_ENTRY_ID, i) || + nla_put_u8(rep_skb, TSN_QBV_ATTR_ENTRY_GC, gs) || + nla_put_u32(rep_skb, TSN_QBV_ATTR_ENTRY_TM, tp)) { + nla_nest_cancel(rep_skb, qbv_table); + goto err; + } + + nla_nest_end(rep_skb, qbv_table); + } + + if (qbvstatus.oper.gate_states) { + if (nla_put_u8(rep_skb, TSN_QBV_ATTR_CTRL_GATESTATE, + qbvstatus.oper.gate_states)) + goto err; + } + + if (qbvstatus.oper.cycle_time) { + if (nla_put_u32(rep_skb, TSN_QBV_ATTR_CTRL_CYCLETIME, + qbvstatus.oper.cycle_time)) + goto err; + } + + if (qbvstatus.oper.cycle_time_extension) { + if (nla_put_u32(rep_skb, TSN_QBV_ATTR_CTRL_CYCLETIMEEXT, + qbvstatus.oper.cycle_time_extension)) + goto err; + } + + if (qbvstatus.oper.base_time) { + if (NLA_PUT_U64(rep_skb, TSN_QBV_ATTR_CTRL_BASETIME, + qbvstatus.oper.base_time)) + goto err; + } + + kfree(qbvstatus.oper.control_list); + } else { + pr_info("tsn: error get operation list data."); + } + + nla_nest_end(rep_skb, qbvoperattr); + + if (qbvstatus.config_change_time) { + if (NLA_PUT_U64(rep_skb, TSN_QBV_ATTR_CONFIGCHANGETIME, + qbvstatus.config_change_time)) + goto err; + } + + if (qbvstatus.tick_granularity) { + if (nla_put_u32(rep_skb, TSN_QBV_ATTR_GRANULARITY, + qbvstatus.tick_granularity)) + goto err; + } + + if (qbvstatus.current_time) { + if (NLA_PUT_U64(rep_skb, TSN_QBV_ATTR_CURRENTTIME, + qbvstatus.current_time)) + goto err; + } + + if (qbvstatus.config_pending) { + if (nla_put_flag(rep_skb, TSN_QBV_ATTR_CONFIGPENDING)) + goto err; + } + + if (qbvstatus.config_change_error) { + if (NLA_PUT_U64(rep_skb, TSN_QBV_ATTR_CONFIGCHANGEERROR, + qbvstatus.config_change_error)) + goto err; + } + + if (qbvstatus.supported_list_max) { + if (nla_put_u32(rep_skb, TSN_QBV_ATTR_LISTMAX, + qbvstatus.supported_list_max)) + goto err; + } + + nla_nest_end(rep_skb, qbv); + + tsn_send_reply(rep_skb, info); + + return ret; +err: + nlmsg_free(rep_skb); + tsn_simple_reply(info, TSN_CMD_REPLY, netdev->name, ret); + return ret; +} + +static int tsn_qbv_status_get(struct sk_buff *skb, struct genl_info *info) +{ + if (info->attrs[TSN_ATTR_IFNAME]) + cmd_qbv_status_get(info); + + return 0; +} + +static int tsn_qbv_get(struct sk_buff *skb, struct genl_info *info) +{ + if (info->attrs[TSN_ATTR_IFNAME]) + cmd_qbv_get(info); + + return 0; +} + +static int tsn_cbs_set(struct sk_buff *skb, struct genl_info *info) +{ + struct nlattr *na; + struct nlattr *cbsa[TSN_CBS_ATTR_MAX + 1]; + struct net_device *netdev; + const struct tsn_ops *tsnops; + int ret; + u8 tc, bw; + struct tsn_port *port; + + port = tsn_init_check(info, &netdev); + if (!port) + return -ENODEV; + + tsnops = port->tsnops; + + if (!info->attrs[TSN_ATTR_CBS]) { + tsn_simple_reply(info, TSN_CMD_REPLY, + netdev->name, -EINVAL); + return -EINVAL; + } + + na = info->attrs[TSN_ATTR_CBS]; + + if (!tsnops->cbs_set) { + tsn_simple_reply(info, TSN_CMD_REPLY, + netdev->name, -EPERM); + return -1; + } + + ret = NLA_PARSE_NESTED(cbsa, TSN_CBS_ATTR_MAX, na, cbs_policy); + if (ret) { + tsn_simple_reply(info, TSN_CMD_REPLY, + netdev->name, -EINVAL); + return -EINVAL; + } + + if (!cbsa[TSN_CBS_ATTR_TC_INDEX]) { + pr_err("tsn: no TSN_CBS_ATTR_TC_INDEX input\n"); + tsn_simple_reply(info, TSN_CMD_REPLY, + netdev->name, -EINVAL); + return -EINVAL; + } + tc = nla_get_u8(cbsa[TSN_CBS_ATTR_TC_INDEX]); + + if (!cbsa[TSN_CBS_ATTR_BW]) { + pr_err("tsn: no TSN_CBS_ATTR_BW input\n"); + tsn_simple_reply(info, TSN_CMD_REPLY, + netdev->name, -EINVAL); + return -EINVAL; + } + + bw = nla_get_u8(cbsa[TSN_CBS_ATTR_BW]); + if (bw > 100) { + pr_err("tsn: TSN_CBS_ATTR_BW isn't in the range of 0~100\n"); + tsn_simple_reply(info, TSN_CMD_REPLY, + netdev->name, -EINVAL); + return -EINVAL; + } + + ret = tsnops->cbs_set(netdev, tc, bw); + if (ret < 0) { + tsn_simple_reply(info, TSN_CMD_REPLY, + netdev->name, ret); + return ret; + } + + tsn_simple_reply(info, TSN_CMD_REPLY, + netdev->name, 0); + return 0; +} + +static int tsn_cbs_get(struct sk_buff *skb, struct genl_info *info) +{ + struct nlattr *na, *cbsattr; + struct nlattr *cbsa[TSN_CBS_ATTR_MAX + 1]; + struct net_device *netdev; + const struct tsn_ops *tsnops; + struct sk_buff *rep_skb; + int ret; + struct genlmsghdr *genlhdr; + u8 tc; + struct tsn_port *port; + + port = tsn_init_check(info, &netdev); + if (!port) + return -ENODEV; + + tsnops = port->tsnops; + + if (!info->attrs[TSN_ATTR_CBS]) { + tsn_simple_reply(info, TSN_CMD_REPLY, + netdev->name, -EINVAL); + return -EINVAL; + } + + if (!tsnops->cbs_get) { + tsn_simple_reply(info, TSN_CMD_REPLY, + netdev->name, -EPERM); + return -1; + } + + na = info->attrs[TSN_ATTR_CBS]; + ret = NLA_PARSE_NESTED(cbsa, TSN_CBS_ATTR_MAX, na, cbs_policy); + if (ret) { + pr_err("tsn: parse value TSN_CBS_ATTR_MAX error."); + tsn_simple_reply(info, TSN_CMD_REPLY, + netdev->name, -EINVAL); + return -EINVAL; + } + + /* Get status data from device */ + genlhdr = info->genlhdr; + + /* Form netlink reply data */ + ret = tsn_prepare_reply(info, genlhdr->cmd, &rep_skb, + NLMSG_ALIGN(MAX_ATTR_SIZE)); + if (ret < 0) + goto err; + + if (nla_put_string(rep_skb, TSN_ATTR_IFNAME, netdev->name)) + goto err; + + cbsattr = nla_nest_start_noflag(rep_skb, TSN_ATTR_CBS); + if (!cbsattr) + goto err; + + if (!cbsa[TSN_CBS_ATTR_TC_INDEX]) { + pr_err("tsn: must to specify the TSN_CBS_ATTR_TC_INDEX\n"); + ret = -EINVAL; + goto err; + } + tc = nla_get_u8(cbsa[TSN_CBS_ATTR_TC_INDEX]); + + ret = tsnops->cbs_get(netdev, tc); + if (ret < 0) { + pr_err("tsn: cbs_get return error\n"); + goto err; + } + + if (nla_put_u8(rep_skb, TSN_CBS_ATTR_BW, ret & 0XF)) + goto err; + + nla_nest_end(rep_skb, cbsattr); + return tsn_send_reply(rep_skb, info); +err: + nlmsg_free(rep_skb); + tsn_simple_reply(info, TSN_CMD_REPLY, netdev->name, ret); + return ret; +} + +static int cmd_qbu_set(struct genl_info *info) +{ + struct nlattr *na; + struct nlattr *qbua[TSN_QBU_ATTR_MAX + 1]; + struct net_device *netdev; + const struct tsn_ops *tsnops; + int ret; + u8 preemptible = 0; + struct tsn_port *port; + + port = tsn_init_check(info, &netdev); + if (!port) + return -ENODEV; + + tsnops = port->tsnops; + + if (!info->attrs[TSN_ATTR_QBU]) { + tsn_simple_reply(info, TSN_CMD_REPLY, + netdev->name, -EINVAL); + return -EINVAL; + } + + na = info->attrs[TSN_ATTR_QBU]; + + ret = NLA_PARSE_NESTED(qbua, TSN_QBU_ATTR_MAX, na, qbu_policy); + if (ret) { + tsn_simple_reply(info, TSN_CMD_REPLY, + netdev->name, -EINVAL); + return -EINVAL; + } + + if (qbua[TSN_QBU_ATTR_ADMIN_STATE]) + preemptible = nla_get_u8(qbua[TSN_QBU_ATTR_ADMIN_STATE]); + else + pr_info("No preemptible TSN_QBU_ATTR_ADMIN_STATE config!\n"); + + if (!tsnops->qbu_set) { + tsn_simple_reply(info, TSN_CMD_REPLY, + netdev->name, -EPERM); + return -EINVAL; + } + + ret = tsnops->qbu_set(netdev, preemptible); + if (ret < 0) { + tsn_simple_reply(info, TSN_CMD_REPLY, + netdev->name, ret); + return ret; + } + + tsn_simple_reply(info, TSN_CMD_REPLY, + netdev->name, 0); + return 0; +} + +static int tsn_qbu_set(struct sk_buff *skb, struct genl_info *info) +{ + if (info->attrs[TSN_ATTR_IFNAME]) + return cmd_qbu_set(info); + + return -1; +} + +static int cmd_qbu_get_status(struct genl_info *info) +{ + struct nlattr *qbuattr; + struct net_device *netdev; + const struct tsn_ops *tsnops; + struct sk_buff *rep_skb; + int ret; + struct genlmsghdr *genlhdr; + struct tsn_preempt_status pps; + struct tsn_port *port; + + port = tsn_init_check(info, &netdev); + if (!port) + return -ENODEV; + + tsnops = port->tsnops; + + /* Get status data from device */ + genlhdr = info->genlhdr; + + memset(&pps, 0, sizeof(struct tsn_preempt_status)); + + if (!tsnops->qbu_get) { + tsn_simple_reply(info, TSN_CMD_REPLY, + netdev->name, -EPERM); + return -1; + } + + ret = tsnops->qbu_get(netdev, &pps); + if (ret < 0) { + tsn_simple_reply(info, TSN_CMD_REPLY, + netdev->name, ret); + return ret; + } + + /* Form netlink reply data */ + ret = tsn_prepare_reply(info, genlhdr->cmd, + &rep_skb, NLMSG_ALIGN(MAX_ATTR_SIZE)); + if (ret < 0) + return ret; + + if (nla_put_string(rep_skb, TSN_ATTR_IFNAME, netdev->name)) + goto err; + + qbuattr = nla_nest_start_noflag(rep_skb, TSN_ATTR_QBU); + if (!qbuattr) + goto err; + + if (nla_put_u8(rep_skb, TSN_QBU_ATTR_ADMIN_STATE, pps.admin_state) || + nla_put_u32(rep_skb, + TSN_QBU_ATTR_HOLD_ADVANCE, pps.hold_advance) || + nla_put_u32(rep_skb, + TSN_QBU_ATTR_RELEASE_ADVANCE, pps.release_advance)) + goto err; + + if (pps.preemption_active) { + if (nla_put_flag(rep_skb, TSN_QBU_ATTR_ACTIVE)) + goto err; + } + + if (nla_put_u8(rep_skb, TSN_QBU_ATTR_HOLD_REQUEST, pps.hold_request)) + goto err; + + nla_nest_end(rep_skb, qbuattr); + + return tsn_send_reply(rep_skb, info); +err: + nlmsg_free(rep_skb); + tsn_simple_reply(info, TSN_CMD_REPLY, netdev->name, ret); + return ret; +} + +static int tsn_qbu_get_status(struct sk_buff *skb, struct genl_info *info) +{ + if (info->attrs[TSN_ATTR_IFNAME]) + return cmd_qbu_get_status(info); + + return -1; +} + +static int tsn_tsd_set(struct sk_buff *skb, struct genl_info *info) +{ + struct nlattr *na; + struct nlattr *ntsd[TSN_TSD_ATTR_MAX + 1]; + struct net_device *netdev; + const struct tsn_ops *tsnops; + struct tsn_tsd tsd; + int ret; + struct tsn_port *port; + + port = tsn_init_check(info, &netdev); + if (!port) + return -ENODEV; + + tsnops = port->tsnops; + + memset(&tsd, 0, sizeof(struct tsn_tsd)); + + if (!info->attrs[TSN_ATTR_TSD]) { + tsn_simple_reply(info, TSN_CMD_REPLY, + netdev->name, -EINVAL); + return -EINVAL; + } + + na = info->attrs[TSN_ATTR_TSD]; + + ret = NLA_PARSE_NESTED(ntsd, TSN_TSD_ATTR_MAX, na, tsd_policy); + if (ret) { + tsn_simple_reply(info, TSN_CMD_REPLY, + netdev->name, -EINVAL); + return -EINVAL; + } + + if (!tsnops->tsd_set) { + tsn_simple_reply(info, TSN_CMD_REPLY, + netdev->name, -EPERM); + return -EINVAL; + } + + if (nla_get_flag(ntsd[TSN_TSD_ATTR_DISABLE])) { + tsd.enable = false; + } else { + if (ntsd[TSN_TSD_ATTR_PERIOD]) + tsd.period = nla_get_u32(ntsd[TSN_TSD_ATTR_PERIOD]); + + if (!tsd.period) { + tsn_simple_reply(info, TSN_CMD_REPLY, + netdev->name, -EINVAL); + return -EINVAL; + } + + if (ntsd[TSN_TSD_ATTR_MAX_FRM_NUM]) + tsd.maxFrameNum = + nla_get_u32(ntsd[TSN_TSD_ATTR_MAX_FRM_NUM]); + + if (ntsd[TSN_TSD_ATTR_SYN_IMME]) + tsd.syn_flag = 2; + else + tsd.syn_flag = 1; + + tsd.enable = true; + } + + ret = tsnops->tsd_set(netdev, &tsd); + if (ret < 0) { + tsn_simple_reply(info, TSN_CMD_REPLY, + netdev->name, ret); + return ret; + } + + tsn_simple_reply(info, TSN_CMD_REPLY, + netdev->name, 0); + return 0; +} + +static int tsn_tsd_get(struct sk_buff *skb, struct genl_info *info) +{ + struct nlattr *na, *tsdattr; + struct nlattr *tsda[TSN_TSD_ATTR_MAX + 1]; + struct net_device *netdev; + const struct tsn_ops *tsnops; + struct sk_buff *rep_skb; + int ret; + struct genlmsghdr *genlhdr; + struct tsn_tsd_status tts; + struct tsn_port *port; + + port = tsn_init_check(info, &netdev); + if (!port) + return -ENODEV; + + tsnops = port->tsnops; + + if (!info->attrs[TSN_ATTR_TSD]) { + tsn_simple_reply(info, TSN_CMD_REPLY, + netdev->name, -EINVAL); + return -EINVAL; + } + + if (!tsnops->tsd_get) { + tsn_simple_reply(info, TSN_CMD_REPLY, + netdev->name, -EPERM); + return -1; + } + + ret = tsnops->tsd_get(netdev, &tts); + if (ret < 0) { + tsn_simple_reply(info, TSN_CMD_REPLY, + netdev->name, ret); + return ret; + } + + na = info->attrs[TSN_ATTR_TSD]; + + ret = NLA_PARSE_NESTED(tsda, TSN_TSD_ATTR_MAX, + na, tsd_policy); + if (ret) { + pr_err("tsn: parse value TSN_TSD_ATTR_MAX error."); + tsn_simple_reply(info, TSN_CMD_REPLY, + netdev->name, -EINVAL); + return -EINVAL; + } + + /* Get status data from device */ + genlhdr = info->genlhdr; + + /* Form netlink reply data */ + ret = tsn_prepare_reply(info, genlhdr->cmd, &rep_skb, + NLMSG_ALIGN(MAX_ATTR_SIZE)); + if (ret < 0) + return ret; + + if (nla_put_string(rep_skb, TSN_ATTR_IFNAME, netdev->name)) + goto err; + + tsdattr = nla_nest_start_noflag(rep_skb, TSN_ATTR_TSD); + if (!tsdattr) + goto err; + + if (nla_put_u32(rep_skb, TSN_TSD_ATTR_PERIOD, tts.period) || + nla_put_u32(rep_skb, TSN_TSD_ATTR_MAX_FRM_NUM, tts.maxFrameNum) || + nla_put_u32(rep_skb, TSN_TSD_ATTR_CYCLE_NUM, tts.cycleNum) || + nla_put_u32(rep_skb, TSN_TSD_ATTR_LOSS_STEPS, tts.loss_steps) || + nla_put_u32(rep_skb, TSN_TSD_ATTR_MAX_FRM_NUM, tts.maxFrameNum)) + goto err; + + if (!tts.enable) { + if (nla_put_flag(rep_skb, TSN_TSD_ATTR_DISABLE)) + goto err; + } else { + if (nla_put_flag(rep_skb, TSN_TSD_ATTR_ENABLE)) + goto err; + } + + if (tts.flag == 2) + if (nla_put_flag(rep_skb, TSN_TSD_ATTR_SYN_IMME)) + goto err; + + nla_nest_end(rep_skb, tsdattr); + return tsn_send_reply(rep_skb, info); +err: + nlmsg_free(rep_skb); + tsn_simple_reply(info, TSN_CMD_REPLY, netdev->name, ret); + return ret; +} + +static int tsn_ct_set(struct sk_buff *skb, struct genl_info *info) +{ + struct nlattr *na; + struct nlattr *cta[TSN_CT_ATTR_MAX + 1]; + struct net_device *netdev; + const struct tsn_ops *tsnops; + int ret; + u8 queue_stat; + struct tsn_port *port; + + port = tsn_init_check(info, &netdev); + if (!port) + return -ENODEV; + + tsnops = port->tsnops; + + if (!info->attrs[TSN_ATTR_CT]) { + tsn_simple_reply(info, TSN_CMD_REPLY, + netdev->name, -EINVAL); + return -EINVAL; + } + + na = info->attrs[TSN_ATTR_CT]; + + if (!tsnops->ct_set) { + tsn_simple_reply(info, TSN_CMD_REPLY, + netdev->name, -EPERM); + return -1; + } + + ret = NLA_PARSE_NESTED(cta, TSN_CT_ATTR_MAX, + na, ct_policy); + if (ret) { + tsn_simple_reply(info, TSN_CMD_REPLY, + netdev->name, -EINVAL); + return -EINVAL; + } + + queue_stat = nla_get_u8(cta[TSN_CT_ATTR_QUEUE_STATE]); + + ret = tsnops->ct_set(netdev, queue_stat); + if (ret < 0) { + tsn_simple_reply(info, TSN_CMD_REPLY, + netdev->name, ret); + return ret; + } + + tsn_simple_reply(info, TSN_CMD_REPLY, + netdev->name, 0); + return 0; +} + +static int tsn_cbgen_set(struct sk_buff *skb, struct genl_info *info) +{ + struct nlattr *na; + struct nlattr *cbgena[TSN_CBGEN_ATTR_MAX + 1]; + struct net_device *netdev; + const struct tsn_ops *tsnops; + int ret; + u32 index; + struct tsn_seq_gen_conf sg_conf; + struct tsn_port *port; + + port = tsn_init_check(info, &netdev); + if (!port) + return -ENODEV; + + tsnops = port->tsnops; + + if (!info->attrs[TSN_ATTR_CBGEN]) { + tsn_simple_reply(info, TSN_CMD_REPLY, + netdev->name, -EINVAL); + return -EINVAL; + } + + na = info->attrs[TSN_ATTR_CBGEN]; + + if (!tsnops->cbgen_set) { + tsn_simple_reply(info, TSN_CMD_REPLY, + netdev->name, -EPERM); + return -1; + } + + ret = NLA_PARSE_NESTED(cbgena, TSN_CBGEN_ATTR_MAX, + na, cbgen_policy); + if (ret) { + tsn_simple_reply(info, TSN_CMD_REPLY, + netdev->name, -EINVAL); + return -EINVAL; + } + + index = nla_get_u32(cbgena[TSN_CBGEN_ATTR_INDEX]); + + memset(&sg_conf, 0, sizeof(struct tsn_seq_gen_conf)); + sg_conf.iport_mask = nla_get_u8(cbgena[TSN_CBGEN_ATTR_PORT_MASK]); + sg_conf.split_mask = nla_get_u8(cbgena[TSN_CBGEN_ATTR_SPLIT_MASK]); + sg_conf.seq_len = nla_get_u8(cbgena[TSN_CBGEN_ATTR_SEQ_LEN]); + sg_conf.seq_num = nla_get_u32(cbgena[TSN_CBGEN_ATTR_SEQ_NUM]); + + ret = tsnops->cbgen_set(netdev, index, &sg_conf); + if (ret < 0) { + tsn_simple_reply(info, TSN_CMD_REPLY, + netdev->name, ret); + return ret; + } + + tsn_simple_reply(info, TSN_CMD_REPLY, + netdev->name, 0); + return 0; +} + +static int tsn_cbrec_set(struct sk_buff *skb, struct genl_info *info) +{ + struct nlattr *na; + struct nlattr *cbreca[TSN_CBREC_ATTR_MAX + 1]; + struct net_device *netdev; + const struct tsn_ops *tsnops; + int ret; + u32 index; + struct tsn_seq_rec_conf sr_conf; + struct tsn_port *port; + + port = tsn_init_check(info, &netdev); + if (!port) + return -ENODEV; + + tsnops = port->tsnops; + + if (!info->attrs[TSN_ATTR_CBREC]) { + tsn_simple_reply(info, TSN_CMD_REPLY, + netdev->name, -EINVAL); + return -EINVAL; + } + + na = info->attrs[TSN_ATTR_CBREC]; + + if (!tsnops->cbrec_set) { + tsn_simple_reply(info, TSN_CMD_REPLY, + netdev->name, -EPERM); + return -1; + } + + ret = NLA_PARSE_NESTED(cbreca, TSN_CBREC_ATTR_MAX, + na, cbrec_policy); + if (ret) { + tsn_simple_reply(info, TSN_CMD_REPLY, + netdev->name, -EINVAL); + return -EINVAL; + } + + index = nla_get_u32(cbreca[TSN_CBREC_ATTR_INDEX]); + + memset(&sr_conf, 0, sizeof(struct tsn_seq_rec_conf)); + sr_conf.seq_len = nla_get_u8(cbreca[TSN_CBREC_ATTR_SEQ_LEN]); + sr_conf.his_len = nla_get_u8(cbreca[TSN_CBREC_ATTR_HIS_LEN]); + sr_conf.rtag_pop_en = nla_get_flag(cbreca[TSN_CBREC_ATTR_TAG_POP_EN]); + + ret = tsnops->cbrec_set(netdev, index, &sr_conf); + if (ret < 0) { + tsn_simple_reply(info, TSN_CMD_REPLY, + netdev->name, ret); + return ret; + } + + tsn_simple_reply(info, TSN_CMD_REPLY, + netdev->name, 0); + return 0; +} + +static int tsn_cbstatus_get(struct sk_buff *skb, struct genl_info *info) +{ + struct nlattr *na; + struct nlattr *cba[TSN_CBSTAT_ATTR_MAX + 1]; + struct nlattr *cbattr; + struct net_device *netdev; + const struct tsn_ops *tsnops; + struct sk_buff *rep_skb; + int ret; + unsigned int index; + struct genlmsghdr *genlhdr; + struct tsn_cb_status cbstat; + struct tsn_port *port; + + port = tsn_init_check(info, &netdev); + if (!port) + return -ENODEV; + + tsnops = port->tsnops; + + /* Get status data from device */ + genlhdr = info->genlhdr; + + memset(&cbstat, 0, sizeof(struct tsn_cb_status)); + + if (!tsnops->cb_get) { + tsn_simple_reply(info, TSN_CMD_REPLY, + netdev->name, -EPERM); + return -1; + } + + na = info->attrs[TSN_ATTR_CBSTAT]; + ret = NLA_PARSE_NESTED(cba, TSN_CBSTAT_ATTR_MAX, + na, cbstat_policy); + if (ret) { + tsn_simple_reply(info, TSN_CMD_REPLY, + netdev->name, -EINVAL); + return -EINVAL; + } + + index = nla_get_u32(cba[TSN_CBSTAT_ATTR_INDEX]); + + ret = tsnops->cb_get(netdev, index, &cbstat); + if (ret < 0) { + tsn_simple_reply(info, TSN_CMD_REPLY, + netdev->name, ret); + return ret; + } + + /* Form netlink reply data */ + ret = tsn_prepare_reply(info, genlhdr->cmd, + &rep_skb, NLMSG_ALIGN(MAX_ATTR_SIZE)); + if (ret < 0) + return ret; + + if (nla_put_string(rep_skb, TSN_ATTR_IFNAME, netdev->name)) + goto err; + + cbattr = nla_nest_start_noflag(rep_skb, TSN_ATTR_CBSTAT); + if (!cbattr) + goto err; + + if (nla_put_u8(rep_skb, TSN_CBSTAT_ATTR_GEN_REC, cbstat.gen_rec) || + nla_put_u8(rep_skb, TSN_CBSTAT_ATTR_ERR, cbstat.err) || + nla_put_u32(rep_skb, TSN_CBSTAT_ATTR_SEQ_NUM, + cbstat.seq_num) || + nla_put_u8(rep_skb, TSN_CBSTAT_ATTR_SEQ_LEN, cbstat.seq_len) || + nla_put_u8(rep_skb, TSN_CBSTAT_ATTR_SPLIT_MASK, + cbstat.split_mask) || + nla_put_u8(rep_skb, TSN_CBSTAT_ATTR_PORT_MASK, + cbstat.iport_mask) || + nla_put_u8(rep_skb, TSN_CBSTAT_ATTR_HIS_LEN, cbstat.his_len) || + nla_put_u32(rep_skb, TSN_CBSTAT_ATTR_SEQ_HIS, + cbstat.seq_his)) + goto err; + + nla_nest_end(rep_skb, cbattr); + + return tsn_send_reply(rep_skb, info); +err: + nlmsg_free(rep_skb); + tsn_simple_reply(info, TSN_CMD_REPLY, netdev->name, ret); + return ret; +} + +static int tsn_dscp_set(struct sk_buff *skb, struct genl_info *info) +{ + struct nlattr *na; + struct nlattr *dscpa[TSN_DSCP_ATTR_MAX + 1]; + struct net_device *netdev; + const struct tsn_ops *tsnops; + int ret; + bool enable = 0; + struct tsn_port *port; + int dscp_ix; + struct tsn_qos_switch_dscp_conf dscp_conf; + + port = tsn_init_check(info, &netdev); + if (!port) + return -ENODEV; + + tsnops = port->tsnops; + + if (!info->attrs[TSN_ATTR_DSCP]) { + tsn_simple_reply(info, TSN_CMD_REPLY, + netdev->name, -EINVAL); + return -EINVAL; + } + + na = info->attrs[TSN_ATTR_DSCP]; + + if (!tsnops->dscp_set) { + tsn_simple_reply(info, TSN_CMD_REPLY, + netdev->name, -EPERM); + return -1; + } + + ret = NLA_PARSE_NESTED(dscpa, TSN_DSCP_ATTR_MAX, + na, dscp_policy); + if (ret) { + tsn_simple_reply(info, TSN_CMD_REPLY, + netdev->name, -EINVAL); + return -EINVAL; + } + + enable = 1; + if (dscpa[TSN_DSCP_ATTR_DISABLE]) + enable = 0; + dscp_ix = nla_get_u32(dscpa[TSN_DSCP_ATTR_INDEX]); + dscp_conf.cos = nla_get_u32(dscpa[TSN_DSCP_ATTR_COS]); + dscp_conf.dpl = nla_get_u32(dscpa[TSN_DSCP_ATTR_DPL]); + ret = tsnops->dscp_set(netdev, enable, dscp_ix, &dscp_conf); + if (ret < 0) { + tsn_simple_reply(info, TSN_CMD_REPLY, + netdev->name, ret); + return ret; + } + + tsn_simple_reply(info, TSN_CMD_REPLY, + netdev->name, 0); + + return 0; +} + +static const struct genl_ops tsnnl_ops[] = { + { + .cmd = TSN_CMD_ECHO, + .doit = tsn_echo_cmd, + .flags = GENL_ADMIN_PERM, + }, + { + .cmd = TSN_CMD_CAP_GET, + .doit = tsn_cap_get, + .flags = GENL_ADMIN_PERM, + }, + { + .cmd = TSN_CMD_QBV_SET, + .doit = tsn_qbv_set, + .flags = GENL_ADMIN_PERM, + }, + { + .cmd = TSN_CMD_QBV_GET, + .doit = tsn_qbv_get, + .flags = GENL_ADMIN_PERM, + }, + { + .cmd = TSN_CMD_QBV_GET_STATUS, + .doit = tsn_qbv_status_get, + .flags = GENL_ADMIN_PERM, + }, + { + .cmd = TSN_CMD_CB_STREAMID_SET, + .doit = tsn_cb_streamid_set, + .flags = GENL_ADMIN_PERM, + }, + { + .cmd = TSN_CMD_CB_STREAMID_GET, + .doit = tsn_cb_streamid_get, + .flags = GENL_ADMIN_PERM, + }, + { + .cmd = TSN_CMD_CB_STREAMID_GET_COUNTS, + .doit = tsn_cb_streamid_counters_get, + .flags = GENL_ADMIN_PERM, + }, + { + .cmd = TSN_CMD_QCI_CAP_GET, + .doit = tsn_qci_cap_get, + .flags = GENL_ADMIN_PERM, + }, + { + .cmd = TSN_CMD_QCI_SFI_SET, + .doit = tsn_qci_sfi_set, + .flags = GENL_ADMIN_PERM, + }, + { + .cmd = TSN_CMD_QCI_SFI_GET, + .doit = tsn_qci_sfi_get, + .flags = GENL_ADMIN_PERM, + }, + { + .cmd = TSN_CMD_QCI_SFI_GET_COUNTS, + .doit = tsn_qci_sfi_counters_get, + .flags = GENL_ADMIN_PERM, + }, + { + .cmd = TSN_CMD_QCI_SGI_SET, + .doit = tsn_qci_sgi_set, + .flags = GENL_ADMIN_PERM, + }, + { + .cmd = TSN_CMD_QCI_SGI_GET, + .doit = tsn_qci_sgi_get, + .flags = GENL_ADMIN_PERM, + }, + { + .cmd = TSN_CMD_QCI_SGI_GET_STATUS, + .doit = tsn_qci_sgi_status_get, + .flags = GENL_ADMIN_PERM, + }, + { + .cmd = TSN_CMD_QCI_FMI_SET, + .doit = tsn_qci_fmi_set, + .flags = GENL_ADMIN_PERM, + }, + { + .cmd = TSN_CMD_QCI_FMI_GET, + .doit = tsn_qci_fmi_get, + .flags = GENL_ADMIN_PERM, + }, + { + .cmd = TSN_CMD_CBS_SET, + .doit = tsn_cbs_set, + .flags = GENL_ADMIN_PERM, + }, + { + .cmd = TSN_CMD_CBS_GET, + .doit = tsn_cbs_get, + .flags = GENL_ADMIN_PERM, + }, + { + .cmd = TSN_CMD_QBU_SET, + .doit = tsn_qbu_set, + .flags = GENL_ADMIN_PERM, + }, + { + .cmd = TSN_CMD_QBU_GET_STATUS, + .doit = tsn_qbu_get_status, + .flags = GENL_ADMIN_PERM, + }, + { + .cmd = TSN_CMD_TSD_SET, + .doit = tsn_tsd_set, + .flags = GENL_ADMIN_PERM, + }, + { + .cmd = TSN_CMD_TSD_GET, + .doit = tsn_tsd_get, + .flags = GENL_ADMIN_PERM, + }, + { + .cmd = TSN_CMD_CT_SET, + .doit = tsn_ct_set, + .flags = GENL_ADMIN_PERM, + }, + { + .cmd = TSN_CMD_CBGEN_SET, + .doit = tsn_cbgen_set, + .flags = GENL_ADMIN_PERM, + }, + { + .cmd = TSN_CMD_CBREC_SET, + .doit = tsn_cbrec_set, + .flags = GENL_ADMIN_PERM, + }, + { + .cmd = TSN_CMD_CBSTAT_GET, + .doit = tsn_cbstatus_get, + .flags = GENL_ADMIN_PERM, + }, + { + .cmd = TSN_CMD_DSCP_SET, + .doit = tsn_dscp_set, + .flags = GENL_ADMIN_PERM, + }, +}; + +static const struct genl_multicast_group tsn_mcgrps[] = { + [TSN_MCGRP_QBV] = { .name = TSN_MULTICAST_GROUP_QBV}, + [TSN_MCGRP_QCI] = { .name = TSN_MULTICAST_GROUP_QCI}, +}; + +static struct genl_family tsn_family = { + .name = TSN_GENL_NAME, + .version = TSN_GENL_VERSION, + .maxattr = TSN_CMD_ATTR_MAX, + .module = THIS_MODULE, + .netnsok = true, + .ops = tsnnl_ops, + .n_ops = ARRAY_SIZE(tsnnl_ops), + .mcgrps = tsn_mcgrps, + .n_mcgrps = ARRAY_SIZE(tsn_mcgrps), +}; + +int tsn_port_register(struct net_device *netdev, + struct tsn_ops *tsnops, u16 groupid) +{ + struct tsn_port *port; + + if (list_empty(&port_list)) { + INIT_LIST_HEAD(&port_list); + } else { + list_for_each_entry(port, &port_list, list) { + if (port->netdev == netdev) { + pr_info("TSN device already registered!\n"); + return -1; + } + } + } + + port = kzalloc(sizeof(*port), GFP_KERNEL); + if (!port) + return -1; + + port->netdev = netdev; + port->groupid = groupid; + port->tsnops = tsnops; + port->nd.dev = netdev; + + if (groupid < GROUP_OFFSET_SWITCH) + port->type = TSN_ENDPOINT; + else + port->type = TSN_SWITCH; + + list_add_tail(&port->list, &port_list); + + if (tsnops && tsnops->device_init) + port->tsnops->device_init(netdev); + + return 0; +} +EXPORT_SYMBOL(tsn_port_register); + +void tsn_port_unregister(struct net_device *netdev) +{ + struct tsn_port *p; + + list_for_each_entry(p, &port_list, list) { + if (!p || !p->netdev) + continue; + if (p->netdev == netdev) { + if (p->tsnops->device_deinit) + p->tsnops->device_deinit(netdev); + list_del(&p->list); + kfree(p); + break; + } + } +} +EXPORT_SYMBOL(tsn_port_unregister); + +static int tsn_multicast_to_user(unsigned long event, + struct tsn_notifier_info *tsn_info) +{ + struct sk_buff *skb; + struct genlmsghdr *nlh = NULL; + int res = 0; + struct tsn_qbv_conf *qbvdata; + + /* If new attributes are added, please revisit this allocation */ + skb = genlmsg_new(sizeof(*tsn_info), GFP_KERNEL); + if (!skb) { + pr_err("Allocation failure.\n"); + return -ENOMEM; + } + + switch (event) { + case TSN_QBV_CONFIGCHANGETIME_ARRIVE: + nlh = genlmsg_put(skb, 0, 1, &tsn_family, + GFP_KERNEL, TSN_CMD_QBV_SET); + qbvdata = &tsn_info->ntdata.qbv_notify; + res = NLA_PUT_U64(skb, TSN_QBV_ATTR_CTRL_BASETIME, + qbvdata->admin.base_time); + + if (res) { + pr_err("put data failure!\n"); + goto done; + } + + res = nla_put_u32(skb, TSN_QBV_ATTR_CTRL_CYCLETIME, + qbvdata->admin.cycle_time); + if (res) { + pr_err("put data failure!\n"); + goto done; + } + + if (qbvdata->gate_enabled) + res = nla_put_flag(skb, TSN_QBV_ATTR_ENABLE + + TSN_QBV_ATTR_CTRL_MAX); + else + res = nla_put_flag(skb, TSN_QBV_ATTR_DISABLE + + TSN_QBV_ATTR_CTRL_MAX); + if (res) { + pr_err("put data failure!\n"); + goto done; + } + + res = nla_put_u32(skb, TSN_QBV_ATTR_CTRL_UNSPEC, + tsn_info->dev->ifindex); + if (res) { + pr_err("put data failure!\n"); + goto done; + } + + break; + default: + pr_info("event not supportted!\n"); + break; + } + + if (!nlh) + goto done; + + (void)genlmsg_end(skb, nlh); + + res = genlmsg_multicast_allns(&tsn_family, skb, 0, + TSN_MCGRP_QBV, GFP_KERNEL); + skb = NULL; + if (res && res != -ESRCH) { + pr_err("genlmsg_multicast_allns error: %d\n", res); + goto done; + } + + if (res == -ESRCH) + res = 0; + +done: + if (skb) { + nlmsg_free(skb); + skb = NULL; + } + + return res; +} + +/* called with RTNL or RCU */ +static int tsn_event(struct notifier_block *unused, + unsigned long event, void *ptr) +{ + struct tsn_notifier_info *tsn_info; + int err = NOTIFY_DONE; + + switch (event) { + case TSN_QBV_CONFIGCHANGETIME_ARRIVE: + tsn_info = ptr; + err = tsn_multicast_to_user(event, tsn_info); + if (err) { + err = notifier_from_errno(err); + break; + } + break; + default: + pr_info("event not supportted!\n"); + break; + } + + return err; +} + +static struct notifier_block tsn_notifier = { + .notifier_call = tsn_event, +}; + +static int __init tsn_genetlink_init(void) +{ + int ret; + + pr_info("tsn generic netlink module v%d init...\n", TSN_GENL_VERSION); + + ret = genl_register_family(&tsn_family); + + if (ret != 0) { + pr_info("failed to init tsn generic netlink example module\n"); + return ret; + } + + register_tsn_notifier(&tsn_notifier); + + return 0; +} + +static void __exit tsn_genetlink_exit(void) +{ + int ret; + + ret = genl_unregister_family(&tsn_family); + if (ret != 0) + pr_info("failed to unregister family:%i\n", ret); + + unregister_tsn_notifier(&tsn_notifier); +} + +module_init(tsn_genetlink_init); +module_exit(tsn_genetlink_exit); +MODULE_LICENSE("GPL"); diff --git a/net/wireless/core.c b/net/wireless/core.c index 3f4554723..c24f9d4b0 100644 --- a/net/wireless/core.c +++ b/net/wireless/core.c @@ -405,6 +405,7 @@ struct wiphy *wiphy_new_nm(const struct cfg80211_ops *ops, int sizeof_priv, struct cfg80211_registered_device *rdev; int alloc_size; +#if 0 // adapter for ohos WARN_ON(ops->add_key && (!ops->del_key || !ops->set_default_key)); WARN_ON(ops->auth && (!ops->assoc || !ops->deauth || !ops->disassoc)); WARN_ON(ops->connect && !ops->disconnect); @@ -421,7 +422,7 @@ struct wiphy *wiphy_new_nm(const struct cfg80211_ops *ops, int sizeof_priv, WARN_ON(ops->remain_on_channel && !ops->cancel_remain_on_channel); WARN_ON(ops->tdls_channel_switch && !ops->tdls_cancel_channel_switch); WARN_ON(ops->add_tx_ts && !ops->del_tx_ts); - +#endif // adapter for ohos alloc_size = sizeof(*rdev) + sizeof_priv; rdev = kzalloc(alloc_size, GFP_KERNEL);