net: pse-pd: pd692x0: Add support for PSE PI priority feature

+205

1 changed file

expand all

drivers

net

pse-pd

pd692x0.c

+205

drivers/net/pse-pd/pd692x0.c

··· 12 12 #include <linux/of.h> 13 13 #include <linux/platform_device.h> 14 14 #include <linux/pse-pd/pse.h> 15 + #include <linux/regulator/driver.h> 16 + #include <linux/regulator/machine.h> 15 17 16 18 #define PD692X0_PSE_NAME "pd692x0_pse" 17 19 ··· 78 76 PD692X0_MSG_GET_PORT_CLASS, 79 77 PD692X0_MSG_GET_PORT_MEAS, 80 78 PD692X0_MSG_GET_PORT_PARAM, 79 + PD692X0_MSG_GET_POWER_BANK, 80 + PD692X0_MSG_SET_POWER_BANK, 81 81 82 82 /* add new message above here */ 83 83 PD692X0_MSG_CNT ··· 99 95 unsigned long last_cmd_key_time; 100 96 101 97 enum ethtool_c33_pse_admin_state admin_state[PD692X0_MAX_PIS]; 98 + struct regulator_dev *manager_reg[PD692X0_MAX_MANAGERS]; 99 + int manager_pw_budget[PD692X0_MAX_MANAGERS]; 102 100 }; 103 101 104 102 /* Template list of communication messages. The non-null bytes defined here ··· 175 169 .sub = {0x05, 0xc0}, 176 170 .data = {0x4e, 0x4e, 0x4e, 0x4e, 177 171 0x4e, 0x4e, 0x4e, 0x4e}, 172 + }, 173 + [PD692X0_MSG_GET_POWER_BANK] = { 174 + .key = PD692X0_KEY_REQ, 175 + .sub = {0x07, 0x0b, 0x57}, 176 + .data = { 0, 0x4e, 0x4e, 0x4e, 177 + 0x4e, 0x4e, 0x4e, 0x4e}, 178 + }, 179 + [PD692X0_MSG_SET_POWER_BANK] = { 180 + .key = PD692X0_KEY_CMD, 181 + .sub = {0x07, 0x0b, 0x57}, 178 182 }, 179 183 }; 180 184 ··· 755 739 return (buf.data[0] << 4 | buf.data[1]) * 100; 756 740 } 757 741 742 + static int 743 + pd692x0_pi_get_prio(struct pse_controller_dev *pcdev, int id) 744 + { 745 + struct pd692x0_priv *priv = to_pd692x0_priv(pcdev); 746 + struct pd692x0_msg msg, buf = {0}; 747 + int ret; 748 + 749 + ret = pd692x0_fw_unavailable(priv); 750 + if (ret) 751 + return ret; 752 + 753 + msg = pd692x0_msg_template_list[PD692X0_MSG_GET_PORT_PARAM]; 754 + msg.sub[2] = id; 755 + ret = pd692x0_sendrecv_msg(priv, &msg, &buf); 756 + if (ret < 0) 757 + return ret; 758 + if (!buf.data[2] || buf.data[2] > pcdev->pis_prio_max + 1) 759 + return -ERANGE; 760 + 761 + /* PSE core priority start at 0 */ 762 + return buf.data[2] - 1; 763 + } 764 + 758 765 static struct pd692x0_msg_ver pd692x0_get_sw_version(struct pd692x0_priv *priv) 759 766 { 760 767 struct device *dev = &priv->client->dev; ··· 805 766 806 767 struct pd692x0_manager { 807 768 struct device_node *port_node[PD692X0_MAX_MANAGER_PORTS]; 769 + struct device_node *node; 808 770 int nports; 809 771 }; 810 772 ··· 897 857 if (ret) 898 858 goto out; 899 859 860 + of_node_get(node); 861 + manager[manager_id].node = node; 900 862 nmanagers++; 901 863 } 902 864 ··· 911 869 of_node_put(manager[i].port_node[j]); 912 870 manager[i].port_node[j] = NULL; 913 871 } 872 + of_node_put(manager[i].node); 873 + manager[i].node = NULL; 914 874 } 915 875 916 876 of_node_put(node); 917 877 of_node_put(managers_node); 918 878 return ret; 879 + } 880 + 881 + static const struct regulator_ops dummy_ops; 882 + 883 + static struct regulator_dev * 884 + pd692x0_register_manager_regulator(struct device *dev, char *reg_name, 885 + struct device_node *node) 886 + { 887 + struct regulator_init_data *rinit_data; 888 + struct regulator_config rconfig = {0}; 889 + struct regulator_desc *rdesc; 890 + struct regulator_dev *rdev; 891 + 892 + rinit_data = devm_kzalloc(dev, sizeof(*rinit_data), 893 + GFP_KERNEL); 894 + if (!rinit_data) 895 + return ERR_PTR(-ENOMEM); 896 + 897 + rdesc = devm_kzalloc(dev, sizeof(*rdesc), GFP_KERNEL); 898 + if (!rdesc) 899 + return ERR_PTR(-ENOMEM); 900 + 901 + rdesc->name = reg_name; 902 + rdesc->type = REGULATOR_VOLTAGE; 903 + rdesc->ops = &dummy_ops; 904 + rdesc->owner = THIS_MODULE; 905 + 906 + rinit_data->supply_regulator = "vmain"; 907 + 908 + rconfig.dev = dev; 909 + rconfig.init_data = rinit_data; 910 + rconfig.of_node = node; 911 + 912 + rdev = devm_regulator_register(dev, rdesc, &rconfig); 913 + if (IS_ERR(rdev)) { 914 + dev_err_probe(dev, PTR_ERR(rdev), 915 + "Failed to register regulator\n"); 916 + return rdev; 917 + } 918 + 919 + return rdev; 920 + } 921 + 922 + static int 923 + pd692x0_register_managers_regulator(struct pd692x0_priv *priv, 924 + const struct pd692x0_manager *manager, 925 + int nmanagers) 926 + { 927 + struct device *dev = &priv->client->dev; 928 + size_t reg_name_len; 929 + int i; 930 + 931 + /* Each regulator name len is dev name + 12 char + 932 + * int max digit number (10) + 1 933 + */ 934 + reg_name_len = strlen(dev_name(dev)) + 23; 935 + 936 + for (i = 0; i < nmanagers; i++) { 937 + struct regulator_dev *rdev; 938 + char *reg_name; 939 + 940 + reg_name = devm_kzalloc(dev, reg_name_len, GFP_KERNEL); 941 + if (!reg_name) 942 + return -ENOMEM; 943 + snprintf(reg_name, 26, "pse-%s-manager%d", dev_name(dev), i); 944 + rdev = pd692x0_register_manager_regulator(dev, reg_name, 945 + manager[i].node); 946 + if (IS_ERR(rdev)) 947 + return PTR_ERR(rdev); 948 + 949 + priv->manager_reg[i] = rdev; 950 + } 951 + 952 + return 0; 953 + } 954 + 955 + static int 956 + pd692x0_conf_manager_power_budget(struct pd692x0_priv *priv, int id, int pw) 957 + { 958 + struct pd692x0_msg msg, buf; 959 + int ret, pw_mW = pw / 1000; 960 + 961 + msg = pd692x0_msg_template_list[PD692X0_MSG_GET_POWER_BANK]; 962 + msg.data[0] = id; 963 + ret = pd692x0_sendrecv_msg(priv, &msg, &buf); 964 + if (ret < 0) 965 + return ret; 966 + 967 + msg = pd692x0_msg_template_list[PD692X0_MSG_SET_POWER_BANK]; 968 + msg.data[0] = id; 969 + msg.data[1] = pw_mW >> 8; 970 + msg.data[2] = pw_mW & 0xff; 971 + msg.data[3] = buf.sub[2]; 972 + msg.data[4] = buf.data[0]; 973 + msg.data[5] = buf.data[1]; 974 + msg.data[6] = buf.data[2]; 975 + msg.data[7] = buf.data[3]; 976 + return pd692x0_sendrecv_msg(priv, &msg, &buf); 977 + } 978 + 979 + static int 980 + pd692x0_configure_managers(struct pd692x0_priv *priv, int nmanagers) 981 + { 982 + int i, ret; 983 + 984 + for (i = 0; i < nmanagers; i++) { 985 + struct regulator *supply = priv->manager_reg[i]->supply; 986 + int pw_budget; 987 + 988 + pw_budget = regulator_get_unclaimed_power_budget(supply); 989 + /* Max power budget per manager */ 990 + if (pw_budget > 6000000) 991 + pw_budget = 6000000; 992 + ret = regulator_request_power_budget(supply, pw_budget); 993 + if (ret < 0) 994 + return ret; 995 + 996 + priv->manager_pw_budget[i] = pw_budget; 997 + ret = pd692x0_conf_manager_power_budget(priv, i, pw_budget); 998 + if (ret < 0) 999 + return ret; 1000 + } 1001 + 1002 + return 0; 919 1003 } 920 1004 921 1005 static int ··· 1166 998 return ret; 1167 999 1168 1000 nmanagers = ret; 1001 + ret = pd692x0_register_managers_regulator(priv, manager, nmanagers); 1002 + if (ret) 1003 + goto out; 1004 + 1005 + ret = pd692x0_configure_managers(priv, nmanagers); 1006 + if (ret) 1007 + goto out; 1008 + 1169 1009 ret = pd692x0_set_ports_matrix(priv, manager, nmanagers, port_matrix); 1170 1010 if (ret) 1171 1011 goto out; ··· 1184 1008 1185 1009 out: 1186 1010 for (i = 0; i < nmanagers; i++) { 1011 + struct regulator *supply = priv->manager_reg[i]->supply; 1012 + 1013 + regulator_free_power_budget(supply, 1014 + priv->manager_pw_budget[i]); 1015 + 1187 1016 for (j = 0; j < manager[i].nports; j++) 1188 1017 of_node_put(manager[i].port_node[j]); 1018 + of_node_put(manager[i].node); 1189 1019 } 1190 1020 return ret; 1191 1021 } ··· 1253 1071 return pd692x0_sendrecv_msg(priv, &msg, &buf); 1254 1072 } 1255 1073 1074 + static int pd692x0_pi_set_prio(struct pse_controller_dev *pcdev, int id, 1075 + unsigned int prio) 1076 + { 1077 + struct pd692x0_priv *priv = to_pd692x0_priv(pcdev); 1078 + struct pd692x0_msg msg, buf = {0}; 1079 + int ret; 1080 + 1081 + ret = pd692x0_fw_unavailable(priv); 1082 + if (ret) 1083 + return ret; 1084 + 1085 + msg = pd692x0_msg_template_list[PD692X0_MSG_SET_PORT_PARAM]; 1086 + msg.sub[2] = id; 1087 + /* Controller priority from 1 to 3 */ 1088 + msg.data[4] = prio + 1; 1089 + 1090 + return pd692x0_sendrecv_msg(priv, &msg, &buf); 1091 + } 1092 + 1256 1093 static const struct pse_controller_ops pd692x0_ops = { 1257 1094 .setup_pi_matrix = pd692x0_setup_pi_matrix, 1258 1095 .pi_get_admin_state = pd692x0_pi_get_admin_state, ··· 1285 1084 .pi_get_pw_limit = pd692x0_pi_get_pw_limit, 1286 1085 .pi_set_pw_limit = pd692x0_pi_set_pw_limit, 1287 1086 .pi_get_pw_limit_ranges = pd692x0_pi_get_pw_limit_ranges, 1087 + .pi_get_prio = pd692x0_pi_get_prio, 1088 + .pi_set_prio = pd692x0_pi_set_prio, 1288 1089 }; 1289 1090 1290 1091 #define PD692X0_FW_LINE_MAX_SZ 0xff ··· 1703 1500 priv->pcdev.ops = &pd692x0_ops; 1704 1501 priv->pcdev.dev = dev; 1705 1502 priv->pcdev.types = ETHTOOL_PSE_C33; 1503 + priv->pcdev.supp_budget_eval_strategies = PSE_BUDGET_EVAL_STRAT_DYNAMIC; 1504 + priv->pcdev.pis_prio_max = 2; 1706 1505 ret = devm_pse_controller_register(dev, &priv->pcdev); 1707 1506 if (ret) 1708 1507 return dev_err_probe(dev, ret,

Configure Feed

Configure Feed