inline std::size_t write_at(SyncRandomAccessWriteDevice& d,
boost::uint64_t offset, const ConstBufferSequence& buffers,
CompletionCondition completion_condition)
{
asio::error_code ec;
std::size_t bytes_transferred = write_at(
d, offset, buffers, completion_condition, ec);
asio::detail::throw_error(ec, "write_at");
return bytes_transferred;
}
inline std::size_t write_at(SyncRandomAccessWriteDevice& d,
boost::uint64_t offset, asio::basic_streambuf<Allocator>& b,
CompletionCondition completion_condition)
{
asio::error_code ec;
std::size_t bytes_transferred = write_at(
d, offset, b, completion_condition, ec);
asio::detail::throw_error(ec);
return bytes_transferred;
}
void hh_add(value key, value data) {
unsigned long hash = get_hash(key);
unsigned int slot = hash & (HASHTBL_SIZE - 1);
while(1) {
unsigned long slot_hash = hashtbl[slot].hash;
if(slot_hash == hash) {
write_at(slot, data);
return;
}
if(slot_hash == 0) {
// We think we might have a free slot, try to atomically grab it.
if(__sync_bool_compare_and_swap(&(hashtbl[slot].hash), 0, hash)) {
unsigned long size = __sync_fetch_and_add(hcounter, 1);
assert(size < HASHTBL_SIZE);
write_at(slot, data);
return;
}
// Grabbing it failed -- why? If someone else is trying to insert
// the data we were about to, try to insert it ourselves too.
// Otherwise, keep going.
// Note that this read relies on the __sync call above preventing the
// compiler from caching the value read out of memory. (And of course
// isn't safe on any arch that requires memory barriers.)
if(hashtbl[slot].hash == hash) {
// Some other thread already grabbed this slot to write this
// key, but they might not have written the address (or even
// the sigil value) yet. We can't return from hh_add until we
// know that hh_mem would succeed, which is to say that addr is
// no longer null. To make sure hh_mem will work, we try
// writing the value ourselves; either we insert it ourselves or
// we know the address is now non-NULL.
write_at(slot, data);
return;
}
}
slot = (slot + 1) & (HASHTBL_SIZE - 1);
}
}
/*
* Define the position of a label, resolving any fixups associated with
* the label.
*/
void CTcCodeStream::def_label_pos(CTcCodeLabel *lbl)
{
/* set the label's position */
lbl->ofs = ofs_;
lbl->is_known = TRUE;
/* resolve each fixup */
while (lbl->fhead != 0)
{
CTcLabelFixup *fixup;
long diff;
char buf[4];
/* pull this fixup off of the active list */
fixup = lbl->fhead;
lbl->fhead = lbl->fhead->nxt;
/*
* calculate the offset from the fixup position to the label
* position, applying the bias to the fixup position
*/
diff = lbl->ofs - (fixup->ofs + fixup->bias);
/* convert the offset to the correct format and write it out */
if (fixup->is_long)
{
/* write an INT4 offset value */
oswp4(buf, diff);
write_at(fixup->ofs, buf, 4);
}
else
{
/* write an INT2 offset value */
oswp2(buf, diff);
write_at(fixup->ofs, buf, 2);
}
/* add this fixup to the free list, since we're finished with it */
fixup->nxt = free_fixup_;
free_fixup_ = fixup;
}
}
void clear_screen()
{
u32 r, c, pos;
for(r=0; r<FB_NUM_ROWS; r++) {
for(c=0; c<FB_NUM_COLS; c++) {
pos = (r * FB_NUM_COLS + c);
write_at(pos, NULL_CHAR);
}
}
cursor_pos = 0;
}
bool IridiumSBD::clear_mo_buffer()
{
write_at("AT+SBDD0");
if (read_at_command() != SATCOM_RESULT_OK || _rx_command_buf[0] != '0') {
VERBOSE_INFO("CLEAR MO BUFFER: ERROR");
return false;
}
return true;
}
bool IridiumSBD::is_modem_ready(void)
{
write_at("AT");
if (read_at_command() == SATCOM_RESULT_OK) {
return true;
} else {
return false;
}
}
void IridiumSBD::read_rx_buf(void)
{
if (!is_modem_ready()) {
VERBOSE_INFO("READ SBD: MODEM NOT READY!");
return;
}
pthread_mutex_lock(&_rx_buf_mutex);
write_at("AT+SBDRB");
if (read_at_msg() != SATCOM_RESULT_OK) {
VERBOSE_INFO("READ SBD: MODEM NOT RESPONDING!");
_rx_msg_read_idx = _rx_msg_end_idx;
pthread_mutex_unlock(&_rx_buf_mutex);
return;
}
int data_len = (_rx_msg_buf[0] << 8) + _rx_msg_buf[1];
// rx_buf contains 2 byte length, data, 2 byte checksum and /r/n delimiter
if (data_len != _rx_msg_end_idx - 6) {
PX4_ERR("READ SBD: WRONG DATA LENGTH");
_rx_msg_read_idx = _rx_msg_end_idx;
pthread_mutex_unlock(&_rx_buf_mutex);
return;
}
int checksum = 0;
for (int i = 2; i < data_len + 2; i++) {
checksum += _rx_msg_buf[i];
}
if ((checksum / 256 != _rx_msg_buf[_rx_msg_end_idx - 4]) || ((checksum & 255) != _rx_msg_buf[_rx_msg_end_idx - 3])) {
PX4_ERR("READ SBD: WRONG DATA CHECKSUM");
_rx_msg_read_idx = _rx_msg_end_idx;
pthread_mutex_unlock(&_rx_buf_mutex);
return;
}
_rx_msg_read_idx = 2; // ignore the length
_rx_msg_end_idx -= 4; // ignore the checksum and delimiter
_rx_read_pending = false;
pthread_mutex_unlock(&_rx_buf_mutex);
VERBOSE_INFO("READ SBD: SUCCESS, LEN: %d", data_len);
}
static int append_to_pack(git_indexer *idx, const void *data, size_t size)
{
git_off_t current_size = idx->pack->mwf.size;
if (!size)
return 0;
/* add the extra space we need at the end */
if (p_ftruncate(idx->pack->mwf.fd, current_size + size) < 0) {
giterr_set(GITERR_OS, "Failed to increase size of pack file '%s'", idx->pack->pack_name);
return -1;
}
return write_at(idx, data, idx->pack->mwf.size, size);
}
void IridiumSBD::start_csq(void)
{
if ((_state != SATCOM_STATE_STANDBY) || (_new_state != SATCOM_STATE_STANDBY)) {
VERBOSE_INFO("CANNOT ENTER CSQ STATE");
return;
} else {
VERBOSE_INFO("UPDATING SIGNAL QUALITY");
}
_last_signal_check = hrt_absolute_time();
if (!is_modem_ready()) {
VERBOSE_INFO("UPDATE SIGNAL QUALITY: MODEM NOT READY!");
return;
}
write_at("AT+CSQ");
_new_state = SATCOM_STATE_CSQ;
}
void scroll()
{
u32 r, c, pos;
char buffer[FB_NUM_TOTAL];
for(r=1; r<FB_NUM_ROWS; r++) {
for(c=0; c<FB_NUM_COLS; c++) {
pos = (r * FB_NUM_COLS + c);
buffer[pos - FB_NUM_COLS] = read_cell(r, c);
}
}
for(c=FB_NUM_TOTAL-FB_NUM_COLS; c<FB_NUM_TOTAL; c++) {
buffer[c] = NULL_CHAR;
}
cursor_pos = 0;
for(pos=0; pos<FB_NUM_TOTAL; pos++) {
write_at(pos, buffer[pos]);
}
cursor_pos = FB_NUM_TOTAL - FB_NUM_COLS;
}
void DisplayBattery(int col, int row)
{
#ifdef VX670
char font[256] = {0};
char bat[2];
bat[0] = 'a' + (char)((get_battery_charge() * 6) / 100);
bat[1] = 0;
int old = get_display_coordinate_mode();
set_display_coordinate_mode(CHARACTER_MODE);
get_font(font);
set_font("F:BATTERY.VFT");
write_at (bat, 1, col, row);
set_font(font);
set_display_coordinate_mode(old);
#endif
}
void IridiumSBD::start_test(void)
{
if ((_state != SATCOM_STATE_STANDBY) || (_new_state != SATCOM_STATE_STANDBY)) {
PX4_INFO("CANNOT ENTER TEST STATE");
return;
}
int res = read_at_command();
if (res != SATCOM_RESULT_NA) {
PX4_WARN("SOMETHING WAS IN BUFFER");
printf("TEST RESULT: %d, LENGTH %d\nDATA:\n%s\nRAW DATA:\n", res, _rx_command_len, _rx_command_buf);
for (int i = 0; i < _rx_command_len; i++) {
printf("%d ", _rx_command_buf[i]);
}
printf("\n");
}
if (!is_modem_ready()) {
PX4_WARN("MODEM NOT READY!");
return;
}
if (strlen(_test_command) != 0) {
if ((strstr(_test_command, "AT") != nullptr) || (strstr(_test_command, "at") != nullptr)) {
PX4_INFO("TEST %s", _test_command);
write_at(_test_command);
_new_state = SATCOM_STATE_TEST;
} else {
PX4_WARN("The test command does not include AT or at: %s, ignoring it.", _test_command);
_new_state = SATCOM_STATE_STANDBY;
}
} else {
PX4_INFO("TEST DONE");
}
}
static int update_header_and_rehash(git_indexer *idx, git_transfer_progress *stats)
{
void *ptr;
size_t chunk = 1024*1024;
git_off_t hashed = 0;
git_mwindow *w = NULL;
git_mwindow_file *mwf;
unsigned int left;
mwf = &idx->pack->mwf;
git_hash_init(&idx->trailer);
/* Update the header to include the numer of local objects we injected */
idx->hdr.hdr_entries = htonl(stats->total_objects + stats->local_objects);
if (write_at(idx, &idx->hdr, 0, sizeof(struct git_pack_header)) < 0)
return -1;
/*
* We now use the same technique as before to determine the
* hash. We keep reading up to the end and let
* hash_partially() keep the existing trailer out of the
* calculation.
*/
git_mwindow_free_all(mwf);
idx->inbuf_len = 0;
while (hashed < mwf->size) {
ptr = git_mwindow_open(mwf, &w, hashed, chunk, &left);
if (ptr == NULL)
return -1;
hash_partially(idx, (unsigned char*) ptr, left);
hashed += left;
git_mwindow_close(&w);
}
return 0;
}
static int do_xattr_data_cows(char *ref_pfx, unsigned long iter, int ea_nums)
{
unsigned long i, j;
char dest[PATH_MAX];
int fd, ret = 0, o_ret;
unsigned long offset = 0, write_size = 0;
char *write_buf = NULL;
write_buf = (char *)malloc(HUNK_SIZE);
for (i = 0; i < iter; i++) {
snprintf(dest, PATH_MAX, "%sr%ld", ref_pfx, i);
fd = open64(dest, open_rw_flags);
if (fd < 0) {
o_ret = fd;
fd = errno;
fprintf(stderr, "open file %s failed:%d:%s\n",
dest, fd, strerror(fd));
fd = o_ret;
goto bail;
}
strcpy(filename, dest);
for (j = 0; j < ea_nums; j++) {
/* Update xattr*/
strcpy(xattr_name, xattr_name_list_set[j]);
xattr_value_sz = get_rand(1, XATTR_VALUE_MAX_SZ);
if (xattr_value_sz > xattr_name_sz + 50)
xattr_value_constructor(j);
else
xattr_value_generator(j, xattr_value_sz,
xattr_value_sz);
ret = add_or_update_ea(NORMAL, fd, XATTR_REPLACE,
"update");
if (ret < 0)
ret = add_or_update_ea(NORMAL, fd, XATTR_CREATE,
"add");
if (ret < 0)
continue;
if (xattr_value_sz > xattr_name_sz + 50) {
ret = read_ea(NORMAL, fd);
if (ret < 0)
goto bail;
ret = xattr_value_validator(j);
if (ret < 0)
goto bail;
}
/* Update file data*/
offset = get_rand(0, file_size - 1);
write_size = get_rand(1, HUNK_SIZE);
if (offset + write_size > file_size)
write_size = file_size - offset;
get_rand_buf(write_buf, write_size);
ret = write_at(fd, write_buf, write_size, offset);
if (ret < 0)
goto bail;
}
close(fd);
}
bail:
if (write_buf)
free(write_buf);
return ret;
}
inline std::size_t write_at(SyncRandomAccessWriteDevice& d,
lslboost::uint64_t offset, const ConstBufferSequence& buffers,
lslboost::system::error_code& ec)
{
return write_at(d, offset, buffers, transfer_all(), ec);
}
inline std::size_t write_at(SyncRandomAccessWriteDevice& d,
lslboost::uint64_t offset, lslboost::asio::basic_streambuf<Allocator>& b,
lslboost::system::error_code& ec)
{
return write_at(d, offset, b, transfer_all(), ec);
}
/******************************************************************************
* Funcao: writeDisp *
* Apresenta mensagem no display do POS *
* *
* inBuf : mensagem a ser apresentada *
* szBuf : tamanho mensagem *
* col : coluna display *
* lin : linha display *
* Retornos: >0 = numero de bytes exibidos no display *
******************************************************************************/
int
writeDisp ( char * inBuf, int szBuf, int col, int lin )
{
return write_at (inBuf, szBuf, col, lin);
}
void IridiumSBD::main_loop(int argc, char *argv[])
{
CDev::init();
pthread_mutex_init(&_tx_buf_mutex, NULL);
pthread_mutex_init(&_rx_buf_mutex, NULL);
int arg_i = 3;
int arg_uart_name = 0;
while (arg_i < argc) {
if (!strcmp(argv[arg_i], "-d")) {
arg_i++;
arg_uart_name = arg_i;
} else if (!strcmp(argv[arg_i], "-v")) {
PX4_WARN("verbose mode ON");
_verbose = true;
}
arg_i++;
}
if (arg_uart_name == 0) {
PX4_WARN("no Iridium SBD modem UART port provided!");
_task_should_exit = true;
return;
}
if (open_uart(argv[arg_uart_name]) != SATCOM_UART_OK) {
PX4_WARN("failed to open UART port!");
_task_should_exit = true;
return;
}
// disable flow control
write_at("AT&K0");
if (read_at_command() != SATCOM_RESULT_OK) {
PX4_WARN("modem not responding");
_task_should_exit = true;
return;
}
// disable command echo
write_at("ATE0");
if (read_at_command() != SATCOM_RESULT_OK) {
PX4_WARN("modem not responding");
_task_should_exit = true;
return;
}
param_t param_pointer;
param_pointer = param_find("ISBD_READ_INT");
param_get(param_pointer, &_param_read_interval_s);
param_pointer = param_find("ISBD_SBD_TIMEOUT");
param_get(param_pointer, &_param_session_timeout_s);
if (_param_session_timeout_s < 0) {
_param_session_timeout_s = 60;
}
param_pointer = param_find("ISBD_STACK_TIME");
param_get(param_pointer, &_param_stacking_time_ms);
if (_param_stacking_time_ms < 0) {
_param_stacking_time_ms = 0;
}
VERBOSE_INFO("read interval: %d s", _param_read_interval_s);
VERBOSE_INFO("SBD session timeout: %d s", _param_session_timeout_s);
VERBOSE_INFO("SBD stack time: %d ms", _param_stacking_time_ms);
while (!_task_should_exit) {
switch (_state) {
case SATCOM_STATE_STANDBY:
standby_loop();
break;
case SATCOM_STATE_CSQ:
csq_loop();
break;
case SATCOM_STATE_SBDSESSION:
sbdsession_loop();
break;
case SATCOM_STATE_TEST:
test_loop();
break;
}
if (_new_state != _state) {
VERBOSE_INFO("SWITCHING STATE FROM %s TO %s", satcom_state_string[_state], satcom_state_string[_new_state]);
_state = _new_state;
publish_iridium_status();
} else {
publish_iridium_status();
usleep(100000); // 100ms
}
}
}
int git_indexer_commit(git_indexer *idx, git_transfer_progress *stats)
{
git_mwindow *w = NULL;
unsigned int i, long_offsets = 0, left;
int error;
struct git_pack_idx_header hdr;
git_buf filename = GIT_BUF_INIT;
struct entry *entry;
git_oid trailer_hash, file_hash;
git_hash_ctx ctx;
git_filebuf index_file = {0};
void *packfile_trailer;
if (git_hash_ctx_init(&ctx) < 0)
return -1;
/* Test for this before resolve_deltas(), as it plays with idx->off */
if (idx->off < idx->pack->mwf.size - 20) {
giterr_set(GITERR_INDEXER, "Unexpected data at the end of the pack");
return -1;
}
packfile_trailer = git_mwindow_open(&idx->pack->mwf, &w, idx->pack->mwf.size - GIT_OID_RAWSZ, GIT_OID_RAWSZ, &left);
if (packfile_trailer == NULL) {
git_mwindow_close(&w);
goto on_error;
}
/* Compare the packfile trailer as it was sent to us and what we calculated */
git_oid_fromraw(&file_hash, (unsigned char*) packfile_trailer);
git_mwindow_close(&w);
git_hash_final(&trailer_hash, &idx->trailer);
if (git_oid_cmp(&file_hash, &trailer_hash)) {
giterr_set(GITERR_INDEXER, "packfile trailer mismatch");
return -1;
}
/* Freeze the number of deltas */
stats->total_deltas = stats->total_objects - stats->indexed_objects;
if ((error = resolve_deltas(idx, stats)) < 0)
return error;
if (stats->indexed_objects != stats->total_objects) {
giterr_set(GITERR_INDEXER, "early EOF");
return -1;
}
if (stats->local_objects > 0) {
if (update_header_and_rehash(idx, stats) < 0)
return -1;
git_hash_final(&trailer_hash, &idx->trailer);
write_at(idx, &trailer_hash, idx->pack->mwf.size - GIT_OID_RAWSZ, GIT_OID_RAWSZ);
}
git_vector_sort(&idx->objects);
git_buf_sets(&filename, idx->pack->pack_name);
git_buf_shorten(&filename, strlen("pack"));
git_buf_puts(&filename, "idx");
if (git_buf_oom(&filename))
return -1;
if (git_filebuf_open(&index_file, filename.ptr,
GIT_FILEBUF_HASH_CONTENTS, idx->mode) < 0)
goto on_error;
/* Write out the header */
hdr.idx_signature = htonl(PACK_IDX_SIGNATURE);
hdr.idx_version = htonl(2);
git_filebuf_write(&index_file, &hdr, sizeof(hdr));
/* Write out the fanout table */
for (i = 0; i < 256; ++i) {
uint32_t n = htonl(idx->fanout[i]);
git_filebuf_write(&index_file, &n, sizeof(n));
}
/* Write out the object names (SHA-1 hashes) */
git_vector_foreach(&idx->objects, i, entry, struct entry*) {
git_filebuf_write(&index_file, &entry->oid, sizeof(git_oid));
git_hash_update(&ctx, &entry->oid, GIT_OID_RAWSZ);
}
git_hash_final(&idx->hash, &ctx);
/* Write out the CRC32 values */
git_vector_foreach(&idx->objects, i, entry, struct entry*) {
git_filebuf_write(&index_file, &entry->crc, sizeof(uint32_t));
}
/* Write out the offsets */
git_vector_foreach(&idx->objects, i, entry, struct entry*) {
uint32_t n;
if (entry->offset == UINT32_MAX)
n = htonl(0x80000000 | long_offsets++);
else
n = htonl(entry->offset);
git_filebuf_write(&index_file, &n, sizeof(uint32_t));
}
/* Write out the long offsets */
git_vector_foreach(&idx->objects, i, entry, struct entry*) {
uint32_t split[2];
if (entry->offset != UINT32_MAX)
continue;
split[0] = htonl(entry->offset_long >> 32);
split[1] = htonl(entry->offset_long & 0xffffffff);
git_filebuf_write(&index_file, &split, sizeof(uint32_t) * 2);
}
/* Write out the packfile trailer to the index */
if (git_filebuf_write(&index_file, &trailer_hash, GIT_OID_RAWSZ) < 0)
goto on_error;
/* Write out the hash of the idx */
if (git_filebuf_hash(&trailer_hash, &index_file) < 0)
goto on_error;
git_filebuf_write(&index_file, &trailer_hash, sizeof(git_oid));
/* Figure out what the final name should be */
if (index_path(&filename, idx, ".idx") < 0)
goto on_error;
/* Commit file */
if (git_filebuf_commit_at(&index_file, filename.ptr) < 0)
goto on_error;
git_mwindow_free_all(&idx->pack->mwf);
/* We need to close the descriptor here so Windows doesn't choke on commit_at */
if (p_close(idx->pack->mwf.fd) < 0) {
giterr_set(GITERR_OS, "failed to close packfile");
goto on_error;
}
idx->pack->mwf.fd = -1;
if (index_path(&filename, idx, ".pack") < 0)
goto on_error;
/* And don't forget to rename the packfile to its new place. */
p_rename(idx->pack->pack_name, git_buf_cstr(&filename));
git_buf_free(&filename);
git_hash_ctx_cleanup(&ctx);
return 0;
on_error:
git_mwindow_free_all(&idx->pack->mwf);
git_filebuf_cleanup(&index_file);
git_buf_free(&filename);
git_hash_ctx_cleanup(&ctx);
return -1;
}
