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