aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
authorpx4dev <px4@purgatory.org>2012-10-30 20:51:45 -0700
committerpx4dev <px4@purgatory.org>2012-10-30 20:51:45 -0700
commitb685d46dbfc583a26a92f1466dca64f9d45c3c4e (patch)
tree9231f3f5384a2e816ef9a14e179fa746855cf5d8
parent137afdbd3cf8c9356fabf15e8acbb41661daf40b (diff)
downloadpx4-firmware-b685d46dbfc583a26a92f1466dca64f9d45c3c4e.tar.gz
px4-firmware-b685d46dbfc583a26a92f1466dca64f9d45c3c4e.tar.bz2
px4-firmware-b685d46dbfc583a26a92f1466dca64f9d45c3c4e.zip
Clean out remains of the old EEPROM driver.
-rw-r--r--apps/drivers/boards/px4fmu/px4fmu_init.c8
-rw-r--r--apps/px4/tests/test_eeproms.c328
-rw-r--r--apps/px4/tests/tests_main.c1
-rw-r--r--apps/systemlib/systemlib.c1
-rw-r--r--nuttx/configs/px4fmu/include/drv_eeprom.h60
-rw-r--r--nuttx/configs/px4fmu/src/Makefile2
-rw-r--r--nuttx/configs/px4fmu/src/drv_eeprom.c522
7 files changed, 5 insertions, 917 deletions
diff --git a/apps/drivers/boards/px4fmu/px4fmu_init.c b/apps/drivers/boards/px4fmu/px4fmu_init.c
index 2dc3e60c6..38a284017 100644
--- a/apps/drivers/boards/px4fmu/px4fmu_init.c
+++ b/apps/drivers/boards/px4fmu/px4fmu_init.c
@@ -65,7 +65,6 @@
#include <arch/board/board.h>
#include <arch/board/drv_led.h>
-#include <arch/board/drv_eeprom.h>
#include <drivers/drv_hrt.h>
@@ -220,6 +219,7 @@ __EXPORT int nsh_archinitialize(void)
message("[boot] Successfully initialized SPI port 1\r\n");
+#if 0
/* initialize I2C2 bus */
i2c2 = up_i2cinitialize(2);
@@ -250,7 +250,7 @@ __EXPORT int nsh_archinitialize(void)
FMU_BASEBOARD_EEPROM_TOTAL_SIZE_BYTES,
FMU_BASEBOARD_EEPROM_PAGE_SIZE_BYTES,
FMU_BASEBOARD_EEPROM_PAGE_WRITE_TIME_US, "/dev/baseboard_eeprom", 1);
-
+#endif
#if defined(CONFIG_STM32_SPI3)
/* Get the SPI port */
@@ -276,7 +276,7 @@ __EXPORT int nsh_archinitialize(void)
message("[boot] Successfully bound SPI port 3 to the MMCSD driver\n");
#endif /* SPI3 */
-
+#if 0
/* initialize I2C1 bus */
i2c1 = up_i2cinitialize(1);
@@ -293,7 +293,7 @@ __EXPORT int nsh_archinitialize(void)
/* INIT 3: MULTIPORT-DEPENDENT INITIALIZATION */
/* Get board information if available */
-
+#endif
#ifdef CONFIG_ADC
int adc_state = adc_devinit();
diff --git a/apps/px4/tests/test_eeproms.c b/apps/px4/tests/test_eeproms.c
deleted file mode 100644
index 29ca8267f..000000000
--- a/apps/px4/tests/test_eeproms.c
+++ /dev/null
@@ -1,328 +0,0 @@
-/****************************************************************************
- * px4/eeproms/test_eeproms.c
- *
- * Copyright (C) 2012 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 NuttX 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.
- *
- ****************************************************************************/
-
-/****************************************************************************
- * Included Files
- ****************************************************************************/
-
-#include <nuttx/config.h>
-
-#include <sys/types.h>
-
-#include <stdio.h>
-#include <stdlib.h>
-#include <unistd.h>
-#include <string.h>
-#include <fcntl.h>
-#include <errno.h>
-#include <debug.h>
-
-#include <arch/board/board.h>
-
-#include "tests.h"
-
-#include <arch/board/drv_eeprom.h>
-
-/****************************************************************************
- * Pre-processor Definitions
- ****************************************************************************/
-
-/****************************************************************************
- * Private Types
- ****************************************************************************/
-
-/****************************************************************************
- * Private Function Prototypes
- ****************************************************************************/
-
-static int onboard_eeprom(int argc, char *argv[]);
-static int baseboard_eeprom(int argc, char *argv[]);
-
-/****************************************************************************
- * Private Data
- ****************************************************************************/
-
-struct {
- const char *name;
- const char *path;
- int (* test)(int argc, char *argv[]);
-} eeproms[] = {
- {"onboard_eeprom", "/dev/eeprom", onboard_eeprom},
- {"baseboard_eeprom", "/dev/baseboard_eeprom", baseboard_eeprom},
- {NULL, NULL, NULL}
-};
-
-/****************************************************************************
- * Public Data
- ****************************************************************************/
-
-/****************************************************************************
- * Private Functions
- ****************************************************************************/
-
-static int
-onboard_eeprom(int argc, char *argv[])
-{
- printf("\tonboard_eeprom: test start\n");
- fflush(stdout);
-
- int fd;
- uint8_t buf1[210] = {' ', 'P', 'X', '4', ' ', 'E', 'E', 'P', 'R', 'O', 'M', ' ', 'T', 'E', 'S', 'T', ' '};
- int ret;
- bool force_write = false;
- if (strcmp(argv[0], "jig") == 0) force_write = true;
-
- /* fill with spaces */
- //memset(buf1+16, 'x', sizeof(buf1-16));
-
- /* fill in some magic values at magic positions */
- buf1[63] = 'E';
- buf1[64] = 'S';
- buf1[127] = 'F';
- buf1[128] = 'T';
-
- /* terminate string */
- buf1[sizeof(buf1) - 1] = '\0';
-
- fd = open("/dev/eeprom", O_RDWR | O_NONBLOCK);
-
- if (fd < 0) {
- printf("onboard eeprom: open fail\n");
- return ERROR;
- }
-
- /* read data */
- ret = read(fd, buf1, 1);
-
- if (ret != 1) {
- printf("\tonboard eeprom: ERROR: reading first byte fail: %d\n", ret);
-
- switch (-ret) {
- case EPERM:
- printf("\treason: %s\n", EPERM_STR);
- break;
-
- case ENOENT:
- printf("\treason: %s\n", ENOENT_STR);
- break;
-
- case ESRCH:
- printf("\treason: %s\n", ESRCH_STR);
- break;
-
- case EINTR:
- printf("\treason: %s\n", EINTR_STR);
- break;
-
- }
- }
-
- printf("\tonboard eeprom: first byte: %d\n", buf1[0]);
- if (!force_write) {
- printf("\tonboard eeprom: WARNING: FURTHER TEST STEPS WILL DESTROY YOUR FLIGHT PARAMETER CONFIGURATION. PROCEED? (y/N)\n");
-
- printf("Input: ");
- char c = getchar();
- printf("%c\n", c);
- if (c != 'y' && c != 'Y') {
- /* not yes, abort */
- close(fd);
-
- /* Let user know everything is ok */
- printf("\tOK: onboard eeprom test aborted by user, read test successful\r\n");
- return OK;
- }
- }
-
- printf("\tonboard eeprom: proceeding with write test\r\n");
-
- /* increment counter */
- buf1[0] = buf1[0] + 1;
-
- /* rewind to the start of the file */
- lseek(fd, 0, SEEK_SET);
-
- /* write data */
- ret = write(fd, buf1, sizeof(buf1));
-
- if (ret != sizeof(buf1)) {
- printf("\tonboard eeprom: ERROR: write fail: %d\n", (char)ret);
-
- switch (-ret) {
- case EPERM:
- printf("\treason: %s\n", EPERM_STR);
- break;
-
- case ENOENT:
- printf("\treason: %s\n", ENOENT_STR);
- break;
-
- case ESRCH:
- printf("\treason: %s\n", ESRCH_STR);
- break;
-
- case EINTR:
- printf("\treason: %s\n", EINTR_STR);
- break;
-
- }
-
- //return ERROR;
- }
-
- /* rewind to the start of the file */
- lseek(fd, 0, SEEK_SET);
-
- /* read data */
- ret = read(fd, buf1, sizeof(buf1));
-
- if (ret != sizeof(buf1)) {
- printf("\tonboard eeprom: ERROR: read fail: %d\n", ret);
-
- switch (-ret) {
- case EPERM:
- printf("\treason: %s\n", EPERM_STR);
- break;
-
- case ENOENT:
- printf("\treason: %s\n", ENOENT_STR);
- break;
-
- case ESRCH:
- printf("\treason: %s\n", ESRCH_STR);
- break;
-
- case EINTR:
- printf("\treason: %s\n", EINTR_STR);
- break;
-
- }
-
- return ERROR;
-
- } else {
- /* enforce null termination and print as string */
- if (buf1[sizeof(buf1) - 1] != 0) {
- printf("\tWARNING: Null termination in file not present as expected, enforcing it now..\r\n");
- buf1[sizeof(buf1) - 1] = '\0';
- }
-
- /* read out counter and replace val */
- int counter = buf1[0];
- printf("\tonboard eeprom: count: #%d, read values: %s\n", counter, (char *)buf1 + 1);
- printf("\tAll %d bytes:\n\n\t", sizeof(buf1));
-
- for (int i = 0; i < sizeof(buf1); i++) {
- printf("0x%02x ", buf1[i]);
-
- if (i % 8 == 7) printf("\n\t");
-
- if (i % 64 == 63) printf("\n\t");
- }
-
- /* end any open line */
- printf("\n\n");
- }
-
- close(fd);
-
- /* Let user know everything is ok */
- printf("\tOK: onboard eeprom passed all tests successfully\n");
- return ret;
-}
-
-static int
-baseboard_eeprom(int argc, char *argv[])
-{
- printf("\tbaseboard eeprom: test start\n");
- fflush(stdout);
-
- int fd;
- uint8_t buf[128] = {'R', 'E', 'A', 'D', ' ', 'F', 'A', 'I', 'L', 'E', 'D', '\0'};
- int ret;
-
- fd = open("/dev/baseboard_eeprom", O_RDONLY | O_NONBLOCK);
-
- if (fd < 0) {
- printf("\tbaseboard eeprom: open fail\n");
- return ERROR;
- }
-
- /* read data */
- ret = read(fd, buf, sizeof(buf));
- /* set last char to string termination */
- buf[127] = '\0';
-
- if (ret != sizeof(buf)) {
- printf("\tbaseboard eeprom: ERROR: read fail\n", ret);
- return ERROR;
-
- } else {
- printf("\tbaseboard eeprom: string: %s\n", (char *)buf);
- }
-
- close(fd);
-
- /* XXX more tests here */
-
- /* Let user know everything is ok */
- printf("\tOK: baseboard eeprom passed all tests successfully\n");
- return ret;
-}
-
-/****************************************************************************
- * Public Functions
- ****************************************************************************/
-
-/****************************************************************************
- * Name: test_eeproms
- ****************************************************************************/
-
-int test_eeproms(int argc, char *argv[])
-{
- unsigned i;
-
- printf("Running EEPROMs tests:\n\n");
- fflush(stdout);
-
- for (i = 0; eeproms[i].name; i++) {
- printf(" eeprom: %s\n", eeproms[i].name);
- eeproms[i].test(argc, argv);
- fflush(stdout);
- /* wait 100 ms to make sure buffer is emptied */
- usleep(100000);
- }
-
- return 0;
-}
diff --git a/apps/px4/tests/tests_main.c b/apps/px4/tests/tests_main.c
index b9f6835b0..26f7ef96b 100644
--- a/apps/px4/tests/tests_main.c
+++ b/apps/px4/tests/tests_main.c
@@ -97,7 +97,6 @@ struct {
{"servo", test_servo, OPT_NOJIGTEST | OPT_NOALLTEST, 0},
{"adc", test_adc, OPT_NOJIGTEST, 0},
{"jig_voltages", test_jig_voltages, OPT_NOALLTEST, 0},
- {"eeproms", test_eeproms, 0, 0},
{"uart_loopback", test_uart_loopback, OPT_NOJIGTEST | OPT_NOALLTEST, 0},
{"uart_baudchange", test_uart_baudchange, OPT_NOJIGTEST | OPT_NOALLTEST, 0},
{"uart_send", test_uart_send, OPT_NOJIGTEST | OPT_NOALLTEST, 0},
diff --git a/apps/systemlib/systemlib.c b/apps/systemlib/systemlib.c
index b596b0f0e..84ba809a3 100644
--- a/apps/systemlib/systemlib.c
+++ b/apps/systemlib/systemlib.c
@@ -45,7 +45,6 @@
#include <signal.h>
#include <sys/stat.h>
#include <unistd.h>
-#include <arch/board/drv_eeprom.h>
#include <float.h>
#include <string.h>
diff --git a/nuttx/configs/px4fmu/include/drv_eeprom.h b/nuttx/configs/px4fmu/include/drv_eeprom.h
deleted file mode 100644
index 57589ed84..000000000
--- a/nuttx/configs/px4fmu/include/drv_eeprom.h
+++ /dev/null
@@ -1,60 +0,0 @@
-/****************************************************************************
- *
- * Copyright (C) 2012 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 drv_eeprom.h
- *
- * Config for the non-MTD EEPROM driver.
- */
-
-/* IMPORTANT: Adjust this number! */
-#define MAX_EEPROMS 2
-
-/* FMU onboard */
-#define FMU_BASEBOARD_EEPROM_ADDRESS 0x57
-#define FMU_BASEBOARD_EEPROM_TOTAL_SIZE_BYTES 128
-#define FMU_BASEBOARD_EEPROM_PAGE_SIZE_BYTES 8
-#define FMU_BASEBOARD_EEPROM_PAGE_WRITE_TIME_US 3300
-#define FMU_BASEBOARD_EEPROM_BUS_CLOCK 400000 ///< 400 KHz max. clock
-
-/**
- * @brief i2c I2C bus struct
- * @brief device_address The device address as stated in the datasheet, e.g. for a Microchip 24XX128 0x50 with all ID pins tied to GND
- * @brief total_size_bytes The total size in bytes, e.g. 16K = 16000 bytes for the Microchip 24XX128
- * @brief page_size_bytes The size of one page, e.g. 64 bytes for the Microchip 24XX128
- * @brief device_name The device name to register this device to, e.g. /dev/eeprom
- * @brief fail_if_missing Returns error if the EEPROM was not found. This is helpful if the EEPROM might be attached later when the board is running
- */
-extern int
-eeprom_attach(struct i2c_dev_s *i2c, uint8_t device_address, uint16_t total_size_bytes, uint16_t page_size_bytes, uint16_t page_write_time_us, const char* device_name, uint8_t fail_if_missing);
-
diff --git a/nuttx/configs/px4fmu/src/Makefile b/nuttx/configs/px4fmu/src/Makefile
index 13b26b84f..42c931112 100644
--- a/nuttx/configs/px4fmu/src/Makefile
+++ b/nuttx/configs/px4fmu/src/Makefile
@@ -41,7 +41,7 @@ ASRCS =
AOBJS = $(ASRCS:.S=$(OBJEXT))
CSRCS = up_leds.c \
- drv_led.c drv_eeprom.c
+ drv_led.c
COBJS = $(CSRCS:.c=$(OBJEXT))
diff --git a/nuttx/configs/px4fmu/src/drv_eeprom.c b/nuttx/configs/px4fmu/src/drv_eeprom.c
deleted file mode 100644
index c22062ec5..000000000
--- a/nuttx/configs/px4fmu/src/drv_eeprom.c
+++ /dev/null
@@ -1,522 +0,0 @@
-/*
- * Copyright (C) 2012 Lorenz Meier. 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 of the author or the names of 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.
- */
-
-/*
- * Generic driver for I2C EEPROMs with 8 bit or 16 bit addressing
- */
-
-#include <nuttx/config.h>
-
-#include <stdint.h>
-#include <stdbool.h>
-#include <debug.h>
-#include <errno.h>
-#include <string.h>
-#include <stdio.h>
-#include <unistd.h>
-
-#include <nuttx/i2c.h>
-#include <arch/board/board.h>
-
-#include "up_arch.h"
-#include "chip.h"
-#include "stm32_internal.h"
-#include "px4fmu-internal.h"
-
-#include <arch/board/drv_eeprom.h>
-
-/* Split I2C transfers into smaller chunks to make sure to stay within tight timeout limits */
-
-/* check defines */
-#ifndef MAX_EEPROMS
- #error MAX_EEPROMS number must be defined (1-3)
-#endif
-
-#if (MAX_EEPROMS > 3)
- #error Currently only a maximum of three EEPROMS is supported, add missing code around here: __FILE__:__LINE__
-#endif
-static int eeprom_open0(FAR struct file *filp);
-static int eeprom_close0(FAR struct file *filp);
-static ssize_t eeprom_read0(struct file *filp, FAR char *buffer, size_t buflen);
-static ssize_t eeprom_write0(struct file *filp, FAR const char *buffer, size_t buflen);
-static off_t eeprom_seek0(FAR struct file *filp, off_t offset, int whence);
-#if (MAX_EEPROMS > 1)
-static int eeprom_open1(FAR struct file *filp);
-static int eeprom_close1(FAR struct file *filp);
-static ssize_t eeprom_read1(struct file *filp, FAR char *buffer, size_t buflen);
-static ssize_t eeprom_write1(struct file *filp, FAR const char *buffer, size_t buflen);
-static off_t eeprom_seek1(FAR struct file *filp, off_t offset, int whence);
-#endif
-#if (MAX_EEPROMS > 2)
-static int eeprom_open2(FAR struct file *filp);
-static int eeprom_close2(FAR struct file *filp);
-static ssize_t eeprom_read2(struct file *filp, FAR char *buffer, size_t buflen);
-static ssize_t eeprom_write2(struct file *filp, FAR const char *buffer, size_t buflen);
-static off_t eeprom_seek2(FAR struct file *filp, off_t offset, int whence);
-#endif
-
-static const struct file_operations eeprom_fops[MAX_EEPROMS] = {{
- .open = eeprom_open0,
- .close = eeprom_close0,
- .read = eeprom_read0,
- .write = eeprom_write0,
- .seek = eeprom_seek0,
- .ioctl = 0,
-#ifndef CONFIG_DISABLE_POLL
- .poll = 0
-#endif
-}
-#if (MAX_EEPROMS > 1)
-,{
- .open = eeprom_open1,
- .close = eeprom_close1,
- .read = eeprom_read1,
- .write = eeprom_write1,
- .seek = eeprom_seek1,
-}
-#endif
-#if (MAX_EEPROMS > 2)
-,{
- .open = eeprom_open2,
- .close = eeprom_close2,
- .read = eeprom_read2,
- .write = eeprom_write2,
- .seek = eeprom_seek2,
-}
-#endif
-};
-
-static FAR struct eeprom_dev_s
-{
- struct i2c_dev_s *i2c;
- uint8_t eeprom_address;
- uint16_t eeprom_size_bytes;
- uint16_t eeprom_page_size_bytes;
- uint16_t eeprom_page_write_time;
- off_t offset;
- bool is_open;
-} eeprom_dev[MAX_EEPROMS];
-
-static int
-eeprom_open0(FAR struct file *filp)
-{
- /* only allow one open at a time */
- if (eeprom_dev[0].is_open) {
- errno = EBUSY;
- return -EBUSY;
- }
- /* reset pointer */
- //eeprom_dev[0].is_open = true;
- eeprom_dev[0].offset = 0;
- return OK;
-}
-#if (MAX_EEPROMS > 1)
-static int
-eeprom_open1(FAR struct file *filp)
-{
- /* only allow one open at a time */
- if (eeprom_dev[1].is_open) {
- errno = EBUSY;
- return -EBUSY;
- }
- /* reset pointer */
- //eeprom_dev[1].is_open = true;
- eeprom_dev[1].offset = 0;
- return OK;
-}
-#endif
-#if (MAX_EEPROMS > 2)
-static int
-eeprom_open2(FAR struct file *filp)
-{
- /* only allow one open at a time */
- if (eeprom_dev[2].is_open) {
- errno = EBUSY;
- return -EBUSY;
- }
- /* reset pointer */
- //eeprom_dev[2].is_open = true;
- eeprom_dev[2].offset = 0;
- return OK;
-}
-#endif
-
-static int
-eeprom_close0(FAR struct file *filp)
-{
- eeprom_dev[0].is_open = false;
- return OK;
-}
-#if (MAX_EEPROMS > 1)
-static int
-eeprom_close1(FAR struct file *filp)
-{
- eeprom_dev[1].is_open = false;
- return OK;
-}
-#endif
-#if (MAX_EEPROMS > 2)
-static int
-eeprom_close2(FAR struct file *filp)
-{
- eeprom_dev[2].is_open = false;
- return OK;
-}
-#endif
-
-static int
-eeprom_read_internal(int dev, uint16_t len, uint8_t *data)
-{
- /* abort if the number of requested bytes exceeds the EEPROM size */
- if (eeprom_dev[dev].offset + len > eeprom_dev[dev].eeprom_size_bytes)
- {
- errno = ENOSPC;
- return -ENOSPC;
- }
-
- /* set device address */
- I2C_SETADDRESS(eeprom_dev[dev].i2c, eeprom_dev[dev].eeprom_address, 7);
-
- uint8_t cmd[2] = {0, 0}; /* first (or only) part of address */
- /* second part of address, omitted if eeprom has 256 bytes or less */
- int ret = 0;
- int remaining = len;
- int readcounts = 0;
-
- while (remaining > 0)
- {
- /* read all requested bytes over potentially multiple pages */
- //int readlen = (remaining < eeprom_dev[dev].eeprom_page_size_bytes) ? remaining : eeprom_dev[dev].eeprom_page_size_bytes;
- int read_offset = eeprom_dev[dev].offset + len - remaining;//+ write_counts*eeprom_dev[dev].eeprom_page_size_bytes;
- /* set read length to page border */
- int readlen = eeprom_dev[dev].eeprom_page_size_bytes - (read_offset % eeprom_dev[dev].eeprom_page_size_bytes);//(remaining < eeprom_dev[dev].eeprom_page_size_bytes) ? remaining : eeprom_dev[dev].eeprom_page_size_bytes;
- /* cap read length if not a full page read is needed */
- if (readlen > remaining) readlen = remaining;
-
- if (eeprom_dev[dev].eeprom_size_bytes <= 256)
- {
- cmd[0] = (read_offset); /* set at first byte */
- /* 8 bit addresses */
- ret = I2C_WRITEREAD(eeprom_dev[dev].i2c, cmd, 1, (data+(readcounts*eeprom_dev[dev].eeprom_page_size_bytes)), readlen);
- }
- else
- {
- /* 16 bit addresses */
- /* EEPROM: first address high, then address low */
- cmd[0] = (((uint16_t)read_offset) >> 8);
- cmd[1] = (((uint8_t)read_offset));
- ret = I2C_WRITEREAD(eeprom_dev[dev].i2c, cmd, 2, (data+(readcounts*eeprom_dev[dev].eeprom_page_size_bytes)), readlen);
- }
-
- /* abort on error */
- if (ret < 0) break;
-
- /* handled another chunk */
- remaining -= readlen;
- readcounts++;
- }
-
- /* use the negated value from I2C_TRANSFER to fill errno */
- errno = -ret;
-
- /* return len if data was read, < 0 else */
- if (ret == OK)
- eeprom_dev[dev].offset += len;
- return len;
-
- /* no data, return negated value from I2C_TRANSFER */
- return ret;
-}
-
-static int
-eeprom_write_internal(int dev, uint16_t len, const uint8_t *data)
-{
- /* abort if the number of requested bytes exceeds the EEPROM size */
- if (eeprom_dev[dev].offset + len > eeprom_dev[dev].eeprom_size_bytes)
- {
- errno = ENOSPC;
- return -ENOSPC;
- }
-
- int ret = 0;
- int remaining = len;
- int write_counts = 0;
-
- uint8_t write_buf[2];
-
- while (remaining > 0)
- {
- /* write all requested bytes over potentially multiple pages */
- int write_offset = eeprom_dev[dev].offset + len - remaining;//+ write_counts*eeprom_dev[dev].eeprom_page_size_bytes;
- /* set write length to page border */
- int writelen = eeprom_dev[dev].eeprom_page_size_bytes - (write_offset % eeprom_dev[dev].eeprom_page_size_bytes);//(remaining < eeprom_dev[dev].eeprom_page_size_bytes) ? remaining : eeprom_dev[dev].eeprom_page_size_bytes;
- /* cap write length if not a full page write is requested */
- if (writelen > remaining) writelen = remaining;
-
- if (eeprom_dev[dev].eeprom_size_bytes <= 256)
- {
- write_buf[0] = (write_offset); /* set at first byte */
- /* 8 bit addresses */
-
- const uint8_t* data_ptr = (data+(write_offset));
-
- struct i2c_msg_s msgv_eeprom_write[2] = {
- {
- .addr = eeprom_dev[dev].eeprom_address,
- .flags = I2C_M_NORESTART,
- .buffer = write_buf,
- .length = 1
- },
- {
- .addr = eeprom_dev[dev].eeprom_address,
- .flags = I2C_M_NORESTART,
- .buffer = (uint8_t*)data_ptr,
- .length = writelen
- }
- };
-
-
- if ( (ret = I2C_TRANSFER(eeprom_dev[dev].i2c, msgv_eeprom_write, 2)) == OK )
- {
- //printf("SUCCESS WRITING EEPROM 8BIT ADDR: %d, bytes: %d\n", ret, writelen);
- }
- }
- else
- {
- /* 16 bit addresses */
- /* EEPROM: first address high, then address low */
- write_buf[0] = (((uint16_t)write_offset) >> 8);
- write_buf[1] = (((uint8_t)write_offset));
-
- const uint8_t* data_ptr = data+(write_counts*eeprom_dev[dev].eeprom_page_size_bytes);
-
- struct i2c_msg_s msgv_eeprom_write[2] = {
- {
- .addr = eeprom_dev[dev].eeprom_address,
- .flags = I2C_M_NORESTART,
- .buffer = write_buf,
- .length = 2
- },
- {
- .addr = eeprom_dev[dev].eeprom_address,
- .flags = I2C_M_NORESTART,
- .buffer = (uint8_t*)data_ptr,
- .length = writelen
- }
- };
-
-
- if ( (ret = I2C_TRANSFER(eeprom_dev[dev].i2c, msgv_eeprom_write, 2)) == OK )
- {
- //printf("SUCCESS WRITING EEPROM 16BIT ADDR: %d, bytes: %d\n", ret, writelen);
- }
- }
-
- /* abort on error */
- if (ret < 0) break;
-
- /* handled another chunk */
- remaining -= writelen;
- write_counts++;
- /* wait for the device to write the page */
- usleep(eeprom_dev[dev].eeprom_page_write_time);
- }
-
- /* use the negated value from I2C_TRANSFER to fill errno */
- errno = -ret;
-
- /* return length if data was written, < 0 else */
- if (ret == OK)
- eeprom_dev[dev].offset += len;
- return len;
-
- /* no data, return negated value from I2C_TRANSFER */
- return ret;
-}
-
-static ssize_t
-eeprom_read0(struct file *filp, char *buffer, size_t buflen)
-{
- return eeprom_read_internal(0, buflen, (uint8_t *)buffer);
-}
-#if (MAX_EEPROMS > 1)
-static ssize_t
-eeprom_read1(struct file *filp, char *buffer, size_t buflen)
-{
- return eeprom_read_internal(1, buflen, (uint8_t *)buffer);
-}
-#endif
-#if (MAX_EEPROMS > 2)
-static ssize_t
-eeprom_read2(struct file *filp, char *buffer, size_t buflen)
-{
- return eeprom_read_internal(2, buflen, (uint8_t *)buffer);
-}
-#endif
-
-static ssize_t
-eeprom_write0(struct file *filp, const char *buffer, size_t buflen)
-{
- return eeprom_write_internal(0, buflen, (const uint8_t *)buffer);
-}
-#if (MAX_EEPROMS > 1)
-static ssize_t
-eeprom_write1(struct file *filp, const char *buffer, size_t buflen)
-{
- return eeprom_write_internal(1, buflen, (const uint8_t *)buffer);
-}
-#endif
-#if (MAX_EEPROMS > 2)
-static ssize_t
-eeprom_write2(struct file *filp, const char *buffer, size_t buflen)
-{
- return eeprom_write_internal(2, buflen, (const uint8_t *)buffer);
-}
-#endif
-
-static off_t eeprom_seek0(FAR struct file *filp, off_t offset, int whence)
-{
- switch (whence)
- {
- case SEEK_SET:
- if (offset < eeprom_dev[0].eeprom_size_bytes - 1) {
- eeprom_dev[0].offset = offset;
- } else {
- errno = ESPIPE;
- return -ESPIPE;
- }
- break;
- case SEEK_CUR:
- if (eeprom_dev[0].offset + offset < eeprom_dev[0].eeprom_size_bytes - 1) {
- eeprom_dev[0].offset = eeprom_dev[0].offset + offset;
- } else {
- errno = ESPIPE;
- return -ESPIPE;
- }
- break;
- case SEEK_END:
- errno = ESPIPE;
- return -ESPIPE;
- break;
- }
- return eeprom_dev[0].offset;
-}
-#if (MAX_EEPROMS > 1)
-static off_t eeprom_seek1(FAR struct file *filp, off_t offset, int whence)
-{
- switch (whence)
- {
- case SEEK_SET:
- if (offset < eeprom_dev[1].eeprom_size_bytes - 1) {
- eeprom_dev[1].offset = offset;
- } else {
- errno = ESPIPE;
- return -ESPIPE;
- }
- break;
- case SEEK_CUR:
- if (eeprom_dev[1].offset + offset < eeprom_dev[1].eeprom_size_bytes - 1) {
- eeprom_dev[1].offset = eeprom_dev[1].offset + offset;
- } else {
- errno = ESPIPE;
- return -ESPIPE;
- }
- break;
- case SEEK_END:
- errno = ESPIPE;
- return -ESPIPE;
- break;
- }
- return eeprom_dev[1].offset;
-}
-#endif
-#if (MAX_EEPROMS > 2)
-static off_t eeprom_seek2(FAR struct file *filp, off_t offset, int whence)
-{
- switch (whence)
- {
- case SEEK_SET:
- if (offset < eeprom_dev[2].eeprom_size_bytes - 1) {
- eeprom_dev[2].offset = offset;
- } else {
- errno = ESPIPE;
- return -ESPIPE;
- }
- break;
- case SEEK_CUR:
- if (eeprom_dev[2].offset + offset < eeprom_dev[2].eeprom_size_bytes - 1) {
- eeprom_dev[2].offset = eeprom_dev[2].offset + offset;
- } else {
- errno = ESPIPE;
- return -ESPIPE;
- }
- break;
- case SEEK_END:
- errno = ESPIPE;
- return -ESPIPE;
- break;
- }
- return eeprom_dev[2].offset;
-}
-#endif
-
-int
-eeprom_attach(struct i2c_dev_s *i2c, uint8_t device_address, uint16_t total_size_bytes, uint16_t page_size_bytes, uint16_t page_write_time_us, const char* device_name, uint8_t fail_if_missing)
-{
- static int eeprom_dev_counter = 0;
- eeprom_dev[eeprom_dev_counter].i2c = i2c;
- eeprom_dev[eeprom_dev_counter].eeprom_address = device_address;
- eeprom_dev[eeprom_dev_counter].eeprom_size_bytes = total_size_bytes;
- eeprom_dev[eeprom_dev_counter].eeprom_page_size_bytes = page_size_bytes;
- eeprom_dev[eeprom_dev_counter].eeprom_page_write_time = page_write_time_us;
- eeprom_dev[eeprom_dev_counter].offset = 0;
- eeprom_dev[eeprom_dev_counter].is_open = false;
-
- int ret;
-
- if (fail_if_missing) {
- /* read first value */
- uint8_t read_test;
- ret = (eeprom_read_internal(eeprom_dev_counter, 1, &read_test) == 1) ? OK : ERROR;
- } else {
- ret = OK;
- }
-
- /* make ourselves available */
- if (ret == OK)
- {
- register_driver(device_name, &(eeprom_fops[eeprom_dev_counter]), 0666, NULL);
- eeprom_dev_counter++;
- }
-
- /* Return 0 for device found, error number else */
- return ret;
-}