+static int mxt_prepare_cfg_mem(struct mxt_data *data,
+ const struct firmware *cfg,
+ unsigned int data_pos,
+ unsigned int cfg_start_ofs,
+ u8 *config_mem,
+ size_t config_mem_size)
+{
+ struct device *dev = &data->client->dev;
+ struct mxt_object *object;
+ unsigned int type, instance, size, byte_offset;
+ int offset;
+ int ret;
+ int i;
+ u16 reg;
+ u8 val;
+
+ while (data_pos < cfg->size) {
+ /* Read type, instance, length */
+ ret = sscanf(cfg->data + data_pos, "%x %x %x%n",
+ &type, &instance, &size, &offset);
+ if (ret == 0) {
+ /* EOF */
+ break;
+ } else if (ret != 3) {
+ dev_err(dev, "Bad format: failed to parse object\n");
+ return -EINVAL;
+ }
+ data_pos += offset;
+
+ object = mxt_get_object(data, type);
+ if (!object) {
+ /* Skip object */
+ for (i = 0; i < size; i++) {
+ ret = sscanf(cfg->data + data_pos, "%hhx%n",
+ &val,
+ &offset);
+ data_pos += offset;
+ }
+ continue;
+ }
+
+ if (size > mxt_obj_size(object)) {
+ /*
+ * Either we are in fallback mode due to wrong
+ * config or config from a later fw version,
+ * or the file is corrupt or hand-edited.
+ */
+ dev_warn(dev, "Discarding %zu byte(s) in T%u\n",
+ size - mxt_obj_size(object), type);
+ } else if (mxt_obj_size(object) > size) {
+ /*
+ * If firmware is upgraded, new bytes may be added to
+ * end of objects. It is generally forward compatible
+ * to zero these bytes - previous behaviour will be
+ * retained. However this does invalidate the CRC and
+ * will force fallback mode until the configuration is
+ * updated. We warn here but do nothing else - the
+ * malloc has zeroed the entire configuration.
+ */
+ dev_warn(dev, "Zeroing %zu byte(s) in T%d\n",
+ mxt_obj_size(object) - size, type);
+ }
+
+ if (instance >= mxt_obj_instances(object)) {
+ dev_err(dev, "Object instances exceeded!\n");
+ return -EINVAL;
+ }
+
+ reg = object->start_address + mxt_obj_size(object) * instance;
+
+ for (i = 0; i < size; i++) {
+ ret = sscanf(cfg->data + data_pos, "%hhx%n",
+ &val,
+ &offset);
+ if (ret != 1) {
+ dev_err(dev, "Bad format in T%d\n", type);
+ return -EINVAL;
+ }
+ data_pos += offset;
+
+ if (i > mxt_obj_size(object))
+ continue;
+
+ byte_offset = reg + i - cfg_start_ofs;
+
+ if (byte_offset >= 0 &&
+ byte_offset <= config_mem_size) {
+ *(config_mem + byte_offset) = val;
+ } else {
+ dev_err(dev, "Bad object: reg:%d, T%d, ofs=%d\n",
+ reg, object->type, byte_offset);
+ return -EINVAL;
+ }
+ }
+ }
+
+ return 0;
+}
+
+static int mxt_upload_cfg_mem(struct mxt_data *data, unsigned int cfg_start,
+ u8 *config_mem, size_t config_mem_size)
+{
+ unsigned int byte_offset = 0;
+ int error;
+
+ /* Write configuration as blocks */
+ while (byte_offset < config_mem_size) {
+ unsigned int size = config_mem_size - byte_offset;
+
+ if (size > MXT_MAX_BLOCK_WRITE)
+ size = MXT_MAX_BLOCK_WRITE;
+
+ error = __mxt_write_reg(data->client,
+ cfg_start + byte_offset,
+ size, config_mem + byte_offset);
+ if (error) {
+ dev_err(&data->client->dev,
+ "Config write error, ret=%d\n", error);
+ return error;
+ }
+
+ byte_offset += size;
+ }
+
+ return 0;
+}
+