[SPARC64]: Handle multiple domain-services-port nodes properly.
[linux-2.6] / arch / sparc64 / kernel / ds.c
1 /* ds.c: Domain Services driver for Logical Domains
2  *
3  * Copyright (C) 2007 David S. Miller <davem@davemloft.net>
4  */
5
6 #include <linux/kernel.h>
7 #include <linux/module.h>
8 #include <linux/types.h>
9 #include <linux/module.h>
10 #include <linux/string.h>
11 #include <linux/slab.h>
12 #include <linux/sched.h>
13 #include <linux/delay.h>
14 #include <linux/mutex.h>
15 #include <linux/kthread.h>
16 #include <linux/reboot.h>
17 #include <linux/cpu.h>
18
19 #include <asm/ldc.h>
20 #include <asm/vio.h>
21 #include <asm/mdesc.h>
22 #include <asm/head.h>
23 #include <asm/irq.h>
24
25 #define DRV_MODULE_NAME         "ds"
26 #define PFX DRV_MODULE_NAME     ": "
27 #define DRV_MODULE_VERSION      "1.0"
28 #define DRV_MODULE_RELDATE      "Jul 11, 2007"
29
30 static char version[] __devinitdata =
31         DRV_MODULE_NAME ".c:v" DRV_MODULE_VERSION " (" DRV_MODULE_RELDATE ")\n";
32 MODULE_AUTHOR("David S. Miller (davem@davemloft.net)");
33 MODULE_DESCRIPTION("Sun LDOM domain services driver");
34 MODULE_LICENSE("GPL");
35 MODULE_VERSION(DRV_MODULE_VERSION);
36
37 struct ds_msg_tag {
38         __u32                   type;
39 #define DS_INIT_REQ             0x00
40 #define DS_INIT_ACK             0x01
41 #define DS_INIT_NACK            0x02
42 #define DS_REG_REQ              0x03
43 #define DS_REG_ACK              0x04
44 #define DS_REG_NACK             0x05
45 #define DS_UNREG_REQ            0x06
46 #define DS_UNREG_ACK            0x07
47 #define DS_UNREG_NACK           0x08
48 #define DS_DATA                 0x09
49 #define DS_NACK                 0x0a
50
51         __u32                   len;
52 };
53
54 /* Result codes */
55 #define DS_OK                   0x00
56 #define DS_REG_VER_NACK         0x01
57 #define DS_REG_DUP              0x02
58 #define DS_INV_HDL              0x03
59 #define DS_TYPE_UNKNOWN         0x04
60
61 struct ds_version {
62         __u16                   major;
63         __u16                   minor;
64 };
65
66 struct ds_ver_req {
67         struct ds_msg_tag       tag;
68         struct ds_version       ver;
69 };
70
71 struct ds_ver_ack {
72         struct ds_msg_tag       tag;
73         __u16                   minor;
74 };
75
76 struct ds_ver_nack {
77         struct ds_msg_tag       tag;
78         __u16                   major;
79 };
80
81 struct ds_reg_req {
82         struct ds_msg_tag       tag;
83         __u64                   handle;
84         __u16                   major;
85         __u16                   minor;
86         char                    svc_id[0];
87 };
88
89 struct ds_reg_ack {
90         struct ds_msg_tag       tag;
91         __u64                   handle;
92         __u16                   minor;
93 };
94
95 struct ds_reg_nack {
96         struct ds_msg_tag       tag;
97         __u64                   handle;
98         __u16                   major;
99 };
100
101 struct ds_unreg_req {
102         struct ds_msg_tag       tag;
103         __u64                   handle;
104 };
105
106 struct ds_unreg_ack {
107         struct ds_msg_tag       tag;
108         __u64                   handle;
109 };
110
111 struct ds_unreg_nack {
112         struct ds_msg_tag       tag;
113         __u64                   handle;
114 };
115
116 struct ds_data {
117         struct ds_msg_tag       tag;
118         __u64                   handle;
119 };
120
121 struct ds_data_nack {
122         struct ds_msg_tag       tag;
123         __u64                   handle;
124         __u64                   result;
125 };
126
127 struct ds_info;
128 struct ds_cap_state {
129         __u64                   handle;
130
131         void                    (*data)(struct ds_info *dp,
132                                         struct ds_cap_state *cp,
133                                         void *buf, int len);
134
135         const char              *service_id;
136
137         u8                      state;
138 #define CAP_STATE_UNKNOWN       0x00
139 #define CAP_STATE_REG_SENT      0x01
140 #define CAP_STATE_REGISTERED    0x02
141 };
142
143 static void md_update_data(struct ds_info *dp, struct ds_cap_state *cp,
144                            void *buf, int len);
145 static void domain_shutdown_data(struct ds_info *dp,
146                                  struct ds_cap_state *cp,
147                                  void *buf, int len);
148 static void domain_panic_data(struct ds_info *dp,
149                               struct ds_cap_state *cp,
150                               void *buf, int len);
151 #ifdef CONFIG_HOTPLUG_CPU
152 static void dr_cpu_data(struct ds_info *dp,
153                         struct ds_cap_state *cp,
154                         void *buf, int len);
155 #endif
156 static void ds_pri_data(struct ds_info *dp,
157                         struct ds_cap_state *cp,
158                         void *buf, int len);
159 static void ds_var_data(struct ds_info *dp,
160                         struct ds_cap_state *cp,
161                         void *buf, int len);
162
163 struct ds_cap_state ds_states_template[] = {
164         {
165                 .service_id     = "md-update",
166                 .data           = md_update_data,
167         },
168         {
169                 .service_id     = "domain-shutdown",
170                 .data           = domain_shutdown_data,
171         },
172         {
173                 .service_id     = "domain-panic",
174                 .data           = domain_panic_data,
175         },
176 #ifdef CONFIG_HOTPLUG_CPU
177         {
178                 .service_id     = "dr-cpu",
179                 .data           = dr_cpu_data,
180         },
181 #endif
182         {
183                 .service_id     = "pri",
184                 .data           = ds_pri_data,
185         },
186         {
187                 .service_id     = "var-config",
188                 .data           = ds_var_data,
189         },
190         {
191                 .service_id     = "var-config-backup",
192                 .data           = ds_var_data,
193         },
194 };
195
196 static DEFINE_SPINLOCK(ds_lock);
197
198 struct ds_info {
199         struct ldc_channel      *lp;
200         u8                      hs_state;
201 #define DS_HS_START             0x01
202 #define DS_HS_DONE              0x02
203
204         u64                     id;
205
206         void                    *rcv_buf;
207         int                     rcv_buf_len;
208
209         struct ds_cap_state     *ds_states;
210         int                     num_ds_states;
211
212         struct ds_info          *next;
213 };
214
215 static struct ds_info *ds_info_list;
216
217 static struct ds_cap_state *find_cap(struct ds_info *dp, u64 handle)
218 {
219         unsigned int index = handle >> 32;
220
221         if (index >= dp->num_ds_states)
222                 return NULL;
223         return &dp->ds_states[index];
224 }
225
226 static struct ds_cap_state *find_cap_by_string(struct ds_info *dp,
227                                                const char *name)
228 {
229         int i;
230
231         for (i = 0; i < dp->num_ds_states; i++) {
232                 if (strcmp(dp->ds_states[i].service_id, name))
233                         continue;
234
235                 return &dp->ds_states[i];
236         }
237         return NULL;
238 }
239
240 static int __ds_send(struct ldc_channel *lp, void *data, int len)
241 {
242         int err, limit = 1000;
243
244         err = -EINVAL;
245         while (limit-- > 0) {
246                 err = ldc_write(lp, data, len);
247                 if (!err || (err != -EAGAIN))
248                         break;
249                 udelay(1);
250         }
251
252         return err;
253 }
254
255 static int ds_send(struct ldc_channel *lp, void *data, int len)
256 {
257         unsigned long flags;
258         int err;
259
260         spin_lock_irqsave(&ds_lock, flags);
261         err = __ds_send(lp, data, len);
262         spin_unlock_irqrestore(&ds_lock, flags);
263
264         return err;
265 }
266
267 struct ds_md_update_req {
268         __u64                           req_num;
269 };
270
271 struct ds_md_update_res {
272         __u64                           req_num;
273         __u32                           result;
274 };
275
276 static void md_update_data(struct ds_info *dp,
277                            struct ds_cap_state *cp,
278                            void *buf, int len)
279 {
280         struct ldc_channel *lp = dp->lp;
281         struct ds_data *dpkt = buf;
282         struct ds_md_update_req *rp;
283         struct {
284                 struct ds_data          data;
285                 struct ds_md_update_res res;
286         } pkt;
287
288         rp = (struct ds_md_update_req *) (dpkt + 1);
289
290         printk(KERN_INFO "ds-%lu: Machine description update.\n", dp->id);
291
292         mdesc_update();
293
294         memset(&pkt, 0, sizeof(pkt));
295         pkt.data.tag.type = DS_DATA;
296         pkt.data.tag.len = sizeof(pkt) - sizeof(struct ds_msg_tag);
297         pkt.data.handle = cp->handle;
298         pkt.res.req_num = rp->req_num;
299         pkt.res.result = DS_OK;
300
301         ds_send(lp, &pkt, sizeof(pkt));
302 }
303
304 struct ds_shutdown_req {
305         __u64                           req_num;
306         __u32                           ms_delay;
307 };
308
309 struct ds_shutdown_res {
310         __u64                           req_num;
311         __u32                           result;
312         char                            reason[1];
313 };
314
315 static void domain_shutdown_data(struct ds_info *dp,
316                                  struct ds_cap_state *cp,
317                                  void *buf, int len)
318 {
319         struct ldc_channel *lp = dp->lp;
320         struct ds_data *dpkt = buf;
321         struct ds_shutdown_req *rp;
322         struct {
323                 struct ds_data          data;
324                 struct ds_shutdown_res  res;
325         } pkt;
326
327         rp = (struct ds_shutdown_req *) (dpkt + 1);
328
329         printk(KERN_ALERT "ds-%lu: Shutdown request from "
330                "LDOM manager received.\n", dp->id);
331
332         memset(&pkt, 0, sizeof(pkt));
333         pkt.data.tag.type = DS_DATA;
334         pkt.data.tag.len = sizeof(pkt) - sizeof(struct ds_msg_tag);
335         pkt.data.handle = cp->handle;
336         pkt.res.req_num = rp->req_num;
337         pkt.res.result = DS_OK;
338         pkt.res.reason[0] = 0;
339
340         ds_send(lp, &pkt, sizeof(pkt));
341
342         orderly_poweroff(true);
343 }
344
345 struct ds_panic_req {
346         __u64                           req_num;
347 };
348
349 struct ds_panic_res {
350         __u64                           req_num;
351         __u32                           result;
352         char                            reason[1];
353 };
354
355 static void domain_panic_data(struct ds_info *dp,
356                               struct ds_cap_state *cp,
357                               void *buf, int len)
358 {
359         struct ldc_channel *lp = dp->lp;
360         struct ds_data *dpkt = buf;
361         struct ds_panic_req *rp;
362         struct {
363                 struct ds_data          data;
364                 struct ds_panic_res     res;
365         } pkt;
366
367         rp = (struct ds_panic_req *) (dpkt + 1);
368
369         printk(KERN_ALERT "ds-%lu: Panic request from "
370                "LDOM manager received.\n", dp->id);
371
372         memset(&pkt, 0, sizeof(pkt));
373         pkt.data.tag.type = DS_DATA;
374         pkt.data.tag.len = sizeof(pkt) - sizeof(struct ds_msg_tag);
375         pkt.data.handle = cp->handle;
376         pkt.res.req_num = rp->req_num;
377         pkt.res.result = DS_OK;
378         pkt.res.reason[0] = 0;
379
380         ds_send(lp, &pkt, sizeof(pkt));
381
382         panic("PANIC requested by LDOM manager.");
383 }
384
385 #ifdef CONFIG_HOTPLUG_CPU
386 struct dr_cpu_tag {
387         __u64                           req_num;
388         __u32                           type;
389 #define DR_CPU_CONFIGURE                0x43
390 #define DR_CPU_UNCONFIGURE              0x55
391 #define DR_CPU_FORCE_UNCONFIGURE        0x46
392 #define DR_CPU_STATUS                   0x53
393
394 /* Responses */
395 #define DR_CPU_OK                       0x6f
396 #define DR_CPU_ERROR                    0x65
397
398         __u32                           num_records;
399 };
400
401 struct dr_cpu_resp_entry {
402         __u32                           cpu;
403         __u32                           result;
404 #define DR_CPU_RES_OK                   0x00
405 #define DR_CPU_RES_FAILURE              0x01
406 #define DR_CPU_RES_BLOCKED              0x02
407 #define DR_CPU_RES_CPU_NOT_RESPONDING   0x03
408 #define DR_CPU_RES_NOT_IN_MD            0x04
409
410         __u32                           stat;
411 #define DR_CPU_STAT_NOT_PRESENT         0x00
412 #define DR_CPU_STAT_UNCONFIGURED        0x01
413 #define DR_CPU_STAT_CONFIGURED          0x02
414
415         __u32                           str_off;
416 };
417
418 static void __dr_cpu_send_error(struct ds_info *dp,
419                                 struct ds_cap_state *cp,
420                                 struct ds_data *data)
421 {
422         struct dr_cpu_tag *tag = (struct dr_cpu_tag *) (data + 1);
423         struct {
424                 struct ds_data          data;
425                 struct dr_cpu_tag       tag;
426         } pkt;
427         int msg_len;
428
429         memset(&pkt, 0, sizeof(pkt));
430         pkt.data.tag.type = DS_DATA;
431         pkt.data.handle = cp->handle;
432         pkt.tag.req_num = tag->req_num;
433         pkt.tag.type = DR_CPU_ERROR;
434         pkt.tag.num_records = 0;
435
436         msg_len = (sizeof(struct ds_data) +
437                    sizeof(struct dr_cpu_tag));
438
439         pkt.data.tag.len = msg_len - sizeof(struct ds_msg_tag);
440
441         __ds_send(dp->lp, &pkt, msg_len);
442 }
443
444 static void dr_cpu_send_error(struct ds_info *dp,
445                               struct ds_cap_state *cp,
446                               struct ds_data *data)
447 {
448         unsigned long flags;
449
450         spin_lock_irqsave(&ds_lock, flags);
451         __dr_cpu_send_error(dp, cp, data);
452         spin_unlock_irqrestore(&ds_lock, flags);
453 }
454
455 #define CPU_SENTINEL    0xffffffff
456
457 static void purge_dups(u32 *list, u32 num_ents)
458 {
459         unsigned int i;
460
461         for (i = 0; i < num_ents; i++) {
462                 u32 cpu = list[i];
463                 unsigned int j;
464
465                 if (cpu == CPU_SENTINEL)
466                         continue;
467
468                 for (j = i + 1; j < num_ents; j++) {
469                         if (list[j] == cpu)
470                                 list[j] = CPU_SENTINEL;
471                 }
472         }
473 }
474
475 static int dr_cpu_size_response(int ncpus)
476 {
477         return (sizeof(struct ds_data) +
478                 sizeof(struct dr_cpu_tag) +
479                 (sizeof(struct dr_cpu_resp_entry) * ncpus));
480 }
481
482 static void dr_cpu_init_response(struct ds_data *resp, u64 req_num,
483                                  u64 handle, int resp_len, int ncpus,
484                                  cpumask_t *mask, u32 default_stat)
485 {
486         struct dr_cpu_resp_entry *ent;
487         struct dr_cpu_tag *tag;
488         int i, cpu;
489
490         tag = (struct dr_cpu_tag *) (resp + 1);
491         ent = (struct dr_cpu_resp_entry *) (tag + 1);
492
493         resp->tag.type = DS_DATA;
494         resp->tag.len = resp_len - sizeof(struct ds_msg_tag);
495         resp->handle = handle;
496         tag->req_num = req_num;
497         tag->type = DR_CPU_OK;
498         tag->num_records = ncpus;
499
500         i = 0;
501         for_each_cpu_mask(cpu, *mask) {
502                 ent[i].cpu = cpu;
503                 ent[i].result = DR_CPU_RES_OK;
504                 ent[i].stat = default_stat;
505                 i++;
506         }
507         BUG_ON(i != ncpus);
508 }
509
510 static void dr_cpu_mark(struct ds_data *resp, int cpu, int ncpus,
511                         u32 res, u32 stat)
512 {
513         struct dr_cpu_resp_entry *ent;
514         struct dr_cpu_tag *tag;
515         int i;
516
517         tag = (struct dr_cpu_tag *) (resp + 1);
518         ent = (struct dr_cpu_resp_entry *) (tag + 1);
519
520         for (i = 0; i < ncpus; i++) {
521                 if (ent[i].cpu != cpu)
522                         continue;
523                 ent[i].result = res;
524                 ent[i].stat = stat;
525                 break;
526         }
527 }
528
529 static int dr_cpu_configure(struct ds_info *dp,
530                             struct ds_cap_state *cp,
531                             u64 req_num,
532                             cpumask_t *mask)
533 {
534         struct ds_data *resp;
535         int resp_len, ncpus, cpu;
536         unsigned long flags;
537
538         ncpus = cpus_weight(*mask);
539         resp_len = dr_cpu_size_response(ncpus);
540         resp = kzalloc(resp_len, GFP_KERNEL);
541         if (!resp)
542                 return -ENOMEM;
543
544         dr_cpu_init_response(resp, req_num, cp->handle,
545                              resp_len, ncpus, mask,
546                              DR_CPU_STAT_CONFIGURED);
547
548         mdesc_fill_in_cpu_data(*mask);
549
550         for_each_cpu_mask(cpu, *mask) {
551                 int err;
552
553                 printk(KERN_INFO "ds-%lu: Starting cpu %d...\n",
554                        dp->id, cpu);
555                 err = cpu_up(cpu);
556                 if (err) {
557                         __u32 res = DR_CPU_RES_FAILURE;
558                         __u32 stat = DR_CPU_STAT_UNCONFIGURED;
559
560                         if (!cpu_present(cpu)) {
561                                 /* CPU not present in MD */
562                                 res = DR_CPU_RES_NOT_IN_MD;
563                                 stat = DR_CPU_STAT_NOT_PRESENT;
564                         } else if (err == -ENODEV) {
565                                 /* CPU did not call in successfully */
566                                 res = DR_CPU_RES_CPU_NOT_RESPONDING;
567                         }
568
569                         printk(KERN_INFO "ds-%lu: CPU startup failed err=%d\n",
570                                dp->id, err);
571                         dr_cpu_mark(resp, cpu, ncpus, res, stat);
572                 }
573         }
574
575         spin_lock_irqsave(&ds_lock, flags);
576         __ds_send(dp->lp, resp, resp_len);
577         spin_unlock_irqrestore(&ds_lock, flags);
578
579         kfree(resp);
580
581         /* Redistribute IRQs, taking into account the new cpus.  */
582         fixup_irqs();
583
584         return 0;
585 }
586
587 static int dr_cpu_unconfigure(struct ds_info *dp,
588                               struct ds_cap_state *cp,
589                               u64 req_num,
590                               cpumask_t *mask)
591 {
592         struct ds_data *resp;
593         int resp_len, ncpus, cpu;
594         unsigned long flags;
595
596         ncpus = cpus_weight(*mask);
597         resp_len = dr_cpu_size_response(ncpus);
598         resp = kzalloc(resp_len, GFP_KERNEL);
599         if (!resp)
600                 return -ENOMEM;
601
602         dr_cpu_init_response(resp, req_num, cp->handle,
603                              resp_len, ncpus, mask,
604                              DR_CPU_STAT_UNCONFIGURED);
605
606         for_each_cpu_mask(cpu, *mask) {
607                 int err;
608
609                 printk(KERN_INFO "ds-%lu: Shutting down cpu %d...\n",
610                        dp->id, cpu);
611                 err = cpu_down(cpu);
612                 if (err)
613                         dr_cpu_mark(resp, cpu, ncpus,
614                                     DR_CPU_RES_FAILURE,
615                                     DR_CPU_STAT_CONFIGURED);
616         }
617
618         spin_lock_irqsave(&ds_lock, flags);
619         __ds_send(dp->lp, resp, resp_len);
620         spin_unlock_irqrestore(&ds_lock, flags);
621
622         kfree(resp);
623
624         return 0;
625 }
626
627 static void dr_cpu_data(struct ds_info *dp,
628                         struct ds_cap_state *cp,
629                         void *buf, int len)
630 {
631         struct ds_data *data = buf;
632         struct dr_cpu_tag *tag = (struct dr_cpu_tag *) (data + 1);
633         u32 *cpu_list = (u32 *) (tag + 1);
634         u64 req_num = tag->req_num;
635         cpumask_t mask;
636         unsigned int i;
637         int err;
638
639         switch (tag->type) {
640         case DR_CPU_CONFIGURE:
641         case DR_CPU_UNCONFIGURE:
642         case DR_CPU_FORCE_UNCONFIGURE:
643                 break;
644
645         default:
646                 dr_cpu_send_error(dp, cp, data);
647                 return;
648         }
649
650         purge_dups(cpu_list, tag->num_records);
651
652         cpus_clear(mask);
653         for (i = 0; i < tag->num_records; i++) {
654                 if (cpu_list[i] == CPU_SENTINEL)
655                         continue;
656
657                 if (cpu_list[i] < NR_CPUS)
658                         cpu_set(cpu_list[i], mask);
659         }
660
661         if (tag->type == DR_CPU_CONFIGURE)
662                 err = dr_cpu_configure(dp, cp, req_num, &mask);
663         else
664                 err = dr_cpu_unconfigure(dp, cp, req_num, &mask);
665
666         if (err)
667                 dr_cpu_send_error(dp, cp, data);
668 }
669 #endif /* CONFIG_HOTPLUG_CPU */
670
671 struct ds_pri_msg {
672         __u64                           req_num;
673         __u64                           type;
674 #define DS_PRI_REQUEST                  0x00
675 #define DS_PRI_DATA                     0x01
676 #define DS_PRI_UPDATE                   0x02
677 };
678
679 static void ds_pri_data(struct ds_info *dp,
680                         struct ds_cap_state *cp,
681                         void *buf, int len)
682 {
683         struct ds_data *dpkt = buf;
684         struct ds_pri_msg *rp;
685
686         rp = (struct ds_pri_msg *) (dpkt + 1);
687
688         printk(KERN_INFO "ds-%lu: PRI REQ [%lx:%lx], len=%d\n",
689                dp->id, rp->req_num, rp->type, len);
690 }
691
692 struct ds_var_hdr {
693         __u32                           type;
694 #define DS_VAR_SET_REQ                  0x00
695 #define DS_VAR_DELETE_REQ               0x01
696 #define DS_VAR_SET_RESP                 0x02
697 #define DS_VAR_DELETE_RESP              0x03
698 };
699
700 struct ds_var_set_msg {
701         struct ds_var_hdr               hdr;
702         char                            name_and_value[0];
703 };
704
705 struct ds_var_delete_msg {
706         struct ds_var_hdr               hdr;
707         char                            name[0];
708 };
709
710 struct ds_var_resp {
711         struct ds_var_hdr               hdr;
712         __u32                           result;
713 #define DS_VAR_SUCCESS                  0x00
714 #define DS_VAR_NO_SPACE                 0x01
715 #define DS_VAR_INVALID_VAR              0x02
716 #define DS_VAR_INVALID_VAL              0x03
717 #define DS_VAR_NOT_PRESENT              0x04
718 };
719
720 static DEFINE_MUTEX(ds_var_mutex);
721 static int ds_var_doorbell;
722 static int ds_var_response;
723
724 static void ds_var_data(struct ds_info *dp,
725                         struct ds_cap_state *cp,
726                         void *buf, int len)
727 {
728         struct ds_data *dpkt = buf;
729         struct ds_var_resp *rp;
730
731         rp = (struct ds_var_resp *) (dpkt + 1);
732
733         if (rp->hdr.type != DS_VAR_SET_RESP &&
734             rp->hdr.type != DS_VAR_DELETE_RESP)
735                 return;
736
737         ds_var_response = rp->result;
738         wmb();
739         ds_var_doorbell = 1;
740 }
741
742 void ldom_set_var(const char *var, const char *value)
743 {
744         struct ds_cap_state *cp;
745         struct ds_info *dp;
746         unsigned long flags;
747
748         spin_lock_irqsave(&ds_lock, flags);
749         cp = NULL;
750         for (dp = ds_info_list; dp; dp = dp->next) {
751                 struct ds_cap_state *tmp;
752
753                 tmp = find_cap_by_string(dp, "var-config");
754                 if (tmp && tmp->state == CAP_STATE_REGISTERED) {
755                         cp = tmp;
756                         break;
757                 }
758         }
759         if (!cp) {
760                 for (dp = ds_info_list; dp; dp = dp->next) {
761                         struct ds_cap_state *tmp;
762
763                         tmp = find_cap_by_string(dp, "var-config-backup");
764                         if (tmp && tmp->state == CAP_STATE_REGISTERED) {
765                                 cp = tmp;
766                                 break;
767                         }
768                 }
769         }
770         spin_unlock_irqrestore(&ds_lock, flags);
771
772         if (cp) {
773                 union {
774                         struct {
775                                 struct ds_data          data;
776                                 struct ds_var_set_msg   msg;
777                         } header;
778                         char                    all[512];
779                 } pkt;
780                 char  *base, *p;
781                 int msg_len, loops;
782
783                 memset(&pkt, 0, sizeof(pkt));
784                 pkt.header.data.tag.type = DS_DATA;
785                 pkt.header.data.handle = cp->handle;
786                 pkt.header.msg.hdr.type = DS_VAR_SET_REQ;
787                 base = p = &pkt.header.msg.name_and_value[0];
788                 strcpy(p, var);
789                 p += strlen(var) + 1;
790                 strcpy(p, value);
791                 p += strlen(value) + 1;
792
793                 msg_len = (sizeof(struct ds_data) +
794                            sizeof(struct ds_var_set_msg) +
795                            (p - base));
796                 msg_len = (msg_len + 3) & ~3;
797                 pkt.header.data.tag.len = msg_len - sizeof(struct ds_msg_tag);
798
799                 mutex_lock(&ds_var_mutex);
800
801                 spin_lock_irqsave(&ds_lock, flags);
802                 ds_var_doorbell = 0;
803                 ds_var_response = -1;
804
805                 __ds_send(dp->lp, &pkt, msg_len);
806                 spin_unlock_irqrestore(&ds_lock, flags);
807
808                 loops = 1000;
809                 while (ds_var_doorbell == 0) {
810                         if (loops-- < 0)
811                                 break;
812                         barrier();
813                         udelay(100);
814                 }
815
816                 mutex_unlock(&ds_var_mutex);
817
818                 if (ds_var_doorbell == 0 ||
819                     ds_var_response != DS_VAR_SUCCESS)
820                         printk(KERN_ERR "ds-%lu: var-config [%s:%s] "
821                                "failed, response(%d).\n",
822                                dp->id, var, value,
823                                ds_var_response);
824         } else {
825                 printk(KERN_ERR PFX "var-config not registered so "
826                        "could not set (%s) variable to (%s).\n",
827                        var, value);
828         }
829 }
830
831 void ldom_reboot(const char *boot_command)
832 {
833         /* Don't bother with any of this if the boot_command
834          * is empty.
835          */
836         if (boot_command && strlen(boot_command)) {
837                 char full_boot_str[256];
838
839                 strcpy(full_boot_str, "boot ");
840                 strcpy(full_boot_str + strlen("boot "), boot_command);
841
842                 ldom_set_var("reboot-command", full_boot_str);
843         }
844         sun4v_mach_sir();
845 }
846
847 void ldom_power_off(void)
848 {
849         sun4v_mach_exit(0);
850 }
851
852 static void ds_conn_reset(struct ds_info *dp)
853 {
854         printk(KERN_ERR "ds-%lu: ds_conn_reset() from %p\n",
855                dp->id, __builtin_return_address(0));
856 }
857
858 static int register_services(struct ds_info *dp)
859 {
860         struct ldc_channel *lp = dp->lp;
861         int i;
862
863         for (i = 0; i < dp->num_ds_states; i++) {
864                 struct {
865                         struct ds_reg_req req;
866                         u8 id_buf[256];
867                 } pbuf;
868                 struct ds_cap_state *cp = &dp->ds_states[i];
869                 int err, msg_len;
870                 u64 new_count;
871
872                 if (cp->state == CAP_STATE_REGISTERED)
873                         continue;
874
875                 new_count = sched_clock() & 0xffffffff;
876                 cp->handle = ((u64) i << 32) | new_count;
877
878                 msg_len = (sizeof(struct ds_reg_req) +
879                            strlen(cp->service_id));
880
881                 memset(&pbuf, 0, sizeof(pbuf));
882                 pbuf.req.tag.type = DS_REG_REQ;
883                 pbuf.req.tag.len = (msg_len - sizeof(struct ds_msg_tag));
884                 pbuf.req.handle = cp->handle;
885                 pbuf.req.major = 1;
886                 pbuf.req.minor = 0;
887                 strcpy(pbuf.req.svc_id, cp->service_id);
888
889                 err = __ds_send(lp, &pbuf, msg_len);
890                 if (err > 0)
891                         cp->state = CAP_STATE_REG_SENT;
892         }
893         return 0;
894 }
895
896 static int ds_handshake(struct ds_info *dp, struct ds_msg_tag *pkt)
897 {
898
899         if (dp->hs_state == DS_HS_START) {
900                 if (pkt->type != DS_INIT_ACK)
901                         goto conn_reset;
902
903                 dp->hs_state = DS_HS_DONE;
904
905                 return register_services(dp);
906         }
907
908         if (dp->hs_state != DS_HS_DONE)
909                 goto conn_reset;
910
911         if (pkt->type == DS_REG_ACK) {
912                 struct ds_reg_ack *ap = (struct ds_reg_ack *) pkt;
913                 struct ds_cap_state *cp = find_cap(dp, ap->handle);
914
915                 if (!cp) {
916                         printk(KERN_ERR "ds-%lu: REG ACK for unknown "
917                                "handle %lx\n", dp->id, ap->handle);
918                         return 0;
919                 }
920                 printk(KERN_INFO "ds-%lu: Registered %s service.\n",
921                        dp->id, cp->service_id);
922                 cp->state = CAP_STATE_REGISTERED;
923         } else if (pkt->type == DS_REG_NACK) {
924                 struct ds_reg_nack *np = (struct ds_reg_nack *) pkt;
925                 struct ds_cap_state *cp = find_cap(dp, np->handle);
926
927                 if (!cp) {
928                         printk(KERN_ERR "ds-%lu: REG NACK for "
929                                "unknown handle %lx\n",
930                                dp->id, np->handle);
931                         return 0;
932                 }
933                 printk(KERN_INFO "ds-%lu: Could not register %s service\n",
934                        dp->id, cp->service_id);
935                 cp->state = CAP_STATE_UNKNOWN;
936         }
937
938         return 0;
939
940 conn_reset:
941         ds_conn_reset(dp);
942         return -ECONNRESET;
943 }
944
945 static void __send_ds_nack(struct ds_info *dp, u64 handle)
946 {
947         struct ds_data_nack nack = {
948                 .tag = {
949                         .type = DS_NACK,
950                         .len = (sizeof(struct ds_data_nack) -
951                                 sizeof(struct ds_msg_tag)),
952                 },
953                 .handle = handle,
954                 .result = DS_INV_HDL,
955         };
956
957         __ds_send(dp->lp, &nack, sizeof(nack));
958 }
959
960 static LIST_HEAD(ds_work_list);
961 static DECLARE_WAIT_QUEUE_HEAD(ds_wait);
962
963 struct ds_queue_entry {
964         struct list_head                list;
965         struct ds_info                  *dp;
966         int                             req_len;
967         int                             __pad;
968         u64                             req[0];
969 };
970
971 static void process_ds_work(void)
972 {
973         struct ds_queue_entry *qp, *tmp;
974         unsigned long flags;
975         LIST_HEAD(todo);
976
977         spin_lock_irqsave(&ds_lock, flags);
978         list_splice(&ds_work_list, &todo);
979         INIT_LIST_HEAD(&ds_work_list);
980         spin_unlock_irqrestore(&ds_lock, flags);
981
982         list_for_each_entry_safe(qp, tmp, &todo, list) {
983                 struct ds_data *dpkt = (struct ds_data *) qp->req;
984                 struct ds_info *dp = qp->dp;
985                 struct ds_cap_state *cp = find_cap(dp, dpkt->handle);
986                 int req_len = qp->req_len;
987
988                 if (!cp) {
989                         printk(KERN_ERR "ds-%lu: Data for unknown "
990                                "handle %lu\n",
991                                dp->id, dpkt->handle);
992
993                         spin_lock_irqsave(&ds_lock, flags);
994                         __send_ds_nack(dp, dpkt->handle);
995                         spin_unlock_irqrestore(&ds_lock, flags);
996                 } else {
997                         cp->data(dp, cp, dpkt, req_len);
998                 }
999
1000                 list_del(&qp->list);
1001                 kfree(qp);
1002         }
1003 }
1004
1005 static int ds_thread(void *__unused)
1006 {
1007         DEFINE_WAIT(wait);
1008
1009         while (1) {
1010                 prepare_to_wait(&ds_wait, &wait, TASK_INTERRUPTIBLE);
1011                 if (list_empty(&ds_work_list))
1012                         schedule();
1013                 finish_wait(&ds_wait, &wait);
1014
1015                 if (kthread_should_stop())
1016                         break;
1017
1018                 process_ds_work();
1019         }
1020
1021         return 0;
1022 }
1023
1024 static int ds_data(struct ds_info *dp, struct ds_msg_tag *pkt, int len)
1025 {
1026         struct ds_data *dpkt = (struct ds_data *) pkt;
1027         struct ds_queue_entry *qp;
1028
1029         qp = kmalloc(sizeof(struct ds_queue_entry) + len, GFP_ATOMIC);
1030         if (!qp) {
1031                 __send_ds_nack(dp, dpkt->handle);
1032         } else {
1033                 qp->dp = dp;
1034                 memcpy(&qp->req, pkt, len);
1035                 list_add_tail(&qp->list, &ds_work_list);
1036                 wake_up(&ds_wait);
1037         }
1038         return 0;
1039 }
1040
1041 static void ds_up(struct ds_info *dp)
1042 {
1043         struct ldc_channel *lp = dp->lp;
1044         struct ds_ver_req req;
1045         int err;
1046
1047         req.tag.type = DS_INIT_REQ;
1048         req.tag.len = sizeof(req) - sizeof(struct ds_msg_tag);
1049         req.ver.major = 1;
1050         req.ver.minor = 0;
1051
1052         err = __ds_send(lp, &req, sizeof(req));
1053         if (err > 0)
1054                 dp->hs_state = DS_HS_START;
1055 }
1056
1057 static void ds_reset(struct ds_info *dp)
1058 {
1059         int i;
1060
1061         dp->hs_state = 0;
1062
1063         for (i = 0; i < dp->num_ds_states; i++) {
1064                 struct ds_cap_state *cp = &dp->ds_states[i];
1065
1066                 cp->state = CAP_STATE_UNKNOWN;
1067         }
1068 }
1069
1070 static void ds_event(void *arg, int event)
1071 {
1072         struct ds_info *dp = arg;
1073         struct ldc_channel *lp = dp->lp;
1074         unsigned long flags;
1075         int err;
1076
1077         spin_lock_irqsave(&ds_lock, flags);
1078
1079         if (event == LDC_EVENT_UP) {
1080                 ds_up(dp);
1081                 spin_unlock_irqrestore(&ds_lock, flags);
1082                 return;
1083         }
1084
1085         if (event == LDC_EVENT_RESET) {
1086                 ds_reset(dp);
1087                 spin_unlock_irqrestore(&ds_lock, flags);
1088                 return;
1089         }
1090
1091         if (event != LDC_EVENT_DATA_READY) {
1092                 printk(KERN_WARNING "ds-%lu: Unexpected LDC event %d\n",
1093                        dp->id, event);
1094                 spin_unlock_irqrestore(&ds_lock, flags);
1095                 return;
1096         }
1097
1098         err = 0;
1099         while (1) {
1100                 struct ds_msg_tag *tag;
1101
1102                 err = ldc_read(lp, dp->rcv_buf, sizeof(*tag));
1103
1104                 if (unlikely(err < 0)) {
1105                         if (err == -ECONNRESET)
1106                                 ds_conn_reset(dp);
1107                         break;
1108                 }
1109                 if (err == 0)
1110                         break;
1111
1112                 tag = dp->rcv_buf;
1113                 err = ldc_read(lp, tag + 1, tag->len);
1114
1115                 if (unlikely(err < 0)) {
1116                         if (err == -ECONNRESET)
1117                                 ds_conn_reset(dp);
1118                         break;
1119                 }
1120                 if (err < tag->len)
1121                         break;
1122
1123                 if (tag->type < DS_DATA)
1124                         err = ds_handshake(dp, dp->rcv_buf);
1125                 else
1126                         err = ds_data(dp, dp->rcv_buf,
1127                                       sizeof(*tag) + err);
1128                 if (err == -ECONNRESET)
1129                         break;
1130         }
1131
1132         spin_unlock_irqrestore(&ds_lock, flags);
1133 }
1134
1135 static int __devinit ds_probe(struct vio_dev *vdev,
1136                               const struct vio_device_id *id)
1137 {
1138         static int ds_version_printed;
1139         struct ldc_channel_config ds_cfg = {
1140                 .event          = ds_event,
1141                 .mtu            = 4096,
1142                 .mode           = LDC_MODE_STREAM,
1143         };
1144         struct mdesc_handle *hp;
1145         struct ldc_channel *lp;
1146         struct ds_info *dp;
1147         const u64 *val;
1148         int err, i;
1149
1150         if (ds_version_printed++ == 0)
1151                 printk(KERN_INFO "%s", version);
1152
1153         dp = kzalloc(sizeof(*dp), GFP_KERNEL);
1154         err = -ENOMEM;
1155         if (!dp)
1156                 goto out_err;
1157
1158         hp = mdesc_grab();
1159         val = mdesc_get_property(hp, vdev->mp, "id", NULL);
1160         if (val)
1161                 dp->id = *val;
1162         mdesc_release(hp);
1163
1164         dp->rcv_buf = kzalloc(4096, GFP_KERNEL);
1165         if (!dp->rcv_buf)
1166                 goto out_free_dp;
1167
1168         dp->rcv_buf_len = 4096;
1169
1170         dp->ds_states = kzalloc(sizeof(ds_states_template),
1171                                 GFP_KERNEL);
1172         if (!dp->ds_states)
1173                 goto out_free_rcv_buf;
1174
1175         memcpy(dp->ds_states, ds_states_template,
1176                sizeof(ds_states_template));
1177         dp->num_ds_states = ARRAY_SIZE(ds_states_template);
1178
1179         for (i = 0; i < dp->num_ds_states; i++)
1180                 dp->ds_states[i].handle = ((u64)i << 32);
1181
1182         ds_cfg.tx_irq = vdev->tx_irq;
1183         ds_cfg.rx_irq = vdev->rx_irq;
1184
1185         lp = ldc_alloc(vdev->channel_id, &ds_cfg, dp);
1186         if (IS_ERR(lp)) {
1187                 err = PTR_ERR(lp);
1188                 goto out_free_ds_states;
1189         }
1190         dp->lp = lp;
1191
1192         err = ldc_bind(lp, "DS");
1193         if (err)
1194                 goto out_free_ldc;
1195
1196         spin_lock_irq(&ds_lock);
1197         dp->next = ds_info_list;
1198         ds_info_list = dp;
1199         spin_unlock_irq(&ds_lock);
1200
1201         return err;
1202
1203 out_free_ldc:
1204         ldc_free(dp->lp);
1205
1206 out_free_ds_states:
1207         kfree(dp->ds_states);
1208
1209 out_free_rcv_buf:
1210         kfree(dp->rcv_buf);
1211
1212 out_free_dp:
1213         kfree(dp);
1214
1215 out_err:
1216         return err;
1217 }
1218
1219 static int ds_remove(struct vio_dev *vdev)
1220 {
1221         return 0;
1222 }
1223
1224 static struct vio_device_id ds_match[] = {
1225         {
1226                 .type = "domain-services-port",
1227         },
1228         {},
1229 };
1230
1231 static struct vio_driver ds_driver = {
1232         .id_table       = ds_match,
1233         .probe          = ds_probe,
1234         .remove         = ds_remove,
1235         .driver         = {
1236                 .name   = "ds",
1237                 .owner  = THIS_MODULE,
1238         }
1239 };
1240
1241 static int __init ds_init(void)
1242 {
1243         kthread_run(ds_thread, NULL, "kldomd");
1244
1245         return vio_register_driver(&ds_driver);
1246 }
1247
1248 subsys_initcall(ds_init);