jaseg/matelight
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions
matelight/host/main.c
jaseg 809f8ac2e0 Working on the new UDP protocol
2014-02-01 22:07:03 +01:00

558 lines
15 KiB
C
Raw Blame History

#include "config.h"
#include "main.h"
#include "color.h"
#include "font.h"
#include "net.h"
#include <stdint.h>
#include <stdlib.h>
#include <stdio.h>
#include <errno.h>
#include <string.h>
#include <wchar.h>
#include <locale.h>
#include <sys/timeb.h>
#include <sys/queue.h>
#include <netdb.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <unistd.h>
/* CAUTION: REQUIRES INPUT TO BE \0-TERMINATED
* ...also, it does a hardcodes setlocale of LC_CTYPE to en_US.utf8 for... reasons. */
framebuffer_t *framebuffer_render_text(char *s, glyph_t **glyph_table, unsigned int glyph_table_size){
unsigned int len = strlen(s);
color_t *gbuf = NULL;
unsigned int gbufwidth = 0;
unsigned int gbufheight = 0;
char *p = s;
/* Calculate screen width of string prior to allocating memory for the frame buffer */
wchar_t c;
mbstate_t ps = {0};
memset(&ps, 0, sizeof(mbstate_t));
if(!setlocale(LC_CTYPE, "en_US.utf8")){
fprintf(stderr, "Cannot set locale\n");
goto error;
}
for(;;){
while(*p == '\033'){
p++;
/* Jump over escape sequences */
for(;;p++){
if(!(*p == ';' || *p == '[' || ('0' <= *p && *p <= '9'))){
p++;
break;
}
}
memset(&ps, 0, sizeof(mbstate_t));
}
size_t inc = mbrtowc(&c, p, MB_CUR_MAX, &ps); /* MB_CUR_MAX is safe since p is \0-terminated */
if(inc == -1 || inc == -2){
fprintf(stderr, "Error rendering string: No valid UTF-8 input.\n");
goto error;
}
if(inc == 0) /* Reached end of string */
break;
p += inc;
if(c > glyph_table_size){
fprintf(stderr, "Error rendering string: Codepoint 0x%lx out of valid range (0-%d).\n", (long int)c, glyph_table_size);
goto error;
}
glyph_t *g = glyph_table[c];
if(!g){
fprintf(stderr, "Error rendering string: Codepoint 0x%lx not in font.\n", (long int)c);
goto error;
}
if(g->height > gbufheight)
gbufheight = g->height;
gbufwidth += g->width;
}
/* For easier rendering on the terminal, round up to multiples of two */
gbufheight += gbufheight&1;
size_t gbufsize = gbufwidth*gbufheight;
gbuf = calloc(gbufsize, sizeof(color_t));
if(!gbuf){
fprintf(stderr, "Cannot malloc() %lu bytes.\n", gbufsize*sizeof(color_t));
goto error;
}
memset(gbuf, 0, gbufsize*sizeof(color_t));
unsigned int x = 0;
p = s;
memset(&ps, 0, sizeof(mbstate_t));
struct {
color_t fg;
color_t bg;
unsigned int blink:4;
unsigned int bold:1; /* TODO */
unsigned int underline:1;
unsigned int strikethrough:1;
unsigned int fraktur:1; /* TODO See: Flat10 Fraktur font */
unsigned int invert:1;
} style = {
colortable[DEFAULT_FG_COLOR], colortable[DEFAULT_BG_COLOR], 0, 0, 0, 0, 0, 0
};
/* Render glyphs (now with escape sequence rendering!) */
for(;;){
/* NOTE: This nested escape sequence parsing does not contain any unicode-awareness whatsoever */
if(*p == '\033'){ /* Escape sequence YAY */
char *sequence_start = ++p;
if(*p == '['){ /* This was a CSI! */
/* Disassemble the list of numbers, only accept SGR sequences (those ending with 'm') */
long elems[MAX_CSI_ELEMENTS];
int nelems;
for(nelems = 0; nelems<MAX_CSI_ELEMENTS; nelems++){
p++;
char *endptr;
elems[nelems] = strtol(p, &endptr, 10);
if(p == endptr){
fprintf(stderr, "Invalid escape sequence: \"\\e%s\"\n", sequence_start);
goto error;
}
p = endptr;
if(*endptr == 'm')
break;
if(*endptr != ';'){
fprintf(stderr, "Invalid escape sequence: \"\\e%s\"\n", sequence_start);
goto error;
}
}
p++; /* gobble up trailing 'm' of "\033[23;42m" */
nelems++;
/* By now we know it's a SGR since we error'ed out on anything else */
if(nelems < 1){
fprintf(stderr, "Unsupported escape sequence: \"\\e%s\"\n", sequence_start);
goto error;
}
/* Parse the sequence numbers */
for(int i=0; i<nelems; i++){
switch(elems[i]){
case 0: /* reset style */
style.fg = colortable[DEFAULT_FG_COLOR];
style.bg = colortable[DEFAULT_BG_COLOR];
style.bold = 0;
style.underline = 0;
style.blink = 0;
style.strikethrough = 0;
style.fraktur = 0;
style.invert = 0;
break;
case 1: /* bold */
style.bold = 1;
break;
case 4: /* underline */
style.underline = 1;
break;
case 5: /* slow blink */
style.blink = 1;
break;
case 6: /* rapid blink */
style.blink = 8;
break;
case 7: /* color invert on */
style.invert = 1;
break;
case 9: /* strike-through */
style.strikethrough = 1;
break;
case 20:/* Fraktur */
style.fraktur = 1;
break;
case 22:/* Bold off */
style.bold = 0;
break;
case 24:/* Underline off */
style.underline = 0;
break;
case 25:/* Blink off */
style.blink = 0;
break;
case 27:/* color invert off */
style.invert = 0;
break;
case 29:/* strike-through off */
style.strikethrough = 0;
break;
case 30: /* Set foreground color, "dim" colors */
case 31:
case 32:
case 33:
case 34:
case 35:
case 36:
case 37:
style.fg = colortable[elems[i]-30];
break;
case 38: /* Set xterm-256 foreground color */
i++;
if(nelems-i < 2 || elems[i] != 5){
fprintf(stderr, "Invalid ANSI escape code: \"\\e%s\"\n", sequence_start);
goto error;
}
style.fg = colortable[elems[++i]];
break;
case 39: /* Reset foreground color to default */
style.bg = colortable[DEFAULT_FG_COLOR];
break;
case 40: /* Set background color, "dim" colors */
case 41:
case 42:
case 43:
case 44:
case 45:
case 46:
case 47:
style.bg = colortable[elems[i]-40];
break;
case 48: /* Set xterm-256 background color */
i++;
if(nelems-i < 2 || elems[i] != 5){
fprintf(stderr, "Invalid ANSI escape code: \"\\e%s\"\n", sequence_start);
goto error;
}
style.bg = colortable[elems[++i]];
break;
case 49: /* Reset background color to default */
style.bg = colortable[DEFAULT_BG_COLOR];
break;
case 90: /* Set foreground color, "bright" colors */
case 91:
case 92:
case 93:
case 94:
case 95:
case 96:
case 97:
style.fg = colortable[elems[i]-90+8];
break;
case 100: /* Set background color, "bright" colors */
case 101:
case 102:
case 103:
case 104:
case 105:
case 106:
case 107:
style.bg = colortable[elems[i]-100+8];
break;
default:
fprintf(stderr, "Unsupported escape sequence: \"\\e%s\"\n", sequence_start);
goto error;
}
}
}else{
fprintf(stderr, "Unsupported escape sequence: \"\\e%s\"\n", sequence_start);
goto error;
}
continue;
}
size_t inc = mbrtowc(&c, p, (s+len+1)-p, NULL);
/* If p contained */
if(inc == 0) /* Reached end of string */
break;
p += inc;
/* Render glyph into frame buffer */
struct timeb time = {0};
ftime(&time);
unsigned long int t = time.time*1000 + time.millitm;
int blink = style.blink && (t % (1000/style.blink) < (333/style.blink));
int inv = !(style.invert ^ blink);
color_t fg = inv ? style.fg : style.bg;
color_t bg = inv ? style.bg : style.fg;
glyph_t *g = glyph_table[c];
render_glyph(g, gbuf, gbufwidth, x, 0, fg, bg);
if(style.strikethrough || style.underline){
int sty = gbufheight/2;
/* g->y usually is a negative index of the glyph's baseline measured from the glyph's bottom */
int uly = gbufheight + g->y;
for(int i=0; i<g->width; i++){
if(style.strikethrough)
gbuf[sty*gbufwidth + x + i] = fg;
if(style.underline)
gbuf[uly*gbufwidth + x + i] = fg;
}
}
x += g->width;
}
framebuffer_t *fb = malloc(sizeof(framebuffer_t));
if(!fb){
fprintf(stderr, "Cannot malloc() %lu bytes.\n", sizeof(framebuffer_t));
goto error;
}
fb->w = gbufwidth;
fb->h = gbufheight;
fb->data = gbuf;
return fb;
error:
free(gbuf);
return 0;
}
void console_render_buffer(framebuffer_t *fb){
/* Render framebuffer to terminal, two pixels per character using Unicode box drawing stuff */
color_t lastfg = {0, 0, 0}, lastbg = {0, 0, 0};
printf("\e[38;5;0;48;5;0m");
color_t *data = fb->data;
size_t w = fb->w;
size_t h = fb->h;
for(size_t y=0; y < h; y+=2){
for(size_t x=0; x < w; x++){
/* Da magicks: ▀█▄ */
color_t ct = data[y*w + x]; /* Top pixel */
color_t cb = data[(y+1)*w + x]; /* Bottom pixel */
/* The following, rather convoluted logic tries to "save" escape sequences when rendering. */
if(!memcmp(&ct, &lastfg, sizeof(color_t))){
if(!memcmp(&cb, &lastbg, sizeof(color_t))){
printf("▀");
}else if(!memcmp(&cb, &lastfg, sizeof(color_t))){
printf("█");
}else{
printf("\033[48;5;%dm▀", xterm_color_index(cb));
lastbg = cb;
}
}else if(!memcmp(&ct, &lastbg, sizeof(color_t))){
if(!memcmp(&cb, &lastfg, sizeof(color_t))){
printf("▄");
}else if(!memcmp(&cb, &lastbg, sizeof(color_t))){
printf(" ");
}else{
printf("\033[38;5;%dm▄", xterm_color_index(cb));
lastfg = cb;
}
}else{ /* No matches for the upper pixel */
if(!memcmp(&cb, &lastfg, sizeof(color_t))){
printf("\033[48;5;%dm▄", xterm_color_index(ct));
lastbg = ct;
}else if(!memcmp(&cb, &lastbg, sizeof(color_t))){
printf("\033[38;5;%dm▀", xterm_color_index(ct));
lastfg = ct;
}else{
printf("\033[38;5;%d;48;5;%dm▀", xterm_color_index(ct), xterm_color_index(cb));
lastfg = ct;
lastbg = cb;
}
}
}
printf("\n");
}
}
int main(int argc, char **argv){
FILE *f = NULL;
int udpfd = 0;
int udp6fd = 0;
uint8_t *udpbuf = NULL;
uint8_t *fbdata = NULL;
framebuffer_t *fb = NULL;
if(argc != 2){
fprintf(stderr, "No or too much input text given\n");
return 1;
}
/* Read font file */
FILE *fontfile = fopen("unifont.bdf", "r");
if(!fontfile){
fprintf(stderr, "Error opening font file: %s\n", strerror(errno));
return 1;
}
glyph_t* glyph_table[BLP_SIZE];
if(read_bdf(fontfile, glyph_table, BLP_SIZE)){
fprintf(stderr, "Error reading font file.\n");
fclose(fontfile);
goto error;
}
fclose(fontfile);
/* Set up framebuffer */
fbdata = malloc(DISPLAY_WIDTH*DISPLAY_HEIGHT*sizeof(color_t));
if(!fbdata){
fprintf(stderr, "Cannot alloccate framebuffer\n");
goto error;
}
fb = malloc(sizeof(*fb));
if(!fb){
fprintf(stderr, "Cannot alloccate framebuffer\n");
goto error;
}
fb->w = DISPLAY_WIDTH;
fb->h = DISPLAY_HEIGHT;
fb->data = fbdata;
/* Set up UDP server */
udp4fd = socket(AF_INET, SOCK_DGRAM | SOCK_NONBLOCK, IPPROTO_UDP);
udp6fd = socket(AF_INET6, SOCK_DGRAM | SOCK_NONBLOCK, IPPROTO_UDP);
if(udp4fd < 0 || udp6fd < 0){
fprintf(stderr, "Cannot open UDP sockets: %s\n", strerror(errno));
goto error;
}
struct sockaddr_in udp_addr = {
AF_INET,
INADDR_ANY,
htons(UDP_PORT)
};
bind(udp4fd, &udp_addr, sizeof(udp_addr));
struct sockaddr_in udp6_addr = {
AF_INET6,
IN6ADDR_ANY_INIT,
htons(UDP_PORT)
};
bind(udp6fd, &udp_addr, sizeof(udp_addr));
/* Set up UDP receive buffer */
struct msghdr msg;
memset(&msg, 0, sizeof(msg));
udpbuf = malloc(UDP_BUF_SIZE);
if(!buf){
fprintf(stderr, "Cannot allocate UDP buffer");
goto error;
}
struct iovec iov[1] = {{udpbuf, sizeof(UDP_BUF_SIZE)}};
msg.msg_iov = iov;
msg.msg_iovlen = 1;
/* List of currently active UDP streams */
LIST_HEAD(udp_client_entry, udp_client) udp_client_head;
struct udp_client {
uint64_t last_timestamp;
uint64_t last_sequence_number;
struct sockaddr remote_addr;
int active:1;
LIST_ENTRY(udp_client) entries;
};
LIST_INIT(&udp_client_head);
LIST_HEAD(text_queue_head_t, text_queue_entry) text_queue_head;
struct text_queue_entry {
char *text;
int current_position;
unsigned int loop_count;
LIST_ENTRY(text_queue_entry) entries;
};
LIST_INIT(&text_queue_head);
for(;;){ /* Never gonna give you up, never gonna let you down! */
struct timeb time = {0};
ftime(&time);
uint64_t now = time.time*1000 + time.millitm;
int made_active = 0;
/* Choose next active client */
for(struct udp_client *p = udp_client_head.lh_first; p != NULL; p = p->entries.le_next){
if(now - p->last_frame_received > FRAME_TIMEOUT){ /* Connection timeout */
LIST_REMOVE(p, entries);
continue;
}
if(!made_active){
p->active = 1;
made_active = 1;
}else{
p->active = 0;
}
}
if(!made_active){ /* No active streams, render marquee */
text_queue_entry *entry = text_queue_head.lh_first;
framebuffer_t *text = framebuffer_render_text(entry, glyph_table, BLP_SIZE);
/* COPY TEXT DATA */
for(size_t line = 0; line < DISPLAY_HEIGHT; line++){
color_t *datarow = fb->data+(line*DISPLAY_WIDTH);
color_t *textrow = text->data+(line*text->w);
if(entry->current_position > 0)
memset(datarow, 0, sizeof(color_t)*entry->current_position);
if(entry->current_position+text->w < DISPLAY_WIDTH)
memset(datarow+(entry->curent_position+text->w), 0, DISPLAY_WIDTH-(entry->curent_position+text->w));
memcpy(datarow+(entry->current_position>0 ? entry->current_position : 0),
textrow+(entry->current_position>0 ? 0 : -entry->current_position),
//DISPLAY_WIDTH, text->w-current_position FIXME
}
entry->current_position++;
if(entry->current_position <= -text->w){
entry->loop_count++;
if(entry->loop_count > TEXT_LOOP_COUNT){
LIST_REMOVE(entry, entries);
}
}
}
/* Receive pending IPv4 UDP packets */
ssize_t received = 0;
while((received = recvmsg(udp4fd, &msg, 0)) > 0){
if(received < UDP_BUF_SIZE) /* Packet too short */
continue;
ml_packet_t *pkt = (ml_packet_t*)udpbuf;
if(ntohl(pkt->magic) != 0xDEADBEEF)
continue;
if(ntohs(pkt->width) != DISPLAY_WIDTH || ntohs(pkt->height) != DISPLAY_HEIGHT)
continue;
struct sockaddr_in *src = msg.msg_name;
struct udp_client *entry = NULL;
for(struct udp_client *p = udp_client_head.lh_first; p != NULL; p = p->entries.le_next){
if(!memcmp(&p->remote_addr, src, sizeof(sockaddr))){ /* found */
entry = p;
break;
}
}
if(!entry){
entry = malloc(sizeof(struct udp_client));
if(!entry){
fprintf(stderr, "Cannot allocate UDP connection entry\n");
goto error;
}
memset(entry, 0, sizeof(entry));
LIST_INSERT_HEAD(&udp_client_head, entry, entries);
memcpy(entry->remote_addr, src, sizeof(sockaddr));
}
if(entry->last_sequence_number > pkt->seq) /* Delayed packet containing old data */
continue;
entry->last_timestamp = now;
entry->last_sequence_number = pkt->seq;
if(entry->active){
/* Copy frame, extending RGB UDP packet data to RGBA framebuffer format */
color_t *p = fb->data;
rgb_t *q = pkt->data;
while(p < fb->data+fb->w*fb->h){
p->a = 0;
*(rgb_t *)p++ = *q++;
}
}
}
if(received < 0){
fprintf(stderr, "Error receiving UDP datagram: %s\n", strerror(errno));
goto error;
}
/* Receive pending IPv6 UDP packets */
/* FIXME */
/* Render frame buffer */
printf("\033[2J");
console_render_buffer(fb);
printf("\n");
usb_send_buffer(fb);
usleep(1000000/FRAMERATE);
}
return 0;
error:
fclose(f);
close(udpfd);
close(udp6fd);
free(udpbuf);
free(fbdata);
free(fb);
return 1;
}
Reference in a new issue View git blame Copy permalink