1 files changed, 74 insertions, 0 deletions
diff --git a/cpu/cpu-registers.c b/cpu/cpu-registers.c
new file mode 100644
index 0000000..7cd7dee
--- /dev/null
+++ b/cpu/cpu-registers.c
@@ -0,0 +1,74 @@
+#ifdef HAVE_CONFIG_H
+#include <config.h>
+#endif
+#include <libmcpu.h>
+#include <cpu-context.h>
+#include <cpu-registers.h>
+
+__mpu_size_t
+__cpu_reg_size( unsigned int bits )
+{
+  if( bits < 8 || bits > 65536 || ( bits & ( bits - 1 ) ) ) return( 0 );
+  return( (__mpu_size_t)( bits >> 3 ) );
+}
+
+int
+__cpu_reg_valid( unsigned int bits, __mpu_uint32_t a )
+{
+  __mpu_size_t n = __cpu_reg_size( bits );
+  if( !n ) return( 0 );
+  if( ( a & ( n - 1 ) ) != 0 ) return( 0 );
+  if( a > CPU_REGISTER_ADDRESS_MAX ) return( 0 );
+  if( (__mpu_uint64_t)a + n > CPU_REGISTERS_MEM_SIZE_IN_UNITS ) return( 0 );
+  return( 1 );
+}
+
+__mpu_UNIT_t *
+__cpu_reg_ptr( unsigned int bits, __mpu_uint32_t a )
+{
+  return( __cpu_reg_valid( bits, a ) ? &__REGS[a] : (__mpu_UNIT_t *)0 );
+}
+
+int
+__cpu_encode_reg24( __mpu_UNIT_t out[3], const cpu_register *r )
+{
+  unsigned int code = 0, b;
+  if( !out || !r || !__cpu_reg_valid( r->bits, r->address ) ) return( -1 );
+  b = r->bits;
+  while( b > 8 )
+  {
+    b >>= 1;
+    ++code;
+  }
+  if( code > 13 ) return( -1 );
+  out[0] = (__mpu_UNIT_t)( ( code << 4 ) | ( ( r->address >> 16 ) & 0x0f ) );
+  out[1] = (__mpu_UNIT_t)( ( r->address >> 8 ) & 0xff );
+  out[2] = (__mpu_UNIT_t)( r->address & 0xff );
+  return( 0 );
+}
+
+int
+__cpu_decode_reg24( cpu_register *r, const __mpu_UNIT_t in[3] )
+{
+  unsigned int code;
+  __mpu_uint32_t a;
+  if( !r || !in ) return( -1 );
+  code = ( in[0] >> 4 ) & 0x0f;
+  if( code > 13 ) return( -1 );
+  a = ( (__mpu_uint32_t)( in[0] & 0x0f ) << 16 ) | ( (__mpu_uint32_t)in[1] << 8 ) | in[2];
+  r->bits = 8U << code;
+  r->address = a;
+  return( __cpu_reg_valid( r->bits, a ) ? 0 : -1 );
+}
+
+int
+__cpu_return_reg( cpu_register *r, unsigned int bits )
+{
+  __mpu_size_t n;
+  if( !r ) return( -1 );
+  n = __cpu_reg_size( bits );
+  if( !n || (__mpu_uint64_t)n > CPU_RETURN_AREA_SIZE ) return( -1 );
+  r->bits = bits;
+  r->address = (__mpu_uint32_t)( CPU_RETURN_AREA_END - (__mpu_uint32_t)n );
+  return( __cpu_reg_valid( r->bits, r->address ) ? 0 : -1 );
+}