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     auto data_len = nla_len(tb[NCSI_ATTR_DATA]) - sizeof(NCSIPacketHeader);
231     unsigned char* data =
232         (unsigned char*)nla_data(tb[NCSI_ATTR_DATA]) + sizeof(NCSIPacketHeader);
233     auto s = std::span<const unsigned char>(data, data_len);
234 
235     // Dump the response to stdout. Enhancement: option to save response data
236     std::cout << "Response : " << std::dec << data_len << " bytes" << std::endl;
237     fmt::print("{:02x}", fmt::join(s.begin(), s.end(), " "));
238     std::cout << std::endl;
239 
240     return 0;
241 };
242 
243 int applyCmd(int ifindex, const Command& cmd, int package = DEFAULT_VALUE,
244              int channel = DEFAULT_VALUE, int flags = NONE,
245              CallBack function = nullptr)
246 {
247     int cb_ret = 0;
248     nlSocketPtr socket(nl_socket_alloc(), &::nl_socket_free);
249     if (socket == nullptr)
250     {
251         std::cerr << "Unable to allocate memory for the socket" << std::endl;
252         return -ENOMEM;
253     }
254 
255     auto ret = genl_connect(socket.get());
256     if (ret < 0)
257     {
258         std::cerr << "Failed to open the socket , RC : " << ret << std::endl;
259         return ret;
260     }
261 
262     auto driverID = genl_ctrl_resolve(socket.get(), "NCSI");
263     if (driverID < 0)
264     {
265         std::cerr << "Failed to resolve, RC : " << ret << std::endl;
266         return driverID;
267     }
268 
269     nlMsgPtr msg(nlmsg_alloc(), &::nlmsg_free);
270     if (msg == nullptr)
271     {
272         std::cerr << "Unable to allocate memory for the message" << std::endl;
273         return -ENOMEM;
274     }
275 
276     auto msgHdr = genlmsg_put(msg.get(), 0, 0, driverID, 0, flags, cmd.cmd, 0);
277     if (!msgHdr)
278     {
279         std::cerr << "Unable to add the netlink headers , COMMAND : " << cmd.cmd
280                   << std::endl;
281         return -ENOMEM;
282     }
283 
284     if (package != DEFAULT_VALUE)
285     {
286         ret = nla_put_u32(msg.get(), ncsi_nl_attrs::NCSI_ATTR_PACKAGE_ID,
287                           package);
288         if (ret < 0)
289         {
290             std::cerr << "Failed to set the attribute , RC : " << ret
291                       << "PACKAGE " << std::hex << package << std::endl;
292             return ret;
293         }
294     }
295 
296     if (channel != DEFAULT_VALUE)
297     {
298         ret = nla_put_u32(msg.get(), ncsi_nl_attrs::NCSI_ATTR_CHANNEL_ID,
299                           channel);
300         if (ret < 0)
301         {
302             std::cerr << "Failed to set the attribute , RC : " << ret
303                       << "CHANNEL : " << std::hex << channel << std::endl;
304             return ret;
305         }
306     }
307 
308     ret = nla_put_u32(msg.get(), ncsi_nl_attrs::NCSI_ATTR_IFINDEX, ifindex);
309     if (ret < 0)
310     {
311         std::cerr << "Failed to set the attribute , RC : " << ret
312                   << "INTERFACE : " << std::hex << ifindex << std::endl;
313         return ret;
314     }
315 
316     if (cmd.ncsi_cmd != DEFAULT_VALUE)
317     {
318         std::vector<unsigned char> pl(sizeof(NCSIPacketHeader) +
319                                       cmd.payload.size());
320         NCSIPacketHeader* hdr = (NCSIPacketHeader*)pl.data();
321 
322         std::copy(cmd.payload.begin(), cmd.payload.end(),
323                   pl.begin() + sizeof(NCSIPacketHeader));
324 
325         hdr->type = cmd.ncsi_cmd;
326         hdr->length = htons(cmd.payload.size());
327 
328         ret = nla_put(msg.get(), ncsi_nl_attrs::NCSI_ATTR_DATA, pl.size(),
329                       pl.data());
330         if (ret < 0)
331         {
332             std::cerr << "Failed to set the data attribute, RC : " << ret
333                       << std::endl;
334             return ret;
335         }
336 
337         nl_socket_disable_seq_check(socket.get());
338     }
339 
340     if (function)
341     {
342         cb_ret = 1;
343 
344         // Add a callback function to the socket
345         nl_socket_modify_cb(socket.get(), NL_CB_VALID, NL_CB_CUSTOM, function,
346                             &cb_ret);
347     }
348 
349     ret = nl_send_auto(socket.get(), msg.get());
350     if (ret < 0)
351     {
352         std::cerr << "Failed to send the message , RC : " << ret << std::endl;
353         return ret;
354     }
355 
356     do
357     {
358         ret = nl_recvmsgs_default(socket.get());
359         if (ret < 0)
360         {
361             std::cerr << "Failed to receive the message , RC : " << ret
362                       << std::endl;
363             break;
364         }
365     } while (cb_ret);
366 
367     return ret;
368 }
369 
370 } // namespace internal
371 
372 int sendOemCommand(int ifindex, int package, int channel,
373                    std::span<const unsigned char> payload)
374 {
375     constexpr auto cmd = 0x50;
376 
377     std::cout << "Send OEM Command, CHANNEL : " << std::hex << channel
378               << ", PACKAGE : " << std::hex << package
379               << ", IFINDEX: " << std::hex << ifindex << std::endl;
380     if (!payload.empty())
381     {
382         std::cout << "Payload :";
383         for (auto& i : payload)
384         {
385             std::cout << " " << std::hex << std::setfill('0') << std::setw(2)
386                       << (int)i;
387         }
388         std::cout << std::endl;
389     }
390 
391     return internal::applyCmd(
392         ifindex,
393         internal::Command(ncsi_nl_commands::NCSI_CMD_SEND_CMD, cmd, payload),
394         package, channel, NONE, internal::sendCallBack);
395 }
396 
397 int setChannel(int ifindex, int package, int channel)
398 {
399     std::cout << "Set Channel : " << std::hex << channel
400               << ", PACKAGE : " << std::hex << package
401               << ", IFINDEX :  " << std::hex << ifindex << std::endl;
402     return internal::applyCmd(
403         ifindex, internal::Command(ncsi_nl_commands::NCSI_CMD_SET_INTERFACE),
404         package, channel);
405 }
406 
407 int clearInterface(int ifindex)
408 {
409     std::cout << "ClearInterface , IFINDEX :" << std::hex << ifindex
410               << std::endl;
411     return internal::applyCmd(
412         ifindex, internal::Command(ncsi_nl_commands::NCSI_CMD_CLEAR_INTERFACE));
413 }
414 
415 int getInfo(int ifindex, int package)
416 {
417     std::cout << "Get Info , PACKAGE :  " << std::hex << package
418               << ", IFINDEX :  " << std::hex << ifindex << std::endl;
419     if (package == DEFAULT_VALUE)
420     {
421         return internal::applyCmd(
422             ifindex, internal::Command(ncsi_nl_commands::NCSI_CMD_PKG_INFO),
423             package, DEFAULT_VALUE, NLM_F_DUMP, internal::infoCallBack);
424     }
425     else
426     {
427         return internal::applyCmd(ifindex, ncsi_nl_commands::NCSI_CMD_PKG_INFO,
428                                   package, DEFAULT_VALUE, NONE,
429                                   internal::infoCallBack);
430     }
431 }
432 
433 } // namespace ncsi
434 } // namespace network
435 } // namespace phosphor
436