Am 11.12.2014 16:36, schrieb Bartosz Golaszewski: > This version includes fixes suggested for v1 and implements i2cset. Any > testing and suggestions for size reduction are welcome. > > Next: i2cdump and i2cdetect. > > v1: > http://lists.busybox.net/pipermail/busybox/2014-December/082071.html > > Signed-off-by: Bartosz Golaszewski <bartekg...@gmail.com> > --- > miscutils/i2c_tools.c | 727 > ++++++++++++++++++++++++++++++++++++++++++++++++++ > 1 file changed, 727 insertions(+) > create mode 100644 miscutils/i2c_tools.c > > diff --git a/miscutils/i2c_tools.c b/miscutils/i2c_tools.c > new file mode 100644 > index 0000000..ad04e0d > --- /dev/null > +++ b/miscutils/i2c_tools.c > @@ -0,0 +1,727 @@ > +/* vi: set sw=4 ts=4: */ > +/* > + * Minimal i2c-tools implementation for busybox. > + * Parts of code ported from i2c-tools: > + * http://www.lm-sensors.org/wiki/I2CTools. > + * > + * Copyright (C) 2014 by Bartosz Golaszewski <bartekg...@gmail.com> > + * > + * Licensed under GPLv2 or later, see file LICENSE in this source tree. > + */ > + > +//config:config I2CGET > +//config: bool "i2cget" > +//config: default y > +//config: select PLATFORM_LINUX > +//config: help > +//config: Read from I2C/SMBus chip registers. > +//config: > +//config:config I2CSET > +//config: bool "i2cset" > +//config: default y > +//config: select PLATFORM_LINUX > +//config: help > +//config: Set I2C registers. > +//config: > + > +//applet:IF_I2CGET(APPLET(i2cget, BB_DIR_USR_SBIN, BB_SUID_DROP)) > +//applet:IF_I2CSET(APPLET(i2cset, BB_DIR_USR_SBIN, BB_SUID_DROP)) > + > +//kbuild:lib-$(CONFIG_I2CGET) += i2c_tools.o > +//kbuild:lib-$(CONFIG_I2CSET) += i2c_tools.o > + > +//usage:#define i2cget_trivial_usage > +//usage: "[-f] [-y] BUS CHIP-ADDRESS [DATA-ADDRESS [MODE]]" > +//usage:#define i2cget_full_usage "\n\n" > +//usage: "Read from I2C/SMBus chip registers\n" > +//usage: "\n I2CBUS is an i2c bus number" > +//usage: "\n ADDRESS is an integer (0x03 - 0x77)" > +//usage: "\n MODE is one of:" > +//usage: "\n b (read byte data, default)" > +//usage: "\n w (read word data)" > +//usage: "\n c (write byte/read byte)" > +//usage: "\n\n Append p for SMBus PEC" > +//usage: "\n\n -f - force access to device" > +//usage: "\n -y - disable interactive mode" > +//usage: > +//usage:#define i2cset_trivial_usage > +//usage: "[-f] [-y] [-m MASK] BUS CHIP-ADDR DATA-ADDR [VALUE] ... > [MODE]" > +//usage:#define i2cset_full_usage "\n\n" > +//usage: "Set I2C registers\n" > +//usage: "\n I2CBUS is an i2c bus number" > +//usage: "\n ADDRESS is an integer (0x03 - 0x77)" > +//usage: "\n MODE is one of:" > +//usage: "\n c (byte, no value)" > +//usage: "\n b (byte data, default)" > +//usage: "\n w (word data)" > +//usage: "\n i (I2C block data)" > +//usage: "\n s (SMBus block data)" > +//usage: "\n\n Append p for SMBus PEC" > +//usage: "\n\n -f - force access to device" > +//usage: "\n -y - disable interactive mode" > +//usage: "\n -r - read back and compare the result" > +//usage: "\n -m - mask specifying which bits to write" > +//usage: > + > +#include "libbb.h" > + > +/* > + * /dev/i2c-X ioctl commands. The ioctl's parameter is always an unsigned > long, > + * except for: > + * - I2C_FUNCS, takes pointer to an unsigned long > + * - I2C_RDWR, takes pointer to struct i2c_rdwr_ioctl_data > + * - I2C_SMBUS, takes pointer to struct i2c_smbus_ioctl_data > + */ > + > +/* Number of times a device address should be polled when not acknowledging > */ > +#define I2C_RETRIES 0x0701 > +/* Set timeout in units of 10 ms */ > +#define I2C_TIMEOUT 0x0702 > + > +/* > + * NOTE: Slave address is 7 or 10 bits, but 10-bit addresses > + * are NOT supported! (due to code brokenness) > + */ > + > +/* Use this slave address */ > +#define I2C_SLAVE 0x0703 > +/* Use this slave address, even if it is already in use by a driver! */ > +#define I2C_SLAVE_FORCE 0x0706 > +/* 0 for 7 bit addrs, != 0 for 10 bit */ > +#define I2C_TENBIT 0x0704 > +/* Get the adapter functionality mask */ > +#define I2C_FUNCS 0x0705 > +/* Combined R/W transfer (one STOP only) */ > +#define I2C_RDWR 0x0707 > +/* != 0 to use PEC with SMBus */ > +#define I2C_PEC 0x0708 > +/* SMBus transfer */ > +#define I2C_SMBUS 0x0720 > + > +/* This is the structure as used in the I2C_SMBUS ioctl call */ > +struct i2c_smbus_ioctl_data { > + uint8_t read_write; > + uint8_t command; > + uint32_t size; > + union i2c_smbus_data *data; > +}; > + > +/* This is the structure as used in the I2C_RDWR ioctl call */ > +struct i2c_rdwr_ioctl_data { > + struct i2c_msg *msgs; /* pointers to i2c_msgs */ > + uint32_t nmsgs; /* number of i2c_msgs */ > +}; > + > +/* As specified in SMBus standard */ > +#define I2C_SMBUS_BLOCK_MAX 32 > +/* Not specified but we use same structure */ > +#define I2C_SMBUS_I2C_BLOCK_MAX 32 > + > +/* > + * Data for SMBus Messages > + */ > +union i2c_smbus_data { > + uint8_t byte; > + uint16_t word; > + /* block[0] is used for length and one more for PEC */ > + uint8_t block[I2C_SMBUS_BLOCK_MAX + 2]; > +}; > + > +#define I2C_RDRW_IOCTL_MAX_MSGS 42 > + > +/* smbus_access read or write markers */ > +#define I2C_SMBUS_READ 1 > +#define I2C_SMBUS_WRITE 0 > + > +/* SMBus transaction types (size parameter in the below functions). */ > +#define I2C_SMBUS_QUICK 0 > +#define I2C_SMBUS_BYTE 1 > +#define I2C_SMBUS_BYTE_DATA 2 > +#define I2C_SMBUS_WORD_DATA 3 > +#define I2C_SMBUS_PROC_CALL 4 > +#define I2C_SMBUS_BLOCK_DATA 5 > +#define I2C_SMBUS_I2C_BLOCK_BROKEN 6 > +#define I2C_SMBUS_BLOCK_PROC_CALL 7 > +#define I2C_SMBUS_I2C_BLOCK_DATA 8 > + > +/* Defines to determine what functionality is present */ > +#define I2C_FUNC_I2C 0x00000001 > +#define I2C_FUNC_10BIT_ADDR 0x00000002 > +#define I2C_FUNC_PROTOCOL_MANGLING 0x00000004 > +#define I2C_FUNC_SMBUS_PEC 0x00000008 > +#define I2C_FUNC_SMBUS_BLOCK_PROC_CALL 0x00008000 > +#define I2C_FUNC_SMBUS_QUICK 0x00010000 > +#define I2C_FUNC_SMBUS_READ_BYTE 0x00020000 > +#define I2C_FUNC_SMBUS_WRITE_BYTE 0x00040000 > +#define I2C_FUNC_SMBUS_READ_BYTE_DATA 0x00080000 > +#define I2C_FUNC_SMBUS_WRITE_BYTE_DATA 0x00100000 > +#define I2C_FUNC_SMBUS_READ_WORD_DATA 0x00200000 > +#define I2C_FUNC_SMBUS_WRITE_WORD_DATA 0x00400000 > +#define I2C_FUNC_SMBUS_PROC_CALL 0x00800000 > +#define I2C_FUNC_SMBUS_READ_BLOCK_DATA 0x01000000 > +#define I2C_FUNC_SMBUS_WRITE_BLOCK_DATA 0x02000000 > +#define I2C_FUNC_SMBUS_READ_I2C_BLOCK 0x04000000 > +#define I2C_FUNC_SMBUS_WRITE_I2C_BLOCK 0x08000000 > + > +/* > + * This is needed for ioctl_or_perror_and_die() since it only accepts > pointer. > + */ > +#define INT_TO_PTR(i) ((void*)((long)i)) > + > +static int32_t i2c_smbus_access(int fd, char read_write, uint8_t cmd, > + int size, union i2c_smbus_data *data) > +{ > + struct i2c_smbus_ioctl_data args; > + > + args.read_write = read_write; > + args.command = cmd; > + args.size = size; > + args.data = data; > + > + return ioctl(fd, I2C_SMBUS, &args); > +} > + > +static int32_t i2c_smbus_read_byte(int fd) > +{ > + union i2c_smbus_data data; > + int err; > + > + err = i2c_smbus_access(fd, I2C_SMBUS_READ, 0, I2C_SMBUS_BYTE, &data); > + if (err < 0) > + return err; > + > + return 0xff & data.byte; > +} > + > +static int32_t i2c_smbus_write_byte(int fd, uint8_t val) > +{ > + return i2c_smbus_access(fd, I2C_SMBUS_WRITE, > + val, I2C_SMBUS_BYTE, NULL); > +} > + > +static int32_t i2c_smbus_read_byte_data(int fd, uint8_t cmd) > +{ > + union i2c_smbus_data data; > + int err; > + > + err = i2c_smbus_access(fd, I2C_SMBUS_READ, cmd, > + I2C_SMBUS_BYTE_DATA, &data); > + if (err < 0) > + return err; > + > + return 0xff & data.byte; > +} > + > +static int32_t i2c_smbus_read_word_data(int fd, uint8_t cmd) > +{ > + union i2c_smbus_data data; > + int err; > + > + err = i2c_smbus_access(fd, I2C_SMBUS_READ, cmd, > + I2C_SMBUS_WORD_DATA, &data); > + if (err < 0) > + return err; > + > + return 0xffff & data.word; > +} > + > +#if ENABLE_I2CSET > +static int32_t i2c_smbus_write_byte_data(int file, > + uint8_t cmd, uint8_t value) > +{ > + union i2c_smbus_data data; > + > + data.byte = value; > + > + return i2c_smbus_access(file, I2C_SMBUS_WRITE, cmd, > + I2C_SMBUS_BYTE_DATA, &data); > +} > + > +static int32_t i2c_smbus_write_word_data(int file, uint8_t cmd, uint16_t > value) > +{ > + union i2c_smbus_data data; > + > + data.word = value; > + > + return i2c_smbus_access(file, I2C_SMBUS_WRITE, cmd, > + I2C_SMBUS_WORD_DATA, &data); > +} > + > +static int32_t i2c_smbus_write_block_data(int file, uint8_t cmd, > + uint8_t length, const uint8_t *values) > +{ > + union i2c_smbus_data data; > + int i; > + > + if (length > I2C_SMBUS_BLOCK_MAX) > + length = I2C_SMBUS_BLOCK_MAX; > + for (i = 1; i <= length; i++) > + data.block[i] = values[i-1]; > + data.block[0] = length; > + > + return i2c_smbus_access(file, I2C_SMBUS_WRITE, cmd, > + I2C_SMBUS_BLOCK_DATA, &data); > +} > + > +static int32_t i2c_smbus_write_i2c_block_data(int file, uint8_t cmd, > + uint8_t length, const uint8_t *values) > +{ > + union i2c_smbus_data data; > + int i; > + > + if (length > I2C_SMBUS_BLOCK_MAX) > + length = I2C_SMBUS_BLOCK_MAX; > + for (i = 1; i <= length; i++) > + data.block[i] = values[i-1];
I have no idea how often this is uses but perhaps it is useful to copy the whole block in once instead of byte wise. memcpy(data.block+1,values,length); /* untested */ (Same above) > + data.block[0] = length; > + > + return i2c_smbus_access(file, I2C_SMBUS_WRITE, cmd, > + I2C_SMBUS_I2C_BLOCK_BROKEN, &data); > +} > +#endif /* ENABLE_I2CSET */ > + > +static int i2c_bus_lookup(const char *bus_str) > +{ > + return xstrtoi_range(bus_str, 10, 0, 0xfffff); > +} > + > +static int i2c_parse_bus_addr(const char *addr_str) > +{ > + return xstrtoi_range(addr_str, 16, 0x03, 0x77); > +} > + > +static int i2c_parse_data_addr(const char *data_addr) > +{ > + return xstrtoi_range(data_addr, 16, 0, 0xff); > +} i am not an i2c expert, can you drop a comment here about the magic numbers ? > +/* Opens the device file associated with given i2c bus. */ > +static int i2c_dev_open(int i2cbus) > +{ > + char filename[32]; > + int fd; > + > + snprintf(filename, sizeof(filename), "/dev/i2c-%d", i2cbus); > + fd = open_or_warn(filename, O_RDWR); now user of "/dev/i2c/%d" get warning here even when the next open works. I do not think that this was you intention. /* having one open is more easy :) */ just my 2 cents, wh > + if (fd < 0 && errno == ENOENT) { > + snprintf(filename, sizeof(filename), "/dev/i2c/%d", i2cbus); > + fd = xopen(filename, O_RDWR); > + } > + > + return fd; > +} > + > +static void i2c_set_slave_addr(int fd, int addr, int force) > +{ > + ioctl_or_perror_and_die(fd, force ? I2C_SLAVE_FORCE : I2C_SLAVE, > + INT_TO_PTR(addr), > + "Could not set address to 0x%02x", addr); > +} > + > +static void i2c_set_pec(int fd, int pec) > +{ > + ioctl_or_perror_and_die(fd, I2C_PEC, > + INT_TO_PTR(pec ? 1 : 0), > + "Could not set PEC"); > +} > + > +/* Size reducing helpers for xxx_check_funcs(). */ > +static void get_funcs_matrix(int fd, unsigned long *funcs) > +{ > + ioctl_or_perror_and_die(fd, I2C_FUNCS, funcs, > + "Could not get the adapter functionality matrix"); > +} > + > +static void check_funcs_test_end(int funcs, int pec, const char *err) > +{ > + if (pec && !(funcs & (I2C_FUNC_SMBUS_PEC | I2C_FUNC_I2C))) > + fprintf(stderr, > + "Warning: Adapter does not seem to support PEC\n"); > + > + if (err) > + bb_error_msg_and_die("Adapter does not have %s capability", > + err); > +} > + > +/* > + * The below functions emit an error message and exit if the adapter doesn't > + * support desired functionalities. > + */ > +#if ENABLE_I2CGET > +static void i2cget_check_funcs(int fd, int mode, int data_addr, int pec) > +{ > + unsigned long funcs; > + const char *err = NULL; > + > + get_funcs_matrix(fd, &funcs); > + switch (mode) { > + case I2C_SMBUS_BYTE: > + if (!(funcs & I2C_FUNC_SMBUS_READ_BYTE)) { > + err = "SMBus receive byte"; > + break; > + } > + if (data_addr >= 0 && !(funcs & I2C_FUNC_SMBUS_WRITE_BYTE)) > + err = "SMBus send byte"; > + break; > + case I2C_SMBUS_BYTE_DATA: > + if (!(funcs & I2C_FUNC_SMBUS_READ_BYTE_DATA)) > + err = "SMBus read byte"; > + break; > + case I2C_SMBUS_WORD_DATA: > + if (!(funcs & I2C_FUNC_SMBUS_READ_WORD_DATA)) > + err = "SMBus read word"; > + break; > + } > + check_funcs_test_end(funcs, pec, err); > +} > +#endif /* ENABLE_I2CGET */ > + > +#if ENABLE_I2CSET > +static void i2cset_check_funcs(int fd, int mode, int pec) > +{ > + unsigned long funcs; > + const char *err = NULL; > + > + get_funcs_matrix(fd, &funcs); > + switch (mode) { > + case I2C_SMBUS_BYTE: > + if (!(funcs & I2C_FUNC_SMBUS_WRITE_BYTE)) > + err = "SMBus send byte"; > + break; > + > + case I2C_SMBUS_BYTE_DATA: > + if (!(funcs & I2C_FUNC_SMBUS_WRITE_BYTE_DATA)) > + err = "SMBus write byte"; > + break; > + > + case I2C_SMBUS_WORD_DATA: > + if (!(funcs & I2C_FUNC_SMBUS_WRITE_WORD_DATA)) > + err = "SMBus write word"; > + break; > + > + case I2C_SMBUS_BLOCK_DATA: > + if (!(funcs & I2C_FUNC_SMBUS_WRITE_BLOCK_DATA)) > + err = "SMBus block write"; > + break; > + case I2C_SMBUS_I2C_BLOCK_DATA: > + if (!(funcs & I2C_FUNC_SMBUS_WRITE_I2C_BLOCK)) > + err = "I2C block write"; > + break; > + } > + check_funcs_test_end(funcs, pec, err); > +} > +#endif /* ENABLE_I2CSET */ > + > +/*********************************** XXXX > ***********************************/ > +/* > + * XXX read_line() exists already in util-linux/fdisk.c - move it to > + * libbb and reuse here in the final version of the patch. This will save > + * additional 116 bytes. > + */ > +static char *line_ptr; > +static char line_buffer[128]; > + > +static int read_line(const char *prompt) > +{ > + int sz; > + > + sz = read_line_input(NULL, prompt, line_buffer, > + sizeof(line_buffer), /*timeout*/ -1); > + if (sz <= 0) > + exit(EXIT_SUCCESS); /* Ctrl-D or Ctrl-C */ > + > + if (line_buffer[sz-1] == '\n') > + line_buffer[--sz] = '\0'; > + > + line_ptr = line_buffer; > + while (*line_ptr != '\0' && (unsigned char)*line_ptr <= ' ') > + line_ptr++; > + return *line_ptr; > +} > + > +static void user_confirm(void) > +{ > + if (tolower(read_line("Continue? [y/N] ")) != 'y') { > + fprintf(stderr, "Aborting.\n"); > + xfunc_die(); > + } > +} > +/*********************************** XXXX > ***********************************/ > + > +/* > + * Return only if user confirms the action, abort otherwise. > + * > + * The messages displayed here are much less elaborate than their i2c-tools > + * counterparts - this is done for size reduction. > + */ > +static void confirm_action(int bus_addr, int mode, int data_addr, int pec) > +{ > + fprintf(stderr, "WARNING! This program can confuse your I2C bus\n"); > + > + /* Don't let the user break his/her EEPROMs */ > + if (bus_addr >= 0x50 && bus_addr <= 0x57 && pec) { > + fprintf(stderr, "This is I2C not smbus - using PEC on I2C " > + "devices may result in data loss. Aborting.\n"); > + xfunc_die(); > + } > + > + if (mode == I2C_SMBUS_BYTE && data_addr >= 0 && pec) > + fprintf(stderr, "WARNING! May interpret a write byte command " > + "with PEC as a write byte data command!\n"); > + > + if (pec) > + fprintf(stderr, "PEC checking enabled.\n"); > + > + user_confirm(); > +} > + > +#if ENABLE_I2CGET > +int i2cget_main(int argc, char **argv) MAIN_EXTERNALLY_VISIBLE; > +int i2cget_main(int argc, char **argv) > +{ > + static const unsigned opt_f = (1 << 0), opt_y = (1 << 1); > + static const char *const optstr = "fy"; > + > + int bus_num, bus_addr, data_addr = -1, status; > + int mode = I2C_SMBUS_BYTE, pec = 0, fd; > + unsigned opts; > + > + opts = getopt32(argv, optstr); > + argv += optind; > + argc -= optind; > + > + if (argc < 2) > + bb_show_usage(); > + > + bus_num = i2c_bus_lookup(argv[0]); > + bus_addr = i2c_parse_bus_addr(argv[1]); > + > + if (argc > 2) { > + data_addr = i2c_parse_data_addr(argv[2]); > + mode = I2C_SMBUS_BYTE_DATA; > + } > + > + if (argc > 3) { > + switch (argv[3][0]) { > + case 'b': /* Already set */ break; > + case 'w': mode = I2C_SMBUS_WORD_DATA; break; > + case 'c': mode = I2C_SMBUS_BYTE; break; > + default: > + bb_error_msg("Invalid mode"); > + bb_show_usage(); > + } > + pec = argv[3][1] == 'p'; > + } > + > + fd = i2c_dev_open(bus_num); > + i2cget_check_funcs(fd, mode, data_addr, pec); > + i2c_set_slave_addr(fd, bus_addr, opts & opt_f); > + > + if (!(opts & opt_y)) > + confirm_action(bus_addr, mode, data_addr, pec); > + > + if (pec) > + i2c_set_pec(fd, 1); > + > + switch (mode) { > + case I2C_SMBUS_BYTE: > + if (data_addr >= 0) { > + status = i2c_smbus_write_byte(fd, data_addr); > + if (status < 0) > + fprintf(stderr, "Warning - write failed\n"); > + } > + status = i2c_smbus_read_byte(fd); > + break; > + case I2C_SMBUS_WORD_DATA: > + status = i2c_smbus_read_word_data(fd, data_addr); > + break; > + default: /* I2C_SMBUS_BYTE_DATA */ > + status = i2c_smbus_read_byte_data(fd, data_addr); > + } > + close(fd); > + > + if (status < 0) > + bb_perror_msg_and_die("Read failed"); > + > + printf("0x%0*x\n", mode == I2C_SMBUS_WORD_DATA ? 4 : 2, status); > + > + return 0; > +} > +#endif /* ENABLE_I2CGET */ > + > +#if ENABLE_I2CSET > +int i2cset_main(int argc, char **argv) MAIN_EXTERNALLY_VISIBLE; > +int i2cset_main(int argc, char **argv) > +{ > + static const unsigned opt_f = (1 << 0), opt_y = (1 << 1), > + opt_m = (1 << 2), opt_r = (1 << 3); > + static const char *const optstr = "fym:r"; > + > + int bus_num, bus_addr, data_addr, mode = I2C_SMBUS_BYTE, pec = 0; > + int val, blen = 0, mask = 0, fd, status; > + unsigned char block[I2C_SMBUS_BLOCK_MAX]; > + char *opt_m_arg = NULL; > + unsigned opts; > + > + opts = getopt32(argv, optstr, &opt_m_arg); > + argv += optind; > + argc -= optind; > + > + if (argc < 3) > + bb_show_usage(); > + > + bus_num = i2c_bus_lookup(argv[0]); > + bus_addr = i2c_parse_bus_addr(argv[1]); > + data_addr = i2c_parse_data_addr(argv[2]); > + > + if (argc > 3) { > + switch (argv[argc-1][0]) { > + case 'c': mode = I2C_SMBUS_BYTE; break; > + case 'b': mode = I2C_SMBUS_BYTE_DATA; break; > + case 'w': mode = I2C_SMBUS_WORD_DATA; break; > + case 's': mode = I2C_SMBUS_BLOCK_DATA; break; > + case 'i': mode = I2C_SMBUS_I2C_BLOCK_DATA; break; > + default: > + bb_error_msg("Invalid mode"); > + bb_show_usage(); > + } > + pec = argv[argc-1][1] == 'p'; > + if (mode == I2C_SMBUS_BLOCK_DATA || > + mode == I2C_SMBUS_I2C_BLOCK_DATA) { > + if (pec && mode == I2C_SMBUS_I2C_BLOCK_DATA) > + bb_error_msg_and_die("PEC not supported for " > + "I2C block writes!"); > + if (opts & opt_m) > + bb_error_msg_and_die("Mask not supported " > + "for block writes!\n"); > + } > + } > + > + /* Prepare the value(s) to be written according to current mode. */ > + switch (mode) { > + case I2C_SMBUS_BYTE_DATA: > + val = xstrtoi_range(argv[3], 0, 0, 0xff); > + break; > + case I2C_SMBUS_WORD_DATA: > + val = xstrtoi_range(argv[3], 0, 0, 0xffff); > + break; > + case I2C_SMBUS_BLOCK_DATA: > + case I2C_SMBUS_I2C_BLOCK_DATA: > + for (blen = 3; blen < (argc - 1); blen++) > + block[blen] = xstrtoi_range(argv[blen], 0, 0, 0xff); > + val = -1; > + break; > + default: > + val = -1; > + break; > + } > + > + if (opts & opt_m) { > + mask = xstrtoi_range(opt_m_arg, 0, 0, > + (mode == I2C_SMBUS_BYTE || > + mode == I2C_SMBUS_BYTE_DATA) ? 0xff : 0xffff); > + } > + > + fd = i2c_dev_open(bus_num); > + i2cset_check_funcs(fd, mode, pec); > + i2c_set_slave_addr(fd, bus_addr, opts & opt_f); > + > + if (!(opts & opt_y)) > + confirm_action(bus_addr, mode, data_addr, pec); > + > + /* > + * If we're using mask - read the current value here and adjust the > + * value to be written. > + */ > + if (opts & opt_m) { > + int tmpval; > + > + switch (mode) { > + case I2C_SMBUS_BYTE: > + tmpval = i2c_smbus_read_byte(fd); > + break; > + case I2C_SMBUS_WORD_DATA: > + tmpval = i2c_smbus_read_word_data(fd, data_addr); > + break; > + default: > + tmpval = i2c_smbus_read_byte_data(fd, data_addr); > + } > + > + if (tmpval < 0) > + bb_perror_msg_and_die("Failed to read old value"); > + > + val = (val & mask) | (tmpval & ~mask); > + > + if (!(opts & opt_y)) { > + fprintf(stderr, "Old value 0x%0*x, write mask " > + "0x%0*x: Will write 0x%0*x to register " > + "0x%02x\n", > + mode == I2C_SMBUS_WORD_DATA ? 4 : 2, tmpval, > + mode == I2C_SMBUS_WORD_DATA ? 4 : 2, mask, > + mode == I2C_SMBUS_WORD_DATA ? 4 : 2, val, > + data_addr); > + > + user_confirm(); > + } > + } > + > + if (pec) > + i2c_set_pec(fd, 1); > + > + switch (mode) { > + case I2C_SMBUS_BYTE: > + status = i2c_smbus_write_byte(fd, data_addr); > + break; > + case I2C_SMBUS_WORD_DATA: > + status = i2c_smbus_write_word_data(fd, data_addr, val); > + break; > + case I2C_SMBUS_BLOCK_DATA: > + status = i2c_smbus_write_block_data(fd, data_addr, > + blen, block); > + break; > + case I2C_SMBUS_I2C_BLOCK_DATA: > + status = i2c_smbus_write_i2c_block_data(fd, data_addr, > + blen, block); > + break; > + default: /* I2C_SMBUS_BYTE_DATA */ > + status = i2c_smbus_write_byte_data(fd, data_addr, val); > + break; > + } > + if (status < 0) > + bb_perror_msg_and_die("Write failed\n"); > + > + if (pec) > + i2c_set_pec(fd, 0); /* Clear PEC */ > + > + /* No readback required - we're done. */ > + if (!(opts & opt_r)) > + return 0; > + > + switch (mode) { > + case I2C_SMBUS_BYTE: > + status = i2c_smbus_read_byte(fd); > + val = data_addr; > + break; > + case I2C_SMBUS_WORD_DATA: > + status = i2c_smbus_read_word_data(fd, data_addr); > + break; > + default: /* I2C_SMBUS_BYTE_DATA */ > + status = i2c_smbus_read_byte_data(fd, data_addr); > + } > + > + if (status < 0) { > + printf("Warning - readback failed\n"); > + } else > + if (status != val) { > + printf("Warning - data mismatch - wrote " > + "0x%0*x, read back 0x%0*x\n", > + mode == I2C_SMBUS_WORD_DATA ? 4 : 2, val, > + mode == I2C_SMBUS_WORD_DATA ? 4 : 2, status); > + } else { > + printf("Value 0x%0*x written, readback matched\n", > + mode == I2C_SMBUS_WORD_DATA ? 4 : 2, val); > + } > + > + return 0; > +} > +#endif /* ENABLE_I2CSET */ _______________________________________________ busybox mailing list busybox@busybox.net http://lists.busybox.net/mailman/listinfo/busybox