hwstub: add support for coprocessor operations

At the moment the stub only implement them for MIPS.

Change-Id: Ica835a0e9c70fa5675c3d655eae986e812a47de8

11 files changed, 423 insertions, 17 deletions

diff --git a/utils/hwstub/include/hwstub.hpp b/utils/hwstub/include/hwstub.hpp
index deac976240..e403857200 100644
--- a/utils/hwstub/include/hwstub.hpp
+++ b/utils/hwstub/include/hwstub.hpp
@@ -62,6 +62,8 @@ enum class error
     NET_ERROR, /** Network error */
     TIMEOUT, /** Operation timed out */
     OVERFLW, /** Operation stopped to prevent buffer overflow */
+    UNIMPLEMENTED, /** Operation has not been implemented */
+    UNSUPPORTED, /** Operation is not supported by device/protocol */
 };
 
 /** Return a string explaining the error */
@@ -266,6 +268,15 @@ public:
      *       according to the buffer size and call read_dev() and write_dev() */
     error read(uint32_t addr, void *buf, size_t& sz, bool atomic);
     error write(uint32_t addr, const void *buf, size_t& sz, bool atomic);
+    /** Execute a general cop operation: if out_data is not null, data is appended to header,
+     * if in_data is not null, a read operation follows to retrieve some data.
+     * The in_data parameters is updated to reflect the number of transfered bytes */
+    error cop_op(uint8_t op, uint8_t args[HWSTUB_COP_ARGS], const void *out_data,
+        size_t out_size, void *in_data, size_t *in_size);
+    /** Execute a coprocessor read operation */
+    error read32_cop(uint8_t args[HWSTUB_COP_ARGS], uint32_t& value);
+    /** Execute a coprocessor write operation */
+    error write32_cop(uint8_t args[HWSTUB_COP_ARGS], uint32_t value);
     /** Get device buffer size: any read() or write() greater than this size
      * will be split into several transaction to avoid overflowing device
      * buffer. */
@@ -303,6 +314,11 @@ protected:
     virtual error get_dev_desc(uint16_t desc, void *buf, size_t& buf_sz) = 0;
     virtual error get_dev_log(void *buf, size_t& buf_sz) = 0;
     virtual error exec_dev(uint32_t addr, uint16_t flags) = 0;
+    /* cop operation: if out_data is not null, data is appended to header, if
+     * in_data is not null, a READ2 operation follows to retrieve some data
+     * The in_data parameters is updated to reflect the number of transfered bytes*/
+    virtual error cop_dev(uint8_t op, uint8_t args[HWSTUB_COP_ARGS], const void *out_data,
+        size_t out_size, void *in_data, size_t *in_size) = 0;
 
     std::shared_ptr<device> m_dev; /* pointer to device */
     std::atomic<int> m_refcnt; /* reference count */
@@ -338,6 +354,8 @@ protected:
     virtual error get_dev_desc(uint16_t desc, void *buf, size_t& buf_sz);
     virtual error get_dev_log(void *buf, size_t& buf_sz);
     virtual error exec_dev(uint32_t addr, uint16_t flags);
+    virtual error cop_dev(uint8_t op, uint8_t args[HWSTUB_COP_ARGS], const void *out_data,
+        size_t out_size, void *in_data, size_t *in_size);
     virtual error status() const;
     virtual size_t get_buffer_size();
 
diff --git a/utils/hwstub/include/hwstub_net.hpp b/utils/hwstub/include/hwstub_net.hpp
index 2cf6e07ccb..ae0e09e920 100644
--- a/utils/hwstub/include/hwstub_net.hpp
+++ b/utils/hwstub/include/hwstub_net.hpp
@@ -184,6 +184,8 @@ protected:
     virtual error get_dev_desc(uint16_t desc, void *buf, size_t& buf_sz);
     virtual error get_dev_log(void *buf, size_t& buf_sz);
     virtual error exec_dev(uint32_t addr, uint16_t flags);
+    virtual error cop_dev(uint8_t op, uint8_t args[HWSTUB_COP_ARGS], const void *out_data,
+        size_t out_size, void *in_data, size_t *in_size);
     virtual error status() const;
     virtual size_t get_buffer_size();
 
diff --git a/utils/hwstub/include/hwstub_protocol.h b/utils/hwstub/include/hwstub_protocol.h
index f767e50571..ed26ee78e4 100644
--- a/utils/hwstub/include/hwstub_protocol.h
+++ b/utils/hwstub/include/hwstub_protocol.h
@@ -33,7 +33,7 @@
  */
 
 #define HWSTUB_VERSION_MAJOR    4
-#define HWSTUB_VERSION_MINOR    2
+#define HWSTUB_VERSION_MINOR    3
 
 #define HWSTUB_VERSION__(maj, min) #maj"."#min
 #define HWSTUB_VERSION_(maj, min) HWSTUB_VERSION__(maj, min)
@@ -169,6 +169,7 @@ struct hwstub_net_hdr_t
 #define HWSTUB_EXEC                 0x44
 #define HWSTUB_READ2_ATOMIC         0x45
 #define HWSTUB_WRITE_ATOMIC         0x46
+#define HWSTUB_COPROCESSOR_OP       0x47
 
 /* the following commands and the ACK/NACK mechanism are net only */
 #define HWSERVER_ACK(n) (0x100|(n))
@@ -252,6 +253,35 @@ struct hwstub_exec_req_t
 } __attribute__((packed));
 
 /**
+ * HWSTUB_COPROCESSOR_OP
+ * Execute a coprocessor operation. The operation is describe in the header of
+ * the structure. There are currently two supported operations:
+ * - read: following the HWSTUB_COPROCESSOR_OP, the host can retrieve the data
+ *         by sending a HWSTUB_READ2 request (just like a regular read) of
+ *         the appropriate size
+ * - write: the header is followed by the data to write.
+ * If the request has two parts (second being READ2) then both requests must use
+ * the same ID in wValue, otherwise the second request will be STALLed.
+ * If a particular operation is not supported, it must be STALLed by the device.
+ */
+
+#define HWSTUB_COP_READ     0 /* read operation */
+#define HWSTUB_COP_WRITE    1 /* write operation */
+/* for MIPS */
+#define HWSTUB_COP_MIPS_COP 0 /* coprocessor number */
+#define HWSTUB_COP_MIPS_REG 1 /* coprocessor register */
+#define HWSTUB_COP_MIPS_SEL 2 /* coprocessor select */
+
+#define HWSTUB_COP_ARGS     7 /* maximum number of arguments */
+
+struct hwstub_cop_req_t
+{
+    uint8_t bOp; /* operation to execute */
+    uint8_t bArgs[HWSTUB_COP_ARGS]; /* arguments to the operation */
+    /* followed by data for WRITE operation */
+};
+
+/**
  * HWSERVER_HELLO:
  * Say hello to the server, give protocol version and get server version.
  * Send: args[0] = major << 8 | minor, no data
diff --git a/utils/hwstub/include/hwstub_usb.hpp b/utils/hwstub/include/hwstub_usb.hpp
index 6a9d4d8798..15a0fcaaec 100644
--- a/utils/hwstub/include/hwstub_usb.hpp
+++ b/utils/hwstub/include/hwstub_usb.hpp
@@ -132,6 +132,8 @@ protected:
     virtual error get_dev_desc(uint16_t desc, void *buf, size_t& buf_sz);
     virtual error get_dev_log(void *buf, size_t& buf_sz);
     virtual error exec_dev(uint32_t addr, uint16_t flags);
+    virtual error cop_dev(uint8_t op, uint8_t args[HWSTUB_COP_ARGS], const void *out_data,
+        size_t out_size, void *in_data, size_t *in_size);
     virtual error status() const;
     virtual size_t get_buffer_size();
     /* Probe a device to check if it is an hwstub device and return the interface
@@ -162,6 +164,8 @@ protected:
     virtual error get_dev_desc(uint16_t desc, void *buf, size_t& buf_sz);
     virtual error get_dev_log(void *buf, size_t& buf_sz);
     virtual error exec_dev(uint32_t addr, uint16_t flags);
+    virtual error cop_dev(uint8_t op, uint8_t args[HWSTUB_COP_ARGS], const void *out_data,
+        size_t out_size, void *in_data, size_t *in_size);
     virtual error status() const;
     virtual size_t get_buffer_size();
     error probe();
diff --git a/utils/hwstub/include/hwstub_virtual.hpp b/utils/hwstub/include/hwstub_virtual.hpp
index d35f98e0ec..1e35378840 100644
--- a/utils/hwstub/include/hwstub_virtual.hpp
+++ b/utils/hwstub/include/hwstub_virtual.hpp
@@ -147,6 +147,8 @@ protected:
     virtual error get_dev_desc(uint16_t desc, void *buf, size_t& buf_sz);
     virtual error get_dev_log(void *buf, size_t& buf_sz);
     virtual error exec_dev(uint32_t addr, uint16_t flags);
+    virtual error cop_dev(uint8_t op, uint8_t args[HWSTUB_COP_ARGS], const void *out_data,
+        size_t out_size, void *in_data, size_t *in_size);
     virtual error status() const;
     virtual size_t get_buffer_size();
 
diff --git a/utils/hwstub/lib/hwstub.cpp b/utils/hwstub/lib/hwstub.cpp
index 9dd2915903..5e708c3bb8 100644
--- a/utils/hwstub/lib/hwstub.cpp
+++ b/utils/hwstub/lib/hwstub.cpp
@@ -45,6 +45,8 @@ std::string error_string(error err)
         case error::PROTOCOL_ERROR: return "network protocol error";
         case error::TIMEOUT: return "timeout";
         case error::OVERFLW: return "overflow";
+        case error::UNIMPLEMENTED: return "operation is not implemented";
+        case error::UNSUPPORTED: return "operation unsupported";
         default: return "unknown error";
     }
 }
@@ -457,6 +459,38 @@ error handle::write(uint32_t addr, const void *buf, size_t& sz, bool atomic)
     return error::SUCCESS;
 }
 
+error handle::cop_op(uint8_t op, uint8_t args[HWSTUB_COP_ARGS], const void *out_data,
+    size_t out_size, void *in_data, size_t *in_size)
+{
+    std::unique_lock<std::recursive_mutex> lock(m_mutex);
+    /* get a pointer so that it's not destroyed during the runtime of the function,
+     * the pointer will be released at the end of the function */
+    std::shared_ptr<context> ctx = m_dev->get_context();
+    if(!ctx)
+        return error::NO_CONTEXT;
+    /* ensure valid status */
+    error err = status();
+    if(err != error::SUCCESS)
+        return err;
+    return cop_dev(op, args, out_data, out_size, in_data, in_size);
+}
+
+error handle::read32_cop(uint8_t args[HWSTUB_COP_ARGS], uint32_t& value)
+{
+    size_t sz = sizeof(value);
+    error err = cop_op(HWSTUB_COP_READ, args, nullptr, 0, &value, &sz);
+    if(err != error::SUCCESS)
+        return err;
+    if(sz != sizeof(value))
+        return error::ERROR;
+    return error::SUCCESS;
+}
+
+error handle::write32_cop(uint8_t args[HWSTUB_COP_ARGS], uint32_t value)
+{
+    return cop_op(HWSTUB_COP_WRITE, args, &value, sizeof(value), nullptr, nullptr);
+}
+
 error handle::status() const
 {
     /* check context */
@@ -616,6 +650,18 @@ error dummy_handle::exec_dev(uint32_t addr, uint16_t flags)
     return error::DUMMY;
 }
 
+error dummy_handle::cop_dev(uint8_t op, uint8_t args[HWSTUB_COP_ARGS],
+    const void *out_data, size_t out_size, void *in_data, size_t *in_size)
+{
+    (void) op;
+    (void) args;
+    (void) out_data;
+    (void) out_size;
+    (void) in_data;
+    (void) in_size;
+    return error::DUMMY;
+}
+
 error dummy_handle::status() const
 {
     error err = handle::status();
diff --git a/utils/hwstub/lib/hwstub_net.cpp b/utils/hwstub/lib/hwstub_net.cpp
index ddafea6351..c9d201a761 100644
--- a/utils/hwstub/lib/hwstub_net.cpp
+++ b/utils/hwstub/lib/hwstub_net.cpp
@@ -735,6 +735,18 @@ error handle::exec_dev(uint32_t addr, uint16_t flags)
     return error::SUCCESS;
 }
 
+error handle::cop_dev(uint8_t op, uint8_t args[HWSTUB_COP_ARGS],
+    const void *out_data, size_t out_size, void *in_data, size_t *in_size)
+{
+    (void) op;
+    (void) args;
+    (void) out_data;
+    (void) out_size;
+    (void) in_data;
+    (void) in_size;
+    return error::UNIMPLEMENTED;
+}
+
 error handle::status() const
 {
     return hwstub::handle::status();
diff --git a/utils/hwstub/lib/hwstub_usb.cpp b/utils/hwstub/lib/hwstub_usb.cpp
index 6bb1cfa049..37249f5812 100644
--- a/utils/hwstub/lib/hwstub_usb.cpp
+++ b/utils/hwstub/lib/hwstub_usb.cpp
@@ -399,6 +399,46 @@ error rb_handle::write_dev(uint32_t addr, const void *buf, size_t& sz, bool atom
     return ret;
 }
 
+error rb_handle::cop_dev(uint8_t op, uint8_t args[HWSTUB_COP_ARGS],
+    const void *out_data, size_t out_size, void *in_data, size_t *in_size)
+{
+    (void) op;
+    (void) args;
+    (void) out_data;
+    (void) out_size;
+    (void) in_data;
+    (void) in_size;
+    std::shared_ptr<hwstub::context> hctx = get_device()->get_context();
+    if(!hctx)
+        return error::NO_CONTEXT;
+
+    /* construct out request: header followed by (optional) data */
+    size_t hdr_sz = sizeof(struct hwstub_cop_req_t);
+    uint8_t *tmp_buf = new uint8_t[out_size + hdr_sz];
+    struct hwstub_cop_req_t *req = reinterpret_cast<struct hwstub_cop_req_t *>(tmp_buf);
+    req->bOp = op;
+    for(int i = 0; i < HWSTUB_COP_ARGS; i++)
+        req->bArgs[i] = args[i];
+    if(out_size > 0)
+        memcpy(tmp_buf + hdr_sz, out_data, out_size);
+    error ret = interpret_libusb_error(libusb_control_transfer(m_handle,
+        LIBUSB_REQUEST_TYPE_CLASS | LIBUSB_RECIPIENT_INTERFACE | LIBUSB_ENDPOINT_OUT,
+        HWSTUB_COPROCESSOR_OP, m_transac_id++, m_intf,
+        (unsigned char *)req, out_size + hdr_sz, m_timeout), out_size + hdr_sz);
+    delete[] tmp_buf;
+    /* return errors if any */
+    if(ret != error::SUCCESS)
+        return ret;
+    /* return now if there is no read stage */
+    if(in_data == nullptr)
+        return error::SUCCESS;
+    /* perform read stage (use the same transaction ID) */
+    return interpret_libusb_size(libusb_control_transfer(m_handle,
+        LIBUSB_REQUEST_TYPE_CLASS | LIBUSB_RECIPIENT_INTERFACE | LIBUSB_ENDPOINT_IN,
+        HWSTUB_READ2, m_transac_id - 1, m_intf,
+        (unsigned char *)in_data, *in_size, m_timeout), *in_size);
+}
+
 bool rb_handle::find_intf(struct libusb_device_descriptor *dev,
     struct libusb_config_descriptor *config, int& intf_idx)
 {
@@ -674,6 +714,18 @@ error jz_handle::write_dev(uint32_t addr, const void *buf, size_t& sz, bool atom
     return ret;
 }
 
+error jz_handle::cop_dev(uint8_t op, uint8_t args[HWSTUB_COP_ARGS],
+    const void *out_data, size_t out_size, void *in_data, size_t *in_size)
+{
+    (void) op;
+    (void) args;
+    (void) out_data;
+    (void) out_size;
+    (void) in_data;
+    (void) in_size;
+    return error::UNSUPPORTED;
+}
+
 bool jz_handle::is_boot_dev(struct libusb_device_descriptor *dev,
     struct libusb_config_descriptor *config)
 {
diff --git a/utils/hwstub/lib/hwstub_virtual.cpp b/utils/hwstub/lib/hwstub_virtual.cpp
index fada56fb83..5c9e79e5a1 100644
--- a/utils/hwstub/lib/hwstub_virtual.cpp
+++ b/utils/hwstub/lib/hwstub_virtual.cpp
@@ -221,6 +221,18 @@ error handle::exec_dev(uint32_t addr, uint16_t flags)
     return p ? p->exec_dev(addr, flags) : error::DISCONNECTED;
 }
 
+error handle::cop_dev(uint8_t op, uint8_t args[HWSTUB_COP_ARGS],
+    const void *out_data, size_t out_size, void *in_data, size_t *in_size)
+{
+    (void) op;
+    (void) args;
+    (void) out_data;
+    (void) out_size;
+    (void) in_data;
+    (void) in_size;
+    return error::UNSUPPORTED;
+}
+
 error handle::status() const
 {
     return hwstub::handle::status();
diff --git a/utils/hwstub/stub/main.c b/utils/hwstub/stub/main.c
index c35872f320..3eee8b6a18 100644
--- a/utils/hwstub/stub/main.c
+++ b/utils/hwstub/stub/main.c
@@ -400,10 +400,12 @@ static bool read_atomic(void *dst, void *src, size_t sz)
     }
 }
 
+static void *last_read_addr = 0;
+static uint16_t last_read_id = 0xffff;
+static size_t last_read_max_size = 0;
+
 static void handle_read(struct usb_ctrlrequest *req)
 {
-    static uint32_t last_addr = 0;
-    static uint16_t last_id = 0xffff;
     uint16_t id = req->wValue;
 
     if(req->bRequest == HWSTUB_READ)
@@ -413,26 +415,29 @@ static void handle_read(struct usb_ctrlrequest *req)
             return usb_drv_stall(EP_CONTROL, true, true);
         asm volatile("nop" : : : "memory");
         struct hwstub_read_req_t *read = (void *)usb_buffer;
-        last_addr = read->dAddress;
-        last_id = id;
+        last_read_addr = (void *)read->dAddress;
+        last_read_max_size = usb_buffer_size;
+        last_read_id = id;
         usb_drv_send(EP_CONTROL, NULL, 0);
     }
     else
     {
-        if(id != last_id)
+        /* NOTE: READ2 is also called after a coprocessor operation */
+        if(id != last_read_id)
             return usb_drv_stall(EP_CONTROL, true, true);
+        size_t len = MIN(req->wLength, last_read_max_size);
 
         if(req->bRequest == HWSTUB_READ2_ATOMIC)
         {
             if(set_data_abort_jmp() == 0)
             {
-                if(!read_atomic(usb_buffer, (void *)last_addr, req->wLength))
+                if(!read_atomic(usb_buffer, last_read_addr, len))
                     return usb_drv_stall(EP_CONTROL, true, true);
             }
             else
             {
-                logf("trapped read data abort in [0x%x,0x%x]\n", last_addr,
-                    last_addr + req->wLength);
+                logf("trapped read data abort in [0x%x,0x%x]\n", last_read_addr,
+                    last_read_addr + len);
                 return usb_drv_stall(EP_CONTROL, true, true);
             }
         }
@@ -440,19 +445,19 @@ static void handle_read(struct usb_ctrlrequest *req)
         {
             if(set_data_abort_jmp() == 0)
             {
-                memcpy(usb_buffer, (void *)last_addr, req->wLength);
+                memcpy(usb_buffer, last_read_addr, len);
                 asm volatile("nop" : : : "memory");
             }
             else
             {
-                logf("trapped read data abort in [0x%x,0x%x]\n", last_addr,
-                    last_addr + req->wLength);
+                logf("trapped read data abort in [0x%x,0x%x]\n", last_read_addr,
+                    last_read_addr + len);
                 return usb_drv_stall(EP_CONTROL, true, true);
             }
 
         }
 
-        usb_drv_send(EP_CONTROL, usb_buffer, req->wLength);
+        usb_drv_send(EP_CONTROL, usb_buffer, len);
         usb_drv_recv(EP_CONTROL, NULL, 0);
     }
 }
@@ -562,6 +567,163 @@ static void handle_exec(struct usb_ctrlrequest *req)
     }
 }
 
+#ifdef CPU_MIPS
+static uint32_t rw_cp0_inst_buffer[3];
+typedef uint32_t (*read_cp0_inst_buffer_fn_t)(void);
+typedef void (*write_cp0_inst_buffer_fn_t)(uint32_t);
+
+uint32_t mips_read_cp0(unsigned reg, unsigned sel)
+{
+    /* ok this is tricky because the coprocessor read instruction encoding
+     * contains the register and select, so we need to generate the instruction
+     * on the fly, we generate a "function like" buffer with three instructions:
+     * mfc0 v0, reg, sel
+     * jr ra
+     * nop
+     */
+    rw_cp0_inst_buffer[0] = 0x40000000 | /*v0*/2 << 16 | (sel & 0x7) | (reg & 0x1f) << 11;
+    rw_cp0_inst_buffer[1] = /*ra*/31 << 21 | 0x8; /* jr ra */
+    rw_cp0_inst_buffer[2] = 0; /* nop */
+#ifdef CONFIG_FLUSH_CACHES
+    target_flush_caches();
+#endif
+    read_cp0_inst_buffer_fn_t fn = (read_cp0_inst_buffer_fn_t)rw_cp0_inst_buffer;
+    return fn();
+}
+
+void mips_write_cp0(unsigned reg, unsigned sel, uint32_t val)
+{
+    /* ok this is tricky because the coprocessor write instruction encoding
+     * contains the register and select, so we need to generate the instruction
+     * on the fly, we generate a "function like" buffer with three instructions:
+     * mtc0 a0, reg, sel
+     * jr ra
+     * nop
+     */
+    rw_cp0_inst_buffer[0] = 0x40800000 | /*a0*/4 << 16 | (sel & 0x7) | (reg & 0x1f) << 11;
+    rw_cp0_inst_buffer[1] = /*ra*/31 << 21 | 0x8; /* jr ra */
+    rw_cp0_inst_buffer[2] = 0; /* nop */
+#ifdef CONFIG_FLUSH_CACHES
+    target_flush_caches();
+#endif
+    write_cp0_inst_buffer_fn_t fn = (write_cp0_inst_buffer_fn_t)rw_cp0_inst_buffer;
+    fn(val);
+}
+#endif
+
+/* coprocessor read: return <0 on error (-2 for dull dump), or size to return
+ * to host otherwise */
+int cop_read(uint8_t args[HWSTUB_COP_ARGS], void *out_data, size_t out_max_sz)
+{
+    /* virtually all targets do register-based operation, so 32-bit */
+    if(out_max_sz < 4)
+    {
+        logf("cop read failed: output buffer is too small\n");
+        return -1;
+    }
+#ifdef CPU_MIPS
+    if(args[HWSTUB_COP_MIPS_COP] != 0)
+    {
+        logf("cop read failed: only mips cp0 is supported\n");
+        return -2;
+    }
+    *(uint32_t *)out_data = mips_read_cp0(args[HWSTUB_COP_MIPS_REG],  args[HWSTUB_COP_MIPS_SEL]);
+    return 4;
+#else
+    (void) args;
+    (void) out_data;
+    (void) out_max_sz;
+    logf("cop read failed: unsupported cpu\n");
+    return -1;
+#endif
+}
+
+/* coprocessor write: return <0 on error (-2 for dull dump), or 0 on success */
+int cop_write(uint8_t args[HWSTUB_COP_ARGS], const void *in_data, size_t in_sz)
+{
+    /* virtually all targets do register-based operation, so 32-bit */
+    if(in_sz != 4)
+    {
+        logf("cop read failed: input buffer has wrong size\n");
+        return -1;
+    }
+#ifdef CPU_MIPS
+    if(args[HWSTUB_COP_MIPS_COP] != 0)
+    {
+        logf("cop read failed: only mips cp0 is supported\n");
+        return -2;
+    }
+    mips_write_cp0(args[HWSTUB_COP_MIPS_REG],  args[HWSTUB_COP_MIPS_SEL], *(uint32_t *)in_data);
+    return 0;
+#else
+    (void) args;
+    (void) in_data;
+    (void) in_sz;
+    logf("cop write failed: unsupported cpu\n");
+    return -1;
+#endif
+}
+
+/* return size to return to host or <0 on error */
+int do_cop_op(struct hwstub_cop_req_t *cop, void *in_data, size_t in_sz,
+    void *out_data, size_t out_max_sz)
+{
+    int ret = -2; /* -2 means full debug dump */
+    /* handle operations */
+    if(cop->bOp == HWSTUB_COP_READ)
+    {
+        /* read cannot have extra data */
+        if(in_sz > 0)
+            goto Lerr;
+        ret = cop_read(cop->bArgs, out_data, out_max_sz);
+    }
+    else if(cop->bOp == HWSTUB_COP_WRITE)
+    {
+        ret = cop_write(cop->bArgs, in_data, in_sz);
+    }
+
+Lerr:
+    if(ret == -2)
+    {
+        /* debug output */
+        logf("invalid cop op: %d, ", cop->bOp);
+        for(int i = 0; i < HWSTUB_COP_ARGS; i++)
+            logf("%c0x%x", i == 0 ? '[' : ',', cop->bArgs[i]);
+        logf("] in:%d\n", in_sz);
+    }
+    return ret;
+}
+
+static void handle_cop(struct usb_ctrlrequest *req)
+{
+    int size = usb_drv_recv(EP_CONTROL, usb_buffer, req->wLength);
+    int hdr_sz = sizeof(struct hwstub_cop_req_t);
+    asm volatile("nop" : : : "memory");
+    struct hwstub_cop_req_t *cop = (void *)usb_buffer;
+    /* request should at least contain the header */
+    if(size < hdr_sz)
+        return usb_drv_stall(EP_CONTROL, true, true);
+    /* perform coprocessor operation: put output buffer after the input one,
+     * limit output buffer size to maximum buffer size */
+    uint8_t *in_buf = usb_buffer + hdr_sz;
+    size_t in_sz = req->wLength - hdr_sz;
+    uint8_t *out_buf = in_buf + in_sz;
+    size_t out_max_sz = usb_buffer_size - req->wLength;
+    int ret = do_cop_op(cop, in_buf, in_sz, out_buf, out_max_sz);
+    /* STALL on error */
+    if(ret < 0)
+        return usb_drv_stall(EP_CONTROL, true, true);
+    /* acknowledge */
+    usb_drv_send(EP_CONTROL, NULL, 0);
+    /* if there is a read stage, prepare everything for the READ2 */
+    if(ret > 0)
+    {
+        last_read_id = req->wValue;
+        last_read_addr = out_buf;
+        last_read_max_size = ret;
+    }
+}
+
 static void handle_class_intf_req(struct usb_ctrlrequest *req)
 {
     unsigned intf = req->wIndex & 0xff;
@@ -581,6 +743,8 @@ static void handle_class_intf_req(struct usb_ctrlrequest *req)
             return handle_write(req);
         case HWSTUB_EXEC:
             return handle_exec(req);
+        case HWSTUB_COPROCESSOR_OP:
+            return handle_cop(req);
         default:
             usb_drv_stall(EP_CONTROL, true, true);
     }
diff --git a/utils/hwstub/tools/hwstub_shell.cpp b/utils/hwstub/tools/hwstub_shell.cpp
index b2cf2cec60..336b7fed6b 100644
--- a/utils/hwstub/tools/hwstub_shell.cpp
+++ b/utils/hwstub/tools/hwstub_shell.cpp
@@ -310,9 +310,11 @@ int my_lua_call(lua_State *state)
 {
     int n = lua_gettop(state);
     if(n != 1)
-        luaL_error(state, "call takes target address argument");
+        luaL_error(state, "call takes one argument: target address");
 
-    g_hwdev->exec(mylua_checkunsigned(state, 1), HWSTUB_EXEC_CALL);
+    error ret = g_hwdev->exec(luaL_checkunsigned(state, 1), HWSTUB_EXEC_CALL);
+    if(ret != error::SUCCESS)
+        luaL_error(state, "call failed");
     return 0;
 }
 
@@ -320,9 +322,67 @@ int my_lua_jump(lua_State *state)
 {
     int n = lua_gettop(state);
     if(n != 1)
-        luaL_error(state, "jump takes target address argument");
+        luaL_error(state, "jump takes one argument: target address");
+
+    error ret = g_hwdev->exec(luaL_checkunsigned(state, 1), HWSTUB_EXEC_JUMP);
+    if(ret != error::SUCCESS)
+        luaL_error(state, "jump failed");
+    return 0;
+}
+
+int my_lua_read32_cop(lua_State *state)
+{
+    int n = lua_gettop(state);
+    if(n != 1)
+        luaL_error(state, "read32_cop takes one argument: arguments (array)");
+    uint8_t args[HWSTUB_COP_ARGS] = {0};
+    /* parse array */
+    luaL_checktype(state, 1, LUA_TTABLE);
+    size_t sz = lua_rawlen(state, 1);
+    if(sz > HWSTUB_COP_ARGS)
+        luaL_error(state, "coprocessor operation take at most %d arguments", HWSTUB_COP_ARGS);
+    for(size_t i = 0; i < sz; i++)
+    {
+        lua_pushinteger(state, i + 1); /* index start at 1 */
+        lua_gettable(state, 1);
+        /* check it is an integer */
+        args[i] = luaL_checkunsigned(state, -1);
+        /* pop it */
+        lua_pop(state, 1);
+    }
 
-   g_hwdev->exec(mylua_checkunsigned(state, 1), HWSTUB_EXEC_JUMP);
+    uint32_t val;
+    error ret = g_hwdev->read32_cop(args, val);
+    if(ret != error::SUCCESS)
+        luaL_error(state, "coprocessor read failed");
+    lua_pushunsigned(state, val);
+    return 1;
+}
+
+int my_lua_write32_cop(lua_State *state)
+{
+    int n = lua_gettop(state);
+    if(n != 2)
+        luaL_error(state, "write32_cop takes two arguments: arguments (array) and value");
+    uint8_t args[HWSTUB_COP_ARGS] = {0};
+    /* parse array */
+    luaL_checktype(state, 1, LUA_TTABLE);
+    size_t sz = lua_rawlen(state, 1);
+    if(sz > HWSTUB_COP_ARGS)
+        luaL_error(state, "coprocessor operation take at most %d arguments", HWSTUB_COP_ARGS);
+    for(size_t i = 0; i < sz; i++)
+    {
+        lua_pushinteger(state, i + 1); /* index start at 1 */
+        lua_gettable(state, 1);
+        /* check it is an integer */
+        args[i] = luaL_checkunsigned(state, -1);
+        /* pop it */
+        lua_pop(state, 1);
+    }
+
+    error ret = g_hwdev->write32_cop(args, luaL_checkunsigned(state, 2));
+    if(ret != error::SUCCESS)
+        luaL_error(state, "coprocessor write failed");
     return 0;
 }
 
@@ -753,6 +813,10 @@ bool my_lua_import_hwstub()
     lua_setfield(g_lua, -2, "call");
     lua_pushcclosure(g_lua, my_lua_jump, 0);
     lua_setfield(g_lua, -2, "jump");
+    lua_pushcclosure(g_lua, my_lua_read32_cop, 0);
+    lua_setfield(g_lua, -2, "read32_cop");
+    lua_pushcclosure(g_lua, my_lua_write32_cop, 0);
+    lua_setfield(g_lua, -2, "write32_cop");
 
     lua_setfield(g_lua, -2, "dev");