src/transport/mtp_usb_driver_ffs.c

   1 /* -*- mode: C; c-file-style: "linux" -*-
   2  *
   3  * mtp-responder
   4  *
   5  * Copyright (c) 2014 Samsung Electronics Co., Ltd All Rights Reserved
   6  * PROPRIETARY/CONFIDENTIAL
   7  *
   8  * This software is the confidential and proprietary information of
   9  * SAMSUNG ELECTRONICS ("Confidential Information"). You agree and acknowledge
  10  * that this software is owned by Samsung and you shall not disclose
  11  * such Confidential Information and shall use it only in accordance with
  12  * the terms of the license agreement you entered into with SAMSUNG ELECTRONICS.
  13  * SAMSUNG make no representations or warranties about the suitability of
  14  * the software, either express or implied, including but not limited to
  15  * the implied warranties of merchantability, fitness for a particular purpose,
  16  * or non-infringement. SAMSUNG shall not be liable for any damages suffered by
  17  * licensee arising out of or related to this software.
  18  */
  19
  20 #define _GNU_SOURCE
  21 #include <sys/ioctl.h>
  22 #include <fcntl.h>
  23 #include <unistd.h>
  24 #include <signal.h>
  25 #include <glib.h>
  26 #include "mtp_usb_driver.h"
  27 #include "mtp_device.h"
  28 #include "ptp_datacodes.h"
  29 #include "mtp_support.h"
  30 #include "ptp_container.h"
  31 #include "mtp_msgq.h"
  32 #include "mtp_thread.h"
  33 #include "mtp_transport.h"
  34 #include "mtp_event_handler.h"
  35 #include <sys/prctl.h>
  36 #include <linux/usb/ch9.h>
  37 #include <linux/usb/functionfs.h>
  38
  39 /*
  40  * GLOBAL AND EXTERN VARIABLES
  41  */
  42 extern mtp_config_t g_conf;
  43
  44 /*
  45  * STATIC VARIABLES AND FUNCTIONS
  46  */
  47 #ifndef FUNCTIONFS_DESCRIPTORS_MAGIC_V2
  48 #define FUNCTIONFS_DESCRIPTORS_MAGIC_V2 3
  49 enum functionfs_flags {
  50         FUNCTIONFS_HAS_FS_DESC = 1,
  51         FUNCTIONFS_HAS_HS_DESC = 2,
  52         FUNCTIONFS_HAS_SS_DESC = 4,
  53         FUNCTIONFS_HAS_MS_OS_DESC = 8,
  54 };
  55 #endif
  56
  57 /*PIMA15740-2000 spec*/
  58 #define USB_PTPREQUEST_CANCELIO   0x64    /* Cancel request */
  59 #define USB_PTPREQUEST_GETEVENT   0x65    /* Get extened event data */
  60 #define USB_PTPREQUEST_RESET      0x66    /* Reset Device */
  61 #define USB_PTPREQUEST_GETSTATUS  0x67    /* Get Device Status */
  62 #define USB_PTPREQUEST_CANCELIO_SIZE 6
  63 #define USB_PTPREQUEST_GETSTATUS_SIZE 12
  64
  65 #define cpu_to_le16(x)  htole16(x)
  66 #define cpu_to_le32(x)  htole32(x)
  67 #define le32_to_cpu(x)  le32toh(x)
  68 #define le16_to_cpu(x)  le16toh(x)
  69
  70 static const struct {
  71         struct {
  72                 __le32 magic;
  73                 __le32 length;
  74                 __le32 flags;
  75                 __le32 fs_count;
  76                 __le32 hs_count;
  77                 // __le32 os_count;
  78         } header;
  79         struct {
  80                 struct usb_interface_descriptor intf;
  81                 struct usb_endpoint_descriptor_no_audio bulk_in;
  82                 struct usb_endpoint_descriptor_no_audio bulk_out;
  83                 struct usb_endpoint_descriptor_no_audio int_in;
  84         } __attribute__((packed)) fs_descs, hs_descs;
  85         // struct {} __attribute__((packed)) os_descs;
  86 } __attribute__((packed)) descriptors = {
  87         .header = {
  88                 .magic = cpu_to_le32(FUNCTIONFS_DESCRIPTORS_MAGIC_V2),
  89                 .length = cpu_to_le32(sizeof(descriptors)),
  90                 .flags = FUNCTIONFS_HAS_FS_DESC | FUNCTIONFS_HAS_HS_DESC, // | FUNCTIONFS_HAS_MS_OS_DESC,
  91                 .fs_count = 4,
  92                 .hs_count = 4,
  93                 // .os_count = 0;
  94         },
  95         .fs_descs = {
  96                 // drivers/usb/gadget/f_mtp_slp.c:207
  97                 .intf = {
  98                         .bLength = sizeof(descriptors.fs_descs.intf),
  99                         .bDescriptorType = USB_DT_INTERFACE,
 100                         .bNumEndpoints = 3,
 101                         .bInterfaceClass = USB_CLASS_STILL_IMAGE,
 102                         .bInterfaceSubClass = 1,
 103                         .bInterfaceProtocol = 1,
 104                         .iInterface = 1,
 105                 },
 106                 .bulk_in = {
 107                         .bLength = USB_DT_ENDPOINT_SIZE,
 108                         .bDescriptorType = USB_DT_ENDPOINT,
 109                         .bEndpointAddress = 1 | USB_DIR_IN,
 110                         .bmAttributes = USB_ENDPOINT_XFER_BULK,
 111                         .wMaxPacketSize = __constant_cpu_to_le16(64),
 112                 },
 113                 .bulk_out = {
 114                         .bLength = USB_DT_ENDPOINT_SIZE,
 115                         .bDescriptorType = USB_DT_ENDPOINT,
 116                         .bEndpointAddress = 2 | USB_DIR_OUT,
 117                         .bmAttributes = USB_ENDPOINT_XFER_BULK,
 118                         .wMaxPacketSize = __constant_cpu_to_le16(64),
 119                 },
 120                 .int_in = {
 121                         .bLength = USB_DT_ENDPOINT_SIZE,
 122                         .bDescriptorType = USB_DT_ENDPOINT,
 123                         .bEndpointAddress = 3 | USB_DIR_IN,
 124                         .bmAttributes = USB_ENDPOINT_XFER_INT,
 125                         .wMaxPacketSize = __constant_cpu_to_le16(64),
 126                         .bInterval = 6,
 127                 },
 128         },
 129         .hs_descs = {
 130                 .intf = {
 131                         .bLength = sizeof(descriptors.fs_descs.intf),
 132                         .bDescriptorType = USB_DT_INTERFACE,
 133                         .bNumEndpoints = 3,
 134                         .bInterfaceClass = USB_CLASS_STILL_IMAGE,
 135                         .bInterfaceSubClass = 1,
 136                         .bInterfaceProtocol = 1,
 137                         .iInterface = 1,
 138                 },
 139                 .bulk_in = {
 140                         .bLength = USB_DT_ENDPOINT_SIZE,
 141                         .bDescriptorType = USB_DT_ENDPOINT,
 142                         .bEndpointAddress = 1 | USB_DIR_IN,
 143                         .bmAttributes = USB_ENDPOINT_XFER_BULK,
 144                         .wMaxPacketSize = __constant_cpu_to_le16(512),
 145                 },
 146                 .bulk_out = {
 147                         .bLength = USB_DT_ENDPOINT_SIZE,
 148                         .bDescriptorType = USB_DT_ENDPOINT,
 149                         .bEndpointAddress = 2 | USB_DIR_OUT,
 150                         .bmAttributes = USB_ENDPOINT_XFER_BULK,
 151                         .wMaxPacketSize = __constant_cpu_to_le16(512),
 152                 },
 153                 .int_in = {
 154                         .bLength = USB_DT_ENDPOINT_SIZE,
 155                         .bDescriptorType = USB_DT_ENDPOINT,
 156                         .bEndpointAddress = 3 | USB_DIR_IN,
 157                         .bmAttributes = USB_ENDPOINT_XFER_INT,
 158                         .wMaxPacketSize = __constant_cpu_to_le16(64),
 159                         .bInterval = 6,
 160                 },
 161         },
 162 };
 163
 164 #define STR_INTERFACE "Samsung MTP"
 165
 166 static const struct {
 167         struct usb_functionfs_strings_head header;
 168         struct {
 169                 __le16 code;
 170                 const char str1[sizeof(STR_INTERFACE)];
 171         } __attribute__((packed)) lang0;
 172 } __attribute__((packed)) strings = {
 173         .header = {
 174                 .magic = cpu_to_le32(FUNCTIONFS_STRINGS_MAGIC),
 175                 .length = cpu_to_le32(sizeof(strings)),
 176                 .str_count = cpu_to_le32(1),
 177                 .lang_count = cpu_to_le32(1),
 178         },
 179         .lang0 = {
 180                 cpu_to_le16(0x0409), /* en-us */
 181                 STR_INTERFACE,
 182         },
 183 };
 184
 185 static mtp_int32 g_usb_ep0 = -1;       /* read (g_usb_ep0, ...) */
 186 static mtp_int32 g_usb_ep_in = -1;     /* write (g_usb_ep_in, ...) */
 187 static mtp_int32 g_usb_ep_out = -1;    /* read (g_usb_ep_out, ...) */
 188 static mtp_int32 g_usb_ep_status = -1; /* write (g_usb_ep_status, ...) */
 189
 190 static mtp_max_pkt_size_t pkt_size;
 191 static mtp_uint32 rx_mq_sz;
 192 static mtp_uint32 tx_mq_sz;
 193 static mtp_int32 __handle_usb_read_err(mtp_int32 err,
 194                 mtp_uchar *buf, mtp_int32 buf_len);
 195 static void __clean_up_msg_queue(void *param);
 196 static void __handle_control_request(mtp_int32 request);
 197
 198 /*
 199  * FUNCTIONS
 200  */
 201
 202 static mtp_bool __io_init()
 203 {
 204         int ret;
 205
 206         g_usb_ep0 = open(MTP_EP0_PATH, O_RDWR);
 207         if (g_usb_ep0 < 0)
 208                 goto out;
 209
 210         DBG("Writing USB descriptors");
 211         ret = write(g_usb_ep0, &descriptors, sizeof(descriptors));
 212         if (ret < 0) {
 213                 ERR("Error writing descriptors");
 214                 goto cleanup;
 215         }
 216
 217         DBG("Writing USB strings");
 218         ret = write(g_usb_ep0, &strings, sizeof(strings));
 219         if (ret < 0) {
 220                 ERR("Error writing strings");
 221                 goto cleanup;
 222         }
 223
 224         g_usb_ep_in = open(MTP_EP_IN_PATH, O_RDWR);
 225         if (g_usb_ep_in < 0) {
 226                 ERR("Error opening bulk-in");
 227                 goto cleanup;
 228         }
 229
 230         g_usb_ep_out = open(MTP_EP_OUT_PATH, O_RDWR);
 231         if (g_usb_ep_out < 0) {
 232                 ERR("Error opening bulk-out");
 233                 goto cleanup_in;
 234         }
 235
 236         g_usb_ep_status = open(MTP_EP_STATUS_PATH, O_RDWR);
 237         if (g_usb_ep_status < 0) {
 238                 ERR("Error opening status");
 239                 goto cleanup_out;
 240         }
 241
 242         return TRUE;
 243
 244 cleanup_out:
 245         close(g_usb_ep_out);
 246 cleanup_in:
 247         close(g_usb_ep_in);
 248 cleanup:
 249         close(g_usb_ep0);
 250 out:
 251         return FALSE;
 252 }
 253
 254 static mtp_bool ffs_transport_init_usb_device(void)
 255 {
 256         mtp_int32 status = 0;
 257         int msg_size;
 258
 259         if (g_usb_ep0 > 0) {
 260                 DBG("Device Already open\n");
 261                 return TRUE;
 262         }
 263
 264         status = __io_init();
 265         if (!status) {
 266                 char error[256];
 267                 ERR("Device node [%s] open failed, errno [%s]\n",
 268                     MTP_EP0_PATH, strerror_r(errno, error, sizeof(error)));
 269                 return FALSE;
 270         }
 271
 272         pkt_size.rx = g_conf.read_usb_size;
 273         pkt_size.tx = g_conf.write_usb_size;
 274
 275         DBG("Final : Tx pkt size:[%u], Rx pkt size:[%u]\n", pkt_size.tx, pkt_size.rx);
 276
 277         msg_size = sizeof(msgq_ptr_t) - sizeof(long);
 278         rx_mq_sz = (g_conf.max_io_buf_size / g_conf.max_rx_ipc_size) * msg_size;
 279         tx_mq_sz = (g_conf.max_io_buf_size / g_conf.max_tx_ipc_size) * msg_size;
 280
 281         DBG("RX MQ size :[%u], TX MQ size:[%u]\n", rx_mq_sz, tx_mq_sz);
 282
 283         return TRUE;
 284 }
 285
 286 static void ffs_transport_deinit_usb_device(void)
 287 {
 288         if (g_usb_ep0 >= 0)
 289                 close(g_usb_ep0);
 290         g_usb_ep0 = -1;
 291
 292         if (g_usb_ep_in >= 0)
 293                 close(g_usb_ep_in);
 294         g_usb_ep_in = -1;
 295
 296         if (g_usb_ep_out >= 0)
 297                 close(g_usb_ep_out);
 298         g_usb_ep_out = -1;
 299
 300         if (g_usb_ep_status >= 0)
 301                 close(g_usb_ep_status);
 302         g_usb_ep_status = -1;
 303
 304         return;
 305 }
 306
 307 static mtp_uint32 ffs_get_tx_pkt_size(void)
 308 {
 309         return pkt_size.tx;
 310 }
 311
 312 static mtp_uint32 ffs_get_rx_pkt_size(void)
 313 {
 314         return pkt_size.rx;
 315 }
 316
 317 /*
 318  * static mtp_int32 ffs_transport_mq_init()
 319  * This function create a message queue for MTP,
 320  * A created message queue will be used to help data transfer between
 321  * MTP module and usb buffer.
 322  * @return      This function returns TRUE on success or
 323  *              returns FALSE on failure.
 324  */
 325 static mtp_int32 ffs_transport_mq_init(msgq_id_t *rx_mqid, msgq_id_t *tx_mqid)
 326 {
 327         if (_util_msgq_init(rx_mqid, 0) == FALSE) {
 328                 ERR("RX MQ init Fail [%d]\n", errno);
 329                 return FALSE;
 330         }
 331
 332         if (_util_msgq_set_size(*rx_mqid, rx_mq_sz) == FALSE)
 333                 ERR("RX MQ setting size Fail [%d]\n", errno);
 334
 335         if (_util_msgq_init(tx_mqid, 0) == FALSE) {
 336                 ERR("TX MQ init Fail [%d]\n", errno);
 337                 _util_msgq_deinit(rx_mqid);
 338                 *rx_mqid = -1;
 339                 return FALSE;
 340         }
 341
 342         if (_util_msgq_set_size(*tx_mqid, tx_mq_sz) == FALSE)
 343                 ERR("TX MQ setting size Fail [%d]\n", errno);
 344
 345         return TRUE;
 346 }
 347
 348 static void *ffs_transport_thread_usb_write(void *arg)
 349 {
 350         mtp_int32 status = 0;
 351         mtp_uint32 len = 0;
 352         unsigned char *mtp_buf = NULL;
 353         msg_type_t mtype = MTP_UNDEFINED_PACKET;
 354         msgq_id_t *mqid = (msgq_id_t *)arg;
 355
 356         pthread_cleanup_push(__clean_up_msg_queue, mqid);
 357
 358         do {
 359                 /* original LinuxThreads cancelation didn't work right
 360                  * so test for it explicitly.
 361                  */
 362                 pthread_testcancel();
 363
 364                 _util_rcv_msg_from_mq(*mqid, &mtp_buf, &len, &mtype);
 365
 366                 if (mtype == MTP_BULK_PACKET || mtype == MTP_DATA_PACKET) {
 367                         status = write(g_usb_ep_in, mtp_buf, len);
 368                         if (status < 0) {
 369                                 ERR("USB write fail : %d\n", errno);
 370                                 if (errno == ENOMEM || errno == ECANCELED) {
 371                                         status = 0;
 372                                         __clean_up_msg_queue(mqid);
 373                                 }
 374                         }
 375                         g_free(mtp_buf);
 376                         mtp_buf = NULL;
 377                 } else if (MTP_EVENT_PACKET == mtype) {
 378                         /* Handling the MTP Asynchronous Events */
 379                         DBG("Send Interrupt data to kernel via g_usb_ep_status\n");
 380                         status = write(g_usb_ep_status, mtp_buf, len);
 381                         g_free(mtp_buf);
 382                         mtp_buf = NULL;
 383                 } else if (MTP_ZLP_PACKET == mtype) {
 384                         DBG("Send ZLP data to kerne via g_usb_ep_in\n");
 385                         status = write(g_usb_ep_in, (void*)0xFEE1DEAD, 0);
 386                 } else {
 387                         DBG("mtype = %d is not valid\n", mtype);
 388                         status = -1;
 389                 }
 390
 391                 if (status < 0) {
 392                         ERR("write data to the device node Fail:\
 393                                         status = %d\n", status);
 394                         break;
 395                 }
 396         } while (status >= 0);
 397
 398         DBG("exited Source thread with status %d\n", status);
 399         pthread_cleanup_pop(1);
 400         g_free(mtp_buf);
 401
 402         return NULL;
 403 }
 404
 405 static int __setup(int ep0, struct usb_ctrlrequest *ctrl)
 406 {
 407         const char* requests[] = {
 408                 "CANCELIO",     /* 0x64 */
 409                 "GETEVENT",     /* 0x65 */
 410                 "RESET",        /* 0x66 */
 411                 "GETSTATUS",    /* 0x67 */
 412         };
 413         __u16 wIndex = le16_to_cpu(ctrl->wIndex);
 414         __u16 wValue = le16_to_cpu(ctrl->wValue);
 415         __u16 wLength = le16_to_cpu(ctrl->wLength);
 416         int rc = -EOPNOTSUPP;
 417         int status = 0;
 418
 419         if ((ctrl->bRequestType & 0x7f) != (USB_TYPE_CLASS | USB_RECIP_INTERFACE)) {
 420                 DBG(__FILE__ "(%s):%d: Invalid request type: %d",
 421                     __func__, __LINE__, ctrl->bRequestType);
 422                 goto stall;
 423         }
 424
 425         switch (((ctrl->bRequestType & 0x80) << 8) | ctrl->bRequest) {
 426         case ((USB_DIR_OUT << 8) | USB_PTPREQUEST_CANCELIO):
 427
 428                 DBG(__FILE__ "(%s):%d: USB_PTPREQUEST_%s",
 429                     __func__, __LINE__, requests[ctrl->bRequest-0x64]);
 430                 if (wValue != 0 || wIndex != 0 || wLength != 6) {
 431                         DBG("Invalid request parameters: wValue:%d wIndex:%d wLength:%d\n");
 432                         rc = -EINVAL;
 433                         goto stall;
 434                 }
 435                 __handle_control_request(ctrl->bRequest);
 436                 break;
 437
 438         case ((USB_DIR_IN << 8) | USB_PTPREQUEST_GETSTATUS):
 439         case ((USB_DIR_OUT << 8) | USB_PTPREQUEST_RESET):
 440
 441                 DBG(__FILE__ "(%s):%d: USB_PTPREQUEST_%s",
 442                     __func__, __LINE__, requests[ctrl->bRequest-0x64]);
 443                 __handle_control_request(ctrl->bRequest);
 444                 break;
 445
 446         case ((USB_DIR_IN << 8) | USB_PTPREQUEST_GETEVENT):
 447
 448                 /* Optional, may stall */
 449                 DBG(__FILE__ "(%s):%d: USB_PTPREQUEST_%s",
 450                     __func__, __LINE__, requests[ctrl->bRequest-0x64]);
 451                 rc = -EOPNOTSUPP;
 452                 goto stall;
 453                 break;
 454
 455         default:
 456                 DBG(__FILE__ "(%s):%d: Invalid request: %d", __func__,
 457                     __LINE__, ctrl->bRequest);
 458                 goto stall;
 459         }
 460         return 0;
 461
 462 stall:
 463
 464         DBG(__FILE__"(%s):%d:stall %0x2x.%02x\n",
 465             __func__, __LINE__, ctrl->bRequestType, ctrl->bRequest);
 466         if ((ctrl->bRequestType & 0x80) == USB_DIR_IN)
 467                 status = read(g_usb_ep0, NULL, 0);
 468         else
 469                 status = write(g_usb_ep0, NULL, 0);
 470
 471         if (status != -1 || errno != EL2HLT) {
 472                 ERR(__FILE__"(%s):%d:stall error\n",
 473                     __func__, __LINE__, ctrl->bRequestType, ctrl->bRequest);
 474                 rc = errno;
 475         }
 476         return rc;
 477 }
 478
 479 static void *ffs_transport_thread_usb_read(void *arg)
 480 {
 481         mtp_int32 status = 0;
 482         msgq_ptr_t pkt = {MTP_DATA_PACKET, 0, 0, NULL};
 483         msgq_id_t *mqid = (msgq_id_t *)arg;
 484         mtp_uint32 rx_size = _get_rx_pkt_size();
 485
 486         pthread_cleanup_push(__clean_up_msg_queue, mqid);
 487
 488         do {
 489                 pthread_testcancel();
 490
 491                 pkt.buffer = (mtp_uchar *)g_malloc(rx_size);
 492                 if (NULL == pkt.buffer) {
 493                         ERR("Sink thread: memalloc failed.\n");
 494                         break;
 495                 }
 496
 497                 status = read(g_usb_ep_out, pkt.buffer, rx_size);
 498                 if (status <= 0) {
 499                         status = __handle_usb_read_err(status, pkt.buffer, rx_size);
 500                         if (status <= 0) {
 501                                 ERR("__handle_usb_read_err is failed\n");
 502                                 g_free(pkt.buffer);
 503                                 break;
 504                         }
 505                 }
 506
 507                 pkt.length = status;
 508                 if (FALSE == _util_msgq_send(*mqid, (void *)&pkt,
 509                                              sizeof(msgq_ptr_t) - sizeof(long), 0)) {
 510                         ERR("msgsnd Fail");
 511                         g_free(pkt.buffer);
 512                 }
 513         } while (status > 0);
 514
 515         DBG("status[%d] errno[%d]\n", status, errno);
 516         pthread_cleanup_pop(1);
 517
 518         return NULL;
 519 }
 520
 521 static void *ffs_transport_thread_usb_control(void *arg)
 522 {
 523         mtp_int32 status = 0;
 524         struct usb_functionfs_event event;
 525         msgq_id_t *mqid = (msgq_id_t *)arg;
 526
 527         pthread_cleanup_push(__clean_up_msg_queue, mqid);
 528
 529         do {
 530                 pthread_testcancel();
 531
 532                 status = read(g_usb_ep0, &event, sizeof(event));
 533                 if (status < 0) {
 534                         char error[256];
 535                         ERR("read from ep0 failed: %s",
 536                             strerror_r(errno, error, sizeof(error)));
 537                         continue;
 538                 }
 539                 DBG("FUNCTIONFS event received: %d", event.type);
 540
 541                 switch (event.type) {
 542                 case FUNCTIONFS_SETUP:
 543                         DBG("SETUP: bmRequestType:%d bRequest:%d wValue:%d wIndex:%d wLength:%d\n",
 544                             event.u.setup.bRequestType,
 545                             event.u.setup.bRequest,
 546                             event.u.setup.wValue,
 547                             event.u.setup.wIndex,
 548                             event.u.setup.wLength);
 549                         __setup(g_usb_ep0, &event.u.setup);
 550                         break;
 551                 }
 552         } while (status > 0);
 553
 554         DBG("status[%d] errno[%d]\n", status, errno);
 555         pthread_cleanup_pop(1);
 556
 557         return NULL;
 558 }
 559
 560 static mtp_int32 __handle_usb_read_err(mtp_int32 err,
 561                 mtp_uchar *buf, mtp_int32 buf_len)
 562 {
 563         mtp_int32 retry = 0;
 564         mtp_bool ret;
 565
 566         while (retry++ < MTP_USB_ERROR_MAX_RETRY) {
 567                 if (err == 0) {
 568                         DBG("ZLP(Zero Length Packet). Skip");
 569                 } else if (err < 0 && errno == EINTR) {
 570                         DBG("read () is interrupted. Skip");
 571                 } else if (err < 0 && errno == EIO) {
 572                         DBG("EIO");
 573
 574                         if (MTP_PHONE_USB_CONNECTED !=
 575                             _util_get_local_usb_status()) {
 576                                 ERR("USB is disconnected");
 577                                 break;
 578                         }
 579
 580                         _transport_deinit_usb_device();
 581                         ret = _transport_init_usb_device();
 582                         if (ret == FALSE) {
 583                                 ERR("_transport_init_usb_device Fail");
 584                                 continue;
 585                         }
 586                 } else {
 587                         ERR("Unknown error : %d, errno [%d] \n", err, errno);
 588                         break;
 589                 }
 590
 591                 err = read(g_usb_ep_out, buf, buf_len);
 592                 if (err > 0)
 593                         break;
 594         }
 595
 596         if (err <= 0)
 597                 ERR("USB error handling Fail");
 598
 599         return err;
 600 }
 601
 602 static void __clean_up_msg_queue(void *mq_id)
 603 {
 604         mtp_int32 len = 0;
 605         msgq_ptr_t pkt = { 0 };
 606         msgq_id_t l_mqid = *(msgq_id_t *)mq_id;
 607
 608         ret_if(mq_id == NULL);
 609
 610         _transport_set_control_event(PTP_EVENTCODE_CANCELTRANSACTION);
 611         while (TRUE == _util_msgq_receive(l_mqid, (void *)&pkt,
 612                                           sizeof(msgq_ptr_t) - sizeof(long), 1, &len)) {
 613                 g_free(pkt.buffer);
 614                 memset(&pkt, 0, sizeof(msgq_ptr_t));
 615         }
 616
 617         return;
 618 }
 619
 620 static void __handle_control_request(mtp_int32 request)
 621 {
 622         static mtp_bool kernel_reset = FALSE;
 623         static mtp_bool host_cancel = FALSE;
 624         mtp_int32 status = 0;
 625
 626         switch (request) {
 627         case USB_PTPREQUEST_CANCELIO:
 628                 // XXX: Convert cancel request data from little-endian
 629                 // before use:  le32_to_cpu(x), le16_to_cpu(x).
 630                 DBG("USB_PTPREQUEST_CANCELIO\n");
 631                 cancel_req_t cancelreq_data;
 632
 633                 host_cancel = TRUE;
 634                 _transport_set_control_event(PTP_EVENTCODE_CANCELTRANSACTION);
 635                 status = read(g_usb_ep0, &cancelreq_data, sizeof(cancelreq_data));
 636                 if (status < 0) {
 637                         char error[256];
 638                         ERR("Failed to read data for CANCELIO request\n: %s",
 639                                         strerror_r(errno, error, sizeof(error)));
 640                 }
 641                 break;
 642
 643         case USB_PTPREQUEST_RESET:
 644
 645                 DBG("USB_PTPREQUEST_RESET\n");
 646                 _reset_mtp_device();
 647                 if (kernel_reset == FALSE) {
 648                         kernel_reset = TRUE;
 649                 }
 650
 651                 status = read(g_usb_ep0, NULL, 0);
 652                 if (status < 0) {
 653                         ERR("IOCTL MTP_SEND_RESET_ACK Failed [%d]\n",
 654                                 status);
 655                 }
 656                 break;
 657         case USB_PTPREQUEST_GETSTATUS:
 658
 659                 DBG("USB_PTPREQUEST_GETSTATUS");
 660
 661                 /* Send busy status response just once. This flag is also for
 662                  * the case that mtp misses the cancel request packet.
 663                  */
 664                 static mtp_bool sent_busy = FALSE;
 665                 usb_status_req_t statusreq_data = { 0 };
 666                 mtp_dword num_param = 0;
 667
 668                 memset(&statusreq_data, 0x00, sizeof(usb_status_req_t));
 669                 if (host_cancel == TRUE || (sent_busy == FALSE &&
 670                                             kernel_reset == FALSE)) {
 671                         DBG("Send busy response, set host_cancel to FALSE");
 672                         statusreq_data.len = 0x08;
 673                         statusreq_data.code = PTP_RESPONSE_DEVICEBUSY;
 674                         host_cancel = FALSE;
 675                 } else if (_device_get_phase() == DEVICE_PHASE_NOTREADY) {
 676                         statusreq_data.code =
 677                                 PTP_RESPONSE_TRANSACTIONCANCELLED;
 678                         DBG("PTP_RESPONSE_TRANSACTIONCANCELLED");
 679                         statusreq_data.len = (mtp_word)(sizeof(usb_status_req_t) +
 680                                                         (num_param - 2) * sizeof(mtp_dword));
 681                 } else if (_device_get_status() == DEVICE_STATUSOK) {
 682                         DBG("PTP_RESPONSE_OK");
 683                         statusreq_data.len = 0x08;
 684                         statusreq_data.code = PTP_RESPONSE_OK;
 685
 686                         if (kernel_reset == TRUE)
 687                                 kernel_reset = FALSE;
 688                 } else {
 689                         DBG("PTP_RESPONSE_GEN_ERROR");
 690                         statusreq_data.len = 0x08;
 691                         statusreq_data.code = PTP_RESPONSE_GEN_ERROR;
 692                 }
 693
 694                 if (statusreq_data.code == PTP_RESPONSE_DEVICEBUSY) {
 695                         sent_busy = TRUE;
 696                 } else {
 697                         sent_busy = FALSE;
 698                 }
 699
 700                 /* status = ioctl(g_usb_fd, MTP_SET_SETUP_DATA, &statusreq_data);
 701                 if (status < 0) {
 702                         DBG("IOCTL MTP_SET_SETUP_DATA Fail [%d]\n",
 703                                         status);
 704                         return;
 705                 } */
 706
 707                 break;
 708
 709         case USB_PTPREQUEST_GETEVENT:
 710                 DBG("USB_PTPREQUEST_GETEVENT");
 711                 break;
 712
 713         default:
 714                 DBG("Invalid class specific setup request");
 715                 break;
 716         }
 717         return;
 718 }
 719
 720 /*
 721  * mtp_bool ffs_transport_mq_deinit()
 722  * This function destroy a message queue for MTP,
 723  * @return      This function returns TRUE on success or
 724  *              returns FALSE on failure.
 725  */
 726 static mtp_bool ffs_transport_mq_deinit(msgq_id_t *rx_mqid, msgq_id_t *tx_mqid)
 727 {
 728         mtp_int32 res = TRUE;
 729
 730         if (*rx_mqid) {
 731                 res = _util_msgq_deinit(rx_mqid);
 732                 if (res == FALSE) {
 733                         ERR("rx_mqid deinit Fail [%d]\n", errno);
 734                 } else {
 735                         *rx_mqid = 0;
 736                 }
 737         }
 738
 739         if (*tx_mqid) {
 740                 res = _util_msgq_deinit(tx_mqid);
 741                 if (res == FALSE) {
 742                         ERR("tx_mqid deinit fail [%d]\n", errno);
 743                 } else {
 744                         *tx_mqid = 0;
 745                 }
 746         }
 747
 748         return res;
 749 }
 750
 751 static mtp_uint32 ffs_transport_get_usb_packet_len(void)
 752 {
 753         mtp_int32 status = -1;
 754         static mtp_int32 usb_speed = 0;
 755
 756         if (usb_speed == 0) {
 757
 758                 //status = ioctl(g_usb_fd, MTP_GET_HIGH_FULL_SPEED, &usb_speed);
 759                 if (status < 0) {
 760                         ERR("MTP_GET_HIGH_FULL_SPEED Failed [%d]\n", status);
 761                         return MTP_MAX_PACKET_SIZE_SEND_FS;
 762                 }
 763         }
 764
 765         if (usb_speed % MTP_MAX_PACKET_SIZE_SEND_HS) {
 766                 return MTP_MAX_PACKET_SIZE_SEND_FS;
 767         }
 768
 769         return MTP_MAX_PACKET_SIZE_SEND_HS;
 770 }
 771
 772
 773 const mtp_usb_driver_t mtp_usb_driver_ffs = {
 774         .transport_init_usb_device = ffs_transport_init_usb_device,
 775         .transport_deinit_usb_device = ffs_transport_deinit_usb_device,
 776         .transport_thread_usb_write = ffs_transport_thread_usb_write,
 777         .transport_thread_usb_read = ffs_transport_thread_usb_read,
 778         .transport_thread_usb_control = ffs_transport_thread_usb_control,
 779         .transport_mq_init = ffs_transport_mq_init,
 780         .transport_mq_deinit = ffs_transport_mq_deinit,
 781         .transport_get_usb_packet_len = ffs_transport_get_usb_packet_len,
 782         .get_tx_pkt_size = ffs_get_tx_pkt_size,
 783         .get_rx_pkt_size = ffs_get_rx_pkt_size,
 784 };