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