zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

px4_mtd: fix instantiation for multiple mtds

main
Igor Mišić 3 years ago committed by Beat Küng
parent
08a9e49f3e
commit
432b664acc
3 changed files with 68 additions and 57 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 5
      platforms/common/include/px4_platform_common/px4_mtd.h
  2. 98
      platforms/nuttx/src/px4/common/px4_mtd.cpp
  3. 22
      src/systemcmds/mtd/mtd.cpp

5
platforms/common/include/px4_platform_common/px4_mtd.h
Unescape View File

@ -35,6 +35,8 @@
__BEGIN_DECLS __BEGIN_DECLS
#define MAX_MTD_INSTANCES 5u
// The data needed to interface with mtd device's // The data needed to interface with mtd device's
typedef struct { typedef struct {
@ -60,7 +62,7 @@ typedef struct {
* This can be Null if there are no mtd instances. * 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 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, int px4_at24c_initialize(FAR struct i2c_master_s *dev,
uint8_t address, FAR struct mtd_dev_s **mtd_dev); uint8_t address, FAR struct mtd_dev_s **mtd_dev);
void px4_at24c_deinitialize(void);
__END_DECLS __END_DECLS

98
platforms/nuttx/src/px4/common/px4_mtd.cpp
Unescape View File

@ -64,7 +64,8 @@ extern "C" {
off_t firstblock, off_t nblocks); off_t firstblock, off_t nblocks);
} }
static int num_instances = 0; 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) 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; 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; *count = num_instances;
return instances; return instances;
@ -296,62 +297,69 @@ int px4_mtd_config(const px4_mtd_manifest_t *mft_mtd)
} }
rv = -ENOMEM; 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 (total_new_instances >= MAX_MTD_INSTANCES) {
PX4_ERR("reached limit of max %u mtd instances", MAX_MTD_INSTANCES);
if (instances == nullptr) {
memoryout:
PX4_ERR("failed to allocate memory!");
return rv; 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++; num_instances++;
uint32_t nparts = mtd_list->entries[i]->npart;
instances[i].devid = mtd_list->entries[i]->device->devid; uint32_t nparts = mtd_list->entries[num_entry]->npart;
instances[i].mtd_dev = nullptr; instances[i]->devid = mtd_list->entries[num_entry]->device->devid;
instances[i].n_partitions_current = 0; instances[i]->mtd_dev = nullptr;
instances[i]->n_partitions_current = 0;
rv = -ENOMEM; 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; 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; 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; 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; goto memoryout;
} }
for (uint32_t p = 0; p < nparts; p++) { for (uint32_t p = 0; p < nparts; p++) {
instances[i].partition_block_counts[p] = mtd_list->entries[i]->partd[p].nblocks; instances[i]->partition_block_counts[p] = mtd_list->entries[num_entry]->partd[p].nblocks;
instances[i].partition_names[p] = mtd_list->entries[i]->partd[p].path; instances[i]->partition_names[p] = mtd_list->entries[num_entry]->partd[p].path;
instances[i].partition_types[p] = mtd_list->entries[i]->partd[p].type; 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) { if (mtd_list->entries[num_entry]->device->bus_type == px4_mft_device_t::I2C) {
rv = at24xxx_attach(instances[i]); rv = at24xxx_attach(*instances[i]);
} else if (mtd_list->entries[i]->device->bus_type == px4_mft_device_t::SPI) { } else if (mtd_list->entries[num_entry]->device->bus_type == px4_mft_device_t::SPI) {
rv = ramtron_attach(instances[i]); rv = ramtron_attach(*instances[i]);
} else if (mtd_list->entries[i]->device->bus_type == px4_mft_device_t::ONCHIP) { } else if (mtd_list->entries[num_entry]->device->bus_type == px4_mft_device_t::ONCHIP) {
instances[i].n_partitions_current++; instances[i]->n_partitions_current++;
return 0; return 0;
} }
@ -366,7 +374,7 @@ memoryout:
unsigned int nblocks; unsigned int nblocks;
unsigned int partsize; 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) { if (rv != 0) {
goto errout; goto errout;
@ -379,13 +387,13 @@ memoryout:
unsigned long offset; unsigned long offset;
unsigned part; 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 */ /* 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", PX4_ERR("mtd_partition failed. offset=%lu nblocks=%u",
offset, nblocks); offset, nblocks);
rv = -ENOSPC; rv = -ENOSPC;
@ -396,7 +404,7 @@ memoryout:
snprintf(blockname, sizeof(blockname), "/dev/mtdblock%d", total_blocks); 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) { if (rv < 0) {
PX4_ERR("ftl_initialize %s failed: %d", blockname, rv); PX4_ERR("ftl_initialize %s failed: %d", blockname, rv);
@ -407,14 +415,14 @@ memoryout:
/* Now create a character device on the block device */ /* 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) { 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; goto errout;
} }
instances[i].n_partitions_current++; instances[i]->n_partitions_current++;
} }
errout: errout:
@ -422,9 +430,9 @@ errout:
if (rv < 0) { if (rv < 0) {
PX4_ERR("mtd failure: %d bus %" PRId32 " address %" PRId32 " class %d", PX4_ERR("mtd failure: %d bus %" PRId32 " address %" PRId32 " class %d",
rv, rv,
PX4_I2C_DEVID_BUS(instances[i].devid), PX4_I2C_DEVID_BUS(instances[i]->devid),
PX4_I2C_DEVID_ADDR(instances[i].devid), PX4_I2C_DEVID_ADDR(instances[i]->devid),
mtd_list->entries[i]->partd[instances[i].n_partitions_current].type); mtd_list->entries[num_entry]->partd[instances[i]->n_partitions_current].type);
break; break;
} }
} }
@ -456,14 +464,14 @@ __EXPORT int px4_mtd_query(const char *sub, const char *val, const char **get)
rv = -ENOENT; rv = -ENOENT;
for (int i = 0; i < num_instances; i++) { for (int i = 0; i < num_instances; i++) {
for (unsigned n = 0; n < instances[i].n_partitions_current; n++) { for (unsigned n = 0; n < instances[i]->n_partitions_current; n++) {
if (instances[i].partition_types[n] == key) { if (instances[i]->partition_types[n] == key) {
if (get != nullptr && val == nullptr) { if (get != nullptr && val == nullptr) {
*get = instances[i].partition_names[n]; *get = instances[i]->partition_names[n];
return 0; 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; return 0;
} }
} }

22
src/systemcmds/mtd/mtd.cpp
Unescape View File

@ -80,11 +80,11 @@ static int mtd_status(void)
bool running = false; bool running = false;
unsigned int num_instances; 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) { if (instances) {
for (unsigned int i = 0; i < num_instances; ++i) { for (unsigned int i = 0; i < num_instances; ++i) {
if (instances[i].mtd_dev) { if (instances[i]->mtd_dev) {
unsigned long blocksize; unsigned long blocksize;
unsigned long erasesize; unsigned long erasesize;
@ -93,7 +93,7 @@ static int mtd_status(void)
unsigned int nblocks; unsigned int nblocks;
unsigned int partsize; 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) { if (ret == 0) {
@ -102,17 +102,17 @@ static int mtd_status(void)
printf(" blocksize: %lu\n", blocksize); printf(" blocksize: %lu\n", blocksize);
printf(" erasesize: %lu\n", erasesize); printf(" erasesize: %lu\n", erasesize);
printf(" neraseblocks: %lu\n", neraseblocks); 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 totalnblocks = 0;
unsigned int totalpartsize = 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; 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(" 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); printf(" blocks: %" PRIu32 " (%lu bytes)\n", geo.neraseblocks, erasesize * geo.neraseblocks);
totalnblocks += geo.neraseblocks; totalnblocks += geo.neraseblocks;
totalpartsize += erasesize * geo.neraseblocks; totalpartsize += erasesize * geo.neraseblocks;
@ -323,7 +323,7 @@ int mtd_main(int argc, char *argv[])
if (myoptind < argc) { if (myoptind < argc) {
unsigned int num_instances; 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) { if (instances == nullptr) {
PX4_ERR("Driver not running"); PX4_ERR("Driver not running");
@ -338,11 +338,11 @@ int mtd_main(int argc, char *argv[])
#if !defined(CONSTRAINED_FLASH) #if !defined(CONSTRAINED_FLASH)
if (!strcmp(argv[myoptind], "readtest")) { if (!strcmp(argv[myoptind], "readtest")) {
return mtd_readtest(instances[instance]); return mtd_readtest(*instances[instance]);
} }
if (!strcmp(argv[myoptind], "rwtest")) { if (!strcmp(argv[myoptind], "rwtest")) {
return mtd_rwtest(instances[instance]); return mtd_rwtest(*instances[instance]);
} }
#endif #endif
@ -352,7 +352,7 @@ int mtd_main(int argc, char *argv[])
} }
if (!strcmp(argv[myoptind], "erase")) { if (!strcmp(argv[myoptind], "erase")) {
return mtd_erase(instances[instance]); return mtd_erase(*instances[instance]);
} }
} }

Write Preview
Loading…
Cancel
Save