1 /* ios.c - IO spaces for poke. */
2
3 /* Copyright (C) 2019, 2020, 2021 Jose E. Marchesi */
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 <config.h>
20 #include <gettext.h>
21 #include <stdlib.h>
22 #include <unistd.h>
23 #include <stdio.h>
24 #include <assert.h>
25 #include <limits.h>
26 #define _(str) gettext (str)
27 #include <streq.h>
28
29 #include "byteswap.h"
30
31 #include "pk-utils.h"
32 #include "ios.h"
33 #include "ios-dev.h"
34
35 #define IOS_GET_C_ERR_CHCK(c, io, off) \
36 { \
37 uint8_t ch; \
38 int ret = (io)->dev_if->pread ((io)->dev, &ch, 1, off); \
39 if (ret == IOD_EOF) \
40 return IOS_EIOFF; \
41 (c) = ch; \
42 }
43
44 #define IOS_PUT_C_ERR_CHCK(c, io, len, off) \
45 { \
46 if ((io)->dev_if->pwrite ((io)->dev, c, len, off) \
47 == IOD_EOF) \
48 return IOS_EIOFF; \
49 }
50
51 /* The following struct implements an instance of an IO space.
52
53 `ID' is an unique integer identifying the IO space.
54
55 HANDLER is a copy of the handler string used to open the space.
56
57 DEV is the device operated by the IO space.
58 DEV_IF is the interface to use when operating the device.
59
60 NEXT is a pointer to the next open IO space, or NULL.
61
62 XXX: add status, saved or not saved.
63 */
64
65 struct ios
66 {
67 int id;
68 char *handler;
69 void *dev;
70 struct ios_dev_if *dev_if;
71 ios_off bias;
72
73 struct ios *next;
74 };
75
76 /* Next available IOS id. */
77
78 static int ios_next_id = 0;
79
80 /* List of IO spaces, and pointer to the current one. */
81
82 static struct ios *io_list;
83 static struct ios *cur_io;
84
85 /* The available backends are implemented in their own files, and
86 provide the following interfaces. */
87
88 extern struct ios_dev_if ios_dev_mem; /* ios-dev-mem.c */
89 extern struct ios_dev_if ios_dev_file; /* ios-dev-file.c */
90 extern struct ios_dev_if ios_dev_stream; /* ios-dev-stream.c */
91 #ifdef HAVE_LIBNBD
92 extern struct ios_dev_if ios_dev_nbd; /* ios-dev-nbd.c */
93 #endif
94
95 static struct ios_dev_if *ios_dev_ifs[] =
96 {
97 &ios_dev_mem,
98 &ios_dev_stream,
99 #ifdef HAVE_LIBNBD
100 &ios_dev_nbd,
101 #endif
102 /* File must be last */
103 &ios_dev_file,
104 NULL,
105 };
106
107 void
ios_init(void)108 ios_init (void)
109 {
110 /* Nothing to do here... yet. */
111 }
112
113 void
ios_shutdown(void)114 ios_shutdown (void)
115 {
116 /* Close and free all open IO spaces. */
117 while (io_list)
118 ios_close (io_list);
119 }
120
121 int
ios_open(const char * handler,uint64_t flags,int set_cur)122 ios_open (const char *handler, uint64_t flags, int set_cur)
123 {
124 struct ios *io;
125 struct ios_dev_if **dev_if = NULL;
126 int iod_error = IOD_OK, error = IOS_ERROR;
127
128 /* Allocate and initialize the new IO space. */
129 io = malloc (sizeof (struct ios));
130 if (!io)
131 return IOS_ENOMEM;
132
133 io->handler = NULL;
134 io->dev = NULL;
135 io->next = NULL;
136 io->bias = 0;
137
138 /* Look for a device interface suitable to operate on the given
139 handler. */
140 for (dev_if = ios_dev_ifs; *dev_if; ++dev_if)
141 {
142 iod_error = (*dev_if)->handler_normalize (handler, flags, &io->handler);
143 if (iod_error)
144 goto error;
145 if (io->handler)
146 break;
147 }
148
149 if (*dev_if == NULL)
150 goto error;
151
152 io->dev_if = *dev_if;
153
154 /* Do not re-open an already-open IO space. */
155 for (ios i = io_list; i; i = i->next)
156 if (STREQ (i->handler, io->handler))
157 {
158 error = IOS_EOPEN;
159 goto error;
160 }
161
162 /* Open the device using the interface found above. */
163 iod_error = io->dev_if->open (handler, flags, &io->dev);
164 if (iod_error || io->dev == NULL)
165 goto error;
166
167 /* Increment the id counter after all possible errors are avoided. */
168 io->id = ios_next_id++;
169
170 /* Add the newly created space to the list, and update the current
171 space. */
172 io->next = io_list;
173 io_list = io;
174
175 if (!cur_io || set_cur == 1)
176 cur_io = io;
177
178 return io->id;
179
180 error:
181 if (io)
182 free (io->handler);
183 free (io);
184
185 if (iod_error)
186 error = IOD_ERROR_TO_IOS_ERROR (iod_error);
187
188 return error;
189 }
190
191 void
ios_close(ios io)192 ios_close (ios io)
193 {
194 struct ios *tmp;
195 int r;
196
197 /* XXX: if not saved, ask before closing. */
198
199 /* Close the device operated by the IO space.
200 XXX: handle errors. */
201 r = io->dev_if->close (io->dev);
202 assert (r);
203
204 /* Unlink the IOS from the list. */
205 assert (io_list != NULL); /* The list must contain at least one IO
206 space. */
207 if (io_list == io)
208 io_list = io_list->next;
209 else
210 {
211 for (tmp = io_list; tmp->next != io; tmp = tmp->next)
212 ;
213 tmp->next = io->next;
214 }
215
216 /* Set the new current IO. */
217 if (io == cur_io)
218 cur_io = io_list;
219
220 free (io);
221 }
222
223 uint64_t
ios_flags(ios io)224 ios_flags (ios io)
225 {
226 return io->dev_if->get_flags (io->dev);
227 }
228
229 const char *
ios_handler(ios io)230 ios_handler (ios io)
231 {
232 return io->handler;
233 }
234
235 ios
ios_cur(void)236 ios_cur (void)
237 {
238 return cur_io;
239 }
240
241 void
ios_set_cur(ios io)242 ios_set_cur (ios io)
243 {
244 cur_io = io;
245 }
246
247 ios
ios_search(const char * handler)248 ios_search (const char *handler)
249 {
250 ios io;
251
252 for (io = io_list; io; io = io->next)
253 if (STREQ (io->handler, handler))
254 break;
255
256 return io;
257 }
258
259 ios
ios_search_by_id(int id)260 ios_search_by_id (int id)
261 {
262 ios io;
263
264 for (io = io_list; io; io = io->next)
265 if (io->id == id)
266 break;
267
268 return io;
269 }
270
271 int
ios_get_id(ios io)272 ios_get_id (ios io)
273 {
274 return io->id;
275 }
276
277 char *
ios_get_dev_if_name(ios io)278 ios_get_dev_if_name (ios io)
279 {
280 return io->dev_if->get_if_name ();
281 }
282
283 ios_off
ios_get_bias(ios io)284 ios_get_bias (ios io)
285 {
286 return io->bias;
287 }
288
289 void
ios_set_bias(ios io,ios_off bias)290 ios_set_bias (ios io, ios_off bias)
291 {
292 io->bias = bias;
293 }
294
295 ios
ios_begin(void)296 ios_begin (void)
297 {
298 return io_list;
299 }
300
301 bool
ios_end(const ios io)302 ios_end (const ios io)
303 {
304 return (io == NULL);
305 }
306
307 ios
ios_next(const ios io)308 ios_next (const ios io)
309 {
310 return io->next;
311 }
312
313 void
ios_map(ios_map_fn cb,void * data)314 ios_map (ios_map_fn cb, void *data)
315 {
316 ios io;
317
318 for (io = io_list; io; io = io->next)
319 (*cb) (io, data);
320 }
321
322 /* Set all except the lowest SIGNIFICANT_BITS of VALUE to zero. */
323 #define IOS_CHAR_GET_LSB(value, significant_bits) \
324 (*(value) &= 0xFFU >> (CHAR_BIT - (significant_bits)))
325
326 /* Set all except the highest SIGNIFICANT_BITS of the lowest
327 significant byte of VALUE to zero. */
328 #define IOS_CHAR_GET_MSB(value, significant_bits) \
329 (*(value) &= 0xFFU << (CHAR_BIT - (significant_bits)))
330
331 static inline int
ios_read_int_common(ios io,ios_off offset,int flags,int bits,enum ios_endian endian,uint64_t * value)332 ios_read_int_common (ios io, ios_off offset, int flags,
333 int bits,
334 enum ios_endian endian,
335 uint64_t *value)
336 {
337 /* 64 bits might span at most 9 bytes. */
338 uint8_t c[9] = {0, 0, 0, 0, 0, 0, 0, 0, 0};
339
340 /* Number of signifcant bits in the first byte. */
341 int firstbyte_bits = 8 - (offset % 8);
342
343 /* (Total number of bytes that need to be read) - 1. */
344 int bytes_minus1 = (bits - firstbyte_bits + 7) / 8;
345
346 /* Number of significant bits in the last byte. */
347 int lastbyte_bits = (bits + (offset % 8)) % 8;
348 lastbyte_bits = lastbyte_bits == 0 ? 8 : lastbyte_bits;
349
350 /* Read the bytes and clear the unused bits. */
351 if (io->dev_if->pread (io->dev, c, bytes_minus1 + 1, offset / 8) == IOD_EOF)
352 return IOS_EIOFF;
353 IOS_CHAR_GET_LSB(&c[0], firstbyte_bits);
354
355 switch (bytes_minus1)
356 {
357 case 0:
358 *value = c[0] >> (8 - lastbyte_bits);
359 return IOS_OK;
360
361 case 1:
362 IOS_CHAR_GET_MSB(&c[1], lastbyte_bits);
363 if (endian == IOS_ENDIAN_LSB)
364 {
365 if (bits <= 8)
366 /* We need to shift to align the least significant bit. */
367 *value = (c[0] << lastbyte_bits) | (c[1] >> (8 - lastbyte_bits));
368 else if ((offset % 8) == 0)
369 /* If little endian and the least significant byte is 8 bits aligned,
370 then we can handle the information byte by byte as we read. */
371 *value = (c[1] << lastbyte_bits) | c[0];
372 else
373 {
374 /* Consider the order of bits in a little endian number:
375 7-6-5-4-3-2-1-0-15-14-13-12-11-10-9-8- ... If such an
376 encoding is not byte-aligned, we have to first shift to fill the
377 least significant byte to get the right bits in the same bytes. */
378 uint64_t reg;
379 reg = (c[0] << (8 + offset % 8)) | (c[1] << offset % 8);
380 *value = ((reg & 0xff) << (bits % 8)) | (reg >> 8);
381 }
382 }
383 else
384 {
385 /* We should shift to fill the least significant byte
386 which is the last 8 bits. */
387 *value = (c[0] << lastbyte_bits) | (c[1] >> (8 - lastbyte_bits));
388 }
389 return IOS_OK;
390
391 case 2:
392 IOS_CHAR_GET_MSB(&c[2], lastbyte_bits);
393 if (endian == IOS_ENDIAN_LSB)
394 {
395 if ((offset % 8) == 0)
396 /* If little endian and the least significant byte is 8 bits aligned,
397 then we can handle the information byte by byte as we read. */
398 *value = (c[2] << (8 + lastbyte_bits)) | (c[1] << 8) | c[0];
399 else
400 {
401 /* We have to shift to fill the least significant byte to get
402 the right bits in the same bytes. */
403 uint64_t reg;
404 reg = ((uint64_t) c[0] << (56 + offset % 8))
405 | ((uint64_t) c[1] << (48 + offset % 8))
406 | ((uint64_t) c[2] << (40 + offset % 8));
407 /* The bits in the most-significant-byte-to-be is aligned to left,
408 shift it towards right! */
409 if (bits <= 16)
410 reg = ((reg & 0x00ff000000000000LL) >> (16 - bits))
411 | (reg & 0xff00ffffffffffffLL);
412 else
413 reg = ((reg & 0x0000ff0000000000LL) >> (24 - bits))
414 | (reg & 0xffff00ffffffffffLL);
415 /* Now we can place the bytes correctly. */
416 *value = bswap_64(reg);
417 }
418 }
419 else
420 {
421 /* We should shift to fill the least significant byte
422 which is the last 8 bits. */
423 *value = (c[0] << (8 + lastbyte_bits)) | (c[1] << lastbyte_bits)
424 | (c[2] >> (8 - lastbyte_bits));
425 }
426 return IOS_OK;
427
428 case 3:
429 IOS_CHAR_GET_MSB(&c[3], lastbyte_bits);
430 if (endian == IOS_ENDIAN_LSB)
431 {
432 if ((offset % 8) == 0)
433 /* If little endian and the least significant byte is 8 bits aligned,
434 then we can handle the information byte by byte as we read. */
435 *value = (c[3] << (16 + lastbyte_bits)) | (c[2] << 16) | (c[1] << 8)
436 | c[0];
437 else
438 {
439 /* We have to shift to fill the least significant byte to get
440 the right bits in the same bytes. */
441 uint64_t reg;
442 reg = ((uint64_t) c[0] << (56 + offset % 8))
443 | ((uint64_t) c[1] << (48 + offset % 8))
444 | ((uint64_t) c[2] << (40 + offset % 8))
445 | ((uint64_t) c[3] << (32 + offset % 8));
446 /* The bits in the most-significant-byte-to-be is aligned to left,
447 shift it towards right! */
448 if (bits <= 24)
449 reg = ((reg & 0x0000ff0000000000LL) >> (24 - bits))
450 | (reg & 0xffff00ffffffffffLL);
451 else
452 reg = ((reg & 0x000000ff00000000LL) >> (32 - bits))
453 | (reg & 0xffffff00ffffffffLL);
454 /* Now we can place the bytes correctly. */
455 *value = bswap_64(reg);
456 }
457 }
458 else
459 {
460 /* We should shift to fill the least significant byte
461 which is the last 8 bits. */
462 *value = (c[0] << (16 + lastbyte_bits)) | (c[1] << (8 + lastbyte_bits))
463 | (c[2] << lastbyte_bits) | (c[3] >> (8 - lastbyte_bits));
464 }
465 return IOS_OK;
466
467 case 4:
468 IOS_CHAR_GET_MSB(&c[4], lastbyte_bits);
469 if (endian == IOS_ENDIAN_LSB)
470 {
471 if ((offset % 8) == 0)
472 /* If little endian and the least significant byte is 8 bits aligned,
473 then we can handle the information byte by byte as we read. */
474 *value = ((uint64_t) c[4] << (24 + lastbyte_bits))
475 | ((uint64_t) c[3] << 24) | (c[2] << 16)
476 | (c[1] << 8) | c[0];
477 else
478 {
479 /* We have to shift to fill the least significant byte to get
480 the right bits in the same bytes. */
481 uint64_t reg;
482 reg = ((uint64_t) c[0] << (56 + offset % 8))
483 | ((uint64_t) c[1] << (48 + offset % 8))
484 | ((uint64_t) c[2] << (40 + offset % 8))
485 | ((uint64_t) c[3] << (32 + offset % 8))
486 | ((uint64_t) c[4] << (24 + offset % 8));
487 /* The bits in the most-significant-byte-to-be is aligned to left,
488 shift it towards right! */
489 if (bits <= 32)
490 reg = ((reg & 0x000000ff00000000LL) >> (32 - bits))
491 | (reg & 0xffffff00ffffffffLL);
492 else
493 reg = ((reg & 0x00000000ff000000LL) >> (40 - bits))
494 | (reg & 0xffffffff00ffffffLL);
495 /* Now we can place the bytes correctly. */
496 *value = bswap_64(reg);
497 }
498 }
499 else
500 {
501 /* We should shift to fill the least significant byte
502 which is the last 8 bits. */
503 *value = ((uint64_t) c[0] << (24 + lastbyte_bits))
504 | ((uint64_t) c[1] << (16 + lastbyte_bits))
505 | (c[2] << (8 + lastbyte_bits)) | (c[3] << lastbyte_bits)
506 | (c[4] >> (8 - lastbyte_bits));
507 }
508 return IOS_OK;
509
510 case 5:
511 IOS_CHAR_GET_MSB(&c[5], lastbyte_bits);
512 if (endian == IOS_ENDIAN_LSB)
513 {
514 if ((offset % 8) == 0)
515 /* If little endian and the least significant byte is 8 bits aligned,
516 then we can handle the information byte by byte as we read. */
517 *value = ((uint64_t) c[5] << (32 + lastbyte_bits))
518 | ((uint64_t) c[4] << 32) | ((uint64_t) c[3] << 24)
519 | (c[2] << 16) | (c[1] << 8) | c[0];
520 else
521 {
522 /* We have to shift to fill the least significant byte to get
523 the right bits in the same bytes. */
524 uint64_t reg;
525 reg = ((uint64_t) c[0] << (56 + offset % 8))
526 | ((uint64_t) c[1] << (48 + offset % 8))
527 | ((uint64_t) c[2] << (40 + offset % 8))
528 | ((uint64_t) c[3] << (32 + offset % 8))
529 | ((uint64_t) c[4] << (24 + offset % 8))
530 | (c[5] << (16 + offset % 8));
531 /* The bits in the most-significant-byte-to-be is aligned to left,
532 shift it towards right! */
533 if (bits <= 40)
534 reg = ((reg & 0x00000000ff000000LL) >> (40 - bits))
535 | (reg & 0xffffffff00ffffffLL);
536 else
537 reg = ((reg & 0x0000000000ff0000LL) >> (48 - bits))
538 | (reg & 0xffffffffff00ffffLL);
539 /* Now we can place the bytes correctly. */
540 *value = bswap_64(reg);
541 }
542 }
543 else
544 {
545 /* We should shift to fill the least significant byte
546 which is the last 8 bits. */
547 *value = ((uint64_t) c[0] << (32 + lastbyte_bits))
548 | ((uint64_t) c[1] << (24 + lastbyte_bits))
549 | ((uint64_t) c[2] << (16 + lastbyte_bits))
550 | (c[3] << (8 + lastbyte_bits))
551 | (c[4] << lastbyte_bits) | (c[5] >> (8 - lastbyte_bits));
552 }
553 return IOS_OK;
554
555 case 6:
556 IOS_CHAR_GET_MSB(&c[6], lastbyte_bits);
557 if (endian == IOS_ENDIAN_LSB)
558 {
559 if ((offset % 8) == 0)
560 /* If little endian and the least significant byte is 8 bits aligned,
561 then we can handle the information byte by byte as we read. */
562 *value = ((uint64_t) c[6] << (40 + lastbyte_bits))
563 | ((uint64_t) c[5] << 40) | ((uint64_t) c[4] << 32)
564 | ((uint64_t) c[3] << 24) | (c[2] << 16) | (c[1] << 8)
565 | c[0];
566 else
567 {
568 /* We have to shift to fill the least significant byte to get
569 the right bits in the same bytes. */
570 uint64_t reg;
571 reg = ((uint64_t) c[0] << (56 + offset % 8))
572 | ((uint64_t) c[1] << (48 + offset % 8))
573 | ((uint64_t) c[2] << (40 + offset % 8))
574 | ((uint64_t) c[3] << (32 + offset % 8))
575 | ((uint64_t) c[4] << (24 + offset % 8))
576 | (c[5] << (16 + offset % 8))
577 | (c[6] << (8 + offset % 8));
578 /* The bits in the most-significant-byte-to-be is aligned to left,
579 shift it towards right! */
580 if (bits <= 48)
581 reg = ((reg & 0x0000000000ff0000LL) >> (48 - bits))
582 | (reg & 0xffffffffff00ffffLL);
583 else
584 reg = ((reg & 0x000000000000ff00LL) >> (56 - bits))
585 | (reg & 0xffffffffffff00ffLL);
586 /* Now we can place the bytes correctly. */
587 *value = bswap_64(reg);
588 }
589 }
590 else
591 {
592 /* We should shift to fill the least significant byte
593 which is the last 8 bits. */
594 *value = ((uint64_t) c[0] << (40 + lastbyte_bits))
595 | ((uint64_t) c[1] << (32 + lastbyte_bits))
596 | ((uint64_t) c[2] << (24 + lastbyte_bits))
597 | ((uint64_t) c[3] << (16 + lastbyte_bits))
598 | (c[4] << (8 + lastbyte_bits)) | (c[5] << lastbyte_bits)
599 | (c[6] >> (8 - lastbyte_bits));
600 }
601 return IOS_OK;
602
603 case 7:
604 IOS_CHAR_GET_MSB(&c[7], lastbyte_bits);
605 if (endian == IOS_ENDIAN_LSB)
606 {
607 if ((offset % 8) == 0)
608 /* If little endian and the least significant byte is 8 bits aligned,
609 then we can handle the information byte by byte as we read. */
610 *value = ((uint64_t) c[7] << (48 + lastbyte_bits))
611 | ((uint64_t) c[6] << 48) | ((uint64_t) c[5] << 40)
612 | ((uint64_t) c[4] << 32) | ((uint64_t) c[3] << 24)
613 | (c[2] << 16) | (c[1] << 8) | c[0];
614 else
615 {
616 /* We have to shift to fill the least significant byte to get
617 the right bits in the same bytes. */
618 uint64_t reg;
619 reg = ((uint64_t) c[0] << (56 + offset % 8))
620 | ((uint64_t) c[1] << (48 + offset % 8))
621 | ((uint64_t) c[2] << (40 + offset % 8))
622 | ((uint64_t) c[3] << (32 + offset % 8))
623 | ((uint64_t) c[4] << (24 + offset % 8))
624 | (c[5] << (16 + offset % 8))
625 | (c[6] << (8 + offset % 8)) | (c[7] << offset % 8);
626 /* The bits in the most-significant-byte-to-be is aligned to left,
627 shift it towards right! */
628 if (bits <= 56)
629 reg = ((reg & 0x000000000000ff00LL) >> (56 - bits))
630 | (reg & 0xffffffffffff00ffLL);
631 else
632 reg = ((reg & 0x00000000000000ffLL) >> (64 - bits))
633 | (reg & 0xffffffffffffff00LL);
634 /* Now we can place the bytes correctly. */
635 *value = bswap_64(reg);
636 }
637 }
638 else
639 {
640 /* We should shift to fill the least significant byte
641 which is the last 8 bits. */
642 *value = ((uint64_t) c[0] << (48 + lastbyte_bits))
643 | ((uint64_t) c[1] << (40 + lastbyte_bits))
644 | ((uint64_t) c[2] << (32 + lastbyte_bits))
645 | ((uint64_t) c[3] << (24 + lastbyte_bits))
646 | ((uint64_t) c[4] << (16 + lastbyte_bits))
647 | (c[5] << (8 + lastbyte_bits))
648 | (c[6] << lastbyte_bits) | (c[7] >> (8 - lastbyte_bits));
649 }
650 return IOS_OK;
651
652 case 8:
653 IOS_CHAR_GET_MSB(&c[8], lastbyte_bits);
654 if (endian == IOS_ENDIAN_LSB)
655 {
656 /* We have to shift to fill the least significant byte to get
657 the right bits in the same bytes. */
658 uint64_t reg;
659 reg = ((uint64_t) c[0] << (56 + offset % 8))
660 | ((uint64_t) c[1] << (48 + offset % 8))
661 | ((uint64_t) c[2] << (40 + offset % 8))
662 | ((uint64_t) c[3] << (32 + offset % 8))
663 | ((uint64_t) c[4] << (24 + offset % 8))
664 | (c[5] << (16 + offset % 8))
665 | (c[6] << (8 + offset % 8)) | (c[7] << offset % 8)
666 | (c[8] >> firstbyte_bits);
667 /* The bits in the most-significant-byte-to-be is aligned to left,
668 shift it towards right! */
669 reg = ((reg & 0x00000000000000ffLL) >> (64 - bits))
670 | (reg & 0xffffffffffffff00LL);
671 /* Now we can place the bytes correctly. */
672 *value = bswap_64(reg);
673 }
674 else
675 {
676 /* We should shift to fill the least significant byte
677 which is the last 8 bits. */
678 *value = ((uint64_t) c[0] << (56 + lastbyte_bits))
679 | ((uint64_t) c[1] << (48 + lastbyte_bits))
680 | ((uint64_t) c[2] << (40 + lastbyte_bits))
681 | ((uint64_t) c[3] << (32 + lastbyte_bits))
682 | ((uint64_t) c[4] << (24 + lastbyte_bits))
683 | ((uint64_t) c[5] << (16 + lastbyte_bits))
684 | (c[6] << (8 + lastbyte_bits)) | (c[7] << lastbyte_bits)
685 | (c[8] >> (8 - lastbyte_bits));
686 }
687 return IOS_OK;
688
689 default:
690 assert (0);
691 }
692 }
693
694 int
ios_read_int(ios io,ios_off offset,int flags,int bits,enum ios_endian endian,enum ios_nenc nenc,int64_t * value)695 ios_read_int (ios io, ios_off offset, int flags,
696 int bits,
697 enum ios_endian endian,
698 enum ios_nenc nenc,
699 int64_t *value)
700 {
701 /* Apply the IOS bias. */
702 offset += ios_get_bias (io);
703
704 /* Fast track for byte-aligned 8x bits */
705 if (offset % 8 == 0 && bits % 8 == 0)
706 {
707 uint8_t c[8];
708 if (io->dev_if->pread (io->dev, c, bits / 8, offset / 8) == IOD_EOF)
709 return IOS_EIOFF;
710
711 switch (bits) {
712 case 8:
713 {
714 *value = (int8_t) c[0];
715 return IOS_OK;
716 }
717
718 case 16:
719 {
720 if (endian == IOS_ENDIAN_LSB)
721 *value = (int16_t) (c[1] << 8) | c[0];
722 else
723 *value = (int16_t) (c[0] << 8) | c[1];
724 return IOS_OK;
725 }
726
727 case 24:
728 {
729 if (endian == IOS_ENDIAN_LSB)
730 *value = (c[2] << 16) | (c[1] << 8) | c[0];
731 else
732 *value = (c[0] << 16) | (c[1] << 8) | c[2];
733 *value <<= 40;
734 *value >>= 40;
735 return IOS_OK;
736 }
737
738 case 32:
739 {
740 if (endian == IOS_ENDIAN_LSB)
741 *value = (int32_t) (c[3] << 24) | (c[2] << 16) | (c[1] << 8) | c[0];
742 else
743 *value = (int32_t) (c[0] << 24) | (c[1] << 16) | (c[2] << 8) | c[3];
744 return IOS_OK;
745 }
746
747 case 40:
748 {
749 if (endian == IOS_ENDIAN_LSB)
750 *value = ((uint64_t) c[4] << 32) | ((uint64_t) c[3] << 24)
751 | (c[2] << 16) | (c[1] << 8) | c[0];
752 else
753 *value = ((uint64_t) c[0] << 32) | ((uint64_t) c[1] << 24)
754 | (c[2] << 16) | (c[3] << 8) | c[4];
755 *value <<= 24;
756 *value >>= 24;
757 return IOS_OK;
758 }
759
760 case 48:
761 {
762 if (endian == IOS_ENDIAN_LSB)
763 *value = ((uint64_t) c[5] << 40) | ((uint64_t) c[4] << 32)
764 | ((uint64_t) c[3] << 24) | (c[2] << 16)
765 | (c[1] << 8) | c[0];
766 else
767 *value = ((uint64_t) c[0] << 40) | ((uint64_t) c[1] << 32)
768 | ((uint64_t) c[2] << 24) | (c[3] << 16)
769 | (c[4] << 8) | c[5];
770 *value <<= 16;
771 *value >>= 16;
772 return IOS_OK;
773 }
774
775 case 56:
776 {
777 if (endian == IOS_ENDIAN_LSB)
778 *value = ((uint64_t) c[6] << 48) | ((uint64_t) c[5] << 40)
779 | ((uint64_t) c[4] << 32) | ((uint64_t) c[3] << 24)
780 | (c[2] << 16) | (c[1] << 8) | c[0];
781 else
782 *value = ((uint64_t) c[0] << 48) | ((uint64_t) c[1] << 40)
783 | ((uint64_t) c[2] << 32) | ((uint64_t) c[3] << 24)
784 | (c[4] << 16) | (c[5] << 8) | c[6];
785 *value <<= 8;
786 *value >>= 8;
787 return IOS_OK;
788 }
789
790 case 64:
791 {
792 if (endian == IOS_ENDIAN_LSB)
793 *value = ((uint64_t) c[7] << 56) | ((uint64_t) c[6] << 48)
794 | ((uint64_t) c[5] << 40) | ((uint64_t) c[4] << 32)
795 | ((uint64_t) c[3] << 24) | (c[2] << 16) | (c[1] << 8)
796 | c[0];
797 else
798 *value = ((uint64_t) c[0] << 56) | ((uint64_t) c[1] << 48)
799 | ((uint64_t) c[2] << 40) | ((uint64_t) c[3] << 32)
800 | ((uint64_t) c[4] << 24) | (c[5] << 16) | (c[6] << 8)
801 | c[7];
802 return IOS_OK;
803 }
804 }
805 }
806
807 /* Fall into the case for the unaligned and the sizes other than 8x. */
808 int ret_val = ios_read_int_common (io, offset, flags, bits, endian,
809 (uint64_t *) value);
810 if (ret_val == IOS_OK)
811 {
812 *value <<= 64 - bits;
813 *value >>= 64 - bits;
814 return IOS_OK;
815 }
816 return ret_val;
817 }
818
819 int
ios_read_uint(ios io,ios_off offset,int flags,int bits,enum ios_endian endian,uint64_t * value)820 ios_read_uint (ios io, ios_off offset, int flags,
821 int bits,
822 enum ios_endian endian,
823 uint64_t *value)
824 {
825 /* Apply the IOS bias. */
826 offset += ios_get_bias (io);
827
828 /* Fast track for byte-aligned 8x bits */
829 if (offset % 8 == 0 && bits % 8 == 0)
830 {
831 uint8_t c[8];
832 if (io->dev_if->pread (io->dev, c, bits / 8, offset / 8) == IOD_EOF)
833 return IOS_EIOFF;
834
835 switch (bits) {
836 case 8:
837 *value = c[0];
838 return IOS_OK;
839
840 case 16:
841 if (endian == IOS_ENDIAN_LSB)
842 *value = (c[1] << 8) | c[0];
843 else
844 *value = (c[0] << 8) | c[1];
845 return IOS_OK;
846
847 case 24:
848 if (endian == IOS_ENDIAN_LSB)
849 *value = (c[2] << 16) | (c[1] << 8) | c[0];
850 else
851 *value = (c[0] << 16) | (c[1] << 8) | c[2];
852 return IOS_OK;
853
854 case 32:
855 if (endian == IOS_ENDIAN_LSB)
856 *value = ((uint64_t) c[3] << 24) | (c[2] << 16) | (c[1] << 8) | c[0];
857 else
858 *value = ((uint64_t) c[0] << 24) | (c[1] << 16) | (c[2] << 8) | c[3];
859 return IOS_OK;
860
861 case 40:
862 if (endian == IOS_ENDIAN_LSB)
863 *value = ((uint64_t) c[4] << 32) | ((uint64_t) c[3] << 24)
864 | (c[2] << 16) | (c[1] << 8) | c[0];
865 else
866 *value = ((uint64_t) c[0] << 32) | ((uint64_t) c[1] << 24)
867 | (c[2] << 16) | (c[3] << 8) | c[4];
868 return IOS_OK;
869
870 case 48:
871 if (endian == IOS_ENDIAN_LSB)
872 *value = ((uint64_t) c[5] << 40) | ((uint64_t) c[4] << 32)
873 | ((uint64_t) c[3] << 24) | (c[2] << 16)
874 | (c[1] << 8) | c[0];
875 else
876 *value = ((uint64_t) c[0] << 40) | ((uint64_t) c[1] << 32)
877 | ((uint64_t) c[2] << 24) | (c[3] << 16)
878 | (c[4] << 8) | c[5];
879 return IOS_OK;
880
881 case 56:
882 if (endian == IOS_ENDIAN_LSB)
883 *value = ((uint64_t) c[6] << 48) | ((uint64_t) c[5] << 40)
884 | ((uint64_t) c[4] << 32) | ((uint64_t) c[3] << 24)
885 | (c[2] << 16) | (c[1] << 8) | c[0];
886 else
887 *value = ((uint64_t) c[0] << 48) | ((uint64_t) c[1] << 40)
888 | ((uint64_t) c[2] << 32) | ((uint64_t) c[3] << 24)
889 | (c[4] << 16) | (c[5] << 8) | c[6];
890 return IOS_OK;
891
892 case 64:
893 if (endian == IOS_ENDIAN_LSB)
894 *value = ((uint64_t) c[7] << 56) | ((uint64_t) c[6] << 48)
895 | ((uint64_t) c[5] << 40) | ((uint64_t) c[4] << 32)
896 | ((uint64_t) c[3] << 24) | (c[2] << 16) | (c[1] << 8)
897 | c[0];
898 else
899 *value = ((uint64_t) c[0] << 56) | ((uint64_t) c[1] << 48)
900 | ((uint64_t) c[2] << 40) | ((uint64_t) c[3] << 32)
901 | ((uint64_t) c[4] << 24) | (c[5] << 16) | (c[6] << 8)
902 | c[7];
903 return IOS_OK;
904 }
905 }
906
907 /* Fall into the case for the unaligned and the sizes other than 8x. */
908 return ios_read_int_common (io, offset, flags, bits, endian, value);
909 }
910
realloc_string(char ** str,size_t newsize)911 static int realloc_string (char **str, size_t newsize)
912 {
913 char *newstr = realloc (*str, newsize);
914
915 if (!newstr)
916 return IOS_ENOMEM;
917
918 *str = newstr;
919 return IOS_OK;
920 }
921
922 int
ios_read_string(ios io,ios_off offset,int flags,char ** value)923 ios_read_string (ios io, ios_off offset, int flags, char **value)
924 {
925 char *str = NULL;
926 size_t i = 0;
927 int ret;
928
929 /* Apply the IOS bias. */
930 offset += ios_get_bias (io);
931
932 if (offset % 8 == 0)
933 {
934 /* This is the fast case: the string is aligned to a byte
935 boundary. We just read bytes from the IOD until either EOF
936 or a NULL byte. */
937
938 do
939 {
940 if (i % 128 == 0)
941 {
942 if ((ret = realloc_string (&str, i + 128 * sizeof (char))) < 0)
943 goto error;
944 }
945
946 if (io->dev_if->pread (io->dev, &str[i], 1,
947 offset / 8 + i) == IOD_EOF)
948 {
949 ret = IOS_EIOFF;
950 goto error;
951 }
952 }
953 while (str[i++] != '\0');
954 }
955 else
956 {
957 /* The string is not aligned to a byte boundary. Instead of
958 reading bytes from the IOD, we use the IOS to read 8-byte
959 unsigned integers. */
960
961 do
962 {
963 uint64_t abyte;
964
965 if (i % 128 == 0)
966 {
967 if ((ret = realloc_string (&str, i + 128 * sizeof (char))) < 0)
968 goto error;
969 }
970
971 ret = ios_read_uint (io, offset, flags, 8,
972 IOS_ENDIAN_MSB, /* Arbitrary. */
973 &abyte);
974 if (ret == IOS_EIOFF)
975 goto error;
976
977 str[i] = (char) abyte;
978 offset += 8;
979 }
980 while (str[i++] != '\0');
981 }
982
983 *value = str;
984 return IOS_OK;
985
986 error:
987 free (str);
988 return ret;
989 }
990
991 static inline int
ios_write_int_fast(ios io,ios_off offset,int flags,int bits,enum ios_endian endian,uint64_t value)992 ios_write_int_fast (ios io, ios_off offset, int flags,
993 int bits,
994 enum ios_endian endian,
995 uint64_t value)
996 {
997 uint8_t c[8];
998
999 switch (bits)
1000 {
1001 case 8:
1002 c[0] = value;
1003 break;
1004
1005 case 16:
1006 if (endian == IOS_ENDIAN_LSB)
1007 {
1008 c[0] = value;
1009 c[1] = value >> 8;
1010 }
1011 else
1012 {
1013 c[0] = value >> 8;
1014 c[1] = value;
1015 }
1016 break;
1017
1018 case 24:
1019 if (endian == IOS_ENDIAN_LSB)
1020 {
1021 c[0] = value;
1022 c[1] = value >> 8;
1023 c[2] = value >> 16;
1024 }
1025 else
1026 {
1027 c[0] = value >> 16;
1028 c[1] = value >> 8;
1029 c[2] = value;
1030 }
1031 break;
1032
1033 case 32:
1034 if (endian == IOS_ENDIAN_LSB)
1035 {
1036 c[0] = value;
1037 c[1] = value >> 8;
1038 c[2] = value >> 16;
1039 c[3] = value >> 24;
1040 }
1041 else
1042 {
1043 c[0] = value >> 24;
1044 c[1] = value >> 16;
1045 c[2] = value >> 8;
1046 c[3] = value;
1047 }
1048 break;
1049
1050 case 40:
1051 if (endian == IOS_ENDIAN_LSB)
1052 {
1053 c[0] = value;
1054 c[1] = value >> 8;
1055 c[2] = value >> 16;
1056 c[3] = value >> 24;
1057 c[4] = value >> 32;
1058 }
1059 else
1060 {
1061 c[0] = value >> 32;
1062 c[1] = value >> 24;
1063 c[2] = value >> 16;
1064 c[3] = value >> 8;
1065 c[4] = value;
1066 }
1067 break;
1068
1069 case 48:
1070 if (endian == IOS_ENDIAN_LSB)
1071 {
1072 c[0] = value;
1073 c[1] = value >> 8;
1074 c[2] = value >> 16;
1075 c[3] = value >> 24;
1076 c[4] = value >> 32;
1077 c[5] = value >> 40;
1078 }
1079 else
1080 {
1081 c[0] = value >> 40;
1082 c[1] = value >> 32;
1083 c[2] = value >> 24;
1084 c[3] = value >> 16;
1085 c[4] = value >> 8;
1086 c[5] = value;
1087 }
1088 break;
1089
1090 case 56:
1091 if (endian == IOS_ENDIAN_LSB)
1092 {
1093 c[0] = value;
1094 c[1] = value >> 8;
1095 c[2] = value >> 16;
1096 c[3] = value >> 24;
1097 c[4] = value >> 32;
1098 c[5] = value >> 40;
1099 c[6] = value >> 48;
1100 }
1101 else
1102 {
1103 c[0] = value >> 48;
1104 c[1] = value >> 40;
1105 c[2] = value >> 32;
1106 c[3] = value >> 24;
1107 c[4] = value >> 16;
1108 c[5] = value >> 8;
1109 c[6] = value;
1110 }
1111 break;
1112
1113 case 64:
1114 if (endian == IOS_ENDIAN_LSB)
1115 {
1116 c[0] = value;
1117 c[1] = value >> 8;
1118 c[2] = value >> 16;
1119 c[3] = value >> 24;
1120 c[4] = value >> 32;
1121 c[5] = value >> 40;
1122 c[6] = value >> 48;
1123 c[7] = value >> 56;
1124 }
1125 else
1126 {
1127 c[0] = value >> 56;
1128 c[1] = value >> 48;
1129 c[2] = value >> 40;
1130 c[3] = value >> 32;
1131 c[4] = value >> 24;
1132 c[5] = value >> 16;
1133 c[6] = value >> 8;
1134 c[7] = value;
1135 }
1136 break;
1137
1138 default:
1139 assert (0);
1140 break;
1141 }
1142
1143 if (io->dev_if->pwrite (io->dev, c, bits / 8, offset / 8) == IOD_EOF)
1144 return IOS_EIOFF;
1145 return IOS_OK;
1146 }
1147
1148 static inline int
ios_write_int_common(ios io,ios_off offset,int flags,int bits,enum ios_endian endian,uint64_t value)1149 ios_write_int_common (ios io, ios_off offset, int flags,
1150 int bits,
1151 enum ios_endian endian,
1152 uint64_t value)
1153 {
1154 /* 64 bits might span at most 9 bytes. */
1155 uint8_t c[9] = {0, 0, 0, 0, 0, 0, 0, 0, 0};
1156
1157 /* Number of signifcant bits in the first byte. */
1158 int firstbyte_bits = 8 - (offset % 8);
1159
1160 /* (Total number of bytes that need to be read) - 1. */
1161 int bytes_minus1 = (bits - firstbyte_bits + 7) / 8;
1162
1163 /* Number of significant bits in the last byte. */
1164 int lastbyte_bits = (bits + (offset % 8)) % 8;
1165 lastbyte_bits = lastbyte_bits == 0 ? 8 : lastbyte_bits;
1166
1167 switch (bytes_minus1)
1168 {
1169 case 0:
1170 {
1171 /* We are altering only a single byte. */
1172 uint64_t head, tail;
1173 IOS_GET_C_ERR_CHCK(head, io, offset / 8);
1174 tail = head;
1175 IOS_CHAR_GET_MSB(&head, offset % 8);
1176 IOS_CHAR_GET_LSB(&tail, 8 - lastbyte_bits);
1177
1178 /* Write the byte back without changing the surrounding bits. */
1179 c[0] = head | tail | (value << (8 - lastbyte_bits));
1180 IOS_PUT_C_ERR_CHCK(c, io, 1, offset / 8);
1181 return IOS_OK;
1182 }
1183
1184 case 1:
1185 /* Correctly set the unmodified leading bits of the first byte. */
1186 IOS_GET_C_ERR_CHCK(c[0], io, offset / 8);
1187 IOS_CHAR_GET_MSB(&c[0], offset % 8);
1188 /* Correctly set the unmodified trailing bits of the last byte. */
1189 IOS_GET_C_ERR_CHCK(c[bytes_minus1], io, offset / 8 + 1);
1190 IOS_CHAR_GET_LSB(&c[bytes_minus1], 8 - lastbyte_bits);
1191
1192 if (endian == IOS_ENDIAN_LSB && bits > 8)
1193 {
1194 /* Convert to the little endian format. For example a 12-bit-long
1195 number's bits get reordered as 7-6-5-4-3-2-1-0-11-10-9-8
1196 with leading 0s. */
1197 value = ((value & 0xff) << (bits % 8))
1198 | (value & 0xff00) >> 8;
1199 }
1200 c[0] |= value >> lastbyte_bits;
1201 c[1] |= (value << (8 - lastbyte_bits)) & 0xff;
1202 IOS_PUT_C_ERR_CHCK(c, io, 2, offset / 8);
1203 return IOS_OK;
1204
1205 case 2:
1206 /* Correctly set the unmodified leading bits of the first byte. */
1207 IOS_GET_C_ERR_CHCK(c[0], io, offset / 8);
1208 IOS_CHAR_GET_MSB(&c[0], offset % 8);
1209 /* Correctly set the unmodified trailing bits of the last byte. */
1210 IOS_GET_C_ERR_CHCK(c[bytes_minus1], io, offset / 8 + bytes_minus1);
1211 IOS_CHAR_GET_LSB(&c[bytes_minus1], 8 - lastbyte_bits);
1212
1213 if (endian == IOS_ENDIAN_LSB)
1214 {
1215 /* Convert to the little endian format. For example a 12-bit-long
1216 number's bits get reordered as 7-6-5-4-3-2-1-0-11-10-9-8
1217 with leading 0s. */
1218 if (bits <= 16)
1219 value = ((value & 0xff) << (bits % 8))
1220 | (value & 0xff00) >> 8;
1221 else
1222 value = ((value & 0xff) << (8 + bits % 8))
1223 | (value & 0xff00) >> (8 - bits % 8)
1224 | (value & 0xff0000) >> 16;
1225 }
1226 c[0] |= value >> (8 + lastbyte_bits);
1227 c[1] = (value >> lastbyte_bits) & 0xff;
1228 c[2] |= (value << (8 - lastbyte_bits)) & 0xff;
1229 IOS_PUT_C_ERR_CHCK(c, io, 3, offset / 8);
1230 return IOS_OK;
1231
1232 case 3:
1233 /* Correctly set the unmodified leading bits of the first byte. */
1234 IOS_GET_C_ERR_CHCK(c[0], io, offset / 8);
1235 IOS_CHAR_GET_MSB(&c[0], offset % 8);
1236 /* Correctly set the unmodified trailing bits of the last byte. */
1237 IOS_GET_C_ERR_CHCK(c[bytes_minus1], io, offset / 8 + bytes_minus1);
1238 IOS_CHAR_GET_LSB(&c[bytes_minus1], 8 - lastbyte_bits);
1239
1240 if (endian == IOS_ENDIAN_LSB)
1241 {
1242 /* Convert to the little endian format. For example a 12-bit-long
1243 number's bits get reordered as 7-6-5-4-3-2-1-0-11-10-9-8
1244 with leading 0s. */
1245 if (bits <= 24)
1246 value = ((value & 0xff) << (8 + bits % 8))
1247 | (value & 0xff00) >> (8 - bits % 8)
1248 | (value & 0xff0000) >> 16;
1249 else
1250 value = ((value & 0xff) << (16 + bits % 8))
1251 | (value & 0xff00) << (bits % 8)
1252 | (value & 0xff0000) >> (16 - bits % 8)
1253 | (value & 0xff000000) >> 24;
1254 }
1255 c[0] |= value >> (16 + lastbyte_bits);
1256 c[1] = (value >> (8 + lastbyte_bits)) & 0xff;
1257 c[2] = (value >> lastbyte_bits) & 0xff;
1258 c[3] |= (value << (8 - lastbyte_bits)) & 0xff;
1259 IOS_PUT_C_ERR_CHCK(c, io, 4, offset / 8);
1260 return IOS_OK;
1261
1262 case 4:
1263 /* Correctly set the unmodified leading bits of the first byte. */
1264 IOS_GET_C_ERR_CHCK(c[0], io, offset / 8);
1265 IOS_CHAR_GET_MSB(&c[0], offset % 8);
1266 /* Correctly set the unmodified trailing bits of the last byte. */
1267 IOS_GET_C_ERR_CHCK(c[bytes_minus1], io, offset / 8 + bytes_minus1);
1268 IOS_CHAR_GET_LSB(&c[bytes_minus1], 8 - lastbyte_bits);
1269
1270 if (endian == IOS_ENDIAN_LSB)
1271 {
1272 /* Convert to the little endian format. For example a 12-bit-long
1273 number's bits get reordered as 7-6-5-4-3-2-1-0-11-10-9-8
1274 with leading 0s. */
1275 if (bits <= 32)
1276 value = ((value & 0xff) << (16 + bits % 8))
1277 | (value & 0xff00) << (bits % 8)
1278 | (value & 0xff0000) >> (16 - bits % 8)
1279 | (value & 0xff000000) >> 24;
1280 else
1281 value = ((value & 0xff) << (24 + bits % 8))
1282 | (value & 0xff00) << (8 + bits % 8)
1283 | (value & 0xff0000) >> (8 - bits % 8)
1284 | (value & 0xff000000) >> (24 - bits % 8)
1285 | (value & 0xff00000000) >> 32;
1286 }
1287 c[0] |= value >> (24 + lastbyte_bits);
1288 c[1] = (value >> (16 + lastbyte_bits)) & 0xff;
1289 c[2] = (value >> (8 + lastbyte_bits)) & 0xff;
1290 c[3] = (value >> lastbyte_bits) & 0xff;
1291 c[4] |= (value << (8 - lastbyte_bits)) & 0xff;
1292 IOS_PUT_C_ERR_CHCK(c, io, 5, offset / 8);
1293 return IOS_OK;
1294
1295 case 5:
1296 /* Correctly set the unmodified leading bits of the first byte. */
1297 IOS_GET_C_ERR_CHCK(c[0], io, offset / 8);
1298 IOS_CHAR_GET_MSB(&c[0], offset % 8);
1299 /* Correctly set the unmodified trailing bits of the last byte. */
1300 IOS_GET_C_ERR_CHCK(c[bytes_minus1], io, offset / 8 + bytes_minus1);
1301 IOS_CHAR_GET_LSB(&c[bytes_minus1], 8 - lastbyte_bits);
1302
1303 if (endian == IOS_ENDIAN_LSB)
1304 {
1305 /* Convert to the little endian format. For example a 12-bit-long
1306 number's bits get reordered as 7-6-5-4-3-2-1-0-11-10-9-8
1307 with leading 0s. */
1308 if (bits <= 40)
1309 value = ((value & 0xff) << (24 + bits % 8))
1310 | (value & 0xff00) << (8 + bits % 8)
1311 | (value & 0xff0000) >> (8 - bits % 8)
1312 | (value & 0xff000000) >> (24 - bits % 8)
1313 | (value & 0xff00000000) >> 32;
1314 else
1315 value = ((value & 0xff) << (32 + bits % 8))
1316 | (value & 0xff00) << (16 + bits % 8)
1317 | (value & 0xff0000) << (bits % 8)
1318 | (value & 0xff000000) >> (16 - bits % 8)
1319 | (value & 0xff00000000) >> (32 - bits % 8)
1320 | (value & 0xff0000000000) >> 40;
1321 }
1322 c[0] |= value >> (32 + lastbyte_bits);
1323 c[1] = (value >> (24 + lastbyte_bits)) & 0xff;
1324 c[2] = (value >> (16 + lastbyte_bits)) & 0xff;
1325 c[3] = (value >> (8 + lastbyte_bits)) & 0xff;
1326 c[4] = (value >> lastbyte_bits) & 0xff;
1327 c[5] |= (value << (8 - lastbyte_bits)) & 0xff;
1328 IOS_PUT_C_ERR_CHCK(c, io, 6, offset / 8);
1329 return IOS_OK;
1330
1331 case 6:
1332 /* Correctly set the unmodified leading bits of the first byte. */
1333 IOS_GET_C_ERR_CHCK(c[0], io, offset / 8);
1334 IOS_CHAR_GET_MSB(&c[0], offset % 8);
1335 /* Correctly set the unmodified trailing bits of the last byte. */
1336 IOS_GET_C_ERR_CHCK(c[bytes_minus1], io, offset / 8 + bytes_minus1);
1337 IOS_CHAR_GET_LSB(&c[bytes_minus1], 8 - lastbyte_bits);
1338
1339 if (endian == IOS_ENDIAN_LSB)
1340 {
1341 /* Convert to the little endian format. For example a 12-bit-long
1342 number's bits get reordered as 7-6-5-4-3-2-1-0-11-10-9-8
1343 with leading 0s. */
1344 if (bits <= 48)
1345 value = ((value & 0xff) << (32 + bits % 8))
1346 | (value & 0xff00) << (16 + bits % 8)
1347 | (value & 0xff0000) << (bits % 8)
1348 | (value & 0xff000000) >> (16 - bits % 8)
1349 | (value & 0xff00000000) >> (32 - bits % 8)
1350 | (value & 0xff0000000000) >> 40;
1351 else
1352 value = ((value & 0xff) << (40 + bits % 8))
1353 | (value & 0xff00) << (24 + bits % 8)
1354 | (value & 0xff0000) << (8 + bits % 8)
1355 | (value & 0xff000000) >> (8 - bits % 8)
1356 | (value & 0xff00000000) >> (24 - bits % 8)
1357 | (value & 0xff0000000000) >> (40 - bits % 8)
1358 | (value & 0xff000000000000) >> 48;
1359 }
1360 c[0] |= value >> (40 + lastbyte_bits);
1361 c[1] = (value >> (32 + lastbyte_bits)) & 0xff;
1362 c[2] = (value >> (24 + lastbyte_bits)) & 0xff;
1363 c[3] = (value >> (16 + lastbyte_bits)) & 0xff;
1364 c[4] = (value >> (8 + lastbyte_bits)) & 0xff;
1365 c[5] = (value >> lastbyte_bits) & 0xff;
1366 c[6] |= (value << (8 - lastbyte_bits)) & 0xff;
1367 IOS_PUT_C_ERR_CHCK(c, io, 7, offset / 8);
1368 return IOS_OK;
1369
1370 case 7:
1371 /* Correctly set the unmodified leading bits of the first byte. */
1372 IOS_GET_C_ERR_CHCK(c[0], io, offset / 8);
1373 IOS_CHAR_GET_MSB(&c[0], offset % 8);
1374 /* Correctly set the unmodified trailing bits of the last byte. */
1375 IOS_GET_C_ERR_CHCK(c[bytes_minus1], io, offset / 8 + bytes_minus1);
1376 IOS_CHAR_GET_LSB(&c[bytes_minus1], 8 - lastbyte_bits);
1377
1378 if (endian == IOS_ENDIAN_LSB)
1379 {
1380 /* Convert to the little endian format. For example a 12-bit-long
1381 number's bits get reordered as 7-6-5-4-3-2-1-0-11-10-9-8
1382 with leading 0s. */
1383 if (bits <= 56)
1384 value = ((value & 0xff) << (40 + bits % 8))
1385 | (value & 0xff00) << (24 + bits % 8)
1386 | (value & 0xff0000) << (8 + bits % 8)
1387 | (value & 0xff000000) >> (8 - bits % 8)
1388 | (value & 0xff00000000) >> (24 - bits % 8)
1389 | (value & 0xff0000000000) >> (40 - bits % 8)
1390 | (value & 0xff000000000000) >> 48;
1391 else
1392 value = ((value & 0xff) << (48 + bits % 8))
1393 | (value & 0xff00) << (32 + bits % 8)
1394 | (value & 0xff0000) << (16 + bits % 8)
1395 | (value & 0xff000000) << (bits % 8)
1396 | (value & 0xff00000000) >> (16 - bits % 8)
1397 | (value & 0xff0000000000) >> (32 - bits % 8)
1398 | (value & 0xff000000000000) >> (48 - bits % 8)
1399 | (value & 0xff00000000000000) >> 56;
1400 }
1401 c[0] |= value >> (48 + lastbyte_bits);
1402 c[1] = (value >> (40 + lastbyte_bits)) & 0xff;
1403 c[2] = (value >> (32 + lastbyte_bits)) & 0xff;
1404 c[3] = (value >> (24 + lastbyte_bits)) & 0xff;
1405 c[4] = (value >> (16 + lastbyte_bits)) & 0xff;
1406 c[5] = (value >> (8 + lastbyte_bits)) & 0xff;
1407 c[6] = (value >> lastbyte_bits) & 0xff;
1408 c[7] |= (value << (8 - lastbyte_bits)) & 0xff;
1409 IOS_PUT_C_ERR_CHCK(c, io, 8, offset / 8);
1410 return IOS_OK;
1411
1412 case 8:
1413 /* Correctly set the unmodified leading bits of the first byte. */
1414 IOS_GET_C_ERR_CHCK(c[0], io, offset / 8);
1415 IOS_CHAR_GET_MSB(&c[0], offset % 8);
1416 /* Correctly set the unmodified trailing bits of the last byte. */
1417 IOS_GET_C_ERR_CHCK(c[bytes_minus1], io, offset / 8 + bytes_minus1);
1418 IOS_CHAR_GET_LSB(&c[bytes_minus1], 8 - lastbyte_bits);
1419
1420 if (endian == IOS_ENDIAN_LSB)
1421 {
1422 /* Convert to the little endian format. For example a 12-bit-long
1423 number's bits get reordered as 7-6-5-4-3-2-1-0-11-10-9-8
1424 with leading 0s. */
1425 value = ((value & 0xff) << (48 + bits % 8))
1426 | (value & 0xff00) << (32 + bits % 8)
1427 | (value & 0xff0000) << (16 + bits % 8)
1428 | (value & 0xff000000) << (bits % 8)
1429 | (value & 0xff00000000) >> (16 - bits % 8)
1430 | (value & 0xff0000000000) >> (32 - bits % 8)
1431 | (value & 0xff000000000000) >> (48 - bits % 8)
1432 | (value & 0xff00000000000000) >> 56;
1433 }
1434 c[0] |= value >> (56 + lastbyte_bits);
1435 c[1] = (value >> (48 + lastbyte_bits)) & 0xff;
1436 c[2] = (value >> (40 + lastbyte_bits)) & 0xff;
1437 c[3] = (value >> (32 + lastbyte_bits)) & 0xff;
1438 c[4] = (value >> (24 + lastbyte_bits)) & 0xff;
1439 c[5] = (value >> (16 + lastbyte_bits)) & 0xff;
1440 c[6] = (value >> (8 + lastbyte_bits)) & 0xff;
1441 c[7] = (value >> lastbyte_bits) & 0xff;
1442 c[8] |= (value << (8 - lastbyte_bits)) & 0xff;
1443 IOS_PUT_C_ERR_CHCK(c, io, 9, offset / 8);
1444 return IOS_OK;
1445
1446 default:
1447 assert (0);
1448 }
1449 }
1450
1451 int
ios_write_int(ios io,ios_off offset,int flags,int bits,enum ios_endian endian,enum ios_nenc nenc,int64_t value)1452 ios_write_int (ios io, ios_off offset, int flags,
1453 int bits,
1454 enum ios_endian endian,
1455 enum ios_nenc nenc,
1456 int64_t value)
1457 {
1458 /* Apply the IOS bias. */
1459 offset += ios_get_bias (io);
1460
1461 /* Fast track for byte-aligned 8x bits */
1462 if (offset % 8 == 0 && bits % 8 == 0)
1463 return ios_write_int_fast (io, offset, flags, bits, endian, value);
1464
1465 /* Shift the sign bit right. */
1466 int unused_bits = 64 - bits;
1467 uint64_t uvalue = ((uint64_t) (value << unused_bits)) >> unused_bits;
1468
1469 /* Fall into the case for the unaligned and the sizes other than 8x. */
1470 return ios_write_int_common (io, offset, flags, bits, endian, uvalue);
1471 }
1472
1473 int
ios_write_uint(ios io,ios_off offset,int flags,int bits,enum ios_endian endian,uint64_t value)1474 ios_write_uint (ios io, ios_off offset, int flags,
1475 int bits,
1476 enum ios_endian endian,
1477 uint64_t value)
1478 {
1479 /* Apply the IOS bias. */
1480 offset += ios_get_bias (io);
1481
1482 /* Fast track for byte-aligned 8x bits */
1483 if (offset % 8 == 0 && bits % 8 == 0)
1484 return ios_write_int_fast (io, offset, flags, bits, endian, value);
1485
1486 /* Fall into the case for the unaligned and the sizes other than 8x. */
1487 return ios_write_int_common (io, offset, flags, bits, endian, value);
1488 }
1489
1490 int
ios_write_string(ios io,ios_off offset,int flags,const char * value)1491 ios_write_string (ios io, ios_off offset, int flags,
1492 const char *value)
1493 {
1494 const char *p;
1495
1496 /* Apply the IOS bias. */
1497 offset += ios_get_bias (io);
1498
1499 if (offset % 8 == 0)
1500 {
1501 /* This is the fast case: we want to write a string at a
1502 byte-boundary. Just write the bytes to the IOD. */
1503
1504 p = value;
1505 do
1506 {
1507 if (io->dev_if->pwrite (io->dev, p, 1,
1508 offset / 8 + p - value) == IOD_EOF)
1509 return IOS_EIOFF;
1510 }
1511 while (*(p++) != '\0');
1512 }
1513 else
1514 {
1515 /* We want to write the string in an offset that is not aligned
1516 to a byte. Instead of writing bytes to the IOD, use the IOS
1517 to write 8-byte unsigned integers instead. */
1518
1519 p = value;
1520 do
1521 {
1522 int ret = ios_write_uint (io, offset, flags, 8,
1523 IOS_ENDIAN_MSB, /* Arbitrary. */
1524 (uint64_t) *p);
1525 if (ret == IOS_EIOFF)
1526 return ret;
1527
1528 offset += 8;
1529 }
1530 while (*(p++) != '\0');
1531
1532 }
1533
1534 return IOS_OK;
1535 }
1536
1537 uint64_t
ios_size(ios io)1538 ios_size (ios io)
1539 {
1540 return io->dev_if->size (io->dev) * 8;
1541 }
1542
1543 int
ios_flush(ios io,ios_off offset)1544 ios_flush (ios io, ios_off offset)
1545 {
1546 return io->dev_if->flush (io->dev, offset / 8);
1547 }
1548