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