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;
}
