int pthread_getschedparam(pthread_t thread, FAR int *policy, FAR struct sched_param *param) { int ret; sdbg("Thread ID=%d policy=0x%p param=0x%p\n", thread, policy, param); if (!policy || !param) { ret = EINVAL; } else { /* Get the schedparams of the thread. */ ret = sched_getparam((pid_t)thread, param); if (ret != OK) { ret = EINVAL; } /* Return the policy. */ *policy = sched_getscheduler((pid_t)thread); if (*policy == ERROR) { ret = *get_errno_ptr(); } } sdbg("Returning %d\n", ret); return ret; }
int sem_trywait(FAR sem_t *sem) { FAR _TCB *rtcb = (FAR _TCB*)g_readytorun.head; irqstate_t saved_state; int ret = ERROR; /* This API should not be called from interrupt handlers */ DEBUGASSERT(up_interrupt_context() == false) /* Assume any errors reported are due to invalid arguments. */ *get_errno_ptr() = EINVAL; if (sem) { /* The following operations must be performed with interrupts * disabled because sem_post() may be called from an interrupt * handler. */ saved_state = irqsave(); /* Any further errors could only be occurred because the semaphore * is not available. */ *get_errno_ptr() = EAGAIN; /* If the semaphore is available, give it to the requesting task */ if (sem->semcount > 0) { /* It is, let the task take the semaphore */ sem->semcount--; rtcb->waitsem = NULL; ret = OK; } /* Interrupts may now be enabled. */ irqrestore(saved_state); } return ret; }
int timer_delete(timer_t timerid) { int ret = timer_release((FAR struct posix_timer_s *)timerid); if (ret < 0) { *get_errno_ptr() = -ret; return ERROR; } return OK; }
int usrsock_setsockopt(FAR struct usrsock_conn_s *conn, int level, int option, FAR const void *value, FAR socklen_t value_len) { struct usrsock_reqstate_s state = {}; net_lock_t save; ssize_t ret; DEBUGASSERT(conn); save = net_lock(); if (conn->state == USRSOCK_CONN_STATE_UNINITIALIZED || conn->state == USRSOCK_CONN_STATE_ABORTED) { /* Invalid state or closed by daemon. */ nlldbg("usockid=%d; connect() with uninitialized usrsock.\n", conn->usockid); ret = (conn->state == USRSOCK_CONN_STATE_ABORTED) ? -EPIPE : -ECONNRESET; goto errout_unlock; } /* Set up event callback for usrsock. */ ret = usrsock_setup_request_callback(conn, &state, setsockopt_event, USRSOCK_EVENT_ABORT | USRSOCK_EVENT_REQ_COMPLETE); if (ret < 0) { ndbg("usrsock_setup_request_callback failed: %d\n", ret); goto errout_unlock; } /* Request user-space daemon to close socket. */ ret = do_setsockopt_request(conn, level, option, value, value_len); if (ret >= 0) { /* Wait for completion of request. */ while (net_lockedwait(&state.recvsem) != OK) { DEBUGASSERT(*get_errno_ptr() == EINTR); } ret = state.result; } usrsock_teardown_request_callback(&state); errout_unlock: net_unlock(save); return ret; }
int rmdir(FAR const char *pathname) { FAR struct inode *inode; const char *relpath = NULL; int ret; /* Get an inode for this file */ inode = inode_find(pathname, &relpath); if (!inode) { /* There is no mountpoint that includes in this path */ ret = ENOENT; goto errout; } /* Verify that the inode is a valid mountpoint. */ if (!INODE_IS_MOUNTPT(inode) || !inode->u.i_mops) { ret = ENXIO; goto errout_with_inode; } /* Perform the rmdir operation using the relative path * at the mountpoint. */ if (inode->u.i_mops->rmdir) { ret = inode->u.i_mops->rmdir(inode, relpath); if (ret < 0) { ret = -ret; goto errout_with_inode; } } else { ret = ENOSYS; goto errout_with_inode; } /* Successfully removed the directory */ inode_release(inode); return OK; errout_with_inode: inode_release(inode); errout: *get_errno_ptr() = ret; return ERROR; }
static void uart_takesem(FAR sem_t *sem) { while (sem_wait(sem) != 0) { /* The only case that an error should occur here is if * the wait was awakened by a signal. */ ASSERT(*get_errno_ptr() == EINTR); } }
FAR char *getenv(const char *name) { FAR _TCB *rtcb; FAR environ_t *envp; FAR char *pvar; FAR char *pvalue = NULL; int ret = OK; /* Verify that a string was passed */ if (!name) { ret = EINVAL; goto errout; } /* Get a reference to the thread-private environ in the TCB.*/ sched_lock(); rtcb = (FAR _TCB*)g_readytorun.head; envp = rtcb->envp; /* Check if the variable exists */ if ( !envp || (pvar = env_findvar(envp, name)) == NULL) { ret = ENOENT; goto errout_with_lock; } /* It does! Get the value sub-string from the name=value string */ pvalue = strchr(pvar, '='); if (!pvalue) { /* The name=value string has no '=' This is a bug! */ ret = EINVAL; goto errout_with_lock; } /* Adjust the pointer so that it points to the value right after the '=' */ pvalue++; sched_unlock(); return pvalue; errout_with_lock: sched_unlock(); errout: *get_errno_ptr() = ret; return NULL; }
void nfs_semtake(struct nfsmount *nmp) { /* Take the semaphore (perhaps waiting) */ while (sem_wait(&nmp->nm_sem) != 0) { /* The only case that an error should occur here is if * the wait was awakened by a signal. */ ASSERT(*get_errno_ptr() == EINTR); } }
static inline void _uip_semtake(sem_t *sem) { /* Take the semaphore (perhaps waiting) */ while (uip_lockedwait(sem) != 0) { /* The only case that an error should occur here is if * the wait was awakened by a signal. */ ASSERT(*get_errno_ptr() == EINTR); } }
void smartfs_semtake(struct smartfs_mountpt_s *fs) { /* Take the semaphore (perhaps waiting) */ while (sem_wait(fs->fs_sem) != 0) { /* The only case that an error should occur here is if * the wait was awakened by a signal. */ ASSERT(*get_errno_ptr() == EINTR); } }
static void _net_semtake(FAR struct socketlist *list) { /* Take the semaphore (perhaps waiting) */ while (net_lockedwait(&list->sl_sem) != 0) { /* The only case that an error should occr here is if * the wait was awakened by a signal. */ ASSERT(*get_errno_ptr() == EINTR); } }
off_t telldir(FAR DIR *dirp) { struct fs_dirent_s *idir = (struct fs_dirent_s *)dirp; if (!idir || !idir->fd_root) { *get_errno_ptr() = EBADF; return (off_t)-1; } /* Just return the current position */ return idir->fd_position; }
static inline ssize_t file_write(int fd, FAR const void *buf, size_t nbytes) { FAR struct filelist *list; FAR struct file *this_file; FAR struct inode *inode; int ret; int err; /* Get the thread-specific file list */ list = sched_getfiles(); if (!list) { err = EMFILE; goto errout; } /* Was this file opened for write access? */ this_file = &list->fl_files[fd]; if ((this_file->f_oflags & O_WROK) == 0) { err = EBADF; goto errout; } /* Is a driver registered? Does it support the write method? */ inode = this_file->f_inode; if (!inode || !inode->u.i_ops || !inode->u.i_ops->write) { err = EBADF; goto errout; } /* Yes, then let the driver perform the write */ ret = inode->u.i_ops->write(this_file, buf, nbytes); if (ret < 0) { err = -ret; goto errout; } return ret; errout: *get_errno_ptr() = err; return ERROR; }
static int up_ioctl(FAR struct file *filep, int cmd, unsigned long arg) { *get_errno_ptr() = ENOTTY; return ERROR; }
static int fat_attrib(const char *path, fat_attrib_t *retattrib, fat_attrib_t setbits, fat_attrib_t clearbits) { struct fat_mountpt_s *fs; struct fat_dirinfo_s dirinfo; FAR struct inode *inode; const char *relpath = NULL; uint8_t *direntry; uint8_t oldattributes; uint8_t newattributes; int ret; /* Get an inode for this file */ inode = inode_find(path, &relpath); if (!inode) { /* There is no mountpoint that includes in this path */ ret = ENOENT; goto errout; } /* Verify that the inode is a valid mountpoint. */ if (!INODE_IS_MOUNTPT(inode) || !inode->u.i_mops || !inode->i_private) { ret = ENXIO; goto errout_with_inode; } /* Get the mountpoint private data from the inode structure */ fs = inode->i_private; /* Check if the mount is still healthy */ fat_semtake(fs); ret = fat_checkmount(fs); if (ret != OK) { goto errout_with_semaphore; } /* Find the file/directory entry for the oldrelpath */ ret = fat_finddirentry(fs, &dirinfo, relpath); if (ret != OK) { /* Some error occurred -- probably -ENOENT */ goto errout_with_semaphore; } /* Make sure that we found some valid file or directory */ if (dirinfo.fd_root) { /* Ooops.. we found the root directory */ ret = EACCES; goto errout_with_semaphore; } /* Get the current attributes */ direntry = &fs->fs_buffer[dirinfo.fd_seq.ds_offset]; oldattributes = DIR_GETATTRIBUTES(direntry); newattributes = oldattributes; /* Set or clear any bits as requested */ newattributes &= ~(clearbits & (FATATTR_READONLY|FATATTR_HIDDEN|FATATTR_SYSTEM|FATATTR_ARCHIVE)); newattributes |= (setbits & (FATATTR_READONLY|FATATTR_HIDDEN|FATATTR_SYSTEM|FATATTR_ARCHIVE)); /* Did any thingchange? */ if (newattributes != oldattributes) { DIR_PUTATTRIBUTES(direntry, newattributes); fs->fs_dirty = true; ret = fat_updatefsinfo(fs); if (ret != OK) { ret = -ret; goto errout_with_semaphore; } } /* Success */ if (retattrib) { *retattrib = newattributes; } fat_semgive(fs); inode_release(inode); return OK; errout_with_semaphore: fat_semgive(fs); errout_with_inode: inode_release(inode); errout: *get_errno_ptr() = ret; return ERROR; }
int sensors_main(int argc, char *argv[]) { /* inform about start */ printf("[sensors] Initializing..\n"); fflush(stdout); int ret = OK; /* start sensor reading */ if (sensors_init() != OK) { fprintf(stderr, "[sensors] ERROR: Failed to initialize all sensors\n"); /* Clean up */ close(fd_gyro); close(fd_accelerometer); close(fd_magnetometer); close(fd_barometer); close(fd_adc); fprintf(stderr, "[sensors] rebooting system.\n"); fflush(stderr); fflush(stdout); usleep(100000); /* Sensors are critical, immediately reboot system on failure */ reboot(); /* Not ever reaching here */ } else { /* flush stdout from init routine */ fflush(stdout); } bool gyro_healthy = false; bool acc_healthy = false; bool magn_healthy = false; bool baro_healthy = false; bool adc_healthy = false; bool hil_enabled = false; /**< HIL is disabled by default */ bool publishing = false; /**< the app is not publishing by default, only if HIL is disabled on first run */ int magcounter = 0; int barocounter = 0; int adccounter = 0; unsigned int mag_fail_count = 0; unsigned int mag_success_count = 0; unsigned int baro_fail_count = 0; unsigned int baro_success_count = 0; unsigned int gyro_fail_count = 0; unsigned int gyro_success_count = 0; unsigned int acc_fail_count = 0; unsigned int acc_success_count = 0; unsigned int adc_fail_count = 0; unsigned int adc_success_count = 0; ssize_t ret_gyro; ssize_t ret_accelerometer; ssize_t ret_magnetometer; ssize_t ret_barometer; ssize_t ret_adc; int nsamples_adc; int16_t buf_gyro[3]; int16_t buf_accelerometer[3]; int16_t buf_magnetometer[7]; float buf_barometer[3]; int16_t mag_offset[3] = {0, 0, 0}; int16_t acc_offset[3] = {0, 0, 0}; int16_t gyro_offset[3] = {0, 0, 0}; bool mag_calibration_enabled = false; #pragma pack(push,1) struct adc_msg4_s { uint8_t am_channel1; /**< The 8-bit ADC Channel 1 */ int32_t am_data1; /**< ADC convert result 1 (4 bytes) */ uint8_t am_channel2; /**< The 8-bit ADC Channel 2 */ int32_t am_data2; /**< ADC convert result 2 (4 bytes) */ uint8_t am_channel3; /**< The 8-bit ADC Channel 3 */ int32_t am_data3; /**< ADC convert result 3 (4 bytes) */ uint8_t am_channel4; /**< The 8-bit ADC Channel 4 */ int32_t am_data4; /**< ADC convert result 4 (4 bytes) */ }; #pragma pack(pop) struct adc_msg4_s buf_adc; size_t adc_readsize = 1 * sizeof(struct adc_msg4_s); float battery_voltage_conversion; battery_voltage_conversion = global_data_parameter_storage->pm.param_values[PARAM_BATTERYVOLTAGE_CONVERSION]; if (-1 == (int)battery_voltage_conversion) { /* default is conversion factor for the PX4IO / PX4IOAR board, the factor for PX4FMU standalone is different */ battery_voltage_conversion = 3.3f * 52.0f / 5.0f / 4095.0f; } #ifdef CONFIG_HRT_PPM int ppmcounter = 0; #endif /* initialize to 100 to execute immediately */ int paramcounter = 100; int excessive_readout_time_counter = 0; int read_loop_counter = 0; /* Empty sensor buffers, avoid junk values */ /* Read first two values of each sensor into void */ (void)read(fd_gyro, buf_gyro, sizeof(buf_gyro)); (void)read(fd_accelerometer, buf_accelerometer, sizeof(buf_accelerometer)); (void)read(fd_magnetometer, buf_magnetometer, sizeof(buf_magnetometer)); if (fd_barometer > 0)(void)read(fd_barometer, buf_barometer, sizeof(buf_barometer)); struct sensor_combined_s raw = { .timestamp = hrt_absolute_time(), .gyro_raw = {buf_gyro[0], buf_gyro[1], buf_gyro[2]}, .gyro_raw_counter = 0, .gyro_rad_s = {0, 0, 0}, .accelerometer_raw = {buf_accelerometer[0], buf_accelerometer[1], buf_accelerometer[2]}, .accelerometer_raw_counter = 0, .accelerometer_m_s2 = {0, 0, 0}, .magnetometer_raw = {buf_magnetometer[0], buf_magnetometer[1], buf_magnetometer[2]}, .magnetometer_raw_counter = 0, .baro_pres_mbar = 0, .baro_alt_meter = 0, .baro_temp_celcius = 0, .battery_voltage_v = BAT_VOL_INITIAL, .adc_voltage_v = {0, 0 , 0}, .baro_raw_counter = 0, .battery_voltage_counter = 0, .battery_voltage_valid = false, }; /* condition to wait for */ pthread_mutex_init(&sensors_read_ready_mutex, NULL); pthread_cond_init(&sensors_read_ready, NULL); /* advertise the topic and make the initial publication */ sensor_pub = orb_advertise(ORB_ID(sensor_combined), &raw); publishing = true; /* advertise the rc topic */ struct rc_channels_s rc; memset(&rc, 0, sizeof(rc)); int rc_pub = orb_advertise(ORB_ID(rc_channels), &rc); /* subscribe to system status */ struct vehicle_status_s vstatus; memset(&vstatus, 0, sizeof(vstatus)); int vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); printf("[sensors] rate: %u Hz\n", (unsigned int)(1000000 / SENSOR_INTERVAL_MICROSEC)); struct hrt_call sensors_hrt_call; /* Enable high resolution timer callback to unblock main thread, run after 2 ms */ hrt_call_every(&sensors_hrt_call, 2000, SENSOR_INTERVAL_MICROSEC, &sensors_timer_loop, NULL); while (1) { pthread_mutex_lock(&sensors_read_ready_mutex); struct timespec time_to_wait = {0, 0}; /* Wait 2 seconds until timeout */ time_to_wait.tv_nsec = 0; time_to_wait.tv_sec = time(NULL) + 2; if (pthread_cond_timedwait(&sensors_read_ready, &sensors_read_ready_mutex, &time_to_wait) == OK) { pthread_mutex_unlock(&sensors_read_ready_mutex); bool gyro_updated = false; bool acc_updated = false; bool magn_updated = false; bool baro_updated = false; bool adc_updated = false; /* store the time closest to all measurements */ uint64_t current_time = hrt_absolute_time(); raw.timestamp = current_time; if (paramcounter == 100) { // XXX paramcounter is not a good name, rename / restructure // XXX make counter ticks dependent on update rate of sensor main loop /* Check HIL state */ orb_copy(ORB_ID(vehicle_status), vstatus_sub, &vstatus); /* switching from non-HIL to HIL mode */ //printf("[sensors] Vehicle mode: %i \t AND: %i, HIL: %i\n", vstatus.mode, vstatus.mode & VEHICLE_MODE_FLAG_HIL_ENABLED, hil_enabled); if ((vstatus.mode & VEHICLE_MODE_FLAG_HIL_ENABLED) && !hil_enabled) { hil_enabled = true; publishing = false; int ret = close(sensor_pub); printf("[sensors] Closing sensor pub: %i \n", ret); /* switching from HIL to non-HIL mode */ } else if (!publishing && !hil_enabled) { /* advertise the topic and make the initial publication */ sensor_pub = orb_advertise(ORB_ID(sensor_combined), &raw); hil_enabled = false; publishing = true; } /* Update RC scalings and function mappings */ rc.chan[0].scaling_factor = (10000 / ((global_data_parameter_storage->pm.param_values[PARAM_RC1_MAX] - global_data_parameter_storage->pm.param_values[PARAM_RC1_MIN]) / 2) * global_data_parameter_storage->pm.param_values[PARAM_RC1_REV]); rc.chan[0].mid = (uint16_t)global_data_parameter_storage->pm.param_values[PARAM_RC1_TRIM]; rc.chan[1].scaling_factor = (10000 / ((global_data_parameter_storage->pm.param_values[PARAM_RC2_MAX] - global_data_parameter_storage->pm.param_values[PARAM_RC2_MIN]) / 2) * global_data_parameter_storage->pm.param_values[PARAM_RC2_REV]); rc.chan[1].mid = (uint16_t)global_data_parameter_storage->pm.param_values[PARAM_RC2_TRIM]; rc.chan[2].scaling_factor = (10000 / ((global_data_parameter_storage->pm.param_values[PARAM_RC3_MAX] - global_data_parameter_storage->pm.param_values[PARAM_RC3_MIN]) / 2) * global_data_parameter_storage->pm.param_values[PARAM_RC3_REV]); rc.chan[2].mid = (uint16_t)global_data_parameter_storage->pm.param_values[PARAM_RC3_TRIM]; rc.chan[3].scaling_factor = (10000 / ((global_data_parameter_storage->pm.param_values[PARAM_RC4_MAX] - global_data_parameter_storage->pm.param_values[PARAM_RC4_MIN]) / 2) * global_data_parameter_storage->pm.param_values[PARAM_RC4_REV]); rc.chan[3].mid = (uint16_t)global_data_parameter_storage->pm.param_values[PARAM_RC4_TRIM]; rc.chan[4].scaling_factor = (10000 / ((global_data_parameter_storage->pm.param_values[PARAM_RC5_MAX] - global_data_parameter_storage->pm.param_values[PARAM_RC5_MIN]) / 2) * global_data_parameter_storage->pm.param_values[PARAM_RC5_REV]); rc.chan[4].mid = (uint16_t)global_data_parameter_storage->pm.param_values[PARAM_RC5_TRIM]; rc.function[0] = global_data_parameter_storage->pm.param_values[PARAM_THROTTLE_CHAN] - 1; rc.function[1] = global_data_parameter_storage->pm.param_values[PARAM_ROLL_CHAN] - 1; rc.function[2] = global_data_parameter_storage->pm.param_values[PARAM_PITCH_CHAN] - 1; rc.function[3] = global_data_parameter_storage->pm.param_values[PARAM_YAW_CHAN] - 1; rc.function[4] = global_data_parameter_storage->pm.param_values[PARAM_OVERRIDE_CHAN] - 1; gyro_offset[0] = global_data_parameter_storage->pm.param_values[PARAM_SENSOR_GYRO_XOFFSET]; gyro_offset[1] = global_data_parameter_storage->pm.param_values[PARAM_SENSOR_GYRO_YOFFSET]; gyro_offset[2] = global_data_parameter_storage->pm.param_values[PARAM_SENSOR_GYRO_ZOFFSET]; mag_offset[0] = global_data_parameter_storage->pm.param_values[PARAM_SENSOR_MAG_XOFFSET]; mag_offset[1] = global_data_parameter_storage->pm.param_values[PARAM_SENSOR_MAG_YOFFSET]; mag_offset[2] = global_data_parameter_storage->pm.param_values[PARAM_SENSOR_MAG_ZOFFSET]; paramcounter = 0; } paramcounter++; /* try reading gyro */ uint64_t start_gyro = hrt_absolute_time(); ret_gyro = read(fd_gyro, buf_gyro, sizeof(buf_gyro)); int gyrotime = hrt_absolute_time() - start_gyro; if (gyrotime > 500) printf("GYRO (pure read): %d us\n", gyrotime); /* GYROSCOPE */ if (ret_gyro != sizeof(buf_gyro)) { gyro_fail_count++; if ((((gyro_fail_count % 20) == 0) || (gyro_fail_count > 20 && gyro_fail_count < 100)) && (int)*get_errno_ptr() != EAGAIN) { fprintf(stderr, "[sensors] L3GD20 ERROR #%d: %s\n", (int)*get_errno_ptr(), strerror((int)*get_errno_ptr())); } if (gyro_healthy && gyro_fail_count >= GYRO_HEALTH_COUNTER_LIMIT_ERROR) { // global_data_send_subsystem_info(&gyro_present_enabled); gyro_healthy = false; gyro_success_count = 0; } } else { gyro_success_count++; if (!gyro_healthy && gyro_success_count >= GYRO_HEALTH_COUNTER_LIMIT_OK) { // global_data_send_subsystem_info(&gyro_present_enabled_healthy); gyro_healthy = true; gyro_fail_count = 0; } gyro_updated = true; } gyrotime = hrt_absolute_time() - start_gyro; if (gyrotime > 500) printf("GYRO (complete): %d us\n", gyrotime); /* try reading acc */ uint64_t start_acc = hrt_absolute_time(); ret_accelerometer = read(fd_accelerometer, buf_accelerometer, sizeof(buf_accelerometer)); /* ACCELEROMETER */ if (ret_accelerometer != sizeof(buf_accelerometer)) { acc_fail_count++; if (acc_fail_count & 0b1000 || (acc_fail_count > 20 && acc_fail_count < 100)) { fprintf(stderr, "[sensors] BMA180 ERROR #%d: %s\n", (int)*get_errno_ptr(), strerror((int)*get_errno_ptr())); } if (acc_healthy && acc_fail_count >= ACC_HEALTH_COUNTER_LIMIT_ERROR) { // global_data_send_subsystem_info(&acc_present_enabled); gyro_healthy = false; acc_success_count = 0; } } else { acc_success_count++; if (!acc_healthy && acc_success_count >= ACC_HEALTH_COUNTER_LIMIT_OK) { // global_data_send_subsystem_info(&acc_present_enabled_healthy); acc_healthy = true; acc_fail_count = 0; } acc_updated = true; } int acctime = hrt_absolute_time() - start_acc; if (acctime > 500) printf("ACC: %d us\n", acctime); /* MAGNETOMETER */ if (magcounter == 4) { /* 120 Hz */ uint64_t start_mag = hrt_absolute_time(); /* start calibration mode if requested */ if (!mag_calibration_enabled && vstatus.preflight_mag_calibration) { ioctl(fd_magnetometer, HMC5883L_CALIBRATION_ON, 0); printf("[sensors] enabling mag calibration mode\n"); mag_calibration_enabled = true; } else if (mag_calibration_enabled && !vstatus.preflight_mag_calibration) { ioctl(fd_magnetometer, HMC5883L_CALIBRATION_OFF, 0); printf("[sensors] disabling mag calibration mode\n"); mag_calibration_enabled = false; } ret_magnetometer = read(fd_magnetometer, buf_magnetometer, sizeof(buf_magnetometer)); int errcode_mag = (int) * get_errno_ptr(); int magtime = hrt_absolute_time() - start_mag; if (magtime > 2000) { printf("MAG (pure read): %d us\n", magtime); } if (ret_magnetometer != sizeof(buf_magnetometer)) { mag_fail_count++; if (mag_fail_count & 0b1000 || (mag_fail_count > 20 && mag_fail_count < 100)) { fprintf(stderr, "[sensors] HMC5883L ERROR #%d: %s\n", (int)*get_errno_ptr(), strerror((int)*get_errno_ptr())); } if (magn_healthy && mag_fail_count >= MAGN_HEALTH_COUNTER_LIMIT_ERROR) { // global_data_send_subsystem_info(&magn_present_enabled); magn_healthy = false; mag_success_count = 0; } } else { mag_success_count++; if (!magn_healthy && mag_success_count >= MAGN_HEALTH_COUNTER_LIMIT_OK) { // global_data_send_subsystem_info(&magn_present_enabled_healthy); magn_healthy = true; mag_fail_count = 0; } magn_updated = true; } magtime = hrt_absolute_time() - start_mag; if (magtime > 2000) { printf("MAG (overall time): %d us\n", magtime); fprintf(stderr, "[sensors] TIMEOUT HMC5883L ERROR #%d: %s\n", errcode_mag, strerror(errcode_mag)); } magcounter = 0; } magcounter++; /* BAROMETER */ if (barocounter == 5 && (fd_barometer > 0)) { /* 100 Hz */ uint64_t start_baro = hrt_absolute_time(); *get_errno_ptr() = 0; ret_barometer = read(fd_barometer, buf_barometer, sizeof(buf_barometer)); if (ret_barometer != sizeof(buf_barometer)) { baro_fail_count++; if ((baro_fail_count & 0b1000 || (baro_fail_count > 20 && baro_fail_count < 100)) && (int)*get_errno_ptr() != EAGAIN) { fprintf(stderr, "[sensors] MS5611 ERROR #%d: %s\n", (int)*get_errno_ptr(), strerror((int)*get_errno_ptr())); } if (baro_healthy && baro_fail_count >= BARO_HEALTH_COUNTER_LIMIT_ERROR) { /* switched from healthy to unhealthy */ baro_healthy = false; baro_success_count = 0; // global_data_send_subsystem_info(&baro_present_enabled); } } else { baro_success_count++; if (!baro_healthy && baro_success_count >= MAGN_HEALTH_COUNTER_LIMIT_OK) { /* switched from unhealthy to healthy */ baro_healthy = true; baro_fail_count = 0; // global_data_send_subsystem_info(&baro_present_enabled_healthy); } baro_updated = true; } barocounter = 0; int barotime = hrt_absolute_time() - start_baro; if (barotime > 2000) printf("BARO: %d us\n", barotime); } barocounter++; /* ADC */ if (adccounter == 5) { ret_adc = read(fd_adc, &buf_adc, adc_readsize); nsamples_adc = ret_adc / sizeof(struct adc_msg_s); if (ret_adc < 0 || nsamples_adc * sizeof(struct adc_msg_s) != ret_adc) { adc_fail_count++; if ((adc_fail_count & 0b1000 || adc_fail_count < 10) && (int)*get_errno_ptr() != EAGAIN) { fprintf(stderr, "[sensors] ADC ERROR #%d: %s\n", (int)*get_errno_ptr(), strerror((int)*get_errno_ptr())); } if (adc_healthy && adc_fail_count >= ADC_HEALTH_COUNTER_LIMIT_ERROR) { adc_healthy = false; adc_success_count = 0; } } else { adc_success_count++; if (!adc_healthy && adc_success_count >= ADC_HEALTH_COUNTER_LIMIT_OK) { adc_healthy = true; adc_fail_count = 0; } adc_updated = true; } adccounter = 0; } adccounter++; #ifdef CONFIG_HRT_PPM bool ppm_updated = false; /* PPM */ if (ppmcounter == 5) { /* require at least two channels * to consider the signal valid */ if (ppm_decoded_channels > 1 && (hrt_absolute_time() - ppm_last_valid_decode) < 45000) { /* Read out values from HRT */ for (int i = 0; i < ppm_decoded_channels; i++) { rc.chan[i].raw = ppm_buffer[i]; /* Set the range to +-, then scale up */ rc.chan[i].scale = (ppm_buffer[i] - rc.chan[i].mid) * rc.chan[i].scaling_factor; } rc.chan_count = ppm_decoded_channels; rc.timestamp = ppm_last_valid_decode; /* publish a few lines of code later if set to true */ ppm_updated = true; } ppmcounter = 0; } ppmcounter++; #endif /* Copy values of gyro, acc, magnetometer & barometer */ /* GYROSCOPE */ if (gyro_updated) { /* copy sensor readings to global data and transform coordinates into px4fmu board frame */ raw.gyro_raw[0] = ((buf_gyro[1] == -32768) ? -32767 : buf_gyro[1]); // x of the board is y of the sensor /* assign negated value, except for -SHORT_MAX, as it would wrap there */ raw.gyro_raw[1] = ((buf_gyro[0] == -32768) ? 32767 : -buf_gyro[0]); // y on the board is -x of the sensor raw.gyro_raw[2] = ((buf_gyro[2] == -32768) ? -32767 : buf_gyro[2]); // z of the board is -z of the sensor /* scale measurements */ // XXX request scaling from driver instead of hardcoding it /* scaling calculated as: raw * (1/(32768*(500/180*PI))) */ raw.gyro_rad_s[0] = (raw.gyro_raw[0] - gyro_offset[0]) * 0.000266316109f; raw.gyro_rad_s[1] = (raw.gyro_raw[1] - gyro_offset[1]) * 0.000266316109f; raw.gyro_rad_s[2] = (raw.gyro_raw[2] - gyro_offset[2]) * 0.000266316109f; raw.gyro_raw_counter++; } /* ACCELEROMETER */ if (acc_updated) { /* copy sensor readings to global data and transform coordinates into px4fmu board frame */ /* assign negated value, except for -SHORT_MAX, as it would wrap there */ raw.accelerometer_raw[0] = (buf_accelerometer[1] == -32768) ? 32767 : -buf_accelerometer[1]; // x of the board is -y of the sensor raw.accelerometer_raw[1] = (buf_accelerometer[0] == -32768) ? -32767 : buf_accelerometer[0]; // y on the board is x of the sensor raw.accelerometer_raw[2] = (buf_accelerometer[2] == -32768) ? -32767 : buf_accelerometer[2]; // z of the board is z of the sensor // XXX read range from sensor float range_g = 4.0f; /* scale from 14 bit to m/s2 */ raw.accelerometer_m_s2[0] = (((raw.accelerometer_raw[0] - acc_offset[0]) * range_g) / 8192.0f) / 9.81f; raw.accelerometer_m_s2[1] = (((raw.accelerometer_raw[1] - acc_offset[1]) * range_g) / 8192.0f) / 9.81f; raw.accelerometer_m_s2[2] = (((raw.accelerometer_raw[2] - acc_offset[2]) * range_g) / 8192.0f) / 9.81f; raw.accelerometer_raw_counter++; } /* MAGNETOMETER */ if (magn_updated) { /* copy sensor readings to global data and transform coordinates into px4fmu board frame */ /* assign negated value, except for -SHORT_MAX, as it would wrap there */ raw.magnetometer_raw[0] = (buf_magnetometer[1] == -32768) ? 32767 : -buf_magnetometer[1]; // x of the board is -y of the sensor raw.magnetometer_raw[1] = (buf_magnetometer[0] == -32768) ? -32767 : buf_magnetometer[0]; // y on the board is x of the sensor raw.magnetometer_raw[2] = (buf_magnetometer[2] == -32768) ? -32767 : buf_magnetometer[2]; // z of the board is z of the sensor // XXX Read out mag range via I2C on init, assuming 0.88 Ga and 12 bit res here raw.magnetometer_ga[0] = ((raw.magnetometer_raw[0] - mag_offset[0]) / 4096.0f) * 0.88f; raw.magnetometer_ga[1] = ((raw.magnetometer_raw[1] - mag_offset[1]) / 4096.0f) * 0.88f; raw.magnetometer_ga[2] = ((raw.magnetometer_raw[2] - mag_offset[2]) / 4096.0f) * 0.88f; /* store mode */ raw.magnetometer_mode = buf_magnetometer[3]; raw.magnetometer_raw_counter++; } /* BAROMETER */ if (baro_updated) { /* copy sensor readings to global data and transform coordinates into px4fmu board frame */ raw.baro_pres_mbar = buf_barometer[0]; // Pressure in mbar raw.baro_alt_meter = buf_barometer[1]; // Altitude in meters raw.baro_temp_celcius = buf_barometer[2]; // Temperature in degrees celcius raw.baro_raw_counter++; } /* ADC */ if (adc_updated) { /* copy sensor readings to global data*/ if (ADC_BATTERY_VOLATGE_CHANNEL == buf_adc.am_channel1) { /* Voltage in volts */ raw.battery_voltage_v = (BAT_VOL_LOWPASS_1 * (raw.battery_voltage_v + BAT_VOL_LOWPASS_2 * (uint16_t)(buf_adc.am_data1 * battery_voltage_conversion))); if ((buf_adc.am_data1 * battery_voltage_conversion) < VOLTAGE_BATTERY_IGNORE_THRESHOLD_VOLTS) { raw.battery_voltage_valid = false; raw.battery_voltage_v = 0.f; } else { raw.battery_voltage_valid = true; } raw.battery_voltage_counter++; } } uint64_t total_time = hrt_absolute_time() - current_time; /* Inform other processes that new data is available to copy */ if ((gyro_updated || acc_updated || magn_updated || baro_updated) && publishing) { /* Values changed, publish */ orb_publish(ORB_ID(sensor_combined), sensor_pub, &raw); } #ifdef CONFIG_HRT_PPM if (ppm_updated) { orb_publish(ORB_ID(rc_channels), rc_pub, &rc); } #endif if (total_time > 2600) { excessive_readout_time_counter++; } if (total_time > 2600 && excessive_readout_time_counter > 100 && excessive_readout_time_counter % 100 == 0) { fprintf(stderr, "[sensors] slow update (>2600 us): %d us (#%d)\n", (int)total_time, excessive_readout_time_counter); } else if (total_time > 6000) { if (excessive_readout_time_counter < 100 || excessive_readout_time_counter % 100 == 0) fprintf(stderr, "[sensors] WARNING: Slow update (>6000 us): %d us (#%d)\n", (int)total_time, excessive_readout_time_counter); } read_loop_counter++; #ifdef CONFIG_SENSORS_DEBUG_ENABLED if (read_loop_counter % 1000 == 0) printf("[sensors] read loop counter: %d\n", read_loop_counter); fflush(stdout); if (sensors_timer_loop_counter % 1000 == 0) printf("[sensors] timer/trigger loop counter: %d\n", sensors_timer_loop_counter); #endif } } /* Never really getting here */ printf("[sensors] sensor readout stopped\n"); close(fd_gyro); close(fd_accelerometer); close(fd_magnetometer); close(fd_barometer); close(fd_adc); printf("[sensors] exiting.\n"); return ret; }
/** * Initialize all sensor drivers. * * @return 0 on success, != 0 on failure */ static int sensors_init(void) { printf("[sensors] Sensor configuration..\n"); /* open magnetometer */ fd_magnetometer = open("/dev/hmc5883l", O_RDONLY); if (fd_magnetometer < 0) { fprintf(stderr, "[sensors] HMC5883L open fail (err #%d): %s\n", (int)*get_errno_ptr(), strerror((int)*get_errno_ptr())); fflush(stderr); /* this sensor is critical, exit on failed init */ errno = ENOSYS; return ERROR; } else { printf("[sensors] HMC5883L open ok\n"); } /* open barometer */ fd_barometer = open("/dev/ms5611", O_RDONLY); if (fd_barometer < 0) { fprintf(stderr, "[sensors] MS5611 open fail (err #%d): %s\n", (int)*get_errno_ptr(), strerror((int)*get_errno_ptr())); fflush(stderr); } else { printf("[sensors] MS5611 open ok\n"); } /* open gyro */ fd_gyro = open("/dev/l3gd20", O_RDONLY); if (fd_gyro < 0) { fprintf(stderr, "[sensors] L3GD20 open fail (err #%d): %s\n", (int)*get_errno_ptr(), strerror((int)*get_errno_ptr())); fflush(stderr); /* this sensor is critical, exit on failed init */ errno = ENOSYS; return ERROR; } else { printf("[sensors] L3GD20 open ok\n"); } /* open accelerometer */ fd_accelerometer = open("/dev/bma180", O_RDONLY); if (fd_accelerometer < 0) { fprintf(stderr, "[sensors] BMA180: open fail (err #%d): %s\n", (int)*get_errno_ptr(), strerror((int)*get_errno_ptr())); fflush(stderr); /* this sensor is critical, exit on failed init */ errno = ENOSYS; return ERROR; } else { printf("[sensors] BMA180 open ok\n"); } /* open adc */ fd_adc = open("/dev/adc0", O_RDONLY | O_NONBLOCK); if (fd_adc < 0) { fprintf(stderr, "[sensors] ADC: open fail (err #%d): %s\n", (int)*get_errno_ptr(), strerror((int)*get_errno_ptr())); fflush(stderr); /* this sensor is critical, exit on failed init */ errno = ENOSYS; return ERROR; } else { printf("[sensors] ADC open ok\n"); } /* configure gyro */ if (ioctl(fd_gyro, L3GD20_SETRATE, L3GD20_RATE_760HZ_LP_30HZ) || ioctl(fd_gyro, L3GD20_SETRANGE, L3GD20_RANGE_500DPS)) { fprintf(stderr, "[sensors] L3GD20 configuration (ioctl) fail (err #%d): %s\n", (int)*get_errno_ptr(), strerror((int)*get_errno_ptr())); fflush(stderr); /* this sensor is critical, exit on failed init */ errno = ENOSYS; return ERROR; } else { printf("[sensors] L3GD20 configuration ok\n"); } /* XXX Add IOCTL configuration of remaining sensors */ printf("[sensors] All sensors configured\n"); return OK; }
int wd_start(WDOG_ID wdog, int delay, wdentry_t wdentry, int argc, ...) { va_list ap; FAR wdog_t *curr; FAR wdog_t *prev; FAR wdog_t *next; int32_t now; irqstate_t saved_state; int i; /* Verify the wdog */ if (!wdog || argc > CONFIG_MAX_WDOGPARMS || delay < 0) { *get_errno_ptr() = EINVAL; return ERROR; } /* Check if the watchdog has been started. If so, stop it. * NOTE: There is a race condition here... the caller may receive * the watchdog between the time that wd_start is called and * the critical section is established. */ saved_state = irqsave(); if (wdog->active) { wd_cancel(wdog); } /* Save the data in the watchdog structure */ wdog->func = wdentry; /* Function to execute when delay expires */ up_getpicbase(&wdog->picbase); wdog->argc = argc; va_start(ap, argc); for (i = 0; i < argc; i++) { wdog->parm[i] = va_arg(ap, uint32_t); } #ifdef CONFIG_DEBUG for (; i < CONFIG_MAX_WDOGPARMS; i++) { wdog->parm[i] = 0; } #endif va_end(ap); /* Calculate delay+1, forcing the delay into a range that we can handle */ if (delay <= 0) { delay = 1; } else if (++delay <= 0) { delay--; } /* Do the easy case first -- when the watchdog timer queue is empty. */ if (g_wdactivelist.head == NULL) { sq_addlast((FAR sq_entry_t*)wdog,&g_wdactivelist); } /* There are other active watchdogs in the timer queue */ else { now = 0; prev = curr = (FAR wdog_t*)g_wdactivelist.head; /* Advance to positive time */ while ((now += curr->lag) < 0 && curr->next) { prev = curr; curr = curr->next; } /* Advance past shorter delays */ while (now <= delay && curr->next) { prev = curr; curr = curr->next; now += curr->lag; } /* Check if the new wdog must be inserted before the curr. */ if (delay < now) { /* The relative delay time is smaller or equal to the current delay * time, so decrement the current delay time by the new relative * delay time. */ delay -= (now - curr->lag); curr->lag -= delay; /* Insert the new watchdog in the list */ if (curr == (FAR wdog_t*)g_wdactivelist.head) { sq_addfirst((FAR sq_entry_t*)wdog, &g_wdactivelist); } else { sq_addafter((FAR sq_entry_t*)prev, (FAR sq_entry_t*)wdog, &g_wdactivelist); } } /* The new watchdog delay time is greater than the curr delay time, * so the new wdog must be inserted after the curr. This only occurs * if the wdog is to be added to the end of the list. */ else { delay -= now; if (!curr->next) { sq_addlast((FAR sq_entry_t*)wdog, &g_wdactivelist); } else { next = curr->next; next->lag -= delay; sq_addafter((FAR sq_entry_t*)curr, (FAR sq_entry_t*)wdog, &g_wdactivelist); } } } /* Put the lag into the watchdog structure and mark it as active. */ wdog->lag = delay; wdog->active = true; irqrestore(saved_state); return OK; }
int psock_bind(FAR struct socket *psock, const struct sockaddr *addr, socklen_t addrlen) { #ifdef CONFIG_NET_PKT FAR const struct sockaddr_ll *lladdr = (const struct sockaddr_ll *)addr; #endif socklen_t minlen; int err; int ret = OK; /* Verify that the psock corresponds to valid, allocated socket */ if (!psock || psock->s_crefs <= 0) { err = ENOTSOCK; goto errout; } /* Verify that a valid address has been provided */ switch (addr->sa_family) { #ifdef CONFIG_NET_IPv4 case AF_INET: minlen = sizeof(struct sockaddr_in); break; #endif #ifdef CONFIG_NET_IPv6 case AF_INET6: minlen = sizeof(struct sockaddr_in6); break; #endif #ifdef CONFIG_NET_LOCAL case AF_LOCAL: minlen = sizeof(sa_family_t); break; #endif #ifdef CONFIG_NET_PKT case AF_PACKET: minlen = sizeof(struct sockaddr_ll); break; #endif default: ndbg("ERROR: Unrecognized address family: %d\n", addr->sa_family); err = EAFNOSUPPORT; goto errout; } if (addrlen < minlen) { ndbg("ERROR: Invalid address length: %d < %d\n", addrlen, minlen); err = EBADF; goto errout; } /* Perform the binding depending on the protocol type */ switch (psock->s_type) { #ifdef CONFIG_NET_PKT case SOCK_RAW: ret = pkt_bind(psock->s_conn, lladdr); break; #endif /* Bind a stream socket which may either be TCP/IP or a local, Unix * domain socket. */ #if defined(CONFIG_NET_TCP) || defined(CONFIG_NET_LOCAL_STREAM) case SOCK_STREAM: { #ifdef CONFIG_NET_LOCAL_STREAM #ifdef CONFIG_NET_TCP /* Is this a Unix domain socket? */ if (psock->s_domain == PF_LOCAL) #endif { /* Bind the Unix domain connection structure */ ret = psock_local_bind(psock, addr, addrlen); } #endif /* CONFIG_NET_LOCAL_STREAM */ #ifdef CONFIG_NET_TCP #ifdef CONFIG_NET_LOCAL_STREAM else #endif { /* Bind the TCP/IP connection structure */ ret = tcp_bind(psock->s_conn, addr); } #endif /* CONFIG_NET_TCP */ /* Mark the socket bound */ if (ret >= 0) { psock->s_flags |= _SF_BOUND; } } break; #endif /* CONFIG_NET_TCP || CONFIG_NET_LOCAL_STREAM */ /* Bind a datagram socket which may either be TCP/IP or a local, Unix * domain socket. */ #if defined(CONFIG_NET_UDP) || defined(CONFIG_NET_LOCAL_DGRAM) case SOCK_DGRAM: { #ifdef CONFIG_NET_LOCAL_DGRAM #ifdef CONFIG_NET_UDP /* Is this a Unix domain socket? */ if (psock->s_domain == PF_LOCAL) #endif { /* Bind the Unix domain connection structure */ ret = psock_local_bind(psock, addr, addrlen); } #endif /* CONFIG_NET_LOCAL_DGRAM */ #ifdef CONFIG_NET_UDP #ifdef CONFIG_NET_LOCAL_DGRAM else #endif { /* Bind the UDPP/IP connection structure */ ret = udp_bind(psock->s_conn, addr); } #endif /* CONFIG_NET_UDP */ /* Mark the socket bound */ if (ret >= 0) { psock->s_flags |= _SF_BOUND; } } break; #endif /* CONFIG_NET_UDP || CONFIG_NET_LOCAL_DGRAM */ default: err = EBADF; goto errout; } /* Was the bind successful */ if (ret < 0) { err = -ret; goto errout; } return OK; errout: *get_errno_ptr() = err; return ERROR; }
off_t lseek(int fd, off_t offset, int whence) { FAR struct filelist *list; FAR struct file *filep; FAR struct inode *inode; int err; /* Did we get a valid file descriptor? */ if ((unsigned int)fd >= CONFIG_NFILE_DESCRIPTORS) { err = EBADF; goto errout; } /* Get the thread-specific file list */ list = sched_getfiles(); if (!list) { err = EMFILE; goto errout; } /* Is a driver registered? */ filep = &list->fl_files[fd]; inode = filep->f_inode; if (inode && inode->u.i_ops) { /* Does it support the seek method */ if (inode->u.i_ops->seek) { /* Yes, then let it perform the seek */ err = (int)inode->u.i_ops->seek(filep, offset, whence); if (err < 0) { err = -err; goto errout; } } else { /* No... there are a couple of default actions we can take */ switch (whence) { case SEEK_CUR: offset += filep->f_pos; case SEEK_SET: if (offset >= 0) { filep->f_pos = offset; /* Might be beyond the end-of-file */ break; } else { err = EINVAL; goto errout; } break; case SEEK_END: err = ENOSYS; goto errout; default: err = EINVAL; goto errout; } } } return filep->f_pos; errout: *get_errno_ptr() = err; return (off_t)ERROR; }
int usrsock_socket(int domain, int type, int protocol, FAR struct socket *psock) { struct usrsock_reqstate_s state = {}; FAR struct usrsock_conn_s *conn; net_lock_t save; int err; /* Allocate the usrsock socket connection structure and save in the new * socket instance. */ conn = usrsock_alloc(); if (!conn) { /* Failed to reserve a connection structure */ return -ENOMEM; } save = net_lock(); /* Set up event callback for usrsock. */ err = usrsock_setup_request_callback(conn, &state, socket_event, USRSOCK_EVENT_ABORT | USRSOCK_EVENT_REQ_COMPLETE); if (err < 0) { goto errout_free_conn; } /* Request user-space daemon for new socket. */ err = do_socket_request(conn, domain, type, protocol); if (err < 0) { goto errout_teardown_callback; } /* Wait for completion of request. */ while (net_lockedwait(&state.recvsem) != OK) { DEBUGASSERT(*get_errno_ptr() == EINTR); } if (state.result < 0) { err = state.result; goto errout_teardown_callback; } psock->s_type = SOCK_USRSOCK_TYPE; psock->s_domain = PF_USRSOCK_DOMAIN; conn->type = type; psock->s_conn = conn; conn->crefs = 1; usrsock_teardown_request_callback(&state); net_unlock(save); return OK; errout_teardown_callback: usrsock_teardown_request_callback(&state); errout_free_conn: usrsock_free(conn); net_unlock(save); return err; }
int bind(int sockfd, const struct sockaddr *addr, socklen_t addrlen) { FAR struct socket *psock = sockfd_socket(sockfd); #if defined(CONFIG_NET_TCP) || defined(CONFIG_NET_UDP) #ifdef CONFIG_NET_IPv6 FAR const struct sockaddr_in6 *inaddr = (const struct sockaddr_in6 *)addr; #else FAR const struct sockaddr_in *inaddr = (const struct sockaddr_in *)addr; #endif #endif int err; int ret; /* Verify that the sockfd corresponds to valid, allocated socket */ if (!psock || psock->s_crefs <= 0) { err = EBADF; goto errout; } /* Verify that a valid address has been provided */ #ifdef CONFIG_NET_IPv6 if (addr->sa_family != AF_INET6 || addrlen < sizeof(struct sockaddr_in6)) #else if (addr->sa_family != AF_INET || addrlen < sizeof(struct sockaddr_in)) #endif { err = EBADF; goto errout; } /* Perform the binding depending on the protocol type */ switch (psock->s_type) { #ifdef CONFIG_NET_TCP case SOCK_STREAM: ret = uip_tcpbind(psock->s_conn, inaddr); psock->s_flags |= _SF_BOUND; break; #endif #ifdef CONFIG_NET_UDP case SOCK_DGRAM: ret = uip_udpbind(psock->s_conn, inaddr); break; #endif default: err = EBADF; goto errout; } /* Was the bind successful */ if (ret < 0) { err = -ret; goto errout; } return OK; errout: *get_errno_ptr() = err; return ERROR; }
int ioctl(int fd, int req, unsigned long arg) { int err; #if CONFIG_NFILE_DESCRIPTORS > 0 FAR struct filelist *list; FAR struct file *this_file; FAR struct inode *inode; int ret = OK; /* Did we get a valid file descriptor? */ if ((unsigned int)fd >= CONFIG_NFILE_DESCRIPTORS) #endif { /* Perform the socket ioctl */ #if defined(CONFIG_NET) && CONFIG_NSOCKET_DESCRIPTORS > 0 if ((unsigned int)fd < (CONFIG_NFILE_DESCRIPTORS+CONFIG_NSOCKET_DESCRIPTORS)) { return netdev_ioctl(fd, req, arg); } else #endif { err = EBADF; goto errout; } } #if CONFIG_NFILE_DESCRIPTORS > 0 /* Get the thread-specific file list */ list = sched_getfiles(); if (!list) { err = EMFILE; goto errout; } /* Is a driver registered? Does it support the ioctl method? */ this_file = &list->fl_files[fd]; inode = this_file->f_inode; if (inode && inode->u.i_ops && inode->u.i_ops->ioctl) { /* Yes, then let it perform the ioctl */ ret = (int)inode->u.i_ops->ioctl(this_file, req, arg); if (ret < 0) { err = -ret; goto errout; } } return ret; #endif errout: *get_errno_ptr() = err; return ERROR; }
int stat(const char *path, FAR struct stat *buf) { FAR struct inode *inode; const char *relpath = NULL; int ret = OK; /* Sanity checks */ if (!path || !buf) { ret = EFAULT; goto errout; } if (!path[0]) { ret = ENOENT; goto errout; } /* Check for the fake root directory (which has no inode) */ if (strcmp(path, "/") == 0) { return statroot(buf); } /* Get an inode for this file */ inode = inode_find(path, &relpath); if (!inode) { /* This name does not refer to a psudeo-inode and there is no * mountpoint that includes in this path. */ ret = ENOENT; goto errout; } /* The way we handle the stat depends on the type of inode that we * are dealing with. */ #ifndef CONFIG_DISABLE_MOUNTPOINT if (INODE_IS_MOUNTPT(inode)) { /* The node is a file system mointpoint. Verify that the mountpoint * supports the stat() method */ if (inode->u.i_mops && inode->u.i_mops->stat) { /* Perform the rewinddir() operation */ ret = inode->u.i_mops->stat(inode, relpath, buf); } } else #endif { /* The node is part of the root psuedo file system */ ret = statpsuedo(inode, buf); } /* Check if the stat operation was successful */ if (ret < 0) { ret = -ret; goto errout_with_inode; } /* Successfully stat'ed the file */ inode_release(inode); return OK; /* Failure conditions always set the errno appropriately */ errout_with_inode: inode_release(inode); errout: *get_errno_ptr() = ret; return ERROR; }
ssize_t send(int sockfd, const void *buf, size_t len, int flags) { FAR struct socket *psock = sockfd_socket(sockfd); struct send_s state; uip_lock_t save; int err; int ret = OK; /* Verify that the sockfd corresponds to valid, allocated socket */ if (!psock || psock->s_crefs <= 0) { err = EBADF; goto errout; } /* If this is an un-connected socket, then return ENOTCONN */ if (psock->s_type != SOCK_STREAM || !_SS_ISCONNECTED(psock->s_flags)) { err = ENOTCONN; goto errout; } /* Set the socket state to sending */ psock->s_flags = _SS_SETSTATE(psock->s_flags, _SF_SEND); /* Perform the TCP send operation */ /* Initialize the state structure. This is done with interrupts * disabled because we don't want anything to happen until we * are ready. */ save = uip_lock(); memset(&state, 0, sizeof(struct send_s)); (void)sem_init(&state. snd_sem, 0, 0); /* Doesn't really fail */ state.snd_sock = psock; /* Socket descriptor to use */ state.snd_buflen = len; /* Number of bytes to send */ state.snd_buffer = buf; /* Buffer to send from */ if (len > 0) { struct uip_conn *conn = (struct uip_conn*)psock->s_conn; /* Allocate resources to receive a callback */ state.snd_cb = uip_tcpcallbackalloc(conn); if (state.snd_cb) { /* Get the initial sequence number that will be used */ state.snd_isn = uip_tcpgetsequence(conn->sndseq); /* There is no outstanding, unacknowledged data after this * initial sequence number. */ conn->unacked = 0; /* Update the initial time for calculating timeouts */ #if defined(CONFIG_NET_SOCKOPTS) && !defined(CONFIG_DISABLE_CLOCK) state.snd_time = clock_systimer(); #endif /* Set up the callback in the connection */ state.snd_cb->flags = UIP_ACKDATA|UIP_REXMIT|UIP_POLL|UIP_CLOSE|UIP_ABORT|UIP_TIMEDOUT; state.snd_cb->priv = (void*)&state; state.snd_cb->event = send_interrupt; /* Notify the device driver of the availaibilty of TX data */ netdev_txnotify(&conn->ripaddr); /* Wait for the send to complete or an error to occur: NOTES: (1) * uip_lockedwait will also terminate if a signal is received, (2) interrupts * may be disabled! They will be re-enabled while the task sleeps and * automatically re-enabled when the task restarts. */ ret = uip_lockedwait(&state. snd_sem); /* Make sure that no further interrupts are processed */ uip_tcpcallbackfree(conn, state.snd_cb); } } sem_destroy(&state. snd_sem); uip_unlock(save); /* Set the socket state to idle */ psock->s_flags = _SS_SETSTATE(psock->s_flags, _SF_IDLE); /* Check for a errors. Errors are signaled by negative errno values * for the send length */ if (state.snd_sent < 0) { err = state.snd_sent; goto errout; } /* If uip_lockedwait failed, then we were probably reawakened by a signal. In * this case, uip_lockedwait will have set errno appropriately. */ if (ret < 0) { err = -ret; goto errout; } /* Return the number of bytes actually sent */ return state.snd_sent; errout: *get_errno_ptr() = err; return ERROR; }
FAR struct dirent *readdir(DIR *dirp) { FAR struct fs_dirent_s *idir = (struct fs_dirent_s *)dirp; #ifndef CONFIG_DISABLE_MOUNTPOINT struct inode *inode; #endif int ret; /* Sanity checks */ if (!idir || !idir->fd_root) { ret = EBADF; goto errout; } /* The way we handle the readdir depends on the type of inode * that we are dealing with. */ #ifndef CONFIG_DISABLE_MOUNTPOINT inode = idir->fd_root; if (INODE_IS_MOUNTPT(inode) && !DIRENT_ISPSUEDONODE(idir->fd_flags)) { /* The node is a file system mointpoint. Verify that the mountpoint * supports the readdir() method */ if (!inode->u.i_mops || !inode->u.i_mops->readdir) { ret = EACCES; goto errout; } /* Perform the readdir() operation */ ret = inode->u.i_mops->readdir(inode, idir); } else #endif { /* The node is part of the root psuedo file system */ ret = readpsuedodir(idir); } /* ret < 0 is an error. Special case: ret = -ENOENT is end of file */ if ( ret < 0) { if (ret == -ENOENT) { ret = OK; } else { ret = -ret; } goto errout; } /* Success */ idir->fd_position++; return &idir->fd_dir; errout: *get_errno_ptr() = ret; return NULL; }