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
128
129
130
|
/*
* gensio - A library for abstracting stream I/O
* Copyright (C) 2018 Corey Minyard <minyard@acm.org>
*
* SPDX-License-Identifier: LGPL-2.1-only
*/
#include "config.h"
#include <stdlib.h>
#include <string.h>
#include <gensio/gensio_buffer.h>
#include <gensio/gensio_err.h>
static int
do_write(gensio_buffer_do_write tdo_write, void *cb_data,
void *buf, unsigned int buflen, unsigned int *written)
{
int err = 0;
unsigned int write_count;
err = tdo_write(cb_data, buf, buflen, &write_count);
if (!err)
*written = write_count;
return err;
}
int
gensio_buffer_write(gensio_buffer_do_write tdo_write, void *cb_data,
struct gensio_buffer *buf)
{
int err;
unsigned int write_count;
unsigned int towrite1;
unsigned int towrite2 = 0;
if (buf->pos + buf->cursize > buf->maxsize) {
towrite1 = buf->maxsize - buf->pos;
towrite2 = buf->cursize - towrite1;
} else {
towrite1 = buf->cursize;
}
if (towrite1 > 0) {
err = do_write(tdo_write, cb_data,
buf->buf + buf->pos, towrite1, &write_count);
if (err)
return err;
buf->pos += write_count;
buf->cursize -= write_count;
if (write_count < towrite1)
return 0;
}
if (towrite2 > 0) {
/* We wrapped */
buf->pos = 0;
err = do_write(tdo_write, cb_data, buf->buf, towrite2, &write_count);
if (err)
return err;
buf->pos += write_count;
buf->cursize -= write_count;
}
return 0;
}
unsigned int
gensio_buffer_output(struct gensio_buffer *buf,
const unsigned char *data, unsigned int len)
{
unsigned int end;
if (gensio_buffer_left(buf) < len)
len = gensio_buffer_left(buf);
end = buf->pos + buf->cursize;
if (end > buf->maxsize)
end -= buf->maxsize;
if (end + len > buf->maxsize) {
unsigned int availend = buf->maxsize - end;
memcpy(buf->buf + end, data, availend);
buf->cursize += availend;
end = 0;
len -= availend;
data += availend;
}
memcpy(buf->buf + end, data, len);
buf->cursize += len;
return len;
}
unsigned int
gensio_buffer_outchar(struct gensio_buffer *buf, unsigned char data)
{
unsigned int end;
if (gensio_buffer_left(buf) < 1)
return 0;
end = buf->pos + buf->cursize;
if (end >= buf->maxsize)
end -= buf->maxsize;
buf->buf[end] = data;
buf->cursize += 1;
return 1;
}
int
gensio_buffer_init(struct gensio_buffer *buf,
unsigned char *data, unsigned int datasize)
{
if (data) {
buf->buf = data;
} else {
buf->buf = malloc(datasize);
if (!buf->buf)
return GE_NOMEM;
}
buf->maxsize = datasize;
buf->cursize = 0;
buf->pos = 0;
return 0;
}
|
