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#define _DEFAULT_SOURCE
27#include <stdint.h>
28#include <stdlib.h>
29#include <stdio.h>
30#include <stddef.h>
31#include <assert.h>
32#include "hw/remote-port-proto.h"
33
34#undef MIN
35#define MIN(x, y) (x < y ? x : y)
36
37#if defined(__linux__)
38# include <endian.h>
39#elif defined(__FreeBSD__) || defined(__NetBSD__)
40# include <sys/endian.h>
41#elif defined(__OpenBSD__)
42# include <sys/types.h>
43# define be16toh(x) betoh16(x)
44# define be32toh(x) betoh32(x)
45# define be64toh(x) betoh64(x)
46#elif defined(__WIN32)
47
48# define htobe64(x) _byteswap_uint64(x)
49# define htobe32(x) _byteswap_ulong(x)
50# define htobe16(x) _byteswap_ushort(x)
51
52# define be64toh(x) _byteswap_uint64(x)
53# define be32toh(x) _byteswap_ulong(x)
54# define be16toh(x) _byteswap_ushort(x)
55#endif
56
57
58#ifndef htobe64
59# include <byteswap.h>
60
61# if __BYTE_ORDER == __LITTLE_ENDIAN
62# define htobe64(x) bswap_64(x)
63# define htobe32(x) bswap_32(x)
64# define htobe16(x) bswap_16(x)
65
66# define be64toh(x) bswap_64(x)
67# define be32toh(x) bswap_32(x)
68# define be16toh(x) bswap_16(x)
69# else
70# define htobe64(x) x
71# define htobe32(x) x
72# define htobe16(x) x
73
74# define be64toh(x) x
75# define be32toh(x) x
76# define be16toh(x) x
77# endif
78#endif
79
80static const char *rp_cmd_names[RP_CMD_max + 1] = {
81 [RP_CMD_nop] = "nop",
82 [RP_CMD_hello] = "hello",
83 [RP_CMD_cfg] = "cfg",
84 [RP_CMD_read] = "read",
85 [RP_CMD_write] = "write",
86 [RP_CMD_interrupt] = "interrupt",
87 [RP_CMD_sync] = "sync",
88};
89
90const char *rp_cmd_to_string(enum rp_cmd cmd)
91{
92 assert(cmd <= RP_CMD_max);
93 return rp_cmd_names[cmd];
94}
95
96int rp_decode_hdr(struct rp_pkt *pkt)
97{
98 int used = 0;
99
100 pkt->hdr.cmd = be32toh(pkt->hdr.cmd);
101 pkt->hdr.len = be32toh(pkt->hdr.len);
102 pkt->hdr.id = be32toh(pkt->hdr.id);
103 pkt->hdr.flags = be32toh(pkt->hdr.flags);
104 pkt->hdr.dev = be32toh(pkt->hdr.dev);
105 used += sizeof pkt->hdr;
106 return used;
107}
108
109int rp_decode_payload(struct rp_pkt *pkt)
110{
111 int used = 0;
112
113 uint64_t master_id;
114
115 switch (pkt->hdr.cmd) {
116 case RP_CMD_hello:
117 assert(pkt->hdr.len >= sizeof pkt->hello.version);
118 pkt->hello.version.major = be16toh(pkt->hello.version.major);
119 pkt->hello.version.minor = be16toh(pkt->hello.version.minor);
120 used += sizeof pkt->hello.version;
121
122 if ((pkt->hdr.len - used) >= sizeof pkt->hello.caps) {
123 void *offset;
124 int i;
125
126 pkt->hello.caps.offset = be32toh(pkt->hello.caps.offset);
127 pkt->hello.caps.len = be16toh(pkt->hello.caps.len);
128
129 offset = (char *)pkt + pkt->hello.caps.offset;
130 for (i = 0; i < pkt->hello.caps.len; i++) {
131 uint32_t cap;
132
133
134
135 memcpy(&cap, offset + i * sizeof cap, sizeof cap);
136 cap = be32toh(cap);
137 memcpy(offset + i * sizeof cap, &cap, sizeof cap);
138 }
139 used += sizeof pkt->hello.caps;
140 } else {
141 pkt->hello.caps.offset = 0;
142 pkt->hello.caps.len = 0;
143 }
144
145
146
147 used = pkt->hdr.len;
148 break;
149 case RP_CMD_write:
150 case RP_CMD_read:
151 assert(pkt->hdr.len >= sizeof pkt->busaccess - sizeof pkt->hdr);
152 pkt->busaccess.timestamp = be64toh(pkt->busaccess.timestamp);
153 pkt->busaccess.addr = be64toh(pkt->busaccess.addr);
154 pkt->busaccess.master_id = be16toh(pkt->busaccess.master_id);
155 pkt->busaccess.attributes = be64toh(pkt->busaccess.attributes);
156 pkt->busaccess.len = be32toh(pkt->busaccess.len);
157 pkt->busaccess.width = be32toh(pkt->busaccess.width);
158 pkt->busaccess.stream_width = be32toh(pkt->busaccess.stream_width);
159 master_id = be16toh(pkt->busaccess.master_id);
160
161 used += sizeof pkt->busaccess - sizeof pkt->hdr;
162
163 if (pkt->busaccess.attributes & RP_BUS_ATTR_EXT_BASE) {
164 struct rp_pkt_busaccess_ext_base *pext = &pkt->busaccess_ext_base;
165
166 assert(pkt->hdr.len >= sizeof *pext - sizeof pkt->hdr);
167 master_id |= (uint64_t)be16toh(pext->master_id_31_16) << 16;
168 master_id |= (uint64_t)be32toh(pext->master_id_63_32) << 32;
169 pext->data_offset = be32toh(pext->data_offset);
170 pext->next_offset = be32toh(pext->next_offset);
171 pext->byte_enable_offset = be32toh(pext->byte_enable_offset);
172 pext->byte_enable_len = be32toh(pext->byte_enable_len);
173
174 used += sizeof *pext - sizeof pkt->busaccess;
175 }
176 pkt->busaccess.master_id = master_id;
177 break;
178 case RP_CMD_interrupt:
179 pkt->interrupt.timestamp = be64toh(pkt->interrupt.timestamp);
180 pkt->interrupt.vector = be64toh(pkt->interrupt.vector);
181 pkt->interrupt.line = be32toh(pkt->interrupt.line);
182 pkt->interrupt.val = pkt->interrupt.val;
183 used += pkt->hdr.len;
184 break;
185 case RP_CMD_sync:
186 pkt->sync.timestamp = be64toh(pkt->interrupt.timestamp);
187 used += pkt->hdr.len;
188 break;
189 default:
190 break;
191 }
192 return used;
193}
194
195void rp_encode_hdr(struct rp_pkt_hdr *hdr, uint32_t cmd, uint32_t id,
196 uint32_t dev, uint32_t len, uint32_t flags)
197{
198 hdr->cmd = htobe32(cmd);
199 hdr->len = htobe32(len);
200 hdr->id = htobe32(id);
201 hdr->dev = htobe32(dev);
202 hdr->flags = htobe32(flags);
203}
204
205size_t rp_encode_hello_caps(uint32_t id, uint32_t dev, struct rp_pkt_hello *pkt,
206 uint16_t version_major, uint16_t version_minor,
207 uint32_t *caps, uint32_t *caps_out,
208 uint32_t caps_len)
209{
210 size_t psize = sizeof *pkt + sizeof caps[0] * caps_len;
211 unsigned int i;
212
213 rp_encode_hdr(&pkt->hdr, RP_CMD_hello, id, dev,
214 psize - sizeof pkt->hdr, 0);
215 pkt->version.major = htobe16(version_major);
216 pkt->version.minor = htobe16(version_minor);
217
218
219 pkt->caps.offset = htobe32(sizeof *pkt);
220 pkt->caps.len = htobe16(caps_len);
221
222 for (i = 0; i < caps_len; i++) {
223 uint32_t cap;
224
225 cap = caps[i];
226 caps_out[i] = htobe32(cap);
227 }
228 return sizeof *pkt;
229}
230
231static void rp_encode_busaccess_common(struct rp_pkt_busaccess *pkt,
232 int64_t clk, uint16_t master_id,
233 uint64_t addr, uint64_t attr, uint32_t size,
234 uint32_t width, uint32_t stream_width)
235{
236 pkt->timestamp = htobe64(clk);
237 pkt->master_id = htobe16(master_id);
238 pkt->addr = htobe64(addr);
239 pkt->attributes = htobe64(attr);
240 pkt->len = htobe32(size);
241 pkt->width = htobe32(width);
242 pkt->stream_width = htobe32(stream_width);
243}
244
245size_t rp_encode_read(uint32_t id, uint32_t dev,
246 struct rp_pkt_busaccess *pkt,
247 int64_t clk, uint16_t master_id,
248 uint64_t addr, uint64_t attr, uint32_t size,
249 uint32_t width, uint32_t stream_width)
250{
251 rp_encode_hdr(&pkt->hdr, RP_CMD_read, id, dev,
252 sizeof *pkt - sizeof pkt->hdr, 0);
253 rp_encode_busaccess_common(pkt, clk, master_id, addr, attr,
254 size, width, stream_width);
255 return sizeof *pkt;
256}
257
258size_t rp_encode_read_resp(uint32_t id, uint32_t dev,
259 struct rp_pkt_busaccess *pkt,
260 int64_t clk, uint16_t master_id,
261 uint64_t addr, uint64_t attr, uint32_t size,
262 uint32_t width, uint32_t stream_width)
263{
264 rp_encode_hdr(&pkt->hdr, RP_CMD_read, id, dev,
265 sizeof *pkt - sizeof pkt->hdr + size, RP_PKT_FLAGS_response);
266 rp_encode_busaccess_common(pkt, clk, master_id, addr, attr,
267 size, width, stream_width);
268 return sizeof *pkt + size;
269}
270
271size_t rp_encode_write(uint32_t id, uint32_t dev,
272 struct rp_pkt_busaccess *pkt,
273 int64_t clk, uint16_t master_id,
274 uint64_t addr, uint64_t attr, uint32_t size,
275 uint32_t width, uint32_t stream_width)
276{
277 rp_encode_hdr(&pkt->hdr, RP_CMD_write, id, dev,
278 sizeof *pkt - sizeof pkt->hdr + size, 0);
279 rp_encode_busaccess_common(pkt, clk, master_id, addr, attr,
280 size, width, stream_width);
281 return sizeof *pkt;
282}
283
284size_t rp_encode_write_resp(uint32_t id, uint32_t dev,
285 struct rp_pkt_busaccess *pkt,
286 int64_t clk, uint16_t master_id,
287 uint64_t addr, uint64_t attr, uint32_t size,
288 uint32_t width, uint32_t stream_width)
289{
290 rp_encode_hdr(&pkt->hdr, RP_CMD_write, id, dev,
291 sizeof *pkt - sizeof pkt->hdr, RP_PKT_FLAGS_response);
292 rp_encode_busaccess_common(pkt, clk, master_id, addr, attr,
293 size, width, stream_width);
294 return sizeof *pkt;
295}
296
297
298size_t rp_encode_busaccess(struct rp_peer_state *peer,
299 struct rp_pkt_busaccess_ext_base *pkt,
300 struct rp_encode_busaccess_in *in) {
301 struct rp_pkt_busaccess *pkt_v4_0 = (void *) pkt;
302 uint32_t hsize = 0;
303 uint32_t ret_size = 0;
304
305
306 if (in->cmd == RP_CMD_write && !(in->flags & RP_PKT_FLAGS_response)) {
307 hsize = in->size;
308 }
309 if (in->cmd == RP_CMD_read && (in->flags & RP_PKT_FLAGS_response)) {
310 hsize = in->size;
311 ret_size = in->size;
312 }
313
314
315
316
317
318 if (!peer->caps.busaccess_ext_base && !(in->attr & RP_BUS_ATTR_EXT_BASE)) {
319
320 assert(in->master_id < UINT16_MAX);
321
322 rp_encode_hdr(&pkt->hdr, in->cmd, in->id, in->dev,
323 sizeof *pkt_v4_0 - sizeof pkt->hdr + hsize, in->flags);
324 rp_encode_busaccess_common(pkt_v4_0, in->clk, in->master_id,
325 in->addr, in->attr,
326 in->size, in->width, in->stream_width);
327 return sizeof *pkt_v4_0 + ret_size;
328 }
329
330
331 pkt->master_id_31_16 = htobe16(in->master_id >> 16);
332 pkt->master_id_63_32 = htobe32(in->master_id >> 32);
333
334
335 pkt->data_offset = htobe32(sizeof *pkt);
336 pkt->next_offset = 0;
337
338 pkt->byte_enable_offset = htobe32(sizeof *pkt + hsize);
339 pkt->byte_enable_len = htobe32(in->byte_enable_len);
340 hsize += in->byte_enable_len;
341
342 rp_encode_hdr(&pkt->hdr, in->cmd, in->id, in->dev,
343 sizeof *pkt - sizeof pkt->hdr + hsize, in->flags);
344 rp_encode_busaccess_common(pkt_v4_0, in->clk, in->master_id, in->addr,
345 in->attr | RP_BUS_ATTR_EXT_BASE,
346 in->size, in->width, in->stream_width);
347
348 return sizeof *pkt + ret_size;
349}
350
351size_t rp_encode_interrupt(uint32_t id, uint32_t dev,
352 struct rp_pkt_interrupt *pkt,
353 int64_t clk,
354 uint32_t line, uint64_t vector, uint8_t val)
355{
356 rp_encode_hdr(&pkt->hdr, RP_CMD_interrupt, id, dev,
357 sizeof *pkt - sizeof pkt->hdr, 0);
358 pkt->timestamp = htobe64(clk);
359 pkt->vector = htobe64(vector);
360 pkt->line = htobe32(line);
361 pkt->val = val;
362 return sizeof *pkt;
363}
364
365static size_t rp_encode_sync_common(uint32_t id, uint32_t dev,
366 struct rp_pkt_sync *pkt,
367 int64_t clk, uint32_t flags)
368{
369 rp_encode_hdr(&pkt->hdr, RP_CMD_sync, id, dev,
370 sizeof *pkt - sizeof pkt->hdr, flags);
371 pkt->timestamp = htobe64(clk);
372 return sizeof *pkt;
373}
374
375size_t rp_encode_sync(uint32_t id, uint32_t dev,
376 struct rp_pkt_sync *pkt,
377 int64_t clk)
378{
379 return rp_encode_sync_common(id, dev, pkt, clk, 0);
380}
381
382size_t rp_encode_sync_resp(uint32_t id, uint32_t dev,
383 struct rp_pkt_sync *pkt,
384 int64_t clk)
385{
386 return rp_encode_sync_common(id, dev, pkt, clk, RP_PKT_FLAGS_response);
387}
388
389void rp_process_caps(struct rp_peer_state *peer,
390 void *caps, size_t caps_len)
391{
392 int i;
393
394 assert(peer->caps.busaccess_ext_base == false);
395
396 for (i = 0; i < caps_len; i++) {
397 uint32_t cap;
398
399 memcpy(&cap, caps + i * sizeof cap, sizeof cap);
400
401 switch (cap) {
402 case CAP_BUSACCESS_EXT_BASE:
403 peer->caps.busaccess_ext_base = true;
404 break;
405 case CAP_BUSACCESS_EXT_BYTE_EN:
406 peer->caps.busaccess_ext_byte_en = true;
407 break;
408 }
409 }
410}
411
412
413void rp_dpkt_alloc(RemotePortDynPkt *dpkt, size_t size)
414{
415 if (dpkt->size < size) {
416 char *u8;
417 dpkt->pkt = realloc(dpkt->pkt, size);
418 u8 = (void *) dpkt->pkt;
419 memset(u8 + dpkt->size, 0, size - dpkt->size);
420 dpkt->size = size;
421 }
422}
423
424void rp_dpkt_swap(RemotePortDynPkt *a, RemotePortDynPkt *b)
425{
426 struct rp_pkt *tmp_pkt;
427 size_t tmp_size;
428
429 tmp_pkt = a->pkt;
430 tmp_size = a->size;
431 a->pkt = b->pkt;
432 a->size = b->size;
433 b->pkt = tmp_pkt;
434 b->size = tmp_size;
435}
436
437bool rp_dpkt_is_valid(RemotePortDynPkt *dpkt)
438{
439 return dpkt->size > 0 && dpkt->pkt->hdr.len;
440}
441
442void rp_dpkt_invalidate(RemotePortDynPkt *dpkt)
443{
444 assert(rp_dpkt_is_valid(dpkt));
445 dpkt->pkt->hdr.len = 0;
446}
447
448inline void rp_dpkt_free(RemotePortDynPkt *dpkt)
449{
450 dpkt->size = 0;
451 free(dpkt->pkt);
452}
453