| @@ -1349,6 +1349,9 @@ static int vsc9959_port_setup_tc(struct dsa_switch *ds, int port, · 1348 unmodified lines |
| 1349 | 1349 | struct felix_stream { |
| 1350 | 1350 | struct list_head list; |
| 1351 | 1351 | unsigned long id; |
| 1352 | + bool dummy; |
| 1353 | + int ports; |
| 1354 | + int port; |
| 1352 | 1355 | u8 dmac[ETH_ALEN]; |
| 1353 | 1356 | u16 vid; |
| 1354 | 1357 | s8 prio; |
| @@ -1363,6 +1366,7 @@ struct felix_stream_filter { · 8 unmodified lines |
| 1363 | 1366 | refcount_t refcount; |
| 1364 | 1367 | u32 index; |
| 1365 | 1368 | u8 enable; |
| 1369 | + int portmask; |
| 1366 | 1370 | u8 sg_valid; |
| 1367 | 1371 | u32 sgid; |
| 1368 | 1372 | u8 fm_valid; |
| @@ -1505,10 +1509,12 @@ static int vsc9959_stream_table_add(struct ocelot *ocelot, · 136 unmodified lines |
| 1505 | 1509 | |
| 1506 | 1510 | memcpy(stream_entry, stream, sizeof(*stream_entry)); |
| 1507 | 1511 | |
| 1508 | | − ret = vsc9959_mact_stream_set(ocelot, stream_entry, extack); |
| 1509 | | − if (ret) { |
| 1510 | | − kfree(stream_entry); |
| 1511 | | − return ret; |
| 1512 | + if (!stream->dummy) { |
| 1513 | + ret = vsc9959_mact_stream_set(ocelot, stream_entry, extack); |
| 1514 | + if (ret) { |
| 1515 | + kfree(stream_entry); |
| 1516 | + return ret; |
| 1517 | + } |
| 1512 | 1518 | } |
| 1513 | 1519 | |
| 1514 | 1520 | list_add_tail(&stream_entry->list, stream_list); |
| @@ -1531,7 +1537,8 @@ vsc9959_stream_table_get(struct list_head *stream_list, unsigned long id) · 16 unmodified lines |
| 1531 | 1537 | static void vsc9959_stream_table_del(struct ocelot *ocelot, |
| 1532 | 1538 | struct felix_stream *stream) |
| 1533 | 1539 | { |
| 1534 | | − vsc9959_mact_stream_set(ocelot, stream, NULL); |
| 1540 | + if (!stream->dummy) |
| 1541 | + vsc9959_mact_stream_set(ocelot, stream, NULL); |
| 1535 | 1542 | |
| 1536 | 1543 | list_del(&stream->list); |
| 1537 | 1544 | kfree(stream); |
| @@ -1586,14 +1593,64 @@ static int vsc9959_psfp_sfi_set(struct ocelot *ocelot, · 48 unmodified lines |
| 1586 | 1593 | 10, 100000); |
| 1587 | 1594 | } |
| 1588 | 1595 | |
| 1596 | +static int vsc9959_psfp_sfidmask_set(struct ocelot *ocelot, u32 sfid, int ports) |
| 1597 | +{ |
| 1598 | + u32 val; |
| 1599 | + |
| 1600 | + ocelot_rmw(ocelot, |
| 1601 | + ANA_TABLES_SFIDTIDX_SFID_INDEX(sfid), |
| 1602 | + ANA_TABLES_SFIDTIDX_SFID_INDEX_M, |
| 1603 | + ANA_TABLES_SFIDTIDX); |
| 1604 | + |
| 1605 | + ocelot_write(ocelot, |
| 1606 | + ANA_TABLES_SFID_MASK_IGR_PORT_MASK(ports) | |
| 1607 | + ANA_TABLES_SFID_MASK_IGR_SRCPORT_MATCH_ENA, |
| 1608 | + ANA_TABLES_SFID_MASK); |
| 1609 | + |
| 1610 | + ocelot_rmw(ocelot, |
| 1611 | + ANA_TABLES_SFIDACCESS_SFID_TBL_CMD(SFIDACCESS_CMD_WRITE), |
| 1612 | + ANA_TABLES_SFIDACCESS_SFID_TBL_CMD_M, |
| 1613 | + ANA_TABLES_SFIDACCESS); |
| 1614 | + |
| 1615 | + return readx_poll_timeout(vsc9959_sfi_access_status, ocelot, val, |
| 1616 | + (!ANA_TABLES_SFIDACCESS_SFID_TBL_CMD(val)), |
| 1617 | + 10, 100000); |
| 1618 | +} |
| 1619 | + |
| 1620 | +static int vsc9959_psfp_sfi_list_add(struct ocelot *ocelot, |
| 1621 | + struct felix_stream_filter *sfi, |
| 1622 | + struct list_head *pos) |
| 1623 | +{ |
| 1624 | + struct felix_stream_filter *sfi_entry; |
| 1625 | + int ret; |
| 1626 | + |
| 1627 | + sfi_entry = kzalloc(sizeof(*sfi_entry), GFP_KERNEL); |
| 1628 | + if (!sfi_entry) |
| 1629 | + return -ENOMEM; |
| 1630 | + |
| 1631 | + memcpy(sfi_entry, sfi, sizeof(*sfi_entry)); |
| 1632 | + refcount_set(&sfi_entry->refcount, 1); |
| 1633 | + |
| 1634 | + ret = vsc9959_psfp_sfi_set(ocelot, sfi_entry); |
| 1635 | + if (ret) { |
| 1636 | + kfree(sfi_entry); |
| 1637 | + return ret; |
| 1638 | + } |
| 1639 | + |
| 1640 | + vsc9959_psfp_sfidmask_set(ocelot, sfi->index, sfi->portmask); |
| 1641 | + |
| 1642 | + list_add(&sfi_entry->list, pos); |
| 1643 | + |
| 1644 | + return 0; |
| 1645 | +} |
| 1646 | + |
| 1589 | 1647 | static int vsc9959_psfp_sfi_table_add(struct ocelot *ocelot, |
| 1590 | 1648 | struct felix_stream_filter *sfi) |
| 1591 | 1649 | { |
| 1592 | | − struct felix_stream_filter *sfi_entry, *tmp; |
| 1593 | 1650 | struct list_head *pos, *q, *last; |
| 1651 | + struct felix_stream_filter *tmp; |
| 1594 | 1652 | struct ocelot_psfp_list *psfp; |
| 1595 | 1653 | u32 insert = 0; |
| 1596 | | − int ret; |
| 1597 | 1654 | |
| 1598 | 1655 | psfp = &ocelot->psfp; |
| 1599 | 1656 | last = &psfp->sfi_list; |
| @@ -1602,6 +1659,7 @@ static int vsc9959_psfp_sfi_table_add(struct ocelot *ocelot, · 2 unmodified lines |
| 1602 | 1659 | tmp = list_entry(pos, struct felix_stream_filter, list); |
| 1603 | 1660 | if (sfi->sg_valid == tmp->sg_valid && |
| 1604 | 1661 | sfi->fm_valid == tmp->fm_valid && |
| 1662 | + sfi->portmask == tmp->portmask && |
| 1605 | 1663 | tmp->sgid == sfi->sgid && |
| 1606 | 1664 | tmp->fmid == sfi->fmid) { |
| 1607 | 1665 | sfi->index = tmp->index; |
| @@ -1616,22 +1674,40 @@ static int vsc9959_psfp_sfi_table_add(struct ocelot *ocelot, · 8 unmodified lines |
| 1616 | 1674 | } |
| 1617 | 1675 | sfi->index = insert; |
| 1618 | 1676 | |
| 1619 | | − sfi_entry = kzalloc(sizeof(*sfi_entry), GFP_KERNEL); |
| 1620 | | − if (!sfi_entry) |
| 1621 | | − return -ENOMEM; |
| 1677 | + return vsc9959_psfp_sfi_list_add(ocelot, sfi, last); |
| 1678 | +} |
| 1622 | 1679 | |
| 1623 | | − memcpy(sfi_entry, sfi, sizeof(*sfi_entry)); |
| 1624 | | − refcount_set(&sfi_entry->refcount, 1); |
| 1680 | +static int vsc9959_psfp_sfi_table_add2(struct ocelot *ocelot, |
| 1681 | + struct felix_stream_filter *sfi, |
| 1682 | + struct felix_stream_filter *sfi2) |
| 1683 | +{ |
| 1684 | + struct felix_stream_filter *tmp; |
| 1685 | + struct list_head *pos, *q, *last; |
| 1686 | + struct ocelot_psfp_list *psfp; |
| 1687 | + u32 insert = 0; |
| 1688 | + int ret; |
| 1625 | 1689 | |
| 1626 | | − ret = vsc9959_psfp_sfi_set(ocelot, sfi_entry); |
| 1627 | | − if (ret) { |
| 1628 | | − kfree(sfi_entry); |
| 1629 | | − return ret; |
| 1690 | + psfp = &ocelot->psfp; |
| 1691 | + last = &psfp->sfi_list; |
| 1692 | + |
| 1693 | + list_for_each_safe(pos, q, &psfp->sfi_list) { |
| 1694 | + tmp = list_entry(pos, struct felix_stream_filter, list); |
| 1695 | + /* Make sure that the index is increasing in order. */ |
| 1696 | + if (tmp->index >= insert + 2) |
| 1697 | + break; |
| 1698 | + |
| 1699 | + insert = tmp->index + 1; |
| 1700 | + last = pos; |
| 1630 | 1701 | } |
| 1702 | + sfi->index = insert; |
| 1703 | + |
| 1704 | + ret = vsc9959_psfp_sfi_list_add(ocelot, sfi, last); |
| 1705 | + if (ret) |
| 1706 | + return ret; |
| 1631 | 1707 | |
| 1632 | | − list_add(&sfi_entry->list, last); |
| 1708 | + sfi2->index = insert + 1; |
| 1633 | 1709 | |
| 1634 | | − return 0; |
| 1710 | + return vsc9959_psfp_sfi_list_add(ocelot, sfi2, last->next); |
| 1635 | 1711 | } |
| 1636 | 1712 | |
| 1637 | 1713 | static struct felix_stream_filter * |
| @@ -1832,10 +1908,11 @@ static void vsc9959_psfp_counters_get(struct ocelot *ocelot, u32 index, · 194 unmodified lines |
| 1832 | 1908 | SYS_STAT_CFG); |
| 1833 | 1909 | } |
| 1834 | 1910 | |
| 1835 | | −static int vsc9959_psfp_filter_add(struct ocelot *ocelot, |
| 1911 | +static int vsc9959_psfp_filter_add(struct ocelot *ocelot, int port, |
| 1836 | 1912 | struct flow_cls_offload *f) |
| 1837 | 1913 | { |
| 1838 | 1914 | struct netlink_ext_ack *extack = f->common.extack; |
| 1915 | + struct felix_stream_filter old_sfi, *sfi_entry; |
| 1839 | 1916 | struct felix_stream_filter sfi = {0}; |
| 1840 | 1917 | const struct flow_action_entry *a; |
| 1841 | 1918 | struct felix_stream *stream_entry; |
| @@ -1896,21 +1973,61 @@ static int vsc9959_psfp_filter_add(struct ocelot *ocelot, · 54 unmodified lines |
| 1896 | 1973 | } |
| 1897 | 1974 | } |
| 1898 | 1975 | |
| 1899 | | − /* Check if stream is set. */ |
| 1900 | | − stream_entry = vsc9959_stream_table_lookup(&psfp->stream_list, &stream); |
| 1901 | | − if (stream_entry) { |
| 1902 | | − NL_SET_ERR_MSG_MOD(extack, "This stream is already added"); |
| 1903 | | − ret = -EEXIST; |
| 1904 | | − goto err; |
| 1905 | | − } |
| 1976 | + stream.ports = BIT(port); |
| 1977 | + stream.port = port; |
| 1906 | 1978 | |
| 1979 | + sfi.portmask = stream.ports; |
| 1907 | 1980 | sfi.prio_valid = (stream.prio < 0 ? 0 : 1); |
| 1908 | 1981 | sfi.prio = (sfi.prio_valid ? stream.prio : 0); |
| 1909 | 1982 | sfi.enable = 1; |
| 1910 | 1983 | |
| 1911 | | − ret = vsc9959_psfp_sfi_table_add(ocelot, &sfi); |
| 1912 | | − if (ret) |
| 1913 | | − goto err; |
| 1984 | + /* Check if stream is set. */ |
| 1985 | + stream_entry = vsc9959_stream_table_lookup(&psfp->stream_list, &stream); |
| 1986 | + if (stream_entry) { |
| 1987 | + if (stream_entry->ports & BIT(port)) { |
| 1988 | + NL_SET_ERR_MSG_MOD(extack, |
| 1989 | + "The stream is added on this port"); |
| 1990 | + ret = -EEXIST; |
| 1991 | + goto err; |
| 1992 | + } |
| 1993 | + |
| 1994 | + if (stream_entry->ports != BIT(stream_entry->port)) { |
| 1995 | + NL_SET_ERR_MSG_MOD(extack, |
| 1996 | + "The stream is added on two ports"); |
| 1997 | + ret = -EEXIST; |
| 1998 | + goto err; |
| 1999 | + } |
| 2000 | + |
| 2001 | + stream_entry->ports |= BIT(port); |
| 2002 | + stream.ports = stream_entry->ports; |
| 2003 | + |
| 2004 | + sfi_entry = vsc9959_psfp_sfi_table_get(&psfp->sfi_list, |
| 2005 | + stream_entry->sfid); |
| 2006 | + memcpy(&old_sfi, sfi_entry, sizeof(old_sfi)); |
| 2007 | + |
| 2008 | + vsc9959_psfp_sfi_table_del(ocelot, stream_entry->sfid); |
| 2009 | + |
| 2010 | + old_sfi.portmask = stream_entry->ports; |
| 2011 | + sfi.portmask = stream.ports; |
| 2012 | + |
| 2013 | + if (stream_entry->port > port) { |
| 2014 | + ret = vsc9959_psfp_sfi_table_add2(ocelot, &sfi, |
| 2015 | + &old_sfi); |
| 2016 | + stream_entry->dummy = true; |
| 2017 | + } else { |
| 2018 | + ret = vsc9959_psfp_sfi_table_add2(ocelot, &old_sfi, |
| 2019 | + &sfi); |
| 2020 | + stream.dummy = true; |
| 2021 | + } |
| 2022 | + if (ret) |
| 2023 | + goto err; |
| 2024 | + |
| 2025 | + stream_entry->sfid = old_sfi.index; |
| 2026 | + } else { |
| 2027 | + ret = vsc9959_psfp_sfi_table_add(ocelot, &sfi); |
| 2028 | + if (ret) |
| 2029 | + goto err; |
| 2030 | + } |
| 1914 | 2031 | |
| 1915 | 2032 | stream.sfid = sfi.index; |
| 1916 | 2033 | stream.sfid_valid = 1; |
| @@ -1936,9 +2053,9 @@ err: · 19 unmodified lines |
| 1936 | 2053 | static int vsc9959_psfp_filter_del(struct ocelot *ocelot, |
| 1937 | 2054 | struct flow_cls_offload *f) |
| 1938 | 2055 | { |
| 2056 | + struct felix_stream *stream, tmp, *stream_entry; |
| 1939 | 2057 | static struct felix_stream_filter *sfi; |
| 1940 | 2058 | struct ocelot_psfp_list *psfp; |
| 1941 | | − struct felix_stream *stream; |
| 1942 | 2059 | |
| 1943 | 2060 | psfp = &ocelot->psfp; |
| 1944 | 2061 | |
| @@ -1958,9 +2075,22 @@ static int vsc9959_psfp_filter_del(struct ocelot *ocelot, · 13 unmodified lines |
| 1958 | 2075 | |
| 1959 | 2076 | vsc9959_psfp_sfi_table_del(ocelot, stream->sfid); |
| 1960 | 2077 | |
| 2078 | + memcpy(&tmp, stream, sizeof(tmp)); |
| 2079 | + |
| 1961 | 2080 | stream->sfid_valid = 0; |
| 1962 | 2081 | vsc9959_stream_table_del(ocelot, stream); |
| 1963 | 2082 | |
| 2083 | + stream_entry = vsc9959_stream_table_lookup(&psfp->stream_list, &tmp); |
| 2084 | + if (stream_entry) { |
| 2085 | + stream_entry->ports = BIT(stream_entry->port); |
| 2086 | + if (stream_entry->dummy) { |
| 2087 | + stream_entry->dummy = false; |
| 2088 | + vsc9959_mact_stream_set(ocelot, stream_entry, NULL); |
| 2089 | + } |
| 2090 | + vsc9959_psfp_sfidmask_set(ocelot, stream_entry->sfid, |
| 2091 | + stream_entry->ports); |
| 2092 | + } |
| 2093 | + |
| 1964 | 2094 | return 0; |
| 1965 | 2095 | } |
| 1966 | 2096 | |