devlink: rename doit callbacks for per-instance dump commands
[platform/kernel/linux-rpi.git] / net / devlink / health.c
1 // SPDX-License-Identifier: GPL-2.0-or-later
2 /*
3  * Copyright (c) 2016 Mellanox Technologies. All rights reserved.
4  * Copyright (c) 2016 Jiri Pirko <jiri@mellanox.com>
5  */
6
7 #include <net/genetlink.h>
8 #include <net/sock.h>
9 #include <trace/events/devlink.h>
10 #include "devl_internal.h"
11
12 struct devlink_fmsg_item {
13         struct list_head list;
14         int attrtype;
15         u8 nla_type;
16         u16 len;
17         int value[];
18 };
19
20 struct devlink_fmsg {
21         struct list_head item_list;
22         bool putting_binary; /* This flag forces enclosing of binary data
23                               * in an array brackets. It forces using
24                               * of designated API:
25                               * devlink_fmsg_binary_pair_nest_start()
26                               * devlink_fmsg_binary_pair_nest_end()
27                               */
28 };
29
30 static struct devlink_fmsg *devlink_fmsg_alloc(void)
31 {
32         struct devlink_fmsg *fmsg;
33
34         fmsg = kzalloc(sizeof(*fmsg), GFP_KERNEL);
35         if (!fmsg)
36                 return NULL;
37
38         INIT_LIST_HEAD(&fmsg->item_list);
39
40         return fmsg;
41 }
42
43 static void devlink_fmsg_free(struct devlink_fmsg *fmsg)
44 {
45         struct devlink_fmsg_item *item, *tmp;
46
47         list_for_each_entry_safe(item, tmp, &fmsg->item_list, list) {
48                 list_del(&item->list);
49                 kfree(item);
50         }
51         kfree(fmsg);
52 }
53
54 struct devlink_health_reporter {
55         struct list_head list;
56         void *priv;
57         const struct devlink_health_reporter_ops *ops;
58         struct devlink *devlink;
59         struct devlink_port *devlink_port;
60         struct devlink_fmsg *dump_fmsg;
61         struct mutex dump_lock; /* lock parallel read/write from dump buffers */
62         u64 graceful_period;
63         bool auto_recover;
64         bool auto_dump;
65         u8 health_state;
66         u64 dump_ts;
67         u64 dump_real_ts;
68         u64 error_count;
69         u64 recovery_count;
70         u64 last_recovery_ts;
71 };
72
73 void *
74 devlink_health_reporter_priv(struct devlink_health_reporter *reporter)
75 {
76         return reporter->priv;
77 }
78 EXPORT_SYMBOL_GPL(devlink_health_reporter_priv);
79
80 static struct devlink_health_reporter *
81 __devlink_health_reporter_find_by_name(struct list_head *reporter_list,
82                                        const char *reporter_name)
83 {
84         struct devlink_health_reporter *reporter;
85
86         list_for_each_entry(reporter, reporter_list, list)
87                 if (!strcmp(reporter->ops->name, reporter_name))
88                         return reporter;
89         return NULL;
90 }
91
92 static struct devlink_health_reporter *
93 devlink_health_reporter_find_by_name(struct devlink *devlink,
94                                      const char *reporter_name)
95 {
96         return __devlink_health_reporter_find_by_name(&devlink->reporter_list,
97                                                       reporter_name);
98 }
99
100 static struct devlink_health_reporter *
101 devlink_port_health_reporter_find_by_name(struct devlink_port *devlink_port,
102                                           const char *reporter_name)
103 {
104         return __devlink_health_reporter_find_by_name(&devlink_port->reporter_list,
105                                                       reporter_name);
106 }
107
108 static struct devlink_health_reporter *
109 __devlink_health_reporter_create(struct devlink *devlink,
110                                  const struct devlink_health_reporter_ops *ops,
111                                  u64 graceful_period, void *priv)
112 {
113         struct devlink_health_reporter *reporter;
114
115         if (WARN_ON(graceful_period && !ops->recover))
116                 return ERR_PTR(-EINVAL);
117
118         reporter = kzalloc(sizeof(*reporter), GFP_KERNEL);
119         if (!reporter)
120                 return ERR_PTR(-ENOMEM);
121
122         reporter->priv = priv;
123         reporter->ops = ops;
124         reporter->devlink = devlink;
125         reporter->graceful_period = graceful_period;
126         reporter->auto_recover = !!ops->recover;
127         reporter->auto_dump = !!ops->dump;
128         mutex_init(&reporter->dump_lock);
129         return reporter;
130 }
131
132 /**
133  * devl_port_health_reporter_create() - create devlink health reporter for
134  *                                      specified port instance
135  *
136  * @port: devlink_port to which health reports will relate
137  * @ops: devlink health reporter ops
138  * @graceful_period: min time (in msec) between recovery attempts
139  * @priv: driver priv pointer
140  */
141 struct devlink_health_reporter *
142 devl_port_health_reporter_create(struct devlink_port *port,
143                                  const struct devlink_health_reporter_ops *ops,
144                                  u64 graceful_period, void *priv)
145 {
146         struct devlink_health_reporter *reporter;
147
148         devl_assert_locked(port->devlink);
149
150         if (__devlink_health_reporter_find_by_name(&port->reporter_list,
151                                                    ops->name))
152                 return ERR_PTR(-EEXIST);
153
154         reporter = __devlink_health_reporter_create(port->devlink, ops,
155                                                     graceful_period, priv);
156         if (IS_ERR(reporter))
157                 return reporter;
158
159         reporter->devlink_port = port;
160         list_add_tail(&reporter->list, &port->reporter_list);
161         return reporter;
162 }
163 EXPORT_SYMBOL_GPL(devl_port_health_reporter_create);
164
165 struct devlink_health_reporter *
166 devlink_port_health_reporter_create(struct devlink_port *port,
167                                     const struct devlink_health_reporter_ops *ops,
168                                     u64 graceful_period, void *priv)
169 {
170         struct devlink_health_reporter *reporter;
171         struct devlink *devlink = port->devlink;
172
173         devl_lock(devlink);
174         reporter = devl_port_health_reporter_create(port, ops,
175                                                     graceful_period, priv);
176         devl_unlock(devlink);
177         return reporter;
178 }
179 EXPORT_SYMBOL_GPL(devlink_port_health_reporter_create);
180
181 /**
182  * devl_health_reporter_create - create devlink health reporter
183  *
184  * @devlink: devlink instance which the health reports will relate
185  * @ops: devlink health reporter ops
186  * @graceful_period: min time (in msec) between recovery attempts
187  * @priv: driver priv pointer
188  */
189 struct devlink_health_reporter *
190 devl_health_reporter_create(struct devlink *devlink,
191                             const struct devlink_health_reporter_ops *ops,
192                             u64 graceful_period, void *priv)
193 {
194         struct devlink_health_reporter *reporter;
195
196         devl_assert_locked(devlink);
197
198         if (devlink_health_reporter_find_by_name(devlink, ops->name))
199                 return ERR_PTR(-EEXIST);
200
201         reporter = __devlink_health_reporter_create(devlink, ops,
202                                                     graceful_period, priv);
203         if (IS_ERR(reporter))
204                 return reporter;
205
206         list_add_tail(&reporter->list, &devlink->reporter_list);
207         return reporter;
208 }
209 EXPORT_SYMBOL_GPL(devl_health_reporter_create);
210
211 struct devlink_health_reporter *
212 devlink_health_reporter_create(struct devlink *devlink,
213                                const struct devlink_health_reporter_ops *ops,
214                                u64 graceful_period, void *priv)
215 {
216         struct devlink_health_reporter *reporter;
217
218         devl_lock(devlink);
219         reporter = devl_health_reporter_create(devlink, ops,
220                                                graceful_period, priv);
221         devl_unlock(devlink);
222         return reporter;
223 }
224 EXPORT_SYMBOL_GPL(devlink_health_reporter_create);
225
226 static void
227 devlink_health_reporter_free(struct devlink_health_reporter *reporter)
228 {
229         mutex_destroy(&reporter->dump_lock);
230         if (reporter->dump_fmsg)
231                 devlink_fmsg_free(reporter->dump_fmsg);
232         kfree(reporter);
233 }
234
235 /**
236  * devl_health_reporter_destroy() - destroy devlink health reporter
237  *
238  * @reporter: devlink health reporter to destroy
239  */
240 void
241 devl_health_reporter_destroy(struct devlink_health_reporter *reporter)
242 {
243         devl_assert_locked(reporter->devlink);
244
245         list_del(&reporter->list);
246         devlink_health_reporter_free(reporter);
247 }
248 EXPORT_SYMBOL_GPL(devl_health_reporter_destroy);
249
250 void
251 devlink_health_reporter_destroy(struct devlink_health_reporter *reporter)
252 {
253         struct devlink *devlink = reporter->devlink;
254
255         devl_lock(devlink);
256         devl_health_reporter_destroy(reporter);
257         devl_unlock(devlink);
258 }
259 EXPORT_SYMBOL_GPL(devlink_health_reporter_destroy);
260
261 static int
262 devlink_nl_health_reporter_fill(struct sk_buff *msg,
263                                 struct devlink_health_reporter *reporter,
264                                 enum devlink_command cmd, u32 portid,
265                                 u32 seq, int flags)
266 {
267         struct devlink *devlink = reporter->devlink;
268         struct nlattr *reporter_attr;
269         void *hdr;
270
271         hdr = genlmsg_put(msg, portid, seq, &devlink_nl_family, flags, cmd);
272         if (!hdr)
273                 return -EMSGSIZE;
274
275         if (devlink_nl_put_handle(msg, devlink))
276                 goto genlmsg_cancel;
277
278         if (reporter->devlink_port) {
279                 if (nla_put_u32(msg, DEVLINK_ATTR_PORT_INDEX, reporter->devlink_port->index))
280                         goto genlmsg_cancel;
281         }
282         reporter_attr = nla_nest_start_noflag(msg,
283                                               DEVLINK_ATTR_HEALTH_REPORTER);
284         if (!reporter_attr)
285                 goto genlmsg_cancel;
286         if (nla_put_string(msg, DEVLINK_ATTR_HEALTH_REPORTER_NAME,
287                            reporter->ops->name))
288                 goto reporter_nest_cancel;
289         if (nla_put_u8(msg, DEVLINK_ATTR_HEALTH_REPORTER_STATE,
290                        reporter->health_state))
291                 goto reporter_nest_cancel;
292         if (nla_put_u64_64bit(msg, DEVLINK_ATTR_HEALTH_REPORTER_ERR_COUNT,
293                               reporter->error_count, DEVLINK_ATTR_PAD))
294                 goto reporter_nest_cancel;
295         if (nla_put_u64_64bit(msg, DEVLINK_ATTR_HEALTH_REPORTER_RECOVER_COUNT,
296                               reporter->recovery_count, DEVLINK_ATTR_PAD))
297                 goto reporter_nest_cancel;
298         if (reporter->ops->recover &&
299             nla_put_u64_64bit(msg, DEVLINK_ATTR_HEALTH_REPORTER_GRACEFUL_PERIOD,
300                               reporter->graceful_period,
301                               DEVLINK_ATTR_PAD))
302                 goto reporter_nest_cancel;
303         if (reporter->ops->recover &&
304             nla_put_u8(msg, DEVLINK_ATTR_HEALTH_REPORTER_AUTO_RECOVER,
305                        reporter->auto_recover))
306                 goto reporter_nest_cancel;
307         if (reporter->dump_fmsg &&
308             nla_put_u64_64bit(msg, DEVLINK_ATTR_HEALTH_REPORTER_DUMP_TS,
309                               jiffies_to_msecs(reporter->dump_ts),
310                               DEVLINK_ATTR_PAD))
311                 goto reporter_nest_cancel;
312         if (reporter->dump_fmsg &&
313             nla_put_u64_64bit(msg, DEVLINK_ATTR_HEALTH_REPORTER_DUMP_TS_NS,
314                               reporter->dump_real_ts, DEVLINK_ATTR_PAD))
315                 goto reporter_nest_cancel;
316         if (reporter->ops->dump &&
317             nla_put_u8(msg, DEVLINK_ATTR_HEALTH_REPORTER_AUTO_DUMP,
318                        reporter->auto_dump))
319                 goto reporter_nest_cancel;
320
321         nla_nest_end(msg, reporter_attr);
322         genlmsg_end(msg, hdr);
323         return 0;
324
325 reporter_nest_cancel:
326         nla_nest_cancel(msg, reporter_attr);
327 genlmsg_cancel:
328         genlmsg_cancel(msg, hdr);
329         return -EMSGSIZE;
330 }
331
332 static struct devlink_health_reporter *
333 devlink_health_reporter_get_from_attrs(struct devlink *devlink,
334                                        struct nlattr **attrs)
335 {
336         struct devlink_port *devlink_port;
337         char *reporter_name;
338
339         if (!attrs[DEVLINK_ATTR_HEALTH_REPORTER_NAME])
340                 return NULL;
341
342         reporter_name = nla_data(attrs[DEVLINK_ATTR_HEALTH_REPORTER_NAME]);
343         devlink_port = devlink_port_get_from_attrs(devlink, attrs);
344         if (IS_ERR(devlink_port))
345                 return devlink_health_reporter_find_by_name(devlink,
346                                                             reporter_name);
347         else
348                 return devlink_port_health_reporter_find_by_name(devlink_port,
349                                                                  reporter_name);
350 }
351
352 static struct devlink_health_reporter *
353 devlink_health_reporter_get_from_info(struct devlink *devlink,
354                                       struct genl_info *info)
355 {
356         return devlink_health_reporter_get_from_attrs(devlink, info->attrs);
357 }
358
359 int devlink_nl_health_reporter_get_doit(struct sk_buff *skb,
360                                         struct genl_info *info)
361 {
362         struct devlink *devlink = info->user_ptr[0];
363         struct devlink_health_reporter *reporter;
364         struct sk_buff *msg;
365         int err;
366
367         reporter = devlink_health_reporter_get_from_info(devlink, info);
368         if (!reporter)
369                 return -EINVAL;
370
371         msg = nlmsg_new(NLMSG_DEFAULT_SIZE, GFP_KERNEL);
372         if (!msg)
373                 return -ENOMEM;
374
375         err = devlink_nl_health_reporter_fill(msg, reporter,
376                                               DEVLINK_CMD_HEALTH_REPORTER_GET,
377                                               info->snd_portid, info->snd_seq,
378                                               0);
379         if (err) {
380                 nlmsg_free(msg);
381                 return err;
382         }
383
384         return genlmsg_reply(msg, info);
385 }
386
387 static int
388 devlink_nl_cmd_health_reporter_get_dump_one(struct sk_buff *msg,
389                                             struct devlink *devlink,
390                                             struct netlink_callback *cb)
391 {
392         struct devlink_nl_dump_state *state = devlink_dump_state(cb);
393         struct devlink_health_reporter *reporter;
394         struct devlink_port *port;
395         unsigned long port_index;
396         int idx = 0;
397         int err;
398
399         list_for_each_entry(reporter, &devlink->reporter_list, list) {
400                 if (idx < state->idx) {
401                         idx++;
402                         continue;
403                 }
404                 err = devlink_nl_health_reporter_fill(msg, reporter,
405                                                       DEVLINK_CMD_HEALTH_REPORTER_GET,
406                                                       NETLINK_CB(cb->skb).portid,
407                                                       cb->nlh->nlmsg_seq,
408                                                       NLM_F_MULTI);
409                 if (err) {
410                         state->idx = idx;
411                         return err;
412                 }
413                 idx++;
414         }
415         xa_for_each(&devlink->ports, port_index, port) {
416                 list_for_each_entry(reporter, &port->reporter_list, list) {
417                         if (idx < state->idx) {
418                                 idx++;
419                                 continue;
420                         }
421                         err = devlink_nl_health_reporter_fill(msg, reporter,
422                                                               DEVLINK_CMD_HEALTH_REPORTER_GET,
423                                                               NETLINK_CB(cb->skb).portid,
424                                                               cb->nlh->nlmsg_seq,
425                                                               NLM_F_MULTI);
426                         if (err) {
427                                 state->idx = idx;
428                                 return err;
429                         }
430                         idx++;
431                 }
432         }
433
434         return 0;
435 }
436
437 const struct devlink_cmd devl_cmd_health_reporter_get = {
438         .dump_one               = devlink_nl_cmd_health_reporter_get_dump_one,
439 };
440
441 int devlink_nl_cmd_health_reporter_set_doit(struct sk_buff *skb,
442                                             struct genl_info *info)
443 {
444         struct devlink *devlink = info->user_ptr[0];
445         struct devlink_health_reporter *reporter;
446
447         reporter = devlink_health_reporter_get_from_info(devlink, info);
448         if (!reporter)
449                 return -EINVAL;
450
451         if (!reporter->ops->recover &&
452             (info->attrs[DEVLINK_ATTR_HEALTH_REPORTER_GRACEFUL_PERIOD] ||
453              info->attrs[DEVLINK_ATTR_HEALTH_REPORTER_AUTO_RECOVER]))
454                 return -EOPNOTSUPP;
455
456         if (!reporter->ops->dump &&
457             info->attrs[DEVLINK_ATTR_HEALTH_REPORTER_AUTO_DUMP])
458                 return -EOPNOTSUPP;
459
460         if (info->attrs[DEVLINK_ATTR_HEALTH_REPORTER_GRACEFUL_PERIOD])
461                 reporter->graceful_period =
462                         nla_get_u64(info->attrs[DEVLINK_ATTR_HEALTH_REPORTER_GRACEFUL_PERIOD]);
463
464         if (info->attrs[DEVLINK_ATTR_HEALTH_REPORTER_AUTO_RECOVER])
465                 reporter->auto_recover =
466                         nla_get_u8(info->attrs[DEVLINK_ATTR_HEALTH_REPORTER_AUTO_RECOVER]);
467
468         if (info->attrs[DEVLINK_ATTR_HEALTH_REPORTER_AUTO_DUMP])
469                 reporter->auto_dump =
470                 nla_get_u8(info->attrs[DEVLINK_ATTR_HEALTH_REPORTER_AUTO_DUMP]);
471
472         return 0;
473 }
474
475 static void devlink_recover_notify(struct devlink_health_reporter *reporter,
476                                    enum devlink_command cmd)
477 {
478         struct devlink *devlink = reporter->devlink;
479         struct sk_buff *msg;
480         int err;
481
482         WARN_ON(cmd != DEVLINK_CMD_HEALTH_REPORTER_RECOVER);
483         ASSERT_DEVLINK_REGISTERED(devlink);
484
485         msg = nlmsg_new(NLMSG_DEFAULT_SIZE, GFP_KERNEL);
486         if (!msg)
487                 return;
488
489         err = devlink_nl_health_reporter_fill(msg, reporter, cmd, 0, 0, 0);
490         if (err) {
491                 nlmsg_free(msg);
492                 return;
493         }
494
495         genlmsg_multicast_netns(&devlink_nl_family, devlink_net(devlink), msg,
496                                 0, DEVLINK_MCGRP_CONFIG, GFP_KERNEL);
497 }
498
499 void
500 devlink_health_reporter_recovery_done(struct devlink_health_reporter *reporter)
501 {
502         reporter->recovery_count++;
503         reporter->last_recovery_ts = jiffies;
504 }
505 EXPORT_SYMBOL_GPL(devlink_health_reporter_recovery_done);
506
507 static int
508 devlink_health_reporter_recover(struct devlink_health_reporter *reporter,
509                                 void *priv_ctx, struct netlink_ext_ack *extack)
510 {
511         int err;
512
513         if (reporter->health_state == DEVLINK_HEALTH_REPORTER_STATE_HEALTHY)
514                 return 0;
515
516         if (!reporter->ops->recover)
517                 return -EOPNOTSUPP;
518
519         err = reporter->ops->recover(reporter, priv_ctx, extack);
520         if (err)
521                 return err;
522
523         devlink_health_reporter_recovery_done(reporter);
524         reporter->health_state = DEVLINK_HEALTH_REPORTER_STATE_HEALTHY;
525         devlink_recover_notify(reporter, DEVLINK_CMD_HEALTH_REPORTER_RECOVER);
526
527         return 0;
528 }
529
530 static void
531 devlink_health_dump_clear(struct devlink_health_reporter *reporter)
532 {
533         if (!reporter->dump_fmsg)
534                 return;
535         devlink_fmsg_free(reporter->dump_fmsg);
536         reporter->dump_fmsg = NULL;
537 }
538
539 static int devlink_health_do_dump(struct devlink_health_reporter *reporter,
540                                   void *priv_ctx,
541                                   struct netlink_ext_ack *extack)
542 {
543         int err;
544
545         if (!reporter->ops->dump)
546                 return 0;
547
548         if (reporter->dump_fmsg)
549                 return 0;
550
551         reporter->dump_fmsg = devlink_fmsg_alloc();
552         if (!reporter->dump_fmsg) {
553                 err = -ENOMEM;
554                 return err;
555         }
556
557         err = devlink_fmsg_obj_nest_start(reporter->dump_fmsg);
558         if (err)
559                 goto dump_err;
560
561         err = reporter->ops->dump(reporter, reporter->dump_fmsg,
562                                   priv_ctx, extack);
563         if (err)
564                 goto dump_err;
565
566         err = devlink_fmsg_obj_nest_end(reporter->dump_fmsg);
567         if (err)
568                 goto dump_err;
569
570         reporter->dump_ts = jiffies;
571         reporter->dump_real_ts = ktime_get_real_ns();
572
573         return 0;
574
575 dump_err:
576         devlink_health_dump_clear(reporter);
577         return err;
578 }
579
580 int devlink_health_report(struct devlink_health_reporter *reporter,
581                           const char *msg, void *priv_ctx)
582 {
583         enum devlink_health_reporter_state prev_health_state;
584         struct devlink *devlink = reporter->devlink;
585         unsigned long recover_ts_threshold;
586         int ret;
587
588         /* write a log message of the current error */
589         WARN_ON(!msg);
590         trace_devlink_health_report(devlink, reporter->ops->name, msg);
591         reporter->error_count++;
592         prev_health_state = reporter->health_state;
593         reporter->health_state = DEVLINK_HEALTH_REPORTER_STATE_ERROR;
594         devlink_recover_notify(reporter, DEVLINK_CMD_HEALTH_REPORTER_RECOVER);
595
596         /* abort if the previous error wasn't recovered */
597         recover_ts_threshold = reporter->last_recovery_ts +
598                                msecs_to_jiffies(reporter->graceful_period);
599         if (reporter->auto_recover &&
600             (prev_health_state != DEVLINK_HEALTH_REPORTER_STATE_HEALTHY ||
601              (reporter->last_recovery_ts && reporter->recovery_count &&
602               time_is_after_jiffies(recover_ts_threshold)))) {
603                 trace_devlink_health_recover_aborted(devlink,
604                                                      reporter->ops->name,
605                                                      reporter->health_state,
606                                                      jiffies -
607                                                      reporter->last_recovery_ts);
608                 return -ECANCELED;
609         }
610
611         if (reporter->auto_dump) {
612                 mutex_lock(&reporter->dump_lock);
613                 /* store current dump of current error, for later analysis */
614                 devlink_health_do_dump(reporter, priv_ctx, NULL);
615                 mutex_unlock(&reporter->dump_lock);
616         }
617
618         if (!reporter->auto_recover)
619                 return 0;
620
621         devl_lock(devlink);
622         ret = devlink_health_reporter_recover(reporter, priv_ctx, NULL);
623         devl_unlock(devlink);
624
625         return ret;
626 }
627 EXPORT_SYMBOL_GPL(devlink_health_report);
628
629 void
630 devlink_health_reporter_state_update(struct devlink_health_reporter *reporter,
631                                      enum devlink_health_reporter_state state)
632 {
633         if (WARN_ON(state != DEVLINK_HEALTH_REPORTER_STATE_HEALTHY &&
634                     state != DEVLINK_HEALTH_REPORTER_STATE_ERROR))
635                 return;
636
637         if (reporter->health_state == state)
638                 return;
639
640         reporter->health_state = state;
641         trace_devlink_health_reporter_state_update(reporter->devlink,
642                                                    reporter->ops->name, state);
643         devlink_recover_notify(reporter, DEVLINK_CMD_HEALTH_REPORTER_RECOVER);
644 }
645 EXPORT_SYMBOL_GPL(devlink_health_reporter_state_update);
646
647 int devlink_nl_cmd_health_reporter_recover_doit(struct sk_buff *skb,
648                                                 struct genl_info *info)
649 {
650         struct devlink *devlink = info->user_ptr[0];
651         struct devlink_health_reporter *reporter;
652
653         reporter = devlink_health_reporter_get_from_info(devlink, info);
654         if (!reporter)
655                 return -EINVAL;
656
657         return devlink_health_reporter_recover(reporter, NULL, info->extack);
658 }
659
660 static int devlink_fmsg_nest_common(struct devlink_fmsg *fmsg,
661                                     int attrtype)
662 {
663         struct devlink_fmsg_item *item;
664
665         item = kzalloc(sizeof(*item), GFP_KERNEL);
666         if (!item)
667                 return -ENOMEM;
668
669         item->attrtype = attrtype;
670         list_add_tail(&item->list, &fmsg->item_list);
671
672         return 0;
673 }
674
675 int devlink_fmsg_obj_nest_start(struct devlink_fmsg *fmsg)
676 {
677         if (fmsg->putting_binary)
678                 return -EINVAL;
679
680         return devlink_fmsg_nest_common(fmsg, DEVLINK_ATTR_FMSG_OBJ_NEST_START);
681 }
682 EXPORT_SYMBOL_GPL(devlink_fmsg_obj_nest_start);
683
684 static int devlink_fmsg_nest_end(struct devlink_fmsg *fmsg)
685 {
686         if (fmsg->putting_binary)
687                 return -EINVAL;
688
689         return devlink_fmsg_nest_common(fmsg, DEVLINK_ATTR_FMSG_NEST_END);
690 }
691
692 int devlink_fmsg_obj_nest_end(struct devlink_fmsg *fmsg)
693 {
694         if (fmsg->putting_binary)
695                 return -EINVAL;
696
697         return devlink_fmsg_nest_end(fmsg);
698 }
699 EXPORT_SYMBOL_GPL(devlink_fmsg_obj_nest_end);
700
701 #define DEVLINK_FMSG_MAX_SIZE (GENLMSG_DEFAULT_SIZE - GENL_HDRLEN - NLA_HDRLEN)
702
703 static int devlink_fmsg_put_name(struct devlink_fmsg *fmsg, const char *name)
704 {
705         struct devlink_fmsg_item *item;
706
707         if (fmsg->putting_binary)
708                 return -EINVAL;
709
710         if (strlen(name) + 1 > DEVLINK_FMSG_MAX_SIZE)
711                 return -EMSGSIZE;
712
713         item = kzalloc(sizeof(*item) + strlen(name) + 1, GFP_KERNEL);
714         if (!item)
715                 return -ENOMEM;
716
717         item->nla_type = NLA_NUL_STRING;
718         item->len = strlen(name) + 1;
719         item->attrtype = DEVLINK_ATTR_FMSG_OBJ_NAME;
720         memcpy(&item->value, name, item->len);
721         list_add_tail(&item->list, &fmsg->item_list);
722
723         return 0;
724 }
725
726 int devlink_fmsg_pair_nest_start(struct devlink_fmsg *fmsg, const char *name)
727 {
728         int err;
729
730         if (fmsg->putting_binary)
731                 return -EINVAL;
732
733         err = devlink_fmsg_nest_common(fmsg, DEVLINK_ATTR_FMSG_PAIR_NEST_START);
734         if (err)
735                 return err;
736
737         err = devlink_fmsg_put_name(fmsg, name);
738         if (err)
739                 return err;
740
741         return 0;
742 }
743 EXPORT_SYMBOL_GPL(devlink_fmsg_pair_nest_start);
744
745 int devlink_fmsg_pair_nest_end(struct devlink_fmsg *fmsg)
746 {
747         if (fmsg->putting_binary)
748                 return -EINVAL;
749
750         return devlink_fmsg_nest_end(fmsg);
751 }
752 EXPORT_SYMBOL_GPL(devlink_fmsg_pair_nest_end);
753
754 int devlink_fmsg_arr_pair_nest_start(struct devlink_fmsg *fmsg,
755                                      const char *name)
756 {
757         int err;
758
759         if (fmsg->putting_binary)
760                 return -EINVAL;
761
762         err = devlink_fmsg_pair_nest_start(fmsg, name);
763         if (err)
764                 return err;
765
766         err = devlink_fmsg_nest_common(fmsg, DEVLINK_ATTR_FMSG_ARR_NEST_START);
767         if (err)
768                 return err;
769
770         return 0;
771 }
772 EXPORT_SYMBOL_GPL(devlink_fmsg_arr_pair_nest_start);
773
774 int devlink_fmsg_arr_pair_nest_end(struct devlink_fmsg *fmsg)
775 {
776         int err;
777
778         if (fmsg->putting_binary)
779                 return -EINVAL;
780
781         err = devlink_fmsg_nest_end(fmsg);
782         if (err)
783                 return err;
784
785         err = devlink_fmsg_nest_end(fmsg);
786         if (err)
787                 return err;
788
789         return 0;
790 }
791 EXPORT_SYMBOL_GPL(devlink_fmsg_arr_pair_nest_end);
792
793 int devlink_fmsg_binary_pair_nest_start(struct devlink_fmsg *fmsg,
794                                         const char *name)
795 {
796         int err;
797
798         err = devlink_fmsg_arr_pair_nest_start(fmsg, name);
799         if (err)
800                 return err;
801
802         fmsg->putting_binary = true;
803         return err;
804 }
805 EXPORT_SYMBOL_GPL(devlink_fmsg_binary_pair_nest_start);
806
807 int devlink_fmsg_binary_pair_nest_end(struct devlink_fmsg *fmsg)
808 {
809         if (!fmsg->putting_binary)
810                 return -EINVAL;
811
812         fmsg->putting_binary = false;
813         return devlink_fmsg_arr_pair_nest_end(fmsg);
814 }
815 EXPORT_SYMBOL_GPL(devlink_fmsg_binary_pair_nest_end);
816
817 static int devlink_fmsg_put_value(struct devlink_fmsg *fmsg,
818                                   const void *value, u16 value_len,
819                                   u8 value_nla_type)
820 {
821         struct devlink_fmsg_item *item;
822
823         if (value_len > DEVLINK_FMSG_MAX_SIZE)
824                 return -EMSGSIZE;
825
826         item = kzalloc(sizeof(*item) + value_len, GFP_KERNEL);
827         if (!item)
828                 return -ENOMEM;
829
830         item->nla_type = value_nla_type;
831         item->len = value_len;
832         item->attrtype = DEVLINK_ATTR_FMSG_OBJ_VALUE_DATA;
833         memcpy(&item->value, value, item->len);
834         list_add_tail(&item->list, &fmsg->item_list);
835
836         return 0;
837 }
838
839 static int devlink_fmsg_bool_put(struct devlink_fmsg *fmsg, bool value)
840 {
841         if (fmsg->putting_binary)
842                 return -EINVAL;
843
844         return devlink_fmsg_put_value(fmsg, &value, sizeof(value), NLA_FLAG);
845 }
846
847 static int devlink_fmsg_u8_put(struct devlink_fmsg *fmsg, u8 value)
848 {
849         if (fmsg->putting_binary)
850                 return -EINVAL;
851
852         return devlink_fmsg_put_value(fmsg, &value, sizeof(value), NLA_U8);
853 }
854
855 int devlink_fmsg_u32_put(struct devlink_fmsg *fmsg, u32 value)
856 {
857         if (fmsg->putting_binary)
858                 return -EINVAL;
859
860         return devlink_fmsg_put_value(fmsg, &value, sizeof(value), NLA_U32);
861 }
862 EXPORT_SYMBOL_GPL(devlink_fmsg_u32_put);
863
864 static int devlink_fmsg_u64_put(struct devlink_fmsg *fmsg, u64 value)
865 {
866         if (fmsg->putting_binary)
867                 return -EINVAL;
868
869         return devlink_fmsg_put_value(fmsg, &value, sizeof(value), NLA_U64);
870 }
871
872 int devlink_fmsg_string_put(struct devlink_fmsg *fmsg, const char *value)
873 {
874         if (fmsg->putting_binary)
875                 return -EINVAL;
876
877         return devlink_fmsg_put_value(fmsg, value, strlen(value) + 1,
878                                       NLA_NUL_STRING);
879 }
880 EXPORT_SYMBOL_GPL(devlink_fmsg_string_put);
881
882 int devlink_fmsg_binary_put(struct devlink_fmsg *fmsg, const void *value,
883                             u16 value_len)
884 {
885         if (!fmsg->putting_binary)
886                 return -EINVAL;
887
888         return devlink_fmsg_put_value(fmsg, value, value_len, NLA_BINARY);
889 }
890 EXPORT_SYMBOL_GPL(devlink_fmsg_binary_put);
891
892 int devlink_fmsg_bool_pair_put(struct devlink_fmsg *fmsg, const char *name,
893                                bool value)
894 {
895         int err;
896
897         err = devlink_fmsg_pair_nest_start(fmsg, name);
898         if (err)
899                 return err;
900
901         err = devlink_fmsg_bool_put(fmsg, value);
902         if (err)
903                 return err;
904
905         err = devlink_fmsg_pair_nest_end(fmsg);
906         if (err)
907                 return err;
908
909         return 0;
910 }
911 EXPORT_SYMBOL_GPL(devlink_fmsg_bool_pair_put);
912
913 int devlink_fmsg_u8_pair_put(struct devlink_fmsg *fmsg, const char *name,
914                              u8 value)
915 {
916         int err;
917
918         err = devlink_fmsg_pair_nest_start(fmsg, name);
919         if (err)
920                 return err;
921
922         err = devlink_fmsg_u8_put(fmsg, value);
923         if (err)
924                 return err;
925
926         err = devlink_fmsg_pair_nest_end(fmsg);
927         if (err)
928                 return err;
929
930         return 0;
931 }
932 EXPORT_SYMBOL_GPL(devlink_fmsg_u8_pair_put);
933
934 int devlink_fmsg_u32_pair_put(struct devlink_fmsg *fmsg, const char *name,
935                               u32 value)
936 {
937         int err;
938
939         err = devlink_fmsg_pair_nest_start(fmsg, name);
940         if (err)
941                 return err;
942
943         err = devlink_fmsg_u32_put(fmsg, value);
944         if (err)
945                 return err;
946
947         err = devlink_fmsg_pair_nest_end(fmsg);
948         if (err)
949                 return err;
950
951         return 0;
952 }
953 EXPORT_SYMBOL_GPL(devlink_fmsg_u32_pair_put);
954
955 int devlink_fmsg_u64_pair_put(struct devlink_fmsg *fmsg, const char *name,
956                               u64 value)
957 {
958         int err;
959
960         err = devlink_fmsg_pair_nest_start(fmsg, name);
961         if (err)
962                 return err;
963
964         err = devlink_fmsg_u64_put(fmsg, value);
965         if (err)
966                 return err;
967
968         err = devlink_fmsg_pair_nest_end(fmsg);
969         if (err)
970                 return err;
971
972         return 0;
973 }
974 EXPORT_SYMBOL_GPL(devlink_fmsg_u64_pair_put);
975
976 int devlink_fmsg_string_pair_put(struct devlink_fmsg *fmsg, const char *name,
977                                  const char *value)
978 {
979         int err;
980
981         err = devlink_fmsg_pair_nest_start(fmsg, name);
982         if (err)
983                 return err;
984
985         err = devlink_fmsg_string_put(fmsg, value);
986         if (err)
987                 return err;
988
989         err = devlink_fmsg_pair_nest_end(fmsg);
990         if (err)
991                 return err;
992
993         return 0;
994 }
995 EXPORT_SYMBOL_GPL(devlink_fmsg_string_pair_put);
996
997 int devlink_fmsg_binary_pair_put(struct devlink_fmsg *fmsg, const char *name,
998                                  const void *value, u32 value_len)
999 {
1000         u32 data_size;
1001         int end_err;
1002         u32 offset;
1003         int err;
1004
1005         err = devlink_fmsg_binary_pair_nest_start(fmsg, name);
1006         if (err)
1007                 return err;
1008
1009         for (offset = 0; offset < value_len; offset += data_size) {
1010                 data_size = value_len - offset;
1011                 if (data_size > DEVLINK_FMSG_MAX_SIZE)
1012                         data_size = DEVLINK_FMSG_MAX_SIZE;
1013                 err = devlink_fmsg_binary_put(fmsg, value + offset, data_size);
1014                 if (err)
1015                         break;
1016                 /* Exit from loop with a break (instead of
1017                  * return) to make sure putting_binary is turned off in
1018                  * devlink_fmsg_binary_pair_nest_end
1019                  */
1020         }
1021
1022         end_err = devlink_fmsg_binary_pair_nest_end(fmsg);
1023         if (end_err)
1024                 err = end_err;
1025
1026         return err;
1027 }
1028 EXPORT_SYMBOL_GPL(devlink_fmsg_binary_pair_put);
1029
1030 static int
1031 devlink_fmsg_item_fill_type(struct devlink_fmsg_item *msg, struct sk_buff *skb)
1032 {
1033         switch (msg->nla_type) {
1034         case NLA_FLAG:
1035         case NLA_U8:
1036         case NLA_U32:
1037         case NLA_U64:
1038         case NLA_NUL_STRING:
1039         case NLA_BINARY:
1040                 return nla_put_u8(skb, DEVLINK_ATTR_FMSG_OBJ_VALUE_TYPE,
1041                                   msg->nla_type);
1042         default:
1043                 return -EINVAL;
1044         }
1045 }
1046
1047 static int
1048 devlink_fmsg_item_fill_data(struct devlink_fmsg_item *msg, struct sk_buff *skb)
1049 {
1050         int attrtype = DEVLINK_ATTR_FMSG_OBJ_VALUE_DATA;
1051         u8 tmp;
1052
1053         switch (msg->nla_type) {
1054         case NLA_FLAG:
1055                 /* Always provide flag data, regardless of its value */
1056                 tmp = *(bool *)msg->value;
1057
1058                 return nla_put_u8(skb, attrtype, tmp);
1059         case NLA_U8:
1060                 return nla_put_u8(skb, attrtype, *(u8 *)msg->value);
1061         case NLA_U32:
1062                 return nla_put_u32(skb, attrtype, *(u32 *)msg->value);
1063         case NLA_U64:
1064                 return nla_put_u64_64bit(skb, attrtype, *(u64 *)msg->value,
1065                                          DEVLINK_ATTR_PAD);
1066         case NLA_NUL_STRING:
1067                 return nla_put_string(skb, attrtype, (char *)&msg->value);
1068         case NLA_BINARY:
1069                 return nla_put(skb, attrtype, msg->len, (void *)&msg->value);
1070         default:
1071                 return -EINVAL;
1072         }
1073 }
1074
1075 static int
1076 devlink_fmsg_prepare_skb(struct devlink_fmsg *fmsg, struct sk_buff *skb,
1077                          int *start)
1078 {
1079         struct devlink_fmsg_item *item;
1080         struct nlattr *fmsg_nlattr;
1081         int err = 0;
1082         int i = 0;
1083
1084         fmsg_nlattr = nla_nest_start_noflag(skb, DEVLINK_ATTR_FMSG);
1085         if (!fmsg_nlattr)
1086                 return -EMSGSIZE;
1087
1088         list_for_each_entry(item, &fmsg->item_list, list) {
1089                 if (i < *start) {
1090                         i++;
1091                         continue;
1092                 }
1093
1094                 switch (item->attrtype) {
1095                 case DEVLINK_ATTR_FMSG_OBJ_NEST_START:
1096                 case DEVLINK_ATTR_FMSG_PAIR_NEST_START:
1097                 case DEVLINK_ATTR_FMSG_ARR_NEST_START:
1098                 case DEVLINK_ATTR_FMSG_NEST_END:
1099                         err = nla_put_flag(skb, item->attrtype);
1100                         break;
1101                 case DEVLINK_ATTR_FMSG_OBJ_VALUE_DATA:
1102                         err = devlink_fmsg_item_fill_type(item, skb);
1103                         if (err)
1104                                 break;
1105                         err = devlink_fmsg_item_fill_data(item, skb);
1106                         break;
1107                 case DEVLINK_ATTR_FMSG_OBJ_NAME:
1108                         err = nla_put_string(skb, item->attrtype,
1109                                              (char *)&item->value);
1110                         break;
1111                 default:
1112                         err = -EINVAL;
1113                         break;
1114                 }
1115                 if (!err)
1116                         *start = ++i;
1117                 else
1118                         break;
1119         }
1120
1121         nla_nest_end(skb, fmsg_nlattr);
1122         return err;
1123 }
1124
1125 static int devlink_fmsg_snd(struct devlink_fmsg *fmsg,
1126                             struct genl_info *info,
1127                             enum devlink_command cmd, int flags)
1128 {
1129         struct nlmsghdr *nlh;
1130         struct sk_buff *skb;
1131         bool last = false;
1132         int index = 0;
1133         void *hdr;
1134         int err;
1135
1136         while (!last) {
1137                 int tmp_index = index;
1138
1139                 skb = genlmsg_new(GENLMSG_DEFAULT_SIZE, GFP_KERNEL);
1140                 if (!skb)
1141                         return -ENOMEM;
1142
1143                 hdr = genlmsg_put(skb, info->snd_portid, info->snd_seq,
1144                                   &devlink_nl_family, flags | NLM_F_MULTI, cmd);
1145                 if (!hdr) {
1146                         err = -EMSGSIZE;
1147                         goto nla_put_failure;
1148                 }
1149
1150                 err = devlink_fmsg_prepare_skb(fmsg, skb, &index);
1151                 if (!err)
1152                         last = true;
1153                 else if (err != -EMSGSIZE || tmp_index == index)
1154                         goto nla_put_failure;
1155
1156                 genlmsg_end(skb, hdr);
1157                 err = genlmsg_reply(skb, info);
1158                 if (err)
1159                         return err;
1160         }
1161
1162         skb = genlmsg_new(GENLMSG_DEFAULT_SIZE, GFP_KERNEL);
1163         if (!skb)
1164                 return -ENOMEM;
1165         nlh = nlmsg_put(skb, info->snd_portid, info->snd_seq,
1166                         NLMSG_DONE, 0, flags | NLM_F_MULTI);
1167         if (!nlh) {
1168                 err = -EMSGSIZE;
1169                 goto nla_put_failure;
1170         }
1171
1172         return genlmsg_reply(skb, info);
1173
1174 nla_put_failure:
1175         nlmsg_free(skb);
1176         return err;
1177 }
1178
1179 static int devlink_fmsg_dumpit(struct devlink_fmsg *fmsg, struct sk_buff *skb,
1180                                struct netlink_callback *cb,
1181                                enum devlink_command cmd)
1182 {
1183         struct devlink_nl_dump_state *state = devlink_dump_state(cb);
1184         int index = state->idx;
1185         int tmp_index = index;
1186         void *hdr;
1187         int err;
1188
1189         hdr = genlmsg_put(skb, NETLINK_CB(cb->skb).portid, cb->nlh->nlmsg_seq,
1190                           &devlink_nl_family, NLM_F_ACK | NLM_F_MULTI, cmd);
1191         if (!hdr) {
1192                 err = -EMSGSIZE;
1193                 goto nla_put_failure;
1194         }
1195
1196         err = devlink_fmsg_prepare_skb(fmsg, skb, &index);
1197         if ((err && err != -EMSGSIZE) || tmp_index == index)
1198                 goto nla_put_failure;
1199
1200         state->idx = index;
1201         genlmsg_end(skb, hdr);
1202         return skb->len;
1203
1204 nla_put_failure:
1205         genlmsg_cancel(skb, hdr);
1206         return err;
1207 }
1208
1209 int devlink_nl_cmd_health_reporter_diagnose_doit(struct sk_buff *skb,
1210                                                  struct genl_info *info)
1211 {
1212         struct devlink *devlink = info->user_ptr[0];
1213         struct devlink_health_reporter *reporter;
1214         struct devlink_fmsg *fmsg;
1215         int err;
1216
1217         reporter = devlink_health_reporter_get_from_info(devlink, info);
1218         if (!reporter)
1219                 return -EINVAL;
1220
1221         if (!reporter->ops->diagnose)
1222                 return -EOPNOTSUPP;
1223
1224         fmsg = devlink_fmsg_alloc();
1225         if (!fmsg)
1226                 return -ENOMEM;
1227
1228         err = devlink_fmsg_obj_nest_start(fmsg);
1229         if (err)
1230                 goto out;
1231
1232         err = reporter->ops->diagnose(reporter, fmsg, info->extack);
1233         if (err)
1234                 goto out;
1235
1236         err = devlink_fmsg_obj_nest_end(fmsg);
1237         if (err)
1238                 goto out;
1239
1240         err = devlink_fmsg_snd(fmsg, info,
1241                                DEVLINK_CMD_HEALTH_REPORTER_DIAGNOSE, 0);
1242
1243 out:
1244         devlink_fmsg_free(fmsg);
1245         return err;
1246 }
1247
1248 static struct devlink_health_reporter *
1249 devlink_health_reporter_get_from_cb(struct netlink_callback *cb)
1250 {
1251         const struct genl_dumpit_info *info = genl_dumpit_info(cb);
1252         struct devlink_health_reporter *reporter;
1253         struct nlattr **attrs = info->attrs;
1254         struct devlink *devlink;
1255
1256         devlink = devlink_get_from_attrs_lock(sock_net(cb->skb->sk), attrs);
1257         if (IS_ERR(devlink))
1258                 return NULL;
1259         devl_unlock(devlink);
1260
1261         reporter = devlink_health_reporter_get_from_attrs(devlink, attrs);
1262         devlink_put(devlink);
1263         return reporter;
1264 }
1265
1266 int devlink_nl_cmd_health_reporter_dump_get_dumpit(struct sk_buff *skb,
1267                                                    struct netlink_callback *cb)
1268 {
1269         struct devlink_nl_dump_state *state = devlink_dump_state(cb);
1270         struct devlink_health_reporter *reporter;
1271         int err;
1272
1273         reporter = devlink_health_reporter_get_from_cb(cb);
1274         if (!reporter)
1275                 return -EINVAL;
1276
1277         if (!reporter->ops->dump)
1278                 return -EOPNOTSUPP;
1279
1280         mutex_lock(&reporter->dump_lock);
1281         if (!state->idx) {
1282                 err = devlink_health_do_dump(reporter, NULL, cb->extack);
1283                 if (err)
1284                         goto unlock;
1285                 state->dump_ts = reporter->dump_ts;
1286         }
1287         if (!reporter->dump_fmsg || state->dump_ts != reporter->dump_ts) {
1288                 NL_SET_ERR_MSG(cb->extack, "Dump trampled, please retry");
1289                 err = -EAGAIN;
1290                 goto unlock;
1291         }
1292
1293         err = devlink_fmsg_dumpit(reporter->dump_fmsg, skb, cb,
1294                                   DEVLINK_CMD_HEALTH_REPORTER_DUMP_GET);
1295 unlock:
1296         mutex_unlock(&reporter->dump_lock);
1297         return err;
1298 }
1299
1300 int devlink_nl_cmd_health_reporter_dump_clear_doit(struct sk_buff *skb,
1301                                                    struct genl_info *info)
1302 {
1303         struct devlink *devlink = info->user_ptr[0];
1304         struct devlink_health_reporter *reporter;
1305
1306         reporter = devlink_health_reporter_get_from_info(devlink, info);
1307         if (!reporter)
1308                 return -EINVAL;
1309
1310         if (!reporter->ops->dump)
1311                 return -EOPNOTSUPP;
1312
1313         mutex_lock(&reporter->dump_lock);
1314         devlink_health_dump_clear(reporter);
1315         mutex_unlock(&reporter->dump_lock);
1316         return 0;
1317 }
1318
1319 int devlink_nl_cmd_health_reporter_test_doit(struct sk_buff *skb,
1320                                              struct genl_info *info)
1321 {
1322         struct devlink *devlink = info->user_ptr[0];
1323         struct devlink_health_reporter *reporter;
1324
1325         reporter = devlink_health_reporter_get_from_info(devlink, info);
1326         if (!reporter)
1327                 return -EINVAL;
1328
1329         if (!reporter->ops->test)
1330                 return -EOPNOTSUPP;
1331
1332         return reporter->ops->test(reporter, info->extack);
1333 }