1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
|
/*
* r3PCR Teensy Controller Code
*
*
* Copyright (C) 2013 Bernhard Tittelbach <xro@realraum.at>
*
* This code 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 3 of the License, or
* any later version.
*
* This code 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 spreadspace avr utils. If not, see <http://www.gnu.org/licenses/>.
*/
#include <stdlib.h>
#include <stdio.h>
#include "cmd_queue.h"
//static assert:
//~ static int SDFLKJ[sizeof(int) == sizeof(int16_t)] = { 0 };
#define CMDQ_INPUT_LIST_LEN 2
typedef struct {
uint8_t num_args;
void* function_ptr;
int16_t input_list[CMDQ_INPUT_LIST_LEN];
uint8_t num_args_read;
} cmd_queue_item;
#define CMQ_QUEUE_LENGTH 4
cmd_queue_item cmd_queue_[CMQ_QUEUE_LENGTH];
uint8_t cmd_queue_todo_pos_ = 0;
uint8_t cmd_queue_fillargs_pos_ = 0;
uint8_t cmd_queue_next_here_ = 0;
#define CMDQ_READBUFFER_LEN 20
char cmdq_readbuffer_[CMDQ_READBUFFER_LEN];
uint8_t cmdq_readbuffer_pos_ = 0;
inline void cmdq_finishReadingArgument(void)
{
cmdq_readbuffer_[cmdq_readbuffer_pos_] = '\0';
cmdq_readbuffer_pos_ = 0;
cmd_queue_[cmd_queue_fillargs_pos_].input_list[cmd_queue_[cmd_queue_fillargs_pos_].num_args_read++] = atoi(cmdq_readbuffer_);
}
//call regularily if in state CQ_READING
uint8_t cmdq_addCharToArgumentBuffer(uint8_t c)
{
if ( cmd_queue_todo_pos_ == cmd_queue_next_here_ )
return 1; //nothing to do with us, since no cmd's queued
if (cmd_queue_[cmd_queue_fillargs_pos_].num_args_read >= CMDQ_INPUT_LIST_LEN)
return 1; //nothing to do with us
if (cmd_queue_[cmd_queue_fillargs_pos_].num_args_read >= cmd_queue_[cmd_queue_fillargs_pos_].num_args )
return 1; //nothing to do with us
//if input terminated by \n or \r then addArgument
if (c == '\n' || c == '\r' || c == ',' || c == 0x1B)
{
cmdq_finishReadingArgument();
return 0;
} else { //continue reading
cmdq_readbuffer_[cmdq_readbuffer_pos_++] = (char) c;
}
//if numlen +1 of readbuffer reached, addArgument as well (leave one char free to terminate with \0)
if (cmdq_readbuffer_pos_ +1 >= CMDQ_READBUFFER_LEN)
{
cmdq_finishReadingArgument();
}
return 0;
}
void cmdq_queueCmdWithNumArgs(void* fptr, uint8_t num_args)
{
if (num_args > CMDQ_INPUT_LIST_LEN)
{
printf("{\"cmd_ok\":false,\"error\":\"max args == %d\"}\r\n", CMDQ_INPUT_LIST_LEN);
return; //can't do that Would hang cmdq
}
if ((cmd_queue_next_here_ +1) % CMQ_QUEUE_LENGTH == cmd_queue_todo_pos_) //for this check REQUIRED: CMQ_QUEUE_LENGTH > 2
cmdq_doWork(); //no more space in queue -> execute now !
cmd_queue_[cmd_queue_next_here_].num_args = num_args;
cmd_queue_[cmd_queue_next_here_].function_ptr = fptr;
cmd_queue_[cmd_queue_next_here_].num_args_read = 0;
cmd_queue_fillargs_pos_ = cmd_queue_next_here_;
cmd_queue_next_here_++;
cmd_queue_next_here_ %= CMQ_QUEUE_LENGTH;
}
void cmdq_doWork(void)
{
while ( cmd_queue_todo_pos_ != cmd_queue_next_here_ )
{
// is num_args_read_ now equal to num args we expect ?
if (cmd_queue_[cmd_queue_todo_pos_].num_args_read != cmd_queue_[cmd_queue_todo_pos_].num_args )
break;
if (cmd_queue_[cmd_queue_todo_pos_].function_ptr == 0 )
break;
switch (cmd_queue_[cmd_queue_todo_pos_].num_args)
{
case 0:
((void(*)(void)) cmd_queue_[cmd_queue_todo_pos_].function_ptr)();
break;
case 1:
((void(*)(int16_t)) cmd_queue_[cmd_queue_todo_pos_].function_ptr)(cmd_queue_[cmd_queue_todo_pos_].input_list[0]);
break;
case 2:
((void(*)(int16_t,int16_t)) cmd_queue_[cmd_queue_todo_pos_].function_ptr)(cmd_queue_[cmd_queue_todo_pos_].input_list[0], cmd_queue_[cmd_queue_todo_pos_].input_list[1]);
break;
}
cmd_queue_[cmd_queue_todo_pos_].num_args_read = 0;
cmd_queue_[cmd_queue_todo_pos_].num_args = 0;
cmd_queue_[cmd_queue_todo_pos_].function_ptr = 0;
cmd_queue_todo_pos_++;
cmd_queue_todo_pos_ %= CMQ_QUEUE_LENGTH;
printf("{\"cmd_ok\":true}\r\n");
}
}
|
