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