[net-next,RFC,V1,5/5] net: mdio: Add a driver for InES time stamping IP core.

Message ID	e9def53dd0f751d7f5e3df47e604d7ccb6020159.1521656774.git.richardcochran@gmail.com
State	Changes Requested, archived
Headers	show Return-Path: <devicetree-owner@vger.kernel.org> From: Richard Cochran <richardcochran@gmail.com> To: netdev@vger.kernel.org Cc: devicetree@vger.kernel.org, Andrew Lunn <andrew@lunn.ch>, David Miller <davem@davemloft.net>, Florian Fainelli <f.fainelli@gmail.com>, Mark Rutland <mark.rutland@arm.com>, Miroslav Lichvar <mlichvar@redhat.com>, Rob Herring <robh+dt@kernel.org>, Willem de Bruijn <willemb@google.com> Subject: [PATCH net-next RFC V1 5/5] net: mdio: Add a driver for InES time stamping IP core. Date: Wed, 21 Mar 2018 11:58:18 -0700 Message-Id: <e9def53dd0f751d7f5e3df47e604d7ccb6020159.1521656774.git.richardcochran@gmail.com> In-Reply-To: <cover.1521656774.git.richardcochran@gmail.com> References: <cover.1521656774.git.richardcochran@gmail.com> Sender: devicetree-owner@vger.kernel.org Precedence: bulk
Series	Peer to Peer One-Step time stamping \| expand [net-next,RFC,V1,0/5] Peer to Peer One-Step time stamping [net-next,RFC,V1,5/5] net: mdio: Add a driver for InES time stamping IP core.

diff --git a/Documentation/devicetree/bindings/net/ines-ptp.txt b/Documentation/devicetree/bindings/net/ines-ptp.txt new file mode 100644 index 000000000000..ed7b1d773ded --- /dev/null +++ b/Documentation/devicetree/bindings/net/ines-ptp.txt @@ -0,0 +1,42 @@ +ZHAW InES PTP time stamping IP core + +The IP core needs two different kinds of nodes. The control node +lives somewhere in the memory map and specifies the address of the +control registers. There can be up to three port nodes placed on the +mdio bus. They associate a particular MAC with a port index within +the IP core. + +Required properties of the control node: + +- compatible: "ines,ptp-ctrl" +- reg: physical address and size of the register bank +- phandle: globally unique handle for the ports to point to + +Required properties of the port nodes: + +- compatible: "ines,ptp-port" +- ctrl-handle: points to the control node +- port-index: port channel within the IP core +- reg: phy address. This is required even though the + device does not respond to mdio operations + +Example: + + timestamper@60000000 { + compatible = "ines,ptp-ctrl"; + reg = <0x60000000 0x80>; + phandle = <0x10>; + }; + + ethernet@80000000 { + ... + mdio { + ... + timestamper@1f { + compatible = "ines,ptp-port"; + ctrl-handle = <0x10>; + port-index = <0>; + reg = <0x1f>; + }; + }; + }; diff --git a/drivers/net/phy/Makefile b/drivers/net/phy/Makefile index 01acbcb2c798..e286bb822295 100644 --- a/drivers/net/phy/Makefile +++ b/drivers/net/phy/Makefile @@ -61,6 +61,7 @@ obj-$(CONFIG_DP83848_PHY) += dp83848.o obj-$(CONFIG_DP83867_PHY) += dp83867.o obj-$(CONFIG_FIXED_PHY) += fixed_phy.o obj-$(CONFIG_ICPLUS_PHY) += icplus.o +obj-$(CONFIG_INES_PTP_TSTAMP) += ines_ptp.o obj-$(CONFIG_INTEL_XWAY_PHY) += intel-xway.o obj-$(CONFIG_LSI_ET1011C_PHY) += et1011c.o obj-$(CONFIG_LXT_PHY) += lxt.o diff --git a/drivers/net/phy/ines_ptp.c b/drivers/net/phy/ines_ptp.c new file mode 100644 index 000000000000..4f66459d4417 --- /dev/null +++ b/drivers/net/phy/ines_ptp.c @@ -0,0 +1,857 @@ +// SPDX-License-Identifier: GPL-2.0 +/* + * Copyright (C) 2018 MOSER-BAER AG + */ +#define pr_fmt(fmt) "InES_PTP: " fmt + +#include <linux/ethtool.h> +#include <linux/export.h> +#include <linux/if_vlan.h> +#include <linux/mdio.h> +#include <linux/module.h> +#include <linux/net_tstamp.h> +#include <linux/of.h> +#include <linux/of_address.h> +#include <linux/of_irq.h> +#include <linux/platform_device.h> +#include <linux/ptp_classify.h> +#include <linux/ptp_clock_kernel.h> +#include <linux/stddef.h> + +MODULE_DESCRIPTION("Driver for the ZHAW InES PTP time stamping IP core"); +MODULE_AUTHOR("Richard Cochran <richardcochran@gmail.com>"); +MODULE_VERSION("1.0"); +MODULE_LICENSE("GPL"); + +/* GLOBAL register */ +#define MCAST_MAC_SELECT_SHIFT 2 +#define MCAST_MAC_SELECT_MASK 0x3 +#define IO_RESET BIT(1) +#define PTP_RESET BIT(0) + +/* VERSION register */ +#define IF_MAJOR_VER_SHIFT 12 +#define IF_MAJOR_VER_MASK 0xf +#define IF_MINOR_VER_SHIFT 8 +#define IF_MINOR_VER_MASK 0xf +#define FPGA_MAJOR_VER_SHIFT 4 +#define FPGA_MAJOR_VER_MASK 0xf +#define FPGA_MINOR_VER_SHIFT 0 +#define FPGA_MINOR_VER_MASK 0xf + +/* INT_STAT register */ +#define RX_INTR_STATUS_3 BIT(5) +#define RX_INTR_STATUS_2 BIT(4) +#define RX_INTR_STATUS_1 BIT(3) +#define TX_INTR_STATUS_3 BIT(2) +#define TX_INTR_STATUS_2 BIT(1) +#define TX_INTR_STATUS_1 BIT(0) + +/* INT_MSK register */ +#define RX_INTR_MASK_3 BIT(5) +#define RX_INTR_MASK_2 BIT(4) +#define RX_INTR_MASK_1 BIT(3) +#define TX_INTR_MASK_3 BIT(2) +#define TX_INTR_MASK_2 BIT(1) +#define TX_INTR_MASK_1 BIT(0) + +/* BUF_STAT register */ +#define RX_FIFO_NE_3 BIT(5) +#define RX_FIFO_NE_2 BIT(4) +#define RX_FIFO_NE_1 BIT(3) +#define TX_FIFO_NE_3 BIT(2) +#define TX_FIFO_NE_2 BIT(1) +#define TX_FIFO_NE_1 BIT(0) + +/* PORT_CONF register */ +#define CM_ONE_STEP BIT(6) +#define PHY_SPEED_SHIFT 4 +#define PHY_SPEED_MASK 0x3 +#define P2P_DELAY_WR_POS_SHIFT 2 +#define P2P_DELAY_WR_POS_MASK 0x3 +#define PTP_MODE_SHIFT 0 +#define PTP_MODE_MASK 0x3 + +/* TS_STAT_TX register */ +#define TS_ENABLE BIT(15) +#define DATA_READ_POS_SHIFT 8 +#define DATA_READ_POS_MASK 0x1f +#define DISCARDED_EVENTS_SHIFT 4 +#define DISCARDED_EVENTS_MASK 0xf + +#define INES_N_PORTS 3 +#define INES_REGISTER_SIZE 0x80 +#define INES_PORT_OFFSET 0x20 +#define INES_PORT_SIZE 0x20 +#define INES_FIFO_DEPTH 90 +#define INES_MAX_EVENTS 100 + +#define BC_PTP_V1 0 +#define BC_PTP_V2 1 +#define TC_E2E_PTP_V2 2 +#define TC_P2P_PTP_V2 3 + +#define OFF_PTP_CLOCK_ID 20 +#define OFF_PTP_PORT_NUM 28 + +#define PHY_SPEED_10 0 +#define PHY_SPEED_100 1 +#define PHY_SPEED_1000 2 + +#define PORT_CONF \ + ((PHY_SPEED_1000 << PHY_SPEED_SHIFT) | (BC_PTP_V2 << PTP_MODE_SHIFT)) + +#define ines_read32(s, r) __raw_readl(&s->regs->r) +#define ines_write32(s, v, r) __raw_writel(v, &s->regs->r) + +#define MESSAGE_TYPE_SYNC 1 +#define MESSAGE_TYPE_P_DELAY_REQ 2 +#define MESSAGE_TYPE_P_DELAY_RESP 3 +#define MESSAGE_TYPE_DELAY_REQ 4 + +#define SYNC 0x0 +#define DELAY_REQ 0x1 +#define PDELAY_REQ 0x2 +#define PDELAY_RESP 0x3 + +static LIST_HEAD(ines_clocks); +static DEFINE_MUTEX(ines_clocks_lock); + +struct ines_global_registers { + u32 id; + u32 test; + u32 global; + u32 version; + u32 test2; + u32 int_stat; + u32 int_msk; + u32 buf_stat; +}; + +struct ines_port_registers { + u32 port_conf; + u32 p_delay; + u32 ts_stat_tx; + u32 ts_stat_rx; + u32 ts_tx; + u32 ts_rx; +}; + +struct ines_timestamp { + struct list_head list; + unsigned long tmo; + u16 tag; + u64 sec; + u64 nsec; + u64 clkid; + u16 portnum; + u16 seqid; +}; + +struct ines_port { + struct ines_port_registers *regs; + struct ines_clock *clock; + bool rxts_enabled; + bool txts_enabled; + unsigned int index; + struct delayed_work ts_work; + /* lock protects event list and tx_skb */ + spinlock_t lock; + struct sk_buff *tx_skb; + struct list_head events; + struct list_head pool; + struct ines_timestamp pool_data[INES_MAX_EVENTS]; +}; + +struct ines_clock { + struct ines_port port[INES_N_PORTS]; + struct ines_global_registers *regs; + void __iomem *base; + struct device_node *node; + struct list_head list; +}; + +static bool ines_match(struct sk_buff *skb, unsigned int ptp_class, + struct ines_timestamp *ts); +static int ines_rxfifo_read(struct ines_port *port); +static u64 ines_rxts64(struct ines_port *port, unsigned int words); +static bool ines_timestamp_expired(struct ines_timestamp *ts); +static u64 ines_txts64(struct ines_port *port, unsigned int words); +static void ines_txtstamp_work(struct work_struct *work); +static bool is_sync_pdelay_resp(struct sk_buff *skb, int type); +static u8 tag_to_msgtype(u8 tag); + +static void ines_clock_cleanup(struct ines_clock *clock) +{ + struct ines_port *port; + int i; + + for (i = 0; i < INES_N_PORTS; i++) { + port = &clock->port[i]; + cancel_delayed_work_sync(&port->ts_work); + } +} + +static int ines_clock_init(struct ines_clock *clock, struct device_node *node, + void __iomem *addr) +{ + unsigned long port_addr; + struct ines_port *port; + int i, j; + + INIT_LIST_HEAD(&clock->list); + clock->node = node; + clock->base = addr; + clock->regs = clock->base; + + for (i = 0; i < INES_N_PORTS; i++) { + port = &clock->port[i]; + port_addr = (unsigned long) clock->base + + INES_PORT_OFFSET + i * INES_PORT_SIZE; + port->regs = (struct ines_port_registers *) port_addr; + port->clock = clock; + port->index = i; + INIT_DELAYED_WORK(&port->ts_work, ines_txtstamp_work); + spin_lock_init(&port->lock); + INIT_LIST_HEAD(&port->events); + INIT_LIST_HEAD(&port->pool); + for (j = 0; j < INES_MAX_EVENTS; j++) + list_add(&port->pool_data[j].list, &port->pool); + } + + ines_write32(clock, 0xBEEF, test); + ines_write32(clock, 0xBEEF, test2); + + pr_debug("ID 0x%x\n", ines_read32(clock, id)); + pr_debug("TEST 0x%x\n", ines_read32(clock, test)); + pr_debug("VERSION 0x%x\n", ines_read32(clock, version)); + pr_debug("TEST2 0x%x\n", ines_read32(clock, test2)); + + for (i = 0; i < INES_N_PORTS; i++) { + port = &clock->port[i]; + ines_write32(port, PORT_CONF, port_conf); + } + + return 0; +} + +static void ines_dump_ts(char *label, struct ines_timestamp *ts) +{ +#ifdef DEBUG + pr_err("%s timestamp, tag=0x%04hx t=%llu.%9llu c=0x%llx p=%hu s=%hu\n", + label, ts->tag, ts->sec, ts->nsec, + ts->clkid, ts->portnum, ts->seqid); +#endif +} + +static struct ines_port *ines_find_port(struct device_node *node, u32 index) +{ + struct ines_port *port = NULL; + struct ines_clock *clock; + struct list_head *this; + + if (index > INES_N_PORTS - 1) + return NULL; + + mutex_lock(&ines_clocks_lock); + list_for_each(this, &ines_clocks) { + clock = list_entry(this, struct ines_clock, list); + if (clock->node == node) { + port = &clock->port[index]; + break; + } + } + mutex_unlock(&ines_clocks_lock); + return port; +} + +static u64 ines_find_rxts(struct ines_port *port, struct sk_buff *skb, int type) +{ + struct list_head *this, *next; + struct ines_timestamp *ts; + unsigned long flags; + u64 ns = 0; + + if (type == PTP_CLASS_NONE) + return 0; + + spin_lock_irqsave(&port->lock, flags); + ines_rxfifo_read(port); + list_for_each_safe(this, next, &port->events) { + ts = list_entry(this, struct ines_timestamp, list); + if (ines_timestamp_expired(ts)) { + list_del_init(&ts->list); + list_add(&ts->list, &port->pool); + continue; + } + if (ines_match(skb, type, ts)) { + ns = ts->sec * 1000000000ULL + ts->nsec; + list_del_init(&ts->list); + list_add(&ts->list, &port->pool); + break; + } + } + spin_unlock_irqrestore(&port->lock, flags); + + return ns; +} + +static u64 ines_find_txts(struct ines_port *port, struct sk_buff *skb) +{ + unsigned int class = ptp_classify_raw(skb), i; + u32 data_rd_pos, buf_stat, mask, ts_stat_tx; + struct ines_timestamp ts; + unsigned long flags; + u64 ns = 0; + + mask = TX_FIFO_NE_1 << port->index; + + spin_lock_irqsave(&port->lock, flags); + + for (i = 0; i < INES_FIFO_DEPTH; i++) { + + buf_stat = ines_read32(port->clock, buf_stat); + if (!(buf_stat & mask)) { + pr_debug("Tx timestamp FIFO unexpectedly empty\n"); + break; + } + ts_stat_tx = ines_read32(port, ts_stat_tx); + data_rd_pos = (ts_stat_tx >> DATA_READ_POS_SHIFT) & + DATA_READ_POS_MASK; + if (data_rd_pos) { + pr_err("unexpected Tx read pos %u\n", data_rd_pos); + break; + } + + ts.tag = ines_read32(port, ts_tx); + ts.sec = ines_txts64(port, 3); + ts.nsec = ines_txts64(port, 2); + ts.clkid = ines_txts64(port, 4); + ts.portnum = ines_read32(port, ts_tx); + ts.seqid = ines_read32(port, ts_tx); + + ines_dump_ts("Tx", &ts); + + if (ines_match(skb, class, &ts)) { + ns = ts.sec * 1000000000ULL + ts.nsec; + break; + } + } + + spin_unlock_irqrestore(&port->lock, flags); + return ns; +} + +static int ines_hwtstamp(struct mdio_device *m, struct ifreq *ifr) +{ + u32 cm_one_step = 0, port_conf, ts_stat_rx, ts_stat_tx; + struct ines_port *port = dev_get_drvdata(&m->dev); + struct hwtstamp_config cfg; + unsigned long flags; + + if (copy_from_user(&cfg, ifr->ifr_data, sizeof(cfg))) + return -EFAULT; + + /* reserved for future extensions */ + if (cfg.flags) + return -EINVAL; + + switch (cfg.tx_type) { + case HWTSTAMP_TX_OFF: + ts_stat_tx = 0; + break; + case HWTSTAMP_TX_ON: + ts_stat_tx = TS_ENABLE; + break; + case HWTSTAMP_TX_ONESTEP_P2P: + ts_stat_tx = TS_ENABLE; + cm_one_step = CM_ONE_STEP; + break; + default: + return -ERANGE; + } + + switch (cfg.rx_filter) { + case HWTSTAMP_FILTER_NONE: + ts_stat_rx = 0; + break; + case HWTSTAMP_FILTER_ALL: + case HWTSTAMP_FILTER_PTP_V1_L4_EVENT: + case HWTSTAMP_FILTER_PTP_V1_L4_SYNC: + case HWTSTAMP_FILTER_PTP_V1_L4_DELAY_REQ: + return -ERANGE; + 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_EVENT: + case HWTSTAMP_FILTER_PTP_V2_SYNC: + case HWTSTAMP_FILTER_PTP_V2_DELAY_REQ: + ts_stat_rx = TS_ENABLE; + cfg.rx_filter = HWTSTAMP_FILTER_PTP_V2_EVENT; + break; + default: + return -ERANGE; + } + + spin_lock_irqsave(&port->lock, flags); + + port_conf = ines_read32(port, port_conf); + port_conf &= ~CM_ONE_STEP; + port_conf |= cm_one_step; + + ines_write32(port, port_conf, port_conf); + ines_write32(port, ts_stat_rx, ts_stat_rx); + ines_write32(port, ts_stat_tx, ts_stat_tx); + + port->rxts_enabled = ts_stat_rx == TS_ENABLE ? true : false; + port->txts_enabled = ts_stat_tx == TS_ENABLE ? true : false; + + spin_unlock_irqrestore(&port->lock, flags); + + return copy_to_user(ifr->ifr_data, &cfg, sizeof(cfg)) ? -EFAULT : 0; +} + +static bool ines_match(struct sk_buff *skb, unsigned int ptp_class, + struct ines_timestamp *ts) +{ + u8 *msgtype, *data = skb_mac_header(skb); + unsigned int offset = 0; + u16 *portn, *seqid; + u64 *clkid; + + if (unlikely(ptp_class & PTP_CLASS_V1)) + return false; + + if (ptp_class & PTP_CLASS_VLAN) + offset += VLAN_HLEN; + + switch (ptp_class & 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 false; + } + + if (skb->len + ETH_HLEN < offset + OFF_PTP_SEQUENCE_ID + sizeof(*seqid)) + return false; + + msgtype = data + offset; + clkid = (u64 *)(data + offset + OFF_PTP_CLOCK_ID); + portn = (u16 *)(data + offset + OFF_PTP_PORT_NUM); + seqid = (u16 *)(data + offset + OFF_PTP_SEQUENCE_ID); + + if (tag_to_msgtype(ts->tag & 0x7) != (*msgtype & 0xf)) { + pr_debug("msgtype mismatch ts %hhu != skb %hhu\n", + tag_to_msgtype(ts->tag & 0x7), *msgtype & 0xf); + return false; + } + if (cpu_to_be64(ts->clkid) != *clkid) { + pr_debug("clkid mismatch ts %llx != skb %llx\n", + cpu_to_be64(ts->clkid), *clkid); + return false; + } + if (ts->portnum != ntohs(*portn)) { + pr_debug("portn mismatch ts %hu != skb %hu\n", + ts->portnum, ntohs(*portn)); + return false; + } + if (ts->seqid != ntohs(*seqid)) { + pr_debug("seqid mismatch ts %hu != skb %hu\n", + ts->seqid, ntohs(*seqid)); + return false; + } + + return true; +} + +static bool ines_rxtstamp(struct mdio_device *m, struct sk_buff *skb, int type) +{ + struct ines_port *port = dev_get_drvdata(&m->dev); + struct skb_shared_hwtstamps *ssh; + u64 ns; + + if (!port->rxts_enabled) + return false; + + ns = ines_find_rxts(port, skb, type); + if (!ns) + return false; + + ssh = skb_hwtstamps(skb); + ssh->hwtstamp = ns_to_ktime(ns); + netif_rx(skb); + + return true; +} + +static int ines_rxfifo_read(struct ines_port *port) +{ + u32 data_rd_pos, buf_stat, mask, ts_stat_rx; + struct ines_timestamp *ts; + unsigned int i; + + mask = RX_FIFO_NE_1 << port->index; + + for (i = 0; i < INES_FIFO_DEPTH; i++) { + if (list_empty(&port->pool)) { + pr_err("event pool is empty\n"); + return -1; + } + buf_stat = ines_read32(port->clock, buf_stat); + if (!(buf_stat & mask)) + break; + + ts_stat_rx = ines_read32(port, ts_stat_rx); + data_rd_pos = (ts_stat_rx >> DATA_READ_POS_SHIFT) & + DATA_READ_POS_MASK; + if (data_rd_pos) { + pr_err("unexpected Rx read pos %u\n", data_rd_pos); + break; + } + + ts = list_first_entry(&port->pool, struct ines_timestamp, list); + ts->tmo = jiffies + HZ; + ts->tag = ines_read32(port, ts_rx); + ts->sec = ines_rxts64(port, 3); + ts->nsec = ines_rxts64(port, 2); + ts->clkid = ines_rxts64(port, 4); + ts->portnum = ines_read32(port, ts_rx); + ts->seqid = ines_read32(port, ts_rx); + + ines_dump_ts("Rx", ts); + + list_del_init(&ts->list); + list_add_tail(&ts->list, &port->events); + } + + return 0; +} + +static u64 ines_rxts64(struct ines_port *port, unsigned int words) +{ + unsigned int i; + u64 result; + u16 word; + + word = ines_read32(port, ts_rx); + result = word; + words--; + for (i = 0; i < words; i++) { + word = ines_read32(port, ts_rx); + result <<= 16; + result |= word; + } + return result; +} + +static bool ines_timestamp_expired(struct ines_timestamp *ts) +{ + return time_after(jiffies, ts->tmo); +} + +static int ines_ts_info(struct mdio_device *m, struct ethtool_ts_info *info) +{ + info->so_timestamping = + SOF_TIMESTAMPING_TX_HARDWARE | + SOF_TIMESTAMPING_TX_SOFTWARE | + SOF_TIMESTAMPING_RX_HARDWARE | + SOF_TIMESTAMPING_RX_SOFTWARE | + SOF_TIMESTAMPING_SOFTWARE | + SOF_TIMESTAMPING_RAW_HARDWARE; + + info->phc_index = -1; + + info->tx_types = + (1 << HWTSTAMP_TX_OFF) | + (1 << HWTSTAMP_TX_ON) | + (1 << HWTSTAMP_TX_ONESTEP_P2P); + + info->rx_filters = + (1 << HWTSTAMP_FILTER_NONE) | + (1 << HWTSTAMP_FILTER_PTP_V2_EVENT); + + return 0; +} + +static u64 ines_txts64(struct ines_port *port, unsigned int words) +{ + unsigned int i; + u64 result; + u16 word; + + word = ines_read32(port, ts_tx); + result = word; + words--; + for (i = 0; i < words; i++) { + word = ines_read32(port, ts_tx); + result <<= 16; + result |= word; + } + return result; +} + +static bool ines_txts_onestep(struct ines_port *port, struct sk_buff *skb, int type) +{ + unsigned long flags; + u32 port_conf; + + spin_lock_irqsave(&port->lock, flags); + port_conf = ines_read32(port, port_conf); + spin_unlock_irqrestore(&port->lock, flags); + + if (port_conf & CM_ONE_STEP) + return is_sync_pdelay_resp(skb, type); + + return false; +} + +static void ines_txtstamp(struct mdio_device *m, struct sk_buff *skb, int type) +{ + struct ines_port *port = dev_get_drvdata(&m->dev); + struct sk_buff *old_skb; + unsigned long flags; + + if (!port->txts_enabled || ines_txts_onestep(port, skb, type)) { + kfree_skb(skb); + return; + } + + spin_lock_irqsave(&port->lock, flags); + + if (port->tx_skb) + old_skb = port->tx_skb; + + port->tx_skb = skb; + + spin_unlock_irqrestore(&port->lock, flags); + + if (old_skb) + kfree_skb(old_skb); + + schedule_delayed_work(&port->ts_work, 1); +} + +static void ines_txtstamp_work(struct work_struct *work) +{ + struct ines_port *port = + container_of(work, struct ines_port, ts_work.work); + struct skb_shared_hwtstamps ssh; + struct sk_buff *skb; + unsigned long flags; + u64 ns; + + spin_lock_irqsave(&port->lock, flags); + skb = port->tx_skb; + port->tx_skb = NULL; + spin_unlock_irqrestore(&port->lock, flags); + + ns = ines_find_txts(port, skb); + if (!ns) { + kfree_skb(skb); + return; + } + ssh.hwtstamp = ns_to_ktime(ns); + skb_complete_tx_timestamp(skb, &ssh); +} + +static bool is_sync_pdelay_resp(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; + + switch ((*msgtype & 0xf)) { + case SYNC: + case PDELAY_RESP: + return true; + default: + return false; + } +} + +static u8 tag_to_msgtype(u8 tag) +{ + switch (tag) { + case MESSAGE_TYPE_SYNC: + return SYNC; + case MESSAGE_TYPE_P_DELAY_REQ: + return PDELAY_REQ; + case MESSAGE_TYPE_P_DELAY_RESP: + return PDELAY_RESP; + case MESSAGE_TYPE_DELAY_REQ: + return DELAY_REQ; + } + return 0xf; +} + +static int ines_ptp_ctrl_probe(struct platform_device *pld) +{ + struct ines_clock *clock; + struct resource *res; + void __iomem *addr; + int err = 0; + + res = platform_get_resource(pld, IORESOURCE_MEM, 0); + if (!res) { + dev_err(&pld->dev, "missing memory resource\n"); + return -EINVAL; + } + addr = devm_ioremap_resource(&pld->dev, res); + if (IS_ERR(addr)) { + err = PTR_ERR(addr); + goto out; + } + clock = kzalloc(sizeof(*clock), GFP_KERNEL); + if (!clock) { + err = -ENOMEM; + goto out; + } + if (ines_clock_init(clock, pld->dev.of_node, addr)) { + kfree(clock); + err = -ENOMEM; + goto out; + } + mutex_lock(&ines_clocks_lock); + list_add_tail(&ines_clocks, &clock->list); + mutex_unlock(&ines_clocks_lock); + + dev_set_drvdata(&pld->dev, clock); +out: + return err; +} + +static int ines_ptp_ctrl_remove(struct platform_device *pld) +{ + struct ines_clock *clock = dev_get_drvdata(&pld->dev); + + mutex_lock(&ines_clocks_lock); + list_del(&clock->list); + mutex_unlock(&ines_clocks_lock); + ines_clock_cleanup(clock); + kfree(clock); + return 0; +} + +static int ines_ptp_port_probe(struct mdio_device *mdiodev) +{ + struct device_node *node; + struct ines_port *port; + int err = 0; + u32 index; + + if (of_property_read_u32(mdiodev->dev.of_node, "port-index", &index)) { + dev_err(&mdiodev->dev, "missing port-index\n"); + return -EINVAL; + } + node = of_parse_phandle(mdiodev->dev.of_node, "ctrl-handle", 0); + if (IS_ERR(node)) { + dev_err(&mdiodev->dev, "missing ctrl-handle\n"); + return PTR_ERR(node); + } + port = ines_find_port(node, index); + if (!port) { + dev_err(&mdiodev->dev, "missing port\n"); + err = -ENODEV; + goto out; + } + mdiodev->ts_info = ines_ts_info; + mdiodev->hwtstamp = ines_hwtstamp; + mdiodev->rxtstamp = ines_rxtstamp; + mdiodev->txtstamp = ines_txtstamp; + dev_set_drvdata(&mdiodev->dev, port); +out: + of_node_put(node); + return err; +} + +static void ines_ptp_port_remove(struct mdio_device *mdiodev) +{ +} + +static const struct of_device_id ines_ptp_ctrl_of_match[] = { + { .compatible = "ines,ptp-ctrl" }, + { } +}; + +MODULE_DEVICE_TABLE(of, ines_ptp_ctrl_of_match); + +static struct platform_driver ines_ptp_ctrl_driver = { + .probe = ines_ptp_ctrl_probe, + .remove = ines_ptp_ctrl_remove, + .driver = { + .name = "ines_ptp_ctrl", + .of_match_table = of_match_ptr(ines_ptp_ctrl_of_match), + }, +}; + +static const struct of_device_id ines_ptp_port_of_match[] = { + { .compatible = "ines,ptp-port" }, + { } +}; + +static struct mdio_driver ines_ptp_port_driver = { + .probe = ines_ptp_port_probe, + .remove = ines_ptp_port_remove, + .mdiodrv.driver = { + .name = "ines_ptp_port", + .of_match_table = ines_ptp_port_of_match, + }, +}; + +static int __init ines_ptp_init(void) +{ + int err; + + err = platform_driver_register(&ines_ptp_ctrl_driver); + if (err) + return err; + + err = mdio_driver_register(&ines_ptp_port_driver); + if (err) + platform_driver_unregister(&ines_ptp_ctrl_driver); + + return err; +} + +static void __exit ines_ptp_cleanup(void) +{ + mdio_driver_unregister(&ines_ptp_port_driver); + platform_driver_unregister(&ines_ptp_ctrl_driver); +} + +module_init(ines_ptp_init); +module_exit(ines_ptp_cleanup); diff --git a/drivers/ptp/Kconfig b/drivers/ptp/Kconfig index a21ad10d613c..10ef161dd2a1 100644 --- a/drivers/ptp/Kconfig +++ b/drivers/ptp/Kconfig @@ -88,6 +88,16 @@ config DP83640_PHY In order for this to work, your MAC driver must also implement the skb_tx_timestamp() function. +config INES_PTP_TSTAMP + tristate "ZHAW InES PTP time stamping IP core" + depends on NETWORK_PHY_TIMESTAMPING + depends on PHYLIB + depends on PTP_1588_CLOCK + ---help--- + This driver adds support for using the ZHAW InES 1588 IP + core. This clock is only useful if the MII bus of your MAC + is wired up to the core. + config PTP_1588_CLOCK_PCH tristate "Intel PCH EG20T as PTP clock" depends on X86_32 || COMPILE_TEST

[net-next,RFC,V1,5/5] net: mdio: Add a driver for InES time stamping IP core.

Commit Message

Comments

Patch