1 #include "ncsi_util.hpp" 2 3 #include <fmt/format.h> 4 #include <linux/ncsi.h> 5 #include <netlink/genl/ctrl.h> 6 #include <netlink/genl/genl.h> 7 #include <netlink/netlink.h> 8 9 #include <iomanip> 10 #include <iostream> 11 #include <phosphor-logging/lg2.hpp> 12 #include <vector> 13 14 namespace phosphor 15 { 16 namespace network 17 { 18 namespace ncsi 19 { 20 21 using CallBack = int (*)(struct nl_msg* msg, void* arg); 22 23 namespace internal 24 { 25 26 struct NCSIPacketHeader 27 { 28 uint8_t MCID; 29 uint8_t revision; 30 uint8_t reserved; 31 uint8_t id; 32 uint8_t type; 33 uint8_t channel; 34 uint16_t length; 35 uint32_t rsvd[2]; 36 }; 37 38 class Command 39 { 40 public: 41 Command() = delete; 42 ~Command() = default; 43 Command(const Command&) = delete; 44 Command& operator=(const Command&) = delete; 45 Command(Command&&) = default; 46 Command& operator=(Command&&) = default; 47 Command( 48 int c, int nc = DEFAULT_VALUE, 49 std::span<const unsigned char> p = std::span<const unsigned char>()) : 50 cmd(c), 51 ncsi_cmd(nc), payload(p) 52 { 53 } 54 55 int cmd; 56 int ncsi_cmd; 57 std::span<const unsigned char> payload; 58 }; 59 60 using nlMsgPtr = std::unique_ptr<nl_msg, decltype(&::nlmsg_free)>; 61 using nlSocketPtr = std::unique_ptr<nl_sock, decltype(&::nl_socket_free)>; 62 63 CallBack infoCallBack = [](struct nl_msg* msg, void* arg) { 64 using namespace phosphor::network::ncsi; 65 auto nlh = nlmsg_hdr(msg); 66 67 struct nlattr* tb[NCSI_ATTR_MAX + 1] = {nullptr}; 68 struct nla_policy ncsiPolicy[NCSI_ATTR_MAX + 1] = { 69 {NLA_UNSPEC, 0, 0}, {NLA_U32, 0, 0}, {NLA_NESTED, 0, 0}, 70 {NLA_U32, 0, 0}, {NLA_U32, 0, 0}, 71 }; 72 73 struct nlattr* packagetb[NCSI_PKG_ATTR_MAX + 1] = {nullptr}; 74 struct nla_policy packagePolicy[NCSI_PKG_ATTR_MAX + 1] = { 75 {NLA_UNSPEC, 0, 0}, {NLA_NESTED, 0, 0}, {NLA_U32, 0, 0}, 76 {NLA_FLAG, 0, 0}, {NLA_NESTED, 0, 0}, 77 }; 78 79 struct nlattr* channeltb[NCSI_CHANNEL_ATTR_MAX + 1] = {nullptr}; 80 struct nla_policy channelPolicy[NCSI_CHANNEL_ATTR_MAX + 1] = { 81 {NLA_UNSPEC, 0, 0}, {NLA_NESTED, 0, 0}, {NLA_U32, 0, 0}, 82 {NLA_FLAG, 0, 0}, {NLA_NESTED, 0, 0}, {NLA_UNSPEC, 0, 0}, 83 }; 84 85 *(int*)arg = 0; 86 87 auto ret = genlmsg_parse(nlh, 0, tb, NCSI_ATTR_MAX, ncsiPolicy); 88 if (!tb[NCSI_ATTR_PACKAGE_LIST]) 89 { 90 lg2::error("No Packages"); 91 return -1; 92 } 93 94 auto attrTgt = static_cast<nlattr*>(nla_data(tb[NCSI_ATTR_PACKAGE_LIST])); 95 if (!attrTgt) 96 { 97 lg2::error("Package list attribute is null"); 98 return -1; 99 } 100 101 auto rem = nla_len(tb[NCSI_ATTR_PACKAGE_LIST]); 102 nla_for_each_nested(attrTgt, tb[NCSI_ATTR_PACKAGE_LIST], rem) 103 { 104 ret = nla_parse_nested(packagetb, NCSI_PKG_ATTR_MAX, attrTgt, 105 packagePolicy); 106 if (ret < 0) 107 { 108 lg2::error("Failed to parse package nested"); 109 return -1; 110 } 111 112 if (packagetb[NCSI_PKG_ATTR_ID]) 113 { 114 auto attrID = nla_get_u32(packagetb[NCSI_PKG_ATTR_ID]); 115 lg2::debug("Package has id : {ATTR_ID}", "ATTR_ID", lg2::hex, 116 attrID); 117 } 118 else 119 { 120 lg2::debug("Package with no id"); 121 } 122 123 if (packagetb[NCSI_PKG_ATTR_FORCED]) 124 { 125 lg2::debug("This package is forced"); 126 } 127 128 auto channelListTarget = static_cast<nlattr*>( 129 nla_data(packagetb[NCSI_PKG_ATTR_CHANNEL_LIST])); 130 131 auto channelrem = nla_len(packagetb[NCSI_PKG_ATTR_CHANNEL_LIST]); 132 nla_for_each_nested(channelListTarget, 133 packagetb[NCSI_PKG_ATTR_CHANNEL_LIST], channelrem) 134 { 135 ret = nla_parse_nested(channeltb, NCSI_CHANNEL_ATTR_MAX, 136 channelListTarget, channelPolicy); 137 if (ret < 0) 138 { 139 lg2::error("Failed to parse channel nested"); 140 return -1; 141 } 142 143 if (channeltb[NCSI_CHANNEL_ATTR_ID]) 144 { 145 auto channel = nla_get_u32(channeltb[NCSI_CHANNEL_ATTR_ID]); 146 if (channeltb[NCSI_CHANNEL_ATTR_ACTIVE]) 147 { 148 lg2::debug("Channel Active : {CHANNEL}", "CHANNEL", 149 lg2::hex, channel); 150 } 151 else 152 { 153 lg2::debug("Channel Not Active : {CHANNEL}", "CHANNEL", 154 lg2::hex, channel); 155 } 156 157 if (channeltb[NCSI_CHANNEL_ATTR_FORCED]) 158 { 159 lg2::debug("Channel is forced"); 160 } 161 } 162 else 163 { 164 lg2::debug("Channel with no ID"); 165 } 166 167 if (channeltb[NCSI_CHANNEL_ATTR_VERSION_MAJOR]) 168 { 169 auto major = 170 nla_get_u32(channeltb[NCSI_CHANNEL_ATTR_VERSION_MAJOR]); 171 lg2::debug("Channel Major Version : {CHANNEL_MAJOR_VERSION}", 172 "CHANNEL_MAJOR_VERSION", lg2::hex, major); 173 } 174 if (channeltb[NCSI_CHANNEL_ATTR_VERSION_MINOR]) 175 { 176 auto minor = 177 nla_get_u32(channeltb[NCSI_CHANNEL_ATTR_VERSION_MINOR]); 178 lg2::debug("Channel Minor Version : {CHANNEL_MINOR_VERSION}", 179 "CHANNEL_MINOR_VERSION", lg2::hex, minor); 180 } 181 if (channeltb[NCSI_CHANNEL_ATTR_VERSION_STR]) 182 { 183 auto str = 184 nla_get_string(channeltb[NCSI_CHANNEL_ATTR_VERSION_STR]); 185 lg2::debug("Channel Version Str : {CHANNEL_VERSION_STR}", 186 "CHANNEL_VERSION_STR", str); 187 } 188 if (channeltb[NCSI_CHANNEL_ATTR_LINK_STATE]) 189 { 190 191 auto link = 192 nla_get_u32(channeltb[NCSI_CHANNEL_ATTR_LINK_STATE]); 193 lg2::debug("Channel Link State : {LINK_STATE}", "LINK_STATE", 194 lg2::hex, link); 195 } 196 if (channeltb[NCSI_CHANNEL_ATTR_VLAN_LIST]) 197 { 198 lg2::debug("Active Vlan ids"); 199 auto vids = channeltb[NCSI_CHANNEL_ATTR_VLAN_LIST]; 200 auto vid = static_cast<nlattr*>(nla_data(vids)); 201 auto len = nla_len(vids); 202 while (nla_ok(vid, len)) 203 { 204 auto id = nla_get_u16(vid); 205 lg2::debug("VID : {VLAN_ID}", "VLAN_ID", id); 206 vid = nla_next(vid, &len); 207 } 208 } 209 } 210 } 211 return (int)NL_SKIP; 212 }; 213 214 CallBack sendCallBack = [](struct nl_msg* msg, void* arg) { 215 using namespace phosphor::network::ncsi; 216 auto nlh = nlmsg_hdr(msg); 217 struct nlattr* tb[NCSI_ATTR_MAX + 1] = {nullptr}; 218 static struct nla_policy ncsiPolicy[NCSI_ATTR_MAX + 1] = { 219 {NLA_UNSPEC, 0, 0}, {NLA_U32, 0, 0}, {NLA_NESTED, 0, 0}, 220 {NLA_U32, 0, 0}, {NLA_U32, 0, 0}, {NLA_BINARY, 0, 0}, 221 {NLA_FLAG, 0, 0}, {NLA_U32, 0, 0}, {NLA_U32, 0, 0}, 222 }; 223 224 *(int*)arg = 0; 225 226 auto ret = genlmsg_parse(nlh, 0, tb, NCSI_ATTR_MAX, ncsiPolicy); 227 if (ret) 228 { 229 lg2::error("Failed to parse package"); 230 return ret; 231 } 232 233 if (tb[NCSI_ATTR_DATA] == nullptr) 234 { 235 lg2::error("Response: No data"); 236 return -1; 237 } 238 239 auto data_len = nla_len(tb[NCSI_ATTR_DATA]) - sizeof(NCSIPacketHeader); 240 unsigned char* data = 241 (unsigned char*)nla_data(tb[NCSI_ATTR_DATA]) + sizeof(NCSIPacketHeader); 242 auto s = std::span<const unsigned char>(data, data_len); 243 244 // Dump the response to stdout. Enhancement: option to save response data 245 lg2::debug("Response : {DATA_LEN} bytes, {DATA}", "DATA_LEN", data_len, 246 "DATA", 247 fmt::format("{:02x}", fmt::join(s.begin(), s.end(), " "))); 248 249 return 0; 250 }; 251 252 int applyCmd(int ifindex, const Command& cmd, int package = DEFAULT_VALUE, 253 int channel = DEFAULT_VALUE, int flags = NONE, 254 CallBack function = nullptr) 255 { 256 int cb_ret = 0; 257 nlSocketPtr socket(nl_socket_alloc(), &::nl_socket_free); 258 if (socket == nullptr) 259 { 260 lg2::error("Unable to allocate memory for the socket"); 261 return -ENOMEM; 262 } 263 264 auto ret = genl_connect(socket.get()); 265 if (ret < 0) 266 { 267 lg2::error("Failed to open the socket , RC : {RC}", "RC", ret); 268 return ret; 269 } 270 271 auto driverID = genl_ctrl_resolve(socket.get(), "NCSI"); 272 if (driverID < 0) 273 { 274 lg2::error("Failed to resolve, RC : {RC}", "RC", ret); 275 return driverID; 276 } 277 278 nlMsgPtr msg(nlmsg_alloc(), &::nlmsg_free); 279 if (msg == nullptr) 280 { 281 lg2::error("Unable to allocate memory for the message"); 282 return -ENOMEM; 283 } 284 285 auto msgHdr = genlmsg_put(msg.get(), 0, 0, driverID, 0, flags, cmd.cmd, 0); 286 if (!msgHdr) 287 { 288 lg2::error("Unable to add the netlink headers , COMMAND : {COMMAND}", 289 "COMMAND", cmd.cmd); 290 return -ENOMEM; 291 } 292 293 if (package != DEFAULT_VALUE) 294 { 295 ret = nla_put_u32(msg.get(), ncsi_nl_attrs::NCSI_ATTR_PACKAGE_ID, 296 package); 297 if (ret < 0) 298 { 299 lg2::error("Failed to set the attribute , RC : {RC} PACKAGE " 300 "{PACKAGE}", 301 "RC", ret, "PACKAGE", lg2::hex, package); 302 return ret; 303 } 304 } 305 306 if (channel != DEFAULT_VALUE) 307 { 308 ret = nla_put_u32(msg.get(), ncsi_nl_attrs::NCSI_ATTR_CHANNEL_ID, 309 channel); 310 if (ret < 0) 311 { 312 lg2::error("Failed to set the attribute , RC : {RC} CHANNEL : " 313 "{CHANNEL}", 314 "RC", ret, "CHANNEL", lg2::hex, channel); 315 return ret; 316 } 317 } 318 319 ret = nla_put_u32(msg.get(), ncsi_nl_attrs::NCSI_ATTR_IFINDEX, ifindex); 320 if (ret < 0) 321 { 322 lg2::error("Failed to set the attribute , RC : {RC} INTERFACE : " 323 "{INTERFACE}", 324 "RC", ret, "INTERFACE", lg2::hex, ifindex); 325 return ret; 326 } 327 328 if (cmd.ncsi_cmd != DEFAULT_VALUE) 329 { 330 std::vector<unsigned char> pl(sizeof(NCSIPacketHeader) + 331 cmd.payload.size()); 332 NCSIPacketHeader* hdr = (NCSIPacketHeader*)pl.data(); 333 334 std::copy(cmd.payload.begin(), cmd.payload.end(), 335 pl.begin() + sizeof(NCSIPacketHeader)); 336 337 hdr->type = cmd.ncsi_cmd; 338 hdr->length = htons(cmd.payload.size()); 339 340 ret = nla_put(msg.get(), ncsi_nl_attrs::NCSI_ATTR_DATA, pl.size(), 341 pl.data()); 342 if (ret < 0) 343 { 344 lg2::error("Failed to set the data attribute, RC : {RC}", "RC", 345 ret); 346 return ret; 347 } 348 349 nl_socket_disable_seq_check(socket.get()); 350 } 351 352 if (function) 353 { 354 cb_ret = 1; 355 356 // Add a callback function to the socket 357 nl_socket_modify_cb(socket.get(), NL_CB_VALID, NL_CB_CUSTOM, function, 358 &cb_ret); 359 } 360 361 ret = nl_send_auto(socket.get(), msg.get()); 362 if (ret < 0) 363 { 364 lg2::error("Failed to send the message , RC : {RC}", "RC", ret); 365 return ret; 366 } 367 368 do 369 { 370 ret = nl_recvmsgs_default(socket.get()); 371 if (ret < 0) 372 { 373 lg2::error("Failed to receive the message , RC : {RC}", "RC", ret); 374 break; 375 } 376 } while (cb_ret); 377 378 return ret; 379 } 380 381 } // namespace internal 382 383 int sendOemCommand(int ifindex, int package, int channel, 384 std::span<const unsigned char> payload) 385 { 386 constexpr auto cmd = 0x50; 387 388 lg2::debug("Send OEM Command, CHANNEL : {CHANNEL} , PACKAGE : {PACKAGE}, " 389 "INTERFACE_INDEX: {INTERFACE_INDEX}", 390 "CHANNEL", lg2::hex, channel, "PACKAGE", lg2::hex, package, 391 "INTERFACE_INDEX", lg2::hex, ifindex); 392 if (!payload.empty()) 393 { 394 std::string payloadStr; 395 for (auto& i : payload) 396 { 397 payloadStr += fmt::format(" {:02x}", (int)i); 398 } 399 lg2::debug("Payload :{PAYLOAD}", "PAYLOAD", payloadStr); 400 } 401 402 return internal::applyCmd( 403 ifindex, 404 internal::Command(ncsi_nl_commands::NCSI_CMD_SEND_CMD, cmd, payload), 405 package, channel, NONE, internal::sendCallBack); 406 } 407 408 int setChannel(int ifindex, int package, int channel) 409 { 410 lg2::debug( 411 "Set CHANNEL : {CHANNEL} , PACKAGE : {PACKAGE}, INTERFACE_INDEX: " 412 "{INTERFACE_INDEX}", 413 "CHANNEL", lg2::hex, channel, "PACKAGE", lg2::hex, package, 414 "INTERFACE_INDEX", lg2::hex, ifindex); 415 return internal::applyCmd( 416 ifindex, internal::Command(ncsi_nl_commands::NCSI_CMD_SET_INTERFACE), 417 package, channel); 418 } 419 420 int clearInterface(int ifindex) 421 { 422 lg2::debug("ClearInterface , INTERFACE_INDEX : {INTERFACE_INDEX}", 423 "INTERFACE_INDEX", lg2::hex, ifindex); 424 return internal::applyCmd( 425 ifindex, internal::Command(ncsi_nl_commands::NCSI_CMD_CLEAR_INTERFACE)); 426 } 427 428 int getInfo(int ifindex, int package) 429 { 430 lg2::debug( 431 "Get Info , PACKAGE : {PACKAGE}, INTERFACE_INDEX: {INTERFACE_INDEX}", 432 "PACKAGE", lg2::hex, package, "INTERFACE_INDEX", lg2::hex, ifindex); 433 if (package == DEFAULT_VALUE) 434 { 435 return internal::applyCmd( 436 ifindex, internal::Command(ncsi_nl_commands::NCSI_CMD_PKG_INFO), 437 package, DEFAULT_VALUE, NLM_F_DUMP, internal::infoCallBack); 438 } 439 else 440 { 441 return internal::applyCmd(ifindex, ncsi_nl_commands::NCSI_CMD_PKG_INFO, 442 package, DEFAULT_VALUE, NONE, 443 internal::infoCallBack); 444 } 445 } 446 447 } // namespace ncsi 448 } // namespace network 449 } // namespace phosphor 450