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