[bsp/qemu-virt64-aarch64] Add VirtIO-Console and GPIO driver (#6150)

bsp/qemu-virt64-aarch64/.config

@@ -435,6 +435,7 @@ CONFIG_RT_LWIP_USING_PING=y
 # CONFIG_PKG_USING_RAPIDJSON is not set
 # CONFIG_PKG_USING_JSMN is not set
 # CONFIG_PKG_USING_AGILE_JSMN is not set
+# CONFIG_PKG_USING_PARSON is not set
 
 #
 # XML: Extensible Markup Language
@@ -548,15 +549,6 @@ CONFIG_RT_LWIP_USING_PING=y
 # CONFIG_PKG_USING_RT_KPRINTF_THREADSAFE is not set
 # CONFIG_PKG_USING_RT_VSNPRINTF_FULL is not set
 
-#
-# POSIX extension functions
-#
-# CONFIG_PKG_USING_POSIX_GETLINE is not set
-# CONFIG_PKG_USING_POSIX_WCWIDTH is not set
-# CONFIG_PKG_USING_POSIX_ITOA is not set
-# CONFIG_PKG_USING_POSIX_STRINGS is not set
-# CONFIG_PKG_USING_POSIX_CTYPES is not set
-
 #
 # acceleration: Assembly language or algorithmic acceleration packages
 #
@@ -622,6 +614,7 @@ CONFIG_RT_LWIP_USING_PING=y
 # CONFIG_PKG_USING_REALTEK_AMEBA is not set
 # CONFIG_PKG_USING_SHT2X is not set
 # CONFIG_PKG_USING_SHT3X is not set
+# CONFIG_PKG_USING_ADT74XX is not set
 # CONFIG_PKG_USING_AS7341 is not set
 # CONFIG_PKG_USING_STM32_SDIO is not set
 # CONFIG_PKG_USING_ICM20608 is not set
@@ -779,8 +772,10 @@ CONFIG_BSP_USING_UART=y
 CONFIG_RT_USING_UART0=y
 CONFIG_BSP_USING_RTC=y
 CONFIG_BSP_USING_ALARM=y
+CONFIG_BSP_USING_PIN=y
 CONFIG_BSP_USING_VIRTIO_BLK=y
 CONFIG_BSP_USING_VIRTIO_NET=y
+CONFIG_BSP_USING_VIRTIO_CONSOLE=y
 CONFIG_BSP_USING_VIRTIO_GPU=y
 CONFIG_BSP_USING_VIRTIO_INPUT=y
 CONFIG_BSP_USING_GIC=y

bsp/qemu-virt64-aarch64/README.md

@@ -45,13 +45,20 @@ Hi, this is RT-Thread!!
 msh />
 ```
 
+Use VirtIO-Console in new terminal by:
+````
+telnet 127.0.0.1 4321
+````
+
 ## 4. Condition
 
 | Driver | Condition | Remark |
 | ------ | --------- | ------ |
 | UART   | Support   | UART0  |
 | RTC    | Support   | - |
+| GPIO   | Support   | - |
 | VIRTIO BLK | Support | - |
 | VIRTIO NET | Support | - |
+| VIRTIO Console | Support | - |
 | VIRTIO GPU | Support | 2D |
 | VIRTIO Input | Support | Keyboard, Mouse, Tablet |
\ No newline at end of file

bsp/qemu-virt64-aarch64/README_zh.md

@@ -46,13 +46,20 @@ Hi, this is RT-Thread!!
 msh />
 ```
 
+如果需要使用VirtIO-Console，请在新终端使用以下命令连接控制台：
+```
+telnet 127.0.0.1 4321
+```
+
 ## 4.支持情况
 
 | 驱动 | 支持情况  |  备注  |
 | ------ | ----  | :------:  |
 | UART | 支持 | UART0 |
 | RTC  | 支持 | - |
+| GPIO | 支持 | - |
 | VIRTIO BLK | 支持 | - |
 | VIRTIO NET | 支持 | - |
+| VIRTIO Console | 支持 | - |
 | VIRTIO GPU | 支持 | 2D |
 | VIRTIO Input | 支持 | Keyboard, Mouse, Tablet |
\ No newline at end of file

bsp/qemu-virt64-aarch64/applications/console.c

+/*
+ * Copyright (c) 2006-2021, RT-Thread Development Team
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Change Logs:
+ * Date           Author       Notes
+ * 2021-11-11     GuEe-GUI     the first version
+ */
+
+#include <rtthread.h>
+
+#include <virtio_console.h>
+
+int console_init()
+{
+    rt_err_t status = RT_EOK;
+    rt_device_t device = rt_device_find("virtio-console0");
+
+    if (device != RT_NULL && rt_device_open(device, 0) == RT_EOK)
+    {
+        /* Create vport0p1 */
+        status = rt_device_control(device, VIRTIO_DEVICE_CTRL_CONSOLE_PORT_CREATE, RT_NULL);
+    }
+
+    if (device != RT_NULL)
+    {
+        rt_device_close(device);
+    }
+
+    return status;
+}
+INIT_ENV_EXPORT(console_init);
+
+#ifdef FINSH_USING_MSH
+
+static int console(int argc, char **argv)
+{
+    rt_err_t result = RT_EOK;
+
+    if (argc > 1)
+    {
+        if (!rt_strcmp(argv[1], "set"))
+        {
+            rt_kprintf("console change to %s\n", argv[2]);
+            rt_console_set_device(argv[2]);
+            finsh_set_device(argv[2]);
+        }
+        else
+        {
+            rt_kprintf("Unknown command. Please enter 'console' for help\n");
+            result = -RT_ERROR;
+        }
+    }
+    else
+    {
+        rt_kprintf("Usage: \n");
+        rt_kprintf("console set <name>   - change console by name\n");
+        result = -RT_ERROR;
+    }
+    return result;
+}
+MSH_CMD_EXPORT(console, set console name);
+
+#endif /* FINSH_USING_MSH */

bsp/qemu-virt64-aarch64/applications/pin.c

+/*
+ * Copyright (c) 2006-2022, RT-Thread Development Team
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Change Logs:
+ * Date           Author         Notes
+ * 2022-6-30      GuEe-GUI       first version
+ */
+
+#include <rthw.h>
+#include <rtthread.h>
+#include <rtdevice.h>
+
+#ifdef RT_USING_PIN
+
+void qemu_gpio3_key_poweroff(void *args)
+{
+    rt_kprintf("\nYou power off the machine.\n");
+
+    rt_hw_cpu_shutdown();
+}
+
+int pin_init()
+{
+    rt_pin_attach_irq(3, PIN_IRQ_MODE_FALLING, qemu_gpio3_key_poweroff, RT_NULL);
+    rt_pin_irq_enable(3, RT_TRUE);
+
+    return 0;
+}
+INIT_ENV_EXPORT(pin_init);
+
+#endif /* RT_USING_PIN */

bsp/qemu-virt64-aarch64/driver/Kconfig

@@ -24,6 +24,11 @@ menu "AARCH64 qemu virt64 configs"
                 default n
         endif
 
+    config BSP_USING_PIN
+        bool "Using PIN"
+        select RT_USING_PIN
+        default y
+
     config BSP_USING_VIRTIO_BLK
         bool "Using VirtIO BLK"
         default y
@@ -32,6 +37,10 @@ menu "AARCH64 qemu virt64 configs"
         bool "Using VirtIO NET"
         default y
 
+    config BSP_USING_VIRTIO_CONSOLE
+        bool "Using VirtIO Console"
+        default y
+
     config BSP_USING_VIRTIO_GPU
         bool "Using VirtIO GPU"
         default y

bsp/qemu-virt64-aarch64/driver/board.c

@@ -29,6 +29,7 @@ struct mem_desc platform_mem_desc[] =
 {
     {0x40000000, 0x80000000, 0x40000000, NORMAL_MEM},
     {PL031_RTC_BASE, PL031_RTC_BASE + 0x1000, PL031_RTC_BASE, DEVICE_MEM},
+    {PL061_GPIO_BASE, PL061_GPIO_BASE + 0x1000, PL061_GPIO_BASE, DEVICE_MEM},
     {PL011_UART0_BASE, PL011_UART0_BASE + 0x1000, PL011_UART0_BASE, DEVICE_MEM},
     {VIRTIO_MMIO_BASE, VIRTIO_MMIO_BASE + VIRTIO_MAX_NR * VIRTIO_MMIO_SIZE, VIRTIO_MMIO_BASE, DEVICE_MEM},
 #ifdef BSP_USING_GICV2
@@ -95,6 +96,13 @@ void poweroff(void)
 }
 MSH_CMD_EXPORT(poweroff, poweroff...);
 
+void rt_hw_cpu_shutdown()
+{
+    rt_kprintf("shutdown...\n");
+
+    poweroff();
+}
+
 void reboot(void)
 {
     arm_psci_system_reboot();

bsp/qemu-virt64-aarch64/driver/drv_gpio.c

+/*
+ * Copyright (c) 2006-2022, RT-Thread Development Team
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Change Logs:
+ * Date           Author         Notes
+ * 2022-6-30      GuEe-GUI       first version
+ */
+
+#include <rthw.h>
+#include <rtthread.h>
+#include <rtdevice.h>
+#include <board.h>
+
+#include "drv_gpio.h"
+
+#ifdef BSP_USING_PIN
+
+#define GPIODIR 0x400
+#define GPIOIS  0x404
+#define GPIOIBE 0x408
+#define GPIOIEV 0x40c
+#define GPIOIE  0x410
+#define GPIORIS 0x414
+#define GPIOMIS 0x418
+#define GPIOIC  0x41c
+
+#define BIT(x)  (1 << (x))
+
+#define PL061_GPIO_NR   8
+
+static struct pl061
+{
+#ifdef RT_USING_SMP
+    struct rt_spinlock spinlock;
+#endif
+    void (*(hdr[PL061_GPIO_NR]))(void *args);
+    void *args[PL061_GPIO_NR];
+} _pl061;
+
+rt_inline rt_uint8_t pl061_read8(rt_ubase_t offset)
+{
+    return HWREG8(PL061_GPIO_BASE + offset);
+}
+
+rt_inline void pl061_write8(rt_ubase_t offset, rt_uint8_t value)
+{
+    HWREG8(PL061_GPIO_BASE + offset) = value;
+}
+
+static void pl061_pin_mode(struct rt_device *device, rt_base_t pin, rt_base_t mode)
+{
+    int value;
+    rt_uint8_t gpiodir;
+
+#ifdef RT_USING_SMP
+    rt_base_t level;
+#endif
+
+    if (pin < 0 || pin >= PL061_GPIO_NR)
+    {
+        return;
+    }
+
+#ifdef RT_USING_SMP
+    level = rt_spin_lock_irqsave(&_pl061.spinlock);
+#endif
+
+    switch (mode)
+    {
+    case PIN_MODE_OUTPUT:
+
+        value = !!pl061_read8((BIT(pin + 2)));
+
+        pl061_write8(BIT(pin + 2), 0 << pin);
+        gpiodir = pl061_read8(GPIODIR);
+        gpiodir |= BIT(pin);
+        pl061_write8(GPIODIR, gpiodir);
+
+        /*
+         * gpio value is set again, because pl061 doesn't allow to set value of
+         * a gpio pin before configuring it in OUT mode.
+         */
+        pl061_write8((BIT(pin + 2)), value << pin);
+
+        break;
+    case PIN_MODE_INPUT:
+
+        gpiodir = pl061_read8(GPIODIR);
+        gpiodir &= ~(BIT(pin));
+        pl061_write8(GPIODIR, gpiodir);
+
+        break;
+    }
+
+#ifdef RT_USING_SMP
+    rt_spin_unlock_irqrestore(&_pl061.spinlock, level);
+#endif
+}
+
+static void pl061_pin_write(struct rt_device *device, rt_base_t pin, rt_base_t value)
+{
+    pl061_write8(BIT(pin + 2), !!value << pin);
+}
+
+static int pl061_pin_read(struct rt_device *device, rt_base_t pin)
+{
+    return !!pl061_read8((BIT(pin + 2)));
+}
+
+static rt_err_t pl061_pin_attach_irq(struct rt_device *device, rt_int32_t pin, rt_uint32_t mode, void (*hdr)(void *args), void *args)
+{
+    rt_uint8_t gpiois, gpioibe, gpioiev;
+    rt_uint8_t bit = BIT(mode);
+#ifdef RT_USING_SMP
+    rt_base_t level;
+#endif
+
+    if (pin < 0 || pin >= PL061_GPIO_NR)
+    {
+        return -RT_EINVAL;
+    }
+
+#ifdef RT_USING_SMP
+    level = rt_spin_lock_irqsave(&_pl061.spinlock);
+#endif
+
+    gpioiev = pl061_read8(GPIOIEV);
+    gpiois  = pl061_read8(GPIOIS);
+    gpioibe = pl061_read8(GPIOIBE);
+
+    if (mode == PIN_IRQ_MODE_HIGH_LEVEL || pin == PIN_IRQ_MODE_LOW_LEVEL)
+    {
+        rt_bool_t polarity = (mode == PIN_IRQ_MODE_HIGH_LEVEL);
+
+        /* Disable edge detection */
+        gpioibe &= ~bit;
+        /* Enable level detection */
+        gpiois |= bit;
+
+        /* Select polarity */
+        if (polarity)
+        {
+            gpioiev |= bit;
+        }
+        else
+        {
+            gpioiev &= ~bit;
+        }
+    }
+    else if (mode == PIN_IRQ_MODE_RISING_FALLING)
+    {
+        /* Disable level detection */
+        gpiois &= ~bit;
+        /* Select both edges, setting this makes GPIOEV be ignored */
+        gpioibe |= bit;
+    }
+    else if (mode == PIN_IRQ_MODE_RISING || mode == PIN_IRQ_MODE_FALLING)
+    {
+        rt_bool_t rising = (mode == PIN_IRQ_MODE_RISING);
+
+        /* Disable level detection */
+        gpiois &= ~bit;
+        /* Clear detection on both edges */
+        gpioibe &= ~bit;
+
+        /* Select edge */
+        if (rising)
+        {
+            gpioiev |= bit;
+        }
+        else
+        {
+            gpioiev &= ~bit;
+        }
+    }
+    else
+    {
+        /* No trigger: disable everything */
+        gpiois  &= ~bit;
+        gpioibe &= ~bit;
+        gpioiev &= ~bit;
+    }
+
+    pl061_write8(GPIOIS, gpiois);
+    pl061_write8(GPIOIBE, gpioibe);
+    pl061_write8(GPIOIEV, gpioiev);
+
+    _pl061.hdr[pin] = hdr;
+    _pl061.args[pin] = args;
+
+#ifdef RT_USING_SMP
+    rt_spin_unlock_irqrestore(&_pl061.spinlock, level);
+#endif
+
+    return RT_EOK;
+}
+
+static rt_err_t pl061_pin_detach_irq(struct rt_device *device, rt_int32_t pin)
+{
+    if (pin < 0 || pin >= PL061_GPIO_NR)
+    {
+        return -RT_EINVAL;
+    }
+
+    _pl061.hdr[pin] = RT_NULL;
+    _pl061.args[pin] = RT_NULL;
+
+    return RT_EOK;
+}
+
+static rt_err_t pl061_pin_irq_enable(struct rt_device *device, rt_base_t pin, rt_uint32_t enabled)
+{
+    rt_uint8_t mask = BIT(pin);
+    rt_uint8_t gpioie;
+
+#ifdef RT_USING_SMP
+    rt_base_t level;
+#endif
+
+    if (pin < 0 || pin >= PL061_GPIO_NR)
+    {
+        return -RT_EINVAL;
+    }
+
+#ifdef RT_USING_SMP
+    level = rt_spin_lock_irqsave(&_pl061.spinlock);
+#endif
+
+    if (enabled)
+    {
+        gpioie = pl061_read8(GPIOIE) | mask;
+    }
+    else
+    {
+        gpioie = pl061_read8(GPIOIE) & ~mask;
+    }
+
+    pl061_write8(GPIOIE, gpioie);
+
+#ifdef RT_USING_SMP
+    rt_spin_unlock_irqrestore(&_pl061.spinlock, level);
+#endif
+
+    return RT_EOK;
+}
+
+static const struct rt_pin_ops ops =
+{
+    pl061_pin_mode,
+    pl061_pin_write,
+    pl061_pin_read,
+    pl061_pin_attach_irq,
+    pl061_pin_detach_irq,
+    pl061_pin_irq_enable,
+    RT_NULL,
+};
+
+static void rt_hw_gpio_isr(int irqno, void *param)
+{
+    rt_uint8_t mask;
+    unsigned long pending;
+
+#ifdef RT_USING_SMP
+    rt_base_t level;
+#endif
+
+    pending = pl061_read8(GPIOMIS);
+
+    if (pending)
+    {
+        rt_base_t pin;
+
+        for (pin = 0; pin < PL061_GPIO_NR; ++pin)
+        {
+            if (pending & BIT(pin))
+            {
+                mask |= BIT(pin);
+
+                if (_pl061.hdr[pin] != RT_NULL)
+                {
+                    _pl061.hdr[pin](_pl061.args[pin]);
+                }
+            }
+        }
+    }
+
+#ifdef RT_USING_SMP
+    level = rt_spin_lock_irqsave(&_pl061.spinlock);
+#endif
+
+    pl061_write8(GPIOIC, mask);
+
+#ifdef RT_USING_SMP
+    rt_spin_unlock_irqrestore(&_pl061.spinlock, level);
+#endif
+}
+
+int rt_hw_gpio_init(void)
+{
+#ifdef RT_USING_SMP
+    rt_spin_lock_init(&_pl061.spinlock);
+#endif
+
+    rt_device_pin_register("gpio", &ops, RT_NULL);
+    rt_hw_interrupt_install(PL061_GPIO_IRQNUM, rt_hw_gpio_isr, RT_NULL, "gpio");
+    rt_hw_interrupt_umask(PL061_GPIO_IRQNUM);
+
+    return 0;
+}
+INIT_DEVICE_EXPORT(rt_hw_gpio_init);
+
+#endif /* BSP_USING_PIN */

bsp/qemu-virt64-aarch64/driver/drv_gpio.h

+/*
+ * Copyright (c) 2006-2021, RT-Thread Development Team
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Change Logs:
+ * Date           Author         Notes
+ * 2022-6-30      GuEe-GUI       first version
+ */
+
+#ifndef DRV_GPIO_H__
+#define DRV_GPIO_H__
+
+int rt_hw_gpio_init(void);
+
+#endif

bsp/qemu-virt64-aarch64/driver/drv_virtio.c

@@ -17,6 +17,9 @@
 #ifdef BSP_USING_VIRTIO_NET
 #include <virtio_net.h>
 #endif
+#ifdef BSP_USING_VIRTIO_CONSOLE
+#include <virtio_console.h>
+#endif
 #ifdef BSP_USING_VIRTIO_GPU
 #include <virtio_gpu.h>
 #endif
@@ -34,6 +37,9 @@ static virtio_device_init_handler virtio_device_init_handlers[] =
 #ifdef BSP_USING_VIRTIO_NET
     [VIRTIO_DEVICE_ID_NET]      = rt_virtio_net_init,
 #endif
+#ifdef BSP_USING_VIRTIO_CONSOLE
+    [VIRTIO_DEVICE_ID_CONSOLE]  = rt_virtio_console_init,
+#endif
 #ifdef BSP_USING_VIRTIO_GPU
     [VIRTIO_DEVICE_ID_GPU]      = rt_virtio_gpu_init,
 #endif

bsp/qemu-virt64-aarch64/driver/virt.h

@@ -23,6 +23,11 @@
 #define PL031_RTC_SIZE      0x00001000
 #define PL031_RTC_IRQNUM    (32 + 2)
 
+/* GPIO */
+#define PL061_GPIO_BASE     0x09030000
+#define PL061_GPIO_SIZE     0x00001000
+#define PL061_GPIO_IRQNUM   (32 + 7)
+
 /* VirtIO */
 #define VIRTIO_MMIO_BASE    0x0a000000
 #define VIRTIO_MMIO_SIZE    0x00000200

bsp/qemu-virt64-aarch64/driver/virtio/virtio.c

@@ -54,11 +54,42 @@ void virtio_interrupt_ack(struct virtio_device *dev)
     }
 }
 
+rt_bool_t virtio_has_feature(struct virtio_device *dev, rt_uint32_t feature_bit)
+{
+    _virtio_dev_check(dev);
+
+    return !!(dev->mmio_config->device_features & (1UL << feature_bit));
+}
+
+rt_err_t virtio_queues_alloc(struct virtio_device *dev, rt_size_t queues_num)
+{
+    _virtio_dev_check(dev);
+
+    dev->queues = rt_malloc(sizeof(struct virtq) * queues_num);
+
+    if (dev->queues != RT_NULL)
+    {
+        dev->queues_num = queues_num;
+
+        return RT_EOK;
+    }
+
+    return -RT_ENOMEM;
+}
+
+void virtio_queues_free(struct virtio_device *dev)
+{
+    if (dev->queues != RT_NULL)
+    {
+        dev->queues_num = 0;
+        rt_free(dev->queues);
+    }
+}
+
 rt_err_t virtio_queue_init(struct virtio_device *dev, rt_uint32_t queue_index, rt_size_t ring_size)
 {
     int i;
     void *pages;
-    rt_ubase_t pages_paddr;
     rt_size_t pages_total_size;
     struct virtq *queue;
 
@@ -90,17 +121,16 @@ rt_err_t virtio_queue_init(struct virtio_device *dev, rt_uint32_t queue_index, r
     }
 
     rt_memset(pages, 0, pages_total_size);
-    pages_paddr = VIRTIO_VA2PA(pages);
 
     dev->mmio_config->guest_page_size = VIRTIO_PAGE_SIZE;
     dev->mmio_config->queue_sel = queue_index;
     dev->mmio_config->queue_num = ring_size;
     dev->mmio_config->queue_align = VIRTIO_PAGE_SIZE;
-    dev->mmio_config->queue_pfn = pages_paddr >> VIRTIO_PAGE_SHIFT;
+    dev->mmio_config->queue_pfn = VIRTIO_VA2PA(pages) >> VIRTIO_PAGE_SHIFT;
 
     queue->num = ring_size;
-    queue->desc = (struct virtq_desc *)pages_paddr;
-    queue->avail = (struct virtq_avail *)(pages_paddr + VIRTQ_DESC_TOTAL_SIZE(ring_size));
+    queue->desc = (struct virtq_desc *)((rt_ubase_t)pages);
+    queue->avail = (struct virtq_avail *)(((rt_ubase_t)pages) + VIRTQ_DESC_TOTAL_SIZE(ring_size));
     queue->used = (struct virtq_used *)VIRTIO_PAGE_ALIGN(
             (rt_ubase_t)&queue->avail->ring[ring_size] + VIRTQ_AVAIL_RES_SIZE);
 
@@ -175,7 +205,7 @@ rt_uint16_t virtio_alloc_desc(struct virtio_device *dev, rt_uint32_t queue_index
 
     _virtio_dev_check(dev);
 
-    RT_ASSERT(queue_index < RT_USING_VIRTIO_QUEUE_MAX_NR);
+    RT_ASSERT(queue_index < dev->queues_num);
 
     queue = &dev->queues[queue_index];
 
@@ -206,7 +236,7 @@ void virtio_free_desc(struct virtio_device *dev, rt_uint32_t queue_index, rt_uin
 
     queue = &dev->queues[queue_index];
 
-    RT_ASSERT(queue_index + 1 < RT_USING_VIRTIO_QUEUE_MAX_NR);
+    RT_ASSERT(queue_index < dev->queues_num);
     RT_ASSERT(!queue->free[desc_index]);
 
     queue->desc[desc_index].addr = 0;

bsp/qemu-virt64-aarch64/driver/virtio/virtio.h

@@ -19,10 +19,6 @@
 #define RT_USING_VIRTIO_VERSION 0x1
 #endif
 
-#ifndef RT_USING_VIRTIO_QUEUE_MAX_NR
-#define RT_USING_VIRTIO_QUEUE_MAX_NR 4
-#endif
-
 #include <virtio_mmio.h>
 #include <virtio_queue.h>
 
@@ -102,7 +98,9 @@ struct virtio_device
 {
     rt_uint32_t irq;
 
-    struct virtq queues[RT_USING_VIRTIO_QUEUE_MAX_NR];
+    struct virtq *queues;
+    rt_size_t queues_num;
+
     union
     {
         rt_ubase_t *mmio_base;
@@ -122,7 +120,10 @@ void virtio_reset_device(struct virtio_device *dev);
 void virtio_status_acknowledge_driver(struct virtio_device *dev);
 void virtio_status_driver_ok(struct virtio_device *dev);
 void virtio_interrupt_ack(struct virtio_device *dev);
+rt_bool_t virtio_has_feature(struct virtio_device *dev, rt_uint32_t feature_bit);
 
+rt_err_t virtio_queues_alloc(struct virtio_device *dev, rt_size_t queues_num);
+void virtio_queues_free(struct virtio_device *dev);
 rt_err_t virtio_queue_init(struct virtio_device *dev, rt_uint32_t queue_index, rt_size_t ring_size);
 void virtio_queue_destroy(struct virtio_device *dev, rt_uint32_t queue_index);
 void virtio_queue_notify(struct virtio_device *dev, rt_uint32_t queue_index);

bsp/qemu-virt64-aarch64/driver/virtio/virtio_blk.c

@@ -29,6 +29,14 @@ static void virtio_blk_rw(struct virtio_blk_device *virtio_blk_dev, rt_off_t pos
     /* Allocate 3 descriptors */
     while (virtio_alloc_desc_chain(virtio_dev, 0, 3, idx))
     {
+#ifdef RT_USING_SMP
+        rt_spin_unlock_irqrestore(&virtio_dev->spinlock, level);
+#endif
+        rt_thread_yield();
+
+#ifdef RT_USING_SMP
+        level = rt_spin_lock_irqsave(&virtio_dev->spinlock);
+#endif
     }
 
     virtio_blk_dev->info[idx[0]].status = 0xff;
@@ -150,7 +158,6 @@ static void virtio_blk_isr(int irqno, void *param)
 
         /* Done with buffer */
         virtio_blk_dev->info[id].valid = RT_FALSE;
-        rt_thread_yield();
 
         queue->used_idx++;
     }
@@ -200,12 +207,15 @@ rt_err_t rt_virtio_blk_init(rt_ubase_t *mmio_base, rt_uint32_t irq)
     /* Tell device that feature negotiation is complete and we're completely ready */
     virtio_status_driver_ok(virtio_dev);
 
+    if (virtio_queues_alloc(virtio_dev, 1) != RT_EOK)
+    {
+        goto _alloc_fail;
+    }
+
     /* Initialize queue 0 */
     if (virtio_queue_init(virtio_dev, 0, VIRTIO_BLK_QUEUE_RING_SIZE) != RT_EOK)
     {
-        rt_free(virtio_blk_dev);
-
-        return -RT_ENOMEM;
+        goto _alloc_fail;
     }
 
     virtio_blk_dev->parent.type = RT_Device_Class_Block;
@@ -217,5 +227,14 @@ rt_err_t rt_virtio_blk_init(rt_ubase_t *mmio_base, rt_uint32_t irq)
     rt_hw_interrupt_umask(irq);
 
     return rt_device_register((rt_device_t)virtio_blk_dev, dev_name, RT_DEVICE_FLAG_RDWR | RT_DEVICE_FLAG_REMOVABLE);
+
+_alloc_fail:
+
+    if (virtio_blk_dev != RT_NULL)
+    {
+        virtio_queues_free(virtio_dev);
+        rt_free(virtio_blk_dev);
+    }
+    return -RT_ENOMEM;
 }
 #endif /* BSP_USING_VIRTIO_BLK */

bsp/qemu-virt64-aarch64/driver/virtio/virtio_console.c

+/*
+ * Copyright (c) 2006-2021, RT-Thread Development Team
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Change Logs:
+ * Date           Author       Notes
+ * 2021-11-11     GuEe-GUI     the first version
+ */
+
+#include <rthw.h>
+#include <rtthread.h>
+#include <cpuport.h>
+
+#ifdef BSP_USING_VIRTIO_CONSOLE
+
+#include <virtio_console.h>
+
+struct port_device
+{
+    struct rt_device parent;
+
+    rt_list_t node;
+    rt_uint32_t port_id;
+    rt_bool_t rx_notify;
+    rt_bool_t need_destroy;
+
+    struct virtio_console_device *console;
+
+    struct virtq *queue_rx, *queue_tx;
+    rt_uint32_t queue_rx_index, queue_tx_index;
+
+#ifdef RT_USING_SMP
+    struct rt_spinlock spinlock_rx, spinlock_tx;
+#endif
+
+    struct
+    {
+        char rx_char, tx_char;
+    } info[VIRTIO_CONSOLE_QUEUE_SIZE];
+};
+
+static void virtio_console_send_ctrl(struct virtio_console_device *virtio_console_dev,
+        struct virtio_console_control *ctrl)
+{
+    rt_uint16_t id;
+    void *addr;
+    struct virtio_device *virtio_dev = &virtio_console_dev->virtio_dev;
+    struct virtq *queue_ctrl_tx;
+
+#ifdef RT_USING_SMP
+    rt_base_t level = rt_spin_lock_irqsave(&virtio_dev->spinlock);
+#endif
+
+    queue_ctrl_tx = &virtio_dev->queues[VIRTIO_CONSOLE_QUEUE_CTRL_TX];
+
+    id = queue_ctrl_tx->avail->idx % queue_ctrl_tx->num;
+    addr = &virtio_console_dev->info[id].tx_ctrl;
+
+    rt_memcpy(addr, ctrl, sizeof(struct virtio_console_control));
+
+    virtio_free_desc(virtio_dev, VIRTIO_CONSOLE_QUEUE_CTRL_TX, id);
+
+    virtio_fill_desc(virtio_dev, VIRTIO_CONSOLE_QUEUE_CTRL_TX, id,
+            VIRTIO_VA2PA(addr), sizeof(struct virtio_console_control), 0, 0);
+
+    virtio_submit_chain(virtio_dev, VIRTIO_CONSOLE_QUEUE_CTRL_TX, id);
+
+    virtio_queue_notify(virtio_dev, VIRTIO_CONSOLE_QUEUE_CTRL_TX);
+
+    virtio_alloc_desc(virtio_dev, VIRTIO_CONSOLE_QUEUE_CTRL_TX);
+
+#ifdef RT_USING_SMP
+    rt_spin_unlock_irqrestore(&virtio_dev->spinlock, level);
+#endif
+}
+
+static rt_err_t virtio_console_port_create(struct virtio_console_device *virtio_console_dev,
+    const struct rt_device_ops *ops)
+{
+    rt_uint32_t port_id;
+    char dev_name[RT_NAME_MAX];
+    struct port_device *port_dev, *prev_port_dev = RT_NULL;
+    struct virtio_device *virtio_dev = &virtio_console_dev->virtio_dev;
+
+    if (virtio_console_dev->port_nr > 0 && !virtio_has_feature(virtio_dev, VIRTIO_CONSOLE_F_MULTIPORT))
+    {
+        return -RT_ENOSYS;
+    }
+
+    if (virtio_console_dev->port_nr >= virtio_console_dev->max_port_nr)
+    {
+        return -RT_EFULL;
+    }
+
+    port_id = 0;
+
+    /* The port device list is always ordered, so just find next number for id */
+    rt_list_for_each_entry(port_dev, &virtio_console_dev->port_head, node)
+    {
+        if (port_dev->port_id != port_id)
+        {
+            break;
+        }
+        ++port_id;
+        prev_port_dev = port_dev;
+    }
+
+    port_dev = rt_malloc(sizeof(struct port_device));
+
+    if (port_dev == RT_NULL)
+    {
+        return -RT_ENOMEM;
+    }
+
+    port_dev->parent.type = RT_Device_Class_Char;
+    port_dev->parent.ops  = ops;
+
+    port_dev->parent.rx_indicate = RT_NULL;
+    port_dev->parent.tx_complete = RT_NULL;
+
+    rt_list_init(&port_dev->node);
+    port_dev->port_id = port_id;
+    port_dev->need_destroy = RT_FALSE;
+    port_dev->rx_notify = RT_TRUE;
+    port_dev->console = virtio_console_dev;
+    port_dev->queue_rx_index = VIRTIO_CONSOLE_PORT_QUEUE_INDEX(port_dev->port_id, VIRTIO_CONSOLE_QUEUE_DATA_RX);
+    port_dev->queue_tx_index = VIRTIO_CONSOLE_PORT_QUEUE_INDEX(port_dev->port_id, VIRTIO_CONSOLE_QUEUE_DATA_TX);
+    port_dev->queue_rx = &virtio_dev->queues[port_dev->queue_rx_index];
+    port_dev->queue_tx = &virtio_dev->queues[port_dev->queue_tx_index];
+
+#ifdef RT_USING_SMP
+    rt_spin_lock_init(&port_dev->spinlock_rx);
+    rt_spin_lock_init(&port_dev->spinlock_tx);
+#endif
+
+    rt_snprintf(dev_name, RT_NAME_MAX, "vport%dp%d", virtio_console_dev->console_id, port_id);
+
+    if (rt_device_register((rt_device_t)port_dev, dev_name, RT_DEVICE_FLAG_RDWR | RT_DEVICE_FLAG_INT_RX) != RT_EOK)
+    {
+        rt_free(port_dev);
+
+        return -RT_ERROR;
+    }
+
+    if (prev_port_dev != RT_NULL)
+    {
+        rt_list_insert_after(&prev_port_dev->node, &port_dev->node);
+    }
+    else
+    {
+        /* Port0 */
+        rt_list_insert_after(&virtio_console_dev->port_head, &port_dev->node);
+    }
+
+    virtio_console_dev->port_nr++;
+
+    return RT_EOK;
+}
+
+static void virtio_console_port_destroy(struct virtio_console_device *virtio_console_dev,
+        struct port_device *port_dev)
+{
+    struct virtio_console_control set_ctrl;
+
+    set_ctrl.id = port_dev->port_id;
+    set_ctrl.event = VIRTIO_CONSOLE_PORT_OPEN;
+    set_ctrl.value = 0;
+
+    virtio_console_send_ctrl(virtio_console_dev, &set_ctrl);
+
+    virtio_console_dev->port_nr--;
+
+    rt_list_remove(&port_dev->node);
+
+    rt_device_unregister((rt_device_t)port_dev);
+
+    rt_free(port_dev);
+}
+
+static rt_err_t virtio_console_port_init(rt_device_t dev)
+{
+    rt_uint16_t id;
+    rt_uint16_t idx[VIRTIO_CONSOLE_QUEUE_SIZE];
+    rt_uint16_t rx_queue_index, tx_queue_index;
+    struct port_device *port_dev = (struct port_device *)dev;
+    struct virtio_console_device *virtio_console_dev = port_dev->console;
+    struct virtio_device *virtio_dev = &virtio_console_dev->virtio_dev;
+    struct virtq *queue_rx, *queue_tx;
+
+    rx_queue_index = VIRTIO_CONSOLE_PORT_QUEUE_INDEX(port_dev->port_id, VIRTIO_CONSOLE_QUEUE_DATA_RX);
+    tx_queue_index = VIRTIO_CONSOLE_PORT_QUEUE_INDEX(port_dev->port_id, VIRTIO_CONSOLE_QUEUE_DATA_TX);
+
+    queue_rx = &virtio_dev->queues[rx_queue_index];
+    queue_tx = &virtio_dev->queues[tx_queue_index];
+
+    virtio_alloc_desc_chain(virtio_dev, rx_queue_index, queue_rx->num, idx);
+    virtio_alloc_desc_chain(virtio_dev, tx_queue_index, queue_tx->num, idx);
+
+    for (id = 0; id < queue_rx->num; ++id)
+    {
+        void *addr = &port_dev->info[id].rx_char;
+
+        virtio_fill_desc(virtio_dev, rx_queue_index, id,
+                VIRTIO_VA2PA(addr), sizeof(char), VIRTQ_DESC_F_WRITE, 0);
+
+        queue_rx->avail->ring[id] = id;
+    }
+    rt_hw_dsb();
+
+    queue_rx->avail->flags = 0;
+    queue_rx->avail->idx = queue_rx->num;
+
+    queue_rx->used_idx = queue_rx->used->idx;
+
+    queue_tx->avail->flags = VIRTQ_AVAIL_F_NO_INTERRUPT;
+    queue_tx->avail->idx = 0;
+
+    virtio_queue_notify(virtio_dev, rx_queue_index);
+
+    if (virtio_has_feature(virtio_dev, VIRTIO_CONSOLE_F_MULTIPORT))
+    {
+        struct virtio_console_control set_ctrl;
+
+        set_ctrl.id = VIRTIO_CONSOLE_PORT_BAD_ID;
+        set_ctrl.event = VIRTIO_CONSOLE_DEVICE_READY;
+        set_ctrl.value = 1;
+
+        virtio_console_send_ctrl(virtio_console_dev, &set_ctrl);
+    }
+
+    return RT_EOK;
+}
+
+static rt_err_t virtio_console_port_open(rt_device_t dev, rt_uint16_t oflag)
+{
+    struct port_device *port_dev = (struct port_device *)dev;
+
+    /* Can't use by others, just support only one */
+    if (port_dev->parent.ref_count > 1)
+    {
+        return -RT_EBUSY;
+    }
+
+    if (port_dev->port_id == 0 && virtio_has_feature(&port_dev->console->virtio_dev, VIRTIO_CONSOLE_F_MULTIPORT))
+    {
+        /* Port0 is reserve in multiport */
+        return -RT_ERROR;
+    }
+
+    port_dev->rx_notify = RT_TRUE;
+
+    return RT_EOK;
+}
+
+static rt_err_t virtio_console_port_close(rt_device_t dev)
+{
+    struct port_device *port_dev = (struct port_device *)dev;
+
+    if (port_dev->need_destroy)
+    {
+        virtio_console_port_destroy(port_dev->console, port_dev);
+
+        /*
+         * We released the device memory in virtio_console_port_destroy,
+         * rt_device_close has not finished yet, make the return value
+         * to empty so that rt_device_close will not access the device memory.
+         */
+        return -RT_EEMPTY;
+    }
+
+    port_dev->rx_notify = RT_FALSE;
+
+    return RT_EOK;
+}
+
+static rt_size_t virtio_console_port_read(rt_device_t dev, rt_off_t pos, void *buffer, rt_size_t size)
+{
+    rt_off_t i = 0;
+    rt_uint16_t id;
+    rt_uint32_t len;
+    struct port_device *port_dev = (struct port_device *)dev;
+    struct virtio_device *virtio_dev = &port_dev->console->virtio_dev;
+    rt_uint32_t queue_rx_index = port_dev->queue_rx_index;
+    struct virtq *queue_rx = port_dev->queue_rx;
+
+#ifdef RT_USING_SMP
+    rt_base_t level = rt_spin_lock_irqsave(&port_dev->spinlock_rx);
+#endif
+
+    while (i < size)
+    {
+        if (queue_rx->used_idx == queue_rx->used->idx)
+        {
+            break;
+        }
+        rt_hw_dsb();
+
+        id = queue_rx->used->ring[queue_rx->used_idx % queue_rx->num].id;
+        len = queue_rx->used->ring[queue_rx->used_idx % queue_rx->num].len;
+
+        if (len > sizeof(char))
+        {
+            rt_kprintf("%s: Receive buffer's size = %u is too big!\n", port_dev->parent.parent.name, len);
+            len = sizeof(char);
+        }
+
+        *((char *)buffer + i) = port_dev->info[id].rx_char;
+
+        queue_rx->used_idx++;
+
+        virtio_submit_chain(virtio_dev, queue_rx_index, id);
+
+        virtio_queue_notify(virtio_dev, queue_rx_index);
+
+        i += len;
+    }
+
+#ifdef RT_USING_SMP
+    rt_spin_unlock_irqrestore(&port_dev->spinlock_rx, level);
+#endif
+
+    size = i;
+
+    return size;
+}
+
+static rt_size_t virtio_console_port_write(rt_device_t dev, rt_off_t pos, const void *buffer, rt_size_t size)
+{
+    char ch = 0;
+    rt_off_t i = 0;
+    rt_uint16_t id;
+    struct port_device *port_dev = (struct port_device *)dev;
+    struct virtio_device *virtio_dev = &port_dev->console->virtio_dev;
+    rt_uint32_t queue_tx_index = port_dev->queue_tx_index;
+    struct virtq *queue_tx = port_dev->queue_tx;
+
+#ifdef RT_USING_SMP
+    rt_base_t level = rt_spin_lock_irqsave(&port_dev->spinlock_tx);
+#endif
+
+    while (i < size || ch == '\r')
+    {
+        id = queue_tx->avail->idx % queue_tx->num;
+
+        /* Keep the way until 'new line' are unified */
+        if (ch != '\r')
+        {
+            ch = *((const char *)buffer + i);
+        }
+        else
+        {
+            i -= sizeof(char);
+        }
+
+        port_dev->info[id].tx_char = ch;
+
+        ch = (ch == '\n' ? '\r' : 0);
+
+        virtio_free_desc(virtio_dev, queue_tx_index, id);
+
+        virtio_fill_desc(virtio_dev, queue_tx_index, id,
+                VIRTIO_VA2PA(&port_dev->info[id].tx_char), sizeof(char), 0, 0);
+
+        virtio_submit_chain(virtio_dev, queue_tx_index, id);
+
+        virtio_queue_notify(virtio_dev, queue_tx_index);
+
+        virtio_alloc_desc(virtio_dev, queue_tx_index);
+
+        i += sizeof(char);
+    }
+
+#ifdef RT_USING_SMP
+    rt_spin_unlock_irqrestore(&port_dev->spinlock_tx, level);
+#endif
+
+    return size;
+}
+
+static rt_err_t virtio_console_port_control(rt_device_t dev, int cmd, void *args)
+{
+    rt_err_t status = RT_EOK;
+    struct port_device *port_dev = (struct port_device *)dev;
+
+    switch (cmd)
+    {
+    case RT_DEVICE_CTRL_CLR_INT:
+        /* Disable RX */
+        port_dev->rx_notify = RT_FALSE;
+        break;
+    case RT_DEVICE_CTRL_SET_INT:
+        /* Enable RX */
+        port_dev->rx_notify = RT_TRUE;
+        break;
+    case VIRTIO_DEVICE_CTRL_CONSOLE_PORT_DESTROY:
+        {
+            port_dev->need_destroy = RT_TRUE;
+            port_dev->rx_notify = RT_FALSE;
+        }
+        break;
+    default:
+        status = -RT_EINVAL;
+        break;
+    }
+
+    return status;
+}
+
+const static struct rt_device_ops virtio_console_port_ops =
+{
+    virtio_console_port_init,
+    virtio_console_port_open,
+    virtio_console_port_close,
+    virtio_console_port_read,
+    virtio_console_port_write,
+    virtio_console_port_control
+};
+
+static rt_err_t virtio_console_init(rt_device_t dev)
+{
+    struct virtio_console_device *virtio_console_dev = (struct virtio_console_device *)dev;
+    struct virtio_device *virtio_dev = &virtio_console_dev->virtio_dev;
+
+    if (virtio_has_feature(virtio_dev, VIRTIO_CONSOLE_F_MULTIPORT))
+    {
+        rt_uint16_t id;
+        rt_uint16_t idx[VIRTIO_CONSOLE_QUEUE_SIZE];
+        struct virtq *queue_ctrl_rx, *queue_ctrl_tx;
+
+        queue_ctrl_rx = &virtio_dev->queues[VIRTIO_CONSOLE_QUEUE_CTRL_RX];
+        queue_ctrl_tx = &virtio_dev->queues[VIRTIO_CONSOLE_QUEUE_CTRL_TX];
+
+        virtio_alloc_desc_chain(virtio_dev, VIRTIO_CONSOLE_QUEUE_CTRL_RX, queue_ctrl_rx->num, idx);
+        virtio_alloc_desc_chain(virtio_dev, VIRTIO_CONSOLE_QUEUE_CTRL_TX, queue_ctrl_tx->num, idx);
+
+        for (id = 0; id < queue_ctrl_rx->num; ++id)
+        {
+            void *addr = &virtio_console_dev->info[id].rx_ctrl;
+
+            virtio_fill_desc(virtio_dev, VIRTIO_CONSOLE_QUEUE_CTRL_RX, id,
+                    VIRTIO_VA2PA(addr), sizeof(struct virtio_console_control), VIRTQ_DESC_F_WRITE, 0);
+
+            queue_ctrl_rx->avail->ring[id] = id;
+        }
+        rt_hw_dsb();
+
+        queue_ctrl_rx->avail->flags = 0;
+        queue_ctrl_rx->avail->idx = queue_ctrl_rx->num;
+
+        queue_ctrl_rx->used_idx = queue_ctrl_rx->used->idx;
+
+        queue_ctrl_tx->avail->flags = VIRTQ_AVAIL_F_NO_INTERRUPT;
+        queue_ctrl_tx->avail->idx = 0;
+
+        virtio_queue_notify(virtio_dev, VIRTIO_CONSOLE_QUEUE_CTRL_RX);
+    }
+
+    return virtio_console_port_create(virtio_console_dev, &virtio_console_port_ops);
+}
+
+static rt_err_t virtio_console_control(rt_device_t dev, int cmd, void *args)
+{
+    rt_err_t status = RT_EOK;
+    struct virtio_console_device *virtio_console_dev = (struct virtio_console_device *)dev;
+
+    switch (cmd)
+    {
+    case VIRTIO_DEVICE_CTRL_CONSOLE_PORT_CREATE:
+        status = virtio_console_port_create(virtio_console_dev, &virtio_console_port_ops);
+        break;
+    default:
+        status = -RT_EINVAL;
+        break;
+    }
+
+    return status;
+}
+
+const static struct rt_device_ops virtio_console_ops =
+{
+    virtio_console_init,
+    RT_NULL,
+    RT_NULL,
+    RT_NULL,
+    RT_NULL,
+    virtio_console_control
+};
+
+static void virtio_console_isr(int irqno, void *param)
+{
+    rt_uint32_t id;
+    rt_uint32_t len;
+    struct port_device *port_dev;
+    struct virtio_console_device *virtio_console_dev = (struct virtio_console_device *)param;
+    struct virtio_device *virtio_dev = &virtio_console_dev->virtio_dev;
+    const char *dev_name = virtio_console_dev->parent.parent.name;
+
+#ifdef RT_USING_SMP
+    rt_base_t level = rt_spin_lock_irqsave(&virtio_dev->spinlock);
+#endif
+
+    virtio_interrupt_ack(virtio_dev);
+    rt_hw_dsb();
+
+    do {
+        struct virtq *queue_rx;
+        struct virtio_console_control *ctrl, set_ctrl;
+
+        if (!virtio_has_feature(virtio_dev, VIRTIO_CONSOLE_F_MULTIPORT))
+        {
+            break;
+        }
+
+        queue_rx = &virtio_dev->queues[VIRTIO_CONSOLE_QUEUE_CTRL_RX];
+
+        if (queue_rx->used_idx == queue_rx->used->idx)
+        {
+            break;
+        }
+        rt_hw_dsb();
+
+        id = queue_rx->used->ring[queue_rx->used_idx % queue_rx->num].id;
+        len = queue_rx->used->ring[queue_rx->used_idx % queue_rx->num].len;
+
+        queue_rx->used_idx++;
+
+        if (len != sizeof(struct virtio_console_control))
+        {
+            rt_kprintf("%s: Invalid ctrl!\n", dev_name);
+            break;
+        }
+
+        ctrl = &virtio_console_dev->info[id].rx_ctrl;
+
+        switch (ctrl->event)
+        {
+        case VIRTIO_CONSOLE_PORT_ADD:
+            {
+                set_ctrl.id = ctrl->id;
+                set_ctrl.event = VIRTIO_CONSOLE_PORT_READY;
+                set_ctrl.value = 1;
+
+            #ifdef RT_USING_SMP
+                rt_spin_unlock_irqrestore(&virtio_dev->spinlock, level);
+            #endif
+
+                virtio_console_send_ctrl(virtio_console_dev, &set_ctrl);
+
+            #ifdef RT_USING_SMP
+                level = rt_spin_lock_irqsave(&virtio_dev->spinlock);
+            #endif
+            }
+            break;
+        case VIRTIO_CONSOLE_PORT_REMOVE:
+            break;
+        case VIRTIO_CONSOLE_RESIZE:
+            break;
+        case VIRTIO_CONSOLE_PORT_OPEN:
+            {
+                set_ctrl.id = ctrl->id;
+                set_ctrl.event = VIRTIO_CONSOLE_PORT_OPEN;
+                set_ctrl.value = 1;
+
+            #ifdef RT_USING_SMP
+                rt_spin_unlock_irqrestore(&virtio_dev->spinlock, level);
+            #endif
+
+                virtio_console_send_ctrl(virtio_console_dev, &set_ctrl);
+
+            #ifdef RT_USING_SMP
+                level = rt_spin_lock_irqsave(&virtio_dev->spinlock);
+            #endif
+            }
+            break;
+        case VIRTIO_CONSOLE_PORT_NAME:
+            break;
+        default:
+            rt_kprintf("%s: Unsupport ctrl[id: %d, event: %d, value: %d]!\n",
+                    dev_name, ctrl->id, ctrl->event, ctrl->value);
+            break;
+        }
+
+    } while (0);
+
+#ifdef RT_USING_SMP
+    rt_spin_unlock_irqrestore(&virtio_dev->spinlock, level);
+#endif
+
+    rt_list_for_each_entry(port_dev, &virtio_console_dev->port_head, node)
+    {
+        rt_uint32_t queue_rx_index = port_dev->queue_rx_index;
+        struct virtq *queue_rx = port_dev->queue_rx;
+
+#ifdef RT_USING_SMP
+        rt_base_t level = rt_spin_lock_irqsave(&port_dev->spinlock_rx);
+#endif
+
+        if (queue_rx->used_idx != queue_rx->used->idx)
+        {
+            rt_hw_dsb();
+
+            id = queue_rx->used->ring[queue_rx->used_idx % queue_rx->num].id;
+            len = queue_rx->used->ring[queue_rx->used_idx % queue_rx->num].len;
+
+            if (port_dev->parent.rx_indicate != RT_NULL && port_dev->rx_notify)
+            {
+            #ifdef RT_USING_SMP
+                rt_spin_unlock_irqrestore(&port_dev->spinlock_rx, level);
+            #endif
+                /* rx_indicate call virtio_console_port_read to inc used_idx */
+                port_dev->parent.rx_indicate(&port_dev->parent, len);
+
+            #ifdef RT_USING_SMP
+                level = rt_spin_lock_irqsave(&port_dev->spinlock_rx);
+            #endif
+            }
+            else
+            {
+                queue_rx->used_idx++;
+
+                virtio_submit_chain(virtio_dev, queue_rx_index, id);
+
+                virtio_queue_notify(virtio_dev, queue_rx_index);
+            }
+        }
+
+#ifdef RT_USING_SMP
+        rt_spin_unlock_irqrestore(&port_dev->spinlock_rx, level);
+#endif
+    }
+}
+
+rt_err_t rt_virtio_console_init(rt_ubase_t *mmio_base, rt_uint32_t irq)
+{
+    int i;
+    rt_size_t queues_num;
+    static int dev_no = 0;
+    char dev_name[RT_NAME_MAX];
+    struct virtio_device *virtio_dev;
+    struct virtio_console_device *virtio_console_dev;
+
+    RT_ASSERT(RT_USING_VIRTIO_CONSOLE_PORT_MAX_NR > 0);
+
+    virtio_console_dev = rt_malloc(sizeof(struct virtio_console_device));
+
+    if (virtio_console_dev == RT_NULL)
+    {
+        goto _alloc_fail;
+    }
+
+    virtio_dev = &virtio_console_dev->virtio_dev;
+    virtio_dev->irq = irq;
+    virtio_dev->mmio_base = mmio_base;
+
+    virtio_console_dev->config = (struct virtio_console_config *)virtio_dev->mmio_config->config;
+
+#ifdef RT_USING_SMP
+    rt_spin_lock_init(&virtio_dev->spinlock);
+#endif
+
+    virtio_reset_device(virtio_dev);
+    virtio_status_acknowledge_driver(virtio_dev);
+
+    virtio_dev->mmio_config->driver_features = virtio_dev->mmio_config->device_features & ~(
+            (1 << VIRTIO_F_RING_EVENT_IDX) |
+            (1 << VIRTIO_F_RING_INDIRECT_DESC));
+
+    virtio_status_driver_ok(virtio_dev);
+
+    if (!virtio_has_feature(virtio_dev, VIRTIO_CONSOLE_F_MULTIPORT))
+    {
+        virtio_console_dev->max_port_nr = 1;
+        queues_num = 2;
+    }
+    else
+    {
+        if (virtio_console_dev->config->max_nr_ports > RT_USING_VIRTIO_CONSOLE_PORT_MAX_NR)
+        {
+            virtio_console_dev->max_port_nr = RT_USING_VIRTIO_CONSOLE_PORT_MAX_NR;
+            virtio_console_dev->config->max_nr_ports = virtio_console_dev->max_port_nr;
+        }
+        else
+        {
+            virtio_console_dev->max_port_nr = virtio_console_dev->config->max_nr_ports;
+        }
+
+        queues_num = VIRTIO_CONSOLE_PORT_QUEUE_INDEX(virtio_console_dev->max_port_nr, VIRTIO_CONSOLE_QUEUE_DATA_RX);
+    }
+
+    if (virtio_queues_alloc(virtio_dev, queues_num) != RT_EOK)
+    {
+        goto _alloc_fail;
+    }
+
+    for (i = 0; i < virtio_dev->queues_num; ++i)
+    {
+        if (virtio_queue_init(virtio_dev, i, VIRTIO_CONSOLE_QUEUE_SIZE) != RT_EOK)
+        {
+            for (; i >= 0; --i)
+            {
+                virtio_queue_destroy(virtio_dev, i);
+            }
+            goto _alloc_fail;
+        }
+    }
+
+    virtio_console_dev->parent.type = RT_Device_Class_Char;
+    virtio_console_dev->parent.ops  = &virtio_console_ops;
+
+    virtio_console_dev->parent.rx_indicate = RT_NULL;
+    virtio_console_dev->parent.tx_complete = RT_NULL;
+
+    virtio_console_dev->console_id = dev_no;
+    virtio_console_dev->port_nr = 0;
+    rt_list_init(&virtio_console_dev->port_head);
+
+    rt_snprintf(dev_name, RT_NAME_MAX, "virtio-console%d", dev_no++);
+
+    rt_hw_interrupt_install(irq, virtio_console_isr, virtio_console_dev, dev_name);
+    rt_hw_interrupt_umask(irq);
+
+    return rt_device_register((rt_device_t)virtio_console_dev, dev_name, RT_DEVICE_FLAG_RDWR | RT_DEVICE_FLAG_INT_RX);
+
+_alloc_fail:
+
+    if (virtio_console_dev != RT_NULL)
+    {
+        virtio_queues_free(virtio_dev);
+        rt_free(virtio_console_dev);
+    }
+    return -RT_ENOMEM;
+}
+#endif /* BSP_USING_VIRTIO_CONSOLE */

bsp/qemu-virt64-aarch64/driver/virtio/virtio_console.h

+/*
+ * Copyright (c) 2006-2021, RT-Thread Development Team
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Change Logs:
+ * Date           Author       Notes
+ * 2021-11-11     GuEe-GUI     the first version
+ */
+
+#ifndef __VIRTIO_CONSOLE_H__
+#define __VIRTIO_CONSOLE_H__
+
+#include <rtdef.h>
+
+#include <virtio.h>
+
+#ifndef RT_USING_VIRTIO_CONSOLE_PORT_MAX_NR
+#define RT_USING_VIRTIO_CONSOLE_PORT_MAX_NR 4
+#endif
+
+#define VIRTIO_CONSOLE_QUEUE_DATA_RX    0
+#define VIRTIO_CONSOLE_QUEUE_DATA_TX    1
+#define VIRTIO_CONSOLE_QUEUE_CTRL_RX    2
+#define VIRTIO_CONSOLE_QUEUE_CTRL_TX    3
+#define VIRTIO_CONSOLE_QUEUE_SIZE       64
+
+/* Every port has data rx & tx, and port0 has ctrl rx & tx in multiport */
+#define VIRTIO_CONSOLE_PORT_QUEUE_INDEX(id, queue)  ((id) * 2 + (!!(id)) * 2 + (queue))
+
+#define VIRTIO_CONSOLE_PORT_BAD_ID      (~(rt_uint32_t)0)
+
+#define VIRTIO_CONSOLE_F_SIZE           0   /* Does host provide console size? */
+#define VIRTIO_CONSOLE_F_MULTIPORT      1   /* Does host provide multiple ports? */
+#define VIRTIO_CONSOLE_F_EMERG_WRITE    2   /* Does host support emergency write? */
+
+struct virtio_console_config
+{
+    rt_uint16_t cols;
+    rt_uint16_t rows;
+    rt_uint32_t max_nr_ports;
+    rt_uint32_t emerg_wr;
+} __attribute__((packed));
+
+struct virtio_console_control
+{
+    rt_uint32_t id;     /* Port number */
+    rt_uint16_t event;  /* The kind of control event */
+    rt_uint16_t value;  /* Extra information for the event */
+};
+
+enum virtio_console_control_event
+{
+    VIRTIO_CONSOLE_DEVICE_READY = 0,
+    VIRTIO_CONSOLE_PORT_ADD,
+    VIRTIO_CONSOLE_PORT_REMOVE,
+    VIRTIO_CONSOLE_PORT_READY,
+    VIRTIO_CONSOLE_CONSOLE_PORT,
+    VIRTIO_CONSOLE_RESIZE,
+    VIRTIO_CONSOLE_PORT_OPEN,
+    VIRTIO_CONSOLE_PORT_NAME,
+};
+
+struct virtio_console_resize
+{
+    rt_uint16_t cols;
+    rt_uint16_t rows;
+};
+
+struct virtio_console_device
+{
+    struct rt_device parent;
+
+    struct virtio_device virtio_dev;
+
+    rt_uint32_t console_id;
+    rt_size_t port_nr;
+    rt_size_t max_port_nr;
+    rt_list_t port_head;
+    struct virtio_console_config *config;
+
+    struct
+    {
+        struct virtio_console_control rx_ctrl, tx_ctrl;
+    } info[VIRTIO_CONSOLE_QUEUE_SIZE];
+};
+
+rt_err_t rt_virtio_console_init(rt_ubase_t *mmio_base, rt_uint32_t irq);
+
+enum
+{
+    VIRTIO_DEVICE_CTRL_CONSOLE_PORT_CREATE  = 0x20,
+    VIRTIO_DEVICE_CTRL_CONSOLE_PORT_DESTROY,
+};
+
+#endif /* __VIRTIO_CONSOLE_H__ */

bsp/qemu-virt64-aarch64/driver/virtio/virtio_gpu.c

@@ -74,6 +74,14 @@ static void virtio_gpu_ctrl_send_command(struct virtio_gpu_device *virtio_gpu_de
 
     while (virtio_alloc_desc_chain(virtio_dev, VIRTIO_GPU_QUEUE_CTRL, 2, idx))
     {
+#ifdef RT_USING_SMP
+        rt_spin_unlock_irqrestore(&virtio_dev->spinlock, level);
+#endif
+        rt_thread_yield();
+
+#ifdef RT_USING_SMP
+        level = rt_spin_lock_irqsave(&virtio_dev->spinlock);
+#endif
     }
 
     rt_hw_dsb();
@@ -126,7 +134,17 @@ static void virtio_gpu_cursor_send_command(struct virtio_gpu_device *virtio_gpu_
     rt_base_t level = rt_spin_lock_irqsave(&virtio_dev->spinlock);
 #endif
 
-    id = virtio_alloc_desc(virtio_dev, VIRTIO_GPU_QUEUE_CURSOR);
+    while ((id = virtio_alloc_desc(virtio_dev, VIRTIO_GPU_QUEUE_CURSOR)) == VIRTQ_INVALID_DESC_ID)
+    {
+#ifdef RT_USING_SMP
+        rt_spin_unlock_irqrestore(&virtio_dev->spinlock, level);
+#endif
+        rt_thread_yield();
+
+#ifdef RT_USING_SMP
+        level = rt_spin_lock_irqsave(&virtio_dev->spinlock);
+#endif
+    }
 
     addr = &virtio_gpu_dev->info[id].cursor_cmd;
     virtio_gpu_dev->info[id].cursor_valid = RT_TRUE;
@@ -806,7 +824,6 @@ static void virtio_gpu_isr(int irqno, void *param)
         id = queue_ctrl->used->ring[queue_ctrl->used_idx % queue_ctrl->num].id;
 
         virtio_gpu_dev->info[id].ctrl_valid = RT_FALSE;
-        rt_thread_yield();
 
         queue_ctrl->used_idx++;
     }
@@ -817,7 +834,6 @@ static void virtio_gpu_isr(int irqno, void *param)
         id = queue_cursor->used->ring[queue_cursor->used_idx % queue_cursor->num].id;
 
         virtio_gpu_dev->info[id].cursor_valid = RT_FALSE;
-        rt_thread_yield();
 
         queue_cursor->used_idx++;
     }
@@ -866,6 +882,11 @@ rt_err_t rt_virtio_gpu_init(rt_ubase_t *mmio_base, rt_uint32_t irq)
 
     virtio_status_driver_ok(virtio_dev);
 
+    if (virtio_queues_alloc(virtio_dev, 2) != RT_EOK)
+    {
+        goto _alloc_fail;
+    }
+
     if (virtio_queue_init(virtio_dev, VIRTIO_GPU_QUEUE_CTRL, VIRTIO_GPU_QUEUE_SIZE) != RT_EOK)
     {
         goto _alloc_fail;
@@ -896,6 +917,7 @@ _alloc_fail:
 
     if (virtio_gpu_dev != RT_NULL)
     {
+        virtio_queues_free(virtio_dev);
         rt_free(virtio_gpu_dev);
     }
     return -RT_ENOMEM;

bsp/qemu-virt64-aarch64/driver/virtio/virtio_input.c

@@ -372,6 +372,11 @@ rt_err_t rt_virtio_input_init(rt_ubase_t *mmio_base, rt_uint32_t irq)
 
     virtio_status_driver_ok(virtio_dev);
 
+    if (virtio_queues_alloc(virtio_dev, 2) != RT_EOK)
+    {
+        goto _alloc_fail;
+    }
+
     if (virtio_queue_init(virtio_dev, VIRTIO_INPUT_QUEUE_EVENT, VIRTIO_INPUT_EVENT_QUEUE_SIZE) != RT_EOK)
     {
         goto _alloc_fail;
@@ -425,6 +430,7 @@ _alloc_fail:
 
     if (virtio_input_dev != RT_NULL)
     {
+        virtio_queues_free(virtio_dev);
         rt_free(virtio_input_dev);
     }
     return -RT_ENOMEM;

bsp/qemu-virt64-aarch64/driver/virtio/virtio_net.c

@@ -269,6 +269,11 @@ rt_err_t rt_virtio_net_init(rt_ubase_t *mmio_base, rt_uint32_t irq)
 
     virtio_status_driver_ok(virtio_dev);
 
+    if (virtio_queues_alloc(virtio_dev, 2) != RT_EOK)
+    {
+        goto _alloc_fail;
+    }
+
     if (virtio_queue_init(virtio_dev, VIRTIO_NET_QUEUE_RX, VIRTIO_NET_RTX_QUEUE_SIZE) != RT_EOK)
     {
         goto _alloc_fail;
@@ -301,6 +306,7 @@ _alloc_fail:
 
     if (virtio_net_dev != RT_NULL)
     {
+        virtio_queues_free(virtio_dev);
         rt_free(virtio_net_dev);
     }
     return -RT_ENOMEM;

bsp/qemu-virt64-aarch64/qemu-graphic.bat

@@ -9,4 +9,5 @@ qemu-system-aarch64 -M virt,gic-version=2 -cpu cortex-a53 -smp 4 -kernel rtthrea
 -device virtio-gpu-device,xres=800,yres=600,bus=virtio-mmio-bus.2 ^
 -device virtio-keyboard-device,bus=virtio-mmio-bus.3 ^
 -device virtio-mouse-device,bus=virtio-mmio-bus.4 ^
--device virtio-tablet-device,bus=virtio-mmio-bus.5
+-device virtio-tablet-device,bus=virtio-mmio-bus.5 ^
+-device virtio-serial-device -chardev socket,host=127.0.0.1,port=4321,server=on,wait=off,telnet=on,id=console0 -device virtserialport,chardev=console0

bsp/qemu-virt64-aarch64/qemu-graphic.sh

@@ -7,4 +7,5 @@ qemu-system-aarch64 -M virt,gic-version=2 -cpu cortex-a53 -smp 4 -kernel rtthrea
 -device virtio-gpu-device,xres=800,yres=600,bus=virtio-mmio-bus.2 \
 -device virtio-keyboard-device,bus=virtio-mmio-bus.3 \
 -device virtio-mouse-device,bus=virtio-mmio-bus.4 \
--device virtio-tablet-device,bus=virtio-mmio-bus.5
+-device virtio-tablet-device,bus=virtio-mmio-bus.5 \
+-device virtio-serial-device -chardev socket,host=127.0.0.1,port=4321,server=on,wait=off,telnet=on,id=console0 -device virtserialport,chardev=console0

bsp/qemu-virt64-aarch64/qemu.bat

@@ -5,4 +5,5 @@ qemu-img create -f raw sd.bin 64M
 :run
 qemu-system-aarch64 -M virt,gic-version=2 -cpu cortex-a53 -smp 4 -kernel rtthread.elf -nographic ^
 -drive if=none,file=sd.bin,format=raw,id=blk0 -device virtio-blk-device,drive=blk0,bus=virtio-mmio-bus.0 ^
--netdev user,id=net0 -device virtio-net-device,netdev=net0,bus=virtio-mmio-bus.1
+-netdev user,id=net0 -device virtio-net-device,netdev=net0,bus=virtio-mmio-bus.1 ^
+-device virtio-serial-device -chardev socket,host=127.0.0.1,port=4321,server=on,wait=off,telnet=on,id=console0 -device virtserialport,chardev=console0

bsp/qemu-virt64-aarch64/qemu.sh

@@ -3,4 +3,5 @@ dd if=/dev/zero of=sd.bin bs=1024 count=65536
 fi
 qemu-system-aarch64 -M virt,gic-version=2 -cpu cortex-a53 -smp 4 -kernel rtthread.elf -nographic \
 -drive if=none,file=sd.bin,format=raw,id=blk0 -device virtio-blk-device,drive=blk0,bus=virtio-mmio-bus.0 \
--netdev user,id=net0 -device virtio-net-device,netdev=net0,bus=virtio-mmio-bus.1
+-netdev user,id=net0 -device virtio-net-device,netdev=net0,bus=virtio-mmio-bus.1 \
+-device virtio-serial-device -chardev socket,host=127.0.0.1,port=4321,server=on,wait=off,telnet=on,id=console0 -device virtserialport,chardev=console0

bsp/qemu-virt64-aarch64/rtconfig.h

@@ -241,9 +241,6 @@
 /* enhanced kernel services */
 
 
-/* POSIX extension functions */
-
-
 /* acceleration: Assembly language or algorithmic acceleration packages */
 
 
@@ -276,8 +273,10 @@
 #define RT_USING_UART0
 #define BSP_USING_RTC
 #define BSP_USING_ALARM
+#define BSP_USING_PIN
 #define BSP_USING_VIRTIO_BLK
 #define BSP_USING_VIRTIO_NET
+#define BSP_USING_VIRTIO_CONSOLE
 #define BSP_USING_VIRTIO_GPU
 #define BSP_USING_VIRTIO_INPUT
 #define BSP_USING_GIC