diff --git a/drivers/ntb/hw/mscc/ntb_hw_switchtec.c b/drivers/ntb/hw/mscc/ntb_hw_switchtec.c index 158ed310cbaf..b477a8915245 100644 --- a/drivers/ntb/hw/mscc/ntb_hw_switchtec.c +++ b/drivers/ntb/hw/mscc/ntb_hw_switchtec.c @@ -64,6 +64,7 @@ static inline void _iowrite64(u64 val, void __iomem *mmio) struct shared_mw { u32 magic; + u32 link_sta; u32 partition_id; u64 mw_sizes[MAX_MWS]; }; @@ -104,8 +105,17 @@ struct switchtec_ntb { int peer_nr_direct_mw; int peer_nr_lut_mw; int peer_direct_mw_to_bar[MAX_DIRECT_MW]; + + bool link_is_up; + enum ntb_speed link_speed; + enum ntb_width link_width; }; +static struct switchtec_ntb *ntb_sndev(struct ntb_dev *ntb) +{ + return container_of(ntb, struct switchtec_ntb, ntb); +} + static int switchtec_ntb_part_op(struct switchtec_ntb *sndev, struct ntb_ctrl_regs __iomem *ctl, u32 op, int wait_status) @@ -163,6 +173,17 @@ static int switchtec_ntb_part_op(struct switchtec_ntb *sndev, return -EIO; } +static int switchtec_ntb_send_msg(struct switchtec_ntb *sndev, int idx, + u32 val) +{ + if (idx < 0 || idx >= ARRAY_SIZE(sndev->mmio_self_dbmsg->omsg)) + return -EINVAL; + + iowrite32(val, &sndev->mmio_self_dbmsg->omsg[idx].msg); + + return 0; +} + static int switchtec_ntb_mw_count(struct ntb_dev *ntb, int pidx) { return 0; @@ -194,22 +215,124 @@ static int switchtec_ntb_peer_mw_get_addr(struct ntb_dev *ntb, int idx, return 0; } +static void switchtec_ntb_part_link_speed(struct switchtec_ntb *sndev, + int partition, + enum ntb_speed *speed, + enum ntb_width *width) +{ + struct switchtec_dev *stdev = sndev->stdev; + + u32 pff = ioread32(&stdev->mmio_part_cfg[partition].vep_pff_inst_id); + u32 linksta = ioread32(&stdev->mmio_pff_csr[pff].pci_cap_region[13]); + + if (speed) + *speed = (linksta >> 16) & 0xF; + + if (width) + *width = (linksta >> 20) & 0x3F; +} + +static void switchtec_ntb_set_link_speed(struct switchtec_ntb *sndev) +{ + enum ntb_speed self_speed, peer_speed; + enum ntb_width self_width, peer_width; + + if (!sndev->link_is_up) { + sndev->link_speed = NTB_SPEED_NONE; + sndev->link_width = NTB_WIDTH_NONE; + return; + } + + switchtec_ntb_part_link_speed(sndev, sndev->self_partition, + &self_speed, &self_width); + switchtec_ntb_part_link_speed(sndev, sndev->peer_partition, + &peer_speed, &peer_width); + + sndev->link_speed = min(self_speed, peer_speed); + sndev->link_width = min(self_width, peer_width); +} + +enum { + LINK_MESSAGE = 0, + MSG_LINK_UP = 1, + MSG_LINK_DOWN = 2, + MSG_CHECK_LINK = 3, +}; + +static void switchtec_ntb_check_link(struct switchtec_ntb *sndev) +{ + int link_sta; + int old = sndev->link_is_up; + + link_sta = sndev->self_shared->link_sta; + if (link_sta) { + u64 peer = ioread64(&sndev->peer_shared->magic); + + if ((peer & 0xFFFFFFFF) == SWITCHTEC_NTB_MAGIC) + link_sta = peer >> 32; + else + link_sta = 0; + } + + sndev->link_is_up = link_sta; + switchtec_ntb_set_link_speed(sndev); + + if (link_sta != old) { + switchtec_ntb_send_msg(sndev, LINK_MESSAGE, MSG_CHECK_LINK); + ntb_link_event(&sndev->ntb); + dev_info(&sndev->stdev->dev, "ntb link %s", + link_sta ? "up" : "down"); + } +} + +static void switchtec_ntb_link_notification(struct switchtec_dev *stdev) +{ + struct switchtec_ntb *sndev = stdev->sndev; + + switchtec_ntb_check_link(sndev); +} + static u64 switchtec_ntb_link_is_up(struct ntb_dev *ntb, enum ntb_speed *speed, enum ntb_width *width) { - return 0; + struct switchtec_ntb *sndev = ntb_sndev(ntb); + + if (speed) + *speed = sndev->link_speed; + if (width) + *width = sndev->link_width; + + return sndev->link_is_up; } static int switchtec_ntb_link_enable(struct ntb_dev *ntb, enum ntb_speed max_speed, enum ntb_width max_width) { + struct switchtec_ntb *sndev = ntb_sndev(ntb); + + dev_dbg(&sndev->stdev->dev, "enabling link"); + + sndev->self_shared->link_sta = 1; + switchtec_ntb_send_msg(sndev, LINK_MESSAGE, MSG_LINK_UP); + + switchtec_ntb_check_link(sndev); + return 0; } static int switchtec_ntb_link_disable(struct ntb_dev *ntb) { + struct switchtec_ntb *sndev = ntb_sndev(ntb); + + dev_dbg(&sndev->stdev->dev, "disabling link"); + + sndev->self_shared->link_sta = 0; + switchtec_ntb_send_msg(sndev, LINK_MESSAGE, MSG_LINK_UP); + + switchtec_ntb_check_link(sndev); + return 0; } @@ -577,6 +700,9 @@ static irqreturn_t switchtec_ntb_message_isr(int irq, void *dev) dev_dbg(&sndev->stdev->dev, "message: %d %08x\n", i, (u32)msg); iowrite8(1, &sndev->mmio_self_dbmsg->imsg[i].status); + + if (i == LINK_MESSAGE) + switchtec_ntb_check_link(sndev); } } @@ -679,6 +805,7 @@ static int switchtec_ntb_add(struct device *dev, goto deinit_and_exit; stdev->sndev = sndev; + stdev->link_notifier = switchtec_ntb_link_notification; dev_info(dev, "NTB device registered"); return 0; @@ -702,6 +829,7 @@ void switchtec_ntb_remove(struct device *dev, if (!sndev) return; + stdev->link_notifier = NULL; stdev->sndev = NULL; ntb_unregister_device(&sndev->ntb); switchtec_ntb_deinit_db_msg_irq(sndev);