void ProgramHeader::wedge(ElfFile* elfFile, uint32_t shamt){ if (GET(p_type) == PT_GNU_STACK){ return; } INCREMENT(p_vaddr, shamt); INCREMENT(p_paddr, shamt); }
int ETSAirspeed::collect() { int ret = -EIO; /* read from the sensor */ uint8_t val[2] = {0, 0}; perf_begin(_sample_perf); ret = transfer(nullptr, 0, &val[0], 2); if (ret < 0) { log("error reading from sensor: %d", ret); return ret; } uint16_t diff_pres_pa = val[1] << 8 | val[0]; if (diff_pres_pa == 0) { // not a valid reading return ret; } _reports[_next_report].timestamp = hrt_absolute_time(); _reports[_next_report].differential_pressure_pa = diff_pres_pa; // Track maximum differential pressure measured (so we can work out top speed). if (diff_pres_pa > _reports[_next_report].max_differential_pressure_pa) { _reports[_next_report].max_differential_pressure_pa = diff_pres_pa; } /* announce the airspeed if needed, just publish else */ orb_publish(ORB_ID(differential_pressure), _airspeed_pub, &_reports[_next_report]); /* post a report to the ring - note, not locked */ INCREMENT(_next_report, _num_reports); /* if we are running up against the oldest report, toss it */ if (_next_report == _oldest_report) { perf_count(_buffer_overflows); INCREMENT(_oldest_report, _num_reports); } /* notify anyone waiting for data */ poll_notify(POLLIN); ret = OK; perf_end(_sample_perf); return ret; }
/* * Add a line to the history buffer. */ void rsh_history_add(char *line){ /* In case we are replacing an old line. */ if ( history[end] ) free(history[end]); history[end] = strdup(line); /* Perform book keeping... Handle wrapping, deleting old entries, etc. */ INCREMENT(end); if ( end == start ) INCREMENT(start); }
int MB12XX::collect() { int ret = -EIO; /* read from the sensor */ uint8_t val[2] = {0, 0}; perf_begin(_sample_perf); ret = transfer(nullptr, 0, &val[0], 2); if (ret < 0) { log("error reading from sensor: %d", ret); return ret; } uint16_t distance = val[0] << 8 | val[1]; float si_units = (distance * 1.0f)/ 100.0f; /* cm to m */ /* this should be fairly close to the end of the measurement, so the best approximation of the time */ _reports[_next_report].timestamp = hrt_absolute_time(); _reports[_next_report].distance = si_units; _reports[_next_report].valid = si_units > get_minimum_distance() && si_units < get_maximum_distance() ? 1 : 0; /* publish it */ orb_publish(ORB_ID(sensor_range_finder), _range_finder_topic, &_reports[_next_report]); /* post a report to the ring - note, not locked */ INCREMENT(_next_report, _num_reports); /* if we are running up against the oldest report, toss it */ if (_next_report == _oldest_report) { perf_count(_buffer_overflows); INCREMENT(_oldest_report, _num_reports); } /* notify anyone waiting for data */ poll_notify(POLLIN); ret = OK; out: perf_end(_sample_perf); return ret; return ret; }
int Img_EnsQueue::Push(void * elem) { int result = 0; OSAL_MutexLock(mutex); if(INCREMENT(head, size) == tail) { // queue full !!!! result = 1; } else { queue[head] = elem; head = INCREMENT(head, size); } OSAL_MutexUnlock(mutex); return result; }
// Retrieves the next job from the job array. Will block until a job is // available. Note, if the puzzle has no solution, then this will block forever. // Since only care about puzzles with solutions, this is fine (no need to add // logic for a case that will never happen by assumption). int getNextJob(int pid, job_t* myJob) { int i; job_t* nextJob; for (i = pid; !found; i = (i + 1) % P) { if (jobQueues[i].head == jobQueues[i].tail) continue; omp_set_lock(&(jobQueues[i].headLock)); if (jobQueues[i].head != jobQueues[i].tail && !found) { nextJob = &(jobQueues[i].queue[jobQueues[i].head]); myJob->length = nextJob->length; memcpy(myJob->assignments, nextJob->assignments, sizeof(assignment_t) * nextJob->length); jobQueues[i].head = INCREMENT(jobQueues[i].head); omp_unset_lock(&(jobQueues[i].headLock)); return 1; } omp_unset_lock(&(jobQueues[i].headLock)); } return 0; }
void *CRYPTO_malloc(size_t num, const char *file, int line) { void *ret = NULL; INCREMENT(malloc_count); if (malloc_impl != NULL && malloc_impl != CRYPTO_malloc) return malloc_impl(num, file, line); if (num == 0) return NULL; FAILTEST(); allow_customize = 0; #ifndef OPENSSL_NO_CRYPTO_MDEBUG if (call_malloc_debug) { CRYPTO_mem_debug_malloc(NULL, num, 0, file, line); ret = malloc(num); CRYPTO_mem_debug_malloc(ret, num, 1, file, line); } else { ret = malloc(num); } #else (void)(file); (void)(line); ret = malloc(num); #endif return ret; }
void *CRYPTO_realloc(void *str, size_t num, const char *file, int line) { INCREMENT(realloc_count); if (realloc_impl != NULL && realloc_impl != &CRYPTO_realloc) return realloc_impl(str, num, file, line); FAILTEST(); if (str == NULL) return CRYPTO_malloc(num, file, line); if (num == 0) { CRYPTO_free(str, file, line); return NULL; } allow_customize = 0; #ifndef OPENSSL_NO_CRYPTO_MDEBUG if (call_malloc_debug) { void *ret; CRYPTO_mem_debug_realloc(str, NULL, num, 0, file, line); ret = realloc(str, num); CRYPTO_mem_debug_realloc(str, ret, num, 1, file, line); return ret; } #else (void)(file); (void)(line); #endif return realloc(str, num); }
ssize_t ETSAirspeed::read(struct file *filp, char *buffer, size_t buflen) { unsigned count = buflen / sizeof(struct differential_pressure_s); int ret = 0; /* buffer must be large enough */ if (count < 1) return -ENOSPC; /* if automatic measurement is enabled */ if (_measure_ticks > 0) { /* * While there is space in the caller's buffer, and reports, copy them. * Note that we may be pre-empted by the workq thread while we are doing this; * we are careful to avoid racing with them. */ while (count--) { if (_oldest_report != _next_report) { memcpy(buffer, _reports + _oldest_report, sizeof(*_reports)); ret += sizeof(_reports[0]); INCREMENT(_oldest_report, _num_reports); } } /* if there was no data, warn the caller */ return ret ? ret : -EAGAIN; } /* manual measurement - run one conversion */ /* XXX really it'd be nice to lock against other readers here */ do { _oldest_report = _next_report = 0; /* trigger a measurement */ if (OK != measure()) { ret = -EIO; break; } /* wait for it to complete */ usleep(CONVERSION_INTERVAL); /* run the collection phase */ if (OK != collect()) { ret = -EIO; break; } /* state machine will have generated a report, copy it out */ memcpy(buffer, _reports, sizeof(*_reports)); ret = sizeof(*_reports); } while (0); return ret; }
/********************************************//** @ingroup buffer_function @brief Put an item into the buffer. @return BUFFER_OK: item stored in the buffer.\n BUFFER_FULL: The buffer is full, item is not stored. @param *buffer pointer to the buffer structure. @param item to be stored in the buffer. ***********************************************/ uint8_t buffer_put_item(buffer_struct_t *buffer, uint8_t item) { if (buffer->no_in_buffer<BUFFER_SIZE) { buffer->storage[buffer->in_i] = item; buffer->in_i = INCREMENT(buffer->in_i); buffer->no_in_buffer++; return BUFFER_OK; } return BUFFER_FULL; }
/********************************************//** @ingroup buffer_function @brief Get an item from the buffer. @return BUFFER_OK: item removed from buffer and returned in item.\n BUFFER_EMPTY: The buffer is empty, item is not updated. @param *buffer pointer to the buffer structure. @param *item pointer to the variable where the value of the item is returned. ***********************************************/ uint8_t buffer_get_item(buffer_struct_t *buffer, uint8_t *item) { if (buffer->no_in_buffer > 0) { *item = buffer->storage[buffer->out_i]; buffer->out_i = INCREMENT(buffer->out_i); buffer->no_in_buffer--; return BUFFER_OK; } return BUFFER_EMPTY; }
void rsh_history_print(){ int offset = start; int printed = 1; /* Start is the oldest entry, so start printing there. */ while ( offset != end ){ printf("%-3d %s\n", printed++, history[offset]); INCREMENT(offset); } }
/* * Get a transaction lock from the free list. If the number in use is * greater than the high water mark, wake up the sync daemon. This should * free some anonymous transaction locks. (TXN_LOCK must be held.) */ static lid_t txLockAlloc(void) { lid_t lid; INCREMENT(TxStat.txLockAlloc); if (!TxAnchor.freelock) { INCREMENT(TxStat.txLockAlloc_freelock); } while (!(lid = TxAnchor.freelock)) TXN_SLEEP(&TxAnchor.freelockwait); TxAnchor.freelock = TxLock[lid].next; HIGHWATERMARK(stattx.maxlid, lid); if ((++TxAnchor.tlocksInUse > TxLockHWM) && (jfs_tlocks_low == 0)) { jfs_info("txLockAlloc tlocks low"); jfs_tlocks_low = 1; wake_up_process(jfsSyncThread); } return lid; }
int main() { demo::data d = {0, 0}; print_res ("a", d.a, "b", d.b); // Increment "manually" d.a += 1; d.b += 1; print_res ("a", d.a, "b", d.b); // Increment thanks to macro and concatenation #define INCREMENT(s,VAR) s.VAR += 1 INCREMENT (d, a); INCREMENT (d, b); print_res ("a", d.a, "b", d.b); // Increment thanks to Boost preprocessor #define DATA_NAMES (2, (a, b)) INCREMENT (d, BOOST_PP_ARRAY_ELEM(0, DATA_NAMES)); INCREMENT (d, BOOST_PP_ARRAY_ELEM(1, DATA_NAMES)); print_res ("a", d.a, "b", d.b); // Increment and loop thanks to Boost preprocessor #define BOOST_PP_LOCAL_MACRO(n) \ INCREMENT (d, BOOST_PP_ARRAY_ELEM(n, DATA_NAMES)); // Loop limits (from 0 to N_DATA) #define BOOST_PP_LOCAL_LIMITS (0,1) // Execute the loop #include BOOST_PP_LOCAL_ITERATE() print_res ("a", d.a, "b", d.b); return EXIT_SUCCESS; }
ssize_t SPV1020::read(struct file *filp, char *buffer, size_t buflen) { unsigned count = buflen / sizeof(struct spv1020_report); int ret = 0; /* buffer must be large enough */ if (count < 1) return -ENOSPC; /* if automatic measurement is enabled */ if (_measure_ticks > 0) { /* * While there is space in the caller's buffer, and reports, copy them. * Note that we may be pre-empted by the workq thread while we are doing this; * we are careful to avoid racing with them. */ while (count--) { if (_oldest_report != _next_report) { memcpy(buffer, _reports + _oldest_report, sizeof(*_reports)); ret += sizeof(_reports[0]); INCREMENT(_oldest_report, _num_reports); } } /* if there was no data, warn the caller */ return ret ? ret : -EAGAIN; } /* manual measurement - run one conversion */ do { _measurement_phase = 0; _oldest_report = _next_report = 0; /* Take a MPPT measurement */ if (OK != mppt_measurement()) { ret = -EIO; break; } /* state machine will have generated a report, copy it out */ memcpy(buffer, _reports, sizeof(*_reports)); ret = sizeof(*_reports); } while (0); return ret; }
void umac32_digest (struct umac32_ctx *ctx, size_t length, uint8_t *digest) { uint32_t pad; assert (length > 0); assert (length <= 4); if (ctx->index > 0 || ctx->count == 0) { /* Zero pad to multiple of 32 */ uint64_t y; unsigned pad = (ctx->index > 0) ? 31 & - ctx->index : 32; memset (ctx->block + ctx->index, 0, pad); y = _umac_nh (ctx->l1_key, ctx->index + pad, ctx->block) + 8 * ctx->index; _umac_l2 (ctx->l2_key, ctx->l2_state, 1, ctx->count++, &y); } assert (ctx->count > 0); if ( !(ctx->nonce_low & _UMAC_NONCE_CACHED)) { aes128_encrypt (&ctx->pdf_key, AES_BLOCK_SIZE, (uint8_t *) ctx->pad_cache, ctx->nonce); ctx->nonce_low |= _UMAC_NONCE_CACHED; } pad = ctx->pad_cache[ctx->nonce_low & 3]; /* Increment nonce */ ctx->nonce_low++; if ( !(ctx->nonce_low & 3)) { unsigned i = ctx->nonce_length - 1; ctx->nonce_low = 0; ctx->nonce[i] += 4; if (ctx->nonce[i] == 0 && i > 0) INCREMENT (i, ctx->nonce); } _umac_l2_final (ctx->l2_key, ctx->l2_state, 1, ctx->count); pad ^= ctx->l3_key2[0] ^ _umac_l3 (ctx->l3_key1, ctx->l2_state); memcpy (digest, &pad, length); /* Reinitialize */ ctx->count = ctx->index = 0; }
static inline void __lock_metapage(struct metapage *mp) { DECLARE_WAITQUEUE(wait, current); INCREMENT(mpStat.lockwait); add_wait_queue_exclusive(&mp->wait, &wait); do { set_current_state(TASK_UNINTERRUPTIBLE); if (metapage_locked(mp)) { unlock_page(mp->page); io_schedule(); lock_page(mp->page); } } while (trylock_metapage(mp)); __set_current_state(TASK_RUNNING); remove_wait_queue(&mp->wait, &wait); }
void * Img_EnsQueue::Pop() { void * Elem; OSAL_MutexLock(mutex); if(head == tail) { Elem = NULL; } else { Elem = queue[tail]; tail = INCREMENT(tail, size); } OSAL_MutexUnlock(mutex); return Elem; }
static void Increment(AG_Slider *sl) { AG_Variable *bVal, *bMin, *bMax, *bInc; void *pVal, *pMin, *pMax, *pInc; bVal = AG_GetVariable(sl, "value", &pVal); bMin = AG_GetVariable(sl, "min", &pMin); bMax = AG_GetVariable(sl, "max", &pMax); bInc = AG_GetVariable(sl, "inc", &pInc); switch (AG_VARIABLE_TYPE(bVal)) { case AG_VARIABLE_FLOAT: INCREMENT(float); break; case AG_VARIABLE_DOUBLE: INCREMENT(double); break; #ifdef HAVE_LONG_DOUBLE case AG_VARIABLE_LONG_DOUBLE: INCREMENT(long double); break; #endif case AG_VARIABLE_INT: INCREMENT(int); break; case AG_VARIABLE_UINT: INCREMENT(Uint); break; case AG_VARIABLE_UINT8: INCREMENT(Uint8); break; case AG_VARIABLE_SINT8: INCREMENT(Sint8); break; case AG_VARIABLE_UINT16: INCREMENT(Uint16); break; case AG_VARIABLE_SINT16: INCREMENT(Sint16); break; case AG_VARIABLE_UINT32: INCREMENT(Uint32); break; case AG_VARIABLE_SINT32: INCREMENT(Sint32); break; #ifdef HAVE_64BIT case AG_VARIABLE_UINT64: INCREMENT(Uint64); break; case AG_VARIABLE_SINT64: INCREMENT(Sint64); break; #endif default: break; } AG_PostEvent(NULL, sl, "slider-changed", NULL); AG_UnlockVariable(bVal); AG_UnlockVariable(bMin); AG_UnlockVariable(bMax); AG_UnlockVariable(bInc); AG_Redraw(sl); }
// Split up a job into smaller jobs and add each part to the given queue. // Returns the number of spots used, or -1 if failed to split up job. int addToQueue(int step, cell_t* myCells, constraint_t* myConstraints, job_queue_t* myJobQueue, assignment_t* assignments, int availableSpots) { int cellIndex, spotsUsed; int value = UNASSIGNED_VALUE; int originalAvailableSpots = availableSpots; job_t* job; if (step > MAX_JOB_LENGTH) return -1; cellIndex = getNextCellToFillN(myCells, myConstraints, availableSpots); if (cellIndex == IMPOSSIBLE_STATE) return 0; else if (cellIndex == TOO_MANY_POSSIBLES) return -1; // Preemptively assign each possible a spot in the queue, so there is always // at least room in queue to add the job itself availableSpots -= getNumPossibles(cells, cellIndex); assignments[step].cellIndex = cellIndex; while (UNASSIGNED_VALUE != (value = applyNextValue(myCells, myConstraints, cellIndex, value))) { assignments[step].value = value; spotsUsed = addToQueue(step + 1, myCells, myConstraints, myJobQueue, assignments, availableSpots + 1); // Able to add split up job to queue, so just update available spot count if (spotsUsed >= 0) { availableSpots -= (spotsUsed - 1); continue; } // Add job to queue since failed to add split up job to queue job = &(myJobQueue->queue[myJobQueue->tail]); memcpy(&(job->assignments), assignments, (step + 1) * sizeof(assignment_t)); job->length = step + 1; myJobQueue->tail = INCREMENT(myJobQueue->tail); } // Return the number of spots in the queue that were used // Note: this value is always in range of [0, originalAvailalbeSpots] return (originalAvailableSpots - availableSpots); }
//主函数 int main() { //初始化 int A[N-1]; int x,y; //调用递增函数 for (x=0; x<16; x++) { printf("x=%d:\t", x); INCREMENT(A); } //输出 printf("Final:\n"); for (y=N-1; y>-1; y--) { printf("%d\t", A[y]); } return 0; }
void CRYPTO_free(void *str, const char *file, int line) { INCREMENT(free_count); if (free_impl != NULL && free_impl != &CRYPTO_free) { free_impl(str, file, line); return; } #ifndef OPENSSL_NO_CRYPTO_MDEBUG if (call_malloc_debug) { CRYPTO_mem_debug_free(str, 0, file, line); free(str); CRYPTO_mem_debug_free(str, 1, file, line); } else { free(str); } #else free(str); #endif }
void umac96_digest (struct umac96_ctx *ctx, size_t length, uint8_t *digest) { uint32_t tag[4]; unsigned i; assert (length > 0); assert (length <= 12); if (ctx->index > 0 || ctx->count == 0) { /* Zero pad to multiple of 32 */ uint64_t y[3]; unsigned pad = (ctx->index > 0) ? 31 & - ctx->index : 32; memset (ctx->block + ctx->index, 0, pad); _umac_nh_n (y, 3, ctx->l1_key, ctx->index + pad, ctx->block); y[0] += 8 * ctx->index; y[1] += 8 * ctx->index; y[2] += 8 * ctx->index; _umac_l2 (ctx->l2_key, ctx->l2_state, 3, ctx->count++, y); } assert (ctx->count > 0); aes128_encrypt (&ctx->pdf_key, AES_BLOCK_SIZE, (uint8_t *) tag, ctx->nonce); INCREMENT (ctx->nonce_length, ctx->nonce); _umac_l2_final (ctx->l2_key, ctx->l2_state, 3, ctx->count); for (i = 0; i < 3; i++) tag[i] ^= ctx->l3_key2[i] ^ _umac_l3 (ctx->l3_key1 + 8*i, ctx->l2_state + 2*i); memcpy (digest, tag, length); /* Reinitialize */ ctx->count = ctx->index = 0; }
ssize_t BMA180::read(struct file *filp, char *buffer, size_t buflen) { unsigned count = buflen / sizeof(struct accel_report); int ret = 0; /* buffer must be large enough */ if (count < 1) return -ENOSPC; /* if automatic measurement is enabled */ if (_call_interval > 0) { /* * While there is space in the caller's buffer, and reports, copy them. * Note that we may be pre-empted by the measurement code while we are doing this; * we are careful to avoid racing with it. */ while (count--) { if (_oldest_report != _next_report) { memcpy(buffer, _reports + _oldest_report, sizeof(*_reports)); ret += sizeof(_reports[0]); INCREMENT(_oldest_report, _num_reports); } } /* if there was no data, warn the caller */ return ret ? ret : -EAGAIN; } /* manual measurement */ _oldest_report = _next_report = 0; measure(); /* measurement will have generated a report, copy it out */ memcpy(buffer, _reports, sizeof(*_reports)); ret = sizeof(*_reports); return ret; }
/* Sets V and key based on pdata */ static void drbg_aes_update(struct drbg_aes_ctx *ctx, const uint8_t pdata[DRBG_AES_SEED_SIZE]) { unsigned len = 0; uint8_t tmp[DRBG_AES_SEED_SIZE]; uint8_t *t = tmp; while (len < DRBG_AES_SEED_SIZE) { INCREMENT(sizeof(ctx->v), ctx->v); aes_encrypt(&ctx->key, AES_BLOCK_SIZE, t, ctx->v); t += AES_BLOCK_SIZE; len += AES_BLOCK_SIZE; } memxor(tmp, pdata, DRBG_AES_SEED_SIZE); aes_set_encrypt_key(&ctx->key, DRBG_AES_KEY_SIZE, tmp); memcpy(ctx->v, &tmp[DRBG_AES_KEY_SIZE], AES_BLOCK_SIZE); ctx->reseed_counter = 1; ctx->seeded = 1; }
void _PACK ( DopeVectorType * result, DopeVectorType * source, DopeVectorType * mask, DopeVectorType * vector) { char *cs; /* char ptr to source array */ char *cr; /* char ptr to result array */ char *cv; /* char ptr to vector array */ char * restrict cptr1; /* char */ char * restrict cptr2; /* char */ char * restrict cptr3; /* char */ _f_int8 * restrict uptr1; /* 64-bit */ _f_int8 * restrict uptr2; /* 64-bit */ _f_int8 * restrict uptr3; /* 64-bit */ _f_int * restrict fptr1; /* default-size */ _f_int * restrict fptr2; /* default-size */ _f_int * restrict fptr3; /* default-size */ _f_real16 * restrict dptr1; /* 128-bit */ _f_real16 * restrict dptr2; /* 128-bit */ _f_real16 * restrict dptr3; /* 128-bit */ #ifdef _F_COMP16 dblcmplx * restrict xptr1; /* 256-bit */ dblcmplx * restrict xptr2; /* 256-bit */ dblcmplx * restrict xptr3; /* 256-bit */ #endif _f_int4 * restrict hptr1; /* 32-bit */ _f_int4 * restrict hptr2; /* 32-bit */ _f_int4 * restrict hptr3; /* 32-bit */ _f_mask * restrict iptr4; /* def kind mask */ void * restrict sptr; /* ptr to source */ void * restrict rptr; /* ptr to result */ void * restrict mptr; /* ptr to mask */ void * restrict vptr; /* ptr to vector */ _f_int bucketsize; /* size of each data element */ long nbytes; /* # of bytes in data array */ long nwords; /* # of words in data array */ long curdim[MAXDIM]; /* current indices */ _f_int bytealligned; /* byte alligned flag */ long sindx; /* source index */ long rindx; /* result index */ long mindx; /* mask index */ long vindx; /* vector index */ _f_int type; /* type scalar */ _f_int subtype; /* sub-type */ _f_int arithmetic; /* arithmetic */ _f_int rank; /* dimension of source scalar */ long i, j, k; /* index variables */ long res_strd; /* element stride for result */ long vec_strd; /* element stride for result */ long src_ext[MAXDIM]; /* extents for source */ long src_strd[MAXDIM]; /* element stride for source */ long src_off[MAXDIM]; /* offset values for source */ long msk_strd[MAXDIM]; /* element stride for mask */ long msk_off[MAXDIM]; /* offset values for mask */ long indx1_src; /* index for dim 1 of source */ long indx2_src; /* index for dim 2 of source */ long indx1_vec; /* index for dim 1 of vector */ long indx2_vec; /* index for dim 2 of vector */ long indx1_res; /* index for dim 1 of result */ long indx2_res; /* index for dim 2 of result */ long indx1_msk; /* index for dim 1 of msk */ long indx2_msk; /* index for dim 2 of msk */ long total_ext; /* total extent counter */ long src_ext1; /* extent for dim 1 of source */ long src_ext2; /* extent for dim 1 of source */ long found; /* count of # entries in result */ long mask_el_len; _f_int early_exit; /* early exit flag */ /* Set type and dimension global variables */ type = source->type_lens.type; rank = source->n_dim; mask_el_len = mask->base_addr.a.el_len; /* * Check to see if any of the matrices have size 0. If any do, * return without doing anything. */ early_exit = 0; #ifdef _UNICOS #pragma _CRI shortloop #endif for (i = 0; i < rank; i++) { if (!source->dimension[i].extent) early_exit = 1; } if (result->assoc) { if (!result->dimension[0].extent) early_exit = 1; } if (vector) { if (!vector->dimension[0].extent) early_exit = 1; } if (mask) { if (mask->n_dim > 1) { #ifdef _UNICOS #pragma _CRI shortloop #endif for (i = 0; i < rank; i++) if (!mask->dimension[i].extent) early_exit = 1; } } /* * Initialize every array element to 0. */ #ifdef _UNICOS #pragma _CRI shortloop #endif for (i = 0; i < MAXDIM; i++) { curdim[i] = 0; src_ext[i] = 0; src_strd[i] = 0; src_off[i] = 0; msk_strd[i] = 0; msk_off[i] = 0; } /* Size calculation is based on variable type */ switch (type) { case DVTYPE_ASCII : bytealligned = 1; bucketsize = _fcdlen (source->base_addr.charptr); /* bytes */ subtype = DVSUBTYPE_CHAR; arithmetic = 0; break; case DVTYPE_DERIVEDBYTE : bytealligned = 1; bucketsize = source->base_addr.a.el_len / BITS_PER_BYTE; subtype = DVSUBTYPE_CHAR; arithmetic = 0; break; case DVTYPE_DERIVEDWORD : bytealligned = 0; bucketsize = source->base_addr.a.el_len / BITS_PER_WORD; subtype = DVSUBTYPE_DERIVED; arithmetic = 0; break; default : bytealligned = 0; bucketsize = source->type_lens.int_len / BITS_PER_WORD; if (source->type_lens.int_len == 64) { subtype = DVSUBTYPE_BIT64; } else if (source->type_lens.int_len == 32) { subtype = DVSUBTYPE_BIT32; bucketsize = 1; } else if (source->type_lens.int_len == 256) { subtype = DVSUBTYPE_BIT256; } else { subtype = DVSUBTYPE_BIT128; } arithmetic = 1; } /* If necessary, fill result dope vector */ if (!result->assoc) { result->base_addr.a.ptr = (void *) NULL; result->orig_base = 0; result->orig_size = 0; /* Determine size of space to allocate */ if (!bytealligned) { nbytes = bucketsize * BYTES_PER_WORD; #ifdef _CRAYMPP if (subtype == DVSUBTYPE_BIT32) nbytes /= 2; #endif } else { nbytes = bucketsize; } if (vector) { nbytes *= vector->dimension[0].extent; nwords = vector->dimension[0].extent; } else { #ifdef _UNICOS #pragma _CRI shortloop #endif for (i = 0; i < rank; i++) nbytes *= source->dimension[i].extent; nwords = nbytes / BYTES_PER_WORD; } if (nbytes > 0) { result->base_addr.a.ptr = (void *) malloc (nbytes); if (result->base_addr.a.ptr == NULL) _lerror (_LELVL_ABORT, FENOMEMY); } result->assoc = 1; result->base_addr.a.el_len = source->base_addr.a.el_len; if (type == DVTYPE_ASCII) { cr = (char *) result->base_addr.a.ptr; result->base_addr.charptr = _cptofcd (cr, bucketsize); } result->orig_size = nbytes * BITS_PER_BYTE; /* * These are initial values which may be changed when it is * determined how big the result array actually is. */ result->dimension[0].low_bound = 1; result->dimension[0].extent = nwords; result->dimension[0].stride_mult = bucketsize; /* if result array is already allocated */ } else { if (!bytealligned) nbytes = bucketsize * BYTES_PER_WORD; else nbytes = bucketsize; if (vector) { nbytes *= vector->dimension[0].extent; nwords = vector->dimension[0].extent; } else { nwords = 1; for (i = 0; i < rank; i++) { nbytes *= source->dimension[i].extent; nwords *= source->dimension[i].extent; } } } /* If early exit is required, exit now */ if (early_exit) return; if (mask) { iptr4 = (_f_mask *) mask->base_addr.a.ptr; if (mask->n_dim == 0 && !(vector) && !LTOB(mask_el_len, &iptr4[0])) { result->dimension[0].extent = 0; return; } } /* Set up scalar pointers to all of the argument data areas */ if (mask) mptr = (void *) mask->base_addr.a.ptr; if (!bytealligned) { sptr = (void *) source->base_addr.a.ptr; rptr = (void *) result->base_addr.a.ptr; if (vector) vptr = (void *) vector->base_addr.a.ptr; } else { if (type == DVTYPE_ASCII) { cs = _fcdtocp (source->base_addr.charptr); cr = _fcdtocp (result->base_addr.charptr); if (vector) cv = _fcdtocp (vector->base_addr.charptr); } else { cs = (char *) source->base_addr.a.ptr; cr = (char *) result->base_addr.a.ptr; if (vector) cv = (char *) vector->base_addr.a.ptr; } } /* Set up some 'shortcut' variables used for index calculation */ if (bucketsize > 1 && arithmetic) { res_strd = result->dimension[0].stride_mult / bucketsize; if (vector) vec_strd = vector->dimension[0].stride_mult / bucketsize; } else { res_strd = result->dimension[0].stride_mult; if (vector) vec_strd = vector->dimension[0].stride_mult; } #ifdef _UNICOS #pragma _CRI shortloop #endif for (i = 0; i < rank; i++) { src_ext[i] = source->dimension[i].extent; if (bucketsize > 1 && arithmetic) { src_strd[i] = source->dimension[i].stride_mult / bucketsize; } else { src_strd[i] = source->dimension[i].stride_mult; } } if (mask->n_dim > 0) { #ifdef _UNICOS #pragma _CRI shortloop #endif for (i = 0; i < rank; i++) { msk_strd[i] = mask->dimension[i].stride_mult; iptr4 = (_f_mask *) mptr; #ifdef _CRAYMPP if (mask_el_len == 64 && sizeof (iptr4[0]) == 4) msk_strd[i] <<= 1; #endif } } /* * The program is divided up into three blocks. The first block deals * with arrays of rank 1. Inside each block, the data types are broken * up into groups based on container size. Integer, real, and logical * types are all one word, and the actual value is not used, so they * are all grouped together and treated as long. The same is * true for double and complex, as well as ascii and derivedbyte. * * For each group, the mask array is checked for true values. When one * is encountered, the corresponding value from the source array is put * into the next available position in the result array. If no vector * is passed, the routine is finished at this point with the result * array length set to the number of true elements in the mask. If a * vector is furnished, the size of the vector determines the size of * the result array. If this size has been reached, the routine is done. * If not, elements from the vector array are put into the result array * until it is full. */ if (rank == 1) { found = 0; iptr4 = (_f_mask *) mptr; switch (subtype) { case DVSUBTYPE_BIT64 : uptr1 = (_f_int8 *) sptr; uptr2 = (_f_int8 *) vptr; uptr3 = (_f_int8 *) rptr; rindx = 0; mindx = 0; vindx = 0; sindx = 0; src_ext1 = source->dimension[0].extent; for (i = 0; i < src_ext1; i++) { if (LTOB(mask_el_len, &iptr4[mindx])) { sindx = i * src_strd[0]; uptr3[rindx] = uptr1[sindx]; rindx += res_strd; found++; } mindx += msk_strd[0]; } if (!vector || found == nwords) { result->dimension[0].extent = found; } else { vindx = found * vec_strd; for ( ; found < nwords; found++) { uptr3[rindx] = uptr2[vindx]; rindx += res_strd; vindx += vec_strd; } } break; case DVSUBTYPE_BIT32 : hptr1 = (_f_int4 *) sptr; hptr2 = (_f_int4 *) vptr; hptr3 = (_f_int4 *) rptr; rindx = 0; mindx = 0; vindx = 0; sindx = 0; src_ext1 = source->dimension[0].extent; for (i = 0; i < src_ext1; i++) { if (LTOB(mask_el_len, &iptr4[mindx])) { sindx = i * src_strd[0]; hptr3[rindx] = hptr1[sindx]; rindx += res_strd; found++; } mindx += msk_strd[0]; } if (!vector || found == nwords) { result->dimension[0].extent = found; } else { vindx = found * vec_strd; for ( ; found < nwords; found++) { hptr3[rindx] = hptr2[vindx]; rindx += res_strd; vindx += vec_strd; } } break; case DVSUBTYPE_BIT128 : dptr1 = (_f_real16 *) sptr; dptr2 = (_f_real16 *) vptr; dptr3 = (_f_real16 *) rptr; rindx = 0; mindx = 0; vindx = 0; sindx = 0; src_ext1 = source->dimension[0].extent; for (i = 0; i < src_ext1; i++) { if (LTOB(mask_el_len, &iptr4[mindx])) { sindx = i * src_strd[0]; dptr3[rindx] = dptr1[sindx]; rindx += res_strd; found++; } mindx += msk_strd[0]; } if (!vector || found == nwords) { result->dimension[0].extent = found; } else { vindx = found * vec_strd; for ( ; found < nwords; found++) { dptr3[rindx] = dptr2[vindx]; rindx += res_strd; vindx += vec_strd; } } break; case DVSUBTYPE_CHAR : rindx = 0; mindx = 0; vindx = 0; sindx = 0; src_ext1 = source->dimension[0].extent; for (i = 0; i < src_ext1; i++) { if (LTOB(mask_el_len, &iptr4[mindx])) { cptr3 = (char *) cr + rindx; cptr1 = (char *) cs + (i * src_strd[0]); (void) memcpy (cptr3, cptr1, bucketsize); rindx += res_strd; found++; } mindx += msk_strd[0]; } if (!vector || found == nwords) { result->dimension[0].extent = found; } else { vindx = found * vec_strd; for ( ; found < nwords; found++) { cptr3 = (char *) cr + rindx; cptr2 = (char *) cv + vindx; (void) memcpy (cptr3, cptr2, bucketsize); rindx += res_strd; vindx += vec_strd; } } break; case DVSUBTYPE_DERIVED : fptr1 = (_f_int *) sptr; fptr2 = (_f_int *) vptr; fptr3 = (_f_int *) rptr; src_ext1 = source->dimension[0].extent; indx1_res = 0; /* * The derived word type is handled the same as the other types except * that another loop is added. The assumption was made that extent of * the array would be larger than the number of words in the derived * type. Therefore, to try and make this routine optimal, the first * loop uses the extent as its inner loop, which should provide better * optimization. The second loop is also done this way. */ for (i = 0; i < bucketsize; i++) { rindx = i; mindx = 0; for (j = 0; j < src_ext1; j++) { if (LTOB(mask_el_len, &iptr4[mindx])) { sindx = i + (j * src_strd[0]); fptr3[rindx] = fptr1[sindx]; rindx += res_strd; if (i == 0) { indx1_res = rindx; found++; } } mindx += msk_strd[0]; } } if (!vector || found == nwords) { result->dimension[0].extent = found; } else { indx1_vec = found * vec_strd; found = nwords - found; for (i = 0; i < bucketsize; i++) { rindx = indx1_res + i; vindx = indx1_vec + i; for (j = 0; j < found; j++) { fptr3[rindx] = fptr2[vindx]; rindx += res_strd; vindx += vec_strd; } } } break; #ifdef _F_COMP16 case DVSUBTYPE_BIT256 : xptr1 = (dblcmplx *) sptr; xptr2 = (dblcmplx *) vptr; xptr3 = (dblcmplx *) rptr; rindx = 0; mindx = 0; vindx = 0; sindx = 0; src_ext1 = source->dimension[0].extent; for (i = 0; i < src_ext1; i++) { if (LTOB(mask_el_len, &iptr4[mindx])) { sindx = i * src_strd[0]; xptr3[rindx].re = xptr1[sindx].re; xptr3[rindx].im = xptr1[sindx].im; rindx += res_strd; found++; } mindx += msk_strd[0]; } if (!vector || found == nwords) { result->dimension[0].extent = found; } else { vindx = found * vec_strd; for ( ; found < nwords; found++) { xptr3[rindx].re = xptr2[vindx].re; xptr3[rindx].im = xptr2[vindx].im; rindx += res_strd; vindx += vec_strd; } } break; #endif default : _lerror (_LELVL_ABORT, FEINTDTY); } } else if (rank == 2) { /* * Rank 2 matrices are handled in a manner similar to rank 1 arrays, * except that the first loop in each data type is a nested loop, with * the outer loop being the second dimension, and the inner loop being * the first. This preserves the storage order which is necessary for * pack to work. The second part of each block is not affected by the * number of dimensions in the source matrix. */ found = 0; iptr4 = (_f_mask *) mptr; switch (subtype) { case DVSUBTYPE_BIT64 : uptr1 = (_f_int8 *) sptr; uptr2 = (_f_int8 *) vptr; uptr3 = (_f_int8 *) rptr; indx2_msk = 0; indx2_src = 0; rindx = 0; src_ext1 = src_ext[0]; src_ext2 = src_ext[1]; for (i = 0; i < src_ext2; i++) { indx1_msk = 0; for (j = 0; j < src_ext1; j++) { mindx = indx1_msk + indx2_msk; if (LTOB(mask_el_len, &iptr4[mindx])) { sindx = indx2_src + (j * src_strd[0]); uptr3[rindx] = uptr1[sindx]; rindx += res_strd; found++; } indx1_msk += msk_strd[0]; } indx2_msk += msk_strd[1]; indx2_src += src_strd[1]; } if (!vector || found == nwords) { result->dimension[0].extent = found; } else { vindx = found * vec_strd; for ( ; found < nwords; found++) { uptr3[rindx] = uptr2[vindx]; rindx += res_strd; vindx += vec_strd; } } break; case DVSUBTYPE_BIT32 : hptr1 = (_f_int4 *) sptr; hptr2 = (_f_int4 *) vptr; hptr3 = (_f_int4 *) rptr; indx2_msk = 0; indx2_src = 0; rindx = 0; src_ext1 = src_ext[0]; src_ext2 = src_ext[1]; for (i = 0; i < src_ext2; i++) { indx1_msk = 0; for (j = 0; j < src_ext1; j++) { mindx = indx1_msk + indx2_msk; if (LTOB(mask_el_len, &iptr4[mindx])) { sindx = indx2_src + (j * src_strd[0]); hptr3[rindx] = hptr1[sindx]; rindx += res_strd; found++; } indx1_msk += msk_strd[0]; } indx2_msk += msk_strd[1]; indx2_src += src_strd[1]; } if (!vector || found == nwords) { result->dimension[0].extent = found; } else { vindx = found * vec_strd; for ( ; found < nwords; found++) { hptr3[rindx] = hptr2[vindx]; rindx += res_strd; vindx += vec_strd; } } break; case DVSUBTYPE_BIT128 : dptr1 = (_f_real16 *) sptr; dptr2 = (_f_real16 *) vptr; dptr3 = (_f_real16 *) rptr; indx2_msk = 0; indx2_src = 0; rindx = 0; src_ext1 = src_ext[0]; src_ext2 = src_ext[1]; for (i = 0; i < src_ext2; i++) { indx1_msk = 0; for (j = 0; j < src_ext1; j++) { mindx = indx1_msk + indx2_msk; if (LTOB(mask_el_len, &iptr4[mindx])) { sindx = indx2_src + (j * src_strd[0]); dptr3[rindx] = dptr1[sindx]; rindx += res_strd; found++; } indx1_msk += msk_strd[0]; } indx2_msk += msk_strd[1]; indx2_src += src_strd[1]; } if (!vector || found == nwords) { result->dimension[0].extent = found; } else { vindx = found * vec_strd; for ( ; found < nwords; found++) { dptr3[rindx] = dptr2[vindx]; rindx += res_strd; vindx += vec_strd; } } break; case DVSUBTYPE_CHAR : indx2_msk = 0; indx2_src = 0; rindx = 0; src_ext1 = src_ext[0]; src_ext2 = src_ext[1]; for (i = 0; i < src_ext2; i++) { indx1_msk = 0; for (j = 0; j < src_ext1; j++) { mindx = indx1_msk + indx2_msk; if (LTOB(mask_el_len, &iptr4[mindx])) { sindx = indx2_src + (j * src_strd[0]); cptr1 = (char *) cs + sindx; cptr3 = (char *) cr + rindx; (void) memcpy (cptr3, cptr1, bucketsize); rindx += res_strd; found++; } indx1_msk += msk_strd[0]; } indx2_msk += msk_strd[1]; indx2_src += src_strd[1]; } if (!vector || found == nwords) { result->dimension[0].extent = found; } else { vindx = found * vec_strd; for ( ; found < nwords; found++) { cptr2 = (char *) cv + vindx; cptr3 = (char *) cr + rindx; (void) memcpy (cptr3, cptr2, bucketsize); rindx += res_strd; vindx += vec_strd; } } break; case DVSUBTYPE_DERIVED : fptr1 = (_f_int *) sptr; fptr2 = (_f_int *) vptr; fptr3 = (_f_int *) rptr; src_ext1 = src_ext[0]; src_ext2 = src_ext[1]; for (i = 0; i < bucketsize; i++) { indx2_msk = 0; indx2_src = 0; rindx = i; for (j = 0; j < src_ext2; j++) { indx1_msk = 0; for (k = 0; k < src_ext1; k++) { mindx = indx1_msk + indx2_msk; if (LTOB(mask_el_len, &iptr4[mindx])) { sindx = indx2_src + i + (k * src_strd[0]); fptr3[rindx] = fptr1[sindx]; rindx += res_strd; if (i == 0) found++; } indx1_msk += msk_strd[0]; } indx2_msk += msk_strd[1]; indx2_src += src_strd[1]; } } if (!vector || found == nwords) { result->dimension[0].extent = found; } else { indx1_res = found * res_strd; indx1_vec = found * vec_strd; found = nwords - found; for (i = 0; i < bucketsize; i++) { rindx = indx1_res + i; vindx = indx1_vec + i; for (j = 0; j < found; j++) { fptr3[rindx] = fptr2[vindx]; rindx += res_strd; vindx += vec_strd; } } } break; #ifdef _F_COMP16 case DVSUBTYPE_BIT256 : xptr1 = (dblcmplx *) sptr; xptr2 = (dblcmplx *) vptr; xptr3 = (dblcmplx *) rptr; indx2_msk = 0; indx2_src = 0; rindx = 0; src_ext1 = src_ext[0]; src_ext2 = src_ext[1]; for (i = 0; i < src_ext2; i++) { indx1_msk = 0; for (j = 0; j < src_ext1; j++) { mindx = indx1_msk + indx2_msk; if (LTOB(mask_el_len, &iptr4[mindx])) { sindx = indx2_src + (j * src_strd[0]); xptr3[rindx].re = xptr1[sindx].re; xptr3[rindx].im = xptr1[sindx].im; rindx += res_strd; found++; } indx1_msk += msk_strd[0]; } indx2_msk += msk_strd[1]; indx2_src += src_strd[1]; } if (!vector || found == nwords) { result->dimension[0].extent = found; } else { vindx = found * vec_strd; for ( ; found < nwords; found++) { xptr3[rindx].re = xptr2[vindx].re; xptr3[rindx].im = xptr2[vindx].im; rindx += res_strd; vindx += vec_strd; } } break; #endif default : _lerror (_LELVL_ABORT, FEINTDTY); } } else { /* rank 3-7 */ /* * Ranks 3 through 7 are all handled in this last block. It was assumed * that ranks 1 and 2 would account for the majority of calls to pack, * and that the remaining ranks could be done in one block. * * The logic behind these blocks is the same as for the other ranks. * The first part of the routine uses two nested loops, with the inner * loop being the first dimension, and the outer loop being the product * of all of the remaining dimensions. A array of counters keeps track * of the values for each of the dimensions. Two macros are used in * this block. INCREMENT are used to calculate the values of each of * the dimension counters, and to calculate the offsets into the array * for each index. FIND_INDX sums these offsets into one offset, which * is used for each iteration of the inner loop. As with the other two * blocks, the second part of each section is not affected by the number * of dimensions in the source matrix. * * Calculate the product of each of the dimensions 2-n. This is the * number of times the outer loop will be executed. Also, initialize * the offset and dimension counter arrays. */ total_ext = 1; #ifdef _UNICOS #pragma _CRI shortloop #endif for (i = 0; i < MAXDIM; i++) { curdim[i] = 0; msk_off[i] = 0; src_off[i] = 0; } #ifdef _UNICOS #pragma _CRI shortloop #endif for (i = 1; i < rank; i++) total_ext *= source->dimension[i].extent; iptr4 = (_f_mask *) mptr; found = 0; switch (subtype) { case DVSUBTYPE_BIT64 : uptr2 = (_f_int8 *) vptr; uptr3 = (_f_int8 *) rptr; rindx = 0; for (i = 0; i < total_ext; i++) { FIND_INDX(); uptr1 = (_f_int8 *) sptr + indx1_src; iptr4 = (_f_mask *) mptr + indx1_msk; for (j = 0; j < src_ext[0]; j++) { mindx = j * msk_strd[0]; if (LTOB(mask_el_len, &iptr4[mindx])) { sindx = j * src_strd[0]; uptr3[rindx] = uptr1[sindx]; rindx += res_strd; found++; } } INCREMENT(); } if (!vector || found == nwords) { result->dimension[0].extent = found; } else { vindx = found * vec_strd; for ( ; found < nwords; found++) { uptr3[rindx] = uptr2[vindx]; rindx += res_strd; vindx += vec_strd; } } break; case DVSUBTYPE_BIT32 : hptr2 = (_f_int4 *) vptr; hptr3 = (_f_int4 *) rptr; rindx = 0; for (i = 0; i < total_ext; i++) { FIND_INDX(); hptr1 = (_f_int4 *) sptr + indx1_src; iptr4 = (_f_mask *) mptr + indx1_msk; for (j = 0; j < src_ext[0]; j++) { mindx = j * msk_strd[0]; if (LTOB(mask_el_len, &iptr4[mindx])) { sindx = j * src_strd[0]; hptr3[rindx] = hptr1[sindx]; rindx += res_strd; found++; } } INCREMENT(); } if (!vector || found == nwords) { result->dimension[0].extent = found; } else { vindx = found * vec_strd; for ( ; found < nwords; found++) { hptr3[rindx] = hptr2[vindx]; rindx += res_strd; vindx += vec_strd; } } break; case DVSUBTYPE_BIT128 : dptr2 = (_f_real16 *) vptr; dptr3 = (_f_real16 *) rptr; rindx = 0; for (i = 0; i < total_ext; i++) { FIND_INDX(); dptr1 = (_f_real16 *) sptr + indx1_src; iptr4 = (_f_mask *) mptr + indx1_msk; for (j = 0; j < src_ext[0]; j++) { mindx = j * msk_strd[0]; if (LTOB(mask_el_len, &iptr4[mindx])) { sindx = j * src_strd[0]; dptr3[rindx] = dptr1[sindx]; rindx += res_strd; found++; } } INCREMENT(); } if (!vector || found == nwords) { result->dimension[0].extent = found; } else { vindx = found * vec_strd; for ( ; found < nwords; found++) { dptr3[rindx] = dptr2[vindx]; rindx += res_strd; vindx += vec_strd; } } break; case DVSUBTYPE_CHAR : cptr2 = (char *) vptr; cptr3 = (char *) rptr; rindx = 0; for (i = 0; i < total_ext; i++) { FIND_INDX(); iptr4 = (_f_mask *) mptr + indx1_msk; for (j = 0; j < src_ext[0]; j++) { mindx = j * msk_strd[0]; if (LTOB(mask_el_len, &iptr4[mindx])) { sindx = indx1_src + (j * src_strd[0]); cptr1 = (char *) cs + sindx; cptr3 = (char *) cr + rindx; (void) memcpy (cptr3, cptr1, bucketsize); rindx += res_strd; found++; } } INCREMENT(); } if (!vector || found == nwords) { result->dimension[0].extent = found; } else { vindx = found * vec_strd; for ( ; found < nwords; found++) { cptr3 = (char *) cr + rindx; cptr2 = (char *) cv + vindx; (void) memcpy (cptr3, cptr2, bucketsize); rindx += res_strd; vindx += vec_strd; } } break; case DVSUBTYPE_DERIVED : fptr2 = (_f_int *) vptr; fptr3 = (_f_int *) rptr; for (i = 0; i < bucketsize; i++) { rindx = i; for (j = 0; j < rank; j++) { msk_off[j] = 0; src_off[j] = 0; curdim[j] = 0; } for (j = 0; j < total_ext; j++) { FIND_INDX(); fptr1 = (_f_int *) sptr + i + indx1_src; iptr4 = (_f_mask *) mptr + indx1_msk; for (k = 0; k < src_ext[0]; k++) { mindx = k * msk_strd[0]; if (LTOB(mask_el_len, &iptr4[mindx])) { sindx = k * src_strd[0]; fptr3[rindx] = fptr1[sindx]; rindx += res_strd; if (i == 0) found++; } } INCREMENT(); } } if (!vector || found == nwords) { result->dimension[0].extent = found; } else { indx1_res = found * res_strd; indx1_vec = found * vec_strd; found = nwords - found; for (i = 0; i < bucketsize; i++) { rindx = indx1_res + i; vindx = indx1_vec + i; for (j = 0; j < found; j++) { fptr3[rindx] = fptr2[vindx]; rindx += res_strd; vindx += vec_strd; } } } break; #ifdef _F_COMP16 case DVSUBTYPE_BIT256 : xptr2 = (dblcmplx *) vptr; xptr3 = (dblcmplx *) rptr; rindx = 0; for (i = 0; i < total_ext; i++) { FIND_INDX(); xptr1 = (dblcmplx *) sptr + indx1_src; iptr4 = (_f_mask *) mptr + indx1_msk; for (j = 0; j < src_ext[0]; j++) { mindx = j * msk_strd[0]; if (LTOB(mask_el_len, &iptr4[mindx])) { sindx = j * src_strd[0]; xptr3[rindx].re = xptr1[sindx].re; xptr3[rindx].im = xptr1[sindx].im; rindx += res_strd; found++; } } INCREMENT(); } if (!vector || found == nwords) { result->dimension[0].extent = found; } else { vindx = found * vec_strd; for ( ; found < nwords; found++) { xptr3[rindx].re = xptr2[vindx].re; xptr3[rindx].im = xptr2[vindx].im; rindx += res_strd; vindx += vec_strd; } } if (!vector || found == nwords) { result->dimension[0].extent = found; } else { vindx = found * vec_strd; for ( ; found < nwords; found++) { xptr3[rindx].re = xptr2[vindx].re; xptr3[rindx].im = xptr2[vindx].im; rindx += res_strd; vindx += vec_strd; } } break; #endif default : _lerror (_LELVL_ABORT, FEINTDTY); } } }
/* collect MPPT measurements: */ int SPV1020::mppt_measurement() { /* read the most recent measurement */ perf_begin(_sample_perf); /* this should be fairly close to the end of the conversion, so the best approximation of the time */ _reports[_next_report].timestamp = hrt_absolute_time(); for(uint8_t mppt_device=0; mppt_device < MAX_NUMBER_OF_MPPT_DEVICES; mppt_device++){ mppt_current[mppt_device] = 0.0f; mppt_voltage[mppt_device] = 0.0f; mppt_pwm[mppt_device] = 0; mppt_status[mppt_device] = 0; } for(uint8_t mppt_device=0; mppt_device < max_mppt_devices; mppt_device++){ if (OK != mppt_current_read(mppt_device)){ perf_count(_comms_errors); return -EIO; } if (OK != mppt_voltage_read(mppt_device)){ perf_count(_comms_errors); return -EIO; } if (OK != mppt_pwm_read(mppt_device)){ perf_count(_comms_errors); return -EIO; } if (OK != mppt_status_read(mppt_device)){ perf_count(_comms_errors); return -EIO; } //warnx("MPPT device %d: Current: %10.4f[A], Voltage: %10.4f[v], PWM: %d, Status vector: %x, \n", mppt_device, (double)(mppt_current[mppt_device]), (double)(mppt_voltage[mppt_device]), mppt_pwm[mppt_device], mppt_status[mppt_device]); // remove display!!!!!!!! } memcpy(&_reports[_next_report].current, mppt_current, sizeof(mppt_current)); /* current report [amp] */ memcpy(&_reports[_next_report].voltage, mppt_voltage, sizeof(mppt_voltage)); /* voltage report [v] */ memcpy(&_reports[_next_report].pwm, mppt_pwm, sizeof(mppt_pwm)); /* pwm report */ memcpy(&_reports[_next_report].status, mppt_status, sizeof(mppt_status)); /* Status vector report */ /* publish it */ orb_publish(ORB_ID(sensor_spv1020), _mppt_topic, &_reports[_next_report]); /* post a report to the ring - note, not locked */ INCREMENT(_next_report, _num_reports); /* if we are running up against the oldest report, toss it */ if (_next_report == _oldest_report) { perf_count(_buffer_overflows); INCREMENT(_oldest_report, _num_reports); } /* notify anyone waiting for data */ poll_notify(POLLIN); perf_end(_sample_perf); return OK; }
int HMC5883::collect() { #pragma pack(push, 1) struct { /* status register and data as read back from the device */ uint8_t x[2]; uint8_t z[2]; uint8_t y[2]; } hmc_report; #pragma pack(pop) struct { int16_t x, y, z; } report; int ret = -EIO; uint8_t cmd; perf_begin(_sample_perf); /* this should be fairly close to the end of the measurement, so the best approximation of the time */ _reports[_next_report].timestamp = hrt_absolute_time(); /* * @note We could read the status register here, which could tell us that * we were too early and that the output registers are still being * written. In the common case that would just slow us down, and * we're better off just never being early. */ /* get measurements from the device */ cmd = ADDR_DATA_OUT_X_MSB; ret = transfer(&cmd, 1, (uint8_t *)&hmc_report, sizeof(hmc_report)); if (ret != OK) { perf_count(_comms_errors); debug("data/status read error"); goto out; } /* swap the data we just received */ report.x = (((int16_t)hmc_report.x[0]) << 8) + hmc_report.x[1]; report.y = (((int16_t)hmc_report.y[0]) << 8) + hmc_report.y[1]; report.z = (((int16_t)hmc_report.z[0]) << 8) + hmc_report.z[1]; /* * If any of the values are -4096, there was an internal math error in the sensor. * Generalise this to a simple range check that will also catch some bit errors. */ if ((abs(report.x) > 2048) || (abs(report.y) > 2048) || (abs(report.z) > 2048)) goto out; /* * RAW outputs * * to align the sensor axes with the board, x and y need to be flipped * and y needs to be negated */ _reports[_next_report].x_raw = report.y; _reports[_next_report].y_raw = ((report.x == -32768) ? 32767 : -report.x); /* z remains z */ _reports[_next_report].z_raw = report.z; /* scale values for output */ /* * 1) Scale raw value to SI units using scaling from datasheet. * 2) Subtract static offset (in SI units) * 3) Scale the statically calibrated values with a linear * dynamically obtained factor * * Note: the static sensor offset is the number the sensor outputs * at a nominally 'zero' input. Therefore the offset has to * be subtracted. * * Example: A gyro outputs a value of 74 at zero angular rate * the offset is 74 from the origin and subtracting * 74 from all measurements centers them around zero. */ #ifdef PX4_I2C_BUS_ONBOARD if (_bus == PX4_I2C_BUS_ONBOARD) { /* to align the sensor axes with the board, x and y need to be flipped */ _reports[_next_report].x = ((report.y * _range_scale) - _scale.x_offset) * _scale.x_scale; /* flip axes and negate value for y */ _reports[_next_report].y = ((((report.x == -32768) ? 32767 : -report.x) * _range_scale) - _scale.y_offset) * _scale.y_scale; /* z remains z */ _reports[_next_report].z = ((report.z * _range_scale) - _scale.z_offset) * _scale.z_scale; } else { #endif /* the standard external mag by 3DR has x pointing to the right, y pointing backwards, and z down, * therefore switch x and y and invert y */ _reports[_next_report].x = ((((report.y == -32768) ? 32767 : -report.y) * _range_scale) - _scale.x_offset) * _scale.x_scale; /* flip axes and negate value for y */ _reports[_next_report].y = ((report.x * _range_scale) - _scale.y_offset) * _scale.y_scale; /* z remains z */ _reports[_next_report].z = ((report.z * _range_scale) - _scale.z_offset) * _scale.z_scale; #ifdef PX4_I2C_BUS_ONBOARD } #endif /* publish it */ orb_publish(ORB_ID(sensor_mag), _mag_topic, &_reports[_next_report]); /* post a report to the ring - note, not locked */ INCREMENT(_next_report, _num_reports); /* if we are running up against the oldest report, toss it */ if (_next_report == _oldest_report) { perf_count(_buffer_overflows); INCREMENT(_oldest_report, _num_reports); } /* notify anyone waiting for data */ poll_notify(POLLIN); ret = OK; out: perf_end(_sample_perf); return ret; }
int BAROSIM::collect() { int ret; #pragma pack(push, 1) struct { float pressure; float altitude; float temperature; } baro_report; #pragma pack(pop) perf_begin(_sample_perf); struct baro_report report; /* this should be fairly close to the end of the conversion, so the best approximation of the time */ report.timestamp = hrt_absolute_time(); report.error_count = perf_event_count(_comms_errors); /* read the most recent measurement - read offset/size are hardcoded in the interface */ ret = _interface->dev_read(0, (void *)&baro_report, sizeof(baro_report)); if (ret < 0) { perf_count(_comms_errors); perf_end(_sample_perf); return ret; } /* handle a measurement */ if (_measure_phase == 0) { report.pressure = baro_report.pressure; report.altitude = baro_report.altitude; report.temperature = baro_report.temperature; report.timestamp = hrt_absolute_time(); } else { report.pressure = baro_report.pressure; report.altitude = baro_report.altitude; report.temperature = baro_report.temperature; report.timestamp = hrt_absolute_time(); /* publish it */ if (!(_pub_blocked)) { if (_baro_topic != nullptr) { /* publish it */ orb_publish(ORB_ID(sensor_baro), _baro_topic, &report); } else { PX4_WARN("BAROSIM::collect _baro_topic not initialized"); } } if (_reports->force(&report)) { perf_count(_buffer_overflows); } /* notify anyone waiting for data */ poll_notify(POLLIN); } /* update the measurement state machine */ INCREMENT(_measure_phase, BAROSIM_MEASUREMENT_RATIO + 1); perf_end(_sample_perf); return OK; }
int MEASAirspeed::collect() { int ret = -EIO; /* read from the sensor */ uint8_t val[4] = {0, 0, 0, 0}; perf_begin(_sample_perf); ret = transfer(nullptr, 0, &val[0], 4); if (ret < 0) { log("error reading from sensor: %d", ret); return ret; } uint8_t status = val[0] & 0xC0; if (status == 2) { log("err: stale data"); } else if (status == 3) { log("err: fault"); } //uint16_t diff_pres_pa = (val[1]) | ((val[0] & ~(0xC0)) << 8); uint16_t temp = (val[3] & 0xE0) << 8 | val[2]; // XXX leaving this in until new calculation method has been cross-checked //diff_pres_pa = abs(diff_pres_pa - (16384 / 2.0f)); //diff_pres_pa -= _diff_pres_offset; int16_t dp_raw = 0, dT_raw = 0; dp_raw = (val[0] << 8) + val[1]; dp_raw = 0x3FFF & dp_raw; dT_raw = (val[2] << 8) + val[3]; dT_raw = (0xFFE0 & dT_raw) >> 5; float temperature = ((200 * dT_raw) / 2047) - 50; // XXX we may want to smooth out the readings to remove noise. // Calculate differential pressure. As its centered around 8000 // and can go positive or negative, enforce absolute value uint16_t diff_press_pa = abs(dp_raw - (16384 / 2.0f)); _reports[_next_report].timestamp = hrt_absolute_time(); _reports[_next_report].temperature = temperature; _reports[_next_report].differential_pressure_pa = diff_press_pa; // Track maximum differential pressure measured (so we can work out top speed). if (diff_press_pa > _reports[_next_report].max_differential_pressure_pa) { _reports[_next_report].max_differential_pressure_pa = diff_press_pa; } /* announce the airspeed if needed, just publish else */ orb_publish(ORB_ID(differential_pressure), _airspeed_pub, &_reports[_next_report]); /* post a report to the ring - note, not locked */ INCREMENT(_next_report, _num_reports); /* if we are running up against the oldest report, toss it */ if (_next_report == _oldest_report) { perf_count(_buffer_overflows); INCREMENT(_oldest_report, _num_reports); } /* notify anyone waiting for data */ poll_notify(POLLIN); ret = OK; perf_end(_sample_perf); return ret; }