diff --git a/arch/arm/boot/dts/overlays/Makefile b/arch/arm/boot/dts/overlays/Makefile index 367cbf437e1b0f..2d34c137c730d6 100644 --- a/arch/arm/boot/dts/overlays/Makefile +++ b/arch/arm/boot/dts/overlays/Makefile @@ -342,7 +342,8 @@ dtbo-$(CONFIG_ARCH_BCM2835) += \ waveshare-can-fd-hat-mode-a.dtbo \ waveshare-can-fd-hat-mode-b.dtbo \ wittypi.dtbo \ - wm8960-soundcard.dtbo + wm8960-soundcard.dtbo \ + ws2812-pio.dtbo targets += dtbs dtbs_install targets += $(dtbo-y) diff --git a/arch/arm/boot/dts/overlays/README b/arch/arm/boot/dts/overlays/README index 4cde0bacd25397..3660d55a8a15d0 100644 --- a/arch/arm/boot/dts/overlays/README +++ b/arch/arm/boot/dts/overlays/README @@ -5599,6 +5599,28 @@ Params: alsaname Changes the card name in ALSA compatible Changes the codec compatibility +Name: ws2812-pio +Info: Configures a GPIO pin to drive a string of WS2812 LEDS using pio. It + can be enabled on any RP1 GPIO in bank 0 (0-27). Up to 4 are supported, + assuming nothing else is using PIO. Pi 5 only. +Load: dtoverlay=ws2812-pio,= +Params: brightness Set the initial brightness for the LEDs. The + brightness can be changed at runtime by writing + a single byte to offset 0 of the device. Note + that brightness is a multiplier for the pixel + values, and only white pixels can reach the + maximum visible brightness. (range 0-255, + default 255) + dev_name The name for the /dev/ device entry. Note that + if the name includes '%d' it will be replaced + by the instance number. (default 'leds%d') + gpio Output GPIO (0-27, default 4) + num_leds Number of LEDs (default 60) + rgbw 'rgbw=on' (or 'rgbw') indicates that each pixel + includes a white LED as well as the usual red, + green and blue. (default 'off') + + Troubleshooting =============== diff --git a/arch/arm/boot/dts/overlays/ws2812-pio-overlay.dts b/arch/arm/boot/dts/overlays/ws2812-pio-overlay.dts new file mode 100644 index 00000000000000..4550e16d44e1c7 --- /dev/null +++ b/arch/arm/boot/dts/overlays/ws2812-pio-overlay.dts @@ -0,0 +1,46 @@ +// SPDX-License-Identifier: GPL-2.0 +// Device tree overlay for RP1 PIO WS2812 driver. +/dts-v1/; +/plugin/; + +/ { + compatible = "brcm,bcm2712"; + + fragment@0 { + target = <&gpio>; + __overlay__ { + ws2812_pio_pins: ws2812_pio_pins@4 { + brcm,pins = <4>; /* gpio 4 */ + function = "pio"; + bias-disable; + }; + }; + }; + + fragment@1 { + target-path = "/"; + __overlay__ { + ws2812_pio: ws2812_pio@4 { + compatible = "raspberrypi,ws2812-pio-rp1"; + pinctrl-names = "default"; + pinctrl-0 = <&ws2812_pio_pins>; + dev-name = "leds%d"; + leds-gpios = <&gpio 4 0>; + rpi,num-leds = <60>; + rpi,brightness = <255>; + }; + }; + }; + + __overrides__ { + brightness = <&ws2812_pio>, "rpi,brightness:0"; + dev_name = <&ws2812_pio>, "dev-name"; + gpio = <&ws2812_pio>,"leds-gpios:4", + <&ws2812_pio_pins>,"brcm,pins:0", + /* modify reg values to allow multiple instantiation */ + <&ws2812_pio>,"reg:0", + <&ws2812_pio_pins>,"reg:0"; + num_leds = <&ws2812_pio>, "rpi,num-leds:0"; + rgbw = <&ws2812_pio>, "rpi,rgbw?"; + }; +}; diff --git a/arch/arm64/configs/bcm2711_defconfig b/arch/arm64/configs/bcm2711_defconfig index 221ae9410f2443..6411b2d68c566a 100644 --- a/arch/arm64/configs/bcm2711_defconfig +++ b/arch/arm64/configs/bcm2711_defconfig @@ -455,6 +455,7 @@ CONFIG_DEVTMPFS=y CONFIG_DEVTMPFS_MOUNT=y # CONFIG_BRCMSTB_GISB_ARB is not set CONFIG_RASPBERRYPI_FIRMWARE=y +CONFIG_FIRMWARE_RP1=m # CONFIG_EFI_VARS_PSTORE is not set CONFIG_MTD=m CONFIG_MTD_BLOCK=m @@ -475,6 +476,7 @@ CONFIG_BLK_DEV_RBD=m CONFIG_BLK_DEV_NVME=y CONFIG_NVME_HWMON=y CONFIG_RP1_PIO=m +CONFIG_WS2812_PIO_RP1=m CONFIG_SRAM=y CONFIG_EEPROM_AT24=m CONFIG_EEPROM_AT25=m diff --git a/arch/arm64/configs/bcm2712_defconfig b/arch/arm64/configs/bcm2712_defconfig index 3373df51f285ee..cf1f73f526c9ae 100644 --- a/arch/arm64/configs/bcm2712_defconfig +++ b/arch/arm64/configs/bcm2712_defconfig @@ -458,6 +458,7 @@ CONFIG_DEVTMPFS=y CONFIG_DEVTMPFS_MOUNT=y # CONFIG_BRCMSTB_GISB_ARB is not set CONFIG_RASPBERRYPI_FIRMWARE=y +CONFIG_FIRMWARE_RP1=m # CONFIG_EFI_VARS_PSTORE is not set CONFIG_MTD=m CONFIG_MTD_BLOCK=m @@ -478,6 +479,7 @@ CONFIG_BLK_DEV_RBD=m CONFIG_BLK_DEV_NVME=y CONFIG_NVME_HWMON=y CONFIG_RP1_PIO=m +CONFIG_WS2812_PIO_RP1=m CONFIG_SRAM=y CONFIG_EEPROM_AT24=m CONFIG_EEPROM_AT25=m diff --git a/drivers/misc/Kconfig b/drivers/misc/Kconfig index df42bd077b1942..74c592d4eafd20 100644 --- a/drivers/misc/Kconfig +++ b/drivers/misc/Kconfig @@ -19,12 +19,22 @@ config BCM2835_SMI config RP1_PIO tristate "Raspberry Pi RP1 PIO driver" - select FIRMWARE_RP1 + depends on FIRMWARE_RP1 || COMPILE_TEST default n help Driver providing control of the Raspberry Pi PIO block, as found in RP1. +config WS2812_PIO_RP1 + tristate "Raspberry Pi PIO-base WS2812 driver" + depends on RP1_PIO || COMPILE_TEST + default n + help + Driver for the WS2812 (NeoPixel) LEDs using the RP1 PIO hardware. + The driver creates a character device to which rgbw pixels may be + written. Single-byte writes to offset 0 set the brightness at + runtime. + config AD525X_DPOT tristate "Analog Devices Digital Potentiometers" depends on (I2C || SPI) && SYSFS diff --git a/drivers/misc/Makefile b/drivers/misc/Makefile index 16656f4c3c923d..93491775156856 100644 --- a/drivers/misc/Makefile +++ b/drivers/misc/Makefile @@ -19,6 +19,7 @@ obj-$(CONFIG_PHANTOM) += phantom.o obj-$(CONFIG_QCOM_COINCELL) += qcom-coincell.o obj-$(CONFIG_QCOM_FASTRPC) += fastrpc.o obj-$(CONFIG_RP1_PIO) += rp1-pio.o +obj-$(CONFIG_WS2812_PIO_RP1) += ws2812-pio-rp1.o obj-$(CONFIG_SENSORS_BH1770) += bh1770glc.o obj-$(CONFIG_SENSORS_APDS990X) += apds990x.o obj-$(CONFIG_ENCLOSURE_SERVICES) += enclosure.o diff --git a/drivers/misc/rp1-pio.c b/drivers/misc/rp1-pio.c index a5469cdf156bb2..c0576432281d26 100644 --- a/drivers/misc/rp1-pio.c +++ b/drivers/misc/rp1-pio.c @@ -61,9 +61,15 @@ #define DMA_BOUNCE_BUFFER_SIZE 0x1000 #define DMA_BOUNCE_BUFFER_COUNT 4 +struct dma_xfer_state { + struct dma_info *dma; + void (*callback)(void *param); + void *callback_param; +}; + struct dma_buf_info { void *buf; - dma_addr_t phys; + dma_addr_t dma_addr; struct scatterlist sgl; }; @@ -572,21 +578,34 @@ static void rp1_pio_sm_dma_callback(void *param) up(&dma->buf_sem); } +static void rp1_pio_sm_kernel_dma_callback(void *param) +{ + struct dma_xfer_state *dxs = param; + + dxs->dma->tail_idx++; + up(&dxs->dma->buf_sem); + + dxs->callback(dxs->callback_param); + + kfree(dxs); +} + static void rp1_pio_sm_dma_free(struct device *dev, struct dma_info *dma) { dmaengine_terminate_all(dma->chan); while (dma->buf_count > 0) { dma->buf_count--; dma_free_coherent(dev, ROUND_UP(dma->buf_size, PAGE_SIZE), - dma->bufs[dma->buf_count].buf, dma->bufs[dma->buf_count].phys); + dma->bufs[dma->buf_count].buf, + dma->bufs[dma->buf_count].dma_addr); } dma_release_channel(dma->chan); } -static int rp1_pio_sm_config_xfer(struct rp1_pio_client *client, void *param) +static int rp1_pio_sm_config_xfer_internal(struct rp1_pio_client *client, uint sm, uint dir, + uint buf_size, uint buf_count) { - struct rp1_pio_sm_config_xfer_args *args = param; struct rp1_pio_sm_set_dmactrl_args set_dmactrl_args; struct rp1_pio_device *pio = client->pio; struct platform_device *pdev = pio->pdev; @@ -596,17 +615,18 @@ static int rp1_pio_sm_config_xfer(struct rp1_pio_client *client, void *param) struct dma_info *dma; uint32_t dma_mask; char chan_name[4]; - uint buf_size; int ret = 0; - if (args->sm >= RP1_PIO_SMS_COUNT || args->dir >= RP1_PIO_DIR_COUNT || - !args->buf_size || (args->buf_size & 3) || - !args->buf_count || args->buf_count > DMA_BOUNCE_BUFFER_COUNT) + if (sm >= RP1_PIO_SMS_COUNT || dir >= RP1_PIO_DIR_COUNT) + return -EINVAL; + if ((buf_count || buf_size) && + (!buf_size || (buf_size & 3) || + !buf_count || buf_count > DMA_BOUNCE_BUFFER_COUNT)) return -EINVAL; - dma_mask = 1 << (args->sm * 2 + args->dir); + dma_mask = 1 << (sm * 2 + dir); - dma = &pio->dma_configs[args->sm][args->dir]; + dma = &pio->dma_configs[sm][dir]; spin_lock(&pio->lock); if (pio->claimed_dmas & dma_mask) @@ -615,16 +635,16 @@ static int rp1_pio_sm_config_xfer(struct rp1_pio_client *client, void *param) client->claimed_dmas |= dma_mask; spin_unlock(&pio->lock); - dma->buf_size = args->buf_size; + dma->buf_size = buf_size; /* Round up the allocations */ - buf_size = ROUND_UP(args->buf_size, PAGE_SIZE); + buf_size = ROUND_UP(buf_size, PAGE_SIZE); sema_init(&dma->buf_sem, 0); /* Allocate and configure a DMA channel */ /* Careful - each SM FIFO has its own DREQ value */ - chan_name[0] = (args->dir == RP1_PIO_DIR_TO_SM) ? 't' : 'r'; + chan_name[0] = (dir == RP1_PIO_DIR_TO_SM) ? 't' : 'r'; chan_name[1] = 'x'; - chan_name[2] = '0' + args->sm; + chan_name[2] = '0' + sm; chan_name[3] = '\0'; dma->chan = dma_request_chan(dev, chan_name); @@ -632,37 +652,37 @@ static int rp1_pio_sm_config_xfer(struct rp1_pio_client *client, void *param) return PTR_ERR(dma->chan); /* Alloc and map bounce buffers */ - for (dma->buf_count = 0; dma->buf_count < args->buf_count; dma->buf_count++) { + for (dma->buf_count = 0; dma->buf_count < buf_count; dma->buf_count++) { struct dma_buf_info *dbi = &dma->bufs[dma->buf_count]; dbi->buf = dma_alloc_coherent(dma->chan->device->dev, buf_size, - &dbi->phys, GFP_KERNEL); + &dbi->dma_addr, GFP_KERNEL); if (!dbi->buf) { ret = -ENOMEM; goto err_dma_free; } sg_init_table(&dbi->sgl, 1); - sg_dma_address(&dbi->sgl) = dbi->phys; + sg_dma_address(&dbi->sgl) = dbi->dma_addr; } fifo_addr = pio->phys_addr; - fifo_addr += args->sm * (RP1_PIO_FIFO_TX1 - RP1_PIO_FIFO_TX0); - fifo_addr += (args->dir == RP1_PIO_DIR_TO_SM) ? RP1_PIO_FIFO_TX0 : RP1_PIO_FIFO_RX0; + fifo_addr += sm * (RP1_PIO_FIFO_TX1 - RP1_PIO_FIFO_TX0); + fifo_addr += (dir == RP1_PIO_DIR_TO_SM) ? RP1_PIO_FIFO_TX0 : RP1_PIO_FIFO_RX0; config.src_addr_width = DMA_SLAVE_BUSWIDTH_4_BYTES; config.dst_addr_width = DMA_SLAVE_BUSWIDTH_4_BYTES; config.src_addr = fifo_addr; config.dst_addr = fifo_addr; - config.direction = (args->dir == RP1_PIO_DIR_TO_SM) ? DMA_MEM_TO_DEV : DMA_DEV_TO_MEM; + config.direction = (dir == RP1_PIO_DIR_TO_SM) ? DMA_MEM_TO_DEV : DMA_DEV_TO_MEM; ret = dmaengine_slave_config(dma->chan, &config); if (ret) goto err_dma_free; - set_dmactrl_args.sm = args->sm; - set_dmactrl_args.is_tx = (args->dir == RP1_PIO_DIR_TO_SM); + set_dmactrl_args.sm = sm; + set_dmactrl_args.is_tx = (dir == RP1_PIO_DIR_TO_SM); set_dmactrl_args.ctrl = RP1_PIO_DMACTRL_DEFAULT; - if (args->dir == RP1_PIO_DIR_FROM_SM) + if (dir == RP1_PIO_DIR_FROM_SM) set_dmactrl_args.ctrl = (RP1_PIO_DMACTRL_DEFAULT & ~0x1f) | 1; ret = rp1_pio_sm_set_dmactrl(client, &set_dmactrl_args); @@ -682,8 +702,16 @@ static int rp1_pio_sm_config_xfer(struct rp1_pio_client *client, void *param) return ret; } +static int rp1_pio_sm_config_xfer_user(struct rp1_pio_client *client, void *param) +{ + struct rp1_pio_sm_config_xfer_args *args = param; + + return rp1_pio_sm_config_xfer_internal(client, args->sm, args->dir, + args->buf_size, args->buf_count); +} + static int rp1_pio_sm_tx_user(struct rp1_pio_device *pio, struct dma_info *dma, - const void __user *userbuf, size_t bytes) + const void __user *userbuf, size_t bytes) { struct platform_device *pdev = pio->pdev; struct dma_async_tx_descriptor *desc; @@ -723,7 +751,7 @@ static int rp1_pio_sm_tx_user(struct rp1_pio_device *pio, struct dma_info *dma, DMA_PREP_INTERRUPT | DMA_CTRL_ACK | DMA_PREP_FENCE); if (!desc) { - dev_err(dev, "DMA preparation failedzn"); + dev_err(dev, "DMA preparation failed\n"); ret = -EIO; break; } @@ -757,7 +785,7 @@ static int rp1_pio_sm_tx_user(struct rp1_pio_device *pio, struct dma_info *dma, } static int rp1_pio_sm_rx_user(struct rp1_pio_device *pio, struct dma_info *dma, - void __user *userbuf, size_t bytes) + void __user *userbuf, size_t bytes) { struct platform_device *pdev = pio->pdev; struct dma_async_tx_descriptor *desc; @@ -779,7 +807,7 @@ static int rp1_pio_sm_rx_user(struct rp1_pio_device *pio, struct dma_info *dma, if (!bytes || dma->head_idx - dma->tail_idx == dma->buf_count) { if (down_timeout(&dma->buf_sem, msecs_to_jiffies(1000))) { - dev_err(dev, "DMA wait timed out"); + dev_err(dev, "DMA wait timed out\n"); ret = -ETIMEDOUT; break; } @@ -801,7 +829,7 @@ static int rp1_pio_sm_rx_user(struct rp1_pio_device *pio, struct dma_info *dma, DMA_PREP_INTERRUPT | DMA_CTRL_ACK | DMA_PREP_FENCE); if (!desc) { - dev_err(dev, "DMA preparation failed"); + dev_err(dev, "DMA preparation failed\n"); ret = -EIO; break; } @@ -809,8 +837,7 @@ static int rp1_pio_sm_rx_user(struct rp1_pio_device *pio, struct dma_info *dma, desc->callback = rp1_pio_sm_dma_callback; desc->callback_param = dma; - // Submit the buffer - the callback will kick the semaphore - + /* Submit the buffer - the callback will kick the semaphore */ ret = dmaengine_submit(desc); if (ret < 0) break; @@ -824,7 +851,7 @@ static int rp1_pio_sm_rx_user(struct rp1_pio_device *pio, struct dma_info *dma, return ret; } -static int rp1_pio_sm_xfer_data32(struct rp1_pio_client *client, void *param) +static int rp1_pio_sm_xfer_data32_user(struct rp1_pio_client *client, void *param) { struct rp1_pio_sm_xfer_data32_args *args = param; struct rp1_pio_device *pio = client->pio; @@ -842,7 +869,7 @@ static int rp1_pio_sm_xfer_data32(struct rp1_pio_client *client, void *param) return rp1_pio_sm_rx_user(pio, dma, args->data, args->data_bytes); } -static int rp1_pio_sm_xfer_data(struct rp1_pio_client *client, void *param) +static int rp1_pio_sm_xfer_data_user(struct rp1_pio_client *client, void *param) { struct rp1_pio_sm_xfer_data_args *args = param; struct rp1_pio_sm_xfer_data32_args args32; @@ -852,17 +879,97 @@ static int rp1_pio_sm_xfer_data(struct rp1_pio_client *client, void *param) args32.data_bytes = args->data_bytes; args32.data = args->data; - return rp1_pio_sm_xfer_data32(client, &args32); + return rp1_pio_sm_xfer_data32_user(client, &args32); +} + +int rp1_pio_sm_config_xfer(struct rp1_pio_client *client, uint sm, uint dir, + uint buf_size, uint buf_count) +{ + return rp1_pio_sm_config_xfer_internal(client, sm, dir, buf_size, buf_count); } +EXPORT_SYMBOL_GPL(rp1_pio_sm_config_xfer); + +int rp1_pio_sm_xfer_data(struct rp1_pio_client *client, uint sm, uint dir, + uint data_bytes, void *data, dma_addr_t dma_addr, + void (*callback)(void *param), void *param) +{ + struct rp1_pio_device *pio = client->pio; + struct platform_device *pdev = pio->pdev; + struct dma_async_tx_descriptor *desc; + struct dma_xfer_state *dxs = NULL; + struct device *dev = &pdev->dev; + struct dma_buf_info *dbi = NULL; + struct scatterlist sg; + struct dma_info *dma; + int ret = 0; + + if (sm >= RP1_PIO_SMS_COUNT || dir >= RP1_PIO_DIR_COUNT) + return -EINVAL; + + dma = &pio->dma_configs[sm][dir]; + + if (!dma_addr) { + dxs = kmalloc(sizeof(*dxs), GFP_KERNEL); + dxs->dma = dma; + dxs->callback = callback; + dxs->callback_param = param; + callback = rp1_pio_sm_kernel_dma_callback; + param = dxs; + + if (!dma->buf_count || data_bytes > dma->buf_size) + return -EINVAL; + + /* Grab a dma buffer */ + if (dma->head_idx - dma->tail_idx == dma->buf_count) { + if (down_timeout(&dma->buf_sem, msecs_to_jiffies(1000))) { + dev_err(dev, "DMA wait timed out\n"); + return -ETIMEDOUT; + } + } + + dbi = &dma->bufs[dma->head_idx % dma->buf_count]; + dma_addr = dbi->dma_addr; + + if (dir == PIO_DIR_TO_SM) + memcpy(dbi->buf, data, data_bytes); + } + + sg_init_table(&sg, 1); + sg_dma_address(&sg) = dma_addr; + sg_dma_len(&sg) = data_bytes; + + desc = dmaengine_prep_slave_sg(dma->chan, &sg, 1, + (dir == PIO_DIR_TO_SM) ? DMA_MEM_TO_DEV : DMA_DEV_TO_MEM, + DMA_PREP_INTERRUPT | DMA_CTRL_ACK | + DMA_PREP_FENCE); + if (!desc) { + dev_err(dev, "DMA preparation failed\n"); + return -EIO; + } + + desc->callback = callback; + desc->callback_param = param; + + ret = dmaengine_submit(desc); + if (ret < 0) { + dev_err(dev, "dmaengine_submit failed (%d)\n", ret); + return ret; + } + + dma_async_issue_pending(dma->chan); + + return 0; +} +EXPORT_SYMBOL_GPL(rp1_pio_sm_xfer_data); struct handler_info { const char *name; int (*func)(struct rp1_pio_client *client, void *param); int argsize; } ioctl_handlers[] = { - HANDLER(SM_CONFIG_XFER, sm_config_xfer), - HANDLER(SM_XFER_DATA, sm_xfer_data), - HANDLER(SM_XFER_DATA32, sm_xfer_data32), + HANDLER(SM_CONFIG_XFER, sm_config_xfer_user), + HANDLER(SM_XFER_DATA, sm_xfer_data_user), + HANDLER(SM_XFER_DATA32, sm_xfer_data32_user), HANDLER(CAN_ADD_PROGRAM, can_add_program), HANDLER(ADD_PROGRAM, add_program), @@ -903,7 +1010,7 @@ struct handler_info { HANDLER(WRITE_HW, write_hw), }; -struct rp1_pio_client *pio_open(void) +struct rp1_pio_client *rp1_pio_open(void) { struct rp1_pio_client *client; @@ -915,9 +1022,9 @@ struct rp1_pio_client *pio_open(void) return client; } -EXPORT_SYMBOL_GPL(pio_open); +EXPORT_SYMBOL_GPL(rp1_pio_open); -void pio_close(struct rp1_pio_client *client) +void rp1_pio_close(struct rp1_pio_client *client) { struct rp1_pio_device *pio = client->pio; uint claimed_dmas = client->claimed_dmas; @@ -959,31 +1066,31 @@ void pio_close(struct rp1_pio_client *client) kfree(client); } -EXPORT_SYMBOL_GPL(pio_close); +EXPORT_SYMBOL_GPL(rp1_pio_close); -void pio_set_error(struct rp1_pio_client *client, int err) +void rp1_pio_set_error(struct rp1_pio_client *client, int err) { client->error = err; } -EXPORT_SYMBOL_GPL(pio_set_error); +EXPORT_SYMBOL_GPL(rp1_pio_set_error); -int pio_get_error(const struct rp1_pio_client *client) +int rp1_pio_get_error(const struct rp1_pio_client *client) { return client->error; } -EXPORT_SYMBOL_GPL(pio_get_error); +EXPORT_SYMBOL_GPL(rp1_pio_get_error); -void pio_clear_error(struct rp1_pio_client *client) +void rp1_pio_clear_error(struct rp1_pio_client *client) { client->error = 0; } -EXPORT_SYMBOL_GPL(pio_clear_error); +EXPORT_SYMBOL_GPL(rp1_pio_clear_error); -static int rp1_pio_open(struct inode *inode, struct file *filp) +static int rp1_pio_file_open(struct inode *inode, struct file *filp) { struct rp1_pio_client *client; - client = pio_open(); + client = rp1_pio_open(); if (IS_ERR(client)) return PTR_ERR(client); @@ -992,11 +1099,11 @@ static int rp1_pio_open(struct inode *inode, struct file *filp) return 0; } -static int rp1_pio_release(struct inode *inode, struct file *filp) +static int rp1_pio_file_release(struct inode *inode, struct file *filp) { struct rp1_pio_client *client = filp->private_data; - pio_close(client); + rp1_pio_close(client); return 0; } @@ -1083,7 +1190,7 @@ static long rp1_pio_compat_ioctl(struct file *filp, unsigned int ioctl_num, param.dir = compat_param.dir; param.data_bytes = compat_param.data_bytes; param.data = compat_ptr(compat_param.data); - return rp1_pio_sm_xfer_data(client, ¶m); + return rp1_pio_sm_xfer_data_user(client, ¶m); } case PIO_IOC_SM_XFER_DATA32_COMPAT: { @@ -1096,7 +1203,7 @@ static long rp1_pio_compat_ioctl(struct file *filp, unsigned int ioctl_num, param.dir = compat_param.dir; param.data_bytes = compat_param.data_bytes; param.data = compat_ptr(compat_param.data); - return rp1_pio_sm_xfer_data32(client, ¶m); + return rp1_pio_sm_xfer_data32_user(client, ¶m); } case PIO_IOC_READ_HW_COMPAT: @@ -1125,8 +1232,8 @@ static long rp1_pio_compat_ioctl(struct file *filp, unsigned int ioctl_num, const struct file_operations rp1_pio_fops = { .owner = THIS_MODULE, - .open = rp1_pio_open, - .release = rp1_pio_release, + .open = rp1_pio_file_open, + .release = rp1_pio_file_release, .unlocked_ioctl = rp1_pio_ioctl, .compat_ioctl = rp1_pio_compat_ioctl, }; @@ -1153,6 +1260,10 @@ static int rp1_pio_probe(struct platform_device *pdev) return -EINVAL; } + pdev->id = of_alias_get_id(pdev->dev.of_node, "pio"); + if (pdev->id < 0) + return dev_err_probe(dev, pdev->id, "alias is missing\n"); + fw = devm_rp1_firmware_get(dev, dev->of_node); if (IS_ERR(fw)) return PTR_ERR(fw); @@ -1185,31 +1296,26 @@ static int rp1_pio_probe(struct platform_device *pdev) goto out_err; } - cdev_init(&pio->cdev, &rp1_pio_fops); - ret = cdev_add(&pio->cdev, pio->dev_num, 1); - if (ret) { - dev_err(dev, "cdev_add failed (err %d)\n", ret); - goto out_unregister; - } - pio->dev_class = class_create(DRIVER_NAME); if (IS_ERR(pio->dev_class)) { ret = PTR_ERR(pio->dev_class); dev_err(dev, "class_create failed (err %d)\n", ret); - goto out_cdev_del; + goto out_unregister; } - pdev->id = of_alias_get_id(pdev->dev.of_node, "pio"); - if (pdev->id < 0) { - dev_err(dev, "alias is missing\n"); - return -EINVAL; + + cdev_init(&pio->cdev, &rp1_pio_fops); + ret = cdev_add(&pio->cdev, pio->dev_num, 1); + if (ret) { + dev_err(dev, "cdev_add failed (err %d)\n", ret); goto out_class_destroy; } + sprintf(dev_name, "pio%d", pdev->id); cdev = device_create(pio->dev_class, NULL, pio->dev_num, NULL, dev_name); if (IS_ERR(cdev)) { ret = PTR_ERR(cdev); dev_err(dev, "%s: device_create failed (err %d)\n", __func__, ret); - goto out_class_destroy; + goto out_cdev_del; } g_pio = pio; @@ -1217,12 +1323,12 @@ static int rp1_pio_probe(struct platform_device *pdev) dev_info(dev, "Created instance as %s\n", dev_name); return 0; -out_class_destroy: - class_destroy(pio->dev_class); - out_cdev_del: cdev_del(&pio->cdev); +out_class_destroy: + class_destroy(pio->dev_class); + out_unregister: unregister_chrdev_region(pio->dev_num, 1); diff --git a/drivers/misc/ws2812-pio-rp1.c b/drivers/misc/ws2812-pio-rp1.c new file mode 100644 index 00000000000000..ee840c6b33122e --- /dev/null +++ b/drivers/misc/ws2812-pio-rp1.c @@ -0,0 +1,507 @@ +// SPDX-License-Identifier: GPL-2.0 +/* + * Raspberry Pi PIO-based WS2812 driver + * + * Copyright (C) 2014-2024 Raspberry Pi Ltd. + * + * Author: Phil Elwell (phil@raspberrypi.com) + * + * Based on the ws2812 driver by Gordon Hollingworth + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#define DRIVER_NAME "ws2812-pio-rp1" +#define MAX_INSTANCES 4 + +#define RESET_US 50 +#define PIXEL_BYTES 4 + +struct ws2812_pio_rp1_state { + struct device *dev; + struct gpio_desc *gpiod; + struct gpio_desc *power_gpiod; + uint gpio; + PIO pio; + uint sm; + uint offset; + + u8 *buffer; + u8 *pixbuf; + u32 pixbuf_size; + u32 write_end; + + u8 brightness; + u32 invert; + u32 num_leds; + u32 xfer_end_us; + bool is_rgbw; + struct delayed_work deferred_work; + + struct completion dma_completion; + struct cdev cdev; + dev_t dev_num; + const char *dev_name; +}; + +static DEFINE_MUTEX(ws2812_pio_mutex); +static DEFINE_IDA(ws2812_pio_ida); +static long ws2812_pio_ref_count; +static struct class *ws2812_pio_class; +static dev_t ws2812_pio_dev_num; +/* + * WS2812B gamma correction + * GammaE=255*(res/255).^(1/.45) + * From: http://rgb-123.com/ws2812-color-output/ + */ + +static const u8 ws2812_gamma[] = { + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 1, 1, 1, 1, 1, 1, 1, 2, 2, 2, + 2, 2, 2, 3, 3, 3, 3, 3, 4, 4, 4, 4, 5, 5, 5, 5, + 6, 6, 6, 7, 7, 7, 8, 8, 8, 9, 9, 9, 10, 10, 11, 11, + 11, 12, 12, 13, 13, 13, 14, 14, 15, 15, 16, 16, 17, 17, 18, 18, + 19, 19, 20, 21, 21, 22, 22, 23, 23, 24, 25, 25, 26, 27, 27, 28, + 29, 29, 30, 31, 31, 32, 33, 34, 34, 35, 36, 37, 37, 38, 39, 40, + 40, 41, 42, 43, 44, 45, 46, 46, 47, 48, 49, 50, 51, 52, 53, 54, + 55, 56, 57, 58, 59, 60, 61, 62, 63, 64, 65, 66, 67, 68, 69, 70, + 71, 72, 73, 74, 76, 77, 78, 79, 80, 81, 83, 84, 85, 86, 88, 89, + 90, 91, 93, 94, 95, 96, 98, 99, 100, 102, 103, 104, 106, 107, 109, 110, + 111, 113, 114, 116, 117, 119, 120, 121, 123, 124, 126, 128, 129, 131, 132, 134, + 135, 137, 138, 140, 142, 143, 145, 146, 148, 150, 151, 153, 155, 157, 158, 160, + 162, 163, 165, 167, 169, 170, 172, 174, 176, 178, 179, 181, 183, 185, 187, 189, + 191, 193, 194, 196, 198, 200, 202, 204, 206, 208, 210, 212, 214, 216, 218, 220, + 222, 224, 227, 229, 231, 233, 235, 237, 239, 241, 244, 246, 248, 250, 252, 255 +}; + +// ------ // +// ws2812 // +// ------ // + +#define ws2812_wrap_target 0 +#define ws2812_wrap 3 + +#define ws2812_T1 3 +#define ws2812_T2 4 +#define ws2812_T3 3 + +static const uint16_t ws2812_program_instructions[] = { + // .wrap_target + 0x6221, // 0: out x, 1 side 0 [2] + 0x1223, // 1: jmp !x, 3 side 1 [2] + 0x1300, // 2: jmp 0 side 1 [3] + 0xa342, // 3: nop side 0 [3] + // .wrap +}; + +static const struct pio_program ws2812_program = { + .instructions = ws2812_program_instructions, + .length = 4, + .origin = -1, +}; + +static inline pio_sm_config ws2812_program_get_default_config(uint offset) +{ + pio_sm_config c = pio_get_default_sm_config(); + + sm_config_set_wrap(&c, offset + ws2812_wrap_target, offset + ws2812_wrap); + sm_config_set_sideset(&c, 1, false, false); + return c; +} + +static inline void ws2812_program_init(PIO pio, uint sm, uint offset, uint pin, uint freq, + bool rgbw) +{ + int cycles_per_bit = ws2812_T1 + ws2812_T2 + ws2812_T3; + struct fp24_8 div; + pio_sm_config c; + + pio_gpio_init(pio, pin); + pio_sm_set_consecutive_pindirs(pio, sm, pin, 1, true); + c = ws2812_program_get_default_config(offset); + sm_config_set_sideset_pins(&c, pin); + sm_config_set_out_shift(&c, false, true, rgbw ? 32 : 24); + sm_config_set_fifo_join(&c, PIO_FIFO_JOIN_TX); + div = make_fp24_8(clock_get_hz(clk_sys), freq * cycles_per_bit); + sm_config_set_clkdiv(&c, div); + pio_sm_init(pio, sm, offset, &c); + pio_sm_set_enabled(pio, sm, true); +} + +static uint8_t ws2812_apply_gamma(uint8_t brightness, uint8_t val) +{ + int bright; + + if (!val) + return 0; + bright = (val * brightness) / 255; + return ws2812_gamma[bright]; +} + +static inline uint8_t *rgbw_u32(const struct ws2812_pio_rp1_state *state, + uint8_t r, uint8_t g, uint8_t b, uint8_t w, uint8_t *p) +{ + p[0] = ws2812_apply_gamma(state->brightness, w); + p[1] = ws2812_apply_gamma(state->brightness, b); + p[2] = ws2812_apply_gamma(state->brightness, r); + p[3] = ws2812_apply_gamma(state->brightness, g); + return p + 4; +} + +static void ws2812_dma_complete(void *param) +{ + struct ws2812_pio_rp1_state *state = param; + + complete(&state->dma_completion); +} + +static void ws2812_update_leds(struct ws2812_pio_rp1_state *state, uint length) +{ + init_completion(&state->dma_completion); + if (!pio_sm_xfer_data(state->pio, state->sm, PIO_DIR_TO_SM, length, state->buffer, 0, + (void (*)(void *))ws2812_dma_complete, state)) { + wait_for_completion(&state->dma_completion); + usleep_range(RESET_US, RESET_US + 100); + } +} + +static void ws2812_clear_leds(struct ws2812_pio_rp1_state *state) +{ + uint8_t *p_buffer; + uint length; + int i; + + p_buffer = state->buffer; + for (i = 0; i < state->num_leds; i++) + p_buffer = rgbw_u32(state, 0, 0, 0, 0, p_buffer); + + length = (void *)p_buffer - (void *)state->buffer; + + ws2812_update_leds(state, length); +} + +/* + * Function to write the RGB buffer to the WS2812 leds, the input buffer + * contains a sequence of up to num_leds RGB32 integers, these are then + * gamma-corrected before being sent to the PIO state machine. + */ + +static ssize_t ws2812_pio_rp1_write(struct file *filp, const char __user *buf, size_t count, + loff_t *ppos) +{ + struct ws2812_pio_rp1_state *state; + uint32_t pixbuf_size; + unsigned long delay; + loff_t pos = *ppos; + int err = 0; + + state = (struct ws2812_pio_rp1_state *)filp->private_data; + pixbuf_size = state->pixbuf_size; + + if (pos > pixbuf_size) + return -EFBIG; + + if (count > pixbuf_size) { + err = -EFBIG; + count = pixbuf_size; + } + + if (pos + count > pixbuf_size) { + if (!err) + err = -ENOSPC; + + count = pixbuf_size - pos; + } + + if (!pos && count == 1) { + if (copy_from_user(&state->brightness, buf, 1)) + return -EFAULT; + } else { + if (copy_from_user(state->pixbuf + pos, buf, count)) + return -EFAULT; + pos += count; + state->write_end = (u32)pos; + } + + *ppos = pos; + + delay = (state->write_end == pixbuf_size) ? 0 : HZ / 20; + schedule_delayed_work(&state->deferred_work, delay); + + return err ? err : count; +} + +static void ws2812_pio_rp1_deferred_work(struct work_struct *work) +{ + struct ws2812_pio_rp1_state *state = + container_of(work, struct ws2812_pio_rp1_state, deferred_work.work); + uint8_t *p_buffer; + uint32_t *p_rgb; + int blank_bytes; + uint length; + int i; + + blank_bytes = state->pixbuf_size - state->write_end; + if (blank_bytes > 0) + memset(state->pixbuf + state->write_end, 0, blank_bytes); + + p_rgb = (uint32_t *)state->pixbuf; + p_buffer = state->buffer; + + for (i = 0; i < state->num_leds; i++) { + uint32_t rgbw_pix = *(p_rgb++); + + p_buffer = rgbw_u32(state, + (uint8_t)(rgbw_pix >> 0), + (uint8_t)(rgbw_pix >> 8), + (uint8_t)(rgbw_pix >> 16), + (uint8_t)(rgbw_pix >> 24), + p_buffer); + } + + length = (void *)p_buffer - (void *)state->buffer; + + ws2812_update_leds(state, length); +} + +static int ws2812_pio_rp1_open(struct inode *inode, struct file *file) +{ + struct ws2812_pio_rp1_state *state; + + state = container_of(inode->i_cdev, struct ws2812_pio_rp1_state, cdev); + file->private_data = state; + + return 0; +} + +const struct file_operations ws2812_pio_rp1_fops = { + .owner = THIS_MODULE, + .write = ws2812_pio_rp1_write, + .open = ws2812_pio_rp1_open, +}; + +/* + * Probe function + */ +static int ws2812_pio_rp1_probe(struct platform_device *pdev) +{ + struct device_node *np = pdev->dev.of_node; + struct of_phandle_args of_args = { 0 }; + struct ws2812_pio_rp1_state *state; + struct device *dev = &pdev->dev; + struct device *char_dev; + const char *dev_name; + uint32_t brightness; + bool is_rp1; + int minor; + int ret; + + state = devm_kzalloc(dev, sizeof(*state), GFP_KERNEL); + if (IS_ERR(state)) + return PTR_ERR(state); + + state->dev = dev; + + platform_set_drvdata(pdev, state); + + ret = of_property_read_u32(np, "rpi,num-leds", &state->num_leds); + if (ret) + return dev_err_probe(dev, ret, "Could not get num-leds\n"); + + brightness = 255; + of_property_read_u32(np, "rpi,brightness", &brightness); + state->brightness = min(brightness, 255); + + state->pixbuf_size = state->num_leds * PIXEL_BYTES; + + state->is_rgbw = of_property_read_bool(np, "rpi,rgbw"); + state->gpiod = devm_gpiod_get(dev, "leds", GPIOD_ASIS); + if (IS_ERR(state->gpiod)) + return dev_err_probe(dev, PTR_ERR(state->gpiod), + "Could not get a gpio\n"); + + /* This must be an RP1 GPIO in the first bank, and retrieve the offset. */ + /* Unfortunately I think this has to be done by parsing the gpios property */ + + /* This really shouldn't fail, given that we have a gpiod */ + if (of_parse_phandle_with_args(np, "leds-gpios", "#gpio-cells", 0, &of_args)) + return dev_err_probe(dev, -EINVAL, + "Can't find gpio declaration\n"); + + is_rp1 = of_device_is_compatible(of_args.np, "raspberrypi,rp1-gpio"); + of_node_put(of_args.np); + if (!is_rp1 || of_args.args_count != 2) + return dev_err_probe(dev, -EINVAL, + "Not an RP1 gpio\n"); + + state->gpio = of_args.args[0]; + + state->pixbuf = devm_kmalloc(dev, state->pixbuf_size, GFP_KERNEL); + if (state->pixbuf == NULL) + return -ENOMEM; + + state->buffer = devm_kmalloc(dev, state->num_leds * PIXEL_BYTES, GFP_KERNEL); + if (state->buffer == NULL) + return -ENOMEM; + + ret = of_property_read_string(np, "dev-name", &dev_name); + if (ret) { + pr_err("Failed to read 'dev-name' property\n"); + return ret; + } + + state->pio = pio_open(); + if (IS_ERR(state->pio)) + return dev_err_probe(dev, PTR_ERR(state->pio), + "Could not open PIO\n"); + + state->sm = pio_claim_unused_sm(state->pio, false); + if ((int)state->sm < 0) { + dev_err(dev, "No free PIO SM\n"); + ret = -EBUSY; + goto fail_pio; + } + + state->offset = pio_add_program(state->pio, &ws2812_program); + if (state->offset == PIO_ORIGIN_ANY) { + dev_err(dev, "Not enough PIO program space\n"); + ret = -EBUSY; + goto fail_pio; + } + + pio_sm_config_xfer(state->pio, state->sm, PIO_DIR_TO_SM, state->num_leds * sizeof(int), 1); + + pio_sm_clear_fifos(state->pio, state->sm); + pio_sm_set_clkdiv(state->pio, state->sm, make_fp24_8(1, 1)); + ws2812_program_init(state->pio, state->sm, state->offset, state->gpio, 800000, + state->is_rgbw); + + mutex_lock(&ws2812_pio_mutex); + + if (!ws2812_pio_ref_count) { + ret = alloc_chrdev_region(&ws2812_pio_dev_num, 0, MAX_INSTANCES, DRIVER_NAME); + if (ret < 0) { + dev_err(dev, "alloc_chrdev_region failed (rc=%d)\n", ret); + goto fail_mutex; + } + + ws2812_pio_class = class_create(DRIVER_NAME); + if (IS_ERR(ws2812_pio_class)) { + pr_err("Unable to create class " DRIVER_NAME "\n"); + ret = PTR_ERR(ws2812_pio_class); + goto fail_chrdev; + } + } + + ws2812_pio_ref_count++; + + minor = ida_alloc_range(&ws2812_pio_ida, 0, MAX_INSTANCES - 1, GFP_KERNEL); + if (minor < 0) { + pr_err("No free instances\n"); + ret = minor; + goto fail_class; + + } + + mutex_unlock(&ws2812_pio_mutex); + + state->dev_num = MKDEV(MAJOR(ws2812_pio_dev_num), minor); + state->dev_name = devm_kasprintf(dev, GFP_KERNEL, dev_name, minor); + + char_dev = device_create(ws2812_pio_class, NULL, state->dev_num, NULL, state->dev_name); + + if (IS_ERR(char_dev)) { + pr_err("Unable to create device %s\n", state->dev_name); + ret = PTR_ERR(char_dev); + goto fail_ida; + } + + state->cdev.owner = THIS_MODULE; + cdev_init(&state->cdev, &ws2812_pio_rp1_fops); + + ret = cdev_add(&state->cdev, state->dev_num, 1); + if (ret) { + pr_err("cdev_add failed\n"); + goto fail_device; + } + + INIT_DELAYED_WORK(&state->deferred_work, ws2812_pio_rp1_deferred_work); + + ws2812_clear_leds(state); + + dev_info(&pdev->dev, "Instantiated %d LEDs on GPIO %d as /dev/%s\n", + state->num_leds, state->gpio, state->dev_name); + + return 0; + +fail_device: + device_destroy(ws2812_pio_class, state->dev_num); +fail_ida: + mutex_lock(&ws2812_pio_mutex); + ida_free(&ws2812_pio_ida, minor); +fail_class: + ws2812_pio_ref_count--; + if (ws2812_pio_ref_count) + goto fail_mutex; + class_destroy(ws2812_pio_class); +fail_chrdev: + unregister_chrdev_region(ws2812_pio_dev_num, MAX_INSTANCES); +fail_mutex: + mutex_unlock(&ws2812_pio_mutex); +fail_pio: + pio_close(state->pio); + + return ret; +} + +static void ws2812_pio_rp1_remove(struct platform_device *pdev) +{ + struct ws2812_pio_rp1_state *state = platform_get_drvdata(pdev); + + cancel_delayed_work(&state->deferred_work); + platform_set_drvdata(pdev, NULL); + + cdev_del(&state->cdev); + device_destroy(ws2812_pio_class, state->dev_num); + + mutex_lock(&ws2812_pio_mutex); + ida_free(&ws2812_pio_ida, MINOR(state->dev_num)); + ws2812_pio_ref_count--; + if (!ws2812_pio_ref_count) { + class_destroy(ws2812_pio_class); + unregister_chrdev_region(ws2812_pio_dev_num, MAX_INSTANCES); + } + mutex_unlock(&ws2812_pio_mutex); + + pio_close(state->pio); +} + +static const struct of_device_id ws2812_pio_rp1_match[] = { + { .compatible = "raspberrypi,ws2812-pio-rp1" }, + { } +}; +MODULE_DEVICE_TABLE(of, ws2812_pio_rp1_match); + +static struct platform_driver ws2812_pio_rp1_driver = { + .driver = { + .name = "ws2812-pio-rp1", + .of_match_table = ws2812_pio_rp1_match, + }, + .probe = ws2812_pio_rp1_probe, + .remove_new = ws2812_pio_rp1_remove, +}; +module_platform_driver(ws2812_pio_rp1_driver); + +MODULE_DESCRIPTION("WS2812 PIO RP1 driver"); +MODULE_AUTHOR("Phil Elwell"); +MODULE_LICENSE("GPL"); diff --git a/include/linux/pio_rp1.h b/include/linux/pio_rp1.h index 0e5cc97a6eb7d1..f262fdd9c8f1c3 100644 --- a/include/linux/pio_rp1.h +++ b/include/linux/pio_rp1.h @@ -171,13 +171,25 @@ enum gpio_drive_strength { GPIO_DRIVE_STRENGTH_12MA = 3 }; +struct fp24_8 { + uint32_t val; +}; + typedef rp1_pio_sm_config pio_sm_config; typedef struct rp1_pio_client *PIO; -void pio_set_error(struct rp1_pio_client *client, int err); -int pio_get_error(const struct rp1_pio_client *client); -void pio_clear_error(struct rp1_pio_client *client); +int rp1_pio_init(void); +PIO rp1_pio_open(void); +void rp1_pio_close(struct rp1_pio_client *client); +void rp1_pio_set_error(struct rp1_pio_client *client, int err); +int rp1_pio_get_error(const struct rp1_pio_client *client); +void rp1_pio_clear_error(struct rp1_pio_client *client); +int rp1_pio_sm_config_xfer(struct rp1_pio_client *client, uint sm, uint dir, + uint buf_size, uint buf_count); +int rp1_pio_sm_xfer_data(struct rp1_pio_client *client, uint sm, uint dir, + uint data_bytes, void *data, dma_addr_t dma_addr, + void (*callback)(void *param), void *param); int rp1_pio_can_add_program(struct rp1_pio_client *client, void *param); int rp1_pio_add_program(struct rp1_pio_client *client, void *param); @@ -211,12 +223,55 @@ int rp1_pio_gpio_set_oeover(struct rp1_pio_client *client, void *param); int rp1_pio_gpio_set_input_enabled(struct rp1_pio_client *client, void *param); int rp1_pio_gpio_set_drive_strength(struct rp1_pio_client *client, void *param); -int pio_init(void); -PIO pio_open(void); -void pio_close(PIO pio); +static inline int pio_init(void) +{ + return rp1_pio_init(); +} + +static inline struct rp1_pio_client *pio_open(void) +{ + return rp1_pio_open(); +} -int pio_sm_config_xfer(PIO pio, uint sm, uint dir, uint buf_size, uint buf_count); -int pio_sm_xfer_data(PIO pio, uint sm, uint dir, uint data_bytes, void *data); +static inline void pio_close(struct rp1_pio_client *client) +{ + rp1_pio_close(client); +} + +static inline void pio_set_error(struct rp1_pio_client *client, int err) +{ + rp1_pio_set_error(client, err); +} + +static inline int pio_get_error(const struct rp1_pio_client *client) +{ + return rp1_pio_get_error(client); +} + +static inline void pio_clear_error(struct rp1_pio_client *client) +{ + rp1_pio_clear_error(client); +} + +static inline int pio_sm_config_xfer(struct rp1_pio_client *client, uint sm, uint dir, + uint buf_size, uint buf_count) +{ + return rp1_pio_sm_config_xfer(client, sm, dir, buf_size, buf_count); +} + +static inline int pio_sm_xfer_data(struct rp1_pio_client *client, uint sm, uint dir, + uint data_bytes, void *data, dma_addr_t dma_addr, + void (*callback)(void *param), void *param) +{ + return rp1_pio_sm_xfer_data(client, sm, dir, data_bytes, data, dma_addr, callback, param); +} + +static inline struct fp24_8 make_fp24_8(uint mul, uint div) +{ + struct fp24_8 res = { .val = ((unsigned long long)mul << 8) / div }; + + return res; +} static inline bool pio_can_add_program(struct rp1_pio_client *client, const pio_program_t *program) @@ -396,16 +451,18 @@ static inline int pio_sm_clear_fifos(struct rp1_pio_client *client, uint sm) return rp1_pio_sm_clear_fifos(client, &args); } -static inline bool pio_calculate_clkdiv_from_float(float div, uint16_t *div_int, +static inline bool pio_calculate_clkdiv_from_fp24_8(struct fp24_8 div, uint16_t *div_int, uint8_t *div_frac) { - if (bad_params_if(NULL, div < 1 || div > 65536)) + uint inum = (div.val >> 8); + + if (bad_params_if(NULL, inum < 1 || inum > 65536)) return false; - *div_int = (uint16_t)div; + *div_int = (uint16_t)inum; if (*div_int == 0) *div_frac = 0; else - *div_frac = (uint8_t)((div - (float)*div_int) * (1u << 8u)); + *div_frac = div.val & 0xff; return true; } @@ -421,11 +478,11 @@ static inline int pio_sm_set_clkdiv_int_frac(struct rp1_pio_client *client, uint return rp1_pio_sm_set_clkdiv(client, &args); } -static inline int pio_sm_set_clkdiv(struct rp1_pio_client *client, uint sm, float div) +static inline int pio_sm_set_clkdiv(struct rp1_pio_client *client, uint sm, struct fp24_8 div) { struct rp1_pio_sm_set_clkdiv_args args = { .sm = sm }; - if (!pio_calculate_clkdiv_from_float(div, &args.div_int, &args.div_frac)) + if (!pio_calculate_clkdiv_from_fp24_8(div, &args.div_int, &args.div_frac)) return -EINVAL; return rp1_pio_sm_set_clkdiv(client, &args); } @@ -745,12 +802,12 @@ static inline void sm_config_set_clkdiv_int_frac(pio_sm_config *c, uint16_t div_ (((uint)div_int) << PROC_PIO_SM0_CLKDIV_INT_LSB); } -static inline void sm_config_set_clkdiv(pio_sm_config *c, float div) +static inline void sm_config_set_clkdiv(pio_sm_config *c, struct fp24_8 div) { uint16_t div_int; uint8_t div_frac; - pio_calculate_clkdiv_from_float(div, &div_int, &div_frac); + pio_calculate_clkdiv_from_fp24_8(div, &div_int, &div_frac); sm_config_set_clkdiv_int_frac(c, div_int, div_frac); }