diff --git a/dmctl3.py b/dmctl3.py index 2bc5233..5716737 100755 --- a/dmctl3.py +++ b/dmctl3.py @@ -6,6 +6,7 @@ f = os.open(sys.argv[1], os.O_RDWR | os.O_CLOEXEC) # TODO: windows: os.O_BINARY try: os.write(f, b'\x00') # get version resp = os.read(f, 4) # response: status, paylaod len (should be 2), payload + print("resp=%s"%repr(resp)) print("stat=%d plen=%d ver=%04x" % (resp[0], resp[1], struct.unpack('private_data; + void *kbuf = NULL; + struct device *dev = dmjch->dev; - kbuf = kmalloc(len + DMJ_READ_BUFSIZE, GFP_KERNEL); - if (!kbuf) return -ENOMEM; + if (len > INT_MAX) return -EINVAL; + ilen = (int)len; - dmjch = file->private_data; - dev = dmjch->dev; - todo = len; - - spin_lock(&dmjch->devopen_lock); - bpos = dmjch->bufpos; - - /* TODO: ioctl to control this buffering? */ - if (bpos) { /* data in buffer */ - if (len <= DMJ_READ_BUFSIZE - bpos) { - /* doable in a single copy, no USB xfers needed */ - memcpy(kbuf, &dmjch->rdbuf[bpos], len); - bpos += len; - if (bpos == DMJ_READ_BUFSIZE) bpos = 0; - - done += len; - todo -= len; - } else { - /* initial copy stuff */ - memcpy(kbuf, &dmjch->rdbuf[bpos], DMJ_READ_BUFSIZE - bpos); - todo -= DMJ_READ_BUFSIZE - bpos; - done += DMJ_READ_BUFSIZE - bpos; - bpos = 0; - } + res = dmj_transfer(dmjch->pdev, -1, DMJ_XFER_FLAGS_FILL_RECVBUF, NULL, 0, &kbuf, &ilen); + if (res < 0 || ilen < 0 || !kbuf) { + if (kbuf) kfree(kbuf); + return (res >= 0) ? -EIO : ret; } - if /*while*/ (todo) { /* TODO: do we want a while here? */ - bsize = DMJ_READ_BUFSIZE; + ret = copy_to_user(buf, kbuf, (size_t)ilen); + kfree(kbuf); - res = dmj_read(dmjch->pdev, 0, &kbuf[done], &bsize); - if (res < 0 || bsize < 0) { - /* ah snap */ - spin_unlock(&dmjch->devopen_lock); - return res; - } + if (ret) return -EFAULT; - if ((size_t)bsize > todo) { - if ((size_t)bsize > todo + DMJ_READ_BUFSIZE) { - /* can't hold all this data, time to bail out... */ - dev_err(dev, "too much data (%zu B excess), can't buffer, AAAAAA", - (size_t)bsize - (todo + DMJ_READ_BUFSIZE)); - spin_unlock(&dmjch->devopen_lock); - BUG(); /* some stuff somewhere will have been corrupted, so, get out while we can */ - } - - /* stuff for next call */ - done = todo; - bpos = DMJ_READ_BUFSIZE - ((size_t)bsize - todo); - memcpy(&dmjch->rdbuf[bpos], &kbuf[done], (size_t)bsize - todo); - todo = 0; - } else { - todo -= (size_t)bsize; - done += (size_t)bsize; - } - } - - dmjch->bufpos = bpos; - spin_unlock(&dmjch->devopen_lock); - - res = copy_to_user(buf, kbuf, len); - if (res) return (res < 0) ? res : -EFAULT; - - return done; + return (ssize_t)ilen; } static ssize_t dmj_char_write(struct file *file, const char *buf, size_t len, loff_t *off) { unsigned long ret; - struct dmj_char_dev *dmjch; + int res; + struct dmj_char_dev *dmjch = file->private_data; void *kbuf; - dmjch = file->private_data; - kbuf = kmalloc(len, GFP_KERNEL); if (!kbuf) return -ENOMEM; ret = copy_from_user(kbuf, buf, len); if (ret) { kfree(kbuf); - return (ret < 0) ? ret : -EFAULT; + return -EFAULT; } - ret = dmj_transfer(dmjch->pdev, -1, 0, kbuf, len, NULL, NULL); - if (ret < 0) { - kfree(kbuf); - return ret; - } + res = dmj_transfer(dmjch->pdev, -1, 0, kbuf, len, NULL, NULL); kfree(kbuf); - return len; + return (res < 0) ? res : len; } static int dmj_char_open(struct inode *inode, struct file *file) { struct dmj_char_dev *dmjch; - int ret; dmjch = container_of(inode->i_cdev, struct dmj_char_dev, cdev); - spin_lock(&dmjch->devopen_lock); - ret = dmjch->bufpos; - if (~ret == 0) dmjch->bufpos = 0; - spin_unlock(&dmjch->devopen_lock); - - if (~ret != 0) return -ETXTBSY; // already open - file->private_data = dmjch; return 0; @@ -174,10 +104,6 @@ static int dmj_char_release(struct inode *inode, struct file *file) dmjch = container_of(inode->i_cdev, struct dmj_char_dev, cdev); - spin_lock(&dmjch->devopen_lock); - dmjch->bufpos = ~(size_t)0; - spin_unlock(&dmjch->devopen_lock); - return 0; } @@ -231,9 +157,6 @@ static int dmj_char_probe(struct platform_device *pdev) dmjch->dev = device; dmjch->minor = minor; dmjch->pdev = pdev; - dmjch->bufpos = ~(size_t)0; - - spin_lock_init(&dmjch->devopen_lock); return 0; } diff --git a/host/modules/dmj.c b/host/modules/dmj.c index c453a68..72a41f5 100644 --- a/host/modules/dmj.c +++ b/host/modules/dmj.c @@ -96,39 +96,15 @@ static int dmj_send_wait(struct dmj_dev *dmj, int cmd, const void *wbuf, int wbu return ret; } -static int dmj_recv_wait(struct dmj_dev *dmj, void **kbuf, int rbufsize, bool parse_hdr) -{ - int len, actual; - int ret; - void *buf; - - *kbuf = NULL; - - if (rbufsize <= 0) len = 0; - else if (parse_hdr) len = rbufsize + DMJ_RESP_HDR_SIZE; - else len = rbufsize; - - buf = kmalloc(len, GFP_KERNEL); - if (!buf) return -ENOMEM; - - ret = usb_bulk_msg(dmj->usb_dev, usb_rcvbulkpipe(dmj->usb_dev, dmj->ep_in), - buf, len, &actual, DMJ_USB_TIMEOUT); - if (ret < 0) kfree(buf); - - *kbuf = buf; - return actual; -} - int dmj_xfer_internal(struct dmj_dev *dmj, int cmd, int recvflags, - const void *wbuf, int wbufsize, void *rbuf, int *rbufsize) + const void *wbuf, int wbufsize, void **rbuf, int *rbufsize) { - int ret = 0, actual; + int ret = 0, actual, pl_off, todo; struct device *dev = &dmj->interface->dev; - int respstat, total_len, bytes_read; uint32_t pl_len; - void *buf; - uint8_t *dbuf; - ptrdiff_t pl_off; + void *tmpbuf = NULL, *longbuf = NULL; + uint8_t *bbuf; + uint8_t respstat; spin_lock(&dmj->disconnect_lock); if (dmj->disconnect) ret = -ENODEV; @@ -144,125 +120,172 @@ int dmj_xfer_internal(struct dmj_dev *dmj, int cmd, int recvflags, } } - if ((recvflags & DMJ_XFER_FLAGS_PARSE_RESP) == 0 - && !(rbufsize && *rbufsize > 0 && rbuf)) { - /* don't want any type of response? then dont do the urb stuff either */ - return 0; - } - - /* first recv buffer, with optional response header parsing */ - ret = dmj_recv_wait(dmj, &buf, (rbufsize && *rbufsize > 0) ? *rbufsize : 0, - (recvflags & DMJ_XFER_FLAGS_PARSE_RESP) != 0); - if (ret < 0 || !buf) { - dev_err(dev, "USB read failed: %d\n", ret); - return ret; - } - actual = ret; + if (rbuf) *rbuf = NULL; if (recvflags & DMJ_XFER_FLAGS_PARSE_RESP) { - dbuf = buf; + /* + * if we do want to parse the response, we'll split the reads into + * blocks of 64 bytes, first to read the response header, and then + * allocate a big reponse buffer, and then keep doing 64b reads + * to fill that buffer. result length will be put in rbufsize, its + * value when passed to the function will not matter + */ - /* decode payload length */ - if (dbuf[1] & 0x80) { + if (*rbufsize) *rbufsize = -1; + + tmpbuf = kmalloc(64, GFP_KERNEL); + if (!tmpbuf) return -ENOMEM; + /* first read: 64b, with header data to parse */ + ret = usb_bulk_msg(dmj->usb_dev, usb_rcvbulkpipe(dmj->usb_dev, dmj->ep_in), + tmpbuf, 64, &actual, DMJ_USB_TIMEOUT); + if (ret < 0) goto err_freetmp; + if (actual < 0) { ret = -EREMOTEIO; goto err_freetmp; } + + bbuf = tmpbuf; + + if (bbuf[1] & 0x80) { if (actual < 3) { dev_err(dev, "short response (%d, expected at least 3)\n", actual); - kfree(buf); - return -EREMOTEIO; + ret = -EREMOTEIO; + goto err_freetmp; } - pl_len = (uint32_t)(dbuf[1] & 0x7f); + pl_len = (uint32_t)(bbuf[1] & 0x7f); - if (dbuf[2] & 0x80) { + if (bbuf[2] & 0x80) { if (actual < 4) { dev_err(dev, "short response (%d, expected at least 4)\n", actual); - kfree(buf); - return -EREMOTEIO; + ret = -EREMOTEIO; + goto err_freetmp; } - pl_len |= (uint32_t)(dbuf[2] & 0x7f) << 7; - pl_len |= (uint32_t)dbuf[3] << 14; + pl_len |= (uint32_t)(bbuf[2] & 0x7f) << 7; + pl_len |= (uint32_t)bbuf[3] << 14; pl_off = 4; } else { - pl_len |= (uint32_t)dbuf[2] << 7; + pl_len |= (uint32_t)bbuf[2] << 7; pl_off = 3; } } else { if (actual < 2) { dev_err(dev, "short response (%d, expected at least 2)\n", actual); - kfree(buf); - return -EREMOTEIO; + ret = -EREMOTEIO; + goto err_freetmp; } - pl_len = (uint32_t)dbuf[1]; + pl_len = (uint32_t)bbuf[1]; pl_off = 2; } - respstat = dbuf[0]; - total_len = pl_len; - actual -= pl_off; + respstat = bbuf[0]; - /*dev_dbg(dev, "pl_len=%d,off=%ld,resp=%d\n", pl_len, pl_off, respstat);*/ - } else { - pl_off = 0; - if (rbufsize && *rbufsize > 0) total_len = *rbufsize; - else total_len = actual; - respstat = 0; - } + dev_dbg(dev, "got packet hdr: status %02x, payload len 0x%x, off %u\n", + respstat, pl_len, pl_off); - if (rbuf && rbufsize && *rbufsize > 0) { - if (*rbufsize < total_len) total_len = *rbufsize; + /* now that the header has been parsed, we can start filling in the + * actual response buffer */ + longbuf = kmalloc(pl_len, GFP_KERNEL); + todo = (int)pl_len; + /* rest of the data of the 1st read */ + memcpy(longbuf, tmpbuf + pl_off, actual - pl_off); + todo -= (actual - pl_off); + pl_off = actual - pl_off; - if (actual > total_len) { - /*if (recvflags & DMJ_XFER_FLAGS_FILL_RECVBUF)*/ - { - dev_err(dev, "aaa msgsize %d > %d\n", actual, total_len); - kfree(buf); - return -EMSGSIZE; - } /*else { - actual = total_len; - }*/ + while (todo) { + actual = 64; + if (todo < actual) actual = todo; + + ret = usb_bulk_msg(dmj->usb_dev, usb_rcvbulkpipe(dmj->usb_dev, dmj->ep_in), + tmpbuf, actual, &actual, DMJ_USB_TIMEOUT); + if (ret < 0) goto err_freelong; + if (actual < 0) { ret = -EREMOTEIO; goto err_freelong; } + if (actual > todo) { + dev_err(dev, "USB protocol violation! bulk reply longer than payload header!\n"); + ret = -EMSGSIZE; + goto err_freelong; + } + + memcpy(longbuf + pl_off, tmpbuf, actual); + + todo -= actual; + pl_off += actual; } - memcpy(rbuf + bytes_read, buf + pl_off, actual); - kfree(buf); - pl_off = -1; - buf = NULL; + /* we're done! */ - bytes_read = actual; + /* response maybe not always wanted in this case, maybe was just called + * to check the status */ + if (rbuf) *rbuf = longbuf; + else kfree(longbuf); + if (rbufsize) *rbufsize = (int)pl_len; - while (bytes_read < total_len && (recvflags & DMJ_XFER_FLAGS_FILL_RECVBUF) != 0) { - ret = dmj_recv_wait(dmj, &buf, total_len - bytes_read, false); - if (ret < 0 || !buf) { - dev_err(dev, "USB read failed: %d\n", ret); - return ret; - } - actual = ret; - - if (bytes_read + actual > total_len) { - /*actual = total_len - bytes_read;*/ - dev_err(dev, "aaa2 msgsize %d > %d\n", actual+bytes_read, total_len); - kfree(buf); - return -EMSGSIZE; - } - memcpy(rbuf + bytes_read, buf, actual); - kfree(buf); - buf = NULL; - - bytes_read += actual; - } + ret = respstat; } else { - bytes_read = 0; - kfree(buf); + /* + * otherwise, read max. rbufsize bytes (if using + * DMJ_XFER_FLAGS_FILL_RECVBUF, will try to fill it exactly, but it + * will error when going beyond!). also done in 64b chunks + */ + + if (!rbuf || !rbufsize || *rbufsize <= 0) return 0; + + if (recvflags & DMJ_XFER_FLAGS_FILL_RECVBUF) { + tmpbuf = kmalloc(64, GFP_KERNEL); + if (!tmpbuf) return -ENOMEM; + longbuf = kmalloc(*rbufsize, GFP_KERNEL); + if (!longbuf) { ret = -ENOMEM; goto err_freetmp; } + + todo = *rbufsize; + pl_off = 0; + while (todo) { + actual = 64; + if (todo < actual) actual = todo; + + ret = usb_bulk_msg(dmj->usb_dev, usb_rcvbulkpipe(dmj->usb_dev, dmj->ep_in), + tmpbuf, actual, &actual, DMJ_USB_TIMEOUT); + if (ret < 0) goto err_freelong; + if (actual < 0) { ret = -EREMOTEIO; goto err_freelong; } + if (actual > todo) { ret = -EMSGSIZE; goto err_freelong; } + + memcpy(longbuf + pl_off, tmpbuf, actual); + + todo -= actual; + pl_off += actual; + } + + ret = 0; + *rbuf = longbuf; + *rbufsize = pl_off; + } else { + /* just try it at once & see what happens */ + tmpbuf = NULL; + longbuf = kmalloc(*rbufsize, GFP_KERNEL); + if (!longbuf) return -ENOMEM; + + ret = usb_bulk_msg(dmj->usb_dev, usb_rcvbulkpipe(dmj->usb_dev, dmj->ep_in), + longbuf, *rbufsize, rbufsize, DMJ_USB_TIMEOUT); + if (ret < 0) goto err_freelong; + if (actual < 0) { ret = -EREMOTEIO; goto err_freelong; } + + ret = 0; + *rbuf = longbuf; + } } - *rbufsize = bytes_read; + if (tmpbuf) kfree(tmpbuf); + return ret; - /*dev_dbg(dev, "all good! resp=%02x\n", respstat);*/ - return respstat; + +err_freelong: + if (longbuf) kfree(longbuf); + if (rbuf) *rbuf = NULL; +err_freetmp: + if (tmpbuf) kfree(tmpbuf); + return ret; } int dmj_transfer(struct platform_device *pdev, int cmd, int recvflags, - const void *wbuf, int wbufsize, void *rbuf, int *rbufsize) + const void *wbuf, int wbufsize, void **rbuf, int *rbufsize) { struct dmj_platform_data *dmj_pdata; struct dmj_dev *dmj; @@ -280,57 +303,66 @@ static int dmj_check_hw(struct dmj_dev *dmj) { struct device *dev = &dmj->interface->dev; - int ret; - __le16 protover; - int len = sizeof(protover); + int ret, len; + uint16_t protover; + uint8_t *buf = NULL; ret = dmj_xfer_internal(dmj, DMJ_CMD_CFG_GET_VERSION, - DMJ_XFER_FLAGS_PARSE_RESP, NULL, 0, &protover, &len); + DMJ_XFER_FLAGS_PARSE_RESP, NULL, 0, (void**)&buf, &len); ret = dmj_check_retval(ret, len, dev, "version check", true, sizeof(protover), sizeof(protover)); - if (ret < 0) return ret; - if (le16_to_cpu(protover) != DMJ_USB_CFG_PROTO_VER) { - dev_err(&dmj->interface->dev, HARDWARE_NAME " config protocol version 0x%04x too %s\n", - le16_to_cpu(protover), (le16_to_cpu(protover) > DMJ_USB_CFG_PROTO_VER) ? "new" : "old"); - return -ENODEV; - } + if (ret < 0 || !buf) goto out; - return 0; + protover = (uint16_t)buf[0] | ((uint16_t)buf[1] << 8); + + if (protover != DMJ_USB_CFG_PROTO_VER) { + dev_err(&dmj->interface->dev, HARDWARE_NAME " config protocol version 0x%04x too %s\n", + protover, (protover > DMJ_USB_CFG_PROTO_VER) ? "new" : "old"); + + ret = -ENODEV; + } else + ret = 0; + +out: + if (buf) kfree(buf); + return ret; } static int dmj_print_info(struct dmj_dev *dmj) { int ret, i, j, len; - __le16 modes, mversion; + uint16_t modes, mversion; uint8_t curmode, features; struct device *dev = &dmj->interface->dev; - char strinfo[65]; + uint8_t *buf; char modeinfo[16]; + char *strinfo; /* info string */ - len = sizeof(strinfo)-1; ret = dmj_xfer_internal(dmj, DMJ_CMD_CFG_GET_INFOSTR, - DMJ_XFER_FLAGS_PARSE_RESP, NULL, 0, strinfo, &len); + DMJ_XFER_FLAGS_PARSE_RESP, NULL, 0, (void**)&buf, &len); ret = dmj_check_retval(ret, len, dev, "get info", true, -1, sizeof(strinfo)); - if (ret < 0) return ret; - strinfo[len] = 0; /*strinfo[64] = 0;*/ - dev_info(dev, HARDWARE_NAME " '%s'\n", strinfo); + if (ret < 0 || !buf) goto out; + buf[len] = 0; + dev_info(dev, HARDWARE_NAME " '%s'\n", buf); + kfree(buf); buf = NULL; /* cur mode */ - len = sizeof(curmode); ret = dmj_xfer_internal(dmj, DMJ_CMD_CFG_GET_CUR_MODE, - DMJ_XFER_FLAGS_PARSE_RESP, NULL, 0, &curmode, &len); + DMJ_XFER_FLAGS_PARSE_RESP, NULL, 0, (void**)&buf, &len); ret = dmj_check_retval(ret, len, dev, "get info", true, sizeof(curmode), sizeof(curmode)); - if (ret < 0) return ret; - dmj->dmj_mode = curmode; + if (ret < 0 || !buf) goto out; + dmj->dmj_mode = curmode = buf[0]; + kfree(buf); buf = NULL; /* map of available modes */ - len = sizeof(modes); ret = dmj_xfer_internal(dmj, DMJ_CMD_CFG_GET_MODES, - DMJ_XFER_FLAGS_PARSE_RESP, NULL, 0, &modes, &len); + DMJ_XFER_FLAGS_PARSE_RESP, NULL, 0, (void**)&buf, &len); ret = dmj_check_retval(ret, len, dev, "get info", true, sizeof(modes), sizeof(modes)); - if (ret < 0) return ret; + if (ret < 0 || !buf) goto out; + modes = (uint16_t)buf[0] | ((uint16_t)buf[1] << 8); + kfree(buf); buf = NULL; for (i = 1; i < 16; ++i) { /* build the string, uglily */ - if (le16_to_cpu(modes) & (1<= sizeof(strinfo)) return -EMSGSIZE; - strinfo[len] = 0; /*strinfo[64] = 0;*/ + DMJ_XFER_FLAGS_PARSE_RESP, NULL, 0, (void**)&buf, &len); + ret = dmj_check_retval(ret, len, dev, "get info", true, -1, -1); + if (ret < 0 || !buf) goto out; + buf[len] = 0; + strinfo = buf; buf = NULL; /* version */ - len = sizeof(mversion); ret = dmj_xfer_internal(dmj, (i<<4) | DMJ_CMD_MODE_GET_VERSION, - DMJ_XFER_FLAGS_PARSE_RESP, NULL, 0, &mversion, &len); + DMJ_XFER_FLAGS_PARSE_RESP, NULL, 0, (void**)&buf, &len); ret = dmj_check_retval(ret, len, dev, "get info", true, sizeof(mversion), sizeof(mversion)); - if (ret < 0) return ret; + if (ret < 0 || !buf) { kfree(strinfo); goto out; } + mversion = (uint16_t)buf[0] | ((uint16_t)buf[1] << 8); + kfree(buf); buf = NULL; /* features */ - len = sizeof(features); ret = dmj_xfer_internal(dmj, (i<<4) | DMJ_CMD_MODE_GET_FEATURES, - DMJ_XFER_FLAGS_PARSE_RESP, NULL, 0, &features, &len); + DMJ_XFER_FLAGS_PARSE_RESP, NULL, 0, (void**)&buf, &len); ret = dmj_check_retval(ret, len, dev, "get info", true, sizeof(features), sizeof(features)); - if (ret < 0) return ret; + if (ret < 0 || !buf) { kfree(strinfo); goto out; } + features = (uint16_t)buf[0] | ((uint16_t)buf[1] << 8); + kfree(buf); buf = NULL; if (i == 1) dmj->dmj_m1feature = features; @@ -375,9 +408,14 @@ static int dmj_print_info(struct dmj_dev *dmj) dev_dbg(dev, "Mode %d: '%s' version 0x%04x, features: %s\n", i, strinfo, mversion, modeinfo); + kfree(strinfo); } return 0; + +out: + if (buf) kfree(buf); + return ret; } static int dmj_hw_init(struct dmj_dev *dmj) { @@ -461,8 +499,6 @@ static int dmj_probe(struct usb_interface *itf, const struct usb_device_id *usb_ if (ret) { dev_err(dev, "failed to add MFD I2C devices\n"); goto out_free; - } else { - dev_warn(dev, "added i2c mfd\n"); } } if (dmj->dmj_m1feature & DMJ_FEATURE_MODE1_TEMPSENSOR) { diff --git a/host/modules/dmj.h b/host/modules/dmj.h index e229de1..8d00fe2 100644 --- a/host/modules/dmj.h +++ b/host/modules/dmj.h @@ -71,11 +71,13 @@ inline static int dmj_check_retval(int ret, int len, struct device *dev, return 0; } +/* TODO: split up in "raw" read & writes, and higher-level ones with cmd and + * repstat, because the way this is currently overloaded, is, bad */ int dmj_transfer(struct platform_device *pdev, int cmd, int recvflags, - const void *wbuf, int wbufsize, void *rbuf, int *rbufsize); + const void *wbuf, int wbufsize, void **rbuf, int *rbufsize); inline static int dmj_read(struct platform_device *pdev, int recvflags, - void *rbuf, int *rbufsize) + void **rbuf, int *rbufsize) { return dmj_transfer(pdev, -1, recvflags, NULL, 0, rbuf, rbufsize); } diff --git a/host/modules/i2c-dmj.c b/host/modules/i2c-dmj.c index 3979c46..7f9e90e 100644 --- a/host/modules/i2c-dmj.c +++ b/host/modules/i2c-dmj.c @@ -54,14 +54,10 @@ struct dmj_i2c { static int dmj_i2c_read(struct dmj_i2c *dmji, struct i2c_msg *msg, int cmd) { struct device *dev = &dmji->pdev->dev; - uint8_t *respbuf; + void *respbuf = NULL; int ret, len; uint8_t cmdbuf[1+2+2+2]; /* cmd, flags, addr, len */ - len = msg->len; - respbuf = kzalloc(len, GFP_KERNEL); /* 0 length: returns status byte */ - if (!respbuf) return -ENOMEM; - cmdbuf[0] = cmd; cmdbuf[1] = (msg->flags >> 0) & 0xff; cmdbuf[2] = (msg->flags >> 8) & 0xff; @@ -71,16 +67,15 @@ static int dmj_i2c_read(struct dmj_i2c *dmji, struct i2c_msg *msg, int cmd) cmdbuf[6] = (msg->len >> 8) & 0xff; ret = dmj_transfer(dmji->pdev, DMJ_CMD_MODE1_I2C, DMJ_XFER_FLAGS_PARSE_RESP, - cmdbuf, sizeof(cmdbuf), respbuf, &len); - ret = dmj_check_retval(ret, len, dev, "i2c read", true, -1, -1); - if (ret < 0) goto err_free; + cmdbuf, sizeof(cmdbuf), &respbuf, &len); + ret = dmj_check_retval(ret, len, dev, "i2c read", true, -1, msg->len); + if (ret < 0 || !respbuf) goto err_free; memcpy(msg->buf, respbuf, msg->len); - kfree(respbuf); - return len; + ret = len; err_free: - kfree(respbuf); + if (respbuf) kfree(respbuf); return ret; } static int dmj_i2c_write(struct dmj_i2c *dmji, struct i2c_msg *msg, int cmd) @@ -89,8 +84,8 @@ static int dmj_i2c_write(struct dmj_i2c *dmji, struct i2c_msg *msg, int cmd) uint8_t *cmdbuf; int ret, len; - len = msg->len + 1+2+2+2; - cmdbuf = kzalloc(len, GFP_KERNEL); /* cmd, flags, addr, len */ + len = msg->len + 1+2+2+2; /* cmd, flags, addr, len */ + cmdbuf = kzalloc(len, GFP_KERNEL); if (!cmdbuf) return -ENOMEM; cmdbuf[0] = cmd; @@ -106,8 +101,7 @@ static int dmj_i2c_write(struct dmj_i2c *dmji, struct i2c_msg *msg, int cmd) ret = dmj_check_retval(ret, len, dev, "i2c write", true, -1, -1); if (ret < 0) goto err_free; - kfree(cmdbuf); - return msg->len; + ret = msg->len; err_free: kfree(cmdbuf); @@ -120,7 +114,7 @@ static int dmj_i2c_xfer(struct i2c_adapter *a, struct i2c_msg *msgs, int nmsg) struct device *dev = &dmji->pdev->dev; struct i2c_msg *pmsg; int i, ret, cmd, stlen; - uint8_t status, i2ccmd; + uint8_t *status = NULL, i2ccmd; for (i = 0; i < nmsg; ++i) { cmd = DMJ_I2C_CMD_DO_XFER; @@ -154,14 +148,13 @@ static int dmj_i2c_xfer(struct i2c_adapter *a, struct i2c_msg *msgs, int nmsg) /* read status */ i2ccmd = DMJ_I2C_CMD_GET_STATUS; - stlen = sizeof(status); ret = dmj_transfer(dmji->pdev, DMJ_CMD_MODE1_I2C, DMJ_XFER_FLAGS_PARSE_RESP, - &i2ccmd, sizeof(i2ccmd), &status, &stlen); + &i2ccmd, sizeof(i2ccmd), (void**)&status, &stlen); ret = dmj_check_retval(ret, stlen, dev, "i2c stat", true, sizeof(status), sizeof(status)); - if (ret < 0) goto err_ret; + if (ret < 0 || !status) goto err_ret; - dev_warn(dev, " status = %d\n", status); - if (status == DMJ_I2C_STAT_NAK) { + dev_warn(dev, " status = %d\n", *status); + if (*status == DMJ_I2C_STAT_NAK) { ret = -ENXIO; goto err_ret; } @@ -170,6 +163,7 @@ static int dmj_i2c_xfer(struct i2c_adapter *a, struct i2c_msg *msgs, int nmsg) ret = i; err_ret: + if (status) kfree(status); return ret; } static uint32_t dmj_i2c_func(struct i2c_adapter *a) @@ -177,22 +171,27 @@ static uint32_t dmj_i2c_func(struct i2c_adapter *a) struct dmj_i2c *dmji = i2c_get_adapdata(a); struct device *dev = &dmji->pdev->dev; - __le32 func = 0; - int len = sizeof(func), ret; + uint32_t func = 0; + int len, ret; uint8_t i2ccmd = DMJ_I2C_CMD_GET_FUNC; + uint8_t *fbuf = NULL; ret = dmj_transfer(dmji->pdev, DMJ_CMD_MODE1_I2C, DMJ_XFER_FLAGS_PARSE_RESP, - &i2ccmd, sizeof(i2ccmd), &func, &len); + &i2ccmd, sizeof(i2ccmd), (void**)&fbuf, &len); ret = dmj_check_retval(ret, len, dev, "i2c get_func", true, sizeof(func), sizeof(func)); - if (ret < 0) return 0; + if (ret < 0 || !fbuf) return 0; - dev_warn(dev, "I2C functionality: 0x%08x\n", le32_to_cpu(func)); + func = (uint32_t)fbuf[0] | ((uint32_t)fbuf[1] << 8) + | ((uint32_t)fbuf[2] << 16) | ((uint32_t)fbuf[3] << 24); - return le32_to_cpu(func); + dev_warn(dev, "I2C functionality: 0x%08x\n", func); + + kfree(fbuf); + return func; } static const struct i2c_algorithm dmj_i2c_algo = { - .master_xfer = dmj_i2c_xfer, + .master_xfer = dmj_i2c_xfer, .functionality = dmj_i2c_func }; static const struct i2c_adapter_quirks dmj_i2c_quirks = { @@ -210,57 +209,87 @@ static int dmj_i2c_check_hw(struct platform_device *pdev) */ struct device *dev = &pdev->dev; - __le16 m1ver; + uint16_t m1ver; uint8_t curmode, m1feat, echoval; const int ver_min = 0x0010, ver_max = 0x0010; uint8_t i2ccmd[2]; int ret = 0, len; + uint8_t *buf = NULL; - len = sizeof(curmode); ret = dmj_transfer(pdev, DMJ_CMD_CFG_GET_CUR_MODE, - DMJ_XFER_FLAGS_PARSE_RESP, NULL, 0, &curmode, &len); + DMJ_XFER_FLAGS_PARSE_RESP, NULL, 0, (void**)&buf, &len); ret = dmj_check_retval(ret, len, dev, "i2c test 1", true, sizeof(curmode), sizeof(curmode)); - if (ret < 0) return ret; + if (ret < 0 || !buf) goto out; + + dev_warn(dev, "check hw 1\n"); + + curmode = buf[0]; + kfree(buf); buf = NULL; if (curmode != 0x1) { dev_err(dev, "device must be in mode 1 for ICD to work, but it is in mode %d\n", curmode); - return -EIO; + ret = -EIO; + goto out; } - len = sizeof(m1ver); + dev_warn(dev, "check hw 2\n"); + ret = dmj_transfer(pdev, (1<<4) | DMJ_CMD_MODE_GET_VERSION, - DMJ_XFER_FLAGS_PARSE_RESP, NULL, 0, &m1ver, &len); + DMJ_XFER_FLAGS_PARSE_RESP, NULL, 0, (void**)&buf, &len); ret = dmj_check_retval(ret, len, dev, "i2c test 2", true, sizeof(m1ver), sizeof(m1ver)); - if (ret < 0) return ret; - if (le16_to_cpu(m1ver) > ver_max || le16_to_cpu(m1ver) < ver_min) { + if (ret < 0 || !buf) goto out; + + dev_warn(dev, "check hw 3\n"); + + m1ver = (uint16_t)buf[0] | ((uint16_t)buf[1] << 8); + kfree(buf); buf = NULL; + if (m1ver > ver_max || m1ver < ver_min) { dev_err(dev, "bad mode 1 version %04x on device, must be between %04x and %04x\n", - le16_to_cpu(m1ver), ver_min, ver_max); - return -EIO; + m1ver, ver_min, ver_max); + ret = -EIO; + goto out; } - len = sizeof(m1feat); + dev_warn(dev, "check hw 4\n"); + ret = dmj_transfer(pdev, (1<<4) | DMJ_CMD_MODE_GET_FEATURES, - DMJ_XFER_FLAGS_PARSE_RESP, NULL, 0, &m1feat, &len); + DMJ_XFER_FLAGS_PARSE_RESP, NULL, 0, (void**)&buf, &len); ret = dmj_check_retval(ret, len, dev, "i2c test 3", true, sizeof(m1feat), sizeof(m1feat)); - if (ret < 0) return ret; + if (ret < 0 || !buf) goto out; + m1feat = buf[0]; + kfree(buf); buf = NULL; if (!(m1feat & DMJ_FEATURE_MODE1_I2C)) { dev_err(dev, "device's mode 1 does not support I2C\n"); - return -EIO; + ret = -EIO; + goto out; } + dev_warn(dev, "check hw 5\n"); + echoval = 0x42; i2ccmd[0] = DMJ_I2C_CMD_ECHO; i2ccmd[1] = ~echoval; - len = sizeof(echoval); ret = dmj_transfer(pdev, (1<<4) | DMJ_CMD_MODE1_I2C, - DMJ_XFER_FLAGS_PARSE_RESP, i2ccmd, sizeof(i2ccmd), &m1feat, &len); - ret = dmj_check_retval(ret, len, dev, "i2c test", true, sizeof(m1feat), sizeof(m1feat)); - if (ret < 0) return ret; + DMJ_XFER_FLAGS_PARSE_RESP, i2ccmd, sizeof(i2ccmd), (void**)&buf, &len); + ret = dmj_check_retval(ret, len, dev, "i2c test", true, sizeof(echoval), sizeof(echoval)); + if (ret < 0 || !buf) goto out; + + echoval = buf[0]; + kfree(buf); buf = NULL; if (echoval != i2ccmd[1]) { dev_err(dev, "I2C echo test command not functional\n"); - return -EIO; + ret = -EIO; + goto out; } - return 0; + dev_warn(dev, "check hw 6\n"); + + ret = 0; + +out: + dev_warn(dev, "check hw 7\n"); + + if (buf) kfree(buf); + return ret; } static int dmj_i2c_set_delay(struct platform_device *pdev, uint16_t us) @@ -269,14 +298,18 @@ static int dmj_i2c_set_delay(struct platform_device *pdev, uint16_t us) uint8_t i2ccmd[3]; int ret = 0; + dev_warn(dev, "set delay 1\n"); + i2ccmd[0] = DMJ_I2C_CMD_SET_DELAY; i2ccmd[1] = (us >> 0) & 0xff; i2ccmd[2] = (us >> 8) & 0xff; - ret = dmj_transfer(pdev, (1<<4) | DMJ_CMD_MODE1_I2C, DMJ_XFER_FLAGS_PARSE_RESP, - i2ccmd, sizeof(i2ccmd), NULL, NULL); + ret = dmj_write(pdev, (1<<4) | DMJ_CMD_MODE1_I2C, i2ccmd, sizeof(i2ccmd)); + dev_dbg(dev, "set delay to %hu us, result %d\n", us, ret); ret = dmj_check_retval(ret, -1, dev, "i2c set delay", true, -1, -1); + dev_warn(dev, "set delay 2\n"); + return ret; } @@ -302,18 +335,27 @@ static int dmj_i2c_probe(struct platform_device *pdev) dmji->pdev = pdev; + dev_warn(dev, "probe 2\n"); + dmji->adapter.owner = THIS_MODULE; dmji->adapter.class = I2C_CLASS_HWMON; dmji->adapter.algo = &dmj_i2c_algo; dmji->adapter.quirks = &dmj_i2c_quirks; /* TODO: is this needed? probably... */ dmji->adapter.dev.of_node = dev->of_node; + dev_warn(dev, "probe 3\n"); i2c_set_adapdata(&dmji->adapter, dmji); + dev_warn(dev, "probe 4\n"); + snprintf(dmji->adapter.name, sizeof(dmji->adapter.name), "%s-%s", - "dln2-i2c", dev_name(pdev->dev.parent)); + "dmj-i2c", dev_name(pdev->dev.parent)); + + dev_warn(dev, "probe 5\n"); platform_set_drvdata(pdev, dmji); + dev_warn(dev, "probe 6\n"); + return i2c_add_adapter(&dmji->adapter); } static int dmj_i2c_remove(struct platform_device *pdev)