diff --git a/platforms/common/include/px4_platform_common/px4_mtd.h b/platforms/common/include/px4_platform_common/px4_mtd.h index 4e9722cfa2..7313bd78a9 100644 --- a/platforms/common/include/px4_platform_common/px4_mtd.h +++ b/platforms/common/include/px4_platform_common/px4_mtd.h @@ -35,6 +35,8 @@ __BEGIN_DECLS +#define MAX_MTD_INSTANCES 5u + // The data needed to interface with mtd device's typedef struct { @@ -60,7 +62,7 @@ typedef struct { * This can be Null if there are no mtd instances. * */ -__EXPORT mtd_instance_s *px4_mtd_get_instances(unsigned int *count); +__EXPORT mtd_instance_s **px4_mtd_get_instances(unsigned int *count); /* Get device complete geometry or a device @@ -78,5 +80,6 @@ __EXPORT ssize_t px4_mtd_get_partition_size(const mtd_instance_s *instance, cons int px4_at24c_initialize(FAR struct i2c_master_s *dev, uint8_t address, FAR struct mtd_dev_s **mtd_dev); +void px4_at24c_deinitialize(void); __END_DECLS diff --git a/platforms/nuttx/src/px4/common/px4_mtd.cpp b/platforms/nuttx/src/px4/common/px4_mtd.cpp index bb5ac75456..bffa56900b 100644 --- a/platforms/nuttx/src/px4/common/px4_mtd.cpp +++ b/platforms/nuttx/src/px4/common/px4_mtd.cpp @@ -64,7 +64,8 @@ extern "C" { off_t firstblock, off_t nblocks); } static int num_instances = 0; -static mtd_instance_s *instances = nullptr; +static int total_blocks = 0; +static mtd_instance_s *instances[MAX_MTD_INSTANCES] = {}; static int ramtron_attach(mtd_instance_s &instance) @@ -234,7 +235,7 @@ ssize_t px4_mtd_get_partition_size(const mtd_instance_s *instance, const char *p return instance->partition_block_counts[partn] * blocksize; } -mtd_instance_s *px4_mtd_get_instances(unsigned int *count) +mtd_instance_s **px4_mtd_get_instances(unsigned int *count) { *count = num_instances; return instances; @@ -296,62 +297,69 @@ int px4_mtd_config(const px4_mtd_manifest_t *mft_mtd) } rv = -ENOMEM; - int total_blocks = 0; + uint8_t total_new_instances = mtd_list->nconfigs + num_instances; - instances = new mtd_instance_s[mtd_list->nconfigs]; - - if (instances == nullptr) { -memoryout: - PX4_ERR("failed to allocate memory!"); + if (total_new_instances >= MAX_MTD_INSTANCES) { + PX4_ERR("reached limit of max %u mtd instances", MAX_MTD_INSTANCES); return rv; } - for (uint32_t i = 0; i < mtd_list->nconfigs; i++) { + for (uint8_t i = num_instances, num_entry = 0u; i < total_new_instances; ++i, ++num_entry) { + + instances[i] = new mtd_instance_s; + + if (instances == nullptr) { +memoryout: + PX4_ERR("failed to allocate memory!"); + return rv; + } + num_instances++; - uint32_t nparts = mtd_list->entries[i]->npart; - instances[i].devid = mtd_list->entries[i]->device->devid; - instances[i].mtd_dev = nullptr; - instances[i].n_partitions_current = 0; + + uint32_t nparts = mtd_list->entries[num_entry]->npart; + instances[i]->devid = mtd_list->entries[num_entry]->device->devid; + instances[i]->mtd_dev = nullptr; + instances[i]->n_partitions_current = 0; rv = -ENOMEM; - instances[i].part_dev = new FAR struct mtd_dev_s *[nparts]; + instances[i]->part_dev = new FAR struct mtd_dev_s *[nparts]; - if (instances[i].part_dev == nullptr) { + if (instances[i]->part_dev == nullptr) { goto memoryout; } - instances[i].partition_block_counts = new int[nparts]; + instances[i]->partition_block_counts = new int[nparts]; - if (instances[i].partition_block_counts == nullptr) { + if (instances[i]->partition_block_counts == nullptr) { goto memoryout; } - instances[i].partition_types = new int[nparts]; + instances[i]->partition_types = new int[nparts]; - if (instances[i].partition_types == nullptr) { + if (instances[i]->partition_types == nullptr) { goto memoryout; } - instances[i].partition_names = new const char *[nparts]; + instances[i]->partition_names = new const char *[nparts]; - if (instances[i].partition_names == nullptr) { + if (instances[i]->partition_names == nullptr) { goto memoryout; } for (uint32_t p = 0; p < nparts; p++) { - instances[i].partition_block_counts[p] = mtd_list->entries[i]->partd[p].nblocks; - instances[i].partition_names[p] = mtd_list->entries[i]->partd[p].path; - instances[i].partition_types[p] = mtd_list->entries[i]->partd[p].type; + instances[i]->partition_block_counts[p] = mtd_list->entries[num_entry]->partd[p].nblocks; + instances[i]->partition_names[p] = mtd_list->entries[num_entry]->partd[p].path; + instances[i]->partition_types[p] = mtd_list->entries[num_entry]->partd[p].type; } - if (mtd_list->entries[i]->device->bus_type == px4_mft_device_t::I2C) { - rv = at24xxx_attach(instances[i]); + if (mtd_list->entries[num_entry]->device->bus_type == px4_mft_device_t::I2C) { + rv = at24xxx_attach(*instances[i]); - } else if (mtd_list->entries[i]->device->bus_type == px4_mft_device_t::SPI) { - rv = ramtron_attach(instances[i]); + } else if (mtd_list->entries[num_entry]->device->bus_type == px4_mft_device_t::SPI) { + rv = ramtron_attach(*instances[i]); - } else if (mtd_list->entries[i]->device->bus_type == px4_mft_device_t::ONCHIP) { - instances[i].n_partitions_current++; + } else if (mtd_list->entries[num_entry]->device->bus_type == px4_mft_device_t::ONCHIP) { + instances[i]->n_partitions_current++; return 0; } @@ -366,7 +374,7 @@ memoryout: unsigned int nblocks; unsigned int partsize; - rv = px4_mtd_get_geometry(&instances[i], &blocksize, &erasesize, &neraseblocks, &blkpererase, &nblocks, &partsize); + rv = px4_mtd_get_geometry(instances[i], &blocksize, &erasesize, &neraseblocks, &blkpererase, &nblocks, &partsize); if (rv != 0) { goto errout; @@ -379,13 +387,13 @@ memoryout: unsigned long offset; unsigned part; - for (offset = 0, part = 0; rv == 0 && part < nparts; offset += instances[i].partition_block_counts[part], part++) { + for (offset = 0, part = 0; rv == 0 && part < nparts; offset += instances[i]->partition_block_counts[part], part++) { /* Create the partition */ - instances[i].part_dev[part] = mtd_partition(instances[i].mtd_dev, offset, instances[i].partition_block_counts[part]); + instances[i]->part_dev[part] = mtd_partition(instances[i]->mtd_dev, offset, instances[i]->partition_block_counts[part]); - if (instances[i].part_dev[part] == nullptr) { + if (instances[i]->part_dev[part] == nullptr) { PX4_ERR("mtd_partition failed. offset=%lu nblocks=%u", offset, nblocks); rv = -ENOSPC; @@ -396,7 +404,7 @@ memoryout: snprintf(blockname, sizeof(blockname), "/dev/mtdblock%d", total_blocks); - rv = ftl_initialize(total_blocks, instances[i].part_dev[part]); + rv = ftl_initialize(total_blocks, instances[i]->part_dev[part]); if (rv < 0) { PX4_ERR("ftl_initialize %s failed: %d", blockname, rv); @@ -407,14 +415,14 @@ memoryout: /* Now create a character device on the block device */ - rv = bchdev_register(blockname, instances[i].partition_names[part], false); + rv = bchdev_register(blockname, instances[i]->partition_names[part], false); if (rv < 0) { - PX4_ERR("bchdev_register %s failed: %d", instances[i].partition_names[part], rv); + PX4_ERR("bchdev_register %s failed: %d", instances[i]->partition_names[part], rv); goto errout; } - instances[i].n_partitions_current++; + instances[i]->n_partitions_current++; } errout: @@ -422,9 +430,9 @@ errout: if (rv < 0) { PX4_ERR("mtd failure: %d bus %" PRId32 " address %" PRId32 " class %d", rv, - PX4_I2C_DEVID_BUS(instances[i].devid), - PX4_I2C_DEVID_ADDR(instances[i].devid), - mtd_list->entries[i]->partd[instances[i].n_partitions_current].type); + PX4_I2C_DEVID_BUS(instances[i]->devid), + PX4_I2C_DEVID_ADDR(instances[i]->devid), + mtd_list->entries[num_entry]->partd[instances[i]->n_partitions_current].type); break; } } @@ -456,14 +464,14 @@ __EXPORT int px4_mtd_query(const char *sub, const char *val, const char **get) rv = -ENOENT; for (int i = 0; i < num_instances; i++) { - for (unsigned n = 0; n < instances[i].n_partitions_current; n++) { - if (instances[i].partition_types[n] == key) { + for (unsigned n = 0; n < instances[i]->n_partitions_current; n++) { + if (instances[i]->partition_types[n] == key) { if (get != nullptr && val == nullptr) { - *get = instances[i].partition_names[n]; + *get = instances[i]->partition_names[n]; return 0; } - if (val != nullptr && strcmp(instances[i].partition_names[n], val) == 0) { + if (val != nullptr && strcmp(instances[i]->partition_names[n], val) == 0) { return 0; } } diff --git a/src/systemcmds/mtd/mtd.cpp b/src/systemcmds/mtd/mtd.cpp index 609c7ada29..1aee24b958 100644 --- a/src/systemcmds/mtd/mtd.cpp +++ b/src/systemcmds/mtd/mtd.cpp @@ -80,11 +80,11 @@ static int mtd_status(void) bool running = false; unsigned int num_instances; - const mtd_instance_s *instances = px4_mtd_get_instances(&num_instances); + mtd_instance_s **instances = px4_mtd_get_instances(&num_instances); if (instances) { for (unsigned int i = 0; i < num_instances; ++i) { - if (instances[i].mtd_dev) { + if (instances[i]->mtd_dev) { unsigned long blocksize; unsigned long erasesize; @@ -93,7 +93,7 @@ static int mtd_status(void) unsigned int nblocks; unsigned int partsize; - ret = px4_mtd_get_geometry(&instances[i], &blocksize, &erasesize, &neraseblocks, &blkpererase, &nblocks, &partsize); + ret = px4_mtd_get_geometry(instances[i], &blocksize, &erasesize, &neraseblocks, &blkpererase, &nblocks, &partsize); if (ret == 0) { @@ -102,17 +102,17 @@ static int mtd_status(void) printf(" blocksize: %lu\n", blocksize); printf(" erasesize: %lu\n", erasesize); printf(" neraseblocks: %lu\n", neraseblocks); - printf(" No. partitions: %u\n", instances[i].n_partitions_current); + printf(" No. partitions: %u\n", instances[i]->n_partitions_current); unsigned int totalnblocks = 0; unsigned int totalpartsize = 0; - for (unsigned int p = 0; p < instances[i].n_partitions_current; p++) { + for (unsigned int p = 0; p < instances[i]->n_partitions_current; p++) { FAR struct mtd_geometry_s geo; - ret = instances[i].part_dev[p]->ioctl(instances[i].part_dev[p], MTDIOC_GEOMETRY, (unsigned long)((uintptr_t)&geo)); + ret = instances[i]->part_dev[p]->ioctl(instances[i]->part_dev[p], MTDIOC_GEOMETRY, (unsigned long)((uintptr_t)&geo)); printf(" partition: %u:\n", p); - printf(" name: %s\n", instances[i].partition_names[p]); + printf(" name: %s\n", instances[i]->partition_names[p]); printf(" blocks: %" PRIu32 " (%lu bytes)\n", geo.neraseblocks, erasesize * geo.neraseblocks); totalnblocks += geo.neraseblocks; totalpartsize += erasesize * geo.neraseblocks; @@ -323,7 +323,7 @@ int mtd_main(int argc, char *argv[]) if (myoptind < argc) { unsigned int num_instances; - mtd_instance_s *instances = px4_mtd_get_instances(&num_instances); + mtd_instance_s **instances = px4_mtd_get_instances(&num_instances); if (instances == nullptr) { PX4_ERR("Driver not running"); @@ -338,11 +338,11 @@ int mtd_main(int argc, char *argv[]) #if !defined(CONSTRAINED_FLASH) if (!strcmp(argv[myoptind], "readtest")) { - return mtd_readtest(instances[instance]); + return mtd_readtest(*instances[instance]); } if (!strcmp(argv[myoptind], "rwtest")) { - return mtd_rwtest(instances[instance]); + return mtd_rwtest(*instances[instance]); } #endif @@ -352,7 +352,7 @@ int mtd_main(int argc, char *argv[]) } if (!strcmp(argv[myoptind], "erase")) { - return mtd_erase(instances[instance]); + return mtd_erase(*instances[instance]); } }