pfunc_core.c

linux内核源码

/*
 *
 * FIXME: Properly make this race free with refcounting etc...
 *
 * FIXME: LOCKING !!!
 */

#include <linux/init.h>
#include <linux/delay.h>
#include <linux/kernel.h>
#include <linux/spinlock.h>
#include <linux/module.h>
#include <linux/mutex.h>

#include <asm/semaphore.h>
#include <asm/prom.h>
#include <asm/pmac_pfunc.h>

/* Debug */
#define LOG_PARSE(fmt...)
#define LOG_ERROR(fmt...)	printk(fmt)
#define LOG_BLOB(t,b,c)

#undef DEBUG
#ifdef DEBUG
#define DBG(fmt...)		printk(fmt)
#else
#define DBG(fmt...)
#endif

/* Command numbers */
#define PMF_CMD_LIST			0
#define PMF_CMD_WRITE_GPIO		1
#define PMF_CMD_READ_GPIO		2
#define PMF_CMD_WRITE_REG32		3
#define PMF_CMD_READ_REG32		4
#define PMF_CMD_WRITE_REG16		5
#define PMF_CMD_READ_REG16		6
#define PMF_CMD_WRITE_REG8		7
#define PMF_CMD_READ_REG8		8
#define PMF_CMD_DELAY			9
#define PMF_CMD_WAIT_REG32		10
#define PMF_CMD_WAIT_REG16		11
#define PMF_CMD_WAIT_REG8		12
#define PMF_CMD_READ_I2C		13
#define PMF_CMD_WRITE_I2C		14
#define PMF_CMD_RMW_I2C			15
#define PMF_CMD_GEN_I2C			16
#define PMF_CMD_SHIFT_BYTES_RIGHT	17
#define PMF_CMD_SHIFT_BYTES_LEFT	18
#define PMF_CMD_READ_CFG		19
#define PMF_CMD_WRITE_CFG		20
#define PMF_CMD_RMW_CFG			21
#define PMF_CMD_READ_I2C_SUBADDR	22
#define PMF_CMD_WRITE_I2C_SUBADDR	23
#define PMF_CMD_SET_I2C_MODE		24
#define PMF_CMD_RMW_I2C_SUBADDR		25
#define PMF_CMD_READ_REG32_MASK_SHR_XOR	26
#define PMF_CMD_READ_REG16_MASK_SHR_XOR	27
#define PMF_CMD_READ_REG8_MASK_SHR_XOR	28
#define PMF_CMD_WRITE_REG32_SHL_MASK	29
#define PMF_CMD_WRITE_REG16_SHL_MASK	30
#define PMF_CMD_WRITE_REG8_SHL_MASK	31
#define PMF_CMD_MASK_AND_COMPARE	32
#define PMF_CMD_COUNT			33

/* This structure holds the state of the parser while walking through
 * a function definition
 */
struct pmf_cmd {
	const void		*cmdptr;
	const void		*cmdend;
	struct pmf_function	*func;
	void			*instdata;
	struct pmf_args		*args;
	int			error;
};

#if 0
/* Debug output */
static void print_blob(const char *title, const void *blob, int bytes)
{
	printk("%s", title);
	while(bytes--) {
		printk("%02x ", *((u8 *)blob));
		blob += 1;
	}
	printk("\n");
}
#endif

/*
 * Parser helpers
 */

static u32 pmf_next32(struct pmf_cmd *cmd)
{
	u32 value;
	if ((cmd->cmdend - cmd->cmdptr) < 4) {
		cmd->error = 1;
		return 0;
	}
	value = *((u32 *)cmd->cmdptr);
	cmd->cmdptr += 4;
	return value;
}

static const void* pmf_next_blob(struct pmf_cmd *cmd, int count)
{
	const void *value;
	if ((cmd->cmdend - cmd->cmdptr) < count) {
		cmd->error = 1;
		return NULL;
	}
	value = cmd->cmdptr;
	cmd->cmdptr += count;
	return value;
}

/*
 * Individual command parsers
 */

#define PMF_PARSE_CALL(name, cmd, handlers, p...) \
	do { \
		if (cmd->error) \
			return -ENXIO; \
		if (handlers == NULL) \
			return 0; \
		if (handlers->name)				      \
			return handlers->name(cmd->func, cmd->instdata, \
					      cmd->args, p);	      \
		return -1; \
	} while(0) \


static int pmf_parser_write_gpio(struct pmf_cmd *cmd, struct pmf_handlers *h)
{
	u8 value = (u8)pmf_next32(cmd);
	u8 mask = (u8)pmf_next32(cmd);

	LOG_PARSE("pmf: write_gpio(value: %02x, mask: %02x)\n", value, mask);

	PMF_PARSE_CALL(write_gpio, cmd, h, value, mask);
}

static int pmf_parser_read_gpio(struct pmf_cmd *cmd, struct pmf_handlers *h)
{
	u8 mask = (u8)pmf_next32(cmd);
	int rshift = (int)pmf_next32(cmd);
	u8 xor = (u8)pmf_next32(cmd);

	LOG_PARSE("pmf: read_gpio(mask: %02x, rshift: %d, xor: %02x)\n",
		  mask, rshift, xor);

	PMF_PARSE_CALL(read_gpio, cmd, h, mask, rshift, xor);
}

static int pmf_parser_write_reg32(struct pmf_cmd *cmd, struct pmf_handlers *h)
{
	u32 offset = pmf_next32(cmd);
	u32 value = pmf_next32(cmd);
	u32 mask = pmf_next32(cmd);

	LOG_PARSE("pmf: write_reg32(offset: %08x, value: %08x, mask: %08x)\n",
		  offset, value, mask);

	PMF_PARSE_CALL(write_reg32, cmd, h, offset, value, mask);
}

static int pmf_parser_read_reg32(struct pmf_cmd *cmd, struct pmf_handlers *h)
{
	u32 offset = pmf_next32(cmd);

	LOG_PARSE("pmf: read_reg32(offset: %08x)\n", offset);

	PMF_PARSE_CALL(read_reg32, cmd, h, offset);
}


static int pmf_parser_write_reg16(struct pmf_cmd *cmd, struct pmf_handlers *h)
{
	u32 offset = pmf_next32(cmd);
	u16 value = (u16)pmf_next32(cmd);
	u16 mask = (u16)pmf_next32(cmd);

	LOG_PARSE("pmf: write_reg16(offset: %08x, value: %04x, mask: %04x)\n",
		  offset, value, mask);

	PMF_PARSE_CALL(write_reg16, cmd, h, offset, value, mask);
}

static int pmf_parser_read_reg16(struct pmf_cmd *cmd, struct pmf_handlers *h)
{
	u32 offset = pmf_next32(cmd);

	LOG_PARSE("pmf: read_reg16(offset: %08x)\n", offset);

	PMF_PARSE_CALL(read_reg16, cmd, h, offset);
}


static int pmf_parser_write_reg8(struct pmf_cmd *cmd, struct pmf_handlers *h)
{
	u32 offset = pmf_next32(cmd);
	u8 value = (u16)pmf_next32(cmd);
	u8 mask = (u16)pmf_next32(cmd);

	LOG_PARSE("pmf: write_reg8(offset: %08x, value: %02x, mask: %02x)\n",
		  offset, value, mask);

	PMF_PARSE_CALL(write_reg8, cmd, h, offset, value, mask);
}

static int pmf_parser_read_reg8(struct pmf_cmd *cmd, struct pmf_handlers *h)
{
	u32 offset = pmf_next32(cmd);

	LOG_PARSE("pmf: read_reg8(offset: %08x)\n", offset);

	PMF_PARSE_CALL(read_reg8, cmd, h, offset);
}

static int pmf_parser_delay(struct pmf_cmd *cmd, struct pmf_handlers *h)
{
	u32 duration = pmf_next32(cmd);

	LOG_PARSE("pmf: delay(duration: %d us)\n", duration);

	PMF_PARSE_CALL(delay, cmd, h, duration);
}

static int pmf_parser_wait_reg32(struct pmf_cmd *cmd, struct pmf_handlers *h)
{
	u32 offset = pmf_next32(cmd);
	u32 value = pmf_next32(cmd);
	u32 mask = pmf_next32(cmd);

	LOG_PARSE("pmf: wait_reg32(offset: %08x, comp_value: %08x,mask: %08x)\n",
		  offset, value, mask);

	PMF_PARSE_CALL(wait_reg32, cmd, h, offset, value, mask);
}

static int pmf_parser_wait_reg16(struct pmf_cmd *cmd, struct pmf_handlers *h)
{
	u32 offset = pmf_next32(cmd);
	u16 value = (u16)pmf_next32(cmd);
	u16 mask = (u16)pmf_next32(cmd);

	LOG_PARSE("pmf: wait_reg16(offset: %08x, comp_value: %04x,mask: %04x)\n",
		  offset, value, mask);

	PMF_PARSE_CALL(wait_reg16, cmd, h, offset, value, mask);
}

static int pmf_parser_wait_reg8(struct pmf_cmd *cmd, struct pmf_handlers *h)
{
	u32 offset = pmf_next32(cmd);
	u8 value = (u8)pmf_next32(cmd);
	u8 mask = (u8)pmf_next32(cmd);

	LOG_PARSE("pmf: wait_reg8(offset: %08x, comp_value: %02x,mask: %02x)\n",
		  offset, value, mask);

	PMF_PARSE_CALL(wait_reg8, cmd, h, offset, value, mask);
}

static int pmf_parser_read_i2c(struct pmf_cmd *cmd, struct pmf_handlers *h)
{
	u32 bytes = pmf_next32(cmd);

	LOG_PARSE("pmf: read_i2c(bytes: %ud)\n", bytes);

	PMF_PARSE_CALL(read_i2c, cmd, h, bytes);
}

static int pmf_parser_write_i2c(struct pmf_cmd *cmd, struct pmf_handlers *h)
{
	u32 bytes = pmf_next32(cmd);
	const void *blob = pmf_next_blob(cmd, bytes);

	LOG_PARSE("pmf: write_i2c(bytes: %ud) ...\n", bytes);
	LOG_BLOB("pmf:   data: \n", blob, bytes);

	PMF_PARSE_CALL(write_i2c, cmd, h, bytes, blob);
}


static int pmf_parser_rmw_i2c(struct pmf_cmd *cmd, struct pmf_handlers *h)
{
	u32 maskbytes = pmf_next32(cmd);
	u32 valuesbytes = pmf_next32(cmd);
	u32 totalbytes = pmf_next32(cmd);
	const void *maskblob = pmf_next_blob(cmd, maskbytes);
	const void *valuesblob = pmf_next_blob(cmd, valuesbytes);

	LOG_PARSE("pmf: rmw_i2c(maskbytes: %ud, valuebytes: %ud, "
		  "totalbytes: %d) ...\n",
		  maskbytes, valuesbytes, totalbytes);
	LOG_BLOB("pmf:   mask data: \n", maskblob, maskbytes);
	LOG_BLOB("pmf:   values data: \n", valuesblob, valuesbytes);

	PMF_PARSE_CALL(rmw_i2c, cmd, h, maskbytes, valuesbytes, totalbytes,
		       maskblob, valuesblob);
}

static int pmf_parser_read_cfg(struct pmf_cmd *cmd, struct pmf_handlers *h)
{
	u32 offset = pmf_next32(cmd);
	u32 bytes = pmf_next32(cmd);

	LOG_PARSE("pmf: read_cfg(offset: %x, bytes: %ud)\n", offset, bytes);

	PMF_PARSE_CALL(read_cfg, cmd, h, offset, bytes);
}


static int pmf_parser_write_cfg(struct pmf_cmd *cmd, struct pmf_handlers *h)
{
	u32 offset = pmf_next32(cmd);
	u32 bytes = pmf_next32(cmd);
	const void *blob = pmf_next_blob(cmd, bytes);

	LOG_PARSE("pmf: write_cfg(offset: %x, bytes: %ud)\n", offset, bytes);
	LOG_BLOB("pmf:   data: \n", blob, bytes);

	PMF_PARSE_CALL(write_cfg, cmd, h, offset, bytes, blob);
}

static int pmf_parser_rmw_cfg(struct pmf_cmd *cmd, struct pmf_handlers *h)
{
	u32 offset = pmf_next32(cmd);
	u32 maskbytes = pmf_next32(cmd);
	u32 valuesbytes = pmf_next32(cmd);
	u32 totalbytes = pmf_next32(cmd);
	const void *maskblob = pmf_next_blob(cmd, maskbytes);
	const void *valuesblob = pmf_next_blob(cmd, valuesbytes);

	LOG_PARSE("pmf: rmw_cfg(maskbytes: %ud, valuebytes: %ud,"
		  " totalbytes: %d) ...\n",
		  maskbytes, valuesbytes, totalbytes);
	LOG_BLOB("pmf:   mask data: \n", maskblob, maskbytes);
	LOG_BLOB("pmf:   values data: \n", valuesblob, valuesbytes);

	PMF_PARSE_CALL(rmw_cfg, cmd, h, offset, maskbytes, valuesbytes,
		       totalbytes, maskblob, valuesblob);
}


static int pmf_parser_read_i2c_sub(struct pmf_cmd *cmd, struct pmf_handlers *h)
{
	u8 subaddr = (u8)pmf_next32(cmd);
	u32 bytes = pmf_next32(cmd);

	LOG_PARSE("pmf: read_i2c_sub(subaddr: %x, bytes: %ud)\n",
		  subaddr, bytes);

	PMF_PARSE_CALL(read_i2c_sub, cmd, h, subaddr, bytes);
}

static int pmf_parser_write_i2c_sub(struct pmf_cmd *cmd, struct pmf_handlers *h)
{
	u8 subaddr = (u8)pmf_next32(cmd);
	u32 bytes = pmf_next32(cmd);
	const void *blob = pmf_next_blob(cmd, bytes);

	LOG_PARSE("pmf: write_i2c_sub(subaddr: %x, bytes: %ud) ...\n",
		  subaddr, bytes);
	LOG_BLOB("pmf:   data: \n", blob, bytes);

	PMF_PARSE_CALL(write_i2c_sub, cmd, h, subaddr, bytes, blob);
}

static int pmf_parser_set_i2c_mode(struct pmf_cmd *cmd, struct pmf_handlers *h)
{
	u32 mode = pmf_next32(cmd);

	LOG_PARSE("pmf: set_i2c_mode(mode: %d)\n", mode);

	PMF_PARSE_CALL(set_i2c_mode, cmd, h, mode);
}


static int pmf_parser_rmw_i2c_sub(struct pmf_cmd *cmd, struct pmf_handlers *h)
{
	u8 subaddr = (u8)pmf_next32(cmd);
	u32 maskbytes = pmf_next32(cmd);
	u32 valuesbytes = pmf_next32(cmd);
	u32 totalbytes = pmf_next32(cmd);
	const void *maskblob = pmf_next_blob(cmd, maskbytes);
	const void *valuesblob = pmf_next_blob(cmd, valuesbytes);

	LOG_PARSE("pmf: rmw_i2c_sub(subaddr: %x, maskbytes: %ud, valuebytes: %ud"
		  ", totalbytes: %d) ...\n",
		  subaddr, maskbytes, valuesbytes, totalbytes);
	LOG_BLOB("pmf:   mask data: \n", maskblob, maskbytes);
	LOG_BLOB("pmf:   values data: \n", valuesblob, valuesbytes);

	PMF_PARSE_CALL(rmw_i2c_sub, cmd, h, subaddr, maskbytes, valuesbytes,
		       totalbytes, maskblob, valuesblob);
}

static int pmf_parser_read_reg32_msrx(struct pmf_cmd *cmd,
				      struct pmf_handlers *h)
{
	u32 offset = pmf_next32(cmd);
	u32 mask = pmf_next32(cmd);
	u32 shift = pmf_next32(cmd);
	u32 xor = pmf_next32(cmd);

	LOG_PARSE("pmf: read_reg32_msrx(offset: %x, mask: %x, shift: %x,"
		  " xor: %x\n", offset, mask, shift, xor);

	PMF_PARSE_CALL(read_reg32_msrx, cmd, h, offset, mask, shift, xor);
}

static int pmf_parser_read_reg16_msrx(struct pmf_cmd *cmd,
				      struct pmf_handlers *h)
{
	u32 offset = pmf_next32(cmd);
	u32 mask = pmf_next32(cmd);
	u32 shift = pmf_next32(cmd);
	u32 xor = pmf_next32(cmd);

	LOG_PARSE("pmf: read_reg16_msrx(offset: %x, mask: %x, shift: %x,"
		  " xor: %x\n", offset, mask, shift, xor);

	PMF_PARSE_CALL(read_reg16_msrx, cmd, h, offset, mask, shift, xor);
}
static int pmf_parser_read_reg8_msrx(struct pmf_cmd *cmd,
				     struct pmf_handlers *h)
{
	u32 offset = pmf_next32(cmd);
	u32 mask = pmf_next32(cmd);
	u32 shift = pmf_next32(cmd);
	u32 xor = pmf_next32(cmd);

	LOG_PARSE("pmf: read_reg8_msrx(offset: %x, mask: %x, shift: %x,"
		  " xor: %x\n", offset, mask, shift, xor);

	PMF_PARSE_CALL(read_reg8_msrx, cmd, h, offset, mask, shift, xor);
}

static int pmf_parser_write_reg32_slm(struct pmf_cmd *cmd,
				      struct pmf_handlers *h)
{
	u32 offset = pmf_next32(cmd);
	u32 shift = pmf_next32(cmd);
	u32 mask = pmf_next32(cmd);

	LOG_PARSE("pmf: write_reg32_slm(offset: %x, shift: %x, mask: %x\n",
		  offset, shift, mask);

	PMF_PARSE_CALL(write_reg32_slm, cmd, h, offset, shift, mask);
}

static int pmf_parser_write_reg16_slm(struct pmf_cmd *cmd,
				      struct pmf_handlers *h)
{
	u32 offset = pmf_next32(cmd);
	u32 shift = pmf_next32(cmd);
	u32 mask = pmf_next32(cmd);

	LOG_PARSE("pmf: write_reg16_slm(offset: %x, shift: %x, mask: %x\n",
		  offset, shift, mask);

	PMF_PARSE_CALL(write_reg16_slm, cmd, h, offset, shift, mask);
}

static int pmf_parser_write_reg8_slm(struct pmf_cmd *cmd,
				     struct pmf_handlers *h)
{
	u32 offset = pmf_next32(cmd);
	u32 shift = pmf_next32(cmd);
	u32 mask = pmf_next32(cmd);

	LOG_PARSE("pmf: write_reg8_slm(offset: %x, shift: %x, mask: %x\n",
		  offset, shift, mask);

	PMF_PARSE_CALL(write_reg8_slm, cmd, h, offset, shift, mask);
}

static int pmf_parser_mask_and_compare(struct pmf_cmd *cmd,
				       struct pmf_handlers *h)
{
	u32 bytes = pmf_next32(cmd);
	const void *maskblob = pmf_next_blob(cmd, bytes);
	const void *valuesblob = pmf_next_blob(cmd, bytes);

	LOG_PARSE("pmf: mask_and_compare(length: %ud ...\n", bytes);
	LOG_BLOB("pmf:   mask data: \n", maskblob, bytes);
	LOG_BLOB("pmf:   values data: \n", valuesblob, bytes);

	PMF_PARSE_CALL(mask_and_compare, cmd, h,
		       bytes, maskblob, valuesblob);
}


typedef int (*pmf_cmd_parser_t)(struct pmf_cmd *cmd, struct pmf_handlers *h);

static pmf_cmd_parser_t pmf_parsers[PMF_CMD_COUNT] =
{
	NULL,
	pmf_parser_write_gpio,
	pmf_parser_read_gpio,
	pmf_parser_write_reg32,
	pmf_parser_read_reg32,
	pmf_parser_write_reg16,
	pmf_parser_read_reg16,