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