net: aquantia: add support for ptp ioctls

Here we add support for PTP specific IOCTLs of HW timestamp get/set.

These will use filters to configure flows onto the required queue ids.

Co-developed-by: Sergey Samoilenko <sergey.samoilenko@aquantia.com>
Signed-off-by: Sergey Samoilenko <sergey.samoilenko@aquantia.com>
Signed-off-by: Egor Pomozov <epomozov@marvell.com>
Signed-off-by: Igor Russkikh <igor.russkikh@aquantia.com>
Signed-off-by: David S. Miller <davem@davemloft.net>

diff --git a/drivers/net/ethernet/aquantia/atlantic/aq_main.c b/drivers/net/ethernet/aquantia/atlantic/aq_main.c
index f630032..a26d4a6 100644 (file)
--- a/drivers/net/ethernet/aquantia/atlantic/aq_main.c
+++ b/drivers/net/ethernet/aquantia/atlantic/aq_main.c
@@ -218,6 +218,87 @@ static void aq_ndev_set_multicast_settings(struct net_device *ndev)
        (void)aq_nic_set_multicast_list(aq_nic, ndev);
 }
 
+static int aq_ndev_config_hwtstamp(struct aq_nic_s *aq_nic,
+                                  struct hwtstamp_config *config)
+{
+       if (config->flags)
+               return -EINVAL;
+
+       switch (config->tx_type) {
+       case HWTSTAMP_TX_OFF:
+       case HWTSTAMP_TX_ON:
+               break;
+       default:
+               return -ERANGE;
+       }
+
+       switch (config->rx_filter) {
+       case HWTSTAMP_FILTER_PTP_V2_L4_EVENT:
+       case HWTSTAMP_FILTER_PTP_V2_L4_SYNC:
+       case HWTSTAMP_FILTER_PTP_V2_L4_DELAY_REQ:
+       case HWTSTAMP_FILTER_PTP_V2_L2_EVENT:
+       case HWTSTAMP_FILTER_PTP_V2_L2_SYNC:
+       case HWTSTAMP_FILTER_PTP_V2_L2_DELAY_REQ:
+       case HWTSTAMP_FILTER_PTP_V2_SYNC:
+       case HWTSTAMP_FILTER_PTP_V2_DELAY_REQ:
+               config->rx_filter = HWTSTAMP_FILTER_PTP_V2_EVENT;
+               break;
+       case HWTSTAMP_FILTER_PTP_V2_EVENT:
+       case HWTSTAMP_FILTER_NONE:
+               break;
+       default:
+               return -ERANGE;
+       }
+
+       return aq_ptp_hwtstamp_config_set(aq_nic->aq_ptp, config);
+}
+
+static int aq_ndev_hwtstamp_set(struct aq_nic_s *aq_nic, struct ifreq *ifr)
+{
+       struct hwtstamp_config config;
+       int ret_val;
+
+       if (!aq_nic->aq_ptp)
+               return -EOPNOTSUPP;
+
+       if (copy_from_user(&config, ifr->ifr_data, sizeof(config)))
+               return -EFAULT;
+
+       ret_val = aq_ndev_config_hwtstamp(aq_nic, &config);
+       if (ret_val)
+               return ret_val;
+
+       return copy_to_user(ifr->ifr_data, &config, sizeof(config)) ?
+              -EFAULT : 0;
+}
+
+static int aq_ndev_hwtstamp_get(struct aq_nic_s *aq_nic, struct ifreq *ifr)
+{
+       struct hwtstamp_config config;
+
+       if (!aq_nic->aq_ptp)
+               return -EOPNOTSUPP;
+
+       aq_ptp_hwtstamp_config_get(aq_nic->aq_ptp, &config);
+       return copy_to_user(ifr->ifr_data, &config, sizeof(config)) ?
+              -EFAULT : 0;
+}
+
+static int aq_ndev_ioctl(struct net_device *netdev, struct ifreq *ifr, int cmd)
+{
+       struct aq_nic_s *aq_nic = netdev_priv(netdev);
+
+       switch (cmd) {
+       case SIOCSHWTSTAMP:
+               return aq_ndev_hwtstamp_set(aq_nic, ifr);
+
+       case SIOCGHWTSTAMP:
+               return aq_ndev_hwtstamp_get(aq_nic, ifr);
+       }
+
+       return -EOPNOTSUPP;
+}
+
 static int aq_ndo_vlan_rx_add_vid(struct net_device *ndev, __be16 proto,
                                  u16 vid)
 {
@@ -255,6 +336,7 @@ static const struct net_device_ops aq_ndev_ops = {
        .ndo_change_mtu = aq_ndev_change_mtu,
        .ndo_set_mac_address = aq_ndev_set_mac_address,
        .ndo_set_features = aq_ndev_set_features,
+       .ndo_do_ioctl = aq_ndev_ioctl,
        .ndo_vlan_rx_add_vid = aq_ndo_vlan_rx_add_vid,
        .ndo_vlan_rx_kill_vid = aq_ndo_vlan_rx_kill_vid,
 };

diff --git a/drivers/net/ethernet/aquantia/atlantic/aq_ptp.c b/drivers/net/ethernet/aquantia/atlantic/aq_ptp.c
index 82409cb..5661379 100644 (file)
--- a/drivers/net/ethernet/aquantia/atlantic/aq_ptp.c
+++ b/drivers/net/ethernet/aquantia/atlantic/aq_ptp.c
@@ -44,6 +44,7 @@ struct ptp_tx_timeout {
 
 struct aq_ptp_s {
        struct aq_nic_s *aq_nic;
+       struct hwtstamp_config hwtstamp_config;
        spinlock_t ptp_lock;
        spinlock_t ptp_ring_lock;
        struct ptp_clock *ptp_clock;
@@ -388,6 +389,68 @@ static void aq_ptp_rx_hwtstamp(struct aq_ptp_s *aq_ptp, struct sk_buff *skb,
        aq_ptp_convert_to_hwtstamp(aq_ptp, skb_hwtstamps(skb), timestamp);
 }
 
+void aq_ptp_hwtstamp_config_get(struct aq_ptp_s *aq_ptp,
+                               struct hwtstamp_config *config)
+{
+       *config = aq_ptp->hwtstamp_config;
+}
+
+static void aq_ptp_prepare_filters(struct aq_ptp_s *aq_ptp)
+{
+       aq_ptp->udp_filter.cmd = HW_ATL_RX_ENABLE_FLTR_L3L4 |
+                              HW_ATL_RX_ENABLE_CMP_PROT_L4 |
+                              HW_ATL_RX_UDP |
+                              HW_ATL_RX_ENABLE_CMP_DEST_PORT_L4 |
+                              HW_ATL_RX_HOST << HW_ATL_RX_ACTION_FL3F4_SHIFT |
+                              HW_ATL_RX_ENABLE_QUEUE_L3L4 |
+                              aq_ptp->ptp_rx.idx << HW_ATL_RX_QUEUE_FL3L4_SHIFT;
+       aq_ptp->udp_filter.p_dst = PTP_EV_PORT;
+
+       aq_ptp->eth_type_filter.ethertype = ETH_P_1588;
+       aq_ptp->eth_type_filter.queue = aq_ptp->ptp_rx.idx;
+}
+
+int aq_ptp_hwtstamp_config_set(struct aq_ptp_s *aq_ptp,
+                              struct hwtstamp_config *config)
+{
+       struct aq_nic_s *aq_nic = aq_ptp->aq_nic;
+       const struct aq_hw_ops *hw_ops;
+       int err = 0;
+
+       hw_ops = aq_nic->aq_hw_ops;
+       if (config->tx_type == HWTSTAMP_TX_ON ||
+           config->rx_filter == HWTSTAMP_FILTER_PTP_V2_EVENT) {
+               aq_ptp_prepare_filters(aq_ptp);
+               if (hw_ops->hw_filter_l3l4_set) {
+                       err = hw_ops->hw_filter_l3l4_set(aq_nic->aq_hw,
+                                                        &aq_ptp->udp_filter);
+               }
+               if (!err && hw_ops->hw_filter_l2_set) {
+                       err = hw_ops->hw_filter_l2_set(aq_nic->aq_hw,
+                                                      &aq_ptp->eth_type_filter);
+               }
+               aq_utils_obj_set(&aq_nic->flags, AQ_NIC_PTP_DPATH_UP);
+       } else {
+               aq_ptp->udp_filter.cmd &= ~HW_ATL_RX_ENABLE_FLTR_L3L4;
+               if (hw_ops->hw_filter_l3l4_set) {
+                       err = hw_ops->hw_filter_l3l4_set(aq_nic->aq_hw,
+                                                        &aq_ptp->udp_filter);
+               }
+               if (!err && hw_ops->hw_filter_l2_clear) {
+                       err = hw_ops->hw_filter_l2_clear(aq_nic->aq_hw,
+                                                       &aq_ptp->eth_type_filter);
+               }
+               aq_utils_obj_clear(&aq_nic->flags, AQ_NIC_PTP_DPATH_UP);
+       }
+
+       if (err)
+               return -EREMOTEIO;
+
+       aq_ptp->hwtstamp_config = *config;
+
+       return 0;
+}
+
 bool aq_ptp_ring(struct aq_nic_s *aq_nic, struct aq_ring_s *ring)
 {
        struct aq_ptp_s *aq_ptp = aq_nic->aq_ptp;

diff --git a/drivers/net/ethernet/aquantia/atlantic/aq_ptp.h b/drivers/net/ethernet/aquantia/atlantic/aq_ptp.h
index 2c84483..7a7f36f 100644 (file)
--- a/drivers/net/ethernet/aquantia/atlantic/aq_ptp.h
+++ b/drivers/net/ethernet/aquantia/atlantic/aq_ptp.h
@@ -38,6 +38,12 @@ void aq_ptp_clock_init(struct aq_nic_s *aq_nic);
 int aq_ptp_xmit(struct aq_nic_s *aq_nic, struct sk_buff *skb);
 void aq_ptp_tx_hwtstamp(struct aq_nic_s *aq_nic, u64 timestamp);
 
+/* Must be to check available of PTP before call */
+void aq_ptp_hwtstamp_config_get(struct aq_ptp_s *aq_ptp,
+                               struct hwtstamp_config *config);
+int aq_ptp_hwtstamp_config_set(struct aq_ptp_s *aq_ptp,
+                              struct hwtstamp_config *config);
+
 /* Return either ring is belong to PTP or not*/
 bool aq_ptp_ring(struct aq_nic_s *aq_nic, struct aq_ring_s *ring);