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