add source

1 files changed, 157 insertions, 0 deletions

diff --git a/src/twa.c b/src/twa.c
new file mode 100644
index 0000000..9cd89b2
--- /dev/null
+++ b/src/twa.c
@@ -0,0 +1,157 @@
+/*
+ * twa.c:
+ *
+ * Copyright (c) 2017 James McKenzie <foss@madingley.org>,
+ * All rights reserved.
+ *
+ */
+
+#include "project.h"
+
+ssize_t
+twa_get_parameter (int fd, int table_id, int parameter_id, void *data,
+                   size_t data_len)
+{
+
+  TW_Command *command;
+  TW_Param_Apache *param;
+  TW_Command_Apache_Header *header;
+  TW_Ioctl_Buf_Apache *tw_ioctl_apache;
+  int rc;
+  size_t buffer_len, ret_len;
+  char error[512];
+
+  buffer_len = data_len + sizeof (TW_Param_Apache);
+
+  tw_ioctl_apache = xmalloc (sizeof (TW_Ioctl_Buf_Apache) + buffer_len);
+
+  memset (tw_ioctl_apache, 0, sizeof (TW_Ioctl_Buf_Apache) + buffer_len);
+
+  tw_ioctl_apache->driver_command.control_code =
+    TW_IOCTL_FIRMWARE_PASS_THROUGH;
+  tw_ioctl_apache->driver_command.buffer_length = buffer_len;
+
+  header =
+    (TW_Command_Apache_Header *) & (tw_ioctl_apache->firmware_command.header);
+  header->header_desc.size_header = sizeof (*header);
+
+  command =
+    (TW_Command *) & (tw_ioctl_apache->firmware_command.command.oldcommand);
+
+  command->byte0.opcode = TW_OP_GET_PARAM;
+  command->byte0.sgl_offset = 0x2;
+
+  command->size = TW_COMMAND_SIZE;
+
+  command->byte6.parameter_count = cpu_to_le16 (1);
+
+  param = (TW_Param_Apache *) tw_ioctl_apache->data_buffer;
+
+  param->table_id = cpu_to_le16 (table_id | 0x8000);
+  param->parameter_id = cpu_to_le16 (parameter_id);
+  param->parameter_size_bytes = cpu_to_le16 (data_len);
+
+  rc = ioctl (fd, TW_IOCTL_FIRMWARE_PASS_THROUGH, tw_ioctl_apache);
+
+  if (rc)
+    {
+      fprintf (stderr,
+               "twa_get_parameter: ioctl TW_IOCTL_FIRMWARE_PASS_THROUGH failed: %m\n");
+      return -1;
+    }
+
+  if (header->status_block.error)
+    {
+      memset (error, 0, sizeof (error));
+      memcpy (error, header->err_specific_desc,
+              sizeof (header->err_specific_desc));
+
+      fprintf (stderr,
+               "twa_get_parameter: controller returns error: (0x%x) %s\n",
+               le16_to_cpu (header->status_block.error), error + 1);
+      return -1;
+    }
+
+  ret_len = le16_to_cpu (param->actual_parameter_size_bytes);
+
+  if (ret_len > data_len)
+    {
+      fprintf (stderr,
+               "twa_get_parameter: buffer passed for table 0x%x, parameter 0x%x too short: given %d bytes need %d\n",
+               table_id, parameter_id, data_len, ret_len);
+      return -1;
+    }
+
+  memcpy (data, param->data, ret_len);
+
+  return ret_len;
+}
+
+int
+twa_set_parameter (int fd, int table_id, int parameter_id, void *data,
+                   size_t data_len)
+{
+
+  TW_Command *command;
+  TW_Param_Apache *param;
+  TW_Command_Apache_Header *header;
+  TW_Ioctl_Buf_Apache *tw_ioctl_apache;
+  int rc;
+  size_t buffer_len, ret_len;
+  char error[512];
+
+  buffer_len = data_len + sizeof (TW_Param_Apache);
+
+  tw_ioctl_apache = xmalloc (sizeof (TW_Ioctl_Buf_Apache) + buffer_len);
+
+  memset (tw_ioctl_apache, 0, sizeof (TW_Ioctl_Buf_Apache) + buffer_len);
+
+  tw_ioctl_apache->driver_command.control_code =
+    TW_IOCTL_FIRMWARE_PASS_THROUGH;
+  tw_ioctl_apache->driver_command.buffer_length = buffer_len;
+
+  header =
+    (TW_Command_Apache_Header *) & (tw_ioctl_apache->firmware_command.header);
+  header->header_desc.size_header = sizeof (*header);
+
+  command =
+    (TW_Command *) & (tw_ioctl_apache->firmware_command.command.oldcommand);
+
+  command->byte0.opcode = TW_OP_SET_PARAM;
+  command->byte0.sgl_offset = 0x2;
+
+  command->size = TW_COMMAND_SIZE;
+
+  command->byte6.parameter_count = cpu_to_le16 (1);
+
+  param = (TW_Param_Apache *) tw_ioctl_apache->data_buffer;
+
+  param->table_id = cpu_to_le16 (table_id | 0x8000);
+  param->parameter_id = cpu_to_le16 (parameter_id);
+  param->parameter_size_bytes = cpu_to_le16 (data_len);
+
+  memcpy (param->data, data, data_len);
+
+  rc = ioctl (fd, TW_IOCTL_FIRMWARE_PASS_THROUGH, tw_ioctl_apache);
+
+  if (rc)
+    {
+      fprintf (stderr,
+               "twa_get_parameter: ioctl TW_IOCTL_FIRMWARE_PASS_THROUGH failed: %m\n");
+      return -1;
+    }
+
+  if (header->status_block.error)
+    {
+      memset (error, 0, sizeof (error));
+      memcpy (error, header->err_specific_desc,
+              sizeof (header->err_specific_desc));
+
+      fprintf (stderr,
+               "twa_get_parameter: controller returns error: (0x%x) %s\n",
+               le16_to_cpu (header->status_block.error), error + 1);
+      return -1;
+    }
+
+  return 0;
+}