diff --git a/SMSD-1.5/client.c b/SMSD-1.5/client.c index 11d5c16..3e60f30 100644 --- a/SMSD-1.5/client.c +++ b/SMSD-1.5/client.c @@ -149,7 +149,7 @@ int read_console(char *buf, size_t len){ * wait until at least one character pressed * @return character readed */ -int mygetchar(){ // аналог getchar() без необходимости жать Enter +int mygetchar(){ // getchar() Enter int ret; do ret = read_console(NULL, 1); while(ret == 0); @@ -162,7 +162,7 @@ int mygetchar(){ // аналог getchar() без необходимости ж * @param length - buffer len * @return amount of readed bytes */ -size_t read_tty(uint8_t *buff, size_t length){ +size_t read_tty(char *buff, size_t length){ ssize_t L = 0; fd_set rfds; struct timeval tv; @@ -175,7 +175,7 @@ size_t read_tty(uint8_t *buff, size_t length){ if(FD_ISSET(comfd, &rfds)){ if((L = read(comfd, buff, length)) < 1){ fprintf(stderr, "ERROR on bus, exit!\n"); - exit(-4); + quit(-4); } } return (size_t)L; @@ -208,11 +208,13 @@ void help(){ printf("\n\nUse this commands:\n" "0\tMove to end-switch 0\n" "1\tMove to end-switch 1\n" - "Lxxx\tMake xxx steps toward zero's end-switch (0 main infinity)\n" - "Rxxx\tMake xxx steps toward end-switch 1 (0 main infinity)\n" + "-xxx\tMake xxx steps toward zero's end-switch (0 main infinity)\n" + "+xxx\tMake xxx steps toward end-switch 1 (0 main infinity)\n" "S\tStop/start motor when program is running\n" "A\tRun previous command again or stop when running\n" "E\tErase previous program from controller's memory\n" + "R\tTurn relay ON\n" + "r\tTurn relay OFF\n" "\n" ); } @@ -221,11 +223,11 @@ void write_tty(char *str, int L){ ssize_t D = write(comfd, str, L); if(D != L){ fprintf(stderr, "ERROR on bus, exit!\n"); - exit(-3); + quit(-3); } } -#define dup_pr(...) do{printf(__VA_ARGS__); if(fout) fprintf(fout, __VA_ARGS__);}while(0) +//#define dup_pr(...) do{printf(__VA_ARGS__); if(fout) fprintf(fout, __VA_ARGS__);}while(0) size_t read_ctrl_command(char *buf, size_t L){ // read data from controller to buffer buf int i, j; @@ -235,7 +237,7 @@ size_t read_ctrl_command(char *buf, size_t L){ // read data from controller to b for(j = 0; j < L; j++, ptr++){ R = 0; for(i = 0; i < 10 && !R; i++){ - R = read_tty((uint8_t*)ptr, 1); + R = read_tty(ptr, 1); } if(!R){j--; break;} // nothing to read if(*ptr == '*') // read only one command @@ -247,52 +249,71 @@ size_t read_ctrl_command(char *buf, size_t L){ // read data from controller to b return (size_t) j + 1; } +int parse_ctrlr_ans(char *ans){ + char *E = NULL, *Star = NULL; + if(!ans || !*ans) return 1; + bus_error = NO_ERROR; + if(!(E = strchr(ans, 'E')) || !(Star = strchr(ans, '*')) || E[1] != '1'){ + fprintf(stderr, "Answer format error (got: %s)\n", ans); + bus_error = UNDEFINED_ERR; + return 0; + } + switch (E[2]){ // E = "E1x" + case '0': // 10 - normal execution + break; + case '4': // 14 - program end + printf("Last command exectuted normally\n"); + break; + case '2': // command interrupt by other signal + fprintf(stderr, "Last command terminated\n"); + break; + case '3': + bus_error = CODE_ERR; + fprintf(stderr, "runtime"); + break; + case '5': + bus_error = BUS_ERR; + fprintf(stderr, "data bus"); + break; + case '6': + bus_error = COMMAND_ERR; + fprintf(stderr, "command"); + break; + case '9': + bus_error = CMD_DATA_ERR; + fprintf(stderr, "command data"); + break; + default: + bus_error = UNDEFINED_ERR; + fprintf(stderr, "undefined (%s)", ans); + } + if(bus_error != NO_ERROR){ + fprintf(stderr, " error in controller\n"); + return 0; + } + return 1; +} + int send_command(char *cmd){ int L = strlen(cmd); size_t R = 0; char ans[256]; write_tty(cmd, L); R = read_ctrl_command(ans, 255); - DBG("readed: %s (cmd: %s, R = %zd, L = %d)\n", ans, cmd, R, L); +// DBG("readed: %s (cmd: %s, R = %zd, L = %d)\n", ans, cmd, R, L); if(!R || (strncmp(ans, cmd, L) != 0)){ fprintf(stderr, "Error: controller doesn't respond (answer: %s)\n", ans); return 0; } R = read_ctrl_command(ans, 255); - DBG("readed: %s\n", ans); +// DBG("readed: %s\n", ans); if(!R){ // controller is running or error fprintf(stderr, "Controller doesn't answer!\n"); return 0; } - bus_error = NO_ERROR; - //if(strncasecmp(ptr, "HM") - //else - if( strncmp(ans, "E10*", 4) != 0 && - strncmp(ans, "E14*", 4) != 0 && - strncmp(ans, "E12*", 4) != 0){ - fprintf(stderr, "Error in controller: "); - if(strncmp(ans, "E13*", 4) == 0){ - bus_error = CODE_ERR; - fprintf(stderr, "runtime"); - }else if(strncmp(ans, "E15*", 4) == 0){ - bus_error = BUS_ERR; - fprintf(stderr, "databus"); - }else if(strncmp(ans, "E16*", 4) == 0){ - bus_error = COMMAND_ERR; - fprintf(stderr, "command"); - }else if(strncmp(ans, "E19*", 4) == 0){ - bus_error = CMD_DATA_ERR; - fprintf(stderr, "command data"); - }else{ - bus_error = UNDEFINED_ERR; - fprintf(stderr, "undefined (%s)", ans); - } - fprintf(stderr, " error\n"); - return 0; - } - DBG("ALL OK\n"); - return 1; + return parse_ctrlr_ans(ans); } + /* int send5times(char *cmd){ // sends command 'cmd' up to 5 times (if errors), return 0 in case of false int N, R = 0; @@ -310,8 +331,9 @@ int erase_ctrlr(){ if(!send_command("LD1*")){ // start writing a program if(bus_error == COMMAND_ERR){ // motor is moving printf("Found running program, stop it\n"); - if(send_command("ST1*")) - send_command("LD1*"); + if(!send_command("ST1*")) + send_command("SP*"); + send_command("LD1*"); }else{ fprintf(stderr, "Controller doesn't answer: try to press S or E\n"); return 1; @@ -332,7 +354,7 @@ void con_sig(int rb){ char command[256]; if(rb < 1) return; if(rb == 'q') quit(0); // q == exit - if(rb == 'L' || rb == 'R'){ + if(rb == '-' || rb == '+'){ if(!fgets(command, 255, stdin)){ fprintf(stderr, "You should give amount of steps after commands 'L' and 'R'\n"); return; @@ -344,15 +366,17 @@ void con_sig(int rb){ } } #define Die_on_error(arg) do{if(!send_command(arg)) goto erase_;}while(0) - if(strchr("LR01", rb)){ // command to execute + if(strchr("-+01Rr", rb)){ // command to execute got_command = 1; if(!send_command("LD1*")){ // start writing a program fprintf(stderr, "Error: previous program is running!\n"); return; } Die_on_error("BG*"); // move address pointer to beginning - Die_on_error("EN*"); // enable power - Die_on_error("SD10000*"); // set speed to max (625 steps per second with 1/16) + if(strchr("-+01", rb)){ + Die_on_error("EN*"); // enable power + Die_on_error("SD10000*"); // set speed to max (625 steps per second with 1/16) + } } switch(rb){ case 'h': @@ -366,7 +390,7 @@ void con_sig(int rb){ Die_on_error("DR*"); Die_on_error("ML*"); break; - case 'R': + case '+': Die_on_error("DR*"); if(stepsN) sprintf(command, "MV%d*", stepsN); @@ -374,7 +398,7 @@ void con_sig(int rb){ sprintf(command, "MV*"); Die_on_error(command); break; - case 'L': + case '-': Die_on_error("DL*"); if(stepsN) sprintf(command, "MV%d*", stepsN); @@ -383,7 +407,7 @@ void con_sig(int rb){ Die_on_error(command); break; case 'S': - Die_on_error("PS1*"); + Die_on_error("SP*"); break; case 'A': Die_on_error("ST1*"); @@ -391,6 +415,12 @@ void con_sig(int rb){ case 'E': erase_ctrlr(); break; + case 'R': + Die_on_error("SF*"); + break; + case 'r': + Die_on_error("CF*"); + break; /* default: cmd = (uint8_t) rb; write(comfd, &cmd, 1);*/ @@ -402,7 +432,7 @@ void con_sig(int rb){ } return; erase_: - if(!erase_ctrlr()) quit(1); + erase_ctrlr(); } /** @@ -429,13 +459,13 @@ uint32_t get_int(uint8_t *buff, size_t len){ int main(int argc, char *argv[]){ int rb; - uint8_t buff[128]; + char buff[128], *bufptr = buff; size_t L; if(argc == 2){ fout = fopen(argv[1], "a"); if(!fout){ perror("Can't open output file"); - exit(-1); + return (-1); } setbuf(fout, NULL); } @@ -445,16 +475,25 @@ int main(int argc, char *argv[]){ signal(SIGQUIT, SIG_IGN); // ctrl+\ . signal(SIGTSTP, SIG_IGN); // ctrl+Z setbuf(stdout, NULL); - if(!erase_ctrlr()) quit(1); + erase_ctrlr(); //t0 = dtime(); while(1){ rb = read_console(NULL, 1); if(rb > 0) con_sig(rb); - L = read_tty(buff, 127); + L = read_tty(bufptr, 127); if(L){ - buff[L] = 0; - printf("%s", buff); - if(fout) fprintf(fout, "%zd\t%s\n", time(NULL), buff); + bufptr += L; + if(bufptr - buff > 127){ + fprintf(stderr, "Error: input buffer overflow!\n"); + bufptr = buff; + } + if(bufptr[-1] == '*'){ // end of input command + *bufptr = 0; + parse_ctrlr_ans(buff); + //printf("%s", buff); + if(fout) fprintf(fout, "%zd\t%s\n", time(NULL), buff); + bufptr = buff; + } } } } diff --git a/Trinamic/SHA_client/Makefile b/Trinamic/SHA_client/Makefile new file mode 100644 index 0000000..19bac1f --- /dev/null +++ b/Trinamic/SHA_client/Makefile @@ -0,0 +1,22 @@ +PROGRAM = client +LDFLAGS = +SRCS = client.c +CC = gcc +DEFINES = -D_XOPEN_SOURCE=501 +CXX = gcc +CFLAGS = -Wall -Werror $(DEFINES) +OBJS = $(SRCS:.c=.o) +all : $(PROGRAM) clean +$(PROGRAM) : $(OBJS) + $(CC) $(CFLAGS) $(OBJS) $(LDFLAGS) -o $(PROGRAM) + +# some addition dependencies +# %.o: %.c +# $(CC) $(LDFLAGS) $(CFLAGS) $< -o $@ +#$(SRCS) : %.c : %.h $(INDEPENDENT_HEADERS) +# @touch $@ + +clean: + /bin/rm -f *.o *~ +depend: + $(CXX) -MM $(CXX.SRCS) diff --git a/Trinamic/SHA_client/client b/Trinamic/SHA_client/client new file mode 100755 index 0000000..15d2fd6 Binary files /dev/null and b/Trinamic/SHA_client/client differ diff --git a/Trinamic/SHA_client/client.c b/Trinamic/SHA_client/client.c new file mode 100644 index 0000000..30266b8 --- /dev/null +++ b/Trinamic/SHA_client/client.c @@ -0,0 +1,526 @@ +/* + * client.c - simple terminal client for operationg with + * Standa's 8MT175-150 translator by SMSD-1.5 driver + * + * Hardware operates in microsterpping mode (1/16), + * max current = 1.2A + * voltage = 12V + * "0" of driver connected to end-switch at opposite from motor side + * switch of motor's side connected to "IN1" + * + * Copyright 2013 Edward V. Emelianoff + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, + * MA 02110-1301, USA. + */ + +#include // tcsetattr +#include // tcsetattr, close, read, write +#include // ioctl +#include // printf, getchar, fopen, perror +#include // exit +#include // read +#include // read +#include // signal +#include // time +#include // memcpy, strcmp etc +#include // int types +#include // gettimeofday + +#define DBG(...) do{fprintf(stderr, __VA_ARGS__); }while(0) + +//double t0; // start time +static int bus_error = 0; // last error of data output +enum{ + NO_ERROR = 0, // normal execution + CODE_ERR, // error of exexuted program code + BUS_ERR, // data transmission error + COMMAND_ERR, // wrong command + CMD_DATA_ERR, // wrong data of command + UNDEFINED_ERR // something else wrong +}; + +FILE *fout = NULL; // file for messages duplicating +char *comdev = "/dev/ttyUSB0"; +int BAUD_RATE = B9600; +uint16_t step_spd = 900; // stepper speed: 225 steps per second in 1/1 mode +struct termio oldtty, tty; // TTY flags +struct termios oldt, newt; // terminal flags +int comfd = -1; // TTY fd + +int erase_ctrlr(); + +/** + * function for different purposes that need to know time intervals + * @return double value: time in seconds + * +double dtime(){ + double t; + struct timeval tv; + gettimeofday(&tv, NULL); + t = tv.tv_sec + ((double)tv.tv_usec)/1e6; + return t; +}*/ + +/** + * Exit & return terminal to old state + * @param ex_stat - status (return code) + */ +void quit(int ex_stat){ + tcsetattr(STDIN_FILENO, TCSANOW, &oldt); // return terminal to previous state + if(comfd > 0){ + erase_ctrlr(); + ioctl(comfd, TCSANOW, &oldtty ); // return TTY to previous state + close(comfd); + } + if(fout) fclose(fout); + printf("Exit! (%d)\n", ex_stat); + exit(ex_stat); +} + +/** + * Open & setup TTY, terminal + */ +void tty_init(){ + // terminal without echo + tcgetattr(STDIN_FILENO, &oldt); + newt = oldt; + //newt.c_lflag &= ~(ICANON | ECHO); + newt.c_lflag &= ~(ICANON); + if(tcsetattr(STDIN_FILENO, TCSANOW, &newt) < 0) quit(-2); + printf("\nOpen port...\n"); + if ((comfd = open(comdev,O_RDWR|O_NOCTTY|O_NONBLOCK)) < 0){ + fprintf(stderr,"Can't use port %s\n",comdev); + quit(1); + } + printf(" OK\nGet current settings...\n"); + if(ioctl(comfd,TCGETA,&oldtty) < 0) quit(-1); // Get settings + tty = oldtty; + tty.c_lflag = 0; // ~(ICANON | ECHO | ECHOE | ISIG) + tty.c_oflag = 0; + tty.c_cflag = BAUD_RATE|CS8|CREAD|CLOCAL | PARENB; // 9.6k, 8N1, RW, ignore line ctrl + tty.c_cc[VMIN] = 0; // non-canonical mode + tty.c_cc[VTIME] = 5; + if(ioctl(comfd,TCSETA,&tty) < 0) quit(-1); // set new mode + printf(" OK\n"); +} + +/** + * Read characters from console without echo + * @return char readed + */ +int read_console(char *buf, size_t len){ + int rb = 0; + ssize_t L = 0; + struct timeval tv; + int retval; + fd_set rfds; + tcsetattr(STDIN_FILENO, TCSANOW, &newt); + FD_ZERO(&rfds); + FD_SET(STDIN_FILENO, &rfds); + tv.tv_sec = 0; tv.tv_usec = 1000; + retval = select(1, &rfds, NULL, NULL, &tv); + if(retval){ + if(FD_ISSET(STDIN_FILENO, &rfds)){ + if(len < 2 || !buf) // command works as simple getchar + rb = getchar(); + else{ // read all data from console buffer + if((L = read(STDIN_FILENO, buf, len)) > 0) rb = (int)L; + } + } + } + //tcsetattr(STDIN_FILENO, TCSANOW, &oldt); + return rb; +} + +/** + * getchar() without echo + * wait until at least one character pressed + * @return character readed + */ +int mygetchar(){ // getchar() Enter + int ret; + do ret = read_console(NULL, 1); + while(ret == 0); + return ret; +} + +/** + * Read data from TTY + * @param buff (o) - buffer for data read + * @param length - buffer len + * @return amount of readed bytes + */ +size_t read_tty(char *buff, size_t length){ + ssize_t L = 0; + fd_set rfds; + struct timeval tv; + int retval; + FD_ZERO(&rfds); + FD_SET(comfd, &rfds); + tv.tv_sec = 0; tv.tv_usec = 50000; + retval = select(comfd + 1, &rfds, NULL, NULL, &tv); + if(retval < 1) return 0; + if(FD_ISSET(comfd, &rfds)){ + if((L = read(comfd, buff, length)) < 1){ + fprintf(stderr, "ERROR on bus, exit!\n"); + quit(-4); + } + } + return (size_t)L; +} + +/** + * wait for answer from server + * @param sock - socket fd + * @return 0 in case of error or timeout, 1 in case of socket ready + * +int waittoread(int sock){ + fd_set fds; + struct timeval timeout; + int rc; + timeout.tv_sec = 0; + timeout.tv_usec = 1000; + FD_ZERO(&fds); + FD_SET(sock, &fds); + rc = select(sock+1, &fds, NULL, NULL, &timeout); + if(rc < 0){ + perror("select failed"); + return 0; + } + if(rc > 0 && FD_ISSET(sock, &fds)) return 1; + return 0; +} +*/ + +void help(){ + printf("\n\nUse this commands:\n" + "0\tMove to end-switch 0\n" + "1\tMove to end-switch 1\n" + "-xxx\tMake xxx steps toward zero's end-switch (0 main infinity)\n" + "+xxx\tMake xxx steps toward end-switch 1 (0 main infinity)\n" + "S\tPause motor when program is running\n" + "A\tStop motor when programm is running\n" + "E\tErase previous program from controller's memory\n" + "R\tTurn relay ON\n" + "r\tTurn relay OFF\n" + ">\tincrease speed for 25pulses per second\n" + "<\tdecrease speed for 25pulses per second\n" + "\n" + ); +} + +void write_tty(char *str, int L){ + ssize_t D = write(comfd, str, L); + if(D != L){ + fprintf(stderr, "ERROR on bus, exit!\n"); + quit(-3); + } +} + +//#define dup_pr(...) do{printf(__VA_ARGS__); if(fout) fprintf(fout, __VA_ARGS__);}while(0) + +size_t read_ctrl_command(char *buf, size_t L){ // read data from controller to buffer buf + int i, j; + char *ptr = buf; + size_t R; + memset(buf, 0, L); + for(j = 0; j < L; j++, ptr++){ + R = 0; + for(i = 0; i < 10 && !R; i++){ + R = read_tty(ptr, 1); + } + if(!R){j--; break;} // nothing to read + if(*ptr == '*') // read only one command + break; + if(*ptr < ' '){ // omit spaces & non-characters + j--; ptr--; + } + } + return (size_t) j + 1; +} + +int parse_ctrlr_ans(char *ans){ + char *E = NULL, *Star = NULL; + if(!ans || !*ans) return 1; + bus_error = NO_ERROR; + if(!(E = strchr(ans, 'E')) || !(Star = strchr(ans, '*')) || E[1] != '1'){ + fprintf(stderr, "Answer format error (got: %s)\n", ans); + bus_error = UNDEFINED_ERR; + return 0; + } + switch (E[2]){ // E = "E1x" + case '0': // 10 - normal execution + break; + case '4': // 14 - program end + printf("Last command exectuted normally\n"); + break; + case '2': // command interrupt by other signal + fprintf(stderr, "Last command terminated\n"); + break; + case '3': + bus_error = CODE_ERR; + fprintf(stderr, "runtime"); + break; + case '5': + bus_error = BUS_ERR; + fprintf(stderr, "data bus"); + break; + case '6': + bus_error = COMMAND_ERR; + fprintf(stderr, "command"); + break; + case '9': + bus_error = CMD_DATA_ERR; + fprintf(stderr, "command data"); + break; + default: + bus_error = UNDEFINED_ERR; + fprintf(stderr, "undefined (%s)", ans); + } + if(bus_error != NO_ERROR){ + fprintf(stderr, " error in controller\n"); + return 0; + } + return 1; +} + +int send_command(char *cmd){ + int L = strlen(cmd); + size_t R = 0; + char ans[256]; + write_tty(cmd, L); + R = read_ctrl_command(ans, 255); +// DBG("readed: %s (cmd: %s, R = %zd, L = %d)\n", ans, cmd, R, L); + if(!R || (strncmp(ans, cmd, L) != 0)){ + fprintf(stderr, "Error: controller doesn't respond (answer: %s)\n", ans); + return 0; + } + R = read_ctrl_command(ans, 255); +// DBG("readed: %s\n", ans); + if(!R){ // controller is running or error + fprintf(stderr, "Controller doesn't answer!\n"); + return 0; + } + return parse_ctrlr_ans(ans); +} + +/* +int send5times(char *cmd){ // sends command 'cmd' up to 5 times (if errors), return 0 in case of false + int N, R = 0; + for(N = 0; N < 5 && !R; N++){ + R = send_command(cmd); + } + return R; +}*/ + + +int erase_ctrlr(){ + char *errmsg = "\n\nCan't erase controller's memory: some errors occured!\n\n"; + #define safely_send(x) do{ if(bus_error != NO_ERROR){ \ + fprintf(stderr, errmsg); return 0;} send_command(x); }while(0) + if(!send_command("LD1*")){ // start writing a program + //if(!send_command("LB*")){ // start writing a program into op-buffer + if(bus_error == COMMAND_ERR){ // motor is moving + printf("Found running program, stop it\n"); + if(!send_command("ST1*")) + //if(!send_command("ST*")) + send_command("SP*"); + send_command("LD1*"); + //send_command("LB*"); + }else{ + fprintf(stderr, "Controller doesn't answer: try to press S or E\n"); + return 1; + } + } + safely_send("BG*"); // move address pointer to beginning + safely_send("DS*"); // turn off motor + safely_send("ED*"); // end of program + if(bus_error != NO_ERROR){ + fprintf(stderr, errmsg); + return 0; + } + return 1; +} + +void con_sig(int rb){ + int stepsN = 0, got_command = 0; + char command[256]; + char buf[13]; + if(rb < 1) return; + if(rb == 'q') quit(0); // q == exit + if(rb == '-' || rb == '+'){ + if(!fgets(command, 255, stdin)){ + fprintf(stderr, "You should give amount of steps after commands 'L' and 'R'\n"); + return; + } + stepsN = atoi(command); // in ticks + if(stepsN < 0 || stepsN > 10000000){ + fprintf(stderr, "\n\nSteps amount should be > -1 and < 10000000 (0 means infinity)!\n\n"); + return; + } + } + #define Die_on_error(arg) do{if(!send_command(arg)) goto erase_;}while(0) + if(strchr("-+01Rr", rb)){ // command to execute + got_command = 1; + if(!send_command("LD1*")){ // start writing a program + //if(!send_command("LB*")){ // start writing a program into op-buffer + fprintf(stderr, "Error: previous program is running!\n"); + return; + } + Die_on_error("BG*"); // move address pointer to beginning + if(strchr("-+01", rb)){ + Die_on_error("EN*"); // enable power + //Die_on_error("SD10000*"); // set speed to max (156.25 steps per second with 1/16) + snprintf(buf, 12, "SD%u*", step_spd); + Die_on_error(buf); + } + } + switch(rb){ + case 'h': + help(); + break; + case '0': + Die_on_error("DL*"); + Die_on_error("HM*"); + break; + case '1': + Die_on_error("DR*"); + Die_on_error("ML*"); + break; + case '+': + Die_on_error("DR*"); + if(stepsN) + sprintf(command, "MV%d*", stepsN); + else + sprintf(command, "MV*"); + Die_on_error(command); + break; + case '-': + Die_on_error("DL*"); + if(stepsN) + sprintf(command, "MV%d*", stepsN); + else + sprintf(command, "MV*"); + Die_on_error(command); + break; + case 'S': + Die_on_error("SP*"); + break; + case 'A': + Die_on_error("ST1*"); + //Die_on_error("ST*"); + break; + case 'E': + erase_ctrlr(); + break; + case 'R': + Die_on_error("SF*"); + break; + case 'r': + Die_on_error("CF*"); + break; + case '>': // increase speed for 25 pulses + step_spd += 25; + printf("\nCurrent speed: %u pulses per sec\n", step_spd); + //snprintf(buf, 12, "SD%u*", step_spd); + //Die_on_error(buf); + break; + case '<': // decrease speed for 25 pulses + if(step_spd > 25){ + step_spd -= 25; + printf("\nCurrent speed: %u pulses per sec\n", step_spd); + //snprintf(buf, 12, "SD%u*", step_spd); + //Die_on_error(buf); + }else + printf("\nSpeed is too low\n"); + break; + +/* default: + cmd = (uint8_t) rb; + write(comfd, &cmd, 1);*/ + } + if(got_command){ // there was some command: write ending words + Die_on_error("DS*"); // turn off power from motor at end + Die_on_error("ED*"); // signal about command end + Die_on_error("ST1*");// start program + } + return; +erase_: + erase_ctrlr(); +} + +/** + * Get integer value from buffer + * @param buff (i) - buffer with int + * @param len - length of data in buffer (could be 2 or 4) + * @return + * +uint32_t get_int(uint8_t *buff, size_t len){ + int i; + printf("read %zd bytes: ", len); + for(i = 0; i < len; i++) printf("0x%x ", buff[i]); + printf("\n"); + if(len != 2 && len != 4){ + fprintf(stdout, "Bad data length!\n"); + return 0xffffffff; + } + uint32_t data = 0; + uint8_t *i8 = (uint8_t*) &data; + if(len == 2) memcpy(i8, buff, 2); + else memcpy(i8, buff, 4); + return data; +}*/ + +int main(int argc, char *argv[]){ + int rb; + char buff[128], *bufptr = buff; + size_t L; + if(argc == 2){ + fout = fopen(argv[1], "a"); + if(!fout){ + perror("Can't open output file"); + return (-1); + } + setbuf(fout, NULL); + } + tty_init(); + signal(SIGTERM, quit); // kill (-15) + signal(SIGINT, quit); // ctrl+C + signal(SIGQUIT, SIG_IGN); // ctrl+\ . + signal(SIGTSTP, SIG_IGN); // ctrl+Z + setbuf(stdout, NULL); + erase_ctrlr(); + //t0 = dtime(); + while(1){ + rb = read_console(NULL, 1); + if(rb > 0) con_sig(rb); + L = read_tty(bufptr, 127); + if(L){ + bufptr += L; + if(bufptr - buff > 127){ + fprintf(stderr, "Error: input buffer overflow!\n"); + bufptr = buff; + } + if(bufptr[-1] == '*'){ // end of input command + *bufptr = 0; + parse_ctrlr_ans(buff); + //printf("%s", buff); + if(fout) fprintf(fout, "%zd\t%s\n", time(NULL), buff); + bufptr = buff; + } + } + } +} diff --git a/Trinamic/SHA_client_cmdline/Makefile b/Trinamic/SHA_client_cmdline/Makefile new file mode 100644 index 0000000..ce771cf --- /dev/null +++ b/Trinamic/SHA_client_cmdline/Makefile @@ -0,0 +1,22 @@ +PROGRAM = client +LDFLAGS = +SRCS = client.c parceargs.c cmdlnopts.c +CC = gcc +DEFINES = -D_XOPEN_SOURCE=701 +CXX = gcc +CFLAGS = -Wall -Werror $(DEFINES) +OBJS = $(SRCS:.c=.o) +all : $(PROGRAM) clean +$(PROGRAM) : $(OBJS) + $(CC) $(CFLAGS) $(OBJS) $(LDFLAGS) -o $(PROGRAM) + +# some addition dependencies +# %.o: %.c +# $(CC) $(LDFLAGS) $(CFLAGS) $< -o $@ +#$(SRCS) : %.c : %.h $(INDEPENDENT_HEADERS) +# @touch $@ + +clean: + /bin/rm -f *.o *~ +depend: + $(CXX) -MM $(CXX.SRCS) diff --git a/Trinamic/SHA_client_cmdline/client.c b/Trinamic/SHA_client_cmdline/client.c new file mode 100644 index 0000000..e04614d --- /dev/null +++ b/Trinamic/SHA_client_cmdline/client.c @@ -0,0 +1,405 @@ +/* + * client.c - simple terminal client for operationg with + * Standa's 8MT175-150 translator by SMSD-1.5 driver + * + * Hardware operates in microsterpping mode (1/16), + * max current = 1.2A + * voltage = 12V + * "0" of driver connected to end-switch at opposite from motor side + * switch of motor's side connected to "IN1" + * + * Copyright 2013 Edward V. Emelianoff + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, + * MA 02110-1301, USA. + */ + +#include // tcsetattr +#include // tcsetattr, close, read, write +#include // ioctl +#include // printf, getchar, fopen, perror +#include // exit +#include // read +#include // read +#include // signal +#include // time +#include // memcpy, strcmp etc +#include // strcasecmp +#include // int types +#include // gettimeofday +#include "cmdlnopts.h" + +#define DBG(...) do{fprintf(stderr, __VA_ARGS__); }while(0) + +//double t0; // start time +static int bus_error = 0; // last error of data output +enum{ + NO_ERROR = 0, // normal execution + CODE_ERR, // error of exexuted program code + BUS_ERR, // data transmission error + COMMAND_ERR, // wrong command + CMD_DATA_ERR, // wrong data of command + UNDEFINED_ERR // something else wrong +}; + +int BAUD_RATE = B9600; +uint16_t step_spd = 900; // stepper speed: 225 steps per second in 1/1 mode +struct termio oldtty, tty; // TTY flags +int comfd = -1; // TTY fd + +int erase_ctrlr(); + +/** + * Exit & return terminal to old state + * @param ex_stat - status (return code) + */ +void quit(int ex_stat){ + if(comfd > 0){ + erase_ctrlr(); + ioctl(comfd, TCSANOW, &oldtty ); // return TTY to previous state + close(comfd); + } + printf("Exit! (%d)\n", ex_stat); + exit(ex_stat); +} + +/** + * Open & setup TTY, terminal + */ +void tty_init(){ + printf("\nOpen port...\n"); + if ((comfd = open(G.comdev,O_RDWR|O_NOCTTY|O_NONBLOCK)) < 0){ + fprintf(stderr,"Can't use port %s\n", G.comdev); + quit(1); + } + printf(" OK\nGet current settings...\n"); + if(ioctl(comfd,TCGETA,&oldtty) < 0) quit(-1); // Get settings + tty = oldtty; + tty.c_lflag = 0; // ~(ICANON | ECHO | ECHOE | ISIG) + tty.c_oflag = 0; + tty.c_cflag = BAUD_RATE|CS8|CREAD|CLOCAL | PARENB; // 9.6k, 8N1, RW, ignore line ctrl + tty.c_cc[VMIN] = 0; // non-canonical mode + tty.c_cc[VTIME] = 5; + if(ioctl(comfd,TCSETA,&tty) < 0) quit(-1); // set new mode + printf(" OK\n"); +} + +/** + * Read data from TTY + * @param buff (o) - buffer for data read + * @param length - buffer len + * @return amount of readed bytes + */ +size_t read_tty(char *buff, size_t length){ + ssize_t L = 0; + fd_set rfds; + struct timeval tv; + int retval; + FD_ZERO(&rfds); + FD_SET(comfd, &rfds); + tv.tv_sec = 0; tv.tv_usec = 50000; + retval = select(comfd + 1, &rfds, NULL, NULL, &tv); + if(retval < 1) return 0; + if(FD_ISSET(comfd, &rfds)){ + if((L = read(comfd, buff, length)) < 1){ + fprintf(stderr, "ERROR on bus, exit!\n"); + quit(-4); + } + } + return (size_t)L; +} + +void write_tty(char *str, int L){ + ssize_t D = write(comfd, str, L); + if(D != L){ + fprintf(stderr, "ERROR on bus, exit!\n"); + quit(-3); + } +} + +size_t read_ctrl_command(char *buf, size_t L){ // read data from controller to buffer buf + int i, j; + char *ptr = buf; + size_t R; + memset(buf, 0, L); + for(j = 0; j < L; j++, ptr++){ + R = 0; + for(i = 0; i < 10 && !R; i++){ + R = read_tty(ptr, 1); + } + if(!R){j--; break;} // nothing to read + if(*ptr == '*') // read only one command + break; + if(*ptr < ' '){ // omit spaces & non-characters + j--; ptr--; + } + } + return (size_t) j + 1; +} + +int parse_ctrlr_ans(char *ans){ + char *E = NULL, *Star = NULL; + if(!ans || !*ans) return 1; + bus_error = NO_ERROR; + if(!(E = strchr(ans, 'E')) || !(Star = strchr(ans, '*')) || E[1] != '1'){ + fprintf(stderr, "Answer format error (got: %s)\n", ans); + bus_error = UNDEFINED_ERR; + return 0; + } + switch (E[2]){ // E = "E1x" + case '0': // 10 - normal execution + break; + case '4': // 14 - program end + printf("Last command exectuted normally\n"); + break; + case '2': // command interrupt by other signal + fprintf(stderr, "Last command terminated\n"); + break; + case '3': + bus_error = CODE_ERR; + fprintf(stderr, "runtime"); + break; + case '5': + bus_error = BUS_ERR; + fprintf(stderr, "data bus"); + break; + case '6': + bus_error = COMMAND_ERR; + fprintf(stderr, "command"); + break; + case '9': + bus_error = CMD_DATA_ERR; + fprintf(stderr, "command data"); + break; + default: + bus_error = UNDEFINED_ERR; + fprintf(stderr, "undefined (%s)", ans); + } + if(bus_error != NO_ERROR){ + fprintf(stderr, " error in controller\n"); + return 0; + } + return 1; +} + +int send_command(char *cmd){ + int L = strlen(cmd); + size_t R = 0; + char ans[256]; + write_tty(cmd, L); + R = read_ctrl_command(ans, 255); +// DBG("readed: %s (cmd: %s, R = %zd, L = %d)\n", ans, cmd, R, L); + if(!R || (strncmp(ans, cmd, L) != 0)){ + fprintf(stderr, "Error: controller doesn't respond (answer: %s)\n", ans); + return 0; + } + R = read_ctrl_command(ans, 255); +// DBG("readed: %s\n", ans); + if(!R){ // controller is running or error + fprintf(stderr, "Controller doesn't answer!\n"); + return 0; + } + return parse_ctrlr_ans(ans); +} + +int erase_ctrlr(){ + char *errmsg = "\n\nCan't erase controller's memory: some errors occured!\n\n"; + printf("Erasing old program\n"); + #define safely_send(x) do{ if(bus_error != NO_ERROR){ \ + fprintf(stderr, errmsg); return 0;} send_command(x); }while(0) + if(!send_command("LD1*")){ // start writing a program + //if(!send_command("LB*")){ // start writing a program into op-buffer + if(bus_error == COMMAND_ERR){ // motor is moving + printf("Found running program, stop it\n"); + if(!send_command("ST1*")) + //if(!send_command("ST*")) + send_command("SP*"); + send_command("LD1*"); + //send_command("LB*"); + }else{ + fprintf(stderr, "Controller doesn't answer: maybe no power?\n"); + return 1; + } + } + safely_send("BG*"); // move address pointer to beginning + safely_send("DS*"); // turn off motor + safely_send("ED*"); // end of program + if(bus_error != NO_ERROR){ + fprintf(stderr, errmsg); + return 0; + } + return 1; +} + +int con_sig(int rb, int stepsN){ + int got_command = 0; + char command[256]; + char buf[13]; + #define Die_on_error(arg) do{if(!send_command(arg)) goto erase_;}while(0) + if(strchr("-+01Rr", rb)){ // command to execute + got_command = 1; + if(!send_command("LD1*")){ // start writing a program + //if(!send_command("LB*")){ // start writing a program into op-buffer + fprintf(stderr, "Error: previous program is running!\n"); + return 0; + } + Die_on_error("BG*"); // move address pointer to beginning + if(strchr("-+01", rb)){ + Die_on_error("EN*"); // enable power + //Die_on_error("SD10000*"); // set speed to max (156.25 steps per second with 1/16) + snprintf(buf, 12, "SD%u*", step_spd); + Die_on_error(buf); + } + } + switch(rb){ + case '0': + Die_on_error("DL*"); + Die_on_error("HM*"); + break; + case '1': + Die_on_error("DR*"); + Die_on_error("ML*"); + break; + case '+': + Die_on_error("DR*"); + if(stepsN) + sprintf(command, "MV%d*", stepsN); + else + sprintf(command, "MV*"); + Die_on_error(command); + break; + case '-': + Die_on_error("DL*"); + if(stepsN) + sprintf(command, "MV%d*", stepsN); + else + sprintf(command, "MV*"); + Die_on_error(command); + break; + case 'S': + Die_on_error("SP*"); + break; + case 'A': + Die_on_error("ST1*"); + //Die_on_error("ST*"); + break; + case 'E': + erase_ctrlr(); + break; + case 'R': + Die_on_error("SF*"); + break; + case 'r': + Die_on_error("CF*"); + break; + case '>': // increase speed for 25 pulses + step_spd += 25; + printf("\nCurrent speed: %u pulses per sec\n", step_spd); + //snprintf(buf, 12, "SD%u*", step_spd); + //Die_on_error(buf); + break; + case '<': // decrease speed for 25 pulses + if(step_spd > 25){ + step_spd -= 25; + printf("\nCurrent speed: %u pulses per sec\n", step_spd); + //snprintf(buf, 12, "SD%u*", step_spd); + //Die_on_error(buf); + }else + printf("\nSpeed is too low\n"); + break; + + } + if(got_command){ // there was some command: write ending words + Die_on_error("DS*"); // turn off power from motor at end + Die_on_error("ED*"); // signal about command end + Die_on_error("ST1*");// start program + return 1; + } + return 0; +erase_: + erase_ctrlr(); + return 0; +} + +void wait_for_answer(){ + char buff[128], *bufptr = buff; + size_t L; + while(1){ + L = read_tty(bufptr, 127); + if(L){ + bufptr += L; + if(bufptr - buff > 127){ + fprintf(stderr, "Error: input buffer overflow!\n"); + bufptr = buff; + } + if(bufptr[-1] == '*'){ // end of input command + *bufptr = 0; + parse_ctrlr_ans(buff); + return; + } + } + } +} + +int main(int argc, char *argv[]){ + parce_args(argc, argv); + tty_init(); + signal(SIGTERM, quit); // kill (-15) + signal(SIGINT, quit); // ctrl+C + signal(SIGQUIT, SIG_IGN); // ctrl+\ . + signal(SIGTSTP, SIG_IGN); // ctrl+Z + setbuf(stdout, NULL); + erase_ctrlr(); + if(G.erasecmd) return 0; + if(G.relaycmd == -1 && G.gotopos == NULL){ + printf("No commands given!\n"); + return -1; + } + if(G.relaycmd != -1){ + int ans; + if(G.relaycmd) // turn on + ans = con_sig('R',0); + else // turn off + ans = con_sig('r',0); + if(ans) + wait_for_answer(); + else + return -1; + } + if(G.gotopos){ + if(strcasecmp(G.gotopos, "refmir") == 0){ + if(!con_sig('1',0)) return -1; + printf("Go to last end-switch\n"); + wait_for_answer(); + if(!con_sig('-',500)) return -1; + }else if(strcasecmp(G.gotopos, "diagmir") == 0){ + if(!con_sig('0',0)) return -1; + printf("Go to zero's end-switch\n"); + wait_for_answer(); + if(!con_sig('+',2500)) return -1; + }else if(strcasecmp(G.gotopos, "shack") == 0){ + if(!con_sig('1',0)) return -1; + printf("Go to last end-switch\n"); + wait_for_answer(); + if(!con_sig('-',30000)) return -1; + }else{ + printf("Wrong goto command, should be one of refmir/diagmir/shack\n"); + return -1; + } + printf("Go to position\n"); + wait_for_answer(); + } + return 0; +} diff --git a/Trinamic/SHA_client_cmdline/cmdlnopts.c b/Trinamic/SHA_client_cmdline/cmdlnopts.c new file mode 100644 index 0000000..358c136 --- /dev/null +++ b/Trinamic/SHA_client_cmdline/cmdlnopts.c @@ -0,0 +1,84 @@ +/* + * cmdlnopts.c - the only function that parce cmdln args and returns glob parameters + * + * Copyright 2013 Edward V. Emelianoff + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, + * MA 02110-1301, USA. + */ +#include +#include +#include + +#include "cmdlnopts.h" + +/* + * here are global parameters initialisation + */ +glob_pars G; // internal global parameters structure +int help = 0; // whether to show help string + +glob_pars Gdefault = { + .comdev = "/dev/ttyUSB0", + .relaycmd = -1, + .erasecmd = 0, + .gotopos = NULL +}; + +/* + * Define command line options by filling structure: + * name has_arg flag val type argptr help +*/ +myoption cmdlnopts[] = { + /// " " + {"help", 0, NULL, 'h', arg_int, APTR(&help), "show this help"}, + /// " " + {"comdev",1, NULL, 'd', arg_string, APTR(&G.comdev), "input device path"}, + /// " (1)/ (0) " + {"relay", 1, NULL, 'r', arg_int, APTR(&G.relaycmd), "turn relay on (1)/off (0)"}, + /// " " + {"erase-old",0,NULL, 'e', arg_none, APTR(&G.erasecmd), "only erase controller's memory"}, + /// " (refmir/diagmir/shack)" + {"goto", 1, NULL, 'g', arg_string, APTR(&G.gotopos), "go to position (refmir/diagmir/shack)"}, + // ... + end_option +}; + +/** + * Parce command line options and return dynamically allocated structure + * to global parameters + * @param argc - copy of argc from main + * @param argv - copy of argv from main + * @return allocated structure with global parameters + */ +glob_pars *parce_args(int argc, char **argv){ + int i; + void *ptr; + ptr = memcpy(&G, &Gdefault, sizeof(G)); assert(ptr); + // format of help: "Usage: progname [args]\n" + /// ": %s []\n\n\t :\n" + change_helpstring("Usage: %s [args]\n\n\tWhere args are:\n"); + // parse arguments + parceargs(&argc, &argv, cmdlnopts); + if(help) showhelp(-1, cmdlnopts); + if(argc > 0){ + /// " []:" + printf("\n%s\n", "Ignore argument[s]:"); + for (i = 0; i < argc; i++) + printf("\t%s\n", argv[i]); + } + return &G; +} + diff --git a/Trinamic/SHA_client_cmdline/cmdlnopts.h b/Trinamic/SHA_client_cmdline/cmdlnopts.h new file mode 100644 index 0000000..eaa5560 --- /dev/null +++ b/Trinamic/SHA_client_cmdline/cmdlnopts.h @@ -0,0 +1,43 @@ +/* + * cmdlnopts.h - comand line options for parceargs + * + * Copyright 2013 Edward V. Emelianoff + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, + * MA 02110-1301, USA. + */ + +#pragma once +#ifndef __CMDLNOPTS_H__ +#define __CMDLNOPTS_H__ + +#include "parceargs.h" + +/* + * here are some typedef's for global data + */ + +typedef struct{ + char *comdev; // input device + int relaycmd; // -1 - nothing, 1 - on, 0 - off + int erasecmd; // 1 to erase old + char *gotopos; // position name: refmir, diagmir, shack +}glob_pars; + +extern glob_pars G; + +glob_pars *parce_args(int argc, char **argv); + +#endif // __CMDLNOPTS_H__ diff --git a/Trinamic/SHA_client_cmdline/parceargs.c b/Trinamic/SHA_client_cmdline/parceargs.c new file mode 100644 index 0000000..7e0c433 --- /dev/null +++ b/Trinamic/SHA_client_cmdline/parceargs.c @@ -0,0 +1,314 @@ +/* + * parceargs.c - parcing command line arguments & print help + * + * Copyright 2013 Edward V. Emelianoff + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, + * MA 02110-1301, USA. + */ + +#include // DBG +#include // getopt_long +#include // calloc, exit, strtoll +#include // assert +#include // strdup, strchr, strlen +#include // INT_MAX & so on +#include // gettext +#include // isalpha +#include "parceargs.h" + +#define DBG(...) + +// macro to print help messages +#ifndef PRNT + #define PRNT(x) gettext(x) +#endif + +char *helpstring = "%s\n"; + +/** + * Change standard help header + * MAY consist ONE "%s" for progname + * @param str (i) - new format + */ +void change_helpstring(char *s){ + int pcount = 0, scount = 0; + char *str = s; + // check `helpstring` and set it to default in case of error + for(; pcount < 2; str += 2){ + if(!(str = strchr(str, '%'))) break; + if(str[1] != '%') pcount++; // increment '%' counter if it isn't "%%" + else{ + str += 2; // pass next '%' + continue; + } + if(str[1] == 's') scount++; // increment "%s" counter + }; + DBG("pc: %d, sc: %d\n", pcount, scount); + if(pcount > 1 || pcount != scount){ // amount of pcount and/or scount wrong + fprintf(stderr, "Wrong helpstring!\n"); + exit(-1); + } + helpstring = s; + DBG("hs: %s\n", helpstring); +} + +/** + * Carefull atoll/atoi + * @param num (o) - returning value (or NULL if you wish only check number) - allocated by user + * @param str (i) - string with number must not be NULL + * @param t (i) - T_INT for integer or T_LLONG for long long (if argtype would be wided, may add more) + * @return TRUE if conversion sone without errors, FALSE otherwise + */ +bool myatoll(void *num, char *str, argtype t){ + long long tmp, *llptr; + int *iptr; + char *endptr; + assert(str); + assert(num); + tmp = strtoll(str, &endptr, 0); + if(endptr == str || *str == '\0' || *endptr != '\0') + return FALSE; + switch(t){ + case arg_longlong: + llptr = (long long*) num; + *llptr = tmp; + break; + case arg_int: + default: + if(tmp < INT_MIN || tmp > INT_MAX){ + fprintf(stderr, "Integer out of range\n"); + return FALSE; + } + iptr = (int*)num; + *iptr = (int)tmp; + } + return TRUE; +} + +// the same as myatoll but for double +// There's no NAN & INF checking here (what if they would be needed?) +bool myatod(void *num, const char *str, argtype t){ + double tmp, *dptr; + float *fptr; + char *endptr; + assert(str); + tmp = strtod(str, &endptr); + if(endptr == str || *str == '\0' || *endptr != '\0') + return FALSE; + switch(t){ + case arg_double: + dptr = (double *) num; + *dptr = tmp; + break; + case arg_float: + default: + fptr = (float *) num; + *fptr = (float)tmp; + break; + } + return TRUE; +} + +/** + * Get index of current option in array options + * @param opt (i) - returning val of getopt_long + * @param options (i) - array of options + * @return index in array + */ +int get_optind(int opt, myoption *options){ + int oind; + myoption *opts = options; + assert(opts); + for(oind = 0; opts->name && opts->val != opt; oind++, opts++); + if(!opts->name || opts->val != opt) // no such parameter + showhelp(-1, options); + return oind; +} + +/** + * Parce command line arguments + * ! If arg is string, then value will be strdup'ed! + * + * @param argc (io) - address of argc of main(), return value of argc stay after `getopt` + * @param argv (io) - address of argv of main(), return pointer to argv stay after `getopt` + * BE CAREFUL! if you wanna use full argc & argv, save their original values before + * calling this function + * @param options (i) - array of `myoption` for arguments parcing + * + * @exit: in case of error this function show help & make `exit(-1)` + */ +void parceargs(int *argc, char ***argv, myoption *options){ + char *short_options, *soptr; + struct option *long_options, *loptr; + size_t optsize, i; + myoption *opts = options; + // check whether there is at least one options + assert(opts); + assert(opts[0].name); + // first we count how much values are in opts + for(optsize = 0; opts->name; optsize++, opts++); + // now we can allocate memory + short_options = calloc(optsize * 3 + 1, 1); // multiply by three for '::' in case of args in opts + long_options = calloc(optsize + 1, sizeof(struct option)); + opts = options; loptr = long_options; soptr = short_options; + // fill short/long parameters and make a simple checking + for(i = 0; i < optsize; i++, loptr++, opts++){ + // check + assert(opts->name); // check name + if(opts->has_arg){ + assert(opts->type != arg_none); // check error with arg type + assert(opts->argptr); // check pointer + } + if(opts->type != arg_none) // if there is a flag without arg, check its pointer + assert(opts->argptr); + // fill long_options + // don't do memcmp: what if there would be different alignment? + loptr->name = opts->name; + loptr->has_arg = opts->has_arg; + loptr->flag = opts->flag; + loptr->val = opts->val; + // fill short options if they are: + if(!opts->flag){ + *soptr++ = opts->val; + if(opts->has_arg) // add ':' if option has required argument + *soptr++ = ':'; + if(opts->has_arg == 2) // add '::' if option has optional argument + *soptr++ = ':'; + } + } + // now we have both long_options & short_options and can parse `getopt_long` + while(1){ + int opt; + int oindex = 0, optind = 0; // oindex - number of option in argv, optind - number in options[] + if((opt = getopt_long(*argc, *argv, short_options, long_options, &oindex)) == -1) break; + if(opt == '?'){ + opt = optopt; + optind = get_optind(opt, options); + if(options[optind].has_arg == 1) showhelp(optind, options); // need argument + } + else{ + if(opt == 0 || oindex > 0) optind = oindex; + else optind = get_optind(opt, options); + } + opts = &options[optind]; +#ifdef EBUG +DBG ("\n*******\noption %s (oindex = %d / optind = %d)", options[optind].name, oindex, optind); +if(optarg) DBG (" with arg %s", optarg); +DBG ("\n"); +#endif + if(opt == 0 && opts->has_arg == 0) continue; // only long option changing integer flag +DBG("opt = %c, arg type: ", opt); + // now check option + if(opts->has_arg == 1) assert(optarg); + bool result = TRUE; + // even if there is no argument, but argptr != NULL, think that optarg = "1" + if(!optarg) optarg = "1"; + switch(opts->type){ + default: + case arg_none: +DBG("none\n"); + if(opts->argptr) *((int*)opts->argptr) = 1; // set argptr to 1 + break; + case arg_int: +DBG("integer\n"); + result = myatoll(opts->argptr, optarg, arg_int); + break; + case arg_longlong: +DBG("long long\n"); + result = myatoll(opts->argptr, optarg, arg_longlong); + break; + case arg_double: +DBG("double\n"); + result = myatod(opts->argptr, optarg, arg_double); + break; + case arg_float: +DBG("double\n"); + result = myatod(opts->argptr, optarg, arg_float); + break; + case arg_string: +DBG("string\n"); + result = (*((char **)opts->argptr) = strdup(optarg)); + break; + case arg_function: +DBG("function\n"); + result = ((argfn)opts->argptr)(optarg, optind); + break; + } + if(!result){ +DBG("OOOPS! Error in result\n"); + showhelp(optind, options); + } + } + *argc -= optind; + *argv += optind; +} + +/** + * Show help information based on myoption->help values + * @param oindex (i) - if non-negative, show only help by myoption[oindex].help + * @param options (i) - array of `myoption` + * + * @exit: run `exit(-1)` !!! + */ +void showhelp(int oindex, myoption *options){ + // ATTENTION: string `help` prints through macro PRNT(), bu default it is gettext, + // but you can redefine it before `#include "parceargs.h"` + int max_opt_len = 0; // max len of options substring - for right indentation + const int bufsz = 255; + char buf[bufsz+1]; + myoption *opts = options; + assert(opts); + assert(opts[0].name); // check whether there is at least one options + if(oindex > -1){ // print only one message + opts = &options[oindex]; + printf(" "); + if(!opts->flag && isalpha(opts->val)) printf("-%c, ", opts->val); + printf("--%s", opts->name); + if(opts->has_arg == 1) printf("=arg"); + else if(opts->has_arg == 2) printf("[=arg]"); + printf(" %s\n", PRNT(opts->help)); + exit(-1); + } + // header, by default is just "progname\n" + printf("\n"); + if(strstr(helpstring, "%s")) // print progname + printf(helpstring, __progname); + else // only text + printf("%s", helpstring); + printf("\n"); + // count max_opt_len + do{ + int L = strlen(opts->name); + if(max_opt_len < L) max_opt_len = L; + }while((++opts)->name); + max_opt_len += 14; // format: '-S , --long[=arg]' - get addition 13 symbols + opts = options; + // Now print all help + do{ + int p = sprintf(buf, " "); // a little indent + if(!opts->flag && isalpha(opts->val)) // .val is short argument + p += snprintf(buf+p, bufsz-p, "-%c, ", opts->val); + p += snprintf(buf+p, bufsz-p, "--%s", opts->name); + if(opts->has_arg == 1) // required argument + p += snprintf(buf+p, bufsz-p, "=arg"); + else if(opts->has_arg == 2) // optional argument + p += snprintf(buf+p, bufsz-p, "[=arg]"); + assert(p < max_opt_len); // there would be magic if p >= max_opt_len + printf("%-*s%s\n", max_opt_len+1, buf, PRNT(opts->help)); // write options & at least 2 spaces after + }while((++opts)->name); + printf("\n\n"); + exit(-1); +} diff --git a/Trinamic/SHA_client_cmdline/parceargs.h b/Trinamic/SHA_client_cmdline/parceargs.h new file mode 100644 index 0000000..554803b --- /dev/null +++ b/Trinamic/SHA_client_cmdline/parceargs.h @@ -0,0 +1,105 @@ +/* + * parceargs.h - headers for parcing command line arguments + * + * Copyright 2013 Edward V. Emelianoff + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, + * MA 02110-1301, USA. + */ +#pragma once +#ifndef __PARCEARGS_H__ +#define __PARCEARGS_H__ + +#include // bool +#include + +#ifndef TRUE + #define TRUE true +#endif + +#ifndef FALSE + #define FALSE false +#endif + +// macro for argptr +#define APTR(x) ((void*)x) + +// if argptr is a function: +typedef bool(*argfn)(void *arg, int N); + +/* + * type of getopt's argument + * WARNING! + * My function change value of flags by pointer, so if you want to use another type + * make a latter conversion, example: + * char charg; + * int iarg; + * myoption opts[] = { + * {"value", 1, NULL, 'v', arg_int, &iarg, "char val"}, ..., end_option}; + * ..(parce args).. + * charg = (char) iarg; + */ +typedef enum { + arg_none = 0, // no arg + arg_int, // integer + arg_longlong, // long long + arg_double, // double + arg_float, // float + arg_string, // char * + arg_function // parce_args will run function `bool (*fn)(char *optarg, int N)` +} argtype; + +/* + * Structure for getopt_long & help + * BE CAREFUL: .argptr is pointer to data or pointer to function, + * conversion depends on .type + * + * ATTENTION: string `help` prints through macro PRNT(), bu default it is gettext, + * but you can redefine it before `#include "parceargs.h"` + * + * if arg is string, then value wil be strdup'ed like that: + * char *str; + * myoption opts[] = {{"string", 1, NULL, 's', arg_string, &str, "string val"}, ..., end_option}; + * *(opts[1].str) = strdup(optarg); + * in other cases argptr should be address of some variable (or pointer to allocated memory) + * + * NON-NULL argptr should be written inside macro APTR(argptr) or directly: (void*)argptr + * + * !!!LAST VALUE OF ARRAY SHOULD BE `end_option` or ZEROS !!! + * + */ +typedef struct{ + // these are from struct option: + const char *name; // long option's name + int has_arg; // 0 - no args, 1 - nesessary arg, 2 - optionally arg + int *flag; // NULL to return val, pointer to int - to set its value of val (function returns 0) + int val; // short opt name (if flag == NULL) or flag's value + // and these are mine: + argtype type; // type of argument + void *argptr; // pointer to variable to assign optarg value or function `bool (*fn)(char *optarg, int N)` + char *help; // help string which would be shown in function `showhelp` or NULL +} myoption; + +// last string of array (all zeros) +#define end_option {0,0,0,0,0,0,0} + + +extern const char *__progname; + +void showhelp(int oindex, myoption *options); +void parceargs(int *argc, char ***argv, myoption *options); +void change_helpstring(char *s); + +#endif // __PARCEARGS_H__ diff --git a/fixed_sort_example.c b/fixed_sort_example.c index cfe5463..f48abdf 100644 --- a/fixed_sort_example.c +++ b/fixed_sort_example.c @@ -28,16 +28,17 @@ static __inline__ unsigned long long rdtsc(void){ } int arra[5][3] = {{3,2,1}, {3,6,4}, {7,5,8}, {1024,5543,9875}, {1001,-1001,1002}}; int arrb[5][3] = {{3,2,1}, {3,6,4}, {7,5,8}, {1024,5543,9875}, {1001,-1001,1002}}; +int arrc[5][3] = {{3,2,1}, {3,6,4}, {7,5,8}, {1024,5543,9875}, {1001,-1001,1002}}; /* - * It seems that this manner of sorting would be less productive than sort3b - * but even on -O1 it gives better results (median by 1000 seq); - * sort3c works best until -O3: + * It seems that this manner of sorting would be less productive than sort3b or sort3c + * but even on -O1 it gives better results (median by 1000 seq): + * * -Ox timing a timing b timing c - * 0 444 396 243 - * 1 108 144 93 - * 2 126 141 93 - * 3 105 138 159 + * 0 453 402 366 + * 1 111 174 147 + * 2 126 165 144 + * 3 117 171 141 */ static inline void sort3a(int *d){ #define min(x, y) (x +#include +#include +#include +#include +#include +#include +#include + +#define BUFSZ 1024 + +static void getpath(const char *modname, char *buf, size_t bufsize){ + int filedes[2]; + char cmd[256]; + ssize_t count; + snprintf(cmd, 255, "modinfo -F filename %s", modname); + if(pipe(filedes) == -1) + err(3, "pipe"); + pid_t pid = fork(); + if(pid == -1) + err(4, "fork"); + else if (pid == 0){ + while ((dup2(filedes[1], STDOUT_FILENO) == -1) && (errno == EINTR)) {} + close(filedes[1]); + close(filedes[0]); + system(cmd); + exit(0); + } + while(1){ + count = read(filedes[0], buf, bufsize); + if(count == -1){ + if(errno == EINTR) + continue; + else + err(7, "read"); + }else + break; + } + if(count > 1) buf[count-1] = 0; + close(filedes[0]); + wait(0); +} + +int main(){ + int fd = open("/proc/modules", O_RDONLY); + if(fd < 0) err(1, "open"); + char buf[BUFSZ+1], *end = buf; + size_t readed, toread = BUFSZ; + buf[BUFSZ] = 0; + while(1){ + readed = read(fd, end, toread); + if(readed < 1 && end == buf) break; + end += readed; + *end = 0; + char *E; +#define getspace(X) {E = strchr(X, ' '); if(!E) break; *E = 0; E++;} + getspace(buf); + char *name = buf, *start = E; + if(start >= end) break; + getspace(start); +#undef getspace + size_t size = atoll(start); + char path[1024]; + getpath(name, path, sizeof(path)); + printf("%s: %zd\n", path, size); + if(E < end){ + if((start = strchr(E, '\n'))) start++; + else start = end; + }else start = E; + if(start < end){ + readed = end - start; + memmove(buf, start, readed); + end = buf + readed; + toread = BUFSZ - readed; + }else{ + toread = BUFSZ; + end = buf; + } + } + close(fd); + return 0; +} diff --git a/websockets/Makefile b/websockets/Makefile new file mode 100644 index 0000000..bfd8f10 --- /dev/null +++ b/websockets/Makefile @@ -0,0 +1,22 @@ +PROGRAM = websocktest +LDFLAGS = $(shell pkg-config --libs libwebsockets) -lpthread +SRCS = test.c +CC = gcc +DEFINES = -D_XOPEN_SOURCE=501 -DCUR_PATH=\"$(shell pwd)\" +CXX = gcc +CFLAGS = -Wall -Werror -Wextra $(DEFINES) $(shell pkg-config --cflags libwebsockets) +OBJS = $(SRCS:.c=.o) +all : $(PROGRAM) clean +$(PROGRAM) : $(OBJS) + $(CC) $(CFLAGS) $(OBJS) $(LDFLAGS) -o $(PROGRAM) + +# some addition dependencies +# %.o: %.c +# $(CC) $(LDFLAGS) $(CFLAGS) $< -o $@ +#$(SRCS) : %.c : %.h $(INDEPENDENT_HEADERS) +# @touch $@ + +clean: + /bin/rm -f *.o *~ +depend: + $(CXX) -MM $(CXX.SRCS) diff --git a/websockets/leaf.jpg b/websockets/leaf.jpg new file mode 100644 index 0000000..1a3f46b Binary files /dev/null and b/websockets/leaf.jpg differ diff --git a/websockets/leaf1.jpg b/websockets/leaf1.jpg new file mode 100644 index 0000000..f75a50a Binary files /dev/null and b/websockets/leaf1.jpg differ diff --git a/websockets/test.c b/websockets/test.c new file mode 100644 index 0000000..844109c --- /dev/null +++ b/websockets/test.c @@ -0,0 +1,631 @@ +#include +#include +#include +#include +#include + +#include + +#include + +#include // tcsetattr +#include // int types +#include // read +#include // read + +#include + +#define _U_ __attribute__((__unused__)) + +#define MESSAGE_QUEUE_SIZE 3 +#define MESSAGE_LEN 128 +// individual data per session +typedef struct{ + int num; + int idxwr; + int idxrd; + char message[MESSAGE_QUEUE_SIZE][MESSAGE_LEN]; +}per_session_data; + +per_session_data global_queue; +pthread_mutex_t command_mutex; + +char cmd_buf[5] = {0}; +int data_in_buf = 0; // signals that there's some data in cmd_buf to send to motors + +void put_message_to_queue(char *msg, per_session_data *dat){ + int L = strlen(msg); + if(dat->num >= MESSAGE_QUEUE_SIZE) return; + dat->num++; + if(L < 1 || L > MESSAGE_LEN - 1) L = MESSAGE_LEN - 1; + strncpy(dat->message[dat->idxwr], msg, L); + dat->message[dat->idxwr][L] = 0; + if((++(dat->idxwr)) >= MESSAGE_QUEUE_SIZE) dat->idxwr = 0; +} + +char *get_message_from_queue(per_session_data *dat){ + char *R = dat->message[dat->idxrd]; + if(dat->num <= 0) return NULL; + if((++dat->idxrd) >= MESSAGE_QUEUE_SIZE) dat->idxrd = 0; + dat->num--; + return R; +} + +int force_exit = 0; + +uint8_t buf[9]; +#define TTYBUFLEN 128 +uint8_t ttybuff[TTYBUFLEN]; +char *comdev = "/dev/ttyUSB0"; +int BAUD_RATE = B115200; +int comfd = -1; // TTY fd +uint32_t motor_speed = 50; + +//**************************************************************************// +int32_t get_integer(uint8_t *buff){ + int32_t val; + int ii; + uint8_t *valptr = (uint8_t*) &val + 3; + for(ii = 4; ii < 8; ii++) + *valptr-- = buff[ii]; + return val; +} +void inttobuf(uint8_t *buf, int32_t value){// copy param to buffer + int i; + uint8_t *bytes = (uint8_t*) &value + 3; + for(i = 4; i < 8; i++) + buf[i] = *bytes--; +} + +/** + * read tty + * @return number of readed symbols + */ +size_t read_tty(){ + ssize_t L = 0, l, buffsz = TTYBUFLEN; + struct timeval tv; + int sel; + fd_set rfds; + FD_ZERO(&rfds); + FD_SET(comfd, &rfds); + tv.tv_sec = 0; tv.tv_usec = 100000; + sel = select(comfd + 1, &rfds, NULL, NULL, &tv); + if(sel > 0){ + if(FD_ISSET(comfd, &rfds)){ + if((L = read(comfd, ttybuff, buffsz)) < 1){ // disconnect or other troubles + fprintf(stderr, "USB error or disconnected!\n"); + exit(1); + }else{ + if(L == 0){ // USB disconnected + fprintf(stderr, "USB disconnected!\n"); + exit(1); + } + if(L == 9) return 9; + // all OK continue reading + //DBG("readed %zd bytes, try more.. ", L); + buffsz -= L; + select(comfd + 1, &rfds, NULL, NULL, &tv); + while(L < 9 && buffsz > 0 && (l = read(comfd, ttybuff+L, buffsz)) > 0){ + L += l; + buffsz -= l; + select(comfd + 1, &rfds, NULL, NULL, &tv); + } + } + } + } + return (size_t) L; +} + +static inline uint32_t log_2(const uint32_t x){ + if(x == 0) return 0; + return (31 - __builtin_clz (x)); +} + +int send_command(uint8_t *ninebytes); + +#define log2(x) ((int)log_2((uint32_t)x)) +/* + * check L bytes of ttybuf (maybe there was a command to check endpoint) + */ +void check_tty_sig(size_t l){ + int L = l; + uint8_t movbk[]= {1,4,1,0,0,0,0,0,0}; + //if(L < 9) return; // WTF? + uint8_t* buf = ttybuff; + char msg[128]; + const uint8_t pattern[] = {2,1,128,138,0,0,0}; + while(L > 0){ + int32_t Ival = get_integer(buf); + if(memcmp((void*)buf, (void*)pattern, sizeof(pattern)) == 0){ // motor has reached position + int motnum = log2(Ival); + snprintf(msg, 127, "Motor %d has reached position!", motnum); + printf(" %s (%d)\n", msg, Ival); + put_message_to_queue(msg, &global_queue); + if(motnum == 0){ // move motor 0 to 2000 usteps + inttobuf(movbk, 2000); + send_command(movbk); + }else if(motnum == 2){ // move motor 2 to 3450 usteps + movbk[3] = 2; + inttobuf(movbk, 3450); + send_command(movbk); + }else{ + printf(" WTF?\n"); + } + }else printf(" %d\n", Ival); + L -= 9; + buf += 9; + }; +} + +int send_command(uint8_t *ninebytes){ + uint8_t crc = 0; + size_t L; + int i; + printf("send: "); + for(i = 0; i < 8; crc += ninebytes[i++]) + printf("%u,", ninebytes[i]); + ninebytes[8] = crc; + printf("%u\n",ninebytes[8]); + if(9 != write(comfd, ninebytes, 9)){ + perror("Can't write to Trinamic"); + return 0; + } + if((L = read_tty())){ + printf("got %zd bytes from tty: ", L); + check_tty_sig(L); + } + return 1; +} + +void process_buf(char *command){ + memset(buf, 0, 9); + buf[0] = 1; // controller # + if(command[0] == 'W'){ // 1/16 + buf[1] = 5; + buf[2] = 140; // ustep resolution + buf[7] = 4; // 1/16 + send_command(buf); + buf[3] = 2; // motor #2 + send_command(buf); + return; + }else if(command[0] == 'S'){ // change current speed + long X = strtol(&command[1], NULL, 10); + if(X > 9 && X < 501){ + motor_speed = (uint32_t) X; + printf("set speed to %u\n", motor_speed); + buf[1] = 5; // SAP + buf[2] = 4; // max pos speed + inttobuf(buf, motor_speed); + send_command(buf); + buf[3] = 2; // 2nd motor + send_command(buf); + } + return; + } + if(command[1] == '0'){ // go to start point for further moving to middle + if(command[0] == 'U') return; + uint8_t wt[] = {1,138,0,0,0,0,0,5,0}; + uint8_t movbk[]= {1,4,1,0,0,0,0,0,0}; + inttobuf(movbk, -4500); // 1st motor + send_command(movbk); + movbk[3] = 2; + inttobuf(movbk, -7300); // 2nd motor + send_command(movbk); + send_command(wt); // wait + return; + } + if(command[1] == 'X') buf[3] = 2; // X motor -> #2 + else if(command[1] == 'Y') buf[3] = 0; // Y motor -> #0 + if(command[0] == 'D'){ // start moving + if(command[2] == '+') buf[1] = 1; // ROR + else if(command[2] == '-') buf[1] = 2; // ROL + }else if(command[0] == 'U'){ // stop + buf[1] = 3; // STP + } + inttobuf(buf, motor_speed); + if(!send_command(buf)){ + printf("Can't send command"); + }; +} + +#define MESG(X) do{if(dat) put_message_to_queue(X, dat);}while(0) +void websig(char *command, per_session_data *dat){ + if(command[0] == 'W' || command[1] == '0' || command[0] == 'S'){ + if(command[0] == 'W'){ + MESG("Set microstepping to 1/16"); + }else if(command[1] == '0'){ + MESG("Go to the middle. Please, wait!"); + }else{ + MESG("Change speed"); + } + goto ret; + } + if(command[1] != 'X' && command[1] != 'Y'){ // error + MESG("Undefined coordinate"); + return; + } + if(command[0] != 'D' && command[0] != 'U'){ + MESG("Undefined command"); + return; + } +ret: + pthread_mutex_lock(&command_mutex); + strncpy(cmd_buf, command, 4); + data_in_buf = 1; + pthread_mutex_unlock(&command_mutex); + while(data_in_buf); // wait for execution +} + +static void dump_handshake_info(struct libwebsocket *wsi){ + int n; + static const char *token_names[] = { + "GET URI", + "POST URI", + "OPTIONS URI", + "Host", + "Connection", + "key 1", + "key 2", + "Protocol", + "Upgrade", + "Origin", + "Draft", + "Challenge", + + /* new for 04 */ + "Key", + "Version", + "Sworigin", + + /* new for 05 */ + "Extensions", + + /* client receives these */ + "Accept", + "Nonce", + "Http", + + /* http-related */ + "Accept:", + "Ac-Request-Headers:", + "If-Modified-Since:", + "If-None-Match:", + "Accept-Encoding:", + "Accept-Language:", + "Pragma:", + "Cache-Control:", + "Authorization:", + "Cookie:", + "Content-Length:", + "Content-Type:", + "Date:", + "Range:", + "Referer:", + "Uri-Args:", + + + "MuxURL", + + /* use token storage to stash these */ + + "Client sent protocols", + "Client peer address", + "Client URI", + "Client host", + "Client origin", + + /* always last real token index*/ + "WSI token count" + }; + char buf[256]; + int L = sizeof(token_names) / sizeof(token_names[0]); + for (n = 0; n < L; n++) { + if (!lws_hdr_total_length(wsi, n)) + continue; + lws_hdr_copy(wsi, buf, sizeof buf, n); + printf(" %s = %s\n", token_names[n], buf); + } +} + +static int my_protocol_callback(_U_ struct libwebsocket_context *context, + _U_ struct libwebsocket *wsi, + enum libwebsocket_callback_reasons reason, + _U_ void *user, void *in, _U_ size_t len){ + unsigned char buf[LWS_SEND_BUFFER_PRE_PADDING + MESSAGE_LEN + + LWS_SEND_BUFFER_POST_PADDING]; + unsigned char *p = &buf[LWS_SEND_BUFFER_PRE_PADDING]; + char client_name[128]; + char client_ip[128]; + char *M, *msg = (char*) in; + per_session_data *dat = (per_session_data *) user; + int L, W; + void parse_queue_msg(per_session_data *d){ + if((M = get_message_from_queue(d))){ + L = strlen(M); + strncpy((char *)p, M, L); + W = libwebsocket_write(wsi, p, L, LWS_WRITE_TEXT); + if(L != W){ + lwsl_err("Can't write to socket"); + } + } + } + //struct lws_tokens *tok = (struct lws_tokens *) user; + switch (reason) { + case LWS_CALLBACK_ESTABLISHED: + printf("New Connection\n"); + memset(dat, 0, sizeof(per_session_data)); + libwebsocket_callback_on_writable(context, wsi); + break; + case LWS_CALLBACK_SERVER_WRITEABLE: + if(dat->num == 0 && global_queue.num == 0){ + libwebsocket_callback_on_writable(context, wsi); + return 0; + }else{ + parse_queue_msg(dat); + parse_queue_msg(&global_queue); + libwebsocket_callback_on_writable(context, wsi); + } + break; + case LWS_CALLBACK_RECEIVE: + websig(msg, dat); + break; + case LWS_CALLBACK_FILTER_NETWORK_CONNECTION: + libwebsockets_get_peer_addresses(context, wsi, (int)(long)in, + client_name, 127, client_ip, 127); + printf("Received network connection from %s (%s)\n", + client_name, client_ip); + break; + case LWS_CALLBACK_FILTER_PROTOCOL_CONNECTION: + printf("Client asks for %s\n", msg); + dump_handshake_info(wsi); + break; + case LWS_CALLBACK_CLOSED: + printf("Client disconnected\n"); + break; + default: + break; + } + + return 0; +} + + +static char encoding_table[] = {'A', 'B', 'C', 'D', 'E', 'F', 'G', 'H', + 'I', 'J', 'K', 'L', 'M', 'N', 'O', 'P', + 'Q', 'R', 'S', 'T', 'U', 'V', 'W', 'X', + 'Y', 'Z', 'a', 'b', 'c', 'd', 'e', 'f', + 'g', 'h', 'i', 'j', 'k', 'l', 'm', 'n', + 'o', 'p', 'q', 'r', 's', 't', 'u', 'v', + 'w', 'x', 'y', 'z', '0', '1', '2', '3', + '4', '5', '6', '7', '8', '9', '+', '/'}; +static int mod_table[] = {0, 2, 1}; + + +unsigned char *base64_encode(const unsigned char *data, + size_t input_length, + size_t *output_length) { + size_t i,j; + *output_length = 4 * ((input_length + 2) / 3); + + unsigned char *encoded_data = malloc(*output_length); + if (encoded_data == NULL) return NULL; + + for (i = 0, j = 0; i < input_length;) { + + uint32_t octet_a = i < input_length ? (unsigned char)data[i++] : 0; + uint32_t octet_b = i < input_length ? (unsigned char)data[i++] : 0; + uint32_t octet_c = i < input_length ? (unsigned char)data[i++] : 0; + + uint32_t triple = (octet_a << 0x10) + (octet_b << 0x08) + octet_c; + + encoded_data[j++] = encoding_table[(triple >> 3 * 6) & 0x3F]; + encoded_data[j++] = encoding_table[(triple >> 2 * 6) & 0x3F]; + encoded_data[j++] = encoding_table[(triple >> 1 * 6) & 0x3F]; + encoded_data[j++] = encoding_table[(triple >> 0 * 6) & 0x3F]; + } + + for (i = 0; i < (size_t)mod_table[input_length % 3]; i++) + encoded_data[*output_length - 1 - i] = '='; + + return encoded_data; +} + + + + +static int improto_callback(_U_ struct libwebsocket_context *context, + _U_ struct libwebsocket *wsi, + enum libwebsocket_callback_reasons reason, + _U_ void *user, void *in, _U_ size_t len){ + char client_name[128]; + char client_ip[128]; + char *msg = (char*) in; + void send_image(){ + static int c = 0; + struct stat stat_buf; + unsigned char *buffer = NULL, *b64 = NULL; + unsigned char *p = NULL; + int fd; + size_t L, W; + if(c) fd = open("leaf.jpg", O_RDONLY); + else fd = open("leaf1.jpg", O_RDONLY); + c = !c; + if(fd < 0){ + lwsl_err("Can't open image file"); + return; + } + fstat(fd, &stat_buf); + L = stat_buf.st_size; + printf("image size (c=%d): %zd\n", c, L); + buffer = malloc(L); + if(!buffer) return; + if(L != (size_t)read(fd, buffer, L)){printf("err\n"); goto ret;} + b64 = base64_encode(buffer, L, &W); + L = W; free(buffer); + buffer = malloc(L+LWS_SEND_BUFFER_PRE_PADDING+LWS_SEND_BUFFER_POST_PADDING); + if(!buffer){printf("malloc\n"); free(b64); return;} + memcpy(buffer+LWS_SEND_BUFFER_PRE_PADDING,b64, L); + free(b64); + p = buffer + LWS_SEND_BUFFER_PRE_PADDING; + W = 0; + do{ + p += W; + L -= W; + W = libwebsocket_write(wsi, p, L, LWS_WRITE_TEXT); + printf("write: %zd (L=%zd)\n", W, L); + }while(W>0 && W!=L); + if(W<=0) printf("<0\n"); + //W = libwebsocket_write(wsi, p, L, LWS_WRITE_BINARY); + if(L != W){ + printf("err: needed: %zd, writed %zd\n", L, W); + //lwsl_err("Can't write image to socket"); + } + ret: + free(buffer); + close(fd); + } + //struct lws_tokens *tok = (struct lws_tokens *) user; + switch (reason) { + case LWS_CALLBACK_ESTABLISHED: + printf("New Connection\n"); + send_image(); + libwebsocket_callback_on_writable(context, wsi); + break; + case LWS_CALLBACK_SERVER_WRITEABLE: + libwebsocket_callback_on_writable(context, wsi); + break; + case LWS_CALLBACK_RECEIVE: + send_image(); + break; + case LWS_CALLBACK_FILTER_NETWORK_CONNECTION: + libwebsockets_get_peer_addresses(context, wsi, (int)(long)in, + client_name, 127, client_ip, 127); + printf("Received network connection from %s (%s)\n", + client_name, client_ip); + break; + case LWS_CALLBACK_FILTER_PROTOCOL_CONNECTION: + printf("Client asks for %s\n", msg); + dump_handshake_info(wsi); + break; + case LWS_CALLBACK_CLOSED: + printf("Client disconnected\n"); + break; + default: + break; + } + + return 0; +} + +//**************************************************************************// +/* list of supported protocols and callbacks */ +//**************************************************************************// +static struct libwebsocket_protocols protocols[] = { + { + "image-protocol", + improto_callback, + 0, + 0, + 0, NULL, 0 + }, + { + "XY-protocol", // name + my_protocol_callback, // callback + sizeof(per_session_data), // per_session_data_size + 10, // max frame size / rx buffer + 0, NULL, 0 + }, + { NULL, NULL, 0, 0, 0, NULL, 0} /* terminator */ +}; + +//**************************************************************************// +void sighandler(_U_ int sig){ + close(comfd); + force_exit = 1; +} + +void *websock_thread(_U_ void *buf){ + struct libwebsocket_context *context; + int n = 0; + int opts = 0; + const char *iface = NULL; + int syslog_options = LOG_PID | LOG_PERROR; + //unsigned int oldus = 0; + struct lws_context_creation_info info; + int debug_level = 7; + + if(pthread_setcancelstate(PTHREAD_CANCEL_ENABLE, NULL)){ + force_exit = 1; + return NULL; + } + + memset(&info, 0, sizeof info); + info.port = 9999; + + /* we will only try to log things according to our debug_level */ + setlogmask(LOG_UPTO (LOG_DEBUG)); + openlog("lwsts", syslog_options, LOG_DAEMON); + + /* tell the library what debug level to emit and to send it to syslog */ + lws_set_log_level(debug_level, lwsl_emit_syslog); + + info.iface = iface; + info.protocols = protocols; + info.extensions = libwebsocket_get_internal_extensions(); + info.ssl_cert_filepath = NULL; + info.ssl_private_key_filepath = NULL; + + info.gid = -1; + info.uid = -1; + info.options = opts; + + context = libwebsocket_create_context(&info); + if (context == NULL){ + lwsl_err("libwebsocket init failed\n"); + force_exit = 1; + return NULL; + } + + while(n >= 0 && !force_exit){ + n = libwebsocket_service(context, 500); + }//while n>=0 + libwebsocket_context_destroy(context); + lwsl_notice("libwebsockets-test-server exited cleanly\n"); + closelog(); + return NULL; +} + +//**************************************************************************// +int main(_U_ int argc, _U_ char **argv){ + pthread_t w_thread; + //size_t L; + + signal(SIGINT, sighandler); +/* + if(argc == 2) comdev = argv[1]; + + printf("\nOpen port %s...\n", comdev); + if ((comfd = open(comdev,O_RDWR|O_NOCTTY|O_NONBLOCK)) < 0){ + fprintf(stderr,"Can't use port %s\n",comdev); + return 1; + } + + process_buf("W"); // step: 1/16 +*/ + pthread_create(&w_thread, NULL, websock_thread, NULL); + + while(!force_exit){ + /* if((L = read_tty())){ + printf("got %zd bytes from tty:\n", L); + check_tty_sig(L); + }*/ +/* + pthread_mutex_lock(&command_mutex); + if(data_in_buf) process_buf(cmd_buf); + data_in_buf = 0; + pthread_mutex_unlock(&command_mutex); +*/ + } + pthread_join(w_thread, NULL); // wait for closing of libsockets thread + return 0; +}//main diff --git a/websockets/test.html b/websockets/test.html new file mode 100644 index 0000000..95a66ef --- /dev/null +++ b/websockets/test.html @@ -0,0 +1,175 @@ + + + + A test + + + +
+ + + + + + + + + + + + +
+
+ Speed: + 50 +
+

+
No connection
+
+
+
+ + + + diff --git a/websockets/testlog2.c b/websockets/testlog2.c new file mode 100644 index 0000000..533aedf --- /dev/null +++ b/websockets/testlog2.c @@ -0,0 +1,15 @@ +#include +#include + +static inline uint32_t log_2(const uint32_t x){ + if(x == 0) return 0; + return (31 - __builtin_clz (x)); +} + +int main(){ + uint32_t i; + for(i = 0; i < 150; i++){ + printf("log2(%u) = %u\n", i, log_2(i)); + } + return 0; +}