hw/usb_cfs_client/usb_cfs_client.c

   1 /*
   2  * device-node
   3  *
   4  * Copyright (c) 2016 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
  21 #include "../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 #include <systemd/sd-bus.h>
  32
  33 #include <unistd.h>
  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 struct cfs_client {
  51         struct usb_client client;
  52         usbg_state *ctx;
  53         usbg_gadget *gadget;
  54         usbg_udc *udc;
  55 };
  56
  57 /* Based on values in slp-gadget kernel module */
  58 struct usbg_gadget_attrs default_g_attrs = {
  59         .bcdUSB = 0x0200,
  60         .idVendor = 0x04e8,
  61         .idProduct = 0x6860,
  62         .bcdDevice = 0x0100,
  63 };
  64
  65 struct usbg_gadget_strs default_g_strs = {
  66         .manufacturer = "Samsung",
  67         .product = "TIZEN",
  68         .serial = "01234TEST",
  69 };
  70
  71 static void cfs_free_config(struct usb_configuration *config)
  72 {
  73         int i;
  74
  75         if (!config)
  76                 return;
  77
  78         if (config->strs) {
  79                 for (i = 0; config->strs[i].lang_code; ++i)
  80                         free(config->strs[i].config_str);
  81
  82                 free(config->strs);
  83         }
  84
  85         /*
  86          * Each function will be free later,
  87          * for now we cleanup only pointers.
  88          */
  89         if (config->funcs)
  90                 free(config->funcs);
  91
  92         free(config);
  93 }
  94
  95 static void cfs_free_gadget(struct usb_gadget *gadget)
  96 {
  97         int i;
  98
  99         if (!gadget)
 100                 return;
 101
 102         if (gadget->strs) {
 103                 for (i = 0; gadget->strs[i].lang_code; ++i) {
 104                         free(gadget->strs[i].manufacturer);
 105                         free(gadget->strs[i].product);
 106                         free(gadget->strs[i].serial);
 107                 }
 108                 free(gadget->strs);
 109         }
 110
 111         if (gadget->configs) {
 112                 for (i = 0; gadget->configs[i]; ++i)
 113                         cfs_free_config(gadget->configs[i]);
 114
 115                 free(gadget->configs);
 116         }
 117
 118         if (gadget->funcs) {
 119                 for (i = 0; gadget->funcs[i]; ++i)
 120                         gadget->funcs[i]->free_func(gadget->funcs[i]);
 121
 122                 free(gadget->funcs);
 123         }
 124 }
 125
 126 static int cfs_read_gadget_attrs_strs(usbg_gadget *gadget,
 127                                       struct usb_gadget *usb_gadget)
 128 {
 129         struct usbg_gadget_attrs attrs;
 130         struct usbg_gadget_strs strs;
 131         int ret;
 132
 133         ret = usbg_get_gadget_attrs(gadget, &attrs);
 134         if (ret)
 135                 goto out;
 136
 137         usb_gadget->attrs.bDeviceClass = attrs.bDeviceClass;
 138         usb_gadget->attrs.bDeviceSubClass = attrs.bDeviceSubClass;
 139         usb_gadget->attrs.bDeviceProtocol = attrs.bDeviceProtocol;
 140         usb_gadget->attrs.idVendor = attrs.idVendor;
 141         usb_gadget->attrs.idProduct = attrs.idProduct;
 142         usb_gadget->attrs.bcdDevice = attrs.bcdDevice;
 143
 144
 145         ret = usbg_get_gadget_strs(gadget, LANG_US_ENG, &strs);
 146         if (ret)
 147                 goto out;
 148
 149         usb_gadget->strs[0].manufacturer = strdup(strs.manufacturer);
 150         usb_gadget->strs[0].product = strdup(strs.product);
 151         usb_gadget->strs[0].serial = strdup(strs.serial);
 152
 153         if (!usb_gadget->strs[0].manufacturer ||
 154             !usb_gadget->strs[0].product ||
 155             !usb_gadget->strs[0].serial) {
 156                 ret = -ENOMEM;
 157                 goto err_strs;
 158         }
 159
 160         return 0;
 161 err_strs:
 162         free(usb_gadget->strs[0].manufacturer);
 163         free(usb_gadget->strs[0].product);
 164         free(usb_gadget->strs[0].serial);
 165 out:
 166         return ret;
 167 }
 168
 169 static bool cfs_match_func(struct usb_function *f,
 170                          const char *name, const char *instance) {
 171         if (strcmp(name, usbg_get_function_type_str(USBG_F_FFS))) {
 172                 /* Standard functions */
 173                 if (!strcmp(name, f->name) && !strcmp(instance, f->instance))
 174                         return true;
 175         } else {
 176                 /* Function with service */
 177                 const char *sep, *fname, *finst;
 178                 int len;
 179
 180                 sep = strchr(instance, NAME_INSTANCE_SEP);
 181                 if (!sep || strlen(sep + 1) < 1)
 182                         return false;
 183
 184                 fname = instance;
 185                 len = sep - instance;
 186                 finst = sep + 1;
 187
 188                 if (strlen(f->name) == len
 189                     && !strncmp(f->name, fname, len)
 190                     && !strcmp(f->instance, finst))
 191                         return true;
 192         }
 193
 194         return false;
 195 }
 196
 197
 198 static int cfs_find_func(const char *name, const char *instance)
 199 {
 200         int i;
 201
 202         for (i = 0; i < ARRAY_SIZE(_available_funcs); ++i)
 203                 if (cfs_match_func(_available_funcs[i], name, instance))
 204                         return i;
 205
 206         return -ENOENT;
 207 }
 208
 209 static int cfs_alloc_new_func(struct usb_gadget *gadget, const char *fname,
 210                               const char *instance, struct usb_function **_func)
 211 {
 212         struct usb_function *func;
 213         int ret;
 214
 215         ret = cfs_find_func(fname, instance);
 216         if (ret < 0)
 217                 return -ENOTSUP;
 218
 219         ret = _available_funcs[ret]->clone(_available_funcs[ret], &func);
 220         if (ret)
 221                 return ret;
 222
 223         *_func = func;
 224         return 0;
 225 }
 226
 227 static int cfs_read_funcs(usbg_gadget *gadget, struct usb_gadget *usb_gadget)
 228 {
 229         usbg_function *func;
 230         int i;
 231         int ret;
 232
 233         i = 0;
 234         usbg_for_each_function(func, gadget) {
 235                 char *func_name = (char *)usbg_get_function_type_str(
 236                                             usbg_get_function_type(func));
 237                 char *instance = (char *)usbg_get_function_instance(func);
 238
 239                 ret = cfs_alloc_new_func(usb_gadget, func_name, instance,
 240                                          usb_gadget->funcs + i);
 241                 if (ret < 0)
 242                         goto clean_prev;
 243                 ++i;
 244         }
 245
 246         return 0;
 247 clean_prev:
 248         while (i >= 0) {
 249                 usb_gadget->funcs[i]->free_func(usb_gadget->funcs[i]);
 250                 --i;
 251         }
 252
 253         return ret;
 254 }
 255
 256 static struct usb_function *cfs_find_func_in_gadget(
 257         struct usb_gadget *gadget, const char *name, const char *instance)
 258 {
 259         int i;
 260
 261         for (i = 0; gadget->funcs[i]; ++i)
 262                 if (cfs_match_func(gadget->funcs[i], name, instance))
 263                         return gadget->funcs[i];
 264
 265         return NULL;
 266 }
 267
 268 static int cfs_alloc_config(int n_funcs, struct usb_configuration **_config)
 269 {
 270         struct usb_configuration *config;
 271
 272         config = zalloc(sizeof(*config));
 273         if (!config)
 274                 goto out;
 275
 276         config->strs = calloc(2, sizeof(*config->strs));
 277         if (!config->strs)
 278                 goto free_config;
 279
 280         config->funcs = calloc(n_funcs + 1, sizeof(*config->funcs));
 281         if (!config->funcs)
 282                 goto free_strs;
 283
 284         *_config = config;
 285
 286         return 0;
 287 free_strs:
 288         free(config->strs);
 289 free_config:
 290         free(config);
 291 out:
 292         return -ENOMEM;
 293 }
 294
 295 static int cfs_read_config(usbg_config *config, struct usb_gadget *gadget,
 296                            struct usb_configuration *usb_config)
 297 {
 298         usbg_binding *b;
 299         usbg_function *func;
 300         char *name, *instance;
 301         struct usbg_config_attrs c_attrs;
 302         struct usbg_config_strs c_strs;
 303         int i = 0;
 304         int ret;
 305
 306         usbg_for_each_binding(b, config) {
 307                 func = usbg_get_binding_target(b);
 308
 309                 name = (char *)usbg_get_function_type_str(
 310                         usbg_get_function_type(func));
 311                 instance = (char *)usbg_get_function_instance(func);
 312
 313                 usb_config->funcs[i] = cfs_find_func_in_gadget(gadget,
 314                                                                name, instance);
 315                 if (!usb_config->funcs[i]) {
 316                         return -ENOTSUP;
 317                 }
 318                 ++i;
 319         }
 320
 321         ret = usbg_get_config_attrs(config, &c_attrs);
 322         if (ret)
 323                 return ret;
 324
 325         usb_config->attrs.MaxPower = c_attrs.bMaxPower*2;
 326         usb_config->attrs.bmAttributs = c_attrs.bmAttributes;
 327
 328         ret = usbg_get_config_strs(config, LANG_US_ENG, &c_strs);
 329         if (ret) {
 330                 usb_config->strs[0].lang_code = 0;
 331         } else {
 332                 usb_config->strs[0].lang_code = LANG_US_ENG;
 333                 usb_config->strs[0].config_str = strdup(c_strs.configuration);
 334                 if (!usb_config->strs[0].config_str)
 335                         return -ENOMEM;
 336         }
 337
 338         return 0;
 339 }
 340
 341 static int cfs_count_bindings(usbg_config *config)
 342 {
 343         usbg_binding *b;
 344         int i = 0;
 345
 346         usbg_for_each_binding(b, config) ++i;
 347
 348         return i;
 349 }
 350
 351 static int cfs_read_configs(usbg_gadget *gadget, struct usb_gadget *usb_gadget)
 352 {
 353         usbg_config *config;
 354         int i = 0;
 355         int n_funcs;
 356         int ret;
 357
 358         usbg_for_each_config(config, gadget) {
 359                 n_funcs = cfs_count_bindings(config);
 360
 361                 ret = cfs_alloc_config(n_funcs, usb_gadget->configs + i);
 362                 if (ret)
 363                         goto clean_prev;
 364                 ret = cfs_read_config(config, usb_gadget,
 365                                       usb_gadget->configs[i]);
 366                 if (ret)
 367                         goto free_current;
 368
 369                 ++i;
 370         }
 371
 372         return 0;
 373 free_current:
 374         free(usb_gadget->configs[i]->strs);
 375         free(usb_gadget->configs[i]->funcs);
 376         free(usb_gadget->configs[i]);
 377 clean_prev:
 378         while (i >= 0)
 379                 cfs_free_config(usb_gadget->configs[i--]);
 380         return ret;
 381 }
 382
 383 static int cfs_count_configs(usbg_gadget *gadget)
 384 {
 385         usbg_config *config;
 386         int i = 0;
 387
 388         usbg_for_each_config(config, gadget) ++i;
 389
 390         return i;
 391 }
 392
 393 static int cfs_count_functions(usbg_gadget *gadget)
 394 {
 395         usbg_function *func;
 396         int i = 0;
 397
 398         usbg_for_each_function(func, gadget) ++i;
 399
 400         return i;
 401 }
 402
 403 static int cfs_get_current_gadget(struct usb_client *usb,
 404                                      struct usb_gadget **_usb_gadget)
 405 {
 406         struct cfs_client *cfs_client;
 407         struct usb_gadget *usb_gadget;
 408         struct usb_gadget_strings *strs;
 409         struct usb_configuration **usb_configs;
 410         struct usb_function **usb_funcs;
 411         int n_funcs, n_configs;
 412         int i;
 413         int ret = -ENOMEM;
 414
 415         if (!usb)
 416                 return -EINVAL;
 417
 418         cfs_client = container_of(usb, struct cfs_client,
 419                                   client);
 420
 421         usb_gadget = zalloc(sizeof(*usb_gadget));
 422         if (!usb_gadget)
 423                 goto out;
 424
 425         /*
 426          * Currently there is no interface in libusbg which
 427          * allows to list all string languages.
 428          * That's why we do this only for USA english
 429          */
 430         strs = calloc(2, sizeof(*strs));
 431         if (!strs)
 432                 goto free_gadget;
 433
 434         strs[0].lang_code = LANG_US_ENG;
 435
 436         usb_gadget->strs = strs;
 437
 438         ret = cfs_read_gadget_attrs_strs(cfs_client->gadget, usb_gadget);
 439         if (ret)
 440                 goto free_strs;
 441
 442
 443         n_funcs = cfs_count_functions(cfs_client->gadget);
 444         usb_funcs = calloc(n_funcs + 1, sizeof(*usb_funcs));
 445         if (!usb_funcs)
 446                 goto free_strs_with_content;
 447
 448         usb_gadget->funcs = usb_funcs;
 449
 450         ret = cfs_read_funcs(cfs_client->gadget, usb_gadget);
 451         if (ret)
 452                 goto free_funcs;
 453
 454         n_configs = cfs_count_configs(cfs_client->gadget);
 455         usb_configs = calloc(n_configs + 1, sizeof(*usb_configs));
 456         if (!usb_configs)
 457                 goto free_funcs_with_content;
 458
 459         usb_gadget->configs = usb_configs;
 460
 461         ret = cfs_read_configs(cfs_client->gadget, usb_gadget);
 462         if (ret)
 463                 goto free_configs;
 464
 465         *_usb_gadget = usb_gadget;
 466         return 0;
 467
 468 free_configs:
 469         free(usb_configs);
 470 free_funcs_with_content:
 471         for (i = 0; usb_gadget->funcs[i]; ++i)
 472                 usb_gadget->funcs[i]->free_func(usb_gadget->funcs[i]);
 473 free_funcs:
 474         free(usb_funcs);
 475 free_strs_with_content:
 476         for (i = 0; usb_gadget->strs[i].lang_code; ++i) {
 477                 free(usb_gadget->strs[i].manufacturer);
 478                 free(usb_gadget->strs[i].product);
 479                 free(usb_gadget->strs[i].serial);
 480         }
 481 free_strs:
 482         free(usb_gadget->strs);
 483 free_gadget:
 484         free(usb_gadget);
 485 out:
 486         return ret;
 487 }
 488
 489 static bool cfs_is_function_supported(struct usb_client *usb,
 490                                          struct usb_function *func)
 491 {
 492         bool res;
 493         int ret;
 494
 495         switch (func->function_group) {
 496         case USB_FUNCTION_GROUP_SIMPLE:
 497                 ret = usbg_lookup_function_type(func->name);
 498                 res = ret >= 0;
 499                 break;
 500         case USB_FUNCTION_GROUP_WITH_SERVICE:
 501                 /* TODO: Check if socket is available */
 502                 res = true;
 503                 break;
 504         default:
 505                 res = false;
 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 #define SYSTEMD_DBUS_SERVICE "org.freedesktop.systemd1"
 591 #define SYSTEMD_DBUS_PATH "/org/freedesktop/systemd1"
 592 #define SYSTEMD_DBUS_MANAGER_IFACE "org.freedesktop.systemd1.Manager"
 593
 594 #define SYSTEMD_SOCKET_SUFFIX ".socket"
 595 #define MAX_SOCKET_NAME 1024
 596
 597 struct bus_ctx {
 598         const char *unit;
 599         sd_event *loop;
 600 };
 601
 602 static int socket_started(sd_bus_message *m, void *userdata,
 603                           sd_bus_error *ret_error)
 604 {
 605         struct bus_ctx *ctx = userdata;
 606         char *signal_unit;
 607         int ret;
 608
 609         ret = sd_bus_message_read(m, "uoss", NULL, NULL, &signal_unit, NULL);
 610         if (ret < 0) {
 611                 sd_event_exit(ctx->loop, ret);
 612                 return 0;
 613         }
 614
 615         if (!strcmp(signal_unit, ctx->unit))
 616                 sd_event_exit(ctx->loop, 0);
 617
 618         return 0;
 619 }
 620
 621 static int systemd_unit_interface_sync(const char *method, const char *unit,
 622                                        bool wait)
 623 {
 624         sd_bus *bus = NULL;
 625         sd_event *loop = NULL;
 626         struct bus_ctx ctx;
 627         int ret;
 628
 629         ret = sd_bus_open_system(&bus);
 630         if (ret < 0)
 631                 return ret;
 632
 633         if (wait) {
 634                 ret = sd_event_new(&loop);
 635                 if (ret < 0)
 636                         goto unref_bus;
 637
 638                 ctx.loop = loop;
 639                 ctx.unit = unit;
 640
 641                 ret = sd_bus_attach_event(bus, loop, SD_EVENT_PRIORITY_NORMAL);
 642                 if (ret < 0)
 643                         goto unref_loop;
 644
 645                 ret = sd_bus_add_match(bus, NULL,
 646                                        "type='signal',"
 647                                        "sender='" SYSTEMD_DBUS_SERVICE "',"
 648                                        "interface='" SYSTEMD_DBUS_MANAGER_IFACE "',"
 649                                        "member='JobRemoved',"
 650                                        "path_namespace='" SYSTEMD_DBUS_PATH "'",
 651                                        socket_started,
 652                                        &ctx);
 653                 if (ret < 0)
 654                         goto unref_loop;
 655         }
 656
 657
 658         ret = sd_bus_call_method(bus,
 659                         SYSTEMD_DBUS_SERVICE,
 660                         SYSTEMD_DBUS_PATH,
 661                         SYSTEMD_DBUS_MANAGER_IFACE,
 662                         method,
 663                         NULL,
 664                         NULL,
 665                         "ss",
 666                         unit,
 667                         "replace");
 668         if (ret < 0)
 669                 goto unref_loop;
 670
 671         if (wait)
 672                 ret = sd_event_loop(loop);
 673
 674 unref_loop:
 675         if (wait)
 676                 sd_event_unref(loop);
 677 unref_bus:
 678         sd_bus_unref(bus);
 679         return ret;
 680 }
 681
 682 static int systemd_start_socket(const char *socket_name)
 683 {
 684         char unit[MAX_SOCKET_NAME];
 685         int ret;
 686
 687         ret = snprintf(unit, sizeof(unit), "%s" SYSTEMD_SOCKET_SUFFIX,
 688                        socket_name);
 689         if (ret < 0 || ret >= sizeof(unit))
 690                 return -ENAMETOOLONG;
 691
 692
 693         return systemd_unit_interface_sync("StartUnit", unit, true);
 694 }
 695
 696 static int systemd_stop_socket(const char *socket_name)
 697 {
 698         char unit[MAX_SOCKET_NAME];
 699         int ret;
 700
 701         ret = snprintf(unit, sizeof(unit), "%s" SYSTEMD_SOCKET_SUFFIX,
 702                        socket_name);
 703         if (ret < 0 || ret >= sizeof(unit))
 704                 return -ENAMETOOLONG;
 705
 706         return systemd_unit_interface_sync("StopUnit", unit, false);
 707 }
 708
 709 static int cfs_ensure_dir(char *path)
 710 {
 711         int ret;
 712
 713         ret = mkdir(path, 0770);
 714         if (ret < 0)
 715                 ret = errno == EEXIST ? 0 : errno;
 716
 717         return ret;
 718 }
 719
 720 static int cfs_prep_ffs_service(const char *name, const char *instance,
 721                                 const char *dev_name, const char *socket_name)
 722 {
 723         char buf[PATH_MAX];
 724         size_t left;
 725         char *pos;
 726         int ret;
 727
 728         /* TODO: Add some good error handling */
 729
 730         left = sizeof(buf);
 731         pos = buf;
 732         ret = snprintf(pos, left, "%s", USB_FUNCS_PATH);
 733         if (ret < 0 || ret >= left) {
 734                 return -ENAMETOOLONG;
 735         } else {
 736                 left -= ret;
 737                 pos += ret;
 738         }
 739         ret = cfs_ensure_dir(buf);
 740         if (ret < 0)
 741                 return ret;
 742
 743         ret = snprintf(pos, left, "/%s", name);
 744         if (ret < 0 || ret >= left) {
 745                 return -ENAMETOOLONG;
 746         } else {
 747                 left -= ret;
 748                 pos += ret;
 749         }
 750         ret = cfs_ensure_dir(buf);
 751         if (ret < 0)
 752                 return ret;
 753
 754         ret = snprintf(pos, left, "/%s", instance);
 755         if (ret < 0 || ret >= left) {
 756                 return -ENAMETOOLONG;
 757         } else {
 758                 left -= ret;
 759                 pos += ret;
 760         }
 761         ret = cfs_ensure_dir(buf);
 762         if (ret < 0)
 763                 return ret;
 764
 765         ret = mount(dev_name, buf, "functionfs", 0, NULL);
 766         if (ret < 0)
 767                 return ret;
 768
 769         ret = systemd_start_socket(socket_name);
 770         if (ret < 0)
 771                 goto umount_ffs;
 772
 773         return 0;
 774 umount_ffs:
 775         umount(buf);
 776         return ret;
 777 }
 778
 779 static int cfs_set_gadget_config(struct cfs_client *cfs_client,
 780                                     int config_id,
 781                                     struct usb_configuration *usb_config)
 782 {
 783         struct usbg_config_attrs cattrs = {
 784                 .bmAttributes = usb_config->attrs.bmAttributs,
 785                 .bMaxPower = usb_config->attrs.MaxPower/2,
 786         };
 787         usbg_config *config;
 788         int i;
 789         int ret;
 790
 791         if (!usb_config->funcs || !usb_config->funcs[0])
 792                 return -EINVAL;
 793
 794         config = usbg_get_config(cfs_client->gadget, config_id, NULL);
 795         if (config) {
 796                 ret = usbg_rm_config(config, USBG_RM_RECURSE);
 797                 if (ret)
 798                         return ret;
 799         }
 800
 801         ret = usbg_create_config(cfs_client->gadget, config_id,
 802                                  CONFIGFS_CONFIG_LABEL, &cattrs, NULL, &config);
 803         if (ret)
 804                 return ret;
 805
 806         for (i = 0; usb_config->strs && usb_config->strs[i].lang_code; ++i) {
 807                 ret = usbg_set_config_string(config, usb_config->strs[i].lang_code,
 808                                              usb_config->strs[i].config_str);
 809                 if (ret)
 810                         return ret;
 811         }
 812
 813         for (i = 0; usb_config->funcs && usb_config->funcs[i]; ++i) {
 814                 struct usb_function *usb_func = usb_config->funcs[i];
 815                 char instance[MAX_INSTANCE_LEN];
 816                 int type;
 817                 usbg_function *func;
 818
 819                 switch (usb_func->function_group) {
 820                 case USB_FUNCTION_GROUP_SIMPLE:
 821                         type = usbg_lookup_function_type(usb_func->name);
 822                         if (strlen(usb_func->instance) >= MAX_INSTANCE_LEN)
 823                                 return -ENAMETOOLONG;
 824                         strncpy(instance, usb_func->instance, MAX_INSTANCE_LEN);
 825                         instance[MAX_INSTANCE_LEN - 1] = '\0';
 826                         break;
 827                 case USB_FUNCTION_GROUP_WITH_SERVICE:
 828                         type = USBG_F_FFS;
 829                         ret = snprintf(instance, sizeof(instance), "%s%c%s",
 830                                        usb_func->name, NAME_INSTANCE_SEP,
 831                                        usb_func->instance);
 832                         if (ret < 0 || ret >= sizeof(instance))
 833                                 return -ENAMETOOLONG;
 834                         break;
 835                 default:
 836                         return -EINVAL;
 837                 }
 838
 839
 840                 func = usbg_get_function(cfs_client->gadget, type, instance);
 841                 if (!func) {
 842                         ret = usbg_create_function(cfs_client->gadget,
 843                                                    type,
 844                                                    instance,
 845                                                    NULL, &func);
 846                         if (ret)
 847                                 return ret;
 848
 849                         if (usb_func->function_group ==
 850                             USB_FUNCTION_GROUP_WITH_SERVICE) {
 851                                 struct usb_function_with_service *fws;
 852
 853                                 fws = container_of(usb_func,
 854                                                    struct usb_function_with_service,
 855                                                    func);
 856                                 ret = cfs_prep_ffs_service(usb_func->name,
 857                                                            usb_func->instance,
 858                                                            instance,
 859                                                            fws->service);
 860                                 if (ret)
 861                                         return ret;
 862                         }
 863
 864                 }
 865
 866                 ret = usbg_add_config_function(config, NULL, func);
 867                 if (ret)
 868                         return ret;
 869         }
 870
 871         return ret;
 872 }
 873
 874 static int cfs_cleanup_left_configs(struct cfs_client *cfs_client,
 875                                     int last_config)
 876 {
 877         usbg_config *lconfig, *config;
 878         int ret;
 879
 880         lconfig = usbg_get_config(cfs_client->gadget, last_config, NULL);
 881         for (config = usbg_get_next_config(lconfig);
 882              config;
 883              config = usbg_get_next_config(lconfig)) {
 884                 ret = usbg_rm_config(config, USBG_RM_RECURSE);
 885                 if (ret)
 886                         return ret;
 887         }
 888
 889         return 0;
 890 }
 891
 892 static int cfs_reconfigure_gadget(struct usb_client *usb,
 893                                   struct usb_gadget *gadget)
 894 {
 895         struct cfs_client *cfs_client;
 896         int i;
 897         int ret;
 898
 899         if (!usb)
 900                 return -EINVAL;
 901
 902         cfs_client = container_of(usb, struct cfs_client,
 903                                   client);
 904
 905         if (!usb || !gadget || !cfs_is_gadget_supported(usb, gadget))
 906                 return -EINVAL;
 907
 908         ret = cfs_set_gadget_attrs(cfs_client, &gadget->attrs);
 909         if (ret)
 910                 goto out;
 911
 912         for (i = 0; gadget->strs && gadget->strs[i].lang_code > 0; ++i) {
 913                 ret = cfs_set_gadget_strs(cfs_client, gadget->strs + i);
 914                 if (ret)
 915                         goto out;
 916         }
 917
 918         for (i = 0; gadget->configs && gadget->configs[i]; ++i) {
 919                 ret = cfs_set_gadget_config(cfs_client, i + 1,
 920                                             gadget->configs[i]);
 921                 if (ret)
 922                         goto out;
 923         }
 924
 925         ret = cfs_cleanup_left_configs(cfs_client, i);
 926
 927         /* TODO
 928          * Cleanup things which are left after previous gadget
 929          */
 930 out:
 931         return ret;
 932 }
 933
 934 static int cfs_enable(struct usb_client *usb)
 935 {
 936         struct cfs_client *cfs_client;
 937
 938         if (!usb)
 939                 return -EINVAL;
 940
 941         cfs_client = container_of(usb, struct cfs_client,
 942                                   client);
 943
 944         return usbg_enable_gadget(cfs_client->gadget, cfs_client->udc);
 945 }
 946
 947 static int cfs_disable(struct usb_client *usb)
 948 {
 949         struct cfs_client *cfs_client;
 950
 951         if (!usb)
 952                 return -EINVAL;
 953
 954         cfs_client = container_of(usb, struct cfs_client,
 955                                   client);
 956
 957         return usbg_disable_gadget(cfs_client->gadget);
 958 }
 959
 960 static int cfs_gadget_open(struct hw_info *info,
 961                 const char *id, struct hw_common **common)
 962 {
 963         struct cfs_client *cfs_client;
 964         int ret;
 965
 966         if (!info || !common)
 967                 return -EINVAL;
 968
 969         /* used exclusively with slp usb_client*/
 970         if (!access("/sys/class/usb_mode/usb0/enable", F_OK))
 971                 return -ENOENT;
 972
 973         cfs_client = zalloc(sizeof(*cfs_client));
 974         if (!cfs_client)
 975                 return -ENOMEM;
 976
 977         ret = usbg_init(CONFIGFS_PATH, &cfs_client->ctx);
 978         if (ret)
 979                 goto err_usbg_init;
 980
 981         cfs_client->udc = usbg_get_first_udc(cfs_client->ctx);
 982         if (!cfs_client->udc) {
 983                 ret = -ENODEV;
 984                 goto err_no_udc;
 985         }
 986
 987         ret = usbg_create_gadget(cfs_client->ctx, CONFIGFS_GADGET_NAME,
 988                                  &default_g_attrs, &default_g_strs,
 989                                  &cfs_client->gadget);
 990         if (ret)
 991                 goto err_create_gadget;
 992
 993         cfs_client->client.common.info = info;
 994         cfs_client->client.get_current_gadget = cfs_get_current_gadget;
 995         cfs_client->client.reconfigure_gadget = cfs_reconfigure_gadget;
 996         cfs_client->client.is_gadget_supported = cfs_is_gadget_supported;
 997         cfs_client->client.is_function_supported = cfs_is_function_supported;
 998         cfs_client->client.enable = cfs_enable;
 999         cfs_client->client.disable = cfs_disable;
1000         cfs_client->client.free_gadget = cfs_free_gadget;
1001
1002         *common = &cfs_client->client.common;
1003         return 0;
1004
1005 err_create_gadget:
1006 err_no_udc:
1007         usbg_cleanup(cfs_client->ctx);
1008 err_usbg_init:
1009         free(cfs_client);
1010
1011         return ret;
1012 }
1013
1014 static int cfs_gadget_close(struct hw_common *common)
1015 {
1016         struct cfs_client *cfs_client;
1017
1018         if (!common)
1019                 return -EINVAL;
1020
1021         cfs_client = container_of(common, struct cfs_client,
1022                                   client.common);
1023
1024         /*
1025          * For now we don't check for errors
1026          * but we should somehow handle them
1027          */
1028         usbg_rm_gadget(cfs_client->gadget, USBG_RM_RECURSE);
1029         usbg_cleanup(cfs_client->ctx);
1030         free(cfs_client);
1031
1032         return 0;
1033 }
1034
1035 HARDWARE_MODULE_STRUCTURE = {
1036         .magic = HARDWARE_INFO_TAG,
1037         .hal_version = HARDWARE_INFO_VERSION,
1038         .device_version = USB_CLIENT_HARDWARE_DEVICE_VERSION,
1039         .id = USB_CFS_CLIENT_HARDWARE_DEVICE_ID,
1040         .name = "cfs-gadget",
1041         .open = cfs_gadget_open,
1042         .close = cfs_gadget_close,
1043 };