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