From: Łukasz Stelmach Date: Tue, 6 Feb 2018 18:04:18 +0000 (+0100) Subject: Implement FunctionFS transport X-Git-Tag: accepted/tizen/unified/20180406.144331~5 X-Git-Url: http://review.tizen.org/git/?a=commitdiff_plain;h=6263c60a6d11c64a07668e181a414b13f4e1a7d0;p=platform%2Fcore%2Fconnectivity%2Fmtp-responder.git Implement FunctionFS transport FunctionFS is a generic interface between the kernel and userland processes that allows implementation of USB functions as userspace processes. There may be many instances of FunctionFS mounted each one for a different USB functions. Authored-by: Łukasz Stelmach [Backported from other repository] Signed-off-by: Krzysztof Opasiak [Rebased] Signed-off-by: Paweł Szewczyk Change-Id: Idba4fd657006d445bf1399218784f1c40e3cf626 --- diff --git a/include/transport/mtp_usb_driver.h b/include/transport/mtp_usb_driver.h index cc63874..6320983 100755 --- a/include/transport/mtp_usb_driver.h +++ b/include/transport/mtp_usb_driver.h @@ -17,12 +17,22 @@ #ifndef _MTP_USB_DRIVER_H_ #define _MTP_USB_DRIVER_H_ +#include "mtp_config.h" #include "mtp_datatype.h" #include "mtp_msgq.h" /* Start of driver related defines */ #define MTP_DRIVER_PATH "/dev/usb_mtp_gadget" +/* FunctionFS endpoint paths */ +#ifndef MTP_FFS_PATH +#define MTP_FFS_PATH "/dev/usb-funcs/mtp" +#endif +#define MTP_EP0_PATH MTP_FFS_PATH "/ep0" +#define MTP_EP_IN_PATH MTP_FFS_PATH "/ep1" +#define MTP_EP_OUT_PATH MTP_FFS_PATH "/ep2" +#define MTP_EP_STATUS_PATH MTP_FFS_PATH "/ep3" + /* These values come from f_mtp_slp.h of kernel source */ #define MTP_IOCTL_LETTER 'Z' #define MTP_GET_HIGH_FULL_SPEED _IOR(MTP_IOCTL_LETTER, 1, int) @@ -44,6 +54,32 @@ typedef struct mtp_max_pkt_size { mtp_uint32 rx; } mtp_max_pkt_size_t; +/* Transport driver interface */ +typedef struct mtp_usb_driver { + mtp_bool (*transport_init_usb_device)(void); + void *(*transport_thread_fake_usb_read)(void *arg); + void *(*transport_thread_fake_usb_write)(void *arg); + void (*transport_deinit_usb_device)(void); + void *(*transport_thread_usb_write)(void *arg); + void *(*transport_thread_usb_read)(void *arg); + void *(*transport_thread_usb_control)(void *arg); + mtp_int32 (*transport_mq_init)(msgq_id_t *rx_ipc, msgq_id_t *tx_ipc); + mtp_bool (*transport_mq_deinit)(msgq_id_t *rx_ipc, msgq_id_t *tx_ipc); + mtp_uint32 (*transport_get_usb_packet_len)(void); + mtp_uint32 (*get_tx_pkt_size)(void); + mtp_uint32 (*get_rx_pkt_size)(void); +} mtp_usb_driver_t; + +extern const mtp_usb_driver_t mtp_usb_driver_slp; +extern const mtp_usb_driver_t mtp_usb_driver_ffs; + +typedef enum { + MTP_TRANSPORT_SLP = 0, + MTP_TRANSPORT_FFS, + MTP_TRANSPORT_NUMBER, /* number of supported transports */ + MTP_TRANSPORT_UNKNOWN, +} mtp_transport_type_t; + /* Maximum repeat count for USB error recovery */ #define MTP_USB_ERROR_MAX_RETRY 5 @@ -51,10 +87,12 @@ mtp_bool _transport_init_usb_device(void); void _transport_deinit_usb_device(void); void *_transport_thread_usb_write(void *arg); void *_transport_thread_usb_read(void *arg); +void *_transport_thread_usb_control(void *arg); mtp_int32 _transport_mq_init(msgq_id_t *rx_mqid, msgq_id_t *tx_mqid); mtp_bool _transport_mq_deinit(msgq_id_t *rx_mqid, msgq_id_t *tx_mqid); mtp_uint32 _transport_get_usb_packet_len(void); mtp_uint32 _get_tx_pkt_size(void); mtp_uint32 _get_rx_pkt_size(void); +mtp_transport_type_t _transport_get_type(void); #endif /* _MTP_USB_DRIVER_H_ */ diff --git a/src/transport/mtp_transport.c b/src/transport/mtp_transport.c index 7c73bbd..c178c51 100755 --- a/src/transport/mtp_transport.c +++ b/src/transport/mtp_transport.c @@ -40,6 +40,7 @@ static mtp_mgr_t *g_mgr = &g_mtp_mgr; static mtp_bool g_usb_threads_created = FALSE; static pthread_t g_tx_thrd = 0; static pthread_t g_rx_thrd = 0; +static pthread_t g_ctrl_thrd = 0; static pthread_t g_data_rcv = 0; static msgq_id_t mtp_to_usb_mqid; static msgq_id_t g_usb_to_mtp_mqid; @@ -290,6 +291,7 @@ static mtp_err_t __transport_init_io() mtp_int32 res = 0; thread_func_t usb_write_thread = _transport_thread_usb_write; thread_func_t usb_read_thread = _transport_thread_usb_read; + thread_func_t usb_control_thread = _transport_thread_usb_control; res = _util_thread_create(&g_tx_thrd, "usb write thread", PTHREAD_CREATE_JOINABLE, usb_write_thread, @@ -307,6 +309,17 @@ static mtp_err_t __transport_init_io() goto cleanup; } + if (_transport_get_type() == MTP_TRANSPORT_FFS) { + res = _util_thread_create(&g_ctrl_thrd, "usb control thread", + PTHREAD_CREATE_JOINABLE, + usb_control_thread, + NULL); + if (FALSE == res) { + ERR("CTRL thread creation failed\n"); + goto cleanup; + } + } + g_usb_threads_created = TRUE; return MTP_ERROR_NONE; @@ -314,6 +327,11 @@ static mtp_err_t __transport_init_io() cleanup: _util_print_error(); + if (g_ctrl_thrd) { + res = _util_thread_cancel(g_ctrl_thrd); + DBG("pthread_cancel [%d]\n", res); + g_ctrl_thrd = 0; + } if (g_rx_thrd) { res = _util_thread_cancel(g_rx_thrd); DBG("pthread_cancel [%d]\n", res); @@ -337,6 +355,19 @@ static void __transport_deinit_io() } errno = 0; + if (_transport_get_type() == MTP_TRANSPORT_FFS) { + if (FALSE == _util_thread_cancel(g_ctrl_thrd)) { + ERR("Fail to cancel pthread of g_ctrl_thrd\n"); + } else { + DBG("Succeed to cancel pthread of g_ctrl_thrd\n"); + } + + if (_util_thread_join(g_ctrl_thrd, 0) == FALSE) + ERR("pthread_join of g_ctrl_thrd failed\n"); + + g_ctrl_thrd = 0; + } + if (FALSE == _util_thread_cancel(g_rx_thrd)) ERR("_util_thread_cancel(rx) Fail"); diff --git a/src/transport/mtp_usb_driver.c b/src/transport/mtp_usb_driver.c old mode 100755 new mode 100644 index e392bb9..5576c55 --- a/src/transport/mtp_usb_driver.c +++ b/src/transport/mtp_usb_driver.c @@ -14,107 +14,41 @@ * limitations under the License. */ -#include -#include #include -#include -#include #include "mtp_usb_driver.h" -#include "mtp_device.h" -#include "ptp_datacodes.h" -#include "mtp_support.h" -#include "ptp_container.h" -#include "mtp_msgq.h" -#include "mtp_util.h" -#include "mtp_thread.h" -#include "mtp_transport.h" -#include "mtp_event_handler.h" -/* - * GLOBAL AND EXTERN VARIABLES - */ -extern mtp_config_t g_conf; - -/* - * STATIC VARIABLES AND FUNCTIONS - */ -static mtp_int32 g_usb_fd = -1; -static mtp_max_pkt_size_t pkt_size; -static mtp_uint32 rx_mq_sz; -static mtp_uint32 tx_mq_sz; - -static mtp_int32 __handle_usb_read_err(mtp_int32 err, - mtp_uchar *buf, mtp_int32 buf_len); -static void __clean_up_msg_queue(void *pmsqid); -static void __handle_control_request(mtp_int32 request); -static void __receive_signal(mtp_int32 n, siginfo_t *info, void *unused); +static const mtp_usb_driver_t *usb_driver; /* * FUNCTIONS */ mtp_bool _transport_init_usb_device(void) { - mtp_int32 status = 0; - struct sigaction sig; - pid_t mtp_pid = 0; - int msg_size; - - /* Kernel will inform to User Space using signal. */ - memset(&sig, 0, sizeof(sig)); - sig.sa_sigaction = __receive_signal; - sig.sa_flags = SA_SIGINFO; - sigaction(SIG_SETUP, &sig, NULL); - - if (g_usb_fd > 0) { - DBG("Device Already open"); - return TRUE; - } - - g_usb_fd = open(MTP_DRIVER_PATH, O_RDWR); - if (g_usb_fd < 0) { - ERR("Device node [%s] open Fail,errno [%d]\n", MTP_DRIVER_PATH, errno); - return FALSE; - } - - mtp_pid = getpid(); - status = ioctl(g_usb_fd, MTP_SET_USER_PID, &mtp_pid); - if (status < 0) { - ERR("IOCTL MTP_SET_USER_PID Fail = [%d]\n", status); - _transport_deinit_usb_device(); + if (access(MTP_DRIVER_PATH, F_OK) == 0) { + usb_driver = &mtp_usb_driver_slp; + } else if (access(MTP_EP0_PATH, F_OK) == 0) { + usb_driver = &mtp_usb_driver_ffs; + } else { + ERR("No suport for USB gadgets in kernel"); return FALSE; } - pkt_size.rx = g_conf.read_usb_size; - pkt_size.tx = g_conf.write_usb_size; - - DBG("Final : Tx pkt size:[%u], Rx pkt size:[%u]\n", pkt_size.tx, pkt_size.rx); - - msg_size = sizeof(msgq_ptr_t) - sizeof(long); - rx_mq_sz = (g_conf.max_io_buf_size / g_conf.max_rx_ipc_size) * msg_size; - tx_mq_sz = (g_conf.max_io_buf_size / g_conf.max_tx_ipc_size) * msg_size; - - DBG("RX MQ size :[%u], TX MQ size:[%u]\n", rx_mq_sz, tx_mq_sz); - - return TRUE; + return usb_driver->transport_init_usb_device(); } void _transport_deinit_usb_device(void) { - if (g_usb_fd >= 0) - close(g_usb_fd); - g_usb_fd = -1; - - return; + usb_driver->transport_deinit_usb_device(); } mtp_uint32 _get_tx_pkt_size(void) { - return pkt_size.tx; + return usb_driver->get_rx_pkt_size(); } mtp_uint32 _get_rx_pkt_size(void) { - return pkt_size.rx; + return usb_driver->get_rx_pkt_size(); } /* @@ -127,294 +61,22 @@ mtp_uint32 _get_rx_pkt_size(void) */ mtp_int32 _transport_mq_init(msgq_id_t *rx_mqid, msgq_id_t *tx_mqid) { - if (_util_msgq_init(rx_mqid, 0) == FALSE) { - ERR("RX MQ init Fail [%d]\n", errno); - return FALSE; - } - - if (_util_msgq_set_size(*rx_mqid, rx_mq_sz) == FALSE) - ERR("RX MQ setting size Fail [%d]\n", errno); - - if (_util_msgq_init(tx_mqid, 0) == FALSE) { - ERR("TX MQ init Fail [%d]\n", errno); - _util_msgq_deinit(rx_mqid); - *rx_mqid = -1; - return FALSE; - } - - if (_util_msgq_set_size(*tx_mqid, tx_mq_sz) == FALSE) - ERR("TX MQ setting size Fail [%d]\n", errno); - - return TRUE; + return usb_driver->transport_mq_init(rx_mqid, tx_mqid); } void *_transport_thread_usb_write(void *arg) { - mtp_int32 status = 0; - mtp_uint32 len = 0; - unsigned char *mtp_buf = NULL; - msg_type_t mtype = MTP_UNDEFINED_PACKET; - msgq_id_t *mqid = (msgq_id_t *)arg; - - pthread_cleanup_push(__clean_up_msg_queue, mqid); - - do { - /* original LinuxThreads cancelation didn't work right - * so test for it explicitly. - */ - pthread_testcancel(); - - _util_rcv_msg_from_mq(*mqid, &mtp_buf, &len, &mtype); - - if (mtype == MTP_BULK_PACKET || mtype == MTP_DATA_PACKET) { - status = write(g_usb_fd, mtp_buf, len); - if (status < 0) { - ERR("USB write fail : %d\n", errno); - if (errno == ENOMEM || errno == ECANCELED) { - status = 0; - __clean_up_msg_queue(mqid); - } - } - g_free(mtp_buf); - mtp_buf = NULL; - } else if (MTP_EVENT_PACKET == mtype) { - /* Handling the MTP Asynchronous Events */ - DBG("Send Interrupt data to kernel by IOCTL "); - status = ioctl(g_usb_fd, MTP_WRITE_INT_DATA, mtp_buf); - g_free(mtp_buf); - mtp_buf = NULL; - } else if (MTP_ZLP_PACKET == mtype) { - DBG("Send ZLP data to kernel by IOCTL "); - status = ioctl(g_usb_fd, MTP_SET_ZLP_DATA, NULL); - } else { - DBG("mtype = %d is not valid\n", mtype); - status = -1; - } - - if (status < 0) { - ERR("write data to the device node Fail:\ - status = %d\n", status); - break; - } - } while (status >= 0); - - DBG("exited Source thread with status %d\n", status); - pthread_cleanup_pop(1); - g_free(mtp_buf); - - return NULL; + return usb_driver->transport_thread_usb_write(arg); } void *_transport_thread_usb_read(void *arg) { - mtp_int32 status = 0; - msgq_ptr_t pkt = {MTP_DATA_PACKET, 0, 0, NULL}; - msgq_id_t *mqid = (msgq_id_t *)arg; - mtp_uint32 rx_size = _get_rx_pkt_size(); - - pthread_cleanup_push(__clean_up_msg_queue, mqid); - - do { - pthread_testcancel(); - - pkt.buffer = (mtp_uchar *)g_malloc(rx_size); - if (NULL == pkt.buffer) { - ERR("Sink thread: memalloc Fail."); - break; - } - - status = read(g_usb_fd, pkt.buffer, rx_size); - if (status <= 0) { - status = __handle_usb_read_err(status, pkt.buffer, rx_size); - if (status <= 0) { - ERR("__handle_usb_read_err Fail"); - g_free(pkt.buffer); - break; - } - } - - pkt.length = status; - if (FALSE == _util_msgq_send(*mqid, (void *)&pkt, - sizeof(msgq_ptr_t) - sizeof(long), 0)) { - ERR("msgsnd Fail"); - g_free(pkt.buffer); - } - } while (status > 0); - - DBG("status[%d] errno[%d]\n", status, errno); - pthread_cleanup_pop(1); - - return NULL; -} - -static mtp_int32 __handle_usb_read_err(mtp_int32 err, - mtp_uchar *buf, mtp_int32 buf_len) -{ - mtp_int32 retry = 0; - mtp_bool ret; - - while (retry++ < MTP_USB_ERROR_MAX_RETRY) { - if (err == 0) { - DBG("ZLP(Zero Length Packet). Skip"); - } else if (err < 0 && errno == EINTR) { - DBG("read () is interrupted. Skip"); - } else if (err < 0 && errno == EIO) { - DBG("EIO"); - - if (MTP_PHONE_USB_CONNECTED != - _util_get_local_usb_status()) { - ERR("USB is disconnected"); - break; - } - - _transport_deinit_usb_device(); - ret = _transport_init_usb_device(); - if (ret == FALSE) { - ERR("_transport_init_usb_device Fail"); - continue; - } - } else { - ERR("Unknown error : %d, errno [%d] \n", err, errno); - break; - } - - err = read(g_usb_fd, buf, buf_len); - if (err > 0) - break; - } - - if (err <= 0) - ERR("USB error handling Fail"); - - return err; + return usb_driver->transport_thread_usb_read(arg); } -static void __clean_up_msg_queue(void *mq_id) +void *_transport_thread_usb_control(void *arg) { - mtp_int32 len = 0; - msgq_ptr_t pkt = { 0 }; - msgq_id_t l_mqid = *(msgq_id_t *)mq_id; - - ret_if(mq_id == NULL); - - _transport_set_control_event(PTP_EVENTCODE_CANCELTRANSACTION); - while (TRUE == _util_msgq_receive(l_mqid, (void *)&pkt, - sizeof(msgq_ptr_t) - sizeof(long), 1, &len)) { - g_free(pkt.buffer); - memset(&pkt, 0, sizeof(msgq_ptr_t)); - } - - return; -} - -static void __handle_control_request(mtp_int32 request) -{ - static mtp_bool kernel_reset = FALSE; - static mtp_bool host_cancel = FALSE; - mtp_int32 status = 0; - - switch (request) { - case USB_PTPREQUEST_CANCELIO: - DBG("USB_PTPREQUEST_CANCELIO"); - cancel_req_t cancelreq_data; - mtp_byte buffer[USB_PTPREQUEST_CANCELIO_SIZE + 1] = { 0 }; - - host_cancel = TRUE; - _transport_set_control_event(PTP_EVENTCODE_CANCELTRANSACTION); - status = ioctl(g_usb_fd, MTP_GET_SETUP_DATA, buffer); - if (status < 0) { - ERR("IOCTL GET_SETUP_DATA Fail [%d]\n", status); - return; - } - - memcpy(&(cancelreq_data.io_code), buffer, sizeof(mtp_word)); - memcpy(&(cancelreq_data.tid), &buffer[2], sizeof(mtp_dword)); - DBG("cancel io code [%d], transaction id [%ld]\n", - cancelreq_data.io_code, cancelreq_data.tid); - break; - - case USB_PTPREQUEST_RESET: - - DBG("USB_PTPREQUEST_RESET"); - _reset_mtp_device(); - if (kernel_reset == FALSE) - kernel_reset = TRUE; - - status = ioctl(g_usb_fd, MTP_SEND_RESET_ACK, NULL); - if (status < 0) { - ERR("IOCTL MTP_SEND_RESET_ACK Fail [%d]\n", - status); - } - break; - - case USB_PTPREQUEST_GETSTATUS: - - DBG("USB_PTPREQUEST_GETSTATUS"); - - /* Send busy status response just once. This flag is also for - * the case that mtp misses the cancel request packet. - */ - static mtp_bool sent_busy = FALSE; - usb_status_req_t statusreq_data = { 0 }; - mtp_dword num_param = 0; - - memset(&statusreq_data, 0x00, sizeof(usb_status_req_t)); - if (host_cancel == TRUE || (sent_busy == FALSE && - kernel_reset == FALSE)) { - DBG("Send busy response, set host_cancel to FALSE"); - statusreq_data.len = 0x08; - statusreq_data.code = PTP_RESPONSE_DEVICEBUSY; - host_cancel = FALSE; - } else if (_device_get_phase() == DEVICE_PHASE_NOTREADY) { - statusreq_data.code = - PTP_RESPONSE_TRANSACTIONCANCELLED; - DBG("PTP_RESPONSE_TRANSACTIONCANCELLED"); - statusreq_data.len = (mtp_word)(sizeof(usb_status_req_t) + - (num_param - 2) * sizeof(mtp_dword)); - } else if (_device_get_status() == DEVICE_STATUSOK) { - DBG("PTP_RESPONSE_OK"); - statusreq_data.len = 0x08; - statusreq_data.code = PTP_RESPONSE_OK; - - if (kernel_reset == TRUE) - kernel_reset = FALSE; - } else { - DBG("PTP_RESPONSE_GEN_ERROR"); - statusreq_data.len = 0x08; - statusreq_data.code = PTP_RESPONSE_GEN_ERROR; - } - - if (statusreq_data.code == PTP_RESPONSE_DEVICEBUSY) - sent_busy = TRUE; - else - sent_busy = FALSE; - - status = ioctl(g_usb_fd, MTP_SET_SETUP_DATA, &statusreq_data); - if (status < 0) { - DBG("IOCTL MTP_SET_SETUP_DATA Fail [%d]\n", - status); - return; - } - break; - - case USB_PTPREQUEST_GETEVENT: - DBG("USB_PTPREQUEST_GETEVENT"); - break; - - default: - DBG("Invalid class specific setup request"); - break; - } - return; -} - -static void __receive_signal(mtp_int32 n, siginfo_t *info, void *arg) -{ - mtp_int32 request = info->si_int; - - DBG("Received SIgnal From Kernel"); - __handle_control_request(request); - return; + return usb_driver->transport_thread_usb_control(arg); } /* @@ -425,43 +87,21 @@ static void __receive_signal(mtp_int32 n, siginfo_t *info, void *arg) */ mtp_bool _transport_mq_deinit(msgq_id_t *rx_mqid, msgq_id_t *tx_mqid) { - mtp_int32 res = TRUE; - - if (*rx_mqid) { - res = _util_msgq_deinit(rx_mqid); - if (res == FALSE) - ERR("rx_mqid deinit Fail [%d]\n", errno); - else - *rx_mqid = 0; - } - - if (*tx_mqid) { - res = _util_msgq_deinit(tx_mqid); - if (res == FALSE) - ERR("tx_mqid deinit fail [%d]\n", errno); - else - *tx_mqid = 0; - } - - return res; + return usb_driver->transport_mq_deinit(rx_mqid, tx_mqid); } mtp_uint32 _transport_get_usb_packet_len(void) { - mtp_int32 status = 0; - static mtp_int32 usb_speed = 0; - - if (usb_speed == 0) { + return usb_driver->transport_get_usb_packet_len(); +} - status = ioctl(g_usb_fd, MTP_GET_HIGH_FULL_SPEED, &usb_speed); - if (status < 0) { - ERR("MTP_GET_HIGH_FULL_SPEED Fail [%d]\n", status); - return MTP_MAX_PACKET_SIZE_SEND_FS; - } - } +mtp_transport_type_t _transport_get_type(void) +{ + if (usb_driver == &mtp_usb_driver_slp) + return MTP_TRANSPORT_SLP; - if (usb_speed % MTP_MAX_PACKET_SIZE_SEND_HS) - return MTP_MAX_PACKET_SIZE_SEND_FS; + if (usb_driver == &mtp_usb_driver_ffs) + return MTP_TRANSPORT_FFS; - return MTP_MAX_PACKET_SIZE_SEND_HS; + return MTP_TRANSPORT_UNKNOWN; } diff --git a/src/transport/mtp_usb_driver_ffs.c b/src/transport/mtp_usb_driver_ffs.c new file mode 100755 index 0000000..e972756 --- /dev/null +++ b/src/transport/mtp_usb_driver_ffs.c @@ -0,0 +1,784 @@ +/* -*- mode: C; c-file-style: "linux" -*- + * + * mtp-responder + * + * Copyright (c) 2014 Samsung Electronics Co., Ltd All Rights Reserved + * PROPRIETARY/CONFIDENTIAL + * + * This software is the confidential and proprietary information of + * SAMSUNG ELECTRONICS ("Confidential Information"). You agree and acknowledge + * that this software is owned by Samsung and you shall not disclose + * such Confidential Information and shall use it only in accordance with + * the terms of the license agreement you entered into with SAMSUNG ELECTRONICS. + * SAMSUNG make no representations or warranties about the suitability of + * the software, either express or implied, including but not limited to + * the implied warranties of merchantability, fitness for a particular purpose, + * or non-infringement. SAMSUNG shall not be liable for any damages suffered by + * licensee arising out of or related to this software. + */ + +#define _GNU_SOURCE +#include +#include +#include +#include +#include +#include "mtp_usb_driver.h" +#include "mtp_device.h" +#include "ptp_datacodes.h" +#include "mtp_support.h" +#include "ptp_container.h" +#include "mtp_msgq.h" +#include "mtp_thread.h" +#include "mtp_transport.h" +#include "mtp_event_handler.h" +#include +#include +#include + +/* + * GLOBAL AND EXTERN VARIABLES + */ +extern mtp_config_t g_conf; + +/* + * STATIC VARIABLES AND FUNCTIONS + */ +#ifndef FUNCTIONFS_DESCRIPTORS_MAGIC_V2 +#define FUNCTIONFS_DESCRIPTORS_MAGIC_V2 3 +enum functionfs_flags { + FUNCTIONFS_HAS_FS_DESC = 1, + FUNCTIONFS_HAS_HS_DESC = 2, + FUNCTIONFS_HAS_SS_DESC = 4, + FUNCTIONFS_HAS_MS_OS_DESC = 8, +}; +#endif + +/*PIMA15740-2000 spec*/ +#define USB_PTPREQUEST_CANCELIO 0x64 /* Cancel request */ +#define USB_PTPREQUEST_GETEVENT 0x65 /* Get extened event data */ +#define USB_PTPREQUEST_RESET 0x66 /* Reset Device */ +#define USB_PTPREQUEST_GETSTATUS 0x67 /* Get Device Status */ +#define USB_PTPREQUEST_CANCELIO_SIZE 6 +#define USB_PTPREQUEST_GETSTATUS_SIZE 12 + +#define cpu_to_le16(x) htole16(x) +#define cpu_to_le32(x) htole32(x) +#define le32_to_cpu(x) le32toh(x) +#define le16_to_cpu(x) le16toh(x) + +static const struct { + struct { + __le32 magic; + __le32 length; + __le32 flags; + __le32 fs_count; + __le32 hs_count; + // __le32 os_count; + } header; + struct { + struct usb_interface_descriptor intf; + struct usb_endpoint_descriptor_no_audio bulk_in; + struct usb_endpoint_descriptor_no_audio bulk_out; + struct usb_endpoint_descriptor_no_audio int_in; + } __attribute__((packed)) fs_descs, hs_descs; + // struct {} __attribute__((packed)) os_descs; +} __attribute__((packed)) descriptors = { + .header = { + .magic = cpu_to_le32(FUNCTIONFS_DESCRIPTORS_MAGIC_V2), + .length = cpu_to_le32(sizeof(descriptors)), + .flags = FUNCTIONFS_HAS_FS_DESC | FUNCTIONFS_HAS_HS_DESC, // | FUNCTIONFS_HAS_MS_OS_DESC, + .fs_count = 4, + .hs_count = 4, + // .os_count = 0; + }, + .fs_descs = { + // drivers/usb/gadget/f_mtp_slp.c:207 + .intf = { + .bLength = sizeof(descriptors.fs_descs.intf), + .bDescriptorType = USB_DT_INTERFACE, + .bNumEndpoints = 3, + .bInterfaceClass = USB_CLASS_STILL_IMAGE, + .bInterfaceSubClass = 1, + .bInterfaceProtocol = 1, + .iInterface = 1, + }, + .bulk_in = { + .bLength = USB_DT_ENDPOINT_SIZE, + .bDescriptorType = USB_DT_ENDPOINT, + .bEndpointAddress = 1 | USB_DIR_IN, + .bmAttributes = USB_ENDPOINT_XFER_BULK, + .wMaxPacketSize = __constant_cpu_to_le16(64), + }, + .bulk_out = { + .bLength = USB_DT_ENDPOINT_SIZE, + .bDescriptorType = USB_DT_ENDPOINT, + .bEndpointAddress = 2 | USB_DIR_OUT, + .bmAttributes = USB_ENDPOINT_XFER_BULK, + .wMaxPacketSize = __constant_cpu_to_le16(64), + }, + .int_in = { + .bLength = USB_DT_ENDPOINT_SIZE, + .bDescriptorType = USB_DT_ENDPOINT, + .bEndpointAddress = 3 | USB_DIR_IN, + .bmAttributes = USB_ENDPOINT_XFER_INT, + .wMaxPacketSize = __constant_cpu_to_le16(64), + .bInterval = 6, + }, + }, + .hs_descs = { + .intf = { + .bLength = sizeof(descriptors.fs_descs.intf), + .bDescriptorType = USB_DT_INTERFACE, + .bNumEndpoints = 3, + .bInterfaceClass = USB_CLASS_STILL_IMAGE, + .bInterfaceSubClass = 1, + .bInterfaceProtocol = 1, + .iInterface = 1, + }, + .bulk_in = { + .bLength = USB_DT_ENDPOINT_SIZE, + .bDescriptorType = USB_DT_ENDPOINT, + .bEndpointAddress = 1 | USB_DIR_IN, + .bmAttributes = USB_ENDPOINT_XFER_BULK, + .wMaxPacketSize = __constant_cpu_to_le16(512), + }, + .bulk_out = { + .bLength = USB_DT_ENDPOINT_SIZE, + .bDescriptorType = USB_DT_ENDPOINT, + .bEndpointAddress = 2 | USB_DIR_OUT, + .bmAttributes = USB_ENDPOINT_XFER_BULK, + .wMaxPacketSize = __constant_cpu_to_le16(512), + }, + .int_in = { + .bLength = USB_DT_ENDPOINT_SIZE, + .bDescriptorType = USB_DT_ENDPOINT, + .bEndpointAddress = 3 | USB_DIR_IN, + .bmAttributes = USB_ENDPOINT_XFER_INT, + .wMaxPacketSize = __constant_cpu_to_le16(64), + .bInterval = 6, + }, + }, +}; + +#define STR_INTERFACE "Samsung MTP" + +static const struct { + struct usb_functionfs_strings_head header; + struct { + __le16 code; + const char str1[sizeof(STR_INTERFACE)]; + } __attribute__((packed)) lang0; +} __attribute__((packed)) strings = { + .header = { + .magic = cpu_to_le32(FUNCTIONFS_STRINGS_MAGIC), + .length = cpu_to_le32(sizeof(strings)), + .str_count = cpu_to_le32(1), + .lang_count = cpu_to_le32(1), + }, + .lang0 = { + cpu_to_le16(0x0409), /* en-us */ + STR_INTERFACE, + }, +}; + +static mtp_int32 g_usb_ep0 = -1; /* read (g_usb_ep0, ...) */ +static mtp_int32 g_usb_ep_in = -1; /* write (g_usb_ep_in, ...) */ +static mtp_int32 g_usb_ep_out = -1; /* read (g_usb_ep_out, ...) */ +static mtp_int32 g_usb_ep_status = -1; /* write (g_usb_ep_status, ...) */ + +static mtp_max_pkt_size_t pkt_size; +static mtp_uint32 rx_mq_sz; +static mtp_uint32 tx_mq_sz; +static mtp_int32 __handle_usb_read_err(mtp_int32 err, + mtp_uchar *buf, mtp_int32 buf_len); +static void __clean_up_msg_queue(void *param); +static void __handle_control_request(mtp_int32 request); + +/* + * FUNCTIONS + */ + +static mtp_bool __io_init() +{ + int ret; + + g_usb_ep0 = open(MTP_EP0_PATH, O_RDWR); + if (g_usb_ep0 < 0) + goto out; + + DBG("Writing USB descriptors"); + ret = write(g_usb_ep0, &descriptors, sizeof(descriptors)); + if (ret < 0) { + ERR("Error writing descriptors"); + goto cleanup; + } + + DBG("Writing USB strings"); + ret = write(g_usb_ep0, &strings, sizeof(strings)); + if (ret < 0) { + ERR("Error writing strings"); + goto cleanup; + } + + g_usb_ep_in = open(MTP_EP_IN_PATH, O_RDWR); + if (g_usb_ep_in < 0) { + ERR("Error opening bulk-in"); + goto cleanup; + } + + g_usb_ep_out = open(MTP_EP_OUT_PATH, O_RDWR); + if (g_usb_ep_out < 0) { + ERR("Error opening bulk-out"); + goto cleanup_in; + } + + g_usb_ep_status = open(MTP_EP_STATUS_PATH, O_RDWR); + if (g_usb_ep_status < 0) { + ERR("Error opening status"); + goto cleanup_out; + } + + return TRUE; + +cleanup_out: + close(g_usb_ep_out); +cleanup_in: + close(g_usb_ep_in); +cleanup: + close(g_usb_ep0); +out: + return FALSE; +} + +static mtp_bool ffs_transport_init_usb_device(void) +{ + mtp_int32 status = 0; + int msg_size; + + if (g_usb_ep0 > 0) { + DBG("Device Already open\n"); + return TRUE; + } + + status = __io_init(); + if (!status) { + char error[256]; + ERR("Device node [%s] open failed, errno [%s]\n", + MTP_EP0_PATH, strerror_r(errno, error, sizeof(error))); + return FALSE; + } + + pkt_size.rx = g_conf.read_usb_size; + pkt_size.tx = g_conf.write_usb_size; + + DBG("Final : Tx pkt size:[%u], Rx pkt size:[%u]\n", pkt_size.tx, pkt_size.rx); + + msg_size = sizeof(msgq_ptr_t) - sizeof(long); + rx_mq_sz = (g_conf.max_io_buf_size / g_conf.max_rx_ipc_size) * msg_size; + tx_mq_sz = (g_conf.max_io_buf_size / g_conf.max_tx_ipc_size) * msg_size; + + DBG("RX MQ size :[%u], TX MQ size:[%u]\n", rx_mq_sz, tx_mq_sz); + + return TRUE; +} + +static void ffs_transport_deinit_usb_device(void) +{ + if (g_usb_ep0 >= 0) + close(g_usb_ep0); + g_usb_ep0 = -1; + + if (g_usb_ep_in >= 0) + close(g_usb_ep_in); + g_usb_ep_in = -1; + + if (g_usb_ep_out >= 0) + close(g_usb_ep_out); + g_usb_ep_out = -1; + + if (g_usb_ep_status >= 0) + close(g_usb_ep_status); + g_usb_ep_status = -1; + + return; +} + +static mtp_uint32 ffs_get_tx_pkt_size(void) +{ + return pkt_size.tx; +} + +static mtp_uint32 ffs_get_rx_pkt_size(void) +{ + return pkt_size.rx; +} + +/* + * static mtp_int32 ffs_transport_mq_init() + * This function create a message queue for MTP, + * A created message queue will be used to help data transfer between + * MTP module and usb buffer. + * @return This function returns TRUE on success or + * returns FALSE on failure. + */ +static mtp_int32 ffs_transport_mq_init(msgq_id_t *rx_mqid, msgq_id_t *tx_mqid) +{ + if (_util_msgq_init(rx_mqid, 0) == FALSE) { + ERR("RX MQ init Fail [%d]\n", errno); + return FALSE; + } + + if (_util_msgq_set_size(*rx_mqid, rx_mq_sz) == FALSE) + ERR("RX MQ setting size Fail [%d]\n", errno); + + if (_util_msgq_init(tx_mqid, 0) == FALSE) { + ERR("TX MQ init Fail [%d]\n", errno); + _util_msgq_deinit(rx_mqid); + *rx_mqid = -1; + return FALSE; + } + + if (_util_msgq_set_size(*tx_mqid, tx_mq_sz) == FALSE) + ERR("TX MQ setting size Fail [%d]\n", errno); + + return TRUE; +} + +static void *ffs_transport_thread_usb_write(void *arg) +{ + mtp_int32 status = 0; + mtp_uint32 len = 0; + unsigned char *mtp_buf = NULL; + msg_type_t mtype = MTP_UNDEFINED_PACKET; + msgq_id_t *mqid = (msgq_id_t *)arg; + + pthread_cleanup_push(__clean_up_msg_queue, mqid); + + do { + /* original LinuxThreads cancelation didn't work right + * so test for it explicitly. + */ + pthread_testcancel(); + + _util_rcv_msg_from_mq(*mqid, &mtp_buf, &len, &mtype); + + if (mtype == MTP_BULK_PACKET || mtype == MTP_DATA_PACKET) { + status = write(g_usb_ep_in, mtp_buf, len); + if (status < 0) { + ERR("USB write fail : %d\n", errno); + if (errno == ENOMEM || errno == ECANCELED) { + status = 0; + __clean_up_msg_queue(mqid); + } + } + g_free(mtp_buf); + mtp_buf = NULL; + } else if (MTP_EVENT_PACKET == mtype) { + /* Handling the MTP Asynchronous Events */ + DBG("Send Interrupt data to kernel via g_usb_ep_status\n"); + status = write(g_usb_ep_status, mtp_buf, len); + g_free(mtp_buf); + mtp_buf = NULL; + } else if (MTP_ZLP_PACKET == mtype) { + DBG("Send ZLP data to kerne via g_usb_ep_in\n"); + status = write(g_usb_ep_in, (void*)0xFEE1DEAD, 0); + } else { + DBG("mtype = %d is not valid\n", mtype); + status = -1; + } + + if (status < 0) { + ERR("write data to the device node Fail:\ + status = %d\n", status); + break; + } + } while (status >= 0); + + DBG("exited Source thread with status %d\n", status); + pthread_cleanup_pop(1); + g_free(mtp_buf); + + return NULL; +} + +static int __setup(int ep0, struct usb_ctrlrequest *ctrl) +{ + const char* requests[] = { + "CANCELIO", /* 0x64 */ + "GETEVENT", /* 0x65 */ + "RESET", /* 0x66 */ + "GETSTATUS", /* 0x67 */ + }; + __u16 wIndex = le16_to_cpu(ctrl->wIndex); + __u16 wValue = le16_to_cpu(ctrl->wValue); + __u16 wLength = le16_to_cpu(ctrl->wLength); + int rc = -EOPNOTSUPP; + int status = 0; + + if ((ctrl->bRequestType & 0x7f) != (USB_TYPE_CLASS | USB_RECIP_INTERFACE)) { + DBG(__FILE__ "(%s):%d: Invalid request type: %d", + __func__, __LINE__, ctrl->bRequestType); + goto stall; + } + + switch (((ctrl->bRequestType & 0x80) << 8) | ctrl->bRequest) { + case ((USB_DIR_OUT << 8) | USB_PTPREQUEST_CANCELIO): + + DBG(__FILE__ "(%s):%d: USB_PTPREQUEST_%s", + __func__, __LINE__, requests[ctrl->bRequest-0x64]); + if (wValue != 0 || wIndex != 0 || wLength != 6) { + DBG("Invalid request parameters: wValue:%d wIndex:%d wLength:%d\n"); + rc = -EINVAL; + goto stall; + } + __handle_control_request(ctrl->bRequest); + break; + + case ((USB_DIR_IN << 8) | USB_PTPREQUEST_GETSTATUS): + case ((USB_DIR_OUT << 8) | USB_PTPREQUEST_RESET): + + DBG(__FILE__ "(%s):%d: USB_PTPREQUEST_%s", + __func__, __LINE__, requests[ctrl->bRequest-0x64]); + __handle_control_request(ctrl->bRequest); + break; + + case ((USB_DIR_IN << 8) | USB_PTPREQUEST_GETEVENT): + + /* Optional, may stall */ + DBG(__FILE__ "(%s):%d: USB_PTPREQUEST_%s", + __func__, __LINE__, requests[ctrl->bRequest-0x64]); + rc = -EOPNOTSUPP; + goto stall; + break; + + default: + DBG(__FILE__ "(%s):%d: Invalid request: %d", __func__, + __LINE__, ctrl->bRequest); + goto stall; + } + return 0; + +stall: + + DBG(__FILE__"(%s):%d:stall %0x2x.%02x\n", + __func__, __LINE__, ctrl->bRequestType, ctrl->bRequest); + if ((ctrl->bRequestType & 0x80) == USB_DIR_IN) + status = read(g_usb_ep0, NULL, 0); + else + status = write(g_usb_ep0, NULL, 0); + + if (status != -1 || errno != EL2HLT) { + ERR(__FILE__"(%s):%d:stall error\n", + __func__, __LINE__, ctrl->bRequestType, ctrl->bRequest); + rc = errno; + } + return rc; +} + +static void *ffs_transport_thread_usb_read(void *arg) +{ + mtp_int32 status = 0; + msgq_ptr_t pkt = {MTP_DATA_PACKET, 0, 0, NULL}; + msgq_id_t *mqid = (msgq_id_t *)arg; + mtp_uint32 rx_size = _get_rx_pkt_size(); + + pthread_cleanup_push(__clean_up_msg_queue, mqid); + + do { + pthread_testcancel(); + + pkt.buffer = (mtp_uchar *)g_malloc(rx_size); + if (NULL == pkt.buffer) { + ERR("Sink thread: memalloc failed.\n"); + break; + } + + status = read(g_usb_ep_out, pkt.buffer, rx_size); + if (status <= 0) { + status = __handle_usb_read_err(status, pkt.buffer, rx_size); + if (status <= 0) { + ERR("__handle_usb_read_err is failed\n"); + g_free(pkt.buffer); + break; + } + } + + pkt.length = status; + if (FALSE == _util_msgq_send(*mqid, (void *)&pkt, + sizeof(msgq_ptr_t) - sizeof(long), 0)) { + ERR("msgsnd Fail"); + g_free(pkt.buffer); + } + } while (status > 0); + + DBG("status[%d] errno[%d]\n", status, errno); + pthread_cleanup_pop(1); + + return NULL; +} + +static void *ffs_transport_thread_usb_control(void *arg) +{ + mtp_int32 status = 0; + struct usb_functionfs_event event; + msgq_id_t *mqid = (msgq_id_t *)arg; + + pthread_cleanup_push(__clean_up_msg_queue, mqid); + + do { + pthread_testcancel(); + + status = read(g_usb_ep0, &event, sizeof(event)); + if (status < 0) { + char error[256]; + ERR("read from ep0 failed: %s", + strerror_r(errno, error, sizeof(error))); + continue; + } + DBG("FUNCTIONFS event received: %d", event.type); + + switch (event.type) { + case FUNCTIONFS_SETUP: + DBG("SETUP: bmRequestType:%d bRequest:%d wValue:%d wIndex:%d wLength:%d\n", + event.u.setup.bRequestType, + event.u.setup.bRequest, + event.u.setup.wValue, + event.u.setup.wIndex, + event.u.setup.wLength); + __setup(g_usb_ep0, &event.u.setup); + break; + } + } while (status > 0); + + DBG("status[%d] errno[%d]\n", status, errno); + pthread_cleanup_pop(1); + + return NULL; +} + +static mtp_int32 __handle_usb_read_err(mtp_int32 err, + mtp_uchar *buf, mtp_int32 buf_len) +{ + mtp_int32 retry = 0; + mtp_bool ret; + + while (retry++ < MTP_USB_ERROR_MAX_RETRY) { + if (err == 0) { + DBG("ZLP(Zero Length Packet). Skip"); + } else if (err < 0 && errno == EINTR) { + DBG("read () is interrupted. Skip"); + } else if (err < 0 && errno == EIO) { + DBG("EIO"); + + if (MTP_PHONE_USB_CONNECTED != + _util_get_local_usb_status()) { + ERR("USB is disconnected"); + break; + } + + _transport_deinit_usb_device(); + ret = _transport_init_usb_device(); + if (ret == FALSE) { + ERR("_transport_init_usb_device Fail"); + continue; + } + } else { + ERR("Unknown error : %d, errno [%d] \n", err, errno); + break; + } + + err = read(g_usb_ep_out, buf, buf_len); + if (err > 0) + break; + } + + if (err <= 0) + ERR("USB error handling Fail"); + + return err; +} + +static void __clean_up_msg_queue(void *mq_id) +{ + mtp_int32 len = 0; + msgq_ptr_t pkt = { 0 }; + msgq_id_t l_mqid = *(msgq_id_t *)mq_id; + + ret_if(mq_id == NULL); + + _transport_set_control_event(PTP_EVENTCODE_CANCELTRANSACTION); + while (TRUE == _util_msgq_receive(l_mqid, (void *)&pkt, + sizeof(msgq_ptr_t) - sizeof(long), 1, &len)) { + g_free(pkt.buffer); + memset(&pkt, 0, sizeof(msgq_ptr_t)); + } + + return; +} + +static void __handle_control_request(mtp_int32 request) +{ + static mtp_bool kernel_reset = FALSE; + static mtp_bool host_cancel = FALSE; + mtp_int32 status = 0; + + switch (request) { + case USB_PTPREQUEST_CANCELIO: + // XXX: Convert cancel request data from little-endian + // before use: le32_to_cpu(x), le16_to_cpu(x). + DBG("USB_PTPREQUEST_CANCELIO\n"); + cancel_req_t cancelreq_data; + + host_cancel = TRUE; + _transport_set_control_event(PTP_EVENTCODE_CANCELTRANSACTION); + status = read(g_usb_ep0, &cancelreq_data, sizeof(cancelreq_data)); + if (status < 0) { + char error[256]; + ERR("Failed to read data for CANCELIO request\n: %s", + strerror_r(errno, error, sizeof(error))); + } + break; + + case USB_PTPREQUEST_RESET: + + DBG("USB_PTPREQUEST_RESET\n"); + _reset_mtp_device(); + if (kernel_reset == FALSE) { + kernel_reset = TRUE; + } + + status = read(g_usb_ep0, NULL, 0); + if (status < 0) { + ERR("IOCTL MTP_SEND_RESET_ACK Failed [%d]\n", + status); + } + break; + case USB_PTPREQUEST_GETSTATUS: + + DBG("USB_PTPREQUEST_GETSTATUS"); + + /* Send busy status response just once. This flag is also for + * the case that mtp misses the cancel request packet. + */ + static mtp_bool sent_busy = FALSE; + usb_status_req_t statusreq_data = { 0 }; + mtp_dword num_param = 0; + + memset(&statusreq_data, 0x00, sizeof(usb_status_req_t)); + if (host_cancel == TRUE || (sent_busy == FALSE && + kernel_reset == FALSE)) { + DBG("Send busy response, set host_cancel to FALSE"); + statusreq_data.len = 0x08; + statusreq_data.code = PTP_RESPONSE_DEVICEBUSY; + host_cancel = FALSE; + } else if (_device_get_phase() == DEVICE_PHASE_NOTREADY) { + statusreq_data.code = + PTP_RESPONSE_TRANSACTIONCANCELLED; + DBG("PTP_RESPONSE_TRANSACTIONCANCELLED"); + statusreq_data.len = (mtp_word)(sizeof(usb_status_req_t) + + (num_param - 2) * sizeof(mtp_dword)); + } else if (_device_get_status() == DEVICE_STATUSOK) { + DBG("PTP_RESPONSE_OK"); + statusreq_data.len = 0x08; + statusreq_data.code = PTP_RESPONSE_OK; + + if (kernel_reset == TRUE) + kernel_reset = FALSE; + } else { + DBG("PTP_RESPONSE_GEN_ERROR"); + statusreq_data.len = 0x08; + statusreq_data.code = PTP_RESPONSE_GEN_ERROR; + } + + if (statusreq_data.code == PTP_RESPONSE_DEVICEBUSY) { + sent_busy = TRUE; + } else { + sent_busy = FALSE; + } + + /* status = ioctl(g_usb_fd, MTP_SET_SETUP_DATA, &statusreq_data); + if (status < 0) { + DBG("IOCTL MTP_SET_SETUP_DATA Fail [%d]\n", + status); + return; + } */ + + break; + + case USB_PTPREQUEST_GETEVENT: + DBG("USB_PTPREQUEST_GETEVENT"); + break; + + default: + DBG("Invalid class specific setup request"); + break; + } + return; +} + +/* + * mtp_bool ffs_transport_mq_deinit() + * This function destroy a message queue for MTP, + * @return This function returns TRUE on success or + * returns FALSE on failure. + */ +static mtp_bool ffs_transport_mq_deinit(msgq_id_t *rx_mqid, msgq_id_t *tx_mqid) +{ + mtp_int32 res = TRUE; + + if (*rx_mqid) { + res = _util_msgq_deinit(rx_mqid); + if (res == FALSE) { + ERR("rx_mqid deinit Fail [%d]\n", errno); + } else { + *rx_mqid = 0; + } + } + + if (*tx_mqid) { + res = _util_msgq_deinit(tx_mqid); + if (res == FALSE) { + ERR("tx_mqid deinit fail [%d]\n", errno); + } else { + *tx_mqid = 0; + } + } + + return res; +} + +static mtp_uint32 ffs_transport_get_usb_packet_len(void) +{ + mtp_int32 status = -1; + static mtp_int32 usb_speed = 0; + + if (usb_speed == 0) { + + //status = ioctl(g_usb_fd, MTP_GET_HIGH_FULL_SPEED, &usb_speed); + if (status < 0) { + ERR("MTP_GET_HIGH_FULL_SPEED Failed [%d]\n", status); + return MTP_MAX_PACKET_SIZE_SEND_FS; + } + } + + if (usb_speed % MTP_MAX_PACKET_SIZE_SEND_HS) { + return MTP_MAX_PACKET_SIZE_SEND_FS; + } + + return MTP_MAX_PACKET_SIZE_SEND_HS; +} + + +const mtp_usb_driver_t mtp_usb_driver_ffs = { + .transport_init_usb_device = ffs_transport_init_usb_device, + .transport_deinit_usb_device = ffs_transport_deinit_usb_device, + .transport_thread_usb_write = ffs_transport_thread_usb_write, + .transport_thread_usb_read = ffs_transport_thread_usb_read, + .transport_thread_usb_control = ffs_transport_thread_usb_control, + .transport_mq_init = ffs_transport_mq_init, + .transport_mq_deinit = ffs_transport_mq_deinit, + .transport_get_usb_packet_len = ffs_transport_get_usb_packet_len, + .get_tx_pkt_size = ffs_get_tx_pkt_size, + .get_rx_pkt_size = ffs_get_rx_pkt_size, +}; diff --git a/src/transport/mtp_usb_driver_slp.c b/src/transport/mtp_usb_driver_slp.c new file mode 100644 index 0000000..79f07b0 --- /dev/null +++ b/src/transport/mtp_usb_driver_slp.c @@ -0,0 +1,482 @@ +/* + * Copyright (c) 2012, 2013 Samsung Electronics Co., Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include +#include +#include +#include +#include +#include "mtp_usb_driver.h" +#include "mtp_device.h" +#include "ptp_datacodes.h" +#include "mtp_support.h" +#include "ptp_container.h" +#include "mtp_msgq.h" +#include "mtp_thread.h" +#include "mtp_transport.h" +#include "mtp_event_handler.h" +#include + +/* + * GLOBAL AND EXTERN VARIABLES + */ +extern mtp_config_t g_conf; + +/* + * STATIC VARIABLES AND FUNCTIONS + */ +static mtp_int32 g_usb_fd = -1; +static mtp_max_pkt_size_t pkt_size; +static mtp_uint32 rx_mq_sz; +static mtp_uint32 tx_mq_sz; + +static mtp_int32 __handle_usb_read_err(mtp_int32 err, + mtp_uchar *buf, mtp_int32 buf_len); +static void __clean_up_msg_queue(void *pmsqid); +static void __handle_control_request(mtp_int32 request); +static void __receive_signal(mtp_int32 n, siginfo_t *info, void *unused); + +/* + * FUNCTIONS + */ +static mtp_bool slp_transport_init_usb_device(void) +{ + mtp_int32 status = 0; + struct sigaction sig; + pid_t mtp_pid = 0; + int msg_size; + + /* Kernel will inform to User Space using signal. */ + memset(&sig, 0, sizeof(sig)); + sig.sa_sigaction = __receive_signal; + sig.sa_flags = SA_SIGINFO; + sigaction(SIG_SETUP, &sig, NULL); + + if (g_usb_fd > 0) { + DBG("Device Already open\n"); + return TRUE; + } + + g_usb_fd = open(MTP_DRIVER_PATH, O_RDWR); + if (g_usb_fd < 0) { + ERR("Device node [%s] open failed,errno [%d]\n", MTP_DRIVER_PATH, errno); + return FALSE; + } + + mtp_pid = getpid(); + status = ioctl(g_usb_fd, MTP_SET_USER_PID, &mtp_pid); + if (status < 0) { + ERR("IOCTL MTP_SET_USER_PID failed = [%d]\n", status); + _transport_deinit_usb_device(); + return FALSE; + } + + pkt_size.rx = g_conf.read_usb_size; + pkt_size.tx = g_conf.write_usb_size; + + DBG("Final : Tx pkt size:[%u], Rx pkt size:[%u]\n", pkt_size.tx, pkt_size.rx); + + msg_size = sizeof(msgq_ptr_t) - sizeof(long); + rx_mq_sz = (g_conf.max_io_buf_size / g_conf.max_rx_ipc_size) * msg_size; + tx_mq_sz = (g_conf.max_io_buf_size / g_conf.max_tx_ipc_size) * msg_size; + + DBG("RX MQ size :[%u], TX MQ size:[%u]\n", rx_mq_sz, tx_mq_sz); + + return TRUE; +} + +static void slp_transport_deinit_usb_device(void) +{ + if (g_usb_fd >= 0) + close(g_usb_fd); + g_usb_fd = -1; + + return; +} + +static mtp_uint32 slp_get_tx_pkt_size(void) +{ + return pkt_size.tx; +} + +static mtp_uint32 slp_get_rx_pkt_size(void) +{ + return pkt_size.rx; +} + +/* + * static mtp_int32 slp_transport_mq_init() + * This function create a message queue for MTP, + * A created message queue will be used to help data transfer between + * MTP module and usb buffer. + * @return This function returns TRUE on success or + * returns FALSE on failure. + */ +static mtp_int32 slp_transport_mq_init(msgq_id_t *rx_mqid, msgq_id_t *tx_mqid) +{ + if (_util_msgq_init(rx_mqid, 0) == FALSE) { + ERR("RX MQ init Fail [%d]\n", errno); + return FALSE; + } + + if (_util_msgq_set_size(*rx_mqid, rx_mq_sz) == FALSE) + ERR("RX MQ setting size Fail [%d]\n", errno); + + if (_util_msgq_init(tx_mqid, 0) == FALSE) { + ERR("TX MQ init Fail [%d]\n", errno); + _util_msgq_deinit(rx_mqid); + *rx_mqid = -1; + return FALSE; + } + + if (_util_msgq_set_size(*tx_mqid, tx_mq_sz) == FALSE) + ERR("TX MQ setting size Fail [%d]\n", errno); + + return TRUE; +} + +static void *slp_transport_thread_usb_write(void *arg) +{ + mtp_int32 status = 0; + mtp_uint32 len = 0; + unsigned char *mtp_buf = NULL; + msg_type_t mtype = MTP_UNDEFINED_PACKET; + msgq_id_t *mqid = (msgq_id_t *)arg; + + pthread_cleanup_push(__clean_up_msg_queue, mqid); + + do { + /* original LinuxThreads cancelation didn't work right + * so test for it explicitly. + */ + pthread_testcancel(); + + _util_rcv_msg_from_mq(*mqid, &mtp_buf, &len, &mtype); + + if (mtype == MTP_BULK_PACKET || mtype == MTP_DATA_PACKET) { + status = write(g_usb_fd, mtp_buf, len); + if (status < 0) { + ERR("USB write fail : %d\n", errno); + if (errno == ENOMEM || errno == ECANCELED) { + status = 0; + __clean_up_msg_queue(mqid); + } + } + g_free(mtp_buf); + mtp_buf = NULL; + } else if (MTP_EVENT_PACKET == mtype) { + /* Handling the MTP Asynchronous Events */ + DBG("Send Interrupt data to kernel by IOCTL "); + status = ioctl(g_usb_fd, MTP_WRITE_INT_DATA, mtp_buf); + g_free(mtp_buf); + mtp_buf = NULL; + } else if (MTP_ZLP_PACKET == mtype) { + DBG("Send ZLP data to kernel by IOCTL "); + status = ioctl(g_usb_fd, MTP_SET_ZLP_DATA, NULL); + } else { + DBG("mtype = %d is not valid\n", mtype); + status = -1; + } + + if (status < 0) { + ERR("write data to the device node Fail:\ + status = %d\n", status); + break; + } + } while (status >= 0); + + DBG("exited Source thread with status %d\n", status); + pthread_cleanup_pop(1); + g_free(mtp_buf); + + return NULL; +} + +static void *slp_transport_thread_usb_read(void *arg) +{ + mtp_int32 status = 0; + msgq_ptr_t pkt = {MTP_DATA_PACKET, 0, 0, NULL}; + msgq_id_t *mqid = (msgq_id_t *)arg; + mtp_uint32 rx_size = _get_rx_pkt_size(); + + pthread_cleanup_push(__clean_up_msg_queue, mqid); + + do { + pthread_testcancel(); + + pkt.buffer = (mtp_uchar *)g_malloc(rx_size); + if (NULL == pkt.buffer) { + ERR("Sink thread: memalloc Fail."); + break; + } + + status = read(g_usb_fd, pkt.buffer, rx_size); + if (status <= 0) { + status = __handle_usb_read_err(status, pkt.buffer, rx_size); + if (status <= 0) { + ERR("__handle_usb_read_err Fail"); + g_free(pkt.buffer); + break; + } + } + + pkt.length = status; + if (FALSE == _util_msgq_send(*mqid, (void *)&pkt, + sizeof(msgq_ptr_t) - sizeof(long), 0)) { + ERR("msgsnd Fail"); + g_free(pkt.buffer); + } + } while (status > 0); + + DBG("status[%d] errno[%d]\n", status, errno); + pthread_cleanup_pop(1); + + return NULL; +} + +static mtp_int32 __handle_usb_read_err(mtp_int32 err, + mtp_uchar *buf, mtp_int32 buf_len) +{ + mtp_int32 retry = 0; + mtp_bool ret; + + while (retry++ < MTP_USB_ERROR_MAX_RETRY) { + if (err == 0) { + DBG("ZLP(Zero Length Packet). Skip"); + } else if (err < 0 && errno == EINTR) { + DBG("read () is interrupted. Skip"); + } else if (err < 0 && errno == EIO) { + DBG("EIO"); + + if (MTP_PHONE_USB_CONNECTED != + _util_get_local_usb_status()) { + ERR("USB is disconnected"); + break; + } + + _transport_deinit_usb_device(); + ret = _transport_init_usb_device(); + if (ret == FALSE) { + ERR("_transport_init_usb_device Fail"); + continue; + } + } else { + ERR("Unknown error : %d, errno [%d] \n", err, errno); + break; + } + + err = read(g_usb_fd, buf, buf_len); + if (err > 0) + break; + } + + if (err <= 0) + ERR("USB error handling Fail"); + + return err; +} + +static void __clean_up_msg_queue(void *mq_id) +{ + mtp_int32 len = 0; + msgq_ptr_t pkt = { 0 }; + msgq_id_t l_mqid = *(msgq_id_t *)mq_id; + + ret_if(mq_id == NULL); + + _transport_set_control_event(PTP_EVENTCODE_CANCELTRANSACTION); + while (TRUE == _util_msgq_receive(l_mqid, (void *)&pkt, + sizeof(msgq_ptr_t) - sizeof(long), 1, &len)) { + g_free(pkt.buffer); + memset(&pkt, 0, sizeof(msgq_ptr_t)); + } + + return; +} + +static void __handle_control_request(mtp_int32 request) +{ + static mtp_bool kernel_reset = FALSE; + static mtp_bool host_cancel = FALSE; + mtp_int32 status = 0; + + switch (request) { + case USB_PTPREQUEST_CANCELIO: + DBG("USB_PTPREQUEST_CANCELIO"); + cancel_req_t cancelreq_data; + mtp_byte buffer[USB_PTPREQUEST_CANCELIO_SIZE + 1] = { 0 }; + + host_cancel = TRUE; + _transport_set_control_event(PTP_EVENTCODE_CANCELTRANSACTION); + status = ioctl(g_usb_fd, MTP_GET_SETUP_DATA, buffer); + if (status < 0) { + ERR("IOCTL GET_SETUP_DATA Fail [%d]\n", status); + return; + } + + memcpy(&(cancelreq_data.io_code), buffer, sizeof(mtp_word)); + memcpy(&(cancelreq_data.tid), &buffer[2], sizeof(mtp_dword)); + DBG("cancel io code [%d], transaction id [%ld]\n", + cancelreq_data.io_code, cancelreq_data.tid); + break; + + case USB_PTPREQUEST_RESET: + + DBG("USB_PTPREQUEST_RESET"); + _reset_mtp_device(); + if (kernel_reset == FALSE) + kernel_reset = TRUE; + + status = ioctl(g_usb_fd, MTP_SEND_RESET_ACK, NULL); + if (status < 0) { + ERR("IOCTL MTP_SEND_RESET_ACK Fail [%d]\n", + status); + } + break; + + case USB_PTPREQUEST_GETSTATUS: + + DBG("USB_PTPREQUEST_GETSTATUS"); + + /* Send busy status response just once. This flag is also for + * the case that mtp misses the cancel request packet. + */ + static mtp_bool sent_busy = FALSE; + usb_status_req_t statusreq_data = { 0 }; + mtp_dword num_param = 0; + + memset(&statusreq_data, 0x00, sizeof(usb_status_req_t)); + if (host_cancel == TRUE || (sent_busy == FALSE && + kernel_reset == FALSE)) { + DBG("Send busy response, set host_cancel to FALSE"); + statusreq_data.len = 0x08; + statusreq_data.code = PTP_RESPONSE_DEVICEBUSY; + host_cancel = FALSE; + } else if (_device_get_phase() == DEVICE_PHASE_NOTREADY) { + statusreq_data.code = + PTP_RESPONSE_TRANSACTIONCANCELLED; + DBG("PTP_RESPONSE_TRANSACTIONCANCELLED"); + statusreq_data.len = (mtp_word)(sizeof(usb_status_req_t) + + (num_param - 2) * sizeof(mtp_dword)); + } else if (_device_get_status() == DEVICE_STATUSOK) { + DBG("PTP_RESPONSE_OK"); + statusreq_data.len = 0x08; + statusreq_data.code = PTP_RESPONSE_OK; + + if (kernel_reset == TRUE) + kernel_reset = FALSE; + } else { + DBG("PTP_RESPONSE_GEN_ERROR"); + statusreq_data.len = 0x08; + statusreq_data.code = PTP_RESPONSE_GEN_ERROR; + } + + if (statusreq_data.code == PTP_RESPONSE_DEVICEBUSY) + sent_busy = TRUE; + else + sent_busy = FALSE; + + status = ioctl(g_usb_fd, MTP_SET_SETUP_DATA, &statusreq_data); + if (status < 0) { + DBG("IOCTL MTP_SET_SETUP_DATA Fail [%d]\n", + status); + return; + } + break; + + case USB_PTPREQUEST_GETEVENT: + DBG("USB_PTPREQUEST_GETEVENT"); + break; + + default: + DBG("Invalid class specific setup request"); + break; + } + return; +} + +static void __receive_signal(mtp_int32 n, siginfo_t *info, void *arg) +{ + mtp_int32 request = info->si_int; + + DBG("Received SIgnal From Kernel\n"); + __handle_control_request(request); + return; +} + +/* + * mtp_bool slp_transport_mq_deinit() + * This function destroy a message queue for MTP, + * @return This function returns TRUE on success or + * returns FALSE on failure. + */ +static mtp_bool slp_transport_mq_deinit(msgq_id_t *rx_mqid, msgq_id_t *tx_mqid) +{ + mtp_int32 res = TRUE; + + if (*rx_mqid) { + res = _util_msgq_deinit(rx_mqid); + if (res == FALSE) + ERR("rx_mqid deinit Fail [%d]\n", errno); + else + *rx_mqid = 0; + } + + if (*tx_mqid) { + res = _util_msgq_deinit(tx_mqid); + if (res == FALSE) { + ERR("tx_mqid deinit fail [%d]\n", errno); + } else { + *tx_mqid = 0; + } + } + + return res; +} + +static mtp_uint32 slp_transport_get_usb_packet_len(void) +{ + mtp_int32 status = 0; + static mtp_int32 usb_speed = 0; + + if (usb_speed == 0) { + + status = ioctl(g_usb_fd, MTP_GET_HIGH_FULL_SPEED, &usb_speed); + if (status < 0) { + ERR("MTP_GET_HIGH_FULL_SPEED Failed [%d]\n", status); + return MTP_MAX_PACKET_SIZE_SEND_FS; + } + } + + if (usb_speed % MTP_MAX_PACKET_SIZE_SEND_HS) + return MTP_MAX_PACKET_SIZE_SEND_FS; + + return MTP_MAX_PACKET_SIZE_SEND_HS; +} + + +const mtp_usb_driver_t mtp_usb_driver_slp = { + .transport_init_usb_device = slp_transport_init_usb_device, + .transport_deinit_usb_device = slp_transport_deinit_usb_device, + .transport_thread_usb_write = slp_transport_thread_usb_write, + .transport_thread_usb_read = slp_transport_thread_usb_read, + .transport_thread_usb_control = NULL, + .transport_mq_init = slp_transport_mq_init, + .transport_mq_deinit = slp_transport_mq_deinit, + .transport_get_usb_packet_len = slp_transport_get_usb_packet_len, + .get_tx_pkt_size = slp_get_tx_pkt_size, + .get_rx_pkt_size = slp_get_rx_pkt_size, +};