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