From 8fabbb008c1a31c809a3d97f22351f141a2bd02d Mon Sep 17 00:00:00 2001 From: Amaury Pouly Date: Thu, 4 Aug 2016 17:06:11 +0100 Subject: hwstub: add support for coprocessor operations At the moment the stub only implement them for MIPS. Change-Id: Ica835a0e9c70fa5675c3d655eae986e812a47de8 --- utils/hwstub/include/hwstub.hpp | 18 +++ utils/hwstub/include/hwstub_net.hpp | 2 + utils/hwstub/include/hwstub_protocol.h | 32 +++++- utils/hwstub/include/hwstub_usb.hpp | 4 + utils/hwstub/include/hwstub_virtual.hpp | 2 + utils/hwstub/lib/hwstub.cpp | 46 ++++++++ utils/hwstub/lib/hwstub_net.cpp | 12 ++ utils/hwstub/lib/hwstub_usb.cpp | 52 +++++++++ utils/hwstub/lib/hwstub_virtual.cpp | 12 ++ utils/hwstub/stub/main.c | 188 ++++++++++++++++++++++++++++++-- utils/hwstub/tools/hwstub_shell.cpp | 72 +++++++++++- 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 m_dev; /* pointer to device */ std::atomic 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)) @@ -251,6 +252,35 @@ struct hwstub_exec_req_t uint16_t bmFlags; } __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. 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 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 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 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(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"); -- cgit v1.2.3