hw/usb_cfs_client_common.c

   1 /*
   2  * libdevice-node
   3  *
   4  * Copyright (c) 2018 Samsung Electronics Co., Ltd.
   5  *
   6  * Licensed under the Apache License, Version 2.0 (the License);
   7  * you may not use this file except in compliance with the License.
   8  * You may obtain a copy of the License at
   9  *
  10  *     http://www.apache.org/licenses/LICENSE-2.0
  11  *
  12  * Unless required by applicable law or agreed to in writing, software
  13  * distributed under the License is distributed on an "AS IS" BASIS,
  14  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  15  * See the License for the specific language governing permissions and
  16  * limitations under the License.
  17  */
  18
  19 #include <hw/usb_client.h>
  20 #include <hw/systemd.h>
  21 #include <hw/shared.h>
  22
  23 #include <limits.h>
  24 #include <stdio.h>
  25 #include <string.h>
  26 #include <sys/stat.h>
  27 #include <sys/types.h>
  28 #include <sys/mount.h>
  29 #include <usbg/usbg.h>
  30 #include <unistd.h>
  31
  32 #include <unistd.h>
  33
  34 #define zalloc(amount) calloc(1, amount)
  35
  36 #define MAX_GADGET_STR_LEN 256
  37 #define MAX_FUNCS 32
  38
  39 #define CONFIGFS_PATH "/sys/kernel/config"
  40
  41 #define CONFIGFS_GADGET_NAME "hal-gadget"
  42 #define CONFIGFS_CONFIG_LABEL "hal-config"
  43
  44 #define NAME_INSTANCE_SEP '.'
  45 #define MAX_INSTANCE_LEN 512
  46
  47 #define USB_FUNCS_PATH "/dev/usb-funcs/"
  48
  49 #ifndef EXPORT
  50 #define EXPORT  __attribute__ ((visibility("default")))
  51 #endif
  52
  53 struct cfs_client {
  54         struct usb_client client;
  55         usbg_state *ctx;
  56         usbg_gadget *gadget;
  57         usbg_udc *udc;
  58 };
  59
  60 /* Based on values in slp-gadget kernel module */
  61 struct usbg_gadget_attrs default_g_attrs = {
  62         .bcdUSB = 0x0200,
  63         .idVendor = 0x04e8,
  64         .idProduct = 0x6860,
  65         .bcdDevice = 0x0100,
  66 };
  67
  68 struct usbg_gadget_strs default_g_strs = {
  69         .manufacturer = "Samsung",
  70         .product = "TIZEN",
  71         .serial = "01234TEST",
  72 };
  73
  74 static void cfs_free_config(struct usb_configuration *config)
  75 {
  76         int i;
  77
  78         if (!config)
  79                 return;
  80
  81         if (config->strs) {
  82                 for (i = 0; config->strs[i].lang_code; ++i)
  83                         free(config->strs[i].config_str);
  84
  85                 free(config->strs);
  86         }
  87
  88         /*
  89          * Each function will be free later,
  90          * for now we cleanup only pointers.
  91          */
  92         if (config->funcs)
  93                 free(config->funcs);
  94
  95         free(config);
  96 }
  97
  98 static void cfs_free_gadget(struct usb_gadget *gadget)
  99 {
 100         int i;
 101
 102         if (!gadget)
 103                 return;
 104
 105         if (gadget->strs) {
 106                 for (i = 0; gadget->strs[i].lang_code; ++i) {
 107                         free(gadget->strs[i].manufacturer);
 108                         free(gadget->strs[i].product);
 109                         free(gadget->strs[i].serial);
 110                 }
 111                 free(gadget->strs);
 112         }
 113
 114         if (gadget->configs) {
 115                 for (i = 0; gadget->configs[i]; ++i)
 116                         cfs_free_config(gadget->configs[i]);
 117
 118                 free(gadget->configs);
 119         }
 120
 121         if (gadget->funcs) {
 122                 for (i = 0; gadget->funcs[i]; ++i)
 123                         gadget->funcs[i]->free_func(gadget->funcs[i]);
 124
 125                 free(gadget->funcs);
 126         }
 127 }
 128
 129 static int cfs_read_gadget_attrs_strs(usbg_gadget *gadget,
 130                                       struct usb_gadget *usb_gadget)
 131 {
 132         struct usbg_gadget_attrs attrs;
 133         struct usbg_gadget_strs strs;
 134         int ret;
 135
 136         ret = usbg_get_gadget_attrs(gadget, &attrs);
 137         if (ret)
 138                 goto out;
 139
 140         usb_gadget->attrs.bDeviceClass = attrs.bDeviceClass;
 141         usb_gadget->attrs.bDeviceSubClass = attrs.bDeviceSubClass;
 142         usb_gadget->attrs.bDeviceProtocol = attrs.bDeviceProtocol;
 143         usb_gadget->attrs.idVendor = attrs.idVendor;
 144         usb_gadget->attrs.idProduct = attrs.idProduct;
 145         usb_gadget->attrs.bcdDevice = attrs.bcdDevice;
 146
 147
 148         ret = usbg_get_gadget_strs(gadget, LANG_US_ENG, &strs);
 149         if (ret)
 150                 goto out;
 151
 152         usb_gadget->strs[0].manufacturer = strdup(strs.manufacturer);
 153         usb_gadget->strs[0].product = strdup(strs.product);
 154         usb_gadget->strs[0].serial = strdup(strs.serial);
 155
 156         if (!usb_gadget->strs[0].manufacturer ||
 157             !usb_gadget->strs[0].product ||
 158             !usb_gadget->strs[0].serial) {
 159                 ret = -ENOMEM;
 160                 goto err_strs;
 161         }
 162
 163         return 0;
 164 err_strs:
 165         free(usb_gadget->strs[0].manufacturer);
 166         free(usb_gadget->strs[0].product);
 167         free(usb_gadget->strs[0].serial);
 168 out:
 169         return ret;
 170 }
 171
 172 static bool cfs_match_func(struct usb_function *f,
 173                          const char *name, const char *instance) {
 174         if (strcmp(name, usbg_get_function_type_str(USBG_F_FFS))) {
 175                 /* Standard functions */
 176                 if (!strcmp(name, f->name) && !strcmp(instance, f->instance))
 177                         return true;
 178         } else {
 179                 /* Function with service */
 180                 const char *sep, *fname, *finst;
 181                 int len;
 182
 183                 sep = strchr(instance, NAME_INSTANCE_SEP);
 184                 if (!sep || strlen(sep + 1) < 1)
 185                         return false;
 186
 187                 fname = instance;
 188                 len = sep - instance;
 189                 finst = sep + 1;
 190
 191                 if (strlen(f->name) == len
 192                     && !strncmp(f->name, fname, len)
 193                     && !strcmp(f->instance, finst))
 194                         return true;
 195         }
 196
 197         return false;
 198 }
 199
 200
 201 static int cfs_find_func(const char *name, const char *instance)
 202 {
 203         int i;
 204
 205         for (i = 0; i < ARRAY_SIZE(_available_funcs); ++i)
 206                 if (cfs_match_func(_available_funcs[i], name, instance))
 207                         return i;
 208
 209         return -ENOENT;
 210 }
 211
 212 static int cfs_alloc_new_func(struct usb_gadget *gadget, const char *fname,
 213                               const char *instance, struct usb_function **_func)
 214 {
 215         struct usb_function *func;
 216         int ret;
 217
 218         ret = cfs_find_func(fname, instance);
 219         if (ret < 0)
 220                 return -ENOTSUP;
 221
 222         ret = _available_funcs[ret]->clone(_available_funcs[ret], &func);
 223         if (ret)
 224                 return ret;
 225
 226         *_func = func;
 227         return 0;
 228 }
 229
 230 static int cfs_read_funcs(usbg_gadget *gadget, struct usb_gadget *usb_gadget)
 231 {
 232         usbg_function *func;
 233         int i;
 234         int ret;
 235
 236         i = 0;
 237         usbg_for_each_function(func, gadget) {
 238                 char *func_name = (char *)usbg_get_function_type_str(
 239                                             usbg_get_function_type(func));
 240                 char *instance = (char *)usbg_get_function_instance(func);
 241
 242                 ret = cfs_alloc_new_func(usb_gadget, func_name, instance,
 243                                          usb_gadget->funcs + i);
 244                 if (ret < 0)
 245                         goto clean_prev;
 246                 ++i;
 247         }
 248
 249         return 0;
 250 clean_prev:
 251         while (i >= 0) {
 252                 usb_gadget->funcs[i]->free_func(usb_gadget->funcs[i]);
 253                 --i;
 254         }
 255
 256         return ret;
 257 }
 258
 259 static struct usb_function *cfs_find_func_in_gadget(
 260         struct usb_gadget *gadget, const char *name, const char *instance)
 261 {
 262         int i;
 263
 264         for (i = 0; gadget->funcs[i]; ++i)
 265                 if (cfs_match_func(gadget->funcs[i], name, instance))
 266                         return gadget->funcs[i];
 267
 268         return NULL;
 269 }
 270
 271 static int cfs_alloc_config(int n_funcs, struct usb_configuration **_config)
 272 {
 273         struct usb_configuration *config;
 274
 275         config = zalloc(sizeof(*config));
 276         if (!config)
 277                 goto out;
 278
 279         config->strs = calloc(2, sizeof(*config->strs));
 280         if (!config->strs)
 281                 goto free_config;
 282
 283         config->funcs = calloc(n_funcs + 1, sizeof(*config->funcs));
 284         if (!config->funcs)
 285                 goto free_strs;
 286
 287         *_config = config;
 288
 289         return 0;
 290 free_strs:
 291         free(config->strs);
 292 free_config:
 293         free(config);
 294 out:
 295         return -ENOMEM;
 296 }
 297
 298 static int cfs_read_config(usbg_config *config, struct usb_gadget *gadget,
 299                            struct usb_configuration *usb_config)
 300 {
 301         usbg_binding *b;
 302         usbg_function *func;
 303         char *name, *instance;
 304         struct usbg_config_attrs c_attrs;
 305         struct usbg_config_strs c_strs;
 306         int i = 0;
 307         int ret;
 308
 309         usbg_for_each_binding(b, config) {
 310                 func = usbg_get_binding_target(b);
 311
 312                 name = (char *)usbg_get_function_type_str(
 313                         usbg_get_function_type(func));
 314                 instance = (char *)usbg_get_function_instance(func);
 315
 316                 usb_config->funcs[i] = cfs_find_func_in_gadget(gadget,
 317                                                                name, instance);
 318                 if (!usb_config->funcs[i]) {
 319                         return -ENOTSUP;
 320                 }
 321                 ++i;
 322         }
 323
 324         ret = usbg_get_config_attrs(config, &c_attrs);
 325         if (ret)
 326                 return ret;
 327
 328         usb_config->attrs.MaxPower = c_attrs.bMaxPower*2;
 329         usb_config->attrs.bmAttributs = c_attrs.bmAttributes;
 330
 331         ret = usbg_get_config_strs(config, LANG_US_ENG, &c_strs);
 332         if (ret) {
 333                 usb_config->strs[0].lang_code = 0;
 334         } else {
 335                 usb_config->strs[0].lang_code = LANG_US_ENG;
 336                 usb_config->strs[0].config_str = strdup(c_strs.configuration);
 337                 if (!usb_config->strs[0].config_str)
 338                         return -ENOMEM;
 339         }
 340
 341         return 0;
 342 }
 343
 344 static int cfs_count_bindings(usbg_config *config)
 345 {
 346         usbg_binding *b;
 347         int i = 0;
 348
 349         usbg_for_each_binding(b, config) ++i;
 350
 351         return i;
 352 }
 353
 354 static int cfs_read_configs(usbg_gadget *gadget, struct usb_gadget *usb_gadget)
 355 {
 356         usbg_config *config;
 357         int i = 0;
 358         int n_funcs;
 359         int ret;
 360
 361         usbg_for_each_config(config, gadget) {
 362                 n_funcs = cfs_count_bindings(config);
 363
 364                 ret = cfs_alloc_config(n_funcs, usb_gadget->configs + i);
 365                 if (ret)
 366                         goto clean_prev;
 367                 ret = cfs_read_config(config, usb_gadget,
 368                                       usb_gadget->configs[i]);
 369                 if (ret)
 370                         goto free_current;
 371
 372                 ++i;
 373         }
 374
 375         return 0;
 376 free_current:
 377         free(usb_gadget->configs[i]->strs);
 378         free(usb_gadget->configs[i]->funcs);
 379         free(usb_gadget->configs[i]);
 380 clean_prev:
 381         while (i >= 0)
 382                 cfs_free_config(usb_gadget->configs[i--]);
 383         return ret;
 384 }
 385
 386 static int cfs_count_configs(usbg_gadget *gadget)
 387 {
 388         usbg_config *config;
 389         int i = 0;
 390
 391         usbg_for_each_config(config, gadget) ++i;
 392
 393         return i;
 394 }
 395
 396 static int cfs_count_functions(usbg_gadget *gadget)
 397 {
 398         usbg_function *func;
 399         int i = 0;
 400
 401         usbg_for_each_function(func, gadget) ++i;
 402
 403         return i;
 404 }
 405
 406 static int cfs_get_current_gadget(struct usb_client *usb,
 407                                      struct usb_gadget **_usb_gadget)
 408 {
 409         struct cfs_client *cfs_client;
 410         struct usb_gadget *usb_gadget;
 411         struct usb_gadget_strings *strs;
 412         struct usb_configuration **usb_configs;
 413         struct usb_function **usb_funcs;
 414         int n_funcs, n_configs;
 415         int i;
 416         int ret = -ENOMEM;
 417
 418         if (!usb)
 419                 return -EINVAL;
 420
 421         cfs_client = container_of(usb, struct cfs_client,
 422                                   client);
 423
 424         usb_gadget = zalloc(sizeof(*usb_gadget));
 425         if (!usb_gadget)
 426                 goto out;
 427
 428         /*
 429          * Currently there is no interface in libusbg which
 430          * allows to list all string languages.
 431          * That's why we do this only for USA english
 432          */
 433         strs = calloc(2, sizeof(*strs));
 434         if (!strs)
 435                 goto free_gadget;
 436
 437         strs[0].lang_code = LANG_US_ENG;
 438
 439         usb_gadget->strs = strs;
 440
 441         ret = cfs_read_gadget_attrs_strs(cfs_client->gadget, usb_gadget);
 442         if (ret)
 443                 goto free_strs;
 444
 445
 446         n_funcs = cfs_count_functions(cfs_client->gadget);
 447         usb_funcs = calloc(n_funcs + 1, sizeof(*usb_funcs));
 448         if (!usb_funcs)
 449                 goto free_strs_with_content;
 450
 451         usb_gadget->funcs = usb_funcs;
 452
 453         ret = cfs_read_funcs(cfs_client->gadget, usb_gadget);
 454         if (ret)
 455                 goto free_funcs;
 456
 457         n_configs = cfs_count_configs(cfs_client->gadget);
 458         usb_configs = calloc(n_configs + 1, sizeof(*usb_configs));
 459         if (!usb_configs)
 460                 goto free_funcs_with_content;
 461
 462         usb_gadget->configs = usb_configs;
 463
 464         ret = cfs_read_configs(cfs_client->gadget, usb_gadget);
 465         if (ret)
 466                 goto free_configs;
 467
 468         *_usb_gadget = usb_gadget;
 469         return 0;
 470
 471 free_configs:
 472         free(usb_configs);
 473 free_funcs_with_content:
 474         for (i = 0; usb_gadget->funcs[i]; ++i)
 475                 usb_gadget->funcs[i]->free_func(usb_gadget->funcs[i]);
 476 free_funcs:
 477         free(usb_funcs);
 478 free_strs_with_content:
 479         for (i = 0; usb_gadget->strs[i].lang_code; ++i) {
 480                 free(usb_gadget->strs[i].manufacturer);
 481                 free(usb_gadget->strs[i].product);
 482                 free(usb_gadget->strs[i].serial);
 483         }
 484 free_strs:
 485         free(usb_gadget->strs);
 486 free_gadget:
 487         free(usb_gadget);
 488 out:
 489         return ret;
 490 }
 491
 492 static bool cfs_is_function_supported(struct usb_client *usb,
 493                                          struct usb_function *func)
 494 {
 495         bool res;
 496         int ret;
 497
 498         switch (func->function_group) {
 499         case USB_FUNCTION_GROUP_SIMPLE:
 500                 ret = usbg_lookup_function_type(func->name);
 501                 res = ret >= 0;
 502                 break;
 503         case USB_FUNCTION_GROUP_WITH_SERVICE:
 504                 /* TODO: Check if socket is available */
 505                 res = true;
 506                 break;
 507         default:
 508                 res = false;
 509         }
 510
 511         return res;
 512 }
 513
 514 static bool cfs_is_gadget_supported(struct usb_client *usb,
 515                                        struct usb_gadget *gadget)
 516 {
 517         int i, j;
 518
 519         if (!gadget || !gadget->configs || !gadget->funcs)
 520                 return false;
 521
 522         /*
 523          * TODO
 524          * Here is a good place to ensure that serial is immutable
 525          */
 526
 527         /* No real restrictions for strings */
 528         for (j = 0; gadget->configs && gadget->configs[j]; ++j) {
 529                 struct usb_configuration *config = gadget->configs[j];
 530
 531                 if (!config->funcs)
 532                         return false;
 533
 534                 for (i = 0; config->funcs[i]; ++i)
 535                         if (!cfs_is_function_supported(usb, config->funcs[i]))
 536                                 return false;
 537         }
 538
 539         if (j == 0)
 540                 return false;
 541
 542         return true;
 543 }
 544
 545 static int cfs_set_gadget_attrs(struct cfs_client *cfs_client,
 546                                 struct usb_gadget_attrs *attrs)
 547 {
 548         int ret;
 549         struct usbg_gadget_attrs gadget_attrs;
 550
 551         ret = usbg_get_gadget_attrs(cfs_client->gadget, &gadget_attrs);
 552         if (ret)
 553                 return ret;
 554
 555         gadget_attrs.bDeviceClass = attrs->bDeviceClass;
 556         gadget_attrs.bDeviceSubClass = attrs->bDeviceSubClass;
 557         gadget_attrs.bDeviceProtocol = attrs->bDeviceProtocol;
 558         gadget_attrs.idVendor = attrs->idVendor;
 559         gadget_attrs.idProduct = attrs->idProduct;
 560         gadget_attrs.bcdDevice = attrs->bcdDevice;
 561
 562         ret = usbg_set_gadget_attrs(cfs_client->gadget, &gadget_attrs);
 563
 564         return ret;
 565 }
 566
 567 static int cfs_set_gadget_strs(struct cfs_client *cfs_client,
 568                                   struct usb_gadget_strings *strs)
 569 {
 570         int ret = 0;
 571
 572         /*
 573          * TODO
 574          * Here is a good place to ensure that serial is immutable
 575          */
 576 #define SET_STR(FIELD, STR_ID)                          \
 577         if (strs->FIELD) {                              \
 578                 ret = usbg_set_gadget_str(cfs_client->gadget,   \
 579                                           STR_ID,               \
 580                                           strs->lang_code,      \
 581                                           strs->FIELD);         \
 582                 if (ret)                                        \
 583                         return ret;                             \
 584         }
 585
 586         SET_STR(manufacturer, USBG_STR_MANUFACTURER);
 587         SET_STR(product, USBG_STR_PRODUCT);
 588         SET_STR(serial, USBG_STR_SERIAL_NUMBER);
 589 #undef SET_STR
 590         return ret;
 591 }
 592
 593 static int cfs_ensure_dir(char *path)
 594 {
 595         int ret;
 596
 597         ret = mkdir(path, 0770);
 598         if (ret < 0)
 599                 ret = errno == EEXIST ? 0 : errno;
 600
 601         return ret;
 602 }
 603
 604 static int cfs_prep_ffs_service(const char *name, const char *instance,
 605                                 const char *dev_name, const char *socket_name)
 606 {
 607         char buf[PATH_MAX];
 608         size_t left;
 609         char *pos;
 610         int ret;
 611
 612         /* TODO: Add some good error handling */
 613
 614         left = sizeof(buf);
 615         pos = buf;
 616         ret = snprintf(pos, left, "%s", USB_FUNCS_PATH);
 617         if (ret < 0 || ret >= left) {
 618                 return -ENAMETOOLONG;
 619         } else {
 620                 left -= ret;
 621                 pos += ret;
 622         }
 623         ret = cfs_ensure_dir(buf);
 624         if (ret < 0)
 625                 return ret;
 626
 627         ret = snprintf(pos, left, "/%s", name);
 628         if (ret < 0 || ret >= left) {
 629                 return -ENAMETOOLONG;
 630         } else {
 631                 left -= ret;
 632                 pos += ret;
 633         }
 634         ret = cfs_ensure_dir(buf);
 635         if (ret < 0)
 636                 return ret;
 637
 638         ret = snprintf(pos, left, "/%s", instance);
 639         if (ret < 0 || ret >= left) {
 640                 return -ENAMETOOLONG;
 641         } else {
 642                 left -= ret;
 643                 pos += ret;
 644         }
 645         ret = cfs_ensure_dir(buf);
 646         if (ret < 0)
 647                 return ret;
 648
 649         ret = mount(dev_name, buf, "functionfs", 0, NULL);
 650         if (ret < 0)
 651                 return ret;
 652
 653         ret = systemd_start_socket(socket_name);
 654         if (ret < 0)
 655                 goto umount_ffs;
 656
 657         return 0;
 658 umount_ffs:
 659         umount(buf);
 660         return ret;
 661 }
 662
 663 static int cfs_set_gadget_config(struct cfs_client *cfs_client,
 664                                     int config_id,
 665                                     struct usb_configuration *usb_config)
 666 {
 667         struct usbg_config_attrs cattrs = {
 668                 .bmAttributes = usb_config->attrs.bmAttributs,
 669                 .bMaxPower = usb_config->attrs.MaxPower/2,
 670         };
 671         usbg_config *config;
 672         int i;
 673         int ret;
 674
 675         if (!usb_config->funcs || !usb_config->funcs[0])
 676                 return -EINVAL;
 677
 678         config = usbg_get_config(cfs_client->gadget, config_id, NULL);
 679         if (config) {
 680                 ret = usbg_rm_config(config, USBG_RM_RECURSE);
 681                 if (ret)
 682                         return ret;
 683         }
 684
 685         ret = usbg_create_config(cfs_client->gadget, config_id,
 686                                  CONFIGFS_CONFIG_LABEL, &cattrs, NULL, &config);
 687         if (ret)
 688                 return ret;
 689
 690         for (i = 0; usb_config->strs && usb_config->strs[i].lang_code; ++i) {
 691                 ret = usbg_set_config_string(config, usb_config->strs[i].lang_code,
 692                                              usb_config->strs[i].config_str);
 693                 if (ret)
 694                         return ret;
 695         }
 696
 697         for (i = 0; usb_config->funcs && usb_config->funcs[i]; ++i) {
 698                 struct usb_function *usb_func = usb_config->funcs[i];
 699                 char instance[MAX_INSTANCE_LEN];
 700                 int type;
 701                 usbg_function *func;
 702
 703                 switch (usb_func->function_group) {
 704                 case USB_FUNCTION_GROUP_SIMPLE:
 705                         type = usbg_lookup_function_type(usb_func->name);
 706                         if (strlen(usb_func->instance) >= MAX_INSTANCE_LEN)
 707                                 return -ENAMETOOLONG;
 708                         strncpy(instance, usb_func->instance, MAX_INSTANCE_LEN);
 709                         instance[MAX_INSTANCE_LEN - 1] = '\0';
 710                         break;
 711                 case USB_FUNCTION_GROUP_WITH_SERVICE:
 712                         type = USBG_F_FFS;
 713                         ret = snprintf(instance, sizeof(instance), "%s%c%s",
 714                                        usb_func->name, NAME_INSTANCE_SEP,
 715                                        usb_func->instance);
 716                         if (ret < 0 || ret >= sizeof(instance))
 717                                 return -ENAMETOOLONG;
 718                         break;
 719                 default:
 720                         return -EINVAL;
 721                 }
 722
 723
 724                 func = usbg_get_function(cfs_client->gadget, type, instance);
 725                 if (!func) {
 726                         ret = usbg_create_function(cfs_client->gadget,
 727                                                    type,
 728                                                    instance,
 729                                                    NULL, &func);
 730                         if (ret)
 731                                 return ret;
 732
 733                         if (usb_func->function_group ==
 734                             USB_FUNCTION_GROUP_WITH_SERVICE) {
 735                                 struct usb_function_with_service *fws;
 736
 737                                 fws = container_of(usb_func,
 738                                                    struct usb_function_with_service,
 739                                                    func);
 740                                 ret = cfs_prep_ffs_service(usb_func->name,
 741                                                            usb_func->instance,
 742                                                            instance,
 743                                                            fws->service);
 744                                 if (ret)
 745                                         return ret;
 746                         }
 747
 748                 }
 749
 750                 ret = usbg_add_config_function(config, NULL, func);
 751                 if (ret)
 752                         return ret;
 753         }
 754
 755         return ret;
 756 }
 757
 758 static int cfs_cleanup_left_configs(struct cfs_client *cfs_client,
 759                                     int last_config)
 760 {
 761         usbg_config *lconfig, *config;
 762         int ret;
 763
 764         lconfig = usbg_get_config(cfs_client->gadget, last_config, NULL);
 765         for (config = usbg_get_next_config(lconfig);
 766              config;
 767              config = usbg_get_next_config(lconfig)) {
 768                 ret = usbg_rm_config(config, USBG_RM_RECURSE);
 769                 if (ret)
 770                         return ret;
 771         }
 772
 773         return 0;
 774 }
 775
 776 static int cfs_reconfigure_gadget(struct usb_client *usb,
 777                                   struct usb_gadget *gadget)
 778 {
 779         struct cfs_client *cfs_client;
 780         int i;
 781         int ret;
 782
 783         if (!usb)
 784                 return -EINVAL;
 785
 786         cfs_client = container_of(usb, struct cfs_client,
 787                                   client);
 788
 789         if (!usb || !gadget || !cfs_is_gadget_supported(usb, gadget))
 790                 return -EINVAL;
 791
 792         ret = cfs_set_gadget_attrs(cfs_client, &gadget->attrs);
 793         if (ret)
 794                 goto out;
 795
 796         for (i = 0; gadget->strs && gadget->strs[i].lang_code > 0; ++i) {
 797                 ret = cfs_set_gadget_strs(cfs_client, gadget->strs + i);
 798                 if (ret)
 799                         goto out;
 800         }
 801
 802         for (i = 0; gadget->configs && gadget->configs[i]; ++i) {
 803                 ret = cfs_set_gadget_config(cfs_client, i + 1,
 804                                             gadget->configs[i]);
 805                 if (ret)
 806                         goto out;
 807         }
 808
 809         /* Workaround for enabling extcon notification */
 810         ret = usbg_enable_gadget(cfs_client->gadget, cfs_client->udc);
 811         if (ret)
 812                 goto out;
 813
 814         ret = cfs_cleanup_left_configs(cfs_client, i);
 815
 816         /* TODO
 817          * Cleanup things which are left after previous gadget
 818          */
 819 out:
 820         return ret;
 821 }
 822
 823 static int cfs_enable(struct usb_client *usb)
 824 {
 825         struct cfs_client *cfs_client;
 826
 827         if (!usb)
 828                 return -EINVAL;
 829
 830         cfs_client = container_of(usb, struct cfs_client,
 831                                   client);
 832
 833         return usbg_enable_gadget(cfs_client->gadget, cfs_client->udc);
 834 }
 835
 836 static int cfs_disable(struct usb_client *usb)
 837 {
 838         struct cfs_client *cfs_client;
 839
 840         if (!usb)
 841                 return -EINVAL;
 842
 843         cfs_client = container_of(usb, struct cfs_client,
 844                                   client);
 845
 846         return usbg_disable_gadget(cfs_client->gadget);
 847 }
 848
 849 EXPORT
 850 int hw_cfs_gadget_open(struct hw_info *info,
 851                 const char *id, struct hw_common **common)
 852 {
 853         struct cfs_client *cfs_client;
 854         int ret;
 855
 856         if (!info || !common)
 857                 return -EINVAL;
 858
 859         cfs_client = zalloc(sizeof(*cfs_client));
 860         if (!cfs_client)
 861                 return -ENOMEM;
 862
 863         ret = usbg_init(CONFIGFS_PATH, &cfs_client->ctx);
 864         if (ret)
 865                 goto err_usbg_init;
 866
 867         cfs_client->udc = usbg_get_first_udc(cfs_client->ctx);
 868         if (!cfs_client->udc) {
 869                 ret = -ENODEV;
 870                 goto err_no_udc;
 871         }
 872
 873         ret = usbg_create_gadget(cfs_client->ctx, CONFIGFS_GADGET_NAME,
 874                                  &default_g_attrs, &default_g_strs,
 875                                  &cfs_client->gadget);
 876         if (ret)
 877                 goto err_create_gadget;
 878
 879         cfs_client->client.common.info = info;
 880         cfs_client->client.get_current_gadget = cfs_get_current_gadget;
 881         cfs_client->client.reconfigure_gadget = cfs_reconfigure_gadget;
 882         cfs_client->client.is_gadget_supported = cfs_is_gadget_supported;
 883         cfs_client->client.is_function_supported = cfs_is_function_supported;
 884         cfs_client->client.enable = cfs_enable;
 885         cfs_client->client.disable = cfs_disable;
 886         cfs_client->client.free_gadget = cfs_free_gadget;
 887
 888         *common = &cfs_client->client.common;
 889         return 0;
 890
 891 err_create_gadget:
 892 err_no_udc:
 893         usbg_cleanup(cfs_client->ctx);
 894 err_usbg_init:
 895         free(cfs_client);
 896
 897         return ret;
 898 }
 899
 900 EXPORT
 901 int hw_cfs_gadget_close(struct hw_common *common)
 902 {
 903         struct cfs_client *cfs_client;
 904
 905         if (!common)
 906                 return -EINVAL;
 907
 908         cfs_client = container_of(common, struct cfs_client,
 909                                   client.common);
 910
 911         /*
 912          * For now we don't check for errors
 913          * but we should somehow handle them
 914          */
 915         usbg_rm_gadget(cfs_client->gadget, USBG_RM_RECURSE);
 916         usbg_cleanup(cfs_client->ctx);
 917         free(cfs_client);
 918
 919         return 0;
 920 }
 921