1 #include "ncsi_util.hpp"
2
3 #include <linux/ncsi.h>
4 #include <netlink/genl/ctrl.h>
5 #include <netlink/genl/genl.h>
6 #include <netlink/netlink.h>
7
8 #include <phosphor-logging/lg2.hpp>
9
10 #include <optional>
11 #include <span>
12 #include <vector>
13
14 namespace phosphor
15 {
16 namespace network
17 {
18 namespace ncsi
19 {
20
NCSICommand(uint8_t opcode,uint8_t package,std::optional<uint8_t> channel,std::span<unsigned char> payload)21 NCSICommand::NCSICommand(uint8_t opcode, uint8_t package,
22 std::optional<uint8_t> channel,
23 std::span<unsigned char> payload) :
24 opcode(opcode), package(package), channel(channel)
25 {
26 this->payload.assign(payload.begin(), payload.end());
27 }
28
getChannel()29 uint8_t NCSICommand::getChannel()
30 {
31 return channel.value_or(CHANNEL_ID_NONE);
32 }
33
34 using CallBack = int (*)(struct nl_msg* msg, void* arg);
35
36 namespace internal
37 {
38
39 struct NCSIPacketHeader
40 {
41 uint8_t MCID;
42 uint8_t revision;
43 uint8_t reserved;
44 uint8_t id;
45 uint8_t type;
46 uint8_t channel;
47 uint16_t length;
48 uint32_t rsvd[2];
49 };
50
51 struct NCSIResponsePayload
52 {
53 uint16_t response;
54 uint16_t reason;
55 };
56
57 class NetlinkCommand
58 {
59 public:
60 NetlinkCommand() = delete;
61 ~NetlinkCommand() = default;
62 NetlinkCommand(const NetlinkCommand&) = delete;
63 NetlinkCommand& operator=(const NetlinkCommand&) = delete;
64 NetlinkCommand(NetlinkCommand&&) = default;
65 NetlinkCommand& operator=(NetlinkCommand&&) = default;
NetlinkCommand(int ncsiCmd,int operation=DEFAULT_VALUE,std::span<const unsigned char> p=std::span<const unsigned char> ())66 NetlinkCommand(
67 int ncsiCmd, int operation = DEFAULT_VALUE,
68 std::span<const unsigned char> p = std::span<const unsigned char>()) :
69 ncsi_cmd(ncsiCmd), operation(operation), payload(p)
70 {}
71
72 int ncsi_cmd;
73 int operation;
74 std::span<const unsigned char> payload;
75 };
76
77 using nlMsgPtr = std::unique_ptr<nl_msg, decltype(&::nlmsg_free)>;
78 using nlSocketPtr = std::unique_ptr<nl_sock, decltype(&::nl_socket_free)>;
79
80 struct infoCallBackContext
81 {
82 InterfaceInfo* info;
83 };
84
__anon66112f010102(struct nl_msg* msg, void* arg) 85 CallBack infoCallBack = [](struct nl_msg* msg, void* arg) {
86 if (arg == nullptr)
87 {
88 lg2::error("Internal error: invalid info callback context");
89 return -1;
90 }
91
92 struct infoCallBackContext* info = (struct infoCallBackContext*)arg;
93 using namespace phosphor::network::ncsi;
94 auto nlh = nlmsg_hdr(msg);
95
96 struct nlattr* tb[NCSI_ATTR_MAX + 1] = {nullptr};
97 struct nla_policy ncsiPolicy[NCSI_ATTR_MAX + 1] = {
98 {NLA_UNSPEC, 0, 0}, {NLA_U32, 0, 0}, {NLA_NESTED, 0, 0},
99 {NLA_U32, 0, 0}, {NLA_U32, 0, 0},
100 };
101
102 struct nlattr* packagetb[NCSI_PKG_ATTR_MAX + 1] = {nullptr};
103 struct nla_policy packagePolicy[NCSI_PKG_ATTR_MAX + 1] = {
104 {NLA_UNSPEC, 0, 0}, {NLA_NESTED, 0, 0}, {NLA_U32, 0, 0},
105 {NLA_FLAG, 0, 0}, {NLA_NESTED, 0, 0},
106 };
107
108 struct nlattr* channeltb[NCSI_CHANNEL_ATTR_MAX + 1] = {nullptr};
109 struct nla_policy channelPolicy[NCSI_CHANNEL_ATTR_MAX + 1] = {
110 {NLA_UNSPEC, 0, 0}, {NLA_NESTED, 0, 0}, {NLA_U32, 0, 0},
111 {NLA_FLAG, 0, 0}, {NLA_NESTED, 0, 0}, {NLA_UNSPEC, 0, 0},
112 };
113
114 auto ret = genlmsg_parse(nlh, 0, tb, NCSI_ATTR_MAX, ncsiPolicy);
115 if (!tb[NCSI_ATTR_PACKAGE_LIST])
116 {
117 lg2::error("No Packages");
118 return -1;
119 }
120
121 auto attrTgt = static_cast<nlattr*>(nla_data(tb[NCSI_ATTR_PACKAGE_LIST]));
122 if (!attrTgt)
123 {
124 lg2::error("Package list attribute is null");
125 return -1;
126 }
127
128 auto rem = nla_len(tb[NCSI_ATTR_PACKAGE_LIST]);
129 nla_for_each_nested(attrTgt, tb[NCSI_ATTR_PACKAGE_LIST], rem)
130 {
131 ret = nla_parse_nested(packagetb, NCSI_PKG_ATTR_MAX, attrTgt,
132 packagePolicy);
133 if (ret < 0)
134 {
135 lg2::error("Failed to parse package nested");
136 return -1;
137 }
138
139 PackageInfo pkg;
140
141 if (packagetb[NCSI_PKG_ATTR_ID])
142 {
143 auto attrID = nla_get_u32(packagetb[NCSI_PKG_ATTR_ID]);
144 pkg.id = attrID;
145 }
146 else
147 {
148 lg2::debug("Package with no id");
149 }
150
151 if (packagetb[NCSI_PKG_ATTR_FORCED])
152 {
153 pkg.forced = true;
154 }
155
156 auto channelListTarget = static_cast<nlattr*>(
157 nla_data(packagetb[NCSI_PKG_ATTR_CHANNEL_LIST]));
158
159 auto channelrem = nla_len(packagetb[NCSI_PKG_ATTR_CHANNEL_LIST]);
160 nla_for_each_nested(channelListTarget,
161 packagetb[NCSI_PKG_ATTR_CHANNEL_LIST], channelrem)
162 {
163 ret = nla_parse_nested(channeltb, NCSI_CHANNEL_ATTR_MAX,
164 channelListTarget, channelPolicy);
165 if (ret < 0)
166 {
167 lg2::error("Failed to parse channel nested");
168 continue;
169 }
170
171 ChannelInfo chan;
172
173 if (channeltb[NCSI_CHANNEL_ATTR_ID])
174 {
175 chan.id = nla_get_u32(channeltb[NCSI_CHANNEL_ATTR_ID]);
176 chan.active = !!channeltb[NCSI_CHANNEL_ATTR_ACTIVE];
177 chan.forced = !!channeltb[NCSI_CHANNEL_ATTR_FORCED];
178 }
179 else
180 {
181 lg2::debug("Channel with no ID");
182 continue;
183 }
184
185 if (channeltb[NCSI_CHANNEL_ATTR_VERSION_MAJOR])
186 {
187 chan.version_major =
188 nla_get_u32(channeltb[NCSI_CHANNEL_ATTR_VERSION_MAJOR]);
189 }
190 if (channeltb[NCSI_CHANNEL_ATTR_VERSION_MINOR])
191 {
192 chan.version_minor =
193 nla_get_u32(channeltb[NCSI_CHANNEL_ATTR_VERSION_MINOR]);
194 }
195 if (channeltb[NCSI_CHANNEL_ATTR_VERSION_STR])
196 {
197 chan.version =
198 nla_get_string(channeltb[NCSI_CHANNEL_ATTR_VERSION_STR]);
199 }
200 if (channeltb[NCSI_CHANNEL_ATTR_LINK_STATE])
201 {
202 chan.link_state =
203 nla_get_u32(channeltb[NCSI_CHANNEL_ATTR_LINK_STATE]);
204 }
205 if (channeltb[NCSI_CHANNEL_ATTR_VLAN_LIST])
206 {
207 auto vids = channeltb[NCSI_CHANNEL_ATTR_VLAN_LIST];
208 auto vid = static_cast<nlattr*>(nla_data(vids));
209 auto len = nla_len(vids);
210 while (nla_ok(vid, len))
211 {
212 auto id = nla_get_u16(vid);
213 chan.vlan_ids.push_back(id);
214 vid = nla_next(vid, &len);
215 }
216 }
217 pkg.channels.push_back(chan);
218 }
219
220 info->info->packages.push_back(pkg);
221 }
222 return static_cast<int>(NL_STOP);
223 };
224
225 struct sendCallBackContext
226 {
227 NCSIResponse resp;
228 };
229
__anon66112f010202(struct nl_msg* msg, void* arg) 230 CallBack sendCallBack = [](struct nl_msg* msg, void* arg) {
231 using namespace phosphor::network::ncsi;
232 auto nlh = nlmsg_hdr(msg);
233 struct nlattr* tb[NCSI_ATTR_MAX + 1] = {nullptr};
234 static struct nla_policy ncsiPolicy[NCSI_ATTR_MAX + 1] = {
235 {NLA_UNSPEC, 0, 0}, {NLA_U32, 0, 0}, {NLA_NESTED, 0, 0},
236 {NLA_U32, 0, 0}, {NLA_U32, 0, 0}, {NLA_BINARY, 0, 0},
237 {NLA_FLAG, 0, 0}, {NLA_U32, 0, 0}, {NLA_U32, 0, 0},
238 };
239
240 if (arg == nullptr)
241 {
242 lg2::error("Internal error: invalid send callback context");
243 return -1;
244 }
245
246 struct sendCallBackContext* ctx = (struct sendCallBackContext*)arg;
247
248 auto ret = genlmsg_parse(nlh, 0, tb, NCSI_ATTR_MAX, ncsiPolicy);
249 if (ret)
250 {
251 lg2::error("Failed to parse message");
252 return ret;
253 }
254
255 if (tb[NCSI_ATTR_DATA] == nullptr)
256 {
257 lg2::error("Response: No data");
258 return -1;
259 }
260
261 size_t data_len = nla_len(tb[NCSI_ATTR_DATA]);
262 unsigned char* data = (unsigned char*)nla_data(tb[NCSI_ATTR_DATA]);
263
264 ctx->resp.full_payload.assign(data, data + data_len);
265
266 int rc = ctx->resp.parseFullPayload();
267 if (rc)
268 {
269 return -1;
270 }
271
272 return static_cast<int>(NL_STOP);
273 };
274
applyCmd(NetlinkInterface & interface,const NetlinkCommand & cmd,int package=DEFAULT_VALUE,int channel=DEFAULT_VALUE,int flags=NONE,CallBack function=nullptr,void * arg=nullptr)275 int applyCmd(NetlinkInterface& interface, const NetlinkCommand& cmd,
276 int package = DEFAULT_VALUE, int channel = DEFAULT_VALUE,
277 int flags = NONE, CallBack function = nullptr, void* arg = nullptr)
278 {
279 nlSocketPtr socket(nl_socket_alloc(), &::nl_socket_free);
280 if (socket == nullptr)
281 {
282 lg2::error("Unable to allocate memory for the socket");
283 return -ENOMEM;
284 }
285
286 nl_socket_disable_auto_ack(socket.get());
287
288 auto ret = genl_connect(socket.get());
289 if (ret < 0)
290 {
291 lg2::error("Failed to open the socket , RC : {RC}", "RC", ret);
292 return ret;
293 }
294
295 auto driverID = genl_ctrl_resolve(socket.get(), "NCSI");
296 if (driverID < 0)
297 {
298 lg2::error("Failed to resolve, RC : {RC}", "RC", ret);
299 return driverID;
300 }
301
302 nlMsgPtr msg(nlmsg_alloc(), &::nlmsg_free);
303 if (msg == nullptr)
304 {
305 lg2::error("Unable to allocate memory for the message");
306 return -ENOMEM;
307 }
308
309 auto msgHdr = genlmsg_put(msg.get(), NL_AUTO_PORT, NL_AUTO_SEQ, driverID, 0,
310 flags, cmd.ncsi_cmd, 0);
311 if (!msgHdr)
312 {
313 lg2::error("Unable to add the netlink headers , COMMAND : {COMMAND}",
314 "COMMAND", cmd.ncsi_cmd);
315 return -ENOMEM;
316 }
317
318 if (package != DEFAULT_VALUE)
319 {
320 ret = nla_put_u32(msg.get(), ncsi_nl_attrs::NCSI_ATTR_PACKAGE_ID,
321 package);
322 if (ret < 0)
323 {
324 lg2::error("Failed to set the attribute , RC : {RC} PACKAGE "
325 "{PACKAGE}",
326 "RC", ret, "PACKAGE", lg2::hex, package);
327 return ret;
328 }
329 }
330
331 if (channel != DEFAULT_VALUE)
332 {
333 ret = nla_put_u32(msg.get(), ncsi_nl_attrs::NCSI_ATTR_CHANNEL_ID,
334 channel);
335 if (ret < 0)
336 {
337 lg2::error("Failed to set the attribute , RC : {RC} CHANNEL : "
338 "{CHANNEL}",
339 "RC", ret, "CHANNEL", lg2::hex, channel);
340 return ret;
341 }
342 }
343
344 ret = nla_put_u32(msg.get(), ncsi_nl_attrs::NCSI_ATTR_IFINDEX,
345 interface.ifindex);
346 if (ret < 0)
347 {
348 lg2::error("Failed to set the attribute , RC : {RC} INTERFACE : "
349 "{INTERFACE}",
350 "RC", ret, "INTERFACE", interface);
351 return ret;
352 }
353
354 if ((cmd.ncsi_cmd == ncsi_nl_commands::NCSI_CMD_SET_PACKAGE_MASK) ||
355 (cmd.ncsi_cmd == ncsi_nl_commands::NCSI_CMD_SET_CHANNEL_MASK))
356 {
357 if (cmd.payload.size() != sizeof(unsigned int))
358 {
359 lg2::error("Package/Channel mask must be 32-bits");
360 return -EINVAL;
361 }
362 int maskAttr =
363 cmd.ncsi_cmd == ncsi_nl_commands::NCSI_CMD_SET_PACKAGE_MASK
364 ? NCSI_ATTR_PACKAGE_MASK
365 : NCSI_ATTR_CHANNEL_MASK;
366 ret = nla_put_u32(
367 msg.get(), maskAttr,
368 *(reinterpret_cast<const unsigned int*>(cmd.payload.data())));
369 if (ret < 0)
370 {
371 lg2::error("Failed to set the mask attribute, RC : {RC}", "RC",
372 ret);
373 return ret;
374 }
375 }
376 else if (cmd.ncsi_cmd == ncsi_nl_commands::NCSI_CMD_SEND_CMD)
377 {
378 std::vector<unsigned char> pl(
379 sizeof(NCSIPacketHeader) + cmd.payload.size());
380 NCSIPacketHeader* hdr = (NCSIPacketHeader*)pl.data();
381
382 std::copy(cmd.payload.begin(), cmd.payload.end(),
383 pl.begin() + sizeof(NCSIPacketHeader));
384
385 hdr->type = cmd.operation;
386 hdr->length = htons(cmd.payload.size());
387
388 ret = nla_put(msg.get(), ncsi_nl_attrs::NCSI_ATTR_DATA, pl.size(),
389 pl.data());
390 if (ret < 0)
391 {
392 lg2::error("Failed to set the data attribute, RC : {RC}", "RC",
393 ret);
394 return ret;
395 }
396
397 nl_socket_disable_seq_check(socket.get());
398 }
399
400 // Add a callback function to the socket
401 enum nl_cb_kind cb_kind = function ? NL_CB_CUSTOM : NL_CB_DEFAULT;
402 nl_socket_modify_cb(socket.get(), NL_CB_VALID, cb_kind, function, arg);
403
404 ret = nl_send_auto(socket.get(), msg.get());
405 if (ret < 0)
406 {
407 lg2::error("Failed to send the message , RC : {RC}", "RC", ret);
408 return ret;
409 }
410
411 ret = nl_recvmsgs_default(socket.get());
412 if (ret < 0)
413 {
414 lg2::error("Failed to receive the message , RC : {RC}", "RC", ret);
415 return ret;
416 }
417
418 return 0;
419 }
420
421 } // namespace internal
422
to_string(Interface & interface)423 std::string to_string(Interface& interface)
424 {
425 return interface.toString();
426 }
427
NetlinkInterface(int ifindex)428 NetlinkInterface::NetlinkInterface(int ifindex) : ifindex(ifindex) {}
429
toString()430 std::string NetlinkInterface::toString()
431 {
432 return std::to_string(ifindex);
433 }
434
sendCommand(NCSICommand & cmd)435 std::optional<NCSIResponse> NetlinkInterface::sendCommand(NCSICommand& cmd)
436 {
437 lg2::debug("Send Command, CHANNEL : {CHANNEL} , PACKAGE : {PACKAGE}, "
438 "INTERFACE: {INTERFACE}",
439 "CHANNEL", lg2::hex, cmd.getChannel(), "PACKAGE", lg2::hex,
440 cmd.package, "INTERFACE", this);
441
442 internal::sendCallBackContext ctx{};
443
444 internal::NetlinkCommand nl_cmd(ncsi_nl_commands::NCSI_CMD_SEND_CMD,
445 cmd.opcode, cmd.payload);
446
447 int rc = internal::applyCmd(*this, nl_cmd, cmd.package, cmd.getChannel(),
448 NONE, internal::sendCallBack, &ctx);
449
450 if (rc < 0)
451 {
452 return {};
453 }
454
455 return ctx.resp;
456 }
457
setChannel(int package,int channel)458 int NetlinkInterface::setChannel(int package, int channel)
459 {
460 lg2::debug("Set CHANNEL : {CHANNEL} , PACKAGE : {PACKAGE}, INTERFACE : "
461 "{INTERFACE}",
462 "CHANNEL", lg2::hex, channel, "PACKAGE", lg2::hex, package,
463 "INTERFACE", this);
464
465 internal::NetlinkCommand cmd(ncsi_nl_commands::NCSI_CMD_SET_INTERFACE);
466
467 return internal::applyCmd(*this, cmd, package, channel);
468 }
469
clearInterface()470 int NetlinkInterface::clearInterface()
471 {
472 lg2::debug("ClearInterface , INTERFACE : {INTERFACE}", "INTERFACE", this);
473
474 internal::NetlinkCommand cmd(ncsi_nl_commands::NCSI_CMD_CLEAR_INTERFACE);
475 return internal::applyCmd(*this, cmd);
476 }
477
getInfo(int package)478 std::optional<InterfaceInfo> NetlinkInterface::getInfo(int package)
479 {
480 int rc, flags = package == DEFAULT_VALUE ? NLM_F_DUMP : NONE;
481 InterfaceInfo info;
482
483 lg2::debug("Get Info , PACKAGE : {PACKAGE}, INTERFACE: {INTERFACE}",
484 "PACKAGE", lg2::hex, package, "INTERFACE", this);
485
486 struct internal::infoCallBackContext ctx = {
487 .info = &info,
488 };
489
490 internal::NetlinkCommand cmd(ncsi_nl_commands::NCSI_CMD_PKG_INFO);
491
492 rc = internal::applyCmd(*this, cmd, package, DEFAULT_VALUE, flags,
493 internal::infoCallBack, &ctx);
494
495 if (rc < 0)
496 {
497 return {};
498 }
499
500 return info;
501 }
502
setPackageMask(unsigned int mask)503 int NetlinkInterface::setPackageMask(unsigned int mask)
504 {
505 lg2::debug("Set Package Mask , INTERFACE: {INTERFACE} MASK: {MASK}",
506 "INTERFACE", this, "MASK", lg2::hex, mask);
507 auto payload = std::span<const unsigned char>(
508 reinterpret_cast<const unsigned char*>(&mask),
509 reinterpret_cast<const unsigned char*>(&mask) + sizeof(decltype(mask)));
510
511 internal::NetlinkCommand cmd(ncsi_nl_commands::NCSI_CMD_SET_PACKAGE_MASK, 0,
512 payload);
513 return internal::applyCmd(*this, cmd);
514 }
515
setChannelMask(int package,unsigned int mask)516 int NetlinkInterface::setChannelMask(int package, unsigned int mask)
517 {
518 lg2::debug(
519 "Set Channel Mask , INTERFACE: {INTERFACE}, PACKAGE : {PACKAGE} MASK: {MASK}",
520 "INTERFACE", this, "PACKAGE", lg2::hex, package, "MASK", lg2::hex,
521 mask);
522 auto payload = std::span<const unsigned char>(
523 reinterpret_cast<const unsigned char*>(&mask),
524 reinterpret_cast<const unsigned char*>(&mask) + sizeof(decltype(mask)));
525
526 internal::NetlinkCommand cmd(ncsi_nl_commands::NCSI_CMD_SET_CHANNEL_MASK, 0,
527 payload);
528 return internal::applyCmd(*this, cmd);
529 }
530
parseFullPayload()531 int NCSIResponse::parseFullPayload()
532 {
533 if (this->full_payload.size() < sizeof(internal::NCSIPacketHeader) +
534 sizeof(internal::NCSIResponsePayload))
535 {
536 lg2::error("Response: Not enough data for a response message");
537 return -1;
538 }
539
540 internal::NCSIPacketHeader* respHeader =
541 reinterpret_cast<decltype(respHeader)>(this->full_payload.data());
542
543 unsigned int payloadLen = ntohs(respHeader->length & htons(0x0fff));
544 /* we have determined that the payload size is larger than *respHeader,
545 * so cannot underflow here */
546 if (payloadLen > this->full_payload.size() - sizeof(*respHeader))
547 {
548 lg2::error("Invalid header length {HDRLEN} (vs {LEN}) in response",
549 "HDRLEN", payloadLen, "LEN",
550 this->full_payload.size() - sizeof(*respHeader));
551 return -1;
552 }
553
554 this->opcode = respHeader->type;
555 this->payload =
556 std::span(this->full_payload.begin() + sizeof(*respHeader), payloadLen);
557
558 internal::NCSIResponsePayload* respPayload =
559 reinterpret_cast<decltype(respPayload)>(this->payload.data());
560 this->response = ntohs(respPayload->response);
561 this->reason = ntohs(respPayload->reason);
562
563 return 0;
564 }
565
566 } // namespace ncsi
567 } // namespace network
568 } // namespace phosphor
569