Aligner.c 2.4 KB

12345678910111213141516171819202122232425262728293031323334353637383940414243444546474849505152535455565758596061626364656667686970717273
  1. /* vim: set expandtab ts=4 sw=4: */
  2. /*
  3. * You may redistribute this program and/or modify it under the terms of
  4. * the GNU General Public License as published by the Free Software Foundation,
  5. * either version 3 of the License, or (at your option) any later version.
  6. *
  7. * This program is distributed in the hope that it will be useful,
  8. * but WITHOUT ANY WARRANTY; without even the implied warranty of
  9. * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
  10. * GNU General Public License for more details.
  11. *
  12. * You should have received a copy of the GNU General Public License
  13. * along with this program. If not, see <http://www.gnu.org/licenses/>.
  14. */
  15. #include "interface/Aligner.h"
  16. #include "interface/InterfaceWrapper.h"
  17. #include "util/Bits.h"
  18. #include "util/Identity.h"
  19. #include "wire/Message.h"
  20. struct Aligner_pvt
  21. {
  22. struct Aligner pub;
  23. struct Interface* wrapped;
  24. uint32_t alignmentBytes;
  25. Identity
  26. };
  27. static int isAligned(uint8_t* pointer, uint32_t alignmentBytes)
  28. {
  29. return !( ((uintptr_t)(pointer)) % alignmentBytes );
  30. }
  31. static void alignMessage(struct Message* msg, uint32_t alignmentBytes)
  32. {
  33. if (isAligned(msg->bytes, alignmentBytes)) { return; }
  34. uint8_t* bytes = msg->bytes;
  35. int length = msg->length;
  36. do {
  37. Message_push8(msg, 0, NULL);
  38. } while (!isAligned(msg->bytes, alignmentBytes));
  39. Bits_memmove(msg->bytes, bytes, length);
  40. msg->length = length;
  41. }
  42. static uint8_t receiveMessage(struct Message* msg, struct Interface* iface)
  43. {
  44. struct Aligner_pvt* al = Identity_check((struct Aligner_pvt*)iface->receiverContext);
  45. alignMessage(msg, al->alignmentBytes);
  46. Interface_receiveMessage(&al->pub.generic, msg);
  47. return 0;
  48. }
  49. static uint8_t sendMessage(struct Message* msg, struct Interface* iface)
  50. {
  51. struct Aligner_pvt* al = Identity_check((struct Aligner_pvt*)iface);
  52. alignMessage(msg, al->alignmentBytes);
  53. return Interface_sendMessage(al->wrapped, msg);
  54. }
  55. struct Aligner* Aligner_new(struct Interface* external,
  56. struct Allocator* alloc,
  57. uint32_t alignmentBytes)
  58. {
  59. Assert_true(Bits_popCountx32(alignmentBytes) == 1 && "alignmentBytes must be a power of 2");
  60. struct Aligner_pvt* out = Allocator_clone(alloc, (&(struct Aligner_pvt) {
  61. .wrapped = external,
  62. .alignmentBytes = alignmentBytes
  63. }));
  64. Identity_set(out);
  65. InterfaceWrapper_wrap(external, sendMessage, receiveMessage, &out->pub.generic);
  66. return &out->pub;
  67. }