Add netdev ops for rpmsg-eth driver. This patch introduces the netdev
operations for the rpmsg-eth driver, enabling the driver to interact
with the Linux networking stack. The following functionalities are
implemented:
1. `ndo_open` and `ndo_stop`:
- Handles the initialization and cleanup of the network device
during open and stop operations.
- Manages the state transitions of the rpmsg-eth driver.
2. `ndo_start_xmit`:
- Implements the transmit functionality by copying data from the
skb to the shared memory buffer and updating the head index.
3. `ndo_set_mac_address`:
- Allows setting the MAC address of the network device and sends
the updated MAC address to the remote processor.
4. RX Path:
- Adds a timer-based mechanism to poll for received packets in
shared memory.
- Implements NAPI-based packet processing to handle received
packets efficiently.
5. State Machine:
- Introduces a state machine to manage the driver's state
transitions, such as PROBE, OPEN, READY, and RUNNING.
6. Initialization:
- Adds necessary initialization for locks, timers, and work
structures.
- Registers the network device and sets up NAPI and RX timer.
7. Cleanup:
- Ensures proper cleanup of resources during driver removal,
including NAPI and timers.
This patch enhances the rpmsg-eth driver to function as a fully
operational network device in the Linux kernel.
Signed-off-by: MD Danish Anwar <[email protected]>
---
drivers/net/ethernet/rpmsg_eth.c | 318 +++++++++++++++++++++++++++++++
drivers/net/ethernet/rpmsg_eth.h | 2 +
2 files changed, 320 insertions(+)
diff --git a/drivers/net/ethernet/rpmsg_eth.c b/drivers/net/ethernet/rpmsg_eth.c
index b6fe5628933d..a2248e21cf22 100644
--- a/drivers/net/ethernet/rpmsg_eth.c
+++ b/drivers/net/ethernet/rpmsg_eth.c
@@ -49,6 +49,109 @@ static int rpmsg_eth_validate_handshake(struct
rpmsg_eth_port *port,
return 0;
}
+static int create_request(struct rpmsg_eth_common *common,
+ enum rpmsg_eth_rpmsg_type rpmsg_type)
+{
+ struct message *msg = &common->send_msg;
+ int ret = 0;
+
+ msg->msg_hdr.src_id = common->port->port_id;
+ msg->req_msg.type = rpmsg_type;
+
+ switch (rpmsg_type) {
+ case RPMSG_ETH_REQ_SHM_INFO:
+ msg->msg_hdr.msg_type = RPMSG_ETH_REQUEST_MSG;
+ break;
+ case RPMSG_ETH_REQ_SET_MAC_ADDR:
+ msg->msg_hdr.msg_type = RPMSG_ETH_REQUEST_MSG;
+ ether_addr_copy(msg->req_msg.mac_addr.addr,
+ common->port->ndev->dev_addr);
+ break;
+ case RPMSG_ETH_NOTIFY_PORT_UP:
+ case RPMSG_ETH_NOTIFY_PORT_DOWN:
+ msg->msg_hdr.msg_type = RPMSG_ETH_NOTIFY_MSG;
+ break;
+ default:
+ ret = -EINVAL;
+ dev_err(common->dev, "Invalid RPMSG request\n");
+ }
+ return ret;
+}
+
+static int rpmsg_eth_create_send_request(struct rpmsg_eth_common *common,
+ enum rpmsg_eth_rpmsg_type rpmsg_type,
+ bool wait)
+{
+ unsigned long flags;
+ int ret = 0;
+
+ if (wait)
+ reinit_completion(&common->sync_msg);
+
+ spin_lock_irqsave(&common->send_msg_lock, flags);
+ ret = create_request(common, rpmsg_type);
+ if (ret)
+ goto release_lock;
+
+ ret = rpmsg_send(common->rpdev->ept, (void *)(&common->send_msg),
+ sizeof(common->send_msg));
+ if (ret) {
+ dev_err(common->dev, "Failed to send RPMSG message\n");
+ goto release_lock;
+ }
+
+ spin_unlock_irqrestore(&common->send_msg_lock, flags);
+ if (wait) {
+ ret = wait_for_completion_timeout(&common->sync_msg,
+
RPMSG_ETH_REQ_TIMEOUT_JIFFIES);
+
+ if (!ret) {
+ dev_err(common->dev, "Failed to receive response within
%ld jiffies\n",
+ RPMSG_ETH_REQ_TIMEOUT_JIFFIES);
+ return -ETIMEDOUT;
+ }
+ ret = 0;
+ }
+ return ret;
+release_lock:
+ spin_unlock_irqrestore(&common->send_msg_lock, flags);
+ return ret;
+}
+
+static void rpmsg_eth_state_machine(struct work_struct *work)
+{
+ struct delayed_work *dwork = to_delayed_work(work);
+ struct rpmsg_eth_common *common;
+ struct rpmsg_eth_port *port;
+ int ret;
+
+ common = container_of(dwork, struct rpmsg_eth_common, state_work);
+ port = common->port;
+
+ mutex_lock(&common->state_lock);
+
+ switch (common->state) {
+ case RPMSG_ETH_STATE_PROBE:
+ break;
+ case RPMSG_ETH_STATE_OPEN:
+ rpmsg_eth_create_send_request(common, RPMSG_ETH_REQ_SHM_INFO,
false);
+ break;
+ case RPMSG_ETH_STATE_CLOSE:
+ break;
+ case RPMSG_ETH_STATE_READY:
+ ret = rpmsg_eth_create_send_request(common,
RPMSG_ETH_REQ_SET_MAC_ADDR, false);
+ if (!ret) {
+ napi_enable(&port->rx_napi);
+ netif_carrier_on(port->ndev);
+ mod_timer(&port->rx_timer, RX_POLL_TIMEOUT_JIFFIES);
+ }
+ break;
+ case RPMSG_ETH_STATE_RUNNING:
+ break;
+ }
+ mutex_unlock(&common->state_lock);
+}
+
static int rpmsg_eth_rpmsg_cb(struct rpmsg_device *rpdev, void *data, int len,
void *priv, u32 src)
{
@@ -85,6 +188,17 @@ static int rpmsg_eth_rpmsg_cb(struct rpmsg_device *rpdev,
void *data, int len,
dev_err(common->dev, "RPMSG handshake failed
%d\n", ret);
return ret;
}
+
+ mutex_lock(&common->state_lock);
+ common->state = RPMSG_ETH_STATE_READY;
+ mutex_unlock(&common->state_lock);
+
+ mod_delayed_work(system_wq,
+ &common->state_work,
+ STATE_MACHINE_TIME_JIFFIES);
+
+ break;
+ case RPMSG_ETH_RESP_SET_MAC_ADDR:
break;
}
break;
@@ -92,6 +206,20 @@ static int rpmsg_eth_rpmsg_cb(struct rpmsg_device *rpdev,
void *data, int len,
rpmsg_type = msg->notify_msg.type;
dev_dbg(common->dev, "Msg type = %d, RPMsg type = %d, Src Id =
%d, Msg Id = %d\n",
msg_type, rpmsg_type, msg->msg_hdr.src_id,
msg->notify_msg.id);
+ switch (rpmsg_type) {
+ case RPMSG_ETH_NOTIFY_REMOTE_READY:
+ mutex_lock(&common->state_lock);
+ common->state = RPMSG_ETH_STATE_RUNNING;
+ mutex_unlock(&common->state_lock);
+
+ mod_delayed_work(system_wq,
+ &common->state_work,
+ STATE_MACHINE_TIME_JIFFIES);
+ break;
+ case RPMSG_ETH_NOTIFY_PORT_UP:
+ case RPMSG_ETH_NOTIFY_PORT_DOWN:
+ break;
+ }
break;
default:
dev_err(common->dev, "Invalid msg type\n");
@@ -163,6 +291,181 @@ static int rpmsg_eth_get_shm_info(struct rpmsg_eth_common
*common)
return 0;
}
+static void rpmsg_eth_rx_timer(struct timer_list *timer)
+{
+ struct rpmsg_eth_port *port = timer_container_of(port, timer, rx_timer);
+ struct napi_struct *napi;
+ int num_pkts = 0;
+ u32 head, tail;
+
+ head = readl(port->shm + port->rx_offset + HEAD_IDX_OFFSET);
+ tail = readl(port->shm + port->rx_offset +
+ TAIL_IDX_OFFSET(port->rx_max_buffers));
+
+ num_pkts = tail - head;
+ num_pkts = num_pkts >= 0 ? num_pkts :
+ (num_pkts + port->rx_max_buffers);
+
+ napi = &port->rx_napi;
+ if (num_pkts && likely(napi_schedule_prep(napi)))
+ __napi_schedule(napi);
+ else
+ mod_timer(&port->rx_timer, RX_POLL_JIFFIES);
+}
+
+static int rpmsg_eth_rx_packets(struct napi_struct *napi, int budget)
+{
+ struct rpmsg_eth_port *port = container_of(napi, struct rpmsg_eth_port,
rx_napi);
+ u32 count, process_pkts;
+ struct sk_buff *skb;
+ u32 head, tail;
+ int num_pkts;
+ u32 pkt_len;
+
+ head = readl(port->shm + port->rx_offset + HEAD_IDX_OFFSET);
+ tail = readl(port->shm + port->rx_offset +
+ TAIL_IDX_OFFSET(port->rx_max_buffers));
+
+ num_pkts = head - tail;
+
+ num_pkts = num_pkts >= 0 ? num_pkts :
+ (num_pkts + port->rx_max_buffers);
+ process_pkts = min(num_pkts, budget);
+ count = 0;
+ while (count < process_pkts) {
+ memcpy_fromio((void *)&pkt_len,
+ port->shm + port->rx_offset + MAGIC_NUM_SIZE_TYPE
+
+ PKT_START_OFFSET((tail + count) %
port->rx_max_buffers),
+ PKT_LEN_SIZE_TYPE);
+ /* Start building the skb */
+ skb = napi_alloc_skb(napi, pkt_len);
+ if (!skb) {
+ port->ndev->stats.rx_dropped++;
+ goto rx_dropped;
+ }
+
+ skb->dev = port->ndev;
+ skb_put(skb, pkt_len);
+ memcpy_fromio((void *)skb->data,
+ port->shm + port->rx_offset + PKT_LEN_SIZE_TYPE +
+ MAGIC_NUM_SIZE_TYPE +
+ PKT_START_OFFSET((tail + count) %
port->rx_max_buffers),
+ pkt_len);
+
+ skb->protocol = eth_type_trans(skb, port->ndev);
+
+ /* Push skb into network stack */
+ napi_gro_receive(napi, skb);
+
+ count++;
+ port->ndev->stats.rx_packets++;
+ port->ndev->stats.rx_bytes += skb->len;
+ }
+
+rx_dropped:
+
+ if (num_pkts) {
+ writel((tail + count) % port->rx_max_buffers,
+ port->shm + port->rx_offset +
+ TAIL_IDX_OFFSET(port->rx_max_buffers));
+
+ if (num_pkts < budget && napi_complete_done(napi, count))
+ mod_timer(&port->rx_timer, RX_POLL_TIMEOUT_JIFFIES);
+ }
+
+ return count;
+}
+
+static int rpmsg_eth_ndo_open(struct net_device *ndev)
+{
+ struct rpmsg_eth_common *common = rpmsg_eth_ndev_to_common(ndev);
+
+ mutex_lock(&common->state_lock);
+ common->state = RPMSG_ETH_STATE_OPEN;
+ mutex_unlock(&common->state_lock);
+ mod_delayed_work(system_wq, &common->state_work, msecs_to_jiffies(100));
+
+ return 0;
+}
+
+static int rpmsg_eth_ndo_stop(struct net_device *ndev)
+{
+ struct rpmsg_eth_common *common = rpmsg_eth_ndev_to_common(ndev);
+ struct rpmsg_eth_port *port = rpmsg_eth_ndev_to_port(ndev);
+
+ mutex_lock(&common->state_lock);
+ common->state = RPMSG_ETH_STATE_CLOSE;
+ mutex_unlock(&common->state_lock);
+
+ netif_carrier_off(port->ndev);
+
+ cancel_delayed_work_sync(&common->state_work);
+ timer_delete_sync(&port->rx_timer);
+ napi_disable(&port->rx_napi);
+
+ return 0;
+}
+
+static netdev_tx_t rpmsg_eth_start_xmit(struct sk_buff *skb, struct net_device
*ndev)
+{
+ struct rpmsg_eth_port *port = rpmsg_eth_ndev_to_port(ndev);
+ u32 head, tail;
+ int num_pkts;
+ u32 len;
+
+ len = skb_headlen(skb);
+ head = readl(port->shm + port->tx_offset + HEAD_IDX_OFFSET);
+ tail = readl(port->shm + port->tx_offset +
+ TAIL_IDX_OFFSET(port->tx_max_buffers));
+
+ /* If the buffer queue is full, then drop packet */
+ num_pkts = head - tail;
+ num_pkts = num_pkts >= 0 ? num_pkts :
+ (num_pkts + port->tx_max_buffers);
+
+ if ((num_pkts + 1) == port->tx_max_buffers) {
+ netdev_warn(ndev, "Tx buffer full %d\n", num_pkts);
+ goto ring_full;
+ }
+ /* Copy length */
+ memcpy_toio(port->shm + port->tx_offset + PKT_START_OFFSET(head) +
MAGIC_NUM_SIZE_TYPE,
+ (void *)&len, PKT_LEN_SIZE_TYPE);
+ /* Copy data to shared mem */
+ memcpy_toio(port->shm + port->tx_offset + PKT_START_OFFSET(head) +
MAGIC_NUM_SIZE_TYPE +
+ PKT_LEN_SIZE_TYPE, (void *)skb->data, len);
+ writel((head + 1) % port->tx_max_buffers,
+ port->shm + port->tx_offset + HEAD_IDX_OFFSET);
+
+ ndev->stats.tx_packets++;
+ ndev->stats.tx_bytes += skb->len;
+
+ dev_consume_skb_any(skb);
+ return NETDEV_TX_OK;
+
+ring_full:
+ return NETDEV_TX_BUSY;
+}
+
+static int rpmsg_eth_set_mac_address(struct net_device *ndev, void *addr)
+{
+ struct rpmsg_eth_common *common = rpmsg_eth_ndev_to_common(ndev);
+ int ret;
+
+ ret = eth_mac_addr(ndev, addr);
+
+ if (ret < 0)
+ return ret;
+ ret = rpmsg_eth_create_send_request(common, RPMSG_ETH_REQ_SET_MAC_ADDR,
false);
+ return ret;
+}
+
+static const struct net_device_ops rpmsg_eth_netdev_ops = {
+ .ndo_open = rpmsg_eth_ndo_open,
+ .ndo_stop = rpmsg_eth_ndo_stop,
+ .ndo_start_xmit = rpmsg_eth_start_xmit,
+ .ndo_set_mac_address = rpmsg_eth_set_mac_address,
+};
+
static int rpmsg_eth_init_ndev(struct rpmsg_eth_common *common)
{
struct device *dev = &common->rpdev->dev;
@@ -186,6 +489,7 @@ static int rpmsg_eth_init_ndev(struct rpmsg_eth_common
*common)
ndev_priv = netdev_priv(port->ndev);
ndev_priv->port = port;
+ port->ndev->netdev_ops = &rpmsg_eth_netdev_ops;
SET_NETDEV_DEV(port->ndev, dev);
port->ndev->min_mtu = RPMSG_ETH_MIN_PACKET_SIZE;
@@ -197,6 +501,8 @@ static int rpmsg_eth_init_ndev(struct rpmsg_eth_common
*common)
}
netif_carrier_off(port->ndev);
+ netif_napi_add(port->ndev, &port->rx_napi, rpmsg_eth_rx_packets);
+ timer_setup(&port->rx_timer, rpmsg_eth_rx_timer, 0);
err = register_netdev(port->ndev);
if (err)
dev_err(dev, "error registering rpmsg_eth net device %d\n",
err);
@@ -225,6 +531,12 @@ static int rpmsg_eth_probe(struct rpmsg_device *rpdev)
if (ret)
return ret;
+ spin_lock_init(&common->send_msg_lock);
+ spin_lock_init(&common->recv_msg_lock);
+ mutex_init(&common->state_lock);
+ INIT_DELAYED_WORK(&common->state_work, rpmsg_eth_state_machine);
+ init_completion(&common->sync_msg);
+
/* Register the network device */
ret = rpmsg_eth_init_ndev(common);
if (ret)
@@ -235,6 +547,12 @@ static int rpmsg_eth_probe(struct rpmsg_device *rpdev)
static void rpmsg_eth_remove(struct rpmsg_device *rpdev)
{
+ struct rpmsg_eth_common *common = dev_get_drvdata(&rpdev->dev);
+ struct rpmsg_eth_port *port = common->port;
+
+ netif_napi_del(&port->rx_napi);
+ timer_delete_sync(&port->rx_timer);
+
dev_dbg(&rpdev->dev, "rpmsg-eth client driver is removed\n");
}
diff --git a/drivers/net/ethernet/rpmsg_eth.h b/drivers/net/ethernet/rpmsg_eth.h
index 0c2ae89fbfbf..992d05bd9386 100644
--- a/drivers/net/ethernet/rpmsg_eth.h
+++ b/drivers/net/ethernet/rpmsg_eth.h
@@ -209,6 +209,7 @@ enum rpmsg_eth_state {
* @dev: Device
* @state: Interface state
* @state_work: Delayed work for state machine
+ * @sync_msg: Completion for synchronous message
*/
struct rpmsg_eth_common {
struct rpmsg_device *rpdev;
@@ -224,6 +225,7 @@ struct rpmsg_eth_common {
/** @state_lock: Lock for changing interface state */
struct mutex state_lock;
struct delayed_work state_work;
+ struct completion sync_msg;
};
/**
--
2.34.1
