From b577ec296f87ecb59e4b950b0226a956492d7d99 Mon Sep 17 00:00:00 2001 From: Tomaz Zaman Date: Mon, 10 Aug 2026 15:38:37 +0200 Subject: [PATCH 07/59] layerscape: add Mono Gateway DK LED drivers kmod-sfp-led (SFP cage link/activity from module presence and optical signal) and kmod-leds-lp5812 (TI LP5812 status LED matrix), vendored from the meta-mono layer. lp5812 loads early (AutoLoad 10) so /sys/class/leds/status:* exists before LED setup runs. Co-Authored-By: Claude Fable 5 --- package/mono/mono-leds/Makefile | 51 ++ .../mono-leds/src/leds-lp5812/leds-lp5812.c | 734 ++++++++++++++++++ .../mono-leds/src/leds-lp5812/leds-lp5812.h | 197 +++++ package/mono/mono-leds/src/sfp-led/sfp-led.c | 677 ++++++++++++++++ target/linux/layerscape/image/armv8_64b.mk | 2 +- 5 files changed, 1660 insertions(+), 1 deletion(-) create mode 100644 package/mono/mono-leds/Makefile create mode 100644 package/mono/mono-leds/src/leds-lp5812/leds-lp5812.c create mode 100644 package/mono/mono-leds/src/leds-lp5812/leds-lp5812.h create mode 100644 package/mono/mono-leds/src/sfp-led/sfp-led.c diff --git a/package/mono/mono-leds/Makefile b/package/mono/mono-leds/Makefile new file mode 100644 index 0000000000..55abd2017e --- /dev/null +++ b/package/mono/mono-leds/Makefile @@ -0,0 +1,51 @@ +include $(TOPDIR)/rules.mk + +PKG_NAME:=mono-leds +PKG_VERSION:=1.0 +PKG_RELEASE:=1 +PKG_LICENSE:=GPL-2.0 + +include $(INCLUDE_DIR)/kernel.mk +include $(INCLUDE_DIR)/package.mk + +define KernelPackage/leds-lp5812 + SUBMENU:=LED modules + TITLE:=TI LP5812 LED matrix controller + DEPENDS:=@TARGET_layerscape + FILES:=$(PKG_BUILD_DIR)/leds-lp5812/leds-lp5812.ko + AUTOLOAD:=$(call AutoLoad,10,leds-lp5812) +endef + +define KernelPackage/leds-lp5812/description + I2C driver for the TI LP5812 4x3 LED matrix controller + (status LEDs on the Mono Gateway). Loads early so + /sys/class/leds/status:* exists before LED setup scripts run. +endef + +define KernelPackage/sfp-led + SUBMENU:=LED modules + TITLE:=Mono SFP port LED controller + DEPENDS:=@TARGET_layerscape + FILES:=$(PKG_BUILD_DIR)/sfp-led/sfp-led.ko + AUTOLOAD:=$(call AutoLoad,60,sfp-led) +endef + +define KernelPackage/sfp-led/description + Drives the SFP cage link/activity LEDs from module presence and + optical signal state (mono,sfp-led device tree node). +endef + +define Build/Prepare + mkdir -p $(PKG_BUILD_DIR) + $(CP) ./src/. $(PKG_BUILD_DIR)/ + printf 'obj-m := sfp-led.o\n' > $(PKG_BUILD_DIR)/sfp-led/Makefile + printf 'obj-m := leds-lp5812.o\n' > $(PKG_BUILD_DIR)/leds-lp5812/Makefile +endef + +define Build/Compile + +$(KERNEL_MAKE) M=$(PKG_BUILD_DIR)/sfp-led modules + +$(KERNEL_MAKE) M=$(PKG_BUILD_DIR)/leds-lp5812 modules +endef + +$(eval $(call KernelPackage,leds-lp5812)) +$(eval $(call KernelPackage,sfp-led)) diff --git a/package/mono/mono-leds/src/leds-lp5812/leds-lp5812.c b/package/mono/mono-leds/src/leds-lp5812/leds-lp5812.c new file mode 100644 index 0000000000..c7ad69f734 --- /dev/null +++ b/package/mono/mono-leds/src/leds-lp5812/leds-lp5812.c @@ -0,0 +1,734 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * LP5812 LED driver + * + * Copyright (C) 2025 Texas Instruments + * + * Author: Jared Zhou + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "leds-lp5812.h" + +static const struct lp5812_mode_mapping chip_mode_map[] = { + {"direct_mode", 0, 0, 0, 0, 0, 0}, + {"tcm:1:0", 1, 0, 0, 0, 0, 0}, + {"tcm:1:1", 1, 1, 0, 0, 0, 0}, + {"tcm:1:2", 1, 2, 0, 0, 0, 0}, + {"tcm:1:3", 1, 3, 0, 0, 0, 0}, + {"tcm:2:0:1", 2, 0, 1, 0, 0, 0}, + {"tcm:2:0:2", 2, 0, 2, 0, 0, 0}, + {"tcm:2:0:3", 2, 0, 3, 0, 0, 0}, + {"tcm:2:1:2", 2, 1, 2, 0, 0, 0}, + {"tcm:2:1:3", 2, 1, 3, 0, 0, 0}, + {"tcm:2:2:3", 2, 2, 3, 0, 0, 0}, + {"tcm:3:0:1:2", 3, 0, 1, 2, 0, 0}, + {"tcm:3:0:1:3", 3, 0, 1, 3, 0, 0}, + {"tcm:3:0:2:3", 3, 0, 2, 3, 0, 0}, + {"tcm:4:0:1:2:3", 4, 0, 1, 2, 3, 0}, + {"mix:1:0:1", 5, 1, 0, 0, 0, 0}, + {"mix:1:0:2", 5, 2, 0, 0, 0, 0}, + {"mix:1:0:3", 5, 3, 0, 0, 0, 0}, + {"mix:1:1:0", 5, 0, 0, 0, 0, 1}, + {"mix:1:1:2", 5, 2, 0, 0, 0, 1}, + {"mix:1:1:3", 5, 3, 0, 0, 0, 1}, + {"mix:1:2:0", 5, 0, 0, 0, 0, 2}, + {"mix:1:2:1", 5, 1, 0, 0, 0, 2}, + {"mix:1:2:3", 5, 3, 0, 0, 0, 2}, + {"mix:1:3:0", 5, 0, 0, 0, 0, 3}, + {"mix:1:3:1", 5, 1, 0, 0, 0, 3}, + {"mix:1:3:2", 5, 2, 0, 0, 0, 3}, + {"mix:2:0:1:2", 6, 1, 2, 0, 0, 0}, + {"mix:2:0:1:3", 6, 1, 3, 0, 0, 0}, + {"mix:2:0:2:3", 6, 2, 3, 0, 0, 0}, + {"mix:2:1:0:2", 6, 0, 2, 0, 0, 1}, + {"mix:2:1:0:3", 6, 0, 3, 0, 0, 1}, + {"mix:2:1:2:3", 6, 2, 3, 0, 0, 1}, + {"mix:2:2:0:1", 6, 0, 1, 0, 0, 2}, + {"mix:2:2:0:3", 6, 0, 3, 0, 0, 2}, + {"mix:2:2:1:3", 6, 1, 3, 0, 0, 2}, + {"mix:2:3:0:1", 6, 0, 1, 0, 0, 3}, + {"mix:2:3:0:2", 6, 0, 2, 0, 0, 3}, + {"mix:2:3:1:2", 6, 1, 2, 0, 0, 3}, + {"mix:3:0:1:2:3", 7, 1, 2, 3, 0, 0}, + {"mix:3:1:0:2:3", 7, 0, 2, 3, 0, 1}, + {"mix:3:2:0:1:3", 7, 0, 1, 3, 0, 2}, + {"mix:3:3:0:1:2", 7, 0, 1, 2, 0, 3} +}; + +static int lp5812_write(struct lp5812_chip *chip, u16 reg, u8 val) +{ + struct device *dev = &chip->client->dev; + struct i2c_msg msg; + u8 buf[LP5812_DATA_LENGTH]; + u8 reg_addr_bit8_9; + int ret; + + /* Extract register address bits 9 and 8 for Address Byte 1 */ + reg_addr_bit8_9 = (reg >> LP5812_REG_ADDR_HIGH_SHIFT) & LP5812_REG_ADDR_BIT_8_9_MASK; + + /* Prepare payload: Address Byte 2 (bits [7:0]) and value to write */ + buf[LP5812_DATA_BYTE_0_IDX] = (u8)(reg & LP5812_REG_ADDR_LOW_MASK); + buf[LP5812_DATA_BYTE_1_IDX] = val; + + /* Construct I2C message for a write operation */ + msg.addr = (chip->client->addr << LP5812_CHIP_ADDR_SHIFT) | reg_addr_bit8_9; + msg.flags = 0; + msg.len = sizeof(buf); + msg.buf = buf; + + ret = i2c_transfer(chip->client->adapter, &msg, 1); + if (ret == 1) + return 0; + + dev_err(dev, "I2C write error, ret=%d\n", ret); + return ret < 0 ? ret : -EIO; +} + +static int lp5812_read(struct lp5812_chip *chip, u16 reg, u8 *val) +{ + struct device *dev = &chip->client->dev; + struct i2c_msg msgs[LP5812_READ_MSG_LENGTH]; + u8 ret_val; + u8 reg_addr_bit8_9; + u8 converted_reg; + int ret; + + /* Extract register address bits 9 and 8 for Address Byte 1 */ + reg_addr_bit8_9 = (reg >> LP5812_REG_ADDR_HIGH_SHIFT) & LP5812_REG_ADDR_BIT_8_9_MASK; + + /* Lower 8 bits go in Address Byte 2 */ + converted_reg = (u8)(reg & LP5812_REG_ADDR_LOW_MASK); + + /* Prepare I2C write message to set register address */ + msgs[LP5812_MSG_0_IDX].addr = + (chip->client->addr << LP5812_CHIP_ADDR_SHIFT) | reg_addr_bit8_9; + msgs[LP5812_MSG_0_IDX].flags = 0; + msgs[LP5812_MSG_0_IDX].len = 1; + msgs[LP5812_MSG_0_IDX].buf = &converted_reg; + + /* Prepare I2C read message to retrieve register value */ + msgs[LP5812_MSG_1_IDX].addr = + (chip->client->addr << LP5812_CHIP_ADDR_SHIFT) | reg_addr_bit8_9; + msgs[LP5812_MSG_1_IDX].flags = I2C_M_RD; + msgs[LP5812_MSG_1_IDX].len = 1; + msgs[LP5812_MSG_1_IDX].buf = &ret_val; + + ret = i2c_transfer(chip->client->adapter, msgs, LP5812_READ_MSG_LENGTH); + if (ret == LP5812_READ_MSG_LENGTH) { + *val = ret_val; + return 0; + } + + dev_err(dev, "I2C read error, ret=%d\n", ret); + *val = 0; + return ret < 0 ? ret : -EIO; +} + +static int lp5812_read_tsd_config_status(struct lp5812_chip *chip, u8 *reg_val) +{ + return lp5812_read(chip, chip->cfg->reg_tsd_config_status.addr, reg_val); +} + +static int lp5812_update_regs_config(struct lp5812_chip *chip) +{ + u8 reg_val; + int ret; + + ret = lp5812_write(chip, chip->cfg->reg_cmd_update.addr, LP5812_UPDATE_CMD_VAL); + if (ret) + return ret; + + ret = lp5812_read_tsd_config_status(chip, ®_val); + if (ret) + return ret; + + return reg_val & LP5812_CFG_ERR_STATUS_MASK; +} + +static ssize_t parse_drive_mode(struct lp5812_chip *chip, const char *str) +{ + int i; + + chip->u_drive_mode.s_drive_mode.mix_sel_led_0 = false; + chip->u_drive_mode.s_drive_mode.mix_sel_led_1 = false; + chip->u_drive_mode.s_drive_mode.mix_sel_led_2 = false; + chip->u_drive_mode.s_drive_mode.mix_sel_led_3 = false; + + if (sysfs_streq(str, LP5812_MODE_DIRECT_NAME)) { + chip->u_drive_mode.s_drive_mode.led_mode = LP5812_MODE_DIRECT_VALUE; + return 0; + } + + for (i = 0; i < ARRAY_SIZE(chip_mode_map); i++) { + if (sysfs_streq(str, chip_mode_map[i].mode_name)) { + chip->u_drive_mode.s_drive_mode.led_mode = chip_mode_map[i].mode; + chip->u_scan_order.s_scan_order.scan_order_0 = + chip_mode_map[i].scan_order_0; + chip->u_scan_order.s_scan_order.scan_order_1 = + chip_mode_map[i].scan_order_1; + chip->u_scan_order.s_scan_order.scan_order_2 = + chip_mode_map[i].scan_order_2; + chip->u_scan_order.s_scan_order.scan_order_3 = + chip_mode_map[i].scan_order_3; + + switch (chip_mode_map[i].selection_led) { + case LP5812_MODE_MIX_SELECT_LED_0: + chip->u_drive_mode.s_drive_mode.mix_sel_led_0 = true; + break; + case LP5812_MODE_MIX_SELECT_LED_1: + chip->u_drive_mode.s_drive_mode.mix_sel_led_1 = true; + break; + case LP5812_MODE_MIX_SELECT_LED_2: + chip->u_drive_mode.s_drive_mode.mix_sel_led_2 = true; + break; + case LP5812_MODE_MIX_SELECT_LED_3: + chip->u_drive_mode.s_drive_mode.mix_sel_led_3 = true; + break; + default: + return -EINVAL; + } + + return 0; + } + } + + return -EINVAL; +} + +static int lp5812_set_drive_mode_scan_order(struct lp5812_chip *chip) +{ + u8 val; + int ret; + + /* Set led mode */ + val = chip->u_drive_mode.drive_mode_val; + ret = lp5812_write(chip, chip->cfg->reg_dev_config_1.addr, val); + if (ret) + return ret; + + /* Setup scan order */ + val = chip->u_scan_order.scan_order_val; + ret = lp5812_write(chip, chip->cfg->reg_dev_config_2.addr, val); + + return ret; +} + +static int lp5812_set_led_mode(struct lp5812_chip *chip, int led_number, + enum control_mode mode) +{ + u8 reg_val; + u16 reg; + int ret; + + if (led_number < LP5812_NUMBER_LED_IN_REG) + reg = chip->cfg->reg_dev_config_3.addr; + else + reg = chip->cfg->reg_dev_config_4.addr; + + ret = lp5812_read(chip, reg, ®_val); + if (ret) + return ret; + + if (mode == LP5812_MODE_MANUAL) + reg_val &= ~(1 << (led_number % LP5812_NUMBER_LED_IN_REG)); + else + reg_val |= (1 << (led_number % LP5812_NUMBER_LED_IN_REG)); + + ret = lp5812_write(chip, reg, reg_val); + if (ret) + return ret; + + ret = lp5812_update_regs_config(chip); + + return ret; +} + +static int lp5812_manual_dc_pwm_control(struct lp5812_chip *chip, int led_number, + u8 val, enum dimming_type dimming_type) +{ + u16 led_base_reg; + int ret; + + if (dimming_type == LP5812_DIMMING_ANALOG) + led_base_reg = chip->cfg->reg_manual_dc_base.addr; + else + led_base_reg = chip->cfg->reg_manual_pwm_base.addr; + ret = lp5812_write(chip, led_base_reg + led_number, val); + + return ret; +} + +static int lp5812_auto_dc(struct lp5812_chip *chip, + int led_number, u8 val) +{ + return lp5812_write(chip, chip->cfg->reg_auto_dc_base.addr + led_number, val); +} + +static int lp5812_multicolor_brightness(struct lp5812_led *led) +{ + int ret, i; + struct lp5812_chip *chip = led->chip; + + guard(mutex)(&chip->lock); + for (i = 0; i < led->mc_cdev.num_colors; i++) { + ret = lp5812_manual_dc_pwm_control(chip, led->mc_cdev.subled_info[i].channel, + led->mc_cdev.subled_info[i].brightness, + LP5812_DIMMING_PWM); + if (ret) + return ret; + } + + return 0; +} + +static int lp5812_led_brightness(struct lp5812_led *led) +{ + struct lp5812_chip *chip = led->chip; + struct lp5812_led_config *led_cfg; + int ret; + + led_cfg = &chip->led_config[led->chan_nr]; + + guard(mutex)(&chip->lock); + ret = lp5812_manual_dc_pwm_control(chip, led_cfg->led_id[0], + led->brightness, LP5812_DIMMING_PWM); + + return ret; +} + +static int lp5812_set_brightness(struct led_classdev *cdev, + enum led_brightness brightness) +{ + struct lp5812_led *led = container_of(cdev, struct lp5812_led, cdev); + + led->brightness = (u8)brightness; + return lp5812_led_brightness(led); +} + +static int lp5812_set_mc_brightness(struct led_classdev *cdev, + enum led_brightness brightness) +{ + struct led_classdev_mc *mc_dev = lcdev_to_mccdev(cdev); + struct lp5812_led *led = container_of(mc_dev, struct lp5812_led, mc_cdev); + + led_mc_calc_color_components(&led->mc_cdev, brightness); + return lp5812_multicolor_brightness(led); +} + +static int lp5812_init_led(struct lp5812_led *led, struct lp5812_chip *chip, int chan) +{ + struct device *dev = &chip->client->dev; + struct mc_subled *mc_led_info; + struct led_classdev *led_cdev; + int i, ret; + + if (chip->led_config[chan].name) { + led->cdev.name = chip->led_config[chan].name; + } else { + led->cdev.name = devm_kasprintf(dev, GFP_KERNEL, "%s:channel%d", + chip->label ? : chip->client->name, chan); + if (!led->cdev.name) + return -ENOMEM; + } + + if (!chip->led_config[chan].is_sc_led) { + mc_led_info = devm_kcalloc(dev, + chip->led_config[chan].num_colors, + sizeof(*mc_led_info), GFP_KERNEL); + if (!mc_led_info) + return -ENOMEM; + + led_cdev = &led->mc_cdev.led_cdev; + led_cdev->name = led->cdev.name; + led_cdev->brightness_set_blocking = lp5812_set_mc_brightness; + led->mc_cdev.num_colors = chip->led_config[chan].num_colors; + for (i = 0; i < led->mc_cdev.num_colors; i++) { + mc_led_info[i].color_index = + chip->led_config[chan].color_id[i]; + mc_led_info[i].channel = + chip->led_config[chan].led_id[i]; + } + + led->mc_cdev.subled_info = mc_led_info; + } else { + led->cdev.brightness_set_blocking = lp5812_set_brightness; + } + + led->chan_nr = chan; + + if (chip->led_config[chan].is_sc_led) { + ret = devm_led_classdev_register(dev, &led->cdev); + if (ret == 0) + led->cdev.dev->platform_data = led; + } else { + ret = devm_led_classdev_multicolor_register(dev, &led->mc_cdev); + if (ret == 0) + led->mc_cdev.led_cdev.dev->platform_data = led; + } + + return ret; +} + +static int lp5812_register_leds(struct lp5812_led *led, struct lp5812_chip *chip) +{ + struct lp5812_led *each; + int num_channels = chip->num_channels; + u8 reg_val; + u16 reg; + int ret, i, j; + + for (i = 0; i < num_channels; i++) { + each = led + i; + /* Set before registration: the classdev is live once + * lp5812_init_led() returns and the brightness ops + * dereference chip. + */ + each->chip = chip; + + ret = lp5812_init_led(each, chip, i); + if (ret) + goto err_init_led; + + for (j = 0; j < chip->led_config[i].num_colors; j++) { + ret = lp5812_auto_dc(chip, chip->led_config[i].led_id[j], + chip->led_config[i].max_current[j]); + if (ret) + goto err_init_led; + + ret = lp5812_manual_dc_pwm_control(chip, chip->led_config[i].led_id[j], + chip->led_config[i].max_current[j], + LP5812_DIMMING_ANALOG); + if (ret) + goto err_init_led; + + ret = lp5812_set_led_mode(chip, chip->led_config[i].led_id[j], + LP5812_MODE_MANUAL); + if (ret) + goto err_init_led; + + reg = (chip->led_config[i].led_id[j] < LP5812_NUMBER_LED_IN_REG) ? + chip->cfg->reg_led_en_1.addr : + chip->cfg->reg_led_en_2.addr; + + ret = lp5812_read(chip, reg, ®_val); + if (ret) + goto err_init_led; + + reg_val |= (1 << (chip->led_config[i].led_id[j] % + LP5812_NUMBER_LED_IN_REG)); + + ret = lp5812_write(chip, reg, reg_val); + if (ret) + goto err_init_led; + } + } + + return 0; + +err_init_led: + return ret; +} + +static int lp5812_init_device(struct lp5812_chip *chip) +{ + int ret; + + usleep_range(LP5812_WAIT_DEVICE_STABLE_MIN, LP5812_WAIT_DEVICE_STABLE_MAX); + + ret = lp5812_write(chip, chip->cfg->reg_chip_en.addr, (u8)1); + if (ret) { + dev_err(&chip->client->dev, "lp5812_enable_disable failed\n"); + return ret; + } + + ret = lp5812_write(chip, chip->cfg->reg_dev_config_12.addr, LP5812_LSD_LOD_START_UP); + if (ret) { + dev_err(&chip->client->dev, "write 0x0B to DEV_CONFIG12 failed\n"); + return ret; + } + + ret = parse_drive_mode(chip, chip->scan_mode); + if (ret) + return ret; + + ret = lp5812_set_drive_mode_scan_order(chip); + if (ret) + return ret; + + ret = lp5812_update_regs_config(chip); + if (ret) { + dev_err(&chip->client->dev, "lp5812_update_regs_config failed\n"); + return ret; + } + + return 0; +} + +static void lp5812_deinit_device(struct lp5812_chip *chip) +{ + lp5812_write(chip, chip->cfg->reg_led_en_1.addr, 0); + lp5812_write(chip, chip->cfg->reg_led_en_2.addr, 0); + lp5812_write(chip, chip->cfg->reg_chip_en.addr, 0); +} + +static int lp5812_parse_led_channel(struct device_node *np, + struct lp5812_led_config *cfg, + int color_number) +{ + int color_id = 0, reg, ret; + u32 max_cur = 0; + + ret = of_property_read_u32(np, "reg", ®); + if (ret) + return ret; + + cfg->led_id[color_number] = reg; + + of_property_read_u32(np, "led-max-microamp", &max_cur); + cfg->max_current[color_number] = max_cur / 100; + + of_property_read_u32(np, "color", &color_id); + cfg->color_id[color_number] = color_id; + + return 0; +} + +static int lp5812_parse_led(struct device_node *np, + struct lp5812_led_config *cfg, + int led_index) +{ + int num_colors = 0, ret; + + of_property_read_string(np, "label", &cfg[led_index].name); + + ret = of_property_read_u32(np, "reg", &cfg[led_index].chan_nr); + if (ret) + return ret; + + for_each_available_child_of_node_scoped(np, child) { + ret = lp5812_parse_led_channel(child, &cfg[led_index], num_colors); + if (ret) + return ret; + num_colors++; + } + + if (num_colors == 0) { + ret = lp5812_parse_led_channel(np, &cfg[led_index], 0); + if (ret) + return ret; + num_colors = 1; + cfg[led_index].is_sc_led = true; + } else { + cfg[led_index].is_sc_led = false; + } + + cfg[led_index].num_colors = num_colors; + + return 0; +} + +static int lp5812_of_populate_pdata(struct device *dev, + struct device_node *np, + struct lp5812_chip *chip) +{ + struct lp5812_led_config *cfg; + int num_channels, i = 0, ret; + + num_channels = of_get_available_child_count(np); + if (num_channels == 0) { + dev_err(dev, "no LED channels\n"); + return -EINVAL; + } + + cfg = devm_kcalloc(dev, num_channels, sizeof(*cfg), GFP_KERNEL); + if (!cfg) + return -ENOMEM; + + chip->led_config = &cfg[0]; + chip->num_channels = num_channels; + + for_each_available_child_of_node_scoped(np, child) { + ret = lp5812_parse_led(child, cfg, i); + if (ret) + return -EINVAL; + i++; + } + + ret = of_property_read_string(np, "ti,scan-mode", &chip->scan_mode); + if (ret) + chip->scan_mode = LP5812_MODE_DIRECT_NAME; + + of_property_read_string(np, "label", &chip->label); + + return 0; +} + +static int lp5812_probe(struct i2c_client *client) +{ + struct lp5812_chip *chip; + struct device_node *np = dev_of_node(&client->dev); + struct lp5812_led *led; + int ret; + + if (!np) + return -EINVAL; + + chip = devm_kzalloc(&client->dev, sizeof(*chip), GFP_KERNEL); + if (!chip) + return -ENOMEM; + + chip->cfg = i2c_get_match_data(client); + ret = lp5812_of_populate_pdata(&client->dev, np, chip); + if (ret) + return ret; + + led = devm_kcalloc(&client->dev, chip->num_channels, sizeof(*led), GFP_KERNEL); + if (!led) + return -ENOMEM; + + chip->client = client; + mutex_init(&chip->lock); + i2c_set_clientdata(client, led); + + ret = lp5812_init_device(chip); + if (ret) + return ret; + + ret = lp5812_register_leds(led, chip); + if (ret) + goto err_out; + + return 0; + +err_out: + lp5812_deinit_device(chip); + return ret; +} + +static void lp5812_remove(struct i2c_client *client) +{ + struct lp5812_led *led = i2c_get_clientdata(client); + + lp5812_deinit_device(led->chip); +} + +/* Chip specific configurations */ +static const struct lp5812_device_config lp5812_cfg = { + .reg_reset = { + .addr = LP5812_REG_RESET, + .val = LP5812_RESET + }, + .reg_chip_en = { + .addr = LP5812_REG_ENABLE, + .val = LP5812_ENABLE_DEFAULT + }, + .reg_dev_config_0 = { + .addr = LP5812_DEV_CONFIG0, + .val = 0 + }, + .reg_dev_config_1 = { + .addr = LP5812_DEV_CONFIG1, + .val = 0 + }, + .reg_dev_config_2 = { + .addr = LP5812_DEV_CONFIG2, + .val = 0 + }, + .reg_dev_config_3 = { + .addr = LP5812_DEV_CONFIG3, + .val = 0 + }, + .reg_dev_config_4 = { + .addr = LP5812_DEV_CONFIG4, + .val = 0 + }, + .reg_dev_config_5 = { + .addr = LP5812_DEV_CONFIG5, + .val = 0 + }, + .reg_dev_config_6 = { + .addr = LP5812_DEV_CONFIG6, + .val = 0 + }, + .reg_dev_config_7 = { + .addr = LP5812_DEV_CONFIG7, + .val = 0 + }, + .reg_dev_config_12 = { + .addr = LP5812_DEV_CONFIG12, + .val = LP5812_DEV_CONFIG12_DEFAULT + }, + .reg_cmd_update = { + .addr = LP5812_CMD_UPDATE, + .val = 0 + }, + .reg_tsd_config_status = { + .addr = LP5812_TSD_CONFIG_STATUS, + .val = 0 + }, + .reg_led_en_1 = { + .addr = LP5812_LED_EN_1, + .val = 0 + }, + .reg_led_en_2 = { + .addr = LP5812_LED_EN_2, + .val = 0 + }, + .reg_fault_clear = { + .addr = LP5812_FAULT_CLEAR, + .val = 0 + }, + .reg_manual_dc_base = { + .addr = LP5812_MANUAL_DC_BASE, + .val = 0 + }, + .reg_auto_dc_base = { + .addr = LP5812_AUTO_DC_BASE, + .val = 0 + }, + .reg_manual_pwm_base = { + .addr = LP5812_MANUAL_PWM_BASE, + .val = 0 + }, + .reg_lod_status_base = { + .addr = LP5812_LOD_STATUS, + .val = 0 + }, + .reg_lsd_status_base = { + .addr = LP5812_LSD_STATUS, + .val = 0 + } +}; + +static const struct of_device_id of_lp5812_match[] = { + { .compatible = "ti,lp5812", .data = &lp5812_cfg }, + {/* NULL */} +}; + +MODULE_DEVICE_TABLE(of, of_lp5812_match); + +static struct i2c_driver lp5812_driver = { + .driver = { + .name = "lp5812", + .of_match_table = of_lp5812_match, + }, + .probe = lp5812_probe, + .remove = lp5812_remove, +}; + +module_i2c_driver(lp5812_driver); + +MODULE_DESCRIPTION("Texas Instruments LP5812 LED Driver"); +MODULE_AUTHOR("Jared Zhou"); +MODULE_LICENSE("GPL"); diff --git a/package/mono/mono-leds/src/leds-lp5812/leds-lp5812.h b/package/mono/mono-leds/src/leds-lp5812/leds-lp5812.h new file mode 100644 index 0000000000..59af1ccfe4 --- /dev/null +++ b/package/mono/mono-leds/src/leds-lp5812/leds-lp5812.h @@ -0,0 +1,197 @@ +/* SPDX-License-Identifier: GPL-2.0-only */ +/* + * LP5812 Driver Header + * + * Copyright (C) 2025 Texas Instruments + * + * Author: Jared Zhou + */ + +#ifndef _LP5812_H_ +#define _LP5812_H_ + +#include +#include +#include +#include +#include +#include +#include +#include + +#define LP5812_REG_ENABLE 0x0000 +#define LP5812_REG_RESET 0x0023 +#define LP5812_DEV_CONFIG0 0x0001 +#define LP5812_DEV_CONFIG1 0x0002 +#define LP5812_DEV_CONFIG2 0x0003 +#define LP5812_DEV_CONFIG3 0x0004 +#define LP5812_DEV_CONFIG4 0x0005 +#define LP5812_DEV_CONFIG5 0x0006 +#define LP5812_DEV_CONFIG6 0x0007 +#define LP5812_DEV_CONFIG7 0x0008 +#define LP5812_DEV_CONFIG8 0x0009 +#define LP5812_DEV_CONFIG9 0x000A +#define LP5812_DEV_CONFIG10 0x000B +#define LP5812_DEV_CONFIG11 0x000c +#define LP5812_DEV_CONFIG12 0x000D +#define LP5812_CMD_UPDATE 0x0010 +#define LP5812_LED_EN_1 0x0020 +#define LP5812_LED_EN_2 0x0021 +#define LP5812_FAULT_CLEAR 0x0022 +#define LP5812_MANUAL_DC_BASE 0x0030 +#define LP5812_AUTO_DC_BASE 0x0050 +#define LP5812_MANUAL_PWM_BASE 0x0040 + +#define LP5812_TSD_CONFIG_STATUS 0x0300 +#define LP5812_LOD_STATUS 0x0301 +#define LP5812_LSD_STATUS 0x0303 + +#define LP5812_ENABLE_DEFAULT 0x01 +#define FAULT_CLEAR_ALL 0x07 +#define TSD_CLEAR_VAL 0x04 +#define LSD_CLEAR_VAL 0x02 +#define LOD_CLEAR_VAL 0x01 +#define LP5812_RESET 0x66 +#define LP5812_DEV_CONFIG12_DEFAULT 0x08 + +#define LP5812_UPDATE_CMD_VAL 0x55 +#define LP5812_REG_ADDR_HIGH_SHIFT 8 +#define LP5812_REG_ADDR_BIT_8_9_MASK 0x03 +#define LP5812_REG_ADDR_LOW_MASK 0xFF +#define LP5812_CHIP_ADDR_SHIFT 0 /* no address shift for LP5810 */ +#define LP5812_DATA_LENGTH 2 +#define LP5812_DATA_BYTE_0_IDX 0 +#define LP5812_DATA_BYTE_1_IDX 1 + +#define LP5812_READ_MSG_LENGTH 2 +#define LP5812_MSG_0_IDX 0 +#define LP5812_MSG_1_IDX 1 +#define LP5812_CFG_ERR_STATUS_MASK 0x01 +#define LP5812_CFG_TSD_STATUS_SHIFT 1 +#define LP5812_CFG_TSD_STATUS_MASK 0x01 + +#define LP5812_FAULT_CLEAR_LOD 0 +#define LP5812_FAULT_CLEAR_LSD 1 +#define LP5812_FAULT_CLEAR_TSD 2 +#define LP5812_FAULT_CLEAR_ALL 3 +#define LP5812_NUMBER_LED_IN_REG 8 + +#define LP5812_WAIT_DEVICE_STABLE_MIN 1000 +#define LP5812_WAIT_DEVICE_STABLE_MAX 1100 + +#define LP5812_LSD_LOD_START_UP 0x0B +#define LP5812_MODE_NAME_MAX_LEN 20 +#define LP5812_MODE_DIRECT_NAME "direct_mode" +#define LP5812_MODE_DIRECT_VALUE 0 +#define LP5812_MODE_MIX_SELECT_LED_0 0 +#define LP5812_MODE_MIX_SELECT_LED_1 1 +#define LP5812_MODE_MIX_SELECT_LED_2 2 +#define LP5812_MODE_MIX_SELECT_LED_3 3 + +enum control_mode { + LP5812_MODE_MANUAL = 0, + LP5812_MODE_AUTONOMOUS +}; + +enum dimming_type { + LP5812_DIMMING_ANALOG, + LP5812_DIMMING_PWM +}; + +union u_scan_order { + struct { + u8 scan_order_0:2; + u8 scan_order_1:2; + u8 scan_order_2:2; + u8 scan_order_3:2; + } s_scan_order; + u8 scan_order_val; +}; + +union u_drive_mode { + struct { + u8 mix_sel_led_0:1; + u8 mix_sel_led_1:1; + u8 mix_sel_led_2:1; + u8 mix_sel_led_3:1; + u8 led_mode:3; + u8 pwm_fre:1; + } s_drive_mode; + u8 drive_mode_val; +}; + +struct lp5812_reg { + u16 addr; + union { + u8 val; + u8 mask; + u8 shift; + }; +}; + +struct lp5812_mode_mapping { + char mode_name[LP5812_MODE_NAME_MAX_LEN]; + u8 mode; + u8 scan_order_0; + u8 scan_order_1; + u8 scan_order_2; + u8 scan_order_3; + u8 selection_led; +}; + +struct lp5812_led_config { + bool is_sc_led; + const char *name; + u8 color_id[LED_COLOR_ID_MAX]; + u32 max_current[LED_COLOR_ID_MAX]; + int chan_nr; + int num_colors; + int led_id[LED_COLOR_ID_MAX]; +}; + +struct lp5812_chip { + u8 num_channels; + struct i2c_client *client; + struct mutex lock; /* Protects register access */ + struct lp5812_led_config *led_config; + const char *label; + const char *scan_mode; + const struct lp5812_device_config *cfg; + union u_scan_order u_scan_order; + union u_drive_mode u_drive_mode; +}; + +struct lp5812_led { + u8 brightness; + int chan_nr; + struct led_classdev cdev; + struct led_classdev_mc mc_cdev; + struct lp5812_chip *chip; +}; + +struct lp5812_device_config { + const struct lp5812_reg reg_reset; + const struct lp5812_reg reg_chip_en; + const struct lp5812_reg reg_dev_config_0; + const struct lp5812_reg reg_dev_config_1; + const struct lp5812_reg reg_dev_config_2; + const struct lp5812_reg reg_dev_config_3; + const struct lp5812_reg reg_dev_config_4; + const struct lp5812_reg reg_dev_config_5; + const struct lp5812_reg reg_dev_config_6; + const struct lp5812_reg reg_dev_config_7; + const struct lp5812_reg reg_dev_config_12; + const struct lp5812_reg reg_cmd_update; + + const struct lp5812_reg reg_led_en_1; + const struct lp5812_reg reg_led_en_2; + const struct lp5812_reg reg_fault_clear; + const struct lp5812_reg reg_manual_dc_base; + const struct lp5812_reg reg_auto_dc_base; + const struct lp5812_reg reg_manual_pwm_base; + const struct lp5812_reg reg_tsd_config_status; + const struct lp5812_reg reg_lod_status_base; + const struct lp5812_reg reg_lsd_status_base; +}; + +#endif /*_LP5812_H_*/ \ No newline at end of file diff --git a/package/mono/mono-leds/src/sfp-led/sfp-led.c b/package/mono/mono-leds/src/sfp-led/sfp-led.c new file mode 100644 index 0000000000..82125a2d5e --- /dev/null +++ b/package/mono/mono-leds/src/sfp-led/sfp-led.c @@ -0,0 +1,677 @@ +// SPDX-License-Identifier: GPL-2.0-or-later +/* + * SFP LED Control Platform Driver (Passive Monitor) + * + * Controls SFP port LEDs based on module presence and link state. + * This driver passively monitors I2C and netdev state without interfering + * with the SFP subsystem or MAC driver. + * + * Design principle: + * This driver does NOT interact with the kernel's SFP state machine. + * This avoids conflicts with MAC drivers that use fixed-link configuration + * (like NXP DPAA SDK). + * + * Detection method: + * - Module presence: Probe I2C EEPROM at 0x50 + * - Link state: Check netdev operstate (for DAC) or I2C DDM (for fiber) + * - Activity: Poll netdev tx/rx packet counters + * + * LED behavior: + * + * State | Green (Link) | Orange (Activity) + * ---------------------------|--------------|------------------- + * No module | OFF | OFF + * Module present, no link | OFF | ON (solid) + * Module present, link up | ON | Blinks on traffic + * + * Module type detection: + * - Fiber SFP: Uses I2C DDM (A2h byte 110 LOS bit) for link detection + * - DAC cable: Uses netdev operstate (DAC has no DDM support) + * - Detection via A0h page bytes 3 and 8 copper compliance bits + * + * Device tree binding example: + * + * sfp0: sfp-0 { + * compatible = "sff,sfp"; + * i2c-bus = <&sfp0_i2c>; + * }; + * + * sfp-led-controller { + * compatible = "mono,sfp-led"; + * + * port@0 { + * sfp = <&sfp0>; + * leds = <&led_sfp0_link>, <&led_sfp0_activity>; + * }; + * port@1 { + * sfp = <&sfp1>; + * leds = <&led_sfp1_link>, <&led_sfp1_activity>; + * }; + * }; + * + * // MAC node must reference SFP for netdev association + * &fman_mac { + * sfp = <&sfp0>; + * }; + * + * Copyright 2026 Mono Technologies Inc. + * Author: Tomaz Zaman + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#define DRIVER_NAME "sfp-led" +#define SFP_LED_NAME_SIZE 64 + +/* Polling interval in milliseconds */ +#define SFP_LED_POLL_INTERVAL_MS 100 +#define SFP_LED_NETDEV_RETRY_MS 1000 +#define SFP_LED_MAX_NETDEV_RETRIES 30 + +/* SFP I2C addresses per SFP MSA */ +#define SFP_EEPROM_ADDR 0x50 /* A0h page - module ID/capabilities */ +#define SFP_DIAG_ADDR 0x51 /* A2h page - diagnostics/status */ + +/* SFP A2h page registers */ +#define SFP_STATUS_CTRL_REG 110 /* Status/Control register */ +#define SFP_STATUS_LOS BIT(1) /* RX Loss of Signal */ +#define SFP_STATUS_TX_FAULT BIT(2) /* TX Fault */ + +/* SFP A0h page - cable type detection (per SFP MSA) */ +#define SFP_PHYS_EXT_ID 1 /* Extended identifier */ +#define SFP_COMPLIANCE_3 3 /* 10G/1G Ethernet compliance */ +#define SFP_COMPLIANCE_8 8 /* SFP+ cable technology */ +#define SFP_8472_COMPLIANCE 94 /* SFF-8472 compliance (DDM support) */ + +/* Byte 3 bits - 1G Ethernet copper compliance */ +#define SFP_IF_1X_COPPER_PASSIVE BIT(0) +#define SFP_IF_1X_COPPER_ACTIVE BIT(1) + +/* Byte 8 bits - SFP+ cable technology */ +#define SFP_CT_PASSIVE BIT(2) /* Passive cable */ +#define SFP_CT_ACTIVE BIT(3) /* Active cable */ + +struct sfp_led_port { + struct sfp_led_priv *priv; + struct device_node *sfp_np; + struct i2c_adapter *i2c_adapter; /* I2C bus for module detection */ + struct net_device *netdev; + char netdev_name[IFNAMSIZ]; + + struct led_classdev *link_led; + struct led_classdev *activity_led; + + char link_led_name[SFP_LED_NAME_SIZE]; + char activity_led_name[SFP_LED_NAME_SIZE]; + + struct delayed_work poll_work; + int netdev_retries; + int debug_count; /* Rate limit debug output */ + + /* Cached state for change detection */ + bool last_module_present; + bool last_carrier; + bool is_dac; /* True if DAC cable (no DDM support) */ + u64 last_tx_packets; + u64 last_rx_packets; + bool activity_led_on; +}; + +struct sfp_led_priv { + struct device *dev; + int num_ports; + struct sfp_led_port *ports; +}; + +/* + * Find the netdev associated with an SFP by traversing device tree. + * + * DPAA device tree structure: + * fsldpaa/ethernet@8 (fsl,dpa-ethernet) -> fsl,fman-mac = <&enet6> + * fman0/ethernet@f0000 (enet6) -> sfp = <&sfp_xfi0> + */ +static struct net_device *sfp_led_find_netdev(struct device_node *sfp_np) +{ + struct net_device *dev, *found = NULL; + + if (!sfp_np) + return NULL; + + /* + * Use rtnl_trylock to avoid deadlock: this is called from a + * workqueue, and networking teardown can flush the system workqueue + * while holding rtnl_lock. If we can't get the lock, the caller + * will retry on the next poll cycle. + */ + if (!rtnl_trylock()) + return NULL; + + for_each_netdev(&init_net, dev) { + struct device *parent = dev->dev.parent; + struct device_node *dpaa_node, *mac_node, *sfp_ref; + + if (!parent || !parent->of_node) + continue; + + dpaa_node = parent->of_node; + + mac_node = of_parse_phandle(dpaa_node, "fsl,fman-mac", 0); + if (!mac_node) + continue; + + sfp_ref = of_parse_phandle(mac_node, "sfp", 0); + of_node_put(mac_node); + + if (sfp_ref == sfp_np) { + of_node_put(sfp_ref); + found = dev; + dev_hold(found); + break; + } + of_node_put(sfp_ref); + } + + rtnl_unlock(); + + return found; +} + +/* + * Detect SFP module presence by probing I2C EEPROM at address 0x50. + * All SFP/SFP+ modules have an EEPROM that responds to this address. + * Returns true if module is present, false otherwise. + */ +static bool sfp_led_i2c_module_present(struct sfp_led_port *port) +{ + union i2c_smbus_data data; + int ret; + + if (!port->i2c_adapter) + return false; + + /* + * Try to read byte 0 from SFP EEPROM (identifier byte). + * If the read succeeds, a module is present. + */ + ret = i2c_smbus_xfer(port->i2c_adapter, SFP_EEPROM_ADDR, + 0, I2C_SMBUS_READ, 0, + I2C_SMBUS_BYTE_DATA, &data); + + return ret >= 0; +} + +static bool sfp_led_module_present(struct sfp_led_port *port) +{ + return sfp_led_i2c_module_present(port); +} + +/* + * Detect if the SFP module is a DAC (Direct Attach Copper) cable. + * DAC cables don't have optical transceivers and don't support DDM. + * Detection via A0h page compliance bytes per SFP MSA: + * - Byte 3 bits 0-1: 1G copper passive/active + * - Byte 8 bits 2-3: SFP+ cable technology passive/active + * Returns true if DAC cable, false if fiber/optical module. + */ +static bool sfp_led_is_dac_cable(struct sfp_led_port *port) +{ + union i2c_smbus_data data; + int ret; + u8 byte3, byte8; + bool is_dac; + + if (!port->i2c_adapter) + return false; /* Can't detect, assume fiber */ + + /* Read byte 3 - 10G/1G Ethernet compliance */ + ret = i2c_smbus_xfer(port->i2c_adapter, SFP_EEPROM_ADDR, + 0, I2C_SMBUS_READ, SFP_COMPLIANCE_3, + I2C_SMBUS_BYTE_DATA, &data); + if (ret < 0) { + dev_warn(port->priv->dev, "%s: failed to read A0h byte 3: %d\n", + port->link_led_name, ret); + return false; + } + byte3 = data.byte; + + /* Read byte 8 - SFP+ cable technology */ + ret = i2c_smbus_xfer(port->i2c_adapter, SFP_EEPROM_ADDR, + 0, I2C_SMBUS_READ, SFP_COMPLIANCE_8, + I2C_SMBUS_BYTE_DATA, &data); + if (ret < 0) { + dev_warn(port->priv->dev, "%s: failed to read A0h byte 8: %d\n", + port->link_led_name, ret); + return false; + } + byte8 = data.byte; + + /* Check copper compliance bits */ + is_dac = (byte3 & (SFP_IF_1X_COPPER_PASSIVE | SFP_IF_1X_COPPER_ACTIVE)) || + (byte8 & (SFP_CT_PASSIVE | SFP_CT_ACTIVE)); + + dev_dbg(port->priv->dev, "%s: A0h byte3=0x%02x byte8=0x%02x -> %s\n", + port->link_led_name, byte3, byte8, + is_dac ? "DAC cable" : "fiber/optical"); + + return is_dac; +} + +/* + * Read LOS (Loss of Signal) status from SFP module via I2C. + * SFP MSA defines status register at A2h (0x51), byte 110. + * Returns true if signal is lost, false if signal is present. + */ +static bool sfp_led_i2c_los(struct sfp_led_port *port) +{ + union i2c_smbus_data data; + int ret; + if (!port->i2c_adapter) + return true; /* Assume no signal if can't check */ + + ret = i2c_smbus_xfer(port->i2c_adapter, SFP_DIAG_ADDR, + 0, I2C_SMBUS_READ, SFP_STATUS_CTRL_REG, + I2C_SMBUS_BYTE_DATA, &data); + + /* Debug: log every 50th read or on error */ + if (ret < 0) { + if (port->debug_count++ % 50 == 0) + dev_dbg(port->priv->dev, "%s: A2h read failed: %d (no DDM?)\n", + port->link_led_name, ret); + return true; /* Error reading - assume no signal */ + } + + /* Debug: log status byte periodically */ + if (port->debug_count++ % 50 == 0) + dev_dbg(port->priv->dev, "%s: A2h[110]=0x%02x LOS=%d\n", + port->link_led_name, data.byte, + (data.byte & SFP_STATUS_LOS) ? 1 : 0); + + return (data.byte & SFP_STATUS_LOS) != 0; +} + +/* + * Check if interface has operational link. + * For DAC cables with DPAA fixed-link, get_link() is unreliable. + * Use operstate which accurately reflects actual link status. + */ +static bool sfp_led_has_operational_link(struct net_device *netdev) +{ + /* + * operstate reflects actual link status: + * - IF_OPER_UP: interface is up and link is established + * - IF_OPER_DOWN: interface is down or no link + * This is what ethtool uses for "Link detected" field. + */ + return netdev->operstate == IF_OPER_UP; +} + +/* + * Check if we have signal/link. + * For fiber modules: I2C DDM status register (A2h byte 110 LOS bit) + * For DAC cables: operstate (DAC has no DDM support) + * Returns true if link/signal is present. + */ +static bool sfp_led_has_signal(struct sfp_led_port *port) +{ + /* + * DAC cables don't have optical transceivers and don't support DDM. + * The A2h page reads return 0xff which looks like LOS=1. + * For DAC, check operstate which accurately reflects link status, + * unlike get_link()/carrier which may be stale with fixed-link config. + */ + if (port->is_dac) { + if (port->netdev) + return sfp_led_has_operational_link(port->netdev); + return false; + } + + /* Fiber: Use I2C DDM status register (most fiber SFPs support DDM) */ + if (port->i2c_adapter) + return !sfp_led_i2c_los(port); + + /* Last resort: check operstate */ + if (port->netdev) + return sfp_led_has_operational_link(port->netdev); + + return false; +} + +static void sfp_led_set_link(struct sfp_led_port *port, bool on) +{ + if (!port->link_led) + return; + + led_set_brightness(port->link_led, + on ? port->link_led->max_brightness : LED_OFF); +} + +static void sfp_led_set_activity(struct sfp_led_port *port, bool on) +{ + if (!port->activity_led) + return; + + /* Don't override if user has configured a trigger */ + if (port->activity_led->trigger) + return; + + led_set_brightness(port->activity_led, + on ? port->activity_led->max_brightness : LED_OFF); +} + +static void sfp_led_poll_work_handler(struct work_struct *work) +{ + struct sfp_led_port *port = container_of(work, struct sfp_led_port, + poll_work.work); + struct net_device *netdev; + bool module_present, carrier; + struct rtnl_link_stats64 stats; + bool had_activity; + + /* Check module presence via I2C */ + module_present = sfp_led_module_present(port); + + /* Handle module state changes */ + if (module_present != port->last_module_present) { + port->last_module_present = module_present; + + if (!module_present) { + /* Module removed - turn off both LEDs and reset state */ + sfp_led_set_link(port, false); + sfp_led_set_activity(port, false); + port->last_carrier = false; + port->is_dac = false; + dev_dbg(port->priv->dev, "%s: module removed\n", + port->link_led_name); + } else { + /* Module inserted - detect cable type */ + port->is_dac = sfp_led_is_dac_cable(port); + /* Set initial state: module present, no link yet */ + sfp_led_set_link(port, false); + sfp_led_set_activity(port, true); + dev_dbg(port->priv->dev, "%s: module inserted (%s)\n", + port->link_led_name, + port->is_dac ? "DAC" : "fiber"); + } + } + + if (!module_present) + goto reschedule; + + /* Try to find netdev if we don't have one yet */ + netdev = READ_ONCE(port->netdev); + if (!netdev) { + netdev = sfp_led_find_netdev(port->sfp_np); + if (netdev) { + if (cmpxchg(&port->netdev, NULL, netdev) != NULL) { + dev_put(netdev); + } else { + strscpy(port->netdev_name, netdev->name, + sizeof(port->netdev_name)); + dev_dbg(port->priv->dev, "%s: found netdev %s\n", + port->link_led_name, netdev->name); + } + } else { + port->netdev_retries++; + if (port->netdev_retries < SFP_LED_MAX_NETDEV_RETRIES) { + /* Keep activity LED on to show module present */ + sfp_led_set_activity(port, true); + } + } + netdev = READ_ONCE(port->netdev); + } + + if (!netdev) { + /* No netdev yet - show module present only */ + sfp_led_set_link(port, false); + sfp_led_set_activity(port, true); + goto reschedule; + } + + /* Check signal/carrier state */ + carrier = sfp_led_has_signal(port); + + if (carrier != port->last_carrier) { + port->last_carrier = carrier; + sfp_led_set_link(port, carrier); + + if (carrier) { + dev_dbg(port->priv->dev, "%s: link up\n", + port->link_led_name); + /* Turn off activity LED and reset counters on link up */ + sfp_led_set_activity(port, false); + port->last_tx_packets = 0; + port->last_rx_packets = 0; + port->activity_led_on = false; + } else { + dev_dbg(port->priv->dev, "%s: link down\n", + port->link_led_name); + /* Module present but no link - solid activity LED */ + sfp_led_set_activity(port, true); + } + } + + if (!carrier) + goto reschedule; + + /* Monitor activity when link is up */ + if (!netif_running(netdev)) + goto reschedule; + + dev_get_stats(netdev, &stats); + + had_activity = (stats.tx_packets != port->last_tx_packets || + stats.rx_packets != port->last_rx_packets); + + if (had_activity) { + /* Toggle LED for visible blink */ + port->activity_led_on = !port->activity_led_on; + sfp_led_set_activity(port, port->activity_led_on); + port->last_tx_packets = stats.tx_packets; + port->last_rx_packets = stats.rx_packets; + } else if (port->activity_led_on) { + /* No activity - turn off */ + port->activity_led_on = false; + sfp_led_set_activity(port, false); + } + +reschedule: + schedule_delayed_work(&port->poll_work, + msecs_to_jiffies(SFP_LED_POLL_INTERVAL_MS)); +} + +static int sfp_led_register_port(struct sfp_led_priv *priv, + struct device_node *port_np, int index) +{ + struct sfp_led_port *port = &priv->ports[index]; + struct device_node *sfp_np, *i2c_np; + + /* Parse SFP node from port child node */ + sfp_np = of_parse_phandle(port_np, "sfp", 0); + if (!sfp_np) { + dev_err(priv->dev, "port %d: missing sfp phandle\n", index); + return -ENODEV; + } + + port->priv = priv; + port->sfp_np = sfp_np; + /* sfp_np has ref from of_parse_phandle — don't add of_node_get */ + + /* Get I2C adapter for module detection */ + i2c_np = of_parse_phandle(sfp_np, "i2c-bus", 0); + if (i2c_np) { + port->i2c_adapter = of_get_i2c_adapter_by_node(i2c_np); + of_node_put(i2c_np); + } + + /* + * of_get_i2c_adapter_by_node() returns NULL, never an ERR_PTR, so + * an absent adapter cannot be told apart from one that has not + * probed yet and deferral is impossible here. Loading after the + * I2C mux adapters exist is the loader's responsibility (S99 in + * the recovery image). + */ + if (!port->i2c_adapter) { + dev_err(priv->dev, "port %d: i2c-bus not available\n", index); + of_node_put(sfp_np); + port->sfp_np = NULL; + return -ENODEV; + } + + /* Get LEDs from port child node */ + port->link_led = of_led_get(port_np, 0); + if (IS_ERR(port->link_led)) { + dev_dbg(priv->dev, "port %d: link LED not in DT: %ld\n", + index, PTR_ERR(port->link_led)); + port->link_led = NULL; + } + + port->activity_led = of_led_get(port_np, 1); + if (IS_ERR(port->activity_led)) { + dev_dbg(priv->dev, "port %d: activity LED not in DT: %ld\n", + index, PTR_ERR(port->activity_led)); + port->activity_led = NULL; + } + + /* Set LED names for logging */ + if (port->link_led && port->link_led->name) + strscpy(port->link_led_name, port->link_led->name, + sizeof(port->link_led_name)); + else + snprintf(port->link_led_name, sizeof(port->link_led_name), + "sfp%d:link", index); + + if (port->activity_led && port->activity_led->name) + strscpy(port->activity_led_name, port->activity_led->name, + sizeof(port->activity_led_name)); + else + snprintf(port->activity_led_name, sizeof(port->activity_led_name), + "sfp%d:activity", index); + + /* Initialize work */ + INIT_DELAYED_WORK(&port->poll_work, sfp_led_poll_work_handler); + + /* Start polling */ + schedule_delayed_work(&port->poll_work, 0); + + dev_dbg(priv->dev, "registered port %d: %pOFn (link=%s, activity=%s)\n", + index, sfp_np, port->link_led_name, port->activity_led_name); + + return 0; +} + +static void sfp_led_cleanup_port(struct sfp_led_port *port) +{ + if (!port->sfp_np) + return; + + cancel_delayed_work_sync(&port->poll_work); + + sfp_led_set_link(port, false); + sfp_led_set_activity(port, false); + + if (port->activity_led) { + led_put(port->activity_led); + port->activity_led = NULL; + } + + if (port->link_led) { + led_put(port->link_led); + port->link_led = NULL; + } + + if (port->netdev) { + dev_put(port->netdev); + port->netdev = NULL; + } + + if (port->i2c_adapter) { + i2c_put_adapter(port->i2c_adapter); + port->i2c_adapter = NULL; + } + + of_node_put(port->sfp_np); + port->sfp_np = NULL; +} + +static int sfp_led_probe(struct platform_device *pdev) +{ + struct sfp_led_priv *priv; + struct device_node *child; + int count, i, registered = 0; + + priv = devm_kzalloc(&pdev->dev, sizeof(*priv), GFP_KERNEL); + if (!priv) + return -ENOMEM; + + priv->dev = &pdev->dev; + platform_set_drvdata(pdev, priv); + + count = of_get_child_count(pdev->dev.of_node); + if (count == 0) { + dev_err(&pdev->dev, "no port child nodes specified\n"); + return -ENODEV; + } + + priv->ports = devm_kcalloc(&pdev->dev, count, + sizeof(*priv->ports), GFP_KERNEL); + if (!priv->ports) + return -ENOMEM; + + priv->num_ports = count; + + i = 0; + for_each_child_of_node(pdev->dev.of_node, child) { + if (sfp_led_register_port(priv, child, i) == 0) + registered++; + i++; + } + + if (registered == 0) { + dev_err(&pdev->dev, "no SFP ports registered\n"); + return -ENODEV; + } + + dev_dbg(&pdev->dev, "loaded (passive monitor, %d ports)\n", registered); + return 0; +} + +static void sfp_led_remove(struct platform_device *pdev) +{ + struct sfp_led_priv *priv = platform_get_drvdata(pdev); + int i; + + for (i = 0; i < priv->num_ports; i++) + sfp_led_cleanup_port(&priv->ports[i]); + + dev_dbg(&pdev->dev, "unloaded\n"); +} + +static const struct of_device_id sfp_led_of_match[] = { + { .compatible = "mono,sfp-led" }, + { } +}; +MODULE_DEVICE_TABLE(of, sfp_led_of_match); + +static struct platform_driver sfp_led_driver = { + .probe = sfp_led_probe, + .remove = sfp_led_remove, + .driver = { + .name = DRIVER_NAME, + .of_match_table = sfp_led_of_match, + }, +}; +module_platform_driver(sfp_led_driver); + +MODULE_AUTHOR("Tomaz Zaman "); +MODULE_DESCRIPTION("SFP LED Control Platform Driver (Passive Monitor)"); +MODULE_LICENSE("GPL"); diff --git a/target/linux/layerscape/image/armv8_64b.mk b/target/linux/layerscape/image/armv8_64b.mk index ff58009063..d495b55fde 100644 --- a/target/linux/layerscape/image/armv8_64b.mk +++ b/target/linux/layerscape/image/armv8_64b.mk @@ -429,7 +429,7 @@ define Device/mono_gateway-dk DEVICE_VENDOR := Mono DEVICE_MODEL := Gateway DK DEVICE_PACKAGES := kmod-ask-cdx kmod-ask-fci kmod-ask-auto-bridge \ - cmm dpa-app fmc + cmm dpa-app fmc kmod-leds-lp5812 kmod-sfp-led fancontrol KERNEL_NAME := Image KERNEL := kernel-bin | gzip MONO_BOOTFS_SIZE := 64 -- 2.47.3