size_t FifoBuffer<T, Container>::read(T* out, size_t num_elems)
{
size_t read_amount = num_elems > read_avail() ? read_avail() : num_elems;
std::copy(m_container.begin(), m_container.begin() + read_amount, out);
m_container.erase(m_container.begin(), m_container.begin() + read_amount);
return read_amount;
}
size_t read(void *dst, size_t len, bool peek = false)
{
size_t const avail = read_avail();
if (avail == 0) return 0;
size_t new_rpos = rpos;
size_t const limit_len = len > avail ? avail : len;
size_t const total = new_rpos + len;
size_t first, rest;
if (total > CAPACITY) {
first = CAPACITY - new_rpos;
rest = total % CAPACITY;
} else {
first = limit_len;
rest = 0;
}
Genode::memcpy(dst, &_data[new_rpos], first);
new_rpos = (new_rpos + first) % CAPACITY;
if (rest) {
Genode::memcpy(((char*)dst) + first, &_data[new_rpos], rest);
new_rpos = (new_rpos + rest) % CAPACITY;
}
if (!peek) rpos = new_rpos;
return limit_len;
}
blargg_err_t Data_Reader::read_avail( void* p, long* n )
{
int i = STATIC_CAST(int, *n);
blargg_err_t err = read_avail( p, &i );
*n = i;
return err;
}
const char * Data_Reader::read_avail( void* p, long* n )
{
int i = STATIC_CAST(int, *n);
const char * err = read_avail( p, &i );
*n = i;
return err;
}
blargg_err_t Data_Reader::read( void* p, long s )
{
RETURN_VALIDITY_CHECK( s > 0 );
long result = read_avail( p, s );
if ( result != s )
{
if ( result >= 0 && result < s )
return eof_error;
return "Read error";
}
return 0;
}
int recvsome(uint32_t* readoff, uint32_t* writeoff, uint32_t bufsize, char* readbuf, void* outbuf, int length) {
int available = (int)read_avail(readoff, writeoff, bufsize);
if(!available)
return 0;
int tocopy = available < length ? available : length;
if(read_would_wrap(readoff, bufsize, tocopy)) {
int bytestoend = (bufsize - *readoff);
memcpy(outbuf, readbuf + (*readoff), bytestoend);
memcpy(((char*)outbuf) + bytestoend, readbuf, tocopy - bytestoend);
}
else {
memcpy(outbuf, readbuf + (*readoff), tocopy);
}
return tocopy;
}
void discard_asynchronous_messages(int block) {
uint32_t bufsize = ((uint32_t*)singleton_screen->rx_buffer)[0];
uint32_t* readoff = &(((uint32_t*)singleton_screen->rx_buffer)[1]);
uint32_t* writeoff = &(((uint32_t*)singleton_screen->rx_buffer)[2]);
char* readbuf = (char*)&(((uint32_t*)singleton_screen->rx_buffer)[3]);
if(block) {
DBG("Discarding async messages (blocking)\n");
}
else {
DBG("Discarding async messages (non-blocking)\n");
}
// WARNING HERE: This method assumes that no async failure notification
// is bigger than the ring. If one were, we would wait fruitlessly until
// the entire thing were present before deleting it from the ring.
/* If block is set, we should wait until a synchronous message arrives.
If block is not set, we should return as soon as we're short of data. */
int need_more_bytes = 0;
while(1) {
struct message_header head;
if(need_more_bytes) {
if(block) {
DBG("Insufficient data, but invoked blocking: sleeping\n");
wait_for_rx_not_empty();
need_more_bytes = 0;
DBG("Woken\n");
}
else {
DBG("Non-blocking discard_async_messages: exiting\n");
return;
}
}
if(drain_events(singleton_screen->socketfd) == -1) {
printf("Socket error draining events in discard_async\n");
return;
}
int bytes_peeked = recvsome(readoff, writeoff, bufsize, readbuf, (char*)&head, sizeof(struct message_header));
if(bytes_peeked < sizeof(struct message_header)) {
DBG(" Less than a header available (%d bytes); exiting\n", bytes_peeked);
need_more_bytes = 1;
continue;
}
if(!message_is_asynchronous(head.opcode)) {
DBG(" Next message (%s) not asynchronous: exiting\n", optotext(head.opcode));
return;
}
int bytes_available = read_avail(readoff, writeoff, bufsize);
if(bytes_available < head.length) {
DBG(" Message had length %d but only %d available: exiting\n", head.length, bytes_available);
need_more_bytes = 1;
continue;
}
char* message_proper = malloc(head.length);
if(!message_proper) {
printf("OOM checking for asynchronous failures\n");
return;
}
int bytes_read = recvsome(readoff, writeoff, bufsize, readbuf, message_proper, head.length);
if(bytes_read < head.length) {
printf("BUG! read_avail returned %d, yet we could only actually read %d bytes\n", bytes_available, bytes_read);
free(message_proper);
return;
}
DBG(" Got a message: checking its result\n");
check_async_result(message_proper, head.opcode);
free(message_proper);
discard_ring_bytes(readoff, bufsize, head.length);
}
}
struct message_header* get_message() {
uint32_t bufsize = ((uint32_t*)singleton_screen->rx_buffer)[0];
uint32_t* readoff = &(((uint32_t*)singleton_screen->rx_buffer)[1]);
uint32_t* writeoff = &(((uint32_t*)singleton_screen->rx_buffer)[2]);
char* readbuf = (char*)&(((uint32_t*)singleton_screen->rx_buffer)[3]);
int size;
DBG("Entering get_message: readoff = %u, writeoff = %u\n", *readoff, *writeoff);
int avail;
wait_for_synchronous_message();
if(!read_would_wrap(readoff, bufsize, sizeof(struct message_header))) {
DBG("Looking for a message header's worth of data\n");
while((avail = read_avail(readoff, writeoff, bufsize)) < sizeof(struct message_header)) {
DBG("Waiting for more to be written (currently have %d, wanting %d\n)\n", avail, sizeof(struct message_header));
wait_for_rx_not_empty();
}
size = ((struct message_header*)(readbuf + *readoff))->length;
DBG("Found message has length %d\n", size);
if(read_would_wrap(readoff, bufsize, size)) {
DBG("Read would wrap; not reading in place\n");
last_read_is_in_place = 0;
}
else {
DBG("Read would not wrap; reading in place\n");
last_read_is_in_place = 1;
}
}
else {
DBG("Reading only a message_header would wrap; not reading in place\n");
last_read_is_in_place = 0;
}
if(last_read_is_in_place) {
DBG("Reading in place\n");
// Pass out a pointer to the ring buffer in-place, once it is full enough
while((avail = read_avail(readoff, writeoff, bufsize)) < size) {
DBG("Waiting for more to be written (got %d, wanting %d)\n", avail, size);
wait_for_rx_not_empty();
}
DBG("Got enough; passing out in-place pointer\n");
return (struct message_header*)(readbuf + *readoff);
}
else {
DBG("Not reading in place\n");
// Malloc a temporary and progressively copy into it
struct message_header* new_message = (struct message_header*)malloc(sizeof(struct message_header));
int bytes_received = 0;
partial_recv(readoff, writeoff, bufsize, readbuf, new_message, &bytes_received, sizeof(struct message_header));
if(new_message->length < sizeof(struct message_header)) {
printf("Unreasonably short message encountered whilst receiving (length %d)\n", new_message->length);
exit(1);
}
new_message = (struct message_header*)realloc(new_message, new_message->length);
partial_recv(readoff, writeoff, bufsize, readbuf, new_message, &bytes_received, new_message->length);
DBG("Received %s (length %u)\n", optotext(new_message->opcode), new_message->length);
return new_message;
}
}
