// Reads data from the physical layer decodes it to frames and adds it to the receive queue
void DataLinkLayer::readData() {
uint32_t available_bytes = _physicalLayer->available();
if (available_bytes) {
if (_current_rx_buffer == NULL) {
_current_rx_buffer = (uint8_t*)calloc(available_bytes, sizeof(uint8_t));
} else {
uint8_t * temp_buffer = (uint8_t*)realloc(_current_rx_buffer, (_current_rx_buffer_size + available_bytes) * sizeof(uint8_t));
_current_rx_buffer = temp_buffer;
}
_physicalLayer->read(_current_rx_buffer + _current_rx_buffer_size, available_bytes);
_current_rx_buffer_size += available_bytes;
}
if (_current_rx_buffer_size >= sizeof(Frame)) {
for(int i = 0; i < _current_rx_buffer_size; i++) {
if (_current_rx_buffer[i] == FRAME_START_BYTE) {
Frame * frame = frame_decode(_current_rx_buffer + i, _current_rx_buffer_size - i);
// Only accept valid frames and from other clients
// to us
if (Crc.validate(frame)) {
PrintUtl.prints("Read a valid frame.\r\n");
if (should_read_frame(frame)) {
PrintUtl.prints("It is for me\r\n");
Frame ** newRxFrameList = (Frame **)realloc(_receiveFrameList, (_rx_buffer_size + 1) * sizeof(Frame *));
if (newRxFrameList == NULL) {
//BOOM!! We were not able to allocate memory
return;
} else if (newRxFrameList != _receiveFrameList) {
_receiveFrameList = newRxFrameList;
}
_receiveFrameList[_rx_buffer_size] = frame;
_rx_buffer_size++;
process_rx_frame(frame);
PrintUtl.prints("Received ");
frame->print();
} else {
PrintUtl.prints("It is NOT for me\r\n");
}
free(_current_rx_buffer);
_current_rx_buffer_size = 0;
PrintUtl.prints("Freed\r\n");
}
else {
free(frame);
}
}
}
}
}
int test_frame_ctl(uint8_t key[SHAREDKEY_BYTESIZE]) {
#define TESTBUFSIZE 4096
int i,ret;
ssize_t crypted_len;
struct frame_st *crypted_frame = NULL;
internal_msg_t *ori_frame_body;
internal_msg_t decrypt_frame_body;
ori_frame_body = malloc(sizeof(*ori_frame_body));
ori_frame_body->msg_type = MSG_CTL_TYPE;
ori_frame_body->ctl_frame_body.code = CTL_PIPELINE_OPEN;
ori_frame_body->ctl_frame_body.arg.pipeline_open.flow_id = 1234;
ori_frame_body->ctl_frame_body.arg.pipeline_open.max_delay_in_ms = 500;
ori_frame_body->ctl_frame_body.arg.pipeline_open.reply_frame_flags = 1;
ori_frame_body->ctl_frame_body.arg.pipeline_open.data = malloc(TESTBUFSIZE);
ori_frame_body->ctl_frame_body.arg.pipeline_open.data_len = TESTBUFSIZE;
for (i=0;i<TESTBUFSIZE;++i) {
ori_frame_body->ctl_frame_body.arg.pipeline_open.data[i] = i%63;
}
printf("Original: %d bytes.\n", TESTBUFSIZE);
crypted_len = frame_encode(&crypted_frame, ori_frame_body, FRAME_FLAG_MAKE(1, 1, 1), key);
printf("frame_encode() = %d bytes.\n", (int)crypted_len);
ret = frame_decode(&decrypt_frame_body, crypted_frame, key);
printf("frame_decode() = %d bytes.\n", decrypt_frame_body.ctl_frame_body.arg.pipeline_open.data_len);
if (memcmp(ori_frame_body->ctl_frame_body.arg.pipeline_open.data, decrypt_frame_body.ctl_frame_body.arg.pipeline_open.data, TESTBUFSIZE)==0) {
puts("encrypt_frame_body/decrypt_frame_body OK.");
} else {
puts("encrypt_frame_body/decrypt_frame_body FAILed.");
}
printf("Original: ");
bin_dump(stdout, ori_frame_body->ctl_frame_body.arg.pipeline_open.data, TESTBUFSIZE);
printf("Crypted: ");
bin_dump(stdout, crypted_frame->frame_body, crypted_len);
printf("Derypted: ");
bin_dump(stdout, decrypt_frame_body.ctl_frame_body.arg.pipeline_open.data, decrypt_frame_body.ctl_frame_body.arg.pipeline_open.data_len);
free(ori_frame_body->ctl_frame_body.arg.pipeline_open.data);
free(ori_frame_body);
free(crypted_frame);
return 0;
}
int8_t frame_get_stream(struct frames_buffer *f_buf, void *dest_buf, uint16_t len, uint8_t *data_len)
{
struct frame *frame_rec = &f_buf->frame_record[f_buf->tail];
uint8_t fr_type;
int8_t ret, f_ret = FRAME_OK;
if (f_buf->cnt == 0 && f_buf->tail == f_buf->head) {
return FRAME_BUF_EMPTY; // Buffer is EMPTY
}
if (len < FRAME_LEN(frame_rec))
return -FRAME_ERR_NOMEM;
ret = frame_decode(frame_rec, dest_buf,
data_len, &fr_type);
if (ret) {
f_ret = ret;
} else {
if (fr_type == FRAME_START) {
if (f_buf->start_found == 1) {
f_buf->start_found = 0;
return -FRAME_BRK_STREAM;
} else {
f_buf->start_found = 1;
}
} else if (fr_type == FRAME_END) {
f_buf->start_found = 0;
f_ret = FRAME_OK_END;
} else if (fr_type == FRAME_SIMPLE) {
if (f_buf->start_found == 1) {
f_buf->start_found = 0;
return -FRAME_BRK_STREAM;
}
f_ret = FRAME_OK_SIMPLE;
}
}
FRAME_BUF_PTR_INC(f_buf->tail);
f_buf->cnt--;
return f_ret;
}
int test_frame_data(uint8_t key[SHAREDKEY_BYTESIZE]) {
#define TESTBUFSIZE 4096
int i,ret;
ssize_t crypted_len;
struct frame_st *crypted_frame = NULL;
internal_msg_t *ori_frame_body;
internal_msg_t decrypt_frame_body;
ori_frame_body = malloc(sizeof(*ori_frame_body));
ori_frame_body->msg_type = 0;
ori_frame_body->data_frame_body.flow_id = 12345;
ori_frame_body->data_frame_body.data = malloc(TESTBUFSIZE);
ori_frame_body->data_frame_body.data_len = TESTBUFSIZE;
for (i=0;i<TESTBUFSIZE;++i) {
ori_frame_body->data_frame_body.data[i] = i%63;
}
printf("Original: %d bytes.\n", TESTBUFSIZE);
crypted_len = frame_encode(&crypted_frame, ori_frame_body, FRAME_FLAG_MAKE(1, 1, 1), key);
printf("frame_encode() = %d bytes.\n", (int)crypted_len);
ret = frame_decode(&decrypt_frame_body, crypted_frame, key);
printf("frame_decode() = %d bytes.\n", decrypt_frame_body.data_frame_body.data_len);
if (memcmp(ori_frame_body->data_frame_body.data, decrypt_frame_body.data_frame_body.data, TESTBUFSIZE)==0) {
puts("encrypt_frame_body/decrypt_frame_body OK.");
} else {
puts("encrypt_frame_body/decrypt_frame_body FAILed.");
}
printf("Original: ");
bin_dump(stdout, ori_frame_body->data_frame_body.data, TESTBUFSIZE);
printf("Crypted: ");
bin_dump(stdout, crypted_frame->frame_body, crypted_len);
printf("Derypted: ");
bin_dump(stdout, decrypt_frame_body.data_frame_body.data, decrypt_frame_body.data_frame_body.data_len);
free(ori_frame_body->data_frame_body.data);
free(ori_frame_body);
free(crypted_frame);
return 0;
}
/**
* Draw one plane of a DMD frame.
* ID identifies the source of the frame data.
* The output is always drawn to the low-mapped buffer.
*/
void frame_draw_plane (U16 id)
{
/* Lookup the image number in the global table.
* For real ROMs, this is located at a fixed address.
* In native mode, the images are kept in a separate file.
*/
U8 type;
struct frame_pointer *p;
U8 *data;
page_push (IMAGEMAP_PAGE);
p = (struct frame_pointer *)IMAGEMAP_BASE + id;
data = PTR(p);
/* Switch to the page containing the image data.
* Pull the type byte out, then decode the remaining bytes
* to the display buffer. */
pinio_set_bank (PINIO_BANK_ROM, p->page);
type = data[0];
frame_decode (data + 1, type & ~0x1);
page_pop ();
}
void modem_loop(modem_rx_cb_t cb)
{
#define BIT_1 0
#define BIT_2 1
#define BIT_3 2
#define BIT_4 3
#define BIT_5 4
#define WAIT_FOR_DATA 5
int state = 0, recvd = 0;
uint64_t mark;
static uint8_t indata[1024], decdata[512], pktdata[1024], outdata[32768];
buffer_t inbuf, decbuf, pktbuf, outbuf;
buffer_init(&inbuf, indata, sizeof(indata));
buffer_init(&decbuf, decdata, sizeof(decdata));
buffer_init(&pktbuf, pktdata, sizeof(pktdata));
buffer_init(&outbuf, outdata, sizeof(outdata));
while (1)
{
uint8_t u_sample;
size_t bytes = 1;
if (ready_to_read() == 0) {
// transmit(STDERR, "flush!\n", 7, NULL);
flush_output();
}
if (get_byte(&u_sample) != 0)
break;
// if (receive(STDIN, &u_sample, 1, &bytes) != 0 || bytes != 1)
// break;
recvd++;
buffer_write_bytes(&inbuf, &u_sample, 1);
// process samples every 0.01 ms
if (buffer_read_remaining(&inbuf) / 8 >= SAMPLES_PER_ITER)
{
modem_decode(&inbuf, &decbuf);
while (state != WAIT_FOR_DATA && buffer_read_remaining(&decbuf) >= 8)
{
uint8_t bit;
bit = buffer_read_bit(&decbuf);
switch (state)
{
case BIT_1:
state = BIT_2;
mark = buffer_read_tell(&decbuf);
if (bit != 1)
goto reset;
break;
case BIT_2:
state = BIT_3;
if (bit != 0)
goto reset;
break;
case BIT_3:
state = BIT_4;
if (bit != 0)
goto reset;
break;
case BIT_4:
state = BIT_5;
if (bit != 1)
goto reset;
break;
case BIT_5:
state = WAIT_FOR_DATA;
if (bit != 1)
goto reset;
break;
}
continue;
reset:
state = BIT_1;
buffer_read_seek(&decbuf, mark);
}
if (state == WAIT_FOR_DATA && buffer_read_remaining(&decbuf) > FRAME_SIZE)
{
int result = frame_decode(&decbuf, &pktbuf);
if (result == FRAME_FAIL)
{
goto reset;
}
else if (result == FRAME_END)
{
// XXX send packet to callback
cb(pktdata, buffer_read_remaining(&pktbuf) / 8);
buffer_init(&pktbuf, pktdata, sizeof(pktdata));
}
state = BIT_1;
}
}
if (buffer_read_remaining(&outbuf) == 0)
modem_encode_frame(&outbuf);
if (buffer_read_remaining(&outbuf) > 0)
buffer_read_bytes(&outbuf, &u_sample, 1);
else
u_sample = _modem_encode(1);
write_byte(u_sample);
// transmit(STDOUT, &u_sample, 1, &bytes);
}
}
static int socket_end_protocol(struct socket_end_st *se)
{
uint8_t *chunk;
char *frame_buf;
struct pipeline_end_st *pe;
int plid, code, ret;
internal_msg_t body; /* Broken down frame body */
struct epoll_event ev;
size_t size = MSG_FRAME_MAX;
chunk = malloc(size);
if (chunk == NULL) {
mylog(L_ERR, "Allocate decode chunk buf failed, se[%d]", se->id);
abort();
}
frame_buf = malloc(ntohs(se->buf_len)+2);
if (unlikely(frame_buf == NULL)) {
mylog(L_ERR, "Allocate frame buf failed, se[%d]", se->id);
abort();
}
/* Copy body_len to buf */
memcpy(frame_buf, &se->buf_len, 2);
memcpy(frame_buf+2, se->buf_tail, ntohs(se->buf_len));
ret = frame_decode(&body, (struct frame_st *)frame_buf, se->shared_key, chunk, size);
if (ret < 0) {
mylog(L_ERR, "Frame decode failed: unknown protocol");
free(frame_buf);
frame_buf = NULL;
free(chunk);
chunk = NULL;
return -1;
}
free(frame_buf);
frame_buf = NULL;
if (body.msg_type == 0) { /* Data frame */
plid = body.data_frame_body.flow_id;
pe = se->pipeline_end[plid];
if (unlikely((pe == NULL) || pe->closed)) {
mylog(L_ERR, "Unvalid flowid %d, drop it, client %s, se[%d], port %d", plid, se->client_str, se->id, se->client_port);
free(body.data_frame_body.data);
} else {
int size = body.data_frame_body.data_len;
if (size > 0) {
struct streambuf_iov_st *iov = NULL;
iov = streambuf_iov_construct(body.data_frame_body.data, size);
if (iov == NULL) {
mylog(L_ERR, "Alloc iov failed, se[%d]", se->id);
free(body.data_frame_body.data);
return 0;
}
if (unlikely(pe->header_past==0)) {
if (http_header_xff_process(iov, se->client_str)==0) {
mylog(L_DEBUG, "HTTP header XFF process OK.");
} else {
mylog(L_ERR, "HTTP header XFF process FAILED.");
}
pe->header_past = 1;
}
mylog(L_DEBUG, "Write %d to upstream send buffer", size);
if (pe->iov_send_pending) {
mylog(L_INFO, "Merge to upstream_send_buf, se[%d] pe[%d]", se->id, pe->id);
pe->iov_send_pending = streambuf_iov_merge(pe->iov_send_pending, iov);
if (pe->iov_send_pending == NULL) {
/* Should not be here */
mylog(L_ERR, "Send pending iov merge failed");
abort();
}
streambuf_iov_free(iov);
} else {
ret = streambuf_write_nb(pe->upstream_send_buf, iov);
if (ret == ENOMEM) {
mylog(L_INFO, "Write to upstream_send_buf failed, set send pending se[%d] pe[%d]", se->id, pe->id);
pe->iov_send_pending = iov;
se->send_pending_count++;
}
}
} else {
free(chunk);
}
}
} else if (body.msg_type) { /* Control frame */
code = body.ctl_frame_body.code;
if (code == CTL_SOCKET_CERT_REQ) {
/* Send CTL_SOCKET_CERT */
socket_end_cert_request(se);
free(chunk);
} else if (code == CTL_SOCKET_KEY_SYNC) {
/* Send CTL_SOCKET_KEY_OK or CTL_SOCKET_KEY_REJ */
socket_end_key_sync(se, &body.ctl_frame_body.arg.socket_key_sync);
free(chunk);
} else if (code == CTL_PIPELINE_OPEN) {
mylog(L_DEBUG, "Got cmd pipeline_open");
plid = body.ctl_frame_body.arg.pipeline_open.flow_id;
if (se->pipeline_end[plid]) {
mylog(L_ERR, "Duplicated pipeline open id %d, Ignored, client %s, se[%d]", plid, se->client_str, se->id);
free(chunk);
return 0;
}
if (unlikely(socket_end_create_pipeline(se, &body.ctl_frame_body.arg.pipeline_open))) {
mylog(L_ERR, "Create pipeline failed, client %s, se[%d]", se->client_str, se->id);
free(chunk);
return 0;
}
pe = se->pipeline_end[plid];
se->pipeline_nr++;
streambuf_setvolume(se->send_buffer, se->pipeline_nr * BUFVOL_MIN);
ev.events = EPOLLIN|EPOLLRDHUP;
ev.data.ptr = se;
epoll_ctl(epollfd, EPOLL_CTL_ADD, pe->upstream_sd, &ev);
int size = body.ctl_frame_body.arg.pipeline_open.data_len;
if (size > 0) {
struct streambuf_iov_st *iov = NULL;
iov = streambuf_iov_construct(body.ctl_frame_body.arg.pipeline_open.data, size);
if (unlikely(iov == NULL)) {
mylog(L_ERR, "Fatal error: alloc iov failed");
free(body.ctl_frame_body.arg.pipeline_open.data);
socket_end_delete_pipeline(se, plid);
return 0;
}
if (likely(pe->header_past==0)) {
if (http_header_xff_process(iov, se->client_str)==0) {
mylog(L_DEBUG, "HTTP header XFF process OK.");
} else {
mylog(L_ERR, "HTTP header XFF process FAILED.");
}
pe->header_past = 1;
}
ret = streambuf_write_nb(pe->upstream_send_buf, iov);
if (ret == ENOMEM) {
mylog(L_INFO, "Write to upstream_send_buf failed, set send pending");
pe->iov_send_pending = iov;
se->send_pending_count++;
}
} else {
free(chunk);
}
} else if (code == CTL_PIPELINE_CLOSE) {
/* Close this pipeline */
free(chunk);
mylog(L_DEBUG, "Got cmd pipeline_close, se[%d]", se->id);
plid = body.ctl_frame_body.arg.pipeline_close.flow_id;
pe = se->pipeline_end[plid];
if (unlikely(pe == NULL)) {
/* Pipeline not exist */
mylog(L_ERR, "Pipeline %d not exist, can not close, se[%d]", plid, se->id);
return 0;
}
socket_end_delete_pipeline(se, plid);
} else {
free(chunk);
mylog(L_INFO, "Invalid control code, client %s, se[%d]", se->client_str, se->id);
}
}
return 0;
}
