Signed-off-by: Ayush Singh ayushdevel1325@gmail.com --- .../greybus/beagleplay-greybus-driver.c | 264 ++++++++++++++++++ .../greybus/beagleplay-greybus-driver.h | 28 ++ 2 files changed, 292 insertions(+) create mode 100644 drivers/staging/greybus/beagleplay-greybus-driver.c create mode 100644 drivers/staging/greybus/beagleplay-greybus-driver.h
diff --git a/drivers/staging/greybus/beagleplay-greybus-driver.c b/drivers/staging/greybus/beagleplay-greybus-driver.c new file mode 100644 index 000000000000..1b4225a9fc8d --- /dev/null +++ b/drivers/staging/greybus/beagleplay-greybus-driver.c @@ -0,0 +1,264 @@ +// SPDX-License-Identifier: GPL-2.0 +/* + * Beagleplay Linux Driver for Greybus + * + * Copyright (c) 2023 Ayush Singh ayushdevel1325@gmail.com + */ + +#include "beagleplay-greybus-driver.h" +#include "hdlc.h" +#include <linux/crc-ccitt.h> +#include <linux/device.h> +#include <linux/gfp.h> +#include <linux/greybus.h> +#include <linux/module.h> +#include <linux/of.h> +#include <linux/printk.h> +#include <linux/serdev.h> +#include <linux/tty.h> +#include <linux/tty_driver.h> + +#define BEAGLEPLAY_GREYBUS_DRV_VERSION "0.1.0" +#define BEAGLEPLAY_GREYBUS_DRV_NAME "bcfgreybus" + +#define BEAGLEPLAY_GB_MAX_CPORTS 32 + +static const struct of_device_id beagleplay_greybus_of_match[] = { + { + .compatible = "beagle,beagleplaygreybus", + }, + {}, +}; +MODULE_DEVICE_TABLE(of, beagleplay_greybus_of_match); + +static int beagleplay_greybus_serdev_write_locked(void *drvdata, + const unsigned char *buf, + size_t buf_len) +{ + struct beagleplay_greybus *beagleplay_greybus; + + beagleplay_greybus = drvdata; + return serdev_device_write_buf(beagleplay_greybus->serdev, buf, + buf_len); +} + +static void +beagleplay_greybus_handle_greybus_frame(struct beagleplay_greybus *bg, + u8 *buffer, size_t buffer_len) +{ + u16 cport_id; + struct gb_operation_msg_hdr *hdr = + (struct gb_operation_msg_hdr *)buffer; + memcpy(&cport_id, hdr->pad, sizeof(u16)); + if (hdr->result) { + dev_warn( + &bg->serdev->dev, + "Failed Greybus Operation %u of Type %X on CPort %u with Status %u", + hdr->operation_id, hdr->type, cport_id, hdr->result); + } else { + dev_dbg(&bg->serdev->dev, + "Successful Greybus Operation %u of Type %X on CPort %u", + hdr->operation_id, hdr->type, cport_id); + } + greybus_data_rcvd(bg->gb_host_device, cport_id, buffer, buffer_len); +} + +static void beagleplay_greybus_handle_dbg_frame(struct beagleplay_greybus *bg, + const char *buffer, + size_t buffer_len) +{ + char buf[RX_HDLC_PAYLOAD]; + + memcpy(buf, buffer, buffer_len); + buf[buffer_len] = 0; + dev_dbg(&bg->serdev->dev, "CC1352 Debug: %s", buf); +} + +static void beagleplay_greybus_handle_hdlc_rx_frame(void *drv_data, u8 *buffer, + size_t buffer_len, + uint8_t address) +{ + struct beagleplay_greybus *beagleplay_greybus; + + beagleplay_greybus = drv_data; + + switch (address) { + case ADDRESS_DBG: + beagleplay_greybus_handle_dbg_frame(beagleplay_greybus, buffer, + buffer_len); + break; + case ADDRESS_GREYBUS: + beagleplay_greybus_handle_greybus_frame(beagleplay_greybus, + buffer, buffer_len); + break; + default: + dev_warn(&beagleplay_greybus->serdev->dev, + "Got Unknown Frame %u", address); + } +} + +static int beagleplay_greybus_tty_receive(struct serdev_device *serdev, + const unsigned char *data, + size_t count) +{ + struct beagleplay_greybus *beagleplay_greybus; + + beagleplay_greybus = serdev_device_get_drvdata(serdev); + return hdlc_rx(beagleplay_greybus->hdlc_drv, data, count); +} + +static void beagleplay_greybus_tty_wakeup(struct serdev_device *serdev) +{ + struct beagleplay_greybus *beagleplay_greybus; + + beagleplay_greybus = serdev_device_get_drvdata(serdev); + hdlc_tx_wakeup(beagleplay_greybus->hdlc_drv); +} + +static struct serdev_device_ops beagleplay_greybus_ops = { + .receive_buf = beagleplay_greybus_tty_receive, + .write_wakeup = beagleplay_greybus_tty_wakeup, +}; + +static int gb_message_send(struct gb_host_device *hd, u16 cport_id, + struct gb_message *message, gfp_t gfp_mask) +{ + struct beagleplay_greybus *beagleplay_greybus; + char block_payload[HDLC_MAX_BLOCK_SIZE]; + + dev_dbg(&hd->dev, + "Sending Greybus message with Operation %u, Type: %X on Cport %u", + message->header->operation_id, message->header->type, cport_id); + + if (message->header->size > HDLC_MAX_BLOCK_SIZE) { + dev_err(&hd->dev, "Greybus message too big"); + return -1; + } + + beagleplay_greybus = dev_get_drvdata(&hd->dev); + memcpy(message->header->pad, &cport_id, sizeof(u16)); + memcpy(&block_payload, message->header, + sizeof(struct gb_operation_msg_hdr)); + memcpy(&block_payload[sizeof(struct gb_operation_msg_hdr)], + message->payload, message->payload_size); + hdlc_send_async(beagleplay_greybus->hdlc_drv, message->header->size, + &block_payload, ADDRESS_GREYBUS, 0x03); + + greybus_message_sent(beagleplay_greybus->gb_host_device, message, 0); + return 0; +} + +static void gb_message_cancel(struct gb_message *message) +{ +} + +static struct gb_hd_driver gb_hdlc_driver = { .message_send = gb_message_send, + .message_cancel = + gb_message_cancel }; + +static void beagleplay_greybus_start_svc(struct beagleplay_greybus *bg) +{ + const u8 command[1] = { CONTROL_SVC_START }; + + hdlc_send_async(bg->hdlc_drv, sizeof(command), command, ADDRESS_CONTROL, + 0x03); +} + +static void beagleplay_greybus_stop_svc(struct beagleplay_greybus *bg) +{ + const u8 command = CONTROL_SVC_STOP; + + hdlc_send_async(bg->hdlc_drv, 1, &command, ADDRESS_CONTROL, 0x03); +} + +static int beagleplay_greybus_probe(struct serdev_device *serdev) +{ + struct beagleplay_greybus *bg; + u32 speed = 115200; + int ret = 0; + + bg = devm_kmalloc(&serdev->dev, sizeof(struct beagleplay_greybus), + GFP_KERNEL); + + bg->serdev = serdev; + + serdev_device_set_drvdata(serdev, bg); + serdev_device_set_client_ops(serdev, &beagleplay_greybus_ops); + + ret = serdev_device_open(serdev); + if (ret) { + dev_err(&bg->serdev->dev, "Unable to Open Device"); + return ret; + } + + speed = serdev_device_set_baudrate(serdev, speed); + dev_info(&bg->serdev->dev, "Using baudrate %u\n", speed); + + serdev_device_set_flow_control(serdev, false); + + // Init HDLC + bg->hdlc_drv = + hdlc_init(&serdev->dev, beagleplay_greybus_serdev_write_locked, + beagleplay_greybus_handle_hdlc_rx_frame); + if (!bg->hdlc_drv) { + dev_err(&serdev->dev, "Failed to initialize HDLC"); + return -ENOMEM; + } + + // Greybus setup + bg->gb_host_device = + gb_hd_create(&gb_hdlc_driver, &serdev->dev, TX_CIRC_BUF_SIZE, + BEAGLEPLAY_GB_MAX_CPORTS); + if (IS_ERR(bg->gb_host_device)) { + dev_err(&bg->serdev->dev, + "Unable to create Greybus Host Device"); + return -1; + } + ret = gb_hd_add(bg->gb_host_device); + if (ret) { + dev_err(&serdev->dev, "Failed to add Greybus Host Device"); + return ret; + } + dev_set_drvdata(&bg->gb_host_device->dev, bg); + + beagleplay_greybus_start_svc(bg); + + dev_info(&bg->serdev->dev, "Successful Beagleplay Greybus Probe"); + + return 0; +} + +static void beagleplay_greybus_remove(struct serdev_device *serdev) +{ + struct beagleplay_greybus *beagleplay_greybus = + serdev_device_get_drvdata(serdev); + + dev_info(&beagleplay_greybus->serdev->dev, + "Remove BeaglePlay Greybus Driver"); + + // Free greybus stuff + gb_hd_del(beagleplay_greybus->gb_host_device); + gb_hd_put(beagleplay_greybus->gb_host_device); + + beagleplay_greybus_stop_svc(beagleplay_greybus); + + hdlc_deinit(beagleplay_greybus->hdlc_drv); + + serdev_device_close(serdev); +} + +static struct serdev_device_driver beagleplay_greybus_driver = { + .probe = beagleplay_greybus_probe, + .remove = beagleplay_greybus_remove, + .driver = { + .name = BEAGLEPLAY_GREYBUS_DRV_NAME, + .of_match_table = of_match_ptr(beagleplay_greybus_of_match), + }, +}; + +module_serdev_device_driver(beagleplay_greybus_driver); + +MODULE_LICENSE("GPL v2"); +MODULE_AUTHOR("Ayush Singh ayushdevel1325@gmail.com"); +MODULE_DESCRIPTION("A Greybus driver for BeaglePlay"); +MODULE_VERSION(BEAGLEPLAY_GREYBUS_DRV_VERSION); diff --git a/drivers/staging/greybus/beagleplay-greybus-driver.h b/drivers/staging/greybus/beagleplay-greybus-driver.h new file mode 100644 index 000000000000..47b601b7fff4 --- /dev/null +++ b/drivers/staging/greybus/beagleplay-greybus-driver.h @@ -0,0 +1,28 @@ +/* SPDX-License-Identifier: GPL-2.0 + * + * Copyright (c) 2023 Ayush Singh ayushdevel1325@gmail.com + */ + +#ifndef BEAGLEPLAY_GREBUBS_DRIVER_H +#define BEAGLEPLAY_GREBUBS_DRIVER_H + +#include "hdlc.h" +#include <linux/greybus/hd.h> +#include <linux/init.h> + +#define ADDRESS_GREYBUS 0x01 +#define ADDRESS_DBG 0x02 +#define ADDRESS_CONTROL 0x04 + +#define CONTROL_SVC_START 0x01 +#define CONTROL_SVC_STOP 0x02 + +struct beagleplay_greybus { + struct serdev_device *serdev; + + struct hdlc_driver *hdlc_drv; + + struct gb_host_device *gb_host_device; +}; + +#endif