108 lines · c
1// SPDX-License-Identifier: BSD-3-Clause OR GPL-2.02/* Copyright (c) 2020 Marvell International Ltd. All rights reserved */3 4#include <linux/bitfield.h>5#include <linux/bitops.h>6#include <linux/errno.h>7#include <linux/string.h>8 9#include "prestera_dsa.h"10 11#define PRESTERA_DSA_W0_CMD GENMASK(31, 30)12#define PRESTERA_DSA_W0_IS_TAGGED BIT(29)13#define PRESTERA_DSA_W0_DEV_NUM GENMASK(28, 24)14#define PRESTERA_DSA_W0_PORT_NUM GENMASK(23, 19)15#define PRESTERA_DSA_W0_VPT GENMASK(15, 13)16#define PRESTERA_DSA_W0_EXT_BIT BIT(12)17#define PRESTERA_DSA_W0_VID GENMASK(11, 0)18 19#define PRESTERA_DSA_W1_EXT_BIT BIT(31)20#define PRESTERA_DSA_W1_CFI_BIT BIT(30)21#define PRESTERA_DSA_W1_PORT_NUM GENMASK(11, 10)22#define PRESTERA_DSA_W1_MASK_CPU_CODE GENMASK(7, 0)23 24#define PRESTERA_DSA_W2_EXT_BIT BIT(31)25#define PRESTERA_DSA_W2_PORT_NUM BIT(20)26 27#define PRESTERA_DSA_W3_VID GENMASK(30, 27)28#define PRESTERA_DSA_W3_DST_EPORT GENMASK(23, 7)29#define PRESTERA_DSA_W3_DEV_NUM GENMASK(6, 0)30 31#define PRESTERA_DSA_VID GENMASK(15, 12)32#define PRESTERA_DSA_DEV_NUM GENMASK(11, 5)33 34int prestera_dsa_parse(struct prestera_dsa *dsa, const u8 *dsa_buf)35{36 __be32 *dsa_words = (__be32 *)dsa_buf;37 enum prestera_dsa_cmd cmd;38 u32 words[4];39 u32 field;40 41 words[0] = ntohl(dsa_words[0]);42 words[1] = ntohl(dsa_words[1]);43 words[2] = ntohl(dsa_words[2]);44 words[3] = ntohl(dsa_words[3]);45 46 /* set the common parameters */47 cmd = (enum prestera_dsa_cmd)FIELD_GET(PRESTERA_DSA_W0_CMD, words[0]);48 49 /* only to CPU is supported */50 if (unlikely(cmd != PRESTERA_DSA_CMD_TO_CPU))51 return -EINVAL;52 53 if (FIELD_GET(PRESTERA_DSA_W0_EXT_BIT, words[0]) == 0)54 return -EINVAL;55 if (FIELD_GET(PRESTERA_DSA_W1_EXT_BIT, words[1]) == 0)56 return -EINVAL;57 if (FIELD_GET(PRESTERA_DSA_W2_EXT_BIT, words[2]) == 0)58 return -EINVAL;59 60 field = FIELD_GET(PRESTERA_DSA_W3_VID, words[3]);61 62 dsa->vlan.is_tagged = FIELD_GET(PRESTERA_DSA_W0_IS_TAGGED, words[0]);63 dsa->vlan.cfi_bit = FIELD_GET(PRESTERA_DSA_W1_CFI_BIT, words[1]);64 dsa->vlan.vpt = FIELD_GET(PRESTERA_DSA_W0_VPT, words[0]);65 dsa->vlan.vid = FIELD_GET(PRESTERA_DSA_W0_VID, words[0]);66 dsa->vlan.vid &= ~PRESTERA_DSA_VID;67 dsa->vlan.vid |= FIELD_PREP(PRESTERA_DSA_VID, field);68 69 field = FIELD_GET(PRESTERA_DSA_W3_DEV_NUM, words[3]);70 71 dsa->hw_dev_num = FIELD_GET(PRESTERA_DSA_W0_DEV_NUM, words[0]);72 dsa->hw_dev_num |= FIELD_PREP(PRESTERA_DSA_DEV_NUM, field);73 74 dsa->port_num = (FIELD_GET(PRESTERA_DSA_W0_PORT_NUM, words[0]) << 0) |75 (FIELD_GET(PRESTERA_DSA_W1_PORT_NUM, words[1]) << 5) |76 (FIELD_GET(PRESTERA_DSA_W2_PORT_NUM, words[2]) << 7);77 78 dsa->cpu_code = FIELD_GET(PRESTERA_DSA_W1_MASK_CPU_CODE, words[1]);79 80 return 0;81}82 83int prestera_dsa_build(const struct prestera_dsa *dsa, u8 *dsa_buf)84{85 __be32 *dsa_words = (__be32 *)dsa_buf;86 u32 dev_num = dsa->hw_dev_num;87 u32 words[4] = { 0 };88 89 words[0] |= FIELD_PREP(PRESTERA_DSA_W0_CMD, PRESTERA_DSA_CMD_FROM_CPU);90 91 words[0] |= FIELD_PREP(PRESTERA_DSA_W0_DEV_NUM, dev_num);92 dev_num = FIELD_GET(PRESTERA_DSA_DEV_NUM, dev_num);93 words[3] |= FIELD_PREP(PRESTERA_DSA_W3_DEV_NUM, dev_num);94 95 words[3] |= FIELD_PREP(PRESTERA_DSA_W3_DST_EPORT, dsa->port_num);96 97 words[0] |= FIELD_PREP(PRESTERA_DSA_W0_EXT_BIT, 1);98 words[1] |= FIELD_PREP(PRESTERA_DSA_W1_EXT_BIT, 1);99 words[2] |= FIELD_PREP(PRESTERA_DSA_W2_EXT_BIT, 1);100 101 dsa_words[0] = htonl(words[0]);102 dsa_words[1] = htonl(words[1]);103 dsa_words[2] = htonl(words[2]);104 dsa_words[3] = htonl(words[3]);105 106 return 0;107}108