diff --git a/src/modules/systemlib/CMakeLists.txt b/src/modules/systemlib/CMakeLists.txt index 4507974e85..04fccf3566 100644 --- a/src/modules/systemlib/CMakeLists.txt +++ b/src/modules/systemlib/CMakeLists.txt @@ -56,6 +56,8 @@ if(${OS} STREQUAL "nuttx") err.c printload.c param/param.c + flashparams/flashparams.c + flashparams/flashfs.c up_cxxinitialize.c ) elseif ("${CONFIG_SHMEM}" STREQUAL "1") diff --git a/src/modules/systemlib/bson/tinybson.c b/src/modules/systemlib/bson/tinybson.c index 41ffbcf822..099dffe4bc 100644 --- a/src/modules/systemlib/bson/tinybson.c +++ b/src/modules/systemlib/bson/tinybson.c @@ -456,7 +456,9 @@ bson_encoder_fini(bson_encoder_t encoder) } /* sync file */ - BSON_FSYNC(encoder->fd); + if (encoder->fd > -1) { + BSON_FSYNC(encoder->fd); + } return 0; } diff --git a/src/modules/systemlib/flashparams/flashfs.c b/src/modules/systemlib/flashparams/flashfs.c new file mode 100644 index 0000000000..c1c05ad321 --- /dev/null +++ b/src/modules/systemlib/flashparams/flashfs.c @@ -0,0 +1,1122 @@ +/**************************************************************************** + * + * Copyright (c) 2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file flashparam.c + * + * Global flash based parameter store. + * + * This provides the mechanisms to interface to the PX4 + * parameter system but replace the IO with non file based flash + * i/o routines. So that the code my be implemented on a SMALL memory + * foot print device. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "flashfs.h" +#include +#include + + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/**************************************************************************** + * Private Types + ****************************************************************************/ + +typedef uint32_t h_magic_t; +typedef uint16_t h_size_t; +typedef uint16_t h_flag_t; +typedef uint16_t data_size_t; + +typedef enum flash_config_t { + LargestBlock = 2 * 1024, // This represents the size need for backing store + MagicSig = 0xaa553cc3, + BlankSig = 0xffffffff +} flash_config_t; + +typedef enum flash_flags_t { + SizeMask = 0x0003, + MaskEntry = ~SizeMask, + BlankEntry = (h_flag_t)BlankSig, + ValidEntry = (0xa5ac & ~SizeMask), + ErasedEntry = 0x0000, +} flash_flags_t; + + +/* File flash_entry_header_t will be sizeof(h_magic_t) aligned + * The Size will be the actual length of the header plus the data + * and any padding needed to have the size be an even multiple of + * sizeof(h_magic_t) + * The + */ +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wattributes" +typedef struct packed_struct flash_entry_header_t { + h_magic_t magic; /* Used to ID files*/ + h_flag_t flag; /* Used to erase this entry */ + uint32_t crc; /* Calculated over the size - end of data */ + h_size_t size; /* When added to a byte pointer to flash_entry_header_t + * Will result the offset of the next active file or + * free space. */ + flash_file_token_t file_token; /* file token type - essentially the name/type */ +} flash_entry_header_t; +#pragma GCC diagnostic pop + +/**************************************************************************** + * Private Function Prototypes + ****************************************************************************/ + +/**************************************************************************** + * Private Data + ****************************************************************************/ +static uint8_t *working_buffer; +static uint16_t working_buffer_size; +static bool working_buffer_static; +static sector_descriptor_t *sector_map; +static int last_erased; + +/**************************************************************************** + * Public Data + ****************************************************************************/ + +const flash_file_token_t parameters_token = { + .n = {'p', 'a', 'r', 'm'}, +}; + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: parameter_flashfs_free + * + * Description: + * Frees dynamicaly allocated memory + * + * + ****************************************************************************/ + +static void parameter_flashfs_free(void) +{ + if (!working_buffer_static && working_buffer != NULL) { + free(working_buffer); + working_buffer = NULL; + working_buffer_size = 0; + } +} + +/**************************************************************************** + * Name: blank_flash + * + * Description: + * This helper function returns true if the pointer points to a blank_flash + * location + * + * Input Parameters: + * pf - A pointer to memory aligned on sizeof(uint32_t) boundaries + * + * Returned value: + * true if blank + * + * + ****************************************************************************/ + +static inline int blank_flash(uint32_t *pf) +{ + return *pf == BlankSig; +} + +/**************************************************************************** + * Name: blank_check + * + * Description: + * Given a pointer to a flash entry header and a new size + * + * Input Parameters: + * pf - A pointer to the current flash entry header + * new_size - The total number of bytes to be written + * + * Returned value: + * true if space is blank, If it is not blank it returns false + * + ****************************************************************************/ + +static bool blank_check(flash_entry_header_t *pf, + size_t new_size) +{ + bool rv = true; + uint32_t *pm = (uint32_t *) pf; + new_size /= sizeof(uint32_t); + + while (new_size-- && rv) { + if (!blank_flash(pm++)) { + rv = false; + } + } + + return rv; +} + +/**************************************************************************** + * Name: valid_magic + * + * Description: + * This helper function returns true if the pointer points to a valid + * magic signature + * + * Input Parameters: + * pm - A pointer to memory aligned on sizeof(h_magic_t) boundaries + * + * Returned value: + * true if magic is valid + * + * + ****************************************************************************/ + +static inline int valid_magic(h_magic_t *pm) +{ + return *pm == MagicSig; +} + +/**************************************************************************** + * Name: blank_magic + * + * Description: + * This helper function returns true if the pointer points to a valid + * blank magic signature + * + * Input Parameters: + * pm - A pointer to memory aligned on sizeof(h_magic_t) boundaries + * + * Returned value: + * true if magic is valid + * + * + ****************************************************************************/ + +static inline int blank_magic(h_magic_t *pm) +{ + return *pm == BlankSig; +} +/**************************************************************************** + * Name: erased_entry + * + * Description: + * This helper function returns true if the entry is Erased + * + * Input Parameters: + * fi - A pointer to the current flash entry header + * + * Returned value: + * true if erased + * + * + ****************************************************************************/ + +static inline int erased_entry(flash_entry_header_t *fi) +{ + return (fi->flag & MaskEntry) == ErasedEntry; +} + +/**************************************************************************** + * Name: blank_entry + * + * Description: + * This helper function returns true if the entry is Blank + * + * Input Parameters: + * fi - A pointer to the current flash entry header + * + * Returned value: + * true if Blank + * + * + ****************************************************************************/ + +static inline int blank_entry(flash_entry_header_t *fi) +{ + return fi->magic == BlankSig && fi->flag == BlankEntry; +} + +/**************************************************************************** + * Name: valid_entry + * + * Description: + * This helper function returns true if the entry is Blank + * + * Input Parameters: + * fi - A pointer to the current flash entry header + * + * Returned value: + * true if valid_entry + * + * + ****************************************************************************/ + +static inline int valid_entry(flash_entry_header_t *fi) +{ + return (fi->flag & MaskEntry) == ValidEntry; +} + +/**************************************************************************** + * Name: entry_size_adjust + * + * Description: + * This helper function returns the size adjust + * + * Input Parameters: + * fi - A pointer to the current flash entry header + * + * Returned value: + * true if valid_entry + * + * + ****************************************************************************/ + +static inline int entry_size_adjust(flash_entry_header_t *fi) +{ + return fi->flag & SizeMask; +} + +/**************************************************************************** + * Name: next_entry + * + * Description: + * This helper function advances the flash entry header pointer to the + * locations of the next entry. + * + * Input Parameters: + * fh - A pointer to the current file header + * + * Returned value: + * - A pointer to the next file header location + * + * + ****************************************************************************/ + +static inline flash_entry_header_t *next_entry(flash_entry_header_t *fi) +{ + uint8_t *pb = (uint8_t *)fi; + return (flash_entry_header_t *) &pb[fi->size]; +} + +/**************************************************************************** + * Name: entry_data + * + * Description: + * This helper function returns a pointer the the data in the entry + * + * Input Parameters: + * fh - A pointer to the current file header + * + * Returned value: + * - A pointer to the next file header location + * + * + ****************************************************************************/ + +static inline uint8_t *entry_data(flash_entry_header_t *fi) +{ + return ((uint8_t *)fi) + sizeof(flash_entry_header_t); +} + +/**************************************************************************** + * Name: entry_data_length + * + * Description: + * This helper function returns the size of the user data + * + * Input Parameters: + * fh - A pointer to the current file header + * + * Returned value: + * - The length of the data in the entry + * + * + ****************************************************************************/ + +static inline data_size_t entry_data_length(flash_entry_header_t *fi) +{ + return fi->size - (sizeof(flash_entry_header_t) + entry_size_adjust(fi)); +} + +/**************************************************************************** + * Name: entry_crc_start + * + * Description: + * This helper function returns a const byte pointer to the location + * where the CRC is calculated over + * + * Input Parameters: + * fi - A pointer to the current file header + * + * Returned value: + * A pointer to the point at which the crc is calculated from. + * + * + ****************************************************************************/ + +static inline const uint8_t *entry_crc_start(flash_entry_header_t *fi) +{ + return (const uint8_t *)&fi->size; +} + +/**************************************************************************** + * Name: entry_crc_length + * + * Description: + * This helper function returns the length of the regone where the CRC is + * calculated over + * + * Input Parameters: + * fi - A pointer to the current file header + * + * Returned value: + * Number of bytes to to crc + * + * + ****************************************************************************/ + +static inline data_size_t entry_crc_length(flash_entry_header_t *fi) +{ + return fi->size - offsetof(flash_entry_header_t, size); +} + +/**************************************************************************** + * Name: find_entry + * + * Description: + * This helper function locates an "file" from the the file token + * + * Input Parameters: + * token - A flash file token, the pseudo file name + * + * Returned value: + * On Success a pointer to flash entry header or NULL on failure + * + * + ****************************************************************************/ + +static flash_entry_header_t *find_entry(flash_file_token_t token) +{ + for (int s = 0; sector_map[s].address; s++) { + + h_magic_t *pmagic = (h_magic_t *) sector_map[s].address; + h_magic_t *pe = pmagic + (sector_map[s].size / sizeof(h_magic_t)) - 1; + + /* Hunt for Magic Signature */ +cont: + + while (pmagic != pe && !valid_magic(pmagic)) { + pmagic++; + } + + /* Did we reach the end + * if so try the next sector */ + + if (pmagic == pe) { continue; } + + /* Found a magic So assume it is a file header */ + + flash_entry_header_t *pf = (flash_entry_header_t *) pmagic; + + /* Test the CRC */ + + if (pf->crc == crc32(entry_crc_start(pf), entry_crc_length(pf))) { + + /* Good CRC is it the one we are looking for ?*/ + + if (valid_entry(pf) && pf->file_token.t == token.t) { + + return pf; + + } else { + + /* Not the one we wanted but we can trust the size */ + + pf = next_entry(pf); + pmagic = (h_magic_t *) pf; + + /* If the next one is erased */ + + if (blank_entry(pf)) { + continue; + } + } + + goto cont; + + } else { + + /* in valid CRC so keep looking */ + + pmagic++; + } + } + + return NULL; +} + +/**************************************************************************** + * Name: find_free + * + * Description: + * This helper function locates free space for the number of bytes required + * + * Input Parameters: + * required - Number of bytes required + * + * Returned value: + * On Success a pointer to flash entry header or NULL on failure + * + * + ****************************************************************************/ + +static flash_entry_header_t *find_free(data_size_t required) +{ + for (int s = 0; sector_map[s].address; s++) { + + h_magic_t *pmagic = (h_magic_t *) sector_map[s].address; + h_magic_t *pe = pmagic + (sector_map[s].size / sizeof(h_magic_t)) - 1; + + /* Hunt for Magic Signature */ + + do { + + if (valid_magic(pmagic)) { + + flash_entry_header_t *pf = (flash_entry_header_t *) pmagic; + + /* Test the CRC */ + + if (pf->crc == crc32(entry_crc_start(pf), entry_crc_length(pf))) { + + /* Valid Magic and CRC look for the next record*/ + + pmagic = ((uint32_t *) next_entry(pf)); + + } else { + + pmagic++; + } + } + + if (blank_magic(pmagic)) { + + flash_entry_header_t *pf = (flash_entry_header_t *) pmagic; + + if (blank_entry(pf) && blank_check(pf, required)) { + return pf; + } + + } + } while (++pmagic != pe); + } + + return NULL; +} + +/**************************************************************************** + * Name: get_next_sector_descriptor + * + * Description: + * Given a pointer to sector_descriptor_t, this helper function + * returns a pointer to the next sector_descriptor_t + * + * Input Parameters: + * current - A pointer to the current sector_descriptor_t + * + * Returned value: + * On Success A pointer to the next sector_descriptor_t, + * otherwise NULL + * + * + ****************************************************************************/ + +static sector_descriptor_t *get_next_sector_descriptor(sector_descriptor_t * + current) +{ + for (int s = 0; sector_map[s].address; s++) { + if (current == §or_map[s]) { + if (sector_map[s + 1].address) { + s++; + + } else { + s = 0; + } + + return §or_map[s]; + } + } + + return NULL; +} + +/**************************************************************************** + * Name: get_next_sector + * + * Description: + * Given a pointer to a flash entry header returns the sector descriptor + * for the file is located in + * + * Input Parameters: + * current - A pointer to the current flash entry header + * + * Returned value: + * On Success A pointer to the next sector_descriptor_t, + * otherwise NULL + * + * + ****************************************************************************/ + +static sector_descriptor_t *get_sector_info(flash_entry_header_t *current) +{ + for (int s = 0; sector_map[s].address != 0; s++) { + uint8_t *pb = (uint8_t *) sector_map[s].address; + uint8_t *pe = pb + sector_map[s].size - 1; + uint8_t *pc = (uint8_t *) current; + + if (pc >= pb && pc <= pe) { + return §or_map[s]; + } + } + + return 0; +} + +/**************************************************************************** + * Name: erase_sector + * + * Description: + * Given a pointer to sector_descriptor_t, this function + * erases the sector and updates the last_erased using + * the pointer to the flash_entry_header_t as a sanity check + * + * Input Parameters: + * sm - A pointer to the current sector_descriptor_t + * pf - A pointer to the current flash entry header + * + * Returned value: + * O On Success or a negative errno + * + * + ****************************************************************************/ + +static int erase_sector(sector_descriptor_t *sm, flash_entry_header_t *pf) +{ + int rv = 0; + ssize_t page = up_progmem_getpage((size_t)pf); + + if (page > 0 && page == sm->page) { + last_erased = sm->page; + ssize_t size = up_progmem_erasepage(page); + + if (size < 0 || size != sm->size) { + rv = size; + } + } + + return rv; +} + +/**************************************************************************** + * Name: erase_entry + * + * Description: + * Given a pointer to a flash entry header erases the entry + * + * Input Parameters: + * pf - A pointer to the current flash entry header + * + * + * Returned value: + * >0 On Success or a negative errno + * + * + ****************************************************************************/ + +static int erase_entry(flash_entry_header_t *pf) +{ + h_flag_t data = ErasedEntry; + size_t size = sizeof(h_flag_t); + int rv = up_progmem_write((size_t) &pf->flag, &data, size); + return rv; +} + +/**************************************************************************** + * Name: check_free_space_in_sector + * + * Description: + * Given a pointer to a flash entry header and a new size + * + * Input Parameters: +* pf - A pointer to the current flash entry header + * new_size - The total number of bytes to be written + * + * Returned value: + * 0 if there is enough space left to write new size + * If not it returns the flash_file_sector_t * that needs to be erased. + * + ****************************************************************************/ + +static sector_descriptor_t *check_free_space_in_sector(flash_entry_header_t + *pf, size_t new_size) +{ + sector_descriptor_t *sm = get_sector_info(pf); + uint8_t *psector_first = (uint8_t *) sm->address; + uint8_t *psector_last = psector_first + sm->size - 1; + uint8_t *pnext_end = (uint8_t *)(valid_magic((h_magic_t *)pf) ? next_entry(pf) : pf) + new_size; + + if (pnext_end >= psector_first && pnext_end <= psector_last) { + sm = 0; + } + + return sm; +} + +/**************************************************************************** + * Public Functions + ****************************************************************************/ +/**************************************************************************** + * Name: parameter_flashfs_read + * + * Description: + * This function returns a pointer to the locations of the data associated + * with the file token. On successful return *buffer will be set to Flash + * location and *buf_size the length of the user data. + * + * Input Parameters: + * token - File Token File to read + * buffer - A pointer to a pointer that will receive the address + * in flash of the data of this "files" data + * buf_size - A pointer to receive the number of bytes in the "file" + * + * Returned value: + * On success number of bytes read or a negative errno value, + * + * + ****************************************************************************/ + +int parameter_flashfs_read(flash_file_token_t token, uint8_t **buffer, size_t + *buf_size) +{ + int rv = -ENXIO; + + if (sector_map) { + + rv = -ENOENT; + flash_entry_header_t *pf = find_entry(token); + + if (pf) { + (*buffer) = entry_data(pf); + rv = entry_data_length(pf); + *buf_size = rv; + } + } + + return rv; +} + +/**************************************************************************** + * Name: parameter_flashfs_write + * + * Description: + * This function writes user data from the buffer allocated with a previous call + * to parameter_flashfs_alloc. flash starting at the given address + * + * Input Parameters: + * token - File Token File to read + * buffer - A pointer to a buffer with buf_size bytes to be written + * to the flash. This buffer must be allocated + * with a previous call to flash_alloc_buffer + * buf_size - Number of bytes to write + * + * Returned value: + * On success the number of bytes written On Error a negative value of errno + * + ****************************************************************************/ + +int +parameter_flashfs_write(flash_file_token_t token, uint8_t *buffer, size_t buf_size) +{ + int rv = -ENXIO; + + if (sector_map) { + + rv = 0; + + /* Calculate the total space needed */ + + size_t total_size = buf_size + sizeof(flash_entry_header_t); + size_t alignment = sizeof(h_magic_t) - 1; + size_t size_adjust = ((total_size + alignment) & ~alignment) - total_size; + total_size += size_adjust; + + /* Is this and existing entry */ + + flash_entry_header_t *pf = find_entry(token); + + if (!pf) { + + /* No Entry exists for this token so find a place for it */ + + pf = find_free(total_size); + + /* No Space */ + + if (pf == 0) { + parameter_flashfs_free(); + return -ENOSPC; + } + + } else { + + /* Do we have space after the entry in the sector for the update */ + + sector_descriptor_t *current_sector = check_free_space_in_sector(pf, + total_size); + + + if (current_sector == 0) { + + /* Mark the last entry erased */ + + /* todo:consider a 2 stage erase or write before erase and do a fs check + * at start up + */ + + rv = erase_entry(pf); + + if (rv < 0) { + parameter_flashfs_free(); + return rv; + } + + /* We had space and marked the last entry erased so use the Next Free */ + + pf = next_entry(pf); + + } else { + + /* + * We did not have space in the current sector so select the next sector + */ + + current_sector = get_next_sector_descriptor(current_sector); + + /* Will the data fit */ + + if (current_sector->size < total_size) { + parameter_flashfs_free(); + return -ENOSPC; + } + + /* Mark the last entry erased */ + + /* todo:consider a 2 stage erase or write before erase and do a fs check + * at start up + */ + + rv = erase_entry(pf); + + if (rv < 0) { + parameter_flashfs_free(); + return rv; + } + + pf = (flash_entry_header_t *) current_sector->address; + } + + if (!blank_check(pf, total_size)) { + rv = erase_sector(current_sector, pf); + } + } + + flash_entry_header_t *pn = (flash_entry_header_t *)(buffer - sizeof(flash_entry_header_t)); + pn->magic = MagicSig; + pn->file_token.t = token.t; + pn->flag = ValidEntry + size_adjust; + pn->size = total_size; + + for (size_t a = 0; a < size_adjust; a++) { + buffer[buf_size + a] = (uint8_t)BlankSig; + } + + pn->crc = crc32(entry_crc_start(pn), entry_crc_length(pn)); + rv = up_progmem_write((size_t) pf, pn, pn->size); + int system_bytes = (sizeof(flash_entry_header_t) + size_adjust); + + if (rv >= system_bytes) { + rv -= system_bytes; + } + } + + parameter_flashfs_free(); + return rv; +} + +/**************************************************************************** + * Name: parameter_flashfs_alloc + * + * Description: + * This function is called to get a buffer to use in a subsequent call + * to parameter_flashfs_write. The address returned is advanced into the + * buffer to reserve space for the flash entry header. + * + * Input Parameters: + * token - File Token File to read (not used) + * buffer - A pointer to return a pointer to Memory of buf_size length + * suitable for calling parameter_flashfs_write + * buf_size - In: the size needed for the write operation. + * Out: The maximum number of bytes that can be written to + * the buffer + * + * Returned value: + * On success the number of bytes written On Error a negative value of errno + * + ****************************************************************************/ + +int parameter_flashfs_alloc(flash_file_token_t token, uint8_t **buffer, size_t *buf_size) +{ + int rv = -ENXIO; + + if (sector_map) { + + rv = -ENOMEM; + + if (!working_buffer_static) { + + working_buffer_size = *buf_size + sizeof(flash_entry_header_t); + working_buffer = malloc(working_buffer_size); + + } + + /* Allocation failed or not provided */ + + if (working_buffer == NULL) { + + working_buffer_size = 0; + + } else { + + /* We have a buffer reserve space and init it */ + *buffer = &working_buffer[sizeof(flash_entry_header_t)]; + *buf_size = working_buffer_size - sizeof(flash_entry_header_t); + memset(working_buffer, 0xff , working_buffer_size); + rv = 0; + + } + + } + + return rv; +} + +/**************************************************************************** + * Name: parameter_flashfs_erase + * + * Description: + * This function erases the sectors that were passed to parameter_flashfs_init + * + * Input Parameters: + * + * Returned value: + * On success the number of bytes erased + * On Error a negative value of errno + * + ****************************************************************************/ + +int parameter_flashfs_erase(void) +{ + int rv = -ENXIO; + + if (sector_map) { + rv = 0; + + for (int s = 0; sector_map[s].address; s++) { + int sz = erase_sector(§or_map[s], (flash_entry_header_t *)sector_map[s].address); + + if (sz != 0) { + return sz; + } + + rv += sector_map[s].size; + } + } + + return rv; +} + +/**************************************************************************** + * Name: parameter_flashfs_init + * + * Description: + * This helper function advances the flash entry header pointer to the + * locations of the next entry. + * + * Input Parameters: + * fconfig - A pointer to an null entry terminated array of + * flash_file_sector_t + * buffer - A pointer to a memory to make available to callers + * for write operations. When allocated to the caller + * space is reserved in the front for the + * flash_entry_header_t. + * If this is passes as NULL. The buffer will be + * allocated from the heap on callse to + * parameter_flashfs_alloc and fread on calls calls + * to parameter_flashfs_write + * + * size - The size of the buffer in bytes. Should be be 0 if buffer + * is NULL + * + * Returned value: + * - A pointer to the next file header location + * + * + ****************************************************************************/ + +int parameter_flashfs_init(sector_descriptor_t *fconfig, uint8_t *buffer, uint16_t size) +{ + int rv = 0; + sector_map = fconfig; + working_buffer_static = buffer != NULL; + + if (!working_buffer_static) { + size = 0; + } + + working_buffer = buffer; + working_buffer_size = size; + last_erased = -1; + + /* Sanity check */ + + flash_entry_header_t *pf = find_entry(parameters_token); + + /* No paramaters */ + + if (pf == NULL) { + size_t total_size = size + sizeof(flash_entry_header_t); + size_t alignment = sizeof(h_magic_t) - 1; + size_t size_adjust = ((total_size + alignment) & ~alignment) - total_size; + total_size += size_adjust; + + /* Do we have free space ?*/ + + if (find_free(total_size) == NULL) { + + /* No paramates and no free space => neeed erase */ + + rv = parameter_flashfs_erase(); + } + } + + return rv; +} + +#if defined(FLASH_UNIT_TEST) + +static sector_descriptor_t test_sector_map[] = { + {1, 16 * 1024, 0x08004000}, + {2, 16 * 1024, 0x08008000}, + {0, 0, 0}, +}; + +__EXPORT void test(void); + +uint8_t test_buf[32 * 1024]; + +__EXPORT void test(void) +{ + uint16_t largest_block = (32 * 1024) + 32; + uint8_t *buffer = malloc(largest_block); + + parameter_flashfs_init(test_sector_map, buffer, largest_block); + + for (int t = 0; t < sizeof(test_buf); t++) { + test_buf[t] = (uint8_t) t; + } + + int er = parameter_flashfs_erase(); + uint8_t *fbuffer; + size_t buf_size; + int written = 0; + int read = 0; + int rv = 0; + + for (int a = 0; a <= 4; a++) { + rv = parameter_flashfs_alloc(parameters_token, &fbuffer, &buf_size); + memcpy(fbuffer, test_buf, a); + buf_size = a; + written = parameter_flashfs_write(parameters_token, fbuffer, buf_size); + read = parameter_flashfs_read(parameters_token, &fbuffer, &buf_size); + + if (read != written) { + static volatile int j; + j++; + } + } + + int block = 2048; + + for (int a = 0; a <= 8; a++) { + rv = parameter_flashfs_alloc(parameters_token, &fbuffer, &buf_size); + memcpy(fbuffer, test_buf, block); + buf_size = block; + written = parameter_flashfs_write(parameters_token, fbuffer, buf_size); + read = parameter_flashfs_read(parameters_token, &fbuffer, &buf_size); + + if (read != written) { + static volatile int j; + j++; + } + + block += 2048; + } + + rv++; + er++; + free(buffer); +} +#endif /* FLASH_UNIT_TEST */ diff --git a/src/modules/systemlib/flashparams/flashfs.h b/src/modules/systemlib/flashparams/flashfs.h new file mode 100644 index 0000000000..65bcb48d00 --- /dev/null +++ b/src/modules/systemlib/flashparams/flashfs.h @@ -0,0 +1,220 @@ +/**************************************************************************** + * + * Copyright (c) 2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file param.h + * + * Global flash based parameter store. + * + * This provides the mechanisms to interface to the PX4 + * parameter system but replace the IO with non file based flash + * i/o routines. So that the code my be implemented on a SMALL memory + * foot print device. + * + */ + +#ifndef _SYSTEMLIB_FLASHPARAMS_NUTTX_PARAM_H +#define _SYSTEMLIB_FLASHPARAMS_NUTTX_PARAM_H + +#include +#include + + + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ +/* + * PARAMETER_BUFFER_SIZE must be defined larger then the maximum parameter + * memory needed to commit the recored + ~20 bytes. For the syslib's parameter + * this would be the size of the bson representations of the data + */ +#if !defined(PARAMETER_BUFFER_SIZE) +#define PARAMETER_BUFFER_SIZE 512 +#endif + +__BEGIN_DECLS + +/* + * Define the interface data a flash_file_token_t + * is like a file name + * + */ +typedef uint32_t flash_file_tokens_t; + +typedef struct flash_file_token_t { + union { + flash_file_tokens_t t; + uint8_t n[sizeof(flash_file_tokens_t)]; + }; +} flash_file_token_t; + +/* + * Define the parameter "file name" Currently there is only + * and it is hard coded. If more are added the + * parameter_flashfs_write would need to support a backing buffer + * for when a sector is erased. + */ +__EXPORT extern const flash_file_token_t parameters_token; + +/* Define the elements of the array passed to the + * parameter_flashfs_init function + * + * For example + * static sector_descriptor_t sector_map[] = { + * {1, 16 * 1024, 0x08004000}, + * {2, 16 * 1024, 0x08008000}, + * {0, 0, 0}, + * + */ +typedef struct sector_descriptor_t { + uint8_t page; + uint16_t size; + uint32_t address; +} sector_descriptor_t; + + +/**************************************************************************** + * Name: parameter_flashfs_init + * + * Description: + * This helper function advances the flash entry header pointer to the + * locations of the next entry. + * + * Input Parameters: + * fconfig - A pointer to an null entry terminated array of + * flash_file_sector_t + * buffer - A pointer to a memory to make available to callers + * for write operations. When allocated to the caller + * space is reserved in the front for the + * flash_entry_header_t. + * If this is passes as NULL. The buffer will be + * allocated from the heap on callse to + * parameter_flashfs_alloc and fread on calls calls + * to parameter_flashfs_write + * + * size - The size of the buffer in bytes. Should be be 0 if buffer + * is NULL + * + * Returned value: + * - A pointer to the next file header location + * + * + ****************************************************************************/ + +__EXPORT int parameter_flashfs_init(sector_descriptor_t *fconfig, uint8_t *buffer, uint16_t size); + +/**************************************************************************** + * Name: parameter_flashfs_read + * + * Description: + * This function returns a pointer to the locations of the data associated + * with the file token. On successful return *buffer will be set to Flash + * location and *buf_size the length of the user data. + * + * Input Parameters: + * token - File Token File to read + * buffer - A pointer to a pointer that will receive the address + * in flash of the data of this "files" data + * buf_size - A pointer to receive the number of bytes in the "file" + * + * Returned value: + * On success number of bytes read or a negative errno value, + * + * + ****************************************************************************/ + +__EXPORT int parameter_flashfs_read(flash_file_token_t ft, uint8_t **buffer, size_t *buf_size); + +/**************************************************************************** + * Name: parameter_flashfs_write + * + * Description: + * This function writes user data from the buffer allocated with a previous call + * to parameter_flashfs_alloc. flash starting at the given address + * + * Input Parameters: + * token - File Token File to read + * buffer - A pointer to a buffer with buf_size bytes to be written + * to the flash. This buffer must be allocated + * with a previous call to flash_alloc_buffer + * buf_size - Number of bytes to write + * + * Returned value: + * On success the number of bytes written On Error a negative value of errno + * If static buffer was not provided to parameter_flashfs_init the + * buffer will be freed. + * + ****************************************************************************/ + +__EXPORT int parameter_flashfs_write(flash_file_token_t ft, uint8_t *buffer, size_t buf_size); + +/**************************************************************************** + * Name: parameter_flashfs_erase + * + * Description: + * This function erases the sectors that were passed to parameter_flashfs_init + * + * Input Parameters: + * + * Returned value: + * On success the number of bytes erased + * On Error a negative value of errno + * + ****************************************************************************/ + +__EXPORT int parameter_flashfs_erase(void); + +/**************************************************************************** + * Name: parameter_flashfs_alloc + * + * Description: + * This function is called to get a buffer to use in a subsequent call + * to parameter_flashfs_write. The address returned is advanced into the + * buffer to reserve space for the flash entry header. + * + * Input Parameters: + * token - File Token File to read (not used) + * buffer - Memory of buf_size length suitable for calling + * parameter_flashfs_write + * buf_size - The maximum number of bytes that can be written to + * the buffer + * + * Returned value: + * On success the number of bytes written On Error a negative value of errno + * + ****************************************************************************/ + +__EXPORT int parameter_flashfs_alloc(flash_file_token_t ft, uint8_t **buffer, size_t *buf_size); +__END_DECLS +#endif /* _SYSTEMLIB_FLASHPARAMS_NUTTX_PARAM_H */ diff --git a/src/modules/systemlib/flashparams/flashparams.c b/src/modules/systemlib/flashparams/flashparams.c new file mode 100644 index 0000000000..623715b635 --- /dev/null +++ b/src/modules/systemlib/flashparams/flashparams.c @@ -0,0 +1,363 @@ +/**************************************************************************** + * + * Copyright (c) 2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ +/** + * @file flashparam.c + * + * Global flash based parameter store. + * + * This provides the mechanisms to interface to the PX4 + * parameter system but replace the IO with non file based flash + * i/o routines. So that the code my be implemented on a SMALL memory + * foot print device. + */ + +#include +#include +#include +#include +#include + +#include "systemlib/param/param.h" +#include "systemlib/uthash/utarray.h" +#include "systemlib/bson/tinybson.h" +#include "flashparams.h" +#include "flashfs.h" + +#if 0 +# define debug(fmt, args...) do { warnx(fmt, ##args); } while(0) +#else +# define debug(fmt, args...) do { } while(0) +#endif + + +/** + * Storage for modified parameters. + */ +struct param_wbuf_s { + param_t param; + union param_value_u val; + bool unsaved; +}; + + +/** lock the parameter store */ +static void +param_lock(void) +{ + //do {} while (sem_wait(¶m_sem) != 0); +} + +/** unlock the parameter store */ +static void +param_unlock(void) +{ + //sem_post(¶m_sem); +} + + +static int +param_export_internal(bool only_unsaved) +{ + struct param_wbuf_s *s = NULL; + struct bson_encoder_s encoder; + int result = -1; + + param_lock(); + + /* Use realloc */ + + bson_encoder_init_buf(&encoder, NULL, 0); + + /* no modified parameters -> we are done */ + if (param_values == NULL) { + result = 0; + goto out; + } + + while ((s = (struct param_wbuf_s *)utarray_next(param_values, s)) != NULL) { + + int32_t i; + float f; + + /* + * If we are only saving values changed since last save, and this + * one hasn't, then skip it + */ + if (only_unsaved && !s->unsaved) { + continue; + } + + s->unsaved = false; + + /* append the appropriate BSON type object */ + + switch (param_type(s->param)) { + + case PARAM_TYPE_INT32: + param_get(s->param, &i); + + if (bson_encoder_append_int(&encoder, param_name(s->param), i)) { + debug("BSON append failed for '%s'", param_name(s->param)); + goto out; + } + + break; + + case PARAM_TYPE_FLOAT: + param_get(s->param, &f); + + if (bson_encoder_append_double(&encoder, param_name(s->param), f)) { + debug("BSON append failed for '%s'", param_name(s->param)); + goto out; + } + + break; + + case PARAM_TYPE_STRUCT ... PARAM_TYPE_STRUCT_MAX: + if (bson_encoder_append_binary(&encoder, + param_name(s->param), + BSON_BIN_BINARY, + param_size(s->param), + param_get_value_ptr_external(s->param))) { + debug("BSON append failed for '%s'", param_name(s->param)); + goto out; + } + + break; + + default: + debug("unrecognized parameter type"); + goto out; + } + } + + result = 0; + +out: + param_unlock(); + + if (result == 0) { + + /* Finalize the bison encoding*/ + + bson_encoder_fini(&encoder); + + /* Get requiered space */ + + size_t buf_size = bson_encoder_buf_size(&encoder); + + /* Get a buffer from the flash driver with enough space */ + + uint8_t *buffer; + result = parameter_flashfs_alloc(parameters_token, &buffer, &buf_size); + + if (result == OK) { + + void *enc_buff = bson_encoder_buf_data(&encoder); + memcpy(buffer, enc_buff, buf_size); + free(enc_buff); + result = parameter_flashfs_write(parameters_token, buffer, buf_size); + result = result == buf_size ? OK : -EFBIG; + } + } + + return result; +} + +struct param_import_state { + bool mark_saved; +}; + +static int +param_import_callback(bson_decoder_t decoder, void *private, bson_node_t node) +{ + float f; + int32_t i; + void *v, *tmp = NULL; + int result = -1; + struct param_import_state *state = (struct param_import_state *)private; + + /* + * EOO means the end of the parameter object. (Currently not supporting + * nested BSON objects). + */ + if (node->type == BSON_EOO) { + debug("end of parameters"); + return 0; + } + + /* + * Find the parameter this node represents. If we don't know it, + * ignore the node. + */ + param_t param = param_find_no_notification(node->name); + + if (param == PARAM_INVALID) { + debug("ignoring unrecognised parameter '%s'", node->name); + return 1; + } + + /* + * Handle setting the parameter from the node + */ + + switch (node->type) { + case BSON_INT32: + if (param_type(param) != PARAM_TYPE_INT32) { + debug("unexpected type for '%s", node->name); + goto out; + } + + i = node->i; + v = &i; + break; + + case BSON_DOUBLE: + if (param_type(param) != PARAM_TYPE_FLOAT) { + debug("unexpected type for '%s", node->name); + goto out; + } + + f = node->d; + v = &f; + break; + + case BSON_BINDATA: + if (node->subtype != BSON_BIN_BINARY) { + debug("unexpected subtype for '%s", node->name); + goto out; + } + + if (bson_decoder_data_pending(decoder) != param_size(param)) { + debug("bad size for '%s'", node->name); + goto out; + } + + /* XXX check actual file data size? */ + tmp = malloc(param_size(param)); + + if (tmp == NULL) { + debug("failed allocating for '%s'", node->name); + goto out; + } + + if (bson_decoder_copy_data(decoder, tmp)) { + debug("failed copying data for '%s'", node->name); + goto out; + } + + v = tmp; + break; + + default: + debug("unrecognised node type"); + goto out; + } + + if (param_set_external(param, v, state->mark_saved, true, false)) { + + debug("error setting value for '%s'", node->name); + goto out; + } + + if (tmp != NULL) { + free(tmp); + tmp = NULL; + } + + /* don't return zero, that means EOF */ + result = 1; + +out: + + if (tmp != NULL) { + free(tmp); + } + + return result; +} + +static int +param_import_internal(bool mark_saved) +{ + struct bson_decoder_s decoder; + int result = -1; + struct param_import_state state; + + uint8_t *buffer = 0; + size_t buf_size; + parameter_flashfs_read(parameters_token, &buffer, &buf_size); + + if (bson_decoder_init_buf(&decoder, buffer, buf_size, param_import_callback, &state)) { + debug("decoder init failed"); + goto out; + } + + state.mark_saved = mark_saved; + + do { + result = bson_decoder_next(&decoder); + + } while (result > 0); + +out: + + if (result < 0) { + debug("BSON error decoding parameters"); + } + + return result; +} + +int flash_param_save(void) +{ + return param_export_internal(false); +} + + +int flash_param_save_default(void) +{ + return param_export_internal(false); +} + + +int flash_param_load(void) +{ + param_reset_all(); + return param_import_internal(true); +} + +int flash_param_import(void) +{ + return 0; +} diff --git a/src/modules/systemlib/flashparams/flashparams.h b/src/modules/systemlib/flashparams/flashparams.h new file mode 100644 index 0000000000..8300669bd4 --- /dev/null +++ b/src/modules/systemlib/flashparams/flashparams.h @@ -0,0 +1,73 @@ +/**************************************************************************** + * + * Copyright (c) 2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file param.h + * + * Global flash based parameter store. + * + * This provides the mechanisms to interface to the PX4 + * parameter system but replace the IO with non file based flash + * i/o routines. So that the code my be implemented on a SMALL memory + * foot print device. + * + */ + +#ifndef _SYSTEMLIB_FLASHPARAMS_FLASHPARAMS_H +#define _SYSTEMLIB_FLASHPARAMS_FLASHPARAMS_H + +#include +#include +#include +#include "systemlib/uthash/utarray.h" + +__BEGIN_DECLS + +/* + * When using the flash based parameter store we have to force + * the param_values and 2 functions to be global + */ + +#define FLASH_PARAMS_EXPOSE __EXPORT + +__EXPORT extern UT_array *param_values; +__EXPORT int param_set_external(param_t param, const void *val, bool mark_saved, bool notify_changes, bool is_saved); +__EXPORT const void *param_get_value_ptr_external(param_t param); + +/* The interface hooks to the Flash based storage */ +__EXPORT int flash_param_save(void); +__EXPORT int flash_param_save_default(void); +__EXPORT int flash_param_load(void); +__EXPORT int flash_param_import(void); +__END_DECLS +#endif /* _SYSTEMLIB_FLASHPARAMS_FLASHPARAMS_H */ diff --git a/src/modules/systemlib/param/param.c b/src/modules/systemlib/param/param.c index e758099124..265c5f7e14 100644 --- a/src/modules/systemlib/param/param.c +++ b/src/modules/systemlib/param/param.c @@ -63,12 +63,21 @@ #include "systemlib/uthash/utarray.h" #include "systemlib/bson/tinybson.h" -#include "uORB/uORB.h" -#include "uORB/topics/parameter_update.h" -#include "px4_parameters.h" +#if !defined(PARAM_NO_ORB) +# include "uORB/uORB.h" +# include "uORB/topics/parameter_update.h" +#endif +#if !defined(FLASH_BASED_PARAMS) +# define FLASH_PARAMS_EXPOSE +#else +# include "systemlib/flashparams/flashparams.h" +#endif + +#include "px4_parameters.h" #include + #if 0 # define debug(fmt, args...) do { warnx(fmt, ##args); } while(0) #else @@ -131,13 +140,16 @@ get_param_info_count(void) } /** flexible array holding modified parameter values */ -UT_array *param_values; +FLASH_PARAMS_EXPOSE UT_array *param_values; /** array info for the modified parameters array */ -const UT_icd param_icd = {sizeof(struct param_wbuf_s), NULL, NULL, NULL}; +FLASH_PARAMS_EXPOSE const UT_icd param_icd = {sizeof(struct param_wbuf_s), NULL, NULL, NULL}; + +#if !defined(PARAM_NO_ORB) /** parameter update topic handle */ static orb_advert_t param_topic = NULL; +#endif static void param_set_used_internal(param_t param); @@ -235,9 +247,11 @@ param_find_changed(param_t param) static void param_notify_changes(bool is_saved) { - struct parameter_update_s pup; - pup.timestamp = hrt_absolute_time(); - pup.saved = is_saved; +#if !defined(PARAM_NO_ORB) + struct parameter_update_s pup = { + .timestamp = hrt_absolute_time(), + .saved = is_saved + }; /* * If we don't have a handle to our topic, create one now; otherwise @@ -249,6 +263,8 @@ param_notify_changes(bool is_saved) } else { orb_publish(ORB_ID(parameter_update), param_topic, &pup); } + +#endif } param_t @@ -436,6 +452,7 @@ param_size(param_t param) return 0; } + /** * Obtain a pointer to the storage allocated for a parameter. * @@ -583,6 +600,19 @@ out: return result; } +#if defined(FLASH_BASED_PARAMS) +int param_set_external(param_t param, const void *val, bool mark_saved, bool notify_changes, bool is_saved) +{ + return param_set_internal(param, val, mark_saved, notify_changes, is_saved); +} + +const void *param_get_value_ptr_external(param_t param) +{ + return param_get_value_ptr(param); +} + +#endif + int param_set(param_t param, const void *val) { @@ -734,6 +764,7 @@ int param_save_default(void) { int res; +#if !defined(FLASH_BASED_PARAMS) int fd; const char *filename = param_get_default_file(); @@ -759,6 +790,9 @@ param_save_default(void) } PARAM_CLOSE(fd); +#else + res = flash_param_save(); +#endif return res; } diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h index 1861d27ede..b41dc7b0eb 100644 --- a/src/platforms/px4_defines.h +++ b/src/platforms/px4_defines.h @@ -120,6 +120,10 @@ typedef param_t px4_param_t; #define PRId64 "lld" #endif +#if !defined(offsetof) +# define offsetof(TYPE, MEMBER) __builtin_offsetof (TYPE, MEMBER) +#endif + /* * POSIX Specific defines */ diff --git a/src/systemcmds/param/param.c b/src/systemcmds/param/param.c index ea698ae5a1..034c342597 100644 --- a/src/systemcmds/param/param.c +++ b/src/systemcmds/param/param.c @@ -55,6 +55,9 @@ #include "systemlib/systemlib.h" #include "systemlib/param/param.h" +#if defined(FLASH_BASED_PARAMS) +# include "systemlib/flashparams/flashparams.h" +#endif #include "systemlib/err.h" __EXPORT int param_main(int argc, char *argv[]); @@ -65,6 +68,7 @@ enum COMPARE_OPERATOR { }; static int do_save(const char *param_file_name); +static int do_save_default(void); static int do_load(const char *param_file_name); static int do_import(const char *param_file_name); static int do_show(const char *search_string); @@ -84,7 +88,7 @@ param_main(int argc, char *argv[]) return do_save(argv[2]); } else { - if (param_save_default()) { + if (do_save_default()) { warnx("Param export failed."); return 1; @@ -213,6 +217,35 @@ param_main(int argc, char *argv[]) return 1; } +#if defined(FLASH_BASED_PARAMS) +/* If flash based parameters are uses we call out + * to the following set of flash routines + */ +static int + +do_save(const char *param_file_name) +{ + return flash_param_save(); +} +static int +do_save_default(void) +{ + return flash_param_save_default(); +} + +static int +do_load(const char *param_file_name) +{ + return flash_param_load(); +} + +static int +do_import(const char *param_file_name) +{ + return flash_param_import(); +} +#else + static int do_save(const char *param_file_name) { @@ -238,6 +271,12 @@ do_save(const char *param_file_name) return 0; } +static int +do_save_default(void) +{ + return param_save_default(); +} + static int do_load(const char *param_file_name) { @@ -279,6 +318,7 @@ do_import(const char *param_file_name) return 0; } +#endif static int do_show(const char *search_string)