From 8bf10c7536d31ff2e401eb1d2180eda77f2648b3 Mon Sep 17 00:00:00 2001 From: Przemyslaw Czarnota Date: Thu, 16 Dec 2021 15:28:58 +0100 Subject: pmapper: add OMCI support for pmapper DSCP to pbit to gem mappings handling Add functions to set and read DSCP to PCP mapping --- include/netlink/route/link/pmapper.h | 10 + lib/route/link/pmapper.c | 289 ++++++++++++++++++++++++++- libnl-route-3.sym | 4 + 3 files changed, 301 insertions(+), 2 deletions(-) diff --git a/include/netlink/route/link/pmapper.h b/include/netlink/route/link/pmapper.h index 8351e24f..582c4407 100644 --- a/include/netlink/route/link/pmapper.h +++ b/include/netlink/route/link/pmapper.h @@ -14,12 +14,22 @@ extern "C" { #endif +#define RTNL_PMAPPER_MODE_PCP 1 +#define RTNL_PMAPPER_MODE_DSCP 2 +#define RTNL_PMAPPER_MODE_DSCP_ONLY 3 + extern struct rtnl_link *rtnl_link_pmapper_alloc(void); extern int rtnl_link_is_pmapper(struct rtnl_link *); extern int rtnl_link_pmapper_set_default_pcp(struct rtnl_link *, uint8_t); extern int rtnl_link_pmapper_set_pcp_ifindex(struct rtnl_link *, uint8_t, int); extern int rtnl_link_pmapper_get_default_pcp(struct rtnl_link *); extern int rtnl_link_pmapper_get_pcp_ifindex(struct rtnl_link *, uint8_t); +extern int rtnl_link_pmapper_set_dscp_to_pcp(struct rtnl_link *link, + uint8_t dscp, uint8_t pcp); +extern int rtnl_link_pmapper_get_dscp_to_pcp(struct rtnl_link *link, + uint8_t dscp); +int rtnl_link_pmapper_set_mode(struct rtnl_link *link, uint8_t mode); +int rtnl_link_pmapper_get_mode(struct rtnl_link *link); #ifdef __cplusplus } diff --git a/lib/route/link/pmapper.c b/lib/route/link/pmapper.c index b9af2bb6..75c0ac47 100644 --- a/lib/route/link/pmapper.c +++ b/lib/route/link/pmapper.c @@ -16,14 +16,23 @@ /** @cond SKIP */ #define PMAPPER_HAS_DEF (1<<0) #define PMAPPER_HAS_PCP(i) (1 << ((i) + 1)) +#define PMAPPER_HAS_DSCP (1 << 9) +#define PMAPPER_HAS_MODE (1 << 10) + +#define PMAPPER_DSCP_HAS_DSCP(i) \ + (1 << (i)) #define PMAPPER_PCP_COUNT 8 +#define PMAPPER_DSCP_COUNT 64 struct pmapper_info { uint8_t def; uint32_t pcp[8]; uint32_t mask; + uint8_t dscp[64]; + uint64_t dscp_mask; + uint8_t mode; }; /* The NetLink messages the PMapper configuration. */ @@ -38,6 +47,8 @@ enum { IFLA_PON_PMAPPER_PCP_5, IFLA_PON_PMAPPER_PCP_6, IFLA_PON_PMAPPER_PCP_7, + IFLA_PON_PMAPPER_MODE, + IFLA_PON_PMAPPER_DSCP, IFLA_PON_PMAPPER_MAX }; @@ -55,6 +66,143 @@ static struct nla_policy pmapper_policy[IFLA_PON_PMAPPER_MAX + 1] = { [IFLA_PON_PMAPPER_PCP_7] = {.type = NLA_U32}, }; +enum { + IFLA_PON_PMAPPER_DSCP_UNSPEC, + IFLA_PON_PMAPPER_DSCP_0, + IFLA_PON_PMAPPER_DSCP_1, + IFLA_PON_PMAPPER_DSCP_2, + IFLA_PON_PMAPPER_DSCP_3, + IFLA_PON_PMAPPER_DSCP_4, + IFLA_PON_PMAPPER_DSCP_5, + IFLA_PON_PMAPPER_DSCP_6, + IFLA_PON_PMAPPER_DSCP_7, + IFLA_PON_PMAPPER_DSCP_8, + IFLA_PON_PMAPPER_DSCP_9, + IFLA_PON_PMAPPER_DSCP_10, + IFLA_PON_PMAPPER_DSCP_11, + IFLA_PON_PMAPPER_DSCP_12, + IFLA_PON_PMAPPER_DSCP_13, + IFLA_PON_PMAPPER_DSCP_14, + IFLA_PON_PMAPPER_DSCP_15, + IFLA_PON_PMAPPER_DSCP_16, + IFLA_PON_PMAPPER_DSCP_17, + IFLA_PON_PMAPPER_DSCP_18, + IFLA_PON_PMAPPER_DSCP_19, + IFLA_PON_PMAPPER_DSCP_20, + IFLA_PON_PMAPPER_DSCP_21, + IFLA_PON_PMAPPER_DSCP_22, + IFLA_PON_PMAPPER_DSCP_23, + IFLA_PON_PMAPPER_DSCP_24, + IFLA_PON_PMAPPER_DSCP_25, + IFLA_PON_PMAPPER_DSCP_26, + IFLA_PON_PMAPPER_DSCP_27, + IFLA_PON_PMAPPER_DSCP_28, + IFLA_PON_PMAPPER_DSCP_29, + IFLA_PON_PMAPPER_DSCP_30, + IFLA_PON_PMAPPER_DSCP_31, + IFLA_PON_PMAPPER_DSCP_32, + IFLA_PON_PMAPPER_DSCP_33, + IFLA_PON_PMAPPER_DSCP_34, + IFLA_PON_PMAPPER_DSCP_35, + IFLA_PON_PMAPPER_DSCP_36, + IFLA_PON_PMAPPER_DSCP_37, + IFLA_PON_PMAPPER_DSCP_38, + IFLA_PON_PMAPPER_DSCP_39, + IFLA_PON_PMAPPER_DSCP_40, + IFLA_PON_PMAPPER_DSCP_41, + IFLA_PON_PMAPPER_DSCP_42, + IFLA_PON_PMAPPER_DSCP_43, + IFLA_PON_PMAPPER_DSCP_44, + IFLA_PON_PMAPPER_DSCP_45, + IFLA_PON_PMAPPER_DSCP_46, + IFLA_PON_PMAPPER_DSCP_47, + IFLA_PON_PMAPPER_DSCP_48, + IFLA_PON_PMAPPER_DSCP_49, + IFLA_PON_PMAPPER_DSCP_50, + IFLA_PON_PMAPPER_DSCP_51, + IFLA_PON_PMAPPER_DSCP_52, + IFLA_PON_PMAPPER_DSCP_53, + IFLA_PON_PMAPPER_DSCP_54, + IFLA_PON_PMAPPER_DSCP_55, + IFLA_PON_PMAPPER_DSCP_56, + IFLA_PON_PMAPPER_DSCP_57, + IFLA_PON_PMAPPER_DSCP_58, + IFLA_PON_PMAPPER_DSCP_59, + IFLA_PON_PMAPPER_DSCP_60, + IFLA_PON_PMAPPER_DSCP_61, + IFLA_PON_PMAPPER_DSCP_62, + IFLA_PON_PMAPPER_DSCP_63, + IFLA_PON_PMAPPER_DSCP_MAX +}; + +static const struct nla_policy +pmapper_dscp_policy[IFLA_PON_PMAPPER_DSCP_MAX] = { + [IFLA_PON_PMAPPER_DSCP_0] = { .type = NLA_U8 }, + [IFLA_PON_PMAPPER_DSCP_1] = { .type = NLA_U8 }, + [IFLA_PON_PMAPPER_DSCP_2] = { .type = NLA_U8 }, + [IFLA_PON_PMAPPER_DSCP_3] = { .type = NLA_U8 }, + [IFLA_PON_PMAPPER_DSCP_4] = { .type = NLA_U8 }, + [IFLA_PON_PMAPPER_DSCP_5] = { .type = NLA_U8 }, + [IFLA_PON_PMAPPER_DSCP_6] = { .type = NLA_U8 }, + [IFLA_PON_PMAPPER_DSCP_7] = { .type = NLA_U8 }, + [IFLA_PON_PMAPPER_DSCP_8] = { .type = NLA_U8 }, + [IFLA_PON_PMAPPER_DSCP_9] = { .type = NLA_U8 }, + [IFLA_PON_PMAPPER_DSCP_10] = { .type = NLA_U8 }, + [IFLA_PON_PMAPPER_DSCP_11] = { .type = NLA_U8 }, + [IFLA_PON_PMAPPER_DSCP_12] = { .type = NLA_U8 }, + [IFLA_PON_PMAPPER_DSCP_13] = { .type = NLA_U8 }, + [IFLA_PON_PMAPPER_DSCP_14] = { .type = NLA_U8 }, + [IFLA_PON_PMAPPER_DSCP_15] = { .type = NLA_U8 }, + [IFLA_PON_PMAPPER_DSCP_16] = { .type = NLA_U8 }, + [IFLA_PON_PMAPPER_DSCP_17] = { .type = NLA_U8 }, + [IFLA_PON_PMAPPER_DSCP_18] = { .type = NLA_U8 }, + [IFLA_PON_PMAPPER_DSCP_19] = { .type = NLA_U8 }, + [IFLA_PON_PMAPPER_DSCP_20] = { .type = NLA_U8 }, + [IFLA_PON_PMAPPER_DSCP_21] = { .type = NLA_U8 }, + [IFLA_PON_PMAPPER_DSCP_22] = { .type = NLA_U8 }, + [IFLA_PON_PMAPPER_DSCP_23] = { .type = NLA_U8 }, + [IFLA_PON_PMAPPER_DSCP_24] = { .type = NLA_U8 }, + [IFLA_PON_PMAPPER_DSCP_25] = { .type = NLA_U8 }, + [IFLA_PON_PMAPPER_DSCP_26] = { .type = NLA_U8 }, + [IFLA_PON_PMAPPER_DSCP_27] = { .type = NLA_U8 }, + [IFLA_PON_PMAPPER_DSCP_28] = { .type = NLA_U8 }, + [IFLA_PON_PMAPPER_DSCP_29] = { .type = NLA_U8 }, + [IFLA_PON_PMAPPER_DSCP_30] = { .type = NLA_U8 }, + [IFLA_PON_PMAPPER_DSCP_31] = { .type = NLA_U8 }, + [IFLA_PON_PMAPPER_DSCP_32] = { .type = NLA_U8 }, + [IFLA_PON_PMAPPER_DSCP_33] = { .type = NLA_U8 }, + [IFLA_PON_PMAPPER_DSCP_34] = { .type = NLA_U8 }, + [IFLA_PON_PMAPPER_DSCP_35] = { .type = NLA_U8 }, + [IFLA_PON_PMAPPER_DSCP_36] = { .type = NLA_U8 }, + [IFLA_PON_PMAPPER_DSCP_37] = { .type = NLA_U8 }, + [IFLA_PON_PMAPPER_DSCP_38] = { .type = NLA_U8 }, + [IFLA_PON_PMAPPER_DSCP_39] = { .type = NLA_U8 }, + [IFLA_PON_PMAPPER_DSCP_40] = { .type = NLA_U8 }, + [IFLA_PON_PMAPPER_DSCP_41] = { .type = NLA_U8 }, + [IFLA_PON_PMAPPER_DSCP_42] = { .type = NLA_U8 }, + [IFLA_PON_PMAPPER_DSCP_43] = { .type = NLA_U8 }, + [IFLA_PON_PMAPPER_DSCP_44] = { .type = NLA_U8 }, + [IFLA_PON_PMAPPER_DSCP_45] = { .type = NLA_U8 }, + [IFLA_PON_PMAPPER_DSCP_46] = { .type = NLA_U8 }, + [IFLA_PON_PMAPPER_DSCP_47] = { .type = NLA_U8 }, + [IFLA_PON_PMAPPER_DSCP_48] = { .type = NLA_U8 }, + [IFLA_PON_PMAPPER_DSCP_49] = { .type = NLA_U8 }, + [IFLA_PON_PMAPPER_DSCP_50] = { .type = NLA_U8 }, + [IFLA_PON_PMAPPER_DSCP_51] = { .type = NLA_U8 }, + [IFLA_PON_PMAPPER_DSCP_52] = { .type = NLA_U8 }, + [IFLA_PON_PMAPPER_DSCP_53] = { .type = NLA_U8 }, + [IFLA_PON_PMAPPER_DSCP_54] = { .type = NLA_U8 }, + [IFLA_PON_PMAPPER_DSCP_55] = { .type = NLA_U8 }, + [IFLA_PON_PMAPPER_DSCP_56] = { .type = NLA_U8 }, + [IFLA_PON_PMAPPER_DSCP_57] = { .type = NLA_U8 }, + [IFLA_PON_PMAPPER_DSCP_58] = { .type = NLA_U8 }, + [IFLA_PON_PMAPPER_DSCP_59] = { .type = NLA_U8 }, + [IFLA_PON_PMAPPER_DSCP_60] = { .type = NLA_U8 }, + [IFLA_PON_PMAPPER_DSCP_61] = { .type = NLA_U8 }, + [IFLA_PON_PMAPPER_DSCP_62] = { .type = NLA_U8 }, + [IFLA_PON_PMAPPER_DSCP_63] = { .type = NLA_U8 }, +}; + static int pmapper_alloc(struct rtnl_link *link) { struct pmapper_info *pi; @@ -106,6 +254,34 @@ static int pmapper_parse(struct rtnl_link *link, struct nlattr *data, } } + if (pb[IFLA_PON_PMAPPER_MODE]) { + pi->mode = nla_get_u8(pb[IFLA_PON_PMAPPER_MODE]); + pi->mask |= PMAPPER_HAS_MODE; + } + + if (pb[IFLA_PON_PMAPPER_DSCP]) { + struct nlattr *dscp[IFLA_PON_PMAPPER_DSCP_MAX + 1]; + + pi->mask |= PMAPPER_HAS_DSCP; + + err = nla_parse_nested(dscp, IFLA_PON_PMAPPER_DSCP_MAX, + pb[IFLA_PON_PMAPPER_DSCP], + pmapper_dscp_policy); + if (err < 0) + return err; + + for (i = 0; i < PMAPPER_DSCP_COUNT; i++) { + struct nlattr *nl_attr; + + nl_attr = dscp[IFLA_PON_PMAPPER_DSCP_0 + i]; + if (!nl_attr) + continue; + + pi->dscp[i] = nla_get_u8(nl_attr); + pi->dscp_mask |= PMAPPER_DSCP_HAS_DSCP(i); + } + } + err = 0; errout: return err; @@ -137,6 +313,10 @@ static void pmapper_dump_details(struct rtnl_link *link, for (i = 0; i < PMAPPER_PCP_COUNT; ++i) if (pi->mask & PMAPPER_HAS_PCP(i)) nl_dump(p, "pmapper-pcp%d %d\n", i, pi->pcp[i]); + + for (i = 0; i < PMAPPER_DSCP_COUNT; ++i) + if (pi->dscp_mask & PMAPPER_DSCP_HAS_DSCP(i)) + nl_dump(p, "pmapper-dscp%d %d\n", i, pi->dscp[i]); } static int pmapper_clone(struct rtnl_link *dst, struct rtnl_link *src) @@ -158,7 +338,8 @@ static int pmapper_clone(struct rtnl_link *dst, struct rtnl_link *src) static int pmapper_put_attrs(struct nl_msg *msg, struct rtnl_link *link) { struct pmapper_info *pi = link->l_info; - struct nlattr *data; + struct nlattr *data = NULL; + struct nlattr *dscp_data = NULL; int i = 0; data = nla_nest_start(msg, IFLA_INFO_DATA); @@ -173,11 +354,27 @@ static int pmapper_put_attrs(struct nl_msg *msg, struct rtnl_link *link) NLA_PUT_U32(msg, IFLA_PON_PMAPPER_PCP_0 + i, pi->pcp[i]); + if (pi->mask & PMAPPER_HAS_MODE) + NLA_PUT_U8(msg, IFLA_PON_PMAPPER_MODE, pi->mode); + + if (pi->mask & PMAPPER_HAS_DSCP) { + dscp_data = nla_nest_start(msg, IFLA_PON_PMAPPER_DSCP); + for (i = 0; i < PMAPPER_DSCP_COUNT; ++i) + if (pi->dscp_mask & PMAPPER_DSCP_HAS_DSCP(i)) + NLA_PUT_U8(msg, IFLA_PON_PMAPPER_DSCP_0 + i, + pi->dscp[i]); + nla_nest_end(msg, dscp_data); + dscp_data = NULL; + } + nla_nest_end(msg, data); return 0; nla_put_failure: - nla_nest_cancel(msg, data); + if (dscp_data) + nla_nest_cancel(msg, dscp_data); + if (data) + nla_nest_cancel(msg, data); return -NLE_MSGSIZE; } @@ -286,6 +483,94 @@ extern int rtnl_link_pmapper_set_pcp_ifindex(struct rtnl_link *link, return 0; } +/** + * Set DSCP to PCP mapping + * @arg link Link object + * @arg ifindex Interface index + * @arg dscp Dscp + * @arg pcp PCP + * + * @return 0 on success or a negative error code + */ +int rtnl_link_pmapper_set_dscp_to_pcp(struct rtnl_link *link, uint8_t dscp, + uint8_t pcp) +{ + struct pmapper_info *pi = link->l_info; + + IS_PMAPPER_LINK_ASSERT(link); + + if (pcp >= PMAPPER_PCP_COUNT) + return -NLE_RANGE; + if (dscp >= PMAPPER_DSCP_COUNT) + return -NLE_RANGE; + + pi->dscp[dscp] = pcp; + pi->mask |= PMAPPER_HAS_DSCP; + pi->dscp_mask |= PMAPPER_DSCP_HAS_DSCP(dscp); + + return 0; +} + +/** + * Get DSCP to PCP mapping + * @arg link Link object + * @arg DSCP DSCP value + * + * @return PCP or negative error code + */ +int rtnl_link_pmapper_get_dscp_to_pcp(struct rtnl_link *link, uint8_t dscp) +{ + struct pmapper_info *pi = link->l_info; + + IS_PMAPPER_LINK_ASSERT(link); + + if (dscp >= PMAPPER_DSCP_COUNT) + return -NLE_RANGE; + + if (pi->mask & PMAPPER_HAS_DSCP && + pi->dscp_mask & PMAPPER_DSCP_HAS_DSCP(dscp)) + return pi->dscp[dscp]; + else + return -NLE_MISSING_ATTR; +} + +/** + * Set PMAPPER mode + * @arg link Link object + * @arg mode Mode + * + * @return 0 on success or a negative error code + */ +int rtnl_link_pmapper_set_mode(struct rtnl_link *link, uint8_t mode) +{ + struct pmapper_info *pi = link->l_info; + + IS_PMAPPER_LINK_ASSERT(link); + + pi->mode = mode; + pi->mask |= PMAPPER_HAS_MODE; + + return 0; +} + +/** + * Get PMAPPER mode + * @arg link Link object + * + * @return mode on success or a negative error code + */ +int rtnl_link_pmapper_get_mode(struct rtnl_link *link) +{ + struct pmapper_info *pi = link->l_info; + + IS_PMAPPER_LINK_ASSERT(link); + + if (pi->mask & PMAPPER_HAS_MODE) + return pi->mode; + else + return -NLE_MISSING_ATTR; +} + /** * Get PMAPPER Default pcp * @arg link Link object diff --git a/libnl-route-3.sym b/libnl-route-3.sym index 0ede3b95..fccff179 100644 --- a/libnl-route-3.sym +++ b/libnl-route-3.sym @@ -466,6 +466,10 @@ global: rtnl_link_pmapper_set_pcp_ifindex; rtnl_link_pmapper_get_default_pcp; rtnl_link_pmapper_get_pcp_ifindex; + rtnl_link_pmapper_set_dscp_to_pcp; + rtnl_link_pmapper_get_dscp_to_pcp; + rtnl_link_pmapper_set_mode; + rtnl_link_pmapper_get_mode; rtnl_link_pon_alloc; rtnl_link_pon_get_queue_lookup_mode; rtnl_link_pon_set_queue_lookup_mode;