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 "mtp_descs_strings.h"
  29 #include "ptp_datacodes.h"
  30 #include "mtp_support.h"
  31 #include "ptp_container.h"
  32 #include "mtp_msgq.h"
  33 #include "mtp_thread.h"
  34 #include "mtp_transport.h"
  35 #include "mtp_event_handler.h"
  36 #include <sys/prctl.h>
  37 #include <systemd/sd-daemon.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
  48 /*PIMA15740-2000 spec*/
  49 #define USB_PTPREQUEST_CANCELIO   0x64    /* Cancel request */
  50 #define USB_PTPREQUEST_GETEVENT   0x65    /* Get extened event data */
  51 #define USB_PTPREQUEST_RESET      0x66    /* Reset Device */
  52 #define USB_PTPREQUEST_GETSTATUS  0x67    /* Get Device Status */
  53 #define USB_PTPREQUEST_CANCELIO_SIZE 6
  54 #define USB_PTPREQUEST_GETSTATUS_SIZE 12
  55
  56 static mtp_int32 g_usb_ep0 = -1;       /* read (g_usb_ep0, ...) */
  57 static mtp_int32 g_usb_ep_in = -1;     /* write (g_usb_ep_in, ...) */
  58 static mtp_int32 g_usb_ep_out = -1;    /* read (g_usb_ep_out, ...) */
  59 static mtp_int32 g_usb_ep_status = -1; /* write (g_usb_ep_status, ...) */
  60
  61 static mtp_max_pkt_size_t pkt_size;
  62 static mtp_uint32 rx_mq_sz;
  63 static mtp_uint32 tx_mq_sz;
  64 static mtp_int32 __handle_usb_read_err(mtp_int32 err,
  65                 mtp_uchar *buf, mtp_int32 buf_len);
  66 static void __clean_up_msg_queue(void *param);
  67 static void __handle_control_request(mtp_int32 request);
  68
  69 /*
  70  * FUNCTIONS
  71  */
  72
  73 static mtp_bool __io_init()
  74 {
  75         int ret;
  76
  77         if (sd_listen_fds(0) >= 4) {
  78                 g_usb_ep0 = SD_LISTEN_FDS_START;
  79                 g_usb_ep_in = SD_LISTEN_FDS_START + 1;
  80                 g_usb_ep_out = SD_LISTEN_FDS_START + 2;
  81                 g_usb_ep_status = SD_LISTEN_FDS_START + 3;
  82
  83                 return TRUE;
  84         }
  85
  86         g_usb_ep0 = open(MTP_EP0_PATH, O_RDWR);
  87         if (g_usb_ep0 < 0)
  88                 goto out;
  89
  90         DBG("Writing USB descriptors");
  91         ret = write(g_usb_ep0, &descriptors, sizeof(descriptors));
  92         if (ret < 0) {
  93                 ERR("Error writing descriptors");
  94                 goto cleanup;
  95         }
  96
  97         DBG("Writing USB strings");
  98         ret = write(g_usb_ep0, &strings, sizeof(strings));
  99         if (ret < 0) {
 100                 ERR("Error writing strings");
 101                 goto cleanup;
 102         }
 103
 104         g_usb_ep_in = open(MTP_EP_IN_PATH, O_RDWR);
 105         if (g_usb_ep_in < 0) {
 106                 ERR("Error opening bulk-in");
 107                 goto cleanup;
 108         }
 109
 110         g_usb_ep_out = open(MTP_EP_OUT_PATH, O_RDWR);
 111         if (g_usb_ep_out < 0) {
 112                 ERR("Error opening bulk-out");
 113                 goto cleanup_in;
 114         }
 115
 116         g_usb_ep_status = open(MTP_EP_STATUS_PATH, O_RDWR);
 117         if (g_usb_ep_status < 0) {
 118                 ERR("Error opening status");
 119                 goto cleanup_out;
 120         }
 121
 122         return TRUE;
 123
 124 cleanup_out:
 125         close(g_usb_ep_out);
 126 cleanup_in:
 127         close(g_usb_ep_in);
 128 cleanup:
 129         close(g_usb_ep0);
 130 out:
 131         return FALSE;
 132 }
 133
 134 static mtp_bool ffs_transport_init_usb_device(void)
 135 {
 136         mtp_int32 status = 0;
 137         int msg_size;
 138
 139         if (g_usb_ep0 > 0) {
 140                 DBG("Device Already open\n");
 141                 return TRUE;
 142         }
 143
 144         status = __io_init();
 145         if (!status) {
 146                 char error[256];
 147                 ERR("Device node [%s] open failed, errno [%s]\n",
 148                     MTP_EP0_PATH, strerror_r(errno, error, sizeof(error)));
 149                 return FALSE;
 150         }
 151
 152         pkt_size.rx = g_conf.read_usb_size;
 153         pkt_size.tx = g_conf.write_usb_size;
 154
 155         DBG("Final : Tx pkt size:[%u], Rx pkt size:[%u]\n", pkt_size.tx, pkt_size.rx);
 156
 157         msg_size = sizeof(msgq_ptr_t) - sizeof(long);
 158         rx_mq_sz = (g_conf.max_io_buf_size / g_conf.max_rx_ipc_size) * msg_size;
 159         tx_mq_sz = (g_conf.max_io_buf_size / g_conf.max_tx_ipc_size) * msg_size;
 160
 161         DBG("RX MQ size :[%u], TX MQ size:[%u]\n", rx_mq_sz, tx_mq_sz);
 162
 163         return TRUE;
 164 }
 165
 166 static void ffs_transport_deinit_usb_device(void)
 167 {
 168         if (g_usb_ep0 >= 0)
 169                 close(g_usb_ep0);
 170         g_usb_ep0 = -1;
 171
 172         if (g_usb_ep_in >= 0)
 173                 close(g_usb_ep_in);
 174         g_usb_ep_in = -1;
 175
 176         if (g_usb_ep_out >= 0)
 177                 close(g_usb_ep_out);
 178         g_usb_ep_out = -1;
 179
 180         if (g_usb_ep_status >= 0)
 181                 close(g_usb_ep_status);
 182         g_usb_ep_status = -1;
 183
 184         return;
 185 }
 186
 187 static mtp_uint32 ffs_get_tx_pkt_size(void)
 188 {
 189         return pkt_size.tx;
 190 }
 191
 192 static mtp_uint32 ffs_get_rx_pkt_size(void)
 193 {
 194         return pkt_size.rx;
 195 }
 196
 197 /*
 198  * static mtp_int32 ffs_transport_mq_init()
 199  * This function create a message queue for MTP,
 200  * A created message queue will be used to help data transfer between
 201  * MTP module and usb buffer.
 202  * @return      This function returns TRUE on success or
 203  *              returns FALSE on failure.
 204  */
 205 static mtp_int32 ffs_transport_mq_init(msgq_id_t *rx_mqid, msgq_id_t *tx_mqid)
 206 {
 207         if (_util_msgq_init(rx_mqid, 0) == FALSE) {
 208                 ERR("RX MQ init Fail [%d]\n", errno);
 209                 return FALSE;
 210         }
 211
 212         if (_util_msgq_set_size(*rx_mqid, rx_mq_sz) == FALSE)
 213                 ERR("RX MQ setting size Fail [%d]\n", errno);
 214
 215         if (_util_msgq_init(tx_mqid, 0) == FALSE) {
 216                 ERR("TX MQ init Fail [%d]\n", errno);
 217                 _util_msgq_deinit(rx_mqid);
 218                 *rx_mqid = -1;
 219                 return FALSE;
 220         }
 221
 222         if (_util_msgq_set_size(*tx_mqid, tx_mq_sz) == FALSE)
 223                 ERR("TX MQ setting size Fail [%d]\n", errno);
 224
 225         return TRUE;
 226 }
 227
 228 static void *ffs_transport_thread_usb_write(void *arg)
 229 {
 230         mtp_int32 status = 0;
 231         mtp_uint32 len = 0;
 232         unsigned char *mtp_buf = NULL;
 233         msg_type_t mtype = MTP_UNDEFINED_PACKET;
 234         msgq_id_t *mqid = (msgq_id_t *)arg;
 235
 236         pthread_cleanup_push(__clean_up_msg_queue, mqid);
 237
 238         do {
 239                 /* original LinuxThreads cancelation didn't work right
 240                  * so test for it explicitly.
 241                  */
 242                 pthread_testcancel();
 243
 244                 _util_rcv_msg_from_mq(*mqid, &mtp_buf, &len, &mtype);
 245
 246                 if (mtype == MTP_BULK_PACKET || mtype == MTP_DATA_PACKET) {
 247                         status = write(g_usb_ep_in, mtp_buf, len);
 248                         if (status < 0) {
 249                                 ERR("USB write fail : %d\n", errno);
 250                                 if (errno == ENOMEM || errno == ECANCELED) {
 251                                         status = 0;
 252                                         __clean_up_msg_queue(mqid);
 253                                 }
 254                         }
 255                         g_free(mtp_buf);
 256                         mtp_buf = NULL;
 257                 } else if (MTP_EVENT_PACKET == mtype) {
 258                         /* Handling the MTP Asynchronous Events */
 259                         DBG("Send Interrupt data to kernel via g_usb_ep_status\n");
 260                         status = write(g_usb_ep_status, mtp_buf, len);
 261                         g_free(mtp_buf);
 262                         mtp_buf = NULL;
 263                 } else if (MTP_ZLP_PACKET == mtype) {
 264                         DBG("Send ZLP data to kerne via g_usb_ep_in\n");
 265                         status = write(g_usb_ep_in, (void*)0xFEE1DEAD, 0);
 266                 } else {
 267                         DBG("mtype = %d is not valid\n", mtype);
 268                         status = -1;
 269                 }
 270
 271                 if (status < 0) {
 272                         ERR("write data to the device node Fail:\
 273                                         status = %d\n", status);
 274                         break;
 275                 }
 276         } while (status >= 0);
 277
 278         DBG("exited Source thread with status %d\n", status);
 279         pthread_cleanup_pop(1);
 280         g_free(mtp_buf);
 281
 282         return NULL;
 283 }
 284
 285 static int __setup(int ep0, struct usb_ctrlrequest *ctrl)
 286 {
 287         const char* requests[] = {
 288                 "CANCELIO",     /* 0x64 */
 289                 "GETEVENT",     /* 0x65 */
 290                 "RESET",        /* 0x66 */
 291                 "GETSTATUS",    /* 0x67 */
 292         };
 293         __u16 wIndex = le16_to_cpu(ctrl->wIndex);
 294         __u16 wValue = le16_to_cpu(ctrl->wValue);
 295         __u16 wLength = le16_to_cpu(ctrl->wLength);
 296         int rc = -EOPNOTSUPP;
 297         int status = 0;
 298
 299         if ((ctrl->bRequestType & 0x7f) != (USB_TYPE_CLASS | USB_RECIP_INTERFACE)) {
 300                 DBG(__FILE__ "(%s):%d: Invalid request type: %d",
 301                     __func__, __LINE__, ctrl->bRequestType);
 302                 goto stall;
 303         }
 304
 305         switch (((ctrl->bRequestType & 0x80) << 8) | ctrl->bRequest) {
 306         case ((USB_DIR_OUT << 8) | USB_PTPREQUEST_CANCELIO):
 307
 308                 DBG(__FILE__ "(%s):%d: USB_PTPREQUEST_%s",
 309                     __func__, __LINE__, requests[ctrl->bRequest-0x64]);
 310                 if (wValue != 0 || wIndex != 0 || wLength != 6) {
 311                         DBG("Invalid request parameters: wValue:%d wIndex:%d wLength:%d\n");
 312                         rc = -EINVAL;
 313                         goto stall;
 314                 }
 315                 __handle_control_request(ctrl->bRequest);
 316                 break;
 317
 318         case ((USB_DIR_IN << 8) | USB_PTPREQUEST_GETSTATUS):
 319         case ((USB_DIR_OUT << 8) | USB_PTPREQUEST_RESET):
 320
 321                 DBG(__FILE__ "(%s):%d: USB_PTPREQUEST_%s",
 322                     __func__, __LINE__, requests[ctrl->bRequest-0x64]);
 323                 __handle_control_request(ctrl->bRequest);
 324                 break;
 325
 326         case ((USB_DIR_IN << 8) | USB_PTPREQUEST_GETEVENT):
 327
 328                 /* Optional, may stall */
 329                 DBG(__FILE__ "(%s):%d: USB_PTPREQUEST_%s",
 330                     __func__, __LINE__, requests[ctrl->bRequest-0x64]);
 331                 rc = -EOPNOTSUPP;
 332                 goto stall;
 333                 break;
 334
 335         default:
 336                 DBG(__FILE__ "(%s):%d: Invalid request: %d", __func__,
 337                     __LINE__, ctrl->bRequest);
 338                 goto stall;
 339         }
 340         return 0;
 341
 342 stall:
 343
 344         DBG(__FILE__"(%s):%d:stall %0x2x.%02x\n",
 345             __func__, __LINE__, ctrl->bRequestType, ctrl->bRequest);
 346         if ((ctrl->bRequestType & 0x80) == USB_DIR_IN)
 347                 status = read(g_usb_ep0, NULL, 0);
 348         else
 349                 status = write(g_usb_ep0, NULL, 0);
 350
 351         if (status != -1 || errno != EL2HLT) {
 352                 ERR(__FILE__"(%s):%d:stall error\n",
 353                     __func__, __LINE__, ctrl->bRequestType, ctrl->bRequest);
 354                 rc = errno;
 355         }
 356         return rc;
 357 }
 358
 359 static void *ffs_transport_thread_usb_read(void *arg)
 360 {
 361         mtp_int32 status = 0;
 362         msgq_ptr_t pkt = {MTP_DATA_PACKET, 0, 0, NULL};
 363         msgq_id_t *mqid = (msgq_id_t *)arg;
 364         mtp_uint32 rx_size = _get_rx_pkt_size();
 365
 366         pthread_cleanup_push(__clean_up_msg_queue, mqid);
 367
 368         do {
 369                 pthread_testcancel();
 370
 371                 pkt.buffer = (mtp_uchar *)g_malloc(rx_size);
 372                 if (NULL == pkt.buffer) {
 373                         ERR("Sink thread: memalloc failed.\n");
 374                         break;
 375                 }
 376
 377                 status = read(g_usb_ep_out, pkt.buffer, rx_size);
 378                 if (status <= 0) {
 379                         status = __handle_usb_read_err(status, pkt.buffer, rx_size);
 380                         if (status <= 0) {
 381                                 ERR("__handle_usb_read_err is failed\n");
 382                                 g_free(pkt.buffer);
 383                                 break;
 384                         }
 385                 }
 386
 387                 pkt.length = status;
 388                 if (FALSE == _util_msgq_send(*mqid, (void *)&pkt,
 389                                              sizeof(msgq_ptr_t) - sizeof(long), 0)) {
 390                         ERR("msgsnd Fail");
 391                         g_free(pkt.buffer);
 392                 }
 393         } while (status > 0);
 394
 395         DBG("status[%d] errno[%d]\n", status, errno);
 396         pthread_cleanup_pop(1);
 397
 398         return NULL;
 399 }
 400
 401 static void *ffs_transport_thread_usb_control(void *arg)
 402 {
 403         mtp_int32 status = 0;
 404         struct usb_functionfs_event event;
 405         msgq_id_t *mqid = (msgq_id_t *)arg;
 406
 407         pthread_cleanup_push(__clean_up_msg_queue, mqid);
 408
 409         do {
 410                 pthread_testcancel();
 411
 412                 status = read(g_usb_ep0, &event, sizeof(event));
 413                 if (status < 0) {
 414                         char error[256];
 415                         ERR("read from ep0 failed: %s",
 416                             strerror_r(errno, error, sizeof(error)));
 417                         continue;
 418                 }
 419                 DBG("FUNCTIONFS event received: %d", event.type);
 420
 421                 switch (event.type) {
 422                 case FUNCTIONFS_SETUP:
 423                         DBG("SETUP: bmRequestType:%d bRequest:%d wValue:%d wIndex:%d wLength:%d\n",
 424                             event.u.setup.bRequestType,
 425                             event.u.setup.bRequest,
 426                             event.u.setup.wValue,
 427                             event.u.setup.wIndex,
 428                             event.u.setup.wLength);
 429                         __setup(g_usb_ep0, &event.u.setup);
 430                         break;
 431                 }
 432         } while (status > 0);
 433
 434         DBG("status[%d] errno[%d]\n", status, errno);
 435         pthread_cleanup_pop(1);
 436
 437         return NULL;
 438 }
 439
 440 static mtp_int32 __handle_usb_read_err(mtp_int32 err,
 441                 mtp_uchar *buf, mtp_int32 buf_len)
 442 {
 443         mtp_int32 retry = 0;
 444         mtp_bool ret;
 445
 446         while (retry++ < MTP_USB_ERROR_MAX_RETRY) {
 447                 if (err == 0) {
 448                         DBG("ZLP(Zero Length Packet). Skip");
 449                 } else if (err < 0 && errno == EINTR) {
 450                         DBG("read () is interrupted. Skip");
 451                 } else if (err < 0 && errno == EIO) {
 452                         DBG("EIO");
 453
 454                         if (MTP_PHONE_USB_CONNECTED !=
 455                             _util_get_local_usb_status()) {
 456                                 ERR("USB is disconnected");
 457                                 break;
 458                         }
 459
 460                         _transport_deinit_usb_device();
 461                         ret = _transport_init_usb_device();
 462                         if (ret == FALSE) {
 463                                 ERR("_transport_init_usb_device Fail");
 464                                 continue;
 465                         }
 466                 } else {
 467                         ERR("Unknown error : %d, errno [%d] \n", err, errno);
 468                         break;
 469                 }
 470
 471                 err = read(g_usb_ep_out, buf, buf_len);
 472                 if (err > 0)
 473                         break;
 474         }
 475
 476         if (err <= 0)
 477                 ERR("USB error handling Fail");
 478
 479         return err;
 480 }
 481
 482 static void __clean_up_msg_queue(void *mq_id)
 483 {
 484         mtp_int32 len = 0;
 485         msgq_ptr_t pkt = { 0 };
 486         msgq_id_t l_mqid = *(msgq_id_t *)mq_id;
 487
 488         ret_if(mq_id == NULL);
 489
 490         _transport_set_control_event(PTP_EVENTCODE_CANCELTRANSACTION);
 491         while (TRUE == _util_msgq_receive(l_mqid, (void *)&pkt,
 492                                           sizeof(msgq_ptr_t) - sizeof(long), 1, &len)) {
 493                 g_free(pkt.buffer);
 494                 memset(&pkt, 0, sizeof(msgq_ptr_t));
 495         }
 496
 497         return;
 498 }
 499
 500 static void __handle_control_request(mtp_int32 request)
 501 {
 502         static mtp_bool kernel_reset = FALSE;
 503         static mtp_bool host_cancel = FALSE;
 504         mtp_int32 status = 0;
 505
 506         switch (request) {
 507         case USB_PTPREQUEST_CANCELIO:
 508                 // XXX: Convert cancel request data from little-endian
 509                 // before use:  le32_to_cpu(x), le16_to_cpu(x).
 510                 DBG("USB_PTPREQUEST_CANCELIO\n");
 511                 cancel_req_t cancelreq_data;
 512
 513                 host_cancel = TRUE;
 514                 _transport_set_control_event(PTP_EVENTCODE_CANCELTRANSACTION);
 515                 status = read(g_usb_ep0, &cancelreq_data, sizeof(cancelreq_data));
 516                 if (status < 0) {
 517                         char error[256];
 518                         ERR("Failed to read data for CANCELIO request\n: %s",
 519                                         strerror_r(errno, error, sizeof(error)));
 520                 }
 521                 break;
 522
 523         case USB_PTPREQUEST_RESET:
 524
 525                 DBG("USB_PTPREQUEST_RESET\n");
 526                 _reset_mtp_device();
 527                 if (kernel_reset == FALSE) {
 528                         kernel_reset = TRUE;
 529                 }
 530
 531                 status = read(g_usb_ep0, NULL, 0);
 532                 if (status < 0) {
 533                         ERR("IOCTL MTP_SEND_RESET_ACK Failed [%d]\n",
 534                                 status);
 535                 }
 536                 break;
 537         case USB_PTPREQUEST_GETSTATUS:
 538
 539                 DBG("USB_PTPREQUEST_GETSTATUS");
 540
 541                 /* Send busy status response just once. This flag is also for
 542                  * the case that mtp misses the cancel request packet.
 543                  */
 544                 static mtp_bool sent_busy = FALSE;
 545                 usb_status_req_t statusreq_data = { 0 };
 546                 mtp_dword num_param = 0;
 547
 548                 memset(&statusreq_data, 0x00, sizeof(usb_status_req_t));
 549                 if (host_cancel == TRUE || (sent_busy == FALSE &&
 550                                             kernel_reset == FALSE)) {
 551                         DBG("Send busy response, set host_cancel to FALSE");
 552                         statusreq_data.len = 0x08;
 553                         statusreq_data.code = PTP_RESPONSE_DEVICEBUSY;
 554                         host_cancel = FALSE;
 555                 } else if (_device_get_phase() == DEVICE_PHASE_NOTREADY) {
 556                         statusreq_data.code =
 557                                 PTP_RESPONSE_TRANSACTIONCANCELLED;
 558                         DBG("PTP_RESPONSE_TRANSACTIONCANCELLED");
 559                         statusreq_data.len = (mtp_word)(sizeof(usb_status_req_t) +
 560                                                         (num_param - 2) * sizeof(mtp_dword));
 561                 } else if (_device_get_status() == DEVICE_STATUSOK) {
 562                         DBG("PTP_RESPONSE_OK");
 563                         statusreq_data.len = 0x08;
 564                         statusreq_data.code = PTP_RESPONSE_OK;
 565
 566                         if (kernel_reset == TRUE)
 567                                 kernel_reset = FALSE;
 568                 } else {
 569                         DBG("PTP_RESPONSE_GEN_ERROR");
 570                         statusreq_data.len = 0x08;
 571                         statusreq_data.code = PTP_RESPONSE_GEN_ERROR;
 572                 }
 573
 574                 if (statusreq_data.code == PTP_RESPONSE_DEVICEBUSY) {
 575                         sent_busy = TRUE;
 576                 } else {
 577                         sent_busy = FALSE;
 578                 }
 579
 580                 /* status = ioctl(g_usb_fd, MTP_SET_SETUP_DATA, &statusreq_data);
 581                 if (status < 0) {
 582                         DBG("IOCTL MTP_SET_SETUP_DATA Fail [%d]\n",
 583                                         status);
 584                         return;
 585                 } */
 586
 587                 break;
 588
 589         case USB_PTPREQUEST_GETEVENT:
 590                 DBG("USB_PTPREQUEST_GETEVENT");
 591                 break;
 592
 593         default:
 594                 DBG("Invalid class specific setup request");
 595                 break;
 596         }
 597         return;
 598 }
 599
 600 /*
 601  * mtp_bool ffs_transport_mq_deinit()
 602  * This function destroy a message queue for MTP,
 603  * @return      This function returns TRUE on success or
 604  *              returns FALSE on failure.
 605  */
 606 static mtp_bool ffs_transport_mq_deinit(msgq_id_t *rx_mqid, msgq_id_t *tx_mqid)
 607 {
 608         mtp_int32 res = TRUE;
 609
 610         if (*rx_mqid) {
 611                 res = _util_msgq_deinit(rx_mqid);
 612                 if (res == FALSE) {
 613                         ERR("rx_mqid deinit Fail [%d]\n", errno);
 614                 } else {
 615                         *rx_mqid = 0;
 616                 }
 617         }
 618
 619         if (*tx_mqid) {
 620                 res = _util_msgq_deinit(tx_mqid);
 621                 if (res == FALSE) {
 622                         ERR("tx_mqid deinit fail [%d]\n", errno);
 623                 } else {
 624                         *tx_mqid = 0;
 625                 }
 626         }
 627
 628         return res;
 629 }
 630
 631 static mtp_uint32 ffs_transport_get_usb_packet_len(void)
 632 {
 633         mtp_int32 status = -1;
 634         static mtp_int32 usb_speed = 0;
 635
 636         if (usb_speed == 0) {
 637
 638                 //status = ioctl(g_usb_fd, MTP_GET_HIGH_FULL_SPEED, &usb_speed);
 639                 if (status < 0) {
 640                         ERR("MTP_GET_HIGH_FULL_SPEED Failed [%d]\n", status);
 641                         return MTP_MAX_PACKET_SIZE_SEND_FS;
 642                 }
 643         }
 644
 645         if (usb_speed % MTP_MAX_PACKET_SIZE_SEND_HS) {
 646                 return MTP_MAX_PACKET_SIZE_SEND_FS;
 647         }
 648
 649         return MTP_MAX_PACKET_SIZE_SEND_HS;
 650 }
 651
 652
 653 const mtp_usb_driver_t mtp_usb_driver_ffs = {
 654         .transport_init_usb_device = ffs_transport_init_usb_device,
 655         .transport_deinit_usb_device = ffs_transport_deinit_usb_device,
 656         .transport_thread_usb_write = ffs_transport_thread_usb_write,
 657         .transport_thread_usb_read = ffs_transport_thread_usb_read,
 658         .transport_thread_usb_control = ffs_transport_thread_usb_control,
 659         .transport_mq_init = ffs_transport_mq_init,
 660         .transport_mq_deinit = ffs_transport_mq_deinit,
 661         .transport_get_usb_packet_len = ffs_transport_get_usb_packet_len,
 662         .get_tx_pkt_size = ffs_get_tx_pkt_size,
 663         .get_rx_pkt_size = ffs_get_rx_pkt_size,
 664 };