792 lines · c
1// SPDX-License-Identifier: GPL-2.02//3// Copyright (C) 2018 MOSER-BAER AG4//5 6#define pr_fmt(fmt) "InES_PTP: " fmt7 8#include <linux/ethtool.h>9#include <linux/export.h>10#include <linux/if_vlan.h>11#include <linux/mii_timestamper.h>12#include <linux/module.h>13#include <linux/net_tstamp.h>14#include <linux/of.h>15#include <linux/of_address.h>16#include <linux/of_irq.h>17#include <linux/phy.h>18#include <linux/platform_device.h>19#include <linux/ptp_classify.h>20#include <linux/ptp_clock_kernel.h>21#include <linux/stddef.h>22 23MODULE_DESCRIPTION("Driver for the ZHAW InES PTP time stamping IP core");24MODULE_AUTHOR("Richard Cochran <richardcochran@gmail.com>");25MODULE_VERSION("1.0");26MODULE_LICENSE("GPL");27 28/* GLOBAL register */29#define MCAST_MAC_SELECT_SHIFT 230#define MCAST_MAC_SELECT_MASK 0x331#define IO_RESET BIT(1)32#define PTP_RESET BIT(0)33 34/* VERSION register */35#define IF_MAJOR_VER_SHIFT 1236#define IF_MAJOR_VER_MASK 0xf37#define IF_MINOR_VER_SHIFT 838#define IF_MINOR_VER_MASK 0xf39#define FPGA_MAJOR_VER_SHIFT 440#define FPGA_MAJOR_VER_MASK 0xf41#define FPGA_MINOR_VER_SHIFT 042#define FPGA_MINOR_VER_MASK 0xf43 44/* INT_STAT register */45#define RX_INTR_STATUS_3 BIT(5)46#define RX_INTR_STATUS_2 BIT(4)47#define RX_INTR_STATUS_1 BIT(3)48#define TX_INTR_STATUS_3 BIT(2)49#define TX_INTR_STATUS_2 BIT(1)50#define TX_INTR_STATUS_1 BIT(0)51 52/* INT_MSK register */53#define RX_INTR_MASK_3 BIT(5)54#define RX_INTR_MASK_2 BIT(4)55#define RX_INTR_MASK_1 BIT(3)56#define TX_INTR_MASK_3 BIT(2)57#define TX_INTR_MASK_2 BIT(1)58#define TX_INTR_MASK_1 BIT(0)59 60/* BUF_STAT register */61#define RX_FIFO_NE_3 BIT(5)62#define RX_FIFO_NE_2 BIT(4)63#define RX_FIFO_NE_1 BIT(3)64#define TX_FIFO_NE_3 BIT(2)65#define TX_FIFO_NE_2 BIT(1)66#define TX_FIFO_NE_1 BIT(0)67 68/* PORT_CONF register */69#define CM_ONE_STEP BIT(6)70#define PHY_SPEED_SHIFT 471#define PHY_SPEED_MASK 0x372#define P2P_DELAY_WR_POS_SHIFT 273#define P2P_DELAY_WR_POS_MASK 0x374#define PTP_MODE_SHIFT 075#define PTP_MODE_MASK 0x376 77/* TS_STAT_TX register */78#define TS_ENABLE BIT(15)79#define DATA_READ_POS_SHIFT 880#define DATA_READ_POS_MASK 0x1f81#define DISCARDED_EVENTS_SHIFT 482#define DISCARDED_EVENTS_MASK 0xf83 84#define INES_N_PORTS 385#define INES_REGISTER_SIZE 0x8086#define INES_PORT_OFFSET 0x2087#define INES_PORT_SIZE 0x2088#define INES_FIFO_DEPTH 9089#define INES_MAX_EVENTS 10090 91#define BC_PTP_V1 092#define BC_PTP_V2 193#define TC_E2E_PTP_V2 294#define TC_P2P_PTP_V2 395 96#define PHY_SPEED_10 097#define PHY_SPEED_100 198#define PHY_SPEED_1000 299 100#define PORT_CONF \101 ((PHY_SPEED_1000 << PHY_SPEED_SHIFT) | (BC_PTP_V2 << PTP_MODE_SHIFT))102 103#define ines_read32(s, r) __raw_readl((void __iomem *)&s->regs->r)104#define ines_write32(s, v, r) __raw_writel(v, (void __iomem *)&s->regs->r)105 106#define MESSAGE_TYPE_SYNC 1107#define MESSAGE_TYPE_P_DELAY_REQ 2108#define MESSAGE_TYPE_P_DELAY_RESP 3109#define MESSAGE_TYPE_DELAY_REQ 4110 111static LIST_HEAD(ines_clocks);112static DEFINE_MUTEX(ines_clocks_lock);113 114struct ines_global_regs {115 u32 id;116 u32 test;117 u32 global;118 u32 version;119 u32 test2;120 u32 int_stat;121 u32 int_msk;122 u32 buf_stat;123};124 125struct ines_port_registers {126 u32 port_conf;127 u32 p_delay;128 u32 ts_stat_tx;129 u32 ts_stat_rx;130 u32 ts_tx;131 u32 ts_rx;132};133 134struct ines_timestamp {135 struct list_head list;136 unsigned long tmo;137 u16 tag;138 u64 sec;139 u64 nsec;140 u64 clkid;141 u16 portnum;142 u16 seqid;143};144 145struct ines_port {146 struct ines_port_registers *regs;147 struct mii_timestamper mii_ts;148 struct ines_clock *clock;149 bool rxts_enabled;150 bool txts_enabled;151 unsigned int index;152 struct delayed_work ts_work;153 /* lock protects event list and tx_skb */154 spinlock_t lock;155 struct sk_buff *tx_skb;156 struct list_head events;157 struct list_head pool;158 struct ines_timestamp pool_data[INES_MAX_EVENTS];159};160 161struct ines_clock {162 struct ines_port port[INES_N_PORTS];163 struct ines_global_regs __iomem *regs;164 void __iomem *base;165 struct device_node *node;166 struct device *dev;167 struct list_head list;168};169 170static bool ines_match(struct sk_buff *skb, unsigned int ptp_class,171 struct ines_timestamp *ts, struct device *dev);172static int ines_rxfifo_read(struct ines_port *port);173static u64 ines_rxts64(struct ines_port *port, unsigned int words);174static bool ines_timestamp_expired(struct ines_timestamp *ts);175static u64 ines_txts64(struct ines_port *port, unsigned int words);176static void ines_txtstamp_work(struct work_struct *work);177static bool is_sync_pdelay_resp(struct sk_buff *skb, int type);178static u8 tag_to_msgtype(u8 tag);179 180static void ines_clock_cleanup(struct ines_clock *clock)181{182 struct ines_port *port;183 int i;184 185 for (i = 0; i < INES_N_PORTS; i++) {186 port = &clock->port[i];187 cancel_delayed_work_sync(&port->ts_work);188 }189}190 191static int ines_clock_init(struct ines_clock *clock, struct device *device,192 void __iomem *addr)193{194 struct device_node *node = device->of_node;195 unsigned long port_addr;196 struct ines_port *port;197 int i, j;198 199 INIT_LIST_HEAD(&clock->list);200 clock->node = node;201 clock->dev = device;202 clock->base = addr;203 clock->regs = clock->base;204 205 for (i = 0; i < INES_N_PORTS; i++) {206 port = &clock->port[i];207 port_addr = (unsigned long) clock->base +208 INES_PORT_OFFSET + i * INES_PORT_SIZE;209 port->regs = (struct ines_port_registers *) port_addr;210 port->clock = clock;211 port->index = i;212 INIT_DELAYED_WORK(&port->ts_work, ines_txtstamp_work);213 spin_lock_init(&port->lock);214 INIT_LIST_HEAD(&port->events);215 INIT_LIST_HEAD(&port->pool);216 for (j = 0; j < INES_MAX_EVENTS; j++)217 list_add(&port->pool_data[j].list, &port->pool);218 }219 220 ines_write32(clock, 0xBEEF, test);221 ines_write32(clock, 0xBEEF, test2);222 223 dev_dbg(device, "ID 0x%x\n", ines_read32(clock, id));224 dev_dbg(device, "TEST 0x%x\n", ines_read32(clock, test));225 dev_dbg(device, "VERSION 0x%x\n", ines_read32(clock, version));226 dev_dbg(device, "TEST2 0x%x\n", ines_read32(clock, test2));227 228 for (i = 0; i < INES_N_PORTS; i++) {229 port = &clock->port[i];230 ines_write32(port, PORT_CONF, port_conf);231 }232 233 return 0;234}235 236static struct ines_port *ines_find_port(struct device_node *node, u32 index)237{238 struct ines_port *port = NULL;239 struct ines_clock *clock;240 struct list_head *this;241 242 mutex_lock(&ines_clocks_lock);243 list_for_each(this, &ines_clocks) {244 clock = list_entry(this, struct ines_clock, list);245 if (clock->node == node) {246 port = &clock->port[index];247 break;248 }249 }250 mutex_unlock(&ines_clocks_lock);251 return port;252}253 254static u64 ines_find_rxts(struct ines_port *port, struct sk_buff *skb, int type)255{256 struct list_head *this, *next;257 struct ines_timestamp *ts;258 unsigned long flags;259 u64 ns = 0;260 261 if (type == PTP_CLASS_NONE)262 return 0;263 264 spin_lock_irqsave(&port->lock, flags);265 ines_rxfifo_read(port);266 list_for_each_safe(this, next, &port->events) {267 ts = list_entry(this, struct ines_timestamp, list);268 if (ines_timestamp_expired(ts)) {269 list_del_init(&ts->list);270 list_add(&ts->list, &port->pool);271 continue;272 }273 if (ines_match(skb, type, ts, port->clock->dev)) {274 ns = ts->sec * 1000000000ULL + ts->nsec;275 list_del_init(&ts->list);276 list_add(&ts->list, &port->pool);277 break;278 }279 }280 spin_unlock_irqrestore(&port->lock, flags);281 282 return ns;283}284 285static u64 ines_find_txts(struct ines_port *port, struct sk_buff *skb)286{287 unsigned int class = ptp_classify_raw(skb), i;288 u32 data_rd_pos, buf_stat, mask, ts_stat_tx;289 struct ines_timestamp ts;290 unsigned long flags;291 u64 ns = 0;292 293 mask = TX_FIFO_NE_1 << port->index;294 295 spin_lock_irqsave(&port->lock, flags);296 297 for (i = 0; i < INES_FIFO_DEPTH; i++) {298 299 buf_stat = ines_read32(port->clock, buf_stat);300 if (!(buf_stat & mask)) {301 dev_dbg(port->clock->dev,302 "Tx timestamp FIFO unexpectedly empty\n");303 break;304 }305 ts_stat_tx = ines_read32(port, ts_stat_tx);306 data_rd_pos = (ts_stat_tx >> DATA_READ_POS_SHIFT) &307 DATA_READ_POS_MASK;308 if (data_rd_pos) {309 dev_err(port->clock->dev,310 "unexpected Tx read pos %u\n", data_rd_pos);311 break;312 }313 314 ts.tag = ines_read32(port, ts_tx);315 ts.sec = ines_txts64(port, 3);316 ts.nsec = ines_txts64(port, 2);317 ts.clkid = ines_txts64(port, 4);318 ts.portnum = ines_read32(port, ts_tx);319 ts.seqid = ines_read32(port, ts_tx);320 321 if (ines_match(skb, class, &ts, port->clock->dev)) {322 ns = ts.sec * 1000000000ULL + ts.nsec;323 break;324 }325 }326 327 spin_unlock_irqrestore(&port->lock, flags);328 return ns;329}330 331static int ines_hwtstamp(struct mii_timestamper *mii_ts,332 struct kernel_hwtstamp_config *cfg,333 struct netlink_ext_ack *extack)334{335 struct ines_port *port = container_of(mii_ts, struct ines_port, mii_ts);336 u32 cm_one_step = 0, port_conf, ts_stat_rx, ts_stat_tx;337 unsigned long flags;338 339 switch (cfg->tx_type) {340 case HWTSTAMP_TX_OFF:341 ts_stat_tx = 0;342 break;343 case HWTSTAMP_TX_ON:344 ts_stat_tx = TS_ENABLE;345 break;346 case HWTSTAMP_TX_ONESTEP_P2P:347 ts_stat_tx = TS_ENABLE;348 cm_one_step = CM_ONE_STEP;349 break;350 default:351 return -ERANGE;352 }353 354 switch (cfg->rx_filter) {355 case HWTSTAMP_FILTER_NONE:356 ts_stat_rx = 0;357 break;358 case HWTSTAMP_FILTER_ALL:359 case HWTSTAMP_FILTER_PTP_V1_L4_EVENT:360 case HWTSTAMP_FILTER_PTP_V1_L4_SYNC:361 case HWTSTAMP_FILTER_PTP_V1_L4_DELAY_REQ:362 return -ERANGE;363 case HWTSTAMP_FILTER_PTP_V2_L4_EVENT:364 case HWTSTAMP_FILTER_PTP_V2_L4_SYNC:365 case HWTSTAMP_FILTER_PTP_V2_L4_DELAY_REQ:366 case HWTSTAMP_FILTER_PTP_V2_L2_EVENT:367 case HWTSTAMP_FILTER_PTP_V2_L2_SYNC:368 case HWTSTAMP_FILTER_PTP_V2_L2_DELAY_REQ:369 case HWTSTAMP_FILTER_PTP_V2_EVENT:370 case HWTSTAMP_FILTER_PTP_V2_SYNC:371 case HWTSTAMP_FILTER_PTP_V2_DELAY_REQ:372 ts_stat_rx = TS_ENABLE;373 cfg->rx_filter = HWTSTAMP_FILTER_PTP_V2_EVENT;374 break;375 default:376 return -ERANGE;377 }378 379 spin_lock_irqsave(&port->lock, flags);380 381 port_conf = ines_read32(port, port_conf);382 port_conf &= ~CM_ONE_STEP;383 port_conf |= cm_one_step;384 385 ines_write32(port, port_conf, port_conf);386 ines_write32(port, ts_stat_rx, ts_stat_rx);387 ines_write32(port, ts_stat_tx, ts_stat_tx);388 389 port->rxts_enabled = ts_stat_rx == TS_ENABLE;390 port->txts_enabled = ts_stat_tx == TS_ENABLE;391 392 spin_unlock_irqrestore(&port->lock, flags);393 394 return 0;395}396 397static void ines_link_state(struct mii_timestamper *mii_ts,398 struct phy_device *phydev)399{400 struct ines_port *port = container_of(mii_ts, struct ines_port, mii_ts);401 u32 port_conf, speed_conf;402 unsigned long flags;403 404 switch (phydev->speed) {405 case SPEED_10:406 speed_conf = PHY_SPEED_10 << PHY_SPEED_SHIFT;407 break;408 case SPEED_100:409 speed_conf = PHY_SPEED_100 << PHY_SPEED_SHIFT;410 break;411 case SPEED_1000:412 speed_conf = PHY_SPEED_1000 << PHY_SPEED_SHIFT;413 break;414 default:415 dev_err(port->clock->dev, "bad speed: %d\n", phydev->speed);416 return;417 }418 spin_lock_irqsave(&port->lock, flags);419 420 port_conf = ines_read32(port, port_conf);421 port_conf &= ~(0x3 << PHY_SPEED_SHIFT);422 port_conf |= speed_conf;423 424 ines_write32(port, port_conf, port_conf);425 426 spin_unlock_irqrestore(&port->lock, flags);427}428 429static bool ines_match(struct sk_buff *skb, unsigned int ptp_class,430 struct ines_timestamp *ts, struct device *dev)431{432 struct ptp_header *hdr;433 u16 portn, seqid;434 u8 msgtype;435 u64 clkid;436 437 if (unlikely(ptp_class & PTP_CLASS_V1))438 return false;439 440 hdr = ptp_parse_header(skb, ptp_class);441 if (!hdr)442 return false;443 444 msgtype = ptp_get_msgtype(hdr, ptp_class);445 clkid = be64_to_cpup((__be64 *)&hdr->source_port_identity.clock_identity.id[0]);446 portn = be16_to_cpu(hdr->source_port_identity.port_number);447 seqid = be16_to_cpu(hdr->sequence_id);448 449 if (tag_to_msgtype(ts->tag & 0x7) != msgtype) {450 dev_dbg(dev, "msgtype mismatch ts %hhu != skb %hhu\n",451 tag_to_msgtype(ts->tag & 0x7), msgtype);452 return false;453 }454 if (ts->clkid != clkid) {455 dev_dbg(dev, "clkid mismatch ts %llx != skb %llx\n",456 ts->clkid, clkid);457 return false;458 }459 if (ts->portnum != portn) {460 dev_dbg(dev, "portn mismatch ts %hu != skb %hu\n",461 ts->portnum, portn);462 return false;463 }464 if (ts->seqid != seqid) {465 dev_dbg(dev, "seqid mismatch ts %hu != skb %hu\n",466 ts->seqid, seqid);467 return false;468 }469 470 return true;471}472 473static bool ines_rxtstamp(struct mii_timestamper *mii_ts,474 struct sk_buff *skb, int type)475{476 struct ines_port *port = container_of(mii_ts, struct ines_port, mii_ts);477 struct skb_shared_hwtstamps *ssh;478 u64 ns;479 480 if (!port->rxts_enabled)481 return false;482 483 ns = ines_find_rxts(port, skb, type);484 if (!ns)485 return false;486 487 ssh = skb_hwtstamps(skb);488 ssh->hwtstamp = ns_to_ktime(ns);489 netif_rx(skb);490 491 return true;492}493 494static int ines_rxfifo_read(struct ines_port *port)495{496 u32 data_rd_pos, buf_stat, mask, ts_stat_rx;497 struct ines_timestamp *ts;498 unsigned int i;499 500 mask = RX_FIFO_NE_1 << port->index;501 502 for (i = 0; i < INES_FIFO_DEPTH; i++) {503 if (list_empty(&port->pool)) {504 dev_err(port->clock->dev, "event pool is empty\n");505 return -1;506 }507 buf_stat = ines_read32(port->clock, buf_stat);508 if (!(buf_stat & mask))509 break;510 511 ts_stat_rx = ines_read32(port, ts_stat_rx);512 data_rd_pos = (ts_stat_rx >> DATA_READ_POS_SHIFT) &513 DATA_READ_POS_MASK;514 if (data_rd_pos) {515 dev_err(port->clock->dev, "unexpected Rx read pos %u\n",516 data_rd_pos);517 break;518 }519 520 ts = list_first_entry(&port->pool, struct ines_timestamp, list);521 ts->tmo = jiffies + HZ;522 ts->tag = ines_read32(port, ts_rx);523 ts->sec = ines_rxts64(port, 3);524 ts->nsec = ines_rxts64(port, 2);525 ts->clkid = ines_rxts64(port, 4);526 ts->portnum = ines_read32(port, ts_rx);527 ts->seqid = ines_read32(port, ts_rx);528 529 list_del_init(&ts->list);530 list_add_tail(&ts->list, &port->events);531 }532 533 return 0;534}535 536static u64 ines_rxts64(struct ines_port *port, unsigned int words)537{538 unsigned int i;539 u64 result;540 u16 word;541 542 word = ines_read32(port, ts_rx);543 result = word;544 words--;545 for (i = 0; i < words; i++) {546 word = ines_read32(port, ts_rx);547 result <<= 16;548 result |= word;549 }550 return result;551}552 553static bool ines_timestamp_expired(struct ines_timestamp *ts)554{555 return time_after(jiffies, ts->tmo);556}557 558static int ines_ts_info(struct mii_timestamper *mii_ts,559 struct kernel_ethtool_ts_info *info)560{561 info->so_timestamping =562 SOF_TIMESTAMPING_TX_HARDWARE |563 SOF_TIMESTAMPING_TX_SOFTWARE |564 SOF_TIMESTAMPING_RX_HARDWARE |565 SOF_TIMESTAMPING_RAW_HARDWARE;566 567 info->tx_types =568 (1 << HWTSTAMP_TX_OFF) |569 (1 << HWTSTAMP_TX_ON) |570 (1 << HWTSTAMP_TX_ONESTEP_P2P);571 572 info->rx_filters =573 (1 << HWTSTAMP_FILTER_NONE) |574 (1 << HWTSTAMP_FILTER_PTP_V2_EVENT);575 576 return 0;577}578 579static u64 ines_txts64(struct ines_port *port, unsigned int words)580{581 unsigned int i;582 u64 result;583 u16 word;584 585 word = ines_read32(port, ts_tx);586 result = word;587 words--;588 for (i = 0; i < words; i++) {589 word = ines_read32(port, ts_tx);590 result <<= 16;591 result |= word;592 }593 return result;594}595 596static bool ines_txts_onestep(struct ines_port *port, struct sk_buff *skb, int type)597{598 unsigned long flags;599 u32 port_conf;600 601 spin_lock_irqsave(&port->lock, flags);602 port_conf = ines_read32(port, port_conf);603 spin_unlock_irqrestore(&port->lock, flags);604 605 if (port_conf & CM_ONE_STEP)606 return is_sync_pdelay_resp(skb, type);607 608 return false;609}610 611static void ines_txtstamp(struct mii_timestamper *mii_ts,612 struct sk_buff *skb, int type)613{614 struct ines_port *port = container_of(mii_ts, struct ines_port, mii_ts);615 struct sk_buff *old_skb = NULL;616 unsigned long flags;617 618 if (!port->txts_enabled || ines_txts_onestep(port, skb, type)) {619 kfree_skb(skb);620 return;621 }622 623 spin_lock_irqsave(&port->lock, flags);624 625 if (port->tx_skb)626 old_skb = port->tx_skb;627 628 port->tx_skb = skb;629 630 spin_unlock_irqrestore(&port->lock, flags);631 632 kfree_skb(old_skb);633 634 schedule_delayed_work(&port->ts_work, 1);635}636 637static void ines_txtstamp_work(struct work_struct *work)638{639 struct ines_port *port =640 container_of(work, struct ines_port, ts_work.work);641 struct skb_shared_hwtstamps ssh;642 struct sk_buff *skb;643 unsigned long flags;644 u64 ns;645 646 spin_lock_irqsave(&port->lock, flags);647 skb = port->tx_skb;648 port->tx_skb = NULL;649 spin_unlock_irqrestore(&port->lock, flags);650 651 ns = ines_find_txts(port, skb);652 if (!ns) {653 kfree_skb(skb);654 return;655 }656 ssh.hwtstamp = ns_to_ktime(ns);657 skb_complete_tx_timestamp(skb, &ssh);658}659 660static bool is_sync_pdelay_resp(struct sk_buff *skb, int type)661{662 struct ptp_header *hdr;663 u8 msgtype;664 665 hdr = ptp_parse_header(skb, type);666 if (!hdr)667 return false;668 669 msgtype = ptp_get_msgtype(hdr, type);670 671 switch (msgtype) {672 case PTP_MSGTYPE_SYNC:673 case PTP_MSGTYPE_PDELAY_RESP:674 return true;675 default:676 return false;677 }678}679 680static u8 tag_to_msgtype(u8 tag)681{682 switch (tag) {683 case MESSAGE_TYPE_SYNC:684 return PTP_MSGTYPE_SYNC;685 case MESSAGE_TYPE_P_DELAY_REQ:686 return PTP_MSGTYPE_PDELAY_REQ;687 case MESSAGE_TYPE_P_DELAY_RESP:688 return PTP_MSGTYPE_PDELAY_RESP;689 case MESSAGE_TYPE_DELAY_REQ:690 return PTP_MSGTYPE_DELAY_REQ;691 }692 return 0xf;693}694 695static struct mii_timestamper *ines_ptp_probe_channel(struct device *device,696 unsigned int index)697{698 struct device_node *node = device->of_node;699 struct ines_port *port;700 701 if (index > INES_N_PORTS - 1) {702 dev_err(device, "bad port index %u\n", index);703 return ERR_PTR(-EINVAL);704 }705 port = ines_find_port(node, index);706 if (!port) {707 dev_err(device, "missing port index %u\n", index);708 return ERR_PTR(-ENODEV);709 }710 port->mii_ts.rxtstamp = ines_rxtstamp;711 port->mii_ts.txtstamp = ines_txtstamp;712 port->mii_ts.hwtstamp = ines_hwtstamp;713 port->mii_ts.link_state = ines_link_state;714 port->mii_ts.ts_info = ines_ts_info;715 716 return &port->mii_ts;717}718 719static void ines_ptp_release_channel(struct device *device,720 struct mii_timestamper *mii_ts)721{722}723 724static struct mii_timestamping_ctrl ines_ctrl = {725 .probe_channel = ines_ptp_probe_channel,726 .release_channel = ines_ptp_release_channel,727};728 729static int ines_ptp_ctrl_probe(struct platform_device *pld)730{731 struct ines_clock *clock;732 void __iomem *addr;733 int err = 0;734 735 addr = devm_platform_ioremap_resource(pld, 0);736 if (IS_ERR(addr)) {737 err = PTR_ERR(addr);738 goto out;739 }740 clock = kzalloc(sizeof(*clock), GFP_KERNEL);741 if (!clock) {742 err = -ENOMEM;743 goto out;744 }745 if (ines_clock_init(clock, &pld->dev, addr)) {746 kfree(clock);747 err = -ENOMEM;748 goto out;749 }750 err = register_mii_tstamp_controller(&pld->dev, &ines_ctrl);751 if (err) {752 kfree(clock);753 goto out;754 }755 mutex_lock(&ines_clocks_lock);756 list_add_tail(&ines_clocks, &clock->list);757 mutex_unlock(&ines_clocks_lock);758 759 dev_set_drvdata(&pld->dev, clock);760out:761 return err;762}763 764static void ines_ptp_ctrl_remove(struct platform_device *pld)765{766 struct ines_clock *clock = dev_get_drvdata(&pld->dev);767 768 unregister_mii_tstamp_controller(&pld->dev);769 mutex_lock(&ines_clocks_lock);770 list_del(&clock->list);771 mutex_unlock(&ines_clocks_lock);772 ines_clock_cleanup(clock);773 kfree(clock);774}775 776static const struct of_device_id ines_ptp_ctrl_of_match[] = {777 { .compatible = "ines,ptp-ctrl" },778 { }779};780 781MODULE_DEVICE_TABLE(of, ines_ptp_ctrl_of_match);782 783static struct platform_driver ines_ptp_ctrl_driver = {784 .probe = ines_ptp_ctrl_probe,785 .remove_new = ines_ptp_ctrl_remove,786 .driver = {787 .name = "ines_ptp_ctrl",788 .of_match_table = ines_ptp_ctrl_of_match,789 },790};791module_platform_driver(ines_ptp_ctrl_driver);792