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->is_functionfs) {
 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_cleanup_ffs_service(usbg_function *function)
 663 {
 664         int ret;
 665         int index;
 666         const char *name;
 667         const char *instance;
 668         char buf[MAX_INSTANCE_LEN];
 669         struct usb_function *usb_function;
 670
 671         if (!function)
 672                 return -EINVAL;
 673
 674         instance = usbg_get_function_instance(function); /* Ex: sdb.default */
 675         name = usbg_get_function_type_str(usbg_get_function_type(function)); /* Fixed: ffs */
 676
 677         index = cfs_find_func(name, instance);
 678         if (index < 0)
 679                 return index;
 680
 681         usb_function = _available_funcs[index];
 682
 683         /* stop .socket first and stop .service later becuase of socket activation */
 684         if (usb_function->service) {
 685                 (void)systemd_stop_unit_wait_stopped(usb_function->service, ".socket", -1);
 686                 (void)systemd_stop_unit_wait_stopped(usb_function->service, ".service", -1);
 687         }
 688
 689         /* umount /dev/usb-funcs/[sdb|mtp]/default and remove it's directory */
 690         ret = snprintf(buf, sizeof(buf), "%s%s/%s", USB_FUNCS_PATH, usb_function->name, usb_function->instance);
 691         if (ret < 0)
 692                 return ret;
 693
 694         ret = umount(buf);
 695         if (ret < 0)
 696                 return ret;
 697
 698         ret = rmdir(buf);
 699         if (ret < 0)
 700                 return ret;
 701
 702         /* remove /dev/usb-funcs/[sdb|mtp] directory */
 703         ret = snprintf(buf, sizeof(buf), "%s%s", USB_FUNCS_PATH, usb_function->name);
 704         if (ret < 0)
 705                 return ret;
 706
 707         ret = rmdir(buf);
 708         if (ret < 0 && errno != ENOTEMPTY)
 709                 return ret;
 710
 711         /* remove /dev/usb-funcs/ directory */
 712         ret = rmdir(USB_FUNCS_PATH);
 713         if (ret < 0 && errno != ENOTEMPTY)
 714                 return ret;
 715
 716         return 0;
 717 }
 718
 719
 720 static int cfs_set_rndis_mac_addr(usbg_gadget *gadget, usbg_function *func)
 721 {
 722         int i, ret;
 723         struct ether_addr ethaddr;
 724         struct usbg_gadget_strs strs;
 725         struct usbg_f_net *nf = usbg_to_net_function(func);
 726
 727         if (!nf)
 728                 return -EINVAL;
 729
 730         ret = usbg_get_gadget_strs(gadget, LANG_US_ENG, &strs);
 731         if (ret != USBG_SUCCESS)
 732                 return ret;
 733
 734         for (i = 0; i < ETHER_ADDR_LEN; i++)
 735                 ethaddr.ether_addr_octet[i] = 0;
 736
 737         for (i = 0; (i < 256) && strs.serial[i]; i++) {
 738                 ethaddr.ether_addr_octet[i % (ETHER_ADDR_LEN - 1) + 1] ^= strs.serial[i];
 739         }
 740         ethaddr.ether_addr_octet[0] &= 0xfe;     /* clear multicast bit */
 741         ethaddr.ether_addr_octet[0] |= 0x02;     /* set local assignment bit (IEEE802) */
 742
 743         usbg_free_gadget_strs(&strs);
 744
 745         /* host_addr changes mac address */
 746         ret = usbg_f_net_set_host_addr(nf, &ethaddr);
 747
 748         return ret;
 749 }
 750
 751 static int cfs_set_gadget_config(struct cfs_client *cfs_client,
 752                                     int config_id,
 753                                     struct usb_configuration *usb_config)
 754 {
 755         struct usbg_config_attrs cattrs = {
 756                 .bmAttributes = usb_config->attrs.bmAttributs,
 757                 .bMaxPower = usb_config->attrs.MaxPower/2,
 758         };
 759         usbg_config *config;
 760         int i;
 761         int ret;
 762
 763         if (!usb_config->funcs || !usb_config->funcs[0])
 764                 return -EINVAL;
 765
 766         config = usbg_get_config(cfs_client->gadget, config_id, NULL);
 767         if (config) {
 768                 ret = usbg_rm_config(config, USBG_RM_RECURSE);
 769                 if (ret)
 770                         return ret;
 771         }
 772
 773         ret = usbg_create_config(cfs_client->gadget, config_id,
 774                                  CONFIGFS_CONFIG_LABEL, &cattrs, NULL, &config);
 775         if (ret)
 776                 return ret;
 777
 778         for (i = 0; usb_config->strs && usb_config->strs[i].lang_code; ++i) {
 779                 ret = usbg_set_config_string(config, usb_config->strs[i].lang_code,
 780                                              usb_config->strs[i].config_str);
 781                 if (ret)
 782                         return ret;
 783         }
 784
 785         for (i = 0; usb_config->funcs && usb_config->funcs[i]; ++i) {
 786                 struct usb_function *usb_func = usb_config->funcs[i];
 787                 char instance[MAX_INSTANCE_LEN];
 788                 int type;
 789                 usbg_function *func;
 790
 791                 if (!usb_func->is_functionfs) {
 792                         type = usbg_lookup_function_type(usb_func->name);
 793                         if (strlen(usb_func->instance) >= MAX_INSTANCE_LEN)
 794                                 return -ENAMETOOLONG;
 795                         strncpy(instance, usb_func->instance, MAX_INSTANCE_LEN);
 796                         instance[MAX_INSTANCE_LEN - 1] = '\0';
 797                 } else {
 798                         type = USBG_F_FFS;
 799                         ret = snprintf(instance, sizeof(instance), "%s%c%s",
 800                                        usb_func->name, NAME_INSTANCE_SEP,
 801                                        usb_func->instance);
 802                         if (ret < 0 || ret >= sizeof(instance))
 803                                 return -ENAMETOOLONG;
 804                 }
 805
 806                 func = usbg_get_function(cfs_client->gadget, type, instance);
 807                 if (!func) {
 808                         ret = usbg_create_function(cfs_client->gadget,
 809                                                    type,
 810                                                    instance,
 811                                                    NULL, &func);
 812                         if (ret)
 813                                 return ret;
 814
 815                         /* Setting rndis mac address. This should be done at this point,
 816                          * since the node host_addr changes to read only after the function
 817                          * is added to config. */
 818                         if (usbg_get_function_type(func) == USBG_F_RNDIS)
 819                                 (void)cfs_set_rndis_mac_addr(cfs_client->gadget, func); /* A random value is used if fails */
 820
 821                         if (usb_func->is_functionfs) {
 822                                 ret = cfs_prep_ffs_service(usb_func->name,
 823                                                         usb_func->instance,
 824                                                         instance,
 825                                                         usb_func->service);
 826                                 if (ret)
 827                                         return ret;
 828                         }
 829                 }
 830
 831                 ret = usbg_add_config_function(config, NULL, func);
 832                 if (ret)
 833                         return ret;
 834         }
 835
 836         return ret;
 837 }
 838
 839 static int cfs_cleanup_left_configs(struct cfs_client *cfs_client,
 840                                     int last_config)
 841 {
 842         usbg_config *lconfig, *config;
 843         int ret;
 844
 845         lconfig = usbg_get_config(cfs_client->gadget, last_config, NULL);
 846         for (config = usbg_get_next_config(lconfig);
 847              config;
 848              config = usbg_get_next_config(lconfig)) {
 849                 ret = usbg_rm_config(config, USBG_RM_RECURSE);
 850                 if (ret)
 851                         return ret;
 852         }
 853
 854         return 0;
 855 }
 856
 857 static int cfs_function_is_binded(struct usbg_gadget *gadget, struct usbg_function *func)
 858 {
 859         usbg_binding *b;
 860         usbg_config *config;
 861
 862         usbg_for_each_config(config, gadget)
 863                 usbg_for_each_binding(b, config)
 864                         if (usbg_get_binding_target(b) == func)
 865                                 return 1;
 866
 867         return 0;
 868 }
 869
 870 static int cfs_cleanup_unused_function(usbg_gadget *gadget)
 871 {
 872         int ret;
 873         usbg_function *function;
 874
 875         if (!gadget)
 876                 return -EINVAL;
 877
 878 restart:
 879         usbg_for_each_function(function, gadget) {
 880                 if(cfs_function_is_binded(gadget, function))
 881                         continue;
 882
 883                 if (usbg_get_function_type(function) == USBG_F_FFS) {
 884                         ret = cfs_cleanup_ffs_service(function);
 885                         if (ret)
 886                                 return ret;
 887                 }
 888
 889                 ret = usbg_rm_function(function, USBG_RM_RECURSE);
 890                 if (ret)
 891                         return ret;
 892
 893                 /* You cannot delete a usbg_function directly in an iterator. */
 894                 goto restart;
 895         }
 896
 897         return 0;
 898 }
 899
 900
 901 static int cfs_reconfigure_gadget(struct usb_client *usb,
 902                                   struct usb_gadget *gadget)
 903 {
 904         struct cfs_client *cfs_client;
 905         int i;
 906         int ret;
 907
 908         if (!usb || !gadget || !cfs_is_gadget_supported(usb, gadget))
 909                 return -EINVAL;
 910
 911         cfs_client = container_of(usb, struct cfs_client,
 912                                   client);
 913
 914         ret = cfs_set_gadget_attrs(cfs_client, &gadget->attrs);
 915         if (ret)
 916                 goto out;
 917
 918         for (i = 0; gadget->strs && gadget->strs[i].lang_code > 0; ++i) {
 919                 ret = cfs_set_gadget_strs(cfs_client, gadget->strs + i);
 920                 if (ret)
 921                         goto out;
 922         }
 923
 924         for (i = 0; gadget->configs && gadget->configs[i]; ++i) {
 925                 ret = cfs_set_gadget_config(cfs_client, i + 1,
 926                                             gadget->configs[i]);
 927                 if (ret)
 928                         goto out;
 929         }
 930
 931         ret = cfs_cleanup_left_configs(cfs_client, i);
 932         if(ret)
 933                 goto out;
 934
 935         /* Cleanup things which are left after previous gadget */
 936         ret = cfs_cleanup_unused_function(cfs_client->gadget);
 937
 938 out:
 939         return ret;
 940 }
 941
 942 static void cfs_start_stop_service_handler(usbg_gadget *gadget, bool start, bool post_stop)
 943 {
 944         int index;
 945         const char *name;
 946         const char *instance;
 947         usbg_config *config;
 948         usbg_binding *binding;
 949         usbg_function *function;
 950         struct usb_function *usb_function;
 951
 952         /* Execute service and handler related to functions bound to configs */
 953         usbg_for_each_config(config, gadget) {
 954                 usbg_for_each_binding(binding, config) {
 955                         function = usbg_get_binding_target(binding);
 956                         instance = usbg_get_function_instance(function);
 957                         name = usbg_get_function_type_str(usbg_get_function_type(function));
 958
 959                         index = cfs_find_func(name, instance);
 960                         if (index < 0)
 961                                 continue;
 962
 963                         usb_function = _available_funcs[index];
 964
 965                         if (start) {
 966                                 if (usb_function->handler)
 967                                         usb_function->handler(1);
 968
 969                                 /* functionfs service is automatically started by socket activation */
 970                                 if (!usb_function->is_functionfs && usb_function->service)
 971                                         (void)systemd_start_unit_wait_started(usb_function->service, ".service", -1);
 972                         } else {
 973                                 if (post_stop) {
 974                                         if (usb_function->is_functionfs && usb_function->service)
 975                                                 (void)systemd_stop_unit_wait_stopped(usb_function->service, ".service", -1);
 976                                 } else {
 977                                         if (!usb_function->is_functionfs && usb_function->service)
 978                                                 (void)systemd_stop_unit_wait_stopped(usb_function->service, ".service", -1);
 979
 980                                         if (usb_function->handler)
 981                                                 usb_function->handler(0);
 982                                 }
 983                         }
 984                 }
 985         }
 986 }
 987
 988 static int cfs_enable(struct usb_client *usb)
 989 {
 990         int ret;
 991         struct cfs_client *cfs_client;
 992
 993         if (!usb)
 994                 return -EINVAL;
 995
 996         cfs_client = container_of(usb, struct cfs_client, client);
 997
 998         ret = usbg_enable_gadget(cfs_client->gadget, cfs_client->udc);
 999         if (ret)
1000                 return ret;
1001
1002         cfs_start_stop_service_handler(cfs_client->gadget, true, false);
1003
1004         return 0;
1005 }
1006
1007 static int cfs_disable(struct usb_client *usb)
1008 {
1009         int ret;
1010         struct cfs_client *cfs_client;
1011
1012         if (!usb)
1013                 return -EINVAL;
1014
1015         cfs_client = container_of(usb, struct cfs_client, client);
1016
1017         cfs_start_stop_service_handler(cfs_client->gadget, false, false);
1018
1019         ret = usbg_disable_gadget(cfs_client->gadget); /* ignore error checking */
1020
1021         /*
1022          * Since functionfs service works with socket activation, you must stop it after disabling gadget.
1023          * If usb data may come in after stopping functionfs service and before disabling gadget,
1024          * functionfs service wakes up again by socket activation.
1025          */
1026         cfs_start_stop_service_handler(cfs_client->gadget, false, true);
1027
1028         return ret;
1029 }
1030
1031 EXPORT
1032 int hw_cfs_gadget_open(struct hw_info *info,
1033                 const char *id, struct hw_common **common)
1034 {
1035         struct cfs_client *cfs_client;
1036         int ret;
1037
1038         if (!info || !common)
1039                 return -EINVAL;
1040
1041         cfs_client = zalloc(sizeof(*cfs_client));
1042         if (!cfs_client)
1043                 return -ENOMEM;
1044
1045         ret = usbg_init(CONFIGFS_PATH, &cfs_client->ctx);
1046         if (ret)
1047                 goto err_usbg_init;
1048
1049         cfs_client->udc = usbg_get_first_udc(cfs_client->ctx);
1050         if (!cfs_client->udc) {
1051                 ret = -ENODEV;
1052                 goto err_no_udc;
1053         }
1054
1055         ret = usbg_create_gadget(cfs_client->ctx, CONFIGFS_GADGET_NAME,
1056                                  &default_g_attrs, &default_g_strs,
1057                                  &cfs_client->gadget);
1058         if (ret)
1059                 goto err_create_gadget;
1060
1061         cfs_client->client.common.info = info;
1062         cfs_client->client.get_current_gadget = cfs_get_current_gadget;
1063         cfs_client->client.reconfigure_gadget = cfs_reconfigure_gadget;
1064         cfs_client->client.is_gadget_supported = cfs_is_gadget_supported;
1065         cfs_client->client.is_function_supported = cfs_is_function_supported;
1066         cfs_client->client.enable = cfs_enable;
1067         cfs_client->client.disable = cfs_disable;
1068         cfs_client->client.free_gadget = cfs_free_gadget;
1069
1070         *common = &cfs_client->client.common;
1071         return 0;
1072
1073 err_create_gadget:
1074 err_no_udc:
1075         usbg_cleanup(cfs_client->ctx);
1076 err_usbg_init:
1077         free(cfs_client);
1078
1079         return ret;
1080 }
1081
1082 EXPORT
1083 int hw_cfs_gadget_close(struct hw_common *common)
1084 {
1085         struct cfs_client *cfs_client;
1086         usbg_function *func;
1087         int ret;
1088
1089         if (!common)
1090                 return -EINVAL;
1091
1092         cfs_client = container_of(common, struct cfs_client,
1093                                   client.common);
1094
1095         usbg_for_each_function(func, cfs_client->gadget) {
1096                 char *name = (char *)usbg_get_function_type_str(
1097                                             usbg_get_function_type(func));
1098                 char *instance = (char *)usbg_get_function_instance(func);
1099                 struct usb_function *usb_func;
1100
1101                 ret = cfs_find_func(name, instance);
1102                 if (ret < 0)
1103                         continue;
1104
1105                 usb_func = _available_funcs[ret];
1106
1107                 if (usb_func->is_functionfs && usb_func->service) {
1108                         (void)systemd_stop_unit_wait_stopped(usb_func->service, ".socket", -1);
1109                         (void)systemd_stop_unit_wait_stopped(usb_func->service, ".service", -1);
1110                 }
1111         }
1112
1113         /*
1114          * For now we don't check for errors
1115          * but we should somehow handle them
1116          */
1117         usbg_rm_gadget(cfs_client->gadget, USBG_RM_RECURSE);
1118         usbg_cleanup(cfs_client->ctx);
1119         free(cfs_client);
1120
1121         return 0;
1122 }
1123