1 /*
2 * command_handler.c
3 *
4 * Ctools Profiler Server Implementation
5 *
6 * Copyright (C) 2013 Texas Instruments Incorporated - http://www.ti.com/
7 *
8 *
9 * Redistribution and use in source and binary forms, with or without
10 * modification, are permitted provided that the following conditions
11 * are met:
12 *
13 * Redistributions of source code must retain the above copyright
14 * notice, this list of conditions and the following disclaimer.
15 *
16 * Redistributions in binary form must reproduce the above copyright
17 * notice, this list of conditions and the following disclaimer in the
18 * documentation and/or other materials provided with the
19 * distribution.
20 *
21 * Neither the name of Texas Instruments Incorporated nor the names of
22 * its contributors may be used to endorse or promote products derived
23 * from this software without specific prior written permission.
24 *
25 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
26 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
27 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
28 * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
29 * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
30 * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
31 * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
32 * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
33 * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
34 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
35 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
36 *
37 */
38 #include <stdio.h>
39 #include <stdlib.h>
40 #include <stdbool.h>
41 #include <string.h>
42 #include <stdint.h>
43 #include "error_handler.h"
44 #include "logging.h"
45 #include "socket.h"
46 #include "remote_commands.h"
48 /*****************************************************************************
49 * Internal Function Prototypes
50 * - See Private Function section below for implementations )
51 *****************************************************************************/
52 static inline uint8_t calculate_checksum( uint8_t *buf, unsigned int len );
53 static inline uint8_t int_to_hexdigit(unsigned val);
54 static inline void send_response(uint8_t * response);
55 static void command_dispatch(char * cmd, int cmd_len, char * response, size_t rsp_size);
56 static void command_clr_buf(uint8_t * pbuf);
58 static void get_memory_arguments(char * cbuf, uint32_t * addr, size_t * len, char ** data);
59 static void process_memory_write(char * cbuf, char * rbuf, size_t rsp_size);
60 static void process_memory_read(char * cbuf, char * rbuf, size_t rsp_size);
61 static void process_remote(char * cbuf, char * rbuf, size_t rsp_size);
62 static void process_disable_ack(char * cbuf, char * rbuf, size_t rsp_size);
64 /*****************************************************************************
65 * Remote Serial Protocol (RSP) Definitions
66 * - RSP commands consists of remote commands and target commands (memory read
67 * and memory write.
68 *
69 *****************************************************************************/
71 /* RSP Commands */
72 typedef enum {
73 READ_MEMORY_CMD,
74 WRITE_MEMORY_CMD,
75 REMOTE_CMD,
76 DISABLE_ACK_CMD
77 } rsp_cmd_t;
79 typedef void (* proc_func_t)(char * cbuf, char * rbuf, size_t rbuf_size);
81 struct rsp_command_table_t {
82 char * cmd;
83 proc_func_t process;
84 };
86 static struct rsp_command_table_t rsp_command_table[] = {
87 [READ_MEMORY_CMD] = {"m", process_memory_read},
88 [WRITE_MEMORY_CMD] = {"M", process_memory_write},
89 [REMOTE_CMD] = {"qRcmd,", process_remote},
90 [DISABLE_ACK_CMD] = {"QStartNoAckMode", process_disable_ack}
91 };
93 /* RSP remote command table */
94 typedef void (* rmt_func_t)(int argc, char *argv[], char * rbuf, size_t rsp_size);
96 struct remote_command_table_t {
97 char * cmd;
98 rmt_func_t process;
99 };
101 /* The commands from this table are provided from remote_commands.h */
102 static struct remote_command_table_t remote_command_table[] = {
103 {"set", set_command_handler},
104 {"mmap", mmap_command_handler},
105 {"ummap", ummap_command_handler},
106 {"version", version_command_handler},
107 {"arm", modcntl_add_command_handler},
108 {"disarm", modcntl_add_command_handler},
109 {"rm_arm", modcntl_remove_command_handler},
110 {"rm_disarm", modcntl_remove_command_handler},
111 {"start", start_command_handler},
112 {"end", end_command_handler},
113 {"status", status_command_handler}
114 };
116 /* RSP constants */
117 static bool rsp_ack_enabled = true;
118 static const char *ACK = "+";
119 static const char *NACK = "-";
120 static const char *rsp_msg_ok = "OK";
122 /* RSP State */
123 typedef enum {AWAITING_COMMAND, AWAITING_ACK} command_state_t;
124 static command_state_t command_state = AWAITING_COMMAND;
126 /* RSP buffers*/
127 #define MAX_COMMAND_SIZE 256
128 static uint8_t cmd_buf[MAX_COMMAND_SIZE];
129 static uint8_t rsp_buf[MAX_COMMAND_SIZE];
131 /*****************************************************************************
132 * Statics
133 *
134 *****************************************************************************/
135 static int cmd_socket;
137 /*****************************************************************************
138 * Public Functions
139 *
140 *****************************************************************************/
141 /*****************************************************************************
142 * command_init()
143 *
144 * Initialize the client command handler with a socket.
145 *
146 *****************************************************************************/
147 void command_init(int socket)
148 {
149 LOGFUNC();
150 cmd_socket = socket;
151 command_clr_buf(cmd_buf);
153 command_state = AWAITING_COMMAND;
154 rsp_ack_enabled = true;
155 }
157 /*****************************************************************************
158 * command_process()
159 *
160 * This is where all client commands are received and executed.
161 *
162 * - RSP packet definition is:
163 * $packet-data#checksum
164 *
165 * Where checksum is a 2 digit module 256 sum (in hex) of the packet-data.
166 * Initial response from the server is “+/-“ where “+” indicates the packet-data
167 * transmitted successfully, and “-“ indicates the packet-data needs to be resent.
168 * All values are sent in HEX format without leading “0x”.
169 *
170 * Examples:
171 * $Maddr,length:data#checksum - Write length bytes of memory starting at addr.
172 * $maddr,length#checksum - Read length bytes of memory starting at addr.
173 * $qRcmd,mmap addr,size#checksum - Map memory block at addr of size in bytes.
174 *
175 *****************************************************************************/
176 void command_process()
177 {
178 uint8_t * pbuf = cmd_buf;
179 uint8_t * cksum_marker;
180 int retry;
181 size_t cmd_len;
183 LOGFUNC();
185 /* Provide size of receive buffer as MAX_COMMAND_SIZE -1
186 * because we want room for at least one NULL character in the buffer.
187 */
188 retry = socket_recv_data(cmd_socket, cmd_buf, MAX_COMMAND_SIZE - 1, 0,
189 &cmd_len);
191 if ((retry) || (cmd_len == 0)) {
192 return;
193 }
195 LOGMSG1("%s:Received msg length %d, message is:%s, command state is %d",
196 __func__, cmd_len, cmd_buf, command_state);
198 /* Note: Since the server is non-blocking the ACK (if AWAITING_ACK set)
199 * and the next command can be combined into a single message.
200 */
201 if (command_state == AWAITING_ACK) {
202 if (cmd_buf[0] == '+') {
203 command_state = AWAITING_COMMAND;
204 LOGMSG1("%s:Processed ACK", __func__);
206 if (cmd_len > 1) {
207 pbuf++;
208 } else {
209 command_clr_buf(cmd_buf);
210 command_clr_buf(rsp_buf);
211 return;
212 }
214 }
215 if (cmd_buf[0] == '-') {
216 send_response(rsp_buf);
217 LOGMSG1("%s:Processed NACK", __func__);
218 return;
219 }
220 }
222 /* First character of RSP commands must be $ */
223 if (('$' != pbuf[0]) || !( cksum_marker = (uint8_t *)strchr((char *)pbuf, '#'))) {
224 err_handler(ERR_TYPE_LOCAL, ERR_RSP_INVALID);
225 }
227 /* get past first character and determine the actual command length */
228 pbuf++;
229 cmd_len = cksum_marker - pbuf;
231 /* Validate checksum */
232 if (rsp_ack_enabled == true) {
233 uint8_t ck_sum = calculate_checksum( pbuf, cmd_len );
234 uint8_t c1 = pbuf[cmd_len+1];
235 uint8_t c2 = pbuf[cmd_len+2];
237 if ((c1 != int_to_hexdigit(ck_sum >> 4)) ||
238 (c2 != int_to_hexdigit(ck_sum & 0xf ))) {
239 /* Simply means resend command*/
240 send_response((uint8_t *)NACK);
241 return;
242 }
243 else {
244 send_response((uint8_t *)ACK);
245 }
247 }
249 /* Replace the # with 0 so it looks like the end of the string */
250 *cksum_marker = 0;
252 LOGMSG1("%s:Received command from client:%s", __func__, pbuf);
254 /* Execute the command */
255 {
256 uint8_t * rsp_p = rsp_buf;
257 uint8_t ck_sum = 0;
258 int rsp_len;
260 command_clr_buf(rsp_buf);
261 rsp_p[0] = '$';
262 rsp_p++;
264 command_dispatch((char *)pbuf, cmd_len, (char *)rsp_p, sizeof(rsp_buf)-1);
266 rsp_len = strlen((char *)rsp_p);
268 if (rsp_ack_enabled == true) {
269 ck_sum = calculate_checksum(rsp_p, rsp_len);
270 }
272 rsp_p[rsp_len] = '#';
273 rsp_p[rsp_len+1] = int_to_hexdigit(ck_sum >> 4);
274 rsp_p[rsp_len+2] = int_to_hexdigit(ck_sum & 0xf);
276 send_response(rsp_buf);
278 LOGMSG1("%s:Sent response to client:%s", __func__, rsp_buf);
280 if (rsp_ack_enabled == true) {
281 command_state = AWAITING_ACK;
282 } else {
283 command_clr_buf(cmd_buf);
284 }
285 }
287 }
289 /*****************************************************************************
290 * Private functions
291 *
292 *****************************************************************************/
293 /*****************************************************************************
294 * command_clr_buf()
295 *
296 * Fill the command buffer with the NULL character.
297 * Note: This is done so strchr will not have problems if a non-conforming rsp
298 * command is received.
299 *
300 *****************************************************************************/
301 void command_clr_buf(uint8_t * pbuf)
302 {
303 memset((char *)pbuf, '\0', MAX_COMMAND_SIZE);
304 }
306 /*****************************************************************************
307 * calculate_checksum()
308 *
309 * Calculate the RSP checksum
310 *
311 *****************************************************************************/
312 static inline uint8_t calculate_checksum( uint8_t *buf, unsigned int len )
313 {
314 unsigned checksum = 0;
315 while ( len-- > 0 ) {
316 checksum += *buf++;
317 }
318 return checksum & 0xFF;
319 }
321 /*****************************************************************************
322 * int_to_hexdigit()
323 *
324 * Convert a int digit to a hex ascii value.
325 *
326 *****************************************************************************/
327 static const uint8_t inttohex_table[] = {
328 '0', '1', '2', '3', '4', '5', '6', '7', '8', '9', 'a', 'b', 'c', 'd', 'e', 'f'
329 };
331 static inline uint8_t int_to_hexdigit(unsigned int val)
332 {
333 if ( val > 0xF) {
334 err_handler(ERR_TYPE_LOCAL, ERR_RSP_INVALID);
335 }
336 return inttohex_table[val];
337 }
339 /*****************************************************************************
340 * send_response()
341 *
342 * Once the command is processed this function is used to send the response
343 * back to the client.
344 *
345 *****************************************************************************/
346 static inline void send_response(uint8_t * response)
347 {
348 size_t send_offset = 0;
349 size_t retlen;
350 int retval;
352 /* The command socket is non-blocking so must check for EAGAIN */
353 do {
354 retval = socket_send_data(cmd_socket, response,
355 strlen((char *)response), send_offset, &retlen);
356 } while(retval);
357 }
359 /*****************************************************************************
360 * command_dispatch()
361 *
362 * - Search the rsp_command_table for a known cmd that matches the received cmd
363 * - Use the command index to execute the rsp command handler.
364 *
365 *****************************************************************************/
366 void command_dispatch(char * cmd, int cmd_len, char * response, size_t rsp_size)
367 {
368 int rsp_cmd_elements = sizeof(rsp_command_table)/sizeof(struct rsp_command_table_t);
369 int cmd_index;
370 char * pcmd = cmd;
372 LOGFUNC();
374 /* Search the rsp_command_table for a known cmd that matches the received cmd. */
375 for ( cmd_index = 0; cmd_index < rsp_cmd_elements; cmd_index++) {
377 char *rsp_cmd = strstr(cmd, rsp_command_table[cmd_index].cmd);
378 if (rsp_cmd == cmd) {
379 pcmd += strlen(rsp_command_table[cmd_index].cmd);
381 LOGMSG1("%s:RSP Command match found:%s", __func__,
382 rsp_command_table[cmd_index].cmd);
384 break;
385 }
386 }
388 if (cmd_index == rsp_cmd_elements) {
389 err_handler(ERR_TYPE_LOCAL, ERR_RSP_INVALID);
390 }
393 /* Process the command */
394 rsp_command_table[cmd_index].process(pcmd, response, rsp_size);
395 }
397 /*****************************************************************************
398 * get_memory_arguments()
399 *
400 * Convert ascii memory command arguments (addr, len, and data) and into
401 * values.
402 *
403 * Note - data is not converted to value in this function. It simply returns
404 * a char * to the data value if this is a write, else NULL is returned.
405 *
406 *****************************************************************************/
407 static void get_memory_arguments(char * cbuf, uint32_t * addr, size_t * len, char ** data)
408 {
409 char * tokins = ",:";
410 /* arg[0] used to hold the char * to the address */
411 /* arg[1] used to hold the char * to the length in bytes */
412 /* arg[2] used to hold a char * to the data if this is a write */
413 char * arg[3] = {NULL,NULL,NULL};
414 char addr_str[] = "0x00000000";
415 char len_str[] = "0x00000000";
416 int starting_digit;
418 int i = 0;
419 arg[i] = strtok(cbuf, tokins);
420 while (arg[i] != NULL) {
421 #if DEBUG
422 if ((strlen(arg[i]) == 0) || (strlen(arg[i]) > 8)) {
423 err_handler(ERR_TYPE_LOCAL, ERR_DEBUG);
424 }
425 #endif
426 arg[++i] = strtok(NULL, tokins);
427 /* Don't overflow the arg array */
428 if (i == 2) break;
429 }
431 #if DEBUG
432 if ( (arg[0] == NULL) || (arg[1] == NULL)) {
433 err_handler(ERR_TYPE_LOCAL, ERR_DEBUG);
434 }
436 #endif
438 starting_digit = 10 - strlen(arg[0]);
439 strcpy(&addr_str[starting_digit], arg[0]);
441 starting_digit = 10 - strlen(arg[1]);
442 strcpy(&len_str[starting_digit], arg[1]);
444 *addr = strtoul(addr_str, NULL, 16);
445 *len = strtoul(len_str, NULL, 16); /* in bytes */
446 *data = arg[2];
448 }
450 /*****************************************************************************
451 * process_memory_write()
452 *
453 * RSP target memory write command.
454 *
455 * Note - data is restricted to N 32-bit hex values.
456 *
457 *****************************************************************************/
458 static void process_memory_write(char * cbuf, char * rbuf, size_t rsp_size)
459 {
460 uint32_t addr;
461 size_t len;
462 char * data_str;
464 get_memory_arguments(cbuf, &addr, &len, &data_str);
465 #if DEBUG
466 if ((data_str == NULL) || (len % 4 != 0) || (addr & 0x3)) {
467 err_handler(ERR_TYPE_LOCAL, ERR_DEBUG);
468 }
469 #endif
471 /* Client must send 8 hex digits per word */
472 do {
473 /* Do a 32-bit write */
474 uint32_t data;
475 int offset = 0;
476 char word[] = "0x00000000"; /* data_str is in byte order 78563412 for 0x12345678*/
478 int num_digits = strlen(data_str + offset);
479 if (num_digits > 8) {
480 num_digits = 8;
481 }
483 for (int i = 0; i < num_digits; i += 2) {
484 word[8 - i] = data_str[offset + i];
485 word[9 - i] = data_str[offset + i + 1];
486 }
488 data = strtoul(word, NULL, 16);
490 remote_memory_write( addr, sizeof(uint32_t), &data);
492 offset += 8;
493 len -= 4;
495 } while (len != 0);
497 #if DEBUG
498 if (strlen(rsp_msg_ok) > rsp_size) {
499 err_handler(ERR_TYPE_LOCAL, ERR_DEBUG);
500 }
501 #endif
502 strcpy(rbuf, rsp_msg_ok);
504 }
506 /*****************************************************************************
507 * process_memory_read()
508 *
509 * RSP target memory read command.
510 *
511 * Note - data is restricted to N 32-bit hex values.
512 *
513 *****************************************************************************/
514 static void process_memory_read(char * cbuf, char * rbuf, size_t rsp_size)
515 {
516 uint32_t addr;
517 size_t len;
518 char * rsp = rbuf;
519 char * data_str;
520 #if DEBUG
521 int digit_cnt = 0;
522 #endif
524 get_memory_arguments(cbuf, &addr, &len, &data_str);
526 #if DEBUG
527 /* data_str only valid for write, not read */
528 if ((data_str != NULL) || (len % 4 != 0) || (addr & 0x3)) {
529 err_handler(ERR_TYPE_LOCAL, ERR_DEBUG);
530 }
531 #endif
533 /* Will send 8 hex digits per word */
534 do {
535 /* Do a 32-bit write */
536 uint32_t data, adj_data, mask;
537 int num_digits;
539 #if DEBUG
540 if (digit_cnt > rsp_size) {
541 err_handler(ERR_TYPE_LOCAL, ERR_DEBUG);
542 }
543 #endif
545 remote_memory_read(addr, sizeof(uint32_t), &data);
547 /* when len < 4 bytes suppress leading zeros */
548 if (len > 4) {
549 num_digits = 8;
550 } else {
551 if (data < 256) num_digits = 2;
552 else if ((data > 255) && (data < 65536)) num_digits = 4;
553 else if ((data > 65535) && ( data < 16777216)) num_digits = 6;
554 else if (data > 16777215) num_digits = 8;
555 }
557 for (int i = 0; i < num_digits; i += 2) {
558 adj_data = data >> (i * 4);
559 mask = 0xf0;
560 *rsp++ = int_to_hexdigit((adj_data & mask) >> 4);
561 *rsp++ = int_to_hexdigit(adj_data & (mask >> 4));
562 }
564 #if DEBUG
565 digit_cnt += num_digits;
566 #endif
567 len -= sizeof(uint32_t);
569 } while (len != 0);
571 }
573 /*****************************************************************************
574 * process_remote()
575 *
576 * Process remote RSP commands
577 *
578 * - Search the remote_command_table for a known cmd that matches the received cmd.
579 * - Tokenize the remote command into argv argc pair.
580 * - Execute the command from the remote command table.
581 *
582 *****************************************************************************/
583 static void process_remote(char * cbuf, char * rbuf, size_t rsp_size)
584 {
586 int remote_element_cnt = sizeof(remote_command_table)/sizeof(struct remote_command_table_t);
587 int op_index;
589 LOGFUNC();
591 /* Search the remote_command_table for a known cmd that matches the received cmd */
592 for ( op_index = 0; op_index < remote_element_cnt; op_index++) {
593 /* Search the cmd string for the rsp command prefix */
594 char *rsp_cmd = strstr(cbuf, remote_command_table[op_index].cmd);
596 if (rsp_cmd == cbuf) {
597 LOGMSG1("%s:Remote command match found:%s", __func__,
598 remote_command_table[op_index].cmd);
599 break;
600 }
601 }
603 if (op_index == remote_element_cnt) {
604 err_handler(ERR_TYPE_LOCAL, ERR_RSP_INVALID);
605 }
607 /* Tokenize the remote command into argv argc pair. */
608 {
609 #define MAX_REMOTE_ARGS 6
610 char * argv[MAX_REMOTE_ARGS];
611 int argc = 0;
612 char * tokins = " ,"; /* Space and comma */
614 argv[argc] = strtok(cbuf, tokins);
615 while (argv[argc]) {
616 argc++;
617 #if DEBUG
618 if ( argc == MAX_REMOTE_ARGS) {
619 err_handler(ERR_TYPE_LOCAL, ERR_DEBUG);
620 }
621 #endif
622 argv[argc] = strtok(NULL, tokins);
623 }
625 /* Process the command */
627 IF_LOGGING_ENABLED {
628 for (int i = 0; i < argc; i++) {
629 LOGMSG1("%s: argv[%d] is:%s", __func__, i, argv[i]);
630 }
631 }
633 remote_command_table[op_index].process(argc, argv, rbuf, rsp_size);
635 LOGMSG1("%s:Remote command complete, responding with:%s", __func__, rbuf);
637 }
639 }
641 static void process_disable_ack(char * cbuf, char * rbuf, size_t rsp_size)
642 {
643 rsp_ack_enabled = false;
645 #if DEBUG
646 if (strlen(rsp_msg_ok) > rsp_size) {
647 err_handler(ERR_TYPE_LOCAL, ERR_DEBUG);
648 }
649 #endif
650 strcpy(rbuf, rsp_msg_ok);
652 }