pfunc_core.c

linux内核源码

	pmf_parser_write_reg8,
	pmf_parser_read_reg8,
	pmf_parser_delay,
	pmf_parser_wait_reg32,
	pmf_parser_wait_reg16,
	pmf_parser_wait_reg8,
	pmf_parser_read_i2c,
	pmf_parser_write_i2c,
	pmf_parser_rmw_i2c,
	NULL, /* Bogus command */
	NULL, /* Shift bytes right: NYI */
	NULL, /* Shift bytes left: NYI */
	pmf_parser_read_cfg,
	pmf_parser_write_cfg,
	pmf_parser_rmw_cfg,
	pmf_parser_read_i2c_sub,
	pmf_parser_write_i2c_sub,
	pmf_parser_set_i2c_mode,
	pmf_parser_rmw_i2c_sub,
	pmf_parser_read_reg32_msrx,
	pmf_parser_read_reg16_msrx,
	pmf_parser_read_reg8_msrx,
	pmf_parser_write_reg32_slm,
	pmf_parser_write_reg16_slm,
	pmf_parser_write_reg8_slm,
	pmf_parser_mask_and_compare,
};

struct pmf_device {
	struct list_head	link;
	struct device_node	*node;
	struct pmf_handlers	*handlers;
	struct list_head	functions;
	struct kref		ref;
};

static LIST_HEAD(pmf_devices);
static DEFINE_SPINLOCK(pmf_lock);
static DEFINE_MUTEX(pmf_irq_mutex);

static void pmf_release_device(struct kref *kref)
{
	struct pmf_device *dev = container_of(kref, struct pmf_device, ref);
	kfree(dev);
}

static inline void pmf_put_device(struct pmf_device *dev)
{
	kref_put(&dev->ref, pmf_release_device);
}

static inline struct pmf_device *pmf_get_device(struct pmf_device *dev)
{
	kref_get(&dev->ref);
	return dev;
}

static inline struct pmf_device *pmf_find_device(struct device_node *np)
{
	struct pmf_device *dev;

	list_for_each_entry(dev, &pmf_devices, link) {
		if (dev->node == np)
			return pmf_get_device(dev);
	}
	return NULL;
}

static int pmf_parse_one(struct pmf_function *func,
			 struct pmf_handlers *handlers,
			 void *instdata, struct pmf_args *args)
{
	struct pmf_cmd cmd;
	u32 ccode;
	int count, rc;

	cmd.cmdptr		= func->data;
	cmd.cmdend		= func->data + func->length;
	cmd.func       		= func;
	cmd.instdata		= instdata;
	cmd.args		= args;
	cmd.error		= 0;

	LOG_PARSE("pmf: func %s, %d bytes, %s...\n",
		  func->name, func->length,
		  handlers ? "executing" : "parsing");

	/* One subcommand to parse for now */
	count = 1;

	while(count-- && cmd.cmdptr < cmd.cmdend) {
		/* Get opcode */
		ccode = pmf_next32(&cmd);
		/* Check if we are hitting a command list, fetch new count */
		if (ccode == 0) {
			count = pmf_next32(&cmd) - 1;
			ccode = pmf_next32(&cmd);
		}
		if (cmd.error) {
			LOG_ERROR("pmf: parse error, not enough data\n");
			return -ENXIO;
		}
		if (ccode >= PMF_CMD_COUNT) {
			LOG_ERROR("pmf: command code %d unknown !\n", ccode);
			return -ENXIO;
		}
		if (pmf_parsers[ccode] == NULL) {
			LOG_ERROR("pmf: no parser for command %d !\n", ccode);
			return -ENXIO;
		}
		rc = pmf_parsers[ccode](&cmd, handlers);
		if (rc != 0) {
			LOG_ERROR("pmf: parser for command %d returned"
				  " error %d\n", ccode, rc);
			return rc;
		}
	}

	/* We are doing an initial parse pass, we need to adjust the size */
	if (handlers == NULL)
		func->length = cmd.cmdptr - func->data;

	return 0;
}

static int pmf_add_function_prop(struct pmf_device *dev, void *driverdata,
				 const char *name, u32 *data,
				 unsigned int length)
{
	int count = 0;
	struct pmf_function *func = NULL;

	DBG("pmf: Adding functions for platform-do-%s\n", name);

	while (length >= 12) {
		/* Allocate a structure */
		func = kzalloc(sizeof(struct pmf_function), GFP_KERNEL);
		if (func == NULL)
			goto bail;
		kref_init(&func->ref);
		INIT_LIST_HEAD(&func->irq_clients);
		func->node = dev->node;
		func->driver_data = driverdata;
		func->name = name;
		func->phandle = data[0];
		func->flags = data[1];
		data += 2;
		length -= 8;
		func->data = data;
		func->length = length;
		func->dev = dev;
		DBG("pmf: idx %d: flags=%08x, phandle=%08x "
		    " %d bytes remaining, parsing...\n",
		    count+1, func->flags, func->phandle, length);
		if (pmf_parse_one(func, NULL, NULL, NULL)) {
			kfree(func);
			goto bail;
		}
		length -= func->length;
		data = (u32 *)(((u8 *)data) + func->length);
		list_add(&func->link, &dev->functions);
		pmf_get_device(dev);
		count++;
	}
 bail:
	DBG("pmf: Added %d functions\n", count);

	return count;
}

static int pmf_add_functions(struct pmf_device *dev, void *driverdata)
{
	struct property *pp;
#define PP_PREFIX "platform-do-"
	const int plen = strlen(PP_PREFIX);
	int count = 0;

	for (pp = dev->node->properties; pp != 0; pp = pp->next) {
		char *name;
		if (strncmp(pp->name, PP_PREFIX, plen) != 0)
			continue;
		name = pp->name + plen;
		if (strlen(name) && pp->length >= 12)
			count += pmf_add_function_prop(dev, driverdata, name,
						       pp->value, pp->length);
	}
	return count;
}


int pmf_register_driver(struct device_node *np,
			struct pmf_handlers *handlers,
			void *driverdata)
{
	struct pmf_device *dev;
	unsigned long flags;
	int rc = 0;

	if (handlers == NULL)
		return -EINVAL;

	DBG("pmf: registering driver for node %s\n", np->full_name);

	spin_lock_irqsave(&pmf_lock, flags);
	dev = pmf_find_device(np);
	spin_unlock_irqrestore(&pmf_lock, flags);
	if (dev != NULL) {
		DBG("pmf: already there !\n");
		pmf_put_device(dev);
		return -EBUSY;
	}

	dev = kzalloc(sizeof(struct pmf_device), GFP_KERNEL);
	if (dev == NULL) {
		DBG("pmf: no memory !\n");
		return -ENOMEM;
	}
	kref_init(&dev->ref);
	dev->node = of_node_get(np);
	dev->handlers = handlers;
	INIT_LIST_HEAD(&dev->functions);

	rc = pmf_add_functions(dev, driverdata);
	if (rc == 0) {
		DBG("pmf: no functions, disposing.. \n");
		of_node_put(np);
		kfree(dev);
		return -ENODEV;
	}

	spin_lock_irqsave(&pmf_lock, flags);
	list_add(&dev->link, &pmf_devices);
	spin_unlock_irqrestore(&pmf_lock, flags);

	return 0;
}
EXPORT_SYMBOL_GPL(pmf_register_driver);

struct pmf_function *pmf_get_function(struct pmf_function *func)
{
	if (!try_module_get(func->dev->handlers->owner))
		return NULL;
	kref_get(&func->ref);
	return func;
}
EXPORT_SYMBOL_GPL(pmf_get_function);

static void pmf_release_function(struct kref *kref)
{
	struct pmf_function *func =
		container_of(kref, struct pmf_function, ref);
	pmf_put_device(func->dev);
	kfree(func);
}

static inline void __pmf_put_function(struct pmf_function *func)
{
	kref_put(&func->ref, pmf_release_function);
}

void pmf_put_function(struct pmf_function *func)
{
	if (func == NULL)
		return;
	module_put(func->dev->handlers->owner);
	__pmf_put_function(func);
}
EXPORT_SYMBOL_GPL(pmf_put_function);

void pmf_unregister_driver(struct device_node *np)
{
	struct pmf_device *dev;
	unsigned long flags;

	DBG("pmf: unregistering driver for node %s\n", np->full_name);

	spin_lock_irqsave(&pmf_lock, flags);
	dev = pmf_find_device(np);
	if (dev == NULL) {
		DBG("pmf: not such driver !\n");
		spin_unlock_irqrestore(&pmf_lock, flags);
		return;
	}
	list_del(&dev->link);

	while(!list_empty(&dev->functions)) {
		struct pmf_function *func =
			list_entry(dev->functions.next, typeof(*func), link);
		list_del(&func->link);
		__pmf_put_function(func);
	}

	pmf_put_device(dev);
	spin_unlock_irqrestore(&pmf_lock, flags);
}
EXPORT_SYMBOL_GPL(pmf_unregister_driver);

struct pmf_function *__pmf_find_function(struct device_node *target,
					 const char *name, u32 flags)
{
	struct device_node *actor = of_node_get(target);
	struct pmf_device *dev;
	struct pmf_function *func, *result = NULL;
	char fname[64];
	const u32 *prop;
	u32 ph;

	/*
	 * Look for a "platform-*" function reference. If we can't find
	 * one, then we fallback to a direct call attempt
	 */
	snprintf(fname, 63, "platform-%s", name);
	prop = of_get_property(target, fname, NULL);
	if (prop == NULL)
		goto find_it;
	ph = *prop;
	if (ph == 0)
		goto find_it;

	/*
	 * Ok, now try to find the actor. If we can't find it, we fail,
	 * there is no point in falling back there
	 */
	of_node_put(actor);
	actor = of_find_node_by_phandle(ph);
	if (actor == NULL)
		return NULL;
 find_it:
	dev = pmf_find_device(actor);
	if (dev == NULL)
		return NULL;

	list_for_each_entry(func, &dev->functions, link) {
		if (name && strcmp(name, func->name))
			continue;
		if (func->phandle && target->node != func->phandle)
			continue;
		if ((func->flags & flags) == 0)
			continue;
		result = func;
		break;
	}
	of_node_put(actor);
	pmf_put_device(dev);
	return result;
}


int pmf_register_irq_client(struct device_node *target,
			    const char *name,
			    struct pmf_irq_client *client)
{
	struct pmf_function *func;
	unsigned long flags;

	spin_lock_irqsave(&pmf_lock, flags);
	func = __pmf_find_function(target, name, PMF_FLAGS_INT_GEN);
	if (func)
		func = pmf_get_function(func);
	spin_unlock_irqrestore(&pmf_lock, flags);
	if (func == NULL)
		return -ENODEV;

	/* guard against manipulations of list */
	mutex_lock(&pmf_irq_mutex);
	if (list_empty(&func->irq_clients))
		func->dev->handlers->irq_enable(func);

	/* guard against pmf_do_irq while changing list */
	spin_lock_irqsave(&pmf_lock, flags);
	list_add(&client->link, &func->irq_clients);
	spin_unlock_irqrestore(&pmf_lock, flags);

	client->func = func;
	mutex_unlock(&pmf_irq_mutex);

	return 0;
}
EXPORT_SYMBOL_GPL(pmf_register_irq_client);

void pmf_unregister_irq_client(struct pmf_irq_client *client)
{
	struct pmf_function *func = client->func;
	unsigned long flags;

	BUG_ON(func == NULL);

	/* guard against manipulations of list */
	mutex_lock(&pmf_irq_mutex);
	client->func = NULL;

	/* guard against pmf_do_irq while changing list */
	spin_lock_irqsave(&pmf_lock, flags);
	list_del(&client->link);
	spin_unlock_irqrestore(&pmf_lock, flags);

	if (list_empty(&func->irq_clients))
		func->dev->handlers->irq_disable(func);
	mutex_unlock(&pmf_irq_mutex);
	pmf_put_function(func);
}
EXPORT_SYMBOL_GPL(pmf_unregister_irq_client);


void pmf_do_irq(struct pmf_function *func)
{
	unsigned long flags;
	struct pmf_irq_client *client;

	/* For now, using a spinlock over the whole function. Can be made
	 * to drop the lock using 2 lists if necessary
	 */
	spin_lock_irqsave(&pmf_lock, flags);
	list_for_each_entry(client, &func->irq_clients, link) {
		if (!try_module_get(client->owner))
			continue;
		client->handler(client->data);
		module_put(client->owner);
	}
	spin_unlock_irqrestore(&pmf_lock, flags);
}
EXPORT_SYMBOL_GPL(pmf_do_irq);


int pmf_call_one(struct pmf_function *func, struct pmf_args *args)
{
	struct pmf_device *dev = func->dev;
	void *instdata = NULL;
	int rc = 0;

	DBG(" ** pmf_call_one(%s/%s) **\n", dev->node->full_name, func->name);

	if (dev->handlers->begin)
		instdata = dev->handlers->begin(func, args);
	rc = pmf_parse_one(func, dev->handlers, instdata, args);
	if (dev->handlers->end)
		dev->handlers->end(func, instdata);

	return rc;
}
EXPORT_SYMBOL_GPL(pmf_call_one);

int pmf_do_functions(struct device_node *np, const char *name,
		     u32 phandle, u32 fflags, struct pmf_args *args)
{
	struct pmf_device *dev;
	struct pmf_function *func, *tmp;
	unsigned long flags;
	int rc = -ENODEV;

	spin_lock_irqsave(&pmf_lock, flags);

	dev = pmf_find_device(np);
	if (dev == NULL) {
		spin_unlock_irqrestore(&pmf_lock, flags);
		return -ENODEV;
	}
	list_for_each_entry_safe(func, tmp, &dev->functions, link) {
		if (name && strcmp(name, func->name))
			continue;
		if (phandle && func->phandle && phandle != func->phandle)
			continue;
		if ((func->flags & fflags) == 0)
			continue;
		if (pmf_get_function(func) == NULL)
			continue;
		spin_unlock_irqrestore(&pmf_lock, flags);
		rc = pmf_call_one(func, args);
		pmf_put_function(func);
		spin_lock_irqsave(&pmf_lock, flags);
	}
	pmf_put_device(dev);
	spin_unlock_irqrestore(&pmf_lock, flags);

	return rc;
}
EXPORT_SYMBOL_GPL(pmf_do_functions);


struct pmf_function *pmf_find_function(struct device_node *target,
				       const char *name)
{
	struct pmf_function *func;
	unsigned long flags;

	spin_lock_irqsave(&pmf_lock, flags);
	func = __pmf_find_function(target, name, PMF_FLAGS_ON_DEMAND);
	if (func)
		func = pmf_get_function(func);
	spin_unlock_irqrestore(&pmf_lock, flags);
	return func;
}
EXPORT_SYMBOL_GPL(pmf_find_function);

int pmf_call_function(struct device_node *target, const char *name,
		      struct pmf_args *args)
{
	struct pmf_function *func = pmf_find_function(target, name);
	int rc;

	if (func == NULL)
		return -ENODEV;

	rc = pmf_call_one(func, args);
	pmf_put_function(func);
	return rc;
}
EXPORT_SYMBOL_GPL(pmf_call_function);