4 * Copyright (c) 2018 Samsung Electronics Co., Ltd.
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
10 * http://www.apache.org/licenses/LICENSE-2.0
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.
22 #include <sys/types.h>
23 #include <sys/mount.h>
25 #include <usbg/usbg.h>
26 #include <usbg/function/net.h>
27 #include <libsyscommon/dbus-systemd.h>
29 #include <hw/usb_gadget.h>
30 #include <hw/usb_client.h>
32 #define CONFIGFS_PATH "/sys/kernel/config"
34 #define CONFIGFS_GADGET_NAME "hal-gadget"
35 #define CONFIGFS_CONFIG_LABEL "hal-config"
37 #define NAME_INSTANCE_SEP '.'
38 #define MAX_INSTANCE_LEN 512
40 #define USB_FUNCS_PATH "/dev/usb-funcs"
43 #define EXPORT __attribute__ ((visibility("default")))
46 enum cfs_function_service_operation {
47 CFS_FUNCTION_SERVICE_START,
48 CFS_FUNCTION_SERVICE_STOP,
49 CFS_FUNCTION_SERVICE_POST_STOP,
53 struct usb_client client;
59 /* Based on values in slp-gadget kernel module */
60 struct usbg_gadget_attrs default_g_attrs = {
61 .idVendor = DEFAULT_VID,
62 .idProduct = DEFAULT_PID,
63 .bcdDevice = DEFAULT_BCD_DEVICE,
66 struct usbg_gadget_strs default_g_strs = {
67 .manufacturer = DEFAULT_MANUFACTURER,
68 .product = DEFAULT_PRODUCT,
69 .serial = DEFAULT_SERIAL,
72 static struct usb_function *cfs_find_usb_function(usbg_function *function)
75 char buf[MAX_INSTANCE_LEN];
76 const char *instance = usbg_get_function_instance(function);
77 const char *name = usbg_get_function_type_str(usbg_get_function_type(function));
79 /* Ex. name:"ffs", instance: "sdb.default" */
80 if (strcmp(name, usbg_get_function_type_str(USBG_F_FFS)) == 0) {
81 strncpy(buf, instance, sizeof(buf) - 1);
82 buf[sizeof(buf) - 1] = '\0';
84 /* Ex. "sdb.default" ==> "sdb" + "default" */
85 sep = strchr(buf, NAME_INSTANCE_SEP);
94 return find_usb_function_by_name_instance(name, instance);
97 static bool cfs_is_function_supported(struct usb_client *usb,
98 struct usb_function *func)
103 if (!func->is_functionfs) {
104 ret = usbg_lookup_function_type(func->name);
107 /* TODO: Check if socket is available */
114 static bool cfs_is_gadget_supported(struct usb_client *usb,
115 struct usb_gadget *gadget)
119 if (!gadget || !gadget->configs || !gadget->funcs)
122 /* No real restrictions for strings */
123 for (j = 0; gadget->configs && gadget->configs[j]; ++j) {
124 struct usb_configuration *config = gadget->configs[j];
129 for (i = 0; config->funcs[i]; ++i)
130 if (!cfs_is_function_supported(usb, config->funcs[i]))
140 static int cfs_set_gadget_attrs(struct cfs_client *cfs_client,
141 struct usb_gadget_attrs *attrs)
144 struct usbg_gadget_attrs gadget_attrs;
146 ret = usbg_get_gadget_attrs(cfs_client->gadget, &gadget_attrs);
150 gadget_attrs.bDeviceClass = attrs->bDeviceClass;
151 gadget_attrs.bDeviceSubClass = attrs->bDeviceSubClass;
152 gadget_attrs.bDeviceProtocol = attrs->bDeviceProtocol;
153 gadget_attrs.idVendor = attrs->idVendor;
154 gadget_attrs.idProduct = attrs->idProduct;
155 gadget_attrs.bcdDevice = attrs->bcdDevice;
157 ret = usbg_set_gadget_attrs(cfs_client->gadget, &gadget_attrs);
162 static int cfs_set_gadget_strs(struct cfs_client *cfs_client,
163 struct usb_gadget_strings *strs)
167 #define SET_STR(FIELD, STR_ID) \
169 ret = usbg_set_gadget_str(cfs_client->gadget, \
177 SET_STR(manufacturer, USBG_STR_MANUFACTURER);
178 SET_STR(product, USBG_STR_PRODUCT);
179 SET_STR(serial, USBG_STR_SERIAL_NUMBER);
185 static int cfs_ensure_dir(char *path)
187 if (mkdir(path, 0770) < 0)
188 return (errno == EEXIST) ? 0 : -errno;
194 static int cfs_prep_ffs_service(struct usb_function *usb_func, usbg_function *function)
199 const char *instance;
200 const char *dev_name;
201 char buf[MAX_INSTANCE_LEN];
203 if (!usb_func || !function)
206 if (usbg_get_function_type(function) != USBG_F_FFS)
209 name = usb_func->name;
210 service = usb_func->service;
211 instance = usb_func->instance;
212 dev_name = usbg_get_function_instance(function);
214 /* "/dev/usb-funcs" + "/" + "sdb" + "/" + "default" + '0' */
215 if (strlen(USB_FUNCS_PATH) + strlen(name) + strlen(instance) + 3 > sizeof(buf))
216 return -ENAMETOOLONG;
218 /* mkdir /dev/usb-funcs */
219 ret = cfs_ensure_dir(USB_FUNCS_PATH);
223 /* mkdir /dev/usb-funcs/sdb */
224 snprintf(buf, sizeof(buf), "%s/%s", USB_FUNCS_PATH, name);
225 ret = cfs_ensure_dir(buf);
229 /* mkdir /dev/usb-funcs/sdb/default */
230 snprintf(buf, sizeof(buf), "%s/%s/%s", USB_FUNCS_PATH, name, instance);
231 ret = cfs_ensure_dir(buf);
235 /* mount -t functionfs sdb.default /dev/usb-funcs/sdb/default */
236 ret = mount(dev_name, buf, "functionfs", 0, NULL);
240 /* start sdbd.socket */
241 ret = systemd_start_unit_wait_started(service, ".socket", -1);
251 snprintf(buf, sizeof(buf), "%s/%s/%s", USB_FUNCS_PATH, name, instance);
254 snprintf(buf, sizeof(buf), "%s/%s", USB_FUNCS_PATH, name);
257 rmdir(USB_FUNCS_PATH);
262 static int cfs_cleanup_ffs_service(usbg_function *function)
265 char buf[MAX_INSTANCE_LEN];
266 struct usb_function *usb_function;
271 usb_function = cfs_find_usb_function(function);
275 /* stop .socket first and stop .service later becuase of socket activation */
276 if (usb_function->service) {
277 (void)systemd_stop_unit_wait_stopped(usb_function->service, ".socket", -1);
278 (void)systemd_stop_unit_wait_stopped(usb_function->service, ".service", -1);
281 /* umount /dev/usb-funcs/[sdb|mtp]/default and remove it's directory */
282 ret = snprintf(buf, sizeof(buf), "%s/%s/%s", USB_FUNCS_PATH, usb_function->name, usb_function->instance);
294 /* remove /dev/usb-funcs/[sdb|mtp] directory */
295 ret = snprintf(buf, sizeof(buf), "%s/%s", USB_FUNCS_PATH, usb_function->name);
300 if (ret < 0 && errno != ENOTEMPTY)
303 /* remove /dev/usb-funcs/ directory */
304 ret = rmdir(USB_FUNCS_PATH);
305 if (ret < 0 && errno != ENOTEMPTY)
312 static int cfs_set_rndis_mac_addr(usbg_gadget *gadget, usbg_function *func)
315 struct ether_addr ethaddr;
316 struct usbg_gadget_strs strs;
317 struct usbg_f_net *nf = usbg_to_net_function(func);
322 ret = usbg_get_gadget_strs(gadget, LANG_US_ENG, &strs);
323 if (ret != USBG_SUCCESS)
326 for (i = 0; i < ETHER_ADDR_LEN; i++)
327 ethaddr.ether_addr_octet[i] = 0;
329 for (i = 0; (i < 256) && strs.serial[i]; i++) {
330 ethaddr.ether_addr_octet[i % (ETHER_ADDR_LEN - 1) + 1] ^= strs.serial[i];
332 ethaddr.ether_addr_octet[0] &= 0xfe; /* clear multicast bit */
333 ethaddr.ether_addr_octet[0] |= 0x02; /* set local assignment bit (IEEE802) */
335 usbg_free_gadget_strs(&strs);
337 /* host_addr changes mac address */
338 ret = usbg_f_net_set_host_addr(nf, ðaddr);
343 static int cfs_cleanup_all_config_and_function(struct cfs_client *cfs_client)
347 usbg_function *function;
349 /* delete all configs */
351 usbg_for_each_config(config, cfs_client->gadget) {
352 ret = usbg_rm_config(config, USBG_RM_RECURSE);
356 goto restart_rm_config; /* You cannot delete a config directly in an iterator. */
359 /* delete all functions */
361 usbg_for_each_function(function, cfs_client->gadget) {
362 if (usbg_get_function_type(function) == USBG_F_FFS) {
363 ret = cfs_cleanup_ffs_service(function);
368 ret = usbg_rm_function(function, USBG_RM_RECURSE);
372 goto restart_rm_function; /* You cannot delete a function directly in an iterator. */
378 static int cfs_set_gadget_config(struct cfs_client *cfs_client, int config_id, struct usb_configuration *usb_config)
384 usbg_function *function;
385 struct usb_function *usb_func;
386 char instance[MAX_INSTANCE_LEN];
387 struct usbg_config_attrs cattrs = {
388 .bmAttributes = usb_config->attrs.bmAttributs,
389 .bMaxPower = usb_config->attrs.MaxPower/2,
392 if (!usb_config->funcs || !usb_config->funcs[0])
395 ret = usbg_create_config(cfs_client->gadget, config_id, CONFIGFS_CONFIG_LABEL, &cattrs, NULL, &config);
399 for (i = 0; usb_config->strs && usb_config->strs[i].lang_code; ++i) {
400 ret = usbg_set_config_string(config, usb_config->strs[i].lang_code, usb_config->strs[i].config_str);
405 for (i = 0; usb_config->funcs[i]; ++i) {
406 usb_func = usb_config->funcs[i];
408 /* name("sdb") + NAME_INSTANCE_SEP(".") + instance("default") + '\0' */
409 if (strlen(usb_func->name) + strlen(usb_func->instance) + 2 > sizeof(instance))
410 return -ENAMETOOLONG;
412 /* In functionfs, the instance is used in the format "[sdb|mtp].default" instead of "default" */
413 if (usb_func->is_functionfs) {
414 function_type = USBG_F_FFS;
415 snprintf(instance, sizeof(instance), "%s%c%s", usb_func->name, NAME_INSTANCE_SEP, usb_func->instance);
417 function_type = usbg_lookup_function_type(usb_func->name);
418 strncpy(instance, usb_func->instance, sizeof(instance) - 1);
419 instance[sizeof(instance) - 1] = '\0';
422 function = usbg_get_function(cfs_client->gadget, function_type, instance);
424 ret = usbg_create_function(cfs_client->gadget, function_type, instance, NULL, &function);
428 /* Setting rndis mac address. This should be done at this point,
429 * since the node host_addr changes to read only after the function
430 * is added to config. */
431 if (usbg_get_function_type(function) == USBG_F_RNDIS)
432 (void)cfs_set_rndis_mac_addr(cfs_client->gadget, function); /* A random value is used if fails */
434 if (usbg_get_function_type(function) == USBG_F_FFS) {
435 ret = cfs_prep_ffs_service(usb_func, function);
441 ret = usbg_add_config_function(config, NULL, function);
449 static int cfs_reconfigure_gadget(struct usb_client *usb, struct usb_gadget *gadget)
453 struct cfs_client *cfs_client;
455 if (!usb || !gadget || !cfs_is_gadget_supported(usb, gadget))
458 cfs_client = container_of(usb, struct cfs_client, client);
460 ret = cfs_set_gadget_attrs(cfs_client, &gadget->attrs);
464 for (i = 0; gadget->strs && gadget->strs[i].lang_code > 0; ++i) {
465 ret = cfs_set_gadget_strs(cfs_client, gadget->strs + i);
470 ret = cfs_cleanup_all_config_and_function(cfs_client);
474 for (i = 0; gadget->configs && gadget->configs[i]; ++i) {
475 ret = cfs_set_gadget_config(cfs_client, i + 1, gadget->configs[i]);
483 static void cfs_start_stop_service_and_handler(usbg_gadget *gadget, enum cfs_function_service_operation operation)
485 usbg_function *function;
486 struct usb_function *usb_function;
488 usbg_for_each_function(function, gadget) {
489 usb_function = cfs_find_usb_function(function);
494 case CFS_FUNCTION_SERVICE_START:
495 if (usb_function->handler)
496 usb_function->handler(1);
498 /* functionfs service is automatically started by socket activation */
499 if (!usb_function->is_functionfs && usb_function->service)
500 (void)systemd_start_unit_wait_started(usb_function->service, ".service", -1);
503 case CFS_FUNCTION_SERVICE_STOP:
504 if (!usb_function->is_functionfs && usb_function->service)
505 (void)systemd_stop_unit_wait_stopped(usb_function->service, ".service", -1);
507 if (usb_function->handler)
508 usb_function->handler(0);
511 case CFS_FUNCTION_SERVICE_POST_STOP:
512 if (usb_function->is_functionfs && usb_function->service)
513 (void)systemd_stop_unit_wait_stopped(usb_function->service, ".service", -1);
522 static int cfs_enable(struct usb_client *usb)
525 struct cfs_client *cfs_client;
530 cfs_client = container_of(usb, struct cfs_client, client);
532 ret = usbg_enable_gadget(cfs_client->gadget, cfs_client->udc);
536 cfs_start_stop_service_and_handler(cfs_client->gadget, CFS_FUNCTION_SERVICE_START);
541 static int cfs_disable(struct usb_client *usb)
544 struct cfs_client *cfs_client;
549 cfs_client = container_of(usb, struct cfs_client, client);
551 cfs_start_stop_service_and_handler(cfs_client->gadget, CFS_FUNCTION_SERVICE_STOP);
553 ret = usbg_disable_gadget(cfs_client->gadget); /* ignore error checking */
556 * Since functionfs service works with socket activation, you must stop it after disabling gadget.
557 * If usb data may come in after stopping functionfs service and before disabling gadget,
558 * functionfs service wakes up again by socket activation.
560 cfs_start_stop_service_and_handler(cfs_client->gadget, CFS_FUNCTION_SERVICE_POST_STOP);
566 int hw_cfs_gadget_open(struct hw_info *info,
567 const char *id, struct hw_common **common)
569 struct cfs_client *cfs_client;
572 if (!info || !common)
575 cfs_client = calloc(1, sizeof(*cfs_client));
579 ret = usbg_init(CONFIGFS_PATH, &cfs_client->ctx);
583 cfs_client->udc = usbg_get_first_udc(cfs_client->ctx);
584 if (!cfs_client->udc) {
589 ret = usbg_create_gadget(cfs_client->ctx, CONFIGFS_GADGET_NAME,
590 &default_g_attrs, &default_g_strs,
591 &cfs_client->gadget);
593 goto err_create_gadget;
595 cfs_client->client.common.info = info;
597 cfs_client->client.reconfigure_gadget = cfs_reconfigure_gadget;
598 cfs_client->client.enable = cfs_enable;
599 cfs_client->client.disable = cfs_disable;
601 *common = &cfs_client->client.common;
607 usbg_cleanup(cfs_client->ctx);
615 int hw_cfs_gadget_close(struct hw_common *common)
617 usbg_function *function;
618 struct cfs_client *cfs_client;
619 struct usb_function *usb_func;
624 cfs_client = container_of(common, struct cfs_client, client.common);
626 usbg_for_each_function(function, cfs_client->gadget) {
627 usb_func = cfs_find_usb_function(function);
631 if (usb_func->is_functionfs && usb_func->service) {
632 (void)systemd_stop_unit_wait_stopped(usb_func->service, ".socket", -1);
633 (void)systemd_stop_unit_wait_stopped(usb_func->service, ".service", -1);
638 * For now we don't check for errors
639 * but we should somehow handle them
641 usbg_rm_gadget(cfs_client->gadget, USBG_RM_RECURSE);
642 usbg_cleanup(cfs_client->ctx);