2023-12-01 09:17:09 +00:00
# include "usb_io.h"
# include <sys/fcntl.h>
# include <unistd.h>
# include <linux/usb/functionfs.h>
# include <thread>
# include <base/encrypt.h>
# include "usb_dev.h"
# include "default_cfg.h"
// #define TEST
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// async_usb_gadget
async_usb_gadget : : async_usb_gadget ( std : : function < FUNCTION_PROTO_COMMAND_HANDLE > cmd_handler
, std : : function < void ( bool ) > dev_conn )
: handle_cmd_ ( cmd_handler ) , dev_connect_ ( dev_conn )
, enc_head_ ( ENCRYPT_CMD_NONE ) , enc_payload_ ( ENCRYPT_NONE ) , enc_data_ ( 0 )
2023-12-13 06:57:08 +00:00
# ifdef MEM_POOL
, io_buf_ ( nullptr )
# else
, io_buf_ ( " IO-buf " )
# endif
, cmd_que_ ( " in-queue " ) , sent_que_ ( " out-queue " )
2023-12-02 02:15:44 +00:00
, wait_in_ ( " wait_usb_enable_in " ) , wait_out_ ( " wait_usb_enable_out " )
2023-12-01 09:17:09 +00:00
{
2023-12-09 10:21:05 +00:00
cmd_que_ . enable_wait_log ( false ) ;
sent_que_ . enable_wait_log ( false ) ;
2023-12-01 09:17:09 +00:00
dev_ = new usb_device ( " fe900000.dwc3 " , " /opt/cfg/usb_gadget/g1/UDC " , " linaro " ) ;
dev_ - > add_endpoint ( USB_EP_BULK_IN , true , true ) ;
dev_ - > add_endpoint ( USB_EP_BULK_OUT , true , false ) ;
2023-12-01 10:03:10 +00:00
if ( ( last_err_ = dev_ - > open_device ( USB_DEV , true ) ) )
2023-12-01 09:17:09 +00:00
{
dev_ - > release ( ) ;
dev_ = nullptr ;
run_ = false ;
return ;
}
auto ep0 = [ this ] ( void ) - > void
{
thread_read_ep0 ( ) ;
} ;
auto task = [ this ] ( void ) - > void
{
thread_pump_task ( ) ;
} ;
auto bulkw = [ this ] ( void ) - > void
{
2023-12-02 02:15:44 +00:00
while ( run_ )
2023-12-01 09:17:09 +00:00
{
2023-12-02 02:15:44 +00:00
wait_in_ . wait ( ) ;
if ( ! run_ )
break ;
int fd = - 1 ,
err = dev_ - > open_endpoint ( EP_IND_BULK_IN , & fd ) ;
if ( err = = 0 )
{
thread_write_bulk ( fd ) ;
2023-12-26 01:33:05 +00:00
if ( run_ )
dev_ - > close_endpoint ( EP_IND_BULK_IN ) ;
2023-12-02 02:15:44 +00:00
}
2023-12-01 09:17:09 +00:00
}
} ;
auto bulkr = [ this ] ( void ) - > void
{
2023-12-02 02:15:44 +00:00
while ( run_ )
2023-12-01 09:17:09 +00:00
{
2023-12-02 02:15:44 +00:00
wait_out_ . wait ( ) ;
if ( ! run_ )
break ;
int fd = - 1 ,
err = dev_ - > open_endpoint ( EP_IND_BULK_OUT , & fd ) ;
if ( err = = 0 )
{
thread_read_bulk ( fd ) ;
2023-12-26 01:33:05 +00:00
if ( run_ )
dev_ - > close_endpoint ( EP_IND_BULK_OUT ) ;
2023-12-02 02:15:44 +00:00
}
2023-12-01 09:17:09 +00:00
}
} ;
auto excep = [ & ] ( const char * thread_name ) - > void
{
threads_ . stop ( thread_name ) ;
// threads_.start(task, "thread_pump_task");
} ;
threads_ . set_exception_handler ( excep ) ;
2023-12-12 01:36:06 +00:00
unit_in_ = unit_out_ = sys_info : : page_size ; // dev_->get_max_packet();
// allocate 10MB for IO
2023-12-13 06:57:08 +00:00
# ifdef MEM_POOL
io_buf_ = new dyn_mem_pool ( 256 , unit_out_ ) ;
utils : : to_log ( LOG_LEVEL_DEBUG , " Prepare %u(%u * %u) for IO. \n " , io_buf_ - > count ( ) * unit_out_ , io_buf_ - > count ( ) , unit_in_ ) ;
# else
2023-12-12 01:36:06 +00:00
for ( int i = 0 ; i < SIZE_MB ( 10 ) / unit_out_ ; + + i )
{
dyn_mem_ptr buf = dyn_mem : : memory ( unit_out_ ) ;
if ( buf )
io_buf_ . save ( buf ) ;
}
utils : : to_log ( LOG_LEVEL_DEBUG , " Prepare %u(%u * %u) for IO. \n " , io_buf_ . size ( ) * unit_out_ , io_buf_ . size ( ) , unit_in_ ) ;
2023-12-13 06:57:08 +00:00
# endif
2023-12-12 01:36:06 +00:00
2023-12-09 10:21:05 +00:00
threads_ . start ( task , " thread_pump_task " , ( void * ) & async_usb_gadget : : thread_pump_task ) ;
threads_ . start ( bulkw , " thread_write_bulk " , ( void * ) & async_usb_gadget : : thread_write_bulk ) ;
threads_ . start ( bulkr , " thread_read_bulk " , ( void * ) & async_usb_gadget : : thread_read_bulk ) ;
threads_ . start ( ep0 , " thread_read_ep0 " , ( void * ) & async_usb_gadget : : thread_read_ep0 ) ;
2023-12-01 09:17:09 +00:00
}
async_usb_gadget : : ~ async_usb_gadget ( )
{
stop ( ) ;
2023-12-12 01:36:06 +00:00
2023-12-13 06:57:08 +00:00
# ifdef MEM_POOL
io_buf_ - > release ( ) ;
# else
2023-12-12 01:36:06 +00:00
dyn_mem_ptr buf = nullptr ;
while ( io_buf_ . take ( buf , false ) )
{
if ( buf )
buf - > release ( ) ;
}
2023-12-13 06:57:08 +00:00
# endif
2023-12-12 01:36:06 +00:00
}
dyn_mem_ptr async_usb_gadget : : get_io_buffer ( void )
{
dyn_mem_ptr buf = nullptr ;
2023-12-13 06:57:08 +00:00
# ifdef MEM_POOL
buf = io_buf_ - > take ( ) ;
# else
2023-12-12 01:36:06 +00:00
io_buf_ . take ( buf , true ) ;
2023-12-13 06:57:08 +00:00
# endif
2023-12-12 01:36:06 +00:00
return buf ;
}
void async_usb_gadget : : free_io_buffer ( dyn_mem_ptr buf )
{
buf - > clear_data ( ) ;
2023-12-13 06:57:08 +00:00
# ifdef MEM_POOL
io_buf_ - > put ( buf ) ;
# else
2023-12-12 01:36:06 +00:00
io_buf_ . save ( buf , true ) ;
2023-12-13 06:57:08 +00:00
# endif
2023-12-01 09:17:09 +00:00
}
const char * async_usb_gadget : : ep0_status_desc ( int ep0_status , char * unk_buf /*>= 20 bytes*/ )
{
RETURN_ENUM_STR ( ep0_status , EP0_STATUS_IDLE ) ;
RETURN_ENUM_STR ( ep0_status , EP0_STATUS_READ_DATA ) ;
RETURN_ENUM_STR ( ep0_status , EP0_STATUS_READ_INVAL_DATA ) ;
RETURN_ENUM_STR ( ep0_status , EP0_STATUS_HANDLE_CMD ) ;
sprintf ( unk_buf , " Unknown status (%d) " , ep0_status ) ;
return unk_buf ;
}
int async_usb_gadget : : wait_fd_event ( int fd , int to_ms )
{
struct timeval timeout , * pto = NULL ;
fd_set read_set ;
int ret = 0 ;
FD_ZERO ( & read_set ) ;
FD_SET ( fd , & read_set ) ;
if ( to_ms ! = - 1 )
{
timeout . tv_sec = to_ms / 1000 ;
timeout . tv_usec = ( to_ms % 1000 ) * 1000 ;
pto = & timeout ;
}
ret = select ( fd + 1 , & read_set , NULL , NULL , pto ) ;
return ret ;
}
uint16_t async_usb_gadget : : get_buf_coefficient ( void )
{
SIMPLE_LOCK ( buf_coef_lock_ ) ;
return buf_coef_ ;
}
void async_usb_gadget : : set_buf_coefficient ( int coef )
{
SIMPLE_LOCK ( buf_coef_lock_ ) ;
buf_coef_ = ( coef < = 0 | | coef > 10000 ) ? 1 : coef ;
}
dyn_mem_ptr async_usb_gadget : : handle_ctrl_message ( dyn_mem_ptr data )
{
struct usb_functionfs_event * pev = ( struct usb_functionfs_event * ) data - > ptr ( ) ;
dyn_mem_ptr reply = nullptr ;
switch ( pev - > type )
{
case FUNCTIONFS_ENABLE :
2023-12-02 02:15:44 +00:00
session_id_ + + ;
2023-12-09 10:21:05 +00:00
utils : : to_log ( LOG_LEVEL_DEBUG , " ////////////// EP0 FFS ENABLE, start session %u ... \n " , session_id_ ) ;
online_ = true ;
2023-12-25 09:39:34 +00:00
cancel_io_ = false ;
2023-12-02 02:15:44 +00:00
wait_in_ . trigger ( ) ;
wait_out_ . trigger ( ) ;
2023-12-01 09:17:09 +00:00
if ( dev_connect_ )
2023-12-09 10:21:05 +00:00
dev_connect_ ( online_ ) ;
2023-12-01 09:17:09 +00:00
break ;
case FUNCTIONFS_DISABLE :
2023-12-02 02:15:44 +00:00
online_ = false ;
2023-12-25 09:39:34 +00:00
cancel_io_ = true ;
2023-12-01 09:17:09 +00:00
if ( dev_connect_ )
2023-12-09 10:21:05 +00:00
dev_connect_ ( online_ ) ;
utils : : to_log ( LOG_LEVEL_DEBUG , " ////////////// EP0 FFS DISABLE, end session %u ... \n \n " , session_id_ ) ;
2023-12-01 09:17:09 +00:00
break ;
case FUNCTIONFS_SETUP :
reply = handle_ctrl_setup ( data ) ;
break ;
case FUNCTIONFS_BIND :
2023-12-25 09:39:34 +00:00
utils : : to_log ( LOG_LEVEL_ALL , " EP0 FFS BIND \n " ) ;
// utils::log_mem_info("EP0 FFS BIND:", pev, sizeof(*pev), LOG_LEVEL_DEBUG);
2023-12-01 09:17:09 +00:00
break ;
case FUNCTIONFS_UNBIND :
utils : : to_log ( LOG_LEVEL_ALL , " EP0 FFS UNBIND \n " ) ;
break ;
case FUNCTIONFS_SUSPEND :
utils : : to_log ( LOG_LEVEL_ALL , " EP0 FFS SUSPEND \n " ) ;
break ;
case FUNCTIONFS_RESUME :
utils : : to_log ( LOG_LEVEL_ALL , " EP0 FFS RESUME \n " ) ;
break ;
}
if ( pev - > u . setup . bRequestType & USB_DIR_IN )
{
// if host need data back, and reply is null, we send nonsense data back ...
if ( ! reply )
{
reply = dyn_mem : : memory ( pev - > u . setup . wLength + sizeof ( uint16_t ) ) ;
reply - > set_len ( pev - > u . setup . wLength ) ;
for ( int i = 0 ; i < = pev - > u . setup . wLength / sizeof ( uint16_t ) ; + + i )
( ( uint16_t * ) reply - > ptr ( ) ) [ i ] = 0x0baad ;
utils : : to_log ( LOG_LEVEL_DEBUG , " fake control message reply(%u bytes) while request with USB_DIR_IN flag but no reply returned. \n " , reply - > get_rest ( ) ) ;
}
}
else if ( reply )
{
utils : : to_log ( LOG_LEVEL_DEBUG , " discard control message reply(%u bytes) while request without USB_DIR_IN flag. \n " , reply - > get_rest ( ) ) ;
reply - > release ( ) ;
reply = nullptr ;
}
return reply ;
}
dyn_mem_ptr async_usb_gadget : : handle_ctrl_setup ( dyn_mem_ptr data )
{
struct usb_functionfs_event * pev = ( struct usb_functionfs_event * ) data - > ptr ( ) ;
bool handled = ( pev - > u . setup . bRequestType & USB_TYPE_MASK ) = = USB_TYPE_VENDOR ;
dyn_mem_ptr reply = nullptr ;
uint32_t err = 0 ;
if ( handled )
{
switch ( pev - > u . setup . bRequest )
{
2023-12-25 09:39:34 +00:00
case USB_REQ_EP0_HAND_SHAKE :
if ( pev - > u . setup . bRequestType & USB_DIR_IN )
{
reply = dyn_mem : : memory ( sizeof ( PEERCFG ) ) ;
( ( LPPEERCFG ) reply - > ptr ( ) ) - > ver = PROTOCOL_VER ;
( ( LPPEERCFG ) reply - > ptr ( ) ) - > io_size = unit_out_ ;
( ( LPPEERCFG ) reply - > ptr ( ) ) - > pid = GetCurrentProcessId ( ) ;
reply - > set_len ( sizeof ( PEERCFG ) ) ;
}
else
{
host_pid_ = ( ( LPPEERCFG ) ( pev + 1 ) ) - > pid ;
utils : : to_log ( LOG_LEVEL_DEBUG , " Host process %llu connected. \n " , host_pid_ ) ;
}
2023-12-01 09:17:09 +00:00
break ;
2023-12-26 01:33:05 +00:00
case USB_REQ_EP0_GOOD_BYE :
{
host_pid_ = ( ( LPPEERCFG ) ( pev + 1 ) ) - > pid ;
utils : : to_log ( LOG_LEVEL_DEBUG , " Host process %llu disconnected. \n " , host_pid_ ) ;
host_pid_ = 0 ;
}
break ;
2023-12-01 09:17:09 +00:00
case USB_REQ_EP0_GET_STATUS :
reply = dyn_mem : : memory ( sizeof ( struct _ep0_reply ) ) ;
2023-12-09 10:21:05 +00:00
reply - > set_len ( sizeof ( struct _ep0_reply ) ) ;
2023-12-01 09:17:09 +00:00
{
2023-12-09 10:21:05 +00:00
LPEP0REPLYSTATUS lps = ( LPEP0REPLYSTATUS ) reply - > ptr ( ) ;
lps - > in_status = statu_in_ ;
lps - > out_status = statu_out_ ;
2023-12-14 06:24:15 +00:00
lps - > task_cnt = cmd_que_ . size ( ) ;
2023-12-09 10:21:05 +00:00
lps - > task_cmd = task_cmd_ ;
lps - > task_pack_id = task_packet_id_ ;
lps - > task_required_bytes = want_bytes_task_ ;
lps - > packets_to_sent = sent_que_ . size ( ) ;
lps - > bytes_to_sent = want_bytes_in_ ;
2023-12-19 07:27:16 +00:00
if ( pev - > u . setup . wValue )
utils : : log_mem_info ( " threads status: " , lps , sizeof ( * lps ) , LOG_LEVEL_DEBUG ) ;
2023-12-01 09:17:09 +00:00
}
break ;
2023-12-14 06:24:15 +00:00
case USB_REQ_EP0_CANCEL_IO :
if ( pev - > u . setup . wLength = = sizeof ( uint32_t ) )
{
uint32_t val = * ( uint32_t * ) & pev [ 1 ] ;
cancel_io_ = ( val = = CANCEL_IO_CANCEL ) ;
}
2023-12-01 09:17:09 +00:00
break ;
case USB_REQ_EP0_SET_ENCRYPT :
if ( pev - > u . setup . wLength = = sizeof ( PACK_BASE ) )
{
LPPACK_BASE pack = ( LPPACK_BASE ) & pev [ 1 ] ;
enc_head_ = pack - > enc_cmd ;
enc_payload_ = pack - > encrypt ;
enc_data_ = pack - > enc_data ;
utils : : to_log ( LOG_LEVEL_DEBUG , " Set encrypting method: command - %d; payload - %d \n " , enc_head_ , enc_payload_ ) ;
}
break ;
default :
handled = false ;
}
}
if ( ! handled )
{
utils : : to_log ( LOG_LEVEL_ALL , " EP0 event(0x%x, 0x%x, 0x%x, 0x%x, 0x%x) has not handled by user business. \n "
, pev - > u . setup . bRequestType , pev - > u . setup . bRequest , pev - > u . setup . wValue , pev - > u . setup . wIndex , pev - > u . setup . wLength ) ;
}
2023-12-09 10:21:05 +00:00
if ( reply )
reply - > set_session_id ( data - > get_session_id ( ) ) ;
2023-12-01 09:17:09 +00:00
return reply ;
}
2023-12-16 07:27:40 +00:00
int async_usb_gadget : : inner_write_bulk_memory ( int fd , uint8_t * buf , uint32_t * len , uint32_t bulk_size , data_source_ptr prog )
2023-12-01 09:17:09 +00:00
{
2023-12-13 06:57:08 +00:00
int w = 0 , to = 0 , err = 0 , total = * len , off = 0 ,
size = * len ;
2023-12-16 07:27:40 +00:00
uint64_t all = * len ,
cur = 0 ;
auto real_prog = [ & ] ( uint64_t total , uint64_t cur , uint32_t err ) - > int
{
return prog - > notify_progress ( total , cur , err ) ;
} ;
auto empty_prog = [ & ] ( uint64_t total , uint64_t cur , uint32_t err ) - > int
{
return 0 ;
} ;
std : : function < int ( uint64_t , uint64_t , uint32_t ) > progr = empty_prog ;
if ( prog )
progr = real_prog ;
2023-12-13 06:57:08 +00:00
2023-12-14 06:24:15 +00:00
while ( ! cancel_io_ )
2023-12-13 06:57:08 +00:00
{
w = write ( fd , buf + off , size ) ;
if ( w = = - 1 )
{
if ( errno = = ETIMEDOUT )
{
if ( to + + > 3 )
{
utils : : to_log ( LOG_LEVEL_DEBUG , " Write bulk failed at (%u/%u) for err: %d \n " , total , * len , errno ) ;
break ;
}
utils : : to_log ( LOG_LEVEL_DEBUG , " Write bulk timeout at (%u/%u), try again after %ums ... \n " , off , total , to * 100 ) ;
std : : this_thread : : sleep_for ( std : : chrono : : milliseconds ( to * 100 ) ) ;
continue ;
}
err = errno ;
2023-12-16 07:27:40 +00:00
{
cur = off ;
progr ( all , cur , err ) ;
}
2023-12-13 06:57:08 +00:00
break ;
}
to = 0 ;
off + = w ;
want_bytes_in_ - = w ;
2023-12-16 07:27:40 +00:00
{
cur = off ;
progr ( all , cur , err ) ;
}
2023-12-13 06:57:08 +00:00
if ( off > = total )
break ;
if ( off + bulk_size < total )
size = bulk_size ;
else
size = total - off ;
}
* len = off ;
return err ;
}
int async_usb_gadget : : inner_write_bulk ( int fd , data_source_ptr data , dyn_mem_ptr mem , uint32_t bulk_size )
{
int ret = 0 ;
uint32_t total = data - > get_rest ( ) ;
2023-12-01 09:17:09 +00:00
2023-12-09 10:21:05 +00:00
statu_in_ = WORKER_STATUS_BUSY ;
want_bytes_in_ = total ;
2023-12-01 09:17:09 +00:00
if ( data - > is_memory_block ( ) )
{
2023-12-13 06:57:08 +00:00
ret = inner_write_bulk_memory ( fd , data - > ptr ( ) , & total , bulk_size ) ;
2023-12-01 09:17:09 +00:00
}
else
{
uint32_t len = bulk_size ;
2023-12-13 06:57:08 +00:00
while ( data - > fetch_data ( mem - > ptr ( ) , & len ) = = 0 )
2023-12-01 09:17:09 +00:00
{
2023-12-13 06:57:08 +00:00
ret = inner_write_bulk_memory ( fd , mem - > ptr ( ) , & len , bulk_size ) ;
if ( ret | | data - > get_rest ( ) = = 0 )
2023-12-01 09:17:09 +00:00
break ;
2023-12-13 06:57:08 +00:00
total + = len ;
2023-12-01 09:17:09 +00:00
len = bulk_size ;
}
}
2023-12-13 06:57:08 +00:00
statu_in_ = ret ? WORKER_STATUS_ERROR : WORKER_STATUS_IDLE ;
want_bytes_in_ - = total ;
return ret ;
2023-12-01 09:17:09 +00:00
}
dyn_mem_ptr async_usb_gadget : : handle_bulk_command ( dyn_mem_ptr data , uint32_t * used , packet_data_base_ptr * pkd )
{
dyn_mem_ptr decrypt = packet_decrypt ( data ) ;
if ( decrypt )
{
data - > release ( ) ;
data = decrypt ;
return handle_cmd_ ( data , used , pkd ) ;
}
else
{
* used = data - > get_rest ( ) ;
* pkd = nullptr ;
return nullptr ;
}
}
void async_usb_gadget : : thread_read_ep0 ( void )
{
struct timeval timeout ;
int headl = sizeof ( struct usb_functionfs_event ) ,
datal = 128 , // gadget_->usb_config->dev_desc.bMaxPacketSize0,
ret = 0 ,
fd = dev_ - > get_device_fd ( ) ;
dyn_mem_ptr mem = dyn_mem : : memory ( headl + datal ) ;
uint8_t * ptr = mem - > ptr ( ) ;
uint32_t recycles = 0 ;
std : : string msg ( " " ) ;
struct usb_functionfs_event * pe = ( struct usb_functionfs_event * ) ptr ;
ret = dev_ - > pull_up ( & msg ) ;
while ( run_ )
{
ret = wait_fd_event ( fd ) ;
2023-12-26 01:33:05 +00:00
if ( ! run_ | | ret < = 0 )
2023-12-01 09:17:09 +00:00
{
2023-12-26 03:30:26 +00:00
if ( run_ )
utils : : to_log ( LOG_LEVEL_ALL , " select EP0(%d) failed: %d(%s) \n " , fd , errno , strerror ( errno ) ) ;
2023-12-01 09:17:09 +00:00
break ;
}
// timeout.tv_sec = 0;
2023-12-25 09:39:34 +00:00
ret = read ( fd , ptr , headl + datal ) ;
2023-12-01 09:17:09 +00:00
if ( ret < 0 )
{
utils : : to_log ( LOG_LEVEL_ALL , " read EP0 failed: %d(%s) \n " , errno , strerror ( errno ) ) ;
break ;
}
bool good = true ;
if ( pe - > u . setup . bRequestType & USB_DIR_IN )
2023-12-25 09:39:34 +00:00
{
2023-12-01 09:17:09 +00:00
mem - > set_len ( headl ) ;
2023-12-25 09:39:34 +00:00
}
2023-12-01 09:17:09 +00:00
else
{
if ( pe - > u . setup . wLength > datal )
{
good = false ;
utils : : to_log ( LOG_LEVEL_ALL , " EP0 data too long(%d > %d), we will discard this packet(0x%x, 0x%x, 0x%x, 0x%x)! \n " , pe - > u . setup . wLength , datal ,
pe - > u . setup . bRequestType , pe - > u . setup . bRequest , pe - > u . setup . wValue , pe - > u . setup . wIndex ) ;
// we read-out whole data here ...
int rest = pe - > u . setup . wLength ;
while ( rest > 0 )
{
read ( fd , ptr + headl , rest > datal ? datal : rest ) ;
rest - = datal ;
}
}
else
{
read ( fd , ptr + headl , pe - > u . setup . wLength ) ;
mem - > set_len ( headl + pe - > u . setup . wLength ) ;
}
}
if ( good )
{
dyn_mem_ptr reply = handle_ctrl_message ( mem ) ;
if ( reply )
{
write ( fd , reply - > ptr ( ) , reply - > get_rest ( ) ) ;
reply - > release ( ) ;
}
}
}
mem - > release ( ) ;
}
void async_usb_gadget : : thread_read_bulk ( int fd )
{
2023-12-12 01:36:06 +00:00
size_t bulk_size = unit_out_ ; // * get_buf_coefficient();
2023-12-01 09:17:09 +00:00
uint32_t cnt_0 = 0 ;
while ( run_ )
{
2023-12-14 06:24:15 +00:00
statu_out_ = WORKER_STATUS_WAIT_RESOURCE ;
2023-12-12 01:36:06 +00:00
dyn_mem_ptr buf = get_io_buffer ( ) ;
2023-12-01 09:17:09 +00:00
int l = 0 ;
2023-12-13 06:57:08 +00:00
if ( ! buf )
{
# ifdef MEM_POOL
utils : : to_log ( LOG_LEVEL_FATAL , " NO IO buffer got at position %u! \n " , io_buf_ - > take_pos ( ) ) ;
# else
utils : : to_log ( LOG_LEVEL_FATAL , " NO IO buffer got while %u elements in queue! \n " , io_buf_ . size ( ) ) ;
# endif
continue ;
}
2023-12-02 02:15:44 +00:00
buf - > set_session_id ( session_id_ ) ;
2023-12-09 10:21:05 +00:00
statu_out_ = WORKER_STATUS_IDLE ;
2023-12-16 07:27:40 +00:00
l = read ( fd , buf - > ptr ( ) , bulk_size ) ;
statu_out_ = WORKER_STATUS_BUSY ;
2023-12-14 06:24:15 +00:00
if ( ! run_ )
{
buf - > release ( ) ;
break ;
}
else if ( l < = 0 )
2023-12-01 09:17:09 +00:00
{
2023-12-12 01:36:06 +00:00
free_io_buffer ( buf ) ;
2023-12-01 09:17:09 +00:00
if ( errno )
{
2023-12-13 06:57:08 +00:00
utils : : to_log ( LOG_LEVEL_FATAL , " read bulk(%d) failed: %d(%s) \n " , l , errno , strerror ( errno ) ) ;
2023-12-09 10:21:05 +00:00
statu_out_ = WORKER_STATUS_ERROR ;
2023-12-01 09:17:09 +00:00
break ;
}
else
{
cnt_0 + + ;
}
}
2023-12-14 06:24:15 +00:00
else
2023-12-01 09:17:09 +00:00
{
2023-12-14 06:24:15 +00:00
// if(!cancel_io_) // ensure to trigger task thread clean it's work
{
buf - > set_len ( l ) ;
cmd_que_ . save ( buf , true ) ;
}
// else
// {
// free_io_buffer(buf);
// utils::to_log(LOG_LEVEL_DEBUG, "Cancel read with %u bytes.\n", l);
// }
2023-12-01 09:17:09 +00:00
}
}
2023-12-09 10:21:05 +00:00
statu_out_ = WORKER_STATUS_NOT_START ;
2023-12-01 09:17:09 +00:00
}
void async_usb_gadget : : thread_write_bulk ( int fd )
{
2023-12-13 06:57:08 +00:00
uint32_t bulk_size = unit_in_ * get_buf_coefficient ( ) ;
dyn_mem_ptr mem = dyn_mem : : memory ( bulk_size ) ;
2023-12-09 10:21:05 +00:00
statu_in_ = WORKER_STATUS_IDLE ;
2023-12-01 09:17:09 +00:00
while ( run_ )
{
data_source_ptr data ;
int err = 0 ;
if ( sent_que_ . take ( data , true ) )
{
2023-12-09 10:21:05 +00:00
want_bytes_in_ = data - > get_rest ( ) ;
2023-12-14 06:24:15 +00:00
if ( ! cancel_io_ )
2023-12-13 06:57:08 +00:00
{
2023-12-14 06:24:15 +00:00
err = inner_write_bulk ( fd , data , mem , bulk_size ) ;
2023-12-13 06:57:08 +00:00
{
2023-12-14 06:24:15 +00:00
file_reader_ptr fr = dynamic_cast < file_reader_ptr > ( data ) ;
if ( fr )
{
utils : : to_log ( LOG_LEVEL_DEBUG , " Sent file(%s) with error: %d. \n " , fr - > path_file ( ) , err ) ;
}
2023-12-13 06:57:08 +00:00
}
}
2023-12-14 06:24:15 +00:00
else
{
utils : : to_log ( LOG_LEVEL_DEBUG , " Cancel write with %u bytes. \n " , data - > get_rest ( ) ) ;
}
2023-12-01 09:17:09 +00:00
data - > release ( ) ;
if ( err )
{
2023-12-13 06:57:08 +00:00
utils : : to_log ( LOG_LEVEL_FATAL , " write bulk failed: %d(%s) \n " , errno , strerror ( errno ) ) ;
2023-12-01 09:17:09 +00:00
break ;
}
}
}
2023-12-13 06:57:08 +00:00
mem - > release ( ) ;
2023-12-09 10:21:05 +00:00
statu_in_ = WORKER_STATUS_NOT_START ;
2023-12-01 09:17:09 +00:00
}
void async_usb_gadget : : thread_pump_task ( void )
{
dyn_mem_ptr prev = nullptr , data = nullptr , reply = nullptr ;
bool in_err_statu = false ;
uint32_t required = 0 , used = 0 , restore_err_cnt = 0 , max_que = 0 ;
data_holder * dh = nullptr ;
LPPACK_BASE pack = nullptr ;
uint64_t total_size = 0 ;
while ( run_ )
{
data = nullptr ;
2023-12-09 10:21:05 +00:00
statu_task_ = WORKER_STATUS_IDLE ;
2023-12-14 06:24:15 +00:00
task_cmd_ = 0 ;
2023-12-01 09:17:09 +00:00
if ( cmd_que_ . take ( data , true ) & & data )
{
2023-12-12 01:36:06 +00:00
bool pool = true ;
2023-12-09 10:21:05 +00:00
statu_task_ = WORKER_STATUS_BUSY ;
if ( max_que < cmd_que_ . size ( ) )
max_que = cmd_que_ . size ( ) ;
2023-12-01 09:17:09 +00:00
2023-12-14 06:24:15 +00:00
if ( cancel_io_ )
{
if ( prev )
{
utils : : to_log ( LOG_LEVEL_DEBUG , " Cancel task previous packet with %u bytes. \n " , prev - > get_rest ( ) ) ;
prev - > release ( ) ;
prev = nullptr ;
}
if ( dh )
{
utils : : to_log ( LOG_LEVEL_DEBUG , " Cancel task holder need %u bytes. \n " , dh - > get_required ( ) ) ;
dh - > cancel ( ) ;
dh - > release ( ) ;
dh = nullptr ;
}
if ( reply )
{
reply - > release ( ) ;
reply = nullptr ;
}
want_bytes_task_ = 0 ;
utils : : to_log ( LOG_LEVEL_DEBUG , " Cancel task with %u bytes. \n " , data - > get_rest ( ) ) ;
data - > release ( ) ;
continue ;
}
2023-12-02 02:15:44 +00:00
if ( prev )
2023-12-01 09:17:09 +00:00
{
2023-12-02 02:15:44 +00:00
if ( prev - > get_session_id ( ) ! = data - > get_session_id ( ) )
{
utils : : to_log ( LOG_LEVEL_DEBUG , " Discard previous packet for the session ID(%u) is not equal to now(%u). \n " , prev - > get_session_id ( ) , data - > get_session_id ( ) ) ;
prev - > release ( ) ;
}
else
{
utils : : to_log ( LOG_LEVEL_ALL , " Combine partial packet with length %u and %u ... \n " , prev - > get_rest ( ) , data - > get_rest ( ) ) ;
* prev + = * data ;
2023-12-12 01:36:06 +00:00
free_io_buffer ( data ) ;
2023-12-02 02:15:44 +00:00
data = prev ;
2023-12-12 01:36:06 +00:00
pool = false ;
2023-12-02 02:15:44 +00:00
}
prev = nullptr ;
}
2023-12-01 09:17:09 +00:00
2023-12-02 02:15:44 +00:00
if ( dh & & dh - > get_session_id ( ) ! = data - > get_session_id ( ) )
{
dh - > cancel ( ) ;
dh - > release ( ) ;
dh = nullptr ;
2023-12-01 09:17:09 +00:00
}
2023-12-02 02:15:44 +00:00
if ( ! online_ | | data - > get_session_id ( ) ! = session_id_ )
2023-12-01 09:17:09 +00:00
{
2023-12-02 02:15:44 +00:00
utils : : to_log ( LOG_LEVEL_DEBUG , " Discard task for session ID(%u) is not equal now(%u) or is offline. \n " , data - > get_session_id ( ) , session_id_ ) ;
2023-12-12 01:36:06 +00:00
if ( pool )
free_io_buffer ( data ) ;
else
data - > release ( ) ;
2023-12-02 02:15:44 +00:00
continue ;
2023-12-01 09:17:09 +00:00
}
2023-12-02 02:15:44 +00:00
2023-12-01 09:17:09 +00:00
do
{
packet_data_base_ptr pack_data = nullptr ;
data_source_ptr ds = nullptr ;
bool notify_reply_ok = false ;
if ( dh = = nullptr )
{
2023-12-02 02:15:44 +00:00
if ( ! online_ | | data - > get_rest ( ) < sizeof ( PACK_BASE ) )
2023-12-01 09:17:09 +00:00
break ;
else
{
pack = ( LPPACK_BASE ) data - > ptr ( ) ;
2023-12-09 10:21:05 +00:00
task_cmd_ = pack - > cmd ;
task_packet_id_ = pack - > pack_id ;
want_bytes_task_ = 0 ;
2023-12-01 09:17:09 +00:00
used = data - > get_rest ( ) ;
reply = handle_bulk_command ( data , & used , & pack_data ) ;
notify_reply_ok = used & ( INT32_MAX + 1 ) ;
used & = INT32_MAX ;
if ( pack_data )
{
2023-12-02 02:15:44 +00:00
pack_data - > set_session_id ( data - > get_session_id ( ) ) ;
2023-12-01 09:17:09 +00:00
dh = dynamic_cast < data_holder_ptr > ( pack_data ) ;
if ( ! dh )
{
ds = dynamic_cast < data_source_ptr > ( pack_data ) ;
if ( ! ds )
pack_data - > release ( ) ;
2023-12-09 10:21:05 +00:00
else
want_bytes_task_ = ds - > get_rest ( ) ;
2023-12-01 09:17:09 +00:00
}
else
2023-12-09 10:21:05 +00:00
{
2023-12-01 09:17:09 +00:00
total_size = 0 ;
2023-12-09 10:21:05 +00:00
want_bytes_task_ = dh - > get_required ( ) ;
}
2023-12-01 09:17:09 +00:00
}
}
}
else
{
uint32_t len = data - > get_rest ( ) ;
int err = dh - > put_data ( data - > ptr ( ) , & len ) ;
if ( len < data - > get_rest ( ) )
{
utils : : to_log ( LOG_LEVEL_WARNING , " Put partial data %u/%u! at +%08X with error %d \n " , len , data - > get_rest ( ) , total_size , err ) ;
}
total_size + = len ;
if ( dh - > is_complete ( ) | | err )
{
reply = dyn_mem : : memory ( sizeof ( PACK_BASE ) ) ;
2023-12-02 02:15:44 +00:00
reply - > set_session_id ( dh - > get_session_id ( ) ) ;
2023-12-01 09:17:09 +00:00
pack = ( LPPACK_BASE ) reply - > ptr ( ) ;
BASE_PACKET_REPLY ( * pack , dh - > get_packet_command ( ) + 1 , dh - > get_packet_id ( ) , err ) ;
reply - > set_len ( sizeof ( PACK_BASE ) ) ;
2023-12-13 06:57:08 +00:00
utils : : to_log ( LOG_LEVEL_ALL , " Received large data(%p) with error(Max queue size is %u): %d. \n " , total_size , max_que , err ) ;
2023-12-01 09:17:09 +00:00
dh - > release ( ) ;
dh = nullptr ;
2023-12-09 10:21:05 +00:00
want_bytes_task_ = 0 ;
}
else
{
want_bytes_task_ = dh - > get_required ( ) ;
2023-12-01 09:17:09 +00:00
}
used = len ;
if ( err )
{
in_err_statu = true ;
restore_err_cnt = 0 ;
}
}
// first reply the packet ...
if ( reply )
{
// encrypt ...
dyn_mem_ptr enc = packet_encrypt ( reply , enc_head_ , enc_payload_ , enc_data_ ) ;
if ( enc )
{
reply - > release ( ) ;
reply = enc ;
}
2023-12-09 10:21:05 +00:00
reply - > set_session_id ( data - > get_session_id ( ) ) ;
2023-12-01 09:17:09 +00:00
write_bulk ( dynamic_cast < data_source_ptr > ( reply ) ) ;
if ( notify_reply_ok )
handle_bulk_command ( data , nullptr , nullptr ) ;
reply - > release ( ) ;
reply = nullptr ;
}
// second send appendix data ...
if ( ds )
{
write_bulk ( ds ) ;
ds - > release ( ) ;
ds = nullptr ;
}
data - > used ( used ) ;
} while ( used & & data - > get_rest ( ) ) ;
2023-12-09 10:21:05 +00:00
2023-12-01 09:17:09 +00:00
if ( data - > get_rest ( ) )
2023-12-12 01:36:06 +00:00
{
if ( pool )
{
prev = dyn_mem : : memory ( unit_out_ ) ;
prev - > put ( data - > ptr ( ) , data - > get_rest ( ) ) ;
free_io_buffer ( data ) ;
}
else
{
prev = data ;
}
}
2023-12-01 09:17:09 +00:00
else
2023-12-09 10:21:05 +00:00
{
if ( ! dh )
{
task_cmd_ = task_packet_id_ = want_bytes_task_ = 0 ;
}
2023-12-12 01:36:06 +00:00
if ( pool )
free_io_buffer ( data ) ;
else
data - > release ( ) ;
2023-12-09 10:21:05 +00:00
}
2023-12-01 09:17:09 +00:00
}
}
if ( prev )
prev - > release ( ) ;
if ( data )
data - > release ( ) ;
if ( reply )
reply - > release ( ) ;
if ( dh )
dh - > release ( ) ;
}
int async_usb_gadget : : stop ( void )
{
run_ = false ;
2023-12-13 06:57:08 +00:00
# ifdef MEM_POOL
io_buf_ - > stop ( ) ;
# else
io_buf_ . trigger ( ) ;
# endif
2023-12-02 02:15:44 +00:00
wait_in_ . trigger ( ) ;
wait_out_ . trigger ( ) ;
cmd_que_ . trigger ( ) ;
sent_que_ . trigger ( ) ;
2023-12-01 09:17:09 +00:00
if ( dev_ )
{
dev_ - > pull_down ( ) ;
dev_ - > close_device ( ) ;
dev_ - > release ( ) ;
dev_ = nullptr ;
}
return 0 ;
}
int async_usb_gadget : : write_bulk ( data_source_ptr data )
{
2023-12-02 02:15:44 +00:00
if ( data - > get_session_id ( ) ! = session_id_ | | ! online_ )
{
2023-12-09 10:21:05 +00:00
utils : : to_log ( LOG_LEVEL_DEBUG , " Discard reply packet for the session ID(%u) is not equal current session(%u) or is offline. \n " , data - > get_session_id ( ) , session_id_ ) ;
2023-12-02 02:15:44 +00:00
data - > release ( ) ;
return sent_que_ . size ( ) ;
}
else
{
int quel = 0 ;
2023-12-01 09:17:09 +00:00
2023-12-02 02:15:44 +00:00
data - > add_ref ( ) ;
quel = sent_que_ . save ( data , true ) ;
2023-12-01 09:17:09 +00:00
2023-12-02 02:15:44 +00:00
return quel ;
}
2023-12-01 09:17:09 +00:00
}
2023-12-01 10:03:10 +00:00
int async_usb_gadget : : last_error ( void )
{
return last_err_ ;
2023-12-09 10:21:05 +00:00
}