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