kernel: Make the patches apply on top of 4.19
[openwrt/staging/chunkeey.git] / target / linux / generic / files / drivers / net / phy / swconfig_leds.c
1 /*
2 * swconfig_led.c: LED trigger support for the switch configuration API
3 *
4 * Copyright (C) 2011 Gabor Juhos <juhosg@openwrt.org>
5 *
6 * This program is free software; you can redistribute it and/or
7 * modify it under the terms of the GNU General Public License
8 * as published by the Free Software Foundation; either version 2
9 * of the License, or (at your option) any later version.
10 *
11 */
12
13 #ifdef CONFIG_SWCONFIG_LEDS
14
15 #include <linux/leds.h>
16 #include <linux/ctype.h>
17 #include <linux/device.h>
18 #include <linux/workqueue.h>
19
20 #define SWCONFIG_LED_TIMER_INTERVAL (HZ / 10)
21 #define SWCONFIG_LED_NUM_PORTS 32
22
23 #define SWCONFIG_LED_PORT_SPEED_NA 0x01 /* unknown speed */
24 #define SWCONFIG_LED_PORT_SPEED_10 0x02 /* 10 Mbps */
25 #define SWCONFIG_LED_PORT_SPEED_100 0x04 /* 100 Mbps */
26 #define SWCONFIG_LED_PORT_SPEED_1000 0x08 /* 1000 Mbps */
27 #define SWCONFIG_LED_PORT_SPEED_ALL (SWCONFIG_LED_PORT_SPEED_NA | \
28 SWCONFIG_LED_PORT_SPEED_10 | \
29 SWCONFIG_LED_PORT_SPEED_100 | \
30 SWCONFIG_LED_PORT_SPEED_1000)
31
32 #define SWCONFIG_LED_MODE_LINK 0x01
33 #define SWCONFIG_LED_MODE_TX 0x02
34 #define SWCONFIG_LED_MODE_RX 0x04
35 #define SWCONFIG_LED_MODE_TXRX (SWCONFIG_LED_MODE_TX | \
36 SWCONFIG_LED_MODE_RX)
37 #define SWCONFIG_LED_MODE_ALL (SWCONFIG_LED_MODE_LINK | \
38 SWCONFIG_LED_MODE_TX | \
39 SWCONFIG_LED_MODE_RX)
40
41 struct switch_led_trigger {
42 struct led_trigger trig;
43 struct switch_dev *swdev;
44
45 struct delayed_work sw_led_work;
46 u32 port_mask;
47 u32 port_link;
48 unsigned long long port_tx_traffic[SWCONFIG_LED_NUM_PORTS];
49 unsigned long long port_rx_traffic[SWCONFIG_LED_NUM_PORTS];
50 u8 link_speed[SWCONFIG_LED_NUM_PORTS];
51 };
52
53 struct swconfig_trig_data {
54 struct led_classdev *led_cdev;
55 struct switch_dev *swdev;
56
57 rwlock_t lock;
58 u32 port_mask;
59
60 bool prev_link;
61 unsigned long prev_traffic;
62 enum led_brightness prev_brightness;
63 u8 mode;
64 u8 speed_mask;
65 };
66
67 static void
68 swconfig_trig_set_brightness(struct swconfig_trig_data *trig_data,
69 enum led_brightness brightness)
70 {
71 led_set_brightness(trig_data->led_cdev, brightness);
72 trig_data->prev_brightness = brightness;
73 }
74
75 static void
76 swconfig_trig_update_port_mask(struct led_trigger *trigger)
77 {
78 struct list_head *entry;
79 struct switch_led_trigger *sw_trig;
80 u32 port_mask;
81
82 if (!trigger)
83 return;
84
85 sw_trig = (void *) trigger;
86
87 port_mask = 0;
88 read_lock(&trigger->leddev_list_lock);
89 list_for_each(entry, &trigger->led_cdevs) {
90 struct led_classdev *led_cdev;
91 struct swconfig_trig_data *trig_data;
92
93 led_cdev = list_entry(entry, struct led_classdev, trig_list);
94 trig_data = led_cdev->trigger_data;
95 if (trig_data) {
96 read_lock(&trig_data->lock);
97 port_mask |= trig_data->port_mask;
98 read_unlock(&trig_data->lock);
99 }
100 }
101 read_unlock(&trigger->leddev_list_lock);
102
103 sw_trig->port_mask = port_mask;
104
105 if (port_mask)
106 schedule_delayed_work(&sw_trig->sw_led_work,
107 SWCONFIG_LED_TIMER_INTERVAL);
108 else
109 cancel_delayed_work_sync(&sw_trig->sw_led_work);
110 }
111
112 static ssize_t
113 swconfig_trig_port_mask_store(struct device *dev, struct device_attribute *attr,
114 const char *buf, size_t size)
115 {
116 struct led_classdev *led_cdev = dev_get_drvdata(dev);
117 struct swconfig_trig_data *trig_data = led_cdev->trigger_data;
118 unsigned long port_mask;
119 int ret;
120 bool changed;
121
122 ret = kstrtoul(buf, 0, &port_mask);
123 if (ret)
124 return ret;
125
126 write_lock(&trig_data->lock);
127 changed = (trig_data->port_mask != port_mask);
128 trig_data->port_mask = port_mask;
129 write_unlock(&trig_data->lock);
130
131 if (changed) {
132 if (port_mask == 0)
133 swconfig_trig_set_brightness(trig_data, LED_OFF);
134
135 swconfig_trig_update_port_mask(led_cdev->trigger);
136 }
137
138 return size;
139 }
140
141 static ssize_t
142 swconfig_trig_port_mask_show(struct device *dev, struct device_attribute *attr,
143 char *buf)
144 {
145 struct led_classdev *led_cdev = dev_get_drvdata(dev);
146 struct swconfig_trig_data *trig_data = led_cdev->trigger_data;
147 u32 port_mask;
148
149 read_lock(&trig_data->lock);
150 port_mask = trig_data->port_mask;
151 read_unlock(&trig_data->lock);
152
153 sprintf(buf, "%#x\n", port_mask);
154
155 return strlen(buf) + 1;
156 }
157
158 static DEVICE_ATTR(port_mask, 0644, swconfig_trig_port_mask_show,
159 swconfig_trig_port_mask_store);
160
161 /* speed_mask file handler - display value */
162 static ssize_t swconfig_trig_speed_mask_show(struct device *dev,
163 struct device_attribute *attr,
164 char *buf)
165 {
166 struct led_classdev *led_cdev = dev_get_drvdata(dev);
167 struct swconfig_trig_data *trig_data = led_cdev->trigger_data;
168 u8 speed_mask;
169
170 read_lock(&trig_data->lock);
171 speed_mask = trig_data->speed_mask;
172 read_unlock(&trig_data->lock);
173
174 sprintf(buf, "%#x\n", speed_mask);
175
176 return strlen(buf) + 1;
177 }
178
179 /* speed_mask file handler - store value */
180 static ssize_t swconfig_trig_speed_mask_store(struct device *dev,
181 struct device_attribute *attr,
182 const char *buf, size_t size)
183 {
184 struct led_classdev *led_cdev = dev_get_drvdata(dev);
185 struct swconfig_trig_data *trig_data = led_cdev->trigger_data;
186 u8 speed_mask;
187 int ret;
188
189 ret = kstrtou8(buf, 0, &speed_mask);
190 if (ret)
191 return ret;
192
193 write_lock(&trig_data->lock);
194 trig_data->speed_mask = speed_mask & SWCONFIG_LED_PORT_SPEED_ALL;
195 write_unlock(&trig_data->lock);
196
197 return size;
198 }
199
200 /* speed_mask special file */
201 static DEVICE_ATTR(speed_mask, 0644, swconfig_trig_speed_mask_show,
202 swconfig_trig_speed_mask_store);
203
204 static ssize_t swconfig_trig_mode_show(struct device *dev,
205 struct device_attribute *attr, char *buf)
206 {
207 struct led_classdev *led_cdev = dev_get_drvdata(dev);
208 struct swconfig_trig_data *trig_data = led_cdev->trigger_data;
209 u8 mode;
210
211 read_lock(&trig_data->lock);
212 mode = trig_data->mode;
213 read_unlock(&trig_data->lock);
214
215 if (mode == 0) {
216 strcpy(buf, "none\n");
217 } else {
218 if (mode & SWCONFIG_LED_MODE_LINK)
219 strcat(buf, "link ");
220 if (mode & SWCONFIG_LED_MODE_TX)
221 strcat(buf, "tx ");
222 if (mode & SWCONFIG_LED_MODE_RX)
223 strcat(buf, "rx ");
224 strcat(buf, "\n");
225 }
226
227 return strlen(buf)+1;
228 }
229
230 static ssize_t swconfig_trig_mode_store(struct device *dev,
231 struct device_attribute *attr, const char *buf, size_t size)
232 {
233 struct led_classdev *led_cdev = dev_get_drvdata(dev);
234 struct swconfig_trig_data *trig_data = led_cdev->trigger_data;
235 char copybuf[128];
236 int new_mode = -1;
237 char *p, *token;
238
239 /* take a copy since we don't want to trash the inbound buffer when using strsep */
240 strncpy(copybuf, buf, sizeof(copybuf));
241 copybuf[sizeof(copybuf) - 1] = 0;
242 p = copybuf;
243
244 while ((token = strsep(&p, " \t\n")) != NULL) {
245 if (!*token)
246 continue;
247
248 if (new_mode < 0)
249 new_mode = 0;
250
251 if (!strcmp(token, "none"))
252 new_mode = 0;
253 else if (!strcmp(token, "tx"))
254 new_mode |= SWCONFIG_LED_MODE_TX;
255 else if (!strcmp(token, "rx"))
256 new_mode |= SWCONFIG_LED_MODE_RX;
257 else if (!strcmp(token, "link"))
258 new_mode |= SWCONFIG_LED_MODE_LINK;
259 else
260 return -EINVAL;
261 }
262
263 if (new_mode < 0)
264 return -EINVAL;
265
266 write_lock(&trig_data->lock);
267 trig_data->mode = (u8)new_mode;
268 write_unlock(&trig_data->lock);
269
270 return size;
271 }
272
273 /* mode special file */
274 static DEVICE_ATTR(mode, 0644, swconfig_trig_mode_show,
275 swconfig_trig_mode_store);
276
277 static int
278 swconfig_trig_activate(struct led_classdev *led_cdev)
279 {
280 struct switch_led_trigger *sw_trig;
281 struct swconfig_trig_data *trig_data;
282 int err;
283
284 trig_data = kzalloc(sizeof(struct swconfig_trig_data), GFP_KERNEL);
285 if (!trig_data)
286 return -ENOMEM;
287
288 sw_trig = (void *) led_cdev->trigger;
289
290 rwlock_init(&trig_data->lock);
291 trig_data->led_cdev = led_cdev;
292 trig_data->swdev = sw_trig->swdev;
293 trig_data->speed_mask = SWCONFIG_LED_PORT_SPEED_ALL;
294 trig_data->mode = SWCONFIG_LED_MODE_ALL;
295 led_cdev->trigger_data = trig_data;
296
297 err = device_create_file(led_cdev->dev, &dev_attr_port_mask);
298 if (err)
299 goto err_free;
300
301 err = device_create_file(led_cdev->dev, &dev_attr_speed_mask);
302 if (err)
303 goto err_dev_free;
304
305 err = device_create_file(led_cdev->dev, &dev_attr_mode);
306 if (err)
307 goto err_mode_free;
308
309 return 0;
310
311 err_mode_free:
312 device_remove_file(led_cdev->dev, &dev_attr_speed_mask);
313
314 err_dev_free:
315 device_remove_file(led_cdev->dev, &dev_attr_port_mask);
316
317 err_free:
318 led_cdev->trigger_data = NULL;
319 kfree(trig_data);
320
321 return err;
322 }
323
324 #if LINUX_VERSION_CODE < KERNEL_VERSION(4,19,0)
325 static void
326 swconfig_trig_activate_void(struct led_classdev *led_cdev)
327 {
328 swconfig_trig_activate(led_cdev);
329 }
330 #endif
331
332 static void
333 swconfig_trig_deactivate(struct led_classdev *led_cdev)
334 {
335 struct swconfig_trig_data *trig_data;
336
337 swconfig_trig_update_port_mask(led_cdev->trigger);
338
339 trig_data = (void *) led_cdev->trigger_data;
340 if (trig_data) {
341 device_remove_file(led_cdev->dev, &dev_attr_port_mask);
342 device_remove_file(led_cdev->dev, &dev_attr_speed_mask);
343 device_remove_file(led_cdev->dev, &dev_attr_mode);
344 kfree(trig_data);
345 }
346 }
347
348 /*
349 * link off -> led off (can't be any other reason to turn it on)
350 * link on:
351 * mode link: led on by default only if speed matches, else off
352 * mode txrx: blink only if speed matches, else off
353 */
354 static void
355 swconfig_trig_led_event(struct switch_led_trigger *sw_trig,
356 struct led_classdev *led_cdev)
357 {
358 struct swconfig_trig_data *trig_data;
359 u32 port_mask;
360 bool link;
361 u8 speed_mask, mode;
362 enum led_brightness led_base, led_blink;
363
364 trig_data = led_cdev->trigger_data;
365 if (!trig_data)
366 return;
367
368 read_lock(&trig_data->lock);
369 port_mask = trig_data->port_mask;
370 speed_mask = trig_data->speed_mask;
371 mode = trig_data->mode;
372 read_unlock(&trig_data->lock);
373
374 link = !!(sw_trig->port_link & port_mask);
375 if (!link) {
376 if (trig_data->prev_brightness != LED_OFF)
377 swconfig_trig_set_brightness(trig_data, LED_OFF); /* and stop */
378 }
379 else {
380 unsigned long traffic;
381 int speedok; /* link speed flag */
382 int i;
383
384 led_base = LED_FULL;
385 led_blink = LED_OFF;
386 traffic = 0;
387 speedok = 0;
388 for (i = 0; i < SWCONFIG_LED_NUM_PORTS; i++) {
389 if (port_mask & (1 << i)) {
390 if (sw_trig->link_speed[i] & speed_mask) {
391 traffic += ((mode & SWCONFIG_LED_MODE_TX) ?
392 sw_trig->port_tx_traffic[i] : 0) +
393 ((mode & SWCONFIG_LED_MODE_RX) ?
394 sw_trig->port_rx_traffic[i] : 0);
395 speedok = 1;
396 }
397 }
398 }
399
400 if (speedok) {
401 /* At least one port speed matches speed_mask */
402 if (!(mode & SWCONFIG_LED_MODE_LINK)) {
403 led_base = LED_OFF;
404 led_blink = LED_FULL;
405 }
406
407 if (trig_data->prev_brightness != led_base)
408 swconfig_trig_set_brightness(trig_data,
409 led_base);
410 else if (traffic != trig_data->prev_traffic)
411 swconfig_trig_set_brightness(trig_data,
412 led_blink);
413 } else if (trig_data->prev_brightness != LED_OFF)
414 swconfig_trig_set_brightness(trig_data, LED_OFF);
415
416 trig_data->prev_traffic = traffic;
417 }
418
419 trig_data->prev_link = link;
420 }
421
422 static void
423 swconfig_trig_update_leds(struct switch_led_trigger *sw_trig)
424 {
425 struct list_head *entry;
426 struct led_trigger *trigger;
427
428 trigger = &sw_trig->trig;
429 read_lock(&trigger->leddev_list_lock);
430 list_for_each(entry, &trigger->led_cdevs) {
431 struct led_classdev *led_cdev;
432
433 led_cdev = list_entry(entry, struct led_classdev, trig_list);
434 swconfig_trig_led_event(sw_trig, led_cdev);
435 }
436 read_unlock(&trigger->leddev_list_lock);
437 }
438
439 static void
440 swconfig_led_work_func(struct work_struct *work)
441 {
442 struct switch_led_trigger *sw_trig;
443 struct switch_dev *swdev;
444 u32 port_mask;
445 u32 link;
446 int i;
447
448 sw_trig = container_of(work, struct switch_led_trigger,
449 sw_led_work.work);
450
451 port_mask = sw_trig->port_mask;
452 swdev = sw_trig->swdev;
453
454 link = 0;
455 for (i = 0; i < SWCONFIG_LED_NUM_PORTS; i++) {
456 u32 port_bit;
457
458 sw_trig->link_speed[i] = 0;
459
460 port_bit = BIT(i);
461 if ((port_mask & port_bit) == 0)
462 continue;
463
464 if (swdev->ops->get_port_link) {
465 struct switch_port_link port_link;
466
467 memset(&port_link, '\0', sizeof(port_link));
468 swdev->ops->get_port_link(swdev, i, &port_link);
469
470 if (port_link.link) {
471 link |= port_bit;
472 switch (port_link.speed) {
473 case SWITCH_PORT_SPEED_UNKNOWN:
474 sw_trig->link_speed[i] =
475 SWCONFIG_LED_PORT_SPEED_NA;
476 break;
477 case SWITCH_PORT_SPEED_10:
478 sw_trig->link_speed[i] =
479 SWCONFIG_LED_PORT_SPEED_10;
480 break;
481 case SWITCH_PORT_SPEED_100:
482 sw_trig->link_speed[i] =
483 SWCONFIG_LED_PORT_SPEED_100;
484 break;
485 case SWITCH_PORT_SPEED_1000:
486 sw_trig->link_speed[i] =
487 SWCONFIG_LED_PORT_SPEED_1000;
488 break;
489 }
490 }
491 }
492
493 if (swdev->ops->get_port_stats) {
494 struct switch_port_stats port_stats;
495
496 memset(&port_stats, '\0', sizeof(port_stats));
497 swdev->ops->get_port_stats(swdev, i, &port_stats);
498 sw_trig->port_tx_traffic[i] = port_stats.tx_bytes;
499 sw_trig->port_rx_traffic[i] = port_stats.rx_bytes;
500 }
501 }
502
503 sw_trig->port_link = link;
504
505 swconfig_trig_update_leds(sw_trig);
506
507 schedule_delayed_work(&sw_trig->sw_led_work,
508 SWCONFIG_LED_TIMER_INTERVAL);
509 }
510
511 static int
512 swconfig_create_led_trigger(struct switch_dev *swdev)
513 {
514 struct switch_led_trigger *sw_trig;
515 int err;
516
517 if (!swdev->ops->get_port_link)
518 return 0;
519
520 sw_trig = kzalloc(sizeof(struct switch_led_trigger), GFP_KERNEL);
521 if (!sw_trig)
522 return -ENOMEM;
523
524 sw_trig->swdev = swdev;
525 sw_trig->trig.name = swdev->devname;
526 #if LINUX_VERSION_CODE < KERNEL_VERSION(4,19,0)
527 sw_trig->trig.activate = swconfig_trig_activate_void;
528 #else
529 sw_trig->trig.activate = swconfig_trig_activate;
530 #endif
531 sw_trig->trig.deactivate = swconfig_trig_deactivate;
532
533 INIT_DELAYED_WORK(&sw_trig->sw_led_work, swconfig_led_work_func);
534
535 err = led_trigger_register(&sw_trig->trig);
536 if (err)
537 goto err_free;
538
539 swdev->led_trigger = sw_trig;
540
541 return 0;
542
543 err_free:
544 kfree(sw_trig);
545 return err;
546 }
547
548 static void
549 swconfig_destroy_led_trigger(struct switch_dev *swdev)
550 {
551 struct switch_led_trigger *sw_trig;
552
553 sw_trig = swdev->led_trigger;
554 if (sw_trig) {
555 cancel_delayed_work_sync(&sw_trig->sw_led_work);
556 led_trigger_unregister(&sw_trig->trig);
557 kfree(sw_trig);
558 }
559 }
560
561 #else /* SWCONFIG_LEDS */
562 static inline int
563 swconfig_create_led_trigger(struct switch_dev *swdev) { return 0; }
564
565 static inline void
566 swconfig_destroy_led_trigger(struct switch_dev *swdev) { }
567 #endif /* CONFIG_SWCONFIG_LEDS */