| @@ -1349,6 +1349,9 @@ static int vsc9959_port_setup_tc(struct dsa_switch *ds, int port, · 1348 unmodified lines |
| 1349 | struct felix_stream { | 1349 | struct felix_stream { |
| 1350 | struct list_head list; | 1350 | struct list_head list; |
| 1351 | unsigned long id; | 1351 | unsigned long id; |
| | 1352 | bool dummy; |
| | 1353 | int ports; |
| | 1354 | int port; |
| 1352 | u8 dmac[ETH_ALEN]; | 1355 | u8 dmac[ETH_ALEN]; |
| 1353 | u16 vid; | 1356 | u16 vid; |
| 1354 | s8 prio; | 1357 | s8 prio; |
| @@ -1363,6 +1366,7 @@ struct felix_stream_filter { · 8 unmodified lines |
| 1363 | refcount_t refcount; | 1366 | refcount_t refcount; |
| 1364 | u32 index; | 1367 | u32 index; |
| 1365 | u8 enable; | 1368 | u8 enable; |
| | 1369 | int portmask; |
| 1366 | u8 sg_valid; | 1370 | u8 sg_valid; |
| 1367 | u32 sgid; | 1371 | u32 sgid; |
| 1368 | u8 fm_valid; | 1372 | u8 fm_valid; |
| @@ -1505,10 +1509,12 @@ static int vsc9959_stream_table_add(struct ocelot *ocelot, · 136 unmodified lines |
| 1505 | | 1509 | |
| 1506 | memcpy(stream_entry, stream, sizeof(*stream_entry)); | 1510 | memcpy(stream_entry, stream, sizeof(*stream_entry)); |
| 1507 | | 1511 | |
| 1508 | ret = vsc9959_mact_stream_set(ocelot, stream_entry, extack); | 1512 | if (!stream->dummy) { |
| 1509 | if (ret) { | 1513 | ret = vsc9959_mact_stream_set(ocelot, stream_entry, extack); |
| 1510 | kfree(stream_entry); | 1514 | if (ret) { |
| 1511 | return ret; | 1515 | kfree(stream_entry); |
| | 1516 | return ret; |
| | 1517 | } |
| 1512 | } | 1518 | } |
| 1513 | | 1519 | |
| 1514 | list_add_tail(&stream_entry->list, stream_list); | 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 | static void vsc9959_stream_table_del(struct ocelot *ocelot, | 1537 | static void vsc9959_stream_table_del(struct ocelot *ocelot, |
| 1532 | struct felix_stream *stream) | 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 | list_del(&stream->list); | 1543 | list_del(&stream->list); |
| 1537 | kfree(stream); | 1544 | kfree(stream); |
| @@ -1586,14 +1593,64 @@ static int vsc9959_psfp_sfi_set(struct ocelot *ocelot, · 48 unmodified lines |
| 1586 | 10, 100000); | 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 | static int vsc9959_psfp_sfi_table_add(struct ocelot *ocelot, | 1647 | static int vsc9959_psfp_sfi_table_add(struct ocelot *ocelot, |
| 1590 | struct felix_stream_filter *sfi) | 1648 | struct felix_stream_filter *sfi) |
| 1591 | { | 1649 | { |
| 1592 | struct felix_stream_filter *sfi_entry, *tmp; | | |
| 1593 | struct list_head *pos, *q, *last; | 1650 | struct list_head *pos, *q, *last; |
| | 1651 | struct felix_stream_filter *tmp; |
| 1594 | struct ocelot_psfp_list *psfp; | 1652 | struct ocelot_psfp_list *psfp; |
| 1595 | u32 insert = 0; | 1653 | u32 insert = 0; |
| 1596 | int ret; | | |
| 1597 | | 1654 | |
| 1598 | psfp = &ocelot->psfp; | 1655 | psfp = &ocelot->psfp; |
| 1599 | last = &psfp->sfi_list; | 1656 | last = &psfp->sfi_list; |
| @@ -1602,6 +1659,7 @@ static int vsc9959_psfp_sfi_table_add(struct ocelot *ocelot, · 2 unmodified lines |
| 1602 | tmp = list_entry(pos, struct felix_stream_filter, list); | 1659 | tmp = list_entry(pos, struct felix_stream_filter, list); |
| 1603 | if (sfi->sg_valid == tmp->sg_valid && | 1660 | if (sfi->sg_valid == tmp->sg_valid && |
| 1604 | sfi->fm_valid == tmp->fm_valid && | 1661 | sfi->fm_valid == tmp->fm_valid && |
| | 1662 | sfi->portmask == tmp->portmask && |
| 1605 | tmp->sgid == sfi->sgid && | 1663 | tmp->sgid == sfi->sgid && |
| 1606 | tmp->fmid == sfi->fmid) { | 1664 | tmp->fmid == sfi->fmid) { |
| 1607 | sfi->index = tmp->index; | 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 | sfi->index = insert; | 1675 | sfi->index = insert; |
| 1618 | | 1676 | |
| 1619 | sfi_entry = kzalloc(sizeof(*sfi_entry), GFP_KERNEL); | 1677 | return vsc9959_psfp_sfi_list_add(ocelot, sfi, last); |
| 1620 | if (!sfi_entry) | 1678 | } |
| 1621 | return -ENOMEM; | | |
| 1622 | | 1679 | |
| 1623 | memcpy(sfi_entry, sfi, sizeof(*sfi_entry)); | 1680 | static int vsc9959_psfp_sfi_table_add2(struct ocelot *ocelot, |
| 1624 | refcount_set(&sfi_entry->refcount, 1); | 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); | 1690 | psfp = &ocelot->psfp; |
| 1627 | if (ret) { | 1691 | last = &psfp->sfi_list; |
| 1628 | kfree(sfi_entry); | 1692 | |
| 1629 | return ret; | 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 | static struct felix_stream_filter * | 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 | SYS_STAT_CFG); | 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 | struct flow_cls_offload *f) | 1912 | struct flow_cls_offload *f) |
| 1837 | { | 1913 | { |
| 1838 | struct netlink_ext_ack *extack = f->common.extack; | 1914 | struct netlink_ext_ack *extack = f->common.extack; |
| | 1915 | struct felix_stream_filter old_sfi, *sfi_entry; |
| 1839 | struct felix_stream_filter sfi = {0}; | 1916 | struct felix_stream_filter sfi = {0}; |
| 1840 | const struct flow_action_entry *a; | 1917 | const struct flow_action_entry *a; |
| 1841 | struct felix_stream *stream_entry; | 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. */ | 1976 | stream.ports = BIT(port); |
| 1900 | stream_entry = vsc9959_stream_table_lookup(&psfp->stream_list, &stream); | 1977 | stream.port = port; |
| 1901 | if (stream_entry) { | | |
| 1902 | NL_SET_ERR_MSG_MOD(extack, "This stream is already added"); | | |
| 1903 | ret = -EEXIST; | | |
| 1904 | goto err; | | |
| 1905 | } | | |
| 1906 | | 1978 | |
| | 1979 | sfi.portmask = stream.ports; |
| 1907 | sfi.prio_valid = (stream.prio < 0 ? 0 : 1); | 1980 | sfi.prio_valid = (stream.prio < 0 ? 0 : 1); |
| 1908 | sfi.prio = (sfi.prio_valid ? stream.prio : 0); | 1981 | sfi.prio = (sfi.prio_valid ? stream.prio : 0); |
| 1909 | sfi.enable = 1; | 1982 | sfi.enable = 1; |
| 1910 | | 1983 | |
| 1911 | ret = vsc9959_psfp_sfi_table_add(ocelot, &sfi); | 1984 | /* Check if stream is set. */ |
| 1912 | if (ret) | 1985 | stream_entry = vsc9959_stream_table_lookup(&psfp->stream_list, &stream); |
| 1913 | goto err; | 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 | stream.sfid = sfi.index; | 2032 | stream.sfid = sfi.index; |
| 1916 | stream.sfid_valid = 1; | 2033 | stream.sfid_valid = 1; |
| @@ -1936,9 +2053,9 @@ err: · 19 unmodified lines |
| 1936 | static int vsc9959_psfp_filter_del(struct ocelot *ocelot, | 2053 | static int vsc9959_psfp_filter_del(struct ocelot *ocelot, |
| 1937 | struct flow_cls_offload *f) | 2054 | struct flow_cls_offload *f) |
| 1938 | { | 2055 | { |
| | 2056 | struct felix_stream *stream, tmp, *stream_entry; |
| 1939 | static struct felix_stream_filter *sfi; | 2057 | static struct felix_stream_filter *sfi; |
| 1940 | struct ocelot_psfp_list *psfp; | 2058 | struct ocelot_psfp_list *psfp; |
| 1941 | struct felix_stream *stream; | | |
| 1942 | | 2059 | |
| 1943 | psfp = &ocelot->psfp; | 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 | vsc9959_psfp_sfi_table_del(ocelot, stream->sfid); | 2076 | vsc9959_psfp_sfi_table_del(ocelot, stream->sfid); |
| 1960 | | 2077 | |
| | 2078 | memcpy(&tmp, stream, sizeof(tmp)); |
| | 2079 | |
| 1961 | stream->sfid_valid = 0; | 2080 | stream->sfid_valid = 0; |
| 1962 | vsc9959_stream_table_del(ocelot, stream); | 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 | return 0; | 2094 | return 0; |
| 1965 | } | 2095 | } |
| 1966 | | 2096 | |