[PATCH net-next v14 5/7] net: dsa: mv88e6xxx: rmu: Add functionality to get RMON

@@ -1229,6 +1229,80 @@ static int mv88e6xxx_get_sset_count(struct dsa_switch *ds, int port, int sset)
 	return count;
 }
 
+int mv88e6xxx_stats_get_sset_count_rmu(struct dsa_switch *ds, int port, int sset)
+{
+	if (sset != ETH_SS_STATS)
+		return 0;
+
+	return ARRAY_SIZE(mv88e6xxx_hw_stats);
+}
+
+void mv88e6xxx_stats_get_strings_rmu(struct dsa_switch *ds, int port,
+				     u32 stringset, uint8_t *data)
+{
+	struct mv88e6xxx_hw_stat *stat;
+	int i;
+
+	if (stringset != ETH_SS_STATS)
+		return;
+
+	for (i = 0; i < ARRAY_SIZE(mv88e6xxx_hw_stats); i++) {
+		stat = &mv88e6xxx_hw_stats[i];
+		memcpy(data + i * ETH_GSTRING_LEN, stat->string, ETH_GSTRING_LEN);
+	}
+}
+
+int mv88e6xxx_state_get_stats_rmu(struct mv88e6xxx_chip *chip, int port,
+				  __be32 *raw_cnt, int num_cnt, uint64_t *data)
+{
+	struct mv88e6xxx_hw_stat *stat;
+	int offset = 0;
+	u64 high;
+	int i, j;
+
+	for (i = 0, j = 0; i < ARRAY_SIZE(mv88e6xxx_hw_stats); i++) {
+		stat = &mv88e6xxx_hw_stats[i];
+		offset = stat->reg;
+
+		if (stat->type & STATS_TYPE_PORT) {
+			/* The offsets for the port counters below
+			 * are different in the RMU packet.
+			 */
+			switch (stat->reg) {
+			case 0x10: /* sw_in_discards */
+				offset = 0x81;
+				break;
+			case 0x12: /* sw_in_filtered */
+				offset = 0x85;
+				break;
+			case 0x13: /* sw_out_filtered */
+				offset = 0x89;
+				break;
+			default:
+				dev_err(chip->dev,
+					"RMU: port %d wrong offset requested: %d\n",
+					port, stat->reg);
+				return j;
+			}
+		} else if (stat->type & STATS_TYPE_BANK1) {
+			offset += 32;
+		}
+
+		if (offset >= num_cnt)
+			continue;
+
+		data[j] = be32_to_cpu(raw_cnt[offset]);
+
+		if (stat->size == 8) {
+			high = be32_to_cpu(raw_cnt[offset + 1]);
+			data[j] += (high << 32);
+		}
+
+		j++;
+	}
+	return j;
+}
+
 static int mv88e6xxx_stats_get_stats(struct mv88e6xxx_chip *chip, int port,
 				     uint64_t *data, int types,
 				     u16 bank1_select, u16 histogram)

@@ -433,6 +433,7 @@ struct mv88e6xxx_bus_ops {
 	int (*read)(struct mv88e6xxx_chip *chip, int addr, int reg, u16 *val);
 	int (*write)(struct mv88e6xxx_chip *chip, int addr, int reg, u16 val);
 	int (*init)(struct mv88e6xxx_chip *chip);
+	void (*get_rmon)(struct mv88e6xxx_chip *chip, int port, uint64_t *data);
 };
 
 struct mv88e6xxx_mdio_bus {

@@ -822,4 +823,9 @@ static inline void mv88e6xxx_reg_unlock(struct mv88e6xxx_chip *chip)
 
 int mv88e6xxx_fid_map(struct mv88e6xxx_chip *chip, unsigned long *bitmap);
 
+int mv88e6xxx_state_get_stats_rmu(struct mv88e6xxx_chip *chip, int port,
+				  __be32 *raw_cnt, int num_cnt, uint64_t *data);
+int mv88e6xxx_stats_get_sset_count_rmu(struct dsa_switch *ds, int port, int sset);
+void mv88e6xxx_stats_get_strings_rmu(struct dsa_switch *ds, int port,
+				     u32 stringset, uint8_t *data);
 #endif /* _MV88E6XXX_CHIP_H */

@@ -137,6 +137,50 @@ static int mv88e6xxx_rmu_get_id(struct mv88e6xxx_chip *chip, int port)
 	return ret;
 }
 
+static void mv88e6xxx_rmu_stats_get(struct mv88e6xxx_chip *chip, int port, uint64_t *data)
+{
+	u16 req[4] = { MV88E6XXX_RMU_REQ_FORMAT_SOHO,
+		       MV88E6XXX_RMU_REQ_PAD,
+		       MV88E6XXX_RMU_REQ_CODE_DUMP_MIB,
+		       MV88E6XXX_RMU_REQ_DATA};
+	struct mv88e6xxx_dump_mib_resp resp;
+	struct mv88e6xxx_port *p;
+	u8 resp_port;
+	int resp_len;
+	int num_mibs;
+	int ret;
+
+	/* Populate port number in request */
+	req[3] = FIELD_PREP(MV88E6XXX_RMU_REQ_DUMP_MIB_PORT_MASK, port);
+
+	resp_len = sizeof(resp);
+	ret = mv88e6xxx_rmu_send_wait(chip, req, sizeof(req),
+				      &resp, &resp_len);
+	if (ret) {
+		dev_dbg(chip->dev, "RMU: error for command DUMP_MIB %pe port %d\n",
+			ERR_PTR(ret), port);
+		return;
+	}
+
+	/* Got response */
+	ret = mv88e6xxx_rmu_validate_response(&resp.rmu_header, MV88E6XXX_RMU_RESP_CODE_DUMP_MIB);
+	if (ret)
+		return;
+
+	resp_port = FIELD_GET(MV88E6XXX_SOURCE_PORT, resp.portnum);
+	p = &chip->ports[resp_port];
+	if (!p) {
+		dev_err_ratelimited(chip->dev, "RMU: illegal port number in response: %d\n",
+				    resp_port);
+		return;
+	}
+
+	/* Update MIB for port */
+	num_mibs = (resp_len - offsetof(struct mv88e6xxx_dump_mib_resp, mib)) / sizeof(__be32);
+
+	mv88e6xxx_state_get_stats_rmu(chip, port, resp.mib, num_mibs, data);
+}
+
 static void mv88e6xxx_disable_rmu(struct mv88e6xxx_chip *chip)
 {
 	chip->smi_ops = chip->rmu.smi_ops;

@@ -255,6 +299,15 @@ int mv88e6xxx_rmu_setup(struct mv88e6xxx_chip *chip)
 	chip->rmu.smi_ops = chip->smi_ops;
 	chip->rmu.ds_ops = chip->ds->ops;
 
+	/* Change rmu ops with our own pointer */
+	chip->rmu.smi_rmu_ops = (struct mv88e6xxx_bus_ops *)chip->rmu.smi_ops;
+	chip->rmu.smi_rmu_ops->get_rmon = mv88e6xxx_rmu_stats_get;
+
+	/* Also change get stats strings for our own */
+	chip->rmu.ds_rmu_ops = (struct dsa_switch_ops *)chip->ds->ops;
+	chip->rmu.ds_rmu_ops->get_sset_count = mv88e6xxx_stats_get_sset_count_rmu;
+	chip->rmu.ds_rmu_ops->get_strings = mv88e6xxx_stats_get_strings_rmu;
+
 	/* Start disabled, we'll enable RMU in master_state_change */
 	if (chip->info->ops->rmu_disable)
 		return chip->info->ops->rmu_disable(chip);