linux-2.6.15_ide-gtm-stm.diff

   1 diff -ur linux-2.6.15-orig/drivers/ide/ide.c linux-2.6.15/drivers/ide/ide.c
   2 --- linux-2.6.15-orig/drivers/ide/ide.c 2006-01-05 20:42:35.000000000 +0100
   3 +++ linux-2.6.15/drivers/ide/ide.c      2006-01-05 20:20:35.000000000 +0100
   4 @@ -1214,6 +1214,237 @@
   5
   6  EXPORT_SYMBOL(system_bus_clock);
   7
   8 +#if 1
   9 +#include <linux/acpi.h>
  10 +#define DBG(x...) printk(x)
  11 +static int ide_acpi_find_device(struct device *dev, acpi_handle *handle)
  12 +{
  13 +       int i, tmp;
  14 +       acpi_integer addr;
  15 +
  16 +       if (sscanf(dev->bus_id, "%u.%u", &tmp, &i) != 2)
  17 +               return -ENODEV;
  18 +
  19 +       addr = i;
  20 +       *handle = acpi_get_child(DEVICE_ACPI_HANDLE(dev->parent), addr);
  21 +       if (!*handle)
  22 +               return -ENODEV;
  23 +       return 0;
  24 +}
  25 +
  26 +/* This assumes the ide controller is a PCI device */
  27 +static int ide_acpi_find_channel(struct device *dev, acpi_handle *handle)
  28 +{
  29 +       int num;
  30 +       int channel;
  31 +       acpi_integer addr;
  32 +
  33 +       num = sscanf(dev->bus_id, "ide%x", &channel);
  34 +
  35 +       if (num != 1 || !dev->parent)
  36 +               return -ENODEV;
  37 +       addr = channel;
  38 +       *handle = acpi_get_child(DEVICE_ACPI_HANDLE(dev->parent), addr);
  39 +       if (!*handle)
  40 +               return -ENODEV;
  41 +       return 0;
  42 +}
  43 +
  44 +static struct acpi_bus_type ide_acpi_bus = {
  45 +       .bus = &ide_bus_type,
  46 +       .find_device = ide_acpi_find_device,
  47 +       .find_bridge = ide_acpi_find_channel,
  48 +};
  49 +
  50 +static int __init ide_acpi_init(void)
  51 +{
  52 +       return register_acpi_bus_type(&ide_acpi_bus);
  53 +}
  54 +
  55 +#define MAX_DEVICES 10
  56 +#define GTM_LEN (sizeof(u32) * 5)
  57 +static struct acpi_ide_stat {
  58 +       acpi_handle handle; /* channel device"s handle */
  59 +       u32     gtm[GTM_LEN/sizeof(u32)]; /* info from _GTM */
  60 +       struct hd_driveid id_buff[2];
  61 +       int channel_handled;
  62 +} device_state[MAX_DEVICES];
  63 +
  64 +static struct acpi_ide_stat *ide_get_acpi_state(acpi_handle handle)
  65 +{
  66 +       int i;
  67 +       for (i = 0; i < MAX_DEVICES; i ++)
  68 +               if (device_state[i].handle == handle)
  69 +                       break;
  70 +       if (i < MAX_DEVICES)
  71 +               return &device_state[i];
  72 +       for (i = 0; i < MAX_DEVICES; i ++)
  73 +               if (device_state[i].handle == NULL)
  74 +                       break;
  75 +       if (i >= MAX_DEVICES)
  76 +               return NULL;
  77 +
  78 +       memset(&device_state[i], 0, sizeof(struct acpi_ide_stat));
  79 +       return &device_state[i];
  80 +}
  81 +
  82 +int acpi_ide_suspend(struct device *dev)
  83 +{
  84 +       acpi_handle handle, parent_handle;
  85 +       struct acpi_ide_stat *stat;
  86 +       acpi_status     status;
  87 +       struct acpi_buffer buffer = {ACPI_ALLOCATE_BUFFER, NULL};
  88 +       union acpi_object *package;
  89 +       ide_drive_t *drive = dev->driver_data;
  90 +       int drive_id = 0;
  91 +
  92 +       handle = DEVICE_ACPI_HANDLE(dev);
  93 +       if (!handle) {
  94 +               DBG("IDE device ACPI handler is NULL\n");
  95 +               return -ENODEV;
  96 +       }
  97 +       if (ACPI_FAILURE(acpi_get_parent(handle, &parent_handle))) {
  98 +               printk(KERN_ERR "ACPI get parent handler error\n");
  99 +               return -ENODEV;
 100 +       }
 101 +       stat = ide_get_acpi_state(parent_handle);
 102 +       if (stat == NULL)
 103 +               return -ENODEV;
 104 +       if (stat->channel_handled) {
 105 +               drive_id = 1;
 106 +               goto id;
 107 +       }
 108 +
 109 +       status = acpi_evaluate_object(parent_handle, "_GTM", NULL, &buffer);
 110 +       if (ACPI_FAILURE(status)) {
 111 +               printk(KERN_ERR "Error evaluating _GTM\n");
 112 +               return -ENODEV;
 113 +       }
 114 +       package = (union acpi_object *) buffer.pointer;
 115 +       if (package->buffer.length != GTM_LEN) {
 116 +               printk(KERN_ERR "Buffer length returned by _GTM is wrong\n");
 117 +               acpi_os_free(buffer.pointer);
 118 +               return -ENODEV;
 119 +       }
 120 +       memcpy(stat->gtm, package->buffer.pointer, GTM_LEN);
 121 +       stat->handle = parent_handle;
 122 +       stat->channel_handled = 1;
 123 +       acpi_os_free(buffer.pointer);
 124 +id:
 125 +       taskfile_lib_get_identify(drive, &stat->id_buff[drive_id]);
 126 +       DBG("GTM info %x,%x,%x,%x,%x\n", stat->gtm[0],
 127 +               stat->gtm[1], stat->gtm[2],
 128 +               stat->gtm[3], stat->gtm[4]);
 129 +       return 0;
 130 +}
 131 +
 132 +static int acpi_ide_stm(struct acpi_ide_stat *stat)
 133 +{
 134 +       struct acpi_object_list input;
 135 +       union acpi_object params[3];
 136 +       acpi_status status;
 137 +
 138 +       input.count = 3;
 139 +       input.pointer = params;
 140 +       params[0].type = ACPI_TYPE_BUFFER;
 141 +       params[0].buffer.length = sizeof(stat->gtm);
 142 +       params[0].buffer.pointer = (char*)stat->gtm;
 143 +
 144 +       params[1].type = ACPI_TYPE_BUFFER;
 145 +       params[1].buffer.length = sizeof(stat->id_buff[0]);
 146 +       params[1].buffer.pointer = (char *)&stat->id_buff[0];
 147 +
 148 +       params[2].type = ACPI_TYPE_BUFFER;
 149 +       params[2].buffer.length = sizeof(stat->id_buff[1]);
 150 +       params[2].buffer.pointer = (char *)&stat->id_buff[1];
 151 +
 152 +       status = acpi_evaluate_object(stat->handle, "_STM", &input, NULL);
 153 +       if (ACPI_FAILURE(status)) {
 154 +               printk(KERN_ERR "Evaluating _STM error\n");
 155 +               return -ENODEV;
 156 +       }
 157 +       return 0;
 158 +}
 159 +
 160 +static int acpi_ide_gtf(acpi_handle handle, ide_drive_t *drive)
 161 +{
 162 +       struct acpi_buffer      output = {ACPI_ALLOCATE_BUFFER, NULL};
 163 +       ide_task_t      args;
 164 +       int index = 0;
 165 +       unsigned char *data;
 166 +       union acpi_object       *package = NULL;
 167 +       acpi_status status;
 168 +
 169 +       status = acpi_evaluate_object(handle, "_GTF", NULL, &output);
 170 +       if (ACPI_FAILURE(status)) {
 171 +               printk(KERN_ERR "evaluate _GTF error\n");
 172 +               return -ENODEV;
 173 +       }
 174 +       package = (union acpi_object *) output.pointer;
 175 +       if (package->type != ACPI_TYPE_BUFFER
 176 +               || (package->buffer.length % 7) != 0) {
 177 +               acpi_os_free(output.pointer);
 178 +               printk(KERN_ERR "_GTF returned value is wrong\n");
 179 +               return -ENODEV;
 180 +       }
 181 +       printk("start GTF\n");
 182 +
 183 +       data = package->buffer.pointer;
 184 +       while (index < package->buffer.length) {
 185 +               memset(&args, 0, sizeof(ide_task_t));
 186 +               args.tfRegister[IDE_ERROR_OFFSET] = data[index];
 187 +               args.tfRegister[IDE_NSECTOR_OFFSET] = data[index + 1];
 188 +               args.tfRegister[IDE_SECTOR_OFFSET] = data[index + 2];
 189 +               args.tfRegister[IDE_LCYL_OFFSET] = data[index + 3];
 190 +               args.tfRegister[IDE_HCYL_OFFSET] = data[index + 4];
 191 +               args.tfRegister[IDE_SELECT_OFFSET] = data[index + 5];
 192 +               args.tfRegister[IDE_STATUS_OFFSET] = data[index + 6];
 193 +               args.command_type = IDE_DRIVE_TASK_NO_DATA;
 194 +               args.handler = &task_no_data_intr;
 195 +               printk("data %x,%x,%x,%x,%x,%x,%x\n",
 196 +                       data[index], data[index+1], data[index+2],
 197 +                       data[index+3],data[index+4],data[index+5],
 198 +                       data[index+6]);
 199 +               /* submit command request */
 200 +//             printk("return value %d\n", ide_raw_taskfile(drive, &args, NULL));
 201 +               index += 7;
 202 +       }
 203 +       acpi_os_free(output.pointer);
 204 +       return 0;
 205 +}
 206 +
 207 +int acpi_ide_resume(struct device *dev)
 208 +{
 209 +       acpi_handle handle, parent_handle;
 210 +       struct acpi_ide_stat *stat;
 211 +       ide_drive_t *drive = dev->driver_data;
 212 +
 213 +       handle = DEVICE_ACPI_HANDLE(dev);
 214 +       if (!handle) {
 215 +               DBG("IDE device ACPI handler is NULL\n");
 216 +               return -ENODEV;
 217 +       }
 218 +       if (ACPI_FAILURE(acpi_get_parent(handle, &parent_handle))) {
 219 +               printk(KERN_ERR "ACPI get parent handler error\n");
 220 +               return -ENODEV;
 221 +       }
 222 +       stat = ide_get_acpi_state(parent_handle);
 223 +       if (stat == NULL || stat->handle != parent_handle)
 224 +               return -ENODEV;
 225 +
 226 +       if (stat->channel_handled == 0) {
 227 +               stat->handle = NULL;
 228 +               goto gtf;
 229 +       }
 230 +DBG("Start STM\n");
 231 +       if (acpi_ide_stm(stat))
 232 +               return -ENODEV;
 233 +       stat->channel_handled = 0;
 234 +gtf:
 235 +       return acpi_ide_gtf(handle, drive);
 236 +}
 237 +#endif
 238 +
 239  static int generic_ide_suspend(struct device *dev, pm_message_t state)
 240  {
 241         ide_drive_t *drive = dev->driver_data;
 242 @@ -1230,6 +1461,7 @@
 243         rqpm.pm_step = ide_pm_state_start_suspend;
 244         rqpm.pm_state = state.event;
 245
 246 +       acpi_ide_suspend(dev);
 247         return ide_do_drive_cmd(drive, &rq, ide_wait);
 248  }
 249
 250 @@ -1239,7 +1471,7 @@
 251         struct request rq;
 252         struct request_pm_state rqpm;
 253         ide_task_t args;
 254 -
 255 +       acpi_ide_resume(dev);
 256         memset(&rq, 0, sizeof(rq));
 257         memset(&rqpm, 0, sizeof(rqpm));
 258         memset(&args, 0, sizeof(args));
 259 @@ -1922,6 +2154,7 @@
 260         devfs_mk_dir("ide");
 261         system_bus_speed = ide_system_bus_speed();
 262
 263 +ide_acpi_init();
 264         bus_register(&ide_bus_type);
 265
 266         init_ide_data();