1 //
2 // Copyright (c) 2010-2024 Antmicro
3 //
4 //  This file is licensed under the MIT License.
5 //  Full license text is available in 'licenses/MIT.txt'.
6 //
7 using System;
8 using System.Collections.Generic;
9 using System.Linq;
10 using System.Reflection;
11 using Antmicro.Renode.Utilities.Collections;
12 
13 namespace Antmicro.Renode.Utilities.Packets
14 {
15     public class Packet
16     {
CalculateLength()17         public static int CalculateLength<T>()
18         {
19             return CalculateLength(typeof(T));
20         }
21 
CalculateOffset(string fieldName)22         public static int CalculateOffset<T>(string fieldName)
23         {
24             lock(cache)
25             {
26                 return cache.Get(Tuple.Create(typeof(T), fieldName), _ =>
27                 {
28                     var fieldsAndProperties = GetFieldsAndProperties(typeof(T));
29 
30                     var maxOffset = 0;
31                     var offset = 0;
32                     foreach(var element in fieldsAndProperties)
33                     {
34                         if(element.ElementName == fieldName)
35                         {
36                             return maxOffset;
37                         }
38 
39                         if(element.ByteOffset.HasValue)
40                         {
41                             offset = element.ByteOffset.Value;
42                         }
43 
44                         var bytesRequired = element.BytesRequired;
45                         var co = offset + bytesRequired;
46                         maxOffset = Math.Max(co, maxOffset);
47                         offset += bytesRequired;
48                     }
49 
50                     return -1;
51                 });
52             }
53         }
54 
Decode(IList<byte> data, int dataOffset = 0)55         public static T Decode<T>(IList<byte> data, int dataOffset = 0)
56         {
57             if(!TryDecode<T>(data, out var result, dataOffset))
58             {
59                 throw new ArgumentException($"Could not decode the packet of type {typeof(T)} due to insufficient data. Required {Packet.CalculateLength<T>()} bytes, but received {(data.Count - dataOffset)}");
60             }
61             return result;
62         }
63 
TryDecode(IList<byte> data, out T result, int dataOffset = 0)64         public static bool TryDecode<T>(IList<byte> data, out T result, int dataOffset = 0)
65         {
66             var success = TryDecode(typeof(T), data, out var tryResult, dataOffset);
67             result = (T)tryResult;
68             return success;
69         }
70 
71         public static dynamic DecodeDynamic<T>(byte[] data, int dataOffset = 0) where T : class
72         {
73             if(!typeof(T).IsInterface)
74             {
75                 throw new ArgumentException("This can be called on interfaces only");
76             }
77 
78             var result = new DynamicPropertiesObject();
79 
80             var fieldsAndProperties = GetFieldsAndProperties(typeof(T));
81 
82             var offset = dataOffset;
83             foreach(var field in fieldsAndProperties)
84             {
85                 if(field.ByteOffset.HasValue)
86                 {
87                     offset = dataOffset + field.ByteOffset.Value;
88                 }
89 
90                 if(field.ElementType == typeof(uint))
91                 {
92                     var lOffset = offset;
93                     result.ProvideProperty(field.ElementName, getter: () => (uint)((data[lOffset] << 24) | (data[lOffset + 1] << 16) | (data[lOffset + 2] << 8) | (data[lOffset + 3])));
94 
95                     offset += 4;
96                 }
97                 else if(field.ElementType == typeof(ushort))
98                 {
99                     var lOffset = offset;
100                     result.ProvideProperty(field.ElementName, getter: () => (ushort)((data[lOffset] << 16) | (data[lOffset + 1])));
101 
102                     offset += 2;
103                 }
104                 else
105                 {
106                     throw new ArgumentException($"Unsupported field type: {typeof(T).Name}");
107                 }
108             }
109 
110             return (dynamic)result;
111         }
112 
Encode(T packet)113         public static byte[] Encode<T>(T packet)
114         {
115             var size = CalculateLength<T>();
116             var result = new byte[size];
117             if(size == 0)
118             {
119                 return result;
120             }
121 
122             EncodeInner(packet.GetType(), packet, result, 0);
123 
124             return result;
125         }
126 
TryDecode(Type t, IList<byte> data, out object result, int dataOffset = 0)127         private static bool TryDecode(Type t, IList<byte> data, out object result, int dataOffset = 0)
128         {
129             var offset = dataOffset;
130             if(offset < 0)
131             {
132                 throw new ArgumentException("Data offset cannot be less than zero", "dataOffset");
133             }
134 
135             result = Activator.CreateInstance(t);
136 
137             var fieldsAndProperties = GetFieldsAndProperties(t);
138 
139             foreach(var field in fieldsAndProperties)
140             {
141                 var type = field.ElementType;
142                 if(type.IsEnum)
143                 {
144                     type = type.GetEnumUnderlyingType();
145                 }
146 
147                 if(field.ByteOffset.HasValue)
148                 {
149                     offset = dataOffset + field.ByteOffset.Value;
150                 }
151                 var bitOffset = field.BitOffset ?? 0;
152 
153                 if(type == typeof(byte[]))
154                 {
155                     if(bitOffset != 0)
156                     {
157                         throw new ArgumentException("Bit offset for byte array is not supported.");
158                     }
159 
160                     var width = field.Width;
161                     if(offset + width > data.Count)
162                     {
163                         return false;
164                     }
165                     if(width == 0)
166                     {
167                         throw new ArgumentException("Positive width must be provided to decode byte array");
168                     }
169 
170                     var v = new byte[width];
171                     for(var i = 0; i < width; i++)
172                     {
173                         v[i] = data[offset + i];
174                     }
175 
176                     field.SetValue(result, v);
177                     offset += width;
178                     continue;
179                 }
180 
181                 var bytesRequired = field.BytesRequired;
182                 if(offset + bytesRequired > data.Count)
183                 {
184                     return false;
185                 }
186 
187                 if(Misc.IsStructType(type))
188                 {
189                     if (!TryDecode(type, data, out var nestedPacket, offset))
190                     {
191                         return false;
192                     }
193 
194                     field.SetValue(result, nestedPacket);
195                     offset += bytesRequired;
196                     continue;
197                 }
198 
199                 var intermediate = 0UL;
200                 {
201                     var i = 0;
202                     foreach(var b in data.Skip(offset))
203                     {
204                         if(i >= bytesRequired)
205                         {
206                             break;
207                         }
208                         // assume host LSB bit ordering
209                         var shift = (i << 3) - bitOffset;
210                         intermediate |= shift > 0 ? (ulong)b << shift : (ulong)b >> -shift;
211                         i += 1;
212                     }
213                 }
214 
215                 if(type == typeof(int) || type == typeof(uint))
216                 {
217                     var v = (uint)intermediate;
218 
219                     if(!field.IsLSBFirst)
220                     {
221                         v = BitHelper.ReverseBytes(v);
222                     }
223                     v = BitHelper.GetValue(v, 0, field.BitWidth ?? 32);
224                     if(type == typeof(int))
225                     {
226                         field.SetValue(result, (int)v);
227                     }
228                     else
229                     {
230                         field.SetValue(result, v);
231                     }
232                 }
233                 else if(type == typeof(short) || type == typeof(ushort))
234                 {
235                     var v = (ushort)intermediate;
236 
237                     if(!field.IsLSBFirst)
238                     {
239                         v = BitHelper.ReverseBytes(v);
240                     }
241                     v = (ushort)BitHelper.GetValue(v, 0, field.BitWidth ?? 16);
242                     if(type == typeof(short))
243                     {
244                         field.SetValue(result, (short)v);
245                     }
246                     else
247                     {
248                         field.SetValue(result, v);
249                     }
250                 }
251                 else if(type == typeof(byte) || type == typeof(sbyte))
252                 {
253                     var v = (byte)intermediate;
254                     v = BitHelper.GetValue(v, 0, field.BitWidth ?? 8);
255                     if(type == typeof(sbyte))
256                     {
257                         field.SetValue(result, (sbyte)v);
258                     }
259                     else
260                     {
261                         field.SetValue(result, v);
262                     }
263                 }
264                 else if(type == typeof(long) || type == typeof(ulong))
265                 {
266                     var v = intermediate;
267                     if(!field.IsLSBFirst)
268                     {
269                         v = BitHelper.ReverseBytes(v);
270                     }
271                     v = BitHelper.GetValue(v, 0, field.BitWidth ?? 64);
272                     if(type == typeof(long))
273                     {
274                         field.SetValue(result, (long)v);
275                     }
276                     else
277                     {
278                         field.SetValue(result, v);
279                     }
280                 }
281                 else if(type == typeof(bool))
282                 {
283                     field.SetValue(result, BitHelper.IsBitSet(intermediate, 0));
284                 }
285                 else
286                 {
287                     throw new ArgumentException($"Unsupported field type: {type.Name}");
288                 }
289                 offset += bytesRequired;
290             }
291 
292             return true;
293         }
294 
EncodeInner(Type t, object packet, byte[] result, int offset)295         private static int EncodeInner(Type t, object packet, byte[] result, int offset)
296         {
297             var fieldsAndProperties = GetFieldsAndProperties(t);
298             var startingOffset = offset;
299 
300             foreach(var field in fieldsAndProperties)
301             {
302                 var type = field.ElementType;
303                 if(type.IsEnum)
304                 {
305                     type = type.GetEnumUnderlyingType();
306                 }
307 
308                 if(field.ByteOffset.HasValue)
309                 {
310                     offset = startingOffset + field.ByteOffset.Value;
311                 }
312                 var bitOffset = field.BitOffset ?? 0;
313 
314                 if(type == typeof(byte[]))
315                 {
316                     if(bitOffset != 0)
317                     {
318                         throw new ArgumentException("Bit offset for byte array is not supported.");
319                     }
320 
321                     var width = field.Width;
322                     if(width == 0)
323                     {
324                         throw new ArgumentException("Positive width must be provided to decode byte array");
325                     }
326 
327                     var val = (byte[])field.GetValue(packet);
328 
329                     if(val == null)
330                     {
331                         // If field is not defined, assume this field is filled with zeros
332                         val = new byte[width];
333                     }
334 
335                     if(width != val.Length)
336                     {
337                         throw new ArgumentException("Declared and actual width is different: {0} vs {1}".FormatWith(width, val.Length));
338                     }
339 
340                     Array.Copy(val, 0, result, offset, width);
341                     offset += width;
342                     continue;
343                 }
344 
345                 var intermediate = 0UL;
346                 var bitWidth = field.BitWidth ?? 0;
347 
348                 if(type == typeof(int))
349                 {
350                     var v = (int)field.GetValue(packet);
351                     intermediate = field.IsLSBFirst ? (uint)v : BitHelper.ReverseBytes((uint)v);
352                 }
353                 else if(type == typeof(uint))
354                 {
355                     var v = (uint)field.GetValue(packet);
356                     intermediate = field.IsLSBFirst ? v : BitHelper.ReverseBytes(v);
357                 }
358                 else if(type == typeof(short))
359                 {
360                     var v = (short)field.GetValue(packet);
361                     intermediate = field.IsLSBFirst ? (ushort)v : BitHelper.ReverseBytes((ushort)v);
362                 }
363                 else if(type == typeof(ushort))
364                 {
365                     var v = (ushort)field.GetValue(packet);
366                     intermediate = field.IsLSBFirst ? v : BitHelper.ReverseBytes(v);
367                 }
368                 else if(type == typeof(byte))
369                 {
370                     intermediate = (byte)field.GetValue(packet);
371                 }
372                 else if(type == typeof(sbyte))
373                 {
374                     intermediate = (ulong)(sbyte)field.GetValue(packet);
375                 }
376                 else if(type == typeof(long))
377                 {
378                     var v = (long)field.GetValue(packet);
379                     intermediate = field.IsLSBFirst ? (ulong)v : BitHelper.ReverseBytes((ulong)v);
380                 }
381                 else if(type == typeof(ulong))
382                 {
383                     var v = (ulong)field.GetValue(packet);
384                     intermediate = field.IsLSBFirst ? v : BitHelper.ReverseBytes(v);
385                 }
386                 else if(type == typeof(bool))
387                 {
388                     intermediate = (bool)field.GetValue(packet) ? 1UL : 0UL;
389                 }
390                 else if(Misc.IsStructType(type))
391                 {
392                     var nestedPacket = field.GetValue(packet);
393                     offset += EncodeInner(type, nestedPacket, result, offset);
394                     continue;
395                 }
396                 else
397                 {
398                     throw new ArgumentException($"Unsupported field type: {field.ElementType}");
399                 }
400 
401                 // write first byte
402                 result[offset] = result[offset].ReplaceBits((byte)intermediate, Math.Min(8, bitWidth), bitOffset);
403                 bitWidth -= Math.Min(8 - bitOffset, bitWidth);
404                 offset += 1;
405                 intermediate >>= 8 - bitOffset;
406                 // write next full bytes
407                 for(; bitWidth > 8; bitWidth -= 8)
408                 {
409                     result[offset] = (byte)intermediate;
410                     intermediate >>= 8;
411                     offset += 1;
412                 }
413                 // write last non-full byte if exist
414                 if(bitWidth > 0)
415                 {
416                     result[offset] = result[offset].ReplaceBits((byte)intermediate, bitWidth);
417                     offset += 1;
418                 }
419             }
420 
421             return offset - startingOffset;
422         }
423 
CalculateLength(Type t)424         private static int CalculateLength(Type t)
425         {
426             lock(cache)
427             {
428                 return cache.Get(t, _ =>
429                 {
430                     var fieldsAndProperties = GetFieldsAndProperties(t);
431 
432                     var maxOffset = 0;
433                     var offset = 0;
434                     foreach(var element in fieldsAndProperties)
435                     {
436                         var bytesRequired = element.BytesRequired;
437                         offset = element.ByteOffset ?? offset;
438 
439                         var co = offset + bytesRequired;
440                         maxOffset = Math.Max(co, maxOffset);
441                         offset += bytesRequired;
442                     }
443 
444                     return maxOffset;
445                 });
446             }
447         }
448 
GetFieldsAndProperties(Type t)449         private static FieldPropertyInfoWrapper[] GetFieldsAndProperties(Type t)
450         {
451             lock(cache)
452             {
453                 return cache.Get(t, _ =>
454                 {
455                     return t.GetFields()
456                         .Where(x => Attribute.IsDefined(x, typeof(PacketFieldAttribute)))
457                         .Select(x => new FieldPropertyInfoWrapper(x))
458                         .Union(t.GetProperties()
459                             .Where(x => Attribute.IsDefined(x, typeof(PacketFieldAttribute)))
460                             .Select(x => new FieldPropertyInfoWrapper(x))
461                         ).OrderBy(x => x.Order).ToArray();
462                 });
463             }
464         }
465 
466         private static readonly SimpleCache cache = new SimpleCache();
467 
468         private class FieldPropertyInfoWrapper
469         {
FieldPropertyInfoWrapper(FieldInfo info)470             public FieldPropertyInfoWrapper(FieldInfo info)
471             {
472                 fieldInfo = info;
473             }
474 
FieldPropertyInfoWrapper(PropertyInfo info)475             public FieldPropertyInfoWrapper(PropertyInfo info)
476             {
477                 propertyInfo = info;
478             }
479 
GetValue(object o)480             public object GetValue(object o)
481             {
482                 return (fieldInfo != null)
483                     ? fieldInfo.GetValue(o)
484                     : propertyInfo.GetValue(o);
485             }
486 
SetValue(object o, object v)487             public bool SetValue(object o, object v)
488             {
489                 if(fieldInfo != null)
490                 {
491                     fieldInfo.SetValue(o, v);
492                 }
493                 else
494                 {
495                     if(!propertyInfo.CanWrite)
496                     {
497                         return false;
498                     }
499                     propertyInfo.SetValue(o, v);
500                 }
501 
502                 return true;
503             }
504 
GetAttribute()505             public T GetAttribute<T>()
506             {
507                 return (T)((MemberInfo)fieldInfo ?? propertyInfo).GetCustomAttributes(typeof(T), false).FirstOrDefault();
508             }
509 
510             public bool IsLSBFirst => ((MemberInfo)fieldInfo ?? propertyInfo).DeclaringType.GetCustomAttribute<LeastSignificantByteFirst>() != null
511                     || GetAttribute<LeastSignificantByteFirst>() != null;
512 
513             public int? ByteOffset => (int?)GetAttribute<OffsetAttribute>()?.OffsetInBytes;
514 
515             public int? BitOffset => (int?)GetAttribute<OffsetAttribute>()?.OffsetInBits;
516 
517             public int BytesRequired => ((BitOffset ?? 0) + BitWidth + 7) / 8 ?? (Width + (BitOffset > 0 ? 1 : 0));
518 
519             public int Order => GetAttribute<PacketFieldAttribute>().Order;
520 
521             public int Width
522             {
523                 get
524                 {
525                     var type = ElementType;
526                     if(type.IsEnum)
527                     {
528                         type = type.GetEnumUnderlyingType();
529                     }
530 
531                     if(type == typeof(byte) || type == typeof(bool) || type == typeof(sbyte))
532                     {
533                         return 1;
534                     }
535                     if(type == typeof(ushort) || type == typeof(short))
536                     {
537                         return 2;
538                     }
539                     if(type == typeof(uint) || type == typeof(int))
540                     {
541                         return 4;
542                     }
543                     if(type == typeof(ulong) || type == typeof(long))
544                     {
545                         return 8;
546                     }
547                     if(type == typeof(byte[]))
548                     {
549                         return (int)(GetAttribute<WidthAttribute>()?.Value ?? 0);
550                     }
551                     if(Misc.IsStructType(type))
552                     {
553                         return CalculateLength(type);
554                     }
555 
556                     throw new ArgumentException($"Unknown width of type: {type}");
557                 }
558             }
559 
560             public int? BitWidth
561             {
562                 get
563                 {
564                     var type = ElementType;
565                     if(type.IsEnum)
566                     {
567                         type = type.GetEnumUnderlyingType();
568                     }
569 
570                     if(type == typeof(bool))
571                     {
572                         return 1;
573                     }
574 
575                     int inBytes;
576                     if(type == typeof(byte) || type == typeof(sbyte))
577                     {
578                         inBytes = sizeof(byte);
579                     }
580                     else if(type == typeof(ushort) || type == typeof(short))
581                     {
582                         inBytes = sizeof(ushort);
583                     }
584                     else if(type == typeof(uint) || type == typeof(int))
585                     {
586                         inBytes = sizeof(uint);
587                     }
588                     else if(type == typeof(ulong) || type == typeof(long))
589                     {
590                         inBytes = sizeof(ulong);
591                     }
592                     else
593                     {
594                         return null;
595                     }
596 
597                     var width = inBytes << 3;
598                     var setWidth = (int?)GetAttribute<WidthAttribute>()?.Value;
599                     if(setWidth < 0)
600                     {
601                         throw new ArgumentException($"Width is less than zero.");
602                     }
603                     if(setWidth > width)
604                     {
605                         throw new ArgumentException($"Width is greater than the size of type: {type} ({width} bits).");
606                     }
607                     return setWidth ?? width;
608                 }
609             }
610 
611             public Type ElementType => fieldInfo?.FieldType ?? propertyInfo.PropertyType;
612 
613             public string ElementName => fieldInfo?.Name ?? propertyInfo.Name;
614 
615             private readonly FieldInfo fieldInfo;
616             private readonly PropertyInfo propertyInfo;
617         }
618     }
619 }
620