Statistics
| Branch: | Tag: | Revision:

amiro-os / os / core / src / aos_ssm.c @ e545e620

History | View | Annotate | Download (8.45 KB)

1
/*
2
AMiRo-OS is an operating system designed for the Autonomous Mini Robot (AMiRo) platform.
3
Copyright (C) 2016..2018  Thomas Schöpping et al.
4

5
This program is free software: you can redistribute it and/or modify
6
it under the terms of the GNU General Public License as published by
7
the Free Software Foundation, either version 3 of the License, or
8
(at your option) any later version.
9

10
This program is distributed in the hope that it will be useful,
11
but WITHOUT ANY WARRANTY; without even the implied warranty of
12
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
13
GNU General Public License for more details.
14

15
You should have received a copy of the GNU General Public License
16
along with this program.  If not, see <http://www.gnu.org/licenses/>.
17
*/
18

    
19
#include <aos_ssm.h>
20

    
21
/**
22
 * @brief   Sequential Stream Multiplexer write.
23
 *
24
 * @param[in] ip      pointer to a @p SequentialStreamMux object
25
 * @param[in] bp      pointer to the data buffer
26
 * @param[in] n       the maximum amount of bytes to be transferred
27
 *
28
 * @return            The minimum number of bytes written to one of the BaseSequentialStreams.
29
 *                    If the return value is lower than n, one or multiple (or even all) streams could not write all bytes as requested.
30
 */
31
static size_t write(void *ip, const uint8_t *bp, size_t n)
32
{
33
  // initialize the number of written bytes
34
  size_t outbytes = ~0;
35
  // iterate over all output streams associated with te mux
36
  ssm_output_t* out = ((SequentialStreamMux*)ip)->outputs;
37
  while (out != NULL) {
38
    // if the output is enabled
39
    if (out->flags | SSM_OUTPUT_FLAG_ENABLED) {
40
      // write to the stream
41
      register const size_t ob = streamWrite(out->stream, bp, n);
42
      // retrieve the minimal number of bytes written across all streams
43
      outbytes = (ob < outbytes) ? ob : outbytes;
44
    }
45
    // iterate to the next stream
46
    out = out->next;
47
  }
48

    
49
  return (outbytes == (size_t)~0) ? 0 : outbytes;
50
}
51

    
52
/**
53
 * @brief   Sequential Stream Multiplexer read.
54
 *
55
 * @param[in]  ip     pointer to a @p SequentialStreamMux object
56
 * @param[out] bp     pointer to the data buffer
57
 * @param[in]  n      the maximum amount of data to be transferred
58
 *
59
 * @return            The number of bytes transferred.
60
 */
61
static size_t read(void *ip, uint8_t *bp, size_t n)
62
{
63
  // check whether a input stream is set
64
  if ( ((SequentialStreamMux*)ip)->input != NULL ) {
65
    // read from the input stream
66
    return streamRead(((SequentialStreamMux*)ip)->input, bp, n);
67
  } else {
68
    // return zero since
69
    return 0;
70
  }
71
}
72

    
73
/**
74
 * @brief   Sequential Stream Multiplexer blocking byte write
75
 *
76
 * @param[in] ip      pointer to a @p SequentialStreamMux object
77
 * @param[in] b       the byte value to be written
78
 *
79
 * @return            The operation status.
80
 *                    The operation status is ORed from all enabled output streams.
81
 *                    The operation status of each output stream depends on its implementation.
82
 */
83
static msg_t put(void *ip, uint8_t b)
84
{
85
  // initialize the return value
86
  msg_t retval = MSG_OK;
87
  // iterate over all output streams associated with the mux
88
  ssm_output_t* out = ((SequentialStreamMux*)ip)->outputs;
89
  while (out != NULL) {
90
    // if the output is enabled
91
    if (out->flags | SSM_OUTPUT_FLAG_ENABLED) {
92
      // put the given byte on the stream
93
      retval |= streamPut(out->stream, b);
94
    }
95
    // iterate to rhe next stream
96
    out = out->next;
97
  }
98

    
99
  return retval;
100
}
101

    
102
/**
103
 * @brief   Sequential Stream Multiplexer blocking byte read
104
 *
105
 * @param[in] ip      pointer to the @p SequentialStreamMux object
106
 *
107
 * @return            A byte value from the input.
108
 */
109
static msg_t get(void *ip)
110
{
111
  // check whether an input is set
112
  if ( ((SequentialStreamMux*)ip)->input != NULL) {
113
    // get a byte from the input
114
    return streamGet(((SequentialStreamMux*)ip)->input);
115
  } else {
116
    // return an error
117
    return MSG_RESET;
118
  }
119
}
120

    
121
static const struct SequentialStreamMuxVMT vmt = {
122
  write, read, put, get
123
};
124

    
125
/**
126
 * @brief   Initializes a @p SequentialStreamMux object
127
 *
128
 * @param[in] ssmp    pointer to the @p SequentialStreamMux object
129
 */
130
void ssmObjectInit(SequentialStreamMux *ssmp)
131
{
132
  aosDbgCheck(ssmp != NULL);
133

    
134
  // initialize object variables
135
  ssmp->vmt = &vmt;
136
  ssmp->outputs = NULL;
137
  ssmp->input = NULL;
138

    
139
  return;
140
}
141

    
142
/**
143
 * @brief   Initializes a @p ssm_output_t object
144
 *
145
 * @param[in] op    pointer to the @p ssm_output_t object
146
 */
147
void ssmOutputInit(ssm_output_t* op, BaseSequentialStream* stream)
148
{
149
  aosDbgCheck(op != NULL);
150

    
151
  // initialize object variables
152
  op->stream = stream;
153
  op->next = NULL;
154
  op->flags = 0;
155

    
156
  return;
157
}
158

    
159
/**
160
 * @brief   Adds an output to the SSM object.
161
 * @note    New outputs are always prepended to the list.
162
 *
163
 * @param[in] ssmp    pointer to the @p SequentialStreamMux object to be modified
164
 * @param[in] output  pointer to the @p ssm_output_t object to be inserted
165
 */
166
void ssmAddOutput(SequentialStreamMux* ssmp, ssm_output_t* output)
167
{
168
  aosDbgCheck(ssmp != NULL);
169
  aosDbgCheck(output != NULL);
170
  aosDbgCheck(output->next == NULL && output->flags ^ SSM_OUTPUT_FLAG_ATTACHED);
171

    
172
  // prepend output to the list
173
  output->flags = SSM_OUTPUT_FLAG_ATTACHED;
174
  output->next = ssmp->outputs;
175
  ssmp->outputs = output;
176

    
177
  return;
178
}
179

    
180
/**
181
 * @brief   Removes an output from the SSM object.
182
 *
183
 * @param[in] ssmp      pointer to the @p SequentialStreamMux object to be modified
184
 * @param[in] stream    pointer to the @p BaseSequentialStream to remove
185
 * @param[out] removed  pointer to the @p ssm_output_t that was removed, or NULL
186
 *
187
 * @return              Error code indicating any errors or warnings.
188
 * @retval AOS_SUCCESS  the stream was removed sucessfully
189
 * @retval AOS_ERROR    the stream was not found in the list
190
 */
191
aos_status_t ssmRemoveOutput(SequentialStreamMux* ssmp, BaseSequentialStream* stream, ssm_output_t** removed)
192
{
193
  aosDbgCheck(ssmp != NULL);
194
  aosDbgCheck(stream != NULL);
195

    
196
  // local varibales
197
  ssm_output_t* prev = NULL;
198
  ssm_output_t* curr = ssmp->outputs;
199

    
200
  // iterate through the list and search for the specified stream
201
  while (curr != NULL) {
202
    // if the stream was found, remove it
203
    if (curr->stream == stream) {
204
      // special case: the first stream matches
205
      if (prev == NULL) {
206
        ssmp->outputs->next = curr->next;
207
      } else {
208
        prev->next = curr->next;
209
      }
210
      curr->next = NULL;
211
      curr->flags &= ~SSM_OUTPUT_FLAG_ATTACHED;
212
      // set the optional output argument
213
      if (removed != NULL) {
214
        *removed = curr;
215
      }
216
      return AOS_SUCCESS;
217
    }
218
    // iterate through to the next item
219
    else {
220
      prev = curr;
221
      curr = curr->next;
222
    }
223
  }
224

    
225
  // if the stream was not found, return an error
226
  return AOS_ERROR;
227
}
228

    
229
/**
230
 * @brief   Enable an associated stream
231
 *
232
 * @param[in] ssmp    pointer to the @p SequentialStreamMux object
233
 * @param[in] stream  pointer to the @p BaseSequentialStream to enable
234
 *
235
 * @return            The operation status.
236
 * @retval AOS_SUCCESS  if the operation succeeded.
237
 * @retval AOS_ERROR    if the specified stream is not associated to the @p SequentialStreamMux.
238
 */
239
aos_status_t ssmEnableOutput(SequentialStreamMux *ssmp, BaseSequentialStream *stream)
240
{
241
  aosDbgCheck(ssmp != NULL);
242
  aosDbgCheck(stream != NULL);
243

    
244
  // iterate over all output streams associated to the given mux
245
  ssm_output_t* out = ssmp->outputs;
246
  while (out != NULL) {
247
    // if the output was found, activate the enabled flag
248
    if (out->stream == stream) {
249
      out->flags |= SSM_OUTPUT_FLAG_ENABLED;
250
      return AOS_SUCCESS;
251
    } else {
252
      out = out->next;
253
    }
254
  }
255
  // error if the stream is not associated with the mux
256
  return AOS_ERROR;
257
}
258

    
259
/**
260
 * @brief   Disable an associated stream
261
 *
262
 * @param[in] ssmp    pointer to the @p SequentialStreamMux or derived class
263
 * @param[in] stream  pointer to the stream to disable
264
 *
265
 *
266
 * @return            The operation status.
267
 * @retval AOS_OK     if the operation succeeded.
268
 * @retval AOS_ERROR  if the specified stream is not associated to the @p SequentialStreamMux.
269
 */
270
aos_status_t ssmDisableOutput(SequentialStreamMux *ssmp, BaseSequentialStream *stream)
271
{
272
  aosDbgCheck(ssmp != NULL);
273
  aosDbgCheck(stream != NULL);
274

    
275
  // iterate over all output streams associated to the given mux
276
  ssm_output_t* out = ssmp->outputs;
277
  while (out != NULL) {
278
    // if the output was found, activate the enabled flag
279
    if (out->stream == stream) {
280
      out->flags &= ~SSM_OUTPUT_FLAG_ENABLED;
281
      return AOS_SUCCESS;
282
    } else {
283
      out = out->next;
284
    }
285
  }
286
  // error if the stream is not associated with the mux
287
  return AOS_ERROR;
288
}