net: phy: mchp: Add 1588 support for LAN8814 Quad PHY
Subsystems (3)
PATCH v4 2/2 net: phy: mchp: Add 1588 support for LAN8814 Quad PHY
This patch add supports for 1588 Hardware Timestamping support to LAN8814 Quad Phy. It supports L2 and Ipv4 encapsulations.
---
v1 -> v2
* Fixed warnings
v2 -> v3
* Split the patch to linkup/linkdown interrupt handling and 1588 Driver support to LAN8814 phy.
* Changed lan8814_priv to kszphy_ptp_priv and modified code Accordingly.
v3 -> v4
* Adding PTP maintainer into Cc list
--- drivers/net/phy/micrel.c | 942 ++++++++++++++++++++++++++++++++++++++- 1 file changed, 941 insertions(+), 1 deletion(-)
drivers/net/phy/micrel.c +941 -1
@@ -86,6 +86,111 @@#define LAN8814_INTS_ALL (LAN8814_INTS_LINK_UP |\ LAN8814_INTS_LINK_DOWN) +#define LAN8814_PTP_MAX_FREQ_ADJ_IN_PPB (31249999) +#define LAN8814_PTP_MAX_FINE_ADJ_IN_SCALED_PPM (2047999934) + +#define PTP_RX_MOD 0x024F +#define PTP_RX_RSVD_BYTE_CFG 0x0250 +#define PTP_RX_TIMESTAMP_EN 0x024D +#define PTP_TX_TIMESTAMP_EN 0x028D + +#define PTP_TX_PARSE_L2_ADDR_EN 0x0284 +#define PTP_RX_PARSE_L2_ADDR_EN 0x0244 +#define PTP_FLAG_ISR_ENABLED BIT(2) +#define PTP_FLAG_PTP_CLOCK_REGISTERED BIT(1) + +#define LTC_HARD_RESET 0x023F +#define LTC_HARD_RESET_ BIT(0) + +#define TSU_HARD_RESET 0x02C1 +#define TSU_HARD_RESET_ BIT(0) + +#define PTP_CMD_CTL 0x0200 +#define PTP_CMD_CTL_PTP_DISABLE_ BIT(0) +#define PTP_CMD_CTL_PTP_ENABLE_ BIT(1) +#define PTP_CMD_CTL_PTP_CLOCK_READ_ BIT(3) +#define PTP_CMD_CTL_PTP_CLOCK_LOAD_ BIT(4) +#define PTP_CMD_CTL_PTP_LTC_STEP_SEC_ BIT(5) +#define PTP_CMD_CTL_PTP_LTC_STEP_NSEC_ BIT(6) + +#define PTP_CLOCK_SET_SEC_HI (0x0205) +#define PTP_CLOCK_SET_SEC_MID (0x0206) +#define PTP_CLOCK_SET_SEC_LO (0x0207) +#define PTP_CLOCK_SET_NS_HI (0x0208) +#define PTP_CLOCK_SET_NS_LO (0x0209) +#define PTP_CLOCK_SET_SUBNS_LO (0x020B) +#define PTP_CLOCK_SET_SUBNS_HI (0x020A) + +#define PTP_CLOCK_READ_SEC_HI (0x0229) +#define PTP_CLOCK_READ_SEC_MID (0x022A) +#define PTP_CLOCK_READ_SEC_LO (0x022B) +#define PTP_CLOCK_READ_NS_HI (0x022C) +#define PTP_CLOCK_READ_NS_LO (0x022D) +#define PTP_CLOCK_READ_SUBNS_HI (0x022E) +#define PTP_CLOCK_READ_SUBNS_LO (0x022F) + +#define PTP_OPERATING_MODE (0x0241) +#define PTP_OPERATING_MODE_STANDALONE 0x01 + +#define PTP_TX_MOD (0x028F) +#define PTP_TX_MOD_TX_PTP_SYNC_TS_INSERT_ BIT(12) +#define PTP_TX_MOD_BAD_UDPV4_CHKSUM_FORCE_FCS_DIS_ BIT(3) + +#define PTP_RX_PARSE_CONFIG (0x0242) +#define PTP_RX_PARSE_CONFIG_LAYER2_EN_ BIT(0) +#define PTP_RX_PARSE_CONFIG_IPV4_EN_ BIT(1) + +#define PTP_TX_PARSE_CONFIG (0x0282) +#define PTP_TX_PARSE_CONFIG_LAYER2_EN_ BIT(0) +#define PTP_TX_PARSE_CONFIG_IPV4_EN_ BIT(1) + +#define PTP_CLOCK_RATE_ADJ_HI_ (0x020C) +#define PTP_CLOCK_RATE_ADJ_LO_ (0x020D) +#define PTP_CLOCK_RATE_ADJ_DIR_ BIT(15) + +#define PTP_LTC_STEP_ADJ_HI (0x0212) +#define PTP_LTC_STEP_ADJ_LO (0x0213) +#define PTP_LTC_STEP_ADJ_DIR_ BIT(15) + +#define PTP_TX_MSG_HEADER1 (0x0298) +#define PTP_TX_MSG_HEADER1_MSG_TYPE_ (0x000F) +#define PTP_TX_MSG_HEADER1_MSG_TYPE_SYNC_ (0x00000000) + +#define LAN8814_INTR_CTRL_REG (0x34) +#define LAN8814_INTR_CTRL_REG_POLARITY BIT(1) +#define LAN8814_INTR_CTRL_REG_INTR_ENABLE BIT(0) + +#define LAN8814_INTR_STS_REG (0x33) +#define LAN8814_INTR_STS_REG_1588_TSU0 BIT(0) + +#define PTP_CAP_INFO (0x022A) +#define PTP_CAP_INFO_TX_TS_CNT_GET_ (reg_val) (((reg_val) & 0x0f00) >> 8) + +#define PTP_TX_EGRESS_SEC_HI (0x0296) +#define PTP_TX_EGRESS_SEC_LO (0x0297) +#define PTP_TX_EGRESS_NS_HI (0x0294) +#define PTP_TX_EGRESS_NS_HI_TX_TS_VALID_ BIT(15) +#define PTP_TX_EGRESS_NS_LO (0x0295) +#define PTP_TX_MSG_HEADER2 (0x0299) + +#define PTP_RX_INGRESS_SEC_HI (0x0256) +#define PTP_RX_INGRESS_SEC_LO (0x0257) +#define PTP_RX_INGRESS_NS_HI (0x0254) +#define PTP_RX_INGRESS_NS_LO (0x0255) +#define PTP_RX_MSG_HEADER2 (0x0259) + +#define PTP_TSU_INT_EN (0x200) +#define PTP_TSU_INT_EN_PTP_TX_TS_OVRFL_EN BIT(3) +#define PTP_TSU_INT_EN_PTP_TX_TS_EN BIT(2) +#define PTP_TSU_INT_EN_PTP_RX_TS_OVRFL_EN BIT(1) +#define PTP_TSU_INT_EN_PTP_RX_TS_EN BIT(0) + +#define PTP_TSU_INT_STS (0x201) +#define PTP_TSU_INT_STS_PTP_TX_TS_OVRFL_INT BIT(3) +#define PTP_TSU_INT_STS_PTP_TX_TS_EN BIT(2) +#define PTP_TSU_INT_STS_PTP_RX_TS_OVRFL_INT BIT(1) +#define PTP_TSU_INT_STS_PTP_RX_TS_EN BIT(0) +/* PHY Control 1 */ #define MII_KSZPHY_CTRL_1 0x1e
@@ -132,7 +237,44 @@ struct kszphy_type {bool has_rmii_ref_clk_sel; }; +struct lan8814_ptp { +int flags; +struct ptp_clock *ptp_clock; +struct ptp_clock_info ptp_clock_info; +struct ptp_pin_desc pin_config[1 ]; +enum hwtstamp_tx_types tx_type; +enum hwtstamp_rx_filters rx_filter; +struct sk_buff_head tx_queue; +}; + +struct lan8814_ptphdr { +u8 tsmt;/* transportSpecific | messageType */ +u8 ver;/* reserved0 | versionPTP */ +__be16 msglen; +u8 domain; +u8 rsrvd1; +__be16 flags; +__be64 correction; +__be32 rsrvd2; +__be64 clk_identity; +__be16 src_port_id; +__be16 seq_id; +u8 ctrl; +u8 log_interval; +} __attribute__((__packed__ )); + +struct kszphy_ptp_priv { +struct mii_timestamper mii_ts; +struct phy_device *phydev; +struct lan8814_ptp ptp; +int hwts_tx_en; +int hwts_rx_en; +int layer; +int version; +}; +struct kszphy_priv { +struct kszphy_ptp_priv ptp_priv;const struct kszphy_type *type;int led_mode;bool rmii_ref_clk_sel;
@@ -171,6 +313,38 @@ static const struct kszphy_type ksz9021_type = { .interrupt_level_mask = BIT(14), }; +static void lan8814_ptp_clock_get (struct phy_device *phydev, +u32 *seconds,u32 *nano_seconds, +u32 *sub_nano_seconds); + +static int lan8814_read_page_reg (struct phy_device *phydev, +int page,u32 addr) +{ +u32 data; + +phy_write (phydev,KSZ_EXT_PAGE_ACCESS_CONTROL , page); +phy_write (phydev,KSZ_EXT_PAGE_ACCESS_ADDRESS_DATA , addr); +phy_write (phydev,KSZ_EXT_PAGE_ACCESS_CONTROL , (page |0x4000 )); + data =phy_read (phydev,KSZ_EXT_PAGE_ACCESS_ADDRESS_DATA ); + +return data; +} + +static int lan8814_write_page_reg (struct phy_device *phydev, +int page,u16 addr,u16 val) +{ +phy_write (phydev,KSZ_EXT_PAGE_ACCESS_CONTROL , page); +phy_write (phydev,KSZ_EXT_PAGE_ACCESS_ADDRESS_DATA , addr); +phy_write (phydev,KSZ_EXT_PAGE_ACCESS_CONTROL , (page |0x4000 )); + val =phy_write (phydev,KSZ_EXT_PAGE_ACCESS_ADDRESS_DATA , val); +if (val !=0 ) { +phydev_err (phydev,"Error: phy_write has returned error %d\n" , + val); +return val; + } +return 0 ; +} +static int kszphy_extended_write (struct phy_device *phydev,u32 regnum,u16 val) {
@@ -185,10 +359,156 @@ static int kszphy_extended_read(struct phy_device *phydev,return phy_read (phydev,MII_KSZPHY_EXTREG_READ ); } +static void lan8814_ptp_tx_ts_get (struct phy_device *phydev, +u32 *seconds,u32 *nano_seconds, +u32 *seq_id); + +static struct lan8814_ptphdr *get_ptp_header_l4 (struct sk_buff *skb, +struct iphdr *iphdr, +struct udphdr *udphdr) +{ +if (iphdr->version !=4 || iphdr->protocol !=IPPROTO_UDP ) +return NULL ; + +return (struct lan8814_ptphdr *)(((unsigned char *)udphdr) +UDP_HLEN ); +} + +static struct lan8814_ptphdr *get_ptp_header_tx (struct sk_buff *skb) +{ +struct ethhdr *ethhdr =eth_hdr (skb); +struct udphdr *udphdr; +struct iphdr *iphdr; + +if (ethhdr->h_proto ==htons (ETH_P_1588 )) +return (struct lan8814_ptphdr *)(((unsigned char *)ethhdr) + +skb_mac_header_len (skb)); + +if (ethhdr->h_proto !=htons (ETH_P_IP )) +return NULL ; + + iphdr =ip_hdr (skb); + udphdr =udp_hdr (skb); + +return get_ptp_header_l4 (skb, iphdr, udphdr); +} + +static int get_sig (struct sk_buff *skb,u8 *sig) +{ +struct lan8814_ptphdr *ptphdr =get_ptp_header_tx (skb); +struct ethhdr *ethhdr =eth_hdr (skb); +unsigned int i; + +if (!ptphdr) +return -EOPNOTSUPP ; + + sig[0 ] = (__force u16 )ptphdr->seq_id >>8 ; + sig[1 ] = (__force u16 )ptphdr->seq_id &GENMASK (7 ,0 ); + sig[2 ] = ptphdr->domain; + sig[3 ] = ptphdr->tsmt &GENMASK (3 ,0 ); + +memcpy (&sig[4 ], ethhdr->h_dest,ETH_ALEN ); + +/* Fill the last bytes of the signature to reach a 16B signature */ +for (i =10 ; i <16 ; i++) + sig[i] = ptphdr->tsmt &GENMASK (3 ,0 ); + +return 0 ; +} + +static void lan8814_dequeue_skb (struct lan8814_ptp *ptp) +{ + struct kszphy_ptp_priv *priv = container_of(ptp, struct kszphy_ptp_priv, ptp); +struct phy_device *phydev = priv->phydev; +struct skb_shared_hwtstamps shhwtstamps; +struct sk_buff *skb; +u8 skb_sig[16 ]; +int len; +u32 reg, cnt; +u32 seconds, nsec, seq_id; + + reg =lan8814_read_page_reg (phydev,5 ,PTP_CAP_INFO ); + cnt =PTP_CAP_INFO_TX_TS_CNT_GET_ (reg); + +/* FIFO is Empty*/ +if (cnt ==0 ) +return ; + + len =skb_queue_len (&ptp->tx_queue); +if (len <1 ) +return ; + +while (len--) { + skb =__skb_dequeue (&ptp->tx_queue); +if (!skb) +return ; + +/* Can't get the signature of the packet, won't ever +* be able to have one so let's dequeue the packet. +*/ +if (get_sig (skb, skb_sig) <0 ) { +kfree_skb (skb); +continue ; + } + +lan8814_ptp_tx_ts_get (phydev, &seconds, &nsec, &seq_id); + +/* Check if we found the signature we were looking for. */ +if (memcmp (skb_sig, &seq_id,sizeof (seq_id))) { +memset (&shhwtstamps,0 ,sizeof (shhwtstamps)); + shhwtstamps.hwtstamp =ktime_set (seconds, nsec); +skb_complete_tx_timestamp (skb, &shhwtstamps); + +return ; + } + +/* Valid signature but does not match the one of the +* packet in the FIFO right now, reschedule it for later +* packets. +*/ +__skb_queue_tail (&ptp->tx_queue, skb); + } +} + +static void lan8814_get_tx_ts (struct lan8814_ptp *ptp) +{ +u32 reg; + struct kszphy_ptp_priv *priv = container_of(ptp, struct kszphy_ptp_priv, ptp); +struct phy_device *phydev = priv->phydev; + +do { +lan8814_dequeue_skb (ptp); + +/* If other timestamps are available in the FIFO, process them. */ + reg =lan8814_read_page_reg (phydev,5 ,PTP_CAP_INFO ); + }while (PTP_CAP_INFO_TX_TS_CNT_GET_ (reg) >1 ); +} + +static irqreturn_t lan8814_handle_ptp_interrupt (struct phy_device *phydev) +{ +struct kszphy_ptp_priv *lan8814 = phydev->priv; +int rc; + + rc =lan8814_read_page_reg (phydev,5 ,PTP_TSU_INT_STS ); + +if (!(rc &PTP_TSU_INT_STS_PTP_TX_TS_EN )) +return IRQ_NONE ; + +lan8814_get_tx_ts (&lan8814->ptp); + +return IRQ_HANDLED ; +} +static irqreturn_t lan8814_handle_interrupt (struct phy_device *phydev) {int irq_status; + irq_status =lan8814_read_page_reg (phydev,4 ,LAN8814_INTR_STS_REG ); +if (irq_status <0 ) +return IRQ_NONE ; + +if (irq_status &LAN8814_INTR_STS_REG_1588_TSU0 ) +return lan8814_handle_ptp_interrupt (phydev); + irq_status =phy_read (phydev,LAN8814_INTS );if (irq_status <0 )return IRQ_NONE ;
@@ -199,10 +519,20 @@ static irqreturn_t lan8814_handle_interrupt(struct phy_device *phydev)return IRQ_HANDLED ; } +static int lan8814_config_ts_intr (struct phy_device *phydev) +{ +return lan8814_write_page_reg (phydev,5 ,PTP_TSU_INT_EN ,PTP_TSU_INT_EN_PTP_TX_TS_EN | +PTP_TSU_INT_EN_PTP_TX_TS_OVRFL_EN ); +} +static int lan8814_config_intr (struct phy_device *phydev) {int temp; +lan8814_write_page_reg (phydev,4 ,LAN8814_INTR_CTRL_REG , +LAN8814_INTR_CTRL_REG_POLARITY | +LAN8814_INTR_CTRL_REG_INTR_ENABLE ); +/* enable / disable interrupts */ if (phydev->interrupts ==PHY_INTERRUPT_ENABLED ) temp =LAN8814_INTC_ALL ;
@@ -1187,6 +1517,534 @@ static int kszphy_resume(struct phy_device *phydev)return 0 ; } +static bool lan8814_ptp_is_enable (struct phy_device *phydev) +{ +if (phy_read (phydev,PTP_CMD_CTL ) &PTP_CMD_CTL_PTP_ENABLE_ ) +return true; + +return false; +} + +static void lan8814_ptp_rx_ts_get (struct phy_device *phydev, +u32 *seconds,u32 *nano_seconds,u32 *seq_id) +{ +if (seconds) { + (*seconds) =lan8814_read_page_reg (phydev,5 ,PTP_RX_INGRESS_SEC_HI ); + (*seconds) = ((*seconds) <<16 ) | +lan8814_read_page_reg (phydev,5 ,PTP_RX_INGRESS_SEC_LO ); + } + +if (nano_seconds) { + (*nano_seconds) =lan8814_read_page_reg (phydev,5 ,PTP_RX_INGRESS_NS_HI ); + (*nano_seconds) = (((*nano_seconds) &0x3fff ) <<16 ) + |lan8814_read_page_reg (phydev,5 ,PTP_RX_INGRESS_NS_LO ); + } + +if (seq_id) + (*seq_id) =lan8814_read_page_reg (phydev,5 ,PTP_RX_MSG_HEADER2 ); +} + +static void lan8814_ptp_tx_ts_get (struct phy_device *phydev, +u32 *seconds,u32 *nano_seconds,u32 *seq_id) +{ +if (seconds) { + (*seconds) =lan8814_read_page_reg (phydev,5 ,PTP_TX_EGRESS_SEC_HI ); + (*seconds) = ((*seconds) <<16 ) | +lan8814_read_page_reg (phydev,5 ,PTP_TX_EGRESS_SEC_LO ); + } + +if (nano_seconds) { + (*nano_seconds) =lan8814_read_page_reg (phydev,5 ,PTP_TX_EGRESS_NS_HI ); + (*nano_seconds) = (((*nano_seconds) &0x3fff ) <<16 ) | +lan8814_read_page_reg (phydev,5 ,PTP_TX_EGRESS_NS_LO ); + } + +if (seq_id) + (*seq_id) =lan8814_read_page_reg (phydev,5 ,PTP_TX_MSG_HEADER2 ); +} + +static int lan8814_ts_info (struct mii_timestamper *mii_ts,struct ethtool_ts_info *info) +{ + struct kszphy_ptp_priv *lan8814 = container_of(mii_ts, struct kszphy_ptp_priv, mii_ts); + + info->so_timestamping = +SOF_TIMESTAMPING_TX_HARDWARE | +SOF_TIMESTAMPING_RX_HARDWARE | +SOF_TIMESTAMPING_RAW_HARDWARE ; + + info->phc_index =ptp_clock_index (lan8814->ptp.ptp_clock); + + info->tx_types = + (1 <<HWTSTAMP_TX_OFF ) | + (1 <<HWTSTAMP_TX_ON ) | + (1 <<HWTSTAMP_TX_ONESTEP_SYNC ); + + info->rx_filters = + (1 <<HWTSTAMP_FILTER_NONE ) | + (1 <<HWTSTAMP_FILTER_PTP_V1_L4_EVENT ) | + (1 <<HWTSTAMP_FILTER_PTP_V2_L4_EVENT ) | + (1 <<HWTSTAMP_FILTER_PTP_V2_L2_EVENT ) | + (1 <<HWTSTAMP_FILTER_PTP_V2_EVENT ); + +return 0 ; +} + +static int lan8814_hwtstamp (struct mii_timestamper *mii_ts,struct ifreq *ifr) +{ + struct kszphy_ptp_priv *lan8814 = container_of(mii_ts, struct kszphy_ptp_priv, mii_ts); +struct hwtstamp_config config; +int txcfg, rxcfg; + +if (copy_from_user (&config, ifr->ifr_data,sizeof (config))) +return -EFAULT ; + + lan8814->hwts_tx_en = config.tx_type; + + lan8814->ptp.rx_filter = config.rx_filter; + lan8814->ptp.tx_type = config.tx_type; + +switch (config.rx_filter) { +case HWTSTAMP_FILTER_NONE : + lan8814->hwts_rx_en =0 ; + lan8814->layer =0 ; + lan8814->version =0 ; +break ; +case HWTSTAMP_FILTER_PTP_V2_L4_EVENT : +case HWTSTAMP_FILTER_PTP_V2_L4_SYNC : +case HWTSTAMP_FILTER_PTP_V2_L4_DELAY_REQ : + lan8814->hwts_rx_en =1 ; + lan8814->layer =PTP_CLASS_L4 ; + lan8814->version =PTP_CLASS_V2 ; +break ; +case HWTSTAMP_FILTER_PTP_V2_L2_EVENT : +case HWTSTAMP_FILTER_PTP_V2_L2_SYNC : +case HWTSTAMP_FILTER_PTP_V2_L2_DELAY_REQ : + lan8814->hwts_rx_en =1 ; + lan8814->layer =PTP_CLASS_L2 ; + lan8814->version =PTP_CLASS_V2 ; +break ; +case HWTSTAMP_FILTER_PTP_V2_EVENT : +case HWTSTAMP_FILTER_PTP_V2_SYNC : +case HWTSTAMP_FILTER_PTP_V2_DELAY_REQ : + lan8814->hwts_rx_en =1 ; + lan8814->layer =PTP_CLASS_L4 |PTP_CLASS_L2 ; + lan8814->version =PTP_CLASS_V2 ; +break ; +default : +return -ERANGE ; + } + +lan8814_write_page_reg (lan8814->phydev,5 ,PTP_RX_PARSE_CONFIG ,0 ); +lan8814_write_page_reg (lan8814->phydev,5 ,PTP_TX_PARSE_CONFIG ,0 ); + +if (lan8814->hwts_rx_en && (lan8814->layer &PTP_CLASS_L2 )) + rxcfg =PTP_RX_PARSE_CONFIG_LAYER2_EN_ ; + +if (lan8814->hwts_tx_en && (lan8814->layer &PTP_CLASS_L2 )) + txcfg =PTP_TX_PARSE_CONFIG_LAYER2_EN_ ; + +if (lan8814->hwts_rx_en && (lan8814->layer &PTP_CLASS_L4 )) + rxcfg |=PTP_RX_PARSE_CONFIG_IPV4_EN_ ; + +if (lan8814->hwts_tx_en && (lan8814->layer &PTP_CLASS_L4 )) + txcfg |=PTP_TX_PARSE_CONFIG_IPV4_EN_ ; + +if (lan8814_ptp_is_enable (lan8814->phydev)) +lan8814_write_page_reg (lan8814->phydev,4 ,PTP_CMD_CTL , +PTP_CMD_CTL_PTP_DISABLE_ ); + +lan8814_write_page_reg (lan8814->phydev,5 ,PTP_RX_PARSE_CONFIG , rxcfg); +lan8814_write_page_reg (lan8814->phydev,5 ,PTP_TX_PARSE_CONFIG , txcfg); +lan8814_write_page_reg (lan8814->phydev,5 ,PTP_RX_TIMESTAMP_EN ,0x3 ); +lan8814_write_page_reg (lan8814->phydev,5 ,PTP_TX_TIMESTAMP_EN ,0x3 ); +lan8814_write_page_reg (lan8814->phydev,5 ,PTP_TX_PARSE_L2_ADDR_EN ,0 ); +lan8814_write_page_reg (lan8814->phydev,5 ,PTP_RX_PARSE_L2_ADDR_EN ,0 ); + +lan8814_write_page_reg (lan8814->phydev,4 ,PTP_CMD_CTL ,PTP_CMD_CTL_PTP_ENABLE_ ); +if (lan8814->hwts_tx_en ==HWTSTAMP_TX_ONESTEP_SYNC ) { +lan8814_write_page_reg (lan8814->phydev,5 ,PTP_TX_MOD , +PTP_TX_MOD_TX_PTP_SYNC_TS_INSERT_ ); + }else if (lan8814->hwts_tx_en ==HWTSTAMP_TX_ON ) { +/* Enabling 2 step offloading and all option for TS insertion/correction fields */ +lan8814_write_page_reg (lan8814->phydev,5 ,PTP_TX_MOD ,0x800 ); +lan8814_config_ts_intr (lan8814->phydev); + } + +return copy_to_user (ifr->ifr_data, &config,sizeof (config)) ? -EFAULT :0 ; +} + +static bool lan8814_rxtstamp (struct mii_timestamper *mii_ts,struct sk_buff *skb,int type) +{ + struct kszphy_ptp_priv *lan8814 = container_of(mii_ts, struct kszphy_ptp_priv, mii_ts); +struct skb_shared_hwtstamps *shhwtstamps =NULL ; +u32 seconds, nsec, seq_id; + +if (!lan8814->hwts_rx_en) +return false; + +if (!skb) +return false; + +if ((type & lan8814->version) ==0 || (type & lan8814->layer) ==0 ) +return false; + +if (lan8814->ptp.rx_filter ==HWTSTAMP_FILTER_NONE || + type ==PTP_CLASS_NONE ) +return false; + +lan8814_ptp_rx_ts_get (lan8814->phydev, &seconds, &nsec, &seq_id); + +/* Saving timestamp and sending through skbuff */ + shhwtstamps =skb_hwtstamps (skb); +memset (shhwtstamps,0 ,sizeof (*shhwtstamps)); + shhwtstamps->hwtstamp =ktime_set (seconds, nsec); + +netif_rx_ni (skb); + +return true; +} + +static int is_sync (struct sk_buff *skb,int type) +{ +u8 *data = skb->data, *msgtype; +unsigned int offset =0 ; + +if (type &PTP_CLASS_VLAN ) + offset +=VLAN_HLEN ; + +switch (type &PTP_CLASS_PMASK ) { +case PTP_CLASS_IPV4 : + offset +=ETH_HLEN +IPV4_HLEN (data + offset) +UDP_HLEN ; +break ; +case PTP_CLASS_IPV6 : + offset +=ETH_HLEN +IP6_HLEN +UDP_HLEN ; +break ; +case PTP_CLASS_L2 : + offset +=ETH_HLEN ; +break ; +default : +return 0 ; + } + +if (type &PTP_CLASS_V1 ) + offset +=OFF_PTP_CONTROL ; + +if (skb->len < offset +1 ) +return 0 ; + + msgtype = data + offset; + +return (*msgtype &0xf ) ==0 ; +} + +static void lan8814_txtstamp (struct mii_timestamper *mii_ts, +struct sk_buff *skb,int type) +{ + struct kszphy_ptp_priv *lan8814 = container_of(mii_ts, struct kszphy_ptp_priv, mii_ts); + +switch (lan8814->hwts_tx_en) { +case HWTSTAMP_TX_ONESTEP_SYNC : +if (is_sync (skb, type)) { +kfree_skb (skb); +return ; + } +break ; + +case HWTSTAMP_TX_ON : +skb_shinfo (skb)->tx_flags |=SKBTX_IN_PROGRESS ; +skb_queue_tail (&lan8814->ptp.tx_queue, skb); +break ; + +case HWTSTAMP_TX_OFF : +default : +kfree_skb (skb); +break ; + } +} + +static void lan8814_ptp_clock_set (struct phy_device *phydev, +u32 seconds,u32 nano_seconds, +u32 sub_nano_seconds) +{ +u32 sec_low, sec_high, nsec_low, nsec_high, snsec_low, snsec_high; + + sec_low = seconds &0xffff ; + sec_high = ((seconds >>16 ) &0xffff ); + nsec_low = nano_seconds &0xffff ; + nsec_high = ((nano_seconds >>16 ) &0x3fff ); + snsec_low = sub_nano_seconds &0xffff ; + snsec_high = ((sub_nano_seconds >>16 ) &0xffff ); + +lan8814_write_page_reg (phydev,4 ,PTP_CLOCK_SET_SEC_LO , sec_low); +lan8814_write_page_reg (phydev,4 ,PTP_CLOCK_SET_SEC_MID , sec_high); +lan8814_write_page_reg (phydev,4 ,PTP_CLOCK_SET_NS_LO , nsec_low); +lan8814_write_page_reg (phydev,4 ,PTP_CLOCK_SET_NS_HI , nsec_high); +lan8814_write_page_reg (phydev,4 ,PTP_CLOCK_SET_SUBNS_LO , snsec_low); +lan8814_write_page_reg (phydev,4 ,PTP_CLOCK_SET_SUBNS_HI , snsec_high); + +lan8814_write_page_reg (phydev,4 ,PTP_CMD_CTL ,PTP_CMD_CTL_PTP_CLOCK_LOAD_ ); +} + +static void lan8814_ptp_clock_get (struct phy_device *phydev, +u32 *seconds,u32 *nano_seconds, +u32 *sub_nano_seconds) +{ +lan8814_write_page_reg (phydev,4 ,PTP_CMD_CTL ,PTP_CMD_CTL_PTP_CLOCK_READ_ ); + +if (seconds) { + (*seconds) =lan8814_read_page_reg (phydev,4 ,PTP_CLOCK_READ_SEC_MID ); + (*seconds) = ((*seconds) <<16 ) | +lan8814_read_page_reg (phydev,4 ,PTP_CLOCK_READ_SEC_LO ); + } + +if (nano_seconds) { + (*nano_seconds) =lan8814_read_page_reg (phydev,4 ,PTP_CLOCK_READ_NS_HI ); + (*nano_seconds) = (((*nano_seconds) &0x3fff ) <<16 ) | +lan8814_read_page_reg (phydev,4 ,PTP_CLOCK_READ_NS_LO ); + } + +if (sub_nano_seconds) { + (*sub_nano_seconds) =lan8814_read_page_reg (phydev,4 , +PTP_CLOCK_READ_SUBNS_HI ); + (*sub_nano_seconds) = ((*sub_nano_seconds) <<16 ) | +lan8814_read_page_reg (phydev,4 , +PTP_CLOCK_READ_SUBNS_LO ); + } +} + +static int lan8814_ptpci_gettime64 (struct ptp_clock_info *ptpci, +struct timespec64 *ts) +{ +struct lan8814_ptp *ptp = + container_of(ptpci, struct lan8814_ptp, ptp_clock_info); +struct kszphy_ptp_priv *priv = + container_of(ptp, struct kszphy_ptp_priv, ptp); +struct phy_device *phydev = priv->phydev; +u32 nano_seconds =0 ; +u32 seconds =0 ; + +lan8814_ptp_clock_get (phydev, &seconds, &nano_seconds,NULL ); + ts->tv_sec = seconds; + ts->tv_nsec = nano_seconds; + +return 0 ; +} + +static void lan8814_ptp_clock_step (struct phy_device *phydev, +s64 time_step_ns) +{ +u32 nano_seconds_step =0 ; +u64 abs_time_step_ns =0 ; +u32 unsigned_seconds =0 ; +u32 nano_seconds =0 ; +u32 remainder =0 ; +s32 seconds =0 ; + +if (time_step_ns >15000000000LL ) { +/* convert to clock set */ +lan8814_ptp_clock_get (phydev, &unsigned_seconds, &nano_seconds,NULL ); + unsigned_seconds +=div_u64_rem (time_step_ns,1000000000LL , + &remainder); + nano_seconds += remainder; +if (nano_seconds >=1000000000 ) { + unsigned_seconds++; + nano_seconds -=1000000000 ; + } +lan8814_ptp_clock_set (phydev, unsigned_seconds, + nano_seconds,0 ); +return ; + }else if (time_step_ns <-15000000000LL ) { +/* convert to clock set */ + time_step_ns = -time_step_ns; + +lan8814_ptp_clock_get (phydev, &unsigned_seconds, + &nano_seconds,NULL ); + unsigned_seconds -=div_u64_rem (time_step_ns,1000000000LL , + &remainder); + nano_seconds_step = remainder; +if (nano_seconds < nano_seconds_step) { + unsigned_seconds--; + nano_seconds +=1000000000 ; + } + nano_seconds -= nano_seconds_step; +lan8814_ptp_clock_set (phydev, unsigned_seconds, + nano_seconds,0 ); +return ; + } + +/* do clock step */ +if (time_step_ns >=0 ) { + abs_time_step_ns = (u64)(time_step_ns); + seconds = (s32 )div_u64_rem (abs_time_step_ns,1000000000 , + &remainder); + nano_seconds = (u32 )remainder; + }else { + abs_time_step_ns = (u64)(-time_step_ns); + seconds = -((s32 )div_u64_rem (abs_time_step_ns,1000000000 , + &remainder)); + nano_seconds = (u32 )remainder; +if (nano_seconds >0 ) { +/* subtracting nano seconds is not allowed +* convert to subtracting from seconds, +* and adding to nanoseconds +*/ + seconds--; + nano_seconds = (1000000000 - nano_seconds); + } + } + +if (nano_seconds >0 ) { +/* add 8 ns to cover the likely normal increment */ + nano_seconds +=8 ; + } + +if (nano_seconds >=1000000000 ) { +/* carry into seconds */ + seconds++; + nano_seconds -=1000000000 ; + } + +while (seconds) { +if (seconds >0 ) { +u32 adjustment_value = (u32 )seconds; +u16 adjustment_value_lo, adjustment_value_hi; + + adjustment_value_lo = adjustment_value &0xffff ; + adjustment_value_hi = (adjustment_value >>16 ) &0x3fff ; + +if (adjustment_value >0xF ) + adjustment_value =0xF ; +lan8814_write_page_reg (phydev,4 ,PTP_LTC_STEP_ADJ_LO , + adjustment_value_lo); +lan8814_write_page_reg (phydev,4 ,PTP_LTC_STEP_ADJ_HI , +PTP_LTC_STEP_ADJ_DIR_ | + adjustment_value_hi); + seconds -= ((s32 )adjustment_value); + }else { +u32 adjustment_value = (u32)(-seconds); +u16 adjustment_value_lo, adjustment_value_hi; + + adjustment_value_lo = adjustment_value &0xffff ; + adjustment_value_hi = (adjustment_value >>16 ) &0x3fff ; +if (adjustment_value >0xF ) + adjustment_value =0xF ; +lan8814_write_page_reg (phydev,4 ,PTP_LTC_STEP_ADJ_LO , + adjustment_value_lo); +lan8814_write_page_reg (phydev,4 ,PTP_LTC_STEP_ADJ_HI , +PTP_LTC_STEP_ADJ_DIR_ | + adjustment_value_hi); + seconds += ((s32 )adjustment_value); + } +lan8814_write_page_reg (phydev,4 ,PTP_CMD_CTL , +PTP_CMD_CTL_PTP_LTC_STEP_SEC_ ); + } +if (nano_seconds) { +u16 nano_seconds_lo; +u16 nano_seconds_hi; + + nano_seconds_lo = nano_seconds &0xffff ; + nano_seconds_hi = (nano_seconds >>16 ) &0x3fff ; + +lan8814_write_page_reg (phydev,4 ,PTP_LTC_STEP_ADJ_LO , + nano_seconds_lo); +lan8814_write_page_reg (phydev,4 ,PTP_LTC_STEP_ADJ_HI , +PTP_LTC_STEP_ADJ_DIR_ | + nano_seconds_hi); +lan8814_write_page_reg (phydev,4 ,PTP_CMD_CTL , +PTP_CMD_CTL_PTP_LTC_STEP_NSEC_ ); + } +} + +static int lan8814_ptpci_adjtime (struct ptp_clock_info *ptpci,s64 delta) +{ +struct lan8814_ptp *ptp = + container_of(ptpci, struct lan8814_ptp, ptp_clock_info); +struct kszphy_ptp_priv *priv = + container_of(ptp, struct kszphy_ptp_priv, ptp); +struct phy_device *phydev = priv->phydev; + +lan8814_ptp_clock_step (phydev, delta); + +return 0 ; +} + +static int lan8814_ptpci_adjfine (struct ptp_clock_info *ptpci,long scaled_ppm) +{ +struct lan8814_ptp *ptp = + container_of(ptpci, struct lan8814_ptp, ptp_clock_info); + struct kszphy_ptp_priv *priv = container_of(ptp, struct kszphy_ptp_priv, ptp); +struct phy_device *phydev = priv->phydev; +u32 kszphy_rate_adj =0 ; +u16 kszphy_rate_adj_lo =0 , kszphy_rate_adj_hi =0 ; +bool positive = true; +u64 u64_delta =0 ; + +if ((scaled_ppm < (-LAN8814_PTP_MAX_FINE_ADJ_IN_SCALED_PPM )) || + scaled_ppm >LAN8814_PTP_MAX_FINE_ADJ_IN_SCALED_PPM ) { +return -EINVAL ; + } +if (scaled_ppm >0 ) { + u64_delta = (u64 )scaled_ppm; + positive = true; + }else { + u64_delta = (u64)(-scaled_ppm); + positive = false; + } + u64_delta = (u64_delta <<13 ); + kszphy_rate_adj =div_u64 (u64_delta,15625 ); + + kszphy_rate_adj_lo = kszphy_rate_adj &0xffff ; + kszphy_rate_adj_hi = (kszphy_rate_adj >>16 ) &0x3fff ; + +if (positive) + kszphy_rate_adj_hi |=PTP_CLOCK_RATE_ADJ_DIR_ ; + +lan8814_write_page_reg (phydev,4 ,PTP_CLOCK_RATE_ADJ_HI_ , kszphy_rate_adj_hi); +lan8814_write_page_reg (phydev,4 ,PTP_CLOCK_RATE_ADJ_LO_ , kszphy_rate_adj_lo); + +return 0 ; +} + +static void lan8814_ptp_reset (struct phy_device *phydev) +{ +if (lan8814_ptp_is_enable (phydev)) +lan8814_write_page_reg (phydev,4 ,PTP_CMD_CTL ,PTP_CMD_CTL_PTP_DISABLE_ ); + +lan8814_write_page_reg (phydev,4 ,LTC_HARD_RESET ,LTC_HARD_RESET_ ); +lan8814_write_page_reg (phydev,5 ,TSU_HARD_RESET ,TSU_HARD_RESET_ ); +} + +static void lan8814_ptp_enable (struct phy_device *phydev) +{ +if (lan8814_ptp_is_enable (phydev)) +return ; + +lan8814_write_page_reg (phydev,4 ,PTP_CMD_CTL ,PTP_CMD_CTL_PTP_ENABLE_ ); +} + +static void lan8814_ptp_sync_to_system_clock (struct phy_device *phydev) +{ +struct timespec64 ts; + +ktime_get_clocktai_ts64 (&ts); + +lan8814_ptp_clock_set (phydev, ts.tv_sec, ts.tv_nsec,0 ); +} + +static void lan8814_ptp_init (struct phy_device *phydev) +{ +u32 temp; + +lan8814_ptp_reset (phydev); +lan8814_ptp_sync_to_system_clock (phydev); + temp =lan8814_read_page_reg (phydev,5 ,PTP_TX_MOD ); + temp |=PTP_TX_MOD_BAD_UDPV4_CHKSUM_FORCE_FCS_DIS_ ; +lan8814_write_page_reg (phydev,5 ,PTP_TX_MOD , temp); +lan8814_write_page_reg (phydev,4 ,PTP_OPERATING_MODE , +PTP_OPERATING_MODE_STANDALONE ); +lan8814_ptp_enable (phydev); +} +static int kszphy_probe (struct phy_device *phydev) {const struct kszphy_type *type = phydev->drv->driver_data;
@@ -1248,6 +2106,88 @@ static int kszphy_probe(struct phy_device *phydev)return 0 ; } +static int lan8814_probe (struct phy_device *phydev) +{ +struct kszphy_priv *priv; +struct clk *clk; +const struct kszphy_type *type = phydev->drv->driver_data; +const struct device_node *np = phydev->mdio.dev.of_node; + + priv =devm_kzalloc (&phydev->mdio.dev,sizeof (*priv),GFP_KERNEL ); +if (!priv) +return -ENOMEM ; + +skb_queue_head_init (&priv->ptp_priv.ptp.tx_queue); + + priv->ptp_priv.phydev = phydev; + + priv->type = type; + + priv->ptp_priv.mii_ts.rxtstamp = lan8814_rxtstamp; + priv->ptp_priv.mii_ts.txtstamp = lan8814_txtstamp; + priv->ptp_priv.mii_ts.hwtstamp = lan8814_hwtstamp; + priv->ptp_priv.mii_ts.ts_info = lan8814_ts_info; + + phydev->mii_ts = &priv->ptp_priv.mii_ts; + + phydev->priv = priv; + +lan8814_ptp_init (phydev); + + priv->ptp_priv.ptp.flags |=PTP_FLAG_ISR_ENABLED ; + +if (!IS_ENABLED (CONFIG_PTP_1588_CLOCK )) +return 0 ; + + clk =devm_clk_get (&phydev->mdio.dev,"rmii-ref" ); +/* NOTE: clk may be NULL if building without CONFIG_HAVE_CLK */ +if (!IS_ERR_OR_NULL (clk)) { +unsigned long rate =clk_get_rate (clk); +bool rmii_ref_clk_sel_25_mhz; + + priv->rmii_ref_clk_sel = type->has_rmii_ref_clk_sel; + rmii_ref_clk_sel_25_mhz =of_property_read_bool (np, +"micrel,rmii-reference-clock-select-25-mhz" ); + +if (rate >24500000 && rate <25500000 ) { + priv->rmii_ref_clk_sel_val = rmii_ref_clk_sel_25_mhz; + }else if (rate >49500000 && rate <50500000 ) { + priv->rmii_ref_clk_sel_val = !rmii_ref_clk_sel_25_mhz; + }else { +phydev_err (phydev,"Clock rate out of range: %ld\n" , + rate); +return -EINVAL ; + } + } + + priv->ptp_priv.ptp.ptp_clock_info.owner =THIS_MODULE ; +snprintf (priv->ptp_priv.ptp.ptp_clock_info.name,30 ,"%s" , + phydev->drv->name); + priv->ptp_priv.ptp.ptp_clock_info.max_adj =LAN8814_PTP_MAX_FREQ_ADJ_IN_PPB ; + priv->ptp_priv.ptp.ptp_clock_info.n_alarm =0 ; + priv->ptp_priv.ptp.ptp_clock_info.n_ext_ts =0 ; + priv->ptp_priv.ptp.ptp_clock_info.n_per_out =1 ; + priv->ptp_priv.ptp.ptp_clock_info.n_pins =0 ; + priv->ptp_priv.ptp.ptp_clock_info.pps =0 ; + priv->ptp_priv.ptp.ptp_clock_info.pin_config =NULL ; + priv->ptp_priv.ptp.ptp_clock_info.adjfine = lan8814_ptpci_adjfine; + priv->ptp_priv.ptp.ptp_clock_info.adjtime = lan8814_ptpci_adjtime; + priv->ptp_priv.ptp.ptp_clock_info.gettime64 = lan8814_ptpci_gettime64; + priv->ptp_priv.ptp.ptp_clock_info.getcrosststamp =NULL ; + + priv->ptp_priv.ptp.ptp_clock =ptp_clock_register (&priv->ptp_priv.ptp.ptp_clock_info, + &phydev->mdio.dev); + +if (IS_ERR_OR_NULL (priv->ptp_priv.ptp.ptp_clock)) +phydev_err (phydev,"ptp_clock_register failed %lu\n" , +PTR_ERR (priv->ptp_priv.ptp.ptp_clock)); + + priv->ptp_priv.ptp.flags |=PTP_FLAG_PTP_CLOCK_REGISTERED ; +phydev_info (phydev,"successfully registered ptp clock\n" ); + +return 0 ; +} +static struct phy_driver ksphy_driver[] = { { .phy_id =PHY_ID_KS8737 ,
@@ -1415,7 +2355,7 @@ static struct phy_driver ksphy_driver[] = { .phy_id_mask = MICREL_PHY_ID_MASK, .name = "Microchip INDY Gigabit Quad PHY", .driver_data = &ksz9021_type, - .probe = kszphy_probe, + .probe = lan8814_probe, .soft_reset = genphy_soft_reset, .read_status = ksz9031_read_status, .get_sset_count = kszphy_get_sset_count,
Signature · 1 line
2.17.1
Andrew Lunn
maintainerQuoted 17 lines
drivers/net/phy/micrel.c +17
-0+static int lan8814_read_page_reg (struct phy_device *phydev, +int page,u32 addr) +{ +u32 data; + +phy_write (phydev,KSZ_EXT_PAGE_ACCESS_CONTROL , page); +phy_write (phydev,KSZ_EXT_PAGE_ACCESS_ADDRESS_DATA , addr); +phy_write (phydev,KSZ_EXT_PAGE_ACCESS_CONTROL , (page |0x4000 )); + data =phy_read (phydev,KSZ_EXT_PAGE_ACCESS_ADDRESS_DATA ); + +return data; +} + +static int lan8814_write_page_reg (struct phy_device *phydev, +int page,u16 addr,u16 val) +{ +phy_write (phydev,KSZ_EXT_PAGE_ACCESS_CONTROL , page);
drivers/net/phy/micrel.c static const struct kszphy_type ksz9021_type = { · line 346
Quoted 10 lines
drivers/net/phy/micrel.c +10
-0+phy_write (phydev,KSZ_EXT_PAGE_ACCESS_ADDRESS_DATA , addr); +phy_write (phydev,KSZ_EXT_PAGE_ACCESS_CONTROL , (page |0x4000 )); + val =phy_write (phydev,KSZ_EXT_PAGE_ACCESS_ADDRESS_DATA , val); +if (val !=0 ) { +phydev_err (phydev,"Error: phy_write has returned error %d\n" , + val); +return val; + } +return 0 ; +}
You seem to of ignored my comment about using phy_read_mmd() and phy_write_mmd()? If i'm wrong and these will not work, please say so. Don't just ignore comments.
Andrew
Quoted 5 lines · Divya Koppera
drivers/net/phy/micrel.c +5
-0+struct lan8814_ptphdr { +u8 tsmt;/* transportSpecific | messageType */ +u8 ver;/* reserved0 | versionPTP */ +__be16 msglen; +u8 domain;
drivers/net/phy/micrel.c struct kszphy_type { · line 264
Quoted 10 lines
drivers/net/phy/micrel.c +10
-0+u8 rsrvd1; +__be16 flags; +__be64 correction; +__be32 rsrvd2; +__be64 clk_identity; +__be16 src_port_id; +__be16 seq_id; +u8 ctrl; +u8 log_interval; +} __attribute__((__packed__));
Please use the new 'struct ptp_header' from ptp_classify.h instead.
Quoted 21 lines
drivers/net/phy/micrel.c +13
-0+struct kszphy_ptp_priv { +struct mii_timestamper mii_ts; +struct phy_device *phydev; +struct lan8814_ptp ptp; +int hwts_tx_en; +int hwts_rx_en; +int layer; +int version; +}; +struct kszphy_priv { +struct kszphy_ptp_priv ptp_priv;const struct kszphy_type *type;int led_mode;bool rmii_ref_clk_sel;@@ -171,6 +313,38 @@ static const struct kszphy_type ksz9021_type = { .interrupt_level_mask = BIT(14), }; +static void lan8814_ptp_clock_get(struct phy_device *phydev, + u32 *seconds, u32 *nano_seconds,
drivers/net/phy/micrel.c static const struct kszphy_type ksz9021_type = { · line 327
Quoted 10 lines
drivers/net/phy/micrel.c +10
-0+ u32 *sub_nano_seconds); + +static int lan8814_read_page_reg (struct phy_device *phydev, +int page,u32 addr) +{ +u32 data; + +phy_write (phydev,KSZ_EXT_PAGE_ACCESS_CONTROL , page); +phy_write (phydev,KSZ_EXT_PAGE_ACCESS_ADDRESS_DATA , addr); +phy_write (phydev,KSZ_EXT_PAGE_ACCESS_CONTROL , (page |0x4000 ));
What prevents concurrent reads from happening? Nothing, AFAICT.
drivers/net/phy/micrel.c static const struct kszphy_type ksz9021_type = { · line 336
Quoted 9 lines
drivers/net/phy/micrel.c +9
-0+ data =phy_read (phydev,KSZ_EXT_PAGE_ACCESS_ADDRESS_DATA ); + +return data; +} + +static int lan8814_write_page_reg (struct phy_device *phydev, +int page,u16 addr,u16 val) +{ +phy_write (phydev,KSZ_EXT_PAGE_ACCESS_CONTROL , page);
Consider caching the page value in order to make a sequence of reads/writes more efficient. (example in dp83640.c)
Review static const struct kszphy_type ksz9021_type = { · line 338
Quoted 2 lines
(quoted diff) +2
-0+ phy_write(phydev, KSZ_EXT_PAGE_ACCESS_ADDRESS_DATA, addr); + phy_write(phydev, KSZ_EXT_PAGE_ACCESS_CONTROL, (page | 0x4000));
Again, this needs some kind of serialization. Maybe not here but rather in the callers... you need to sort that out.
Quoted 37 lines
drivers/net/phy/micrel.c +30
-0+ val =phy_write (phydev,KSZ_EXT_PAGE_ACCESS_ADDRESS_DATA , val); +if (val !=0 ) { +phydev_err (phydev,"Error: phy_write has returned error %d\n" , + val); +return val; + } +return 0 ; +} +static int kszphy_extended_write (struct phy_device *phydev,u32 regnum,u16 val) {@@ -185,10 +359,156 @@ static int kszphy_extended_read(struct phy_device *phydev,return phy_read (phydev,MII_KSZPHY_EXTREG_READ ); } +static void lan8814_ptp_tx_ts_get (struct phy_device *phydev, +u32 *seconds,u32 *nano_seconds, +u32 *seq_id); + +static struct lan8814_ptphdr *get_ptp_header_l4 (struct sk_buff *skb, +struct iphdr *iphdr, +struct udphdr *udphdr) +{ +if (iphdr->version !=4 || iphdr->protocol !=IPPROTO_UDP ) +return NULL ; + +return (struct lan8814_ptphdr *)(((unsigned char *)udphdr) +UDP_HLEN ); +} + +static struct lan8814_ptphdr *get_ptp_header_tx (struct sk_buff *skb) +{ +struct ethhdr *ethhdr =eth_hdr (skb); +struct udphdr *udphdr; +struct iphdr *iphdr; + +if (ethhdr->h_proto ==htons (ETH_P_1588 ))
drivers/net/phy/micrel.c static int kszphy_extended_read(struct phy_device *phydev, · line 392
Quoted 10 lines
drivers/net/phy/micrel.c +10
-0+return (struct lan8814_ptphdr *)(((unsigned char *)ethhdr) + +skb_mac_header_len (skb)); + +if (ethhdr->h_proto !=htons (ETH_P_IP )) +return NULL ; + + iphdr =ip_hdr (skb); + udphdr =udp_hdr (skb); + +return get_ptp_header_l4 (skb, iphdr, udphdr);
Use ptp_parse_header() from ptp_classify.h
Quoted 149 lines
drivers/net/phy/micrel.c +127
-0+} + +static int get_sig (struct sk_buff *skb,u8 *sig) +{ +struct lan8814_ptphdr *ptphdr =get_ptp_header_tx (skb); +struct ethhdr *ethhdr =eth_hdr (skb); +unsigned int i; + +if (!ptphdr) +return -EOPNOTSUPP ; + + sig[0 ] = (__force u16 )ptphdr->seq_id >>8 ; + sig[1 ] = (__force u16 )ptphdr->seq_id &GENMASK (7 ,0 ); + sig[2 ] = ptphdr->domain; + sig[3 ] = ptphdr->tsmt &GENMASK (3 ,0 ); + +memcpy (&sig[4 ], ethhdr->h_dest,ETH_ALEN ); + +/* Fill the last bytes of the signature to reach a 16B signature */ +for (i =10 ; i <16 ; i++) + sig[i] = ptphdr->tsmt &GENMASK (3 ,0 ); + +return 0 ; +} + +static void lan8814_dequeue_skb (struct lan8814_ptp *ptp) +{ + struct kszphy_ptp_priv *priv = container_of(ptp, struct kszphy_ptp_priv, ptp); +struct phy_device *phydev = priv->phydev; +struct skb_shared_hwtstamps shhwtstamps; +struct sk_buff *skb; +u8 skb_sig[16 ]; +int len; +u32 reg, cnt; +u32 seconds, nsec, seq_id; + + reg =lan8814_read_page_reg (phydev,5 ,PTP_CAP_INFO ); + cnt =PTP_CAP_INFO_TX_TS_CNT_GET_ (reg); + +/* FIFO is Empty*/ +if (cnt ==0 ) +return ; + + len =skb_queue_len (&ptp->tx_queue); +if (len <1 ) +return ; + +while (len--) { + skb =__skb_dequeue (&ptp->tx_queue); +if (!skb) +return ; + +/* Can't get the signature of the packet, won't ever +* be able to have one so let's dequeue the packet. +*/ +if (get_sig (skb, skb_sig) <0 ) { +kfree_skb (skb); +continue ; + } + +lan8814_ptp_tx_ts_get (phydev, &seconds, &nsec, &seq_id); + +/* Check if we found the signature we were looking for. */ +if (memcmp (skb_sig, &seq_id,sizeof (seq_id))) { +memset (&shhwtstamps,0 ,sizeof (shhwtstamps)); + shhwtstamps.hwtstamp =ktime_set (seconds, nsec); +skb_complete_tx_timestamp (skb, &shhwtstamps); + +return ; + } + +/* Valid signature but does not match the one of the +* packet in the FIFO right now, reschedule it for later +* packets. +*/ +__skb_queue_tail (&ptp->tx_queue, skb); + } +} + +static void lan8814_get_tx_ts (struct lan8814_ptp *ptp) +{ +u32 reg; + struct kszphy_ptp_priv *priv = container_of(ptp, struct kszphy_ptp_priv, ptp); +struct phy_device *phydev = priv->phydev; + +do { +lan8814_dequeue_skb (ptp); + +/* If other timestamps are available in the FIFO, process them. */ + reg =lan8814_read_page_reg (phydev,5 ,PTP_CAP_INFO ); + }while (PTP_CAP_INFO_TX_TS_CNT_GET_ (reg) >1 ); +} + +static irqreturn_t lan8814_handle_ptp_interrupt (struct phy_device *phydev) +{ +struct kszphy_ptp_priv *lan8814 = phydev->priv; +int rc; + + rc =lan8814_read_page_reg (phydev,5 ,PTP_TSU_INT_STS ); + +if (!(rc &PTP_TSU_INT_STS_PTP_TX_TS_EN )) +return IRQ_NONE ; + +lan8814_get_tx_ts (&lan8814->ptp); + +return IRQ_HANDLED ; +} +static irqreturn_t lan8814_handle_interrupt (struct phy_device *phydev) {int irq_status; + irq_status =lan8814_read_page_reg (phydev,4 ,LAN8814_INTR_STS_REG ); +if (irq_status <0 ) +return IRQ_NONE ; + +if (irq_status &LAN8814_INTR_STS_REG_1588_TSU0 ) +return lan8814_handle_ptp_interrupt (phydev); + irq_status =phy_read (phydev,LAN8814_INTS );if (irq_status <0 )return IRQ_NONE ;@@ -199,10 +519,20 @@ static irqreturn_t lan8814_handle_interrupt(struct phy_device *phydev)return IRQ_HANDLED ; } +static int lan8814_config_ts_intr (struct phy_device *phydev) +{ +return lan8814_write_page_reg (phydev,5 ,PTP_TSU_INT_EN ,PTP_TSU_INT_EN_PTP_TX_TS_EN | +PTP_TSU_INT_EN_PTP_TX_TS_OVRFL_EN ); +} +static int lan8814_config_intr (struct phy_device *phydev) {int temp; +lan8814_write_page_reg (phydev,4 ,LAN8814_INTR_CTRL_REG , +LAN8814_INTR_CTRL_REG_POLARITY | +LAN8814_INTR_CTRL_REG_INTR_ENABLE ); +/* enable / disable interrupts */ if (phydev->interrupts ==PHY_INTERRUPT_ENABLED ) temp =LAN8814_INTC_ALL ;@@ -1187,6 +1517,534 @@ static int kszphy_resume(struct phy_device *phydev)return 0 ; } +static bool lan8814_ptp_is_enable (struct phy_device *phydev) +{
drivers/net/phy/micrel.c static int kszphy_resume(struct phy_device *phydev) · line 1531
Quoted 10 lines
drivers/net/phy/micrel.c +10
-0+if (phy_read (phydev,PTP_CMD_CTL ) &PTP_CMD_CTL_PTP_ENABLE_ ) +return true; + +return false; +} + +static void lan8814_ptp_rx_ts_get (struct phy_device *phydev, +u32 *seconds,u32 *nano_seconds,u32 *seq_id) +{ +if (seconds) {
No need to check this. The one and only caller provides a pointer.
drivers/net/phy/micrel.c static int kszphy_resume(struct phy_device *phydev) · line 1537
Quoted 6 lines
drivers/net/phy/micrel.c +6
-0+ (*seconds) =lan8814_read_page_reg (phydev,5 ,PTP_RX_INGRESS_SEC_HI ); + (*seconds) = ((*seconds) <<16 ) | +lan8814_read_page_reg (phydev,5 ,PTP_RX_INGRESS_SEC_LO ); + } + +if (nano_seconds) {
same here.
drivers/net/phy/micrel.c static int kszphy_resume(struct phy_device *phydev) · line 1543
Quoted 6 lines
drivers/net/phy/micrel.c +6
-0+ (*nano_seconds) =lan8814_read_page_reg (phydev,5 ,PTP_RX_INGRESS_NS_HI ); + (*nano_seconds) = (((*nano_seconds) &0x3fff ) <<16 ) + |lan8814_read_page_reg (phydev,5 ,PTP_RX_INGRESS_NS_LO ); + } + +if (seq_id)
and here.
drivers/net/phy/micrel.c static int kszphy_resume(struct phy_device *phydev) · line 1550
Quoted 7 lines
drivers/net/phy/micrel.c +7
-0+ (*seq_id) =lan8814_read_page_reg (phydev,5 ,PTP_RX_MSG_HEADER2 ); +} + +static void lan8814_ptp_tx_ts_get (struct phy_device *phydev, +u32 *seconds,u32 *nano_seconds,u32 *seq_id) +{ +if (seconds) {
ditto.
Quoted 94 lines
drivers/net/phy/micrel.c +94
-0+ (*seconds) =lan8814_read_page_reg (phydev,5 ,PTP_TX_EGRESS_SEC_HI ); + (*seconds) = ((*seconds) <<16 ) | +lan8814_read_page_reg (phydev,5 ,PTP_TX_EGRESS_SEC_LO ); + } + +if (nano_seconds) { + (*nano_seconds) =lan8814_read_page_reg (phydev,5 ,PTP_TX_EGRESS_NS_HI ); + (*nano_seconds) = (((*nano_seconds) &0x3fff ) <<16 ) | +lan8814_read_page_reg (phydev,5 ,PTP_TX_EGRESS_NS_LO ); + } + +if (seq_id) + (*seq_id) =lan8814_read_page_reg (phydev,5 ,PTP_TX_MSG_HEADER2 ); +} + +static int lan8814_ts_info (struct mii_timestamper *mii_ts,struct ethtool_ts_info *info) +{ + struct kszphy_ptp_priv *lan8814 = container_of(mii_ts, struct kszphy_ptp_priv, mii_ts); + + info->so_timestamping = +SOF_TIMESTAMPING_TX_HARDWARE | +SOF_TIMESTAMPING_RX_HARDWARE | +SOF_TIMESTAMPING_RAW_HARDWARE ; + + info->phc_index =ptp_clock_index (lan8814->ptp.ptp_clock); + + info->tx_types = + (1 <<HWTSTAMP_TX_OFF ) | + (1 <<HWTSTAMP_TX_ON ) | + (1 <<HWTSTAMP_TX_ONESTEP_SYNC ); + + info->rx_filters = + (1 <<HWTSTAMP_FILTER_NONE ) | + (1 <<HWTSTAMP_FILTER_PTP_V1_L4_EVENT ) | + (1 <<HWTSTAMP_FILTER_PTP_V2_L4_EVENT ) | + (1 <<HWTSTAMP_FILTER_PTP_V2_L2_EVENT ) | + (1 <<HWTSTAMP_FILTER_PTP_V2_EVENT ); + +return 0 ; +} + +static int lan8814_hwtstamp (struct mii_timestamper *mii_ts,struct ifreq *ifr) +{ + struct kszphy_ptp_priv *lan8814 = container_of(mii_ts, struct kszphy_ptp_priv, mii_ts); +struct hwtstamp_config config; +int txcfg, rxcfg; + +if (copy_from_user (&config, ifr->ifr_data,sizeof (config))) +return -EFAULT ; + + lan8814->hwts_tx_en = config.tx_type; + + lan8814->ptp.rx_filter = config.rx_filter; + lan8814->ptp.tx_type = config.tx_type; + +switch (config.rx_filter) { +case HWTSTAMP_FILTER_NONE : + lan8814->hwts_rx_en =0 ; + lan8814->layer =0 ; + lan8814->version =0 ; +break ; +case HWTSTAMP_FILTER_PTP_V2_L4_EVENT : +case HWTSTAMP_FILTER_PTP_V2_L4_SYNC : +case HWTSTAMP_FILTER_PTP_V2_L4_DELAY_REQ : + lan8814->hwts_rx_en =1 ; + lan8814->layer =PTP_CLASS_L4 ; + lan8814->version =PTP_CLASS_V2 ; +break ; +case HWTSTAMP_FILTER_PTP_V2_L2_EVENT : +case HWTSTAMP_FILTER_PTP_V2_L2_SYNC : +case HWTSTAMP_FILTER_PTP_V2_L2_DELAY_REQ : + lan8814->hwts_rx_en =1 ; + lan8814->layer =PTP_CLASS_L2 ; + lan8814->version =PTP_CLASS_V2 ; +break ; +case HWTSTAMP_FILTER_PTP_V2_EVENT : +case HWTSTAMP_FILTER_PTP_V2_SYNC : +case HWTSTAMP_FILTER_PTP_V2_DELAY_REQ : + lan8814->hwts_rx_en =1 ; + lan8814->layer =PTP_CLASS_L4 |PTP_CLASS_L2 ; + lan8814->version =PTP_CLASS_V2 ; +break ; +default : +return -ERANGE ; + } + +lan8814_write_page_reg (lan8814->phydev,5 ,PTP_RX_PARSE_CONFIG ,0 ); +lan8814_write_page_reg (lan8814->phydev,5 ,PTP_TX_PARSE_CONFIG ,0 ); + +if (lan8814->hwts_rx_en && (lan8814->layer &PTP_CLASS_L2 )) + rxcfg =PTP_RX_PARSE_CONFIG_LAYER2_EN_ ; + +if (lan8814->hwts_tx_en && (lan8814->layer &PTP_CLASS_L2 )) + txcfg =PTP_TX_PARSE_CONFIG_LAYER2_EN_ ;
drivers/net/phy/micrel.c static int kszphy_resume(struct phy_device *phydev) · line 1654
Quoted 10 lines
drivers/net/phy/micrel.c +10
-0+ +if (lan8814->hwts_rx_en && (lan8814->layer &PTP_CLASS_L4 )) + rxcfg |=PTP_RX_PARSE_CONFIG_IPV4_EN_ ; + +if (lan8814->hwts_tx_en && (lan8814->layer &PTP_CLASS_L4 )) + txcfg |=PTP_TX_PARSE_CONFIG_IPV4_EN_ ; + +if (lan8814_ptp_is_enable (lan8814->phydev)) +lan8814_write_page_reg (lan8814->phydev,4 ,PTP_CMD_CTL , +PTP_CMD_CTL_PTP_DISABLE_ );
Huh? If the PTP mode is enabled, you now disable it?
Quoted 6 lines
drivers/net/phy/micrel.c +6
-0+lan8814_write_page_reg (lan8814->phydev,5 ,PTP_RX_PARSE_CONFIG , rxcfg); +lan8814_write_page_reg (lan8814->phydev,5 ,PTP_TX_PARSE_CONFIG , txcfg); +lan8814_write_page_reg (lan8814->phydev,5 ,PTP_RX_TIMESTAMP_EN ,0x3 ); +lan8814_write_page_reg (lan8814->phydev,5 ,PTP_TX_TIMESTAMP_EN ,0x3 ); +lan8814_write_page_reg (lan8814->phydev,5 ,PTP_TX_PARSE_L2_ADDR_EN ,0 ); +lan8814_write_page_reg (lan8814->phydev,5 ,PTP_RX_PARSE_L2_ADDR_EN ,0 );
drivers/net/phy/micrel.c static int kszphy_resume(struct phy_device *phydev) · line 1671
Quoted 10 lines
drivers/net/phy/micrel.c +10
-0+ +lan8814_write_page_reg (lan8814->phydev,4 ,PTP_CMD_CTL ,PTP_CMD_CTL_PTP_ENABLE_ ); +if (lan8814->hwts_tx_en ==HWTSTAMP_TX_ONESTEP_SYNC ) { +lan8814_write_page_reg (lan8814->phydev,5 ,PTP_TX_MOD , +PTP_TX_MOD_TX_PTP_SYNC_TS_INSERT_ ); + }else if (lan8814->hwts_tx_en ==HWTSTAMP_TX_ON ) { +/* Enabling 2 step offloading and all option for TS insertion/correction fields */ +lan8814_write_page_reg (lan8814->phydev,5 ,PTP_TX_MOD ,0x800 ); +lan8814_config_ts_intr (lan8814->phydev); + }
Again, there is no protection here against concurrent changes from user space.
Quoted 4 lines
drivers/net/phy/micrel.c +4
-0+ +return copy_to_user (ifr->ifr_data, &config,sizeof (config)) ? -EFAULT :0 ; +} +
drivers/net/phy/micrel.c static int kszphy_resume(struct phy_device *phydev) · line 1685
Quoted 10 lines
drivers/net/phy/micrel.c +10
-0+static bool lan8814_rxtstamp (struct mii_timestamper *mii_ts,struct sk_buff *skb,int type) +{ + struct kszphy_ptp_priv *lan8814 = container_of(mii_ts, struct kszphy_ptp_priv, mii_ts); +struct skb_shared_hwtstamps *shhwtstamps =NULL ; +u32 seconds, nsec, seq_id; + +if (!lan8814->hwts_rx_en) +return false; + +if (!skb)
Remove the useless check. skb is guaranteed to be non-null.
drivers/net/phy/micrel.c static int kszphy_resume(struct phy_device *phydev) · line 1692
Quoted 7 lines
drivers/net/phy/micrel.c +7
-0+return false; + +if ((type & lan8814->version) ==0 || (type & lan8814->layer) ==0 ) +return false; + +if (lan8814->ptp.rx_filter ==HWTSTAMP_FILTER_NONE || + type ==PTP_CLASS_NONE )
Isn't this test redundant? You just checked (type & lan8814->version), and lan8814->version depends on lan8814->ptp.rx_filter.
drivers/net/phy/micrel.c static int kszphy_resume(struct phy_device *phydev) · line 1695
Quoted 3 lines
drivers/net/phy/micrel.c +3
-0+return false; + +lan8814_ptp_rx_ts_get (lan8814->phydev, &seconds, &nsec, &seq_id);
This read the time stamp unconditionally. Can't that fail? What happens when the time stamp doesn't match the skb?
Quoted 4 lines
drivers/net/phy/micrel.c +4
-0+/* Saving timestamp and sending through skbuff */ + shhwtstamps =skb_hwtstamps (skb); +memset (shhwtstamps,0 ,sizeof (*shhwtstamps)); + shhwtstamps->hwtstamp =ktime_set (seconds, nsec);
drivers/net/phy/micrel.c static int kszphy_resume(struct phy_device *phydev) · line 1710
Quoted 10 lines
drivers/net/phy/micrel.c +10
-0+ +netif_rx_ni (skb); + +return true; +} + +static int is_sync (struct sk_buff *skb,int type) +{ +u8 *data = skb->data, *msgtype; +unsigned int offset =0 ;
Please use ptp_parse_header() and ptp_get_msgtype();
Quoted 64 lines
drivers/net/phy/micrel.c +64
-0+if (type &PTP_CLASS_VLAN ) + offset +=VLAN_HLEN ; + +switch (type &PTP_CLASS_PMASK ) { +case PTP_CLASS_IPV4 : + offset +=ETH_HLEN +IPV4_HLEN (data + offset) +UDP_HLEN ; +break ; +case PTP_CLASS_IPV6 : + offset +=ETH_HLEN +IP6_HLEN +UDP_HLEN ; +break ; +case PTP_CLASS_L2 : + offset +=ETH_HLEN ; +break ; +default : +return 0 ; + } + +if (type &PTP_CLASS_V1 ) + offset +=OFF_PTP_CONTROL ; + +if (skb->len < offset +1 ) +return 0 ; + + msgtype = data + offset; + +return (*msgtype &0xf ) ==0 ; +} + +static void lan8814_txtstamp (struct mii_timestamper *mii_ts, +struct sk_buff *skb,int type) +{ + struct kszphy_ptp_priv *lan8814 = container_of(mii_ts, struct kszphy_ptp_priv, mii_ts); + +switch (lan8814->hwts_tx_en) { +case HWTSTAMP_TX_ONESTEP_SYNC : +if (is_sync (skb, type)) { +kfree_skb (skb); +return ; + } +break ; + +case HWTSTAMP_TX_ON : +skb_shinfo (skb)->tx_flags |=SKBTX_IN_PROGRESS ; +skb_queue_tail (&lan8814->ptp.tx_queue, skb); +break ; + +case HWTSTAMP_TX_OFF : +default : +kfree_skb (skb); +break ; + } +} + +static void lan8814_ptp_clock_set (struct phy_device *phydev, +u32 seconds,u32 nano_seconds, +u32 sub_nano_seconds) +{ +u32 sec_low, sec_high, nsec_low, nsec_high, snsec_low, snsec_high; + + sec_low = seconds &0xffff ; + sec_high = ((seconds >>16 ) &0xffff ); + nsec_low = nano_seconds &0xffff ; + nsec_high = ((nano_seconds >>16 ) &0x3fff ); + snsec_low = sub_nano_seconds &0xffff ;
drivers/net/phy/micrel.c static int kszphy_resume(struct phy_device *phydev) · line 1785
Quoted 10 lines
drivers/net/phy/micrel.c +10
-0+ snsec_high = ((sub_nano_seconds >>16 ) &0xffff ); + +lan8814_write_page_reg (phydev,4 ,PTP_CLOCK_SET_SEC_LO , sec_low); +lan8814_write_page_reg (phydev,4 ,PTP_CLOCK_SET_SEC_MID , sec_high); +lan8814_write_page_reg (phydev,4 ,PTP_CLOCK_SET_NS_LO , nsec_low); +lan8814_write_page_reg (phydev,4 ,PTP_CLOCK_SET_NS_HI , nsec_high); +lan8814_write_page_reg (phydev,4 ,PTP_CLOCK_SET_SUBNS_LO , snsec_low); +lan8814_write_page_reg (phydev,4 ,PTP_CLOCK_SET_SUBNS_HI , snsec_high); + +lan8814_write_page_reg (phydev,4 ,PTP_CMD_CTL ,PTP_CMD_CTL_PTP_CLOCK_LOAD_ );
Needs protection against concurrent calls to clock_settime();
Quoted 326 lines
drivers/net/phy/micrel.c +319
-0+} + +static void lan8814_ptp_clock_get (struct phy_device *phydev, +u32 *seconds,u32 *nano_seconds, +u32 *sub_nano_seconds) +{ +lan8814_write_page_reg (phydev,4 ,PTP_CMD_CTL ,PTP_CMD_CTL_PTP_CLOCK_READ_ ); + +if (seconds) { + (*seconds) =lan8814_read_page_reg (phydev,4 ,PTP_CLOCK_READ_SEC_MID ); + (*seconds) = ((*seconds) <<16 ) | +lan8814_read_page_reg (phydev,4 ,PTP_CLOCK_READ_SEC_LO ); + } + +if (nano_seconds) { + (*nano_seconds) =lan8814_read_page_reg (phydev,4 ,PTP_CLOCK_READ_NS_HI ); + (*nano_seconds) = (((*nano_seconds) &0x3fff ) <<16 ) | +lan8814_read_page_reg (phydev,4 ,PTP_CLOCK_READ_NS_LO ); + } + +if (sub_nano_seconds) { + (*sub_nano_seconds) =lan8814_read_page_reg (phydev,4 , +PTP_CLOCK_READ_SUBNS_HI ); + (*sub_nano_seconds) = ((*sub_nano_seconds) <<16 ) | +lan8814_read_page_reg (phydev,4 , +PTP_CLOCK_READ_SUBNS_LO ); + } +} + +static int lan8814_ptpci_gettime64 (struct ptp_clock_info *ptpci, +struct timespec64 *ts) +{ +struct lan8814_ptp *ptp = + container_of(ptpci, struct lan8814_ptp, ptp_clock_info); +struct kszphy_ptp_priv *priv = + container_of(ptp, struct kszphy_ptp_priv, ptp); +struct phy_device *phydev = priv->phydev; +u32 nano_seconds =0 ; +u32 seconds =0 ; + +lan8814_ptp_clock_get (phydev, &seconds, &nano_seconds,NULL ); + ts->tv_sec = seconds; + ts->tv_nsec = nano_seconds; + +return 0 ; +} + +static void lan8814_ptp_clock_step (struct phy_device *phydev, +s64 time_step_ns) +{ +u32 nano_seconds_step =0 ; +u64 abs_time_step_ns =0 ; +u32 unsigned_seconds =0 ; +u32 nano_seconds =0 ; +u32 remainder =0 ; +s32 seconds =0 ; + +if (time_step_ns >15000000000LL ) { +/* convert to clock set */ +lan8814_ptp_clock_get (phydev, &unsigned_seconds, &nano_seconds,NULL ); + unsigned_seconds +=div_u64_rem (time_step_ns,1000000000LL , + &remainder); + nano_seconds += remainder; +if (nano_seconds >=1000000000 ) { + unsigned_seconds++; + nano_seconds -=1000000000 ; + } +lan8814_ptp_clock_set (phydev, unsigned_seconds, + nano_seconds,0 ); +return ; + }else if (time_step_ns <-15000000000LL ) { +/* convert to clock set */ + time_step_ns = -time_step_ns; + +lan8814_ptp_clock_get (phydev, &unsigned_seconds, + &nano_seconds,NULL ); + unsigned_seconds -=div_u64_rem (time_step_ns,1000000000LL , + &remainder); + nano_seconds_step = remainder; +if (nano_seconds < nano_seconds_step) { + unsigned_seconds--; + nano_seconds +=1000000000 ; + } + nano_seconds -= nano_seconds_step; +lan8814_ptp_clock_set (phydev, unsigned_seconds, + nano_seconds,0 ); +return ; + } + +/* do clock step */ +if (time_step_ns >=0 ) { + abs_time_step_ns = (u64)(time_step_ns); + seconds = (s32 )div_u64_rem (abs_time_step_ns,1000000000 , + &remainder); + nano_seconds = (u32 )remainder; + }else { + abs_time_step_ns = (u64)(-time_step_ns); + seconds = -((s32 )div_u64_rem (abs_time_step_ns,1000000000 , + &remainder)); + nano_seconds = (u32 )remainder; +if (nano_seconds >0 ) { +/* subtracting nano seconds is not allowed +* convert to subtracting from seconds, +* and adding to nanoseconds +*/ + seconds--; + nano_seconds = (1000000000 - nano_seconds); + } + } + +if (nano_seconds >0 ) { +/* add 8 ns to cover the likely normal increment */ + nano_seconds +=8 ; + } + +if (nano_seconds >=1000000000 ) { +/* carry into seconds */ + seconds++; + nano_seconds -=1000000000 ; + } + +while (seconds) { +if (seconds >0 ) { +u32 adjustment_value = (u32 )seconds; +u16 adjustment_value_lo, adjustment_value_hi; + + adjustment_value_lo = adjustment_value &0xffff ; + adjustment_value_hi = (adjustment_value >>16 ) &0x3fff ; + +if (adjustment_value >0xF ) + adjustment_value =0xF ; +lan8814_write_page_reg (phydev,4 ,PTP_LTC_STEP_ADJ_LO , + adjustment_value_lo); +lan8814_write_page_reg (phydev,4 ,PTP_LTC_STEP_ADJ_HI , +PTP_LTC_STEP_ADJ_DIR_ | + adjustment_value_hi); + seconds -= ((s32 )adjustment_value); + }else { +u32 adjustment_value = (u32)(-seconds); +u16 adjustment_value_lo, adjustment_value_hi; + + adjustment_value_lo = adjustment_value &0xffff ; + adjustment_value_hi = (adjustment_value >>16 ) &0x3fff ; +if (adjustment_value >0xF ) + adjustment_value =0xF ; +lan8814_write_page_reg (phydev,4 ,PTP_LTC_STEP_ADJ_LO , + adjustment_value_lo); +lan8814_write_page_reg (phydev,4 ,PTP_LTC_STEP_ADJ_HI , +PTP_LTC_STEP_ADJ_DIR_ | + adjustment_value_hi); + seconds += ((s32 )adjustment_value); + } +lan8814_write_page_reg (phydev,4 ,PTP_CMD_CTL , +PTP_CMD_CTL_PTP_LTC_STEP_SEC_ ); + } +if (nano_seconds) { +u16 nano_seconds_lo; +u16 nano_seconds_hi; + + nano_seconds_lo = nano_seconds &0xffff ; + nano_seconds_hi = (nano_seconds >>16 ) &0x3fff ; + +lan8814_write_page_reg (phydev,4 ,PTP_LTC_STEP_ADJ_LO , + nano_seconds_lo); +lan8814_write_page_reg (phydev,4 ,PTP_LTC_STEP_ADJ_HI , +PTP_LTC_STEP_ADJ_DIR_ | + nano_seconds_hi); +lan8814_write_page_reg (phydev,4 ,PTP_CMD_CTL , +PTP_CMD_CTL_PTP_LTC_STEP_NSEC_ ); + } +} + +static int lan8814_ptpci_adjtime (struct ptp_clock_info *ptpci,s64 delta) +{ +struct lan8814_ptp *ptp = + container_of(ptpci, struct lan8814_ptp, ptp_clock_info); +struct kszphy_ptp_priv *priv = + container_of(ptp, struct kszphy_ptp_priv, ptp); +struct phy_device *phydev = priv->phydev; + +lan8814_ptp_clock_step (phydev, delta); + +return 0 ; +} + +static int lan8814_ptpci_adjfine (struct ptp_clock_info *ptpci,long scaled_ppm) +{ +struct lan8814_ptp *ptp = + container_of(ptpci, struct lan8814_ptp, ptp_clock_info); + struct kszphy_ptp_priv *priv = container_of(ptp, struct kszphy_ptp_priv, ptp); +struct phy_device *phydev = priv->phydev; +u32 kszphy_rate_adj =0 ; +u16 kszphy_rate_adj_lo =0 , kszphy_rate_adj_hi =0 ; +bool positive = true; +u64 u64_delta =0 ; + +if ((scaled_ppm < (-LAN8814_PTP_MAX_FINE_ADJ_IN_SCALED_PPM )) || + scaled_ppm >LAN8814_PTP_MAX_FINE_ADJ_IN_SCALED_PPM ) { +return -EINVAL ; + } +if (scaled_ppm >0 ) { + u64_delta = (u64 )scaled_ppm; + positive = true; + }else { + u64_delta = (u64)(-scaled_ppm); + positive = false; + } + u64_delta = (u64_delta <<13 ); + kszphy_rate_adj =div_u64 (u64_delta,15625 ); + + kszphy_rate_adj_lo = kszphy_rate_adj &0xffff ; + kszphy_rate_adj_hi = (kszphy_rate_adj >>16 ) &0x3fff ; + +if (positive) + kszphy_rate_adj_hi |=PTP_CLOCK_RATE_ADJ_DIR_ ; + +lan8814_write_page_reg (phydev,4 ,PTP_CLOCK_RATE_ADJ_HI_ , kszphy_rate_adj_hi); +lan8814_write_page_reg (phydev,4 ,PTP_CLOCK_RATE_ADJ_LO_ , kszphy_rate_adj_lo); + +return 0 ; +} + +static void lan8814_ptp_reset (struct phy_device *phydev) +{ +if (lan8814_ptp_is_enable (phydev)) +lan8814_write_page_reg (phydev,4 ,PTP_CMD_CTL ,PTP_CMD_CTL_PTP_DISABLE_ ); + +lan8814_write_page_reg (phydev,4 ,LTC_HARD_RESET ,LTC_HARD_RESET_ ); +lan8814_write_page_reg (phydev,5 ,TSU_HARD_RESET ,TSU_HARD_RESET_ ); +} + +static void lan8814_ptp_enable (struct phy_device *phydev) +{ +if (lan8814_ptp_is_enable (phydev)) +return ; + +lan8814_write_page_reg (phydev,4 ,PTP_CMD_CTL ,PTP_CMD_CTL_PTP_ENABLE_ ); +} + +static void lan8814_ptp_sync_to_system_clock (struct phy_device *phydev) +{ +struct timespec64 ts; + +ktime_get_clocktai_ts64 (&ts); + +lan8814_ptp_clock_set (phydev, ts.tv_sec, ts.tv_nsec,0 ); +} + +static void lan8814_ptp_init (struct phy_device *phydev) +{ +u32 temp; + +lan8814_ptp_reset (phydev); +lan8814_ptp_sync_to_system_clock (phydev); + temp =lan8814_read_page_reg (phydev,5 ,PTP_TX_MOD ); + temp |=PTP_TX_MOD_BAD_UDPV4_CHKSUM_FORCE_FCS_DIS_ ; +lan8814_write_page_reg (phydev,5 ,PTP_TX_MOD , temp); +lan8814_write_page_reg (phydev,4 ,PTP_OPERATING_MODE , +PTP_OPERATING_MODE_STANDALONE ); +lan8814_ptp_enable (phydev); +} +static int kszphy_probe (struct phy_device *phydev) {const struct kszphy_type *type = phydev->drv->driver_data;@@ -1248,6 +2106,88 @@ static int kszphy_probe(struct phy_device *phydev)return 0 ; } +static int lan8814_probe (struct phy_device *phydev) +{ +struct kszphy_priv *priv; +struct clk *clk; +const struct kszphy_type *type = phydev->drv->driver_data; +const struct device_node *np = phydev->mdio.dev.of_node; + + priv =devm_kzalloc (&phydev->mdio.dev,sizeof (*priv),GFP_KERNEL ); +if (!priv) +return -ENOMEM ; + +skb_queue_head_init (&priv->ptp_priv.ptp.tx_queue); + + priv->ptp_priv.phydev = phydev; + + priv->type = type; + + priv->ptp_priv.mii_ts.rxtstamp = lan8814_rxtstamp; + priv->ptp_priv.mii_ts.txtstamp = lan8814_txtstamp; + priv->ptp_priv.mii_ts.hwtstamp = lan8814_hwtstamp; + priv->ptp_priv.mii_ts.ts_info = lan8814_ts_info; + + phydev->mii_ts = &priv->ptp_priv.mii_ts; + + phydev->priv = priv; + +lan8814_ptp_init (phydev); + + priv->ptp_priv.ptp.flags |=PTP_FLAG_ISR_ENABLED ; + +if (!IS_ENABLED (CONFIG_PTP_1588_CLOCK )) +return 0 ; + + clk =devm_clk_get (&phydev->mdio.dev,"rmii-ref" ); +/* NOTE: clk may be NULL if building without CONFIG_HAVE_CLK */ +if (!IS_ERR_OR_NULL (clk)) { +unsigned long rate =clk_get_rate (clk); +bool rmii_ref_clk_sel_25_mhz; + + priv->rmii_ref_clk_sel = type->has_rmii_ref_clk_sel; + rmii_ref_clk_sel_25_mhz =of_property_read_bool (np, +"micrel,rmii-reference-clock-select-25-mhz" ); + +if (rate >24500000 && rate <25500000 ) { + priv->rmii_ref_clk_sel_val = rmii_ref_clk_sel_25_mhz; + }else if (rate >49500000 && rate <50500000 ) { + priv->rmii_ref_clk_sel_val = !rmii_ref_clk_sel_25_mhz; + }else { +phydev_err (phydev,"Clock rate out of range: %ld\n" , + rate); +return -EINVAL ; + } + } + + priv->ptp_priv.ptp.ptp_clock_info.owner =THIS_MODULE ; +snprintf (priv->ptp_priv.ptp.ptp_clock_info.name,30 ,"%s" , + phydev->drv->name);
drivers/net/phy/micrel.c static int kszphy_probe(struct phy_device *phydev) · line 2175
Quoted 10 lines
drivers/net/phy/micrel.c +10
-0+ priv->ptp_priv.ptp.ptp_clock_info.max_adj =LAN8814_PTP_MAX_FREQ_ADJ_IN_PPB ; + priv->ptp_priv.ptp.ptp_clock_info.n_alarm =0 ; + priv->ptp_priv.ptp.ptp_clock_info.n_ext_ts =0 ; + priv->ptp_priv.ptp.ptp_clock_info.n_per_out =1 ; + priv->ptp_priv.ptp.ptp_clock_info.n_pins =0 ; + priv->ptp_priv.ptp.ptp_clock_info.pps =0 ; + priv->ptp_priv.ptp.ptp_clock_info.pin_config =NULL ; + priv->ptp_priv.ptp.ptp_clock_info.adjfine = lan8814_ptpci_adjfine; + priv->ptp_priv.ptp.ptp_clock_info.adjtime = lan8814_ptpci_adjtime; + priv->ptp_priv.ptp.ptp_clock_info.gettime64 = lan8814_ptpci_gettime64;
Don't forget about settime64()!
Thanks,
Richard
Quoted 30 lines
drivers/net/phy/micrel.c +16
-1+ priv->ptp_priv.ptp.ptp_clock_info.getcrosststamp =NULL ; + + priv->ptp_priv.ptp.ptp_clock =ptp_clock_register (&priv->ptp_priv.ptp.ptp_clock_info, + &phydev->mdio.dev); + +if (IS_ERR_OR_NULL (priv->ptp_priv.ptp.ptp_clock)) +phydev_err (phydev,"ptp_clock_register failed %lu\n" , +PTR_ERR (priv->ptp_priv.ptp.ptp_clock)); + + priv->ptp_priv.ptp.flags |=PTP_FLAG_PTP_CLOCK_REGISTERED ; +phydev_info (phydev,"successfully registered ptp clock\n" ); + +return 0 ; +} +static struct phy_driver ksphy_driver[] = { { .phy_id =PHY_ID_KS8737 ,@@ -1415,7 +2355,7 @@ static struct phy_driver ksphy_driver[] = { .phy_id_mask = MICREL_PHY_ID_MASK, .name = "Microchip INDY Gigabit Quad PHY", .driver_data = &ksz9021_type, - .probe = kszphy_probe, + .probe = lan8814_probe, .soft_reset = genphy_soft_reset, .read_status = ksz9031_read_status, .get_sset_count = kszphy_get_sset_count,Signature · 1 line
2.17.1