bool
platformNvramWrite16( void * prAdapter,unsigned char ucWordOffset,unsigned short u2Data )
{
if( nvram_write( WIFI_NVRAM_FILE_NAME, (char *)&u2Data, sizeof(unsigned short), ucWordOffset*sizeof(unsigned short)) != sizeof(unsigned short) )
return false;
else
return true;
}
bool
customDataWrite8( void * prAdapter,unsigned char ucByteOffset,unsigned char ucData )
{
if( nvram_write( WIFI_NVRAM_CUSTOM_NAME, (char *)&ucData, sizeof(unsigned char), ucByteOffset*sizeof(unsigned char)) != sizeof(unsigned char) )
return false;
else
return true;
}
int env_save(void)
{
int size;
unsigned char *buffer;
unsigned char *buffer_end;
unsigned char *ptr;
queue_t *qb;
cfe_envvar_t *env;
int namelen;
int valuelen;
int flg;
flg = nvram_open();
if (flg < 0) return flg;
nvram_erase();
size = nvram_getsize();
buffer = KMALLOC(size,0);
if (buffer == NULL) return CFE_ERR_NOMEM;
buffer_end = buffer + size;
ptr = buffer;
for (qb = env_envvars.q_next; qb != &env_envvars; qb = qb->q_next) {
env = (cfe_envvar_t *) qb;
if (env->flags & (ENV_FLG_BUILTIN)) continue;
namelen = strlen(env->name);
valuelen = strlen(env->value);
if ((ptr + 2 + namelen + valuelen + 1 + 1 + 1) > buffer_end) break;
*ptr++ = ENV_TLV_TYPE_ENV; /* TLV record type */
*ptr++ = (namelen + valuelen + 1 + 1); /* TLV record length */
*ptr++ = (unsigned char)env->flags;
memcpy(ptr,env->name,namelen); /* TLV record data */
ptr += namelen;
*ptr++ = '=';
memcpy(ptr,env->value,valuelen);
ptr += valuelen;
}
*ptr++ = ENV_TLV_TYPE_END;
size = nvram_write(buffer,0,ptr-buffer);
KFREE(buffer);
nvram_close();
return (size == (ptr-buffer)) ? 0 : CFE_ERR_IOERR;
}
/**
* @brief Calibrate magnetometer
*
* This function measures the magnetometer output if 3 different device
* orientations, calculates average offset values, and stores these offsets
* in non-volatile memory. The offsets will later be used during normal
* measurements, to compensate for fixed magnetic effects.
*
* This routine must be called 3 times total, with the "step" parameter
* indicating what stage of the calibration is being performed. This
* multi-step mechanism allows the application to prompt for physical
* placement of the sensor device before this routine is called.
*
* @param sensor Address of an initialized sensor device descriptor.
* @param calib_type The address of a vector storing sensor axis data.
* @param step The calibration stage number [1,3].
* @param info Unimplemented (ignored) parameter.
* @return bool true if the call succeeds, else false is returned.
*/
static bool ak8975_calibrate(sensor_t *sensor,
sensor_calibration_t calib_type, int step, void *info)
{
sensor_hal_t *const hal = sensor->hal;
static vector3_t step_data [3];
/* Validate the supported calibration types and step number. */
if ((calib_type != MANUAL_CALIBRATE) || ((step < 1) || (step > 3))) {
return false;
}
/* Read sensor data and test for data overflow. */
vector3_t *const ptr_step_data = &step_data[ step - 1 ];
if ((!ak8975_get_data(hal, AK8975_SINGLE_MODE, ptr_step_data)) ||
ak8975_check_overflow(ptr_step_data)) {
return false;
}
switch (step) {
/* There's nothing to do on the first two passes. */
case 1:
case 2:
break;
/* Calculate & update the calibrated offsets on the final pass. */
case 3:
calibrated_offsets.x = (step_data[0].x + step_data[1].x) / 2;
calibrated_offsets.y = (step_data[0].y + step_data[1].y) / 2;
calibrated_offsets.z = (step_data[1].z + step_data[2].z) / 2;
/* Write the calibration data then read it back and confirm it
* was written correctly
*/
nvram_write(0, &calibrated_offsets, sizeof(calibrated_offsets));
vector3_t read_back;
nvram_read(0, &read_back, sizeof(vector3_t));
if (memcmp(&calibrated_offsets, &read_back,
sizeof(vector3_t))) {
sensor->err = SENSOR_ERR_IO;
return false;
}
break;
/* Any other step number is invalid */
default:
sensor->err = SENSOR_ERR_PARAMS;
return false;
}
return true;
}
/**
* @brief Calibrate proximity sensor detection thresholds
*
* This function measures the proximity sensor output in 3 different steps,
* one for each channel in the device. This function should be called when
* a sample object has been placed at the desired distance from the device
* to define the threshold for near-proximity detection. The measured
* proximity sensor value for each channel is stored in non-volatile memory.
* These thresholds will later be used to set the threshold during the
* device initialization sequence.
*
* This routine must be called 3 times total, with the "step" parameter
* indicating what stage of the calibration is being performed (i.e. which
* channel of the proximity sensor). This multi-step mechanism allows
* the application to prompt for physical placement of the object to be
* detected before this routine is called.
*
* @param sensor Address of an initialized sensor device descriptor.
* @param calib_type The address of a vector storing sensor axis data.
* @param step The calibration stage number [1,3].
* @param info Unimplemented (ignored) parameter.
* @return bool true if the call succeeds, else false is returned.
*/
static bool sfh7770_calibrate(sensor_t *sensor,
sensor_calibration_t calib_type, int step, void *info)
{
sensor_hal_t *const hal = sensor->hal;
static uint8_t prox_data[3];
uint8_t read_data[3];
/* Validate the specified calibration type */
if (calib_type != MANUAL_CALIBRATE) {
sensor->err = SENSOR_ERR_PARAMS;
return false;
}
/* Read proximity sensor for individual channel based on step number. */
switch (step) {
case 1:
prox_data[0] = sensor_bus_get(hal, SFH7770_PS_DATA_LED1);
break;
case 2:
prox_data[1] = sensor_bus_get(hal, SFH7770_PS_DATA_LED2);
break;
case 3:
prox_data[2] = sensor_bus_get(hal, SFH7770_PS_DATA_LED3);
/* Write data */
nvram_write((SFH7770_NVRAM_OFFSET), prox_data,
sizeof(prox_data));
/* Read back data and confirm it was written correctly */
nvram_read(SFH7770_NVRAM_OFFSET, read_data, sizeof(read_data));
if (memcmp(prox_data, read_data, sizeof(prox_data))) {
sensor->err = SENSOR_ERR_IO;
return false;
}
/* Apply stored proximity thresholds from nvram */
sensor_bus_write(hal, (SFH7770_PS_THR_LED1), read_data,
sizeof(read_data));
break;
/* Any other step number is invalid */
default:
sensor->err = SENSOR_ERR_PARAMS;
return false;
}
return true;
}
/* offs buf length -- status actlen */
static int
rc_nvram_store( ulong args[], ulong ret[] )
{
/* printm("nvram-store %04lx %08lX %ld\n", args[0], args[1], args[2] ); */
ret[1] = nvram_write( args[0], (char*)args[1], args[2] );
ret[0] = 0;
if( ret[1] != args[2] ){
printm("---> nvram_store parameter error\n");
ret[0] = -3; /* parameter error */
}
return 0;
}
int main(void)
{
uint32_t pos, len;
board_init();
nvram_init();
LED_On(LED0);
printk("Writing firmware data to flash\n");
pos = 0;
while (pos < fw_len) {
if (fw_len - pos > SECTOR_SIZE)
len = SECTOR_SIZE;
else
len = fw_len - pos;
nvram_write(pos, fw_buf + pos, len);
pos += len;
}
LED_Off(LED0);
printk("Verifying firmware data\n");
pos = 0;
while (pos < fw_len) {
static uint8_t page_buf[SECTOR_SIZE];
uint32_t i;
if (fw_len - pos > SECTOR_SIZE)
len = SECTOR_SIZE;
else
len = fw_len - pos;
nvram_read(pos, page_buf, len);
for (i = 0; i < len; i++)
if (*(page_buf + i) != *(fw_buf + pos + i)) {
printk("Verify failed at byte %d, 0x%02x != 0x%02x\n",
pos + i, *(page_buf + i), *(fw_buf + pos + i));
return 0;
}
pos += len;
}
LED_On(LED0);
printk("Firmware successfully stored in flash!\n");
return 0;
}
/*----------------------------------------------------------------------------*/
BOOLEAN
kalCfgDataWrite16(
IN P_GLUE_INFO_T prGlueInfo,
UINT_32 u4Offset,
UINT_16 u2Data
)
{
if(nvram_write(WIFI_NVRAM_FILE_NAME,
(char *)&u2Data,
sizeof(unsigned short),
u4Offset) != sizeof(unsigned short)) {
return FALSE;
}
else {
return TRUE;
}
}
int saveenv(void)
{
env_t env_new;
int rcode = 0;
rcode = env_export(&env_new);
if (rcode)
return rcode;
#ifdef CONFIG_SYS_NVRAM_ACCESS_ROUTINE
nvram_write(CONFIG_ENV_ADDR, &env_new, CONFIG_ENV_SIZE);
#else
if (memcpy((char *)CONFIG_ENV_ADDR, &env_new, CONFIG_ENV_SIZE) == NULL)
rcode = 1;
#endif
return rcode;
}
int saveenv (void)
{
int rcode = 0;
#ifdef CONFIG_AMIGAONEG3SE
enable_nvram();
#endif
#ifdef CFG_NVRAM_ACCESS_ROUTINE
nvram_write(CFG_ENV_ADDR, env_ptr, CFG_ENV_SIZE);
#else
if (memcpy ((char *)CFG_ENV_ADDR, env_ptr, CFG_ENV_SIZE) == NULL)
rcode = 1 ;
#endif
#ifdef CONFIG_AMIGAONEG3SE
udelay(10000);
disable_nvram();
#endif
return rcode;
}
int saveenv(void)
{
env_t env_new;
ssize_t len;
char *res;
int rcode = 0;
res = (char *)&env_new.data;
len = hexport('\0', &res, ENV_SIZE);
if (len < 0) {
error("Cannot export environment: errno = %d\n", errno);
return 1;
}
env_new.crc = crc32(0, env_new.data, ENV_SIZE);
#ifdef CONFIG_SYS_NVRAM_ACCESS_ROUTINE
nvram_write(CONFIG_ENV_ADDR, &env_new, CONFIG_ENV_SIZE);
#else
if (memcpy((char *)CONFIG_ENV_ADDR, &env_new, CONFIG_ENV_SIZE) == NULL)
rcode = 1;
#endif
return rcode;
}
int saveenv(void)
{
#ifdef CONFIG_SYS_NVRAM_ACCESS_ROUTINE
env_t *env_new = (env_t *)env_buf;
#else
env_t *env_new = (env_t *)CONFIG_ENV_ADDR;
#endif
ssize_t len;
char *res;
int rcode = 0;
res = (char *)env_new->data;
len = hexport_r(&env_htab, '\0', 0, &res, ENV_SIZE, 0, NULL);
if (len < 0) {
error("Cannot export environment: errno = %d\n", errno);
return 1;
}
env_new->crc = crc32(0, env_new->data, ENV_SIZE);
#ifdef CONFIG_SYS_NVRAM_ACCESS_ROUTINE
nvram_write(CONFIG_ENV_ADDR, env_new, CONFIG_ENV_SIZE);
#endif
return rcode;
}
/**
* @brief Calibrate magnetometer
*
* This function measures the magnetometer output if 3 different device
* orientations, calculates average offset values, and stores these offsets
* in non-volatile memory. The offsets will later be used during normal
* measurements, to compensate for fixed magnetic effects.
*
* This routine must be called 3 times total, with the "step" parameter
* indicating what stage of the calibration is being performed. This
* multi-step mechanism allows the application to prompt for physical
* placement of the sensor device before this routine is called.
*
* @param sensor Address of an initialized sensor descriptor.
* @param data The address of a vector storing sensor axis data.
* @param step The calibration stage number (1 to 3).
* @param info Unimplemented (ignored) parameter.
* @return bool true if the call succeeds, else false is returned.
*/
bool hmc5883l_calibrate(sensor_t *sensor, sensor_calibration_t calib_type,
int step, void *info)
{
static vector3_t step_data[3]; /* sensor readings during calibration */
vector3_t dummy_data; /* data from first sensor read (ignored) */
vector3_t read_back; /* data read back from nvram to validate */
sensor_data_t test_data; /* readings during self test */
int test_code; /* self-test code & result */
sensor_hal_t *const hal = sensor->hal;
/* Validate the supported calibration types and step number. */
if ((calib_type != MANUAL_CALIBRATE) || ((step < 1) || (step > 3))) {
return false;
}
/* During first pass, use self-test to determine sensitivity scaling */
if (step == 1) {
/* Run internal self test with known bias field */
test_code = SENSOR_TEST_BIAS_POS;
if ((hmc5883l_selftest(sensor, &test_code,
&test_data) == false) ||
(test_code != SENSOR_TEST_ERR_NONE)) {
return false;
}
/* Calculate & store sensitivity adjustment values */
cal_data.sensitivity.x
= ((scalar_t)HMC5883L_TEST_X_NORM / test_data.axis.x);
cal_data.sensitivity.z
= ((scalar_t)HMC5883L_TEST_Z_NORM / test_data.axis.z);
cal_data.sensitivity.y
= ((scalar_t)HMC5883L_TEST_Y_NORM / test_data.axis.y);
nvram_write(CAL_SENSITIVITY_ADDR, &cal_data.sensitivity,
sizeof(cal_data.sensitivity));
/* Read back data and confirm it was written correctly */
nvram_read(CAL_SENSITIVITY_ADDR, &read_back, sizeof(vector3_t));
if (memcmp(&cal_data.sensitivity, &read_back,
sizeof(vector3_t))) {
sensor->err = SENSOR_ERR_IO;
return false;
}
}
/* Read sensor data and test for data overflow.
* Note: Sensor must be read twice - the first reading may
* contain stale data from previous orientation.
*/
if (hmc5883l_get_data(hal, &dummy_data) != true) {
return false;
}
delay_ms(READ_DELAY_MSEC);
if (hmc5883l_get_data(hal, &(step_data [step - 1])) != true) {
return false;
}
/* Apply sensitivity scaling factors */
hmc5883l_apply_sensitivity(&(step_data [step - 1]));
switch (step) {
/* There's nothing more to do on the first two passes. */
case 1:
case 2:
break;
/* Calculate & store the offsets on the final pass. */
case 3:
cal_data.offsets.x = (step_data[0].x + step_data[1].x) / 2;
cal_data.offsets.y = (step_data[0].y + step_data[1].y) / 2;
cal_data.offsets.z = (step_data[1].z + step_data[2].z) / 2;
nvram_write(CAL_OFFSETS_ADDR, &cal_data.offsets,
sizeof(cal_data.offsets));
/* Read back data and confirm it was written correctly */
nvram_read(0, &read_back, sizeof(vector3_t));
if (memcmp(&cal_data.offsets, &read_back, sizeof(vector3_t))) {
sensor->err = SENSOR_ERR_IO;
return false;
}
break;
default:
return false; /* bad step value */
}
return true;
}
static void NVRAM_set_byte(Nvram *nvram, uint32_t addr, uint8_t value)
{
nvram_write(nvram, addr, value);
}
/**
* @brief Set event threshold value
*
* @param hal Address of an initialized sensor hardware descriptor.
* @param channel Channel (LED) number to set threshold
* @param threshold Address of threshold descriptor.
* @return bool true if the call succeeds, else false is returned.
*/
static bool sfh7770_set_threshold(sensor_hal_t *hal, int16_t channel,
sensor_threshold_desc_t *threshold)
{
struct {
uint8_t lsb;
uint8_t msb;
}
reg_thresh;
uint8_t led_count = 0;
uint8_t index;
uint8_t value = threshold->value;
bool result = false;
switch (threshold->type) { /* check threshold type */
case SENSOR_THRESHOLD_NEAR_PROXIMITY: /* high (near) prox. threshold
**/
/* Write to sensor register based on current channel (LED
* selection)
*/
switch (channel) {
case SENSOR_CHANNEL_ALL: /* "channel -1" = all 3 LEDs */
led_count = 3;
index = 0;
break;
case 1:
case 2:
case 3:
led_count = 1;
index = channel - 1;
break;
default:
return false; /* invalid channel selection */
}
while (led_count--) {
sensor_bus_put(hal, (SFH7770_PS_THR_LED1 + index),
(uint8_t)threshold->value);
/* Write to nvram */
nvram_write((SFH7770_NVRAM_OFFSET + index), &value, 1);
++index;
}
result = true;
break;
case SENSOR_THRESHOLD_LOW_LIGHT: /* lower light level threshold
**/
reg_thresh.lsb = (uint8_t)(threshold->value & 0xFF);
reg_thresh.msb = (uint8_t)((threshold->value >> 8) & 0xFF);
low_light_threshold = (uint16_t)threshold->value;
if (sensor_bus_write(hal, SFH7770_ALS_LO_THR_LSB, ®_thresh,
sizeof(reg_thresh)) == sizeof(reg_thresh)) {
result = true;
}
break;
case SENSOR_THRESHOLD_HIGH_LIGHT: /* upper light level threshold
**/
reg_thresh.lsb = (uint8_t)(threshold->value & 0xFF);
reg_thresh.msb = (uint8_t)((threshold->value >> 8) & 0xFF);
high_light_threshold = (uint16_t)threshold->value;
if (sensor_bus_write(hal, SFH7770_ALS_UP_THR_LSB, ®_thresh,
sizeof(reg_thresh)) == sizeof(reg_thresh)) {
result = true;
}
break;
default:
/* Invalid/unsupported threshold type - do nothing, return false */
break;
}
return result;
}
